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

本发明专利技术公开了一种面向水声探测的水下航行器目标跟踪控制方法，所述方法包括：步骤1)在t

A target tracking control method for unmanned underwater vehicle

【技术实现步骤摘要】
一种无人水下航行器的目标跟踪控制方法
本专利技术涉及无人水下航行器建模领域，尤其涉及一种水下航行器的目标跟踪控制方法。
技术介绍
无人水下航行器(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当前位姿进行主动探测获取目标的信息；位置预测过程以水声的目标信息为基础，为运动控制模型提供运动控制参数；运动控制过程在位置预测的驱动下，调整运动控制策略，不断输出位姿到探测模本文档来自技高网...

【技术保护点】
1.一种面向水声探测的水下航行器目标跟踪控制方法，所述方法包括：/n步骤1)在t

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


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


3.根据权利要求1或2所述的面向水声探测的水下航行器目标跟踪控制方法，其特征在于，所述步骤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个采样点的状态值；
计算归一化的权重为：



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


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

【专利技术属性】
技术研发人员：孙飞虎，张扬帆，李宇，尹力，
申请(专利权)人：中国科学院声学研究所，
类型：发明
国别省市：北京;11