一种个人携行式多源定位信息辅助修正方法
一种个人携行式多源定位信息辅助修正方法
【技术领域】
[0001] 本发明属于导航定位技术领域，特别是涉及一种行人运动的静止步态特征与外部 辅助定位信息共同修正定位误差的方法。
【背景技术】
[0002] 随着移动通信技术和互联网技术的快速发展，基于定位与导航的相关应用越来越 受到人们以及相应企业的关注，尤其在复杂的室内环境，如机场大厅、展厅、仓库、图书馆、 地下停车场、矿井等环境中，常常需要确定移动终端或其持有者在室内的位置信息。目前 主要用于个人定位技术主要分为三大分支：依靠预置节点定位、全自主定位和预置节点与 IMU相结合定位。现有依靠预置节点定位技术，为保证定位精度，均需提前预置高密度节点 网络。由于受到诸多现场环境的制约，预置节点定位技术很难大规模应用。而全自主定位的 特点是通过探测人体躯干运动形态，结合陀螺仪、加速度计、磁阻传感器来预测人体位置。 由于惯性器件的长时漂移，导致误差的不断增加。预置节点与IMU相结合的定位方式将两 种定位进行优势互补，有效的抑制了长时漂移所带来的位置误差，本发明基于此类定位方 式，提出了一种个人携行式多源定位信息辅助修正方法，有效的融合了自身惯性定位单元 与外部辅助电磁信息的定位，而且复杂程度低，易于工程实现。
[0003] 与同领域的相关申请专利进行对比，本发明的创造性及优点较为明显。比如，申请 号为：201310566710. 6,专利名称是《基于捷联惯性解算和零位校正的自主导航系统定位方 法》的专利，其采用的是利用安装位于鞋部的惯性测量单元，运动并采集传感器信息，融合 信息进行零速检测，并以此作为触发条件触发滤波器进行零速修正。该方法仅融合个人运 动约束进行惯性导航定位，无法长时间抑制惯性漂移，续航能力较差。再比如，申请号为： 201410416423. 1，专利名称是《室内定位装置及系统》的专利，需要大量定位标签，虽然定位 精度较高，但实施难度较大、成本高，不利于推广。
【发明内容】
[0004] 本发明的主要目的在于：面向行人的个人定位，利用行人运动的静止步态特征与 外部辅助电磁定位信息共同抑制惯性漂移，修正定位误差。
[0005] 为了实现上述目的，本发明采用如下技术方案：
[0006] 1、一种个人携行式多源定位信息辅助修正方法，包括以下步骤：
[0007] 步骤1，获取行人运动时的加速度、角速度和地磁信息并进行捷联惯性三维空间定 位信息的计算；
[0008] 步骤2,获取电磁定位信号并求解空间三维定位信息；
[0009] 步骤3,依据行人运动检测约束条件进行静止检测；
[0010] 步骤4,利用零速信息与磁航向信息构建局部卡尔曼滤波器对行人定位信息进行 滤波估计，并修正行人的定位信息；
[0011] 步骤5,构建主滤波器利用电磁定位信息对局部修正后的行人定位信息进行滤波 估计，利用最终主滤波器的估计量修正行人定位信息。
[0012] 进一步的，通过行人身上的测量传感器件的测量信息解算出步骤1中所述的三维 空间信息；测量传感器件包括加速度计、陀螺仪和地磁传感器。
[0013] 进一步的，所述步骤2中，利用极大似然估计法进行三维定位信息的计算；电磁定 位信号测量装置包含外部辅助定位基站与移动电磁定位终端。
[0014] 进一步的，所述步骤3中，利用如下公式检测静止，检测模块包括三轴加速度计、 三轴陀螺仪。
[0016] 式中，A为标定常数，W为滑窗值，g为重力加速度，|为检验阈值，>^与>^分别 为第k时刻的加速度与角速度值，
分别表示加速度计的方差与陀螺 仪的量测噪声。I|a| |2=aTa，（ ?）T表示转置运算。表示样本均值，n表示数据总量。
[0017] 进一步的，所述步骤4中，零速信息为运动过程中的零速误差；磁航向信息为由地 磁传感器信息估算出的航向。将静止时刻的零速误差连同航向误差一起构造量测，通过局 部卡尔曼滤波器，实现行走过程中累积误差的估计与修正。局部卡尔曼滤波器选取的状态 量X(k)为：
[0018]X(k) = [ 8itk 8vk 8pk]T
[0019] 其中S!Dk为姿态角误差，SVk为速度误差，SPk为位置误差。
[0020] 量测量Z(k)包含静止时刻的导航解算速度值与航向误差：
[0022] 其中，
，航向误差由磁航向角与捷联解算出的航向角共同求得。 Avx，Avy，Avz为速度误差沿导航坐标轴x，y，z的分量。量测阵H为：
[0024] 只有在运动过程中的静止时刻卡尔曼滤波器才做完整的更新，否则只做时间更 新。
[0025] 进一步的，所述步骤5中，构建主滤波器利用电磁定位信息对局部修正后的行人 的位置、速度等定位信息进行卡尔曼滤波估计并修正。主滤波器选取的状态量X(k)为：
[0026] X(k) = [8i]) 8v8p]T
[0027] 其中S为姿态角误差，SV为速度误差，Sp为位置误差。
[0028] 量测量Z(k)包含局部修正后的定位信息与电磁定位信息的误差：
[0029] Z(k) =[AvAp]T= [AvxAvyAvzApxApyApz]T
[0030] 其中，速度误差由电磁速度信息与局部修正后速度信息共同求得，Av= VM| -v局 SP;位置误差由电磁位置信息与局部修正后位置信息共同求得Ap=p 部。Avx，Avy， 八^与APx，APy，APz*别为速度与位置误差沿导航坐标轴x，y，z的分量。
[0031] 量测阵H为：
[0033] 利用最终主滤波器的估计量修正行人定位信息，得到最终定位信息。
[0034] 与现有技术相比，本发明的有益效果是：
[0035] (1)本发明利用双滤波器有效的结合了惯性导航定位与预置节点定位的优势，实 现了长时的精确定位。
[0036] (2)对于局部滤波器部分，利用人体的运动检测约束条件，对惯导航的误差进行了 补偿；将补偿后的定位信息与外部辅助定位信息进行滤波估计并再次补偿，有效的保证了 定位的精准性。
[0037] (3)本发明具有可穿戴的特点，使用操作更加方便。
【附图说明】
[0038] 图1整体流程图；
[0039] 图2多源定位信息融合流程图；
[0040] 图3极大似然估计法图示；
[0041] 图4携行装置安置示意图。
【具体实施方式】
[0042] 本发明提供的一种个人携行式多源定位信息辅助修正算法具体步骤如下：
[0043] 如图2,首先利用惯性/地磁测量单元（2-1)所测量到的人体运动时的加速度、角 速度和地磁信息（2-2)进行惯性导航更新（2-3)，更新算法采用传统的捷联惯性导航解算 算法。选取北东地为导航坐标系，采用一阶龙格-库塔法更新的四元数法进行姿态信息求 解，然后根据姿态矩阵信息和比力信息对重力进行补偿，获得导航系下三轴加速度瞬时值 ，同时利用加速度瞬时值计算速度和位置信息。由于选用的小型MEMS陀螺仪的精度较 低，不能有效敏感地球的自转角速度，因此初始对准可简化为：通过水平加速度计的输出估 算俯仰角0和滚转角y，然后由地磁传感器的输出估算出航向角步。
[0044] 同时加速度、角速度和地磁信息（2-2)进行静止检测与航位角推算（2-5)。行人在 运动过程中存在绝对静止时刻，也就是"零瞬态"，行人的运动状态处于静止或是运动，其加 速度与角速度的量测序列是不同的。如果行人处于静止状态，则存在：
[0046] 其中)是检验统计量，Zf与fZf'分别为加速度与角速度的两侧序列，【为 检验阈值。其中I由下式确定：
[0049] 式中P(Zn;ri)与P (zn;rJ表示静止与运动这两种假设观测值的概率密度函数，L(zn)为每个量测序列\的似然比，a为静止假定状态的最小概率。
[0051] 其中，A为标定常数，w为滑窗值，g为重力加速度，|为检验阈值，与3^分别 为第k时刻的加速度与角速度值，
：分别表示加速度计的方差与陀螺仪 的量测噪声。I|a| |2=aTa，（ ?）T表示转置运算。表示样本均 值，n表示数据总量。
[0053] 磁航向推算过程与初始对准过程类似，根据获得的x、y、z轴向上的加速度计信 息，俯仰角9和滚转角Y:
[0056] 其中：
为第k时刻经过补偿后的加速度计输出值。
[0057] 根据计算出俯仰角0和滚转角Y，计算导航坐标系下的磁场强度为：
[0059] 与分别为导航坐标系下磁场强度与载体坐标下磁场强度，则可得到磁航向 角供m为：
[0061] 将静止时刻的零速连同航位一起构造量测，通过局部卡尔曼滤波器（2-7)，实现行 走过程中累积误差的估计与修正。局部卡尔曼滤波器（2-7)选取的状态量X(k)为：
[0062] X(k) = [ 8i])k 8vk 8pk]T (10)
[0063] 其中S!Dk为姿态角误差，SVk为速度误差，SPk为位置误差。
[0064] 量测量Z(k)包含静止时刻的导航解算速度值与航向误差：
[0066]其中
，航向误差由磁航向角与捷联解算出的航向角共同求得。 Avx，Avy，Avz为速度误差沿导航坐标轴x，y，z的分量。量测阵H为：
[0068] 只有在运动过程中的静止时刻卡尔曼滤波器才做完整的更新，否则只做时间更 新。
[0069] 与之同步利用电磁信号接收单元（2-9)接收到的电磁信号（2-10)进行电磁定 位信息的计算（2-11)。采用极大似然估计法进行三维定位信息的计算，如图3,在参考坐 标系（3-1)中设待测节点（3-2)坐标为X(x，y，z)，n个参考节点的坐标分别为4(&1，bp Cl)、A2 (a2，b2，c2) ? ? ?An (an，bn，cn)，X到Ai(3-3)、A2 (3-4) ? ? ?An (3-8)的距离分别为屯(3-9)、 d2 (3-10). ..dn (3-14)，则根据三维空间两点间距离公式可得到方程组：
[0071] 上式中，通过移项化简可得如下非齐次线性方程组：
[0073] 上式可记为：AX=b
[0074]其中
[0077] 可解得X坐标为
。从而完成电磁定位信息的计算（2-11)。
[0078] 构建主滤波器（2-13)利用电磁定位信息（2-12)对局部修正后的行人的位置、速 度等定位信息（2-8)进行卡尔曼滤波估计并修正。主滤波器（2-13)选取的状态量X(k)为：
[0079] X(k) = [8i]) 8v8p]T (17)
[0080] 其中S 为姿态角误差，SV为速度误差，Sp为位置误差。
[0081] 量测量Z(k)包含局部修正后的定位信息（2-8)与电磁定位信息（2-12)的误差：
[0083] 其中，姿态角误差由电磁姿态角（2-12)与局部修正后姿态角（2-8)共同求得， A步=步Mi-步msp;速度误差由电磁速度信息（2-12)与局部修正后速度信息（2-8)共同 求得，Av=vg-vg;位置误差由电磁位置信息（2-12)与局部修正后位置信息（2-8)共 同求得AP=P_-Pmsp。A'，，Avy，A '与APx，APy，APz分别为速度与位置误差沿导 航坐标轴x，y，z的分量。
[0084] 量测阵H为：
[0086] 利用最终主滤波器（2-13)的估计量修正行人定位信息（2-14)，实现最终定位 (2-15)〇
[0087] 将携行装置（4-2)安置在行人脚踝（4-1)处，其惯性参考坐标系（4-3)定义如下： x轴（4-4)指向行人脚踝（4-1)的左侧，y轴（4-5)指向行人脚踝（4-1)前方，z轴（4-6) 指向行人脚踝（4-1)下方。其安置轴向及具体位置如图4所示。
【主权项】
1. 一种个人携行式多源定位信息辅助修正方法，其特征在于：包括以下步骤： 步骤1，获取行人运动时的加速度、角速度和地磁信息并进行捷联惯性三维空间定位信 息的计算； 步骤2,获取电磁定位信号并求解空间三维定位信息； 步骤3,依据行人运动检测约束条件进行静止检测； 步骤4,利用零速信息与磁航向信息构建局部卡尔曼滤波器对行人定位信息进行滤波 估计，并修正行人的定位信息； 步骤5,构建主滤波器利用电磁定位信息对局部修正后的行人定位信息进行滤波估计， 利用最终主滤波器的估计量修正行人定位信息。2. 根据权利要求1所述的个人携行式多源定位信息辅助修正方法，其特征在于：通过 行人身上的测量传感器件的测量信息解算出步骤1中所述的三维空间信息；测量传感器件 包括加速度计、陀螺仪和地磁传感器。3. 根据权利要求1所述的个人携行式多源定位信息辅助修正方法，其特征在于：所述 步骤2中，利用极大似然估计法进行三维定位信息的计算；电磁定位信号测量装置包含外 部辅助定位基站与移动电磁定位终端。4. 根据权利要求1所述的个人携行式多源定位信息辅助修正方法，其特征在于：所述 步骤3中，利用如下公式检测静止，检测模块包括三轴加速度计和三轴陀螺仪。式中，λ为标定常数，W为滑窗值，g为重力加速度，ξ为检验阈值，>^与>^分别为第 k时刻的加速度与角速度值，σα2 e R1与e R1分别表示加速度计的方差与陀螺仪的量 测噪声。I |a| I2= &\，（*)1表示转置运算。表示样本均值，η表示数据总量。5. 根据权利要求1所述的个人携行式多源定位信息辅助修正方法，其特征在于：所述 步骤4中，将静止时刻的零速误差连同航向误差一起构造量测，通过局部卡尔曼滤波器，实 现行走过程中累积误差的估计与修正。局部卡尔曼滤波器选取的状态量X(k)为： X (k) = [ δ φ k δ Vk δ pk]τ 其中δ φ,为姿态角误差，δ Vk为速度误差，δ Pk为位置误差。 量测量Z(k)包含静止时刻的导航解算速度值与航向误差：其中，Δ?^=供WS -炉m，航向误差由磁航向角K与捷联解算出的航向角巧ΛΒ共同求得， Δνχ，Avy，Δ Vz为速度误差沿导航坐标轴X，y，Z的分量。量测阵H为：只有在运动过程中的静止时刻卡尔曼滤波器才做完整的更新，否则只做时间更新。6.根据权利要求1所述的个人携行式多源定位信息辅助修正方法，其特征在于：所述 步骤5中，构建主滤波器利用电磁定位信息对局部修正后的行人的位置、速度等定位信息 进行卡尔曼滤波估计并修正。主滤波器选取的状态量X(k)为： X (k) = [δ φ δ V δ ρ]τ 其中δ φ为姿态角误差，δ V为速度误差，δ ρ为位置误差。 量测量Z(k)包含局部修正后的定位信息与电磁定位信息的误差： Z(k) = [Δν Δρ]τ= [Δνχ Avy Δ νζ Δρχ Apy Δρζ]τ 其中，速度误差由电磁速度信息与局部修正后速度信息共同求得，Δ ν = Vew;位 置误差由电磁位置信息与局部修正后位置信息共同求得Δρ = ρ电磁-ρ局部。Δνχ，Avy，Δνζ与ΛΡχ，APy，ΛΡζ*别为速度与位置误差沿导航坐标轴 x，y，z的分量。 量测阵H为：利用最终主滤波器的估计量修正行人定位信息，得到最终定位信息。
【专利摘要】一种个人携行式多源定位信息辅助修正方法，利用行人运动的静止步态特征与外部辅助电磁定位信息共同抑制惯性漂移，修正定位误差。该方法包含：(1)获取行人运动时的加速度、角速度和地磁信息并进行捷联惯性三维空间定位信息的计算；(2)获取电磁定位信号并求解空间三维定位信息；(3)依据行人运动检测约束条件进行静止检测；(4)利用零速信息与磁航向信息构建局部卡尔曼滤波器对行人定位信息进行滤波估计，并修正行人的定位信息。(5)构建主滤波器利用电磁定位信息对局部修正后的行人定位信息进行滤波估计，利用最终主滤波器的估计量修正行人定位信息，得到最终定位信息。本发明依据行人运动静止步态特征，利用双滤波器对行人的多源定位信息进行深度融合，提高了导航定位精度。
【IPC分类】G01C21/20, G01C21/16
【公开号】CN104897155
【申请号】CN201510300517
【发明人】费程羽, 苏中, 李擎, 范军芳, 付国栋, 刘宁, 李超
【申请人】北京信息科技大学, 北京德维创盈科技有限公司
【公开日】2015年9月9日
【申请日】2015年6月5日
【技术领域】
[0001] 本发明属于导航定位技术领域，特别是涉及一种行人运动的静止步态特征与外部 辅助定位信息共同修正定位误差的方法。
【背景技术】
[0002] 随着移动通信技术和互联网技术的快速发展，基于定位与导航的相关应用越来越 受到人们以及相应企业的关注，尤其在复杂的室内环境，如机场大厅、展厅、仓库、图书馆、 地下停车场、矿井等环境中，常常需要确定移动终端或其持有者在室内的位置信息。目前 主要用于个人定位技术主要分为三大分支：依靠预置节点定位、全自主定位和预置节点与 IMU相结合定位。现有依靠预置节点定位技术，为保证定位精度，均需提前预置高密度节点 网络。由于受到诸多现场环境的制约，预置节点定位技术很难大规模应用。而全自主定位的 特点是通过探测人体躯干运动形态，结合陀螺仪、加速度计、磁阻传感器来预测人体位置。 由于惯性器件的长时漂移，导致误差的不断增加。预置节点与IMU相结合的定位方式将两 种定位进行优势互补，有效的抑制了长时漂移所带来的位置误差，本发明基于此类定位方 式，提出了一种个人携行式多源定位信息辅助修正方法，有效的融合了自身惯性定位单元 与外部辅助电磁信息的定位，而且复杂程度低，易于工程实现。
[0003] 与同领域的相关申请专利进行对比，本发明的创造性及优点较为明显。比如，申请 号为：201310566710. 6,专利名称是《基于捷联惯性解算和零位校正的自主导航系统定位方 法》的专利，其采用的是利用安装位于鞋部的惯性测量单元，运动并采集传感器信息，融合 信息进行零速检测，并以此作为触发条件触发滤波器进行零速修正。该方法仅融合个人运 动约束进行惯性导航定位，无法长时间抑制惯性漂移，续航能力较差。再比如，申请号为： 201410416423. 1，专利名称是《室内定位装置及系统》的专利，需要大量定位标签，虽然定位 精度较高，但实施难度较大、成本高，不利于推广。
【发明内容】
[0004] 本发明的主要目的在于：面向行人的个人定位，利用行人运动的静止步态特征与 外部辅助电磁定位信息共同抑制惯性漂移，修正定位误差。
[0005] 为了实现上述目的，本发明采用如下技术方案：
[0006] 1、一种个人携行式多源定位信息辅助修正方法，包括以下步骤：
[0007] 步骤1，获取行人运动时的加速度、角速度和地磁信息并进行捷联惯性三维空间定 位信息的计算；
[0008] 步骤2,获取电磁定位信号并求解空间三维定位信息；
[0009] 步骤3,依据行人运动检测约束条件进行静止检测；
[0010] 步骤4,利用零速信息与磁航向信息构建局部卡尔曼滤波器对行人定位信息进行 滤波估计，并修正行人的定位信息；
[0011] 步骤5,构建主滤波器利用电磁定位信息对局部修正后的行人定位信息进行滤波 估计，利用最终主滤波器的估计量修正行人定位信息。
[0012] 进一步的，通过行人身上的测量传感器件的测量信息解算出步骤1中所述的三维 空间信息；测量传感器件包括加速度计、陀螺仪和地磁传感器。
[0013] 进一步的，所述步骤2中，利用极大似然估计法进行三维定位信息的计算；电磁定 位信号测量装置包含外部辅助定位基站与移动电磁定位终端。
[0014] 进一步的，所述步骤3中，利用如下公式检测静止，检测模块包括三轴加速度计、 三轴陀螺仪。
[0016] 式中，A为标定常数，W为滑窗值，g为重力加速度，|为检验阈值，>^与>^分别 为第k时刻的加速度与角速度值，
分别表示加速度计的方差与陀螺 仪的量测噪声。I|a| |2=aTa，（ ?）T表示转置运算。表示样本均值，n表示数据总量。
[0017] 进一步的，所述步骤4中，零速信息为运动过程中的零速误差；磁航向信息为由地 磁传感器信息估算出的航向。将静止时刻的零速误差连同航向误差一起构造量测，通过局 部卡尔曼滤波器，实现行走过程中累积误差的估计与修正。局部卡尔曼滤波器选取的状态 量X(k)为：
[0018]X(k) = [ 8itk 8vk 8pk]T
[0019] 其中S!Dk为姿态角误差，SVk为速度误差，SPk为位置误差。
[0020] 量测量Z(k)包含静止时刻的导航解算速度值与航向误差：
[0022] 其中，
，航向误差由磁航向角与捷联解算出的航向角共同求得。 Avx，Avy，Avz为速度误差沿导航坐标轴x，y，z的分量。量测阵H为：
[0024] 只有在运动过程中的静止时刻卡尔曼滤波器才做完整的更新，否则只做时间更 新。
[0025] 进一步的，所述步骤5中，构建主滤波器利用电磁定位信息对局部修正后的行人 的位置、速度等定位信息进行卡尔曼滤波估计并修正。主滤波器选取的状态量X(k)为：
[0026] X(k) = [8i]) 8v8p]T
[0027] 其中S为姿态角误差，SV为速度误差，Sp为位置误差。
[0028] 量测量Z(k)包含局部修正后的定位信息与电磁定位信息的误差：
[0029] Z(k) =[AvAp]T= [AvxAvyAvzApxApyApz]T
[0030] 其中，速度误差由电磁速度信息与局部修正后速度信息共同求得，Av= VM| -v局 SP;位置误差由电磁位置信息与局部修正后位置信息共同求得Ap=p 部。Avx，Avy， 八^与APx，APy，APz*别为速度与位置误差沿导航坐标轴x，y，z的分量。
[0031] 量测阵H为：
[0033] 利用最终主滤波器的估计量修正行人定位信息，得到最终定位信息。
[0034] 与现有技术相比，本发明的有益效果是：
[0035] (1)本发明利用双滤波器有效的结合了惯性导航定位与预置节点定位的优势，实 现了长时的精确定位。
[0036] (2)对于局部滤波器部分，利用人体的运动检测约束条件，对惯导航的误差进行了 补偿；将补偿后的定位信息与外部辅助定位信息进行滤波估计并再次补偿，有效的保证了 定位的精准性。
[0037] (3)本发明具有可穿戴的特点，使用操作更加方便。
【附图说明】
[0038] 图1整体流程图；
[0039] 图2多源定位信息融合流程图；
[0040] 图3极大似然估计法图示；
[0041] 图4携行装置安置示意图。
【具体实施方式】
[0042] 本发明提供的一种个人携行式多源定位信息辅助修正算法具体步骤如下：
[0043] 如图2,首先利用惯性/地磁测量单元（2-1)所测量到的人体运动时的加速度、角 速度和地磁信息（2-2)进行惯性导航更新（2-3)，更新算法采用传统的捷联惯性导航解算 算法。选取北东地为导航坐标系，采用一阶龙格-库塔法更新的四元数法进行姿态信息求 解，然后根据姿态矩阵信息和比力信息对重力进行补偿，获得导航系下三轴加速度瞬时值 ，同时利用加速度瞬时值计算速度和位置信息。由于选用的小型MEMS陀螺仪的精度较 低，不能有效敏感地球的自转角速度，因此初始对准可简化为：通过水平加速度计的输出估 算俯仰角0和滚转角y，然后由地磁传感器的输出估算出航向角步。
[0044] 同时加速度、角速度和地磁信息（2-2)进行静止检测与航位角推算（2-5)。行人在 运动过程中存在绝对静止时刻，也就是"零瞬态"，行人的运动状态处于静止或是运动，其加 速度与角速度的量测序列是不同的。如果行人处于静止状态，则存在：
[0046] 其中)是检验统计量，Zf与fZf'分别为加速度与角速度的两侧序列，【为 检验阈值。其中I由下式确定：
[0049] 式中P(Zn;ri)与P (zn;rJ表示静止与运动这两种假设观测值的概率密度函数，L(zn)为每个量测序列\的似然比，a为静止假定状态的最小概率。
[0051] 其中，A为标定常数，w为滑窗值，g为重力加速度，|为检验阈值，与3^分别 为第k时刻的加速度与角速度值，
：分别表示加速度计的方差与陀螺仪 的量测噪声。I|a| |2=aTa，（ ?）T表示转置运算。表示样本均 值，n表示数据总量。
[0053] 磁航向推算过程与初始对准过程类似，根据获得的x、y、z轴向上的加速度计信 息，俯仰角9和滚转角Y:
[0056] 其中：
为第k时刻经过补偿后的加速度计输出值。
[0057] 根据计算出俯仰角0和滚转角Y，计算导航坐标系下的磁场强度为：
[0059] 与分别为导航坐标系下磁场强度与载体坐标下磁场强度，则可得到磁航向 角供m为：
[0061] 将静止时刻的零速连同航位一起构造量测，通过局部卡尔曼滤波器（2-7)，实现行 走过程中累积误差的估计与修正。局部卡尔曼滤波器（2-7)选取的状态量X(k)为：
[0062] X(k) = [ 8i])k 8vk 8pk]T (10)
[0063] 其中S!Dk为姿态角误差，SVk为速度误差，SPk为位置误差。
[0064] 量测量Z(k)包含静止时刻的导航解算速度值与航向误差：
[0066]其中
，航向误差由磁航向角与捷联解算出的航向角共同求得。 Avx，Avy，Avz为速度误差沿导航坐标轴x，y，z的分量。量测阵H为：
[0068] 只有在运动过程中的静止时刻卡尔曼滤波器才做完整的更新，否则只做时间更 新。
[0069] 与之同步利用电磁信号接收单元（2-9)接收到的电磁信号（2-10)进行电磁定 位信息的计算（2-11)。采用极大似然估计法进行三维定位信息的计算，如图3,在参考坐 标系（3-1)中设待测节点（3-2)坐标为X(x，y，z)，n个参考节点的坐标分别为4(&1，bp Cl)、A2 (a2，b2，c2) ? ? ?An (an，bn，cn)，X到Ai(3-3)、A2 (3-4) ? ? ?An (3-8)的距离分别为屯(3-9)、 d2 (3-10). ..dn (3-14)，则根据三维空间两点间距离公式可得到方程组：
[0071] 上式中，通过移项化简可得如下非齐次线性方程组：
[0073] 上式可记为：AX=b
[0074]其中
[0077] 可解得X坐标为
。从而完成电磁定位信息的计算（2-11)。
[0078] 构建主滤波器（2-13)利用电磁定位信息（2-12)对局部修正后的行人的位置、速 度等定位信息（2-8)进行卡尔曼滤波估计并修正。主滤波器（2-13)选取的状态量X(k)为：
[0079] X(k) = [8i]) 8v8p]T (17)
[0080] 其中S 为姿态角误差，SV为速度误差，Sp为位置误差。
[0081] 量测量Z(k)包含局部修正后的定位信息（2-8)与电磁定位信息（2-12)的误差：
[0083] 其中，姿态角误差由电磁姿态角（2-12)与局部修正后姿态角（2-8)共同求得， A步=步Mi-步msp;速度误差由电磁速度信息（2-12)与局部修正后速度信息（2-8)共同 求得，Av=vg-vg;位置误差由电磁位置信息（2-12)与局部修正后位置信息（2-8)共 同求得AP=P_-Pmsp。A'，，Avy，A '与APx，APy，APz分别为速度与位置误差沿导 航坐标轴x，y，z的分量。
[0084] 量测阵H为：
[0086] 利用最终主滤波器（2-13)的估计量修正行人定位信息（2-14)，实现最终定位 (2-15)〇
[0087] 将携行装置（4-2)安置在行人脚踝（4-1)处，其惯性参考坐标系（4-3)定义如下： x轴（4-4)指向行人脚踝（4-1)的左侧，y轴（4-5)指向行人脚踝（4-1)前方，z轴（4-6) 指向行人脚踝（4-1)下方。其安置轴向及具体位置如图4所示。
【主权项】
1. 一种个人携行式多源定位信息辅助修正方法，其特征在于：包括以下步骤： 步骤1，获取行人运动时的加速度、角速度和地磁信息并进行捷联惯性三维空间定位信 息的计算； 步骤2,获取电磁定位信号并求解空间三维定位信息； 步骤3,依据行人运动检测约束条件进行静止检测； 步骤4,利用零速信息与磁航向信息构建局部卡尔曼滤波器对行人定位信息进行滤波 估计，并修正行人的定位信息； 步骤5,构建主滤波器利用电磁定位信息对局部修正后的行人定位信息进行滤波估计， 利用最终主滤波器的估计量修正行人定位信息。2. 根据权利要求1所述的个人携行式多源定位信息辅助修正方法，其特征在于：通过 行人身上的测量传感器件的测量信息解算出步骤1中所述的三维空间信息；测量传感器件 包括加速度计、陀螺仪和地磁传感器。3. 根据权利要求1所述的个人携行式多源定位信息辅助修正方法，其特征在于：所述 步骤2中，利用极大似然估计法进行三维定位信息的计算；电磁定位信号测量装置包含外 部辅助定位基站与移动电磁定位终端。4. 根据权利要求1所述的个人携行式多源定位信息辅助修正方法，其特征在于：所述 步骤3中，利用如下公式检测静止，检测模块包括三轴加速度计和三轴陀螺仪。式中，λ为标定常数，W为滑窗值，g为重力加速度，ξ为检验阈值，>^与>^分别为第 k时刻的加速度与角速度值，σα2 e R1与e R1分别表示加速度计的方差与陀螺仪的量 测噪声。I |a| I2= &\，（*)1表示转置运算。表示样本均值，η表示数据总量。5. 根据权利要求1所述的个人携行式多源定位信息辅助修正方法，其特征在于：所述 步骤4中，将静止时刻的零速误差连同航向误差一起构造量测，通过局部卡尔曼滤波器，实 现行走过程中累积误差的估计与修正。局部卡尔曼滤波器选取的状态量X(k)为： X (k) = [ δ φ k δ Vk δ pk]τ 其中δ φ,为姿态角误差，δ Vk为速度误差，δ Pk为位置误差。 量测量Z(k)包含静止时刻的导航解算速度值与航向误差：其中，Δ?^=供WS -炉m，航向误差由磁航向角K与捷联解算出的航向角巧ΛΒ共同求得， Δνχ，Avy，Δ Vz为速度误差沿导航坐标轴X，y，Z的分量。量测阵H为：只有在运动过程中的静止时刻卡尔曼滤波器才做完整的更新，否则只做时间更新。6.根据权利要求1所述的个人携行式多源定位信息辅助修正方法，其特征在于：所述 步骤5中，构建主滤波器利用电磁定位信息对局部修正后的行人的位置、速度等定位信息 进行卡尔曼滤波估计并修正。主滤波器选取的状态量X(k)为： X (k) = [δ φ δ V δ ρ]τ 其中δ φ为姿态角误差，δ V为速度误差，δ ρ为位置误差。 量测量Z(k)包含局部修正后的定位信息与电磁定位信息的误差： Z(k) = [Δν Δρ]τ= [Δνχ Avy Δ νζ Δρχ Apy Δρζ]τ 其中，速度误差由电磁速度信息与局部修正后速度信息共同求得，Δ ν = Vew;位 置误差由电磁位置信息与局部修正后位置信息共同求得Δρ = ρ电磁-ρ局部。Δνχ，Avy，Δνζ与ΛΡχ，APy，ΛΡζ*别为速度与位置误差沿导航坐标轴 x，y，z的分量。 量测阵H为：利用最终主滤波器的估计量修正行人定位信息，得到最终定位信息。
【专利摘要】一种个人携行式多源定位信息辅助修正方法，利用行人运动的静止步态特征与外部辅助电磁定位信息共同抑制惯性漂移，修正定位误差。该方法包含：(1)获取行人运动时的加速度、角速度和地磁信息并进行捷联惯性三维空间定位信息的计算；(2)获取电磁定位信号并求解空间三维定位信息；(3)依据行人运动检测约束条件进行静止检测；(4)利用零速信息与磁航向信息构建局部卡尔曼滤波器对行人定位信息进行滤波估计，并修正行人的定位信息。(5)构建主滤波器利用电磁定位信息对局部修正后的行人定位信息进行滤波估计，利用最终主滤波器的估计量修正行人定位信息，得到最终定位信息。本发明依据行人运动静止步态特征，利用双滤波器对行人的多源定位信息进行深度融合，提高了导航定位精度。
【IPC分类】G01C21/20, G01C21/16
【公开号】CN104897155
【申请号】CN201510300517
【发明人】费程羽, 苏中, 李擎, 范军芳, 付国栋, 刘宁, 李超
【申请人】北京信息科技大学, 北京德维创盈科技有限公司
【公开日】2015年9月9日
【申请日】2015年6月5日
最新回复(0)