在轨服务飞行器机械臂系统运动规划

李岩，蔡远文

(装备学院 航天装备系，北京101416)

在轨服务飞行器(orbital servicing vehicle，OSV)工作在微重力环境下，因存在动量和动量矩守恒，OSV姿态会随机械臂的运动而改变.此时，机械臂系统的运动规划和固定基座的机械臂有所不同.诸多文献的研究成果表明[1-4]，OSV机械臂系统的运动规划一般以机械臂的运动对基座扰动最小和控制过程能耗最小为优化目标.多采取的研究思路为:根据守恒定律建立OSV机械臂系统的运动模型，将其转化为非线性控制系统状态方程，确定系统优化的目标函数，利用最优化理论和方法计算最优控制律.本文在此思路的基础上，考虑了多种初末状态约束，对运动规划目标和规划方法进行了分析和改进.

1 运动规划的目标函数

以文献[5]中建立的单臂二关节机械臂系统为例，讨论其运动规划步骤和方法(如图1).定义系统的位形x=[q1q2qB]T为状态变量，机械臂各关节相对转动的角速度q·(i=1，2)作为输入变量，则系统的状态方程可表示为

图1 单臂二关节OSV机械臂系统参数定义Fig.1 Parameter definition of single arm and double joints OSV manipulator system

根据最小能量控制原理，选择机械臂关节相对转动的耗散能作为最优控制指标，用泛函表示为

式中:＜u(t)，u(t)＞表示内积，u(t)为 Hilbert空间L2的可测向量函数.在实际计算时，常用有限维的子空间代替，由函数空间中的投影定理，取Fourier基向量ei张成 N维线性子空间，则u(t)在 N维Fourier子空间上的投影就是Fourier级数中前N项部分和[7-9]，即

式中:ai为函数u(t)在{ei}(i=1，…N)基上的投影，a为投影向量;Φ是Fourier正交基向量组成的n×N维矩阵.将a看作新的控制变量，根据Fourier正交基的积分特性，同时考虑系统终端约束条件，控制目标可表示为以下函数:

式中:λ为惩罚系数，取值足够大;x(T)是式(1)在给定控制输入u(t)时，系统在t=T时的状态.显然，x(T)也是a的函数，记作x(T)=f(a)，当给定N和λ时，式(4)可写为

因此，寻找控制输入u(t)使式(4)为最小的问题即为寻找a使式(5)为最小值的问题[10-13].按照上述思路得到以下几类最优规划问题的目标函数.

1)在机械臂关节角运动初末速度无约束的条件下，目标函数为式(5).

2)一般情况下，机械臂位形变化时，要求其关节角速度在初始时刻和终端时刻为零.在该条件约束下，将目标函数增加输入条件约束及其惩罚系数γ，即

3)OSV机械臂系统，在展开过程中对OSV本体(载体或基座)的姿态造成一定的影响.这种影响可以通过机械臂从收缩状态到完全展开过程中，基座姿态的变化情况来观察和分析.在此过程中，假设系统满足能量最优以及初末时刻关节角速度为零的条件(即式(7)的条件)，且关节角初末状态分别为q10=q20=180°，q1T=q2T=0°，其目标函数如下:

4)为了寻找控制输入u(t)使机械臂展开对基座的影响最小，需要得到衡量基座姿态变化的指标量.从姿态变化过程方面考虑，可以用单位时间姿态变化量绝对值的总和来表示，得到以下指标量:

所以总的目标函数为

5)为了使机械臂展开过程始末，载体姿态初始和末端状态一致，即qB0=qBT(本例为0°)，在系统能耗最小且初末关节角速度为零条件下，提出以下目标函数:

2 求解运动规划问题的遗传算法

遗传算法是一种新型的优化算法，由于其在进化搜索中能基于多点进行整体搜索，有较好的全局搜索性能.利用遗传算法求解优化问题时，当变量维数过高且取值范围较大时，可使用精度高、便于大空间搜索的实数编码.在设计算法和仿真计算过程中，选择合适的幂函数对适应度进行收放，能够有效的解决进化过程不收敛，或出现未成熟收敛现象的问题[14].结合空间机械臂的运动规划最优控制问题，相应的遗传算法步骤如下:

1)染色体编码.利用遗传算法的并行搜索，对式(5)中函数u(t)在Fourier基上的投影a采用实数编码，染色体编码为ai(i=1，2，…，N)组成的N维向量.

式中:ai为实数.

2)初始群体的生成.随机产生M个个体，将M个个体的每一分量初始化为0均值，方差为σ的高斯分布随机数.

3)适应度函数的建立.染色体评价的适应度函数设为

4)尺度变换函数，收放适应度:

式中:b、c为收放参数.

5)遗传操作设计.

①选择:根据式(12)和式(13)计算每个染色体的适应度值 g'(ai)(i=1，2，…，M)，那么第 i个体被选择的概率为

个体的选择采用轮盘赌选择方法.

②交叉:根据交叉概率Pc选择参加交叉的个体(偶数个).采用随机线性组合方式进行交叉计算.设被选中交叉的个体为:v，w，则其后代 v'，w'为

式中:r为(0，1)均匀分布的随机数.

③变异:根据变异概率Pm，将被选择变异的基因变为任一同均值和方差的高斯分布随机数δj:

6)重复3)至5)步，直到求出满足条件的最优解，或到达终止代数G.

3 仿真算例和分析

以上述空间机械臂系统参数作为算例，如图1所示.设系统物理和几何参数为[5]:m0=1 800 kg，m1=m2=50 kg;I0=1 260 kg·m2，I1=I2=71 kg·m2;b0=3.5 m，a1=b1=a2=b2=2.0 m.

载体与机械臂完成规定运动时间设为T=20 s.遗传算法的控制参数分别为:群体规模M=40，染色体长度N=10，交叉概率 Pc=0.8，变异概率 Pm=0.15，进化代数G=2 000.在仿真试验中，选取10个Fourier正交基矢量

3.1 从初始位置到终端位置的运动规划

假设机械臂系统从初始位形 qB0=0°，q10=-21.8°，q20=117.4°，运 动 到 终 端 位 形 qBT=-31.394 4°，q1T=66.310 2°，q2T=94.959 6°[5]，选择投影a，使运动过程机械臂消耗最小.

1)初末时刻关节角速度无约束情况.

目标函数为式(5)，取λ=1 000.在按照上述步骤执行遗传算法的同时，需要根据多次试验的数据，进一步确定合适的适应度收放参数b，c，以及高斯分布的方差σ.为了获得更加准确的最优目标值，可选择多次试验的最优结果，作为初始种群多次计算，得到表1的最优值.其仿真过程轨迹和各参数的变化情况如图2、3.

表1 图2中遗传算法计算结果统计Table 1 Result of genetic algorithm in Fig.2

图2 无初末端速度约束的位形变化Fig.2 The motion without the original or the terminal velocity restriction

图3 图2中关节角速度，关节角，基座姿态角以及基座位置变化曲线Fig.3 Curve of the joint angle ＆ angle speed and the base attitude ＆ position in Fig.2

2)关节角速度初末时刻为零的情况.

其目标函数为式(6)，取λ=γ=1 000，下同.表2中是遗传算法计算结果，图4中是最优输入下的变化轨迹.从图5中相关参数变化曲线能够更清楚地看到机械臂关节角速度初末时刻为0.

图4 初末速为零约束条件下的位形变化Fig.4 The motion under the restriction of zero original and terminal velocity

图5 图4中关节角速度，关节角，基座姿态角以及基座位置变化曲线Fig.5 Curve of the joint angle ＆ angle speed and the base attitude ＆ position in Fig.4

3.2 机械臂展开过程的扰动分析

1)能耗最小，初末时刻关节角速度为零的情况.目标函数为式(7).由于机械臂系统物理参数参考真实航天器，因而机械臂系统的运动时间尽量符合实际，设T=200 s.假设机械臂在Z轴垂直平面内，不受载体形状约束，可以完全收缩.机械臂系统以最小的能耗从完全收缩状态q10=q20=180°，到完全展开状态q1T=q2T=0°，分析该过程对基座的影响情况.此时，机械臂伸展到达惯性系中的最大作用距离|rt|max=11.105 3 m.设基座初始状态为qB0=0°.计算数据如表3.

图6 机械臂从收缩到展开的初末状态对比Fig.6 Comparison between contraction status and extension status of the manipulator

图6和图7中的轨迹和参数变化曲线可以看出，在初末速(关节角速度)为零，能耗最小的条件下，机械臂展开过程中，基座姿态由0°变为52.33°.且杆1和杆2几乎同时同步展开，关节展开速度变化几乎一致.算例结果表明，机械臂在展开过程中对基座姿态造成了较大影响.

图7 展开过程中关节角速度，关节角，基座姿态角以及基座位置变化曲线Fig.7 Curve of the joint angle ＆ angle speed and the base attitude＆position during the motion

2)能耗最小，初末时刻关节角速度为零，同时对基座扰动最小情况.计算数据如表4.目标函数为式(9).图8和图9中可以得出，增加扰动最小的目标要求后，机械臂展开过程中，关节角q1的变化出现了非单调的特征.为了减小对基座的影响，关节角q1在110 s附近开始“过零”和“回调”.机械臂基座姿态由0°变为26.26°，比上例减小近半.此外，为减小对基座的扰动，展开过程中，杆1(关节1)首先展开;杆2展开动作稍滞后，且q2单调变化.该算例表明，合理设计规划目标，可以减小机械臂对基座的姿态扰动.

图8 扰动最小情况下，机械臂的初末状态对比Fig.8 Comparison between original and terminal status of the manipulator with the minimum disturbance to the base

图9 图8中关节角速度、关节角、基座姿态角以及基座位置变化曲线Fig.9 Curve of the joint angle ＆ angle speed and the base attitude ＆ position during the motion in Fig.8

3)能耗最小，初末时刻关节角速度为零，扰动最小，且基座末端姿态与初始姿态一致.计算数据如表5.目标函数为式(10).

图10 基座扰动最小且初末时刻姿态一致条件下的机械臂展开过程Fig.10 Motion of the manipulator from contraction to extension with the same original and terminal base attitude and minimum disturbance

图10、11中可以看出，机械臂的展开过程中关节角q1和q2的变化都呈现非单调性.为了使基座初末状态保持一致，机械臂需要通过自身的伸展和收缩运动相协调，对基座进行姿态回调.该算例结果表明，合理设计目标函数，可以通过机械臂运动调整基座姿态.

图11 图10中关节角速度、关节角、基座姿态角以及基座位置变化曲线Fig.11 Curve of the joint angle ＆ angle speed and the base attitude ＆ position during the motion in Fig.10

表2 图4中遗传算法计算结果统计Table 2 Result of genetic algorithm in Fig.4

表3 图6中遗传算法计算结果统计Table 3 Result of genetic algorithm in Fig.6

表4 图8中遗传算法计算结果统计Table 4 Result of genetic algorithm in Fig.8

表5 图10中遗传算法计算结果统计Table 5 Result of genetic algorithm in Fig.10

上述分析表明，带机械臂的OSV系统中，机械臂的运动给基座造成较大影响.一方面，这种影响可以代替GNC系统对基座姿态进行调整，从而节省调姿发动机的燃料消耗.另一方面，通过研究影响的变化规律，可以设计相应的姿态补偿系统，消除机械臂作业过程中对基座的不必要扰动.

4 结束语

本文提出了一种可行的OSV机械臂系统运动规划方法.首先确定规划目标，将机械臂系统运动守恒模型转化为非线性控制系统状态方程，将运动规划转化为寻求满足目标函数的最优控制问题.为了简化目标函数，将输入控制函数表示为Fourier正交基与其投影的积的形式，而目标函数中对于初末时刻约束条件的处理采用增加惩罚系数的方法.状态方程和目标函数明确以后，可以用多种最优化方法解决问题，而遗传算法具有通用性强，最优解全局性好的特点，能够求解多种复杂的目标函数.

OSV机械臂系统相当复杂，本文仅以较简单的二关节单臂系统为例进行分析，得出了解决机械臂系统运动规划问题的一般思路.文中内容还有不完善的地方需要进一步深入研究.如:系统模型为二维简化模型，仅考虑了关节轴平行的情况，且未进行误差分析;OSV机械臂运动控制本体姿态的方法需要进一步细化;遗传算法效率较低，可进一步尝试多种优化算法，以提高计算效率，实现在线规划等.

[1]DUBOWSKY S，PAPADOPOULOS E.The kinematics，dynamics，and control of free-flying and free-floating space robotic system[J].IEEE Transactions on Robotics and Automation，1993，9(5):531-542.

[2]YAMADA K，YOSHIKAWA S.Feedback control of space robot attitude by cyclic motion[J].Journal of Guidance，Control，and Dynamics，1997，2 0(4):715-720.

[3]戈新生，陈立群.空间机械臂非完整运动规划的遗传算法研究[J].宇航学报，2005，26(3):262-266.GE Xinsheng，CHEN Liqun.Nonholonomic motion planning of a space manipulator system using genetic algorithm[J].Journal of Astronautics，2005，26(3):262-266.

[4]梁斌，刘良栋，李庚田.空间机器人的动力学等价机械臂[J].自动化学报，1998，24(6):761-767.LIANG Bin，LIU Liangdong，LI Gengtian.Dynamically equivalent manipulator for a space manipulator[J].Acta Automatica Sinica，1998，24(6):761-767.

[5]李岩，蔡远文.在轨服务飞行器机械臂系统动力学模型[J].装备指挥技术学院学报，2011，22(2):66-72.LI Yan，CAI Yuanwen.Dynamic model of on-orbit service vehicle manipulator system[J].Journal of The Academy of Equipment Command＆ Technology，2011，22(2):66-72.

[6]高海波，朱利民，熊有伦.空间机器人系统的简化动力学模型与应用研究[J].华中科技大学学报，2001，29(3):64-66.GAO Haibo，ZHU Limin，XIONG Youlun.The kinematics and dynamics model of space robot system and its application[J].Journal of Huazhong University of Science and Technology，2001，29(3):64-66.

[7]陆启韶，周梦.现代数学基础[M].北京:北京航空航天大学出版社，2005:76-85.

[8]熊有伦.机器人技术基础[M].武汉:华中理工大学出版社，1996:55-61.

[9]王景，王昊瀛.自由飞行空间机器人的运动学建模研究[J].航天控制，1999(1):7-10.WANG Jing，WANG Haoying.Study of kinematic model of free-flying space robot[J].Aerospace Control，1999(1):7-10.

[10]赵晓东，王树国，严艳军，等.基于轨迹规划的自由漂浮空间机器人抓取运动物体的研究[J].宇航学报，2002，23(3):48-51.ZHAO Xiaodong，WANG Shuguo，YAN Yanjun.Algorithm of capturing at a moving body for a free-floating space manipulator based on trajectory planning[J].Journal of Astronautics，2002，23(3):48-51.

[11]王昊瀛，王景，吴宏鑫.平面型两关节空间机器人的自适应轨迹控制[J].航天控制，1999(3):13-19.WANG Haoying，WANG Jing，WU Hongxin.Adaptive trajectory control of planar two-joint space robot[J].Aerospace Control，1999(3):13-19.

[12]戈新生，吕杰.带空间机械臂航天器姿态运动规划的数值算法研究[J].应用力学学报，2006，24(2):227-232.GE Xinsheng，LV Jie.Numerical algorithm for attitude motion planning of spacecraft with space manipulators[J].Chinese Journal of Applied Mechanics，2006，24(2):227-232.

[13]徐文福，詹文法.自由飘浮空间机器人系统基座姿态调整路径规划方法的研究[J].机器人，2006，28(3):291-296.XU Wenfu，ZHAN Wenfa.Path planning for base attitude adjustment of a free-floating space robot system[J].Robot，2006，28(3):291-296.

[14]戈新生，张奇志，刘延柱.基于遗传算法的空间机械臂的运动规划的最优控制[J].空间科学学报，2000，20(2):185-191.GE Xinsheng，ZHANG Qizhi，LIU Yanzhu.An optimized method to control the motion planning of space manipulators base genetic algorithm[J].Chinese Journal of Space Science，2000，20(2):185-191.