一种跑步状态下基于近零点速度观测的惯性定位方法

1.本发明属于行人自主导航定位领域，尤其涉及一种跑步状态下基于近零点速度观测的惯性定位方法。


背景技术：

2.基于惯性传感器的行人导航定位技术是在卫星拒止情况下实现人员精准定位的关键技术，不加修正的惯性导航系统的定位误差随时间累积而发散，零速修正算法(zero velocity update zupt)利用人员在走路时脚部存在零速区间的特性，采用固定于足部的惯性传感器的配置方案，引入速度为零的观测量进行卡尔曼滤波估计系统误差，修正系统状态，提高定位精度。传统zupt算法所采用的固定阈值零速检测方法与零速度观测方法很难适应跑步状态下的高动态特性，进而导致传统zupt算法在跑步状态下失效，目前已有相关研究采用自适应阈值、多传感器零速检测、深度学习等方法解决上述问题，然而这些方法虽然可以取得较好的效果，但其需要不断地进行参数调整，不够灵活、简洁和方便，同时泛化能力有限，不利于普及应用，因此如何设计一套更为简单、灵活、稳定的跑步状态下的惯性定位算法是迫切需要解决的问题。


技术实现要素：

3.为解决上述问题，本发明提供一种跑步状态下基于近零点速度观测的惯性定位方法，具有简洁性、方便性和稳定性，能够有效抑制跑步状态下惯导系统的误差发散，提高跑步状态下惯性定位精度。
4.一种跑步状态下基于近零点速度观测的惯性定位方法，包括：
5.步骤一、在落地脚着地过程中，将脚跟与地面的接触点作为旋转中心，将惯性传感器设置在靠近旋转中心的位置；其中，设惯性传感器与旋转中心的距离为δr；在人脚落地减速时，将脚部与小腿近似看作为一个“l”形固连刚体；
6.步骤二、检测最佳近零点以及近零区间，具体为：
7.对于惯性传感器在各个时刻采集的落地脚的俯仰角速度ω
x
(k)序列，当某个时刻的俯仰角速度的值比其前一时刻和后一时刻的值都大时，且该时刻的俯仰角速度为负值时，该时刻的俯仰角速度为峰值点，按照该规则确定各个峰值点，组成最佳近零点的集合peak；
8.取俯仰角速度ω
x
(k)序列中最佳近零点前的若干数据点作为近零区间；
9.步骤三、对近零区间中各点小速度进行估计，利用固连刚体圆周运动的特性进行如下计算：
[0010][0011]
其中v(k)为k时刻人脚的绝对速度，v
x
(k)、vy(k)、vz(k)为导航坐标系下k时刻的x、y和z轴方向的速度；导航坐标系定义为东北天坐标系；
[0012]
在近零区间的各个采样时刻，利用惯性导航系统解算获得人脚相对于地面的俯仰
角θ，继而获得导航坐标系下人脚在z轴方向的速度分量：
[0013]
vz(k)≈εz(k)＝v(k)sin(θ)
[0014]
εz(k)定义为当前k时刻的z轴方向小速度；
[0015]
再获得x轴与y轴的合速度，即速度的水平分量为：
[0016][0017]
ε
xy
(k)定义为当前k时刻的x轴与y轴的合方向的小速度；
[0018]
由此根据速度的水平分量v
xy
(k)得到当前时刻k的导航坐标系下人脚x轴方向和y轴方向的速度：
[0019][0020][0021]
其中，和分别表示当前k时刻之前一段设定时间计算的x方向速度的均值和y方向速度的均值；ε
x
(k)和εy(k)分别定义为当前时刻k的x轴方向和y轴方向的小速度；
[0022]
步骤四、在近零区间，以k时刻惯性传感器所处位置的小速度作为观测量进行卡尔曼滤波，对惯性导航系统输出的参数值进行修正，其中，观测量为：
[0023][0024]
pz(k)表示惯导系统解算得到的自身高度；v
′
x
(k)、v
′y(k)和v
′z(k)分别表示惯导系统解算得到的k时刻的三个坐标轴方向的速度。
[0025]
较佳的，所述步骤一中，δr小于10厘米。
[0026]
进一步的，在检测最佳近零点时，对惯性传感器在各个时刻采集的落地脚的俯仰角速度ω
x
(k)进行均值滤波，利用滤波后的俯仰角速度进行最佳近零点检测。
[0027]
较佳的，所述均值滤波的窗口长度为3。
[0028]
较佳的，所述步骤二中，若干个数据点的数为表示向下取整，n表示均值滤波的窗口长度。
[0029]
较佳的，所述步骤二中，将对于峰值点小于或者等于设定阈值ω
t
从集合peak中剔除。
[0030]
较佳的，所述步骤三中，设定时间取0.5s以内。
[0031]
本发明具有如下有益效果：
[0032]
本发明提供一种跑步状态下基于近零点速度观测的惯性定位方法，将惯性传感器固定于足跟部，引入下肢“l”形固连刚体旋转模型，基于俯仰角速度衰减特性，采用峰值检测方法确定速度最佳近零点及近零区间，可以准确有效地检测出行人惯性导航系统在跑步状态下的速度修正点，之后在近零区间内估算此时小速度，以此时传感器所处位置的小速
度作为观测量进行卡尔曼滤波，可以有效抑制跑步状态下系统误差的发散，提高惯性定位精度。
附图说明
[0033]
图1为本发明提供的一种跑步状态下基于近零点速度观测的惯性定位方法的流程图。
[0034]
图2为惯性传感器配置示意图；
[0035]
图3脚跟落地起落状态示意图；
[0036]
图4为峰值点检测方法示意图；
[0037]
图5为本发明方法与传统方法的效果对比轨迹图。
具体实施方式
[0038]
为了使本技术领域的人员更好地理解本技术方案，下面将结合本技术实施例中的附图，对本技术实施例中的技术方案进行清楚、完整地描述。
[0039]
参见图1，该图为本实施例提供的一种跑步状态下基于近零点速度观测的惯性定位方法的流程图。一种跑步状态下基于近零点速度观测的惯性定位方法，包括以下步骤：
[0040]
s1：如图2所示为惯性传感器配置示意图，在落地脚着地过程中，脚跟与地面的接触点即旋转中心为理想零速部位，但是由于惯性传感器尺寸限制与安装限制，惯性传感器将无法被配置到旋转中心，因此目前所配置的位置与理想零速部位存在偏差δr(δr小于10厘米)；
[0041]
s2：如图2所示为人体脚跟部落地时起落状态示意图，在落地减速时人体脚部与小腿可近似看作为一个“l”形固连刚体，单脚从落地的减速状态开始到脚掌贴地，可看作是“l”形固连刚体绕o1点旋转的过程，此过程中整个刚体处于减速状态(实际情况中脚跟落地后先加速旋转后快速减速)，脚跟部的o1点为刚体旋转的旋转中心点o1为理想零速部位。由于在跑步状态中，“l”形刚体的旋转角速度不会衰减至零，因此实际上在旋转减速区间中，传感器所处部位并非处于一个真正的零速状态，但是随着脚面逐渐贴近地面，传感器所处部位将会逐渐逼近零速状态，当落地脚面完全贴地且与人体躯干在同一条直线上时(即头部在地面的投影与脚重合)，惯性传感器所处部位将会达到最接近零速的状态。
[0042]
s3：定义若存在时刻t0，满足落地脚面完全贴地且与人体躯干在同一条直线上，则将以t0为中心，以δ为半径的邻域称为近零区间。在近零区间内落地脚的速度很小，在(t
0-δ,t0)的区间内，落地脚虽然速度很小但是其旋转中心仍为o1，速度呈现减小的趋势。在(t0,t0 δ)的区间内落地脚速度依旧很小，但是其旋转中心变为o2，速度呈现增大的趋势。在这个区间内的所有时刻都被称之为近零点，脚部速度在t0时刻最接近于零，因此将t0时刻称为近零区间内的最佳近零点。
[0043]
s4：传统基于零速度观测的修正算法依赖于零速度检测准确性，由于人体正常行进时具有相对较长的零速区间与较低的运动速度，某些点错检和漏检不会对结果造成十分大的影响，但当人体处于跑步运动状态时，不存在真正意义上的零速区间，只会存在小速度近零区间，由于人体运动处于相对高速、高步频状态，因此小速度的近零区间的检测与小速度的估计将会极大地影响最终定位结果。因此本发明进行近零区间的检测是首先利用角速
度衰减特性寻找到最佳近零点，之后以最佳近零点为起点回溯少数点，以此作为近零区间，之后根据所建立的旋转模型，利用近零区间的各点角速度计算近零区间各点的小速度，以此小速度作为观测量进行卡尔曼滤波修正惯导系统各状态变量。
[0044]
最佳近零点检测问题本质上就是一个峰值检测问题，为了有效避免干扰，本发明首先对惯性传感器采集的原始信号进行了一次均值滤波：
[0045][0046]
之后利用长度为3的滑动窗口进行采样以检测峰值，如图4所示，当某个时刻的俯仰角速度的值比其前一时刻和后一时刻的值都大时，且该时刻的俯仰角速度为负值时，该时刻的俯仰角速度为峰值点，按照该规则确定各个峰值点，组成最佳近零点的集合peak，即：
[0047][0048]
其中，ω
x
(k)为惯性传感器在k时刻测得的俯仰角速度，为惯性传感器均值滤波后的俯仰角速度，n为均值滤波的窗口长度；为防止出现异常近零点，要求峰值点大于一个值为负的设定阈值ω
t
。
[0049]
利用这种方法可能会在信号峰值部分检测出多个峰值点，但只要其在减速区间与加速区间之间就无需进行再调整。从已检测到的峰值点出发向前向后连续取点可以确定一段近零区间，由于均值滤波后的峰值点相较真实峰值点滞后，因此真实最佳近零点应该偏前，为了保证所取得的近零区间包含最佳近零点，同时也为了保证旋转中心为o1，需要向前回溯若干个点并全部保留，进而确定一段近零区间。一般去回溯点数为表示向下取整。
[0050]
s5：使用近零区间速度修正可以避免最佳近零点检测错误所引起的误差，根据前文所述可知，当惯性传感器所处位置处于近零区间附近时，其虽然不是真正的零速但其速度也十分小，但如果强行将其置为零将会引起里程估计偏小，因此需要对近零区间中各点小速度进行估计，利用固连刚体圆周运动的特性进行如下计算：
[0051][0052]
其中v(k)为k时刻人脚的绝对速度，v
x
(k)、vy(k)、vz(k)为导航坐标系下三个方向的速度；导航坐标系定义为东北天坐标系；
[0053]
在近零区间内，利用惯性导航系统ins解算获得k时刻人脚相对于地面的俯仰角θ，获得导航坐标系下z轴方向的速度分量为：
[0054]
vz(k)≈εz(k)＝v(k)sin(θ)
[0055]
εz(k)定义为当前k时刻的z轴方向小速度；
[0056]
再获得x轴与y轴的合速度，即水平分量为：
[0057][0058]
ε
xy
(k)定义为当前k时刻的x轴与y轴的合方向的小速度；
[0059]
由此根据合速度v
xy
(k)就可以得到当前时刻k的导航坐标系下人脚x轴方向和y轴方向的速度：
[0060][0061][0062]
其中，和分别表示当前k时刻之前一段时间(0.5s以内)计算的x方向速度的均值和y方向速度的均值；ε
x
(k)和εy(k)分别定义为当前时刻k的x轴方向和y轴方向的小速度；
[0063]
s6：以k时刻惯性传感器所处位置的小速度作为观测量进行卡尔曼滤波，对惯性导航系统输出的参数值进行修正，其中，以导航系统误差作为状态变量x＝δx＝[δp
t δv
t δψ
t
]
t
，以速度误差和高度误差为观测量z＝[δp
z δv
x δv
y δvz]
t
，建立系统状态方程和量测方程：
[0064]
xk＝φ
k|k-1
x
k-1
γ
k-1wk-1
[0065]
zk＝hx
k|k
vk[0066]
其中φ
k|k-1
为k-1到k时刻的状态转移矩阵，γ
k-1
为系统噪声驱动矩阵，h为量测矩阵，w
k-1
为系统激励噪声矩阵，vk为量测噪声矩阵。
[0067]
在观测方程中，有如下关系：
[0068][0069]
其中k为近零区间内各散点的时刻，pz(k)表示惯导系统解算得到的自身高度；v
′
x
(k)、v
′y(k)和v
′z(k)分别表示惯导系统解算得到的k时刻的三个坐标轴方向的速度；ε
x
(k)、εy(k)、εz(k)为此时所估计的三个坐标轴方向的小速度，δr为惯性传感器安装位置到脚跟的距离。
[0070]
在这里认为每次修正后误差都能接近于零，因此不需要使用时间更新方程来传播误差状态，进而建立卡尔曼滤波方程估计系统误差，进而修正系统状态：
[0071]
xk＝kkzk[0072][0073][0074]
pk＝[i-k
khk
]p
k,k-1
[0075]
图5所示为人员操场跑步实验，所发明方法可以有效提高跑步状态下的惯性定位精度。
[0076]
本实施例不仅有效地提高跑步状态下惯性定位精度，而且提高了方法的简洁性、方便性和稳定性，有利于方法的推广。
[0077]
当然，本发明还可有其他多种实施例，在不背离本发明精神及其实质的情况下，熟悉本领域的技术人员当然可根据本发明作出各种相应的改变和变形，但这些相应的改变和变形都应属于本发明所附的权利要求的保护范围。