CN107084722B - 一种用于提高惯性-地磁组合静动态综合性能的方法 - Google Patents

Abstract

本发明提供一种基于卡尔曼滤波器和隐马尔科夫模型的用于提高惯性‑地磁组合综合性能的方法，引入一种新的基于卡尔曼和隐马尔科夫模型的加权和动态调整机制，从而使得在动态条件下扩展卡尔曼算法和互补滤波算法能够完全依靠三轴陀螺仪实现姿态解算，而在静态条件下又能够主要依靠加速度计和地磁传感器实现姿态解算，保证了较好的动态性能和静态性能，从而提高惯性‑地磁组合的综合性能。

Description

一种用于提高惯性-地磁组合静动态综合性能的方法

技术领域

本发明涉及一种基于卡尔曼滤波器和隐马尔科夫模型的用于提高惯性-地磁组合综合性能的方法。

背景技术

惯性-地磁组合是一种传感器组合，主要由三轴加速度计、三轴陀螺仪和三轴地磁传感器构成，用于实现载体的姿态测量。所谓的“姿态”指的是载体坐标系相对于参考坐标系之间的坐标转换关系，通常用欧拉角、四元数或旋转矩阵等表示。参考坐标系通常选为当地地理坐标系。惯性-地磁组合需要依靠其姿态解算算法来融合其传感器数据进而计算出载体的姿态，目前常用的姿态解算算法主要有两类，即扩展卡尔曼算法和互补滤波算法。一般来说，只要给定初始姿态，然后利用方向余弦算法等算法通过融合三轴陀螺仪输出数据也能够解算出姿态(命名为姿态1)，并且解算出的姿态不受载体线加速度影响，并因此具有很好的动态精度，然而由于方向余弦算法等算法通过角速度的积分来获取姿态的本质，陀螺仪的输出误差不可避免的会被积分并且叠加进姿态解算结果中，这就使得所解算出的姿态会随时间发散并因此具有很差的长时间静态精度。另一方面，利用矢量定姿算法，比如QUEST算法等，通过融合加速度计和地磁传感器输出数据也能够解算出姿态(命名为姿态2)，并且所解算出的姿态不受误差积分的影响并因此具有很好的静态精度，然而由于在动态条件下，加速度计所测得的加速度既包含重力加速度也包含载体线加速度，即加速度计的输出是上述两种加速度的叠加，这就使得所解算出的姿态受载体线加速度影响并因此具有很差的动态精度。扩展卡尔曼算法和互补滤波算法所解算出的姿态是姿态1和姿态2的加权和，其目的是对动态性能和静态性能做折中处理，从而使惯性-地磁组合能够兼具较好的动态和静态性能，但是性能折中会造成动态性能和静态性能均下降。

发明内容

本发明所要解决的技术问题是：为了解决性能折中造成的动态性能和静态性能均下降的问题，本发明提供一种基于卡尔曼滤波器和隐马尔科夫模型的用于提高惯性-地磁组合综合性能的方法，引入一种新的基于卡尔曼和隐马尔科夫模型的加权和动态调整机制，从而使得在动态条件下扩展卡尔曼算法和互补滤波算法能够完全依靠三轴陀螺仪实现姿态解算，而在静态条件下又能够主要依靠加速度计和地磁传感器实现姿态解算。

目前最具竞争力的动态调整加权和的方法有两种，这两种方法的基本思想是类似的。

第一种方法是设置陀螺仪的权值为

其中a_i为由三轴加速度输出构成的三维矢量，g为当地重力加速度矢量。α为某一阈值。事实上，|||a_i||-||g|||表示的是载体线加速度矢量模的估计。

另一种方法为

第一种方法为离散式调整方法，即权值要么为“1”，要么为“0”，而第二种方法为连续式调整方法。对于第一种方法，在加速度计输出噪声较大时，由于其苛刻的判断载体是否静止的条件，使得在静态条件下其更倾向于依靠陀螺仪实现姿态解算，因而在采样速率偏低时容易使扩展卡尔曼算法或互补滤波算法发散。对于第二种方法，即使负指数函数的输出能够随着输入的增大而快速衰减，但是在载体线加速度存在但偏小时，该函数仍不能使输出衰减至0，从而使扩展卡尔曼算法或互补滤波算法在此情形下仍然受载体线加速度影响。

本发明解决其技术问题所要采用的技术方案不同于目前上述的两种方法，本发明的一种基于卡尔曼滤波器和隐马尔科夫模型的用于提高惯性-地磁组合综合性能的方法，采用||a_i×m_i||作为本方法的输入，即判断载体是否运动的依据，由三轴加速度计输出构成三维矢量a_i＝[a_ix a_iy a_iz]^T，其中a_ij(j＝x,y,z)为i时刻j轴向加速度计输出值，由地磁传感器输出构成三维矢量m_i＝[m_ix m_iy m_iz]^T，其中，m_ij(j＝x,y,z)为i时刻j轴向地磁传感器输出值，由三轴陀螺仪输出构成三维矢量ω_i＝[ω_ix ω_iy ω_iz]^T，其中ω_ij(j＝x,y,z)为i时刻j轴向陀螺仪输出值，由a_i和m_i获得a_i×m_i，作为卡尔曼算法的输入；相对于载体线加速度矢量模的估计，即|||a_i||-||g|||来说，||a_i×m_i||包含更多有关载体线加速度的信息，即载体线加速度矢量的角度信息。由于加速度计和地磁传感器输出均含有噪声，这会对||a_i×m_i||造成双重冲击，从而使得||a_i×m_i||的噪声很大。

为了解决这一问题，本发明利用卡尔曼算法实现对||a_i×m_i||的准确估计。其中卡尔曼算法的实现流程如下，

首先设x＝a_i×m_i，然后构建如下方程组，

其中

CWT＝cos(ω_i0T)，SWT＝sin(ω_i0T)，TCWT＝1-CWT，

T为采样周期；

噪声ξ(i)和η(i)的协方差阵计算方法为：

其中Q、R为常量，δ_ik为狄拉克函数；

待x估计出之后，再计算||a_i×m_i||，计算方法为，

步骤3：估计出的||a_i×m_i||随后作为隐马尔科夫模型的输入；隐马尔科夫模型的训练方法为，使载体处于静止状态，然后通过步骤2获得载体处于静止状态的观测序列

最后利用Baum-Welch算法对隐马尔科夫模型进行训练从而获得模型参数λ₀＝(A₀,B₀,π₀)；载体处于运动状态时，通过步骤2获得载体处于运动状态的观测序列

与所建立的隐马尔科夫模型λ₀吻合的概率

利用Forward算法计算出，并且有

P(||a_i×m_i|||λ₀)是指观测序列||a_i×m_i||与所建立的隐马尔科夫模型λ₀匹配的概率，用于判断载体是否运动并因而用于调整陀螺仪的权值；

步骤4：对于扩展卡尔曼算法，陀螺仪权值的调整方法是，

其中Q和R分别为调整前的卡尔曼算法的过程噪声和观测噪声的协方差阵，而Q′和R′分别为调整后的卡尔曼算法的过程噪声和观测噪声的协方差阵；

对于互补滤波算法，则P(||a_i×m_i|||λ₀)直接用于调整其唯一的权值K，

K＝P(||a_i×m_i|||λ₀) (8)

设P(||a_i×m_i|||λ₀)的值域范围为[p_max,p_min]，一旦获得隐马尔科夫模型的参数λ₀＝(A₀,B₀,π₀)，该值域范围很容易通过测试集测试手段得到；需要注意的是，通常该值域范围不适合式(7)和式(8)，因而需要再调整，调整的方法为，

进一步，步骤3具体采用遍历式隐马尔科夫模型，因为陀螺仪、加速度计和地磁传感器这三种传感器的输出特性并没有给出任何关于模型隐含状态转移概率的信息，对于每个隐含状态，观测概率通过下式计算，

其中F(X,μ_jm,Σ_jm)为高斯函数，定义为，

其中L为高斯函数的个数,μ_jm和Σ_jm分别为第m个高斯函数的均值和协方差。

本发明采用遍历式隐马尔科夫模型，因为三种传感器的输出特性并没有给出任何关于模型隐含状态转移概率的信息。对于每个隐含状态，观测概率通过式(10)计算。

对于扩展卡尔曼算法，陀螺仪权值的调整方法如式(7)所示，而对于互补滤波算法，陀螺仪权值的调整方法如式(8)所示。需要注意的是，P(||a_i×m_i|||λ₀)的值域范围通常不适合上式，因而需要再调整，调整方式如式(9)所示。

本发明的有益效果是：本发明提供的一种基于卡尔曼滤波器和隐马尔科夫模型的用于提高惯性-地磁组合综合性能的方法，引入一种新的基于卡尔曼和隐马尔科夫模型的加权和动态调整机制，从而使得在动态条件下扩展卡尔曼算法和互补滤波算法能够完全依靠三轴陀螺仪实现姿态解算，而在静态条件下又能够主要依靠加速度计和地磁传感器实现姿态解算，保证了较好的动态性能和静态性能，从而提高惯性-地磁组合的综合性能。

附图说明

下面结合附图和实施例对本发明作进一步说明。

图1是本发明最佳实施例的原理示意图。

具体实施方式

现在结合附图对本发明作详细的说明。此图为简化的示意图，仅以示意方式说明本发明的基本结构，因此其仅显示与本发明有关的构成。

如图1所示，本发明的一种基于卡尔曼滤波器和隐马尔科夫模型的用于提高惯性-地磁组合综合性能的方法，采用||a_i×m_i||作为本方法的输入，即判断载体是否运动的依据，由三轴加速度计输出构成三维矢量a_i＝[a_ix a_iy a_iz]^T，其中a_ij(j＝x,y,z)为i时刻j轴向加速度计输出值，由地磁传感器输出构成三维矢量m_i＝[m_ix m_iy m_iz]^T，其中，m_ij(j＝x,y,z)为i时刻j轴向地磁传感器输出值，由三轴陀螺仪输出构成三维矢量ω_i＝[ω_ix ω_iyω_iz]^T，其中ω_ij(j＝x,y,z)为i时刻j轴向陀螺仪输出值，由a_i和m_i获得a_i×m_i，作为卡尔曼算法的输入；相对于载体线加速度矢量模的估计，即|||a_i||-||g|||来说，||a_i×m_i||包含更多有关载体线加速度的信息，即载体线加速度矢量的角度信息。由于加速度计和地磁传感器输出均含有噪声，这会对||a_i×m_i||造成双重冲击，从而使得||a_i×m_i||的噪声很大。

为了解决这一问题，本发明利用卡尔曼算法实现对||a_i×m_i||的准确估计。其中卡尔曼算法的实现流程如下，

首先设x＝a_i×m_i，然后构建如下方程组，

其中

CWT＝cos(ω_i0T)，SWT＝sin(ω_i0T)，TCWT＝1-CWT，

T为采样周期；

噪声ξ(i)和η(i)的协方差阵计算方法为：

其中Q、R为常量，δ_ik为狄拉克函数；

待x估计出之后，再计算||a_i×m_i||，计算方法为，

步骤3：估计出的||a_i×m_i||随后作为隐马尔科夫模型的输入；隐马尔科夫模型的训练方法为，使载体处于静止状态，然后通过步骤2获得载体处于静止状态的观测序列

最后利用Baum-Welch算法对隐马尔科夫模型进行训练从而获得模型参数λ₀＝(A₀,B₀,π₀)；载体处于运动状态时，通过步骤2获得载体处于运动状态的观测序列

与所建立的隐马尔科夫模型λ₀吻合的概率利用Forward算法计算出，并且有

P(||a_i×m_i|||λ₀)是指观测序列||a_i×m_i||与所建立的隐马尔科夫模型λ₀匹配的概率，用于判断载体是否运动并因而用于调整陀螺仪的权值；

步骤4：对于扩展卡尔曼算法，陀螺仪权值的调整方法是，

其中Q和R分别为调整前的卡尔曼算法的过程噪声和观测噪声的协方差阵，而Q′和R′分别为调整后的卡尔曼算法的过程噪声和观测噪声的协方差阵；

对于互补滤波算法，则P(||a_i×m_i|||λ₀)直接用于调整其唯一的权值K，

K＝P(||a_i×m_i|||λ₀) (8)

设P(||a_i×m_i|||λ₀)的值域范围为[p_max,p_min]，一旦获得隐马尔科夫模型的参数λ₀＝(A₀,B₀,π₀)，该值域范围很容易通过测试集测试手段得到；需要注意的是，通常该值域范围不适合式(7)和式(8)，因而需要再调整，调整的方法为，

进一步，步骤3具体采用遍历式隐马尔科夫模型，因为陀螺仪、加速度计和地磁传感器这三种传感器的输出特性并没有给出任何关于模型隐含状态转移概率的信息，对于每个隐含状态，观测概率通过下式计算，

其中F(X,μ_jm,Σ_jm)为高斯函数，定义为，

其中L为高斯函数的个数,μ_jm和Σ_jm分别为第m个高斯函数的均值和协方差。

本发明采用遍历式隐马尔科夫模型，因为三种传感器的输出特性并没有给出任何关于模型隐含状态转移概率的信息。对于每个隐含状态，观测概率通过式(10)计算。

对于扩展卡尔曼算法，陀螺仪权值的调整方法如式(7)所示，而对于互补滤波算法，陀螺仪权值的调整方法如式(8)所示。需要注意的是，P(||a_i×m_i|||λ₀)的值域范围通常不适合上式，因而需要再调整，调整方式如式(9)所示。

以上述依据本发明的理想实施例为启示，通过上述的说明内容，相关的工作人员完全可以在不偏离本发明的范围内，进行多样的变更以及修改。本项发明的技术范围并不局限于说明书上的内容，必须要根据权利要求范围来确定其技术性范围。

Claims (2)

1.一种用于提高惯性-地磁组合静动态综合性能的方法，其特征在于：包括陀螺仪、加速度计和地磁传感器，并包括以下步骤，

步骤1：由三轴加速度计输出构成三维矢量a_i＝[a_ix a_iy a_iz]^T，其中a_ij(j＝x,y,z)为i时刻j轴向加速度计输出值，由地磁传感器输出构成三维矢量m_i＝[m_ix m_iy m_iz]^T，其中，m_ij(j＝x,y,z)为i时刻j轴向地磁传感器输出值，由三轴陀螺仪输出构成三维矢量ω_i＝[ω_ixω_iy ω_iz]^T，其中ω_ij(j＝x,y,z)为i时刻j轴向陀螺仪输出值，由a_i和m_i获得a_i×m_i，作为卡尔曼算法的输入；

步骤2：利用卡尔曼算法实现对||a_i×m_i||的准确估计，其中卡尔曼算法的实现流程如下，

首先设x＝a_i×m_i，然后构建如下方程组，

其中

其中，CWT＝cos(ω_i0T)，SWT＝sin(ω_i0T)，TCWT＝1-CWT，

T为采样周期；

噪声ξ(i)和η(i)的协方差阵计算方法为：

其中Q、R为常量，δ_ik为狄拉克函数；

待x估计出之后，再计算||a_i×m_i||，计算方法为，

步骤3：估计出的||a_i×m_i||随后作为隐马尔科夫模型的输入；隐马尔科夫模型的训练方法为，使载体处于静止状态，然后通过步骤2获得载体处于静止状态的观测序列

最后利用Baum-Welch算法对隐马尔科夫模型进行训练从而获得模型参数λ₀＝(A₀,B₀,π₀)；载体处于运动状态时，通过步骤2获得载体处于运动状态的观测序列

与所建立的隐马尔科夫模型λ₀吻合的概率

利用Forward算法计算出，并且有

P(||a_i×m_i|||λ₀)是指观测序列||a_i×m_i||与所建立的隐马尔科夫模型λ₀匹配的概率，用于判断载体是否运动并因而用于调整陀螺仪的权值；

步骤4：对于扩展卡尔曼算法，陀螺仪权值的调整方法是，

其中Q和R分别为调整前的卡尔曼算法的过程噪声和观测噪声的协方差阵，而Q′和R′分别为调整后的卡尔曼算法的过程噪声和观测噪声的协方差阵；

对于互补滤波算法，则P(||a_i×m_i|||λ₀)直接用于调整其唯一的权值K，

K＝P(||a_i×m_i|||λ₀) (8)

设P(||a_i×m_i|||λ₀)的值域范围为[p_max,p_min]，一旦获得隐马尔科夫模型的参数λ₀＝(A₀,B₀,π₀)，该值域范围很容易通过测试集测试手段得到；当该值域范围不适合式(7)和式(8)，需要再调整，调整的方法为，

2.如权利要求1所述的用于提高惯性-地磁组合静动态综合性能的方法，其特征在于：步骤3具体采用遍历式隐马尔科夫模型，因为陀螺仪、加速度计和地磁传感器这三种传感器的输出特性并没有给出任何关于模型隐含状态转移概率的信息，对于每个隐含状态，观测概率通过下式计算，

其中F(X,μ_jm,Σ_jm)为高斯函数，定义为，

其中L为高斯函数的个数,μ_jm和Σ_jm分别为第m个高斯函数的均值和协方差。

Images