CN113325849A - 一种针对高地隙植保机的运动控制方法 - Google Patents

Abstract

本发明公开了一种针对高地隙植保机的运动控制方法，判断高地隙植保机的最小转弯半径与目标点的曲率半径的大小，判断当前规划行驶路径是否适合高地隙植保机的通行，当高地隙植保机的最小转弯半径小于目标点的曲率半径，此时根据改进的纯追踪计算方法确定前轮转向角，用误差模型对前轮转向角进行误差补偿，然后高地隙植保机正常通行；当高地隙植保机的最小转弯半径大于等于目标点的曲率半径，高地隙植保机制动；保证了高地隙植保机的导航控制安全性；保证了目标点的曲率半径的精准度，从而更加准确的确定前轮转向角计算方法，保证转向控制的稳定性；通过误差补偿，使得高地隙植保机的前轮实际转向角更加准确，保证路径跟踪的精度。

Description

一种针对高地隙植保机的运动控制方法

技术领域

本发明属于农机自动化技术领域，具体地说，本发明涉及一种针对高地隙植保机的运动控制方法。

背景技术

现有技术的缺陷和不足：

农业机械最优控制器的建立，要依赖精确的农业机械运动学和动力学模型，但是在田间作业条件下，土壤特性的变化、农业机械与地面相互作用的变化，导致农业机械运动学和动力学模型的建立比较困难。

农业机械导航控制的主要任务是根据各个传感器得到农业机械的导航位置，确定该导航位置与规划的路径位置关系，综合农业机械的运动状态以及合适的运动控制方法，决策出相应的前轮转向角，以修正路径跟踪误差。

农业机械导航控制的重难点是提高路径跟踪的精度和转向控制的稳定性。常规的线性模型控制方法包括PID控制方法和模糊控制方法；其中，PID控制方法路径跟踪的精度较高，但是抗干扰能力较弱；模糊控制方法转向控制的稳定性良好，但是路径跟踪的稳态精度难以保证。

高地隙植保机的底盘离地面间隙较高，在循迹规划的路径过程中，易发生跟踪误差较大的情况，高地隙植保机的控制精度和稳定性受到了考验。

发明内容

本发明提供一种针对高地隙植保机的运动控制方法，以解决上述背景技术中存在的问题。

为了实现上述目的，本发明采取的技术方案为：一种针对高地隙植保机的运动控制方法，具体包括以下步骤，

步骤S1：计算高地隙植保机在纯追踪计算方法下输出的前轮理论转向角与前轮实际转向角的误差模型；

步骤S2：通过北斗导航系统规划行驶路径；通过高地隙植保机质心位置上的惯性导航传感器获取高地隙植保机的当前位置，同时获取当前位置与规划行驶路径上的路径点之间的距离；

步骤S3：通过高地隙植保机质心位置上的惯性导航传感器获取当前高地隙植保机的行驶速度，基于行驶速度计算前视距离；将当前位置与所有路径点之间的距离与前视距离做对比，将最接近前视距离的点设置为目标点；并计算目标点的曲率半径；

步骤S4：计算高地隙植保机质心位置与规划行驶路径之间的横向误差；

步骤S5：判断高地隙植保机的最小转弯半径与目标点的曲率半径的大小，当高地隙植保机的最小转弯半径小于目标点的曲率半径，此时根据改进的纯追踪计算方法确定前轮转向角，用步骤S1中的误差模型对前轮转向角进行误差补偿，然后高地隙植保机正常通行；当高地隙植保机的最小转弯半径大于等于目标点的曲率半径，高地隙植保机制动；

步骤S6：循环步骤S2、步骤S3和步骤S4，直到完全行驶完规划行驶路径。

优选的，所述步骤S1中的前轮理论转向角与前轮实际转向角的误差模型为，

Δα＝(α₂-α₁)＝K_αα₁+b………………………(1)；

其中：α₁为前轮理论转向角；α₂为前轮实际转向角；K_α为误差比例系数；b为常数；

采用最小二乘法直线拟合，通过最小化误差的平方和，使得拟合对象无限接近目标对象；然后以前轮理论转向角α₁为横坐标，以前轮实际转向角与前轮理论转向角的差值(α₂-α₁)为纵坐标，建立线性回归方程，根据线性回归方程的斜率确定误差比例系数K_α和常数b。

优选的，所述步骤S2中的规划行驶路径以路径点的形式存储在路径文件中，具体为路径点P在大地坐标系下以经度和纬度坐标格式存储在路径文件中，标记为(x₁，y₁)、(x₂，y₂)、(x₃，y₃)、…(x_i，y_i)、…(x_n，y_n)；通过高地隙植保机质心位置上的惯性导航传感器获取高地隙植保机在大地坐标系下的当前位置坐标N(x，y)，并计算当前位置坐标N(x，y)与所有路径点P坐标(x₁，y₁)、(x₂，y₂)、(x₃，y₃)、…(x_i，y_i)、…(x_n，y_n)之间的距离d₁、d₂、d₃、…d_i、…d_n。

优选的，所述步骤S3中具体过程如下，

前视距离的计算：前视距离与行驶速度的关系可以表示为，

L_d＝Kv+L_min…………………………(2)；

其中：K为当前行驶速度的比例系数；L_d为前视距离；L_min为前视距离最小值,取该值为高地隙植保机的最小转弯半径；v为当前高地隙植保机的行驶速度；

目标点的确定：将d₁、d₂、d₃……d_n与前视距离L_d相比，将最接近前视距离L_d的点设置为目标点P(x_i，y_i)；

目标点曲率半径的计算：目标点P(x_i，y_i)的曲率K_i为，

则目标点的曲率半径R_i为，

优选的，所述步骤S3中K的确定方法，令高地隙植保机在存在初始误差的条件下，在预设直线上行驶，获取在不同速度比例系数下的跟踪误差，对比分析该误差，K取平均跟踪误差最小的值，其中，初始误差指的是高地隙植保机开始循迹时离规划的直线路径起点距离。

优选的，所述步骤S4中的横向误差e_i的计算方法：即高地隙植保机质心位置N(x，y)到规划行驶路径最近的垂直距离，

其中，A、B、C为规划行驶路径所在直线中的系数。

优选的，所述步骤S1中纯追踪计算方法利用航向偏差建立几何模型，求解高地隙植保机到达目标点P(x_i，y_i)所需要的转弯半径R，然后根据转弯半径R求解出前轮转向角δ₁，d_i为高地隙植保机当前位置与目标点之间的距离，具体如下，由正玹定理可得：

整理可得：

设前后轮之间的轮距为L，则：

将公式(7)和公式(8)联合，得高地隙植保机在纯追踪计算方法下前轮转向角δ₁的表达式：

由于原始的纯追踪计算方法在曲率半径较小的弯道上的跟踪精度较差，且该跟踪精度受到车辆当前速度和路径的曲率半径影响较大，因此使用改进的纯追踪计算方法，即在原始的纯追踪计算方法的基础上增加一个控制系数K_δ，从而增加跟踪精度。

优选的，所述K_δ由训练过的BP神经网络模型得到，

确定BP神经网络模型：输入层有三个节点，输入节点分别为当前高地隙植保机的行驶速度v，目标点曲率半径R_i，横向偏差e_i；一个隐含层，输出层有一个节点，输出节点为控制系数K_δ，隐含层的节点数按照公式选取，

其中：L为隐含层节点数；m为输出层节点数；n为输入层节点数；a为0～10之间的常数；

函数的选取：

隐含层神经元的函数选取S型正切函数：

输出层神经元的函数选取线性传递函数：

f₂(x)＝x………………………………(12)；

计算隐含层和输出层的输出，根据网络的期望输出和实际输出计算误差，用误差更新网络的权值和阈值，最后利用训练出的神经网络，得到控制系数K_δ；

即最终高地隙植保机前轮转向角δ的表达式：

误差补偿方法：通过转向角控制器得到前轮理论转向角δ，在前轮理论转向角δ的基础上加上误差模型中的转角误差，即最终的高地隙植保机前轮转向角为：δ+Δα。

采用以上技术方案的有益效果是：

1、本发明的针对高地隙植保机的运动控制方法，首先，判断高地隙植保机的最小转弯半径与目标点的曲率半径的大小，判断当前规划行驶路径是否适合高地隙植保机的通行，当高地隙植保机的最小转弯半径小于目标点的曲率半径，此时根据改进的纯追踪计算方法确定前轮转向角，用步骤S1中的误差模型对前轮转向角进行误差补偿，然后高地隙植保机正常通行；当高地隙植保机的最小转弯半径大于等于目标点的曲率半径，高地隙植保机制动。本发明增加了判断当前规划行驶路径是否适合高地隙植保机通行的功能，保证了高地隙植保机的导航控制安全性。

2、改进了前视距离的计算方法，前视距离由当前高地隙植保机的航向偏差和行驶速度作为控制量，在循迹规划的路径过程中，根据行驶速度动态调整前视距离，使得目标点的确定更加合理，保证了目标点的曲率半径的精准度，从而更加准确的确定前轮转向角计算方法，保证转向控制的稳定性。

3、增加了用误差模型对高地隙植保机前轮转向角进行误差补偿，计算出高地隙植保机的前轮理论转向角与前轮实际转向角的差值，建立线性回归方程，对转角控制器输出的前轮转向角进行误差补偿，使得高地隙植保机的前轮实际转向角更加准确，保证路径跟踪的精度。

4、在原始的纯追踪计算方法的基础上增加一个控制系数K_δ，该控制系数K_δ根据当前高地隙植保机的行驶速度v，目标点曲率半径R_i，横向偏差e_i的值进行实时调整，保证了高地隙植保机的路径跟踪精度。

附图说明

图1是本发明的纯追踪计算方法的几何模型图；

图2是全轮偏转转向原理图；

图3是误差比例系数K_α的计算图；

图4是速度比例系数K的整定图；

图5是神经网络结构图；

具体实施方式

下面对照附图，通过对实施例的描述，对本发明的具体实施方式作进一步详细的说明，目的是帮助本领域的技术人员对本发明的构思、技术方案有更完整、准确和深入的理解，并有助于其实施。

如图1至图5所示，本发明是一种针对高地隙植保机的运动控制方法，保证了目标点的曲率半径的精准度，从而更加准确的确定前轮转向角计算方法，保证转向控制的稳定性；通过误差补偿，使得高地隙植保机的前轮实际转向角更加准确，保证路径跟踪的精度。

具体的说，如图1至图5所示，具体包括以下步骤，步骤S1：计算高地隙植保机在纯追踪计算方法下输出的前轮理论转向角与前轮实际转向角的误差模型；

步骤S2：通过北斗导航系统规划行驶路径；通过高地隙植保机质心位置上的惯性导航传感器获取高地隙植保机的当前位置，同时获取当前位置与规划行驶路径上的路径点之间的距离；

步骤S3：通过高地隙植保机质心位置上的惯性导航传感器获取当前高地隙植保机的行驶速度，基于行驶速度计算前视距离；将当前位置与所有路径点之间的距离与前视距离做对比，将最接近前视距离的点设置为目标点；并计算目标点的曲率半径；

步骤S4：计算高地隙植保机质心位置与规划行驶路径之间的横向误差；

步骤S5：判断高地隙植保机的最小转弯半径与目标点的曲率半径的大小，当高地隙植保机的最小转弯半径小于目标点的曲率半径，此时根据改进的纯追踪计算方法确定前轮转向角，用步骤S1中的误差模型对前轮转向角进行误差补偿，然后高地隙植保机正常通行；当高地隙植保机的最小转弯半径大于等于目标点的曲率半径，高地隙植保机制动；

步骤S6：循环步骤S2、步骤S3和步骤S4，直到完全行驶完规划行驶路径。

优选的，所述步骤S1中的前轮理论转向角与前轮实际转向角的误差模型为，

Δα＝(α₂-α₁)＝K_αα₁+b………………………(1)；

其中：α₁为前轮理论转向角；α₂为前轮实际转向角；K_α为误差比例系数；b为常数；

采用最小二乘法直线拟合，通过最小化误差的平方和，使得拟合对象无限接近目标对象；然后以前轮理论转向角α₁为横坐标，以前轮实际转向角与前轮理论转向角的差值(α₂-α₁)为纵坐标，建立线性回归方程，根据线性回归方程的斜率确定误差比例系数K_α和常数b。

优选的，所述步骤S2中的规划行驶路径以路径点的形式存储在路径文件中，具体为路径点P在大地坐标系下以经度和纬度坐标格式存储在路径文件中，标记为(x₁，y₁)、(x₂，y₂)、(x₃，y₃)、…(x_i，y_i)、…(x_n，y_n)；通过高地隙植保机质心位置上的惯性导航传感器获取高地隙植保机在大地坐标系下的当前位置坐标N(x，y)，并计算当前位置坐标N(x，y)与所有路径点P坐标(x₁，y₁)、(x₂，y₂)、(x₃，y₃)、…(x_i，y_i)、…(x_n，y_n)之间的距离d₁、d₂、d₃、…d_i、…d_n。

优选的，所述步骤S3中具体过程如下，

前视距离的计算：前视距离与行驶速度的关系可以表示为，

L_d＝Kv+L_min…………………………(2)；

其中：K为当前行驶速度的比例系数；L_d为前视距离；L_min为前视距离最小值,取该值为高地隙植保机的最小转弯半径；v为当前高地隙植保机的行驶速度；

目标点的确定：将d₁、d₂、d₃……d_n与前视距离L_d相比，将最接近前视距离L_d的点设置为目标点P(x_i，y_i)；

目标点曲率半径的计算：目标点P(x_i，y_i)的曲率K_i为，

则目标点的曲率半径R_i为，

优选的，所述步骤S3中K的确定方法，令高地隙植保机在存在初始误差的条件下，在预设直线上行驶，获取在不同速度比例系数下的跟踪误差，对比分析该误差，K取平均跟踪误差最小的值，其中，初始误差指的是高地隙植保机开始循迹时离规划的直线路径起点距离。

优选的，所述步骤S4中的横向误差e_i的计算方法：即高地隙植保机质心位置N(x，y)到规划行驶路径最近的垂直距离，

其中，A、B、C为规划行驶路径所在直线中的系数。

优选的，所述步骤S1中纯追踪计算方法利用航向偏差建立几何模型，求解高地隙植保机到达目标点P(x_i，y_i)所需要的转弯半径R，然后根据转弯半径R求解出前轮转向角δ₁，d_i为高地隙植保机当前位置与目标点之间的距离，具体如下，由正玹定理可得：

整理可得：

设前后轮之间的轮距为L，则：

将公式(7)和公式(8)联合，得高地隙植保机在纯追踪计算方法下前轮转向角δ₁的表达式：

由于原始的纯追踪计算方法在曲率半径较小的弯道上的跟踪精度较差，且该跟踪精度受到车辆当前速度和路径的曲率半径影响较大，因此使用改进的纯追踪计算方法，即在原始的纯追踪计算方法的基础上增加一个控制系数K_δ，从而增加跟踪精度。

优选的，所述K_δ由训练过的BP神经网络模型得到，

确定BP神经网络模型：输入层有三个节点，输入节点分别为当前高地隙植保机的行驶速度v，目标点曲率半径R_i，横向偏差e_i；一个隐含层，输出层有一个节点，输出节点为控制系数K_δ，隐含层的节点数按照公式选取，

其中：L为隐含层节点数；m为输出层节点数；n为输入层节点数；a为0～10之间的常数；

函数的选取：

隐含层神经元的函数选取S型正切函数：

输出层神经元的函数选取线性传递函数：

f₂(x)＝x………………………………(12)；

计算隐含层和输出层的输出，根据网络的期望输出和实际输出计算误差，用误差更新网络的权值和阈值，最后利用训练出的神经网络，得到控制系数K_δ；

即最终高地隙植保机前轮转向角δ的表达式：

误差补偿方法：通过转向角控制器得到前轮理论转向角δ，在前轮理论转向角δ的基础上加上误差模型中的转角误差，即最终的高地隙植保机前轮转向角为：δ+Δα。

以下用具体实施例对具体工作方式进行阐述：

实施例1：

本发明的针对高地隙植保机的运动控制方法，首先，判断高地隙植保机的最小转弯半径与目标点的曲率半径的大小，判断当前规划行驶路径是否适合高地隙植保机的通行，当高地隙植保机的最小转弯半径小于目标点的曲率半径，此时根据改进的纯追踪计算方法确定前轮转向角，用步骤S1中的误差模型对前轮转向角进行误差补偿，然后高地隙植保机正常通行；当高地隙植保机的最小转弯半径大于等于目标点的曲率半径，高地隙植保机制动。本发明增加了判断当前规划行驶路径是否适合高地隙植保机通行的功能，保证了高地隙植保机的导航控制安全性。

实施例2：

在实施例1的基础上，改进了前视距离的计算方法，前视距离由当前高地隙植保机的航向偏差和行驶速度作为控制量，在循迹规划的路径过程中，根据行驶速度动态调整前视距离，使得目标点的确定更加合理，保证了目标点的曲率半径的精准度，从而更加准确的确定前轮转向角计算方法，保证转向控制的稳定性。

实施例3：

在实施例1的基础上，增加了用误差模型对高地隙植保机前轮转向角进行误差补偿，计算出高地隙植保机的前轮理论转向角与前轮实际转向角的差值，建立线性回归方程，对转角控制器输出的前轮转向角进行误差补偿，使得高地隙植保机的前轮实际转向角更加准确，保证路径跟踪的精度。

在原始的纯追踪计算方法的基础上增加一个控制系数K_δ，该控制系数K_δ根据当前高地隙植保机的行驶速度v，目标点曲率半径R_i，横向偏差e_i的值进行实时调整，保证了高地隙植保机的路径跟踪精度。

以上结合附图对本发明进行了示例性描述，显然，本发明具体实现并不受上述方式的限制，只要是采用了本发明的方法构思和技术方案进行的各种非实质性的改进；或未经改进，将本发明的上述构思和技术方案直接应用于其它场合的，均在本发明的保护范围之内。

Claims (8)

1.一种针对高地隙植保机的运动控制方法，其特征在于：具体包括以下步骤，

步骤S1：计算高地隙植保机在纯追踪计算方法下输出的前轮理论转向角与前轮实际转向角的误差模型；

步骤S2：通过北斗导航系统规划行驶路径；通过高地隙植保机质心位置上的惯性导航传感器获取高地隙植保机的当前位置，同时获取当前位置与规划行驶路径上的路径点之间的距离；

步骤S3：通过高地隙植保机质心位置上的惯性导航传感器获取当前高地隙植保机的行驶速度，基于行驶速度计算前视距离；将当前位置与所有路径点之间的距离与前视距离做对比，将最接近前视距离的点设置为目标点；并计算目标点的曲率半径；

步骤S4：计算高地隙植保机质心位置与规划行驶路径之间的横向误差；

步骤S5：判断高地隙植保机的最小转弯半径与目标点的曲率半径的大小，当高地隙植保机的最小转弯半径小于目标点的曲率半径，此时根据改进的纯追踪计算方法确定前轮转向角，用步骤S1中的误差模型对前轮转向角进行误差补偿，然后高地隙植保机正常通行；当高地隙植保机的最小转弯半径大于等于目标点的曲率半径，高地隙植保机制动；

步骤S6：循环步骤S2、步骤S3和步骤S4，直到完全行驶完规划行驶路径。

2.根据权利要求1所述的一种针对高地隙植保机的运动控制方法，其特征在于：所述步骤S1中的前轮理论转向角与前轮实际转向角的误差模型为，

Δα＝(α₂-α₁)＝K_αα₁+b………………………(1)；

其中：α₁为前轮理论转向角；α₂为前轮实际转向角；K_α为误差比例系数；b为常数；

采用最小二乘法直线拟合，通过最小化误差的平方和，使得拟合对象无限接近目标对象；然后以前轮理论转向角α₁为横坐标，以前轮实际转向角与前轮理论转向角的差值(α₂-α₁)为纵坐标，建立线性回归方程，根据线性回归方程的斜率确定误差比例系数K_α和常数b。

3.根据权利要求1所述的一种针对高地隙植保机的运动控制方法，其特征在于：所述步骤S2中的规划行驶路径以路径点的形式存储在路径文件中，具体为路径点P在大地坐标系下以经度和纬度坐标格式存储在路径文件中，标记为(x₁，y₁)、(x₂，y₂)、(x₃，y₃)、…(x_i，y_i)、…(x_n，y_n)；通过高地隙植保机质心位置上的惯性导航传感器获取高地隙植保机在大地坐标系下的当前位置坐标N(x，y)，并计算当前位置坐标N(x，y)与所有路径点P坐标(x₁，y₁)、(x₂，y₂)、(x₃，y₃)、…(x_i，y_i)、…(x_n，y_n)之间的距离d₁、d₂、d₃、…d_i、…d_n。

4.根据权利要求1所述的一种针对高地隙植保机的运动控制方法，其特征在于：所述步骤S3中具体过程如下，

前视距离的计算：前视距离与行驶速度的关系可以表示为，

L_d＝Kv+L_min…………………………(2)；

其中：K为当前行驶速度的比例系数；L_d为前视距离；L_min为前视距离最小值,取该值为高地隙植保机的最小转弯半径；v为当前高地隙植保机的行驶速度；

目标点的确定：将d₁、d₂、d₃……d_n与前视距离L_d相比，将最接近前视距离L_d的点设置为目标点P(x_i，y_i)；

目标点曲率半径的计算：目标点P(x_i，y_i)的曲率K_i为，

则目标点的曲率半径R_i为，

5.根据权利要求4所述的一种针对高地隙植保机的运动控制方法，其特征在于：所述步骤S3中K的确定方法，令高地隙植保机在存在初始误差的条件下，在预设直线上行驶，获取在不同速度比例系数下的跟踪误差，对比分析该误差，K取平均跟踪误差最小的值，其中，初始误差指的是高地隙植保机开始循迹时离规划的直线路径起点距离。

6.根据权利要求1所述的一种针对高地隙植保机的运动控制方法，其特征在于：所述步骤S4中的横向误差e_i的计算方法：即高地隙植保机质心位置N(x，y)到规划行驶路径最近的垂直距离，

其中，A、B、C为规划行驶路径所在直线中的系数。

7.根据权利要求1所述的一种针对高地隙植保机的运动控制方法，其特征在于：所述步骤S1中纯追踪计算方法利用航向偏差建立几何模型，求解高地隙植保机到达目标点P(x_i，y_i)所需要的转弯半径R，然后根据转弯半径R求解出前轮转向角δ₁，d_i为高地隙植保机当前位置与目标点之间的距离，具体如下，由正玹定理可得：

整理可得：

设前后轮之间的轮距为L，则：

将公式(7)和公式(8)联合，得高地隙植保机在纯追踪计算方法下前轮转向角δ₁的表达式：

由于原始的纯追踪计算方法在曲率半径较小的弯道上的跟踪精度较差，且该跟踪精度受到车辆当前速度和路径的曲率半径影响较大，因此使用改进的纯追踪计算方法，即在原始的纯追踪计算方法的基础上增加一个控制系数K_δ，从而增加跟踪精度。

8.根据权利要求7所述的一种针对高地隙植保机的运动控制方法，其特征在于：所述K_δ由训练过的BP神经网络模型得到，

确定BP神经网络模型：输入层有三个节点，输入节点分别为当前高地隙植保机的行驶速度v，目标点曲率半径R_i，横向偏差e_i；一个隐含层，输出层有一个节点，输出节点为控制系数K_δ，隐含层的节点数按照公式选取，

其中：L为隐含层节点数；m为输出层节点数；n为输入层节点数；a为0～10之间的常数；

函数的选取：

隐含层神经元的函数选取S型正切函数：

输出层神经元的函数选取线性传递函数：

f₂(x)＝x………………………………(12)；

计算隐含层和输出层的输出，根据网络的期望输出和实际输出计算误差，用误差更新网络的权值和阈值，最后利用训练出的神经网络，得到控制系数K_δ；

即最终高地隙植保机前轮转向角δ的表达式：

误差补偿方法：通过转向角控制器得到前轮理论转向角δ，在前轮理论转向角δ的基础上加上误差模型中的转角误差，即最终的高地隙植保机前轮转向角为：δ+Δα。

Images