用于农业机械的捷联惯导系统以及控制方法
【技术领域】
[0001] 本发明涉及机械控制领域，尤其涉及高精度机械控制，具体是指一种用于农业机 械的捷联惯导系统以及控制方法。
【背景技术】
[0002] 随着MEMS (Micro-Electro-Mechanical-System)传感器、导航和控制技术的发展 以及国家对农业扶持力度的进一步加大，精准农业正在快速变成一种趋势，而在农业机械 辅助驾驶控制过程中，车体的姿态（包括俯仰角、翻滚角和航向角）、速度和位置信息能够 实时反映出车体的运动和位置信息，这些信息能够为高精度的组合导航和控制算法提供重 要的数据输入。
[0003] 在农业机械辅助驾驶控制过程中，车体在导航坐标系中的位置信息和航向信息是 最重要的两个参数。农业机械辅助驾驶系统中常用的定位方式有：机械触觉、推算定位、机 器视觉、激光定位和多传感器信息融合（IMU+GPS)。多传感器信息融合充分利用多个传感器 资源，通过对这些传感器及其观测信息的合理支配和利用，把多个传感器在空间或时间上 的冗余或互补信息依据某种准则来进行组合，以获得被测对象的一致性解释或描述，此种 方法相对于其它几种具有定位精度高、体积小和较高的性价比。
[0004] 多传感器信息融合中位置信息的获取主要依据高精度的GPS定位信息（如RTK)， 但是GPS数据输出精度和输出频率与价格成正比，而且在有障碍物遮挡或者天气等原因 时，GPS不能保证有效的数据输出，此时需要一套高频高精度的数据采集和计算系统为这种 盲区实时提供数据填补。
[0005]目前，惯性导航系统分为 PINS (Platform Inertial Navigation System)和 SINS (Strapdown Inertial Navigation System), SINS 相比于 PINS 是米用 IMU (Inertial Measuring Unit)传感器通过计算建立一个"数学平台"来代替PINS。SINS多使用在飞行 器导航控制系统中，而针对农业机械控制领域的研究和应用则属于起步阶段，而且二者的 应用对象和环境条件有较大的差别，飞行器控制系统中捷联惯导实现的方法在农业机械控 制中不尽适用。
【发明内容】
[0006] 本发明的目的是克服了上述现有技术的缺点，提供了一种能够实现的用于农业机 械的捷联惯导系统以及控制方法。
[0007] 为了实现上述目的，本发明的用于农业机械的捷联惯导系统以及控制方法具有如 下构成：
[0008] 该用于农业机械的捷联惯导系统，其主要特点是，所述的系统包括六轴惯性传感 器、中央控制器；所述的六轴惯性传感器包括三个方向的加速度传感器以及三轴的陀螺仪 传感器；所述的中央控制器包括：
[0009] 坐标变换模块，用以将所述的加速度传感器发送的所述的农业机械的载体坐标系 的加速度转换为航天坐标系的加速度；
[0010] 速度位置计算模块，用以根据所述的坐标变换模块输出的航天坐标系的加速度计 算获得所述的农业机械的速度信息以及位置信息；
[0011] 姿态矩阵计算模块，用以根据所述的陀螺仪传感器发送的所述的农业机械的角速 度值以及所述的速度位置计算模块计算的位置角速度值计算获得更新后的姿态矩阵；以及
[0012] 姿态计算模块，用以根据所述的姿态矩阵计算模块的更新后的姿态矩阵获得所述 的农业机械的姿态角。
[0013] 本发明还涉及一种实现农业机械的控制方法，其主要特点是，所述的方法包括以 下步骤：
[0014] (1)所述的加速度传感器实时地将所述的农业机械的加速度发送至所述的坐标变 换模块，且同时所述的陀螺仪传感器实时地将所述的农业机械的角速度发送至所述的姿态 矩阵计算模块；
[0015] (2)所述的姿态矩阵计算模块根据所述的速度位置计算模块计算的位置角速度值 以及所接收的陀螺仪传感器的角速度值计算获得更新后的姿态矩阵，并将更新后的姿态矩 阵发送至坐标变换模块；
[0016] (3)所述的坐标变换模块根据更新后的姿态矩阵将所述的加速度传感器发送的所 述的农业机械的载体坐标系的加速度转换为航天坐标系的加速度；
[0017] (4)所述的速度位置计算模块根据所述的坐标变换模块输出的航天坐标系的加速 度输出所述的农业机械的位置信息以及速度信息，且所述的姿态计算模块根据所述的姿态 矩阵计算模块的更新后的姿态矩阵输出所述的农业机械的姿态角；
[0018] (5)所述的农业机械的控制中心根据所述的位置信息、速度信息以及姿态角控制 所述的农业机械。
[0019] 进一步地，导航坐标系为"东北天"地理坐标系，所述的姿态矩阵计算模块根据所 述的速度位置计算模块计算的位置角速度值以及所接收的陀螺仪传感器的角速度值计算 获得更新后的姿态矩阵，具体包括以下步骤：
[0020] (2. 1)所述的姿态矩阵计算模块根据所述的速度位置计算模块计算的位置角速度 值以及所接收的陀螺仪传感器的角速度值计算姿态速率；
[0021] (2. 2)所述的姿态矩阵计算模块根据所述的姿态速率求解四元数微分方程以获得 第一姿态矩阵；
[0022] (2. 3)所述的姿态矩阵计算模块将所述的第一姿态矩阵归一化后获得更新后的姿 态矩阵。
[0023] 更进一步地，所述的步骤（2. 1)具体为：
[0024] 所述的姿态矩阵计算模块通过以下公式获得姿态速率：
[0025]
【主权项】
1. 一种用于农业机械的捷联惯导系统，其特征在于，所述的系统包括六轴惯性传感器、 中央控制器；所述的六轴惯性传感器包括三个方向的加速度传感器以及三轴的陀螺仪传感 器；所述的中央控制器包括： 坐标变换模块，用以将所述的加速度传感器发送的所述的农业机械的载体坐标系的加 速度转换为航天坐标系的加速度； 速度位置计算模块，用以根据所述的坐标变换模块输出的航天坐标系的加速度计算获 得所述的农业机械的速度信息以及位置信息； 姿态矩阵计算模块，用以根据所述的陀螺仪传感器发送的所述的农业机械的角速度值 以及所述的速度位置计算模块计算的位置角速度值计算获得更新后的姿态矩阵；以及 姿态计算模块，用以根据所述的姿态矩阵计算模块的更新后的姿态矩阵获得所述的农 业机械的姿态角。
2. -种基于权利要求1所述的系统实现农业机械的控制方法，其特征在于，所述的方 法包括以下步骤： (1) 所述的加速度传感器实时地将所述的农业机械的加速度发送至所述的坐标变换模 块，且同时所述的陀螺仪传感器实时地将所述的农业机械的角速度发送至所述的姿态矩阵 计算模块； (2) 所述的姿态矩阵计算模块根据所述的速度位置计算模块计算的位置角速度值以及 所接收的陀螺仪传感器的角速度值计算获得更新后的姿态矩阵，并将更新后的姿态矩阵发 送至坐标变换模块； (3) 所述的坐标变换模块根据更新后的姿态矩阵将所述的加速度传感器发送的所述的 农业机械的载体坐标系的加速度转换为航天坐标系的加速度； (4) 所述的速度位置计算模块根据所述的坐标变换模块输出的航天坐标系的加速度输 出所述的农业机械的位置信息以及速度信息，且所述的姿态计算模块根据所述的姿态矩阵 计算模块的更新后的姿态矩阵输出所述的农业机械的姿态角； (5) 所述的农业机械的控制中心根据所述的位置信息、速度信息以及姿态角控制所述 的农业机械。
3. 根据权利要求2所述的实现农业机械的控制方法，其特征在于，导航坐标系为"东北 天"地理坐标系，所述的姿态矩阵计算模块根据所述的速度位置计算模块计算的位置角速 度值以及所接收的陀螺仪传感器的角速度值计算获得更新后的姿态矩阵，具体包括以下步 骤： (2.1)所述的姿态矩阵计算模块根据所述的速度位置计算模块计算的位置角速度值以 及所接收的陀螺仪传感器的角速度值计算姿态速率； (2.2)所述的姿态矩阵计算模块根据所述的姿态速率求解四元数微分方程以获得第一 姿态矩阵； (2. 3)所述的姿态矩阵计算模块将所述的第一姿态矩阵归一化后获得更新后的姿态矩 阵。
4. 根据权利要求3所述的实现农业机械的控制方法，其特征在于，所述的步骤（2. 1)具 体为： 所述的姿态矩阵计算模块通过以下公式获得姿态速率： (1) 其中，Ct是欧拉角表示的姿态矩阵，即
为所述的陀螺仪传感器输出的所述的农业机械的角速度，f
4为地球 角速度，是已知的，）
，航坐标系相对地球的角速