CN109387220A - 一种步行微惯性导航设备方位安装偏差校正方法 - Google Patents
一种步行微惯性导航设备方位安装偏差校正方法 Download PDFInfo
- Publication number
- CN109387220A CN109387220A CN201710655650.3A CN201710655650A CN109387220A CN 109387220 A CN109387220 A CN 109387220A CN 201710655650 A CN201710655650 A CN 201710655650A CN 109387220 A CN109387220 A CN 109387220A
- Authority
- CN
- China
- Prior art keywords
- inertial navigation
- micro
- walking
- initial
- equipment
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Granted
Links
Classifications
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C25/00—Manufacturing, calibrating, cleaning, or repairing instruments or devices referred to in the other groups of this subclass
- G01C25/005—Manufacturing, calibrating, cleaning, or repairing instruments or devices referred to in the other groups of this subclass initial alignment, calibration or starting-up of inertial devices
Abstract
本发明属于微惯性导航系统技术领域,具体公开了一种步行微惯性导航设备方位安装偏差校正方法。初始静止条件设置之后利用加速度计信息确定步行微惯性导航设备的水平姿态信息,对步行微惯性导航进行状态修正,确定方位安装偏差,对步行微惯性导航设备采样值进行补偿,得到由于方位安装误差会造成的定位误差累积,即侧向位移,从而估算出方位安装误差,在惯导解算前予以校正。操作方法简单,方位安装误差估计准确,校正方法易行,能够有效提高步行微惯性导航设备的导航定位精度。
Description
技术领域
本发明属于微惯性导航系统技术领域,具体涉及一种足部安装的步行微惯性导航系统方位安装偏差校正方法。
背景技术
步行微惯性导航设备在使用中,由于每个人的步态习惯不同,导致足尖方向与行进方存在着一个角度,这个角度虽然对于每个人都比较固定,但是却因人而异,无法事先标定和校正。这个角度对于步行微惯性导航设备来讲,等同于方位安装误差,将会造成导航定位误差的持续累积。因而,必须采取一定的措施,对其进行校正,从而提高步行微惯性导航设备的导航定位精度。
发明内容
本发明的目的是提供一种步行微惯性导航设备方位安装偏差校正方法,抑制方位安装偏差对步行微惯性导航设备定位误差的影响,提高步行微惯性导航设备的定位精度。
本发明的技术方案如下:
一种步行微惯性导航设备方位安装偏差校正方法,该方法包括如下步骤:
1-初始静止条件设置
2)在静止初始条件状态下,利用加速度计信息确定步行微惯性导航设备的水平姿态信息;
3)设备使用人向前直线行走,对步行微惯性导航进行状态修正;
4)确定行走起点指向导航定位结果的直线与行走方向的夹角,即由于行走习惯造成的方位安装偏差α;
其中,Px为直线行走终点的位置n系下x方向的值;Py直线行走终点的位置n系下y方向的值;Px0为直线行走起点的位置在n系下x方向的值;Py0为直线行走起点的位置n系下y方向的值;
5)确定计算补偿转换矩阵T,对步行微惯性导航设备采样值进行补偿
确定误差校正后的步行微惯性导航设备惯性器件陀螺和加速度计的输出值
其中和分别为步行微惯性导航设备惯性器件原始输出的陀螺测量值和加速度计测量值。
步骤2)具体为:
2.1)读取静止状态的步行微惯性导航设备中惯性期间输出数据,计算载体坐标系下加速度计输出在三个方向上的平均值;
2.2)确定步行微惯性导航设备的初始姿态角;
2.3)确定步行微惯性导航设备的初始姿态矩阵;
2.4)确定步行微惯性导航设备的初始四元数。
步骤2.2)具体为:
初始姿态角包括初始俯仰角θ、初始倾斜角γ和初始航向角ψ,初始俯仰角θ=arcsin(ax/g),其中g为当地重力加速度;
如果az≥0,初始倾斜角γ=-arcsin(az/(g·cosθ));
如果az<0,ay≥0,初始倾斜角γ=π+arcsin(az/(g·cosθ));
如果az<0,ay<0,初始倾斜角γ=π-arcsin(az/(g·cosθ));
初始航向角ψ置为零。
步骤2.3)的初始姿态矩阵采用下式确定
步骤2.4)具体为:
将确定的设备初始姿态矩阵的各元素表示成如下形式:
将设备的四元数Q表示成:
Q=[q0 q1 q2 q3]T
则,设备的四元数的初始值由下式得到
步骤3)对微惯性导航进行状态修正为速度误差、水平姿态角误差、陀螺漂移及加速度零偏的修正。
步骤3)对微惯性导航进行状态修正之前,首先对步行微惯性导航设备的运动状态进行判断,可以采用A3T3ND、AM1T3ND、A3ND或者AM1ND等公知方法,得到设备当前或运动或静止的运动状态。
步骤3)对微惯性导航进行状态修正采用卡尔曼滤波方法进行状态修正。
本发明的显著效果如下:利用北向直线前进条件,采用零速修正算法辅助微惯导解算,得到由于方位安装误差会造成的定位误差累积,即侧向位移,从而估算出方位安装误差,在惯导解算前予以校正。操作方法简单,方位安装误差估计准确,校正方法易行,能够有效提高步行微惯性导航设备的导航定位精度。
具体实施方式
下面通过具体实施方式对本发明作进一步说明。
本坐标系定义如下:
b:载体坐标系oxbybzb,右前上坐标系,xb轴指向人体的右,yb轴指向人体的前,zb轴指向人体的上;n:导航坐标系oxnynzn,zn轴指向天,yn轴在水平面内指向载体初始时刻的前向,xn轴与yn轴和zn轴构成笛卡尔坐标系。
步骤1)初始静止条件设置
将步行微惯性导航设备安装在使用人的足底,使用人面向北方站立,静止保持30s以上(30~50s)。
步骤2)在静止初始条件状态下,利用加速度计信息确定步行微惯性导航设备的水平姿态信息;
2.1)读取静止状态的步行微惯性导航设备中惯性期间输出数据,计算载体坐标系下加速度计输出在三个方向上的平均值:
ab=[ax ay az]
其中,ab为加速度计输出的平均值在载体坐标系下的矢量形式,ax为加速度计输出在载体坐标系xb下分量的平均值,ay为加速度计输出在载体坐标系yb下分量的平均值,az为加速度计输出在载体坐标系zb下分量的平均值。
2.2)确定步行微惯性导航设备的初始姿态角
包括初始俯仰角θ、初始倾斜角γ和初始航向角ψ,初始俯仰角θ=arcsin(ax/g),其中g为当地重力加速度;
如果az≥0,初始倾斜角γ=-arcsin(az/(g·cosθ));
如果az<0,ay≥0,初始倾斜角γ=π+arcsin(az/(g·cosθ));
如果az<0,ay<0,初始倾斜角γ=π-arcsin(az/(g·cosθ));
由于当前行人面向北方,初始航向角ψ置为零。
2.3)确定步行微惯性导航设备的初始姿态矩阵
2.4)确定步行微惯性导航设备的初始四元数
将确定的设备初始姿态矩阵的各元素表示成如下形式:
将设备的四元数Q表示成:
Q=[q0 q1 q2 q3]T
则,设备的四元数的初始值由下式得到
步骤3)设备使用人向前直线行走,对微惯性导航进行状态修正。
本实施例中采用零速修正算法对微惯性导航进行修正,包括修正速度误差、水平姿态角误差、陀螺漂移及加速度零偏。
3.1)首先对步行微惯性导航设备的运动状态进行判断,可以采用A3T3ND、AM1T3ND、A3ND或者AM1ND等公知方法,得到设备当前或运动或静止的运动状态。
3.2)采用卡尔曼滤波方法进行状态修正
卡尔曼滤波器的状态量选为
式中:δVn=[δvx δvy δvz]T为沿n系的速度误差;
δPn=[δPx δPy δPz]T为沿n系的位移误差;
Φn=[φx φy φz]T为沿n系的失准角;
为沿b系的加速度计零偏;
εb=[εx εy εz]T为沿b系的陀螺常值漂移;
上述几个式子中下角标分别代表各个坐标系不同方向的对应物理量。
卡尔曼滤波状态方程为
式中:F为状态转移矩阵,表达式为
其中,为的转置;
W为状态噪声向量,表达式为
W=[ωa 03×1 ωg 03×1 03×1]T
其中,ωa=[ωax ωay ωaz]T为加速度计的随机游走;ωg=[ωgx ωgy ωgz]T为陀螺的随机游走。下角标代表不同方向下的对应物理量。
卡尔曼滤波量测方程为
Z=HX+V
式中:Z为观测量,包含n系下三个方向的速度误差,n系下的速度误差即惯导解算的速度在n系下的表示即
H为量测矩阵,表达式为H=[I3×3 03×12]。
当检测为运动时,卡尔曼滤波只进行时间更新;当检测为静止时,卡尔曼滤波进行完整更新,并闭环修正速度误差、水平姿态角误差、陀螺漂移及加速度零偏。
步骤4)确定行走起点指向导航定位结果的直线与北向的夹角,即由于行走习惯造成的方位安装偏差。
记采用零速修正算法辅助的微惯性导航解算得到的直线行走终点的位置在n系下的值为Pn=[Px Py Pz]T,记直线行走起点的位置在n系下的值为不同下角标代表相应坐标系下不同方向的物理量。
则方位安装偏差α的计算方法为
步骤5)确定计算补偿转换矩阵T,对步行微惯性导航设备采样值进行补偿。
补偿转换矩阵T的计算方法为
记步行微惯性导航设备惯性器件输出的陀螺测量值和加速度计测量值分别为和那么经过方位安装误差校正后的步行微惯性导航设备惯性器件输出的陀螺测量值和加速度计测量值分别
Claims (9)
1.一种步行微惯性导航设备方位安装偏差校正方法,其特征在于,该方法包括如下步骤:
1)初始静止条件设置
2)在静止初始条件状态下,利用加速度计信息确定步行微惯性导航设备的水平姿态信息;
3)设备使用人向前直线行走,对步行微惯性导航进行状态修正;
4)确定行走起点指向导航定位结果的直线与行走方向的夹角,即由于行走习惯造成的方位安装偏差α;
其中,Px为直线行走终点的位置n系下x方向的值;Py直线行走终点的位置n系下y方向的值;Px0为直线行走起点的位置在n系下x方向的值;Py0为直线行走起点的位置n系下y方向的值;
5)确定计算补偿转换矩阵T,对步行微惯性导航设备采样值进行补偿
确定误差校正后的步行微惯性导航设备惯性器件陀螺和加速度计的输出值
其中和分别为步行微惯性导航设备惯性器件原始输出的陀螺测量值和加速度计测量值。
2.如权利要求1所述的一种步行微惯性导航设备方位安装偏差校正方法,其特征在于,步骤2)具体为:
2.1)读取静止状态的步行微惯性导航设备中惯性期间输出数据,计算载体坐标系下加速度计输出在三个方向上的平均值;
2.2)确定步行微惯性导航设备的初始姿态角;
2.3)确定步行微惯性导航设备的初始姿态矩阵;
2.4)确定步行微惯性导航设备的初始四元数。
3.如权利要求2所述的一种步行微惯性导航设备方位安装偏差校正方法,其特征在于,所述的步骤2.2)具体为:
初始姿态角包括初始俯仰角θ、初始倾斜角γ和初始航向角ψ,初始俯仰角θ=arcsin(ax/g),其中g为当地重力加速度;
如果az≥0,初始倾斜角γ=-arcsin(az/(g·cosθ));
如果az<0,ay≥0,初始倾斜角γ=π+arcsin(az/(g·cosθ));
如果az<0,ay<0,初始倾斜角γ=π-arcsin(az/(g·cosθ));
初始航向角ψ置为零。
4.如权利要求2所述的一种步行微惯性导航设备方位安装偏差校正方法,其特征在于,所述步骤2.3)的初始姿态矩阵采用下式确定
5.如权利要求2所述的一种步行微惯性导航设备方位安装偏差校正方法,其特征在于,所述步骤2.4)具体为:
将确定的设备初始姿态矩阵的各元素表示成如下形式:
将设备的四元数Q表示成:
Q=[q0 q1 q2 q3]T
则,设备的四元数的初始值由下式得到
6.如权利要求1所述的一种步行微惯性导航设备方位安装偏差校正方法,其特征在于,所述步骤3)对微惯性导航进行状态修正为速度误差、水平姿态角误差、陀螺漂移及加速度零偏的修正。
7.如权利要求1所述的一种步行微惯性导航设备方位安装偏差校正方法,其特征在于,所述步骤3)对微惯性导航进行状态修正之前,首先对步行微惯性导航设备的运动状态进行判断,可以采用A3T3ND、AM1T3ND、A3ND或者AM1ND等公知方法,得到设备当前或运动或静止的运动状态。
8.如权利要求1所述的一种步行微惯性导航设备方位安装偏差校正方法,其特征在于,所述步骤3)对微惯性导航进行状态修正采用卡尔曼滤波方法进行状态修正。
9.如权利要求8所述的一种步行微惯性导航设备方位安装偏差校正方法,其特征在于,采用卡尔曼滤波方法具体为:
卡尔曼滤波器的状态量选为
X=[δVn δPn Φn ▽b εb]T
式中:δVn=[δvx δvy δvz]T为沿n系的速度误差;δPn=[δPx δPy δPz]T为沿n系的位移误差;Φn=[φx φy φz]T为沿n系的失准角;▽b=[▽x ▽y ▽z]T为沿b系的加速度计零偏;εb=[εx εy εz]T为沿b系的陀螺常值漂移;
卡尔曼滤波状态方程为
式中:F为状态转移矩阵,表达式为
为初始姿态矩阵的转置;
W为状态噪声向量,表达式为
W=[ωa 03×1 ωg 03×1 03×1]T
其中,ωa=[ωax ωay ωaz]T为加速度计的随机游走;ωg=[ωgx ωgy ωgz]T为陀螺的随机游走;
卡尔曼滤波量测方程为
Z=HX+V
式中:Z为观测量,包含n系下三个方向的速度误差,n系下的速度误差即惯导解算的速度在n系下的表示即H为量测矩阵,表达式为H=[I3×3 03×12];
当检测为运动时,卡尔曼滤波只进行时间更新;当检测为静止时,卡尔曼滤波进行完整更新,并闭环修正速度误差、水平姿态角误差、陀螺漂移及加速度零偏。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201710655650.3A CN109387220B (zh) | 2017-08-03 | 2017-08-03 | 一种步行微惯性导航设备方位安装偏差校正方法 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201710655650.3A CN109387220B (zh) | 2017-08-03 | 2017-08-03 | 一种步行微惯性导航设备方位安装偏差校正方法 |
Publications (2)
Publication Number | Publication Date |
---|---|
CN109387220A true CN109387220A (zh) | 2019-02-26 |
CN109387220B CN109387220B (zh) | 2022-07-08 |
Family
ID=65412093
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN201710655650.3A Active CN109387220B (zh) | 2017-08-03 | 2017-08-03 | 一种步行微惯性导航设备方位安装偏差校正方法 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN109387220B (zh) |
Cited By (1)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN112781622A (zh) * | 2020-12-31 | 2021-05-11 | 厦门华源嘉航科技有限公司 | 一种行人导航mimu安装误差在线标定方法 |
Citations (5)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
JP2004239643A (ja) * | 2003-02-03 | 2004-08-26 | Furuno Electric Co Ltd | ハイブリッド航法装置 |
CN102278988A (zh) * | 2011-07-01 | 2011-12-14 | 微迈森惯性技术开发(北京)有限公司 | 一种步行定位方法与设备 |
CN103968827A (zh) * | 2014-04-09 | 2014-08-06 | 北京信息科技大学 | 一种可穿戴式人体步态检测的自主定位方法 |
CN106821391A (zh) * | 2017-03-23 | 2017-06-13 | 北京精密机电控制设备研究所 | 基于惯性传感器信息融合的人体步态采集分析系统及方法 |
CN106908021A (zh) * | 2017-01-19 | 2017-06-30 | 浙江大学 | 一种用于可穿戴式设备的人体步长测量方法及其测量设备 |
-
2017
- 2017-08-03 CN CN201710655650.3A patent/CN109387220B/zh active Active
Patent Citations (5)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
JP2004239643A (ja) * | 2003-02-03 | 2004-08-26 | Furuno Electric Co Ltd | ハイブリッド航法装置 |
CN102278988A (zh) * | 2011-07-01 | 2011-12-14 | 微迈森惯性技术开发(北京)有限公司 | 一种步行定位方法与设备 |
CN103968827A (zh) * | 2014-04-09 | 2014-08-06 | 北京信息科技大学 | 一种可穿戴式人体步态检测的自主定位方法 |
CN106908021A (zh) * | 2017-01-19 | 2017-06-30 | 浙江大学 | 一种用于可穿戴式设备的人体步长测量方法及其测量设备 |
CN106821391A (zh) * | 2017-03-23 | 2017-06-13 | 北京精密机电控制设备研究所 | 基于惯性传感器信息融合的人体步态采集分析系统及方法 |
Non-Patent Citations (1)
Title |
---|
张金亮等: "基于MEMS惯性技术的鞋式个人导航系统", 《中国惯性技术学报》 * |
Cited By (1)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN112781622A (zh) * | 2020-12-31 | 2021-05-11 | 厦门华源嘉航科技有限公司 | 一种行人导航mimu安装误差在线标定方法 |
Also Published As
Publication number | Publication date |
---|---|
CN109387220B (zh) | 2022-07-08 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
US11105633B2 (en) | Navigation system utilizing yaw rate constraint during inertial dead reckoning | |
JP3602095B2 (ja) | 地形を進む有脚動物のための地形ナビゲーション装置 | |
CN110553646B (zh) | 基于惯性和磁航向及零速修正的行人导航方法 | |
US9791575B2 (en) | GNSS and inertial navigation system utilizing relative yaw as an observable for an ins filter | |
US8010308B1 (en) | Inertial measurement system with self correction | |
CN105606094B (zh) | 一种基于mems/gps组合系统的信息条件匹配滤波估计方法 | |
CN110501024A (zh) | 一种车载ins/激光雷达组合导航系统的量测误差补偿方法 | |
Cavallo et al. | A step toward GPS/INS personal navigation systems: real-time assessment of gait by foot inertial sensing | |
KR20170104621A (ko) | 9축 mems 센서에 기반하여 농기계의 전-자세 각도를 갱신하는 방법 | |
EP1582840A1 (en) | Inertial navigation system error correction | |
CN110221332A (zh) | 一种车载gnss/ins组合导航的动态杆臂误差估计和补偿方法 | |
CN106153069B (zh) | 自主导航系统中的姿态修正装置和方法 | |
CN109579836A (zh) | 一种基于mems惯性导航的室内行人方位校准方法 | |
CN111024070A (zh) | 一种基于航向自观测的惯性足绑式行人定位方法 | |
CN110398245A (zh) | 基于脚戴式惯性测量单元的室内行人导航姿态估计方法 | |
CN109000640A (zh) | 基于离散灰色神经网络模型的车辆gnss/ins组合导航方法 | |
CN108458714A (zh) | 一种姿态检测系统中不含重力加速度的欧拉角求解方法 | |
CN109612460A (zh) | 一种基于静止修正的垂线偏差测量方法 | |
CN108627152A (zh) | 一种微型无人机基于多传感器数据融合的导航方法 | |
CN108180923A (zh) | 一种基于人体里程计的惯性导航定位方法 | |
CN112562077A (zh) | 一种融合pdr和先验地图的行人室内定位方法 | |
CN110207692A (zh) | 一种地图辅助的惯性预积分行人导航方法 | |
JP2014240266A (ja) | センサドリフト量推定装置及びプログラム | |
CN105606093B (zh) | 基于重力实时补偿的惯性导航方法及装置 | |
Zhang et al. | A multi-position calibration algorithm for inertial measurement units |
Legal Events
Date | Code | Title | Description |
---|---|---|---|
PB01 | Publication | ||
PB01 | Publication | ||
SE01 | Entry into force of request for substantive examination | ||
SE01 | Entry into force of request for substantive examination | ||
GR01 | Patent grant | ||
GR01 | Patent grant |