CN105527967A - 一种auv反演对接控制方法 - Google Patents

Abstract

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

Description

一种AUV反演对接控制方法

技术领域

本发明属于水下航行器回收对接技术领域，具体涉及一种AUV反演对接控制方法。

背景技术

AUV(AutonomousUnderwaterVehicle)作为一种水下自主运载工具，依靠自身所携带的能源航行，可完成水下测量、物品投送等任务，在军事海洋侦察、海洋环境监测、油田勘测等领域有着重要的作用。AUV执行任务时携带的能源有限，对于长时间作业的AUV，必须在执行任务的区域通过作业母船对AUV进行布放和回收，以便完成补充能源、读取信息、维护保障等工作。为了提高作业效率，人们提出了AUV的水下自主回收系统，自主回收控制已经成为当前海洋工程领域的研究热点之一。

目前，AUV回坞导引方法主要有：

(1)基于横向跟踪控制的回坞导引方法。该方法首先需要定义一条AUV回收的中轴线，然后AUV根据当前位置与中轴线之间的距离偏差来控制AUV沿中轴线航行，最终实现AUV的回收。该方法的主要缺点是不适合初始位置在侧面或后面的情况，也不适用于AUV的动态回收。

(2)基于人工势场的回坞导引方法。该方法采用传统的人工势场法，产生一条可到达回收站的无碰航路，然后控制AUV沿航路航行，从而实现AUV的回收。该方法的主要缺点是存在着局部极小点的问题，即AUV在靠近回收站时同时受到引力和斥力，合力可能为零，一旦AUV陷入局部极小点就会出现回收站不可达问题，且在狭窄区域易发生路径抖动等现象。

(3)基于模糊理论的AUV回收运动方法。依据模糊控制理论的基础，并根据控制对象的特点，制定模糊控制规则，设计出模糊控制器，对AUV的回收运动过程进行控制。该方法的主要缺点是模糊控制规则的获取以及输入模糊变量论域和隶属函数的确定都是实验分析和操作人员经验的汇总，受人为主观因素的影响较大。

发明内容

要解决的技术问题

为了避免现有技术的不足之处，本发明提出一种AUV反演对接控制方法，在AUV靠近对接站的过程中，AUV通过USBL定位系统获取应答器在体坐标系中的位置,AUV应答器位置测量方程式对时间求导，在AUV体坐标系中定义对接误差。对接过程中AUV处于全驱动航行模式，针对对接误差，设计非线性反演控制器，使AUV沿着期望的直线航迹航行直至抵达对接站。

技术方案

一种AUV反演对接控制方法，其特征在于：USBL收发器安装在AUV的头部，两个USBL应答器B₁和B₂安装在回收站的两侧，AUV通过USBL定位系统获取应答器在体坐标系中的位置；对接控制步骤如下：

步骤1：AUV在回收坐标系xoy下从初始点X(0)[x0y0]^T以初始航向角ψ(0)开始进行回坞导引，其中：x、y为AUV在回收坐标系下的位置坐标，(x(0),y(0))为AUV航行之前设定的初始值；

所述回收坐标系xoy是：以回坞面对水面的侧边为y轴，侧边的中心为o点，与侧边垂直的回坞中心轴线为x轴；

步骤2：当接收到AUV的USBL收发器的测量信息，从中提取AUV通过USBL定位系统获取应答器在体坐标系中的位置，表示为：

$\begin{array}{cc}\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\end{array}$

其中：为获得的USBL应答器位置测量，l为收发器与体坐标系原点的距离；

i＝1,2和i＝1,2分别为应答器在回收坐标系和体坐标系下的坐标；

所述体坐标系是：以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}$

在航行过程中需满足： $\begin{array}{cc}{x}_{T1}^B-x_{T2}^B=0,& y_{T1}^B+y_{T2}^B=0\end{array}$

在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为常参数正定增益矩阵。

有益效果

本发明提出的一种AUV反演对接控制方法，在AUV靠近对接站的过程中，AUV通过USBL定位系统获取应答器在体坐标系中的位置,AUV应答器位置测量方程式对时间求导，在AUV体坐标系中定义对接误差。对接过程中AUV处于全驱动航行模式，针对对接误差，设计非线性反演控制器，使AUV沿着期望的直线航迹航行直至抵达对接站。该方法反演自适应速度跟踪在存在参数不确定性时具有良好的速度跟踪性能，对接效果良好。

附图说明

图1：AUV对接系统示意图

图2：AUV航行轨迹

图3：对接误差曲线

图4：预测对接速度指令与实际速度曲线

图5：对接控制输入曲线

具体实施方式

现结合实施例、附图对本发明作进一步描述：

如图1所示，其特征在于AUV的头部安装有USBL收发器，回收站的两侧安装两个USBL应答器B₁和B₂。

实施示例：

步骤1：AUV在回收坐标系xoy下从初始点(-200,-30)以初始航向角30°开始进行回坞导引。初始前向速度和侧向速度分别为0.8m/s和0m/s，角速度为0m/s。

步骤2：应答器B₁和B₂在全局坐标系下的坐标为(0,-1)和(0,1)。

步骤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}\\ \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}$

步骤4：计算对接误差。

在AUV体坐标系中定义对接误差，即在航行过程中需满足：

$\begin{array}{cc}{x}_{T1}^B-x_{T2}^B=0,& y_{T1}^B+y_{T2}^B=0\end{array}$

$e=\left[\begin{array}{c}{x}_{T1}^B-x_{T2}^B\\ y_{T1}^B+y_{T2}^B\end{array}\right];$ 定义为对接误差矢量。

步骤5：计算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].$

步骤6：为设计反演控制律，选择参考速度输入为：

$v_d=\left[\begin{array}{c}{v}_d\\ r_d\end{array}\right]=-L^{-1}{\mathrm{\Γ}}_{1}e$

其中，Γ₁＝diag(0.06,1)为常数正定增益矩阵，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]$

步骤7：为设计反演控制律，选择参考速度输入为：

${\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].$

步骤8：定义速度跟踪误差为：

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}$

步骤9：计算控制力，采用PI控制

τ＝[Y,N]为输入向量，考虑到θ矩阵部分未知，因此设计如下控制输入使得负定：

τ＝-Γ₂e₁-L^Te

其中，Γ₂＝diag(50,10)。

通过仿真可以看出，反演自适应速度跟踪在存在参数不确定性时具有良好的速度跟踪性能，对接效果良好，验证了算法的有效性。

Claims (1)

1.一种AUV反演对接控制方法，其特征在于：USBL收发器安装在AUV的头部，两个USBL应答器B₁和B₂安装在回收站的两侧，AUV通过USBL定位系统获取应答器在体坐标系中的位置；对接控制步骤如下：

步骤1：AUV在回收坐标系xoy下从初始点X(0)＝[x0y0^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为常参数正定增益矩阵。