CN103712620B

CN103712620B - 一种利用车体非完整性约束的惯性自主导航方法

Info

Publication number: CN103712620B
Application number: CN201310609301.XA
Authority: CN
Inventors: 高文劭
Original assignee: Beijing Machinery Equipment Research Institute
Current assignee: Beijing Machinery Equipment Research Institute
Priority date: 2013-11-27
Filing date: 2013-11-27
Publication date: 2016-03-30
Anticipated expiration: 2033-11-27
Also published as: CN103712620A

Abstract

本发明公开了一种利用车体非完整性约束的惯性自主导航方法。本方法利用在运动过程中车体非完整性约束，通过平滑惯性自主导航***输出的侧向和垂向速度累积，构建卡尔曼滤波观测方程，并利用卡尔曼滤波方程周期性计算出载车的导航参数误差并修正导航参数，从而实现了利用车体非完整性约束惯性导航***的自主导航。本发明有效的解决了惯性导航***自主导航精度发散的问题，有效的保证了惯性定位定向设备的精度。该方法使用方便，无需增加额外辅助设备。

Description

一种利用车体非完整性约束的惯性自主导航方法

技术领域

本发明涉及一种惯性自主导航方法，特别是一种利用车体非完整性约束的惯性自主导航方法。

背景技术

惯性导航***是一种直接利用惯性器件(陀螺仪和加速度计)的测量信息建立方向基准，并通过加速度信息自动完成解算，从而获得载车瞬时速度和位置的导航***。惯性导航***具有完全自主的特点，被广泛应用于陆海空天领域。由于惯性器件存在随机漂移，因此惯性导航***具有定向和定位误差随时间增长的特性。对于低精度器件(如MEMS)，其定位定向误差随时间增长的特性尤为明显，因此很难用于自主导航定位。通常，采用***辅助设备和惯性导航***组成组合导航***，以抑制惯性导航***的误差发散，实现导航定位的功能。

在载车运动条件下，采用低精度器件的惯性导航***需要与GPS、里程计、高程计等外部设备组成组合导航***。外部设备可以通过载车速度或位置等信息，建立惯性导航***在运动条件下的误差模型，采用卡尔曼滤波器实现对惯性导航***的导航信息误差的补偿。这种方法有效解决了惯性导航***误差发散的问题，能够实现高精度的定位定向。但是这也破坏了惯性导航***完全自主的特点，同时当***设备故障或无法工作时，惯性导航***还存在误差发散的问题，这些问题导致采用低精度器件的惯性导航***在运动条件下无法实现自主导航。

发明内容

本发明目的在于提供一种利用车体非完整性约束的惯性自主导航方法，解决由于采用低精度器件的惯性导航***在运动条件下无法自主导航的问题。

一种利用车体非完整性约束的惯性自主导航方法，其具体步骤为：

第一步构建利用车体非完整性约束的惯性自主导航***

利用车体非完整性约束的惯性自主导航***，包括：三轴硅微陀螺仪、三轴硅微加速度计、导航计算机、惯性导航解算模块、导航修正模块和通讯模块。三轴硅微陀螺仪和三轴硅微加速度计分别与导航计算机实现SPI总线连接。惯性导航解算模块、导航修正模块和通讯模块分别置于导航计算机中。

惯性导航解算模块用于利用采集的角速率和加速度通过数学运算得到载体在地理坐标系内的姿态、速度和位置。

导航修正模块用于利用车体非完整性约束构建观测方程，采用卡尔曼滤波方程周期性修正利用车体非完整性约束的惯性自主导航***的导航参数，即载车的姿态、速度和位置。

通讯模块用于利用车体非完整性约束的惯性自主导航***内外的数据采集和通讯。

第二步惯性导航解算模块对导航参数初始化

载车静止条件下，惯性导航解算模块接收来自三轴硅微陀螺仪和三轴硅微加速度计的数据，对三轴硅微陀螺仪和三轴硅微加速度计的零偏进行补偿，同时接收预存信息完成对惯性自主导航***姿态、速度和位置信息的初始化。

第三步惯性导航解算模块进行导航解算

载车启动后，惯性导航解算模块接收三轴硅微陀螺仪和三轴硅微加速度计的数据，计算出载车在地理坐标系内的姿态、速度和位置。

第四步导航修正模块进行修正准备

导航修正模块接收导航解算模块输出的速度，并利用姿态转换矩阵将接收到的速度转换为车体的侧向速度和垂向速度。

转换公式为：

{\hat{v}}_{b} = {({\hat{C}}_{b}^{n})}^{T} {\hat{v}}_{n} .

()^T表示向量转置，为三维向量，其第一项和第三项为侧向和垂向速度为惯性自主导航***的姿态转换矩阵；为惯性导航解算模块解算的速度，为三维向量，表示为

{\hat{v}}_{n} = {[\begin{matrix} v_{E}^{n} & v_{N}^{n} & v_{U}^{n} \end{matrix}]}^{T} .

导航修正模块同时计算载车运动情况下的***状态转移矩阵。

第五步导航修正模块利用车体非完整性约束计算观测向量和观测矩阵

车体在运动情况下，受非完整性约束的影响，其侧向和垂向速度为零，和就是车体的速度误差。

将

Z (k) = {[\begin{matrix} v_{x}^{b} & v_{z}^{b} \end{matrix}]}^{T}

作为***的观测向量。导航修正模块在修正时刻，计算从上一次修正时刻到本次修正时刻累积接收的车体的侧向速度和垂向速度之和，并计算这两项的平均值，将这两项作为和

计算滤波时刻的观测矩阵H(k)，

H (k) = [\begin{matrix} C_{13} v_{N}^{n} - C_{12} v_{U}^{n} & C_{11} v_{N}^{n} - C_{33} v_{E}^{n} & C_{12} v_{E}^{n} - C_{11} v_{N}^{n} & C_{11} & C_{12} & C_{13} & 0_{1 \times 9} \\ C_{33} v_{N}^{n} - C_{32} v_{U}^{n} & C_{31} v_{N}^{n} - C_{33} v_{E}^{n} & C_{32} v_{E}^{n} - C_{31} v_{N}^{n} & C_{31} & C_{32} & C_{33} & 0_{1 \times 9} \end{matrix}] .

其中C_ij为计算姿态矩阵的(i,j)项；和为惯性导航解算模块计算的速度分量的第一项和第三项。

第六步导航修正模块计算导航参数误差

导航修正模块利用计算得到的运动误差方程、观测向量、观测矩阵，采用卡尔曼滤波方程计算得到利用车体非完整性约束的惯性自主导航***的导航参数误差。

第七步惯性导航解算模块修正导航参数

惯性导航解算模块接收导航修正模块传递的导航参数误差，修正利用车体非完整性约束的惯性自主导航***的姿态、速度和位置误差，将导航参数误差和观测向量Z(k)值归零。

经过上述各步骤实现了利用车体非完整性约束的惯性自主导航。

本方法通过利用车体非完整性约束构建了***的观测向量，采用卡尔曼滤波器周期性计算出载车运动过程中的导航参数误差，并修正导航参数，从而实现了利用车体非完整性约束的惯性自主导航。本方法能有效克服惯性导航***由于器件零偏导致的导航误差累积，提高惯性导航***的导航精度，而且无需***辅助设备，仅需通过优化软件流程即可实现，实施简单方便。

具体实施方式

第一步构建利用车体非完整性约束的惯性自主导航***

第二步惯性导航解算模块对导航参数初始化

第三步惯性导航解算模块进行导航解算

第四步导航修正模块进行修正准备

转换公式为：

{\hat{v}}_{b} = {({\hat{C}}_{b}^{n})}^{T} {\hat{v}}_{n} .

{\hat{v}}_{n} = {[\begin{matrix} v_{E}^{n} & v_{N}^{n} & v_{U}^{n} \end{matrix}]}^{T} .

导航修正模块同时计算载车运动情况下的***状态转移矩阵。

将

Z (k) = {[\begin{matrix} v_{x}^{b} & v_{z}^{b} \end{matrix}]}^{T}

计算滤波时刻的观测矩阵H(k)，

H (k) = [\begin{matrix} C_{13} v_{N}^{n} - C_{12} v_{U}^{n} & C_{11} v_{N}^{n} - C_{33} v_{E}^{n} & C_{12} v_{E}^{n} - C_{11} v_{N}^{n} & C_{11} & C_{12} & C_{13} & 0_{1 \times 9} \\ C_{33} v_{N}^{n} - C_{32} v_{U}^{n} & C_{31} v_{N}^{n} - C_{33} v_{E}^{n} & C_{32} v_{E}^{n} - C_{31} v_{N}^{n} & C_{31} & C_{32} & C_{33} & 0_{1 \times 9} \end{matrix}] .

第六步导航修正模块计算导航参数误差

第七步惯性导航解算模块修正导航参数

Claims

1.一种利用车体非完整性约束的惯性自主导航方法，其特征在于具体步骤为：

第一步构建利用车体非完整性约束的惯性自主导航***

利用车体非完整性约束的惯性自主导航***，包括：三轴硅微陀螺仪、三轴硅微加速度计、导航计算机、惯性导航解算模块、导航修正模块和通讯模块；三轴硅微陀螺仪和三轴硅微加速度计分别与导航计算机实现SPI总线连接；惯性导航解算模块、导航修正模块和通讯模块分别置于导航计算机中；

惯性导航解算模块用于利用采集的角速率和加速度通过数学运算得到载体在地理坐标系内的姿态、速度和位置；

导航修正模块用于利用车体非完整性约束构建观测方程，采用卡尔曼滤波方程周期性修正利用车体非完整性约束的惯性自主导航***的导航参数，即载车的姿态、速度和位置；

通讯模块用于利用车体非完整性约束的惯性自主导航***内外的数据采集和通讯；

第二步惯性导航解算模块对导航参数初始化

载车静止条件下，惯性导航解算模块接收来自三轴硅微陀螺仪和三轴硅微加速度计的数据，对三轴硅微陀螺仪和三轴硅微加速度计的零偏进行补偿，同时接收预存信息完成对惯性自主导航***姿态、速度和位置信息的初始化；

第三步惯性导航解算模块进行导航解算

载车启动后，惯性导航解算模块接收三轴硅微陀螺仪和三轴硅微加速度计的数据，计算出载车在地理坐标系内的姿态、速度和位置；

第四步导航修正模块进行修正准备

导航修正模块接收导航解算模块输出的速度，并利用姿态转换矩阵将接收到的速度转换为车体的侧向速度和垂向速度；

转换公式为：

{\hat{v}}_{b} = {({\hat{C}}_{b}^{n})}^{T} {\hat{v}}_{n};

{\hat{v}}_{n} = {[\begin{matrix} v_{E}^{n} & v_{N}^{n} & v_{U}^{n} \end{matrix}]}^{T};

导航修正模块同时计算载车运动情况下的***状态转移矩阵；

车体在运动情况下，受非完整性约束的影响，其侧向和垂向速度为零，和就是车体的速度误差；

将

Z (k) = {[\begin{matrix} v_{x}^{b} & v_{z}^{b} \end{matrix}]}^{T}

作为***的观测向量；导航修正模块在修正时刻，计算从上一次修正时刻到本次修正时刻累积接收的车体的侧向速度和垂向速度之和，并计算这两项的平均值，将这两项作为和

计算滤波时刻的观测矩阵H(k)，

H (k) = [\begin{matrix} C_{13} v_{N}^{n} - C_{12} v_{U}^{n} & C_{11} v_{N}^{n} - C_{13} v_{E}^{n} & C_{12} v_{E}^{n} - C_{11} v_{N}^{n} & C_{11} & C_{12} & C_{13} & 0_{1 \times 9} \\ C_{33} v_{N}^{n} - C_{32} v_{U}^{n} & C_{31} v_{N}^{n} - C_{33} v_{E}^{n} & C_{32} v_{E}^{n} - C_{31} v_{N}^{n} & C_{31} & C_{32} & C_{33} & 0_{1 \times 9} \end{matrix}];

其中C_ij为计算姿态矩阵的(i,j)项；和为惯性导航解算模块计算的速度分量的第一项和第三项；

第六步导航修正模块计算导航参数误差

导航修正模块利用计算得到的运动误差方程、观测向量、观测矩阵，采用卡尔曼滤波方程计算得到利用车体非完整性约束的惯性自主导航***的导航参数误差；

第七步惯性导航解算模块修正导航参数

惯性导航解算模块接收导航修正模块传递的导航参数误差，修正利用车体非完整性约束的惯性自主导航***的姿态、速度和位置误差，将导航参数误差和观测向量Z(k)值归零；