水下滑翔机轻量化建模及执行器约束下非线性MPC 控制器设计

王洁茹,李 崇,綦声波,赵圆圆

(中国海洋大学 工程学院,山东 青岛,266100)

0 引言

水下滑翔机作为一种新型智能海上移动观测平台,因其观测范围大、续航能力强及便于仪器搭载等优点,被广泛应用于海洋勘测和通信等领域,成为认识、理解和开发海洋的重要载体[1]。随着海洋科学的不断发展以及海洋研究的不断深入,水下观测任务对水下滑翔机提出了低功耗、高稳定性及精准姿态控制的要求[2]。水下滑翔机的姿态控制精度直接决定了其所采集观测数据的准确性。但水下滑翔机动力学模型复杂、建模维度过高等问题也给其运动分析及后续姿态控制带来巨大挑战。

水下滑翔机的动力学建模是分析水下滑翔机运动特性及控制器设计的重要基础。美国普林斯顿大学Leonard 教授是水下滑翔机研究领域的先驱,其以通过内部质量块的主动分配来进行姿态调节的浮力推进式固定翼滑翔机为研究对象,利用几何关系和动力学原理对水下滑翔机进行分析,建立了相应6 自由度的动力学模型[3-4]。Graver 等[5]基于拉格朗日法,建立了11 自由度的动力学模型,并完成了仿真实验及海试验证。侯巍等[6]基于牛顿-欧拉法建立了水下滑翔机的6 自由度模型,对模型进行合理简化后设计相应控制器。Song 等[7]基于拉格朗日法建立了六维度的考虑洋流和浮力损失的水下滑翔机动力学模型。范双双[8]采用参数化分析方法建立了洋流影响下的5 自由度水下滑翔机多体系统动力学模型。

总体而言,水下滑翔机的建模多基于牛顿-欧拉法、拉格朗日法等动力学原理,并考虑内部各机构的科氏力、向心力及其耦合作用以及各类水动力参数的干扰,模型较为详尽地对滑翔机的线速度和各机构位置等状态量进行了描述,但存在动力学模型维度过高、耦合程度大及欠缺对于执行器机构的实际建模等问题。

针对水下滑翔机精准姿态控制的需求,研究人员提出了一系列的控制方法。Lenaord 等[4]在简化的水下滑翔机动力学模型上设计了线性二次型调节器(linear quadratic regulator,LQR)并进行了海试验证。Mahmoudian 等[9]在所建立的动力学模型上设计了基于前馈/反馈环节的运动控制器并成功应用于水下滑翔机。Tchilian 等[10]针对水下滑翔机竖直平面上的动力学方程设计了LQR 控制器。Wang 等[11]提取水下滑翔机在竖直平面的动力学方程,设计了模糊自适应线性自抗扰控制器。孙秀军[12]利用浮基多刚体理论推导了水下滑翔机的动力学方程,使用反向传播(back propagation,BP)神经网络设计了适用于水下滑翔机的轨迹跟踪控制器,同时设计了具有高自适应和容错能力的比例-积 分-微 分(proportional-integral-derivative,PID)神经网络定深运动和姿态解耦控制器。严升等[13]对水下滑翔机的纵剖面运动方程进行精确建模,设计了PID 控制器,并完成了控制策略优化。李志超等[14]针对飞翼式滑翔机建立了纵垂面的动力学方程,设计了跟踪微分模糊PID 控制器,并通过实验验证其有效性。陈弈煿等[15]建立了水下滑翔机水平面内运动模型,构建了参数自整定的PID控制方法。综上所述,现有水下滑翔机的控制算法主要集中于PID、LQR 及其衍生算法,虽可实现良好的控制效果,但在控制算法的设计中,多针对于线性模型,尚未对滑翔机自身姿态极限、执行机构的物理约束及速率极限等实际约束问题进行研究。

文中基于拉格朗日法,针对水下滑翔机实际工作状况及执行机构延迟影响,忽略次要影响因素建立轻量级滑翔机动力学模型以降低模型复杂度,便于对水下滑翔机作运动分析及相应控制器设计。并进一步提取其纵垂面的动力学方程,基于模型预测控制理论,引入实际物理约束,设计了有效的水下滑翔机俯仰角回路控制器。仿真验证了文中模型和控制器的有效性。

1 水下滑翔机轻量化建模

1.1 动力学建模

为便于描述建模过程中各参量之间的转换,文中对相应坐标系进行定义(如图1 所示),惯性坐标系E0:(i,j,k)定义在大地坐标系下,以水下滑翔机入水点作为原点,i轴和j轴位于水平面且相互垂直,k轴则沿重力方向,以向下为正方向。机体坐标系e0:(x,y,z)以浮心作为原点,x轴沿水下滑翔机机体主轴指向其艏部,y轴垂直于x轴指向其右翼方向,z轴分别垂直于x轴和y轴,向下为正方向。速度坐标系 π0:(π1,π2,π3)的原点与机体坐标系的原点一致,将机体坐标系绕y轴旋转角度 -α,此时机体坐标z轴则为速度坐标系的 π3轴,然后绕π3轴 旋转角度 β,即可得到最终确定的速度坐标系。图1 中,θp为俯仰角;ϕ为横滚角;ψ为航向角;Vt为系统在机体坐标系下的合速度。

图1 坐标系定义Fig.1 Definition of coordinate system

水下滑翔机的建模基于拉格朗日动力学原理,为了便于建模过程中受力分析方便,根据水下滑翔机结构组成特点及工作机理,将其看成由质量块所组成的系统,包括壳体、控制系统等非运动部件的固定质量块,用以调节水下滑翔机俯仰角和横滚角的可移动质量块以及表示净浮力的浮力调节质量块,并以rs,rr,rb分别表示它们在机体坐标系下的坐标。

根据各部件质量块的受力情况,获取其在机体坐标系下的系统总动能(包括平动动能和转动动能)。浮力调节质量块主要是靠水囊的抽放水来控制,其位置固定于浮心上,不会相对于机体运动,对系统总动能并无贡献,因此水下滑翔机相对于机体坐标系下的总动能为

式中:T为总动能;Ts、Tr和Tf分别为固定质量块、可移动质量块以及水阻尼项的动能;v=[V,Ω]T为水下滑翔机机体坐标下的广义速度(包含线速度及角速度);M为广义惯性矩阵;MA为由科氏力及水动力引起的附加质量矩阵;Ct为总附加耦合项;CA为科氏力及水动力作用引起的附加耦合项;It为总惯性矩阵;Is为固定质量块的惯性矩阵;Ir为可移动质量块的惯性矩阵;IA为附加惯性矩阵;ms和mr分别为固定质量块和可移动质量块的质量。

随后,通过总动能T对速度(包括线速度和角速度)求导,获得水下滑翔机相对于机体坐标系下的总动量为

式中,定义 η=[P,Π]T为相对与机体坐标系下的动量(包括平动量和角动量分别为P和 Π)。

同时,分析水下滑翔机在惯性坐标系下的受力和力矩情况,得

式中:p和 π为水下滑翔机在惯性坐标系下的平动量和角动量;qsE,qrE,qbE为各质量块相对于惯性坐标系原点的位置;fext,τext分别为惯性坐标系下的水动力和水动力矩;kw为惯性坐标系沿z轴方向的单位向量;mb为浮力调节质量块的质量;g为重力加速度。

经过坐标转换矩阵变换后,得到其相对于机体坐标系下的总动量,并与总动能进行关联,解得水下滑翔机在机体坐标系下的合外力,从而建立滑翔机的动力学模型,即

式中:F=REB fext,T=REBτext分别为机体坐标系下的水动力和水动力力矩;REB为坐标转换矩阵;g为重力加速度;V=[V1,V2,V3]T。

水下滑翔机上浮和下潜过程中姿态角的改变主要通过执行机构(如图2 所示)中的电机动作,利用齿轮传动完成可移动质量块的平移和横滚来实现。而电机转速受限和齿轮机构传动都导致可移动质量块位置的改变需要一定时间。实际运作过程中,电机推动可移动质量块不断运动,需经过相应时间方可到达指定位置,输入量与可移动质量块移动距离之间呈现积分关系。因此结合上述内容,建立水下滑翔机最终动力学模型为

图2 水下滑翔机及其执行机构Fig.2 Underwater glider and its actuator

式中: τ为时间常数;u为系统的输入量。

1.2 模型轻量化

建模过程中,水动力项主要用于描述水下滑翔机运动过程中与周围水体相互作用所导致的额外施加于水下滑翔机系统的作用力,其大小主要取决于其速度及其速度方向[5,16]。但在实际工作中,鉴于水下滑翔机是弱驱动器,且为了采集到尽可能多的精确数据,其实际转向速度及稳定滑翔的速度较慢。因此水动力项在水下滑翔机稳定滑翔过程中所产生的影响微弱。则可依此将式(5)中的F和T忽略。

进一步地,式(5)中P×Ω、Π×Ω及P×V分别为由科氏力引起的线速度和角速度之间的耦合项。而上文分析已知,水下滑翔机稳定滑行过程中,其线速度和角速度较小,即可将上述物理量及式(5)里M所包含的相关科氏力项忽略。一般情况下,水下滑翔机是一个质量分布均匀的整体,则rs=0;而由于将浮力质量块位置固定于浮心上,则rb=0[8],基于此对M再次简化,得到最终的轻量化模型为

式中: 系统惯性矩阵Io=Is+Ir和θ=[ϕ,θp,ψ]T分别为定义在机体坐标系下的线速度和角速度。

1.3 纵垂面轻量化建模

水下滑翔机在工作状态下的主要运动形式是在纵垂面上进行“V”型锯齿状运动,通过所搭载传感器完成对各类海洋数据的收集。因此,文中针对水下滑翔机的主要工作模式,提取滑翔机在纵垂面的轻量级动力学模型,并基于此进行后续运动分析及控制器设计。将水下滑翔机的运动约束在纵垂平面上(即机体坐标系下x-y平面及惯性坐标系下的i-k平面),有

式中: θp为水下滑翔机的俯仰角;Io2为其相对于y轴的惯性矩阵分量。

由文献[16]得水下滑翔机机体长度Lglider=1.99 m,机身直径Dglider=0.22 m,则rr33与rr11为十分微小的变化量,将其近似为一个常数,即mr(2rr33+2rr11)=D,则式(7)可化为

2 基于模型预测控制的水下滑翔机姿态控制算法设计

模型预测控制(model predictive control,MPC)是近年来被广泛应用于工业生产的一种反馈控制策略,相较于其他控制算法,其能够处理含有约束条件的多变量非线性控制问题,实现多目标优化[17]。针对水下滑翔机控制的相应算法应具备低计算量、低功耗的特性[18],文中已建立了轻量化水下滑翔机动力学模型,以减轻所设计控制器的计算负担,下面将基于MPC 设计水下滑翔机的姿态控制器。

2.1 模型近似线性化和离散化

水下滑翔机进行俯仰角调节时,主要通过执行机构在x轴方向的平移运动实现。因此,基于上文推导的水下滑翔机动力学模型(式(8)),将rr3置0,即可得到水下滑翔机的俯仰角回路方程为

根据目标轨迹对系统模型进行线性化。假设系统的参考轨迹为

将偏差量xs-xr,uin-ur设置成新的变量和。f相对于参考状态量的雅克比矩阵为A(t),相对于参考控制量的雅克比矩阵为B(t),则上式变为

利用欧拉法对式(13)进行离散化,可得离散化后的模型为

式中:Akt,t=A(t)·Δt+I,Δt为采样间隔时间,I为单位矩阵;Bk,t=B(t)·Δt;Pout为输出矩阵。

2.2 目标函数设计

为使水下滑翔机平稳、快速地跟踪期望轨迹,以系统的控制量、控制增量以及状态量偏差来构造目标函数,并加入终端不等式项确保系统的稳定性。

式中:Np为预测时域;Nc为控制时域;Q和R为权重系数矩阵;Pε为终端惩罚矩阵。

定义

则式(16)可以写成紧凑的矩阵形式

根据模型预测控制原理,基于式(15)预测系统未来的动态,可得其预测状态序列,写成矩阵形式为

进一步,可得系统的预测输出序列为

由式(19)和式(20)得

联合式(18)和式(21)得

系统经过一段时间到达终端域后,在终端域内可以通过无约束线性反馈控制系统。结合式(15),必能找到一个线性反馈增益K,即存在一个线性状态反馈u=Kε,保持kt,t+kt,tK渐进稳定,且总是存在一个邻域 Ωε,使得系统满足控制量约束和输出约束。

通过求解Lyapunov 方程

即可得唯一确定的正定阵Pε(即终端惩罚矩阵)。

2.3 约束条件设置

考虑到水下滑翔机自身姿态极限,如俯仰角不能超过 ±90°、执行机构的运动范围最大不超过机体长度以及执行电机的功率及转速有限等因素,通过设置umin、umax、Δumin、Δumax、ymin和ymax,对其输入量、输入增量及系统输出量进行约束。为保证闭环系统的稳定性,另加入终端约束ε(kt+Np|kt)∈Ωε。

最终设置约束条件如下

将约束条件(24)与目标函数(22)相结合,可将该控制问题转化成以下优化求解问题

假设该优化问题有解,记为ΔU*(kt),根据滚动优化的原理,只取第1 个元素作为控制输入增量,即

因此,定义每个时刻作用于系统的实际非线性控制量为

2.4 稳定性证明

假设在kt时刻优化问题有解,记为

则相对应得预测状态序列和预测输出序列为

假设系统无干扰及建模误差,则

在kt+1时刻,预选一个预测控制序列为

ΔUkt+1的 前Nc-1个元素为kt时刻优化解的后Nc-1个元素,在此基础上选取最后一个元素为Kε*(kt+Nc|kt)。

此时对应的预测状态序列和预测输出序列为

由于最优解必定优于可行解,有

由此得J*kt是单调递减的。当且仅当y¯=0,Δu=0,ε=0时其值为0,即闭环系统是渐进稳定的。

3 仿真实验

3.1 运动仿真

针对上文所建水下滑翔机纵垂面动力学方程(8),并与轻量化建模前的模型进行对比,在Matlab平台上进行仿真实验。

仿真中所用的模型参数参考文献[16],具体见表1。表中:KM、KM0和Kq分别为计算水动力时所用到的系数;IA2为附加惯性矩阵分量;Is2为固定质量块的惯性矩阵分量;Ir2为可移动质量块的惯性矩阵分量。

表1 实验参数列表Table 1 List of experimental parameters

分析已知,水下滑翔机主要通过改变电池包在x轴方向上的位置改变俯仰角,从而实现在纵垂面的姿态调整,因此令rr3=0 m。选取初始状态量为: 俯仰角角速度=0(°)/s,俯仰角 θ0=0,可移动质量块在x轴上的移动距离rr1_0=0 m;初始控制输入为: 系统控制输入u=0 m/s2。

图3为不同输入阶跃信号下俯仰角变化曲线。其中,以u作为输入,分别在30 s 时加入0.2 m/s2和-0.2 m/s2的阶跃信号,可以发现,在相同的输入作用下,两者的运动变化曲线基本重合,运动变化趋势一致,验证了文中所建立轻量化模型的可行性。

图3 不同输入阶跃信号下俯仰角变化曲线Fig.3 Change curves of pitch angle under different input step signals

分析图3 可知: 当u为零时,俯仰角 θp角度不变,角速度不变,可移动质量块的位置rr1保持不变;当u为常值时,俯仰角以固定速率变化并最终稳定于 ±90°,角速度随着俯仰角的变化而变化,可移动质量块的位置则随着输入作用以一定速率不断变化。显然,输入信号数值相同,符号相反,俯仰角及其角速度以及可移动质量块位置的绝对值相同,变化趋势相反,符合滑翔机的工作原理,验证了所建立轻量化模型的有效性。

3.2 控制器仿真

针对水下滑翔机俯仰角回路的控制需求,利用所设计控制器对滑翔机俯仰角进行控制。滑翔机搭载声学多普勒海流记(acoustic doppler dual current profiler,AD2CP)进行海流观测时,为了保证所采集数据的精确性,其最佳工作状态为±17.4°[19];为了实现长航程观测,其最佳工作状态为± 22.5°[20]。因此分别针对水下滑翔机以上2 种工作状态检验所设计控制器。

1) 工作状态为±17.4°时

预测时域Np作为模型预测控制算法中的重要参数,代表算法预测未来动态的时长,表示控制器对系统未来发展趋势的预测程度,对控制效果有直接影响。因此,首先设置不同的预测时域对比不同的输出结果,选择不同参数如下:Np=80,Nc=80;Np=100,Nc=80;Np=160,Nc=80。对应的结果输出如图4 所示。

图4 不同预测时域对比结果Fig.4 Comparison of different prediction time domains

较小的预测时域无法预测足够多的未来动向,可能导致跟踪效果较差,过大的预测时域则会使算法考虑更多的未来轨迹趋势变化,引入误差导致控制效果变差。综合考虑后,最终确定预测时域和控制时域分别为:Np=100,Nc=80。

为对比控制效果,另设计了PID 控制器进行对比仿真实验,调整PID 的参数使其保持最佳的控制效果,通过试凑法确定最终的PID 控制参数为:kp=-1,kd=-5。时间t=40、80、120、160 s为滑翔机的状态切换点,两者的输出比对结果如图5所示。图中,RL-MPC(real-time linearization MPC)为文中设计的实时线性化模型预测控制器。

图5 ±17.4°时控制效果对比Fig.5 Comparison of control effect at ±17.4°

2) 工作状态为±22.5°时。

工作状态为±22.5°时,设置约束条件为: -1.5 ≤Δu≤1.5、-20≤u≤20、-b≤y≤b,b=[25 25 0.8]T。

参考1)中的调参步骤,选取相应的预测时域和控制时域为:Np=100,Nc=80。

同样,为了对比控制效果,设计了相应的PID控制器,其控制参数为:kp=-1,kd=-5。两者输出对比结果如图6 所示。

图6 ±22.5°时控制效果对比Fig.6 Comparison of control effect at ±22.5°

从图5 与图6 的仿真结果可以看出,水下滑翔机通过输入信号调节可移动质量块的位置来改变自身状态,能对控制指令实现及时响应。同时,对比二者的俯仰角调节结果可以发现,文中所设计控制器与PID 控制器均可做到无超调地实现对水下滑翔机俯仰角的稳定控制。两控制器的性能指标如表2 所示。

表2 RL-MPC 与PID 控制器性能指标对比Table 2 Performance Index of RL-MPC and PID Controller

由上表可知,在0～40 s 时间段,水下滑翔机完成下潜运动,在工作状态为±17.4°时,PID 所需上升时间和调节时间分别为9.27 s 和13.06 s;RL-MPC控制器的上升时间和调节时间为2.41 s 和3.60 s,分别较PID 缩短74.0%和72.4%。在工作状态为±22.5°时,PID 所用上升时 间和调节时间 分别为9.40 s 和13.23 s;RL-MPC 控制器的上升时间和调节时间为2.60 s 和3.88 s,分别较PID 缩短72.3%和70.7%。

在40～80 s 时间段,水下滑翔机完成上浮运动,在工作状态为±17.4°时,PID 所需上升时间和调节时间分别为9.47 s 和13.42 s;RL-MPC 控制器的上升时间和调节时间为2.50 s 和3.75 s,分别较PID缩短73.6%和72.1%;在工作状态为±22.5°时,PID所用上升时间和调节时间分别为9.48 s 和13.51 s,RL-MPC 控制器的上升时间和调节时间为2.65 s和3.92 s,分别较PID 缩短72%和71%。显然,RLMPC 控制器比PID 控制器的控制性能更好,体现在更快的响应速度、更短的调节时间和上升时间,说明采用该控制器的水下滑翔机能及时、快速地实现滑翔机姿态的改变,从而可更好地收集海洋数据,提升数据的准确性。

4 结束语

针对水下滑翔机控制算法未充分考虑实际物理约束限制的问题,以及动力学建模普遍维度过高而难以准确分析运动状况并设计相应控制器的问题,依据水下滑翔机的工作原理,忽略建模过程中的次要影响因素,并进一步将传动机构的延迟纳入建模之中,建立了轻量化的水下滑翔机动力学模型,利用更简单的模型实现对滑翔机运动状态的准确描述。同时针对水下滑翔机纵垂平面的俯仰角回路控制问题,考虑状态量和控制机构物理极限等约束情况,设计了非线性约束下的模型预测控制器,以实现对水下滑翔机俯仰角的精确控制。

对比实验表明,文中所提出的轻量化模型符合水下滑翔机的工作原理,能实现对水下滑翔机运动情况的准确描述,仿真验证了其有效性。在±17.4°和 ±22.5°这2 种水下滑翔机常见工况下,所设计的RL-MPC 控制算法能有效提高水下滑翔机的控制精度。现阶段的验证工作均在仿真环境中进行,海试试验有待下一步开展,同时控制器在实际试验中的可靠性也有待进一步分析和研究。