启发式RRT算法的AGV路径规划

杨 瑶，付克昌，蒋 涛，张国良，刘甲甲，孟 易

成都信息工程大学 控制工程学院，成都610225

1 引言

近年来，随着智能车的快速发展，智能车自身的优势也日渐凸显。比如：智能车能够减少驾驶压力、提高驾驶的安全性、避免交通拥堵和降低对环境的污染[1]。而路径规划作为智能驾驶技术[2]和多机器人协同技术[3]的核心，对未知道路的规划能力是衡量智能车的重要标准。目前，学者们对路径规划算法进行了大量研究，主要包括：基于随机采样的快速搜索随机树等方法[4]；基于图搜索的A*算法[5]、D*算法[6]等；基于模型的人工势场法[7]、动态窗口法[8]等；基于生物启发式的遗传算法[9]等。其中，1998年，LaValle首次提出RRT算法，采用随机采样的规划方式，不需要对状态空间进行预处理，搜索速度较快，且具有概率完备性，因此被广泛运用于机器人的路径规划中。

但传统的RRT算法仍存在如下缺点：（1）所使用的全局均匀随机采样，使得算法运行时间增加，收敛的速度变慢[10]；（2）采用最近节点选择算法往往会导致其在复杂场景时所规划的路径失败[11]；（3）所规划的路径没有考虑车辆运动学约束，使得规划的路径不能运用于智能车的路径规划中[12]。

针对上述问题，国内外学者都提出不同的改进和优化方法。在2001年Kuffner和LaValle[13]提出并行生成随机树（RRT-Connect）的方法，以提高该算法的收敛速度，但其未考虑车辆的运动学约束。在2010年Karaman和Frazzoli[14]提出RRT*算法，用于解决RRT算法产生的路径并非概率最优解的问题，但其运行效率较低。在2013年，M.Jordan和A.Perez[15]提出B-RRT*，用RRT*算法进行起始点和目标点的双向扩展，以减少RRT*算法所执行的时间，但其应用于移动机器人中，不能直接用于智能车。在2015年，杜明博，梅涛等人[12]提出连续曲率的RRT算法，通过加入环境约束和车辆自身约束，使得所规划的路径能够较好运用于智能车中，但未考虑车辆到障碍物的约束。2017年，冯来春，梁华为等人[16]通过A*算法形成引导域，然后使用RRT算法在其引导域中进行扩展，用于解决RRT算法随机采样所造成的问题，但其未解决智能车在目标点朝向的问题。贺伊琳，高奇等人[17]于2018年在传统RRT算法基础上加入采样可行区域，并使用三次B样条曲线进行平滑处理，以解决RRT算法所规划的路径曲折不平滑的问题，但未考虑智能车与障碍物的关系使得所生成的路径存在着与障碍物过近的情况。2018年张捍东，陈阳，吴玉秀[18]提出将RRT算法与视野域自适应滑动窗口相结合，用于解决在未知环境下的路径规划，但是其改进未考虑智能车的运动学约束，不能直接用于智能车的路径规划。

针对上述优化中存在的不足，本文在B-RRT*算法基础上提出一种适用于智能车的路径规划算法[19]。首先，使用Reeds_Shepp曲线进行预处理以解决车辆在目标点朝向的问题，并且使用双向目标偏向采样策略以提高算法的效率。其次，提出启发式滑动窗口的方法以限制B-RRT*算法采样带来的不确定因素。同时，在新生成节点上加入障碍物约束以解决B-RRT*算法所规划的路径离障碍物过近的问题。然后，在B-RRT*算法重新选择父节点和重布随机树的过程中加入车辆最大转角约束使扩展树能够满足车辆的运动学约束。最后，使用贝塞尔曲线拟合路径点使得路径更加平滑。通过对基本B-RRT*算法进行上述改进，使得所规划的路径更加安全合理且更适用于车辆的运动学特性。

2 车辆的运动学模型

为了验证算法的适用性，本文采用智能汽车作为研究对象。由于汽车是一种非完整性约束的系统[12]且分析较为复杂，所以本文只考虑其简化模型。本文的实验平台为野马E70电动汽车，如图1所示，其简化模型如图2所示。在惯性坐标系XOY下，(x,y)为车体在此坐标系下的坐标位置，φ为车体的横摆角（航向角），L=1.56 m是智能车的轴距，D=1.835 m表示智能车的宽度，智能车的长度为5.555 m,R是智能车的转弯半径，θ为车辆纵轴y与横轴x之间的夹角。由于车辆只能用前轮转向，所以需要满足阿克曼转向条件(φ＜φmax)。本文只考虑车辆在二维平面上运动，不考虑俯仰和侧倾带来的影响[12]。同时，假设在车辆运动瞬间，车辆的速度与车体保持平衡，即满足：

其中，Kmax=0.16m-1车辆最大转折曲率，Rmin=8 m车辆最小转弯半径，φmax=40°车辆最大转角。

图1 基于野马E70电动汽车改造的智能车驾驶平台

图2 车辆简化运动学模型

3 B-RRT＊算法

B-RRT*算法是一种双向渐近最优算法[20]，其过程如下：

（1）初始化：从起始点向目标点构建扩展树tree1，从目标点向起始点构建扩展树tree2。

（2）在地图上生成随机点qrand1和qrand2，如果随机点qrand1和qrand2落在无障碍物的区间内，则遍历tree1和tree2找到离随机点最近的节点qnearest1和qnearest2。否则，重新从搜索空间选择随机点。

（3）判断qrand1和qnearest1，qrand2和qnearest2连线上是否有障碍物。

如果连线上无障碍物，则判断qrand1和qnearest1，qrand2和qnearest2之间的距离是否小于扩展步长（本文设置为5 m），若距离小于扩展步长，则将随机点添加到扩展树中。否则，在最近节点和随机点的连线上取扩展步长长度的点qnew添加到扩展树上，再将随机点添加到扩展树上。

如果连线上有障碍物，则执行（2）。

（4）在tree1和tree2分别执行重新选择父节点和重布随机树的过程。

（5）判断tree1和tree2之间的距离是否小于阈值。若两棵树之间的距离大于阈值（本文设置为5 m），重复执行（2）～（4）。若小于阈值，则从相遇的tree1和tree2的节点上，分别反向取到起始点和目标点并保存路径（如图3的示意图）。

（6）输出B-RRT*算法所规划的路径。

图3 B_RRT*算法示意图

4 改进B-RRT＊算法

与微小型移动机器人路径规划相比，智能车的路径规划是一种三维的规划，不仅要考虑车辆的位置(x,y)而且还考虑车辆到达目标点的航向φ。因为智能车并非质点，所以在智能车的路径规划中，还需要考虑所规划的路径的转折角度是否小于最大转折角度、路径与障碍物之间的距离是否安全合理等情况。

4.1 预处理过程

Reeds-Shepp曲线[21]是Reeds和Shepp二人在1990年，针对车辆的运动学提出的最优曲线。通过对车辆进行运动学建模，得到车辆的最小转弯半径和起始点与目标点的姿态，生成适用于车辆的运动曲线。

在执行B-RRT*算法之前或在目标点附近，使用Reeds-Shepp曲线进行路径规划得到路径，并判断路径上是否与障碍物发生碰撞。若没有，则保留Reeds-Shepp曲线所生成的路径，完成规划。

图4表示了预处理过程，图4（a）、图4（b）分别表示起始点朝向与目标点朝向相同和垂直的情况。蓝色路径为使用Reeds-Shepp曲线预处理生成的路径，绿色的路径为B-RRT*算法规划的路径。从图4中，能够看出使用Reeds-Shepp曲线能够有效解决车辆朝向的问题，并且所生成的轨迹也更加平滑。

图4 预处理过程

4.2 启发式滑动采样窗口

由于基本的B-RRT*算法在搜索空间（节点qrand生成的范围）随机采样，导致所生成的节点不合理并使得算法收敛速度的减慢。本文的实验环境是校园道路，通过加入滑动窗口使得B-RRT*算法的采样点能够更多地集中于道路中，而启发式改进使得采样点能够朝着目标点“生长”并能提高算法的收敛速度。因此，本文提出双向偏向性采样与启发式滑动采样窗口相融合的方法（如公式（3）所示）。

首先，设定一个目标偏向概率[21]p1（本文设置为10%），获得采样时的概率p并代入公式（3）中。若p＜p1，则将目标点或起始点直接赋值给qrand1或qrand2，这样可使随机树能够较快地向外“生长”。当p＞p1时，在tree1中寻找与目标点最近的节点qnearest1并判断在该节点周围以D为长度的正方形内有无障碍物。如果没有，则在最近节点建立滑动窗口（窗口长度为2 m应略大于智能车辆的宽度），并在该窗口中生成qrand1，以避免采样的随机性。在tree2的“生长”过程中，寻找与起始点最近的节点qnearest2，进行上述过程并建立滑动窗口，约束qrand2的坐标（如图5示意图所示）。

为了验证启发式滑动窗口对B-RRT*算法的改进作用，将带有启发式滑动窗口B-RRT*算法和基本B-RRT*算法进行对比。图6中的坐标点为所测试的起始点和目标点，其中S表示起始点，分别选取G1,G2,G3,G4,G5作为目标点，采样窗口宽度为2 m。图7表示改进B-RRT*算法和基本B-RRT*算法在各个测试点运行的平均时间（每种算法执行20次，取平均值）。

图5 启发式滑动采样窗口示意图

图6 采用启发式滑动采样窗口的B-RRT*算法与基本B-RRT*算法的测试点

图7 带有启发式滑动采样窗口的B-RRT*算法和基本B-RRT*算法测试对比

从图7（a）和（b）中可以看出，加入启发式滑动采样窗口能够减少B-RRT*算法的迭代次数，并提升了B-RRT*算法的运行时间。同时，从图7（b）中可以看出，启发式滑动采样窗口能够较好解决B-RRT*算法随机采样所带来的不确定性并提高算法的收敛性速度，使得B-RRT*算法能够更好地运用于智能车。

4.3 新节点q new到障碍物安全距离约束

由于定位和地图本身存在一定的误差，所以必须保证规划的路径与障碍物之间的距离大于车身宽度，不能使所规划的路径贴合障碍物或离障碍物过近（如图8表示路径与障碍物之间的关系）。但在基本B-RRT*算法中，未考虑路径和障碍物的距离关系使得所规划的路径点存在着与离障碍物过近的情况，不能被智能车直接使用。

图8 规划路径和障碍物示意图

在基本B-RRT*算法产生新的节点qnew的过程中，仅仅是通过在qrand和qnearest连线上，以扩展步长为长度生成节点qnew，而未考虑该节点与障碍物所需的安全距离。本文通过生成qnew周围的待选节点（如图8所示）并将其与障碍物安全距离约束相结合的方法（如图9所示），使得所规划的路径与障碍物之间的距离大于安全距离。即d＞Dsafe，其中Dsafe=0.917 5m表示安全距离，取为车辆宽度的一半，d表示规划路径与障碍物之间的距离。

图9 q new周围待选节点的选取示意图

图9 中，qnew与周围的待选节点poses1之间的距离D=1.835 m。=1 m表示待选节点poses1与周围节点poses2的距离。

图10流程图的基本步骤如下：

（1）首先获得节点qnew，初始化dmin（初始化定义dmin等于无穷大）。

（2）获得qnew四周的节点poses1，依次从中选取pose并判断是否为障碍物。如果是，则从poses1中重新选取pose。否则在pose节点四周添加poses2(poses1={A1,B1,C1,D1},poses2={A11,A12,A13,A14,B11,B12,B13,B14,C11,C12,C13,C14,D11,D12,D13,D14})。

图10 q new到障碍物安全距离约束流程图

（3）在poses2中依次选择poses1，并判断poses1是否为障碍物。如果是，则执行（2）。否则，计算pose到目标点距离d。

（4）判断d和dmin大小，如果dmin＞d，更新qnew和dmin。否则执行（2）。

（5）输出qnew。

为了验证新节点qnew的障碍物安全距离约束对算法对运算成本的影响，将带有新生节点安全距离约束的B-RRT*算法和基本B-RRT*算法在图6处进行测试对比。图11表示改进B-RRT*算法和基本B-RRT*算法在各个测试点运行的平均时间（每种算法执行20次，取平均值）。

图11 加入障碍物安全距离约束对规划时间的影响

从图11可以看出，对新节点qnew加入障碍物安全距离约束使得B-RRT*算法的运算成本增加，但是通过加入这项约束能够使得B-RRT*算法所规划的路径与障碍物保持一定的距离，增加智能车在行驶中的安全性。

4.4 车辆最大转角约束

重新选择父节点和重布随机树是基本B-RRT*算法的重要部分，但在基本B-RRT*算法中并未考虑车辆的运动特性，导致所规划的路径存在着转折次数较多和转折角度较大等问题不利于智能车的行驶。因此本文通过在重新选择父节点和重布随机树过程中加入车辆最大转角约束和障碍物安全距离约束相结合的方式使得重选父节点和重布随机树的过程结果更加合理。

（1）重选父节点过程

在基本B-RRT算法中，qnearest是新生成节点qnew的父节点。然而，在B-RRT*中会以qnew为圆心和R为半径选择新的父节点，并计算从根节点到待选父节点距离和待选父节点到qnew的距离的总和，选择最短距离的待选父节点作为qnew父节点（如图12（a）和12（b））。虽然B-RRT*算法能得到相对较短的路径，但忽略了智能车自身的约束条件。在本论文中，将车辆最大转折曲率作为约束条件，加入重选父节点的过程中（如图12（c）所示）。此外，在更新父节点的过程中还需判断在父节点周围以D为边长的正方形内有无障碍物，只有在该正方形内无障碍物才更新父节点。

图12 重新选择父节点过程

其中，图12（a）是以R为半径⑨为圆心的重选选择父节点的过程，图12（b）是使用基本B-RRT*算法执行重选父节点的结果⓪➝⑤➝⑧➝⑨其路径的长度为11），图12（c）是加入车辆最大转角约束（如公式（4））的重新选择父节点的结果⓪➝④➝⑨其路径的长度为14）。通过图12（b）和（c）可知，虽然（b）的路径比（c）短，但是φ5-8-9＞φmaxφ5-8-9为路径⑤➝⑧➝⑨的角度）不满足路径点的转角小于智能车最大转角的要求，所以放弃节点⑧作为节点⑨的父节点。

φparent表示待选的父节点（圈内的节点）的角度，A表示常数，d1表示树根到qnew的距离。Cparent表示待选父节点的代价值。

其步骤如下：

①获得节点qnew并以阈值R为半径，找出所有待选父节点。

②依次从待选父节点qparent中，计算qparent,qparent的父节点和qnew所形成角度是否小于车辆的最大转角φmax。如果是，则计算根节点到qnew的距离并代入公式（4）得到待选父节点的代价值。否则，从待选父节点中重新获得新的父节点qparent。

③从所有待选父节点中，依次判断父节点在D为边长的正方形内有无障碍物。若无障碍物，则更新最小代价值并保留qnew新的父节点。否则，继续从待选父节点中选取父节点。

（2）重布随机树过程

在qnew重新选择父节点后，为了进一步使得随机树之间连接代价减小，所以要为随机树重新布线。即以新生成的节点qnew⑨为圆心，R为半径并以④为父节点，圆内其他的节点为子节点qnew的父节点除外）。但是由于车辆存在最大转角度约束，因此在重布随机树过程中需要加入车辆约束。图13（a）表示基本B-RRT*算法重新选择子节点的结果，（b）表示基本B-RRT*算法重新布线的结果⓪➝④➝⑨➝⑥其路径为15），（c）表示加入车辆最大转角约束所重新布线的结果⓪➝④➝⑨➝⑩其路径为16）。通过图13（b）和（c）可知虽然（b）的路径比（c）短，但是φ4-9-6＞φmaxφ4-9-6为路径④➝⑨➝⑥的角度）不满足路径点的转角小于智能车最大转角的要求，所以放弃节点⑥作为节点⑨的子节点。

其步骤如下：

①获得节点qnew并以阈值R为半径获得待选子节点qchild。

②依次从待选子节点qchild中，计算qnew,qnew的父节点和qchild所形成角度是否大于车辆的最大转角φmax。如果是，则计算根节点到qchild的距离并代入公式（5）得到待选子节点的代价值。否则，从待选子节点中重新获得新的子节点。

③从所有待选子节点中，依次判断子节点在以D为边长的正方形内有无障碍物。若无，则更新最小代价值和qnew新的子节点。否则，继续从待选子节点中选取子节点。

φchild表示待选的子节点（圈内的节点）的角度，B表示常数，d2表示树根到待选子节点的距离。Cchild表示待选子节点的代价值。

图13 重布随机树过程

为了验证车辆最大转角约束对算法运算成本的影响，将带有车辆最大转角约束的B-RRT*算法和基本B-RRT*算法在图6的环境中进行测试对比。图14表示改进B-RRT*算法和基本B-RRT*算法在各个测试点运行的平均时间（每种算法执行20次，取平均值）。

图14 加入车辆最大转角约束对规划时间的影响

从图14中可以看出，加入车辆最大转角约束使得B-RRT*算法的运算时间略微增加，但所增加的运算成本也在可以接受范围之内，而加入这项约束能够解决B-RRT*算法中重新父节点和更新子节点未考虑车辆约束的问题。

4.5 路径点优化和拟合

在加入启发式滑动采样窗口、障碍物安全距离约束和车辆最大转角约束后，可得到相关的路径点poses，然后对路径点进行优化，其步骤如下。

（1）依次从路径点poses中取出pose1和pose2并计算pose1与pose2的距离D1，再判断D1是否大于扩展步长。如果是，则执行（2）。否则，继续执行（1）。

（2）在pose1和pose2连线上，取距离pose1以扩展步长为长度的点作为posenew。

（3）对posenew进行到障碍物的安全距离约束，使posenew更加合理并将posenew插入poses。

（4）使用贝塞尔曲线对路径点poses进行拟合。

贝塞尔曲线是法国工程师皮埃尔。贝塞尔在1962年提出，其形状可由曲线的控制点确定。因此，将改进B-RRT*算法的路径点作为贝塞尔曲线的输入，消除路径不平滑的情况，如图15所示。

图15 使用贝塞尔曲线示意图

使用不同拟合方法示意图如图15所示，其中A点，B点和C点表示改进B-RRT*算法的路径点。从图15可以看出，使用二次样条插值所生成的路径的曲率较大，二次B样条曲线拟合的起始点是A点和B点连线的中点，目标点是B点和C点连线的中点，导致所生成的路径在起始点和目标点处不连续，而使用二次贝塞尔曲线拟合能够较好解决上述二次样条插值和二次B样条曲线拟合所带来的问题。

公式（6）为贝塞尔曲线的一般方程，公式（7）是本文所使用的二阶贝塞尔曲线，P0,P1,P2为二阶贝塞尔曲线的控制点。

5 算法验证

为了验证改进B-RRT*算法的有效性，本文分别将RRT、RRT*、B-RRT*和改进的B-RRT*算法应用于成都信息工程大学校园的栅格地图（4846×2816）上（如图16），栅格大小为0.298 m/pixel，选择不同的地点进行实验。实验基于ROS（开源机器人操作系统）平台，并使用Rviz对规划结果进行可视化显示，计算机配置为：ubuntu14.04 LTS，处理器intel i5-6500，主频为3.2 GHz，运行内存为8 GB。

图16 学校的校园地图

改进B-RRT*算法的参数选取：（1）扩展步长和阈值的选择，当“相遇”的两个节点之间的距离小于车身长度时，则算法收敛，因此本文将扩展步长和阈值设置为5 m便于计算；（2）为了使更多的采样点落在智能车的正前方，所以启发式滑动采样窗口长度应略大于智能车辆的宽度，本文设置为2 m；（3）安全距离Dsafe的选择，因为本文是以车辆后轴上中心点处为车辆位置的参考点，所以安全距离应为车宽的一半，本文设置为0.917 5 m；（4）由于智能车的宽度为1.835 m，所以以车宽为距离获得待选节点poses1能够使得qnew更为合理；（5）D1around的选择，待选节点poses1与四周节点poses2的距离D1around应大于安全距离Dsafe以便为智能车提供一定的冗余距离，本文设置为1 m；（6）其他参数选择，车辆的最大转角为φmax=40°。

本次实验通过在地图上选择起始点和终点，使用RRT、RRT*、B-RRT*和改进的B-RRT*算法规划。通过路径的长度、到障碍物的距离和转折数量（转折角度＞φmax=40°，则认为是转折）等方式来评价路径的好坏。对区域a和区域b进行路径规划，规划结果见图17、图18和图19，其中蓝色节点为起始点向目标点的采样节点，红色节点为目标点向起始点的采样节点。表1、表2和表3分别为连续转折区域、直角转折曲线区域和停车场连续转折区域的指标对比。

表1 区域1测试对比

从图17、图18和图19中可以看出，改进B-RRT*算法所规划出来的路径较为平滑，且具有连续的曲率，能够解决基本RRT、RRT*、B-RRT*算法在初始时刻路径不合理的问题，同时与障碍物形成一定的安全距离并且能够较好地满足车辆运动学特性。

图17 在区域1测试的结果

图18 在区域2测试的结果

图19 在区域3测试的结果

表2 区域2测试对比

表3 区域3测试对比

通过表1、表2和表3能够看到虽然基本RRT、RRT*、B-RRT*算法能够在较短的时间内规划出路径，但是所规划的路径转折角度过大、路径点贴合障碍物，导致路径不能被智能车使用。而本文提出的改进B-RRT*算法在牺牲较短时间的基础上获得了更加平滑、安全且更符合车辆运动学的路径。同时，通过加入对新生节点安全距离约束和车辆最大转向约束等增加了算法的复杂度使得算法的运行时间增加，但对处于实验阶段的中低速智能车或者AGV来说所增加的时间也是在接受的范围之内，并且通过上述对B-RRT*算法的改进增加了改算法在实际应用中的可行性。

6 结论

由于定位和地图所带来的误差，以及车辆运动学约束，使得基本RRT、RRT*和B-RRT算法不适用于本文所测试的环境。所以，本文提出的改进B-RRT*算法克服了基本的RRT、RRT*、B-RRT*算法存在的转折点较多、路线不平滑、规划路径与障碍物过近和曲率不连续等问题，使得所规划的路径能更加满足车辆的运动学模型。同时，通过使用Reeds-Shepp曲线在目标点附近重新规划能够解决目标点朝向的问题，并通过约束采样范围增强了算法的鲁棒性。需要指出的是，本文算法主要解决实验阶段中低速智能车或轮式机器人的规划问题。后续将对该算法进行改进，使其能够应用在高速行驶的智能车上。