一种用于自主驾驶车辆避障轨迹规划与跟踪控制的优先级分层预测控制方法技术

一种用于自主驾驶车辆避障轨迹规划与跟踪控制的优先级分层预测控制方法，本方法基于离散时间车辆动力学模型，首先定义出描述行车环境的势场函数，再将离散时间动力学模型作为预测模型，然后建立有限时域优先级分层优化控制问题，并结合滚动优化控制原理，在每个采样时刻计算得到最优控制解，实现基于目标优先级驱动的自主车辆避障轨迹规划与跟踪控制的预测控制。本发明专利技术方法的优点是理解简单，通用性强，控制量满足自主驾驶车辆安全避障规划要求和车辆操纵稳定性。

【技术实现步骤摘要】
一种用于自主驾驶车辆避障轨迹规划与跟踪控制的优先级分层预测控制方法
本专利技术属于自主驾驶车辆轨迹规划与跟踪控制领域，涉及一种用于自主驾驶车辆避障轨迹规划与跟踪控制的优先级分层预测控制方法。
技术介绍
自主无人驾驶是智能车辆发展的重要方向，自主车辆的轨迹规划在环境感知的基础上，为车辆规划一条安全无碰撞且可行的行驶路径，是实现车辆自主无人驾驶的关键技术。自主驾驶车辆轨迹规划与跟踪控制应满足车辆动力学/运动学特性以及各种约束，预测控制算法能够很好地解决此类问题。通过对现有基于预测控制的自主驾驶车辆轨迹规划与跟踪控制的文献检索，发现现有方法将避障目标函数和跟踪目标函数直接相加，转换为单个综合目标函数，并用加权系数表示避障目标和跟踪目标的相对重要性，但避障目标函数和跟踪目标函数不同，度量的单位不同，而且避障目标和跟踪目标存在相互冲突特性，因此，将避障目标函数和跟踪目标函数直接相加的方法不能真实反映自主驾驶车辆的避障高优先级要求，尤其是对于车辆这种复杂非线性动力学系统而言，固定两者的加权系数适应性差，但行驶过程在线调整两者的加权系数将增加驾驶危险性而且难以实现。本方法把自主驾驶车辆避障轨迹规划与跟踪控制过程建立为一个目标优先级明确的多目标控制问题，将车辆的避障轨迹规划目标作为最高优先级，跟踪控制目标作为低优先级，然后从高优先级避障轨迹规划目标要求开始计算，逐层实现自主车辆避障轨迹与跟踪控制目标，保证对更高优先级避障目标的控制效果。本方法与现有自主驾驶车辆避障轨迹规划与跟踪控制方法相比，不需要将避障目标函数和跟踪目标函数相加，从而不存在两者的加权系数，因而本专利技术方法使用上更具灵活性和实用性。
技术实现思路
相对现有自主驾驶车辆避障轨迹规划与跟踪控制的加权预测控制方法，为了克服现有技术的不足，本专利技术设定自主驾驶车辆的避障轨迹规划目标为高优先级控制目标，自主驾驶车辆的跟踪为次优先级控制目标，根据排序结果，依次优化自主驾驶车辆的避障轨迹，同时确保自主驾驶车辆对优化避障轨迹中的安全跟踪控制，避免了避障轨迹规划目标和跟踪控制目标的加权系数复杂的在线调整和重新配置过程。本专利技术解决其技术问题所采用的技术方案是：一种用于自主驾驶车辆避障轨迹规划与跟踪控制的优先级分层预测控制方法，所述方法包括如下步骤：1)、考虑自主驾驶车辆离散时间动力学模型，参见式(1)：其中，k为采样时间，Ts为采样周期，X、Y、分别为车辆在全局坐标系下的纵向位置、横向位置、横摆角，vx、vy、r分别为车辆在质心处的纵向速度、横向速度、横摆角速度，m为车辆质量，Iz为车辆绕其垂直轴的转动惯量，df和dr分别是车辆质心到前、后轴的距离，δf和ax分别为前轮转向角和纵向加速度，Ccf和Ccr分别表示前、后轮的侧偏刚度，定义车辆行驶环境势场函数，参见式(2)：其中，UA为自主驾驶车辆总势场，常数e为自然指数，Al为分道线势场强度，σl为分道线势场收敛幅度，yl,i为第i条道路分道线的横向位置，Ar为道路边界势场强度，yr,i为第i条道路边界的横向位置，Xgoal为目标点纵向位置，组合参数ε和β为正整数，D是自主驾驶车辆与障碍车辆之间的横向距离，σo为横向势场收敛幅度，Uo为障碍车辆纵向势场函数，参见式(3)：其中，Ac,1和Ac,2分别为自主驾驶车辆在区域S1和S2内的纵向势场强度，K为自主驾驶车辆与障碍车辆之间的纵向距离，ρ为自主驾驶车辆在区域S1内的纵向势场收敛幅度，符号“∈”表示“属于”，区域S1定义为自主驾驶车辆距离障碍车辆在纵向安全距离以外区域，区域S2定义为自主驾驶车辆距离障碍车辆在纵向安全距离以内区域和等于纵向安全距离区域；2)、定义状态列向量和控制列向量u＝[δf,ax]T，其中，符号“T”表示“向量转置”，将状态列向量和控制列向量代入式(1)，得到状态方程，参见式(4)：x(k+1)＝x(k)+Tsf(x(k),u(k))(4)其中，f(x(k),u(k))为函数列向量，参见式(5)：3)、考虑模型式(4)，建立自主驾驶车辆的动态预测模型，参见式(6)：x(i+1|k)＝x(i|k)+Tsf(x(i|k),u(i|k)),i＝0,1,...,N-1(6)其中，x(i|k)为自主驾驶车辆在时刻k对未来时刻k+i的预测状态列向量，u(i|k)为自主驾驶车辆在时刻k对未来时刻k+i的预测控制列向量，正整数N为预测时间窗口；4)、考虑自主驾驶车辆的动态预测模型式(6)，分别定义表征车辆操纵稳定性的车辆前轮侧偏角αf和后轮侧偏角αr，分别参见式(7)和(8)：αf(i|k)＝δf(i|k)-[vy(i|k)+ax(i|k)r(i|k)]/vx(i|k)(7)αr(i|k)＝[drr(i|k)-vy(i|k)]/vx(i|k)(8)再分别定义控制列向量约束、控制增量约束、前轮侧偏角αf和后轮侧偏角αr的边界约束，分别参见式(9)至(12)：umin≤u(i|k)≤umax,i＝0,1,...,N-1(9)Δumin≤Δu(i|k)≤Δumax,i＝0,1,...,N-1(10)αfmin≤αf(i|k)≤αfmax,i＝0,1,...,N-1(11)αrmin≤αr(i|k)≤αrmax,i＝0,1,...,N-1(12)其中，Δu(i|k)＝u(i|k)-u(i-1|k)为相邻预测时刻k+i和k+i-1的预测控制增量，umin、Δumin、αfmin、αrmin分别为控制列向量、控制增量、前轮侧偏角、后轮侧偏角的约束下界，umax、Δumax、αfmax、αrmax分别为控制列向量、控制增量、前轮侧偏角、后轮侧偏角的约束上界；5)、考虑自主驾驶车辆的动态预测模型式(6)，分别定义避障目标函数J1和跟踪目标函数J2，参见式(13)和(14)：其中，xgoal为车辆轨迹规划目标点，vx,goal为车辆轨迹规划的纵向目标速度，矩阵S和W都为正定的加权矩阵，系数Q和R都为正的加权系数，||·||为2范数；6)、自主驾驶车辆避障轨迹规划与跟踪控制首先要保证车辆行车安全性，再尽可能恒速跟踪行驶，因此，检测当前时刻k的状态x(k)，定义避障轨迹规划与跟踪控制优先级分层预测控制问题，参见式(15)和(16)：其中，符号“|”表示约束，up(k)为当前时刻k的决策控制变量序列up(k)＝{u(0|k),u(1|k),…,u(N-1|k)}，等式x(0|k)＝x(k)为式(15)和(16)的初始条件，J1*为式(15)的最优值，J2*为式(16)的最优值；7)、求解式(16)得对应J2*的最优解up*(k)＝{u*(0|k),u*(1|k),…,u*(N-1|k)}，参见式(17)：取第一个分量u*(0|k)作用于自主驾驶车辆；在下一个采样时刻k+1到达后，检测车辆的运动状态x(k+1)，并以该状态更新本文档来自技高网...

【技术保护点】
1.一种用于自主驾驶车辆避障轨迹规划与跟踪控制的优先级分层预测控制方法，其特征在于，所述方法包括如下步骤：/n1)、考虑自主驾驶车辆离散时间动力学模型，参见式(1)：/n

【技术特征摘要】
1.一种用于自主驾驶车辆避障轨迹规划与跟踪控制的优先级分层预测控制方法，其特征在于，所述方法包括如下步骤：
1)、考虑自主驾驶车辆离散时间动力学模型，参见式(1)：



其中，k为采样时间，Ts为采样周期，X、Y、分别为车辆在全局坐标系下的纵向位置、横向位置、横摆角，vx、vy、r分别为车辆在质心处的纵向速度、横向速度、横摆角速度，m为车辆质量，Iz为车辆绕其垂直轴的转动惯量，df和dr分别是车辆质心到前、后轴的距离，δf和ax分别为前轮转向角和纵向加速度，Ccf和Ccr分别表示前、后轮的侧偏刚度，定义车辆行驶环境势场函数，参见式(2)：



其中，UA为自主驾驶车辆总势场，常数e为自然指数，Al为分道线势场强度，σl为分道线势场收敛幅度，yl,i为第i条道路分道线的横向位置，Ar为道路边界势场强度，yr,i为第i条道路边界的横向位置，Xgoal为目标点纵向位置，组合参数ε和β为正整数，D是自主驾驶车辆与障碍车辆之间的横向距离，σo为横向势场收敛幅度，Uo为障碍车辆纵向势场函数，参见式(3)：



其中，Ac,1和Ac,2分别为自主驾驶车辆在区域S1和S2内的纵向势场强度，K为自主驾驶车辆与障碍车辆之间的纵向距离，ρ为自主驾驶车辆在区域S1内的纵向势场收敛幅度，符号“∈”表示“属于”，区域S1定义为自主驾驶车辆距离障碍车辆在纵向安全距离以外区域，区域S2定义为自主驾驶车辆距离障碍车辆在纵向安全距离以内区域和等于纵向安全距离区域；
2)、定义状态列向量和控制列向量u＝[δf,ax]T，其中，符号“T”表示“向量转置”，将状态列向量和控制列向量代入式(1)，得到状态方程，参见式(4)：
x(k+1)＝x(k)+Tsf(x(k),u(k))(4)
其中，f(x(k),u(k))为函数列向量，参见式(5)：



3)、考虑模型式(4)，建立自主驾驶车辆的动态预测模型，参见式(6)：
x(i+1|k)＝x(i|k)+Tsf(x(i|k),u(i|k)),i＝0,1,...,N-1(6)
其中，x(i|k)为自主驾驶车辆在时刻k对未来时刻k+i的预测状态列向量，u(i|k)为自主驾驶车辆在时刻k对未来时刻k+i的预测控制列向量，正整数N为预测时间窗口；
4)、考虑自主驾驶车辆的动态预测模型式(6)，分别定义表征车辆操纵稳定性的车辆前轮侧偏角αf和后轮侧偏角αr，分别参见式(7)和(8)：
αf(i|k)＝δf(i|k)-[vy(i|k)+ax(i|k)r(i|k)]/vx(i|k)(7)

【专利技术属性】
技术研发人员：何德峰，徐广琪，余世明，朱威，孙哲，
申请(专利权)人：浙江工业大学，
类型：发明
国别省市：浙江;33