基于人体/环境约束的行人导航系统三维空间定位方法
【专利摘要】本发明涉及一种基于人体/环境约束的行人导航系统三维空间定位方法，解决了基于低成本MEMS惯性测量单元设计的行人导航系统在三维空间内的高精度定位问题；本方法采用鞋部安装MEMS惯性测量单元的方式进行数据采集并完成捷联导航解算，同时结合鞋底安装的压力传感器输出的数据对行人步态中的零速区间进行检测。本发明结合行人运动特点及周围环境中建筑物特点，以零速修正为基础，提出了抑制零速区间内磁场异常及抑制SINS高度通道发散的方案，并设计了扩展卡尔曼滤波器对导航解算结果进行误差修正，实现了行人在三维空间的高精度定位功能。
【专利说明】
基于人体/环境约束的行人导航系统三维空间定位方法
技术领域
[0001] 本发明属于导航定位技术领域，具体来说，涉及一种基于人体/环境约束的行人导 航系统三维空间定位方法。
【背景技术】
[0002] 行人导航系统能够帮助执行任务的人员实时定位自己的位置并与指挥中心取得 联系，从而极大地保障紧急救援人员在未知环境中的生命安全。此外，在机场、剧院、地下停 车场等大型公共场所及高楼林立的现代化城市中，精度高、实时性强的行人导航系统能够 有效地帮助行人进行自我定位及目标找寻。但现有的卫星定位系统在城市峡谷、室内等卫 星信号受限或者失效的环境中无法正常工作，因而无法实现连续自主定位。因此，开展无卫 星信号下的行人导航定位技术具有广泛的应用前景。
[0003] 随着MEMS技术的发展，MEMS传感器具有成本低、重量轻、尺寸小、功耗低等优势，同 时考虑到惯性导航具有自主性强、抗干扰性强的特点，脚部安装MEMS惯性传感器的行人导 航系统成为了当前的研究热点。但MEMS传感器一般精度较低，在采用SINS算法计算导航参 数时，积分过程中加速度误差引起的位置误差会随时间的三次方增长。
[0004] 因此，如何提高定位的精度，已成为现在急需解决的问题。
【发明内容】
[0005] 为解决上述问题，本发明提供了一种基于人体/环境约束的行人导航系统三维空 间定位方法，该方法包括如下步骤：
[0006] S101，在鞋跟部位安装MEMS惯性测量单元，在鞋底脚跟部位安装压力传感器；
[0007] S102,采集MEMS惯性测量单元及压力传感器输出数据，提取行人运动步态中的零 速区间；具体提取行人运动步态中的零速区间的公式为：
[0008] amin<ax(k) 2+ay(k)2+az(k)2<a max
[0010] f(k)-Tf>0
[0011] Qy(k) I <Τω
[0012] 若以上四个判断条件同时成立，则认为当前时刻行人脚部处于静止状态；
[00?3 ] 其中，ai (k)表示k时刻i轴向上加速度计输出，i = X，y，ζ。amin、amax分别是阈值区间 的左边界和右边界;ak是加速度计输出，昆是窗口内数据点的平均值，η表示窗口宽度， 为方差阈值;f(k)表示k时刻检测到的压力值，Tf表示压力阈值；| coy(k)|表示k时刻y轴陀螺 输出的绝对值，Τω为角速率阈值；
[0014] S103,利用步骤S102中MEMS惯性测量单元测量的加速度、角速度信息，并结合以下 公式进行捷联导航解算，得到行人的位置、速度、姿态信息；
[0018] 其中，上标η表示导航坐标系，b表示载体坐标系。Q表示姿态四元数，V表示速度，P 表示位置。? 1是由b系到η系的传输矩阵，表示由陀螺仪测量的角速度构成的反对称矩 阵，fb表示比力，表示地球自转角速度，ω二表示导航系相对地球系的转动角速率，g n表示 地球重力场矢量，在地球上某一范围内可近似为常值；
[0019] S104,利用步骤S102中提取到的零速区间开展卡尔曼滤波，估计行人系统状态误 差并对SINS解算结果进行修正；
[0020] 系统卡尔曼滤波模型为：
[0021] 5xk+i= ?k5xk+wk
[0022] 5zk = Hk5xk+vk
[0023] 其中，Sxl^k时刻的系统状态，是状态转移矩阵，概表示过程噪声，SzAk时刻 误差观测量，Hk为测量矩阵，vk为测量噪声；
[0024]定义误差状态向量为：
[0025] δχ=[δφη 5Vn δΡηeb Vb]
[0026] 其中误差状态向量中包含9个导航误差及6个传感器误差，它们分别是三维的位置 误差向量δ Φη，速度误差向量δνη，姿态向量δΡη，陀螺偏置误差向量J及加表偏置误差向量 ▽ b;
[0027] 状态转移矩阵为：
[0029] 其中，At表示采样时间间隔，fnX为导航坐标系下比力分量构成的反对称矩阵；
[0030] 零速区间内速度测量误差表达式为：
[0031] AV = ViNS-Vst ance
[0032] 零速修正的量测矩阵为：
[0033] Hzupt=[03X3 13X3 03X3 03X3 03X3]
[0034] S105,在执行步骤S104的同时，利用磁传感器的输出计算零速区间内每一个零速 点对应的航向角，在步骤S104执行到零速区间内最后一个零速点时，对零速区间内磁传感 器计算的所有航向角作平滑滤波，并将处理结果作为观测量输出给卡尔曼滤波器进行航向 修正；
[0035] S106,在执行步骤S104的同时，判断环境约束条件下行人的高度变化特性，实时更 新高度观测量并将其输出给卡尔曼滤波器进行高度修正；
[0036] 通过步骤S102到S106的循环执行即可实时得到修正后的行人导航参数。
[0037] 进一步的，上述MEMS惯性测量单元，包含三轴MEMS加速度计、三轴MEMS陀螺仪及三 轴MEMS磁传感器。
[0038] 进一步的，所述步骤S105中：在执行步骤S104的同时，利用磁传感器的输出计算零 速区间内每一个零速点对应的航向角，在步骤S104执行到零速区间内最后一个零速点时， 对零速区间内磁传感器计算的所有航向角作平滑滤波，并将处理结果输出给卡尔曼滤波 器;零速区间内航向角及航向角测量误差的计算表达式为： η-1
[0039] Headings = ^Heading,,^ (i) ?=1
[0040] Heading= (Headingt〇tai+HeadingMag(n) )/η [0041 ] Αφ = φΜ5 - Heading
[0042] 其中，η表示零速区间内零速点的个数，Head i ngtotai表示零速区间内除最后一个零 速点外其它零速点对应的磁航向角总和，Head i ngMag (i)表示零速区间内第i个零速点对应 的磁航向角，Heading零速区间内磁传感器计算的所有航向角作平滑滤波的值，Δ炉表示航 向角误差，表不SINS计算得到的航向角。
[0043]航向修正的量测矩阵为：
[0044] Hheading=[[0 0 1] 〇3X3 〇3X3 〇3X3 〇3X3]
[0045] 进一步的，所述步骤S106中：在执行步骤S104的同时，判断在由环境约束条件下的 行人高度变化特性，实时更新高度观测量并将其输出给卡尔曼滤波器;利用楼梯上台阶高 度进行约束时，需首先提取步态中的零速区间，然后判断零速区间内相邻零速点之间高度 变化值与设定高度阈值间的关系，识别行人的运动状态;定义k时刻SINS解算的高度信息为 hlNS，k-l时刻更新后的高度信息为hprev，二者的高度差记为Ah = hiNS-hprev，高度阈值记为 thh，k时刻更新后的高度信息为h_r，此时导航解算的高度值与观测的高度值间的关系可表 达为：
[0046] Δ H=hiNS-hcuur
[0047] 高度修正的量测矩阵为：
[0048] Hheight= [03X3 03X3 [0 0 l] 〇3X3 〇3X3]〇
[0049] 与现有技术相比，本发明所述的基于人体/环境约束的行人导航系统三维空间定 位方法具有如下优点：
[0050] (1)本发明在利用MEMS惯性测量单元输出数据进行零速区间检测的基础上，引入 压力传感器辅助MIMU提取步态中的零速区间。基于压力传感器的输出特性，多传感器数据 输出组合的方式极大地降低了零速区间的误检与漏检率，能有效提高行人导航系统的解算 精度。
[0051] (2)本发明结合行人运动特点，对零速区间内磁传感器计算的航向角作平滑滤波， 降低了局部零速点处磁场异常导致的航向误差，从而在保证航向修正的基础上提高了可靠 性。
[0052] (3)本发明所述的方法考虑了 SINS解算过程中高度通道发散的问题，同时结合行 人运动过程中周围环境中多层建筑物内外存在楼梯且楼梯上台阶高度一致的特性，提出了 利用楼梯尺寸进行高度约束的方法，提高了行人系统在三维空间内的定位精度。
【附图说明】
[0053]图1为基于人体/环境约束的行人导航系统三维空间定位方法流程图。
[0054] 图2为基于零速区间内航向角约束的导航解算流程图。
[0055] 图3为基于环境中楼梯台阶高度约束的导航解算流程图。
【具体实施方式】
[0056] 下面通过具体实施例对本发明作进一步详述，以下实施例只是描述性的，不是限 定性的，不能以此限定本发明的保护范围。
[0057]如图1所示，本发明请求保护一种基于人体/环境约束的行人导航系统三维空间定 位方法，该方法包含以下步骤：
[0058] S101，在鞋跟部位安装MEMS(Micr〇-Electro_Mechanical System)惯性测量单元， Mmu坐标轴分别指向人体的前、右、下方向；在鞋底脚跟部位安装压力传感器，保证其敏感 轴朝竖直方向；
[0059] 上述MEMS惯性测量单元，包含三轴MEMS加速度计、三轴MEMS陀螺仪及三轴MEMS磁 传感器；
[0060] S102,采集MEMS惯性测量单元及压力传感器输出数据，提取行人运动步态中的零 速区间；具体提取行人运动步态中的零速区间的公式为：
[0061 ] ￡lmin<￡lx(k) +￡ly(k) +￡lz(k) <￡lmax
[0063] f(k)-Tf>0
[0064] oy(k) | <Τω
[0065] 若以上四个判断条件同时成立，则认为当前时刻行人脚部处于静止状态；
[0066] 其中，ai (k)表示k时亥lj i轴向上加速度计输出，i = X，y，ζ。amin、ama^别是阈值区间 的左边界和右边界；ak是加速度计输出，&是窗口内数据点的平均值，η表示窗口宽度， 为方差阈值;f(k)表示k时刻检测到的压力值，Tf表示压力阈值；| coy(k)|表示k时刻y 轴陀螺输出的绝对值，Τω为角速率阈值；
[0067] S103,利用步骤S102中MEMS惯性测量单元测量的加速度、角速度信息，并结合以下 公式进行捷联导航解算，得到行人的位置、速度、姿态信息；
[0069] r! = Clfb -(2αζ + <)xV"+ gn
[0070] pn - γη
[0071] 其中，上标n表示导航坐标系，b表示载体坐标系。Q表示姿态四元数，V表示速度，P 表示位置。^是由b系到η系的传输矩阵，6&X表示由陀螺仪测量的角速度构成的反对称 矩阵，fb表示比力，<2《表示地球自转角速度，表示导航系相对地球系的转动角速率，gn表 示地球重力场矢量，在地球上某一范围内可近似为常值。
[0072] S104,利用步骤S102中提取到的零速区间开展卡尔曼滤波，估计行人系统状态误 差并对SINS解算结果进行修正；
[0073]系统卡尔曼滤波模型为：
[0074] 5xk+i= ?k5xk+Wk
[0075] 5zk = Hk5xk+vk
[0076] 其中，3处是1^时刻的系统状态，是状态转移矩阵，概表示过程噪声，SZkSk时刻 误差观测量，H k为测量矩阵，vk为测量噪声；
[0077]定义误差状态向量为：
[0078] δχ=[δφη 5Vn δΡηeb Vb]
[0079] 其中误差状态向量中包含9个导航误差及6个传感器误差，它们分别是三维的位置 误差向量δ Φη，速度误差向量δνη，姿态向量δρη，陀螺偏置误差向量J及加表偏置误差向量 Vb〇
[0080] 状态转移矩阵为：
[0082]其中，At表示采样时间间隔，fnX为导航坐标系下比力分量构成的反对称矩阵。 [0083] 零速区间内速度测量误差表达式为：
[0084] AV = ViNS-Vst ance
[0085] 零速修正的量测矩阵为：
[0086] HZUpt=[03X3 13X3 03X3 03X3 03X3]
[0087] S105,在执行步骤S104的同时，利用磁传感器的输出计算零速区间内每一个零速 点对应的航向角，在步骤S104执行到零速区间内最后一个零速点时，对零速区间内磁传感 器计算的所有航向角作平滑滤波，并将处理结果输出给卡尔曼滤波器进行航向修正；
[0088]图2为基于零速区间内航向角约束的导航解算流程图，零速区间内航向角及航向 角测量误差的计算表达式为： n-l
[0089] Headingtml =^Heading"uv (/) .Μ.
[0090] Heading= (Headingt〇tai+HeadingMag(n) )/n
[0091 ] Αφ = - Heading
[0092] 其中，n表示零速区间内零速点的个数，Head i ngtotai表示零速区间内除最后一个零 速点外其它零速点对应的磁航向角总和，Head i ngMag (i)表示零速区间内第i个零速点对应 的磁航向角，Heading零速区间内磁传感器计算的所有航向角作平滑滤波的值，Δ0_表示航 向角误差，約鄉表不31吧计算得到的航向角。
[0093]航向修正的量测矩阵为：
[0094] Hheading=[[0 0 1] 〇3Χ3 〇3Χ3 〇3Χ3 〇3Χ3]
[0095] S106,在执行步骤S104的同时，判断在由环境约束条件下的行人高度变化特性，实 时更新高度观测量并将其输出给卡尔曼滤波器进行高度修正。
[0096] 图3为基于环境中楼梯台阶高度约束的导航解算流程图。利用楼梯上台阶高度进 行约束时，需首先提取步态中的零速区间，然后判断零速区间内相邻零速点之间高度变化 值与设定高度阈值间的关系，识别行人的运动状态(上楼、下楼、水平运动）。定义k时刻SINS 解算的高度信息为h INS，k_l时刻更新后的高度信息为hprev，二者的高度差记为Ah = hINS_ hpre3V，高度阈值记为thh，k时刻更新后的高度信息为h_ r，此时导航解算的高度值与观测的 高度值间的关系可表达为：
[0097] Δ H=hiNS-hcuur
[0098] 高度修正的量测矩阵为：
[0099] Hheight=[03X3 03Χ3 [0 0 1] 〇3X3 〇3X3]
[0100] 通过步骤S102到S106的循环执行即可实时得到修正后的行人导航参数。
[0101] 应当理解的是，本发明的应用不限于上述的举例，对本领域普通技术人员来说，可 以根据上述说明加以改进或变换，所有这些改进和变换都应属于本发明所附权利要求的保 护范围。
【主权项】
1. 一种基于人体/环境约束的行人导航系统三维空间定位方法，其特征在于，该方法包 括如下步骤： SlOl，在鞋跟部位安装MEMS惯性测量单元，在鞋底脚跟部位安装压力传感器； S102,采集MEMS惯性测量单元及压力传感器输出数据，提取行人运动步态中的零速区 间；具体提取行人运动步态中的零速区间的公式为：若以上四个判断条件同时成立，则认为当前时刻行人脚部处于静止状态； 其中，ai (k)表示k时刻i轴向上加速度计输出，i = X，y，z。amin、amax分别是阈值区间的左 边界和右边界;￡ik是加速度计输出，<是窗口内数据点的平均值，η表示窗口宽度，L ,为 方差阈值;f(k)表示k时刻检测到的压力值，Tf表示压力阈值；I coy(k) I表示k时刻y轴陀螺输 出的绝对值，Τω为角速率阈值； S103，利用步骤S102中MEMS惯性测量单元测量的加速度、角速度信息，并结合以下公式 进行捷联导航解算，得到行人的位置、速度、姿态信息；其中，上标η表示导航坐标系，b表示载体坐标系。Q表示姿态四元数，V表示速度，P表示 位置。Cf是由b系到η系的传输矩阵，X表示由陀螺仪测量的角速度构成的反对称矩阵， fb表示比力，<表示地球自转角速度，份=,表示导航系相对地球系的转动角速率，gn表示地 球重力场矢量，在地球上某一范围内可近似为常值； S104,利用步骤S102中提取到的零速区间开展卡尔曼滤波，估计行人系统状态误差并 对SINS解算结果进行修正； 系统卡尔曼滤波模型为： 3xk+l = Φ k3xk+Wk 3zk - HkSxk+Vk 其中，3^是1^时刻的系统状态，是状态转移矩阵，概表示过程噪声，SzkSk时刻误差观 测量，Hk为测量矩阵，Vk为测量噪声； 定义误差状态向量为： δχ=[δφη δνη δρηeb vb] 其中误差状态向量中包含9个导航误差及6个传感器误差，它们分别是三维的位置误差 向量δ Φη，速度误差向量δνη，姿态向量δΡη，陀螺偏置误差向量J及加表偏置误差向量V b; 状态转移矩阵为：其中，At表示采样时间间隔，fnX为导航坐标系下比力分量构成的反对称矩阵； 零速区间内速度测量误差表达式为： AV = ViNs-Vst ance 零速修正的量测矩阵为： Hzupt= [〇3Χ3 13X3 〇3Χ3 〇3Χ3 〇3Χ3] 5105, 在执行步骤S104的同时，利用磁传感器的输出计算零速区间内每一个零速点对 应的航向角，在步骤S104执行到零速区间内最后一个零速点时，对零速区间内磁传感器计 算的所有航向角作平滑滤波，并将处理结果作为观测量输出给卡尔曼滤波器进行航向修 正； 5106, 在执行步骤S104的同时，判断环境约束条件下行人的高度变化特性，实时更新高 度观测量并将其输出给卡尔曼滤波器进行高度修正； 通过步骤S102到S106的循环执行即可实时得到修正后的行人导航参数。2. 如权利要求1所述的方法，其特征在于，上述MEMS惯性测量单元，包含三轴MEMS加速 度计、三轴MEMS陀螺仪及三轴MEMS磁传感器。3. 如权利要求1所述的方法，其特征在于： 所述步骤S105中：在执行步骤S104的同时，利用磁传感器的输出计算零速区间内每一 个零速点对应的航向角，在步骤S104执行到零速区间内最后一个零速点时，对零速区间内 磁传感器计算的所有航向角作平滑滤波，并将处理结果输出给卡尔曼滤波器;零速区间内 航向角及航向角测量误差的计算表达式为：其中，η表示零速区间内零速点的个数，HeadingtoteI表示零速区间内除最后一个零速点 外其它零速点对应的磁航向角总和，Head i ngMag (i)表示零速区间内第i个零速点对应的磁 航向角，Heading零速区间内磁传感器计算的所有航向角作平滑滤波的值，Δ妒表示航向角 误差，表不SINS计算得到的航向角。 航向修正的量测矩阵为： Hheading一 [ [O O I ] 〇3X3 〇3X3 〇3X3 〇3X3 ] 〇4.如权利要求1所述的方法，其特征在于： 所述步骤S106中：在执行步骤S104的同时，判断在由环境约束条件下的行人高度变化 特性，实时更新高度观测量并将其输出给卡尔曼滤波器;利用楼梯上台阶高度进行约束时， 需首先提取步态中的零速区间，然后判断零速区间内相邻零速点之间高度变化值与设定高 度阈值间的关系，识别行人的运动状态;定义k时刻SINS解算的高度信息为h INS，k-l时刻更 新后的高度信息为hprev，二者的高度差记为Ah = hINS-hprev，高度阈值记为thh，k时刻更新后 的高度信息为h_ r，此时导航解算的高度值与观测的高度值间的关系可表达为： A H=hiNS-h cuur 高度修正的量测矩阵为： Hheight= [〇3Χ3 〇3Χ3 [〇 O l] 〇3X3 〇3X3]〇
【文档编号】G01C21/16GK106017461SQ201610339701
【公开日】2016年10月12日
【申请日】2016年5月19日
【发明人】田晓春, 韩勇强, 陈家斌, 谢玲, 李楠
【申请人】北京理工大学