基于人工勢場法的月球表層采樣裝置避障規(guī)劃

鄭燕紅 鄧湘金 彭兢 姚猛 趙志暉

(北京空間飛行器總體設(shè)計部，北京 100094)

基于人工勢場法的月球表層采樣裝置避障規(guī)劃

鄭燕紅 鄧湘金 彭兢 姚猛 趙志暉

(北京空間飛行器總體設(shè)計部，北京 100094)

表層采樣技術(shù)是獲取地外天體土壤樣品的重要手段，為在月表復(fù)雜地形、不確定著陸姿態(tài)、不規(guī)則器表包絡(luò)約束下，快速獲取安全的表層采樣動態(tài)軌跡，文章根據(jù)四自由度表層采樣裝置設(shè)計，分析了工作過程中的正逆運動學(xué)關(guān)系；根據(jù)表層采樣裝置動、靜態(tài)目標點運動要求，將約束空間分解為有效障礙檢測區(qū)與不可達集，進行表層采樣避障規(guī)劃；為加快規(guī)劃速度，針對表層采樣裝置瞬態(tài)構(gòu)型構(gòu)建了動態(tài)障礙檢測區(qū)、動態(tài)不可達集；結(jié)合人工勢場法提出了避障規(guī)劃算法。對典型姿態(tài)下表層采樣過程的避障規(guī)劃仿真表明，該算法可實現(xiàn)約束條件下的表層采樣軌跡規(guī)劃，并對表層采樣裝置適應(yīng)性設(shè)計提供支持。

表層采樣；機械臂；四自由度；避障規(guī)劃；人工勢場法；月球探測

1 引言

中國探月工程確定了繞、落、回三步走的發(fā)展思路[1]，從月球上采集樣品返回地球進行科學(xué)分析將是中國深空探測技術(shù)中重要的一步。美國先后發(fā)射的“海盜號”(Viking)、“勇氣號”與“機遇號”(Spirit，Opportunity)、“鳳凰號”(Phoenix)、“好奇號”(Curiosity)火星探測器上均采用機械臂帶動采樣器或科學(xué)設(shè)備完成表層采樣或科學(xué)探測，歐洲航天局(ESA)“獵兔犬2(Beagle 2)著陸器也攜帶了四自由度機械臂獲取火星表面基礎(chǔ)數(shù)據(jù)，俄羅斯“福布斯”(Phobos Grunt)采樣返回任務(wù)采用了機械臂配置吸納式采樣器完成“火衛(wèi)一”表層土壤的獲取[2]。目前計劃實施的“奧特西斯”(OSIRIS)、“馬可波羅”(Marco Polo-R)等近地小行星采樣返回任務(wù)也規(guī)劃了機械臂與采樣器配合完成表層采樣工作。從國內(nèi)外成功實施和計劃實施的地外天體采樣任務(wù)來看，表層采樣技術(shù)是獲得地外土壤樣品的重要手段，開展表層采樣關(guān)鍵技術(shù)研究，可為中國月球采樣返回任務(wù)提供支持。

目前，表層采樣裝置通常采用二連桿機械臂配置采樣器的形式，具有多個運動自由度[3-4]，實現(xiàn)表層土壤樣品采集與轉(zhuǎn)移。機械臂的避障規(guī)劃方法主要有Lozano-Perez提出的C空間方法[5]和Khatib提出的人工勢場法[6]。文獻[7]針對空間機械臂模型和障礙物模型進行了簡化，得到了桿件與障礙物碰撞條件的解析表達式，將空間障礙映射到關(guān)節(jié)空間，并利用A*啟發(fā)式搜索算法在自由空間進行了路徑搜索；文獻[8]利用人工勢場法對采樣器與月面巖石障礙物進行了避障規(guī)劃，但未考慮與探測器表面安裝設(shè)備的碰撞約束。

本文針對四自由度表層采樣裝置設(shè)計，分析了工作過程中的正逆運動學(xué)關(guān)系，根據(jù)月球探測器構(gòu)型布局及表取采樣任務(wù)的特點，歸納了在笛卡兒空間表層采樣軌跡規(guī)劃的約束；基于人工勢場法提出了有效障礙檢測區(qū)、局部不可達檢測區(qū)、鄰域不可達集相結(jié)合的避障規(guī)劃算法；并針對水平姿態(tài)、傾斜姿態(tài)下表層采樣過程，進行了軌跡規(guī)劃仿真驗證，可實現(xiàn)對障礙的規(guī)避，有效到達目標點。

2 表層采樣裝置運動關(guān)系

圖1 表層采樣裝置示意

表層采樣裝置由如圖1所示的4個旋轉(zhuǎn)關(guān)節(jié)、2段連桿及采樣器組成。表層采樣裝置通過關(guān)節(jié)的回轉(zhuǎn)運動帶動采樣器到達期望采樣點，實現(xiàn)對土壤樣品的鏟挖。在表層采樣裝置上建立基座坐標系O0X0Y0Z0，腰關(guān)節(jié)坐標系J1X1Y1Z1，肩關(guān)節(jié)坐標系J2X2Y2Z2，肘關(guān)節(jié)坐標系J3X3Y3Z3，腕關(guān)節(jié)坐標系J4X4Y4Z4，為使圖示更加簡潔，圖1中各坐標系均未繪制Y軸。

表層采樣裝置的DH(Denavit-Hartenberg)參數(shù)[9]見表1。

表1 表層采樣裝置DH參數(shù)

(1)

式中ci，si表示cos(θi)，sin(θi)；cαi-1表示cos(αi-1)；sαi-1表示sin(αi-1)。由表層采樣裝置相鄰坐標系轉(zhuǎn)換關(guān)系，可得腕關(guān)節(jié)坐標系至基座坐標系變換矩陣：

(2)

式中c23，s23，c234，s234表示下標對應(yīng)關(guān)節(jié)角相加的余弦或正弦。

設(shè)[nx,ny,nz],[ox,oy,oz],[ax,ay,az]為腕關(guān)節(jié)坐標系X4、Y4、Z4軸單位矢量在基坐標系各坐標軸的分量，給定期望分量nz,對于點(x，y，z)可確定目標位姿矩陣為

(3)

通過逆運動學(xué)求解關(guān)節(jié)變量(θ1，θ2，θ3，θ4)，可得：

(4)

3 表層采樣任務(wù)及約束

圖2 表層采樣主要目標點示意

表層采樣裝置隨探測器著陸月面后，處于收攏壓緊狀態(tài)，需通過在月面的運動完成展開、采樣、放樣、轉(zhuǎn)移、釋放等過程，運動過程中表層采樣裝置末端先后經(jīng)過壓緊點、采樣點、放樣點、釋放點等目標點，在各個目標點采樣器將保持不同的姿態(tài)，如圖2所示。

采樣前，表層采樣裝置收攏壓緊于探測器下艙體頂面，位于設(shè)備A、B下方，設(shè)備C、D內(nèi)側(cè)，一次封裝容器上方，壓緊點位置由收攏壓緊狀態(tài)完全確定，在表層采樣裝置基座坐標系中為靜態(tài)目標點。

采樣過程中，表層采樣裝置關(guān)節(jié)進行回轉(zhuǎn)運動，驅(qū)動采樣器到達采樣點實施土壤樣品采集；表層采樣裝置第一段連桿位于設(shè)備E、F附近區(qū)域；采樣點選擇受著陸區(qū)地形、科學(xué)探測需求、探測器著陸姿態(tài)等影響。任務(wù)執(zhí)行前，采樣點位置具有不確定性，此外選取多個采樣點可豐富采集樣品的特性，這也對采樣點的位置帶來了變化。采樣點在表層采樣裝置基座坐標系中為動態(tài)目標點。

樣品采集完成后，表層采樣裝置將獲取的土壤樣品轉(zhuǎn)移至放樣點，并將樣品傾瀉至安裝于探測器下艙體頂面的一次封裝容器中。經(jīng)多次采樣后，表層采樣裝置抓取一次封裝容器，經(jīng)過設(shè)備A附近區(qū)域，轉(zhuǎn)移至二次封裝容器上方的釋放點，完成對準分離動作；一次封裝容器進入二次封裝容器中，由于放樣時的一次封裝容器、釋放時的二次封裝容器均處于固定安裝狀態(tài)，放樣點、釋放點在表層采樣裝置基座坐標系中為靜態(tài)目標點。

圖3 表層采樣不可達影響示意

然而，表層采樣裝置各目標點間的運動是在笛卡兒空間完成的，受表層采樣裝置工作空間的限制，運動規(guī)劃應(yīng)保證運動軌跡的可達性。當(dāng)表層采樣裝置連桿2比連桿1略短時，以表層采樣裝置基座安裝點為球心的內(nèi)部區(qū)域不可達，當(dāng)起始點與目標點處于如圖3所示位置時，表層采樣裝置從起始點運動至目標點過程中，在眾多規(guī)劃中最為快捷的應(yīng)為軌跡2，但軌跡2的大部分中間點進入了不可達區(qū)域，在運動規(guī)劃中應(yīng)尋找類似與軌跡1的可避讓不可達區(qū)的規(guī)劃軌跡，確保運動規(guī)劃的可達性。

此外，從前面任務(wù)分析看，表層采樣裝置在各目標點間的運動軌跡，受探測器上各設(shè)備的限制，運動規(guī)劃應(yīng)具有對設(shè)備的避障能力。從圖2中表層采樣任務(wù)各目標點間的相對位置來看，表層采樣裝置從收攏壓緊狀態(tài)展開過程中，先需考慮表層采樣裝置與設(shè)備C、設(shè)備D、探測器下艙體頂面可能發(fā)生的碰撞，當(dāng)表層采樣裝置轉(zhuǎn)動一定角度后，需考慮上方設(shè)備A、B對其轉(zhuǎn)動的約束；表層采樣裝置向工程任務(wù)確定的采樣點或從采樣點至放樣點運動過程中，需考慮與探測器下艙體側(cè)面、著陸腿、設(shè)備E、設(shè)備F可能發(fā)生的碰撞；表層采樣裝置抓取一次封裝容器過程中，設(shè)備A、探測器上艙體側(cè)面可能成為其運動路徑上的障礙；當(dāng)一次封裝容器接近二次封裝容器時，應(yīng)避免表層采樣裝置與探測器上艙體頂面、二次封裝裝置發(fā)生碰撞?？梢姡韺硬蓸友b置在運動過程中，探測器上障礙物較多，障礙物相對較為分散，并影響展開、采樣、放樣、轉(zhuǎn)移、釋放等各個過程。

4 避障規(guī)劃算法

表層采樣裝置在月面工作過程中，需結(jié)合科學(xué)探測目標、探測器著陸姿態(tài)、采樣裝置安裝點與月面距離等[10-11]，根據(jù)展開、采樣、放樣、轉(zhuǎn)移、釋放等動作要求，確定各關(guān)鍵點的目標位置及姿態(tài)。由于表層采樣裝置運動過程中受到障礙、不可達區(qū)域的約束，基于人工勢場法的思想[12]，將不可達點看作一類特殊的障礙點，對障礙點、不可達點分別建立斥力勢場；與障礙距離越小，斥力越大，當(dāng)表層采樣裝置的點進入障礙影響域時，則發(fā)生碰撞。對目標點建立引力勢場，與目標距離越大，吸引力越大，當(dāng)有效規(guī)劃點進入目標點鄰域時，則規(guī)劃完成。

為構(gòu)建引力勢場，在表層采樣裝置上選取特定吸引力控制點(xj,yj,zj)，j=1，2，…，k，采用目標引力勢場函數(shù)Ua：

(5)

為構(gòu)建障礙點的斥力勢場，在表層采樣裝置上選取動態(tài)斥力控制點，采用障礙斥力勢場函數(shù)Uro：

(6)

式中vmom表示動態(tài)斥力控制點(xm，ym，zm)與距離最近的障礙物點(xom，yom，zom)的向量，m=1，2，…，t；ρ0為障礙點的影響域半徑;ηm為斥力加權(quán)系數(shù)。由于表層采樣裝置運動過程中的障礙設(shè)備較多，若在每個運動點均引入所有的障礙點，計算量將十分龐大。從第3節(jié)表層采樣任務(wù)及約束可知，在表層采樣裝置特定運動點，部分障礙設(shè)備與之距離較遠，可忽略該類設(shè)備產(chǎn)生的障礙點，如圖4所示。通過選擇表層采樣裝置特征點J1、J2、N1、J3、N2、J4，劃定表層采樣裝置當(dāng)前構(gòu)型狀態(tài)下的有效障礙檢測區(qū)DEFG-D′E′F′G′，設(shè)備C進入該有效障礙檢測區(qū)內(nèi)，但設(shè)備A、設(shè)備B處于該區(qū)域之外，動態(tài)斥力控制點搜索過程中將只對設(shè)備C上的點進行計算。

圖4 有效障礙檢測區(qū)、局部不可達檢測區(qū)、鄰域不可到達集示意

為避免進行全空間不可達點檢測而引起的時間消耗，根據(jù)起始點、目標點位置坐標確定局部不可達檢測區(qū)，如圖4所示，以L點為起始點，Q′為目標點，可劃定局部不可達檢測區(qū)為KLPQ-K′L′P′Q′，搜索該區(qū)域內(nèi)所有不可達點，組成局部不可達集A，然而從起始點到目標點的可選路徑并不完全限定在KLPQ-K′L′P′Q′區(qū)域內(nèi)，當(dāng)運動軌跡跳出該區(qū)域如到達M點時，即將進入另一不可達區(qū)域，此時在M點構(gòu)建鄰域不可到達檢測區(qū)，其交集即為不可到達集B。局部不可到達集A、鄰域不可到達集B組成了表層采樣裝置當(dāng)前位姿下的不可到達集，為構(gòu)建不可達集中各點的斥力勢場，在表層采樣裝置上選取腕關(guān)節(jié)坐標系原點J4為可達判斷點，采用如下不可達斥力勢場函數(shù)Uru：

(7)

表層采樣裝置從當(dāng)前位置運動至目標位置過程中，選定吸引力沿引力勢場的負梯度方向，斥力沿斥力勢場的負梯度方向，即：

(8)

因此，表層采樣裝置工作過程中的運動，可轉(zhuǎn)化為目標點引力Fa、障礙點和不可到達點斥力Fr加權(quán)合力作用下的位置迭代過程，使勢場函數(shù)Ua+Uro+Uru最小化。表層采樣裝置的備選規(guī)劃點坐標迭代計算公式為

(9)

根據(jù)上述方法，表層采樣裝置避障規(guī)劃算可歸納如圖5所示。

圖5 表層采樣避障規(guī)劃算法流程

5 仿真分析

本文假設(shè)在表層采樣任務(wù)中壓緊、采樣、放樣、釋放等主要目標點位置及姿態(tài)(在基座坐標系中)如表2所示，以表層采樣裝置連桿1末端點、肘關(guān)節(jié)坐標系原點、連桿2末端點、腕關(guān)節(jié)坐標系原點為吸引力控制點，選取系數(shù)λ1=λ2=λ3=0.1，λ4=1，α=2，β=1，ρ0=ρ1=γ=20mm。考慮到探測器著陸月面姿態(tài)的不確定性，分別在0°、±8°(以表層采樣裝置安裝點、當(dāng)?shù)刂亓€構(gòu)成的平面內(nèi)傾斜為例)姿態(tài)下，結(jié)合如圖2所示的設(shè)備布局約束，利用如圖5所示的避障規(guī)劃算法進行軌跡規(guī)劃仿真，規(guī)劃軌跡如圖6所示。

表2 表層采樣仿真主要目標點位姿

圖6 表層采樣規(guī)劃軌跡示意

從圖6可見，表層采樣裝置在接近或離開采樣點、接近二次封裝容器過程中，規(guī)劃軌跡接近直線，說明在該過程中表層采樣裝置主要受目標點吸引力作用，而障礙點、不可到達點產(chǎn)生的斥力對其影響十分微弱，表層采樣裝置以盡可能短的路徑到達目標點；在展開初始過程、接近或離開一次封裝容器附近區(qū)域過程中，規(guī)劃軌跡彎曲程度較大，說明在該過程中由于障礙點或不可到達點產(chǎn)生的斥力對運動軌跡方向進行了有效的調(diào)整，避免與障礙設(shè)備碰撞或進入不可到達區(qū)域，并最終到達目標點。從探測器上設(shè)備布局情況來看，在選定的采樣點區(qū)域、二次封裝容器安裝區(qū)域，對表層采樣裝置運動構(gòu)成障礙的設(shè)備較少，而在一次封裝容器附近區(qū)域，構(gòu)成障礙的設(shè)備相對較多，這與規(guī)劃軌跡所反映的情況一致，說明避障規(guī)劃算法合理有效。

6 結(jié)束語

1)月球表層采樣過程中，由于探測器器表包絡(luò)極不規(guī)則、著陸姿態(tài)具有不確定性，且月面地形地貌復(fù)雜，表層采樣運動軌跡的中間點應(yīng)劃分為靜態(tài)目標點和動態(tài)目標點兩類。由于月球探測器表面安裝設(shè)備多、布局緊湊，對表層采樣運動的障礙物較多，且相對分散，將影響展開、采樣、放樣、轉(zhuǎn)移、釋放過程；由于采樣過程受光照、溫度等限制，對月表采樣點等動態(tài)目標，軌跡規(guī)劃的快速性具有重要意義。

2)為獲取月面土壤樣品，表層采樣裝置設(shè)計的運動空間通常較廣，采用全區(qū)域障礙檢測和可達性分析，計算量大，耗費時間長，根據(jù)表層采樣裝置瞬時運動構(gòu)型，動態(tài)組織障礙檢測區(qū)、不可達集，可顯著降低計算量，但動態(tài)邊界的選取需要權(quán)衡。

3)通過對+8°、0°、-8°三種姿態(tài)下的表層采樣過程軌跡規(guī)劃仿真表明，本文算法可實現(xiàn)對障礙的規(guī)避、有效到達目標點，對月面已知障礙具有一定的適應(yīng)性，但對復(fù)雜未知障礙，還需結(jié)合月面圖像進行場景重構(gòu)，提高采樣點附近區(qū)域避障規(guī)劃的準確性與安全性。

[1]葉培建，彭兢. 深空探測與我國深空探測展望 [J].中國工程科學(xué)，2006，8(10)：13-18.

YE PEIJIAN，PENG JING. Deep space exploration and its prospect in China [J]. Engineering Science， 2006，8(10)：13-18.

[2]VIOREL BADESCU. Mars-prospective energy and material resources [M]. Berlin: Springer Verlag，2009：3-14.

[3]危清清，王耀兵，劉志全. 空間機械臂關(guān)節(jié)零部件對關(guān)節(jié)總剛度的影響分析 [J]. 中國空間科學(xué)技術(shù)，2013，33(5)：77-81.

WEI QINGQING，WANG YAOBIN，LIU ZHIQUAN. Effect of parts′ stiffness on general stiffness of space manipulator joint [J]. Chinese Space Science and Technology，2013，33(5)：77-81.

[4]危清清，劉志全，王耀兵，等. 柔性機械臂輔助空間站艙段對接阻抗控制 [J]. 中國空間科學(xué)技術(shù)，2014，34(6)：57-63.

WEI QINGQING，LIU ZHIQUAN，WANG YAOBIN，et al. Impedance control of space flexible manipulator system assisted docking of space station [J]. Chinese Space Science and Technology，2014，34(6)：57-63.

[5]TOMAS LOZANO-PEREZ. Spatial planning：a configuration space approach [J]. IEEE Transaction on Computers，1983，C-32(2)：108-120.

[6]KHATIB O. Real time obstacle avoidance for manipulators and mobile robots [J]. The International Journal of Robotics Research，1986，5(1)：90-98.

[7]賈慶軒，陳鋼，孫漢旭，等. 基于A*算法的空間機械臂避障路徑規(guī)劃 [J]. 機械工程學(xué)報，2010，46(13)：109-114.

JIA QINGXUAN，CHEN GANG，SUN HANXU，et al. Path planning for space manipulator to avoid obstacle based on A*algorithm [J]. Journal of Mechanical Engineering，2010，46(13)：109-114.

[8]于世淼. 月壤表層采樣機械臂及末端執(zhí)行器運動規(guī)劃與位姿檢測 [D]. 哈爾濱：哈爾濱工業(yè)大學(xué)，2013：30-34.

YU SHIMIAO. Trajectory planning and pose measurement of moon-soil sampling robotic arm and end-effector[D].Harbin:Harbin institute of technology， 2013：30-34.

[9]JOHN J.CRAIG. Introduction to robotics—mechanics and control [M]. 3rd ed. New Jersey: Pearson Prentice Hall，2005：73-75.

[10]鄭燕紅，鄧湘金，龐彧，等. 著陸姿態(tài)對地外天體表層采樣的影響研究 [J]. 航天器工程，2013，22(5)：30-33.

ZHENG YANHONG，DENG XIANGJIN，PANG YU，et al. Research on influence of landing attitude on extraterrestrial body surface sampling [J]. Spacecraft Engineering，2013，22(5)：30-33.

[11]劉志全，黃傳平. 月球探測器軟著陸機構(gòu)發(fā)展綜述 [J]. 中國空間科學(xué)技術(shù)，2006，26(1)：33-39.

LIU ZHIQUAN，HUANG CHUANPING. Review of the development of soft-landing mechanisms for lunar explorations [J]. Chinese Space Science and Technology，2006，26(1)：33-39.

[12]MARK W SPONG，SETH HUTCHINSON，M VIDYASAGAR. Robot modeling and control [M]. New York：John Wiley & Sons Inc.，1989：158-161.

(編輯：車曉玲)

Lunar Surface Sampling Device Collision Avoidance Planning Based on Artificial Potential Field Method

ZHENG Yanhong DENG Xiangjin PENG Jing YAO Meng ZHAO Zhihui

(Beijing Institute of Spacecraft System Engineering，Beijing 100094)

The surface sampling technology is an important method for getting soil sample. It is a key technology for collision avoidance trajectory planning with complex lunar terrain, uncertain landing attitude and irregular lunar probe envelope. According to the design of four degree-of-freedom surface sampling device，kinematics and inverse kinematics were analyzed. Considering the constraints of dynamic and static object points, the restriction space was divided into available collision detectability area and unreachable set. To expedite the planning speed,the dynamic available collision detectability area and unreachable set were introduced to the artificial potential field method. And the collision avoidance planning algorithm was proposed. The simulation result proves that algorithm is effective for surface sampling trajectory planning， and it can be used to support surface sampling design.

Surface sampling；Manipulator；four degree-of-freedom；Collision avoidance planning；Artificial potential field method；Lunar exploration

2015-04-14。收修改稿日期：2015-07-27

10.3780/j.issn.1000-758X.2015.06.008

鄭燕紅 1982年生，2010年獲哈爾濱工業(yè)大學(xué)控制科學(xué)與工程專業(yè)博士學(xué)位，工程師。研究方向為地外天體采樣封裝技術(shù)、魯棒控制等。