冰雪路面条件下多场景自动驾驶车辆主动避障路径规划

Active Obstacle Avoidance Path Planning for Multi-scenario Autonomous Vehicles Under Icy and Snowy Road Conditions

摘要

针对自动驾驶车辆在冰雪路面易失稳的问题，提出改进的快速扩展随机树（RRT）路径规划算法.首先，建立冰雪路面车辆动力学模型，引入路面附着系数；然后，采用结合车头指向及转向角的全局目标偏向性采样，结合避撞检测与速度-附着系数下的最大曲率约束，改善传统RRT算法问题；最后，使用双五次多项式平滑路径，满足稳定性、制动器约束及舒适性.通过MATLAB-Simulink与CarSim联合仿真，比较改进RRT算法与传统算法在多场景条件下的性能.实验表明，改进RRT算法显著提升路径平滑度，降低曲率突变，用时短、成功率高，且在冰雪路面行驶时稳定性良好.

Abstract

Addressing the issue of autonomous vehicles’ instability on icy and snowy roads， an improved rapidly-exploring random tree （RRT） path planning algorithm is proposed. Firstly， a dynamic model introducing road adhesion coefficient on icy and snowy roads is established. Secondly， the global target deflection sampling combined with the front pointing and steering angle of the vehicle， combined with the collision avoidance detection and the maximum curvature constraint under the velocity-adhesion coefficient， is used to improve the traditional RRT algorithm problem.Finally， a double quintic polynomial is used for path smoothing to ensure stability， brake constraints， and comfort. The performance of the improved algorithm RRT is compared with that of the traditional algorithm under multi-scenario conditions through the joint simulation of MATLAB-Simulink and CarSim. The experiments show that the improved RRT algorithm significantly improves the path smoothness， reduces the curvature mutation， has short time， high success rate and good stability when driving on ice and snow.

Graphical abstract

关键词

自动驾驶车辆 / 路径规划 / 快速扩展随机树 / 冰雪路面 / 多场景

Key words

autonomous vehicle / path planning / rapidly-exploring random tree / ice and snow roads / multi-scenario

路径规划是自动驾驶车辆导航和控制的基础，可分为局部路径规划和全局路径规划.在自动驾驶车辆行驶过程中必须以局部环境信息和自身状态信息为基础，规划出一段无碰撞的理想局部路径.目前，对路径规划算法的研究较为成熟，基于图搜索的算法是遍历的确定性搜索方法，最具代表性的是基于广度优先搜索（BFS）的Dijkstra算法^［1］、利用启发式函数提高路径搜索效率的A*算法^［2］和基于Voronoi图^［3］的路径规划算法；基于随机采样^［4-5］的路径规划算法适用于高维度空间，以概率完备性替代完备性，但在性能方面表现较差；Lai等^［6］通过分布贝叶斯学习改进了快速扩展随机树（RRT）算法，实现自适应平衡规划中的全局搜索与局部开发，提高了传统算法的性能；Karaman等^［7］提出基于概率采样路径规划（PRM）算法的渐进最优版本概率路网（PRM*）算法.除此之外，还有基于启发式智能算法^［8-11］的路径规划算法.

目前对路径规划算法的研究较为成熟，RRT算法作为解决自动驾驶车辆路径规划问题的算法，众多学者对其优越性以及行驶环境适用性进行了研究.La Valle等^［12］首次提出了具有采样概率完备性的基于RRT的路径规划算法.RRT算法采用增量迭代方式对规划空间进行搜索，具有无须对环境空间离散建模、规划效率高等优势，在路径规划领域内获得了广泛应用，但其随机采样策略会导致随机树拓展的盲目性，规划出的路径会出现明显的拐角甚至存在绕远现象^［13］.Chang等^［14］提出了结构感知快速探索随机树（SARRT）算法，使随机树在温度场中偏向目标区域生长，但方程求解成本较高导致算法效率大幅下降.Kuffner等^［15］提出的快速扩展随机树-连接（RRT-connect）算法使得随机树从起点、终点2个位置同时生长，显著提高了算法效率，但当车辆处于狭窄环境时算法的效率、实用性和安全性都会受到一定程度影响.轨迹参数空间RRT（TP-space RRT）^［16］是一种新的RRT变体，它精准地结合了车辆形状和车辆运动学约束.

现有的对于二维平面环境中自动驾驶车辆路径规划方法的研究同时考虑算法效率、路径长度、避障功能的需求会使算法计算成本过高.虽然部分学者以牺牲其中一项为代价来保证算法整体效益，且这些方法未考虑车辆的自身尺寸、最大转弯角和安全行驶约束，但仍无法很好地在实际应用中满足自动驾驶车辆的需求.Ghosh等^［17］在RRT算法节点采样的过程中，融入了车辆最大转弯角的运动学约束，扩展节点符合该约束则采用，否则删除该采样点并重新采样.在算法验证中，由于车辆最大转弯角的限制，新采样点符合车辆自身约束的成功采样概率小于抛弃采样点的失败概率，降低了算法效率，在动态环境处理问题上存在一定难度.

考虑冰雪路面条件下车辆动力学特征，利用全局路径的先验信息，提出面向自动驾驶车辆在冰雪路面条件下改进RRT路径规划算法，旨在提高规划路径的实用性，在不同场景下保证算法效率的同时满足车辆自身运动学约束的要求，为自动驾驶车辆提供有力的技术支撑.

1 冰雪路面条件下车辆动力学模型构建

1.1 冰雪路面条件下车辆动力学建模

车辆动力学模型通过对轮胎和路面之间复杂相互作用来描述车辆的运动，研究目标是使车辆稳定地跟踪规划路径，属于操纵稳定性问题.虽然复杂的车辆模型可以提高精度，但会降低算法效率.为兼顾效率与精度，采用二自由度模型描述车辆运动，对模型作如下理想化假设^［18］： ①车辆在平坦路面行驶，忽略车辆垂向、俯仰运动；② 车辆及其悬架是刚性的，忽略悬架对车身结构的影响；③ 只考虑纯侧偏轮胎特性，忽略轮胎力的纵向、横向耦合关系；④ 冰雪路面条件下车辆行驶速度变化缓慢，忽略前后轴的载荷转移.

基于以上4点理想化假设进行车辆动力学建模，如图1所示.控制输入量可简化为\left(\mathit{a},{\delta }_{\mathrm{f}}\right)，其中， a 是车辆加速度，{\delta }_{\mathrm{f}}是方向盘转角，即前轮转角.定义状态量\left(\dot{\mathit{x}},\dot{\mathit{y}},\dot{\mathit{\psi }},X,Y\right)，\dot{\mathit{x}}和\dot{\mathit{y}}表示车身的纵向和侧向速度，\dot{\mathit{\psi }}为偏航角速度，\left(X,Y\right)为车身当前所处位置，这些状态量在时间尺度上的微分方程如式（1）所示，该模型满足以上理想假设条件.

\left.\begin{array}{l}\ddot{\mathit{x}}=\dot{\mathit{\psi }}\dot{\mathit{y}}+{\mathit{a}}_x,\\ \ddot{\mathit{y}}=-\dot{\mathit{\psi }}\dot{\mathit{x}}+\frac{\mathrm{2}}{m}\left({\mathit{F}}_{\mathrm{c},\mathrm{f}}\mathrm{c}\mathrm{o}\mathrm{s}{\delta }_{\mathrm{f}}+{\mathit{F}}_{\mathrm{c},\mathrm{r}}\right),\\ \dot{\psi }=\frac{\mathrm{2}}{I_{\mathrm{z}}}(l_{\mathrm{f}}{\mathit{F}}_{\mathrm{c},\mathrm{f}}-l_{\mathrm{r}}{\mathit{F}}_{\mathrm{c},\mathrm{r}}),\\ X=\dot{\mathit{x}}\mathrm{c}\mathrm{o}\mathrm{s}\dot{\mathit{\psi }}-\dot{\mathit{y}}\mathrm{s}\mathrm{i}\mathrm{n}\dot{\mathit{\psi }},\\ Y=\dot{\mathit{x}}\mathrm{s}\mathrm{i}\mathrm{n}\dot{\mathit{\psi }}+\dot{\mathit{y}}\mathrm{c}\mathrm{o}\mathrm{s}\dot{\mathit{\psi }}.\end{array}\right\}（1）

式中：m为车辆总质量； a_x 为车辆纵向加速度；I_z为偏航惯性；\psi为车辆当前偏航角； F_c，f， F_c，r为前后轮胎受到的侧向力；l_{\mathrm{f}}，l_{\mathrm{r}}为质心至前轴和后轴的距离；{\delta }_{\mathrm{f}}为前轮转角；{\delta }_{\mathrm{r}}为后轮转角，{\delta }_{\mathrm{r}}=\mathrm{0}（见图1）；X为全局坐标系中车辆的纵向速度分量；Y为全局坐标系中车辆的横向速度分量.

1.2 冰雪路面条件下的轮胎模型

轮胎模型可用于评价轮胎的制动性能，分为用于预测车辆在驱动和制动工况时纵向力的轮胎纵滑模型、用于预测轮胎的侧向力和回正力矩的轮胎侧偏和侧倾模型、用于高频垂向振动评价的轮胎垂向振动模型.其中，以刷子模型、魔术公式和Lugre动态模型为代表，根据1.1节的4点理想假设，本文采用Bakker等^［19］提出的魔术公式轮胎模型，该模型能较好地反映车辆运动状态和轮胎摩擦情况，能描述轮胎在定速、变滑移率下产生的制动力.

基于MATLAB-Simulink软件建立魔术公式轮胎模型，模型输入和输出变量之间的关系如图2所示.

由图2可以看出，基于魔术公式对轮胎进行建模，是通过描述轮胎的纵向力、侧向力、回正力矩、翻转力矩与侧偏角、纵向滑移率之间的关系，能够表达冰雪路面条件下不同驱动情况的轮胎特性^［20］，表达式一般为

\left.\begin{array}{l}\tilde{Y}=\tilde{y}+S_{\mathrm{v}},\\ \tilde{y}=D\mathrm{s}\mathrm{i}\mathrm{n}\left\{C\mathrm{a}\mathrm{r}\mathrm{c}\mathrm{t}\mathrm{a}\mathrm{n}[B\tilde{x}-E(B\tilde{x}-\mathrm{a}\mathrm{r}\mathrm{c}\mathrm{t}\mathrm{a}\mathrm{n}B\tilde{x})]\right.\},\\ \tilde{x}=\tilde{X}+S_{\mathrm{h}}.\end{array}\right\}（2）

式中：\tilde{Y}为输出变量，包括侧向力{\mathit{F}}_y和纵向力{\mathit{F}}_x；\tilde{X}为输入变量，表示侧偏角或纵向滑移率；B为刚度因子；C为曲线形状因子；D为峰值因子；E为曲线曲率因子；S_{\mathrm{v}}为曲线的垂直方向漂移；S_{\mathrm{h}}为曲线的水平方向漂移，\tilde{x}为修正后的输入变量，\tilde{y}非线性中间输出量.

不同类型冰雪路面具有不同的路面附着系数，车辆在不同附着系数的冰雪路面换道时，轨迹参数也不同^［21］.车辆在弯道冰雪路面行驶时，为防止侧向加速度过大导致侧滑、侧偏甚至侧翻，还需解决车辆的平衡问题，从而保证冰雪路面自动驾驶车辆行驶安全性能及车内乘客的舒适性.对车辆沿车体坐标系进行横向、纵向受力分解，根据受力平衡可以得到以下等式：

m{\mathit{a}}_y\mathrm{c}\mathrm{o}\mathrm{s}\beta ={\mathit{F}}_{\mathrm{f}\mathrm{i}}+{\mathit{F}}_{\mathrm{f}\mathrm{o}}+m\mathit{g}\mathrm{s}\mathrm{i}\mathrm{n}\beta ；（3）

m\mathit{g}\mathrm{c}\mathrm{o}\mathrm{s}\beta +m{\mathit{a}}_y\mathrm{s}\mathrm{i}\mathrm{n}\beta ={\mathit{N}}_{\mathrm{i}}+{\mathit{N}}_{\mathrm{o}}.（4）

式中：{\mathit{a}}_y为侧向加速度，\mathrm{m}/{\mathrm{s}}^{\mathrm{2}}；\mathit{g}为重力加速度，\mathrm{m}/{\mathrm{s}}^{\mathrm{2}}；\beta为超高坡度；{\mathit{F}}_{\mathrm{f}\mathrm{i}}，{\mathit{F}}_{\mathrm{f}\mathrm{o}}分别为冰雪路面上车辆内侧轮胎和外侧轮胎的摩擦力，N；{\mathit{N}}_{\mathrm{i}}，{\mathit{N}}_{\mathrm{o}}分别为路面对车辆的内侧和外侧支持力，N.

当车辆将要发生侧滑时，轮胎的最大横向摩擦力等于车轮与路面的附着力，即

({\mathit{N}}_{\mathrm{i}}+{\mathit{N}}_{\mathrm{o}})f={\mathit{F}}_{\mathrm{f}\mathrm{i}}+{\mathit{F}}_{\mathrm{f}\mathrm{o}}.（5）

式中，f为路面附着系数.

由式（3）~（5）联立可知，车辆在侧滑临界阈值下侧向加速度公式如式（6）所示：

{\mathit{a}}_{\mathrm{y}\mathrm{c}}=\frac{f\mathit{g}\mathrm{c}\mathrm{o}\mathrm{s}\beta +\mathit{g}\mathrm{s}\mathrm{i}\mathrm{n}\beta }{\mathrm{c}\mathrm{o}\mathrm{s}\beta -f\mathrm{s}\mathrm{i}\mathrm{n}\beta }.（6）

式中，{\mathit{a}}_{\mathrm{y}\mathrm{c}}为侧滑临界阈值下车辆侧向加速度，\mathrm{m}/{\mathrm{s}}^{\mathrm{2}}.

根据文献［22］的弯道路段超高取值，可将\mathrm{s}\mathrm{i}\mathrm{n}\beta近似为i，\mathrm{c}\mathrm{o}\mathrm{s}\beta近似等于1，式（6） 可化简为

{\mathit{a}}_{\mathrm{y}\mathrm{c}}=\frac{f\mathit{g}+\mathit{g}i}{\mathrm{1}-fi}.（7）

式中，i为弯道路段超高取值.

转弯半径阈值和侧向加速度与行车速度有关，关系式为

R_{\mathrm{c}}=\frac{{\mathit{v}}^{\mathrm{2}}}{{\mathit{a}}_{\mathrm{y}\mathrm{c}}},

即R_{\mathrm{c}}=\frac{{\mathit{v}}^{\mathrm{2}}(\mathrm{1}-fi)}{f\mathit{g}+\mathit{g}i}.（8）

式中：R_{\mathrm{c}}为转弯半径阈值，m；\mathit{v}为行车速度，\mathrm{m}/\mathrm{s}.

2 改进RRT路径规划算法

2.1 改进的自适应目标偏向采样策略

相对于传统RRT算法，本文引入了改进的自适应目标偏向采样策略^［23］，该策略考虑到目标地点对于车辆的吸引，使得随机树以指定概率偏向目标点生长，避免了原算法由于无方向生长导致的随机采样范围较大、算法效率低的问题.当随机概率时（P_{\mathrm{b}\mathrm{i}\mathrm{a}\mathrm{s}}为偏向概率，用于算法中确定采样的倾向性），探索树朝目标点生长，且P越小，偏向概率越高；当P>P_{\mathrm{b}\mathrm{i}\mathrm{a}\mathrm{s}}时，探索树在状态空间中随机选择采样点生长，P越大，随机概率越高，其目标偏向性和随机延展性受随机概率P的影响越大.目标偏向采样策略降低了路径节点的扩展时间，但该方法未考虑到自动驾驶车辆当前时刻的车头朝向及车辆实际运动方向，不符合车辆运行特性.

为了克服以上问题，本文考虑车辆在换道、掉头、转弯等城市道路真实场景中易发生侧滑、侧偏及侧翻的现象，以局部定点采样的方式来扩展随机树分支，在此基础上结合车辆当前车头指向的全局偏向性采样方法，提高该算法的路径搜索效率，使规划的路径更适用于冰雪路面条件下自动驾驶车辆的行驶.

2.2 考虑体积碰撞的车辆环境空间构建

传统RRT算法将研究目标简化成质点，但仅通过障碍车辆的质心相对位置来判断车辆是否发生碰撞是不准确的，车身边界也是决定是否发生碰撞的关键因素.有学者考虑到车体结构的影响，对障碍物空间进行膨胀预处理，膨胀尺寸为车辆宽度的一半.当该方法不能精细化车辆轮廓时，难以保证采样过程中车辆与障碍物保持安全距离的需求.为了解决此问题，在进行初步采样的过程中，引入Zhou等^［24］提出的碰撞判断方法来评价本车与障碍车辆发生碰撞的风险.

首先对本车与障碍车辆的矩形顶点进行定义，然后进行碰撞检测.k时刻代表车辆矩形轮廓的4个顶点位置，定义为

\left[\begin{array}{l}{x}_{c\mathrm{1}}\left(k\right)y_{c\mathrm{1}}\left(k\right)\\ x_{c\mathrm{2}}\left(k\right)y_{c\mathrm{2}}\left(k\right)\\ x_{c\mathrm{3}}\left(k\right)y_{c\mathrm{3}}\left(k\right)\\ x_{c\mathrm{4}}\left(k\right)y_{c\mathrm{4}}\left(k\right)\end{array}\right]=\frac{\mathrm{1}}{\mathrm{2}}\left[\begin{array}{l}-l_c-w_c\\ l_c-w_c\\ l_c{w}_c\\ -l_c{w}_c\end{array}\right]\cdot \left[\begin{array}{cc}\mathrm{c}\mathrm{o}\mathrm{s}\left({\phi }_c\left(k\right)\right)& \mathrm{c}\mathrm{o}\mathrm{s}\left({\phi }_c\left(k\right)\right)\\ -\mathrm{s}\mathrm{i}\mathrm{n}\left({\phi }_c\left(k\right)\right)& \mathrm{s}\mathrm{i}\mathrm{n}\left({\phi }_c\left(k\right)\right)\\ \mathrm{0}& \mathrm{0}\\ \mathrm{0}& \mathrm{0}\end{array}\right]+\left[\begin{array}{cc}{x}_c\left(k\right)& y_c\left(k\right)\\ x_c\left(k\right)& y_c\left(k\right)\\ x_c\left(k\right)& y_c\left(k\right)\\ x_c\left(k\right)& y_c\left(k\right)\end{array}\right],(9)

{\phi }_c\left(k\right)=\mathrm{a}\mathrm{r}\mathrm{c}\mathrm{t}\mathrm{a}\mathrm{n}\left(\frac{y_c\left(k+T\right)-y_c\left(k\right)}{x_c\left(k+T\right)-x_c\left(k\right)}\right),\mathrm{0}\le k\le N_{\mathrm{p}}.（10）

式中：N_{\mathrm{p}}为预测时域；T为状态变化的时间尺度；下标c包括\mathrm{h}和\mathrm{o}，分别表示本车和障碍车辆；l_{\mathrm{h}}，l_{\mathrm{o}}为车长；w_{\mathrm{h}}，w_{\mathrm{o}}为车宽；{\phi }_{\mathrm{h}}\left(k\right)，{\phi }_{\mathrm{o}}\left(k\right)为航向角；x_{\mathrm{h}}\left(k\right)，y_{\mathrm{h}}\left(k\right)，x_{\mathrm{o}}\left(k\right)，y_{\mathrm{o}}\left(k\right)分别为k时刻车辆质心的纵坐标和横坐标；x_{\mathrm{h}i}\left(k\right)，y_{\mathrm{h}i}\left(k\right)，x_{\mathrm{o}i}\left(k\right)，y_{\mathrm{o}i}\left(k\right)\left(i=\mathrm{1,2},\mathrm{3,4}\right)分别代表车辆在k时刻4个顶点坐标.

通过RRT算法生成的下一时刻本车和障碍车的节点坐标，根据车辆尺寸及车头指向推算其顶点坐标，通过碰撞检测判断本车与障碍车是否发生碰撞，从而便于RRT算法对路径节点进行调整优化，具体碰撞检测流程见图3.

2.3 基于速度-附着系数的最大曲率约束

自动驾驶车辆前轮转角\left|{\delta }_{\mathrm{f}}\right|\le {\delta }_{\mathrm{m}\mathrm{a}\mathrm{x}}（一般为30°～40°，这里取{\delta }_{\mathrm{m}\mathrm{a}\mathrm{x}}=\mathrm{35}°），若要使车辆能够满足行驶的平顺性和舒适性，应保证最小转弯半径小于规划路径的曲率半径^［25］.约束前轮侧偏角会间接约束前轮转角，过大的约束会出现转弯困难的问题，最大曲率约束相对于改进的自适应目标偏向采样策略而言，在最大转向角偏向采样策略的基础上对车辆路径规划进行二次约束，以满足实际情况中冰雪路面下车辆动力学特性.

本文采用剪枝策略和移枝策略.剪枝策略是在随机采样阶段对不符合车辆自身运动学及动力学约束的采样点进行删减处理，并筛选出当前节点与采样点连线方向符合车头指向和其自身最大转弯角要求的采样点，以保证规划出的局部路径更符合车辆实际行驶特性，如图4所示.采用移枝策略对不满足自动驾驶车辆最小转弯半径的规划路径进行调整，减少前轮转角，降低曲率突变量，减少路径平滑的计算量，如图5所示.

2.4 动态环境下路径规划策略

自动驾驶车辆在城市道路复杂的交通环境下行驶，存在较多不可控动态因素，将环境视为静态进行路径规划处理不能满足其正常行驶需求.环境的改变会对规划结果造成持续影响，自动驾驶车辆环境感知传感器会以一定的周期实时识别周边环境信息，连续计算新的转向角，更新车辆的位置和航向.本文利用周期变换后的迟滞时间，采用递进式静态RRT算法进行路径的规划与变更，在每个感知数据更新的周期内对前一周期规划的路径进行局部修正.

为了满足自动驾驶车辆在各种工况下安全性和舒适性的要求，引入了双五次多项式理论^［26］对改进的RRT算法的规划路径进行平滑处理，使整个路径适应新环境并保持行驶的安全性和稳定性.

3 仿真实验与分析

为了验证改进RRT算法对于换道、转弯及掉头3种环境路径规划的可行性，分别进行静态环境和动态环境的路径规划仿真.静态环境下的改进RRT算法用于验证算法的改进效果，动态环境下算法用于确定冰雪路面条件约束下RRT算法的有效性，在MATLAB中Simulink模块搭建目标车辆动力学模型，并联合CarSim进行仿真，将改进RRT算法与传统RRT，RRT-connect算法进行对比，仿真环境为MATLAB2021a，硬件平台为Win10+Corei7-8750H CPU @ 2.20 GHz+16 GB RAM.

路径规划是在二维平面空间中进行的，环境中的实体包括静态实体障碍物、动态实体障碍物、动态实体自动驾驶车辆，建立直道、弯道、掉头3种场景，对地图环境及车辆环境进行栅格化处理，便于算法识别运行.

3.1 静态路径规划仿真

3.1.1 静态环境构建

车辆在冰雪路面条件下进行避障，一般存在2种横向失稳的情况：①小范围侧滑失稳；②后车身大范围甩尾侧滑.本文设置了5种静态场景，如图6~图8所示，分别满足换道、转弯、掉头3种场景的实际应用需求.换道场景1的起始点坐标为（375，195），目标点坐标为（103，139）；换道场景2的起始点坐标为（389，268），目标点坐标为（78，295）；转弯场景1的起始点坐标为（266，170），目标点坐标为（143，389）；转弯场景2起始点坐标为（120，222），目标点坐标为（335，426）；掉头场景的起始点坐标为（767，515），目标点坐标为（867，161）.

3.1.2 静态环境仿真分析

1） 静态障碍环境算法仿真.考虑不同障碍物及不同环境下改进RRT算法的路径规划性能，对3种算法在换道场景1、换道场景2、转弯场景1、转弯场景2中各进行30次仿真，改进RRT算法比传统RRT和RRT-connect算法的扩展时间短，这表明改进的自适应目标偏向策略显著降低了搜索成本.在掉头场景下，改进RRT算法的节点扩展时间比RRT-connect算法慢1.295 s，这是因为双向扩展采样策略初始方向一致，同步搜索使其树节点相交的时间减少，而改进RRT算法起始点处车头指向和目标点处车头指向相反.尽管引入目标偏向概率相对于未引入目标偏向概率的搜索时间更短，但相对于RRT-connect算法仍产生了多余的搜索时间，这是因为较低的偏向概率无法满足效率需求，而较高的偏向概率导致采样点所在位置与车辆所在位置连线方向指向中央分隔带，使规划路径不满足车辆运行特性.此时，速度-附着系数约束使得这些采样点失效，故扩展时间和节点利用率相对于RRT-connect算法的低.

在不同场景下，不同算法的性能适应能力不同，因此，本文提出的改进RTT算法在掉头场景下仍有进一步改进的空间.在其他场景下改进RRT算法在扩展时间、节点利用率以及路径长度方面均有良好表现，极大地提升了路径搜索效率.

2） 静态障碍环境车辆动力学仿真.在MATLAB算法的基础上联合CarSim车辆动力学仿真，换道场景1、换道场景2、转弯场景1、转弯场景2、掉头场景下改进RRT算法规划出的路径、前轮转角、横摆角、横摆角速度如图9~图13.

换道场景1的目的实现动作为超过目标车道前方车辆，然后实施换道.图9a和图9c反映了换道场景1中车辆横摆角及前轮转角的变化情况.横摆角在［-3.7°，-1.5°］内，前轮转角高峰值为40.1°，低峰值为-35.1°，存在4个峰阈值超过范围，但94.7%以上基本在［-30.0°，30.0°］内，由于空白区域较多，障碍物约束较少，所以偏向较为随机，存在波动现象.图9b反映了换道场景1中横摆角速度变化情况，低峰值为-11.2 °/s，高峰值为14.8 °/s，对结果统计分析发现，横摆角速度95.0%以上位于［-10，10］ °/s内，超车结束后横摆角速度可稳定在0附近，车辆沿规划路径行驶能保持冰雪路面低附着系数条件下的横向稳定性.

与换道场景1不同的是，换道场景2的车辆目的实现动作为向左换道超越初始车道前方车辆、再向右换道到达右侧目标车道.图10a和图10c反映了换道场景2中车辆横摆角及前轮转角的变化情况，横摆角基本在［2.3°，4.1°］内，满足不发生横向侧滑的横摆角阈值［-5°，5°］，前轮转角高峰值为29.0°，低峰值为-39.8°，低峰数为2，前轮转角出现第1次峰值，是由于前轮转角变化在整个过程中90.5%以上在［-30°，30°］内，且动力学参数较场景1波动小，这是因为在车辆路径规划的过程中，障碍物数量多且密集，环境约束降低了改进RRT算法的路径搜索离散性，而且此时轮胎侧向力处于线性区域，侧向力位于线性区域内可减少轮胎模型线性化处理后的精度损失^［27］.图10b反映了换道场景2中横摆角速度变化情况，低峰值为-14.5 °/s，高峰值为10.3 °/s，对结果统计分析发现，横摆角速度93.2%以上位于［-10，10］ °/s区间内，超车结束后横摆角速度可稳定在0值附近，车辆具有良好的横向稳定性.

转弯场景1反映了车辆在交叉口转弯时无障碍物的一般情况，由图11a可知，车辆初始横摆角为2.75°，最大横摆角为4.65°，低于5.0°阈值，车辆转弯过程中目标偏向随机概率P较低时，指向性越强、波动性越小；由图11b和图11c可知，横摆角速度波动范围在［-4.5，9.0］ °/s区间内，前轮转角99.1%位于［-30°，30°］内，符合车辆行驶稳定性判断标准，转弯场景1转弯效果最佳.

转弯场景2在转弯场景1的基础上增加了多个障碍物，其目的实现动作不仅仅是转弯场景1中的沿当前车道转弯，由于前方存在多个限制车辆通行的障碍车辆，所以本车最优路径规划结果应从上到下第3车道驶出交叉口，图12a反映车辆在该场景下的横摆角变化，横摆角处于［0.4°，-1.75°］内，相对于转弯场景1的横摆角变化幅度及变化率都大幅下降，图12b和图12c横摆角范围均处于横向侧滑的阈值范围内，转弯结束后各参数均在稳定值上下浮动，说明改进RRT算法在路径规划过程中路径搜索范围受到障碍物的限制，并继续向前搜索，直到在满足碰撞检测的条件下节点可以向右扩展到达目标位置.

图13a~图13c为掉头场景下改进RRT算法动态路径规划下目标车辆速度分别为5 \mathrm{m}/\mathrm{s}和10 \mathrm{m}/\mathrm{s}的横摆角、横摆角速度、前轮转角变化.

在掉头转弯场景中，各项动力学指标虽基本满足低附着系数下自动驾驶车辆的横向行驶稳定性，但是在高速行驶时出现不期望的波动，且波动幅度大、频率高，这是由于改进的目标偏向策略存在随机性质，其目标偏向性和随机延展性受随机概率P的影响.掉头场景下车辆的转向角度大，达到180°，而起始点和目标点附近存在道路中央分隔带，在随机概率P的调参过程中通过牺牲目标偏向性来满足车辆动力学转角及避撞要求，导致改进RRT算法在掉头场景中的节点扩展随机性较高，使其横摆角速度、前轮转角等动力学参数的波动幅度较大、波动频率较高，在5种仿真场景中，随着目标点处车头指向与起始点处车头指向夹角的增大，考虑改进自适应目标偏向采样策略与速度-附着系数曲率约束策略相结合的方法适用性逐渐增强，虽然在掉头行驶工况下所规划的路径长度较长，与静态环境算法仿真中的实验结果相符合，但相对而言平滑且曲率适当的路径增强了车辆行驶稳定性.

3.2 动态路径规划仿真

3.2.1 动态环境构建

根据城市道路车辆行驶的实际情况，以城市双车道道路为例，进行动态环境下改进RRT算法的功能性验证.该场景假设车辆在双车道上行驶，根据换道决策确定主车目标车道为左车道.主车分别以10 \mathrm{m}/\mathrm{s}和8 \mathrm{m}/\mathrm{s}的纵向速度匀速行驶，目标车道前方存在1辆行驶速度为5 \mathrm{m}/\mathrm{s}的黄色障碍车，黄色障碍车存在减速意向，初始车道前方存在1辆行驶速度为6 \mathrm{m}/\mathrm{s}的黑色障碍车，路面附着系数采用普通雪面附着系数临界值，取为0.25，采样频率越高，路径结果越优，故将自动驾驶车辆环境感知更新时间设置为0.1 s.

3.2.2 动态环境仿真分析

图14动态展示了有2个动态障碍物的直道双车道换道轨迹规划过程，以验证该方法的可行性，如图14a所示，1号车辆为初始状态，11号车辆为终末状态，红色实线为最终可行规划路径.分别抽取采样帧为0，2，3，5和6 s 代表性时刻下改进RRT算法的路径规划更新状态进行分析.

主车从起始点出发0 s时自动驾驶车辆传感器将周围环境状态数据传入改进RRT算法，包括全局路径规划阶段获得的起始点和目标点位置、周围车辆的位置及速度、路面附着系数等信息，在\mathrm{\Delta }t时刻（改进RRT算法节点扩展时间），规划出该时刻路径，并采用双五次多项式对路径进行平滑，形成如图14b所示的蓝色点线示意轨迹.

主车从起始点出发2 s时各车位置信息如图14c所示，通过环境感知进行信息更新，重新进行此时刻的路径规划，主车按上一信息更新时刻规划的路径行驶时，碰撞检测未通过，故采取改进剪枝策略，在此时降低目标采样偏向性，向右偏移使得主车满足不发生碰撞这一约束条件.如图14d所示，主车从起始点出发3 s时，由于上一信息更新时刻规划路径在同一目标时刻距离黑色障碍车过近，容易产生刮擦、碰撞现象，故改进RRT算法采用移枝策略修正了路径，使轨迹更加平滑、可靠且安全性更高.图14e和图14f反映了主车成功换道且与目标车道后方车辆、初始车道前方车辆保持一定距离，换道轨迹与期望轨迹相符合.

主车目标车道为左侧车道，前方黄色障碍车、黑色障碍车的行驶速度不能满足主车的期望速度，因此主车产生换道超车决策并采用改进RRT算法进行动态路径规划，由图15可见车辆前轮转角、横摆角满足车辆运输约束，轨迹波动较小.实验表明，主车成功完成了冰雪路面下的轨迹规划任务，在双五次多项式平滑下生成的轨迹能够快速避开动态障碍物，不会产生突然转向，不会引起速度和加速度的剧烈变化，有助于提高驾驶的舒适性和稳定性.

4 结 语

传统RRT算法不能满足真实冰雪路面条件下自动驾驶车辆路径规划的实际需求，车辆高速转弯极易发生侧滑、侧偏、侧翻等危险，为提高路径规划的效率和质量，在传统RRT算法的基础上，提出若干改进措施，包括引入车辆形状约束、全局目标偏向采样策略、最大曲率约束、冰雪路面速度-附着系数约束以及基于双五次多项式的曲线平滑处理.不同环境下的仿真实验表明，改进RRT算法能够提高搜索效率，规划路径能满足自动驾驶车辆在冰雪路面上安全稳定行驶的需要，且在障碍物较多的复杂环境下性能较优.

通过静态和动态仿真实验可知，起始点和目标点连线方向与自动驾驶车辆初始车头方向的偏离角度对改进RRT算法的路径搜索效率以及车辆横向稳定性能的影响较大.在路径规划中，未考虑悬架和车身结构可能会导致车辆零部件的损坏，影响车辆的性能和寿命，同时，二者也会对乘坐舒适性产生一定影响.

参考文献

[1] Dijkstra E W. A note on two problems in connexion with graphs［M］//Edsger Wybe Dijkstra. New York： ACM， 2022.

[2] 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.

[3] Chi W Z， Ding Z Y， Wang J K，et al. A generalized Voronoi diagram-based efficient heuristic path planning method for RRTs in mobile robots［J］. IEEE Transactions on Industrial Electronics， 2022， 69（5）： 4926-4937.

[4] Wang J K， Chi W Z， Li C M， et al. Efficient robot motion planning using bidirectional-unidirectional RRT extend function［J］. IEEE Transactions on Automation Science and Engineering， 2022， 19（3）： 1859-1868.

[5] Hauser K.Lazy collision checking in asymptotically-optimal motion planning［C］//IEEE International Conference on Robotics and Automation（ICRA）. Seattle， 2015： 2951-2957.

[6] Lai T， Morere P， Ramos F， et al. Bayesian local sampling-based planning［J］. IEEE Robotics and Automation Letters， 2020， 5（2）： 1954-1961.

[7] Karaman S， Frazzoli E. Sampling-based algorithms for optimal motion planning［J］. The International Journal of Robotics Research， 2011， 30（7）： 846-894.

[8] Song Q， Zhao Q L， Wang S X， et al. Dynamic path planning for unmanned vehicles based on fuzzy logic and improved ant colony optimization［J］. IEEE Access， 2020， 8： 62107-62115.

[9] Li R H， Chang Y L， Wang Z C. Study of optimal allocation of water resources in Dujiangyan irrigation district of China based on an improved genetic algorithm［J］. Water Supply， 2021， 21（6）： 2989-2999.

[10] 邵琪，时维国.基于改进蚁群算法的机器人路径规划研究［J］.现代制造工程，2023（6）：46-51.

[11] Shao Qi， Shi Wei-guo. Research on robot path planning based on improved ant colony algorithm［J］. Modern Manufacturing Engineering， 2023（6）： 46-51.

[12] Yu Z H， Si Z J， Li X B， et al. A novel hybrid particle swarm optimization algorithm for path planning of UAVs［J］. IEEE Internet of Things Journal， 2022， 9（22）： 22547-22558.

[13] La Valle S M， Kuffner J J. Randomized kinodynamic planning［C］// IEEE International Conference on Robotics and Automation. Detroit： 1999： 473-479.

[14] 宋金泽，戴斌，单恩忠，等.一种改进的RRT路径规划算法［J］.电子学报，2010，38（2A）：225-228.

[15] Song Jin-ze， Dai Bin， Shan En-zhong， et al. An improved RRT path planning algorithm［J］. Acta Electronica Sinica， 2010， 38（2A）： 225-228.

[16] Chang X F， Wang Y Z， Yi X D， et al. SARRT： a structure-aware RRT-based approach for 2D path planning［C］//IEEE International Conference on Robotics and Biomimetics. Zhuhai， 2015： 1698-1703.

[17] Kuffner J J， LaValle S M. RRT-connect： an efficient approach to single-query path planning［C］//IEEE International Conference on Robotics and Automation. San Francisco， 2000： 995-1001.

[18] Blanco J L， Bellone M， Gimenez-Fernandez A. TP-space RRT-kinematic path planning of non-holonomic any-shape vehicles［J］. International Journal of Advanced Robotic Systems， 2015， 12（5）： 55.

[19] Ghosh D， Nandakumar G， Narayanan K， et al. Kinematic constraints based Bi-directional RRT（KB-RRT） with parameterized trajectories for robot path planning in cluttered environment［C］//2019 International Conference on Robotics and Automation. Montreal. 2019： 8627-8633.

[20] 毛丁丁，邓亚东.四轮转向智能车辆轨迹跟踪及稳定控制研究［J］.机械科学与技术，2020，39（7）：1094-1099.

[21] Mao Ding-ding， Deng Ya-dong. Research on trajectory tracking and stability control of 4WS intelligent vehicle［J］. Mechanical Science and Technology for Aerospace Engineering， 2020， 39（7）： 1094-1099.

[22] Bakker E， Nyborg L， Pacejka H B. Tyre modelling for use in vehicle dynamics studies［J/OL］. SAE Transactions， 1987： 190-204［2022-11-11］.

[23] 郑香美，高兴旺，赵志忠.基于“魔术公式”的轮胎动力学仿真分析［J］.机械与电子，2012，30（9）：16-20.

[24] Zheng Xiang-mei， Gao Xing-wang， Zhao Zhi-zhong. Simulation analysis of tire dynamic based on “magic formula”［J］. Machinery & Electronics， 2012， 30（9）： 16-20.

[25] 王中阳.冰雪路面下智能车辆自主换道决策规划与控制研究［D］. 淄博：山东理工大学，2020.

[26] Wang Zhong-yang. Research on decision planning and control of autonomous lane change of intelligent vehicle on ice snow road［D］. Zibo：Shandong University of Technology， 2020.

[27] 交通运输部公路局， 中交第一公路勘察设计研究院有限公司. 公路工程技术标准： JTG B01—2014 ［S］. 北京： 人民交通出版社，2015.

[28] Highway Bureau of Ministry of Transportation and Communications， CCCC First Highway Survey and Design Institute Co. Technical standard for highway engineering： JTG B01—2014 ［S］. Beijing： People’s Transportation Press， 2015.

[29] Song X G， Fan X， Cao Z Q， et al. A TC-RRT-based path planning algorithm for the nonholonomic mobile robots［C］//Chinese Control Conference. Dalian， 2017： 6638-6643.

[30] Zhou J， Zheng H Y， Wang J M， et al. Multiobjective optimization of lane-changing strategy for intelligent vehicles in complex driving environments［J］. IEEE Transactions on Vehicular Technology， 2019， 69（2）： 1291-1308.

[31] 吴彬彬，罗峰.基于RRT*的智能车辆路径规划算法［J］.机电一体化，2017，23（10）：15-23.

[32] Wu Bin-bin， Luo Feng. RRT*-based path planning algorithm for intelligent vehicle［J］. Mechatronics， 2017， 23（10）： 15-23.

[33] 牛国臣，李文帅，魏洪旭.基于双五次多项式的智能汽车换道轨迹规划［J］.汽车工程，2021，43（7）：978-986，1004.

[34] Niu Guo-chen， Li Wen-shuai， Wei Hong-xu. Intelligent vehicle lane changing trajectory planning based on double quintic polynomials［J］. Automotive Engineering， 2021， 43（7）： 978-986，1004.

[35] 龚国铮，郑少武，钟思祺，等.基于预测模型的无人赛车路径规划算法研究［J］.汽车技术，2022（7）：32-41.

[36] Gong Guo-zheng， Zheng Shao-wu， Zhong Si-qi， et al. Research on path planning algorithm of driverless race car based on predictive model［J］. Automobile Technology， 2022（7）： 32-41.