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

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

Info

Publication number
CN104613963B
CN104613963B CN201510037160.8A CN201510037160A CN104613963B CN 104613963 B CN104613963 B CN 104613963B CN 201510037160 A CN201510037160 A CN 201510037160A CN 104613963 B CN104613963 B CN 104613963B
Authority
CN
China
Prior art keywords
mrow
msub
mover
navigation
munderover
Prior art date
Application number
CN201510037160.8A
Other languages
English (en)
Other versions
CN104613963A (zh
Inventor
钱伟行
熊智
曾庆化
赵宾
谢非
彭晨
马刚
杨建飞
Original Assignee
南京师范大学
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by 南京师范大学 filed Critical 南京师范大学
Priority to CN201510037160.8A priority Critical patent/CN104613963B/zh
Publication of CN104613963A publication Critical patent/CN104613963A/zh
Application granted granted Critical
Publication of CN104613963B publication Critical patent/CN104613963B/zh

Links

Classifications

    • GPHYSICS
    • G01MEASURING; TESTING
    • G01CMEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/10Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
    • G01C21/12Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
    • G01C21/16Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
    • G01C21/165Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation combined with non-inertial navigation instruments
    • GPHYSICS
    • G01MEASURING; TESTING
    • G01CMEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/04Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by terrestrial means
    • G01C21/08Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by terrestrial means involving use of the magnetic field of the earth
    • GPHYSICS
    • G01MEASURING; TESTING
    • G01CMEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/20Instruments for performing navigational calculations

Abstract

本发明公开了一种基于人体运动学模型的行人导航系统与导航定位方法,属于生物动力学与惯性导航技术相结合的领域。本发明利用人体运动中下肢(包括足部、小腿、大腿,髋部)以及连接它们的各个关节之间的运动学规律构建虚拟惯性传感器组件,并实时修正该虚拟惯性传感器组件以及行人导航系统的各项误差,能实现人体运动中的精确导航定位。本发明方法在人体高过载运动中,可有效克服测量信息超量程、冲击信号等对导航解算的影响,具有较高的实时定位性能。

Description

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

技术领域

[0001] 本发明涉及一种基于人体运动学模型的行人导航系统与导航定位方法,属于生物 动力学与惯性导航技术领域。

背景技术

[0002] 行人导航是导航定位领域中的一个重要分支。行人导航系统可实时确定并监测个 人的位置以及人体的运动状态,从而有效地提高军事作战人员、抢险搜救人员的快速反应 能力,以及提供民用背景下行人实时、精确的定位信息,具有广阔的军事与民用应用前景。

[0003] 基于惯性技术的行人导航技术始于上世纪九十年代,主要应用于美国等发达国家 的未来士兵系统,该类技术中行人导航系统通常采用基于微惯性/地磁测量组件足部安装 的系统结构,系统经无干扰磁环境中磁传感器误差标定与补偿后进行初始对准,之后导航 系统进入导航工作状态,通过人体步态相位检测对导航系统进行间断性的零速修正,估计 导航系统级误差以及陀螺仪与加速度计的部分漂移误差,但该方法无法有效地估计与修正 航向误差,因此磁航向的精度对于行人导航系统的性能具有至关重要的作用。在上述研究 中发现,人体运动中足部在常规动作中的加速度与角速度是人体躯干的3-5倍,在剧烈运动 中甚至可达10倍。目前的中低精度惯性传感器件无法兼顾测量精度与量程间的矛盾,而这 两个因素都是决定行人导航系统性能的重要因素,人体运动状态下的实时定位对惯性传感 器量程与精度的综合性能提出了很高的要求。

发明内容

[0004] 为了克服现有技术的缺点,本发明提供了一种基于人体运动学模型的行人导航系 统与导航定位方法,将惯性传感组件分布式地安装于人体髋部与下肢,在人体高过载运动 中可有效克服测量信息超量程、冲击信号等对导航解算的影响,具有较高的实时定位性能。

[0005] 本发明解决其技术问题所采用的技术方案是:

[0006] 基于人体运动学模型的行人导航系统,所述行人导航系统包括惯性传感组件和微 型导航计算机,所述惯性传感组件包括惯性与地磁测量单元以及惯性测量单元,其中,一组 惯性与地磁测量单元安装于人体躯干髋部,多组惯性测量单元分别安装于双腿,微型导航 计算机安装在人体躯干上,所述微型导航计算机与惯性传感组件之间通过电缆或无线通讯 完成数据传输。

[0007] 利用所述的基于人体运动学模型的行人导航系统的导航定位方法:躯干的惯性与 地磁测量单元以及下肢的惯性测量单元通过确定足部零力矩点实现下肢各级杆臂效应补 偿,并结合关节坐标系的肌肉骨骼间附着点坐标转换实现陀螺仪所测量的角速度的转换, 从而构成足部虚拟惯性传感组件,通过该虚拟惯性传感组件完成行人导航过程。其具体步 骤如下:

[0008] (一)惯性传感组件的信号采集:

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

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

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

[0012] (三)惯性传感组件的初始对准:

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

Figure CN104613963BD00061

[0014]

[0015]

[0016] 其中,Θ为俯仰角,γ为横滚角,g为重力加速度,

Figure CN104613963BD00062

分别为加速度计理论输出 估计值在载体坐标系中投影的X与y轴分量;

[0017] (四)人体下肢零力矩点位置的确定:

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

[0019]

Figure CN104613963BD00071

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

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

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

[0023] fn=fn-l-Wn-iX (ωη-iXRn-i) ,n=l,2,3,4

[0024] 其中,踝关节处比力5由零力矩点位置比力fo结合足部运动角速度ω〇与零力矩点 位置距踝关节的距离Ro进行补偿;膝关节处的比力5由踝关节处比力心结合踝关节运动角 速度U1与胫骨长度R1进行补偿;髋关节处的比力f3由膝关节处比力f2结合膝关节运动角速 度《2与股骨长度R2进行补偿;躯干部位的比力f4由髋关节处比力f3结合髋关节运动角速度 ω 3与惯性测量组件和髋关节之间的位置矢量R3补偿;若要在腿部任意位置的惯性测量组件 之间进行杆臂效应的补偿,可将R^R2与R3替代成惯性测量组件与相应关节之间的距离即 可;

[0025] (六)虚拟捷联惯性导航系统的惯性导航解算:

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

[0027] (七)虚拟捷联惯性导航系统的零速修正:

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

[0029] 所述步骤(六)的具体步骤为:

[0030] (1)虚拟惯性导航系统的姿态解算:基于虚拟惯性传感器组件中的三轴陀螺仪,采 用四元数法进行姿态解算;

[0031] (2)虚拟惯性导航系统的速度、位置解算:把虚拟惯性传感器组件中的三轴加速度 计原始输出,通过姿态转移矩阵

Figure CN104613963BD00081

转换到导航坐标系中,记为

Figure CN104613963BD00082

,从而可

Figure CN104613963BD00083

以解算出人体足部在地理系中的速度,其微分方程为:

Figure CN104613963BD00084

,其中, Γ为加速度计理论输出估计值在导航坐标系中的投影,

Figure CN104613963BD00085

为地球自转角速度在导航坐标 系中的投影,

Figure CN104613963BD00086

为导航坐标系相对于地球坐标系的角速度在导航坐标系中的投影,可通过 行进中的速度在导航坐标系中的投影求得,将上式移项并展开得:

[0032]

Figure CN104613963BD00087

[0033] 式中,

Figure CN104613963BD00088

分别是

Figure CN104613963BD00089

在东向、北向和地向的投影分量,Ve3Ud分别是东 向、北向和地向的速度分量,λ、L分别为当地的经度与炜度,g为当地重力加速度;

[0034] 利用Ve、Vn、Vd并结合上个时刻人体的炜度、经度、高度与速度,求解当前时刻导航 坐标系相对于惯性坐标系的角速度在导航坐标系中的投影以及当前时刻载体的炜度L、 经度λ、高度h,完成整个虚拟捷联惯性导航系统的算法流程。

[0035] 本发明利用人体运动中下肢(包括足部、小腿、大腿,髋部)以及连接它们的各个关 节之间的运动学规律构建虚拟惯性传感器组件,并实时修正该虚拟惯性传感器组件以及行 人导航系统的各项误差,能实现人体运动中的精确导航定位。本发明方法解决了在快速步 态下由于人体下肢的运动会超出传感器组件的量程,以及肢体出现冲击、振动等现象时,现 有行人导航系统无法有效实现导航定位的问题,并且在人体高过载行进中可有效克服测量 信息超量程、冲击信号等对导航解算的影响,具有较高的实时定位性能。

附图说明

[0036] 图1为行人导航系统惯性传感组件安装位置示意图;1-惯性传感组件安装位置(躯 干与右腿),2-惯性传感组件安装位置(左腿)。

[0037] 图2为行人导航系统原理与流程图,其中,頂MU为惯性与地磁测量单元,頂U为惯性 测量单元;

[0038] 图3为人体下肢运动学模型在人体侧平面的投影图。

具体实施方式

[0039] 下面结合附图对本发明进一步说明。

[0040] 如图1所示,基于惯性传感组件分布式安装的的行人导航系统:惯性传感组件(包 括惯性与地磁测量单元以及惯性测量单元)在人体躯干与下肢的分布式安装,其中,一组惯 性与地磁测量单元安装于人体躯干髋部位置,实际应用中可都采用中低精度的惯性与地磁 测量单元,如MPU-6050型传感组件等;四组惯性测量单元分别安装于双腿的大腿与小腿质 心位置附近,也可进一步在人体双脚分别安装一组惯性传感组件;微型导航计算机可安装 于人体躯干,与上述传感器组件之间通过电缆连接,或采用无线通讯完成数据传输。

[0041] 此外,人体下肢骨肌系统模型中骨骼附近的点表示接近肢体质心的肌肉表层,即 惯性测量组件的安装位置;而微型导航计算机安装于躯干时其位置无特定要求。

[0042] 如图2所示,基于人体下肢运动学机理的行人导航方法:其中,頂MU为惯性与地磁 测量单元,IMU为惯性测量单元,躯干的惯性与地磁测量单元与下肢的惯性测量单元通过确 定足部零力矩点实现下肢各级杆臂效应补偿,并结合关节坐标系的肌肉骨骼间附着点坐标 转换实现陀螺仪所测量的角速度的转换,从而构成足部虚拟惯性传感组件,通过该虚拟惯 性传感组件完成行人导航过程,具体步骤如下:

[0043] 1)确定人体运动中的下肢零力矩点:人体运动中支撑腿可视为动态杆臂,该杆臂 以下肢零力矩点为原点进行转动来实现人体质心的移动。由于常规运动中人体下肢运动中 围绕各关节进行较低速的转动,可忽略各肢节转动惯量的影响。在此前提下,可根据人体下 肢运动学方程得到足部零力矩点关于下肢质心位置与加速度、各关节的角度、角速度以及 角加速度的表达式,通过分布式安装于人体躯干与下肢的惯性传感组件的输出,确定人体 运动中的下肢零力矩点。

[0044] 2)基于多级杆臂效应动态补偿方法的虚拟惯性传感组件构建机理:支撑腿的下肢 零力矩点位置可视为安装了一个静态的虚拟惯性传感器组件,该组件在足部静止相位中的 速度与角速度均为〇。本发明中暂忽略人体运动中下肢肌肉与脂肪等所引起的肢体变形,利 用下肢各肢节的惯性参数,通过多级传递对准基本原理可实现对髋部惯性传感组件的杆臂 效应补偿,从而得到虚拟惯性传感器组件的实时输出。

[0045] 3)基于虚拟惯性传感组件实现行人导航定位:对分布式结构的导航系统中的各惯 性传感组件进行误差在线标定、初始对准,在人体步行运动中通过人体下肢运动检测方法 来判断足部虚拟惯性传感组件是否处于静止相位,并在静止相位中通过零速修正技术实时 修正其导航定位与虚拟惯性传感器的误差,从而完成行人导航定位功能。

[0046] 本实施例导航定位方法的具体步骤如下:

[0047] 1、惯性传感组件的信号采集:

[0048] 同步采集各组惯性传感组件中陀螺仪、加速度计及磁传感器的输出信号,得到各 组角速度、比力和磁场强度,并将信号通过通信电缆传输给安装于躯干的以DSP为处理器的 微型导航计算机。

[0049] 2、惯性传感组件的随机误差的实时建模与校正:

[0050] 对于每组惯性传感组件,在陀螺仪与加速度计的零位偏置随时间变化较明显的情 况下,首先均对零位偏置的特性进行统计分析,分别建立符合陀螺仪与加速度计的零位偏 置趋势项,确定陀螺仪与加速度计的零偏趋势项模型后,在陀螺仪与加速度计的信号输出 端加入零偏趋势项校正,对数据的零偏趋势项进行实时校正。

[0051] 对校正趋势项后的陀螺仪与加速度计分别建立随机误差模型(如求和自回归滑动 平均模型ARIMA模型),将经过零偏趋势项建模与校正步骤后的陀螺仪与加速度计输出作为 观测量,将陀螺仪与加速度计的理想输出估计值作为状态量,建立基于随机误差模型的卡 尔曼滤波器,分别实时估计陀螺仪与加速度计的理论输出。

[0052] 将惯性传感组件的理论输出估计值作为初始对准、捷联惯性导航、多级杆臂效应 补偿以及虚拟惯性传感组件零速修正等步骤的数据来源。

[0053] 3、惯性传感组件的初始对准:

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

Figure CN104613963BD00101

[0055] (1)

[0056] (2)

[0057] 其中,Θ为俯仰角,γ为横滚角,g为重力加速度,

Figure CN104613963BD00102

分别为加速度计理论输出 估计值在载体坐标系中投影的X与y轴分量。

[0058] 4、人体下肢零力矩点位置的确定:

[0059] 为实现下肢杆臂效应动态补偿,首先须得到人体运动中位于足底的下肢零力矩 点,该位置决定了杆臂效应补偿的起始点。人体运动中支撑腿可视为动态杆臂,该杆臂以下 肢零力矩点为原点进行转动来实现人体质心的移动。由于常规运动中人体下肢运动中围绕 各关节进行较低速的转动,可忽略各肢节转动惯量的影响。人体下肢(包括足部、小腿、大 腿,髋部)以及连接它们的各个关节的运动学模型在人体侧平面的投影如图3所示。

[0060] 在此前提下,根据生物动力学领域通用的人体运动学方程,下肢零力矩点关于下 肢质心位置、各关节的角度、角速度和角加速度的表达式:

[0061]

Figure CN104613963BD00111

[0062] 式中,qi为连杆i与z方向的夹角,Ind1分别为连杆i的长度及其质心到关节的距 离,G为人体所受重力。从该公式可知,下肢零力矩点的位置由各关节的角度qi、角速度¾.和 (3) 角加速度ί的函数等决定,这些物理量均可通过安装于下肢的微惯性测量组件测量和计算 得到。

[0063] 5、虚拟惯性传感组件的构建与杆臂效应动态补偿

[0064] 基于步骤4中确定下肢零力矩点位置的基本原理,可在人体运动中实时计算支撑 腿上零力矩点的精确位置,结合下肢各肢节的惯性参数,即可实时补偿足部至髋关节的杆 臂效应;而支撑腿的零力矩点位置可视为一个静态的虚拟惯性传感器组件,该组件在足部 静止相位中的速度与角速度均为0,为简化计算过程其器件误差也可视为0。利用该位置的 虚拟惯性传感组件与下肢的运动学信息,可通过传递对准基本原理实现对髋关节位置惯性 系统的杆臂效应补偿,并实现对系统误差的实时修正。本研究中暂忽略人体运动中下肢肌 肉与脂肪等所引起的肢体变形,各关节处的杆臂效应补偿公式如下:

[0065] fn=fn-l-«n-iX (ωη-iXRn-i) ,n=l,2,3,4 ⑷

[0066] 其中,踝关节处比力fi由零力矩点位置比力fo结合足部运动角速度ω ο与零力矩点 位置距踝关节的距离Ro进行补偿;膝关节处的比力5由踝关节处比力心结合踝关节运动角 速度U1与胫骨长度R1进行补偿;髋关节处的比力f3由膝关节处比力f2结合膝关节运动角速 度《2与股骨长度R2进行补偿;躯干部位的比力f4由髋关节处比力f3结合髋关节运动角速度 ω3与惯性测量组件和髋关节之间的位置矢量R3补偿。若要在腿部任意位置的惯性测量组件 之间进行杆臂效应的补偿,可将办、1?2与1?3替代成组件与相应关节之间的距离即可。

[0067] 6、虚拟捷联惯性导航系统的惯性导航解算

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

[0069] ⑴姿态解算

[0070] 虚拟捷联惯性导航系统的姿态解算有欧拉角法、方向余弦法和四元数法,而四元 数法具有可以全姿态工作、计算工作量小等特点,故本发明采用四元数法进行姿态解算,主 要步骤如下:

[0071] 1)载体相对导航坐标系角速度

Figure CN104613963BD00121

的计算:

[0072]

Figure CN104613963BD00122

(5)

[0073] 其中,

Figure CN104613963BD00123

为MEMS陀螺仪输出的理论估计值,

Figure CN104613963BD00124

可通过行进速度在导航坐标系中 的投影、地球自转角速度ω1θ以及上个周期的姿态转移矩阵

Figure CN104613963BD00125

求得。

[0074] 2)四元数微分方程的求解:

[0075] 四元数微分方程的离散化形式,即四元数迭代计算公式如下:

[0076]

Figure CN104613963BD00126

(6)

[0077] 其中,

Figure CN104613963BD00127

为规范化后的四元数,

Figure CN104613963BD00128

为相互正交的单 位矢量;1为四元数算法计算的单位时间间隔;

Figure CN104613963BD00129

. ω2、ω3分别为在一次姿态解算周期内对MEMS陀螺仪输出角速度的3次采样值,即采用3子样 采样方法补偿陀螺非定轴转动时角速度矢量积分产生的计算误差,MEMS陀螺仪采样周期为 h〇

Figure CN104613963BD001210

[0078] (7)

[0079] (Sj

Figure CN104613963BD001211

[0080]上式中,[]表示向量的矩阵扩展,

[0081] 3)四元数规范化:

[0082] 姿态解算中计算误差的存在使得计算的变换四元数的范数不再等于1,即计算的 四元数失去规范性,因此对计算的四元数必须周期性地进行规范化处理。设

Figure CN104613963BD001212

为直接由四元数微分方程计算得到的四元数,而设规范化后的四 元数为

Figure CN104613963BD001213

,其中

Figure CN104613963BD001214

为相互正交的单位矢量,利用最小二乘判 据可以求得最优规范化的变换四元数,公式如下: (9)

Figure CN104613963BD00131

[0083]

[0084] 4)由四元数计算姿态转移矩阵I

Figure CN104613963BD00132

[0085] 依据四元数表示的固定矢量之间的变换关系,可获得四元数与方向余弦矩阵的关 系为:

[0086]

Figure CN104613963BD00133

[0087] 5)由姿态转移矩阵

Figure CN104613963BD00134

提取姿态角:

Figure CN104613963BD00135

[0088] (11)

[0089] (12)

[0090] (13)

[0091] 其中,θ、γ、φ分别为捷联惯性导航系统的俯仰角、横滚角和航向角;c’n、c’12、 C ’ 13、C ’21、C ’22、C ’23、C ’31、C ’32、C ’33分别为姿态转移矩阵

Figure CN104613963BD00136

对应行列位置的各兀素,即Cll ’ = Cos γ cos Φ +sin γ sinQsin Φ、ci2 ’ =CosQsin Φ、ci3 ’=-Sin γ cos Φ -cos γ sinQsin Φ、 C21 ’ =_cos γ sin Φ +sin γ sinQcos Φ、C22 ’ =cosQcos Φ、C23 ’ =_sin γ sin Φ -cos γ sinQcos Φ、C3i’ =-sin y cosQ、c32’ =sinQ、C33’ =cos y cosQ〇

[0092] (2)速度、位置解算:

[0093] 虚拟捷联惯性导航系统的速度解算中,由于加速度计固连在MEMS惯性测量组件 上,它的输出是载体相对于惯性空间的比力在载体坐标系中的投影,因此需要把加速度计 原始输出

Figure CN104613963BD00137

通过姿态转移矩阵

Figure CN104613963BD00138

转换到导航坐标系中,记为

Figure CN104613963BD00139

,即

[0094]

Figure CN104613963BD001310

(14)

[0095] 从而可以解算出载体在地理系中的速度,其微分方程为:

[0096]

Figure CN104613963BD001311

(15)

[0097] 其中,fn为MEMS加速度计理论输出估计值在导航坐标系中的投影,

Figure CN104613963BD001312

为地球自转 角速度在导航坐标系中的投影,

Figure CN104613963BD001313

为导航坐标系相对于地球坐标系的角速度在导航坐标 系中的投影,可通过行进中的速度在导航坐标系中的投影求得。将上式移项并展开得:

[0098]

Figure CN104613963BD00141

(16)

[0099] 式中,

Figure CN104613963BD00142

分别是

Figure CN104613963BD00143

在东向、北向和地向的投影分量,VhVhVd分别是东 向、北向和地向的速度分量,λ、L分别为当地的经度与炜度,g为当地重力加速度。

[0100] 利用Ve、Vn、Vd并结合上个时刻载体的炜度、经度、高度与速度,求解当前时刻导航 坐标系相对于惯性坐标系的角速度在导航坐标系中的投影< 以及当前时刻载体的炜度L、 经度λ、高度h,完成整个MEMS捷联惯性导航系统的算法流程。

[0101] 7、虚拟捷联惯性导航系统的零速修正:

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

[0103]

Figure CN104613963BD00144

(.17)

[0104] 式中,误差状态量

Figure CN104613963BD00145

其中5Vn、5Ve为水 平速度误差沿北、东方向的分量,Φν、Φε、Φϋ为平台误差角在北、东、地方向的分量;

Figure CN104613963BD00146

分别为加速度计的常值零偏在载体坐标系中x、y方向的分量;分别为陀螺 零位常值漂移在载体坐标系中x、y、z方向的分量;状态转移矩阵

Figure CN104613963BD00147

,其中,

Figure CN104613963BD00148

,Qd, Ω N为地球自转角速度在地 向与北向的分量,cn’〜C33’分别为惯性导航系统姿态转移矩阵

Figure CN104613963BD00149

中的对应行列位置上的 各元素,定义同上;g为当地重力加速度;W=[wi W2 W3 W4 W5],式中W1、W2为等效北向、东向 的加速度计输出白噪声分量,W3、W4、W5为等效北向、东向、地向的陀螺仪输出白噪声分量。以 足部着地点MEMS惯性导航系统的水平速度输出作为观测量,建立系统的观测方程:

[0105]

Figure CN104613963BD001410

(IS)

[0106] 式中,δνΝ、δνΕ分别为水平速度误差沿北、东方向的分量,X定义同上;V为观测噪声 矢量。

[0107] 运用最优滤波方法实时估计惯性导航系统的初始平台误差角,陀螺仪随机常值误 差以及加速度计随机常值误差,并以反馈校正的方式提高虚拟捷联惯性导航系统的水平姿 态精度与虚拟惯性器件精度。

Claims (3)

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

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN201510037160.8A CN104613963B (zh) 2015-01-23 2015-01-23 基于人体运动学模型的行人导航系统与导航定位方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201510037160.8A CN104613963B (zh) 2015-01-23 2015-01-23 基于人体运动学模型的行人导航系统与导航定位方法

Publications (2)

Publication Number Publication Date
CN104613963A CN104613963A (zh) 2015-05-13
CN104613963B true CN104613963B (zh) 2017-10-10

Family

ID=53148522

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201510037160.8A CN104613963B (zh) 2015-01-23 2015-01-23 基于人体运动学模型的行人导航系统与导航定位方法

Country Status (1)

Country Link
CN (1) CN104613963B (zh)

Families Citing this family (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN106017454B (zh) * 2016-06-16 2018-12-14 东南大学 一种基于多传感器融合技术的行人导航装置和方法
CN106595653A (zh) * 2016-12-08 2017-04-26 南京航空航天大学 一种穿戴式行人自主导航系统及其导航方法
CN106821391B (zh) * 2017-03-23 2020-03-24 北京精密机电控制设备研究所 基于惯性传感器信息融合的人体步态采集分析系统及方法
CN107218938A (zh) * 2017-05-22 2017-09-29 南京航空航天大学 基于人体运动模型辅助的穿戴式行人导航定位方法和设备
EP3671119A4 (en) * 2017-09-30 2020-08-26 Huawei Tech Co Ltd Attitude matrix calculating method and device
CN109059910A (zh) * 2018-07-25 2018-12-21 南京师范大学 基于惯性和肌电信息并结合机器学习的行人导航系统和方法

Citations (8)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN102261915A (zh) * 2011-04-27 2011-11-30 浙江大学 行人惯性导航装置和方法
CN102445200A (zh) * 2011-09-30 2012-05-09 南京理工大学 微小型个人组合导航系统及其导航定位方法
CN101579238B (zh) * 2009-06-15 2012-12-19 吴健康 人体运动捕获三维再现系统及其方法
CN103076619A (zh) * 2012-12-27 2013-05-01 山东大学 一种消防员室内外3d无缝定位及姿态检测系统及方法
CN103759730A (zh) * 2014-01-16 2014-04-30 南京师范大学 一种基于导航信息双向融合的行人与智能移动载体的协同导航系统及其导航方法
GB2510548A (en) * 2012-02-08 2014-08-13 Ian Pratt Personal navigation system
CN104007460A (zh) * 2014-05-30 2014-08-27 北京中电华远科技有限公司 一种消防员单兵定位导航装置
CN104019828A (zh) * 2014-05-12 2014-09-03 南京航空航天大学 高动态环境下惯性导航系统杆臂效应误差在线标定方法

Family Cites Families (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US7561960B2 (en) * 2006-04-20 2009-07-14 Honeywell International Inc. Motion classification methods for personal navigation

Patent Citations (8)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN101579238B (zh) * 2009-06-15 2012-12-19 吴健康 人体运动捕获三维再现系统及其方法
CN102261915A (zh) * 2011-04-27 2011-11-30 浙江大学 行人惯性导航装置和方法
CN102445200A (zh) * 2011-09-30 2012-05-09 南京理工大学 微小型个人组合导航系统及其导航定位方法
GB2510548A (en) * 2012-02-08 2014-08-13 Ian Pratt Personal navigation system
CN103076619A (zh) * 2012-12-27 2013-05-01 山东大学 一种消防员室内外3d无缝定位及姿态检测系统及方法
CN103759730A (zh) * 2014-01-16 2014-04-30 南京师范大学 一种基于导航信息双向融合的行人与智能移动载体的协同导航系统及其导航方法
CN104019828A (zh) * 2014-05-12 2014-09-03 南京航空航天大学 高动态环境下惯性导航系统杆臂效应误差在线标定方法
CN104007460A (zh) * 2014-05-30 2014-08-27 北京中电华远科技有限公司 一种消防员单兵定位导航装置

Non-Patent Citations (2)

* Cited by examiner, † Cited by third party
Title
基于ZMP误差校正的仿人机器人步行控制;付根平 等;《机器人》;20130131;第35卷(第1期);第39-44页 *
基于足部微惯性/地磁测量组件的个人导航方法;钱伟行 等;《中国惯性技术学报》;20121031;第20卷(第5期);第567-572页 *

Also Published As

Publication number Publication date
CN104613963A (zh) 2015-05-13

Similar Documents

Publication Publication Date Title
Fourati Heterogeneous data fusion algorithm for pedestrian navigation via foot-mounted inertial measurement unit and complementary filter
Qiu et al. Inertial/magnetic sensors based pedestrian dead reckoning by means of multi-sensor fusion
Yun et al. Design, implementation, and experimental results of a quaternion-based Kalman filter for human body motion tracking
Roetenberg et al. Estimating body segment orientation by applying inertial and magnetic sensing near ferromagnetic materials
US8005635B2 (en) Self-calibrated azimuth and attitude accuracy enhancing method and system (SAAAEMS)
Won et al. A triaxial accelerometer calibration method using a mathematical model
EP1985233B1 (fr) Procédé et dispositif de détection d&#39;un axe de rotation sensiblement invariant
Chen et al. An effective pedestrian dead reckoning algorithm using a unified heading error model
Schepers et al. Ambulatory assessment of ankle and foot dynamics
US8165844B2 (en) Motion tracking system
DE602004010923T2 (de) Verwendung eines oder mehrerer signale auf der basis einer oder mehrerer verbundbewegungen eines individuums zur durchführung einer bestimmung einer positionsänderung des individuums
Giansanti et al. Is it feasible to reconstruct body segment 3-D position and orientation using accelerometric data?
ES2576731T3 (es) Monitor de marcha
Brigante et al. Towards miniaturization of a MEMS-based wearable motion capture system
Giansanti et al. The development and test of a device for the reconstruction of 3-D position and orientation by means of a kinematic sensor assembly with rate gyroscopes and accelerometers
US7561960B2 (en) Motion classification methods for personal navigation
Bachmann et al. Inertial and magnetic posture tracking for inserting humans into networked virtual environments
US8315823B2 (en) Force and/or motion measurement system having inertial compensation and method thereof
US6826477B2 (en) Pedestrian navigation method and apparatus operative in a dead reckoning mode
Sabatini et al. Assessment of walking features from foot inertial sensing
CN103575299B (zh) 利用外观测信息的双轴旋转惯导系统对准及误差修正方法
Jiménez et al. Indoor pedestrian navigation using an INS/EKF framework for yaw drift reduction and a foot-mounted IMU
Ojeda et al. Personal dead-reckoning system for GPS-denied environments
JP3602095B2 (ja) 地形を進む有脚動物のための地形ナビゲーション装置
CN105203098B (zh) 基于九轴mems传感器的农业机械全姿态角更新方法

Legal Events

Date Code Title Description
PB01 Publication
C06 Publication
SE01 Entry into force of request for substantive examination
C10 Entry into substantive examination
GR01 Patent grant
GR01 Patent grant