CN105203129A - 一种惯导装置初始对准方法 - Google Patents

Abstract

本发明提供了一种惯导装置初始对准方法，包括如下步骤：提供一装载有传感器的器械，并对传感器进行预处理；进行相对对准，以求得传感器的安装误差角；进行绝对对准，以求得传感器的安装姿态角误差，进而提高相对对准计算出的误差姿态角精度。相对对准过程计算出相对的误差姿态角，相对误差姿态角作为绝对对准过程中状态向量中姿态误差的初始值从而可以加快卡尔曼滤波收敛速度，通过绝对对准过程可以进一步提高对准精度。

Description

一种惯导装置初始对准方法

技术领域

本发明针对农业机械和工程机械领域传统初始对准算法在较低成本和性能的惯导装置不适用的问题，设计发明了一种针对上述领域主要是农业机械的初始对准方法。

背景技术

随着MEMS(Micro-Electro-Mechanical-System)传感器、导航和控制技术的发展以及国家对农业扶持力度的进一步加大，精准农业正在快速变成一种趋势，而在农业机械辅助驾驶控制过程中，车体的姿态(包括俯仰角、翻滚角和航向角)、速度和位置信息能够实时反映出车体的运动和位置信息，这些信息能够为高精度的组合导航和控制算法提供重要的数据输入。

捷联惯性导航(Strapdown-Inertial-Navigation-System，SINS)具有自主导航、保密性好、抗干扰能力强、导航参数丰富和短时间内精度高等特点被广泛应用，但是由于惯性传感器固有误差的存在，使得导航误差随着时间积累长时间导航精度较差，需要其他误差稳定的导航系统辅助，例如高精度GPS-RTK。惯性导航系统是根据测得的车体加速度，经过积分运算求得速度和位置。为此，必须知道初始速度和位置。此外，以地理坐标系为导航坐标系的惯性系统中，物理平台和数学平台都是测量加速度的基准，而且平台必须准确地对准和跟踪地理坐标系，以避免平台误差引起加速度测量误差。初始对准的精度直接关系到导航系统的工作精度，也是重要关键技术之一。

目前常用的对准方法有粗对准和精对准两个步骤，但是这两种对准方法主要针对的是高精度的陀螺仪能够感应到地球自转角速度，明显在低成本和低性能的惯导装置中，这两种方法并不适用。

发明内容

本发明针对农业机械和工程机械领域传统初始对准算法在较低成本和性能的惯导装置不适用的问题，设计发明了一种针对上述领域主要是农业机械的初始对准方法。

本发明的技术方案为，一种惯导装置初始对准方法，包括如下步骤：

提供一装载有传感器的器械，并对传感器进行预处理；

进行相对对准处理，以求得传感器的安装误差角；

进行绝对对准处理，以求得传感器的安装姿态角误差，进而提高相对对准计算出的误差姿态角精度。

上述的方法，其中，对传感器进行预处理的步骤包括：

对传感器进行初始化设置；

对传感器进行滤波处理，以减小由于器械震动对传感器的影响。

上述的方法，其中，进行相对对准的步骤包括：

将器械朝一个方向静止，采集三轴加速度数据，然后将器械掉头180°，采集三轴加速度数据；

进行传感器的安装误差角度计算，求得传感器在不同方向上的安装误差角度。

上述的方法，其中，求得安装误差角度的公式为：

$A\·f_X^b+B\·f_Y^b+C\·f_Z^b=1$

$C=\frac{cos\mathrm{\γ}cos\mathrm{\θ}}{gcos\mathrm{\α}}$

其中，A、B、C为模型系数，分别为传感器中加速度计输出值，γ、θ、分别为传感器的安装误差角度，g为地球重力加速度，α为器械平面与水平面的夹角。

上述的方法，其中，进行最小二乘法数据拟合，以得到可计算出模型系数A、B、C的公式：

$\left[\begin{array}{c}A\\ B\\ C\end{array}\right]=\left[\begin{array}{ccc}\stackrel{n}{\Σ}{f}_{Xi}^2& \stackrel{n}{\Σ}{f}_{Xi}{f}_{Yi}& \stackrel{n}{\Σ}{f}_{Xi}{f}_{Zi}\\ \stackrel{n}{\Σ}{f}_{Xi}{f}_{Yi}& \stackrel{n}{\Σ}{f}_{Yi}^2& \stackrel{n}{\Σ}{f}_{Yi}{f}_{Zi}\\ \stackrel{n}{\Σ}{f}_{Xi}{f}_{Zi}& \stackrel{n}{\Σ}{f}_{Yi}{f}_{Zi}& \stackrel{n}{\Σ}{f}_{Zi}^2\end{array}\right];$

其中，分别为传感器中加速度计输出值，n为总的采样个数，i为采样点，i＝1,2.....,n，n为正整数。

上述的方法，其中，器械平面与水平面的夹角α计算方式为：

假设第一次器械停放的航向角为β，利用停放角度为β时的三轴加速度数据和(β+π)时的三轴加速度数据来求得器械平面与水平面的夹角α，α的求解公式为：

$cos\mathrm{\α}=\frac{\sqrt{{\left(f_X\right(\mathrm{\β})+f_X(\mathrm{\β}+\mathrm{\π}\left)\right)}^2+{\left(f_Y\right(\mathrm{\β})+f_Y(\mathrm{\β}+\mathrm{\π}\left)\right)}^2+{\left(f_Z\right(\mathrm{\β})+f_Z(\mathrm{\β}+\mathrm{\π}\left)\right)}^2}}{2g}.$

上述的方法，其中，进行绝对对准的步骤包括：

绝对对准卡尔曼滤波模型建立：

卡尔曼滤波状态向量： ${\mathrm{\δx}}^T=\left[\begin{array}{ccccc}{\left(\mathrm{\δ}p\right)}^T& {\left(\mathrm{\δ}v\right)}^T& {\left({\mathrm{\δ\ψ}}_{nb}\right)}^T& {\left(b_a^b\right)}^T& {\left(b_g^b\right)}^T\end{array}\right];$

其中，δx误差系统向量，δp、δv、δψ_nb分别为位置误差、速度误差和姿态角误差，和分别为加速度和陀螺仪零偏，T为转置；

卡尔曼滤波观测向量：y＝[p^Tv^T]；

其中，p为GPS位置信息，v为GPS速度信息；

卡尔曼滤波系统更新方程为

$\mathrm{\δ}\stackrel{\·}{p}={\mathrm{\δv}}^n$

$\mathrm{\δ}\stackrel{\·}{v}=\[{\left(C_b^n{f}^b\right)}^{^}\]{\mathrm{\δ\ψ}}_{nb}^n+C_b^n{\mathrm{\δf}}^b;$

$\mathrm{\δ}{\stackrel{\·}{\mathrm{\ψ}}}_{nb}^n\≈-C_b^n{\mathrm{\δ\ω}}_{ib}^b$

其中，分别对应为δp、δv、δψ_nb的导数，为器械坐标系到导航坐标系旋转矩阵，f^b、分别为传感器输出的加速度和角速度；

由相对对准中计算的初始姿态角α作为卡尔曼滤波状态向量中δψ_nb的初始值，然后按卡尔曼滤波系统更新方程来更新系统向量以及进行卡尔曼滤波，当卡尔曼滤波收敛后，计算出传感器的安装姿态角误差δψ_nb。

上述的方法，其中，所述器械为农用器械；

所述农用器械为拖拉机。

本发明通过相对对准和绝对对准，相对对准主要是解决MEMS-IMU传感器坐标系与拖拉机坐标系的对准，绝对对准解决的是拖拉机坐标系与导航坐标系的对准。相对对准过程计算出相对的误差姿态角，相对误差姿态角作为绝对对准过程中状态向量中姿态误差的初始值从而可以加快卡尔曼滤波收敛速度，通过绝对对准过程可以进一步提高对准精度。

附图说明

通过阅读参照以下附图对非限制性实施例所作的详细描述，本发明及其特征、外形和优点将会变得更明显。在全部附图中相同的标记指示相同的部分。并未刻意按照比例绘制附图，重点在于示出本发明的主旨。

图1为本发明公开的一种惯导装置初始对准方法的示意图；

图2为本发明进行相对对准处理的流程图；

图3为本发明进行绝对对准处理的流程图。

具体实施方式

在下文的描述中，给出了大量具体的细节以便提供对本发明更为彻底的理解。然而，对于本领域技术人员而言显而易见的是，本发明可以无需一个或多个这些细节而得以实施。在其他的例子中，为了避免与本发明发生混淆，对于本领域公知的一些技术特征未进行描述。

为了彻底理解本发明，将在下列的描述中提出详细的步骤以及详细的结构，以便阐释本发明的技术方案。本发明的较佳实施例详细描述如下，然而除了这些详细描述外，本发明还可以具有其他实施方式。

本发明提供了一种惯导装置初始对准方法，参照图1所示，主要包括如下步骤：

步骤S1：提供一装载有传感器的器械，并对传感器进行预处理。

在发明一可选的实施例中，对传感器进行预处理的步骤包括：步骤S1a：对传感器进行初始化设置；步骤S1b：对传感器进行滤波处理，也即图示的特殊处理，以减小由于器械震动对传感器的影响。

步骤S2：进行相对对准处理，以求得传感器的安装误差角。

在本发明一可选的实施例中，结合图2所示，进行相对对准处理的步骤包括：首先将器械朝一个方向静止，采集三轴加速度数据；然后将器械掉头180°，采集三轴加速度数据；进行传感器的安装误差角度计算，求得传感器在不同方向上的安装误差角度。

其中，求得安装误差角度的公式为：

$A\·f_X^b+B\·f_Y^b+C\·f_Z^b=1$

$C=\frac{cos\mathrm{\γ}cos\mathrm{\θ}}{gcos\mathrm{\α}}$

其中，A、B、C为模型系数，分别为传感器中加速度计输出值，γ、θ、分别为传感器的安装误差角度，g为地球重力加速度，α为器械平面与水平面的夹角。

进行最小二乘法数据拟合，以得到可计算出模型系数A、B、C的公式：

$\left[\begin{array}{c}A\\ B\\ C\end{array}\right]=\left[\begin{array}{ccc}\stackrel{n}{\mathrm{\Σ}}{f}_{\mathrm{Xi}}^2& \stackrel{n}{\mathrm{\Σ}}{f}_{\mathrm{Xi}}{f}_{\mathrm{Yi}}& \stackrel{n}{\mathrm{\Σ}}{f}_{\mathrm{Xi}}{f}_{\mathrm{Zi}}\\ \stackrel{n}{\mathrm{\Σ}}{f}_{\mathrm{Xi}}{f}_{\mathrm{Yi}}& \stackrel{n}{\mathrm{\Σ}}{f}_{\mathrm{Yi}}^2& \stackrel{n}{\mathrm{\Σ}}{f}_{\mathrm{Yi}}{f}_{\mathrm{Zi}}\\ \stackrel{n}{\mathrm{\Σ}}{f}_{\mathrm{Xi}}{f}_{\mathrm{Zi}}& \stackrel{n}{\mathrm{\Σ}}{f}_{\mathrm{Yi}}{f}_{\mathrm{Zi}}& \stackrel{n}{\mathrm{\Σ}}{f}_{\mathrm{Zi}}^2\end{array}\right];$ ②

其中，分别为传感器中加速度计输出值，n为总的采样个数，i为采样点，i＝1,2.....,n，n为正整数，例如n＝8，9，10，12……。

器械平面与水平面的夹角α计算方式为：

假设第一次器械停放的航向角为β，利用停放角度为β时的三轴加速度数据和(β+π)时的三轴加速度数据来求得器械平面与水平面的夹角α，求解公式为：

$\cos \mathrm{\α}=\frac{\sqrt{{(f_X\left(\mathrm{\β}\right)+f_X(\mathrm{\β}+\mathrm{\π}))}^2+{(f_Y\left(\mathrm{\β}\right)+f_Y(\mathrm{\β}+\mathrm{\π}))}^2+{(f_Z\left(\mathrm{\β}\right)+f_Z(\mathrm{\β}+\mathrm{\π}))}^2}}{2g};$ ③

利用公式②、公式③可求得A、B、C和α，然后代入公式①即可求得安装误差角γ、θ、

步骤S3：进行绝对对准处理，以求得传感器的安装姿态角误差，进而提高相对对准计算出的误差姿态角精度。

在本发明中，结合图3所示，进行绝对对准处理的步骤包括：

首先进行绝对对准卡尔曼滤波(kalmanfilter)模型建立：

卡尔曼滤波状态向量： ${\mathrm{\δx}}^T=\left[\begin{array}{ccccc}{\left(\mathrm{\δp}\right)}^T& {\left(\mathrm{\δv}\right)}^T& {\left({\mathrm{\δ\ψ}}_{\mathrm{nb}}\right)}^T& {\left(b_a^b\right)}^T& {\left(b_g^b\right)}^T\end{array}\right];$ ④

其中，δx误差系统向量，δp、δv、δψ_nb分别为位置误差、速度误差和姿态角误差，和分别为加速度和陀螺仪零偏，T为转置；

之后，获取卡尔曼滤波观测向量：y＝[p^Tv^T]；⑤

其中，p为GPS位置信息，v为GPS速度信息；

再者，建立卡尔曼滤波系统的更新方程：

$\mathrm{\δ}\stackrel{\·}{p}={\mathrm{\δv}}^n$

$\mathrm{\δ}\stackrel{\·}{v}=\left[{\left(C_b^n{f}^b\right)}^{^}\right]{\mathrm{\δ\ψ}}_{\mathrm{nb}}^n+C_b^n{\mathrm{\δf}}^b;$ ⑥

$\mathrm{\δ}{\stackrel{\·}{\mathrm{\ψ}}}_{nb}^n\≈-C_b^n{\mathrm{\δ\ω}}_{ib}^b$

其中，分别对应为δp、δv、δψ_nb的导数，为器械坐标系到导航坐标系旋转矩阵，f^b、分别为传感器输出的加速度和角速度；

由相对对准中的公式③计算的初始姿态角α作为卡尔曼滤波状态向量(或称Kalman状态向量)中δψ_nb的初始值，然后按公式⑥更新卡尔曼滤波系统向量以及进行kalman滤波；

当kalman收敛后，计算输出的姿态角为较为准确的传感器的安装姿态角误差δψ_nb，即对准角度。本发明通过该安装姿态角误差δψ_nb可进一步提高相对对准计算出的误差姿态角精度。

在本发明一可选的实施例中，器械为农用器械，例如农用的拖拉机。

本发明的农业机械低成本惯导装置(三轴加速度计和三轴陀螺仪)初始对准实现方法分为相对对准和绝对对准两个方面，相对对准主要是解决MEMS-IMU传感器坐标系与拖拉机坐标系的对准，绝对对准解决的是拖拉机坐标系与导航坐标系的对准。相对对准过程计算出相对的误差姿态角，相对误差姿态角作为绝对对准过程中状态向量中姿态误差的初始值从而可以加快卡尔曼滤波收敛速度，通过绝对对准过程可以进一步提高对准精度。

由于本发明采用了如上技术方案，具有如下优点：

(1)本发明实现的惯导初始对准方法可解决传统对准方法中对高精度陀螺仪的要求；

(2)本发明主要不需要借助其他外围设备即可完成对准，对准精度较高；

(3)经本发明的惯导初始对准方法可加快kalman滤波的收敛速度且能大大降低随时间积累的姿态角、速度和位置误差；

以上对本发明的较佳实施例进行了描述。需要理解的是，本发明并不局限于上述特定实施方式，其中未尽详细描述的设备和结构应该理解为用本领域中的普通方式予以实施；任何熟悉本领域的技术人员，在不脱离本发明技术方案范围情况下，都可利用上述揭示的方法和技术内容对本发明技术方案做出许多可能的变动和修饰，或修改为等同变化的等效实施例，这并不影响本发明的实质内容。因此，凡是未脱离本发明技术方案的内容，依据本发明的技术实质对以上实施例所做的任何简单修改、等同变化及修饰，均仍属于本发明技术方案保护的范围内。

Claims (8)

1.一种惯导装置初始对准方法，其特征在于，包括如下步骤：

提供一装载有传感器的器械，并对传感器进行预处理；

进行相对对准，以求得传感器的安装误差角；

进行绝对对准，以求得传感器的安装姿态角误差，进而提高相对对准计算出的误差姿态角精度。

2.如权利要求1所述的方法，其特征在于，对传感器进行预处理的步骤包括：

对传感器进行初始化设置；

对传感器进行滤波处理，以减小由于器械震动对传感器的影响。

3.如权利要求1所述的方法，其特征在于，进行相对对准的步骤包括：

将器械朝一个方向静止，采集三轴加速度数据，然后将器械掉头180°，采集三轴加速度数据；

进行传感器的安装误差角度计算，求得传感器在不同方向上的安装误差角度。

4.如权利要求3所述的方法，其特征在于，求得安装误差角度的公式为：

$A\·f_X^b+B\·f_Y^b+C\·f_Z^b=1$

$C=\frac{cos\mathrm{\γ}cos\mathrm{\θ}}{gcos\mathrm{\α}}$

其中，A、B、C为模型系数，分别为传感器中加速度计输出值，γ、θ、分别为传感器的安装误差角度，g为地球重力加速度，α为器械平面与水平面的夹角。

5.如权利要求4所述的方法，其特征在于，进行最小二乘法数据拟合，以得到可计算出模型系数A、B、C的公式：

$\left[\begin{array}{c}A\\ B\\ C\end{array}\right]=\left[\begin{array}{ccc}\stackrel{n}{\mathrm{\Σ}}{f}_{Xi}^2& \stackrel{n}{\mathrm{\Σ}}{f}_{Xi}{f}_{Yi}& \stackrel{n}{\mathrm{\Σ}}{f}_{Xi}{f}_{Zi}\\ \stackrel{n}{\mathrm{\Σ}}{f}_{Xi}{f}_{Yi}& \stackrel{n}{\mathrm{\Σ}}{f}_{Yi}^2& \stackrel{n}{\mathrm{\Σ}}{f}_{Yi}{f}_{Zi}\\ \stackrel{n}{\mathrm{\Σ}}{f}_{Xi}{f}_{Zi}& \stackrel{n}{\mathrm{\Σ}}{f}_{Yi}{f}_{Zi}& \stackrel{n}{\mathrm{\Σ}}{f}_{Zi}^2\end{array}\right];$

其中，分别为传感器中加速度计输出值，n为总的采样个数，i为采样点，i＝1,2.....,n，n为正整数。

6.如权利要求4所述的方法，其特征在于，器械平面与水平面的夹角α计算方式为：

假设第一次器械停放的航向角为β，利用停放角度为β时的三轴加速度数据和(β+π)时的三轴加速度数据来求得器械平面与水平面的夹角α，α的求解公式为：

$cos\mathrm{\α}=\frac{\sqrt{{\left(f_X\right(\mathrm{\β})+f_X(\mathrm{\β}+\mathrm{\π}\left)\right)}^2+{\left(f_Y\right(\mathrm{\β})+f_Y(\mathrm{\β}+\mathrm{\π}\left)\right)}^2+{\left(f_Z\right(\mathrm{\β})+f_Z(\mathrm{\β}+\mathrm{\π}\left)\right)}^2}}{2g}.$

7.如权利要求6所述的方法，其特征在于，进行绝对对准的步骤包括：

绝对对准卡尔曼滤波模型建立：

卡尔曼滤波状态向量： ${\mathrm{\δx}}^T=\left[\begin{array}{ccccc}{\left(\mathrm{\δ}p\right)}^T& {\left(\mathrm{\δ}v\right)}^T& {\left({\mathrm{\δ\ψ}}_{nb}\right)}^T& {\left(b_a^b\right)}^T& {\left(b_g^b\right)}^T\end{array}\right];$

其中，δx误差系统向量，δp、δv、δψ_nb分别为位置误差、速度误差和姿态角误差，和分别为加速度和陀螺仪零偏，T为转置；

卡尔曼滤波观测向量：y＝[p^Tv^T]；

其中，p为GPS位置信息，v为GPS速度信息；

卡尔曼滤波系统更新方程为

$\mathrm{\δ}\stackrel{\·}{p}={\mathrm{\δv}}^n$

$\mathrm{\δ}\stackrel{\·}{v}=\[\left(C_b^n{f}^b\right)^\]{\mathrm{\δ\ψ}}_{nb}^n+C_b^n{\mathrm{\δf}}^b;$

$\mathrm{\δ}{\stackrel{\·}{\mathrm{\ψ}}}_{nb}^n\≈-C_b^n{\mathrm{\δ\ω}}_{ib}^b$

其中，分别对应为δp、δv、δψ_nb的导数，为器械坐标系到导航坐标系旋转矩阵，f^b、分别为传感器输出的加速度和角速度；

由相对对准中计算的初始姿态角α作为卡尔曼滤波状态向量中δψ_nb的初始值，然后按卡尔曼滤波系统更新方程来更新系统向量以及进行卡尔曼滤波，当卡尔曼滤波收敛后，计算出传感器的安装姿态角误差δψ_nb。

8.如权利要求1所述的方法，其特征在于，所述器械为农用器械；

所述农用器械为拖拉机。