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

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

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

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

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

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

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

0 引言

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

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

1 空間機(jī)械臂數(shù)學(xué)模型

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

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

a)機(jī)械臂末端位置的運(yùn)動學(xué)方程

(1)

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

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

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

此處：φ，θ，ψ分別為本體的滾動角、俯仰角和偏航角；θi(i=0～7)為機(jī)械臂的關(guān)節(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é)機(jī)械臂相對第i節(jié)機(jī)械臂固聯(lián)坐標(biāo)系的轉(zhuǎn)換矩陣；AIb為本體系相對慣性系的轉(zhuǎn)換矩陣；AIi為第i節(jié)機(jī)械臂相對慣性系的轉(zhuǎn)換矩陣；rij為第j節(jié)機(jī)械臂固聯(lián)坐標(biāo)系原點(diǎn)相對第i節(jié)的位移；上標(biāo)“×”表示叉乘斜對稱矩陣；Γj為投影矩陣，其取值為[1 0 0]T，[0 1 0]T或[0 0 1]T，與坐標(biāo)系選取有關(guān)。

b)機(jī)械臂末端姿態(tài)的運(yùn)動學(xué)方程

將式(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é)機(jī)械臂的轉(zhuǎn)換矩陣。

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

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

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

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

rtI(tf)=rtIf

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

(3)

式中：t為時刻。

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

(4)

為實(shí)現(xiàn)對機(jī)械臂末端的位置和姿態(tài)運(yùn)動的聯(lián)合控制，需給出機(jī)械臂末端運(yù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

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

J+=JT(J·JT)-1

(6)

另由式(5)可得

(7)

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

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

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

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

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

rtI(tf)=rtIf

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

(8)

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

(9)

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

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

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

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

(10)

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

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

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

(11)

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

(12)

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

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

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

4 結(jié)束語

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

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

[2] 張海濤, 唐立才, 張敬鵬, 等. 空間機(jī)械臂雙臂協(xié)調(diào)操作路徑規(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è)超. 空間機(jī)械臂技術(shù)發(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ī)器人的運(yùn)動學(xué)建模研究[J]. 航天控制, 1999, 17(1): 6-10.

[8] 葛景華. 雙臂空間機(jī)器人運(yùn)動學(xué)、動力學(xué)及控制[D]. 福州: 福州大學(xué), 2005.

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

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

[11] 呂燕, 安凱. 空間6R機(jī)械臂圓弧軌跡規(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—)，男，碩士，主要研究方向?yàn)榭臻g飛行器控制。

TP242.3

A

10.19328/j.cnki.1006-1630.2017.05.005