﻿ 基于连续滑模控制的水下无人航行器航向跟踪研究
 舰船科学技术  2023, Vol. 45 Issue (12): 63-68    DOI: 10.3404/j.issn.1672-7619.2023.12.012 PDF

Research on underwater submersible course tracking based on continuous sliding mode control
YANG Chen-yu, LIU Dan, LIU Lu, WANG Yong-hua, QI Xiang-dong, LIU Shuang-feng, XUE Chen-yang
Key Laboratory of Instrumentation Science and Dynamic Measurement, Ministry of Education, North University of China, Taiyuan 030051, China
Abstract: This paper proposes to design a continuous sliding mode controller based on a time-varying disturbance observer under the premise of constructing a fuzzy submersible kinematic model. The disturbance observer equates the system error and external disturbance to the control side, and the continuous sliding mode controller converges the system to the equilibrium point by switching the control quantity. The controller can respond quickly and is insensitive to model parameter changes and external disturbances, and has strong robustness. In addition, straight line, sinusoidal wave and custom curve are set as the predetermined heading, and the simulation is carried out by Matlab/Simulink to verify whether the submersible can achieve the trajectory tracking in complex waters and achieve the heading control purpose.
Key words: UUV     continuous sliding mode control     heading tracking     interference observer
0 引　言

1 水下无人航行器建模 1.1 坐标系构建

1.1.1 地面坐标系（固定坐标系） $\boldsymbol{E}-\mathit{\xi }\mathit{\eta }\mathit{\zeta }$

 图 1 固定坐标与运动坐标系 Fig. 1 Fixed and moving coordinates
1.1.2 UUV坐标系（运动坐标系）O-xyz

1.2 运动参数 1.2.1 姿态角

1.2.2 坐标系之间的转化

 $\left[\begin{array}{c}\xi \\ \eta \\ \zeta \end{array}\right] ={{T}} \left[\begin{array}{c}x\\ y\\ z\end{array}\right],$ (1)
 $\begin{split} {\rm{T}}=&\left[\begin{array}{ccc}{\rm{cos}}\varPsi {\rm{cos}}\theta & {\rm{cos}}\varPsi {\rm{sin}}\theta {\rm{sin}}\phi -{\rm{sin}}\varPsi {\rm{cos}}\varPsi \\ {\rm{sin}}\varPsi {\rm{cos}}\theta & {\rm{sin}}\varPsi {\rm{sin}}\theta {\rm{sin}}\phi +{\rm{cos}}\varPsi {\rm{cos}}\phi\\ -{\rm{sin}}\theta & {\rm{cos}}\theta {\rm{sin}}\phi \end{array}\right. \\ & \left.\begin{array}{ccc}{\rm{cos}}\varPsi sin\theta {\rm{cos}}\varPsi +{\rm{sin}}\varPsi {\rm{sin}}\phi \\ {\rm{sin}}\varPsi {\rm{sin}}\theta {\rm{cos}}\phi -{\rm{cos}}\varPsi {\rm{sin}}\phi \\ {\rm{cos}}\theta{\rm{cos}}\phi \end{array}\right] 。\end{split}$ (2)

1.3 数学模型

 $\sum _{i}{m}_{i}\frac{{{\rm{d}}v}_{i}}{{\rm{d}}t} = \sum _{t}{F}_{i}。$ (3)

1.3.1 运动学模型（固定坐标系下）

UUV行器通常有6个自由度，并且它的运动可以看做是刚体运动[9]。在UUV运动过程中，令 $v=\left[{v}_{x}0 0 \right]T$ 表示潜航器只有沿着x轴的线速度，其他方向线速度为0， $p=[x,y,z]T$ 。令 $\omega =\left[{\omega }_{x},{\omega }_{y},{\omega }_{z}\right]T$ 为沿xyz轴方向的角速度。推导可得 $\dot{p}={J}_{1}\left(\eta \right){v}_{x},\dot{\eta }=$ ${J}_{2}\left(\eta \right)\omega 。$

 $\begin{split} \left[\begin{array}{c}\dot{x}\left(t\right)\\ \dot{y}\left(t\right)\\ \dot{z}\left(t\right)\\ \dot{\varphi }\left(t\right)\\ \dot{\theta }\left(t\right)\\ \dot{\varPsi }\left(t\right)\end{array}\right]= &\left[\begin{array}{c}{\rm{cos}}\varPsi {\rm{cos}}\theta \\ {\rm{sin}}\varPsi {\rm{cos}}\theta \\ -{\rm{sin}}\theta \\ 0\\ 0\\ 0\end{array}\right]{v}_{x}+ \left[\begin{array}{c}0\\ 0\\ 0\\ 1\\ 0\\ 0\end{array}\right]{\mathrm{\omega }}_{x}+ \\ &\left[\begin{array}{c}0\\ 0\\ 0\\ {\rm{sin}}\phi {\rm{tan}}\theta \\ {\rm{cos}}\phi \\ {\rm{sin}}\phi {\rm{sec}}\theta \end{array}\right]{\mathrm{\omega }}_{y}+ \left[\begin{array}{c}0\\ 0\\ 0\\ {\rm{cos}}\phi {\rm{t}}an\theta \\ -{\rm{sin}}\phi \\ {\rm{cos}}\phi {\rm{sec}}\theta \end{array}\right]{\mathrm{\omega }}_{z} ，\end{split}$ (4)

 ${s}^{{\rm{T}}}\dot{p}=0 ，$ (5)
 ${a}^{{\rm{T}}}\dot{p} =0，$ (6)

sap展开并且用A(q) $\dot{q}$ =0表示为：

 $\begin{split}{{A(q)}} = &\left[ \begin{array}{*{20}{c}} {\rm{cos}}\varPsi {\rm{sin}}\theta {\rm{sin}}\phi - {\rm{sin}}\varPsi {\rm{cos}}\phi\\ {\rm{cos}}\varPsi {\rm{sin}}\theta {\rm{cos}}\phi + {\rm{sin}}\varPsi {\rm{sin}}\phi\end{array}\right.\\ & \left.\begin{array}{*{20}{c}} {\rm{sin}}\varPsi {\rm{sin}}\theta {\rm{sin}}\phi +{\rm{cos}}\varPsi {\rm{cos}}\phi & {\rm{cos}}\theta sin\phi & 0 & 0 & 0\\ {\rm{sin}}\varPsi {\rm{sin}}\theta {\rm{cos}}\phi -{\rm{cos}}\varPsi {\rm{sin}}\phi & {\rm{cos}}\theta {\rm{cos}}\phi & 0 & 0 & 0\end{array} \right]。\end{split}$ (7)

 $\dot{q}\left(t\right)= {f}_{1}\left(q\right){v}_{1} + {f}_{2}\left(q\right){v}_{2}+ {f}_{3}\left(q\right){v}_{3}+ {f}_{4}\left(q\right){v}_{4} 。$ (8)

 $\left\{\begin{array}{*{20}{l}} {f}_{1}\left(q\right)=[{\rm{cos}}\varPsi {\rm{cos}}\theta \;\; {\rm{cos}}\theta {\rm{sin}}\varPsi \;\; -{\rm{sin}}\theta \;\; 0 \;\; 0 \;\; 0]，\\ {f}_{2}\left(q\right)=[0 \;\; 0 \;\; 0 \;\; 1 \;\; 0 \;\; 0]，\\ {f}_{3}\left(q\right)=[0 \;\; 0 \;\; 0 \;\; {\rm{sin}}\phi \mathrm{tan}\theta \;\; {\rm{cos}}\phi \;\; {\rm{sin}}\phi sec\theta ]，\\ {f}_{4}\left(q\right)=[0 \;\; 0 \;\; 0 \;\; {\rm{cos}}\phi \mathrm{tan}\theta \;\; -{\rm{sin}}\phi \;\; {\rm{cos}}\phi \mathrm{sec}\theta ]。\end{array}\right.$ (9)
1.4 模型简化 1.4.1 纵向运动模型

 ${m}\dot{v}={-A}_{x}{v}^{2}+{T}\mathrm{cos}\alpha -{P}\mathrm{sin}\varTheta，$ (10)
 $\begin{split} {m}{v}\dot{\varTheta }=&{A}_{y}^{2}{v}^{2}\mathrm{\alpha }+{A}_{y}^{\delta }{v}^{2}{\mathrm{\delta }}_{\mathrm{e}}{+A}_{y}^{\omega }v{\omega }_{{z}_{1}}+\\ & {\lambda }_{22}\left(\dot{v}\alpha +v\dot{\alpha }\right)-{\lambda }_{26}{\dot{\omega }}_{{z}_{1}}+{T}\mathrm{sin}\mathrm{\alpha }-{P}\mathrm{cos}\varTheta，\end{split}$ (11)
 $\begin{split} {J}_{{z}_{1}}{\dot{\omega }}_{{z}_{1}}=&{A}_{{m}_{z}}^{a}{v}^{2}a-{A}_{y}^{\delta }{x}_{e}{v}^{2}{\delta }_{e}-{A}_{{m}_{z1}}^{\omega }v{\omega }_{{z}_{1}}+\\ &{\lambda }_{26}\left(\dot{v}a+v\dot{a}\right)-{\lambda }_{66}{\dot{\omega }}_{{z}_{1}}- \\&B\left(h\mathrm{sin}\theta -{x}_{b}\mathrm{cos}\theta \right)-Th。\end{split}$ (12)

1.4.2 侧向运动模型

 $m{v}_{0}\dot{\kappa } = \left({A}_{z}^{\beta }{v}_{0}^{2} + T\right)\beta + {A}_{z}^{\delta }{v}_{0}^{2}{\delta }_{r} + {A}_{z}^{\omega }{v}_{0}{\omega }_{{y}_{1}} + {\lambda }_{33}{v}_{0}\dot{\beta } + {\lambda }_{35}{\dot{\omega }}_{{y}_{1}}，$ (13)
 $\begin{split} {J}_{{y}_{1}}{\dot{\omega }}_{{y}_{1}}=&{A}_{{m}_{y}}^{\beta }{v}_{0}^{2}\beta -{A}_{z}^{\delta }{x}_{r}{v}_{0}^{2}{\delta }_{r}-{A}_{{m}_{y1}}^{\omega }{v}_{0}{\omega }_{{y}_{1}}-\\ &{\lambda }_{55}{\dot{\omega }}_{{y}_{1}}-{\lambda }_{35}{v}_{0}\dot{\beta }+T{z}_{b}。\end{split}$ (14)

1.4.3 横滚运动模型

UUV绕x轴转动被称之为横滚，用滚角 $\mathrm{\phi }$ 来表示其程度大小。分析可得包括横滚的侧向运动模型[12]关于速度 ${v}$ 的运动模型为：

 $\begin{split} & {J}_{{y}_{1}}{\dot{\omega }}_{{y}_{1}}+\left({J}_{{z}_{1}}{-J}_{{y}_{1}}\right){\omega }_{{y}_{1}}{\omega }_{{z}_{1}}={A}_{{m}_{x}}^{\beta }{v}^{2}\beta -{A}_{{m}_{x}}^{\delta }{v}^{2}{\delta }_{d}-\\ & {A}_{{m}_{x1}}^{\beta }v{\omega }_{{x}_{1}}+{A}_{{m}_{x}p}{v}^{2}-{\lambda }_{44}{\dot{\omega }}_{{x}_{1}}-\\& B\left({Z}_{b}\mathrm{cos}\phi +h\mathrm{sin}\phi \right)\mathrm{cos}\theta，\end{split}$ (15)
 $\dot{\phi }={\omega }_{{x}_{1}}-\left({\omega }_{{y}_{1}}{{\rm{cos}}}\phi -{\omega }_{{z}_{1}}{{\rm{si}}n}\phi \right){{\rm{tan}}}\theta ，$ (16)
 $\begin{split} {{\rm{cos}}}\varTheta {{\rm{sin}}}\mathrm{\varPhi }=& {{\rm{cos}}}\beta {{\rm{cos}}}\theta {{\rm{sin}}}\phi -{{\rm{sin}}}\alpha {{\rm{sin}}}\beta {{\rm{cos}}}\theta {{\rm{cos}}}\phi +\\ & {{\rm{cos}}}\alpha {{\rm{sin}}}\beta {{\rm{sin}}}\theta。\end{split}$ (17)

2 基于数学模型的航向滑模控制

 \begin{aligned} & u={J}_{{z}_{1}}{\dot{\omega }}_{{z}_{1}}+{J}_{{y}_{1}}{\dot{\omega }}_{{y}_{1}}+\left({J}_{{z}_{1}}{-J}_{{y}_{1}}\right){\omega }_{{y}_{1}}{\omega }_{{z}_{1}} ={A}_{{m}_{z}}^{a}{v}^{2}a-\\ & {A}_{y}^{\delta }{x}_{e}{v}^{2}{\delta }_{e}-{A}_{{m}_{z1}}^{\omega }v{\omega }_{{z}_{1}}+{\lambda }_{26}\left(\dot{v}a+v\dot{a}\right)-{\lambda }_{66}{\dot{\omega }}_{{z}_{1}} -\\ & B\left(h{{\rm{sin}}}\theta -{x}_{b}\mathit{{\rm{cos}}}\theta \right)-Th+{A}_{{m}_{x}}^{\beta }{v}^{2}\beta -{A}_{{m}_{x}}^{\delta }{v}^{2}{\delta }_{d}-{A}_{{m}_{x1}}^{\beta }v{\omega }_{{x}_{1}}+\\ & {A}_{{m}_{x}p}{v}^{2}-{\lambda }_{44}{\dot{\omega }}_{{x}_{1}}-B\left({Z}_{b}\mathrm{cos}\phi +h\mathrm{sin}\phi \right)\mathrm{cos}\theta ，\\[-10pt]\end{aligned} (18)

 ${\ddot{x}}_{1}=-b{\dot{x}}_{1}+au-F。$ (19)

2.1 干扰观测器设计

 $\left\{\begin{array}{l}\dot{\widehat{F}}={k}_{1}\left(\widehat{\omega }-{\dot{x}}_{1}\right)，\\ \dot{\widehat{\omega }}=-\widehat{F}+au-{k}_{2}\left(\widehat{\omega }-{\dot{x}}_{1}\right)-b{\dot{x}}_{1}。\end{array}\right.$ (20)

 $F=-{\ddot{x}}_{1}-b{\dot{x}}_{1}+au ，$ (21)
 $\widehat{F}=-\dot{\widehat{\omega }}-b{\dot{x}}_{1}+au-{k}_{2}\left(\widehat{\omega }-{\dot{x}}_{1}\right)，$ (22)

$\stackrel{~}{F}=\left(\dot{\widehat{\omega }}+{\ddot{x}}_{1}\right)+{k}_{2}\left(\widehat{\omega }-{\dot{x}}_{1}\right)=-\dot{\stackrel{~}{\omega }}-{k}_{2}\stackrel{~}{\omega } 。$

 ${V}_{1}=\frac{1}{2{k}_{1}}{\stackrel{~}{F}}^{2}+\frac{1}{2}{\stackrel{~}{\omega }}^{2}，$ (23)
 $\stackrel{~}{F}=F-\widehat{F}\text{，}\stackrel{~}{\omega }={\dot{x}}_{1}-\widehat{\omega }。$ (24)

 图 2 干扰及观测结果 Fig. 2 Interference and results of their observations
2.2 基于干扰观测器的连续滑模控制

 $S={\dot{Z}}_{1}+c{Z}_{1}\text{，} c > 0，$ (25)
 $\dot{{S}}={\ddot{Z}}_{1}+c{\dot{Z}}_{1} ={\ddot{x}}_{d}-{\ddot{x}}_{1}+c{\dot{Z}}_{1} = {\ddot{x}}_{d}+b{\dot{x}}_{1}-au+F+c{\dot{Z}}_{1}，$ (26)

 ${\rm{U}}=\frac{1}{a}\left[{\ddot{x}}_{d}+b{\dot{x}}_{1}+c{\dot{Z}}_{1}+\widehat{F}+\eta {\rm{sgn}}\left(s\right)\right] ，$ (27)

 ${V}_{2}=\frac{1}{2}{S}^{2}，$ (28)
 \begin{aligned} & {\dot{V}}_{2}=S\dot{S}=\\ & S\left({\ddot{x}}_{d}+b{\dot{x}}_{1}-au+F+c{\dot{Z}}_{1}\right)=\\ &{\rm{S}} \left({\ddot{x}}_{d}+b{\dot{x}}_{1}-\left({\ddot{x}}_{d}+b{\dot{x}}_{1}+c{\dot{Z}}_{1}+\widehat{F}+\eta {\rm{sgn}}\left(s\right)\right)+F+c{\dot{Z}}_{1}\right) =\\ &{\rm{S}} \left(F-\widehat{F}-\eta {\rm{sgn}}\left(s\right)\right)= \widehat{F}S-\eta \left|S\right|\leqslant 0 ，\end{aligned} (29)
 ${{V}}={{V}}_{1}+{{V}}_{2}=\frac{1}{2{K}_{1}}{\stackrel{~}{F}}^{2} + \frac{1}{2}{\stackrel{~}{\omega }}^{2}+\frac{1}{2}{S}^{2}。$ (30)

 图 3 干扰及其估计值之间的误差 Fig. 3 Error between the interference and its estimate

 图 4 预设定航向及航向跟踪 Fig. 4 Pre-set heading and course tracking

 图 5 预设正弦航向及航向跟踪 Fig. 5 Preset sinusoidal heading and course tracking

 图 6 预设自定义航向及航向跟踪 Fig. 6 Preset custom heading and course tracking
3 结　语