多传感器融合的拖拉机导航控制系统及其定位、控制方法与流程

技术特征：

1.一种多传感器融合的拖拉机导航控制系统，包括上位机和下位机，其特征在于：所述的上位机和下位机通过串口通信模块相互连接，共同实现拖拉机位置精准定位和设定路径的精确跟踪；上位机连接触摸屏、北斗定位接收模块、图像采集模块、位姿检测模块、串口通信模块；下位机连接数据存储模块、压力检测模块、CAN通信模块、换向阀控制模块、电压检测模块、车速检测模块、车轮转角检测模块、电源模块、报警及紧急处理模块和串口通信模块；所述的换向阀控制模块分别与电压检测模块和液压转向系统电连接；所述的压力检测模块还与液压转向系统电连接。

2.根据权利要求1所述的一种多传感器融合的拖拉机导航控制系统，其特征在于：所述的上位机通过串口通信模块发送给下位机用于路径跟踪的数据包括：当前航线偏移量、当前航向角偏移量、预估行驶距离、当前行驶速度、预估路径曲度。

3.根据权利要求1所述的一种多传感器融合的拖拉机导航控制系统的定位方法，其特征在于：通过上位机对拖拉机进行定位，包括如下步骤：

(1)通过触摸屏设定拖拉机工作田间模式：无农作物田地，有农作物田地；

(2)采用双天线的北斗接收装置，用北斗定位接收模块获取拖拉机的绝对位置、行驶速度和航向角信息；

(3)采用位姿检测模块获取拖拉机的航向角、横滚角、俯仰角数据，并依据这些数据对北斗定位信息进行修正；

(4)如果田间模式设定为无农作物田地，则关闭图像采集模块，不采用机器视觉定位方式；如果田间模式设定为有农作物田地，则开启图采集模块，对采集图像数据进行处理，并生成导航基准线、获取表征农作物特征的基本点；

(5)如果田间模式设定为无农作物田地，则完全采用经过位姿信息修正的北斗定位和路径数据；如果田间模式设定为有农作物田地，则采用模糊自适应扩展卡尔曼滤波器对北斗定位数据和机器视觉数据进行融合，生成精确的拖拉机位置数据和路径规划数据。

4.根据权利要求1所述的一种多传感器融合的拖拉机导航控制系统的控制方法，其特征在于，控制方法为路径跟踪控制，包括如下步骤：

(1)路径跟踪控制系统采用串级控制结构进行设计，其中主回路采取前馈加反馈控制方法，副回路采取单位负反馈方法，整个系统结构由偏移量设定值O_S(s)、当前航线偏移量O_R(s)、偏移量控制器C_O(s)、转角控制器C_S(s)、换向阀控制器C_V(s)、电压采集V_M(s)、转向系统G(s)、预估路径曲度PC_R(s)、当前航向角偏移量A_R(s)、航向角控制器C_A(s)、车轮转角反馈A(s)、上位机发送的车速V_R(s)、车速采集V_M(s)、参考车速V_U(s)组成；

(2)主回路中的反馈控制器为偏移量控制器C_O(s)，其设定值输入为偏移量设定值O_S(s)和当前航线偏移量O_R(s)的差值，偏移量控制器C_O(s)的输出为转向角度θ_FB，通过如下公式计算：

其中ΔO＝O_S(s)-O_R(s)，K_FB1为偏移量控制器比例系数，K_FB2为偏移量控制器微分系数，K_FB3为偏移量控制器二阶微分系数；

(3)主回路中的前馈控制器为航向角控制器C_A(s)，其参考输入为预估路径曲度PC_R(s)、当前航向角偏移量A_R(s)和当前航线偏移量O_R(s)，航向角控制器C_A(s)的输出为转向角度θ_FF，通过如下公式计算：

其中K_FF1为航向角控制器路径曲度控制比例系数，K_FF2为航向角控制器比例系数，K_FF3为航向角控制器微分比例系数；

(4)副回路控制器为转角控制器C_S(s)，其设定值为θ_FB与θ_FF之和与车轮转角反馈A(s)的差值，参考输入量为车轮转角反馈A(s)和参考车速V_U(s)，其中参考车速V_U(s)由上位机发送的车速V_R(s)和车速采集V_M(s)决定，按照如下公式确定参考车速：

$V_U\left(s\right)=\left\{\begin{array}{cc}{V}_M\left(s\right)& \left(V_M\right(s)\≤2)\∪\left(V_R\right(s)=0)\\ V_R\left(s\right)& \left(V_M\right(s)\>2)\∪\left(\right(V_M\left(s\right)=0)\∩(V_R\left(s\right)\≠0\left)\right)\end{array}\right.$

转角控制器C_S(s)的输出为车轮转角设定值θ，通过如下公式计算：

其中K_θ为转角控制器C_S(s)的比例系数；

(5)换向阀控制器C_V(s)的设定值为车轮转角设定值θ，其通过换向阀控制模块输出负10V至正10V范围的电压控制液压转向系统实现转向。