一种基于GPS和INS组合导航的整平机装置的制作方法

本实用新型涉及定位和导航领域，具体涉及一种基于GPS和INS组合导航的整平机装置。

背景技术：

水稻为全球人口提供主要的粮食来源，也是亚洲地区最重要的粮食作物，随着现代社会工业化和城镇化的迅速发展，种植水稻的劳动力大量向二、三产业转移，传统的以人畜作为动力的生产方式已无法满足水稻产量供给的需求。美国、欧洲（意大利、西班牙等国家）和澳大利亚等工业发达国家已经实现利用无人机进行地形勘探和直播种子的全自动化过程。亚洲国家多以移栽和传统方式种植水稻，其中日本和韩国在水稻种植方面的自动化程度较高。目前我国的农田灌溉又多以地面灌溉为主。由于水田表面高低不平，造成了水资源的严重浪费，所以高精度激光整平机的设计与研制就显得十分重要。

近年来我国水稻生产机械化发展迅速，土地平整技术的发展将小块的水田整合成大块的新水田进行集中作业。相比较于之前人工作业的粗整平，激光整平机可实现三倍平整精度以及密实度上的改进，并且提高了工作效率。由于整平机的许多施工地点都在交通不方便的地区，使用无人机勘探地形、播种种子时快速、准确获取无人机的三维坐标信息显得尤为重要。农业上对整平机进行平地作业的劳动补贴时，需要对整平机进行跟踪，准确获知整平机实时位置从而可以根据工作距离来进行补贴。此外，施工现场整平机出现故障和被盗的情况时有发生，如果无法获知整平机的位置信息，那么在经济和工作效率上都会遭受损失。如果施工单位恶意欠款造成设备无法收回，设备的生产销售公司就会遭受损失，然而目前我国对整平机的水平组合导航定位还没有进行相关方面的研究，而整平机在农业和工业上越来越得到广泛应用因此对整平机的水平定位就显得尤为重要。

考虑到整平机工作环境，采用卫星定位技术遇到复杂地形时GPS信号易失锁，C/A码（粗捕获码）和P码（精确码）的能量集中在中心频率附近，定位精度易受电子欺骗的影响，迫切需要加以改进。

技术实现要素：

为解决上述问题，本实用新型提供了一种基于GPS和INS组合导航的整平机装置，本实用新型组合了各自存在优缺点的全球定位系统和惯性导航，使得本实用新型输出的姿态角更加精确，减轻了劳动人员的工作量、提高了工作效率，实现了整平机的水平定位。

为实现所述技术目的，本实用新型的技术方案是：一种基于GPS和INS组合导航的整平机装置，包括：整平机及其平地铲、GPS导航模块，INS惯性导航模块；

所述INS惯性导航模块安装固定于所述平地铲上，所述GPS导航模块安装于整平机机身上；设置主控制器连接GPS导航模块和INS惯性导航模块，用于对GPS导航模块和INS惯性导航模块所测量的数据进行融合。

进一步，所述INS惯性导航模块外连接磁力计，所述INS惯性导航模块用于补偿磁力计的测量值。

进一步，所述INS惯性导航模块使用陀螺仪和加速度计融合的型号为MPU6050的6轴运动处理芯片，且通过I2C接口连接所述主控制器。陀螺仪和加速度计选用MPU6050模块，提供俯仰角和横滚角，为磁力计提供水平的测量环境，且磁力计测量返回值于主控制器；同时给出整平机偏离北向的角度，得出当地精确的磁场信息去补偿陀螺仪。

进一步，所述磁力计采用型号为HMC5983L的霍尼韦尔磁阻式三轴地磁传感器，且通过I2C接口连接所述主控制器。

进一步，所述GPS导航模块采用型号为MAX-6Q的GPS芯片，且通过I2C接口连接所述主控制器。

进一步，所述主控制器为型号为STM32F103TB的单片机最小控制系统。

进一步，所述主控制器外设置气压计。

作为本实用新型的优选，所述气压计采用型号为BMP180的气压计芯片，且通过串口连接所述主控制器。

本实用新型的有益效果在于：

1）本实用新型在控制器内部融合磁力计，气压计，陀螺仪和加速度计等传感器的数据后再进行GPS导航模块与INS惯性导航模块的数据融合，得出误差的最优估计。将整平机的位置信息实时发送到整平机搭载的上位机中，实现了整平机的水平定位。

2）本实用新型INS惯性导航模块提供的姿态角里的俯仰角可以用来代替整平机调平杆上的倾角传感器，保证高精度的同时有效降低了成本，使输出的姿态角更加精确。

3）本实用新型的航向角用来保证整平机始终处于直线运动状态，用以减轻劳动人员的工作量、提高工作效率。

综上，本实用新型组合了各自存在优缺点的全球定位系统和惯性导航，使得本实用新型输出的姿态角更加精确，减轻了劳动人员的工作量、提高了工作效率，实现了整平机的水平定位。

附图说明

图1是本实用新型的电路模块原理；

图2是本实用新型的GPS导航模块电路原理图；

图3是本实用新型的INS惯性导航模块电路原理图；

图4是本实用新型的磁力计的电路原理图；

图5是本实用新型的气压计的电路原理图；

图6是本实用新型的主控模块的电路原理图；

图7是本实用新型的电源模块电路原理图。

具体实施方式

下面将对本实用新型的技术方案进行清楚、完整地描述。

如图1所示，一种基于GPS和INS组合导航的整平机装置，包括：整平机及其平地铲、GPS导航模块，INS惯性导航模块；

所述INS惯性导航模块安装固定于所述平地铲上，用来代替整平机调平杆上的倾角传感器；所述GPS导航模块安装于整平机机身上；设置主控制器连接GPS导航模块和INS惯性导航模块，用于对GPS导航模块和INS惯性导航模块所测量的数据进行融合，得出整平机实时、准确的速度、位置信息。

进一步，所述INS惯性导航模块外连接磁力计，所述INS惯性导航模块用于补偿磁力计的横滚角和俯仰角，进一步补偿磁力计提供的航向角。如图3和图4所示，磁力计通过AUX SCL和AUX SDA两个引脚和INS惯性导航模块连接。

进一步，如图3所示，所述INS惯性导航模块使用陀螺仪和加速度计融合的型号为MPU6050的6轴运动处理芯片，且通过I2C接口连接所述主控制器。型号为MPU6050的6轴运动处理芯片作为全球首例，同时也是惯导、航姿领域最常用的组件。在此之前，陀螺仪与加速度计融合的时之轴间差问题一直有待解决。而其领先于其他组件之处正是在于解决了这个问题。

进一步，如图4所示，所述磁力计采用型号为HMC5983L的霍尼韦尔磁阻式三轴地磁传感器，且通过I2C接口连接所述主控制器，为整平机提供航向角。型号为HMC5983L的霍尼韦尔磁阻式三轴地磁传感器内置复位和偏置驱动器实现自测和偏移补偿，罗盘精度高，磁场范围广，含有简易的I2C总线接口，给出载体偏离北向的角度。

进一步，如图2所示，所述GPS导航模块采用型号为MAX-6Q的GPS芯片，且通过I2C接口连接所述主控制器。GPS芯片内部搭载高性能的U-blox6定位引擎，该引擎具有良好的跟踪灵敏度、冷启动灵敏度、捕获速度以及抗干扰技术。模块系列采用超小型封装，体积小，使用时具有非常高的集成度。易于与u-blox无线模块集成。提供低功耗和低成本的同时保证了良好的定位精度。突破了针对低功耗应用的智能电源管理技术。

进一步，如图6所示，所述主控制器为型号为STM32F103TB的单片机最小控制系统。

进一步，如图5所示，所述主控制器外设置气压计，提供短时的高程、温度测量。

注：本实用新型中所述的GPS导航模块、INS惯性导航模块、磁力计、气压计、主控制器的供电模块，均采用型号为MIC5219的电压转换芯片。

本实用新型中所述姿态角中的横滚角、俯仰角、航向角是针对整平机的右、前、上三个方向构成右手系，绕向前的轴旋转就是横滚角，绕向右的轴旋转就是俯仰角，绕向上的轴旋转就是航向角；且所述平地铲位于为整平机的前方。

作为本实用新型的优选，所述气压计采用型号为BMP180的气压计芯片，且通过串口连接所述主控制器。

本实用新型的实施步骤如下：

GPS导航模块与INS惯性导航模块中的各个传感器将采集到的原始数据通过I2C发送给主控制器，在主控制器中先通过软件进行原始数据的一阶低通滤波，滤除高频噪声后通过卡尔曼滤波算法进行数据融合。量测量为两种导航系统各自产生的导航参数的差值，对惯导系统的误差进行预测，利用预测后的最优值对惯导系统进行校正。将校正后的速度、位置信息通过串口发送给整平机搭载的上位机，提供实时的位置、速度信息。

由于加速度计本身的特性，以及整平机工作时平地铲的振动，安装在平地铲上的MPU6050采集的原始数据包含大量的噪声，且陀螺仪存在漂移现象。磁力计须保持水平才能精确提供航向角的实时数据，因此需要INS惯性导航模块中的陀螺仪提供横滚角和俯仰角参数对其进行补偿。综上，必须对各传感器的原始数据进行一阶低通数字滤波，去除高频噪声，从而得到平地铲正确的姿态。故采用简单的递推平均滤波算法与FIFO（First In First Out）机制结合，把连续取的N个采样值看成一个长度固定的序列，将每次传感器采样到的新的ADC值放入队尾，相应存储在队首的数据就被去除，滤波结果为序列中N个数的平均值。递推平均滤波算法对信号的周期性干扰有良好的抑制性作用，去除了信号中的高频部分，使数据的平滑度更高。

使用UD分解滤波算法与扩展卡尔曼滤波相结合。对传感器测量值误差协方差进行UD分解，减少运算数量级，降低了线性化舍入误差的影响，增加了稳定性，使输出的姿态角更加精确。

对于本领域的普通技术人员来说，在不脱离本实用新型创造构思的前提下，还可以做出若干变形和改进，这些都属于本实用新型的保护范围。