CN110260860B - 基于足部惯性传感器的室内移动测量定位定姿方法及系统 - Google Patents

基于足部惯性传感器的室内移动测量定位定姿方法及系统 Download PDF

Info

Publication number
CN110260860B
CN110260860B CN201910536631.8A CN201910536631A CN110260860B CN 110260860 B CN110260860 B CN 110260860B CN 201910536631 A CN201910536631 A CN 201910536631A CN 110260860 B CN110260860 B CN 110260860B
Authority
CN
China
Prior art keywords
inertial sensor
inertial
speed
information
foot
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.)
Active
Application number
CN201910536631.8A
Other languages
English (en)
Other versions
CN110260860A (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.)
Wuhan University WHU
Original Assignee
Wuhan University WHU
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 Wuhan University WHU filed Critical Wuhan University WHU
Priority to CN201910536631.8A priority Critical patent/CN110260860B/zh
Publication of CN110260860A publication Critical patent/CN110260860A/zh
Application granted granted Critical
Publication of CN110260860B publication Critical patent/CN110260860B/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
    • 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
    • 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
    • Y02D—CLIMATE CHANGE MITIGATION TECHNOLOGIES IN INFORMATION AND COMMUNICATION TECHNOLOGIES [ICT], I.E. INFORMATION AND COMMUNICATION TECHNOLOGIES AIMING AT THE REDUCTION OF THEIR OWN ENERGY USE
    • Y02D30/00—Reducing energy consumption in communication networks
    • Y02D30/70—Reducing energy consumption in communication networks in wireless communication networks

Abstract

本发明提供一种基于足部惯性传感器的室内移动测量定位定姿方法及系统，包括将第一个惯性传感器安装在作业人员足部脚后跟位置，将第二个惯性传感器安装在作业人员的背部位置，并时间同步；解算第一个惯性传感器的位置、速度和姿态角信息；根据行人在直立行走过程中行人背部和足部在静止时段中间时刻相对位置不变的条件，提取出第一个惯性传感器在静止时段中间时刻的位置信息；结合第二个惯性传感器采集的惯性传感器数据，构建行人移动测量定位定姿系统的数学模型，解算得到行人背部的位置、速度和姿态角信息，为室内行人移动测量系统提供精确的位置和姿态角信息。本发明实现了高精度、高效率、较低成本、操作简单的室内移动测量定位定姿。

Description

基于足部惯性传感器的室内移动测量定位定姿方法及系统
技术领域
本发明涉及室内移动测量领域，具体涉及一种基于足部安装惯性传感器的室内行人移动测量定位定姿技术方案。
背景技术
移动测量系统(MobileMappingSystem，MMS)是在移动载体上安装导航定位系统、视频系统、惯性导航系统等多种传感器和设备，快速高效地获取道路两侧建筑物的空间位置数据和属性数据，可广泛应用于军事、勘测、道路管理、三维重建等领域。在MMS系统中，移动载体的位置和姿态精度是衡量整个系统精度和可靠性的重要指标。
室外环境中，由全球卫星导航系统(GlobalNavigationSatelliteSystem，GNSS)、里程计和惯性导航系统(InertialNavigationSystem，INS)构成组合导航系统能为MMS系统提供高精度的位置和姿态信息。室内环境中，由于无法接收到GNSS信号，利用GNSS及其相关的技术无法提供高精度的位置信息。在目前室内移动测量中，MMS系统主要利用脉冲超宽带、激光和超声波等无线定位技术进行测距和定位，再与INS进行组合导航解算得到更精确可靠的位置和姿态信息。该方法定位精度高、可靠性强，但受非视距遮挡影响明显且需要提前量测基准站坐标，在移动测量的过程中需要不断地变换基准站的位置和量测基准站的坐标，难以进行跨楼层测量，硬件系统成本较高，实际操作过程复杂，作业效率缓慢。
发明内容
本发明旨在解决或者至少部分解决现有室内移动测量定位定姿中存在的上述技术问题。
为此，本发明的目的在于提出一种基于足部安装惯性传感器的室内行人移动测量定位定姿技术方案，从而满足在室内环境下进行高效快递、操作简单、精确可靠的移动测量。
为了实现上述目的，本发明提供了一种基于足部惯性传感器的室内移动测量定位定姿方法，包括以下步骤：
步骤S1，将第一个惯性传感器安装在作业人员足部脚后跟位置，将第二个惯性传感器安装在作业人员的背部位置，利用同步设备完成两个惯性传感器的时间同步；
步骤S2，在数据采集开始阶段，初始化两个惯性传感器的导航状态，然后在数据采集过程中，两个惯性传感器各自利用加速度计实时采集加速度信息，且各自利用陀螺仪实时采集角速率信息，并将采集的加速度和角速率信息实时保存；
步骤S3，根据第一个惯性传感器所采集的惯性传感器数据，利用捷联惯性导航算法推算得到第一个惯性传感器的位置、速度和姿态角信息，并利用零速修正算法、稀疏的控制点校正算法和RTS平滑算法抑制捷联惯性导航算法的误差发散，得到第一个惯性传感器更加精确可靠的位置、速度和姿态角信息；
步骤S4，根据行人在直立行走过程中行人背部和足部在静止时段中间时刻相对位置不变的条件，提取出第一个惯性传感器在静止时段中间时刻的位置信息；
步骤S5，根据第二个惯性传感器采集的惯性传感器数据，根据所提取的第一个惯性传感器的位置信息构建行人移动测量定位定姿系统的数学模型，利用卡尔曼滤波和RTS平滑算法解算得到行人背部的位置、速度和姿态角信息，为室内行人移动测量系统提供精确的位置和姿态角信息。
而且，步骤S1中，所述同步设备是具有进行时间同步或时间分配或时间分配控制功能的设备，两个惯性传感器均各自内置有时钟设备，利用同步设备保证两个惯性传感器采集的数据在时间上是同步对齐的
而且，步骤S2中，初始化两个惯性传感器的导航状态的实现方式包括以下子步骤，
步骤S21，以第二个惯性传感器中心作为坐标原点，以第二个惯性传感器的正前方向、正右方向和高度3个方向，测量第一个惯性传感器相对第二个惯性传感器的相对位置，将相对位置记为第一个惯性传感器相对于第二个惯性传感器的杆臂信息；
步骤S22：确定两个惯性传感器的初始位置、初始速度和初始姿态角。
而且，步骤S3实现过程如下，
步骤S31，利用第一个惯性传感器采集的角速率和加速度信息，通过捷联惯性导航算法推算得到下一时刻第一个惯性传感器的位置、速度和姿态角信息，利用零速修正算法和稀疏的控制点修正算法抑制捷联惯性导航算法的误差发散；
步骤S32，利用RTS平滑算法对所述步骤S31得到的位置、速度和姿态角信息进行平滑处理，得到第一个惯性传感器更高精度的位置、速度和姿态角信息。
而且，步骤S31实现方式包括以下子步骤，
步骤S311，根据移动测量区域内预先布置稀疏的控制点，判断当前解算时刻第一个惯性传感器是否处于预习布置的控制点处，如果是则利用捷联惯性导航算法推算位置和控制点位置的差值作为卡尔曼滤波器的量测输入信息，进入步骤S32通过控制点修正模型对捷联惯性导航算法解算的结果进行修正，否则进入步骤S312；
步骤S312，利用第一个惯性传感器所采集的角速率和加速度信息判断当前解算时刻是否为零速时刻，如果第一个惯性传感器在当前解算时刻处于零速时刻且不处于事先布置的控制点处，则根据零速时刻理论速度为零且理论角速率为零的条件，利用捷联惯性导航算法推算速度和理论零速的差值，以及角速率输出值和理论零角速率的差值作为卡尔曼滤波器的量测输入信息，通过零速修正模型和零角速率修正模型]对捷联惯性导航算法解算的结果进行进一步的修正，然后进入步骤6。
而且，步骤S5实现方式如下，
利用第二个惯性传感器输出的加速度信息和角速率信息、步骤S4所提取的第一个惯性传感器的位置信息、第一个惯性传感器相对于第二个惯性传感器的杆臂信息构建行人移动测量定位定姿系统的松组合模型，利用卡尔曼滤波进行数据融合并估计行人背部处的每个时刻的位置、速度和姿态角信息；为了进一步提高导航状态信息估计的精度，利用RTS平滑算法解算得到行人背部处第二个惯性传感器更高精度的位置、速度和姿态角信息。
本发明还提供一种基于足部惯性传感器的室内移动测量定位定姿系统，用于执行如上所述的基于足部惯性传感器的室内移动测量定位定姿方法。
本发明采用以上技术方案与现有技术相比，具有以下技术效果：
本发明所提供的方法中，将第一个惯性传感器安装在足部脚后跟处，利用行人行走过程中脚触地时刻的速度和角速率为零的信息，利用零速修正算法和零角速率修正算法抑制惯性导航解算中位置速度等误差随时间的不断累积；利用事先布置的稀疏的高精度控制点的位置进一步修正惯性导航的累积误差；利用RTS平滑算法在卡尔曼滤波器的基础上进一步提高导航状态信息估计的精度；接下来，利用行人室内移动测量过程中行人背部和足部在空间相对位置不变时刻的位置信息、第一个惯性传感器相对于第二个惯性传感器的杆臂信息构建行人移动测量定位定姿系统的数学模型，有效地解决了行人背部处惯性导航解算结果的位置误差、速度误差和姿态角误差随时间不断累积的问题。本发明能很好地解决现有室内移动测量定位定姿中存在的成本高昂、易受环境影响、实际操作复杂、作业效率缓慢的技术问题。
附图说明
图1是本发明实施例中一种基于足部惯性传感器的室内行人移动测量定位定姿系统的总体流程图。
图2是本发明实施例中一种基于足部惯性传感器的室内行人移动测量定位定姿系统的安装示意图。
具体实施方式
为使本发明实施例的目的、技术方案和优点更加清楚，下面结合优选实施例和附图对本发明的技术方案做进一步的详细说明。下面描述中的附图仅仅是本发明实施例中的一些特定实施方式，来表示实施本发明实施例的原理的方式。本领域技术人员应当理解，下面所具体描述的内容是说明性的而非限制性的，可以根据这些描述获得其他的实施方式，不应以此限制本发明的保护范围。
如图1所示，本发明的一个实施例提供了一种基于足部安装惯性传感器的室内行人移动测量定位定姿方法，包括以下步骤：
步骤S1：将第一个惯性传感器安装在作业人员足部脚后跟位置，将第二个惯性传感器安装在作业人员的背部位置，利用同步设备完成两个惯性传感器的时间同步。
具体地，所述步骤S1中的利用同步设备完成两个惯性传感器的时间同步，同步设备是具有进行时间同步或时间分配或时间分配控制功能的设备，两个惯性传感器均各自内置有时钟设备，利用同步设备保证两个惯性传感器采集的数据在时间上是同步对齐的。
具体来说，基于足部安装惯性传感器的室内行人移动测量定位定姿系统的安装示意图如图2所示，包括第一个惯性传感器1、第二个惯性传感器2和一个同步设备，其中，第一个惯性传感器1安装在作业人员一只脚的足部脚后跟位置，第二个惯性传感器2安装于作业人员背部位置。在具体实施过程中，第一个惯性传感器1的安装位置在足部脚后跟位置，具体实施时可以为足部脚后跟外侧位置、足部脚后跟内侧位置或足部脚后跟处脚底位置，这几个位置的数据处理过程完全相同、导航解算结果的性能也几乎一致，因此实际应用时可视需要设置；第二个惯性传感器2安装的位置包括作业人员背部区域的任意位置。两个惯性传感器1和2安装过程中需要保证各惯性传感器与各自安装位置处的刚性连接。
在室内行人移动测量定位定姿系统中，需要通过同步设备保证两个惯性传感器采集的数据在时间上是同步对齐的，同步设备不作安装要求，可以根据具体的场景和仪器选择安装、固定或者放在作业人员身上。在具体的实施过程中，为了便携性和可操作性，本发明实施例中利用手机作为两个惯性传感器的时间控制，利用两个惯性传感器内置的蓝牙模块同时连接手机，手机通过开始按钮同时控制两个惯性传感器开始数据采集，手机通过停止按钮同时控制两个惯性传感器停止数据采集。
步骤S2：在数据采集开始阶段，初始化两个惯性传感器的导航状态，然后在数据采集过程中，两个惯性传感器各自利用加速度计实时采集加速度信息，且各自利用陀螺仪实时采集角速率信息，并将采集的加速度和角速率信息实时保存在各自内置的SD存储卡之中。
具体地，所述步骤S2中初始化两个惯性传感器的导航状态具体包括以下子步骤：
步骤S21：以第二个惯性传感器中心作为坐标原点，以第二个惯性传感器的正前方向、正右方向和高度3个方向，测量第一个惯性传感器相对第二个惯性传感器的相对位置，将该相对位置记为第一个惯性传感器相对于第二个惯性传感器的杆臂信息。
具体来说，在数据采集开始阶段，需要测量第一个惯性传感器1相对第二个惯性传感器2的杆臂信息，为了保证杆臂信息测量的准确性和可靠性，可以利用全站仪等高精度设备进行测量，在测量的过程中作业人员需要保持直立且静止不动。为了保证两个惯性传感器在行人在相邻两个静止阶段的中间时刻的相对位置不变，在数据采集过程中作业人员需要保证直立行走，不做匍匐、弯腰、爬下等非直立动作。
步骤S22：初始化两个惯性传感器的初始位置、初始速度和初始姿态角。
具体来说，在数据采集开始阶段，需要确定第一个惯性传感器和第二个惯性传感器在初始时刻的位置、速度和姿态角信息，其中两个惯性传感器的初始速度和初始位置很容易确定。为了获得准确可靠的初始姿态角信息，作业人员在数据采集开始阶段需要保证静止一段时间，该时间段的长度设置至少为3分钟。
两个惯性传感器内部均包括加速度计模块、陀螺仪模块、SD存储卡、时钟模块、蓝牙模块、可充电电源等传感器，本发明实施例中，作业人员的移动测量的过程中只进行惯性传感器数据采集的工作，在完成移动测量数据采集之后在计算机上进行离线的数据解算。
步骤S3：根据第一个惯性传感器所采集的惯性传感器数据，利用捷联惯性导航算法推算得到第一个惯性传感器的位置、速度和姿态角信息，并利用零速修正算法、稀疏的控制点校正算法和RTS平滑算法抑制捷联惯性导航算法的误差发散，从而得到第一个惯性传感器更加精确可靠的位置、速度和姿态角信息。
具体地，所述步骤S3中使用的捷联惯性导航算法、零速修正算法、稀疏的控制点修正算法和RTS平滑算法为现有技术，本发明不予赘述。RTS(Rauch-Tung-Striebel)算法是一种固定时间间隔平滑算法。
步骤S3具体实现过程包括：
步骤S31：利用第一个惯性传感器采集的角速率和加速度信息，通过捷联惯性导航算法推算得到下一时刻第一个惯性传感器的位置、速度和姿态角信息。惯性传感器中陀螺仪和加速度计存在噪声，捷联惯性导航算法的误差会随着时间不断的累积并发散，因此利用零速修正算法和稀疏的控制点修正算法抑制捷联惯性导航算法的误差发散，具体包括：
步骤S311：根据移动测量区域内预先布置稀疏的控制点，判断当前解算时刻第一个惯性传感器是否处于预习布置的控制点处，如果是则利用捷联惯性导航算法推算位置和控制点位置的差值作为卡尔曼滤波器的量测输入信息，进入步骤S32，通过控制点修正模型对捷联惯性导航算法解算的结果进行修正，否则进入步骤S312。
具体来说，事先在需要进行移动测量的室内区域内布置稀疏的少量的控制点，作业人员在移动测量的数据采集过程中，第一个惯性传感器所处的脚部需要经过设定的控制点。如果当前解算时刻第一个惯性传感器处于事先布置的控制点位置，则利用控制点的高精度的位置信息作为约束信息修正捷联惯性导航算法的误差累积，从而得到更高精度的位置、速度和姿态角信息。
步骤S312：利用第一个惯性传感器所采集的角速率和加速度信息判断当前解算时刻是否为零速时刻。如果第一个惯性传感器在当前解算时刻处于零速时刻且不处于事先布置的控制点处，则根据零速时刻理论速度为零且理论角速率为零的条件，利用捷联惯性导航算法推算速度和理论零速的差值，以及角速率输出值和理论零角速率的差值作为卡尔曼滤波器的量测输入信息，通过零速修正模型[1]和零角速率修正模型[2]对捷联惯性导航算法解算的结果进行进一步的修正。然后进入步骤S32。
[1]E.Foxlin.Pedestrian tracking with shoe-mounted inertial sensors,IEEE Computer graphics and applications,2005,no.6,pp.38-46.
[2]A.R.Jiménez,F.Seco,J.C.Prieto,J.Guevara,Indoor pedestriannavigation using an INS/EKF framework for yaw drift reduction and a foot-mounted IMU,in:2010 7th Workshop on Positioning,Navigation and Communication,2010,pp.135-143.
步骤S32：为了进一步提高卡尔曼滤波器估计的精度和可靠性，利用RTS平滑算法对所述步骤S31得到的位置、速度和姿态角信息进行平滑处理，从而得到第一个惯性传感器更高精度的位置、速度和姿态角信息。
具体来说，所述步骤S31中会存储每一个解算时刻的状态转移矩阵、预测的状态向量、预测的状态向量协方差矩阵、估计的状态向量和估计的状态向量协方差矩阵，在步骤S31对第一个惯性传感器所采集的数据进行数据解算完成之后进行整体区间RTS平滑得到每一个解算时刻第一个惯性传感器更加平滑稳定的位置、速度和姿态角信息。
步骤S4：根据行人在直立行走过程中行人背部和足部在静止时段中间时刻相对位置不变的条件，提取出第一个惯性传感器在静止时段中间时刻的位置信息。
具体来说，行人行走过程中脚步的运动可以看作单次脚步运动状态的周期性的循环运动，当行人脚部接触地面时速度为零，行人脚步速度为零的过程是一段连续为零的时段而不是几个单独的时刻；在行人脚步速度为零的时间段中，行人上身是从后往前的移动过程。当行人完全直立行走时，在零速时段的中间时刻行人背部移动到身体的平衡状态，此时背部和足部的相对位置与静止不动时的相对位置完全相同，行人行走的过程中每一次脚步运动周期内都存在背部与足部的相对位置不发生改变的时刻。将此时第一个惯性传感器的位置提取出来将能够用于修正行人背部的捷联惯性导航解算结果。
步骤S5：根据第二个惯性传感器采集的惯性传感器数据，根据所提取的第一个惯性传感器的位置信息构建行人移动测量定位定姿系统的数学模型，利用卡尔曼滤波和RTS平滑算法解算得到行人背部的位置、速度和姿态角信息，从而为室内行人移动测量系统提供精确的位置和姿态角信息。
具体来说，利用惯性导航系统中的位置误差方程、速度误差方程、姿态角误差方程、陀螺和加速度计的零偏模型构建行人移动测量定位定姿系统的状态模型。依据步骤S4所提取的第一个惯性传感器的位置信息和第一个惯性传感器相对于第二个惯性传感器的杆臂信息，构建行人移动测量定位定姿系统的量测模型。
行人移动测量定位定姿系统的状态模型如下所示：
Figure BDA0002101361710000081
其中，X(t)＝[δrn δvn ψ bg ba]T为系统的状态误差向量，
Figure BDA0002101361710000082
为X(t)的微分，(t)表示和时间有关的量，δrn为北方向、东方向和高度方向的位置误差向量，δvn为北方向、东方向和高度方向的速度误差向量，ψ为姿态角误差向量，bg和ba分别为陀螺零偏和加速度计零偏；F(t)为系统的状态转移矩阵，G(t)为系统的噪声投影矩阵，W(t)＝[wg wa]T为系统的过程噪声向量且假设为零均值的高斯白噪声，wg和wa分别为陀螺噪声和加速度计噪声，n为导航坐标系，设W(t)对应的协方差矩阵为Qk，k用于标识时刻，AT为向量或者矩阵A的转置。系统的状态转移矩阵和噪声投影矩阵如下所示：
Figure BDA0002101361710000083
其中，
Figure BDA0002101361710000084
为导航坐标系相对于地心地固坐标系的旋转角速度，/>
Figure BDA0002101361710000085
为地球自转角速度在导航坐标系下的投影，/>
Figure BDA0002101361710000086
为导航坐标系相对于惯性坐标系的旋转角速度，/>
Figure BDA0002101361710000087
为姿态旋转矩阵，/>
Figure BDA0002101361710000088
为比力向量在导航坐标系下的投影，fb为加速度计输出的比力向量，Γg＝τgI为陀螺相关时间τg构成的对角矩阵，Γa＝τaI为加速度计相关时间τa构成的对角矩阵，(A×)代表向量A反对称矩阵，0为3维的零矩阵，I为3维的单位矩阵。
在卡尔曼滤波进行数据融合中，需要对系统连续的状态模型式(1)进行离散化得到离散的状态模型，离散化后的状态模型如下所示：
Xk+1＝Φk+1,kXk+wk (3)
其中，设Xk为tk时刻的状态向量，Xk+1为tk+1时刻的状态向量，Φk+1,k和wk分别为离散化后的状态转移矩阵和噪声向量，wk假设为零均值的高斯白噪声且其对应的协方差矩阵为qk，具体公式如下所示：
Figure BDA0002101361710000091
其中，Δt为惯性传感器数据的采样间隔，exp(·)为自然常数e为底的指数函数，F(tk)为tk时刻的状态转移矩阵，G(tk)为tk时刻的系统噪声投影矩阵，tk为时间。
行人移动测量定位定姿系统的量测模型如下所示：
Zk＝HkXk+Vk (5)
其中，
Figure BDA0002101361710000092
为系统的量测向量，/>
Figure BDA0002101361710000093
为背部处通过捷联惯性导航算法推算的位置信息，/>
Figure BDA0002101361710000094
为提取的足部在静止时段中间时刻的位置信息，Lb为第一个惯性传感器相对于第二个惯性传感器的杆臂信息，/>
Figure BDA0002101361710000095
为系统的量测矩阵，Vk为系统噪声向量且假设为零均值的高斯白噪声且其对应的协方差矩阵为Rk。
在建立了行人移动测量定位定姿系统的状态模型和量测模型之后，就可以利用卡尔曼滤波进行预测更新得到预测的状态向量和预测状态向量的协方差矩阵；在行人背部与足部相对位置不变的时刻，利用卡尔曼滤波进行量测更新得到估计的位置误差、速度误差和姿态角误差，从而对当前时刻估计的位置、速度和姿态角信息进行误差修正，获得第二个惯性传感器的位置、速度和姿态角信息。
在卡尔曼滤波过程中，存储每一时刻的状态转移矩阵、预测的状态向量、预测的状态向量协方差矩阵、估计的状态向量和估计的状态向量协方差矩阵；在完成整个时间段的卡尔曼滤波之后，进行整体区间RTS平滑得到平滑之后的每一时刻的位置、速度和姿态角信息；从而获取第二个惯性传感器的精确可靠的位置、速度和姿态角信息，为室内行人移动测量系统提供精确的位置和姿态角信息。
惯性导航系统是相对递推的解算过程，受到陀螺仪和加速度计的元件误差的影响，惯性导航解算的误差随时间不断累积。在行人的行走过程中，采用零速修正算法和稀疏的控制点校正算法能有效地抑制惯性导航解算误差的发散，极大减缓误差累积的速度，从而获得行人足部脚后跟处高精度的可靠的位置信息。根据行人直立行走过程中背部和足部的相对位置在静止时段中间时刻的相对位置不变的条件，通过第一个惯性传感器相对于第二个惯性传感器的杆臂信息将足部脚后跟处的位置信息传递至行人背部，将行人足部脚后跟的位置信息作为约束信息抑制行人背部处捷联惯性导航解算的误差随时间累积。利用RTS平滑算法进一步估计和平滑行人背部处的位置、速度和姿态角信息，从而保证行人室内移动测量过程中位置和姿态角信息的精度和可靠性。
具体实施时，可采用软件技术实现以上流程的自动运行。相应系统也应当在本发明的保护范围内。
本发明实施例的基于足部安装惯性传感器的室内行人移动测量定位定姿系统中，将第一个惯性传感器安装在作业人员足部脚后跟位置，将第二个惯性传感器安装在作业人员的背部位置，利用同步设备完成两个惯性传感器的时间同步；工作过程参见以上流程：在室内移动测量数据采集的开始阶段，测量第一个惯性传感器相对于第二个惯性传感器的相对位置，并初始化两个惯性传感器的导航状态信息；然后进行室内移动测量的数据采集，通过SD存储卡实时保存惯性传感器测量的加速度、角速率等传感器数据；之后对第一个惯性传感器所采集的数据进行数据处理，利用纯惯性导航算法、零速修正算法、稀疏的控制点校正算法和RTS平滑算法进行解算得到第一个惯性传感器的位置、速度和姿态角信息；再根据行人在直立行走过程中行人背部和足部在静止时段中间时刻的相对位置不变的条件，提取出第一个惯性传感器在静止时段中间时刻的位置信息；最后利用所提取的第一个惯性传感器的位置信息和第一个惯性传感器相对于第二个惯性传感器的杆臂信息构建行人移动测量定位定姿系统的数学模型，利用卡尔曼滤波和RTS平滑算法得到行人背部处的位置、速度和姿态角信息，为室内移动测量系统提供精确可靠的位置和姿态角信息。在本发明实施例中，利用行人作为移动载体，不需要安装基准站，不存在上下楼等实际环境限制，且不受外界环境干扰。解决了现有室内移动测量定位定姿中存在的成本高昂、易受环境影响、实际操作复杂、作业效率缓慢的技术问题。
本技术领域技术人员可以理解的是，除非另外定义，这里使用的所有术语(包括技术术语和科学术语)具有与本发明所属领域中的普通技术人员的一般理解相同的意义。还应该理解的是，诸如通用字典中定义的那些术语应该被理解为具有与现有技术的上下文中的意义一致的意义，并且除非像这里一样定义，不会用理想化或过于正式的含义来解释。
显然，本发明的上述实施例只是为清楚地说明本发明所作的举例，而并非是对本发明的实施方式的限定，对于本领域的普通技术人员来说，在上述说明的基础上还可以对本发明实施例进行各种改动和变型而不脱离本发明实施例的精神和范围。这样，倘若本发明实施例的这些修改和变型属于本发明权利要求及其等同技术的范围之内，则本发明也意图包含这些改动和变型在内。

Claims (7)

1.一种基于足部惯性传感器的室内移动测量定位定姿方法，其特征在于，包括以下步骤：
步骤S1，将第一个惯性传感器安装在作业人员足部脚后跟位置，将第二个惯性传感器安装在作业人员的背部位置，利用同步设备完成两个惯性传感器的时间同步；
步骤S2，在数据采集开始阶段，初始化两个惯性传感器的导航状态，然后在数据采集过程中，两个惯性传感器各自利用加速度计实时采集加速度信息，且各自利用陀螺仪实时采集角速率信息，并将采集的加速度和角速率信息实时保存；
步骤S3，根据第一个惯性传感器所采集的惯性传感器数据，利用捷联惯性导航算法推算得到第一个惯性传感器的位置、速度和姿态角信息，并利用零速修正算法、稀疏的控制点校正算法和RTS平滑算法抑制捷联惯性导航算法的误差发散，得到第一个惯性传感器更加精确可靠的位置、速度和姿态角信息；
步骤S4，根据行人在直立行走过程中行人背部和足部在静止时段中间时刻相对位置不变的条件，提取出第一个惯性传感器在静止时段中间时刻的位置信息；
步骤S5，根据第二个惯性传感器采集的惯性传感器数据，根据所提取的第一个惯性传感器的位置信息构建行人移动测量定位定姿系统的数学模型，包括根据步骤S4所提取的第一个惯性传感器的位置信息和第一个惯性传感器相对于第二个惯性传感器的杆臂信息，构建行人移动测量定位定姿系统的量测模型；利用卡尔曼滤波和RTS平滑算法解算得到行人背部的位置、速度和姿态角信息，为室内行人移动测量系统提供精确的位置和姿态角信息。
2.如权利要求1所述的基于足部惯性传感器的室内移动测量定位定姿方法，其特征在于：步骤S1中，所述同步设备是具有进行时间同步或时间分配或时间分配控制功能的设备，两个惯性传感器均各自内置有时钟设备，利用同步设备保证两个惯性传感器采集的数据在时间上是同步对齐的。
3.如权利要求1所述的基于足部惯性传感器的室内移动测量定位定姿方法，其特征在于：步骤S2中，初始化两个惯性传感器的导航状态的实现方式包括以下子步骤，
步骤S21，以第二个惯性传感器中心作为坐标原点，以第二个惯性传感器的正前方向、正右方向和高度3个方向，测量第一个惯性传感器相对第二个惯性传感器的相对位置，将相对位置记为第一个惯性传感器相对于第二个惯性传感器的杆臂信息；
步骤S22：确定两个惯性传感器的初始位置、初始速度和初始姿态角。
4.如权利要求1所述的基于足部惯性传感器的室内移动测量定位定姿方法，其特征在于：步骤S3实现过程如下，
步骤S31，利用第一个惯性传感器采集的角速率和加速度信息，通过捷联惯性导航算法推算得到下一时刻第一个惯性传感器的位置、速度和姿态角信息，利用零速修正算法和稀疏的控制点修正算法抑制捷联惯性导航算法的误差发散；
步骤S32，利用RTS平滑算法对所述步骤S31得到的位置、速度和姿态角信息进行平滑处理，得到第一个惯性传感器更高精度的位置、速度和姿态角信息。
5.如权利要求4所述的基于足部惯性传感器的室内移动测量定位定姿方法，其特征在于：步骤S31实现方式包括以下子步骤，
步骤S311，根据移动测量区域内预先布置稀疏的控制点，判断当前解算时刻第一个惯性传感器是否处于预先布置的控制点处，如果是则利用捷联惯性导航算法推算位置和控制点位置的差值作为卡尔曼滤波器的量测输入信息，进入步骤S32通过控制点修正模型对捷联惯性导航算法解算的结果进行修正，否则进入步骤S312；
步骤S312，利用第一个惯性传感器所采集的角速率和加速度信息判断当前解算时刻是否为零速时刻，如果第一个惯性传感器在当前解算时刻处于零速时刻且不处于事先布置的控制点处，则根据零速时刻理论速度为零且理论角速率为零的条件，利用捷联惯性导航算法推算速度和理论零速的差值，以及角速率输出值和理论零角速率的差值作为卡尔曼滤波器的量测输入信息，通过零速修正模型和零角速率修正模型对捷联惯性导航算法解算的结果进行进一步的修正，然后进入步骤S32。
6.如权利要求1或2或3或4或5所述的基于足部惯性传感器的室内移动测量定位定姿方法，其特征在于：步骤S5实现方式如下，
利用第二个惯性传感器输出的加速度信息和角速率信息、步骤S4所提取的第一个惯性传感器的位置信息、第一个惯性传感器相对于第二个惯性传感器的杆臂信息构建行人移动测量定位定姿系统的松组合模型，利用卡尔曼滤波进行数据融合并估计行人背部处的每个时刻的位置、速度和姿态角信息；为了进一步提高导航状态信息估计的精度，利用RTS平滑算法解算得到行人背部处第二个惯性传感器更高精度的位置、速度和姿态角信息。
7.一种基于足部惯性传感器的室内移动测量定位定姿系统，其特征在于：用于执行如权利要求1至6任一项所述的基于足部惯性传感器的室内移动测量定位定姿方法。
CN201910536631.8A 2019-06-20 2019-06-20 基于足部惯性传感器的室内移动测量定位定姿方法及系统 Active CN110260860B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN201910536631.8A CN110260860B (zh) 2019-06-20 2019-06-20 基于足部惯性传感器的室内移动测量定位定姿方法及系统

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201910536631.8A CN110260860B (zh) 2019-06-20 2019-06-20 基于足部惯性传感器的室内移动测量定位定姿方法及系统

Publications (2)

Publication Number Publication Date
CN110260860A CN110260860A (zh) 2019-09-20
CN110260860B true CN110260860B (zh) 2023-06-23

Family

ID=67919744

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201910536631.8A Active CN110260860B (zh) 2019-06-20 2019-06-20 基于足部惯性传感器的室内移动测量定位定姿方法及系统

Country Status (1)

Country Link
CN (1) CN110260860B (zh)

Families Citing this family (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN110763229B (zh) * 2019-11-12 2022-04-15 武汉大学 一种便携式惯性导航定位杆及其定位定姿方法
EP3851806B1 (de) 2020-01-15 2023-01-11 Leuze electronic GmbH + Co. KG Sensoranordnung und ein verfahren zum betrieb einer sensoranordnung
CN114088090A (zh) * 2021-12-03 2022-02-25 山东大学 一种足绑式行人足部零速度检测方法及系统

Citations (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
TW201224459A (en) * 2010-12-06 2012-06-16 Inst Information Industry Human motion identification and locating method
CN105115507A (zh) * 2015-08-10 2015-12-02 济南大学 一种基于双imu的双模式室内个人导航系统及方法
DE102017122465B3 (de) * 2017-09-27 2019-02-14 Deutsches Zentrum für Luft- und Raumfahrt e.V. Verfahren zur Fußgänger-Navigation

Patent Citations (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
TW201224459A (en) * 2010-12-06 2012-06-16 Inst Information Industry Human motion identification and locating method
CN105115507A (zh) * 2015-08-10 2015-12-02 济南大学 一种基于双imu的双模式室内个人导航系统及方法
DE102017122465B3 (de) * 2017-09-27 2019-02-14 Deutsches Zentrum für Luft- und Raumfahrt e.V. Verfahren zur Fußgänger-Navigation

Non-Patent Citations (4)

* Cited by examiner, † Cited by third party
Title
"Dual IMU Indoor Navigation with particle filter based map-matching on a smartphone";Ascher, C. 等，;《2010 International Conference on Indoor Positioning and Indoor Navigation (IPIN)》;20100917;1-5页 *
"Multi-sensor indoor pedestrian navigation system with vision aiding";Keßler, C. 等，;《Gyroscopy and Navigation》;20121231;第3卷(第2期);79-90页 *
"一种基于双IMU框架的室内个人导航方法";徐元 等，;《中国惯性技术学报》;20151215;第23卷(第6期);714-717页 *
"一种脚绑IMU辅助的行人室内定位定姿方法";旷俭 等，;《无线电工程》;20200831;第50卷(第8期);637-642页 *

Also Published As

Publication number Publication date
CN110260860A (zh) 2019-09-20

Similar Documents

Publication Publication Date Title
Zhuang et al. Tightly-coupled integration of WiFi and MEMS sensors on handheld devices for indoor pedestrian navigation
US10267646B2 (en) Method and system for varying step length estimation using nonlinear system identification
Bai et al. A high-precision and low-cost IMU-based indoor pedestrian positioning technique
US10337884B2 (en) Method and apparatus for fast magnetometer calibration
EP2946167B1 (en) Method and apparatus for determination of misalignment between device and pedestrian
CA2673795C (en) System and method for tracking a moving person
CN104713554A (zh) 一种基于mems惯性器件与安卓智能手机融合的室内定位方法
US11035915B2 (en) Method and system for magnetic fingerprinting
CN110260860B (zh) 基于足部惯性传感器的室内移动测量定位定姿方法及系统
Shi et al. A robust pedestrian dead reckoning system using low-cost magnetic and inertial sensors
CN104596504A (zh) 应急救援场景下快速构建地图辅助室内定位方法及系统
CN106705959A (zh) 检测移动终端航向的方法和装置
JP5742794B2 (ja) 慣性航法装置及びプログラム
Woyano et al. Evaluation and comparison of performance analysis of indoor inertial navigation system based on foot mounted IMU
Wu et al. Indoor positioning system based on inertial MEMS sensors: Design and realization
KR101523147B1 (ko) 실내 측위 장치 및 방법
CN104897155A (zh) 一种个人携行式多源定位信息辅助修正方法
JP2018155635A (ja) 位置推定装置、位置推定方法及びプログラム
JP7077598B2 (ja) 位置決定及び追跡のための方法、プログラム、及びシステム
US10914793B2 (en) Method and system for magnetometer calibration
CN116817905A (zh) 一种可穿戴多mems准实时协同导航系统及方法
Asano et al. A robust pedestrian dead-reckoning positioning based on pedestrian behavior and sensor validity
GB2592894A (en) Method and system for geotracking objects
Emilsson et al. Sensor fusion for improved indoor navigation
Hide et al. Low cost IMU, GPS and camera integration for handheld indoor positioning

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