本发明属于农业机械智能导航与运动控制，具体涉及一种基于辅助导航的拖拉机侧滑角实时估计系统及方法。
背景技术：
1、随着精准农业技术的快速发展，搭载自动导航系统的农业机械（如拖拉机）已广泛应用于耕、种、管、收等全环节作业中。在自动导航控制闭环中，侧滑角作为表征车辆实际运动矢量与车身纵轴夹角的关键状态参数，是实现精准路径跟踪与姿态控制的必要反馈变量，决定了路径跟踪控制器的横向纠偏能力。
2、现有农业导航系统多采用gnss（全球导航卫星系统）与imu（惯性测量单元）松/紧耦合的方式进行状态估计。然而，农田非结构化环境具有地形起伏大、土壤松软湿滑、附着系数时变等特征，导致农机极易发生非线性侧滑。传统的基于运动学模型或单一动力学模型的估计算法，在低速重载或微小侧滑工况下，常因观测性不足导致估计发散、响应滞后，难以满足高精度导航对实时状态反馈的需求。
3、此外，虽然基于视觉、激光雷达等外部环境感知传感器的侧滑角测量方案在理论上可行，但其系统架构复杂、硬件成本高昂。更重要的是，田间作业常伴随高粉尘遮挡、剧烈光照变化及作物抖动，严重影响此类光学传感器的信噪比与鲁棒性，限制了其在农业生产一线的规模化应用。
4、综上所述，研发一种无需昂贵附加传感器、且能适应复杂田间时变工况的拖拉机侧滑角实时高精度估计方法，对于提升农业机械自动导航系统的抗扰动能力与作业质量具有重要的工程意义。
技术实现思路
1、本发明旨在解决现有农业机械自动导航系统中侧滑角估计精度不足、动态响应滞后以及对田间非结构化环境适应性差等技术难题，提供一种基于动力学模型、运动学模型融合的拖拉机田间直线行驶侧滑角实时估计方法。
2、本发明通过依托于高精度车载导航硬件平台，构建包含车辆运动学与动力学特性的多模型融合框架，结合传感器数据进行状态观测，通过构建多源信息融合观测器，实时对车辆在复杂田间环境下的侧滑状态进行高精度估计。该方法能够显著提升导航系统对车辆侧滑状态的感知鲁棒性，为路径跟踪控制器提供准确的状态反馈，从而有效增强自动导航的控制稳定性与直线跟踪精度，保障田间自动化作业的质量一致性与作业效率。
3、为实现上述目的，本发明提供了如下方案：一种基于辅助导航的拖拉机侧滑角实时估计系统，包括：
4、车辆运动状态向量获取模块、用于基于拖拉机辅助导航系统实时获取拖拉机的多源运动状态信息，并对所述多源运动状态信息进行处理，得到车辆运动状态向量；所述辅助导航系统包括：双天线gnss定位单元、imu、主控制器、上位机、电源管理模块以及软件模块；
5、模型构建模块、用于基于所述车辆运动状态向量构建扩展运动学模型以及二自由度动力学模型；
6、初始状态估计模块、用于基于所述扩展运动学模型与扩展卡尔曼滤波算法得到轮胎侧滑角初始状态；
7、修正模块、用于基于所述二自由度动力学模型与龙伯格状态观测器，对所述轮胎侧滑角初始状态进行在线识别与修正，得到修正后的前后轮胎侧滑角估计值；
8、融合加权模块、用于基于修正后的所述前后轮胎侧滑角估计值、车辆速度以及车辆转向角，得到前后轮侧滑角的融合加权因子，并基于所述融合加权因子得到最终的前后轮侧滑角估计值。
9、进一步优选地，所述车辆运动状态向量获取模块得到所述车辆运动状态向量的过程包括：
10、利用所述双天线gnss定位单元获取拖拉机实时的全局位置坐标及航向信息，确立导航基准；利用前轮imu得到前轮转角；利用车身imu监测车身横摆角速度，捕捉车身动态姿态；对得到的原始数据执行数字滤波去噪、时间戳同步对齐以及基于安装位置的外参标定，得到时空统一的所述车辆运动状态向量。
11、进一步优选地，所述扩展运动学模型包括：
12、；
13、式中，表示车辆相对于参考路径的横向速度；表示拖拉机纵向速度；表示航向偏差；表示最终前轮侧滑角的估计值；表示最终后轮侧滑角的估计值；表示车辆的轴距。
14、进一步优选地，所述二自由度动力学模型包括：
15、；
16、；
17、式中，表示横摆角加速度； a、 b分别表示前轴距和后轴距；表示质心侧滑角；表示整车质量；表示车辆重心的纵向速度；表示车辆转向角；表示轮胎侧偏刚度；表示转动惯量。
18、进一步优选地，所述初始状态估计模块得到轮胎侧滑角初始状态的过程包括：
19、将所述扩展运动学模型表示为状态方程，选择状态向量为，控制输入为，对非线性函数关于状态向量求偏导数，计算时刻的状态雅可比矩阵；其中，表示横向偏差观测值；表示航向偏差观测值；上标 t表示转置；表示前后轮侧滑角估计值；
20、基于横向偏差及航向偏差构建观测向量，以所述状态方程为系统模型，基于所述状态雅可比矩阵，建立扩展卡尔曼滤波算法的状态转移模型；
21、基于扩展卡尔曼滤波算法的预测与更新步骤，对所述状态向量进行递推估计，从收敛的状态向量中解算得到前轮侧滑角和后轮侧滑角的估计值。
22、进一步优选地，所述修正模块得到修正后的前后轮胎侧滑角估计值的过程包括：
23、首先，以前轮侧滑角估计值与后轮侧滑角估计值为输入，依据车辆前后轴距的几何比例关系，计算得到质心侧滑角的初始几何观测值；
24、基于所述二自由度动力学模型，得到标准的状态空间更新方程；
25、所述龙伯格观测器结构包括：
26、；
27、式中，表示系统状态估计向量的导数；表示系统状态向量的估计值； a表示状态矩阵； b表示输入矩阵； c表示输出矩阵； h表示最优状态反馈增益矩阵；
28、构建误差动态方程，采用线性二次型调节器方法求解最优状态反馈增益矩阵 h；
29、将最优状态反馈增益矩阵代入龙伯格观测器，利用传感器实测的横摆角速度与模型预测值的偏差，对状态向量进行闭环修正，得到修正后的前后轮侧滑角估计值。
30、进一步优选地，前轮侧滑角的所述融合加权因子包括：
31、；
32、式中，表示前轮速度影响因子；表示前轮转角影响因子；
33、后轮侧滑角的所述融合加权因子包括：
34、；
35、式中，表示后轮速度影响因子。
36、进一步优选地，最终的前后轮侧滑角估计值包括：
37、
38、本发明还提供了一种基于辅助导航的拖拉机侧滑角实时估计方法，包括以下步骤：
39、s1、基于拖拉机辅助导航系统实时获取拖拉机的多源运动状态信息，并对所述多源运动状态信息进行处理，得到车辆运动状态向量；所述辅助导航系统包括：双天线gnss定位单元、imu、主控制器、上位机、电源管理模块以及软件模块；
40、s2、基于所述车辆运动状态向量构建扩展运动学模型以及二自由度动力学模型；
41、s3、基于所述扩展运动学模型与扩展卡尔曼滤波算法得到轮胎侧滑角初始状态；
42、s4、基于所述二自由度动力学模型与龙伯格状态观测器，对所述轮胎侧滑角初始状态进行在线识别与修正，得到修正后的前后轮胎侧滑角估计值；
43、s5、基于修正后的所述前后轮胎侧滑角估计值、车辆速度以及车辆转向角，得到前后轮侧滑角的融合加权因子，并基于所述融合加权因子得到最终的前后轮侧滑角估计值。
44、与现有技术相比，本发明的有益效果为：
45、按照本发明的方法，以东方红ly1104拖拉机为实验对象，加装可运行本发明提供的侧滑角实时估计算法的辅助导航系统。在田间进行直线导航作业行走，在行走过程中：
46、1.估计精度显著提升：采用本发明的侧滑角估计算法后，侧滑角估计误差控制在±0.5°以内。在复杂田间工况下（如湿滑地面、坡地作业），估计精度仍能保持稳定，满足高精度导航作业需求。
47、2.实时性能优异：算法单次计算耗时小于20ms，完全满足导航系统实时性要求。即使在拖拉机高速转向或急加速等动态工况下，系统仍能实时准确地输出侧滑角估计值，为导航控制提供可靠的状态信息。
48、3.导航精度明显改善：应用该侧滑角估计算法后，考虑侧滑角拖拉机直线导航作业的横向偏差从原来的±8cm降低至±3cm，偏差减小62.5%，导航精度达到厘米级水平，满足精准农业作业要求。
49、综上所述，本发明提供的侧滑角实时估计算法具有高精度、实时性能优异，能够显著提升农业机械自动导航系统的整体性能，为精准农业作业提供有力的技术支撑。