空间冗余机械臂零反作用最优路径规划

李文龙，孔祥龙，2，裘 俊，马 伟，张晓静

（1.上海卫星工程研究所，上海 200240；2.哈尔滨工业大学 航天学院，黑龙江 哈尔滨 150001；3.上海交通大学 航空航天学院，上海 200240）

0 引言

在纷繁复杂的空间任务中，空间机械臂越来越多地得到应用。由于太空环境具有潜在的致命危险，加上经常需要进行重复的操作，因此，空间机械臂成为空间操作任务中不可或缺的工具［1］。机械臂的运动学规划问题一直是研究的热点［2-5］。对于自由漂浮空间机械臂系统来说，系统的线动量和角动量都是守恒的，与固定基座机械臂系统不同的是，机械臂运动与基座运动是相互耦合的。也就是说，由于系统需要遵循动量守恒原理，那么机械臂动量的改变势必引起基座动量的变化［2-3］。机械臂每一根杆的运动都会产生反作用力和反作用力矩，通过各个关节传递到基座，从而引起基座的平动和转动，为了确保机械臂操作过程中的安全性和精确性，基座的受扰运动是我们所不期望的。例如，抓捕过程中需要确保抓捕目标始终位于飞行器视场范围内［6］。此外，基座的位置和方向会影响末端执行器的位置和方向，从而使得机械臂末端不能精确跟踪事先规划好的路径，末端位置的变化也就可能导致机械臂不能完成抓捕任务［7］。

为了使得在机械臂运动过程中航天器的位置和姿态都保持不变，那么需要消除或补偿所有的干扰力和干扰力矩。对于航天器的姿态运动来讲，有两种方法：第一种是利用主动姿态控制系统。姿态控制系统通过施加主动控制力矩从而使得航天器姿态相对于惯性坐标系保持不变。然而，在实际的空间机械臂操作过程中，姿态推力控制系统通常处于关闭状态［8］，一个主要原因是为了避免助推器突然点火时机械臂末端执行器与目标发生碰撞，如果依赖飞轮等执行机构，由于操作过程中机械臂动态变化较大，基座飞轮易饱和需卸载，需姿控推力系统介入；另一个主要原因则是为了节省有限的空间燃料。考虑到这些原因，就需要采取其他的减小姿态干扰的方法。第二个方法可以通过纯机械反作用补偿的方式，或者通过机械臂自身的运动来实现对载体航天器姿态的零干扰。通过机械反作用补偿存在一系列的缺点，如极大地增加了系统质量，且其补偿能力也是有限的［9］。为此，通过合理规划机械臂运动路径，使其对基座干扰达到最小，这个理念越来越受到研究人员的重视［10-12］。

DUBOWSKY 等［13］在VAFA 等［9］提出的干扰图法的基础上，对多自由度机械臂系统提出了增强干扰图法。该方法同样适用于冗余机械臂，其缺点是计算量大，占用内存高。戈新生等［14］结合各种控制理论与空间机械臂系统的非完整特性，对空间机械臂的轨迹进行研究，利用机械臂运动控制载体姿态。付宜利等［15］分析了载体姿态无扰情况下机械臂的运动特性，分析了其可达空间。QUINN 等［16］针对冗余机械臂系统提出一种优化算法，使得机械臂运动引起的反作用影响最小，其性能指标选取的是基座反作用力的平方加权，分别采用了局部和全局最优法求解问题。但是，以上方法并不是基于零反运动解析解的。从运动学的角度，NENCHEV等［17］针对自由漂浮基空间机械臂系统提出一种零反作用控制方法，其反作用零空间具有解析形式。以此为基础，PIERSIGILLI［18］和PUTILOVA［19］考虑冗余机械臂抓捕移动目标任务中机械臂最优路径规划问题，但未考虑关节约束问题，且采用的是间接法求解。间接法存在求解过程复杂、收敛域小、对初值估计精度要求高的缺点，尤其是对有路径约束的问题的求解有一定困难。高斯伪谱法［20］作为一种直接法具有求解精度高，且适用于具有各种带有动力学约束、路径约束和边界条件约束的非线性最优控制问题的特点，在工程应用中已获取了极大的成功。

本文针对已有研究存在的不足，提出一种基于冗余机械臂反作用零空间的最优路径规划算法。采用高斯伪谱法求解最优路径问题，在零反作用轨迹的解集中寻找满足边界条件和路径约束的使性能指标最优的机械臂运动规律。最后，通过数值仿真分析得到运动对基座姿态的干扰，以验证其有效性。

1 机械臂零反运动

1.1 机械臂运动学方程

空间链式冗余机械臂系统如图1 所示，系统由1 个基座航天器和n节刚性杆组成。图中，S0为基座本体系，SI为惯性系，Si（i=1，…，n）为臂杆连体坐标系，ro为基座质心位置矢量，rg为系统质心位置矢量，rog为基座质心到系统质心矢量，pi为臂杆i关节位置矢量，ai为臂杆i质心在其连体系中位置矢量，为臂杆i角速度矢量。

假设系统中各体为刚体，系统不受任何外力和外力矩作用，不考虑任何动量交换装置，如反作用飞轮、控制力矩陀螺等。基于这些假设，空间机械臂系统满足线动量和角动量方程，也即线动量P和角动量L为常值，可表示为

式中：M为航天器系统质量；E为单位矩阵；Hω为系统惯量；JTω为机械臂雅克比矩阵；Hωφ为基座与机械臂的耦合惯量；v0为基座线速度；ω0为基座角速度。

1.2 机械臂零反运动解

由于本文只考虑基座的姿态运动而不考虑其平动，因此，不失一般性，假设系统线动量P=0，此时式（1）中角动量方程部分可写为

从式（3）可以看出，为了满足机械臂运动对基座姿态的干扰为零，则必须使得反作用力矩Fm=0，也就要求耦合动量Lm为常值。考虑到Hωφ∈Rm×n，其中，m为基座自由度，n为机械臂关节自由度，当机械臂满足运动学冗余，也即n＞m时，满足耦合动量Lm=const 的关节运动的解为

式中：E∈Rn×n为n阶单位矩阵；（.）+表示矩阵伪逆；ξ为任意n维向量。

不失一般性，假设Lm=0，则有

将式（5）对时间积分，则可得到零反作用机械臂在关节空间中的运动路径，所有满足该条件的路径称为零反路径集。

2 最优路径规划

由式（5）可以看出，零反运动关节角速度由零空间投影算子和任意n维向量ξ共同决定。零空间投影算子仅与机械臂固有的质量特性和关节构型相关。因此，为了确定，需要设计ξ，一旦ξ给定，那么整个机械臂在其关节空间中的运动路径也就确定了。选取ξ的方法有很多，本文考虑关节角（机构物理限制）和关节角加速度（对应电机执行能力）约束，寻找一组满足给定性能指标的最优解。

定义状态量x=［ξTφT］T，控制变量为

取关节角约束为

式中：φmin=［φ1minφ2min…φnmin］T；φmax=［φ1maxφ2max…φnmax］T。将式（5）对时间求导，可得

取关节角加速度约束为

结合式（5）和式（6），优化问题的动力学方程可以写为

式中：

接下来将针对不同的目标函数，采用高斯伪谱法求解最优问题。

2.1 高斯伪谱法

高斯伪谱法是求解连续Bolza 型问题的直接法中配点法的一种，其基本原理是在一系列离散的Legendre Gauss 节点上，采用Lagrange 全局插值多项式来近似状态变量和控制变量。然后，通过对插值多项式的求导来近似动力学方程中状态变量对时间的导数，并使得离散点处满足动力学方程的右函数约束条件。性能指标中的积分项采用近似精度最高的Gauss 积分来近似。经过一系列变换，从而将本文的最优路径规划问题转化具有一系列代数约束的离散的非线性规划问题。

因此，本文的最优路径规划问题可以描述为：求解离散状态变量xi、控制变量ui，使对应的性能指标达到最小。

2.2 时间最优解

时间最优路径规划问题可描述为寻找控制变量u的一组解，使得具有形如

目标函数在满足如下约束条件时取最小值（式中t0为初始时刻，tf为终端时刻）：1）动态约束见式（10）；2）边界条件见式（7）；3）路径约束见式（9）；4）初始条件为初始时刻t0=0，关节角φ(t0)=φ0；5）终端约束为φ(tf)=φf。

2.3 沿轨迹最小加速度解

沿轨迹最小加速度最优路径规划问题可描述为给定终端时刻tf，寻找控制变量u的一组解，使得具有形如

目标函数在满足2.2 节中约束条件1）～5）的同时取最小值。

3 仿真算例

平面3 自由度自由漂浮空间机械臂系统如图2所示，由于只考虑基座的姿态运动，此时基座自由度m=1，机械臂自由度n=3，机械臂冗余自由度为n-m＞0，满足零反运动必要条件。机械臂和基座的质量特性参数、几何参数、初始构型以及期望达到的终端构型（见表1），在表1 中详细列出，给定的关节角约束以及关节角加速度约束同样在表1 中给出。

图2 平面3 自由度冗余空间机械臂Fig.2 Planar redundant space manipulator with 3 degrees of freedom

在仿真过程中，采用由弗洛里达大学的Rao等［21-23］开发开源免费软件包GPOPS（Gauss Pseudospectral Optimization Software）对最优问题进行求解。初始时刻姿态角和角速度都为0，给出了针对两个性能指标的仿真结果。

3.1 时间最优仿真结果

设置高斯节点数变化区间为4～12，容许偏差为10-4，求解过程中目标函数的梯度信息和约束方程的雅克比矩阵求解方式选为“有限差分”法，状态变量和控制变量的初始猜测值可在各自的约束区间内任意选取。考虑到关节和关节角加速度约束，选取ξ的上下限分别为［0.5 0.5 0.5］T和［-0.5 -0.5 -0.5］T，控制变量u的上下限分别为［0.1 0.1 0.1］T和［-0.1 -0.1 -0.1］T，最后求得机械臂最快到达终端构型的时间为tf，min=127.120 4 s。

表1 机械臂系统参数Tab.1 Parameters of the manipulator system

整个过程中各关节角的变化曲线如图3 所示。由图可见，各关节都在各自的容许范围内。

图3 关节角-时间最优Fig.3 Joint angle-time optimal

整个过程中各关节角加速度的变化曲线如图4所示。同样地，从图中可以看出各关节角加速度满足路径约束。

图4 关节角加速度-时间最优Fig.4 Joint acceleration-time optimal

为了证明在整个运动过程中机械臂运动对基座姿态没有影响，机械臂姿态角速率随时间变化的曲线如图5所示，姿态角速率量级为10-17rad/s。图6给出了机械臂运动对基座引起的反作用力矩，为10-15N⋅m 量级，也即干扰力矩为0，这也就说明基座姿态不受机械臂运动的干扰。

图5 基座角速率-时间最优Fig.5 Angular velocity of base-time optimal

图6 基座反作用力矩-时间最优Fig.6 Reaction torque of base-time optimal

3.2 沿轨迹最小加速度仿真结果

选取终端时刻tf=300 s，其他仿真参数的选取与时间最优规划问题相同。经仿真求解，最后得到的目标函数值为0.151 3。

整个过程中各关节角的变化曲线如图7 所示，整个过程中各关节角加速度的变化曲线如图8 所示。同样地，两者都满足约束条件。

从图7 和图8 中还可以看出，与时间最优问题不同的是，加速度最小目标下的轨迹更加平缓，关节角速度较小，所以到达期望状态所需的时间也更长。时间最优目标下加速度大部分时间处于正向最大或反向最大阶段，类似于Bang-Bang 控制，所以也能更快地达到期望状态。

机械臂姿态角速率随时间变化的曲线如图9 所示，机械臂运动对基座引起的反作用力矩如图10 所示。同样可以说明，基座姿态不受机械臂运动的干扰，从而验证了零反最优路径规划算法的有效性。

图7 关节角-最小加速度Fig.7 Joint angle-minimum acceleration

图8 关节角加速度-最小加速度Fig.8 Joint acceleration-minimum acceleration

图9 基座角速率-最小加速度Fig.9 Angular velocity of base-minimum acceleration

图10 基座反作用力矩-最小加速度Fig.10 Reaction torque of base-minimum acceleration

4 结束语

本文提出了一种空间冗余机械臂最优路径规划算法，算法在保证机械臂达到期望末端位置状态的同时对基座航天器姿态无干扰。基于冗余机械臂的反作用零空间，该算法求解得到的最优解是以零反作用机械臂运动的理论解析解为动力学约束条件的，因此，具有非常高的精度。此外，在采用高斯伪谱法求解最优路径解的过程中考虑了两个实际工程问题中的重要约束，分别为关节角运动范围物理约束以及驱动关节运动的电机输出加速度约束，因此，具有很高的工程实用价值。