一种基于助力车的惯性组合导航方向定位方法技术

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

【技术实现步骤摘要】
一种基于助力车的惯性组合导航方向定位方法


[0001]本专利技术涉及一种导航定位领域，尤其涉及一种基于助力车的惯性组合导航方向定位方法。

技术介绍

[0002]惯性导航就是通过惯性器件来计算载体的位置速度和姿态。加速度计是测量载体XYZ方向上的加速度，陀螺仪是用来测载体绕XYZ方向旋转的角速度，对加速度积分就是速度，对速度积分就是位移，对角速度积分就是角度，但我们所说载体的位置速度和姿态一般都是相对于导航坐标系来说的，而对于传感器来说它测量的是载体坐标系下的加速度和角速度，那么就需要进行导航过程中坐标系的转换。
[0003]目前惯性导航系统可以分为平台式导航系统(GINS)和捷联式惯性导航系统(SINS)。捷联式惯性导航系统直接与载体固连，其特点是成本低，体积小，具有较高的可靠性，不会与外界发生任何的光电联系也就是隐蔽性良好。捷联式惯性导航可以自主进行行位推算且不受外界干扰。输出信息具有很高的更新率，通常可以达到上百赫兹，并且具有高实时性。目前在航空航天和航海上具有广泛的应用。但缺点就是需要初始对准姿态位置和速度信息，并且由于计算中包含大量的积分运算，会使导航误差随着时间的累计而增大，并且不能收敛。而GNSS长期精度较好，且误差不会随时间累计。但缺点明显，主要有数据更新速率低，短期定位精度较差，容易受外界环境干扰，不能输出姿态信息。
[0004]目前组合导航的应用场景在军用上可应用于航空航天以及航海，民用上可应用车载导航，自动驾驶。GNSS/INS组合导航系统，能通过一定的滤波技术，将GNSS信息和INS信息有效的进行融合，实现两者优势互补，弥补各自的缺点，充分发挥INS的短期高精度性能和GNSS的长期高精度性能。提供高更新速率，高精度，高抗干扰性的导航信息。而实现两种导航信息的融合一般采用卡尔曼滤波的最优估计方法。
[0005]例如，一种在中国专利文献上公开的“一种基于MEMS惯性组件的INS/DR&GNSS松组合导航方法”，其公告号CN111156994A，包括：以INS/DR系统的位置误差、失准角、陀螺零偏、以及里程计的尺度因子为状态向量来建立状态微分方程；以GNSS系统的位置信息和INS/DR系统的位置信息建立观测方程；以所述状态微分方程与观测方程进入kalman滤波器迭代过程计算出INS/DR系统的状态向量；利用所述状态向量对INS/DR系统的位置、速度、姿态角和陀螺零偏误差进行校正，得出校正后的位置、速度和姿态信息。
[0006]但该方案获取方向的准确性不高，而且车辆需要达到一定的速度才能获取到方向，且在转弯时，误差非常大，单纯采用六轴无法通过GPS进行初始方向的校准。当速度太小时，小于定位精度，此时的航向信息是不准的；助力车和汽车不同，汽车转弯一般半径都比较大，转弯计算的航向信息相对准确，而助力车转弯半径较小，速度较小，两定位点连线和真实航向相差较大。

技术实现思路

[0007]本专利技术主要解决现有技术单纯GNSS在助力车上无法提供连续稳定的航向信息的问题；提供一种基于助力车的惯性组合导航方向定位方法。
[0008]本专利技术的上述技术问题主要是通过下述技术方案得以解决的：一种基于助力车的惯性组合导航方向定位方法，包括以下步骤：S1：选择载体和导航坐标系，定义姿态航向角后对传感器自检粗对准，根据姿态航向角对姿态四元数和姿态矩阵进行初始化；S2：姿态更新，设置姿态更新周期，根据前一时刻的姿态四元数和当前时刻陀螺输出的角速度计算当前时刻的姿态四元数；通过当前时刻的姿态四元数得到载体当前时刻的姿态航向角；S3：对描述系统动态特性的连续微分方程离散化后使用卡尔曼滤波对系统的状态向量进行估计；S4：根据速度、卫星数和定位精度信息综合判断，剔除错误信息；进行陀螺仪温度漂移补偿后输出传感信息。
[0009]本方案采用误差估计的方式，通过GNSS和传感器计算的结果的误差值估计六轴传感器的误差进行补偿，从而实现惯性组合导航，得到准确的导航数据，能为助力车提供连续稳定的航向信息。增加了温度漂移、错误数据剔除，提高了系统的准确度。
[0010]作为优选，所述的传感器自检粗对准得到的俯仰角θ和横滚角γ如下：作为优选，所述的传感器自检粗对准得到的俯仰角θ和横滚角γ如下：其中，设加速度计输出为f
b
＝[f
x f
y f
z
]T
；f
x
、f
x
和f
x
分别为加速度计在X轴、Y轴和Z轴上的输出；重力加速度大小为g；航向角的初始值为0。
[0011]考虑到MEMS陀螺精度低，无法感测地球自转角速率，在粗对准的过程中不使用陀螺的测量值。根据加速度计的输出计算得到了载体的初始俯仰角和横滚角，同时给定航向角的初始值为0度。
[0012]作为优选，根据俯仰角θ、横滚角γ和航向角得到姿态四元数Q＝[q
0 q
1 q
2 q3]T
，计算公式如下：
根据四元数Q＝[q
0 q
1 q
2 q3]T
计算得到的姿态矩阵为：其中，q0、q1、q2和q3分别为姿态四元数中的元素。
[0013]粗对准结束后，根据姿态航向角对姿态四元数和姿态矩阵进行初始化。
[0014]作为优选，当前时刻的姿态四元数Q
k
的计算过程为：1)根据陀螺输出的角速率和姿态更新周期Δt计算角增量Δθ，公式如下：2)计算角增量Δθ的模值|Δθ|。
[0015]3)根据角增量Δθ计算中间变量ΔΘ。
[0016]4)计算姿态四元数Q
k
，计算公式如下：其中，I为4维单位矩阵；Q
k
‑1为前一时刻的姿态四元数。
[0017]姿态四元数Q＝[q
0 q
1 q
2 q3]T
应该是规范化四元数，即||Q||＝1。
[0018]作为优选，姿态更新后，对四元数做规范化处理：其中，i＝0，1，2，3；在完成姿态四元数Q
k
更新并规范化处理之后，计算当前时刻的姿态矩阵依据
姿态矩阵与俯仰角θ、横滚角γ和航向角之间的关系，得到载体当前时刻的姿态航向角：其中，T
00
、T
01
、T
02
、T
10
、T
11
、T
12
、T
20
、T
21
和T
22
对应姿态矩阵和的元素。
[0019]由于计算误差等因素，计算过程中的四元数会逐渐失去规范化特性，因此，姿态更新后，对四元数做规范化处理。
[0020]作为优选，所述的卡尔曼滤波的具体过程为：(1)滤波中变量初始化，将被估计的状态向量X
k
的初始值设置为0；(2)根据第k
‑
1时刻的状态预测第k时刻的状态；其中，为状态向量前一时刻的估计值；为状态一本文档来自技高网...

【技术保护点】

【技术特征摘要】
1.一种基于助力车的惯性组合导航方向定位方法，其特征在于，包括以下步骤：S1：选择载体和导航坐标系，定义姿态航向角后对传感器自检粗对准，根据姿态航向角对姿态四元数和姿态矩阵进行初始化；S2：姿态更新，设置姿态更新周期，根据前一时刻的姿态四元数和当前时刻陀螺输出的角速度计算当前时刻的姿态四元数；通过当前时刻的姿态四元数得到载体当前时刻的姿态航向角；S3：对描述系统动态特性的连续微分方程离散化后使用卡尔曼滤波对系统的状态向量进行估计；S4：根据速度、卫星数和定位精度信息综合判断，剔除错误信息；进行陀螺仪温度漂移补偿后输出传感信息。2.根据权利要求1所述的一种基于助力车的惯性组合导航方向定位方法，其特征在于，所述的传感器自检粗对准得到的俯仰角θ和横滚角γ如下：所述的传感器自检粗对准得到的俯仰角θ和横滚角γ如下：其中，设加速度计输出为f
b
＝[f
x f
y f
z
]
T
；f
x
、f
x
和f
x
分别为加速度计在X轴、Y轴和Z轴上的输出；重力加速度大小为g；航向角的初始值为0。3.根据权利要求1或2所述的一种基于助力车的惯性组合导航方向定位方法，其特征在于，根据俯仰角θ、横滚角γ和航向角得到姿态四元数Q＝[q
0 q
1 q
2 q3]
T
，计算公式如下：根据四元数Q＝[q
0 q
1 q
2 q3]
T
计算得到的姿态矩阵为：其中，q0、q1、q2和q3分别为姿态四元数中的元素。4.根据权利要求3所述的一种基于助力车的惯性组合导航方向定位方法，其特征在于，当前时刻的姿态四元数Q
k
的计算过程为：
1)根据陀螺输出的角速率和姿态更新周期Δt计算角增量Δθ，公式如下：2)计算角增量Δθ的模值|Δθ|；3)根据角增量Δθ计算中间变量ΔΘ；4)计算姿态四元数Q
k
，计算公式如下：其中，I为4维单位矩阵；Q
k
‑1为前一时刻的姿态四元数。5.根据权利要求4所述的一种基于助力车的惯性组合导航方向定位方法，其特征在于，姿态更新后，对四元数做规范化处理：其中，i＝0，1，2，3；在完成姿态四元数Q
k
更新并规范化处理之后，计算当前时刻的姿态矩阵依据姿态矩阵与俯仰角θ、横滚角γ和航向角之间的关系，得到载体当前时刻的姿态航向角：其中，T
00
、T
01
、T
02
、T
10
、T
11
、T
12
、T
20
、T
21

【专利技术属性】
技术研发人员：赵鑫，邹建敏，李鹏飞，王明坤，王宽，
申请(专利权)人：杭州金通科技集团股份有限公司，
类型：发明
国别省市：