基于安全A∗与DWA 算法融合的军队车场巡检机器人路径规划①

金 梅 李清天 张立国

(燕山大学电气工程学院 秦皇岛 066000)

0 引言

车场日是陆军部队一项经常性的车辆技术状况管理制度,其目的在于恢复和保持车辆、器材的良好状态,以保证完成各项执勤备战任务的需要。由于陆军在组织实施车场日过程中,存在方法不当、质量不高、人员重视程度低等问题,巡检机器人被逐渐引入进来。巡检机器人可以通过搭载设备对战士进行有效监管、指导帮扶及自动化检查评比,实现高效、高质、自动化,是传统摄像监控设备所不能及的,其中路径规划是巡检机器人有效作业的重要前提[1-2]。

目前根据机器人的作业范围,可以将路径规划分为静态全局环境的全局路径规划和基于局部障碍的局部路径规划方法。常见的全局路径规划算法有基于节点的Dijkstra、A∗算法,基于采样的快速搜索随机树方法和基于启发式的遗传算法、蜂群算法等。局部路规划算法主要有人工势场法、动态窗口法等[3-5]。

A∗算法在机器人的路径规划问题中应用相当广泛,受其节点搜索策略的影响,存在得到路径非最优、规划的路径拐点较多等问题。文献[6]通过简化路径坐标点和计算拐点处旋转方向和角度对A∗进行了两方面的改进,简化了路径,提高了机器人行进的准确性,但未解决规划路线拐点较多的问题且运行时间增加实时性不高。文献[7]提出了跳点搜索算法,通过在拓展节点的过程中筛选出有代表性的跳点,只针对跳点拓展,从而减小了计算量,提高了速度,但存在内存消耗大,计算花费时间较长的问题。文献[8]通过加入预处理的方法对其进行了改进,解决了实时性的问题,但拐点较多的问题依然没有解决。文献[9]提出了将搜索邻域拓展到24 邻域的方法,使得轨迹更加平滑,但所选邻域过多,导致计算耗时较大,且不具有局部避障的能力。文献[10]结合曼哈顿距离和欧几里得距离提出一种新的启发函数,改进了关键点选取策略,并且融合了A∗和动态窗口法,使得算法同时具备局部避障能力。但上述传统的路径规划算法中,最终规划的路径在直角边处往往距离障碍过近,使得机器人存在极大的与障碍物发生碰撞的危险,特别是在军队车场环境下,存在较多直角边,可能导致巡检机器人损坏或破坏军队战车等负面影响,造成不必要的损失,影响作业效率。

针对上述问题,本文使用危险指数来表示潜在碰撞程度,并基于此重新定义启发函数从而提出一种安全A∗算法,解决了传统A∗算法在直角边处与障碍物存在较高碰撞风险的问题,提高了导航的安全性。同时将其搜索邻域扩展并去除同方向冗余子节点,由传统的8 邻域搜索优化到16 邻域搜索,提高了搜索效率,减少转折点个数,提高了巡检机器人工作效率。最后将改进的A∗算法和动态窗口法结合,使得算法能同时具备全局路径规划和局部的动态避障能力,符合军队车场巡检躲避临时摆放的杂物和正在作业的战士的要求。

1 军队车场环境建模

在路径规划问题中,首先需要建立环境地图。依据军队车场地形规整(见图1)、直角边较多的特点,这里选择使用简单有效且直观的栅格法来建立环境地图[11]。将巡检机器人所在环境用大小相同的正方形栅格来表示,被填充的栅格表示障碍物,未被填充的表示可行进区域。根据军队车场地图建模如图2 所示,其中战车及其停车基座、墙壁、柱子、杂物均已填充表示为固定障碍物(图中1 栅格宽度的条形障碍物表示车库的墙壁和柱子,4×9 的矩形障碍物块表示停放的8 辆战车及其停车基座),设定单位栅格边长为1 m,在此基础上进行路径规划。

图1 军队车场地形

图2 中,白色栅格表示自由区域,黑色栅格表示障碍区域,每个栅格都有唯一的序号和对应的位置坐标。栅格序号和对应坐标之间的转换关系如下[12]。

图2 移动机器人栅格环境图

其中,(xi,yi)表示第i个栅格的位置坐标;i是栅格的序号;mod( )是冗余运算符;ceil()是正无穷大的舍入运算符;Nx和Ny分别是行和列方向的栅格数。

2 所提路径规划方法

2.1 全局安全路径规划

2.1.1 路径规划阶段的危险指数设计

在移动机器人同障碍物共存的军队停车场环境中,保障移动机器人运行的安全性对提升机器人的导航效率有着至关重要的意义。因此,必须对环境中机器人运行的安全性进行量化估计,以改善移动机器人在环境中的安全自主导航性能。危险指数(danger index,DI)可以用于评估机器人运行过程中与障碍物存在的潜在碰撞程度[13]。该指标可以量化移动机器人同障碍物之间的安全距离与碰撞程度,用于提升移动机器人运行过程中的安全自主性。

在机器人路径规划的阶段,机器人同障碍物之间的距离是影响机器人安全性的重要因素[14]。如果障碍物同机器人的距离过小,会导致移动机器人直接和障碍物相撞,反之控制好这个距离可以改善机器人的运行安全性(如图3 所示),避免机器人与障碍物相碰。将障碍物和移动机器人之间的危险指数定义如下。

其中,δ为距离比例因子,具体计算为δ=,Dmax为障碍物对移动机器人安全影响的最大距离,Dmin为机器人与障碍物之间接触的最小距离,dk为机器人当前位置为障碍物之间的距离(如图3 所示)。

图3 基于危险指数的移动机器人安全域示例

2.1.2 基于危险指数的安全A∗算法

在移动机器人路径规划领域中,Dijkstra 算法是最早被提出用于最优路径的搜索,该算法具有良好的性能[15]。与Dijkstra 算法相比,A∗算法的提出极大提升了路径的搜索效率,通过提出启发式路径搜索策略,引入了评价函数用来指导节点的扩展与搜索,提升了算法收敛速度与机器人路径规划的效率。该算法在尽可能保证最优路径的同时,可以减少搜索时间,提高路径的搜索效率。根据定义的代价条件,A∗算法确定从给定起始节点到目标节点的最小代价路径。A∗算法在起始节点上开展路径搜索,从起始节点到当前节点k的路径成本可以根据行进距离G(k)来计算,即:

其中,k表示要搜索的当前节点,sk-1,k表示2 个连续节点k-1 和k之间的距离。

A∗算法最小成本路径是由参与节点产生的路径成本决定的,而剩余路径的最小成本是通过启发式函数H(k)来评估。由此可知,A∗算法是以最短的距离和最小的完成时间代价完成最优路径选择。A∗算法的评估函数定义如下[16]。

其中H(k)=sk,goal,sk,goal表示节点k和目标节点之间的距离。

传统A∗算法以8 邻域算法搜索节点策略为基础,导致在这种搜索策略下,规划出的路径存在路径冗余的节点和拐点[17]。机器人在运行过程中需要不断地转向,直接导致机器人的运行效率低。其搜索模式如图4 所示。因此为了改进搜索方向,本文提出基于16 邻域的搜索节点策略来改进路径存在冗余拐点的不足,改进策略如图5 所示。该方法有助于去除同方向的多余子节点,突破传统A∗算法节点移动方向的局限性,提高A∗算法的搜索效率。

图4 8 邻域搜索方式

图5 16 邻域搜索方式

基于前述危险指数的定义,为保障移动机器人运行的安全性,重新定义如下启发式函数H(k)。

于是得到改进后的A∗算法评价函数如下。

基于危险指数的安全A∗算法的流程图如图6所示。

图6 安全A∗算法流程图

2.2 动态窗口法

动态窗口方法(dynamic window approach,DWA)是机器人局部路径规划中主流的方法,它可以跟踪机器人基于全局规划生成的全局路径,同时结合实时避障原理,实现机器人的自主安全导航[18]。

2.2.1 机器人运动模型

首先需要建立移动机器人的运动模型,基于运动模型才能进一步探索机器人运行的轨迹特点,依据非完整约束的通用模型建立机器人运动模型如图7所示。在本文中假设机器人的移动轨迹是一段圆弧,那么可以通过微分的原理将路径轨迹划分为许多段小圆弧,然后以一对运动指令(vi,wi)来代表一小段的圆弧轨迹。对于一个移动机器人而言,其运动学模型可以描述如下[10]。

图7 机器人运动模型

其中,[xt+dt,yt+dt,θt+dt]T表示机器人在时间t+dt时在世界坐标系中的位置,[xt,yt,θt]T表示机器人在时间t时在世界坐标系中的位置,[dx,dy,dθ]T表示机器人在时间dt时的位置。底盘坐标系中的理想变化量,[dx,dy,dθ]T=[vxdt,vydt,wdt]T,vx、vy为机器人在底盘坐标系中沿x和y轴的线速度,w表示移动机器人的角速度。

2.2.2 速度采样

根据上文描述的机器人运动模型,通过约束机器人的速度,可以有效采集到机器人的速度,并推算出机器人在该采集速度下的运行轨迹。机器人的搜索空间受到机器人的运动学约束Vk、动力学约束Vb和障碍物约束Vo的限制[17]。机器人运动学约束了机器人采样速度和角速度的上下界。机器人动力学约束是基于电机性能对于机器人运行速度的限制,因受到电机转矩的限制,机器人实际能够到达的速度被限制在一定的范围之内。为保障一定机器人的安全性,障碍物约束作用是避免移动机器人同障碍物之间的碰撞,当机器人检测到障碍物时,需要立即减速,从而达到有效避障的目的。

相关约束描述如下。

其中,vmin和vmax代表机器人速度的上限和下限,wmin和wmax表示机器人角速度的上限和下限,vc是小车的当前线速度,wc是当前角速度,分别对应线加速度的上限和下限,分别对应角加速度的上限和下限,dmin表示机器人在下一时刻与障碍物的距离。

2.2.3 DWA 评价函数设计

通过设计path(v,ω)评价子函数,可以提高局部路径描述和全局路径描述的融合权重(见图8)。设计公式为

图8 path(v,ω)评价函数示例

其中(x1,y1)为DWA 模拟出的局部路径末端坐标,(x2,y2)为全局路径规划节点坐标。

通过机器人在运行过程中需要不断进行速度采样工作,可以确定机器人的速度搜索空间,筛选有效的采样速度簇就可以有效推算出机器人的多条路径轨迹。然而,对于众多的轨迹需要采用一个评价准则来选择最优轨迹,于是本文设计评价函数为

其中,heading(v,ω)用于测量朝向目标的进度,当机器人直接移动到目标时,该进度最大;dist(v,ω)表示到静态障碍物或临时障碍物的最近距离;vel(v,ω)代表前进速度;path(v,ω)子函数为提高局部路径和全局路径的融合权重。ε、τ、π和γ是权重,σ是4 个评价函数的归一化参数。

最后动态窗口法通过选择最小的G(v,ω)值下的路径轨迹作为机器人的最优路径。

2.3 基于安全A∗算法与动态窗口法的融合路径规划方法

动态窗口法具备良好的避障性能,然而在机器人的运行过程中,机器人只能采集到部分局部环境信息,这容易引导机器人陷入局部最优解[16]。由于采集信息的不完备性,本文将基于安全A∗算法搜索的全局路径信息作为DWA 的先验路径信息,设计融合函数对全局路径信息和DWA 局部轨迹信息进行实时约束,可以有效解决移动机器人陷入局部最优解的问题,从而提升移动机器人的导航性能。因此,实现安全A∗算法与动态窗口法的结合,可以有效获得全局最优的路径轨迹,同时可以保障移动机器人实时避障的能力。具体融合算法的实现如图9所示。

具体实现步骤为:首先,依据前文搭建的环境模型,建立机器人运动的栅格环境地图;其次,给定起始点和终点,采用安全A∗算法搜索有效的全局安全路径,保存机器人的路径信息Path_list;然后,读取安全路径信息Path_list,通过检测环境中出现的临时障碍物,以安全A∗算法的有效路径节点为DWA 的子目标点进行实时路径规划;最后,通过筛选最小的G(v,ω)值,规划出最优的安全路径。

3 仿真与实验

本文以Matlab 为仿真平台,依据文献[18-20],通过合理的参数设置可以提升移动机器人的导航性能。移动机器人的仿真参数设置如下:移动机器人的最大速度为0.6 m/s,移动机器人的最大角速度设置为3.6 rad/s,根据已有文献的经验数据设置移动机器人的数据采样周期为0.1 s,动态窗口法的评价函数的加权系数ε=0.1、τ=0.2、π=0.4 和γ=0.3。

3.1 仿真验证与结果分析

3.1.1 验证安全A∗算法

首先验证所提的基于危险指数的安全A∗算法。通过设置安全A∗算法、传统A∗算法以及改进A∗算法[20],对比并验证所提安全A∗算法的有效性及可行性。依据前文搭建的军队装甲车停车场的环境模型,设置合理的移动机器人仿真配置环境。

定期检查保养车辆、维护车场设施、整理车场内务,离不开移动机器人的广泛运用,在任务期间对整个车场开展巡视工作可以有效帮助战士开展工作。据此,本文采用多起点与多终点的仿真设置,通过依次连接环境中多个起始点和终点,方便移动机器人巡视环境中的每一个位置。仿真结果如图10、图11和图12 所示,分别为原始A∗算法、改进A∗方法以及本文方法的路径生成示意图。仿真输出结果如表1所示。此外,本文还依据军队车场直角边多的特点搭建简单环境验证所提方法的有效性,避免偶然性,仿真结果如图13 所示,表2 对应为输出结果。

图10 原始A∗算法

图11 改进A∗算法

图12 本文方法

图13 简单环境仿真结果

由实验得到的路径可以看出,原始A∗算法在车辆后方(图10 中序号①处)及墙壁(图10 中序号②)规划的路径较差,出现了较多的拐点,并且其路径在转弯处距离直角边过近,极易发生碰撞;由图11可以看出,改进的A∗算法拐点个数有所减少但碰撞风险依然存在;由图12 可见,本文所提的安全A∗算法转折点最少,且在直角边处保持了安全的距离,较好地解决了碰撞的问题。

表1 仿真结果表明,本文所提方法在处理军队装甲车停车场的路径规划问题时具备良好的性质,即相对于其他两种方法,安全A∗算法可以获得安全的路径,且路径转折点更少,路径长度更短。依据表2 结果,小环境中的安全A∗算法具备路径安全性,但是小环境下的仿真效果较其他方法优势不是特别突出。综上,本文所提方法适用于大环境的军队装甲车停车场的路径规划问题,且路径具备良好的性质。

表1 A∗、安全A∗以及改进A∗算法的仿真结果

表2 简单环境仿真结果

3.1.2 验证混合路径规划方法

通过在环境中设置临时障碍物代表突然出现的人或其他障碍物,以验证本文提出混合方法的有效性。依据前文设置的机器人仿真参数,对仿真环境进行合理设置。仿真结果如图14 所示。输出机器人的线速度、角速度以及角度曲线如图15、图16和图17 所示。

图14 混合方法仿真结果

图15 移动机器人线速度

图16 移动机器人角速度

图17 移动机器人角度

混合方法的平滑路路径长度为322.99 m,仿真时间为579.1 s。仿真结果表明,混合方法可以有效开展局部避障工作,且能够避开突然出现的障碍物,具有较好的导航性能。

3.2 实物验证与结果分析

本文使用树莓派3b +、rplidar A1 单线激光雷达、STM32F103、24 V 直流霍尔编码器减速电机自行搭建机器人操作系统(ROS)平台对本文所提混合方法进行验证。首先依据搭建的环境模型,设置固定障碍物;然后给机器人设置起始点和终点。在机器人运行过程中,突然人为加入临时障碍物,测试机器人的局部避障性能。

图18 所示为机器人实际环境下的运行示意图,共有2 次路径规划实验,分别对应图18(a)、(b)、(c)和(d)、(e),其中前一次实验加入了临时障碍。从实验结果可以看出,机器人能够在保持安全距离的情况下有效避开固定的障碍物以及临时加入的障碍物,并完成机器人路径规划任务。机器人状态参数输出结果如图19 所示,分别输出机器人的位置坐标、线速度随时间的变化趋势、角速度随时间的变化趋势以及角度随时间的变化趋势。本次实验机器人运行50.1 s,运行距离为6.75 m。结果表明,机器人可有效避障并完成巡检任务。

图18 实物验证结果与现场环境对应

图19 机器人输出结果

4 结论

本文就军队车场巡检机器人工作情景,针对传统路径规划算法所规划路线在拐角处容易发生碰撞的问题,将危险指数融入启发函数,提出一种安全A∗算法,使得规划的路径在拐角处能与障碍保持一定距离,大幅降低碰撞的风险;由传统8 搜索邻域扩展并去除同方向冗余子节点,平滑了路径,提高搜索效率;并将改进的A∗算法所规划的全局路径返回与动态窗口法结合,使得巡检机器人同时具备了局部动态避障能力。

本文在依据军队车场地形创建栅格地图中,通过多起点多终点的仿真设置,模拟真实的巡检情景,对传统A∗算法、改进A∗算法和本文所提安全A∗算法进行了对比验证,用简单地图作为补充验证,避免偶然性。实验表明,本文所提算法能够有效降低拐角处碰撞风险,且在运行时间、转折点数、规划路径上有较明显优势。在仿真地图设置临时障碍运行融合算法,证明了本文所提融合算法的可行性和有效性。使用ROS 小车平台在自行搭建的真实物理环境下实验,结果表明算法在真实环境下同样具有安全路径规划和避开临时障碍的能力,进一步验证了本文算法的可行性和有效性。