基于B样条曲线的无人车路径规划算法

屈盼让++李林++任晓琨++荆立雄

摘要：路径规划是无人车运动控制的重要研究内容之一，路径规划算法的性能优劣直接影响无人车能否在复杂交通场景中自主行驶。考虑到车辆运动学模型对轨迹平滑性的要求，本文提出了基于B样条曲线解决无人车路径规划问题的方法，建立了完整的分步规划模型，即将路径规划的过程分为路径簇生成、最大曲率约束、碰撞检测以及最优评价四个步骤串行进行，最终得到最优路径。该方法实现过程简单，运行效率较高，仿真结果表明了本文算法的有效性。

关键词： B样条曲线； 无人车； 路径规划； 碰撞检测； 最大曲率约束； 最优路径

中图分类号：TP181 文献标识码：A 文章编号：1009-3044（2016）26-0235-03

B-spline Curve based Trajectory Planning for Autonomous Vehicles

QU Pan-rang，LI Lin ， REN Xiao-kun ，JING Li-xiong

（Institute of Aeronautics Computing Technique Research， Xian 710065， China）

Abstract：Path planning is an important research topic in the field of the unmanned vehicle motion planning， and it directly affects the performance of unmanned vehicles in a complex traffic environment. Taking the requirement for smoothness into account， this paper proposed a method based on B-spline curve and built a planning model which can be divided into four steps， including path clusters， constraint of maximal curvature， collision detection and optimal path. This method works efficiently and simulation results show efficiency of the method.

Key words：B-spline curve； autonomous vehicle； path planning； collide detection； constraint of maximal curvature

1 引言

近年来，无人驾驶技术备受关注，各大研究机构和企业争相推出各自的无人驾驶平台。无人车作为未来智能交通的主要主体也逐渐融入到我们的日常生活中，比如自主巡航[1]和自动泊车等等。然而，为了使其更好地服务于我们，需要进一步提高其智能化水平，而路径规划作为连接环境感知和运动规划的桥梁，是无人车智能化水平的重要体现[2]。

由于受到自身动力学和运动学模型的约束，车辆的路径规划问题除过要严格满足端点状态约束之外，还要求其中间状态满足运动系统的微分约束。由于实现简单，并且高阶多项式曲线能够很好地满足运动系统的微分约束，生成高阶平滑的路径，所以很多路径规划系统选择使用基于多项式曲线的方法生成路径。B样条曲线是一种典型的多项式曲线，且因为其所有的中间状态均是由控制点加权生成，所以其能够完全满足端点状态约束。综合考虑无人车路径规划的要求和实现复杂度，在仅已知初始位姿和目标位姿的情况下，本文选择B样条曲线生成路径，重点讲述分步规划模型，即路径簇生成、最大曲率约束、碰撞检测以及最优评价四个过程，并通过Matlab仿真对本文方法进行了验证。

2 问题描述

本节分别描述了无人车路径规划问题和B样条曲线。

2.1 路径规划问题描述

路径规划得到的是一条从初始位置到目标位置的路径，即二维平面内一条从初始位置点到目标位置点的曲线，曲线上的每一个点表示车在行驶过程中的一个状态。考虑到实现方便，本文将路径描述成离散点序列[Sstart，S1，???，Sn，Sgoal]，如图1所示，序列中每一个点[Si（xi，yi，θi）]表示车的一个状态，其中[（xi，yi）]表示此时刻车辆的位置，[θi]表示车辆的航向，[Sstart]和[Sgoal]分别表示车辆的初始状态和目标状态。图1中的圆[（xobs1，yobs1，robs1）]表示环境中的障碍物，[（xobs1，yobs1）]表示障碍物的位置信息，[robs1]表示障碍物的半径。

2.2 B样条曲线

如果给定[m+n+1]个控制点[Pi（i=0，1，???，m+n）]，就可以构造[m+1]段[n]次B样条曲线，其可以表示为公式1：

[Pi，n（t）=k=0nPi+k?Fk，n（t） ，t∈[0，1]Fk，n（t）=1n！j=0n-k（-1）j?Cjn+1?（t+n-k-j）n ， t∈[0，1] ， k∈0，1，???，n] （1）

其中，[Pi，n（t）]表示第[i]个[n]次B样条曲线片段，[n]表示B样条曲线的次数，[t]为控制参数，其取值范围为[[0，1]]，[Pi+k]为控制点，[Fk，n（t）]为B样条基。依次连接全部[n]阶B样条曲线段就组成了整条B样条曲线。

3 本文算法

本节重点讲述基于B样条曲线的路径规划方法和基于该方法生成路径的过程。

3.1 基于B样条曲线的路径规划方法

选择三阶B样条曲线生成几何路径，即每四个控制点生成一个路径片段，然后通过片段的拼接就可以实现从初始状态到目标状态的路径规划，下面着重讲述基于六控制点的三阶B样条曲线生成满足车辆端点位姿约束路径的方法，如图2所示。

l 依据初始状态选择控制点[P0，P1，P2]。当[P0，P1，P2]三个控制点共线，并且[P1]为线段[P0P2]的中点时，生成的B样条曲线与线段[P0P2]相切于点[P1]。所以选择无人车的初始位置为控制点[P1]，将控制点[P0]和[P2]选在初始航向角[θstart]所在直线上，并关于控制点[P1]对称。如是，即能满足车辆的初始位姿约束；

l 依据目标状态选择控制点[P3，P4，P5]。当[P3，P4，P5]三个控制点共线，并且[P4]为线段[P3P5]的中点时，生成的B样条曲线与线段[P3P5]相切与点[P4]。所以选择无人车的目标位置为控制点[P3]，将控制点[P3]和[P5]选在目标航向角[θgoal]所在的直线上，并关于控制点[P3]对称。如是，即能满足车辆的目标位姿约束。

（a） 路径簇

（b） 最大曲率约束

（c） 碰撞检测

3.2 分步路径规划

本小节将以图3所给定的场景为例，讲述最优路径生成的过程。

3.2.1 路径簇生成

在选定控制点[P1]和[P4]之后，通过选择不同的控制点[P2]和[P3]，从而得到多组控制点，进而得到多条路径。将控制点选择的极限定为线段[P1P2]、[P3P4]与[P1P4]相等，但是[P1P2]和[P3P4]不能出现交叉。将这个范围等间隔量化，各取四个点，共组成16组控制点，得到16条路径，如图3（a）中的蓝色曲线所示。

3.2.3 最大曲率约束

理论上，车辆的最小转弯半径[Rmin=Lsin（θmax）]是其本身属性[3]，只取决于车辆的轴距[L]和最大前轮转角[θmax]。那么，车辆可行驶路径的最大曲率[κmax=1Rmin]也是固定的，假设无人车可行驶路径的最大曲率[κmax=16]，以此为约束条件，在路径簇中选择满足最大曲率约束的路径，如图3（b）所示，图中绿色曲线表示不满足最大曲率约束的路径。

3.2.4 碰撞检测

碰撞检测的目的是保证车辆在规划路径上行驶而不与障碍物发生碰撞。采取的碰撞检测的方法很简单，因为环境中的障碍物采用圆来描述，所以只要判断路径上每一点到圆心的距离与障碍物半径的关系，就能确定其是否发生碰撞。由两点间距离公式：

[d=（x1-x2）2+（y1-y2）2] （2）

如果[d]大于障碍物半径，则不发生碰撞；如果[d]小于障碍物半径，则发生碰撞。图3（c）中的蓝色曲线表示既满足最大曲率约束，又不与障碍物碰撞的路径。

3.2.2 最优路径

路径要求的侧重点不同，优化的目标函数也可以有多种选择，常用的目标函数有最短和最平滑等。其中，路径最短可以抽象成优化问题：

[traoptimal=arg mintraids] （3）

路径最平滑可以抽象成优化问题：

[traoptimal=arg mintraiκ2] （4）

式中，[traoptimal]为最优路径，[traids]为第[i]条路径的长度，[traiκ2]为第[i]条路径上所有点处的曲率平方之和。图3（d）中的红色曲线即为得到的最短可行驶路径。

如是，就能得到满足车辆运动学约束，并且无碰撞的最优路径。

4 结论

本文选择使用B样条曲线解决无人车路径规划问题，并建立了基于B样条曲线的分步规划模型。仿真结果表明，使用基于B样条曲线的路径规划方法能够很好地解决简单障碍物场景中无人车的路径规划问题，并且因为路径生成过程简单，所以该方法常常表现得十分高效，能够完全满足无人车路径规划系统对算法实时性的要求。

参考文献：

[1] Vahidi A， Eskandarian A. Research advances in intelligent collision avoidance and adaptive cruise control [J]. IEEE Transactions on Intelligent Transportation Systems， 2003， 4（3）：143-153.

[2] Siegwart R， Nourbakhsh I R， Scaramuzza D. Introduction to autonomous mobile robots [M]. US： MIT press， 2011.

[3] Snider J M. Automatic steering methods for autonomous automobile path tracking [J]. Robotics Institute， Pittsburgh， 2009.