基于拉盖尔函数的AUV自适应预测轨迹跟踪

闫景昊， 王伟然， 杨冠军， 朱志宇， 曾庆军， 智鹏飞

(江苏科技大学电子信息学院，江苏 镇江 212000)

0 引言

无人自治水下机器人(AUV)作为人类探索和开发海洋的利器，已经引起世界各国研究者的广泛关注[1]。近年来，AUV已被广泛应用于科学、工业、商业和军事等领域。由于工作环境中存在复杂的外部干扰，所以在这种情况下运行时，AUV要求运动控制器具有较强的抗干扰能力和快速的反应能力。但是，运动控制过程中如数据计算量过于庞大，会导致AUV响应速度过慢，严重时会偏离预设轨迹甚至于失联。如何减小数据计算量，提高AUV的控制精度、动态性能以及复杂环境下的适应能力，成为当前研究的热点[2-4]。

目前，多数关于轨迹跟踪控制文献[5-7]中，常用的控制方法有滑模控制[8-9]、反步控制[10-11]、自适应控制[12-13]等。复杂水文条件下水下机器人轨迹跟踪时，物理模型的限制会对控制器性能造成一定影响，而上述几种控制方法均没有涉及此问题。模型预测控制(MPC) 可以将控制约束和状态约束纳入优化计算。此外，其可以运用于线性系统和非线性系统对象的控制器设计，在水下机器人轨迹跟踪中具有广泛的应用[14-15]。ZHANG等[16]设计了一种基于MPC的三维轨迹跟踪方法，针对AUV的轨迹跟踪控制有较好的效果，对MPC在AUV中的轨迹跟踪有一定的参考价值；YAN等[17]基于MPC设计了双闭环三维轨迹跟踪算法，外环运动学控制器生成速度指令后传递给内环动力学控制器进一步生成控制力矩，并考虑系统的输入和状态约束，但是未考虑外部水流等复杂干扰对模型参数的影响;PENG等[18]设计了扩展状态观测器来估计位置的内部动态特性和外部干扰，但是只在二维垂直面进行了验证，并未考虑三维空间内存在复杂干扰时的运动情况；文献[19-20]针对具有参数不确定性和扰动的离散线性系统设计了自适应预测控制算法，对带有参数不确定性的AUV系统具有一定的参考价值；文献[21-22]针对预测控制器的动态响应能力设计了基于拉盖尔函数的MPC，极大地提高了计算速度，但并未考虑系统参数不确定性对控制效果的影响。从以上分析中可以看出，参数不确定性和庞大的计算量在一定程度上限制了MPC控制器的性能。因此，如何补偿参数不确定性和减小计算量提高响应速度对AUV的轨迹跟踪具有重要意义。

本文设计了一种基于拉盖尔函数的AUV自适应预测轨迹跟踪控制方法。首先，建立AUV在三维空间内的运动控制模型，引入预测控制方法，设计基于预测控制器的AUV控制模型；然后，为了消除AUV模型在近似线性化过程中的误差和轨迹调整时参数变化所导致的鲁棒性下降，在预测控制算法的基础上，引入递推最小二乘法设计自适应算法，实时在线估算参数的变化量，实现自适应预测控制的参数调整。此外，水下环境存在复杂的外部水流干扰以及AUV自身预设跟踪轨迹的大幅度改变，这些均需要大量的控制量求解运算。本文利用拉盖尔函数正交化的特点，将数量较多的被优化控制输入转化为数量较少的拉盖尔系数，牺牲部分精确性，但是能极大程度减小预测控制中的计算量，从而大幅提高系统的响应速度。

1 水下机器人运动建模

本文所述的AUV在结构上关于轴线上下、左右对称。主推进器置于AUV艉部，用于实现对AUV的纵荡运动控制，首尾各两个垂向与舷向辅助推进器可以实现对纵摇艏摇的控制。忽略安装误差，通过主/辅推进器的配合，AUV在空间上具有全驱运动的特性。AUV在大地坐标系中的运动由6个自由度组成，即描述直线运动的3个平移分量(纵荡x、横荡y、垂荡z)，描述旋转运动的3个旋转分量(横摇角φ、纵摇角θ和艏摇角ψ)。AUV的体坐标系的原点与重心重合，在体坐标系下的运动学与动力学模型方程分别为

(1)

(2)

(3)

2 AUV轨迹跟踪控制策略

2.1 轨迹跟踪预测控制模型

设采样时间为T，在任意时刻k对式(3)离散化后得到线性时不变状态方程为

(4)

(5)

其中:a(k)=I5+aT,I5为五阶单位矩阵;b(k)=bT;c=diag(I3，02),I3为三阶单位矩阵，02为二阶零矩阵。

(6)

在实际运动过程中，AUV受到自身转角以及推进系统中电机转速的限制，状态向量和输入向量必须始终在一定的范围内，即

(7)

式中，Umin,Umax和Xmin,Xmax分别为输入量与状态量的下限和上限。优化控制器可以使AUV快速且平稳地追踪期望轨迹，传统预测控制中带有约束的优化目标函数为

J=||

CA(k)x(k)+

(8)

s.t.Umin≤U(k)≤Umax,Xmin≤X(k)≤Xmax

式中:Yd(k)为给定的参考输出,Y(k)=[yd(k+1|k)yd(k+2|k)…yd(k+Np|k)]T；Q=diag(Q1,Q2,…,QNP),为误差控制加权矩阵；R=diag(R1,R2,…,RNc),为输入控制加权矩阵。由式(8)还能看出，控制器的控制量计算添加了AUV速度和状态的约束，使得优化求解变得复杂，降低了控制器的计算速度。

2.2 自适应预测控制器

由于MPC控制器在设计时，非线性的AUV运动方程在参考轨迹点进行了近似线性化，从预测模型可以看出模型参数随时间变化剧烈，这导致控制器鲁棒性变差，难以取得良好的控制效果。为了解决上述问题，本文根据递推最小二乘法设计自适应更新算法[23-24]。在每个控制周期开始时更新修正控制模型，使预测模型方程可以更好地描述水下机器人实时轨迹跟踪时的状态。

根据系统预测方程式(6)设计估计系统，即

(9)

将预测方程和估计系统方程重写为紧凑形式，即

x(k+1)=Θ(k)Φ(k)

(10)

(11)

(12)

(13)

(14)

采用梯度法使代价函数最小化可得

(15)

式中，λ>0,为自适应更新率。此外，为了使系统带有约束的目标函数式(8)成立，λ还要满足

(16)

式中:0<γ<2；Umax为控制量U的上限；Φmax为不确定参数Φ的上界。

2.3 基于拉盖尔函数的自适应预测控制器

由于AUV控制系统采样迅速，要实现更好的控制效果和稳定性,往往在设计预测控制器时采用更长的预测时域和控制时域,这样就不可避免地在每个周期内都存在着庞大的计算量。因此设计基于拉盖尔函数的预测控制，通过参数化控制序列，减少预测时域内约束的数量，从而减少每个控制周期内涉及的参数数量，以达到优化和减少控制器的运算量、提高系统整体的计算速度、增强实时性的目的[25]。拉盖尔函数的z传递函数为

(17)

式中:N为拉盖尔函数的阶数；α为拉盖尔函数的极点，称为比例因子，取值范围为0≤α<1；z为z变换中的相关变量。将式(17)中各项进行逆z变换后得到离散时间拉盖尔函数

L(k)=[l1(k)l2(k)l3(k)…lN(k)]T

(18)

式中,l1(k)表示Γ1(z)的逆z变换，即l1(k)=z-1·(Γ1(z))。式(18)的递推形式可写为

L(k+1)=AlL(k)

(19)

式中:

(20)

特别地，当α=0时，式(20)所示的拉盖尔函数转化为脉冲函数。假设当前时刻为k0，则在ki时刻后的控制量可以由拉盖尔函数表示为

(21)

即

(22)

由式(22)可知，通过选择合适的α，N参数值，能在保证系统动态性能的同时减少设计参数的数量，从而减小计算量。

此时，基于拉盖尔函数预测第m步的输出变量为

(23)

根据式 (22),(23)对传统预测控制的优化目标函数式(8)进行重构可得到

(24)

式中，利用拉盖尔函数的标准正交性质

(25)

根据拉盖尔函数的标准正交性，当预测时域Np足够大时,有

(26)

(27)

通过使用拉盖尔函数，可以减少预测范围内约束的数量，从而减小计算负荷。

3 仿真结果及分析

为了验证本文所提方法的有效性和可靠性，采用了Matlab/Simulink软件对AUV在三维空间内的运动来完成仿真。

3.1 三维曲线轨迹跟踪仿真

本文首先进行三维折线轨迹跟踪仿真。三维折线参考轨迹为

(28)

设水下机器人的初始位置在大地坐标系中为坐标原点，对应的初始状态即为[00000]T。

基于拉盖尔函数的自适应预测控制器(LF-AMPC)、自适应预测控制器(AMPC)、传统预测控制器(MPC)在复杂干扰下的三维折线轨迹跟踪性能如图1所示，其细节如图2所示。

图1 复杂干扰下的三维折线轨迹跟踪仿真Fig.1 Simulation of three-dimensional,broken-line trajectory tracking under complex interference

图2 复杂干扰下的三维折线轨迹跟踪仿真细节Fig.2 Simulation details of three-dimensional,broken-line trajectory tracking under complex interference

从图1中可看出，在存在复杂干扰的情况下进行三维折线轨迹跟踪任务时，采用MPC的系统在初始时刻的实际轨迹与参考轨迹之间的误差较大，并且抗干扰能力较差，响应速度明显落后于LF-AMPC和AMPC。采用AMPC的系统响应速度较采用MPC的系统有较大提升，并且抗干扰能力明显增强，实际轨迹与参考轨迹间的误差大大减少。而采用LF-AMPC的系统的响应速度较AMPC进一步加快，并且实际轨迹与参考轨迹之间几乎没有误差，抗干扰能力大大增强。

图2(a)是复杂干扰下的AUV三维折线轨迹跟踪仿真的起始时刻细节图。从图中可看出，在该阶段，AUV的实际位置与预测轨迹的初始位置有较大差异，采用LF-AMPC控制器的AUV能够迅速响应，在X轴上运动2 m时已跟踪上参考轨迹，并且实际轨迹与参考轨迹的误差几乎为0 m。由此可以看出，采用LF-AMPC控制器的AUV在响应速度与跟踪精度上的优越性。图2(b)为复杂干扰下的AUV三维折线轨迹跟踪仿真左转弯90°时的细节图。从图中可看出，采用LF-AMPC的AUV转弯时与参考轨迹间的误差约为0.186 m，并且转弯后实际轨迹与参考轨迹间的误差最小，约为0.153 m。由此可以看出，在转弯前后参数变化较大的情况下AUV能够保持良好的跟踪效果，系统鲁棒性有了明显提升。图2(c) 为复杂干扰下的AUV三维折线轨迹跟踪仿真直行时的细节图。从图中可看出，在受到干扰时实际轨迹与参考轨迹的平均误差最小，约为0.138 m，抗干扰能力有了较大增强。图2(d) 为复杂干扰下的AUV三维折线轨迹跟踪仿真右转弯90°时的细节图。从图中可以看出，采用LF-AMPC的AUV实际轨迹较为平缓，与参考轨迹间的平均误差最小，约为0.144 m，并且能够快速完成转弯动作，跟踪效果最好，响应速度与控制精度得到明显提高。从以上分析可以看出，采用LF-AMPC的AUV跟踪效果明显优于采用AMPC和MPC的AUV系统，其鲁棒性、抗干扰能力、响应速度均得到了明显提升。

3.2 控制器计算时间对比

在使用Matlab/Simulink联合仿真时，本文采用“set_param(‘mymodel’,‘Profile’,‘on’)”代码来获得仿真程序执行时间的报告,具体数据如表1所示。

表1 运行时间对比

从表1中可看出，在针对图1设定仿真时间为100 s的情况下,采用MPC控制器的系统实际运行时间为51 s,而采用AMPC控制器的系统运行时间为63 s，较传统MPC控制器增加了约23.5%。这是因为采用MPC控制器的系统结构相对简单，采用AMPC控制器的系统加入了自适应算法增加了系统的复杂性，虽然改善了控制效果，但在一定程度上牺牲了实时性，所以采用AMPC控制器的系统实际运行时间相对较长。采用LF-AMPC控制器的系统实际运行时间为27 s。运行时间相比于采用传统MPC控制器和AMPC控制器的系统分别减少47.06%和57.14%。采用LF-AMPC控制器的系统虽然也存在自适应算法，但引入的拉盖尔函数使得系统求解速度变快，所以LF-AMPC控制器能够同时改善控制效果并加快计算速度。

4 结论

针对复杂水文条件下无人自治水下机器人(AUV)轨迹跟踪时面对复杂扰动而偏离预定轨迹的问题，以拉盖尔函数和最小二乘法为参考设计了一种基于拉盖尔函数的AUV自适应预测轨迹跟踪控制方法。仿真结果对比表明，利用最小二乘法可有效减小系统误差，增强AUV控制系统的抗干扰性和鲁棒性，并且超调量小，控制得到改善。通过拉盖尔函数对传统预测控制进行参数重构能够有效地减小控制器的计算负担，提高控制器的计算速度，增强动态性。此外，本文只研究了基于运动学方程的控制手段，后续工作将深入到动力学方程，使研究更加完善。