CN105737842A - 基于旋转调制和虚拟里程仪的车载自主导航方法 - Google Patents

Abstract

本发明公开了基于旋转调制和虚拟里程仪的车载自主导航方法，将MEMS IMU安装于车轮转动轴附近，即将车轮作为MEMS旋转惯导的转动机构实现旋转调制，利用车轮的旋转，周期性改变惯性器件相对于导航系的位置，抵消惯性器件常值误差对导航精度的影响，并通过车轮的旋转角度，推算出车辆的行驶距离和速度，实现虚拟里程仪，辅助MEMS旋转惯导系统导航。

Description

基于旋转调制和虚拟里程仪的车载自主导航方法

技术领域

本发明属于车载导航技术领域，特别涉及了基于旋转调制和虚拟里程仪的车载自主导航方法。

背景技术

惯性导航系统是一种不依赖于外部信息的自主式导航系统，它通过正交安装的三轴陀螺仪和加速度计测量载体相对于空间的角速度和加速度，并基于航位推算原理推导出载体实时运动状态。但是惯导系统的价格较昂贵，因此一般应用于飞行器、导弹和陆地战车等军事装备的导航定位。近年来，随着科技的进步，出现了低成本MEMS惯导系统，其体积小、价格低，为民用车辆使用惯导设备创造了机会，然而MEMS惯导系统具有精度低的缺点，定位误差会随着时间增加而快速积累，需要使用其他方法进行辅助导航，因此，将旋转调制技术应用于MEMS惯导系统(如期刊文章《MEMS器件捷联惯导系统旋转调制技术》，东北大学学报，2014年第35卷第4期，页码：494-498)。

旋转调制技术是一种误差抑制技术，通过转动机构带动惯性器件按照既定方案旋转，使得惯性器件常值偏差沿导航系统投影呈周期振荡形式，在一个旋转周期内积分结果为零，进而抵消惯性器件常值偏差对导航精度的影响来提高导航精度(如专利CN201410143285.4公开的《一种单轴旋转惯导系统惯性器件误差补偿方法》)。

然而，传统的MEMS旋转惯导进行旋转调制需要高精度的转动机构和控制机构，而转动机构和控制机构体积大、成本高，削弱了MEMS惯导体积小、成本低的优势。

发明内容

为了解决上述背景技术提出的技术问题，本发明旨在提供基于旋转调制和虚拟里程仪的车载自主导航方法，解决了传统MEMS惯导进行旋转调制时，因其转动机构和控制机构体积大、成本高而削弱了MEMS惯导体积小、成本低的优势这一问题。

为了实现上述技术目的，本发明的技术方案为：

基于旋转调制和虚拟里程仪的车载自主导航方法，包括以下步骤：

(1)在车身安装3个加速度计，将3个加速度计的敏感轴方向作为X、Y、Z轴，建立载体坐标系，载体坐标系的X、Y、Z轴分别沿车身横轴向右、车身纵轴向前、车身竖轴向上，载体坐标系的原点与车身质心重合，在车轮转动轴的附近安装1个惯性测量单元，它包括三轴加速度计和三轴陀螺仪，惯性测量单元的3个敏感轴方向分别作为X、Y、Z轴，建立旋转坐标系，旋转坐标系的X轴与车轮旋转轴平行，旋转坐标系的Y、Z轴在车轮平面内且分别与X轴垂直，呈右手坐标系，旋转坐标系的原点与车轮质心重合，将地理东向、北向、天向作为X、Y、Z轴，建立导航坐标系；

(2)以车轮的旋转角度、车轮的旋转角速率以及惯性测量单元安装时偏离车轮转动轴的距离作为状态量，建立状态方程，以惯性测量单元中三轴加速度计输出的信息作为量测量，建立量测方程，根据状态方程和量测方程，利用滤波器估计出车轮的旋转角度和惯性测量单元安装误差造成的向心加速度；

(3)根据惯性测量单元和车身上3个加速度计采集到的数据，以及步骤(2)估计出的车轮旋转角度和惯性测量单元安装误差造成的向心加速度，解算出MEMS旋转惯导系统位置、速度和姿态信息的输出；

(4)根据步骤(2)估计出的车轮旋转角度，推算出车轮转动的圈数，根据车轮转动圈数计算出车辆的行驶距离和速度，从而实现虚拟里程仪的作用；

(5)选取状态量和量测量，建立MEMS旋转惯导系统/虚拟里程仪组合导航系统的状态方程和量测方程，利用卡尔曼滤波器实现MEMS旋转惯导/虚拟里程仪组合导航系统的信息融合和状态估计。

进一步地，步骤(3)的具体过程为：

惯性测量单元输出的数据经步骤(2)估计出的惯性测量单元安装误差造成的向心加速度的误差补偿后，通过三轴陀螺仪采集的数据计算出姿态矩阵，再结合三轴加速度计采集的数据，解算出旋转惯导系统输出的位置、速度信息；根据步骤(2)估计出的车轮旋转角度求出旋转坐标系到载体坐标系的转换矩阵，根据三轴陀螺仪采集的数据以及上一时刻的速度信息计算出旋转坐标下的姿态信息，从而解算出载体坐标系下的姿态信息。

进一步地，所述旋转坐标系到载体坐标系的转换矩阵为：

$C_r^b=\left[\begin{array}{ccc}1& 0& 0\\ 0& cos\left(\mathrm{\θ}\right)& -sin\left(\mathrm{\θ}\right)\\ 0& \sin \left(\mathrm{\θ}\right)& cos\left(\mathrm{\θ}\right)\end{array}\right]$

其中，θ为车轮的旋转角度。

进一步地，步骤(5)中选取的状态量为：

X＝[φ_e,φ_n,φ_u,δv_e,δv_n,δv_u,δL,δλ,δh,ε_bx,ε_by,ε_bz,ε_rx,ε_ry,ε_rz,▽_x,▽_y,▽_z]

其中，φ_e,φ_n,φ_u为平台误差角，δv_e,δv_n,δv_u为东、北、天方向速度误差，δL,δλ,δh为纬度、经度、高度误差，ε_bx,ε_by,ε_bz为旋转坐标系下陀螺仪的随机常值漂移，ε_rx,ε_ry,ε_rz为旋转坐标系下陀螺仪的一阶马尔可夫过程，▽_x,▽_y,▽_z为旋转坐标系下加速度计的一阶马尔可夫过程；

则状态方程为：

$\stackrel{\·}{X}=FX+GW$

其中，F为系统状态矩阵，G为误差系数矩阵，W为白噪声随机误差矢量。

进一步地，步骤(5)中选取的量测量为旋转惯导系统输出的速度信息与虚拟里程仪输出的速度信息的差值，则量测方程为：

Z＝HX+V

其中，V为系统量测噪声，H为量测矩阵。

采用上述技术方案带来的有益效果：

(1)本发明通过测量车轮旋转角度，实现基于车轮旋转的旋转调制技术，抵消了惯性器件的常值误差对导航精度的影响，从而提高车辆姿态、速度和位置精度；

(2)本发明提出的测量车轮旋转角度的方法，可以有效消除运动加速度对计算车轮旋转角度的影响，从而使该自主导航方法可以适用于车辆匀速或非匀速运动状态下；

(3)本发明通过测量的车轮旋转角度，推算出车辆的行驶距离以及速度，起到了虚拟里程仪的作用；

(4)本发明利用滤波器实现MEMS旋转调制/虚拟里程仪系统的信息融合，从而提高导航精度。

附图说明

图1为本发明的总体结构框图；

图2为本发明安装于车轮上的MEMS旋转惯导系统的解算流程图；

图3为本发明涉及的坐标系示意图；

图4为本发明中旋转坐标系和载体坐标系关系示意图。

具体实施方式

以下将结合附图，对本发明的技术方案进行详细说明。

步骤1：

在车身上安装一组加速度计，用来测量车辆三个方向的运动加速度，三个加速度计的敏感轴分别沿载体横轴向右，沿载体纵轴向前，沿载体竖轴向上，即“右前上”坐标系。

在车轮转动轴附近安装一个MEMSIMU，包括三个陀螺和三个加速度计，IMU的x轴与车轮旋转轴平行，y、z轴在车轮平面内，分别与X轴垂直，呈右手坐标系关系。

步骤2：

将车轮作为MEMS旋转惯导的转动机构，考虑IMU的安装偏差，选取合适的状态量与量测量，利用滤波器，实现对车轮旋转角度的估计。

a)首先，阐释本文中使用的坐标系，定义如下，如图3所示：

n系：导航坐标系，x、y、z轴分别指向东向、北向、天向。

b系：载体坐标系，原点与载体质心重合，x轴沿载体横轴向右，y轴沿载体纵轴向前，z轴沿载体竖轴向上，即“右前上”坐标系。

r系：旋转坐标系，随车轮的转动而实时变化。r系的原点在车轮质心处，x、y、z轴分别指向MEMSIMU各轴惯性器件敏感轴方向，x轴与车轮旋转轴平行，y、z轴在车轮平面内，分别与x轴垂直，呈右手坐标系关系。

b)状态方程的建立

状态量X如下:

X＝[θωr]^T(1)

其中θ为车轮旋转角度，ω为车轮旋转角速率，r为IMU安装时加速度计敏感轴距离车轮转动轴的距离。

状态量之间的关系为：

θ_k+1＝θ_k+Δt*ω_k(2)

ω_k+1＝ω_k(3)

r_k+1＝r_k(4)

其中，Δt为采样时间。

记为状态方程：

$\left[\begin{array}{c}\stackrel{\·}{\mathrm{\θ}}\\ \stackrel{\·}{\mathrm{\ω}}\\ \stackrel{\·}{r}\end{array}\right]=\left[\begin{array}{ccc}0& 1& 0\\ 0& 0& 0\\ 0& 0& 0\end{array}\right]\left[\begin{array}{c}\mathrm{\θ}\\ \mathrm{\ω}\\ r\end{array}\right]---\left(5\right)$

即

其中，W为系统噪声，是均值为0的高斯白噪声。

c)量测方程的建立

当车辆行驶时，车轮绕着X轴旋转，如图4所示，则r系到b系的转换矩阵为：

$C_r^b=\left[\begin{array}{ccc}1& 0& 0\\ 0& cos\left(\mathrm{\θ}\right)& -sin\left(\mathrm{\θ}\right)\\ 0& sin\left(\mathrm{\θ}\right)& cos\left(\mathrm{\θ}\right)\end{array}\right]---\left(7\right)$

考虑到IMU不能刚好安装在车轮转动轴处，则垂直于旋转轴的两轴加表将敏感到向心加速度，基于此，建立量测方程为：

$A_r={\left(C_r^b\right)}^T*A_b+A_c---\left(8\right)$

即

其中，A_bx,A_by,A_bz为安装于车身的载体系下加速度计的输出(包括车辆运动加速度和重力加速度)，A_rx,A_ry,A_rz为安装于车轮的旋转系下加速度计的输出，A_c为向心加速度。

由式(8)可得：

A_bycos(θ)+A_bzsin(θ)+rω²＝A_ry(10)

-A_bysin(θ)+A_bzcos(θ)+rω²＝A_rz(11)

选取安装于车轮的旋转系下加速度计的输出为量测量，即：

Z＝[A_ryA_rz]^T(12)

量测方程记为：

Z＝h(X,V)(13)

其中，V为量测噪声，是均值为0的高斯白噪声。

根据建立的状态方程和量测方程，利用滤波器，实现对车轮旋转角度的估计以及IMU安装误差造成的向心加速度的估计。

步骤3：

采集安装于车轮上的惯性器件数据，扣除IMU安装误差造成的向心加速度值，再结合步骤2中求得的车轮旋转角度，解算MEMS旋转惯导系统输出。

图2为安装于车轮上的MEMS旋转惯导系统的解算流程图，由此可以看出：

MEMS旋转惯导系统的解算流程与传统MEMS惯导系统解算流程类似：使用陀螺仪测量的载体角速度解算姿态矩阵，通过姿态矩阵解算姿态，并使用姿态矩阵将加速度计的输出由载体坐标系转换到导航坐标系，从而计算速度、位置。

两者解算流程的主要区别在于：传统的MEMS惯导在数学平台内部解算得到的姿态角即为机体姿态角，而MEMS旋转惯导系统需要使用车轮旋转角度计算r系到b系之间的转换矩阵通过转换才能得到机体的姿态角。

即从到存在以下关系：

$C_n^b=C_r^b{C}_n^r---\left(14\right)$

步骤4：

根据步骤2中求得的车轮旋转角度，推算出车轮的转动圈数，进而实现对车辆的行驶距离以及速度的求解，起到了虚拟里程仪的作用。

车辆的行驶距离和速度分别为：

S＝C*2πR(15)

$V_k=\frac{S_k-S_{k-1}}{\mathrm{\Δ}t}---\left(16\right)$

其中，C为车轮转动圈数，R为车轮半径。

步骤5：

选取合适的状态量和观测量，建立MEMS旋转调制/虚拟里程仪组合导航系统的状态方程和量测方程，利用卡尔曼滤波器实现MEMS旋转调制/虚拟里程仪系统的信息融合和状态估计。

MEMS旋转调制/虚拟里程仪系统误差状态变量，定义如下：

X＝[φ_e,φ_n,φ_u,δv_e,δv_n,δv_u,δL,δλ,δh,ε_bx,ε_by,ε_bz,ε_rx,ε_ry,ε_rz,▽_x,▽_y,▽_z](17)

式中，φ_e,φ_n,φ_u为平台误差角；δv_e,δv_n,δv_u为东、北、天方向速度误差；δL,δλ,δh为纬度、经度、高度误差；ε_bx,ε_by,ε_bz,ε_rx,ε_ry,ε_rz分别为安装于旋转坐标系系下陀螺仪的随机常值漂移和一阶马尔可夫过程；▽_x,▽_y,▽_z为旋转坐标系下加速度计的一阶马尔可夫过程。

状态方程为：

$\stackrel{\·}{X}=\mathrm{FX}+\mathrm{GW}---\left(18\right)$

式中，F为系统状态矩阵，G为误差系数矩阵，W为白噪声随机误差矢量。

选取MEMS旋转惯导与虚拟里程仪速度的差值作为量测量，

则量测方程为：

Z＝HX+V(19)

式中，V为系统量测噪声，H为量测矩阵。

根据建立的状态方程和量测方程，利用卡尔曼滤波器实现MEMS旋转调制/虚拟里程仪系统的信息融合和状态估计。

以上实施例仅为说明本发明的技术思想，不能以此限定本发明的保护范围，凡是按照本发明提出的技术思想，在技术方案基础上所做的任何改动，均落入本发明保护范围之内。

Claims (5)

1.基于旋转调制和虚拟里程仪的车载自主导航方法，其特征在于，包括以下步骤：

(1)在车身安装3个加速度计，将3个加速度计的敏感轴方向作为X、Y、Z轴，建立载体坐标系，载体坐标系的X、Y、Z轴分别沿车身横轴向右、车身纵轴向前、车身竖轴向上，载体坐标系的原点与车身质心重合，在车轮转动轴的附近安装1个MEMS惯性测量单元，它包括三轴加速度计和三轴陀螺仪，惯性测量单元的3个敏感轴方向分别作为X、Y、Z轴，建立旋转坐标系，旋转坐标系的X轴与车轮旋转轴平行，旋转坐标系的Y、Z轴在车轮平面内且分别与X轴垂直，呈右手坐标系，旋转坐标系的原点与车轮质心重合，将地理东向、北向、天向作为X、Y、Z轴，建立导航坐标系；

(2)以车轮的旋转角度、车轮的旋转角速率以及惯性测量单元安装时偏离车轮转动轴的距离作为状态量，建立状态方程，以惯性测量单元中三轴加速度计输出的信息作为量测量，建立量测方程，根据状态方程和量测方程，利用滤波器估计出车轮的旋转角度和惯性测量单元安装误差造成的向心加速度；

(3)根据惯性测量单元和车身上3个加速度计采集到的数据，以及步骤(2)估计出的车轮旋转角度和惯性测量单元安装误差造成的向心加速度，解算出MEMS旋转惯导系统位置、速度和姿态信息；

(4)根据步骤(2)估计出的车轮旋转角度，推算出车轮转动的圈数，根据车轮转动圈数计算出车辆的行驶距离和速度，从而实现虚拟里程仪的作用；

(5)选取状态量和量测量，建立MEMS旋转惯导/虚拟里程仪组合导航系统的状态方程和量测方程，利用卡尔曼滤波器实现MEMS旋转惯导/虚拟里程仪组合导航系统的信息融合和状态估计。

2.根据权利要求1所述基于旋转调制和虚拟里程仪的车载自主导航方法，其特征在于：步骤(3)的具体过程为：

惯性测量单元输出的数据经步骤(2)估计出的惯性测量单元安装误差造成的向心加速度的误差补偿后，通过三轴陀螺仪采集的数据计算出姿态矩阵，再结合三轴加速度计采集的数据，解算出旋转惯导系统输出的位置、速度信息；根据步骤(2)估计出的车轮旋转角度求出旋转坐标系到载体坐标系的转换矩阵，根据三轴陀螺仪采集的数据以及上一时刻的速度信息计算出旋转坐标下的姿态信息，从而解算出载体坐标系下的姿态信息。

3.根据权利要求2所述基于旋转调制和虚拟里程仪的车载自主导航方法，其特征在于：所述旋转坐标系到载体坐标系的转换矩阵为：

$C_r^b=\left[\begin{array}{ccc}1& 0& 0\\ 0& cos\left(\mathrm{\θ}\right)& -sin\left(\mathrm{\θ}\right)\\ 0& sin\left(\mathrm{\θ}\right)& \cos \left(\mathrm{\θ}\right)\end{array}\right]$

其中，θ为车轮的旋转角度。

4.根据权利要求1所述基于旋转调制和虚拟里程仪的车载自主导航方法，其特征在于：步骤(5)中选取的状态量为：

$X=[{\mathrm{\φ}}_e,{\mathrm{\φ}}_n,{\mathrm{\φ}}_u,\mathrm{\δ}{v}_e,\mathrm{\δ}{v}_n,\mathrm{\δ}{v}_u,\mathrm{\δL},\mathrm{\δ\λ},\mathrm{\δh},{\mathrm{\ϵ}}_{\mathrm{bx}},{\mathrm{\ϵ}}_{\mathrm{by}},{\mathrm{\ϵ}}_{\mathrm{bz}},{\mathrm{\ϵ}}_{\mathrm{rx}},{\mathrm{\ϵ}}_{\mathrm{ry}},{\mathrm{\ϵ}}_{\mathrm{rz}},{\▿}_x,{\▿}_y,{\▿}_z]$

其中，φ_e,φ_n,φ_u为平台误差角，δv_e,δv_n,δv_u为东、北、天方向速度误差，δL,δλ,δh为纬度、经度、高度误差，ε_bx,ε_by,ε_bz为旋转坐标系下陀螺仪的随机常值漂移，ε_rx,ε_ry,ε_rz为旋转坐标系下陀螺仪的一阶马尔可夫过程，为旋转坐标系下加速度计的一阶马尔可夫过程；

则状态方程为：

$\stackrel{\·}{X}=FX+GW$

其中，F为系统状态矩阵，G为误差系数矩阵，W为白噪声随机误差矢量。

5.根据权利要求4所述基于旋转调制和虚拟里程仪的车载自主导航方法，其特征在于：步骤(5)中选取的量测量为旋转惯导系统输出的速度信息与虚拟里程仪输出的速度信息的差值，则量测方程为：

Z＝HX+V

其中，H为量测矩阵，V为系统量测噪声。