基于步态特征的微惯性导航Kalman滤波器参数自适应设置方法与流程

基于步态特征的微惯性导航kalman滤波器参数自适应设置方法
技术领域
1.本发明属于基于微惯性传感器的足绑式行人导航技术领域，涉及到一种基于步态特征的微惯性导航kalman滤波器参数自适应设置方法。


背景技术：

2.传统的足绑式行人导航算法在利用kalman滤波器修正导航误差时，滤波器q矩阵和r矩阵的值都是不变的，但行人在直线走、弧线走、转弯等不同步态下的姿态误差估计精度和速度观测准确度不同，利用固定参数滤波器进行误差修正的精度会下降。


技术实现要素：

3.针对利用固定参数滤波器进行误差修正精度会下降的问题，本发明目的提供基于步态特征的微惯性导航kalman滤波器参数自适应设置方法，能够提升滤波修正的精度，从而提高定位精度。
4.为实现本发明目的，本发明提供基于步态特征的微惯性导航kalman滤波器参数自适应设置方法，采取技术方案如下：
5.将微惯性传感器绑定于行人足部，采集行人在直线走、弧线走、转弯等步态下的原始惯性数据；利用每一步零速区间内解算航向角的均值计算相邻步航向角差值；利用航向角差值判断行人步态；在行人弧线走或转弯时，利用航向角差值自适应调整系统噪声q矩阵的值，利用新息自适应调整量测噪声r矩阵的值。
6.进一步的，利用相邻步航向角差值判断行人步态，方法为：设定步态判别阈值，当相邻步航向角差值小于设定的阈值，表明行人处于直线走状态，当相邻步航向角差值大于设定的阈值，表明行人处于弧线走或转弯状态。
7.进一步的，当行人在进行弧线走或转弯机动运动时，姿态误差对应q矩阵的值自适应调整为：
[0008][0009]
其中，i＝1,2,3，qi′
为调整后q矩阵对角线元素值，为两个相邻步间的航向角差值，表示根据运动剧烈程度引入的噪声修正量，δ
base
表示修正的基底阈值，为常值。
[0010]
进一步的，当行人在进行弧线走或转弯机动运动时，利用新息对r矩阵进行自适应调整：
[0011]ri
′
＝ri+rk*rk
[0012]
其中，i＝1,2,3，ri′
为调整后r矩阵对角线元素值，同时对r矩阵进行限幅设置：
[0013][0014]
其中，thr为r矩阵限幅判别阈值。
[0015]
本发明与现有技术对比有益效果如下：
[0016]
本发明将微惯性传感器固定在人体足部，通过捷联惯性导航解算实时更新行人导航信息，利用基于速度误差观测的kalman滤波器抑制导航误差发散。为了挖掘行人直线走、弧线走、转弯等步态特征，利用相邻步间航向角的差值判断运动状态，基于步态判别结果自适应调整kalman滤波器的q矩阵和r矩阵，提升了滤波修正的精度，从而提高定位精度。本发明尤其适用于解决楼宇、巷道等城市环境中的行人高精度定位导航问题。
附图说明
[0017]
所包括的附图用来提供对本发明实施例的进一步的理解，其构成了说明书的一部分，用于例示本发明的实施例，并与文字描述一起来阐释本发明的原理。显而易见地，下面描述中的附图仅仅是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他的附图。
[0018]
图1示出了根据本发明的具体实施例提供的基于步态特征的微惯性导航kalman滤波器参数自适应设置方法基本原理框图。
具体实施方式
[0019]
需要说明的是，在不冲突的情况下，本技术中的实施例及实施例中的特征可以相互组合。下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。以下对至少一个示例性实施例的描述实际上仅仅是说明性的，决不作为对本发明及其应用或使用的任何限制。基于本发明中的实施例，本领域普通技术人员在没有作出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。
[0020]
作为本发明实施例提供的基于步态特征的微惯性导航kalman滤波器参数自适应设置方法内容如下：
[0021]
将微惯性传感器绑定于行人足部，采集行人在直线走、弧线走、转弯等步态下的原始惯性数据；利用每一步零速区间内解算航向角的均值计算相邻步航向角差值；利用航向角差值判断行人步态；在行人弧线走或转弯时，利用航向角差值自适应调整系统噪声q矩阵的值，利用新息自适应调整量测噪声r矩阵的值。
[0022]
如图1所示，本发明的具体方法包括如下步骤：
[0023]
步骤1.惯性数据采集
[0024]
将微惯性传感器绑定于行人足部，采集多种行进运动状态下的原始惯性数据，包括直线走、弧线走、转弯等动作。
[0025]
步骤2.零速区间检测
[0026]
行人在运动过程中的每一个单步都存在周期性脚面贴地的时段，该时段内行人足部的运动速度为零，被称为零速区间。利用阈值法、滑动方差法、广义似然比检测等方法找到每一个单步的零速区间。
[0027]
步骤3.相邻步航向角差值计算
[0028]
假设第k个单步的零速区间起止时刻分别为t
k,start
和t
k,end
，该零速区间内共包含nk个惯性数据采样时刻，其中i时刻的航向解算角为第k+1个单步的零速区间起止时刻分别为t
k+1,start
和t
k+1,end
，该零速区间内共包含n
k+1
个惯性数据采样时刻，其中i时刻的航向解算角为则第k步和第k+1步的航向角可以分别表示为：
[0029][0030][0031]
则第k步和第k+1步这两个相邻步间的航向角差值可以表示为：
[0032][0033]
步骤4.行人步态判别
[0034]
利用相邻步航向角差值判断行人步态。设th
heading
为步态判别阈值，当满足：
[0035][0036]
表明行人处于直线走状态。
[0037]
当满足：
[0038][0039]
表明行人处于弧线走或转弯状态。
[0040]
步骤5.kalman滤波器q矩阵自适应设置
[0041]
设kalman滤波器的系统状态向量为：
[0042][0043]
其中，δvn，δvu，δve分别表示微惯性导航系统的北向、天向、东向速度误差；δl，δh，δλ分别表示纬度误差、高度误差、经度误差；δγ，δθ分别表示滚转角误差、航向角误差、俯仰角误差。
[0044]
系统状态方程为：
[0045][0046]
其中，f(t)为状态转移矩阵，w(t)为系统噪声向量，满足：
[0047]
e[wk]＝0,cov[wk,wj]＝e[wkw
jt
]＝qδ
kj
[0048]
其中，wk、wj分别为k、j时刻的噪声向量，δ
kj
表示单位矩阵，q矩阵为kalman滤波器的系统噪声方差阵，其中，滚转角误差、航向角误差、俯仰角误差对应的对角线元素常值分别为q1、q2、q3。
[0049]
当行人在直线行走时，q1、q2、q3准确反应了常规运动条件下的器件噪声特性，其值
不变。
[0050]
当行人在进行弧线走或转弯等机动运动时，由于行人动态特性增强、运动幅度增大，惯导对瞬时运动的敏感程度增强，姿态误差估计准确度提升，对应噪声值应自适应减小。此时，姿态误差对应q矩阵的值自适应调整为：
[0051][0052]
其中，qi′
(i＝1,2,3)为调整后q矩阵对角线元素值，表示根据运动剧烈程度引入的噪声修正量，δ
base
表示修正的基底阈值，为常值。
[0053]
步骤6.kalman滤波器r矩阵自适应设置
[0054]
kalman滤波器的量测量z为零速区间内的速度误差，即：
[0055]
z＝[v
n-0 v
u-0 v
e-0]
t
＝[v
n v
u ve]
t
[0056]
设r矩阵为三维量测量对应的量测噪声方差矩阵，其对角线元素常值分别为r1、r2、r3。
[0057]
由一步预测代替真实状态x引起对量测的估计误差(即新息)为：
[0058][0059]
其中，h为系统量测矩阵。
[0060]
当行人在直线行走时，由于运动幅度小、步态相对平稳，所以速度误差观测准确度较高，r矩阵为所设定的常值。
[0061]
当行人在进行弧线走或转弯等机动运动时，运动剧烈程度增大，导致速度误差观测精度下降，对应噪声值应自适应增大。此时，利用新息对r矩阵进行自适应调整：
[0062]ri
′
＝ri+rk*rk,(i＝1,2,3)
[0063]
其中，ri′
(i＝1,2,3)为调整后r矩阵对角线元素值。同时对r矩阵进行限幅设置：
[0064][0065]
其中，thr为r矩阵限幅判别阈值。
[0066]
最后应说明的是：以上实施例仅用以说明本发明的技术方案，而非对其限制；尽管参照前述实施例对本发明进行了详细的说明，本领域的普通技术人员应当理解：其依然可以对前述各实施例所记载的技术方案进行修改，或者对其中部分技术特征进行等同替换；而这些修改或者替换，并不使相应技术方案的本质脱离本发明各实施例技术方案的精神和范围。