下载掌阅APP，畅读海量书库
立即打开
张旭东，徐福康，邹渊，尹鑫
北京理工大学
【摘要】 为实现无人车辆在未知非结构化环境中的导航，并在前往目标点过程中建立全局地图，本文基于TangentBug算法的框架，提出了一种运动规划算法。通过测距传感器获取局部环境信息，并结合SLAM算法实现了实时定位与建图。通过构建规划参考点集合，获取最优的局部目标点。通过结合Dubins曲线和模型预测的方法，在线生成符合车辆非完整约束的轨迹，并用碰撞检测保证车辆的安全性。通过加入考虑定位误差的状态转换规则，使车辆的状态更加稳定。室外实车实验表明，本文算法可实时规划出安全、平滑的轨迹，使无人车辆到达目标点并获取全局地图。
【关键词】 无人车辆，运动规划，未知非结构化环境，Bug类算法
A Motion Planning Algorithm for Autonomous Vehicles in Unknown Unconstructed Environments
Zhang Xudong, Xu Fukang, Zou Yuan, Yin Xin
Beijing Institute of Technology
Abstract: In order to navigate autonomous vehicles in unknown unconstructed environments,and establish the global map in the process of moving to the target,this paper proposes a motion planning algorithm based on TangentBug.The range sensor is utilized to provide the local environment information and realize real-time localization and mapping combined with the SLAM algorithm.The local target set is established to choose an optimal local target.The Dubins path and a model predictive method are fused to generate a trajectory satisfying the nonholonomic constraints and the safeness is guaranteed by collision checking.Considering the localization errors,a more robust state switching mechanism is presented to make the state of a vehicle more stable.The results of outdoor experiments show that our algorithm can generate a safe and smooth trajectory in real-time,and navigate the autonomous vehicle to the target and obtain the global map successfully.
Key words: autonomous vehicles,motion planning,unknown unconstructed environments,Bug algorithms
随着无人车辆技术的发展，无人车辆的运动规划算法在近年来得到了广泛的研究 [1] 。在无人系统中，运动规划模块利用环境感知模块处理后的传感器信息，在起点和终点间规划出安全、平滑的路径 [2] 。
在许多经典的规划算法中，完全已知的全局环境信息是进行规划的前提 [3，4] 。而在现实条件下，全局环境信息往往不能提前获得，即环境是未知的。此时，无人车辆一般能获取的信息包括局部环境信息、车辆自身和目标点的位姿。
本文算法面向的非结构化环境主要指无明显道路标志、无道路交通规则的区域，如封闭园区、野外环境等。目前针对这类环境的运动规划算法主要有：面向迷宫的规划算法、人工势场类算法和Bug类算法。其中，由于面向迷宫的规划算法 [5，6] 一般假设迷宫中的墙壁互相连接为一个整体，当存在孤立的障碍物时，常陷入死循环而导致规划失败。人工势场类算法 [7]，[8] 由于引力和斥力的叠加，容易陷入局部最优解，进而导致规划失败。
针对上述两类算法存在的缺陷，Lumelsky等 [9] 提出了Bug1和Bug2算法，通过朝目标移动和环绕障碍物两种运动状态的组合，无人车辆在行驶过程中可避免陷入死循环和局部最优解。根据Zhu等 [10] 的研究表明，Bug类算法是目前唯一一类被证明具有完备性的算法。基于Bug1和Bug2的理论基础，不断衍生出Bug类算法的变体，使得Bug类算法在规划路径的总长度上得到优化 [11] 。Franco等 [12] 将改进后的Bug类算法应用于多个轮式机器人对未知环境进行探索建图。Kovács等 [13] 在人工势场类算法中加入了Bug类算法，通过两类算法的转换，使轮式机器人能自行摆脱局部最优解。Zaki等 [14] 将A*与Bug2相结合，构建了一种多算法融合的规划算法，用于无人巡逻车辆。Kim等 [15] 在应用Bug类算法时考虑了无人车辆的非完整约束，生成了圆弧曲线作为参考路径。Xu等 [16] 以轮式机器人和目标点的位姿作为端点，生成Dubins曲线作为Bug类算法的参考路径。
虽然Bug类算法具有完备性的优点，但是根据McGuire等 [17] 的研究表明，Bug类算法易受定位误差和障碍物检测误差的影响，并随着误差的增大，Bug类算法规划成功的概率显著减小。而在上述Bug类算法的研究中，对这些误差的考虑较少，算法的假设过于理想。此外，上述生成圆弧和Dubins曲线的算法虽然能够满足车辆的非完整约束，但在不同算法运行周期间规划出路径的曲率变化较大，不利于进行运动控制。
受上述研究启发，本文提出了一种面向未知非结构化环境的无人车辆运动规划算法，具体包括：通过构建规划参考点集合以及考虑定位误差的状态转换规则 [18] ，提高了Bug类算法的规划成功率；通过结合车辆运动学模型和运动控制算法，在线生成符合非完整约束的平滑轨迹，并以Dubins曲线作为生成失败时的轨迹；通过SLAM算法进行实时定位，并沿规划路径构建全局地图。
无人车辆的运动学模型如图1和式（1）所示。在惯性坐标系 OX I Y I 下，状态量 q =[ X ， Y ， Ψ ] T ，控制量 u =[ v ， ω ] T ，其中车辆坐标为（ X ， Y ），横摆角为 Ψ ，纵向车速为 v ，横摆角速度为 ω 。前轮转角 δ 和转向半径 r 的关系如式（2）所示，最大的前轮转角 δ max 对应最小的转向半径 r min 。
图1 车辆运动学模型
式中， l w 为车辆轴距（m）。
无人车辆的规划模块需要在一定环境模型的基础上进行规划。本文假设无人车辆装配有测距传感器，可在各个方向上获取最近障碍物的距离信息。基于感知模块处理过的距离信息，本文构建了两种环境模型，一种为规划参考点集合，用于在每个算法运行周期提供局部目标点；另一种为栅格地图，用于规划路径的碰撞检测。
规划参考点集合的构建如图2所示，图中 O i 为第 i 个障碍物； o 为车辆坐标系的原点，即车辆的位置； λ max 为测距传感器的最大探测距离； φ r 为测距传感器的角度分辨率； s n 为各个方向上最近的障碍点，其组成了集合 S ，若没有障碍物，则 s n 落在探测边界上。设落在探测边界上的 s n 为 b n ，并组成了集合 B 。
对于障碍物的边界点，可通过分析探测距离的不连续性获得，在本文中，障碍物的边界点被分成了两种： e cw 和 e ccw 。考虑到无人车辆的安全性，其不能被视为一个质点，因此本文对障碍物的边界点进行了旋转采样。其中， e cw 为需要顺时针采样的边界点， e ccw 为需要逆时针采样的边界点，对其进行旋转采样后分别得到了 r cw 和 r ccw 。设 r cw 和 r ccw 分别组成了集合 R cw 和 R ccw 。在图2中， θ r 为采样步长； θ min 、 θ max 分别为旋转采样的最小和最大角度，可分别由式（3）和式（4）确定。
图2 规划参考点集合构建
式中， r safe 为无人车辆的安全距离（m）； k safe 为安全系数； d （·，·）为两点间的距离（m）； k s 为采样比例系数。
设目标点在车辆坐标系下的位姿为 t ，其组成了集合 T ，若目标点不在测距传感器的探测范围内，则 T =Ø。基于以上讨论，本文定义规划参考点集合 F 如式（5）所示：
即规划参考点集合 F 包括了探测边界上的点、旋转采样后的障碍物边界点和探测范围内的目标点。设 F front 为 F 中位于车辆前方的点组成的集合，那么有：
式中，atan2（ a ， b ）为点 a 相对于点 b 的方位角。
此外，为保证无人车辆的安全性，本文用文献[19]中的方法对规划路径在栅格地图中进行了碰撞检测。
本文算法的整体架构如图3所示。其中，环境感知模块需要对测距传感器提供的原始环境信息中的地面点等进行处理。无人车辆的定位与未知环境的建图采用了文献[20]中的SLAM算法，结合上文构建的两种环境模型，运动规划模块为运动控制模块提供了一条安全、平滑的轨迹。
本文算法将轨迹生成分为路径生成和参考速度生成两步。通过按一定的代价在规划参考点集合中选取最优的元素，路径生成即在车辆坐标系下生成一条连接 q o =[0，0，0] T 和 f i =[ x f ， y f ， φ f ] T 的曲线。不同生成路径方法的对比如图4所示，其中绿色的为Dubins曲线，虽然其长度最短且满足车辆的非完整约束，但在圆弧和直线段的连接处存在曲率不连续；蓝色的为经过 q o 和 f i 的圆弧，该圆弧在 f i 处并不能满足其航向的要求；红色的为用预瞄算法 [21] 沿经过点 o 和 f i 的直线迭代生成的曲线， l p 为预瞄距离， r p 为预瞄半径，该曲线有曲率连续、能满足车辆的非完整约束和航向要求的优点。此外，用预瞄算法和车辆运动学模型可保证迭代路径能在线生成。
图3 本文算法的整体架构
由于车辆存在最小转向半径，对于给定的 r min 和 f i ，存在 d （ o ， f i ） min ，若 d （ o ， f i ）< d （ o ， f i ） min ，则不能如图4所示生成期望的Dubins曲线。在本文中，除 f i = t 外，此类 f i 被舍弃。如图5所示，在车辆坐标系的第一象限中，当 φ f ∈（0，π/2）时，设过点 o 和 f i 的直线为 y = kx ，那么 d （ o ， f i ） min 等价于点 p 1 到直线y=-1/ kx 的距离；当 φ f =π/2时， d （ o ， f i ） min 等价于点 p 2 到 x 轴的距离。当 φ f ∈[-π/2，0）时，与上述情况类似。通过联立解析几何方程，可求得 d （ o ， f i ） min 的取值如式（7）所示。
图4 不同生成路径方法对比
式中， k =tan φ f 。
在生成路径时，若 f i = t 或 d （ o ， f i ）≥ d （ o ， f i ） min ，则首先生成连接 q o 和 f i 的Dubins曲线，若其通过了碰撞检测且 d （ o ， f i ）＞ l p ，则用预瞄算法迭代生成路径，预瞄距离和迭代车速 v p 分别由式（8）和式（9）确定。设迭代路径在 f i 处的位置误差和航向误差分别为Δ x 、Δ y 和Δ φ ，若误差满足式（10）且迭代路径通过了碰撞检测，则更新参考路径。当 T ≠ θ 时，优先生成满足目标航向要求的Dubins曲线作为参考路径，若失败，则尝试按相对方位角生成参考路径。
图5 生成期望Dubins曲线的距离限制
式中， l pmin 为最小预瞄距离（m）； k p 为车速比例系数。
式中， v pmin 为最小迭代车速（m/s）。
式中， ε 为迭代路径允许误差。
当无人车辆在未知非结构化环境中行驶时，其车速一般不高。设期望的车速区间为[ v min ， v max ]，本文根据规划路径的曲率生成对应的期望车速 v ref ：
式中， κ 为规划路径的曲率（m -1 ）， κ max =1/ r min 。
本文的决策规划算法基于TangentBug算法 [22] ，在保留其完备性优点的同时，对安全性，以及对定位和测距传感器误差的鲁棒性等进行了改进。其中，车辆的状态分为“朝目标移动”和“环绕障碍物”两种，在面对“凸障碍物”和“凹障碍物”时的情况分别如图6和图7所示。
图6 面对凸障碍物的情况
在图6中，车辆从 q s 处开始朝目标移动状态前进，在点 H i 处遇到凸障碍物 O i ，然后开始沿其边界前进，在点 D i 处离开。
图7 面对凹障碍物的情况
在图7中，车辆在点 H j 处遇到凹障碍物 O j ，并在之后检测到局部最近点 M j 。在车辆开始远离目标点 q t 一定距离后，在点 SW j 处转换为环绕障碍物状态，并根据此时目标点的方位开始逆时针环绕障碍物。在点 L j 处，由于存在点 L R j ，使得 d （ L R j ， q t ）< d （ M j ， q t ）且到点 L R j 生成的路径通过了碰撞检测，则车辆转换为朝目标移动状态。最后按目标点的航向要求在 q t 处停止。
本文决策规划算法的伪代码在表1、表2和表3列出。其中，集合 F 中元素的代价 c f i 为车辆到该元素的距离加该元素到目标点的距离：
当车辆处于朝目标移动状态时，其前进方向为当前式（12）值最小的安全方向。故在表1中对车辆与目标点间的距离 d t 更新时，除成功生成连接目标点的轨迹外，保留了当前车辆与目标点间距离 d t 的最小值。配合表2中的状态转换距离阈值 d m ，可防止车辆的状态因定位误差而改变。
在本文中，非结构化环境中的障碍物按其拓扑结构被分为“凸障碍物”和“凹障碍物”两种。在无人车辆仅靠局部环境信息进行运动规划的过程中，无人车辆与目标点间的距离不会因避让凸障碍物而增加。而在避让凹障碍物时，无人车辆与目标点间的距离存在最小值。此时，若仅靠“朝目标移动”状态下的算法选取局部目标点，则会使无人车辆困在局部最近点处。因此需要“环绕障碍物”状态下的算法来避让凹障碍物，其通过改变局部目标点的选取方式，使无人车辆摆脱局部最近点。
表1 决策规划算法
（续）
表2 朝目标移动状态算法
表3 环绕障碍物状态算法
基于如图8所示的松灵SCOUT轮式机器人平台，对本文算法在室外非结构化园区环境中进行了验证。SCOUT采用了线控底盘，可通过CAN信号对其驱动电机进行控制。通过在SCOUT上加装传感器和上位机等设备，其可作为本文的无人车辆实验平台。其他实验设备包括：作为测距传感器的镭神16线激光雷达，并用于基于激光雷达的SLAM算法；装有Ubuntu 16.04和ROS Kinetic的上位机，用于运行由C++编写的算法；作为通信设备的CANet，用于连接上位机和SCOUT，将以太网与CAN信号进行相互转换。通过限制SCOUT左右两侧的轮速差，可获得与阿克曼转向车辆类似的最小转向半径。
图8 实验平台
在本文实验选取的室外非结构化园区环境中，障碍物主要包括可被视为凹障碍物的花坛和可被视为凸障碍物的行人和停放车辆等。本文算法关键参数的取值见表4。
表4 算法关键参数取值
实验中环境建模和轨迹规划的结果如图9所示。其中，红色箭头为车辆位姿；蓝色箭头为规划参考点集合中的元素；粉色曲线为规划出的轨迹；白色区域为可行区域；黑色区域为障碍物。
图9 实验中环境建模和轨迹规划的结果
本文算法结合基于激光雷达的SLAM建图过程如图10所示，其中白色区域为可行区域，黑色区域为障碍物或未知区域。
图10 SLAM建图过程
实验车辆在全局地图中的行驶轨迹和状态转换点如图11所示。其中，蓝色曲线为车辆行驶轨迹，表明实验车辆成功避开了未知非结构化环境中的障碍并按给定航向到达目标点。车辆以朝目标移动状态从 q s 出发，在点 SW 1 处转换为环绕障碍物状态，并检测到局部最近点 M 1 ，之后在点 L 1 处成功生成连接点 L R1 的轨迹，并转换为朝目标移动状态。在点 SW 1 和点 L 1 之间，车辆状态并未发生频繁转换，可表明本文算法对定位和测距传感器的误差有较好的鲁棒性。
图11 车辆行驶轨迹
在实验过程中规划路径的最大曲率与仅用Dubins曲线的对比如图12所示。通过将Dubins曲线与用模型预测生成的路径相结合，其最大曲率的标准差为0.2691，与仅用Dubins曲线的0.4676相比，提升了42.45%。此外，在实验过程中算法计算的最大时间为0.0098s，远小于算法固定的循环时间0.05s，可满足实时性的要求。
图12 规划路径最大曲率对比
本文以TangentBug算法为基础，依靠测距传感器提供的信息，提出了一种面向未知非结构化环境的无人车辆运动规划算法。通过在室外进行实车实验，验证了本文算法在现实环境中可为无人车辆实时规划出安全、平滑的参考轨迹，并对定位和测距传感器的误差有较好的鲁棒性。
本文通过结合SLAM算法，实现了实时定位与建图。在后续的研究工作中，可在本文算法生成的全局地图基础上，进行全局路径规划。
[1]余卓平，李奕姗，熊璐.无人车运动规划算法综述[J].同济大学学报（自然科学版），2017，45（8）：1150-1159.
[2]陈慧岩，熊光明，龚建伟，等.无人驾驶汽车概论[M].北京：北京理工大学出版社，2014.
[3]HART P E,NILSSON N J,RAPHAEL B.A formal basis for the heuristic determination of minimum cost paths[J].IEEE transactions on Systems Science and Cybernetics,1968,4(2):100-107.
[4]LAVALLE S M.Rapidly-exploring random trees:A new tool for path planning[J].1998.
[5]HEMMERLING A.Labyrinth problems:Labyrinth-searching abilities of automata[M].Berlin:Springer-Verlag,2013.
[6]KAMPHANS T,LANGETEPE E.The Pledge algorithm reconsidered under errors in sensors and motion[C]//International Workshop on Approximation and Online Algorithms.Springer,Berlin,Heidelberg,2003:165-178.
[7]BOUNINI F,GINGRAS D,POLLART H,et al.Modified artificial potential field method for online path planning applications[C]//2017 IEEE Intelligent Vehicles Symposium(IV).IEEE,2017:180-185.
[8]LEE D,JEONG J,KIM Y H,et al.An improved artificial potential field method with a new point of attractive force for a mobile robot[C]//2017 2nd International Conference on Robotics and Automation Engineering(ICRAE).IEEE,2017:63-67.
[9]LUMELSKY V,STEPANOV A.Dynamic path planning for a mobile automaton with limited information on the environment[J].IEEE transactions on Automatic control,1986,31(11):1058-1063.
[10]ZHU Y,ZHANG T,SONG J,et al.A new hybrid navigation algorithm for mobile robots in environments with incomplete knowledge[J].Knowledge-Based Systems,2012,27:302-313.
[11]NG J,BRÄUNL T.Performance comparison of bug navigation algorithms[J].Journal of Intelligent and Robotic Systems,2007,50(1):73-84.
[12]FRANCO C,
, LÓPEZ-NICOLÁS G,et al. Persistent coverage control for a team of agents with collision avoidance[J]. European Journal of Control, 2015, 22: 30-45.
[13]KOVÁCS B,SZAYER G,TAJTI F,et al.A novel potential field method for path planning of mobile robots by adapting animal motion attributes[J].Robotics and Autonomous Systems,2016,82:24-34.
[14]ZAKI O,DUNNIGAN M.A navigation strategy for an autonomous patrol vehicle based on multi-fusion planning algorithms and multi-paradigm representation schemes[J].Robotics and Autonomous Systems,2017,96:133-142.
[15]KIM D H,SHIN K,HAN C S,et al.Sensor-based navigation of a car-like robot based on Bug family algorithms[J].Proceedings of the Institution of Mechanical Engineers,Part C:Journal of Mechanical Engineering Science,2013,227(6):1224-1241.
[16]XU Q L,YU T,BAI J.The mobile robot path planning with motion constraints based on Bug algorithm[C]//2017 Chinese Automation Congress(CAC).IEEE,2017:2348-2352.
[17]MCGUIRE K N,DE CROON G,TUYLS K.A comparative study of bug algorithms for robot navigation[J].Robotics and Autonomous Systems,2019,121:103261.
[18]张旭东，徐福康，邹渊，等.融合TangentBug与Dubins曲线的智能轮式车辆局部路径规划算法[J].汽车工程，2020，in press.
[19]ZIEGLER J,BENDER P,DANG T,et al.Trajectory planning for bertha—a local,continuous method[C]//2014 IEEE intelligent vehicles symposium proceedings.IEEE,2014:450-457.
[20]郑壮壮.无人地面平台建图定位与地形分析[D].北京：北京理工大学，2020.
[21]SORNIOTTI A,BARBER P,DE PINTO S.Path tracking for automated driving:A tutorial on control system formulations and ongoing research[J].Automated driving,2017:71-140.
[22]KAMON I,RIMON E,RIVLIN E.Tangentbug:A range-sensor-based navigation algorithm[J].The International Journal of Robotics Research,1998,17(9):934-953.