多传感器融合的拖拉机导航控制系统的控制方法

专利主权项内容

1.一种多传感器融合的拖拉机导航控制系统的控制方法，其特征在于：所述的导航控制系统，包括上位机和下位机，所述的上位机和下位机通过串口通信模块相互连接，共同实现拖拉机位置精准定位和设定路径的精确跟踪；所述的上位机连接触摸屏、北斗定位接收模块、图像采集模块、位姿检测模块、串口通信模块；所述的下位机连接数据存储模块、压力检测模块、CAN通信模块、换向阀控制模块、电压检测模块、车速检测模块、车轮转角检测模块、电源模块、报警及紧急处理模块和串口通信模块；所述的换向阀控制模块分别与电压检测模块和液压转向系统电连接；所述的压力检测模块还与液压转向系统电连接，所述的控制方法为路径跟踪控制，包括如下步骤：(1)为路径跟踪控制采用串级控制结构进行设计，其中主回路采取前馈加反馈控制方法，副回路采取单位负反馈方法，整个系统结构由偏移量设定值O(s)、当前航线偏移量O(s)、偏移量控制器C(s)、转角控制器C(s)、换向阀控制器C(s)、电压采集V(s)、转向系统G(s)、预估路径曲度PC(s)、当前航向角偏移量A(s)、航向角控制器C(s)、车轮转角反馈A(s)、上位机发送的车速V(s)、车速采集V(s)、参考车速V(s)组成；SROSVMRRARMU(2)主回路中的反馈控制器为偏移量控制器C(s)，其设定值输入为偏移量设定值O(s)和当前航线偏移量O(s)的差值，偏移量控制器C(s)的输出为转向角度θ，通过如下公式计算：OSROFB其中ΔO＝O(s)-O(s)，K为偏移量控制器比例系数，K为偏移量控制器微分系数，K为偏移量控制器二阶微分系数；SRFB1FB2FB3(3)主回路中的前馈控制器为航向角控制器C(s)，其参考输入为预估路径曲度PC(s)、当前航向角偏移量A(s)和当前航线偏移量O(s)，航向角控制器C(s)的输出为转向角度θ，通过如下公式计算：ARRRAFF其中，K为航向角控制器路径曲度控制比例系数，K为航向角控制器比例系数，K为航向角控制器微分比例系数；FF1FF2FF3(4)副回路控制器为转角控制器C(s)，其设定值为θ与θ之和与车轮转角反馈A(s)的差值，参考输入量为车轮转角反馈A(s)和参考车速V(s)，其中参考车速V(s)由上位机发送的车速V(s)和车速采集V(s)决定，按照如下公式确定参考车速：SFBFFUURM转角控制器C(s)的输出为车轮转角设定值θ，通过如下公式计算：S其中K为转角控制器C(s)的比例系数；θS(5)换向阀控制器C(s)的设定值为车轮转角设定值θ，其通过换向阀控制模块输出负10V至正10V范围的电压控制液压转向系统实现转向。V