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

1.本发明涉及拖拉机自动导航领域，具体地讲，是多传感器融合的拖拉机导航控制系统的定位与控制方法。


背景技术：

2.目前的农业作业中，通常由驾驶员独自操作，劳动强度大且重复单调，容易使人疲劳并产生误操作。农机自动导航技术是精细农业的一项重要技术，该技术可以自动获得导航路径并控制农业机械沿着目标路径行驶，辅助驾驶员操作农业机械进行作业。拖拉机自动导航是农业现代化的重要基础，实现拖拉机自动导航可以让农业作业者降低工作强度，并且能显著地提高农机的作业精度，提高农田的土地利用率，降低生产成本，提高作物产量与经济效益，将劳动者从繁重的农田作业中解放出来。
3.拖拉机导航技术在北美、日本和欧洲一些国家中研究起步较早，并取得了较多研究成果，国内的专家学者也做了广泛的研究，取得了一定的成果。如申请号为2014101751440的发明专利公开了一种对射式激光拖拉机辅助导航控制系统，授权号101833334a的发明专利公开了一种拖拉机自动导航控制系统及其方法，申请号为104656647a的发明专利公开了一种低矮作物田间自走拖拉机导航控制系统等。可以看出，采用的都是单一导航定位技术，比如gps导航、激光发射导航技术等，每种导航方式各有优点，但是因单一导航技术受条件限制都存在一定的使用条件，很难能够提供连续、稳定高质量的定位信息；另外，目前拖拉机路径跟踪控制算法中，大多都仅仅将拖拉机航向角和偏移量做为被控量或参考量，但没有考虑到选取了拖拉机预估路径曲度和当前行驶速度；导致现有的技术方案往往只能保证直线行驶时的控制效果，而很难保证曲线行驶或拐弯时的路径跟踪效果。


技术实现要素：

4.本发明要解决的技术问题是提供多传感器融合的拖拉机导航控制系统的定位与控制方法，具有精确的车辆定位和路径跟踪性能。
5.本发明采用如下技术方案实现发明目的：
6.一种多传感器融合的拖拉机导航控制系统，包括上位机和下位机，所述的上位机和下位机通过串口通信模块相互连接，实现拖拉机位置的精准定位和设定路径的精确跟踪。所述的上位机连接触摸屏、北斗定位接收模块、图像采集模块、位姿检测模块、串口通信模块。所述的下位机连接数据存储模块、压力检测模块、can通信模块、换向阀控制模块、电压检测模块、车速检测模块、车轮转角检测模块、电源模块、报警及紧急处理模块和串口通信模块。
7.(1)本技术方案中导航控制系统通过采用北斗定位和机器视觉定位相结合的方式，能够实现两种导航技术的优势互补，可以避免使用单一定位造成的车辆位置误差；组合导航功能既可以用在有作物的农田作业中也可以用在无作物的农田中，组合导航定位技术
在可靠性、准确性等方面都较单独导航情况要好。(2)采用双天线的北斗信号接收装置，并且采用位姿检测模块的数据对北斗定位信息进行修正，得到的拖拉机位置数据更精确；(3)不仅选取了拖拉机当前航线偏移量、当前航向角偏移量，而且选取了拖拉机预估路径曲度和参考车速进行处理，使导航控制系统具有良好的路径控制效果。
8.拖拉机当前航线偏移量、当前航向角偏移量、拖拉机预估路径曲度值由上位机通过北斗导航数据或图像数据计算输出；拖拉机参考车速vu(s)由上位机发送的车速vr(s)和车速采集vm(s)决定，按照如下公式确定：
[0009][0010]
因拖拉机在低速行驶时，通过北斗导航获得的行驶速度其准确度低于车速检测模块获得的车速，所以在确定车速时，如果车速检测模块采集的车速vm(s)小于等于2公里/秒或接收的上位机的车速vr(s)等于0时，则确定的参考车速为vm(s)，这样可以避免因北斗导航在低速或故障时造成的速度误差；当车速检测模块采集的车速vm(s)大于2公里/秒或车速检测模块出现故障时，确定参考车速为上位机发送车速vr(s)。
[0011]
作为对本技术方案的进一步限定：
[0012]
换向阀控制模块分别与电压检测模块和液压转向系统电连接。
[0013]
压力检测模块还与液压转向系统电连接。
[0014]
上位机采用epcs-8980型工控机。
[0015]
触摸屏选用四线电阻式触摸屏sv0804s-03。
[0016]
北斗定位接收模块采用um220模块。
[0017]
图像采集模块由okac1310ccd摄像头和信号调理电路组成。
[0018]
位姿检测模块选用ahrs-3000小型航姿测量模块。
[0019]
下位机采用16位飞思卡尔单片机mc9s12xs128mal。
[0020]
数据存储模块采用两片32mb的sdram mt48lc4m32b2数据存储芯片。
[0021]
压力检测模块由压力传感器mbs1250和信号调理电路组成。
[0022]
can通信模块由tja1043t芯片与其外围电路组成，通过该模块，下位机能够将根据预估行驶距离和预估路径曲度确定的拖拉机期望速度发送给拖拉机控制器，确保在拖拉机行驶至终点或拐弯时的控制效果，特别在当前航线偏移量or(s)超过可允许最大航线偏移量om时，通过该模块给拖拉机控制器发送停车信号。
[0023]
换向阀控制模块由d/a转换芯片dac0832、运算放大器lm358n及外围电路组成。
[0024]
电压检测模块由运算放大器ad741和外围电路组成。
[0025]
通过设计电压检测模块，能够对换向阀控制模块的输出电压实施闭环控制，通过实时检测输出电压，避免输出电压失控现象发生。
[0026]
车速检测模块由测速传感器vb-z9400及信号调理电路组成。
[0027]
车轮转角检测模块由线位移传感器hps-m1和信号调理电路组成。
[0028]
电源模块由产生24v电压芯片lt4356is、产生5v电压芯片lm7805、产生正负15电压芯片md20-12d15、产生10v电压芯片ad581及外围电路组成。
[0029]
串口通信模块由串口通信芯片max232芯片及其外围电路组成。
[0030]
报警及紧急处理模块由喇叭、发光灯、光耦和继电器等组成。报警和紧急处理模块
能够在液压阀运行至最大或当曲线路径弯度过大时发出报警信息并启动相应的紧急响应动作。
[0031]
上位机通过串口通信模块发送给下位机用于路径跟踪的数据包括：当前航线偏移量、当前航向角偏移量、预估行驶距离、当前行驶速度、预估路径曲度。
[0032]
多传感器融合的拖拉机导航控制系统的定位方法，通过上位机进行拖拉机定位，包括如下步骤：
[0033]
(1)通过触摸屏设定拖拉机工作田间模式：无农作物田地，有农作物田地；
[0034]
(2)采用双天线的北斗接收装置，用北斗定位接收模块获取拖拉机的绝对位置、行驶速度和航向角信息；
[0035]
(3)采用位姿检测模块获取拖拉机的航向角、横滚角、俯仰角数据，并依据这些数据对北斗定位信息进行修正；
[0036]
(4)如果田间模式设定为无农作物田地，则关闭图像采集模块，不采用机器视觉定位方式；如果田间模式设定为有农作物田地，则开启图采集模块，对采集图像数据进行处理，并生成导航基准线、获取表征农作物特征的基本点。
[0037]
(5)如果田间模式设定为无农作物田地，则完全采用经过位姿信息修正的北斗定位和路径数据；如果田间模式设定为有农作物田地，则采用模糊自适应扩展卡尔曼滤波器对北斗定位数据和机器视觉数据进行融合，其中北斗定位提供拖拉机的绝对位置坐标、航向角度和行驶速度，机器视觉提供导航路径中已知点的相对位置坐标，北斗定位系统和机器视觉系统坐标统一后，由滤波器对北斗提供的位置数据和机器视觉提供的位置数据进行滤波，得到精确的位置数据和路径数据。
[0038]
与目前多传感器融合技术中常用的扩展卡尔曼滤波器和无迹卡尔曼滤波器相比，模糊自适应扩展卡尔曼滤波算法能够通过实时得到的测量新息方差和理论方差的比值，并利用模糊自适应控制器不断调整量测噪声协方差阵的加权系数，减少校正位置误差，提高拖拉机定位数据的准确性和稳定性。
[0039]
一种多传感器融合的拖拉机导航控制系统定位后的控制方法，控制方法为路径跟踪控制，包括如下步骤：
[0040]
(1)路径跟踪控制系统采用串级控制结构进行设计，其中主回路采取前馈加反馈控制方法，副回路采用单位负反馈方法，整个系统结构由偏移量设定值os(s)、当前航线偏移量or(s)、偏移量控制器co(s)、转角控制器cs(s)、换向阀控制器cv(s)、电压采集vm(s)、转向系统g(s)、预估路径曲度pcr(s)、当前航向角偏移量ar(s)、航向角控制器ca(s)、车轮转角反馈a(s)、上位机发送的车速vr(s)、车速采集vm(s)、参考车速vu(s)组成；
[0041]
(2)主回路中的反馈控制器为偏移量控制器co(s)，其设定值输入为偏移量设定值os(s)和当前航线偏移量or(s)的差值，偏移量控制器co(s)的输出为转向角度θ
fb
，通过如下公式计算：
[0042][0043]
其中δo＝os(s)-or(s)，k
fb1
为偏移量控制器比例系数，k
fb2
为偏移量控制器微分系数，k
fb3
为偏移量控制器二阶微分系数。因拖拉机路径跟踪的直接控制量是航线偏移量，本
发明通过对偏移量差值实施比例、微分和二阶微分的控制，可减少控制过程中的超调，克服振荡现象发生；通过设定可允许最大航线偏移量om，可使拖拉机在航线偏差超过可允许范围时不再继续执行转向，并且控制偏移量控制器输出为0，避免损坏液压阀或安全事故发生；
[0044]
(3)主回路中的前馈控制器为航向角控制器ca(s)，其参考输入为预估路径曲度pcr(s)、当前航向角偏移量ar(s)和当前航线偏移量or(s)，航向角控制器ca(s)的输出为转向角度θ
ff
，通过如下公式计算：
[0045][0046]
其中k
ff1
为航向角控制器路径曲度控制比例系数，k
ff2
为航向角控制器比例系数，k
ff3
为航向角控制器微分比例系数。在航线偏移量允许值o
p
范围内，通过将航向角作为参考量，可实施预先控制，减少路径跟踪过程的波动；将预估路径曲度作为参考量，可实现曲线行驶或拐弯时的预先控制，提高非直线路径的跟踪效果。
[0047]
(4)副回路控制器为转角控制器cs(s)，其设定值为θ
fb
与θ
ff
之和与车轮转角反馈a(s)的差值，参考输入量为车轮转角反馈a(s)和参考车速vu(s)，其中参考车速vu(s)由上位机发送的车速vr(s)和车速采集vm(s)决定，按照如下公式确定参考车速：
[0048][0049]
转角控制器cs(s)的输出为车轮转角设定值θ，通过如下公式计算：其中k
θ
为转角控制器cs(s)的比例系数。因拖拉机在低速行驶时，通过北斗导航获得的行驶速度其准确度低于车速检测模块获得的车速，所以在确定车速时，如果车速检测模块采集的车速vm(s)小于等于2公里/秒或接收的上位机的车速vr(s)等于0时，则确定的参考车速为vm(s)，这样可以避免因北斗导航在低速或故障时造成的速度误差；当车速检测模块采集的车速vm(s)大于2公里/秒或车速检测模块出现故障时，确定参考车速为上位机发送车速vr(s)。将车速信号作为确定车轮转角设定值的参数，可减少系统调整时间；
[0050]
(5)换向阀控制器cv(s)的设定值为车轮转角设定值θ，其通过换向阀控制模块输出负10v至正10v范围的电压控制液压转向系统实施转向。
[0051]
与现有技术相比，本发明在控制量和参考量设定时，不仅选取了拖拉机当前航线偏移量、当前航向角偏移量，而且选取了拖拉机预估路径曲度和当前行驶速度，提出的拖拉机路径跟踪串级控制结构，主回路采取前馈加反馈控制方法，副回路采取单位负反馈方法，该控制方法不仅保证直线行驶时的路径跟踪效果，而且在拐弯时也具有很高的控制精度。
附图说明
[0052]
图1为本发明的原理框图。
[0053]
图2为本发明所采用的控制方法结构示意图。
[0054]
图3为本发明的换向阀控制模块的电路原理图。
[0055]
图4为本发明的电压检测模块的电路原理图。
[0056]
图5为本发明的车速检测模块的电路原理图。
[0057]
图6为本发明的转角检测模块的电路原理图。
[0058]
附图标记说明：1－触摸屏；2－数据存储模块；3－压力检测模块；4－北斗定位接收模块；5－上位机；6－报警及紧急处理模块；7－下位机；8－can通信模块；9－图像采集模块；10－串口通信模块；11－换向阀控制模块；12－液压转向系统；13－拖拉机；14－位姿检测模块；15－电源模块；16－车速检测模块；17－车轮转角检测模块；18－电压检测模块。
具体实施方式
[0059]
为了使本发明的目的、技术方案及优点更加清楚明白，以下结合附图及实施例，对本发明进行进一步详细说明。此处所描述的具体实施例仅用以解释本发明，并不用于限定本发明。本发明涵盖任何由权利要求定义的在本发明的精髓和范围上做的替代、修改、等效方法及方案。为了使公众对本发明有更好的了解，在具体实施方式中对本发明的细节描述中，详尽描述了一些特定的细节部分，对于未描述的部分，均为本领域技术的常规技术。
[0060]
结合图1、图2可知，本发明实施例提供了一种多传感器融合的拖拉机导航控制系统，由上位机5和下位机7组成，上位机5和下位机7通过串口通信模块10相互连接，实现拖拉机13位置的精准定位和设定路径的精确跟踪。所述的上位机5连接触摸屏1、北斗定位接收模块4、图像采集模块9、位姿检测模块14、串口通信模块10。所述的下位机7连接数据存储模块2、压力检测模块3、can通信模块8、换向阀控制模块11、电压检测模块18、车速检测模块16、车轮转角检测模块17、电源模块15、报警及紧急处理模块6和串口通信模块10。换向阀控制模块11分别与电压检测模块18和液压转向系统12电连接，压力检测模块3还与液压转向系统12电连接。
[0061]
本发明中上位机5选用epcs-8980型工控机，它是整个导航系统中进行车辆位置信息处理的核心部件以及人机对话的中心，与触摸屏1结合完成田间模式设定、导航参数配置、路径下载、导航任务设定、路径规划、车辆运行轨迹实时显示等功能。
[0062]
触摸屏1与上位机5通过功能引脚相互连接，作为拖拉机13作业时的人机交互界面，可实现导航模式选择、导航参数输入和车辆运行轨迹显示等功能，方便操作人员对拖拉机13导航系统工作状态进行判断，本发明选用四线电阻式触摸屏1sv0804s-03。
[0063]
北斗定位接收模块4与上位机5通过串行通信引脚相互连接，本发明采用um220模块接收北斗卫星信号用来确定车辆位置，因采用的是北斗常规接收技术，在此不再赘述。
[0064]
图像采集模块9与上位机5通过串行通信引脚相互连接，由okac1310ccd摄像头采集农作物图像，经过信号调理电路后经串口送至上位机5，经灰度变换、图像分割、图像去噪后提取出导航基准线，确定导航路径特征值。
[0065]
位姿检测模块14与上位机5通过串行通信引脚相互连接，选用ahrs-3000小型航姿测量模块将拖拉机13动静态环境下的横滚角、俯仰角和航向角通过串口发送至上位机5，上位机5利用这些参数对北斗定位数据进行补偿，可减少或避免因受地面起伏影响车体产生倾斜而导航的定位误差。
[0066]
下位机7采用16位飞思卡尔单片机mc9s12xs128mal，是拖拉机13路径跟踪控制的
核心，通过串口通信模块10接收上位机5发送的拖拉机13位置信息并经过控制运算后，由换向阀控制模块11输出电压信号实现对液压转向系统12的控制。
[0067]
数据存储模块2，采用两片32mb的sdram mt48lc4m32b2，分别与飞思卡尔单片机通过功能引脚相互连接，用于存储导航状态信息和控制输出信息，方便后期对导航系统工作效果进行评估和分析。
[0068]
压力检测模块3与下位机7和液压转向系统12电连接，用于判断拖拉机13的驾驶模式，由安装在导航阀组上的压力传感器mbs1250检测阀组压力并转换成电压信号送至下位机7的数字量输入口，传感器输出低压信号表示拖拉机13处于自动导航模式，传感器输出高压信号表示拖拉机13处于人工驾驶模式。
[0069]
can通信模块8由tja1043t芯片与其外围电路组成，通过该模块，下位机7能够将根据预估行驶距离和预估路径曲度确定的拖拉机13期望速度发送给拖拉机13控制器，确保在拖拉机13行驶至终点或拐弯时的控制效果，特别在当前航线偏移量or(s)超过可允许最大航线偏移量om时，通过该模块给拖拉机13控制器发送停车信号。
[0070]
结合图3可知，换向阀控制模块11分别与电压检测模块18和液压转向系统12电连接，选用d/a转换芯片dac0832输出0-255的数字量，该数字量经过放大器u18-u21产生正10v至负10v范围内的电压用于控制换向电磁阀，实现对液压转向系统12的控制。本发明选用4wra6e20-10b/24z4型号的比例方向阀。
[0071]
结合图4可知，电压检测模块18由运算放大器和外围电路组成，通过设计电压检测模块，能够对换向阀控制模块11的输出电压实施闭环控制，通过实时检测输出电压，避免输出电压失控现象发生。
[0072]
结合图5可知，车速检测模块16与下位机7的中断输入口相连接，该模块由测速传感器和调理电路组成，测速传感器选用vb-z9400霍尔转速传感器，它通过感应导磁体上凸起的齿或是凹下的槽，输出方波信号，该方波信号经过四分之一个lm339和电阻r38,r39,r40构成的滞回比较电路去除噪声干扰，送至下位机7的外部中断引脚，下位机7通过下降沿触发的外部中断方式对车速信号进行捕捉。
[0073]
结合图6可知，车轮转角检测模块17与下位机7电连接，该模块用于完成对车轮转角的实时检测，选用hps-m1型线位移传感器将车轮转角信号转换成电压信号后经过电阻r13和电容c11滤波后进入放大器u12的反向输入端，放大器u12的正向输入端经电阻r15接地，u12的输出端经电阻r14后接入其反向输入端，同时u12的输出端经电阻r16后接入放大器u13的反向输入端，放大器u13的同向输入端经电阻r18后接地，u13的输出端经电阻r17后接入其反向输入端，同时与下位机7的模拟量输入口pa5相接。
[0074]
电源模块15与下位机7电连接，由车载蓄电池供电，分别经过lt4356is电路和lm7805电路产生24v防浪涌电压和5v电压，经过md20-12d15模块产生正负15的电压，经过ad581产生10v的基准电压，为保证下位机7系统的供电电压不至于过高或过低，设计了电源监控保护电路。
[0075]
串口通信模块10分别与上位机5和下位机7相连接，该模块由串口通信芯片max232芯片及其外围电路组成，通过串口通信模块10可完成下位机7和上位机5的数据交换。
[0076]
报警及紧急处理模块6与下位机7电连接，用于在液压阀运行至最大或当曲线路径弯度过大时，下位机7可以控制继电器打开，使与蜂鸣器和发光灯相连的引脚为高电平，发
出报警信息，并且控制输出电压为零使换向阀回归中心位置，同时置位手动模式。
[0077]
上位机5通过串口通信模块10发送给下位机7用于路径跟踪的数据包括：当前航线偏移量、当前航向角偏移量、预估行驶距离、当前行驶速度、预估路径曲度。
[0078]
定位方法，通过上位机5对拖拉机13进行定位，包括如下步骤：
[0079]
(1)通过触摸屏1设定拖拉机13工作田间模式：无农作物田地，有农作物田地；
[0080]
(2)采用双天线的北斗接收装置，用北斗定位接收模块4获取拖拉机13的绝对位置、行驶速度和航向角信息；
[0081]
(3)采用位姿检测模块14获取拖拉机13的航向角、横滚角、俯仰角数据，并依据这些数据对北斗定位信息进行修正；
[0082]
(4)如果田间模式设定为无农作物田地，则关闭图像采集模块9，不采用机器视觉定位方式；如果田间模式设定为有农作物田地，则开启图像采集模块9，对采集图像数据进行处理，并生成导航基准线、获取表征农作物特征的基本点。
[0083]
(5)如果田间模式设定为无农作物田地，则完全采用经过位姿信息修正的北斗定位和路径数据；如果田间模式设定为有农作物田地，则采用模糊自适应扩展卡尔曼滤波器对北斗定位数据和机器视觉数据进行融合，其中北斗定位提供拖拉机13的绝对位置坐标、航向角度和行驶速度，机器视觉提供导航路径中已知点的相对位置坐标，北斗定位系统和机器视觉系统坐标统一后，由滤波器对北斗提供的位置数据和机器视觉提供的位置数据进行滤波，得到精确的位置数据和路径数据。
[0084]
与目前多传感器融合技术中常用的扩展卡尔曼滤波器和无迹卡尔曼滤波器相比，模糊自适应扩展卡尔曼滤波算法能够通过实时得到的测量新息方差和理论方差的比值，并利用模糊自适应控制器不断调整量测噪声协方差阵的加权系数，减少校正位置误差，提高拖拉机13定位数据的准确性和稳定性。
[0085]
控制方法为路径跟踪控制，包括如下步骤：
[0086]
(1)路径跟踪控制系统采用串级控制结构进行设计，其中主回路采取前馈加反馈控制方法，副回路采用单位负反馈方法，整个系统结构由偏移量设定值os(s)、当前航线偏移量or(s)、偏移量控制器co(s)、转角控制器cs(s)、换向阀控制器cv(s)、电压采集vm(s)、转向系统g(s)、预估路径曲度pcr(s)、当前航向角偏移量ar(s)、航向角控制器ca(s)、车轮转角反馈a(s)、上位机5发送的车速vr(s)、车速采集vm(s)、参考车速vu(s)组成；
[0087]
(2)主回路中的反馈控制器为偏移量控制器co(s)，其设定值输入为偏移量设定值os(s)和当前航线偏移量or(s)的差值，偏移量控制器co(s)的输出为转向角度θ
fb
，通过如下公式计算：
[0088][0089]
其中δo＝os(s)-or(s)，k
fb1
为偏移量控制器比例系数，k
fb2
为偏移量控制器微分系数，k
fb3
为偏移量控制器二阶微分系数。因拖拉机13路径跟踪的直接控制量是航线偏移量，本发明通过对偏移量差值实施比例、微分和二阶微分的控制，可减少控制过程中的超调，克服振荡现象发生；通过设定可允许最大航线偏移量om，可使拖拉机13在航线偏差超过可允许范围时不再继续执行转向，并且控制偏移量控制器输出为0，避免损坏液压阀或安全事故
发生；
[0090]
(3)主回路中的前馈控制器为航向角控制器ca(s)，其参考输入为预估路径曲度pcr(s)、当前航向角偏移量ar(s)和当前航线偏移量or(s)，航向角控制器ca(s)的输出为转向角度θ
ff
，通过如下公式计算：
[0091][0092]
其中k
ff1
为航向角控制器路径曲度控制比例系数，k
ff2
为航向角控制器比例系数，k
ff3
为航向角控制器微分比例系数。在航线偏移量允许值o
p
范围内，通过将航向角作为参考量，可实施预先控制，减少路径跟踪过程的波动；将预估路径曲度作为参考量，可实现曲线行驶或拐弯时的预先控制，提高非直线路径的跟踪效果。
[0093]
(4)副回路控制器为转角控制器cs(s)，其设定值为θ
fb
与θ
ff
之和与车轮转角反馈a(s)的差值，参考输入量为车轮转角反馈a(s)和参考车速vu(s)，其中参考车速vu(s)由上位机5发送的车速vr(s)和车速采集vm(s)决定，按照如下公式确定参考车速：
[0094][0095]
转角控制器cs(s)的输出为车轮转角设定值θ，通过如下公式计算：其中k
θ
为转角控制器cs(s)的比例系数。因拖拉机13在低速行驶时，通过北斗导航获得的行驶速度其准确度低于车速检测模块16获得的车速，所以在确定车速时，如果车速检测模块16采集的车速vm(s)小于等于2公里/秒或接收的上位机5的车速vr(s)等于0时，则确定的参考车速为vm(s)，这样可以避免因北斗导航在低速或故障时造成的速度误差；当车速检测模块16采集的车速vm(s)大于2公里/秒或车速检测模块16出现故障时，确定参考车速为上位机5发送车速vr(s)。将车速信号作为确定车轮转角设定值的参数，可减少系统调整时间。
[0096]
(5)换向阀控制器cv(s)的设定值为车轮转角设定值θ，其通过换向阀控制模块11输出负10v至正10v范围的电压控制液压转向系统12实施转向。
[0097]
与现有技术相比，本发明在控制量和参考量设定时，不仅选取了拖拉机13当前航线偏移量、当前航向角偏移量，而且选取了拖拉机13预估路径曲度和当前行驶速度。提出的拖拉机13路径跟踪串级控制结构，主回路采取前馈加反馈控制方法，副回路采取单位负反馈方法，该控制方法不仅保证直线行驶时的路径跟踪效果，而且在拐弯时也具有很高的控制精度。
[0098]
最后应说明的是：以上所述仅为本发明的优选实施例而已，并不用于限制本发明，尽管参照前述实施例对本发明进行了详细的说明，对于本领域的技术人员来说，其依然可以对前述各实施例所记载的技术方案进行修改，或者对其中部分技术特征进行等同替换。凡在本发明的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。

技术特征：
1.一种多传感器融合的拖拉机导航控制系统的定位与控制方法，所述的导航控制系统，包括上位机和下位机，所述的上位机和下位机通过串口通信模块相互连接，共同实现拖拉机位置精准定位和设定路径的精确跟踪；所述的上位机连接触摸屏、北斗定位接收模块、图像采集模块、位姿检测模块、串口通信模块；所述的下位机连接数据存储模块、压力检测模块、can通信模块、换向阀控制模块、电压检测模块、车速检测模块、车轮转角检测模块、电源模块、报警及紧急处理模块和串口通信模块；所述的换向阀控制模块分别与电压检测模块和液压转向系统电连接；所述的压力检测模块还与液压转向系统电连接，其特征在于：(一)所述的定位方法是通过上位机对拖拉机进行定位，定位方法包括如下步骤：(1)通过触摸屏设定拖拉机工作田间模式：无农作物田地，有农作物田地；(2)采用双天线的北斗接收装置，用北斗定位接收模块获取拖拉机的绝对位置、行驶速度和航向角信息；(3)采用位姿检测模块获取拖拉机的航向角、横滚角、俯仰角数据，并依据这些数据对北斗定位信息进行修正；(4)如果田间模式设定为无农作物田地，则关闭图像采集模块，不采用机器视觉定位方式；如果田间模式设定为有农作物田地，则开启图采集模块，对采集图像数据进行处理，并生成导航基准线、获取表征农作物特征的基本点；(5)如果田间模式设定为无农作物田地，则完全采用经过位姿信息修正的北斗定位和路径数据；如果田间模式设定为有农作物田地，则采用模糊自适应扩展卡尔曼滤波器对北斗定位数据和机器视觉数据进行融合，生成精确的拖拉机位置数据和路径规划数据。(二)所述的控制方法为路径跟踪控制，包括如下步骤：(1)路径跟踪控制系统采用串级控制结构进行设计，其中主回路采取前馈加反馈控制方法，副回路采取单位负反馈方法，整个系统结构由偏移量设定值o
s
(s)、当前航线偏移量o
r
(s)、偏移量控制器c
o
(s)、转角控制器c
s
(s)、换向阀控制器cv(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)决定，按照如下公式确定参考车速：转角控制器c
s
(s)的输出为车轮转角设定值θ，通过如下公式计算：其中k
θ
为转角控制器c
s
(s)的比例系数；(5)换向阀控制器cv(s)的设定值为车轮转角设定值θ，其通过换向阀控制模块输出负10v至正10v范围的电压控制液压转向系统实现转向。

技术总结
本发明提供的多传感器融合的拖拉机导航控制系统的定位与控制方法，控制系统包括上位机、下位机及相应模块；定位与控制方法中，定位方法采用模糊自适应扩展卡尔曼滤波算法能够通过实时得到的测量新息方差和理论方差的比值，并利用模糊自适应控制器不断调整量测噪声协方差阵的加权系数，减少校正位置误差，提高拖拉机定位数据的准确性和稳定性；控制方法，选取了拖拉机当前航线偏移量、当前航向角偏移量、预估路径曲度和当前行驶速度，提出路径跟踪串级控制结构，主回路采取前馈加反馈控制方法，副回路采取单位负反馈方法，保证行驶时的路径跟踪效果；本发明具有精确的车辆定位和路径跟踪性能。径跟踪性能。径跟踪性能。


技术研发人员：孙群 尹成强 高洁 武健
受保护的技术使用者：聊城大学
技术研发日：2016.12.05
技术公布日：2023/9/23