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

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

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

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

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

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

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

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

1 引言

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

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

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

2 表層采樣裝置運動關系

圖1 表層采樣裝置示意

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

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

表1 表層采樣裝置DH參數

(1)

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

(2)

式中c23，s23，c234，s234表示下標對應關節角相加的余弦或正弦。

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

(3)

通過逆運動學求解關節變量(θ1，θ2，θ3，θ4)，可得：

(4)

3 表層采樣任務及約束

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

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

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

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

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

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

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

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

4 避障規劃算法

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

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

(5)

為構建障礙點的斥力勢場，在表層采樣裝置上選取動態斥力控制點，采用障礙斥力勢場函數Uro：

(6)

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

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

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

(7)

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

(8)

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

(9)

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

圖5 表層采樣避障規劃算法流程

5 仿真分析

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

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

圖6 表層采樣規劃軌跡示意

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

6 結束語

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

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

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

[1]葉培建，彭兢. 深空探測與我國深空探測展望 [J].中國工程科學，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]危清清，王耀兵，劉志全. 空間機械臂關節零部件對關節總剛度的影響分析 [J]. 中國空間科學技術，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]. 中國空間科學技術，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*算法的空間機械臂避障路徑規劃 [J]. 機械工程學報，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]于世淼. 月壤表層采樣機械臂及末端執行器運動規劃與位姿檢測 [D]. 哈爾濱：哈爾濱工業大學，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]鄭燕紅，鄧湘金，龐彧，等. 著陸姿態對地外天體表層采樣的影響研究 [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]劉志全，黃傳平. 月球探測器軟著陸機構發展綜述 [J]. 中國空間科學技術，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年獲哈爾濱工業大學控制科學與工程專業博士學位，工程師。研究方向為地外天體采樣封裝技術、魯棒控制等。