CN112346479B - 一种基于集中式卡尔曼滤波的无人航行器状态估计方法 - Google Patents

Abstract

本发明公开了一种基于集中式卡尔曼滤波的无人航行器状态估计方法，包括以下步骤：建立无人航行器关于位置和速度的状态方程和量测方程；预测无人航行器的下一时刻速度和位置，得到无人航行器的下一时刻速度预测值和位置预测值，建立下一时刻速度和位置真值的分布函数，通过引入置信概率，得到无人航行器当前时刻可靠的速度和位置量测值；基于新型集中式卡尔曼滤波器的结构，确定无人航行器更新过程的协方差和卡尔曼滤波器的增益，再通过结合当前时刻速度和位置量测值得到无人航行器的位置信息和速度信息最优估计值，将此方法具有置信检验环节的集中式卡尔曼滤波，对速度和位置信息的最终估计值更加准确。

Description

一种基于集中式卡尔曼滤波的无人航行器状态估计方法

技术领域

本发明涉及传感器信息融合领域，尤其涉及一种基于集中式卡尔曼滤波的无人航行器状态估计方法。

背景技术

对线性系统进行分析时，集中式卡尔曼滤波是一种常用的参数估计方法，多个子系统的观测信息在集中卡尔曼滤波中被同时处理，所有相关的状态量在同一个全局滤波器中被处理，最后得到的结果为多个参数的最优估计解。集中式卡尔曼滤波器结构中由于其主滤波器需对全部子滤波器的估计值进行处理，故其具备相应的缺点：由于多个传感器为并行估计，当任意一个传感器出现故障时，最终的估计值将会受到影响；

采用集中式卡尔曼滤波进行状态估计时，当任一传感器信息出现不可靠测量时，最终估计值的精度将会受到或多或少的影响。

发明内容

根据现有技术存在的问题，本发明公开了一种基于集中式卡尔曼滤波的无人航行器状态估计方法，包括以下步骤：

S1:建立无人航行器关于位置和速度的状态方程和量测方程；

S2:根据无人航行器的状态方程，预测无人航行器的下一时刻速度和位置，得到无人航行器的下一时刻速度预测值和位置预测值；

S3:根据无人航行器的下一时刻速度预测值和位置预测值，建立下一时刻速度和位置真值的分布函数，下一时刻速度和位置真值应服从期望为预测值，方差为系统噪声方差的正态分布，通过引入置信概率，得到无人航行器当前时刻可靠的速度和位置量测值；

S4:基于新型集中式卡尔曼滤波器的结构，确定无人航行器更新过程的协方差和卡尔曼滤波器的增益，再通过结合当前时刻速度和位置量测值得到无人航行器的位置信息和速度信息最优估计值。

进一步地，所述通过引入置信概率，得到无人航行器当前时刻可靠的速度和位置量测值，过程如下：

通过引入置信概率，判断无人航行器当前时刻速度和位置量测值是否可靠；当无人航行器当前时刻速度和位置量测值可靠，则采用当前时刻速度和位置量；当无人航行器当前时刻速度和位置量测值不可靠，利用量测方程获得无人航行器的速度和位置可靠的量测值。

进一步地，所述新型集中式卡尔曼滤波器包括N个子滤波器，所述子滤波器的卡尔曼增益如下：

其中：K_l(k)为第l个子滤波器在k时刻的增益，X(k)为k时刻的状态值，X(k|k-1)为对k时刻进行预测的预测值，Z_l(k)为第l个子滤波器在k时刻的量测值，Z_l(k|k-1)为第l个子滤波器对k时刻进行预测的预测量测值。

进一步地，所述速度和位置量测值是否可靠的判断方法如下：

引入一个置信概率α，为使得速度和位置量测值处于置信区间内，根据高斯分布的分布函数Φ(x)，则可令：

当Z(k)∈(x₁,x₂]时，其中x₁+x₂＝2X′，下一时刻的Z(k)属于可靠测量，其中，Z(k)表示当前时刻的量测值。

由于采用了上述技术方案，本发明提供的一种基于集中式卡尔曼滤波的无人航行器状态估计方法，该方法引入一个置信概率，在每次预测过程中量测值均与状态预测值进行对比，对于位于置信区间之内的量测值选择相信，对于位于置信区间之外的量测值进行剔除，进行野值剔除后考虑使用前一时刻的最优估计值进行下一时刻的预测，将此方法应用于机动目标中的CV模型进行验证后，仿真实验验证了当传感器出现故障时，具有置信检验环节的集中式卡尔曼滤波对速度和位置信息的最终估计值更加准确。

附图说明

为了更清楚地说明本申请实施例或现有技术中的技术方案，下面将对实施例或现有技术描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本申请中记载的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他的附图。

图1为具备置信检验的最优估计集中式卡尔曼滤波器结构图；

图2为不同置信概率下CV模型的最优位置估计图；

图3为不同置信概率下CV模型的最优速度估计图；

图4为不同置信概率下CV模型的位置误差对比图；

图5为不同置信概率下CV模型的速度误差对比图。

具体实施方式

为使本发明的技术方案和优点更加清楚，下面结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚完整的描述：

一种基于集中式卡尔曼滤波的无人航行器状态估计方法，主要可应用于包含多个传感器测量信息的导航定位、目标跟踪等领域；该方法包括以下步骤：

S1:建立无人航行器关于位置和速度的状态方程和量测方程；

S2:根据无人航行器的状态方程，预测无人航行器的下一时刻速度和位置，得到无人航行器的下一时刻速度预测值和位置预测值；

S3:根据无人航行器的下一时刻速度预测值和位置预测值，建立下一时刻速度和位置真值的分布函数，下一时刻速度和位置真值应服从期望为预测值，方差为系统噪声方差的正态分布，通过引入置信概率，得到无人航行器当前时刻可靠的速度和位置量测值；

S4:基于新型集中式卡尔曼滤波器的结构，确定无人航行器更新过程的协方差和卡尔曼滤波器的增益，再通过结合当前时刻速度和位置量测值得到无人航行器的位置信息和速度信息最优估计值。

在传统集中式卡尔曼滤波的融合结构中，需使用下一时刻对应的量测值对下一时刻的状态进行最优估计，针对每次利用量测值进行估计的环节，考虑对量测值的可靠性进行判断，即是否包含野值，通过引入一个置信概率，对每次下一阶段预测时使用的量测值进行判断，如若量测值在置信区间内，则可认为本时刻的量测值值得信赖，可以用以进行下一时刻的预测过程，同时根据这一处理环节的加入，每次预测时均使用主滤波器得到的最优估计值进行下一阶段的预测，提高最终的估计精度。

传统集中式卡尔曼滤波的状态方程和量测方程为：

对于多传感器测量系统来说，状态方程不变，量测方程可表示为：

其中，v(k)和w_l(k)(l＝1,2,…,n)均为零均值高斯白噪声，其协方差分别为Q和R_l，且满足E[w_l(k)(w_m(k))^T]＝0(m≠l,m∈{1,2,…,n})，其中，l＝1,2,…,n，为子滤波器个数，X(k+1)为无人航行器位置与速度估计系统对k时刻的状态估计值，F为无人航行器位置与速度估计系统的状态转移矩阵，X(k)为无人航行器位置与速度估计系统k时刻的状态变量，v(k)为无人航行器位置与速度估计系统在k时刻的预测噪声，Z_i(k)为无人航行器位置与速度估计系统子滤波器l在k时刻的量测值，H_l为无人航行器位置与速度估计系统子滤波器l在k时刻的量测矩阵，w_l(k)为无人航行器位置与速度估计系统子滤波器l在k时刻的量测噪声。

对各个传统子滤波器而言，无人航行器的位置和速度预测过程为：

X_l(k|k-1)＝FX_l(k|k-1)(l＝1,2,…n) (3)

P_l(k|k-1)＝FP_l(k-1|k-1)F^T+Q (4)

K_l(k)＝P_l(k|k-1)H^T(HP_l(k|k-1)H^T+R)^-1 (5)

其中，P_l(k+1|k)为子滤波器l的一步预测误差协方差矩阵，X_l(k+1|k)为子滤波器l的状态一步预测，K_l(k)为子滤波器l的增益矩阵。

更新过程为：

X_l(k|k)＝X_l(k|k-1)+K_l(k)(Z_l(k)-H_lX_l(k|k-1))(l＝1,2,…n) (6)

P_l(k|k)＝(I-K_l(k)H)P_l(k|k-1) (7)

每一时刻的最优估计值可表述为：

在每次对下一阶段进行预测时，受各方面因素的影响，量测值均存在一定的不可靠性，为避免不可靠测量对最终的估计结果造成影响，应对量测值进行可靠性估计。

对任何一个子滤波器的任意一个时刻而言，下一时刻状态的预测值为：

X_l(k|k-1)＝FX_l(k-1|k-1) (9)

为判定量测值的可靠度，设预测值为X′，可认为下一时刻的最优估计值服从均值为X′，方差为Q的高斯分布，即最优估计值～N(X′,Q)。

引入一个置信概率α，为使得Z(k)处于置信区间内，根据高斯分布的分布函数Φ(x)，则可令当Z(k)∈(x₁,x₂]时，其中x₁+x₂＝2X′，可认为下一时刻的Z(k)属于可靠测量，可用于接下来的预测环节，又每一子滤波器的协方差均相同，故将预测过程进行合并，

图1为具备置信检验的最优估计集中式卡尔曼滤波器结构图。

改变滤波结构后，子滤波器的预测过程不变，如下所示：

X(k|k-1)＝FX(k-1|k-1) (10)

P_l(k|k-1)＝FP_l(k-1|k-1)F^T+Q (11)

当Z_l(k)满足相应范围时，定义每个子滤波器新的卡尔曼增益，如下所示：

其中，K_l(k)为第l个子滤波器在k时刻的增益，X(k)为k时刻的状态值，X(k|k-1)为对k时刻进行预测的预测值，Z_l(k)为第l个子滤波器在k时刻的量测值，Z_l(k|k-1)为第l个子滤波器对k时刻进行预测的预测量测值。

每个子滤波器的估计值可表示为：

Z_l(k|k-1)＝H_lX(k|k-1) (13)

X_l(k|k)＝X(k|k-1)+K_l(k)[Z_l(k)-Z_l(k|k-1)] (14)

主滤波器得到的最优状态估计值与传统集中式卡尔曼滤波器相同：

因状态方程唯一，故每一时刻各子滤波器的协方差均相同，即

故最优估计值化简可得：

为了说明方法的有效性和可行性，选取无人航行器的运动模型选取CV模型，虽然干扰的存在会改变目标下一时刻的运动速度，但是无人航行器在整体上是匀速运动(非机动状态)。这一模型的最大优点是形式简单，当目标机动幅度很小或采样间隔很短时，目标的运动完全可以有效近似为匀速运动。

CV模型可表示为：

当采样周期间隔较短时，对采样周期为T的离散时间状态方程为：

其中X(k)＝[x(k),v(k)]^T。

为了避免卡尔曼滤波器的整个滤波系统的运算量过大，同时说明本方法的有效性，取n＝3，则量测方程为：

Z_l(k)＝[1 0]X(k)+w_l(k)(l＝1,2,3) (19)

同时为验证结果的有效性，取T＝1，Q＝0.1，R₁＝0.1，R₂＝0.2，R₃＝0.3，初始状态为[0，0.5]^T，滤波次数为500，分别取置信概率为95％、90％和80％。图2为不同置信概率下CV模型的最优位置估计图，展示的是置信概率为95％、90％、80％和未引入置信检测环节的位置对比图，从图中可以看出加入了置信环节后对位置的估计精度比未加入置信检测环节对位置的估计精度高，同时置信概率选的越高，精度越高。

图3为不同置信概率下CV模型的最优速度估计图；图3中展示的是置信概率为95％、90％、80％和未引入置信检测环节的速度对比关系，从图中可以看出加入了置信环节后对速度的估计精度比未加入置信检测环节对速度的估计精度高，同时置信概率选的越高，精度越高。

图4为不同置信概率下CV模型的位置误差对比图；图4中展示的是置信概率为95％、90％、80％和未引入置信检测环节的位置误差对比关系，从图4中可以看出置信概率选的越高，对位置的估计误差越小。

图5为不同置信概率下CV模型的速度误差对比图；图中展示置信概率为95％、90％、80％和未引入置信检测环节的速度误差对比关系，从图5中可以看出置信概率选的越高，对速度的估计误差越小。

以上所述，仅为本发明较佳的具体实施方式，但本发明的保护范围并不局限于此，任何熟悉本技术领域的技术人员在本发明揭露的技术范围内，根据本发明的技术方案及其发明构思加以等同替换或改变，都应涵盖在本发明的保护范围之内。

Claims (2)

1.一种基于集中式卡尔曼滤波的无人航行器状态估计方法，其特征在于：包括以下步骤：

S1:建立无人航行器关于位置和速度的状态方程和量测方程；

S2:根据无人航行器的状态方程，预测无人航行器的下一时刻速度和位置，得到无人航行器的下一时刻速度预测值和位置预测值；

S3:根据无人航行器的下一时刻速度预测值和位置预测值，建立下一时刻速度和位置真值的分布函数，下一时刻速度和位置真值应服从期望为预测值，方差为系统噪声方差的正态分布，通过引入置信概率，得到无人航行器当前时刻可靠的速度和位置量测值；

S4:基于新型集中式卡尔曼滤波器的结构，确定无人航行器更新过程的协方差和卡尔曼滤波器的增益，再通过结合当前时刻速度和位置量测值得到无人航行器的位置信息和速度信息最优估计值；

所述新型集中式卡尔曼滤波器包括N个子滤波器，所述子滤波器的卡尔曼增益如下：

其中：K_l(k)为第l个子滤波器在k时刻的增益，X(k)为k时刻的状态值，X(k|k-1)为对k时刻进行预测的预测值，Z_l(k)为第l个子滤波器在k时刻的量测值，Z_l(k|k-1)为第l个子滤波器对k时刻进行预测的预测量测值；

所述速度和位置量测值是否可靠的判断方法如下：

引入一个置信概率α，为使得速度和位置量测值处于置信区间内，根据高斯分布的分布函数Φ(x)，则可令：

当Z(k)∈(x₁,x₂]时，其中x₁+x₂＝2X′，下一时刻的Z(k)属于可靠测量，其中，Z(k)表示当前时刻的量测值。

2.根据权利要求1所述的一种基于集中式卡尔曼滤波的无人航行器状态估计方法，其特征还在于：所述通过引入置信概率，得到无人航行器当前时刻可靠的速度和位置量测值，过程如下：

通过引入置信概率，判断无人航行器当前时刻速度和位置量测值是否可靠；当无人航行器当前时刻速度和位置量测值可靠，则采用当前时刻速度和位置量；当无人航行器当前时刻速度和位置量测值不可靠，利用量测方程获得无人航行器的速度和位置可靠的量测值。