[发明专利]基于人体运动学模型的行人导航系统与导航定位方法

权利要求书

1.基于人体运动学模型的行人导航系统，其特征在于，所述行人导航系统包括惯性传感组件和微型导航计算机，所述惯性传感组件包括惯性与地磁测量单元以及惯性测量单元，其中，一组惯性与地磁测量单元安装于人体躯干髋部，多组惯性测量单元分别安装于双腿，微型导航计算机安装在人体躯干上，所述微型导航计算机与惯性传感组件之间通过电缆或无线通讯完成数据传输；其中，躯干的惯性与地磁测量单元以及下肢的惯性测量单元通过确定足部零力矩点实现下肢各级杆臂效应补偿，并结合关节坐标系的肌肉骨骼间附着点坐标转换实现陀螺仪所测量的角速度的转换，从而构成足部虚拟惯性传感组件，通过该虚拟惯性传感组件完成行人导航过程。

2.利用如权利要求1所述基于人体运动学模型的行人导航系统的导航定位方法，其特征在于，其具体步骤如下：

(一)惯性传感组件的信号采集：

同步采集各组惯性传感组件的输出信号，得到各组角速度、比力和磁场强度，并将信号传输给微型导航计算机；

(二)惯性传感组件的随机误差的实时建模与校正：

首先对零位偏置的特性进行统计分析，分别建立符合陀螺仪与加速度计的零位偏置趋势项，在陀螺仪与加速度计的信号输出端加入零位偏置趋势项校正；分别建立陀螺仪与加速度计的随机误差模型，将经过零位偏置趋势项建模与校正步骤后的陀螺仪与加速度计输出作为观测量，将陀螺仪与加速度计的理论输出估计值作为状态量，建立基于随机误差模型的卡尔曼滤波器，分别实时估计陀螺仪与加速度计的理论输出估计值；

(三)惯性传感组件的初始对准：

该步骤同步应用于每组惯性传感组件，在微型导航计算机中进行；系统启动后在静态条件下，利用步骤(二)中加速度计的理论输出估计值，在微型导航计算机中通过水平自对准得到惯性传感组件初始横滚角与俯仰角，并将磁强计的信号输出通过通讯模块传送至微型导航计算机，得到惯性传感组件的初始航向角，水平自对准的公式为：

θ=arcsin(fyb/g)]]>

γ=arcsin(-fxb/(g*cosθ))]]>

其中，θ为俯仰角，γ为横滚角，g为重力加速度，与分别为加速度计理论输出估计值在载体坐标系中投影的x与y轴分量；

(四)人体下肢零力矩点位置的确定：

为实现下肢杆臂效应动态补偿，首先须得到人体运动中位于足底的下肢零力矩点；人体运动中支撑腿可视为动态杆臂，以下肢零力矩点为原点进行转动来实现人体质心的移动，下肢零力矩点关于下肢质心位置、各关节的角度、角速度和角加速度的表达式：

xZMP=Σi=16Σk=1i-1Σj=1i-1lkljq·j2sin(qj-qk)-Σk=1i-1Σj=1i-1lkljq··jcos(qj-qk)+diΣj=1i-1lj(q·j2-q·i2)sin(qj-qi)-diΣj=1i-1lj(q··i+q··j)cos(qj-qi)-di2q··i+loΣj=1i-1ljq·j2sin qj-loΣj=1i-1ljq··jcos qj+lodiq·i2sin qi-lodiq··icos qi+gΣj=1i-1ljsin qj+gdisin qiG-Σi=16miΣj=1i-1ljq·j2cos qj+Σj=1i-1ljq··jsin qj+diq·i2cos qi+diq··isin qi]]>

式中，q_i为连杆i与z方向的夹角，l_i、d_i分别为连杆i的长度及其质心到关节的距离，G为人体所受重力；从该公式可知，下肢零力矩点的位置由各关节的角度q_i、角速度和角加速度的函数，这些物理量均可通过安装于下肢的惯性测量单元测量与计算得到；

(五)虚拟惯性传感组件的构建与杆臂效应动态补偿：

基于步骤(四)的原理，可在人体运动中实时计算支撑腿上零力矩点的精确位置，结合下肢各肢节的惯性参数，即可实时补偿足部至髋关节的杆臂效应；而支撑腿的零力矩点位置可视为一个静态的虚拟惯性传感器组件，该组件在足部静止相位中的速度与角速度均为0，其器件误差也可视为0，暂忽略人体运动中下肢肌肉与脂肪等所引起的肢体变形，各关节处的杆臂效应补偿公式如下：

f_n＝f_n-1-ω_n-1×(ω_n-1×R_n-1),n＝1,2,3,4

其中，踝关节处比力f₁由零力矩点位置比力f₀结合足部运动角速度ω₀与零力矩点位置距踝关节的距离R₀进行补偿；膝关节处的比力f₂由踝关节处比力f₁结合踝关节运动角速度ω₁与胫骨长度R₁进行补偿；髋关节处的比力f₃由膝关节处比力f₂结合膝关节运动角速度ω₂与股骨长度R₂进行补偿；躯干部位的比力f₄由髋关节处比力f₃结合髋关节运动角速度ω₃与惯性测量组件和髋关节之间的位置矢量R₃补偿；若要在腿部任意位置的惯性测量组件之间进行杆臂效应的补偿，可将R₁、R₂与R₃替代成惯性测量组件与相应关节之间的距离即可；

(六)虚拟捷联惯性导航系统的惯性导航解算：

该步骤应用于虚拟惯性传感组件所构成的虚拟捷联惯性导航系统，虚拟捷联惯性导航系统的惯性导航解算步骤分为姿态解算与速度、位置解算；

(七)虚拟捷联惯性导航系统的零速修正：

该步骤应用于虚拟捷联惯性导航系统，在微型导航计算机中进行，求解加速度计输出经步骤(二)中建模与校正后的理论输出估计值的三轴矢量和，通过判断该值是否接近重力来检测人体足部是否处于着地时间段，并利用着地时间段相对地面静止的特点进行间断式的零速修正，即根据虚拟捷联惯性导航系统误差与虚拟惯性传感组件误差建立系统状态方程与观测方程，实现人体行走中的零速修正，实时修正虚拟捷联惯性导航系统的误差与虚拟惯性传感组件误差。

3.如权利要求2所述的导航定位方法，其特征在于，所述步骤(六)的具体步骤为：

(一)虚拟惯性导航系统的姿态解算：基于虚拟惯性传感器组件中的三轴陀螺仪，采用四元数法进行姿态解算；

(二)虚拟惯性导航系统的速度、位置解算：把虚拟惯性传感器组件中的三轴加速度计原始输出通过姿态转移矩阵转换到导航坐标系中，解算出人体足部在地理系中的速度，并结合上个时刻人体的纬度、经度、高度与速度，求解当前时刻载体的纬度、经度与高度，从而完成整个虚拟捷联惯性导航系统的算法流程。