机器人运动过程中机械臂位姿奇异问题是由于机械臂逆向运动学解算的伪逆算法固有缺陷导致的，一般会发生在机械臂肘部关节趋近于零、处于安全停靠构型以及其他未知的状态。陷入奇异点后，通常采取路径规划方式避开，处置过程存在状态研判困难、规划耗时、再次操控仍然容易陷入奇异等问题。针对此问题，根据机械臂陷入奇异位置的逆运动学特点，提出了一种基于雅可比矩阵模值和条件数的预警方法和基于距离投影的运动路径处置方法，并进行了仿真验证。结果表明雅可比矩阵模值对奇异位置发生过程更为敏感，适合作为监测参数。 The singular position and attitude of mechanical arm in the process of remote control is resulted from the inherent defects of pseudo inverse algorithm for inverse kinematics solution of mechanical arm and generally occurs when the elbow joint angle of mechanical arm is close to zero and the mechanical arm is in a safe docking configuration and other unknown states. In case of getting into the singular point, the method of path planning is usually used to avoid it, and some problems will happen in the process of disposal, for example, it is difficult to judge the state, it takes a lot of time to plan, recontrol will easily get into the singular point again. In order to solve these problems, according to the inverse kinematics characteristics when mechanical arm gets into the singular point, this paper put forward a warning method based on jacobian matrix module value and conditional number and a disposal method based on distance projection and carried out simulation verification. The results show that jacobian matrix module value is more sensitive to the generating process of singular position and can be used as the detection parameter.
机器人运动过程中机械臂位姿奇异问题是由于机械臂逆向运动学解算的伪逆算法固有缺陷导致的，一般会发生在机械臂肘部关节趋近于零、处于安全停靠构型以及其他未知的状态。陷入奇异点后，通常采取路径规划方式避开，处置过程存在状态研判困难、规划耗时、再次操控仍然容易陷入奇异等问题。针对此问题，根据机械臂陷入奇异位置的逆运动学特点，提出了一种基于雅可比矩阵模值和条件数的预警方法和基于距离投影的运动路径处置方法，并进行了仿真验证。结果表明雅可比矩阵模值对奇异位置发生过程更为敏感，适合作为监测参数。
工业机器人，机械臂，遥操作，奇异位置，雅可比矩阵
Fuqiang Cheng, An Wang, Changfeng Xia
Key Laboratory of Fault Diagnosis and Maintenance of In-Orbit, Xi’an Shaanxi
Received: Feb. 2nd, 2024; accepted: Mar. 7th, 2024; published: Mar. 13th, 2024
The singular position and attitude of mechanical arm in the process of remote control is resulted from the inherent defects of pseudo inverse algorithm for inverse kinematics solution of mechanical arm and generally occurs when the elbow joint angle of mechanical arm is close to zero and the mechanical arm is in a safe docking configuration and other unknown states. In case of getting into the singular point, the method of path planning is usually used to avoid it, and some problems will happen in the process of disposal, for example, it is difficult to judge the state, it takes a lot of time to plan, re-control will easily get into the singular point again. In order to solve these problems, according to the inverse kinematics characteristics when mechanical arm gets into the singular point, this paper put forward a warning method based on jacobian matrix module value and conditional number and a disposal method based on distance projection and carried out simulation verification. The results show that jacobian matrix module value is more sensitive to the generating process of singular position and can be used as the detection parameter.
Keywords:Industrial Robot, Mechanical Arm, Teleoperation, Singular Position, Jacobi Matrix
Copyright © 2024 by author(s) and beplay安卓登录
This work is licensed under the Creative Commons Attribution International License (CC BY 4.0).
http://creativecommons.org/licenses/by/4.0/
工业机器人是一种自动控制的、可再编程的多用途操作机械臂，一般由三个或以上的可编程关节组成 [
机械臂位姿奇异问题是由于机械臂逆向运动学解算的伪逆算法固有缺陷导致的。一旦机械臂运动至奇异区，会导致该算法失效，操控无法进行下去，一般会发生在机械臂肘部关节趋近于零、处于安全停靠构型以及其他未知的状态。由于冗余机械臂和运动学算法固有特性，机械臂奇异位置理论上有无穷多个。目前的遥操作系统尚未对机械臂位姿奇异问题形成有效的预警和处置方法。陷入奇异点后，通常采取路径规划方式避开，处置过程存在状态研判困难、规划耗时、再次操控仍然容易陷入奇异等问题。本研究针对此问题，根据机械臂陷入奇异位置的逆运动学特点，提出来了一种基于雅可比矩阵模值和条件数的预警方法和基于距离投影的运动路径处置方法，并进行了仿真验证。
机械臂采用7自由度近似对称构型，一端安装机械臂基座，一端安装快换装置。机械臂基座用于机械臂与卫星舱体连接，快换装置用于实现末端操作工具的更换和多模手眼相机的安装。机械臂由肩部三关节、肘部关节、腕部三关节、臂杆、快换装置、左/右多模手眼相机、末端监测相机、锁紧释放机构，以及热控组件等几部分组成。肩部关节控制器用于控制肩部三关节及肘部关节。腕部关节控制器用于控制腕部三关节和快换装置以及实现与末端工具的通讯。表1所示为机械臂MDH参数，建立机械臂各杆件的坐标系如图1所示，Z轴为旋转轴，α为杆件扭角，a为杆件偏置，d为杆件长度，θ为杆件旋转角，{0}为基座坐标系。
| 关节 | α (˚) | a (mm) | d (mm) | θ (˚) |
|---|---|---|---|---|
| 1 | 90 | 200.0 | 0 | 0 |
| 2 | 90 | 210.0 | 0 | 0 |
| 3 | −90 | 230.0 | 0 | −90 |
| 4 | 0 | 210.0 | 1420.0 | 0 |
| 5 | 0 | 210.0 | 1420.0 | 90 |
| 6 | 90 | 220.0 | 0 | 0 |
| 7 | −90 | 270.0 | 0 | 0 |
表1. 七自由度机械臂MDH参数
图1. 七自由度机械臂各杆件坐标系示意
从第i轴，到第i−1轴的坐标变换 i i − 1 A 为
i i − 1 A = r o t a t e ( a i , α i ) ⋅ r o t a t e ( d i , θ i ) = [ C ( θ i ) − S ( θ i ) 0 a i S ( θ i ) C ( α i ) C ( θ i ) C ( α i ) − S ( α i ) − d i ⋅ S ( α i ) S ( θ i ) S ( α i ) C ( θ i ) C ( α i ) C ( α i ) d i ⋅ C ( α i ) 0 0 0 1 ] (1)
其中， C ( θ i ) = cos ( θ i ) ， S ( θ i ) = sin ( θ i ) ， S ( α i ) = sin ( α i ) ， C ( α i ) = cos ( α i ) 。由此，正运动学问题变成了7个字问题，即7个齐次变换矩阵。可得机械臂末端坐标系{7}相对于基座坐标系{0}的位姿，应用齐次变换矩阵的串接性质可得到机械臂支路的齐次变换矩阵：
7 0 A = ∏ k = 1 7 A k k − 1 = [ r 11 r 12 r 12 p x r 21 r 22 r 23 p y r 31 r 32 r 33 p z 0 0 0 1 ]
式中， [ r 11 r 12 r 13 r 21 r 22 r 23 r 31 r 32 r 33 ] 为腕部坐标系相对于基座坐标系旋转矩阵，三行分别描述了坐标轴XYZ在基座坐标系三个坐标轴上的投影。[pxpypz]T为腕部坐标系相对于基座坐标系的平移矢量。
机械臂末端位置运动学方程为 [
r ˙ tI = J L ( q ) q ˙ (2)
式中， r ˙ tI ∈ R m 为机械臂末端在笛卡尔坐标系下的广义速度， q ∈ R n 、 J L ( q ) ∈ R m × n 分别为本体与机械臂联合角位移列向量和位置雅可比矩阵，m = 3为机械臂末端运动空间维数，即三个位置方向，n = 7为机械臂自由度个数。q和JL(q)形式为
q = [ θ 1 θ 2 θ 3 θ 4 θ 5 θ 6 θ 7 ] T
J L ( q ) = [ J L1 J L2 J L3 J L4 J L5 J L6 J L7 ] 3 × 7 T
其中， θ i ( i = 1 , ⋯ , 7 ) 为机械臂的关节角，JLi为第i节刚性臂相对于其他刚性臂关节的转换矩阵与位移矩阵形成的位置分量。刚性臂末端的姿态运动学方程为
e ˙ tI = J A ( q ) q ˙
式中， e ˙ tI 为刚性臂末端相对于本体系的姿态角速度，JΛ(q)为姿态雅可比矩阵，且
J Λ ( q ) = [ J Λ 1 J Λ 2 J Λ 3 J Λ 4 J Λ 5 J Λ 6 J Λ 7 ] 3 × 7 T
式中，JΛi为第i节刚性臂对于第7节的转换矩阵与位移矩阵形成的姿态分量。定义位置姿态联合适量为XE， X E = [ r t I e t I ] T ，则有 X ˙ E = J ( q ) q ˙ ，雅可比矩阵J(q)定义为
J ( q ) = [ J L ( q ) J Λ ( q ) ] T 6 × 7
由于J(q)不为方阵，因此求解各关节运动规律时需要用J的广义逆J+进行计算，为 J + = J T ( J ⋅ J T ) − 1 。
当刚性臂运动至奇异位置附近时， J ⋅ J T 的模值会趋于或等于0，而条件数则趋于一个极大值，并且这一过程中模值和条件数呈现连续变化。利用这一特点，通过设定合理阈值可以实现奇异位置的预警。 J ⋅ J T 矩阵模值ModJ表示为 M o d J = | J ⋅ J T | ，条件数κJ表示为 κ J = ‖ J ‖ ‖ J − 1 ‖ ，运用中取条件数的倒数
即 1 κ J 作为指标。两种指标在应用中同时使用，任何一种指标出现低于阈值均会触发报警，并进行紧急处置。
出现奇异位置预警后的处置方法如下：
步骤1：根据遥操作手控器记录的操控中每次沿刚性臂末端坐标系三个方向进动距离，得到第i次操控的距离矢量 L = [ L x , L y , L z ] ；
步骤2：当某次操控中模值或条件数倒数低于设定阈值时，触发报警，停止手控器输出；统计手控器前N次的操控结果，计算臂末端移动距离在三个方向上的移动距离之和，如图2所示，并计算三个方向上的投影角：
α x = ∑ i = 1 N L x i ∑ i = 1 N L x i 2 + L y i 2 + L z i 2 ， α y = ∑ i = 1 N L y i ∑ i = 1 N L x i 2 + L y i 2 + L z i 2 和 α z = ∑ i = 1 N L z i ∑ i = 1 N L x i 2 + L y i 2 + L z i 2 ；
步骤3：设定一个验证距离LT，计算得到该距离在三个方向的投影距离为(LT∙cos(αx), LT∙cos(αy), LT∙cos(αz))；
步骤4：采用刚性臂逆向运动模型，预测计算刚性臂末端分别沿三个方向移动距离LT∙cos(αx)、LT∙cos(αy)和LT∙cos(αz)后的模值ModJ与条件数倒数 1 κ J 的变化；
步骤5：迭代计算6步，得到两种特征指标的变化趋势进行分析，如果沿某一方向两种指标显著增大则说明刚性臂末端向该方向运动可以避免奇异位置，反之则不可取。
图2. 刚性臂末端坐标系中的操控距离投影示意
设定 J ⋅ J T 矩阵模值的报警阈值为Thmod= 8，条件数倒数的报警阈值为Thcond= 0.05。某次刚性臂B检视的操控过程中初始关节角为[−7.97, 90.0, 16.86, −111.31, −0.086, −96.56, 26.0] (单位：度)，对应第零次操控，锁定第二个关节角为90.0˚。随着操控的持续，模值与条件数倒数的变化如图3所示。当操控进行至第46步时，模值为5.8、小于报警阈值8，触发报警，此时条件数倒数为0.55，略大于设定阈值。操控过程中的刚性臂末端位置在三个坐标方向的位移变化如图4所示。计算得到操控过程中末端平均位移矢量为Lm= [0, 0.007, 0.033]，可知末端主要沿Z轴和Y轴方向运动，位移矢量与Z轴的夹角 α z ≈ 12 ˚ ，平均距离为0.337 m。从两类指标变化趋势看，在数值下降阶段均变化显著，适合作为预警指标。
图3. 操控过程中特征矩阵模值(左)与条件数倒数(右)的变化
图4. 操控过程中刚性臂末端位置移动距离的变化
图5. 沿不同操控方向的J∙JT模值的预测值变化
出现奇异位置报警后，分别沿+X，−X，+Y，−Y，+Z和−Z方向以验证操控距离LT= 0.337 m迭代计算6步 J ⋅ J T 的模值和条件数倒数的数值。沿−X和+Z方向计算1步，刚性臂便陷入奇异位置，模值和条件数倒数趋于0。预测得到其他四个方向的两类指标随预测步的变化如图5所示。可见，操控方向沿+X和−Z运动模值变化最为显著，意味着刚性臂可以迅速脱离奇异位置附近。而沿+Y方向操控模值变化幅度很小，−Y方向则导致模值进一步下降，因此沿两个方向的操控不可取。相对比模值的变化，条件数倒数的变化对预测步并不敏感，只有沿−Z方向操控会有显著的增大变化，如图6所示。
图6. 沿不同操控方向的J∙JT条件数倒数的预测值变化
针对机械臂操控过程中陷入奇异位置后地面处置难得问题，根据机械臂陷入奇异位置的逆运动学特点，提出来了一种基于雅可比矩阵模值和条件数的预警方法，以及基于距离投影的运动路径规避与处置方法，并进行了仿真验证。主要结论如下：
1) 相比于条件数， J ⋅ J T 矩阵模值的变化对奇异位置的预测计算过程更为敏感，更适合作为主要的刚性臂奇异位置报警指标；
2) 提出的基于操控正交投影法的处置方式合理可行，能够有效避开奇异位置，实现机械臂运动路径的重新规划，达到平稳操控目的。
程富强,王 安,夏长峰. 冗余机械臂奇异位置监测与路径规避方法Monitoring and Path Avoiding Method of Singular Position of the Redundant Manipulator[J]. 国际航空航天科学, 2024, 12(01): 21-27. https://doi.org/10.12677/JAST.2024.121004
https://doi.org/10.1109/ACCESS.2021.3056397