[发明专利]基于低成本多传感器融合行人航位推算方法

摘要

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

主权项

1.一种基于低成本多传感器融合行人航位推算方法，其特征在于，包括下列步骤：1)静态粗对准1.1)i表示惯性坐标系，e表示地球坐标系，表示e系相对i系的角速度在b系下的分量，表示b系相对e系的角速度在b系下的分量，表示b系相对i系的加速度在b系下的分量，静态初始时刻速度为0，加速度计的输出为：1.2)俯仰角θ和横滚角φ分别为：1.3)采用三轴磁力计来计算航向角：mx、my和mz分别代表磁力计三个方向的输出值，所以航向角为：ψ＝Δψ+DD为本地磁偏角；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·Δtr0为初始位置；2.17)EKF的量测方程为：Zk＝HδX(k,k)+nkZk是量测值，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的状态误差向量为：其中，δX_k|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)‑12.26)一步预测均方误差方程：Pk|k‑1＝Φk‑1Pk‑1|k‑1Φk‑1T+Qk‑12.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·Δtr0为初始位置；4.17)判定该时刻是否为平稳进入移动的时刻，当进入ki时刻，则在该时刻采用零速度修正与磁力计组合，构造量测矩阵为：4.18)组合后的量测噪声为：4.19)组合后的量测值为：4.20)EKF的量测方程为：Zk＝HδX(k,k)+nk4.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‑14.23)滤波增益方程：Kk＝Pk|k‑1Hk′(HkPk|k‑1Hk′+Rk)‑14.24)一步预测均方误差方程：Pk|k‑1＝Φk‑1Pk‑1|k‑1Φk‑1T+Qk‑14.25)估计均方误差方程：Pk|k＝(I15×15‑KkH)Pk|k‑1(I15×15‑KkH)′+KkRkK'k4.26)根据δX(k,k)值，更新速度、位置和姿态信息；4.27)循环遍历步骤4.1)～步骤4.26)，可得行人行走过程中的速度、位置和姿态。