基于势场和运动约束的月面六足机器人路径规划算法

徐昭辉, 齐臣坤*,, 王卫军, , 韩亮亮, 陈 萌

(1上海交通大学 机械与动力工程学院,上海 200240,E-mail: chenkqi@sjtu.edu.cn;2上海宇航系统工程研究所,上海 201109)

月球是人类探索宇宙的重要目标,月球探测是我国航天工程的重要任务[1]。按照我国深空探测和载人月球探测发展战略,在探测准备阶段,由月面探测机器人提前开展航天员登月区域的预先详查,获取着陆区域详细的数据信息,为载人登月提供数据基础[2-4]。目前各国的探月活动主要针对月海较为平缓的地形进行探测,但这样的探测活动并不能全面地了解月球。对于复杂地形的探测可以扩大探测的范围和地形种类,有助更深入了解和研究月球。

目前,国内外登陆成功的星表移动机器人均采用轮式移动系统和被动悬挂,以适应外星球的不平坦地形。例如美国1997年的“探路者”号火星车、2004年的“勇气号”和“机遇号”火星车[5]、2012年的“好奇号”火星车[6]、中国2013年的“玉兔”号月球车和2019年的“玉兔”二号月球车都是采用同样方式适应不平地形。尽管轮式移动系统能够适应一定程度的不平地形,但难以进入复杂的崎岖地形,因此研究能够适应月面复杂地形的足式机器人具有重要的意义。

美国卡内基梅隆大学(CMU)早在1992年就研发了一款名为Dante[7]的八足探测机器人尝试进行火山口探测,而1994年研发的Dante Ⅱ[8]对火山口进行了考察,在回程途中失去稳定性,掉入火山口中。CMU之后在前两代机器人的技术基础上,针对火星探测任务研发了六足探测机器人Ambler[9],但由于机器人质量体积过大未被用于实际探测。德国人工智能研究中心(DFKI)曾研发八足机器人SCORPION Ⅰ-Ⅳ[10]与四足机器人ARAMIES[11],之后基于经验开发了一款针对月表探测的六足机器人SpaceClimber[12]。此外一些实验性质的研究成果还有:德国DLR机构研发了微型六足机器人DLR-Crawler[13],实现了起伏地形下的自主运动规划;德国Karlsruhe大学研发了LAURON Ⅰ-Ⅴ[14]用于救援、勘探、监测等危险任务;瑞士苏黎世联邦理工学院(ETH)研发了一款适用于低重力条件下运动的四足机器人SpaceBok[15];国内哈尔滨工业大学也曾研发一款重载月面探测机器人ELSpider[16]等等。总体来说,目前针对月面足式机器人研究已经有了一定进展,但离实际落地完成月面机器人任务还有不小差距。在月面机器人任务中,为保证后续载人登月任务的安全性,月面探测机器人需开展登月着陆区域的先导巡视详查,因此需要对月面探测机器人的运动路径进行合理的规划。而移动机器人的路径规划算法的开发与研究是当下移动机器人的发展带来的需求[17]。目前主要的路径规划算法主要有:

(1) A*算法:A*算法是一种典型的启发式算法,A*算法的优点体现在搜索路径直接,对环境反应快, 缺点是随着地图增大由于每一节点计算量大,导致运算时间变长,并且规划路径不考虑机器人运动模型而过于理想化。为了保证移动机器人能实际跟踪规划出的路径,文献[18]提出将动态窗口法(DWA)和A*算法融合,实现了路径全局最优和局部避障效果。

(2) 快速随机搜索树算法(Rapidly-exploring Random Tree, RRT):快速随机搜索树是一种采用随机采样的搜索方法。其优点体现在理论简单、不需要对状态空间进行预处理。其缺点是搜索方向随机生成导致计算量大、生成路径转折点多,不适合机器人直接行走。文献[19]讨论了RRT算法在三维环境下的运用,以及利用贝塞尔曲线对路径进行优化,有效的避免了RRT算法的缺陷。

(3) 蚁群算法等启发式算法:蚁群算法是一种模拟蚂蚁觅食行为的仿生算法[20]。其优点是具有较强的鲁棒性,但是依然存在收敛慢、计算效率低和容易陷入局部最优等问题。文献[21]提出一种自适应优化算法,在最优路径与规划时间之间做到均衡。文献[22]针对蚁群算法易陷入局部最优、搜索时间过长等问题,使用Logistic 模型改进,加快了蚁群算法后期收敛速度。文献[23]采取将动态窗口方法和改进蚁群算法相结合,实现了机器人在车间动态环境下安全导航。

(4) 人工势场法(APF):人工势场法是一种虚拟力场法。人工势场法的优点是结构简单,生成的路径一般比较平滑且安全,它的缺点是对障碍有一定要求,并且经过狭窄通道时会发生摆动现象,易陷入局部极小点。文献[24]结合了双向RRT算法与人工势场法实现了月面环境下的路径规划。

(5) 基于人工智能的方法:近年来结合强化学习的路径规划算法研究趋于热门,比如文献[25]中提出了一种基于Q学习和Sarsa算法的路径规划算法,并结合RRT算法成功实现了兼顾全局路径和局部动态避障。

总的来说,当前路径规划算法虽然种类繁多并且研究深度较深,但是这些算法没有与实际环境做很好的结合应用,并且主要考虑的是轮式移动平台的运动模型而非足式机器人。因此需要结合月面足式机器人的运动模型与约束,建立适用于月面足式机器人的路径规划算法。基于上述问题,本文先通过对月面地形的分析建立了二维路径规划仿真模型,之后建立六足机器人的运动模型分析得到其运动约束,再结合势场法和六足机器人运动约束提出一种适用于月面六足机器人的路径规划方法,并通过仿真实验验证了该方法的性能和可行性。

1 月面地形分析与路径规划仿真环境建模

1.1 月面地形分析

月球表面主要分为月海和月陆两大地理模块。人类在地球上用肉眼看到的月面暗黑色斑块,称为“月海”。月海由古代玄武岩熔岩凝而成,是月面上宽广的平原,约占月表面积的17%。月球上颜色较浅的区域被称为月陆,约占月表面积的83%。月陆地区有大量的山脉,虽然月球比地球小得多,但月球表面的最大起伏可达16 km(地球最高处和最低处相差约20 km)。月陆地区较月海地区的起伏更大。

月海和月陆均覆盖有大大小小的石块和撞击坑。石块是指月面上分布的岩石,主要由月球表面或其附近由岩浆直接结晶和固化形成;撞击坑是指布满月球表面的环形凹坑构造,包括环形山、辐射纹以及与撞击坑有关的隆起构造,这些是因为小行星、彗星等宇宙内星体与月球表面直接相撞,常年累月积累形成。此外,月球表面覆盖有松软的月壤,月壤颗粒直径以小于1 mm为主。月面复杂地形实景照片如图1所示。

图1 月面复杂地形

1.2 月面路径规划仿真环境

路径规划往往是将三维环境投影到二维平面。目前月面环境难以在现实中复刻,因此采取仿真环境进行路径规划算法的测试和实现。从图2月面俯瞰图可得,月面的障碍物主要由撞击坑和大型石块等组成。月面环境由于缺少空气流动和地质运动,大型石块的分布比较随机,形状也近似规则形状,因此可以用二维平面内的圆形障碍物替代。本文提出了一种月面二维地图模型生成算法,生成地图效果如图3所示。

图2 月面俯瞰图

图3 仿真模型

月面二维地图模型随机生成算法流程如下:

算法1 随机月面仿真环境二维平面生成算法

输入 地图的最大尺寸size与地图内部的障碍物数量num

输出 带有障碍物的二维地图

(1) 根据地图尺寸size和障碍物数量num在地图随机选取障碍物中心坐标。

(2) 为每一个障碍物随机生成半径。

(3) 检测是否发生干涉,如果没有则输出地图,否则重复1-2。

2 六足机器人运动模型与约束

当地形环境较复杂时,如石块、坑洼、斜坡等崎岖地形,腿式机器人可以选择较好的落脚支撑点,通过性较好。足式机器人通过抬腿迈步可以避免机器人受困于松软月壤,具有较好的适应能力[26]。而与双足机器人和四足机器人对比,六足机器人行走总是处于静态稳定状态,稳定裕度大,更适合月面复杂崎岖的地形[27]。因此,六足机器人是月面复杂崎岖地形环境下探测的较优方案。六足机器人如图4所示。

图4 六足机器人

2.1 六足机器人腿部运动学模型

六足机器人腿机构的坐标系和参数定义如图5所示,ΣL为腿坐标系,与机架固定连接,位于腿的髋关节,三个主动驱动的轴线交点,x轴向前,与侧摆电机转动轴线同轴,y轴向左,z轴向上。

图5 六足机器人腿机构

记ΣL腿坐标系下腿部构机构的输出广义坐标Pep为(xep,yep,zep),其中,xep,yep,zep为足尖末端在ΣL中的笛卡尔坐标。另一方面,记腿部机构的输入广义坐标系q为(γ,α,β),其中,γ,α,β分别为如图所示的驱动侧摆、大腿和小腿电机的转角。该机构大腿长度为Lt,小腿长度为Lc。根据这些参数可以计算腿机构的运动学位置正解。运动学位置正解,即已知腿的输入广义坐标q,求解得到输出广义坐标Pep:

(1)

根据式(1)求腿机构的运动学位置反解,即已知腿的输出广义坐标Pep,求解输入广义坐标q,整理可得:

(2)

六足机器人的坐标系主要有机身坐标系、各腿机构坐标系。机身与各腿机构的关系为:ΣLi(i=1,2,…,6)为第i条腿的腿机构坐标系。ΣB为机器人机身坐标系,位于ΣLi(i=1,2,…,6)的几何中心。机器人各腿坐标系与机身坐标系ΣB之间旋转变换矩阵和位移变换矩阵定义如表1所示。

表1 机器人各腿坐标系与机身坐标系ΣB之间变换矩阵

六足机器人在设计上具有中心对称布局,因此各腿坐标系位移变换矩阵可以通过机身宽度DW和机身长度DL表示出来,旋转变换矩阵一般视为无旋转变换的单位矩阵。根据上述定义,若已知第i条腿末端在腿坐标系ΣLi下的位置Pep(ΣLi),则在机身坐标系ΣB下的描述Pep,Li(ΣB)为:

Pep,Li(ΣB)=TB,Li+RB,LiPep(ΣLi)

(3)

2.2 六足机器人运动模型

机器人二维平面内当前坐标为(x,y,θ), 当前运动状态为(v,ω),其中x,y为当前坐标,θ为当前角度,v为当前线速度,ω为当前角速度。若当前轮式机器人的转向半径为r,轮式机器人的运动模型为:

(3)

轮式机器人一般通过控制线速度v和转向半径r的大小来控制机器人的运动。与轮式机器人相比,六足机器人主要通过控制单步步长d和当前转向变化角Δθ来控制运动。设六足机器人迈出一步前后的坐标分别为(xi,yi,θi)、(xi+1,yi+1,θi+1),其运动模型为:

(4)

对于轮式机器人,由于线速度v和转向半径r的大小可以在运动过程中随时调整,所以轮式机器人的运动轨迹是连续的。但是六足机器人的运动模型是离散化的,其运动轨迹是一系列连续的点。这是因为单步步长d和当前转向变化角Δθ在迈步过程中是无法修改的。其运动轨迹如图6所示。

图6 六足机器人运动轨迹

图7 人工势场

2.3 六足机器人运动约束

由式 (5)总结可以得到,六足机器人的机身中心在二维平面内的运动轨迹是离散的,而根据式(3)可以得到,在世界坐标系下,第i条腿末端位置Pep,Li(W)为:

(7)

由于支撑腿末端在运动过程中在世界坐标系下是静止不动的,所以六足机器人的足端支撑轨迹也是一系列离散的点。而摆动腿末端的运动目标位置是下一次支撑腿-摆动腿轮换的支撑腿末端位置,所以六足机器人与障碍物之间需要进行碰撞检测Collision(x)的位置就是每一步的支撑腿末端位置(Collision(x)=0视为未发生碰撞),该位置可以通过式(6)计算得到。而在实际行走过程中,六足机器人的单步步长d一般也是固定值,所以六足机器人受到的运动控制主要是每一步的转向变化角Δθ,该转向变化角受到最大值和最小值约束。因此六足机器人的运动约束为:

(7)

3 路径规划算法

基于式(7)的六足机器人运动约束,本文提出了一种适用于月面六足机器人的路径规划算法,其核心是一个基于势场和运动约束的代价函数F(xi),具体组成在本节后续内容中会详细介绍。算法具体流程如下所示:

算法2 结合势场的六足机器月面路径规划算法

输入 二维地图模型,起始点,目标点,转向变化角限制

输出 算法规划的月面六足机器人运动路径

1.计算当前六足机器人运动状态

2.代入代价函数F(xi),建立最优化问题模型

3求解该最优化问题

4.将最优解代入,计算下一步六足机器人的位置

5.重复1-4,直到六足机器人到达目标点

3.1 势场法

势场法是用人工虚拟的一种模仿重力势场的方法来规划控制机器人的避障行驶。这种路径规划算法的具体实现思路是:在移动机器人运行的环境空间中构造一个虚拟的人工势场,该势场由两部分组成:源于目标点的引力势场Uatt(xi),和源于障碍物的斥力势场Ureq(xi),移动机器人采用

基于人工势场法的路径规划时,其前进方向是由当前所处位置的总势场的梯度下降方向决定的。

移动机器人、障碍物中心点、目标点以点的形式在以起始点为原点的环境二维坐标系空间中表示,每个运动点xi可表示为:

(8)

当前环境内的全局人工虚拟势场函数由目标终点所产生的引力势场和各个障碍物所产生的斥力势场共同组成,全局势场函数为:

U(xi)=Ureq(xi)+Uatt(xi)

(9)

在虚拟势场中,当机器人当前位置的总势能为零时,则说明机器人已经到达目标点。相比于其他避障算法,人工势场法具有计算空间较小、计算速度快等优点,但也存在难以解决的问题,如目标点不可到达问题、局部最小点问题等。目标点不可到达问题即移动机器人在目标点周围不停的振荡,且始终无法到达目标点问题,可以通过适当调整势场函数克服。人工势场的数学模型主要由斥力函数Ureq(xi)和引力函数Uatt(xi)构成。斥力势场和引力势场函数的数学公式设计如下:

(10)

(11)

式(11)中引力函数可以保证机器人在距离目标点较近的情况下仍旧保有一定势场梯度,从而避免机器人无法到达目标点的情况。然而在全局势场中,障碍物的分布可能会导致局部最优点。下面通过结合六足步行机器人运动约束突破困境。

3.2 结合运动约束的地图代价函数设计

根据地图内障碍物位置尺寸信息以及目标点位置信息,基于式(10)和(11)计算地图内人工势场函数U(x)。结合六足步行机器人运动特性,为了保证机器人能够完全避免碰撞,将机器人每条腿末端的坐标作为代价函数计算的变量,构造代价函数f(θi):

(12)

(13)

3.3 路径规划的平滑优化方法

为了使得规划出来的路径更加平滑并且保证最后可以收敛到目标点位置,在式(12)的代价函数中添加平滑函数s(θi)和收敛函数h(θi):

s(θi)=c3|θi|

(14)

h(θi)=c4|θi-θgoal|

(15)

式中:θgoal为当前路径点对目标位置点的朝向角。代价函数F(θi)修改为:

F(θi)=f(θi)+s(θi)+h(θi)

(16)

根据以上代价函数求解最优化问题:

minF(θi)
s.t.θi∈[θi-1+θd,min,θi-1+θd,max]

(17)

为了加快求解速度,可以通过均匀采样方法求解该最优化问题,得到θi,计算该情况下机器人下一步新坐标。重复该最优化求解过程,直到当前路径点到达目标位置点。

4 实验与分析

4.1 平滑优化方法

为了验证算法中的平滑优化方法的有效,c1、c2、c4)的值固定,调整c3的值,以c3、c4)的比值ratio为变量,实验比较不同情况下算法规划的路径长度和路径点平均转向变化角绝对值,规划的路径样例结果见图8,实验结果见表2。

表2 平滑优化参数调整影响

图8 ratio=1.0的规划路径

根据实验结果可得,当比值越大时,平均转向变化角绝对值越小,即规划路径越光滑,但是当比值大于等于2.0时,会导致规划出来的路径无法到达目标点,即平滑代价函数在总代价函数中的占比过高。此外,比值大小对规划路径长度影响较小。因此选择一个合适的比值主要影响算法的可行性和规划路径平滑性。

4.2 算法性能对比

为了验证算法的性能,本文算法命名为HEX-APF,与常用的A*算法、RRT算法和传统APF算法在二维月面仿真环境中进行对比实验,具体实验结果如下所示:

根据表3可得,HEX-APF算法相比于A*算法和RRT算法具有更高的运行效率,因此更适合机器人机载平台电脑算力有限的实际限制,而APF算法尽管运行效率更高,但由于局部最优问题存在一定失败概率,鲁棒性较差。而根据表4可得,HEX-APF规划出来的路径长度小于APF和RRT算法结果,略高于A*算法结果。此外根据图9和图10,A*算法和RRT算法规划的路径点中一些点距离障碍物太近,有可能发生碰撞。因此综合考虑下,HEX-APF是这些算法中性能表现最好的算法。

表3 算法运行时间/s

表4 规划路径长度/m

图9 算法性能对比实验1结果

5 结论

针对月面环境六足机器人的需求,提出了一种有效鲁棒的路径规划算法。通过人工势场法和结合六足机器人运动约束,引入平滑函数和收敛函数,算法具有较高的运行效率和性能。在根据月面地形建立的二维月面仿真环境中,本文算法与常用的A*算法、RRT算法和APF算法相比,具有与A*算法相近的运行结果和更高的运行效率,比RRT算法性能更好,比APF算法具有更高鲁棒性。

下一步工作:考虑月面崎岖地形的影响,引入结合传感器的实时路径规划和六足机器人步态规划。