针对欠驱动海洋航行器的高精度非线性路径跟踪控制方法技术领域
本发明涉及水下自主航行器(Autonomous Underwater Vehicle，AUV)、水面无人
艇(Unmanned Surface Vehicle，USV)等欠驱动海洋水面或水下航行器的路径跟踪控制方
法，尤其涉及一种多不确定性情况下的非线性路径跟踪控制方法。
背景技术
随着水面或水下自主航行器在海洋研究和开发领域的应用越来越广泛，其高精度
路径跟踪控制已经成为当前的研究热点之一。路径跟踪是指控制航行器跟踪一条满足航向
要求和性能约束，且与时间无关的期望轨迹。由于受到重量、可靠性、复杂性及效率等多方
面的因素影响，目前大部分的海洋航行器属于欠驱动系统，加上其本身具有高度的非线性、
耦合性以及加速度不可积的非完整约束等特性，给运动控制带来了较大的挑战。尤其在欠
驱动航行器存在未建模动态、模型参数不确定性及风浪流等多不确定性干扰时，传统的控
制方法往往不能保证路径跟踪控制的精度，有时甚至无法满足实际作业的需要。
发明内容
本发明的目的在于，在Serret-Frenet坐标系下构建欠驱动系统路径跟踪误差模
型和基于多不确定性的动力学模型，设计一种针对欠驱动海洋航行器的高精度非线性路径
跟踪控制方法，用于消除模型参数不确定性、未建模动态及外部环境扰动等对路径跟踪的
影响，实现对期望路径的精确跟踪控制。
首先，本发明的非线性路径跟踪控制方法，将航行器侧滑角的变化率看作一个不
确定项dψ，建立欠驱动海洋航行器水平面内包含多不确定性的路径跟踪误差模型和动力学
模型，表示为如下三个子系统：
(1)位置子系统
(2)姿态子系统
(3)速度子系统
其中，s为期望路径上某点的横坐标；cc为期望路径上该点处的曲率；(xe,ye,ψe)为
航行器在Serret-Frenet坐标系下的位置和姿态误差；u、v和r分别为航行器质心在惯性坐
标系下的纵向速度、横向速度及航向角速度；vt为航行器的合成速度，m为航行
器质量，Iz为航行器绕z轴的转动惯量；Xu,Xu|u|,Yv,Yv|v|,Nr,和Nr|r|为水动力参数；
du,dv和dr为动力学模型集总不确定性，Δm,ΔIz,ΔXu,ΔXu|u|,ΔYv,ΔYv|v|,ΔNr,ΔNr|r|,
为动力学模型参数不确定项，Δτu,Δτv和Δτr为外部环境扰动不确定项，Δi(i
＝u,v,r)为其它未建模动态；τu为外部控制输入力，τr为外部控制输入力矩。
本发明提供了一种高精度非线性路径跟踪控制方法，实现步骤如下：
步骤一：设计期望视线角ψlos；
步骤二：设计观测器估计运动学和动力学不确定性；
对运动学不确定性项dψ和动力学不确定性项du,dv和dr，通过观测器进行实时在线
观测，得到观测值dψ，du,dv和dr；
步骤三：设计基于观测器的控制器；
采用反步法(back-stepping)获取虚拟向导控制律虚拟航向角速度控制律rd、
输入力控制律τu及输入力矩控制律τr，并对步骤二观测的不确定性dψ、du、dv和dr进行实时补
偿；
步骤四：采用跟踪微分器对控制器进行简化，解决传统back-stepping控制方法中
的“计算膨胀”问题。
所述的步骤三中，根据位置子系统，获取虚拟向导控制律为：
其中，k2＞0，为控制增益；
根据姿态子系统，获得虚拟航向角速度控制律rd和输入力矩控制律τr为：
其中，k1,k4＞0为控制增益；
根据速度子系统，获得输入力控制律τu为：
其中，k3＞0为控制增益，ud为恒定期望速度。
所述的步骤四中得到简化后的控制器可以表示为：
其中，rc和为虚拟航向角速度控制律rd通过跟踪微分器后得到的跟踪信号和微
分信号。
本发明的优点和积极效果在于：
(1)本发明的非线性路径跟踪控制方法，将运动学跟踪误差模型中的未知参数-航
行器侧滑角的变化率当作运动学不确定性，将动力学模型中的内部参数不确定性、外部环
境扰动及未建模动态等当作动力学集总不确定性，并且采用观测器对上述运动学和动力学
不确定性进行实时在线观测，实现对期望路径的精确跟踪控制；
(2)本发明的非线性路径跟踪控制方法，采用back-stepping控制方法设计运动学
和动力学控制器，并且对观测到的多不确定性实时补偿；通过跟踪微分器对控制器进行简
化，使得控制器更适合移植到实时运算平台，使得本路径跟踪控制方法及实现的控制器更
加适用于工程实践；
(3)本发明的非线性路径跟踪控制方法，不依赖于控制对象精确的数学模型，在多
种不确定性的影响下可以保持名义模型同样的性能，可以扩展到海洋航行器在多不确定性
下的三维空间路径跟踪控制中。
附图说明
图1是海洋航行器水平面内的路径跟踪示意图；
图2是本发明基于观测器的非线性路径跟踪控制方法示意图；
图3是本发明的非线性路径跟踪控制方法和传统back-stepping控制器作用下路
径跟踪对比图；
图4是本发明的非线性路径跟踪控制方法和传统back-stepping控制器作用下路
径跟踪控制误差对比图；
图5是本发明的非线性路径跟踪控制方法和传统back-stepping控制器作用下航
行器速度对比图。
具体实施方式
下面将结合附图对本发明的技术方案进行详细说明。
本发明提供了一种基于观测器的高精度非线性路径跟踪控制方法，以实现在多不
确定性情况下的欠驱动海洋航行器高精度非线性路径跟踪。
如图1所示，海洋航行器坐标系定义及水平面内的路径跟踪示意图。{I},{B}和
{SF}分别为惯性坐标系、载体坐标系及Serret-Frenet曲线坐标系。其中{I}和{B}坐标系遵
循标准规范，{SF}的坐标原点为期望路径上的任意一点，其横轴XF沿路径切线方向，纵轴YF
为法线方向。
下面在上述坐标系下对多不确定性下的航行器水平面内的路径跟踪控制系统进
行建模和分析。
首先，海洋航行器在惯性坐标系下的运动学方程可以表示为：
其中，x、y和r分别为航行器质心Q在惯性坐标系下的横坐标和纵坐标，ψ为航行器
航向角；u、v和r分别为航行器在惯性坐标系下的纵向速度、横向速度及航向角速度。字符上
加点表示求导操作。
其次，海洋航行器在Serret-Frenet曲线坐标系路径跟踪误差模型可以表示为：
其中，s为期望路径上某点P的横坐标，cc(s)为期望路径上点P处的曲率，简写为cc；
(xe,ye,ψe)为航行器在Serret-Frenet坐标系下的位置和姿态误差，xe和ye分别为航行器质
心Q在Serret-Frenet坐标系下的横坐标和纵坐标，姿态误差ψe＝ψw-ψF，ψw＝ψ+β为航行器的
航迹角，ψF为惯性坐标系横轴和Serret-Frenet坐标系横轴之间的夹角；β为航行器的侧滑
角，为侧滑角的导数，也就是侧滑角变化率；vt为航行器的合成速度，
可以看出，上述模型中包含未知参数根据公式及动力学模型，取
决于航行器纵向速度u、横向速度v、航向角速度r及模型参数和不确定性du、dv。一种简单的
方法是通过测量加速度和获得但是此方法会受到传感器测量噪声的影响。因此本发
明将看作一个未知扰动进行处理，即设置不确定性参数进而运动学模型的第三个
方程转化为：
最后，欠驱动海洋航行器的包含多不确定性的动力学模型可以表示为：
其中，m为航行器质量；Iz为航行器绕z轴的转动惯量，是指在{I}坐标系下；τu为外
部控制输入力，调整海洋航行器的纵向速度；τr为外部控制输入力矩，调整海洋航行器的航
向角；Xu,Xu|u|，Yv,Yv|v|,Nr,和Nr|r|为水动力参数；du,dv和dr为动力学模型的总不确
定性，Δm,ΔIz,ΔXu,ΔXu|u|,ΔYv,ΔYv|v|,ΔNr,ΔNr|r|,为动力学模型参数不
确定项，Δτu,Δτv和Δτr为风、浪、流等外部环境扰动不确定项，Δi(i＝u,v,r)为其它未建
模动态。公式中|.|表示求取绝对值，例如|u|为速度的绝对值。
为便于控制器设计，将上述模型转化为以下三个子系统：
(1)位置子系统
(2)姿态子系统
(3)速度子系统
其中，姿态子系统中，将未知参数看作不确定性项dψ来进行处理，是对现有系统
的改进，是本发明的创新点之一，在下面步骤二中设计观测器对dψ进行观测。现有控制方法
是通过动力学模型求解得到因此依赖于系统精确的数学模型。
本发明提供的基于观测器的高精度非线性路径跟踪控制方法包括四个步骤：根据
视线角制导率计算视线角ψlos；设计观测器估计运动学和动力学不确定性；基于观测器的控
制器设计，包含设计虚拟向导控制律虚拟航向角速度控制律rd、输入力控制律τu及输入
力矩控制律τr；采用跟踪微分器对整个控制器进行简化。
如图2所示，对本发明的自适应解耦控制方法的四个步骤进行具体说明。
步骤一：设计视线角制导率。
为了降低算法的复杂度，本发明采用传统的视线角制导率计算期望视线角，如下：
其中，ψlos为期望视线角，Δ为视线距离。
步骤二：运动学和动力学不确定性观测器设计。
为了实时估计海洋航行器受到的内外部扰动，可以采用扩张状态观测器、二阶滑
模观测器等多种观测器，其中降阶扩展状态观测器可以表示为：
其中，dψ为运动学不确定性观测值，dr和du为动力学不确定性观测值,ξi(i＝1,2,
3,4)为观测器辅助变量，ωi(i＝1,2,3,4)为观测器增益。需要指出的是，动力学不确定项dv
隐含在运动学不确定性dψ中，即
步骤三：基于观测器的控制器设计。
(1)根据位置子系统模型(5)，获取的虚拟向导控制律可以表示为：
其中，k2＞0为设计的控制增益。
(2)进行姿态子系统控制律设计。
首先，进行运动学控制器设计。采用航向角速度r作为虚拟指令，为使得ψe＝ψlos，ψe
的期望动态特性可以表示为：
其中，是期望视线角的导数，k1为控制增益。
将式(11)代入姿态子系统模型(6)可得虚拟向导控制律rd为：
结合观测器(9a)，虚拟向导控制律rd进一步表示为：
其次，采用back-stepping方法将上述设计的运动学控制器反推至动力学。实际航
向角速度r的期望动态特性可以表示为：
其中，k4为控制增益，将式(14)代入动力学模型，结合观测器(9b)可得输入力矩控
制律τr为：
(3)进行速度子系统控制律设计。
纵向速度u的期望动态可以表示为：
其中，k3为控制增益，ud为恒定期望速度，因此将式(16)代入动力学模型，结
合观测器(9c)可得输入力控制律τu为：
步骤四：采用跟踪微分器对整个控制器进行简化。
可以看出，步骤三设计的动力学控制器τr中包括表达式结合虚拟指令表达式
(13)，可知动力学控制器非常复杂，且包含不确定性观测值dψ，本发明采用线性或者非线性
跟踪微分器对运动学虚拟控制指令rd进行数值求导，更适合于实际工程应用。
中科院系统所韩京清研究员给出的非线性跟踪微分器可以表示为：
其中，h为采样周期，q为加速因子，k为离散时间变量，fhan(·)函数的详细表达式
可以参照相关文献(韩京清著,《自抗扰控制技术》[M].国防工业出版社,2008.p66-73)。
本发明简化后的控制器表示为：
其中，rc和为虚拟参考指令rd通过跟踪微分器后得到的跟踪信号和微分信号。
下面证明本发明方法的闭环系统稳定性：
定义闭环跟踪误差：E2＝u＝u-ud和E3＝[xe,ye]T。
首先，证明误差E1在本发明涉及控制器作用下的有界性，考虑如下的Lyapunov函
数：
沿方程(6)的轨迹，求公式(20)对时间的导数，可得:
将方程(9a),(9b),(13)和(15)代入上式(21)，可得：
其中，ε1和ε2为跟踪微分器的跟踪误差，dψ、dr为观测器的观测误差。
进而采用杨氏不等式，可得：
其中，选择可以保证矩阵K为正定矩阵。因此，闭环系统误差E1可以
表示为：
其中，λmin(K)为正定矩阵K的最小特征值，λmax(G)为矩阵G的最大特征值。
其次，证明闭环误差E2在本发明控制方法作用下的有界性，考虑如下的Lyapunov
函数：
沿方程(7)的轨迹，求公式(25)对时间的导数，可得:
设观测器观测误差收敛，即du有界，因此当k3足够大时，能够保证且使得闭
环误差满足||E2||≤du/k3。
最后，证明误差E3在本发明控制方法作用下的有界性，考虑如下的Lyapunov函数：
沿方程(5)的轨迹，求公式(27)对时间的导数，可得:
设ye有界，且满足|ye|≤y0，y0为一个正常数，闭环误差E3可以表示为：
其中，λmin(M)为矩阵M的最小特征值，λmax(N)为矩阵N的最大特征值。
因此可以看出，闭环系统误差取决于设计的控制参数、跟踪微分器的跟踪误差和
观测器的观测误差。以上，闭环系统的稳定性证明完毕。
为了验证上述提出的控制器的有效性，在MATLAB/Simulink环境下建立海洋航行
器水平面内的路径跟踪控制系统的仿真模型，并对航行器在受多不确定性情况下的路径跟
踪控制性能进行验证。航行器模型参数如表1所示：
表1航行器水动力参数
初始值设置为：x(0)＝30m,y(0)＝-20m,u(0)＝0.1m/s,v(0)＝0m/s,r(0)＝0rad/
s,ψ(0)＝π/2rad,及s(0)＝0m；期望速度ud＝1m/s；控制器参数k1＝1,k2＝0.1,k3＝1,k4＝
10,ω1＝5,ω2＝20,ω3＝20,ω4＝20,h＝0.001s-1,r＝1000m/s2。
假设期望路径的参数化曲线方程可以表示为式(30)：
其中，参数μ可以通过下式得到：期望路径参数如表2所
示。
表2期望路径参数
路径参数
i＝0
i＝1
i＝2
i＝3
i＝4
ai
0
0.87
-0.02
10-5
1.5×10-6
bi
0
0.5
-5×10-4
10-5
10-7
假设所有的水动力参数在名义值的基础上增加30％，航行器所受的外部环境扰动
表示为：
本发明的基于观测器的非线性路径跟踪控制方法和传统back-stepping控制方法
作用下路径跟踪曲线图如图3所示。从图3可以看出，本发明基于观测器的非线性路径跟踪
控制方法和传统back-stepping控制方法都可以使得航行器渐进收敛于期望路径。但本发
明方法精度更高，且没有受到多不确定性的影响。两种控制方法作用下，路径跟踪控制误差
曲线如图4所示。由图4可知，本发明方法使得路径跟踪误差渐进收敛于零附近，而传统的
back-stepping控制方法则明显受到多不确定性的影响而性能变差。
图5为本发明的非线性路径跟踪控制方法和传统back-stepping控制器作用下航
行器速度对比曲线。可以看出，在本发明控制方法作用下，航行器纵向速度u(t)快速平稳收
敛于期望速度ud＝1m/s，而在传统的back-stepping控制方法作用下，u(t)随着航行器的不
确定性在期望速度附近波动。另外，两种控制方法作用下，航行器横向速度v及航向角速度r
均有界，但本发明方法更平滑。
通过和传统back-stepping控制器作用下的对比分析可知，在本发明的基于观测
器的非线性路径跟踪控制方法作用下，航行器的路径跟踪控制精度明显提高，抗干扰能力
显著增强。