一种基于脚部和腿部微惯性测量单元的行人定位方法

技术特征：
1.一种基于脚部和腿部微惯性测量单元的行人定位方法，其特征在于，所述行人定位方法包括以下步骤：s1，在设备佩戴者的脚尖、脚跟和小腿处分别安装3个惯性测量单元，依据脚面与小腿的姿态以及传感器速度将步态周期划分为四个阶段：站立阶段、摆动前阶段、摆动阶段和摆动后阶段；根据相应的运动学特性以及加速度和角速度的统计特性，采用阈值法进行步态相位判断；s2，基于动态几何约束和科里奥利理论，根据3个惯性测量单元的空间位置信息构建四个连续步态阶段的速度和位置约束；根据人体运动学相关特征，在站立阶段、摆动前阶段、摆动后阶段，脚的速度为零，将零赋值给各传感器，周期性地修正速度真实值与传感器测量值之间的误差，构建各运动状态下的速度约束方程；s3，将修正后的传感器值通过互补滤波器以消除噪声的干扰，再通过卡尔曼滤波器获得各运动状态下的观测方程，由各运动状态下的高精度观测方程得到各运动状态修正后的定位结果。2.根据权利要求1所述的基于脚部和腿部微惯性测量单元的行人定位方法，其特征在于，步骤s1中，所述相应的运动学特性是指：摆动前阶段是以脚尖为支点的摆动动作，摆动后阶段是以脚跟为中心的旋转动作；当系统处于摆动前阶段时，脚跟处的加速度大于脚尖处，摆动后阶段则相反。3.根据权利要求1所述的基于脚部和腿部微惯性测量单元的行人定位方法，其特征在于，步骤s1中，采用阈值法进行步态相位判断的过程包括以下步骤：对惯性测量单元信号进行包括平滑、滤波和归一化在内的预处理；采用滑动窗口对预处理后的数据进行分割，对单个窗口提取运动学特征；结合测量的加速度信息，判断下肢各部分运动的先行趋势；结合角度阈值判断，对步态切换时刻点进行精确定位。4.根据权利要求1所述的基于脚部和腿部微惯性测量单元的行人定位方法，其特征在于，步骤s2中，基于动态几何约束和科里奥利理论，根据3个惯性测量单元的空间位置信息构建四个连续步态阶段的速度和位置约束的过程包括以下步骤：定义3个惯性测量单元分别为传感器0、传感器1和传感器2；结合动态三点几何约束推导得到位置约束方程：；其中，i为传感器的序号，表示传感器i所在的坐标系，n坐标系为导航坐标系，其原点表示为o，a代表踝关节；为从i指向j的向量，向量是在n坐标系下定义的；表示由传感器0所在的坐标系中踝关节a位置指向传感器0所在坐标系原点的向量，表示由传感器1所在的坐标系中踝关节a位置指向传感器1所在坐标系原点的向量，表示由传感器2所在的坐标系中踝关节a位置指向传感器2所在坐标系原点的向量，表示传感器0所在的
坐标系原点指向导航坐标系原点的向量，表示传感器1所在的坐标系原点指向导航坐标系原点的向量，表示传感器2所在的坐标系原点指向导航坐标系原点的向量，表示从传感器坐标系到n坐标系的旋转矩阵；和分别为三阶识别矩阵和零矩阵；根据站立时的零速校正公式：；得到站立时的速度校正方程为：；为向量在导航坐标系n中的速度,为n坐标系对s2坐标系的相对角速度, 表示传感器2所在的坐标系中踝关节位置指向导航坐标系原点的向量，表示该行元素都为0；为n坐标系中第i个传感器中的速度；根据摆动前后阶段的运动特征公式：；其中为传感器0所在的坐标系原点指向踝关节a的向量，为向量相对于踝关节a的速度, 为向量在导航坐标系n中的速度, 为踝关节a相对于导航坐标系的相对角速度, 为n坐标系对s0坐标系的相对角速度，为n坐标系对s1坐标系的相对角速度，到摆动前后阶段的速度校正方程为：。5.根据权利要求1所述的基于脚部和腿部微惯性测量单元的行人定位方法，其特征在于，步骤s3中，将修正后的传感器值通过互补滤波器以消除噪声的干扰，再通过卡尔曼滤波器获得各运动状态下的观测方程，由各运动状态下的高精度观测方程得到各运动状态修正后的定位结果的过程包括以下步骤：构建信息融合系统，信息融合系统包括基于互补滤波器的分散式姿态估计模块和用于集中处理速度和位置估计值的线性卡尔曼滤波器；在分散式姿态估计模块中，通过陀螺仪、加速度计与磁力计对三个惯性测量单元进行数据采样，分别输出角速度、加速度和磁场矢量，根据不同传感器的特性，经过互补滤波算法解算测量的最优估计值；线性卡尔曼滤波器根据分散式姿态估计模块的最优估计值输出各运动阶段的观测方程。6.根据权利要求5所述的基于脚部和腿部微惯性测量单元的行人定位方法，其特征在于，所述信息融合系统的离散状态空间表示为：；
；其中，为k时刻的状态向量，为k时刻系统输入，为k时刻观察向量，和分别为k时刻过程噪声和测量噪声，为k时刻状态转移矩阵，为k时刻观测矩阵；选择状态向量为：；其中，和分别为第i个传感器在n坐标系中k时刻的速度和位置，上述参量满足系统状态转移公式：；其中，t为采样周期，为k时刻下坐标系中点的加速度的投影，为重力加速度，表示从传感器坐标系到n坐标系的第k个旋转矩阵，表示k时刻的第1个观测方程，表示k时刻的第2个观测方程；根据理论测量方法并结合系统的状态转移方程，得到各运动阶段的观测方程：站立、摆动前和摆动后阶段的观测方程为：； ；
；摆动阶段的观测方程为： 。7.根据权利要求5所述的基于脚部和腿部微惯性测量单元的行人定位方法，其特征在于，所述基于互补滤波器的分散式姿态估计模块根据加速度计、陀螺仪和磁力计这三个不同传感器的互补特性，采用互补滤波算法对传感器的数据进行姿态角的求解；其中，在预设时长范围内选择陀螺仪值作为姿态角的最优估计，在预设时长范围外对加速度计和磁力计测量值取平均修正陀螺仪测量的姿态角。

技术总结
本发明公开了一种基于脚部和腿部微惯性测量单元的行人定位方法，包括：依据脚面与小腿的姿态以及传感器速度将步态周期划分为四个阶段：站立阶段、摆动前阶段、摆动阶段和摆动后阶段；采用阈值法进行步态相位判断；构建四个连续步态阶段的速度和位置约束；根据人体运动学相关特征，构建各运动状态下的速度约束方程；将修正后的传感器值通过互补滤波器以消除噪声的干扰，再通过卡尔曼滤波器获得各运动状态下的观测方程，由各运动状态下的高精度观测方程得到各运动状态修正后的定位结果。本发明提高了设备佩戴者的定位精度，能够获得较为准确的佩戴者实时定位信息。确的佩戴者实时定位信息。确的佩戴者实时定位信息。


技术研发人员：宋治儒 钱伟行 万游 周学成 方铭宇 刘童 沈皓轩 洪越
受保护的技术使用者：南京师范大学
技术研发日：2022.04.08
技术公布日：2022/5/6