自由漂浮空间机器人力矩最优轨迹规划算法

胡庆雷，王永智，石 忠，3

(1.哈尔滨工业大学 航天学院，150001 哈尔滨，huqinglei@hit.edu.cn;2.中国民航大学 航空自动化学院，300300 天津;3.滨州职业学院，256603 山东滨州)

自由漂浮空间机器人力矩最优轨迹规划算法

胡庆雷1，王永智2，石 忠2，3

(1.哈尔滨工业大学 航天学院，150001 哈尔滨，huqinglei@hit.edu.cn;2.中国民航大学 航空自动化学院，300300 天津;3.滨州职业学院，256603 山东滨州)

针对自由漂浮空间机器人的轨迹规划问题，提出一种基于粒子群优化算法的机械臂关节角驱动力矩最优轨迹规划算法.首先通过对自由漂浮空间机器人系统的动力学方程进行分析，给出了以机械臂关节角驱动力矩为目标函数的轨迹最优控制算法，并采用高阶多项式插值方法逼近机械臂关节角轨迹，结合粒子群优化算法对机械臂关节角轨迹进行优化求解.数值仿真表明，规划出的关节角轨迹平滑连续，在完成自由漂浮空间机器人姿态调整任务的同时，机械臂关节角驱动力矩降至最低.

自由漂浮;空间机器人;动力学方程;粒子群优化算法;力矩最优

空间机器人在未来的空间活动中将被广泛应用于卫星的捕捉、维修、燃料补给等空间在轨服务［1－2］.自由漂浮空间机器人可以仅依靠机械臂的运动，同时调整载体航天器和机械臂末端执行器的位姿，节省了姿控燃料.机械臂由关节处的伺服电机输出的控制力矩驱动，消耗系统电能.因此，为节省系统能量的消耗，需要通过规划空间机械臂的运动，使空间机器人在完成姿态调整任务的同时，将系统能量消耗降至最低.

近年来，针对不同的工程背景和优化目标，许多学者提出了相应的空间机器人运动规划方案.Dubowsky等［3］采用增强扰动图技术规划空间机械臂运动，使机械臂对载体姿态的扰动最小，但这种方法仍然需要消耗燃料才能维持载体航天器姿态的不变.Fernandes等［4］将带有空间机械臂的航天器姿态控制问题转化为非完整运动规划问题，基于最优控制方法求解非完整运动规划问题，定义机械臂关节角耗散能为优化目标函数，在最优控制算法中采用修整牛顿迭代算法寻优，该算法需要进行微分求导，计算量较大.张奇志等［5］在此基础上，将遗传算法引入到空间机械臂非完整运动规划中.针对空间机器人的扰动特性，黄攀峰等［6］定义了空间机械臂与载体航天器之间的扰动映射关系，基于遗传算法提出了空间机器人最小扰动路径规划算法，但该算法仅适用于自由飞行空间机器人，仍然需要消耗姿控燃料调整载体航天器的姿态.徐文福等［7－8］利用最优控制方法，探讨了自由漂浮空间机器人系统存在动力学约束情况下的非完整运动规划方法，利用遗传算法进行轨迹的优化求解，并且对捕捉空间目标及捕捉目标后自由漂浮空间机器人的停靠和重新定向时的轨迹规划算法进行了相应的研究.

为节省机械臂运动过程中伺服电机消耗的能量，本文通过对系统动力学方程进行分析，提出了以机械臂关节角驱动力矩为最优控制目标函数的轨迹规划算法，结合粒子群算法对目标函数进行优化求解，并且将粒子群优化算法在自由漂浮空间机器人轨迹规划中的寻优性能同遗传算法进行了比较.本文最后对力矩最优轨迹规划算法进行仿真，在完成空间机器人姿态调整任务的同时，系统消耗的能量降到了最低.

1 自由漂浮空间机器人系统

1.1 自由漂浮空间机器人系统动力学

考虑由载体航天器和二连杆机械臂组成的空间机器人在平面内的运动(如图1所示)，连接机器人各构件的关节均为只有1个旋转自由度的转动关节.

图1 空间机器人系统结构

当空间机器人系统处于自由漂浮状态时，忽略系统所受的微弱重力作用，因此系统的势能为零.将系统总动能表达式代入拉格朗日方程中即可得到系统的动力学方程，表达式如下:

其中:M(q)为正定对称的系统惯量矩阵;q=［θq1q2］T为系统相对关节角矢量;C(q，˙q)为含有离心力和科氏力的矩阵;τ=［0 τTm］T，τm表示机械臂关节处输出的控制力矩.令

其中:qm=［q1q2］T为机械臂的相对关节角矢量;Cb和Cm分别为与载体航天器和机械臂运动相关的非线性力项与对应的姿态角速度的乘积，其中

则系统动力学方程可以转化为

由式(1)可得

将式(3)代入式(2)中经过整理可得

则系统动力学方程可以转化为

1.2 自由漂浮空间机器人系统非完整运动规划

自由漂浮空间机器人系统的系统动量及动量矩守恒.由于自由漂浮空间机器人系统动量矩守恒方程不可积分，因此，系统的运动受非完整性约束.非完整运动规划问题即为:在给定系统的初始状态和终止状态的情况下，设计适当的控制输入，使系统在有限时间内沿某一轨线从给定的初始状态运动到终止状态.假设系统在初始时刻的动量矩为零，则系统的动量矩守恒方程可以写成［9］

上式中Bi(i=0，1，2)是系统惯性参数和机械臂关节角q1、q2的非线性函数.

选取机械臂相对关节角速度˙qm为输入变量，由式(5)可以得到

式中

当给定空间机器人系统的初始和期望状态qin和qdes时，非完整运动规划问题转化为通过优化目标函数寻找控制输入˙qm，确定系统状态转移的最优轨线.

2 自由漂浮空间机器人关节力矩最优轨迹规划

空间机械臂的运动由关节角处的伺服电机驱动，需要消耗系统的电能.为了提高系统电能的利用效率，本文从机械臂关节角控制力矩优化提出了自由漂浮空间机器人力矩最优轨迹规划算法.定义力矩最优控制的目标函数为

其中t0和T分别是空间机器人系统运动过程的初始和终止时间，通过计算系统动力学方程的变形(4)可以求得空间机械臂关节角处的驱动力矩τ1和τ2.

基于机械臂关节角驱动力矩最优的轨迹规划问题可以表示成如下求解非线性约束优化问题:

惩罚函数法是求解非线性约束优化问题常用的一种方法，它根据约束条件的特点构造某种惩罚函数并加入到目标函数中，将约束优化问题转化为无约束优化问题.此处引入罚函数法，则式(7)改写为

其中:α为惩罚因子;θfin是载体的期望姿态角;θ(T)是在给定输入˙qm(T)根据式(5)求得的载体姿态角.在进行数值求解的过程中，使用四阶龙格-库塔方法，该方法计算精度高，程序实现容易.

机械臂关节角q1和q2的运动轨迹是时间t的非线性函数，故可以采用时间t的高阶多项式逼近.式(6)中的控制输入可以写成

由式(6)和式(8)可知，θ(T)是插值多项式的系数b的函数，则有g(b，t).因此，目标函数可以写成

当惩罚因子α确定时，基于机械臂关节角驱动力矩最优的自由漂浮空间机器人轨迹规划问题便转化为寻找插值多项式的系数b使目标函数(9)最小的问题.

3 粒子群优化算法在自由漂浮空间机器人轨迹规划算法中的应用

3.1 粒子群算法的基本原理

在对式(9)进行参数寻优的过程中引入粒子群优化算法［10］.粒子群优化算法模拟鸟类的捕食行为，优化目标函数的每个解都是搜索空间的1个“粒子”，所有粒子都对应于1个由目标函数确定的适应度值.粒子在搜索空间中以一定的速度和方向飞行，通过对环境的学习与适应，根据个体与群体的飞行经验的综合分析结果来动态调整飞行速度和飞行方向［11］.

在D维连续空间中，粒子i在第d维子空间中的飞行速度vid和位置xid根据下式进行更新:

其中:pgd是整体粒子群的历史最优位置记录;pid是当前微粒的历史最优位置记录;ω为惯性权值;c1和c2为加速度常数;rand(1)为随机变量.粒子i在d维子空间中的位置xid对应于目标函数(9)的解，即插值多项式的系数b.

3.2 粒子群算法在力矩最优轨迹规划中的应用

由前文分析可得，基于粒子群算法的自由漂浮空间机器人力矩最优轨迹规划算法如下:

首先，设定种群的粒子个数N和算法最大迭代次数Nmax，初始化粒子群算法参数.定义种群的第 i个粒子，其中和分别表示用于逼近关节角q1和q2轨迹的高阶多项式系数;每个粒子的最优位置pid设为其初始位置xi，pid中的最优值设为pgd.

Step1 令迭代次数k=1，初始化含有N个粒子(X1(k)，X2(k)，…，XN(k))的种群;

Step2 根据目标函数式(9)计算每个粒子的适应度J(Xi(k));

Step3将每个微粒的适应值J(Xi(k))与其经历过的最优位置pid进行比较，如果优于pid，则将其作为当前的最优位置pgd;

Step4 将每个微粒的适应值J(Xi(k))与群体所经历过的最优位置pgd进行比较，如果优于pgd，则将其作为群体最优位置pgd;

Step5 根据式(10)和(11)调整当前粒子的位置xi和速度vi;

Step6 当达到最大迭代次数Nmax时，终止迭代;否则返回Step2.

4 仿真实例

为了验证本文所提出的自由漂浮空间机器人力矩最优轨迹规划算法的有效性，在Matlab7.0环境下进行仿真，模型采用文献［9］给出的空间机器人的物理参数，如表1所示.

表1 空间机器人系统参数

系统运动时间T=10，为保证粒子群优化算法的收敛性，采用文献［12］的中参数:惯性权值ω=0.729 8，最大迭代次数取200，加速度常数c1=c2=1.496 2，初始化群体个数N=25.

空间机器人系统的初始和期望姿态分别为:qin=(-40°，90°，30°)，qdes=(0°，-60°，90°)，初始及期望状态下的姿态角速度、加速度均为零.

通过粒子群算法优化目标函数(9)，可以求得高阶多项式的系数b.仿真结果如图2～图7所示.从仿真结果可以看出，利用本文算法规划出的轨迹平滑连续，机械臂关节角速度和角加速度分别被限制在(－0.5，0.5)rad/s及(－0.2，0.3)rad/s2范围内，较小的角(加)速度保证了系统运行的稳定.图5显示了空间机器人系统仅通过机械臂关节的运动同时调整载体航天器和空间机械臂位姿的运动过程.图6为最优目标函数值随着迭代次数增加的变化情况，较快的收敛速度验证了粒子群算法在求解非线性无约束优化问题上的适用性.

为验证粒子群优化算法在搜索自由漂浮空间机器人关节角驱动力矩最优轨迹上的优越性，分别采用粒子群优化算法和遗传算法对式(9)进行优化求解.2种算法的迭代次数均选为200，种群大小均选为25，遗传算法中采用20位的二进制编码表示决策变量，交叉概率和变异概率分别为0.8和0.01.粒子群算法其他参数与上例相同.2种算法寻优过程如下:经过200代寻优计算，分别使用PSO和GA优化目标函数得到的最优值为1.12×106和1.79×106.图7分别是2种算法的进化过程，使用PSO算法寻优时，起始阶段优化得出的目标函数值较大，但经过大约30代左右的进化即可快速收敛于最优解;而遗传算法的收敛速度较慢.从上述仿真实验及前文的算法对比分析可以看出，PSO算法在多数的情况下，比遗传算法更适合应用于自由漂浮空间机器人力矩最优轨迹规划算法，计算速度和效率更高.

图2 载体姿态角、关节角1、关节角2轨迹

图3 关节角1角速度、关节角2角速度

图4 关节角1加速度、关节角2加速度

图5 系统运动过程

图6 目标函数值

图7 粒子群算法与遗传算法的搜索性能对比

5 结论

空间机械臂由关节角处的伺服电机驱动，需要消耗系统电能.为提高自由漂浮空间机器人系统电能的使用效率，本文通过对系统动力学方程的分析，提出了基于机械臂关节角控制力矩最优的轨迹规划算法，并结合粒子群优化算法寻找最优姿态轨迹.最后，通过实例仿真验证了本文提出的自由漂浮空间机器人力矩最优轨迹规划算法的有效性，在完成空间机器人位姿调整任务的同时，机械臂关节处输出的控制力矩得到了优化，节省了系统宝贵的能源.

［1］PAPADOPOULOS E，DUBOWSKY S.Dynamic singularities in free-floating space manipulators［J］.Journal of Dynamics Systems Measurement and Control Transactions of the ASME，1993，115(1):44－52.

［2］WANG H L，XIE Y CH.Adaptive jacobian position/force tracking control of free-flying manipulators［J］.Journal of Robotics and Autonomous Systems，2009，57:173－181.

［3］DUBOWSKY S，TORRES M.Path planning for space manipulators to minimize spacecraft attitude disturbances［C］//Proceedings of the 1991 IEEE International Conference on Robotics and Automation.Piscataway:IEEE，1991:2522－2528.

［4］FERNANDES C，GURVITS L，LI Z X.Attitude control of space platform/manipulator system using internal motion［C］//Proceedings of the 1992 IEEE International Conference on Robotics and Automation.Piscataway:IEEE，1992:893－898.

［5］张奇志，戈新生，刘延柱.基于小波逼近的空间机械臂非完整运动最优规划的遗传算法［J］.机器人，1999，21(5):357 －362，369.

［6］HUANG P F，CHEN K，XU Y SH.Optimal path planning for minimizing disturbance of space robot［C］//Proceedings of 9th International Conference on Control，Automation，Robotics and Vision.Pisacataway:IEEE，2006:1－6.

［7］XU W F，LIANG B，QIANG W Y，et al.Trajectory planning of space robot system for target berthing and reorientation after capturing［C］//Proceedings of the 6th World Congress on Intelligent Control and Automation.Dalian:［s.n.］，2006:8981 －8985.

［8］XU W F，LIANG B，XU Y SH.Autonomous Path Planning and Experiment Study of Free-Floating Space Robot for Target Capturing［J］.Journal of Intelligent and Robotic Systems，2008，51(3):303－331.

［9］TORTOPIDIS I，PAPADOPOULOS E.On point-to-point planning for underactuated space manipulator systems［J］.Journal and Autonomous Systems，2007，55:122 －131.

［10］EBERHART R，KENNEDY J.A new optimizer using particles swarm theory［C］//Proceedings of the Sixth International Symposium on Micro Machine and Human Science.Nagoya:［s.n.］，1995:39 －43.

［11］吴启迪，汪镭.智能微粒群算法研究及其应用［M］.南京:江苏教育出版社，2005:15－18.

［12］CLERC M，KENNEDY J.The particle swarm explosion，stability，and convergence in a multi-dimensional complex space［J］.IEEE Transaction on Evolutionary Computation，2002，6(1):58 －73.

Minimum torque trajectory planning algorithm for free-floating space robot

HU Qing-lei1，WANG Yong-zhi2，SHI Zhong2，3

(1.School of Astronuantics，Harbin Institute of Technology，150001 Harbin，China，huqinglei@hit.edu.cn;2.College of Aeronautical Automation，Civil Aviation University of China，300300 Tianjin，China;3.Binzhou Vocation College，256603 Shandong Binzhou，China)

Considering the problem of trajectory planning for Free-Floating Space Robot(FFSR)，a particle swarm optimization(PSO)based on minimum torque trajectory planning scheme is presented.By analyzing the dynamic equation of system，an optimal control algorithm is applied to FFSR trajectory planning，in which the driving torque of manipulator joint angles is considered as the objective function.Moreover，the high order polynomial is used to approach the trajectories of the joint angles.To achieve the optimal trajectory，the PSO algorithm is implemented so that the planned joint angle trajectories are smooth and continuous.Numerical simulation results show that the proposed methodology is effective and available for solving FFSR trajectory planning problem.With accomplishing the space robot attitude adjusting mission，the driving torque is reduced to the least.free-floating;space robot;dynamic equation;particle swarm optimization;minimizing torque

free-floating;space robot;dynamic equation;particle swarm optimization;minimizing torque

V42

A

0367－6234(2011)11－0020－05

2010－08－18.

国家自然科学基金资助项目(60774062、61004072);高等学校博士学科点专项科研基金资助项目(20070213061、20102302110031);教育部留学回国人员科研启动基金资助;黑龙江省留学回国人员科学基金资助项目(LC08C01);中央高校基本科研业务费专项资金资助项目(HIT.NSRIF.2009003);哈尔滨市科技创新人才研究专项基金资助项目(2010RFLXG001).

胡庆雷(1979—)，男，副教授，博士生导师.

(编辑 张 宏)