[发明专利]基于人体/环境约束的行人导航系统三维空间定位方法

摘要

本发明涉及一种基于人体/环境约束的行人导航系统三维空间定位方法，解决了基于低成本MEMS惯性测量单元设计的行人导航系统在三维空间内的高精度定位问题；本方法采用鞋部安装MEMS惯性测量单元的方式进行数据采集并完成捷联导航解算，同时结合鞋底安装的压力传感器输出的数据对行人步态中的零速区间进行检测。本发明结合行人运动特点及周围环境中建筑物特点，以零速修正为基础，提出了抑制零速区间内磁场异常及抑制SINS高度通道发散的方案，并设计了扩展卡尔曼滤波器对导航解算结果进行误差修正，实现了行人在三维空间的高精度定位功能。

主权项

1.一种基于人体/环境约束的行人导航系统三维空间定位方法，其特征在于，该方法包括如下步骤：S101，在鞋跟部位安装MEMS惯性测量单元，在鞋底脚跟部位安装压力传感器；S102，采集MEMS惯性测量单元及压力传感器输出数据，提取行人运动步态中的零速区间；具体提取行人运动步态中的零速区间的公式为：amin＜ax(k)2+ay(k)2+az(k)2＜amaxf(k)‑Tf＞0|ωy(k)|＜Tω若以上四个判断条件同时成立，则认为当前时刻行人脚部处于静止状态；其中，a_i(k)表示k时刻i轴向上加速度计输出，i＝x,y,z；a_min、a_max分别是阈值区间的左边界和右边界；a_k是加速度计输出，是窗口内数据点的平均值，n表示窗口宽度，为方差阈值；f(k)表示k时刻检测到的压力值，T_f表示压力阈值；|ω_y(k)|表示k时刻y轴陀螺输出的绝对值，T_ω为角速率阈值；S103，利用步骤S102中MEMS惯性测量单元测量的加速度、角速度信息，并结合以下公式进行捷联导航解算，得到行人的位置、速度、姿态信息；其中，上标n表示导航坐标系，b表示载体坐标系，Q表示姿态四元数，V表示速度，P表示位置；是由b系到n系的传输矩阵，表示由陀螺仪测量的角速度构成的反对称矩阵，f^b表示比力，表示地球自转角速度，表示导航系相对地球系的转动角速率，gⁿ表示地球重力场矢量，在地球上某一范围内可近似为常值；S104，利用步骤S102中提取到的零速区间开展卡尔曼滤波，估计行人系统状态误差并对SINS解算结果进行修正；系统卡尔曼滤波模型为：δxk+1＝Φkδxk+wkδzk＝Hkδxk+vk其中，δxk是k时刻的系统状态，Φk是状态转移矩阵，wk表示过程噪声，δzk为k时刻误差观测量，Hk为测量矩阵，vk为测量噪声；定义误差状态向量为：其中误差状态向量中包含9个导航误差及6个传感器误差，它们分别是三维的位置误差向量δφⁿ，速度误差向量δVⁿ，姿态向量δPⁿ，陀螺偏置误差向量ε^b及加表偏置误差向量状态转移矩阵为：其中，Δt表示采样时间间隔，fn×为导航坐标系下比力分量构成的反对称矩阵；零速区间内速度测量误差表达式为：ΔV＝VINS‑VStance零速修正的量测矩阵为：Hzupt＝[03×3 I3×3 03×3 03×3 03×3]S105，在执行步骤S104的同时，利用磁传感器的输出计算零速区间内每一个零速点对应的航向角，在步骤S104执行到零速区间内最后一个零速点时，对零速区间内磁传感器计算的所有航向角作平滑滤波，并将处理结果作为观测量输出给卡尔曼滤波器进行航向修正；S106，在执行步骤S104的同时，判断环境约束条件下行人的高度变化特性，实时更新高度观测量并将其输出给卡尔曼滤波器进行高度修正；通过步骤S102到S106的循环执行即可实时得到修正后的行人导航参数。