空間冗余機械臂的路徑規(guī)劃算法研究

陳 撼，裴甲瑞，陸智俊，吳敬玉，陳秀梅

(上海航天控制技術研究所,上海 201109)

空間冗余機械臂的路徑規(guī)劃算法研究

陳 撼，裴甲瑞，陸智俊，吳敬玉，陳秀梅

(上海航天控制技術研究所,上海 201109)

對空間機械臂的路徑規(guī)劃算法進行了研究。針對一種星體和冗余機械臂(7自由度)的星臂聯(lián)合系統(tǒng)，采用星臂聯(lián)合路徑規(guī)劃方法。建立了機械臂末端位置和姿態(tài)的運動學方程。用基于多項式的偽逆路徑規(guī)劃算法對路徑進行規(guī)劃。仿真發(fā)現(xiàn)用三次多項式規(guī)劃算法所得系統(tǒng)運行平滑且穩(wěn)定末端能跟蹤上目標，運行初始和結束時刻角速度達到設計要求，但系統(tǒng)運行結束時角加速度未收斂至零，存在軟沖擊缺陷。增加兩個角加速度約束條件，用五次多項式改進了規(guī)劃算法，仿真發(fā)現(xiàn)該系統(tǒng)解決了軟沖擊問題，且運行更穩(wěn)定。為避免偽逆算法因奇異而失效的固有問題，設計了一種基于五次多項式的偽逆路徑和規(guī)避奇異路徑的聯(lián)合規(guī)劃方法。采用倒數(shù)法規(guī)避奇異問題，當奇異出現(xiàn)時，采用規(guī)避奇異路徑規(guī)劃算法，計算阻尼最小方差廣義逆矩陣；當奇異不出現(xiàn)時，采用基于五次多項式的路徑規(guī)劃算法，計算雅可比矩陣的廣義逆。最后可規(guī)劃出機械臂各關節(jié)的角速度。仿真表明：用該聯(lián)合規(guī)劃方法所得系統(tǒng)在保證運行的精度和穩(wěn)定度的同時，可避免奇異問題，提供了系統(tǒng)的穩(wěn)定性和安全性。研究對空間機械臂設計有一定的參考價值。

空間冗余機械臂； 路徑規(guī)劃； 動力學耦合； 軟沖擊； 奇異； 偽逆路徑規(guī)劃算法； 規(guī)避奇異路徑規(guī)劃算法； 五次多項式

0 引言

隨著空間技術的進步，全球發(fā)射的衛(wèi)星和航天器數(shù)量不斷增多，隨之而來的是空間工作的多樣性和復雜性迅速增長，對艙外活動的需求也日益增加，如對在軌航天器的抓捕、維修和燃料補充，外空間環(huán)境的科學試驗，以及空間垃圾的清理等[1-3]。根據(jù)國際空間站的搭建和維護經(jīng)驗，利用空間機械臂輔助航天員完成空間搭建和載荷維護等任務，可明顯減小航天員出艙風險，減輕航天員的工作壓力，提高空間探索活動的效率[4]。近年來，各航天大國均投入巨資競相開展航天器在軌操控技術研究[5-6]。實踐證明，空間機械臂是一種非常有效的完成在軌操控任務的手段，特別適于對操控精度要求較高的空間精細操控。

動力學建模、路徑規(guī)劃和控制技術是機械臂在軌操控的三大核心技術[7-8]。本文研究了空間機械臂的路徑規(guī)劃技術，并重點解決兩大問題：一是在空間微重力環(huán)境中，動量守恒的約束使機械臂與星體間存在動力學耦合問題；二是基于多項式的偽逆路徑規(guī)劃算法的固有缺陷導致的奇異問題[9]。對此，本文分別設計了星臂聯(lián)合路徑規(guī)劃方法(即將星體和冗余機械臂(7自由度)結合成一個系統(tǒng)，進行聯(lián)合建模)和常規(guī)路徑規(guī)劃算法與規(guī)避奇異規(guī)劃算法的聯(lián)合路徑規(guī)劃方法[10]。同時，針對系統(tǒng)軟性沖擊的問題提出了基于五次多項式的偽逆路徑規(guī)劃算法，并進行了仿真驗證。

1 空間機械臂數(shù)學模型

本文研究的是對象是星體和冗余機械臂(7自由度)組成的星臂聯(lián)合系統(tǒng)，如圖1所示。采用星臂聯(lián)合路徑規(guī)劃方法，可有效解決機械臂與星體間存在的動力學耦合問題，同時避免了數(shù)學模型因增加解決耦合問題的算法而帶來的復雜度，而且增加了系統(tǒng)運行的靈活度(增加了3自由度)，豐富了系統(tǒng)的運行軌跡。同時，該方法能大量節(jié)約由于抵消機械臂運行對星體產(chǎn)生的擾動而消耗的燃料。

建立該系統(tǒng)的運行學數(shù)學模型如下。

a)機械臂末端位置的運動學方程

(1)

式中：q，JL(q)分別為本體與機械臂聯(lián)合角位移列向量和位置雅克比矩陣，且

q=[φθψθ1θ2…θ7]T

JL(q)=[JL0JL1JL2…JL6JL7]3×10

此處：φ，θ，ψ分別為本體的滾動角、俯仰角和偏航角；θi(i=0～7)為機械臂的關節(jié)角；

JL1=-(AI1(r12)×+AI2(r23)×A21+AI3(r34)×A31+AI4(r45)×A41+AI5(r56)×A51+AI6(r67)×A61+AI7(rt)×A71)Γ1

JL2=-(AI2(r23)×+AI3(r34)×A32+AI4(r45)×A42+AI5(r56)×A52+AI6(r67)×A62+AI7(rt)×A72)Γ2

JL3=-(AI3(r34)×+AI4(r45)×A43+AI5(r56)×A53+AI6(r67)×A63+AI7(rt)×A73)Γ3

JL4=-(AI4(r45)×+AI5(r56)×A54+AI6(r67)×A64+AI7(rt)×A74)Γ4

JL5=-(AI5(r56)×+AI6(r67)×A65+AI7(rt)×A75)Γ5

JL6=-(AI6(r67)×+AI7(rt)×A76)Γ6

JL7=-(AI7(rt)×)Γ7

此處：Aij為第j節(jié)機械臂相對第i節(jié)機械臂固聯(lián)坐標系的轉換矩陣；AIb為本體系相對慣性系的轉換矩陣；AIi為第i節(jié)機械臂相對慣性系的轉換矩陣；rij為第j節(jié)機械臂固聯(lián)坐標系原點相對第i節(jié)的位移；上標“×”表示叉乘斜對稱矩陣；Γj為投影矩陣，其取值為[1 0 0]T，[0 1 0]T或[0 0 1]T，與坐標系選取有關。

b)機械臂末端姿態(tài)的運動學方程

將式(1)寫為矩陣形式

(2)

JA0=AI7(et)×A7b

JA1=AI7(et)×A71Γ1

JA2=AI7(et)×A72Γ2

JA3=AI7(et)×A73Γ3

JA4=AI7(et)×A74Γ4

JA5=AI7(et)×A75Γ5

JA6=AI7(et)×A76Γ6

此處：A7b為本體系相對第7節(jié)機械臂的轉換矩陣。

2 基于多項式的偽逆路徑規(guī)劃方法

采用基于多項式的偽逆路徑規(guī)劃算法。該算法是通過機械臂末端位姿變化相對本體的姿態(tài)及機械臂關節(jié)角速率間的關系建立的基于廣義雅可比矩陣的逆軌跡規(guī)劃，可對目標的相對運動進行反饋補償，能實時對運動進行精細的調整與控制，具精度高的優(yōu)點。

為使運動較平緩，不出現(xiàn)劇烈的變化，可設計一個二階連續(xù)光滑的函數(shù)，并使之滿足上述約束條件即可。一般可用圓弧函數(shù)、多項式函數(shù)、三角函數(shù)或樣條插值函數(shù)等，本文采用多項式函數(shù)[11]。

設初始時刻為t0，末端參考點相對慣性坐標系原點的位置為rtI0，終止時刻為tf，末端參考點相對位置為rtIf。則對所要設計的位置運動規(guī)律rtI(t)來說，需滿足初始時刻和終止時刻的位置，速度的約束條件，即

rtI(tf)=rtIf

本文采用三階多項式，得到末端位置的運動規(guī)律為

(3)

式中：t為時刻。

設計姿態(tài)運動律時，考慮參考矢量的方位角e(t)，設初始時刻t0時的值為e0，終止時刻tf時的值為ef，則由與位置運動規(guī)律類似的方法，可得姿態(tài)運動的變化規(guī)律為

(4)

為實現(xiàn)對機械臂末端的位置和姿態(tài)運動的聯(lián)合控制，需給出機械臂末端運動的位置姿態(tài)聯(lián)合方程。定義位置姿態(tài)聯(lián)合矢量為XE，XE=[rtIetI]T。

合并式(3)、(4)，有

(5)

式中：J(q)為雅克比矩陣，且

J(q)=[JL(q)JA(q)]T

求解各關節(jié)角的運動規(guī)律，須對J求逆，但J不為方陣，不一定可逆，故需用J的Moore-Pseudo廣義逆矩陣進行計算[12-13]。根據(jù)時刻t的q(t)求解矩陣J(q)，并進而求解J的廣義逆矩陣J+。其求解結果為

J+=JT(J·JT)-1

(6)

另由式(5)可得

(7)

將式(6)求出的J+和前文獲得的位置姿態(tài)矢量的變化規(guī)律XE(t)代入式(8)即可求出各關節(jié)角的角速度變化規(guī)律。

用本文的路徑規(guī)劃算法進行數(shù)值仿真。選擇一個3-1-3(肩、肘、腕)構型的關節(jié)型機械臂作為仿真目標，該機械臂的最大臂展長度1.1 m。機械臂的空間構型和機械臂的參數(shù)分別如圖2、3所示。

用本文的路徑規(guī)劃算法仿真所得航天器本體和機械臂各關節(jié)角的姿態(tài)角、角速度、角加速度結果分別如圖4～9所示。

由圖4～9可知：系統(tǒng)運行平滑且穩(wěn)定，且末端跟蹤上指定目標，運行初始和結束時的角速度為零，達到預定設計要求，但系統(tǒng)運行結束時的角加速度并未收斂至零，這會對系統(tǒng)產(chǎn)生軟沖擊，降低星臂聯(lián)合系統(tǒng)的末端跟蹤精度和穩(wěn)定度，而且會顯著縮短電機的使用壽命，甚至會破壞整個系統(tǒng)。因此，需對該系統(tǒng)進行改進，解決該算法的軟性沖擊缺陷。

對要設計的位置運動規(guī)律rtI(t)來說，需滿足初始時刻和終止時刻的位置、速度和加速度約束條件

rtI(tf)=rtIf

為使運動更平緩，運動結束時不出現(xiàn)大的角加速度變化，本文采用五次多項式進行參數(shù)化處理的方法，則末端位置的運動規(guī)律為

(8)

同樣，用與位置運動規(guī)律完全類似的方法，可得末端姿態(tài)的運動規(guī)律為

(9)

用本文的改進規(guī)劃算法仿真所得航天器本體和機械臂各關節(jié)角的姿態(tài)角、角速度、角加速度結果分別如圖10～15所示。

由圖10～15可知：改進后的系統(tǒng)本體姿態(tài)角和機械臂關節(jié)角、角速度與角加速度在運行開始和結束時都收斂至0。在不影響基于多項式的偽逆路徑規(guī)劃算法條件下，徹底解決了軟沖擊的缺陷問題，在提高系統(tǒng)的末端跟蹤精度、穩(wěn)定度的同時解決了系統(tǒng)的潛在風險。用該改進方法設計仿真的角度，角速度和角加速度的結果顯示：曲線更平滑，明顯降低了電機負荷，又提高了系統(tǒng)的運行穩(wěn)定性。同時，該算法只增加了兩個約束方程，并未增加算法的復雜度，不影響整個系統(tǒng)的運行。但偽逆算法的固有缺陷(J·JT奇異的問題)，一旦系統(tǒng)運行至奇異區(qū)，會導致該算法失效，因此需設計一個規(guī)避奇異的方法進行改進。

3 規(guī)避奇異的聯(lián)合路徑規(guī)劃方法

基于運動學的算法，將機械臂末端的位置和姿態(tài)變化路徑通過雅可比矩陣的偽逆轉換為航天器本體姿態(tài)及機械臂各關節(jié)角的運行路徑過程中，有時會遇到矩陣J·JT接近奇異的情況，此時雅可比矩陣的偽逆無法求解，或所得的角速度幅值很大，導致路徑規(guī)劃算法的失效。這個固有缺陷限制了該算法的應用。因此，需用規(guī)避奇異的路徑規(guī)劃方法解決此問題。本文用阻尼倒數(shù)法規(guī)避奇異問題，其原理是：在J·JT接近奇異值(J·JT的模值等于或接近于0)時，該矩陣的廣義逆矩陣J+=Jt(J·JT)-1會出現(xiàn)無限大的值，設計對倒數(shù)添加一阻尼系數(shù)，在所給系統(tǒng)精度損失的可控范圍內(nèi)使系統(tǒng)快速脫離奇異區(qū)間。

(10)

阻尼倒數(shù)(取λ=0.1)與倒數(shù)比較(k=1～10)如圖16所示。

由圖16可知：當設計的阻尼系數(shù)相對系數(shù)k的絕對值很小時，阻尼倒數(shù)能保證近似等于系數(shù)的倒數(shù)。針對多自由度的機械臂系統(tǒng)的規(guī)避奇異路徑規(guī)劃算法，即為對雅可比矩陣的J+進行修改(即阻尼最小方差廣義逆矩陣)，有

J+=JT(J·JT+λ2I)-1

(11)

式中：I為單位陣。將式(11)代入式(7)可得

(12)

由上述分析可知：阻尼倒數(shù)能保證一定的精度條件，但無法避免誤差；基于五次多項式的路徑規(guī)劃算法不能規(guī)避奇異。為解決該問題，可聯(lián)合基于五次多項式的路徑規(guī)劃算法與規(guī)避奇異路徑規(guī)劃方法，具體為：設計λ=0.1，系統(tǒng)正常運行(J·JT的模值較大，取≥4)時，為保證精度，采用基于五次多項式的路徑規(guī)劃算法；當J·JT發(fā)生奇異或接近奇異(即J·JT的模等于0或極小值)時，則切換為規(guī)避奇異路徑規(guī)劃方法。聯(lián)合路徑規(guī)劃方法的流程如圖17所示。

用聯(lián)合路徑規(guī)劃方法仿真所得航天器本體和機械臂各關節(jié)角的姿態(tài)角、角速度、角加速度結果分別如圖18～23所示。

由圖18～23可知：系統(tǒng)各關節(jié)角的角度、角速度和角加速度變化曲線平滑且穩(wěn)定，在奇異區(qū)間并未出現(xiàn)突變或極大值的現(xiàn)象。分析可知：規(guī)避奇異的聯(lián)合路徑規(guī)劃方法在能保證系統(tǒng)運行的精度和穩(wěn)定度的同時，解決了偽逆路徑規(guī)劃方法奇異的固有缺陷，提高了系統(tǒng)的穩(wěn)定性和安全性。

4 結束語

本文設計了星臂聯(lián)合路徑規(guī)劃方法，避免了數(shù)學模型因增加解決耦合問題的算法而帶來的復雜度的同時，增加了系統(tǒng)運行的靈活度(增加了3自由度)，豐富了系統(tǒng)的運行軌跡。同時，該方法能大量節(jié)約用于抵消機械臂運動對星體的擾動而消耗的燃料。針對該模型，采用基于多項式的偽逆路徑規(guī)劃算法，該法具有算法簡便、運行軌跡平滑等優(yōu)點，并用仿真進行了驗證。根據(jù)系統(tǒng)軟沖擊的缺陷分析，提出了該方法的改進算法，解決了系統(tǒng)軟沖擊的問題，提高了系統(tǒng)的精度、穩(wěn)定度。為解決偽逆路徑規(guī)劃算法奇異的缺陷，采用了聯(lián)合規(guī)避奇異路徑規(guī)劃算法，并通過仿真得到驗證。路徑規(guī)劃技術是空間機械臂的三大核心技術之一，對在軌捕獲、維修、對接等任務有重要的意義。但本文的空間機械臂路徑規(guī)劃過程中，并未實現(xiàn)實時避障、本體姿態(tài)零擾動等功能，后續(xù)研究將改進算法以實現(xiàn)相應功能。

[1] 吳立成, 孫富春, 孫增析. 空間機器人建模、規(guī)劃與控制研究現(xiàn)狀[J]. 中南大學學報, 2005, 36(1): 18-24.

[2] 張海濤, 唐立才, 張敬鵬, 等. 空間機械臂雙臂協(xié)調操作路徑規(guī)劃算法[J]. 控制工程, 2015, 22(6): 1028-1032.

[3] EVANS B G, THOMPSON P T, CORAZZA G E, et al. 1945-2010: 65 years of satellite history from early visions to latest missions[J]. Proceedings of the IEEE, 2011, 99(11): 1840-1857.

[4] 劉宏, 蔣再男, 劉業(yè)超. 空間機械臂技術發(fā)展綜述[J]. 載人航天, 2015, 21(5): 435-436.

[5] HIRZINGER G, BRUNNER B, LAMPARIELLO R, et al. Advances in orbital robotics[C]// Proceedings of the IEEE international Conference on Robotics and Automation. San Francisco: IEEE, 2000: 898-907.

[6] WHELAN D A, ADLER E A, WILSON S B. Effecting a revolution in space-based systems[J]. Proceedings of SPIE, The International Society for Optical Engineering, 2000: 48-56.

[7] 王景, 王昊瀛, 劉良棟. 自由飛行空間機器人的運動學建模研究[J]. 航天控制, 1999, 17(1): 6-10.

[8] 葛景華. 雙臂空間機器人運動學、動力學及控制[D]. 福州: 福州大學, 2005.

[9] 丁希侖, 戰(zhàn)強, 解玉文. 自由漂浮的空間機器人系統(tǒng)的動力學奇異特性分析及其運動規(guī)劃[J]. 航空學報, 2001, 22(5): 474-477.

[10] 董曉星, 李戈, 劉剛峰, 等. 冗余空間機械臂的運動學和笛卡爾阻抗控制方法[J]. 中國機械工程, 2014, 25(1): 36-37.

[11] 呂燕, 安凱. 空間6R機械臂圓弧軌跡規(guī)劃及仿真[J]. 上海航天, 2012, 29(6): 63-67.

[12] OPTIMUM G T. Trajectory planning for redundant and hyper redundant manipulators through inverse dynamics[C]// Proceedings of 17th International Conference on Methods and Models in Automation and Robotics. Albuquerque: IEEE, 2012: 27-30.

[13] GASPARETTO A, ZANOTTO V. Optimal trajectory planning for industrial robots[J]. Advances in Engineering Software, 2010, 41: 548-556.

StudyonPathPlanningMethodofSpaceRedundancyManipulatorSystem

CHEN Han， PEI Jia-rui， LU Zhi-jun， WU Jing-yu， CHEN Xiu-mei

(Shanghai Institute of Spacecraft Control Technology, Shanghai 201109, China)

The path planning method of space redundancy manipulator system was studied in this paper. The satellite and manipulator combined path planning was adopted for a satellite and manipulator system with 7 degree of freedom. The kinematics equations of the position and attitude of the manipulator end were established. The path was planned by pseudo-inverse path planning method based on polynomial. The simulation found that the operation of the system planned by cubic polynomial path planning method was smooth and the manipulator end could track the target. The angular velocity met the design requirement at the initial and finishing time. But the angular acceleration would not converge on zero when the system operation was completed, which meant that there was flexible strike. The path planning method was improved by adding two angular constrained conditions based on quintic polynomial. The simulation found that the flexible strike was eliminated and the system planned by the improved path planning method would operate much smoothly. To avoid the failure of pseudo-inverse path planning because of singularity, a new planning method united pseudo-inverse path planning based on quintic polynomial and singularity avoiding planning was designed. The singularity was avoided by reciprocal method. When the singularity appeared, the singularity avoiding path planning was adopted and calculated damp minimum variance generalized inverse matrices. When the singularity did not appear, the pseudo-inverse path planning based on quintic polynomial was adopted and calculated generalized inverse of Jacobian matrix. At last, the angular velocities of each joint of the manipulator were planned. The simulation showed that the system planned by the new united planning method could avoid singularity when the operation accuracy and smooth were guaranteed, which provided the stability and safety of the redundancy space manipulator system. The study has some valuable for the design of the space manipulator system.

space redundancy manipulator system; path planning; dynamic coupling; flexible strike; singularity; pseudo-inverse path planning; singularity avoidance path planning; quintic polynomial

1006-1630(2017)05-0030-10

2016-12-23；

2017-04-10

陳 撼(1991—)，男，碩士，主要研究方向為空間飛行器控制。

TP242.3

A

10.19328/j.cnki.1006-1630.2017.05.005