CN115480570A - 农机车辆自动导航控制方法和装置、农机车辆 - Google Patents

Abstract

本发明涉及车辆控制技术领域，尤其涉及一种农机车辆自动导航控制方法和装置、农机车辆，该控制方法包括：基于车辆的运动学模型，基于纯追踪控制算法获取车辆前轮转角前馈量δ₁；同时采用模糊控制算法根据车辆的横向偏差和航向偏差得到车辆的前轮转角补偿量δ_re；根据前轮转角前馈量δ₁和前轮转角补偿量δ_re控制车辆完成自动导航。本发明在纯追踪控制算法的基础上，采用模糊控制算法作为反馈控制，模糊控制算法作为补偿机制，避免了跟踪过程中出现稳态误差，提高了农机车辆跟踪控制的精度。

Description

农机车辆自动导航控制方法和装置、农机车辆

技术领域

本发明涉及车辆控制技术领域，尤其涉及一种农机车辆自动导航控制方法和装置、农机车辆。

背景技术

目前自动导航技术多是应用于乘用车领域，农机自动导航技术应用过少，且多是针对直线跟踪或曲率固定的地头转弯等行驶过程，忽略了前方目标路径对车辆行驶状态的影响，其跟踪误差大，很难快速修正。近年来，学术界多将前视距离视为速度的函数方程，但农业作业场景下，农机作业速度较低，车速与前视距离耦合性较弱，该方法对不同速度的农机播种任务适应度较差。随着神经网络的发展，深度学习算法也在农机导航技术中得以应用，多是使用BP神经网络实现动态调整前视距离改进纯追踪算法，但该方法需要大量的高质量训练样本，需要对不同种类的农机的起始位姿样本进行训练，因此具有一定的局限性。

可见，现有技术中采用纯追踪算法对农机车辆进行控制以实现自动导航时，对于不同种类的农机具有一定的局限性，同时跟踪的误差较大。

发明内容

本发明提供一种农机车辆自动导航控制方法和装置、农机车辆，用以解决现有技术中控制农机车辆进行自动导航运动时误差较大的技术问题。

一方面，本发明提供一种农机车辆自动导航控制方法，包括：

基于车辆的运动学模型，采用纯追踪控制算法获取车辆前轮转角前馈量δ₁；

采用模糊控制算法根据车辆的横向偏差和航向偏差得到车辆的前轮转角补偿量δ_re；

根据所述前轮转角前馈量δ₁和前轮转角补偿量δ_re控制车辆完成自动导航。

根据本发明提供的一种农机车辆自动导航控制方法，所述采用纯追踪控制算法获取车辆前轮转角前馈量δ₁包括：

获取车辆后轮中心与预瞄点之间的距离L_d；

获取车辆后轮中心经过所述预瞄点所需转过的航向角α；

根据车辆的轴距L以及所述距离L_d和航向角α得到所述车辆前轮转角前馈量δ₁。

根据本发明提供的一种农机车辆自动导航控制方法，所述根据车辆的轴距L以及所述距离L_d和航向角α得到所述车辆前轮转角前馈量δ₁包括：

采用以下公式(1)根据车辆的轴距L以及所述距离L_d和航向角α得到所述车辆前轮转角前馈量δ₁：

根据本发明提供的一种农机车辆自动导航控制方法，所述获取车辆后轮中心与预瞄点之间的距离L_d以及获取所述航向角α包括：

获取车辆当前的车速；

根据车辆当前的车速采用预设的方法获取车辆前视区域的长度L_sum；

获取车辆前视区域的长度L_sum对应的弯曲度Ω；

根据所述前视区域的长度L_sum和弯曲度Ω计算预瞄点在所述前视区域内距离前视区域起点的曲线长度L_p；

根据所述前视区域内预瞄点和起点的曲线长度L_p确定所述距离L_d和航向角α。

根据本发明提供的一种农机车辆自动导航控制方法，所述根据车辆当前的车速采用预设的方法获取车辆前视区域的长度L_sum包括：

通过以下公式(2)根据车辆当前的车速v获取车辆前视区域的长度L_sum：

上述公式(2)中，L_{min_sum}为前视区域的最小长度，L_{max_sum}为前视区域的最大长度，v_min为设定的车辆最小行驶车速；v_max为设定的车辆的最大行驶车速；p和q均为预设的常数；

所述获取车辆前视区域的长度L_sum对应的弯曲度Ω包括：

将所述车辆前视区域离散成多个路径段，获取任意两个相邻路径段之间的夹角β_i，通过以下公式(3)对所有的夹角进行累加得到所述弯曲度Ω；

其中，公式(3)中n为相邻的路径段组成的所有夹角的个数。

根据本发明提供的一种农机车辆自动导航控制方法，所述根据所述前视区域的长度L_sum和弯曲度Ω计算预瞄点在所述前视区域内距离前视区域起点的曲线长度L_p包括：

通过以下公式(4)根据长度L_sum和弯曲度Ω计算预瞄点在所述前视区域内距前视区域起点的曲线长度L_p：

上述公式(4)中，Ω_min、Ω_max为车辆的前视区域的弯曲度的两个临界值；

其中，b＝L_sim/|Ω_min-Ω_max|。

根据本发明提供的一种农机车辆自动导航控制方法，所述采用模糊控制算法根据车辆的横向偏差和航向偏差得到车辆的前轮转角补偿量δ_re包括：

将所述横向偏差和航向偏差输入到模糊控制器中，所述模糊控制器用于对所述横向偏差和航向偏差进行模糊化处理，输出所述前轮转角补偿量δ_re；

所述横向偏差的基本论域为：[-0.12m，0.12m]，量化等级为：{-0.12，-0.08，-0.04，0，0.04，0.08，0.12}；

所述航向偏差的基本论域为：[-6°，6°]，量化等级为：{-6，-4，-2，0，2，4，6}；

所述前轮转角补偿量δ_re的基本论域为：[-3.6°，4.8°]，量化等级为：{-3.6，-2.4，-1.2，0，1.2，2.4，3.6}。

根据本发明提供的一种农机车辆自动导航控制方法，所述根据所述前轮转角前馈量δ₁和前轮转角补偿量δ_re控制车辆完成自动导航包括：

采用以下公式(5)根据前轮转角前馈量δ₁和前轮转角补偿量δ_re得到前轮转角δ_f；

δ_f＝δ₁+δ_re (5)

根据所述前轮转角δ_f控制车辆的前轮转动以完成自动化导航。

另一方面，本发明还提供一种农机车辆自动导航控制装置，包括:

前馈控制模块，用于基于车辆的运动学模型，采用纯追踪控制算法获取车辆前轮转角前馈量δ₁；

模糊控制模块，用于采用模糊控制算法根据车辆的横向偏差和航向偏差得到车辆的前轮转角补偿量δ_re；

主控制模块，用于根据所述前轮转角前馈量δ₁和前轮转角补偿量δ_re控制车辆完成自动导航。

另一方面，本发明还提供一种农机车辆，所述农机车辆包括如上所述的农机车辆自动导航控制装置。

本发明提供的一种农机车辆自动导航控制方法，该控制方法包括：基于车辆的运动学模型，采用纯追踪控制算法获取车辆前轮转角前馈量δ₁；同时采用模糊控制算法根据车辆的横向偏差和航向偏差得到车辆的前轮转角补偿量δ_re；根据前轮转角前馈量δ₁和前轮转角补偿量δ_re控制车辆完成自动导航。本发明在纯追踪控制算法的基础上，采用模糊控制算法作为反馈控制，模糊控制算法作为补偿机制，避免了跟踪过程中出现稳态误差，提高了农机车辆跟踪控制的精度。

附图说明

为了更清楚地说明本发明或现有技术中的技术方案，下面将对实施例或现有技术描述中所需要使用的附图作一简单地介绍，显而易见地，下面描述中的附图是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他的附图。

图1是本发明提供的导航控制方法的流程示意图；

图2是本发明提供的转角前馈量获取方法的流程示意图；

图3是本发明提供的农机车辆运动学模型示意图；

图4是本发明提供的追踪算法运动状态几何示意图；

图5是本发明提供的前视区域示意图；

图6是本发明提供的路径弯曲度计算示意图；

图7是本发明提供的模糊控制器原理框图；

图8是本发明提供的车辆导航偏差的确定方法示意图；

图9是本发明提供的模糊控制曲面示意图；

图10是本发明提供的导航控制装置结构示意图；

图11是本发明提供的电子设备的结构示意图。

具体实施方式

为使本发明的目的、技术方案和优点更加清楚，下面将结合本发明中的附图，对本发明中的技术方案进行清楚、完整地描述，显然，所描述的实施例是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有作出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。

本发明针对农机导航技术大多采用纯追踪控制算法(pure pursuit算法)实现农机的路径跟踪控制。纯追踪算法是一种模拟人工驾驶过程的几何路径跟踪控制方法，是农业机械自动导航技术中常用的控制算法，前视距离作为纯追踪算法的唯一输入参数，对农机的路径跟踪效果起到至关重要的作用。

本发明提出一种农机车辆自动导航控制方法，该方法为基于改进的动态调整前视距离的纯追踪算法和模糊控制算法相结合的路径跟踪方法，其中纯追踪控制模型作为前馈控制，动态调整前视距离输出前轮转角前馈量，充分考虑农机规划路径的道路曲度、速度等因素动态调整前视区域的积分长度，进而确定纯追踪控制模型的预瞄点，达到动态调整前视距离的目的。考虑到纯追踪控制算法是对前方道路采用预瞄机制，忽略了当前实时位置的误差影响，因此应用模糊控制模型作为反馈控制作为补偿机制，其中农机实时的横向误差和航向误差作为模糊控制器的输入，农机前轮转角补偿量作为其输出，避免了跟踪过程中出现稳态误差，提高了导航精度。

下面结合图1-图11对本发明的技术方案进行进一步的说明。

实施例一：

本实施例提供一种农机车辆自动导航控制方法，如图1，该导航控制方法包括：

步骤101：基于车辆的运动学模型，采用纯追踪控制算法获取车辆前轮转角前馈量δ₁。

本实施例中假设农机在田内或机耕道的路径跟踪过程中，行驶速度较低，行驶路面起伏程度较小，转向车轮转角一致，轮胎与地面不产生横向的相互作用，车辆航向角速度即为车辆横摆角速度，则可将农机简化为二轮车模型。本实施例以正北方向为Y轴正方向，正东方向为X轴正方向建立惯性坐标系，得到如图3所示的农机在惯性坐标系XOY下的车辆运动学模型。

在惯性坐标系XOY中，δ₁为前轮转角，其单位是rad；θ为车辆航向角，其单位是rad；A为车辆前轮中心点；B为车辆后轮中心点；v为车辆速度，其单位是m/s；L为车辆轴距，其单位是m；R为转向半径，其单位是m。A(x_f，y_f)为车辆二轮车模型前轮坐标，B(x_l，y_l)为车辆二轮车模型后轮坐标，以顺时针方向为正方向，取车辆与坐标系Y轴正方向的夹角，取值范围为0-360°。由图3几何关系可得二轮车模型前轮转角为；

δ₁＝arctan(L/R)

考虑前后轮几何约束及运动学约束可得车辆(本实施例为拖拉机)的车辆运动学模型为：

其中：

为车辆在坐标系X轴方向的速度，其单位是m/s；

为车辆在坐标系Y轴方向的速度，其单位是m/s；

为车辆航向角速度，其单位是rad/s。

本实施例提出一种动态调整纯追踪控制算法中前视距离的方法，该方法的主要流程如图2，根据农机的参数、实时位置、车速等确定前视区域。计算前视区域内道路路径点的曲度加和，根据目标路径的道路弯曲程度确定预瞄点，进而确定前视距离L_d和前视角α。最后基于纯追踪控制算法输出前轮转角实现路径跟踪。

换言之，本实施例中采用纯追踪控制算法获取车辆前轮转角前馈量δ₁包括以下步骤：获取车辆后轮中心与预瞄点之间的距离L_d；获取车辆后轮中心经过预瞄点所需转过的航向角α；根据车辆的轴距L以及距离L_d和航向角α得到车辆前轮转角前馈量δ₁。

其中，如图4，L_d为车辆后轮中心与预瞄点C的距离，即前视距离。α为使车辆后轮中心经过预瞄点所需转过的航向角，即前视角。R为车辆转弯半径。根据图4中的几何关系，本实施例可以得到以下公式(1),采用以下公式(1)根据车辆的轴距L以及距离L_d和航向角α得到车辆前轮转角前馈量δ₁：

其中，获取车辆后轮中心与预瞄点之间的距离L_d以及获取航向角α均是基于运动学模型实现，具体包括：获取车辆当前的车速；根据车辆当前的车速采用预设的方法获取车辆前视区域的长度L_sum；获取车辆前视区域的长度L_sum对应的弯曲度Ω；根据前视区域的长度L_sum和弯曲度Ω计算预瞄点在前视区域内距前视区域起点的曲线长度L_p；根据前视区域内预瞄点和起点的曲线长度L_p确定距离L_d和航向角α。

根据车速确定前视区域积分长度，前视区域示意图如图5所示。前视区域起点S(x_s，y_s)由最短前视距离L_dmin确定，L_dmin为农机前进方向上与农机后轮中心距离大于L_dmin的最近路径点。前视区域的长度L_sum将根据当前车速进行动态调整。车速越快、道路越平缓，前视区域更广，相反前视区域更小，结合上述分析，提出一种前视区域积分长度的确定方法。即通过以下公式(2)根据车辆当前的车速v获取车辆前视区域的长度L_sum：

上述公式(2)中，L_{min_sum}为前视区域的最小长度，其单位为m；L_{max_sum}为前视区域的最大长度，其单位为m；v_min为设定的车辆最小行驶车速，其单位为m/s；v_max为设定的车辆的最大行驶车速，其单位为m/s；p和q均为预设的常数。L_{min_sum}与L_{max_sum}根据实车试验进行确定后，采用一次线性函数的方法即可确定p和q。一般的，L_{min_sum}为0.3倍轴距，L_{max_sum}为一倍轴距。

其中，本实施例获取车辆前视区域的长度L_sum对应的弯曲度Ω包括：将车辆前视区域离散成多个路径段，获取任意两个相邻路径段之间的夹角β_i，通过以下公式(3)对所有的夹角进行累加得到弯曲度Ω；

其中，公式(3)中n为相邻的路径段组成的所有夹角的个数。

具体的，如图6，将前视区域离散成若干个路径点，用折线将相邻路径点连接，相邻折线的夹角β_i为相邻路径点方向角的改变量，前视区域的路径弯曲度Ω定义为：

式中：n为前视区域内的折线组成的夹角数量，n数值由前视区域内离散路径点的数量确定。

例如图6中相邻折线MQ和QP之间的夹角β_i由下式计算：

在前视区域内根据道路弯曲度确定预瞄点，提出一种前视距离L_d随路径弯曲度Ω变化而变化的方法，即预瞄点在前视区域内距前视区域起点的曲线长度L_p随目标路径弯曲度变化而变化的方法：

上述公式(4)中，Ω_min、Ω_max为车辆当前的前视区域的弯曲度的两个临界值，b为预设常数。

为了确保预瞄点能随路径弯曲度变化而连续变化，b的取值应与L_p的最大值相对应，则有：

b＝L_sum/|Ω_min-Ω_max|

确定L_p便可在前视区域内确定预瞄点，确定预瞄点后，结合图4的几何关系可知，根据起始点和预瞄点的几何关系，即可确定前视距离L_d与车辆前视角α，根据公式纯追踪控制算法计算前轮转角公式，便可得出车辆前轮转角前馈量δ₁。图4中，α为车辆前视角，其单位为rad；C为车辆预瞄点；L_d为车辆后轴中心与目标点的距离，其单位为m。

本实施例采用以下公式(5)根据前轮转角前馈量δ₁和前轮转角补偿量δ_re得到前轮转角δ_f；

δ_f＝δ₁+δ_re (5)

根据前轮转角δ_f控制车辆的前轮转动以完成自动化导航。

具体的，由于纯追踪控制算法在追踪预瞄点的过程中没有考虑农机当前位姿与参考路径的误差，可能会导致农机行驶过程中存在稳态误差的现象，因此需要进一步改进跟踪控制算法。本实施例提出一种基于模糊控制算法的模糊控制器实现对农机前轮转角的补偿。具体的，本实施例选用二维模糊控制器确定前轮转角补偿量δ_re。模糊控制器以农机横向偏差和航向偏差作为模糊控制器的输入，前轮转角补偿量δ_re作为其输出。前轮转角δ_f与前轮转角前馈量δ₁与前轮转角补偿量δ_re满足以下关系：

δ_f＝δ₁+δ_re

其中，本实施的模糊控制器原理框图如图7所示。

以农机横向偏差和航向偏差作为模糊控制器的输入，前轮转角补偿量δ_re作为其输出。首先对输入、输出变量进行模糊化处理。

(1)横向偏差e_c的基本论域为：[-0.12m，0.12m]，量化等级为：{-0.12，-0.08，-0.04，0，0.04，0.08，0.12}＝{NB，NM，NS，ZO，PS，PM，PB}，其量化因子取1。

(2)航向偏差e_h的基本论域为：[-6°，6°]，量化等级为：{-6，-4，-2，0，2，4，6}＝{NB，NM，NS，ZO，PS，PM，PB}，其量化因子取1。

(3)前轮转角补偿量δ_re的基本论域为：[-3.6°，4.8°]，量化等级为：{-3.6，-2.4，-1.2，0，1.2，2.4，3.6}＝{NB，NM，NS，ZO，PS，PM，PB}，其量化因子取1。

本发明中输入输出量的隶属函数均选定为三角隶属度函数。横向偏差e_c与航向偏差e_h确定方法如图8。图8中R(x_r，y_r)、T(x_t，y_t)分别为目标路径点中距离车辆后轮中心的最近点与次近点。车辆后轴中心B到线TR的距离即为农机行驶过程中的横向偏差e_c。

为最近点车辆前进方向与正北方向顺时针夹角，范围为0～360°，根据如下公式可得农机行驶过程中的航向偏差e_h：

本实施例的模糊规则设计包括：

实现农机前轮转角自动补偿的模糊控制规则的基本原则为：当农机与目标路径的横向偏差为负值较大，航向偏差为负值较大时，前轮转角补偿量应输出为正值较大。当农机与目标路径的横向偏差为正值较大，航向偏差为正值较大时，前轮转角补偿量应输出为负值较大。用模糊语言描述为：横向偏差为正大(PB)或负大(NB)，航向偏差为正大(PB)或者负大(NB)，那么前轮转角为负大(NB)或者正大(PB)。以此类推，根据农机不同的位置状态，可得出49条控制规则。本实施例的模糊控制规则如表1，模糊控制曲面图如图9所示。

表1模糊控制规则

采用本实施例的控制方法，能够有效提高农机车辆在不规则田块或田间运输行驶时的变曲度路径跟踪精度，降低农机作业用人成本，为农业全场景、全方位高度无人化作业提供了技术支撑。

实施例二：

下面对本发明提供的农机车辆自动导航控制装置进行描述，下文描述的农机车辆自动导航控制装置与上文描述的农机车辆自动导航控制方法可相互对应参照。

本实施例提供一种农机车辆自动导航控制装置，如图10，其包括:前馈控制模块201、模糊控制模块202和主控制模块203。

其中，前馈控制模块201用于基于车辆的运动学模型，采用纯追踪控制算法获取车辆前轮转角前馈量δ₁；模糊控制模块202用于采用模糊控制算法根据车辆的横向偏差和航向偏差得到车辆的前轮转角补偿量δ_re；主控制模块203用于根据前轮转角前馈量δ₁和前轮转角补偿量δ_re控制车辆完成自动导航。

其中，本实施例中的各个模块的功能的实现方法和上述实施例一中相同，此处不再赘述。

图11示例了一种电子设备的实体结构示意图，如图11所示，该电子设备可以包括：处理器(processor)310、通信接口(Communications Interface)320、存储器(memory)330和通信总线340，其中，处理器310，通信接口320，存储器330通过通信总线340完成相互间的通信。处理器310可以调用存储器330中的逻辑指令，以执行农机车辆自动导航控制方法，该方法包括：基于车辆的运动学模型，采用纯追踪控制算法获取车辆前轮转角前馈量δ₁；采用模糊控制算法根据车辆的横向偏差和航向偏差得到车辆的前轮转角补偿量δ_re；根据前轮转角前馈量δ₁和前轮转角补偿量δ_re控制车辆完成自动导航。

此外，上述的存储器330中的逻辑指令可以通过软件功能单元的形式实现并作为独立的产品销售或使用时，可以存储在一个计算机可读取存储介质中。基于这样的理解，本发明的技术方案本质上或者说对现有技术做出贡献的部分或者该技术方案的部分可以以软件产品的形式体现出来，该计算机软件产品存储在一个存储介质中，包括若干指令用以使得一台计算机设备(可以是个人计算机，服务器，或者网络设备等)执行本发明各个实施例所述方法的全部或部分步骤。而前述的存储介质包括：U盘、移动硬盘、只读存储器(ROM，Read-Only Memory)、随机存取存储器(RAM，Random Access Memory)、磁碟或者光盘等各种可以存储程序代码的介质。

另一方面，本发明还提供一种农机车辆，该农机车辆包括如上述实施例提供的农机车辆自动导航控制装置或者电子设备。

另一方面，本发明还提供一种计算机程序产品，所述计算机程序产品包括计算机程序，计算机程序可存储在非暂态计算机可读存储介质上，所述计算机程序被处理器执行时，计算机能够执行上述各方法所提供的农机车辆自动导航控制方法。

又一方面，本发明还提供一种非暂态计算机可读存储介质，其上存储有计算机程序，该计算机程序被处理器执行时实现以执行上述各方法提供的农机车辆自动导航控制方法。

以上所描述的装置实施例仅仅是示意性的，其中所述作为分离部件说明的单元可以是或者也可以不是物理上分开的，作为单元显示的部件可以是或者也可以不是物理单元，即可以位于一个地方，或者也可以分布到多个网络单元上。可以根据实际的需要选择其中的部分或者全部模块来实现本实施例方案的目的。本领域普通技术人员在不付出创造性的劳动的情况下，即可以理解并实施。

通过以上的实施方式的描述，本领域的技术人员可以清楚地了解到各实施方式可借助软件加必需的通用硬件平台的方式来实现，当然也可以通过硬件。基于这样的理解，上述技术方案本质上或者说对现有技术做出贡献的部分可以以软件产品的形式体现出来，该计算机软件产品可以存储在计算机可读存储介质中，如ROM/RAM、磁碟、光盘等，包括若干指令用以使得一台计算机设备(可以是个人计算机，服务器，或者网络设备等)执行各个实施例或者实施例的某些部分所述的方法。

最后应说明的是：以上实施例仅用以说明本发明的技术方案，而非对其限制；尽管参照前述实施例对本发明进行了详细的说明，本领域的普通技术人员应当理解：其依然可以对前述各实施例所记载的技术方案进行修改，或者对其中部分技术特征进行等同替换；而这些修改或者替换，并不使相应技术方案的本质脱离本发明各实施例技术方案的精神和范围。

Claims (10)

1.一种农机车辆自动导航控制方法，其特征在于，包括：

基于车辆的运动学模型，采用纯追踪控制算法获取车辆前轮转角前馈量δ₁；

采用模糊控制算法根据车辆的横向偏差和航向偏差得到车辆的前轮转角补偿量δ_re；

根据所述前轮转角前馈量δ₁和前轮转角补偿量δ_re控制车辆完成自动导航。

2.根据权利要求1所述的农机车辆自动导航控制方法，其特征在于，所述采用纯追踪控制算法获取车辆前轮转角前馈量δ₁包括：

获取车辆后轮中心与预瞄点之间的距离L_d；

获取车辆后轮中心经过所述预瞄点所需转过的航向角α；

根据车辆的轴距L以及所述距离L_d和航向角α得到所述车辆前轮转角前馈量δ₁。

3.根据权利要求2所述的农机车辆自动导航控制方法，其特征在于，所述根据车辆的轴距L以及所述距离L_d和航向角α得到所述车辆前轮转角前馈量δ₁包括：

采用以下公式(1)根据车辆的轴距L以及所述距离L_d和航向角α得到所述车辆前轮转角前馈量δ₁：

4.根据权利要求2所述的农机车辆自动导航控制方法，其特征在于，所述获取车辆后轮中心与预瞄点之间的距离L_d以及获取所述航向角α包括：

获取车辆当前的车速；

根据车辆当前的车速采用预设的方法获取车辆前视区域的长度L_sum；

获取车辆前视区域的长度L_sum对应的弯曲度Ω；

根据所述前视区域的长度L_sum和弯曲度Ω计算预瞄点在所述前视区域内距离前视区域起点的曲线长度L_p；

根据所述前视区域内预瞄点和起点的曲线长度L_p确定所述距离L_d和航向角α。

5.根据权利要求4所述的农机车辆自动导航控制方法，其特征在于，所述根据车辆当前的车速采用预设的方法获取车辆前视区域的长度L_sum包括：

通过以下公式(2)根据车辆当前的车速v获取车辆前视区域的长度L_sum：

上述公式(2)中，L_{min_sum}为前视区域的最小长度，L_{max_sum}为前视区域的最大长度，v_min为设定的车辆最小行驶车速；v_max为设定的车辆的最大行驶车速；p和q均为预设的常数；

所述获取车辆前视区域的长度L_sum对应的弯曲度Ω包括：

将所述车辆前视区域离散成多个路径段，获取任意两个相邻路径段之间的夹角β_i，通过以下公式(3)对所有的夹角进行累加得到所述弯曲度Ω；

其中，公式(3)中n为相邻的路径段组成的所有夹角的个数。

6.根据权利要求4所述的农机车辆自动导航控制方法，其特征在于，所述根据所述前视区域的长度L_sum和弯曲度Ω计算预瞄点在所述前视区域内距离前视区域起点的曲线长度L_p包括：

通过以下公式(4)根据长度L_sum和弯曲度Ω计算预瞄点在所述前视区域内距前视区域起点的曲线长度L_p：

上述公式(4)中，Ω_min、Ω_max为车辆的前视区域的弯曲度的两个临界值；

其中，b＝L_sim/|Ω_min-Ω_max|。

7.根据权利要求1所述的农机车辆自动导航控制方法，其特征在于，所述采用模糊控制算法根据车辆的横向偏差和航向偏差得到车辆的前轮转角补偿量δ_re包括：

将所述横向偏差和航向偏差输入到模糊控制器中，所述模糊控制器用于对所述横向偏差和航向偏差进行模糊化处理，输出所述前轮转角补偿量δ_re；

所述横向偏差的基本论域为：[-0.12m，0.12m]，量化等级为：{-0.12，-0.08，-0.04，0，0.04，0.08，0.12}；

所述航向偏差的基本论域为：[-6°，6°]，量化等级为：{-6，-4，-2，0，2，4，6}；

所述前轮转角补偿量δ_re的基本论域为：[-3.6°，4.8°]，量化等级为：{-3.6，-2.4，-1.2，0，1.2，2.4，3.6}。

8.根据权利要求1所述的农机车辆自动导航控制方法，其特征在于，所述根据所述前轮转角前馈量δ₁和前轮转角补偿量δ_re控制车辆完成自动导航包括：

采用以下公式(5)根据前轮转角前馈量δ₁和前轮转角补偿量δ_re得到前轮转角δ_f；

δ_f＝δ₁+δ_re (5)

根据所述前轮转角δ_f控制车辆的前轮转动以完成自动化导航。

9.一种农机车辆自动导航控制装置，其特征在于，包括:

前馈控制模块，用于基于车辆的运动学模型，采用纯追踪控制算法获取车辆前轮转角前馈量δ₁；

模糊控制模块，用于采用模糊控制算法根据车辆的横向偏差和航向偏差得到车辆的前轮转角补偿量δ_re；

主控制模块，用于根据所述前轮转角前馈量δ₁和前轮转角补偿量δ_re控制车辆完成自动导航。

10.一种农机车辆，其特征在于，所述农机车辆包括如权利要求9所述的农机车辆自动导航控制装置。

Images