室内机器人避碰路径规划

张 智,翁宗南,苏 丽,光正慧

(哈尔滨工程大学 自动化学院,哈尔滨 150001)E-mail:2728397813@qq.com

1 引 言

移动机器人的任意路径规划[1]问题是机器人研究领域中遇到的需要突破的且极具挑战性的问题,更是机器人研究的核心内容之一.在现阶段研究的全局路径规划中机器人所处的环境大多以不动的障碍物为主,但是实际生活中机器人需要面对复杂的人类生活.因此针对机器人的研究的工作还有许多关键性的难题亟待突破.很多成熟的算法已经可以解决路径规划的相关问题,如人工势场法、视图法、切线图法、拓扑法、A*算法[2]、D*算法等.

1)Khatib提出的人工势场法的基本思想是基于虚拟力法(参考物理学中受力分析),对活动环境中运动的机器人进行模拟的受力分析.这种方法结构简单,但在该人为抽象出来的场作用下机器人很可能会被困在某一平衡点上,从而发生死锁现象,这样可能会让移动机器人[4]在到达目标点之前就停留在某个局部平衡点上从而发生死锁.

2)栅格分解法使用大小相同的方格把机器人可能搜索到的环境进行划分,划分后的方格在程序中用数组代替.划分后的环境分为两类,机器人可自由移动的区域,阻碍机器人运动的路障区.缺点是表示效率不高.

3)A*算法又名启发式(heuristic)算法,是一种静态路网中求解最短路最有效的方法,主要搜索过程:创建两个表,OPEN表保存所有已生成而未考察的节点,CLOSED表中记录已访问过的节点.遍历当前节点的各个节点,将n节点放入CLOSE中,取n节点的子节点X并算X的估价值.A* 在静态路网中非常有效(very efficient for static worlds),但不适于在动态路网,环境如权重等不断变化的动态环境下.D*是动态A*(D-Star,Dynamic A Star)卡内基梅隆机器人中心的Stentz在1994和1995年两篇文章提出,主要用于机器人探路,也是火星探测器采用的寻路算法[3].

本文利用A*(A Star)算法与D*算法的转换思路加以改进,实现较快速地规划出较优的全局路径,即模拟A*算法,实现移动机器人在环境地图未知的情况下,快速精确的规划出最优的全局路径[5],并且保证算法的简捷有效性,并基于SLAM[7]激光定位机器人利用模拟A*动态规划算法进行避碰路径规划.

2 路径规划方法

2.1 A*算法

A*算法具有悠久的历史,该算法在很多领域又被称为启发式搜索法.A*算法的函数表达式为:

f(n)=g(n)+h(n)

(1)

其中f(n)是节点n从初始点到目标点的估价函数,g(n)是在状态空间中从初始节点到n节点的实际代价,h(n)是从n到目标节点最佳路径的估计代价.保证找到最短路径(最优解的)条件,关键在于估价函数h(n)的选取:估价值h(n)<=n到目标节点的距离实际值,这种情况下,搜索的点数多,搜索范围大,效率低.但能得到最优解.如果估价值大于实际值,搜索的点数少,搜索范围小,效率高,但不能保证得到最优解.估价值与实际值越接近,估价函数取得就越好.

1)实际应用中A*算法应严格符合h(x)<=h*(x),式(1)中的h*(x)是实际的启发值,h*(x)在现实生活中往往是不能提前得知的,但是上述要求是容易符合的,只要满足上述要求,最优的路径规划[9]可以被获得.

2)假设最短的路径距离是C,那么在算法运行完成之前,在OPEN表里至少有一个点n,能够使得f(n)小于等于C.该性质可以理解为:因为最短路径存在,我们可以将start-a-b-c-…-n-…-end这条路径设为最优.并且此时的节点为n,在此节点之前的所有点,都已经被记录在CLOSED表中,此时的节点n在OPEN表里.又因为这条路径是最短的,所以我们可以得到现在OPEN表中n的关于g(n)的值即我们所要的最小值.

通过上述的性质,可以得到,通过A*算法搜索到目标点时,就会有f(goal)=g(goal)<=C成立.

3)在搜索未知节点[10]的时候会出现多次搜索某一点的问题,如果正在搜索的点已经包含在CLOSED表中了,并且该点的f取值比CLOSED表里的小,则需要及时更新该点的f值.假设h代表的函数能满足相容的性质,这一步就可以省掉了.

图1为A*算法流程图.

图1 A*算法流程图Fig.1 A star algorithm flow chart

2.2 D*算法

D*是根据动态A*思想由卡内及梅隆机器人中心的Stentz在20世纪末刊登的文章中提出来的,D*算法主要用于解决机器人路径搜索[6]的问题.在国内外的在火星探测器中大多数都是采用了D*寻路算法.

1)用Dijstra算法[8]从目标点E向起始点S进行搜索.记录环境地图中目标点到各个点的最短距离以及该位置到目标点的实际值距离.任意节点都记录了该节点前一个结点距离目标点距离的信息,比如存在某一路径1(3),3(6),6(5),7(9).则1到7的最短路为1-3-6-7.各个节点从起始点到目标点的信息都已存入OPEN表和CLOSED表.

2)机器人在环境中以自己规划的最优路径运动,当运动到下一节点时最优路径没有发生变化,利用记录的上一次最优路径信息从起始点向后进行搜索.比如机器人在Y点,当它扫描到Y点的下一节点X点的状态发生改变,机器人会即刻调整它所在的位置到目标点的距离值h(Y),h(Y)是机器人从节点X运动到节点Y新的权值c(X,Y)与机器人在节点X的原来的实际值h(X)之和是机器人将要到达的下一节点(到目标点方向Y点到X点再到G点),节点Y是机器人所在的当前点.

2.3 路径规划算法仿真

搜索算法的方式主要有两种,主要分为:盲目搜索和启发式搜索,这两种方式的一个主要共同点:从解空间中寻找出一条从起始点到目标节点的最优路径.路径规划就是机器人的最优路径规划问题,即依据某些优化条件(如工作代价最小、行走路径最短、行走时间最短等),在复杂环境中找到一个从起始点到目标点能避开障碍物的最优路径.所以,应注意以下三点:

1)明确起始位置及终点;

2)避开障碍物;

3)尽可能做到路径上的优化.

针对路径规划算法仿真主要是分三步进行:静态路径规划仿真和基于静态的动态路径规划仿真以及静态和动态结合仿真.

2.4 静态路径规划仿真

静态路径规划仿真模拟机器人对环境已知,起点、目标点、障碍点全部已知的情况,需要在算法中实现规划出模拟机器人从起点到目标点寻找出最优的路径.

2.4.1 构建仿真图

模拟环境是在电脑上能模仿现实环境,设计者将实际环境抽象成虚拟环境,并将虚拟环境用算法语言以形象的图表或者图像等的方式展现出来.根据理论依据,我们所要模拟的机器人活动的环境应该是有起点、有终点、还存在固定的障碍物,通过数组对环境进行模拟在数组中要对模拟环境的各个点进行标注,并附上相应的含义.如图2所示.

2.4.2 模拟仿真路径规划

算法描述:从目标点开始从后向前查找,每次判断其当前点四邻域中g值(表示该点距离起始点的距离)最接近当前g值的点,然后设置该点为当前点,并设置该点的p值(以此判断是否为最优路径中的点)为0,以此例推,直至找到起始点.经过优化得到相对复杂的寻路图.

2.4.3 路径规划算法对比

模拟Dijstra算法:在构造抽象静态地图的时候模拟Dijstra的方法,对构造的空间进行多次遍历,判断每个点以及四邻域点的type值然后对该点进行相应的处理,将所有的点遍历到之后即为结束.

图2 寻路仿真Fig.2 Pathfinding simulation

Plan1的流程图简单易懂运行起来现象也比较清晰,通过图3可以看出该算法的可行性.

图3 复杂环境规划图Fig.3 Complex environmental planning map

模拟A*算法:模拟Dijstra法里,不论起点与终点距离多远,算法总会对整个地图进行遍历,这样既浪费资源又浪费时间.所以在Plan3算法中将会实现遍历的简约,只遍历到目标点,碰到目标点就会停止遍历,大大的提高了效率和容错率.

模拟A*算法流程图如图4.

从图5可以看出,运用模拟A*算法模拟环境越复杂所显现的效果越明显,同样是从起始点到达目标点,复杂图像中扫描过的区域与未扫描过的区域区分的较为明显,而在简单图像中则不然.

3 动态路径规划仿真

动态的路径规划在很多领域都被广泛应用.凡是可简化为类似D*或动态A*的规划问题基本上都可以采用动态路径规划的方法来解决.在动态路径规划中将使用模拟A*算法这种最简约最便捷的方法进行讨论.

3.1 人机交互

在对机器人的操纵中我们通常是通过一个界面输入,而后根据该输入实现对机器人的控制,所以输入控制和机器人在并不在同一个平台中,因此引入了人机交互的概念.通过人机交互实现实时更新.下面将依次介绍相关功能.

图4 模拟A*算法流程图Fig.4 Simulated A star algorithm flow chart

a)设置人机交互式对话框

图5 简单与复杂环境仿真对比Fig.5 Simulation comparison between simple and complex environment

本课题中人机交互对话框是对动态障碍物进行初始化和启动的入口,主要思路是通过对话框与动态地图联系起来然后在对话框里设置需要在动态地图中需要实现的功能与现象.

b)动态障碍的控制

在对话框中设置好了坐标数值之后,在算法程序中设置各个障碍点的纵坐标为:

D_y1=D_x1+1

(2)

D_y2=D_x2+2

(3)

D_y3=D_x3+3

(4)

D_y4=D_x4+2

(5)

D_y5=D_x5+3

(6)

3.2 动态仿真

动态路径规划是静态路径规划的延伸.在仿真中加入既能体现静态路径规划的,也能体现动态路径规划的功能;擦除、刷新、设置动态目标与启动动态目标.图6为三个不同时刻t1、t2、t3动态障碍点、扫描区域、最优路径的更新情况.

图6 动态规划不同时刻规划图Fig.6 Dynamic planning of road maps at different times

4 综合试验

本实验基于SLAM激光定位机器人利用模拟A*动态规划算法进行避碰路径规划.要求在活动范围里中规划出一条最优的避碰路径,使机器人到达目标点,同时基于建立的环境地图,实现机器人的实时定位,由已有的定位功能直观地反映出算法的可行与否.

4.1 模拟仿真平台

为验证基于模拟A*算法与SLAM激光定位系统在移动机器人路径规划中的正确性以及有效性,利用VC++6.0软件平台基于MFC开发了仿真算法.本文中搜索的是四邻域[11],在仿真环境中设计30*40的仿真地图,可以在地图中任意的加入起始点、目标点和障碍点.

图7 基于实际仿真环境与路径规划图Fig.7 Path planning map based on actual simulation environment

其中图7为静态图上显示的界面,其上设置好了起始点、目标点、障碍物,以及通过算法仿真自动规划出的一条最优路径,灰色部分为扫描过的区域,白色的部分为未扫描区域.

4.2 机器人路径规划实物验证

本节要介绍的是验证模拟A*算法应用到实物平台上在室内环境下的应用情况.图8所表示的是在实物演示平台上对机器人实时跟踪所走过的路径记录,其中左上角为起始点,右下角为目标点,两侧为墙壁,根据机器人平台所记录下的实际路径,和算法所规划出的路径基本相似;其中图8下半部分是实物机器人平台验证算法实验的过程,通过观察该过程机器人平台的路径选择与自己设计的算法基本一致,即模拟A*算法在实物平台上的应用实验验证成功.

图8 机器人平台实验Fig.8 Robot platform experiment

表1 寻路仿真平均时间对比Table 1 Pathfinding simulation average time comparison

通过100次模拟仿实验结果得出,在同样的地图中模拟Dijstra算法在寻找最优路径用时平均在5秒左右,而本文模拟A*算法寻找最优路径用时平均仅有1.8秒左右;此外本文利用SLAM平台共进行综合实验20次,其中有19次实验结果的优化路径与仿真结果基本吻合,只有一次由于人员走动影响了雷达扫描结果,与仿真结果有较大出入.

表2 平台实验结果Table 2 Platform experiment results

4.3 实验结果分析

本文利用VC++6.0软件平台基于MFC开发了仿真算法,在载有SLAM激光扫描雷达定位的机器人平台进行验证算法的可行性和有效性.在试验过程中,选在了室内,避免不必要的误差和干扰,其次选择了比较规则的障碍物.在进行了多次的试验之后,如图5所示的静态仿真实验、图6所示的动态仿真实验以及图8 所示的平台实物实验对模拟A*进行验证得出了表1、表2的结果,彰显出该算法具有A*算法的估价值大于实际值,搜索的点数少,搜索范围小,效率高的特点,避免了D*算法不能保证得到最优解的缺点,当然该算法不太适用路程太长的路径规划,但是对大部分的路径规划能保持良好的鲁棒性.

5 总 结

本文主要研究基于激光雷达扫描机器人的静态及动态下避碰路径规划算法,在激光雷达扫描定位与创建地图功能完善的前提下,学习A*算法、人工势场法、D*算法的基础知识,并对A*算法进行改进,实现先开发模拟仿真软件验证算法的可行性,然后学习开发软件与现有平台的接口通信功能.根据以上实验结果可知模拟A*算法在控制机器人完成路径规划具有一定可行性.