法，它包括如下步骤：
[0047] 步骤⑴：将包括三轴加速度计、三轴陀螺仪和三轴磁力计的MEMS-頂U固定连接 于人体，选取北东地为导航坐标系，由经过补偿后的加速度计输出值计算获得俯仰角Θ和 滚转角Y，由磁场强度B、俯仰角Θ和滚转角γ计算获得偏航角炉，完成初始对准。对准 后，在行人运动过程中采集MU数据。
[0048] 步骤（2)，根据硬件结构及人体运动特征，建立包括3个位置误差、3个姿态角误差 (俯仰角Θ、滚转角Y和偏航角P )、3个速度误差、3个重力不确定误差三轴陀螺仪零偏误 差和三轴加速度计零偏误差，共计18维的状态空间方程，即滤波模型。
[0050] W(t)为系统过程噪声矩阵，其表达式为
行人坐标系（b系）到导航坐标系（η系）的方向余弦矩阵，〇/为行人角速度，abS行人加 速度；G(t)为对应的噪声矩阵系数；F(t)为系统状态矩阵；Z(t)为观测量；H(t)为观测矩 阵；V(t)为观测噪声矩阵。
[0051] 被估计状态向量为：
[0052] 其中，δ r为位置误差，δ V为速度误差，δ ψ为姿态角误差，1V4为加速度计零偏 误差，eb为陀螺仪零偏误差，Sg为重力不确定误差。
[0053] 人体在正常运动过程中，与地面接触时间为毫秒级或更长，当检测到人体与地面 接触达到瞬间静止时，其输出速度、角速度为零，但这只是理论上。由于传感器的测量误差、 噪声以及算法误差等原因，此时速度、角速度计算值并不为零。因此选取瞬间静止时刻速 度计算值的误差和陀螺仪的角速度输出误差作为观测量。为了增加可观性，同时将磁力计 计算的航向角与捷联解算出的航向角作差获得的航向角误差，利用加速度计计算出的俯仰 角和横滚角与捷联解算出的姿态角作差获得的俯仰角误差和横滚角误差同样作为观测量。
[0054] 观测向量和观测矩阵为：
[0057] 步骤（3)，同时满足"四条件"时进行自适应卡尔曼滤波，完成各状态量的估计。
[0058] 进一步的，"四条件"具体为：
[0059] (1)三轴加速度计合成的加速度幅值输出量满足阈值条件；
[0060] (2)加速度计z轴加速度输出量满足阈值条件；
[0061] (3)三轴陀螺仪合成的角速度幅值输出量满足阈值条件；
[0062] (4)陀螺仪y轴角速度输出量满足阈值条件；
[0063] 当上述四项条件同时满足时，进行自适应滤波。
[0064] 其中，条件（1)中三轴加速度计合成的加速度幅值表示为
，定义给定的阈值条件为th amin= 9m/ s2< I a k I < th_x= Ilm/s 2。条件（3)中三轴陀螺仪合成的角速度幅值表示为
，定义给定的阈值条件为I ?k| < th"max= 50° /s〇
[0065] 在行人运动过程中，z轴加速度和y轴角速度变化最明显，由于安装偏差、行走晃 动等影响，頂U会有所倾斜。因此在静止时刻y轴角速度不是完全零值，z轴加速度也不是 完全g值，但都不会大于所设定阈值。条件（2)和条件（4)中的阈值在初始静止阶段分别 以取加速度计输出的平均值和陀螺仪输出的平均值而设定。
[0066] 进一步的，如图3自适应卡尔曼滤波框图所示。其中包括捷联惯性导航信息 (3-1)、磁力计的地磁信息（3-2)、时变噪声统计估计器（3-3)和滤波增益（3-4)。自适应卡 尔曼滤波是利用观测数据进行递推滤波的同时，通过时变噪声统计估计器实时估计和修正 系统噪声以及观测噪声的统计特性，从而降低模型误差、抑制滤波发散，提高滤波精度。 [0067]自适应滤波的算法描述为：
[0069] 上式中，A、分别是k时刻系统状态Xk的当前和预测线性最小方差估计 值，文^是k-Ι时刻的系统状态Xk i的当前线性最小方差估计值；Φ k, k i是状态过渡矩阵， ?^^,是〇k,k i的转置矩阵；Hk是观测矩阵，叾,是观测量，%是观测量估计误差；&是滤波增 益；Pk、Pk/k i分别是X # X k/k i对应的状态估计协方差阵；、忿分别是系统过程噪声和 观测噪声的时变均值，、^^分别是系统过程噪声和观测噪声的时变协方差阵。
[0070] 其中，&、I、由时变噪声统计估值器递推获得：
[0072] 式中，
>0 <b < 1为遗忘因子，针对本方法取b = 0.98。
[0073] 此处分母中的bk+1表示b的（k+Ι)次幂。k为离散时间点，k = 1，2,3···Ν，Ν为采 样次数。
[0074] 步骤（4)，通过公式
·反馈修正行人速 度、位置和姿态信息。
[0075] 如图4为行人自主导航定位的自适应卡尔曼滤波方法的实施例结果对比图：
[0076] 实验中，将自研的集成三轴加速度计、陀螺仪和磁力计的MEMS-頂U固定于人体脚 部，其中各传感器参数如表1所示。
[0077] 表1传感器参数
[0078]
[0079] 实验中，行人绕某花坛外侧行走一圈，约230m。图4给出了利用本发明行人自主 导航定位的自适应卡尔曼滤波方法进行行人导航的结果，并与只有纯惯导时的轨迹结果 图作比较。其中左侧为纯惯性导航时的轨迹，可看出轨迹发散情况严重。右侧为应用自适 应卡尔曼滤波后的轨迹图，可以看出此方法的收敛效果极好，最终得到了较为平滑并且始 末点重复性较高的行进轨迹图。经过多个人多种运动速度试验，此发明的水平定位误差为 1.35% (3〇)。此实施例表明本发明行人自主导航定位的自适应卡尔曼滤波方法可显著提 高行人导航定位的精度。
【主权项】
1. 一种行人自主导航定位的自适应卡尔曼滤波方法，其特征在于：包括以下步骤： 步骤（1)，将MEMS-頂U系统固定连接于人体，頂U初始对准后，行人开始运动，运动过程 中采集IMU数据； 步骤（2)，合理选取观测量建立融合人体运动特征的自适应卡尔曼滤波模型； 步骤（3)，同时满足"四条件"时进行自适应卡尔曼滤波，同步修正晃动噪声干扰，完成 各状态量的估计。 步骤（4)，利用估计误差修正运动人体信息。2. 根据权利要求1所述的行人自主导航定位的自适应卡尔曼滤波方法，其特征在于： 步骤（1)中所述的初始对准选取北东地为导航坐标系，俯仰角e和滚转角Y由经过补偿 后的加速度计输出值计算获得，偏航角识由磁场强度B、俯仰角0和滚转角Y计算获得。3. 根据权利要求1所述的行人自主导航定位的自适应卡尔曼滤波方法，其特征在于： 在步骤（2)中，滤波模型为包括3个位置误差、3个姿态角误差（俯仰角0、滚转角Y和偏 航角0 )、3个速度误差、3个重力不确定误差、三轴陀螺仪零偏误差和三轴加速度计零偏误 差，共计18维的状态空间方程。W(t)为系统过程噪声矩阵，其表达式为⑴=[一C6V 其中C；为行人 坐标系b系到导航坐标系n系的方向余弦矩阵，〇/为行人角速度，ab为行人加速度；G(t) 为对应的噪声矩阵系数；F(t)为系统状态矩阵；Z(t)为观测量；H(t)为观测矩阵；V(t)为 观测噪声矩阵。其中，Sr为位置误差，5v为速度误差，5Ijr为姿态角误差,Va为加速度计零偏误差，eb为陀螺仪零偏误差，Sg为重力不确定误差。 观测向量和观测矩阵为： Zk= [AvkAokAIirJ1其中，AVk包括磁力计计算的航向角与捷联解算出的航向角作差获得的航向角误差， 利用加速度计计算出的俯仰角和横滚角与捷联解算出的姿态角作差获得的俯仰角和横滚 角误差。4. 根据权利要求1所述的行人自主导航定位的自适应卡尔曼滤波方法，其特征在于： 所述的步骤（3)中"四条件"包括： (1) 三轴加速度计合成的加速度幅值输出量满足阈值条件； (2) 加速度计z轴加速度输出量满足阈值条件； (3) 三轴陀螺仪合成的角速度幅值输出量满足阈值条件； (4)陀螺仪y轴角速度输出量满足阈值条件； 当上述四项条件同时满足时，进行自适应滤波。5. 根据权利要求1所述的行人自主导航定位的自适应卡尔曼滤波方法，其特征在于： 所述的步骤（3)中的自适应卡尔曼滤波是利用观测数据进行递推滤波的同时，通过时变噪 声统计估计器实时估计和修正系统噪声以及观测噪声的统计特性，从而降低模型误差、抑 制滤波发散，提高滤波精度。 自适应卡尔曼滤波的算法描述为：上式中，i；、分别是k时刻系统状态&的当前和预测线性最小方差估计值， 义；_|是1^1时刻的系统状态\:的当前线性最小方差估计值；Ok,kl是状态过渡矩阵， ^^^是〇k,k :的转置矩阵；Hk是观测矩阵，2,是观测量，是观测量估计误差；Kk是滤波 增益;Pk、Pk/k :分别是X,和Xk/ki对应的状态估计协方差阵；么_,、衿分别是系统过程噪声 和观测噪声的时变均值，A分别是系统过程噪声和观测噪声的时变协方差阵。 其中，Z5t、免、么、Q4由时变噪声统计估值器递推获得：此处分母中的bk+1表示b的（k+1)次幂。k为离散时间点，k= 1，2,3…N，N为采样次 数。6. 根据权利要求1所述的行人自主导航定位的自适应卡尔曼滤波方法，其特征在于： 所述的步骤（4)中，通过公式p= 〇-+况、P=户+夕和P= 反馈修正行人速 度、位置和姿态信息。
【专利摘要】本发明公开了一种行人自主导航定位的自适应卡尔曼滤波方法，包括：将一个集成了加速度计、陀螺仪、磁力计的MEMS-IMU系统连接于人体，在行人运动过程中进行IMU数据采集；建立包含18维状态量、9维观测量的自适应滤波模型，在同时满足“四条件”时进行递推滤波，其间通过时变噪声统计估计器实时估计和修正系统噪声以及观测噪声的统计特性；本发明在使用零速校正作为误差补偿修正算法的基础上，设计融合人体运动特征的自适应滤波方法，实时处理人体晃动带入的噪声干扰信号，有效提高了行人自主导航定位的精度。本发明方法稳定性强、实时性好，并且不增加任何额外硬件成本。
【IPC分类】G01C21/20, G01C21/16
【公开号】CN105043385
【申请号】CN201510300546
【发明人】高哲, 李擎, 苏中, 付国栋, 刘宁
【申请人】北京信息科技大学, 北京德维创盈科技有限公司
【公开日】2015年11月11日
【申请日】2015年6月5日