CN107168319B - 一种基于模型预测控制的无人车避障方法 - Google Patents

Abstract

本发明涉及一种基于模型预测控制的无人车避障方法，包括以下步骤：1)建立无人驾驶车辆的动力学模型，描述无人驾驶车辆动力学特征；2)构建模型预测控制最优化问题的代价函数和约束条件：将障碍物威胁函数和关于目标位置的代价函数之和作为模型预测控制最优化问题的代价函数，并将无碰撞条件的合取约束作为约束条件之一；3)求解模型预测控制最优化问题，获取无人车避障的最优路径。与现有技术相比，本发明具有察觉域由设计者决定、具有更好的乘客舒适度水平等优点。

Description

一种基于模型预测控制的无人车避障方法

技术领域

本发明涉及无人驾驶车辆避障领域，尤其是涉及一种基于模型预测控制的无人车避障方法。

背景技术

由于无人驾驶车辆在减少交通事故及人员伤亡、缓解降低交通拥堵、减少用户在驾驶上消耗的精力等方面的优势，获得了学术界和产业界的广泛关注。无人驾驶车辆的实施涉及各个领域，包括信息与传感技术、轨迹跟踪技术与避障技术。实现无人车在各种情况下的避障具有十分重要的意义。避障能力是无人车的基础，只有具有良好的避障能力的无人车才有可能真正具有实用性。

无人车避障是通过路径规划算法实现的。路径规划是指寻找一条恰从给定起点到目标点的路径，使得无人车能够安全无碰撞地绕过环境中的障碍物达到目标点。传统的路径规划方法主要包括栅格法与快速扩展随机树法。栅格法，包括知名的A*与D*算法，采用大小相同的栅格对环境地图中的的二维运动空间进行划分，然后将路径规划问题转化为图搜索问题。栅格法的主要缺点在于，实时性能不强，对动态环境中适应性较差，不能够处理车辆动力学约束。快速扩展随机树算法由S.M.LaValle于1999年提出，该方法通过创建随机扩展树来连接起始位姿和目标位姿。快速扩展随机树算法缺点主要包括：需要对全局空间进行随机搜索，由于随机搜索具有不确定性，运算量大，规划时间长，导致算法的实时性较差；路径由多个随机采样点连接而成，导致最终的路径不够光滑；不能处理车辆动力学约束。

模型预测控制算法是一种通过滚动求解优化问题获取控制序列的方法，已经在多个领域得到广泛应用。能够处理车辆动力学约束是模型预测控制算法的突出优势。文献(L.Blackmore,M.Ono,A.Bektassov and B.C.Williams,”A probabilistic particle-control approximation of chance-constrained stochastic predictive control”,IEEE Transactions on Robotics,v 26,n 3,June 2010.)提出一种基于约束模型预测控制的避障方法，即通过在模型预测控制约束项中添加可行区域约束项，来反映环境信息，保证车辆安全性。

障碍物威胁函数在提升避障***的察觉域(即避障算法中避障动作的启动距离)上的优点并没有在已有的约束模型预测控制避障方法中认识到，也缺少相关方法。较大的察觉域能够降低避障过程中的车辆横摆角速度峰值，提升无人驾驶车辆的乘客舒适度。

根据模型预测控制理论，UMPC避障***的察觉域(记作d_pr)计算如下：

其中

表示车辆纵向速度，T表示预测周期。由此可知，UMPC避障***的察觉域依赖于预测步长和预测时域的长度。然而，由于预测时域的增加会增加约束的数量，优化MPC最优化问题的计算时间会显著增加；同时过大的预测步长会使得整个避障算法不可用。由于UMPC避障***的察觉域受限于预测步长和预测时域的长度，算法察觉域的增大会影响避障***性能，这导致受限的察觉域限制了车辆乘客舒适度的提升。

发明内容

本发明的目的就是为了克服上述现有技术存在的缺陷而提供一种基于模型预测控制的无人车避障方法。

本发明的目的可以通过以下技术方案来实现：

一种基于模型预测控制的无人车避障方法，包括以下步骤：

1)建立无人驾驶车辆的动力学模型，描述无人驾驶车辆动力学特征；

2)构建模型预测控制最优化问题的代价函数和约束条件：将障碍物威胁函数和关于目标位置的代价函数之和作为模型预测控制最优化问题的代价函数，并将无碰撞条件的合取约束作为约束条件之一；

3)求解模型预测控制最优化问题，获取无人车避障的最优路径。

所述的步骤1)中，无人驾驶车辆的动力学模型为：

其中，x为状态向量，u为输入向量，

为车身坐标系下的横向速度，

为车身坐标系下的纵向速度，

为横摆角，

为横摆角速度，

为全局坐标系下的横向速度，

为全局坐标系下的纵向速度，δ_f为前轮转角，a为质心到前轴的距离，b为质心到后轴的距离，C_cf为轮胎前轮侧偏刚度、C_cr为轮胎后轮侧偏刚度，m为车辆总质量，I_z为车辆的转动惯量，

为横向加速度，

为横摆加速度。

所述的步骤2)中，障碍物威胁函数h_o(u(t-1|t),x(t|t),ΔU(t))为：

c_ij＝υξ_jη_jφ(Y_p(t|t),Y_o,j)Y_p(t+i|t)

其中，X_p(t|t)与Y_p(t|t)分别为t时刻全局坐标系下的车辆纵向位置与横向位置，τ、κ、X_ellipsoid、Y_ellipsoid与υ均为常数，Y_p(t+i|t)为第i个预测时域的车辆横向坐标，X_o,j与Y_o,j分别为第j个障碍物的纵向与横向坐标，N_p为预测时域的长度，N_o为障碍物的个数，u(t-1|t)为上一时刻的***输入，x(t|t)为当前时刻的***状态，ΔU(t)为当前时刻的控制增量。

所述的步骤2)中，所述的无碰撞条件为：

其中，Z_v为包含定义车身约束的编序的集合，Z_p为包含预测时域序列的集合；Z_o为包含障碍物序列的集合，i、j和k分别为集合Z_p、Z_o和Z_v的元素，s、f_k、b_k均为中间变量，x_j为第j个障碍物的位置，x(t+i|t)为在t时刻第i个预测时域的车辆状态，w为车辆宽度，lh与lt分别表示车头与车尾到质心的纵向距离，X_o,j与Y_o,j分别为障碍物在全局坐标系下的横向位置与纵向位置。

所述的步骤2)中，模型预测控制的代价函数为：

minJ((ΔU,x(t|t),u(t-1|t))＝h(ΔU,x(t|t),u(t-1|t))+h_o(ΔU,x(t|t),u(t-1|t)))

其中，h(ΔU,x(t|t),u(t-1|t))为关于目标位置的代价函数，h_o(ΔU,x(t|t),u(t-1|t)为障碍物威胁函数，x_goal为目标位置，Q和R均为权重矩阵，Nc为控制时域的长度；

约束条件为：

x(t+i+1|t)＝Ax(t+i|t)+Bu(t+i|t)+d(t+i|t),i＝0,1,...N_p-1

u(t+i|t)＝u(t+i-1|t)+Δu(t+i|t),i＝0,1,...N_c-1

ΔU(t)＝[Δu(t|t),Δu(t+1|t),…Δu(t+N_c-1|t)]^T

ρ_i,j,k∈{0,1},i∈Z_p,j∈Z_o,k∈Z_v

其中，t为时间，x(t+i|t)为在t时刻第i个预测时域的车辆状态，

为在t时刻第i个预测时域的控制输入，

为在t时刻第i个预测时域的控制增量，A、B和d表示状态空间矩阵，M为一足够大的常数，ρ_i,j,k为0-1变量。

与现有技术相比，本发明具有以下优点：

本发明中的察觉域完全由设计者决定，而不采用障碍物威胁函数的约束模型预测控制(UMPC)避障***仅通过约束反映环境信息，具有较大的察觉域，因此基于FMPC算法的车辆能够更早的启动避障动作，相对于UMPC***，基于FMPC避障***的车辆在避障过程中具有更小的横摆角速度峰值，即具有更好的乘客舒适度水平。

附图说明

图1为本发明***流程图；

图2为UMPC与FMPC避障***车辆轨迹曲线对比图；

图3为横摆角速度曲线对比图；

具体实施方式

下面结合附图和具体实施例对本发明进行详细说明。

实施例

如图1所示，一种基于模型预测控制的无人车避障方法包括以下步骤：

S1:建立车辆动力学模型，该模型用于描述无人驾驶车辆动力学特征：

车辆动力学模型描述如下：

其中，状态向量和输入向量分别为：

与u＝δ_f，

与

分别表示车身坐标系下的横向速度与纵向速度，纵向速度

恒定，

为横摆角，

为横摆角速度，

和

分别表示全局坐标系下的横向与纵向速度，δ_f为前轮转角，a和b分别表示质心到前轴距离，质心到后轴距离，C_cf与C_cr分别表示轮胎前轮侧偏刚度与后轮侧偏刚度，m表示车辆总质量，I_z表示车辆的转动惯量。

S2:无碰撞条件设计：

将原车身占据区域约束视作由一系列约束组成的凸多边形。

原无碰撞条件R_cprvs描述如下：

ΔU(t)＝[Δu(t|t),Δu(t+1|t),…Δu(t+N_c-1|t)]^T (3)

其中，t表示时间。

表示在t时刻第i个预测时域的控制增量。x(t|t)表示在t时刻的车辆状态，

表示在t-1时刻的车辆控制输入。Z_v是包含定义车身约束的编序的集合；Z_p是包含预测时域序列的集合；Z_o是包含障碍物序列的集合。i，j，k分别是集合Z_p，Z_o与Z_v的元素，预测时域内f_i,k(ΔU,x(t|t),u(t-1|t))与b_i,k(ΔU,x(t|t),u(t-1|t))可以表述为关于车辆长度与宽度、质心位置和横摆角的函数。由于预测时域内质心位置和横摆角均可以表述为ΔU，x(t|t)与u(t-1|t)的函数，同时车辆长度与宽度是固定的，所以，f_i,k(ΔU,x(t|t),u(t-1|t))与b_i,k(ΔU,x(t|t),u(t-1|t))可以表述为关于ΔU，x(t|t)与u(t-1|t)的函数，正方向定义为指向车身朝外的方向，具体的表达式通过简单的几何关系计算得出。∧表示逻辑与；∨表示逻辑或。

由于公式(2)具体形式比较复杂，关于决策变量的凸性是未知的，因此本发明提出原无碰撞条件的估计R_acprvs，描述如下：

其中，lh与lt分别表示车头与车尾到质心的纵向距离，w表示车辆宽度。f_k与b_k的值如表1所示。x_j定义如下：

其中，X_o,j与Y_o,j分别表示障碍物在全局坐标系下的横向位置与纵向位置。

表1 f_k与b_k的参数

将公式(2)定义的析取约束转化为以下形式的合取约束：

ρ_i,j,k∈{0,1},i∈Z_p,j∈Z_o,k∈Z_v (8)

其中。M表示足够大的常数；ρ_i,j,k为0-1变量，只取值为整数0或者整数1；取值为0表示公式(2)描述的约束满足。

S3:障碍物威胁函数设计：

通过障碍物威胁函数，将环境信息反映到模型预测控制的代价函数上，与仅采用约束项保证车辆安全性的方法相比，本发明公开的联合障碍物威胁函数与约束项的避障方法，能够提升车辆乘客舒适度；障碍物威胁函数满足以下公式：

其中：

c_ij＝υξ_jη_jφ(Y_p(t|t),Y_o,j)Y_p(t+i|t) (10)

其中X_p(t|t)与Y_p(t|t)分别是t时刻全局坐标系下的车辆纵向位置与横向位置，τ、κ、X_ellipsoid、Y_ellipsoid与υ均为常数，Y_p(t+i|t)表示第i个预测时域的车辆横向坐标，X_o,j与Y_o,j分别表示第j个障碍物的纵向与横向坐标，N_p表示预测时域的长度，N_o是障碍物的个数；

通公式(9)定义了如下形式的避障启动椭圆：

车辆在该椭圆内启动避障动作，用于过滤过远的障碍物点。横向势场的影响在预测时域内得到反映。纵向势场的影响通过η_j和ξ_j反映。

根据公式(9)描述的障碍物威胁函数，FMPC避障***的察觉域(记作d_oaf)计算如下：

由上可知，FMPC避障***的察觉域取决于X_ellipsoid与τ，而X_ellipsoid与τ都可以完全由用户定义。所以FMPC避障***的察觉域是可以由设计者决定的，不会受限于预测步长和预测时域。

S4:代价函数设计：

模型预测控制的代价函数设计如下：

其中，h(ΔU,x(t|t),u(t-1|t))表示关于目标位置的代价函数：

其中x_goal表示目标位置，Q和R均为权重矩阵，第一项反映***趋近于目标位置的能力，第二项反映了避障***要求控制增量的变化较为平滑；

S5:模型预测控制最优化问题设计与求解：

综上，模型预测控制最优化问题设计如下：

s.t.

u(t+i|t)＝u(t+i-1|t)+Δu(t+i|t),i＝0,1,...N_c-1 (20)

ΔU(t)＝[Δu(t|t),Δu(t+1|t),…Δu(t+N_c-1|t)]^T (23)

ρ_i,j,k∈{0,1},i∈Z_p,j∈Z_o,k∈Z_v (26)

其中，t表示时间，x(t+i|t)表示在t时刻第i个预测时域的车辆状态，

表示在t时刻第i个预测时域的控制输入，

表示在t时刻第i个预测时域的控制增量，N_p表示预测时域的长度，N_c表示控制时域的长度，A、B和d表示状态空间矩阵，通过对车辆动力学模型线性化离散化得到；优化问题通过Cplex工具求解。在每个优化时刻求解上述优化问题获取的控制增量，将第一个控制增量代入公式(20)，与上一时刻控制量累积，得到控制量u(t|t)，并将它作用于***；舍弃除第一个控制增量以外的其它控制增量；在下一个优化时刻到来时，重复上述过程，实现滚动优化。

根据具体实施方式进行仿真实验，仿真结果如图2与图3所示；

图2与图3比较了UMPC与FMPC避障***的仿真结果。如图2所示，相对于察觉域受限的UMPC方法，由于FMPC避障***的察觉域能够完全由设计者决定，具有较大的察觉域，因此基于FMPC算法的车辆能够更早的启动避障动作。这使得，如图3所示，相对于UMPC***，基于FMPC避障***的车辆在避障过程中具有更小的横摆角速度峰值，即具有更好的乘客舒适度水平。

Claims (1)

1.一种基于模型预测控制的无人车避障方法，其特征在于，包括以下步骤：

1)建立无人驾驶车辆的动力学模型，描述无人驾驶车辆动力学特征，无人驾驶车辆的动力学模型为：

u＝δ_f

其中，x为状态向量，u为输入向量，

为车身坐标系下的横向速度，

为车身坐标系下的纵向速度，

为横摆角，

为横摆角速度，

为全局坐标系下的横向速度，

为全局坐标系下的纵向速度，δ_f为前轮转角，a为质心到前轴的距离，b为质心到后轴的距离，C_cf为轮胎前轮侧偏刚度、C_cr为轮胎后轮侧偏刚度，m为车辆总质量，I_z为车辆的转动惯量，

为横向加速度，

为横摆加速度，X_P与Y_P分别为车辆在全局坐标系下的纵向位置与横向位置；

2)构建模型预测控制最优化问题的代价函数和约束条件：将障碍物威胁函数和关于目标位置的代价函数之和作为模型预测控制最优化问题的代价函数，并将无碰撞条件的合取约束作为约束条件之一，障碍物威胁函数h_o(u(t-1|t),x(t|t),ΔU(t))为：

c_ij＝υξ_jη_jφ(Y_p(t|t),Y_o,j)Y_p(t+i|t)

其中，X_p(t|t)与Y_p(t|t)分别为t时刻全局坐标系下的车辆纵向位置与横向位置，τ、κ、X_ellipsoid、Y_ellipsoid与υ均为常数，Y_p(t+i|t)为第i个预测时域的车辆横向坐标，X_o,j与Y_o,j分别为第j个障碍物的纵向与横向坐标，N_p为预测时域的长度，N_o为障碍物的个数，u(t-1|t)为上一时刻的***输入，x(t|t)为当前时刻的***状态，ΔU(t)为当前时刻的控制增量，ε为一大于零且与障碍物大小相关的常数；

所述的无碰撞条件为：

其中，Z_v为包含定义车身约束的编序的集合，Z_p为包含预测时域序列的集合；Z_o为包含障碍物序列的集合，i、j和k分别为集合Z_p、Z_o和Z_v的元素，s、f_k、b_k均为中间变量，x_j为第j个障碍物的位置，x(t+i|t)为在t时刻第i个预测时域的车辆状态，w为车辆宽度，lh与lt分别表示车头与车尾到质心的纵向距离，X_o,j与Y_o,j分别为障碍物在全局坐标系下的横向位置与纵向位置；

模型预测控制的代价函数为：

minJ((ΔU,x(t|t),u(t-1|t))＝h(ΔU,x(t|t),u(t-1|t))+h_o(ΔU,x(t|t),u(t-1|t)))

其中，h(ΔU,x(t|t),u(t-1|t))为关于目标位置的代价函数，h_o(ΔU,x(t|t),u(t-1|t)为障碍物威胁函数，x_goal为目标位置，Q和R均为权重矩阵，Nc为控制时域的长度；

约束条件为：

x(t+i+1|t)＝Ax(t+i|t)+Bu(t+i|t)+d(t+i|t),i＝0,1,...N_p-1

u(t+i|t)＝u(t+i-1|t)+Δu(t+i|t),i＝0,1,...N_c-1

ΔU(t)＝[Δu(t|t),Δu(t+1|t),…Δu(t+N_c-1|t)]^T

ρ_i,j,k∈{0,1},i∈Z_p,j∈Z_o,k∈Z_v

其中，t为时间，x(t+i|t)为在t时刻第i个预测时域的车辆状态，

为在t时刻第i个预测时域的控制输入，

为在t时刻第i个预测时域的控制增量，A、B和d表示状态空间矩阵，M为一足够大的常数，ρ_i,j,k为0-1变量；

3)求解模型预测控制最优化问题，获取无人车避障的最优路径。

Images