CN114526731A - 一种基于助力车的惯性组合导航方向定位方法 - Google Patents

Abstract

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

Description

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

技术领域

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

背景技术

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

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

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

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

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

发明内容

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

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

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

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

S2：姿态更新，设置姿态更新周期，根据前一时刻的姿态四元数和当前时刻陀螺输出的角速度计算当前时刻的姿态四元数；通过当前时刻的姿态四元数得到载体当前时刻的姿态航向角；

S3：对描述系统动态特性的连续微分方程离散化后使用卡尔曼滤波对系统的状态向量进行估计；

S4：根据速度、卫星数和定位精度信息综合判断，剔除错误信息；进行陀螺仪温度漂移补偿后输出传感信息。

本方案采用误差估计的方式，通过GNSS和传感器计算的结果的误差值估计六轴传感器的误差进行补偿，从而实现惯性组合导航，得到准确的导航数据，能为助力车提供连续稳定的航向信息。增加了温度漂移、错误数据剔除，提高了系统的准确度。

作为优选，所述的传感器自检粗对准得到的俯仰角θ和横滚角γ如下：

其中，设加速度计输出为f^b＝[f_x f_y f_z]^T；

f_x、f_x和f_x分别为加速度计在X轴、Y轴和Z轴上的输出；

重力加速度大小为g；

航向角

的初始值为0。

考虑到MEMS陀螺精度低，无法感测地球自转角速率，在粗对准的过程中不使用陀螺的测量值。根据加速度计的输出计算得到了载体的初始俯仰角和横滚角，同时给定航向角的初始值为0度。

作为优选，根据俯仰角θ、横滚角γ和航向角

得到姿态四元数Q＝[q₀ q₁ q₂ q₃]^T，计算公式如下：

根据四元数Q＝[q₀ q₁ q₂ q₃]^T计算得到的姿态矩阵

为：

其中，q₀、q₁、q₂和q₃分别为姿态四元数中的元素。

粗对准结束后，根据姿态航向角对姿态四元数和姿态矩阵进行初始化。

作为优选，当前时刻的姿态四元数Q_k的计算过程为：

1)根据陀螺输出的角速率

和姿态更新周期Δt计算角增量Δθ，公式如下：

2)计算角增量Δθ的模值|Δθ|。

3)根据角增量Δθ计算中间变量ΔΘ。

4)计算姿态四元数Q_k，计算公式如下：

其中，I为4维单位矩阵；

Q_k-1为前一时刻的姿态四元数。

姿态四元数Q＝[q₀ q₁ q₂ q₃]^T应该是规范化四元数，即||Q||＝1。

作为优选，姿态更新后，对四元数做规范化处理：

其中，i＝0，1，2，3；

在完成姿态四元数Q_k更新并规范化处理之后，计算当前时刻的姿态矩阵

依据姿态矩阵

与俯仰角θ、横滚角γ和航向角

之间的关系，得到载体当前时刻的姿态航向角：

其中，T₀₀、T₀₁、T₀₂、T₁₀、T₁₁、T₁₂、T₂₀、T₂₁和T₂₂对应姿态矩阵

和的元素。

由于计算误差等因素，计算过程中的四元数会逐渐失去规范化特性，因此，姿态更新后，对四元数做规范化处理。

作为优选，所述的卡尔曼滤波的具体过程为：

(1)滤波中变量初始化，将被估计的状态向量X_k的初始值设置为0；

(2)根据第k-1时刻的状态预测第k时刻的状态；

其中，

为状态向量前一时刻的估计值；

为状态一步转移矩阵；

为状态一步预测估计值；

(3)根据第k-1时刻的系统预测误差估计第k时刻的系统预测误差；

(4)计算卡尔曼滤波增益K_k；

(5)计算系统当前时刻最优估计值

。状态估计过程为：

其中，Z_k为量测向量；

H_k为转换矩阵；

(6)计算当前时刻的系统预测误差：

根据预测误差判断滤波是否结束，若是，则结束；否则，返回步骤(2)。

作为优选，所述的量测向量Z_k的获取过程为：

将加速度计测量输出的比力f^b＝[f_x f_y f_z]^T投影到导航坐标系，得到

卡尔曼滤波选取的量测向量Z_k为fⁿ的前两维，即

描述量测向量Z_k与状态向量X_k之间关系的量测方程为：

其中，g为重力加速度的大小；

T₀₀、T₀₁、T₀₂、T₁₀、T₁₁和T₁₂为姿态矩阵

中前两行的元素；

V_k为量测噪声序列，其均值为0。

实际滤波计算过程中选取fⁿ在滤波周期T内的均值作为量测向量，从而降低测量噪声干扰的影响。

作为优选，离散化后的状态一步转移矩阵

为：

其中，

为姿态矩阵；

I为四维单位阵；

T为卡尔曼滤波周期。

在使用卡尔曼滤波基本方程对系统的状态向量进行估计前，必须对连续型微分方程进行离散化处理。

作为优选，当GNSS信号弱时，通过可用卫星数和定位精度信息进行判断，当定位精度因子小于设定的精度阈值，且可用卫星数大于设定的数量阈值时，认为GNSS提供的信息是可信的；

当车辆转弯的角速度大于设定的角速度阈值，并且航向信息在一定时间内的变化值大于额定变化阈值时，则判定车辆发生了转弯，GNSS航向信息不可信。

错误信息主要是GNSS带来的。单纯的GNSS在一定情况下不能提供准确的航向信息，当设备没有移动时，位置会有2m左右的误差，也就是实际测量的定位点不可能一成不变，会定位到一个范围内，此时GNSS无法提供正确的航向信息，此时航向信息可能是0，也可能是保留上一次测量的航向信息，甚至是随机跳动的值，那么对此情况需要进行甄别，当速度超过一定值之后，航向信息会变得准确。

作为优选，所述的陀螺仪温度漂移补偿过程为：

实时采集陀螺仪传感器的温度信息以及零偏信息，通过采集的信息实时拟合温度特性曲线，得到陀螺仪传感器相对于温度的自动标定，从而输出陀螺仪传感器信息。

本发明的有益效果是：

1.采用误差估计的方式，通过GNSS和传感器计算的结果的误差值估计六轴传感器的误差进行补偿，从而实现惯性组合导航，得到准确的导航数据，能为助力车提供连续稳定的航向信息。

2.增加了温度漂移、错误数据剔除，提高了系统的准确度。

3.自适应卡尔曼滤波算法，无需进行人为校准。

附图说明

图1是本发明的基于助力车的惯性组合导航方向定位方法流程图。

图2是本实施例的组合导航系统整体流程图。

具体实施方式

下面通过实施例，并结合附图，对本发明的技术方案作进一步具体的说明。

实施例：

本实施例的一种基于助力车的惯性组合导航方向定位方法，如图1所示，包括以下步骤：

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

在本实施例中，坐标系的选择如下：

载体坐标系(b系)：右前上；即载体的X轴指向右，Y轴指向前，Z轴指向上。

导航坐标系(n系)：东北天；即在地球表面的切平面上地理的X轴指向东，Y轴指向真北，Z轴垂直于地球表面向上。

姿态航向角定义的姿态航向角方向及值域如下：

导航解算前首先进行粗对准，获得载体的初始姿态角，即俯仰角和横滚角。

粗对准原理：忽略晃动干扰的影响，将陀螺的输出近似看作对地球旋转角速度的测量值，将加速度计的输出近似看作对重力加速度的测量值。根据陀螺和加速度计的输出直接解算出姿态角或姿态矩阵。

考虑到MEMS陀螺精度低，无法感测地球自转角速率，在粗对准的过程中不使用陀螺的测量值。根据加速度计的输出计算得到了载体的初始俯仰角和横滚角，同时给定航向角的初始值为0度。

设加速度计输出为f^b＝[f_x f_y f_z]^T，其中，f_x、f_x和f_x分别为加速度计在X轴、Y轴和Z轴上的输出；重力加速度大小为g，则粗对准得到的俯仰角θ和横滚角γ如下：

具体解算过程分析如下：

粗对准过程中要求载体处于水平静止状态先进行传感器自检。自检通过后，在主函数中将粗对准时间设定为1s；在中断处理函数中，读取加速度计输出，单位是m/s²，在对准时间内将加速度计的输出进行累加求和得到，当计数达到1s，求加速度计输出的平均值，并使用平均值计算初始俯仰角和横滚角；将初始航向角置H0，H0为设定初始航向，程序中为0。

粗对准结束后，根据姿态航向角对姿态四元数和姿态矩阵进行初始化。

根据俯仰角θ、横滚角γ和航向角

得到姿态四元数Q＝[q₀ q₁ q₂ q₃]^T，计算公式如下：

四元数可以看作一个有三个虚部的虚数，不管载体现在是什么姿态，总存在一个轴，使得刚体绕着这个轴转一个角度后就可以到达另一个指定的姿态，而四元数描述的就是这种转动。

根据姿态四元数Q＝[q₀ q₁ q₂ q₃]^T计算得到的姿态矩阵

为：

其中，q₀、q₁、q₂和q₃分别为姿态四元数中的元素。这里四元数的表示方式都用矩阵式表示，其复数形式为Q＝q₀+q₁i+q₂j+q₃k。

S2：姿态更新，设置姿态更新周期，根据前一时刻的姿态四元数和当前时刻陀螺输出的角速度计算当前时刻的姿态四元数；通过当前时刻的姿态四元数得到载体当前时刻的姿态航向角。

设姿态更新周期为Δt，根据前一时刻的姿态四元数Q_k-1，当前时刻陀螺输出的角速率

计算当前时刻的姿态四元数Q_k。

在本实施例中，采用的四元数更新算法为毕卡算法，具体步骤如下：

1.根据陀螺输出的角速率

和姿态更新周期Δt计算角增量Δθ，公式如下：

2.计算角增量Δθ的模值|Δθ|。

3.根据角增量Δθ计算中间变量ΔΘ。

4.计算姿态四元数Q_k，计算公式如下：

其中，I为4维单位矩阵。

姿态四元数Q＝[q₀ q₁ q₂ q₃]^T应该是规范化四元数，即||Q||＝1。由于计算误差等因素，计算过程中的四元数会逐渐失去规范化特性，因此，姿态更新后，对四元数做规范化处理。

其中，i＝0，1，2，3。

在完成姿态四元数Q_k更新并规范化处理之后，根据公式计算当前时刻的姿态矩阵

依据姿态矩阵

与俯仰角θ、横滚角γ和航向角

之间的关系，可以得到载体当前时刻的姿态航向角：

其中，T₀₀、T₀₁、T₀₂、T₁₀、T₁₁、T₁₂、T₂₀、T₂₁和T₂₂对应姿态矩阵

中的元素。

当粗对准结束后，系统转入姿态更新状态，考虑到陀螺精度的限制，仅使用陀螺测量的角速率完成姿态更新无法达到精度要求，故使用加速度计的输出信息构造量测向量，对姿态误差角进行滤波估计并补偿，以提高姿态解算精度。

S3：对描述系统动态特性的连续微分方程离散化后使用卡尔曼滤波对系统的状态向量进行估计。

卡尔曼滤波选取的状态向量X(t)为9维，依次为姿态误差角

陀螺常值漂移ε^b和加速度计常值零偏

ε^b＝[ε_x ε_y ε_z]^T

其中，

表示俯仰角误差；

表示横滚角误差；

表示航向角误差；

ε_x、ε_y和ε_z分别表示x、y和z轴上的陀螺常值漂移；

和

分别表示x、y和z轴上的加速度计常值零偏。

状态向量X(t)即为：

描述系统动态特性的连续微分方程为：

其中，

G(t)为t时刻的一个噪声权重；

w(t)为t时刻预测的高斯噪声，其均值为0。

在使用卡尔曼滤波基本方程对系统的状态向量进行估计前，必须对连续型微分方程进行离散化处理。设卡尔曼滤波周期为T，当滤波周期较短时，F(t)可以近似看作常值矩阵，得到离散化后的状态一步转移矩阵

为：

将加速度计测量输出的比力f^b＝[f_x f_y f_z]^T投影到导航坐标系，得到

卡尔曼选取的量测向量Z_k为fⁿ的前两维，即

实际滤波计算过程中选取fⁿ在滤波周期T内的均值作为量测向量，从而降低测量噪声干扰的影响。

描述量测向量Z_k与状态向量X_k之间关系的量测方程为：

其中，g为重力加速度的大小；

T₀₀、T₀₁、T₀₂、T₁₀、T₁₁和T₁₂为姿态矩阵

中前两行的元素。

V_k为量测噪声序列，其均值为0，量测噪声序列的方差阵为R_k，滤波过程中R_k应根据噪声统计特性进行设定。

实际滤波解算过程中将量测噪声序列方差阵R_k设定为对角阵，对角线元素的值根据给定的方差值

对于不同状态下R₀值根据实际情况调整，量测向

的大小和设定的权重ω进行调整，与自适应滤波类似，即

执行卡尔曼滤波的具体过程为：

(1)滤波中变量初始化，将被估计的状态向量X_k的初始值设置为0。

(2)根据第k-1时刻的状态预测第k时刻的状态。状态一步预测方程为：

其中，

为状态向量前一时刻的估计值；

为状态一步转移矩阵；

为状态一步预测估计值。

(3)根据第k-1时刻的系统预测误差估计第k时刻的系统预测误差。一步预测均方误差阵为：

其中：

P_k/k-1为一步预测均方误差阵；

P_k-1为前一时刻状态估计的均方误差阵；

Q_k-1为系统噪声方差阵；

(4)计算卡尔曼滤波增益。计算滤波增益阵为：

其中，H_k为转换矩阵，它将状态向量映射到测量值所在的空间中。

(5)计算系统最优估计值。状态估计过程为：

(6)计算当前时刻的系统预测误差：

判断滤波是否结束，若是，则结束；否则，返回步骤(2)。

自适应卡尔曼滤波算法，无需进行人为校准。校准主要包括传感器的零偏校准和初始角度对准，每个设备存储的参数都是不一样的，都是经过卡尔曼滤波估计以及采集数据的不断累积，达到校准数据的自适应效果。

S4：根据速度、卫星数和定位精度信息综合判断，剔除错误信息；进行陀螺仪温度漂移补偿后输出传感信息。

错误信息主要是GNSS带来的。单纯的GNSS在一定情况下不能提供准确的航向信息，当设备没有移动时，位置会有2m左右的误差，也就是实际测量的定位点不可能一成不变，会定位到一个范围内，此时GNSS无法提供正确的航向信息，此时航向信息可能是0，也可能是保留上一次测量的航向信息，甚至是随机跳动的值，那么对此情况需要进行甄别，当速度超过一定值之后，航向信息会变得准确。

当GNSS信号弱，比如进隧道的时候由于定位点测量不准确，也会导致航向信息甚至是速度信息变得不准确，此时可以通过可用卫星数和定位精度信息进行判断，当定位精度因子小于某个值，用卫星数大于某个值时，认为GNSS提供的信息是可信的。

转弯情况时，虽然速度、可用卫星数和定位精度都可用，但此时的航向信息确实不可用的，可以通过惯性传感器测量出来，当车辆转弯的角速度大于某个值，并且观测测量的航向信息在一定时间内的变化值大于某个值时，则可以判定车辆发生了转弯，此时GNSS提供的航向可能不准确。

判断出GNSS提供的航向信息何时不准确之后，就可以根据实际情况，有选择的让GNSS测量的航向信息参与滤波运算。

精巧灵活的工作模式切换，工作模式切换是自主和组合的模式切换，就是速度低或者GNSS没有信号的时候会自动切换到自主模式也就是惯导模式。GNSS可用时自动切换成组合模式，在速度较低、卫星数量较少的情况下，自动切换为惯性导航，在转弯时，自动将GPS导航切换为惯性导航，当卫星数量达到一定程度，自动切换为GPS导航，结合卡尔曼滤波进行车辆方向的预测。

MEMS传感器输出值受温度影响较大，此时需要进行温度补偿，最常用的方法是进行温度标定，但此方法太过繁琐，不适合产品的批量生产。

在本实施例的助力车运行过程中，实时采集传感器的温度信息以及零偏信息，通过采集的信息实时拟合温度特性曲线，得到传感器相对于温度的自动标定。从而输出可靠的传感器信息。

精简实用的误差模型，系统增加了温度漂移、错误数据剔除，提高了系统的准确度。

陀螺仪传感器也就是角速度传感器，忽略地球自转带来的影响，静止时三个轴向的输出理论值为0，但是测量发现不同温度下传感器的会产生漂移，也就是温度漂移。本实施例的方案主要通过两个方面进行补偿。第一是通过采集大量的温度和传感器数据进行温度角速度曲线拟合，第二是通过卡尔曼滤波来进行误差估计。

如图2所示的本实施例的组合导航系统整体流程图。

通过GNSS获取的位置、速度和航向信息，结合温度传感器数据计算后通过卡尔曼滤波器获得误差补偿矫正，与补偿后的温度传感器数据通过惯性导航算法计算，得到准确的导航数据。

采用误差估计的方式，六轴传感器为主，通过GNSS和传感器计算的结果的误差值估计六轴传感器的误差进行补偿，从而实现惯性组合导航，得到准确的导航数据。

应理解，实施例仅用于说明本发明而不用于限制本发明的范围。此外应理解，在阅读了本发明讲授的内容之后，本领域技术人员可以对本发明作各种改动或修改，这些等价形式同样落于本申请所附权利要求书所限定的范围。

Claims (10)

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₀ q₁ q₂ q₃]^T，计算公式如下：

根据四元数Q＝[q₀ q₁ q₂ q₃]^T计算得到的姿态矩阵

为：

其中，q₀、q₁、q₂和q₃分别为姿态四元数中的元素。

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₀₀、T₀₁、T₀₂、T₁₀、T₁₁、T₁₂、T₂₀、T₂₁和T₂₂对应姿态矩阵

中的元素。

6.根据权利要求1所述的一种基于助力车的惯性组合导航方向定位方法，其特征在于，所述的卡尔曼滤波的具体过程为：

(1)滤波中变量初始化，将被估计的状态向量X_k的初始值设置为0；

(2)根据第k-1时刻的状态预测第k时刻的状态；

其中，

为状态向量前一时刻的估计值；

为状态一步转移矩阵；

为状态一步预测估计值；

(3)根据第k-1时刻的系统预测误差估计第k时刻的系统预测误差；

(4)计算卡尔曼滤波增益K_k；

(5)计算系统当前时刻最优估计值

状态估计过程为：

其中，Z_k为量测向量；

H_k为转换矩阵；

(6)计算当前时刻的系统预测误差：

根据预测误差判断滤波是否结束，若是，则结束；否则，返回步骤(2)。

7.根据权利要求6所述的一种基于助力车的惯性组合导航方向定位方法，其特征在于，所述的量测向量Z_k的获取过程为：

将加速度计测量输出的比力f^b＝[f_x f_y f_z]^T投影到导航坐标系，得到

卡尔曼滤波选取的量测向量Z_k为fⁿ的前两维，即

描述量测向量Z_k与状态向量X_k之间关系的量测方程为：

其中，g为重力加速度的大小；

T₀₀、T₀₁、T₀₂、T₁₀、T₁₁和T₁₂为姿态矩阵

中前两行的元素；

V_k为量测噪声序列，其均值为0。

8.根据权利要求6所述的一种基于助力车的惯性组合导航方向定位方法，其特征在于，离散化后的状态一步转移矩阵

为：

其中，

为姿态矩阵；

I为四维单位阵；

T为卡尔曼滤波周期。

9.根据权利要求1所述的一种基于助力车的惯性组合导航方向定位方法，其特征在于，当GNSS信号弱时，通过可用卫星数和定位精度信息进行判断，当定位精度因子小于设定的精度阈值，且可用卫星数大于设定的数量阈值时，认为GNSS提供的信息是可信的；

当车辆转弯的角速度大于设定的角速度阈值，并且航向信息在一定时间内的变化值大于额定变化阈值时，则判定车辆发生了转弯，GNSS航向信息不可信。

10.根据权利要求1或9所述的一种基于助力车的惯性组合导航方向定位方法，其特征在于，所述的陀螺仪温度漂移补偿过程为：

实时采集陀螺仪传感器的温度信息以及零偏信息，通过采集的信息实时拟合温度特性曲线，得到陀螺仪传感器相对于温度的自动标定，从而输出陀螺仪传感器信息。

Images