王明明，羅建軍，余 敏，袁建平

(1. 西北工業(yè)大學(xué)航天飛行動力學(xué)技術(shù)重點(diǎn)實(shí)驗室，西安 710072;2. 西北工業(yè)大學(xué)深圳研究院，深圳 518057)

0 引 言

隨著衛(wèi)星維護(hù)、在軌裝配和空間碎片移除等任務(wù)需求的與日俱增，我們期望利用空間機(jī)器人去執(zhí)行特殊惡劣空間環(huán)境下的任務(wù)。典型實(shí)例有“Robot Technology Experiment(ROTEX)”，“Engineering Test Satellite VII(ETS-VII)”，“軌道快車(Orbit Express, OE)”和“Robonaut 2”等[3]?？臻g機(jī)器人在軌服務(wù)中最具挑戰(zhàn)性的工作之一是抓捕后階段的消旋。抓捕后階段的目的是消除目標(biāo)衛(wèi)星的旋轉(zhuǎn)運(yùn)動，同步穩(wěn)定基座。因此，針對目標(biāo)衛(wèi)星的消旋問題必須研究特定規(guī)劃與控制方法。

文獻(xiàn)中已提出過多種空間機(jī)器人軌跡規(guī)劃和運(yùn)動控制方法?；诹阕饔每臻g(Reaction Null-Space, RNS)的概念，Yoshida等人[1]提出了無反作用機(jī)械臂，解決了運(yùn)動學(xué)非冗余臂和冗余臂時間損失和機(jī)械臂速度限制的問題。此外，文獻(xiàn)[2]中也采用了基于RNS的軌跡規(guī)劃方法，根據(jù)動量守恒定律實(shí)現(xiàn)了抓捕自旋目標(biāo)。文獻(xiàn)[3]中給出對目標(biāo)衛(wèi)星自主抓捕與服務(wù)的框架。之后，Xu等人[4]利用自由漂浮空間機(jī)器人非完整特性提出了一種點(diǎn)到點(diǎn)的路徑規(guī)劃方法，同時調(diào)節(jié)基座姿態(tài)和末端的位姿。Lampariello等人[5]研究了一種機(jī)器人最優(yōu)抓捕機(jī)動目標(biāo)的實(shí)時軌跡規(guī)劃方法，利用B樣條參數(shù)化期望的關(guān)節(jié)軌跡，采用序列二次規(guī)劃方法搜索最優(yōu)解[6]。羊等人[7]針對自由漂浮空間機(jī)器人末端軌跡跟蹤優(yōu)化控制中慣性參數(shù)不確定問題，提出一種組合優(yōu)化控制方法，實(shí)現(xiàn)了系統(tǒng)跟蹤的能量優(yōu)化，同時保證了對期望末端軌跡的有效跟蹤。文獻(xiàn)[8]中應(yīng)用Bézier曲線規(guī)劃關(guān)節(jié)軌跡，采用粒子群優(yōu)化方法求解最優(yōu)化問題。郭等人[9]分析了利用空間機(jī)器人抓捕目標(biāo)衛(wèi)星時產(chǎn)生的碰撞問題，并基于粒子群算法給出了最優(yōu)碰撞前構(gòu)型。此外，結(jié)合RNS和動力學(xué)平衡控制的概念，Wang等人[10]提出一種基于任務(wù)優(yōu)先級的運(yùn)動學(xué)冗余機(jī)械臂路徑規(guī)劃方法。文獻(xiàn)[11]結(jié)合命題邏輯理論和模型預(yù)測控制方法，解決了空間機(jī)器人在含有障礙物工作空間中的最優(yōu)控制問題。針對空間機(jī)器人的運(yùn)動控制問題，考慮到動力學(xué)不確定特性，Xu等人[12]提出了一種針對自由飛行機(jī)器人的自適應(yīng)控制方法。Matsuno和Saito等人[13]引入了一種協(xié)調(diào)和輸入轉(zhuǎn)換算法，將含漂移項的仿射系統(tǒng)轉(zhuǎn)換成時間狀態(tài)控制項。此后，文獻(xiàn)[14]提出了一種考慮避撞的非線性模型預(yù)測控制方法，實(shí)現(xiàn)了末端的路徑跟蹤控制。近年來，多自由度機(jī)械臂的控制中，也可采用滑?？刂芠15]及魯棒精確控制[16]等。

前文所提的研究大都是抓捕前的軌跡規(guī)劃與運(yùn)動控制。當(dāng)機(jī)械臂抓捕一個不可控的衛(wèi)星后，應(yīng)使目標(biāo)盡可能快速停靠，即通過機(jī)械臂逐步對目標(biāo)施加力矩，消除二者間的相對速度。然而，針對最優(yōu)消旋運(yùn)動規(guī)劃的研究卻很少。針對抓捕后階段，文獻(xiàn)[17]中提出了一種基于RNS概念的機(jī)械臂控制方法，將基座的角動量傳到機(jī)械臂，減小了關(guān)節(jié)角速度。Yoshida等人[18]提出了一種分布式動量控制方法，保證對基座姿態(tài)干擾最小情況下將目標(biāo)角動量傳到反作用輪。文獻(xiàn)[19]提出了一種自由漂浮空間機(jī)器人抓捕翻滾目標(biāo)存在模型不確定性時的阻抗控制方法。羅等人[20]針對抓捕后的動力學(xué)參數(shù)辨識問題，提出一種參數(shù)辨識的持續(xù)激勵軌跡設(shè)計方法，提高了空間機(jī)器人參數(shù)辨識的收斂速度并保證其準(zhǔn)確性。然而，這些工作中沒有研究最優(yōu)路徑規(guī)劃方法和基座姿態(tài)穩(wěn)定。基于龐特里亞金極小值原理，文獻(xiàn)[21]中提出一種機(jī)械臂先攔截非合作目標(biāo)衛(wèi)星再消旋的方法，主要考慮消旋時間最少?？紤]了初末邊界條件的不確定性，Abad等人[22]設(shè)計了一種減小對基座姿態(tài)干擾的最優(yōu)控制方法。針對空間機(jī)器人抓捕大慣量目標(biāo)的情形，Zhang等人[23]提出了一種基于自適應(yīng)滑模控制方法的協(xié)同穩(wěn)定技術(shù)。如何消除目標(biāo)的旋轉(zhuǎn)運(yùn)動是未來在軌服務(wù)的關(guān)鍵技術(shù)之一。迄今為止，設(shè)計機(jī)械臂的消旋策略仍是一個具有挑戰(zhàn)性的問題。

本文旨在提出一種利用運(yùn)動學(xué)冗余臂抓捕非合作目標(biāo)的新消旋策略。由于運(yùn)動學(xué)冗余臂存在多解，利用此特性可以滿足多種約束條件?？紤]簡潔性和規(guī)范化因素，選擇Bézier曲線規(guī)劃抓捕后目標(biāo)的路徑。此外，利用自適應(yīng)DE算法搜索最優(yōu)消旋時間和目標(biāo)的終端位姿，并生成末端的參考消旋軌跡。本文推導(dǎo)了一種同時穩(wěn)定基座和末端的協(xié)調(diào)控制方法，主要貢獻(xiàn)是針對運(yùn)動學(xué)冗余空間機(jī)器人抓捕自旋衛(wèi)星后階段，設(shè)計了最優(yōu)消旋策略和協(xié)調(diào)控制方法。

本文組織如下：第1節(jié)介紹了抓捕后階段組合體系統(tǒng)的多體動力學(xué)?；谧顑?yōu)化理論，第2節(jié)推導(dǎo)了利用四階Bézier曲線和自適應(yīng)DE算法的路徑規(guī)劃與消旋策略。第3節(jié)給出了消旋過程中協(xié)調(diào)控制基座姿態(tài)和機(jī)械臂末端的閉環(huán)解。第4節(jié)展示了所提消旋策略與協(xié)調(diào)控制方法應(yīng)用于運(yùn)動學(xué)冗余空間機(jī)器人的仿真結(jié)果。最后，第5節(jié)給出結(jié)論。

1 抓捕后動力學(xué)建模

機(jī)器人系統(tǒng)由基座和n自由度的機(jī)械臂構(gòu)成，共計n+1個部件。抓捕后階段，機(jī)器人和目標(biāo)衛(wèi)星連接成一個組合體，如圖1所示。由文獻(xiàn)[21]，根據(jù)拉格朗日力學(xué)原理，空間機(jī)器人動力學(xué)方程表示如下：

表1 本文所用運(yùn)動學(xué)和動力學(xué)符號Table 1 Kinematic and dynamic symbols used in the paper

(1)

目標(biāo)衛(wèi)星的動力學(xué)方程表示如下：

(2)

式中：mt和It分別是目標(biāo)衛(wèi)星的質(zhì)量和慣性張量。抓捕矩陣Jte表示如下：

(3)

參考式(2)，由機(jī)械臂施加的力和力矩fe可寫成：

(4)

在抓捕后階段，末端與抓捕點(diǎn)速度和加速度關(guān)系建立如下：

(5)

(6)

(7)

將式(4)和式(7)代入式(1)，消除fe項，可得組合體系統(tǒng)的動力學(xué)方程：

(8)

2 消旋策略

利用歐拉方程建立目標(biāo)衛(wèi)星姿態(tài)動力學(xué)：

(9)

參考文獻(xiàn)[21],假設(shè)通過參數(shù)辨識方法可得到目標(biāo)的慣性參數(shù)It。在抓捕前階段，合理假設(shè)目標(biāo)衛(wèi)星不受任何合外力和力矩，即τt=0而且ft=0。

·在可行域內(nèi)優(yōu)化特定的目標(biāo)；

·保證末端執(zhí)行器路徑的連續(xù)性；

·盡量減小運(yùn)動過程中不期望的影響。

2.1 消旋軌跡參數(shù)化

(10)

(11)

(12)

(13)

(14)

(15)

(16)

給定目標(biāo)的終端狀態(tài)φf，通過式(10)～(13)可解得僅含單個變量T的目標(biāo)消旋軌跡。參考式(16)，在給定的約束下，利用二分搜索法可確定最優(yōu)消旋時間T：

T≥max{Ti=min(fi1(T)≥0∩fi2(T)≥0)}，i=1,2,3

(17)

實(shí)際上，不僅要考慮最優(yōu)消旋時間，施加于目標(biāo)的控制力矩也要盡量減小。因此，優(yōu)化過程中，定義目標(biāo)函數(shù)如下，

(18)

ω1,ω2>0分別是最優(yōu)時間和控制力矩的權(quán)重因子。

定義設(shè)計變量φf，消旋策略可轉(zhuǎn)化為：

(19)

如上所述，采用四階Bézier曲線，消旋策略最終轉(zhuǎn)化為以φf為設(shè)計變量的優(yōu)化問題。根據(jù)目標(biāo)自旋運(yùn)動的初始狀態(tài)確定搜索域[φmin,φmax]。一旦確定φf，則可解算出最優(yōu)消旋時間T和目標(biāo)消旋運(yùn)動。推導(dǎo)出末端執(zhí)行器的期望軌跡如下：

(20)

(21)

式中：Rt(φ)是從目標(biāo)體坐標(biāo)系到慣性系的坐標(biāo)轉(zhuǎn)換矩陣，Euler2quat(φ)函數(shù)將以歐拉角形式表示的姿態(tài)轉(zhuǎn)化為以四元數(shù)形式表示。

2.2 微分進(jìn)化算法

(22)

式中：rand(0,1)是[0,1]中生成的隨機(jī)數(shù)。xj,min和xj,max分別是變量xj的下界、上界。

初始化后采取突變策略，生成目標(biāo)向量xi,g對應(yīng)的突變向量vi,g=(vi1,g,vi2,g,…,vid,g)。突變方法包括DE/rand/1、DE/best/2、DE/current-to-best/1等。xbest,g是當(dāng)前整體中的最佳個體。接下來，vi,g和xi,g之間進(jìn)行交叉操作，生成新試驗向量ui,g=(ui1,g,ui2,g,…,uid,g)。通常DE算法采用二項式交叉策略。交叉后，針對試驗向量ui,g評估式(18)中所示的目標(biāo)函數(shù)。根據(jù)貪心算法，只有當(dāng)ui,g越優(yōu)，才能選擇xi,g作為幸存的子代。

DE算法重復(fù)進(jìn)行上述操作生成新一代個體，直到滿足終端標(biāo)準(zhǔn)。如[25]所述，貪心算法可能會導(dǎo)致過早收斂、置信度低，自適應(yīng)參數(shù)機(jī)制有助于提高進(jìn)化種群的多樣性和算法的魯棒性。參考文獻(xiàn)[26]，在第g代迭代中，個體xi,g的縮放因子Fi和交叉速率Gi通過自適應(yīng)參數(shù)機(jī)制生成。自適應(yīng)機(jī)制的目的是記錄近期有效參數(shù)Fi和Gi，以構(gòu)建好的參數(shù)引導(dǎo)生成子代。完成迭代后，自適應(yīng)DE算法將給出T*和p*的最優(yōu)解。參考式(20)和(21)，確定末端執(zhí)行器消除目標(biāo)自旋運(yùn)動的期望軌跡。

3 協(xié)調(diào)控制

(23)

(24)

(25)

注意式(25)闡明了追蹤星與目標(biāo)組合體關(guān)于其在任務(wù)空間的動力學(xué)方程。實(shí)際上，在抓捕后階段保持基座姿態(tài)的穩(wěn)定十分重要，因為：1)需要保持基座上測量儀器和掃描設(shè)備的指向；2)在消旋過程中減小碰撞的風(fēng)險。本文中，不考慮基座位移變化，即不對基座施加控制力而考慮對基座姿態(tài)進(jìn)行穩(wěn)定。廣義力輸入us表示如下：

(26)

為推導(dǎo)簡化形式的動力學(xué)方程(33)，定義如下：

(27)

式中：D11∈R3×3，c1∈R3。可得以下方程：

(28)

(29)

(30)

(31)

(32)

式中：K.p，K.d分別表示位姿和速度的反饋增益，均為正定矩陣。采用單位四元數(shù)q={η,ε}∈R4設(shè)計協(xié)調(diào)控制方法(η是四元數(shù)中標(biāo)量部分，ε是其矢量部分)。q1與q2的四元數(shù)誤差計算如下，其中?是四元數(shù)乘法符號：

(33)

(34)

(35)

(36)

(37)

代入四元數(shù)誤差，計算V關(guān)于時間的導(dǎo)數(shù)：

(38)

4 仿真結(jié)果

為了驗證所提消旋策略和協(xié)調(diào)控制方法的實(shí)用性，本節(jié)給出了采用運(yùn)動學(xué)冗余機(jī)械臂抓捕自旋目標(biāo)的演示實(shí)例。如圖1和圖5所示，追蹤星有6DOF，其機(jī)械臂包含7DOF。表2中列出空間機(jī)器人的運(yùn)動學(xué)和動力學(xué)參數(shù)，其中ai，bi和Ii均表示在其本體坐標(biāo)系下。

4.1 最優(yōu)時間和終端狀態(tài)確定

空間機(jī)器人抓捕目標(biāo)衛(wèi)星的典型過程可粗略地分為抓捕前和抓捕后兩個階段：抓捕前，機(jī)械臂從初始位置接近目標(biāo)上的抓捕點(diǎn)，捕獲目標(biāo)時相對速度最小從而實(shí)現(xiàn)最小碰撞。抓捕點(diǎn)為ρtg=[-0.35,0,0.4275] m。抓捕后，機(jī)械臂需要在給定的約束下實(shí)現(xiàn)目標(biāo)的消旋與穩(wěn)定。本文主要關(guān)注抓捕后階段的消旋策略和控制方法。圖6給出了抓捕前末端執(zhí)行器和抓捕點(diǎn)的軌跡。

基座桿1桿2桿3桿4桿5桿6桿7目標(biāo)m/kg2004.08.02.06.02.02.05.020.0Ixx500.30.80.10.70.10.10.48.4Iyy1000.30.50.10.40.10.10.112.18Izz1000.20.80.10.70.10.10.212.84ax/m0.00.00.00.00.00.00.00.0ay/m0.00.0-0.70.0-0.50.00.00.0az/m0.00.1280.0840.090.0840.090.0840.12bx/m0.90.00.00.00.00.00.00.0by/m0.0840.084-0.66-0.084-0.460.084-0.090.0bz/m-0.60.1280.00.00.00.00.00.21

標(biāo)和空間機(jī)器人的狀態(tài)如下：

(39)

本文中，搜索算法采用帶有自適應(yīng)縮放因子和交叉概率的突變方法，即DE/current-to-best/1。每次迭代中，利用DE算法先確定最優(yōu)消旋時間T，然后利用適應(yīng)度函數(shù)優(yōu)化目標(biāo)的終端狀態(tài)，生成相應(yīng)的消旋路徑。DE算法涉及的參數(shù)分別設(shè)置為np=10，c=0.5。smax=100是最大迭代次數(shù)，作為DE算法的終止條件。

基于給定的初始狀態(tài)，如式(39)，DE算法成功地搜索獲得滿足所有約束的最優(yōu)解T*=4 s，p*=(0.1226,-0.0119,6.725×10-3)。如圖7所示，最佳適應(yīng)度函數(shù)的收斂性與迭代次數(shù)呈負(fù)相關(guān)，適應(yīng)度值的快速遞減顯示了DE算法的搜索能力。就搜索的最優(yōu)解p*和消旋時間T*而言，圖8給出了抓捕點(diǎn)的期望運(yùn)動規(guī)律和所需控制力矩?？梢钥闯?，通過設(shè)計的消旋策略，目標(biāo)在預(yù)設(shè)的約束下可成功地實(shí)現(xiàn)消旋穩(wěn)定。

4.2 協(xié)調(diào)控制仿真

5 結(jié) 論

在抓捕后階段，為了盡快使非合作目標(biāo)衛(wèi)星穩(wěn)定，并考慮控制力矩約束，本文給出了一種空間機(jī)器人最優(yōu)消旋策略和協(xié)調(diào)控制方法。首先，基于廣義速度推導(dǎo)了運(yùn)動學(xué)冗余空間機(jī)器人抓捕非合作目標(biāo)衛(wèi)星后的動力學(xué)方程。然后，采用四階Bézier曲線規(guī)劃消旋軌跡，利用帶自適應(yīng)縮放因子和交叉概率的DE算法搜索給定約束條件下的最優(yōu)終端狀態(tài)，優(yōu)化過程中同時考慮消旋時間和控制力矩最優(yōu)。最后，研究了一種在消除了目標(biāo)的自旋運(yùn)動的同時穩(wěn)定基座姿態(tài)的協(xié)調(diào)控制方法。仿真結(jié)果表明本文所提消旋策略和協(xié)調(diào)控制器的可行性和有效性。

參 考 文 獻(xiàn)

[1] Yoshida K, Hashizume K, Abiko S. Zeros reaction maneuver: flight validation with ets-vii space robot and extension to kinematically redundant arm[C]. Proceedings of the 2001 IEEE International Conference on Robotics and Automation, Seoul, Korea, May 21-26, 2001.

[2] Shah S V, Sharf I, Misra A. Reactionless path planning strategies for capture of tumbling objects in space using a dual-arm robotic system[C]. Proceedings of AIAA Guidance, Navigation, and Control Conference, Boston, MA, Aug 19-22, 2013.

[3] Rouleau G, Rekleitis I, L′Archeveque R, et al. Autonomous capture of a tumbling satellite[C]. IEEE International Conference on Robotics and Automation, Roma, Italy, April 10-14, 2007.

[4] Xu W F, Li C, Wang X, et al. Study on non-holonomic cartesian path planning of a free-floating space robotic system [J]. Advanced Robot, 2009, 23(1-2): 113-143.

[5] Lampariello R, Tuong D, Castellini C, et al. Trajectory planning for optimal robot catching in real-time[C]. Proceedings of the 2011 IEEE International Conference on Robotics and Automation, Shanghai, China, May 9-13, 2011.

[6] Nanos K, Papadopoulos E. Avoiding dynamic singularities in cartesian motions of free-floating manipulators [J]. IEEE Transactions on Aerospace and Electronic Systems, 2015, 51(3): 2305-2318.

[7] 羊帆, 張國良, 原磊,等. 自由漂浮空間機(jī)器人末端軌跡優(yōu)化跟蹤控制[J]. 宇航學(xué)報, 2016, 37(7):846-853. [Yang Fan, Zhang Guo-liang, Yuan Lei, et al. End-effector optimal tracking control of free-floating space robot [J]. Journal of Astronautica, 2016, 37(7):846-853.]

[8] Wang M M, Luo J J, Walter U. Trajectory planning of free-floating space robot using particle swarm optimization (pso) [J]. Acta Astronautica, 2015, 112: 77-88.

[9] 郭聞昊, 王天舒. 空間機(jī)器人抓捕目標(biāo)星碰撞前構(gòu)型優(yōu)化[J]. 宇航學(xué)報, 2015, 36(4):390-396. [Guo Wen-hao, Wang Tian-shu. Pre-impact configuration optimization for a space robot capturing target satellite [J]. Journal of Astronautica, 2015, 36(4):390-396.]

[10] Wang M M, Luo J J, Walter U. Novel synthesis method for minimizing attitude disturbance of the free-floating space robots [J]. Journal of Guidance, Control, and Dynamics, 2016, 39 (3): 695-704.

[11] 宗立軍, 羅建軍, 王明明,等. 自由漂浮空間機(jī)器人多約束混合整數(shù)預(yù)測控制[J]. 宇航學(xué)報, 2016, 37(8):992-1000.[Zong Li-jun, Luo Jian-jun, Wang Ming-ming. A mixed integer predictive controller with multi-constraint for free-floating space robots [J]. Journal of Astronautica, 2016, 37(8):992-1000.]

[12] Xu Y, Shum H Y, Lee J J, et al. Adaptive control of space robot system with an attitude controlled base[C]. Proceedings 1992 IEEE International Conference on Robotics and Automation, Nice, France, May 12-14, 1992.

[13] Matsuno F, Saito K. Attitude control of a space robot with initial angular momentum[C]. Proceedings 2001 IEEE International Conference on Robotics and Automation, Seoul, Korea, May 21-26, 2001.

[14] Wang M M, Luo J J, Walter U. A non-linear model predictive controller with obstacles avoidance for a space robot [J]. Advances in Space Research, 2016, 57: 1737-1746.

[15] Zadeh H, Mohammad S, Saeed K, et al. Optimal sliding mode control of a robot manipulator under uncertainty using pso [J]. Nonlinear Dynamics, 2016, 84(4): 2227-2239.

[16] Wei C S, Luo J J, Dai H H, et al. Globally robust explicit model predictive control of constrained systems exploiting svm-based approximation [J]. International Journal of Robust and Nonlinear Control, 2016.

[17] Nenchev D N, Yoshida K. Impact analysis and post-impact motion control issues of a free-floating space robot subject to a force impulse [J]. IEEE Transactions on Robotics and Automation, 1999, 15(3): 548-557.

[18] Yoshida K, Dimitrov D, Nakanishi H. On the capture of tumbling satellite by a space robot[C]. Proceedings of the 2006 IEEE/RSJ International Conference on Intelligent Robots and Systems, Beijing, China, Oct 9-15, 2006.

[19] Abiko S, Lampariello R, Hirzinger G. Impedance control for a free-floating robot in the grasping of a tumbling target with parameter uncertainty[C]. IEEE/RSJ International Conference on Intelligent Robots and Systems, Beijing, China, Oct 9-15, 2006.

[20] 羅建軍, 薛爽爽, 馬衛(wèi)華,等. 空間機(jī)器人抓捕目標(biāo)后動力學(xué)參數(shù)辨識研究[J]. 宇航學(xué)報, 2016, 37(12):1411-1418. [Luo Jian-jun, Xue Shuang-shuang, Ma Wei-hua, et al. Dynamic parameter identification of free-floating space robots after capturing targets [J]. Journal of Astronautica, 2016, 37(12):1411-1418.]

[21] Aghili F. A prediction and motion-planning scheme for visually guided robotic capturing of free-floating tumbling objects with uncertain dynamics [J]. IEEE Transactions on Robotics, 2012, 28(3): 634-649.

[22] Flores-Abad A, Wei Z, Ma O, et al. Optimal control of space robots for capturing a tumbling object with uncertainties [J]. Journal of Guidance, Control, and Dynamics, 2014, 37(6): 2014-2017.

[23] Zhang B, Liang B, Wang Z, et al. Coordinated stabilization for space robot after capturing a noncooperative target with large inertia [J]. Acta Astronautica , 2017, 134: 75-84.

[24] Storn R, Price K. Differential evolution-a simple and efficient heuristic for global optimization over continuous spaces [J]. Journal of Global Optimization, 1997, 11 (4): 341-359.

[25] Piotrowski A. Review of differential evolution population size [J]. Swarm and Evolutionary Computation, 2017, 32: 1-24.

[26] Zhang J, Sanderson A C. JADE: adaptive differential evolution with optional external archive [J]. IEEE Transactions on Evolutionary Computation, 2009, 13 (5): 945-958.

[27] Siciliano B, Sciavicco L, Villani L, et al, Robotics Modelling, Planning and Control[M]. 1st Edition, Springer, London, 2009.