冗余机械臂奇异位置监测与路径规避方法

Monitoring and Path Avoiding Method of Singular Position of the Redundant Manipulator

摘要: 机器人运动过程中机械臂位姿奇异问题是由于机械臂逆向运动学解算的伪逆算法固有缺陷导致的，一般会发生在机械臂肘部关节趋近于零、处于安全停靠构型以及其他未知的状态。陷入奇异点后，通常采取路径规划方式避开，处置过程存在状态研判困难、规划耗时、再次操控仍然容易陷入奇异等问题。针对此问题，根据机械臂陷入奇异位置的逆运动学特点，提出了一种基于雅可比矩阵模值和条件数的预警方法和基于距离投影的运动路径处置方法，并进行了仿真验证。结果表明雅可比矩阵模值对奇异位置发生过程更为敏感，适合作为监测参数。

Abstract: 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.

1. 引言

工业机器人是一种自动控制的、可再编程的多用途操作机械臂，一般由三个或以上的可编程关节组成 [1] 。近年来，机器人技术得到了快速发展。冗余自由度机械臂作为机器人行业中的主要分支，较于传统的六自由度机械臂具有高度的灵活性、可靠性和容错性，被广泛应用于社会的各个行业和领域 [2] [3] 。冗余机械臂之所以具有以上的优点，是因为这类机械臂独有的冗余特征：给定一个机械臂位姿，通常有多组逆解与其对应同。促使其不但能够完成首要任务，还能够更好的实现人机交互、规避奇异位姿等性能 [4] [5] 。机械臂遥操作过程中，特别是信息传输大时延条件下，保证遥操作过程的安全性和稳定性是机械臂操控任务顺利实施的基本条件。机械臂操控过程中往往面临器臂碰撞、臂位姿奇异、臂关节角速度超速等风险。

机械臂位姿奇异问题是由于机械臂逆向运动学解算的伪逆算法固有缺陷导致的。一旦机械臂运动至奇异区，会导致该算法失效，操控无法进行下去，一般会发生在机械臂肘部关节趋近于零、处于安全停靠构型以及其他未知的状态。由于冗余机械臂和运动学算法固有特性，机械臂奇异位置理论上有无穷多个。目前的遥操作系统尚未对机械臂位姿奇异问题形成有效的预警和处置方法。陷入奇异点后，通常采取路径规划方式避开，处置过程存在状态研判困难、规划耗时、再次操控仍然容易陷入奇异等问题。本研究针对此问题，根据机械臂陷入奇异位置的逆运动学特点，提出来了一种基于雅可比矩阵模值和条件数的预警方法和基于距离投影的运动路径处置方法，并进行了仿真验证。

2. 机械臂基本构型

机械臂采用7自由度近似对称构型，一端安装机械臂基座，一端安装快换装置。机械臂基座用于机械臂与卫星舱体连接，快换装置用于实现末端操作工具的更换和多模手眼相机的安装。机械臂由肩部三关节、肘部关节、腕部三关节、臂杆、快换装置、左/右多模手眼相机、末端监测相机、锁紧释放机构，以及热控组件等几部分组成。肩部关节控制器用于控制肩部三关节及肘部关节。腕部关节控制器用于控制腕部三关节和快换装置以及实现与末端工具的通讯。表1所示为机械臂MDH参数，建立机械臂各杆件的坐标系如图1所示，Z轴为旋转轴，α为杆件扭角，a为杆件偏置，d为杆件长度，θ为杆件旋转角，{0}为基座坐标系。

Table 1. The MDH parameters of the seven DOF manipulator

表1. 七自由度机械臂MDH参数

Figure 1. The coordinates sketch of the seven DOF manipulator

图1. 七自由度机械臂各杆件坐标系示意

3. 运动学解算原理

3.1. 正向运动学

从第i轴，到第i−1轴的坐标变换 ${}_i^{i-1}A$ 为

${}_i^{i-1}A=rotate\left(a_i,{\alpha }_i\right)\cdot rotate\left(d_i,{\theta }_i\right)=\left[\begin{array}{cccc}\text{C}\left({\theta }_i\right)& -\text{S}\left({\theta }_i\right)& 0& a_i\\ \text{S}\left({\theta }_i\right)\text{C}\left({\alpha }_i\right)& \text{C}\left({\theta }_i\right)\text{C}\left({\alpha }_i\right)& -\text{S}\left({\alpha }_i\right)& -d_i\cdot \text{S}\left({\alpha }_i\right)\\ \text{S}\left({\theta }_i\right)\text{S}\left({\alpha }_i\right)& \text{C}\left({\theta }_i\right)\text{C}\left({\alpha }_i\right)& \text{C}\left({\alpha }_i\right)& d_i\cdot \text{C}\left({\alpha }_i\right)\\ 0& 0& 0& 1\end{array}\right]$ (1)

其中， $\text{C}\left({\theta }_i\right)=\text{cos}\left({\theta }_i\right)$ ， $\text{S}\left({\theta }_i\right)=\text{sin}\left({\theta }_i\right)$ ， $\text{S}\left({\alpha }_i\right)=\text{sin}\left({\alpha }_i\right)$ ， $\text{C}\left({\alpha }_i\right)=\text{cos}\left({\alpha }_i\right)$ 。由此，正运动学问题变成了7个字问题，即7个齐次变换矩阵。可得机械臂末端坐标系{7}相对于基座坐标系{0}的位姿，应用齐次变换矩阵的串接性质可得到机械臂支路的齐次变换矩阵：

${}_7^{0}A={\displaystyle \underset{k=1}{\overset{7}{\prod }}{}_k{}^{k-1}A}=\left[\begin{array}{cccc}{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\end{array}\right]$

式中， $\left[\begin{array}{ccc}{r}_{11}& r_{12}& r_{13}\\ r_{21}& r_{22}& r_{23}\\ r_{31}& r_{32}& r_{33}\end{array}\right]$ 为腕部坐标系相对于基座坐标系旋转矩阵，三行分别描述了坐标轴XYZ在基座坐标系三个坐标轴上的投影。[p_x p_y p_z]^T为腕部坐标系相对于基座坐标系的平移矢量。

3.2. 逆向运动学

机械臂末端位置运动学方程为 [6] [7]

${\stackrel{\dot{}}{r}}_{\text{tI}}=J_{\text{L}}\left(q\right)\stackrel{\dot{}}{q}$ (2)

式中， ${\stackrel{\dot{}}{r}}_{\text{tI}}\in R^m$ 为机械臂末端在笛卡尔坐标系下的广义速度， $q\in R^n$ 、 $J_{\text{L}}\left(q\right)\in R^{m\times n}$ 分别为本体与机械臂联合角位移列向量和位置雅可比矩阵，m = 3为机械臂末端运动空间维数，即三个位置方向，n = 7为机械臂自由度个数。q和J_L(q)形式为

$q={\left[\begin{array}{ccccccc}{\theta }_1& {\theta }_2& {\theta }_3& {\theta }_4& {\theta }_5& {\theta }_6& {\theta }_7\end{array}\right]}^{\text{T}}$

$J_{\text{L}}\left(q\right)={\left[\begin{array}{ccccccc}{J}_{\text{L1}}& J_{\text{L2}}& J_{\text{L3}}& J_{\text{L4}}& J_{\text{L5}}& J_{\text{L6}}& J_{\text{L7}}\end{array}\right]}_{3\times 7}^{\text{T}}$

其中， ${\theta }_i\left(i=\text{1},\cdots ,\text{7}\right)$ 为机械臂的关节角，J_Li为第i节刚性臂相对于其他刚性臂关节的转换矩阵与位移矩阵形成的位置分量。刚性臂末端的姿态运动学方程为

${\stackrel{\dot{}}{e}}_{\text{tI}}=J_{\text{A}}\left(q\right)\stackrel{\dot{}}{q}$

式中， ${\stackrel{\dot{}}{e}}_{\text{tI}}$ 为刚性臂末端相对于本体系的姿态角速度，J_Λ(q)为姿态雅可比矩阵，且

$J_{\Lambda }\left(q\right)={\left[\begin{array}{ccccccc}{J}_{\Lambda \text{1}}& J_{\Lambda \text{2}}& J_{\Lambda \text{3}}& J_{\Lambda \text{4}}& J_{\Lambda \text{5}}& J_{\Lambda \text{6}}& J_{\Lambda \text{7}}\end{array}\right]}_{3\times 7}^{\text{T}}$

式中，J_Λi为第i节刚性臂对于第7节的转换矩阵与位移矩阵形成的姿态分量。定义位置姿态联合适量为X_E， $X_{\text{E}}={\left[\begin{array}{cc}{r}_{tI}& e_{tI}\end{array}\right]}^{\text{T}}$ ，则有 ${\stackrel{\dot{}}{X}}_{\text{E}}=J\left(q\right)\stackrel{\dot{}}{q}$ ，雅可比矩阵J(q)定义为

$J\left(q\right)={\left[\begin{array}{cc}{J}_L\left(q\right)& J_{\Lambda }\left(q\right)\end{array}\right]}^{\text{T}}{}_{6\times 7}$

由于J(q)不为方阵，因此求解各关节运动规律时需要用J的广义逆J⁺进行计算，为 $J^{+}=J^{\text{T}}{\left(J\cdot J^{\text{T}}\right)}^{-1}$ 。

4. 奇异位置预警与处置方法

当刚性臂运动至奇异位置附近时， $J\cdot J^{\text{T}}$ 的模值会趋于或等于0，而条件数则趋于一个极大值，并且这一过程中模值和条件数呈现连续变化。利用这一特点，通过设定合理阈值可以实现奇异位置的预警。 $J\cdot J^{\text{T}}$ 矩阵模值Mod_J表示为 $Mo{d}_{\text{J}}=\left|J\cdot J^{\text{T}}\right|$ ，条件数κ_J表示为 ${\kappa }_{\text{J}}=\Vert J\Vert \Vert J^{-1}\Vert$ ，运用中取条件数的倒数

即 $\frac{1}{{\kappa }_{\text{J}}}$ 作为指标。两种指标在应用中同时使用，任何一种指标出现低于阈值均会触发报警，并进行紧急处置。

出现奇异位置预警后的处置方法如下：

步骤1：根据遥操作手控器记录的操控中每次沿刚性臂末端坐标系三个方向进动距离，得到第i次操控的距离矢量 $L=\left[L_{\text{x}},L_{\text{y}},L_{\text{z}}\right]$ ；

步骤2：当某次操控中模值或条件数倒数低于设定阈值时，触发报警，停止手控器输出；统计手控器前N次的操控结果，计算臂末端移动距离在三个方向上的移动距离之和，如图2所示，并计算三个方向上的投影角：

${\alpha }_{\text{x}}=\frac{{\displaystyle \underset{i=1}{\overset{N}{\sum }}{L}_{\text{x}i}}}{{\displaystyle \underset{i=1}{\overset{N}{\sum }}\sqrt{L_{\text{x}i}^{\text{2}}+L_{\text{y}i}^{\text{2}}+L_{\text{z}i}^{\text{2}}}}}$ ， ${\alpha }_{\text{y}}=\frac{{\displaystyle \underset{i=1}{\overset{N}{\sum }}{L}_{\text{y}i}}}{{\displaystyle \underset{i=1}{\overset{N}{\sum }}\sqrt{L_{\text{x}i}^{\text{2}}+L_{\text{y}i}^{\text{2}}+L_{\text{z}i}^{\text{2}}}}}$ 和 ${\alpha }_{\text{z}}=\frac{{\displaystyle \underset{i=1}{\overset{N}{\sum }}{L}_{\text{z}i}}}{{\displaystyle \underset{i=1}{\overset{N}{\sum }}\sqrt{L_{\text{x}i}^{\text{2}}+L_{\text{y}i}^{\text{2}}+L_{\text{z}i}^{\text{2}}}}}$ ；

步骤3：设定一个验证距离L_T，计算得到该距离在三个方向的投影距离为(L_T∙cos(α_x), L_T∙cos(α_y), L_T∙cos(α_z))；

步骤4：采用刚性臂逆向运动模型，预测计算刚性臂末端分别沿三个方向移动距离L_T∙cos(α_x)、L_T∙cos(α_y)和L_T∙cos(α_z)后的模值Mod_J与条件数倒数 $\frac{1}{{\kappa }_{\text{J}}}$ 的变化；

步骤5：迭代计算6步，得到两种特征指标的变化趋势进行分析，如果沿某一方向两种指标显著增大则说明刚性臂末端向该方向运动可以避免奇异位置，反之则不可取。

Figure 2. The sketch of the projected operational distance in the manipulator terminal coordinates

图2. 刚性臂末端坐标系中的操控距离投影示意

5. 仿真验证

设定 $J\cdot J^{\text{T}}$ 矩阵模值的报警阈值为Th_mod = 8，条件数倒数的报警阈值为Th_cond = 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所示。计算得到操控过程中末端平均位移矢量为L_m = [0, 0.007, 0.033]，可知末端主要沿Z轴和Y轴方向运动，位移矢量与Z轴的夹角 ${\alpha }_{\text{z}}\approx \text{12}˚$ ，平均距离为0.337 m。从两类指标变化趋势看，在数值下降阶段均变化显著，适合作为预警指标。

Figure 3. The module value (left) and reciprocal of the conditional number (right) of the jacobian matrix during operation

图3. 操控过程中特征矩阵模值(左)与条件数倒数(右)的变化

Figure 4. The variation of the moving distance of the manipulator terminal during operation

图4. 操控过程中刚性臂末端位置移动距离的变化

Figure 5. The prediction value variation of the module J∙J^T value along different operation directions

图5. 沿不同操控方向的J∙J^T模值的预测值变化

出现奇异位置报警后，分别沿+X，−X，+Y，−Y，+Z和−Z方向以验证操控距离L_T = 0.337 m迭代计算6步 $J\cdot J^{\text{T}}$ 的模值和条件数倒数的数值。沿−X和+Z方向计算1步，刚性臂便陷入奇异位置，模值和条件数倒数趋于0。预测得到其他四个方向的两类指标随预测步的变化如图5所示。可见，操控方向沿+X和−Z运动模值变化最为显著，意味着刚性臂可以迅速脱离奇异位置附近。而沿+Y方向操控模值变化幅度很小，−Y方向则导致模值进一步下降，因此沿两个方向的操控不可取。相对比模值的变化，条件数倒数的变化对预测步并不敏感，只有沿−Z方向操控会有显著的增大变化，如图6所示。

Figure 6. The prediction value variation of the reciprocal of the conditional number of the J∙J^T along different operation directions

图6. 沿不同操控方向的J∙J^T条件数倒数的预测值变化

6. 结论

针对机械臂操控过程中陷入奇异位置后地面处置难得问题，根据机械臂陷入奇异位置的逆运动学特点，提出来了一种基于雅可比矩阵模值和条件数的预警方法，以及基于距离投影的运动路径规避与处置方法，并进行了仿真验证。主要结论如下：

1) 相比于条件数， $J\cdot J^{\text{T}}$ 矩阵模值的变化对奇异位置的预测计算过程更为敏感，更适合作为主要的刚性臂奇异位置报警指标；

2) 提出的基于操控正交投影法的处置方式合理可行，能够有效避开奇异位置，实现机械臂运动路径的重新规划，达到平稳操控目的。