CN108592917A - 一种基于失准角的卡尔曼滤波姿态估计方法 - Google Patents

Abstract

本发明实施例公开了一种基于失准角的卡尔曼滤波姿态估计方法。所述方法包括：构建载体坐标系，并确定载体坐标系到NED导航坐标系的转换矩阵；采用东向水平失准角、北向水平失准角和陀螺漂移作为***状态，以水平加速度作为量测状态；根据失准角方程构建卡尔曼滤波***的状态方程；基于卡尔曼滤波实现载体姿态的估计。另外，综合考虑载体运动状态，确定是否对载体姿态进行修正。本发明实施例的技术方案根据失准角方程构建4维状态量的卡尔曼滤波，在保证姿态估计精确度的同时，降低了***运算复杂度，并且基于载体运动状态，判断是否进行姿态修正，能够进一步提高运算效率，提高***的鲁棒性。

Description

一种基于失准角的卡尔曼滤波姿态估计方法

技术领域

本发明实施例涉及导航领域，尤其涉及一种基于失准角的卡尔曼滤波姿态估计方法。

背景技术

惯性传感器IMU中通常内置有卡尔曼滤波***，能够采用卡尔曼滤波进行姿态的估计和修正，卡尔曼滤波的效率在载体姿态估计上起到了重要的作用。目前，为了保证姿态估计的准确性，往往设计较为复杂的状态方程，导致解算复杂，容易受到噪声的干扰。并且，当应用于无人机或小型飞行器时，受到载体体积和设备成本的制约，置于载体上的惯性传感器IMU通常不具备较强的运算能力，无法执行复杂的算法。另外，基于卡尔曼滤波进行姿态修正的过程中，还面对何时启动姿态修正的难题。现有技术中，通常首先计算惯性传感器中三轴加速度计的模值，然后判断模值是否在预设范围的方式，确定载体是否处于无运动加速度状态，如果是，则进行卡尔曼滤波的量测修正。然而，由于所述方法仅考虑模值这一单一因素，往往会出现漏判情况，导致错误的量测修正，造成较大的误差影响。

发明内容

有鉴于此，本发明实施例提供了一种基于失准角的卡尔曼滤波姿态估计方法，以解决现有技术中运算量大、精度低，量测修正不及时、不准确的问题。

本发明实施例提供了一种基于失准角的卡尔曼滤波姿态估计方法，所述方法包含以下步骤：

步骤一：设置载体坐标系，所述载体坐标系三轴表示为记作xyz；

步骤二：通过惯性传感器确定当前时刻的载体姿态，记作att＝[φ θ ψ]，其中φ代表滚转角，θ代表俯仰角，ψ代表航向角；

步骤三：确定载体坐标系向导航坐标系的转换矩阵，记作

简化转换矩阵为以下形式

步骤四：采用东向水平失准角北向水平失准角和陀螺漂移ω_bx,ω_by作为***状态，记作

采用水平加速度f_x,f_y作为量测状态，记作Z＝[f_y f_x]^T；

步骤五：构建基于失准角的卡尔曼滤波；

步骤六：根据所述基于失准角的卡尔曼滤波进行载体姿态的估计。

优选地，所述载体坐标系与载体固联，坐标系符合右手法则，原点在载体重心处，x轴指向载体前进方向，y轴由原点指向载体右侧，z轴方向根据xy轴由右手法则确定。

优选地，设卡尔曼滤波***的状态方程为：

其中，为***状态的导数，f(x)为***状态的函数，w为***噪声，Z为***的量测状态，h(x)为***的量测状态函数，v为量测噪声；

记失准角为陀螺漂移为ω_b＝[ω_bx ω_by 0]^T，根据失准角方程，上式可表示为

对f(x)和h(x)其求解雅克比矩阵得到矩阵F和H，得到线性化后的状态方程：

其中，向量X为上一时刻***状态向量，向量为***状态向量的一步预测值，矩阵F为***传递矩阵，向量W,V为***噪声向量，向量为***量测向量，矩阵H为量测矩阵，其中

优选地，所述基于失准角的卡尔曼滤波包括时间更新和量测修正，在载体处于平稳状态时，进行时间更新和量测修正，在载体处于非平稳状态时，仅进行时间更新。

优选地，判断载体是否处于平稳状态的步骤为：

步骤1：计算所述惯性传感器的三轴加速度计的矢量和记为其中f_x,f_y,f_z分别为三轴加速度值；

步骤2：计算载体低机动状态下的三轴加速度；具体步骤为：

在载体低机动的状态下有

其中，重力向量为g_n＝[0 0 g]^T，g为当地重力加速度，f₁为低机动状态下三轴加速度计输出的加速度向量，记作f₁＝[f_x1,f_y1,f_z1]^T；

由式(2)可计算得出理想加速度f_x1,f_y1,f_z1：

步骤3：定义一偏差数值为eps，分别计算若上述任一结果大于偏差数值eps，则载体处于非平稳状态；否则，载体处于平稳状态。

优选地，所述时间更新的具体步骤包括：

其中，向量为***状态向量的一步预测值，向量为上一时刻的估计值，矩阵Φ为矩阵F的离散化形式，Φ_k/k-1为一步转移矩阵，矩阵P_k/k-1为状态向量的一步预测方差矩阵，矩阵P_k/k-1为状态向量的上一时刻方差矩阵，矩阵Q_k为***噪声方差阵；时间更新后的预测值作为姿态估计值。

优选地，所述量测修正的具体步骤包括：

其中，向量为***状态向量的当前最优估算值，矩阵K_k为增益矩阵，矩阵H_k为量测矩阵，矩阵R_k为量测噪声序列方差阵，矩阵I为单位矩阵；所述量测修正即为姿态修正。

本发明实施例提供了一种基于失准角的卡尔曼滤波姿态估计方法，采用基于失准角的卡尔曼滤波，仅使用双轴失准角和双轴陀螺偏置作为状态量，使用水平加速度作为量测量，在保证解算精度的前提下，极大地降低了运算量；并且，采用上述参数作为状态量和量测量，可以自适应地进行时间更新或量测修正，丰富了量测修正的判定条件，减少漏判情况，增强了***的鲁棒性和准确性，并且进一步降低了运算量。

附图说明

图1是本发明实施例一提供的一种基于失准角的卡尔曼滤波姿态估计方法的流程图；

图2是本发明实施例二提供的一种基于失准角的卡尔曼滤波姿态估计和修正流程图。

具体实施方式

为了使本发明的目的、技术方案和优点更加清楚，下面结合附图对本发明具体实施例作进一步的详细描述。可以理解的是，此处所描述的具体实施例仅仅用于解释本发明，而非对本发明的限定。

另外还需要说明的是，为了便于描述，附图中仅示出了与本发明相关的部分而非全部内容。在更加详细地讨论示例性实施例之前应当提到的是，一些示例性实施例被描述成作为流程图描绘的处理或方法。虽然流程图将各项操作(或步骤)描述成顺序的处理，但是其中的许多操作可以被并行地、并发地或者同时实施。此外，各项操作的顺序可以被重新安排。当其操作完成时所述处理可以被终止，但是还可以具有未包括在附图中的附加步骤。所述处理可以对应于方法、函数、规程、子例程、子程序等等。

实施例一

本发明实施例一具体可以应用在需要高精度导航的产品，如无人机定位，自动驾驶，数字城市，机器人导航等。图1为本发明实施例一提供的一种基于失准角的卡尔曼滤波姿态估计方法的流程图。本实施例的方法具体包括：

110、设置载体坐标系，所述载体坐标系三轴表示为记作xyz。

在本市实例中，所述载体坐标系与载体固联，坐标系符合右手法则，原点在载体重心处，x轴指向载体前进方向，y轴由原点指向载体右侧，z轴方向根据xy轴由右手法则确定。

120、通过惯性传感器确定当前时刻的载体姿态，记作att＝[φ θ ψ]，其中φ代表滚转角，θ代表俯仰角，ψ代表航向角。

130、确定载体坐标系向导航坐标系的转换矩阵，记作

简化转换矩阵为以下形式

140、构建基于失准角的卡尔曼滤波。

在本实施例中，采用东向水平失准角北向水平失准角和陀螺漂移ω_bx,ω_by作为***状态，记作

采用水平加速度f_x,f_y作为量测状态，记作Z＝[f_y f_x]^T。

设卡尔曼滤波***的状态方程为：

其中，为***状态的导数，f(x)为***状态的函数，w为***噪声，Z为***的量测状态，h(x)为***的量测状态函数，v为量测噪声；

记失准角为陀螺漂移为ω_b＝[ω_bx ω_by 0]^T，根据失准角方程，上式可表示为

对f(x)和h(x)其求解雅克比矩阵得到矩阵F和H，得到线性化后的状态方程：

其中，向量X为上一时刻***状态向量，向量为***状态向量的一步预测值，矩阵F为***传递矩阵，向量W,V为***噪声向量，向量为***量测向量，矩阵H为量测矩阵，其中

150、根据所述基于失准角的卡尔曼滤波进行载体姿态的估计。

本发明实施例一提供了一种基于失准角的卡尔曼滤波姿态估计方法，采用基于失准角的卡尔曼滤波，仅使用双轴失准角和双轴陀螺偏置作为状态量，使用水平加速度作为量测量，在保证解算精度的前提下，极大地降低了运算量。

实施例二

图2是本发明实施例二提供的一种基于失准角的卡尔曼滤波姿态估计和修正流程图。本实施例以上述实施例为基础进行优化，本实施例的方法具体包括：

210、获取惯性传感器的三轴加速度计的测量值。

220、判断载体是否处于平稳状态。

在本实施例中，判断载体是否处于平稳状态的步骤为：

步骤1：计算所述惯性传感器的三轴加速度计的矢量和记为其中f_x,f_y,f_z分别为三轴加速度值；

步骤2：计算载体低机动状态下的三轴加速度；具体步骤为：

在载体低机动的状态下有

其中，重力向量为g_n＝[0 0 g]^T，g为当地重力加速度，f₁为低机动状态下三轴加速度计输出的加速度向量，记作f₁＝[f_x1,f_y1,f_z1]^T；

由式(2)可计算得出理想加速度f_x1,f_y1,f_z1：

步骤3：定义一偏差数值为eps，分别计算若上述任一结果大于偏差数值eps，则载体处于非平稳状态；否则，载体处于平稳状态。

在载体处于平稳状态时，进行时间更新和量测修正，在载体处于非平稳状态时，仅进行时间更新

230、进行时间更新。

在本实施例中，所述时间更新的具体步骤包括：

其中，向量为***状态向量的一步预测值，向量为上一时刻的估计值，矩阵Φ为矩阵F的离散化形式，Φ_k/k-1为一步转移矩阵，矩阵P_k/k-1为状态向量的一步预测方差矩阵，矩阵P_k/k-1为状态向量的上一时刻方差矩阵，矩阵Q_k为***噪声方差阵。

240、进行时间更新和量测修正。

在本实施例中，所述量测修正的具体步骤包括：

其中，向量为***状态向量的当前最优估算值，矩阵K_k为增益矩阵，矩阵H_k为量测矩阵，矩阵R_k为量测噪声序列方差阵，矩阵I为单位矩阵。

250、确定载体姿态。

本发明实施例二提供了一种基于失准角的卡尔曼滤波姿态更新方法，采用基于失准角的卡尔曼滤波，仅使用双轴失准角和双轴陀螺偏置作为状态量，使用水平加速度作为量测量，在保证解算精度的前提下，极大地降低了运算量；并且，采用上述参数作为状态量和量测量，可以自适应地选择时间更新或量测修正，即使用当前加速度矢量和以及加速度计数值与理想值的差进行自适应卡尔曼滤波，进一步降低了运算量。

在此请注意，上述仅为本发明的较佳实施例及所运用技术原理。本领域技术人员会理解，本发明不限于这里所述的特定实施例，对本领域技术人员来说能够进行各种明显的变化、重新调整和替代而不会脱离本发明的保护范围。因此，虽然通过以上实施例对本发明进行了较为详细的说明，但是本发明不仅仅限于以上实施例，在不脱离本发明构思的情况下，还可以包括更多其他等效实施例，而本发明的范围由所附的权利要求范围决定。

Claims (7)

1.一种基于失准角的卡尔曼滤波姿态估计方法，其特征在于，所述方法包含以下步骤：

步骤一：设置载体坐标系，所述载体坐标系三轴表示为记作xyz；

步骤二：通过惯性传感器确定当前时刻的载体姿态，记作att＝[φ θ ψ]，其中φ代表滚转角，θ代表俯仰角，ψ代表航向角；

步骤三：确定载体坐标系向导航坐标系的转换矩阵，记作

简化转换矩阵为以下形式

步骤四：采用东向水平失准角北向水平失准角和陀螺漂移ω_bx,ω_by作为***状态，记作

采用水平加速度f_x,f_y作为量测状态，记作Z＝[f_y f_x]^T；

步骤五：构建基于失准角的卡尔曼滤波；

步骤六：根据所述基于失准角的卡尔曼滤波进行载体姿态的估计。

2.根据权利要求1所述的方法，其特征在于，所述载体坐标系与载体固联，坐标系符合右手法则，原点在载体重心处，x轴指向载体前进方向，y轴由原点指向载体右侧，z轴方向根据xy轴由右手法则确定。

3.根据权利要求1所述的方法，其特征在于，设卡尔曼滤波***的状态方程为：

其中，为***状态的导数，f(x)为***状态的函数，w为***噪声，Z为***的量测状态，h(x)为***的量测状态函数，v为量测噪声；

记失准角为陀螺漂移为ω_b＝[ω_bx ω_by 0]^T，根据失准角方程，上式可表示为

对f(x)和h(x)其求解雅克比矩阵得到矩阵F和H，得到线性化后的状态方程：

其中，向量X为上一时刻***状态向量，向量为***状态向量的一步预测值，矩阵F为***传递矩阵，向量W,V为***噪声向量，向量为***量测向量，矩阵H为量测矩阵，其中

4.根据权利要求1所述的方法，其特征在于，所述基于失准角的卡尔曼滤波包括时间更新和量测修正，在载体处于平稳状态时，进行时间更新和量测修正，在载体处于非平稳状态时，仅进行时间更新。

5.根据权利要求4所述的方法，其特征在于，判断载体是否处于平稳状态的步骤为：

步骤1：计算所述惯性传感器的三轴加速度计的矢量和记为其中f_x,f_y,f_z分别为三轴加速度值；

步骤2：计算载体低机动状态下的三轴加速度；具体步骤为：

在载体低机动的状态下有

其中，重力向量为g_n＝[0 0 g]^T，g为当地重力加速度，f₁为低机动状态下三轴加速度计输出的加速度向量，记作f₁＝[f_x1,f_y1,f_z1]^T；

由式(2)可计算得出理想加速度f_x1,f_y1,f_z1：

步骤3：定义一偏差数值为eps，分别计算|f_x1-f_x|、|f_y1-f_y|、|f_z1-f_z|，若上述任一结果大于偏差数值eps，则载体处于非平稳状态；否则，载体处于平稳状态。

6.根据权利要求4所述的方法，其特征在于，所述时间更新的具体步骤包括：

其中，向量为***状态向量的一步预测值，向量为上一时刻的估计值，矩阵Φ为矩阵F的离散化形式，Φ_k/k-1为一步转移矩阵，矩阵P_k/k-1为状态向量的一步预测方差矩阵，矩阵P_k/k-1为状态向量的上一时刻方差矩阵，矩阵Q_k为***噪声方差阵；时间更新后的预测值作为姿态估计值。

7.根据权利要求4所述的方法，其特征在于，所述量测修正的具体步骤包括：

其中，向量为***状态向量的当前最优估算值，矩阵K_k为增益矩阵，矩阵H_k为量测矩阵，矩阵R_k为量测噪声序列方差阵，矩阵I为单位矩阵；所述量测修正即为姿态修正。