CN108919792B - 一种自动导航系统路径规划控制方法 - Google Patents

Abstract

本发明涉及一种自动导航系统路径规划控制方法，建立控制终端与电控转向盘和GPS卫星信号接收器之间的信号通讯，然后基于GPS卫星信号接收器的定位信息，用户根据实际作业需求在控制终端上设置规划路径的A点、B点、行距信息后，控制终端自动生成作业路径，最后控制终端基于所设定的作业路径，基于GPS卫星信号接收器的定位数据，实时计算农机与作业路径的位置偏差，计算所得的偏差数据作为电控转向盘的控制信号，电控转向盘根据该控制信号实现农机精确地跟踪作业路径。本发明在简化自动导航路径规划的控制程序以及优化路径路径的规划程序的同时能够实现农机自动换行作业，精确跟踪所设作业路径进行作业，使得农机作业更加自动化。

Description

一种自动导航系统路径规划控制方法

技术领域

本发明涉及农机自动导航控制的技术领域，尤其涉及到一种自动导航系统路径规划控制方法。

背景技术

我国农业自动导航技术在精准农业技术的支持下得到快速发展，并且广泛应用于农业生产各个过程中，自动导航系统的路径规划功能能够使得拖拉机作业区域覆盖整块农田，减少重叠路径、提高作业效率和作业质量。农机作业路径规划控制技术作为农机自动导航系统中的核心控制技术之一，基于农田地块的实际情况设计一条作业效率高的导航路线，农机基于这一作业路径完成农业生产工作。

但农业自动导航技术在农业生产中获得实际应用的主要是农机直线导航技术即AB直线导航。用户在实际作业地块中指定A点和B点的坐标位置，系统将基于AB两点坐标生成一系列平行的导航基准路径，农机将跟踪这一系列作业路径进行农田作业，但是在每条直线路径的尽头需要行驶员手动操纵农机掉头换行即农机地头转向。市面上的农机导航产品已广泛应用于各大农场，但这些导航系统不带有自动转向功能。虽然现阶段农机地头转向以及路径规划控制技术的研究有所突破，但是相关研究仍然进展比较缓慢。现阶段农业导航领域的路径规划仍然停留在在作物行跟踪以及地头转向无约束最优路径规划阶段。

发明内容

本发明的目的在于克服现有技术的不足，提供一种自动导航系统路径规划控制方法,其可简化自动作业路径规划的控制程序，优化路径的规划程序，实现用户基于实际需求精确规划多种作业路径。

本发明的技术方案是这样实现的：

一种自动导航系统路径规划控制方法，包括以下步骤：

S1：搭建自动导航系统路径规划控制平台，控制平台分为控制终端、电控转向盘与GPS卫星信号接收器三部分；

S2：建立控制终端与电控转向盘和GPS卫星信号接收器之间的信号通讯，之间采用CAN总线通讯；

S3：基于GPS卫星信号接收器的定位信息，用户根据实际作业需求在控制终端上设置规划路径的A点、B点、行距信息后，控制终端自动生成一条作业路径；

S4：控制终端基于所设定的作业路径，基于GPS卫星信号接收器的定位数据，实时计算农机与作业路径的位置偏差，计算所得的偏差数据作为电控转向盘的控制信号，电控转向盘根据该控制信号实现农机精确地跟踪作业路径。

上述中，电控转向盘与GPS卫星信号接收器皆为现有的农机自动导航系统，但该自动导航系统不具有路径规划功能；另外，农机路径规划控制平台中，控制终端、电控转向盘与GPS卫星信号接收器皆安装在拖拉机上，因此GPS卫星信号接收器所提供的位置信息皆为农机的实时经纬度坐标。

进一步地，所述步骤S3中，在控制终端上设置作业路径的具体步骤如下：

S3-1：将农机行驶至用户所需的位置a，GPS卫星信号接收器将位置a的经纬度数据发送给控制终端，将其设置为位置A点；

S3-2：将农机行驶至用户所需的位置b，GPS卫星信号接收器将位置b的经纬度数据发送给控制终端,将其设置为位置B点；

S3-3：将所需的行距量输入到控制终端；

S3-4：系统基于步骤S3-1至步骤S3-3的数据完成路径的规划，并且将整个路径进行分段，分为直线路径与转弯路径两种，按照行走的顺序进行编号。

进一步地，所述步骤S4中，控制终端实时计算农机与作业路径的位置偏差的具体步骤如下：

S4-1：控制终端基于WGS-84坐标转换，实时将GPS卫星信号接收器提供的经纬度信息转成导航坐标系下的坐标点信息；

S4-2：控制终端基于转换成导航坐标系下的A点、B点坐标，将导航坐标系旋转成y轴与AB直线平行的坐标系，整个规划的路径与实时的农机的位置信息均转换成该坐标系下的坐标点；

S4-3：控制终端基于旋转后的坐标系，实时计算农机与作业路径的位置偏差。

进一步地，所述步骤S4-1中基于WGS-84的GPS经纬度信息的坐标转换方法，目的是将农机经纬度坐标信息转成基于导航坐标系下的坐标点信息，自动导航系统的控制过程均依据导航坐标系下的坐标点进行计算；这一坐标转换的过程包括经纬度坐标到大地坐标系之间的转换、大地坐标系与导航坐标系的转换两部分；其中，大地坐标系是以地球中心为原点，坐标轴与地球固连，Z轴向沿着地球极轴方向，X轴沿格林尼治子午面和地球赤道平面的交线；导航坐标系即当地地理坐标系，该坐标系原点位于导航系统所处的位置点P，坐标轴指向北、东和当地垂线方向(向下)，根据右手定则，即导航坐标系中z轴指向当地垂线方向(向下)，x轴指向北，y轴指向东；

基于WGS-84的GPS经纬度信息的坐标转换步骤如下：

S4-1-1：经纬度坐标转换成大地坐标系下的坐标点，其转换公式如下：

x₁＝(R_N(φ)+h)cos(φ)cos(λ)；

y₁＝(R_N(φ)+h)cos(φ)sin(λ)；

z₁＝(R_N(φ)(1-e²)+h)sin(φ)；

式中，φ为转换成弧度制后的经度坐标；

λ为转换成弧度制后的纬度坐标；

h为海拔高度；

R_N(φ)为WGS-84模型下地球的横向曲率半径，其计算公式如下：

式中，a为WGS-84模型中地球的长半径，e为WGS-84模型中地球的椭圆偏心率；

S4-1-2：大地坐标转换成导航坐标系下的坐标点，其转换公式如下：

将大地坐标系中的坐标点转换成以点(x₀,y₀,z₀)为原点的导航坐标系，(x₀,y₀,z₀)为大地坐标系中的一个参考点，

为大地坐标系坐标转换成导航坐标系坐标的转换矩阵，其计算公式为：

进一步地，所述步骤S4-2中将导航坐标系旋转成y轴与AB直线平行的坐标系，具体转换过程分为两部分：

S4-2-1、导航坐标系绕y轴旋转180°；

S4-2-2、坐标系绕z轴逆时针旋转90°-θ；

其中，转换步骤S4-2-1相当于导航坐标系绕y轴旋转180°变成坐标系1，其旋转矩阵R₁为：

转换步骤S4-2-2相当于坐标系1绕z轴旋转90°-θ变成局部坐标系，其旋转矩阵R₂为：

即导航坐标系转换成局部坐标系的转换矩阵R的计算公式如下：

最后得出将导航坐标系旋转成y轴与AB直线平行的坐标系的具体转换公式为：

式中,x₀，y₀，z₀为导航坐标系下的坐标点；x₁，y₁，z₁为转换坐标系后的坐标点；

θ为导航坐标系转换至新的坐标系需要旋转的角度值，其计算公式如下：

式中，x_A，y_A为导航坐标系下A坐标点；x_B，y_B为导航坐标系下B坐标点。

进一步地，所述步骤S4-3中实时计算的农机与作业路径的位置偏差，分为直线路径的横向偏差与转弯轨迹的横向偏差；

农机在旋转坐标系下的坐标点为(x_p,y_p),则农机在直线路径的横向偏差计算公式为：

l＝x_p-(x_1A+nL)

式中，x_1A为第一条直线轨迹A点横坐标值，n为农机所跟踪的第n条直线轨迹，L为设置轨迹时输入的行距值；

农机在转弯路径的横向偏差值为农机位置点与圆弧轨迹的圆心的距离减去圆弧半径R；而农机每次转弯时期圆心点位置都不一样；农机跟踪作业路径时，当点农机坐标点的y_p>y_1B且x_p>(x_1B+(n-1)L)时，则农机车辆进入以

为圆心的拐弯半圆弧；当点P坐标的y_p<y_1A且x_p>(x_1A+(n-1)L)时，则农机车辆进入以

为圆心的拐弯半圆弧；其中x_1A，y_1A为第一条直线轨迹A点坐标值的横坐标与纵坐标，x_1B，y_1B为第一条直线轨迹B点坐标值的横坐标与纵坐标。

与现有技术相比，本方案在简化自动导航路径规划的控制程序以及优化路径路径的规划程序的同时能够实现农机自动换行作业，精确跟踪所设作业路径进行作业，使得农机作业更加自动化。

附图说明

图1为本发明一种自动导航系统路径规划控制方法中涉及到的农机路径规划控制平台的示意图；

图2为本发明一种自动导航系统路径规划控制方法实施例所设置的作业路径图；

图3为本发明一种自动导航系统路径规划控制方法中导航坐标系旋转至局部坐标系的旋转步骤拆解示意图；

图4为本发明一种自动导航系统路径规划控制方法中旋转坐标系后新坐标系中的作业路径图；

图5为对本发明进行田间试验时农机路径规划导航时的行走轨迹图；

图6为农机路径规划试验中直线行走轨迹的横向偏差图。

具体实施方式

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

本实施例所述的一种自动导航系统路径规划控制方法，包括以下步骤：

S1：搭建自动导航系统路径规划控制平台，控制平台分为控制终端、电控转向盘与GPS卫星信号接收器三部分；

S2：建立控制终端与电控转向盘和GPS卫星信号接收器之间的信号通讯，之间采用CAN总线通讯；

如图1所示，步骤S1和S2中的电控方向盘以及卫星接收器为市面上现有的自动导航产品，而控制终端自主开发；控制终端包括PCAN-Router CAN路由器和STM32单片机；

S3：农机实时位置信息的获取主要依靠卫星信号接收器来实现，卫星信号接收器实时提供农机位置的经纬度数据；基于GPS卫星信号接收器的定位信息，用户根据实际作业需求在控制终端上设置规划路径的A点、B点、行距信息后，控制终端自动生成一条作业路径，具体步骤如下：

S3-1：将农机行驶至用户所需的位置a，GPS卫星信号接收器将位置a的经纬度数据发送给控制终端，将其设置为位置A点；

S3-2：将农机行驶至用户所需的位置b，GPS卫星信号接收器将位置b的经纬度数据发送给控制终端,将其设置为位置B点；

S3-3：将所需的行距量L输入到控制终端；

S3-4：系统基于步骤S3-1至步骤S3-3的数据完成路径的规划，并且将整个路径进行分段，分为直线路径与转弯路径两种，按照行走的顺序进行编号，如图2所示；

具体地，在导航坐标系上，系统根据点A、B以及行距量L生成整条导航轨迹，其中包括半径为R的转弯路径。点P为农机车辆的位置，农机车辆按照设置的作业路径将依次沿着AB直线、BB1半圆弧、B1A1直线、A1A2半圆弧行走以此类推直至终点；

S4：控制终端基于所设定的作业路径，基于GPS卫星信号接收器的定位数据，实时计算农机与作业路径的位置偏差，计算所得的偏差数据作为电控转向盘的控制信号，电控转向盘根据该控制信号实现农机精确地跟踪作业路径。

其中，控制终端实时计算农机与作业路径的位置偏差的具体步骤如下：

S4-1：控制终端基于WGS-84坐标转换，实时将GPS卫星信号接收器提供的经纬度信息转成导航坐标系下的坐标点信息，过程为：

S4-1-1：经纬度坐标转换成大地坐标系下的坐标点，其转换公式如下：

x₁＝(R_N(φ)+h)cos(φ)cos(λ)；

y₁＝(R_N(φ)+h)cos(φ)sin(λ)；

z₁＝(R_N(φ)(1-e²)+h)sin(φ)；

式中，φ为转换成弧度制后的经度坐标；

λ为转换成弧度制后的纬度坐标；

h为海拔高度；

R_N(φ)为WGS-84模型下地球的横向曲率半径，其计算公式如下：

式中，a为WGS-84模型中地球的长半径，e为WGS-84模型中地球的椭圆偏心率；

S4-1-2：大地坐标转换成导航坐标系下的坐标点，其转换公式如下：

将大地坐标系中的坐标点转换成以点(x₀,y₀,z₀)为原点的导航坐标系，(x₀,y₀,z₀)为大地坐标系中的一个参考点，

为大地坐标系坐标转换成导航坐标系坐标的转换矩阵，其计算公式为：

S4-2：控制终端基于转换成导航坐标系下的A点、B点坐标，将导航坐标系旋转成y轴与AB直线平行的坐标系，整个规划的路径与实时的农机的位置信息均转换成该坐标系下的坐标点，具体的转换公式为：

式中,(x₁，y₁，z₁)为导航坐标系下的坐标点；(x₁，y₁，z₁)为转换坐标系后的坐标点；

θ为导航坐标系转换至新的坐标系需要旋转的角度值，其计算公式如下：

式中，x_A，y_A为导航坐标系下A坐标点；x_B，y_B为导航坐标系下B坐标点；

S4-3：控制终端基于旋转后的坐标系，实时计算农机与作业路径的位置偏差；

图2中，点P到AB直线的距离即为导航的横向偏差，之所以要将导航坐标系进行旋转后，基于旋转后的坐标系计算农机与导航路径的横向偏差，是因为基于导航坐标系计算农机与导航路径的横向偏差计算繁琐，编程复杂，而且容易出错；

以下是基于导航路径计算农机与导航路径的横向偏差的过程：

农机车辆与导航路径的横向偏差是基于点到直线的距离公式来计算的；在设置导航路径之前，需要设定A、B两点以及行间距L，因此A点与B点的坐标是已知的，设定点A坐标为(x_A,y_A)，B点坐标为(x_B,y_B),P点坐标为(x₀,y₀)，则AB直线段的轨迹方程为：

则点P到直线AB段的横向偏差l的计算公式如下：

求得的横向偏差l有正值以及负值，正负之分代表着转弯方向的不同。B₁点坐标为(x_B1，y_B1)。P点的坐标点x₀>x_B时，则农机车辆跟踪的轨迹将从直线AB转至BB₁半圆弧。将点农机车辆位置P点于圆弧圆心的距离与半径R作差即为农机与圆弧轨迹的偏差，偏差的正负之分代表着农机转向的不同。当P点的坐标点x₀<x_B1时，则农机车辆的跟踪轨迹将从BB₁半圆弧转至B₁A₁直线。以此类推，根据P点坐标x₀与直线轨迹终点的x值比较来切换导航路径。

这种导航路径设置方法中，需频繁地求解每条直线的轨迹方程以及每段圆弧坐标的圆心点坐标才能计算农机车辆与导航路径的偏差值，不利于编程控制；

而本实施例将上述导航路径设置的导航坐标系转换成y坐标轴与AB直线相平行的坐标系，如图3所示；将xoy直角坐标系旋转至y轴与AB直线相平行的位置，即图中x₁oy₁直角坐标系；设两个直角坐标系之间的夹角为θ；

在xoy直角坐标系下，A点坐标点为(x_A,y_A),B点坐标点为(x_B,y_B),在x₁oy₁直角坐标系下，A点坐标点为(x_1A,y_1A),B点坐标点为(x_1B,y_1B),则θ的角度求解方法如下式：

则将xoy直角坐标系下的坐标点转换至x₁oy₁局部坐标系下的坐标点，需要经过两步坐标系旋转。如图4所示，为导航坐标系到局部坐标系的转换过程，坐标系中z轴未表示，该轴与x轴与y轴垂直相交。

图3中转换步骤1相当于导航坐标系绕y轴旋转180°变成坐标系1，其旋转矩阵R₁为：

图3中转换步骤2相当于坐标系1绕z轴旋转90°-θ变成局部坐标系，其旋转矩阵R₂为：

因此，导航坐标系转换成局部坐标系的转换矩阵R的计算公式如下：

所以导航坐标系xoy上农机车辆P点的位置坐标(x₀，y₀，z₀)转换局部系上的坐标点(x₁，y₁，z₁)的计算公式为：

如图4所示，基于旋转坐标系x₁oy₁直角坐标系下A、B两点的坐标规划导航路径以及行间距L可推算出后续导航轨迹的坐标点；A点坐标点为(x_1A,y_1A),B点坐标点为(x_1B,y_1B)，则A₁点坐标点为(x_1A+L,y_1A),B₁点坐标点为(x_1B+L,y_1B)，A₂点坐标点为(x_1A+2L,y_1A),B₂点坐标点为(x_1B+2L,y_1B)，以此类推可计算出后续导航路径的点A_n坐标为(x_1A+nL,y_1A)，点B_n坐标为(x_1B+nL,y_1B)，A_n拐弯半圆弧的圆心坐标为

B_n拐弯半圆弧的圆心坐标为

农机车辆点P在旋转坐标系x₁oy₁直角坐标系下的坐标为(x₁,y₁)，P与A_nB_n直线的横向偏差为：

l＝x₁-(x_1A+nL)；

当点P坐标的y₁>y_1B且x₁>(x_1B+(n-1)L)时，则农机车辆进入以

为圆心的拐弯半圆弧，当点P坐标的y₁<y_1A且x₁>(x_1A+(n-1)L)时，则农机车辆进入以

为圆心的拐弯半圆弧；P点与圆弧轨迹的圆心的距离减去圆弧半径R即为农机车辆在(转弯)圆弧轨迹的偏差值；这种基于旋转坐标系下建立导航路径的方法在编程控制上简单、出错率低；

将自动导航系统路径规划控制平台搭载在插秧机上，设计路径导航试验，测试自动导航系统路径规划控制平台的工作性能；由于场地面积有限，因此将插秧机的导航路径轨迹设置成椭圆路径进行自动导航路径规划试验，导航控制终端记录插秧机路径轨迹的经纬度变化，用于分析自动导航系统路径规划控制平台的工作性能；将导航路径规划试验中插秧机的行走轨迹的经纬度坐标转换成基于旋转坐标系下的坐标点，只保留了插秧机在导航状态下的行走轨迹，如图5所示；图中A、B两点位置在图中标注位置所示，两条直线行距L＝7m，AB直线距离约67m；图中所展示的轨迹图为插秧机沿着该椭圆轨迹行走12圈所叠加在一起的轨迹图形。

自动导航系统路径规划控制平台的主要功能在于控制拖拉机在直线路径时的作业效果，保证农机在直线导航时的精度达到标准。因此在路径规划导航试验中，主要分析插秧机在直线路径的导航精度。计算插秧机路径轨迹相对于AB直线的横向偏差，根据横向偏差结果来分析插秧机直线路径导航的导航精度。如图6，为插秧机沿直线段路径导航时所有行驶轨迹的横向偏差变化曲线。经计算，直线路径段插秧机行走轨迹的平均绝对偏差为2.52cm，标准差为3.88cm，最大横向偏差为13.74cm，符合导航系统的导航精度要求，而且在换行作业时，插秧机能够基于所设定的导航路径自主换行，说明这种导航路径规划控制的方法是可行有效的。

本实施例在简化自动导航路径规划的控制程序以及优化路径路径的规划程序的同时能够实现农机自动换行作业，精确跟踪所设作业路径进行作业，使得农机作业更加自动化。

以上所述之实施例子只为本发明之较佳实施例，并非以此限制本发明的实施范围，故凡依本发明之形状、原理所作的变化，均应涵盖在本发明的保护范围内。

Claims (4)

1.一种自动导航系统路径规划控制方法，其特征在于，包括以下步骤：

S1：搭建自动导航系统路径规划控制平台，控制平台分为控制终端、电控转向盘与GPS卫星信号接收器三部分；

S2：建立控制终端与电控转向盘和GPS卫星信号接收器之间的信号通讯，之间采用CAN总线通讯；

S3：基于GPS卫星信号接收器的定位信息，用户根据实际作业需求在控制终端上设置规划路径的A点、B点、行距信息后，控制终端自动生成一条作业路径；

S4：控制终端基于所设定的作业路径，基于GPS卫星信号接收器的定位数据，实时计算农机与作业路径的位置偏差，计算所得的偏差数据作为电控转向盘的控制信号，电控转向盘根据该控制信号实现农机精确地跟踪作业路径；

所述步骤S4中，控制终端实时计算农机与作业路径的位置偏差的具体步骤如下：

S4-1：控制终端基于WGS-84坐标转换，实时将GPS卫星信号接收器提供的经纬度信息转成导航坐标系下的坐标点信息；

S4-2：控制终端基于转换成导航坐标系下的A点、B点坐标，将导航坐标系旋转成y轴与AB直线平行的坐标系，整个规划的路径与实时的农机的位置信息均转换成该坐标系下的坐标点；

S4-3：控制终端基于旋转后的坐标系，实时计算农机与作业路径的位置偏差；

所述步骤S4-1中基于WGS-84的GPS经纬度信息的坐标转换过程如下：

S4-1-1：经纬度坐标转换成大地坐标系下的坐标点，其转换公式如下：

x₁＝(R_N(φ)+h)cos(φ)cos(λ)；

y₁＝(R_N(φ)+h)cos(φ)sin(λ)；

z₁＝(R_N(φ)(1-e²)+h)sin(φ)；

式中，φ为转换成弧度制后的经度坐标；

λ为转换成弧度制后的纬度坐标；

h为海拔高度；

R_N(φ)为WGS-84模型下地球的横向曲率半径，其计算公式如下：

式中，a为WGS-84模型中地球的长半径，e为WGS-84模型中地球的椭圆偏心率；

S4-1-2：大地坐标转换成导航坐标系下的坐标点，其转换公式如下：

将大地坐标系中的坐标点转换成以点(x₀,y₀,z₀)为原点的导航坐标系，(x₀,y₀,z₀)为大地坐标系中的一个参考点，R_e ^t为大地坐标系坐标转换成导航坐标系坐标的转换矩阵，其计算公式为：

2.根据权利要求1所述的一种自动导航系统路径规划控制方法，其特征在于，所述步骤S3中，在控制终端上设置作业路径的具体步骤如下：

S3-1：将农机行驶至用户所需的位置a，GPS卫星信号接收器将位置a的经纬度数据发送给控制终端，将其设置为位置A点；

S3-2：将农机行驶至用户所需的位置b，GPS卫星信号接收器将位置b的经纬度数据发送给控制终端,将其设置为位置B点；

S3-3：将所需的行距量输入到控制终端；

S3-4：系统基于步骤S3-1至步骤S3-3的数据完成路径的规划，并且将整个路径进行分段，分为直线路径与转弯路径两种，按照行走的顺序进行编号。

3.根据权利要求1所述的一种自动导航系统路径规划控制方法，其特征在于，所述步骤S4-2中将导航坐标系旋转成y轴与AB直线平行的坐标系，具体转换过程分为两部分：

S4-2-1、导航坐标系绕y轴旋转180°；

S4-2-2、坐标系绕z轴逆时针旋转90°-θ；

其中，转换步骤S4-2-1相当于导航坐标系绕y轴旋转180°变成坐标系1，其旋转矩阵R₁为：

转换步骤S4-2-2相当于坐标系1绕z轴旋转90°-θ变成局部坐标系，其旋转矩阵R₂为：

即导航坐标系转换成局部坐标系的转换矩阵R的计算公式如下：

最后得出将导航坐标系旋转成y轴与AB直线平行的坐标系的具体转换公式为：

式中,(X₀，Y₀，Z₀)为导航坐标系下的坐标点；(X₁，Y₁，Z₁)为转换坐标系后的坐标点；

θ为导航坐标系转换至新的坐标系需要旋转的角度值，其计算公式如下：

式中，x_A，y_A为导航坐标系下A坐标点；x_B，y_B为导航坐标系下B坐标点。

4.根据权利要求1所述的一种自动导航系统路径规划控制方法，其特征在于，所述步骤S4-3中实时计算的农机与作业路径的位置偏差，分为直线路径的横向偏差与转弯轨迹的横向偏差；

农机在旋转坐标系下的坐标点为(x_p,y_p),则农机在直线路径的横向偏差计算公式为：

l＝x_p-(x_1A+nL)

式中，x_1A为第一条直线轨迹A点横坐标值，n为农机所跟踪的第n条直线轨迹，L为设置轨迹时输入的行距值；

农机在转弯路径的横向偏差值为农机位置点与圆弧轨迹的圆心的距离减去圆弧半径R；而农机每次转弯时期圆心点位置都不一样；农机跟踪作业路径时，当点农机坐标点的y_p＞y_1B且x_p＞(x_1B+(n-1)L)时，则农机车辆进入以

为圆心的拐弯半圆弧；当点P坐标的y_p＜y_1A且x_p＞(x_1A+(n-1)L)时，则农机车辆进入以

为圆心的拐弯半圆弧；其中x_1A，y_1A为第一条直线轨迹A点坐标值的横坐标与纵坐标，x_1B，y_1B为第一条直线轨迹B点坐标值的横坐标与纵坐标。

Images