CN104833357A - 一种多系统多模型混合交互式信息融合定位方法 - Google Patents

Abstract

本发明公开了一种多系统多模型混合交互式信息融合定位方法，针对导航定位过程中单一观测系统定位范围有限，单一运动模型描述运动状态不准确的问题，提出了从运动模型、观测系统两个维度，采用多模型多系统对目标进行跟踪定位的方法；该方法将多模型和多系统的定位结果按照模型概率、系统概率进行加权融合，很好的发挥了多系统共同定位和多模型并行建模定位的性能优势。

Description

一种多系统多模型混合交互式信息融合定位方法

技术领域

本发明涉及导航与制导技术领域，尤其涉及一种基于贝叶斯理论的多系统多模型混合交互式信息融合定位方法。

背景技术

传统的导航定位算法主要通过建立机动目标的运动状态模型和外界系统对目标的观测模型来进行滤波，获得机动目标的准确定位信息。但是一般情况下，机动目标的运动方式存在随机性，基本很难通过一种特定的模型完成对机动目标的运动进行建模描述。此外单一的定位系统对目标进行观测时，起作用范围有限，比如GNSS卫星导航在城市高楼间、树荫下、峡谷中等复杂环境下，定位性能受限。

针对机动目标运动状态多变而无法准确建模的问题，研究人员提出了交互式多模型(interacting multiple model，IMM)的算法，通过用多个模型同时对机动目标的运动状态进行建模描述，滤波前先将各模型前一采样时刻的状态向量根据贝叶斯理论进行混合，然后对各模型进行并行滤波，并根据滤波产生的新息计算各模型的概率，将各模型的滤波结果按照模型概率进行融合，融合结果做为最终的定位结果进行输出。与传统的单模型滤波相比，IMM算法能够有效提高运动状态多变的机动目标的定位性能。

针对机动目标运动中环境不断变化情况引起的单系统定位范围、性能受限问题，各种非GNSS导航定位方法被广泛研究，如：基于wifi网络、视觉、RFID的导航定位等。将各种导航定位手段的信息进行融合的算法也研究较多，如基于贝叶斯网络的融合算法、基于DS证据理论的融合算法、基于联邦结构的融合 算法等。其中联邦结构的融合算法研究较多，并且已经在工程中获得应用。其基本原理是先将各种系统的导航定位信息进行分布式滤波，然后将分布式滤波结果根据定位误差进行加权融合，获得较高精度的定位结果。

尽管针对机动目标定位的状态模型和系统模型两个方面的研究都有不少，但是还没有一种方案将两方面的问题综合考虑，观测系统的模型和运动状态模型的匹配直接决定着机动目标的导航定位滤波的效果。

发明内容

有鉴于此，本发明提供了一种多系统多模型混合交互式信息融合定位方法，能够通过多观测系统协同定位实现多个运动模型下的机动目标导航定位。

本发明的一种多系统多模型混合交互式信息融合定位方法，包括如下步骤：

步骤1、在导航定位过程中，针对待定位的目标，采用该目标可能的运动模型描述其运动状态，确定该目标的状态向量；同时，采用多个可用的观测系统对目标进行观测，确定观测系统获得的观测信息与目标的状态向量之间的函数关系；其中，运动模型的个数设为N个，观测系统的个数设为M个；

步骤2、对目标的N个运动模型进行混合，具体为：

将第n个运动模型的状态向量与其它各运动模型进行混合：针对第n个运动模型，将第n个运动模型与其它各运动模型之间的条件概率作为加权因子，将t-1时刻的各运动模型对应的状态向量进行加权求和处理，得到第n个运动模型的混合后的t-1时刻的状态向量；其中，n＝1,2，…N；

将第n个运动模型的状态误差与N个运动模型中其它各运动模型进行混合：先计算在该运动模型n下，运动模型n’在t时刻的状态误差初值，即：将运动模型n’的t-1时刻状态向量与运动模型n在t时刻状态向量之间的误差方差加上运动模型n’在t-1时刻状态误差而得到的和值；其中，n’＝1,2，…N；分别将运 动模型n与所述N个运动模型之间的条件概率作为加权因子，对得到的运动模型n下各个运动模型对应的状态误差初值进行加权求和，则得到第n个运动模型混合后的t-1时刻状态误差；

步骤3、基于步骤1中建立的所述函数关系，采用M个观测系统的观测信息分别对第n个运动模型对应的混合后的t-1时刻状态向量和状态误差进行滤波，获得t时刻第n个模型在各观测系统下的状态向量和状态误差，以及获得第n个运动模型下各观测系统对应的新息和新息方差；

步骤4、针对步骤3获得的第n个运动模型对应的所有观测系统的新息和新息方差，通过贝叶斯全概率公式，计算第n个模型下各观测系统的概率；以各观测系统概率为加权因子，将步骤3获得的第n个模型在各观测系统下的状态向量进行加权求和，获得t时刻多系统融合后运动模型n的状态向量；

同理，以观测系统概率为加权因子，对步骤3获得的第n个模型在各观测系统下的状态误差进行加权求和，获得t时刻多系统融合后运动模型n的状态误差；

步骤5、将各个运动模型的后验概率作为加权因子，将步骤4获得的多系统融合后各运动模型的状态向量进行加权求和，得到t时刻多系统多模型融合后的状态向量；

将各个运动模型的后验概率作为加权因子，将步骤4获得的多系统融合后各运动模型的状态误差进行加权求和，得到t时刻多系统多模型融合后的状态误差；

所述t时刻多系统多模型融合后的状态向量和状态误差之和即为当前t时刻的定位信息。

本发明具有如下有益效果：

(1)本发明针对导航定位过程中单一观测系统定位范围有限，单一运动模 型描述运动状态不准确的问题，提出了从运动模型、观测系统两个维度，采用多模型多系统对目标进行跟踪定位的方法。该方法将多模型和多系统的定位结果按照模型概率、系统概率进行加权融合，很好的发挥了多系统共同定位和多模型并行建模定位的性能优势。

附图说明

图1为本发明中多模型融合的系统框图。

图2为本发明中多模型融合的原理图。

图3为本发明的定位方法的流程图。

具体实施方式

下面结合附图并举实施例，对本发明进行详细描述。

1、目标定位的“多系统多模型”建模

一般情况下，对一个目标进行导航定位(或者称作跟踪滤波)，需要两个方程——“运动模型”及“观测模型”。

“运动模型”就是对目标的运动过程的一种数学描述，比如一个正在进行匀速直线运动的目标可以描述为：x(t+Δt)＝x(t)+vΔt做匀加速直线运动的目标可以描述为： $x(t+\mathrm{\Δt})=x\left(t\right)+\mathrm{v\Δt}+\frac{1}{2}a{\mathrm{\Δt}}^2.$

“观测模型”是目标外的其他系统对目标观测的一种数学描述，即建立从目标状态信息到观测信息之间的一种函数关系，比如通过GPS卫星观测运动目标可以建立卫星观测信息“伪距”和目标状态信息“X,Y,Z坐标”之间的函数关系：z(t)＝f(x(t),v(t))；再如通过移动基站观测运动目标可以建立基站观测信息“移动信号强度”和目标状态信息“X，Y，Z坐标”之间的函数关系。

本发明所指“多模型”是说针对特定的运动目标，一般很难用一种运动模 型描述。比如路面上的运动车辆在Δt时间内，可能先做匀速直线运动，然后在路况较好的时候开始加速，看到前方红灯又进行减速运动，在十字路口进行了拐弯运动，整个过程中涉及到四种运动。所以用单一的一个运动模型去描述已经不够。所以考虑采用多个运动模型对目标运动状态进行描述，描述的状态方程统一记为：

x(t)＝A^px^p(t-1)+G^pw^p(t)

上标p表示一个运动集合，针对上边的例子这里p表示集合； 

{匀速直线运动，匀加速直线运动，转弯运动}

A表示t-1目标状态到t时刻目标状态函数关系(用矩阵表示)。例如当p为匀速直线运动时，状态向量 $x^p\left(t\right)=\left(\begin{array}{c}x\left(t\right)\\ v\left(t\right)\end{array}\right),$ 根据运动学定律，矩阵 $A=\left[\begin{array}{cc}1& \mathrm{\Δt}\\ 0& 1\end{array}\right],$ 则：

$x^p\left(t\right)=A^p{x}^p(t-1)\⇒\left(\begin{array}{c}x(t+\mathrm{\Δt})\\ v\left(t\right)\end{array}\right)=\left[\begin{array}{cc}1& \mathrm{\Δt}\\ 0& 1\end{array}\right]\×\left(\begin{array}{c}x\left(t\right)\\ v\left(t\right)\end{array}\right)$

w^p(t)是噪声，G^p是噪声作用到状态变量的函数变化关系。

本发明所指“多系统”是说针对某一运动目标，一个系统的观测很难覆盖，需要多个系统。比如，运动车辆在比较开阔的道路上行进时，GPS卫星导航系统和移动基站信号可以对其进行定位，但是当车辆进入隧道后这两种信号都消失，但是如果车上装有wifi芯片，可以通过隧道里边的AP热点进行定位，本发明采用多个定位系统对目标定位；这里将第i个系统的观测信息(GPS系统伪距，移动基站的信号强度)和目标状态信息(XYZ坐标)之间的函数关系统一表示为：

z_i(t)＝H_ix(t)+v_i(t)

下标i表示观测系统集合，针对上边的例子这里i表示：

{GPS系统，移动基站，wifi系统}

H表示从目标状态到系统观测信息之间的函数关系(用矩阵表示)，例如当i为GPS系统时，矩阵H表示从目标状态x(t)(XYZ坐标)到卫星观测伪距之间的函数关系。

2、多模型混合：

一般情况下，导航定位的滤波过程是一个迭代过程——根据上一时刻的目标状态信息x(t-1)(如XYZ坐标)，和下一时刻的系统观测信息z(t)(如GPS观测得到的目标伪距)，根据之前所述两个方程“状态方程”和“观测方程”进行滤波。

本发明重点不在滤波算法本身，而是基于“多模型”和“多系统”的多维跟踪定位并行处理，以及并行滤波中涉及到的预处理、融合处理算法的发明。

为便于理解，本示例中假设目标只作两种运动——“匀速直线运动和匀加速直线运动”；只有两个系统对目标进行观测——“GPS和移动基站”。本发明所设计的“多模型多系统交互处理”的系统框图如图1：

多模型混合就是将上一时刻各个滤波器的定位结果进行混合，混合的方法是按照条件概率加权。

(1)状态向量的混合：将第n个运动模型的状态向量与其它各运动模型进行混合：针对第n个运动模型，将第n个运动模型与其它各运动模型之间的条件概率作为加权因子，将t-1时刻的各运动模型对应的状态向量进行加权求和处理，得到第n个运动模型的混合后的t-1时刻的状态向量；其中，n＝1,2，…N；

如图1所示，模型1(混合后)的数据来自于混合前两个模型的数据按照条件概率加权后的和。

x₁(t-1)_混合后＝x₁(t-1)_混合前×p_{混合前|混合后}(x₁|x₁)+x₂(t-1)_混合前×p_{混合前|混合后}(x₂|x₁) 

同理，混合后模型2的状态信息为：

x₂(t-1)_混合后＝x₁(t-1)_混合前×p_{混合前|混合后}(x₁|x₂)+x₂(t-1)_混合前×p_{混合前|混合后}(x₂|x₂) 

混合时，加权条件概率表示从混合前某一模型到混合后某一模型的概率，如p_{混合前|混合后}(x₁|x₁)表示的是混合后x₁来自混合前x₁的概率。

混合概率即加权因子是通过计算得来的，其依据的是贝叶斯概率理论，以p_{混合前|混合后}(x₁|x₂)的推导为例：

其中p_{混合后|混合前}(x₂|x₁)为从一个模型转移到另一个模型的概率，简单起见，这个概率是提前给定的；p_混合前(x₁)是混合前的模型概率，初次滤波时认为各个模型概率相等后续滤波过程中，不断更新。

(2)目标定位误差混合

由于滤波中不光用到目标的定位状态信息x，还要用到误差方差P＝(x-x_真值)²，所以也需要对定位误差方差进行混合，将第n个运动模型的状态误差与N个运动模型中其它各运动模型进行混合：先计算在该运动模型n下，运动模型n’在t时刻的状态误差初值，即：将动模型n’的t-1时刻状态向量与运动模型n在t时刻状态向量之间的误差方差加上运动模型n’在t-1时刻状态误差而得到的和值；其中，n’＝1,2，…N；分别将运动模型n与所述N个运动模型之间的条件概率作为加权因子，对得到的运动模型n下各个运动模型对应的状态误差初值进行加权求和，则得到第n个运动模型混合后的t-1时刻状态误差；比如混合后模型1的误差方差为：

混合后模型2的误差方差： 

(x₁(t-1)_混合前-x₁(t-1)_混合后)²表示混合前后两状态间的误差偏差；

3、基于步骤1中建立的所述函数关系，采用M个观测系统的观测信息分别对第n个运动模型对应的混合后的t-1时刻状态向量和状态误差进行滤波，获得t时刻第n个模型在各观测系统下的状态向量和状态误差，以及获得第n个运动模型下各观测系统对应的新息和新息方差；

以双系统为例，混合后的两模型状态信息x₁(t-1)_混合后和x₂(t-1)_混合后，分别和两个观测系统的观测信息(GPS、移动基站)z₁(t)和z₂(t)进行滤波。

这里滤波器可以自由选择，不属于本发明的内容，这里采用最简单的扩展卡尔曼滤波器(extended kalman filter-EKF)，该滤波器的详细算法流程在任何一本导航定位的书籍中都能找到，这里只给出滤波计算公式，不展开讲解。

x_i(t|t-1)＝A_ix_i(t-1)  i＝1,…，N

v_i,j(t)＝z_j(k)-A_ix_i(t-1)  j＝1,…,M

$P_t\left(t\right|t-1)=A_i{P}_i(t-1)A_i^T$

$S_{i,j}\left(t\right)=H_j{P}_i\left(t\right|t-1)H_j^T+R_j\left(t\right)$

$K_{i,j}\left(t\right)=P_i\left(t\right|t-1)H_j^T{S}_{i,j}^{-1}\left(t\right)$

x_i(t)＝x_i(t|t-1)+K_i,j(t)v_i,j(t) 

$P_i\left(t\right|t)=P_i\left(t\right|t-1)-K_{i,j}\left(t\right)S_{i,j}\left(t\right)K_{i,j}^T\left(t\right)$

4、多系统交互 

“多系统交互”是本发明的关键算法之一，其基本思想是将某一模型下的多个观测系统所对应的滤波结果进行融合。

针对步骤3获得的第n个运动模型对应的所有观测系统的新息和新息方差，通过贝叶斯全概率公式，计算第n个模型下各观测系统的概率；以各观测系统概率为加权因子，将步骤3获得的第n个模型在各观测系统下的状态向量进行加权求和，获得t时刻多系统融合后运动模型n的状态向量；

同理，以观测系统概率为加权因子，对步骤3获得的第n个模型在各观测系统下的状态误差进行加权求和，获得t时刻多系统融合后运动模型n的状态误差；

以上述的双模型双系统为例，匀速直线运动模型下的“多系统交互”就是将对应的两个并行滤波器(GPS定位系统、移动基站定位系统)所获得的定位信息按照系统概率进行加权融合：

x_CV(t|t)＝x_GPS|CV(t|t)×p{ε_GPS(t)|M_CV(t),Z^t}+ 

x_基站|CV(t|t)×p{ε_基站(t)|M_CV(t),Z^t} 

x_GPS|CV(t|t)是CV模型下，GPS观测系统滤波器的定位结果；

x_基站|CV(t|t)是CV模型下，基站系统滤波器的定位结果；

p{ε_GPS(t)|M_CV(t),Z^t}——匀速直线运动模型下GPS的系统概率，其计算推导是本发明的关键点之一主要依据是贝叶斯理论。其计算方法为：

其中：

是在CV模型下，以GPS观测系统观测量为输入,进行滤波所得的似然函数。代表高斯分布，v_GPS,CV(t),S_GPS,CV(t)代表滤波器滤波所获得的新息和新息协方差。同理，其 中v_基站,CV(t),S_基站,CV(t)为CV模型下，基站观测输入所对应的滤波器的滤波新息和新息协方差。

p{ε_GPS(t)|M_CV(t),Z^t-1}是CV模型下GPS的系统预测概率，其计算依据t-1时刻各系统的系统概率与各系统到GPS系统的转移概率乘积之和即：

p{ε_GPS(t)|M_CV(t),Z^t-1}＝p{ε_GPS(t)|ε_GPS(t-1)}×p{ε_GPS(t-1)|Z^t-1}+

p{ε_GPS(t)|ε_基站(t-1)}×p{ε_基站(t-1)|Z^t-1}

同理，也可计算获得p{z(t)|ε_基站(t),M_CV(t),Z^t-1}和p{ε_基站(t)|M_CV(t),Z^t-1}；

最后，按照贝叶斯后验概率理论计算获得p{ε_GPS(t)|M_CV(t),Z^t}，同理也可计算出p{ε_基站(t)|M_CV(t),Z^t}。

5、多系统下的多模型混合

通过步骤4的论述，实现了在特定模型下的多系统融合，最后一个步骤，也是本发明的最后一个关键点就是在多系统融合的条件下，进行多模型的融合。其融合的基本思路是将之前针对每个模型的结果按照模型的后验概率进行加权求和，即将各个运动模型的后验概率作为加权因子，将步骤4获得的多系统融合后各运动模型的状态向量进行加权求和，得到t时刻多系统多模型融合后的状态向量；

将各个运动模型的后验概率作为加权因子，将步骤4获得的多系统融合后各运动模型的状态误差进行加权求和，得到t时刻多系统多模型融合后的状态误差；t时刻多系统多模型融合后的状态向量和状态误差之和即为当前t时刻的定位信息。

以之前的双系统双模型为例，分别将CV模型下GPS和移动基站滤波融合结果x_CV(t|t)，和CA模型下GPS和移动基站滤波融合结果x_CA(t|t)，按照模型后验概率进行加权融合：

x(t|t)＝x_CV(t|t)×p{M_CV(t)|Z^t}+x_CA(t|t)×p{M_CV(t)|Z^t} 

x_CV(t|t)是CV模型下GPS和基站系统的定位融合结果，x_CA(t|t)是CA模型下GPS和基站两系统定位融合结果；

p{M_CV(t)|Z^t}是t时刻CV模型后验概率；

p{M_CA(t)|Z^t}是t时刻CA模型后验概率；

模型后验概率的计算依据贝叶斯公式，以p{M_CV(t)|Z^t}为例进行说明(p{M_CA(t)|Z^t}的推导类似，不再赘述)：

$p\left\{M_{\mathrm{CV}}\left(t\right)\right|Z^t\}=\frac{p\left\{z\left(t\right)\right|M_{\mathrm{CV}}\left(t\right),Z^{t-1}\}\×p\{M_{\mathrm{CV}}\left(t\right)\left|Z^{t-1}\right\}}{p\left\{z\left(t\right)\right|M_{\mathrm{CV}}\left(t\right),Z^{t-1}\}\×p\{M_{\mathrm{CV}}\left(t\right)\left|Z^{t-1}\right\}+p\left\{z\left(t\right)\right|M_{\mathrm{CA}}\left(t\right),Z^{t-1}\}\×p\{M_{\mathrm{CA}}\left(t\right)\left|Z^{t-1}\right\}}$

其中：

①p{z(t)|M_CV(t),Z^t-1}是CV模型的似然概率，可根据贝叶斯全概率公式推导获得：

p{z(t)|M_CV(t),Z^t-1}＝p{z(t)|ε_GPS,M_CV(t),Z^t-1}×p{ε_GPS|M_CV(t),Z^t-1}+ 

p{z(t)|ε_基站,M_CV(t),Z^t-1}×p{ε_基站|M_CV(t),Z^t-1} 

而p{z(t)|ε_GPS,M_CV(t),Z^t-1}和p{ε_GPS|M_CV(t),Z^t-1}的推导参考步骤4。

②p{M_CV(t)|Z^t-1}是CV模型的预测概率，可根据贝叶斯全概率公式推导获得：

p{M_CV(t)|Z^t-1}＝p{M_CV(t)|M_CV(t-1)}×p{M_CV(t-1)|Z^t-1}+ 

p{M_CV(t)|M_CA(t-1)}×p{M_CA(t-1)|Z^t-1} 

总结：以上关于多系统多模型混合交互定位融合方法的说明中，采用两模型两系统(CA、CV模型，GPS系统、基站系统)进行举例。但该发明适用范围不仅限于两系统两模型，在处理多系统多模型的情况时，方法类似。

综上所述，以上仅为本发明的较佳实施例而已，并非用于限定本发明的保护范围。凡在本发明的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。

Claims (1)

1.一种多系统多模型混合交互式信息融合定位方法，其特征在于，包括如下步骤：

步骤1、在导航定位过程中，针对待定位的目标，采用该目标可能的运动模型描述其运动状态，确定该目标的状态向量；同时，采用多个可用的观测系统对目标进行观测，确定观测系统获得的观测信息与目标的状态向量之间的函数关系；其中，运动模型的个数设为N个，观测系统的个数设为M个；

步骤2、对目标的N个运动模型进行混合，具体为：

将第n个运动模型的状态向量与其它各运动模型进行混合：针对第n个运动模型，将第n个运动模型与其它各运动模型之间的条件概率作为加权因子，将t-1时刻的各运动模型对应的状态向量进行加权求和处理，得到第n个运动模型的混合后的t-1时刻的状态向量；其中，n＝1,2，...N；

将第n个运动模型的状态误差与N个运动模型中其它各运动模型进行混合：先计算在该运动模型n下，运动模型n’在t时刻的状态误差初值，即：将运动模型n’的t-1时刻状态向量与运动模型n在t时刻状态向量之间的误差方差加上运动模型n’在t-1时刻状态误差而得到的和值；其中，n’＝1,2，...N；分别将运动模型n与所述N个运动模型之间的条件概率作为加权因子，对得到的运动模型n下各个运动模型对应的状态误差初值进行加权求和，则得到第n个运动模型混合后的t-1时刻状态误差；

步骤3、基于步骤1中建立的所述函数关系，采用M个观测系统的观测信息分别对第n个运动模型对应的混合后的t-1时刻状态向量和状态误差进行滤波，获得t时刻第n个模型在各观测系统下的状态向量和状态误差，以及获得第n个运动模型下各观测系统对应的新息和新息方差；

步骤4、针对步骤3获得的第n个运动模型对应的所有观测系统的新息和新息方差，通过贝叶斯全概率公式，计算第n个模型下各观测系统的概率；以各观测系统概率为加权因子，将步骤3获得的第n个模型在各观测系统下的状态向量进行加权求和，获得t时刻多系统融合后运动模型n的状态向量；

同理，以观测系统概率为加权因子，对步骤3获得的第n个模型在各观测系统下的状态误差进行加权求和，获得t时刻多系统融合后运动模型n的状态误差；

步骤5、将各个运动模型的后验概率作为加权因子，将步骤4获得的多系统融合后各运动模型的状态向量进行加权求和，得到t时刻多系统多模型融合后的状态向量；

将各个运动模型的后验概率作为加权因子，将步骤4获得的多系统融合后各运动模型的状态误差进行加权求和，得到t时刻多系统多模型融合后的状态误差；

所述t时刻多系统多模型融合后的状态向量和状态误差之和即为当前t时刻的定位信息。