基于动力学模型的履带无人车辆轨迹跟踪控制器设计方法与流程

1.本发明涉及车辆控制技术领域，具体涉及一种基于动力学模型的轨迹跟踪控制器的设计方法。


背景技术：

2.目前国内外研究学者对履带式自主平台的轨迹跟踪控制进行了大量研究，但存在以下几点不足：
3.1.研究对象都是低速运动的小型履带式移动机器人，仿真车速不超过2m/s。并且仿真时给定的轨迹过于单一，普遍是跟踪一段直线或者圆形轨迹。因此控制算法在车辆高速运动条件下跟踪复杂轨迹时的准确性有待进一步验证。
4.2.控制算法主要基于理想的车辆运动学模型进行构建，没有考虑到打滑对车辆运动造成的影响，在复杂道路条件下跟踪精度会有所降低。
5.3.仿真结果没有体现出跟踪过程中车辆速度和加速度的变化情况，评价指标只有跟踪轨迹的精确度，缺乏对车辆行驶稳定性和跟踪速度的考虑。
6.由于模型预测控制算法能够有效结合车辆的约束条件对目标函数进行反复的在线实时优化，并能根据系统实际状态与预测控制之间的误差对预测值进行实时修正，能够有效克服系统不确定性产生的扰动，消除模型误差，非常适合于求解不能精确建立数学模型且存在约束条件的控制系统。因而基于模型预测控制算法来设计履带无人车辆的轨迹跟踪控制器，可使无人车辆在高速运动和非结构化道路环境下能够准确、稳定的跟踪参考轨迹。


技术实现要素：

7.有鉴于此，本发明提供了一种基于动力学模型的履带无人车辆轨迹跟踪控制器设计方法，能够保证车辆在各种速度条件下都具有高精度且稳定的跟踪能力。
8.基于动力学模型的履带无人车辆轨迹跟踪控制器设计方法，该方法的实现步骤如下：
9.步骤一：根据履带车辆动力学模型建立状态空间方程；
10.步骤二：根据设定的目标函数进行标准二次型转化；
11.步骤三：在下一个控制周期内系统通过目标函数的优化过程计算出新的控制序列，反复进行滚动优化，从而实现轨迹跟踪控制。
12.进一步地，所述步骤一中履带车辆动力学模型建立状态空间方程的过程如下：
13.根据履带车辆动力学模型以及车辆局部坐标与全局坐标的转换关系，得到式(1.1)所示的状态方程：
[0014][0015]
将其表示为如下的非线性模型：
[0016][0017]
设定车辆的状态量为控制量μ(t)＝[ω
l
,ωr]
t
，输出量为
[0018][0019]
将(1.2)所示的非线性系统进行线性化和离散化，得到的状态空间方程表示为：
[0020][0021]
其中，其中，
[0022]
进一步地，所述步骤二中设定目标函数和进行标准二次型转化的过程如下：设定目标函数：
[0023][0024]
将上述目标函数转化为如下标准二次型：
[0025][0026]
其中，已知状态量个数n＝6，控制量个数m＝2，输出量个数p＝3，设定预测时域h
p
＝60，控制时域hc＝15，控制周期t＝0.1可以得到：
[0027][0028][0029][0030]
p
t
＝ε(t)
t
qeε(t)
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(1.9)
[0031]
其中，y
ref
为预测时域内的参考输出量；在t时刻给定参考轨迹点在预测时域内根据车辆动力学模型预测系统输出，并作为参考输出，计算过程如下：
[0032][0033][0034]
其中，每一时刻的系统状态量均可以从车辆传感器获得，则系统参考输出矩阵为
[0035][0036]
设定如下约束条件：
[0037]
(a)两侧履带转速约束
[0038]umin-u(t-1)≤mδu(t)≤u
max-u(t-1)
ꢀꢀꢀꢀꢀꢀꢀ
(1.13)
[0039]
其中，u(t-1)为前一时刻车辆反馈的两侧履带转速；
[0040]
(b)转速增量约束
[0041]
δu
min
≤δu(t)≤δu
max
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(1.14)
[0042]
其中，
[0043]
(c)纵向加速度约束
[0044]
a1δu≤b1ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(1.15)
[0045][0046]
[0047]
其中，
[0048]
(d)侧向加速度约束
[0049][0050]
其中，v为上一时刻测得的车辆纵向速度，ω
l
(t-1),ωr(t-1)分别为上一时刻测得两侧履带转速；
[0051]
根据以上建立的目标优化函数和设定的约束条件，将线性时变预测控制问题转化为如下的标准二次规划问题；
[0052]
建立的标准二次型表示如下：
[0053][0054]
有益效果：
[0055]
本发明设计的控制器在高速条件下保证了车辆具有良好的跟踪精度，最大跟踪误差不超过0.6m，直线跟踪稳态误差趋近于0，在高速转向时纵向和侧向加速度都保持在设定的约束范围内，使得车辆具有良好的转向稳定性。因此，基于动力学模型建立的控制器能够保证车辆在各种速度条件下都具有精度而稳定的跟踪能力。
附图说明
[0056]
图1为本发明实现的步骤流程图；
[0057]
图2为10m/s速度下连续曲线的参考轨迹和仿真轨迹变化曲线图；
[0058]
图3为10m/s速度下连续曲线的状态量变化曲线图；
[0059]
图4为10m/s速度下连续曲线的状态量误差变化曲线图；
[0060]
图5为10m/s速度下连续曲线的两侧转速变化曲线图；
[0061]
图6为10m/s速度下离散点的参考轨迹和仿真轨迹变化曲线图；
[0062]
图7为10m/s速度下离散点的状态量误差变化曲线图；
[0063]
图8为10m/s速度下离散点的纵向和侧向车速变化曲线图。
具体实施方式
[0064]
下面结合附图并举实施例，对本发明进行详细描述。
[0065]
如附图1所示，本发明提供了一种基于动力学模型的履带无人车辆轨迹跟踪控制器设计方法，该方法的实现步骤如下：
[0066]
步骤一：根据履带车辆动力学模型建立状态空间方程；
[0067]
根据履带车辆动力学模型以及车辆局部坐标与全局坐标的转换关系，得到式(1.1)所示的状态方程：
[0068][0069]
将其表示为如下的非线性模型：
[0070][0071]
设定车辆的状态量为控制量μ(t)＝[ω
l
,ωr]
t
，输出量为
[0072][0073]
将(1.2)所示的非线性系统进行线性化和离散化，得到的状态空间方程表示为：
[0074][0075]
其中，其中，
[0076]
步骤二：根据设定的目标函数进行标准二次型转化；
[0077]
首先设定目标函数：
[0078][0079]
将上述目标函数转化为如下标准二次型：
[0080][0081]
其中，已知状态量个数n＝6，控制量个数m＝2，输出量个数p＝3，设定预测时域h
p
＝60，控制时域hc＝15，控制周期t＝0.1可以得到：
[0082][0083][0084][0085]
p
t
＝ε(t)
t
qeε(t)
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(1.9)
[0086]
其中，y
ref
为预测时域内的参考输出量；在t时刻给定参考轨迹点在预测时域内根据车辆动力学模型预测系统输出，并作为参考输出，计算过程如下：
[0087][0088]
[0089]
其中，每一时刻的系统状态量均可以从车辆传感器获得，则系统参考输出矩阵为
[0090][0091]
设定如下约束条件：
[0092]
(a)两侧履带转速约束
[0093]umin-u(t-1)≤mδu(t)≤u
max-u(t-1)
ꢀꢀꢀꢀꢀꢀꢀ
(1.13)
[0094]
其中，u(t-1)为前一时刻车辆反馈的两侧履带转速；
[0095]
(b)转速增量约束
[0096]
δu
min
≤δu(t)≤δu
max
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(1.14)
[0097]
其中，
[0098]
(c)纵向加速度约束
[0099]
a1δu≤b1ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(1.15)
[0100][0101]
其中，
[0102]
(d)侧向加速度约束
[0103][0104]
其中，v为上一时刻测得的车辆纵向速度，ω
l
(t-1),ωr(t-1)分别为上一时刻测得两侧履带转速；
[0105]
根据以上建立的目标优化函数和设定的约束条件，将线性时变预测控制问题转化为如下的标准二次规划问题；
[0106]
建立的标准二次型表示如下：
[0107][0108]
步骤三：在下一个控制周期t内系统通过目标函数的优化过程计算出新的控制序列，反复进行滚动优化，从而实现轨迹跟踪控制。
[0109]
在matlab中采用有效集法(active-set)对上述标准二次问题进行求解，得到最优控制序列：取序列中的第一个元素作为控制输入量，即：
[0110][0111]
为了验证轨迹跟踪控制器在高速和复杂路径条件下的轨迹跟踪能力，设计了高速
条件下连续曲线和离散点的跟踪仿真实验。无人车辆相关结构参数如表1所示
[0112]
表1履带无人车辆结构参数
[0113][0114]
其中m为整车质量，l为履带接地长，b为履带中心距，b为履带板宽度，iz为车辆绕z轴的转动惯量，f为滚动阻力系数，μ为履带与地面之间的摩擦系数，k为履带与地面之间的剪切模量，r为主动轮半径，c
x
、cy分别为质心与车辆局部坐标系的横向和纵向距离。
[0115]
其中，航向角的定义为：x轴正方向为0
°
，y轴正方向为90
°
，逆时针方向依次增加，取值范围为[0
°
，360
°
)。
[0116]
实施例1：10m/s速度下连续曲线的轨迹跟踪仿真
[0117]
仿真车速设置为10m/s，仿真时间为110s，采用四阶龙格库塔算法，仿真步长设置为0.1s。在0-20s内跟踪直线，20s-60s内跟踪圆形，60s-110s跟踪双移线，得到的仿真结果如图2所示。
[0118]
从图2-4可以看出，基于动力学模型的控制器最大轨迹误差不超过1m，行驶路径基本上覆盖了期望路径，没有出现较大的路径偏差，且运动轨迹光滑。从图5可以看出，基于动力学模型的控制器两侧转速变化曲线更加光滑，说明车辆行驶过程中不会出现急减速或者急加速的情况，车辆的行驶平顺性更好。
[0119]
基于动力学模型的控制器在高速运动跟踪连续的轨迹曲线时具有更高的跟踪精度，行驶路径与期望路径的覆盖程度更高，行驶稳定性更好。最终统计x方向跟踪误差的均方根值为0.191m，y方向跟踪误差的均方根值为0.165m。
[0120]
实施例2：10m/s速度下离散点的轨迹跟踪仿真
[0121]
仿真车速设置为10m/s，仿真时间为110s，采用四阶龙格库塔算法，仿真步长设置为0.1s。离散的轨迹点有10个，在离散轨迹点间进行线性插值后为9段折线。得到的仿真结果如下。
[0122]
从图6可以看出，基于动力学模型建立的控制器在高速条件下具有良好的跟踪性能，在转折点处出现了超调，后续误差逐渐收敛，行驶路径基本覆盖了期望路径，尤其是在直线跟踪时几乎与期望路径重合。从图7可以看出，控制器在x和y方向的跟踪误差不超过1.8m，在10m/s的车速下依然能够精确而稳定的跟踪期望轨迹。基于动力学模型建立的控制器最大跟踪误差不超过1.2m，直线跟踪不超过0.6m，最后10s的跟踪偏差趋近于0，具有较高的跟踪精度。最终统计x方向跟踪误差的均方根值为0.223m，y方向跟踪误差的均方根值为0.205m。
[0123]
从图8可以看出，车辆在转折点处车速会有所降低，直线跟踪时车速能基本保持在10m/s。由于进行了侧向加速度约束，纵向和侧向加速度不超过2m/s2，横摆角速度不超过0.5rad/s，角加速度不超过2rad/s2，两侧滑移率在转折点处不超过0.2，因此控制器在车辆高速转向时依然具有良好的纵向和侧向稳定性。
[0124]
仿真结果表明，基于动力学模型建立的控制器在车辆高速运动跟踪航向反复变化
的离散点时轨迹过渡更加平滑，在转折点处的轨迹偏离现象更小。跟踪结果更加精确，车辆运动更加稳定。
[0125]
综上所述，以上仅为本发明的较佳实施例而已，并非用于限定本发明的保护范围。凡在本发明的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。