CN109387220B - 一种步行微惯性导航设备方位安装偏差校正方法 - Google Patents

Abstract

本发明属于微惯性导航***技术领域，具体公开了一种步行微惯性导航设备方位安装偏差校正方法。初始静止条件设置之后利用加速度计信息确定步行微惯性导航设备的水平姿态信息，对步行微惯性导航进行状态修正，确定方位安装偏差，对步行微惯性导航设备采样值进行补偿，得到由于方位安装误差会造成的定位误差累积，即侧向位移，从而估算出方位安装误差，在惯导解算前予以校正。操作方法简单，方位安装误差估计准确，校正方法易行，能够有效提高步行微惯性导航设备的导航定位精度。

Description

一种步行微惯性导航设备方位安装偏差校正方法

技术领域

本发明属于微惯性导航***技术领域，具体涉及一种足部安装的步行微惯性导航***方位安装偏差校正方法。

背景技术

步行微惯性导航设备在使用中，由于每个人的步态习惯不同，导致足尖方向与行进方存在着一个角度，这个角度虽然对于每个人都比较固定，但是却因人而异，无法事先标定和校正。这个角度对于步行微惯性导航设备来讲，等同于方位安装误差，将会造成导航定位误差的持续累积。因而，必须采取一定的措施，对其进行校正，从而提高步行微惯性导航设备的导航定位精度。

发明内容

本发明的目的是提供一种步行微惯性导航设备方位安装偏差校正方法，抑制方位安装偏差对步行微惯性导航设备定位误差的影响，提高步行微惯性导航设备的定位精度。

本发明的技术方案如下：

一种步行微惯性导航设备方位安装偏差校正方法，该方法包括如下步骤：

1-初始静止条件设置

2)在静止初始条件状态下，利用加速度计信息确定步行微惯性导航设备的水平姿态信息；

3)设备使用人向前直线行走，对步行微惯性导航进行状态修正；

4)确定行走起点指向导航定位结果的直线与行走方向的夹角，即由于行走习惯造成的方位安装偏差α；

其中，P_x为直线行走终点的位置n系下x方向的值；P_y直线行走终点的位置n系下y方向的值；P_x0为直线行走起点的位置在n系下x方向的值；P_y0为直线行走起点的位置n系下y方向的值；

5)确定计算补偿转换矩阵T，对步行微惯性导航设备采样值进行补偿

确定误差校正后的步行微惯性导航设备惯性器件陀螺和加速度计的输出值

其中

和

分别为步行微惯性导航设备惯性器件原始输出的陀螺测量值和加速度计测量值。

步骤2)具体为：

2.1)读取静止状态的步行微惯性导航设备中惯性期间输出数据，计算载体坐标系下加速度计输出在三个方向上的平均值；

2.2)确定步行微惯性导航设备的初始姿态角；

2.3)确定步行微惯性导航设备的初始姿态矩阵；

2.4)确定步行微惯性导航设备的初始四元数。

步骤2.2)具体为：

初始姿态角包括初始俯仰角θ、初始倾斜角γ和初始航向角ψ，初始俯仰角θ＝arcsin(a_x/g)，其中g为当地重力加速度；

如果a_z≥0，初始倾斜角γ＝-arcsin(a_z/(g·cosθ))；

如果a_z＜0，a_y≥0，初始倾斜角γ＝π+arcsin(a_z/(g·cosθ))；

如果a_z＜0，a_y＜0，初始倾斜角γ＝π-arcsin(a_z/(g·cosθ))；

初始航向角ψ置为零。

步骤2.3)的初始姿态矩阵

采用下式确定

步骤2.4)具体为：

将确定的设备初始姿态矩阵的各元素表示成如下形式：

将设备的四元数Q表示成：

Q＝[q₀ q₁ q₂ q₃]^T

则，设备的四元数的初始值由下式得到

步骤3)对微惯性导航进行状态修正为速度误差、水平姿态角误差、陀螺漂移及加速度零偏的修正。

步骤3)对微惯性导航进行状态修正之前，首先对步行微惯性导航设备的运动状态进行判断，可以采用A3T3ND、AM1T3ND、A3ND或者AM1ND等公知方法，得到设备当前或运动或静止的运动状态。

步骤3)对微惯性导航进行状态修正采用卡尔曼滤波方法进行状态修正。

本发明的显著效果如下：利用北向直线前进条件，采用零速修正算法辅助微惯导解算，得到由于方位安装误差会造成的定位误差累积，即侧向位移，从而估算出方位安装误差，在惯导解算前予以校正。操作方法简单，方位安装误差估计准确，校正方法易行，能够有效提高步行微惯性导航设备的导航定位精度。

具体实施方式

下面通过具体实施方式对本发明作进一步说明。

本坐标系定义如下：

b：载体坐标系ox_by_bz_b，右前上坐标系，x_b轴指向人体的右，y_b轴指向人体的前，z_b轴指向人体的上；n：导航坐标系ox_ny_nz_n，z_n轴指向天，y_n轴在水平面内指向载体初始时刻的前向，x_n轴与y_n轴和z_n轴构成笛卡尔坐标系。

步骤1)初始静止条件设置

将步行微惯性导航设备安装在使用人的足底，使用人面向北方站立，静止保持30s以上(30～50s)。

步骤2)在静止初始条件状态下，利用加速度计信息确定步行微惯性导航设备的水平姿态信息；

2.1)读取静止状态的步行微惯性导航设备中惯性期间输出数据，计算载体坐标系下加速度计输出在三个方向上的平均值：

a^b＝[a_x a_y a_z]

其中，a^b为加速度计输出的平均值在载体坐标系下的矢量形式，a_x为加速度计输出在载体坐标系x_b下分量的平均值，a_y为加速度计输出在载体坐标系y_b下分量的平均值，a_z为加速度计输出在载体坐标系z_b下分量的平均值。

2.2)确定步行微惯性导航设备的初始姿态角

包括初始俯仰角θ、初始倾斜角γ和初始航向角ψ，初始俯仰角θ＝arcsin(a_x/g)，其中g为当地重力加速度；

如果a_z≥0，初始倾斜角γ＝-arcsin(a_z/(g·cosθ))；

如果a_z＜0，a_y≥0，初始倾斜角γ＝π+arcsin(a_z/(g·cosθ))；

如果a_z＜0，a_y＜0，初始倾斜角γ＝π-arcsin(a_z/(g·cosθ))；

由于当前行人面向北方，初始航向角ψ置为零。

2.3)确定步行微惯性导航设备的初始姿态矩阵

2.4)确定步行微惯性导航设备的初始四元数

将确定的设备初始姿态矩阵的各元素表示成如下形式：

将设备的四元数Q表示成：

Q＝[q₀ q₁ q₂ q₃]^T

则，设备的四元数的初始值由下式得到

步骤3)设备使用人向前直线行走，对微惯性导航进行状态修正。

本实施例中采用零速修正算法对微惯性导航进行修正，包括修正速度误差、水平姿态角误差、陀螺漂移及加速度零偏。

3.1)首先对步行微惯性导航设备的运动状态进行判断，可以采用A3T3ND、AM1T3ND、A3ND或者AM1ND等公知方法，得到设备当前或运动或静止的运动状态。

3.2)采用卡尔曼滤波方法进行状态修正

卡尔曼滤波器的状态量选为

式中：δVⁿ＝[δv_x δv_y δv_z]^T为沿n系的速度误差；

δPⁿ＝[δP_x δP_y δP_z]^T为沿n系的位移误差；

Φⁿ＝[φ_x φ_y φ_z]^T为沿n系的失准角；

为沿b系的加速度计零偏；

ε^b＝[ε_x ε_y ε_z]^T为沿b系的陀螺常值漂移；

上述几个式子中下角标分别代表各个坐标系不同方向的对应物理量。

卡尔曼滤波状态方程为

式中：F为状态转移矩阵，表达式为

其中，

为

的转置；

W为状态噪声向量，表达式为

W＝[ω_a 0_3×1 ω_g 0_3×1 0_3×1]^T

其中，ω_a＝[ω_ax ω_ay ω_az]^T为加速度计的随机游走；ω_g＝[ω_gx ω_gy ω_gz]^T为陀螺的随机游走。下角标代表不同方向下的对应物理量。

卡尔曼滤波量测方程为

Z＝HX+V

式中：Z为观测量，包含n系下三个方向的速度误差，n系下的速度误差即惯导解算的速度在n系下的表示

即

H为量测矩阵，表达式为H＝[I_3×3 0_3×12]。

当检测为运动时，卡尔曼滤波只进行时间更新；当检测为静止时，卡尔曼滤波进行完整更新，并闭环修正速度误差、水平姿态角误差、陀螺漂移及加速度零偏。

步骤4)确定行走起点指向导航定位结果的直线与北向的夹角，即由于行走习惯造成的方位安装偏差。

记采用零速修正算法辅助的微惯性导航解算得到的直线行走终点的位置在n系下的值为Pⁿ＝[P_x P_y P_z]^T，记直线行走起点的位置在n系下的值为

不同下角标代表相应坐标系下不同方向的物理量。

则方位安装偏差α的计算方法为

步骤5)确定计算补偿转换矩阵T，对步行微惯性导航设备采样值进行补偿。

补偿转换矩阵T的计算方法为

记步行微惯性导航设备惯性器件输出的陀螺测量值和加速度计测量值分别为

和

那么经过方位安装误差校正后的步行微惯性导航设备惯性器件输出的陀螺测量值和加速度计测量值分别

Claims (7)

1.一种步行微惯性导航设备方位安装偏差校正方法，其特征在于，该方法包括如下步骤：

1)初始静止条件设置；

2)在静止初始条件状态下，利用加速度计信息确定步行微惯性导航设备的水平姿态信息；

3)设备使用人向前直线行走，对步行微惯性导航进行状态修正；

4)确定行走起点指向导航定位结果的直线与行走方向的夹角，即由于行走习惯造成的方位安装偏差α；

其中，P_x为直线行走终点的位置n系下x方向的值；P_y直线行走终点的位置n系下y方向的值；P_x0为直线行走起点的位置在n系下x方向的值；P_y0为直线行走起点的位置n系下y方向的值；

5)确定计算补偿转换矩阵T，对步行微惯性导航设备采样值进行补偿

确定误差校正后的步行微惯性导航设备惯性器件陀螺和加速度计的输出值

其中

和

分别为步行微惯性导航设备惯性器件原始输出的陀螺测量值和加速度计测量值；

步骤2)具体为：

2.1)读取静止状态的步行微惯性导航设备中惯性期间输出数据，计算载体坐标系下加速度计输出在三个方向上的平均值；

2.2)确定步行微惯性导航设备的初始姿态角；

2.3)确定步行微惯性导航设备的初始姿态矩阵；

2.4)确定步行微惯性导航设备的初始四元数；

所述的步骤2.1)中，载体坐标系下加速度计输出在三个方向上的平均值表示为：

a^b＝[a_x a_y a_z]

其中，a^b为加速度计输出的平均值在载体坐标系下的矢量形式，a_x为加速度计输出在载体坐标系x_b下分量的平均值，a_y为加速度计输出在载体坐标系y_b下分量的平均值，a_z为加速度计输出在载体坐标系z_b下分量的平均值；

所述的步骤2.2)具体为：

初始姿态角包括初始俯仰角θ、初始倾斜角γ和初始航向角ψ，初始俯仰角θ＝arcsin(a_x/g)，其中g为当地重力加速度；

如果a_z≥0，初始倾斜角γ＝-arcsin(a_z/(g·cosθ))；

如果a_z＜0，a_y≥0，初始倾斜角γ＝π+arcsin(a_z/(g·cosθ))；

如果a_z＜0，a_y＜0，初始倾斜角γ＝π-arcsin(a_z/(g·cosθ))；

初始航向角ψ置为零。

2.如权利要求1所述的一种步行微惯性导航设备方位安装偏差校正方法，其特征在于，所述步骤2.3)的初始姿态矩阵

采用下式确定

3.如权利要求1所述的一种步行微惯性导航设备方位安装偏差校正方法，其特征在于，所述步骤2.4)具体为：

将确定的设备初始姿态矩阵的各元素表示成如下形式：

将设备的四元数Q表示成：

Q＝[q₀ q₁ q₂ q₃]^T

则，设备的四元数的初始值由下式得到

4.如权利要求1所述的一种步行微惯性导航设备方位安装偏差校正方法，其特征在于，所述步骤3)对微惯性导航进行状态修正为速度误差、水平姿态角误差、陀螺漂移及加速度零偏的修正。

5.如权利要求1所述的一种步行微惯性导航设备方位安装偏差校正方法，其特征在于，所述步骤3)对微惯性导航进行状态修正之前，首先对步行微惯性导航设备的运动状态进行判断，采用A3T3ND、AM1T3ND、A3ND或者AM1ND方法，得到设备当前运动或静止的状态。

6.如权利要求1所述的一种步行微惯性导航设备方位安装偏差校正方法，其特征在于，所述步骤3)对微惯性导航进行状态修正采用卡尔曼滤波方法进行状态修正。

7.如权利要求6所述的一种步行微惯性导航设备方位安装偏差校正方法，其特征在于，采用卡尔曼滤波方法具体为：

卡尔曼滤波器的状态量选为

式中：δVⁿ＝[δv_x δv_y δv_z]^T为沿n系的速度误差；δPⁿ＝[δP_x δP_y δP_z]^T为沿n系的位移误差；Φⁿ＝[φ_x φ_y φ_z]^T为沿n系的失准角；

为沿b系的加速度计零偏；ε^b＝[ε_x ε_y ε_z]^T为沿b系的陀螺常值漂移；

卡尔曼滤波状态方程为

式中：F为状态转移矩阵，表达式为

为初始姿态矩阵

的转置；

W为状态噪声向量，表达式为

W＝[ω_a 0_3×1 ω_g 0_3×1 0_3×1]^T

其中，ω_a＝[ω_ax ω_ay ω_az]^T为加速度计的随机游走；ω_g＝[ω_gx ω_gy ω_gz]^T为陀螺的随机游走；

卡尔曼滤波量测方程为

Z＝HX+V

式中：Z为观测量，包含n系下三个方向的速度误差，n系下的速度误差即惯导解算的速度在n系下的表示

即

H为量测矩阵，表达式为H＝[I_3×3 0_3×12]；

当检测为运动时，卡尔曼滤波只进行时间更新；当检测为静止时，卡尔曼滤波进行完整更新，并闭环修正速度误差、水平姿态角误差、陀螺漂移及加速度零偏。