未知环境下智能汽车轨迹规划滚动窗口优化

张 琳，章新杰，郭孔辉，王 超，刘 洋，刘 涛

(1.吉林大学 汽车仿真与控制国家重点实验室，长春 130022；2.长春孔辉汽车科技股份有限公司, 长春130012；3. 中国第一汽车集团公司 技术中心，长春 130011)

0 引 言

轨迹规划是实现汽车智能化的关键技术之一，其主要任务是在有障碍物的行驶环境中找到一条从初始点到目标点的安全轨迹，并满足所有约束条件。智能汽车的轨迹规划问题具有如下特点：①不确定性：行车过程中，车辆只能通过传感器或车载雷达在有限的探测区域中获取局部信息，在探测未知环境时，障碍物的突然出现会增加轨迹规划的难度。②多约束：智能汽车的轨迹规划不仅要考虑车辆运动存在的几何约束，还需要考虑非完整性约束以及车辆的动力学约束对轨迹规划的影响。除此之外，道路边界、交通规则以及驾驶员的驾驶习惯等也对轨迹规划提出了更高的要求[1]。

针对多约束条件下智能汽车在未知环境中的轨迹规划问题，传统方法是将应用在机器人领域的轨迹规划算法用于生成车辆轨迹，例如人工势场法、模拟退火算法等[2-4]，这类方法具有结构简单、计算量较小的优点，但算法难以处理动态变化的驾驶环境且容易陷入局部最优解，限制了轨迹规划在智能汽车中的应用。为克服传统方法的缺点，文献[5,6]提出了基于学习和人工智能理论的规划方法，例如神经网络，遗传算法等，车辆在行进中能够不断融合、分析探测到的信息，逐步建立起全局环境模型并进行一定程度的全局优化，但是这种方法信息冗余较多、计算量大，在实际应用中不能满足实时性的要求。文献[7-9]对上述方法进行了改进，如A*, D*等，这类方法通过一个估价函数来估计当前点到终点的距离，并由此决定搜索方向，可以根据车辆在移动中探测到的环境信息快速修正和规划出最优路径，具有一定的复杂环境自适应能力，但是没有考虑车辆非完整约束的限制，影响了智能汽车的轨迹跟踪效果。文献[10,11]提出的快速搜索随机树(Rapidly exploring random tree, RRT)，采用树结构解决复杂约束下的轨迹规划问题，由于搜索速度快且适用于包含运动学和动力学约束的规划，使得该算法在机器人领域得到了广泛应用。但是RRT采用随机采样策略，导致生成的轨迹重复性不强，算法的随机性还会导致生成的轨迹出现明显的拐角或者绕远，无法被车辆直接执行。

为解决智能汽车在未知环境中生成全局最优轨迹的问题，本文提出了一种基于滚动窗口优化的轨迹规划方法。该方法在建立车辆模型和环境模型的基础上，通过6次多项式生成平滑轨迹，利用滚动窗口周期性地刷新窗口内的环境信息，并根据车辆当前状态对轨迹进行在线优化。最后仿真验证了本文算法的有效性。

1 问题描述及分析

汽车的工作环境如图1所示，在二维平面中(x0,y0)、(xf,yf)分别为车辆的起点位置和目标位置；i1,i2,…,in为分布在车辆周围的静态障碍物；Rs为车辆行驶过程中的探测半径；Rs范围内的环境为探测到的已知环境，Rs范围外的环境为未知环境。为了简化计算，采用圆形对行驶车辆及障碍物进行包络。因此，可以将全局环境未知情况下的轨迹规划问题定义为：已知车辆起始位姿、目标运动位姿以及探测到的环境信息，根据所给定的地图规划出一条满足各项约束的最优行驶轨迹。

图1 智能汽车工作环境描述Fig.1 Intelligent vehicle working environment description

1.1 路径表示

图2 车辆运动学模型Fig.2 Vehicle kinematic model

为了便于描述，建立了车辆运动学模型，如图2所示，模型可表示为：

(1)

式中：(x,y)为车辆后轴中点坐标；φ为前轮转角；θ为车身方向角；l为前后轴距；u1为车速；u2为前轮转角速度。

由式(1)可进一步获取状态量之间的关系表达式：

(2)

结合式(1)(2)，并选取(x,y,θ,φ)为车辆的位姿信息，求解路径曲线的边界条件可表示为：

⟺

(3)

式中：x0、xf分别为惯性坐标系下路径起始点和目标点的横坐标值。

考虑车辆在轨迹过程中的平稳性以及参数的求解效率，采用具有连续曲率的多项式轨迹模型，将路径曲线表示为路径上任一点的纵坐标y关于横坐标x的六次多项式函数：

y=a0+a1x+a2x2+a3x3+

a4x4+a5x5+a6x6

(4)

x0≤x≤xf

式中：a=[a0,a1,…,a6]为路径曲线参数向量。

在每一时刻，若以车辆当前位置为初始位置进行轨迹规划，联立式(1)～(4)可解得a0～a5，其表达式为：

[a0a1a2a3a4a5]T=

(Bk)-1(Yk-Aka6)

(5)

(6)

(7)

(8)

式中：xk为车辆当前位置的横坐标值，初始状态下xk=x0。

y=[1xx2x3x4x5x6]

(9)

1.2 轨迹规划的约束条件

汽车轨迹规划的两类最重要的约束为路径可行性约束和安全性约束[12]，可具体分为几何约束和运动学约束、道路边界约束以及车辆动力学约束。

1.2.1 几何约束和运动学约束

为保证车辆安全行驶，在行驶过程中需要与障碍物之间有一定的安全避障距离，以静态障碍物为研究对象，避障约束示意图如图3所示。

图3 避障约束示意图Fig.3 Schematic of obstacle constraint

图3中，分别用半径为ri、r0的圆包裹行驶车辆和障碍物。(xi,yi)为包裹障碍物圆的圆心坐标；Xa、Xb分别为障碍物左边界和右边界横坐标；(xp,yp)为路径上任意一点坐标；dis为(xi,yi)到(xp,yp)的距离，若要使车辆避开障碍物，dis需满足式(10)：

dis≥(ri+r0)⟹

(yp-yi)2+(xp-xi)2≥(ri+r0)2

(10)

Xa≤xp≤Xb

联立式(9)(10)可得到利用多项式描述路径的避障约束数学表达式：

(11)

式中：

h0,i(x)=[g(x)(Bk)-1Yk-yi]2+

(x-xi)2-(ri+r0)2；

h1,i(x)=2[x6-g(x)(Bk)-1Ak]×

[g(x)(Bk)-1Yk+yi]2；

h2(x)=[x6-g(x)(Bk)-1Ak]；

g(x)=[1xx2x3x4x5]。

(12)

(13)

1.2.2 道路边界约束

在真实的行车环境中，需要对道路边界进行识别并考虑道路边界对轨迹规划的影响。为了减小计算量，假设道路边界为矩形，将道路边界也视为一种障碍物，即保证车辆在行驶过程中不触碰到道路边界。用直径为矩形最小边长的圆对道路边界进行包络，并且将道路边界视为障碍物，因此道路边界约束被化简为几何约束。对道路边界的处理如图 4所示。

图4 道路边界处理Fig 4 Road boundary processing

1.2.3 车辆动力学约束

智能汽车控制系统是一个受非完整性约束的非线性系统，因此车辆轨迹规划在满足避障约束的同时还要满足动力学约束要求。当车速一定时，行驶轨迹曲率与侧向加速度成正比，车辆在实际运动过程中产生的侧向加速度受到发动机、轮胎、地面附着系数等多方面限制，同时为了保证行驶过程中驾驶员乘坐的舒适性，需要对最大侧向加速度进行约束，侧向加速度约束如下所示：

(14)

式中：aymax为最大侧向加速度，m/s2。

受车辆转向系统限制，其前轮转向角变化范围有限且变化过程连续进行，由于任意时刻后轴中心点处的线速度始终垂直于后轴，因此转向角约束可以被转换成几何约束来处理，由几何关系易得：

tanφ=l/R

(15)

式中：R为转弯半径；l为前、后轴距。

因此，最大转向角约束以及转向角连续约束可以转化为对转弯半径的约束，其数学表达式为：

κmax=1/Rmin=tanφmax/l

(16)

(17)

式中：κ为轨迹曲率；Rmin为最小转弯半径；κmax为最大曲率；φmax为最大转向角。

1.3 目标函数

本文以路径最短作为性能指标，因此选取距离函数作为目标函数，通过求解目标函数在约束条件中的极值来确定点集的运动方程，最终使迭代路径点集趋于最优规划路径[13]：

(18)

式中：(xk,yk)为车辆当前位置的坐标。

2 基于滚动窗口优化的轨迹规划算法

当环境信息已知时，离线的全局轨迹规划可以使智能汽车在有障碍物的工作环境中寻找一条从给定起点到终点的可行路径，使其在运动过程中能安全、无碰撞地绕过所有障碍物。然而，在许多情况下车辆只能探测到局部环境信息，本文提出了一种的基于滚动窗口优化的轨迹规划算法。

2.1 滚动窗口优化原理

障碍物未知环境中，移动车辆只能通过传感器获取到静态的局部环境信息，充分利用这些信息便可以实现一个局部环境的轨迹规划，若干个这样的局部规划前后衔接就可以完成一项全局轨迹规划任务，其原理如图5所示。

图5 滚动窗口原理Fig.5 Rolling window principle

图6 目标点的选取Fig.6 Target point selection

将传感器可以探测到的区域定义为视觉窗口，车辆当前位置到目标位置的区域定义为规划窗口，如图5所示。规划窗口在每一步长下都向车辆行驶方向滚动，在滚动的每一步，根据探测到的局部信息，用启发式方法生成优化子目标，在当前滚动窗口内进行局部轨迹规划，随着滚动窗口的推进，不断取得新的环境信息，从而在滚动中实现优化与反馈的结合，故该算法能够解决全局环境未知时的轨迹规划问题。另外，规划窗口目标位置的选取会影响满足约束条件的算法的求解难度，如图6所示，P为车辆的当前位置，在道路边界约束下，f1f2为目标位置的选取范围，若以直线上任意一点A～E为目标位置，则规划出不同曲率的路径。若以B、C为目标位置，将不满足避障约束；若以A为目标位置，将不满足侧向加速度约束，通过判断确定PD曲线为满足多项约束条件的最短路径。

2.2 约束简化

智能汽车通过视觉系统检测到多个障碍物，若将规划窗口中的所有障碍物不做区分，并作为对轨迹的约束条件，将降低算法的效率。为克服这一缺点，本文引入了度量函数L(di)对障碍物进行分类。分类原则为：优先考虑距离车辆较近的障碍物对轨迹的安全约束，障碍物距离当前车辆越远，障碍物对轨迹的约束越弱。度量函数表达式如下所示：

(19)

度量函数L(di)表示当前时刻规划出的轨迹点与障碍物之间的安全距离。由于本文用圆对车辆和障碍物进行包络，属于较为保守的安全方案，因此当障碍物距离车辆较近(dmin≤di≤dmax)时，安全距离L(di)为0也能保证车辆与障碍物有一定距离；当车辆与障碍物较远(di≥dmax)时，安全距离L(di)为负值且随di线性变化(L(di)=k×di+b)，优点是在减弱较远障碍物对车辆约束的同时，保证了车辆与该障碍物的安全距离随着相对距离动态变化；当障碍物距离车辆足够远时，安全距离为Lmin且不发生改变，L(di)与di变化关系以及简化约束原理如图7所示。

图7 L(di)与di变化关系Fig.7 Relationship between L(di)and di

2.3 速度规划

通过上述规划方法可以得到满足动力学约束、初始状态及目标状态约束的路径，为了保证车辆安全行驶,需要规划车辆在该路径上的行驶速度，即在车辆不失稳的情况下尽快通过该路径。为保证车辆在任意时刻都可以安全停车，使每个时刻下的终止车速为0，主要分为以下3个步骤：

(1)只考虑前轮不侧滑的条件下，计算最大车速约束：

(20)

式中：μ、g分别为静摩擦因数以及重力加速度；l为轴距；κ为不同路径点对应的曲率；vmax为最大行驶车速。

(2)计算车辆的在路段下的行驶距离D,即规划出的路径点的总长度：

(21)

式中：pk为第k个路径点(xk,yk)，共有n个路径点。

(3)计算巡航车速vcoast，假设车辆加速度恒定，则满足下式：

(22)

式中：adecel为减速段的减速度；aaccel为加速度段的加速度；v0为当前车速；ε>0表示当前车速与最高车速限值vmax的接近程度，vmax-v0≥ε表示当前车速小于且远离最高车速，可以执行加速-巡航-减速过程；tmin为最短巡航时间；vmax-v0<ε表示当前车速接近最高车速，需要执行减速-巡航-减速过程。规划出的速度曲线如图8所示。

图8 车速曲线Fig.8 Speed profile

设车辆行驶该段路径的总时间为ttotal,在vmax范围内对式(22)中巡航车速vcoast进行遍历，可得到不同vcoast对应的总时间ttotal,选择ttotal最小值对应的巡航车速，由此确定满足约束的车速曲线。

2.4 轨迹规划算法框架

算法流程图如图9所示。算法分为主规划、滚动窗口和寻优3个模块。主规划模块通过对比车辆当前位置与终点位置坐标判断是否需要启动轨迹规划算法。若启动规划算法，在检测到障碍物的情况下根据车辆当前位姿信息以及环境信息进行轨迹寻优，并更新当前步长下的轨迹。滚动窗口模块有两个主要功能：一方面在车辆的行驶过程中更新窗口内的环境信息；另一方面，在规划窗口滚动后，获取当前车道满足约束的目标点的位姿信息。如果无法规划出满足各项约束的路径(例如无法避开障碍物)，需要紧急停车以保证车辆安全。

图9 算法流程图Fig.9 Algorithm flowchart

3 仿真验证

通过Simulink-CarSim软件搭建联合仿真平台，在CarSim中设置道路边界、障碍物、车辆动力学参数及约束条件，利用Sfunction(C语言)编写轨迹规划模块，并搭建最优预瞄驾驶员模型[15]。上述滚动窗口优化的轨迹规划问题是一个典型的非线性优化问题，目前对该问题的求解通常转化为非线性规划问题。本文采用Matlab中非线性规划求解器对此问题进行求解，模型原理如图10所示。

图10 轨迹规划算法模型搭建Fig.10 Model of trajectory planning

如图10所示，轨迹规划模块在线接收障碍物信息和当前车辆状态信息，通过滚动窗口优化算法得到最优可行轨迹，并结合设定的预瞄时间计算最优侧向加速度和纵向期望车速，驾驶员模型根据期望值决策出方向盘转角以及踏板开度并输入给CarSim车辆模型。

在蒙特卡洛赛道中验证算法的可行性，设置道路宽度为7 m，路面附着系数为0.7，只允许车辆在道路边界内行驶，故将道路边界当做障碍物处理，车辆起始状态和终止状态分别为(x0,y0,θ0,φ0)=(0，0，0，0)和(xf,yf,θf,φf)=(-90，0，0，0)，车辆主要参数如下所示：轴距为2340 mm；长度为3550 mm;宽度为1495 mm;最小转弯半径为4.5 m; 极限前轮转角为29.79°；最大纵向加速度为7 m/s2; 最高车速为85 km/h。

仿真采用微型车，车辆的对角线半长是1.92 m，包络车辆的圆半径设为2 m。用于检测障碍物传感器检测半径设为20 m，设aymax=4 m/s2时乘坐较为舒适[8]。基于滚动窗口优化的在线轨迹规划结果如图11所示。

图11 蒙特卡洛赛道规划结果Fig.11 Result of Monte Carlo circuit

如图11所示，轨迹规划算法能够生成满足车辆运动连续性要求的平滑轨迹，轨迹跟踪最大侧向误差为0.3 m;全程平均误差为0.12 m；最大车速跟随误差为1.5 m/s;全程平均误差为0.8 m/s，故在目标工况内轨迹跟踪效果较好。同时，车辆在与道路边界保证一定安全距离的情况下选择较近行驶路径，例如直角弯中靠近道路内边界行驶，典型情况如局部放大图(Ⅰ)所示；U形弯道中,为保证不降低车速的情况下顺利通过，则采用“切弯动作”，典型情况如局部放大图(Ⅱ)所示；在较短距离内通过多个曲率较大弯道时，能够考虑多个弯道的特点并规划出可执行轨迹，避免了传统规划方法的“振荡”现象，典型情况如局部放大图(Ⅲ)和(Ⅳ)所示。

从图12可以看出：在蒙特卡洛赛道中，前轮转角随时间变化较为平缓，在接近100和200 s时，车辆进入急转弯，轨迹曲率分别为0.08、0.1 m-1，前轮转角分别出现较大值-16.0428°、-14.8969°，但并未超过极限值29.7938°。

图12 前轮转角变化曲线Fig.12 Change curve of front wheel angle

图13 车速与轨迹曲率关系Fig.13 Relationship between speed and curvature

为保证车辆不发生侧滑，规划出的车速随轨迹曲率变化关系如图13所示。当曲率较小时，车速较高，最高可达22 m/s(75 km/h)，典型情况如图中轨迹长度为1200～1400 m段所示；当曲率由0增加到0.07 m-1时，车速迅速下降的合理范围为4.7～10.6 m/s(17～38 km/h)，典型情况如图中轨迹长度为1800～2400 m段所示；若轨迹曲率由较大值频繁变化，考虑到行车安全，车速先下降到较低值，随后根据曲率变化逐渐增加，典型情况如图中轨迹长度为1800～2300 m段所示。为进一步验证算法的避障功能，恒定车速下要求车辆躲避多个障碍物，试验结果如图14所示。

图14 多障碍物环境中车辆运动轨迹Fig.14 Vehicle trajectory in multi-obstacle environment

结果表明，本文方法在传感范围有限的情况下，能够规划出曲率连续的避碰轨迹且满足多个约束条件。滚动窗口的引入有效结合了优化和反馈机制，规划出的轨迹接近全局最优，保证了全局的收敛性。

4 结束语

参考文献：

[1] Ziegler J,Bender P,Dang T,et al. Trajectory planning for Bertha—a local, continuous method[C]∥Intelligent Vehicles Symposium Proceedings, Dearborn, MI, USA,2014:450-457.

[2] Turker T,Sahingoz O K,Yilmaz G. 2D path planning for UAVs in radar threatening environment using simulated annealing algorithm[C]∥2015 International Conference on Unmanned Aircraft Systems (ICUAS), Denver,CO,USA, 2015:56-61.

[3] Li G, Yamashita A, Asama H, et al. An efficient improved artificial potential field based regression search method for robot path planning[C]∥2012 IEEE International Conference on Mechatronics and Automation, Chengdu, China, 2012:1227-1232.

[4] Amiryan J,Jamzad M. Adaptive motion planning with artificial potential fields using a prior path[C]∥Rsi International Conference on Robotics and Mechatronics,Tehran,Iran,2015:731-736.

[5] Syed U A, Kunwar F, Iqbal M. Guided autowave pulse coupled neural network(GAPCNN) based real time path planning and an obstacle avoidance scheme for mobile robots[J]. Robotics and autonomous systems,2014,62(4): 474-486.

[6] Aamer N, Ramachandran S. Neural networks based adaptive approach for path planning and obstacle avoidance for autonomous mobile robot (AMR)[J]. International Journal of Research in Computer Applications and Robotics (IJRCAR),2015,3(12):66-79.

[7] Montemerlo M,Becker J,Bhat S,et al. Junior: the stanford entry in the urban challenge[J]. Journal of Field Robotics,2008,25(9):569-597.

[8] Dolgov D,Thrun S,Montemerlo M,et al. Path planning for autonomous vehicles in unknown semi-structured environments[J]. The International Journal of Robotics Research,2010,29(5):485-501.

[9] Yao J, Lin C,Xie X,et al. Path planning for virtual human motion using improved A*star algorithm[C]∥Seventh International Conference on Information Technology: New Generations,Las Vegas,NV,USA,2010:1154-1158.

[10] McNaughton M，Urmson C，Dolan J M，et al. Motion planning for autonomous driving with a conformal spatiotemporal lattice[C]∥2011 IEEE International Conference on Robotics and Automation (ICRA)，Shanghai,China,2011:4889-4895.

[11] Xanthidis M, Rekleitis I M,O′Kane J M. RRT+:fast planning for high-dimensional configuration spaces[J/OL]. [2017-04-02].https:∥www.researchgate.net/publication/312222541_RRT_Fast_Planning_for_High-Dimensional_Configuration_Spaces.

[12] González D,Pérez J,Milanés V,et al. A review of motion planning techniques for automated vehicles[J]. IEEE Transactions on Intelligent Transportation Systems,2016,17(4):1135-1145.

[13] Li Y，Shin B S. Internal Topology Based Flexible Shortest Path Planning Method for Indoor Navigation[M]. Berlin Heidelberg:Springer,2015:171-176.

[14] Yang J,Qu Z,Wang J,et al. Comparison of optimal solutions to real-time path planning for a mobile vehicle[J]. IEEE Transactions on Systems, Man, and Cybernetics—Part A: Systems and Humans,2010,40(4):721-731.

[15] 郭孔辉,马凤军,孔繁森. 人-车-路闭环系统驾驶员模型参数辨识[J]. 汽车工程,2002,24(1):20-24.

Guo Kong-hui, Ma Feng-jun, Kong Fan-sen. Driver model parameter identification of the driver-vehicle-road closed-loop system[J]. Automotive Engineering,2002,24(1):20-24.