基于人体运动学模型的行人导航系统与导航定位方法

1.基于人体运动学模型的行人导航系统，其特征在于，所述行人导航系统包括惯性传感组件和微型导航计算机，所述惯性传感组件包括惯性与地磁测量单元以及惯性测量单元，其中，一组惯性与地磁测量单元安装于人体躯干髋部，多组惯性测量单元分别安装于双腿，微型导航计算机安装在人体躯干上，所述微型导航计算机与惯性传感组件之间通过电缆或无线通讯完成数据传输；其中，躯干的惯性与地磁测量单元以及下肢的惯性测量单元通过确定足部零力矩点实现下肢各级杆臂效应补偿，并结合关节坐标系的肌肉骨骼间附着点坐标转换实现陀螺仪所测量的角速度的转换，从而构成足部虚拟惯性传感组件，通过该虚拟惯性传感组件完成行人导航过程。
2.利用如权利要求1所述基于人体运动学模型的行人导航系统的导航定位方法，其特征在于，其具体步骤如下：
(一)惯性传感组件的信号采集：
同步采集各组惯性传感组件的输出信号，得到各组角速度、比力和磁场强度，并将信号传输给微型导航计算机；
(二)惯性传感组件的随机误差的实时建模与校正：
首先对零位偏置的特性进行统计分析，分别建立符合陀螺仪与加速度计的零位偏置趋势项，在陀螺仪与加速度计的信号输出端加入零位偏置趋势项校正；分别建立陀螺仪与加速度计的随机误差模型，将经过零位偏置趋势项建模与校正步骤后的陀螺仪与加速度计输出作为观测量，将陀螺仪与加速度计的理论输出估计值作为状态量，建立基于随机误差模型的卡尔曼滤波器，分别实时估计陀螺仪与加速度计的理论输出估计值；
(三)惯性传感组件的初始对准：
该步骤同步应用于每组惯性传感组件，在微型导航计算机中进行；系统启动后在静态条件下，利用步骤(二)中加速度计的理论输出估计值，在微型导航计算机中通过水平自对准得到惯性传感组件初始横滚角与俯仰角，并将磁强计的信号输出通过通讯模块传送至微型导航计算机，得到惯性传感组件的初始航向角，水平自对准的公式为：
其中，θ为俯仰角，γ为横滚角，g为重力加速度， 与 分别为加速度计理论输出估计值在载体坐标系中投影的x与y轴分量；
(四)人体下肢零力矩点位置的确定：
为实现下肢杆臂效应动态补偿，首先须得到人体运动中位于足底的下肢零力矩点；人体运动中支撑腿可视为动态杆臂，以下肢零力矩点为原点进行转动来实现人体质心的移动，下肢零力矩点关于下肢质心位置、各关节的角度、角速度和角加速度的表达式：
式中，qi为连杆i与z方向的夹角，li、di分别为连杆i的长度及其质心到关节的距离，G为人体所受重力；从该公式可知，下肢零力矩点的位置由各关节的角度qi、角速度 和角加速度 的函数，这些物理量均可通过安装于下肢的惯性测量单元测量与计算得到；
(五)虚拟惯性传感组件的构建与杆臂效应动态补偿：
基于步骤(四)的原理，可在人体运动中实时计算支撑腿上零力矩点的精确位置，结合下肢各肢节的惯性参数，即可实时补偿足部至髋关节的杆臂效应；而支撑腿的零力矩点位置可视为一个静态的虚拟惯性传感器组件，该组件在足部静止相位中的速度与角速度均为
0，其器件误差也可视为0，暂忽略人体运动中下肢肌肉与脂肪等所引起的肢体变形，各关节处的杆臂效应补偿公式如下：
fn＝fn-1-ωn-1×(ωn-1×Rn-1),n＝1,2,3,4
其中，踝关节处比力f1由零力矩点位置比力f0结合足部运动角速度ω0与零力矩点位置距踝关节的距离R0进行补偿；膝关节处的比力f2由踝关节处比力f1结合踝关节运动角速度ω1与胫骨长度R1进行补偿；髋关节处的比力f3由膝关节处比力f2结合膝关节运动角速度ω2与股骨长度R2进行补偿；躯干部位的比力f4由髋关节处比力f3结合髋关节运动角速度ω3与惯性测量组件和髋关节之间的位置矢量R3补偿；若要在腿部任意位置的惯性测量组件之间进行杆臂效应的补偿，可将R1、R2与R3替代成惯性测量组件与相应关节之间的距离即可；
(六)虚拟捷联惯性导航系统的惯性导航解算：
该步骤应用于虚拟惯性传感组件所构成的虚拟捷联惯性导航系统，虚拟捷联惯性导航系统的惯性导航解算步骤分为姿态解算与速度、位置解算；
(七)虚拟捷联惯性导航系统的零速修正：
该步骤应用于虚拟捷联惯性导航系统，在微型导航计算机中进行，求解加速度计输出经步骤(二)中建模与校正后的理论输出估计值的三轴矢量和，通过判断该值是否接近重力来检测人体足部是否处于着地时间段，并利用着地时间段相对地面静止的特点进行间断式的零速修正，即根据虚拟捷联惯性导航系统误差与虚拟惯性传感组件误差建立系统状态方程与观测方程，实现人体行走中的零速修正，实时修正虚拟捷联惯性导航系统的误差与虚拟惯性传感组件误差。
3.如权利要求2所述的导航定位方法，其特征在于，所述步骤(六)的具体步骤为：
(一)虚拟惯性导航系统的姿态解算：基于虚拟惯性传感器组件中的三轴陀螺仪，采用四元数法进行姿态解算；
(二)虚拟惯性导航系统的速度、位置解算：把虚拟惯性传感器组件中的三轴加速度计原始输出通过姿态转移矩阵转换到导航坐标系中，解算出人体足部在地理系中的速度，并结合上个时刻人体的纬度、经度、高度与速度，求解当前时刻载体的纬度、经度与高度，从而完成整个虚拟捷联惯性导航系统的算法流程。

基于人体运动学模型的行人导航系统与导航定位方法\n技术领域\n[0001] 本发明涉及一种基于人体运动学模型的行人导航系统与导航定位方法，属于生物动力学与惯性导航技术领域。\n背景技术\n[0002] 行人导航是导航定位领域中的一个重要分支。行人导航系统可实时确定并监测个人的位置以及人体的运动状态，从而有效地提高军事作战人员、抢险搜救人员的快速反应能力，以及提供民用背景下行人实时、精确的定位信息，具有广阔的军事与民用应用前景。\n[0003] 基于惯性技术的行人导航技术始于上世纪九十年代，主要应用于美国等发达国家的未来士兵系统，该类技术中行人导航系统通常采用基于微惯性/地磁测量组件足部安装的系统结构，系统经无干扰磁环境中磁传感器误差标定与补偿后进行初始对准，之后导航系统进入导航工作状态，通过人体步态相位检测对导航系统进行间断性的零速修正，估计导航系统级误差以及陀螺仪与加速度计的部分漂移误差，但该方法无法有效地估计与修正航向误差，因此磁航向的精度对于行人导航系统的性能具有至关重要的作用。在上述研究中发现，人体运动中足部在常规动作中的加速度与角速度是人体躯干的3-5倍，在剧烈运动中甚至可达10倍。目前的中低精度惯性传感器件无法兼顾测量精度与量程间的矛盾，而这两个因素都是决定行人导航系统性能的重要因素，人体运动状态下的实时定位对惯性传感器量程与精度的综合性能提出了很高的要求。\n发明内容\n[0004] 为了克服现有技术的缺点，本发明提供了一种基于人体运动学模型的行人导航系统与导航定位方法，将惯性传感组件分布式地安装于人体髋部与下肢，在人体高过载运动中可有效克服测量信息超量程、冲击信号等对导航解算的影响，具有较高的实时定位性能。\n[0005] 本发明解决其技术问题所采用的技术方案是：\n[0006] 基于人体运动学模型的行人导航系统，所述行人导航系统包括惯性传感组件和微型导航计算机，所述惯性传感组件包括惯性与地磁测量单元以及惯性测量单元，其中，一组惯性与地磁测量单元安装于人体躯干髋部，多组惯性测量单元分别安装于双腿，微型导航计算机安装在人体躯干上，所述微型导航计算机与惯性传感组件之间通过电缆或无线通讯完成数据传输。\n[0007] 利用所述的基于人体运动学模型的行人导航系统的导航定位方法：躯干的惯性与地磁测量单元以及下肢的惯性测量单元通过确定足部零力矩点实现下肢各级杆臂效应补偿，并结合关节坐标系的肌肉骨骼间附着点坐标转换实现陀螺仪所测量的角速度的转换，从而构成足部虚拟惯性传感组件，通过该虚拟惯性传感组件完成行人导航过程。其具体步骤如下：\n[0008] (一)惯性传感组件的信号采集：\n[0009] 同步采集各组惯性传感组件的输出信号，得到各组角速度、比力和磁场强度，并将信号传输给微型导航计算机；\n[0010] (二)惯性传感组件的随机误差的实时建模与校正：\n[0011] 首先对零位偏置的特性进行统计分析，分别建立符合陀螺仪与加速度计的零位偏置趋势项，在陀螺仪与加速度计的信号输出端加入零位偏置趋势项校正；分别建立陀螺仪与加速度计的随机误差模型，将经过零位偏置趋势项建模与校正步骤后的陀螺仪与加速度计输出作为观测量，将陀螺仪与加速度计的理论输出估计值作为状态量，建立基于随机误差模型的卡尔曼滤波器，分别实时估计陀螺仪与加速度计的理论输出估计值；\n[0012] (三)惯性传感组件的初始对准：\n[0013] 该步骤同步应用于每组惯性传感组件，在微型导航计算机中进行；系统启动后在静态条件下，利用步骤(二)中加速度计的理论输出估计值，在微型导航计算机中通过水平自对准得到惯性传感组件初始横滚角与俯仰角，并将磁强计的信号输出通过通讯模块传送至微型导航计算机，得到惯性传感组件的初始航向角，水平自对准的公式为：\n[0014]\n[0015]\n[0016] 其中，θ为俯仰角，γ为横滚角，g为重力加速度， 与 分别为加速度计理论输出估计值在载体坐标系中投影的x与y轴分量；\n[0017] (四)人体下肢零力矩点位置的确定：\n[0018] 为实现下肢杆臂效应动态补偿，首先须得到人体运动中位于足底的下肢零力矩点；人体运动中支撑腿可视为动态杆臂，以下肢零力矩点为原点进行转动来实现人体质心的移动，下肢零力矩点关于下肢质心位置、各关节的角度、角速度和角加速度的表达式：\n[0019]\n[0020] 式中，qi为连杆i与z方向的夹角，li、di分别为连杆i的长度及其质心到关节的距离，G为人体所受重力；从该公式可知，下肢零力矩点的位置由各关节的角度qi、角速度 和角加速度 的函数，这些物理量均可通过安装于下肢的惯性测量单元测量与计算得到；\n[0021] (五)虚拟惯性传感组件的构建与杆臂效应动态补偿：\n[0022] 基于步骤(四)的原理，可在人体运动中实时计算支撑腿上零力矩点的精确位置，结合下肢各肢节的惯性参数，即可实时补偿足部至髋关节的杆臂效应；而支撑腿的零力矩点位置可视为一个静态的虚拟惯性传感器组件，该组件在足部静止相位中的速度与角速度均为0，其器件误差也可视为0，暂忽略人体运动中下肢肌肉与脂肪等所引起的肢体变形，各关节处的杆臂效应补偿公式如下：\n[0023] fn＝fn-1-ωn-1×(ωn-1×Rn-1),n＝1,2,3,4\n[0024] 其中，踝关节处比力f1由零力矩点位置比力f0结合足部运动角速度ω0与零力矩点位置距踝关节的距离R0进行补偿；膝关节处的比力f2由踝关节处比力f1结合踝关节运动角速度ω1与胫骨长度R1进行补偿；髋关节处的比力f3由膝关节处比力f2结合膝关节运动角速度ω2与股骨长度R2进行补偿；躯干部位的比力f4由髋关节处比力f3结合髋关节运动角速度ω3与惯性测量组件和髋关节之间的位置矢量R3补偿；若要在腿部任意位置的惯性测量组件之间进行杆臂效应的补偿，可将R1、R2与R3替代成惯性测量组件与相应关节之间的距离即可；\n[0025] (六)虚拟捷联惯性导航系统的惯性导航解算：\n[0026] 该步骤应用于虚拟惯性传感组件所构成的虚拟捷联惯性导航系统，虚拟捷联惯性导航系统的惯性导航解算步骤分为姿态解算与速度、位置解算；\n[0027] (七)虚拟捷联惯性导航系统的零速修正：\n[0028] 该步骤应用于虚拟捷联惯性导航系统，在微型导航计算机中进行，求解加速度计输出经步骤(二)中建模与校正后的理论输出估计值的三轴矢量和，通过判断该值是否接近重力来检测人体足部是否处于着地时间段，并利用着地时间段相对地面静止的特点进行间断式的零速修正，即根据虚拟捷联惯性导航系统误差与虚拟惯性传感组件误差建立系统状态方程与观测方程，实现人体行走中的零速修正，实时修正虚拟捷联惯性导航系统的误差与虚拟惯性传感组件误差。\n[0029] 所述步骤(六)的具体步骤为：\n[0030] (1)虚拟惯性导航系统的姿态解算：基于虚拟惯性传感器组件中的三轴陀螺仪，采用四元数法进行姿态解算；\n[0031] (2)虚拟惯性导航系统的速度、位置解算：把虚拟惯性传感器组件中的三轴加速度计原始输出 通过姿态转移矩阵 转换到导航坐标系中，记为 从而可\n以解算出人体足部在地理系中的速度，其微分方程为： 其中，\nfn为加速度计理论输出估计值在导航坐标系中的投影， 为地球自转角速度在导航坐标系中的投影， 为导航坐标系相对于地球坐标系的角速度在导航坐标系中的投影，可通过行进中的速度在导航坐标系中的投影求得，将上式移项并展开得：\n[0032]\n[0033] 式中， 分别是 在东向、北向和地向的投影分量，Ve、Vn、Vd分别是东向、北向和地向的速度分量，λ、L分别为当地的经度与纬度，g为当地重力加速度；\n[0034] 利用Ve、Vn、Vd并结合上个时刻人体的纬度、经度、高度与速度，求解当前时刻导航坐标系相对于惯性坐标系的角速度在导航坐标系中的投影 以及当前时刻载体的纬度L、经度λ、高度h，完成整个虚拟捷联惯性导航系统的算法流程。\n[0035] 本发明利用人体运动中下肢(包括足部、小腿、大腿，髋部)以及连接它们的各个关节之间的运动学规律构建虚拟惯性传感器组件，并实时修正该虚拟惯性传感器组件以及行人导航系统的各项误差，能实现人体运动中的精确导航定位。本发明方法解决了在快速步态下由于人体下肢的运动会超出传感器组件的量程，以及肢体出现冲击、振动等现象时，现有行人导航系统无法有效实现导航定位的问题，并且在人体高过载行进中可有效克服测量信息超量程、冲击信号等对导航解算的影响，具有较高的实时定位性能。\n附图说明\n[0036] 图1为行人导航系统惯性传感组件安装位置示意图；1-惯性传感组件安装位置(躯干与右腿)，2-惯性传感组件安装位置(左腿)。\n[0037] 图2为行人导航系统原理与流程图，其中，IMMU为惯性与地磁测量单元，IMU为惯性测量单元；\n[0038] 图3为人体下肢运动学模型在人体侧平面的投影图。\n具体实施方式\n[0039] 下面结合附图对本发明进一步说明。\n[0040] 如图1所示，基于惯性传感组件分布式安装的的行人导航系统：惯性传感组件(包括惯性与地磁测量单元以及惯性测量单元)在人体躯干与下肢的分布式安装，其中，一组惯性与地磁测量单元安装于人体躯干髋部位置，实际应用中可都采用中低精度的惯性与地磁测量单元，如MPU-6050型传感组件等；四组惯性测量单元分别安装于双腿的大腿与小腿质心位置附近，也可进一步在人体双脚分别安装一组惯性传感组件；微型导航计算机可安装于人体躯干，与上述传感器组件之间通过电缆连接，或采用无线通讯完成数据传输。\n[0041] 此外，人体下肢骨肌系统模型中骨骼附近的点表示接近肢体质心的肌肉表层，即惯性测量组件的安装位置；而微型导航计算机安装于躯干时其位置无特定要求。\n[0042] 如图2所示，基于人体下肢运动学机理的行人导航方法：其中，IMMU为惯性与地磁测量单元，IMU为惯性测量单元，躯干的惯性与地磁测量单元与下肢的惯性测量单元通过确定足部零力矩点实现下肢各级杆臂效应补偿，并结合关节坐标系的肌肉骨骼间附着点坐标转换实现陀螺仪所测量的角速度的转换，从而构成足部虚拟惯性传感组件，通过该虚拟惯性传感组件完成行人导航过程，具体步骤如下：\n[0043] 1)确定人体运动中的下肢零力矩点：人体运动中支撑腿可视为动态杆臂，该杆臂以下肢零力矩点为原点进行转动来实现人体质心的移动。由于常规运动中人体下肢运动中围绕各关节进行较低速的转动，可忽略各肢节转动惯量的影响。在此前提下，可根据人体下肢运动学方程得到足部零力矩点关于下肢质心位置与加速度、各关节的角度、角速度以及角加速度的表达式，通过分布式安装于人体躯干与下肢的惯性传感组件的输出，确定人体运动中的下肢零力矩点。\n[0044] 2)基于多级杆臂效应动态补偿方法的虚拟惯性传感组件构建机理：支撑腿的下肢零力矩点位置可视为安装了一个静态的虚拟惯性传感器组件，该组件在足部静止相位中的速度与角速度均为0。本发明中暂忽略人体运动中下肢肌肉与脂肪等所引起的肢体变形，利用下肢各肢节的惯性参数，通过多级传递对准基本原理可实现对髋部惯性传感组件的杆臂效应补偿，从而得到虚拟惯性传感器组件的实时输出。\n[0045] 3)基于虚拟惯性传感组件实现行人导航定位：对分布式结构的导航系统中的各惯性传感组件进行误差在线标定、初始对准，在人体步行运动中通过人体下肢运动检测方法来判断足部虚拟惯性传感组件是否处于静止相位，并在静止相位中通过零速修正技术实时修正其导航定位与虚拟惯性传感器的误差，从而完成行人导航定位功能。\n[0046] 本实施例导航定位方法的具体步骤如下：\n[0047] 1、惯性传感组件的信号采集：\n[0048] 同步采集各组惯性传感组件中陀螺仪、加速度计及磁传感器的输出信号，得到各组角速度、比力和磁场强度，并将信号通过通信电缆传输给安装于躯干的以DSP为处理器的微型导航计算机。\n[0049] 2、惯性传感组件的随机误差的实时建模与校正：\n[0050] 对于每组惯性传感组件，在陀螺仪与加速度计的零位偏置随时间变化较明显的情况下，首先均对零位偏置的特性进行统计分析，分别建立符合陀螺仪与加速度计的零位偏置趋势项，确定陀螺仪与加速度计的零偏趋势项模型后，在陀螺仪与加速度计的信号输出端加入零偏趋势项校正，对数据的零偏趋势项进行实时校正。\n[0051] 对校正趋势项后的陀螺仪与加速度计分别建立随机误差模型(如求和自回归滑动平均模型ARIMA模型)，将经过零偏趋势项建模与校正步骤后的陀螺仪与加速度计输出作为观测量，将陀螺仪与加速度计的理想输出估计值作为状态量，建立基于随机误差模型的卡尔曼滤波器，分别实时估计陀螺仪与加速度计的理论输出。\n[0052] 将惯性传感组件的理论输出估计值作为初始对准、捷联惯性导航、多级杆臂效应补偿以及虚拟惯性传感组件零速修正等步骤的数据来源。\n[0053] 3、惯性传感组件的初始对准：\n[0054] 该步骤同步应用于每组惯性传感组件，在微型导航计算机中进行，系统启动后在静态条件下，利用步骤2中加速度计的理论输出估计值，在微型导航计算机中通过水平自对准得到惯性传感组件初始横滚角与俯仰角，并将磁强计的信号输出通过通讯模块传送至微型导航计算机，得到惯性传感组件的初始航向角，水平自对准的公式为：\n[0055]\n[0056]\n[0057] 其中，θ为俯仰角，γ为横滚角，g为重力加速度， 与 分别为加速度计理论输出估计值在载体坐标系中投影的x与y轴分量。\n[0058] 4、人体下肢零力矩点位置的确定：\n[0059] 为实现下肢杆臂效应动态补偿，首先须得到人体运动中位于足底的下肢零力矩点，该位置决定了杆臂效应补偿的起始点。人体运动中支撑腿可视为动态杆臂，该杆臂以下肢零力矩点为原点进行转动来实现人体质心的移动。由于常规运动中人体下肢运动中围绕各关节进行较低速的转动，可忽略各肢节转动惯量的影响。人体下肢(包括足部、小腿、大腿，髋部)以及连接它们的各个关节的运动学模型在人体侧平面的投影如图3所示。\n[0060] 在此前提下，根据生物动力学领域通用的人体运动学方程，下肢零力矩点关于下肢质心位置、各关节的角度、角速度和角加速度的表达式：\n[0061]\n[0062] 式中，qi为连杆i与z方向的夹角，li、di分别为连杆i的长度及其质心到关节的距离，G为人体所受重力。从该公式可知，下肢零力矩点的位置由各关节的角度qi、角速度 和角加速度 的函数等决定，这些物理量均可通过安装于下肢的微惯性测量组件测量和计算得到。\n[0063] 5、虚拟惯性传感组件的构建与杆臂效应动态补偿\n[0064] 基于步骤4中确定下肢零力矩点位置的基本原理，可在人体运动中实时计算支撑腿上零力矩点的精确位置，结合下肢各肢节的惯性参数，即可实时补偿足部至髋关节的杆臂效应；而支撑腿的零力矩点位置可视为一个静态的虚拟惯性传感器组件，该组件在足部静止相位中的速度与角速度均为0，为简化计算过程其器件误差也可视为0。利用该位置的虚拟惯性传感组件与下肢的运动学信息，可通过传递对准基本原理实现对髋关节位置惯性系统的杆臂效应补偿，并实现对系统误差的实时修正。本研究中暂忽略人体运动中下肢肌肉与脂肪等所引起的肢体变形，各关节处的杆臂效应补偿公式如下：\n[0065] fn＝fn-1-ωn-1×(ωn-1×Rn-1),n＝1,2,3,4   (4)\n[0066] 其中，踝关节处比力f1由零力矩点位置比力f0结合足部运动角速度ω0与零力矩点位置距踝关节的距离R0进行补偿；膝关节处的比力f2由踝关节处比力f1结合踝关节运动角速度ω1与胫骨长度R1进行补偿；髋关节处的比力f3由膝关节处比力f2结合膝关节运动角速度ω2与股骨长度R2进行补偿；躯干部位的比力f4由髋关节处比力f3结合髋关节运动角速度ω3与惯性测量组件和髋关节之间的位置矢量R3补偿。若要在腿部任意位置的惯性测量组件之间进行杆臂效应的补偿，可将R1、R2与R3替代成组件与相应关节之间的距离即可。\n[0067] 6、虚拟捷联惯性导航系统的惯性导航解算\n[0068] 该步骤应用于虚拟惯性传感组件所构成的虚拟捷联惯性导航系统。虚拟捷联惯性导航系统的惯性导航解算步骤分为姿态解算和速度、位置解算。\n[0069] (1)姿态解算\n[0070] 虚拟捷联惯性导航系统的姿态解算有欧拉角法、方向余弦法和四元数法，而四元数法具有可以全姿态工作、计算工作量小等特点，故本发明采用四元数法进行姿态解算，主要步骤如下：\n[0071] 1)载体相对导航坐标系角速度 的计算：\n[0072]\n[0073] 其中， 为MEMS陀螺仪输出的理论估计值， 可通过行进速度在导航坐标系中的投影、地球自转角速度ωie以及上个周期的姿态转移矩阵 求得。\n[0074] 2)四元数微分方程的求解：\n[0075] 四元数微分方程的离散化形式，即四元数迭代计算公式如下：\n[0076]\n[0077] 其中， 为规范化后的四元数， 为相互正交的单\n位矢量；l为四元数算法计算的单位时间间隔； ω1、\nω2、ω3分别为在一次姿态解算周期内对MEMS陀螺仪输出角速度的3次采样值，即采用3子样采样方法补偿陀螺非定轴转动时角速度矢量积分产生的计算误差，MEMS陀螺仪采样周期为h。\n[0078]\n[0079]\n[0080] 上式中，[]表示向量的矩阵扩展，ΔΦ02＝ΔΦx2+ΔΦy2+ΔΦz2。\n[0081] 3)四元数规范化：\n[0082] 姿态解算中计算误差的存在使得计算的变换四元数的范数不再等于1，即计算的四元数失去规范性，因此对计算的四元数必须周期性地进行规范化处理。设\n为直接由四元数微分方程计算得到的四元数，而设规范化后的四\n元数为 其中 为相互正交的单位矢量，利用最小二乘判\n据可以求得最优规范化的变换四元数,公式如下：\n[0083]\n[0084] 4)由四元数计算姿态转移矩阵\n[0085] 依据四元数表示的固定矢量之间的变换关系，可获得四元数与方向余弦矩阵的关系为：\n[0086]\n[0087] 5)由姿态转移矩阵 提取姿态角：\n[0088]\n[0089]\n[0090]\n[0091] 其中，θ、γ、φ分别为捷联惯性导航系统的俯仰角、横滚角和航向角；c'11、c'12、c'13、c'21、c'22、c'23、c'31、c'32、c'33分别为姿态转移矩阵 对应行列位置的各元素，即c11'＝cosγcosφ+sinγsinθsinφ、c12'＝cosθsinφ、c13'＝-sinγcosφ-cosγsinθsinφ、c21'＝-cosγsinφ+sinγsinθcosφ、c22'＝cosθcosφ、c23'＝-sinγsinφ-cosγsinθcosφ、c31'＝-sinγcosθ、c32'＝sinθ、c33'＝cosγcosθ。\n[0092] (2)速度、位置解算：\n[0093] 虚拟捷联惯性导航系统的速度解算中，由于加速度计固连在MEMS惯性测量组件上，它的输出是载体相对于惯性空间的比力在载体坐标系中的投影，因此需要把加速度计原始输出 通过姿态转移矩阵 转换到导航坐标系中，记为 即\n[0094]\n[0095] 从而可以解算出载体在地理系中的速度，其微分方程为：\n[0096]\n[0097] 其中，fn为MEMS加速度计理论输出估计值在导航坐标系中的投影， 为地球自转角速度在导航坐标系中的投影， 为导航坐标系相对于地球坐标系的角速度在导航坐标系中的投影，可通过行进中的速度在导航坐标系中的投影求得。将上式移项并展开得：\n[0098]\n[0099] 式中， 分别是 在东向、北向和地向的投影分量，Ve、Vn、Vd分别是东向、北向和地向的速度分量，λ、L分别为当地的经度与纬度，g为当地重力加速度。\n[0100] 利用Ve、Vn、Vd并结合上个时刻载体的纬度、经度、高度与速度，求解当前时刻导航坐标系相对于惯性坐标系的角速度在导航坐标系中的投影 以及当前时刻载体的纬度L、经度λ、高度h，完成整个MEMS捷联惯性导航系统的算法流程。\n[0101] 7、虚拟捷联惯性导航系统的零速修正：\n[0102] 该步骤应用于虚拟捷联惯性导航系统，在微型导航计算机中进行，求解加速度计输出经步骤2误差建模与校正后的估计值的三轴矢量和，通过判断该值是否接近重力来检测人体足部是否处于着地时间段，并利用着地时间段相对地面静止的特点进行间断式的零速修正，即根据惯性导航系统误差与惯性器件误差特性误差建立状态方程：\n[0103]\n[0104] 式中，误差状态量 其中δVN、δVE为水\n平速度误差沿北、东方向的分量，φN、φE、φD为平台误差角在北、东、地方向的分量；\n分别为加速度计的常值零偏在载体坐标系中x、y方向的分量；εx、εy、εz分别为陀螺零位常值漂移在载体坐标系中x、y、z方向的分量；状态转移矩阵 其中，\nΩD,ΩN为地球自转角速度在地\n向与北向的分量，c11'～c33'分别为惯性导航系统姿态转移矩阵 中的对应行列位置上的各元素，定义同上；g为当地重力加速度；W＝[w1 w2 w3 w4 w5]，式中w1、w2为等效北向、东向的加速度计输出白噪声分量，w3、w4、w5为等效北向、东向、地向的陀螺仪输出白噪声分量。以足部着地点MEMS惯性导航系统的水平速度输出作为观测量，建立系统的观测方程：\n[0105]\n[0106] 式中，δVN、δVE分别为水平速度误差沿北、东方向的分量，X定义同上；V为观测噪声矢量。\n[0107] 运用最优滤波方法实时估计惯性导航系统的初始平台误差角，陀螺仪随机常值误差以及加速度计随机常值误差，并以反馈校正的方式提高虚拟捷联惯性导航系统的水平姿态精度与虚拟惯性器件精度。