基于遗传算法的空间自由漂浮机械臂系统运动规划*

李 岩 蔡远文

装备学院，北京 101416

由于空间自由漂浮机械臂(FSM)系统工作在微重力环境下，存在动量和动量矩守恒，其姿态会随机械臂的运动而改变。此时，机械臂系统的运动规划和固定基座的机械臂完全不同，其运动过程满足非完整约束。如何结合非完整性对系统进行运动规划和控制，是FSM系统运动规划需要解决的问题[1-4]。

FSM系统的运动规划一般以机械臂的运动对基座扰动最小和控制过程能耗最小为优化目标。研究思路为：首先建立FSM系统的动力学模型，将其转化为非线性控制系统状态方程；然后确定系统优化的目标函数，利用最优化理论和方法计算最优控制律，实现机械臂运动最优规划。本文以文献[5]中建立的单臂二关节机械臂系统为例，讨论其运动规划步骤和方法，几何与物理参数定义和数值参见文献[5]2.1节。

图1 单臂二关节FSM系统参数定义图

1 系统状态方程和运动规划目标函数

(1)

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

(2)

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

Φa

(3)

其中，ai为函数u(t)在{ei}(i=1，…，N)基上的投影，a为投影向量；Φ是Fourier正交基向量组成的n×N维矩阵。将a看作新的控制变量，根据Fourier正交基的积分特性：

(4)

同时考虑系统终端约束条件，控制目标可表示为以下函数：

(5)

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

(6)

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

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

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

γ[u0(a)2+uT(a)2]

(7)

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

minJ(a)=〈a,a〉+λ[q1T(a)2+q2T(a)2]+

γ[u0(a)2+uT(a)2]

(8)

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

(9)

所以总的目标函数为：

q2T(a)2]+γ[u0(a)2+uT(a)2]

(10)

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

γ[u0(a)2+uT(a)2]

(11)

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

遗传算法是一种新型的优化算法，由于其在进化搜索中能基于多点进行整体搜索，有较好的全局搜索性能；且仅以目标函数为依据，在多变量优化中优于传统的梯度方法，因而在工程优化问题中得到广泛的应用。利用遗传算法求解优化问题时，当变量维数过高且取值范围较大时，可使用精度高、便于大空间搜索的实数编码[14-15]。在设计算法和仿真计算过程中，发现群体中个体的适应度值相差很小，致使平均适应度值和最大个体适应度值比较接近，在采用赌轮选择机制时，平均适应度值附近的个体和适应度值最高的个体被选择的几率几乎相等。这种情况在算法最初，有利于扩大搜索范围，增加种群多样性，减小得到局部收敛点的可能。但是，在遗传算法后期，适应度差别不明显，会使最优个体与大多数个体具有等同的淘汰几率。这样就会导致进化过程不收敛，或产生未成熟收敛现象，这时需要对适应度进行放大，增加个体间的竞争力。选择合适的幂函数对适应度进行收放，能够有效解决上述问题。结合空间机械臂的运动规划最优控制问题，相应的遗传算法步骤如下：

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

a=[a1,a2,…,aN]T

(12)

其中，ai为实数。

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

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

(13)

其中，a为染色体，J(a)为目标函数式(6)。

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

g′(a)=F(g(a))=(b·g(a))c

(14)

式中，b，c为收放参数，g′(a)变换后的适应度函数值。

5)遗传操作设计。

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

/

(15)

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

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

v′=rv+(1-r)w

w′=rw+(1-r)v

(16)

其中，r为(0,1)均匀分布的随机数。

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

aij=δj

(17)

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

3 运动规划仿真算例和分析

以上述空间机械臂系统参数作为算例，如图1所示。设系统物理和几何参数为：m0=1800kg，m1=m2=50kg；I0=1260kg·m2，I1=I2=71kg·m2；b0=3.5m，a1=b1=a2=b2=2.0m。

,,,

(18)

4 已知初末时刻位置的运动规划

(1)初末时刻关节角速度无约束

假设机械臂系统从初始位形qB0=0°，q10=-21.8°，q20=117.4°，运动到终端位形qBT=-31.3944°，q1T=66.3102°，q2T=94.9596°[5]，如何选择投影a，使运动过程机械臂消耗最小。目标函数为：

取λ=1000。

在按照上述步骤执行遗传算法的同时，需要根据多次试验的数据，进一步确定合适的适应度收放参数b,c，以及高斯分布的方差σ。实际操作过程中，本例首先选择σ=1.0，b=c=10，计算数据表明，满足最优目标的投影a分量均小于1，且目标值J(a)也小于1。为了使遗传算法收敛更快，得到更加准确的结果，调整收放参数为b=1，c=5，种群分布σ=0.1。经过3次计算，分别得到结果如表1所示。表1中最优值及其遗传代数是指同一次试验的2000次迭代中，得到的最优值及其代数。为了获得更加准确的最优目标值，将三次试验中的最优结果，即第二次计算的投影值a，作为优选样本插入遗传算法的初始种群中，再次执行遗传算法，得到表2的最优值。

表1 计算结果统计

表2 插入优选样本试验得到的最优结果

图2 无初末端速度约束的位形变化

如果仍然没有得到满意的最优值，可以进一步将多次计算的最优值结果作为种子插入初始种群如此往复直到得到满足精度要求的结果。以下计算过程同样采取这种方法。本例以表中的第5次试验结果作为最优值，其仿真过程轨迹和各参数的变化情况如图2和图3。

(2)关节角速度初末时刻为0。其目标函数为：

γ[u0(a)2+uT(a)2]

取λ=γ=1000，下同。表3中是遗传算法计算结果，图4中是最优输入下的变化轨迹。从图5中相关参数变化曲线能够更清楚的看到机械臂关节角速度初末时刻为0。

图3 关节角速度，关节角，基座姿态角以及基座位置变化曲线

表3 遗传算法计算结果统计

图4 初末速为0约束条件下的位形变化

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

1)能耗最小，初末时刻关节角速度为0的情况。目标函数为：

minJ(a)=〈a,a〉+λ[q1T(a)2+q2T(a)2]+

γ[u0(a)2+uT(a)2]

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

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

图5 关节角速度，关节角，基座姿态角以及基座位置变化曲线

表4 遗传算法计算结果统计

图6 机械臂从收缩到展开的过程

2)能耗最小，初末时刻关节角速度为0，同时对基座扰动最小情况。计算数据如表5。目标函数为：

λ[q1T(a)2+q2T(a)2]+γ[u0(a)2+uT(a)2]

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

图7 展开过程中关节角速度，关节角，基座姿态角以及基座位置变化曲线

表5 遗传算法计算结果统计

3)能耗最小，初末时刻关节角速度为0，扰动最小，且基座末端姿态与初始姿态一致。计算数据如表6。目标函数为：

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

表6 遗传算法计算结果统计

图10 基座扰动最小且初末时刻姿态一致条件下机械臂展开过程

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

图11 关节角速度，关节角，基座姿态角以及基座位置变化曲线

6 结论

本文提出了一种可行的FSM系统运动规划方法。该方法首先确定规划目标，将机械臂系统动力学模型转化为非线性控制系统状态方程，将运动规划转化为寻求满足目标函数的最优控制问题。为了进一步简化目标函数，将输入控制函数表示为Fourier正交基与其投影的积的形式，而目标函数中对于初末时刻约束条件的处理采用增加惩罚系数的方法。

状态方程和目标函数明确以后，可以用多种最优化方法解决问题，而遗传算法具有通用性强，最优解全局性好的特点，能够求解多种复杂的目标函数。算法设计过程中，染色体采用实数编码，长度与Fourier基的数量一致；种群数一般选择20～100(本文40)；适应度函数需要根据遗传代数进行适当缩放，有助于选择最优个体；交叉概率的选择既不能破坏好的个体的优良性，又不能使新个体产生速度过慢，一般选择0.4～0.99(本文为0.8)；变异概率同样要保持个体优良性并较快地产生新个体，一般选择0.01～0.2(本文为0.15)；遗传代数是算法的终止条件之一，为得到更高精度的最优解，遗传代数越大越好，但计算时间较长，效率不高，一般建议选择500～5000(本文2000)。为确保算法结果更快地收敛到全局最优点，可先进行若干次计算，从中选择最优结果作为优秀个体再次参与遗传计算，直到得到满足精度要求的结果为止。

FSM系统相当复杂，本文仅以较简单的二关节单臂系统为例进行分析，得出了解决机械臂系统运动规划问题的一般思路。文中内容还有不完善的地方需要进一步深入研究。如：动力学模型为二维简化模型，仅考虑了关节轴平行的情况，需要进一步分析多杆机械臂系统在三维空间的运动模型；载体姿态随机械臂运动轨迹和方向等因素的变化规律还需进一步明确，以实现机械臂运动控制载体姿态的目标；机械臂系统运动规划需要进一步尝试多种最优化方法，以提高计算效率等。

参 考 文 献

[1] 戈新生,陈立群.空间机械臂非完整运动规划的遗传算法研究[J].宇航学报,2005,26(3):262-266.(GE Xin-sheng,CHEN Li-qun.Nonholonomic Motion Planning of a Space Manipulator System Using Genetic Algorithm[J].Journal of Astronautics, 2005,26(3):262-266.)

[2] 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.

[3] 梁斌,刘良栋,李庚田.空间机器人的动力学等价机械臂[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.)

[4] 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.

[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].武汉:华中理工大学出版社,1996:55-61.

[8] 王景,王昊瀛.自由飞行空间机器人的运动学建模研究[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.)

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

[10] 王昊瀛,王景,吴宏鑫.平面型两关节空间机器人的自适应轨迹控制[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.)

[11] 徐文福,詹文法.自由飘浮空间机器人系统基座姿态调整路径规划方法的研究[J].机器人,2006,28(3):291-296.(XU Wen-fu, ZHAN Wen-fa.Path Planning for Base Attitude Adjustment of a Free-floating Space Robot System[J].Robot,2006,28(3):291-296.)

[12] 赵晓东,王树国,严艳军等.基于轨迹规划的自由漂浮空间机器人抓取运动物体的研究[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.)

[13] 戈新生,吕杰.带空间机械臂航天器姿态运动规划的数值算法研究[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.)

[14] 戈新生,张奇志,刘延柱.基于遗传算法的空间机械臂的运动规划的最优控制[J].空间科学学报,2000,20(2):185-191.(Ge Xinsheng, Zhang Qizhi, Liu Yanzhu.An Optimized Method to Control the Motion Planning of Space Manipulatorsbase Genetic Algorithm[J].Chinese Journal of Space Science,2000,20(2):185-191.)

[15] Goldberg D E.Real-coded genetic algorithm, virtual alphabets and blocking [J].Complex System, 1991, 5(2):139-167.