贝叶斯网络增强型多模型AUV组合导航算法

王 磊，程向红，冉昌艳，陈红梅，胡 杰

（1．东南大学仪器科学与工程学院，江苏南京210096；2．东南大学微惯性仪表与先进导航技术教育部重点实验室，江苏南京210096）

贝叶斯网络增强型多模型AUV组合导航算法

王 磊1，2，程向红1，2，冉昌艳1，2，陈红梅1，2，胡 杰1，2

（1．东南大学仪器科学与工程学院，江苏南京210096；2．东南大学微惯性仪表与先进导航技术教育部重点实验室，江苏南京210096）

针对复杂环境下自主水下航行器（autonomous underwater vehicle，AUV）组合导航系统中存在噪声不确定或者易发生变化的情况，提出一种贝叶斯网络增强型交互式多模型（interactive multiple model filter based on Bayesian network，BN-IM M）滤波算法。该算法在多模型估计基础上，引入特征变量，并根据变量与系统模型之间存在的因果关系建立贝叶斯网络；利用贝叶斯网络参数修正多模型估计中的模型切换概率，能够降低多模型算法中真实模式识别对先验知识的依赖性。该算法能够解决交互式多模型（interactive multiple model，IMM）算法中模型转换存在滞后、模型概率易发生跳变等问题，增强多模型算法的自适应能力。以陀螺和加速度计的输出作为特征变量建立贝叶斯网络，对AUV组合导航系统进行仿真，结果表明所提出的BN-IM M算法相比于传统的IMM算法能够显著提高机动状态时模型转换速度和估计精度。

自主水下航行器；组合导航；多模型估计；贝叶斯网络

0 引 言

基于混合系统的多模型（multiple model，MM）估计是一种强有力的自适应估计方法，尤其适用于结构或参数易发生变化的系统，在机动目标跟踪［1］、图像识别［2］、故障诊断［3］以及组合导航［4］等领域得到了广泛的研究。其主要思想是在对象和扰动的数学模型不完全确定或者模型变化不确定的情况下，设计多个模型来逼近系统复杂的时变或非线性过程，从而使在建模条件下分析得到的系统性能保持或接近最优。文献［5］提出了一种以广义伪贝叶斯算法为框架、具有Markov切换系数的交互式多模型（interactive multiple model，IM M）算法。IM M方法使用一个更好的假设管理技术，兼具了广义伪贝叶斯（generalized pseudo Bayes，GPB）一阶算法（GPB1）计算复杂度低和二阶算法（GPB2）估计精度高的优势，被认为是一种最有效的混合估计方案［6］。

IMM与传统随机系统估计方法的主要区别是当前计算模式会随着模型的Markov链转移，由先验的Markov转移概率和量测信息共同确定模型转移概率，求得的模型转移概率与估计结果在下一时刻的估计过程中进行输入交互，进而对下一时刻的状态估计产生影响。实际应用表明，当系统模式发生变化时，由于滤波系统的惯性使得IM M算法对于实际系统模式切换的辨识有一定的滞后，文献［7］指出，IMM算法利用先验信息来确定模型转移概率，而忽略了系统当前时刻的量测信息，通过先验信息确定的滤波参数是模式切换与模式未切换情况下的折中。文献［8］提出了一种利用当前的量测信息在线推导模型转移概率的方法，能够有效降低先验知识不足造成的影响，提高了算法的有效性。实际上，混合系统模式发生变化是事物之间存在因果关系的一种体现，通常的多模型估计方法均能够用贝叶斯网络（Bayesian network，BN）进行表示，文献［9］指出，利用BN的图形论表示方法可能建立更具广泛意义的混合系统，从而更加清晰直观地表征异类信息的关系以及多个特征级别的信息融合问题。

自主水下航行器（autonomous underwater vehicle，AUV）长时间在水下工作，其工作环境中存在很多不确定因素，例如洋流干扰、海水温度和盐度变化等。因此，一般采用组合导航的方法来确保自主水下航行器具有较高的可靠性和定位精度［1012］。本文将过程噪声和观测噪声作为模型参数，利用多模型思想对组合导航系统状态及其噪声变化进行估计，将陀螺和加速度计的输出作为特征变量建立BN，利用网络参数与先验知识共同确定多模型算法中当前时刻的模型转移概率，解决IMM算法由于人为先验知识不足造成的系统模式切换滞后以及估计精度降低等问题，提高AUV机动状态时组合导航算法估计性能。

1 BN交互式多模型算法

1.1 BN

BN又被称为贝叶斯因果网，是一种利用概率统计理论为复杂系统、不确定性推理和数据分析提供直观、紧凑的图形表示方法。它主要由有向无环图（directed acyclic graph，DAG）和条件概率分布表两个部分构成，其中，DAG的节点代表随机变量，两个节点之间带箭头的连线称为有向边，表示随机变量之间的概率依赖关系，BN更详细的定义可参考文献［13］。

设变量集U＝｛X1，X2，…，Xn｝，则BN的联合概率分布可以表示为每个节点Xi相对于其父节点Pa（Xi）的条件概率分布的乘积，表示为

BN既能够定性描述网络中节点之间的概率依赖关系和条件独立关系，又可以定量的描述节点和其父节点之间的概率依赖程度。因此，BN很适合用于具有时序相关性的复杂系统中，进行多特征建模，可以准确地跟踪系统的机动性、时变性等。

1.2 多模型估计的图论方法描述

MM估计和BN均是由贝叶斯理论发展而来，两者有较好的相容性，利用图论的概率和方法可以更加清晰地认识二者之间的关系。MM估计可以采用支撑有向图来表示，系统的模式为支撑有向图中的顶点，有向图的边代表可能的模式切换，每条边所对应的权重即为模式切换概率。固定结构的MM算法在所有时刻都使用同一个支撑有向图，变结构的MM算法使用时变的支撑有向图。

MM估计系统中，事物之间的相关性可以通过在BN中建立共同的隐节点实现。由此构建的多模型估计系统中，模型切换概率体现了系统过去结构与现在结构因果关系的强弱，可以被推广为BN的网络参数。该参数不仅可以体现系统结构的突变性，而且可以描述节点之间的相关性、因果关系的强弱。因此，本文把BN与MM估计结合起来，对IM M算法中模型概率更新过程进行改进，使得到的模型概率能够更加清晰地描述转换模型之间的内在关系。

1.3 BN-IMM算法

设模型集合为M，共包含r个模型，k时刻的有效模式为mk，动态系统可描述为

式中，xk为状态向量；zk为观测向量；Γk为过程噪声矩阵；Hk为量测矩阵；wk与Ψk分别为过程噪声和观测噪声序列。初始Markov转移概率πji满足条件

BN-IMM算法的步骤同IMM算法［7-8］一样，区别是在模型概率更新过程中引入对贝叶斯概率的更新过程，算法结构如图1所示，每一个递推主要由以下4个步骤组成：

步骤1 模型条件重新初始化。假定k－1时刻的匹配模型是mik－1，k时刻的匹配模型为对k－1时刻各滤波器的估计进行混合得到与匹配的滤波器的输入。

（1）概率混合，由k－1时刻的模型概率与先验的Markov转移概率πji进行交互，计算混合概率为

（2）估计混合，对于第j（j＝1，2，…，r）个模型，重新初始化状态与协方差阵为

图1 BN-IMM算法结构图（模型个数r＝3）

步骤2 模型滤波。在获得新的量测zk之后，利用步骤1计算得到的重新初始化状态和协方差阵，进行状态估计更新，可采用卡尔曼、扩展卡尔曼或其他高斯滤波器进行估计。

（1）状态预测，对于每一个模型滤波器j（j＝1，2，…，r），分别计算

（2）量测预测残差及其协方差计算，对于每一个模型滤波器j（j＝1，2，…，r），分别计算

（3）滤波更新，对于每一个模型滤波器j（j＝1，2，…，r），分别计算

步骤3 模型概率更新。分为MM模型概率更新和BN概率更新，更新之后求和即为广义的BN概率。

（1）MM模型概率更新，利用步骤2中所求得的残差估值εjk、残差协方差Sjk以及匹配模型混合后的概率进行计算

（2）BN概率更新，通过对BN中变量Xτ（τ＝1，2，…，S）所有父节点Pa（Xτ）的条件概率求积，得到更新的BN概率为

（3）将MM模型概率与BN概率进行加权求和，权值系数为η，并归一化，得到新的模式切换概率为

步骤4 输出交互。对各滤波器估计值进行概率加权融合得到输出结果为

2 BN AUV组合导航系统

在MM估计系统中，模型概率更新步骤隐含着对系统当前模式的辨识，辨识的结果即为模型切换概率，其数值大小体现了模型与真实模式的匹配程度。这种隐含的对系统模式辨识的方法反应速度慢，容易导致模型切换滞后，在模式发生变化时会降低估计精度。因此，本文在MM算法基础上考虑引入与系统模式存在因果关系的特征变量，通过特征变量直接参与对系统模式的预测。在AUV组合导航系统中，对载体当前机动状态的准确预测非常有利于导航定位，静止或匀速运动时传感器噪声较小；加速或转弯机动时则会激发出一些未知的误差因素，例如机械振动、电磁干扰以及杆臂效应分量等。对于某些误差因素，可以利用叠加高斯白噪声的方法进行等效，通过实验的方法可以找到等效高斯白噪声的参数。与AUV机动状态存在联系的特征变量都可以用来确定载体的机动情况，例如，电机的转速、推进器的力矩大小以及传感器输出等。

本文考虑三轴陀螺、三加速度计的捷联惯性导航系统（strapdown inertial navigation system，SINS）组合导航系统，以陀螺、加速度计的输出作为特征变量进行分析，当某个方向上加速度计的输出超过阈值λe，陀螺输出未达到阈值λb时，认为载体处于机动较弱的模式2；当某方向上陀螺输出超过阈值λb时，认为载体处于机动较强的模式3；二者都小于阈值时，认为载体处于比较平稳的状态模式1，反之，载体处于机动较强的模式3。由此建立如图2所示的BN，网络中主要包括5个变量，分别为：加速度计输出大于阈值λe（E）、陀螺输出大于阈值λb（B）、模式1（M1）、模式2（M2）、模式3（M3）。每个变量分别具有两种取值：1和0，分别代表“是”和“否”两种状态。

图2 组合导航系统BN

BN是对组合导航系统蕴含的因果关系的描述，当加速度计输出大于阈值λe，即事件E发生时，载体处于加速模式，即M2发生的概率为0.96；当陀螺输出大于阈值λb，即事件B发生时，载体处于转弯模式，即M3发生的概率为0.98；事件E和B均未发生时，载体处于平稳状态，即M1发生的概率为0.97。实际系统中，对载体所处机动状态的辨识可以借助更多特征变量，辨识的结果即为BN概率，按照第1．3节所描述的方法将BN概率引入到多模型估计中，对MM模型概率更新过程进行改进，即可把多模型估计扩展为广义的BN多模型估计。

3 SINS／DVL／MCP／TAN组合导航

由SINS、多普勒测速仪（Doppler velocity log，DVL）、磁航向仪（magnetic compass，MCP）和地形匹配（terrain aided navigation，TAN）设备构成的AUV组合导航系统如图3所示，DVL提供AUV在载体坐标系中的速度，MCP提供航向信息，TAN提供位置信息。传感器信息均输入到集中BN-IMM滤波器中进行信息融合，得到位置、速度和失准角估计。

图3 AUV组合导航系统

3.1 状态方程

以东北天（east-north-up，ENU）坐标系为导航坐标系（n），右前上（right-front-up，RFU）坐标系为载体坐标系（b）。列写15维的系统状态方程为

式中，状态变量

δVE、δVN、δVU为速度误差；φE、φN、φU为东向、北向和天向失准角；δL、δλ、δh为位置误差；εbx、εby、εbz为陀螺的常值漂移；为加速度计常值漂移；F（t）为状态转移矩阵［14］；W（t）为状态噪声，方差为Q（t）。

3.2 系统量测方程

将捷联惯导输出与其他传感器测量之差作为集中滤波器的量测值，其中DVL输出的速度是在载体坐标系（b）系中得到的，需要变换到导航坐标系（n）中［15-16］。观测方程可以表示为

式中，VDE、VDN和VDU是由载体坐标系转换到导航坐标系下的DVL的速度；VSE、VSN和VSU为SINS输出的速度；φM和φS为MCP和SINS所测得的航向角；LTN、λTN和hTN为TAN所测量的经度、纬度和高度；LSN、λSN和hSN为SINS得到的位置信息；Ψ（t）为观测噪声向量，方差为R（t）。观测矩阵H（t）可表示为

式中，（v×）为速度向量v＝［vEvNvU］的反对称矩阵，可表示为（v×）＝为东北天坐标系下载体的速度。

4 仿真实验

将提出的BN-IMM算法应用于AUV组合导航系统，并进行仿真验证。假设AUV的初始位置为东经118°，北纬32°，高度0 m，初始位置误差为0 m；初始速度为0 m／s，初始速度误差为0．1 m／s；SINS初始水平姿态误差角为6′，航向误差角为10′。设载体匀速运动时各传感器误差为：陀螺的随机常值漂移为0．03°／h，陀螺随机游走系数为加速度计常值偏置误差为2×10－4g，加速度计量测白噪声标准差为DVL测速误差的均方根为0．05 m／s；TAN的水平位置均方误差为10 m，深度测量均方误差为8 m；MCP偏航角均方误差为0．3°。SINS的采样周期为5 ms，DVL、TAN和MCP的采样周期为1 s。

AUV运动模拟海底典型的“割草机”运动［17］，轨迹如图4所示，仿真过程中，假设过程噪声Q和量测噪声R会随着载体机动状态的改变而发生变化，如表1所示。

图4 AUV航迹仿真曲线

表1 不同状态时噪声情况

表1中，Q0和R0分别为｛（0．05 m／s）2，（0．05 m／s）2，（0．05 m／s）2，（10 m）2，（10 m）2，（8 m）2，（0．3°／h）2｝。分别采用IMM算法和本文提出的BN-IMM算法进行滤波估计，采用的模型集包含3个模型，为了清楚地呈现模型转换的过程，将过程噪声和量测噪声分别取值为Q0，R0，3Q0、6R0和6Q0、12R0。Markov转移概率设为Ωij＝BN-IMM算法中，按照图2所示方法建立BN，根据陀螺和加速度计输出来判定AUV所处的运动模式；在进行模型概率更新时，式（17）中MM模型概率与BN概率进行求和的权值系数η设为0.5。

仿真时间为3 600 s，图5、图6给出了采用IMM、BN-IMM算法时系统中3个模型的概率，图7～图9分别为采用两种算法得到的姿态（纵摇角P、横摇角R和航向角H）、速度、位置估计误差。

图5 IMM模型概率

图6 BN-IMM模型概率

图7 IMM与BN-IMM姿态估计误差

图8 IMM与BN-IMM速度估计误差

图9 IMM与BN-IMM位置估计误差

对比图5、图6可以看出，IM M算法中模型概率切换出现了滞后现象，模型概率变化幅度很大，模型转换过程时间较长；BN-IMM算法由于引入了特征变量直接对系统的模式进行辨识，降低了对前一时刻模型概率和观测量的依赖程度，模型转换速度快，模型概率变化幅度较小。

从图7～图9可以看出，IM M与BN-IMM算法在AUV机动状态情况下，均能够很好地进行姿态、速度、位置估计，体现了多模型估计很强的抗干扰能力。在0～220 s，730～1 030 s，2 470～2 770 s，3 380～3 600 s匀速直线运动阶段，过程噪声和量测噪声较小，两种算法精度相当；在220～730 s，2 770～3 380 s加速和1 030～2 470 s转弯机动过程中，由于IMM算法中模型概率变化幅度很大，引起的姿态、速度、位置估计误差也存在很大的波动。将两种算法的位置估计误差进行比较，采用IMM算法得到的经度估计误差均方差为21 m，最大达到82．5 m；纬度估计误差均方差为21 m，最大达到72．8 m；高度估计误差均方差为20 m，最大达到81．6 m。采用BN-IM M算法得到的经度估计误差为14 m，最大为65．8 m；纬度估计误差均方差为13．5 m，最大为59．5 m；高度估计误差均方差为14 m，最大为53．7 m。可以看出，基于BN的BN-IMM算法性能优于IMM算法。

5 结 论

由于BN在描述非线性、时序性以及不确定性等方面具有显著的优势，本文将BN与当前多模型估计中应用最为广泛的IMM算法相结合，利用BN参数修正多模型估计算法中的模型概率，得出一种广义的BN多模型估计算法。根据AUV组合导航系统载体机动特性，将该算法应用于SINS、DVL、MCP和TAN构成的组合导航系统中，引入与载体机动状态相关的特征变量并建立BN，提出的BN-IMM算法模型能够提高AUV机动状态时模型切换的速度，解决IMM算法中存在的模型切换滞后问题。通过在Matlab环境下仿真，验证了提出的BN-IMM算法的优越性。

［1］Dunne D，Kirubarajan T．Multiple model multi-Bernoulli filters for manoeuvering targets［J］．IEEE Trans.on Aerospace and Electronic Systems，2013，49（4）：2679- 2692．

［2］Evans J S，Evans R J．Image-enhanced multiple model tracking［J］．Automatica，1999，35（11）：1769- 1786．

［3］Meskin N，Naderi E，Khorasani K．A multiple model-based approach for fault diagnosis of jet engines［J］．IEEE Trans.on Control Systems Technology，2013，21（1）：254- 262．

［4］Oliveira P．MMAE terrain reference navigation for underwater vehicles using PCA［J］．International Journal of Control，2007，80（7）：1008- 1017．

［5］Blom H A P，Bar-Shalom Y．The interacting multiple model algorithm for systems with Markov switching coefficients［J］．IEEE Trans.on Automatic Control，1988，33（8）：780- 783．

［6］Li X R，Jilkov V P，Ru J F，et al．Multiple-model estimation with variable structure part VI：expected-mode augmentation［J］．IEEE Trans.on Aerospace and Electronic systems，2005，41（3），853- 867．

［7］Liang Y，Cheng Y M，Jia Y G，et al．Analysis on the performance and properties of interacting multiple models algorithm［J］．Control Theory and Applications，2001，18（4）：487- 492．（梁彦，程咏梅，贾宇岗，等．交互式多模型算法性能分析［J］．控制理论与应用，2001，18（4）：487- 492．）

［8］Luo X B，Wang H Q，Li X．Interacting multiple model algorithm with adaptive Markov transition probabilities［J］．Journal of Electronics and Information Technology，2005，27（10）：1539 -1541．（罗笑冰，王宏强，黎湘．模型转移概率自适应的交互式多模型跟踪算法［J］．电子与信息学报，2005，27（10）：1539- 1541．）

［9］Liang Y，Zhou D H，Pan Q．Multiple model estimation represented by Bayesian networks［C］∥Proc.of the 4th World Congress on Intelligent Control and Automation，2002：863- 866．

［10］Guo Z，Hao Y，Sun F．A new method to improve the maneuver capability of AUV integrated navigation systems［J］．Journal of Computers，2010，5（5）：757- 764．

［11］Thurman E，Riordan J，Toal D．Real-time adaptive control of multiple colocated acoustic sensors for an unmanned underwater vehicle［J］．IEEE Journal of Oceanic Engineering，2013，38（3）：419- 432．

［12］Maki T，Matsuda T，Sakamaki T，et al．Navigation method for underwater vehicles based on mutual acoustical positioning with a single seafloor station［J］．IEEE Journal of Oceanic Engineering，2013，38（1）：167- 177．

［13］Finn V J．Bayesian networks and decision graphs［M］．Springer：Information Science and Statistics，2001．

［14］Li Y，Xu X S，Wu B X．Observable degree of information matching in AUV integrated navigation［J］．Journal of Chinese Inertial Technology，2008，16（5）：589- 594．（李瑶，徐晓苏，吴炳祥．AUV组合导航系统信息匹配的可观测度［J］．中国惯性技术学报，2008，16（5）：589- 594．）

［15］Han S，Wang J．A novel initial alignment scheme for low-cost INS aided by GPS for land vehicle applications［J］．Journal of Navigation，2010，63（4），663- 680．

［16］Li W，Wang J，Lu L，et al．A novel scheme for DVL-aided SINSin-motion alignment using UKF techniques［J］．Sensors，2013，13（1），1046- 1063．

［17］Hegren s，Hallingstad O．Model-aided ins with sea current estimation for robust underwater navigation［J］．IEEE Journal of Oceanic Engineering，2011，36（2）：316- 337．

Improved multiple model algorithm based on Bayesian network for AUV integrated navigation

WANG Lei1，2，CHENG Xiang-hong1，2，RAN Chang-yan1，2，CHEN Hong-mei1，2，HU Jie1，2
（1.School of Instrument Science and Engineering，Southeast University，Nanjing 210096，China；2.Key Laboratory of Micro Inertial Instrument and Advanced Navigation，
Southeast University，Nanjing 210096，China）

An improved interactive multiple model filter based on Bayesian network（BN-IMM）is proposed．The aim is to resolve the problem when the noise of the autonomous underwater vehicle（AUV）integrated navigation system in the tough environment is uncertain or time-varying．The proposed algorithm builds a Bayesian network according to the relationship of characteristic variables and the system model．The parameters of the Bayesian network are used to correct the model probabilities in the interactive multiple model（IMM）algorithm which can reduce the dependence to the prior knowledge in the real mode recognition of the system．The proposed method can solve the problems of time lag in model transformation and probability jump in the IMM algorithm．The outputs of gyros and accelerometers are used as characteristic variables to establish the Bayesian network．Simulation results show that the BN-IMM algorithm can improve the model converting speed and the precision of estimation significantly when the AUV is in maneuvering state．

autonomous underwater vehicle（AUV）；integrated navigation；multiple model cstimation；Bayesian network

U 666．1

A

10．3969／j．issn．1001-506X．2015．04．27

王 磊（1984-），男，博士研究生，主要研究方向为多传感器信息融合技术及其在自主水下航行器中的应用。E-mail：frank_408＠163．com

程向红（1963 ），通讯作者，女，教授，博士，主要研究方向为惯性导航技术、组合导航系统理论与方法。E-mail：xhcheng＠seu．edu．cn

冉昌艳（1974-），女，博士研究生，主要研究方向为非线性滤波及其在水下导航系统中的应用。E-mail：ranchangyan＠126．com

陈红梅（1977-），女，博士研究生，主要研究方向为高斯过程回归与组合导航算法研究。E-mail：ldyichm＠163．com

胡 杰（1977-），男，博士研究生，主要研究方向为捷联惯性导航技术、自适应滤波。E-mail：hj_student＠163．com

1001-506X（2015）04-0901-06

2014- 01- 06；

2014- 09- 05；网络优先出版日期：2014- 10- 28。

网络优先出版地址：http：∥w ww．cnki．net／kcms／detail／11．2422．TN．20141028．1612．007．html

国家自然科学基金（61374215）资助课题