一种结合行人运动场景约束的方位角校准算法的制作方法

本发明设计的是一种方位角校准算法，尤其是涉及一种结合行人运动场景约束的自主方位角校准算法。

背景技术：

捷联惯性导航系统可以实现完全自主导航，不需要外部设施。近年来，基于低成本捷联惯导系统，小型mems惯性传感器在一些军事和民用导航定位中已经受到了越来越多的关注，特别是卫星导航不可用的情况下，如室内、水下或地下。对于捷联式惯性导航定位系统来说，载体方位角的解算主要依靠陀螺仪完成，而陀螺仪具有积分漂移特性，当方位角偏差较大时，该偏差将成为位置误差的主要来源。因此，根据mems陀螺仪精度的现状，偏差随时间累积上升很快，如果不采用必要的方位角误差定期校正措施，很难支持长时间的准确导航定位。

为了提高mems捷联惯导系统的方位角估计精度，往往引入磁强计作为辅助进行方位角的校准，磁强计是最常用的辅助方位角校准装置，由于磁强计也属于自主型元件，因此引入磁强计的惯性导航定位系统能保持其自主性不受影响。但是当存在干扰磁场时，磁强计的数据反而会使方位角产生较大偏差，使得系统的长期精度差。故现有的方位角校准算法在存在磁场干扰的环境下不能保证方位角长时间高精度的准确输出。

技术实现要素：

本发明的目的在于克服现有方位角校准算法的不足，提供一种结合行人运动场景约束的方位角校准算法。

本发明采用的技术方案为：

一种结合行人运动场景约束的方位角校准算法，包括以下步骤：

(1)根据传感器输出的行人在直线行走模式或行人在楼梯行走模式中的测量数据，通过捷联惯导算法得到行人在直线行走模式或行人在楼梯行走模式中每一时刻行人的速度、位置解算结果和方位解算结果；并根据陀螺仪测量的数据进行四阶龙格库塔算法得到的四元数；所述传感器输出的测量数据包括加速度计测量的数据和陀螺仪测量的数据；

(2)根据位置解算结果、零速检测和零速修正技术，得到相邻两个零速区间上的竖直方向位移差；根据方位角解算结果、零速检测和零速修正技术，得到相邻两个零速区间上的航向角偏差；

(3)根据相邻两个零速区间上的竖直方向位移差和航向角偏差以及四元数建立卡尔曼滤波器，进行行人在直线行走模式或行人在楼梯行走模式中航向角的更新和校准。

其中，步骤(3)中建立的卡尔曼滤波器具体为：

式中，x(k)和x(k-1)分别为卡尔曼滤波算法中k时刻和k-1时刻的状态量，z(k)为卡尔曼滤波算法中k时刻的观测量；φ(k,k-1)为卡尔曼滤波算法中的过程转移矩阵，γ(k-1)为卡尔曼滤波器的外加矩阵，w(k-1)为陀螺噪声矩阵，h(k)为状态变化过程中的调整系数，v(k)为观测噪声；

其中，k-1时刻的状态量和观测量分别表示为：

x(k-1)＝[q0,q1,q2,q3]^t

z(k-1)＝[δsz；δyaw]

式中，q0,q1,q2,q3为陀螺仪测量的数据进行四阶龙格库塔算法得到的四元数，δsz为相邻两个零速区间上的竖直方向位移差，δyaw为相邻两个零速区间上的航向角偏差。

其中，陀螺噪声矩阵表示为：

w(k-1)＝[wx_offest；wy_offest；wz_offest]

式中，wx_offest，wy_offest，wz_offest分别为陀螺仪三个轴上的白噪声；

尔曼滤波器的外加矩阵γ(k-1)表示为：

式中，δt为采样周期；

其中，卡尔曼滤波算法中的过程转移矩阵表示为：

式中，δθx＝wxδt，δθy＝wyδt，δθz＝wzδt，δθ0＝δθx²+δθy²+δθz²，wx、wy和wz分别为陀螺仪测量的数据。

本发明对比现有技术有如下的有益效果：

本发明克服了在地磁受到长时间严重干扰的场合，电子罗盘将无法准确确定方位信息的问题，提高了定位精度。本发明方法简单，稳定性和可靠性高，有效的提高了行人导航的定位精度。

附图说明:

图1是本发明的原理框图。

图2是本发明实施的直线方位解算图。

图3是本发明实施的上楼梯方位角校准算法结果。

图4是本发明实施的上楼梯轨迹图。

具体实施方式：

下面结合附图和具体实施方式对本发明做进一步描述：

图1是本发明的原理框图，在行走中，将脚接触到地面的阶段称为零速区间，行走中相邻两个零速区间的竖直方向位移差和航向角之差应该为零，设计了图1所示的方位角解算算法。

本发明实现步骤如下：

(1)根据陀螺仪和加速度计输出的行人在直线行走模式或行人在楼梯行走模式中的测量数据，通过捷联惯导算法得到行人在直线行走模式或行人在楼梯行走模式中每一时刻行人的速度、位置解算结果和方位解算结果；并根据陀螺仪测量的数据进行四阶龙格库塔算法得到的四元数；所述传感器输出的测量数据包括加速度计测量的数据和陀螺仪测量的数据；

(2)根据位置解算结果、零速检测和零速修正技术，得到相邻两个零速区间上的竖直方向位移差；根据方位角解算结果、零速检测和零速修正技术，得到相邻两个零速区间上的航向角偏差；相邻两个零速区间上的竖直方向位移差用δsz表示；

δsz(i)＝sl(i)-sl(i-1)i＝2,3,........nn

其中，设零速区间个数为nn，零速区间的竖直方向位移为sl。

相邻两个零速区间上的航向角偏差；

δyaw(i)＝yaw(i)-yaw(i-1)i＝2,3,........nn

其中i表示第几个零速区间，设零速区间个数为nn。

(3)根据相邻两个零速区间上的竖直方向位移差和航向角偏差以及四元数建立卡尔曼滤波器，进行行人在直线行走模式或行人在楼梯行走模式中航向角的更新和校准。

建立的卡尔曼滤波器具体为：

式中，x(k)和x(k-1)分别为卡尔曼滤波算法中k时刻和k-1时刻的状态量，z(k)为卡尔曼滤波算法中k时刻的观测量；φ(k,k-1)为卡尔曼滤波算法中的过程转移矩阵，γ(k-1)为卡尔曼滤波器的外加矩阵，w(k-1)为陀螺噪声矩阵，h(k)为状态变化过程中的调整系数，v(k)为观测噪声；

其中，k-1时刻的状态量和观测量分别表示为：

x(k-1)＝[q0,q1,q2,q3]^t

z(k-1)＝[δsz；δyaw]

式中，q0,q1,q2,q3为陀螺仪测量的数据进行四阶龙格库塔算法得到的四元数，δsz为相邻两个零速区间上的竖直方向位移差，δyaw为相邻两个零速区间上的航向角偏差。

其中，陀螺噪声矩阵表示为：

w(k-1)＝[wx_offest；wy_offest；wz_offest]

式中，wx_offest，wy_offest，wz_offest分别为陀螺仪三个轴上的白噪声；

尔曼滤波器的外加矩阵γ(k-1)表示为：

式中，δt为采样周期；

卡尔曼滤波算法中的过程转移矩阵表示为：

式中，δθx＝wxδt，δθy＝wyδt，δθz＝wzδt，δθ0＝δθx²+δθy²+δθz²，wx、wy和wz分别为陀螺仪测量的数据。

为了验证直线算法的可行性分别进行了两组实验，实验模式均为将传感器绑在行人脚上，发出指令使其在室内某一位置出发，沿直线行走，两组实验直线行走距离分别为8米。图2表示行人沿直线行走8米的方位解算图和轨迹图。从方位解算图中不容易直观的观测在直线行走模式中行人的方位偏差，为了进一步判定本算法的实用性，利用直线行走中的始末位置偏差来进行判定，通过matlab软件可以查出在楼梯行走模式中行人初始方位和末位置方位的真实数值，如直线行走模式表格所示。从直线行走模式表格中可以看出通过结合直线运动约束模式的方法，在行人以直线模式行走8米内方位角精度可以保持在0.5°以内，该实验结果表明，结合直线运动约束模式的方法可以很好地对方位角进行校准，利用该算法可以保证方位角长时间高精度输出。

结合楼梯运动模式时，在楼梯行走过程中每一个台阶的高度都是一定的，而且每个台阶的高度相差不大，根据这一特点，可以在楼梯行走中，计算出相邻台阶的竖直方向的位移，然后根据相邻两个台阶竖直方向位移之差来进行航向的修正，脚和台阶接触的区间恰好是零速区间，即可认为两个相邻的零速区间竖直方向位置差近似为恒定值即为一个台阶的高度。首先，根据陀螺仪和加速度计进行姿态解算得到速度、位置和方位，同时对移动模式进行检测，当判定行人在楼梯行走时，对零速修正之后的相邻两个零速区间的竖直方向位移作为下一步卡尔曼滤波算法的量测值，将陀螺仪测量数据过程中解算的四元数作为状态量，通过卡尔曼滤波不断更新，最终得到楼梯行走的最优方位、速度、位置。在卡尔曼滤波中利用δsz作为观测量，该值在上述已经详细介绍，利用导航解算得到的四元数作为状态量来对上下楼梯运动模式中的方位角进行不断的更新校准。

为了验证基于楼梯运动模式的方位角校准算法的有效性，分别进行了上楼梯实验，实验模式为将传感器模块绑在行人脚上进行上楼梯行走(规定其楼梯台阶的某一位置为起点，为了便于求取始末位置航向偏差，最后一个台阶停止的位置和起点位置必须在一条直线上，从起点到最后一个台阶的停止位置过程中对航向不进行限制)。图3表示在上楼梯过程中提出的方位角校准算法解算结果。图4表示上楼梯过程中利用提出的方位角校准算法得出的轨迹图。通过matlab软件可以查出在楼梯行走模式中行人初始方位和末位置方位的真实数值。由结合楼梯模式的表格可知结合上楼梯运动约束模式的方位角校准技术相比于导航定位解算中仅仅采用零速修正的补偿解算方法得到的方位角精度有了明显提高。

结合以下实验对本发明的优益效果作进一步的说明：

本发明使用stm32f103zet6开发板(尼莫m3s)和invensense公司生产的mpu9150(gy-9150)传感器模块来采集数据，在电梯运动模式中，将传感器模块绑在行人脚上，行人在电梯内保持静止状态；在直线运动模式中，行人携带绑在脚上的传感器模块进行直线行走；在楼梯运动模式中，将传感器绑在行人脚上进行上楼梯模式行走。试验场景均为河北工业大学学院楼内。

以满足行人运动的大动态要求。该模块封装了3轴陀螺仪、3轴加速度计、测量值以数字量通过iic接口输出，量程、灵敏度、采样率、带宽可编程控制，其主要技术性能指标如下：

(1)传感器动态测量范围(最大)：陀螺仪：±2000°/s；加速度计：±16g。

(2)传感器灵敏度(最大量程时)：陀螺仪：1/16.4°/s；加速度计：1/2048g。

磁强计：±0.3ut；温度计：1/340℃。

(3)传感器噪声：陀螺仪为加速度计为

(4)最高采样率：陀螺仪：8000sps；加速度计：1000sps。

(5)传感器带宽(可编程)：陀螺仪：2-256hz；加速度计：5-260hz。

实验中行人导航定位系统的采样频率为100hz，为了更形象的说明本发明的有效性。直线行走模式的表格表1所示和楼梯行走模式的表格表2所示给出了本发明提出的结合直线运动模式、结合楼梯运动模式的方位校准偏差。其中在上楼梯模式中结合上楼梯运动约束模式的方位角校准算法较之前的方位角精度提高了81.36％。利用本发明可以很好地对方位角进行校准，利用该发明可以保证方位角长时间高精度输出。

表1

表2

以上所述仅为本发明的较佳实施例而已，并不用以限制本发明，凡在本发明的原则之内所作的任何修改、等同替换和改进等，均应包含在本发明的保护范围之内。