基于栅格地图的火星车路径规划方法

董元元,崔祜涛,田阳

(哈尔滨工业大学深空探测基础研究中心,哈尔滨150001)

基于栅格地图的火星车路径规划方法

董元元,崔祜涛,田阳

(哈尔滨工业大学深空探测基础研究中心,哈尔滨150001)

火星车路径规划是实现火星车完成预定探测任务的关键。然而传统路径规划算法,如A*和D*等,存在计算速度较慢,算法复杂度较高等问题。本文将对传统路径规划方法A*法改进并且得到一种快速高效的全局路径规划算法,之后结合合适的局部避障算法,得到一种基于栅格地图的完整的火星车路径规划方法,最后通过仿真验证了此方法的有效性及合理性。

路径规划;PAA*;局部避障;A*

0 引言

火星车的路径规划,是指火星车根据自身传感器对周围环境进行感知,自行规划出一条安全的运行路径。火星车路径规划问题主要包括两个方面: 1)使火星车从初始点运行到目标点;2)用一定的算法使火星车能够绕开障碍物,并且经过某些规划好的点。对于火星车路径规划,按照选取目标的范围看,其可分为全局路径规划和局部路径规划。全局路径规划从全局出发,得到一条从起点到终点的全局轨迹。而局部路径规划考虑机器人移动中的局部细节,得到符合运动学规律的路径。

栅格法是一种广泛应用于各种移动机器人的全局规划方法,其通过创建栅格地图来代表机器人当前的三维环境。常见的栅格法有A*法、D*法等[1-2]。A*法是通过定义启发式函数来得到全局路径的方法,其算法较为原始,在路径的二次规划中速度较慢。D*法是基于A*法的一种改进算法,能够更好地运用于动态场景,基于D*法改进的field-D*法已经成功运用于在“勇气号”火星探测器全局规划过程中[3],但是D*算法原理过于复杂,难以实现。并且对于火星车路径规划而言,使用栅格法得到的轨迹为离散的栅格,不符合火星车的运动学规律,不能直接适用于火星车运动控制。

因此,在本文中将首先对A*法改进,得到一种快速高效的路径规划算法:PAA*法来实现全局路径规划,之后结合符合火星车运动学约束的局部避障算法,建立一种完整的火星车路径规划方法。

1 全局路径规划方法

1.1 栅格地图建立

在使用栅格法全局路径规划算法之前,需要建立当前环境的栅格地图。栅格地图建立如文献[4]中所介绍,根据当前地形高程图拟合平面,对火星大小的斑点进行高度、倾斜角以及粗糙度分析,可以得到当前的栅格地图。栅格地图一般为二值图:其中0代表当前栅格不可通行,即为障碍栅格;1代表当前栅格为可行栅格。如图1所示,其中黑色部分为障碍区域。通过建立栅格地图A之后,可以得到当前栅格中的障碍栅格集合Obs⊂A,和可行域Free⊂A。这样路径规划算法可以描述为在/区域中规划一条从起点到终点的最优路径。

图1 栅格地图示意图Fig.1 The grid map

1.2 A*算法

A*算法[5]是一种静态的栅格路径规划方法。其主要过程为:从起始节点出发,依次对当前节点的子节点权重进行更新,并用子节点中权重f(n)最小者对当前节点更新,直至遍历所有节点(或者当前节点为目标节点)为止,其中f(n)=g(n)+h(n),这3个函数将在下文中定义。当扩展完成之后,A*算法结合移动机器人目标点位置信息,沿着目标点位置开始搜寻,到达起始点为止,得到一条起点到终点的最优路径。对于A*算法中函数给出如下定义:

S为初始节点;

G为目标节点;

k为路径规划中已规划过的某一节点;

gi为路径规划搜索过程中的栅格节点;

gi,x,gi,y分别为节点的横、纵坐标;

g(k)为初始节点到k节点的距离;

h(k)为启发式函数,为k节点的启发距离;

f(k)为k节点的路径评价函数;

OPEN为等待扩展节点的集合;

CLOSE为已扩展节点集合;

ne为扩展节点函数,节点不是障碍或者之前未扩展时,执行节点扩展;

ni为插入节点函数,将节点按照f(n)值大小降序放入OPEN列表;

在栅格地图中,A*启发式函数/定义为从当前k节点到目标点的距离,其表达式为

其中:kx、ky、Gx、Gy分别为当前节点和目标点在栅格地图中的坐标位置。

通过上面定义的函数,可以得到A*算法的具体计算流程图,如图2所示。通过计算可以得到火星车的初始全局路径。

图2 A*算法流程图Fig.2 Flowchart of A*algorithm

从图2中可以看出,A*算法选择f(n)最小的节点扩展,并且将其存入到CLOSED列表中,将扩展的节点存入到OPEN列表中。当扩展到目标点,即将目标点存入到CLOSED列表中时,停止搜索。之后在CLOSED列表中,搜索最小g(n)值可以得到一条从起始点到目标点的最优轨迹。

1.3 PAA*算法

对于全局规划路径而言,在已规划好的路径中间,当有一可行栅格变为障碍栅格时,需要重新计算全局路径。使用A*算法时,重新规划过程需要对整个栅格地图继续按照A*方法遍历,求出新的路径,这样之前已规划好的路径没有参与到重新规划中。因此,我们使用PAA*算法解决这种重复计算问题。其对于CLOSED列表中启发式函数h(k)为

其中:g*为上一步得到的最小代价值路径的代价值;g(k)定义如上节所示。

在之后规划过程中,将增加障碍栅格之后的每个路径点假设为中介目标点,搜索从增加的障碍栅格到某个中介目标点合适的路径,之后沿着这个中介路径点到达目标点。这样减少了重新规划时计算的栅格数,增加了计算速度。

图3显示了使用PAA*算法时的全局路径规划结果。在图3(a)中,当在规划完成路径中(5,4)位置增加了新的障碍栅格后,算法重新规划得到点划线的路径。图3(b)中灰色栅格为重新规划过程中所计算的栅格。

图3 PAA*算法演示Fig.3 PAA*algorithm example

2 局部路径规划方法

在本部分将选择简化的GESTALT方法来实现局部路径生成。在此过程中,忽略GESTALT建立优异值地图过程,并且忽略其确定值,仅考虑建立二值栅格地图,如上述栅格地图建立中所描述,将整个地图标记为可行区域与不可行区域。之后建立一系列的轨迹组,从中选择接近最远全局路径点,并且不经过不可行区域的轨迹为下一步执行轨迹,如图4所示。

图4 火星车避障系统流程图Fig.4 Flowchart of Mar's rovers'obstacle avoiding system

2.1 轨迹组建立

此过程需要建立一组符合火星车运动学的轨迹。对于类似车辆的火星车,定义V(α)=[v(α),ω (α)]T为一给定轨迹的速度矢量(其中α为轨迹参数,不同值代表不同轨迹),由火星车的线速度和角速度构成。令x(α)、y(α)、ϕ(α)为火星车位姿参数,其中(x(α),y(α))代表火星车当前位置,而ϕ(α)代表火星车的当前姿态角,定义P(α)=[x(α,t),y(α, t),ϕ(α,t)]T,则其运动学方程为

之后建立如图5所示的圆弧形轨迹为局部路径,其速度表达式为

其中:v0、w0为固定值;K=±1代表轨迹方向(向前或者向后)。对于火星车,我们选择v0=0.2 m/s, w0=0.05 rad/s作为初始参数,建立如图6所示的局部轨迹组,在下一步中将从此轨迹组中选择最优轨迹。

图5 圆弧形轨迹示意图Fig.5 Circle trajectories

图6 建立局部轨迹组Fig.6 Local circle trajectories with series of parameters

2.2 局部轨迹选择

建立轨迹组之后,从中选择接近最远全局路径点,并且不经过不可行区域的轨迹为下一步执行轨迹。具体为分析每条轨迹所经过的栅格,得到每条轨迹经过的全局路径栅格和不可行栅格,将其经过的离目标点最近的栅格作为其最终选择路径点。之后选择经过最远全局路径点并且在可行区域的轨迹为下一步执行轨迹。如图7所示,点划线为全局路径,细虚线弧线为规划轨迹组,粗虚线轨迹为选择的不经过不可行区域,并且经过第三个路径点的轨迹。

由于全局路径的精确度较差,因此规划好的全局路径点周围有可能出现障碍物,导致此路径点成为不可行区域,这种障碍物可称为局部障碍物,如图8中加入的局部障碍物。对于此种情况,使用上述介绍的方法并不能完成避障,并且可能会产生错误,因此需要图8的避障方案。当没有局部轨迹经过路径点,并且没有穿过不可行区域时,选择最容易实现的轨迹,也就是弧度最小的轨迹,先绕出局部障碍物,之后再按照上述方法,寻找经过最远路径点并且不经过不可行区域的轨迹,直到到达目标点为止。

图7 局部路径选择示意Fig.7 Local trajectories selecting example

图8 出现局部障碍物时情形Fig.8 Local obstacle avoiding example

在全局规划完成后,使用上述所介绍的局部路径规划方法,得到如图9所示的结果。其中点划线轨迹为全局规划路径,而实线轨迹为局部跟踪之后的路径,这样得到的局部路径为平滑路径,其符合运动学约束,可应用于火星车运动控制中。

图9 局部路径规划结果Fig.9 Result of complete rover path-planning method

3 结 论

本文提出了一种基于栅格地图的火星车全局路径规划方法,其主要有以下特点:

1)使用了PAA*法作为全局规划方法,此方法在二次规划时重新定义了启发式函数,相对于传统的全局规划方法A*和D*来说,有速度快和实现简单等优点;

2)建立符合火星车运动学约束的轨迹组,之后以得到的全局路径为中间目标点,通过避障和选择中间目标点的共同作用,得到了合适的局部避障轨迹以及对应的速度向量。

[1]Koren Yoram,Johann Borenstein.Potential field methods and their inherent limitations for mobile robot navigation[C]∥Robotics and Automation,Proceedings of 1991 IEEE International Conference on.[S.l.]:IEEE,1991.

[2]Koenig Sven,Maxim Likhachev.Improved fast replanning for robot navigation in unknown terrain[C]∥Robotics and Automation,Proceedings of 2002.IEEE International Conference on.[S.l.]:IEEE,2002.

[3]Carsten Joseph.Global path planning on board the mars exploration rovers[C]∥/Aerospace Conference,2007 IEEE.[S.l.]:IEEE,2007.

[4]Goldberg Steven B,Mark Maimone M,Larry Matthies. Stereo vision and rover navigation software for planetary exploration[C]∥Proceedings of 2002 IEEE Aerospace Conference.[S.l.]:IEEE,2002.

[5]Durfee Edmund H,Charles L Ortiz Jr,Michael J Wolverton. A survey of research in distributed,continual planning[J]. AI Magazine,1999,20(4):13.

A Path-Planning Method for Mars Rovers based on Grid Map

DONG Yuanyuan,CUI Hutao,TIAN Yang
(Deep Space Exploration Research Center,Harbin Institute of Technology,Harbin 150001,China)

Path-planning is one of the major parts for Mars to complete exploration missions.With running slowly and higher computing complexity,traditional path-planning algorithms,such as A*and D*algorithms, are no longer in using.In this paper,a fast and efficient global path-planning method is got through improving the conretional A*algorithm.Then combining with local obstacle avoiding method,a complete rover path-planning method based on grid map is obtained.Finally,by using simulation provefs this method is effective and available.

path-planning;PAA*;local obstacles avoidance;A*

P2

:A

:2095-7777(2014)04-0289-05

10.15982/j.issn.2095-7777.2014.04.007

董元元(1991—),男,硕士研究生,主要研究方向为火星车导航及路径规划。

[责任编辑:宋宏]

2014-10-14

2014-10-30

国家重点基础研究发展计划项目(2012CB720005);国家自然科学基金资助项目(61174201)

电话:15004542438