基于足部航姿参考和肩部电子罗盘的个人导航系统及方法
基于足部航姿参考和肩部电子罗盘的个人导航系统及方法
【技术领域】
[0001] 本发明涉及复杂环境下组合定位技术领域，尤其涉及一种基于足部航姿参考和肩 部电子罗盘的个人导航系统及方法。
【背景技术】
[0002] 近年来，行人导航（PedestrianNavigation,PN)作为导航技术应用的新兴领域， 正越来越受到各国学者的重视，并逐渐成为该领域的研宄热点。然而在隧道、大型仓库、地 下停车场等室内环境下，外界无线电信号微弱、电磁干扰强烈等因素都会对目标行人导航 信息获取的准确性、实时性及鲁棒性有很大影响。如何将室内环境下获取的有限信息进行 有效的融合以消除室内复杂环境影响，保证行人导航精度的持续稳定，具有重要的科学理 论意义和实际应用价值。
[0003] 在现有的定位方式中，全球卫星导航系统（GlobalNavigationSatellite System，GNSS)是最为常用的一种方式。虽然GNSS能够通过精度持续稳定的位置信息，但是 其易受电磁干扰、遮挡等外界环境影响的缺点限制了其应用范围，特别是在室内、地下巷道 等一些密闭的、环境复杂的场景，GNSS信号被严重遮挡，无法进行有效的工作。近年来，WSN 以其低成本、低功耗和低系统复杂度的特点在短距离局部定位领域表现出很大的潜力。
[0004] 学者们提出将基于WSN的目标跟踪应用于GNSS失效环境下的行人导航。这种方 式虽然能够实现室内定位，但是由于室内环境复杂多变，WSN信号十分容易受到干扰而导致 定位精度下降甚至失锁；与此同时，目前的导航定位精度为米级，不能保证对室内行人高精 度的导航需求；除此之外，由于WSN采用的通信技术通常为短距离无线通信技术，因此若想 完成大范围的室内目标跟踪定位，需要大量的网络节点共同完成，这必将引入网络组织结 构优化设计、多节点多簇网络协同通信等一系列问题。因此现阶段基于WSN的目标跟踪在 室内导航领域仍旧面临很多挑战。
[0005] 为了克服上述两种导航方法需要参考节点并容易产生失锁的缺点，学者们提出将 航姿参考系统（AHRS)应用于小区域目标跟踪领域。AHRS具有全自主、运动信息全面、短时、 高精度的优点，虽然可以实现自主导航，但误差随时间积累，长航时运行条件下将导致导航 精度严重下降。
【发明内容】
[0006] 本发明的目的就是解决上述问题，提供一种基于足部航姿参考和肩部电子罗盘的 个人导航系统及方法，该方法将肩部电子罗盘测量得到行人航向信息引入到足部AHRS的 导航信息解算过程中，提高足部AHRS的姿态转移矩阵的计算精度。
[0007] 为实现上述目的，本发明采用下述技术方案，包括：
[0008] -种基于足部航姿参考和肩部电子罗盘的个人导航系统，包括：参考系统和行人 导航系统；
[0009] 所述参考系统包括：码盘和固定于码盘的AHRS;
[0010] 所述行人导航系统包括：足部AHRS、肩部电子罗盘和控制器；足部AHRS与肩部电 子罗盘连接，参考系统、足部AHRS和肩部电子罗盘分别于控制器连接；
[0011]肩部电子罗盘将其测量得到的行人航向信息直接输入到足部AHRS，作为其解算姿 态转移矩阵时所需的航向信息；足部AHRS通过自身携带的加速度计和陀螺仪以及肩部电 子罗盘提供的行人航向信息对行人的轨迹进行解算。
[0012] 一种基于足部航姿参考和肩部电子罗盘的个人导航方法，包括以下步骤：
[0013] (1)将两个低成本AHRS分别安装于行人的鞋上和肩上，其中安装于鞋上的AHRS主 要负责导航解算，肩上的AHRS作为电子罗盘主要负责行人航向测量；
[0014] (2)肩上的AHRS将其测量得到的行人航向信息直接输入到鞋上的AHRS，作为其解 算姿态转移矩阵时所需的航向信息；鞋上的AHRS通过自身携带的加速度计和陀螺仪以及 肩上的AHRS提供的行人航向信息对行人的轨迹进行解算；
[0015] (3)将人的行走分为静止和运动两个状态，对行人的当前运动状态进行判断；
[0016] (4) 一旦行人处于静止状态，将鞋上的AHRS通过将自身携带的3个加速度和3个 陀螺仪传感器所采集到的数据作为卡尔曼滤波器的观测量，并对AHRS采集到的数据进行 误差补偿，最终得到当前时刻最优的导航信息预估；
[0017] (5)将AHRS解算的速度和姿态信息与卡尔曼滤波器输出的误差预估分别作为人 工智能算法的培训输入和目标输入，对二者的映射关系进行构建；
[0018] (6) -旦行人处于运动状态，人工智能算法进入预估阶段；在这一阶段，AHRS中输 出的速度和姿态信息作为预估输入传送到人工智能算法中，通过构建的映射关系，对卡尔 曼滤波器失锁的观测向量进行预估，保证卡尔曼滤波器的正常工作，进而保证整个导航系 统的无缝导航。
[0019] 所述步骤（3)中判断行人的当前运动状态的方法为：
[0020] 鞋上的AHRS通过将自身携带的加速度和陀螺仪传感器所采集到的数据计算当前 时刻鞋子的加速度模值，通过加速度模值的大小判断鞋子的运动状态。
[0021] 通过加速度模值的大小判断鞋子的运动状态的具体方法为：
[0023] 其中，加速度模值
ax、ay、az分别为在载体坐标系下的X、 y和z三个方向的加速度值。
[0024] 所述步骤（4)中一旦当前时刻鞋子处于静止状态，所采用的卡尔曼滤波器的状态 方程具体为：
[0026] 其中，
为AHRS测量得到的导航坐标系下的 东向、北向和天向三个方向的加速度误差；SVn为AHRS测量得到的导航坐标系下的东向、 北向和天向三个方向的速度误差；巾为AHRS测量得到的导航坐标系下的横摇、纵摇和航向 角误差；SP^^/AHRS测量得到的导航坐标系下的东向、北向和天向三个方向的位置误差； ▽"为导航坐标系下的东向、北向和天向三个方向的加速度误差；en为导航坐标系下的东 向、北向和天向三个方向的角速度误差；为状态噪声；T为采样周期；I为单位矩阵。
[0027] 所述卡尔曼滤波器的观测方程具体为：
[0029] 其中，为当前时刻测量到的AHRS解算的导航坐标系下的东向、北向和天向三 个方向的速度；S为当前时刻测量到的AHRS解算的姿态误差；nk为滤波器的观测噪声； SVn为AHRS测量得到的导航坐标系下的东向、北向和天向三个方向的速度误差；巾为AHRS 测量得到的导航坐标系下的横摇、纵摇和航向角误差；SPnSAHRS测量得到的导航坐标系 下的东向、北向和天向三个方向的位置误差；▽"为导航坐标系下的东向、北向和天向三个 方向的加速度误差；e"为导航坐标系下的东向、北向和天向三个方向的角速度误差。
[0030] 鞋子处于静止状态时，AHRS测量得到的速度均为AHRS的解算误差；AHRS测量得到 的姿态角均为AHRS的解算误差。
[0031] 本发明有益效果：
[0032] 1、该方法将肩部电子罗盘测量得到行人航向信息引入到足部AHRS的导航信息解 算过程中，以提高足部AHRS的姿态转移矩阵的计算精度。
[0033] 2、将人的行走分为静止和运动两个状态。在人处于静止状态时，将鞋上的AHRS通 过将自身携带的3个加速度和3个陀螺仪传感器所采集到的数据作为卡尔曼滤波器的观测 量，并对AHRS采集到的数据进行误差补偿，最终得到当前时刻最优的导航信息预估。
[0034] 人工智能算法在此时处于培训状态，将AHRS解算的速度和姿态信息与卡尔曼滤 波器输出的误差预估分别作为人工智能算法的培训输入和目标输入，对二者的映射关系进 行构建。一旦行人处于运动状态，人工智能算法进入预估阶段；在这一阶段，AHRS中输出的 速度和姿态信息作为预估输入传送到人工智能算法中，通过构建的映射关系，对卡尔曼滤 波器失锁的观测向量进行预估，保证卡尔曼滤波器的正常工作，进而保证整个导航系统的 无缝导航。
[0035] 3、可用于室内环境下的智能鞋子的中高精度无缝定位和定向。
【附图说明】
[0036] 图1为用于一种基于足部航姿参考和肩部电子罗盘的个人导航系统示意图；
[0037] 图2为行人静止情况下一种基于足部航姿参考和肩部电子罗盘的室内个人导航 方法的示意图；
[0038] 图3为行人运动情况下一种基于足部航姿参考和肩部电子罗盘的室内个人导航 方法的示意图。
【具体实施方式】
[0039] 下面结合附图与实施例对本发明做进一步说明：
[0040] 如图1所示，用于一种基于足部航姿参考和肩部电子罗盘的室内个人导航方法的 系统，包括参考系统和行人导航系统两个部分。其中行人导航系统包括一个足部AHRS、一个 肩部电子罗盘和一个计算机，参考系统包括一个码盘和一个固定于码盘的AHRS。
[0041] 足部AHRS与肩部电子罗盘连接，参考系统、足部AHRS和肩部电子罗盘分别于控制 器连接；
[0042]肩部电子罗盘将其测量得到的行人航向信息直接输入到足部AHRS，作为其解算姿 态转移矩阵时所需的航向信息 ；足部AHRS通过自身携带的加速度计和陀螺仪以及肩部电 子罗盘提供的行人航向信息对行人的轨迹进行解算。
[0043] 如图2所示，在一种基于足部航姿参考系统和肩部电子罗盘的室内个人导航方法 使用卡尔曼滤波器完成系统的数据融合，具体步骤如下：
[0044] 一种基于足部航姿参考和肩部电子罗盘的室内个人导航方法，属于复杂环境下组 合定位技术领域，其特征是，包括以下步骤：
[0045] (1)将两个低成本AHRS分别安装于行人的鞋上和肩上，其中安装于鞋上的AHRS主 要负责导航解算，肩上的AHRS主要负责行人航向测量；
[0046] (2)肩上的AHRS将其测量得到的行人航向信息直接输入到鞋上的AHRS，作为其解 算姿态转移矩阵时所需的航向信息；鞋上的AHRS通过自身携带的加速度计和陀螺仪以及 肩上的AHRS提供的行人航向信息对行人的轨迹进行解算；
[0047] (3)鞋上的AHRS通过将自身携带的3个加速度和3个陀螺仪传感器所采集到的数 据进行计算，判断行人的运动状态；
[0048] 鞋上的AHRS通过将自身携带的加速度和陀螺仪传感器所采集到的数据计算当前 时刻鞋子的加速度模值，通过加速度模值的大小判断鞋子的运动状态。
[0049] 通过加速度模值的大小判断鞋子的运动状态的具体方法为：
[0051] 其中，加速度模值：，ax、ay、az分别为在载体坐标系下的X、 y和z三个方向的加速度值。
[0052] (4) -旦鞋子处于静止状态，将鞋上的AHRS通过将自身携带的3个加速度和3个 陀螺仪传感器所采集到的数据作为卡尔曼滤波器的观测量，并对AHRS采集到的数据进行 误差补偿，最终得到当前时刻最优的导航信息预估。所采用的卡尔曼滤波器的状态方程具 体为：
[0054] 其中，
为AHRS测量得到的导航坐标系下的 东向、北向和天向三个方向的加速度误差；SVn为AHRS测量得到的导航坐标系下的东向、 北向和天向三个方向的速度误差；巾为AHRS测量得到的导航坐标系下的横摇、纵摇和航向 角误差；SP^^/AHRS测量得到的导航坐标系下的东向、北向和天向三个方向的位置误差； ▽"为导航坐标系下的东向、北向和天向三个方向的加速度误差；en为导航坐标系下的东 向、北向和天向三个方向的角速度误差；wk为状态噪声；
[0055] 卡尔曼滤波器的观测方程具体为：
[0057] 其中，沐丨为当前时刻测量到的AHRS解算的导航坐标系下的东向、北向和天向三 个方向的速度，由于鞋子处于静止状态，因此当前时刻的理论值为零，故AHRS测量得到的 速度均为AHRS的解算误差；€为当前时刻测量到的AHRS解算的姿态误差，由于本局部滤波 器工作时，鞋子处于静止状态，因此当前时刻的理论值为零，故AHRS测量得到的姿态角均 为AHRS的解算误差；nk为滤波器的观测噪声。
[0058] 此时，人工智能算法在此时处于培训状态，将AHRS解算的速度和姿态信息与卡尔 曼滤波器输出的误差预估分别作为人工智能算法的培训输入和目标输入，对二者的映射关 系进行构建。
[0059] (5) -旦行人处于运动状态，人工智能算法进入预估阶段；如图3所示，在这一阶 段，AHRS中输出的速度和姿态信息作为预估输入传送到人工智能算法中，通过构建的映射 关系，对卡尔曼滤波器失锁的观测向量进行预估，保证卡尔曼滤波器的正常工作，进而保证 整个导航系统的无缝导航。
[0060] 上述虽然结合附图对本发明的【具体实施方式】进行了描述，但并非对本发明保护范 围的限制，所属领域技术人员应该明白，在本发明的技术方案的基础上，本领域技术人员不 需要付出创造性劳动即可做出的各种修改或变形仍在本发明的保护范围以内。
【主权项】
1. 一种基于足部航姿参考和肩部电子罗盘的个人导航系统，其特征是，包括：参考系 统和行人导航系统； 所述参考系统包括：码盘和固定于码盘的AHRS ; 所述行人导航系统包括：足部AHRS、肩部电子罗盘和控制器；足部AHRS与肩部电子罗 盘连接，参考系统、足部AHRS和肩部电子罗盘分别于控制器连接； 肩部电子罗盘将其测量得到的行人航向信息直接输入到足部AHRS，作为其解算姿态转 移矩阵时所需的航向信息；足部AHRS通过自身携带的加速度计和陀螺仪以及肩部电子罗 盘提供的行人航向信息对行人的轨迹进行解算。2. -种基于足部航姿参考和肩部电子罗盘的个人导航方法，其特征是，包括以下步 骤： (1) 将两个低成本AHRS分别安装于行人的鞋上和肩上，其中安装于鞋上的AHRS主要负 责导航解算，肩上的AHRS作为电子罗盘主要负责行人航向测量； (2) 肩上的AHRS将其测量得到的行人航向信息直接输入到鞋上的AHRS，作为其解算姿 态转移矩阵时所需的航向信息；鞋上的AHRS通过自身携带的加速度计和陀螺仪以及肩上 的AHRS提供的行人航向信息对行人的轨迹进行解算； (3) 将人的行走分为静止和运动两个状态，对行人的当前运动状态进行判断； (4) 一旦行人处于静止状态，将鞋上的AHRS通过将自身携带的加速度传感器和陀螺仪 传感器所采集到的数据作为卡尔曼滤波器的观测量，并对足部AHRS采集到的数据进行误 差补偿，最终得到当前时刻最优的导航信息预估； (5) 将足部AHRS解算的速度和姿态信息与卡尔曼滤波器输出的误差预估分别作为人 工智能算法的培训输入和目标输入，对二者的映射关系进行构建； (6) -旦行人处于运动状态，人工智能算法进入预估阶段；在这一阶段，足部AHRS中输 出的速度和姿态信息作为预估输入传送到人工智能算法中，通过构建的映射关系，对卡尔 曼滤波器失锁的观测向量进行预估，保证卡尔曼滤波器的正常工作，进而保证整个导航系 统的无缝导航。3. 如权利要求1所述的一种基于足部航姿参考和肩部电子罗盘的个人导航方法，其特 征是，所述步骤（3)中判断行人的当前运动状态的方法为： 鞋上的AHRS通过将自身携带的加速度和陀螺仪传感器所采集到的数据计算当前时刻 鞋子的加速度模值，通过加速度模值的大小判断鞋子的运动状态。4. 如权利要求3所述的一种基于足部航姿参考和肩部电子罗盘的个人导航方法，其特 征是，通过加速度模值的大小判断鞋子的运动状态的具体方法为：其中，加速度模值：，ax、ay、a z分别为在载体坐标系下的X、y和 z三个方向的加速度值。5. 如权利要求1所述的一种基于足部航姿参考和肩部电子罗盘的个人导航方法，其特 征是，所述步骤（4)中一旦当前时刻鞋子处于静止状态，所采用的卡尔曼滤波器的状态方程 具体为：其中；为AHRS测量得到的导航坐标系下的东 向、北向和天向三个方向的加速度误差；S VnS AHRS测量得到的导航坐标系下的东向、北 向和天向三个方向的速度误差；Φ为AHRS测量得到的导航坐标系下的横摇、纵摇和航向角 误差；S PnS AHRS测量得到的导航坐标系下的东向、北向和天向三个方向的位置误差；▽ n为导航坐标系下的东向、北向和天向三个方向的加速度误差；ε n为导航坐标系下的东向、 北向和天向三个方向的角速度误差；为状态噪声；T为采样周期；I为单位矩阵。6. 如权利要求1所述的一种基于足部航姿参考和肩部电子罗盘的个人导航方法，其特 征是，所述卡尔曼滤波器的观测方程具体为：其中，冰"为当前时刻测量到的AHRS解算的导航坐标系下的东向、北向和天向三个方 向的速度；€为当前时刻测量到的AHRS解算的姿态误差；Iik为滤波器的观测噪声；δ ￥"为 AHRS测量得到的导航坐标系下的东向、北向和天向三个方向的速度误差；Φ为AHRS测量得 到的导航坐标系下的横摇、纵摇和航向角误差；S PnS AHRS测量得到的导航坐标系下的东 向、北向和天向三个方向的位置误差；▽ η为导航坐标系下的东向、北向和天向三个方向的 加速度误差；ε "为导航坐标系下的东向、北向和天向三个方向的角速度误差。7. 如权利要求1所述的一种基于足部航姿参考和肩部电子罗盘的个人导航方法，其特 征是，鞋子处于静止状态时，AHRS测量得到的速度均为AHRS的解算误差；AHRS测量得到的 姿态角均为AHRS的解算误差。
【专利摘要】本发明公开了一种基于足部航姿参考和肩部电子罗盘的个人导航系统及方法，包括：参考系统码盘和固定于码盘的AHRS；行人导航系统包括足部AHRS、肩部电子罗盘和控制器；足部AHRS与肩部电子罗盘连接，参考系统、足部AHRS和肩部电子罗盘分别于控制器连接；肩部电子罗盘将其测量得到的行人航向信息直接输入到足部AHRS，作为其解算姿态转移矩阵时所需的航向信息；足部AHRS通过自身携带的加速度计和陀螺仪以及肩部电子罗盘提供的行人航向信息对行人的轨迹进行解算。本发明有益效果：将肩部电子罗盘测量得到行人航向信息引入到足部AHRS的导航信息解算过程中，以提高足部AHRS的姿态转移矩阵的计算精度。
【IPC分类】G01C21/16, G01C21/20
【公开号】CN104897157
【申请号】CN201510358598
【发明人】徐元, 张勇, 程金, 赵钦君, 张勤
【申请人】济南大学
【公开日】2015年9月9日
【申请日】2015年6月25日
【技术领域】
[0001] 本发明涉及复杂环境下组合定位技术领域，尤其涉及一种基于足部航姿参考和肩 部电子罗盘的个人导航系统及方法。
【背景技术】
[0002] 近年来，行人导航（PedestrianNavigation,PN)作为导航技术应用的新兴领域， 正越来越受到各国学者的重视，并逐渐成为该领域的研宄热点。然而在隧道、大型仓库、地 下停车场等室内环境下，外界无线电信号微弱、电磁干扰强烈等因素都会对目标行人导航 信息获取的准确性、实时性及鲁棒性有很大影响。如何将室内环境下获取的有限信息进行 有效的融合以消除室内复杂环境影响，保证行人导航精度的持续稳定，具有重要的科学理 论意义和实际应用价值。
[0003] 在现有的定位方式中，全球卫星导航系统（GlobalNavigationSatellite System，GNSS)是最为常用的一种方式。虽然GNSS能够通过精度持续稳定的位置信息，但是 其易受电磁干扰、遮挡等外界环境影响的缺点限制了其应用范围，特别是在室内、地下巷道 等一些密闭的、环境复杂的场景，GNSS信号被严重遮挡，无法进行有效的工作。近年来，WSN 以其低成本、低功耗和低系统复杂度的特点在短距离局部定位领域表现出很大的潜力。
[0004] 学者们提出将基于WSN的目标跟踪应用于GNSS失效环境下的行人导航。这种方 式虽然能够实现室内定位，但是由于室内环境复杂多变，WSN信号十分容易受到干扰而导致 定位精度下降甚至失锁；与此同时，目前的导航定位精度为米级，不能保证对室内行人高精 度的导航需求；除此之外，由于WSN采用的通信技术通常为短距离无线通信技术，因此若想 完成大范围的室内目标跟踪定位，需要大量的网络节点共同完成，这必将引入网络组织结 构优化设计、多节点多簇网络协同通信等一系列问题。因此现阶段基于WSN的目标跟踪在 室内导航领域仍旧面临很多挑战。
[0005] 为了克服上述两种导航方法需要参考节点并容易产生失锁的缺点，学者们提出将 航姿参考系统（AHRS)应用于小区域目标跟踪领域。AHRS具有全自主、运动信息全面、短时、 高精度的优点，虽然可以实现自主导航，但误差随时间积累，长航时运行条件下将导致导航 精度严重下降。
【发明内容】
[0006] 本发明的目的就是解决上述问题，提供一种基于足部航姿参考和肩部电子罗盘的 个人导航系统及方法，该方法将肩部电子罗盘测量得到行人航向信息引入到足部AHRS的 导航信息解算过程中，提高足部AHRS的姿态转移矩阵的计算精度。
[0007] 为实现上述目的，本发明采用下述技术方案，包括：
[0008] -种基于足部航姿参考和肩部电子罗盘的个人导航系统，包括：参考系统和行人 导航系统；
[0009] 所述参考系统包括：码盘和固定于码盘的AHRS;
[0010] 所述行人导航系统包括：足部AHRS、肩部电子罗盘和控制器；足部AHRS与肩部电 子罗盘连接，参考系统、足部AHRS和肩部电子罗盘分别于控制器连接；
[0011]肩部电子罗盘将其测量得到的行人航向信息直接输入到足部AHRS，作为其解算姿 态转移矩阵时所需的航向信息；足部AHRS通过自身携带的加速度计和陀螺仪以及肩部电 子罗盘提供的行人航向信息对行人的轨迹进行解算。
[0012] 一种基于足部航姿参考和肩部电子罗盘的个人导航方法，包括以下步骤：
[0013] (1)将两个低成本AHRS分别安装于行人的鞋上和肩上，其中安装于鞋上的AHRS主 要负责导航解算，肩上的AHRS作为电子罗盘主要负责行人航向测量；
[0014] (2)肩上的AHRS将其测量得到的行人航向信息直接输入到鞋上的AHRS，作为其解 算姿态转移矩阵时所需的航向信息；鞋上的AHRS通过自身携带的加速度计和陀螺仪以及 肩上的AHRS提供的行人航向信息对行人的轨迹进行解算；
[0015] (3)将人的行走分为静止和运动两个状态，对行人的当前运动状态进行判断；
[0016] (4) 一旦行人处于静止状态，将鞋上的AHRS通过将自身携带的3个加速度和3个 陀螺仪传感器所采集到的数据作为卡尔曼滤波器的观测量，并对AHRS采集到的数据进行 误差补偿，最终得到当前时刻最优的导航信息预估；
[0017] (5)将AHRS解算的速度和姿态信息与卡尔曼滤波器输出的误差预估分别作为人 工智能算法的培训输入和目标输入，对二者的映射关系进行构建；
[0018] (6) -旦行人处于运动状态，人工智能算法进入预估阶段；在这一阶段，AHRS中输 出的速度和姿态信息作为预估输入传送到人工智能算法中，通过构建的映射关系，对卡尔 曼滤波器失锁的观测向量进行预估，保证卡尔曼滤波器的正常工作，进而保证整个导航系 统的无缝导航。
[0019] 所述步骤（3)中判断行人的当前运动状态的方法为：
[0020] 鞋上的AHRS通过将自身携带的加速度和陀螺仪传感器所采集到的数据计算当前 时刻鞋子的加速度模值，通过加速度模值的大小判断鞋子的运动状态。
[0021] 通过加速度模值的大小判断鞋子的运动状态的具体方法为：
[0023] 其中，加速度模值
ax、ay、az分别为在载体坐标系下的X、 y和z三个方向的加速度值。
[0024] 所述步骤（4)中一旦当前时刻鞋子处于静止状态，所采用的卡尔曼滤波器的状态 方程具体为：
[0026] 其中，
为AHRS测量得到的导航坐标系下的 东向、北向和天向三个方向的加速度误差；SVn为AHRS测量得到的导航坐标系下的东向、 北向和天向三个方向的速度误差；巾为AHRS测量得到的导航坐标系下的横摇、纵摇和航向 角误差；SP^^/AHRS测量得到的导航坐标系下的东向、北向和天向三个方向的位置误差； ▽"为导航坐标系下的东向、北向和天向三个方向的加速度误差；en为导航坐标系下的东 向、北向和天向三个方向的角速度误差；为状态噪声；T为采样周期；I为单位矩阵。
[0027] 所述卡尔曼滤波器的观测方程具体为：
[0029] 其中，为当前时刻测量到的AHRS解算的导航坐标系下的东向、北向和天向三 个方向的速度；S为当前时刻测量到的AHRS解算的姿态误差；nk为滤波器的观测噪声； SVn为AHRS测量得到的导航坐标系下的东向、北向和天向三个方向的速度误差；巾为AHRS 测量得到的导航坐标系下的横摇、纵摇和航向角误差；SPnSAHRS测量得到的导航坐标系 下的东向、北向和天向三个方向的位置误差；▽"为导航坐标系下的东向、北向和天向三个 方向的加速度误差；e"为导航坐标系下的东向、北向和天向三个方向的角速度误差。
[0030] 鞋子处于静止状态时，AHRS测量得到的速度均为AHRS的解算误差；AHRS测量得到 的姿态角均为AHRS的解算误差。
[0031] 本发明有益效果：
[0032] 1、该方法将肩部电子罗盘测量得到行人航向信息引入到足部AHRS的导航信息解 算过程中，以提高足部AHRS的姿态转移矩阵的计算精度。
[0033] 2、将人的行走分为静止和运动两个状态。在人处于静止状态时，将鞋上的AHRS通 过将自身携带的3个加速度和3个陀螺仪传感器所采集到的数据作为卡尔曼滤波器的观测 量，并对AHRS采集到的数据进行误差补偿，最终得到当前时刻最优的导航信息预估。
[0034] 人工智能算法在此时处于培训状态，将AHRS解算的速度和姿态信息与卡尔曼滤 波器输出的误差预估分别作为人工智能算法的培训输入和目标输入，对二者的映射关系进 行构建。一旦行人处于运动状态，人工智能算法进入预估阶段；在这一阶段，AHRS中输出的 速度和姿态信息作为预估输入传送到人工智能算法中，通过构建的映射关系，对卡尔曼滤 波器失锁的观测向量进行预估，保证卡尔曼滤波器的正常工作，进而保证整个导航系统的 无缝导航。
[0035] 3、可用于室内环境下的智能鞋子的中高精度无缝定位和定向。
【附图说明】
[0036] 图1为用于一种基于足部航姿参考和肩部电子罗盘的个人导航系统示意图；
[0037] 图2为行人静止情况下一种基于足部航姿参考和肩部电子罗盘的室内个人导航 方法的示意图；
[0038] 图3为行人运动情况下一种基于足部航姿参考和肩部电子罗盘的室内个人导航 方法的示意图。
【具体实施方式】
[0039] 下面结合附图与实施例对本发明做进一步说明：
[0040] 如图1所示，用于一种基于足部航姿参考和肩部电子罗盘的室内个人导航方法的 系统，包括参考系统和行人导航系统两个部分。其中行人导航系统包括一个足部AHRS、一个 肩部电子罗盘和一个计算机，参考系统包括一个码盘和一个固定于码盘的AHRS。
[0041] 足部AHRS与肩部电子罗盘连接，参考系统、足部AHRS和肩部电子罗盘分别于控制 器连接；
[0042]肩部电子罗盘将其测量得到的行人航向信息直接输入到足部AHRS，作为其解算姿 态转移矩阵时所需的航向信息 ；足部AHRS通过自身携带的加速度计和陀螺仪以及肩部电 子罗盘提供的行人航向信息对行人的轨迹进行解算。
[0043] 如图2所示，在一种基于足部航姿参考系统和肩部电子罗盘的室内个人导航方法 使用卡尔曼滤波器完成系统的数据融合，具体步骤如下：
[0044] 一种基于足部航姿参考和肩部电子罗盘的室内个人导航方法，属于复杂环境下组 合定位技术领域，其特征是，包括以下步骤：
[0045] (1)将两个低成本AHRS分别安装于行人的鞋上和肩上，其中安装于鞋上的AHRS主 要负责导航解算，肩上的AHRS主要负责行人航向测量；
[0046] (2)肩上的AHRS将其测量得到的行人航向信息直接输入到鞋上的AHRS，作为其解 算姿态转移矩阵时所需的航向信息；鞋上的AHRS通过自身携带的加速度计和陀螺仪以及 肩上的AHRS提供的行人航向信息对行人的轨迹进行解算；
[0047] (3)鞋上的AHRS通过将自身携带的3个加速度和3个陀螺仪传感器所采集到的数 据进行计算，判断行人的运动状态；
[0048] 鞋上的AHRS通过将自身携带的加速度和陀螺仪传感器所采集到的数据计算当前 时刻鞋子的加速度模值，通过加速度模值的大小判断鞋子的运动状态。
[0049] 通过加速度模值的大小判断鞋子的运动状态的具体方法为：
[0051] 其中，加速度模值：，ax、ay、az分别为在载体坐标系下的X、 y和z三个方向的加速度值。
[0052] (4) -旦鞋子处于静止状态，将鞋上的AHRS通过将自身携带的3个加速度和3个 陀螺仪传感器所采集到的数据作为卡尔曼滤波器的观测量，并对AHRS采集到的数据进行 误差补偿，最终得到当前时刻最优的导航信息预估。所采用的卡尔曼滤波器的状态方程具 体为：
[0054] 其中，
为AHRS测量得到的导航坐标系下的 东向、北向和天向三个方向的加速度误差；SVn为AHRS测量得到的导航坐标系下的东向、 北向和天向三个方向的速度误差；巾为AHRS测量得到的导航坐标系下的横摇、纵摇和航向 角误差；SP^^/AHRS测量得到的导航坐标系下的东向、北向和天向三个方向的位置误差； ▽"为导航坐标系下的东向、北向和天向三个方向的加速度误差；en为导航坐标系下的东 向、北向和天向三个方向的角速度误差；wk为状态噪声；
[0055] 卡尔曼滤波器的观测方程具体为：
[0057] 其中，沐丨为当前时刻测量到的AHRS解算的导航坐标系下的东向、北向和天向三 个方向的速度，由于鞋子处于静止状态，因此当前时刻的理论值为零，故AHRS测量得到的 速度均为AHRS的解算误差；€为当前时刻测量到的AHRS解算的姿态误差，由于本局部滤波 器工作时，鞋子处于静止状态，因此当前时刻的理论值为零，故AHRS测量得到的姿态角均 为AHRS的解算误差；nk为滤波器的观测噪声。
[0058] 此时，人工智能算法在此时处于培训状态，将AHRS解算的速度和姿态信息与卡尔 曼滤波器输出的误差预估分别作为人工智能算法的培训输入和目标输入，对二者的映射关 系进行构建。
[0059] (5) -旦行人处于运动状态，人工智能算法进入预估阶段；如图3所示，在这一阶 段，AHRS中输出的速度和姿态信息作为预估输入传送到人工智能算法中，通过构建的映射 关系，对卡尔曼滤波器失锁的观测向量进行预估，保证卡尔曼滤波器的正常工作，进而保证 整个导航系统的无缝导航。
[0060] 上述虽然结合附图对本发明的【具体实施方式】进行了描述，但并非对本发明保护范 围的限制，所属领域技术人员应该明白，在本发明的技术方案的基础上，本领域技术人员不 需要付出创造性劳动即可做出的各种修改或变形仍在本发明的保护范围以内。
【主权项】
1. 一种基于足部航姿参考和肩部电子罗盘的个人导航系统，其特征是，包括：参考系 统和行人导航系统； 所述参考系统包括：码盘和固定于码盘的AHRS ; 所述行人导航系统包括：足部AHRS、肩部电子罗盘和控制器；足部AHRS与肩部电子罗 盘连接，参考系统、足部AHRS和肩部电子罗盘分别于控制器连接； 肩部电子罗盘将其测量得到的行人航向信息直接输入到足部AHRS，作为其解算姿态转 移矩阵时所需的航向信息；足部AHRS通过自身携带的加速度计和陀螺仪以及肩部电子罗 盘提供的行人航向信息对行人的轨迹进行解算。2. -种基于足部航姿参考和肩部电子罗盘的个人导航方法，其特征是，包括以下步 骤： (1) 将两个低成本AHRS分别安装于行人的鞋上和肩上，其中安装于鞋上的AHRS主要负 责导航解算，肩上的AHRS作为电子罗盘主要负责行人航向测量； (2) 肩上的AHRS将其测量得到的行人航向信息直接输入到鞋上的AHRS，作为其解算姿 态转移矩阵时所需的航向信息；鞋上的AHRS通过自身携带的加速度计和陀螺仪以及肩上 的AHRS提供的行人航向信息对行人的轨迹进行解算； (3) 将人的行走分为静止和运动两个状态，对行人的当前运动状态进行判断； (4) 一旦行人处于静止状态，将鞋上的AHRS通过将自身携带的加速度传感器和陀螺仪 传感器所采集到的数据作为卡尔曼滤波器的观测量，并对足部AHRS采集到的数据进行误 差补偿，最终得到当前时刻最优的导航信息预估； (5) 将足部AHRS解算的速度和姿态信息与卡尔曼滤波器输出的误差预估分别作为人 工智能算法的培训输入和目标输入，对二者的映射关系进行构建； (6) -旦行人处于运动状态，人工智能算法进入预估阶段；在这一阶段，足部AHRS中输 出的速度和姿态信息作为预估输入传送到人工智能算法中，通过构建的映射关系，对卡尔 曼滤波器失锁的观测向量进行预估，保证卡尔曼滤波器的正常工作，进而保证整个导航系 统的无缝导航。3. 如权利要求1所述的一种基于足部航姿参考和肩部电子罗盘的个人导航方法，其特 征是，所述步骤（3)中判断行人的当前运动状态的方法为： 鞋上的AHRS通过将自身携带的加速度和陀螺仪传感器所采集到的数据计算当前时刻 鞋子的加速度模值，通过加速度模值的大小判断鞋子的运动状态。4. 如权利要求3所述的一种基于足部航姿参考和肩部电子罗盘的个人导航方法，其特 征是，通过加速度模值的大小判断鞋子的运动状态的具体方法为：其中，加速度模值：，ax、ay、a z分别为在载体坐标系下的X、y和 z三个方向的加速度值。5. 如权利要求1所述的一种基于足部航姿参考和肩部电子罗盘的个人导航方法，其特 征是，所述步骤（4)中一旦当前时刻鞋子处于静止状态，所采用的卡尔曼滤波器的状态方程 具体为：其中；为AHRS测量得到的导航坐标系下的东 向、北向和天向三个方向的加速度误差；S VnS AHRS测量得到的导航坐标系下的东向、北 向和天向三个方向的速度误差；Φ为AHRS测量得到的导航坐标系下的横摇、纵摇和航向角 误差；S PnS AHRS测量得到的导航坐标系下的东向、北向和天向三个方向的位置误差；▽ n为导航坐标系下的东向、北向和天向三个方向的加速度误差；ε n为导航坐标系下的东向、 北向和天向三个方向的角速度误差；为状态噪声；T为采样周期；I为单位矩阵。6. 如权利要求1所述的一种基于足部航姿参考和肩部电子罗盘的个人导航方法，其特 征是，所述卡尔曼滤波器的观测方程具体为：其中，冰"为当前时刻测量到的AHRS解算的导航坐标系下的东向、北向和天向三个方 向的速度；€为当前时刻测量到的AHRS解算的姿态误差；Iik为滤波器的观测噪声；δ ￥"为 AHRS测量得到的导航坐标系下的东向、北向和天向三个方向的速度误差；Φ为AHRS测量得 到的导航坐标系下的横摇、纵摇和航向角误差；S PnS AHRS测量得到的导航坐标系下的东 向、北向和天向三个方向的位置误差；▽ η为导航坐标系下的东向、北向和天向三个方向的 加速度误差；ε "为导航坐标系下的东向、北向和天向三个方向的角速度误差。7. 如权利要求1所述的一种基于足部航姿参考和肩部电子罗盘的个人导航方法，其特 征是，鞋子处于静止状态时，AHRS测量得到的速度均为AHRS的解算误差；AHRS测量得到的 姿态角均为AHRS的解算误差。
【专利摘要】本发明公开了一种基于足部航姿参考和肩部电子罗盘的个人导航系统及方法，包括：参考系统码盘和固定于码盘的AHRS；行人导航系统包括足部AHRS、肩部电子罗盘和控制器；足部AHRS与肩部电子罗盘连接，参考系统、足部AHRS和肩部电子罗盘分别于控制器连接；肩部电子罗盘将其测量得到的行人航向信息直接输入到足部AHRS，作为其解算姿态转移矩阵时所需的航向信息；足部AHRS通过自身携带的加速度计和陀螺仪以及肩部电子罗盘提供的行人航向信息对行人的轨迹进行解算。本发明有益效果：将肩部电子罗盘测量得到行人航向信息引入到足部AHRS的导航信息解算过程中，以提高足部AHRS的姿态转移矩阵的计算精度。
【IPC分类】G01C21/16, G01C21/20
【公开号】CN104897157
【申请号】CN201510358598
【发明人】徐元, 张勇, 程金, 赵钦君, 张勤
【申请人】济南大学
【公开日】2015年9月9日
【申请日】2015年6月25日
最新回复(0)