CN114935345A - 一种基于模式识别的车载惯导安装角误差补偿方法 - Google Patents

Abstract

本发明提供一种基于模式识别的车载惯导安装角误差补偿方法。首先，将惯性导航设备安装于车上，读取惯性测量单元IMU的六轴数据，将陀螺仪z轴输出值投影在车体系上得到车体横摆角速度，利用IMU数据进行捷联惯导更新，对车辆运动模式进行识别；接着，利用GNSS进行观测融合；最后，通过卡尔曼滤波器对车速进行融合，实时估计安装角误差。与现有方法相比，本方法降低了GNSS/INS组合导航使用门槛，只需将惯导设备固定在车辆上，行驶过程中即可自动完成安装角度补偿；且提供精确、实时的车辆运动状态识别方法，可以针对不同车辆运动状态进行不同观测融合，确保惯性设备安装角估计不会受到错误观测量影响，提高了惯性设备安装角估计的鲁棒性和估计精度。

Description

一种基于模式识别的车载惯导安装角误差补偿方法

技术领域

本发明涉及定位导航领域，具体涉及一种基于模式识别的车载惯导安装角误差补偿方法。

背景技术

在定位导航领域中，全球定位导航系统(Global Navigation Satellite System，GNSS)和惯性导航系统(Inertial Navigation System，INS)具有很强的互补性，两者组合可以取长补短，得到更为精确的定位信息。

组合导航应用在车载环境时，惯性导航设备安装时无法与车身坐标系完全对齐，这会影响最终姿态、速度、位置的推算结果。目前常见的安装角估计方法有加速度矢量法、磁力计观测法等，但发动机抖动、强磁干扰等车载复杂条件会影响现有方法的估计效果，于是本发明提出一种基于车辆模式识别的惯导安装角误差补偿方法。本方法引入控制器局域网络(Controller Area Network,CAN)提供的车速信息，为组合导航提供额外观测。而在转弯、低速等工况下，车速的观测效果并不理想，于是本发明提出在车辆运动状态识别基础上，利用CAN车速和GNSS观测进行惯性设备安装角度估计。

发明内容

为了弥补惯导安装角误差，提高组合导航定位精度，本发明提供一种基于模式识别的车载惯导安装角误差补偿方法。首先，对车辆运动模式进行识别；其次，利用GNSS进行观测融合；最后，在车辆运动模式为高速直行状态下，通过卡尔曼滤波器对车速进行融合，实时估计安装角误差。与现有方法相比，本方法具有更强的鲁棒性和更快的估计速度。

本发明采用以下技术方案：

步骤S1、将惯性导航设备安装于试用车上，进行系统初始化。

步骤S2、实时读取惯性测量单元(Inertial Measurement Unit,IMU)的六轴数据，包括陀螺仪和加速度计的x、y、z轴数据，并对原始数据进行滑动窗口均值预处理。

步骤S3、在车辆静止时，利用IMU的加速度计数据计算惯性导航设备与车体之间的横滚安装角θ_r和俯仰安装角θ_p，另外，认为航向角θ_y为0。将θ_r、θ_p和θ_y通过欧拉角转姿态阵公式计算出惯性导航设备系与车体系之间的转换矩阵

步骤S4、利用

将陀螺仪z轴输出值wbz_b投影在车体系上得到车体横摆角速度wbz_v：

步骤S5、利用IMU数据进行捷联惯导更新，得到更新后的姿态

位置p_IMU和速度v_IMU信息。

步骤S6、当接收到GNSS信号时，通过卡尔曼滤波器进行信息融合，卡尔曼滤波过程包括滤波器初始化、一步预测和量测更新。初始化过程包括误差状态量、系统协方差矩阵、后验协方差矩阵初始化。一步预测由捷联惯导误差传播方程得到，量测更新中以GNSS输出的三维速度与步骤S5中捷联惯导更新后的速度之差、GNSS输出的位置与步骤S5中捷联惯导更新后的位置之差作为观测，最终得到矫正后的误差状态量。再对步骤S5中捷联惯导更新输出的姿态、速度和位置减去卡尔曼估计出的误差状态量，得到更为准确的姿态、速度和位置信息。

所述步骤S6包括以下步骤：

步骤S6-1、卡尔曼滤波器初始化，包括误差状态向量X，系统噪声协方差Q，后验协方差P初始化。

取误差状态向量X为18维向量：

其中，

为导航坐标系下姿态的估计误差，δv_n为导航系下速度的估计误差，δp_n为导航系下位置的估计误差，b^ω和b^a分别为陀螺和加速度计的动态零偏，δk为CAN车速的尺度因子误差，δθ_p为惯导设备安装角的俯仰角误差，δθ_y为惯导设备安装角的航向角误差。

系统协方差矩阵Q为：

Q＝diag([IMU_arw IMU_vrw IMU_gpsd IMU_apsd]²)

其中，IMU_arw和IMU_vrw为陀螺的随机游走系数和加速度计的随机游走系数，IMU_gpsd和IMU_apsd分别为陀螺和加速度计的零偏不稳定性系数。

后验协方差矩阵P为：

P＝diag([IMU_install GNSS_stdv GNSS_stdpIMU_gpsd IMU_apsd IMU_err]²)

其中，IMU_install为安装误差夹角，GNSS_stdv为GNSS速度误差方差，GNSS_stdp为GNSS位置误差方差，IMU_err由安装角误差角和车速尺度因子组成。

步骤S6-2、卡尔曼滤波一步预测，利用捷联惯导误差传播方程进行一步预测，得到先验状态量X_prior。

步骤S6-3、卡尔曼滤波量测更新，构造GNSS观测量Z_gnss、GNSS观测矩阵H_gnss，对先验状态量进行量测更新得到后验状态量X_post。

GNSS观测量Z_gnss：

Z_gnss＝[v_gnss-v_IMU p_gnss-p_IMU]^T

其中，v_gnss为GNSS输出的三维速度，v_IMU为步骤S5中捷联惯导更新输出的三维速度，p_gnss为GNSS输出的位置(包括经纬高信息)，p_IMU步骤S5中捷联惯导更新输出的位置(包括经纬高信息)。

GNSS观测矩阵H_gnss：

其中，O₃是3阶零矩阵，I₃是3阶单位阵，

R_m为子午曲率半径，R_n为横向曲率半径，L为纬度，h为海拔高度。

量测更新后，得到后验误差状态向量X_post：

式中，下标中包含post的变量均为误差状态向量X中相应变量的后验误差。

步骤S6-4、对步骤S5中捷联惯导更新得到的姿态

位置p_IMU和速度v_IMU减去后验状态量X_post中的姿态误差

位置误差δp_{n_post}和速度误差δv_{n_post}。

p_ins＝p_IMU-δp_{n_post}

v_ins＝v_IMU-δv_{n_post}

其中，

p_ins、v_ins分别为组合导航得到的姿态、位置和速度估计。

步骤S7、通过CAN接收车速信息，当接收到车速信息时，利用wbz_v以及车速信息进行车辆基本运动状态识别，运动状态可以分为高速直行状态、转弯状态、停车状态和普通状态。

所述步骤S7包括以下步骤：

步骤S7-1、对wbz_v和CAN车速进行滑动窗口均值处理，得到均值处理后的车体横摆角速度wbz_{v_filtered}和CAN车速v_filtered。

步骤S7-2、当v_filtered大于车速第一阈值，wbz_{v_filtered}小于角速度第一阈值且大于角速度第二阈值，则判定为高速直行状态；当wbz_{v_filtered}大于角速度第一阈值或小于角速度第二阈值，则判定为转弯状态；当v_filtered小于车速第二阈值，则判定为停车状态；其他情况则判定为普通运动状态；

所述的车速第一阈值、车速第二阈值、角速度第一阈值、角速度第二阈值，均根据本领域常用的车辆各运行状态车速、角速度的经验值确定。

步骤S8、在高速直行状态下，CAN车速观测模型选择加入安装角误差的观测模型，通过卡尔曼滤波器进行惯导安装角误差和CAN车速尺度误差估计。而在其他运动状态下，CAN车速观测模型选择普通车速观测模型，通过卡尔曼滤波器对其他状态量进行矫正，而不影响安装角误差和CAN车速尺度因子误差。

所述步骤S8包括以下步骤：

在高速直行运动状态下，选择组合导航的速度估计结果与加入尺度因子的CAN车速的差值作为观测量Z_{CAN_k}，选择加入安装角误差的观测矩阵H_{CAN_install}进行观测融合。

加入尺度因子的CAN车速观测量Z_{CAN_k}：

其中，

v_ins为步骤S6中组合导航速度估计结果，

是一个三维的向量，

分别是其第一维、第二维、第三维；

为组合导航姿态估计结果，

为车身坐标系至惯导设备系的变换矩阵，v_D为CAN车速值，k为车速尺度因子；

因为卡尔曼得到的结果是姿态误差，修正以后得到正确的姿态，表示为欧拉角也就是

对应到旋转矩阵即为

与组合导航姿态估计结果

实质上相同。

加入安装角误差的观测矩阵H_{CAN_install}:

H_{CAN_install}＝[M₁ M₂ O₃ O₃ O₃ M₃]^T

其中，

()_×表示反对称矩阵，O₃是3阶零矩阵。

在其他运动状态下，由于转弯或低速运动状态下GNSS提供的速度观测不准确，导致系统估计的速度v_ins也不准确，故此时不进行惯导设备安装角的估计。选择组合导航的速度估计结果与不加入尺度因子的CAN车速的差值作为观测量Z_CAN，选择不加入安装角误差的观测矩阵H_CAN进行观测融合。

观测量Z_CAN：

观测矩阵H_CAN:

H_CAN＝[M₁ M₂ O₃ O₃ O₃ O₃]^T

步骤S9、利用步骤S8中估计出的CAN车速尺度误差、惯导设备俯仰角安装角误差和惯导设备航向角安装角误差修正当前CAN车速尺度和安装角变换矩阵。

所述步骤S9包括以下步骤：

步骤S9-1、利用估计出的CAN车速尺度误差δk修正上一步的CAN车速尺度因子：

k＝k_(-)+δk

步骤S9-2、利用惯导设备俯仰角安装角误差、惯导设备航向角安装角误差修正上一步的安装角坐标变换矩阵

如以下公式所示：

其中，

即为惯性导航设备系与车体系之间的姿态转换矩阵，即惯性导航设备安装角的矩阵表示形式。步骤S3中得到的

是转换矩阵的初步估计值，而步骤S9中不断对其进行修正，从而使该值越来越接近真实惯性导航设备与车辆之间的安装角。

与现有技术相比，本发明的有益效果为：

1.降低了GNSS/INS组合导航使用门槛，使用者无需关心惯导设备的摆放角度，只需将惯导设备固定在车辆上，行驶过程中即可自动完成安装角度补偿。这受益于本发明提供的CAN车速观测方法，利用始终指向车头正方向的CAN车速即可实时估计惯性设备的安装角度并进行补偿。

2.提高了惯性设备安装角标定的鲁棒性和标定精度，使用者在惯性设备安装角标定过程中无需关心车辆的行驶路线，在任意行驶工况下均可完成安装角补偿。这受益于本发明提供精确、实时的车辆运动状态识别方法，可以针对不同车辆运动状态进行不同观测融合，确保惯性设备安装角估计不会受到错误观测量影响，从而提高了惯性设备安装角估计的鲁棒性和估计精度。

附图说明

图1为本发明实施例车载惯导安装角误差补偿流程图。

图2为本发明实施例中航向角估计误差图。

具体实施方式

为使本发明实施例的目的、技术方案和优点更加清楚明白，下面将以附图及详细叙述清楚说明本发明所揭示内容的精神，任何所属技术领域技术人员在了解本发明内容的实施例后，当可由本发明内容所教示的技术，加以改变及修饰，其并不脱离本发明内容的精神与范围。本发明的示意性实施例及其说明用于解释本发明，但并不作为对本发明的限定。

本实施例中，通过实车验证本发明的可行性和优越性，具体包括如下步骤：

步骤S1、将惯性导航设备安装对于试用车上，惯性导航设备的x轴与车头方向大致平齐。

步骤S2、以100hz的频率读取惯性测量单元(Inertial Measurement Unit,IMU)的六轴数据，包括陀螺仪和加速度计的x、y、z轴数据。本实施例中对原始陀螺仪数据wb_xyz和加速度计速度ab_xyz进行滑动窗口均值预处理：

步骤S3、在车辆静止时，利用IMU的加速度计数据计算惯性导航设备与车体之间的横滚安装角θ_r和俯仰安装角θ_p，另外，认为航向角θ_y为0。将θ_r、θ_p和θ_y通过欧拉角转姿态阵公式计算出惯性导航设备系与车体系之间的姿态转换矩阵

在本实施例中，安装角θ_install为：

θ_install＝[θ_r θ_p θ_y]^T＝[0.021 -0.039 0]^T

步骤S4、利用

将陀螺仪z轴输出值wbz_b投影在车体系上得到wbz_v：

步骤S5、利用捷联惯导算法进行更新，得到更新后的姿态、位置和速度信息。

步骤S6、当接收到GNSS信号时，以GNSS输出的三维速度和经纬度信息作为观测，与上一步得到的姿态、位置和速度通过卡尔曼滤波器进行信息融合，得到更为可信的姿态、速度和位置组合导航结果。

所述步骤S6包括以下步骤：

步骤S6-1、卡尔曼滤波器初始化，包括误差状态向量X，系统噪声协方差Q，后验协方差P初始化。

取误差状态向量X为18维向量：

其中，

为导航坐标系下姿态的估计误差，δv_n为导航系下速度的估计误差，δp_n为导航系下位置的估计误差，b^ω和b^a分别为陀螺和加速度计的动态零偏，δk为CAN车速的尺度因子误差，δθ_p为惯导设备安装角的俯仰角误差，δθ_y为惯导设备安装角的航向角误差。

在本实施例中，系统协方差矩阵Q为：

Q＝diag([0.001I₃,0.003I₃,1×10^-4I₃,0.003I₃]²)

后验协方差矩阵P为：

P＝diag([0.1I₃,I₃,4×10^-7I₃,O₃,O₃,0.1,0.01,0.01]²)

步骤S6-2、利用捷联惯导误差传播方程进行卡尔曼滤波一步预测。

步骤S6-3、卡尔曼滤波量测更新，构造GNSS观测量Z_gnss、GNSS观测矩阵H_gnss。

GNSS观测量Z_gnss：

Z_gnss＝[v_gnss-v_IMU p_gnss-p_IMU]^T

GNSS观测矩阵H_gnss：

其中，O₃是3阶零矩阵，I₃是3阶单位阵，

R_m为子午曲率半径，R_n为横向曲率半径，L为纬度，h为海拔高度。

步骤S6-4、使用卡尔曼滤波器估计出来的误差修正系统状态向量，最终输出定位、姿态估计结果。

步骤S7、以10hz的频率接收CAN车速信息，当接收到车速信息时，利用wbz_v以及车速信息进行车辆基本运动状态识别，运动状态可以分为高速直行状态、转弯状态、停车状态和普通状态。

所述步骤S7包括以下步骤：

步骤S7-1、对wbz_v和CAN车速进行滑动窗口均值处理。

步骤S7-2、当车速大于车速第一阈值，陀螺z轴角速度小于角速度第一阈值且大于角速度第二阈值，则判定为高速直行状态；当陀螺z轴角速度大于角速度第三阈值或小于角速度第四阈值，则判定为转弯状态；当车速小于车速第二阈值，则判定为停车状态；其他情况则判定为普通运动状态。

本实施例中采用以下阈值，车速第一阈值为5m/s，车速第二阈值为0.01m/s；角速度第一阈值为0.05rad/s，角速度第二阈值为-0.05rad/s，角速度第三阈值为0.1rad/s，角速度第三阈值为-0.1rad/s。

步骤S8、在高速直行状态下，CAN车速观测模型选择加入安装角误差的观测模型，通过卡尔曼滤波器进行惯导安装角误差和CAN车速尺度误差估计。而在其他运动状态下，CAN车速观测模型选择普通车速观测模型，通过卡尔曼滤波器对其他状态量进行矫正，而不影响安装角误差和CAN车速尺度误差。

所述步骤S8包括以下步骤：

在高速直行运动状态下，选择组合导航的速度估计结果与CAN车速的差值作为观测量Z，选择加入安装角误差的观测矩阵H进行观测融合。

观测量Z_{CAN_k}：

其中，

v_n为步骤S6中组合导航速度估计结果，

为组合导航姿态估计结果，

为车体系至惯导设备系的变换矩阵，v_D为CAN车速值，k为CAN车速尺度因子。

观测矩阵H_{CAN_install}:

H_{CAN_install}＝[M₁ M₂ O₃ O₃ O₃ M₃]^T

其中，

()_×表示反对称矩阵，O₃是3阶零矩阵。

在其他运动状态下，由于转弯或低速运动状态下GNSS提供的速度观测不准确，导致系统估计的速度v_ins也不准确，故此时不进行惯导设备安装角的估计。选择以下观测量Z、观测矩阵H进行观测融合。

观测量Z_CAN：

观测矩阵H_CAN:

H_CAN＝[M₁ M₂ O₃ O₃ O₃ O₃]^T

步骤S9、利用步骤S7中估计出的CAN车速尺度误差、惯导设备俯仰角安装角误差和惯导设备航向角安装角误差修正当前CAN车速尺度和安装角变换矩阵。

所述步骤S9包括以下步骤：

步骤S9-1、利用估计出的CAN车速尺度误差修正上一步的CAN车速尺度：

k＝k_(-)+δk

步骤S9-2、利用惯导设备俯仰角安装角误差、惯导设备航向角安装角误差修正上一步的安装角坐标变换矩阵

如下式所示：

其中，

即为惯性导航设备系与车体系之间的姿态转换矩阵，即惯性导航设备安装角的矩阵表示形式。随着时间推移，

的估计值会越来越接近真实惯性导航设备与车辆之间的安装角。

图2为本实施例的航向安装角估计误差图，初始航向安装角有30°误差，在算法运行100s内，航向安装角即可收敛至真值附近，误差角度小于3°。相比于利用加速度计和地磁观测计算惯性设备安装角的方法，本发明提供的安装角补偿方法具有更强的鲁棒性和估计精度。

上述实施例为本发明较佳的实施方式，但本发明的实施方式并不受上述实施例的限制，其他的任何未背离本发明的精神实质与原理下所作的改变、修饰、替代、组合、简化，均应为等效的置换方式，都包含在本发明的保护范围之内。

Claims (6)

1.一种基于模式识别的车载惯导安装角误差补偿方法，其特征在于：所述方法包括以下步骤：

步骤S1、将惯性导航设备安装于车上，进行系统初始化；

步骤S2、实时读取惯性测量单元IMU的六轴数据，包括陀螺仪和加速度计的x、y、z轴数据，并对原始数据进行滑动窗口均值预处理；

步骤S3、在车辆静止时，利用IMU的加速度计数据计算惯性导航设备与车体之间的横滚安装角θ_r和俯仰安装角θ_p，另外，认为航向角θ_y为0；将θ_r、θ_p和θ_y通过欧拉角转姿态阵公式计算出惯导设备系与车身坐标系的转换矩阵；

步骤S4、利用惯导设备系与车身坐标系的转换矩阵将陀螺仪z轴输出值投影在车体系上得到车体横摆角速度；

步骤S5、利用IMU数据进行捷联惯导更新，得到更新后的姿态

位置p_IMU和速度v_IMU；

步骤S6、当接收到GNSS信号时，以GNSS输出的三维速度和经纬度信息作为观测，与上一步得到的更新后的姿态、位置和速度通过卡尔曼滤波器进行信息融合，得到组合导航姿态、位置和速度的估计结果；

步骤S7、通过CAN接收车速信息，利用车体横摆角速度以及CAN车速信息进行车辆基本运动状态识别，运动状态分为高速直行状态、转弯状态、停车状态和普通运动状态；

步骤S8、在高速直行状态下，CAN车速观测模型选择加入安装角误差的观测矩阵，通过卡尔曼滤波器进行惯导设备安装角误差和CAN车速尺度误差估计；在其他运动状态下，CAN车速观测模型选择不加入安装角误差的观测矩阵，通过卡尔曼滤波器对其他状态量进行矫正，而不影响安装角误差和CAN车速尺度误差；

步骤S9、利用步骤S8中得到的CAN车速尺度误差、惯导设备俯仰角安装角误差和惯导设备航向角安装角误差修正当前CAN车速尺度和安装角变换矩阵。

2.如权利要求1所述的基于模式识别的车载惯导安装角误差补偿方法，其特征在于，步骤S4中，利用惯导设备系与车身坐标系的转换矩阵

将陀螺仪z轴输出值wbz_b投影在车体系上得到车体横摆角速度wbz_v，具体为：

3.如权利要求1所述的基于模式识别的车载惯导安装角误差补偿方法，其特征在于，所述步骤S6具体包括以下步骤：

步骤S6-1、卡尔曼滤波器初始化，包括误差状态向量X、系统噪声协方差Q和后验协方差P的初始化；

取误差状态向量X为18维向量：

其中，

为导航坐标系下姿态的估计误差，δv_n为导航坐标系下速度的估计误差，δp_n为导航坐标系下位置的估计误差，b^ω和b^a分别为陀螺和加速度计的动态零偏，δk为CAN车速尺度因子误差，δθ_p为惯导设备俯仰角安装角误差，δθ_y为惯导设备航向角安装角误差；

系统协方差矩阵Q为：

Q＝diag([IMU_arw IMU_vrw IMU_gpsd IMU_apsd]²)

其中，IMU_arw和IMU_vrw为陀螺的随机游走系数和加速度计的随机游走系数，IMU_gpsd和IMU_apsd分别为陀螺和加速度计的零偏不稳定性系数；

后验协方差矩阵P为：

P＝diag([IMU_install GNSS_stdv GNSS_stdp IMU_gpsd IMU_apsd IMU_err]²)

其中，IMU_install为安装误差夹角，GNSS_stdv为GNSS速度误差方差，GNSS_stdp为GNSS位置误差方差，IMU_err由安装角误差角和车速尺度因子组成；

步骤S6-2、利用捷联惯导误差传播方程进行卡尔曼一步预测，得到先验状态量X_prior；

步骤S6-3、卡尔曼滤波量测更新，构造GNSS观测量Z_gnss、GNSS观测矩阵H_gnss，对先验状态量X_prior进行量测更新得到后验误差状态向量X_post；

所述的GNSS观测量Z_gnss：

Z_gnss＝[v_gnss-v_IMU p_gnss-p_IMU]^T

其中，v_gnss为GNSS输出的三维速度，v_IMU为步骤S5中捷联惯导更新后的速度，p_gnss为GNSS输出的位置，p_IMU步骤S5中捷联惯导更新后的位置；

所述的GNSS观测矩阵H_gnss：

其中，O₃是3阶零矩阵，I₃是3阶单位矩阵，

R_m为子午曲率半径，R_n为横向曲率半径，L为纬度，h为海拔高度；

量测更新后，得到后验误差状态向量X_post：

式中，下标中包含post的变量均为误差状态向量X中相应变量的后验误差；

步骤S6-4、对步骤S5中捷联惯导更新得到的姿态

位置p_IMU和速度v_IMU减去后验误差状态向量X_post中的姿态误差

位置误差δp_{n_post}和速度误差δv_{n_post}：

p_ins＝p_IMU-δp_{n_post}

v_ins＝v_IMU-δv_{n_post}

式中，

p_ins、v_ins分别为组合导航姿态、位置和速度的估计结果。

4.如权利要求1所述的基于模式识别的车载惯导安装角误差补偿方法，其特征在于：

所述步骤S7包括以下步骤：

步骤S7-1、对wbz_v和CAN车速进行滑动窗口均值处理，得到均值处理后的车体横摆角速度wbz_{v_filtered}和CAN车速v_filtered。；

步骤S7-2、当v_filtered大于车速第一阈值，wbz_{v_filtered}小于角速度第一阈值且大于角速度第二阈值，则判定为高速直行状态；当wbz_{v_filtered}大于角速度第一阈值或小于角速度第二阈值，则判定为转弯状态；当v_filtered小于车速第二阈值，则判定为停车状态；其他情况则判定为普通运动状态。

5.如权利要求1所述的基于模式识别的车载惯导安装角误差补偿方法，其特征在于，所述步骤S8具体如下：

在高速直行运动状态下，选择组合导航的速度估计结果与加入尺度因子的CAN车速的差值作为观测量Z_{CAN_k}，选择加入安装角误差的观测矩阵H_{CAN_install}进行观测融合；

所述的观测量Z_{CAN_k}：

其中，

v_ins为步骤S6中组合导航速度估计结果，

是一个三维的向量，

分别是其第一维、第二维、第三维；

为组合导航姿态估计结果，

为惯导设备系与车身坐标系的转换矩阵，v_D为CAN车速值，k为CAN车速尺度因子；

加入安装角误差的观测矩阵H_{CAN_install}：

H_{CAN_install}＝[M₁ M₂ O₃ O₃ O₃ M₃]^T

其中，

()_×表示反对称矩阵，O₃是3阶零矩阵；

在其他运动状态下，选择组合导航的速度估计结果与不加入尺度因子的CAN车速的差值作为观测量Z_CAN，选择不加入安装角误差的观测矩阵H_CAN进行观测融合：

所述的观测量Z_CAN：

所述的观测矩阵H_CAN：

H_CAN＝[M₁ M₂ O₃ O₃ O₃ O₃]^T。

6.如权利要求1所述的基于模式识别的车载惯导安装角误差补偿方法，其特征在于，所述步骤S9具体包括以下步骤：

步骤S9-1、利用CAN车速尺度因子误差修正CAN车速尺度因子k：

k＝k_(-)+δk

其中，k_(-)表示上一时刻的CAN车速尺度因子，k为CAN车速尺度因子，δk为CAN车速尺度因子误差；

步骤S9-2、利用惯导设备俯仰角安装角误差δθ_p、惯导设备航向角安装角误差δθ_y修正上一步的惯导设备系与车身坐标系的转换矩阵

公式如下所示：

其中，I₃是3阶单位矩阵，

为上一时刻的惯导设备系与车身坐标系的转换矩阵。

Images