Nothing Special   »   [go: up one dir, main page]

CN112362057A - 基于零速修正与姿态自观测的惯性行人导航算法 - Google Patents

基于零速修正与姿态自观测的惯性行人导航算法 Download PDF

Info

Publication number
CN112362057A
CN112362057A CN202011157273.9A CN202011157273A CN112362057A CN 112362057 A CN112362057 A CN 112362057A CN 202011157273 A CN202011157273 A CN 202011157273A CN 112362057 A CN112362057 A CN 112362057A
Authority
CN
China
Prior art keywords
zero
speed
navigation
error
inertial
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
CN202011157273.9A
Other languages
English (en)
Other versions
CN112362057B (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.)
Naval Aeronautical University
Original Assignee
Naval Aeronautical University
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 Naval Aeronautical University filed Critical Naval Aeronautical University
Priority to CN202011157273.9A priority Critical patent/CN112362057B/zh
Publication of CN112362057A publication Critical patent/CN112362057A/zh
Application granted granted Critical
Publication of CN112362057B publication Critical patent/CN112362057B/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
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
    • G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
    • G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
    • G01C21/18—Stabilised platforms, e.g. by gyroscope
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
    • G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
    • G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/20—Instruments for performing navigational calculations
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/20—Instruments for performing navigational calculations
    • G01C21/206—Instruments for performing navigational calculations specially adapted for indoor navigation
    • 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

Landscapes

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

Abstract

本发明涉及一种基于零速修正与姿态自观测的惯性行人导航算法。行人运动过程中的数据被MIMU惯性传感器接收，并传输到导航计算机中，导航计算机对采集的数据进行捷联惯导解算，得到行人运动的速度、位置以及传感器姿态信息，由于采用捷联惯导解算时，导航误差会随时间逐渐积累，因此需要对导航误差进行修正，对采集的数据同时进行零速检测，检测出行走时的零速区间，设计卡尔曼滤波器，在零速区间内运用零速修正与姿态自观测算法估计出导航姿态、位置以及速度误差，并对惯导解算结果进行误差修正，从而实现对行人的精确定位与导航。本发明的算法充分利用零速区间的步态特性，无需引入外部设备或预设行走方向，具有较好的实用价值。

Description

基于零速修正与姿态自观测的惯性行人导航算法
技术领域
本发明涉及一种基于零速修正与姿态自观测的惯性行人导航算法，属于惯性行人导航算法技术领域。
背景技术
基于位置的服务逐渐在个人消费领域兴起，行人获取自身精确空间位置的需求正变得越来越迫切，而建立在空间定位基础上的行人导航也正成为当前的研究热点。
当前的行人定位与导航算法，可以分为有源算法和无源算法两类。有源算法主要有GNSS定位算法、WIFI定位算法、蓝牙定位算法、超声波定位算法等，而无源算法主要是惯性导航算法。尽管现在的有源算法，如GNSS定位算法已经广泛地应用于生产和生活中，但其也存在比较明显的局限性。有源算法严重依赖于外部信号，在室内信号受阻以及未知环境中无法应用。而无源算法中的惯性导航算法具有全场景应用、可靠性高等优点，相比于有源算法具有更好的适用性。
利用惯性导航算法进行行人定位与导航的方法简称为惯性行人导航。惯性行人导航主要通过将微惯性测量单元(Micro Inertial Measure Unit，MIMU)固定于行人足部，采集行人足部的运动加速度和角速度，通过捷联惯导算法解算出行人的位置，从而实现对行人的定位与导航。但由于MIMU存在测量误差，捷联惯导算法的解算精度会随着行走距离的增加而下降，最终会导致导航结果不可用。
针对上述问题，Eric Foxlin在文献“Foxlin E.Pedestrian tracking withshoe-mounted inertial sensors[J].IEEE Computer Graphics and Applications,2005,25(6):38-46.”中利用行人运动的周期性步态特征，首次将零速修正(zero-velocityupdates，ZUPTs)方法引入到惯性行人导航系统捷联解算中。该方法将零速时刻的速度误差作为观测量，利用扩展卡尔曼滤波(Extended Kalman Filter,EKF)对零速时刻导航误差进行估计与校正，从而实现导航误差的周期性修正，提高了导航精度。尽管ZUPTS算法能够有效修正导航误差，但其仍存在航向不可观的问题，使得导航精度不高。
文献“一种基于双IMU框架的室内个人导航方法(英文)[J].中国惯性技术学报,2015,(06):714-717.”提出一种双惯性测量单元框架，分别在行人足部与肩部固定一个惯性测量单元，通过两个惯性测量单元采集数据，并将两者解算出的航向角之差作为航向角误差的观测量。该算法能够一定程度提升导航精度，但增加惯性单元不仅导致系统复杂度提升，也有可能引入新的测量误差。
文献“Abdulrahim K,Hide C,Moore T,et al.Aiding Low Cost InertialNavigation with Building Heading for Pedestrian Navigation[J].Journal ofNavigation,2011,64(2):219-233.”提出建筑物航向观测算法，该算法通过预设建筑物的四个方向，利用行人在建筑物内直线行走一定处于四个方向之一的特点，将惯导解算出的航向角与上述四个方向之一做差，作为航向角误差的观测量，从而解决航向不可观的问题。上述算法导航结果较好，但其实现过程中需要严格规定行人的行走方向以及预设建筑物方向，具有较大的应用局限性。
文献“Wang Q,Yin J,Noureldin A,et al.Research on an Improved Methodfor Foot-Mounted Inertial/Magnetometer Pedestrian-Positioning Based on theAdaptive Gradient Descent Algorithm[J].Sensors,2018,18(12).”和文献“Qiu S,WangZ L,Zhao H Y,et al.Inertial/magnetic sensors based pedestrian dead reckoningby means of multi-sensor fusion[J].Information Fusion,2018,39:108-119.”利用磁强计测量行人的磁航向，将磁航向与惯导解算航向之差作为航向角误差的观测量，从而实现对航向角误差的观测。该方法由于采用广泛存在的地磁场测量航向，在户外环境具有较好的适用性，但在室内以及复杂城市环境中，磁场容易受到干扰，使得磁航向角测量产生较大误差，从而影响导航误差的校正。
发明内容
为解决现有技术的不足，本发明的目的是提供一种基于零速修正与姿态自观测的惯性行人导航算法。该算法一方面通过零速修正算法解决纯惯导解算误差积累的问题，使惯性行人导航的基本功能得以实现。另一方面，通过姿态自观测方法在零速区间内解决姿态误差增大影响导航精度的问题，本发明的算法充分利用零速区间的步态特性，无需引入外部设备或预设行走方向，具有较好的实用价值。
本发明的一种基于零速修正与姿态自观测的惯性行人导航系统，由MIMU惯性传感器、导航计算机共同构成；所述MIMU惯性传感器安装于行人的左脚后跟位置，通过数据线与导航计算机相连，将采集到的行走数据传输到导航计算机中。
一种基于零速修正与姿态自观测的惯性行人导航算法，其特殊之处在于包括以下步骤：
行人运动过程中的数据被MIMU惯性传感器接收，并传输到导航计算机中，导航计算机对采集的数据进行捷联惯导解算，得到行人运动的速度、位置以及传感器姿态信息，由于采用捷联惯导解算时，导航误差会随时间逐渐积累，因此需要对导航误差进行修正，对采集的数据同时进行零速检测，检测出行走时的零速区间，设计卡尔曼滤波器，在零速区间内运用零速修正与姿态自观测算法估计出导航姿态、位置以及速度误差，并对惯导解算结果进行误差修正，从而实现对行人的精确定位与导航。
所述零速检测之后，在检测到的零速区间内，利用重力加速度分量可以计算出俯仰角θACC和滚转角γACC，捷联惯导解算之后能获得解算俯仰角θINS、滚转角γINS以及航向角φ，θACC和γACC分别与θINS和γINS作差获得俯仰角和滚转角误差观测量，而当前时刻的航向角φ与前两个时刻的航向角平均值作差可以得到航向角误差观测量，俯仰角、滚转角和航向角误差观测量共同构成姿态误差观测量；同时，在零速区间内，惯导解算速度vINS就作为速度误差观测量。对上述速度误差观测量与姿态误差观测量，运用卡尔曼滤波进行状态估计，就可以得到姿态、位置和速度误差的估计值，利用误差估计值就可以实现系统的误差修正；
所述零速检测采用加速度模值、加速度模值标准差、角速度模值、角速度模值标准差，四条件零速检测算法，应用于本发明行走数据的零速区间检测，保证检测结果具有更高的检测精度；
1、四条件零速检测算法如下：
(1)加速度模值条件：
Figure BDA0002743172360000041
Figure BDA0002743172360000042
其中，i为采样点，ax,ay,az分别为三轴输出加速度信息，|ai|为加速度模值，th|a|max与th|a|min分别为阈值上下限。
(2)角速度模值条件：
Figure BDA0002743172360000043
Figure BDA0002743172360000044
其中，wx,wy,wz分别为三轴输出角速度信息，|wi|为角速度模值，th|ω|为角速度模值阈值；
(3)加速度模值标准差条件：
Figure BDA0002743172360000051
Figure BDA0002743172360000052
其中，
Figure BDA0002743172360000053
是以|ai|为采样中点，2n+1为采样区间的加速度模值标准差，
Figure BDA0002743172360000054
为加速度模值均值，|aj|表示采样点j的加速度模值，thσ|a|为加速度模值标准差阈值；
(4)角速度模值标准差条件：
Figure BDA0002743172360000055
Figure BDA0002743172360000056
其中，
Figure BDA0002743172360000057
是以|wi|为采样中点，2n+1为采样区间的角速度模值标准差，
Figure BDA0002743172360000058
为角速度模值均值，|wj|表示采样点j的角速度模值，thσ|w|为角速度模值标准差阈值，阈值设置和采样窗口的选取参考文献“惯性行人导航系统传感器安装位置研究[J].传感技术学报,2020,33(06):815-824.”；
当采样点i同时满足四个条件时，即
C1(i)&C2(i)&C3(i)&C4(i)＝1 (9)
则将采样点i判定为零速点，连续的零速点则构成行走时的零速区间。
所述惯性行人导航系统采用捷联惯导算法进行导航解算，在小扰动情况下，捷联惯导的系统误差模型表示如下：
Figure BDA0002743172360000061
Figure BDA0002743172360000062
Figure BDA0002743172360000063
由于行人速度相对较低且MIMU惯性传感器中的噪声较大，地球自转角速度在测量过程中会被噪声淹没，因此上述模型中的
Figure BDA0002743172360000064
以及δgn可以忽略不计，因此针对惯性行人导航系统简化后的捷联惯导误差模型可以表示为：
Figure BDA0002743172360000065
Figure BDA0002743172360000066
Figure BDA0002743172360000067
其中，
Figure BDA0002743172360000068
表示捷联惯导的姿态角误差，
Figure BDA0002743172360000069
表示捷联惯导的速度误差，δp＝[δx δy δz]T表示捷联惯导的位置误差，
Figure BDA00027431723600000610
为陀螺仪测量误差，
Figure BDA00027431723600000611
为加速度计测量误差，
Figure BDA00027431723600000612
为载体系中的加速度计测量值。
所述行人的脚部运动从脚跟离地开始，经过脚尖离地、空中摆动、脚跟着地、脚尖着地，最后脚部与地面保持相对静止并持续一段时间，之后又开始下一步的脚跟离地，以此做周期性循环，从脚跟离地到持续静止结束之间的脚部动作过程称为一个步态周期，步态周期中脚部在空中的运动阶段称为运动区间，而脚部与地面保持静止的阶段称为零速区间。
所述卡尔曼滤波器设计
根据误差模型，选取上述姿态角、位置以及速度误差作为卡尔曼滤波器的状态变量，建立连续系统状态方程：
Figure BDA0002743172360000071
其中，
Figure BDA0002743172360000072
Figure BDA0002743172360000073
Figure BDA0002743172360000074
Figure BDA0002743172360000075
系统观测方程为：
z(t)＝Hx(t)+v(t) (13)
对上述连续系统进行离散化可得：
Xk＝Φk/k-1Xk-1+Γk/k-1Wk-1 (14)
Zk＝HXk+Vk (15)
其中，
Figure BDA0002743172360000076
Figure BDA0002743172360000081
所述零速修正算法包括以下步骤
当行人处于零速区间时，脚部的理论速度应该等于零，但由于存在测量误差等因素，导致实际计算得到的速度并不等于零。因此，将零速时刻解算得到的速度作为系统速度误差的观测量，零速修正方法的观测方程为：
Zvk＝δVk n＝Vk n-0＝HvXk+Vvk (16)
其中，
Figure BDA0002743172360000082
Hv＝[03×3 03×3 I3×3]。
所述姿态自观测算法包括以下步骤
当行人处于零速区间时，由于脚部与地面保持静止，因此惯性传感器只受到重力加速度的作用，则下列等式在零速区间成立：
Figure BDA0002743172360000083
此时可以通过三轴加速度计输出确定出惯性传感器的俯仰角和滚转角：
Figure BDA0002743172360000084
此时，可以利用惯导解算出的俯仰角θINS和滚转角γINS与通过加速度确定的俯仰角θACC和滚转角γACC作差，构造俯仰角和滚转角误差观测量，但由于无法通过加速度计算航向角，基于上述条件无法构造航向角误差观测量，因此无法通过卡尔曼滤波修正航向角误差。而通过进一步分析零速区间可以发现，脚部与地面处于相对静止时，同一零速区间内的航向角也应具有不变的特性，因此可以将当前采样点航向角解算值与前两个采样点航向角平均值之差作为航向角误差的观测值
Figure BDA0002743172360000091
由此，可以得到姿态自观测方法的观测方程为：
Figure BDA0002743172360000092
其中，
Figure BDA0002743172360000093
将零速修正算法与姿态自观测算法的观测方程进行组合，得到系统观测方程为：
Figure BDA0002743172360000094
其中，
Figure BDA0002743172360000095
根据上述零速修正与姿态自观测算法，通过卡尔曼滤波就可以得到系统误差的估计值
Figure BDA0002743172360000096
从而实现对捷联惯导解算参数的误差修正，
Figure BDA0002743172360000097
本发明的一种基于零速修正与姿态自观测的惯性行人导航算法，首先，通过四条件零速检测算法对行走步态中的零速区间进行检测。在检测得到的零速区间内，利用零速修正算法原理构造速度误差的观测量；利用零速区间内行人脚部与地面保持静止、只受重力加速度以及姿态、航向角不变的特性，构造姿态角误差的观测量；然后，应用卡尔曼滤波对零速区间内的姿态角、位置以及速度误差进行估计。最后，利用得到的误差状态估计结果对行人导航进行误差校正，提高惯性行人导航的精度。实际行走实验表明，该算法的导航轨迹相对误差平均值仅为0.98％，比零速修正算法减小了78.13％，导航轨迹误差标准差仅为0.14m，比零速修正算法减小了88.62％，解算轨迹与实际轨迹匹配度高，具有良好的应用价值。
附图说明
图1：本发明一种基于零速修正与姿态自观测的惯性行人导航算法的原理图；
图2：行人的步态周期图；
图3：四条件法零速检测结果图；
图4：纯惯导算法轨迹图；
图5：零速修正算法轨迹图；
图6：零速修正与俯仰角、滚转角自观测算法轨迹图；
图7：零速修正与姿态自观测算法轨迹图；
图8：不同算法轨迹对比图；
图9：实际行走轨迹参考点示意图。
具体实施方式
下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。
实施例1
请参阅图1-9，一种基于零速修正与姿态自观测的惯性行人导航系统，由MIMU惯性传感器、导航计算机共同构成；所述MIMU惯性传感器安装于行人的左脚后跟位置，通过数据线与导航计算机相连，将采集到的行走数据传输到导航计算机中。
一种基于零速修正与姿态自观测的惯性行人导航算法，包括以下步骤：
行人运动过程中的数据被MIMU惯性传感器接收，并传输到导航计算机中，导航计算机对采集的数据进行捷联惯导解算，得到行人运动的速度、位置以及传感器姿态信息，由于采用捷联惯导解算时，导航误差会随时间逐渐积累，因此需要对导航误差进行修正，对采集的数据同时进行零速检测，检测出行走时的零速区间，设计卡尔曼滤波器，在零速区间内运用零速修正与姿态自观测算法估计出导航姿态、位置以及速度误差，并对惯导解算结果进行误差修正，从而实现对行人的精确定位与导航。
零速检测之后，在检测到的零速区间内，利用重力加速度分量可以计算出俯仰角θACC和滚转角γACC，捷联惯导解算之后能获得解算俯仰角θINS、滚转角γINS以及航向角φ，θACC和γACC分别与θINS和γINS作差获得俯仰角和滚转角误差观测量，而当前时刻的航向角φ与前两个时刻的航向角平均值作差可以得到航向角误差观测量，俯仰角、滚转角和航向角误差观测量共同构成姿态误差观测量；同时，在零速区间内，惯导解算速度vINS就作为速度误差观测量。对上述速度误差观测量与姿态误差观测量，运用卡尔曼滤波进行状态估计，就可以得到姿态、位置和速度误差的估计值，利用误差估计值就可以实现系统的误差修正；
所述零速检测采用加速度模值、加速度模值标准差、角速度模值、角速度模值标准差，四条件零速检测算法，应用于本发明行走数据的零速区间检测，保证检测结果具有更高的检测精度；
1、四条件零速检测算法如下：
(5)加速度模值条件：
Figure BDA0002743172360000111
Figure BDA0002743172360000121
其中，i为采样点，ax,ay,az分别为三轴输出加速度信息，|ai|为加速度模值，th|a|max与th|a|min分别为阈值上下限。
(6)角速度模值条件：
Figure BDA0002743172360000122
Figure BDA0002743172360000123
其中，wx,wy,wz分别为三轴输出角速度信息，|wi|为角速度模值，th|ω|为角速度模值阈值；
(7)加速度模值标准差条件：
Figure BDA0002743172360000124
Figure BDA0002743172360000125
其中，
Figure BDA0002743172360000126
是以|ai|为采样中点，2n+1为采样区间的加速度模值标准差，
Figure BDA0002743172360000127
为加速度模值均值，|aj|表示采样点j的加速度模值，thσ|a|为加速度模值标准差阈值；
(8)角速度模值标准差条件：
Figure BDA0002743172360000128
Figure BDA0002743172360000129
其中，
Figure BDA0002743172360000131
是以|wi|为采样中点，2n+1为采样区间的角速度模值标准差，
Figure BDA0002743172360000132
为角速度模值均值，|wj|表示采样点j的角速度模值，thσ|w|为角速度模值标准差阈值，阈值设置和采样窗口的选取参考文献“惯性行人导航系统传感器安装位置研究[J].传感技术学报,2020,33(06):815-824.”；
当采样点i同时满足四个条件时，即
C1(i)&C2(i)&C3(i)&C4(i)＝1 (9)
则将采样点i判定为零速点，连续的零速点则构成行走时的零速区间。
所述惯性行人导航系统采用捷联惯导算法进行导航解算，在小扰动情况下，捷联惯导的系统误差模型表示如下：
Figure BDA0002743172360000133
Figure BDA0002743172360000134
Figure BDA0002743172360000135
由于行人速度相对较低且MIMU惯性传感器中的噪声较大，地球自转角速度在测量过程中会被噪声淹没，因此上述模型中的
Figure BDA0002743172360000136
以及δgn可以忽略不计，因此针对惯性行人导航系统简化后的捷联惯导误差模型可以表示为：
Figure BDA0002743172360000141
Figure BDA0002743172360000142
Figure BDA0002743172360000143
其中，
Figure BDA0002743172360000144
表示捷联惯导的姿态角误差，
Figure BDA0002743172360000145
表示捷联惯导的速度误差，δp＝[δx δy δz]T表示捷联惯导的位置误差，
Figure BDA0002743172360000146
为陀螺仪测量误差，
Figure BDA0002743172360000147
为加速度计测量误差，
Figure BDA0002743172360000148
为载体系中的加速度计测量值。
所述行人的脚部运动从脚跟离地开始，经过脚尖离地、空中摆动、脚跟着地、脚尖着地，最后脚部与地面保持相对静止并持续一段时间，之后又开始下一步的脚跟离地，以此做周期性循环，从脚跟离地到持续静止结束之间的脚部动作过程称为一个步态周期，步态周期中脚部在空中的运动阶段称为运动区间，而脚部与地面保持静止的阶段称为零速区间。
所述卡尔曼滤波器设计
根据误差模型，选取上述姿态角、位置以及速度误差作为卡尔曼滤波器的状态变量，建立连续系统状态方程：
Figure BDA0002743172360000149
其中，
Figure BDA00027431723600001410
Figure BDA00027431723600001411
Figure BDA0002743172360000151
Figure BDA0002743172360000152
系统观测方程为：
z(t)＝Hx(t)+v(t) (13)
对上述连续系统进行离散化可得：
Xk＝Φk/k-1Xk-1+Γk/k-1Wk-1 (14)
Zk＝HXk+Vk (15)
其中，
Figure BDA0002743172360000153
Figure BDA0002743172360000154
所述零速修正算法包括以下步骤
当行人处于零速区间时，脚部的理论速度应该等于零，但由于存在测量误差等因素，导致实际计算得到的速度并不等于零。因此，将零速时刻解算得到的速度作为系统速度误差的观测量，零速修正方法的观测方程为：
Zvk＝δVk n＝Vk n-0＝HvXk+Vvk (16)
其中，
Figure BDA0002743172360000155
Hv＝[03×3 03×3 I3×3]。
所述姿态自观测算法包括以下步骤
当行人处于零速区间时，由于脚部与地面保持静止，因此惯性传感器只受到重力加速度的作用，则下列等式在零速区间成立：
Figure BDA0002743172360000161
此时可以通过三轴加速度计输出确定出惯性传感器的俯仰角和滚转角：
Figure BDA0002743172360000162
此时，可以利用惯导解算出的俯仰角θINS和滚转角γINS与通过加速度确定的俯仰角θACC和滚转角γACC作差，构造俯仰角和滚转角误差观测量，但由于无法通过加速度计算航向角，基于上述条件无法构造航向角误差观测量，因此无法通过卡尔曼滤波修正航向角误差。而通过进一步分析零速区间可以发现，脚部与地面处于相对静止时，同一零速区间内的航向角也应具有不变的特性，因此可以将当前采样点航向角解算值与前两个采样点航向角平均值之差作为航向角误差的观测值
Figure BDA0002743172360000163
由此，可以得到姿态自观测方法的观测方程为：
Figure BDA0002743172360000164
其中，
Figure BDA0002743172360000165
将零速修正算法与姿态自观测算法的观测方程进行组合，得到系统观测方程为：
Figure BDA0002743172360000171
其中，
Figure BDA0002743172360000172
根据上述零速修正与姿态自观测算法，通过卡尔曼滤波就可以得到系统误差的估计值
Figure BDA0002743172360000173
从而实现对捷联惯导解算参数的误差修正，
Figure BDA0002743172360000174
本实施例采用的MIMU惯性传感器型号为荷兰Xsens公司生产的MTi-710产品，其能够实时测量载体的加速度以及角速度。加速度计的量程为±50m/s2，陀螺仪的量程为±450°/s，惯性传感器的工作频率设置为50Hz。利用本文设计的惯性行人导航系统，进行实际行走实验，行走的路径为9×17m的矩形。实验得到的结果如图3～8所示。
图3为采用四条件零速检测算法得到零速检测结果，当逻辑值为1时，则代表相应的采样点处于零速时刻，逻辑值为0时，代表相应的采样点处于非零速时刻。从图中可以看到，四条件零速检测算法能够较好检测出行人运动的零速时刻。但对于理想的步态周期，零速区间内的零速时刻点应该是连续的，中间不会出现逻辑值为0的间断点。而实际情况下由于存在数据测量误差，零速区间内出现间断点不可避免，这些间断点即为误检点。而四条件零速检测算法能够充分利用采集到的数据信息，使零速区间内出现误检点的数量尽可能地少，使零速检测具有较高的精确度。为评价零速检测算法的精确度，采用零速区间平均单步误检率指标，对本文的零速检测算法精确度进行评估。零速区间平均单步误检率计算方法如下：
Figure BDA0002743172360000181
其中，N为零速区间个数，Mk表示第k个零速区间内零速时刻点个数，mk表示第k个零速区间内误检点个数。
上式的含义即首先计算每个零速区间的误检率，之后对所有零速区间的误检率求平均值，得到平均单步误检率。对于本文零速检测结果计算平均单步误检率，得到结果为η＝2.38％。零速区间内零速时刻点大致在70个左右，因此平均每个零速区间会出现1～2个间断点，这对于系统解算影响很小，因此四条件零速检测法能够适用于惯性行人导航零速区间的检测，具有较高的检测精确度。
图4为纯惯导算法解算得到的行走轨迹图，由于纯惯导算法无法对误差进行修正，误差会随时间不断积累，因此得到的解算轨迹是一条发散的曲线。为便于比较，在图中同时画出实际行走的轨迹，由于实际行走轨迹与纯惯导解算的轨迹在量级上存在较大差距，因此在图4中显示位于起点附近。图4轨迹证明，行人导航中无法利用纯惯导算法进行导航。
图5～7为采用不同修正算法得到的轨迹图，图5为仅采用零速修正算法进行误差修正得到的轨迹图，为便于表述及图例显示，后文将零速修正算法称作算法1；图6为采用零速修正与俯仰角、滚转角观测算法进行误差修正得到的轨迹图，后文将其称作算法2。算法2相比于算法1加入了俯仰角、滚转角误差观测量，但未加入航向角误差观测量；图7为采用本文提出的零速修正与姿态自观测算法进行误差修正得到的轨迹图，后文将其称作算法3。算法3相比于算法2增加了航向角误差观测量，与算法2中的俯仰角、滚转角误差观测量构成完整的姿态误差观测量。
从图5可以得到，算法1解算轨迹由于存在零速修正的作用，不再呈现为发散的曲线，而是能够大致刻画出实际的行走形状，且起点与终点位置较为接近。但由于只有速度误差的观测量，缺少姿态误差的观测，算法1对于姿态误差的修正效果较差，导致轨迹中出现部分的曲折形状，曲折处轨迹与实际行走轨迹存在较大的偏离，且整个轨迹的航向角也存在一定程度的偏差，算法解算精度较低。
从图6可以得到，算法2解算轨迹由于在零速修正基础上加入了俯仰角、滚转角误差的观测量，成功消除了算法1解算轨迹中存在的曲折形状，使曲折轨迹变成直线轨迹，且终点与起点位置非常接近，使得整个解算轨迹呈现出较为理想的矩形路径形状。但由于缺少航向角误差的观测量，算法2对于航向角误差的修正效果较差，使得解算得到的轨迹与实际行走轨迹存在较大的航向角误差，两条轨迹的匹配度较低。
从图7可以得到，由于算法3在零速修正与俯仰角、滚转角观测算法基础上又加入了航向角误差的观测量，使得算法2中存在的航向角误差得到一定地消除，得到的轨迹图与实际行走轨迹更加接近，导航误差得到进一步减小。
图8为3种算法计算得到的轨迹对比图。从图8可以直观地看出三种算法导航误差的大小，算法3导航误差最小且解算轨迹与实际行走轨迹最接近，算法1导航误差最大且解算轨迹与实际行走轨迹差异最大。
为定量对比三种算法的导航精度，选取实际行走轨迹中的四个点作为参考点，分别为转折点1，转折点2，转折点3以及终点，如图9所示。
在三种算法的解算轨迹中，同时取相应的转折点及终点，作为实际行走轨迹参考点在解算轨迹中的对应点。对应点与参考点的位置误差定义为
Figure BDA0002743172360000191
分别计算四个对应点与四个参考点的位置误差，将四个位置误差取平均值，作为解算轨迹与实际轨迹的导航轨迹误差平均值；对四个位置误差求标准差，作为解算轨迹与实际轨迹的导航轨迹误差标准差，计算方法为
Figure BDA0002743172360000201
Figure BDA0002743172360000202
其中，i＝1,2,3,4分别代表转折点1，转折点2，转折点3和终点。
通过导航轨迹误差平均值以及标准差两个定量指标，即可量化三种算法解算轨迹与实际轨迹的接近程度，从而评估三种算法的导航精度。
由于本文涉及的行走轨迹为标准的矩形，解算得到的轨迹也较为接近矩形，因此可以通过上述四个参考点的位置误差平均值以及标准差来描述整个轨迹的导航误差，对于其他较为复杂的行走路径，则应该选择轨迹中多个具有代表性的点来描述导航误差。
三种算法的四个位置误差以及导航轨迹误差计算结果如表1所示。
表1三种算法位置误差与导航轨迹误差(单位:m)
Tab1.Position error and navigation track error of three kinds ofalgorithm
Figure BDA0002743172360000211
从表中的计算结果可以得到，零速修正与姿态自观测方法具有最小的导航轨迹误差平均值ΔR，仅为0.51m，只占总路程的0.98％，相对零速修正算法减小了78.13％，表明零速修正与姿态自观测方法导航的精度最高。而在具有最小导航轨迹误差平均值ΔR的基础上，该算法又具有最小的导航轨迹误差标准差ΔS，仅为0.14m，相对零速修正算法减小了88.62％，则表明该算法解算得到的轨迹与实际轨迹的误差均维持在较小水平且误差变化较小，不存在误差积累或者突变的情况。上述结果共同表明，本文算法得到的解算轨迹与实际轨迹的吻合度较高，导航准确性较高。
本发明针对惯性行人导航中航向角发散致使导航精度降低的问题，提出了一种基于零速修正与姿态自观测的惯性行人导航算法，该算法无需增加传感器且不需要对行人行走轨迹进行约束就能实现对航向角误差的自观测，具有较好的实用价值。本文提出的算法将零速修正与姿态自观测进行结合，与零速修正算法相比增加了姿态角误差的观测量，提高了误差估计的精度。通过实际行走实验，计算得到本文算法的导航轨迹误差平均值仅为0.51m，只占总路程的0.98％，相对零速修正算法减小了78.13％，导航轨迹误差标准差仅为0.14m，相对零速修正算法减小了88.62％，表明本发明算法相比于零速修正算法，有效减小了航向角误差的发散，进一步提高了惯性行人导航的精度。
尽管已经示出和描述了本发明的实施例，对于本领域的普通技术人员而言，可以理解在不脱离本发明的原理和精神的情况下可以对这些实施例进行多种变化、修改、替换和变型，本发明的范围由所附权利要求及其等同物限定。

Claims (7)

1.基于零速修正与姿态自观测的惯性行人导航算法，其特征处在于包括以下步骤：
行人运动过程中的数据被MIMU惯性传感器接收，并传输到导航计算机中，导航计算机对采集的数据进行捷联惯导解算，得到行人运动的速度、位置以及传感器姿态信息，由于采用捷联惯导解算时，导航误差会随时间逐渐积累，因此需要对导航误差进行修正，对采集的数据同时进行零速检测，检测出行走时的零速区间，设计卡尔曼滤波器，在零速区间内运用零速修正与姿态自观测算法估计出导航姿态、位置以及速度误差，并对惯导解算结果进行误差修正，从而实现对行人的精确定位与导航。
2.按照权利要求1所述的基于零速修正与姿态自观测的惯性行人导航算法，其特征处在于所述零速检测之后，在检测到的零速区间内，利用重力加速度分量可以计算出俯仰角θACC和滚转角γACC，捷联惯导解算之后能获得解算俯仰角θINS、滚转角γINS以及航向角φ，θACC和γACC分别与θINS和γINS作差获得俯仰角和滚转角误差观测量，而当前时刻的航向角φ与前两个时刻的航向角平均值作差可以得到航向角误差观测量，俯仰角、滚转角和航向角误差观测量共同构成姿态误差观测量；同时，在零速区间内，惯导解算速度vINS就作为速度误差观测量。对上述速度误差观测量与姿态误差观测量，运用卡尔曼滤波进行状态估计，就可以得到姿态、位置和速度误差的估计值，利用误差估计值就可以实现系统的误差修正。
3.按照权利要求1所述的基于零速修正与姿态自观测的惯性行人导航算法，其特征处在于所述零速检测采用加速度模值、加速度模值标准差、角速度模值、角速度模值标准差，四条件零速检测算法，应用于本发明行走数据的零速区间检测，保证检测结果具有更高的检测精度；
四条件零速检测算法如下：
(1)加速度模值条件：
Figure FDA0002743172350000021
Figure FDA0002743172350000022
其中，i为采样点，ax,ay,az分别为三轴输出加速度信息，|ai|为加速度模值，th|a|max与th|a|min分别为阈值上下限；
(2)角速度模值条件：
Figure FDA0002743172350000023
Figure FDA0002743172350000024
其中，wx,wy,wz分别为三轴输出角速度信息，|wi|为角速度模值，th|ω|为角速度模值阈值；
(3)加速度模值标准差条件：
Figure FDA0002743172350000025
Figure FDA0002743172350000026
其中，
Figure FDA0002743172350000027
是以|ai|为采样中点，2n+1为采样区间的加速度模值标准差，
Figure FDA0002743172350000028
为加速度模值均值，|aj|表示采样点j的加速度模值，thσ|a|为加速度模值标准差阈值；
(4)角速度模值标准差条件：
Figure FDA0002743172350000029
Figure FDA0002743172350000031
其中，
Figure FDA0002743172350000032
是以|wi|为采样中点，2n+1为采样区间的角速度模值标准差，
Figure FDA0002743172350000033
为角速度模值均值，|wj|表示采样点j的角速度模值，thσ|w|为角速度模值标准差阈值，阈值设置和采样窗口的选取参考文献“惯性行人导航系统传感器安装位置研究[J].传感技术学报,2020,33(06):815-824.”；
当采样点i同时满足四个条件时，即
C1(i)&C2(i)&C3(i)&C4(i)＝1 (9)
则将采样点i判定为零速点，连续的零速点则构成行走时的零速区间。
4.按照权利要求1所述的基于零速修正与姿态自观测的惯性行人导航算法，其特征处在于所述惯性行人导航系统采用捷联惯导算法进行导航解算，在小扰动情况下，捷联惯导的系统误差模型表示如下：
Figure FDA0002743172350000034
Figure FDA0002743172350000035
Figure FDA0002743172350000036
由于行人速度相对较低且MIMU惯性传感器中的噪声较大，地球自转角速度在测量过程中会被噪声淹没，因此上述模型中的
Figure FDA0002743172350000037
以及δgn可以忽略不计，因此针对惯性行人导航系统简化后的捷联惯导误差模型可以表示为：
Figure FDA0002743172350000041
Figure FDA0002743172350000042
Figure FDA0002743172350000043
其中，
Figure FDA0002743172350000044
表示捷联惯导的姿态角误差，
Figure FDA0002743172350000045
表示捷联惯导的速度误差，δp＝[δx δy δz]T表示捷联惯导的位置误差，
Figure FDA0002743172350000046
为陀螺仪测量误差，
Figure FDA0002743172350000047
为加速度计测量误差，
Figure FDA0002743172350000048
为载体系中的加速度计测量值。
5.按照权利要求1所述的基于零速修正与姿态自观测的惯性行人导航算法，其特征处在于所述卡尔曼滤波器设计，根据误差模型，选取上述姿态角、位置以及速度误差作为卡尔曼滤波器的状态变量，建立连续系统状态方程：
Figure FDA0002743172350000049
其中，
Figure FDA00027431723500000410
Figure FDA00027431723500000411
Figure FDA00027431723500000412
Figure FDA00027431723500000413
系统观测方程为：
z(t)＝Hx(t)+v(t) (13)
对上述连续系统进行离散化可得：
Xk＝Φk/k-1Xk-1+Γk/k-1Wk-1 (14)
Zk＝HXk+Vk (15)
其中，
Figure FDA0002743172350000051
Figure FDA0002743172350000052
6.按照权利要求1所述的基于零速修正与姿态自观测的惯性行人导航算法，其特征处在于所述零速修正算法包括以下步骤：当行人处于零速区间时，脚部的理论速度应该等于零，但由于存在测量误差等因素，导致实际计算得到的速度并不等于零，因此，将零速时刻解算得到的速度作为系统速度误差的观测量，零速修正方法的观测方程为：
Zvk＝δVk n＝Vk n-0＝HvXk+Vvk (16)
其中，
Figure FDA0002743172350000053
Hv＝[03×3 03×3 I3×3]。
7.按照权利要求1所述的基于零速修正与姿态自观测的惯性行人导航算法，其特征处在于所述姿态自观测算法包括以下步骤：当行人处于零速区间时，由于脚部与地面保持静止，因此惯性传感器只受到重力加速度的作用，则下列等式在零速区间成立：
Figure FDA0002743172350000061
此时可以通过三轴加速度计输出确定出惯性传感器的俯仰角和滚转角：
Figure FDA0002743172350000062
此时，可以利用惯导解算出的俯仰角θINS和滚转角γINS与通过加速度确定的俯仰角θACC和滚转角γACC作差，构造俯仰角和滚转角误差观测量，但由于无法通过加速度计算航向角，基于上述条件无法构造航向角误差观测量，因此无法通过卡尔曼滤波修正航向角误差。而通过进一步分析零速区间可以发现，脚部与地面处于相对静止时，同一零速区间内的航向角也应具有不变的特性，因此可以将当前采样点航向角解算值与前两个采样点航向角平均值之差作为航向角误差的观测值
Figure FDA0002743172350000063
由此，可以得到姿态自观测方法的观测方程为：
Figure FDA0002743172350000064
其中，
Figure FDA0002743172350000065
将零速修正算法与姿态自观测算法的观测方程进行组合，得到系统观测方程为：
Figure FDA0002743172350000066
其中，
Figure FDA0002743172350000071
根据上述零速修正与姿态自观测算法，通过卡尔曼滤波就可以得到系统误差的估计值
Figure FDA0002743172350000072
从而实现对捷联惯导解算参数的误差修正，
Figure FDA0002743172350000073
CN202011157273.9A 2020-10-26 2020-10-26 基于零速修正与姿态自观测的惯性行人导航算法 Active CN112362057B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202011157273.9A CN112362057B (zh) 2020-10-26 2020-10-26 基于零速修正与姿态自观测的惯性行人导航算法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202011157273.9A CN112362057B (zh) 2020-10-26 2020-10-26 基于零速修正与姿态自观测的惯性行人导航算法

Publications (2)

Publication Number Publication Date
CN112362057A true CN112362057A (zh) 2021-02-12
CN112362057B CN112362057B (zh) 2023-05-02

Family

ID=74510468

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202011157273.9A Active CN112362057B (zh) 2020-10-26 2020-10-26 基于零速修正与姿态自观测的惯性行人导航算法

Country Status (1)

Country Link
CN (1) CN112362057B (zh)

Cited By (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN113008230A (zh) * 2021-02-26 2021-06-22 广州偶游网络科技有限公司 智能穿戴设备及其姿态朝向识别方法、装置
CN113092819A (zh) * 2021-04-14 2021-07-09 东方红卫星移动通信有限公司 足部加速度计动态零速校准方法及系统
CN114509071A (zh) * 2022-04-20 2022-05-17 中国空气动力研究与发展中心低速空气动力研究所 一种风洞试验模型姿态测量方法
CN115597602A (zh) * 2022-10-20 2023-01-13 Oppo广东移动通信有限公司（Cn） 机器人导航定位方法及装置、可读介质和电子设备
CN116576851A (zh) * 2022-12-26 2023-08-11 中国人民解放军陆军装甲兵学院 一种基于支持向量机算法的行人足部零速判定方法

Citations (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US20140094717A1 (en) * 2010-08-06 2014-04-03 Richard R. Wilson Gait analysis system and methods
CN106482733A (zh) * 2016-09-23 2017-03-08 南昌大学 行人导航中基于足底压力检测的零速修正方法
CN107655476A (zh) * 2017-08-21 2018-02-02 南京航空航天大学 基于多信息融合补偿的行人高精度足部导航算法
CN108426574A (zh) * 2018-02-02 2018-08-21 哈尔滨工程大学 一种基于zihr的航向角修正算法的mems行人导航方法
CN111024070A (zh) * 2019-12-23 2020-04-17 哈尔滨工程大学 一种基于航向自观测的惯性足绑式行人定位方法

Patent Citations (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US20140094717A1 (en) * 2010-08-06 2014-04-03 Richard R. Wilson Gait analysis system and methods
CN106482733A (zh) * 2016-09-23 2017-03-08 南昌大学 行人导航中基于足底压力检测的零速修正方法
CN107655476A (zh) * 2017-08-21 2018-02-02 南京航空航天大学 基于多信息融合补偿的行人高精度足部导航算法
CN108426574A (zh) * 2018-02-02 2018-08-21 哈尔滨工程大学 一种基于zihr的航向角修正算法的mems行人导航方法
CN111024070A (zh) * 2019-12-23 2020-04-17 哈尔滨工程大学 一种基于航向自观测的惯性足绑式行人定位方法

Cited By (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN113008230A (zh) * 2021-02-26 2021-06-22 广州偶游网络科技有限公司 智能穿戴设备及其姿态朝向识别方法、装置
CN113008230B (zh) * 2021-02-26 2024-04-02 广州市偶家科技有限公司 智能穿戴设备及其姿态朝向识别方法、装置
CN113092819A (zh) * 2021-04-14 2021-07-09 东方红卫星移动通信有限公司 足部加速度计动态零速校准方法及系统
CN114509071A (zh) * 2022-04-20 2022-05-17 中国空气动力研究与发展中心低速空气动力研究所 一种风洞试验模型姿态测量方法
CN115597602A (zh) * 2022-10-20 2023-01-13 Oppo广东移动通信有限公司（Cn） 机器人导航定位方法及装置、可读介质和电子设备
CN116576851A (zh) * 2022-12-26 2023-08-11 中国人民解放军陆军装甲兵学院 一种基于支持向量机算法的行人足部零速判定方法

Also Published As

Publication number Publication date
CN112362057B (zh) 2023-05-02

Similar Documents

Publication Publication Date Title
CN112362057A (zh) 基于零速修正与姿态自观测的惯性行人导航算法
CN109827577B (zh) 基于运动状态检测的高精度惯性导航定位算法
Tong et al. A double-step unscented Kalman filter and HMM-based zero-velocity update for pedestrian dead reckoning using MEMS sensors
CN106705968B (zh) 基于姿态识别和步长模型的室内惯性导航算法
Jimenez et al. A comparison of pedestrian dead-reckoning algorithms using a low-cost MEMS IMU
CN104061934B (zh) 基于惯性传感器的行人室内位置跟踪方法
CN104180805B (zh) 基于智能手机的室内行人定位与跟踪方法
CN110398245B (zh) 基于脚戴式惯性测量单元的室内行人导航姿态估计方法
US10267646B2 (en) Method and system for varying step length estimation using nonlinear system identification
Shi et al. A robust pedestrian dead reckoning system using low-cost magnetic and inertial sensors
CN103776446B (zh) 一种基于双mems-imu的行人自主导航解算算法
CN106996780B (zh) 一种航向误差修正方法与装置及磁场检测方法与装置
CN109099913B (zh) 一种基于mems惯性器件的穿戴式导航装置和方法
CN108957510B (zh) 基于惯性/零速/gps的行人无缝组合导航定位方法
CN104406586A (zh) 基于惯性传感器的行人导航装置和方法
Chen et al. IMU/GPS based pedestrian localization
US9677888B2 (en) Determining sensor orientation in indoor navigation
CN106153069B (zh) 自主导航系统中的姿态修正装置和方法
CN104613965B (zh) 一种基于双向滤波平滑技术的步进式行人导航方法
CN109459028A (zh) 一种基于梯度下降的自适应步长估计方法
CN108592907A (zh) 一种基于双向滤波平滑技术的准实时步进式行人导航方法
CN110672095A (zh) 一种基于微惯导的行人室内自主定位算法
CN115767412A (zh) 融合超宽带和惯性测量单元的室内定位方法
Tao et al. Precise displacement estimation from time-differenced carrier phase to improve PDR performance
CN106643713B (zh) 对阈值平滑自适应调整的零速修正步行者轨迹的推算方法及装置

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