CN113359710B - 一种基于los理论的农机路径跟踪方法 - Google Patents

Abstract

本发明涉及一种基于LOS理论的农机路径跟踪方法，属于农机智能控制领域，农机在已设定的直线路径上行驶时，由导航定位模块获取农机实时位置信息；将位置信息通过上位机***解析的坐标、横向跟踪误差等参数传送到LOS算法器中得出当前行驶的航向角；最后前轮反馈控制器根据航向角和横向跟踪误差，输出***期望的车轮转向角度，由此可以得出车轮的转向偏差角，重复以上步骤，实现路径跟踪控制。本发明的基于LOS导航法的农机***，可以适用于不同农机作业场景，有效减少了路径跟踪误差过大问题，具有鲁棒性，精确性等特点。该方法思路清晰简单，在有外界干扰或者***模型参数不确定的情况下能够使农机在跟踪期望路径过程中车轮转角变化更加平缓，稳定。

Description

一种基于LOS理论的农机路径跟踪方法

技术领域

本发明涉及一种基于LOS理论的农机路径跟踪方法，属于农机智能控制技术领域。

背景技术

我国是一个农业大国，农业是社会经济发展的基础和人民物质生活的保障，为了促进农业生产率的提高，实现农业现代化的发展，人们提出了精准农业这一概念，它主要是利用导航卫星定位技术、传感器技术和遥感控制等技术完成农用机械自主作业，自动化导航目的，有效地减少了农田耕作时间，提高了耕作效率，并且可以代替人力劳动实现自动驾驶，缓解了驾驶员的工作疲劳，降低生产事故发生率，提高了生产安全性。

其中农机路径跟踪方法作为农机自动化导航的关键技术，一直是农机智能控制技术领域的研究热点。现有的农机路径跟踪技术主要依赖农机上位机***导航定位技术和下位机***传感器精度、液压转向来控制，这样大大增加了***设计的复杂性和控制模型的依赖性，提高了生产成本，降低了生产效益，并且农机作业环境适应性不强。所以如何在简化农机自动导航***结构的前提下，优化出一套鲁棒性强，精确性高的路径跟踪控制器，是实现农业精准化、智能化的关键所在。

发明内容

本发明提供一种基于LOS理论的农机路径跟踪控制方法，可以在***模型参数不确定或作业环境扰动性大的情况下稳定有效地对无人农机进行路径跟踪控制，使得农机在原有硬件***结构简化的基础上保证了非线性反馈控制器的稳定性和精确性。

为了解决以上技术问题，本发明采取以下技术方案：

一种基于LOS理论的农机路径跟踪方法，包括以下步骤：

S1，农机在已设定的直线路径上行驶时，由导航定位模块获取农机实时位置信息；

S2，将位置信息通过上位机***解析的坐标、横向跟踪误差等参数传送到LOS算法器中得出当前行驶的航向角；

S3，前轮反馈控制器根据航向角和横向跟踪误差，输出***期望的车轮转向角度，由此可以得出车轮的转向偏差角，重复以上步骤，实现路径跟踪控制。

进一步，所述步骤S1具体包括：

S11，通过北斗RTK定位模块和视觉识别CDD模块设定农机工作田块边界，进而打点出若干条平行的直线轨迹。

S12，北斗RTK导航模块实时输出农机的位置，轨迹点信息，同时与矩形田块的三个顶点坐标转化到高斯平面坐标系中，并将规划的路径以二维数组形式储存到导航***中，实现导航路径规划。

进一步，所述步骤S11具体为：

利用RTK-CCD定位视觉传感器融合***，测定试验田三个坐标顶点确定一个矩形，假设为地头节点A、B、C、D定位坐标；以每条作业行距离最长为原则，结合地块实际几何形状，选取边界AD作为作业行规划的基准线，以其为基准划定若干条平行作业线，平行线距离依作业行间距而定，最后一条作业线与边界BC的最短距离不得小于1/2个作业间距，将所有Q条平行作业线与边界AB和CD的焦点，作为规划作业线节点，并将相应作业线节点存进二维数组中，供基于LOS算法器的导航控制软件调用。

进一步，所述步骤S2具体包括：

S21：确定一条由两个路径点

和

定义的直线跟踪路径，并且路径坐标系的原点在

农机在路径固定的坐标系内坐标为pⁿ(t)＝[x(t),y(t)]，且满足公式：

式中：

α_k＝atan2(y_k+1-y_k,x_k+1-x_k)∈S；

α_k为大地坐标系正北方与期望路径P_k和P_k+1的夹角，a为系数；

S22：令ε(t)＝[s(t),e(t)]^T∈R²，其中：

s(t)＝[x(t)-x_k]cos(α_k)+[y(t)-y_k]sin(α_k)

e(t)＝-[x(t)-x_k]sin(α_k)+[y(t)-y_k]cos(α_k)

s(t)为农机路径跟踪距离，e(t)为横向跟踪误差，在实际路径跟踪过程中，只需要关注农机本身行驶的横向偏差，因为当横向偏差e(t)＝0时，意味着农机已经收敛于期望跟踪路径上，路径跟踪控制目标即为：

S23：设定LOS导航算法为：

式中：

为速度—路径相关角，其中Δ为农机的前视距离，e为横向跟踪误差；

若农机当行驶方向角为

则航向角为：

进一步，所述步骤S3具体包括：

S31：分别通过导航定位***和位姿采集***获取农机实时位置、车轮速度和转角等信息，结合农机结构参数和二自由度模型理论，进行运动学建模，具体为：

将农机路径跟踪过程看作是在X-Y平面坐标系内的低速运动，则根据上下三角弦定理得：

其中δ_f、δ_r分别为前轮转角和后轮转角；l_f、l_r分别为前轴距和后轴距；β为农机行驶速度方向角；R为转弯半径；

两式化简并在两侧分别乘以

得：

农机在低速行驶工况条件下，车辆方向变化率为：

即

由于农机不考虑后轮转向情况，即tan(δ_r)＝0时整理得：

其中

为航向角，v为行驶速度，δ为前轮转角；

在低速作业环境下，假设车轮转角方向与速度方向一致，即δ＝β，则运动学模型为：

S32：由农机转向特点可得前轮转角：

其中

为航向角，可由LOS算法器求出；δ_e(t)为转向偏差角；

在不考虑行驶跟踪偏差情况下，横向跟踪误差e(t)越大，前轮转向角越大，假设车辆预期轨迹在距离前轮延长线d(t)与给定路径上的最近切线相交，根据几何关系得出如下非线性比例函数：

其中d(t)与车速相关，用车速v(t)与增益参数k表示。

在不考虑横向跟踪误差情况下，前轮转向偏差角与期望路径切线方向一致，即在没有横向误差时，前轮方向与期望路径方向相同：

随着横向跟踪误差的增大，非线性比例函数产生一个直接指向期望路径的前轮偏角，且：

为使两个微分方程在零点误差交界处具有全局渐进稳定平衡性，综合两方面控制因素，非线性前轮反馈控制器设计如下：

并且根据几何关系可得：

所以

因此，e(t)收敛速度介于v(t)的线性收敛速度和增益参数k的指数收敛速度之间，当横向跟踪误差e(t)很小时，(ke(t)/v(t))²趋近于0，则

积分可得：

e(t)＝e(0)*exp^-kt

对于任意横向误差，微分方程都单调收敛到0，实现了对路径跟踪控制目标。

进一步，将导航控制器得出的车轮转向偏差信息传送到农机下位机***中，由转向传感器和光电编码器的值来修正真正的偏差值，将其传送到控制机构液压***，来控制液压***的流量和流向，最终控制车轮的转向，达到车辆按照设定的路线行驶。

通过以上技术方案，与现有技术相比，本发明具有以下有益效果：

为了解决农机路径跟踪技术适应性差，***设计复杂，控制不稳定问题，本发明公开了一种基于LOS理论的农机路径跟踪方法；利用LOS算法器获取农机路径跟踪的航向角，并且能够自动切换跟踪路径目标点，确保了当前控制器输入的横向跟踪误差等状态参数的更新，实现对转向的实时控制，能够更快稳定地收敛到期望跟踪路径；并且LOS算法不依赖***参数模型，在外界扰动大的时候也能够输出精确的农机航向角，结合所设计的前轮反馈非线性控制器使得农机作业环境适应性增强，同时简化整体控制***框架，鲁棒性高，适合农业机械使用。

附图说明

图1为基于LOS理论的农机路径跟踪***结构图；

图2为LOS控制算法原理图；

图3为农机运动学模型图；

图4为两种***控制方法的仿真对比图；(a)为期望跟踪和实际跟踪路径对比图；(b)为期望跟踪和基于LOS法跟踪路径对比图。

具体实施方式

为了能进一步了解本发明的特征、技术手段以及所达到了具体目的、功能，下面结合附图和具体实施方式对本发明作进一步详细描述。

如图1所示的***结构图，一种基于LOS理论的农机路径跟踪方法，包括以下步骤：

S1，农机在已设定的直线路径上行驶时，由导航定位模块获取农机实时位置信息；

S2，将位置信息通过上位机***解析的坐标、横向跟踪误差等参数传送到LOS算法器中得出当前行驶的航向角；

S3，前轮反馈控制器根据航向角和横向跟踪误差，输出***期望的车轮转向角度，由此可以得出车轮的转向偏差角，重复以上步骤，实现路径跟踪控制。

所述步骤S1具体包括：

S11，通过北斗RTK定位模块和视觉识别CDD模块设定农机工作田块边界，进而打点出若干条平行的直线轨迹。

S12，北斗RTK导航模块实时输出农机的位置，轨迹点信息，同时与矩形田块的三个顶点坐标转化到高斯平面坐标系中，并将规划的路径以二维数组形式储存到导航***中，实现导航路径规划。

所述步骤S11具体为：

利用RTK-CCD定位视觉传感器融合***，测定试验田三个坐标顶点确定一个矩形，假设为地头节点A、B、C、D定位坐标；以每条作业行距离最长为原则，结合地块实际几何形状，选取边界AD作为作业行规划的基准线，以其为基准划定若干条平行作业线，平行线距离依作业行间距而定，最后一条作业线与边界BC的最短距离不得小于1/2个作业间距，将所有Q条平行作业线与边界AB和CD的焦点，作为规划作业线节点，并将相应作业线节点存进二维数组中，供基于LOS算法器的导航控制软件调用。

如图2所示，基于LOS导航算法处理过程，步骤S2具体包括：

S21：确定一条由两个路径点

和

定义的直线跟踪路径，并且路径坐标系的原点在

农机在路径固定的坐标系内坐标为pⁿ(t)＝[x(t),y(t)]，且满足公式：

式中：

α_k＝atan2(y_k+1-y_k,x_k+1-x_k)∈S；

α_k为大地坐标系正北方与期望路径P_k和P_k+1的夹角。

S22：令ε(t)＝[s(t),e(t)]^T∈R²，其中：

s(t)＝[x(t)-x_k]cos(α_k)+[y(t)-y_k]sin(α_k)

e(t)＝-[x(t)-x_k]sin(α_k)+[y(t)-y_k]cos(α_k)

s(t)为农机路径跟踪距离，e(t)为横向跟踪误差，在实际路径跟踪过程中，我们只需要关注农机本身行驶的横向偏差，因为当横向偏差e(t)＝0时，意味着农机已经收敛于期望跟踪路径上，路径跟踪控制目标即为：

S23：设定LOS导航算法为：

式中：

为速度—路径相关角。其中Δ为农机的前视距离，e为横向跟踪误差；

若农机当行驶方向角为

则航向角为：

所述步骤S3具体包括：

S31：如图3所示，分别通过导航定位***和位姿采集***获取农机实时位置、车轮速度和转角等信息，结合农机结构参数和二自由度模型理论，进行运动学建模，具体为：

将农机路径跟踪过程看作是在X-Y平面坐标系内的低速运动，则根据上下三角弦定理得：

其中δ_f、δ_r分别为前轮转角和后轮转角；l_f、l_r分别为前轴距和后轴距；

β为农机行驶速度方向角；R为转弯半径；

两式化简并在两侧分别乘以

得：

农机在低速行驶工况条件下，车辆方向变化率为：

即

由于农机不考虑后轮转向情况，即tan(δ_r)＝0时整理得：

其中

为航向角，v为行驶速度，δ为前轮转角。

在低速作业环境下，假设车轮转角方向与速度方向一致，即δ＝β，则运动学模型为：

S32：由农机转向特点可得前轮转角：

其中

为航向角，可由LOS算法器求出；δ_e(t)为转向偏差角。

在不考虑行驶跟踪偏差情况下，横向跟踪误差e(t)越大，前轮转向角越大，假设车辆预期轨迹在距离前轮延长线d(t)与给定路径上的最近切线相交，根据几何关系得出如下非线性比例函数：

其中d(t)与车速相关，用车速v(t)与增益参数k表示。

在不考虑横向跟踪误差情况下，前轮转向偏差角与期望路径切线方向一致，即在没有横向误差时，前轮方向与期望路径方向相同：

随着横向跟踪误差的增大，非线性比例函数产生一个直接指向期望路径的前轮偏角，且：

为使两个微分方程在零点误差交界处具有全局渐进稳定平衡性，综合两方面控制因素，非线性前轮反馈控制器设计如下：

并且根据几何关系可得：

所以

因此，e(t)收敛速度介于v(t)的线性收敛速度和增益参数k的指数收敛速度之间，当横向跟踪误差e(t)很小时，(ke(t)/v(t))²趋近于0，则

积分可得：

e(t)＝e(0)*exp^-kt

对于任意横向误差，微分方程都单调收敛到0，实现了对路径跟踪控制目标。

通过农机上位机和下位机等附属设备如GPS、位姿传感器和角度传感器等，将采集的位置、角度等信息，并将信息在控制器与液压转向传动***之间反馈过程部分不再赘述。

如图4所示，通过路径跟踪控制的仿真对比，可以得出结论：本发明的基于LOS理论的农机路径跟踪控制方法，能够使控制目标更加快速、稳定地收敛到期望路径，并且在路径跟踪过程中转角变化次数较少，过程更加平缓，有着良好的控制效果。

以上所述仅为本发明的优选实施例而已，并不用以限制本发明，任何内容的修改替换和改进，只要不脱离本发明的原理规则，均在本发明的保护范围之内。

Claims (4)

1.一种基于LOS理论的农机路径跟踪方法，其特征在于，包括以下步骤：

S1，农机在已设定的直线路径上行驶时，由导航定位模块获取农机实时位置信息；

S2，将位置信息通过上位机***解析的坐标、横向跟踪误差参数传送到LOS算法器中得出当前行驶的航向角；

所述步骤S2具体包括：

S21：确定一条由两个路径点

和

定义的直线跟踪路径，并且路径坐标系的原点在

农机在路径固定的坐标系内坐标为pⁿ(t)＝[x(t),y(t)]，且满足公式：

式中：

α_k＝a tan2(y_k+1-y_k,x_k+1-x_k)∈S；

α_k为大地坐标系正北方与期望路径P_k和P_k+1的夹角，a为系数；

S22：令ε(t)＝[s(t),e(t)]^T∈R²，其中：

s(t)＝[x(t)-x_k]cos(α_k)+[y(t)-y_k]sin(α_k)

e(t)＝-[x(t)-x_k]sin(α_k)+[y(t)-y_k]cos(α_k)

s(t)为农机路径跟踪距离，e(t)为横向跟踪误差，在实际路径跟踪过程中，只需要关注农机本身行驶的横向偏差，因为当横向偏差e(t)＝0时，意味着农机已经收敛于期望跟踪路径上，路径跟踪控制目标即为：

S23：设定LOS导航算法为：

式中：

为速度—路径相关角，其中Δ为农机的前视距离，e为横向跟踪误差；

若农机当行驶方向角为

则航向角为：

S3，前轮反馈控制器根据航向角和横向跟踪误差，输出***期望的车轮转向角度，由此可以得出车轮的转向偏差角，重复以上步骤，实现路径跟踪控制；

所述步骤S3具体包括：

S31：分别通过导航定位***和位姿采集***获取农机实时位置、车轮速度和转角信息，结合农机结构参数和二自由度模型理论，进行运动学建模，具体为：

将农机路径跟踪过程看作是在X-Y平面坐标系内的低速运动，则根据上下三角弦定理得：

其中δ_f、δ_r分别为前轮转角和后轮转角；l_f、l_r分别为前轴距和后轴距；β为农机行驶速度方向角；R为转弯半径；

两式化简并在两侧分别乘以

得：

农机在低速行驶工况条件下，车辆方向变化率为：

即

由于农机不考虑后轮转向情况，即tan(δ_r)＝0时整理得：

其中

为航向角，v为行驶速度，δ为前轮转角；

在低速作业环境下，假设车轮转角方向与速度方向一致，即δ＝β，则运动学模型为：

S32：由农机转向特点可得前轮转角：

其中

为航向角，可由LOS算法器求出；δ_e(t)为转向偏差角；

在不考虑行驶跟踪偏差情况下，横向跟踪误差e(t)越大，前轮转向角越大，假设车辆预期轨迹在距离前轮延长线d(t)与给定路径上的最近切线相交，根据几何关系得出如下非线性比例函数：

其中d(t)与车速相关，用车速v(t)与增益参数k表示；

在不考虑横向跟踪误差情况下，前轮转向偏差角与期望路径切线方向一致，即在没有横向误差时，前轮方向与期望路径方向相同：

随着横向跟踪误差的增大，非线性比例函数产生一个直接指向期望路径的前轮偏角，且：

为使两个微分方程在零点误差交界处具有全局渐进稳定平衡性，综合两方面控制因素，非线性前轮反馈控制器设计如下：

并且根据几何关系可得：

所以

因此，e(t)收敛速度介于v(t)的线性收敛速度和增益参数k的指数收敛速度之间，当横向跟踪误差e(t)很小时，(ke(t)/v(t))²趋近于0，则

积分可得：

e(t)＝e(0)*exp^-kt

对于任意横向误差，微分方程都单调收敛到0，实现了对路径跟踪控制目标。

2.根据权利要求1所述的基于LOS理论的农机路径跟踪方法，其特征在于，所述步骤S1具体包括：

S11，通过北斗RTK定位模块和视觉识别CDD模块设定农机工作田块边界，进而打点出若干条平行的直线轨迹；

S12，北斗RTK导航模块实时输出农机的位置，轨迹点信息，同时与矩形田块的三个顶点坐标转化到高斯平面坐标系中，并将规划的路径以二维数组形式储存到导航***中，实现导航路径规划。

3.根据权利要求2所述的基于LOS理论的农机路径跟踪方法，其特征在于，所述步骤S11具体为：

利用RTK-CCD定位视觉传感器融合***，测定试验田三个坐标顶点确定一个矩形，假设为地头节点A、B、C、D定位坐标；以每条作业行距离最长为原则，结合地块实际几何形状，选取边界AD作为作业行规划的基准线，以其为基准划定若干条平行作业线，平行线距离依作业行间距而定，最后一条作业线与边界BC的最短距离不得小于1/2个作业间距，将所有Q条平行作业线与边界AB和CD的焦点，作为规划作业线节点，并将相应作业线节点存进二维数组中，供基于LOS算法器的导航控制软件调用。

4.根据权利要求1所述的基于LOS理论的农机路径跟踪方法，其特征在于，将导航控制器得出的车轮转向偏差信息传送到农机下位机***中，由转向传感器和光电编码器的值来修正真正的偏差值，将其传送到控制机构液压***，来控制液压***的流量和流向，最终控制车轮的转向，达到车辆按照设定的路线行驶。

Images