欢迎来到知嘟嘟! 联系电话:13336804447 卖家免费入驻,海量在线求购! 卖家免费入驻,海量在线求购!
知嘟嘟
我要发布
联系电话:13336804447
知嘟嘟经纪人
收藏
专利号: 2022106770461
申请人: 浙江工业大学
专利类型:发明专利
专利状态:已下证
专利领域: 测量;测试
更新日期:2026-11-16
缴费截止日期: 暂无
价格&联系人
年费信息
委托购买

摘要:

权利要求书:

1.一种基于模式识别的车载惯导安装角误差补偿方法,其特征在于:所述方法包括以下步骤:步骤S1、将惯性导航设备安装于车上,进行系统初始化;

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

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

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

步骤S5、利用IMU数据进行捷联惯导更新,得到更新后的姿态位置pIMU和速度vIMU;

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

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

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

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

2.如权利要求1所述的基于模式识别的车载惯导安装角误差补偿方法,其特征在于,步骤S4中,利用惯导设备系与车身坐标系的转换矩阵将陀螺仪z轴输出值wbzb投影在车体系上得到车体横摆角速度wbzv,具体为:

3.如权利要求1所述的基于模式识别的车载惯导安装角误差补偿方法,其特征在于,所述步骤S6具体包括以下步骤:步骤S6-1、卡尔曼滤波器初始化,包括误差状态向量X、系统噪声协方差Q和后验协方差P的初始化;

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

其中,为导航坐标系下姿态的估计误差,δvn为导航坐标系下速度的估计误差,δpn为导航坐标系下位置的估计误差,bω和ba分别为陀螺和加速度计的动态零偏,δk为CAN车速尺度因子误差,δθp为惯导设备俯仰安装角误差,δθy为惯导设备航向安装角误差;

系统协方差矩阵Q为:

Q=diag([IMUarwIMUvrwIMUgpsdIMUapsd]2)其中,IMUarw和IMUvrw为陀螺的随机游走系数和加速度计的随机游走系数,IMUgpsd和IMUapsd分别为陀螺和加速度计的零偏不稳定性系数;

后验协方差矩阵P为:

P=diag([IMUinstallGNSSstdvGNSSstdp IMUgpsdIMUapsdIMUerr]2)其中,IMUinstall为安装误差夹角,GNSSstdv为GNSS速度误差方差,GNSSstdp为GNSS位置误差方差,IMUerr由安装角误差角和车速尺度因子组成;

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

步骤S6-3、卡尔曼滤波量测更新,构造GNSS观测量Zgnss、GNSS观测矩阵Hgnss,对先验状态量Xprior进行量测更新得到后验误差状态向量Xpost;

所述的GNSS观测量Zgnss:

Zgnss=[vgnss-vIMU pgnss-pIMu]T其中,vgnss为GNSS输出的三维速度,vIMU为步骤S5中捷联惯导更新后的速度,pgnss为GNSS输出的位置,pIMU步骤S5中捷联惯导更新后的位置;

所述的GNSS观测矩阵Hgnss:

其中,O3是3阶零矩阵,I3是3阶单位矩阵,Rm为子午曲率半径,Rn为横向曲率半径,L为纬度,h为海拔高度;

量测更新后,得到后验误差状态向量Xpost:

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

步骤S6-4、对步骤S5中捷联惯导更新得到的姿态位置pIMU和速度vIMU减去后验误差状态向量Xpost中的姿态误差位置误差δpn_post和速度误差δvn_post:pins=pIMU-δpn_post

vins=vIMU-δvn_post

式中,pins、vins分别为组合导航姿态、位置和速度的估计结果。

4.如权利要求1所述的基于模式识别的车载惯导安装角误差补偿方法,其特征在于:所述步骤S7包括以下步骤:

步骤S7-1、对wbzv和CAN车速进行滑动窗口均值处理,得到均值处理后的车体横摆角速度wbzv_filtered和CAN车速vfiltered;

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

5.如权利要求1所述的基于模式识别的车载惯导安装角误差补偿方法,其特征在于,所述步骤S8具体如下:在高速直行运动状态下,选择组合导航的速度估计结果与加入尺度因子的CAN车速的差值作为观测量ZCAN_k,选择加入安装角误差的观测矩阵HCAN_install进行观测融合;

所述的观测量ZCAN_k:

其中,vins为步骤S6中组合导航速度估计结果,是一个三维的向量,分别是其第一维、第二维、第三维;为组合导航姿态估计结果,为惯导设备系与车身坐标系的转换矩阵,vD为CAN车速值,k为CAN车速尺度因子;

加入安装角误差的观测矩阵HCAN_install:

HCAN_install=[M1M2O3O3O3M3]T其中,()×表示反对称矩阵,O3是3阶零矩阵;

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

所述的观测矩阵HCAN:

HCAN=[M1M2O3O3O3O3]T。

6.如权利要求1所述的基于模式识别的车载惯导安装角误差补偿方法,其特征在于,所述步骤S9具体包括以下步骤:步骤S9-1、利用CAN车速尺度因子误差修正CAN车速尺度因子k:k=k(-)+δk

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

步骤S9-2、利用惯导设备俯仰安装角误差δθp、惯导设备航向安装角误差δθy修正上一步的惯导设备系与车身坐标系的转换矩阵公式如下所示:其中,I3是3阶单位矩阵,为上一时刻的惯导设备系与车身坐标系的转换矩阵。