CN103576693B

CN103576693B - Underwater robot three-dimensional path tracking and controlling method based on second order filter

Info

Publication number: CN103576693B
Application number: CN201310553699.XA
Authority: CN
Inventors: 王宏健; 陈子印; 边信黔; 李娟�; 严浙平; 陈兴华
Original assignee: Harbin Engineering University
Current assignee: Harbin Engineering University
Priority date: 2013-11-11
Filing date: 2013-11-11
Publication date: 2016-06-29
Anticipated expiration: 2033-11-11
Also published as: CN103576693A

Abstract

The present invention provides a three-dimensional path tracking control method for an underwater robot based on a second-order filter. The filter backstepping method is used to perform three-dimensional path tracking control of the underwater robot. By introducing two kinematic errors based on the three-dimensional path tracking of the underwater robot The second-order filter established by the model obtains the virtual control variables and their derivatives of attitude, velocity, and angular velocity, and then combines the underwater robot dynamics model to obtain the control input of the path tracking controller, which acts on the robot propeller and steering gear, and then realizes Track the three-dimensional path; design filter feedback compensation items for the position and attitude control loops according to the Lyapunov energy function, and introduce an integral link for the speed control loop to form a system error compensation loop to improve the accuracy of the tracking system.

Description

Three-dimensional path tracking control method for underwater robot based on second-order filter

技术领域technical field

本发明涉及的是一种水下机器人的运动控制方法，特别是涉及一种基于二阶滤波器的水下机器人三维路径跟踪控制方法。The invention relates to a motion control method of an underwater robot, in particular to a three-dimensional path tracking control method for an underwater robot based on a second-order filter.

背景技术Background technique

水下机器人的跟踪控制问题一直是国内外的研究热点。路径跟踪问题仅要求水下机器人的运动轨迹收敛到期望路径，而对何时到达何处并不作要求。路径跟踪技术通过引入期望路径上虚拟向导点的概念，避免了轨迹跟踪时由于水下机器人的动力学模型受到环境扰动作用，无法精确获得期望状态，导致跟踪控制系统性能变差的问题。路径跟踪相比于轨迹跟踪问题，由于期望位置不受时间条件约束，从而不易导致控制器输出饱和信号，符合工程实际，更具有实际应用价值。The tracking control of underwater robots has always been a research hotspot at home and abroad. The path tracking problem only requires the trajectory of the underwater robot to converge to the desired path, but does not require when and where it will arrive. By introducing the concept of virtual guide points on the desired path, the path tracking technology avoids the problem that the dynamic model of the underwater robot is disturbed by the environment, and the desired state cannot be accurately obtained during trajectory tracking, resulting in poor performance of the tracking control system. Compared with the trajectory tracking problem, the path tracking is not easy to cause the controller to output a saturated signal because the expected position is not constrained by the time condition, which is in line with the engineering reality and has more practical application value.

基于系统分层递推设计思想的反步法，为水下机器人三维跟踪控制系统的设计提供了有效的手段。然而，在反步法递推设计控制器的过程中，需要逐步计算中间虚拟控制量的导数，然后基于反馈线性化的思想通过设计后续子系统的等价输入，实现对前级子系统的镇定，通过不断迭代直到获得最终真实控制输入。当系统的阶数增加或虚拟控制的形式较为复杂时，求导过程将变得十分繁琐。为此本发明采用反馈增益反步法设计三维曲线路径跟踪控制器，通过合理选择控制器的参数消除部分非线性项，相比于传统反步法设计过程简化了虚拟控制量的形式，但仍需要逐步计算虚拟控制导数的解析形式。为克服常规反步法递推过程中对虚拟控制信号逐步求导的不足，文献《TheUseofSlidingModestoSimplifytheBacksteppingControl》和《Theuseoflinearfilteringofsimplifiedintegratorbacksteppingcontrolofnonlinearsystems》分别采用滑模滤波器和线性滤波器逼近虚拟控制的导数；文献《CommandFilteredBackstepping》提出基于滤波器设计的反步法轨迹跟踪控制(IEEETransactionsOnAutomaticControl.2009,第54卷第6期)，采用二阶滤波器逼近虚拟控制信号，简化了控制器设计过程，基于奇异扰动理论证明了滤波轨迹与常规反步法的控制输出之间的误差能够收敛到零点较小邻域。文献《LandVehicleControlUsingaCommandFilteredBacksteppingApproach》将滤波反步法应用于陆地车辆的轨迹跟踪控制中；文献《基于滤波反步法的无人直升机轨迹跟踪控制》(控制与决策.2012,第27卷第4期)和文献《状态受限的小型无人直升机轨迹跟踪控制》(控制理论与应用.2012,第29卷第6期)中公开的直升机的轨迹跟踪控制中，大大简化了控制器设计过程。目前还没有相关文献讨论基于滤波器的水下机器人三维路径跟踪控制设计。The backstepping method based on the system hierarchical recursive design idea provides an effective means for the design of the 3D tracking control system of the underwater robot. However, in the process of recursively designing the controller with the backstepping method, it is necessary to gradually calculate the derivative of the intermediate virtual control variable, and then based on the idea of feedback linearization, the equivalent input of the subsequent subsystem is designed to stabilize the previous subsystem. , through continuous iteration until the final real control input is obtained. When the order of the system increases or the form of virtual control is more complex, the derivation process will become very cumbersome. For this reason, the present invention adopts the feedback gain backstepping method to design the three-dimensional curve path tracking controller, and eliminates part of the nonlinear term by rationally selecting the parameters of the controller. Compared with the traditional backstepping method design process, the form of the virtual control quantity is simplified, but still The analytical form of the virtual control derivative needs to be computed step by step. In order to overcome the deficiency of gradually deriving the virtual control signal in the recursive process of the conventional backstepping method, the literature "The Use of Sliding Modes to Simplify the Backstepping Control" and "The use of linear filtering of simplified integrator backstepping control of nonlinear systems" respectively use sliding mode filter and linear filter to approximate the derivative of virtual control; the literature "CommandFilteredBackstepping" proposes a method based on Backstepping trajectory tracking control based on filter design (IEEETransactionsOnAutomaticControl.2009, Volume 54, Issue 6), using a second-order filter to approximate the virtual control signal, which simplifies the controller design process. The error between the control outputs of the backstepping method can converge to a smaller neighborhood of zero. The literature "LandVehicleControlUsingaCommandFilteredBacksteppingApproach" applies filter backstepping method to the trajectory tracking control of land vehicles; the literature "Unmanned Helicopter Trajectory Tracking Control Based on Filtered Backstepping Method" (Control and Decision. The trajectory tracking control of the helicopter disclosed in "Track Tracking Control of Small Unmanned Helicopter with State Constraints" (Control Theory and Application. 2012, Volume 29, Issue 6) greatly simplifies the controller design process. At present, there is no relevant literature discussing the design of filter-based 3D path-tracking control for underwater vehicles.

发明内容Contents of the invention

本发明提出一种过程简化、跟踪精度高的基于二阶滤波器的水下机器人三维路径跟踪控制方法。The invention proposes a three-dimensional path tracking control method for an underwater robot based on a second-order filter with simplified process and high tracking accuracy.

本发明的基于二阶滤波器的水下机器人三维路径跟踪控制方法的具体过程为：The specific process of the three-dimensional path tracking control method of the underwater robot based on the second-order filter of the present invention is:

步骤1.建立固定坐标系、机器人载体坐标系和Serret-Frenet(路径参考)坐标系，获取期望路径，水下机器人开始路径跟踪，完成两个二阶滤波器的初始化；Step 1. Establish fixed coordinate system, robot carrier coordinate system and Serret-Frenet (path reference) coordinate system, obtain desired path, underwater robot starts path tracking, and completes the initialization of two second-order filters;

步骤2.通过水下机器人搭载的定位声纳传感器、姿态传感器，采集水下机器人当前位置、姿态角、角速度和速度数据信息，并结合期望路径的方向与速度，根据视线角导引思想计算得到水下机器人理想的姿态控制量ψ_co、θ_co，和理想速度控制量u_co；Step 2. Through the positioning sonar sensor and attitude sensor carried by the underwater robot, collect the current position, attitude angle, angular velocity and speed data information of the underwater robot, and combine the direction and speed of the expected path, and calculate according to the guiding idea of the line of sight angle Ideal attitude control quantities ψ _co , θ _co , and ideal velocity control quantities u _co of the underwater robot;

所涉及的水下机器人理想的姿态控制量ψ_co、θ_co，和理想速度控制量u_co的计算表达式为：The calculation expressions of the ideal attitude control quantities ψ _co , θ _co , and ideal speed control quantities u _co of the underwater robot involved are:

${ψ ψ}_{c c o o} = = - - arcsin arcsin (({k k}_{22} e e / / \sqrt{11 + + {(({k k}_{22} e e))}^{22}})) - - - - - - ((11))$

${θ θ}_{c c o o} = = arcsin arcsin (({k k}_{33} h h / / \sqrt{11 + + {(({k k}_{33} h h))}^{22}})) - - - - - - ((22))$

u_co＝-k₁s+u_rcosψ_cocosθ_co(3)u _co ＝-k ₁ s+u _r cosψ _{co co} cosθ _co (3)

其中增益因子k₁＞0，k₂＞0，k₃＞0为视线角导引律归一化参数，变量s、e和h分别表示机器人载体坐标系下机器人与期望路径参考点的前向、横向和垂向跟踪误差。Among them, the gain factor k ₁ >0, k ₂ >0, k ₃ >0 is the normalization parameter of the line-of-sight angle guidance law, and the variables s, e, and h represent the forward direction of the robot and the reference point of the desired path in the robot carrier coordinate system, respectively. , horizontal and vertical tracking errors.

步骤3.将步骤2中得到的理想控制量ψ_co、θ_co、u_co输入至基于水下机器人三维路径跟踪运动学误差模型所建立的二阶滤波器，得到水下机器人的姿态与速度控制量ψ_c、θ_c、u_c，及其导数结合机器人运动变量ψ、θ、u，得到滤波姿态与速度跟踪误差量与理想角速度控制量r_co、q_co；Step 3. Input the ideal control quantities ψ _co , θ _co , and u _co obtained in step 2 to the second-order filter established based on the 3D path tracking kinematic error model of the underwater robot to obtain the attitude and speed control of the underwater robot Quantities ψ _c , θ _c , u _c , and their derivatives Combining the robot motion variables ψ, θ, u to obtain the filter attitude and speed tracking error and ideal angular velocity control quantities r _co , q _co ;

所涉及的水下机器人三维路径跟踪运动学误差模型为：The kinematic error model of the 3D path tracking of the underwater robot involved is:

$\{\begin{matrix} \overset{\cdot &Center Dot;}{s the s} = = r r e e - - q q h h + + u u - - {u u}_{r r} {cosψ cosψ}_{e e} {cosθ cosθ}_{e e} \\ \overset{\cdot \cdot}{e e} = = - - r r s the s + + {u u}_{r r} {sinψ sinψ}_{e e} {cosθ cosθ}_{e e} + + v v \\ \overset{\cdot \cdot}{h h} = = q q s the s - - {u u}_{r r} {sinθ sinθ}_{e e} + + w w \end{matrix} - - - - - - ((44))$

$\{\begin{matrix} {\overset{\cdot &Center Dot;}{ψ ψ}}_{e e} = = \frac{r r}{c c o o s the s θ θ} - - {r r}_{F f} \\ {\overset{\cdot \cdot}{θ θ}}_{e e} = = q q - - {q q}_{F f} \end{matrix} - - - - - - ((55))$

其中ψ_e＝ψ-ψ_F，θ_e＝θ-θ_F，机器人纵向速度u、横向速度v和垂向速度w，艏摇角速度r和纵倾角速度q，u_r为待设计期望路径上虚拟向导点的期望速度，其方向沿曲线路径的切线方向；ψ_F为u_r速度方向与固定坐标系水平轴的夹角，θ_F为u_r速度方向与固定坐标系垂直轴的夹角；ψ为机器人艏向角，θ为机器人纵倾角。where ψ _e = ψ-ψ _F , θ _e = θ-θ _F , Robot longitudinal velocity u, lateral velocity v and vertical velocity w, yaw angular velocity r and pitch angular velocity q, u _r is the expected velocity of the virtual guide point on the desired path to be designed, and its direction is along the tangent direction of the curved path; ψ _F is the angle between the u _r velocity direction and the horizontal axis of the fixed coordinate system, θ _F is the angle between the u _r velocity direction and the vertical axis of the fixed coordinate system; ψ is the heading angle of the robot, and θ is the pitch angle of the robot.

步骤4.将步骤3中得到的水下机器人理想角速度控制量r_co、q_co输入至另一个基于水下机器人三维路径跟踪运动学误差模型所建立的二阶滤波器，得到水下机器人的角速度控制量r_c、q_c，及其导数结合机器人角运动变量r、q，得到滤波角速度跟踪误差量 Step 4. Input the ideal angular velocity control quantities r _co and q _co of the underwater robot obtained in step 3 to another second-order filter established based on the three-dimensional path tracking kinematic error model of the underwater robot to obtain the angular velocity of the underwater robot Controlled quantities r _c , q _c , and their derivatives Combined with the robot's angular motion variables r and q, the filtered angular velocity tracking error amount is obtained

步骤5.利用步骤3中得到的滤波姿态与速度跟踪误差量以及步骤4中得到的滤波角速度跟踪误差量解算得到水下机器人推进器推力F_u，与水平舵角δ_s、垂直舵角δ_r，分别作用于机器人推进器及舵机，实现三维路径跟踪控制；Step 5. Use the filtered attitude and velocity tracking error amount obtained in step 3 and the filtered angular velocity tracking error obtained in step 4 The thrust F _u of the propeller of the underwater robot is obtained through calculation, and the horizontal rudder angle δ _s and the vertical rudder angle δ _r act on the propeller and steering gear of the robot respectively to realize three-dimensional path tracking control;

所涉及的水下机器人推进器推力F_u，与水平舵角δ_s、垂直舵角δ_r的表达式为：The expressions of the propeller thrust Fu of the underwater _robot involved, the horizontal rudder angle δ _s and the vertical rudder angle δ _r are:

$\{\begin{matrix} {F f}_{u u} = = {m m}_{11} ((- - {k k}_{u u} \overset{~ ~}{u u} - - {k k}_{i i u u} {ϵ ϵ}_{11} + + {\overset{\cdot \cdot}{u u}}_{c c} - - {u u}_{b b s the s})) - - {f f}_{u u} \\ {δ δ}_{s the s} = = {b b}_{11}^{- - 11} [[{m m}_{44} ((- - {k k}_{q q} \overset{~ ~}{q q} - - {k k}_{i i q q} {ϵ ϵ}_{22} + + {\overset{\cdot \cdot}{q q}}_{c c} - - {q q}_{b b s the s})) - - {f f}_{q q}]] \\ {δ δ}_{r r} = = {b b}_{22}^{- - 11} [[{m m}_{55} ((- - {k k}_{r r} \overset{~ ~}{r r} - - {k k}_{i i r r} {ϵ ϵ}_{33} + + {\overset{\cdot \cdot}{r r}}_{c c} - - {r r}_{b b s the s})) - - {f f}_{r r}]] \end{matrix} - - - - - - ((66))$

其中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均为控制器参数；in f _u , f _q and f _r are nonlinear hydrodynamic items of the model; u _bs , q _bs and r _bs are feedback compensation robust items; m ₁ , m ₄ , and m ₅ are the additional mass produced by the fluid; k _u , k _q , k _r , k _iu , k _iq and k _ir are all controller parameters;

所涉及的水下机器人动力学模型为：The dynamic model of the underwater robot involved is:

$\begin{matrix} \overset{\cdot \cdot}{u u} = = \frac{{m m}_{22}}{{m m}_{11}} v v r r - - \frac{{m m}_{33}}{{m m}_{11}} w w q q - - \frac{{d d}_{11}}{{m m}_{11}} u u + + \frac{11}{{m m}_{11}} {F f}_{u u} + + {ω ω}_{11} \\ \overset{\cdot \cdot}{v v} = = - - \frac{{m m}_{11}}{{m m}_{22}} v v r r - - \frac{{d d}_{22}}{{m m}_{22}} v v + + {ω ω}_{22} \\ \overset{\cdot \cdot}{w w} = = \frac{{m m}_{11}}{{m m}_{33}} u u q q - - \frac{{d d}_{33}}{{m m}_{33}} w w + + {g g}_{11} + + {ω ω}_{33} \\ \overset{\cdot \cdot}{q q} = = \frac{{m m}_{33} - - {m m}_{11}}{{m m}_{55}} u u w w - - \frac{{d d}_{44}}{{m m}_{55}} q q - - {g g}_{22} + + \frac{11}{{m m}_{55}} {b b}_{11} {δ δ}_{s the s} + + {ω ω}_{44} \\ \overset{\cdot \cdot}{r r} = = \frac{{m m}_{11} - - {m m}_{22}}{{m m}_{66}} u u v v - - \frac{{d d}_{55}}{{m m}_{66}} r r + + \frac{11}{{m m}_{66}} {b b}_{22} {δ δ}_{r r} + + {ω ω}_{55} \end{matrix} - - - - - - ((77))$

其中in

${m m}_{11} = = m m - - {X x}_{\overset{\cdot &Center Dot;}{u u}},, {m m}_{22} = = m m - - {Y Y}_{\overset{\cdot &Center Dot;}{v v}},, {m m}_{33} = = m m - - {Z Z}_{\overset{\cdot &Center Dot;}{w w}}$

${m m}_{55} = = {I I}_{y the y} - - {M m}_{\overset{\cdot &Center Dot;}{q q}},, {m m}_{66} = = {I I}_{z z} - - {N N}_{\overset{\cdot &Center Dot;}{r r}}$

g₁＝(W-B)cosθ,g₂＝(z_gW-z_bB)sinθg ₁ ＝(WB)cosθ, g ₂ ＝(z _g Wz _b B)sinθ

d₁＝X_u+X_u|u_||u|,d₂＝Y_v+Y_v|v||v|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 ₃ ＝Z _w +Z _w|w| |w|, d ₄ ＝M _q +M _q|q| |q|

d₅＝N_r+N_r|r||r|d ₅ ＝N _r +N _r|r| |r|

${b b}_{11} = = {u u}^{22} {M m}_{{δ δ}_{s the s}},, {b b}_{22} = = {u u}^{22} {N N}_{{δ δ}_{r r}}$

其中，m和m_(·)分别表示机器人质量和由流体作用产生的附加质量，I_y为机器人绕y轴的转动惯量，I_z为机器人绕z轴的转动惯量，X_(·)、Y_(·)、Z_(·)、M_(·)和N_(·)为粘性流体水动力系数；z_g和z_b分别为载体坐标下垂直轴上重心和浮心的坐标位置，W和B分别表示机器人受到的重力和浮力，d_(·)为非线性阻尼水动力项，和为水平舵和垂直舵舵效系数，ω_(·)表示为干扰作用项。Among them, m and m _{( )} respectively represent the mass of the robot and the additional mass produced by the fluid action, I _y is the moment of inertia of the robot around the y-axis, I _z is the moment of inertia of the robot around the z-axis, X _{( )} , Y _{( )} , Z _(·) , M _(·) and N _(·) are the hydrodynamic coefficients of the viscous fluid; z _g and z _b are the coordinate positions of the center of gravity and the center of buoyancy on the vertical axis under the carrier coordinates, and W and B respectively represent The gravity and buoyancy of the robot, d _(·) is the nonlinear damping hydrodynamic term, and is the rudder effect coefficient of the horizontal rudder and the vertical rudder, and ω _(·) is expressed as the disturbance action item.

步骤6.利用步骤3中得到的水下机器人姿态与速度控制量ψ_c、θ_c、u_c，滤波姿态与速度跟踪误差量与理想角速度控制量r_co、q_co，结合步骤4中得到的水下机器人的角速度控制量r_c、q_c，以及滤波角速度跟踪误差量构造滤波误差补偿回路；Step 6. Using the underwater robot attitude and velocity control quantities ψ _c , θ _c , u _c obtained in step 3, filter the attitude and velocity tracking error Combined with the ideal angular velocity control quantities r _co , q _co , the angular velocity control quantities r _c , q _c of the underwater robot obtained in step 4, and the filtered angular velocity tracking error Construct a filter error compensation loop;

所涉及的滤波误差补偿回路中误差补偿鲁棒项的表达式为：The expression of the error compensation robust term in the filter error compensation loop involved is:

${ψ ψ}_{b b s the s} = = {g g}^{T T} ((\overset{~ ~}{ψ ψ})) {B B}^{T T} {u u}_{r r} [\begin{matrix} {&upsi; &upsi;}_{x x} \\ {&upsi; &upsi;}_{y the y} \\ {&upsi; &upsi;}_{z z} \end{matrix}] - - - - - - ((88))$

${θ θ}_{b b s the s} = = {g g}^{T T} ((\overset{~ ~}{θ θ})) {C C}^{T T} {u u}_{r r} [\begin{matrix} {&upsi; &upsi;}_{x x} \\ {&upsi; &upsi;}_{y the y} \\ {&upsi; &upsi;}_{z z} \end{matrix}] - - - - - - ((99))$

${u u}_{b b s the s} = = \frac{11}{{p p}_{21 twenty one}} {A A}^{T T} [\begin{matrix} {&upsi; &upsi;}_{x x} \\ {&upsi; &upsi;}_{y the y} \\ {&upsi; &upsi;}_{z z} \end{matrix}] - - - - - - ((1010))$

${r r}_{b b s the s} = = \frac{11}{{p p}_{23 twenty three}} \frac{{&upsi; &upsi;}_{ψ ψ}}{c c o o s the s θ θ} - - - - - - ((1111))$

${q q}_{b b s the s} = = \frac{11}{{p p}_{22 twenty two}} &upsi; &upsi; θ θ - - - - - - ((1212))$

其中，p₂₁、p₂₂、p₂₃为李雅普诺夫方程的解矩阵中的元素；Among them, p ₂₁ , p ₂₂ , and p ₂₃ are elements in the solution matrix of the Lyapunov equation;

$A A = = [\begin{matrix} 11 \\ 00 \\ 00 \end{matrix}],, B B = = [\begin{matrix} {cosθ cosθ}_{e e} {cosψ cosψ}_{c c} & - - {cosθ cosθ}_{e e} {sinψ sinψ}_{c c} \\ {cosθ cosθ}_{c c} {sinψ sinψ}_{c c} & {cosθ cosθ}_{c c} {cosψ cosψ}_{c c} \\ 00 & 00 \end{matrix}],, C C = = [\begin{matrix} {cosψ cosψ}_{e e} {cosθ cosθ}_{c c} & - - {cosψ cosψ}_{e e} {sinθ sinθ}_{c c} \\ {sinψ sinψ}_{e e} {cosθ cosθ}_{c c} & - - {sinψ sinψ}_{e e} {sinθ sinθ}_{c c} \\ - - {sinθ sinθ}_{c c} & - - {cosθ cosθ}_{c c} \end{matrix}],,$

$g (\tilde{ψ}) = [\begin{matrix} \frac{c o s \tilde{ψ} - 1}{\tilde{ψ}} \\ \frac{s i n \tilde{ψ}}{\tilde{ψ}} \end{matrix}], g (\tilde{θ}) = [\begin{matrix} \frac{c o s \tilde{θ} - 1}{\tilde{θ}} \\ \frac{s i n \tilde{θ}}{\tilde{θ}} \end{matrix}],$ 且满足 $\lim_{\tilde{ψ} &RightArrow; 0} g (\tilde{ψ}) = [\begin{matrix} 0 \\ 1 \end{matrix}], \lim_{\tilde{θ} &RightArrow; 0} g (\tilde{θ}) = [\begin{matrix} 0 \\ 1 \end{matrix}];$ $g (\tilde{ψ}) = [\begin{matrix} \frac{c o the s \tilde{ψ} - 1}{\tilde{ψ}} \\ \frac{the s i no \tilde{ψ}}{\tilde{ψ}} \end{matrix}], g (\tilde{θ}) = [\begin{matrix} \frac{c o the s \tilde{θ} - 1}{\tilde{θ}} \\ \frac{the s i no \tilde{θ}}{\tilde{θ}} \end{matrix}],$ and satisfied $\lim_{\tilde{ψ} &Right Arrow; 0} g (\tilde{ψ}) = [\begin{matrix} 0 \\ 1 \end{matrix}], \lim_{\tilde{θ} &Right Arrow; 0} g (\tilde{θ}) = [\begin{matrix} 0 \\ 1 \end{matrix}];$

位置滤波信号补偿误差为The position filter signal compensation error is

$[\begin{matrix} {&upsi; &upsi;}_{x x} \\ {&upsi; &upsi;}_{y the y} \\ {&upsi; &upsi;}_{z z} \end{matrix}] = = [\begin{matrix} \overset{~ ~}{s the s} - - {ζ ζ}_{x x} \\ \overset{~ ~}{s the s} - - {ζ ζ}_{y the y} \\ \overset{~ ~}{h h} - - {ζ ζ}_{z z} \end{matrix}] - - - - - - ((1313))$

其中位置滤波补偿动态量ζ_x、ζ_y和ζ_z表达式为Among them, the position filter compensation dynamic quantities ζ _x , ζ _y and ζ _z are expressed as

$[\begin{matrix} {\overset{\cdot &Center Dot;}{ζ ζ}}_{x x} \\ {\overset{\cdot \cdot}{ζ ζ}}_{y the y} \\ {\overset{\cdot &Center Dot;}{ζ ζ}}_{z z} \end{matrix}] = = [\begin{matrix} {rζ rζ}_{y the y} - - {qζ qζ}_{z z} \\ - - {rζ rζ}_{x x} \\ {qζ qζ}_{x x} \end{matrix}] [\begin{matrix} - - {k k}_{x x} {ζ ζ}_{x x} \\ - - {k k}_{y the y} {ζ ζ}_{y the y} \\ - - {k k}_{z z} {ζ ζ}_{z z} \end{matrix}] + + [\begin{matrix} {\overset{\cdot &Center Dot;}{s the s}}_{c c} - - {\overset{\cdot &Center Dot;}{s the s}}_{c c o o} \\ {\overset{\cdot &Center Dot;}{e e}}_{c c} - - {\overset{\cdot &Center Dot;}{e e}}_{c c o o} \\ {\overset{\cdot &Center Dot;}{h h}}_{c c} - - {\overset{\cdot &Center Dot;}{h h}}_{c c o o} \end{matrix}] + + {u u}_{r r} [[\begin{matrix} A A & B B g g ((\overset{~ ~}{ψ ψ})) & C C g g ((\overset{~ ~}{θ θ})) \end{matrix}]] [\begin{matrix} {ζ ζ}_{u u} \\ {ζ ζ}_{ψ ψ} \\ {ζ ζ}_{θ θ} \end{matrix}] - - - - - - ((1414))$

且有ζ_x(0)＝0、ζ_y(0)＝0、ζ_z(0)＝0；s_co、e_co、h_co为期望位置信号，s_c、e_c、h_c为位置控制信号，ζ_u、ζ_ψ和ζ_θ分别为构造的滤波补偿变量；And there are ζ _x (0) = 0, ζ _y (0) = 0, ζ _z (0) = 0; s _co , e _co , h _co are expected position signals, _{sc , e c} _, h _c are position control Signal, ζ _u , ζ _ψ and ζ _θ are constructed filter compensation variables respectively;

滤波器输出的姿态信号补偿量为：The attitude signal compensation amount output by the filter is:

$[\begin{matrix} {&upsi; &upsi;}_{ψ ψ} \\ {&upsi; &upsi;}_{θ θ} \end{matrix}] = = [\begin{matrix} \overset{~ ~}{ψ ψ} - - {ζ ζ}_{ψ ψ} \\ \overset{~ ~}{θ θ} - - {ζ ζ}_{θ θ} \end{matrix}] - - - - - - ((1515))$

滤波姿态补偿动态表达式为：The dynamic expression of filter attitude compensation is:

${\overset{\cdot &Center Dot;}{ζ ζ}}_{ψ ψ} = = - - {k k}_{ψ ψ} {ζ ζ}_{ψ ψ} + + \frac{{r r}_{c c} - - {r r}_{c c o o}}{c c o o s the s θ θ} + + \frac{{ζ ζ}_{r r}}{cos cos θ θ} - - - - - - ((1616))$

${\overset{\cdot \cdot}{ζ ζ}}_{θ θ} = = - - {k k}_{θ θ} {ζ ζ}_{θ θ} + + (({q q}_{c c} - - {q q}_{c c o o})) + + {ζ ζ}_{q q} - - - - - - ((1717))$

且有ζ_ψ(0)＝0，ζ_θ(0)＝0，ζ_r＝0，ζ_q＝0。And ζ _ψ (0)=0, ζ _θ (0)=0, ζ _r =0, ζ _q =0.

步骤7.计算当前水下机器人位置ηⁿ＝(x,y,z)与标定的转向点WP_k＝(x_k,y_k,z_k)之间的距离若小于设定的切换半径R，则表示完成当前指定路径的跟踪任务，否则继续步骤2。Step 7. Calculate the distance between the current underwater robot position η ⁿ =(x,y,z) and the calibrated turning point WP _k =(x _k ,y _k ,z _k ) If it is smaller than the set switching radius R, it means that the tracking task of the currently specified path is completed, otherwise continue to step 2.

本发明采用滤波反步法进行水下机器人三维路径跟踪控制，通过设计二阶滤波器，能够实现对虚拟控制和其导数信号的估计，避免了对虚拟信号的解析求导，引入滤波补偿系统保证滤波信号的跟踪精度，基于李雅普诺夫稳定性理论保证了系统跟踪误差收敛于零点，控制性能优于动态面控制。The present invention adopts the filter backstepping method to carry out the three-dimensional path tracking control of the underwater robot. By designing a second-order filter, the estimation of the virtual control and its derivative signal can be realized, and the analytical derivation of the virtual signal is avoided, and the filter compensation system is introduced to ensure The tracking accuracy of the filtered signal is based on the Lyapunov stability theory to ensure that the system tracking error converges to zero, and the control performance is better than the dynamic surface control.

本发明具有如下优点及效果：The present invention has following advantage and effect:

1.将三维路径跟踪控制问题，分解为对位置、姿态和速度回路分别控制的问题；1. Decompose the three-dimensional path tracking control problem into separate control problems for position, attitude and velocity loops;

2.采用二阶滤波器获得虚拟控制的滤波信号及其导数形式，避免了反步法设计中由于需要逐步计算中间虚拟控制的导数形式而导致“项数膨胀”的问题，简化了控制器设计过程；2. The second-order filter is used to obtain the filtered signal and its derivative form of the virtual control, which avoids the problem of "term expansion" caused by the need to gradually calculate the derivative form of the intermediate virtual control in the design of the backstepping method, and simplifies the controller design. process;

3.通过构造滤波补偿系统，保证了滤波器对参考输入信号的跟踪精度，实现了系统跟踪误差渐近收敛到零点；3. By constructing the filter compensation system, the tracking accuracy of the filter to the reference input signal is guaranteed, and the system tracking error is asymptotically converged to zero;

4.引入速度的积分环节消除跟踪信号稳态误差。4. Introduce the integral link of speed to eliminate the steady-state error of tracking signal.

附图说明Description of drawings

图1中给出基于滤波反步法的AUV三维路径跟踪控制器框图；Figure 1 shows the block diagram of the AUV three-dimensional path tracking controller based on the filtering backstepping method;

图2机器人三维路径跟踪示意图；Fig. 2 Schematic diagram of three-dimensional path tracking of the robot;

图3二阶滤波器结构图；Fig. 3 second-order filter structure diagram;

图4机器人三维路径跟踪轨迹；Fig. 4 robot three-dimensional path tracking trajectory;

图5机器人三维路径跟踪XY平面投影；Figure 5 Robot 3D path tracking XY plane projection;

图6机器人三维路径跟踪XZ平面投影；Figure 6 Robot 3D path tracking XZ plane projection;

图7机器人三维路径跟踪误差曲线；Fig. 7 Robot 3D path tracking error curve;

图8机器人三维路径跟踪速度响应；Fig. 8 The robot's three-dimensional path tracking velocity response;

图9机器人三维路径跟踪姿态角响应；Fig. 9 The robot's three-dimensional path tracking attitude angle response;

图10机器人三维路径跟踪纵向速度及补偿项；Figure 10 Robot 3D path tracking longitudinal velocity and compensation items;

图11机器人三维路径跟踪艏摇角速度及补偿项；Fig. 11 Robot three-dimensional path tracking yaw angular velocity and compensation items;

图12机器人三维路径跟踪纵倾角速度及补偿项；Fig. 12 The three-dimensional path tracking pitch angular velocity and compensation items of the robot;

图13机器人三维路径跟踪艏向角及补偿项；Figure 13 Robot 3D path tracking heading angle and compensation items;

图14机器人三维路径跟踪纵倾角及补偿项；Fig. 14 Robot 3D path tracking pitch angle and compensation items;

图15机器人三维路径跟踪控制输入。Figure 15 Robot 3D path tracking control input.

具体实施方式detailed description

本发明内容基于二阶滤波器的水下机器人三维路径跟踪控制方法的具体实施方式如下：The specific implementation of the three-dimensional path tracking control method of the underwater robot based on the second-order filter is as follows:

1.图1中标出了AUV系统的状态信号、虚拟控制量和滤波信号及其导数值的相互关系，前向回路主要通过二阶滤波器获得虚拟控制的滤波信号和导数信号，通过设计滤波误差补偿回路保证滤波器对输入信号的跟踪精度。1. Figure 1 shows the relationship between the state signal of the AUV system, the virtual control quantity, the filter signal and its derivative value. The forward loop mainly obtains the filter signal and derivative signal of the virtual control through the second-order filter. By designing the filter error The compensation loop ensures the tracking accuracy of the filter to the input signal.

2.图3为机器人三维路径跟踪示意图，l_k为期望路径，{I}、{B}和{F}分别表示固定坐标系、机器人载体坐标系和Serret-Frenet坐标系；P点为期望路径l_k上的虚拟向导，Q点表示机器人质心位置，对于给定期望路径l_k为为确定的路径参数，以l_k上虚拟向导P的为原点的移动坐标系{F}定义为将坐标系{I}分别绕ζ轴和η旋转ψ_F和θ_F角度，然后平移使固定坐标系原点O与路径上P点重合得到，这里旋转角度定义为2. Figure 3 is a schematic diagram of the three-dimensional path tracking of the robot, l _k is the expected path, {I}, {B} and {F} represent the fixed coordinate system, the robot carrier coordinate system and the Serret-Frenet coordinate system respectively; point P is the expected path The virtual guide on l _k , point Q represents the position of the robot’s center of mass, for a given desired path l _k is To determine the path parameters, the moving coordinate system {F} with the origin of the virtual guide P on l _k is defined as rotating the coordinate system {I} around the ζ axis and η by angles ψ _F and θ _F respectively, and then translating to make the fixed coordinate It is obtained by coincidence of origin O and point P on the path, where the rotation angle is defined as

其中首先给出水下机器人三维路径跟踪运动学误差模型in Firstly, the 3D path tracking kinematics error model of the underwater robot is given

$\{\begin{matrix} \overset{\cdot \cdot}{s the s} = = r r e e - - q q h h + + u u - - {u u}_{r r} c c o o s the s {ψ ψ}_{e e} c c o o s the s {θ θ}_{e e} \\ \overset{\cdot \cdot}{e e} = = - - r r s the s + + {u u}_{r r} {sinψ sinψ}_{e e} {cosθ cosθ}_{e e} + + v v \\ \overset{\cdot \cdot}{h h} = = q q s the s - - {u u}_{r r} s the s i i n no {θ θ}_{e e} + + w w \end{matrix}$

$\{\begin{matrix} {\overset{\cdot &Center Dot;}{ψ ψ}}_{e e} = = \frac{r r}{c c o o s the s θ θ} - - {r r}_{F f} \\ {\overset{\cdot \cdot}{θ θ}}_{e e} = = q q - - {q q}_{F f} \end{matrix} - - - - - - ((44))$

其中变量s、e和h分别表示{B}坐标系下机器人与期望路径上参考点的前向、横向和垂向跟踪误差，ψ和θ为机器人当前艏向角和纵倾角，状态变量u，v，w，q和r分别表示载体坐标系下机器人的纵向速度、横向速度、垂向速度、纵倾角速度和艏摇角速度；u_r为虚拟目标移动速度，其中 The variables s, e, and h represent the forward, lateral, and vertical tracking errors between the robot and the reference point on the desired path in the {B} coordinate system, respectively, ψ and θ are the current heading and pitch angles of the robot, and the state variable u, v, w, q and r respectively represent the longitudinal velocity, lateral velocity, vertical velocity, pitch angular velocity and yaw angular velocity of the robot in the carrier coordinate system; u _r is the moving velocity of the virtual target, where

忽略横摇对机器人三维运动的影响，建立如下五自由度机器人动力学模型。Neglecting the influence of rolling on the three-dimensional motion of the robot, the following five-degree-of-freedom robot dynamics model is established.

$\begin{matrix} \overset{\cdot &Center Dot;}{u u} = = \frac{{m m}_{22}}{{m m}_{11}} v v r r - - \frac{{m m}_{33}}{{m m}_{11}} w w q q - - \frac{{d d}_{11}}{{m m}_{11}} u u + + \frac{11}{{m m}_{11}} {F f}_{u u} + + {ω ω}_{11} \\ \overset{\cdot &Center Dot;}{v v} = = - - \frac{{m m}_{11}}{{m m}_{22}} ur ur - - \frac{{d d}_{22}}{{m m}_{22}} v v + + {ω ω}_{22} \\ \overset{\cdot &Center Dot;}{w w} = = \frac{{m m}_{11}}{{m m}_{33}} u u q q - - \frac{{d d}_{33}}{{m m}_{33}} w w + + {g g}_{11} + + {ω ω}_{33} \\ \overset{\cdot &Center Dot;}{q q} = = \frac{{m m}_{33} - - {m m}_{11}}{{m m}_{55}} u u w w - - \frac{{d d}_{44}}{{m m}_{55}} q q - - {g g}_{22} + + \frac{11}{{m m}_{55}} {b b}_{11} {δ δ}_{s the s} + + {ω ω}_{44} \\ \overset{\cdot &Center Dot;}{r r} = = \frac{{m m}_{11} - - {m m}_{22}}{{m m}_{66}} u u v v - - \frac{{d d}_{55}}{{m m}_{66}} r r + + \frac{11}{{m m}_{66}} {b b}_{22} {δ δ}_{r r} + + {ω ω}_{55} \end{matrix} - - - - - - ((55))$

其中in

${m m}_{11} = = m m - - {X x}_{\overset{\cdot \cdot}{u u}},, {m m}_{22} = = m m - - {Y Y}_{\overset{\cdot &Center Dot;}{v v}},, {m m}_{33} = = m m - - {Z Z}_{\overset{\cdot &Center Dot;}{w w}}$

${m m}_{55} = = {I I}_{y the y} - - {M m}_{\overset{\cdot &Center Dot;}{q q}},, {m m}_{66} = = {I I}_{z z} - - {N N}_{\overset{\cdot \cdot}{r r}}$

d₁＝X_u+X_u|u||u|,d₂＝Y_v+Y_v|v||v|d ₁ ＝X _u +X _u|u| |u|, d ₂ ＝Y _v +Y _v|v| |v|

d₅＝N_r+N_r|r||r|d ₅ ＝N _r +N _r|r| |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推进器推力、水平舵角和垂直舵角，ω_(·)表示为干扰作用项。Among them, the state variables u, v, w, q and r respectively represent the longitudinal velocity, lateral velocity, vertical velocity, pitch angular velocity and yaw angular velocity of the robot in the carrier coordinate system; m and m _{( )} represent the mass and The additional mass produced by the action of fluid, I _y is the moment of inertia of the robot around the y-axis, I _z is the moment of inertia of the robot around the z-axis, X _{( )} , Y _{( )} , Z _{( )} , M _{( )} and N _(·) is the hydrodynamic coefficient of the viscous fluid; z _g and z _b are the coordinate positions of the center of gravity and buoyancy on the vertical axis under the carrier coordinates, W and B represent the gravity and buoyancy of the robot respectively, and d _{(·) is} the non- The linearly damped hydrodynamic term, and are the rudder effect coefficients of the horizontal rudder and the vertical rudder, the control inputs F _u , δ _s and δ _r represent the thrust of the AUV propeller, the horizontal rudder angle and the vertical rudder angle, respectively, and ω _(·) represents the interference term.

3.步骤3中滤波器的设计过程为3. The filter design process in step 3 is

为了避免对虚拟控制量直接解析求导而引入复杂的计算过程，利用二阶滤波器的特性，将虚拟控制量作为滤波器的参考输入，通过积分而非微分的过程获得其滤波信号和导数值，滤波器结构定义如下：In order to avoid the introduction of complicated calculation process for the direct analytical derivation of the virtual control quantity, the characteristics of the second-order filter are used, the virtual control quantity is used as the reference input of the filter, and the filtered signal and derivative value are obtained through the process of integration rather than differentiation , the filter structure is defined as follows:

$[\begin{matrix} {x x}_{11} \\ {\overset{\cdot &Center Dot;}{x x}}_{22} \end{matrix}] = = [\begin{matrix} 00 & 11 \\ - - {ω ω}_{n no}^{22} & - - 22 {ζω ζω}_{n no} \end{matrix}] [\begin{matrix} {x x}_{11} \\ {x x}_{22} \end{matrix}] + + [\begin{matrix} 00 \\ {ω ω}_{n no}^{22} \end{matrix}] \overset{&OverBar; &OverBar;}{x x} - - - - - - ((66))$

其中 ${[\begin{matrix} x_{1} & x_{2} \end{matrix}]}^{T} = {[\begin{matrix} x_{c} & {\overset{\cdot}{x}}_{c} \end{matrix}]}^{T},$ 为滤波器的参考输入信号，上式为线性稳定系统，可见当为有界值，则x_c和均为连续有界信号，从输入信号到输出信号x_c的传递函数为in ${[\begin{matrix} x_{1} & x_{2} \end{matrix}]}^{T} = {[\begin{matrix} x_{c} & {\overset{&Center Dot;}{x}}_{c} \end{matrix}]}^{T},$ is the reference input signal of the filter, the above formula is a linear stable system, it can be seen that when is a bounded value, then x _c and Both are continuous bounded signals, from the input signal The transfer function to the output signal x _c is

$H h ((s the s)) = = \frac{{X x}_{c c} ((s the s))}{\overset{&OverBar; &OverBar;}{X x} ((s the s))} = = \frac{{ω ω}_{n no}^{22}}{{s the s}^{22} + + 22 {ζω ζω}_{n no} s the s + + {ω ω}_{n no}^{22}} - - - - - - ((77))$

其中ζ和ω_n分别表示阻尼比和自然角频率；如果信号的带宽低于H(s)，那么误差信号将会很小，假设在已知带宽的情况下，通过选择足够大的自然角频率ω_n就能够获得x_c和x_c且保证逼近误差很小。从上式可以看出，信号是通过积分过程而非微分过程得到的，这可以大大减少基于状态反馈设计的控制系统中测量噪声的影响，同时选择过大的ω_n又会增加系统高频噪声的影响，这就需要综合考虑，选择合理的ω_n满足控制性能。where ζ and ω _n denote the damping ratio and natural angular frequency, respectively; if the signal bandwidth is lower than H(s), then the error signal will be small, assuming that the known In the case of wide bandwidth, _xc and _xc can be obtained by selecting a sufficiently large natural angular frequency _ωn and the approximation error is guaranteed very small. From the above formula, it can be seen that the signal It is obtained through the integral process rather than the differential process, which can greatly reduce the influence of measurement noise in the control system based on state feedback design. At the same time, choosing too large ω _n will increase the influence of high-frequency noise in the system, which requires comprehensive consideration , choose a reasonable ω _n to satisfy the control performance.

4.步骤6中针对位置跟踪回路，根据视线角导引思想给出路径跟踪的期望视线角导引律4. For the position tracking loop in step 6, the expected line of sight angle guidance law for path tracking is given according to the line of sight angle guidance idea

和纵向速度，并设计位置跟踪滤波补偿系统的过程为and longitudinal velocity, and the process of designing the position tracking filter compensation system is

对于位置跟踪误差系统(3)设计机器人的运动学控制器u、姿态角ψ和θ的虚拟控制量分别为For the position tracking error system (3), the kinematics controller u of the design robot, the virtual control quantities of attitude angles ψ and θ are respectively

u_co＝-k₁s+u_rcosψ_cocosθ_co(8)u _co ＝-k ₁ s+u _r cosψ _{co co} cosθ _co (8)

${ψ ψ}_{c c o o} = = - - arcsin arcsin (({k k}_{22} e e / / \sqrt{11 + + {(({k k}_{22} e e))}^{22}})) - - - - - - ((99))$

${θ θ}_{c c o o} = = arcsin arcsin (({k k}_{33} h h / / \sqrt{11 + + {(({k k}_{33} h h))}^{22}})) - - - - - - ((1010))$

其中增益因子k₁＞0，k₂＞0，k₃＞0为视线角导引律归一化参数，则系统(3)变为Among them, the gain factor k ₁ >0, k ₂ >0, and k ₃ >0 are the normalization parameters of the line-of-sight angle guidance law, then the system (3) becomes

$\{\begin{matrix} \overset{\cdot &Center Dot;}{s the s} = = r r e e - - q q h h - - {k k}_{11} s the s \\ \overset{\cdot &Center Dot;}{e e} = = - - r r s the s - - {u u}_{r r} \frac{{k k}_{22} e e}{\sqrt{11 + + {(({k k}_{22} e e))}^{22}}} \frac{11}{\sqrt{11 + + {(({k k}_{33} h h))}^{22}}} + + v v \\ \overset{\cdot &Center Dot;}{h h} = = q q s the s - - {u u}_{r r} \frac{{k k}_{33} h h}{\sqrt{11 + + {(({k k}_{33} h h))}^{22}}} + + w w \end{matrix} - - - - - - ((1111))$

对于位置跟踪误差系统式(3)，构造李雅普诺夫能量函数For the position tracking error system formula (3), construct the Lyapunov energy function

${V V}_{11} = = \frac{11}{22} {l l}^{22} - - - - - - ((1212))$

其中对方程(12)求导，将式(11)代入得in Take the derivative of Equation (12), and substitute Equation (11) into

$\begin{matrix} {\overset{\cdot &Center Dot;}{V V}}_{11} = = - - {k k}_{11} {s the s}^{22} - - {k k}_{22} {u u}_{r r} \frac{11}{\sqrt{11 + + {(({k k}_{22} e e))}^{22}}} \frac{11}{\sqrt{11 + + {(({k k}_{33} h h))}^{22}}} {e e}^{22} \\ - - {k k}_{33} {u u}_{r r} \frac{11}{\sqrt{11 + + {(({k k}_{33} e e))}^{22}}} {h h}^{22} + + e e v v + + h h w w \end{matrix} - - - - - - ((1313))$

为避免反步法后续设计中需要对θ_co和ψ_co进求导，引起计算“项数膨胀”的不足，定义θ_c，和ψ_c，为理想信号θ_co和ψ_co通过二阶滤波器的滤波信号和其导数值，滤波器定义如下In order to avoid the need to derive derivatives of θ _co and ψ _co in the follow-up design of the backstepping method, which will cause the shortage of "item expansion" in the calculation, define θ _c , and ψ _c , For the ideal signals θ _co and ψ _co pass through the filtered signal of the second-order filter and its derivative value, the filter is defined as follows

$[\begin{matrix} {\overset{\cdot &Center Dot;}{θ θ}}_{c c} \\ {\overset{\cdot\cdot \cdot\cdot}{θ θ}}_{c c} \end{matrix}] = = [\begin{matrix} 00 & 11 \\ - - {ω ω}_{n no}^{22} & - - 22 {ζω ζω}_{n no} \end{matrix}] [\begin{matrix} {θ θ}_{c c} \\ {\overset{\cdot &Center Dot;}{θ θ}}_{c c} \end{matrix}] + + [\begin{matrix} 00 \\ {ω ω}_{n no}^{22} \end{matrix}] {θ θ}_{c c o o} - - - - - - ((1414))$

滤波器的初始值θ_c(0)＝θ_co(0)；The initial value of the filter θ _c (0) = θ _co (0);

$[\begin{matrix} {\overset{\cdot \cdot}{ψ ψ}}_{c c} \\ {\overset{\cdot\cdot \cdot\cdot}{ψ ψ}}_{c c} \end{matrix}] = = [\begin{matrix} 00 & 11 \\ - - {ω ω}_{n no}^{22} & - - 22 {ζω ζω}_{n no} \end{matrix}] [\begin{matrix} {ψ ψ}_{c c} \\ {\overset{\cdot \cdot}{ψ ψ}}_{c c} \end{matrix}] + + [\begin{matrix} 00 \\ {ω ω}_{n no}^{22} \end{matrix}] {ψ ψ}_{c c o o} - - - - - - ((1515))$

滤波器的初始值ψ_c(0)＝ψ_co(0)；The initial value of the filter ψ _c (0) = ψ _co (0);

这里定义位置滤波跟踪误差信号Define the position filter tracking error signal here

$[\begin{matrix} \overset{~ ~}{s the s} \\ \overset{~ ~}{e e} \\ \overset{~ ~}{h h} \end{matrix}] = = [\begin{matrix} s the s - - {s the s}_{c c} \\ e e - - {e e}_{c c} \\ h h - - {h h}_{c c} \end{matrix}] - - - - - - ((1616))$

对式(16)两边求导，将式(3)代入得Deriving both sides of formula (16), and substituting formula (3) into

$[\begin{matrix} \overset{\cdot \cdot}{\overset{~ ~}{s the s}} \\ \overset{\cdot \cdot}{\overset{~ ~}{e e}} \\ \overset{\cdot \cdot}{\overset{~ ~}{h h}} \end{matrix}] = = [\begin{matrix} r r \overset{~ ~}{e e} - - q q \overset{~ ~}{h h} \\ - - r r \overset{~ ~}{s the s} \\ q q \overset{~ ~}{s the s} \end{matrix}] [[\begin{matrix} A A & A A B B g g ((\overset{~ ~}{ψ ψ})) {u u}_{r r} & C C g g ((\overset{~ ~}{θ θ})) {u u}_{r r} \end{matrix}]] [\begin{matrix} \overset{~ ~}{u u} \\ \overset{~ ~}{ψ ψ} \\ \overset{~ ~}{θ θ} \end{matrix}] - - - - - - ((1717))$

其中，定义滤波跟踪误差为 $\tilde{u} = u - u_{c}, \tilde{ψ} = ψ_{e} - ψ_{c}, \tilde{θ} = θ_{e} - θ_{c},$ Among them, the filter tracking error is defined as $\tilde{u} = u - u_{c}, \tilde{ψ} = ψ_{e} - ψ_{c}, \tilde{θ} = θ_{e} - θ_{c},$

$A A = = [\begin{matrix} 11 \\ 00 \\ 00 \end{matrix}],, B B = = [\begin{matrix} {cosθ cosθ}_{e e} {cosψ cosψ}_{c c} & - - {cosθ cosθ}_{e e} {sinψ sinψ}_{c c} \\ {cosθ cosθ}_{c c} {sinψ sinψ}_{c c} & {cosθ cosθ}_{c c} {cosψ cosψ}_{c c} \\ 00 & 00 \end{matrix}]$

$C C = = [\begin{matrix} {cosψ cosψ}_{c c} {cosθ cosθ}_{c c} & - - {cosψ cosψ}_{c c} {sinθ sinθ}_{c c} \\ {sinψ sinψ}_{e e} {cosθ cosθ}_{c c} & - - {sinψ sinψ}_{e e} {sinθ sinθ}_{c c} \\ - - {sinθ sinθ}_{c c} & - - {cosθ cosθ}_{c c} \end{matrix}]$

$g (\tilde{ψ}) = [\begin{matrix} \frac{c o s \tilde{ψ} - 1}{\tilde{ψ}} \\ \frac{\sin \tilde{ψ}}{\tilde{ψ}} \end{matrix}], g (\tilde{θ}) = [\begin{matrix} \frac{c o s \tilde{θ} - 1}{\tilde{θ}} \\ \frac{s i n \tilde{θ}}{\tilde{θ}} \end{matrix}],$ 且满足 $\lim_{\tilde{ψ} &RightArrow; 0} g (\tilde{ψ}) = [\begin{matrix} 0 \\ 1 \end{matrix}], \lim_{\tilde{θ} &RightArrow; 0} g (\tilde{θ}) = [\begin{matrix} 0 \\ 1 \end{matrix}]$ 对式(16)左右移项，增加和减少一项 ${[\begin{matrix} {\overset{\cdot}{s}}_{c o} & {\overset{\cdot}{e}}_{c o} & {\overset{\cdot}{h}}_{c o} \end{matrix}]}^{T}$ 得到 $g (\tilde{ψ}) = [\begin{matrix} \frac{c o the s \tilde{ψ} - 1}{\tilde{ψ}} \\ \frac{\sin \tilde{ψ}}{\tilde{ψ}} \end{matrix}], g (\tilde{θ}) = [\begin{matrix} \frac{c o the s \tilde{θ} - 1}{\tilde{θ}} \\ \frac{the s i no \tilde{θ}}{\tilde{θ}} \end{matrix}],$ and satisfied $\lim_{\tilde{ψ} &Right Arrow; 0} g (\tilde{ψ}) = [\begin{matrix} 0 \\ 1 \end{matrix}], \lim_{\tilde{θ} &Right Arrow; 0} g (\tilde{θ}) = [\begin{matrix} 0 \\ 1 \end{matrix}]$ Shift the left and right terms of formula (16), increase and decrease one term ${[\begin{matrix} {\overset{&Center Dot;}{the s}}_{c o} & {\overset{&Center Dot;}{e}}_{c o} & {\overset{&Center Dot;}{h}}_{c o} \end{matrix}]}^{T}$ get

$[\begin{matrix} \overset{\cdot &Center Dot;}{s the s} \\ \overset{\cdot \cdot}{e e} \\ \overset{\cdot \cdot}{h h} \end{matrix}] = = [\begin{matrix} {\overset{\cdot &Center Dot;}{s the s}}_{c c o o} \\ {\overset{\cdot \cdot}{e e}}_{c c o o} \\ {\overset{\cdot &Center Dot;}{h h}}_{c c o o} \end{matrix}] + + [\begin{matrix} \overset{\cdot \cdot}{\overset{~ ~}{s the s}} \\ \overset{\cdot &Center Dot;}{\overset{~ ~}{e e}} \\ \overset{\cdot &Center Dot;}{\overset{~ ~}{h h}} \end{matrix}] + + [\begin{matrix} {\overset{\cdot \cdot}{s the s}}_{c c} - - {\overset{\cdot \cdot}{s the s}}_{c c o o} \\ {\overset{\cdot \cdot}{e e}}_{c c} - - {\overset{\cdot \cdot}{e e}}_{c c o o} \\ {\overset{\cdot &Center Dot;}{h h}}_{c c} - - {\overset{\cdot &Center Dot;}{h h}}_{c c o o} \end{matrix}] - - - - - - ((1818))$

根据跟踪控制器设计目标，选择理想期望位置信号 ${[\begin{matrix} {\overset{\cdot}{s}}_{c o} & {\overset{\cdot}{e}}_{c o} & {\overset{\cdot}{h}}_{c o} \end{matrix}]}^{T}$ 为According to the design goal of the tracking controller, select the ideal desired position signal ${[\begin{matrix} {\overset{&Center Dot;}{the s}}_{c o} & {\overset{&Center Dot;}{e}}_{c o} & {\overset{&Center Dot;}{h}}_{c o} \end{matrix}]}^{T}$ for

$[\begin{matrix} {\overset{\cdot &Center Dot;}{s the s}}_{c c o o} \\ {\overset{\cdot &Center Dot;}{e e}}_{c c o o} \\ {\overset{\cdot &Center Dot;}{h h}}_{c c o o} \end{matrix}] = = [\begin{matrix} - - {k k}_{x x} \overset{~ ~}{s the s} + + {\overset{\cdot &Center Dot;}{s the s}}_{c c} \\ - - {k k}_{y the y} \overset{~ ~}{e e} + + {\overset{\cdot &Center Dot;}{e e}}_{c c} \\ - - {k k}_{z z} \overset{~ ~}{h h} + + {\overset{~ ~}{h h}}_{c c} \end{matrix}] - - - - - - ((1919))$

定义角度滤波跟踪误差信号为Define the angle filter tracking error signal as

$[\begin{matrix} \overset{~ ~}{u u} \\ \overset{~ ~}{ψ ψ} \\ \overset{~ ~}{θ θ} \end{matrix}] = = [\begin{matrix} u u - - {u u}_{c c} \\ {ψ ψ}_{e e} - - {ψ ψ}_{c c} \\ {θ θ}_{e e} - - {θ θ}_{c c} \end{matrix}] - - - - - - ((2020))$

将式(19)和(17)代入(18)整理得Substitute (19) and (17) into (18) to get

$[\begin{matrix} \overset{\cdot \cdot}{\overset{~ ~}{s the s}} \\ \overset{\cdot &Center Dot;}{\overset{~ ~}{e e}} \\ \overset{\cdot \cdot}{\overset{~ ~}{h h}} \end{matrix}] = = [\begin{matrix} r r \overset{~ ~}{e e} - - q q \overset{~ ~}{h h} \\ - - r r \overset{~ ~}{s the s} \\ q q \overset{~ ~}{s the s} \end{matrix}] + + [\begin{matrix} - - {k k}_{x x} \overset{~ ~}{s the s} \\ - - {k k}_{y the y} \overset{~ ~}{e e} \\ - - {k k}_{z z} \overset{~ ~}{h h} \end{matrix}] + + [\begin{matrix} {\overset{\cdot &Center Dot;}{s the s}}_{c c} - - {\overset{\cdot &Center Dot;}{s the s}}_{c c o o} \\ {\overset{\cdot &Center Dot;}{e e}}_{c c} - - {\overset{\cdot &Center Dot;}{e e}}_{c c o o} \\ {\overset{\cdot &Center Dot;}{h h}}_{c c} - - {\overset{\cdot &Center Dot;}{h h}}_{c c o o} \end{matrix}] + + [[\begin{matrix} A A & B B g g ((\overset{~ ~}{ψ ψ})) {u u}_{r r} & C C g g ((\overset{~ ~}{θ θ})) \end{matrix} {u u}_{r r}]] [\begin{matrix} \overset{~ ~}{u u} \\ \overset{~ ~}{ψ ψ} \\ \overset{~ ~}{θ θ} \end{matrix}] - - - - - - ((21 twenty one))$

定义滤波信号补偿误差Define Filtered Signal Compensation Error

$[\begin{matrix} {&upsi; &upsi;}_{x x} \\ {&upsi; &upsi;}_{y the y} \\ {&upsi; &upsi;}_{z z} \end{matrix}] = = [\begin{matrix} \overset{~ ~}{s the s} - - {ζ ζ}_{x x} \\ \overset{~ ~}{e e} - - {ζ ζ}_{y the y} \\ \overset{~ ~}{h h} - - {ζ ζ}_{z z} \end{matrix}] - - - - - - ((22 twenty two))$

其中构造位置滤波补偿动态ζ_x、ζ_y和ζ_z如下Wherein the construction of position filter compensation dynamic ζ _x , ζ _y and ζ _z is as follows

$[\begin{matrix} {\overset{\cdot &Center Dot;}{ζ ζ}}_{x x} \\ {\overset{\cdot \cdot}{ζ ζ}}_{y the y} \\ {\overset{\cdot &Center Dot;}{ζ ζ}}_{z z} \end{matrix}] = = [\begin{matrix} {rζ rζ}_{y the y} - - {qζ qζ}_{z z} \\ - - {rζ rζ}_{x x} \\ {qζ qζ}_{x x} \end{matrix}] [\begin{matrix} - - {k k}_{x x} {ζ ζ}_{x x} \\ - - {k k}_{y the y} {ζ ζ}_{y the y} \\ - - {k k}_{z z} {ζ ζ}_{z z} \end{matrix}] + + [\begin{matrix} {\overset{\cdot \cdot}{s the s}}_{c c} - - {\overset{\cdot \cdot}{s the s}}_{c c o o} \\ {\overset{\cdot &Center Dot;}{e e}}_{c c} - - {\overset{\cdot \cdot}{e e}}_{c c o o} \\ {\overset{\cdot \cdot}{h h}}_{c c} - - {\overset{\cdot \cdot}{h h}}_{c c o o} \end{matrix}] + + {u u}_{r r} [[\begin{matrix} A A & B B g g ((\overset{~ ~}{ψ ψ})) & C C g g ((\overset{~ ~}{θ θ})) \end{matrix}]] [\begin{matrix} {ζ ζ}_{u u} \\ {ζ ζ}_{ψ ψ} \\ {ζ ζ}_{θ θ} \end{matrix}] - - - - - - ((23 twenty three))$

这里ζ_x(0)＝0、ζ_y(0)＝0、ζ_z(0)＝0。Here, ζ _x (0)=0, ζ _y (0)=0, and ζ _z (0)=0.

5.步骤6中针对姿态角跟踪回路，设计角速度控制律和姿态角跟踪滤波补偿系统的过程为：5. In step 6, for the attitude angle tracking loop, the process of designing the angular velocity control law and the attitude angle tracking filter compensation system is as follows:

对滤波跟踪误差式(20)求导，将姿态角跟踪误差模型式(4)代入得Deriving the filter tracking error formula (20), and substituting the attitude angle tracking error model formula (4) into

$\begin{matrix} \overset{\cdot \cdot}{\overset{~ ~}{ψ ψ}} = = \frac{r r}{c c o o s the s θ θ} - - {r r}_{F f} - - {\overset{\cdot \cdot}{ψ ψ}}_{c c} \\ = = \frac{{r r}_{c c o o} + + (({r r}_{c c} - - {r r}_{c c o o})) + + \overset{~ ~}{r r}}{cos cos θ θ} - - {r r}_{F f} - - {\overset{\cdot \cdot}{ψ ψ}}_{c c} \end{matrix} - - - - - - ((24 twenty four))$

$\begin{matrix} \overset{\cdot &Center Dot;}{\overset{~ ~}{θ θ}} = = q q - - {q q}_{F f} - - {\overset{\cdot \cdot}{θ θ}}_{c c} \\ = = {q q}_{c c o o} + + (({q q}_{c c} - - {q q}_{c c o o})) + + \overset{~ ~}{q q} - - {q q}_{F f} - - {\overset{\cdot &Center Dot;}{θ θ}}_{c c} \end{matrix} - - - - - - ((2525))$

其中角速度跟踪误差定义为这里分别设计理想虚拟控制信号r_co和q_co为where the angular velocity tracking error is defined as Here, the ideal virtual control signals r _co and q _co are respectively designed as

${r r}_{c c o o} = = c c o o s the s θ θ (({r r}_{F f} + + {\overset{\cdot &Center Dot;}{ψ ψ}}_{c c} - - {k k}_{ψ ψ} \overset{~ ~}{ψ ψ} - - {ψ ψ}_{b b s the s})) - - - - - - ((2626))$

${q q}_{c c o o} = = {q q}_{F f} + + {\overset{\cdot &Center Dot;}{θ θ}}_{c c} - - {k k}_{θ θ} \overset{~ ~}{θ θ} - - {θ θ}_{b b s the s} - - - - - - ((2727))$

这里q_c和r_c和分别为理想信号q_co和r_co通过二阶滤波器得到的滤波信号和其导数值，定义滤波器形式为where q _c and r _c and Respectively, the ideal signal q _co and r _co get the filtered signal and its derivative value through the second-order filter, and the filter form is defined as

$[\begin{matrix} {\overset{\cdot &Center Dot;}{q q}}_{c c} \\ {\overset{\cdot\cdot \cdot\cdot}{q q}}_{c c} \end{matrix}] = = [\begin{matrix} 00 & 11 \\ - - {ω ω}_{n no}^{22} & - - 22 {ζω ζω}_{n no} \end{matrix}] [\begin{matrix} {q q}_{c c} \\ {\overset{\cdot &Center Dot;}{q q}}_{c c} \end{matrix}] + + [\begin{matrix} 00 \\ {ω ω}_{n no}^{22} \end{matrix}] {q q}_{c c o o} - - - - - - ((2828))$

滤波器初始条件q_c(0)＝q_co(0)；Filter initial condition q _c (0) = q _co (0);

$[\begin{matrix} {\overset{\cdot &Center Dot;}{r r}}_{c c} \\ {\overset{\cdot\cdot \cdot\cdot}{r r}}_{c c} \end{matrix}] = = [\begin{matrix} 00 & 11 \\ - - {ω ω}_{n no}^{22} & - - 22 {ζω ζω}_{n no} \end{matrix}] [\begin{matrix} {r r}_{c c} \\ {\overset{\cdot \cdot}{r r}}_{c c} \end{matrix}] + + [\begin{matrix} 00 \\ {ω ω}_{n no}^{22} \end{matrix}] {r r}_{c c o o} - - - - - - ((2929))$

滤波器初始条件r_c(0)＝r_co(0)；Filter initial condition r _c (0) = r _co (0);

将式(26)和式(27)代入式(24)和(25)得Substituting formula (26) and formula (27) into formula (24) and (25) to get

$\overset{\cdot &Center Dot;}{\overset{~ ~}{ψ ψ}} = = - - {k k}_{ψ ψ} \overset{~ ~}{ψ ψ} + + \frac{(({r r}_{c c} - - {r r}_{c c o o})) + + \overset{~ ~}{r r}}{cos cos θ θ} - - {ψ ψ}_{b b s the s} - - - - - - ((3030))$

$\overset{\cdot &Center Dot;}{\overset{~ ~}{θ θ}} = = - - {k k}_{θ θ} \overset{~ ~}{θ θ} + + (({q q}_{c c} - - {q q}_{c c o o})) + + \overset{~ ~}{q q} - - {θ θ}_{b b s the s} - - - - - - ((3131))$

其中k_ψ＞0和k_θ＞0为控制器参数，ψ_bs和θ_bs为待设计镇定项，这里通过定义补偿跟踪误差，对滤波器输出的信号进行反馈补偿Where k _ψ > 0 and k _θ > 0 are the controller parameters, ψ _bs and θ _bs are the stabilization items to be designed. Here, the compensation tracking error is defined to perform feedback compensation on the signal output by the filter

$[\begin{matrix} {&upsi; &upsi;}_{ψ ψ} \\ {&upsi; &upsi;}_{θ θ} \end{matrix}] = = [\begin{matrix} \overset{~ ~}{ψ ψ} - - {ζ ζ}_{ψ ψ} \\ \overset{~ ~}{θ θ} - - {ζ ζ}_{θ θ} \end{matrix}] - - - - - - ((3232))$

这里构造滤波补偿动态Here, filter compensation dynamics are constructed

${\overset{\cdot &Center Dot;}{ζ ζ}}_{ψ ψ} = = - - {k k}_{ψ ψ} {ζ ζ}_{ψ ψ} + + \frac{{r r}_{c c} - - {r r}_{c c o o}}{c c o o s the s θ θ} + + \frac{{ζ ζ}_{r r}}{cos cos θ θ} - - - - - - ((3333))$

${\overset{\cdot &Center Dot;}{ζ ζ}}_{θ θ} = = - - {k k}_{θ θ} {\overset{\cdot &Center Dot;}{ζ ζ}}_{θ θ} + + (({q q}_{c c} - - {q q}_{c c o o})) + + {ζ ζ}_{q q} - - - - - - ((3434))$

这里初始条件ζ_ψ(0)＝0，ζ_θ(0)＝0，ζ_r＝0，ζ_q＝0。Here initial conditions ζ _ψ (0)=0, ζ _θ (0)=0, ζ _r =0, ζ _q =0.

6.步骤6中针对速度控制回路，引入积分环节减小稳态误差，设计真实控制输入的具体过程如下：6. For the speed control loop in step 6, an integral link is introduced to reduce the steady-state error. The specific process of designing the real control input is as follows:

为保证跟踪系统存在外干扰下的鲁棒性，引入积分项增加系统的鲁棒性，定义这里设计机器人三维路径跟踪控制器为In order to ensure the robustness of the tracking system in the presence of external disturbances, an integral term is introduced to increase the robustness of the system, defined as Here the robot 3D path tracking controller is designed as

$\{\begin{matrix} {F f}_{u u} = = {m m}_{11} ((- - {k k}_{u u} \overset{~ ~}{u u} - - {k k}_{i i u u} {ϵ ϵ}_{11} + + {\overset{\cdot &Center Dot;}{u u}}_{c c} - - {u u}_{b b s the s})) - - {f f}_{u u} \\ {δ δ}_{s the s} = = {b b}_{11}^{- - 11} [[{m m}_{44} ((- - {k k}_{q q} \overset{~ ~}{q q} - - {k k}_{i i q q} {ϵ ϵ}_{22} + + {\overset{\cdot \cdot}{q q}}_{c c} - - {q q}_{b b s the s})) - - {f f}_{q q}]] \\ {δ δ}_{r r} = = {b b}_{22}^{- - 11} [[{m m}_{55} ((- - {k k}_{r r} \overset{~ ~}{r r} - - {k k}_{i i r r} {ϵ ϵ}_{33} + + {\overset{\cdot \cdot}{r r}}_{c c} - - {r r}_{b b s the s})) - - {f f}_{r r}]] \end{matrix} - - - - - - ((3535))$

其中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中进行设计。Where f _u =m ₂ vr-m ₃ wq+d ₁ u, f _q =(m ₁ -m ₃ )uw+d ₄ qg ₂ and f _r =(m ₁ -m ₂ )uv+d ₅ r are models Non-linear hydrodynamic items, u _bs , q _bs and r _bs are feedback compensation robust items to be designed and designed in step 6.

7.步骤6中反馈控制项设计的具体过程如下：7. The specific process of feedback control item design in step 6 is as follows:

首先对于步骤2中位置跟踪系统结合式(22)构造李雅普诺夫能量函数Firstly, construct the Lyapunov energy function for the position tracking system combined with Equation (22) in step 2

${V V}_{11} = = \frac{11}{22} (({&upsi; &upsi;}_{x x}^{22} + + {&upsi; &upsi;}_{y the y}^{22} + + {&upsi; &upsi;}_{z z}^{22})) - - - - - - ((3636))$

对上式求导，将式(21)和(23)代入式(36)得Taking the derivative of the above formula, substituting formula (21) and (23) into formula (36), we get

$\begin{matrix} {V V}_{11} = = {\overset{\cdot \cdot}{&upsi; &upsi;}}_{x x} {&upsi; &upsi;}_{x x} + + {\overset{\cdot \cdot}{&upsi; &upsi;}}_{y the y} {&upsi; &upsi;}_{y the y} + + {\overset{\cdot &Center Dot;}{&upsi; &upsi;}}_{z z} {&upsi; &upsi;}_{z z} \\ = = - - {k k}_{x x} {&upsi; &upsi;}_{x x}^{22} - - {k k}_{y the y} {&upsi; &upsi;}_{y the y}^{22} - - {k k}_{z z} {&upsi; &upsi;}_{z z}^{22} + + \\ [[\begin{matrix} {&upsi; &upsi;}_{x x} & {&upsi; &upsi;}_{y the y} & {&upsi; &upsi;}_{z z} \end{matrix}]] [[\begin{matrix} A A & B B g g ((\overset{~ ~}{ψ ψ})) {u u}_{r r} & C C g g ((\overset{~ ~}{θ θ})) {u u}_{r r} \end{matrix}]] [\begin{matrix} {&upsi; &upsi;}_{u u} \\ {&upsi; &upsi;}_{ψ ψ} \\ {&upsi; &upsi;}_{θ θ} \end{matrix}] \\ = = - - {k k}_{x x} {&upsi; &upsi;}_{x x}^{22} - - {k k}_{y the y} {&upsi; &upsi;}_{y the y}^{22} - - {k k}_{z z} {&upsi; &upsi;}_{z z}^{22} + + {A A}^{T T} [\begin{matrix} {&upsi; &upsi;}_{x x} \\ {&upsi; &upsi;}_{y the y} \\ {&upsi; &upsi;}_{z z} \end{matrix}] {&upsi; &upsi;}_{u u} + + \\ {g g}^{T T} ((\overset{~ ~}{ψ ψ})) {B B}^{T T} {u u}_{r r} [\begin{matrix} {&upsi; &upsi;}_{x x} \\ {&upsi; &upsi;}_{y the y} \\ {&upsi; &upsi;}_{z z} \end{matrix}] {&upsi; &upsi;}_{ψ ψ} + + {g g}^{T T} ((\overset{~ ~}{θ θ})) {C C}^{T T} {u u}_{r r} [\begin{matrix} {&upsi; &upsi;}_{x x} \\ {&upsi; &upsi;}_{y the y} \\ {&upsi; &upsi;}_{z z} \end{matrix}] {&upsi; &upsi;}_{θ θ} \end{matrix} - - - - - - ((3737))$

其中υ_u、υ_ψ、υ_θ定义如式(32)。Among them, υ _u , υ _ψ , and υ _θ are defined as formula (32).

然后对于姿态跟踪系统结合式(32)构造李雅普诺夫能量函数Then for the attitude tracking system combined with formula (32) to construct the Lyapunov energy function

${V V}_{22} = = \frac{11}{22} (({&upsi; &upsi;}_{ψ ψ}^{22} + + {&upsi; &upsi;}_{θ θ}^{22})) - - - - - - ((3838))$

对上式求导，将式(30)～(34)代入得:To derive the above formula, substitute the formulas (30)~(34) into:

$\begin{matrix} {\overset{\cdot \cdot}{V V}}_{22} = = {\overset{\cdot &Center Dot;}{&upsi; &upsi;}}_{ψ ψ} {&upsi; &upsi;}_{ψ ψ} + + {\overset{\cdot &Center Dot;}{&upsi; &upsi;}}_{00} {&upsi; &upsi;}_{00} \\ = = ((\overset{\cdot &Center Dot;}{\overset{~ ~}{ψ ψ}} - - {\overset{\cdot &Center Dot;}{ζ ζ}}_{ψ ψ})) {&upsi; &upsi;}_{ψ ψ} + + ((\overset{\cdot &Center Dot;}{\overset{~ ~}{θ θ}} - - {\overset{\cdot &Center Dot;}{ζ ζ}}_{θ θ})) {&upsi; &upsi;}_{θ θ} \\ = = ((- - {k k}_{ψ ψ} \overset{~ ~}{ψ ψ} + + \frac{\overset{~ ~}{r r}}{cos cos θ θ} - - {ψ ψ}_{b b s the s} + + {k k}_{ψ ψ} {ζ ζ}_{ψ ψ} - - \frac{{ζ ζ}_{r r}}{cos cos θ θ})) {&upsi; &upsi;}_{ψ ψ} \\ + + ((- - {k k}_{θ θ} \overset{~ ~}{θ θ} + + \overset{~ ~}{q q} - - {θ θ}_{b b s the s} + + {k k}_{θ θ} {ζ ζ}_{θ θ} - - {ζ ζ}_{q q})) {&upsi; &upsi;}_{00} \\ = = - - {k k}_{ψ ψ} {&upsi; &upsi;}_{ψ ψ}^{22} - - {k k}_{θ θ} {&upsi; &upsi;}_{θ θ}^{22} + + {&upsi; &upsi;}_{q q} {&upsi; &upsi;}_{θ θ} + + \frac{{&upsi; &upsi;}_{r r}}{cos cos θ θ} {&upsi; &upsi;}_{ψ ψ} \\ - - {θ θ}_{b b s the s} {&upsi; &upsi;}_{θ θ} - - {ψ ψ}_{b b s the s} {&upsi; &upsi;}_{ψ ψ} \end{matrix} - - - - - - ((3939))$

其中 ${&upsi;}_{u} = \tilde{u} - ζ_{u}, {&upsi;}_{r} = \tilde{r} - ζ_{r}, {&upsi;}_{q} = \tilde{q} - ζ_{q} .$ in ${&upsi;}_{u} = \tilde{u} - ζ_{u}, {&upsi;}_{r} = \tilde{r} - ζ_{r}, {&upsi;}_{q} = \tilde{q} - ζ_{q} .$

再次对于速度控制回路将式(35)代入式(5)得到u、q和r的误差系统为Again, for the speed control loop, substituting Equation (35) into Equation (5) to get the error system of u, q and r as

$\{\begin{matrix} \overset{\cdot &Center Dot;}{\overset{~ ~}{u u}} = = - - {k k}_{u u} \overset{~ ~}{u u} - - {k k}_{i i u u} {ϵ ϵ}_{11} - - {u u}_{b b s the s} \\ \overset{\cdot &Center Dot;}{\overset{~ ~}{q q}} = = - - {k k}_{q q} \overset{~ ~}{q q} - - {k k}_{i i q q} {ϵ ϵ}_{22} - - {q q}_{b b s the s} \\ \overset{\cdot \cdot}{\overset{~ ~}{r r}} = = - - {k k}_{r r} \overset{~ ~}{r r} - - {k k}_{i i r r} {ϵ ϵ}_{33} - - {r r}_{b b s the s} \end{matrix} - - - - - - ((4040))$

由于ζ_u＝ζ_r＝ζ_q＝0，这里得到滤波补偿误差系统的动态为Since ζ _u = ζ _r = ζ _q = 0, here the dynamics of the filter compensation error system is

$\{\begin{matrix} {\overset{\cdot &Center Dot;}{&upsi; &upsi;}}_{u u} = = - - {k k}_{u u} {&upsi; &upsi;}_{u u} - - {k k}_{i i u u} {ϵ ϵ}_{11} - - {u u}_{b b s the s} \\ {\overset{\cdot &Center Dot;}{&upsi; &upsi;}}_{q q} = = - - {k k}_{q q} {&upsi; &upsi;}_{q q} - - {k k}_{i i q q} {ϵ ϵ}_{22} - - {q q}_{b b s the s} \\ {\overset{\cdot &Center Dot;}{&upsi; &upsi;}}_{r r} = = - - {k k}_{r r} {&upsi; &upsi;}_{r r} - - {k k}_{i i r r} {ϵ ϵ}_{33} - - {r r}_{b b s the s} \end{matrix} - - - - - - ((4141))$

由于所以系统(41)可以重写为because So the system (41) can be rewritten as

$\{\begin{matrix} {\overset{\cdot\cdot \cdot\cdot}{ϵ ϵ}}_{11} = = - - {k k}_{u u} {\overset{\cdot &Center Dot;}{ϵ ϵ}}_{11} - - {k k}_{i i u u} {ϵ ϵ}_{11} - - {u u}_{b b s the s} \\ {\overset{\cdot\cdot \cdot\cdot}{ϵ ϵ}}_{22} = = - - {k k}_{q q} {\overset{\cdot &Center Dot;}{ϵ ϵ}}_{22} - - {k k}_{i i q q} {ϵ ϵ}_{22} - - {q q}_{b b s the s} \\ {\overset{\cdot\cdot \cdot\cdot}{ϵ ϵ}}_{33} = = - - {k k}_{r r} {\overset{\cdot &Center Dot;}{ϵ ϵ}}_{33} - - {k k}_{i i r r} {ϵ ϵ}_{33} - - {r r}_{b b s the s} \end{matrix} - - - - - - ((4242))$

定义误差向量ε＝[ε₁,ε₂,ε₃]^T，则系统(42)可以表示为Define the error vector ε=[ε ₁ ,ε ₂ ,ε ₃ ] ^T , Then the system (42) can be expressed as

$\overset{\cdot \cdot}{E E.} = = A A E E. + + B B U u - - - - - - ((4343))$

其中K_I＝diag{-k_iu,-k_iq,-k_ir}，K_P＝diag{-k_u,-k_q,-k_r}in K _I =diag{-k _iu ,-k _iq ,-k _ir }, K _P =diag{-k _u ,-k _q ,-k _r }

最后结合式(36)、(43)和式(38)构造李雅普诺夫能量函数Finally, combine formula (36), (43) and formula (38) to construct the Lyapunov energy function

${V V}_{33} = = {V V}_{11} + + {V V}_{22} + + \frac{11}{22} {E E.}^{T T} P P E E. - - - - - - ((4444))$

其中正定对称矩阵P为线性李雅普诺夫方程的解where the positive definite symmetric matrix P is the solution of the linear Lyapunov equation

A^TP+PA＝-Q(45) ^ATP +PA＝-Q(45)

其中 $P = [\begin{matrix} P_{1} & 0_{3 \times 3} \\ 0_{3 \times 3} & P_{2} \end{matrix}],$ P_i＝diag{p_i1,p_i2,p_i3}，i＝1,2为正定对称矩阵，如果选择P₁＝K_IP₂，则 $Q = [\begin{matrix} 0_{3 \times 3} & 0_{3 \times 3} \\ 0_{3 \times 3} & 2 K_{I} P_{2} \end{matrix}];$ 对式(44)进行求导，将式(37)、(39)和(43)代入整理得in $P = [\begin{matrix} P_{1} & 0_{3 \times 3} \\ 0_{3 \times 3} & P_{2} \end{matrix}],$ P _i ＝diag{p _i1 ,p _i2 ,p _i3 }, i=1,2 is positive definite symmetric matrix, if choose P ₁ ＝K _I P ₂ , then $Q = [\begin{matrix} 0_{3 \times 3} & 0_{3 \times 3} \\ 0_{3 \times 3} & 2 K_{I} P_{2} \end{matrix}];$ Deriving formula (44), substituting formulas (37), (39) and (43) into

$\begin{matrix} {\overset{\cdot &Center Dot;}{V V}}_{33} = = {\overset{\cdot \cdot}{V V}}_{11} + + {\overset{\cdot \cdot}{V V}}_{22} + + {E E.}^{T T} P P \overset{\cdot &Center Dot;}{E E.} \\ = = - - {k k}_{x x} {&upsi; &upsi;}_{x x}^{22} - - {k k}_{y the y} {&upsi; &upsi;}_{y the y}^{22} - - {k k}_{z z} {&upsi; &upsi;}_{z z}^{22} - - {k k}_{u u} {&upsi; &upsi;}_{u u}^{22} - - {k k}_{ψ ψ} {&upsi; &upsi;}_{ψ ψ}^{22} - - {k k}_{θ θ} {&upsi; &upsi;}_{θ θ}^{22} \\ - - \frac{11}{22} {E E.}^{T T} Q Q E E. + + {A A}^{T T} [\begin{matrix} {&upsi; &upsi;}_{x x} \\ {&upsi; &upsi;}_{y the y} \\ {&upsi; &upsi;}_{z z} \end{matrix}] {&upsi; &upsi;}_{u u} + + {g g}^{T T} ((\overset{~ ~}{ψ ψ})) {B B}^{T T} {u u}_{r r} [\begin{matrix} {&upsi; &upsi;}_{x x} \\ {&upsi; &upsi;}_{y the y} \\ {&upsi; &upsi;}_{z z} \end{matrix}] {&upsi; &upsi;}_{ψ ψ} \\ - - {ψ ψ}_{b b s the s} {&upsi; &upsi;}_{ψ ψ} + + \frac{{&upsi; &upsi;}_{r r}}{cos cos θ θ} {&upsi; &upsi;}_{ψ ψ} + + {g g}^{T T} ((\overset{~ ~}{θ θ})) {C C}^{T T} {u u}_{r r} [\begin{matrix} {&upsi; &upsi;}_{x x} \\ {&upsi; &upsi;}_{y the y} \\ {&upsi; &upsi;}_{z z} \end{matrix}] {&upsi; &upsi;}_{θ θ} \\ + + {&upsi; &upsi;}_{q q} {&upsi; &upsi;}_{θ θ} - - {θ θ}_{b b s the s} {&upsi; &upsi;}_{θ θ} + + {E E.}^{T T} P P B B U u \end{matrix} - - - - - - ((4646))$

式(46)进一步变为Equation (46) further becomes

$\begin{matrix} {\overset{\cdot &Center Dot;}{V V}}_{33} = = - - {k k}_{x x} {&upsi; &upsi;}_{x x}^{22} - - {k k}_{y the y} {&upsi; &upsi;}_{y the y}^{22} - - {k k}_{z z} {&upsi; &upsi;}_{z z}^{22} - - {k k}_{u u} {&upsi; &upsi;}_{u u}^{22} - - {k k}_{ψ ψ} {&upsi; &upsi;}_{ψ ψ}^{22} - - {k k}_{θ θ} {&upsi; &upsi;}_{θ θ}^{22} \\ - - \frac{11}{22} {E E.}^{T T} Q Q E E. + + {A A}^{T T} [\begin{matrix} {&upsi; &upsi;}_{x x} \\ {&upsi; &upsi;}_{y the y} \\ {&upsi; &upsi;}_{z z} \end{matrix}] {&upsi; &upsi;}_{u u} + + {g g}^{T T} ((\overset{~ ~}{ψ ψ})) {B B}^{T T} {u u}_{r r} [\begin{matrix} {&upsi; &upsi;}_{x x} \\ {&upsi; &upsi;}_{y the y} \\ {&upsi; &upsi;}_{z z} \end{matrix}] {&upsi; &upsi;}_{ψ ψ} \\ + + {&upsi; &upsi;}_{q q} {&upsi; &upsi;}_{θ θ} + + \frac{{&upsi; &upsi;}_{r r}}{cos cos θ θ} {&upsi; &upsi;}_{ψ ψ} + + {g g}^{T T} ((\overset{~ ~}{θ θ})) {C C}^{T T} {u u}_{r r} [\begin{matrix} {&upsi; &upsi;}_{x x} \\ {&upsi; &upsi;}_{y the y} \\ {&upsi; &upsi;}_{z z} \end{matrix}] {&upsi; &upsi;}_{θ θ} - - {ψ ψ}_{b b s the s} {&upsi; &upsi;}_{ψ ψ} \\ - - {θ θ}_{b b s the s} {&upsi; &upsi;}_{θ θ} - - {p p}_{21 twenty one} {&upsi; &upsi;}_{u u} {u u}_{b b s the s} - - {p p}_{22 twenty two} {&upsi; &upsi;}_{q q} {q q}_{b b s the s} - - {p p}_{23 twenty three} {&upsi; &upsi;}_{r r} {r r}_{b b s the s} \end{matrix} - - - - - - ((4747))$

如果设计反馈补偿项为If the design feedback compensation term is

${ψ ψ}_{b b s the s} = = {g g}^{T T} ((\overset{~ ~}{ψ ψ})) {B B}^{T T} {u u}_{r r} [\begin{matrix} {&upsi; &upsi;}_{x x} \\ {&upsi; &upsi;}_{y the y} \\ {&upsi; &upsi;}_{z z} \end{matrix}] - - - - - - ((4848))$

${θ θ}_{b b s the s} = = {g g}^{T T} ((\overset{~ ~}{θ θ})) {C C}^{T T} {u u}_{r r} [\begin{matrix} {&upsi; &upsi;}_{x x} \\ {&upsi; &upsi;}_{y the y} \\ {&upsi; &upsi;}_{z z} \end{matrix}] - - - - - - ((4949))$

${u u}_{b b s the s} = = \frac{11}{{p p}_{21 twenty one}} {A A}^{T T} [\begin{matrix} {&upsi; &upsi;}_{x x} \\ {&upsi; &upsi;}_{y the y} \\ {&upsi; &upsi;}_{z z} \end{matrix}] - - - - - - ((5050))$

${r r}_{b b s the s} = = \frac{11}{{p p}_{23 twenty three}} \frac{{&upsi; &upsi;}_{ψ ψ}}{c c o o s the s θ θ} - - - - - - ((5151))$

${q q}_{b b s the s} = = \frac{11}{{p p}_{22 twenty two}} {&upsi; &upsi;}_{θ θ} - - - - - - ((5252))$

将式(48)(48)～(52)(52)代入式(47)(47)整理得Substitute formula (48)(48)～(52)(52) into formula (47)(47) to get

$\begin{matrix} {\overset{\cdot \cdot}{V V}}_{33} = = - - {k k}_{x x} {&upsi; &upsi;}_{x x}^{22} - - {k k}_{y the y} {&upsi; &upsi;}_{y the y}^{22} - - {k k}_{z z} {&upsi; &upsi;}_{z z}^{22} - - {k k}_{ψ ψ} {&upsi; &upsi;}_{ψ ψ}^{22} - - {k k}_{θ θ} {&upsi; &upsi;}_{θ θ}^{22} \\ - - \frac{11}{22} {E E.}^{T T} Q Q E E. \leq \leq 00 \end{matrix} - - - - - - ((5353))$

上述定理证明了补偿跟踪误差系统υ_i的指数收敛性，由二阶滤波器的设计过程可知，当选择合适的自然频率ω_n，x_co为滤波器的参考输入信号，滤波器为线性稳定系统，可见当x_co为有界值，则x_c和均为连续有界信号，如果信号x_co的带宽低于滤波器设计带宽，那么误差信号|x_co(t)-x_c(t)|将会很小，由于ζ_i是一阶稳定线性系统，所以ζ_i将趋近于零值，从而系统跟踪误差指数趋近于零值。由于在控制回路中引入了滤波器，滤波器的跟踪精度直接影响系统的控制性能，动态面控制由于未考虑滤波信号的跟踪精度，只能够保证系统跟踪误差收敛到原点较小的邻域。这里通过构造滤波补偿系统，提高滤波信号跟踪精度，通过稳定性分析保证闭环系统跟踪误差收敛到零点。The above theorem proves the exponential convergence of the compensating tracking error system υ _i . From the design process of the second-order filter, it can be seen that when an appropriate natural frequency ω _n is selected, x _co is the reference input signal of the filter, and the filter is a linear stable system , it can be seen that when x _co is a bounded value, then x _c and Both are continuous and bounded signals, if the bandwidth of the signal x _co is lower than the filter design bandwidth, then the error signal |x _co (t)-x _c (t)| will be very small, because ζ _i is a first-order stable linear system , so ζ _i will approach zero value, so the system tracking error index will approach zero value. Since the filter is introduced in the control loop, the tracking accuracy of the filter directly affects the control performance of the system. The dynamic surface control can only ensure that the system tracking error converges to a smaller neighborhood of the origin because the tracking accuracy of the filtered signal is not considered. Here, the filter compensation system is constructed to improve the tracking accuracy of the filter signal, and the stability analysis is used to ensure that the tracking error of the closed-loop system converges to zero.

仿真验证Simulation

下面举例说明，验证本发明方法的有效性：Illustrate below, verify the effectiveness of the inventive method:

根据水动力系数建立机器人六自由度仿真模型，采用Matlab环境搭建机器人三维路径跟踪控制仿真系统。A six-degree-of-freedom simulation model of the robot is established according to the hydrodynamic coefficient, and a three-dimensional path tracking control simulation system of the robot is built using the Matlab environment.

针对机器人螺旋下潜作业，规划期望三维曲线路径为(单位：m)For the robot spiral dive operation, the expected three-dimensional curved path of planning is (unit: 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。The initial position of the robot is selected as [x,y,z] ^T = [10,-5,1] ^T (m), the initial heading is ψ=π/4(rad), and the pitch angle θ=0(rad), The initial velocity of the robot is [u,v,w] ^T ＝0(m/s), the initial angular velocity q＝0(rad/s), r＝0(rad/s), the controller parameter k _x ＝ky _y ＝k _z 2 =, 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；In order to avoid direct analytic derivation of the virtual control variable and introduce complex calculation process, this paper uses the characteristics of the second-order filter to take the ideal virtual control variable a _co as the reference input of the filter, and the signal is obtained through the process of integration rather than differentiation, which can greatly reduce the influence of measurement noise in the control system based on state feedback design. Assuming that a _co bandwidth is known, by selecting a large enough natural angular frequency ω _n able to obtain a _c and And ensure that the approximation error |α _co (t)-α _c (t) | is very small; at the same time, choosing too large ω _n will increase the influence of high-frequency noise in the system, which requires comprehensive consideration, and choosing a reasonable ω _n satisfies the control Performance, here choose ζ=0.9, ω _n =20;

图2～图13给出机器人三维曲线路径跟踪控制仿真对比结果。Figures 2 to 13 show the comparison results of the three-dimensional curve path tracking control simulation of the robot.

图2为机器人三维螺旋下潜路径跟踪轨迹，图3和图4分别为机器人三维路径跟踪轨迹在XY平面和XZ平面的投影曲线。从中可以看出由于常规反步法基于精确数学模型设计的控制器应用于真实模型时，直接对虚拟控制求导获得其导数得解析形式，在存在模型不确定性和测量噪声时控制效果较差，而本文基于滤波器设计的非线性控制器，通过积分过程而非微分过程获得虚拟控制的滤波值和导数值因而对测量噪声具有一定的滤波作用，通过滤波补偿系统，能够保证理想虚拟控制量的滤波值对真实状态的逼近，进而补偿标称模型的状态响应与真实模型状态响应的偏差，对模型不确定性具有较好的鲁棒性，能够很好的实现跟踪控制，提高了跟踪精度。Fig. 2 is the tracking trajectory of the robot's three-dimensional spiral dive path, and Fig. 3 and Fig. 4 are the projection curves of the robot's three-dimensional path tracking trajectory on the XY plane and the XZ plane, respectively. It can be seen that when the controller designed by the conventional backstepping method based on the precise mathematical model is applied to the real model, the analytic form of its derivative is directly derived from the virtual control, and the control effect is poor when there is model uncertainty and measurement noise. , and the nonlinear controller based on the filter design in this paper obtains the filter value and derivative value of the virtual control through the integral process rather than the differential process, so it has a certain filtering effect on the measurement noise. Through the filter compensation system, the ideal virtual control quantity can be guaranteed Approximation of the filtered value to the real state, and then compensate the deviation between the state response of the nominal model and the state response of the real model, which has good robustness to model uncertainty, can realize tracking control well, and improves tracking accuracy .

图5为机器人三维路径跟踪控制中跟踪误差曲线，与常规反步法控制器相比，可以看出本文设计的三维路径控制器提高了路径跟踪的精度，缩短了机器人的冗余航程，具有更加稳定的控制能力保证机器人较快的跟踪并收敛到期望路径，使得跟踪误差最终收敛到零，表明了控制器具有良好的跟踪精度和响应速度。Figure 5 is the tracking error curve in the three-dimensional path tracking control of the robot. Compared with the conventional backstepping controller, it can be seen that the three-dimensional path controller designed in this paper improves the accuracy of path tracking, shortens the redundant voyage of the robot, and has more The stable control ability ensures that the robot can quickly track and converge to the desired path, so that the tracking error eventually converges to zero, which shows that the controller has good tracking accuracy and response speed.

图6和图7分别为机器人三维里路径跟踪控制过程中各状态变量包括线速度和姿态角的变化曲线，可以看出机器人在沿螺旋线下潜过程中横向速度和垂向速度相比于纵向速度较小，且为有界值，常规反步法设计过程无法处理系统状态的测量噪声，而本文基于滤波器设计的控制器对系统的测量噪声具有一定的滤波效果。Fig. 6 and Fig. 7 are respectively the change curves of various state variables including linear velocity and attitude angle during the path tracking control process of the robot in three dimensions. It can be seen that the lateral velocity and vertical velocity of the robot are compared with the The speed is small and has a bounded value. The conventional backstepping design process cannot deal with the measurement noise of the system state, but the controller based on the filter design in this paper has a certain filtering effect on the measurement noise of the system.

图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最终收敛到零。Figure 8 is the response curve of the robot's longitudinal velocity u, the ideal virtual control variable u _co and its filtered signal u _c . From the partially enlarged diagram, it can be seen that the filtered signal u _c tracks the ideal virtual signal u _co well. The measurement noise contained in the state u has a certain filtering effect, and u _bs is a filter compensation item. When the tracking system is stable, u _bs is finally stable and converges to zero. Figure 9 shows the response curves of the robot's yaw angular velocity r, the ideal virtual control value r _co and its filtered signal r _c . From the partially enlarged figure, it can be seen that the filtered signal r _c tracks the ideal virtual signal r _co well, and the filter The measurement noise contained in the yaw rate r has a certain filtering effect, r _bs is a filter compensation item, and r _bs eventually converges to zero when the tracking system is stable.

图10为机器人纵倾角速度q、理想虚拟控制量q_co和其滤波信号q_c的响应曲线，从局部放大图中可以看出滤波信号q_c较好的跟踪了理想虚拟信号q_co，滤波器对于艏摇角速度q包含的测量噪声具有一定的滤波作用，q_bs为滤波补偿项，当跟踪系统稳定时q_bs最终稳定收敛于零。Fig. 10 is the response curve of the robot pitch angular velocity q, the ideal virtual control quantity q _co and its filtered signal q _c . From the partially enlarged figure, it can be seen that the filtered signal q _c tracks the ideal virtual signal q _co well, and the filter The measurement noise contained in the yaw rate q has a certain filtering effect, and q _bs is a filter compensation item. When the tracking system is stable, q _bs eventually converges to zero stably.

图11和12分别为滤波反步法设计中的机器人艏摇角和纵倾角、理想控制信号和滤波信号变化曲线，从局部放大图可以看出滤波信号ψ_c和θ_c较好的跟踪了理想虚拟信号ψ_co和θ_co，滤波器对于艏摇角ψ和纵倾角θ中包含的测量噪声具有一定的滤波作用，从滤波补偿项ψ_bs和θ_bs的变化趋势可以看出，当跟踪系统稳定时ψ_bs和θ_bs将最终收敛到零点。Figures 11 and 12 are the change curves of the _yaw angle and pitch angle of the robot, the ideal control signal and the filtered signal in the design of the filtered _backstepping method respectively. The virtual signals ψ _co and θ _co , the filter has a certain filtering effect on the measurement noise contained in the yaw angle ψ and the pitch angle θ. From the change trend of the filter compensation items ψ _bs and θ _bs , it can be seen that when the tracking system is stable When ψ _bs and θ _bs will eventually converge to zero.

图13为机器人三维路径跟踪控制输入响应，由图可知本发明的方法响应曲线更加平滑。Fig. 13 is the input response of the three-dimensional path tracking control of the robot. It can be seen from the figure that the response curve of the method of the present invention is smoother.

Claims

1. the underwater robot three-dimensional path tracking and controlling method based on second order filter, it is characterised in that comprise the steps of

Step 1. sets up fixed coordinate system, robot carrier coordinate system and Serret-Frenet coordinate system, obtains expected path, and underwater robot starts path trace, completes the initialization of two second order filters；

Fixed sonar sensor that step 2. is carried by underwater robot, attitude transducer, gather underwater robot current location, attitude angle, angular velocity and speed data information, and in conjunction with the direction of expected path and speed, guide thought according to the angle of sight and calculate and obtain the desirable attitude control quantity ψ of underwater robot_co、θ_co, and ideal velocity controlled quentity controlled variable u_co；

The desirable controlled quentity controlled variable ψ that step 3. will obtain in step 2_co、θ_co、u_coInput, to the second order filter set up based on underwater robot three-dimensional path pursuit movement error model, obtains attitude and the rate controlling amount ψ of underwater robot_c、θ_c、u_c, and derivativeIn conjunction with robot motion variable ψ, θ, u, obtain filtering attitude and the speed Tracking margin of error With desirable angle rate controlling amount r_co、q_co；

The underwater robot desirable angle rate controlling amount r that step 4. will obtain in step 3_co、q_coInput, to another second order filter set up based on underwater robot three-dimensional path pursuit movement error model, obtains the angular velocity controlled quentity controlled variable r of underwater robot_c、q_c, and derivativeIn conjunction with robot angular movement variable r, q, obtain filtering angular velocity tracking error amount

Step 5. utilizes the filtering attitude and the speed Tracking margin of error that obtain in step 3And the filtering angular velocity tracking error amount obtained in step 4Resolving obtains underwater robot propeller thrust F_u, with diving-plane angleVertical rudder angle δ_r, it is respectively acting on robot propeller and steering wheel, it is achieved three-dimensional path tracing control；

Step 6. utilizes the underwater robot attitude and rate controlling amount ψ that obtain in step 3_c、θ_c、u_c, filtering attitude and the speed Tracking margin of errorWith desirable angle rate controlling amount r_co、q_co, the angular velocity controlled quentity controlled variable r of the underwater robot obtained in integrating step 4_c、q_c, and filtering angular velocity tracking error amountStructure filtering error compensates loop；

Step 7. calculates current underwater robot position ηⁿ=(x, y, z) with the turning point WP demarcated_k=(x_k,y_k,z_k) between distanceIf less than the switching radius R set, then it represents that complete the tracing task in currently assigned path, otherwise continue step 2.

2. the underwater robot three-dimensional path tracking and controlling method based on second order filter according to claim 1, it is characterised in that the desirable attitude control quantity ψ of underwater robot involved in step 2_co、θ_co, and ideal velocity controlled quentity controlled variable u_coCalculation expression be:

ψ_{c o} = - a r c s i n (k_{2} e / \sqrt{1 + {(k_{2} e)}^{2}}) - - - (1)

θ_{c o} = a r c s i n (k_{3} h / \sqrt{1 + {(k_{3} h)}^{2}}) - - - (2)

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

Wherein gain factor k₁> 0, k₂> 0, k₃> 0 is angle of sight guidance law normalized parameter, and variable s, e and h represent robot and the forward direction of expected path reference point, transverse direction and vertical tracking error under robot carrier coordinate system respectively.

3. the underwater robot three-dimensional path tracking and controlling method based on second order filter according to claim 1, it is characterised in that step 3, underwater robot three-dimensional path pursuit movement error model involved in 4 be:

\{\begin{matrix} \overset{\cdot}{s} = r e - q h + u - u_{r} {cosψ}_{e} {cosθ}_{e} \\ \overset{\cdot}{e} = - r s + u_{r} {sinψ}_{e} {cosθ}_{e} + v \\ \overset{\cdot}{h} = q s - u_{r} {sinθ}_{e} + w \end{matrix} - - - (4)

\{\begin{matrix} {\overset{\cdot}{ψ}}_{e} = \frac{r}{c o s θ} - r_{F} \\ {\overset{\cdot}{θ}}_{e} = q - q_{F} \end{matrix} - - - (5)

Wherein ψ_e=ψ-ψ_F, θ_e=θ-θ_F,Robot longitudinal velocity u, lateral velocity v and vertical velocity w, yaw angle speed r and pitch velocity q, u_rFor the desired speed of virtual guide point on expected path to be designed, its direction is along the tangential direction of curved path；ψ_FFor u_rThe angle of velocity attitude and fixed coordinate system trunnion axis, θ_FFor u_rThe angle of velocity attitude and fixed coordinate system vertical axis；ψ be robot bow to angle, θ is robot Angle of Trim.

4. the underwater robot three-dimensional path tracking and controlling method based on second order filter according to claim 1, it is characterised in that the underwater robot propeller thrust F related in step 5_u, with diving-plane angle δ_s, vertical rudder angle δ_rExpression formula be:

\{\begin{matrix} F_{u} = m_{1} (- k_{u} \tilde{u} - k_{i u} ϵ_{1} + {\overset{\cdot}{u}}_{c} - u_{b s}) - f_{u} \\ δ_{s} = b_{1}^{- 1} [m_{4} (- k_{q} \overset{&OverBar;}{q} - k_{i q} ϵ_{2} + {\overset{\cdot}{q}}_{c} - q_{b s}) - f_{q}] \\ δ_{r} = b_{2}^{- 1} [m_{5} (- k_{r} \tilde{r} - k_{i r} ϵ_{3} + {\overset{\cdot}{r}}_{c} - r_{b s}) - f_{r}] \end{matrix} - - - (6)

Whereinf_u、f_qAnd f_rFor model nonlinear hydrodynamic force item；U_bs、q_bsAnd r_bsFor feedback compensation robust item；M₁、m₄、m₅The additional mass respectively produced by fluid；K_u、k_q、k_r、k_iu、k_iqAnd k_irIt is controller parameter；

Involved underwater human occupant dynamic model is:

\overset{\cdot}{u} = \frac{m_{2}}{m_{1}} v r - \frac{m_{3}}{m_{1}} w q - \frac{d_{1}}{m_{1}} u + \frac{1}{m_{1}} F_{u} + ω_{1}

\overset{\cdot}{v} = - \frac{m_{1}}{m_{2}} u r - \frac{d_{2}}{m_{2}} v + ω_{2}

\overset{\cdot}{w} = \frac{m_{1}}{m_{3}} u q - \frac{d_{3}}{m_{3}} w + g_{1} + ω_{3} - - - (7)

\overset{\cdot}{q} = \frac{m_{3} - m_{1}}{m_{5}} u w - \frac{d_{4}}{m_{5}} q - g_{2} + \frac{1}{m_{5}} b_{1} δ_{s} + ω_{4}

\overset{\cdot}{r} = \frac{m_{1} - m_{2}}{m_{6}} u v - \frac{d_{5}}{m_{6}} r + \frac{1}{m_{6}} b_{2} δ_{r} + ω_{5}

Wherein

m_{1} = m - X_{\overset{\cdot}{u}}, m_{2} = m - Y_{\overset{\cdot}{v}}, m_{3} = m - Z_{\overset{\cdot}{w}}

m_{5} = I_{y} - M_{\overset{\cdot}{q}}, m_{6} = I_{z} - N_{\overset{\cdot}{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_{δ_{s}}, b_{2} = u^{2} N_{δ_{r}}

Wherein, m and m_(·)Represent robot quality and the additional mass produced by fluid matasomatism, I respectively_yFor the robot rotary inertia around y-axis, I_zFor the robot rotary inertia around z-axis, X_(·)、Y_(·)、Z_(·)、M_(·)And N_(·)For viscous fluid hydrodynamic force coefficient；Z_gAnd z_bThe respectively coordinate position of center of gravity and centre of buoyancy on vertical axis under carrier coordinate, W and B represents the gravity and buoyancy, d that robot is subject to respectively_(·)For nonlinear dampling hydrodynamic force item,WithFor hydroplane and vertical rudder steerage coefficient, ω_(·)It is expressed as interference effect item.

5. the underwater robot three-dimensional path tracking and controlling method based on second order filter according to claim 1, it is characterised in that the filtering error related in step 6 compensates the expression formula of error compensation robust item in loop and is:

ψ_{b s} = g^{T} (\tilde{ψ}) B^{T} u_{r} [\begin{matrix} {&upsi;}_{x} \\ {&upsi;}_{y} \\ {&upsi;}_{z} \end{matrix}] - - - (8)

θ_{b s} = g^{T} (\tilde{θ}) C^{T} u_{r} [\begin{matrix} {&upsi;}_{x} \\ {&upsi;}_{y} \\ {&upsi;}_{z} \end{matrix}] - - - (9)

u_{b s} = \frac{1}{p_{21}} A^{T} [\begin{matrix} {&upsi;}_{x} \\ {&upsi;}_{y} \\ {&upsi;}_{z} \end{matrix}] - - - (10)

r_{b s} = \frac{1}{p_{23}} \frac{{&upsi;}_{ψ}}{c o s θ} - - - (11)

q_{b s} = \frac{1}{p_{22}} {&upsi;}_{θ} - - - (12)

Wherein, p₂₁、p₂₂、p₂₃For the element in the dematrix of Lyapunov Equation；

A = [\begin{matrix} 1 \\ 0 \\ 0 \end{matrix}], B = [\begin{matrix} {cosθ}_{e} {cosψ}_{c} & - {cosθ}_{e} {sinψ}_{c} \\ {cosθ}_{c} {sinψ}_{c} & {cosθ}_{c} {cosψ}_{c} \\ 0 & 0 \end{matrix}], C = [\begin{matrix} {cosψ}_{c} {cosθ}_{c} & - {cosψ}_{c} {sinθ}_{c} \\ {sinψ}_{e} {cosθ}_{c} & - {sinψ}_{e} {sinθ}_{c} \\ - {sinθ}_{c} & - {cosθ}_{c} \end{matrix}],

g (\tilde{ψ}) = [\begin{matrix} \frac{c o s \tilde{ψ} - 1}{\tilde{ψ}} \\ \frac{s i n \tilde{ψ}}{\tilde{ψ}} \end{matrix}], g (\tilde{θ}) = [\begin{matrix} \frac{c o s \tilde{θ} - 1}{\tilde{θ}} \\ \frac{s i n \tilde{θ}}{\tilde{θ}} \end{matrix}],

And meet

\lim_{\tilde{ψ} &RightArrow; 0} g (\tilde{ψ}) = [\begin{matrix} 0 \\ 1 \end{matrix}], \lim_{\tilde{θ} &RightArrow; 0} g (\tilde{θ}) = [\begin{matrix} 0 \\ 1 \end{matrix}];

Involved position filtering signal compensation error is

[\begin{matrix} {&upsi;}_{x} \\ {&upsi;}_{y} \\ {&upsi;}_{z} \end{matrix}] = [\begin{matrix} \tilde{s} - ζ_{x} \\ \tilde{e} - ζ_{y} \\ \tilde{h} - ζ_{z} \end{matrix}] - - - (13)

In formula, position filtering compensates and dynamically measures ζ_x、ζ_yAnd ζ_zExpression formula is

[\begin{matrix} {\overset{\cdot}{ζ}}_{x} \\ {\overset{\cdot}{ζ}}_{y} \\ {\overset{\cdot}{ζ}}_{z} \end{matrix}] = [\begin{matrix} {rζ}_{y} - {qζ}_{z} \\ - {rζ}_{x} \\ {qζ}_{x} \end{matrix}] [\begin{matrix} - k_{x} ζ_{x} \\ - k_{y} ζ_{y} \\ - k_{z} ζ_{z} \end{matrix}] + [\begin{matrix} {\overset{\cdot}{s}}_{c} - {\overset{\cdot}{s}}_{c o} \\ {\overset{\cdot}{e}}_{c} - {\overset{\cdot}{e}}_{c o} \\ {\overset{\cdot}{h}}_{c} - {\overset{\cdot}{h}}_{c o} \end{matrix}] + u_{r} [\begin{matrix} A & B g (\tilde{ψ}) & C g (\tilde{θ}) \end{matrix}] [\begin{matrix} ζ_{u} \\ ζ_{ψ} \\ ζ_{θ} \end{matrix}] - - - (14)

And have ζ_x(0)=0, ζ_y(0)=0, ζ_z(0)=0；S_co、e_co、h_coFor expectation position signalling, s_c、e_c、h_cFor position control signal；K_x=k_y=k_z=2；

The attitude signal compensation dosage of involved wave filter output is:

[\begin{matrix} {&upsi;}_{ψ} \\ {&upsi;}_{θ} \end{matrix}] = [\begin{matrix} \tilde{ψ} - ζ_{ψ} \\ \tilde{θ} - ζ_{θ} \end{matrix}] - - - (15)

In formula, filtering attitude compensates Expression formula and is:

{\overset{\cdot}{ζ}}_{ψ} = - k_{ψ} ζ_{ψ} + \frac{r_{c} - r_{c o}}{c o s θ} + \frac{ζ_{r}}{\cos θ} - - - (16)

And have ζ_ψ(0)=0, ζ_θ(0)=0, ζ_r=0, ζ_q=0；K_ψ=k_θ=5.