一种基座姿态可控空间机器人的通用避奇异算法

赵龙泽，佘浩平，*，黄良伟，黄龙飞

（1.北京理工大学 宇航学院，北京 100081； 2.中国空间技术研究院 钱学森空间技术实验室，北京 100094）

对于基座姿态可控的关节型空间机器人，在对其进行笛卡儿空间连续运动规划的过程中，需要利用基于雅可比矩阵求逆或伪逆的速度级运动学求解来实现。然而，当机器人经过一些特定的臂型时，不可避免地会遇到运动学奇异，主要是因为雅可比矩阵出现了病态，此时如果继续采用经典的速度级逆运动学求解方法，则在机械臂的奇异点附近会导致规划关节角速度和角加速度出现无穷大的极端情况，机械臂会因此出现振动，影响机械臂的轨迹跟踪精度，同时也不利于机械臂的关节运动控制，甚至造成控制算法失效［1］。因此，寻求一种有效的回避算法是极为重要的。

早期常采用伪逆法进行机械臂奇异回避的路径规划，该方法虽然能够解决一些超定或欠定的问题，但关节角的解仍会在机械臂的奇异区内出现跳变，造成较大误差［2］。随后Nakamura等［3］和Wampler［4］分别提出了阻尼最小方差法（DLS）和奇异性鲁棒逆解（SRI）的方法。Chiaverini［5］对SRI方法进行了改进，采用“阻尼最小方差+数值滤波”的方法，通过判定最小奇异值施加阻尼系数，提升了规划精度。SRI方法虽然能够有效保障机械臂在奇异区内关节角度的连续性和有限性，但仍存在机械臂末端跟踪精度较差的问题，且由于需要对雅可比矩阵进行实时的SVD分解，运算量也较大。杨震等［6］引入雅可比矩阵的条件数作为DLS法中选取阻尼系数的衡量指标，有效提高了求解精度，但由于其需要频繁地对雅可比矩阵进行奇异值分解，极大地提高了计算量。董伯麟和彭航［7］给出了雅可比矩阵条件数的一个更小的上界估计式，提高了运算速度，但牺牲了一定的位姿精度。

Cheng等［8］以PUMA型机械臂为研究对象，提出了“奇异分离+紧密二次型规划”（SCIQP）法，避免了对雅可比矩阵进行实时SVD分解，有效减少了算法的运算量。徐文福等［9］针对PUMA型机器人的特点，对奇异分离的算法进行了改进，提出了“奇异分离+阻尼倒数”的方法，采用阻尼倒数代替普通倒数，仅牺牲末端部分方向的精度，以较小的运算量完成了奇异回避。崔洪新等［10］采用指数级阻尼倒数代替普通阻尼倒数的方法，进一步减小了跟踪误差。刘成良等［11］采用协调控制的算法回避了机器人的腕部奇异。Choi等［12］引入高斯分布阻尼因子，进一步提高了机械臂在奇异点附近的算法稳定性，但该算法本身就存在一定的不连续性。吴戈等［13］对高斯分布阻尼因子进行了改进，减小了跟踪误差，但鲁棒性并不高。Qiu等［14］通过引入与最小奇异值相关的修正向量，从而控制规划角速度，其性能仅受最小奇异值下限选择的影响，但损失了一定的实时性。Nanos和Papadopoulos［15-16］从 初 始 机 械 臂 的 关 节角和基座位姿出发，通过合理设置初值的方法避免了雅可比矩阵的奇异，但该方法未考虑基座受到扰动的情况。

本文针对不同自由度的基座姿态可控空间机械臂，在进行一般建模后，通过建立雅可比矩阵，进行速度级运动学逆解分析，在进行路径规划的同时判断雅可比矩阵行列式的值，给出奇异判定条件，从而判定机械臂的奇异区。随后，在奇异区内提取各关节角的微分项，通过设计二次拟合方法在奇异区内对关节角进行分段规划，最终实现了一种通用性较强的奇异回避算法。

1 运动学模型分析

1.1 基座姿态可控空间机械臂的建模

在惯性坐标系下建立空间机械臂的一般模型，如图1所示。空间机械臂由n个仅有一个自由度的旋转关节和n+1个刚体杆件串联而成，其中航天器平台记为机械臂的基座，记为连杆0，连杆i为空间机械臂所对应的第i个杆件，关节i为连接第i-1个连杆与第i个连杆的关节。为方便问题分析，可对模型做出以下几点假设：①可以同时对基座姿态及各关节力矩进行控制，实时保证基座姿态可控；②忽略模型的柔性，将模型的所有部分都视为刚体；③忽略各项外力的影响［17］。

图1 空间机械臂的一般模型Fig.1 General model of a space manipulator

图1中的符号说明如下：B0为空间机器人的基座；Bi（i=1，2，…，n）为机械臂的第i个连杆；Ji（i=1，2，…，n）为机械臂的第i个关节；Ci（i=0，1，2，…，n）为第i个连杆的质心；OI为惯性坐标系的原点；ΣI为惯性坐标系；ΣE为机械臂末端执行器所固连的坐标系；Σi（i=0，1，2，…，n）为与连杆i固连的坐标系，本文采用刚体固连坐标系的建立原则；ki（i=1，2，…，n）为Ji旋转方向的单位矢量；rg为空间机械臂系统质心在惯性坐标系中的位置矢量；ri（i=1，2，…，n）为Bi的质心位置矢量；pi（i=0，1，2，…，n）为关节i的位置矢量；pe为机械臂末端位置矢量；θi（i=1，2，…，n）为关节i的关节角；Θ为由θi到θn依次排列组成的关节角向量；b0为Σ1的原点在Σ0坐标系中的位置矢量；ai和bi（i=1，2，…，n）分别为从关节i指向连杆i质心的位置矢量和从连杆i质心指向关节i+1的位置矢量。

定义叉乘操作数：若s=［x y z］T，则

由式（1）可得，矢量叉乘运算表示为

各连杆质心的线速度vi可表示为

式中：v0、ω0分别为基座线速度、角速度。

机械臂的末端线速度ve可表示为

末端角速度ωe可表示为

将式（4）与式（5）联立，可得

式中：Jb、Jm分别为基座运动雅可比矩阵、机械臂运动雅可比矩阵。

由于空间机械臂系统满足线动量守恒，则有

式中：P为系统的线动量；mi（i=0，1，2，…，n）为刚体i的质量。

将式（3）代入式（7）并整理，可得

系统总质量为

式中：M为系统总质量。

由假设③可知，系统不受任何外力，故系统的质心为

将式（8）代入式（7），可得

将式（9）和式（11）代入式（8），可得系统的线动量守恒方程为

式中：

由于基座姿态是指处于受控状态，基座角速度可近似忽略，即

将式（15）代入式（12），可得

将式（15）、式（16）代入式（6），可得

采用虚拟机械臂法［18］建立基座受控空间机器人的微分运动方程为

联立式（17）和式（18），可得

1.2 奇异性分析

2 基于“微分项提取＋二次拟合”的奇异回避算法

2.1 总体思路

在对机械臂进行笛卡儿路径规划时，已经预先得到每一迭代步的末端位姿状态向量。由第1节分析可知，在机械臂进行笛卡儿路径跟踪时，通过确定关节角发生跳变的临界点，从而判定奇异区的关节角范围，即可确定在雅可比矩阵不发生奇异的情况下其行列式所对应的阈值。

在确定奇异区的关节角范围后，实时读取每一迭代步已预先规划好的末端位姿状态向量，采用基于Newton-Raphson迭代法的机械臂逆运动学的数值解法可以得到奇异区内任意迭代步所对应的关节角［19］。随后，采用一种“微分项提取+二次拟合”的方法，对机械臂在奇异区内的关节角序列进行拟合，最终得到完整的奇异区角度序列和角速度序列。在脱离奇异区后，保持期望末端位姿不变，以当前末端位姿矩阵为初始时刻末端位姿，重新进行笛卡儿空间内的连续位姿跟踪，最终完成奇异回避任务。

2.2 机械臂逆运动学的数值解法

在本文所述的奇异回避算法中，需要对奇异区内已规划好的末端位姿进行运动学求逆，得到关节角，从而为奇异区内的关节角拟合提供输入。

对任意自由度的空间机械臂，已知机械臂当前时刻的关节角序列qr，即可得到其所对应末端位姿矩阵为

假设期望末端位姿状态变量为

可得到末端姿态矩阵为

期望末端位置矢量为

当前时刻与终止时刻的末端位置矢量差值为

记下当前时刻的姿态矩阵 Rr=［nrorar］与终止时刻的期望姿态矩阵Rf=［nfofaf］，计算由Rr到Rf的姿态变换的欧拉轴角参数（kf，φf），由此计算当前时刻与终止时刻的末端姿态矢量差值Δe为

联立式（26）、式（27）和式（20），得到基于速度级的逆运动学求解方程为

机械臂逆运动学的数值解法算法流程如图2所示。

图2 机械臂逆运动学的数值解法算法流程Fig.2 Flowchart of inverse kinematics numerical solution algorithm of manipulator

需要说明的是，对于相同的末端位姿矩阵，可能存在关节角的多解情况。本文算法所得到的关节角迭代值与给定初始值的相关程度较大，因此在使用本文算法时，应保证在关节角初值的邻域内至少存在一组解，防止在迭代过程中出现角度突变。

2.3 奇异回避算法的实现流程

2.3.1 奇异区的判断

需要指出，设定εΔq的值越大，奇异区越小［20］。

2.3.2 进入奇异区后的机械臂路径规划

步骤1 以机械臂当前关节角为输入，进行笛卡儿空间连续位姿跟踪，若不出现奇异，则转步骤6，反之转步骤2。

步骤2 判定机械臂进入奇异区后，读取上一迭代时刻的关节角qj-1，作为2.2节所述算法的初始关节角输入。

步骤3 初始化机械臂末端位姿矩阵误差阈值εT，每隔k步读取一次奇异区内所规划的末端位姿状态变量Xek，并记下相邻2次读取时间间隔Δt，作为2.2节所述算法的期望末端位姿矩阵输入，可得到期望关节角qek。

将式（31）代入式（30），可得终端时刻微分项方程组为

式（32）提取出了期望关节角的微分项，在一些特殊情况下，用式（30）所得到的关节角速度曲线会出现龙格现象［21］，同样会造成关节角突变问题，因此需要对关节角进行二次拟合，避免此种问题的产生。

式中：ti为奇异区内第i次迭代时所对应的的仿真时间。令

假设θ″（t）在［ti，ti+1］区间内为线性函数，则

将式（35）积分2次，可得

由式（36）可得

联立式（36）和式（37），可得

记

联立式（36）～式（38），可得

由式（40）和式（41）整理，可得

通过求解式（36）的方程组，即可完成所有I=［ti，ti+1］，i=0，1，2，…，l-1区间内的关节角度、角速度和角加速度序列的求解。

需要指出，k的值越小，跟踪精度越高，同时计算量也会相应增大。

步骤5 记录机械臂在奇异区内的每一迭代步的机械臂末端位姿Td，若满足：

则停止规划，输出机械臂的关节角序列。反之，规划继续进行，直至离开奇异区后，转步骤1。

步骤6 记录全部规划过程的关节角和关节角速度序列，完成机械臂的路径跟踪。

经过上述6个步骤，可完成机械臂在笛卡儿连续位姿跟踪过程中的奇异回避。空间机械臂奇异回避算法流程如图3所示。

图3 空间机械臂奇异回避算法流程Fig.3 Flow-chart of singularity avoidance algorithm for space manipulator

3 仿真验证

3.1 机械臂模型及仿真参数设置

本文以常用的6R空间机械臂为研究对象，其系统模型如图4所示，图4中坐标系基于DH方法创建。

图4 6R机械臂的结构简图Fig.4 Structure of 6R manipulator

文献［19］给出了其质量特性参数，其中L1、L4、L6、L7的值分别为0.30、0.15、0.10、0.234 5 m，d1、d2、d4的值分别为0.30、0.15、0.70 m，a2、a3分别为0.83、0.03 m。

表1 6R机械臂的DH参数Table 1 DH parameters of 6R manipulator

3.2 算法校验

本文采用直线位姿跟踪，以梯形速度插值规划机械臂的末端位姿，验证本文算法的奇异回避效果。

设定基座姿态为ψb=［0，0，0］，初始时刻的系统关节角为

Θ=［47.61° 110.2° -78.07° 85.23°-30° 77.68°］

系统初始时刻的末端位姿Pe0及期望末端位姿Pef分别为

Pe0=［0.384 6 m -0.384 6 m 1.283 0 m 55.67° 61.10° -18.13°］

Pef=［0.597 7 m -0.129 3 m 1.306 5 m 51.86° 51.92° 37.49°］

式中：位姿向量的前3项为机械臂末端相对于基坐标系的位置坐标；后3项为末端姿态欧拉角，设定规划时间为20 s，加减速时间为4 s。

采用经典笛卡儿路径跟踪算法，得到机械臂各关节的角速度曲线，如图5所示。可见，在8.8 s左右，关节角速度开始出现跳变，仿真过程出现奇异。设定εΔq=5，εT=10-8，判定奇异区在8.8～11.2 s区间内。选取k=1，在采用2.3节所述的奇异回避算法进行重新规划后，关节角速度曲线如图6所示。

图5 未进行奇异回避时机械臂各关节角速度Fig.5 Angular velocity of each joint of manipulator without singularity avoidance

在进入奇异区后，发生奇异的关节角速度仍会出现小幅振荡，这是由于在进入奇异区以前，雅可比矩阵已经逐渐趋于病态，随着迭代的进行出现误差积累，在进入奇异区后，机械臂需要较快地调整关节角，以减小迭代误差，完成位姿跟踪。

由图6可见，机械臂在经过奇异区时，关节1到关节3的角速度未受影响，关节5的角速度出现了［-5°，3°］区间内的小幅振荡，原本发生奇异的关节4和关节6的角速度出现了在［-5°，3°］区间内的小幅振荡，但其幅值仍然得到了明显的控制，避免了奇异的发生。

图6 进行奇异回避后的机械臂各关节角速度Fig.6 Angular velocity of each joint of manipulator after singularity avoidance

分别选取k=1、5、10时，代入2.3节所述算法中，得到的末端位置跟踪误差和末端姿态跟踪误差曲线分别如图7和图8所示。

由图7和图8可见，本文算法可以较好地完成连续位姿跟踪任务。k值选取越小，机械臂在刚好进入奇异区时的跟踪误差越大，这同样是由于迭代过程中已经产生误差积累的缘故，上文已进行详细说明。在奇异区内经过短暂规划后，其末端位姿跟踪精度则随着k值的减小显著提升。

图7 k取不同值时末端位置误差对比Fig.7 Comparison of terminal position errors when k is taken as different values

图8 k取不同值时末端姿态误差对比Fig.8 Comparison of terminal attitude errors when k is taken as different values

实验结果表明，本文算法能够实现回避基座可控空间机械臂的奇异，完成预期跟踪任务。

4 结 论

1）与传统的基于奇异分离和阻尼倒数的算法相比，本文算法可适用于不同自由度、不同空间构型的空间机械臂，并能够以较小的跟踪误差完成机械臂的奇异回避，具备通用性。

2）通过合理设定各项参数，本文算法可以以不同的计算时间和跟踪精度完成奇异回避问题的求解，使用者可以根据计算机性能进行权衡，选取适用于自己的参数取值。

3）由于本文算法采取在奇异区内分段规划的策略对末端轨迹进行规划，在期望末端位姿处在奇异区内时，机械臂仍能够完成位姿跟踪任务。