基于双贝叶斯估计的动态威胁运动状态估计

刘 厂，赵俊翔，胡 海

(1.哈尔滨工程大学 自动化学院，黑龙江 哈尔滨 150001； 2.空间物理重点实验室，北京 100076；

3.中国核动力研究设计院，四川 双流 610213)

基于双贝叶斯估计的动态威胁运动状态估计

刘 厂1，赵俊翔2，胡 海3

(1.哈尔滨工程大学 自动化学院，黑龙江 哈尔滨 150001； 2.空间物理重点实验室，北京 100076；

3.中国核动力研究设计院，四川 双流 610213)

现有的水下动态威胁运动状态估计主要采用单级估计方法，该方法将动态威胁的速度分解到三个坐标轴分别进行估计，没有考虑运动体速度的整体性.本文提出了一种双贝叶斯估计方法，首先利用自适应贝叶斯估计方法对水下动态威胁的速度大小和方向进行估计，再以此为输入，利用无迹贝叶斯估计方法估计其位置.与传统方法相比，双贝叶斯估计方法充分考虑了水下动态威胁速度的整体性，其有效性和精确性在仿真试验中得到验证.

双贝叶斯估计；水下动态威胁；运动状态

0 引言

水下潜器在航行过程中不可避免地遇到一些动态威胁，对动态威胁的运动状态进行准确估计才能实现有效规避[1].现有的运动状态估计往往采用单级估计方法，通过无迹卡尔曼、容积卡尔曼、自适应卡尔曼等方法完成目标跟踪的任务.这些方法将水下动态威胁的运动分解为三个坐标轴上的运动并分别进行估计[2-3].该分解方法对水下动态威胁的运动模型适应性不佳，不能完全把握水下动态威胁的运动趋势[4-5]，无法适应快速机动的威胁状态变化.因此，笔者提出了一种双贝叶斯运动状态估计方法，该方法针对动态威胁的运动模型，首先利用自适应贝叶斯估计方法[3，6]对水下动态威胁的速度大小和方向进行估计，进而利用无迹贝叶斯估计方法[7-8]估计动态威胁的位置.仿真试验表明，相对于传统的估计方法，该方法更加贴近水下动态威胁的运动模型[9-10]，具有较高的精确性.

1 动态威胁运动方程和观测方程

为了使水下动态威胁的状态估计更加贴近真实的运动状态首先根据实际运动情况建立水下动态威胁的运动方程和观测方程.对于水下动态威胁的观测一般通过声学传感器来完成，因此得到的观测数据大多为极坐标下的数据，即水下动态威胁相对于观测点的距离R、平面偏向角A和垂直偏向角E.结合观测点自身坐标，可以得到观测方程：

(1)

式中：(ξ,η,ζ)为水下动态威胁的世界坐标系坐标；(x,y,z)为观测点位置坐标.

建立水下动态威胁的运动模型应当结合观测方程进行.由观测方程可以看出，对水下动态威胁的观测信息仅限于动态威胁与观测点的相对位置，通过观测方程无法获得水下动态威胁自身的姿态角.因此，由于观测信息不足，水下动态威胁的转动在建立运动方程时应该被忽略.但是由于动态威胁运动的连续性，在某一时刻的位置信息不能完全反应其运动情况，为了更全面地把握动态威胁的运动状态，通过相邻两个时刻的观测信息可以解算得到动态威胁在两个时刻间速度大小和方向的平均值.在双贝叶斯运动状态估计方法中，将速度作为整体看待，不将其投影至三个坐标轴.

记水下动态威胁的速度大小为v，速度方向与ηEξ平面的夹角为φ，速度方向与Eζ轴正方向夹角为θ，可以得到水下动态威胁的运动方程

(2)

2 水下动态威胁速度和位置的处理

为了实现对水下动态威胁的状态估计，需要解算水下动态威胁的速度大小及其方向，速度方向包括水下动态威胁的纵倾角和偏航角.文中的纵倾角和偏航角指的是水下动态威胁的速度方向与水平面ηEξ和Eζ轴正方向的夹角，而不是水下动态威胁的姿态角.解算方程如下：

(3)

式中：Δξ=ξk-ξk-1,Δη=ηk-ηk-1,Δζ=ζk-ζk-1；φk-1为其k-1时刻的纵倾角；θk-1为其k-1时刻的偏航角；vk-1为其k-1时刻的速度大小.

3 对速度的自适应贝叶斯估计

在实际运动过程中，水下动态威胁的速度和加速度均会发生变化，通常采用的CT模型和CV模型无法完全匹配其运动，而模型失配往往会导致估计发散.自适应估计方法在经典估计方法的基础上，能够充分发挥观测信息的作用，对自身模型进行校正，从而连续校正预测值.因此，采用自适应估计的方法，通过观测结果对模型进行调整，以适应加速度变化的运动模型.笔者将加速度的变化量视作噪声，通过Sage-husa噪声估计器对其均值进行估计.

根据前两节的运动分析，得到水下动态威胁速度大小和方向的状态转移方程和观测方程：

(4)

(5)

记：

(6)

(7)

(8)

基于以上方程，使用自适应贝叶斯估计方法，设计自适应贝叶斯估计器.

预测

(9)

在获得k+1时刻的数据后，对参数进行更新，需要注意的是，在k+1时刻，由于观测值为水下动态威胁的坐标，所以解算出的水下动态威胁速度信息是k到k+1时刻之间的平均值，由于间隔时间较小，将此平均值作为k时刻水下动态威胁的速度信息.

(10)

(11)

(12)

(13)

Pk=[I-KkHk]Pk,k-1.

(14)

噪声估计：

(15)

(16)

(17)

(18)

在Sage-husa自适应贝叶斯估计方法中，遗忘因子b是十分关键的参数，估计精度是否发散与其相关.本文中，使用预报残差法求取遗忘因子.量测残差：

(19)

(20)

又有：

(21)

代入，有：

(22)

通过以上各式，即可得到对水下动态威胁速度大小和方向的估计结果.

4 对位置的无迹贝叶斯估计

获得水下动态威胁的速度大小和方向的估计信息后，下一步是对运动体的未来位置进行估计.不可避免地，由于水下动态威胁的机械误差和环境影响，水下动态威胁的行进过程仍是一个存在过程噪声的动态过程，因此需要使用贝叶斯估计方法对运动体位置进行进一步的预测.由于此时状态方程仍为非线性，传统估计方法无法满足要求.无迹贝叶斯估计方法根据“利用一组确定性的点对概率分布函数的近似要比直接近似非线性函数更容易”的思想，通过无迹变换获得确定性采样点，在非线性估计中能够取得良好的效果.因此，采用无迹贝叶斯估计方法进行运动体位置的估计.建立水下动态威胁的状态方程和观测方程

Xk+1=f(Xk,φ,θ,v)+Wk;

(23)

yk=HXk+Vk,

(24)

式中：

步态选择方面，选取稳定性高的三角步态作为四足机器人在斜面上的运动步态，并采用LF-RH-RF-LH的腿部运动顺序以获得最优的稳定裕度[6]。本文的运动步态时序图如图3所示。其中，黑色矩形表示腿部处于支撑相，白色矩形表示腿部处于摆动相。

(25)

预测：

(26)

(27)

(28)

这样，我们就得到了水下动态威胁位置的最终估计值，在k+1时刻，获取运动体位置的观测值之后，根据观测量对参数进行更新

(29)

(30)

(31)

(32)

(33)

(34)

(35)

式中:λ为点集产生的尺度参数，每个点的权值因采样策略的不同而稍有差异.笔者采用标度采样：

(36)

5 仿真试验

为了验证上述方法的有效性，设计水下动态威胁在三维空间中进行变加速运动，模拟观测数据作为估计算法的输入.分别利用双贝叶斯估计方法和单级贝叶斯估计方法对动态威胁的运动路径进行估计，通过对比估计结果与水下动态威胁实际航路的误差验证双贝叶斯估计方法的有效性。其中，单级贝叶斯估计采用自适应的无迹贝叶斯估计方法，将速度投影至三个坐标轴，直接对水下动态威胁的世界坐标系坐标进行估计.其状态方程为：

(37)

5.1 水下动态威胁运动路径设计

设计动态威胁从(200，250，1 000)点出发，先进行5 s的匀速直线运动，在随后的95 s中，做速度大小和方向不断变化的变速曲线运动.具体运动情况如下.

1～5 s.做速度为5 m/s的匀速直线运动.

5～10 s.速度俯仰角突变为0.429 7，角加速度为-0.05 rad/s，航向角突变为0.707 1，角加速度为0.05 rad/s，加速度为0.05 m/s2.

10～25 s.水下动态威胁改变航向，速度俯仰角突变为0.729 7，角加速度为0.05 rad/s，航向角突变为1.407 1，角加速度为-0.07 rad/s，加速度为-0.05 m/s2.

25～40 s.速度俯仰角突变为0.829 7，角加速度为-0.01 rad/s，航向角突变为0.507 1，角加速度为0.05 rad/s，加速度为-0.05 m/s2.

40～60 s.水下动态威胁突然回转并加速行进，速度俯仰角突变为0.829 7，角加速度为-0.01 rad/s，航向角突变为0.607 1，角加速度为0.05 rad/s，加速度为0.1 m/s2.

60～80 s.再次改变航向，速度俯仰角突变为1.329 7，角加速度为-0.03 rad/s，航向角突变为0.607 1，角加速度为-0.03 rad/s，加速度为0.05 m/s2.

80～100 s.水下动态威胁速度俯仰角突变为0.929 7，角加速度为0.01 rad/s，航向角突变为1.007 1，角加速度为0.01 rad/s，加速度为-0.05 m/s2.

在100 s后运动体消失.在此过程中，水下动态威胁的最高速度达到11 m/s，最大角加速度为0.07 rad/s，最大加速度为0.1 m/s2.

水下动态威胁运动路径如图1所示.

图1 水下动态威胁的模拟航路

5.2 仿真结果

设观测误差为1(单位：m)，方差为1，取无迹贝叶斯滤波中λ为1，分别采用单级贝叶斯方法和笔者设计的双贝叶斯估计方法(取无迹贝叶斯滤波中λ为1)得到的仿真结果如图2～5所示.

通过比较可以发现，在观测噪声较大的条件下，将速度投影至三个坐标轴分别进行估计的单贝叶斯估计方法不能很好地把握运动特征，估计误差较大，在航路附近有较大波动.由于双贝叶斯估计充分考虑到水下动态威胁的运动状态，先对水下动态威胁的速度大小及其方向进行估计，更充分地利用了观测信息，因此，双贝叶斯估计能够更好地消除观测噪声对估计的影响，在观测噪声均值较大的情况下，即使对动态目标的位置估计出现了误差，对于动态目标的运动趋势估计依旧较为精确，也得到了较高精度的估计值.

图2 水下动态威胁航路估计结果

图3 x方向估计误差

图4 y方向估计误差

图5 z方向估计误差

6 结论

提出了一种基于双贝叶斯估计的水下动态威胁运动状态估计方法.首先建立了水下运载体的运动模型，通过自适应贝叶斯估计对动态威胁的速度大小和方向进行估计，再以此为输入，通过第二级无迹贝叶斯估计进行运动体位置估计.仿真试验证明，相对于传统的单级估计方法，双贝叶斯估计方法在进行水下动态威胁运动状态估计时能够更好地贴合运动模型，得到较高的估计精度，并能准确地给出水下动态威胁的运动趋势.

[1] 朱大奇, 颜明重. 移动机器人路径规划技术综述 [J]. 控制与决策, 2010, 25(7): 961-967.

[2] 高晓光, 李青原, 邸若海. 基于DBN威胁评估的MPC无人机三维动态路径规划[J]. 系统工程与电子技术, 2014, 36(11)：2199-2205.

[3] 李峰. 自主水下航行器目标跟踪方法研究[D]. 哈尔滨：哈尔滨工程大学自动化学院, 2013.

[4] 邸若海, 高晓光. 模糊离散动态贝叶斯网络的目标威胁等级评估[J]. 火力与指挥控制, 2012, 37(4):14-17.

[5] GAO M, JIANG J, MING N K, et al. Cooperative path planning for UAVs with UAV loss considerations[C]//Proc. of computationall intelligence for security and defense applications, 2013: 38-44.

[6] 张召友. 非线性 Bayesian 滤波及其在 SINS/GPS 紧耦合导航中的应用研究[D]. 哈尔滨：哈尔滨工程大学自动化学院, 2013.

[7] 张宏启. 新型自适应 kalman 滤波算法在组合导航中的应用研究[D]. 哈尔滨：哈尔滨工程大学自动化学院, 2012.

[8] 王克文, 蒋德珑, 孙栗. 电力系统状态估计算法的模糊综合评价与分析[J]. 郑州大学学报(工学版), 2011, 32(3):85-89.

[9] 张恒艳, 高中文, 李文龙，等. 不确定T-S模糊系统的跟踪控制器设计[J]. 郑州大学学报(工学版), 2016, 37(2):15-19.

[10]范文兵, 郭云, 王振义,等. 一种MIMO时间相干信道下的有限反馈新方法[J]. 郑州大学学报(工学版), 2014, 35(3):43-46.

Research on Motion Status Estimation of Dynamic Threat Base on Double Bayes Estimation

LIU Chang1, ZHAO Junxiang2, HU Hai3

(1.Collage of Automatic, Harbin Engineering University, Harbin 150001, China; 2.Key Laboratory of space physics,Beijing 100076, China; 3.Nuclear Power Institute of China , Shuangliu 610213, China)

For the issue of motion estimation of underwater dynamic threat, single-stage estimation method is utilized extensively in which velocity is decomposed to three coordinate axes for independent calculation. In order to consider the integrity of the speed, a double Bayesian estimation method is proposed in this paper. In the first stage of the improved approach, the velocity and direction of underwater dynamic threat are estimated by the adaptive Bayesian estimation, and then the position is estimated by the unscented Bayesian estimation in the second stage with the joint efforts of the estimated results in the first stage. Simulation results are provided to verify the feasibility and effectiveness of the proposed method.

double Bayes estimation; underwater dynamic threat; motion status

2016-10-07；

2016-12-05

国家自然科学基金资助项目(51379069)；中央高校基础科研业务费支持项目(HEUCF160410)

刘厂(1982— )，男，山东济宁人，哈尔滨工程大学讲师，博士，主要从事水下运载器智能导航技术研究，E-mail:liuchang407@hrbeu.edu.cn.

1671-6833(2017)02-0055-06

TP181

A

10.13705/j.issn.1671-6833.2017.02.013