本发明属于无人驾驶中的轨迹跟踪控制领域,涉及一种基于mpc的牵引式挂车轨迹跟踪方法。
背景技术:
随着我国经济的发展,对货物的运输提出了更高的要求,如何在提高货物运输效率的同时减少交通事故的发生是亟待研究和解决的。目前,牵引式挂车作为国内货物运输的主力军,其今后的发展趋势,将直接影响到货物运输效率。此外,现阶段国内的牵引式挂车企业普遍存在技术含量偏低,缺乏技术创新等问题。所以,为了满足货物运输要求,必须加快推进牵引式挂车朝着智能化、专业化、轻量化的方向发展。
近年来,随着人工智能和汽车行业的发展,交通运输智能化成为了研究的热点。其中,牵引式挂车由于其简单的结构和巨大的货物运输能力受到了广泛的关注。所以,如何实现牵引式挂车的智能化,成为了国内外研究的重点方向。其中,如何对其路径进行精确的跟踪仍然是一个具有挑战性的问题。
技术实现要素:
有鉴于此,本发明的目的在于提供一种基于模型预测控制的牵引式挂车轨迹跟踪方法,实现牵引式挂车对期望路径的跟踪行驶并且保证跟踪的精度和稳定性,在提高了货物运输效率的同时能有效减少交通事故的发生;还实现牵引式挂车的无人驾驶,从而达到智能化运输的最终目的。
为达到上述目的,本发明提供如下技术方案:
一种基于模型预测控制的牵引式挂车轨迹跟踪方法,具体包括以下步骤:
s1:构建牵引式挂车横向跟踪模型,包括横向动力学模型和横向误差模型;
s2:采用mpc对牵引式挂车的横向跟踪模型进行预测控制;
s3:以牵引式挂车的前轮偏角为控制量,实现牵引挂车对期望路径的跟踪行驶,并根据牵引式挂车的横向跟踪模型求解出其下一时刻的所需状态量,再结合参考轨迹信息,进行下一时刻的预测控制。
进一步,所述步骤s1具体包括:根据牛顿第二定律,对牵引式挂车进行动力学建模,即构建横向动力学模型的方程组为:
其中,牵引式挂车中各变量满足如下关系:
vx1=vx2
fyh2=fyh1
vy2=vx1·β2
存在的角度关系如下:
其中,m1和m2分别为牵引车和拖车的质量,vx1、vx2、vy1、vy2分别为牵引车和拖车的纵向速度、侧向速度,iz1和iz2分别为牵引车和拖车的转动惯量,θ为牵引车航向角,δ为牵引车前轮转向角,β1和β2分别为牵引车和拖车的质心侧偏角,ω1和ω2分别为牵引车和拖车的横摆角速度,fyf和fyr分别为牵引车的前轮和后轮侧向力,fys为拖车的侧向力,fyh1和fyh2为牵引车和拖车在铰接处的相互作用力,a1、b1分别为牵引车前轴后轴到其质心的距离,a2、b2分别为铰接点和拖车后轴到其质心的距离,c为牵引车质心到铰接点的距离。
轮胎侧向力满足如下线性关系:
fy=kiα
式中ki为各轮胎侧偏刚度;
将变量关系和角度关系带入方程组中得:
根据车辆的横向误差模型得到如下关系:
其中,ey和δψ分别为车辆和参考路径的横向位置误差和航向角误差,k为参考路径的曲率。
进一步,所述步骤s2具体包括:选取状态量
η(t)=c(t)x(t)
式中a=-m-1p,b=m-1w,d=m-1q,其中:
w=[-k1-(a1+c)k10000]t
q=[00000-vx1]t
其中,ω(t)为道路曲率。
进一步,所述步骤s2具体还包括:将牵引式挂车的状态方程用前向欧拉进行离散化:
ad=i+ta(t)
bd=tb(t)
由此可得:
x(k+1)=adx(k)+bdu(k)+ddω(k)
η(k)=cdx(k)
将上述方程转化,设定:
得到新的状态空间表达式:
其中,
y(k)=ψtx(k)+θtδu(k)+φtw(k)
其中:
其中,np、nc分别表示预测时域和控制时域;
接着设定优化目标函数为:
式中,ρ为权重系数,ε为松弛因子,q、r为权重矩阵,η(k+i)为系统预测时域内的输出,δu(k+i)为系统控制时域内的控制增量。
进一步,所述步骤s2具体还包括:定义预测时域内的输出偏差为:
e(k)=yref(k)-ψtx(k),yref=[ηref(k+1),…,ηref(k+np)]t
其中,ηref(k+np)为系统的参考输出。
经过相应的矩阵运算,将优化目标函数调整为:
j(x(k),u(k-1),δu(k))=[δu(k)t,ε]tht[δu(k)t,ε]+gt[δu(k)t,ε]+pt
式中:
进一步,所述步骤s2具体还包括:横向跟踪模型预测控制在每一步的带约束优化求解问题都等价于求解如下的二次规划问题:
δumin≤δu(k)≤δumax
ymin-ε≤ψtx(k)+θtδu(k)+φtw(k)≤ymax+ε
在每个控制周期内完成对二次规划问题的求解后,得到了控制时域内的一系列控制输入增量:
根据模型预测控制的基本原理,将该控制序列中第一个元素作为实际的输入控制量作用于系统,即:
本发明的有益效果在于:本发明以牛顿第二定律为条件建立牵引式挂车的横向动力学建模,再结合车辆的横向误差模型得到牵引式挂车的横向跟踪模型,再利用mpc对牵引式挂车的横向跟踪模型进行精确预测控制,并满足多种物理约束。最后以牵引挂车的前轮偏角为输出,根据牵引式挂车的模型求解出其下一时刻的所有轨迹状态参数并进行下一时刻的预测控制。在下一个周期中再重复上述过程,从而实现对给定参考路径精确和稳定的跟踪行驶。本发明创造性地将mpc用于对牵引式挂车的横向跟踪模型进行跟踪控制,在提高了货物运输效率的同时能有效减少交通事故的发生。
本发明的其他优点、目标和特征在某种程度上将在随后的说明书中进行阐述,并且在某种程度上,基于对下文的考察研究对本领域技术人员而言将是显而易见的,或者可以从本发明的实践中得到教导。本发明的目标和其他优点可以通过下面的说明书来实现和获得。
附图说明
为了使本发明的目的、技术方案和优点更加清楚,下面将结合附图对本发明作优选的详细描述,其中:
图1为本发明所述轨迹跟踪方法的总体示意图;
图2为牵引式挂车动力学模型;
图3为mpc原理示意图;
图4为车辆的横向误差模型示意图。
具体实施方式
以下通过特定的具体实例说明本发明的实施方式,本领域技术人员可由本说明书所揭露的内容轻易地了解本发明的其他优点与功效。本发明还可以通过另外不同的具体实施方式加以实施或应用,本说明书中的各项细节也可以基于不同观点与应用,在没有背离本发明的精神下进行各种修饰或改变。需要说明的是,以下实施例中所提供的图示仅以示意方式说明本发明的基本构想,在不冲突的情况下,以下实施例及实施例中的特征可以相互组合。
请参阅图1~图4,图1为本发明所述基于mpc的牵引式挂车轨迹跟踪方法的总体示意图,该方法具体包括以下步骤:
s1:构建牵引式挂车横向跟踪模型,包括横向动力学模型和横向误差模型;
s2:采用mpc对牵引式挂车的横向跟踪模型进行预测控制,具体包括:
根据牛顿第二定律,对牵引式挂车进行动力学建模,方程组如下:
其中,牵引式挂车中各变量满足如下关系:
vx1=vx2
fyh2=fyh1
vy2=vx1·β2
其中存在的角度关系如下:
其中,m1和m2分别为牵引车和拖车的质量,vx1、vx2、vy1、vy2分别为牵引车和拖车的纵向速度、侧向速度,iz1和iz2分别为牵引车和拖车的转动惯量,θ为牵引车航向角,δ为牵引车前轮转向角,β1和β2分别为牵引车和拖车的质心侧偏角,ω1和ω2分别为牵引车和拖车的横摆角速度,fyf和fyr分别为牵引车的前轮和后轮侧向力,fys为拖车的侧向力,fyh1和fyh2为牵引车和拖车在铰接处的相互作用力,a1、b1分别为牵引车前轴后轴到其质心的距离,a2、b2分别为铰接点和拖车后轴到其质心的距离,c为牵引车质心到铰接点的距离。
轮胎侧向力满足如下线性关系:
fy=kiα
式中ki为各轮胎侧偏刚度;
将变量关系和角度关系带入方程组中得:
根据车辆的横向误差模型得到如下关系:
其中,ey和δψ分别为车辆和参考路径的横向位置误差和航向角误差,k为参考路径的曲率。
选取状态量
η(t)=c(t)x(t)
式中a=-m-1p,b=m-1w,d=m-1q,其中:
w=[-k1-(a1+c)k10000]t
q=[00000-vx1]t
其中,ω(t)为道路曲率。
将牵引式挂车的状态方程用前向欧拉进行离散化:
ad=i+ta(t)
bd=tb(t)
由此可得:
x(k+1)=adx(k)+bdu(k)+ddω(k)
η(k)=cdx(k)
将上述方程转化,设定:
得到新的状态空间表达式:
其中,
y(k)=ψtx(k)+θtδu(k)+φtw(k)
其中:
其中,np、nc分别表示预测时域和控制时域;
接着设定优化目标函数为:
式中,ρ为权重系数,ε为松弛因子,q、r为权重矩阵,η(k+i)为系统预测时域内的输出,δu(k+i)为系统控制时域内的控制增量。
定义预测时域内的输出偏差为:
e(k)=yref(k)-ψtx(k),yref=[ηref(k+1),…,ηref(k+np)]t
其中,ηref(k+np)为系统的参考输出。
经过相应的矩阵运算,将优化目标函数调整为:
j(x(k),u(k-1),δu(k))=[δu(k)t,ε]tht[δu(k)t,ε]+gt[δu(k)t,ε]+pt
式中:
δumin≤δu(k)≤δumax
ymin-ε≤ψtx(k)+θtδu(k)+φtw(k)≤ymax+ε
在每个控制周期内完成对二次规划问题的求解后,得到了控制时域内的一系列控制输入增量:
根据模型预测控制的基本原理,将该控制序列中第一个元素作为实际的输入控制量作用于系统,即:
s3:以牵引式挂车的前轮偏角为控制量,实现牵引挂车对期望路径的跟踪行驶,并根据牵引式挂车的横向跟踪模型求解出其下一时刻的所需状态量,再结合参考轨迹信息,进行下一时刻的预测控制。
最后说明的是,以上实施例仅用以说明本发明的技术方案而非限制,尽管参照较佳实施例对本发明进行了详细说明,本领域的普通技术人员应当理解,可以对本发明的技术方案进行修改或者等同替换,而不脱离本技术方案的宗旨和范围,其均应涵盖在本发明的权利要求范围当中。