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

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

【技术实现步骤摘要】

本专利技术涉及计算机科学、控制科学、室内定位系统、惯性导航系统等

技术介绍
行人航位推算(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，加速度计的输出为： a i b b = v b + ( 2 ω i e b + ω e b b ) × v b - g b = - g b = - C n b g n = g · - s i n θ s i n φ c o s θ cos φ c o s θ ]]>1.2)俯仰角θ和横滚角φ分别为： θ = - a r c s i n ( a i b x b / g ) ]]> φ = a r c s i n ( a i b y b / ( g · c o s θ ) ) ]]>1.3)采用三轴磁力计来计算航向角： t a n Δ ψ = m y c o s φ - m z m x c o s θ + m y sin φ + m z c o s φ ]]>mx、my和mz分别代表磁力计三个方向的输本文档来自技高网...

【技术保护点】
一种基于低成本多传感器融合行人航位推算方法，其特征在于，包括下列步骤：1)静态粗对准1.1)i表示惯性坐标系，e表示地球坐标系，表示e系相对i系的角速度在b系下的分量，表示b系相对e系的角速度在b系下的分量，表示b系相对i系的加速度在b系下的分量，静态初始时刻速度为0，加速度计的输出为：aibb=vb+(2ωieb+ωebb)×vb-gb=-gb=-Cnbgn=g·-sinθsinφcosθcosφcosθ]]>1.2)俯仰角θ和横滚角φ分别为：θ=-arc sin(aibxb/g)]]>φ=arc sin(aibyb/(g·cosθ))]]>1.3)采用三轴磁力计来计算航向角：tanΔψ=mycosφ-mzmxcosθ+mysinφ+mzcosφ]]>mx、my和mz分别代表磁力计三个方向的输出值，所以航向角为：ψ＝Δψ+DD为本地磁偏角；1.4)利用初始姿态角获取方向余弦矩阵，g是本地重力加速度，b表示载体坐标系，n表示导航坐标系，表示从导航坐标系到载体坐标系的方向余弦矩阵；Cnb=cosθcosψcosθsinψ-sinθ-cosφsinψ+sinφsinθcosψcosθcosψ+sinφsinθsinψsinφcosθsinφsinψ+cosφsinθcosψ-sinφcosψ+cosφsinθcosψcosφcosθ]]>gn＝[0 0 ‑g]′1.5)四元数与姿态矩阵的关系：Cnb=q02+q12-q22-q322(q1q2-q0q3)2(q1q3+q0q2)2(q1q2+q0q3)q02-q12+q22-q322(q2q3-q0q1)2(q1q3-q0q2)2(q2q3+q0q1)q02-q12-q22+q32]]>联立步骤1.4)和步骤1.5)可解得：q0=12·1+Cnb(1,1)-Cnb(2,2)-Cnb(3,3)]]>q1=12·1-Cnb(1,1)+Cnb(2,2)-Cnb(3,3)]]>q2=12·1-Cnb(1,1)-Cnb(2,2)+Cnb(3,3)]]>q3=12·1+Cnb(1,1)+Cnb(2,2)+Cnb(3,3)]]>q＝[q0 q1 q2 q3]1.6)对四元数进行共轭处理：q‾=q0-q1-q2-q3]]>1.7)陀螺转子相对地球的自转角速率：ωien=ωie·cosL0ωie·sinL′]]>1.8)陀螺转子四元数：q~ω=q‾⊗0ωien]]>1.9)陀螺仪y系下的矢量：ωiey=q⊗q~ω]]>1.10)初始时刻角增量的偏差：ϵω=ω-ωiey(2,1)ωiey(3,1)ωiey(4,1)]]>1.11)加速度计四元数：q~a=q‾⊗0gn]]>1.12)加速度计y系下的矢量：aiy=q⊗q~a]]>1.13)初始时刻速度计的偏差：ϵa=a-aiy(2,1)aiy(3,1)aiy(4,1)]]>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)角增量求和ωsum=Δω(1,1)+Δω(1,2)Δω(2,1)+Δω(2,2)Δω(3,1)+Δω(3,2)]]>2.9)速度增量求和vsum=...

【技术特征摘要】
1.一种基于低成本多传感器融合行人航位推算方法，其特征在于，包括下列步骤：1)静态粗对准1.1)i表示惯性坐标系，e表示地球坐标系，表示e系相对i系的角速度在b系下的分量，表示b系相对e系的角速度在b系下的分量，表示b系相对i系的加速度在b系下的分量，静态初始时刻速度为0，加速度计的输出为： a i b b = v b + ( 2 ω i e b + ω e b b ) × v b - g b = - g b = - C n b g n = g · - sin θ sin φ cos θ cos φ cos θ ]]>1.2)俯仰角θ和横滚角φ分别为： θ = - arc s i n ( a i b x b / g ) ]]> φ = arc s i n ( a i b y b / ( g · cos θ ) ) ]]>1.3)采用三轴磁力计来计算航向角： tan Δ ψ = m y cos φ - m z m x cos θ + m y sin φ + m z cos φ ]]>mx、my和mz分别代表磁力计三个方向的输出值，所以航向角为：ψ＝Δψ+DD为本地磁偏角；1.4)利用初始姿态角获取方向余弦矩阵，g是本地重力加速度，b表示载体坐标系，n表示导航坐标系，表示从导航坐标系到载体坐标系的方向余弦矩阵； C n b = cos θ cos ψ cos θ sin ψ - sin θ - cos φ sin ψ + sin φ sin θ cos ψ cos θ cos ψ + sin φ sin θ sin ψ sin φ cos θ sin φ sin ψ + cos φ sin θ cos ψ - sin φ cos ψ + cos φ sin θ cos ψ cos φ cos θ ]]>gn＝[0 0 -g]′1.5)四元数与姿态矩阵的关系： C n b = q 0 2 + q 1 2 - q 2 2 - q 3 2 2 ( q 1 q 2 - q 0 q 3 ) 2 ( q 1 q 3 + q 0 q 2 ) 2 ( q 1 q 2 + q 0 q 3 ) q 0 2 - q 1 2 + q 2 2 - q 3 2 2 ( q 2 q 3 - q 0 q 1 ) 2 ( q 1 q 3 - q 0 q 2 ) 2 ( q 2 q 3 + q 0 q 1 ) q 0 2 - q 1 2 - q 2 2 + q 3 2 ]]>联立步骤1.4)和步骤1.5)可解得： q 0 = 1 2 · 1 + C n b ( 1 , 1 ) - C n b ( 2 , 2 ) - C n b ( 3 , 3 ) ]]> q 1 = 1 2 · 1 - C n b ( 1 , 1 ) + C n b ( 2 , 2 ) - C n b ( 3 , 3 ) ]]> q 2 = 1 2 · 1 - C n b ( 1 , 1 ) - C n b ( 2 , 2 ) + C n b ( 3 , 3 ) ]]> q 3 = 1 2 · 1 + C n b ( 1 , 1 ) + C n b ( 2 , 2 ) + C n b ( 3 , 3 ) ]]>q＝[q0 q1 q2 q3]1.6)对四元数进行共轭处理： q ‾ = q 0 - q 1 - q 2 - q 3 ]]>1.7)陀螺转子相对地球的自转角速率： ω i e n = ω i e · cos L 0 ω i e · sin L ′ ]]>1.8)陀螺转子四元数： q ~ ω = q ‾ ⊗ 0 ω i e n ]]>1.9)陀螺仪y系下的矢量： ω i e y = q ⊗ q ~ ω ]]>1.10)初始时刻角增量的偏差： ϵ ω = ω - ω i e y ( 2 , 1 ) ω i e y ( 3 , 1 ) ω i e y ( 4 , 1 ) ]]>1.11)加速度计四元数： q ~ a = q ‾ ⊗ 0 g n ]]>1.12)加速度计y系下的矢量： a i y = q ⊗ q ~ a ]]>1.13)初始时刻速度计的偏差： ϵ a = a - a i y ( 2 , 1 ) a i y ( 3 , 1 ) a i y ( 4 , 1 ) ]]>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)角增量求和 ω s u m = Δ ω ( 1 , 1 ) + Δ ω ( 1 , 2 ) Δ ω ( 2 , 1 ) + Δ ω ( 2 , 2 ) Δ ω ( 3 , 1 ) + Δ ω ( 3 , 2 ) ]]>2.9)速度增量求和 v s u m = Δ v ( 1 , 1 ) + Δ v ( 1 , 2 ) Δ v ( 2 , 1 ) + Δ v ( 2 , 2 ) Δ v ( 3 , 1 ) + Δ v ( 3 , 2 ) ]]>2.10)可由步骤2.6)和步骤2.8)，求得圆锥补偿后的角增量： ▿ ω = ω s u m + 2 3 · Δ ω ( 1 , 1 ) Δ ω ( 2 , 1 ) Δ ω ( 3 , 1 ) ⊗ Δ ω ( 1 , 2 ) Δ ω ( 2 , 2 ) Δ ω ( 3 , 2 ) ]]>2.11)可由步骤2.6)～步骤2.9)，求得划船补偿后的速度增量： ▿ v = v s u m + 1 2 · v s u m ⊗ ω s u m + 2 3 · Δ ω ( 1 , 1 ) Δ ω ( 2 , 1 ) Δ ω ( 3 , 1 ) ⊗ Δ v ( 1 , 2 ) Δ v ( 2 , 2 ) Δ v ( 3 , 2 ) + Δ v ( 1 , 1 ) Δ v ( 2 , 1 ) Δ v ( 3 , 1 ) ⊗ Δ ω ( 1 , 2 ) Δ ω ( 2 , 2 ) Δ ω ( 3 , 2 ) ]]>2.12)速度四元数： q ~ v = q ⊗ 0 ▿ v ]]>2.13)加速度计y系下的矢量： v i y = ...

【专利技术属性】
技术研发人员：罗孝文，
申请(专利权)人：国家海洋局第二海洋研究所，
类型：发明
国别省市：浙江;33