一种拖拉机自动转向系统的建模系统的制作方法

本发明涉及自动控制技术，具体涉及拖拉机自动转向系统的建模与控制方案。

背景技术：

如今越来越多的拖拉机已经集成或者加装了自动导航驾驶系统，用于在起垄、播种、植保等不同的田间作业过程中实现拖拉机的自动驾驶与转向，从而保证理想的作业效果。

然而，在拖拉机自动转向控制系统的研发过程中，如果不对自动转向系统进行充分建模，则其控制算法的参数只能通过在实际的自动驾驶实验中根据经验进行人为调试。这样会导致调试效率低下，不容易实现理想的控制效果，并且较难找到影响控制效果的因素。市面上还缺少一套较为完整通用的拖拉机自动转向控制系统的理论仿真模型，用于自动转向控制算法的仿真与调试。

技术实现要素：

针对目前市场上所存在上述的问题，需要一种较为完善的拖拉机自动转向系统的理论模型来实现对控制算法较为方便的验证方案。

为此，本发明所要解决的问题是提供一种拖拉机自动转向系统的建模系统，以克服现有技术所存在的缺陷。

为了解决上述问题，本发明提供的拖拉机自动转向系统的建模系统，包括：

期望转角决策程序模块，首先根据期望路径计算出拖拉机的期望航向轨迹角，接着结合拖拉机的实际位置(x,y)，计算出拖拉机当前位置与期望行驶轨迹之间的垂向距离，即横向位置偏差；通过该横向位置偏差与40m的比值求出航向偏差角；最后将期望航向轨迹角与航向偏差角之和作为航向角控制指令输出至航向角控制器程序模块；

航向角控制器，根据期望转角决策程序模块输出的航向角控制指令结合拖拉机的实际航向角计算出航向角速率指令，并输出至角速率控制器；

角速率控制器，根据航向角控制器输出的航向角速率指令结合拖拉机的实际向角速率计算出指令电流值，并输出至电流至前轮转角程序模型；

电流至前轮转角程序模型，根据角速率控制器输出的指令电流值，计算出拖拉机前轮转角值，并输出至前轮转角至航向角程序模型；

前轮转角至航向角程序模型，根据电流至前轮转角程序模型输出的拖拉机前轮转角值计算出拖拉机的实际航向角和拖拉机的实际航向角速率，并将拖拉机的实际航向角传至航向角控制器和航向角至车辆位置程序模型，将拖拉机的实际航向角速率输出至角速率控制器；

航向角至车辆位置程序模型，根据前轮转角至航向角程序模型输出的拖拉机的实际航向角计算出拖拉机当前的相对位置坐标(x,y)，并输出至期望转角决策程序模块。

进一步的，所述航向角控制器采用比例-积分控制方法对拖拉机行驶过程中的航向角进行控制，所述比例-积分控制方法通过如下计算公式来实现：

其中，为航向角控制器所计算出的航向角速率指令，kp，ki分别为比例控制参数与积分控制参数，θ为拖拉机的实际航向角。

进一步的，所述角速度控制器利用比例控制算法对拖拉机行驶过程中的航向角速率进行实时控制，通过如下计算公式来实现：

其中，i为角速度控制器所计算出的指令电流，ka为比例控制参数，为拖拉机的实际航向角速率。

进一步的，所述电流至前轮转角程序模型包括死区程序模块，一个转角增益程序模块以及一个积分环节程序模块，通过死区程序模块计算的结果乘以转角增益程序模块提供的转角增益系数后再由积分环节程序模块对时间进行积分可以得到大致的拖拉机前轮转角。

进一步的，所述前轮转角至航向角程序模型里包含有车速及车身长度信息。

进一步的，所述航向角至车辆位置程序模块通过对车速与当前航向角的正弦值与余弦值的乘积进行积分，可以求出拖拉机当前的相对位置坐标(x,y)。

本发明提供的拖拉机自动转向系统的建模方案，具备以下的技术优点：

1.本方案可以对不同型号的拖拉机的自动驾驶转向系统进行建模，从而方便的调试与验证其控制算法，节省控制算法的开发周期。

2.本方案对拖拉机自动驾驶过程中的电磁阀非线性特征，拖拉机自身特性以及转向特性进行了充分建模，模型完善准确。

3.本方案利用期望轨迹航向角与横向偏差相结合的方式生成航向角控制指令，对拖拉机的航向进行实时控制；该方法可以实现对航向轨迹的准确跟踪控制。

附图说明

以下结合附图和具体实施方式来进一步说明本发明。

图1为发明实例涉及到的拖拉机自动转向系统的系统组成图；

图2为本发明实例提供的拖拉机自动转向系统的建模方案的系统示意图；

图3为本发明实例中电流至前轮转角部分的模型内部结构图；

图4为本发明实例中通过建模所得到的拖拉机行驶轨迹的控制效果图。

具体实施方式

为了使本发明实现的技术手段、创作特征、达成目的与功效易于明白了解，下面结合具体图示，进一步阐述本发明。

如图1所示，一般的拖拉机自动驾驶转向系统包括导航仪11，控制器12，电磁阀13，拖拉机14，陀螺仪15，以及gps模块16。其中导航仪11，控制器12，电磁阀13以及拖拉机14依次数据连接，而陀螺仪15分别与控制器12和拖拉机14数据连接，gps模块16分别与导航仪11与拖拉机14数据连接。

针对上述常见的拖拉机自动驾驶转向系统，本实例建模形成相应的模拟系统。

参见图2，本实例给出的建模系统主要包括：期望转角决策程序模块1，航向角控制器2，角速率控制器3，电流至前轮转角程序模型4，前轮转角至航向角程序模型5，以及航向角至车辆位置程序模型6。

其中，期望转角决策模块1用于模拟导航仪11对拖拉机14实际航向位置(x,y)的控制功能。

航向角控制器2与角速率控制器3用于模拟控制器12中对拖拉机14自动驾驶转向过程中航向角与航向角速率的控制功能。

电流至前轮转角模块4基于电流环往往带宽较高的特性，在模型中将电流环省略，形成增益为1的电流控制效果，该模块模拟电磁阀13根据电流大小对拖拉机14前轮转角的控制效果。

前轮转角至航向角模块5模拟了拖拉机14行驶过程中航向角与前轮转角之间的关系。该模块输出的航向角速率代表陀螺仪15实际测试得到的航向角速率。

航向角至车辆位置模块6模拟拖拉机14在行驶过程中相对位置坐标(x,y)与航向角θ之间的关系。该模块输出的航向位置(x,y)代表gps模块16实际测试得到的航向位置。

基于上述原理，本建模系统在具体实现时，期望转角决策模块1利用期望轨迹航向角与横向偏差相结合的方式生成航向角控制指令α。首先，根据期望路径计算出拖拉机的期望航向轨迹角γ，计算方法为：

其中，xe,ye为期望轨迹中的坐标值，xe(n),ye(n)为当前的期望位置点的坐标值。之后，结合拖拉机的实际位置(x,y)，计算出拖拉机当前位置与期望行驶轨迹之间的垂向距离d，即横向位置偏差。计算方法为：

其中，k和b分别为期望轨迹的斜率和截距。于是，期望转角决策模块1通过将该偏差d与40m做比值求出航向偏差角。最后，将期望航向轨迹角γ与航向偏差角之和作为航向角控制指令α：

本期望转角决策模块1还将计算得到的航向角控制指令α输出至航向角控制器2。

本建模系统中的航向角控制器2，其具体根据期望转角决策程序模块1输出的航向角控制指令α结合拖拉机的实际航向角θ计算出航向角速率指令并输出至角速率控制器3。

该航向角控制器2采用比例-积分控制算法模拟对拖拉机14行驶过程中的航向角进行控制。通过在控制模型中加入了积分环节，有效的消除了电磁阀13死区对控制精度的影响，提升了轨迹跟踪精度。所采用的比例-积分控制算法的计算公式为：

其中，为航向角控制器所计算出的航向角速率指令，kp，ki分别为比例控制参数与积分控制参数，θ为拖拉机的实际航向角。

本建模系统中的角速度控制器3，其具体根据航向角控制器2输出的航向角速率指令结合拖拉机的实际向角速率计算出指令电流值i，并输出至电流至前轮转角程序模型4。

该角速度控制器3利用比例控制算法对拖拉14机行驶过程中的航向角速率进行实时控制，计算公式为：

其中，i为角速度控制器所计算出的指令电流，ka为比例控制参数，为拖拉机的实际航向角速率。

本建模系统中的电流至前轮转角程序模型4，具体根据角速率控制器3输出的指令电流值i，计算出拖拉机前轮转角值β，并输出至前轮转角至航向角程序模型5。

如图3所示，该电流至前轮转角程序模型4里包含一个死区模块12，一个转角增益模块11以及一个积分环节10，三组依次配合。其中死区模块12用于模拟电磁阀13的死区特性，该电磁阀13为控制拖拉机14实现自动转向的执行机构；转角增益模块11用于进行转角增益计算，积分环节10完成积分计算。通过在模型中加入了死区模块12，可以更为真实的模拟自动转向拖拉机的实际工况。所建立的电流至前轮转角程序模型4的表达式为：

其中，fs(i)为死区模块的函数，β为前轮转角，kfa为转角增益系数，kf为流速增益系数，id与iu分别为死区的下限与上限。当电流i在死区区间[id,iu]中时，电磁阀中的液体流速为0。只有当电流大小大于死区的上限或者下限的绝对值时，电磁阀中的液体流速才不为0。拖拉机的前轮转角与其转向油缸中的液体流量成正比关系，而该液体流速与电磁阀线圈中的电流呈线性关系。因此，通过fs(i)乘以转角增益系数后对时间进行积分可以得到大致的拖拉机前轮转角。

本建模系统中的前轮转角至航向角程序模型5，根据电流至前轮转角程序模型4输出的拖拉机前轮转角值β计算出拖拉机的实际航向角θ和拖拉机的实际航向角速率并将拖拉机的实际航向角θ传至航向角控制器2和航向角至车辆位置程序模型6，将拖拉机的实际航向角速率输出至角速率控制器3。

该前轮转角至航向角程序模型5里包含了拖拉机14车速及拖拉机14车身长度等信息，可以更准确的反应拖拉机的真实转向特性。其表达式为：

其中，v为拖拉机的直线行驶速度，lc为拖拉机的车身长度。

本建模系统中的航向角至车辆位置程序模型6，其具体根据前轮转角至航向角程序模型5输出的拖拉机的实际航向角θ计算出拖拉机当前的相对位置坐标(x,y)，并输出至期望转角决策程序模块1。

该航向角至车辆位置程序模块6通过对车速与当前航向角的正弦值与余弦值的乘积进行积分求出拖拉机当前的相对位置坐标(x,y)，具体公式为：

参见图4，其所示为利用本实例给出的建模系统对一条预设的期望路线进行跟踪的控制效果图。其中，期望路线为一条期望航向角为0.3rad，路线长度大致650m的直线轨迹。拖拉机的起点为坐标原点(0,0)。基于本发明所提出的仿真模型与控制算法对该自动驾驶过程进行了仿真。可以看出，本实例给出的方案可以实现对驾驶路径较为准确的跟踪与控制，控制精度较高。

以上显示和描述了本发明的基本原理、主要特征和本发明的优点。本行业的技术人员应该了解，本发明不受上述实施例的限制，上述实施例和说明书中描述的只是说明本发明的原理，在不脱离本发明精神和范围的前提下，本发明还会有各种变化和改进，这些变化和改进都落入要求保护的本发明范围内。本发明要求保护范围由所附的权利要求书及其等效物界定。