CN103576693B - 基于二阶滤波器的水下机器人三维路径跟踪控制方法 - Google Patents

Abstract

本发明提供的是一种基于二阶滤波器的水下机器人三维路径跟踪控制方法，利用滤波反步法进行水下机器人三维路径跟踪控制，通过引入两个基于水下机器人三维路径跟踪运动学误差模型建立的二阶滤波器，获取姿态、速度、角速度的虚拟控制量及其导数，再结合水下机器人动力学模型获取路径跟踪控制器的控制输入，作用于机器人推进器与舵机，进而实现对三维路径的跟踪；并依据李雅普诺夫能量函数对位置、姿态控制回路设计滤波反馈补偿项，对速度控制回路引入积分环节，构成***误差补偿回路，提升跟踪***的精度。

Description

基于二阶滤波器的水下机器人三维路径跟踪控制方法

技术领域

本发明涉及的是一种水下机器人的运动控制方法，特别是涉及一种基于二阶滤波器的水下机器人三维路径跟踪控制方法。

背景技术

水下机器人的跟踪控制问题一直是国内外的研究热点。路径跟踪问题仅要求水下机器人的运动轨迹收敛到期望路径，而对何时到达何处并不作要求。路径跟踪技术通过引入期望路径上虚拟向导点的概念，避免了轨迹跟踪时由于水下机器人的动力学模型受到环境扰动作用，无法精确获得期望状态，导致跟踪控制***性能变差的问题。路径跟踪相比于轨迹跟踪问题，由于期望位置不受时间条件约束，从而不易导致控制器输出饱和信号，符合工程实际，更具有实际应用价值。

基于***分层递推设计思想的反步法，为水下机器人三维跟踪控制***的设计提供了有效的手段。然而，在反步法递推设计控制器的过程中，需要逐步计算中间虚拟控制量的导数，然后基于反馈线性化的思想通过设计后续子***的等价输入，实现对前级子***的镇定，通过不断迭代直到获得最终真实控制输入。当***的阶数增加或虚拟控制的形式较为复杂时，求导过程将变得十分繁琐。为此本发明采用反馈增益反步法设计三维曲线路径跟踪控制器，通过合理选择控制器的参数消除部分非线性项，相比于传统反步法设计过程简化了虚拟控制量的形式，但仍需要逐步计算虚拟控制导数的解析形式。为克服常规反步法递推过程中对虚拟控制信号逐步求导的不足，文献《TheUseofSlidingModestoSimplifytheBacksteppingControl》和《Theuseoflinearfilteringofsimplifiedintegratorbacksteppingcontrolofnonlinearsystems》分别采用滑模滤波器和线性滤波器逼近虚拟控制的导数；文献《CommandFilteredBackstepping》提出基于滤波器设计的反步法轨迹跟踪控制(IEEETransactionsOnAutomaticControl.2009,第54卷第6期)，采用二阶滤波器逼近虚拟控制信号，简化了控制器设计过程，基于奇异扰动理论证明了滤波轨迹与常规反步法的控制输出之间的误差能够收敛到零点较小邻域。文献《LandVehicleControlUsingaCommandFilteredBacksteppingApproach》将滤波反步法应用于陆地车辆的轨迹跟踪控制中；文献《基于滤波反步法的无人直升机轨迹跟踪控制》(控制与决策.2012,第27卷第4期)和文献《状态受限的小型无人直升机轨迹跟踪控制》(控制理论与应用.2012,第29卷第6期)中公开的直升机的轨迹跟踪控制中，大大简化了控制器设计过程。目前还没有相关文献讨论基于滤波器的水下机器人三维路径跟踪控制设计。

发明内容

本发明提出一种过程简化、跟踪精度高的基于二阶滤波器的水下机器人三维路径跟踪控制方法。

本发明的基于二阶滤波器的水下机器人三维路径跟踪控制方法的具体过程为：

步骤1.建立固定坐标系、机器人载体坐标系和Serret-Frenet(路径参考)坐标系，获取期望路径，水下机器人开始路径跟踪，完成两个二阶滤波器的初始化；

步骤2.通过水下机器人搭载的定位声纳传感器、姿态传感器，采集水下机器人当前位置、姿态角、角速度和速度数据信息，并结合期望路径的方向与速度，根据视线角导引思想计算得到水下机器人理想的姿态控制量ψ_co、θ_co，和理想速度控制量u_co；

所涉及的水下机器人理想的姿态控制量ψ_co、θ_co，和理想速度控制量u_co的计算表达式为：

${\mathrm{\ψ}}_{co}=-\arcsin (k_{2}e/\sqrt{1+{\left(k_{2}e\right)}^2})---\left(1\right)$

${\mathrm{\θ}}_{co}=\arcsin (k_{3}h/\sqrt{1+{\left(k_{3}h\right)}^2})---\left(2\right)$

u_co＝-k₁s+u_rcosψ_cocosθ_co(3)

其中增益因子k₁＞0，k₂＞0，k₃＞0为视线角导引律归一化参数，变量s、e和h分别表示机器人载体坐标系下机器人与期望路径参考点的前向、横向和垂向跟踪误差。

步骤3.将步骤2中得到的理想控制量ψ_co、θ_co、u_co输入至基于水下机器人三维路径跟踪运动学误差模型所建立的二阶滤波器，得到水下机器人的姿态与速度控制量ψ_c、θ_c、u_c，及其导数结合机器人运动变量ψ、θ、u，得到滤波姿态与速度跟踪误差量 与理想角速度控制量r_co、q_co；

所涉及的水下机器人三维路径跟踪运动学误差模型为：

$\left\{\begin{array}{c}\stackrel{\·}{s}=re-qh+u-u_r{\mathrm{cos\ψ}}_e{\mathrm{cos\θ}}_e\\ \stackrel{\·}{e}=-rs+u_r{\mathrm{sin\ψ}}_e{\mathrm{cos\θ}}_e+v\\ \stackrel{\·}{h}=qs-u_r{\mathrm{sin\θ}}_e+w\end{array}\right.---\left(4\right)$

$\left\{\begin{array}{c}{\stackrel{\·}{\mathrm{\ψ}}}_e=\frac{r}{cos\mathrm{\θ}}-r_F\\ {\stackrel{\·}{\mathrm{\θ}}}_e=q-q_F\end{array}\right.---\left(5\right)$

其中ψ_e＝ψ-ψ_F，θ_e＝θ-θ_F，机器人纵向速度u、横向速度v和垂向速度w，艏摇角速度r和纵倾角速度q，u_r为待设计期望路径上虚拟向导点的期望速度，其方向沿曲线路径的切线方向；ψ_F为u_r速度方向与固定坐标系水平轴的夹角，θ_F为u_r速度方向与固定坐标系垂直轴的夹角；ψ为机器人艏向角，θ为机器人纵倾角。

步骤4.将步骤3中得到的水下机器人理想角速度控制量r_co、q_co输入至另一个基于水下机器人三维路径跟踪运动学误差模型所建立的二阶滤波器，得到水下机器人的角速度控制量r_c、q_c，及其导数结合机器人角运动变量r、q，得到滤波角速度跟踪误差量

步骤5.利用步骤3中得到的滤波姿态与速度跟踪误差量以及步骤4中得到的滤波角速度跟踪误差量解算得到水下机器人推进器推力F_u，与水平舵角δ_s、垂直舵角δ_r，分别作用于机器人推进器及舵机，实现三维路径跟踪控制；

所涉及的水下机器人推进器推力F_u，与水平舵角δ_s、垂直舵角δ_r的表达式为：

$\left\{\begin{array}{c}{F}_u=m_1(-k_u\tilde{u}-k_{iu}{\mathrm{\ϵ}}_1+{\stackrel{\·}{u}}_c-u_{bs})-f_u\\ {\mathrm{\δ}}_s=b_1^{-1}\[m_4(-k_q\tilde{q}-k_{iq}{\mathrm{\ϵ}}_2+{\stackrel{\·}{q}}_c-q_{bs})-f_q\]\\ {\mathrm{\δ}}_r=b_2^{-1}\[m_5(-k_r\tilde{r}-k_{ir}{\mathrm{\ϵ}}_3+{\stackrel{\·}{r}}_c-r_{bs})-f_r\]\end{array}\right.---\left(6\right)$

其中f_u、f_q和f_r为模型非线性水动力项；u_bs、q_bs和r_bs为反馈补偿鲁棒项；m₁、m₄、m₅分别为由流体产生的附加质量；k_u、k_q、k_r、k_iu、k_iq和k_ir均为控制器参数；

所涉及的水下机器人动力学模型为：

$\begin{array}{c}\stackrel{\·}{u}=\frac{m_2}{m_1}vr-\frac{m_3}{m_1}wq-\frac{d_1}{m_1}u+\frac{1}{m_1}{F}_u+{\mathrm{\ω}}_1\\ \stackrel{\·}{v}=-\frac{m_1}{m_2}vr-\frac{d_2}{m_2}v+{\mathrm{\ω}}_2\\ \stackrel{\·}{w}=\frac{m_1}{m_3}uq-\frac{d_3}{m_3}w+g_1+{\mathrm{\ω}}_3\\ \stackrel{\·}{q}=\frac{m_3-m_1}{m_5}uw-\frac{d_4}{m_5}q-g_2+\frac{1}{m_5}{b}_1{\mathrm{\δ}}_s+{\mathrm{\ω}}_4\\ \stackrel{\·}{r}=\frac{m_1-m_2}{m_6}uv-\frac{d_5}{m_6}r+\frac{1}{m_6}{b}_2{\mathrm{\δ}}_r+{\mathrm{\ω}}_5\end{array}---\left(7\right)$

其中

$m_1=m-X_{\stackrel{\·}{u}},m_2=m-Y_{\stackrel{\·}{v}},m_3=m-Z_{\stackrel{\·}{w}}$

$m_5=I_y-M_{\stackrel{\·}{q}},m_6=I_z-N_{\stackrel{\·}{r}}$

g₁＝(W-B)cosθ,g₂＝(z_gW-z_bB)sinθ

d₁＝X_u+X_u|u_||u|,d₂＝Y_v+Y_v|v||v|

d₃＝Z_w+Z_w|w||w|,d₄＝M_q+M_q|q||q|

d₅＝N_r+N_r|r||r|

$b_1=u^2{M}_{{\mathrm{\δ}}_s},b_2=u^2{N}_{{\mathrm{\δ}}_r}$

其中，m和m_(·)分别表示机器人质量和由流体作用产生的附加质量，I_y为机器人绕y轴的转动惯量，I_z为机器人绕z轴的转动惯量，X_(·)、Y_(·)、Z_(·)、M_(·)和N_(·)为粘性流体水动力系数；z_g和z_b分别为载体坐标下垂直轴上重心和浮心的坐标位置，W和B分别表示机器人受到的重力和浮力，d_(·)为非线性阻尼水动力项，和为水平舵和垂直舵舵效系数，ω_(·)表示为干扰作用项。

步骤6.利用步骤3中得到的水下机器人姿态与速度控制量ψ_c、θ_c、u_c，滤波姿态与速度跟踪误差量与理想角速度控制量r_co、q_co，结合步骤4中得到的水下机器人的角速度控制量r_c、q_c，以及滤波角速度跟踪误差量构造滤波误差补偿回路；

所涉及的滤波误差补偿回路中误差补偿鲁棒项的表达式为：

${\mathrm{\ψ}}_{bs}=g^T\left(\widetilde{\mathrm{\ψ}}\right)B^T{u}_r\left[\begin{array}{c}{\mathrm{\υ}}_x\\ {\mathrm{\υ}}_y\\ {\mathrm{\υ}}_z\end{array}\right]---\left(8\right)$

${\mathrm{\θ}}_{bs}=g^T\left(\widetilde{\mathrm{\θ}}\right)C^T{u}_r\left[\begin{array}{c}{\mathrm{\υ}}_x\\ {\mathrm{\υ}}_y\\ {\mathrm{\υ}}_z\end{array}\right]---\left(9\right)$

$u_{bs}=\frac{1}{p_{21}}{A}^T\left[\begin{array}{c}{\mathrm{\υ}}_x\\ {\mathrm{\υ}}_y\\ {\mathrm{\υ}}_z\end{array}\right]---\left(10\right)$

$r_{bs}=\frac{1}{p_{23}}\frac{{\mathrm{\υ}}_{\mathrm{\ψ}}}{cos\mathrm{\θ}}---\left(11\right)$

$q_{bs}=\frac{1}{p_{22}}\mathrm{\υ}\mathrm{\θ}---\left(12\right)$

其中，p₂₁、p₂₂、p₂₃为李雅普诺夫方程的解矩阵中的元素；

$A=\left[\begin{array}{c}1\\ 0\\ 0\end{array}\right],B=\left[\begin{array}{cc}{\mathrm{cos\θ}}_e{\mathrm{cos\ψ}}_c& -{\mathrm{cos\θ}}_e{\mathrm{sin\ψ}}_c\\ {\mathrm{cos\θ}}_c{\mathrm{sin\ψ}}_c& {\mathrm{cos\θ}}_c{\mathrm{cos\ψ}}_c\\ 0& 0\end{array}\right],C=\left[\begin{array}{cc}{\mathrm{cos\ψ}}_e{\mathrm{cos\θ}}_c& -{\mathrm{cos\ψ}}_e{\mathrm{sin\θ}}_c\\ {\mathrm{sin\ψ}}_e{\mathrm{cos\θ}}_c& -{\mathrm{sin\ψ}}_e{\mathrm{sin\θ}}_c\\ -{\mathrm{sin\θ}}_c& -{\mathrm{cos\θ}}_c\end{array}\right],$

$g\left(\widetilde{\mathrm{\ψ}}\right)=\left[\begin{array}{c}\frac{cos\widetilde{\mathrm{\ψ}}-1}{\widetilde{\mathrm{\ψ}}}\\ \frac{sin\widetilde{\mathrm{\ψ}}}{\widetilde{\mathrm{\ψ}}}\end{array}\right],g\left(\widetilde{\mathrm{\θ}}\right)=\left[\begin{array}{c}\frac{cos\widetilde{\mathrm{\θ}}-1}{\widetilde{\mathrm{\θ}}}\\ \frac{sin\widetilde{\mathrm{\θ}}}{\widetilde{\mathrm{\θ}}}\end{array}\right],$ 且满足 $\underset{\widetilde{\mathrm{\ψ}}\→0}{\lim }g\left(\widetilde{\mathrm{\ψ}}\right)=\left[\begin{array}{c}0\\ 1\end{array}\right],\underset{\widetilde{\mathrm{\θ}}\→0}{\lim }g\left(\widetilde{\mathrm{\θ}}\right)=\left[\begin{array}{c}0\\ 1\end{array}\right];$

位置滤波信号补偿误差为

$\left[\begin{array}{c}{\mathrm{\υ}}_x\\ {\mathrm{\υ}}_y\\ {\mathrm{\υ}}_z\end{array}\right]=\left[\begin{array}{c}\tilde{s}-{\mathrm{\ζ}}_x\\ \tilde{s}-{\mathrm{\ζ}}_y\\ \tilde{h}-{\mathrm{\ζ}}_z\end{array}\right]---\left(13\right)$

其中位置滤波补偿动态量ζ_x、ζ_y和ζ_z表达式为

$\left[\begin{array}{c}{\stackrel{\·}{\mathrm{\ζ}}}_x\\ {\stackrel{\·}{\mathrm{\ζ}}}_y\\ {\stackrel{\·}{\mathrm{\ζ}}}_z\end{array}\right]=\left[\begin{array}{c}{\mathrm{r\ζ}}_y-{\mathrm{q\ζ}}_z\\ -{\mathrm{r\ζ}}_x\\ {\mathrm{q\ζ}}_x\end{array}\right]\left[\begin{array}{c}-k_x{\mathrm{\ζ}}_x\\ -k_y{\mathrm{\ζ}}_y\\ -k_z{\mathrm{\ζ}}_z\end{array}\right]+\left[\begin{array}{c}{\stackrel{\·}{s}}_c-{\stackrel{\·}{s}}_{co}\\ {\stackrel{\·}{e}}_c-{\stackrel{\·}{e}}_{co}\\ {\stackrel{\·}{h}}_c-{\stackrel{\·}{h}}_{co}\end{array}\right]+u_r\[\begin{array}{ccc}A& Bg\left(\widetilde{\mathrm{\ψ}}\right)& Cg\left(\widetilde{\mathrm{\θ}}\right)\end{array}\]\left[\begin{array}{c}{\mathrm{\ζ}}_u\\ {\mathrm{\ζ}}_{\mathrm{\ψ}}\\ {\mathrm{\ζ}}_{\mathrm{\θ}}\end{array}\right]---\left(14\right)$

且有ζ_x(0)＝0、ζ_y(0)＝0、ζ_z(0)＝0；s_co、e_co、h_co为期望位置信号，s_c、e_c、h_c为位置控制信号，ζ_u、ζ_ψ和ζ_θ分别为构造的滤波补偿变量；

滤波器输出的姿态信号补偿量为：

$\left[\begin{array}{c}{\mathrm{\υ}}_{\mathrm{\ψ}}\\ {\mathrm{\υ}}_{\mathrm{\θ}}\end{array}\right]=\left[\begin{array}{c}\widetilde{\mathrm{\ψ}}-{\mathrm{\ζ}}_{\mathrm{\ψ}}\\ \widetilde{\mathrm{\θ}}-{\mathrm{\ζ}}_{\mathrm{\θ}}\end{array}\right]---\left(15\right)$

滤波姿态补偿动态表达式为：

${\stackrel{\·}{\mathrm{\ζ}}}_{\mathrm{\ψ}}=-k_{\mathrm{\ψ}}{\mathrm{\ζ}}_{\mathrm{\ψ}}+\frac{r_c-r_{co}}{cos\mathrm{\θ}}+\frac{{\mathrm{\ζ}}_r}{\cos \mathrm{\θ}}---\left(16\right)$

${\stackrel{\·}{\mathrm{\ζ}}}_{\mathrm{\θ}}=-k_{\mathrm{\θ}}{\mathrm{\ζ}}_{\mathrm{\θ}}+(q_c-q_{co})+{\mathrm{\ζ}}_q---\left(17\right)$

且有ζ_ψ(0)＝0，ζ_θ(0)＝0，ζ_r＝0，ζ_q＝0。

步骤7.计算当前水下机器人位置ηⁿ＝(x,y,z)与标定的转向点WP_k＝(x_k,y_k,z_k)之间的距离若小于设定的切换半径R，则表示完成当前指定路径的跟踪任务，否则继续步骤2。

本发明采用滤波反步法进行水下机器人三维路径跟踪控制，通过设计二阶滤波器，能够实现对虚拟控制和其导数信号的估计，避免了对虚拟信号的解析求导，引入滤波补偿***保证滤波信号的跟踪精度，基于李雅普诺夫稳定性理论保证了***跟踪误差收敛于零点，控制性能优于动态面控制。

本发明具有如下优点及效果：

1.将三维路径跟踪控制问题，分解为对位置、姿态和速度回路分别控制的问题；

2.采用二阶滤波器获得虚拟控制的滤波信号及其导数形式，避免了反步法设计中由于需要逐步计算中间虚拟控制的导数形式而导致“项数膨胀”的问题，简化了控制器设计过程；

3.通过构造滤波补偿***，保证了滤波器对参考输入信号的跟踪精度，实现了***跟踪误差渐近收敛到零点；

4.引入速度的积分环节消除跟踪信号稳态误差。

附图说明

图1中给出基于滤波反步法的AUV三维路径跟踪控制器框图；

图2机器人三维路径跟踪示意图；

图3二阶滤波器结构图；

图4机器人三维路径跟踪轨迹；

图5机器人三维路径跟踪XY平面投影；

图6机器人三维路径跟踪XZ平面投影；

图7机器人三维路径跟踪误差曲线；

图8机器人三维路径跟踪速度响应；

图9机器人三维路径跟踪姿态角响应；

图10机器人三维路径跟踪纵向速度及补偿项；

图11机器人三维路径跟踪艏摇角速度及补偿项；

图12机器人三维路径跟踪纵倾角速度及补偿项；

图13机器人三维路径跟踪艏向角及补偿项；

图14机器人三维路径跟踪纵倾角及补偿项；

图15机器人三维路径跟踪控制输入。

具体实施方式

本发明内容基于二阶滤波器的水下机器人三维路径跟踪控制方法的具体实施方式如下：

1.图1中标出了AUV***的状态信号、虚拟控制量和滤波信号及其导数值的相互关系，前向回路主要通过二阶滤波器获得虚拟控制的滤波信号和导数信号，通过设计滤波误差补偿回路保证滤波器对输入信号的跟踪精度。

2.图3为机器人三维路径跟踪示意图，l_k为期望路径，{I}、{B}和{F}分别表示固定坐标系、机器人载体坐标系和Serret-Frenet坐标系；P点为期望路径l_k上的虚拟向导，Q点表示机器人质心位置，对于给定期望路径l_k为 为确定的路径参数，以l_k上虚拟向导P的为原点的移动坐标系{F}定义为将坐标系{I}分别绕ζ轴和η旋转ψ_F和θ_F角度，然后平移使固定坐标系原点O与路径上P点重合得到，这里旋转角度定义为

其中首先给出水下机器人三维路径跟踪运动学误差模型

$\left\{\begin{array}{c}\stackrel{\·}{s}=re-qh+u-u_{r}cos{\mathrm{\ψ}}_{e}cos{\mathrm{\θ}}_e\\ \stackrel{\·}{e}=-rs+u_r{\mathrm{sin\ψ}}_e{\mathrm{cos\θ}}_e+v\\ \stackrel{\·}{h}=qs-u_{r}sin{\mathrm{\θ}}_e+w\end{array}\right.$

$\left\{\begin{array}{c}{\stackrel{\·}{\mathrm{\ψ}}}_e=\frac{r}{cos\mathrm{\θ}}-r_F\\ {\stackrel{\·}{\mathrm{\θ}}}_e=q-q_F\end{array}\right.---\left(4\right)$

其中变量s、e和h分别表示{B}坐标系下机器人与期望路径上参考点的前向、横向和垂向跟踪误差，ψ和θ为机器人当前艏向角和纵倾角，状态变量u，v，w，q和r分别表示载体坐标系下机器人的纵向速度、横向速度、垂向速度、纵倾角速度和艏摇角速度；u_r为虚拟目标移动速度，其中

忽略横摇对机器人三维运动的影响，建立如下五自由度机器人动力学模型。

$\begin{array}{c}\stackrel{\·}{u}=\frac{m_2}{m_1}vr-\frac{m_3}{m_1}wq-\frac{d_1}{m_1}u+\frac{1}{m_1}{F}_u+{\mathrm{\ω}}_1\\ \stackrel{\·}{v}=-\frac{m_1}{m_2}\mathrm{ur}-\frac{d_2}{m_2}v+{\mathrm{\ω}}_2\\ \stackrel{\·}{w}=\frac{m_1}{m_3}uq-\frac{d_3}{m_3}w+g_1+{\mathrm{\ω}}_3\\ \stackrel{\·}{q}=\frac{m_3-m_1}{m_5}uw-\frac{d_4}{m_5}q-g_2+\frac{1}{m_5}{b}_1{\mathrm{\δ}}_s+{\mathrm{\ω}}_4\\ \stackrel{\·}{r}=\frac{m_1-m_2}{m_6}uv-\frac{d_5}{m_6}r+\frac{1}{m_6}{b}_2{\mathrm{\δ}}_r+{\mathrm{\ω}}_5\end{array}---\left(5\right)$

其中

$m_1=m-X_{\stackrel{\·}{u}},m_2=m-Y_{\stackrel{\·}{v}},m_3=m-Z_{\stackrel{\·}{w}}$

$m_5=I_y-M_{\stackrel{\·}{q}},m_6=I_z-N_{\stackrel{\·}{r}}$

g₁＝(W-B)cosθ,g₂＝(z_gW-z_bB)sinθ

d₁＝X_u+X_u|u||u|,d₂＝Y_v+Y_v|v||v|

d₃＝Z_w+Z_w|w||w|,d₄＝M_q+M_q|q||q|

d₅＝N_r+N_r|r||r|

$b_1=u^2{M}_{{\mathrm{\δ}}_s},b_2=u^2{N}_{{\mathrm{\δ}}_r}$

其中，状态变量u，v，w，q和r分别表示载体坐标系下机器人的纵向速度、横向速度、垂向速度、纵倾角速度和艏摇角速度；m和m_(·)分别表示机器人质量和由流体作用产生的附加质量，I_y为机器人绕y轴的转动惯量，I_z为机器人绕z轴的转动惯量，X_(·)、Y_(·)、Z_(·)、M_(·)和N_(·)为粘性流体水动力系数；z_g和z_b分别为载体坐标下垂直轴上重心和浮心的坐标位置，W和B分别表示机器人受到的重力和浮力，d_(·)为非线性阻尼水动力项，和为水平舵和垂直舵舵效系数，控制输入F_u、δ_s和δ_r分别表示AUV推进器推力、水平舵角和垂直舵角，ω_(·)表示为干扰作用项。

3.步骤3中滤波器的设计过程为

为了避免对虚拟控制量直接解析求导而引入复杂的计算过程，利用二阶滤波器的特性，将虚拟控制量作为滤波器的参考输入，通过积分而非微分的过程获得其滤波信号和导数值，滤波器结构定义如下：

$\left[\begin{array}{c}{x}_1\\ {\stackrel{\·}{x}}_2\end{array}\right]=\left[\begin{array}{cc}0& 1\\ -{\mathrm{\ω}}_n^2& -2{\mathrm{\ζ\ω}}_n\end{array}\right]\left[\begin{array}{c}{x}_1\\ x_2\end{array}\right]+\left[\begin{array}{c}0\\ {\mathrm{\ω}}_n^2\end{array}\right]\stackrel{\‾}{x}---\left(6\right)$

其中 ${\left[\begin{array}{cc}{x}_1& x_2\end{array}\right]}^T={\left[\begin{array}{cc}{x}_c& {\stackrel{\·}{x}}_c\end{array}\right]}^T,$ 为滤波器的参考输入信号，上式为线性稳定***，可见当为有界值，则x_c和均为连续有界信号，从输入信号到输出信号x_c的传递函数为

$H\left(s\right)=\frac{X_c\left(s\right)}{\stackrel{\‾}{X}\left(s\right)}=\frac{{\mathrm{\ω}}_n^2}{s^2+2{\mathrm{\ζ\ω}}_{n}s+{\mathrm{\ω}}_n^2}---\left(7\right)$

其中ζ和ω_n分别表示阻尼比和自然角频率；如果信号的带宽低于H(s)，那么误差信号将会很小，假设在已知带宽的情况下，通过选择足够大的自然角频率ω_n就能够获得x_c和x_c且保证逼近误差很小。从上式可以看出，信号是通过积分过程而非微分过程得到的，这可以大大减少基于状态反馈设计的控制***中测量噪声的影响，同时选择过大的ω_n又会增加***高频噪声的影响，这就需要综合考虑，选择合理的ω_n满足控制性能。

4.步骤6中针对位置跟踪回路，根据视线角导引思想给出路径跟踪的期望视线角导引律

和纵向速度，并设计位置跟踪滤波补偿***的过程为

对于位置跟踪误差***(3)设计机器人的运动学控制器u、姿态角ψ和θ的虚拟控制量分别为

u_co＝-k₁s+u_rcosψ_cocosθ_co(8)

${\mathrm{\ψ}}_{co}=-\arcsin (k_{2}e/\sqrt{1+{\left(k_{2}e\right)}^2})---\left(9\right)$

${\mathrm{\θ}}_{co}=\arcsin (k_{3}h/\sqrt{1+{\left(k_{3}h\right)}^2})---\left(10\right)$

其中增益因子k₁＞0，k₂＞0，k₃＞0为视线角导引律归一化参数，则***(3)变为

$\left\{\begin{array}{c}\stackrel{\·}{s}=re-qh-k_{1}s\\ \stackrel{\·}{e}=-rs-u_r\frac{k_{2}e}{\sqrt{1+{\left(k_{2}e\right)}^2}}\frac{1}{\sqrt{1+{\left(k_{3}h\right)}^2}}+v\\ \stackrel{\·}{h}=qs-u_r\frac{k_{3}h}{\sqrt{1+{\left(k_{3}h\right)}^2}}+w\end{array}\right.---\left(11\right)$

对于位置跟踪误差***式(3)，构造李雅普诺夫能量函数

$V_1=\frac{1}{2}{l}^2---\left(12\right)$

其中对方程(12)求导，将式(11)代入得

$\begin{array}{c}{\stackrel{\·}{V}}_1=-k_1{s}^2-k_2{u}_r\frac{1}{\sqrt{1+{\left(k_{2}e\right)}^2}}\frac{1}{\sqrt{1+{\left(k_{3}h\right)}^2}}{e}^2\\ -k_3{u}_r\frac{1}{\sqrt{1+{\left(k_{3}e\right)}^2}}{h}^2+ev+hw\end{array}---\left(13\right)$

为避免反步法后续设计中需要对θ_co和ψ_co进求导，引起计算“项数膨胀”的不足，定义θ_c，和ψ_c，为理想信号θ_co和ψ_co通过二阶滤波器的滤波信号和其导数值，滤波器定义如下

$\left[\begin{array}{c}{\stackrel{\·}{\mathrm{\θ}}}_c\\ {\stackrel{\·\·}{\mathrm{\θ}}}_c\end{array}\right]=\left[\begin{array}{cc}0& 1\\ -{\mathrm{\ω}}_n^2& -2{\mathrm{\ζ\ω}}_n\end{array}\right]\left[\begin{array}{c}{\mathrm{\θ}}_c\\ {\stackrel{\·}{\mathrm{\θ}}}_c\end{array}\right]+\left[\begin{array}{c}0\\ {\mathrm{\ω}}_n^2\end{array}\right]{\mathrm{\θ}}_{co}---\left(14\right)$

滤波器的初始值θ_c(0)＝θ_co(0)；

$\left[\begin{array}{c}{\stackrel{\·}{\mathrm{\ψ}}}_c\\ {\stackrel{\·\·}{\mathrm{\ψ}}}_c\end{array}\right]=\left[\begin{array}{cc}0& 1\\ -{\mathrm{\ω}}_n^2& -2{\mathrm{\ζ\ω}}_n\end{array}\right]\left[\begin{array}{c}{\mathrm{\ψ}}_c\\ {\stackrel{\·}{\mathrm{\ψ}}}_c\end{array}\right]+\left[\begin{array}{c}0\\ {\mathrm{\ω}}_n^2\end{array}\right]{\mathrm{\ψ}}_{co}---\left(15\right)$

滤波器的初始值ψ_c(0)＝ψ_co(0)；

这里定义位置滤波跟踪误差信号

$\left[\begin{array}{c}\tilde{s}\\ \tilde{e}\\ \tilde{h}\end{array}\right]=\left[\begin{array}{c}s-s_c\\ e-e_c\\ h-h_c\end{array}\right]---\left(16\right)$

对式(16)两边求导，将式(3)代入得

$\left[\begin{array}{c}\stackrel{\·}{\tilde{s}}\\ \stackrel{\·}{\tilde{e}}\\ \stackrel{\·}{\tilde{h}}\end{array}\right]=\left[\begin{array}{c}r\tilde{e}-q\tilde{h}\\ -r\tilde{s}\\ q\tilde{s}\end{array}\right]\[\begin{array}{ccc}A& ABg\left(\widetilde{\mathrm{\ψ}}\right)u_r& Cg\left(\widetilde{\mathrm{\θ}}\right)u_r\end{array}\]\left[\begin{array}{c}\tilde{u}\\ \widetilde{\mathrm{\ψ}}\\ \widetilde{\mathrm{\θ}}\end{array}\right]---\left(17\right)$

其中，定义滤波跟踪误差为 $\tilde{u}=u-u_c,\widetilde{\mathrm{\ψ}}={\mathrm{\ψ}}_e-{\mathrm{\ψ}}_c,\widetilde{\mathrm{\θ}}={\mathrm{\θ}}_e-{\mathrm{\θ}}_c,$

$A=\left[\begin{array}{c}1\\ 0\\ 0\end{array}\right],B=\left[\begin{array}{cc}{\mathrm{cos\θ}}_e{\mathrm{cos\ψ}}_c& -{\mathrm{cos\θ}}_e{\mathrm{sin\ψ}}_c\\ {\mathrm{cos\θ}}_c{\mathrm{sin\ψ}}_c& {\mathrm{cos\θ}}_c{\mathrm{cos\ψ}}_c\\ 0& 0\end{array}\right]$

$C=\left[\begin{array}{cc}{\mathrm{cos\ψ}}_c{\mathrm{cos\θ}}_c& -{\mathrm{cos\ψ}}_c{\mathrm{sin\θ}}_c\\ {\mathrm{sin\ψ}}_e{\mathrm{cos\θ}}_c& -{\mathrm{sin\ψ}}_e{\mathrm{sin\θ}}_c\\ -{\mathrm{sin\θ}}_c& -{\mathrm{cos\θ}}_c\end{array}\right]$

$g\left(\widetilde{\mathrm{\ψ}}\right)=\left[\begin{array}{c}\frac{cos\widetilde{\mathrm{\ψ}}-1}{\widetilde{\mathrm{\ψ}}}\\ \frac{\sin \widetilde{\mathrm{\ψ}}}{\widetilde{\mathrm{\ψ}}}\end{array}\right],g\left(\widetilde{\mathrm{\θ}}\right)=\left[\begin{array}{c}\frac{cos\widetilde{\mathrm{\θ}}-1}{\widetilde{\mathrm{\θ}}}\\ \frac{sin\widetilde{\mathrm{\θ}}}{\widetilde{\mathrm{\θ}}}\end{array}\right],$ 且满足 $\underset{\widetilde{\mathrm{\ψ}}\→0}{\lim }g\left(\widetilde{\mathrm{\ψ}}\right)=\left[\begin{array}{c}0\\ 1\end{array}\right],\underset{\widetilde{\mathrm{\θ}}\→0}{\lim }g\left(\widetilde{\mathrm{\θ}}\right)=\left[\begin{array}{c}0\\ 1\end{array}\right]$ 对式(16)左右移项，增加和减少一项 ${\left[\begin{array}{ccc}{\stackrel{\·}{s}}_{co}& {\stackrel{\·}{e}}_{co}& {\stackrel{\·}{h}}_{co}\end{array}\right]}^T$ 得到

$\left[\begin{array}{c}\stackrel{\·}{s}\\ \stackrel{\·}{e}\\ \stackrel{\·}{h}\end{array}\right]=\left[\begin{array}{c}{\stackrel{\·}{s}}_{co}\\ {\stackrel{\·}{e}}_{co}\\ {\stackrel{\·}{h}}_{co}\end{array}\right]+\left[\begin{array}{c}\stackrel{\·}{\tilde{s}}\\ \stackrel{\·}{\tilde{e}}\\ \stackrel{\·}{\tilde{h}}\end{array}\right]+\left[\begin{array}{c}{\stackrel{\·}{s}}_c-{\stackrel{\·}{s}}_{co}\\ {\stackrel{\·}{e}}_c-{\stackrel{\·}{e}}_{co}\\ {\stackrel{\·}{h}}_c-{\stackrel{\·}{h}}_{co}\end{array}\right]---\left(18\right)$

根据跟踪控制器设计目标，选择理想期望位置信号 ${\left[\begin{array}{ccc}{\stackrel{\·}{s}}_{co}& {\stackrel{\·}{e}}_{co}& {\stackrel{\·}{h}}_{co}\end{array}\right]}^T$ 为

$\left[\begin{array}{c}{\stackrel{\·}{s}}_{co}\\ {\stackrel{\·}{e}}_{co}\\ {\stackrel{\·}{h}}_{co}\end{array}\right]=\left[\begin{array}{c}-k_x\tilde{s}+{\stackrel{\·}{s}}_c\\ -k_y\tilde{e}+{\stackrel{\·}{e}}_c\\ -k_z\tilde{h}+{\tilde{h}}_c\end{array}\right]---\left(19\right)$

定义角度滤波跟踪误差信号为

$\left[\begin{array}{c}\tilde{u}\\ \widetilde{\mathrm{\ψ}}\\ \widetilde{\mathrm{\θ}}\end{array}\right]=\left[\begin{array}{c}u-u_c\\ {\mathrm{\ψ}}_e-{\mathrm{\ψ}}_c\\ {\mathrm{\θ}}_e-{\mathrm{\θ}}_c\end{array}\right]---\left(20\right)$

将式(19)和(17)代入(18)整理得

$\left[\begin{array}{c}\stackrel{\·}{\tilde{s}}\\ \stackrel{\·}{\tilde{e}}\\ \stackrel{\·}{\tilde{h}}\end{array}\right]=\left[\begin{array}{c}r\tilde{e}-q\tilde{h}\\ -r\tilde{s}\\ q\tilde{s}\end{array}\right]+\left[\begin{array}{c}-k_x\tilde{s}\\ -k_y\tilde{e}\\ -k_z\tilde{h}\end{array}\right]+\left[\begin{array}{c}{\stackrel{\·}{s}}_c-{\stackrel{\·}{s}}_{co}\\ {\stackrel{\·}{e}}_c-{\stackrel{\·}{e}}_{co}\\ {\stackrel{\·}{h}}_c-{\stackrel{\·}{h}}_{co}\end{array}\right]+\[\begin{array}{ccc}A& Bg\left(\widetilde{\mathrm{\ψ}}\right)u_r& Cg\left(\widetilde{\mathrm{\θ}}\right)\end{array}{u}_r\]\left[\begin{array}{c}\tilde{u}\\ \widetilde{\mathrm{\ψ}}\\ \widetilde{\mathrm{\θ}}\end{array}\right]---\left(21\right)$

定义滤波信号补偿误差

$\left[\begin{array}{c}{\mathrm{\υ}}_x\\ {\mathrm{\υ}}_y\\ {\mathrm{\υ}}_z\end{array}\right]=\left[\begin{array}{c}\tilde{s}-{\mathrm{\ζ}}_x\\ \tilde{e}-{\mathrm{\ζ}}_y\\ \tilde{h}-{\mathrm{\ζ}}_z\end{array}\right]---\left(22\right)$

其中构造位置滤波补偿动态ζ_x、ζ_y和ζ_z如下

$\left[\begin{array}{c}{\stackrel{\·}{\mathrm{\ζ}}}_x\\ {\stackrel{\·}{\mathrm{\ζ}}}_y\\ {\stackrel{\·}{\mathrm{\ζ}}}_z\end{array}\right]=\left[\begin{array}{c}{\mathrm{r\ζ}}_y-{\mathrm{q\ζ}}_z\\ -{\mathrm{r\ζ}}_x\\ {\mathrm{q\ζ}}_x\end{array}\right]\left[\begin{array}{c}-k_x{\mathrm{\ζ}}_x\\ -k_y{\mathrm{\ζ}}_y\\ -k_z{\mathrm{\ζ}}_z\end{array}\right]+\left[\begin{array}{c}{\stackrel{\·}{s}}_c-{\stackrel{\·}{s}}_{co}\\ {\stackrel{\·}{e}}_c-{\stackrel{\·}{e}}_{co}\\ {\stackrel{\·}{h}}_c-{\stackrel{\·}{h}}_{co}\end{array}\right]+u_r\[\begin{array}{ccc}A& Bg\left(\widetilde{\mathrm{\ψ}}\right)& Cg\left(\widetilde{\mathrm{\θ}}\right)\end{array}\]\left[\begin{array}{c}{\mathrm{\ζ}}_u\\ {\mathrm{\ζ}}_{\mathrm{\ψ}}\\ {\mathrm{\ζ}}_{\mathrm{\θ}}\end{array}\right]---\left(23\right)$

这里ζ_x(0)＝0、ζ_y(0)＝0、ζ_z(0)＝0。

5.步骤6中针对姿态角跟踪回路，设计角速度控制律和姿态角跟踪滤波补偿***的过程为：

对滤波跟踪误差式(20)求导，将姿态角跟踪误差模型式(4)代入得

$\begin{array}{c}\stackrel{\·}{\widetilde{\mathrm{\ψ}}}=\frac{r}{cos\mathrm{\θ}}-r_F-{\stackrel{\·}{\mathrm{\ψ}}}_c\\ =\frac{r_{co}+(r_c-r_{co})+\tilde{r}}{\cos \mathrm{\θ}}-r_F-{\stackrel{\·}{\mathrm{\ψ}}}_c\end{array}---\left(24\right)$

$\begin{array}{c}\stackrel{\·}{\widetilde{\mathrm{\θ}}}=q-q_F-{\stackrel{\·}{\mathrm{\θ}}}_c\\ =q_{co}+(q_c-q_{co})+\tilde{q}-q_F-{\stackrel{\·}{\mathrm{\θ}}}_c\end{array}---\left(25\right)$

其中角速度跟踪误差定义为这里分别设计理想虚拟控制信号r_co和q_co为

$r_{co}=cos\mathrm{\θ}(r_F+{\stackrel{\·}{\mathrm{\ψ}}}_c-k_{\mathrm{\ψ}}\widetilde{\mathrm{\ψ}}-{\mathrm{\ψ}}_{bs})---\left(26\right)$

$q_{co}=q_F+{\stackrel{\·}{\mathrm{\θ}}}_c-k_{\mathrm{\θ}}\widetilde{\mathrm{\θ}}-{\mathrm{\θ}}_{bs}---\left(27\right)$

这里q_c和r_c和分别为理想信号q_co和r_co通过二阶滤波器得到的滤波信号和其导数值，定义滤波器形式为

$\left[\begin{array}{c}{\stackrel{\·}{q}}_c\\ {\stackrel{\·\·}{q}}_c\end{array}\right]=\left[\begin{array}{cc}0& 1\\ -{\mathrm{\ω}}_n^2& -2{\mathrm{\ζ\ω}}_n\end{array}\right]\left[\begin{array}{c}{q}_c\\ {\stackrel{\·}{q}}_c\end{array}\right]+\left[\begin{array}{c}0\\ {\mathrm{\ω}}_n^2\end{array}\right]q_{co}---\left(28\right)$

滤波器初始条件q_c(0)＝q_co(0)；

$\left[\begin{array}{c}{\stackrel{\·}{r}}_c\\ {\stackrel{\·\·}{r}}_c\end{array}\right]=\left[\begin{array}{cc}0& 1\\ -{\mathrm{\ω}}_n^2& -2{\mathrm{\ζ\ω}}_n\end{array}\right]\left[\begin{array}{c}{r}_c\\ {\stackrel{\·}{r}}_c\end{array}\right]+\left[\begin{array}{c}0\\ {\mathrm{\ω}}_n^2\end{array}\right]r_{co}---\left(29\right)$

滤波器初始条件r_c(0)＝r_co(0)；

将式(26)和式(27)代入式(24)和(25)得

$\stackrel{\·}{\widetilde{\mathrm{\ψ}}}=-k_{\mathrm{\ψ}}\widetilde{\mathrm{\ψ}}+\frac{(r_c-r_{co})+\tilde{r}}{\cos \mathrm{\θ}}-{\mathrm{\ψ}}_{bs}---\left(30\right)$

$\stackrel{\·}{\widetilde{\mathrm{\θ}}}=-k_{\mathrm{\θ}}\widetilde{\mathrm{\θ}}+(q_c-q_{co})+\tilde{q}-{\mathrm{\θ}}_{bs}---\left(31\right)$

其中k_ψ＞0和k_θ＞0为控制器参数，ψ_bs和θ_bs为待设计镇定项，这里通过定义补偿跟踪误差，对滤波器输出的信号进行反馈补偿

$\left[\begin{array}{c}{\mathrm{\υ}}_{\mathrm{\ψ}}\\ {\mathrm{\υ}}_{\mathrm{\θ}}\end{array}\right]=\left[\begin{array}{c}\widetilde{\mathrm{\ψ}}-{\mathrm{\ζ}}_{\mathrm{\ψ}}\\ \widetilde{\mathrm{\θ}}-{\mathrm{\ζ}}_{\mathrm{\θ}}\end{array}\right]---\left(32\right)$

这里构造滤波补偿动态

${\stackrel{\·}{\mathrm{\ζ}}}_{\mathrm{\ψ}}=-k_{\mathrm{\ψ}}{\mathrm{\ζ}}_{\mathrm{\ψ}}+\frac{r_c-r_{co}}{cos\mathrm{\θ}}+\frac{{\mathrm{\ζ}}_r}{\cos \mathrm{\θ}}---\left(33\right)$

${\stackrel{\·}{\mathrm{\ζ}}}_{\mathrm{\θ}}=-k_{\mathrm{\θ}}{\stackrel{\·}{\mathrm{\ζ}}}_{\mathrm{\θ}}+(q_c-q_{co})+{\mathrm{\ζ}}_q---\left(34\right)$

这里初始条件ζ_ψ(0)＝0，ζ_θ(0)＝0，ζ_r＝0，ζ_q＝0。

6.步骤6中针对速度控制回路，引入积分环节减小稳态误差，设计真实控制输入的具体过程如下：

为保证跟踪***存在外干扰下的鲁棒性，引入积分项增加***的鲁棒性，定义 这里设计机器人三维路径跟踪控制器为

$\left\{\begin{array}{c}{F}_u=m_1(-k_u\tilde{u}-k_{iu}{\mathrm{\ϵ}}_1+{\stackrel{\·}{u}}_c-u_{bs})-f_u\\ {\mathrm{\δ}}_s=b_1^{-1}\[m_4(-k_q\tilde{q}-k_{iq}{\mathrm{\ϵ}}_2+{\stackrel{\·}{q}}_c-q_{bs})-f_q\]\\ {\mathrm{\δ}}_r=b_2^{-1}\[m_5(-k_r\tilde{r}-k_{ir}{\mathrm{\ϵ}}_3+{\stackrel{\·}{r}}_c-r_{bs})-f_r\]\end{array}\right.---\left(35\right)$

其中f_u＝m₂vr-m₃wq+d₁u、f_q＝(m₁-m₃)uw+d₄q-g₂和f_r＝(m₁-m₂)uv+d₅r为模型非线性水动力项，u_bs、q_bs和r_bs为待设计反馈补偿鲁棒项在步骤6中进行设计。

7.步骤6中反馈控制项设计的具体过程如下：

首先对于步骤2中位置跟踪***结合式(22)构造李雅普诺夫能量函数

$V_1=\frac{1}{2}({\mathrm{\υ}}_x^2+{\mathrm{\υ}}_y^2+{\mathrm{\υ}}_z^2)---\left(36\right)$

对上式求导，将式(21)和(23)代入式(36)得

$\begin{array}{c}{V}_1={\stackrel{\·}{\mathrm{\υ}}}_x{\mathrm{\υ}}_x+{\stackrel{\·}{\mathrm{\υ}}}_y{\mathrm{\υ}}_y+{\stackrel{\·}{\mathrm{\υ}}}_z{\mathrm{\υ}}_z\\ =-k_x{\mathrm{\υ}}_x^2-k_y{\mathrm{\υ}}_y^2-k_z{\mathrm{\υ}}_z^2+\\ \[\begin{array}{ccc}{\mathrm{\υ}}_x& {\mathrm{\υ}}_y& {\mathrm{\υ}}_z\end{array}\]\[\begin{array}{ccc}A& Bg\left(\widetilde{\mathrm{\ψ}}\right)u_r& Cg\left(\widetilde{\mathrm{\θ}}\right)u_r\end{array}\]\left[\begin{array}{c}{\mathrm{\υ}}_u\\ {\mathrm{\υ}}_{\mathrm{\ψ}}\\ {\mathrm{\υ}}_{\mathrm{\θ}}\end{array}\right]\\ =-k_x{\mathrm{\υ}}_x^2-k_y{\mathrm{\υ}}_y^2-k_z{\mathrm{\υ}}_z^2+A^T\left[\begin{array}{c}{\mathrm{\υ}}_x\\ {\mathrm{\υ}}_y\\ {\mathrm{\υ}}_z\end{array}\right]{\mathrm{\υ}}_u+\\ g^T\left(\widetilde{\mathrm{\ψ}}\right)B^T{u}_r\left[\begin{array}{c}{\mathrm{\υ}}_x\\ {\mathrm{\υ}}_y\\ {\mathrm{\υ}}_z\end{array}\right]{\mathrm{\υ}}_{\mathrm{\ψ}}+g^T\left(\widetilde{\mathrm{\θ}}\right)C^T{u}_r\left[\begin{array}{c}{\mathrm{\υ}}_x\\ {\mathrm{\υ}}_y\\ {\mathrm{\υ}}_z\end{array}\right]{\mathrm{\υ}}_{\mathrm{\θ}}\end{array}---\left(37\right)$

其中υ_u、υ_ψ、υ_θ定义如式(32)。

然后对于姿态跟踪***结合式(32)构造李雅普诺夫能量函数

$V_2=\frac{1}{2}({\mathrm{\υ}}_{\mathrm{\ψ}}^2+{\mathrm{\υ}}_{\mathrm{\θ}}^2)---\left(38\right)$

对上式求导，将式(30)～(34)代入得:

$\begin{array}{c}{\stackrel{\·}{V}}_2={\stackrel{\·}{\mathrm{\υ}}}_{\mathrm{\ψ}}{\mathrm{\υ}}_{\mathrm{\ψ}}+{\stackrel{\·}{\mathrm{\υ}}}_0{\mathrm{\υ}}_0\\ =(\stackrel{\·}{\widetilde{\mathrm{\ψ}}}-{\stackrel{\·}{\mathrm{\ζ}}}_{\mathrm{\ψ}}){\mathrm{\υ}}_{\mathrm{\ψ}}+(\stackrel{\·}{\widetilde{\mathrm{\θ}}}-{\stackrel{\·}{\mathrm{\ζ}}}_{\mathrm{\θ}}){\mathrm{\υ}}_{\mathrm{\θ}}\\ =(-k_{\mathrm{\ψ}}\widetilde{\mathrm{\ψ}}+\frac{\tilde{r}}{\cos \mathrm{\θ}}-{\mathrm{\ψ}}_{bs}+k_{\mathrm{\ψ}}{\mathrm{\ζ}}_{\mathrm{\ψ}}-\frac{{\mathrm{\ζ}}_r}{\cos \mathrm{\θ}}){\mathrm{\υ}}_{\mathrm{\ψ}}\\ +(-k_{\mathrm{\θ}}\widetilde{\mathrm{\θ}}+\tilde{q}-{\mathrm{\θ}}_{bs}+k_{\mathrm{\θ}}{\mathrm{\ζ}}_{\mathrm{\θ}}-{\mathrm{\ζ}}_q){\mathrm{\υ}}_0\\ =-k_{\mathrm{\ψ}}{\mathrm{\υ}}_{\mathrm{\ψ}}^2-k_{\mathrm{\θ}}{\mathrm{\υ}}_{\mathrm{\θ}}^2+{\mathrm{\υ}}_q{\mathrm{\υ}}_{\mathrm{\θ}}+\frac{{\mathrm{\υ}}_r}{\cos \mathrm{\θ}}{\mathrm{\υ}}_{\mathrm{\ψ}}\\ -{\mathrm{\θ}}_{bs}{\mathrm{\υ}}_{\mathrm{\θ}}-{\mathrm{\ψ}}_{bs}{\mathrm{\υ}}_{\mathrm{\ψ}}\end{array}---\left(39\right)$

其中 ${\mathrm{\υ}}_u=\tilde{u}-{\mathrm{\ζ}}_u,{\mathrm{\υ}}_r=\tilde{r}-{\mathrm{\ζ}}_r,{\mathrm{\υ}}_q=\tilde{q}-{\mathrm{\ζ}}_q.$

再次对于速度控制回路将式(35)代入式(5)得到u、q和r的误差***为

$\left\{\begin{array}{c}\stackrel{\·}{\tilde{u}}=-k_u\tilde{u}-k_{iu}{\mathrm{\ϵ}}_1-u_{bs}\\ \stackrel{\·}{\tilde{q}}=-k_q\tilde{q}-k_{iq}{\mathrm{\ϵ}}_2-q_{bs}\\ \stackrel{\·}{\tilde{r}}=-k_r\tilde{r}-k_{ir}{\mathrm{\ϵ}}_3-r_{bs}\end{array}\right.---\left(40\right)$

由于ζ_u＝ζ_r＝ζ_q＝0，这里得到滤波补偿误差***的动态为

$\left\{\begin{array}{c}{\stackrel{\·}{\mathrm{\υ}}}_u=-k_u{\mathrm{\υ}}_u-k_{iu}{\mathrm{\ϵ}}_1-u_{bs}\\ {\stackrel{\·}{\mathrm{\υ}}}_q=-k_q{\mathrm{\υ}}_q-k_{iq}{\mathrm{\ϵ}}_2-q_{bs}\\ {\stackrel{\·}{\mathrm{\υ}}}_r=-k_r{\mathrm{\υ}}_r-k_{ir}{\mathrm{\ϵ}}_3-r_{bs}\end{array}\right.---\left(41\right)$

由于所以***(41)可以重写为

$\left\{\begin{array}{c}{\stackrel{\·\·}{\mathrm{\ϵ}}}_1=-k_u{\stackrel{\·}{\mathrm{\ϵ}}}_1-k_{iu}{\mathrm{\ϵ}}_1-u_{bs}\\ {\stackrel{\·\·}{\mathrm{\ϵ}}}_2=-k_q{\stackrel{\·}{\mathrm{\ϵ}}}_2-k_{iq}{\mathrm{\ϵ}}_2-q_{bs}\\ {\stackrel{\·\·}{\mathrm{\ϵ}}}_3=-k_r{\stackrel{\·}{\mathrm{\ϵ}}}_3-k_{ir}{\mathrm{\ϵ}}_3-r_{bs}\end{array}\right.---\left(42\right)$

定义误差向量ε＝[ε₁,ε₂,ε₃]^T，则***(42)可以表示为

$\stackrel{\·}{E}=AE+BU---\left(43\right)$

其中K_I＝diag{-k_iu,-k_iq,-k_ir}，K_P＝diag{-k_u,-k_q,-k_r}

最后结合式(36)、(43)和式(38)构造李雅普诺夫能量函数

$V_3=V_1+V_2+\frac{1}{2}{E}^{T}PE---\left(44\right)$

其中正定对称矩阵P为线性李雅普诺夫方程的解

A^TP+PA＝-Q(45)

其中 $P=\left[\begin{array}{cc}{P}_1& 0_{3\×3}\\ 0_{3\×3}& P_2\end{array}\right],$ P_i＝diag{p_i1,p_i2,p_i3}，i＝1,2为正定对称矩阵，如果选择P₁＝K_IP₂，则 $Q=\left[\begin{array}{cc}{0}_{3\×3}& 0_{3\×3}\\ 0_{3\×3}& 2K_I{P}_2\end{array}\right];$ 对式(44)进行求导，将式(37)、(39)和(43)代入整理得

$\begin{array}{c}{\stackrel{\·}{V}}_3={\stackrel{\·}{V}}_1+{\stackrel{\·}{V}}_2+E^{T}P\stackrel{\·}{E}\\ =-k_x{\mathrm{\υ}}_x^2-k_y{\mathrm{\υ}}_y^2-k_z{\mathrm{\υ}}_z^2-k_u{\mathrm{\υ}}_u^2-k_{\mathrm{\ψ}}{\mathrm{\υ}}_{\mathrm{\ψ}}^2-k_{\mathrm{\θ}}{\mathrm{\υ}}_{\mathrm{\θ}}^2\\ -\frac{1}{2}{E}^{T}QE+A^T\left[\begin{array}{c}{\mathrm{\υ}}_x\\ {\mathrm{\υ}}_y\\ {\mathrm{\υ}}_z\end{array}\right]{\mathrm{\υ}}_u+g^T\left(\widetilde{\mathrm{\ψ}}\right)B^T{u}_r\left[\begin{array}{c}{\mathrm{\υ}}_x\\ {\mathrm{\υ}}_y\\ {\mathrm{\υ}}_z\end{array}\right]{\mathrm{\υ}}_{\mathrm{\ψ}}\\ -{\mathrm{\ψ}}_{bs}{\mathrm{\υ}}_{\mathrm{\ψ}}+\frac{{\mathrm{\υ}}_r}{\cos \mathrm{\θ}}{\mathrm{\υ}}_{\mathrm{\ψ}}+g^T\left(\widetilde{\mathrm{\θ}}\right)C^T{u}_r\left[\begin{array}{c}{\mathrm{\υ}}_x\\ {\mathrm{\υ}}_y\\ {\mathrm{\υ}}_z\end{array}\right]{\mathrm{\υ}}_{\mathrm{\θ}}\\ +{\mathrm{\υ}}_q{\mathrm{\υ}}_{\mathrm{\θ}}-{\mathrm{\θ}}_{bs}{\mathrm{\υ}}_{\mathrm{\θ}}+E^{T}PBU\end{array}---\left(46\right)$

式(46)进一步变为

$\begin{array}{c}{\stackrel{\·}{V}}_3=-k_x{\mathrm{\υ}}_x^2-k_y{\mathrm{\υ}}_y^2-k_z{\mathrm{\υ}}_z^2-k_u{\mathrm{\υ}}_u^2-k_{\mathrm{\ψ}}{\mathrm{\υ}}_{\mathrm{\ψ}}^2-k_{\mathrm{\θ}}{\mathrm{\υ}}_{\mathrm{\θ}}^2\\ -\frac{1}{2}{E}^{T}QE+A^T\left[\begin{array}{c}{\mathrm{\υ}}_x\\ {\mathrm{\υ}}_y\\ {\mathrm{\υ}}_z\end{array}\right]{\mathrm{\υ}}_u+g^T\left(\widetilde{\mathrm{\ψ}}\right)B^T{u}_r\left[\begin{array}{c}{\mathrm{\υ}}_x\\ {\mathrm{\υ}}_y\\ {\mathrm{\υ}}_z\end{array}\right]{\mathrm{\υ}}_{\mathrm{\ψ}}\\ +{\mathrm{\υ}}_q{\mathrm{\υ}}_{\mathrm{\θ}}+\frac{{\mathrm{\υ}}_r}{\cos \mathrm{\θ}}{\mathrm{\υ}}_{\mathrm{\ψ}}+g^T\left(\widetilde{\mathrm{\θ}}\right)C^T{u}_r\left[\begin{array}{c}{\mathrm{\υ}}_x\\ {\mathrm{\υ}}_y\\ {\mathrm{\υ}}_z\end{array}\right]{\mathrm{\υ}}_{\mathrm{\θ}}-{\mathrm{\ψ}}_{bs}{\mathrm{\υ}}_{\mathrm{\ψ}}\\ -{\mathrm{\θ}}_{bs}{\mathrm{\υ}}_{\mathrm{\θ}}-p_{21}{\mathrm{\υ}}_u{u}_{bs}-p_{22}{\mathrm{\υ}}_q{q}_{bs}-p_{23}{\mathrm{\υ}}_r{r}_{bs}\end{array}---\left(47\right)$

如果设计反馈补偿项为

${\mathrm{\ψ}}_{bs}=g^T\left(\widetilde{\mathrm{\ψ}}\right)B^T{u}_r\left[\begin{array}{c}{\mathrm{\υ}}_x\\ {\mathrm{\υ}}_y\\ {\mathrm{\υ}}_z\end{array}\right]---\left(48\right)$

${\mathrm{\θ}}_{bs}=g^T\left(\widetilde{\mathrm{\θ}}\right)C^T{u}_r\left[\begin{array}{c}{\mathrm{\υ}}_x\\ {\mathrm{\υ}}_y\\ {\mathrm{\υ}}_z\end{array}\right]---\left(49\right)$

$u_{bs}=\frac{1}{p_{21}}{A}^T\left[\begin{array}{c}{\mathrm{\υ}}_x\\ {\mathrm{\υ}}_y\\ {\mathrm{\υ}}_z\end{array}\right]---\left(50\right)$

$r_{bs}=\frac{1}{p_{23}}\frac{{\mathrm{\υ}}_{\mathrm{\ψ}}}{cos\mathrm{\θ}}---\left(51\right)$

$q_{bs}=\frac{1}{p_{22}}{\mathrm{\υ}}_{\mathrm{\θ}}---\left(52\right)$

将式(48)(48)～(52)(52)代入式(47)(47)整理得

$\begin{array}{c}{\stackrel{\·}{V}}_3=-k_x{\mathrm{\υ}}_x^2-k_y{\mathrm{\υ}}_y^2-k_z{\mathrm{\υ}}_z^2-k_{\mathrm{\ψ}}{\mathrm{\υ}}_{\mathrm{\ψ}}^2-k_{\mathrm{\θ}}{\mathrm{\υ}}_{\mathrm{\θ}}^2\\ -\frac{1}{2}{E}^{T}QE\≤0\end{array}---\left(53\right)$

上述定理证明了补偿跟踪误差***υ_i的指数收敛性，由二阶滤波器的设计过程可知，当选择合适的自然频率ω_n，x_co为滤波器的参考输入信号，滤波器为线性稳定***，可见当x_co为有界值，则x_c和均为连续有界信号，如果信号x_co的带宽低于滤波器设计带宽，那么误差信号|x_co(t)-x_c(t)|将会很小，由于ζ_i是一阶稳定线性***，所以ζ_i将趋近于零值，从而***跟踪误差指数趋近于零值。由于在控制回路中引入了滤波器，滤波器的跟踪精度直接影响***的控制性能，动态面控制由于未考虑滤波信号的跟踪精度，只能够保证***跟踪误差收敛到原点较小的邻域。这里通过构造滤波补偿***，提高滤波信号跟踪精度，通过稳定性分析保证闭环***跟踪误差收敛到零点。

仿真验证

下面举例说明，验证本发明方法的有效性：

根据水动力系数建立机器人六自由度仿真模型，采用Matlab环境搭建机器人三维路径跟踪控制仿真***。

针对机器人螺旋下潜作业，规划期望三维曲线路径为(单位：m)

选取机器人的初始位置为[x,y,z]^T＝[10,-5,1]^T(m)，初始艏向为ψ＝π/4(rad)，纵倾角θ＝0(rad)，机器人初始速度为[u,v,w]^T＝0(m/s)，初始角速度q＝0(rad/s)，r＝0(rad/s)，控制器参数k_x＝k_y＝k_z2＝，k_ψ＝k_θ＝5，k_u＝k_q＝k_r＝20，k_iu＝k_iq＝k_ir＝10，p₂₁＝p₂₂＝p₂₃＝5。

为了避免对虚拟控制量直接解析求导，引入复杂的计算过程，本文利用二阶滤波器的特性，将理想虚拟控制量a_co作为滤波器的参考输入，信号是通过积分而非微分的过程获得的，这可以大大减少基于状态反馈设计的控制***中测量噪声的影响，假设在已知a_co带宽的情况下，通过选择足够大的自然角频率ω_n就能够获得a_c和且保证逼近误差|α_co(t)-α_c(t)|很小；同时选择过大的ω_n又会增加***高频噪声的影响，这就需要综合考虑，选择合理的ω_n满足控制性能，这里选取ζ＝0.9，ω_n＝20；

图2～图13给出机器人三维曲线路径跟踪控制仿真对比结果。

图2为机器人三维螺旋下潜路径跟踪轨迹，图3和图4分别为机器人三维路径跟踪轨迹在XY平面和XZ平面的投影曲线。从中可以看出由于常规反步法基于精确数学模型设计的控制器应用于真实模型时，直接对虚拟控制求导获得其导数得解析形式，在存在模型不确定性和测量噪声时控制效果较差，而本文基于滤波器设计的非线性控制器，通过积分过程而非微分过程获得虚拟控制的滤波值和导数值因而对测量噪声具有一定的滤波作用，通过滤波补偿***，能够保证理想虚拟控制量的滤波值对真实状态的逼近，进而补偿标称模型的状态响应与真实模型状态响应的偏差，对模型不确定性具有较好的鲁棒性，能够很好的实现跟踪控制，提高了跟踪精度。

图5为机器人三维路径跟踪控制中跟踪误差曲线，与常规反步法控制器相比，可以看出本文设计的三维路径控制器提高了路径跟踪的精度，缩短了机器人的冗余航程，具有更加稳定的控制能力保证机器人较快的跟踪并收敛到期望路径，使得跟踪误差最终收敛到零，表明了控制器具有良好的跟踪精度和响应速度。

图6和图7分别为机器人三维里路径跟踪控制过程中各状态变量包括线速度和姿态角的变化曲线，可以看出机器人在沿螺旋线下潜过程中横向速度和垂向速度相比于纵向速度较小，且为有界值，常规反步法设计过程无法处理***状态的测量噪声，而本文基于滤波器设计的控制器对***的测量噪声具有一定的滤波效果。

图8为机器人纵向速度u、理想虚拟控制量u_co和其滤波信号u_c的响应曲线，从局部放大图中可以看出滤波信号u_c较好的跟踪了理想虚拟信号u_co，滤波器对于状态u包含的测量噪声具有一定的滤波作用，u_bs为滤波补偿项，当跟踪***稳定时u_bs最终稳定且收敛于零。图9为机器人艏摇角速度r、理想虚拟控制量r_co和其滤波信号r_c的响应曲线，从局部放大图中可以看出滤波信号r_c较好的跟踪了理想虚拟信号r_co，滤波器对于艏摇角速度r包含的测量噪声具有一定的滤波作用，r_bs为滤波补偿项，当跟踪***稳定时r_bs最终收敛到零。

图10为机器人纵倾角速度q、理想虚拟控制量q_co和其滤波信号q_c的响应曲线，从局部放大图中可以看出滤波信号q_c较好的跟踪了理想虚拟信号q_co，滤波器对于艏摇角速度q包含的测量噪声具有一定的滤波作用，q_bs为滤波补偿项，当跟踪***稳定时q_bs最终稳定收敛于零。

图11和12分别为滤波反步法设计中的机器人艏摇角和纵倾角、理想控制信号和滤波信号变化曲线，从局部放大图可以看出滤波信号ψ_c和θ_c较好的跟踪了理想虚拟信号ψ_co和θ_co，滤波器对于艏摇角ψ和纵倾角θ中包含的测量噪声具有一定的滤波作用，从滤波补偿项ψ_bs和θ_bs的变化趋势可以看出，当跟踪***稳定时ψ_bs和θ_bs将最终收敛到零点。

图13为机器人三维路径跟踪控制输入响应，由图可知本发明的方法响应曲线更加平滑。

Claims (5)

1.一种基于二阶滤波器的水下机器人三维路径跟踪控制方法，其特征在于包含以下步骤：

步骤1.建立固定坐标系、机器人载体坐标系和Serret-Frenet坐标系，获取期望路径，水下机器人开始路径跟踪，完成两个二阶滤波器的初始化；

步骤2.通过水下机器人搭载的定位声纳传感器、姿态传感器，采集水下机器人当前位置、姿态角、角速度和速度数据信息，并结合期望路径的方向与速度，根据视线角导引思想计算得到水下机器人理想的姿态控制量ψ_co、θ_co，和理想速度控制量u_co；

步骤3.将步骤2中得到的理想控制量ψ_co、θ_co、u_co输入至基于水下机器人三维路径跟踪运动学误差模型所建立的二阶滤波器，得到水下机器人的姿态与速度控制量ψ_c、θ_c、u_c，及其导数结合机器人运动变量ψ、θ、u，得到滤波姿态与速度跟踪误差量 与理想角速度控制量r_co、q_co；

步骤4.将步骤3中得到的水下机器人理想角速度控制量r_co、q_co输入至另一个基于水下机器人三维路径跟踪运动学误差模型所建立的二阶滤波器，得到水下机器人的角速度控制量r_c、q_c，及其导数结合机器人角运动变量r、q，得到滤波角速度跟踪误差量

步骤5.利用步骤3中得到的滤波姿态与速度跟踪误差量以及步骤4中得到的滤波角速度跟踪误差量解算得到水下机器人推进器推力F_u，与水平舵角垂直舵角δ_r，分别作用于机器人推进器及舵机，实现三维路径跟踪控制；

步骤6.利用步骤3中得到的水下机器人姿态与速度控制量ψ_c、θ_c、u_c，滤波姿态与速度跟踪误差量与理想角速度控制量r_co、q_co，结合步骤4中得到的水下机器人的角速度控制量r_c、q_c，以及滤波角速度跟踪误差量构造滤波误差补偿回路；

步骤7.计算当前水下机器人位置ηⁿ＝(x,y,z)与标定的转向点WP_k＝(x_k,y_k,z_k)之间的距离若小于设定的切换半径R，则表示完成当前指定路径的跟踪任务，否则继续步骤2。

2.根据权利要求1所述的基于二阶滤波器的水下机器人三维路径跟踪控制方法，其特征在于，步骤2中所涉及的水下机器人理想的姿态控制量ψ_co、θ_co，和理想速度控制量u_co的计算表达式为：

${\mathrm{\ψ}}_{co}=-arcsin(k_{2}e/\sqrt{1+{\left(k_{2}e\right)}^2})---\left(1\right)$

${\mathrm{\θ}}_{co}=arcsin\left(k_{3}h/\sqrt{1+{\left(k_{3}h\right)}^2}\right)---\left(2\right)$

u_co＝-k₁s+u_rcosψ_cocosθ_co(3)

其中增益因子k₁＞0，k₂＞0，k₃＞0为视线角导引律归一化参数，变量s、e和h分别表示机器人载体坐标系下机器人与期望路径参考点的前向、横向和垂向跟踪误差。

3.根据权利要求1所述的基于二阶滤波器的水下机器人三维路径跟踪控制方法，其特征在于，步骤3、4中所涉及的水下机器人三维路径跟踪运动学误差模型为：

$\left\{\begin{array}{c}\stackrel{\·}{s}=re-qh+u-u_r{\mathrm{cos\ψ}}_e{\mathrm{cos\θ}}_e\\ \stackrel{\·}{e}=-rs+u_r{\mathrm{sin\ψ}}_e{\mathrm{cos\θ}}_e+v\\ \stackrel{\·}{h}=qs-u_r{\mathrm{sin\θ}}_e+w\end{array}\right.---\left(4\right)$

$\left\{\begin{array}{c}{\stackrel{\·}{\mathrm{\ψ}}}_e=\frac{r}{cos\mathrm{\θ}}-r_F\\ {\stackrel{\·}{\mathrm{\θ}}}_e=q-q_F\end{array}\right.---\left(5\right)$

其中ψ_e＝ψ-ψ_F，θ_e＝θ-θ_F，机器人纵向速度u、横向速度v和垂向速度w，艏摇角速度r和纵倾角速度q，u_r为待设计期望路径上虚拟向导点的期望速度，其方向沿曲线路径的切线方向；ψ_F为u_r速度方向与固定坐标系水平轴的夹角，θ_F为u_r速度方向与固定坐标系垂直轴的夹角；ψ为机器人艏向角，θ为机器人纵倾角。

4.根据权利要求1所述的基于二阶滤波器的水下机器人三维路径跟踪控制方法，其特征在于，步骤5中涉及的水下机器人推进器推力F_u，与水平舵角δ_s、垂直舵角δ_r的表达式为：

$\left\{\begin{array}{c}{F}_u=m_1\left(-k_u\tilde{u}-k_{iu}{\mathrm{\ϵ}}_1+{\stackrel{\·}{u}}_c-u_{bs}\right)-f_u\\ {\mathrm{\δ}}_s=b_1^{-1}\[m_4\left(-k_q\stackrel{\‾}{q}-k_{iq}{\mathrm{\ϵ}}_2+{\stackrel{\·}{q}}_c-q_{bs}\right)-f_q\]\\ {\mathrm{\δ}}_r=b_2^{-1}\[m_5\left(-k_r\tilde{r}-k_{ir}{\mathrm{\ϵ}}_3+{\stackrel{\·}{r}}_c-r_{bs}\right)-f_r\]\end{array}\right.---\left(6\right)$

其中f_u、f_q和f_r为模型非线性水动力项；u_bs、q_bs和r_bs为反馈补偿鲁棒项；m₁、m₄、m₅分别为由流体产生的附加质量；k_u、k_q、k_r、k_iu、k_iq和k_ir均为控制器参数；

所涉及的水下机器人动力学模型为：

$\stackrel{\·}{u}=\frac{m_2}{m_1}vr-\frac{m_3}{m_1}wq-\frac{d_1}{m_1}u+\frac{1}{m_1}{F}_u+{\mathrm{\ω}}_1$

$\stackrel{\·}{v}=-\frac{m_1}{m_2}ur-\frac{d_2}{m_2}v+{\mathrm{\ω}}_2$

$\stackrel{\·}{w}=\frac{m_1}{m_3}uq-\frac{d_3}{m_3}w+g_1+{\mathrm{\ω}}_3---\left(7\right)$

$\stackrel{\·}{q}=\frac{m_3-m_1}{m_5}uw-\frac{d_4}{m_5}q-g_2+\frac{1}{m_5}{b}_1{\mathrm{\δ}}_s+{\mathrm{\ω}}_4$

$\stackrel{\·}{r}=\frac{m_1-m_2}{m_6}uv-\frac{d_5}{m_6}r+\frac{1}{m_6}{b}_2{\mathrm{\δ}}_r+{\mathrm{\ω}}_5$

其中

$m_1=m-X_{\stackrel{\·}{u}},m_2=m-Y_{\stackrel{\·}{v}},m_3=m-Z_{\stackrel{\·}{w}}$

$m_5=I_y-M_{\stackrel{\·}{q}},m_6=I_z-N_{\stackrel{\·}{r}}$

g₁＝(W-B)cosθ,g₂＝(z_gW-z_bB)sinθ

d₁＝X_u+X_u|u||u|,d₂＝Y_v+Y_v|v||v|

d₃＝Z_w+Z_w|w||w|,d₄＝M_q+M_q|q||q|

d₅＝N_r+N_r|r||r|

$b_1=u^2{M}_{{\mathrm{\δ}}_s},b_2=u^2{N}_{{\mathrm{\δ}}_r}$

其中，m和m_(·)分别表示机器人质量和由流体作用产生的附加质量，I_y为机器人绕y轴的转动惯量，I_z为机器人绕z轴的转动惯量，X_(·)、Y_(·)、Z_(·)、M_(·)和N_(·)为粘性流体水动力系数；z_g和z_b分别为载体坐标下垂直轴上重心和浮心的坐标位置，W和B分别表示机器人受到的重力和浮力，d_(·)为非线性阻尼水动力项，和为水平舵和垂直舵舵效系数，ω_(·)表示为干扰作用项。

5.根据权利要求1所述的基于二阶滤波器的水下机器人三维路径跟踪控制方法，其特征在于，步骤6中涉及的滤波误差补偿回路中误差补偿鲁棒项的表达式为：

${\mathrm{\ψ}}_{bs}=g^T\left(\widetilde{\mathrm{\ψ}}\right)B^T{u}_r\left[\begin{array}{c}{\mathrm{\υ}}_x\\ {\mathrm{\υ}}_y\\ {\mathrm{\υ}}_z\end{array}\right]---\left(8\right)$

${\mathrm{\θ}}_{bs}=g^T\left(\widetilde{\mathrm{\θ}}\right)C^T{u}_r\left[\begin{array}{c}{\mathrm{\υ}}_x\\ {\mathrm{\υ}}_y\\ {\mathrm{\υ}}_z\end{array}\right]---\left(9\right)$

$u_{bs}=\frac{1}{p_{21}}{A}^T\left[\begin{array}{c}{\mathrm{\υ}}_x\\ {\mathrm{\υ}}_y\\ {\mathrm{\υ}}_z\end{array}\right]---\left(10\right)$

$r_{bs}=\frac{1}{p_{23}}\frac{{\mathrm{\υ}}_{\mathrm{\ψ}}}{cos\mathrm{\θ}}---\left(11\right)$

$q_{bs}=\frac{1}{p_{22}}{\mathrm{\υ}}_{\mathrm{\θ}}---\left(12\right)$

其中，p₂₁、p₂₂、p₂₃为李雅普诺夫方程的解矩阵中的元素；

$A=\left[\begin{array}{c}1\\ 0\\ 0\end{array}\right],B=\left[\begin{array}{cc}{\mathrm{cos\θ}}_e{\mathrm{cos\ψ}}_c& -{\mathrm{cos\θ}}_e{\mathrm{sin\ψ}}_c\\ {\mathrm{cos\θ}}_c{\mathrm{sin\ψ}}_c& {\mathrm{cos\θ}}_c{\mathrm{cos\ψ}}_c\\ 0& 0\end{array}\right],C=\left[\begin{array}{cc}{\mathrm{cos\ψ}}_c{\mathrm{cos\θ}}_c& -{\mathrm{cos\ψ}}_c{\mathrm{sin\θ}}_c\\ {\mathrm{sin\ψ}}_e{\mathrm{cos\θ}}_c& -{\mathrm{sin\ψ}}_e{\mathrm{sin\θ}}_c\\ -{\mathrm{sin\θ}}_c& -{\mathrm{cos\θ}}_c\end{array}\right],$

$g\left(\widetilde{\mathrm{\ψ}}\right)=\left[\begin{array}{c}\frac{cos\widetilde{\mathrm{\ψ}}-1}{\widetilde{\mathrm{\ψ}}}\\ \frac{sin\widetilde{\mathrm{\ψ}}}{\widetilde{\mathrm{\ψ}}}\end{array}\right],g\left(\widetilde{\mathrm{\θ}}\right)=\left[\begin{array}{c}\frac{cos\widetilde{\mathrm{\θ}}-1}{\widetilde{\mathrm{\θ}}}\\ \frac{sin\widetilde{\mathrm{\θ}}}{\widetilde{\mathrm{\θ}}}\end{array}\right],$ 且满足 $\underset{\widetilde{\mathrm{\ψ}}\→0}{\lim }g\left(\widetilde{\mathrm{\ψ}}\right)=\left[\begin{array}{c}0\\ 1\end{array}\right],\underset{\widetilde{\mathrm{\θ}}\→0}{\lim }g\left(\widetilde{\mathrm{\θ}}\right)=\left[\begin{array}{c}0\\ 1\end{array}\right];$

所涉及的位置滤波信号补偿误差为

$\left[\begin{array}{c}{\mathrm{\υ}}_x\\ {\mathrm{\υ}}_y\\ {\mathrm{\υ}}_z\end{array}\right]=\left[\begin{array}{c}\tilde{s}-{\mathrm{\ζ}}_x\\ \tilde{e}-{\mathrm{\ζ}}_y\\ \tilde{h}-{\mathrm{\ζ}}_z\end{array}\right]---\left(13\right)$

式中，位置滤波补偿动态量ζ_x、ζ_y和ζ_z表达式为

$\left[\begin{array}{c}{\stackrel{\·}{\mathrm{\ζ}}}_x\\ {\stackrel{\·}{\mathrm{\ζ}}}_y\\ {\stackrel{\·}{\mathrm{\ζ}}}_z\end{array}\right]=\left[\begin{array}{c}{\mathrm{r\ζ}}_y-{\mathrm{q\ζ}}_z\\ -{\mathrm{r\ζ}}_x\\ {\mathrm{q\ζ}}_x\end{array}\right]\left[\begin{array}{c}-k_x{\mathrm{\ζ}}_x\\ -k_y{\mathrm{\ζ}}_y\\ -k_z{\mathrm{\ζ}}_z\end{array}\right]+\left[\begin{array}{c}{\stackrel{\·}{s}}_c-{\stackrel{\·}{s}}_{co}\\ {\stackrel{\·}{e}}_c-{\stackrel{\·}{e}}_{co}\\ {\stackrel{\·}{h}}_c-{\stackrel{\·}{h}}_{co}\end{array}\right]+u_r\left[\begin{array}{ccc}A& Bg\left(\widetilde{\mathrm{\ψ}}\right)& Cg\left(\widetilde{\mathrm{\θ}}\right)\end{array}\right]\left[\begin{array}{c}{\mathrm{\ζ}}_u\\ {\mathrm{\ζ}}_{\mathrm{\ψ}}\\ {\mathrm{\ζ}}_{\mathrm{\θ}}\end{array}\right]---\left(14\right)$

且有ζ_x(0)＝0、ζ_y(0)＝0、ζ_z(0)＝0；s_co、e_co、h_co为期望位置信号，s_c、e_c、h_c为位置控制信号；k_x＝k_y＝k_z＝2；

所涉及的滤波器输出的姿态信号补偿量为：

$\left[\begin{array}{c}{\mathrm{\υ}}_{\mathrm{\ψ}}\\ {\mathrm{\υ}}_{\mathrm{\θ}}\end{array}\right]=\left[\begin{array}{c}\widetilde{\mathrm{\ψ}}-{\mathrm{\ζ}}_{\mathrm{\ψ}}\\ \widetilde{\mathrm{\θ}}-{\mathrm{\ζ}}_{\mathrm{\θ}}\end{array}\right]---\left(15\right)$

式中，滤波姿态补偿动态表达式为：

${\stackrel{\·}{\mathrm{\ζ}}}_{\mathrm{\ψ}}=-k_{\mathrm{\ψ}}{\mathrm{\ζ}}_{\mathrm{\ψ}}+\frac{r_c-r_{co}}{cos\mathrm{\θ}}+\frac{{\mathrm{\ζ}}_r}{\cos \mathrm{\θ}}---\left(16\right)$

且有ζ_ψ(0)＝0，ζ_θ(0)＝0，ζ_r＝0，ζ_q＝0；k_ψ＝k_θ＝5。