CN110806760A - 一种无人水下航行器的目标跟踪控制方法 - Google Patents

Abstract

本发明公开了一种面向水声探测的水下航行器目标跟踪控制方法，所述方法包括：步骤1)在t_k时刻水下航行器根据水声探测获取目标当前位置；步骤2)以目标当前位置为输入，基于粒子滤波预测目标可能出现的位置，获取目标更新位置；步骤3)建立运动控制惯性坐标系和载体坐标系，由此得到水下航行器运动学的参数；步骤4)根据当前时刻水下航行器的姿态和位置以及目标更新位置，如果偏航角需要调整则对水下航行器的偏航角进行调整；步骤5)如果俯仰角需要调整则对水下航行器的俯仰角进行调整；步骤6)水下航行器基于偏航角和俯仰角确定的方向向目标前进，计算获取水下航行器在t_k+1时刻的位置和姿态，并令k＝k+1，转入步骤1)；直至目标跟踪结束。

Description

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

技术领域

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

背景技术

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

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

发明内容

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

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

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

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

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

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

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

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

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

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

选取N个采样点以及权重集合为

其中

表示t_k时刻根据水声探测得到的目标位置选取的第i个采样点，对应的权重为

计算系统状态的后验概率密度p(x_k|y_1:k)为：

其中，δ(·)为狄克拉函数，x_k表示探测得到的目标当前位置，y₁:_k表示从时刻t₁到时刻t_k的目标位置观测值；

更新重要性权重

由下式计算：

其中，

表示了系统测量噪声模型；

表示t_k时刻第i个采样点的状态转移概率密度；

表示t_k时刻的在从时刻t₀到时刻t_k-1第i个采样点的状态值和从时刻t₁到时刻t_k的目标位置观测值的前提下的第i个采样点的分布，表示从时刻t₀到时刻t_k-1第i个采样点的状态值；

计算归一化的权重

为：

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

作为上述方法的一种改进，所述运动控制惯性坐标系的原点为地心O，轴OX、OY以及OZ分别远着正北、正东以及正下方向；所述载体坐标系的原点O_U位于载体的体心，轴O_UX沿着水下航行器的纵轴方向向前，轴O_UZ垂直于轴O_UX并指向海底，轴O_UY由右手定则确定；

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

υ＝[υ₁υ₂]^T,υ₁＝[μνω]^T,υ₂＝[pqr]^T

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

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

步骤4-1)根据最小偏航转弯半径，确定运动控制惯性坐标系下的转弯圆心O_yaw＝(x_yaw，y_yaw，z_yaw)^T，具体为：

假定水下航行器的当前航向为

最小偏航转弯半径为R_t，与之对应的最大偏航角为ψ_R；惯性坐标系下航行器的当前位置为P_r＝(x_r,y_r,z_r)^T，惯性坐标系下目标更新位置

表示为P_t＝(x_t,y_t,z_t)^T；

当前航向的法向量

按照下式求取：

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

步骤4-2)判断

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

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

步骤5-1)根据最小俯仰转弯半径，确定惯性坐标系下转弯圆心O_pitch＝(x_pitch，y_pitch，z_pitch)^T，具体为：

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

步骤5-2)判断

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

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

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

υ_O＝T₁ ^-1(φ,θ,ψ)υ_U

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

z_k+1＝z_k+υ_O(t_k+1-t_k)。

本发明的优势在于：

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

附图说明

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

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

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

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

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

具体实施方式

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

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

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

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

TS＝TS₀(16.17-2.98cos2β-3.083cos6β)/22.233

其中，TS₀为目标正横的目标强度，一般TS₀取为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-k_L

当D≤r≤8D时

当r≥8D时

其中，α_T浅海有效衰减系数，它与声信号频率、海水温度等有关，α_T的值与声信号在海水中的反射次数有关，每一次反射的最大跨度与声速、声速梯度有关，其极限声线跨度为

R＝c/g，c为声速，其大小与海水深度、盐度、温度等有关，g为声速梯度；k_L是近场异常衰减，单位为分贝，对于不同的海况和海底类型，对应不同的传播频率，k_L的值均不同。

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

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

TL(r)＝FOM

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

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

x_k＝f(x_k-1,v_k-1)

y_k＝h(x_k,n_k)

设初始概率密度为p(x₀|y₀)＝p(x₀)，预测方程为：

p(x_k|y_0:k-1)＝∫p(x_k|x_k-1)p(x_k-1|y_1:k-1)dx_k-1

状态更新方程为：

其中，

p(y_k|y_1:k-1)＝∫p(y_k|x_k)p(x_k|y_1:k-1)dx_k

将重要性函数改写为

可以得到权值公式为

选取N个采样点以及权重集合为

其中

表示k时刻根据水声探测得到的目标位置选取的第i个采样点，对应的权重为

计算系统状态的后验概率密度p(x_k|y_1:k)为：

其中，δ(·)为狄克拉函数，x_k表示探测得到的目标当前位置，y₁:_k表示从时刻t₁到时刻t_k的目标位置观测值；

更新重要性权重

由下式计算：

其中，

表示了系统测量噪声模型；

表示t_k时刻第i个采样点的状态转移概率密度；表示t_k时刻的在从时刻t₀到时刻t_k-1第i个采样点的状态值和从时刻t₁到时刻t_k的目标位置观测值的前提下的第i个采样点的分布，表示从时刻t₀到时刻t_k-1第i个采样点的状态值；

计算归一化的权重：

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

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

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

如图2所示，建立UUV运动学模型的坐标系：惯性坐标系OXYZ，原点为O，轴OX、OY以及OZ分别远着正北、正东以及正下方向；载体坐标系O_UXYZ，坐标原点O_U位于载体的体心，轴O_UX沿着UUV的纵轴方向向前，轴O_UZ垂直于轴O_UX并指向海底，轴O_UY由右手定则确定。

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

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

υ＝[υ₁ υ₂]^T,υ₁＝[μ ν ω]^T,υ₂＝[p q r]^T

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

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

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

第一，根据最小偏航转弯半径，确定惯性坐标系下转弯圆心O_yaw＝(x_yaw，y_yaw，z_yaw)^T。

假定水下航行器的当前航向为

最小偏航转弯半径为R_t，与之对应的最大偏航角为ψ_R；惯性坐标系下水下航行器的当前位置为P_r＝(x_r,y_r,z_z)^T，惯性坐标系下目标更新位置

表示为P_t＝(x_t,y_t,z_t)^T。

当前航向的法向量

按照下式求取：

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

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

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

第一，根据最小俯仰转弯半径，确定惯性坐标系下转弯圆心O_pitch＝(x_pitch，y_pitch，z_pitch)^T。

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

第二，判断

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

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

υ_O＝T₁ ^-1(φ,θ,ψ)υ_U

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

z_k+1＝z_k+υ_O(t_k+1-t_k)。

当UUV到达指定位置(x_P,y_P,z_P)^T后，即满足：

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

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

Claims (7)

1.一种面向水声探测的水下航行器目标跟踪控制方法，所述方法包括：

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

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

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

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

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

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

2.根据权利要求1所述的面向水声探测的水下航行器目标跟踪控制方法，其特征在于，所述步骤1)中水下航行器根据水声探测获取目标位置的具体过程为：基于主动声纳方程，分析目标强度、海洋环境噪声、检测阈和传播损失因素，计算声纳探测作用距离；如果目标在声纳探测的作用范围内，则输出t_k时刻目标当前位置x_k。

3.根据权利要求1或2所述的面向水声探测的水下航行器目标跟踪控制方法，其特征在于，所述步骤2)的具体过程为：

选取N个采样点以及权重集合为

其中

表示t_k时刻根据水声探测得到的目标位置选取的第i个采样点，对应的权重为

计算系统状态的后验概率密度p(x_k|y_1:k)为：

其中，δ(·)为狄克拉函数，x_k表示探测得到的目标当前位置，y_1:k表示从时刻t₁到时刻t_k的目标位置观测值；

更新重要性权重

由下式计算：

其中，

表示了系统测量噪声模型；

表示t_k时刻第i个采样点的状态转移概率密度；

表示t_k时刻的在从时刻t₀到时刻t_k-1第i个采样点的状态值和从时刻t₁到时刻t_k的目标位置观测值的前提下的第i个采样点的分布，表示从时刻t₀到时刻t_k-1第i个采样点的状态值；

计算归一化的权重

为：

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

4.根据权利要求3所述的面向水声探测的水下航行器目标跟踪控制，其特征在于，所述运动控制惯性坐标系的原点为地心O，轴OX、OY以及OZ分别远着正北、正东以及正下方向；所述载体坐标系的原点O_U位于载体的体心，轴O_UX沿着水下航行器的纵轴方向向前，轴O_UZ垂直于轴O_UX并指向海底，轴O_UY由右手定则确定；

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

υ＝[υ₁υ₂]^T,υ₁＝[μνω]^T,υ₂＝[pqr]^T

其中，ξ,τ,

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

5.根据权利要求4所述的面向水声探测的水下航行器目标跟踪控制，其特征在于，所述步骤4)的具体过程包括：

步骤4-1)根据最小偏航转弯半径，确定运动控制惯性坐标系下的转弯圆心O_yaw＝(x_yaw,y_yaw,z_yaw)^T，具体为：

假定水下航行器的当前航向为

最小偏航转弯半径为R_t，与之对应的最大偏航角为ψ_R；惯性坐标系下航行器的当前位置为P_r＝(x_r,y_r,z_r)^T，惯性坐标系下目标更新位置表示为P_t＝(x_t,y_t,z_t)^T；

当前航向的法向量

按照下式求取：

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

步骤4-2)判断

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

6.根据权利要求5所述的面向水声探测的水下航行器目标跟踪控制，其特征在于，所述步骤5)的具体过程包括：

步骤5-1)根据最小俯仰转弯半径，确定惯性坐标系下转弯圆心O_pitch＝(x_pitch,y_pitch,z_pitch)^T，具体为：

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

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

7.根据权利要求6所述的面向水声探测的水下航行器目标跟踪控制，其特征在于，所述步骤6)的具体过程为：

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

υ_O＝T₁ ^-1(φ,θ,ψ)υ_U

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

z_k+1＝z_k+υ_O(t_k+1-t_k)。

Images