基于奇异摄动法的水下机器人优化PID控制

严 乐，梅 平，刘云平，郑柏超

(南京信息工程大学自动化学院，江苏 南京 210044)

1 引言

水下机器人作为一类特殊的机械，其非线性和强耦合性导致很难确定相应的水动力系数，同时海洋的干扰是随机和时变的，如果搭载不同的设备，必定会影响水下机器人的控制效果[1]。

国内外众多的优秀学者对水下机器人控制问题进行了分析与仿真。王建国等人[2]在估计水动力参数时，引入了极大似然参数估计算法，解决了高速航行给水下机器人带来的系统扰动问题，使得对水下机器人的数学建模更为准确。Xin S.等人[3]针对水下机器人运行时受到的时变扰动，设计了一种具有自适应扰动近似的模糊滑模控制器，有效地补偿了扰动。Bak J.等人[4]针对不同高度下海流对水下机器人的干扰问题，提出了一种基于矢量分解方法的控制方法，一定程度上克服了干扰。B.Wang等人[5]以提高水下机器人控制精度为目标，参照神经网络和PID控制算法，设计了一种自适应PID控制器，使得水下机器人在复杂环境中作业时的稳定性和抗干扰能力有了很大的提高。

目前，在工程运用中使用最广泛的是PID控制，其原理简单，控制效果较好。由于水下机器人一般模型中的不同状态变量均存在耦合现象，导致水下机器人的控制十分困难[6]。而奇异摄动法和快慢分解法是处理快慢动态耦合复杂系统的强有力工具，其思想就是在两个时间尺度内分别完成设计控制器的任务，最后用降阶系统近似替代原系统的动力学行为进行控制器设计[7，8]。奇异摄动法在工业控制中取得了很多的应用，如在文献[9]中，针对F-15飞机设计飞行轨迹指令综合跟踪器时，将整个控制系统分解为空间位置、速度、姿态和角速度控制回路，利用奇异摄动方法实现回路之间的解耦。

本文针对PID控制应用于水下机器人时，因系统耦合度高而产生的误差问题，使用奇异摄动控制方法，设计出水下机器人的优化PID控制律。仿真结果表明，改进后的PID控制方法能够更为精确地控制机器人位置，控制效果较传统的PID控制有很大的改善。

2 水下机器人的奇异摄动系统建模

2.1 坐标系的建立与变换

水下机器人的建模过程却带有一定的矛盾性，即模型的复杂性和控制难度成反比，和控制效果又成正比。所以，对水下机器人建立适当简化的数学模型显得尤为重要。

为了描述水下机器人的位置和姿态变化，本文将设置固定坐标系和体坐标系，并给出两种坐标之间的变换矩阵。

将固定坐标系建立在海平面上，在海平面选取适当点为坐标系原点O，OR轴与OS轴位于海平面且互相垂直，OR指向正北，OT垂直于海平面指向地心，符合右手规定。而体坐标系设置在机器人上，原点C一般设置在机器人体对称点、浮心或重心上，Cx轴指向机器人运动方向，Cy垂直于Cx指向机器人右侧，Cz垂直于Cxy平面指向机器人底部，坐标系如图1。

图1 坐标系示意图

水下机器人在空间上的运动可以由体坐标系各参数表示，现将运动名称规定见表1。

表1 空间运动命名

水下机器人的运动参数，包括速度V、角速度W、合力F和惯量矩M在各方向上的映射见表2。

表2 运动参数在各方向上的映射

同样的，水下机器人的姿态和位置也可以由固定坐标系相对于体坐标系的角度变化和坐标来确定。体坐标系原点的坐标、速度、角速度和姿态角变化在固定坐标系各方向上的分量见表3。

表3 各参数在定坐标系下的分量

固定坐标系可以通过旋转R轴、S轴、T轴，与体坐标系重合，从体坐标系到固定坐标系的变换矩阵Q为：

(1)

固定坐标系和体坐标系之间的转换表示为：

2.2 水下机器人的建模

假设水下机器人运动在理想流体中，利用动量守恒定理和动力矩定理，可以得出水下机器人的六自由度运动模型。

在固定坐标下运用动量定理可得

(2)

其中，VG为重心在固定坐标系下的速度，F为机器人所受合推力。设体坐标下的重心坐标向量RG=(xGyGzG)T，m为水下机器人质量，那么原点速度V与重心速度VG的关系为

VG=V+W×RG

(3)

由式(2)对刚体动量mVG求导，再由式(3)可得

(4)

(5)

同样的，在固定坐标系下利用动量矩定理可得

(6)

其中，HC=IW是固定坐标系原点C的动量矩，TC是机器人对体坐标系原点C的合外力矩，aC是体坐标系原点的加速度，I为机器人对应点C的转动惯量：

(7)

由HC=IW，于是式(6)可变为

(8)

(9)

2.3 模型简化

假设水下机器人在理想的水域中，则水下机器人的六自由度运动模型可简化为

(10)

其中，M为水下机器人惯量矩阵，C(v)为科氏力矩阵，D(v)为流体阻力矩阵，g(η)是竖直方向上的受力矩阵，f=[f1f2f3f4f5f6]T为水下机器人各自由度动力矩阵，v为机器人速度与角速度向量，η为位置与姿态向量。

由于本文所设计的水下机器人为开架式对称结构机器人，设定重心G与体坐标系原点C重合，且重心G与机器人的惯性向量重合，从而

于是，式(10)中各参数可简化为

M=

Q1=FxVx+Fx|Vx|Vx|Vx|，Q2=FyVy+Fy|Vy|Vy|Vy|，

Q3=FzVz+Fx|Vz|Vz|Vz|，Q4=Mxp+Mx|p|p|p|，

Q5=Myq+My|q|q|q|，Q6=Mzr+Mz|r|r|r|。

式中变量的意义为：主体符号表示产生的水动力的方向，下标则表示所代表量的方向和对应运动。例如，FxVx表示机器人在x轴以单位加速度平动时，在x轴方向所产生的水动力，而Fx|Vx|Vx表示机器人在X轴平动时，在x轴方向产生的二次项水动力。

2.4 奇异摄动模型

奇异摄动系统是带有小参数ε的系统，在奇异摄动控制领域，时标分解法是较为重要的方法。时标分解法的基础假设认为，一个整体的系统可以分解为快变化(非主导因素)和慢变化(主导因素)两个子系统。在快子系统变量变化时，慢子系统变量几乎保持不变；当慢子系统的变化已无法忽视时，快子系统变量则已经稳定，也就是说快子系统变化会比慢子系统变量更早地进入稳态。式(10)中的速度、角速度和位置、姿态也存在上述快慢变量的关系.在一般情况下，水下机器人运动时的速度变化几乎是直接的阶跃，其变化速度是非常快的。

在时标分解法的基础上，将式(10)表示的水下机器人动力学模型转换为奇异摄动模型，首先将式(10)简化

(11)

其中，x1(t)=[VxVyVzp q r]为线速度和角速度向量，x2(t)=[R S T φ θ ψ]为位置和姿态向量，a1=-D(v)-C(v)，a2=-g(η)，a3=Q。然而式(11)中的M并不能作为摄动参数ε，则需要引入一个常数从而能构造足够小的摄动参数。由于M为对角阵，于是将式(11)两边同时左乘M-1，变为

(12)

注意，f为列向量，引入一个较大的常量c，将f表示为

(13)

由式(12)(13)，可得

(14)

其中，ε=c-1，a=c-1M-1a1，b=M-1(c-1a2+fc)。式(14)即为水下机器人的奇异摄动模型，u(t)为系统输入控制量，由所采用的系统控制方法决定，y(t)为系统输出。

2.5 快慢子系统分解

奇异摄动方法能够根据不同时间尺度，将原本的耦合系统进行解耦，分解为快子系统和慢子系统，从而得出原系统的两个近似解——快变量和慢变量[10，11]。根据两个近似解，可以分别进行控制律设计达到预期的控制目标。设针对快变量的快子系统控制律为uf(t)，针对慢变量的慢子系统控制律为us(t)，那么针对两个变量的整体控制律为[12，13]

u(t)=us(t)+uf(t)

设慢子系统的时间尺度为t，控制输入为us(t)，令ε=0，那么式(14)有近似解

x1s(t)=-a-1(b+us(t))

(15)

将式(15)代入(14)，得到慢子系统数学模型：

(16)

(17)

式(16)、(17)构成了水下机器人的快慢分解模型，其中x1f(t)为快变量即速度和角速度向量，x2s(t)为慢变量即位置和姿态向量。

3 优化PID控制律

3.1 传统PID控制(TP)

TP算法所要控制的对象是期望输入u(t)和实际输出y(t)的差值e(t)，即：

e(t)=u(t)-y(t)

(18)

TP所包含的三部分，比例(P)、积分(I)、微分(D)控制均是以式(18)为控制目标，将比例(P)、积分(I)、微分(D)控制累加，就会得出TP控制律

u(t)=up(t)+uI(t)+uD(t)

(19)

然而，当水下机器人模型使用TP控制位置时，水下机器人的速度就会变得无法控制，从而出现一些严重影响最终位置准确性的情况：当TP算法要求水下机器人进行一些速度变化时，往往忽略了水下机器人的速度上限，从而出现无法达到的速度要求，导致最终位置出现严重的偏差。

3.2 基于奇异摄动法的优化PID控制(OP)

本文设计的基于奇异摄动方法的水下机器人OP算法的结构如图2。

图2 OP算法结构框图

首先，根据由经验法和流体动力学软件获取的水动力参数和动量定理推导出简化的水下机器人动力学模型。针对目标控制量x1(t)、x2(t)使用奇异摄动方法将原系统解耦为快慢子系统，从而解出x1(t)、x2(t)的近似解x1f(t)、x2s(t)。接着，根据近似解分别设计对应的PID控制器，最后整合为原系统控制u(t)，从而能够实现对两个变量的控制。

对式(16)所示的慢子系统模型，设置对应的PID控制律控制输入us(t)，为：

(20)

其中，Kps、KIs、KDs分别为慢子系统比例、积分、微分系数，es(t)为慢子系统期望与输出误差，ys(t)为慢子系统的实际输出。

对式(17)所示的快子系统模型，设置对应的PID控制律uf(t)，为：

(21)

其中，Kpf、Kif、KDf分别为快子系统比例、积分、微分系数，ef(t)为快子系统期望与输出误差，yf(t)为快子系统的实际输出。

由式(20)(21)可得水下机器人的奇异摄动OP控制律u(t)

u(t)=us(t)+uf(t)

ys(t)=x2(t)，yf(t)=x1(t)

(22)

4 仿真与分析

首先使用Solidworks对水下机器人进行三维建模，获得机器人的刚体惯量矩阵等数据，再根据所使用材料获得质量、重心等数据。

本文所进行的仿真是在Simulink中运行的，仿真目标是实现对x轴位置移动R所进行控制，本章中的“位置”均表示水下机器人在x轴上的位移R。其中，水动力取值使用流体动力学软件计算得出，具体数值如下

(23)

由上述数据，得出式(14)中所有参数：a=0.21+0.056Vx；ε=0.02；b=0.07；a3=1。并且，可以得出水下机器人最高速度为4.2m/s。设置位置的期望值为15，机器人到达指定位置后停下，即最后的速度为0。将参数代入(16)(17)可以得到具体的快慢子系统，对快慢子系统分别使用PID控制，在保证快慢变量都得到良好控制的情况下，得到相应的PID参数为

Kps=5，KIs=2，KDs=2；

Kpf=20，KIf=5，KDf=10。

(24)

整合快慢子系统的控制方法，得到OP控制律

u(t)=us(t)+uf(t)

ys(t)=x2(t)，yf(t)=x1(t)

(25)

由快慢子系统PID系数可以得到与之对应的原系统的TP控制律

(26)

将控制律(25)(26)在Simulink中进行仿真，按需要设置位置期望，得出控制效果如图3。

图3 系统速度输出

如图3所示为水下机器人系统加入TP和OP后，系统的速度输出。可以看到图3(a)所示TP算法速度输出虽然能够较快地达到稳态，但是其最初的超调量十分严重，导致水下机器人的实际速度不能达到TP算法想要达到的速度，从而导致十分严重的误差，影响最终位置的准确性。经多次实验，发现此现象无法避免。而图3(b)所示OP算法输出，虽然调节时间变长，但是能有效地将速度约束在4.2m/s以下，从而大幅度减少误差，使最终的位置更为准确。

OP和TP的理论位置输出见图4。TP的位置输出曲线的上升时间比OP小，而两者的过渡时间相当，均为12.5s，从这两个指标看，TP的快速性比OP好。从超调量上看，OP的超调量仅0.053，小于TP，OP的位置输出波动较小。两者的稳态误差都很小，控制精度较好。然而，TP由于图3(a)所示的极大超调，在最终的位置控制上需要累加由于系统达不到期望速度而产生的误差，从而导致实际操作中位置输出的误差，控制效果较差。

图4 系统位置理论输出

OP和TP所产生的误差和两种算法的控制效果比较见图5。经奇异摄动方法优化后的PID控制几乎没有误差产生，而TP所累积的误差非常大，误差值占控制目标值的47%。将误差累加到最终位置输出上，可以得到图5(b)，直接反映出最终实际的位置控制结果。

图5 两种算法的控制效果比较

为了进一步探究传感器发生故障时，两种控制算法的表现，仿真时在t=10s时刻加入幅值为5的故障信号，得出结果见图6。在传感器发生故障时，OP调节能力比TP快78%，其超调也在可控范围内，其鲁棒性优于TP方法。

图6 传感器发生故障时的位置输出

结合仿真结果可以得出结论，TP虽然调节时间较短，控制响应的快速性更好，但是其产生的位置误差非常大，导致最终会发生偏离，此次仿真中发生了约47%的偏移。而经奇异摄动改进后的OP算法，虽然调节时间有所延长，但是其超调量很小，产生的误差较小，最终的位置控制也因此更为精确，几乎没有误差，在发生传感器故障时的其鲁棒性也有所提高。

5 结论

本文针对水下机器人进行动力学建模，结合奇异摄动控制法、PID控制，设计了水下机器人的奇异摄动优化PID控制方法(OP)，实现对水下机器人的位置控制。对OP方法进行仿真分析，并与TP方法控制效果进行对比。实验结果表明，基于奇异摄动方法优化的PID控制(OP)能够有效地避免TP方法造成了位置误差，控制效果良好，在控制范围适当的情况下能够有效地控制误差。