[发明专利]基于障碍物预测与MPC算法的无人驾驶轨迹规划方法

摘要

本发明公开了一种基于障碍物预测与MPC算法的无人驾驶轨迹规划方法：周围车辆运动轨迹预测；碰撞预测分析与最小安全距离的计算；考轨迹与约束条件的生成；利用模型预测控制(MPC)进行轨迹优化，得到最终轨迹输出。本发明能够利用障碍物运动预测与模型预测控制(MPC)原理的轨迹最优化进行无人驾驶车辆的运动决策与轨迹规划。

主权项

1.一种基于障碍物预测与MPC算法的无人驾驶轨迹规划方法，其特征在于，包括以下步骤：第一步：周围车辆运动轨迹预测周围车辆包括同车道前方车辆、相邻车道前方和后方的车辆，通过车载传感器设备及相应的图像识别算法得到周围车辆的运动速度与方向；假设在预测时域内，周围车辆沿当前车道按照当前速度行驶，得到周围车辆运动轨迹；第二步：碰撞预测分析与最小安全距离的计算无人驾驶车辆V_A，当前运动速度为v_A，由决策层输出的参考速度v_des和运动指令可以估计车辆的大概行驶轨迹；为避免无人驾驶车辆V_A与周围车辆发生碰撞，进行碰撞预测分析与最小安全距离(MSS)的计算；换道场景坐标系为建立在无人驾驶车辆后轴中心的车载坐标系，换道场景中包括同车道前方车辆V₁，速度为v₁，与V_A的距离为L₁；目标车道前方同方向行驶的车辆V₂，速度为v₂，与V_A的距离为L₂；目标车道后方同方向行驶的车辆V₃，速度为v₃，与V_A的距离为L₃。V_A换道过程从时刻t₀＝0开始准备换道，到时刻t_adj为施加横向加速度前的调整时间，t_lat为施加横向加速度的时间，故时刻t_adj+t_lat施加横向加速度结束，V_A完全到达目标车道，t_finish为换道策略预测时间；①V_A与V₁碰撞分析：对V_A施加横向加速度，其右前角通过V₁左侧纵向切线之前易于V₁发生碰撞，两车之间的最小安全距离为：其中，a_A和a₁分别为V_A与V₁的加速度，为V_A施加横向加速度开始到其右前角通过V₁左侧纵向切线的时间；②V_A与V₂碰撞分析：对V_A施加横向加速度，其左前角通过V₂右侧纵向切线之后易于V₂发生碰撞，两车之间的最小安全距离为：其中，a₂为V₂的加速度，为V_A施加横向加速度开始到其左前角通过V₂右侧纵向切线的时间；③V_A与V₃碰撞分析：对V_A施加横向加速度，其左后角通过V₃右侧纵向切线之后易于V₃发生碰撞，两车之间的最小安全距离为：其中，a₃为V₃的加速度，为V_A施加横向加速度开始到左后角通过V₃右侧纵向切线的时间；第三步：参考轨迹与约束条件的生成采用换道场景坐标系，分为纵向和横向两部分，约束条件分别为位置、速度、加速度、加加速度的限制，其中加速度与加加速度的约束为确定范围，根据车辆的一般特性确定；考虑直路避障与十字路口转弯两种情况：①直路避障：在本发明中均设定为纵向匀速换道，一般换道过程航向角φ为3～5°，故得到施加横向加速度的时间为：其中，l为车道宽度；在时刻t_adj前参考轨迹为V_A沿当前车道的匀速和匀减速过程，对应横向位置约束为当前车道左右两条车道线；时间段部分的换道轨迹用一段与纵向夹角为3～5°的直线代替，对应横向位置约束为当前车道和目标车道两侧的车道线；在时刻t_adj+t_lat之后为沿目标车道的匀加速和匀速过程，对应横向位置约束为目标车道的左右两条车道线；向左换道的参考轨迹与横向位置约束中包括横向位置最小值y_min和横向位置最大值y_max；由决策层输出参考速度v_des，产生换道行为一般是在当前车道前方车辆速度较小时，故在时刻t_adj前V_A的纵向速度约束为[v₁,v_des]；到达目标车道后V_A为保持与V₂和V₃的安全距离，故纵向速度最大值为v_xmax＝min{v_des,v₂}，最小值为v_xmin＝max{v₁,v₃}；V_A的横向速度约束由t_lat决定；②十字路口转弯：车辆需要在低速情况下进行转弯较安全，故在转弯前需要进行一段减速过程，减速至安全转弯速度v_turn，故参考轨迹即为沿当前车道中心线的匀速和匀减速过程，对应的横向位置约束为当前车道的左右两侧车道线；转弯时采用一段圆弧，可根据转弯所到达的车道初始点确定转弯半径和圆弧圆心；转弯结束后，车辆先进行匀加速过程，直至速度达到v_des，对应位置约束仍为车道两侧车道线。由于V_A整体的速度大小范围为[v_turn,v_des]，根据各时刻车辆运动方向确定其纵横向速度方向；十字路口右转的参考轨迹与位置约束中包括转弯前横向位置最小值y_min和横向位置最大值y_max，由于转弯后车辆沿横向行驶，还包括转弯后纵向位置最小值x_min、纵向位置最大值x_max以及转弯部分位置约束；第四步：利用模型预测控制(MPC)进行轨迹优化，得到最终轨迹输出本发明采用的模型为物理运动学规律为：其中，x₀和v₀分别表示初始时刻车辆的位置和速度，x和v分别表示预测时刻t的车辆位置和速度，a为相应加速度；利用MPC算法进行优化时，纵向和横向分别进行计算，采用相同的模型，下面以纵向为例介绍计算过程：①选取状态量为x_x、v_x、a_x分别为纵向位置、纵向速度和纵向加速度，控制量为u(k)＝Δa_x(k)，也即Δa_x为纵向加加速度，故得到新的状态空间表达式为：相应的②由状态方程得到未来时刻的输出为：③选取代价函数为：其中，I为单位矩阵，N_p为预测时域，N_c为控制时域，x_ref为参考轨迹转换到当前车载坐标系下的纵向坐标，a和b均为正的权重系数。约束条件为：其中，x_max和x_min分别为纵向位置的约束，v_xmax和v_xmin分别为纵向速度的约束，a_xmax和a_xmin分别为纵向加速度的约束，Δa_xmax和Δa_xmin分别为纵向加加速度的约束；④通过二次规划求解得到最优控制量为U^*＝Δa(t|t)；⑤将得到的最优控制量作为下一步预测的输入，重复①‑④的过程，直到得到t₀～t_finish所有时刻的轨迹输出。