本发明涉及数据处理领域，具体涉及到一种新型的履带式拖拉机自动转向控制系统及控制方法。
背景技术：
随着gnss高精度卫星导航技术和自动化控制技术的普及，精准农业已经成为农业现代化的主要发展方向。拖拉机的自动化导航和控制是一类重要的精准农业装备，对于农业装备现代化具有重要的意义。拖拉机的自动控制按照控制方向可分为纵向的速度控制和横向的转向控制。由于绝大多数普及的拖拉机，如东方红、雷沃、约翰迪尔等拖拉机品牌，大都采用手动变速，需要通过换挡和油门配合才能实现，这就给纵向的拖拉机速度自动控制带来了诸多困难，因此市面上拖拉机自动控制技术主要集中于横向控制。许多导航厂商的拖拉机横向自动控制设备已经在播种、打药、起垄等农业作业中得到了广泛应用。
拖拉机按照行走方式分为轮式拖拉机和履带式拖拉机。其中履带式拖拉机具有对土壤单位面积压力小和对土壤附着性能好等特点，在潮湿和松软的土壤工况，具有比轮式拖拉机更好的通过性能，因此在水田地、潮湿地等作业环境下的履带式拖拉机相比于轮式拖拉机具有先天的优势。目前由于轮式拖拉机市场占有率很高，许多导航厂家的拖拉机导航和自动控制设备大都是针对轮式拖拉机，对履带式拖拉机的自动转向解决方案还比较少。
技术实现要素：
为了解决上述的缺陷，本发明提供了一种新型的履带式拖拉机自动转向控制系统及控制方法，以实现履带式拖拉机的自动转向功能。
本发明提供了一种新型的履带式拖拉机自动转向控制系统，包括导航系统，所述导航系统用于获取拖拉机的运动状态；显示设备，所述显示设备用于存储期望的拖拉机运行轨迹参数和显示拖拉机实时运动信息；自动转向控制器，所述自动转向控制器用于接收导航系统数据和显示设备的数据，实时计算控制量并将控制量以pwm脉宽信号的形式输出给电机驱动器；电机驱动器，所述电机驱动器用于控制电机的位置、速度或力矩；直流无刷电机，所述直流无刷电机用于带动转向操纵机构动作；电机和转向操纵机构连接件，所述电机和转向操纵机构连接件用于连接直流无刷电机和转向操纵机构。
上述的一种新型的履带式拖拉机自动转向控制系统，其中，所述导航系统为gnss和imu组合的导航系统。
上述的一种新型的履带式拖拉机自动转向控制系统，其中，所述显示设备为上位机显示设备，所述轨迹参数包括直线轨迹参数a、b、c和期望航向角dyaw，所述拖拉机实时运动信息包括位置、速度、航向。
上述的一种新型的履带式拖拉机自动转向控制系统，其中，所述电机和转向操纵机构连接件还用于将转向操纵机构手动操作方式改造成电机控制方式，并根据不同的履带式拖拉机转向操纵机构形式设计不同的机械连接方式。
上述的一种新型的履带式拖拉机自动转向控制系统，其中，所述gnss和imu组合的导航系统通过串口输出位置、航向、速度信息给自动转向控制器，所述显示设备通过can总线输出期望轨迹参数和期望航向给自动转向控制器。
上述的一种新型的履带式拖拉机自动转向控制系统，其中，所述自动转向控制器通过pwm信号输出控制量给电机驱动器。
本发明的另一面提供了一种新型的履带式拖拉机自动转向控制方法，包括以下步骤：
步骤(1):获取拖拉机位置、航向和速度信息；
步骤(2):计算拖拉机的横向偏差和航向偏差；
步骤(3):建立拖拉机线性化运动学状态空间模型；
步骤(4):计算转向控制量；
步骤(5):计算pwm信号脉宽。
上述的方法，其中，所述步骤(1)具体包括：通过gnss接收机接收卫星导航信号，通过imu测量拖拉机自身的运动角速度和加速度信息，然后将gnss和imu获取的数据由卡尔曼滤波算法进行融合，输出滤波后的拖拉机位置坐标x和y、航向yaw和速度v并通过串口输出给自动转向控制器。
上述的方法，其中，所述步骤(2)具体包括：取北向坐标系为x方向，东向坐标系为y方向，期望直线轨迹的航向为dyaw，期望直线轨迹方程为：
ax+by+c＝0公式(1)
根据步骤(1)获取的实时拖拉机位置、航向信息，以及期望的拖拉机直线轨迹参数和期望航向，计算得到拖拉机当前的横向偏差xt公式(2)和航向偏差xh公式(3)，直线轨迹参数a、b、c和期望航向dyaw由上位机显示设备计算得到，并通过can总线下发至自动转向控制器；
xh＝dyaw-yaw公式(3)。
上述的方法，其中，所述步骤(3)具体包括：根据步骤(1)得到的拖拉机速度信息v和步骤(2)得到的横向偏差xt和航向偏差xh，取横向偏差xt和航向偏差xh作为系统状态，建立履带式拖拉机线性化运动学状态空间模型公式(4)和公式(5)，进而获取履带式拖拉机的运动学模型状态矩阵a、b、c、d，u为控制量，dxt和dxh分别表示横向偏差的导数和航向偏差的导数：
d＝0公式(9)
记ts为控制周期，则离散化状态空间矩阵ad、bd、cd、dd如下：
cd＝c公式(12)
dd＝0公式(13)；
所述步骤(4)具体包括：根据步骤(3)得到的离散化状态矩阵ad、bd、cd、dd，和步骤(2)计算的当前周期横向偏差xt和航向偏差xh，按照如下mpc模型预测控制算法计算公式得到控制量u；
构造矩阵f和g；
u＝(gtqg+r)-1(gtq(ref-fx))公式(16)
其中ref为参考输入向量，可取零向量，
所述步骤(5)具体包括：根据步骤(4)得到的控制量u，通过转化公式计算左右两个电机驱动器的pwm输入信号脉宽，由自动转向控制器产生相应的脉宽pwm信号，电机驱动器通过接收到的脉宽变化控制无刷电机动作，进而带到转向操纵机构动作，最终实现履带式拖拉机自动转向，计算方式如下：
如果u大于0，则向右转向，右侧履带制动，
pwm左＝pwmmin公式(18)
如果u小于0，则向左转向，左侧履带制动，
pwm右＝pwmmin公式(20)
如果u等于0，则不转向，
pwm左＝pwmmin公式(21)
pwm右＝pwmmin公式(22)
其中umax为最大制动力矩，根据不同的履带式拖拉机进行标定；pwmmin和pwmmax是电机驱动器接收的最小和最大pwm信号脉宽，当驱动器接收到最小脉宽信号时，电机不动作，当接收到最大脉宽pwm信号时，电机处于最大动作状态，最大最小脉宽值由不同的电机驱动器接收信号协议确定。
本发明具有以下有益效果：提出了一种用于履带式拖拉机自动转向的控制方法及系统，以实现履带式拖拉机的自动转向功能。
附图说明
通过阅读参照以下附图对非限制性实施例所作的详细描述，本发明及其特征、外形和优点将会变得更明显。在全部附图中相同的标记指示相同的部分。并未刻意按照比例绘制附图，重点在于示出本发明的主旨。
图1是本发明履带式拖拉机自动转向系统原理图；
图2是本发明履带式拖拉机直线跟踪示意图；
图3是本发明履带式拖拉机自动转向控制方法算法框图；
图4是本发明履带式拖拉机自动转向控制器算法实现流程图。
具体实施方式
在下文的描述中，给出了大量具体的细节以便提供对本发明更为彻底的理解。然而，对于本领域技术人员而言显而易见的是，本发明可以无需一个或多个这些细节而得以实施。在其他的例子中，为了避免与本发明发生混淆，对于本领域公知的一些技术特征未进行描述。
为了彻底理解本发明，将在下列的描述中提出详细的步骤以及详细的结构，以便阐释本发明的技术方案。本发明的较佳实施例详细描述如下，然而除了这些详细描述外，本发明还可以具有其他实施方式。
在本发明中，首先要说明的是，履带式拖拉机的转向原理：
履带式拖拉机的转向由改变两侧履带驱动力而产生的转向力矩实现，而履带驱动力的改变，是通过履带转向系进行制动实现，因此为了实现履带式拖拉机转向需要对两侧履带转向系进行制动。常见履带式拖拉机的转向系由转向机构和转向操纵机构组成，转向机构按照机械结构不同可分为离合器式、行星齿轮式和双差速器式，通常左右各有一个转向操纵机构。转向操纵机构通常是由人手动操作以对左右两侧履带进行制动，进而改变两侧履带的驱动力，当左侧的制动力大于右侧时，拖拉机朝左侧转向，同样当右侧的制动力大于左侧时拖拉机朝右侧转向。
为了便于实现对不同履带式拖拉机转向机构的动转向控制，需要对其转向操纵机构的驱动方式进行改造，将操纵机构的手动操作方式改造成电机驱动方式，通过电机控制操纵机构动作进而实现制动。本发明不针对某一类具体履带式拖拉机转向系，也不涉及具体电机驱动方式的改造方案，而是把自动转向系统从功能上分解为转向机构、转向操纵机构和电机驱动装置，其中电机驱动装置包含直流无刷电机和电机驱动器，从而实现一种通用的履带式拖拉机自动转向控制方法及系统。
参照图1-图4所示，本发明提供了一种新型的履带式拖拉机自动转向控制系统，包括gnss和imu组合导航系统，用于获取拖拉机实时位置、航向和速度等运动状态；上位机显示设备，用于存储期望的拖拉机运行轨迹参数，如直线轨迹参数a、b、c和期望航向角dyaw。显示拖拉机实时运动信息，如位置、速度、航向等。自动转向控制器，用于接收gnss和imu组合导航系统数据和上位机显示设备数据，实时计算控制量并将控制量以pwm脉宽信号的形式输出给电机驱动器。电机驱动器，用于控制电机的位置、速度或力矩。直流无刷电机，用于带动转向操纵机构动作。电机和转向操纵机构连接件，用于连接直流无刷电机和转向操纵机构，将转向操纵机构手动操作方式改造成电机控制方式，根据不同的履带式拖拉机转向操纵机构形式设计不同的机械连接方式。转向操纵机构进而带动转向系实现制动，改变左右两侧履带的制动力矩，从而实现拖拉机转向。还包括其它附属模块，例如：自动转向系统供电电源、连接线缆等。
在本发明中，各模块之间的相互作用方式如下：gnss和imu组合导航系统通过串口输出位置、航向、速度等信息给自动转向控制器；上位机显示设备通过can总线输出期望轨迹参数和期望航向给自动转向控制器；自动转向控制器通过pwm信号输出控制量给电机驱动器；电机驱动器通过调节电流控制电机动作，控制电机转速、位置和力矩等；电机和转向操纵机构机械连接，带动转向操纵机构动作；转向系操纵机构带动转向机构动作，实现履带制动。
在本发明中，参照图2所示，本发明的履带式拖拉机自动转向控制方法特点如下：
本控制方法是基于测量误差的负反馈控制算法，通过计算期望值和反馈值的差值即控制误差，这里的误差包括横向偏差和航向偏差，带入控制器计算控制量；
本控制方法是基于履带式拖拉机运动学模型的控制算法，算法里用到了现代控制理论中的状态空间模型和状态空间矩阵，利用mpc控制算法求解得到控制量。只需根据建立的履带式拖拉机状态空间模型，计算状态空间系数矩阵，按照mpc控制算法求解公式，即可计算得到控制量；
本转向系统的控制器和电机驱动器的信号传递方式是pwm脉宽信号，即通过脉宽表示传递的控制量大小。当pwm信号处于最大脉宽时，电机产生最大动作带动转向操纵机构产生最大制动力，当pwm信号处于最小脉宽时，电机不产生动作，因此没有制动力；
本控制方法具有两路pwm脉宽信号输出，当其中一路pwm输出大于最小脉宽时，另一路pwm输出处于最小脉宽，两路不同时输出大于最小脉宽的信号；
本转向系统的电机驱动器根据转向系操纵机构实现原理的不同，可能控制直流无刷电机的位置、速度或者力矩来控制转向系操纵机构的位置、速度或者作用力，进而带动转向机构产生不同的制动力。
如图3所示，本发明的履带式拖拉机自动转向控制方法，可按照以下具体步骤实现：
步骤(1)：获取拖拉机位置、航向和速度信息，具体包括：gnss和imu组合导航系统需要固定在拖拉机顶部靠近后轴中心位置。一方面由gnss接收机接收卫星导航信号(如gps、伽利略、格拉纳斯和北斗导航系统)，为了获取更高精度的位置信息，需要使用rtk实时运动差分定位技术；另一方面由imu(惯性测量单元)测量拖拉机自身的运动角速度和加速度信息。将gnss和imu获取的数据由卡尔曼滤波算法进行融合，输出滤波后的拖拉机位置坐标x和y、航向yaw和速度v等信息，这里的位置和航向是在当地水平面坐标系(ned或enu坐标系)。测量信息通过串口输出给自动转向控制器。
步骤(2)：计算横向偏差和航向偏差，具体包括：如图2所示为履带式拖拉机直线轨迹跟踪示意图，取北向坐标系为x方向，东向坐标系为y方向，期望直线轨迹的航向为dyaw，期望直线轨迹方程为：
ax+by+c＝0(公式1)
根据步骤(1)获取的实时拖拉机位置、航向信息，以及期望的拖拉机直线轨迹参数和期望航向，计算得到拖拉机当前的横向偏差xt(公式2)和航向偏差xh(公式3)。直线轨迹参数a、b、c和期望航向dyaw由上位机显示设备计算得到，并通过can总线下发至自动转向控制器。
xh＝dyaw-yaw(公式3)
步骤(3)：计算离散系统状态空间方程矩阵，具体包括：根据步骤(1)得到的拖拉机速度信息v和步骤(2)得到的横向偏差xt和航向偏差xh，取横向偏差xt和航向偏差xh作为系统状态，建立履带式拖拉机线性化运动学状态空间模型(公式4和公式5)，进而获取履带式拖拉机的运动学模型状态矩阵a、b、c、d，u为控制量，dxt和dxh分别表示横向偏差的导数和航向偏差的导数。
d＝0(公式9)
记ts为控制周期，则离散化状态空间矩阵ad、bd、cd、dd如下：
cd＝c(公式12)
dd＝0(公式13)
步骤(4)：计算转向控制量，具体包括：根据步骤(3)得到的离散化状态矩阵ad、bd、cd、dd，和步骤(2)计算的当前周期横向偏差xt和航向偏差xh，按照如下mpc模型预测控制算法计算公式得到控制量u。
1、构造矩阵f和g
2、求解控制量序列u
u＝(gtqg+r)-1(gtq(ref-fx))(16)
公式中ref为参考输入向量，可取零向量，
步骤(5)：计算pwm信号脉宽，具体包括：根据步骤(4)得到的控制量u，通过转化公式计算左右两个电机驱动器的pwm输入信号脉宽，由自动转向控制器产生相应的脉宽pwm信号。电机驱动器通过接收到的脉宽变化控制无刷电机动作，进而带到转向操纵机构动作，最终实现履带式拖拉机自动转向。计算方式如下：
如果u大于0，则向右转向，右侧履带制动
pwm左＝pwmmin(公式18)
如果u小于0，则向左转向，左侧履带制动
pwm右＝pwmmin(公式20)
如果u等于0，则不转向
pwm左＝pwmmin(公式21)
pwm右＝pwmmin(公式22)
其中umax为最大制动力矩，根据不同的履带式拖拉机进行标定；pwmmin和pwmmax是电机驱动器接收的最小和最大pwm信号脉宽，当驱动器接收到最小脉宽信号时，电机不动作，当接收到最大脉宽pwm信号时，电机处于最大动作状态，最大最小脉宽值由不同的电机驱动器接收信号协议确定。
在本发明中，如图1所示，本发明提出的履带式拖拉机自动转向控制系统由gnss和imu组合导航系统、上位机显示设备、自动转向控制器、直流电机驱动器、直流无刷电机、电机与转向操纵机构连接件、附属供电电源和连接线组成；其中gnss和imu组合导航系统可以接收gps、伽利略、格拉纳斯和北斗等卫星导航系统卫星信号，利用rtk技术提高实时定位精度，并通过卡尔曼滤波算法进行数据融合，得到更精确的拖拉机位置、速度和航向信息，并通过串口将数据信息发送至自动转向控制器；上位机显示设备用以显示拖拉机当前位置、速度和航向信息，并根据期望直线轨迹和航向计算拖拉机期望轨迹参数a、b、c和期望航向dyaw，通过can总线方式将期望轨迹参数发送至自动转向控制器；自动转向控制器根据接收到的当前拖拉机位置、速度、航向以及期望的轨迹参数和航向数据，计算拖拉机实时的横向偏差xt和航向偏差xh并更新离散状态矩阵ad、bd、cd和dd，自动转向控制器将横向偏差xt、航向偏差xh和状态矩阵ad、bd、cd和dd，由mpc控制算法计算拖拉机转向控制量，最后将控制量转化为左右两侧电机驱动器的pwm脉宽信号，输入给电机驱动器。根据履带式拖拉机转向系的不同，以离合式转向系为例，设计电机和离合操纵机构连接结构件，通过控制直流电机的旋转角度进而控制离合的开合动作，从而产生左右两侧不同的制动力，实现拖拉机转向。
如图2所示，本发明提出的履带式拖拉机自动转向控制方法，横向偏差和航向偏差由拖拉机实时位置、航向以及期望直线轨迹方程和期望航向根据所述公式计算得到。
如图3所示，本发明提出的履带式拖拉机自动转向控制方法，是基于测量负反馈的mpc控制算法，控制的测量数据来源gnss和imu组合导航系统测量的拖拉机位置和航向数据。控制误差是通过上位机显示设备存储的期望直线轨迹和上述测量信息计算得到的横向偏差xt和航向偏差xh。
如图4所示，本发明提出的履带式拖拉机自动转向控制方法，计算流程是先获取gnss和imu组合导航数据，再获取上位机导航设备的期望轨迹数据，根据上述数据和当前的控制周期，更新系统离散状态空间矩阵，再将状态矩阵、横向偏差和航向偏差带入mpc控制算法进行控制量求解，最后将求解的控制量转化到左右两侧电机驱动器输入的脉宽信号，进而实现左右两侧履带不同的制动力，最终实现履带式拖拉机自动转向控制。
在本发明一优选而非限制性的实施例中，履带式拖拉机转向操纵机构由直流无刷电机驱动；在本发明中，直流无刷电机可以是旋转电机也可以是直线电机，根据转向操纵机构的特点进行选择。
在本发明一优选而非限制性的实施例中，直流无刷电机由驱动器进行控制。
在本发明一优选而非限制性的实施例中，履带式拖拉机转向操纵机构和电机之间通过连接件机械连接，且机械连接件根据不同的转向操纵机构可以有不同的机械结构。
在本发明一优选而非限制性的实施例中，自动转向控制器和驱动器之间通过pwm脉宽传递控制量信号，进一步优选，自动转向控制器具有两路pwm脉宽信号，分别控制左右两个直流无刷电机驱动器，再优选，两路pwm脉宽信号，当向拖拉机右侧转向时，右侧pwm脉宽大于最小值，右侧电机动作，左侧pwm脉宽等于最小值，左侧电机不动作；当向左侧转向时，左侧pwm脉宽大于最小值，左侧电机动作，右侧pwm脉宽等于最小值，右侧电机不动作；当不进行转向时，两侧pwm脉宽均为最小值，两侧电机均不动作。
在本发明一优选而非限制性的实施例中，gnss和imu组合导航系统测量履带拖拉机的实际位置、航向和速度信息，以及gnss和imu组合导航系统和自动转向控制器通过串口进行数据传递。
在本发明一优选而非限制性的实施例中，上位机显示设备和自动转向控制器通过can总线进行数据传递。
在本发明一优选而非限制性的实施例中，自动转向控制器输入横向偏差和航向偏差由上位机显示软件下发的期望轨迹和航向参数和所述的gnss和imu组合导航输入的实际测量位置和航向，根据本发明所述的计算公式计算得到。
在本发明一优选而非限制性的实施例中，自动转向控制器根据gnss和imu组合导航系统提供的速度信息，根据本发明所述的计算公式建立状态空间模型，计算离散状态空间矩阵模型ad、bd、cd、dd，以及控制量根据状态空间矩阵、横向偏差和航向偏差离散状态空间矩阵，由mpc控制算法计算得到。
以上对本发明的较佳实施例进行了描述。需要理解的是，本发明并不局限于上述特定实施方式，其中未尽详细描述的设备和结构应该理解为用本领域中的普通方式予以实施；任何熟悉本领域的技术人员，在不脱离本发明技术方案范围情况下，都可利用上述揭示的方法和技术内容对本发明技术方案做出许多可能的变动和修饰，或修改为等同变化的等效实施例，这并不影响本发明的实质内容。因此，凡是未脱离本发明技术方案的内容，依据本发明的技术实质对以上实施例所做的任何简单修改、等同变化及修饰，均仍属于本发明技术方案保护的范围内。