一种惯导装置初始对准方法
【技术领域】
[0001] 本发明针对农业机械和工程机械领域传统初始对准算法在较低成本和性能的惯 导装置不适用的问题，设计发明了一种针对上述领域主要是农业机械的初始对准方法。
【背景技术】
[0002] 随着MEMS(Micro-Electro-Mechanical-System)传感器、导航和控制技术的发展 以及国家对农业扶持力度的进一步加大，精准农业正在快速变成一种趋势，而在农业机械 辅助驾驶控制过程中，车体的姿态（包括俯仰角、翻滚角和航向角）、速度和位置信息能够 实时反映出车体的运动和位置信息，这些信息能够为高精度的组合导航和控制算法提供重 要的数据输入。
[0003] 捷联惯性导航（Strapdown-Inertial-Navigation-System，SINS)具有自主导航、 保密性好、抗干扰能力强、导航参数丰富和短时间内精度高等特点被广泛应用，但是由于惯 性传感器固有误差的存在，使得导航误差随着时间积累长时间导航精度较差，需要其他误 差稳定的导航系统辅助，例如高精度GPS-RTK。惯性导航系统是根据测得的车体加速度，经 过积分运算求得速度和位置。为此，必须知道初始速度和位置。此外，以地理坐标系为导航 坐标系的惯性系统中，物理平台和数学平台都是测量加速度的基准，而且平台必须准确地 对准和跟踪地理坐标系，以避免平台误差引起加速度测量误差。初始对准的精度直接关系 到导航系统的工作精度，也是重要关键技术之一。
[0004] 目前常用的对准方法有粗对准和精对准两个步骤，但是这两种对准方法主要针对 的是高精度的陀螺仪能够感应到地球自转角速度，明显在低成本和低性能的惯导装置中， 这两种方法并不适用。
【发明内容】
[0005] 本发明针对农业机械和工程机械领域传统初始对准算法在较低成本和性能的惯 导装置不适用的问题，设计发明了一种针对上述领域主要是农业机械的初始对准方法。
[0006] 本发明的技术方案为，一种惯导装置初始对准方法，包括如下步骤：
[0007] 提供一装载有传感器的器械，并对传感器进行预处理；
[0008] 进行相对对准处理，以求得传感器的安装误差角；
[0009] 进行绝对对准处理，以求得传感器的安装姿态角误差，进而提高相对对准计算出 的误差姿态角精度。
[0010] 上述的方法，其中，对传感器进行预处理的步骤包括：
[0011] 对传感器进行初始化设置；
[0012] 对传感器进行滤波处理，以减小由于器械震动对传感器的影响。
[0013] 上述的方法，其中，进行相对对准的步骤包括：
[0014] 将器械朝一个方向静止，采集三轴加速度数据，然后将器械掉头180°，采集三轴 加速度数据；
[0015] 进行传感器的安装误差角度计算，求得传感器在不同方向上的安装误差角度。
[0016] 上述的方法，其中，求得安装误差角度的公式为：
[0017]
[0018]
[0019]
[0020]
[0021] 其中，A、B、C为模型系数，力、#、.及分别为传感器中加速度计输出值，Y、0、 梦分别为传感器的安装误差角度，g为地球重力加速度，a为器械平面与水平面的夹角。
[0022] 上述的方法，其中，进行最小二乘法数据拟合，以得到可计算出模型系数A、B、C的 公式：
[0023]
[0024] 其中，方、#、及分别为传感器中加速度计输出值，n为总的采样个数，i为采 样点，i= 1，2.....，n，n为正整数。
[0025] 上述的方法，其中，器械平面与水平面的夹角a计算方式为：
[0026]假设第一次器械停放的航向角为0，利用停放角度为0时的三轴加速度数据和 (0 + 31)时的三轴加速度数据来求得器械平面与水平面的夹角a，a的求解公式为：
[0027]
a
[0028] 上述的方法，其中，进行绝对对准的步骤包括：
[0029] 绝对对准卡尔曼滤波模型建立：
[0030] 卡尔曼滤波状态向量
[0031] 其中，SX误差系统向量，Sp、SV、S!tnb分别为位置误差、速度误差和姿态角误 差，域和^{分别为加速度和陀螺仪零偏，T为转置；
[0032] 卡尔曼滤波观测向量：y= [pTvT];
[0033] 其中，p为GPS位置信息，v为GPS速度信息；
[0034] 卡尔曼滤波系统更新方程为
[0035]
[0036]
[0037]
[0038] 其中，多1、彡[、分别对应为Sp、Sv、Sil)nb的导数为器械坐标系到导 航坐标系旋转矩阵，^分别为传感器输出的加速度和角速度；
[0039] 由相对对准中计算的初始姿态角a作为卡尔曼滤波状态向量中S也nb的初始值， 然后按卡尔曼滤波系统更新方程来更新系统向量以及进行卡尔曼滤波，当卡尔曼滤波收敛 后，计算出传感器的安装姿态角误差S
[0040] 上述的方法，其中，所述器械为农用器械；
[0041] 所述农用器械为拖拉机。
[0042] 本发明通过相对对准和绝对对准，相对对准主要是解决MEMS-頂U传感器坐标系 与拖拉机坐标系的对准，绝对对准解决的是拖拉机坐标系与导航坐标系的对准。相对对准 过程计算出相对的误差姿态角，相对误差姿态角作为绝对对准过程中状态向量中姿态误差 的初始值从而可以加快卡尔曼滤波收敛速度，通过绝对对准过程可以进一步提高对准精 度。
【附图说明】
[0043] 通过阅读参照以下附图对非限制性实施例所作的详细描述，本发明及其特征、外 形和优点将会变得更明显。在全部附图中相同的标记指示相同的部分。并未刻意按照比例 绘制附图，重点在于示出本发明的主旨。
[0044] 图1为本发明公开的一种惯导装置初始对准方法的示意图；
[0045] 图2为本发明进行相对对准处理的流程图；
[0046] 图3为本发明进行绝对对准处理的流程图。
【具体实施方式】
[0047] 在下文的描述中，给出了大量具体的细节以便提供对本发明更为彻底的理解。然 而，对于本领域技术人员而言显而易见的是，本发明可以无需一个或多个这些细节而得以 实施。在其他的例子中，为了避免与本发明发生混淆，对于本领域公知的一些技术特征未进 行描述。
[0048] 为了彻底理解本发明，将在下列的描述中提出详细的步骤以及详细的结构，以便 阐释本发明的技术方案。本发明的较佳实施例详细描述如下，然而除了这些详细描述外，本 发明还可以具有其他实施方式。
[0049] 本发明提供了一种惯导装置初始对准方法，参照图1所示，主要包括如下步骤：
[0050] 步骤S1 :提供一装载有传感器的器械，并对传感器进行预处理。
[0051] 在发明一可选的实施例中，对传感器进行预处理的步骤包括：步骤Sla:对传感器 进行初始化设置；步骤Sib:对传感器进行滤波处理，也即图示的特殊处理，以减小由于器 械震动对传感器的影响。
[0052] 步骤S2:进行相对对准处理，以求得传感器的安装误差角。
[0053] 在本发明一可选的实施例中，结合图2所示，进行相对对准处理的步骤包括：首先 将器械朝一个方向静止，采集三轴加速度数据；然后将器械掉头180°，采集三轴加速度数 据；进行传感器的安装误差角度计算，求得传感器在不同方向上的安装误差角度