CN116215569B - 一种基于行车风险评估的自动驾驶汽车规划方法及系统 - Google Patents

Abstract

本发明涉及一种基于行车风险评估的自动驾驶汽车规划方法及系统，包括：获取周围车辆在世界坐标系下的历史运动轨迹,采用交互式多车道模型算法分析周围车辆预测时域内在各车道的概率分布，融合计算出车辆的目标位置；根据周围车辆当前位置及下一时刻目标位置，拟合生成预测时域内的预测轨迹；根据所获得的预测轨迹，将预测时域内的全局路径跟踪偏差、碰撞风险、本车车辆控制转角的变化作为代价目标建立自动驾驶车辆模型的非线性优化函数，求解获得局部最优轨迹；采用运动控制目标函数来进行局部最佳轨迹跟踪控制，将计算的当前时刻最优车辆转角输入车辆动力学模型，实现车辆避障和循迹行驶。本发明提高了循迹和避障的准确性和实时性。

Description

一种基于行车风险评估的自动驾驶汽车规划方法及系统

技术领域

本发明涉及车辆运动预测技术领域，尤其是一种基于行车风险评估的自动驾驶汽车规划方法及系统。

背景技术

自动驾驶汽车和先进的驾驶辅助系统因在高效、安全和清洁的交通系统中显示出显着的优势，越来越受到关注。轨迹规划是自动驾驶汽车系统的重要组成部分，直接影响自动驾驶交通安全。但目前保证自动驾驶汽车在复杂和不确定的交通场景中做出准确、高效、安全的决策和规划仍然是一个挑战。基于碰撞风险评估的有效运动规划方法，通过对候选路径进行风险评估，实时优化获取最优路径，可有效降低行车风险。

风险评估需要对周围车辆未来时域进行运动预测。目前针对车辆运动预测主要有基于学习的和基于物理模型的方法。基于学习模型主要依赖于机器学习方法，模型需要大量数据训练并且缺乏一定的解释性；而基于物理的运动模型由于长预测范围内的准确性较低且不适合复杂交通场景中的风险评估。

轨迹规划方面，针对目前大多数规划控制方法在运动规划层采用的运动学模型和跟踪控制层中车辆非线性动力学模型不一致，这可能导致车辆的控制效果偏差、甚至失稳的缺点。

综上所述，面对当前复杂和不确定性的交通场景，提出一种能够基于行车风险评估的自动驾驶汽车运动规划控制方法极为重要。

发明内容

针对现有技术的不足，本发明提供一种基于行车风险评估的自动驾驶汽车规划方法及系统，目的是在不确定性的风险场景下实现高效的智能驾驶。

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

本申请提供一种基于行车风险评估的自动驾驶汽车规划方法，包括：

感知预测层，获取周围车辆在世界坐标系下的历史运动轨迹,采用交互式多模型算法分析周围车辆预测时域内在各车道的概率分布，融合计算出车辆的目标位置；根据周围车辆当前位置及下一时刻目标位置，拟合生成预测时域内的预测轨迹；

轨迹重规划层，根据所述预测轨迹，将预测时域内的全局路径跟踪偏差、碰撞风险、本车车辆控制转角的变化作为代价目标建立自动驾驶车辆模型的非线性优化函数，求解获得局部最优轨迹；

跟踪控制层，采用运动控制目标函数来进行局部最佳轨迹跟踪控制，将计算的当前时刻最优车辆转角输入所述车辆动力学模型，实现车辆避障和循迹行驶。

进一步技术方案为：

所述采用交互式多模型算法分析周围车辆预测时域内在各车道的概率分布，预测得到周围车辆下一时刻横向位置，包括：

建立车道模型：

式中，S_k代表k时刻各车道的状态序列，i代表车道编号，n是总车道数，分别为k时刻下i车道的状态值、协方差以及置信概率；

计算混合概率和混合转移概率；

式中，混合概率表示从k-1时刻至k时刻周围车辆处于j车道的概率，混合转移概率/>表示从k-1时刻至k时刻周围车辆在i车道与j车道之间相互转移的概率；π_ij为状态转移矩阵中对应于i车道与j车道之间转移的概率，所述状态转移矩阵是由各车道两两之间的转移概率构成的矩阵；

计算混合状态和混合协方差：

式中，混合状态混合协方差/>分别表示从k-1时刻至k时刻周围车辆处于j车道的更新后的状态值、以及周围车辆处于j车道的更新后的不确定度；

获得各车道的似然估计：

式中，表示j车道的似然估计，输入是混合状态/>与观测值z_k的差值/>R为观测噪声，q_k为k时刻的总状态估计值；

根据所述似然估计对车道模型概率更新获得后验概率

计算各车道的总状态估计值q_k和总协方差P_k：

总状态估计值q_k和总协方差P_k分别代表各车道状态值及协方差的加权平均值，上式中，T代表矩阵转置式中。

采用交互式多模型算法分析周围车辆预测时域内在各车道的概率分布时，假设纵向为匀速运动。因此根据周围车辆当前位置及下一时刻横向位置，拟合生成预测时域内的预测轨迹，包括：

采用如下三次多项式进行拟合：

设置起始和终端航向角边界条件如下：

式中，(x_k,y_k)表示k时刻周围车辆的当前位置，表示k时刻周围车辆偏航角，(x_p,y_p)表示p时刻预测的周围车辆位置，a_i为系数。

所述自动驾驶车辆模型的非线性目标函数：

式中，第一项是指通过最小化参考路径跟踪误差来跟踪参考轨迹，y_e(t+k,t)为本车预测时域的横向坐标，y_des(t+k,t)为全局路径，t代表当前时刻，k代表未来采样时刻。第二项表示最小化本车和周围车辆之间的碰撞风险，N表示周围车辆总数，N_p总数表示预测步长，为风险碰撞评估函数,/>分别为本车和周围车辆之间的纵向距离、横向距离，γ为衰减因子，α₁、α₂分别权重因子，ζ为系数，第三项是指最小化控制输入量消耗u。

所述运动控制目标函数及其约束条件为：

其中，y_ref,local(t+i)为局部参考轨迹序列，Δu(t+i)表示控制增量，Q、R、P为权重矩阵，ε为权重因子，AΔU_t+U_t，U_max，U_min分别为控制量及其上、下限，ΔU_t，ΔU_max，ΔU_min分别为控制量增加量及其上、下限，y_hc、y_hc,max、y_hc,min分别是输出量横向位置及其上、下限。

本申请还提供一种基于行车风险评估的自动驾驶汽车规划系统，包括：

感知预测层，获取周围车辆在世界坐标系下的历史运动轨迹,采用交互式多模型算法分析周围车辆预测时域内在各车道的概率分布，融合计算出车辆的目标位置；根据周围车辆当前位置及下一时刻目标位置，拟合生成预测时域内的预测轨迹；

轨迹重规划层，根据所述预测轨迹，将预测时域内的全局路径跟踪偏差、碰撞风险、本车车辆控制转角的变化作为代价目标建立自动驾驶车辆模型的非线性优化函数，求解获得局部最优轨迹；

跟踪控制层，采用运动控制目标函数来进行局部最佳轨迹跟踪控制，将计算的当前时刻最优车辆转角输入所述车辆动力学模型，实现车辆避障和循迹行驶。

本发明的有益效果如下：

通过对复杂行驶场景提出一种碰撞风险评估算法，建立本车安全高效的运行规划策略，从而有效考虑到场景的不确定性对本车运动控制及安全的影响。

将运动规划层和跟踪控制层分层处理的汽车规划框架，可分别进行任务处理和目标优化，提高算法的解算实时性和拓展性能。

本发明的其它特征和优点将在随后的说明书中阐述，并且，部分地从说明书中变得显而易见，或者通过实施本发明而了解。

附图说明

图1为本发明实施例的方法流程图。

图2为本发明实施例采用的交互式多模型算法的原理图。

具体实施方式

以下结合附图说明本发明的具体实施方式。

参见图1，本申请提供一种基于行车风险评估的自动驾驶汽车规划方法，包括：

一种基于行车风险评估的自动驾驶汽车规划方法，其特征在于，包括：

感知预测层，获取周围车辆在世界坐标系下的历史运动轨迹,采用交互式多模型算法分析周围车辆预测时域内在各车道的概率分布，融合计算出车辆的目标位置；根据周围车辆当前位置及下一时刻目标位置，拟合生成预测时域内的预测轨迹；

轨迹重规划层，根据所述预测轨迹，将预测时域内的全局路径跟踪偏差、碰撞风险、本车车辆控制转角的变化作为代价目标建立自动驾驶车辆模型的非线性优化函数，求解获得局部最优轨迹；

跟踪控制层，采用运动控制目标函数来进行局部最佳轨迹跟踪控制，将计算的当前时刻最优车辆转角输入所述车辆动力学模型，实现车辆避障和循迹行驶。

本申请的规划方法将周围车辆的横向位置预测转化为对各车道模型的概率识别，采用交互式多模型算法分析周围车辆未来预测时域的各车道概率分布，进而预测横向位置，继而获得周围车辆的预测轨迹；基于周围车辆的预测轨迹，将全局路径偏差、碰撞风险、控制量变化作为轨迹重规划层中的控制目标，采用非线性二次规划生成安全可靠的局部轨迹；采用以模型预测控制为框架的运动控制目标函数来进行跟踪控制。本申请的规划方法基于周围车辆的预测轨迹进行轨迹规划和跟踪控制，提高了循迹和避障的准确性和实时性。

本申请中“周围车辆”是指周围的某一车辆，“本车”是指待规划路径的自动驾驶车辆。

参见图2，所述采用交互式多模型算法分析周围车辆预测时域内在各车道的概率分布，预测得到周围车辆下一时刻横向位置，包括模型初始化、过滤、模型概率更新和估计融合，具体步骤为：

建立车道模型：

式中，S_k代表k时刻各车道的状态序列，i代表车道编号，n是总车道数，分别为k时刻下i车道的状态值、协方差以及置信概率，其中，状态值代表车道的横向坐标，协方差代表车道的不确定度；

计算混合概率和混合转移概率；

式中，混合概率表示从k-1时刻至k时刻周围车辆处于j车道的概率，混合转移概率/>表示从k-1时刻至k时刻周围车辆在i车道与j车道之间相互转移的概率；π_ij为状态转移矩阵中对应于i车道与j车道之间转移的概率，所述状态转移矩阵是由所有车道两两之间的转移概率构成的矩阵；

计算混合状态和混合协方差：

式中，混合状态混合协方差/>分别表示从k-1时刻至k时刻周围车辆处于j车道的更新后的状态值、以及周围车辆处于j车道的更新后的不确定度；

获得各车道的似然估计：

式中，表示j车道的似然估计，输入是混合状态/>与观测值z_k的差值/>R为观测噪声，q_k为k时刻的总状态估计值。由于混合状态/>和观测值z_k均服从高斯分布，因此差值/>的协方差为混合状态值协方差/>与观测值协方差R之和，但这里本申请注重对车道模型概率的估计，假设从传感器获得的观测值z_k不存在噪声方差R，固本实施例假设R为0。

根据所述似然估计对车道模型概率更新获得后验概率

计算各车道的总状态估计值q_k和总协方差P_k：

总状态估计值q_k和总协方差P_k分别代表各车道状态值及协方差的加权平均值，式中，T代表矩阵转置。

其中，采用交互式多模型算法分析周围车辆预测时域内在各车道的概率分布，预测得到周围车辆下一时刻横向位置时，假使纵向为匀速运动。

所述根据周围车辆当前位置及下一时刻横向位置，拟合生成预测时域内的预测轨迹，包括：

采用如下三次多项式进行拟合：

设置起始和终端航向角边界条件如下：

式中，(x_k,y_k)表示k时刻周围车辆的当前位置，表示k时刻周围车辆偏航角，(x_p,y_p)表示p时刻预测的周围车辆位置，a_i为系数。

所述自动驾驶车辆模型的非线性目标函数：

式中，第一项是指通过最小化参考路径跟踪误差来跟踪参考轨迹，y_e(t+k,t)为本车预测时域的横向坐标，y_des(t+k,t)为全局路径,t代表当前时刻，k代表未来采样时刻。第二项表示最小化本车和周围车辆之间的碰撞风险，N表示周围车辆总数，N_p总数表示预测步长，为风险碰撞评估函数,/>分别为本车和周围车辆之间的纵向距离、横向距离，γ为衰减因子，α₁、α₂分别权重因子，ζ为系数，防止分母为零造成计算问题，第三项是指最小化控制输入量消耗u。

所述运动控制目标函数及其约束条件为：

其中，y_ref,local(t+i)为局部参考轨迹序列，Δu(t+i)表示控制增量，Q、R、P为权重矩阵，ε为权重因子，AΔU_t+U_t，U_max，U_min分别为控制量及其上、下限，ΔU_t，ΔU_max，ΔU_min分别为控制量增加量及其上、下限，y_hc、y_hc,max、y_hc,min分别是输出量横向位置及其上、下限

本申请还提供一种基于行车风险评估的自动驾驶汽车规划系统，包括：

感知预测层，获取周围车辆在世界坐标系下的历史运动轨迹,采用交互式多模型算法分析周围车辆预测时域内在各车道的概率分布，融合计算出车辆的目标位置；根据周围车辆当前位置及下一时刻目标位置，拟合生成预测时域内的预测轨迹；

轨迹重规划层，根据所述预测轨迹，将预测时域内的全局路径跟踪偏差、碰撞风险、本车车辆控制转角的变化作为代价目标建立自动驾驶车辆模型的非线性优化函数，求解获得局部最优轨迹；

跟踪控制层，采用运动控制目标函数来进行局部最佳轨迹跟踪控制，将计算的当前时刻最优车辆转角输入所述车辆动力学模型，实现车辆避障和循迹行驶。

本领域普通技术人员可以理解：以上所述仅为本发明的优选实施例而已，并不用于限制本发明，尽管参照前述实施例对本发明进行了详细的说明，对于本领域的技术人员来说，其依然可以对前述各实施例记载的技术方案进行修改，或者对其中部分技术特征进行等同替换。凡在本发明的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。

Claims (4)

1.一种基于行车风险评估的自动驾驶汽车规划方法，其特征在于，包括：

感知预测层，获取周围车辆在世界坐标系下的历史运动轨迹，采用交互式多模型算法分析周围车辆预测时域内在各车道的概率分布，融合计算出车辆的目标位置；根据周围车辆当前位置及下一时刻目标位置，拟合生成预测时域内的预测轨迹；

轨迹重规划层，基于所述预测轨迹，将预测时域内的全局路径跟踪偏差、碰撞风险、本车车辆控制转角的变化作为代价目标建立自动驾驶车辆模型的非线性优化函数，求解获得局部最优轨迹；

跟踪控制层，采用运动控制目标函数来进行局部最佳轨迹跟踪控制，将计算的当前时刻最优车辆转角输入所述车辆动力学模型，实现车辆避障和循迹行驶；

所述自动驾驶车辆模型的非线性目标函数：

式中，第一项是指通过最小化参考路径跟踪误差来跟踪参考轨迹，y_e(t+k,t)为本车预测时域的横向坐标，y_des(t+k,t)为全局路径,t代表当前时刻，k代表未来采样时刻；第二项表示最小化本车和周围车辆之间的碰撞风险，N表示周围车辆总数，N_p表示预测步长，为风险碰撞评估函数,/>分别为本车和周围车辆之间的纵向距离、横向距离，γ为衰减因子，α₁、α₂分别权重因子，ζ为系数；第三项是指最小化控制输入量消耗u；

所述运动控制目标函数及其约束条件为：

s.t.ΔU_min≤ΔU_t≤ΔU_max

U_min≤AΔU_t+U_t≤U_max

y_hc,min≤y_hc≤y_hc,max

其中，y_ref,local(t+i)为局部参考轨迹序列，Δu(t+i)表示控制增量，Q、R、P为权重矩阵，ε为权重因子，AΔU_t+U_t，U_max，U_min分别为控制量及其上、下限，ΔU_t，ΔU_max，ΔU_min分别为控制量增加量及其上、下限，y_hc、y_hc,max、y_hc,min分别是输出量横向位置及其上、下限。

2.根据权利要求1所述的基于行车风险评估的自动驾驶汽车规划方法，其特征在于，所述采用交互式多模型算法分析周围车辆预测时域内在各车道的概率分布，预测得到周围车辆下一时刻横向位置，包括：

建立车道模型：

式中，S_k代表k时刻各车道的状态序列，i代表车道编号，n是总车道数，分别为k时刻下i车道的状态值、协方差以及置信概率；

计算混合概率和混合转移概率：

式中，混合概率表示从k-1时刻至k时刻周围车辆处于j车道的概率，混合转移概率/>表示从k-1时刻至k时刻周围车辆在i车道与j车道之间相互转移的概率；π_ij为状态转移矩阵中对应于i车道与j车道之间转移的概率，所述状态转移矩阵是由各车道两两之间的转移概率构成的矩阵；

计算混合状态和混合协方差：

式中，混合状态混合协方差/>分别表示从k-1时刻至k时刻周围车辆处于j车道的更新后的状态值、以及周围车辆处于j车道的更新后的不确定度；

获得各车道的似然估计：

式中，表示j车道的似然估计，输入是混合状态/>与观测值z_k的差值/>R为观测噪声，q_k为k时刻的总状态估计值；

根据所述似然估计对车道模型概率更新获得后验概率

计算各车道的总状态估计值q_k和总协方差P_k：

总状态估计值q_k和总协方差P_k分别代表各车道状态值及协方差的加权平均值，上式中，T代表矩阵转置式中。

3.根据权利要求2所述的基于行车风险评估的自动驾驶汽车规划方法，其特征在于，采用交互式多模型算法分析周围车辆预测时域内在各车道的概率分布时，假设纵向为匀速运动。

4.根据权利要求1所述的基于行车风险评估的自动驾驶汽车规划方法，其特征在于，根据周围车辆当前位置及下一时刻横向位置，拟合生成预测时域内的预测轨迹，包括：

采用如下三次多项式进行拟合：

设置起始和终端航向角边界条件如下：

式中，(x_k,y_k)表示k时刻周围车辆的当前位置，表示k时刻周围车辆偏航角，(x_p,y_p)表示p时刻预测的周围车辆位置，a_i为系数。