本发明涉及无人驾驶车辆避障领域,尤其是涉及一种基于模型预测控制的无人车避障方法。
背景技术:
由于无人驾驶车辆在减少交通事故及人员伤亡、缓解降低交通拥堵、减少用户在驾驶上消耗的精力等方面的优势,获得了学术界和产业界的广泛关注。无人驾驶车辆的实施涉及各个领域,包括信息与传感技术、轨迹跟踪技术与避障技术。实现无人车在各种情况下的避障具有十分重要的意义。避障能力是无人车的基础,只有具有良好的避障能力的无人车才有可能真正具有实用性。
无人车避障是通过路径规划算法实现的。路径规划是指寻找一条恰从给定起点到目标点的路径,使得无人车能够安全无碰撞地绕过环境中的障碍物达到目标点。传统的路径规划方法主要包括栅格法与快速扩展随机树法。栅格法,包括知名的a*与d*算法,采用大小相同的栅格对环境地图中的的二维运动空间进行划分,然后将路径规划问题转化为图搜索问题。栅格法的主要缺点在于,实时性能不强,对动态环境中适应性较差,不能够处理车辆动力学约束。快速扩展随机树算法由s.m.lavalle于1999年提出,该方法通过创建随机扩展树来连接起始位姿和目标位姿。快速扩展随机树算法缺点主要包括:需要对全局空间进行随机搜索,由于随机搜索具有不确定性,运算量大,规划时间长,导致算法的实时性较差;路径由多个随机采样点连接而成,导致最终的路径不够光滑;不能处理车辆动力学约束。
模型预测控制算法是一种通过滚动求解优化问题获取控制序列的方法,已经在多个领域得到广泛应用。能够处理车辆动力学约束是模型预测控制算法的突出优势。文献(l.blackmore,m.ono,a.bektassovandb.c.williams,”aprobabilisticparticle-controlapproximationofchance-constrainedstochasticpredictivecontrol”,ieeetransactionsonrobotics,v26,n3,june2010.)提出一种基于约束模型预测控制的避障方法,即通过在模型预测控制约束项中添加可行区域约束项,来反映环境信息,保证车辆安全性。
障碍物威胁函数在提升避障系统的察觉域(即避障算法中避障动作的启动距离)上的优点并没有在已有的约束模型预测控制避障方法中认识到,也缺少相关方法。较大的察觉域能够降低避障过程中的车辆横摆角速度峰值,提升无人驾驶车辆的乘客舒适度。
根据模型预测控制理论,umpc避障系统的察觉域(记作dpr)计算如下:
其中
技术实现要素:
本发明的目的就是为了克服上述现有技术存在的缺陷而提供一种基于模型预测控制的无人车避障方法。
本发明的目的可以通过以下技术方案来实现:
一种基于模型预测控制的无人车避障方法,包括以下步骤:
1)建立无人驾驶车辆的动力学模型,描述无人驾驶车辆动力学特征;
2)构建模型预测控制最优化问题的代价函数和约束条件:将障碍物威胁函数和关于目标位置的代价函数之和作为模型预测控制最优化问题的代价函数,并将无碰撞条件的合取约束作为约束条件之一;
3)求解模型预测控制最优化问题,获取无人车避障的最优路径。
所述的步骤1)中,无人驾驶车辆的动力学模型为:
其中,x为状态向量,u为输入向量,
所述的步骤2)中,障碍物威胁函数ho(u(t-1|t),x(t|t),δu(t))为:
cij=υξjηjφ(yp(t|t),yo,j)yp(t+i|t)
其中,xp(t|t)与yp(t|t)分别为t时刻全局坐标系下的车辆纵向位置与横向位置,τ、κ、xellipsoid、yellipsoid与υ均为常数,yp(t+i|t)为第i个预测时域的车辆横向坐标,xo,j与yo,j分别为第j个障碍物的纵向与横向坐标,np为预测时域的长度,no为障碍物的个数,u(t-1|t)为上一时刻的系统输入,x(t|t)为当前时刻的系统状态,δu(t)为当前时刻的控制增量。
所述的步骤2)中,所述的无碰撞条件为:
其中,zv为包含定义车身约束的编序的集合,zp为包含预测时域序列的集合;zo为包含障碍物序列的集合,i、j和k分别为集合zp、zo和zv的元素,s、fk、bk均为中间变量,xj为第j个障碍物的位置,x(t+i|t)为在t时刻第i个预测时域的车辆状态,w为车辆宽度,lh与lt分别表示车头与车尾到质心的纵向距离,xo,j与yo,j分别为障碍物在全局坐标系下的横向位置与纵向位置。
所述的步骤2)中,模型预测控制的代价函数为:
minj((δu,x(t|t),u(t-1|t))=h(δu,x(t|t),u(t-1|t))+ho(δu,x(t|t),u(t-1|t)))
其中,h(δu,x(t|t),u(t-1|t))为关于目标位置的代价函数,ho(δu,x(t|t),u(t-1|t)为障碍物威胁函数,xgoal为目标位置,q和r均为权重矩阵,nc为控制时域的长度;
约束条件为:
x(t+i+1|t)=ax(t+i|t)+bu(t+i|t)+d(t+i|t),i=0,1,...np-1
u(t+i|t)=u(t+i-1|t)+δu(t+i|t),i=0,1,...nc-1
δu(t)=[δu(t|t),δu(t+1|t),…δu(t+nc-1|t)]t
ρi,j,k∈{0,1},i∈zp,j∈zo,k∈zv
其中,t为时间,x(t+i|t)为在t时刻第i个预测时域的车辆状态,
与现有技术相比,本发明具有以下优点:
本发明中的察觉域完全由设计者决定,而不采用障碍物威胁函数的约束模型预测控制(umpc)避障系统仅通过约束反映环境信息,具有较大的察觉域,因此基于fmpc算法的车辆能够更早的启动避障动作,相对于umpc系统,基于fmpc避障系统的车辆在避障过程中具有更小的横摆角速度峰值,即具有更好的乘客舒适度水平。
附图说明
图1为本发明系统流程图;
图2为umpc与fmpc避障系统车辆轨迹曲线对比图;
图3为横摆角速度曲线对比图;
具体实施方式
下面结合附图和具体实施例对本发明进行详细说明。
实施例
如图1所示,一种基于模型预测控制的无人车避障方法包括以下步骤:
s1:建立车辆动力学模型,该模型用于描述无人驾驶车辆动力学特征:
车辆动力学模型描述如下:
其中,状态向量和输入向量分别为:
s2:无碰撞条件设计:
将原车身占据区域约束视作由一系列约束组成的凸多边形。
原无碰撞条件rcprvs描述如下:
δu(t)=[δu(t|t),δu(t+1|t),…δu(t+nc-1|t)]t(3)
其中,t表示时间。
由于公式(2)具体形式比较复杂,关于决策变量的凸性是未知的,因此本发明提出原无碰撞条件的估计racprvs,描述如下:
其中,lh与lt分别表示车头与车尾到质心的纵向距离,w表示车辆宽度。fk与bk的值如表1所示。xj定义如下:
其中,xo,j与yo,j分别表示障碍物在全局坐标系下的横向位置与纵向位置。
表1fk与bk的参数
将公式(2)定义的析取约束转化为以下形式的合取约束:
ρi,j,k∈{0,1},i∈zp,j∈zo,k∈zv(8)
其中。m表示足够大的常数;ρi,j,k为0-1变量,只取值为整数0或者整数1;取值为0表示公式(2)描述的约束满足。
s3:障碍物威胁函数设计:
通过障碍物威胁函数,将环境信息反映到模型预测控制的代价函数上,与仅采用约束项保证车辆安全性的方法相比,本发明公开的联合障碍物威胁函数与约束项的避障方法,能够提升车辆乘客舒适度;障碍物威胁函数满足以下公式:
其中:
cij=υξjηjφ(yp(t|t),yo,j)yp(t+i|t)(10)
其中xp(t|t)与yp(t|t)分别是t时刻全局坐标系下的车辆纵向位置与横向位置,τ、κ、xellipsoid、yellipsoid与υ均为常数,yp(t+i|t)表示第i个预测时域的车辆横向坐标,xo,j与yo,j分别表示第j个障碍物的纵向与横向坐标,np表示预测时域的长度,no是障碍物的个数;
通公式(9)定义了如下形式的避障启动椭圆:
车辆在该椭圆内启动避障动作,用于过滤过远的障碍物点。横向势场的影响在预测时域内得到反映。纵向势场的影响通过ηj和ξj反映。
根据公式(9)描述的障碍物威胁函数,fmpc避障系统的察觉域(记作doaf)计算如下:
由上可知,fmpc避障系统的察觉域取决于xellipsoid与τ,而xellipsoid与τ都可以完全由用户定义。所以fmpc避障系统的察觉域是可以由设计者决定的,不会受限于预测步长和预测时域。
s4:代价函数设计:
模型预测控制的代价函数设计如下:
其中,h(δu,x(t|t),u(t-1|t))表示关于目标位置的代价函数:
其中xgoal表示目标位置,q和r均为权重矩阵,第一项反映系统趋近于目标位置的能力,第二项反映了避障系统要求控制增量的变化较为平滑;
s5:模型预测控制最优化问题设计与求解:
综上,模型预测控制最优化问题设计如下:
s.t.
u(t+i|t)=u(t+i-1|t)+δu(t+i|t),i=0,1,...nc-1(20)
δu(t)=[δu(t|t),δu(t+1|t),…δu(t+nc-1|t)]t(23)
ρi,j,k∈{0,1},i∈zp,j∈zo,k∈zv(26)
其中,t表示时间,x(t+i|t)表示在t时刻第i个预测时域的车辆状态,
根据具体实施方式进行仿真实验,仿真结果如图2与图3所示;
图2与图3比较了umpc与fmpc避障系统的仿真结果。如图2所示,相对于察觉域受限的umpc方法,由于fmpc避障系统的察觉域能够完全由设计者决定,具有较大的察觉域,因此基于fmpc算法的车辆能够更早的启动避障动作。这使得,如图3所示,相对于umpc系统,基于fmpc避障系统的车辆在避障过程中具有更小的横摆角速度峰值,即具有更好的乘客舒适度水平。