技术领域
[0001] 本
发明涉及一种基于MEMS-IMU的室内行人导航航向反馈修正方法，属于行人导航技术领域。
背景技术
[0002] 行人导航技术已掀起国内外研究热潮，其中航向作为核心技术之一，对行人导航
定位精度起着至关重要的作用。传统的
姿态解算中利用
陀螺仪获得
角速率，进而通过四元数解算得到角度，因其利用积分原理，故其解算的航向角误差会随时间发散，不满足与长时间的行人定位需求。另一个较为普遍做法是利用磁
力计得到航行角信息，地
磁场作为地球的固有属性，可以用于计算磁航向角，但是室内地磁受到
钢筋、管道等其他金属制品的影响，进而产生严重的航向误差，无法满足室内定位下高精度航向的要求。目前对航向角的修正
算法中，大都需要外界
信号的参与，如基于
移动通信网络的辅助GPS(A-GPS)、伪卫星(Pseudolite)、无线局域网(WLAN)、射频标签(RFID)、Zigbee、蓝牙(Bluetooth，BT)等，这些定位技术都需要提前布置
基础设施，成本较高，后期维护量大等缺点，不适用与如消防人员的
位置跟踪等特殊的救援领域。
发明内容
[0003] 针对上述问题，本发明旨在解决减少纯惯性导航中航向角误差随时间累积的问题。采用MEMS陀螺仪和
加速度计等惯性器件，结合传统的捷联航姿角解算算法和PDR算法，以及室内特殊的环境构造，将行人直行的
角速度和航向等特征作为航向反馈条件，并利用航向差值进行自适应无迹卡尔曼滤波，将最优结果和当前捷联航向角作为下一时刻航向角参与解算，实现的航向精准导航时间满足消防人员定位的特殊需求。
[0004] 为了实现上述目的本发明采用如下技术方案：基于MEMS-IMU的室内行人导航航向反馈修正方法，包括以下步骤：
[0005] (1)通过
建筑物结构特征，构建主方向和主方向航向范围。
[0006] (2)根据加速度计测得的数据进行步态识别，判断行人是否处于行走状态。
[0007] (3)根据相邻步数间航向角和相邻步数间的角速度信息判断行人是否处于直行状态。
[0008] (4)当判断行人在主方向范围内直行，用当前航向角yawi与当前行人所处主方向航向角差值 作为状态量，进行自适应无迹卡尔曼滤波，获得最优估计结果
[0009] (5)将最优估计结果 与当前航向角yawi之和作为此时航向输出值参与位置解算，并将该航向输出值作为下一时刻航向角参与四元数和欧拉角的捷联解算。
[0010] 进一步地，步骤(1)所述构建主方向和主方向航向范围包括：将建筑物内的走廊及过道划分为四个方向，定义为建筑物的主方向；设定行人在主方向上直行的晃动角度范围，建立以主方向为基准的主方向航向角范围。
[0011] 进一步地，步骤(2)所述判断行人是否处于行走状态的方法为，设置一滑动窗口求取加速度方差，然后与预设的方差
阈值比较，判断行人是否处于行走状态。
[0012] 进一步地，步骤(3)所述相邻步数间航向角和相邻步数间的角速度信息判断行人是否处于直行状态，分别采用三步间航向角差值和三步间隔内记录的角速度值进行判断；所述三步间航向角差值作为判断标准时，任两步的航向角绝对差值均小于预设航向角阈值时，行人直行；所述三步间隔内记录的角速度值作为判断标准时，三步间隔内的角速度绝对值小于预设阈值的个数占总个数的比例小于比例设定阈值时，为转向，大于比例设定阈值时，为弯道行走或者直行。判断所述弯道行走或者直行采用的方法是，若记录的角速度中大于零和小于零的个数分差不多时，为直行，否则为弯道行走。
[0013] 本发明的有益效果如下：
[0014] 采用低成本，低功耗，轻
质量的MEMS惯性
传感器，易于普及推广。
[0015] 算法流程简单易操作，无需耗费处理器的太多资源。
[0016] 与不反馈时相比，精确解算航向角的时间提高了两倍以上，在室内导航中可有效提高导航精度，实用性强。
附图说明
[0017] 图1为航向反馈修正算法的流程示意图；
[0018] 图2为行人在室内的主方向和主方向范围示意图；
[0019] 图3为行人直行时的航向角和角速度的变化示意图；
[0020] 图4为行人转向时的航向角和角速度的变化示意图；
[0021] 图5为行人走弯道时的航向角和角速度的变化示意图。
具体实施方式
[0022] 下面结合附图对本发明创造做进一步详细说明。
[0023] 如图1所示，导航解算过程中，陀螺仪可以实时获得角速率信息，通过四元数解算，实时获得捷联航向角，解算
频率一般可设置为50Hz。当行人在主方向范围内直行时，将当前捷联航向角与当前所处的主方向航向角的差值进行自适应无迹卡尔曼滤波，将最优结果与当前捷联航向角之和作为当前航向角的输出，并将其作为下一时刻的航向初始值参与四元和欧拉角解算。
[0024] 基于微
机电系统惯性测量单元(MEMS-IMU)的室内行人导航航向反馈修正算法主要包括以下步骤：
[0025] 步骤一：四元数算法和欧拉角的捷联解算
[0026] 设roll，pitch，yaw分别为
横滚角，
俯仰角和航向角。采用z-x-y的旋转次序，故pitch，roll，yaw为分别为绕着x，y，z轴旋转所得角度。在初始时刻利用加速度值得到俯仰和横滚角分别为
[0027]
[0028]
[0029] 分别表示载体
坐标系下x，y，z轴的加速度值。
[0030] 设置初始航向角yaw＝0°。由初始时刻的roll，pitch，yaw信息可以得出初始四元数为Q(t0)＝[q0(t0),q1(t0),q2(t0),q3(t0)]T，公式(3)为四元数的微分方程。
[0031]
[0032] 其中Q(ti)＝[q0(ti),q1(ti),q2(ti),q3(ti)]T为
采样点i时刻的四元数，采用一阶毕卡法求解四元数微分方程，得到
[0033]
[0034] 其中△t为两相邻采样时刻的时间间隔。ωx，ωy，ωz为分别为载体坐标系相对于惯性坐标系的角速度信息，考虑MEMS-IMU的器件精度，故对其中地球自转角速度和导航系下载体运动引起的角速度均可忽略不计，ωx，ωy，ωz即为陀螺仪所测量的三轴角速度。
[0035] 四元数更新后，为了便于对角度的直观理解，将四元数转换为欧拉角，其对应的关系为
[0036]
[0037] 步骤二：主方向和主方向范围设定：
[0038] 从开放的卫星地图中可以发现，居民楼和商业大厦等绝大部分建筑都是矩形结构，走廊相互
正交或平行，房间的形状也基本为矩形，因此可以将建筑物内的走廊朝向的四个方向定义为建筑物的主方向。如图2中实线所示。考虑到每个行人在直行过程中身体晃动情况不同，根据该行人的晃动情况设定直行晃动角度范围。设置变量range。故当前时刻的航向角与初始时刻航向角(即0°)的差值为-range(即360-range)到range，90-range到90+range，180-range到180+range，270-range到270+range的角度范围内时，即认为处于主方向范围内，如图2中虚线所示。
[0039]
[0040] A1＝1表示行人处于四个主方向范围，A1＝0表示行人未处于四个主方向范围。
[0041] 步骤三：行人直行检测
[0042] 利用加速度滑动方差检测法检测步态：行人在有步态和无步态过程中加速度
波动幅度差别较大，可设置一定的滑动窗口求取方差和适当的方差阈值来对行人是否处于行走状态进行区分，公式如下：
[0043]
[0044] 其中n为窗口中数据的个数，aj为窗口中第j个时刻的目标加速度，为第i-n+1时刻到i时刻目标加速度的均值。Var为此时窗口数据的方差。
[0045]
[0046] Varmax为阈值，当Varmax＝1时，能够区分出行人有无步态。A2＝1，A2＝0分别代表有步态和无步态。然后利用连续三步之间的角速度和航向角信息对行人直行进行判定。
[0047] 行人在直行过程中，航向角变化较小，故利用三步间航向差值进行直行判定。
[0048]
[0049] 上式中m，n可为k，k-1，k-2，m≠n，yawk为当前步态的航向，yawk-1为前一步时的航向，yawk-2为前两步时的航向。行人在直行时身体会出现正负range的角度晃动，如果三步的任两步的航向绝对差值均小于range时，作为行人直行的判断条件之一。A3＝1表示连续三步间的航向差值小于阈值，可作为直行的判定条件，而A3＝0代表行人连续步数间航向变化较大，认为行人未处于直行状态。行人在室内的行走情况简单得分为直行，转向，弯道行走三种模式。图3、图4、图5分别为行人在对应情况下的z轴角速度和航向角的变化图。可看出航向角和角速度变化在三种模式下均有不同，故利用航向角和角速度对行人直行进行判定。为了与三步的航向差值判直行相对应，在角速度判直行中也采集连续运动的三步间隔内所记录的角速度值。从图3，4，5中可看出，在转向中角速度值明显大于弯道和直行。故记录在这段时间内角速度绝对值小于阈值ωth的个数占总个数的比例，记做R1，并对该比例设定阈值r1。当小于r1时，认定是转向，A4记为0，大于r1时则认定为弯道行走或者直行。A4记为1。公式如下所示：
[0050]
[0051] 在弯道行走中由于行人朝着某一个方向转动，故采集到的角速度值会更多得偏向正或者负的方向。如图5中，角速度分布在正值的较多。而行人在直行过程中角速度在正负的分布情况较为均匀，如图3所示。故在记录A4的同时，分别记录大于零和小于零的角速度的个数占总个数的比例，记做R2和R3。选取R2和R3中较大的比例参与直行的判定，并对此比例设定阈值r2，当大于r2时，A5记为0，当小于r2时，A5记为1。公式下所示：
[0052]
[0053] 步骤四：当A2，A3，A4和A5同时为1时才能说明检测到行人直行。
[0054] 步骤五：当检测到行人在主方向范围内直行，将其进行逻辑“与”运算，记做：
[0055] Flag＝A1&A2&A3&A4&A5 (12)
[0056] 如果Flag结果不为1，则进行步骤一。如果A1，A2，A3，A4和A5同时为1时，即Flag为1时，用当前捷联航向角yawi与当前行人所处主方向航向角差值 作为状态量，并对其进行自适应无迹卡尔曼滤波，首先建立6维状态向量：
[0057]
[0058] 式中，航向角和姿态角误差为 δroll，δpitch，δyaw分别表示利用传统和修正两种算法得出的横滚角，俯仰角和航向角差值。角速度误差为δω＝[δωx,δωy,δωz]，δωx，δωy，δωz分别表示x，y，z轴的角速度误差。其中量测矩阵为H＝[0,0,1,0,
0,0]，经过自适应无迹卡尔曼滤波后将最优估计结果 与当前航向角yawi之和作为此刻航向输出值，公式如下：
[0059]
[0060] 该输出值参与行人实时的位置解算，并作为下一时刻航向初始值参与四元数和欧拉角解算。从而实现反馈修正的目的。
[0061] 本发明整个算法流程如下：
[0062] 1、首先利用传统的四元数和欧拉角算法对航姿角进行解算。
[0063] 2、将室内环境划分为四个主方向，并根据行人直行的身体晃动划分为四个主方向范围。
[0064] 3、利用步态识别、三步间航向角和三步间的角速度信息判断行人直行状态；
[0065] 4、利用行人在主方向范围内直行作为航向反馈修正条件。
[0066] 5、利用捷联航向角和当前主方向的航向角的差值作为状态量，与横滚角和俯仰角以及三轴角速度信息构成六维状态向量进行自适应无迹卡尔曼滤波处理，将最优结果与当前捷联航向角之和作为此刻的航向角输出并作为下一时刻的初始航向角参与四元数和欧拉角的解算。
