欠驱动AUV水下动态目标跟踪中的期望路径构造方法与流程

本发明涉及的是一种水下航行器控制方法，具体地说是一种欠驱动auv的期望路径构造方法。

背景技术：

智能水下无人航行器(autonomousunderwatervehicle，auv)作为高技术水下无人平台，在资源开发、海洋科学、水下救援等领域发挥了重要作用，成为各国重点发展的新型装备。

auv的研究与应用是一个系统性工程，包括任务规划、方案设计、样机研制、布放与回收等，其中回收技术是实现auv安全连续作业的关键技术。auv通常自身携带能源工作，在任务完成后需要通过回收来完成补充能源、下载数据、设备检修等一系列操作。目前主要的回收方式包括水面母船回收和水下对接回收。水下对接回收通过auv与固定或者移动的回收装置实现类似空/天对接的过程，使auv完成返航、接近、对接、锁紧等一系列动作，具备自主、无人等优点，具有广阔的发展前景，在这种背景下，auv的水下自主回收对接技术成为了国内外的一个研究热点。要实现auv与母艇的动态对接，首先要保证auv准确跟踪移动母艇的运动，也就是欠驱动auv的动态目标跟踪问题。

目前现有技术中，对路径的光滑性和约束进行了一定的改进，但较难实现三维路径的跟踪，并且对于目标机动性的研究不够深入。欠驱动auv水下目标跟踪中，由于海流干扰以及应急机动的影响，目标的运动存在机动性。并且由于各种约束的存在，如果单纯将跟踪方向指向当前目标，可能产生欠驱动auv不能达到的运动状态。所以提出一种适用于欠驱动auv水下动态目标跟踪的期望路径规划方法对于改善跟踪品质，提高对接成功率具有重要意义。

技术实现要素：

本发明的目的在于提供一种可以实时更新期望路径曲线，提高auv应对目标不确定性以及瞬时干扰的影响，提高跟踪快速性和精度的欠驱动auv水下动态目标跟踪中的期望路径构造方法。

本发明的目的是这样实现的：

步骤一：基于交互多模型得到当前时刻目标位置的估计值；通过超短基线(usbl)和双目视觉定位系统得到当前时刻目标位置的观测值；根据得到估计值和观测值，通过卡尔曼滤波得到当前时刻目标位置的最优估计值；

步骤二：将当前时刻auv位置与目标位置的最优估计值之间的部分或全部历史目标点作为所选历史目标点序列，选择当前时刻auv位置、所选历史目标点序列、当前时刻目标位置的最优估计值作为期望目标点序列；在所选期望目标点序列的基础上，引入几何约束条件，形成一条光滑的三次样条曲线路径；

步骤三：选择步骤二中的三次样条曲线路径作为当前时刻的期望路径，随着时间的推移，通过步骤一、步骤二期望路径不断更新。

本发明还可以包括：

1、设在k时刻，将auv尚未经过的历史目标点位置信息作为已知信息，设历史目标点序列为(ak-n,ak-n+1,ak-n+2,...,ak-3,ak-2,ak-1)，即auv尚未经过的历史目标点的数目为n，当auv距离目标较远、即n≥10时，取首端和末端各5个点，即

(ak-n,ak-n+1,ak-n+2,ak-n+3,ak-n+4,ak-5,ak-4,ak-3,ak-2,ak-1)为所选历史目标点序列，此时期望目标点序列为当前时刻auv位置、所选历史目标点序列、当前时刻目标位置最优估计值，共12个点；当auv距离目标较近、即n＜10时，则所选历史目标点序列为

(ak-n,ak-n+1,ak-n+2,...,ak-3,ak-2,ak-1)，此时期望目标点序列为当前时刻auv位置、所选历史目标点序列、当前时刻目标位置最优估计值，共n+2个点，，其中ai＝(ξi,ηi,ζi)表示大地坐标系下点的位置坐标，设期望目标点序列数目为m、其序列为(p1,p2,...,pm)，则表示为：

当n≥10时，m＝12；

(p1,p2,...,pm)＝(aauv,ak-n,ak-n+1,ak-n+2,ak-n+3,ak-n+4,ak-5,ak-4,ak-3,ak-2,ak-1,ak)当n＜10时，m＝n+2；

(p1,p2,...,pm)＝(aauv,ak-n,ak-n+1,ak-n+2,...,ak-3,ak-2,ak-1,ak)。

2、三次样条曲线的几何约束包括起始约束与末段约束：

起始段约束要求路径在ηauv处的切线方向与当前时刻的auv艏向一致，且以当前时刻auv的位置为路径起点，即：

其中：p1为第一个期望目标点，(ξauv,ηauv,ζauv)表示当前时刻auv的坐标，表示路径曲线在起点处的斜率，ψ表示auv的艏向向量，α为正实数；

末段约束要求所规划路径的期望艏向不超过坞站锥形开角的一半，且以当前时刻目标位置为路径终点，即

其中：pm为最后一个期望目标点，(ξk,ηk,ζk)表示当前时刻目标的坐标，α坞站为坞站锥形开角大小，ψ为auv艏向角。

本发明提供了一种欠驱动auv水下动态目标跟踪中的期望路径构造方法。针对水下auv与母艇的非合作式动态对接需求，考虑到母艇的运动状态存在机动性以及干扰的存在，提出了一种期望路径设计方法。首先根据交互多模型方法得到当前时刻目标位置的估计值，通过超短基线与双目视觉定位系统得到当前时刻目标位置的观测值，然后通过卡尔曼滤波得到当前时刻目标位置的最优估计值；选择auv位置、所选历史目标点序列、当前时刻目标的最优估计值作为期望目标点序列；引入几何约束条件，根据期望目标点序列构造三次样条曲线，选择该样条曲线作为auv跟踪的期望路径曲线。该方法可以实时更新期望路径曲线，提高auv应对目标不确定性以及瞬时干扰的影响，提高跟踪快速性和精度。

与现有技术相比，本发明的有益效果是：利用目标运动的“记忆性”，将历史目标点序列引入到期望路径设计中，并考虑了路径设计中几何约束条件，使所设计的期望路径更利于欠驱动auv的运动，同时对目标运动状态的估计更加准确，提高了跟踪精度，节省了时间。

附图说明

图1是本发明的流程图；

图2是本发明的目标跟踪示意图；

图3是本发明的仿真数据之三次样条曲线。

具体实施方式

下面举例对本发明做更详细的描述。

结合图1：

1.当开始执行目标跟踪时，在当前时刻，首先通过交互多模型方法得到运动目标的位置估计值，同时通过定位系统(包括超短基线usbl和双目视觉定位系统)得到当前时刻目标位置的测量值；根据卡尔曼滤波原理的到此时目标位置的最优估计值。

2.下一步是选择期望目标点序列，该序列由三部分组成：分别是当前时刻auv位置，步骤1求得的目标位置最优估计值和所选历史目标点序列。

3.期望目标点序列后，根据这么目标点构造一条三次样条曲线，且构造过程基于引入的几何约束条件。

4.通过步骤1.2.3得到的三次样条曲线即期望跟踪路径，当时间进入到下一时刻时，继续从头执行该算法。

如图2所示，针对auv1与母艇5的对接过程，将坞站3安装在母艇5的背部，通过auv1对坞站4的跟踪，auv1进入坞站4完成对接。视坞站4为被动目标，当要执行对接任务时，母艇5将返回对接海域，并做低速运动，其运动模型由交互多模型方法给出。auv1为主动体，通过控制auv1沿着期望的路径运动以实现对接的目的，期望路径通过以下步骤实现：

(1)首先选择常速度模型、水平常速度转弯模型以及singer模型为交互多模型子系统，各模型之间的转移由马尔可夫概率转移矩阵确定，得到当前时刻目标位置的预测值。

(2)通过auv1与母艇5所配备的定位系统获得当前时刻坞站4位置的观测值。具体技术手段为：远距离跟踪阶段2采用声学定位技术，传感器为超短基线系统usbl(作用距离：2000m，测距精度：2％斜距，目标方位测量精度：±2°)，auv1上搭载有usbl水听器，坞站4上搭载usbl信标。为保证母艇5自身的安全，只有auv1上的水听器会主动发声，坞站4上的信标只做应答响应。对于近距离对接阶段3，当光源进入摄像机视觉范围后，定位技术切换为光视觉定位，光视觉定位传感器为水下摄像机(作用距离：0.5m-30m，定位精度：厘米级)。在auv1艏部设置1部下视摄像机和1部与水平呈45°的前下视摄像机，坞站4上搭载规则布置的点光(水下540nm绿光led灯)源。

(3)通过步骤(1)(2)得到当前时刻目标位置的估计值和观测值后，通过卡尔曼滤波得到当前时刻目标位置的最优观测值。

(4)选择步骤(3)中的最优观测值、auv当前位置与所选历史目标点序列作为路径设计的期望目标点序列。历史目标点序列的选取规则为：假设在k时刻，将auv尚未经过的历史目标点位置信息作为已知信息，设历史目标点序列为(ak-n,ak-n+1,ak-n+2,...,ak-3,ak-2,ak-1)，即auv尚未经过的历史目标点的数目为n。当auv距离目标较远时，若n≥10，则取首端和末端各5个点，即(ak-n,ak-n+1,ak-n+2,ak-n+3,ak-n+4,ak-5,ak-4,ak-3,ak-2,ak-1)为所选历史目标点序列，此时期望目标点序列为当前时刻auv位置、所选历史目标点序列、当前时刻目标位置最优估计值，共12个点；当auv距离目标较近时，若n＜10，则所选历史目标点序列为(ak-n,ak-n+1,ak-n+2,...,ak-3,ak-2,ak-1)，此时期望目标点序列为当前时刻auv位置、所选历史目标点序列、当前时刻目标位置最优估计值，共n+2个点，，其中ai＝(ξi,ηi,ζi)表示大地坐标系下点的位置坐标。设期望目标点序列数目为m，其序列为(p1,p2,...,pm)，则可以表示为：

当n≥10时，m＝12。

(p1,p2,...,pm)＝(aauv,ak-n,ak-n+1,ak-n+2,ak-n+3,ak-n+4,ak-5,ak-4,ak-3,ak-2,ak-1,ak)当n＜10时，m＝n+2。

(p1,p2,...,pm)＝(aauv,ak-n,ak-n+1,ak-n+2,...,ak-3,ak-2,ak-1,ak)

(5)根据步骤(4)所得到的期望目标点序列，引入构造三次样条曲线的几何约束包括起始约束与末段约束：

起始段约束要求路径在ηauv处的切线方向与当前时刻的auv艏向一致，且以当前时刻auv的位置为路径起点。即：

其中：p1为第一个期望目标点，(ξauv,ηauv,ζauv)表示当前时刻auv的坐标，表示路径曲线在起点处的斜率，ψ表示auv的艏向向量，α为正实数。

末段约束要求所规划路径的期望艏向不超过坞站锥形开角的一半，且以当前时刻目标位置为路径终点，即

其中：pm为最后一个期望目标点，(ξk,ηk,ζk)表示当前时刻目标的坐标，α坞站为坞站锥形开角大小，ψ为auv艏向角。

(6)根据步骤(4)选定的期望目标点序列以及步骤(5)所引入的约束条件，三次样条曲线设计如下：

以仿真数据为例，如图3所示，已知期望目标点序列数目为9，其序列为(p1,p2,...,p9)，设曲线为三次样条曲线，则可以得到三次样条曲线的表达式为：

pi(t)＝∑pi·f(t)i＝1,2,...,8

其中：pi(t)为路径的分段表示形式，表示路径的第i段曲线，pi(t)|t＝0表示分段曲线的起点，pi(t)|t＝1表示分段曲线的终点，pi表示第i段曲线的控制点，f(t)为三次样条的基函数。