[发明专利]一种基于助力车的惯性组合导航方向定位方法

说明书

本发明公开了一种基于助力车的惯性组合导航方向定位方法。为了克服现有的导航方法无法为助力车提供连续稳定航向信息的问题本发明包括：选择载体和导航坐标系，定义姿态航向角后对传感器自检粗对准，根据姿态航向角对姿态四元数和姿态矩阵进行初始化；姿态更新，设置姿态更新周期，根据前一时刻的姿态四元数和当前时刻陀螺输出的角速度计算当前时刻的姿态四元数；通过当前时刻的姿态四元数得到载体当前时刻的姿态航向角；对描述系统动态特性的连续微分方程离散化后使用卡尔曼滤波对系统的状态向量进行估计；根据速度、卫星数和定位精度信息综合判断，剔除错误信息；进行陀螺仪温度漂移补偿后输出传感信息。能够为助力车提供连续稳定航向信息。

技术领域

本发明涉及一种导航定位领域，尤其涉及一种基于助力车的惯性组合导航方向定位方法。

背景技术

惯性导航就是通过惯性器件来计算载体的位置速度和姿态。加速度计是测量载体XYZ方向上的加速度，陀螺仪是用来测载体绕XYZ方向旋转的角速度，对加速度积分就是速度，对速度积分就是位移，对角速度积分就是角度，但我们所说载体的位置速度和姿态一般都是相对于导航坐标系来说的，而对于传感器来说它测量的是载体坐标系下的加速度和角速度，那么就需要进行导航过程中坐标系的转换。

目前惯性导航系统可以分为平台式导航系统(GINS)和捷联式惯性导航系统(SINS)。捷联式惯性导航系统直接与载体固连，其特点是成本低，体积小，具有较高的可靠性，不会与外界发生任何的光电联系也就是隐蔽性良好。捷联式惯性导航可以自主进行行位推算且不受外界干扰。输出信息具有很高的更新率，通常可以达到上百赫兹，并且具有高实时性。目前在航空航天和航海上具有广泛的应用。但缺点就是需要初始对准姿态位置和速度信息，并且由于计算中包含大量的积分运算，会使导航误差随着时间的累计而增大，并且不能收敛。而GNSS长期精度较好，且误差不会随时间累计。但缺点明显，主要有数据更新速率低，短期定位精度较差，容易受外界环境干扰，不能输出姿态信息。

目前组合导航的应用场景在军用上可应用于航空航天以及航海，民用上可应用车载导航，自动驾驶。GNSS/INS组合导航系统，能通过一定的滤波技术，将GNSS信息和INS信息有效的进行融合，实现两者优势互补，弥补各自的缺点，充分发挥INS的短期高精度性能和GNSS的长期高精度性能。提供高更新速率，高精度，高抗干扰性的导航信息。而实现两种导航信息的融合一般采用卡尔曼滤波的最优估计方法。

例如，一种在中国专利文献上公开的“一种基于MEMS惯性组件的INS/DRGNSS松组合导航方法”，其公告号CN111156994A，包括：以INS/DR系统的位置误差、失准角、陀螺零偏、以及里程计的尺度因子为状态向量来建立状态微分方程；以GNSS系统的位置信息和INS/DR系统的位置信息建立观测方程；以所述状态微分方程与观测方程进入kalman滤波器迭代过程计算出INS/DR系统的状态向量；利用所述状态向量对INS/DR系统的位置、速度、姿态角和陀螺零偏误差进行校正，得出校正后的位置、速度和姿态信息。

但该方案获取方向的准确性不高，而且车辆需要达到一定的速度才能获取到方向，且在转弯时，误差非常大，单纯采用六轴无法通过GPS进行初始方向的校准。当速度太小时，小于定位精度，此时的航向信息是不准的；助力车和汽车不同，汽车转弯一般半径都比较大，转弯计算的航向信息相对准确，而助力车转弯半径较小，速度较小，两定位点连线和真实航向相差较大。

发明内容

本发明主要解决现有技术单纯GNSS在助力车上无法提供连续稳定的航向信息的问题；提供一种基于助力车的惯性组合导航方向定位方法。

本发明的上述技术问题主要是通过下述技术方案得以解决的：

一种基于助力车的惯性组合导航方向定位方法，包括以下步骤：

S1：选择载体和导航坐标系，定义姿态航向角后对传感器自检粗对准，根据姿态航向角对姿态四元数和姿态矩阵进行初始化；