CN112362074A - 一种结构化环境下的智能车辆局部路径规划方法 - Google Patents

Abstract

本发明请求保护一种结构化环境下的智能车辆局部路径规划方法，包括以下步骤：定位及地图加载，障碍物信息处理，坐标系转化与分析，决策与重规划方法，速度规划及局部规划更新。首先利用高精度地图和自车定位信息加载对应的局部参考路径，然后检测障碍物信息，将局部参考路径与障碍物信息进行坐标系转化，并判断是否需要重规划，再通过决策与重规划方法得到局部最优路径，最后通过速度规划与更新局部规划结果实现安全、平稳的行驶。本发明集路径规划、变道决策与速度规划于一体，能在保证无人驾驶安全性的同时，提高结构化环境下的智能车辆局部路径规划方法的时间性能。

Description

一种结构化环境下的智能车辆局部路径规划方法

技术领域

本发明属自动化与计算机技术领域，特别是智能车辆的路径规划技术领域，具体涉及一种结构化环境下的局部路径规划方法。

背景技术

智能车辆由于其具有提高交通安全性、减少能源消耗以及提升出行便利性等潜能，被认为是未来汽车发展的主要方向。自动驾驶系统一般而言可以分为：环境感知模块、决策规划模块以及控制执行模块，其中决策规划模块是自动驾驶技术中十分重要的组成部分，为环境感知和运动控制起着承上启下的作用，而具有明显车道线的城市道路以及高速公路等结构化环境是车辆行驶主要面对的道路场景，因此结构化环境下的局部路径规划是汽车无人驾驶技术研究的重要内容之一。

中国专利申请：无人驾驶汽车局部路径运动规划方法和装置(申请号：CN107992050A)公开了一种无人驾驶汽车局部路径运动规划方法，该方法通过采样得到局部目标点簇，利用贝塞尔曲线拟合目标点生成轨迹簇并计算最小代价得到最优轨迹，该方法仅考虑了前方车辆情况而得到的最优轨迹，不涉及变道分析及决策，若最优轨迹存在变道，则可能发生碰撞，安全性较低。中国专利申请：一种结构化环境下无人驾驶车辆的路径规划方法(申请号：CN108519773A)公开了一种集路径规划和换道决策于一体的结构化环境下的无人驾驶车辆规划方法，该方法利用五次样条曲线作为完整重规划模板生成大量曲线并找出代价最小的作为备选路径。该方法生成的候选路径中存在较多无参考价值的路径且无法保证备选路径的最优性，所需计算量大，规划效率较低。中国专利申请：一种结构化道路局部路径规划方法(申请号：CN110766220A)，该方法将障碍物进行分类聚合并计算出障碍物路段的可通行区域，利用改进A*算法搜索各障碍物路段离散的最优避障子路径，再将各子路径用三次样条插值曲线拟合得到连续的避障路径。该方法仅对多障碍物情况下进行了连续的避障规划，不涉及结构化道路的变道分析与决策，动态环境下安全性较低。中国专利申请：一种用于无人驾驶车辆的局部路径规划算法(申请号：202010349542.5)，该方法通过输入局部规划路径和选取预瞄点，生成道路坐标系下的备选五次多项式参数曲线，在此基础上计算备选轨迹上限速度以及对应限速点的位置，最后按照指标函数对候选轨迹进行优选和速度规划。该方法对多个场景进行了速度限制计算，但并未涉及避障相关的路径规划及选择指标，在城市道路中也未涉及变道分析与决策，适应性差且规划效率低。

本发明针对智能车辆规划决策系统中，对与结构化环境下的局部路径规划，在安全性、规划时间性能等方面所存在的不足，提出了一种结构化环境下的智能车辆局部路径规划方法。在该方法中，利用高精度地图和自车定位信息加载对应的局部参考路径，然后考虑障碍物信息的获取与表示，将局部参考路径与障碍物信息进行坐标系转化，并判断是否需要重规划，再通过决策与重规划方法得到局部最优路径，最后通过速度规划与更新局部规划结果实现安全、平稳的行驶。本方法集路径规划、变道决策和速度规划于一体，有助于提高智能车辆运行的安全性和规划时间性能。

发明内容

本发明旨在解决以上现有技术的问题。提出了一种结构化环境下的智能车辆局部路径规划方法。本发明的技术方案如下：

一种结构化环境下的智能车辆局部路径规划方法，其包括以下步骤：

定位及地图加载步骤；障碍物信息处理步骤；坐标系转化与分析步骤；决策与重规划步骤；速度规划及局部规划更新步骤；其中，

定位及地图加载步骤包括：首先根据车辆的定位信息，从高精度全局地图读取车辆当前车道所在全局子路径段和下一段全局子路径段，并将自车当前所在子路径段作为局部参考路径；其中，高精度全局地图提供的道路信息包括参考路径点、车道线、限速和道路数量。

障碍物信息处理步骤：由传感器实时获取道路障碍物信息，其余道路信息由高精度地图提供；障碍物信息在笛卡尔坐标系下以矩形框表示，包括长，宽，中心坐标点以及相对于参考线的方位角；

坐标系转化与分析步骤：将地图参考路径信息、自车和障碍物坐标信息从笛卡尔坐标系转换到曲线坐标系下，设置曲线坐标系下初始局部路径规划长度；再判断是否需要重规划，如果是则转到步骤决策与重规划方法，否则转到速度规划及局部规划更新步骤；

决策与重规划方法：首先根据自车运动信息、障碍物运动信息和地图信息，考虑安全性进行变道决策分析；其次根据决策信息结果选择是否变道，若是则进入可通行区域计算，否则跳转到速度规划及局部规划更新步骤；然后计算变道决策下的可通行区域并等间距离散此区域，得到变道子路径；再采用三次多项式曲线拟合自车当前位置与变道子路径，得到候选曲线簇；最后设计代价函数对候选曲线进行最优选择，代价最小的即为局部规划路径。

速度规划及局部规划更新：在局部规划路径或初始参考路径的基础上规划出相适应的速度，包含速度规划和局部规划路径更新两部分。

进一步的，所述局部路径匹配步骤具体包括以下步骤：

高精度地图数据包含当前驾驶路线的全局路径段，符号表示为S＝{S₁,S₂,S₃,…S_k}，即全局路径由k条全局子路径段S_i构成，每段子路径的起始点分别表示为(x_si,y_si)、(x_ei,y_ei)；

假设自车位于全局路径段车道内且各子路径段长度相近，根据车辆当前定位坐标(x₀,y₀)计算与各全局子路径段S_i中起始点的距离，找出距离最小的坐标点对应的全局子路径作为局部参考路径，并完成当前子路径段和下一段子路径的加载，距离计算方式如下：

其中D_i表示自车与子路径起始点的距离之和，全局最小D_i对应的子路径段S_i即为当前子路径段，下一段为S_i+1，如果i+1或i等于k，则不再计算，如此可实现路径的动态加载。

进一步的，所述障碍物信息处理包括如下子步骤：

(1)障碍物信息获取；

由感知模块实时检测道路障碍物信息，包含障碍物的位置、长宽和运动状态信息；

(2)障碍物信息表示；

障碍物信息在笛卡尔坐标系下以矩形框表示，包括长d_i，宽w_i，中心坐标点(x_i,y_i)以及相对于参考线的方位角θ_i，运动状态信息包括速度v₁和加速度a₁。

进一步的，所述决策与重规划步骤中，变道分析步骤具体：

首先根据自车运动状态，以自车为中心，将左右区域动态划分为3个子区域，编号从左到由分别为R₁、R₂、R₃，每个区域沿参考路径方向以自车定位点向前后延伸且长度相等，表示为E，此长度可根据传感器感知距离和车速动态获得，计算公式如下：

其中D_s为传感器的有效感知距离，V_a为自车车速，v_max为速度上限，D₀为区域初始距离；

自车可能处于R₁、R₂和R₃，符号表示为R_a，障碍物可能处于除R_a外的任何区域，可表示为R_o≠R_a and R_o∈{R}，R包含所有子区域即全集，分析各区域的占据情况和障碍物运动状态，得出变道、停车或者车道保持在内决策。

R₁和R₃区域变道安全距离的计算公式如下：

其中，L为自车质心至R₁或R₃区域障碍物的最小安全距离，V_a为自车的速度，v₁为R₁或R₃区域最近障碍物的速度，t_c为预碰撞时刻，预碰撞时刻为自车质心横坐标与障碍物宽度横坐标相等的时刻；假设自车质心与几何中心重合，w_a为自车质心至自车前端或尾端的距离，w_b为障碍物的长度，a₁为障碍物的加速度，D₁为预设距离常数，U为R₁和R₃区域障碍物距离自车最近的距离；R₁和R₃变道安全区域符号表示为Z∈[s₀-L,s₀+L]。

进一步的，当自车处于区域R₂，向左边变道分析如下：

从传感器获取R₂区域自车前方距离最近目标车辆运动状态，速度记为V_o2，自车速度记为V_a，自车与目标车辆的相对距离记为D_t，计算自车与目标车辆的发生碰撞所需时间(TTC)，假设碰撞时间阈值等级越低代表碰撞将在更短时间发生，碰撞时间定义为当前状态下主车与目标发生碰撞所需要的时间：

①若TTC大于2级碰撞时间阈值T₂或小于0，则认为当前车道可正常行驶，保持当前路径；

②若步骤①的判断结果为否，则判断TTC是否位于区间(T₁,T₂]，T₁为1级碰撞时间阈值；若否，则产生停车决策；

③若步骤②的判断结果为是，则判断R₁区域变道是否安全，若是，则产生向左变道决策；

④若步骤③判断结果为否，则保持当前路径；

根据决策结果，若为决策为变道，则规划长度Δs＝s_i–0.5w_c，s_i为前方障碍物中心在曲线坐标系下的横坐标，否则，自车道最小安全距离可通过变道安全距离的计算公式计算，此时规划长度Δs＝L；

(2)决策是否变道

根据上述变道分析的结果，可将车道保持、停车和变道划分为变道和不变道两个状态，作为判断下一步跳转的条件，若判断为是，则跳转到步骤3(3)；反之，则跳转到步骤4(1)。

进一步的，所述根据决策信息结果选择是否变道，若是则进入可通行区域计算，然后计算变道决策下的可通行区域并等间距离散此区域，得到变道子路径；再采用三次多项式曲线拟合自车当前位置与变道子路径，得到候选曲线簇；具体包括：

根据变道分析的结果、车道的宽度限制、障碍物的长宽和自车宽度信息，再结合设定的安全裕量常数D，可得到障碍物路段的可通行区域；在可通行区域内，按照曲线坐标系l轴方向固定间距Δd均匀采样偏移量，其s轴方向的长度与障碍物的长度d'_i相同，s轴采样开始坐标为Δs，如此，可得到等间距的平行直线段离散子路径；

(4)多项式曲线簇生成

采用三次多项式曲线拟合自车定位点与各离散子路径的端点，得到可通行区域的曲线簇，三次多项式曲线函数表达式如下：

l_i(s)＝c_0i+c_1is+c_2is²+c_3is³

式中l_i(s)为曲线坐标系下候选路径i的纵坐标，是曲线坐标系下参考路径横坐标的函数。下标i表示候选路径的标号，i＝1、2、…N；N为候选路径总数，{c_0i，c_1i，c_2i，c_3i}为曲线方程未知参数，由下述方程组求解到：

其中，s₀、l₀为局部规划路径的初始坐标，s_fi、l_fi为第i条候选路径终点坐标；tanθ₀为车辆初始航向角θ₀的正切值，tanθ_fi为第i条候选路径终点航向角的正切值，θ_fi＝0；

(5)最优路径选择

根据以下公式计算每条候选路径的代价，找出代价最小的作为最优局部规划路径；

目标函数f_i的计算公式为：

f_i＝w_pJ_p(l_i)+w_sJ_s(l_i)+w_dJ_d(l_i)

其中，w_p，w_s，w_d为对应的权重系数，J_p，J_s，J_d为相关指标项，f_i为各候选路径代价值，i＝1、2…N，N为总的候选路径数量；

指标项J_p为路径偏离障碍物指标，计算公式如下：

J_p(l_i)＝(S_max-|l_i(s_f)|)/S_max

其中，S_max为障碍物到道路边界的距离，l_i(s_f)为候选曲线终点的横向偏移，该指标反映了路径局部终点s_f偏离障碍物的程度；

指标J_s为横向运动平滑度指标，计算公式如下：

其中，s₀为局部规划路径的初始横坐标，k(l_i(s))为候选曲线上各离散点的曲率，k_max为车辆行驶最大转向曲率；

指标项J_d为路径偏离参考线指标，计算公式如下：

其中，l_i(s)为候选曲线的横向偏移坐标，D_max为最大偏移距离。

进一步的，所述速度规划步骤包括以下子步骤：

(1)速度规划

首先记录初状态，即为自车重规划开始时刻的速度和加速度；然后根据决策结果和是否需要重规划的判断结果，采样选择自车不同末状态(包含末时刻速度和加速度)，其具体选择方式如下：

①变道：根据初始时刻的速度和加速度作为参考，设置为末状态；

②车道保持：可根据前车的运动状态设置为末状态；

③停车；在不可变道和保持车道运行的情况下，前方为静止或极低速度障碍物，自车末状态速度和加速度均为0；

④若是否需要重规划的判断结果为否，末状态速度读取地图提供的参考速度，加速度为0即保持匀速运动；

根据上述信息采用三次多项式对速度进行平滑规划处理，其公式如下：

v(t)＝v₀+at+bt²+ct³

其中，初速度v₀为上次规划末时刻的速度，t为运行时间，v(t)为局部规划路径上各时刻的速度,{a,b,c,t_f}为未知参数，t_f为局部规划路径的末时刻，未知参数可由下述方程组求解得到：

其中，t₀、t_f为当前局部规划路径的初末时刻，t₀＝0，Δs为当前局部规划路径长度；a₀、a_f为初末时刻的加速度，v_f为末时刻的速度，可由上述自车末状态选择得到。

进一步的，所述局部规划路径更新步骤具体包括：

当前局部路径更新规则：

①自车行驶至当前局部路径的终点；

②障碍物出现在规划的局部路径附近且低于安全阈值。

本发明的优点及有益效果如下：

本发明提出一种结构化环境下的局部路径规划方法，通过对地图和障碍物信息的坐标转化，能降低横纵向运动规划的维度和简化障碍物与车辆的位置关系；通过对前方道路障碍物信息和地图信息的分析，进行决策与重规划，产生的决策将为可通行区域和速度规划提供参考，在可通行区域内得到离散子路径可保证不与障碍物碰撞，可减少子路径碰撞检查产生的额外开销；采用三次多项式曲线拟合当前车辆位置与可通行区域子路径，生成候选曲线簇，通过设计的代价函数得到最优曲线，在保证无碰撞的安全前提下，最优路径满足局部路径的平顺性和车辆最大转向曲率约束，提高了车辆运行的平稳性。综上，本发明的方法集路径规划、变道决策和速度规划于一体，能在保证无人驾驶安全性的同时，提高结构化环境下的智能车辆局部路径规划方法的时间性能。

附图说明

图1是本发明提供优选实施例提供的实施例结构化环境下的局部路径规划方法整体框架示意图；

图2为本发明地图与车辆定位结合加载局部参考路径的示意图；

图3为本发明实施例笛卡尔坐标系下的参考子路径及障碍物信息进行坐标系转化的示意图；

图4为本发明实施例提供的动态感知区域和变道安全区域示意图；

图5为本发明提供的实施例变道分析流程示意图；

图6为本发明实施例提供的障碍物路段可通行区域及其离散子路径生成方法示意图；

图7为本发明实施例提供的变道候选曲线簇生成示意图。

具体实施方式

下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、详细地描述。所描述的实施例仅仅是本发明的一部分实施例。

本发明解决上述技术问题的技术方案是：

如图1所示一种结构化环境下的局部路径规划方法具体包括以下步骤：

1.定位及地图加载。包括如下子步骤：

(1)获取高精度地图和车辆状态信息；

高精度地图数据包含当前驾驶路线的全局路径段，符号表示为S＝{S₁,S₂,S₃,…S_k}，即全局路径由k条全局子路径段S_i构成，S_i＝(x_j,y_j,Line_j，V_ref_j,W_j,Length_j)，全局子路径包含了路径点坐标(x_j,y_j)，道路数量Line_j，限速V_ref_j，路径长度Length_j以及道路宽度W_j，理想情况下若无任何交通参与者或障碍物，智能车辆将参考地图路径信息，不需局部规划便可运行至目的地；车辆状态包括智能车辆当前定位坐标(x₀,y₀)、速度V_a、加速度a₀和航向角θ_j。

(2)加载局部参考路径；

假设自车位于全局路径段车道内且各子路径段长度相近，根据车辆当前定位坐标计算与各全局子路径段中起始点的距离，找出距离最小的坐标点对应的全局子路径作为局部参考路径，并完成当前子路径段和下一段子路径的加载。

2.障碍物信息处理。包括如下子步骤：

(1)障碍物信息获取；

由感知模块实时检测道路障碍物信息，包含障碍物的位置、长宽和运动状态信息；

(2)障碍物信息表示；

障碍物信息在笛卡尔坐标系下以矩形框表示，包括长d_i，宽w_i，中心坐标点(x_i,y_i)以及相对于参考线的方位角θ_i，运动状态信息包括速度v₁和加速度a₁；

3.坐标系转化与分析。包括如下子步骤：

(1)坐标系转化；

①将局部参考路径坐标点和障碍物信息从笛卡尔坐标系转化到曲线坐标系

②障碍物坐标转换；根据障碍物中心点坐标最近的局部参考路径，将笛卡尔坐标系下的障碍物转化为曲线坐标系下。

(2)判断是否需要重规划

首先根据障碍物信息处理结果判断前方是否存在障碍物；其次根据高精度地图和车辆定位确定当前车道ID及车道数量。重规划的条件是：传感器感知当前车道存在速度低于自车的障碍物，且地图信息中当前车道允许变道，否则禁止进行重规划即保持当前车道。

4.决策与重规划方法。包括如下子步骤：

(1)变道分析

根据自车运动状态，以自车为中心，将车左右区域动态的划分为3个子区域，编号从左到由分别为R₁、R₂、R₃，每个区域沿参考路径方向以自车定位点向前后延伸且长度相等，此长度可根据传感器感知距离和车速动态计算。

根据传感器获取的障碍物运动信息，计算碰撞时间和可变道区域的变道安全距离，得出变道、停车或者车道保持等决策。

分析各区域的占据情况和障碍物运动状态，计算变道安全距离和碰撞时间，得出变道、停车或者车道保持等决策。

(2)决策是否变道

根据上述变道分析的结果，可将车道保持、停车和变道划分为变道和不变道两个状态，作为判断下一步跳转的条件。若判断为是，则跳转到计算可通行区域及离散子路径；反之，则跳转到速度规划。

(3)计算可通行区域及离散子路径

根据变道分析的结果、车道的宽度限制、障碍物的长宽和自车宽度信息，再结合设定的安全裕量常数D，可得到障碍物路段的可通行区域；在可通行区域内，按照曲线坐标系l轴方向固定间距Δd均匀采样偏移量，其s轴方向的长度与障碍物的长度d'_i相同，s轴采样开始坐标为Δs，如此，可得到等间距的平行直线段离散子路径。

(4)多项式曲线簇生成

采用三次多项式曲线拟合自车定位点与各离散子路径的端点，得到可通行区域的曲线簇。

(5)最优路径选择

根据以下公式计算每条候选路径的代价，找出代价最小的作为最优局部规划路径。

目标函数f_i的计算公式为：

f_i＝w_pJ_p(l_i)+w_sJ_s(l_i)+w_dJ_d(l_i) (9)

其中，w_p，w_s，w_d为对应的权重系数，J_p，J_s，J_d为相关指标项，f_i为各候选路径代价值，i＝1、2…N，N为总的候选路径数量。

指标项J_p为路径偏离障碍物指标，计算公式如下：

J_p(l_i)＝(S_max-|l_i(s_f)|)/S_max (10)

其中，S_max为障碍物到道路边界的距离，l_i(s_f)为候选曲线终点的横向偏移，该指标反映了路径局部终点s_f偏离障碍物的程度；

指标J_s为横向运动平滑度指标，计算公式如下：

其中，s₀为局部规划路径的初始横坐标，k(l_i(s))为候选曲线上各离散点的曲率，k_max为车辆行驶最大转向曲率；

指标项J_d为路径偏离参考线指标，计算公式如下：

其中，l_i(s)为候选曲线的横向偏移坐标，D_max为最大偏移距离。

5.速度规划及局部规划更新。具体的，如图1第五个虚线框所示，包括如下子步骤：

(1)速度规划

首先记录初状态，即为自车重规划开始时刻的速度和加速度；然后根据决策结果和是否需要重规划的判断结果，采样选择自车不同末状态(包含末时刻速度和加速度)，其具体选择方式如下：

①变道。根据初始时刻的速度和加速度作为参考，设置为末状态。

②车道保持。可根据前车的运动状态设置为末状态。

③停车。在不可变道和保持车道运行的情况下，前方为静止或极低速度障碍物，自车末状态速度和加速度均为0。

④若是否需要重规划的判断结果为否，末状态速度读取地图提供的参考速度，加速度为0即保持匀速运动。

根据上述信息采用三次多项式对速度进行平滑规划处理，其公式如下：

v(t)＝v₀+at+bt²+ct³ (13)

其中，初速度v₀为上次规划末时刻的速度，t为运行时间，v(t)为局部规划路径上各时刻的速度,{a,b,c,t_f}为未知参数，t_f为局部规划路径的末时刻，未知参数可由下述方程组求解得到：

其中，t₀、t_f为当前局部规划路径的初末时刻，t₀＝0，Δs为当前局部规划路径长度；a₀、a_f为初末时刻的加速度，v_f为末时刻的速度，可由上述自车末状态选择得到。

(2)局部路径更新

当前局部路径更新规则：

①自车行驶至当前局部路径的终点；

②障碍物出现在规划的局部路径附近且低于安全阈值；

如图2所示为地图与车辆定位结合加载局部参考路径的示意图，高精度地图数据包含当前驾驶路线的全局路径段，符号表示为S＝{S₁,S₂,S₃,…S_k}，即全局路径由k条全局子路径段S_i构成，每段子路径的起始点分别表示为(x_si,y_si)、(x_ei,y_ei)。

进一步的，假设自车位于全局路径段车道内且各子路径段长度相近，根据车辆当前定位坐标(x₀,y₀)计算与各全局子路径段S_i中起始点的距离，找出距离最小的坐标点对应的全局子路径作为局部参考路径，并完成当前子路径段和下一段子路径的加载。距离计算方式如下：

其中D_i表示自车与子路径起始点的距离之和，全局最小D_i对应的子路径段S_i即为当前子路径段，下一段为S_i+1，如果i+1或i等于k，则不再计算。如此可实现路径的动态加载，特别是在地图数据较大时，可节约存储资源；在遭遇系统重启等事件后，可快速实现当前路径段的定位和加载。

如图3所示为笛卡尔坐标系下的参考子路径及障碍物信息进行坐标系转化的示意图，其方法包括:

①局部参考路径及自车坐标系转换；以局部参考路径道路长度s为横坐标，以垂直于参考路径的横向偏移量l为纵坐标，将笛卡尔坐标系下的局部参考路径Local_waypoints(j)＝(x_j,y_j,W,s_j,θ_j)转换为以局部参考路径为基准的曲线坐标系Local_waypoints(j)＝(s_j,l_j)，参考路径一般偏移l_j取0；自车定位点(x₀,y₀)转换为曲线坐标系下的坐标为(s₀,l₀)。

②障碍物坐标转换；根据障碍物中心点坐标最近的局部参考路径Local_waypoints(j)＝(x_j,y_j,W,s_j,θ_j)，将笛卡尔坐标系下的障碍物obs(i)＝(x_i,y_i,d_i,w_i,θ_i)转化为曲线坐标系下的obs(i)＝(s_i,l_i,d'_i,w'_i)，长宽转换公式如下：

d'_i＝w_i*sin(θ_i-θ_j)+d_i*cos(θ_i-θ_j) (2)

w'_i＝w_i*cos(θ_i-θ_j)+d_i*sin(θ_i-θ_j) (3)

上述参数中，s_i为障碍物对应参考路径最近点的弧长，l_i为障碍物中心距离参考线最近的距离，d'_i和w'_i为转换到曲线坐标系下对应障碍物的长宽。

如图4所示为动态感知区域和变道安全区域示意图，首先根据自车运动状态，以自车为中心，将车左右区域动态的划分为3个子区域，编号从左到由分别为R₁、R₂、R₃，每个区域沿参考路径方向以自车定位点向前后延伸且长度相等，表示为E，此长度可根据传感器感知距离和车速动态获得，计算公式如下：

其中D_s为传感器的有效感知距离，V_a为自车车速，v_max为速度上限，D₀为区域初始距离。

自车可能处于R₁、R₂和R₃，符号表示为R_a，障碍物可能处于除R_a外的任何区域，可表示为R_o≠R_aandR_o∈{R}，R包含所有子区域即全集。分析各区域的占据情况和障碍物运动状态，得出变道、停车或者车道保持等决策。

R₁和R₃区域变道安全距离的计算公式如下：

其中，L为自车质心至R₁或R₃区域障碍物的最小安全距离，V_a为自车的速度，v₁为R₁或R₃区域最近障碍物的速度，t_c为预碰撞时刻，预碰撞时刻为自车质心纵向位移与障碍物宽度相等的时刻；假设自车质心与几何中心重合，w_a为自车质心至自车前端或尾端的距离，w_b为障碍物的长度，a₁为障碍物的加速度，D₁为预设距离常数，U为R₁和R₃区域障碍物距离自车最近的距离；R₁和R₃变道安全区域符号表示为Z∈[s₀-L,s₀+L]。

如图5所示为变道分析流程示意图，自车处于区域R₂，向左边变道流程如图5所示，判断如下：

从传感器获取R₂区域自车前方距离最近目标车辆运动状态，速度记为V_o2，自车速度记为V_a，自车与目标车辆的相对距离记为D_t，计算自车与目标车辆的发生碰撞所需时间(TTC)，假设碰撞时间阈值等级越低代表碰撞将在更短时间发生。碰撞时间定义为当前状态下主车与目标发生碰撞所需要的时间：

①若TTC大于2级碰撞时间阈值T₂或小于0，则认为当前车道可正常行驶，保持当前路径。

②若步骤①的判断结果为否，则判断TTC是否位于区间(T₁,T₂]，T₁为1级碰撞时间阈值；若否，则产生停车决策。

③若步骤②的判断结果为是，则判断R₁区域变道是否安全，若是，则产生向左变道决策。

④若步骤③判断结果为否，则保持当前路径。

根据决策结果，若为决策为变道，则规划长度Δs＝s_i–0.5w_c，s_i为前方障碍物中心在曲线坐标系下的横坐标。否则，自车道最小安全距离可通过变道安全距离的计算公式计算，此时规划长度Δs＝L。

如图6所示为障碍物路段可通行区域及其离散子路径生成方法示意图，首先根据变道分析的结果、车道的宽度限制、障碍物的长宽和自车宽度信息，再结合设定的安全裕量常数D，可得到障碍物路段的可通行区域；在可通行区域内，按照曲线坐标系l轴方向固定间距Δd均匀采样偏移量，其s轴方向的长度与障碍物的长度d'_i相同，s轴采样开始坐标为Δs，如此，可得到等间距的平行直线段离散子路径。

如图7所示为变道候选曲线簇生成示意图，采用三次多项式曲线拟合自车定位点与各离散子路径的端点，得到可通行区域的曲线簇，三次多项式曲线函数表达式如下：

l_i(s)＝c_0i+c_1is+c_2is²+c_3is³ (7)

式中l_i(s)为曲线坐标系下候选路径i的纵坐标，是曲线坐标系下参考路径横坐标的函数。下标i表示候选路径的标号，i＝1、2、…N；N为候选路径总数。{c_0i，c_1i，c_2i，c_3i}为曲线方程未知参数，由下述方程组求解到：

其中，s₀、l₀为局部规划路径的初始坐标，s_fi、l_fi为第i条候选路径终点坐标；tanθ₀为车辆初始航向角θ₀的正切值，tanθ_fi为第i条候选路径终点航向角的正切值，本申请中θ_fi＝0。

以上这些实施例应理解为仅用于说明本发明而不用于限制本发明的保护范围。在阅读了本发明的记载的内容之后，技术人员可以对本发明作各种改动或修改，这些等效变化和修饰同样落入本发明权利要求所限定的范围。

Claims (4)

1.一种结构化环境下的智能车辆局部路径规划方法，其特征在于，包括以下步骤：

定位及地图加载步骤；障碍物信息处理步骤；坐标系转化与分析步骤；决策与重规划步骤；速度规划及局部规划更新步骤；其中，

定位及地图加载步骤包括：首先根据车辆的定位信息，从高精度全局地图读取车辆当前车道所在全局子路径段和下一段全局子路径段，并将自车当前所在子路径段作为局部参考路径；其中，高精度全局地图提供的道路信息包括参考路径点、车道线、限速和道路数量。

障碍物信息处理步骤：由传感器实时获取道路障碍物信息，其余道路信息由高精度地图提供；障碍物信息在笛卡尔坐标系下以矩形框表示，包括长，宽，中心坐标点以及相对于参考线的方位角；

坐标系转化与分析步骤：将地图参考路径信息、自车和障碍物坐标信息从笛卡尔坐标系转换到曲线坐标系下，设置曲线坐标系下初始局部路径规划长度；再判断是否需要重规划，如果是则转到步骤决策与重规划方法，否则转到速度规划及局部规划更新步骤；

决策与重规划方法：首先根据自车运动信息、障碍物运动信息和地图信息，考虑安全性进行变道决策分析；其次根据决策信息结果选择是否变道，若是则进入可通行区域计算，否则跳转到速度规划及局部规划更新步骤；然后计算变道决策下的可通行区域并等间距离散此区域，得到变道子路径；再采用三次多项式曲线拟合自车当前位置与变道子路径，得到候选曲线簇；最后设计代价函数对候选曲线进行最优选择，代价最小的即为局部规划路径。

速度规划及局部规划更新：在局部规划路径或初始参考路径的基础上规划出相适应的速度，包含速度规划和局部规划路径更新两部分。

2.根据权利要求1所述的一种结构化环境下的智能车辆局部路径规划方法，其特征在于，所述局部路径匹配步骤具体包括以下步骤：

高精度地图数据包含当前驾驶路线的全局路径段，符号表示为S＝{S₁,S₂,S₃,…S_k}，即全局路径由k条全局子路径段S_i构成，每段子路径的起始点分别表示为(x_si,y_si)、(x_ei,y_ei)；

假设自车位于全局路径段车道内且各子路径段长度相近，根据车辆当前定位坐标(x₀,y₀)计算与各全局子路径段S_i中起始点的距离，找出距离最小的坐标点对应的全局子路径作为局部参考路径，并完成当前子路径段和下一段子路径的加载，距离计算方式如下：

其中D_i表示自车与子路径起始点的距离之和，全局最小D_i对应的子路径段S_i即为当前子路径段，下一段为S_i+1，如果i+1或i等于k，则不再计算，如此可实现路径的动态加载。

3.根据权利要求1所述的一种结构化环境下的局部路径规划方法及系统，其特征在于，所述决策与重规划方法包括以下子步骤：

(1)变道分析

首先根据自车运动状态，以自车为中心，将左右区域动态划分为3个子区域，编号从左到由分别为R₁、R₂、R₃，每个区域沿参考路径方向以自车定位点向前后延伸且长度相等，表示为E，此长度可根据传感器感知距离和车速动态获得，计算公式如下：

其中D_s为传感器的有效感知距离，V_a为自车车速，v_max为速度上限，D₀为区域初始距离。

自车可能处于R₁、R₂和R₃，符号表示为R_a，障碍物可能处于除R_a外的任何区域，可表示为R_o≠R_a and R_o∈{R}，R包含所有子区域即全集。分析各区域的占据情况和障碍物运动状态，得出变道、停车或者车道保持等决策。

R₁和R₃区域变道安全距离的计算公式如下：

其中，L为自车质心至R₁或R₃区域障碍物的最小安全距离，V_a为自车的速度，v₁为R₁或R₃区域最近障碍物的速度，t_c为预碰撞时刻，预碰撞时刻为自车质心横坐标与障碍物宽度横坐标相等的时刻；假设自车质心与几何中心重合，w_a为自车质心至自车前端或尾端的距离，w_b为障碍物的长度，a₁为障碍物的加速度，D₁为预设距离常数，U为R₁和R₃区域障碍物距离自车最近的距离；R₁和R₃变道安全区域符号表示为Z∈[s₀-L,s₀+L]。

自车处于区域R₂，向左边变道分析如下：

从传感器获取R₂区域自车前方距离最近目标车辆运动状态，速度记为V_o2，自车速度记为V_a，自车与目标车辆的相对距离记为D_t，计算自车与目标车辆的发生碰撞所需时间(TTC)，假设碰撞时间阈值等级越低代表碰撞将在更短时间发生。碰撞时间定义为当前状态下主车与目标发生碰撞所需要的时间：

①若TTC大于2级碰撞时间阈值T₂或小于0，则认为当前车道可正常行驶，保持当前路径。

②若步骤①的判断结果为否，则判断TTC是否位于区间(T₁,T₂]，T₁为1级碰撞时间阈值；若否，则产生停车决策。

③若步骤②的判断结果为是，则判断R₁区域变道是否安全，若是，则产生向左变道决策。

④若步骤③判断结果为否，则保持当前路径。

根据决策结果，若为决策为变道，则规划长度Δs＝s_i–0.5w_c，s_i为前方障碍物中心在曲线坐标系下的横坐标。否则，自车道最小安全距离可通过变道安全距离的计算公式计算，此时规划长度Δs＝L。

(2)决策是否变道

根据上述变道分析的结果，可将车道保持、停车和变道划分为变道和不变道两个状态，作为判断下一步跳转的条件。若判断为是，则跳转到步骤3(3)；反之，则跳转到步骤4(1)。

(3)计算可通行区域及离散子路径

根据变道分析的结果、车道的宽度限制、障碍物的长宽和自车宽度信息，再结合设定的安全裕量常数D，可得到障碍物路段的可通行区域；在可通行区域内，按照曲线坐标系l轴方向固定间距Δd均匀采样偏移量，其s轴方向的长度与障碍物的长度d'_i相同，s轴采样开始坐标为Δs，如此，可得到等间距的平行直线段离散子路径。

(4)多项式曲线簇生成

采用三次多项式曲线拟合自车定位点与各离散子路径的端点，得到可通行区域的曲线簇，三次多项式曲线函数表达式如下：

l_i(s)＝c_0i+c_1is+c_2is²+c_3is³

式中l_i(s)为曲线坐标系下候选路径i的纵坐标，是曲线坐标系下参考路径横坐标的函数。下标i表示候选路径的标号，i＝1、2、…N；N为候选路径总数。{c_0i，c_1i，c_2i，c_3i}为曲线方程未知参数，由下述方程组求解到：

其中，s₀、l₀为局部规划路径的初始坐标，s_fi、l_fi为第i条候选路径终点坐标；tanθ₀为车辆初始航向角θ₀的正切值，tanθ_fi为第i条候选路径终点航向角的正切值，本申请中θ_fi＝0。

(5)最优路径选择

根据以下公式计算每条候选路径的代价，找出代价最小的作为最优局部规划路径。

目标函数f_i的计算公式为：

f_i＝w_pJ_p(l_i)+w_sJ_s(l_i)+w_dJ_d(l_i)

其中，w_p，w_s，w_d为对应的权重系数，J_p，J_s，J_d为相关指标项，f_i为各候选路径代价值，i＝1、2…N，N为总的候选路径数量。

指标项J_p为路径偏离障碍物指标，计算公式如下：

J_p(l_i)＝(S_max-|l_i(s_f)|)/S_max

其中，S_max为障碍物到道路边界的距离，l_i(s_f)为候选曲线终点的横向偏移，该指标反映了路径局部终点s_f偏离障碍物的程度；

指标J_s为横向运动平滑度指标，计算公式如下：

其中，s₀为局部规划路径的初始横坐标，k(l_i(s))为候选曲线上各离散点的曲率，k_max为车辆行驶最大转向曲率；

指标项J_d为路径偏离参考线指标，计算公式如下：

其中，l_i(s)为候选曲线的横向偏移坐标，D_max为最大偏移距离。

4.根据权利要求1所述的一种结构化环境下的局部路径规划方法及系统，其特征在于，所述速度规划及更新局部规划结果包括以下子步骤：

(1)速度规划

首先记录初状态，即为自车重规划开始时刻的速度和加速度；然后根据决策结果和是否需要重规划的判断结果，采样选择自车不同末状态(包含末时刻速度和加速度)，其具体选择方式如下：

①变道：根据初始时刻的速度和加速度作为参考，设置为末状态。

②车道保持：可根据前车的运动状态设置为末状态。

③停车：在不可变道和保持车道运行的情况下，前方为静止或极低速度障碍物，自车末状态速度和加速度均为0。

④若是否需要重规划的判断结果为否，末状态速度读取地图提供的参考速度，加速度为0即保持匀速运动。

根据上述信息采用三次多项式对速度进行平滑规划处理，其公式如下：

v(t)＝v₀+at+bt²+ct³

其中，初速度v₀为上次规划末时刻的速度，t为运行时间，v(t)为局部规划路径上各时刻的速度,{a,b,c,t_f}为未知参数，t_f为局部规划路径的末时刻，未知参数可由下述方程组求解得到：

其中，t₀、t_f为当前局部规划路径的初末时刻，t₀＝0，Δs为当前局部规划路径长度；a₀、a_f为初末时刻的加速度，v_f为末时刻的速度，可由上述自车末状态选择得到。

(2)局部路径更新

当前局部路径的更新触发的规则有：

①自车行驶至当前局部路径的终点；

②障碍物出现在规划的局部路径附近且低于安全阈值。

Images