[发明专利]一种AUV反演对接控制方法

摘要

本发明涉及一种AUV反演对接控制方法，属于水下航行器回收对接技术领域。其特征在于AUV的头部安装有USBL收发器，回收站的两侧安装两个USBL应答器B₁和B₂；在AUV靠近对接站的过程中，AUV通过USBL定位系统获取应答器在体坐标系中的位置,AUV应答器位置测量方程式对时间求导，在AUV体坐标系中定义对接误差。对接过程中AUV处于全驱动航行模式，针对对接误差，设计非线性反演控制器，使AUV沿着期望的直线航迹航行直至抵达对接站。该方法反演自适应速度跟踪在存在参数不确定性时具有良好的速度跟踪性能，对接效果良好。

主权项

一种AUV反演对接控制方法，其特征在于：USBL收发器安装在AUV的头部，两个USBL应答器B₁和B₂安装在回收站的两侧，AUV通过USBL定位系统获取应答器在体坐标系中的位置；对接控制步骤如下：步骤1：AUV在回收坐标系xoy下从初始点X(0)＝[x 0 y 0^T以初始航向角ψ(0)开始进行回坞导引，其中：x、y为AUV在回收坐标系下的位置坐标，(x(0),y(0))为AUV航行之前设定的初始值；所述回收坐标系xoy是：以回坞面对水面的侧边为y轴，侧边的中心为o点，与侧边垂直的回坞中心轴线为x轴；步骤2：当接收到AUV的USBL收发器的测量信息，从中提取AUV通过USBL定位系统获取应答器在体坐标系中的位置，表示为：$\left[\begin{array}{c}{x}_{Ti}^B\\ y_{Ti}^B\end{array}\right]=\left[\begin{array}{c}{\stackrel{\‾}{x}}_{Ti}\\ {\stackrel{\‾}{y}}_{Yi}\end{array}\right]+\left[\begin{array}{c}l\\ 0\end{array}\right]=\left[\begin{array}{cc}cos\mathrm{\ψ}& sin\mathrm{\ψ}\\ -sin\mathrm{\ψ}& cos\mathrm{\ψ}\end{array}\right]\left(\left[\begin{array}{c}{x}_{Ti}^D\\ y_{Ti}^D\end{array}\right]-\left[\begin{array}{c}x\\ y\end{array}\right]\right),i=1,2$其中：为获得的USBL应答器位置测量，l为收发器与体坐标系原点的距离；和分别为应答器在回收坐标系和体坐标系下的坐标；所述体坐标系是：以AUV的浮心为O_B，AUV的纵轴为x_B轴，与纵轴垂直的为y_B；步骤3计算对接误差：AUV应答器位置测量方程式对时间求导：$\begin{array}{c}\left[\begin{array}{c}{\stackrel{\·}{x}}_{Ti}^B\\ {\stackrel{\·}{y}}_{Ti}^B\end{array}\right]=\left[\begin{array}{cc}cos\mathrm{\ψ}& sin\mathrm{\ψ}\\ -sin\mathrm{\ψ}& cos\mathrm{\ψ}\end{array}\right]\left(\left[\begin{array}{c}{\stackrel{\·}{x}}_{Ti}^D\\ {\stackrel{\·}{y}}_{Ti}^D\end{array}\right]-\left[\begin{array}{c}\stackrel{\·}{x}\\ \stackrel{\·}{y}\end{array}\right]\right)+r\left[\begin{array}{cc}-\sin \mathrm{\ψ}& \cos \mathrm{\ψ}\\ -\cos \mathrm{\ψ}& -\sin \mathrm{\ψ}\end{array}\right]\left(\left[\begin{array}{c}{x}_{Ti}^D\\ y_{Ti}^D\end{array}\right]-\left[\begin{array}{c}x\\ y\end{array}\right]\right)\\ =\begin{array}{cc}\left[\begin{array}{ccc}-1& 0& y_{Ti}^B\\ 0& -1& -x_{Ti}^B\end{array}\right]\left[\begin{array}{c}u\\ v\\ r\end{array}\right]& i=1,2\end{array}\end{array}$在航行过程中需满足：$x_{T1}^B-x_{T2}^B=0,y_{T1}^B+y_{T2}^B=0$在AUV体坐标系中的对接误差，$e=\left[\begin{array}{c}{x}_{T1}^B-x_{T2}^B\\ y_{T1}^B+y_{T2}^B\end{array}\right];$其中：定义了AUV与对接路径平行，定义了AUV指向对接坐标系原点；步骤4计算L矩阵：对接误差矢量对时间求导：$\stackrel{\·}{e}=\left[\begin{array}{c}{\stackrel{\·}{x}}_{T1}^B-{\stackrel{\·}{x}}_{T2}^B\\ {\stackrel{\·}{y}}_{T1}^B+{\stackrel{\·}{y}}_{T2}^B\end{array}\right]=\left[\begin{array}{c}r(y_{T1}^B-y_{T2}^B)\\ -2v-r(x_{T1}^B+x_{T2}^B)\end{array}\right]=Lv$其中：v＝[v,r]^T是AUV速度矢量，L为应答器位置测量和体坐标系速度矢量间的相互关系矩阵，并有：$L=\left[\begin{array}{cc}0& y_{T1}^B-y_{T2}^B\\ -2& -(x_{T1}^B+x_{T2}^B)\end{array}\right];$步骤5计算速度指令：$v_d=\left[\begin{array}{c}{v}_d\\ r_d\end{array}\right]=-L^{-1}{\mathrm{\Γ}}_{1}e$其中：Γ₁＝diag(γ₁,γ₂)为常数正定增益矩阵，L^‑1为L的逆，并有：$L^{-1}=\left[\begin{array}{cc}-\frac{x_{T1}^B+x_{T2}^B}{2(y_{T1}^B-y_{T2}^B)}& -\frac{1}{2}\\ \frac{1}{y_{T1}^B-y_{T2}^B}& 0\end{array}\right]$步骤6：选择参考速度输入为：${\stackrel{\·}{v}}_d=-L^{-1}({\mathrm{\Γ}}_{1}Lv+\stackrel{\·}{L}{v}_d)$其中为：$\stackrel{\·}{L}=\left[\begin{array}{cc}0& r(x_{T2}^B-x_{T1}^B)\\ 0& 2u-r(y_{T1}^B+y_{T2}^B)\end{array}\right];$步骤7：速度跟踪误差为：e₁＝v‑v_d求导得：$\begin{array}{c}{\stackrel{\·}{e}}_1=\stackrel{\·}{v}-{\stackrel{\·}{v}}_d\\ =\left[\begin{array}{c}-{\stackrel{\·}{v}}_d-\frac{m_{11}}{m_{22}}ur-\frac{d_{22}}{m_{22}}v+\frac{1}{m_{22}}(Y+Y_c)\\ -{\stackrel{\·}{r}}_d+\frac{m_{11}-m_{22}}{m_{33}}uv-\frac{d_{33}}{m_{33}}r+\frac{1}{m_{33}}(N+N_c)\end{array}\right]\end{array};$步骤8：计算控制力，采用PI控制τ＝[Y,N]为输入向量，考虑到θ矩阵部分未知，控制输入使得负定：τ＝‑Γ₂e₁‑L^Te其中，Γ₂＞0为常参数正定增益矩阵。