基于人工势场法的月球表层采样装置避障规划

郑燕红 邓湘金 彭兢 姚猛 赵志晖

(北京空间飞行器总体设计部，北京 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年获哈尔滨工业大学控制科学与工程专业博士学位，工程师。研究方向为地外天体采样封装技术、鲁棒控制等。