CN114194215B - 一种智能车辆避障换道轨迹规划方法及系统 - Google Patents

Abstract

本发明公开了一种智能车辆避障换道轨迹规划方法及系统，根据道路环境、障碍物信息以及当前车速，基于操稳性极限起始回正点以及多项式曲线生成可行轨迹簇，对可行轨迹簇中的换道轨迹进行预碰撞检测，筛选并剔除出不满足预碰撞检测条件的轨迹得到无碰撞轨迹簇；在无碰撞轨迹簇的基础上建立关于舒适性、避障效率、侧滑这三个评价指标的代价函数；并采用模糊推理来确定三个评价指标的权重，通过求解多目标优化问题，搜索出最优的避障换道回正点，即规划出最终的避障换道轨迹。同时本申请还提出了一种智能车辆避障换道轨迹规划系统，可以在兼顾安全性、实时性以及曲率连续的同时，使得轨迹的性能指标如舒适性、避障效率等最大化，有效改善乘员的乘坐体验。

Description

一种智能车辆避障换道轨迹规划方法及系统

技术领域

本发明属于智能驾驶车辆规划技术领域，具体涉及一种智能车辆避障换道轨迹规划方法。

背景技术

目前我国机动车保有量呈逐年上升趋势，由此所带来的交通安全、道路拥堵等问题日益突出。经研究表明，驾驶员注意力不集中或驾驶经验不足等人为因素是导致交通事故的主要原因。因此，智能驾驶车辆正在飞速的发展和进步，它可承担人类驾驶员的更多负担和压力，从而提高道路安全性，关键技术包括感知、定位、决策、规划和控制。

目前智能车辆的主动避障策略主要分为制动避障和转向避障两种。但车辆在高速行驶时，基于制动的主动避障需要较长的制动距离，仍然会有很高的碰撞风险，甚至有可能导致后方车辆追尾。因此，通过制动避免碰撞更适用于低车速以及障碍车辆较多的城市道路；而如果在车道畅通的高速公路，更适合采用转向来避免碰撞。因此在紧急避障时，快速的规划出一条合理的换道路径是至关重要的。

智能车辆运动规划是指根据决策层输入的行为意图，在相应的时空内搜索出一条具有详细路径、速度信息，并且满足各个约束条件的轨迹，最后发送给控制模块去跟踪。运动规划最初起源于机器人领域，主要分为三类方法：基于采样的方法、基于曲线插值的方法和基于数值优化的方法。其中，基于采样的方法实时性一般且生成的曲线可能存在曲率突变；曲线插值法虽然实时性好且曲率连续，但灵活性较差；而数值优化法需要处理多约束的搜索问题，计算成本很高。其中各类规划方法都有各自的特点，单一的方法无法实现最优。

发明内容

针对现有技术存在的不足，本发明提出了一种智能车辆避障换道轨迹规划方法及系统。可以保证安全、曲率连续以及实时性好，使得车辆在顺利避障的同时，并最大化的满足乘员要求，提高规划轨迹的灵活性。

本发明采用的技术方案如下：

一种智能车辆避障换道轨迹规划方法，包括如下步骤：

S1，采用多项式曲线求得广义换道轨迹，由广义换道轨迹得到参考侧向加速度关于时间的表达式，对参考侧向加速度求极值得到参考轨迹下最大侧向加速度a_{y_rmax}，由当前路面状况结合侧向加速度相关标准定义阈值：

|a_{y_rmax}|≤a_ymax＝fμg，其中系数f<1

其中，a_ymax为最大侧向加速度，系数f<1，μ为路面附着系数；g为重力加速度；

根据最大侧向加速度阈值计算换道起始回正点；由换道起始回正点在横向上进行等距离迭代，采样得到N-1个换道回正点，带入到多项式曲线广义轨迹公式中得到可行轨迹簇；

S2，对可行轨迹簇中的N条换道轨迹进行预碰撞检测，筛选并剔除出不满足预碰撞检测条件的轨迹即可能发生碰撞的轨迹，得到无碰撞轨迹簇；

S3，在无碰撞轨迹簇的基础上建立关于舒适性指标、避障效率指标、侧滑指标这三个评价指标的代价函数；并采用模糊推理来确定舒适性指标、避障效率指标、侧滑指标这三个评价指标的权重，最后通过求解多目标优化问题，搜索出最优的避障换道回正点X_{T_R}，即规划出最终的避障换道轨迹。

进一步，所述S3中所建立的代价函数表示为：

式中，w_C、w_E、w_S分别为舒适性指标、避障效率指标、侧滑指标的权重值；J_C为舒适性指标，J_E为避障效率指标，J_S为侧滑性指标。

进一步，采用模糊推理来确定舒适性指标、避障效率指标、侧滑指标这三个评价指标的权重的方法为：

首先，采用预碰撞时间TTC倒数来表征危险程度，采用路面附着系数μ表征当前路面状况；将预碰撞时间倒数TTC^-1、路面附着系数μ作为输入，w_C、w_E、w_S作为输出；设计如下模糊规则：

规则1)if TTC^-1is B andμis B，then(w_C is B)(w_E is S)(w_S is S)

规则2)if TTC^-1is B andμis S，then(w_C is S)(w_E is S)(w_S is B)

规则3)if TTC^-1is M andμis B，then(w_C is M)(w_E is M)(w_S is S)

规则4)if TTC^-1is M andμis S，then(w_C is S)(w_E is M)(w_S is M)

规则5)if TTC^-1is S andμis B，then(w_C is S)(w_E is B)(w_S is S)

规则6)if TTC^-1is S andμis S，then(w_C is S)(w_E is M)(w_S is M)

其中，S为小，M为中，B为大。

进一步，舒适性指标、避障效率指标、侧滑指标分别表示如下：

J_E＝T

式中：j_{y_r}为期望侧向跃度；a_{y_r}为期望侧向加速度；T为全程所需要的时间。

进一步，所述S2中的预碰撞检测流程为：

1)当自车右前角点与前方障碍车辆尾部的横坐标相等时，即X(A₁)＝X(B)时，开始发生碰撞，此时刻记为t1。

式中，为t时刻的航向角，可由轨迹公式计算得到。x_f(t₁)、B_f分别为前方障碍车辆的质心位置及该质心到前方障碍车辆车尾的距离，可由感知信息获取得到。A_ego、W_ego分别为自车质心到自车车头的距离以及自车车宽。求解得到t₁作为预碰撞检测起始时间点。

2)当自车右后角点与前方障碍车辆尾部的横坐标相等时，即X(A₂)＝X(B)时，完全脱离碰撞风险，此时刻记为t2。

式中，B_ego为自车质心到自车车尾的距离。求解得到t₂作为碰撞检测终止点。

3)在t₁、t₂时间点处，进行预碰撞检测。无碰撞的条件为前方障碍车辆的左后角点到自车轴线的垂直距离大于设定的阈值，即：d_CB≥d_min。

其中，d_min为垂直距离的最小值，筛选并剔除出不满足预碰撞检测条件的轨迹即可能发生碰撞的轨迹，得到无碰撞轨迹簇。

进一步，所述S1中的广义换道轨迹：

式中：以自车建立坐标系，车辆前进方向为x轴，垂直于车身方向为y轴，x(t)，y(t)分别为t时刻的x坐标和y坐标；v为车速，w为车道宽度，t为时间变量，X_T为换道回正点的期望位置。

进一步，由广义换道轨迹求两阶导数得到参考侧向加速度关于时间的表达式：

上式求极值得到参考轨迹下最大侧向加速度：

时，/>

进一步，根据最大侧向加速度阈值计算换道起始回正点：

由换道起始回正点在横向上进行等距离迭代，采样得到N-1个换道回正点，带入到多项式曲线广义轨迹公式中得到可行轨迹簇。

X_Tn＝X_T1+(n-1)d,n＝2,3,…,N

式中，X_Tn为第n个换道回正点的位置，d为迭代距离。

一种智能车辆避障换道轨迹规划系统，包括环境感知模块、候选轨迹生成模块、场景分析模块和候选轨迹筛选模块；

所述环境感知模块用于获取道路环境信息和障碍车辆运动信息，并将所获取的信息输入给其他三个模块；

所述候选轨迹生成模块根据道路环境信息并结合自车状态信息，由最大侧向加速度阈值基于多项式曲线生成操稳性极限换道轨迹，通过迭代生成可行轨迹簇，并输入给候选轨迹筛选模块；

所述场景分析模块根据道路环境信息、障碍车辆运动信息分析当前路面状况及危险程度，通过模糊决策得到当前场景下的各评价指标的权重值；将权重值输入给候选轨迹筛选模块；

所述候选轨迹筛选模块结合障碍车辆运动信息进行预碰撞检测，筛选并剔除出可能发生碰撞的轨迹得到无碰撞轨迹簇，并进一步建立舒适性、避障效率、侧滑指数三个评价指标的代价函数；基于场景分析模块输出的各评价指标相应的权重值，搜索得到最优回正点即规划出最终的避障换道轨迹。

进一步，所述环境感知模块具体包括各环境感知传感器如摄像头、激光雷达、毫米波雷达、GPS、惯导。

本发明有益效果如下：

本发明涉及一种智能车辆避障换道轨迹规划方法及系统。在曲线插值法的基础上结合了采样法、数值优化法的优势，设计了一种多目标优化轨迹规划方法，有效解决了曲线插值法普遍存在的目标点难以选取的问题，提高规划轨迹的灵活性。在实际应用层面，本发明提出的轨迹规划方法可以在兼顾安全性、实时性以及曲率连续的同时，使得轨迹的性能指标如舒适性、避障效率等最大化，有效改善乘员的乘坐体验。

附图说明

图1是避障换道轨迹规划系统总体框图；

图2是基于多项式曲线可行轨迹簇生成示意图；

图3是无碰撞轨迹簇生成示意图；

图4是最优避障换道轨迹生成示意图；

图5、6是预碰撞检测流程示意图。

具体实施方式

为了使本发明的目的、技术方案及优点更加清楚明白，以下结合附图及实施例，对本发明进行进一步详细说明。应当理解，此处所描述的具体实施例仅用于解释本发明，并不用于限定本发明。

如图1所示，本发明提出了一种智能车辆避障换道轨迹规划系统。其中系统的总体结构包括：环境感知模块、候选轨迹生成模块、场景分析模块和候选轨迹筛选模块。

环境感知模块用于获取道路环境信息和障碍车辆运动信息，并将所获取的信息输入给其他三个模块。环境感知模块具体包括各环境感知传感器如摄像头、激光雷达、毫米波雷达、GPS、惯导等。所述道路环境信息包括车道中心线、车道宽度、障碍车辆的几何尺寸等；所述障碍车辆运动信息包括各时刻的位置、速度等。

候选轨迹生成模块根据道路环境信息并结合自车状态信息，由最大侧向加速度阈值基于多项式曲线生成操稳性极限换道轨迹，通过迭代生成可行轨迹簇，并输入给候选轨迹筛选模块。

场景分析模块根据道路环境信息、障碍车辆运动信息分析当前路面状况及危险程度，通过模糊决策得到当前场景下的各评价指标的权重值。最后将权重值输入给候选轨迹筛选模块。

候选轨迹筛选模块结合障碍车辆运动信息进行预碰撞检测，筛选并剔除出可能发生碰撞的轨迹得到无碰撞轨迹簇，并进一步建立舒适性、避障效率、侧滑指数三个评价指标的代价函数；基于场景分析模块输出的各评价指标相应的权重值，搜索得到最优回正点即规划出最终的避障换道轨迹。

一种智能车辆避障换道轨迹规划方法，包括如下步骤：

S1，基于由感知传感器获得车道信息及自车状态信息。采用多项式曲线求得广义换道轨迹：

式中：以自车建立坐标系，车辆前进方向为x轴，垂直于车身方向为y轴，x(t)，y(t)分别为t时刻的x坐标和y坐标；v为车速，w为车道宽度，t为时间变量，X_T为换道回正点的期望位置。

其中，为了最大化车辆的纵向舒适性，上式确保了纵向加速度(d²x/dt²)为0，因此规划车速在大地坐标系下恒定，即：dx/dt＝v，X_T＝vT，T为避障换道全程所需的时间。通过调整回正点X_T的位置，可得到不同的换道轨迹。为了确定换道回正点X_T的具体位置，将采用如下迭代、搜索的方法找到最优的避障换道回正点X_{T_R}。

由上述轨迹曲线表达式求两阶导数可得到参考侧向加速度关于时间的表达式：

上式求极值得到参考轨迹下最大侧向加速度：

时，/>

由于侧向加速度反映了车辆在轨迹跟踪时的操纵稳定性，因此需要保证其在合理范围内，否则该轨迹无法被跟踪。由当前路面状况结合侧向加速度相关标准定义阈值：

|a_{y_rmax}|≤a_ymax＝fμg，

其中，a_ymax为最大侧向加速度，系数f<1，μ为路面附着系数；g为重力加速度。

根据最大侧向加速度阈值计算换道起始回正点：

根据上式求得的起始回正点进而生成第一条换道轨迹，即为操稳性极限换道轨迹，表示车辆在当前路面状况以及车速下能发挥出的最大转向避障潜能。此后回正点距离越远，其轨迹的参考侧向加速度极限值越小，车辆跟踪越平稳。

由换道起始回正点在横向上进行等距离迭代，采样得到(N-1)个换道回正点，带入到多项式曲线广义轨迹公式中得到可行轨迹簇，如图2所示。

X_Tn＝X_T1+(n-1)d,n＝2,3,…,N

式中，X_Tn为第n个换道回正点的位置，d为迭代距离。

S2，对可行轨迹簇中的N条换道轨迹进行搜索，寻找最优轨迹。初步筛选方法为预碰撞检测。由感知传感器获得的信息对前方车辆进行轨迹预测，结合每一条可行轨迹中自车在未来时刻的期望位置，根据自车和障碍车辆之间的相对运动信息求解出碰撞可能发生的时刻，再根据自车和障碍车辆的几何特征定义预碰撞检测条件，如图5、图6所示。

假设向左侧避障时，预碰撞检测流程为：

1)当自车右前角点与前方障碍车辆尾部的横坐标相等时，即X(A₁)＝X(B)时，开始发生碰撞，此时刻记为t1。

式中，为t时刻的航向角，可由轨迹公式计算得到。x_f(t₁)、B_f分别为前方障碍车辆的质心位置及该质心到前方障碍车辆车尾的距离，可由感知信息获取得到。A_ego、W_ego分别为自车质心到自车车头的距离以及自车车宽。求解得到t₁作为预碰撞检测起始时间点。

2)当自车右后角点与前方障碍车辆尾部的横坐标相等时，即X(A₂)＝X(B)时，完全脱离碰撞风险，此时刻记为t2。

式中，B_ego为自车质心到自车车尾的距离。求解得到t₂作为碰撞检测终止点。

3)在t₁、t₂时间点处，进行预碰撞检测。无碰撞的条件为前方障碍车辆的左后角点到自车轴线的垂直距离大于设定的阈值，即：d_CB≥d_min。

其中，d_min为垂直距离的最小值，筛选并剔除出不满足预碰撞检测条件的轨迹即可能发生碰撞的轨迹，得到无碰撞轨迹簇，如图3所示。

S3，在无碰撞轨迹簇的基础上建立三个评价指标的代价函数，通过搜索得到最优避障换道轨迹，三个评价指标分别是舒适性指标、避障效率指标、侧滑指标，表示如下：

其中:/>

J_E＝T

式中：J_C为舒适性指标，j_{y_r}表示为期望侧向加速度的导数即侧向跃度；a_{y_r}为期望侧向加速度；J_E为避障效率指标，表示为全程所需要的时间T；J_S为侧滑性指标，表示为侧向加速度在当前路面状况下的利用率。

基于上述舒适性指标、避障效率指标、侧滑指标建立如下代价函数：

式中，w_C、w_E、w_S分别为舒适性指标、避障效率指标、侧滑指标的权重值。

其中，三个权重值是与当前的危险程度以及路面状况有关的。例如，当此刻距离前方障碍车辆比较近、危险程度很高时，乘员的心理压力是最大的，此时需要尽快的躲避障碍车辆，因此避障效率的权重值应该更高；而当路面状况不太好的时候，车辆在避障时的侧滑指数不能过高，否则会有失稳的风险；最后当危险程度不太高以及路面状况较好时，乘员没有太大的心理压力、车辆的操纵稳定性较好，因此舒适性指标的权重可以更高。由于各权重值与当前具体场景有关，但没有一个定量的关系，因此采用模糊推理来确定三个指标的具体权重。

由感知传感器获得道路信息及障碍车辆信息，分析当前路面状况及危险程度。

采用预碰撞时间(TTC)倒数来表征危险程度：

式中，V_ego为当前自车车速，V_f为当前障碍车辆车速；D_ob为此刻距离前方障碍车辆的间距。当前路面状况由路面附着系数μ表征。

设计如下模糊规则。输入为预碰撞时间倒数TTC^-1、路面附着系数μ；输出为w_C、w_E、w_S。

规则1)if TTC^-1is B andμis B，then(w_C is B)(w_E is S)(w_S is S)

规则2)if TTC^-1is B andμis S，then(w_C is S)(w_E is S)(w_S is B)

规则3)if TTC^-1is M andμis B，then(w_C is M)(w_E is M)(w_S is S)

规则4)if TTC^-1is M andμis S，then(w_C is S)(w_E is M)(w_S is M)

规则5)if TTC^-1is S andμis B，then(w_C is S)(w_E is B)(w_S is S)

规则6)if TTC^-1is S andμis S，then(w_C is S)(w_E is M)(w_S is M)

其中，S为小，M为中，B为大。

将模糊推理确定的各权重值输入到候选轨迹筛选模块，可根据当前具体场景与路面状况得到具体的代价函数，最后通过求解多目标优化问题，搜索出最优的避障换道回正点X_{T_R}，即规划出最终的避障换道轨迹，如图4所示。

以上实施例仅用于说明本发明的设计思想和特点，其目的在于使本领域内的技术人员能够了解本发明的内容并据以实施，本发明的保护范围不限于上述实施例。所以，凡依据本发明所揭示的原理、设计思路所作的等同变化或修饰，均在本发明的保护范围之内。

Claims (9)

1.一种智能车辆避障换道轨迹规划方法，其特征在于，包括如下步骤：

S1，采用多项式曲线求得广义换道轨迹，所述广义换道轨迹：

式中：以自车建立坐标系，车辆前进方向为x轴，垂直于车身方向为y轴，x(t)，y(t)分别为t时刻的x坐标和y坐标；v为车速，w为车道宽度，t为时间变量，X_T为换道回正点的期望位置；

由广义换道轨迹得到参考侧向加速度关于时间的表达式，对参考侧向加速度求极值得到参考轨迹下最大侧向加速度a_{y_rmax}，由当前路面状况结合侧向加速度相关标准定义阈值：

|a_{y_rmax}|≤a_ymax＝fμg，其中系数f<1

其中，a_ymax为最大侧向加速度，系数f<1，μ为路面附着系数；g为重力加速度；

根据最大侧向加速度阈值计算换道起始回正点；由换道起始回正点在横向上进行等距离迭代，采样得到N-1个换道回正点，带入到多项式曲线广义轨迹公式中得到可行轨迹簇；

S2，对可行轨迹簇中的N条换道轨迹进行预碰撞检测，筛选并剔除出不满足预碰撞检测条件的轨迹即可能发生碰撞的轨迹，得到无碰撞轨迹簇；

S3，在无碰撞轨迹簇的基础上建立关于舒适性指标、避障效率指标、侧滑指标这三个评价指标的代价函数；并采用模糊推理来确定舒适性指标、避障效率指标、侧滑指标这三个评价指标的权重，最后通过求解多目标优化问题，搜索出最优的避障换道回正点X_{T_R}，即规划出最终的避障换道轨迹。

2.根据权利要求1所述的一种智能车辆避障换道轨迹规划方法，其特征在于，所述S3中所建立的代价函数表示为：

式中，w_C、w_E、w_S分别为舒适性指标、避障效率指标、侧滑指标的权重值；J_C为舒适性指标，J_E为避障效率指标，J_S为侧滑性指标。

3.根据权利要求2所述的一种智能车辆避障换道轨迹规划方法，其特征在于，采用模糊推理来确定舒适性指标、避障效率指标、侧滑指标这三个评价指标的权重的方法为：

首先，采用预碰撞时间TTC倒数来表征危险程度，采用路面附着系数μ表征当前路面状况；将预碰撞时间倒数TTC^-1、路面附着系数μ作为输入，w_C、w_E、w_S作为输出；设计如下模糊规则：

规则1)if TTC^-1is B andμis B，then(w_C is B)(w_E is S)(w_S is S)

规则2)if TTC^-1is B andμis S，then(w_C is S)(w_E is S)(w_S is B)

规则3)if TTC^-1is M andμis B，then(w_C is M)(w_E is M)(w_S is S)

规则4)if TTC^-1is M andμis S，then(w_C is S)(w_E is M)(w_S is M)

规则5)if TTC^-1is S andμis B，then(w_C is S)(w_E is B)(w_S is S)

规则6)if TTC^-1is S andμis S，then(w_C is S)(w_E is M)(w_S is M)

其中，S为小，M为中，B为大。

4.根据权利要求1、2或3所述的一种智能车辆避障换道轨迹规划方法，其特征在于，舒适性指标、避障效率指标、侧滑指标分别表示如下：

J_E＝T

式中：j_{y_r}为期望侧向跃度；a_{y_r}为期望侧向加速度；T为全程所需要的时间。

5.根据权利要求4所述的一种智能车辆避障换道轨迹规划方法，其特征在于，所述S2中的预碰撞检测流程为：

1)当自车右前角点与前方障碍车辆尾部的横坐标相等时，即X(A₁)＝X(B)时，开始发生碰撞，此时刻记为t₁；

式中，为t时刻的航向角，可由轨迹公式计算得到；x_f(t₁)、B_f分别为前方障碍车辆的质心位置及该质心到前方障碍车辆车尾的距离，可由感知信息获取得到；A_ego、W_ego分别为自车质心到自车车头的距离以及自车车宽；求解得到t₁作为预碰撞检测起始时间点；

2)当自车右后角点与前方障碍车辆尾部的横坐标相等时，即X(A₂)＝X(B)时，完全脱离碰撞风险，此时刻记为t₂；

式中，B_ego为自车质心到自车车尾的距离；求解得到t₂作为碰撞检测终止点；

3)在t₁、t₂时间点处，进行预碰撞检测；无碰撞的条件为前方障碍车辆的左后角点到自车轴线的垂直距离大于设定的阈值，即：d_CB≥d_min：

T1:

T2:

其中，d_min为垂直距离的最小值，筛选并剔除出不满足预碰撞检测条件的轨迹即可能发生碰撞的轨迹，得到无碰撞轨迹簇。

6.根据权利要求1所述的一种智能车辆避障换道轨迹规划方法，其特征在于，由广义换道轨迹求两阶导数得到参考侧向加速度关于时间的表达式：

上式求极值得到参考轨迹下最大侧向加速度：

时，/>

7.根据权利要求1或6所述的一种智能车辆避障换道轨迹规划方法，其特征在于，根据最大侧向加速度阈值计算换道起始回正点：

由换道起始回正点在横向上进行等距离迭代，采样得到N-1个换道回正点，带入到多项式曲线广义轨迹公式中得到可行轨迹簇；

X_Tn＝X_T1+(n-1)d,n＝2,3,…,N

式中，X_Tn为第n个换道回正点的位置，d为迭代距离。

8.一种基于权利要求1所述的一种智能车辆避障换道轨迹规划方法的智能车辆避障换道轨迹规划系统，其特征在于，包括环境感知模块、候选轨迹生成模块、场景分析模块和候选轨迹筛选模块；

所述环境感知模块用于获取道路环境信息和障碍车辆运动信息，并将所获取的信息输入给其他三个模块；

所述候选轨迹生成模块根据道路环境信息并结合自车状态信息，由最大侧向加速度阈值基于多项式曲线生成操稳性极限换道轨迹，通过迭代生成可行轨迹簇，并输入给候选轨迹筛选模块；

所述场景分析模块根据道路环境信息、障碍车辆运动信息分析当前路面状况及危险程度，通过模糊决策得到当前场景下的各评价指标的权重值；将权重值输入给候选轨迹筛选模块；

所述候选轨迹筛选模块结合障碍车辆运动信息进行预碰撞检测，筛选并剔除出可能发生碰撞的轨迹得到无碰撞轨迹簇，并进一步建立舒适性、避障效率、侧滑指数三个评价指标的代价函数；基于场景分析模块输出的各评价指标相应的权重值，搜索得到最优回正点即规划出最终的避障换道轨迹。

9.根据权利要求8所述的一种智能车辆避障换道轨迹规划系统，其特征在于，所述环境感知模块包括环境感知传感器，所述环境感知传感器包括摄像头、激光雷达、毫米波雷达、GPS、惯导。