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

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

背景技术：

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

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

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

技术实现思路

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

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

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

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

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

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

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

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

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

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

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

12、上述任一技术方案中，所述的根据所述角速度将修正后的加速度计测量的加速度投影至导航系下的步骤，具体包括：通过陀螺仪测量的角速度计算人体运动的角速度，具体为下述公式：通过人体运动的所述角速度对行人在当前时刻下的四元数进行更新，具体为下述公式：通过更新后的四元数计算并更新姿态矩阵根据更新后的姿态矩阵作为投影系数将所述加速度投影至导航系下，具体为下述公式：其中，为人体运动的角速度、gb为静态状态下求得的三轴陀螺零偏、ω为测量角速度、α为修正的补偿更新值、b为载体系、n为导航系、为三轴角速度形成的反对称矩阵、qi为第i时刻的四元数、dt为导航时间、fn为导航系下加速度、gn为重力加速度、为姿态矩阵，且

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

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

15、上述任一技术方案中，所述的根据导航系下的加速度更新行人在当前时刻的所述运动参数的步骤包括：通过导航系下的加速度计算获取行人在当前时刻的速度；根据当前时刻的速度计算获取行人的位移；根据更新后的姿态矩阵计算获取在当前时刻的运动姿态，且采用下述公式计算：其中，为航向角、θ为俯仰角、γ为横滚角、是通过姿态矩阵计算的航向角主值、θ主是通过姿态矩阵计算的俯仰角主值、γ主是通过姿态矩阵计算的横滚角主值、所述运动姿态包括：θ和γ。

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

17、上述任一技术方案中，所述s4的步骤包括：在导航系下将比力投影均值与重力加速度进行叉乘，以获得行人在非静态下的误差补偿，具体为：将所述误差补偿通过当前时刻的姿态矩阵投影至载体系下，具体为：通过载体系下的误差补偿计算获取陀螺仪的修正的补偿更新值α，具体为：其中，s1为500ms的数据量个数、eb为载体系下的补偿向量、为俯仰角补偿值、为横滚角补偿值、为航向角补偿值、为比力在导航系下一段时间t内投影的均值、en为非共线误差、为反馈系数其算例为k＝[0.00001 0.00001 0.00001]。

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

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

20、上述任一技术方案中，所述判断阈值包括角速度阈值和加速度阈值，且采用下述规则判断：其中，j属于[1，s2]、fj为当前加速度计测量值、g为重力加速度、ωj为陀螺测量值、εacc为判断是否为静态的加速度条件算例为0.5m/s2、εgyro为判断是否为静态的角速度速度条件算例为1.5°/s。

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

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

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

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

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

26、上述任一技术方案中，所述的根据获取的加速度计算水平姿态角采用下述公式：其中，mean_fx,mean_fy,mean_fz分别为当前三轴测量加速度均值。

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

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

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

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