基于贝塞尔曲线的无人车局部避障应用

高嵩 张金炜 戎辉 王文扬 郭蓬 何佳

摘  要： 文中将基于贝塞尔曲线的路径规划方法应用在无人车局部避障中。首先通过最小化曲线的最大曲率与最小曲率之间的差值确定贝塞尔曲线所需的五个控制点，根据五个控制点确定贝塞尔曲线，再结合实验无人车和道路的实际情况从若干备选曲线中选择最优路线。实验仿真结果证明，该方法能够达到无人车局部避障的要求，能满足包括初始状态约束、目标状态约束、曲率连续约束等行驶条件。

关键词： 路径规划; 局部避障; 无人车; 四阶贝塞尔曲线; 控制点; 状态约束

中图分类号： TN99?34                          文献标识码： A                           文章编号： 1004?373X（2019）09?0163?04

Application of unmanned vehicle local obstacle avoidance method based on Bezier curve

GAO Song1， 2， ZHANG Jinwei3， RONG Hui1， 3， WANG Wenyang1， GUO Peng1， 2， HE Jia1

（1. Automotive Engineering Research Institute， China Automotive Technology & Research Center Co.， Ltd.， Tianjin 300300， China;

2. Tianjin University， Tianjin 300072， China; 3. Hebei University of Technology， Tianjin 300132， China）

Abstract： The path planning method based on quartic Bezier curve is applied to the local obstacle avoidance of unmanned vehicles. The difference between the maximum curvature and minimum curvature of the minimum curve is used to determine the five control points required by Bezier curve. The Bezier curve is determined according to the five control points， and the optimal path is selected from some alternative curves by combining the experimental unmanned vehicle with actual road situation. The experimental results show that the proposed method can meet the requirements of unmanned vehicles for local obstacle avoidance， and satisfy the driving conditions of initial state constraint， target state constraint and curvature continuous constraint.

Keywords： path planning; local obstacle avoidance; unmanned vehicle; quartic Bezier curve; control point; state constraint

0  引  言

無人车的软件层次一般由感知层、决策层和控制层三层组成[1]：感知层包括图像识别、激光雷达与毫米波雷达数据处理、卫星差分与惯性导航定位等算法;决策层包括全局路径规划和局部路径规划等内容;控制层则主要涉及通过模型预测控制、PID、滑模变理论等对车辆的转向、刹车、加速等底层执行单元进行控制。本文聚焦于无人车的局部路径规划，探讨无人车在道路行驶过程中遇到障碍物时如何规划出一条合理的曲线进行避障行驶的问题。

无人车的局部路径规划吸引了国内外的研究者进行了广泛的研究，现有的方法可以分为五大类，分别是：传统路径规划算法（模拟退火法、人工势场法等）[2]、启发式搜索算法（Dijkstra算法、A*算法及其变种等）[3]、离散优化算法（模型预测算法、几何轨线算法等）[4]、随机采样算法（随机路图法、快速随机拓展树法等）[5?6]和智能仿生算法（遗传算法、蚁群算法、神经网络等）[7]。几何轨线法中的贝塞尔曲线方法是法国工程师Bezier在1962年为了设计汽车车身形状提出的[8]，之后贝塞尔曲线由于具有良好的数学特性而被广泛应用到车辆路径规划领域[9?10]。本文研究的是将四阶贝塞尔曲线应用在无人车局部避障中的问题。

首先结合无人车的动力学属性，设计局部避障应该满足的初始状态约束、目标状态约束、曲率约束等条件，然后通过SQP最优化算法[11]求出贝塞尔曲线的五个控制点，确定若干备选曲线，最后结合实际道路工况选出最优行驶路线。Matlab仿真实验证明了本文方法的有效性，可用于实验车的局部路径规划。

1  无人车路径规划目标

无人车须实现从初始点至目标点的行驶，而初始点和目标点的状态则是贝塞尔曲线构建的形成条件，初始点的状态包括二维空间坐标[（x，y）]，航向角[ψ]，曲率[κ]，如图1所示。

曲率[κ]、转弯半径[r]、轴距[L]以及前轮转角之间的关系如下所示：

无人车的轨迹可以看作是由初始状态到目标状态的一组动作组成[11]，设初始状态[XI=xI，yI，ψI，κI]和目标状态[XT=xT，yT，ψT，κT]。

2  基于贝塞尔曲线的路径规划

2.1  贝塞尔曲线性质

本节仅介绍贝塞尔曲线的性质，详细的贝塞尔曲线介绍可参考文献[12]。

如图2所示，五个控制点可以唯一确定平面内一条四阶贝塞尔曲线，曲线具有仿射变换不变的特性，其参数化表达式如下：

图1  无人车转向模型

图2  四阶贝塞尔曲线示例图

根据以上参数表达式可知：曲线经过第一个[P0（u=0）]及第五个控制点[P4（u=1）];根据求导可得曲线在[P0]和[P4]处的切向量分别为[P（0）=4（P1-P0）]和[P（1）=4（P4-P3）]。曲线上任意一点的曲率公式为：

2.2  控制点表示

由于五个控制点即可确定贝塞尔曲线，因而可以根据无人车路径规划的要求先求解五个控制点的平面坐标[13]，再对贝塞尔曲线进行参数化表示，求解步骤如下：

1） 由于贝塞尔曲线具有仿射变换不变的特性，任意旋转或平移该曲线都不会影响它的形状，将曲线放在车辆坐标系下，车辆坐标系[（0，0）]为初始位置[P0]，初始位置状态为[XI=0，0，π2，κI]。

2） 假设[P0]与[P1]的距离为[d1]，为了满足初始航向角[π2]的约束，可求得[P1]的坐标为[（0，d1）]。

3） 结合式（4）初始点曲率计算公式，为了满足控制点[P0]的曲率约束，设[P2=（x2，y2）]，可得：

以上计算可以满足无人车路径规划初始位置和目标位置的约束条件，得到五个控制点的坐标。在这五个控制点中只有三个自由变量，分别是[d1，d4，y2]，也就是说，通过这三个自由变量即可唯一确定一条四阶贝塞尔曲线。

2.3  曲率约束条件

为了满足无人车运动约束条件，还须加上曲率约束。根据式（3）可知，当且仅当[xu=yu=0]时，曲率不连续，但是若满足上述条件，贝塞尔曲线将缩成一个点，因此四阶贝塞尔曲线曲率处处连续。

另外，假设实验无人车的最小转弯半径为[R]，可以得到最大曲率[κMax=1R]，最小曲率[κMin=0]。所以，曲率上下有界约束条件可以表示为：[κMin≤κ（u）≤κMax]。

2.4  轨迹优化求解

本文采用最优化的方法优化三个参数[d1，d4，y2]，使得曲线的最大曲率与最小曲率之间的差值最小。优化目标函数为：

式中：[μ1，μ2∈0，1];目标函数中[p]代表参数[d1，d4，y2]。另外，还须满足实验无人车的最大曲率约束式（6）。曲率最大和最小值可通过曲率式（3）求取极值获得。本文采用的最优化算法为序列二次优化（SQP）[14]，迭代次数为50次。

3  仿真实验与验证

3.1  无人车情况

图3所示为作者所在单位开发的无人观光车——“小黄车”，外形尺寸为4 330 mm×1 510 mm×2 030 mm（长×宽×高），轴距为2 000 mm，轮距为1 200 mm，最小转弯半径为7 000 mm。车顶上具有GPS+惯导，负责采集无人车的位置和航向角等信息，车头安装有16线激光雷达和双目摄像头，负责采集行驶区域内的路况和障碍物信息，后备箱安装有工控机，负责处理感知信息，进行轨迹规划以及决策控制。

图3  中国汽车技术研究中心研发的无人观光车——“小黄车”

3.2  建立仿真道路

无人车在行进中需要实时采集道路信息，由这些道路信息构建环境地图，而环境地图是进行局部路径规划的前提[15]。无人车通过车前的16线激光雷达感知环境信息，将世界坐标系下的障礙物转换到车辆质心坐标系，采用栅格法[16]在二维空间里建立周边环境模型。

图4为实验无人车避障实验道路，该道路位于中国汽车技术研究中心天津市东丽区的主院区内，道路宽为7 m，长为120 m。

图4  拟仿真的真实道路

本文基于以上道路进行仿真实验，将道路范围划分为30×30的栅格，每个栅格的长和宽都设为0.5 m，中间白色地带为7 m长道路，两侧黑色为草地，在道路上设置若干障碍物，并将其进行膨胀处理，不能占满一个栅格要按照占满栅格处理。图5和图6为两种仿真道路上障碍物的分布情况。

3.3  仿真实验结果

在Matlab仿真中，以无人车当前所在的位置为原点建立平面[X?Y]坐标系，通过计算两段贝塞尔曲线实现局部避障。第一段贝塞尔曲线的初始状态为[XI=0，0，π2，0]，第二段贝塞尔曲线的初始状态设定为第一段曲线的目标状态，并将曲率设为0。

图5  避障仿真实验（一）

图6  避障仿真实验（二）

实验中，根据虚拟障碍物的平面坐标，在[X]方向以1 m为间隔设置8个目标点（涵盖障碍物位置），并在每个目标点取转向角[ψT]分别为60°，90°和120°，因此两次路径规划可分别得到24条贝塞尔曲线。实际应用中通常不在目标状态加曲率约束，所以本文未在目标状态设置曲率约束。

考虑到无人车的横向宽度，局部避障规划中最短路径不一定就是最优路径，按照以下方式从计算得到的多条贝塞尔曲线中选取最优路径：由于本文无人车的宽度为1.4 m，因此选取1 m作为安全距离，首先要求待选路线上的每个点（代表无人车中心轴）与周围障碍物的距离大于1 m，然后再从安全距离筛选出来的路径中选择出最短路径，计算每个路线长度，选出最短路线。

两种道路情况下的仿真结果如图5和图6所示（直线为原来的直线路径，虚线为计算得到的贝塞尔曲线，实曲线为选取的避障路径）。从仿真结果来看，本文使用的贝塞尔曲线路径规划方法可成功解决无人车在指定道路上的局部路径规划问题。

4  结  语

本文将贝塞尔曲线的方法应用到无人车的局部路径规划中，结合无人车和道路场景的具体数据进行仿真实验。实验结果证明了本文方法的有效性，规划出的行驶曲线可满足无人车初始状态约束、目标状态约束和曲率连续约束等条件，使得无人车能够按照轨迹平滑行驶。未来会尽快将本文提出的方法运用到车上，进行实车验证。

参考文献

[1] 王世峰，戴祥，徐宁，等.无人驾驶汽车环境感知技术综述[J].长春理工大学学报（自然科学版），2017，40（1）：1?6.

WANG Shifeng， DAI Xiang， XU Ning， et al. Overview on environment perception technology for unmanned ground vehicle [J]. Journal of Changchun University of Science and Technology （Natural Science）， 2017， 40（1）： 1?6.

[2] 鲍庆勇，李舜酩，沈峘，等.自主移动机器人局部路径规划综述[J].传感器与微系统，2009，28（9）：1?4.

BAO Qingyong， LI Shunming， SHEN Huan， et al. Survey of local path planning of autonomous mobile robot [J]. Transducer and microsystem technologies， 2009， 28（9）： 1?4.

[3] ZHANG L， SUN L， ZHANG S， et al. Trajectory planning for an indoor mobile robot using quintic Bezier curves [C]// 2015 IEEE International Conference on Robotics and Biomimetics. Zhuhai： IEEE， 2015： 757?762.

[4] CHU K， LEE M， SUNWOO M. Local path planning for off?road autonomous driving with avoidance of static obstacles [J]. IEEE transactions on intelligent transportation systems， 2012， 13（4）： 1599?1616.

[5] BARRAQUAND J， LATOMBE J C. A Monte?Carlo algorithm for path planning with many degrees of freedom [C]// 1990 IEEE International Conference on Robotics and Automation. Cincinnati： IEEE， 1990： 1712?1717.

[6] KAVRAKI L， LATOMBE J C. Randomized preprocessing of configuration for fast path planning [C]// 1994 IEEE Interna?tional Conference on Robotics and Automation. San Diego： IEEE， 1994： 2138?2145.

[7] 陈刚，沈林成.复杂环境下路径规划问题的遗传路径规划方法[J].机器人，2001，23（1）：40?44.

CHEN Gang， SHEN Lincheng. Genetic path planning algorithm for complex environment path planning [J]. Robot， 2001， 23（1）： 40?44.

[8] FUNKE J， THEODOSIS P， HINDIYEH R， et al. Up to the limits： autonomous Audi TTS [C]// 2012 IEEE Intelligent Vehicles Symposium. Alcala de Henares： IEEE， 2012： 541?547.

[9] JOLLY K G， KUMAR R S， VIJAYAKUMAR R. A Bezier curve based path planning in a multi?agent robot soccer system without violating the acceleration limits [J]. Robotics & autonomous systems， 2009， 57（1）： 23?33.

[10] CHOI J W， CURRY R E， ELKAIM G H. Curvature?continuous trajectory generation with corridor constraint for autonomous ground vehicles [C]// IEEE the 49th Conference on Decision and Control. Atlanta： IEEE， 2011： 7166?7171.

[11] 陈成，何玉庆，卜春光，等.基于四阶贝塞尔曲线的无人车可行轨迹规划[J].自动化学报，2015，41（3）：486?496.

CHEN Cheng， HE Yuqing， BU Chunguang， et al. Feasible trajectory generation for autonomous vehicles based on quartic Bezier curve [J]. Acta automatica Sinica， 2015， 41（3）： 486?496.

[12] DUNCAN M. Applied geometry for computer graphics and CAD [M]. Berlin： Springer， 2005.

[13] 余伶俐，龙子威，周开军.基于贝塞尔曲线的机器人非时间轨迹跟踪方法[J].仪器仪表学报，2016，37（7）：1564?1572.

YU Lingli， LONG Ziwei， ZHOU Kaijun. Non?time trajectory tracking method based on Bezier curve for robot [J]. Chinese journal of scientific instrument， 2016， 37（7）： 1564?1572.

[14] 李振浩.基于序列二次规划：免疫记忆鱼群算法的局放超声定位研究[D].昆明：昆明理工大学，2017.

LI Zhenhao. Partial discharge ultrasound localization research based on sequence quadratic programming & immune memory fish swarm algorithm （SQP?IMAFSA） [D]. Kunming： Kunming University of Science and Technology， 2017.

[15] 胡玉文.城市环境中基于混合地图的智能车辆定位方法研究[D].北京：北京理工大学，2014.

HU Yuwen. Hybrid map based localization method for unmanned ground vehicle in urban scenario [D]. Beijing： Beijing Institute of Technology， 2014.

[16] CHE H， WU Z， KANG R， et al. Global path planning for explosion?proof robot based on improved ant colony optimization [C]// 2016 Asia?Pacific Conference Intelligent Robot Systems. Tokyo： IEEE， 2016： 36?40.