胸牌式消费级IMU行人导航方法

技术领域

本发明涉及微机电系统技术领域，具体而言，涉及胸牌式消费级IMU行人导航方法。

背景技术

随着通信技术和计算机技术的高速发展，人们对于位置服务的需求不断增加，这也促使基于位置服务的应用范围越来越广泛，包括军事、医疗、制造、物流等多个领域和关键系统。而由于城市建筑物结构及面积日趋复杂化、大型化的形势下，室内作为人类主要活动场所具体室内活动时间约占80％，如何准确定位室内用户位置，实现协同导航，成为室内外无缝协同导航的热点问题。

由于卫星信号长距离的传输到达地面时非常的微弱，受到建筑物的遮挡和多径效应等的影响，GNSS定位精度急剧降低甚至无法使用，无法满足室内位置服务需求。目前，针对复杂的室内环境，国内外研究者们提出了基于蓝牙(BLE)、无线网(WLAN)、超宽带(UWB)、红外线、射频技术(RFID)、超声波，激光、视觉SLAM等室内定位技术及应用系统，但通常都处于成本和精度两难的境地，且易受到环境中非视距误差(NLOS)和多径效应的影响，导致在复杂的室内环境中的精度急剧下降。

因此，针对无线电定位中在室内由多径效应和非视距误差引起的精度下降的问题，提出胸牌式消费级IMU行人导航方法。

发明内容

本发明旨在提供胸牌式消费级IMU行人导航方法，以解决或改善上述现有无线电定位在室内复杂环境的多径效应和非视距误差的问题。

有鉴于此，本发明的第一方面在于提供胸牌式消费级IMU行人导航方法。

本发明的第一方面提供了胸牌式消费级IMU行人导航方法，通过陀螺仪和加速度计获取行人的角速度和加速度进行导航，所述导航方法包括如下步骤：S1：获取行人的当前角速度和加速度，基于预先设定的判断阈值，判断行人是否处于静态，若是则进行S2，若否则进行S3；S2：获取行人在当前时刻的运动参数，根据行人处于静态下的角速度和加速度分别对所述陀螺仪和加速度计进行修正，对行人的速度清零并进行S5；S3：获取修正后的陀螺仪测量的角速度，并根据所述角速度将修正后的加速度计测量的加速度投影至导航系下，根据导航系下的加速度更新行人在当前时刻的所述运动参数；S4：计算重力加速度与大于行人一个步长周期的一段时间内导航系下的加速度均值之间的偏角，通过所述偏角对所述陀螺仪进行补偿更新；S5：输出运动参数，并根据所述运动参数进行导航，并返回S1。

本发明提供的胸牌式消费级IMU行人导航方法，通过预设的判断阈值对行人所处的状态进行判定，分别为静止和非静止的状态，针对行人步态具有单步的周期性规律，以行人的单步运动周期为前提进行补偿，具体为在行人处于静止下的角速度和加速度分别对陀螺仪和加速度计进行修正，通过单步周期时间(行走过程中走动一步的时间)的比力在导航系下的投影均值与重力加速度的非线性度进行误差的补偿；

相对于传统互补滤波方法，通过提出的改进的互补滤波方法依据行人周期运动规律，有效的滤除比力自身的随机噪声，并通过重力加速度观测加速度计测量加速度投影时的姿态误差，对姿态误差进行补偿，提高对行人导航精度。

另外，根据本发明的实施例提供的技术方案还可以具有如下附加技术特征：

上述任一技术方案中，所述运动参数包括：行人的速度、位移和运动姿态。

上述任一技术方案中，在对行人的速度清零之前，所述S2还包括：根据获取的加速度计算水平姿态角；通过所述水平姿态角计算行人在当前时刻下的四元数；根据所述四元数计算包括有姿态信息的姿态矩阵。

在该技术方案中，由于消费级MEMS惯性器件，其陀螺仪无法敏感到地球自转角速度，因此需要其他方法进行航向角的解算(需要如磁强计等其他器件，在本发明中并未使用，因此在此不进行赘述)。而对于水平姿态角，则可以通过加速度计测量的三轴加速度进行计算，即此时加速度计测量的三轴加速度是由重力加速度投影所得，因此此时就可以通过投影关系得到水平姿态角；

得到当前的姿态角时，就可以根据姿态角信息计算当前的四元数，用于后续相应的导航解算；

在得到四元数后，在以此进行包含姿态信息的姿态矩阵的计算，用于后续相应的导航解算。

上述任一技术方案中，所述的根据所述角速度将修正后的加速度计测量的加速度投影至导航系下的步骤，具体包括：通过陀螺仪测量的角速度计算人体运动的角速度，具体为下述公式：

在该技术方案中，在得到确定的运动角速度后，在通过实际的角速度变化进行积分去更新四元数，在根据更新的四元数去更新姿态矩阵；

由于加速度计测量的加速度实在载体系下的物体加速度，而载体坐标系是运动变化的，不能作为位置变化的参考，因此需要通过姿态矩阵将加速度计测量的加速度投影到固定的导航坐标系下，并在导航坐标系下去除重力加速度的影响得到真实的物体运动加速度。

上述任一技术方案中，所述的根据导航系下的加速度更新行人在当前时刻的所述运动参数的步骤包括：通过导航系下的加速度计算获取行人在当前时刻的速度；根据当前时刻的速度计算获取行人的位移；根据更新后的姿态矩阵计算获取在当前时刻的运动姿态，且采用下述公式计算：

在该技术方案中，得到姿态矩阵后，可以通过下式解算出物体此前的姿态信息，即物体所在的载体坐标系和固定的导航坐标系之间的旋转变换关系。

上述任一技术方案中，所述S4的步骤包括：在导航系下将比力投影均值与重力加速度进行叉乘，以获得行人在非静态下的误差补偿，具体为：

在该技术方案中，由于惯性导航在长时间的工况下其误差会随着时间累积发散，且在运动起始的加速阶段和运动结束前的减速阶段，其加速度变化波动较大，此时加速度的误差占主导因素，因此在这里针对该问题对传统的互补滤波方法进行了改进，即将500ms内加速度计测量的加速度信息在导航系下的投影均值与导航系下的重力加速度进行比较，则这两个向量的不共线程度反映出来由于时间累积的导航误差和加速度波动引起的误差，该误差主要是由姿态的不准确导致的，因此将该非共线形成的角度误差通过一个反馈系数作为一个零偏补偿闭环到解算之中消除姿态误差，而该反馈系数反映的是加速阶段和减速阶段引起的误差影响，针对加减速的误差进行抑制，从而实现行人导航中的误差补偿，提高导航精度；

其中需要将比力在导航系下的投影均值与重力加速度的非共线误差通过反馈系数闭环到导航中，则该反馈系数主要反映出的是模式识别的思想，通过最优化方案判断加减速状态下的误差大小，并以此设计反馈系数大小，从而使得补偿效果达到最佳。

上述任一技术方案中，所述判断阈值包括角速度阈值和加速度阈值，且采用下述规则判断：

在该技术方案中，选取100ms的MEMS陀螺仪和MEMS加速度计的测量值均值，去除一定随机噪声干扰。

上述任一技术方案中，所述陀螺仪的修正为：当行人处于静态时，将陀螺仪测得的三轴角速度的均值转换为在三轴方向上的偏差；将三轴方向上的偏差汇集为行人在当前静止状态下的陀螺仪的三轴陀螺零偏，并在行人下一次非静态的角速度测量中消除三轴陀螺零偏进行修正；以及所述三轴陀螺零偏采用下述公式计算：

在该技术方案中，计算静态过程中输入的有陀螺仪测量的三轴角速度的均值，该均值即为该陀螺仪的三轴常值偏值，该偏值反映的是在静止状态下，物体仍保持着一定的角速度在运动，该非实际运动是我们不希望存在的，因此，在静态状态下，将这部分偏值计算得出，进而在动态时去除这一部分干扰。

上述任一技术方案中，所述加速度计的修正为：当行人处于静态时，获取加速度计测得的三轴加速度；将三轴加速度分别与重力加速度进行对比，以获得在每个轴上的偏差；根据每个轴上的偏差对加速度计的标度进行修正；以及采用下述公式计算：

在该技术方案中，由于加速度计器件本身存在标度误差，测量出的加速度与真实加速度之间存在一个标度误差。而由于物体静态时，仅受到重力加速度影响，因此可以通过静态时加速度计测量的三轴加速度与重力加速度进行比较，消除由标度问题引起的误差。

上述任一技术方案中，所述的根据获取的加速度计算水平姿态角采用下述公式：

本发明与现有技术相比所具有的有益效果：

针对行人步态具有单步的周期性规律，以行人的单步运动周期为前提，在大于单步运动周期内加速度周期变化的运动特性计算比力在导航系下的投影均值与重力加速度的非共线程度进行误差的补偿；相较于传统的足底佩戴式方案，本方法通过胸牌佩戴方式，避免了行走过程中由于足部接触地面的高动态角速度造成的姿态发散问题，并通过改进互补滤波方法抑制姿态误差，进而提高导航精度；

相对于传统互补滤波方法，通过提出的改进的互补滤波方法依据行人周期运动规律，有效的滤除比力自身的随机噪声，同时利用大于一步时间内加速度呈周期变化的特点，通过重力加速度观测由姿态误差导致测量加速度在导航系下的投影误差，即重力加速度和测量加速度在导航系下投影的非共线程度对姿态误差进行补偿，提高对行人的导航定位精度。

根据本发明的实施例的附加方面和优点将在下面的描述部分中变得明显，或通过根据本发明的实施例的实践了解到。

附图说明

本发明的上述和/或附加的方面和优点从结合下面附图对实施例的描述中将变得明显和容易理解，其中：

图1为本发明的行人导航方法流程图。

具体实施方式

为了可以更清楚地理解本发明的上述目的、特征和优点，下面结合附图和具体实施方式对本发明进行进一步的详细描述。需要说明的是，在不冲突的情况下，本申请的实施例及实施例中的特征可以相互组合。

在下面的描述中阐述了很多具体细节以便于充分理解本发明，但是，本发明还可以采用其他不同于在此描述的其他方式来实施，因此，本发明的保护范围并不受下面公开的具体实施例的限制。

请参阅图1，下面描述本发明一些实施例的胸牌式消费级IMU行人导航方法。

本发明第一方面的实施例提出了胸牌式消费级IMU行人导航方法。在本发明的一些实施例中，如图1所示，提供了胸牌式消费级IMU行人导航方法，该胸牌式消费级IMU行人导航方法包括：

行人导航方法主要分为静态和动态两个部分，动态和静态的判断是通过物体当前状态来判断，如静止状态下合加速度信号应当为地球重力加速度，角速度信号根据器件实际噪声参数来进行设置，其具体的参数设计可参考如下。其中静态部分的主要作用是惯性导航中的初始对准和误差修正，动态部分是根据运动过程中，MEMS陀螺仪和MEMS加速度计测量的实际角速度和加速度数据进行对应的姿态角变化和速度、位置变化的计算；

首先需要进行行人是否处于静态进行判断，静动态判断条件：选取100ms的MEMS陀螺仪和MEMS加速度计的测量值均值，去除一定随机噪声干扰，具体设置参数事例如下。

其中f为当前加速度计测量值，ω为陀螺测量值，ε

静态部分：在本发明中静态部分的输入为静态过程中的陀螺仪和加速度计的三轴角速度和加速度均值。

其中mean_f

步骤1-1：计算三轴陀螺仪的零偏。即计算静态过程中输入的有陀螺仪测量的三轴角速度的均值，该均值即为该陀螺仪的三轴常值偏值，该偏值反映的是在静止状态下，物体仍保持着一定的角速度在运动，该非实际运动是不希望存在的，因此，在静态状态下，将这部分偏值计算得出，进而在动态时去除这一部分干扰。

其中gB为计算三轴陀螺的零偏。

步骤1-2：计算加速度计标度。由于加速度计器件本身存在标度误差，测量出的加速度与真实加速度之间存在一个标度误差。而由于物体静态时，仅受到重力加速度影响，因此可以通过静态时加速度计测量的三轴加速度与重力加速度进行比较，消除由标度问题引起的误差。

其中BD_f为计算加速度计标度。

步骤1-3：计算水平姿态角。由于消费级MEMS惯性器件，其陀螺仪无法敏感到地球自转角速度，因此需要其他方法进行航向角的解算。而对于水平姿态角，则可以通过加速度计测量的三轴加速度进行计算，即此时加速度计测量的三轴加速度是由重力加速度投影所得，因此此时就可以通过投影关系得到水平姿态角。其计算公式如下：

其中mean_f

步骤1-4：计算初始四元数。在得到当前的水平姿态角时，假设航向角为

其中Q为四元数，q

步骤1-5：计算初始姿态矩阵。在得到四元数后，在以此进行包含姿态信息的姿态矩阵的计算，用于后续相应的导航解算。

其中，T

步骤1-6：保持当前位置不变，速度清零。由于动态解算时，惯性导航所存在的误差会随着时间而发散，因此，在行人导航中的静态时使其位置保持不动，速度误差清零从而抑制误差发散。

P

其中P为位置，V为速度，上标n表示在导航系下描述，下标i表示当前状态下的位置和速度，下标i-1表示上一时刻下的位置和速度。

动态部分，具体包括：

步骤2-1：角速度计算。由于在运动状态下，保证准确的定位导航其中的关键就是姿态角的准确，而姿态角的核心就是输入的运动角速度需要准确。因此对于运动角速度的计算是极其重要的，而陀螺仪测量的三轴角速度中包含误差，该误差是由器件本身的偏值也就是零偏引起的。同时，由于运动时具有误差的累积，该部分误差可以通过改进的互补滤波方法进行零偏补偿，消除一定的误差累积，进而提升定位导航精度。

其中，gB为静态状态下求得的三轴陀螺零偏，ω为测量角速度，α为本发明反馈补偿对应的等效零偏值，b和n分别表示载体系和导航系。

步骤2-2：四元数、姿态矩阵更新。在得到确定的运动角速度后，在通过实际的角速度变化进行积分去更新四元数，在根据更新的四元数去更新姿态矩阵，该部分四元数和姿态矩阵就包含载体系和导航系的角度变换关系。

其中

步骤2-3：比力投影。由于加速度计测量的加速度实在载体系下的物体加速度，而载体坐标系是运动变化的，不能作为位置变化的参考，因此需要通过姿态矩阵将加速度计测量的加速度投影到固定的导航坐标系下，并在导航坐标系下去除重力加速度的影响得到真实的物体运动加速度。

其中f

步骤2-4：速度更新。在得到物体在导航系下真实的运动加速度后即可通过加速度积分计算出物体的运动速度。

步骤2-5：位置解算。在得到物体的真实运动速度后，即可通过速度积分计算出物体运动的位移。

步骤2-6：姿态解算。在得到姿态矩阵后，可以通过下式解算出物体此前的姿态信息，即物体所在的载体坐标系和固定的导航坐标系之间的旋转变换关系。

步骤2-7：改进的互补滤波方法对误差进行补偿。由于惯性导航在长时间的工况下其误差会随着时间累积发散，且在运动起始的加速阶段和运动结束前的减速阶段，其加速度变化波动较大，此时加速度的误差占主导因素，因此在这里针对该问题对传统的互补滤波方法进行了改进，即将500ms内加速度计测量的加速度信息在导航系下的投影均值与导航系下的重力加速度进行比较，则这两个向量的不共线程度反映出来由于时间累积的导航误差和加速度波动引起的误差，该误差主要是由姿态的不准确导致的，因此将该非共线形成的角度误差通过一个反馈系数作为一个零偏补偿闭环到解算之中消除姿态误差，而该反馈系数反映的是加速阶段和减速阶段引起的误差影响，针对加减速的误差进行抑制，从而实现行人导航中的误差补偿，提高导航精度。具体实现方法如下。

①在导航系下将比力投影均值与重力加速度叉乘；

其中

②将叉乘结果(即误差补偿角)投影到载体系下；

③则陀螺的零偏补偿值如下

其中需要将比力在导航系下的投影均值与重力加速度的非共线误差通过反馈系数闭环到导航中，则该反馈系数主要反映出的是模式识别的思想，通过最优化方案判断加减速状态下的误差大小，并以此设计反馈系数大小，从而使得补偿效果达到最佳。

其中α为本发明反馈补偿对应的等效零偏值，

在本发明的描述中，需要理解的是，术语“纵向”、“横向”、“上”、“下”、“前”、“后”、“左”、“右”、“竖直”、“水平”、“顶”、“底”、“内”、“外”等指示的方位或位置关系为基于附图所示的方位或位置关系，仅是为了便于描述本发明，而不是指示或暗示所指的装置或元件必须具有特定的方位、以特定的方位构造和操作，因此不能理解为对本发明的限制。

以上所述的实施例仅是对本发明的优选方式进行描述，并非对本发明的范围进行限定，在不脱离本发明设计精神的前提下，本领域普通技术人员对本发明的技术方案做出的各种变形和改进，均应落入本发明权利要求书确定的保护范围内。