用于车载组合导航的安装角补偿方法

摘要

本发明公开了一种用于车载组合导航的安装角补偿方法，包括将目标车载组合导航系统安装到车辆上并初始化；将航向安装误差角和俯仰安装误差角作为状态变量，以车辆侧向速度和车辆垂向速度作为量测观测量构建Kalman滤波器；控制车体进行若干个设定角度的转弯运动；车体在运功过程中Kalman滤波器收敛，航向安装误差角和俯仰安装误差角估算和补偿完成，完成车载组合导航的安装角补偿。本发明大大简化了安装角的补偿过程，降低了对安装角标定的环境要求，提升了安装角标定的速度和计算的简便性，用户体验感更好，而且实施简单快捷，标定效果好且安全可靠。

权利要求

1.一种用于车载组合导航的安装角补偿方法，其特征在于包括如下步骤：S1. 将目标车载组合导航系统安装到车辆上，并对目标车载组合导航系统进行初始化；

S2. 将航向安装误差角和俯仰安装误差角作为状态变量，以车辆侧向速度和车辆垂向速度作为量测观测量，构建Kalman滤波器；具体包括如下步骤：Kalman滤波器的状态变量为：

式中 为姿态误差，且 ， 为x轴方向的姿态误差， 为y轴方向的姿态误差 ， 为z 轴方向的 姿态误差 ； 为速度误差 ，且， 为x轴方向的速度误差， 为y轴方向的速度误差，为z轴方向的速度误差； 为位置误差，且 ，  为x轴方向的位置误差， 为y轴方向的位置误差， 为z轴方向的位置误差； 为陀螺漂移，且， 为x轴方向的陀螺漂移， 为y轴方向的陀螺漂移， 为z轴方向的陀螺漂移； 为加速度计零偏，且 ， 为x轴方向的加速度计零偏， 为y轴方向的加速度计零偏， 为z轴方向的加速度计零偏； 为俯仰安装误差角； 为航向安装误差角；

Kalman滤波器的状态方程为：

式中 为 的状态观测量； 为 的状态观测量； 为 的状态观测量；

为 的状态观测量； 为 的状态观测量； 为 的状态观测量； 为 的状态观测量； 为姿态误差方程中姿态误差的相关部分，且 ， 为地理坐标系相对惯性坐标系的旋转速度，×为叉乘运算符，数学意义为将符号×前面的3维向量变成 3 *3 的 矩阵 ； 为姿 态 误差 方 程中 速 度误 差的 相 关 部分 ，且，RMh为地球的长半轴半径，RNh为地球的短半轴半径，L为纬度； 为姿态误差方程中位置误差的相关部分，且 ，M1为第一中间变量且 ， 为地球自 转角速度 ，M 2为 第二中间变量 且，vE为导航坐标系下东北天速度中的E轴速度；

为载体系到导航系的姿态转换矩阵； 为速度误差方程中姿态误差的相关部分，且， 为比力；Mvv为速度误差方程中速度误差的相关部分，且n

，v 为导航坐标系下的 东北天速度且

，vN为导航坐标系下东北天速度中的N轴速度，vU为导航坐标系下东北天速度中的U轴速度， 为姿态误差方程中速度误差的 相关部分且， 为在导航坐标系下的地球自转角速度， 为导航坐标系转动角速度；Mvp为速度误差方程中位置误差的相关部分，且； 为位置误差方程中速度误差的相关部分，且 ；Mpp为位置误差方程中位置误差的相关部分，且 ；

为陀螺白噪声； 为加速度计白噪声；

Kalman滤波器的量测观测量为 ； 为载体系X轴速度； 为载体系Z轴速度；

Kalman滤波器的量测方程为：

式中H1为第三中间变量，且 ， 为载体坐标系到车辆坐标系的转换矩阵， 为导航坐标系到载体坐标系的转换矩阵， 为导航坐标系下的载体速度， 的定义为：若*为向量则 表示取向量*的第i个元素，若*为矩阵则 表示取矩阵*的第i行元素；H2为第四中间变量，且 ，为取矩阵*的第i行第j列的元素；

S3. 控制车体进行若干个设定角度的转弯运动；

S4. 车体在步骤S3的运功过程中，Kalman滤波器收敛，航向安装误差角和俯仰安装误差角估算和补偿完成，从而完成车载组合导航的安装角补偿。

2.根据权利要求1所述的用于车载组合导航的安装角补偿方法，其特征在于步骤S1所述的对目标车载组合导航系统进行初始化，具体为通过卫星导航系统，计算目标车载组合导航系统的初始的位置参数、初始的速度参数和初始的姿态参数。

3.根据权利要求2所述的用于车载组合导航的安装角补偿方法，其特征在于步骤S3所述的控制车体进行若干个设定角度的转弯运动，具体为控制车体进行至少2个90°的转弯运动。

说明书

用于车载组合导航的安装角补偿方法

技术领域

[0001] 本发明属于导航技术领域，具体涉及一种用于车载组合导航的安装角补偿方法。

背景技术

[0002] 随着经济技术的发展和人们生活水平的提高，导航技术已经广泛应用于人们的生产和生活当中，给人们的生产和生活带来了无尽的便利。因此，保障导航过程的精确和稳定，就成为了导航系统最重要的任务之一。

[0003] 随着车载组合导航系统在城市峡谷场景、工业高楼层园区场景、隧道场景等复杂环境下的大量使用，IMU（惯性传感器）与车体之间的安装角就成为了影响导航精度的一个重要因素。在空旷环境下，由于卫星导航系统的精度可达到厘米级，且速度和精度较高，安装角对位置精度和速度精度的影响就相对较小，而对姿态精度的影响则直接取决于安装角的大小；但是，在卫星导航系统失效的场景下，即使极小的安装角都会对位置精度、速度精度和姿态精度产生巨大影响。因此，需要对安装角进行估算，并将估算结果补偿到导航系统中。

[0004] 图1为航向角的安装误差示意图：其中，N、E为导航地理坐标系（通常选取为地理坐标系，比如东北天坐标系）的北向和东向，N'、E'则为计算坐标系的北向和东向，y轴为车体纵轴，y1轴为惯性导航系统的纵轴。

[0005] 目前，航向安装角的估算方法一般为：事先选好一段标定路线（通常为直线）作为基线，该基线的方位角为 （通过指南针或者双天线卫星导航系统测量获得）。然后，车辆以一定速度沿基线行驶至终点后，惯性导航系统计算得到的东向和北向位移分别为 和；假定在理想情况下，陀螺和加速度计没有误差，因此惯导横轴方向上的位移应该为0，那么则估计航向安装误差角 为 ；计算出航向安装角误差 后，重新装订惯性导航系统的航向角 ，即y1相对N的夹角，效果就是初始时将y1逆时针转过角，使其与车体纵轴y轴重合，对应的坐标变换为 。

[0006] 但是，现有技术的缺陷在于执行标定的过程极其麻烦：首先需要事先选取一条2km以上的直线（目前大多数应用都是低精度的惯性导航系统，需要长时间的平滑才能获得比较准确的结果），而且在标定过程中需要保证车辆完全以直线的方式行驶（完全直线行驶2km以上的难度非常大）；上述原因会导致标定过程所获得的航向安装角的精度不高，进而会影响后续正常工作过程中车载导航系统的导航精度；而且现有方法，在每次重新安装产品后都需要按照重新标定，非常费时费力。

发明内容

[0007] 本发明的目的在于提供一种实施简单快捷，标定效果好且安全可靠的用于车载组合导航的安装角补偿方法。

[0008] 本发明提供的这种用于车载组合导航的安装角补偿方法，包括如下步骤：

[0009] S1. 将目标车载组合导航系统安装到车辆上，并对目标车载组合导航系统进行初始化；

[0010] S2. 将航向安装误差角和俯仰安装误差角作为状态变量，以车辆侧向速度（车体系X轴速度）和车辆垂向速度（车体系Z轴速度）作为量测观测量，构建Kalman滤波器；

[0011] S3. 控制车体进行若干个设定角度的转弯运动；

[0012] S4. 车体在步骤S3的运功过程中，Kalman滤波器收敛，航向安装误差角和俯仰安装误差角估算和补偿完成，从而完成车载组合导航的安装角补偿。

[0013] 步骤S1所述的对目标车载组合导航系统进行初始化，具体为通过卫星导航系统，计算目标车载组合导航系统的初始的位置参数、初始的速度参数和初始的姿态参数。

[0014] 步骤S2所述的将航向安装误差角和俯仰安装误差角作为状态变量，以车辆侧向速度（车体系X轴速度）和车辆垂向速度（车体系Z轴速度）作为量测观测量，构建Kalman滤波器，具体为以姿态误差、速度误差、位置误差、陀螺漂移、加速度计零偏、航向安装误差角和俯仰安装误差角作为状态变量，以车辆侧向速度（车体系X轴速度）和车辆垂向速度（车体系Z轴速度）作为量测观测量，构建Kalman滤波器。

[0015] 所述的将航向安装误差角和俯仰安装误差角作为状态变量，以车辆侧向速度（车体系X轴速度）和车辆垂向速度（车体系Z轴速度）作为量测观测量，构建Kalman滤波器，具体包括如下步骤：

[0016] Kalman滤波器的状态变量为：

[0017]

[0018] 式中 为姿态误差，且 ， 为x轴方向的姿态误差，为y轴方向的姿态误差， 为z轴方向的姿态误差； 为速度误差，且
， 为x轴方向的速度误差， 为y轴方向的速度误差，
为z轴方向的速度误差； 为位置误差，且 ，  为x轴方
向的位置误差， 为y轴方向的位置误差， 为z轴方向的位置误差； 为陀螺漂移，且， 为x轴方向的陀螺漂移， 为y轴方向的陀螺漂移， 为z轴方向
的陀螺漂移； 为加速度计零偏，且 ， 为x轴方向的加速度计
零偏， 为y轴方向的加速度计零偏， 为z轴方向的加速度计零偏； 为俯仰安装误差角； 为航向安装误差角；

[0019] Kalman滤波器的状态方程为：

[0020]

[0021] 式中 为 的状态观测量； 为 的状态观测量； 为 的状态观测量； 为 的状态观测量； 为 的状态观测量； 为 的状态观测量； 为
的状态观测量； 为姿态误差方程中姿态误差的相关部分，且 ， 为地
理坐标系相对惯性坐标系的旋转速度，×为叉乘运算符，数学意义为将符号×前面的3维向量变成3*3的矩阵 ； 为姿态误差方程中 速度误差的相关部分 ，且
，RMh为地球的长半轴半径，RNh为地球的短半轴半径，L为纬
度； 为姿态误差方程中位置误差的相关部分，且 ，M1为第一中间变量
且 ， 为地球自 转角速度 ，M 2为 第二中间变量 且
，vE为导航坐标系下东北天速度中的E轴速度；
为载体系到导航系的姿态转换矩阵； 为速度误差方程中姿态误差的相关部分，且， 为比力；Mvv为速度误差方程中速度误差的相关部分，且
n
，v 为导航坐标系下的 东北天速度且
，vN为导航坐标系下东北天速度中的N轴速度，vU为导航坐标系下东
北天速度中的U轴速度， 为姿态误差方程中速度误差的 相关部分且
， 为地球自转角速率， 为导航坐标系转动角速率；
Mvp为速度误差方程中位置误差的相关部分，且 ； 为位置误
差方程中速度误差的相关部分，且 ；Mpp为位置误差方程
中位置误差的相关部分，且 ； 为陀螺白噪声；
为加速度计白噪声；

[0022] Kalman滤波器的量测观测量为 ； 为载体系X轴速度； 为载体系Z轴速度；

[0023] Kalman滤波器的量测方程为：

[0024]

[0025] 式中H1为第三中间变量，且 ， 为载体坐标系到车辆坐标系的转换矩阵， 为导航坐标系到载体坐标系的转换矩阵， 为导航坐标系下的载体速度， 的定义为：若*为向量则 表示取向量*的第i个元素，若*为矩阵则表示取矩阵*的第i行元素；H2为第四中间变量，且
， 为取矩阵*的第i行第j列的元素。

[0026] 步骤S3所述的控制车体进行若干个设定角度的转弯运动，具体为控制车体进行至少2个90°的转弯运动。

[0027] 本发明提供的这种用于车载组合导航的安装角补偿方法，通过Kalman滤波器估计安装角误差，仅需要简单的机动，通过误差状态量（姿态误差、速度误差、位置误差、安装角误差）变化率与设置的阈值比较来判断滤波器是否收敛，如果滤波器收敛则将估计的安装角误差补偿到导航系统的姿态矩阵中完成安装误差角的补偿；相对于通过特定路线进行安装角标定的方案来说，本发明大大简化了安装角的补偿过程，不仅降低了对安装角标定的环境要求，而且提升了安装角标定的速度和计算的简便性，在日常使用过程中用户的体验感会更好，而且本发明实施简单快捷，标定效果好且安全可靠。

附图说明

[0028] 图1为航向角安装误差示意图。

[0029] 图2为本发明方法的方法流程示意图。

[0030] 图3为本发明方法的实施例的跑车轨迹示意图。

[0031] 图4为本发明方法的实施例的跑车轨迹在地下车库部分的轨迹放大示意图。

具体实施方式

[0032] 如图2所示为本发明方法的方法流程示意图：本发明提供的这种用于车载组合导航的安装角补偿方法，包括如下步骤：

[0033] S1. 将目标车载组合导航系统安装到车辆上，并对目标车载组合导航系统进行初始化；具体为通过卫星导航系统，计算目标车载组合导航系统的初始的位置参数、初始的速度参数和初始的姿态参数；同时，认为车辆在运行过程中没有蹦跳和漂移运动，即车辆的横向速度和垂向速度为0；

[0034] S2. 将航向安装误差角和俯仰安装误差角作为状态变量，以车辆侧向速度（车体系X轴速度）和车辆垂向速度（车体系Z轴速度）作为量测观测量，构建Kalman滤波器；具体为以姿态误差、速度误差、位置误差、陀螺漂移、加速度计零偏、航向安装误差角和俯仰安装误差角作为状态变量，以车辆侧向速度（车体系X轴速度）和车辆垂向速度（车体系Z轴速度）作为量测观测量，构建Kalman滤波器；

[0035] 具体实施时，包括如下步骤：

[0036] Kalman滤波器的状态变量为：

[0037]

[0038] 式中 为姿态误差，且 ， 为x轴方向的姿态误差，为y轴方向的姿态误差， 为z轴方向的姿态误差； 为速度误差，且
， 为x轴方向的速度误差， 为y轴方向的速度误差，
为z轴方向的速度误差； 为位置误差，且 ，  为x轴方
向的位置误差， 为y轴方向的位置误差， 为z轴方向的位置误差； 为陀螺漂移，且， 为x轴方向的陀螺漂移， 为y轴方向的陀螺漂移， 为z轴方向
的陀螺漂移； 为加速度计零偏，且 ， 为x轴方向的加速度计
零偏， 为y轴方向的加速度计零偏， 为z轴方向的加速度计零偏； 为俯仰安装误差角； 为航向安装误差角；

[0039] Kalman滤波器的状态方程为：

[0040]

[0041] 式中 为 的状态观测量； 为 的状态观测量； 为 的状态观测量； 为 的状态观测量； 为 的状态观测量； 为 的状态观测量； 为
的状态观测量； 为姿态误差方程中姿态误差的相关部分，且 ， 为地
理坐标系相对惯性坐标系的旋转速度，×为叉乘运算符，数学意义为将符号×前面的3维向量变成3*3的矩阵 ； 为姿态误差方程中 速度误差的相关部分 ，且
，RMh为地球的长半轴半径，RNh为地球的短半轴半径，L为纬
度； 为姿态误差方程中位置误差的相关部分，且 ，M1为第一中间变量
且 ， 为地球自 转角速度 ，M 2为 第二中间变量 且
，vE为导航坐标系下东北天速度中的E轴速度；
为载体系到导航系的姿态转换矩阵； 为速度误差方程中姿态误差的相关部分，且， 为比力；Mvv为速度误差方程中速度误差的相关部分，且
n
，v 为导航坐标系下的 东北天速度且
，vN为导航坐标系下东北天速度中的N轴速度，vU为导航坐标系下东
北天速度中的U轴速度， 为姿态误差方程中速度误差的 相关部分且
， 为地球自转角速率， 为导航坐标系转动角速率；
Mvp为速度误差方程中位置误差的相关部分，且 ； 为位置误
差方程中速度误差的相关部分，且 ；Mpp为位置误差方程
中位置误差的相关部分，且 ； 为陀螺白噪声；
为加速度计白噪声；

[0042] Kalman滤波器的量测观测量为 ； 为载体系X轴速度； 为载体系Z轴速度；

[0043] Kalman滤波器的量测方程为：

[0044]

[0045] 式中H1为第三中间变量，且 ， 为载体坐标系到车辆坐标系的转换矩阵， 为导航坐标系到载体坐标系的转换矩阵， 为导航坐标系下的载体速度， 的定义为：若*为向量则 表示取向量*的第i个元素，若*为矩阵则表示取矩阵*的第i行元素；H2为第四中间变量，且
， 为取矩阵*的第i行第j列的元素;

[0046] S3. 控制车体进行若干个设定角度的转弯运动；具体为控制车体进行至少2个90°的转弯运动；

[0047] S4. 车体在步骤S3的运功过程中，Kalman滤波器收敛，航向安装误差角和俯仰安装误差角估算和补偿完成，从而完成车载组合导航的安装角补偿。

[0048] 本发明方法通过Kalman滤波器估计安装角误差，仅需要简单的机动，通过误差状态量（姿态误差、速度误差、位置误差、安装角误差）变化率与设置的阈值比较来判断滤波器是否收敛，如果滤波器收敛则将估计的安装角误差补偿到导航系统的姿态矩阵中完成安装误差角的补偿；相对于通过特定路线进行安装角标定的方案来说，本发明大大简化了安装角的补偿过程，不仅降低了对安装角标定的环境要求，而且提升了安装角标定的速度和计算的简便性，在日常使用过程中用户的体验感会更好，而且本发明实施简单快捷，标定效果好且安全可靠；

[0049] 下面以一个地下车库进行案例说明。

[0050] 采用两个模块（模块1对应于采用本发明方法进行安装角标定；模块2对应于采用现有技术进行安装角标定）固定在车辆后备厢，在如图3所示的包含地下车库的区域，进行跑车试验。其中两个模块所对应的航向安装角的结果如表1所示。

[0051] 表1 模块所对应的航向安装角示意表

[0052]

[0053] 实验时，首先选取一条长直线，使用现有方法和本发明方法分别计算安装角补偿到各自的导航系统中后，再开始本次跑车试验。试验轨迹中包含地下车库。跑车结果如图3和图4所示。其中图3为整体跑车轨迹示意图，图4为图3中虚线框区域（对应于地下车库区域）的放大示意图。从上述结果可以看出，现有方法的安装角标定精度不高，导致地下车库的轨迹不平滑，轨迹重合度较差（在同一车道跑车）。而本发明方法标定的航向安装角，精度较高，在整体的跑车轨迹上，轨迹重合度均较高。