一种无人水下航行器的目标跟踪控制方法与流程

本发明涉及无人水下航行器建模领域，尤其涉及一种水下航行器的目标跟踪控制方法。

背景技术：

无人水下航行器(uuv)是一种海上力量倍增器，拥有广泛而重要的军事、科研用途，在未来的海洋探测中发挥不可替代的作用。uuv搭载多种传感器，可以在复杂海况、恶劣环境下完成水下警戒、跟踪、勘探、中继通信等水下航行任务。随着技术的进步，uuv拥有更多的潜在应用领域，尤其是对人员要求严格的深海环境。

uuv在水下作业过程中，一种重要的探测手段是成像探测，包括光视觉成像和声纳探测成像。在声纳探测成像方面，一种方法是基于侧扫声纳的海底成像方法，利用运动信息和声纳探测模型实现uuv定位。在光视觉成像方面，一种方法是通过分割线性成像、gvf-snake模型、pso-bp算法实现目标识别。但是基于成像的探测方法，存在时效性差、能耗高的缺点，在实用中还有很多瓶颈。。

技术实现要素：

本发明的目的是服务于水下探测体系中uuv装备部署，满足uuv主动探测时对目标的持续有效跟踪，提出一种水下航行器的目标跟踪控制方法。

为了实现上述目的，本发明提出了一种水下航行器的目标跟踪控制方法，所述方法包括：

步骤1)在tk时刻，水下航行器根据水声探测获取目标当前位置，k为正整数；

步骤2)以目标当前位置为输入，基于粒子滤波预测目标可能出现的位置，获取目标更新位置；

步骤3)建立运动控制惯性坐标系和载体坐标系，由此得到水下航行器运动学的参数：姿态和位置；

步骤4)根据当前时刻水下航行器的姿态和位置以及目标更新位置，判断水下航行器是否进行偏航角的调整，如果不需要调整，转入步骤5)，否则，对水下航行器的偏航角进行调整，转入步骤5)；

步骤5)判断水下航行器是否进行俯仰角的调整，如果不需要调整，转入步骤6)，如果需要调整，对水下航行器的俯仰角进行调整，转入步骤6)；

步骤6)水下航行器基于偏航角和俯仰角确定的方向向目标前进，计算获取水下航行器在tk+1时刻的位置和姿态，并令k＝k+1，转入步骤1)；直至目标跟踪结束。

作为上述方法的一种改进，所述步骤1)中水下航行器根据水声探测获取目标位置的具体过程为：基于主动声纳方程，分析目标强度、海洋环境噪声、检测阈和传播损失因素，计算声纳探测作用距离；如果目标在声纳探测的作用范围内，则输出tk时刻目标当前位置xk。

作为上述方法的一种改进，所述步骤2)的具体过程为：

选取n个采样点以及权重集合为其中表示tk时刻根据水声探测得到的目标位置选取的第i个采样点，对应的权重为计算系统状态的后验概率密度p(xk|y1:k)为：

其中，δ(·)为狄克拉函数，xk表示探测得到的目标当前位置，y1:k表示从时刻t1到时刻tk的目标位置观测值；

更新重要性权重由下式计算：

其中，表示了系统测量噪声模型；表示tk时刻第i个采样点的状态转移概率密度；表示tk时刻的在从时刻t0到时刻tk-1第i个采样点的状态值和从时刻t1到时刻tk的目标位置观测值的前提下的第i个采样点的分布，表示从时刻t0到时刻tk-1第i个采样点的状态值；

计算归一化的权重为：

则预测的目标更新位置为：

作为上述方法的一种改进，所述运动控制惯性坐标系的原点为地心o，轴ox、oy以及oz分别远着正北、正东以及正下方向；所述载体坐标系的原点ou位于载体的体心，轴oux沿着水下航行器的纵轴方向向前，轴ouz垂直于轴oux并指向海底，轴ouy由右手定则确定；

水下航行器在水下运动具有六个完全自由度，分别为沿着三个轴的旋转运动以及沿着三个轴的平移运动；由此得到水下航行器运动学的参数分别为广义位置η、广义速度υ，具体如下所示：

υ＝[υ1υ2]^t,υ1＝[μνω]^t,υ2＝[pqr]^t

其中，ξ,τ,表示运动控制惯性坐标系下沿着x,y,z轴移动位移，φ,θ,ψ表示运动控制惯性坐标系下沿着x,y,z轴移动角度；μ,ν,ω表示载体坐标系下沿着x,y,z轴移动速度，p,q,r表示载体坐标系下沿着x,y,z轴移动角速度。

作为上述方法的一种改进，所述步骤4)的具体过程包括：

步骤4-1)根据最小偏航转弯半径，确定运动控制惯性坐标系下的转弯圆心oyaw＝(xyaw，yyaw，zyaw)^t，具体为：

假定水下航行器的当前航向为最小偏航转弯半径为rt，与之对应的最大偏航角为ψr；惯性坐标系下航行器的当前位置为pr＝(xr,yr,zr)^t，惯性坐标系下目标更新位置表示为pt＝(xt,yt,zt)^t；

当前航向的法向量按照下式求取：

由此，按照下式计算转弯圆心oyaw的坐标：

步骤4-2)判断是否成立，如果成立，则不需要调整偏航角，转入步骤5)，否则，调整水下航行器的偏航角为ψr，转入步骤5)。

作为上述方法的一种改进，所述步骤5)的具体过程包括：

步骤5-1)根据最小俯仰转弯半径，确定惯性坐标系下转弯圆心opitch＝(xpitch，ypitch，zpitch)^t，具体为：

最小俯仰转弯半径为rp，与之对应的最大偏航角为θr，由此，按照下式计算转弯圆心opitch的坐标：

步骤5-2)判断是否成立，如果成立，水则不需要调整偏航角，转入步骤6)，否则，调整水下航行器的偏航角为θr，转入步骤6)。

作为上述方法的一种改进，所述步骤6)的具体过程为：

水下航行器在tk时刻的位置为zk，水下航行器的速度为υu，在载体坐标系下的速度为υu＝(υu,0,0)^t；由此得到水下航行器在惯性坐标系下的速度

υo＝t1^-1(φ,θ,ψ)υu

其中，φk-1,θk-1,ψk-1分别表示tk时刻的水下航行器的横滚角、俯仰角以及偏航角；当需要偏航角调整时，ψk-1＝ψr；需要进行俯仰角调整时，θk-1＝θr；则tk+1时刻水下航行器的位置为：

zk+1＝zk+υo(tk+1-tk)。

本发明的优势在于：

由于水下目标运动的不确定性和uuv中搭载的探测设备具有局限性，因此在uuv运动过程中对目标的探测存在盲区，应用本发明的方法有助于依靠水声探测手段保持对目标的持续有效跟踪。

附图说明

图1为本发明的面向水声探测的uuv模型框架；

图2为uuv运动学模型的坐标系；

图3为uuv在不同坐标系下的运动变量示意图；

图4为本发明的uuv偏航角调整示意图；

图5为本发明的uuv俯仰角调整示意图。

具体实施方式

下面结合附图和具体实例对本发明提出的方法进行详细描述。

如图1所示，面向水声探测的水下航行器目标跟踪控制方法包括水声探测、位置预测以及运动控制过程。三个环节相互联系，共同实现高效的主动探测：水声探测过程根据uuv当前位姿进行主动探测获取目标的信息；位置预测过程以水声的目标信息为基础，为运动控制模型提供运动控制参数；运动控制过程在位置预测的驱动下，调整运动控制策略，不断输出位姿到探测模型。

本发明设计了基于主动声纳方程的水声探测过程，综合考虑了主动探测的影响因素，包括目标强度、环境噪声、检测阈、传播损失等，获取主动声纳的作用力距离。

根据目标和声纳平台的经纬度，计算出相对于潜艇艇艏艇艉方向的方位角β，因此目标强度ts可以计算如下：

ts＝ts0(16.17-2.98cos2β-3.083cos6β)/22.233

其中，ts0为目标正横的目标强度，一般ts0取为20-25分贝，β为声波入射舷角。

大风大浪的恶劣条件使海洋环境噪声明显增大，从而显著降低声呐的检测性能。作为衡量海洋环境好坏的参数，海况等级按照从好至坏通常分为0～9级。作为声呐参数的海洋环境噪声级，可用经验模型进行测算。

对于浅海，计算噪声谱级nl的近似公式为：

nl＝10lgf^-1.7+6s+55

式中f为频率，s为海况等级(s＝0,1,2,...,9)。

假设主动声纳采用匹配滤波法检测目标回波信号，则检测阈dt计算如下：

其中，t为主动声纳发射脉冲宽度，d为检测指数，根据选定的检测概率和虚警概率从接收机工作特性(roc)曲线上查得。

本发明根据marsh和schulkin浅海传播损失模型(巨人模型)计算传播损失tl。

根据100hz到10khz频率范围内约10万次测量，得到三个距离段上的tl半经验公式。定义距离参数

式中，h表示水深(单位为米)，l表示混合层深度(单位为米)，d为距离参数(单位为千米)。

根据距离远近，三个传播损失tl半经验公式为：

当r＜d时

tl＝20lgr+αr+60-kl

当d≤r≤8d时

当r≥8d时

其中，αt浅海有效衰减系数，它与声信号频率、海水温度等有关，αt的值与声信号在海水中的反射次数有关，每一次反射的最大跨度与声速、声速梯度有关，其极限声线跨度为r＝c/g，c为声速，其大小与海水深度、盐度、温度等有关，g为声速梯度；kl是近场异常衰减，单位为分贝，对于不同的海况和海底类型，对应不同的传播频率，kl的值均不同。

利用声纳方程计算声纳的作用距离，首先需要定音声纳的优质因数fom(figureofmerit)为：

声纳的作用距离r，可以通过求解下式确定。

tl(r)＝fom

进一步地，根据水声探测获取的目标位置信息预测更新目标位置。一方面，目标运动具有不确定性；另一方面，uuv中搭载的探测设备具有局限性，在uuv运动过程中对目标的探测存在盲区。因此，为保证对目标的稳定有效跟踪，需要预测目标的运动，及时调整uuv的运动。基于以上分析，本发明采用基于粒子滤波算法预测目标位置，对uuv决策过程进行建模。

假设状态方程和观测方程如下：

xk＝f(xk-1,vk-1)

yk＝h(xk,nk)

设初始概率密度为p(x0|y0)＝p(x0)，预测方程为：

p(xk|y0:k-1)＝∫p(xk|xk-1)p(xk-1|y1:k-1)dxk-1

状态更新方程为：

其中，

p(yk|y1:k-1)＝∫p(yk|xk)p(xk|y1:k-1)dxk

将重要性函数改写为

可以得到权值公式为

选取n个采样点以及权重集合为其中表示k时刻根据水声探测得到的目标位置选取的第i个采样点，对应的权重为计算系统状态的后验概率密度p(xk|y1:k)为：

其中，δ(·)为狄克拉函数，xk表示探测得到的目标当前位置，y1:k表示从时刻t1到时刻tk的目标位置观测值；

更新重要性权重由下式计算：

其中，表示了系统测量噪声模型；表示tk时刻第i个采样点的状态转移概率密度；表示tk时刻的在从时刻t0到时刻tk-1第i个采样点的状态值和从时刻t1到时刻tk的目标位置观测值的前提下的第i个采样点的分布，表示从时刻t0到时刻tk-1第i个采样点的状态值；

计算归一化的权重：

则预测的目标更新位置为：

以探测到的目标位置为输入，预测目标可能出现的位置，依次迭代，保持对目标运动的预测和持续跟踪。

进一步地，在目标预测获取的信息基础上，设计运动控制过程，控制uuv的偏航运动和俯仰运动，保持对目标的持续跟踪。

如图2所示，建立uuv运动学模型的坐标系：惯性坐标系oxyz，原点为o，轴ox、oy以及oz分别远着正北、正东以及正下方向；载体坐标系ouxyz，坐标原点ou位于载体的体心，轴oux沿着uuv的纵轴方向向前，轴ouz垂直于轴oux并指向海底，轴ouy由右手定则确定。

uuv在水下运动具有六个完全自由度，分别为沿着三个轴的旋转运动以及沿着三个轴的平移运动。uuv在不同坐标系下的运动变量如图3所示。

由此得到uuv运动学的参数分别为广义位置η、广义速度υ，具体如下所示：

υ＝[υ1υ2]^t,υ1＝[μνω]^t,υ2＝[pqr]^t

简化运动控制过程建模，假定uuv在一个运动周期(到达指定位置前)保持速度、航向(俯仰角、偏航角、横滚角)不变。

根据当前时刻水下航行器的姿态和位置以及探测目标的更新位置，判断水下航行器是否进行偏航角和俯仰角的调整，按照需要对水下航行器的偏航角和俯仰角调整。

如图4所示，给出了水下航行器进行偏航控制的基本过程。

第一，根据最小偏航转弯半径，确定惯性坐标系下转弯圆心oyaw＝(xyaw，yyaw，zyaw)^t。

假定水下航行器的当前航向为最小偏航转弯半径为rt，与之对应的最大偏航角为ψr；惯性坐标系下水下航行器的当前位置为pr＝(xr,yr,zz)^t，惯性坐标系下目标更新位置表示为pt＝(xt,yt,zt)^t。

当前航向的法向量按照下式求取：

由此，可以按照下式计算转弯圆心oyaw的坐标。

第二，判断是否成立，如果成立，水下航行器需要按照现有偏航角继续航行，否则，水下航行器直接调整偏航角为ψr。

如图5所示，uuv俯仰角调整过程同偏航角调整过程基本一致。

第一，根据最小俯仰转弯半径，确定惯性坐标系下转弯圆心opitch＝(xpitch，ypitch，zpitch)^t。

假定最小俯仰转弯半径为rp，与之对应的最大偏航角为θr。由此，可以按照下式计算转弯圆心opitch的坐标。

第二，判断是否成立，如果成立，水下航行器需要按照现有俯仰角继续航行，否则，水下航行器直接调整俯仰角为θr。

进一步地，在运动控制过程中，水下航行器在tk时刻的位置为zk，水下航行器的速度为υu，在载体坐标系下的速度为υu＝(υu,0,0)^t；由此得到水下航行器在惯性坐标系下的速度

υo＝t1^-1(φ,θ,ψ)υu

其中，φk-1,θk-1,ψk-1分别表示tk时刻的水下航行器的横滚角、俯仰角以及偏航角；当需要偏航角调整时，ψk-1＝ψr；需要进行俯仰角调整时，θk-1＝θr；则tk+1时刻水下航行器的位置为：

zk+1＝zk+υo(tk+1-tk)。

当uuv到达指定位置(xp,yp,zp)^t后，即满足：

此时，根据新的目标位置，更新uuv的航向角。

最后所应说明的是，以上实施例仅用以说明本发明的技术方案而非限制。尽管参照实施例对本发明进行了详细说明，本领域的普通技术人员应当理解，对本发明的技术方案进行修改或者等同替换，都不脱离本发明技术方案的精神和范围，其均应涵盖在本发明的权利要求范围当中。