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

技术领域

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

背景技术

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

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

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

发明内容

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

本发明的导航系统采用的技术方案如下：

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

本发明提供的导航方法，包括如下步骤：

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

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

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

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

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

（2）行人与智能移动载体同步进入电磁干扰环境中，利用智能移动载体中高精度捷 联惯导系统输出的航向角为航向基准θ_e，并以其磁传感器输出与水平姿态数据解算θ_h， 从而求解Δθ，并将该值实时通过数据通讯传递给行人导航系统，实现行人导航系统的航 向修正。

本发明与现有技术相比，具有如下显著优点：

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

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

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

附图说明

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

图2未知环境中磁传感器所敏感的磁矢量示意图；

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

具体实施方式

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

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

4.1IMU信号采集步骤

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

4.2惯性器件的随机误差的建模与校正步骤

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

f₁(t)=a₁t⁴+b₁t³+c₁t²+d₁t+e₁  (1)

f₂(t)=c₂t²+d₂t+e₂  (2)

式中，f₁(t)与f₂(t)分别为陀螺仪与加速度计零偏的趋势项，t为时间，a₁、b₁、c₁、 d₁、e₁分别为四阶多项式模型的各阶参数，c₂、d₂、e₂分别为二阶多项式模型的各阶参 数。确定陀螺仪与加速度计的零偏趋势项模型后，在MEMS陀螺仪与MEMS加速度计 的信号输出端加入零偏趋势项校正，对数据的零偏趋势项进行实时校正。

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

x_t=φ₁x_t-1+φ₂x_t-2+ε_t-θ₁ε_t-1  (3)

式中x_t为t时刻的信号，ε_t为t时刻噪声，φ₁、φ₂为自回归模型参数，θ₁为滑动平均 模型参数。如某型MEMS加速度计随机误差的适用模型为AR(1)模型，其中2为自回归 模型阶数，其模型结构如公式所示：

y_t=ζ₁y_t-1+τ_t  (4)

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

X_k=AX_k-1+BV_k  (5) 其中为相邻两个时刻的惯性传感器（如MEMS陀螺仪或MEMS加速度 计等）的输出，V_k=[a_k,a_k-1]^T，为相邻两个时刻的系统过程噪声，为离散系 统的状态转移矩阵，为系统过程噪声系数矩阵，卡尔曼滤波的量测方程如公 式(6)所示：

Z_k=HX_k+W_k  (6)

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

4.3惯性系统初始对准步骤

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

$\theta =\arcsin (f_y^b/g)---\left(7\right)$

$\gamma =\arcsin (-f_x^b/(g*\cos \theta ))---\left(8\right)$

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

4.4捷联惯性导航解算步骤

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

(1)姿态解算

捷联惯性导航系统的姿态解算有欧拉角法、方向余弦法和四元数法。由于四元数法 具有可以全姿态工作、计算工作量小等特点，故本方法采用该方法进行姿态解算。 四元数法解算姿态主要有以下几个步骤：

1)载体相对导航坐标系角速度的计算

${\omega }_{\mathrm{nb}}^b={\omega }_{\mathrm{ib}}^b-{\omega }_{\mathrm{in}}^b---\left(9\right)$

其中为MEMS陀螺仪输出的理论估计值，可通过行进速度在导航坐标系 中的投影、地球自转角速度ω_ie以及上个周期的姿态转移矩阵求得。

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

四元数微分方程的离散化形式，即四元数迭代计算公式如下：

$q(t+l)=\{\cos \frac{{\mathrm{\Delta \Phi }}_0}{2}I+\frac{\sin \frac{{\mathrm{\Delta \Phi }}_0}{2}}{{\mathrm{\Delta \Phi }}_0}[\mathrm{\Delta \Phi }\left]\right\}q\left(t\right)---\left(10\right)$

上式中为规范化后的四元数，其中为相互 正交的单位矢量，l为四元数算法计算的单位时间间隔； ω₁、ω₂、ω₃分别为在一次姿态解算周期内对 MEMS陀螺仪输出角速度的3次采样值，即采用3子样采样方法补偿陀螺非定轴转 动时角速度矢量积分产生的计算误差，MEMS陀螺仪采样周期为h。

$\mathrm{\Delta \theta }=\left(\begin{array}{c}{\mathrm{\Delta \theta }}_x\\ {\mathrm{\Delta \theta }}_y\\ {\mathrm{\Delta \theta }}_z\end{array}\right)={\int }_t^{t+h}\left(\begin{array}{c}{\omega }_{\mathrm{nbx}}^b\\ {\omega }_{\mathrm{nby}}^b\\ {\omega }_{\mathrm{nbz}}^b\end{array}\right)\mathrm{dt}---\left(11\right)$

$\left[\mathrm{\Delta \Phi }\right]=\left(\begin{array}{cccc}0& -{\mathrm{\Delta \Phi }}_x& -{\mathrm{\Delta \Phi }}_y& {-\mathrm{\Delta \Phi }}_z\\ {\mathrm{\Delta \Phi }}_x& 0& {\mathrm{\Delta \Phi }}_z& {-\mathrm{\Delta \Phi }}_y\\ {\mathrm{\Delta \Phi }}_y& {-\mathrm{\Delta \Phi }}_z& 0& {\mathrm{\Delta \Phi }}_x\\ {\mathrm{\Delta \Phi }}_z& {\mathrm{\Delta \Phi }}_y& {-\mathrm{\Delta \Phi }}_x& 0\end{array}\right)---\left(12\right)$

上式中[]表示向量的矩阵扩展，ΔΦ₀²=ΔΦ_x²+ΔΦ_y²+ΔΦ_z²。

3)四元数规范化

姿态解算中计算误差的存在使得计算的变换四元数的范数不再等于1，即计算 的四元数失去规范性，因此对计算的四元数必须周期性地进行规范化处理。设 为直接由四元数微分方程计算得到的四元数，而设规范化后 的四元数为其中为相互正交的单位矢量，利用最 小二乘判据可以求得最优规范化的变换四元数。计算公式如下：

$q_i=\frac{q_i^{\prime }}{{\left(\underset{j=0}{\overset{3}{\Sigma }}{q}_j^{\prime 2}\right)}^{1/2}},i=\mathrm{0,1,2,3}---\left(13\right)$

4)由四元数计算姿态转移矩阵

依据四元数表示的固定矢量之间的变换关系，可以获得四元数与方向余弦矩阵的关 系为：

$C_n^b=\left(\begin{array}{ccc}{q}_1^2+q_0^2-q_3^2-q_2^2& 2(q_1{q}_2+q_0{q}_3)& 2(q_1{q}_3-q_0{q}_2)\\ 2(q_1{q}_2-q_0{q}_3)& q_2^2-q_3^2+q_0^2-q_1^2& 2(q_2{q}_3+q_0{q}_1)\\ 2(q_1{q}_3+q_0{q}_2)& 2(q_2{q}_3-q_0{q}_1)& q_3^2-q_2^2-q_1^2+q_0^2\end{array}\right)---\left(14\right)$

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

$\left(\begin{array}{c}\theta =\arctan \left(\frac{{c^{\prime }}_{23}}{\sqrt{{c^{\prime 2}}_{21}+{c^{\prime 2}}_{22}}}\right)\\ \gamma =\arctan (-\frac{{c^{\prime }}_{13}}{{c^{\prime }}_{33}})\\ \phi =\arctan \left({\frac{{c^{\prime }}_{21}}{{c^{\prime }}_{22}}}_{}\right)\end{array}\right)---\left(15\right)$

其中θ、γ、φ分别为捷联惯导系统的俯仰角、横滚角以及航向角，c'₁₁、c'₁₂、c'₁₃、 c'₂₁、c'₂₂、c'₂₃、c'₃₁、c'₃₂、c'₃₃分别为姿态转移矩阵对应行列位置的各元素，即 c₁₁'=cosγcosφ+sinγsinθsinφ、c₁₂'=cosθsinφ、c₁₃'-sinγcosφ-cosγsinθsinφ、 c₂₁'=-cosγsinφ+sinγsinθcosφ、c₂₂'=cosθcosφ、c₂₃'=-sinγsinφ-cosγsinθcosφ、 c₃₁'=-sinγcosθ、c₃₂'=sinθ、c₃₃'=cosγcosθ。

(2)速度、位置解算

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

$f_{\mathrm{ib}}^n=C_b^n·f_{\mathrm{ib}}^b---\left(16\right)$

从而可以解算出载体在地理系中的速度，其微分方程为：

$f^n=\stackrel{·}{v_{\mathrm{ep}}^n}+(2{\omega }_{\mathrm{ie}}^n+{\omega }_{\mathrm{ep}}^n)\times v_{\mathrm{ep}}^n-g^n---\left(17\right)$

式中fⁿ为MEMS加速度计理论输出估计值在导航坐标系中的投影，为地球自转 角速度在导航坐标系中的投影，为导航坐标系相对于地球坐标系的角速度在导航坐标 系中的投影，可通过行进中的速度在导航坐标系中的投影求得。将上式移项并展开得：

$\left(\begin{array}{c}{\stackrel{·}{V}}_e=f_e^n-(\stackrel{·}{\lambda }+{2\omega }_{\mathrm{ie}})\sin L{V}_e+\stackrel{·}{L}{V}_d\\ {\stackrel{·}{V}}_n=f_n^n-(\stackrel{·}{\lambda }+2{\omega }_{\mathrm{ie}})(\sin L{V}_n+\cos {\mathrm{LV}}_d)\\ {\stackrel{·}{V}}_d=f_d^n-\stackrel{·}{L}{V}_n-(\stackrel{·}{\lambda }+{2\omega }_{\mathrm{ie}})\cos L{V}_e+g\end{array}\right)---\left(18\right)$

式中，分别是在东向、北向和地向的投影分量，V_e、V_n、V_d分别 是东向、北向和地向的速度分量，λ、L分别为当地的经度与纬度，g为当地重力加速 度。

利用V_e、V_n、V_d并结合上个时刻载体的纬度、经度、高度与速度，求解当前时刻导 航坐标系相对于惯性坐标系的角速度在导航坐标系中的投影以及当前时刻载体的纬 度L、经度λ、高度h，完成整个MEMS捷联惯性导航的算法流程。

4.5惯性导航系统零速修正步骤

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

$\stackrel{·}{X}=\mathrm{AX}+W---\left(19\right)$

式中，误差状态量$X=\left(\begin{array}{cccccccccc}{\mathrm{\delta V}}_N& \delta V_E& {\phi }_N& {\phi }_E& {\phi }_D& {▿}_x& {▿}_y& {ϵ}_x& {ϵ}_y& {ϵ}_z\end{array}\right),$其中δV_N、δV_E为 水平速度误差沿北、东方向的分量，φ_N、φ_E、φ_D为平台误差角在北、东、地方向的分量； 分别为加速度计的常值零偏在载体坐标系中x、y方向的分量；ε_x、ε_y、ε_z分 别为陀螺零位常值漂移在载体坐标系中x、y、z方向的分量；状态转移矩阵 $A=\left(\begin{array}{cc}F& T\\ 0_5& 0\end{array}\right),$其中$F=\left(\begin{array}{ccccc}0& 2{\Omega }_D& 0& g& 0\\ -2{\Omega }_D& 0& -g& 0& 0\\ 0& 0& 0& {\Omega }_D& 0\\ 0& 0& -{\Omega }_D& 0& {\Omega }_N\\ 0& 0& 0& -{\Omega }_N& 0\end{array}\right),T=\left(\begin{array}{ccccc}{c_{11}}^{\prime }& {c_{12}}^{\prime }& 0& 0& 0\\ {c_{21}}^{\prime }& {c_{22}}^{\prime }& 0& 0& 0\\ 0& 0& {c_{11}}^{\prime }& {c_{12}}^{\prime }& {c_{13}}^{\prime }\\ 0& 0& {c_{21}}^{\prime }& {c_{22}}^{\prime }& {c_{23}}^{\prime }\\ 0& 0& {c_{31}}^{\prime }& {c_{32}}^{\prime }& {c_{33}}^{\prime }\end{array}\right),$Ω_D,Ω_N为地球自转角速度在地向与北向的分量，c₁₁'～c₃₃'分别为惯性导航系统姿态转移矩阵中的对应行列位置上的各元素，定义同上；g为当地重力加速度； W=[w₁w₂w₃w₄w₅]，式中w₁、w₂为等效北向、东向的加速度计输出白噪声分量， w₃、w₄、w₅为等效北向、东向、地向的陀螺仪输出白噪声分量。以足部着地点MEMS 惯性导航系统的水平速度输出作为观测量，建立系统的观测方程：

$Z=\left(\begin{array}{c}{\mathrm{\delta V}}_N\\ {\mathrm{\delta V}}_E\end{array}\right)=\left(\begin{array}{cccccccccc}1& 0& 0& 0& 0& 0& 0& 0& 0& 0\\ 0& 1& 0& 0& 0& 0& 0& 0& 0& 0\end{array}\right)X+V---\left(20\right)$

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

4.6捷联惯性导航系统/卫星导航系统组合导航步骤

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

$\stackrel{·}{X}\left(t\right)=F\left(t\right)X\left(t\right)+G\left(t\right)W\left(t\right)---\left(21\right)$

式中：

$X=\left(\begin{array}{ccccccccccccccccc}{\phi }_E& {\phi }_N& {\phi }_U& \delta v_E& \delta v_N& \delta v_U& \mathrm{\delta L}& \mathrm{\delta \lambda }& \mathrm{\delta h}& {ϵ}_{\mathrm{bx}}& {ϵ}_{\mathrm{by}}& {ϵ}_{\mathrm{bz}}& {ϵ}_{\mathrm{rx}}& {ϵ}_{\mathrm{ry}}& {ϵ}_{\mathrm{rz}}& {▿}_x& {▿}_y\end{array}\right)$${\left(\begin{array}{c}{▿}_z\end{array}\right)}^T$为系统状态量，分别代表惯性导航系统的3维平台误差角(其中航向误差角已经磁 强计修正)、3维速度误差、3维位置误差、陀螺的3轴随机常值和3轴一阶马尔可夫误 差以及加速度计的3轴一阶马尔可夫误差，共18阶。

W=[w_gx w_gy w_gz w_bx w_by w_bz w_ax w_ay w_az]^T为系统的白噪声矢量；

其中I_3×3表示3阶单位矩阵，0_m×n表示m×n阶的0矩阵（下同），F_N表示对应平台误差 角、速度误差、位置误差9个基本导航参数误差的系数矩阵，其非零元素为

$F_N\left(\mathrm{1,2}\right)=w_{\mathrm{ie}}\sin L+\frac{v_E}{R_N+h}\mathrm{tgL}$

$F_N\left(\mathrm{1,3}\right)=-(w_{\mathrm{ie}}\cos L+\frac{v_E}{R_N+h})$

$F_N\left(\mathrm{1,5}\right)=-\frac{1}{R_N+h}$

$F_N\left(\mathrm{2,1}\right)=-(w_{\mathrm{ie}}\sin L+\frac{v_E}{R_N+h}\mathrm{tgL})$

$F_N\left(\mathrm{2,3}\right)=-\frac{v_N}{R_M+h},F_N\left(\mathrm{2,4}\right)=\frac{1}{R_N+h}$

F_N(2,7)=-w_iesinL

$F_N\left(\mathrm{3,1}\right)=w_{\mathrm{ie}}\cos L+\frac{v_E}{R_N+h}$

$F_N\left(\mathrm{3,2}\right)=-\frac{v_N}{R_M+h},F_N\left(\mathrm{3,4}\right)=\frac{1}{R_N+h}\mathrm{tgL}$

$F_N\left(\mathrm{3,7}\right)=w_{\mathrm{ie}}\cos L+\frac{v_E}{R_N+h}{\sec }^{2}L$

F_N(4,2)=-f_U F_N(4,3)=f_U

$F_N\left(\mathrm{4,4}\right)=(\frac{v_N}{R_M+h}\mathrm{tgL}-\frac{v_U}{R_M+h})$

$F_N\left(\mathrm{4,5}\right)={2w}_{\mathrm{ie}}\sin L+\frac{v_E}{R_N+h}\mathrm{tgL}$

$F_N\left(\mathrm{4,6}\right)=-(2w_{\mathrm{ie}}\cos L+\frac{v_E}{R_N+h})$

$F_N\left(\mathrm{4,7}\right)=2w_{\mathrm{ie}}\cos L{v}_N+\frac{v_E{v}_N}{R_N+h}{\sec }^{2}L+2w_{\mathrm{ie}}\sin L{v}_U$

F_N(5,1)=f_U F_N(5,3)=-f_E

$F_N\left(\mathrm{5,4}\right)=-2(w_{\mathrm{ie}}\sin L+\frac{v_E}{R_N+h}\mathrm{tgL})$

$F_N\left(\mathrm{5,5}\right)=-\frac{v_U}{R_M+h},F_N\left(\mathrm{5,6}\right)=-\frac{v_N}{R_N+h}$

$F_N\left(\mathrm{5,7}\right)=-(2w_{\mathrm{ie}}\cos L+\frac{v_E}{R_N+h}{\sec }^{2}L)v_E$

F_N(6,1)=-f_N F_N(6,2)=f_E

$F_N\left(\mathrm{6,4}\right)=2(w_{\mathrm{ie}}\cos L+\frac{v_E}{R_E+h})$

$F_N\left(\mathrm{6,5}\right)=\frac{2v_N}{R_M+h},F_N\left(\mathrm{6,7}\right)=-2v_E{w}_{\mathrm{ie}}\sin L$

$F_N\left(\mathrm{6,9}\right)=-2g/R_e,F_N\left(\mathrm{7,5}\right)=\frac{1}{R_M+h},F_N\left(\mathrm{8,4}\right)=\frac{\sec L}{R_N+h}$

$F_N\left(\mathrm{8,7}\right)=\frac{v_E}{R_N+h}\sec \mathrm{LtgL},F_N\left(\mathrm{9,6}\right)=1$

其中w_ie为地球自转角速率，L为载体所在纬度，R_e为地球赤道平面半径，R_M为地球子 午圈半径，R_N为地球卯酉圈半径，v_N、v_E、v_U为载体北向、东向和天向速度，f_N、f_E、 f_U分别为北向、东向和天向的比力，F_s和F_M分别为：

$F_M=\mathrm{Diag}\left(\begin{array}{ccccccccc}0& 0& 0& -\frac{1}{T_{\mathrm{rx}}}& -\frac{1}{T_{\mathrm{ry}}}& -\frac{1}{T_{\mathrm{rz}}}& -\frac{1}{T_{\mathrm{ax}}}& -\frac{1}{T_{\mathrm{ay}}}& -\frac{1}{T_{\mathrm{az}}}\end{array}\right)$

其中T_rx、T_ry、T_rz分别为陀螺仪一阶马尔科夫过程误差的相关时间，T_ax、T_ay、T_az分 别为加速度计一阶马尔科夫过程误差的相关时间，以惯性导航系统与卫星导航系统所输 出的位置、速度信息之间的差值为量测量建立量测方程，位置差值的观测方程分别为：

Z_p(t)=H_p(t)X(t)+V_p(t)  (22) 式中：Z_p(t)表示位置差值，V_p=[N_EN_NN_h]为量测噪声，可视为白噪声，其方差分别为

速度差值的观测方程为

Z_v(t)=H_v(t)X(t)+V_v(t)  (23)

式中：为量测噪声，可 视为白噪声，其方差分别为

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

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

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

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

忽略地磁场异常等现象且暂时忽略磁偏角，在某地域内的地磁场矢量可视为已知常 量，表示为mag_e，并将磁偏角表示为θ_e；在存在磁场干扰的环境中，将磁传感器所测量 的环境中总磁场矢量表示为mag_h，类似地将带有误差的磁偏角表示为θ_h。

设三轴磁传感器测量的地磁场mag_e在载体坐标系下的投影为而测量总磁场矢 量mag_h在载体坐标系下的投影为其中

${\mathrm{mag}}_e^b=\left(\begin{array}{c}{\mathrm{mag}}_{\mathrm{ex}}\\ {\mathrm{mag}}_{\mathrm{ey}}\\ {\mathrm{mag}}_{\mathrm{ez}}\end{array}\right),{\mathrm{mag}}_h^b=\left(\begin{array}{c}{\mathrm{mag}}_{\mathrm{hx}}\\ {\mathrm{mag}}_{\mathrm{hy}}\\ {\mathrm{mag}}_{\mathrm{hz}}\end{array}\right)$

若已知与磁传感器固联载体的横滚角与俯仰角分别为γ与θ，则可将投影到水平 面与垂直的方向上。

${\mathrm{mag}}_e^n=\left(\begin{array}{c}{\mathrm{mag}}_{\mathrm{ex}}^n\\ {\mathrm{mag}}_{\mathrm{ey}}^n\\ {\mathrm{mag}}_{\mathrm{ez}}^n\end{array}\right)=\left(\begin{array}{ccc}1& 0& 0\\ 0& \cos \theta & -\sin \theta \\ 0& \sin \theta & \cos \theta \end{array}\right)\left(\begin{array}{ccc}\cos \gamma & 0& \sin \gamma \\ 0& 1& 0\\ -\sin \gamma & 0& \cos \gamma \end{array}\right){\mathrm{mag}}_e^b---\left(24\right)$

通过相同方法也可以求得θ_d

${\mathrm{mag}}_h^n=\left(\begin{array}{c}{\mathrm{mag}}_{\mathrm{hx}}^n\\ {\mathrm{mag}}_{\mathrm{hy}}^n\\ {\mathrm{mag}}_{\mathrm{hz}}^n\end{array}\right)=\left(\begin{array}{ccc}1& 0& 0\\ 0& \cos \theta & -\sin \theta \\ 0& \sin \theta & \cos \theta \end{array}\right)\left(\begin{array}{ccc}\cos \gamma & 0& \sin \gamma \\ 0& 1& 0\\ -\sin \gamma & 0& \cos \gamma \end{array}\right){\mathrm{mag}}_h^b---\left(26\right)$

因此即可求得该地理位置上的磁航向角修正值Δθ=θ_d-θ_e。由与物理含义可 知，Δθ与载体的姿态无关，只与载体位置与磁场干扰源的相对位置有关。因此，可通过 该特性实现高精度惯性系统对低精度惯性系统的航向修正。

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

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

（2）行人与智能移动载体同步进入电磁干扰环境中，利用机器人中高精度捷联惯导系统 输出的航向角为航向基准θ_e，并以其磁传感器输出与水平姿态数据解算θ_h，从而求解Δθ， 并将该值实时通过数据通讯传递给行人导航系统，实现行人导航系统的航向修正。

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

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

（1）行人导航系统与传感器误差建模与修正

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

速度误差模型如下：

定位误差模型为：

$\left(\begin{array}{c}\delta \stackrel{·}{L}=\frac{{\mathrm{\delta v}}_N}{R_M+h}\\ \delta \stackrel{·}{\lambda }=\frac{{\mathrm{\delta v}}_E}{R_N+h}\sec L+\frac{v_E}{R_N+h}\sec \mathrm{LtgL\delta L}\\ \delta \stackrel{·}{h}=\delta v_U\end{array}\right)---\left(30\right)$

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

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

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

（2）智能移动载体惯性导航系统的误差建模与修正

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

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

$\stackrel{·}{P}=0---\left(32\right)$

即状态方程扩展为18阶。观测方程的形式如下：

Z=HX+V  (33)

其中观测系数矩阵H扩展为3×21阶矩阵，其中H(16,1)=H(17,2)=H(18,3)=-1。