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

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

背景技术：

1、目前的农业作业中，通常由驾驶员独自操作，劳动强度大且重复单调，容易使人疲劳并产生误操作。农机自动导航技术是精细农业的一项重要技术，该技术可以自动获得导航路径并控制农业机械沿着目标路径行驶，辅助驾驶员操作农业机械进行作业。拖拉机自动导航是农业现代化的重要基础，实现拖拉机自动导航可以让农业作业者降低工作强度，并且能显著地提高农机的作业精度，提高农田的土地利用率，降低生产成本，提高作物产量与经济效益，将劳动者从繁重的农田作业中解放出来。

2、拖拉机导航技术在北美、日本和欧洲一些国家中研究起步较早，并取得了较多研究成果，国内的专家学者也做了广泛的研究，取得了一定的成果。如申请号为2014101751440的发明专利公开了一种对射式激光拖拉机辅助导航控制系统，授权号101833334a的发明专利公开了一种拖拉机自动导航控制系统及其方法，申请号为104656647a的发明专利公开了一种低矮作物田间自走拖拉机导航控制系统等。可以看出，采用的都是单一导航定位技术，比如gps导航、激光发射导航技术等，每种导航方式各有优点，但是因单一导航技术受条件限制都存在一定的使用条件，很难能够提供连续、稳定高质量的定位信息。

技术实现思路

1、本发明要解决的技术问题是提供多传感器融合的拖拉机导航控制系统的定位方法，具有精确的车辆定位和路径跟踪性能。

2、本发明采用如下技术方案实现发明目的：

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

4、(1)本技术方案中导航控制系统通过采用北斗定位和机器视觉定位相结合的方式，能够实现两种导航技术的优势互补，可以避免使用单一定位造成的车辆位置误差；组合导航功能既可以用在有作物的农田作业中也可以用在无作物的农田中，组合导航定位技术在可靠性、准确性等方面都较单独导航情况要好。(2)采用双天线的北斗信号接收装置，并且采用位姿检测模块的数据对北斗定位信息进行修正，得到的拖拉机位置数据更精确；(3)不仅选取了拖拉机当前航线偏移量、当前航向角偏移量，而且选取了拖拉机预估路径曲度和参考车速进行处理，使导航控制系统具有良好的路径控制效果。

5、拖拉机当前航线偏移量、当前航向角偏移量、拖拉机预估路径曲度值由上位机通过北斗导航数据或图像数据计算输出；拖拉机参考车速vu(s)由上位机发送的车速vr(s)和车速采集vm(s)决定，按照如下公式确定：

6、

7、因拖拉机在低速行驶时，通过北斗导航获得的行驶速度其准确度低于车速检测模块获得的车速，所以在确定车速时，如果车速检测模块采集的车速vm(s)小于等于2公里/秒或接收的上位机的车速vr(s)等于0时，则确定的参考车速为vm(s)，这样可以避免因北斗导航在低速或故障时造成的速度误差；当车速检测模块采集的车速vm(s)大于2公里/秒或车速检测模块出现故障时，确定参考车速为上位机发送车速vr(s)。

8、作为对本技术方案的进一步限定：

9、换向阀控制模块分别与电压检测模块和液压转向系统电连接。

10、压力检测模块还与液压转向系统电连接。

11、上位机采用epcs-8980型工控机。

12、触摸屏选用四线电阻式触摸屏sv0804s-03。

13、北斗定位接收模块采用um220模块。

14、图像采集模块由okac1310ccd摄像头和信号调理电路组成。

15、位姿检测模块选用ahrs-3000小型航姿测量模块。

16、下位机采用16位飞思卡尔单片机mc9s12xs128mal。

17、数据存储模块采用两片32mb的sdram mt48lc4m32b2数据存储芯片。

18、压力检测模块由压力传感器mbs1250和信号调理电路组成。

19、can通信模块由tja1043t芯片与其外围电路组成，通过该模块，下位机能够将根据预估行驶距离和预估路径曲度确定的拖拉机期望速度发送给拖拉机控制器，确保在拖拉机行驶至终点或拐弯时的控制效果，特别在当前航线偏移量or(s)超过可允许最大航线偏移量om时，通过该模块给拖拉机控制器发送停车信号。

20、换向阀控制模块由d/a转换芯片dac0832、运算放大器lm358n及外围电路组成。

21、电压检测模块由运算放大器ad741和外围电路组成。

22、通过设计电压检测模块，能够对换向阀控制模块的输出电压实施闭环控制，通过实时检测输出电压，避免输出电压失控现象发生。

23、车速检测模块由测速传感器vb-z9400及信号调理电路组成。

24、车轮转角检测模块由线位移传感器hps-m1和信号调理电路组成。

25、电源模块由产生24v电压芯片lt4356is、产生5v电压芯片lm7805、产生正负15电压芯片md20-12d15、产生10v电压芯片ad581及外围电路组成。

26、串口通信模块由串口通信芯片max232芯片及其外围电路组成。

27、报警及紧急处理模块由喇叭、发光灯、光耦和继电器等组成。报警和紧急处理模块能够在液压阀运行至最大或当曲线路径弯度过大时发出报警信息并启动相应的紧急响应动作。

28、上位机通过串口通信模块发送给下位机用于路径跟踪的数据包括：当前航线偏移量、当前航向角偏移量、预估行驶距离、当前行驶速度、预估路径曲度。

29、本发明的多传感器融合的拖拉机导航控制系统的定位方法，通过上位机进行拖拉机定位，包括如下步骤：

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

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

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

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

34、(5)如果田间模式设定为无农作物田地，则完全采用经过位姿信息修正的北斗定位和路径数据；如果田间模式设定为有农作物田地，则采用模糊自适应扩展卡尔曼滤波器对北斗定位数据和机器视觉数据进行融合，其中北斗定位提供拖拉机的绝对位置坐标、航向角度和行驶速度，机器视觉提供导航路径中已知点的相对位置坐标，北斗定位系统和机器视觉系统坐标统一后，由滤波器对北斗提供的位置数据和机器视觉提供的位置数据进行滤波，得到精确的位置数据和路径数据。

35、与目前多传感器融合技术中常用的扩展卡尔曼滤波器和无迹卡尔曼滤波器相比，模糊自适应扩展卡尔曼滤波算法能够通过实时得到的测量新息方差和理论方差的比值，并利用模糊自适应控制器不断调整量测噪声协方差阵的加权系数，减少校正位置误差，提高拖拉机定位数据的准确性和稳定性。

36、一种多传感器融合的拖拉机导航控制系统定位后的控制方法，控制方法为路径跟踪控制，包括如下步骤：

37、(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)组成；

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

39、

40、其中δo＝os(s)-or(s)，kfb1为偏移量控制器比例系数，kfb2为偏移量控制器微分系数，kfb3为偏移量控制器二阶微分系数。因拖拉机路径跟踪的直接控制量是航线偏移量，本发明通过对偏移量差值实施比例、微分和二阶微分的控制，可减少控制过程中的超调，克服振荡现象发生；通过设定可允许最大航线偏移量om，可使拖拉机在航线偏差超过可允许范围时不再继续执行转向，并且控制偏移量控制器输出为0，避免损坏液压阀或安全事故发生；

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

42、

43、其中kff1为航向角控制器路径曲度控制比例系数，kff2为航向角控制器比例系数，kff3为航向角控制器微分比例系数。在航线偏移量允许值op范围内，通过将航向角作为参考量，可实施预先控制，减少路径跟踪过程的波动；将预估路径曲度作为参考量，可实现曲线行驶或拐弯时的预先控制，提高非直线路径的跟踪效果。

44、(4)副回路控制器为转角控制器cs(s)，其设定值为θfb与θff之和与车轮转角反馈a(s)的差值，参考输入量为车轮转角反馈a(s)和参考车速vu(s)，其中参考车速vu(s)由上位机发送的车速vr(s)和车速采集vm(s)决定，按照如下公式确定参考车速：

45、

46、转角控制器cs(s)的输出为车轮转角设定值θ，通过如下公式计算：其中kθ为转角控制器cs(s)的比例系数。因拖拉机在低速行驶时，通过北斗导航获得的行驶速度其准确度低于车速检测模块获得的车速，所以在确定车速时，如果车速检测模块采集的车速vm(s)小于等于2公里/秒或接收的上位机的车速vr(s)等于0时，则确定的参考车速为vm(s)，这样可以避免因北斗导航在低速或故障时造成的速度误差；当车速检测模块采集的车速vm(s)大于2公里/秒或车速检测模块出现故障时，确定参考车速为上位机发送车速vr(s)。将车速信号作为确定车轮转角设定值的参数，可减少系统调整时间；

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

48、与现有技术相比，本发明在控制量和参考量设定时，不仅选取了拖拉机当前航线偏移量、当前航向角偏移量，而且选取了拖拉机预估路径曲度和当前行驶速度，提出的拖拉机路径跟踪串级控制结构，主回路采取前馈加反馈控制方法，副回路采取单位负反馈方法，该控制方法不仅保证直线行驶时的路径跟踪效果，而且在拐弯时也具有很高的控制精度。