基于生物速度调节的欠驱动AUV三维轨迹跟踪控制方法与流程

技术特征：

1.一种基于生物速度调节的欠驱动AUV三维轨迹跟踪控制方法，其特征是：

步骤1、AUV根据当前任务，给定一个期望状态X_d(t)＝[x_d(t),y_d(t),z_d(t),θ_d(t),ψ_d(t)]^T，并通过所搭载的导航设备和传感器获取AUV的实际状态X(t)＝[x(t),y(t),z(t),θ(t),ψ(t)]^T和速度V(t)＝[u(t),v(t),ω(t),q(t),r(t)]^T信息，其中x_d(t),y_d(t),z_d(t)和θ_d(t),ψ_d(t)为地面坐标下AUV的期望位置与姿态，x(t),y(t),z(t)和θ(t),ψ(t)为地面坐标下AUV的实际位置和姿态，u(t),v(t),ω(t)和q(t),r(t)为船体坐标系下的线速度和角速度；

步骤2、利用步骤1中的得到的AUV实际状态，通过欠驱动AUV的数学模型和轨迹跟踪位置误差模型，计算出实际轨迹与期望轨迹之间的位置误差和姿态误差，将地面坐标信息转换为船体坐标信息；

步骤3、基于步骤2中计算出的位置和姿态误差，采用定义虚拟速度误差变量的方法，将姿态跟踪控制转化为速度控制，计算出纵向速度虚拟控制律u_d、纵倾角速度虚拟控制律q_d和艏摇角速度虚拟控制律r_d；

步骤4、计算得出纵向速度误差e_u，纵倾角速度误差e_q和艏摇角速度误差e_r，并将这三个误差分别通入生物启发模型，通过设置生物启发模型中的参数，完成对速度误差的动态调节；

步骤5、根据给定的数学模型和步骤4中经动态调节后得到的输出量，推导欠驱动AUV三维轨迹跟踪的动态速度调节控制器，包括纵向控制力矩τ_u的控制信号、纵倾控制力矩τ_q和艏摇控制力矩τ_r的控制信号，并对当前环境振动进行估计设计了自适应控制律，实现在外界常值扰动下对欠驱动AUV三维轨迹跟踪控制。

2.根据权利要求1所述的基于生物速度调节的欠驱动AUV三维轨迹跟踪控制方法，其特征是所述欠驱动AUV的数学模型包括运动学模型、动力学模型、轨迹跟踪误差模型、位置误差以及期望姿态，表达式分别为:

运动学模型：

$\left\{\begin{array}{c}\stackrel{\·}{x}=ucos\mathrm{\ψ}cos\mathrm{\θ}-vsin\mathrm{\ψ}+wcos\mathrm{\ψ}sin\mathrm{\θ}\\ \stackrel{\·}{y}=usin\mathrm{\ψ}cos\mathrm{\θ}+vcos\mathrm{\ψ}+w\sin \mathrm{\ψ}\sin \mathrm{\θ}\\ \stackrel{\·}{z}=-usin\mathrm{\θ}+w\cos \mathrm{\θ}\\ \stackrel{\·}{\mathrm{\θ}}=q\\ \stackrel{\·}{\mathrm{\ψ}}=r/\cos \mathrm{\θ}\end{array}\right.;$

动力学模型：

$\left\{\begin{array}{c}{m}_{11}\stackrel{\·}{u}=m_{22}vr-m_{33}wq-d_{11}u+{\mathrm{\τ}}_u+{\mathrm{\ω}}_u\\ m_{22}\stackrel{\·}{v}=-m_{11}ur-d_{22}v+{\mathrm{\ω}}_v\\ m_{33}\stackrel{\·}{w}=m_{11}uq-d_{33}w+{\mathrm{\ω}}_w\\ m_{55}\stackrel{\·}{q}=(m_{33}-m_{11})uw-d_{55}q-\mathrm{\ρ}g\▿\stackrel{\‾}{{\mathrm{GM}}_L}sin\mathrm{\θ}+{\mathrm{\τ}}_q+{\mathrm{\ω}}_q\\ m_{66}\stackrel{\·}{r}=(m_{11}-m_{22})uv-d_{66}r+{\mathrm{\τ}}_r+{\mathrm{\ω}}_r\end{array}\right.,$

其中：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|；X_u,X_u|u|,Y_v,Y_v|v|,Z_w，Z_w|w|,M_q,M_q|q|,N_r,N_r|r|为水动力参数；ρ,g,▽,分别为水密度、重力加速度、水容积和纵向稳心高；ω_u、ω_v、ω_w、ω_q、ω_r为外界干扰在AUV运动系各自由度的分量；

位置误差：

$\left\{\begin{array}{c}{x}_e=x-x_d\\ y_e=y-y_d\\ z_e=z-z_d\end{array}\right.;$

轨迹跟踪位置误差模型：

$\left\{\begin{array}{c}{\stackrel{\·}{e}}_x=u+{\mathrm{re}}_y-{\mathrm{qe}}_z-v_p{\mathrm{sin\θ}}_d\sin \mathrm{\θ}-v_t{\mathrm{cos\θcose}}_{\mathrm{\ψ}}\\ {\stackrel{\·}{e}}_y=v-{\mathrm{re}}_x-e_{z}r\tan \mathrm{\θ}+v_t{\mathrm{sine}}_{\mathrm{\ψ}}\\ {\stackrel{\·}{e}}_z=w+{\mathrm{qe}}_x+e_{y}r\tan \mathrm{\θ}+v_p{\mathrm{sin\θ}}_d\cos \mathrm{\θ}-v_t{\mathrm{sin\θcose}}_{\mathrm{\ψ}}\end{array}\right.,$

其中，

期望姿态：

${\mathrm{\ψ}}_d=arctan\left(\frac{{\stackrel{\·}{y}}_d}{{\stackrel{\·}{x}}_d}\right),{\mathrm{\θ}}_d=-arctan\frac{{\stackrel{\·}{z}}_d}{\sqrt{{\stackrel{\·}{x}}_d^2+{\stackrel{\·}{y}}_d^2}}.$

3.根据权利要求1所述的基于生物速度调节的欠驱动AUV三维轨迹跟踪控制方法，其特征是步骤3中所涉及的纵向速度虚拟控制律u_d，纵倾角速度虚拟控制律q_d和艏摇角速度虚拟控制律r_d，以及定义虚拟速度误差变量的表达式分别为：

虚拟速度误差变量：

α＝v_tsine_ψ，β＝v_psine_θ；

纵向速度虚拟控制律：

u_d＝-k₁e_x/e+v_psinθ_dsinθ+v_tcosθcose_ψ，

其中，k₁为正常数，

艏向角、纵倾角速度虚拟控制律：

$\left\{\begin{array}{c}{r}_d=\frac{(-k_4{\mathrm{\α}}_e-e_y+{\stackrel{\·}{\mathrm{\α}}}_d-{\stackrel{\·}{v}}_t{\mathrm{sine}}_{\mathrm{\ψ}})cos\mathrm{\θ}}{v_t}+{\stackrel{\·}{\mathrm{\ψ}}}_{d}cos\mathrm{\θ}\\ q_d=\frac{(-k_5{\mathrm{\β}}_e+e_z+{\stackrel{\·}{\mathrm{\β}}}_d-{\stackrel{\·}{v}}_p{\mathrm{sine}}_{\mathrm{\θ}})}{v_p}+{\stackrel{\·}{\mathrm{\θ}}}_d\end{array}\right..$

4.根据权利要求1所述的基于生物速度调节的欠驱动AUV三维轨迹跟踪控制方法，其特征是所述的纵向速度误差e_u角速，度误差e_r、e_q，生物启发模型的表达式分别为：

纵向速度误差：

e_u＝u-u_d；

角速度误差：

$\left\{\begin{array}{c}{e}_r=r-r_d\\ e_q=q-q_d\end{array}\right.;$

生物启发模型：

$\begin{array}{c}{\stackrel{\·}{S}}_i=-A_j{S}_i+(B_j-S_i)f\left(e_i\right)-(D_j+S_i)g\left(e_i\right)\\ =-\[A_j+f\left(e_i\right)+g\left(e_i\right)\]S_i+\[B_{j}f\left(e_i\right)-D_{j}g\left(e_i\right)\]\end{array},$

其中i＝u,q,r；j＝1,2,3,S_u、S_q和S_r分别为纵向速度误差、纵倾角速度误差和首向角速度误差动态模型的输出；参数A_j是正常数,为动态速度误差输出的衰减率；正常数B_j和D_j分别为动态速度误差输出的上限和下限；e_u、e_q和e_r的值通过分别设置A_j、B_j和D_j以及生物模型本身的特性来完成对速度误差的动态调节，函数f(e_i)＝max{e_i,0},g(e_i)＝max{-e_i,0}。

5.根据权利要求1所述的基于生物速度调节的欠驱动AUV三维轨迹跟踪控制方法，其特征是所述的纵向控制力矩τ_u的控制信号、纵倾控制力矩τ_q和艏摇控制力矩τ_r的控制信号的表达式分别为：

$\left\{\begin{array}{c}{\mathrm{\τ}}_u=m_{11}({\stackrel{\·}{u}}_d-f_u-x_e-S_u)-{\widehat{\mathrm{\ω}}}_u\\ {\mathrm{\τ}}_q=m_{55}({\stackrel{\·}{q}}_d-f_q-\frac{e_{\mathrm{\α}}{v}_t{\mathrm{cose}}_{\mathrm{\ψ}}}{cos\mathrm{\θ}}-S_q)-{\widehat{\mathrm{\ω}}}_q\\ {\mathrm{\τ}}_r=m_{66}({\stackrel{\·}{r}}_d-f_r-e_{\mathrm{\β}}{v}_p{\mathrm{cose}}_{\mathrm{\θ}}-S_r)-{\widehat{\mathrm{\ω}}}_r\end{array}\right.,$

其中，为对当前环境扰动的估计值；

所述的界扰动自适应控制律为：

$\left\{\begin{array}{c}{\stackrel{\·}{\widetilde{\mathrm{\ω}}}}_u=-{\widetilde{\mathrm{\ω}}}_u-e_u/c_1\\ {\stackrel{\·}{\widetilde{\mathrm{\ω}}}}_q=-{\widetilde{\mathrm{\ω}}}_q-e_q/c_2\\ {\stackrel{\·}{\widetilde{\mathrm{\ω}}}}_r=-{\widetilde{\mathrm{\ω}}}_r-e_r/c_3\end{array}\right.,$

其中，c₁,c₂,c₃均是正常数。