CN103759730B - 一种基于导航信息双向融合的行人与智能移动载体的协同导航系统及其导航方法 - Google Patents

一种基于导航信息双向融合的行人与智能移动载体的协同导航系统及其导航方法 Download PDF

Info

Publication number
CN103759730B
CN103759730B CN201410020089.8A CN201410020089A CN103759730B CN 103759730 B CN103759730 B CN 103759730B CN 201410020089 A CN201410020089 A CN 201410020089A CN 103759730 B CN103759730 B CN 103759730B
Authority
CN
China
Prior art keywords
navigation system
error
pedestrian
intelligent mobile
system
Prior art date
Application number
CN201410020089.8A
Other languages
English (en)
Other versions
CN103759730A (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 CN201410020089.8A priority Critical patent/CN103759730B/zh
Publication of CN103759730A publication Critical patent/CN103759730A/zh
Application granted granted Critical
Publication of CN103759730B publication Critical patent/CN103759730B/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 preceding groups G01C1/00-G01C19/00
    • G01C21/10Navigation; Navigational instruments not provided for in preceding groups G01C1/00-G01C19/00 by using measurements of speed or acceleration
    • G01C21/12Navigation; Navigational instruments not provided for in preceding 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 preceding 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
    • GPHYSICS
    • G01MEASURING; TESTING
    • G01CMEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00Navigation; Navigational instruments not provided for in preceding groups G01C1/00-G01C19/00
    • G01C21/04Navigation; Navigational instruments not provided for in preceding groups G01C1/00-G01C19/00 by terrestrial means
    • G01C21/08Navigation; Navigational instruments not provided for in preceding 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
    • G01C25/00Manufacturing, calibrating, cleaning, or repairing instruments or devices referred to in the other groups of this subclass
    • G01C25/005Manufacturing, calibrating, cleaning, or repairing instruments or devices referred to in the other groups of this subclass initial alignment, calibration or starting-up of inertial devices

Abstract

本发明公开了一种基于导航信息双向融合的行人与智能移动载体协同导航系统及其导航方法,属于惯性与组合导航领域。该系统由安装于人体的行人导航系统与智能移动载体上安装的惯性导航系统组成,在系统进入存在信号屏蔽或电磁干扰的未知环境时,构建信息双向融合滤波器同步修正两套系统的导航信息误差,即利用智能移动载体惯性导航系统中较高的传感器精度完成行人导航系统磁航向误差的实时补偿,同时利用行人导航系统较高的位移精度修正智能移动载体惯性导航系统误差,实现同步提高两套导航系统的定位性能。本方法改进了环境存在信号屏蔽或电磁干扰的情况下行人导航与移动载体导航系统的定位方法,有助于全面提高人机一体化智能系统的综合性能。

Description

-种基于导航信息双向融合的行人与智能移动载体的协同导 航系统及其导航方法

技术领域

[0001] 本发明设及一种基于人体与智能移动载体构建的人机一体化智能导航系统及其 导航方法,属于惯性与组合导航的技术领域。

背景技术

[0002] 人机一体化智能系统(简称人机智能系统)是一类人与智能机器共同感知、共同认 知、共同决策,实现人与智能机器在决策层面上进行融合,产生更高行动效率的智能系统, 该系统在军事行动、工程作业、残疾人行动辅助等领域具有很高的应用价值。人机智能系统 中各类信息的交互融合是建立人机禪合协同作业机制的重要前提。利用行人导航系统与智 能移动载体可构建人机一体化智能导航系统,该系统由安装于人体的行人导航系统与智能 移动载体上安装的惯性导航系统组成,可实现复杂电磁环境中的导航定位任务。

[0003] -体化智能导航系统所设及的行人导航技术是导航定位技术中的一个重要分支, 近年来随着应用环境的不断拓展,行人导航定位技术的性能与环境适应性被提出了新的要 求。目前,较为成熟的行人导航定位方案多数是基于卫星导航系统实现定位功能,例如,美 国、英国、法国为等多个军事发达国家所实施"陆地勇:t"、"FIST"、"阳LIN"等多种高技术± 兵装备计划中,均W卫星导航系统为作战±兵提供定位信息。美国化aper实验室在本世纪 初提出并设计了基于微机电惯性测量组件的个人导航定位方案,并研究了行进中的零速修 正与静止状态下的零姿态修正方法,实时修正惯性导航系统与微惯性传感器的误差;瑞± Vectronix公司的研究部口研制的个人导航系统将磁传感器与MEMS巧螺仪相结合,W克服 环境中的磁场干扰对航向信息的影响,并采用高精度计步器探测人体的运动状态与加速 度,通过优化后的多传感器信息融合算法来实现个人导航。在国内,南京航空航天大学导航 研究中屯、与香港理工大学等研究机构研究了 W视觉障碍者紧急避障、个人定位、语音导航 等服务为应用背景、结合MEMS惯性技术的行人导航方法。

[0004] -体化智能导航系统所设及的智能移动载体则是一类安装了具有特定功能载荷 的移动载体,可通过实时控制或自主的形式实现按一定要求运动,并对环境进行信息与态 势感知的智能化系统,例如智能移动机器人、智能车辆等。智能移动载体的有关研究是机器 人领域里的一个重要方向,由于其应用的广泛性与功能的重要性,近年来受到该领域国内 外学者的深入探讨与研究。针对智能移动载体的导航技术的研究,也成为了智能移动载体 研究的热点之一 :1)微小型无人飞行器正在被国内外研究机构广泛研究,微小型飞行器的 使用场合多在室内等信号遮蔽的环境下,因此视觉导航是广泛应用于运类特定环境下的导 航手段。目前,英国牛津大学,美国斯坦福大学,西班牙马德里理工大学等知名高校都在着 力研究基于视觉信息的无人飞行器导航定位、定姿技术;2)基于移动机器人的导航技术近 些年也被广泛重视,移动机器人大致分为两类,一类按照固定轨迹或者路标点运动,运一类 机器人使用路标点实现实时的导航和控制,典型的如清华大学的TOMR-III等。另一类不受 固定轨迹约束,根据自身对环境的感知进行路径规划并实施任务,例如,卡内基梅隆大学的 机器人Xavier,采用视觉组合测距传感器,利用扩展卡尔曼滤波方式进行导航决策;3)随着 现代交通事业的发展,智能车辆的概念逐渐进入人们的视野,国外对于智能汽车的研究起 步较早且目前已初见成效,德国联邦国防大学与奔驰公司共同开发的智能汽车VaMoRs,该 汽车使用四台CCD相机实时捕捉道路情况并使用卡尔曼滤波器拟合,一般路况条件下的智 能汽车可W达到完全自主的状态,国内清华大学等高校与研究机构采用差分GPS、磁罗盘等 传感器形成组合定位导航系统,此外还配置有激光测距仪,视觉传感器等设备感知周围环 境。

发明内容

[0005] 为了提高一体化智能导航系统的综合定位性能与可靠性,本发明提供了一种人机 一体化智能导航系统,包括行人导航系统和智能移动载体(涵盖上述各类载体)导航系统。 其中智能移动载体导航系统采用中高精度捷联惯性导航系统,在进入存在信号屏蔽或电磁 干扰的未知环境时,如果单独采用惯性系统将无法满足人机智能系统的导航定位性能需 求。针对该问题,本发明又提出了一种基于行人导航系统与智能移动载体惯性导航系统的 信息双向融合协同导航方法。该方法利用两种导航系统不同的误差特性,构建信息双向融 合滤波器,同步修正两者的导航信息误差,即利用智能移动载体的中高精度捷联惯导系统 中较高的传感器精度完成对行人导航系统磁航向误差的实时修正,同时利用行人导航系统 较高的单步位移精度修正机器人捷联惯导系统的误差,从而实现同步提高两类载体导航系 统的定位与航向精度。

[0006] 本发明的导航系统采用的技术方案如下:

[0007] -种基于导航信息双向融合的行人与智能移动载体的协同导航系统,包括行人导 航系统与智能移动载体;其中,所述行人导航系统采用足部微惯性导航系统,安装于人体 上;所述智能移动载体(如移动机器人、智能车等)中安装有高精度捷联惯性导航系统;所述 行人导航系统与智能移动载体的导航系统之间通过无线通讯方式进行数据交换。

[0008] 本发明提供的导航方法,包括如下步骤:

[0009] 行人导航系统经干净磁环境中磁传感器误差标定与补偿后进行导航系统初始对 准,系统进入导航工作状态,通过人体步态相位检测对导航系统进行间断性的零速修正,估 计导航系统级误差W及巧螺仪与加速度计的部分漂移误差,利用智能移动载体的中高精度 惯导系统中较高的传感器精度完成行人导航系统磁航向误差的实时补偿;智能移动载体的 捷联惯性导航系统采用行人导航系统零速修正后的位置信息来实时修正捷联惯性导航系 统误差与惯性传感器误差,从而构成行人与智能移动载体的协同导航,具体步骤如下:

[0010] (1)行人导航系统与传感器误差建模与修正:行人导航系统与传感器误差在初始 对准与零速修正中得到修正,采用捷联惯性导航系统的线性误差模型,同时采用最优估计 理论与滤波方法完成对准与零速修正;行人导航系统误差模型包括平台误差角模型、速度 误差模型、定位误差W及惯性传感器误差模型,经粗对准后MEMS惯性传感器的误差分别近 似为随机常值与白噪声的结合W及导航过程中的惯性器件随机常值误差模型;在足部运动 过程中,采用智能移动载体的捷联惯性导航系统提供的航向误差为观测量构建观测方程, 即采用航向匹配的方法完成行人导航系统的误差估计与补偿;

[0011] (2)智能移动载体的捷联惯性导航系统的误差建模与修正:建立系统误差模型,并 采用随机常值、一阶马尔科夫过程W及白噪声相结合的惯性器件误差模型,行人导航系统 的零速修正使其误差积累速度由时间的=次函数转变随位移的线性函数,行进速度无突变 情况下基于零速修正的行人导航系统误差特性近似为随时间的线性函数;将行人导航系统 的位置信息作为观测量,通过观测量扩充法实现基于最优估计理论的系统误差估计与修 正。

[0012] 若对环境中某一位置地磁场与其他磁场源所产生的磁场进行辨识与分离,则可有 效修正由环境干扰源所导致的磁航向误差。通过分析磁场矢量在水平面上的投影可进行磁 场的辨识方法的研究,求得该地理位置上的磁航向角修正值。由于磁航向角修正值与载体 的姿态无关,只与载体位置与磁场干扰源的相对位置有关,可通过该特性实现高精度惯性 系统对低精度惯性系统的航向修正。基于上述理论分析,利用人机一体化智能系统中智能 移动载体中的惯性导航系统含有的较高精度的传感器实时修正行人导航系统的磁航向误 差,其具体步骤如下:

[0013] (1)在磁场环境纯净的区域,对与智能移动载体的捷联惯性导航系统中的惯性传 感器同轴安装且经过标定的中高精度=轴磁传感器组件进行硬、软磁误差补偿,补偿方法 采用基于楠圆约束的多位置最小二乘拟合方法,采用捷联惯性导航系统来测量机器人水平 姿态,从而结合磁传感器的输出解算磁航向,并利用当地地理位置信息修正磁偏角;

[0014] (2)行人与智能移动载体同步进入电磁干扰环境中,利用智能移动载体中高精度 捷联惯导系统输出的航向角为磁偏角06,并W其磁传感器输出与水平姿态数据解算电磁干 扰条件下带有误差的磁偏角0d,从而求解磁航向角修正值A 0,并将该值实时通过数据通讯 传递给行人导航系统,实现行人导航系统的航向修正。

[0015] 本发明与现有技术相比,具有如下显著优点:

[0016] (1)本发明利用行人导航系统与智能移动载体导航系统中两种不同的系统误差特 性,采用信息双向融合协同导航方法,构建信息双向融合的系统结构,同步修正两个导航系 统的信息误差。

[0017] (2)本发明利用智能移动载体捷联惯导系统中较高的传感器精度完成行人导航系 统磁航向误差的实时补偿,提高行人导航系统的航向精度。

[0018] (3)本发明采用行人导航系统零速修正后的位置信息来实时修正捷联惯导系统误 差与惯性传感器误差,从而提高智能移动载体的导航定位性能。

附图说明

[0019] 图1环境存在信号屏蔽与电磁干扰情况下的信息双向融合协同导航系统原理图;

[0020] 图2未知环境中磁传感器所敏感的磁矢量示意图;

[0021 ]图3地磁场与环境磁源产生磁场的水平分量示意图。

具体实施方式

[0022]本发明内容所设及的人机一体化智能导航系统,由安装于人体的行人导航系统与 智能移动载体上安装的导航系统组成,行人与智能移动载体同步行进,行人导航系统与智 能移动载体的导航系统之间通过蓝牙、UWB等无线通讯方式进行数据交换。在具体的工程实 施过程中,行人导航系统可采用足部微惯性/卫星组合导航系统,而智能移动载体导航系统 可采用中高精度捷联惯性/卫星组合导航系统,从而提高系统的综合定位性能与可靠性。在 卫星导航系统可用的环境中,两者的导航系统可独立工作,但在进入存在电磁干扰或信号 屏蔽的未知环境时,卫星导航系统与磁航向计将会受到严重影响甚至失效,单独采用惯性 系统将无法满足人机智能系统的导航定位性能需求。因此,提出一种基于行人导航系统与 智能移动载体惯性导航系统的信息双向融合协同导航方法,实现同步提高两类载体导航系 统的定位与航向精度。

[0023] 信息双向融合协同导航方法由采集IMU信号、巧螺仪与加速度计的随机误差建模 与校正、惯性导航系统初始对准、捷联惯性导航解算、惯性导航系统零速修正、磁航向误差 实时补偿、信息双向融合方法屯部分组成。信息双向融合协同导航方法中,首先是行人导航 系统采用基于微惯性/地磁测量组件足部安装的行人导航定位方案,经干净磁环境中磁传 感器误差标定与补偿后进行导航系统初始对准,系统进入导航工作状态;通过人体步态相 位检测对导航系统进行间断性的零速修正(Zero Velocity Update,简称ZUPT),估计导航 系统级误差W及巧螺仪与加速度计的部分漂移误差;基于磁航向误差实时补偿方法提高行 人导航系统的航向精度。同时,智能移动载体导航系统由于工作于捷联惯性模式,其精度随 时间降低。为保持智能移动载体捷联惯导系统航向信息的高精度,采用行人导航系统零速 修正后的位置信息来实时修正捷联惯导系统误差与惯性传感器误差,从而构成行人与智能 移动载体的协同导航。信息双向融合协同导航方法流程如图1所示,为了完成个人导航系统 和智能移动载体导航系统的测速、定姿、定位W及信息双向融合协同导航,需要完成的步骤 如W下实例所示:

[0024] 1. IMU信号采集步骤

[0025] 该步骤同步应用于行人导航系统和智能移动载体导航系统。采集个人导航系统和 智能移动载体导航系统的IMU(惯性测量组件)中巧螺与加速度计的输出信号,得到惯导系 统的角速度和比力,并将信号通过安装于足部与躯干的通讯模块传输给微型导航计算机。

[0026] 2.惯性器件的随机误差的建模与校正步骤

[0027] 该步骤同步应用于行人导航系统和智能移动载体导航系统。在两者微型导航计算 机中对上一步采集的巧螺仪与加速度计数据并分别建立相应的信息误差模型,实时估计巧 螺仪与加速度计的理论输出,并将理论输出估计值作为初始对准步骤、捷联惯性导航步骤、 零速修正步骤W及组合导航步骤等的数据源。在巧螺仪与加速度计的零位偏置随时间变化 较明显的情况下,首先需对零位偏置的特性进行统计分析,分别建立符合巧螺仪与加速度 计的多阶多项式零位偏置趋势项,如某型MEMS巧螺仪零偏趋势项适用模型为四阶多项式模 型,某型MEMS加速度计零偏趋势项适用模型为二阶多项式模型,其形式分别如公式(1)与 (2)所示:

Figure CN103759730BD00061

[002引 (1)

[0029] (2)

[0030] 式中,f 1 (t)与f 2(t)分别为巧螺仪与加速度计零偏的趋势项,t为时间,曰1、bi、Cl、 di、ei分别为四阶多项式模型的各阶参数,C2、cb、e2分别为二阶多项式模型的各阶参数。确定 巧螺仪与加速度计的零偏趋势项模型后,在MEMS巧螺仪与MEMS加速度计的信号输出端加入 零偏趋势项校正,对数据的零偏趋势项进行实时校正。

[0031] 对校正趋势项后的巧螺仪与加速度计分别建立求和自回归滑动平均模型即ARIMA 模型,如某型MEMS巧螺仪随机误差的适用模型为ARMA(2,1)模型,其中2为自回归模型阶数, 1为滑动平均模型阶数,其模型结构如公式(3)所示:

[0032] Xt= (}) 巧t-2+£i;-目IEt-I (3)

[0033] 式中Xt为t时刻的信号,Et为t时刻噪声,d) 1、d) 2为自回归模型参数,目1为滑动平均 模型参数。如某型MEMS加速度计随机误差的适用模型为AR(I)模型,其中2为自回归模型阶 数,其模型结构如公式所示:

[0034]

Figure CN103759730BD00071

(4)

[003引式中yt为t时刻的信号,Tt为t时刻噪声,&为自回归模型参数。将经过零偏趋势项 建模与校正步骤后的巧螺仪与加速度计输出作为观测量,将巧螺仪与加速度计的理想输出 估计值作为状态量,建立基于ARIMA模型的卡尔曼滤波器,分别实时估计巧螺仪与加速度计 的理想输出,卡尔曼滤波的状态方程如公式(5)所示:

[0036] Xk = AXk-i+BVk (5)

[0037] 其中疋=瞬,肴,为相邻两个时刻的惯性传感器(如MEMS巧螺仪或MEMS加速度 计等)的输出,Vk=[ak,ak-i]T,为相邻两个时刻的系统过程噪声,

Figure CN103759730BD00072

弓离散系统的 状态转移矩阵,

Figure CN103759730BD00073

^系统过程噪声系数矩阵,卡尔曼滤波的量测方程如公式(6)所 示:

[003引 Zk = HXk+Wk (6)

[0039] 将惯性传感器的理论输出估计值作为初始对准步骤、捷联惯性导航步骤、零速修 正步骤W及组合导航步骤的数据源。

[0040] 3.惯性系统初始对准步骤

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

Figure CN103759730BD00074

[0042] ()

[0043] 烤)

[0044] 其中0为俯仰角,丫为横滚角,g为重力加速度,与分别为加速度计理论输出 估计值在载体坐标系中投影的X与y轴分量;

[004引4.捷联惯性导航解算步骤

[0046] 该步骤同步应用于行人导航系统和智能移动载体导航系统。惯性导航系统的捷联 惯性导航解算步骤分为姿态解算与速度、位置解算。

[0047] (1)姿态解算

[0048] 捷联惯性导航系统的姿态解算有欧拉角法、方向余弦法和四元数法。由于四元数 法具有可W全姿态工作、计算工作量小等特点,故本方法采用该方法进行姿态解算。

[0049] 四元数法解算姿态主要有W下几个步骤:

[0050] 1)载体相对导航坐标系角速度仿已前计算

[0051]

Figure CN103759730BD00081

[0052] 其中做为MEMS巧螺仪输出的理论估计值,成可通过行进速度在导航坐标系中的 投影、地球自转角速度COieW及上个周期的姿态转移矩阵Ct求得。

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

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

[0055]

Figure CN103759730BD00082

(10)

[0056] 上式牛

Figure CN103759730BD00083

对规范化后的四元数,其中1、J、i为相互正 交的单位矢量,1为四元数算法计算的单位时间间隔;

Figure CN103759730BD00084

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

Figure CN103759730BD00085

[0057] (11)

[005引 (1巧

[0059] 上式中[]表示向量的矩阵扩展,A巫〇2= A 〇x2+A Oy2+A巫Z2。

[0060] 3)四元数规范化

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

Figure CN103759730BD00086

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

Figure CN103759730BD00087

,其中7、为相互正交的单位矢量,利用最小二乘判据可 W求得最优规范化的变换四元数。计算公式如下:

Figure CN103759730BD00088

(13)

[0062]

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

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

[00巧]

Figure CN103759730BD00091

谷4)

[0066] 5)由姿态转移矩阵提取姿态角

Figure CN103759730BD00092

[0067]

[006引 科巧

[0069]

[0070] 其中0、丫、4分别为捷联惯导系统的俯仰角、横滚角W及航向角,C'11、C'12、C'13、 0'21、(3'22、(3'23、(3'31、(3'32、(3'33分别为姿态转移矩阵巧对应行列位置的各元素,即(311'=(308 y COS 本+S iny sin 目 sin 本、C12'=CO S 目 sin 本、C13'一S in y CO S 本一COS y sin 目 sin 本、C21'=-cosysin<l)+sinysin9cos(l)、C22'=cos9cos(l)、C23'=-sinysin(l)-cosysin9cos(l)、C3i' = -sin y COS目、C32' =sin目、C33' =COS y COS目 D

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

[0072] 捷联惯性导航速度解算中,由于加速度计固连在MEMS惯性测量组件上,它的输出 是载体相对于惯性空间的比力在载体坐标系中的投影,因此需要把加速度计原始输出.记 通过姿态转移矩阵得转换到导航坐标系中,记为爲,即

[0073]

Figure CN103759730BD00093

(16)

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

[0075]

Figure CN103759730BD00094

(巧

[0076] 式中r为MEMS加速度计理论输出估计值在导航坐标系中的投影,6這为地球自转角 速度在导航坐标系中的投影,<2這为导航坐标系相对于地球坐标系的角速度在导航坐标系 中的投影,可通过行进中的速度在导航坐标系中的投影求得。将上式移项并展开得:

[0077]

Figure CN103759730BD00095

(IS)

[0078] 式中,:货、:乂"、:人"分别是/;;;在东向、北向和地向的投影分量,Ve、Vn、Vd分别是 东向、北向和地向的速度分量,a、l分别为当地的经度与缔度,g为当地重力加速度。

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

[0080] 5.惯性导航系统零速修正步骤

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

[0082]

Figure CN103759730BD00101

(巧)

[008引式中,误差状态量

Figure CN103759730BD00102

,其中SVn、SVe为水 平速度误差沿北、东方向的分量,4 N、4E、& D为平台误差角在北、东、地方向的分量; V、、V,分别为加速度计的常值零偏在载体坐标系中x、y方向的分量;ex、ey、ez分别为巧螺 零位常值漂移在载体坐标系中x、y、z方向的分量;状态转移矩阵

Figure CN103759730BD00103

,其中

Figure CN103759730BD00104

i Q D,Q N为地球自转角速度在地 向与北向的分量,C11'~C33'分别为惯性导航系统姿态转移矩阵巧中的对应行列位置上的 各元素,定义同上;g为当地重力加速度;W=[wi W2 W3 W4 W日],式中W1、W2为等效北向、东向 的加速度计输出白噪声分量,W3、W4、W5为等效北向、东向、地向的巧螺仪输出白噪声分量。W 足部着地点MEMS惯性导航系统的水平速度输出作为观测量,建立系统的观测方程:

[0084]

Figure CN103759730BD00105

(20)

[008引式中,SVn、SVe分别为水平速度误差沿北、东方向的分量,X定义同上;V为观测噪声 矢量。运用最优滤波方法实时估计MEMS惯性导航系统的初始平台误差角,MEMS巧螺仪随机 常值误差,W及MEMS加速度计随机常值误差,并W反馈校正的方式提高惯性导航系统的水 平姿态精度与惯性器件精度。

[0086] 6.捷联惯性导航系统/卫星导航系统组合导航步骤

[0087] 该步骤同步应用于行人导航系统与智能移动载体导航系统。分布构建行人导航系 统与智能移动载体导航系统的捷联惯性/卫星组合导航系统,将捷联惯性导航系统与卫星 导航系统构建组合导航子系统,根据惯性导航系统误差方程与惯性器件误差特性建立状态 方程:

[008引 X{t) = F{t)X{t) + G{t)W{t) (21)

[0089] 式中;

[0090]

Figure CN103759730BD00111

为系统状态量,分别代表惯性导航系统的3维平台误差角(其中航向误差角已经磁强计修 正)、3维速度误差、3维位置误差、巧螺的3轴随机常值和3轴一阶马尔可夫误差W及加速度 计的3轴一阶马尔可夫误差,共18阶。

[0091] W= [Wgx Wgy Wgz Wbx Wby Wbz Wax Way Waz]T为系统的白噪声矢量;

[0092]

Figure CN103759730BD00112

;:

[009引其中13X3表示3阶单位矩阵,Omxn表示mXn阶的0矩阵(下同),Fn表示对应平台误差 角、速度误差、位置误差9个基本导航参数误差的系数矩阵,其非零元素为

Figure CN103759730BD00113

[0094]

[0095]

[0096]

[0097]

[009引

[0099]

[0100]

[0101]

[0102]

[0103]

[0104]

[0105]

Figure CN103759730BD00121

[0106]

[0107]

[010 引

[0109]

[0110]

[0111]

[0112]

[0113]

[0114]

[0115]

[0116]

[0117]其中Wie为地球自转角速率,L为载体所在缔度,Re为地球赤道平面半径,Rm为地球 子午圈半径,Rn为地球卯酉圈半径,VN、VE、VU为载体北向、东向和天向速度,fN、fE、fu分别为 北向、东向和天向的比力,Fs和Fm分别为:

Figure CN103759730BD00122

[011 引

[0119]

[0120] 其中Trx、Try、Trz分别为巧螺仪一阶马尔科夫过程误差的相关时间,Tax、Tay、Taz分别 为加速度计一阶马尔科夫过程误差的相关时间,W惯性导航系统与卫星导航系统所输出的 位置、速度信息之间的差值为量测量建立量测方程,位置差值的观测方程分别为:

[0121] Zp(t)=Hp(t 巧(t)+Vp(t) (22)

[0122] 式中:Zp(t)表示位置差值:

Figure CN103759730BD00123

[0123] Vp=阳E Nn Nh]为量测噪声,可视为白噪声,其方差分别为灯Je,:伊苗,

[0124] 速度差值的观测方程为

[012 引 Zv(t)=Hv(t 化(t)+Vv(t) (23)

[0126] 式中:

Figure CN103759730BD00131

为量测噪声,可视 为白噪声,其方差分别为CT左::,巧Iv,。

[0127] 采用线性卡尔曼滤波等最优估计理论与方法实时估计惯性导航系统与惯性器件 的误差。

[0128] 7.基于智能移动载体中高精度惯性系统的行人导航系统磁航向误差实时补偿步 骤

[0129] 磁航向误差修正基本原理如下:地球磁场在空间中构成一个=维磁矢量,基于对 该磁矢量进行检测与分解,即可求得运动载体当前的磁航向信息。然而,实际应用中磁传感 器所测量的信号,是地磁场与环境干扰磁场源所共同形成的磁场。由于环境中干扰磁场源 的位置、数量、类型等往往无法确定,且随着载体的移动呈现不可预测的特点,因此对各类 干扰源产生的磁场进行建模与分析是十分复杂的,但若可对环境中某一位置地磁场与其他 磁场源所产生的磁场进行辨识与分离,则可有效修正由环境干扰源所导致的磁航向误差。

[0130] 磁航向的常规解算方法是将与载体固联的磁传感器检测的地磁场矢量投影在水 平面上,通过水平分量之间的=角函数关系求解载体磁航向。因此,通过分析磁场矢量在水 平面上的投影可进行磁场的辨识方法的研究。未知环境中的磁场分布示意图如图2所示。图 中mage表示地磁场矢量在水平面上的投影,magd表示环境干扰磁场源所产生的磁场矢量和 在水平面上的投影,ma針表示环境中某位置总磁场矢量的水平分量,即mage与magd的矢量 和。

[0131] 忽略地磁场异常等现象且暂时忽略磁偏角,在某地域内的地磁场矢量可视为已知 常量,表示为mage,并将磁偏角表示为06;在存在磁场干扰的环境中,将磁传感器所测量的环 境中总磁场矢量表示为magh,类似地将带有误差的磁偏角表示为0d。

[0132] 设S轴磁传感器测量的地磁场mage在载体坐标系下的投影为mag!,而测量总磁场 矢量magh在载体坐标系下的投影为mag::,其中

[0133]

Figure CN103759730BD00132

[0134] 若已知与磁传感器固联载体的横滚角与俯仰角分别为T与0,则可将破投影到 水平面与垂直的方向上。

[0135]

Figure CN103759730BD00133

(24)

[0136] (25)

Figure CN103759730BD00141

[0137]

[013引 (26)

[0139] (27)

[0140] 因此即可求得该地理位置上的磁航向角修正值A 0 = 0d-0e。由皿媒与mag;物理含 义可知,A 0与载体的姿态无关,只与载体位置与磁场干扰源的相对位置有关。因此,可通过 该特性实现高精度惯性系统对低精度惯性系统的航向修正。

[0141] 基于上述分析的磁航向角修正值,利用人机一体化智能系统中智能移动载体惯性 导航系统中较高精度的传感器实时修正行人导航系统的磁航向误差。具体实现步骤如下:

[0142] (1)在磁场环境纯净的区域,将与捷联惯性传感器同轴安装且经过标定的中高精 度=轴磁传感器搭载于智能移动载体上进行硬、软磁误差补偿,补偿方法采用基于楠圆约 束的多位置最小二乘拟合方法,采用捷联惯导系统来测量机器人水平姿态,从而结合磁传 感器的输出解算磁航向,并利用当地地理位置信息修正磁偏角;

[0143] (2)行人与智能移动载体同步进入电磁干扰环境中,利用机器人中高精度捷联惯 导系统输出的航向角为磁偏角06,并W其磁传感器输出与水平姿态数据解算0d,从而求解A 9,并将该值实时通过数据通讯传递给行人导航系统,实现行人导航系统的航向修正。

[0144] 8.行人导航系统与智能移动载体导航系统的信息双向融合协同导航步骤

[0145] 根据步骤7中所计算磁航向误差实时补偿方法提高行人导航系统的航向精度。同 时,采用行人导航系统零速修正后的位置信息来实时修正捷联惯导系统误差与惯性传感器 误差,从而构成行人与智能移动载体的协同导航。该步骤实现如下:

[0146] (1)行人导航系统与传感器误差建模与修正

[0147] 行人导航系统与传感器误差可在初始对准与零速修正中得到修正。由于初始对准 时间较短,而零速修正采用闭环校正的方式,系统与惯性传感器误差积累均为小量,因此可 采用捷联惯性导航系统的线性误差模型,同时采用线性卡尔曼滤波完成对准与零速修正。 行人导航系统误差模型由平台误差角模型、速度误差模型、定位误差W及惯性传感器误差 模型组成。其中平台误差角方程为:

[014引

Figure CN103759730BD00142

(28)

[0149]速度误差模型如下:

[0150]

Figure CN103759730BD00151

(29)

[0151] 定位误差模型为:

[0152]

Figure CN103759730BD00152

巧0)

[0153] 经粗对准后MEMS惯性传感器的误差可分别近似为随机常值与白噪声的结合,导航 过程中的惯性器件随机常值误差模型。

[0154] 在足部运动过程中,采用智能移动载体导航系统提供的航向误差为观测量构建观 测方程,即采用航向匹配的方法完成行人导航系统的误差估计与补偿。由于捷联惯性系统 姿态、航向误差与平台误差角之间存在如下转换关系:

[0155]

Figure CN103759730BD00153

(31)

[0156] 航向误差作为观测量本质上是对平台误差角的直接观测,人体足部周期性运动情 况下航向误差对于速度误差与位置误差也存在一定的间接观测性。在足部着地的过程中, 采用航向与速度同时匹配的观测方程。

[0157] (2)智能移动载体惯性导航系统的误差建模与修正

[0158] 为智能移动载体定位精度需求,通常安装中高精度的光纤IMU构成捷联系统,该系 统误差模型同样采用公式(28)-(30),并采用随机常值、一阶马尔科夫过程W及白噪声相结 合的惯性器件误差模型。

[0159] 行人导航系统的零速修正使其误差积累速度由时间的=次函数转变随位移的线 性函数,且人体行走过程中虽然步长不断变化,但总试图维持比较平稳的步速,因此在行进 速度不突变情况下基于零速修正的行人导航系统误差特性也可近似为随时间的线性函数。 将行人导航系统的位置信息作为观测量,通过观测量扩充法实现基于抗差滤波的系统误差 估计与修正,此时系统方程中将增加如下3阶方程组:

[0160]

Figure CN103759730BD00154

(32)

[0161] 即状态方程扩展为18阶。观测方程的形式如下:

[0162] Z =册+V (33)

[0163] 其中观测系数矩阵H扩展为3X21阶矩阵,其中H(16,1)=H(17,2)=H(18,3)=-1。

Claims (3)

1. 一种基于导航信息双向融合的行人与智能移动载体的协同导航系统,其特征在于, 该系统包括行人导航系统与智能移动载体;其中,所述行人导航系统采用足部微惯性导航 系统,安装于人体上;所述智能移动载体中安装有高精度捷联惯性导航系统;所述行人导航 系统与智能移动载体的导航系统之间通过无线通讯方式进行数据交换。
2. 利用如权利要求1所述协同导航系统的导航方法,其特征在于,包括如下步骤:行人 导航系统经干净磁环境中磁传感器误差标定与补偿后进行导航系统初始对准,系统进入导 航工作状态,通过人体步态相位检测对导航系统进行间断性的零速修正,估计导航系统级 误差以及陀螺仪与加速度计的部分漂移误差,利用智能移动载体的中高精度惯导系统中较 高的传感器精度完成行人导航系统磁航向误差的实时补偿;智能移动载体的捷联惯性导航 系统采用行人导航系统零速修正后的位置信息来实时修正捷联惯性导航系统误差与惯性 传感器误差,从而构成行人与智能移动载体的协同导航,具体步骤如下: (1) 行人导航系统与传感器误差建模与修正:行人导航系统与传感器误差在初始对准 与零速修正中得到修正,采用捷联惯性导航系统的线性误差模型,同时采用最优估计理论 与滤波方法完成对准与零速修正;行人导航系统误差模型包括平台误差角模型、速度误差 模型、定位误差以及惯性传感器误差模型,经粗对准后MEMS惯性传感器的误差分别近似为 随机常值与白噪声的结合以及导航过程中的惯性器件随机常值误差模型;在足部运动过程 中,采用智能移动载体的捷联惯性导航系统提供的航向误差为观测量构建观测方程,即采 用航向匹配的方法完成行人导航系统的误差估计与补偿; (2) 智能移动载体的捷联惯性导航系统的误差建模与修正:建立系统误差模型,并采用 随机常值、一阶马尔科夫过程以及白噪声相结合的惯性器件误差模型,行人导航系统的零 速修正使其误差积累速度由时间的三次函数转变随位移的线性函数,行进速度无突变情况 下基于零速修正的行人导航系统误差特性近似为随时间的线性函数;将行人导航系统的位 置信息作为观测量,通过观测量扩充法实现基于最优估计理论的系统误差估计与修正。
3. 根据权利要求2所述的导航方法,其特征在于,所述利用智能移动载体的中高精度惯 导系统中较高的传感器精度完成行人导航系统磁航向误差的实时补偿的具体步骤如下: (1) 在磁场环境纯净的区域,对与智能移动载体的捷联惯性导航系统中的惯性传感器 同轴安装且经过标定的中高精度三轴磁传感器组件进行硬、软磁误差补偿,补偿方法采用 基于椭圆约束的多位置最小二乘拟合方法,采用捷联惯性导航系统来测量机器人水平姿 态,从而结合磁传感器的输出解算磁航向,并利用当地地理位置信息修正磁偏角; (2) 行人与智能移动载体同步进入电磁干扰环境中,利用智能移动载体中高精度捷联 惯导系统输出的航向角为磁偏角,并以其磁传感器输出与水平姿态数据解算电磁干扰条 件下带有误差的磁偏角9d,从而求解磁航向角修正值ΛΘ,并将该值实时通过数据通讯传递 给行人导航系统,实现行人导航系统的航向修正。
CN201410020089.8A 2014-01-16 2014-01-16 一种基于导航信息双向融合的行人与智能移动载体的协同导航系统及其导航方法 CN103759730B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN201410020089.8A CN103759730B (zh) 2014-01-16 2014-01-16 一种基于导航信息双向融合的行人与智能移动载体的协同导航系统及其导航方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201410020089.8A CN103759730B (zh) 2014-01-16 2014-01-16 一种基于导航信息双向融合的行人与智能移动载体的协同导航系统及其导航方法

Publications (2)

Publication Number Publication Date
CN103759730A CN103759730A (zh) 2014-04-30
CN103759730B true CN103759730B (zh) 2016-06-29

Family

ID=50527006

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201410020089.8A CN103759730B (zh) 2014-01-16 2014-01-16 一种基于导航信息双向融合的行人与智能移动载体的协同导航系统及其导航方法

Country Status (1)

Country Link
CN (1) CN103759730B (zh)

Families Citing this family (14)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN103954285B (zh) * 2014-05-13 2018-06-22 北京信息科技大学 一种室内自主三维空间定位信息融合方法
CN104034328B (zh) * 2014-05-21 2017-03-29 哈尔滨工程大学 一种基于滤波方法和曲线拟合方法相结合的协同导航方法
CN104613963B (zh) * 2015-01-23 2017-10-10 南京师范大学 基于人体运动学模型的行人导航系统与导航定位方法
CN105259902B (zh) * 2015-09-06 2018-10-12 江苏科技大学 水下机器人惯性导航方法及系统
CN107289930A (zh) * 2016-04-01 2017-10-24 南京理工大学 基于mems惯性测量单元的纯惯性车辆导航方法
CN106017454B (zh) * 2016-06-16 2018-12-14 东南大学 一种基于多传感器融合技术的行人导航装置和方法
CN106211196B (zh) * 2016-06-22 2019-05-10 西北工业大学 基于马尔科夫随机场的多无人航行体协同一致性方法
CN106595653A (zh) * 2016-12-08 2017-04-26 南京航空航天大学 一种穿戴式行人自主导航系统及其导航方法
CN106767789B (zh) * 2017-01-12 2019-12-24 南京航空航天大学 一种基于自适应卡尔曼滤波的行人航向最优融合方法
CN107218938A (zh) * 2017-05-22 2017-09-29 南京航空航天大学 基于人体运动模型辅助的穿戴式行人导航定位方法和设备
CN107588772B (zh) * 2017-09-01 2020-02-21 北京臻迪科技股份有限公司 涉水机器人姿态监测方法、装置和监测系统
CN107728629A (zh) * 2017-09-19 2018-02-23 富平县韦加无人机科技有限公司 无人机磁异常检测系统及方法
CN108444473B (zh) * 2018-03-20 2020-08-28 南京华苏科技有限公司 一种行人室内轨迹定位方法
CN109883429A (zh) * 2019-04-15 2019-06-14 山东建筑大学 基于隐马尔科夫模型的零速检测方法以及室内行人惯性导航系统

Citations (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US645990A (en) * 1899-07-18 1900-03-27 Richard S Windsor Trolley-wheel.
CN101413800A (zh) * 2008-01-18 2009-04-22 南京航空航天大学 导航/稳瞄一体化系统的导航、稳瞄方法
EP2131147A1 (en) * 2008-05-15 2009-12-09 Honeywell International Inc. systems and methods for improving magnetic heading output from an inertial navigation system
CN102445200A (zh) * 2011-09-30 2012-05-09 南京理工大学 微小型个人组合导航系统及其导航定位方法
CN102980577A (zh) * 2012-12-05 2013-03-20 南京理工大学 一种微型捷联航姿系统及其工作方法

Family Cites Families (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US6459990B1 (en) * 1999-09-23 2002-10-01 American Gnc Corporation Self-contained positioning method and system thereof for water and land vehicles
US8224575B2 (en) * 2008-04-08 2012-07-17 Ensco, Inc. Method and computer-readable storage medium with instructions for processing data in an internal navigation system

Patent Citations (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US645990A (en) * 1899-07-18 1900-03-27 Richard S Windsor Trolley-wheel.
CN101413800A (zh) * 2008-01-18 2009-04-22 南京航空航天大学 导航/稳瞄一体化系统的导航、稳瞄方法
EP2131147A1 (en) * 2008-05-15 2009-12-09 Honeywell International Inc. systems and methods for improving magnetic heading output from an inertial navigation system
CN102445200A (zh) * 2011-09-30 2012-05-09 南京理工大学 微小型个人组合导航系统及其导航定位方法
CN102980577A (zh) * 2012-12-05 2013-03-20 南京理工大学 一种微型捷联航姿系统及其工作方法

Non-Patent Citations (3)

* Cited by examiner, † Cited by third party
Title
基于MIMU的行人导航算法研究;周启帆等;《系统仿真技术》;20101130;第6卷(第4期);281-286 *
基于足部微惯性/地磁测量组件的个人导航方法;钱伟行等;《中国惯性技术学报》;20121031;第20卷(第5期);567-572 *
鞋式个人导航系统算法和试验研究;崔潇等;《鞋式个人导航系统算法和试验研究》;20131231;第32卷(第3期);138-142 *

Also Published As

Publication number Publication date
CN103759730A (zh) 2014-04-30

Similar Documents

Publication Publication Date Title
CN104736963B (zh) 测绘系统和方法
Zhuang et al. Tightly-coupled integration of WiFi and MEMS sensors on handheld devices for indoor pedestrian navigation
Jiancheng et al. Study on innovation adaptive EKF for in-flight alignment of airborne POS
Storms et al. Magnetic field navigation in an indoor environment
CN102829777B (zh) 自主式水下机器人组合导航系统及方法
Cho et al. A dead reckoning localization system for mobile robots using inertial sensors and wheel revolution encoding
US8957812B1 (en) Position tracking system and method using radio signals and inertial sensing
CN103424114B (zh) 一种视觉导航/惯性导航的全组合方法
Morales et al. Signals of opportunity aided inertial navigation
Goshen-Meskin et al. Unified approach to inertial navigation system error modeling
Costanzi et al. An attitude estimation algorithm for mobile robots under unknown magnetic disturbances
Foxlin Pedestrian tracking with shoe-mounted inertial sensors
Titterton et al. Strapdown inertial navigation technology
US8005635B2 (en) Self-calibrated azimuth and attitude accuracy enhancing method and system (SAAAEMS)
El-Sheimy et al. The utilization of artificial neural networks for multisensor system integration in navigation and positioning instruments
Yang et al. Magnetometer and differential carrier phase GPS-aided INS for advanced vehicle control
Aghili et al. Driftless 3-D attitude determination and positioning of mobile robots by integration of IMU with two RTK GPSs
US10352959B2 (en) Method and system for estimating a path of a mobile element or body
CN201266089Y (zh) Ins/gps组合导航系统
Storms Magnetic field aided indoor navigation
CN102538781B (zh) 基于机器视觉和惯导融合的移动机器人运动姿态估计方法
Roumeliotis et al. Smoother based 3D attitude estimation for mobile robot localization
CN103217159B (zh) 一种sins/gps/偏振光组合导航系统建模及动基座初始对准方法
Georgy et al. Low-cost three-dimensional navigation solution for RISS/GPS integration using mixture particle filter
CN101319902B (zh) 一种低成本组合式定位定向装置及组合定位方法

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
C14 Grant of patent or utility model