CN107357168B - 一种基于机会约束模型预测控制的无人车避障方法 - Google Patents

Abstract

本发明涉及一种基于机会约束模型预测控制的无人车避障方法，包括以下步骤：1)建立无人驾驶车辆的动力学模型，描述无人驾驶车辆动力学特征；2)构建模型预测控制最优化问题的代价函数和约束条件；3)求解模型预测控制最优化问题，获取无人车避障的最优路径。与现有技术相比，本发明具有环境适应性好、考虑车辆实际占据区域等优点。

Description

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

技术领域

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

背景技术

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

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

模型预测控制算法是一种通过滚动求解优化问题获取控制序列的方法，已经在多个领域得到广泛应用。能够处理车辆动力学约束是模型预测控制算法的突出优势。

在无人驾驶车辆实际应用中，传感系统的测量误差可能突然增大(比如在无人驾驶车辆经过隧道时，定位系统测量误差会突然增大)。避障算法必须考虑这种不确定性。

机会约束模型预测控制能够有效的处理这种不确定性。由于存在不确定性，在机会约束模型预测控制中，部分约束以一定的置信水平满足，该部分约束称之为机会约束。近年来，许多学者提出了一些基于机会约束模型预测控制的避障方法。

文献(L.Blackmore,M.Ono and B.C.Williams,”Chance-constrained optimalpath planning with obstacles”,IEEE Transactions on Robotics,v 27,n6,p 1080-1094,Dec.2011.)针对线性高斯系统，将路径规划的优化问题估计为析取规划，并通过分支界定法求解。但是，该算法的主要不足在于：没有考虑车辆实际占据区域，仅仅将车辆视作质点；只考虑了车辆状态的不确定性，没有考虑障碍物状态的不确定性。

为了能够同时考虑车辆状态与障碍物信息不确定性，文献(N.E.Toit andJ.W.Burdick,”Probabilistic collision checking with chance constraints”,IEEETransactions on Robotics,v 27,n 4,p 809-815,Aug.2011.)提出了一种基于机会约束的碰撞检查方法，在假设车辆半径非常小的前提下，通过积分运算，分别针对车辆状态与障碍物状态的不确定性独立与否的两种情况，该方法将关于不确定变量的约束近似转化为关于均值与方差的替代约束，并通过大量采样的方法验证了估计的精度。由于替代约束关于决策变量是非凸的，使得最终模型预测控制优化问题不易求取。

发明内容

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

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

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

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

2)构建模型预测控制最优化问题的代价函数和约束条件；

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

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

u＝δ_f

其中，x为状态向量，u为输入向量，为车身坐标系下的横向速度，为车身坐标系下的纵向速度，为横摆角，为横摆角速度，为全局坐标系下的横向速度，为全局坐标系下的纵向速度，δ_f为前轮转角，a为质心到前轴的距离，b为质心到后轴的距离，C_cf为轮胎前轮侧偏刚度、C_cr为轮胎后轮侧偏刚度，m为车辆总质量，I_z为车辆的转动惯量，为横向加速度，为横摆加速度。

所述的步骤2)中，将关于目标位置的代价函数作为模型预测控制的代价函数，并将无碰撞机会约束作为约束条件。

所述的步骤2)中，关于目标位置的代价函数为：

min E(h(ΔU,x(t|t),u(t-1|t)))

其中，E(·)表示期望，h(ΔU,x(t|t),u(t-1|t))为关于目标位置的代价函数，x_goal为目标位置，Q和R均为权重矩阵，N_p为预测时域的长度，N_c为控制时域的长度，ΔU为控制输入的增量向量，x(t|t)为当前时刻的系统状态，u(t-1|t)为上一时刻系统输入，x(t+i|t)为在t时刻第i个预测时域的车辆状态，Δu(t+i|t)为在t时刻第i个预测时域的控制增量。

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

其中，J(ΔU,x(t|t),u(t-1|t))为优化指标，μ_i为在第i个预测时域车辆状态信息所服从高斯分布的期望，

约束条件为：

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

x(t|t)～N(μ_t0,Σ_t0)

x_j～N(μ_j,Σ_j)

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为状态空间矩阵，ρ_i,j,k为0-1变量，M为一足够大的正数，Z_v为包含定义车身约束的编序的集合，Z_p为包含预测时域序列的集合，Z_o为包含障碍物序列的集合，i、j和k分别为集合Z_p、Z_o和Z_v的元素，s、f_k、b_k均为中间变量，F^-1(α)表示标准正态分布累积函数的逆函数，α为给定的置信水平，障碍物位置信息x_j的不确定性描述为高斯分布N(μ_j,Σ_j)，Σ_i为在第i个预测时域车辆状态信息所服从高斯分布的协方差，lh与lt分别表示车头与车尾到质心的纵向距离，w表示车辆宽度，μ_t0,Σ_t0为高斯分布N(μ_t0,Σ_t0)的均值和方差，μ_j、Σ_j为高斯分布N(μ_j,Σ_j)的均值和方差，n_Δu为控制输入增量的数目，n_u为控制输入的数目。

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

一、在以往的研究中，避障算法中仅仅将车辆视作一个质点，车辆实际占据区域被完全忽略了，显然，车辆质心是无碰撞的并不能保证整个车辆是无碰撞的，本发明将车辆实际占据区域视作一系列约束的析取而不是质点，更好地保证了车辆的安全性。

二、本发明能够同时考虑车辆状态信息与障碍物信息的不确定性，因此具有更好的环境适应性。

附图说明

图1为本发明系统流程图。

图2为不同置信水平下车辆轨迹曲线对比图。

具体实施方式

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

实施例

如图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的参数

S3:无碰撞机会约束估计

对应于公式(2)的机会约束记作CC_cprvs，该约束具体写成以下形式：

P(R_cprvs)≥c (6)

其中，c为置信水平。

对应于公式(4)的机会约束记作CC_acprvs，该约束具体写成以下形式：

P(R_acprvs)≥c (7)

其中，c为置信水平。

将障碍物位置信息的不确定性描述为高斯分布x_j～N(μ_j,Σ_j)。将初始车辆状态信息的不确定性描述为高斯分布x(t|t)～N(μ_t0,Σ_t0)且独立于x_j。车辆状态信息在预测时域里服从高斯分布x(t+i|t)～N(μ_i,Σ_i)。通过递推计算μ_i与Σ_i：

S31：单个机会约束转化：

定义一个新的随机变量X_new＝f_kx(t+i|t)-b_kx_j。可知X_new服从高斯分布N(f_kμ_i-b_kμ_j,f_kΣ_if_k ^T+b_kΣ_jb_k ^T)。利用X_new的概率分布，即可得到：

S32:CC_acprvs的计算：

利用公式(10)，避障机会约束CC_acprvs计算如下：

其中，c＝1-N_pN_o(1-α)，N_o是障碍物的个数。

公式(11)是从单个置信水平为α的机会约束推出总体置信水平为c的机会约束。α与c都是接近但小于概率1的数值。公式(11)的直观意义是，如果单个机会约束不满足的概率不超过1-α，那么整个无碰撞条件R_acprvs满足的概率不低于c＝1-N_pN_o(1-α)，即把R_acprvs的机会约束转化为单个机会约束。

S33:CC_cprvs的计算

事件A与事件B分别定义如下：

为了方便表述，将公式(2)中f_k,i(ΔU,x(t|t),u(t-1|t))简写为f_k,i，将公式(2)中b_k,i(ΔU,x(t|t),u(t-1|t))简写为b_k,i。

根据公式(5)，事件A与事件B的关系可以描述如下：

根据公式(12)、公式(13)与公式(14)，将R_cprvs与R_acprvs的关系表述如下：

利用公式(15)，进一步得到：

利用公式(16)与公式(11)，机会约束CC_cprvs计算如下：

其中，c＝1-N_pN_o(1-α)。

S34:析取约束转化：

公式(17)中的析取约束可以通过“大M”方法转化为以下形式：

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

其中，M表示足够大的正数。ρ_i,j,k为0-1变量，只取值为整数0或者整数1。

S4:代价函数设计：

代价函数设计如下：

minE(h(ΔU,x(t|t),u(t-1|t))) (21)

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

其中，x_goal表示目标位置，Q和R均为权重矩阵。公式(22)中第一项反映算法趋近于目标位置的能力，也反映了避障算法的经济性和能耗。第二项反映了避障算法要求控制增量的变化较为平滑，也反映了避障轨迹的平顺性。

利用公式(22)，将公式(21)重写：

将公式(23)重写如下：

其中，Tr(·)表示矩阵的迹。

将与控制增量无关的常数项舍弃，公式(24)重写如下：

至此，期望的计算完成。

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

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

s.t.

x(t|t)～N(μ_t0,Σ_t0) (28)

x_j～N(μ_j,Σ_j) (29)

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

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

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

其中，t表示时间，x(t+i|t)表示在t时刻第i个预测时域的车辆状态，表示在t时刻第i个预测时域的控制输入，表示在t时刻第i个预测时域的控制增量。E(·)表示期望，因为车辆状态与障碍物位置信息包含不确定性，代价函数本身也成为随机变量，所以对包含随机变量的代价函数求取期望，作为优化指标。N_p表示预测时域的长度，N_c表示控制时域的长度，A、B和d表示状态空间矩阵，通过对车辆动力学模型线性化离散化得到。优化问题通过cplex工具包求解。求解上述优化问题获取的控制增量，将第一个控制增量代入公式(30)，与上一时刻控制量累积，得到控制量，并将它作用于系统。舍弃除第一个控制增量以外的其它控制增量。在下一个优化时刻到来时，重复上述过程，实现滚动优化。

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

基于机会约束模型预测控制算法，Σ_t0＝0.2*I₅，Σ_j＝0.2*I₂，在较高置信水平c＝1-10^-10与较低置信水平c＝0.9两种工况下的车辆避障轨迹的均值图2所示。相对于c＝0.9的工况，c＝1-10^-10工况下的车辆总避障轨迹更长，即具有更高的能耗和经济代价。因此，图2说明，更高的置信水平是以能耗为代价的，即在车辆状态与障碍物位置信息存在不确定性的情况下，如果要获取更高的置信水平的避障轨迹，那么就要付出更高的经济代价。

Claims (2)

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

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

u＝δ_f

其中，x为状态向量，u为输入向量，为车身坐标系下的横向速度，为车身坐标系下的纵向速度，为横摆角，为横摆角速度，为全局坐标系下的横向速度，为全局坐标系下的纵向速度，δ_f为前轮转角，a为质心到前轴的距离，b为质心到后轴的距离，C_cf为轮胎前轮侧偏刚度、C_cr为轮胎后轮侧偏刚度，m为车辆总质量，I_z为车辆的转动惯量，为横向加速度，为横摆加速度；

2)构建模型预测控制最优化问题的代价函数和约束条件，关于目标位置的代价函数为：

minE(h(ΔU,x(t|t),u(t-1|t)))

其中，E(·)表示期望，h(ΔU,x(t|t),u(t-1|t))为关于目标位置的代价函数，x_goal为目标位置，Q和R均为权重矩阵，Nc为控制时域的长度，Np为预测时域的长度，ΔU为控制输入的增量向量，x(t|t)为当前时刻的系统状态，u(t-1|t)为上一时刻系统输入，x(t+i|t)为在t时刻第i个预测时域的车辆状态，Δu(t+i|t)为在t时刻第i个预测时域的控制增量；

模型预测控制最优化问题的代价函数为：

其中，J(ΔU,x(t|t),u(t-1|t))为优化指标，μ_i为在第i个预测时域车辆状态信息所服从高斯分布的期望，

约束条件为：

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

x(t|t)～N(μ_t0,Σ_t0)

x_j～N(μ_j,Σ_j)

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为状态空间矩阵，ρ_i,j,k为0-1变量，M为一足够大的正数，Z_v为包含定义车身约束的编序的集合，Z_p为包含预测时域序列的集合，Z_o为包含障碍物序列的集合，i、j和k分别为集合Z_p、Z_o和Z_v的元素，s、f_k、b_k均为中间变量，F^-1(α)表示标准正态分布累积函数的逆函数，α为给定的置信水平，障碍物位置信息x_j的不确定性描述为高斯分布N(μ_j,Σ_j)，Σ_i为在第i个预测时域车辆状态信息所服从高斯分布的协方差，lh与lt分别表示车头与车尾到质心的纵向距离，w表示车辆宽度，μ_t0,Σ_t0为高斯分布N(μ_t0,Σ_t0)的均值和方差，μ_j、Σ_j为高斯分布N(μ_j,Σ_j)的均值和方差，n_Δu为控制输入增量的数目，n_u为控制输入的数目；

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

2.根据权利要求1所述的一种基于机会约束模型预测控制的无人车避障方法，其特征在于，所述的步骤2)中，将关于目标位置的代价函数作为模型预测控制的代价函数，并将无碰撞机会约束作为约束条件。