基于变步长A＊与车身稳态转向模型的UGV路径规划①

江 洪,姜 民

（江苏大学 机械工程学院,镇江 212013）

无人地面车辆（Unmanned Ground Vehicle,UGV）,是指在各种复杂环境中可以自主完成行驶任务的车辆,近年来,随着定位导航与控制技术的发展,UGV凭借可以代替人类完成诸多繁重工作的优势,已被广泛应用于军事、民用与科研等领域,其相关技术的研究也越来越受到人们的重视,尤其是UGV在复杂环境下的路径规划问题一直是一个重要的热点研究方向[1].

路径规划的本质是指在给定的环境场景中,将决策系统的宏观指令转换成一条连接起点与目标点的安全无碰撞的路径[2,3],其核心是寻路算法的设计[4].目前比较常见的寻路算法有人工势场法（ADF）[5],A*算法,快速随机树法（RRT）[6-8],遗传算法[9]等.这些算法各有优缺点,比如人工势场法具有较好的实时性,却容易陷入局部最优解; RRT算法简单实用,在搜索空间中的搜索速度也较快,但由于随机性过大,导致搜索结果不稳定,不能保证规划出的路径为最优路径,遗传算法具备较好的全局求解能力和鲁棒性,但收敛速度较慢、局部搜索能力弱; A*算法作为基于栅格地图的启发式搜索算法[10],具备最优性和完备性的特点,但其存在随着栅格增多搜索效率下降的缺点[11],且生成的路径曲率不连续,不利于车辆跟踪.

为此,有学者提出了一些改进办法:文献[12]中提出了一种双向A*算法,该方法通过从起点和目标点同时运行A*算法,达到缩短规划时间的目的,但该方法容易受实际地图环境影响,规划时间有时可能更长,且生成的路径往往不是最优,文献[13]提出一种基于目标偏向策略的P概率RRT算法,采样时设定一个参数值Pa和一个（0,1）范围内的随机值P,当Pa

针对现有研究中存在的问题,本文通过变步长的方法对A*算法进行改进,缩短规划时间; 并在全局路径的折点处基于车辆运动学模型进行局部重规划,使路径满足车辆运动学约束,易于跟踪; 此外还引入了障碍物延伸策略,提高了路径的安全性,最后通过仿真验证了本文改进算法的有效性.

1 A*算法

1.1 A＊算法的原理

A*算法是一种启发式搜索算法,在人工智能领域,常被用于解决各种路径规划问题,其在寻路前需先将地图栅格化,接着生成两个列表,一个是存放即将搜索的节点的open列表,一个是存放当前open列表中代价最小节点的closed列表,在开始寻路时,首先将起点放入closed列表,再将起点周围的8个临近栅格放入open列表,之后根据成本函数f（n）=g（n）+h（n）,从open列表中选出成本最小的节点,并将其存入closed列表中,循环执行这一过程,直到目标点出现在open列表中为止.此时将目标点加入closed列表,然后在closed列表里从目标点依次返回到起点,便可获得最小成本路径.具体流程图如图1所示.

图1 A*算法流程图

在成本函数f（n）=g（n）+h（n）中,g（n）为过去成本函数,表示当前节点到起始点的成本,h（n）为启发函数,表示当前节点到目标点的成本,在二维栅格化环境中,通常用欧氏距离来表示两个点之间的成本.

1.2 变步长A＊算法

在用A*算法进行路径规划时,需要先对无人地面车辆周围的环境进行栅格化处理,此时若搜索步长（指每个栅格的边长）设置较小,则对车辆周围环境的刻画会更精确,但随之计算量会变大,规划的效率显著降低;若搜索步长设置较大,可以缩短规划时间,但此时对障碍物轮廓的刻画不准确,导致在离障碍物较近时,可能会出现规划不出路径的情况,甚至有时会带来安全隐患[14]; 当前在用A*算法进行路径规划时,出于安全考虑,搜索步长通常较小,导致规划效率低.

为此,本文提出一种变步长A*算法,通过设置子目标点的方式计算当前合适的搜索步长,达到缩短规划时间的效果,设置子目标点的具体步骤如下:

（1）首先通过雷达检测并获取车辆与周围障碍物之间的最短距离Dz,此时可保证以车辆为圆心,Dz为半径的圆形区域是无障碍的安全区域.

（2）当Dz大于γDs时（Ds为UGV当前车速的下最短刹车距离,γ为安全系数）,如图2所示,此时先连接起点Ps与目标点Pg,得到虚拟最短路径LPsPg,再将以车辆当前位置为圆心,Dz为半径的圆弧与LPsPg的交点Pe设置为子目标点.

图2 子目标点选取示意图

如图2所示,在确定了子目标点Pe之后,将起点Ps与子目标点Pe分别作为一组对角栅格的中心点,便可得到一个分辨率为2×2的方形栅格地图（如图2中的点虚线区域所示）,此时的搜索步长为:

式中,Step为搜索步长,LPsPe为起点Ps至子目标点Pe的距离.另外由于起点Ps至子目标点Pe之间为无障碍的安全区域,故可将此2×2分辨率的栅格地图视作无障碍区域.

而当Dz小于γDs时,若仍采用低分辨率地图模型,由前文可知容易带来安全隐患,故此时回归传统A*算法寻路,搜索步长设置为常数N.

综上,UGV在不同环境中的搜索步长设置方法可用式（2）表达:

式（2）中,Ds为车辆当前车速v下的最短刹车距离,γ为安全系数,Ds的计算方法如式（3）所示.

式（3）中,v为车速,μ为地面的摩擦系数,g为重力加速度.

安全系数γ与车速v的关系如式（4）所示.

2 局部重规划

采用变步长搜索方式对A*算法作出改进后,可以提高规划效率,但其并未改变A*算法的寻路原理,因此规划出的路径仍然存在诸多折点,而UGV由于执行机构之间存在约束,导致其无法准确跟踪曲率不连续的路径[15].针对这一问题,本文基于UGV的稳态转向模型对变步长A*算法规划出的全局路径中的折点位置进行局部重规划,使规划出的路径满足UGV转向运动学约束,易于跟踪.

2.1 车辆转向模型分析

考虑到UGV的实时性要求,需要减少计算量,因此对模型做出如下简化假设:

① UGV在跟踪路径过程中,质心侧偏角保持不变;

② 内外轮转向角相同;

③ 忽略UGV的侧倾与俯仰运动;

④ 假设悬架为刚体,忽略垂向运动.

在此基础上,UGV的稳态转向运动学模型如图3所示,图中P为车辆转向时的瞬时转动中心,ω为UGV的横摆角速度,v为质心的速度,vx与vy分别为v在车辆坐标系oxy下x轴和y轴的分量,δ为前轮转角,lo为质心o的预测轨迹,a和b分别为前后轴到质心的距离.

根据汽车理论[16]与UGV的稳态转向模型可知,此时车辆的转向灵敏度（又称横摆角速度增益）为:

故横摆角速度ω为:

式中,K为稳定性系数,是反映车身稳态响应能力的重要参数,具体的计算方法为:

其中,m为车身质量,k1和k2分别为前后轮胎的侧偏刚度.

稳态时车辆的转弯半径R为:

假设车辆质心在当前位置o（Xo,Yo）时的横摆角为φo（图3中的φo=0°）,经过时间 t到达 o′（（Xo′,Yo′））处,则车身位置与横摆角的变化量为:

图3 UGV稳态转向模型

因此,由式（9）-式（11）可知,经过t时刻后质心o′的位置与横摆角为:

由此可知,在给定车速v下,输入一个前轮转角δ（前轮转角范围受车辆机械结构约束）,可推知经过时间t后车辆的位置与横摆角,因此可为行进中的车辆建立如图4所示的预测轨迹集合,图中loi（i=1,2,3,…）为质心o的预测轨迹.

图4 车辆质心预测轨迹集合

2.2 局部路径生成

如图5所示,在变步长A*算法规划出全局路径Lc之后,在Lc的折点处根据车身稳态转向模型生成一组预测轨迹集合loi（i=1,2,3,…）,之后根据设计的评价函数从loi（i=1,2,3,…）中选择一条评价值最高的路径log作为局部重规划路径.

图5 局部路径平滑示意图

为了使选择的最优轨迹log能够安全避开障碍物的同时,长度尽可能短,且能较好贴合Lc,本文设计选择局部重规划路径log的评价函数为:

式（13）中,Di为车辆质心o到Pri（i=1,2,3…）之间的路径长度,θ为弧线路径Li上点Pri处的切线Lq与初始路径Lc的夹角,Ki为每条弧线路径loi的曲率,Ob为障碍物,α1,α2,α3为每项的权重系数.

权重系数α1,α2,α3对应着不同的行驶要求,增大α1,UGV偏向走距离较短的路径; 增大α2,UGV偏向走与全局路径能较好贴合的路径; 增大α3,UGV偏向走曲率较小的路径,注重行驶稳定性; 根据UGV行驶环境可以改变权重系数的大小,来满足不同的行驶要求.

3 障碍物延伸策略

在UGV的路径规划仿真实验中,通常不考虑车身的实际宽度,这就导致规划出的路径可能会紧贴障碍物的边缘,在实际工程应用中容易带来安全隐患.为此,本文提出一种障碍物延伸策略,如图6所示,在障碍物的外边缘处,沿障碍物外边缘曲线的法线方向延伸一段距离Le,考虑到车辆质心与车身两侧的距离为车宽的一半,且实际行驶时UGV与障碍物需保持一定的安全边距Da,故本文将延伸距离Le设置为:

图6 障碍物延伸示意图

式中,w为车辆的宽度,单位为m;Da为安全边距,单位为m.

其中,Da与车速v的关系如式（15）所示:

式中,v为车辆速度,单位为km/h.

4 仿真验证

为了验证本文改进算法的有效性,用Matlab 2018b进行仿真实验,计算机配置为Intel（R）Core（TM）i5-4200U 2.30 GHz CPU、内存为8.00 GB.

4.1 本文改进算法与传统A＊算法仿真分析

本文首先创建了一个包含障碍物的地图模型,地图分辨率为30×30,起点坐标为（5,4）,目标点坐标为（27,26）,黑色部分为障碍物区域,白色部分为无障碍区域,在此地图上分别运用传统A*算法与变步长A*算法进行路径规划,为便于对比分析,设置A*算法的搜索步长N=1; 变步长A*算法的相关参数设置如下:设定路径跟踪时的车速v=10 km/h,道路摩擦系数μ=0.9,重力加速度g=9.8 m/s2.仿真结果如图7所示.

图7 传统A*与变步长A*算法仿真结果

由图7可知,传统A*算法在进行路径规划时,即便在距离障碍物较远的空旷区域,仍会生成许多冗余的路径节点,导致搜索效率下降,而在经过变步长的优化后,可以发现起点至A点、B点至目标点之间的区域,路径节点明显减少,由此达到缩短规划时间的效果,在A点至B点之间的区域,路径节点数量没有减少,这是因为此段区间UGV距离障碍物较近,应将搜索步长重新设置为N,即搜索步长为1.

变步长A*算法可以缩短规划时间,但是生成的路径仍然存在折点,不满足UGV的转向运动学约束,且路径紧贴障碍物边缘,不满足工程实际,为此,在变步长的基础上再依次进行局部重规划和引入障碍物延伸策略,其中相关参数设置如下:α1=1,α2=2,α3=1; 车身宽度w=1.6 m,v=10 km/h.仿真结果如图8、图9所示.

由图8可知,在进行局部重规划后,全局路径的折点处得到了较好的平滑,有利于UGV跟踪,但在CD区间段的路径与障碍物距离较近.

图8 加入局部重规划

如图9所示,引入障碍物延伸策略之后得到的路径,即为本文改进算法所规划出的最终路径,从图中可以发现路径与障碍物之间保持了一定的边距,提升了路径的安全性,更符合工程实际.

图9 引入障碍物延伸策略

传统A*算法与本文改进算法的寻路时间和路径长度数据如图10所示,从图中可以发现,本文改进的算法相比传统A*算法,寻路时间缩短了50.6%,而路径长度略长于传统A*算法,这是因为本文改进算法加入了障碍物延伸策略,生成的路径与障碍物保持了安全间距,导致路径长度也随之增加.

图10 本文改进算法与传统A*算法路径长度与寻路时间对比

4.2 本文改进算法与其他算法仿真对比

之后本文又搭建了3组相对复杂的环境场景,并分别与文献[12]中提出的双向A*算法、文献[13]中提出的P概率RRT算法进行对比.其中,3组仿真场景中的起点坐标分别设置为（5,25）,（3,15）,（4,7）; 目标点坐标分别设置为（25,5）,（25,17）,（26,22）; 仿真结果如图11和图12所示.

由图11可知,在3组不同的仿真场景下,相比双向A*算法与P概率RRT算法,本文改进的算法规划出的路径更平滑,且与障碍物保持了安全边距.

图11 不同算法寻路效果对比

由图12可知,在3组不同的仿真场景中,本文改进算法的规划时间均为最短,其中在图12（a）场景中,本文改进算法的规划时间相比双向A*算法减少了8.3%,相比P概率RRT算法减少了24.6%; 在图12（b）场景中,本文改进算法的规划时间相比双向A*算法减少了11.8%,相比P概率RRT算法减少了31.3%; 在图12（c）场景中,本文改进算法的规划时间相比双向A*算法减少了14.2%,相比P概率RRT算法减少了18.7%; 而在3组仿真场景中,本文改进算法规划出的路径长度略长于双向A*算法,这是因为本文改进算法加入了障碍物延伸策略,导致生成的路径无法紧贴障碍物边缘,从而路径长度也随之增加; 与P概率RRT算法相比,本文改进算法的路径长度优势明显,这是因为P概率RRT算法的随机树在扩展节点时,仍然存在一定概率往随机方向扩展,缺乏完备的目标导向性.

图12 不同算法规划路径长度与寻路时间

5 结论与展望

针对A*算法规划效率低,且生成的路径存在较多折点的问题,本文提出一种基于车身稳态转向模型的变步长A*算法,首先通过设置子目标点的方式调节搜索步长,减少寻路时间; 然后在变步长A*算法生成的全局路径的折点处,基于车辆转向运动学约束进行局部重规划,从而得到一条平滑路径,更利于UGV跟踪;改进后的算法还引入了障碍物延伸策略,提升了路径的安全性; 最后搭建了两组不同的环境场景进行仿真实验,结果表明,本文改进的算法相比其他3种主流寻路算法,规划时间更短,生成的路径更平滑,且与障碍物保持了安全边距.

目前的研究成果对接下来的实车试验具有理论指导作用,下一步将搭建UGV实车平台对提出的改进算法进行验证.