CN106225784B - 基于低成本多传感器融合行人航位推算方法 - Google Patents

基于低成本多传感器融合行人航位推算方法 Download PDF

Info

Publication number
CN106225784B
CN106225784B CN201610415776.9A CN201610415776A CN106225784B CN 106225784 B CN106225784 B CN 106225784B CN 201610415776 A CN201610415776 A CN 201610415776A CN 106225784 B CN106225784 B CN 106225784B
Authority
CN
China
Prior art keywords
time
matrix
speed
error
increment
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.)
Expired - Fee Related
Application number
CN201610415776.9A
Other languages
English (en)
Other versions
CN106225784A (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.)
Second Institute of Oceanography SOA
Original Assignee
Second Institute of Oceanography SOA
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 Second Institute of Oceanography SOA filed Critical Second Institute of Oceanography SOA
Priority to CN201610415776.9A priority Critical patent/CN106225784B/zh
Publication of CN106225784A publication Critical patent/CN106225784A/zh
Application granted granted Critical
Publication of CN106225784B publication Critical patent/CN106225784B/zh
Expired - Fee Related legal-status Critical Current
Anticipated expiration legal-status Critical

Links

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)
  • Navigation (AREA)

Abstract

本发明公开了基于低成本多传感器融合行人航位推算方法，给出了详细的相关方法流程，以解决因低成本传感器精度低，硬件质量差及易受外界干扰等缺点。提出了利用扩展卡尔曼滤波(EKF，extended Kalman Filter)与零速度更新(ZUPT，Zero Velocity Update)和磁力计结合的方法，用于步态检测、修正速度误差，并且对通过加速度计和磁力计对陀螺仪进行误差修正，同时对航向角误差修正。经实际测试，利用该方法可以较好的满足室内行人定位要求，定位误差占总路程的2%左右。该方法利用多传感器融合，可大幅度减少多传感器精度低带来的误差，并且较惯性测量单元能可有效的提升姿态精度。该方法在机器人、基于位置的服务(LBS，Location‑Basic Service)、室内行人航位推算等都有很高的应用前景与价值，具有广泛的实用性和通用性。

Description

基于低成本多传感器融合行人航位推算方法
技术领域
本发明涉及计算机科学、控制科学、室内定位系统、惯性导航系统等技术领域。
背景技术
行人航位推算(Pedestrian Dead Reckoning，PDR)系统是目前实现精确室内定位的主要系统；基于惯性测量单元(Inertial Measurement Unit，IMU)，利用加速度计和陀螺仪推算下一时刻行人的位置，无需任何外来信息，也不向外辐射任何信息，拥有良好的自主性、隐蔽性和数据更新速度快等特点。加速度计可以用来测量行人的运动加速度，利用已知的初始位置和速度，对加速度进行积分计算，获取行人的速度和位置等信息；陀螺仪测量行人的角运动，获取行人角度的变换信息，最终获取行人的方位等信息；但是由于低成本惯性测量单元精度较低，测量误差会随时间累积增大，最终造成较大的位置偏差。如：目前被广泛使用的微机电系统(Micro Electro Mechanical System，MEMS)，单纯的测量单元输出的数据夹杂着大量噪声，对定位结果有很大影响；对于精度比较高的惯性测量单元，如：激光惯导设备精度在0.1°/h，虽然精度高，但是价格昂贵而且体积大，不便用于室内行人定位。
目前广泛用于室内定位还有利用无线传感网络的方式，即事先布置AP节点，通过行人接收端采集到的无线信号值来判断行人在室内的位置信息；但此方法事先需进行大量的专业室内数据测量和人工的信号采集，依赖于AP硬件的部署，需耗费大量的前期数据库构造成本。
在申请专利“行人步长估计及航位推算方法(申请号：2013103884669)”中提到“利用加速度计测量行人行走时个人步长的参数，同时检测行走过程中加速度幅值的最大值amax和最小值amin，计算行人行走一步的步长”，该申请主要是利用非线性步长模型，估算步伐状态和步长，从而推算行人的位置；但是该专利未提出步伐频率不同对步长影响的分析，所以一旦行人在行走过程中，步伐频率不定，势必造成行人位置的偏差。
发明内容
本发明公开了一种基于低成本多传感器融合行人航位推算方法，以解决因低成本传感器精度低，硬件质量差及易受外界干扰等缺点。提出了利用扩展卡尔曼滤波与零速度更新和磁力计结合的方法，用于步态检测、修正速度误差，并且对通过加速度计和磁力计对陀螺仪进行误差修正，同时对航向角误差修正。
本发明是通过下述技术方案得以实现的：
基于低成本多传感器融合行人航位推算方法，包括下列步骤：
1)静态粗对准
1.1)若i表示惯性坐标系，e表示地球坐标系。表示e系相对i系的角速度在b系下的分量。表示b系相对e系的角速度在b系下的分量,表示b系相对i系的加速度在b系下的分量。静态初始时刻速度为0，加速度计的输出为：
1.2)俯仰角θ和横滚角φ分别为：
1.3)采用三轴磁力计来计算航向角：
mx、my和mz分别代表磁力计三个方向的输出值，所以航向角为：
ψ＝Δψ+D
D为本地磁偏角；
1.4)利用初始姿态角获取方向余弦矩阵，g是本地重力加速度，b表示载体坐标系，n表示导航坐标系(选择北西天地理坐标系)，表示从导航坐标系到载体坐标系的方向余弦矩阵；
gn＝[0 0 -g]′
1.5)四元数与姿态矩阵的关系：
联立步骤1.4)和步骤1.5)可解得：
q＝[q0 q1 q2 q3]
1.6)对四元数进行共轭处理：
1.7)陀螺转子相对地球的自转角速率：
1.8)陀螺转子四元数：
1.9)陀螺仪y系下的矢量：
1.10)初始时刻角增量的偏差：
1.11)加速度计四元数：
1.12)加速度计y系下的矢量：
1.13)初始时刻速度计的偏差：
1.14)转入步骤2)。
2)静态精对准
遍历静止时每一时刻的加速度、角速率、时间间隔和加速度漂移，根据子样进行角速度和加速度拟合,更新系统的角增量和速度增量；
2.1)时间间隔Δt＝1/f，f为采样频率；
2.2)采样样本数nsample＝f·t，t初始对准时长；
2.3)样本间隔时间矩阵I为单位矩阵；
2.4)速度间隔时间矩阵
2.5)增量时间矩阵
2.6)角增量
2.7)速度增量
2.8)角增量求和
2.9)速度增量求和
2.10)可由步骤2.6)和步骤2.8)，求得圆锥补偿后的角增量：
2.11)可由步骤2.6)～步骤2.9)，求得划船补偿后的速度增量：
2.12)速度四元数：
2.13)加速度计y系下的矢量：
2.14)更新后的速度：
v0为初始速度；
2.15)更新后的四元数
如果为0，则否则
2.16)更新后的位置：
r更新＝r0+(v0+v更新)/2·Δt
r0为初始位置；
2.17)EKF的量测方程为：
Zk＝HδX(k,k)+nk
Zk是量测值，H是量测矩阵，nk是量测噪声，其方差矩阵为Qk；
2.18)EKF的系统误差状态为：
δX(k,k)＝δX(k,k-1)+Kk[Zk-HδX(k,k-1)]
2.19)EKF的系统状态方程为：
δX(k,k-1)＝ΦkδX(k-1,k-1)+wk-1
δX(k,k-1)是k时刻的预测误差状态，δX(k-1,k-1)是k-1时刻的估计误差状态。wk-1是k-1时刻的系统噪声，其方差矩阵是Rk；
2.20)Φk是15阶的误差状态转移矩阵：
2.21)是的反对称矩阵：
2.22)EKF的状态误差向量为：
其中，δXk|k为k时刻的系统状态误差，和分别表示k时刻加速度三维误差值、角速度三维误差值、位置三维误差值、速度三维误差值和姿态三维误差值；
2.23)在静态初始对准时刻，位置和速度的增量都为零，修正的量测值为：
其中，ψk|k-1为k时刻未修正的航向角，为k时刻由磁力计计算得到的航向角，和分别为k时刻陀螺仪的角速度和地球自转角速度在n系的投影，和分别为k时刻的位置和初始的位置；
2.24)相对应的量测矩阵为：
H＝[010×2 Ι10×10 010×3]
2.25)滤波增益方程：
Kk＝Pk|k-1Hk′(HkPk|k-1Hk′+Rk)-1
2.26)一步预测均方误差方程：
Pk|k-1＝Φk-1Pk-1|k-1Φk-1 T+Qk-1
2.27)估计均方误差方程：
Pk|k＝(I15×15-KkH)Pk|k-1(I15×15-KkH)′+KkRkK'k
其中，下标k|k-1表示由k-1时刻到k时刻的预测值，下标k|k表示k时刻的估计值；
2.28)依次循环遍历步骤2.1)～2.27)，从而获取精准的初始姿态、状态估计误差和协方差矩阵；进入步骤3)。
3)步态检测
3.1)k时刻加速度的振幅：
3.2)k时刻加速度的均值：
其中，代表k时刻平均加速度振幅，s＝15为平滑窗口大小；
3.3)加速度的振幅方差：
3.4)角速度的振幅：
3.5)角速度的均值：
3.6)角速度的振幅方差：
3.7)步伐状态判定阈值：
3.8)循环遍历步骤3.1)～3.7)，将从平稳进入移动的时刻，记为ki时刻；进入步骤4)。
4)ZUPT与磁力计
4.1)时间间隔Δt＝1/f，f为采样频率；
4.2)采样样本数nsample＝f·t，t初始对准时长；
4.3)样本间隔时间矩阵I为单位矩阵；
4.4)速度间隔时间矩阵
4.5)增量时间矩阵
4.6)角增量
4.7)速度增量
4.8)角增量求和：
4.9)速度增量求和：
4.10)可由步骤4.6)和步骤4.8)，求得圆锥补偿后的角增量：
4.11)可由步骤4.6)～步骤4.9)，求得划船补偿后的速度增量：
4.12)速度四元数：
4.13)加速度计y系下的矢量：
4.14)更新后的速度：
v0为初始速度；
4.15)更新后的四元数：
如果为0，则否则
4.16)更新后的位置：
r更新＝r0+(v0+v更新)/2·Δt
r0为初始位置；
4.17)判定该时刻是否为平稳进入移动的时刻，当进入ki时刻，则在该时刻采用零速度修正(ZUPT)与磁力计组合，构造量测矩阵为：
4.18)组合后的量测噪声为：
4.19)组合后的量测值为：
4.20)EKF的量测方程为：
Zk＝HδX(k,k)+nk
4.21)EKF的系统误差状态为：
δX(k,k)＝δX(k,k-1)+Kk[Zk-HδX(k,k-1)]
4.22)EKF的系统状态方程为：
δX(k,k-1)＝ΦkδX(k-1,k-1)+wk-1
4.23)滤波增益方程：
Kk＝Pk|k-1Hk′(HkPk|k-1Hk′+Rk)-1
4.24)一步预测均方误差方程：
Pk|k-1＝Φk-1Pk-1|k-1Φk-1 T+Qk-1
4.25)估计均方误差方程：
Pk|k＝(I15×15-KkH)Pk|k-1(I15×15-KkH)′+KkRkK'k
4.26)根据δX(k,k)值，更新速度、位置和姿态信息；
4.27)循环遍历步骤4.1)～步骤4.26)，可得行人行走过程中的速度、位置和姿态。
本发明的有益效果
本发明提出并实现了基于低成本多传感器融合行人航位推算方法；该方法的优势在于使用低成本低精度多传感器，同样可以获得较理想的室内定位精度，验证了该发明的实用性和通用性。利用惯性测量单元采集行人线运动信息和角运动信息，三轴磁力计获取相应的感应电势。基于扩展卡尔曼滤波对各传感器误差补偿和修正，而后计算出行人准确的位置信息。
附图说明
图1本发明实施例中的流程图；
图2是图1中的初始对准图；
图3是图1中的捷联惯性导航图；
图4是图1中的步态检测图；
图5本发明实施例中实验结果对比图。
具体实施方式
下面结合附图和实施例对本发明做进一步的说明。
实施例1
基于低成本多传感器融合行人航位推算方法，包括下列步骤：
本发明完整的流程图见图1；
步骤一：初始粗对准获取姿态
1.1)若i表示惯性坐标系，e表示地球坐标系。表示e系相对i系的角速度在b系下的分量。表示b系相对e系的角速度在b系下的分量,表示b系相对i系的加速度在b系下的分量。静态初始时刻速度为0，加速度计的输出为：
1.2)俯仰角θ和横滚角φ分别为：
1.3)采用三轴磁力计来计算航向角：
mx、my和mz分别代表磁力计三个方向的输出值，所以航向角为：
ψ＝Δψ+D
D为本地磁偏角；
1.4)利用初始姿态角获取方向余弦矩阵，g是本地重力加速度，b表示载体坐标系，n表示导航坐标系(选择北西天地理坐标系)，表示从导航坐标系到载体坐标系的方向余弦矩阵；
gn＝[0 0 -g]′
1.5)四元数与姿态矩阵的关系：
联立步骤1.4)和步骤1.5)可解得：
q＝[q0 q1 q2 q3]
1.6)对四元数进行共轭处理：
1.7)陀螺转子相对地球的自转角速率：
1.8)陀螺转子四元数：
1.9)陀螺仪y系下的矢量：
1.10)初始时刻角增量的偏差：
1.11)加速度计四元数：
1.12)加速度计y系下的矢量：
1.13)初始时刻速度计的偏差：
1.14)转入步骤2)
步骤二：初始精对准获取姿态、状态估计误差、协方差矩阵(详细技术流程如图2所示)
遍历静止时每一时刻的加速度、角速率、时间间隔和加速度漂移，根据子样进行角速度和加速度拟合,更新系统的角增量和速度增量；
2.1)时间间隔Δt＝1/f，f为采样频率；
2.2)采样样本数nsample＝f·t，t初始对准时长；
2.3)样本间隔时间矩阵I为单位矩阵；
2.4)速度间隔时间矩阵
2.5)增量时间矩阵
2.6)角增量
2.7)速度增量
2.8)角增量求和
2.9)速度增量求和
2.10)可由步骤2.6)和步骤2.8)，求得圆锥补偿后的角增量：
2.11)可由步骤2.6)～步骤2.9)，求得划船补偿后的速度增量：
2.12)速度四元数：
2.13)加速度计y系下的矢量：
2.14)更新后的速度：
v0为初始速度；
2.15)更新后的四元数
如果为0，则否则
2.16)更新后的位置：
r更新＝r0+(v0+v更新)/2·Δt
r0为初始位置；
2.17)EKF的量测方程为：
Zk＝HδX(k,k)+nk
Zk是量测值，H是量测矩阵，nk是量测噪声，其方差矩阵为Qk；
2.18)EKF的系统误差状态为：
δX(k,k)＝δX(k,k-1)+Kk[Zk-HδX(k,k-1)]
2.19)EKF的系统状态方程为：
δX(k,k-1)＝ΦkδX(k-1,k-1)+wk-1
δX(k,k-1)是k时刻的预测误差状态，δX(k-1,k-1)是k-1时刻的估计误差状态。wk-1是k-1时刻的系统噪声，其方差矩阵是Rk；
2.20)Φk是15阶的误差状态转移矩阵：
2.21)是的反对称矩阵：
2.22)EKF的状态误差向量为：
其中，δXk|k为k时刻的系统状态误差，和分别表示k时刻加速度三维误差值、角速度三维误差值、位置三维误差值、速度三维误差值和姿态三维误差值；
2.23)在静态初始对准时刻，位置和速度的增量都为零，修正的量测值为：
其中，ψk|k-1为k时刻未修正的航向角，为k时刻由磁力计计算得到的航向角，和分别为k时刻陀螺仪的角速度和地球自转角速度在n系的投影，和分别为k时刻的位置和初始的位置；
2.24)相对应的量测矩阵为：
H＝[010×2 Ι10×10 010×3]
2.25)滤波增益方程：
Kk＝Pk|k-1Hk′(HkPk|k-1Hk′+Rk)-1
2.26)一步预测均方误差方程：
Pk|k-1＝Φk-1Pk-1|k-1Φk-1 T+Qk-1
2.27)估计均方误差方程：
Pk|k＝(I15×15-KkH)Pk|k-1(I15×15-KkH)′+KkRkK'k
其中，下标k|k-1表示由k-1时刻到k时刻的预测值，下标k|k表示k时刻的估计值；
2.28)依次循环遍历步骤2.1)～2.27)，从而获取精准的初始姿态、状态估计误差和协方差矩阵；进入步骤3)。
步骤三：确定步伐状态(详细判定如图3所示)
3.1)k时刻加速度的振幅：
3.2)k时刻加速度的均值：
其中，代表k时刻平均加速度振幅，s＝15为平滑窗口大小；
3.3)加速度的振幅方差：
3.4)角速度的振幅：
3.5)角速度的均值：
3.6)角速度的振幅方差：
3.7)步伐状态判定阈值：
3.8)循环遍历步骤3.1)～3.7)，将从平稳进入移动的时刻，记为ki时刻；进入步骤4)。
步骤四：基于EKF求取更新后每一步的位置(详细技术流程如图4所示)
4.1)时间间隔Δt＝1/f，f为采样频率；
4.2)采样样本数nsample＝f·t，t初始对准时长；
4.3)样本间隔时间矩阵I为单位矩阵；
4.4)速度间隔时间矩阵
4.5)增量时间矩阵
4.6)角增量
4.7)速度增量
4.8)角增量求和：
4.9)速度增量求和：
4.10)可由步骤4.6)和步骤4.8)，求得圆锥补偿后的角增量：
4.11)可由步骤4.6)～步骤4.9)，求得划船补偿后的速度增量：
4.12)速度四元数：
4.13)加速度计y系下的矢量：
4.14)更新后的速度：
v0为初始速度；
4.15)更新后的四元数：
如果为0，则否则
4.16)更新后的位置：
r更新＝r0+(v0+v更新)/2·Δt
r0为初始位置；
4.17)判定该时刻是否为平稳进入移动的时刻，当进入ki时刻，则在该时刻采用零速度修正(ZUPT)与磁力计组合，构造量测矩阵为：
4.18)组合后的量测噪声为：
4.19)组合后的量测值为：
4.20)EKF的量测方程为：
Zk＝HδX(k,k)+nk
4.21)EKF的系统误差状态为：
δX(k,k)＝δX(k,k-1)+Kk[Zk-HδX(k,k-1)]
4.22)EKF的系统状态方程为：
δX(k,k-1)＝ΦkδX(k-1,k-1)+wk-1
4.23)滤波增益方程：
Kk＝Pk|k-1Hk′(HkPk|k-1Hk′+Rk)-1
4.24)一步预测均方误差方程：
Pk|k-1＝Φk-1Pk-1|k-1Φk-1 T+Qk-1
4.25)估计均方误差方程：
Pk|k＝(I15×15-KkH)Pk|k-1(I15×15-KkH)′+KkRkK'k
4.26)根据δX(k,k)值，更新速度、位置和姿态信息；
4.27)循环遍历步骤4.1)～步骤4.26)，可得行人行走过程中的速度、位置和姿态。
实施例2
为了评估“基于低成本多传感器融合行人航位推算方法”在实际中定位情况，使用实施例1中的技术流程，使用的设备为苹果公司于2014年10月推出的iPad air 2产品。实验在室内的长方形会议厅内完成的，测试路线长度为50.4m，设置采样频率为100Hz，通过手持iPad方式行走采集数据，原始实验数据可以通过SensorLog软件获得。
实验绕着闭合路线行走，其中包含了3个90°的直角转弯，图5为某一次实验路线与真实路线的对比，由图可见在一开始直线行走过程中，实验算法路线与真实路线基本吻合，但当经过第二个90°转弯，ZUPT算法的方向发生了很大的偏移，慢慢地由于误差积累，最终造成后期实验位置与真实位置存在较大的偏差。
本实验路线利用实验终点和真实终点的距离误差与总路程的比值来表示定位误差。经过多次实验定位误差如表1所示。
表1定位误差分布
从表中结果对比可知，基于EKF、ZUPT与磁力计，通过惯性测量单元与磁力计融合，利用加速度计和磁力计对陀螺仪进行误差修正，并且对航向角误差修正；最终减小了航向角误差和速度误差积累，从而提高位置精度。

Claims (1)

1.一种基于低成本多传感器融合行人航位推算方法，其特征在于，包括下列步骤：
1)静态粗对准
1.1)i表示惯性坐标系，e表示地球坐标系，表示e系相对i系的角速度在b系下的分量，表示b系相对e系的角速度在b系下的分量，表示b系相对i系的加速度在b系下的分量，静态初始时刻速度为0，加速度计的输出为：
1.2)俯仰角θ和横滚角φ分别为：
1.3)采用三轴磁力计来计算航向角：
mx、my和mz分别代表磁力计三个方向的输出值，所以航向角为：
ψ＝Δψ+D
D为本地磁偏角；
1.4)利用初始姿态角获取方向余弦矩阵，g是本地重力加速度，b表示载体坐标系，n表示导航坐标系，表示从导航坐标系到载体坐标系的方向余弦矩阵；
gn＝[0 0 -g]′
1.5)四元数与姿态矩阵的关系：
联立步骤1.4)和步骤1.5)可解得：
q＝[q0 q1 q2 q3]
1.6)对四元数进行共轭处理：
1.7)陀螺转子相对地球的自转角速率：
1.8)陀螺转子四元数：
1.9)陀螺仪y系下的矢量：
1.10)初始时刻角增量的偏差：
1.11)加速度计四元数：
1.12)加速度计y系下的矢量：
1.13)初始时刻速度计的偏差：
1.14)转入步骤2)；
2)静态精对准
遍历静止时每一时刻的加速度、角速率、时间间隔和加速度漂移，根据子样进行角速度和加速度拟合,更新系统的角增量和速度增量；
2.1)时间间隔Δt＝1/f，f为采样频率；
2.2)采样样本数nsample＝f·t，t初始对准时长；
2.3)样本间隔时间矩阵I为单位矩阵；
2.4)速度间隔时间矩阵
2.5)增量时间矩阵
2.6)角增量
2.7)速度增量
2.8)角增量求和
2.9)速度增量求和
2.10)可由步骤2.6)和步骤2.8)，求得圆锥补偿后的角增量：
2.11)可由步骤2.6)～步骤2.9)，求得划船补偿后的速度增量：
2.12)速度四元数：
2.13)加速度计y系下的矢量：
2.14)更新后的速度：
v0为初始速度；
2.15)更新后的四元数
如果||▽ω||为0，则q▽ω＝[1 0 0 0]′；否则q▽ω＝[cos(||▽ω||/2)sin(||▽ω||/2)/||▽ω||·▽ω]；
2.16)更新后的位置：
r更新＝r0+(v0+v更新)/2·Δt
r0为初始位置；
2.17)EKF的量测方程为：
Zk＝HδX(k,k)+nk
Zk是量测值，H是量测矩阵，nk是量测噪声，其方差矩阵为Qk；
2.18)EKF的系统误差状态为：
δX(k,k)＝δX(k,k-1)+Kk[Zk-HδX(k,k-1)]
2.19)EKF的系统状态方程为：
δX(k,k-1)＝ΦkδX(k-1,k-1)+wk-1
δX(k,k-1)是k时刻的预测误差状态，δX(k-1,k-1)是k-1时刻的估计误差状态，wk-1是k-1时刻的系统噪声，其方差矩阵是Rk；
2.20)Φk是15阶的误差状态转移矩阵：
2.21)是的反对称矩阵：
2.22)EKF的状态误差向量为：
其中，δXk|k为k时刻的系统状态误差，和分别表示k时刻加速度三维误差值、角速度三维误差值、位置三维误差值、速度三维误差值和姿态三维误差值；
2.23)在静态初始对准时刻，位置和速度的增量都为零，修正的量测值为：
其中，ψk|k-1为k时刻未修正的航向角，为k时刻由磁力计计算得到的航向角，和分别为k时刻陀螺仪的角速度和地球自转角速度在n系的投影，和分别为k时刻的位置和初始的位置；
2.24)相对应的量测矩阵为：
H＝[010×2 Ι10×10 010×3]
2.25)滤波增益方程：
Kk＝Pk|k-1Hk′(HkPk|k-1Hk′+Rk)-1
2.26)一步预测均方误差方程：
Pk|k-1＝Φk-1Pk-1|k-1Φk-1 T+Qk-1
2.27)估计均方误差方程：
Pk|k＝(I15×15-KkH)Pk|k-1(I15×15-KkH)′+KkRkK'k
其中，下标k|k-1表示由k-1时刻到k时刻的预测值，下标k|k表示k时刻的估计值；
2.28)依次循环遍历步骤2.1)～2.27)，从而获取精准的初始姿态、状态估计误差和协方差矩阵；进入步骤3)；
3)步态检测
3.1)k时刻加速度的振幅：
3.2)k时刻加速度的均值：
其中，代表k时刻平均加速度振幅，s＝15为平滑窗口大小；
3.3)加速度的振幅方差：
3.4)角速度的振幅：
3.5)角速度的均值：
3.6)角速度的振幅方差：
3.7)步伐状态判定阈值：
3.8)循环遍历步骤3.1)～3.7)，将从平稳进入移动的时刻，记为ki时刻；进入步骤4)；
4)ZUPT与磁力计
4.1)时间间隔Δt＝1/f，f为采样频率；
4.2)采样样本数nsample＝f·t，t初始对准时长；
4.3)样本间隔时间矩阵I为单位矩阵；
4.4)速度间隔时间矩阵
4.5)增量时间矩阵
4.6)角增量
4.7)速度增量
4.8)角增量求和：
4.9)速度增量求和：
4.10)可由步骤4.6)和步骤4.8)，求得圆锥补偿后的角增量：
4.11)可由步骤4.6)～步骤4.9)，求得划船补偿后的速度增量：
4.12)速度四元数：
4.13)加速度计y系下的矢量：
4.14)更新后的速度：
v0为初始速度；
4.15)更新后的四元数：
如果||▽ω||为0，则q▽ω＝[1 0 0 0]′；否则q▽ω＝[cos(||▽ω||/2)sin(||▽ω||/2)/||▽ω||·▽ω]；
4.16)更新后的位置：
r更新＝r0+(v0+v更新)/2·Δt
r0为初始位置；
4.17)判定该时刻是否为平稳进入移动的时刻，当进入ki时刻，则在该时刻采用零速度修正与磁力计组合，构造量测矩阵为：
4.18)组合后的量测噪声为：
4.19)组合后的量测值为：
4.20)EKF的量测方程为：
Zk＝HδX(k,k)+nk
4.21)EKF的系统误差状态为：
δX(k,k)＝δX(k,k-1)+Kk[Zk-HδX(k,k-1)]
4.22)EKF的系统状态方程为：
δX(k,k-1)＝ΦkδX(k-1,k-1)+wk-1
4.23)滤波增益方程：
Kk＝Pk|k-1Hk′(HkPk|k-1Hk′+Rk)-1
4.24)一步预测均方误差方程：
Pk|k-1＝Φk-1Pk-1|k-1Φk-1 T+Qk-1
4.25)估计均方误差方程：
Pk|k＝(I15×15-KkH)Pk|k-1(I15×15-KkH)′+KkRkK'k
4.26)根据δX(k,k)值，更新速度、位置和姿态信息；
4.27)循环遍历步骤4.1)～步骤4.26)，可得行人行走过程中的速度、位置和姿态。
CN201610415776.9A 2016-06-13 2016-06-13 基于低成本多传感器融合行人航位推算方法 Expired - Fee Related CN106225784B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN201610415776.9A CN106225784B (zh) 2016-06-13 2016-06-13 基于低成本多传感器融合行人航位推算方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201610415776.9A CN106225784B (zh) 2016-06-13 2016-06-13 基于低成本多传感器融合行人航位推算方法

Publications (2)

Publication Number Publication Date
CN106225784A CN106225784A (zh) 2016-12-14
CN106225784B true CN106225784B (zh) 2019-02-05

Family

ID=57519668

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201610415776.9A Expired - Fee Related CN106225784B (zh) 2016-06-13 2016-06-13 基于低成本多传感器融合行人航位推算方法

Country Status (1)

Country Link
CN (1) CN106225784B (zh)

Families Citing this family (17)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN107084718A (zh) * 2017-04-14 2017-08-22 桂林电子科技大学 基于行人航迹推算的室内定位方法
CN108801238B (zh) * 2017-04-27 2021-08-13 电子科技大学中山学院 一种基于gpx的路径优化方法
CN107255474B (zh) * 2017-05-11 2020-06-05 杭州电子科技大学 一种融合电子罗盘和陀螺仪的pdr航向角确定方法
CN107270896A (zh) * 2017-06-20 2017-10-20 华中科技大学 一种行人定位与轨迹跟踪方法和系统
CN107289933B (zh) * 2017-06-28 2019-08-20 东南大学 基于mems传感器和vlc定位融合的双卡尔曼滤波导航装置和方法
CN107289932B (zh) * 2017-06-28 2019-08-20 东南大学 基于mems传感器和vlc定位融合的单卡尔曼滤波导航装置和方法
CN107643535A (zh) * 2017-08-17 2018-01-30 桂林电子科技大学 一种室内外定位导航系统及方法
CN107607119B (zh) * 2017-08-25 2020-06-26 北京麦钉艾特科技有限公司 一种基于空间环境磁场特征的无源组合定位装置
CN107976187B (zh) * 2017-11-07 2020-08-04 北京工商大学 一种融合imu和视觉传感器的室内轨迹重建方法及系统
CN108279618A (zh) * 2018-03-28 2018-07-13 北京钢铁侠科技有限公司 一种用于机器人的控制器、控制方法及机器人
CN109141410B (zh) * 2018-07-25 2020-09-01 深圳市集大自动化有限公司 Agv组合导航的多传感器融合定位方法
CN109827545B (zh) * 2019-03-22 2020-12-29 北京壹氢科技有限公司 一种基于双mems加速度计的在线倾角测量方法
CN110375741B (zh) * 2019-07-09 2021-08-17 中移（杭州）信息技术有限公司 行人航位推算方法和终端
CN111551174A (zh) * 2019-12-18 2020-08-18 无锡北微传感科技有限公司 基于多传感器惯性导航系统的高动态车辆姿态计算方法及系统
CN111189443B (zh) * 2020-01-14 2022-11-11 电子科技大学 一种在线校准步长、修正运动偏差角和自适应能量管理的行人导航方法
CN112985390B (zh) * 2021-02-20 2021-10-01 中南大学 一种基于磁强计辅助的步态检测方法
CN114132358B (zh) * 2021-10-29 2023-03-14 北京自动化控制设备研究所 一种多平台智能化轨道综合检测系统

Citations (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
EP1221586A3 (en) * 2001-01-08 2004-06-16 Motorola, Inc. Position and heading error-correction method and apparatus for vehicle navigation systems
JP2008215917A (ja) * 2007-03-01 2008-09-18 Alpine Electronics Inc 位置検出装置および位置検出方法
CN101949715A (zh) * 2010-08-10 2011-01-19 武汉武大卓越科技有限责任公司 高精度时空数据获取的多传感器集成同步控制方法和系统
CN103885076A (zh) * 2014-03-06 2014-06-25 华南农业大学 基于gps的农业机械导航的多传感器信息融合方法
CN104757976A (zh) * 2015-04-16 2015-07-08 大连理工大学 一种基于多传感器融合的人体步态分析方法和系统

Patent Citations (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
EP1221586A3 (en) * 2001-01-08 2004-06-16 Motorola, Inc. Position and heading error-correction method and apparatus for vehicle navigation systems
JP2008215917A (ja) * 2007-03-01 2008-09-18 Alpine Electronics Inc 位置検出装置および位置検出方法
CN101949715A (zh) * 2010-08-10 2011-01-19 武汉武大卓越科技有限责任公司 高精度时空数据获取的多传感器集成同步控制方法和系统
CN103885076A (zh) * 2014-03-06 2014-06-25 华南农业大学 基于gps的农业机械导航的多传感器信息融合方法
CN104757976A (zh) * 2015-04-16 2015-07-08 大连理工大学 一种基于多传感器融合的人体步态分析方法和系统

Also Published As

Publication number Publication date
CN106225784A (zh) 2016-12-14

Similar Documents

Publication Publication Date Title
CN106225784B (zh) 基于低成本多传感器融合行人航位推算方法
CN108225308B (zh) 一种基于四元数的扩展卡尔曼滤波算法的姿态解算方法
CN109459044B (zh) 一种gnss双天线辅助的车载mems惯导组合导航方法
Zhang et al. Inertial sensor based indoor localization and monitoring system for emergency responders
CN103196448B (zh) 一种机载分布式惯性测姿系统及其传递对准方法
Le Grand et al. 3-axis magnetic field mapping and fusion for indoor localization
US8005635B2 (en) Self-calibrated azimuth and attitude accuracy enhancing method and system (SAAAEMS)
Jiménez et al. Indoor pedestrian navigation using an INS/EKF framework for yaw drift reduction and a foot-mounted IMU
JP4989035B2 (ja) 慣性ナビゲーションシステムの誤差補正
CN103808331B (zh) 一种mems三轴陀螺仪误差标定方法
CN103090870B (zh) 一种基于mems传感器的航天器姿态测量方法
CN100516775C (zh) 一种捷联惯性导航系统初始姿态确定方法
CN103900565B (zh) 一种基于差分gps的惯导系统姿态获取方法
CN109813311A (zh) 一种无人机编队协同导航方法
CN106908759A (zh) 一种基于uwb技术的室内行人导航方法
Ali et al. Low‐cost MEMS‐based pedestrian navigation technique for GPS‐denied areas
CN110886606A (zh) 一种随钻特征量辅助的惯性测斜方法及装置
EP3460399A1 (en) Methods, apparatuses, and computer programs for estimating the heading of an axis of a rigid body
CN104406586A (zh) 基于惯性传感器的行人导航装置和方法
US9677888B2 (en) Determining sensor orientation in indoor navigation
CN108195400A (zh) 捷联式微机电惯性导航系统的动基座对准方法
CN109186597A (zh) 一种基于双mems-imu的室内轮式机器人的定位方法
CN113465599B (zh) 定位定向方法、装置及系统
CN110954102A (zh) 用于机器人定位的磁力计辅助惯性导航系统及方法
CN108871325B (zh) 一种基于两层扩展卡尔曼滤波的WiFi/MEMS组合室内定位方法

Legal Events

Date Code Title Description
C06 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
CF01 Termination of patent right due to non-payment of annual fee
CF01 Termination of patent right due to non-payment of annual fee

Granted publication date: 20190205

Termination date: 20210613