空间机械臂系统轨迹规划仿真分析

王一全，王 敏，杨 闻，王兴龙

（中国空间技术研究院 通信卫星事业部，北京 100094）

0 引言

随着世界各国空间利用的广度和深度逐渐拓展，对航天器的研制需求不断提高。一方面，运载火箭整流罩包络和发射重量的限制，很难满足超大型卫星的研制需要；另一方面，传统卫星从发射到寿命结束的一次性工作方式，已难适应对航天器的灵活性和发射效益要求。在这种情况下，空间在轨组装卫星系统成为未来空间系统发展的一个重要方向。相对于传统卫星平台而言，在轨组装卫星系统的主要特点是可重构，包括整星构型的重构、功能的重构以及业务重构等。空间机械臂是航天器实现在轨重组技术的关键执行部件，用于实现目标部件的捕捉、转移和释放等功能，同时也可用于空间碎片清理以及空间环境探测。但是在空间微重力环境下，空间机械臂在执行任务时会对星本体产生耦合扰动，且受到空间环境中不确定因素的干扰。国内外学者对空间机械臂模型等进行了大量研究[1-6]，但给出的具体动力学模型以及相关实验结论较少。

本文以空间三自由度机械臂为研究对象，采用适用于空间微重力环境的拉格朗日方程，推导机械臂系统的动力学方程，仿真得到不同关节力矩下各关节转角的动力学响应，分析并验证动力学模型的准确性，旨在为空间在轨服务的机械臂系统设计提供参考。

1 空间机械臂动力学建模

1.1 动力学建模基本假设

建模过程中对于机械臂的处理过程需要用到以下基本假设：

1）空间机械臂的臂杆视为刚性杆件，不考虑臂杆的柔性变形和振动。

2）空间机械臂的关节视为铰链连接，不考虑关节间隙损耗等影响，不研究关节内部结构，只计算关节的输出转角和驱动力矩；关节质量作为相应臂杆质量的一部分进行计算，臂杆之间的关节处可视为机械臂末端的一个质点。

3）整个机械臂系统（含基座）均视为处在空间微重力的环境中，故在动力学建模中不考虑重力梯度力矩和其他环境力矩的影响。

4）空间目标捕捉过程分为逼近阶段、捕获阶段以及稳定调整阶段。由于在捕获阶段，机械臂系统会与目标物接触碰撞，碰撞产生的影响不仅与目标物的参数特性有关，也会对整个机械臂产生一个反向作用力，使所研究问题无法讨论，且该碰撞过程不在轨迹规划任务需求中，故所讨论情况只研究机械臂从初始位置平稳到达目标位置的逼近阶段，不考虑运动过程中的外部碰撞等干扰。

5）在捕捉过程中，假设机械臂系统初始的角度和角速度均为0，运动到目标点时刻的速度也为0，此假设可作为动力学模型的边界条件。

1.2 研究构型描述

由于不同构型机械臂所建模型不同，本文采用类似人手的三自由度机械臂。如图1所示，该机械臂为具有三自由度的链式结构，该三自由度机械臂构型3个关节分别用3个直流力矩电机驱动，电机轴与旋转方向已在图中标出。

图1 三自由度机械臂构型示意Fig.1 Schematic diagram of the 3-DOF manipulator

臂杆1：绕着竖直方向旋转，其转角记为θ1，运动范围为180°，杆长记为l1，质心位置记为l1c，质量记为m1，转动惯量记为I1；

臂杆2：在纸面内旋转，其转角记为θ2，运动范围为180°，杆长记为l2，质心位置记为l2c，质量记为m2，转动惯量记为I2；

臂杆3：在纸面内旋转，其转角记为θ3，运动范围为180°，杆长记为l3，质心位置记为l3c，质量记为m3，转动惯量记为I3。

机械臂的主要技术参数和质量参数分别参见表1和表2。

表1 机械臂主要技术参数Table 1 Main technical parameters of the manipulator

表2 机械臂质量参数特性Table 2 Quality characteristics of the manipulator

1.3 拉格朗日方程推导

拉格朗日方程是基于能量平衡的方程，相对于牛顿-欧拉公式，适合于相互约束下的多连杆运动的分析，利用广义坐标系以功和能来表达系统运动，不做功的力和内部约束力将会自动消除，可以直接导出动力学完整形式的方程式。通常把拉格朗日函数L定义为系统的动能T和位能P之差，即

T和P可以用任何方便的坐标系来表示。由于本文研究对象为空间刚性机械臂，处于空间微重力环境下，重力不对系统产生作用，故而可不考虑其系统的位能和势能P，只考虑动能T，因此根据式(1)求拉格朗日函数

通过推导可得最后方程形式

其中Ti为第i杆的动能；q为广义变量；I为转动惯量。

1.4 机械臂的动力学建模

已知各个臂杆的质心速度和质心角速度，带入到拉格朗日方程中，可得系统的动力学方程。计算机械臂系统动能为

整理后可得简化结果为

臂杆1的关节驱动力矩为

臂杆2的关节驱动力矩为

臂杆3的关节驱动力矩为

将以上3个臂杆的驱动力矩方程整理在一起，即得到系统的动力学方程。该方程可以表达系统各个关节的驱动力矩与各个关节转角的角度、角速度和角加速度间的显式函数关系。

2 动力学模型仿真

基于1.4节求出的动力学方程，可将其在MatLab软件中进行动力学仿真，步骤为：1）设置仿真参数；2）设置仿真时间为5 s；3）设置仿真步长为0.1 s；4）设置各个初始值为0。

2.1 已知驱动力矩，求解角度和角速度

2.1.1 只存在臂杆1的驱动力矩的仿真结果

给定臂杆1的关节驱动力矩，同时令臂杆2和臂杆3的关节驱动力矩为0，所得仿真结果如图2所示。

图2 只存在驱动力矩1条件下臂杆转动角度和角速度随时间的变化Fig.2 The changes of angle and angular velocity against the time in the mere presence of driving force 1

在该种情况下，只存在驱动力矩1，可以看出臂杆1的转动角速度是均匀递增的，此时臂杆2与臂杆3的转动角度和角速度均不发生变化，因为臂杆1的转角所在平面与臂杆2和臂杆3的转角所在平面正交，所以转角间不会发生动力学耦合，故而不会产生相互影响，与实际相符合。

2.1.2 只存在臂杆2的驱动力矩的仿真结果

给定臂杆2的关节驱动力矩，同时令臂杆1和臂杆3的关节驱动力矩保持0，所得仿真结果如图3所示。

图3 只存在驱动力矩2条件下臂杆转动角度和角速度随时间的变化Fig.3 The changes of angle and angular velocity against the time in the mere presence of driving force 2

在该种情况下，只存在驱动力矩2，可以看出臂杆1的转动角速度不变，而臂杆2与臂杆3的转动角度和角速度均发生变化，而关节2与关节3的角度和角加速度互相影响；并且从图3可以看出驱动力矩2对臂杆3的影响较大，因此臂杆2的运动很大程度上直接影响臂杆3的运动。仿真结果分析与实际相符合，这也体现了动力学模型的准确性。

2.1.3 只存在臂杆3的驱动力矩的仿真结果

给定臂杆3的关节驱动力矩，同时令臂杆1和臂杆2的关节驱动力矩为0，仿真结果如图4所示。

图4 只存在驱动力矩3条件下臂杆转动角度和角速度随时间的变化Fig.4 The changes of angle and angular velocity against the time in the mere presence of driving force 3

在该种情况下，只存在驱动力矩3，可以看出臂杆1的转动角度和角速度同样是不变的，此时臂杆2与臂杆3的转动角度和转角速度均发生变化，且转角间存在相互影响。从图4可以看出，驱动力矩3对臂杆3的影响较大，对臂杆2的运动也产生一定程度的影响，这体现出臂杆2与臂杆3间动力学耦合运动的结果。

2.2 已知运动轨迹反解力矩的仿真结果

1）首先给出目标点位置坐标，进行运动学反解，求出对应于目标点处各个关节转角的目标值。

2）由给定的初值条件和运动学反解计算得到的终止条件，求出各个关节的关节转角插值函数。

3）将所得的关节转角插值函数进行求导得到各个关节角速度的插值函数，再求导即可得到各个关节角加速度的插值函数。

4）将所得到的各个关节角度、角速度和角加速度代入动力学模型方程(6)～(8)中进行仿真计算。

5）给出仿真时间、步长及仿真环境变量，即可得到各个关节的力矩曲线变化图，按照该曲线控制各个关节的力矩即可使机械臂按照所设计的路径进行运动到达目标位置。

仿真结果如图5所示，可以看出机械臂可按照预期约束条件平稳到达指定目标点。

图5 根据已知运动轨迹求解出的各关节驱动力矩控制曲线Fig.5 The driving torque for each joint extrapolated from specific motion conditions

3 空间点到点路径规划

3.1 直线插补法

通常情况的笛卡儿点到点路径的直线插补法原理是：在已知初始点和目标点的情况下，根据插补次数和取点个数进行等间距插补。如从起始点P0(x0,y0,z0)到终点Pf(xf,yf,zf)的直线路程中间进行n次插补，中间坐标变化量记为 (Δx,Δy,Δz)，则有：

当已知时间间隔和运行速度时，插补的次数为

式中：v为运行速度；Ts为时间间隔。因此直线上各个插补点的坐标可表示为

3.2 二分逐点逼近插补法

由于等间距插补所需的插补次数多，且终止位置附近的控制精度相对低。为了提高路径运动效率，且使得末端在逼近位置附近能更加平稳准确地捕捉目标，可采取前半段路径快速到位，后半段路径尽可能平稳、小扰动的路径规划方法。故本文提出一种新的插补方法——二分逐点逼近法，用于空间环境下快速精确地实现任务的路径轨迹策略，也可以更好地适应空间机械臂的捕捉要求。

已知起始点P0(x0,y0,z0)和终点Pf(xf,yf,zf)，中间路径点记为Pi(xi,yi,zi)，则有二分逐点逼近插补法的递推表达式

式中 ξ为捕捉精度。

该方法运动效率高，且省略路径前半段中不必要的插补点，在靠近目标位置的关键路径点上更加细化精确，不仅能提高机械臂任务执行的运动效率，且在末端捕捉目标时有更高的精度。该方法同样适用于捕捉非静态目标物，通过逐点二分法逐渐逼近目标点直至满足捕捉精度要求，即可捕捉该目标物。

3.3 两种方法的仿真对比

将上述2种方法在MatLab中进行仿真，设置初始点、终止点以及规定距离，给出捕捉精度并设置相同的仿真步长，可分别得到其路径散点图。从起始点(0, 0, 0)运动到目标点(10, 10, 10)，路径插补点计数为100，仿真时间以到达精度或到达目标时刻为准，步长均为1 s，仿真结果如图6、图7所示。

图6 等间距直线插补法的MatLab仿真Fig.6 MatLab simulation of equi-distance linear interpolation

图7 二分逐点逼近直线插补法的MatLab仿真Fig.7 MatLab simulation of point by point approximation for linear interpolation

对比2种轨迹规划方法的仿真图，等间距直线插补法的插补精度为0.17，二分逐点逼近插补法的精度数量级远远小于等间距直线插补法。若以精度0.1为参考，二分逐点逼近法只需要8个插补计数点精度已可达到0.067，满足距离目标点精度要求，相比于等间距直线插补法，在运动时间和插补个数上均有显著优化和缩减。因此不难看出：二分逐点逼近插补法具有运动效率高（即不需要很多插补点即可快速接近目标点），且越接近目标位置控制精度越高等优点，适用于高精度捕捉路径规划设计。

4 结束语

本文给出了空间三自由度机械臂系统的拉格朗日动力学方程，利用MatLab软件对其进行了仿真建模，并给出已知驱动力矩求各个关节运动曲线以及已知运动情况求解各个关节驱动力矩的仿真结果。此外，根据仿真模型研究了空间环境下在轨机械臂系统的轨迹规划问题，分别介绍了等间距直线插补法和二分逐点逼近直线插补法2种逼近策略，并对这2种轨迹规划方法进行了仿真分析对比，结果验证了本文所提出的二分逐点逼近直线插补法性能更优。该方法是一种创新性的路径插补方法，经实践验证可用作一些空间任务中路径逼近的插补策略，具有工程意义和价值。

以上针对空间三自由度机械臂系统所开展的轨迹规划仿真分析工作，为后续机械臂路径规划研究工作提供了理论和模型基础。