基于GNSS/IMU融合的农机悬挂仿形控制算法

本申请提供了一种基于GNSS/IMU融合的农机悬挂仿形控制算法，其通过对GNSS和IMU的信号进行融合处理，能够利用I MU的高频数据弥补GNSS信号更新频率较低的不足，通过分别设置在农机机头和农具上的GNSS模块和I MU模块获取高程数据和加速度数据并通过卡尔曼滤波器的处理得到二者的高精度高程数据，采用欧拉投影对GNSS接收机获得的位置数据以及I MU获取的姿态数据进行融合处理，计算出农机机头和农具的重心点的精确坐标。根据上述两次计算的结果，使用机头的GNSS/IMU数据生成高精度的地形信息及机头的高程变化量作为参考基准，农具的GNSS/IMU数据作为调平仿形控制的输入值作为闭环反馈的参考，使用PID算法对悬挂油缸进行农具的仿形控制，因此能够获取精确的误差信号以供预设的控制模型对农机的悬挂进行控制以确保悬挂系统控制的精确性、连续性和可靠性，从而有效提升控制效果。