CN109974696B - 基于slam与步态imu融合的室内人员自主定位方法 - Google Patents

基于slam与步态imu融合的室内人员自主定位方法 Download PDF

Info

Publication number
CN109974696B
CN109974696B CN201910378821.1A CN201910378821A CN109974696B CN 109974696 B CN109974696 B CN 109974696B CN 201910378821 A CN201910378821 A CN 201910378821A CN 109974696 B CN109974696 B CN 109974696B
Authority
CN
China
Prior art keywords
slam
imu
positioning
personnel
gait
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
CN201910378821.1A
Other languages
English (en)
Other versions
CN109974696A (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.)
Northeastern University China
Original Assignee
Northeastern University China
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 Northeastern University China filed Critical Northeastern University China
Priority to CN201910378821.1A priority Critical patent/CN109974696B/zh
Publication of CN109974696A publication Critical patent/CN109974696A/zh
Application granted granted Critical
Publication of CN109974696B publication Critical patent/CN109974696B/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/165—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 combined with non-inertial navigation instruments
    • 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

Landscapes

  • Engineering & Computer Science (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Automation & Control Theory (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
  • Navigation (AREA)

Abstract

本发明公开了一种基于SLAM与步态IMU融合的室内人员自主定位方法，包括室内人员穿戴定位装置并将其启动，穿戴装置的人员在室内环境中自由移动，SLAM传感器将测得的数据传输给计算机，计算机使用SLAM算法对室内环境进行建图并对人员自身定位，同时，IMU元件将测得的数据传输给计算机，计算机通过基于步态的IMU定位算法计算人员的运动轨迹，再将融合SLAM定位结果与步态IMU定位结果进行人员定位，以对环境依赖小的步态IMU定位技术作为挑战性环境下的辅助定位手段，将两者融合起来的人员自主定位***，不仅在正常环境中具备良好的导航定位性能，在火场等特殊室内环境中也具有较强的鲁棒性。本发明也可用于其他潜在的室内复杂环境下人员自主定位用途。

Description

基于SLAM与步态IMU融合的室内人员自主定位方法
技术领域
本发明属于SLAM技术与IMU技术领域，尤其涉及一种基于SLAM与步态IMU融合的室内人员自主定位方法。
背景技术
IMU是一类由三轴加速度计、三轴角速度计(陀螺仪)，有时还包括磁强计组成的电子设备，能够提供三轴加速度、角速度等数据，使通过对加速度及角速度进行积分计算出各向速度、位移及IMU姿态成为可能。IMU利用陀螺仪特有的力学性质(旋进效应)测量角速度，利用惯性原理测量加速度，具有不依赖外界环境的优点，既不像GPS***依赖来自卫星的GPS信号，只能在空旷地带工作，又不像视觉传感器对光照条件有较高要求，无法在黑暗处工作，也不像激光雷达会受到烟雾影响，因此非常适用于各种特殊、复杂环境中的定位用途。IMU元件的主要缺点是存在“漂移”现象，即累积误差。由于IMU提供定位信息的工作原理是对加速度进行积分得到速度，再对速度进行积分得到位移，同时对角速度进行积分得到姿态变化量，将位移与位姿变化累积到初始位姿上来计算当前位置和姿态，因此不可避免地容易出现累积误差,这将引起IMU测得的运动轨迹产生较明显的失真。本例中我们应用了步态IMU算法定位技术，使用固定在人员脚部的IMU模块，利用人类步态固有模式消除IMU累积误差来实现较精确的定位效果。另外单纯的IMU定位技术只能生成运动轨迹，而不能实时建立环境地图，在导航定位方面的参考价值有限。
SLAM是Simultaneous Localization and Mapping的缩写，中文译作“同步定位与建图”。它是指搭载传感器的主体，在没有环境先验信息的情况下，在运动过程中建立环境模型，同时估计自己的运动，从而实现实时定位的功能。基于激光或视觉SLAM技术的人员定位方案使用人员所穿戴的激光雷达或光学传感器，能对室内环境进行实时建图，并依据所建地图对人员定位。由于室内人员自主定位的一项重要用途为辅助消防人员等在火场等挑战性的室内环境中开展人员搜救等作业，因为火灾及其造成的塌方等可能已经对火场内环境造成了较大改变，这样建立的环境地图时效性更强，能更有效地辅助人员展开搜救。但这样的室内场景情况特殊，光照条件复杂，视觉SLAM技术中的单目和双目SLAM在黑暗的环境中无法使用；而RGB-D SLAM技术因为使用了红外线测距提供深度信息，因此在光照强烈的环境下无法使用；激光雷达具有受环境影响相对较小，精度较高等特点，在一般室内环境下能够很好地完成建图和定位，但是在有较浓的烟雾存在的情况下，烟雾颗粒会反射激光，产生虚假的距离数据，对激光雷达的工作效果存在较大影响。在这种情况下如果只使用激光雷达进行定位，无疑会极大影响定位结果的准确性。
发明内容
如上文背景技术部分所述，目前现有的室内人员自主定位技术均存在较明显的不足，在用于火场等挑战性环境下人员室内自主定位时效果并不理想。
针对现有技术的缺陷，本发明提供一种基于SLAM与步态IMU融合的室内人员自主定位方法，本发明所采用的技术方案为：
步骤1、在室内环境下的人员穿戴上定位装置并将其启动，所述定位装置包括设置有SLAM传感器的头盔、固定于足部的IMU元件以及计算机；所述SLAM传感器与IMU元件均能与计算机实现无线通讯；
步骤2、人员在室内环境中自由移动，SLAM传感器将测得的数据传输给计算机，计算机使用SLAM算法对室内环境进行建图并对人员自身定位；
同时，IMU元件将测得的数据传输给计算机，计算机通过基于步态的IMU定位算法计算人员的运动轨迹；
步骤2.1、以IMU元件启动时刻的位置为原点建立世界坐标系，以IMU元件为原点建立IMU坐标系，从IMU元件获取测得的IMU坐标系下三轴角速度gyrx,gyry,gyrz与x轴、y轴与z轴对应的加速度ax、ay与az；
步骤2.2、根据加速度幅值判断当前时刻IMU所处的人员的脚是动态阶段还是静止阶段，
其中，加速度幅值maga的计算公式为：
Figure GDA0003952347270000021
若maga＜magthresh，则脚部处于静止阶段；
若maga≥magthresh，则脚部处于动态阶段；
其中magthresh为预设的速度幅值阈值；
步骤2.3、使用Magwick AHRS算法，对角速度积分计算姿态，并根据三轴加速度数据进行修正，得到IMU元件在各时刻的姿态数据,记为四元数Q1；
步骤2.4、根据Q1将IMU坐标系下三轴加速度矢量aIMU＝(ax,ay,az)变换到世界坐标系下，得到世界坐标系下的IMU加速度；
Figure GDA0003952347270000022
其中
Qa＝(0，ax，ay，az)
Figure GDA0003952347270000031
是Q1的共轭四元数；
步骤2.5、对世界坐标系下的加速度数据进行积分，得到各时刻的三轴速度数据:
Figure GDA0003952347270000032
Figure GDA0003952347270000033
Figure GDA0003952347270000034
其中V_Xt-1，V_Yt-1，V_Zt-1分别为t-1时刻的x，y，z轴的速度，V_Xt，V_Yt，V_Zt分别为t时刻的x，y，z轴的速度，
Figure GDA0003952347270000035
分别为t时刻的x，y，z轴的加速度，ts为采样周期；
步骤2.6、消除静态部分累积误差；
根据人类行走过程中的步态特征，对于静态阶段可认为此时脚部运动速度为零，据此可进行零速度更新，即将判断出的静止阶段内的速度V_Xt，V_Yt，V_Zt置零，以消除积分的累积误差；
步骤2.7、消除动态部分累积误差；
根据人类行走过程中的步态特征，每个步态循环中动态部分结束时，即时刻t1至tk的脚部实际速度为零，时刻t1至tk积分出的速度值vend视为动态阶段中误差累积的结果，据此计算矢量误差累积速率ae；
ae＝vend/(tk-t1)
并分别计算对应t1至tk时间段内各时刻的矢量累积误差值ve1至vek，
[ve1，ve2，...vek]＝ae×[t，2t，...kt]
将累积误差值从速度数据中减去，得到修正完毕的矢量速度数据vc1至vck，
[vc1，vc2，...vck]＝[v1，v2，...vk]-[ve1，ve2，...vek]
其中，k为大于等于1的正整数；[v1，v2...vk]为修正前t1时刻至tk时刻的速度数据；
步骤2.8、将修正过的速度数据vc＝(V_X，V_Y，V_Z)进行积分计算人员运动轨迹；
xt＝xt-1+V_Xt×ts
yt＝yt-1+V_Yt×ts
zt＝zt-1+V_Zt×ts
其中(xt，yt，zt)为t时刻人员在三维空间中的位置，(xt-1，yt-1，zt-1)为人员t-1时刻的位置，(V_Xt，V_Yt，V_Zt)为人员t时刻速度，ts为采样周期。
步骤3、融合SLAM定位结果与步态IMU定位结果进行人员定位；
使用SLAM进行建图定位的同时，对SLAM传感器获得的原始数据进行检测，根据对应型号SLAM传感器的测量值判断是否受到对应环境因素的影响；
如SLAM传感器探测结果正常未受影响，则使用SLAM定位结果(SLAM_X，SLAM_Y，SLAM_Z)作为人员定位结果；
如SLAM传感器探测结果受环境影响，则定位结果无效，此时使用步态IMU定位测得的运动速度叠加到当前定位结果上计算人员的位置，因为IMU定位结果在较短时间内累积误差较小，而在长时间内定位精度低于正常工作下的SLAM定位***，因此此处采纳IMU测得的速度而非最终定位结果，将IMU速度(V_X，V_Y，V_Z)叠加到SLAM传感器受到环境因素影响前定位结果
[SLAM_Xt0，SLAM_Yt0，SLAM_Zt0]
上计算t时刻人员位置，则tk时刻人员位置为
[Xtk，Ytk，Ztk]＝[SLAM_Xt0，SLAM_Yt0，SLAM_Zt0]+[V_Xt1×ts，V_Yt1×ts，V_Zt1×ts]+…+[V_Xtk×ts，V_Ytk×ts，V_Ztk×ts]
其中，[V_Xtk，V_Ytk，V_Ztk]为tk时刻的IMU元件的在x轴、y轴以及z轴对应的速度。
本发明的有益效果在于：本发明提出了一种以建图与定位效果好的SLAM技术为主要定位技术，以对环境依赖小的步态IMU定位技术作为挑战性环境下的辅助定位手段，将两者融合起来的人员自主定位***，不仅在正常环境中具备良好的导航定位性能，在火场等特殊室内环境中也具有较强的鲁棒性。该方法也可用于其他潜在的室内复杂环境下人员自主定位用途。
附图说明
图1为本发明实施方式的一种基于SLAM与步态IMU技术的人员自主定位方法的流程图；
图2为本发明实施方式提供的人类步态循环过程示意图；
图3为本发明具体实施方式的融合激光SLAM方法与步态IMU的定位仿真及单一的激光SLAM方法定位仿真图；
具体实施方式
下面结合附图和实施例，对本发明的具体实施方式作进一步详细描述。
一种基于传统SLAM方法与步态IMU定位算法融合的人员自主定位方法，如图1所示，包含以下步骤：
步骤1、在室内环境下的人员穿戴上定位装置并将其启动，所述定位装置包括设置有激光雷达的头盔、固定于足部的IMU元件以及计算机；所述SLAM传感器与IMU元件均能与计算机实现无线通讯；
本实施例中，激光雷达固定于人员所戴消防员头盔上固定的托架，IMU元件通过绑带固定于人员的一侧足部背面。
步骤2、人员开始在室内环境中移动，此时激光雷达对周边环境进行扫描获取距离数据，计算机接收数据后通过激光SLAM算法对室内环境进行建图，同时对人员自身定位，本实施例中采取简单的基于scan-matching算法的2D激光SLAM算法；同时通过基于步态的IMU定位算法计算人员的运动轨迹。
本实施例中处理器为装有Linux***，ROS和Matlab的笔记本电脑，激光雷达为日本Hokuyo UTM-30LX 2D激光雷达，IMU元件为Xsens MTI-300型号。
步骤2.1、以IMU元件启动时刻的位置为原点建立世界坐标系，以IMU元件为原点建立IMU坐标系，从IMU元件获取测得的IMU坐标系下三轴角速度gyrx,gyry,gyrz与x轴、y轴与z轴对应的加速度ax、ay与az；
步骤2.2、根据加速度幅值判断当前时刻IMU所处的人员的脚是动态阶段还是静止阶段，
其中，加速度幅值maga的计算公式为：
Figure GDA0003952347270000051
若maga＜magthresh，则脚部处于静止阶段；
若maga≥magthresh，则脚部处于动态阶段；
其中magthresh为预设的速度幅值阈值；
步骤2.3、使用Magwick AHRS算法，对角速度积分计算姿态，并根据三轴加速度数据进行修正，得到IMU元件在各时刻的姿态数据,记为四元数Q1；
步骤2.4、根据Q1将IMU坐标系下三轴加速度矢量aIMU＝(ax,ay,az)变换到世界坐标系下，得到世界坐标系下的IMU加速度；
Figure GDA0003952347270000061
其中
Qa＝(0rax,ay,az)
Figure GDA0003952347270000062
是Q1的共轭四元数；
步骤2.5、对世界坐标系下的加速度数据进行积分，得到各时刻的三轴速度数据:
Figure GDA0003952347270000063
Figure GDA0003952347270000064
Figure GDA0003952347270000065
其中V_Xt-1，V_Yt-1，V_Zt-1分别为t-1时刻的x，y，z轴的速度，V_Xt，V_Yt，V_Zt分别为t时刻的x，y，z轴的速度，
Figure GDA0003952347270000066
分别为t时刻的x，y，z轴的加速度，ts为采样周期；
步骤2.6、消除静态部分累积误差；
如图2所示，根据人类行走过程中的步态特征，对于静态阶段可认为此时脚部运动速度为零，据此可进行零速度更新，即将判断出的静止阶段内的速度V_Xt，V_Yt，V_Zt置零，以消除积分的累积误差；
步骤2.7、消除动态部分累积误差；
根据人类行走过程中的步态特征，每个步态循环中动态部分结束时，即时刻t1至tk的脚部实际速度为零，时刻t1至tk积分出的速度值vend视为动态阶段中误差累积的结果，据此计算矢量误差累积速率ae；
ae＝vend/(tk-t1)
并分别计算对应t1至tk时间段内各时刻的矢量累积误差值ve1至vek，
[ve1，ve2，…vek]＝ae×[t，2t，…kt]
将累积误差值从速度数据中减去，得到修正完毕的矢量速度数据vc1至vck，
[vc1,vc2,…vck]＝[v1,v2,…vk]-[ve1,ve2,…vek]
其中，k为大于等于1的正整数；[v1,v2...vk]为修正前t1时刻至tk时刻的速度数据；
步骤2.8、将修正过的速度数据(V_X，V_Y，V_Z)积分计算人员运动轨迹；
xt＝xt-1+V_Xt×ts
yt＝yt-1+V_Yt×ts
zt＝zt-1+V_Zt×ts
其中(xt，yt，zt)为t时刻人员在三维空间中的位置，(xt-1，yt-1，zt-1)为人员t-1时刻的位置，(V_Xt,V_Yt,V_Zt)为人员t时刻速度，ts为采样周期。
步骤3、融合SLAM定位结果与步态IMU定位结果进行人员定位；
使用激光SLAM进行建图定位的同时对激光雷达获得的原始数据进行检测，判断目前激光雷达的测距结果是否受到火场中烟雾影响；
如激光雷达测距结果未受影响，则使用激光SLAM定位结果作为人员定位结果；
如激光雷达受烟雾影响，测距结果异常，则使用步态IMU定位测得的运动轨迹叠加到当前定位结果上计算人员的位置。
本实施例中，根据烟雾影响激光雷达工作的原理，烟雾颗粒反射激光，使激光雷达提前接收到反射激光信号，从而在该方向测得一过小的距离读数，可通过检测激光雷达测得的距离数据判断激光雷达是否受烟雾影响。设激光雷达在某一时刻测得不同方向上的距离数据共N组，分别为R1到RN，预设最小有效探测距离为Rthresh，将符合R<Rthresh的距离值记为Rm，则在Rm的数量NumRm占所有距离值R的数量NumR的比值大于一预设阈值Thersh时，即
Figure GDA0003952347270000071
则判断当前人员处于高烟雾环境中，烟雾颗粒反射激光导致激光雷达测得虚假距离，此时激光SLAM定位结果已不准确，此时使用IMU测得的人员位移数据叠加到激光SLAM的最后准确定位结果上，继续计算人员位置以提供定位信息；如判断激光雷达测距结果未受烟雾影响，则使用精度更高的激光SLAM定位结果作为人员定位结果。
如图3所示，其中左侧为单纯的激光SLAM方法在模拟消防人员在室内火场环境搜救场景的烟雾环境中的定位结果，可见经过烟雾覆盖区域后激光SLAM定位结果发生了严重错误，导致建图结果与定位轨迹均发生了严重偏差；右侧为本发明实施例中描述的融合了激光SLAM方法与步态IMU的室内人员定位方法在同一环境下的定位结果，可见融合后的定位方法在传统SLAM定位方法的基础上显著提高了在复杂环境下进行人员定位的可靠性。
视SLAM传感器类型不同可使用不同的判别方法，根据对应型号SLAM传感器的测量值判断是否受到对应环境因素的影响，如环境因素为光照时，根据图像像素灰度值判断视觉传感器是否受到强光或无光环境影响，如环境因素为烟雾时，根据激光扫描测得距离值判断激光雷达是否受到烟雾影响。

Claims (3)

1.一种基于SLAM与步态IMU融合的室内人员自主定位方法，其特征在于：包括：
步骤1、在室内环境下的人员穿戴上定位装置并将其启动，所述定位装置包括设置有SLAM传感器的头盔、固定于足部的IMU元件以及计算机；所述SLAM传感器与IMU元件均能与计算机实现无线通讯；
步骤2、人员在室内环境中自由移动，SLAM传感器将测得的数据传输给计算机，计算机使用SLAM算法对室内环境进行建图并对人员自身定位；
同时，IMU元件将测得的数据传输给计算机，计算机通过基于步态的IMU定位算法计算人员的运动轨迹；
步骤2.1、以IMU元件启动时刻的位置为原点建立世界坐标系，以IMU元件为原点建立IMU坐标系，从IMU元件获取测得的IMU坐标系下三轴角速度gyrx,gyry,gyrz与x轴、y轴与z轴对应的加速度ax、ay与az；
步骤2.2、根据加速度幅值判断当前时刻IMU所处的人员的脚是动态阶段还是静止阶段，
其中，加速度幅值maga的计算公式为：
Figure FDA0003952347260000011
若maga<magthresh，则脚部处于静止阶段；
若maga≥magthresh，则脚部处于动态阶段；
其中magthresh为预设的速度幅值阈值；
步骤2.3、使用Magwick AHRS算法，对角速度积分计算姿态，并根据三轴加速度数据进行修正，得到IMU元件在各时刻的姿态数据,记为四元数Q1；
步骤2.4、根据Q1将IMU坐标系下三轴加速度矢量aIMU＝(ax,ay,az)变换到世界坐标系下，得到世界坐标系下的IMU加速度；
Figure FDA0003952347260000013
其中
Qa＝(0,ax,ay,az)
Figure FDA0003952347260000014
是Q1的共轭四元数；
步骤2.5、对世界坐标系下的加速度数据进行积分，得到各时刻的三轴速度数据:
Figure FDA0003952347260000015
Figure FDA0003952347260000021
Figure FDA0003952347260000022
其中V_Xt-1，V_Yt-1，V_Zt-1分别为t-1时刻的x，y，z轴的速度，V_Xt，V_Yt，V_Zt分别为t时刻的x，y，z轴的速度，
Figure FDA0003952347260000023
分别为t时刻的x，y，z轴的加速度，ts为采样周期；
步骤2.6、消除静态部分累积误差；
根据人类行走过程中的步态特征，对于静态阶段可认为此时脚部运动速度为零，据此可进行零速度更新，即将判断出的静止阶段内的速度V_Xt，V_Yt，V_Zt置零，以消除积分的累积误差；
步骤2.7、消除动态部分累积误差；
根据人类行走过程中的步态特征，每个步态循环中动态部分结束时，即时刻t1至tk的脚部实际速度为零，时刻t1至tk积分出的速度值vend视为动态阶段中误差累积的结果，据此计算矢量误差累积速率ae；
ae＝vend/(tk-t1)
并分别计算对应t1至tk时间段内各时刻的矢量累积误差值ve1至vek，
[ve1,ve2,…vek]＝ae×[t,2t,…kt]
将累积误差值从速度数据中减去，得到修正完毕的矢量速度数据vc1至vck，
[vc1,vc2,…vck]＝[v1,v2,…vk]-[ve1,ve2,…vek]
其中，k为大于等于1的正整数；[v1,v2...vk]为修正前t1时刻至tk时刻的速度数据；
步骤2.8、将修正过的速度数据vc＝(V_X，V_Y，V_Z)进行积分计算人员运动轨迹；
xt＝xt-1+V_Xt×ts
yt＝yt-1+V_Yt×ts
zt＝zt-1+V_Zt×ts
其中(xt，yt，zt)为t时刻人员在三维空间中的位置，(xt-1，yt-1，zt-1)为人员t-1时刻的位置，(V_Xt,V_Yt,V_Zt)为人员t时刻速度，ts为采样周期；
步骤3、融合SLAM定位结果与步态IMU定位结果进行人员定位；
使用SLAM进行建图定位的同时，对SLAM传感器获得的原始数据进行检测，根据对应型号SLAM传感器的测量值判断是否受到对应环境因素的影响；
如SLAM传感器探测结果正常未受影响，则使用SLAM定位结果(SLAM_X，SLAM_Y,SLAM_Z)作为人员定位结果；
如SLAM传感器探测结果受环境影响，则定位结果无效，此时使用步态IMU定位测得的运动速度叠加到当前定位结果上计算人员的位置，因为IMU定位结果在较短时间内累积误差较小，而在长时间内定位精度低于正常工作下的SLAM定位***，因此此处采纳IMU测得的速度而非最终定位结果，将IMU速度(V_X,V_Y，V_Z)叠加到SLAM传感器受到环境因素影响前定位结果
[SLAM_Xt0，SLAM_Yt0，SLAM_Zt0]
上计算t时刻人员位置，则tk时刻人员位置为
[Xtk，Ytk，Ztk]＝[SLAM_Xt0，SLAM_Yt0，SLAM_Zt0]+[V_Xt1×ts，V_Yt1×ts，V_Zt1×ts]+…+[V_Xtk×ts，V_Ytk×ts，V_Ztk×ts]
其中，[V_Xtk，V_Ytk，V_Ztk]为tk时刻的IMU元件的在x轴、y轴以及z轴对应的速度。
2.根据权利要求1所述的一种基于SLAM与步态IMU融合的室内人员自主定位方法，其特征在于：所述的环境因素包括光照与烟雾。
3.根据权利要求1所述的一种基于SLAM与步态IMU融合的室内人员自主定位方法，其特征在于：所述的根据对应型号SLAM传感器的测量值判断是否受到对应环境因素的影响包括：当环境因素为光照时，根据图像像素灰度值判断视觉传感器是否受到强光或无光环境影响，当环境因素为烟雾时，根据激光扫描测得距离值判断激光雷达是否受到烟雾影响。
CN201910378821.1A 2019-05-08 2019-05-08 基于slam与步态imu融合的室内人员自主定位方法 Active CN109974696B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN201910378821.1A CN109974696B (zh) 2019-05-08 2019-05-08 基于slam与步态imu融合的室内人员自主定位方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201910378821.1A CN109974696B (zh) 2019-05-08 2019-05-08 基于slam与步态imu融合的室内人员自主定位方法

Publications (2)

Publication Number Publication Date
CN109974696A CN109974696A (zh) 2019-07-05
CN109974696B true CN109974696B (zh) 2023-01-03

Family

ID=67073219

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201910378821.1A Active CN109974696B (zh) 2019-05-08 2019-05-08 基于slam与步态imu融合的室内人员自主定位方法

Country Status (1)

Country Link
CN (1) CN109974696B (zh)

Families Citing this family (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN110579786A (zh) * 2019-07-19 2019-12-17 北京理工新源信息科技有限公司 定位方法及***、导航方法及***和一种车辆管理终端
CN114994672B (zh) * 2022-08-03 2022-10-25 中国人民解放军国防科技大学 毫米波雷达惯性组合的火场烟雾场景定位建图方法与装置

Citations (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN106705968A (zh) * 2016-12-09 2017-05-24 北京工业大学 基于姿态识别和步长模型的室内惯性导航算法
CN107478214A (zh) * 2017-07-24 2017-12-15 杨华军 一种基于多传感器融合的室内定位方法及***

Patent Citations (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN106705968A (zh) * 2016-12-09 2017-05-24 北京工业大学 基于姿态识别和步长模型的室内惯性导航算法
CN107478214A (zh) * 2017-07-24 2017-12-15 杨华军 一种基于多传感器融合的室内定位方法及***

Non-Patent Citations (1)

* Cited by examiner, † Cited by third party
Title
一种基于多传感融合的室内建图和定位算法;纪嘉文等;《成都信息工程大学学报》;20180831(第04期);第400-407页 *

Also Published As

Publication number Publication date
CN109974696A (zh) 2019-07-05

Similar Documents

Publication Publication Date Title
CN109931926B (zh) 一种基于站心坐标系的小型无人机无缝自主式导航方法
CN110095116A (zh) 一种基于lift的视觉定位和惯性导航组合的定位方法
EP2434256B1 (en) Camera and inertial measurement unit integration with navigation data feedback for feature tracking
CA2559236C (en) Apparatus and method of determining 3d pedestrian motion by using projection planes
Schall et al. Global pose estimation using multi-sensor fusion for outdoor augmented reality
CN102843988B (zh) 用于跟踪股骨参照系的基于mems的***
Ladetto et al. Digital magnetic compass and gyroscope integration for pedestrian navigation
Ladetto et al. In step with INS navigation for the blind, tracking emergency crews
CN107909614B (zh) 一种gps失效环境下巡检机器人定位方法
CN107490378B (zh) 一种基于mpu6050与智能手机的室内定位与导航的方法
CN111024075B (zh) 一种结合蓝牙信标和地图的行人导航误差修正滤波方法
KR20150108402A (ko) 디바이스와 보행자 사이의 오정렬 결정을 위한 방법 및 장치
CN109916394A (zh) 一种融合光流位置和速度信息的组合导航算法
Hartmann et al. Indoor 3D position estimation using low-cost inertial sensors and marker-based video-tracking
CN109284006B (zh) 一种人体运动捕获装置和方法
CN109974696B (zh) 基于slam与步态imu融合的室内人员自主定位方法
Woyano et al. Evaluation and comparison of performance analysis of indoor inertial navigation system based on foot mounted IMU
CN115574816B (zh) 仿生视觉多源信息智能感知无人平台
CN108827308B (zh) 一种高精度行人室外定位***及方法
KR101301462B1 (ko) 저가형 관성 센서를 이용한 보행자 관성 항법 장치 및 그 항법
JP2011237452A (ja) 物体の進行方向検知方法、位置検知方法、進行方向検知装置、位置検知装置、移動動態認識方法及び移動動態認識装置
CN116817905A (zh) 一种可穿戴多mems准实时协同导航***及方法
CN116380079A (zh) 一种融合前视声呐与orb-slam3的水下slam方法
Jaekel et al. An inertial navigation system with acoustic obstacle detection for pedestrian applications
Emter et al. Stochastic cloning and smoothing for fusion of multiple relative and absolute measurements for localization and mapping

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