CN112781622A - 一种行人导航mimu安装误差在线标定方法 - Google Patents

一种行人导航mimu安装误差在线标定方法 Download PDF

Info

Publication number
CN112781622A
CN112781622A CN202011633522.7A CN202011633522A CN112781622A CN 112781622 A CN112781622 A CN 112781622A CN 202011633522 A CN202011633522 A CN 202011633522A CN 112781622 A CN112781622 A CN 112781622A
Authority
CN
China
Prior art keywords
axis
mimu
installation error
pedestrian
navigation
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
Application number
CN202011633522.7A
Other languages
English (en)
Other versions
CN112781622B (zh
Inventor
陈天宇
张江源
闻泽阳
杨长浩
张文龙
刘宇尘
邢金龙
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Xiamen Huayuan Jiahang Technology Co ltd
Original Assignee
Xiamen Huayuan Jiahang Technology Co ltd
Priority date (The priority date 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 date listed.)
Filing date
Publication date
Application filed by Xiamen Huayuan Jiahang Technology Co ltd filed Critical Xiamen Huayuan Jiahang Technology Co ltd
Priority to CN202011633522.7A priority Critical patent/CN112781622B/zh
Publication of CN112781622A publication Critical patent/CN112781622A/zh
Application granted granted Critical
Publication of CN112781622B publication Critical patent/CN112781622B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

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
    • Y—GENERAL TAGGING OF NEW TECHNOLOGICAL DEVELOPMENTS; GENERAL TAGGING OF CROSS-SECTIONAL TECHNOLOGIES SPANNING OVER SEVERAL SECTIONS OF THE IPC; TECHNICAL SUBJECTS COVERED BY FORMER USPC CROSS-REFERENCE ART COLLECTIONS [XRACs] AND DIGESTS
    • Y02—TECHNOLOGIES OR APPLICATIONS FOR MITIGATION OR ADAPTATION AGAINST CLIMATE CHANGE
    • Y02T—CLIMATE CHANGE MITIGATION TECHNOLOGIES RELATED TO TRANSPORTATION
    • Y02T90/00—Enabling technologies or technologies with a potential or indirect contribution to GHG emissions mitigation

Landscapes

  • Engineering & Computer Science (AREA)
  • Manufacturing & Machinery (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Navigation (AREA)

Abstract

本发明公开了一种行人导航MIMU安装误差在线标定方法，属于导航技术领域。本发明包括以下步骤：1、将MIMU安装在行人足部；2、让行人在平坦的地面上静止站立1~10秒；3、计算惯导MIMU水平安装误差角；4、补偿惯导MIMU水平安装误差角；5、让行人笔直向前行走至少一步；6、做捷联惯导姿态更新解算；7、计算MIMU航向安装误差角；8、补偿MIMU航向安装误差角。本发明的方法实现了行人导航中MIMU的穿戴者能够在任意水平面内快速标定MIMU的安装误差，解决行人导航中由于MIMU安装误差的存在造成的定位精度降低的问题和行走过程中航向角抖动剧烈的问题，显著提高了行人导航的定位定向精度。

Description

一种行人导航MIMU安装误差在线标定方法
技术领域
本发明属于导航技术领域，具体涉及一种行人导航MIMU安装误差在线标定方法。
背景技术
行人导航系统是导航定位领域中一个新兴分支，旨在充分利用MEMS惯性器件重量轻、体积小、便携性好、价格低廉的优点，发挥惯性导航技术自主性强的特点，实现行人在无GNSS信号覆盖的室内定位与导航。
行人导航技术的核心器件之一即是安装在行人足部的微惯性测量单元(MicroInertial Measurement Unit，简称MIMU)。安装于足部的惯性测量单元通过敏感足部运动的变化，计算得到行人的三轴姿态、步频和步长等信息，而后通过行人航位推算(Pedestrian Dead Reckoning，PDR)算法获得行人的位置信息。
目前，常用的MIMU安装方式有：将MIMU内嵌于鞋垫中、将MIMU安装于脚背位置、将MIMU安装于脚后跟位置等。但不论是哪种方式，在实际安装过程中，由于安装势必会存在误差，MIMU的惯导坐标系p系无法保证与足部载体坐标系b系保持一致，因此MIMU 敏感的三轴角速度信息、三轴加速度信息，以及解算所得的三轴姿态信息均无法真实反映行人足部的实际情况，此外由于安装误差角的存在，也将导致行人在行走过程中航向角发生剧烈的抖动，因此需要对行人导航MIMU安装误差进行标定补偿。
发明内容
鉴于现有技术的不足，本发明的目的在于提供一种行人导航MIMU安装误差在线标定方法，实现了穿戴者能够在任意水平面上快速标定MIMU的安装误差，解决了由于MIMU 安装误差的存在导致的定位精度降低的问题和行人行走过程航向角抖动剧烈的问题，显著提高了行人导航的定位定向精度。
为实现上述目的，本发明采用如下技术方案：一种行人导航MIMU安装误差在线标定方法，包括以下步骤：
步骤1：将MIMU安装在行人足部；
步骤2：让行人在平坦的地面上静止站立1～10秒；
步骤3：计算MIMU水平安装误差角；
步骤4：补偿MIMU水平安装误差角；
步骤5：让行人笔直向前行走至少一步；
步骤6：做捷联惯导姿态更新解算；
步骤7：计算MIMU航向安装误差角；
步骤8：补偿MIMU航向安装误差角。
优选地，所述MIMU包括：
三轴MEMS陀螺仪和三轴MEMS加速度计。
优选地，所述标定方法的使用环境为：
任意水平的室内外环境。
优选地，所述标定方法采用的坐标系定义如下：
p系：惯导坐标系oxpypzp，轴心为惯导IMU的重心，x轴指载体的右方，y轴指载体的前方，且y轴与x轴垂直，z轴指载体的上方，且z轴与x、y轴构成满足右手定则的笛卡儿直角坐标系；
b系：足部载体坐标系oxbybzb，轴心为脚掌重心，x轴指脚掌的右方，y轴指向脚掌的前方，且y轴与x轴垂直，z轴指载体的上方，且z轴与x、y轴构成满足右手定则的笛卡儿直角坐标系；
n系：导航坐标系oxnynzn，采用“东-北-天”地理坐标系，x轴指东，y轴指北，z轴指天；
h系：水平坐标系ohxhyhzh，x轴与y轴在当地的水平面内，y轴与x轴垂直，且z轴与x、y轴构成满足右手定则的笛卡儿直角坐标系；
i系：惯性参考坐标系oixiyizi，原点为地球中心，x轴与y轴在地球赤道平面内，其中x 轴指向春分点，z轴为地球自转轴，并指向北极。
优选地，所述步骤1中，MIMU的安装方式如下：
MIMU直接安装于脚面、脚后跟或内嵌于鞋垫中，且满足足部载体坐标系(b系)和惯导坐标系(p系)的三轴方向基本一致。
优选地，所述步骤3中，MIMU水平安装误差角的标定方法如下：
设俯仰安装误差角为θ0，横滚安装误差角为γ0，航向安装误差角为ψ0，则惯导坐标系(p 系)到足部载体坐标系(b系)的方向余弦矩阵
Figure RE-GDA0002988633210000021
为：
Figure RE-GDA0002988633210000031
因为
Figure RE-GDA0002988633210000032
为正交矩阵，所以有：
Figure RE-GDA0002988633210000033
Figure RE-GDA0002988633210000034
式中：
Figure RE-GDA0002988633210000035
表征三轴加速度计的敏感值，
Figure RE-GDA0002988633210000036
g0为地球重力加速度值；
所以有：
Figure RE-GDA0002988633210000037
因此，得到俯仰安装误差角θ0、横滚安装误差角γ0，分别为：
Figure RE-GDA0002988633210000038
Figure RE-GDA0002988633210000039
优选地，所述步骤4中，MIMU水平安装误差角的补偿方法如下：
Figure RE-GDA00029886332100000310
其中：
Figure RE-GDA00029886332100000311
Figure RE-GDA00029886332100000312
Figure RE-GDA0002988633210000041
式中，矩阵
Figure RE-GDA0002988633210000042
表示经水平安装误差补偿后水平坐标系(h系)相对于导航坐标系(n系) 的方向余弦阵，矩阵
Figure RE-GDA0002988633210000043
表示惯导坐标系(p系)相对于导航坐标系(n系)的方向余弦阵，θ0为俯仰安装误差角，γ0为横滚安装误差角。
优选地，所述步骤6中，捷联惯导姿态更新解算的方法如下：
以n系作为参考系，则姿态微分方程可表示为如下形式：
Figure RE-GDA0002988633210000044
式中，矩阵
Figure RE-GDA0002988633210000045
表示足部载体坐标系(b系)相对于导航坐标系(n系)的姿态阵，由于惯导中陀螺输出的是足部载体坐标系(b系)相对于惯性参考坐标系(i系)的角速度
Figure RE-GDA0002988633210000046
而上式中角速度信息
Figure RE-GDA0002988633210000047
无法直接测量获得，因此将上式作如下变换：
Figure RE-GDA0002988633210000048
式中，
Figure RE-GDA0002988633210000049
表示导航坐标系(n系)相对于惯性参考坐标系(i系)的旋转，它由两部分组成，一是由于地球自转引起的导航系旋转，二是惯导系统在地球表面附近移动因地球表面弯曲引起的n系旋转，即有
Figure RE-GDA00029886332100000410
其中：
Figure RE-GDA00029886332100000411
Figure RE-GDA00029886332100000412
式中，ωie为地球的自转角速率，RM表示子午圈主曲率半径，RN表示卯酉圈主曲率半径， L和h分别为地理纬度和高度，vE为北向速度分量，vN为北向速度分量；由于MEMS惯性器件精度较低，一般无法敏感地球转速，且行人行走速度较慢，因此取ωie＝0，
Figure RE-GDA00029886332100000413
根据矩阵链乘规则，有：
Figure RE-GDA00029886332100000414
式中，角标括号中的符号m表示tm时刻；由于i系是惯性参考坐标系，坐标系绝对不动，因此与时间无关，不需标注时刻；而n系和b系相对于i系都是动坐标系，均跟时间相关，因此需标注时刻；
根据姿态阵微分方程
Figure RE-GDA00029886332100000415
和
Figure RE-GDA00029886332100000416
分别得到相对于惯性参考坐标系的更新算法：
Figure RE-GDA0002988633210000051
Figure RE-GDA0002988633210000052
其中，矩阵
Figure RE-GDA0002988633210000053
表示以i系作为参考基准，b系从tm-1时刻到tm时刻的旋转变化，
Figure RE-GDA0002988633210000054
由陀螺角速度
Figure RE-GDA0002988633210000055
确定；
Figure RE-GDA0002988633210000056
表示以i系作为参考基准，n系从tm时刻到tm-1时刻的旋转变化，
Figure RE-GDA0002988633210000057
由计算角速度
Figure RE-GDA0002988633210000058
确定；
所以有：
Figure RE-GDA0002988633210000059
其中，
Figure RE-GDA00029886332100000510
和
Figure RE-GDA00029886332100000511
分别表示tm-1和tm时刻的捷联姿态矩阵；若陀螺在时间段[tm-1,tm]内 (T＝tm-tm-1)进行了两次等间隔采样，角增量分别为Δθm1和Δθm2，采用二子样圆锥误差补偿算法，有：
Figure RE-GDA00029886332100000512
Figure RE-GDA00029886332100000513
式中，
Figure RE-GDA00029886332100000514
为等效旋转矢量，
Figure RE-GDA00029886332100000515
表示方向余弦阵
Figure RE-GDA00029886332100000516
与等效旋转矢量
Figure RE-GDA00029886332100000517
的函数表达式，满足：
Figure RE-GDA00029886332100000518
通常在导航更新周期[tm-1,tm]内，认为由速度和位置引起的
Figure RE-GDA00029886332100000519
变化很小，即视
Figure RE-GDA00029886332100000520
为常值，记为
Figure RE-GDA00029886332100000521
则有：
Figure RE-GDA00029886332100000522
优选地，所述步骤7中，MIMU航向安装误差角的标定方法如下：
设行人做跨步运动时足部载体坐标系(b系)相对于导航坐标系(n系)的方向余弦阵为
Figure RE-GDA00029886332100000523
[θ γ ψ]T为由
Figure RE-GDA00029886332100000524
转换所得的三轴姿态角，表征行人足部的三轴姿态值；水平坐标系 (h系)相对于导航坐标系(n系)为
Figure RE-GDA00029886332100000525
[θ′ γ′ ψ′]T为由
Figure RE-GDA00029886332100000526
转换所得的三轴姿态角；矩阵
Figure RE-GDA00029886332100000527
表示足部载体坐标系(b系)相对于水平坐标系(h系)的方向余弦阵；
根据矩阵链乘规则，有：
Figure RE-GDA00029886332100000528
Figure RE-GDA0002988633210000061
Figure RE-GDA0002988633210000062
其中，
Figure RE-GDA0002988633210000063
满足：
Figure RE-GDA0002988633210000064
因为行人笔直向前行走时，足部的横滚角θ和航向角ψ几乎不作改变，且足部的俯仰角θ与θ′基本相等，所以有：
Figure RE-GDA0002988633210000065
Figure RE-GDA0002988633210000066
因此可得足部载体坐标系(b系)相对于水平坐标系(h系)的方向余弦阵
Figure RE-GDA0002988633210000067
为：
Figure RE-GDA0002988633210000068
式中，简记三角函数
Figure RE-GDA0002988633210000069
因为：
Figure RE-GDA00029886332100000610
所以航向安装误差角ψ0为：
Figure RE-GDA00029886332100000611
式中：
Figure RE-GDA00029886332100000612
Figure RE-GDA0002988633210000071
在实际标定过程中，为方便对航向安装误差角进行标定，取足部做一次跨步行走过程中ψ′为最小值时的三轴姿态进行航向安装误差角的标定；设ψ′为最小值的时刻为tm，其三轴姿态为[θm γm ψm]T，则航向安装误差角ψ0为：
ψ0＝atan2(C1,C2)
式中：
C1＝cosθm(sinψmcosγm+cosψmsinθmsinγm)-cosθmsinθmsinγm
C2＝cosψm(cosθm)2+(sinθm)2。
优选地，所述步骤8中，MIMU航向安装误差角的补偿方法如下：
Figure RE-GDA0002988633210000072
其中：
Figure RE-GDA0002988633210000073
式中，矩阵
Figure RE-GDA0002988633210000074
表示足部载体坐标系(b系)相对于导航坐标系(n系)的方向余弦阵，矩阵
Figure RE-GDA0002988633210000075
表示水平坐标系(h系)相对于导航坐标系(n系)的方向余弦阵，
Figure RE-GDA0002988633210000076
表示足部载体坐标系(b系)相对于水平坐标系(h系)的方向余弦阵，ψ0为航向安装误差角。
与现有技术相比，本发明具有以下有益效果：利用足部所安装的MIMU，实现了穿戴者能够在任意水平面上快速标定MIMU的安装误差，解决了由于MIMU安装误差的存在导致的定位精度降低的问题和行人行走过程航向角抖动剧烈的问题，显著提高了行人导航的定位定向精度。
附图说明
图1为本发明实施例的行人导航MIMU安装误差在线标定方法的总体流程图。
图2为本发明实施例的行人导航MIMU安装误差标定前后航向角变化曲线对比图。
图3为本发明实施例的行人导航MIMU安装误差标定前后俯仰角变化曲线对比图。
图4为本发明实施例的行人导航MIMU安装误差标定前后横滚角变化曲线对比图。
具体实施方式
为了让本发明的上述特征和优点更明显易懂，下面特举实施例，并配合附图，作详细说明如下。
如图1所示，本实施例提供了一种行人导航MIMU安装误差在线标定方法，包括以下步骤：
步骤1：将MIMU安装在行人足部；
步骤2：让行人在平坦的地面上静止站立1～10秒(如2秒、3秒、4秒、5秒、6秒、7 秒、8秒、9秒等)；
步骤3：计算MIMU水平安装误差角；
步骤4：补偿MIMU水平安装误差角；
步骤5：让行人笔直向前行走至少一步(如一步、两步、三步等)；
步骤6：做捷联惯导姿态更新解算；
步骤7：计算MIMU航向安装误差角；
步骤8：补偿MIMU航向安装误差角。
在本实施例中，所述MIMU包括：
三轴MEMS陀螺仪和三轴MEMS加速度计。
在本实施例中，所述标定方法的使用环境为：
任意水平的室内外环境。
在本实施例中，所述标定方法采用的坐标系定义如下：
p系：惯导坐标系oxpypzp，轴心为惯导IMU的重心，x轴指载体的右方，y轴指载体的前方，且y轴与x轴垂直，z轴指载体的上方，且z轴与x、y轴构成满足右手定则的笛卡儿直角坐标系；
b系：足部载体坐标系oxbybzb，轴心为脚掌重心，x轴指脚掌的右方，y轴指向脚掌的前方，且y轴与x轴垂直，z轴指载体的上方，且z轴与x、y轴构成满足右手定则的笛卡儿直角坐标系；
n系：导航坐标系oxnynzn，采用“东-北-天”地理坐标系，x轴指东，y轴指北，z轴指天；
h系：水平坐标系ohxhyhzh，x轴与y轴在当地的水平面内，y轴与x轴垂直，且z轴与x、y轴构成满足右手定则的笛卡儿直角坐标系；
i系：惯性参考坐标系oixiyizi，原点为地球中心，x轴与y轴在地球赤道平面内，其中x 轴指向春分点，z轴为地球自转轴，并指向北极。
在本实施例中，所述步骤1中，MIMU的安装方式如下：
MIMU直接安装于脚面、脚后跟或内嵌于鞋垫中，且满足足部载体坐标系(b系)和惯导坐标系(p系)的三轴方向基本一致。
在本实施例中，所述步骤3中，MIMU水平安装误差角的标定方法如下：
设俯仰安装误差角为θ0，横滚安装误差角为γ0，航向安装误差角为ψ0，则惯导坐标系(p 系)到足部载体坐标系(b系)的方向余弦矩阵
Figure RE-GDA0002988633210000091
为：
Figure RE-GDA0002988633210000092
因为
Figure RE-GDA0002988633210000093
为正交矩阵，所以有：
Figure RE-GDA0002988633210000094
Figure RE-GDA0002988633210000095
式中：
Figure RE-GDA0002988633210000096
表征三轴加速度计的敏感值，
Figure RE-GDA0002988633210000097
g0为地球重力加速度值；
所以有：
Figure RE-GDA0002988633210000098
因此，得到俯仰安装误差角θ0、横滚安装误差角γ0，分别为：
Figure RE-GDA0002988633210000099
Figure RE-GDA00029886332100000910
在本实施例中，所述步骤4中，MIMU水平安装误差角的补偿方法如下：
Figure RE-GDA00029886332100000911
其中：
Figure RE-GDA00029886332100000912
Figure RE-GDA0002988633210000101
Figure RE-GDA0002988633210000102
式中，矩阵
Figure RE-GDA0002988633210000103
表示经水平安装误差补偿后水平坐标系(h系)相对于导航坐标系(n系) 的方向余弦阵，矩阵
Figure RE-GDA0002988633210000104
表示惯导坐标系(p系)相对于导航坐标系(n系)的方向余弦阵，θ0为俯仰安装误差角，γ0为横滚安装误差角。
在本实施例中，所述步骤6中，捷联惯导姿态更新解算的方法如下：
以n系作为参考系，则姿态微分方程可表示为如下形式：
Figure RE-GDA0002988633210000105
式中，矩阵
Figure RE-GDA0002988633210000106
表示足部载体坐标系(b系)相对于导航坐标系(n系)的姿态阵，由于惯导中陀螺输出的是足部载体坐标系(b系)相对于惯性参考坐标系(i系)的角速度
Figure RE-GDA0002988633210000107
而上式中角速度信息
Figure RE-GDA0002988633210000108
无法直接测量获得，因此将上式作如下变换：
Figure RE-GDA0002988633210000109
式中，
Figure RE-GDA00029886332100001010
表示导航坐标系(n系)相对于惯性参考坐标系(i系)的旋转，它由两部分组成，一是由于地球自转引起的导航系旋转，二是惯导系统在地球表面附近移动因地球表面弯曲引起的n系旋转，即有
Figure RE-GDA00029886332100001015
其中：
Figure RE-GDA00029886332100001011
Figure RE-GDA00029886332100001012
式中，ωie为地球的自转角速率，RM表示子午圈主曲率半径，RN表示卯酉圈主曲率半径， L和h分别为地理纬度和高度，vE为北向速度分量，vN为北向速度分量；由于MEMS惯性器件精度较低，一般无法敏感地球转速，且行人行走速度较慢，因此取ωie＝0，
Figure RE-GDA00029886332100001013
根据矩阵链乘规则，有：
Figure RE-GDA00029886332100001014
式中，角标括号中的符号m表示tm时刻；由于i系是惯性参考坐标系，坐标系绝对不动，因此与时间无关，不需标注时刻；而n系和b系相对于i系都是动坐标系，均跟时间相关，因此需标注时刻；
根据姿态阵微分方程
Figure RE-GDA0002988633210000111
和
Figure RE-GDA0002988633210000112
分别得到相对于惯性参考坐标系的更新算法：
Figure RE-GDA0002988633210000113
Figure RE-GDA0002988633210000114
其中，矩阵
Figure RE-GDA0002988633210000115
表示以i系作为参考基准，b系从tm-1时刻到tm时刻的旋转变化，
Figure RE-GDA0002988633210000116
由陀螺角速度
Figure RE-GDA0002988633210000117
确定；
Figure RE-GDA0002988633210000118
表示以i系作为参考基准，n系从tm时刻到tm-1时刻的旋转变化，
Figure RE-GDA0002988633210000119
由计算角速度
Figure RE-GDA00029886332100001110
确定；
所以有：
Figure RE-GDA00029886332100001111
其中，
Figure RE-GDA00029886332100001112
和
Figure RE-GDA00029886332100001113
分别表示tm-1和tm时刻的捷联姿态矩阵；若陀螺在时间段[tm-1,tm]内 (T＝tm-tm-1)进行了两次等间隔采样，角增量分别为Δθm1和Δθm2，采用二子样圆锥误差补偿算法，有：
Figure RE-GDA00029886332100001114
Figure RE-GDA00029886332100001115
式中，
Figure RE-GDA00029886332100001116
为等效旋转矢量，
Figure RE-GDA00029886332100001117
表示方向余弦阵
Figure RE-GDA00029886332100001118
与等效旋转矢量
Figure RE-GDA00029886332100001119
的函数表达式，满足：
Figure RE-GDA00029886332100001120
通常在导航更新周期[tm-1,tm]内，认为由速度和位置引起的
Figure RE-GDA00029886332100001121
变化很小，即视
Figure RE-GDA00029886332100001122
为常值，记为
Figure RE-GDA00029886332100001123
则有：
Figure RE-GDA00029886332100001124
在本实施例中，所述步骤7中，MIMU航向安装误差角的标定方法如下：
设行人做跨步运动时足部载体坐标系(b系)相对于导航坐标系(n系)的方向余弦阵为
Figure RE-GDA00029886332100001125
[θ γ ψ]T为由
Figure RE-GDA00029886332100001126
转换所得的三轴姿态角，表征行人足部的三轴姿态值；水平坐标系 (h系)相对于导航坐标系(n系)为
Figure RE-GDA00029886332100001127
[θ′ γ′ ψ′]T为由
Figure RE-GDA00029886332100001128
转换所得的三轴姿态角；矩阵
Figure RE-GDA00029886332100001129
表示足部载体坐标系(b系)相对于水平坐标系(h系)的方向余弦阵；
根据矩阵链乘规则，有：
Figure RE-GDA0002988633210000121
Figure RE-GDA0002988633210000122
Figure RE-GDA0002988633210000123
其中，
Figure RE-GDA0002988633210000124
满足：
Figure RE-GDA0002988633210000125
因为行人笔直向前行走时，足部的横滚角θ和航向角ψ几乎不作改变，且足部的俯仰角θ与θ′基本相等，所以有：
Figure RE-GDA0002988633210000126
Figure RE-GDA0002988633210000127
因此可得足部载体坐标系(b系)相对于水平坐标系(h系)的方向余弦阵
Figure RE-GDA0002988633210000128
为：
Figure RE-GDA0002988633210000129
式中，简记三角函数
Figure RE-GDA00029886332100001210
因为：
Figure RE-GDA00029886332100001211
所以航向安装误差角ψ0为：
Figure RE-GDA00029886332100001212
式中：
Figure RE-GDA00029886332100001213
Figure RE-GDA0002988633210000131
在实际标定过程中，为方便对航向安装误差角进行标定，取足部做一次跨步行走过程中ψ′为最小值时的三轴姿态进行航向安装误差角的标定；设ψ′为最小值的时刻为tm，其三轴姿态为[θm γm ψm]T，则航向安装误差角ψ0为：
ψ0＝atan2(C1,C2)
式中：
C1＝cosθm(sinψmcosγm+cosψmsinθmsinγm)-cosθmsinθmsinγm
C2＝cosψm(cosθm)2+(sinθm)2。
在本实施例中，所述步骤8中，MIMU航向安装误差角的补偿方法如下：
Figure RE-GDA0002988633210000132
其中：
Figure RE-GDA0002988633210000133
式中，矩阵
Figure RE-GDA0002988633210000134
表示足部载体坐标系(b系)相对于导航坐标系(n系)的方向余弦阵，矩阵
Figure RE-GDA0002988633210000135
表示水平坐标系(h系)相对于导航坐标系(n系)的方向余弦阵，
Figure RE-GDA0002988633210000136
表示足部载体坐标系(b系)相对于水平坐标系(h系)的方向余弦阵，ψ0为航向安装误差角。
图2～图4为某次行人在“静止-直行-静止”试验中，MIMU安装误差标定前后三轴姿态角的变化曲线对比图；图中：虚线为标定补偿前的航向/俯仰/横滚姿态角曲线，实线为标定补偿后的航向/俯仰/横滚姿态角曲线。由图2可知，航向误差角经补偿后精度不仅得到了提升，而且行走过程中的航向角变化幅度也得到了较大程度的改善；由图3和图4可知，水平安装误差角也得到了补偿，精度得到了提高。由此可得行人导航MIMU安装误差在线标定方法能够有效计算并补偿三轴安装误差角，不仅提高了行人导航的定位定向精度，也提高了对行人步态特征的辨识度，有利于行人导航中对行人步态特征的进一步提取与分析。
以上所述，仅为本发明的较佳实施例，并非对本发明做任何形式上的限制，任何熟悉本领域的技术人员但凡未脱离本发明技术方案的内容，依据本发明的技术实质对以上实施例所做任何简单的修改、均等变化与修饰，皆应属本发明的涵盖范围。

Claims (10)

1.一种行人导航MIMU安装误差在线标定方法，其特征在于：包括以下步骤：
步骤1：将MIMU安装在行人足部；
步骤2：让行人在平坦的地面上静止站立1～10秒；
步骤3：计算MIMU水平安装误差角；
步骤4：补偿MIMU水平安装误差角；
步骤5：让行人笔直向前行走至少一步；
步骤6：做捷联惯导姿态更新解算；
步骤7：计算MIMU航向安装误差角；
步骤8：补偿MIMU航向安装误差角。
2.根据权利要求1所述的一种行人导航MIMU安装误差在线标定方法，其特征在于：所述MIMU包括：
三轴MEMS陀螺仪和三轴MEMS加速度计。
3.根据权利要求1所述的一种行人导航MIMU安装误差在线标定方法，其特征在于：所述标定方法的使用环境为：
任意水平的室内外环境。
4.根据权利要求1所述的一种行人导航MIMU安装误差在线标定方法，其特征在于：所述标定方法采用的坐标系定义如下：
p系：惯导坐标系oxpypzp，轴心为惯导IMU的重心，x轴指载体的右方，y轴指载体的前方，且y轴与x轴垂直，z轴指载体的上方，且z轴与x、y轴构成满足右手定则的笛卡儿直角坐标系；
b系：足部载体坐标系oxbybzb，轴心为脚掌重心，x轴指脚掌的右方，y轴指向脚掌的前方，且y轴与x轴垂直，z轴指载体的上方，且z轴与x、y轴构成满足右手定则的笛卡儿直角坐标系；
n系：导航坐标系oxnynzn，采用“东-北-天”地理坐标系，x轴指东，y轴指北，z轴指天；
h系：水平坐标系ohxhyhzh，x轴与y轴在当地的水平面内，y轴与x轴垂直，且z轴与x、y轴构成满足右手定则的笛卡儿直角坐标系；
i系：惯性参考坐标系oixiyizi，原点为地球中心，x轴与y轴在地球赤道平面内，其中x轴指向春分点，z轴为地球自转轴，并指向北极。
5.根据权利要求4所述的一种行人MIMU导航安装误差在线标定方法，其特征在于：所述步骤1中，MIMU的安装方式如下：
MIMU直接安装于脚面、脚后跟或内嵌于鞋垫中，且满足b系和p系的三轴方向基本一致。
6.根据权利要求4所述的一种行人MIMU导航安装误差在线标定方法，其特征在于：所述步骤3中，MIMU水平安装误差角的标定方法如下：
设俯仰安装误差角为θ0，横滚安装误差角为γ0，航向安装误差角为ψ0，则p系到b系的方向余弦矩阵
Figure FDA0002880640580000021
为：
Figure FDA0002880640580000022
因为
Figure FDA0002880640580000023
为正交矩阵，所以有：
Figure FDA0002880640580000024
Figure FDA0002880640580000025
式中：
Figure FDA0002880640580000026
表征三轴加速度计的敏感值，
Figure FDA0002880640580000027
g0为地球重力加速度值；
所以有：
Figure FDA0002880640580000028
因此，得到俯仰安装误差角θ0、横滚安装误差角γ0，分别为：
Figure FDA0002880640580000029
Figure FDA00028806405800000210
7.根据权利要求4所述的一种行人导航MIMU安装误差在线标定方法，其特征在于：所述步骤4中，MIMU水平安装误差角的补偿方法如下：
Figure FDA0002880640580000031
其中：
Figure FDA0002880640580000032
Figure FDA0002880640580000033
Figure FDA0002880640580000034
式中，矩阵
Figure FDA0002880640580000035
表示h系相对于n系的方向余弦阵，矩阵
Figure FDA0002880640580000036
表示p系相对于n系的方向余弦阵，θ0为俯仰安装误差角，γ0为横滚安装误差角。
8.根据权利要求4所述的一种行人导航MIMU安装误差在线标定方法，其特征在于：所述步骤6中，捷联惯导姿态更新解算的方法如下：
以n系作为参考系，则姿态微分方程可表示为如下形式：
Figure FDA0002880640580000037
式中，矩阵
Figure FDA0002880640580000038
表示b系相对于n系的姿态阵，由于惯导中陀螺输出的是b系相对于i系的角速度
Figure FDA0002880640580000039
而上式中角速度信息
Figure FDA00028806405800000310
无法直接测量获得，因此将上式作如下变换：
Figure FDA00028806405800000311
式中，
Figure FDA00028806405800000312
表示n系相对于i系的旋转，它由两部分组成，一是由于地球自转引起的导航系旋转，二是惯导系统在地球表面附近移动因地球表面弯曲引起的n系旋转，即有
Figure FDA00028806405800000313
其中：
Figure FDA00028806405800000314
Figure FDA00028806405800000315
式中，ωie为地球的自转角速率，RM表示子午圈主曲率半径，RN表示卯酉圈主曲率半径，L和h分别为地理纬度和高度，vE为北向速度分量，vN为北向速度分量；由于MEMS惯性器件精度较低，一般无法敏感地球转速，且行人行走速度较慢，因此取ωie＝0，
Figure FDA00028806405800000316
根据矩阵链乘规则，有：
Figure FDA00028806405800000317
式中，角标括号中的符号m表示tm时刻；由于i系是惯性参考坐标系，坐标系绝对不动，因此与时间无关，不需标注时刻；而n系和b系相对于i系都是动坐标系，均跟时间相关，因此需标注时刻；
根据姿态阵微分方程
Figure FDA0002880640580000041
和
Figure FDA0002880640580000042
分别得到相对于惯性参考坐标系的更新算法：
Figure FDA0002880640580000043
Figure FDA0002880640580000044
其中，矩阵
Figure FDA0002880640580000045
表示以i系作为参考基准，b系从tm-1时刻到tm时刻的旋转变化，
Figure FDA0002880640580000046
由陀螺角速度
Figure FDA0002880640580000047
确定；
Figure FDA0002880640580000048
表示以i系作为参考基准，n系从tm时刻到tm-1时刻的旋转变化，
Figure FDA0002880640580000049
由计算角速度
Figure FDA00028806405800000410
确定；
所以有：
Figure FDA00028806405800000411
其中，
Figure FDA00028806405800000412
和
Figure FDA00028806405800000413
分别表示tm-1和tm时刻的捷联姿态矩阵；若陀螺在时间段[tm-1,tm]内(T＝tm-tm-1)进行了两次等间隔采样，角增量分别为Δθm1和Δθm2，采用二子样圆锥误差补偿算法，有：
Figure FDA00028806405800000414
Figure FDA00028806405800000415
式中，
Figure FDA00028806405800000416
为等效旋转矢量，
Figure FDA00028806405800000417
表示方向余弦阵
Figure FDA00028806405800000418
与等效旋转矢量
Figure FDA00028806405800000419
的函数表达式，满足：
Figure FDA00028806405800000420
通常在导航更新周期[tm-1,tm]内，认为由速度和位置引起的
Figure FDA00028806405800000421
变化很小，即视
Figure FDA00028806405800000422
为常值，记为
Figure FDA00028806405800000423
则有：
Figure FDA00028806405800000424
9.根据权利要求4所述的一种行人导航MIMU安装误差在线标定方法，其特征在于：所述步骤7中，MIMU航向安装误差角的标定方法如下：
设行人做跨步运动时b系相对于n系的方向余弦阵为
Figure FDA00028806405800000425
[θ γ ψ]T为由
Figure FDA00028806405800000426
转换所得的三轴姿态角，表征行人足部的三轴姿态值；h系相对于n系为
Figure FDA00028806405800000427
[θ′ γ′ ψ′]T为由
Figure FDA00028806405800000428
转换所得的三轴姿态角；矩阵
Figure FDA0002880640580000051
表示b系相对于h系的方向余弦阵；
根据矩阵链乘规则，有：
Figure FDA0002880640580000052
Figure FDA0002880640580000053
Figure FDA0002880640580000054
其中，
Figure FDA0002880640580000055
满足：
Figure FDA0002880640580000056
因为行人笔直向前行走时，足部的横滚角θ和航向角ψ几乎不作改变，且足部的俯仰角θ与θ′基本相等，所以有：
Figure FDA0002880640580000057
Figure FDA0002880640580000058
因此可得b系相对于h系的方向余弦阵
Figure FDA0002880640580000059
为：
Figure FDA00028806405800000510
式中，简记三角函数
Figure FDA00028806405800000511
因为：
Figure FDA00028806405800000512
所以航向安装误差角ψ0为：
Figure FDA00028806405800000513
式中：
Figure FDA0002880640580000061
Figure FDA0002880640580000062
在实际标定过程中，为方便对航向安装误差角进行标定，取足部做一次跨步行走过程中ψ′为最小值时的三轴姿态进行航向安装误差角的标定；设ψ′为最小值的时刻为tm，其三轴姿态为[θm γm ψm]T，则航向安装误差角ψ0为：
ψ0＝atan2(C1,C2)
式中：
C1＝cosθm(sinψmcosγm+cosψmsinθmsinγm)-cosθmsinθmsinγm
C2＝cosψm(cosθm)2+(sinθm)2。
10.根据权利要求4所述的一种行人导航MIMU安装误差在线标定方法，其特征在于：所述步骤8中，MIMU航向安装误差角的补偿方法如下：
Figure FDA0002880640580000063
其中：
Figure FDA0002880640580000064
式中，矩阵
Figure FDA0002880640580000065
表示b系相对于n系的方向余弦阵，矩阵
Figure FDA0002880640580000066
表示h系相对于n系的方向余弦阵，
Figure FDA0002880640580000067
表示b系相对于h系的方向余弦阵，ψ0为航向安装误差角。
CN202011633522.7A 2020-12-31 2020-12-31 一种行人导航mimu安装误差在线标定方法 Active CN112781622B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202011633522.7A CN112781622B (zh) 2020-12-31 2020-12-31 一种行人导航mimu安装误差在线标定方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202011633522.7A CN112781622B (zh) 2020-12-31 2020-12-31 一种行人导航mimu安装误差在线标定方法

Publications (2)

Publication Number Publication Date
CN112781622A true CN112781622A (zh) 2021-05-11
CN112781622B CN112781622B (zh) 2022-07-05

Family

ID=75754845

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202011633522.7A Active CN112781622B (zh) 2020-12-31 2020-12-31 一种行人导航mimu安装误差在线标定方法

Country Status (1)

Country Link
CN (1) CN112781622B (zh)

Cited By (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN114705218A (zh) * 2022-03-01 2022-07-05 重庆邮电大学 一种基于大动态环境下不可交换误差抑制方法

Citations (8)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN102445200A (zh) * 2011-09-30 2012-05-09 南京理工大学 微小型个人组合导航系统及其导航定位方法
JP2012088253A (ja) * 2010-10-22 2012-05-10 Casio Comput Co Ltd 測位装置、測位方法およびプログラム
CN103900607A (zh) * 2014-03-06 2014-07-02 哈尔滨工程大学 一种基于惯性系的旋转式捷联惯导系统转位方法
CN103994763A (zh) * 2014-05-21 2014-08-20 北京航空航天大学 一种火星车的sins/cns深组合导航系统及其实现方法
CN108007477A (zh) * 2017-11-29 2018-05-08 哈尔滨工程大学 一种基于正反向滤波的惯性行人定位系统误差抑制方法
CN109387220A (zh) * 2017-08-03 2019-02-26 北京自动化控制设备研究所 一种步行微惯性导航设备方位安装偏差校正方法
CN110398245A (zh) * 2019-07-09 2019-11-01 武汉大学 基于脚戴式惯性测量单元的室内行人导航姿态估计方法
CN111811537A (zh) * 2020-07-02 2020-10-23 重庆青年职业技术学院 一种捷联惯性导航的误差补偿方法及导航系统

Patent Citations (8)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JP2012088253A (ja) * 2010-10-22 2012-05-10 Casio Comput Co Ltd 測位装置、測位方法およびプログラム
CN102445200A (zh) * 2011-09-30 2012-05-09 南京理工大学 微小型个人组合导航系统及其导航定位方法
CN103900607A (zh) * 2014-03-06 2014-07-02 哈尔滨工程大学 一种基于惯性系的旋转式捷联惯导系统转位方法
CN103994763A (zh) * 2014-05-21 2014-08-20 北京航空航天大学 一种火星车的sins/cns深组合导航系统及其实现方法
CN109387220A (zh) * 2017-08-03 2019-02-26 北京自动化控制设备研究所 一种步行微惯性导航设备方位安装偏差校正方法
CN108007477A (zh) * 2017-11-29 2018-05-08 哈尔滨工程大学 一种基于正反向滤波的惯性行人定位系统误差抑制方法
CN110398245A (zh) * 2019-07-09 2019-11-01 武汉大学 基于脚戴式惯性测量单元的室内行人导航姿态估计方法
CN111811537A (zh) * 2020-07-02 2020-10-23 重庆青年职业技术学院 一种捷联惯性导航的误差补偿方法及导航系统

Non-Patent Citations (2)

* Cited by examiner, † Cited by third party
Title
SEN QIU 等: ""Inertial/magnetic sensors based pedestrian dead reckoning by means of multi-sensor fusion"", 《INFORMATION FUSION》 *
李建利等: "微小型捷联惯性测量单元标定及补偿方法", 《宇航学报》 *

Cited By (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN114705218A (zh) * 2022-03-01 2022-07-05 重庆邮电大学 一种基于大动态环境下不可交换误差抑制方法

Also Published As

Publication number Publication date
CN112781622B (zh) 2022-07-05

Similar Documents

Publication Publication Date Title
US9026263B2 (en) Automotive navigation system and method to utilize internal geometry of sensor position with respect to rear wheel axis
ES2826887T3 (es) Método de determinación de la actitud de vehículos con GNSS-INS basándose en una única antena
CN110553646B (zh) 基于惯性和磁航向及零速修正的行人导航方法
JP3602095B2 (ja) 地形を進む有脚動物のための地形ナビゲーション装置
US10352959B2 (en) Method and system for estimating a path of a mobile element or body
CN110779521A (zh) 一种多源融合的高精度定位方法与装置
JP7023234B2 (ja) 歩行者の動きを推定する方法
Cavallo et al. A step toward GPS/INS personal navigation systems: real-time assessment of gait by foot inertial sensing
CN110501024A (zh) 一种车载ins/激光雷达组合导航系统的量测误差补偿方法
CN106908060A (zh) 一种基于mems惯性传感器的高精度室内定位方法
CN109186597B (zh) 一种基于双mems-imu的室内轮式机器人的定位方法
JP2001221652A (ja) 自動車用ナビゲーション・システムのための慣性誘導装置及び方法
US20160018209A1 (en) Vehicle wheel alignment method and system based on gyroscopic sensors or angular rate sensors or mems angular rate sensors
KR20110043538A (ko) 영상 지도의 구축 및 차량 위치 결정을 위한 방법 및 시스템
CN113340298B (zh) 一种惯导和双天线gnss外参标定方法
CN103674064B (zh) 捷联惯性导航系统的初始标定方法
CN111024126A (zh) 一种行人导航定位中的自适应零速修正方法
CN112562077A (zh) 一种融合pdr和先验地图的行人室内定位方法
CN103765226A (zh) 确定惯性传感器方向偏移的方法和系统
JP2021518529A (ja) 車両に装備されたジャイロメータの較正方法
CN112781622B (zh) 一种行人导航mimu安装误差在线标定方法
CN109084755B (zh) 一种基于重力视速度与参数辨识的加速度计零偏估计方法
CN111307114A (zh) 基于运动参考单元的水面舰船水平姿态测量方法
Mizushima et al. Development of a low-cost attitude sensor for agricultural vehicles
RU2548115C1 (ru) Безплатформенный навигационный комплекс с инерциальной системой ориентации на "грубых" чувствительных элементах и способ коррекции его инерциальных датчиков

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