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

权利要求书

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 定位系统获取应答器在体坐标系中的位置，表示为：

xTiByTiB=x‾Tiy‾Yi+l0=cosψsinψ-sinψcosψ(xTiDyTiD-xy),i=1,2]]>

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

和分别为应答器在回收坐标系和体坐标系下 的坐标；

所述体坐标系是：以AUV的浮心为O_B，AUV的纵轴为x_B轴，与纵轴垂直的为y_B；

步骤3计算对接误差：

AUV应答器位置测量方程式对时间求导：

x·TiBy·TiB=cosψsinψ-sinψcosψ(x·TiDy·TiD-x·y·)+r-sinψcosψ-cosψ-sinψ(xTiDyTiD-xy)=-10yTiB0-1-xTiBuvri=1,2]]>

在航行过程中需满足：xT1B-xT2B=0,yT1B+yT2B=0]]>

在AUV体坐标系中的对接误差，e=xT1B-xT2ByT1B+yT2B;]]>

其中：定义了AUV与对接路径平行，定义了AUV指向对 接坐标系原点；

步骤4计算L矩阵：

对接误差矢量对时间求导：

e·=x·T1B-x·T2By·T1B+y·T2B=r(yT1B-yT2B)-2v-r(xT1B+xT2B)=Lv]]>

其中：v＝[v,r]^T是AUV速度矢量，L为应答器位置测量和体坐标系速度矢量间 的相互关系矩阵，并有：L=0yT1B-yT2B-2-(xT1B+xT2B);]]>

步骤5计算速度指令：

vd=vdrd=-L-1Γ1e]]>

其中：Γ₁＝diag(γ₁,γ₂)为常数正定增益矩阵，L^-1为L的逆，并有：

L-1=-xT1B+xT2B2(yT1B-yT2B)-121yT1B-yT2B0]]>

步骤6：

选择参考速度输入为：v·d=-L-1(Γ1Lv+L·vd)]]>

其中为：L·=0r(xT2B-xT1B)02u-r(yT1B+yT2B);]]>

步骤7：速度跟踪误差为：e₁＝v-v_d

求导得：e·1=v·-v·d=-v·d-m11m22ur-d22m22v+1m22(Y+Yc)-r·d+m11-m22m33uv-d33m33r+1m33(N+Nc);]]>

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

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

τ＝-Γ₂e₁-L^Te

其中，Γ₂＞0为常参数正定增益矩阵。