[发明专利]一种基于偶极势场的AUV回坞导引系统及导引方法

权利要求书

1.一种利用基于偶极势场的AUV回坞导引系统导引AUV回坞的方法，所述的基于偶极势场的AUV回坞导引系统：包括USBL收发器和两个USBL应答器；USBL收发器设置于AUV的头部的中心轴线上，两个USBL应答器设置于回收站的两侧；所述两个USBL应答器为电偶极子，其特征在于步骤如下：

步骤1：AUV在回收坐标系xoy下从初始点X(0)＝[x(0)y(0)]_T以初始航向角ψ(0)开始进行回坞导引；其中：x、y为AUV在回收坐标系下的位置坐标，(x(0)y(0))在AUV航行之前进行设定；所述回收坐标系xoy是以回收站的两侧安装的两个USBL应答器B₁和B₂的中心为原点，两个USBL应答器B₁和B₂之间的连线为y轴，水平向上垂直于y轴且过原点的为x的正方向轴，B₁位于B₂的左边，B₁指向B₂的方向为y轴的正方向；

步骤2：AUV上安装的USBL收发器与回收站两侧的应答器B₁和B₂进行通信，测量出应答器B₁和B₂在AUV机体坐标系x_Bo_By_B下的距离L_i和方位μ_i，其中i＝1,2；

步骤3：计算应答器B₁和B₂在AUV机体坐标系x_Bo_By_B下的位置坐标B_i(x_Ti,y_Ti)，i＝1,2：

$\left[\begin{array}{c}{x}_{Ti}\\ y_{Ti}\end{array}\right]=\left[\begin{array}{c}{\mathrm{cos\μ}}_i\\ {\mathrm{sin\μ}}_i\end{array}\right]\[L_i\]+\left[\begin{array}{c}l\\ 0\end{array}\right]$

步骤4：根据电偶极子电场原理，定义电偶极矩矢量应答器中点的位置坐标为由于回收站的入口矢量与垂直，则o为回收坐标系xoy的原点，l为应答器B₁和B₂之间的距离；

步骤5：计算出AUV机体坐标系x_Bo_By_B和AUV回收坐标系xoy之间的夹角：

$\mathrm{\ψ}=arctan\left(\frac{x_{T2}-x_{T1}}{y_{T2}-y_{T1}}\right)$

B_i(x_Ti,y_Ti)是应答器B₁和B₂在AUV机体坐标系x_Bo_By_B下的位置坐标，i＝1,2；

步骤6：计算AUV在回收坐标系xoy下的位置坐标：

$\left[\begin{array}{c}x\\ y\end{array}\right]=-\left[\begin{array}{cc}cos\mathrm{\ψ}& -\sin \mathrm{\ψ}\\ \sin \mathrm{\ψ}& \cos \mathrm{\ψ}\end{array}\right]{(\frac{x_{T1}+x_{T2}}{2},\frac{y_{T1}+y_{T2}}{2})}^T$

步骤7：定义偶极势场F(n)＝λ(p^Tn)n-p(n^Tn),是一个除了原点之外处处非零的二维的矢量场，其中，常量λ≥2，p＝[p_x p_y]^Τ为偶极势场的电偶极矩，n为偶极势场的位置矢量，偶极势场最大的特点是在偶极势场空间中任意一点的运动都沿着势场线的切线方向；

两个应答器在回收坐标系xoy下的位置坐标分别为p₁＝(p_1x,p_1y)和p₂＝(p_2x,p_2y)，则电偶极矩p＝(p_2x-p_1x,p_2y-p_1y)^Τ；

由于AUV在偶极势场中的位置矢量为n＝[x y]^Τ，则偶极势场为

F_nx＝(λ-1)(p_2x-p_1x)x²+λ(p_2y-p_1y)xy-(p_2x-p_1x)y²

F_ny＝(λ-1)(p_2y-p_1y)y²+λ(p_2x-p_1x)xy-(p_2y-p_1y)x²；

步骤8：根据步骤7定义的偶极势场，确定AUV回收的航向角指令为：

${\mathrm{\ψ}}_d=arctan\left(\frac{F_{ny}}{F_{nx}}\right)-arctan\left(\frac{v}{u}\right)$

步骤9：根据步骤8的航向角指令ψ_d，采用自适应非奇异终端滑模控制方法对航向角进行跟踪控制，选择舵角为：

${\mathrm{\δ}}_r=-{\mathrm{\α}}_2({\mathrm{\α}}_0+{\mathrm{\α}}_1\mathrm{\ω}+\mathrm{\β}\frac{q}{p^{\′}}{\mathrm{\ω}}^{2-p^{\′}/q}+({\hat{x}}_0+\mathrm{\η}\left)sgn\right(s\left)\right)$

${\mathrm{\δ}}_{r1}=-{\mathrm{\α}}_2({\mathrm{\α}}_0+{\mathrm{\α}}_{1}w+\mathrm{\β}\frac{q}{p^{\′}}{w}^{2-p/q}+({\hat{x}}_0+\mathrm{\η}\left)\mathrm{sgn}\right(s\left)\right)$

其中，u、v、ω分别是机体坐标系下AUV的前向速度、侧向速度和航向角速度，m₁₁、m₂₂和m₃₃为AUV包含附加质量的惯性系数，d₁₁、d₂₂和d₃₃为AUV的流体动力阻尼系数，N_σ为AUV偏航力矩系数，β、η都为常量且η>0，β>0，p′>q>0且为奇数，x⁰是建模不确定项上界，是x₀的估计值，q₀>0是自适应增益，s为滑模控制量；

步骤10：如果AUV与回收点的距离小于给定的可接受误差时，则转到下一步骤；否则返回到步骤2；

步骤11：AUV已经到达回收点，回坞结束。