CN107289941B - 一种基于惯导的室内定位方法与装置 - Google Patents

一种基于惯导的室内定位方法与装置 Download PDF

Info

Publication number
CN107289941B
CN107289941B CN201710447935.8A CN201710447935A CN107289941B CN 107289941 B CN107289941 B CN 107289941B CN 201710447935 A CN201710447935 A CN 201710447935A CN 107289941 B CN107289941 B CN 107289941B
Authority
CN
China
Prior art keywords
base station
carrier
monitored
sensor base
particle
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
CN201710447935.8A
Other languages
English (en)
Other versions
CN107289941A (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.)
Hunan yunjiangna micro Information Technology Co.,Ltd.
Original Assignee
Hunan Glonavin Information 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 Hunan Glonavin Information Technology Co ltd filed Critical Hunan Glonavin Information Technology Co ltd
Priority to CN201710447935.8A priority Critical patent/CN107289941B/zh
Publication of CN107289941A publication Critical patent/CN107289941A/zh
Application granted granted Critical
Publication of CN107289941B publication Critical patent/CN107289941B/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/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
    • 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

Landscapes

  • Engineering & Computer Science (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Automation & Control Theory (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Navigation (AREA)
  • Position Fixing By Use Of Radio Waves (AREA)

Abstract

本发明提供一种基于惯导的室内定位方法，包括以下步骤：(1)采集待监测载体的陀螺仪测量数据、加速度计测量数据；(2)计算待监测载体的位置；(3)判断是否检测到基站的信号；(4)计算粒子传播距离、传播方向、传播方向变化量；(4)计算粒子传播距离噪声、方向噪声，并计算粒子位置集合；(6)根据检测到的基站的信号计算粒子权值；(7)重采样得到待监测载体的位置估计。本发明还提供一种基于惯导的室内定位装置，包括基站、加速度计、陀螺仪，信号处理单元。本发明建立了基于粒子滤波结合惯性定位与基站数据相融合的柔性定位框架，使得惯性定位与基站信号有效融合，提高定位精确度和可靠性。

Description

一种基于惯导的室内定位方法与装置
技术领域
本发明涉及一种室内定位方法与装置，尤其涉及一种基于惯导与基站数据融合的室内定位方法与装置。本发明属于室内惯性定位领域，特别是以惯性导航定位为基础的数据融合辅助定位领域。
背景技术
在消防救援等应急任务场景中，获取消防员、搜救员、警用机器人等待监测物的实时位置可以极大的提高救援与工作效率，并且可有效避免救援人员在火场迷失方向而造成的不必要人员伤亡。惯性定位作为一种在应急任务中获取消防员等待监测物位置的重要方法，由于其不依靠外部设备，定位自主性强，短时定位精度高，在消防等应急突发事件应用中有不可替代的重要作用。
同时，惯性定位也存在如下的不足之处：(1)惯性器件受加工工艺与安装因素影响存在测量误差，测量误差会随着定位时间增长而累积，从而导致对所需监测物的定位精度下降和定位可靠性降低；(2)在实际应用中由于使用前准备时间的限制，不能充分获取惯性器件的初始偏置误差，导致航向上的误差增大，定位可靠性下降。因此，现有的惯性定位方法与装置无法满足要求。
发明内容
本发明要解决的问题是针对现有惯性定位存在的问题，提供一种基于惯导的室内定位方法及装置，使得定位结果更加精确，获取长时间可靠的定位结果。
为解决上述技术问题，本发明采用的技术方案是：一种基于惯导的室内定位方法，待监测载体行进区域内固定设置有基站，所述基站为iBeacon传感器基站和/或超宽带传感器基站和/或Wifi传感器基站和/或地磁传感器基站和/或RFID基站，待监测载体上设置有加速度计、陀螺仪，所述基于惯导的室内定位方法包括如下步骤：
(1)待监测载体行进中，采集待监测载体在载体坐标系下的陀螺仪测量数据、载体坐标系下的加速度计测量数据；
(2)利用陀螺仪测量数据、加速度计测量数据计算待监测载体在导航坐标系下的位置(xk,yk)；若待监测载体为人员，则k为采样时刻或人员行进的步数；若待监测载体为非人员，则k为采样时刻；
(3)若检测到基站的信号，则跳转到步骤(4)，否则，跳转到步骤(1)；
(4)利用待监测载体在导航坐标系下的位置(xk,yk)计算粒子传播距离lk、粒子传播方向θk、粒子传播方向变化量Δθk，其中θk∈[-π,π]：
Figure GDA0002774791140000021
(5)利用下式进行迭代运算，计算粒子位置集合
Figure GDA0002774791140000022
Figure GDA0002774791140000023
其中i为粒子个数，i＝1,2,…,N，
Figure GDA0002774791140000024
为粒子传播距离噪声，
Figure GDA0002774791140000025
为粒子传播方向噪声，
Figure GDA0002774791140000026
为陀螺误差量，
Figure GDA0002774791140000027
为陀螺误差的噪声，ΔT为粒子更新时间间隔，ΔT由获取的基站2的信号的更新率确定；
Figure GDA0002774791140000028
可采用有色噪声或白噪声；
(6)根据检测到的基站的信号计算粒子权值
Figure GDA0002774791140000029
(7)根据步骤(6)得到的粒子权值
Figure GDA00027747911400000210
对所有粒子进行重采样得到新的粒子位置集合
Figure GDA00027747911400000211
并利用下式对粒子位置求均值，得到待监测载体的位置估计
Figure GDA00027747911400000212
Figure GDA00027747911400000213
本发明中，利用基站测量数据与惯性定位结果相融合，使得定位结果更加精确，获取长时间可靠的定位结果。本发明中，利用陀螺仪测量数据、加速度计测量数据计算待监测载体在导航坐标系下的位置；利用待监测载体在导航坐标系下的位置计算粒子传播距离、粒子传播方向、粒子传播方向变化量，得到计算粒子位置集合；根据检测到的基站的信号计算粒子权值；根据粒子权值对所有粒子进行重采样得到新的粒子位置集合，并对粒子位置求均值，得到待监测载体的位置估计。本发明中，可设置一种类型或多种类型的基站，各个基站位于不同的作用范围内，当待监测载体行进到某个基站的作用范围时，即利用此基站的数据对待监测载体的位置进行修正。由于基站的位置是确定的，因此基站周围的距离与信号强度的关系也是确定的，因此利用基站的数据的修正，可以大大减小随着定位时间增长而累积的误差，使得定位精度大大提高。
上述技术方案中，所述步骤(2)包括如下子步骤：
(2.1)利用陀螺仪测量数据计算姿态变换矩阵；
(2.2)利用姿态变换矩阵和加速度计测量数据计算导航坐标系下的加速度计测量数据；
(2.3)将步骤(2.2)计算得到的导航坐标系下的加速度计测量数据进行积分运算得到待监测载体在导航坐标系下载体的原始计算位置(x'k,y'k)；当载体有零速状态时，利用零速修正技术对(x'k,y'k)进行修正得到待监测载体在导航坐标系下的位置(xk,yk)。
上述技术方案中，所述待监测载体为人员，所述步骤(2)包括如下子步骤：
(2.1)利用下式计算人员行进中第k步的步长Lk
Figure GDA0002774791140000031
其中，
Figure GDA0002774791140000032
为人员行进中第k步的步频，τ1、τ2分别为第一相关系数、第二相关系数；
(2.2)利用下式计算待监测载体在导航坐标系下的位置(xk,yk)
Figure GDA0002774791140000033
其中，φk为人员行进中第k-1步到第k步的步态方向变化角。
本发明中，可利用加速度计测量数据
Figure GDA0002774791140000034
计算人员的步频
Figure GDA0002774791140000035
利用陀螺仪测量数据
Figure GDA0002774791140000036
计算人员行进中第k-1步到第k步的步态方向变化角φk。
上述技术方案中，所述待监测载体为轮式机器人，所述步骤(2)中，利用下式计算轮式机器人在导航坐标系下的位置(xk,yk)
Figure GDA0002774791140000037
其中，Lk为k-1时刻到k时刻轮式机器人的水平位移增量，
Figure GDA0002774791140000038
为k时刻的轮式方向角。
本发明中，可利用陀螺仪测量数据
Figure GDA0002774791140000039
或车轮之间的差速计算k时刻的轮式方向角
Figure GDA00027747911400000310
上述技术方案中，所述步骤(6)中，检测到的信号为iBeacon传感器基站的信号强度srss,k，若srss,k＞sthres，则利用下式计算粒子权值
Figure GDA0002774791140000041
其中，(xibeacon,yibeacon)为iBeacon传感器基站的位置坐标，sthres为信号强度门限，dthres为距离门限。
本发明中，待监测载体行进到iBeacon传感器基站的作用范围时，判断检测到的信号为iBeacon传感器基站的信号强度srss,k是否满足要求，若满足要求，则利用iBeacon传感器基站计算粒子权值，从而实现利用iBeacon传感器基站的数据对待监测载体位置进行修正的目的。
上述技术方案中，所述步骤(6)中，检测到的信号为超宽带传感器基站的信噪比参数ζk，若ζk＞ζthres，则利用下式计算粒子权值
Figure GDA0002774791140000042
其中，
Figure GDA0002774791140000043
(xUBW,yUWB)为超宽带传感器基站的位置坐标，di为超宽带传感器基站与待监测载体(4)之间的相对距离，ζthres为信噪比门限。
本发明中，待监测载体行进到超宽带传感器基站的作用范围时，判断检测到的信号为超宽带传感器基站的信噪比参数ζk是否满足要求，若满足要求，则利用超宽带传感器基站计算粒子权值，从而实现利用超宽带传感器基站的数据对待监测载体位置进行修正的目的。
上述技术方案中，所述步骤(6)中，检测到的信号为Wifi传感器基站的Wifi指纹，利用下式计算粒子权值
Figure GDA0002774791140000044
其中，
Figure GDA0002774791140000045
(xwifi,ywifi)为利用Wifi指纹计算得到的待监测载体的位置，
Figure GDA0002774791140000046
为位置测量方差。
本发明中，待监测载体行进到Wifi传感器基站的作用范围时，则利用Wifi传感器基站计算粒子权值，从而实现利用Wifi传感器基站的数据对待监测载体位置进行修正的目的。
本发明还提供一种实现上述方法的基于惯导的室内定位装置，包括固定设置于待监测载体行进区域内的至少一个基站，所述基站为iBeacon传感器基站和/或超宽带传感器基站和/或Wifi传感器基站和/或地磁传感器基站和/或RFID基站，还包括设置于待监测载体上的加速度计、陀螺仪，还包括与所述加速度计、陀螺仪、基站连接的信号处理单元，所述基站为超宽带传感器基站和/或iBeacon传感器基站和/或Wifi传感器基站和/或地磁传感器基站和/或RFID基站。
本发明具有的优点和积极效果是：本发明建立了一个基于粒子滤波为核心的结合惯性定位结果与其它基站的数据相融合的柔性定位框架，使得多种基站的数据能够有效的与惯性数据相融合提高定位的精确度和可靠性。本发明作为一种柔性的方法框架可以根据实际需求，整合特定的惯性定位装置和基站。本发明具有适用性强，通用性好的优点，可以很大提高定位的长短时精度与可靠性。本发明可以柔性的利用不同的基站的信息，因此在基站的布置上可以采用稀疏布置的方法，根据实际中的成本要求，不要求在同一区域中布置两种类型或更多类型的基站，即各个基站的作用范围互相独立，而且可以根据实际需求可布置成本较为低廉的基站，这样可以极大的降低布置的硬件成本。
附图说明
图1为本发明的基于惯导的室内定位方法的处理流程示意图；
图2为本发明的基于惯导的室内定位装置的结构示意图；
图3为本发明的iBeacon预测作用距离示意图；
图4为本发明的UBW视距条件下测距精度可靠性示意图；
图5为本发明的空间内多个Wifi传感器基站信号强度在空间分布示意图；
图6(a)、(b)、(c)为本发明的实际行走轨迹、惯性定位的轨迹、融合惯性定位与超宽带传感器基站定位的轨迹的结果对比图，图中的矩形块的标注代表超宽带传感器基站；
图6(a)为本发明的实际运动轨迹指示图；
图6(b)为本发明的纯惯导计算轨迹；
图6(c)为本发明的融合惯性定位与超宽带传感器基站的定位结果图；
图7(a)、(b)分别为本发明的惯性定位的轨迹、融合惯性定位与iBeacon传感器基站定位的轨迹的结果，图中的矩形块、三角形块的标注分别代表超宽带传感器基站、Wifi传感器基站；
图7(a)为本发明的惯性定位的轨迹的结果图；
图7(b)为本发明的融合惯性定位与iBeacon传感器基站定位的定位轨迹的结果图；
图8(a)、(b)分别为本发明的惯性定位的轨迹与融合惯性定位、超宽带传感器基站与iBeacon传感器基站定位的轨迹的结果图。
图8(a)为本发明的惯性定位的结果图。
图8(b)为本发明的融合惯性定位、超宽带传感器基站定位、iBeacon传感器基站定位的结果对比图。
上述附图中，11、加速度计，12、陀螺仪，13、基站信号标签，2、基站，3、信号处理单元，4、待监测载体。
具体实施方式
下面结合附图对本发明作更详细地解释。图1所示基于粒子滤波为核心的结合惯性定位结果与其它基站数据相融合的柔性定位框架。
如图1、图2、图3、图4、图5、图6(a)-(c)、图7(a)-(b)、图8(a)-(b)所示，本发明是一种惯性定位与基站(例如多源辅助传感器基站)测量数据融合的定位方法，其对惯性定位装置的型式无特定要求，对所需要的基站种类与数目不做特定要求。专利提出的方法作为一种柔性的方法框架可以根据实际需求，整合特定的惯性定位转置和基站。本发明所提的方法具有适用性强，通用性好的优点，可以很大提高定位的长短时精度与可靠性。基站可采用辅助传感器基站或其他类型的基站。
本发明的方法主要包括：惯性航迹数据获取；粒子传播模型建立；基站数据获取及粒子权值更新；粒子滤波粒子重采样获取估计位置。
如图2所示，本发明还提供一种基于惯导的室内定位装置，待监测载体上设置有加速度计11、陀螺仪12，待监测载体4行进区域内固定设置有至少一个基站2，待监测载体4上设置有基站信号标签13，若基站2采用辅助传感器基站，则基站信号标签13为辅助传感器标签，辅助传感器标签为超宽带(UWB)传感器标签或iBeacon传感器标签或Wifi传感器标签。所述基站为iBeacon传感器基站和/或超宽带传感器基站和/或Wifi传感器基站和/或地磁传感器基站和/或RFID基站。
本发明中，待监测载体4行进区域内固定设置有基站2，即只要在待监测载体4行进过程中可以经过接收到的基站2信号强度高的区域，即可实现对待检测载体4的位置的修正。在实际应用过程中，待监测载体4并非必须每个时刻都在接收到的辅助传感器基站的信号强度高的区域活动，只要待监测载体4在运动过程中偶尔到达可接收到辅助传感器基站的信号强度高的区域，就能达到修正惯性定位结果的作用。
基站2与基站信号标签13通过无线方式连接。基站信号标签13检测到基站2的信号。检测到的基站2的信号为基站2与待监测载体4之间的信号强度srss或相对距离d或相对位置(xwifi,ywifi)，还包括与所述加速度计11、陀螺仪12、基站2连接的信号处理单元3，所述信号处理单元3用于：以固定周期采集时刻待监测载体4在载体坐标系下的陀螺仪测量数据、加速度计测量数据；利用陀螺仪测量数据、加速度计测量数据计算待监测载体4在导航坐标系下的位置；利用待监测载体4在导航坐标系下的位置(xk,yk)计算粒子传播距离lk、粒子传播方向θk、粒子传播方向变化量Δθk；计算粒子传播距离噪声、粒子传播方向噪声；计算粒子位置集合；根据检测到的基站2的信号计算粒子权值；根据粒子权值对所有粒子进行重采样得到新的粒子位置集合，并对粒子位置求均值，得到待监测载体4的位置估计。信号处理单元3可采用数字信号处理芯片，例如DSP、单片机、ARM等。
本发明的基本思路是：首先，利用惯性定位装置获取惯性定位信息，利用固定在待监测载体上的惯性定位装置获取载体在导航坐标系下的实时位置输出。通过捷联惯性导航算法或者航迹推算算法获得待监测载体的惯性定位结果。惯性定位的信息应用于柔性框架的核心粒子滤波的粒子传播模型的建立；然后利用获取的基站的测量数据对粒子滤波的粒子权值进行更新，实现数据融合；最后对粒子滤波的粒子进行重采样操作，得到融合当前数据下的优化估计定位结果。本发明中，利用固定在待监测载体上的惯性定位装置获取载体在导航坐标系下的实时位置输出。通过捷联惯性导航算法或者航迹推算算法获得待监测载体的惯性定位结果。
基站2可采用辅助传感器构成的基站，也可采用其他类型的非辅助传感器构成的基站。在本专利中，可仅布置一种类型的基站，如iBeacon传感器基站或超宽带传感器基站或Wifi传感器基站或地磁传感器基站或RFID基站，也可同时布置上述基站类型中的两种类型或三种类型的基站，例如同时布置超宽带传感器基站和iBeacon传感器基站，或同时布置超宽带传感器基站和wifi传感器基站，或同时布置iBeacon传感器基站和wifi传感器基站，或同时布置超宽带传感器基站、iBeacon传感器基站、wifi传感器基站。实际应用中，测量相对信号强度或相对距离或相对位置的传感器都可以用作本发明中的基站。
布置基站的数量多少根据实际应用决定。例如，根据成本或实际定位过程所涉及的面积决定，面积越大，可布置越多的基站2，即先确定人员的行进区域范围，根据人员的行进区域范围、成本确定布置基站2的数量。在待监测载体4的行进区域范围内，可以通过基站2的信号及时对待检测载体4的定位结果进行修正，使得定位结果更为准确。
布置多个基站时，优选将各个基站进行稀疏布置，即所布置的各个基站的作用范围不重合，即在同一时刻，仅利用一个辅助传感器对粒子权值进行计算。本发明的优势就是可以柔性的利用不同的辅助传感器信息，因此在传感器的布置上可以采用稀疏布置的方法，不要求在同一区域中布置两种及其以上的传感器，这样可以极大的降低布置的硬件成本。本发明中，并不是需要每个时刻都通过外界的基站2修正定位结果，外界的基站2只是用来修正惯导的位置数据的，在没有外界基站的信息时，依靠惯性定位确定待监测载体的位置。由于基站数据的可信定位精度是会随着距离的由近到远而衰减恶化的，所以有时候收到基站的数据并不代表这个数据是可用的。仅在待监测载体进入到基站的作用范围时，才进行修正，否则，仍采用惯性测量的数据。
各个基站都有一定的作用范围，由于基站的作用范围有限，仅布置基站，需要的基站的数量较多，成本较高，而仅布置惯性器件，又容易造成误差累积。本发明中，将惯性器件与基站结合起来进行使用，既降低了应用的成本，又通过布设基站对累积误差进行了修正，保证了定位精度。距离基站2越近的位置，对待监测载体4的位置估计越准确，若待监测载体与基站之间的距离超过一定值，也可能来自收到基站的信号，但由于相对距离较远，可能出现建筑物的阻挡或其他情况，有可能影响信号的准确性，从而影响待检测载体的位置的估计。因此，本发明中，仅当接收到来自基站的信号满足一定条件时，才利用基站2的信号对待监测载体的位置进行估计。
若未接收到来自基站的信号，或来自各个基站的接收信号均不可用，则仍跳转到步骤(1)，利用惯性定位确定待监测载体4的位置。
本发明中，惯性定位装置的安装与载体的类型不做特定要求。
在各个实施例中，给出载体位置获取的三种典型类型加以说明：足部航迹推算(Foot-INS)、腰部航迹推算(PDR-INS)、轮式机器人航迹推算(AGV-INS)，其中前两种适用在人，后一种适用在轮式机器人。
在实施例四至六中，将检测到的基站2的信号给出三种典型类型加以说明：iBeacon，UWB，Wifi。
实施例一
待监测载体为人员，利用人员足部航迹推算(Foot-INS)计算待监测载体的位置；此时k为采样时刻。优选在人员足部设置加速度计、陀螺仪。
本实施例中，基于惯导的室内定位方法的具体步骤如下：
(1)待监测载体行进中，以固定周期采集待监测载体4在载体坐标系下的陀螺仪测量数据
Figure GDA0002774791140000081
载体坐标系下的加速度计测量数据
Figure GDA0002774791140000082
优选为足部的加速度计、陀螺仪测量数据；其中，上标b或下标b表示数据是载体坐标系下的数据；
(2)利用陀螺仪测量数据
Figure GDA0002774791140000083
加速度计测量数据
Figure GDA0002774791140000084
计算待监测载体4在导航坐标系下的位置(xk,yk)；若待监测载体4为人员，则k为采样时刻或人员行进的步数；若待监测载体4为非人员，则k为采样时刻；若k为采样时刻，则k＝t，则
Figure GDA0002774791140000085
加速度计测量数据
Figure GDA0002774791140000086
利用捷联惯性导航算法获得人的位置的具体步骤如下：
(2.1)利用陀螺仪测量数据计算姿态变换矩阵
Figure GDA0002774791140000087
由加速度计测量数据、陀螺仪测量数据解算得到姿态角，姿态角包括横滚角、俯仰角、航向角；利用姿态角计算足部的初始姿态变换矩阵；利用如下的姿态变换矩阵微分方程计算姿态变换矩阵
Figure GDA0002774791140000091
其中，上标nv表示数据是在导航坐标系下的数据，
Figure GDA0002774791140000092
为陀螺数据的反对称矩阵，其表达式为
Figure GDA0002774791140000093
(2.2)利用姿态变换矩阵和加速度计测量数据计算导航坐标系下的加速度计测量数据：
利用下式通过姿态矩阵将加速度投影到导航坐标系下，得到导航坐标系下的加速度计测量数据
Figure GDA0002774791140000094
(2.3)将导航坐标系下的加速度计测量数据进行积分运算得到待监测载体4在导航坐标系下载体的原始计算位置：
对加速度
Figure GDA0002774791140000095
分别进行一次和二次积分分别得到速度{Vx,k,Vy,k}和待监测载体4在导航坐标系下的原始计算位置(x'k,y'k)，具体公式如下
Figure GDA0002774791140000096
其中，当Δt_sample为k时刻和k-1时刻之间的采样时间间隔，下标k表示不同时刻。ΔT为Δt_sample的整数倍。
当载体有零速状态时，利用零速修正技术对步骤(2.3)得到的(x'k,y'k)进行修正得到待监测载体4在导航坐标系下的位置(xk,yk)。
利用零速修正技术对原始计算位置(x'k,y'k)进行修正得到Foot-INS惯性航迹数据，即得到待监测载体4在导航坐标系下的位置(xk,yk)。零速修正技术是利用载体停车时惯性导航系统的速度输出作为系统速度误差的观测量，进而对其他各项误差进行修正。利用现有的零速修正技术即可进行计算。
(3)若检测到基站2的信号，则跳转到步骤(4)，否则，跳转到步骤(1)；
实际使用过程中，基站2不断扫描周围环境的基站信号标签13。当待监测载体4未进入到基站的作用范围时，即设置于待监测载体4上的基站信号标签13接收到来自待监测载体4的第一信号未满足相应条件时，待监测载体4的位置依然通过设置于待监测载体上的加速度计11、陀螺仪12计算。当待监测载体4上的基站信号标签13接收到来自待监测载体4的信号满足相应条件时，利用基站信号标签13收到的来自于辅助传感器4的信号对待监测载体4的位置进行修正。
(4)利用待监测载体4在导航坐标系下的位置(xk,yk)计算粒子传播距离lk、粒子传播方向θk、粒子传播方向变化量Δθk：
若计算粒子传播模型，则需构建粒子传播模型所需的粒子传播距离lk、粒子传播方向θk、粒子传播方向变化量Δθk。当k代表帧数时，在满足设定频率(几Hz到几百Hz)下输出惯性航迹数据{xk,yk}，输出的最大频率要比惯性定位装置内部计算频率要小。根据当前帧k的位置与前一帧k-1的位置得到相对变化长度lk与相对转向角Δθk。因此这里统一将lk定义为粒子滤波传播的距离，将Δθk定义为粒子滤波传播方向变化量，当前粒子集合的传播过程，其中
Figure GDA0002774791140000101
表示粒子集合中所有粒子的位置，N代表粒子集合中粒子总数，
Figure GDA0002774791140000102
为粒子传播的距离噪声和方向噪声。
根据如上所述惯性定位装置输出的航迹信息{xk,yk}建立粒子传播模型，选取任意相邻两个时刻或相邻两个步子的位置信息{xk,yk}与{xk-1,yk-1}，计算得到粒子滤波传播的距离lk与粒子滤波传播的方向Δθk作为粒子滤波状态输入，计算方法如下
Figure GDA0002774791140000103
其中θk∈[-π,π]；
(4)利用待监测载体4在导航坐标系下的位置(xk,yk)计算粒子传播距离lk、粒子传播方向θk、粒子传播方向变化量Δθk，其中θk也可采用θk＝atan2(Δyk,Δxk)进行计算，即θk∈[-π,π]，将生成的角度θk控制在[-π,π]的范围内；
(5)计算粒子传播距离噪声
Figure GDA0002774791140000104
粒子传播方向噪声
Figure GDA0002774791140000105
利用下式进行迭代运算，计算粒子位置集合
Figure GDA0002774791140000111
Figure GDA0002774791140000112
其中i为粒子个数，i＝1,2,…,N，
Figure GDA0002774791140000113
为陀螺误差量，
Figure GDA0002774791140000114
为陀螺误差的噪声，
Figure GDA0002774791140000115
通过有色噪声或白噪声计算得到，ΔT为粒子更新时间间隔，ΔT由获取的基站2的信号的更新率确定。本发明中，
Figure GDA0002774791140000116
的取值范围为0.001-0.05度/每秒。
考虑到粒子传播距离lk与传播方向Δθk由惯性定位装置获取，其存在误差，这里假设lk和Δθk的误差为高斯噪声。对整个粒子集合而言，粒子传播距离噪声为
Figure GDA0002774791140000117
粒子传播方向噪声为
Figure GDA0002774791140000118
由概率分布p(lr,k|lk)经过抽样得到，
Figure GDA0002774791140000119
由概率分布p(Δθr,k|Δθk)经过抽样得到
Figure GDA00027747911400001110
其中，
Figure GDA00027747911400001111
其中，N为粒子集合中包含的粒子数目，(σl)2与(σΔθ)2表示传播距离与传播方向的噪声方差，噪声方差的取值分别与传播距离与传播方向成正比关系，比例系数为c与d。c取值范围为[0.01,0.02]，d取值范围为[0.02,0.06]。
利用噪声粒子
Figure GDA00027747911400001112
与k-1时刻第i个粒子的位置
Figure GDA00027747911400001113
经计算后可得到新的粒子集
Figure GDA00027747911400001114
粒子集的传播过程包含如下迭代步骤
Figure GDA00027747911400001115
其中，Δθk数值是将陀螺Z轴输出进行积分运算的结果，且
Figure GDA00027747911400001116
需要考虑陀螺的误差
Figure GDA0002774791140000121
后，即
Figure GDA0002774791140000122
(6)根据检测到的基站2的信号计算粒子权值
Figure GDA0002774791140000123
各个粒子权值的初始值相等，即若存在N个粒子，那么每个粒子对应的权值为1/N。通过粒子传播距离、粒子传播方向可以更新粒子的位置状态，而粒子权值的更新是需要通过检测到的基站2的信号进行计算。
所检测的基站2的信号为基站与待监测载体之间的信号强度srss和/或相对距离d和/或相对位置(xwifi,ywifi)，检测到的基站2的信号可表示为Z*(*＝R,D,P)；不同类型的辅助测量传感器2获得传感器相对载体的信号强度ZR(RSSI)信息，或相对距离信息ZD，或相对位置信息ZP。对于基站2的布设以及选择的传感器类型不做特殊要求。检测到的基站2的信号用于更新粒子滤波粒子的权重。
仅在待监测载体4进入到基站2的作用范围时才对位置进行修正，否则，仍根据惯性测量方式确定待监测载体4在导航坐标系下的位置。
根据辅助传感器测量结果Z*(*＝R,D,P)对粒子滤波的粒子权值
Figure GDA0002774791140000124
进行更新。更新的方式为：
Figure GDA0002774791140000125
在实施例四至六中，将获取的检测到的基站2的信号给出三种典型类型加以说明：iBeacon，UWB，Wifi。
(7)待监测载体4的位置估计
Figure GDA0002774791140000126
根据得到的权值
Figure GDA0002774791140000127
按照剩余重采样(Residual Resampling)算法对所有粒子进行重采样得到新的粒子位置
Figure GDA0002774791140000128
并且对整个粒子集合中的粒子求均值，得到数据融合后的载体的位置估计。
由基站2的信号Z*(*＝R,D,P)对粒子的权值进行更新后。对粒子进行重采样操作并计算融合后的定位结果。
(7.1)利用下式对当前粒子集合中的所有粒子的权值进行归一化处理：
Figure GDA0002774791140000129
(7.2)根据步骤(7.1)中得到的权值
Figure GDA00027747911400001210
按照剩余重采样算法对所有粒子进行重采样，得到更新后的位置粒子集合
Figure GDA0002774791140000131
本发明的重采样方法利用Comparison of resampling schemes for particle filtering(作者R Douc，O Cappe，出版源Computer Science,2005:64-69)中的方法。确定粒子权值后，采用剩余重采样法去掉权值低的粒子，复制权值高的粒子，再对粒子的位置取均值，即根据粒子的权值决定是否舍弃该粒子或采用该粒子。这时的粒子集合就代表了对位置状态信息的概率分布；
(7.3)对步骤(7.2)中得到的重采样后的粒子位置数据取均值，得到融合后的位置估计
Figure GDA0002774791140000132
Figure GDA0002774791140000133
实施例二
待监测载体4为人员，利用腰部航迹推算(PDR-INS)计算待监测载体的位置；此时k为人员行进中的步数。本实施例与其他实施例相同的步骤可采用其他实施例中的步骤中的内容。本实施例二中，步骤(2)中，PDR-INS利用航迹推算模型计算载体的运动位置变化。加速度计、陀螺仪优选安装于人员腰部，获取的惯性测量数据为人员腰部的陀螺仪测量数据
Figure GDA0002774791140000134
和加速度计测量数据
Figure GDA0002774791140000135
本实施例中，步骤(2)中的具体分步骤如下：
(2.1)利用下式计算人员行进中第k步的步长Lk
Figure GDA0002774791140000136
其中，
Figure GDA0002774791140000137
为人员行进中第k步的步频，τ1、τ2分别为第一相关系数、第二相关系数；
本发明中，利用加速度计测量数据
Figure GDA0002774791140000138
计算人员的步频
Figure GDA0002774791140000139
通过对加速度计测量数据
Figure GDA00027747911400001310
执行过零检测操作，实现运动中的计步功能获取步频
Figure GDA00027747911400001311
根据加速度计测量数据计算静止的时刻和运动的时刻，根据静止时刻和运动时刻的采样点获取人员行进中第k步的步频
Figure GDA00027747911400001312
过零检测是指：在运动过程中，人员完成一个完整步子的过程中，加速度计对腰部的运动的测量得到的加速度计测量数据是一个从正值到负值的数据变化，因此通过判断加速度计测量数据由正值到负值的变化，即通过过零检测即可实现计步功能。
采用典型的步频——步长模型获得步长
Figure GDA00027747911400001313
其中τ1和τ2分别为第一相关系数、第二相关系数，τ1、τ2均通过实际试验中确定，这里给出实验中使用的值τ1＝20，τ2＝0.54。
Figure GDA0002774791140000141
为人员行进中第k步的步频。根据停、走、停判断方法，计算第k-1步、第k步对应的静止时刻之差的倒数即为第k步的步频。
(2.2)利用下式计算待监测载体(4)在导航坐标系下的位置(xk,yk)
Figure GDA0002774791140000142
其中，φk为人员行进中第k-1步到第k步的步态方向变化角。
实施例三
待监测载体4为轮式机器人，利用轮式机器人航迹推算(AGV-INS)计算待监测载体4的位置。本实施例中，k为采样时刻。本实施例与其他实施例相同的步骤可采用其他实施例中的步骤中的内容。本实施例中，步骤(2)中，AGV-INS利用航迹推算算法获得轮式机器人的位置变化。
本实施例中，步骤(2)中，利用下式计算轮式机器人在导航坐标系下的位置(xk,yk)
Figure GDA0002774791140000143
其中，Lk为k-1时刻到k时刻轮式机器人的水平位移增量，
Figure GDA0002774791140000144
为k时刻的轮式方向角，k为固定采样频率下采样点。(xk,yk)即为AGV-INS惯性航迹数据。
其中，采集k-1时刻到k时刻轮式机器人车轮编码器的脉冲数nk，依据下式计算k-1时刻到k时刻轮式机器人的水平位移增量Lk
Figure GDA0002774791140000145
其中，Rk为轮子的半径；ntol为轮式机器人车轮的轮子转一圈总脉冲数。利用陀螺仪测量数据
Figure GDA0002774791140000146
计算k时刻的轮式方向角
Figure GDA0002774791140000147
即对陀螺仪测量数据
Figure GDA0002774791140000148
进行积分得到轮式方向角
Figure GDA0002774791140000149
轮式机器人的位置变化推移，可以分解为两部分：方向变化和距离的变化。方向变化可以利用惯性测量数据给出，也可以利用车轮之间的差速给出。利用车轮之间的差速计算方向变化是现成的技术方案，查找资料可以获取。距离变化可以利用在轮式机器人上设置的里程计获取，这里的获取方法根据所使用的轮式机器人形式不同有不同的计算方法，所使用的方法在相关轮式机器人的资料中都有明确的说明。实际应用中，两轮机器人、基于四轮平台的轮式机器人均可应用于本申请。
实施例四
本实施例中，待监测载体4可为人员或轮式机器人。
本实施例与其他实施例相同的步骤可采用其他实施例中的步骤中的内容。本实施例的步骤(2)可采用实施例一、二、三中任一种方法。本实施例四中，基站为iBeacon传感器基站，在待检测载体4行进区域内固定设置iBeacon传感器基站，即设置iBeacon传感器基站，待监测载体上设置有iBeacon标签。检测到的基站2的信号为基站2与待监测载体之间的信号强度srss,k，利用信号强度srss计算粒子权值
Figure GDA0002774791140000151
超宽带标签接收到的信号为iBeacon传感器基站的信号强度srss,k。设置于人员腰部的蓝牙标签根据接收到来自于iBeacon传感器基站的信号强度RSSI计算出距离。
iBeacon传感器是使用蓝牙低功耗标准的广播信号，其确定范围是一个近似和定性方法。本发明使用iBeacon传感器作为辅助传感器标签获取一个特定的范围内的指示。人员携带的具有蓝牙功能的工具(如手机等)能够从iBeacon传感器获取RSSI(接收信号强度)信息。区域范围信号强度与相对距离之间的关系基本符合图3所示，从图中可以看出iBeacon可以提供:立即，远，近三种信息,其中立即表示范围小于0.5米,靠近表示介于0.5米和2.5米左右。srss,k为k时刻接收的信号强度，sthres为信号强度门限，若srss,k＞sthres，则按下式对粒子权值进行更新
Figure GDA0002774791140000152
其中(xibeacon,yibeacon)为iBeacon传感器基站的位置坐标，dthres为距离门限。如果srss,k小于门限sthres，则忽略本次测量信息。仅当待监测载体进入到iBeacon传感器基站的作用范围时，即srss,k＞sthres时，才进行修正，否则，仍采用惯性测量的数据。本实施例中，步骤(6)中，检测到的信号为iBeacon传感器基站的信号强度srss,k，若srss,k＞sthres，则利用下式计算粒子权值
Figure GDA0002774791140000153
其中，(xibeacon,yibeacon)为iBeacon传感器基站的位置坐标，sthres为信号强度门限，dthres为距离门限。其中信号强度门限sthres、距离门限dthres均通过实际试验或在实际使用环境中确定；
若srss的值不满足式子srss,k＞sthres，则不采用iBeacon传感器基站的信号更新粒子权值，则检测是否接收到其他基站(例如辅助传感器基站)的信号，若未检测到任何基站(例如辅助传感器基站)的信号，则回到步骤(1)，即仍采用惯性定位的结果确定待监测载体4的位置。
i iBeacon传感器基站不断的广播自身所在位置，设置于人员腰部的蓝牙标签根据接收到来自于iBeacon传感器基站的信号强度RSSI计算出相对距离。iBeacon传感器标签可采用TI公司的CC2541、CC2540或Nordic的nRF51822。
实施例五
本实施例中，待监测载体4可为人员或轮式机器人。
本实施例与其他实施例相同的步骤可采用其他实施例中的步骤中的内容。本实施例的步骤(2)可采用实施例一、二、三中任一种方法。本实施例中，基站2为超宽带(UWB)传感器基站，在待检测载体4行进区域内固定设置超宽带传感器基站2，待监测载体4上设置有超宽带传感器标签，即UWB标签。超宽带传感器标签接收到的信号为超宽带传感器基站2的信噪比参数ζk，获取接收到的基站2的信噪比参数ζk是超宽带的一个信噪比指标，信噪比参数ζk可以由超宽带基站发送，反映了当前UWB测距的结果可信度。
检测到的基站2的信号为基站与待监测载体之间的相对距离，利用相对距离计算粒子权值
Figure GDA0002774791140000161
通过超宽带(UWB)传感器能够获取超宽带基站与人员身上的超宽带标签的距离信息di，其测距获取的距离信息可靠度基本符合图4所示的高斯分布。
本实施例中，检测到的信号为超宽带传感器基站(2)的信噪比参数ζk，若ζk＞ζthres，则利用下式计算粒子权值
Figure GDA0002774791140000162
其中，
Figure GDA0002774791140000163
(xUBW,yUWB)为超宽带传感器基站的位置坐标，di为超宽带传感器基站与待监测载体4之间的相对距离，ζthres为信噪比门限，其中信噪比门限ζthres通过实际试验或在实际使用环境中确定。p()是一个均值为
Figure GDA0002774791140000164
方差为σ2的高斯分布。方差σ2根据实际基站的特性确定，即通过超宽带传感器本身确定。方差σ2可采用如下方法通过实验确定：首先依据超宽带传感器基站最大测距范围划归距离测试区段，依次在近距离、中等距离、远距离分别进行测试，对比实际距离和UWB测试距离之差，利用多组实验统计得到方差σ2的值。
本实施例中，利用设置于人员腰部的超宽带标签，以TOF方式与外部的超宽带传感器基站测距，人员腰部的超宽带标签可通过信号的飞行时间计算得到人员与基站2的距离。常见的超宽带标签包括有Decawave公司的DW1000，法国的BeSpoon的超宽带模组。
若ζk的值不满足式子ζk＞ζthres，则不采用超宽带传感器基站的信号更新粒子权值，则检测是否接收到其他基站(例如辅助传感器基站)的信号，若未检测到任何基站(例如辅助传感器基站)的信号，则回到步骤(1)，即仍采用惯性定位的结果确定待监测载体4的位置。
实施例六
本实施例中，待监测载体4可为人员或轮式机器人。
本实施例与其他实施例相同的步骤可采用其他实施例中的步骤中的内容。本实施例的步骤(2)可采用实施例一、二、三中任一种方法。本实施例中，基站2为Wifi传感器基站，在待检测载体4行进区域内固定设置Wifi传感器基站，待监测载体4上设置有Wifi传感器标签，检测到的基站2的信号为基站2与待监测载体4之间的相对位置(xwifi,ywifi)，利用相对位置(xwifi,ywifi)计算粒子权值
Figure GDA0002774791140000171
所述步骤(3)中，还包括获取Wifi传感器基站周边区域的位置与Wifi指纹的关系，接收到的信号为Wifi指纹。其他可以获取相对位置信息的基站也可以应用于本专利中。
图5给出了一个空间内三个Wifi传感器基站辐射信号强度图，图中是wifi信号强度的覆盖的区域示范图。图中，位于同一曲线上的点即为具有相同信号强度的点。对于Wifi传感器基站，辐射信号强度随着待监测载体4与Wifi传感器基站之间的相对距离而变化，相对距离越远，则辐射信号强度越小。在本专利的方法中，依据本方法实现时使用场景区域的大小，可选择布置Wifi传感器基站的数量。在本实施例中，根据实际情况，优选布置3个Wifi传感器基站，并记录各个Wifi传感器基站的位置。布置好的3个wifi传感器基站的位置在当前运动区域内是需要提前确定好的。
Wifi指纹库是指wifi的信号在其所布置空间内的一个分布。Wifi指纹库即为无线信号强度分布图或信号强度经验值数据库。实际应用中，先布设Wifi传感器基站，并确定Wifi传感器基站的位置。在进行定位之前，先通过试验得到Wifi传感器基站周围的信号强度与距离的对应关系，即通过对采集的各个Wifi传感器基站的Wifi信号指纹进行训练，可以得到wifi传感器基站所在区域各个位置和辐射信号强度之间的非线性关系的统计模型，即得到Wifi传感器基站周边区域的Wifi指纹库，根据该统计模型，在实际定位过程中，通过测量辐射信号强度，即可得到wifi传感器基站在其所在区域内的信号强度的分布。布置好的3个Wifi传感器基站的位置在当前运动区域内是需要提前确定好的。通过wifi信号指纹训练得到wifi传感器基站在其所在区域内的信号强度的分布。
本实施例中，检测到的信号为Wifi传感器基站的Wifi指纹，利用下式计算粒子权值
Figure GDA0002774791140000181
其中，
Figure GDA0002774791140000182
(xwifi,ywifi)为利用Wifi指纹计算得到的待监测载体的位置，
Figure GDA0002774791140000183
为位置测量方差，
Figure GDA0002774791140000184
在获取Wifi指纹时就已经确定了信号与位置的相关关系，在有些区域即使收到了wifi信号，但是没有与之相匹配的指纹信息，那么这样的信息是不会被使用的，因此可设置Wifi信号强度门限，若接收到的Wifi指纹大于Wifi信号强度门限，则利用接收到的Wifi指纹计算待监测载体的位置。也可不设置Wifi信号强度门限，若Wifi指纹未包含在所获取的Wifi指纹库的范围内，则不采用Wifi传感器基站的信号更新粒子权值，则检测是否接收到其他基站(例如辅助传感器基站)的信号，若未检测到任何基站(例如辅助传感器基站)的信号，则回到步骤(1)，即仍采用惯性定位的结果确定待监测载体4的位置。
Wifi传感器标签无需采用额外的模块，例如具有wifi功能的手持终端(如三防机、手机)即可，本专利使用的可集成wifi传感器标签是ESP8266。
实施例七
本实施例中，待监测载体4可为人员或轮式机器人。
本实施例与其他实施例相同的步骤可采用其他实施例中的步骤中的内容。本实施例的步骤(2)可采用实施例一、二、三中任一种方法。本实施例中，基站2为RFID基站。
RFID信标的作用机理与ibeacon的作用机理是相同的，即将实施例四中各个公式的iBeacon的数据替换为RFID信标的数据即可，且只是这里RFID信标可以提供的位置和信号强度的范围更大。
实施例八
本实施例中，待监测载体4可为人员或轮式机器人。
本实施例与其他实施例相同的步骤可采用其他实施例中的步骤中的内容。本实施例的步骤(2)可采用实施例一、二、三中任一种方法。本实施例中，基站2为地磁传感器基站。
地磁传感器的作用机理与wifi方案作用机理相同，地磁使用是通过前期的测量获取需要定位的空间内，位置与地磁强度之间的相对数据关系，这里可以把这样的关系理解为地磁指纹，地磁指纹的使用和wifi指纹使用是相同的。实际应用中，先布设地磁传感器基站，并确定地磁传感器基站的位置。在进行定位之前，先通过试验得到地磁传感器基站周围的信号强度与距离的对应关系，即通过对采集的各个地磁传感器基站的地磁信号指纹进行训练，可以得到地磁传感器基站所在区域各个位置和辐射信号强度之间的非线性关系的统计模型，即得到地磁传感器基站周边区域的地磁指纹库，根据该统计模型，在实际定位过程中，通过测量辐射信号强度，即可得到地磁传感器基站在其所在区域内的信号强度的分布。利用接收到的地磁指纹计算粒子权值，进行后续计算。在计算过程中，只要将实施例六中Wifi传感器的数据替换为地磁传感器的数据即可。
实施例九
本实施例中，待监测载体4可为人员或轮式机器人。
本实施例与其他实施例相同的步骤可采用其他实施例中的步骤中的内容。本实施例的步骤(2)可采用实施例一、二、三中任一种方法。本实施例中，步骤(6)中，检测到的基站2的信号为基站2与待监测载体4之间的相对位置(xwifi,ywifi)和/或相对距离d和/或相对位置(xwifi,ywifi)，利用相对位置(xwifi,ywifi)和/或相对距离d和/或相对位置(xwifi,ywifi)计算粒子权值
Figure GDA0002774791140000191
本实施例中，分别布置一个iBeacon传感器基站、一个Wifi传感器基站，优选布置为各个基站2的作用范围不重叠。
图8(a)-(b)给出，采用超宽带传感器基站与iBeacon传感器基站同时布置的情况。同时融合超宽带传感器基站和iBeacon传感器基站就精度而言相比较与单个基站相融合在精度上没有较大的区别，但是由于超宽带传感器基站的布置成本较高，iBeacon传感器基站成本低，因此对于传感器基站的布置要求降低，使灵活性更高。
最后给出专利提出的方法经过试验验证的结果，图6(a)-(c)给出，惯性定位与超宽带传感器融合后的定位结果与只用惯性定位的结果比较，定位误差明显减弱，使得定位结果更加精确。图6(a)为实际的行走轨迹，真实轨迹为沿箭头方向反复行走；图6(b)为本发明的纯惯导计算轨迹；图6(c)为本发明的融合惯性定位与超宽带传感器基站的定位结果图。如图6(a)中，真实的行走轨迹为，先在A-B-C段行走直线轨迹，然后在圆形的D区域行走多圈，绕圈后，沿之前的路径C-B-A段返回。图6(b)中可以看出，在开始的A-B-C段及在D区域绕圈的阶段，惯性定位基本与真实轨迹重合。在绕圈完成后，沿之前的路径C-B-A段运动时，惯性定位方法的轨迹明显偏离真实轨迹，即随着时间越长，惯性定位的累积误差越大，使得定位产生错误。如图6(c)所示，根据本方法，在绕圈完成后，沿之前的路径C-B-A段运动时，定位结果也会与真实运动路径产生偏移，但由于设置了超宽带基站，因此会逐渐将产生误差的定位轨迹拉回真实轨迹，消除了运动过程中产生的误差。
图7(a)-(b)给出，惯性定位与iBeacon传感器融合后的定位结果与只采用惯性定位的结果比较。随着时间的推移，惯性定位轨迹相对于真实行进路线存在较大的惯性漂移，而经过iBeacon传感器融合之后的定位轨迹几乎与原行进路线重合，结果更加精确。惯性定位与iBeacon传感器融合后纠正了惯性定位惯性漂移，定位更准确。根据图7(a)所示，M1、M2、M3的位置分别表示时间依次变长的惯性定位的轨迹，可以看出，M1基本与真实轨迹相符，但M2已出现偏差，M3的位置与真实运动区域完全偏离，定位结果已经不可信。因此图7(a)也验证了惯性定位的误差随时间累积越来越大，完全偏离了真实轨迹。根据图7(b)所示，本发明的方法，在相同的时间过程中，未出现误差累积的过程，始终处于真实轨迹，未出现明显的累积误差，定位精度较高。图7(a)为纯惯导计算轨迹，从图中可以看出纯惯导运动轨迹相对实际运动建筑物存在较大的误差；图7(b)为采用了惯导与Ibeacon融合定位的结果，从图中可以看出采用了融合定位的方法，定位轨迹和建筑物走向重合度明显提高。
图8(a)-(b)给出，采用超宽带传感器基站与iBeacon传感器基站同时布置的情况。如图8(a)所示，M4、M5、M6轨迹分别表示时间依次变长的惯性定位的轨迹，可以看出，随着时间的变长，惯性定位的误差随时间累积越来越大，M5的轨迹甚至出现了穿过墙体的情况，M6的轨迹甚至出现了在错误的房间的定位轨迹，完全偏离了真实轨迹。根据图8(b)所示，本发明的方法中同时布置超宽带传感器基站和iBeacon传感器基站，相同的时间过程中，未出现误差累积的过程，始终处于真实轨迹，未出现明显的累积误差，定位精度较高。
同时融合超宽带传感器基站和iBeacon传感器基站就精度而言相比较与单个基站相融合在精度上没有较大的区别，但是由于超宽带传感器基站的布置成本较高，iBeacon传感器成本低，因此对于传感器的布置要求降低，使灵活性更高。在实际中，可根据实际成本的要求灵活选择如何布置基站及布置哪种类型的基站。
需要说明的是，本说明书中的各个实施例均采用递进的方式描述，每个实施例重点说明的都是与其他实施例的不同之处，各个实施例之间相同相似的部分互相参见即可。
以上对本发明的实施例进行了详细说明，但所述内容仅为本发明的较佳实施例，不能被认为用于限定本发明的实施范围。凡依本发明范围所作的均等变化与改进等，均应仍归属于本专利涵盖范围之内。

Claims (7)

1.一种基于惯导的室内定位方法，其特征在于，待监测载体(4)行进区域内固定设置有基站(2)，所述基站(2)的类型为iBeacon传感器基站和RFID基站中的至少一种，或所述基站(2)的类型为iBeacon传感器基站和RFID基站中的至少一种以及超宽带传感器基站、Wifi传感器基站、地磁传感器基站中的至少一种，待监测载体(4)上设置有加速度计(11)、陀螺仪(12)，所述基于惯导的室内定位方法包括如下步骤：
(1)待监测载体(4)行进中，采集待监测载体(4)在载体坐标系下的陀螺仪测量数据、载体坐标系下的加速度计测量数据；
(2)利用陀螺仪测量数据、加速度计测量数据计算待监测载体(4)在导航坐标系下的位置(xk,yk)；若待监测载体(4)为人员，则k为采样时刻或人员行进的步数；若待监测载体(4)为非人员，则k为采样时刻；
(3)若检测到基站(2)的信号，则跳转到步骤(4)，否则，跳转到步骤(1)；
(4)利用待监测载体(4)在导航坐标系下的位置(xk,yk)计算粒子传播距离lk、粒子传播方向θk、粒子传播方向变化量Δθk，其中θk∈[-π,π]：
Figure FDA0002774791130000011
(5)利用下式计算粒子位置集合
Figure FDA0002774791130000012
Figure FDA0002774791130000013
其中i为粒子个数，i＝1,2,…,N，
Figure FDA0002774791130000014
为粒子传播距离噪声，
Figure FDA0002774791130000015
为粒子传播方向噪声，
Figure FDA0002774791130000016
为陀螺误差量，
Figure FDA0002774791130000017
为陀螺误差的噪声，ΔT为粒子更新时间间隔；
(6)根据检测到的基站(2)的信号计算粒子权值
Figure FDA0002774791130000018
若检测到的信号为iBeacon传感器基站或RFID基站的信号强度srss,k且srss,k＞sthres，则利用下式计算粒子权值
Figure FDA0002774791130000021
其中，(xibeacon,yibeacon)为iBeacon传感器基站或RFID基站的位置坐标，sthres为信号强度门限，dthres为距离门限；
若srss,k的值不满足srss,k＞sthres，则不采用iBeacon传感器基站或RFID基站的信号更新粒子权值，检测是否接收到其他基站(2)的信号，若未检测到其他任何基站(2)的信号，则跳转到步骤(1)；
(7)根据粒子权值
Figure FDA0002774791130000022
对所有粒子进行重采样得到新的粒子位置集合
Figure FDA0002774791130000023
并利用下式对粒子位置求均值，得到待监测载体(4)的位置估计
Figure FDA0002774791130000024
Figure FDA0002774791130000025
2.根据权利要求1所述的基于惯导的室内定位方法，其特征在于，所述步骤(2)包括如下子步骤：
(2.1)利用陀螺仪测量数据计算姿态变换矩阵；
(2.2)利用姿态变换矩阵和加速度计测量数据计算导航坐标系下的加速度计测量数据；
(2.3)将步骤(2.2)计算得到的导航坐标系下的加速度计测量数据进行积分运算得到待监测载体(4)在导航坐标系下的原始计算位置(x'k,y'k)；当载体有零速状态时，利用零速修正技术对(x'k,y'k)进行修正得到待监测载体(4)在导航坐标系下的位置(xk,yk)。
3.根据权利要求1所述的基于惯导的室内定位方法，其特征在于，所述待监测载体(4)为人员，所述步骤(2)包括如下子步骤：
(2.1)利用下式计算人员行进中第k步的步长Lk
Figure FDA0002774791130000026
其中，
Figure FDA0002774791130000027
为人员行进中第k步的步频，τ1、τ2分别为第一相关系数、第二相关系数；
(2.2)利用下式计算待监测载体(4)在导航坐标系下的位置(xk,yk)
Figure FDA0002774791130000028
其中，φk为人员行进中第k-1步到第k步的步态方向变化角。
4.根据权利要求1所述的基于惯导的室内定位方法，其特征在于，所述待监测载体(4)为轮式机器人，所述步骤(2)中，利用下式计算轮式机器人在导航坐标系下的位置(xk,yk)
Figure FDA0002774791130000031
其中，Lk为k-1时刻到k时刻轮式机器人的水平位移增量，
Figure FDA0002774791130000032
为k时刻的轮式方向角。
5.根据权利要求1-4中任一项所述的基于惯导的室内定位方法，其特征在于，所述基站(2)的类型为iBeacon传感器基站和RFID基站中的至少一种以及超宽带传感器基站，或所述基站(2)的类型为iBeacon传感器基站和RFID基站中的至少一种以及Wifi传感器基站、地磁传感器基站中的至少一种以及超宽带传感器基站；所述步骤(6)中，若检测到的信号为超宽带传感器基站(2)的信噪比参数ζk且ζk＞ζthres，则利用下式计算粒子权值
Figure FDA0002774791130000033
其中，
Figure FDA0002774791130000034
(xUBW,yUWB)为超宽带传感器基站的位置坐标，di为超宽带传感器基站与待监测载体(4)之间的相对距离，ζthres为信噪比门限；
若ζk的值不满足ζk＞ζthres，则不采用超宽带传感器基站的信号更新粒子权值，检测是否接收到其他基站(2)的信号，若未检测到其他任何基站(2)的信号，则跳转到步骤(1)。
6.根据权利要求1-4中任一项所述的基于惯导的室内定位方法，其特征在于，所述基站(2)的类型为iBeacon传感器基站和RFID基站中的至少一种以及Wifi传感器基站、地磁传感器基站中的至少一种，或所述基站(2)的类型为iBeacon传感器基站和RFID基站中的至少一种以及Wifi传感器基站、地磁传感器基站中的至少一种以及超宽带传感器基站；所述步骤(6)中，若检测到的信号为Wifi传感器基站的Wifi指纹或地磁传感器基站的地磁指纹，则利用下式计算粒子权值
Figure FDA0002774791130000035
其中，
Figure FDA0002774791130000036
(xwifi,ywifi)为利用Wifi指纹或地磁指纹计算得到的待监测载体的位置，
Figure FDA0002774791130000037
为位置测量方差。
7.一种实现权利要求1-6中任一项方法的基于惯导的室内定位装置，其特征在于，包括固定设置于待监测载体(4)行进区域内的至少一个基站(2)；
所述基站(2)的类型为iBeacon传感器基站和RFID基站中的至少一种，或所述基站(2)的类型为iBeacon传感器基站和RFID基站中的至少一种以及超宽带传感器基站、Wifi传感器基站、地磁传感器基站中的至少一种，还包括设置于待监测载体(4)上的加速度计(11)、陀螺仪(12)，还包括与所述加速度计(11)、陀螺仪(12)、基站(2)连接的信号处理单元(3)。
CN201710447935.8A 2017-06-14 2017-06-14 一种基于惯导的室内定位方法与装置 Active CN107289941B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN201710447935.8A CN107289941B (zh) 2017-06-14 2017-06-14 一种基于惯导的室内定位方法与装置

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201710447935.8A CN107289941B (zh) 2017-06-14 2017-06-14 一种基于惯导的室内定位方法与装置

Publications (2)

Publication Number Publication Date
CN107289941A CN107289941A (zh) 2017-10-24
CN107289941B true CN107289941B (zh) 2021-01-08

Family

ID=60097814

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201710447935.8A Active CN107289941B (zh) 2017-06-14 2017-06-14 一种基于惯导的室内定位方法与装置

Country Status (1)

Country Link
CN (1) CN107289941B (zh)

Families Citing this family (24)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN108279687A (zh) * 2018-03-28 2018-07-13 西北农林科技大学 一种郁闭果园履带拖拉机组合导航系统
CN108801264A (zh) * 2018-06-05 2018-11-13 安徽邵氏华艾生物医疗电子科技有限公司 一种三维校正模型下的室内导航方法
CN109115209B (zh) * 2018-07-20 2022-03-11 湖南格纳微信息科技有限公司 一种管廊内人员定位方法及装置
CN108896044B (zh) * 2018-07-23 2021-09-10 湖南格纳微信息科技有限公司 一种基于惯性导航和卫星导航的定位方法及装置
CN109018077B (zh) * 2018-08-29 2023-08-18 力创科技股份有限公司 一种基站导航双舵轮agv车及使用方法
CN109581287B (zh) * 2019-01-22 2024-02-09 西南石油大学 一种基于Wi-Fi的震后压埋人员定位方法
CN109974690B (zh) * 2019-03-18 2021-07-09 汉海信息技术(上海)有限公司 车辆定位方法、设备及系统
CN109883428B (zh) * 2019-03-27 2022-11-01 成都电科慧安科技有限公司 一种融合惯导、地磁和WiFi信息的高精度定位方法
CN110081888B (zh) * 2019-05-15 2021-06-22 华南师范大学 一种基于可信度的惯导和地磁融合的室内定位算法
CN110285809B (zh) * 2019-07-15 2020-12-08 李素敏 一种室内外一体化的组合定位装置
CN110307849B (zh) * 2019-07-15 2020-12-08 李素敏 一种人员室内定位以及室内外一体化定位方法
CN110440794B (zh) * 2019-07-26 2021-07-30 杭州微萤科技有限公司 一种imu导航的定位校正方法
CN110913332A (zh) * 2019-11-21 2020-03-24 深圳市航天华拓科技有限公司 区域内定位系统及方法
CN111341102B (zh) * 2020-03-02 2021-04-23 北京理工大学 运动基元库构建方法和装置、连接运动基元的方法和装置
CN113438615A (zh) * 2020-03-04 2021-09-24 北京京东乾石科技有限公司 定位方法和装置
CN111542116B (zh) * 2020-05-18 2021-12-21 厦门大学 一种移动目标定位方法及系统
CN111988739B (zh) * 2020-08-06 2023-05-02 普玄物联科技(杭州)有限公司 一种高精度定位的商场导购系统及其应用方法
CN112556689B (zh) * 2020-10-30 2023-09-05 郑州联睿电子科技有限公司 一种融合加速度计和超宽带测距的定位方法
CN113218388B (zh) * 2021-03-02 2022-07-05 济南大学 一种考虑可变有色测量噪声的移动机器人定位方法及系统
CN113093100A (zh) * 2021-03-09 2021-07-09 惠州Tcl移动通信有限公司 一种定位方法、智能终端及计算机可读存储介质
CN113155131B (zh) * 2021-04-14 2022-06-17 浙江工业大学 一种基于粒子滤波的iBeacon和PDR融合室内定位方法
CN114268901B (zh) * 2021-12-24 2024-01-26 国网福建省电力有限公司 一种提高室内定位精度的方法及终端
CN115128540B (zh) * 2022-09-02 2022-12-06 珠海翔翼航空技术有限公司 一种适用于飞行平台的定位定向方法和装置
CN116761254B (zh) * 2023-08-17 2023-11-07 中国电信股份有限公司 室内定位方法、装置、通信设备和存储介质

Citations (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN103175529A (zh) * 2013-03-01 2013-06-26 上海美迪索科电子科技有限公司 基于室内磁场特征辅助的行人惯性定位系统
CN103487047A (zh) * 2013-08-06 2014-01-01 重庆邮电大学 一种基于改进粒子滤波的移动机器人定位方法
CN104180805A (zh) * 2014-08-29 2014-12-03 中国海洋大学 基于智能手机的室内行人定位与跟踪方法
CN106248077A (zh) * 2016-07-06 2016-12-21 北京理工大学 一种基于粒子滤波的可见光组合定位系统和方法
CN106525052A (zh) * 2016-12-14 2017-03-22 广东工业大学 一种图模型约束的室内定位方法

Family Cites Families (8)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US8583400B2 (en) * 2011-05-13 2013-11-12 Google Inc. Indoor localization of mobile devices
KR101833217B1 (ko) * 2011-12-07 2018-03-05 삼성전자주식회사 자기장 지도 기반 측위 시스템에서 이용되는 이동 단말 및 이를 이용한 위치 추정 방법
US9429437B2 (en) * 2012-06-08 2016-08-30 Apple Inc. Determining location and direction of travel using map vector constraints
CN103616030A (zh) * 2013-11-15 2014-03-05 哈尔滨工程大学 基于捷联惯导解算和零速校正的自主导航系统定位方法
CN104197936B (zh) * 2014-09-10 2017-04-12 湖南格纳微信息科技有限公司 一种基于建筑物规则结构信息的室内定位方法
CN105783923B (zh) * 2016-01-05 2018-05-08 山东科技大学 基于rfid和mems惯性技术的人员定位方法
CN105588566B (zh) * 2016-01-08 2019-09-13 重庆邮电大学 一种基于蓝牙与mems融合的室内定位系统及方法
CN106840163A (zh) * 2016-12-29 2017-06-13 上海斐讯数据通信技术有限公司 一种室内定位方法及系统

Patent Citations (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN103175529A (zh) * 2013-03-01 2013-06-26 上海美迪索科电子科技有限公司 基于室内磁场特征辅助的行人惯性定位系统
CN103487047A (zh) * 2013-08-06 2014-01-01 重庆邮电大学 一种基于改进粒子滤波的移动机器人定位方法
CN104180805A (zh) * 2014-08-29 2014-12-03 中国海洋大学 基于智能手机的室内行人定位与跟踪方法
CN106248077A (zh) * 2016-07-06 2016-12-21 北京理工大学 一种基于粒子滤波的可见光组合定位系统和方法
CN106525052A (zh) * 2016-12-14 2017-03-22 广东工业大学 一种图模型约束的室内定位方法

Also Published As

Publication number Publication date
CN107289941A (zh) 2017-10-24

Similar Documents

Publication Publication Date Title
CN107289941B (zh) 一种基于惯导的室内定位方法与装置
JP6783751B2 (ja) 地図情報支援の質を高められたポータブルナビゲーションを使用する方法および装置
US9664521B2 (en) System and method for localizing a trackee at a location and mapping the location using signal-based features
US11162792B2 (en) Method and system for path-based point of sale ordering
JP6054535B2 (ja) 歩行者モーション認識基盤の歩行者位置推定装置、及びその方法
US20150018018A1 (en) Indoor Location-Finding using Magnetic Field Anomalies
CN107014375B (zh) 超低部署的室内定位系统及方法
JP2007093433A (ja) 歩行者の動態検知装置
KR20130059344A (ko) 물체의 제로 속도 상태를 검출하는 방법 및 시스템
CN108957510B (zh) 基于惯性/零速/gps的行人无缝组合导航定位方法
CN109855621A (zh) 一种基于uwb与sins的组合室内行人导航系统及方法
EP2881708A1 (en) System and method for indoor localization using mobile inertial sensors and virtual floor maps
Niu et al. Pedestrian trajectory estimation based on foot-mounted inertial navigation system for multistory buildings in postprocessing mode
Huang et al. Smartphone-based indoor position and orientation tracking fusing inertial and magnetic sensing
US10324197B1 (en) Systems and methods for estimating initial heading at start-up of navigation
Glanzer et al. Self-contained indoor pedestrian navigation by means of human motion analysis and magnetic field mapping
US9250078B2 (en) Method for determining the position of moving objects
Lategahn et al. Robust pedestrian localization in indoor environments with an IMU aided TDoA system
Alaoui et al. Points of interest detection for map-aided PDR in combined outdoor-indoor spaces
Li et al. Pedestrian positioning in urban environment by integration of PDR and traffic mode detection
CN115406435A (zh) 基于wlan和mems的室内电子地图构建方法、装置及存储介质
Uradzinski et al. Pedestrian navigation system based on the inertial measurement unit sensor for outdoor and indoor environments
Zhang et al. A single-anchor calibration indoor positioning system using heterogeneous sensors
Yushuai et al. Indoor navigation system based on foot-mounted IMU and map information fusion
KR20130012714A (ko) 보폭 추정 장치, 및 그 방법

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
CB02 Change of applicant information

Address after: 410205 No. 5, 4th Floor, Building F4, Yuyuan, Lugu, 27 Wenxuan Road, Changsha High-tech Development Zone, Hunan Province

Applicant after: Hunan Wagner micro Mdt InfoTech Ltd

Address before: 410205 Room 1610, 16th Floor, Headquarters Building, Zhongdian Software Park, 39 Jianshan Road, Changsha High-tech Development Zone, Hunan Province

Applicant before: Hunan Wagner micro Mdt InfoTech Ltd

CB02 Change of applicant information
GR01 Patent grant
GR01 Patent grant
CP03 Change of name, title or address

Address after: 410205 1705, building 1b, Hunan military civilian integration science and Technology Innovation Industrial Park, No. 699, Qingshan Road, Changsha high tech Development Zone, Changsha, Hunan

Patentee after: Hunan yunjiangna micro Information Technology Co.,Ltd.

Address before: 410205 No. 5, 4th Floor, Building F4, Yuyuan, Lugu, 27 Wenxuan Road, Changsha High-tech Development Zone, Hunan Province

Patentee before: HUNAN GLONAVIN INFORMATION TECHNOLOGY CO.,LTD.

CP03 Change of name, title or address