一种农机自动作业的路径规划方法与流程

1.本发明涉及农机自动驾驶技术领域，特别是涉及一种农机自动作业的路径规划方法。


背景技术：

2.农机自动驾驶工程中，轨迹规划具有举足轻重的地位和作用。但如果算法生成的跟踪点间距过大，会造成农机的跟踪误差大；生成的跟踪点间距小，会造成算法申请的内存空间较大。
3.轨迹规划本质上是一个路径最优化问题，即期望找到一条遍历所有作业线的最短路径。解决路径最优化问题的现有技术有多种，例如dijkstra算法，a*算法，floyd算法等，但这些方法不太适合农机自动作业。本方法使用了一种现有技术：旅行商算法(简称tsp算法)，这种算法可以用于农机自动作业的轨迹规划，但如果实际工况下作业线数量较大，则大规模旅行商算法的求解效率会降低。且即便一次性求解所有作业线的作业序列可以使得总体的路径最短，但这并不符合实际工况：一是作业序列可能出现跨度极大的情况，二是实际工况下农机没法一次性走完所有作业线，实际工况下所有作业线要好几天才能分段走完。


技术实现要素：

4.基于此，本发明实施例提供一种农机自动作业的路径规划方法，旨在解决减少大面积作业时规划路径点的占用空间。
5.为实现上述目的，本发明提出如下技术方案：
6.一种农机自动作业的路径规划方法，具体为：将作业地块分成三层结构，分别为单元、全局路径点和局部路径点；整个作业地块分为数个相连接的单元，所述单元内有数条作业线，所述作业线由一组全局路径点确定，所述局部路径点由b样条(b
‑
spline)插值生成，所述b样条以全局路径点为节点，以全局路径点附近的切矢量为控制点。所述切矢量通过临近的全局路径点生成。
7.全局路径点的间距在米级，所以直接跟踪全局路径点的误差较大。车辆实际跟踪的路径是局部路径点，局部路径点的间距在分米级，也可根据实际情况增加插值点将间距降到厘米级，这样可以增加跟踪精度，但同时也会增加计算和存储数据的成本。
8.一种农机自动作业的路径规划方法，包括以下步骤：
9.(1)生成作业地块全局路径的全局路径点；
10.(2)将整个作业地块分为数个相连接的单元，单个单元中有n条作业线；单个单元的作业线数量n根据最小转向半径r_min与作业线间距line_width确认；所述作业线由一组全局路径点确定；
11.(3)农机在相邻全局路径点移动时，实际跟踪的路径是局部路径点，所述局部路径点由b样条插值生成，所述b样条以全局路径点为节点，以全局路径点附近的切矢量为控制
点；
12.(4)在每个单元内，采用旅行商算法(tsp)进行单元内的n条作业线的切换顺序，实现路径更新，完成自动作业。
13.在步骤(1)中所述生成全局路径点，具体为：通过在局部坐标系中输入第一条作业线的始点和末点(其中，本实施例中作业线的始点标记为a点，作业线的末点标记为b点)，并输入作业线间距line_width、作业线数量line_num以及最小转向半径r_min共5个参数规划出整个地块的全局路径点；所述作业线由一组全局路径点确定。
14.通过第一条作业线始点和末点位置信息的输入，可以获得起始位点、作业线与地理北的夹角theta、作业线线长line_length的信息。所述第一条作业线上的全局路径点通过在ab点间线性插值生成；生成后初始化一个表示全局路径点位置的指标index_global，用来标识距离当前农机最近的全局路径点。
15.步骤(1)中所述全局路径由作业线或转向路径构成；当农机驶入作业线末端，则查询下一条作业线的序号，生成转向路径，用两条作业线的位置关系生成转向路径，让农机转向过渡；当全局路径类型为转向路径时，用下一条作业线的序号生成下一条作业线；根据农机作业位置，依次逐条生成作业线或转向路径，这样生成路径的好处在于，能够降低数据量；避免了同时生成所有全局路径的大数据量的缺陷。
16.所述作业线的生成方法，具体为：需要用户输入ab点(其中a是作业线的始点，b是作业线的末点，通过这两点可以得到作业线与地理北的夹角theta和作业线的线长line_length)、作业线的线距line_width、作业线的条数line_num.当车辆当前位置靠近转向路径末尾，则将全局路径更新为作业线，全局路径类型置为0；车辆作业过程中每条作业线都有编号index_line，通过编号、线距、线长以及角度就能生成作业线，通过判定编号的奇偶就能判定作业的行驶方向；更新路径时，新路径会把旧路径覆盖。
17.所述的转向路径的生成方法，具体为：如果车辆当前位置靠近作业线末尾，则将全局路径更新为转向路径，全局路径类型置为1；取当前作业线的终点p1和下一作业线的末点p2，在连线间插入半径为最小转向半径的两个圆心，圆心1指向p1的矢量是半径矢量1，将这个矢量旋转90度生成走出当前作业线的1/4圆弧；将半径矢量1的圆心从圆心1变到圆心2，并旋转90度，变成半径矢量2，用同样的方法生成进入下一条作业线的1/4圆弧；然后判定两段圆弧连接处的点是否过近，如果过近，则生成的插值会产生扭结或锯齿，需要合并；如果过远，则取第一段圆弧的末点和第二段圆弧的始点，在这两点间进行线性插值，直到点间距小于一个期望值；新路径会把旧路径覆盖。
18.其中，转向路径中半径矢量的旋转具有方向性，实施例的方向通过叉乘判定，计算当前路径的方向矢量与半径矢量1的叉乘，如果叉乘结果为正，则进行顺时针旋转，否则进行逆时针旋转；旋转方向的判定方法不限于此。
19.步骤(1)中所述的作业线数量line_num不定的持续作业时；可将line_num设置为一个足够大的数(或者设置为无穷大)，此时车辆将持续跟踪路径，直到用户手动切出自动状态。以ab矢量所指方向为第一条作业线的方向，则所有作业线将生成在第一条作业线的一侧。
20.在步骤(2)中：
21.所述的单个单元的作业线数量n满足以下条件：n＝2*ceil(2*r_min/line_width)
+a；其中ceil函数表示向上取整，r_min表示最小转向半径，line_width表示作业线间距，a为≥1的整数。
22.根据实际情况，每个单元的作业线数量n的下限(n＝1时)可以适当增加，但不能减少，减少将导致路径无法生成(前轮最大转向角将超过30度，无法根据规划路径进行转向)。
23.所述的数个相连接的单元，具体的单元的个数＝floor(line_num/n)，其中floor函数为向下取整；line_num为作业线总数量；n为单个单元的作业线数量。单元的数量确定后，初始化一个标识单元的指标index_block，用来标识当前车辆所在单元。
24.步骤(3)中：
25.所述的局部路径点在农机最近的全局路径点附近生成，通过3次b样条生成，农机附近的数个全局路径点均参与局部路径点的生成，这些全局路径点作为局部路径点的节点，算法通过计算这些节点的连线的斜率，在每个节点旁插入两个控制点；这样生成的b样条将通过节点，但不会通过控制点。现有技术的算法中没有将路径点设置为样条节点，因此生成的样条不通过路径点；通过插入控制点将路径点变为节点后便解决了这个问题，增加了插值的精度；局部路径初始化后，初始化一个表示局部路径点位置的指标index_local，用来标识距离当前车辆最近的局部路径点。
26.所述控制点的算法生成方法为：
27.1)检查当前节点是否是第一个全局路径点，如果是，则用下一全局路径点减当前全局路径点的差值作为切矢量；
28.2)检查当前节点是否是最后一个全局路径点，如果是，则用当前全局路径点减前一全局路径点的差值作为切矢量；
29.3)如果当前节点介于起始路径点与终止路径点之间，则用上一路径点减前一路径点的半差值作为切矢量；
30.4)用当前节点坐标加减切矢量生成两个控制点。
31.所述跟踪的算法采用纯跟踪控制，所述纯跟踪控制需要输入3个参数，分别为控制周期dt、车辆轴距l、纵向控制反馈速率，状态矢量为[state_pos_e,state_pos_n,state_heading,state_speed]；其中，state_pos_e为局部坐标系下车辆位置的东向坐标，state_pos_n为局部坐标系下车辆位置的北向坐标，state_heading为车辆航向角，state_speed为车辆速率。
[0032]
初始化时将state_pos_e设置为作业线始点a点的横坐标，state_pos_n设置为a点的纵坐标，state_heading设置为atan2(b(2)
‑
a(2),b(1)
‑
a(1))，state_speed设置为纵向控制反馈速率；其中b点为作业线的末点。
[0033]
所述局部路径点的生成过程，具体为；车辆行驶过程中动态判定距离车辆实际位置最近的全局路径点n，然后在n附近的全局路径点n
‑
1到n+2(实施例不限于四个点，增加全局路径点数会增加局部路径点数，依具体工况进行配置)之间生成局部路径，并判定距离车辆实际位置最近的局部路径点。所有点在车辆运动过程中实时更新。
[0034]
在步骤(4)中：
[0035]
所述的旅行商算法(tsp)给出了单元内n条作业线的切换顺序，使得车辆的行驶路程局部最优。在tsp算法中，把间距小于2*r_min的作业线当作无穷大距离，这样生成的路线便符合阿克曼转向原理，此时前轮转角的变化率光滑，并且在不另加约束条件的情况下可
以自动限制最大前轮转角在合理的范围内；作业线的切换顺序确定后，初始化一个标识作业线位置的指标index_rank，用来标识当前车辆所在作业线。
[0036]
上述所有指标在车辆跟踪路径过程中实时更新。
[0037]
所述的路径更新，具体为：主循环开始时，判定当前车辆状态下需不需要更新单元或作业线，如果需要，则更新单元index_block＝index_block+1，或更新作业线index_rank＝index_rank+1；在更新作业线时重置全局路径点指标index_global。
[0038]
当车辆在作业线上，给全局路径点一个标签(例如采用标签0)；当车辆在转向路径上，给全局路径点另一个标签(例如采用标签1)；更新全局路径时，通过判定当前标签来判定下一条全局路径的类型；如果当前全局路径是作业线，则下一条全局路径更新为转向路径；如果当前全局路径是转向路径，则下一条全局路径更新为作业线。
[0039]
查询距离当前车辆状态最近的全局路径点，如果当前全局路径点发生变化，则更新局部路径；更新后查询距离当前车辆状态最近的局部路径点，将这一点作为路径起点，设置预瞄距离，在局部路径上对路径进行积分，得到预瞄点,并通过纯跟踪控制跟踪预瞄点，通过纯跟踪控制输出的前轮转角对车辆进行横向控制，完成车辆状态更新。
[0040]
所述的农机自动作业的路径规划方法，具体作业逻辑流程步骤如下：
[0041]
1)初始化车辆位置，并且初始化全局路径，选择初始化全局路径类型为作业线；
[0042]
2)通过对距离车辆位置最近的全局路径点靠近全局路径末尾进行判断，
[0043]
2.1)当距离车辆位置最近的全局路径点未靠近全局路径末尾，查询距离当前车辆位置最近的全局路径点，在查询到的全局路径点附近用b样条生成局部路径点，查询距离当前车辆位置最近的局部路径点，轨迹跟踪控制器对车辆状态进行更新，重新进行步骤2)的判断；
[0044]
2.2)当距离车辆位置最近的全局路径点靠近全局路径末尾，判断当前路径类型；
[0045]
2.2.1)判断当前路径类型为转向路径时，生成作业线作为全局路径，切换全局路径类型为作业线；查询距离当前车辆位置最近的全局路径点；
[0046]
2.2.2)判断当前路径类型为作业线时，判断当前单元是否是最后一个单元的最后一条作业线；
[0047]
2.2.2.1)判断当前单元是最后一个单元的最后一条作业线，结束跟踪；
[0048]
2.2.2.2)判断当前单元不是最后一个单元的最后一条作业线，判断当前作业线是否是当前单元的最后一条作业线；
[0049]
2.2.2.2.1)判断当前作业线是当前单元的最后一条作业线；更新重复单元；
[0050]
2.2.2.2.2)判断当前作业线不是当前单元的最后一条作业线，生成转向路径作为全局路径，切换全局路径类型为转向路径。
[0051]
本发明的技术方案与现有技术相比，具有以下有益效果：
[0052]
1.本发明将作业地块分为数个相连接的单元，车辆走完一个单元，就进入下一个单元；设置单元是因为一次性不能作业完大量(例如100条)作业线，且作业线数量过多，旅行商(tsp)算法生成的作业线跨度大、算法计算时间长；储存作业序列也需要较大空间；为此拆分大量作业线，生成多个小单元，逐个作业；这样符合实际工况，降低了tsp作业序列的计算时间，降低了作业序列的存储成本。
[0053]
2.现有技术的算法中没有将路径点设置为样条节点，因此生成的样条不通过路径
点；本发明通过插入控制点将路径点变为节点后便解决了这个问题，增加了插值的精度。
[0054]
3.本发明中所述全局路径由数条作业线和转向路径构成；在作业线生成过程中，是按照作业线逐条生成；根据农机作业位置，由上一条作业线掉头时生成下一条作业线；这样这条生成的好处在于，能够降低数据量。
附图说明
[0055]
为了更清楚地说明本发明实施例或现有技术中的技术方案，下面将对实施例或现有技术描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图示出的结构获得其他的附图。
[0056]
图1为本发明实施例中所述的全局路径的示意图；
[0057]
图2为本发明实施例中所述的转向路径的示意图；
[0058]
图3为本发明实施例中转向路径的生成过程示意图；
[0059]
图4为本发明实施例中局部路径的生成过程示意图；
[0060]
图5为本发明实施例中局部路径点的跟踪示意图一；
[0061]
图6为本发明实施例中局部路径点的跟踪示意图二；
[0062]
图7为本发明实施例中作业地块分的单元和全局路径点的局部示意图；
[0063]
图8为本发明实施例中作业地块分的单元和全局路径点的示意图；
[0064]
图9为本发明实施例中所述路径规划方法的流程示意图。
[0065]
其中图1
‑
图8中：横向坐标轴的e指代为局部坐标系下车辆位置的东向坐标；纵向坐标轴的n指代为局部坐标系下车辆位置的北向坐标。
[0066]
本发明目的的实现、功能特点及优点将结合实施例，参照附图做进一步说明。
具体实施方式
[0067]
下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明的一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有作出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。
[0068]
需要说明，若本发明实施例中有涉及方向性指示(诸如上、下、左、右、前、后、顶、底
……
)，则该方向性指示仅用于解释在某一特定姿态(如附图所示)下各部件之间的相对位置关系、运动情况等，如果该特定姿态发生改变时，则该方向性指示也相应地随之改变。
[0069]
另外，若本发明实施例中有涉及“第一”、“第二”等的描述，则该“第一”、“第二”等的描述仅用于描述目的，而不能理解为指示或暗示其相对重要性或者隐含指明所指示的技术特征的数量。由此，限定有“第一”、“第二”的特征可以明示或者隐含地包括至少一个该特征。另外，各个实施例之间的技术方案可以相互结合，但是必须是以本领域普通技术人员能够实现为基础，当技术方案的结合出现相互矛盾或无法实现时应当认为这种技术方案的结合不存在，也不在本发明要求的保护范围之内。
[0070]
为了解决减少大面积作业时规划路径点的占用空间的技术问题，本发明提出了一种农机自动作业的路径规划方法。
[0071]
下面结合实施例对本路径规划方法作进一步详细的描述，但本算法的实施方式不限于此。
[0072]
在农机作业过程中，在100条300米作业线的工况下，一次性走完100条作业线并不符合实际，并且此时tsp算法给出的作业线可能会出现非常大的跨度，比如从第10条作业线跨到第80条作业线，再跨到第5条作业线。并且当作业线数量过大时，最优路径的解算速率也会变慢。
[0073]
本发明的路径规划方法采用的单元结构设计符合实际作业工况，并且规避了大规模tsp难以快速求解的问题。本算法可将100条作业线拆分成数个单元，使得每个单元都满足tsp问题的解。虽然这样的结果使得全局的规划路径并不是最短的，但在局部上仍然是最优的。这样规划的路径更加符合实际工况，并且降低了数据的计算量和存储量。
[0074]
基于此，本发明实施例提供了一种农机自动作业的路径规划方法，具体为：将作业地块分成三层结构，分别为单元、全局路径点和局部路径点；整个作业地块分为数个相连接的单元，所述单元内有数条作业线，所述作业线由一组全局路径点确定，所述局部路径点由b样条(b
‑
spline)插值生成，所述b样条以全局路径点为节点，以全局路径点附近的切矢量为控制点；所述切矢量通过临近的全局路径点生成。
[0075]
全局路径点的间距在米级，所以直接跟踪全局路径点的误差较大。车辆实际跟踪的路径是局部路径点，局部路径点的间距在分米级，也可根据实际情况增加插值点将间距降到厘米级，这样可以增加跟踪精度，但同时也会增加计算和存储数据的成本。
[0076]
一种农机自动作业的路径规划方法，包括以下步骤：
[0077]
(1)生成作业地块全局路径的全局路径点；
[0078]
(2)将整个作业地块分为数个相连接的单元，单个单元中有n条作业线；单个单元的作业线数量n根据最小转向半径r_min与作业线间距line_width确认；所述作业线由一组全局路径点确定；
[0079]
(3)农机在相邻全局路径点移动时，实际跟踪的路径是局部路径点，所述局部路径点由b样条插值生成，所述b样条以全局路径点为节点，以全局路径点附近的切矢量为控制点；
[0080]
(4)在每个单元内，采用旅行商算法(tsp)进行单元内的n条作业线的切换顺序，实现路径更新，完成路径规划作业。
[0081]
在步骤(1)中：
[0082]
所述生成全局路径点，具体为：通过在局部坐标系中输入第一条作业线的始点和末点(其中，本实施例中作业线的始点标记为a点，作业线的末点标记为b点)，并输入作业线间距line_width、作业线数量line_num以及最小转向半径r_min共5个参数规划出整个地块的全局路径点；所述作业线由一组全局路径点确定。
[0083]
通过第一条作业线始点和末点位置信息的输入，可以获得起始位点、作业线与地理北的夹角theta，作业线上的全局路径点通过在ab点间线性插值生成，生成后初始化一个表示全局路径点位置的指标index_global，用来标识距离当前农机最近的全局路径点。
[0084]
所述全局路径由数条作业线和转向路径构成；在作业线生成过程中，是按照作业线逐条生成；根据农机作业、作业线线长line_length的信息。
[0085]
步骤(1)中所述全局路径由作业线或转向路径构成；当农机驶入作业线末端，则查
询下一条作业线的序号，生成转向路径，用两条作业线的位置关系生成转向路径，让农机转向过渡；当全局路径类型为转向路径时，用下一条作业线的序号生成下一条作业线；根据农机作业位置，依次逐条生成作业线或转向路径，这样生成路径的好处在于，能够降低数据量；避免了同时生成所有全局路径的大数据量的缺陷。
[0086]
所述全局路径的作业线主要由图1所示构成。图1这个例子中，第一条作业线的方向是从(0,0)向右上方作业；每条作业线作业完之后就掉头驶入下一条作业线，直到将所有作业线走完；实际跟踪时，不会一次性将图1所示的所有作业线一并生成，而是逐条生成，农机移动到一条作业线的始点时生成当条作业线，这样就降低了数据量。
[0087]
当车辆在作业线上，给全局路径点一个标签(例如采用标签0)；当车辆在转向路径上，给全局路径点另一个标签(例如采用标签1)；更新全局路径时，通过判定当前标签来判定下一条全局路径的类型；如果当前全局路径是作业线，则下一条全局路径更新为转向路径；如果当前全局路径是转向路径，则下一条全局路径更新为作业线。
[0088]
所述作业线的生成方法，具体为：需要用户输入ab点(其中a是作业线的始点，b是作业线的末点，通过这两点可以得到作业线与地理北的夹角theta和作业线的线长line_length)、作业线的线距line_width、作业线的条数line_num.当车辆当前位置靠近转向路径末尾，则将全局路径更新为作业线，全局路径类型置为0。车辆作业过程中每条作业线都有编号index_line，通过编号、线距、线长以及角度就能生成作业线，通过判定编号的奇偶就能判定作业的行驶方向；更新路径时，新路径会把旧路径覆盖。
[0089]
所述的转向路径的生成方法(如图2和图3所示)，具体为：如果车辆当前位置靠近作业线末尾，则将全局路径更新为转向路径，全局路径类型置为1；取当前作业线的终点p1和下一作业线的末点p2，在连线间插入半径为最小转向半径的两个圆心，圆心1(o1)指向p1的矢量是半径矢量1(r1)，将这个矢量旋转10、20、
…
、80、90度生成走出当前作业线的1/4圆弧；将半径矢量1(r1)的圆心从圆心1(o1)变到圆心2(o2)，并旋转90度，变成半径矢量2(r2)，用同样的方法生成进入下一条作业线的1/4圆弧；然后判定两段圆弧连接处的点是否过近，如果过近，则生成的插值会产生扭结或锯齿，需要合并；如果过远，则取第一段圆弧的末点和第二段圆弧的始点，在这两点间进行线性插值，直到点间距小于一个期望值；插值时设置一个上限，如果插值点数过多，则跳出插值；更新路径时，新路径会把旧路径覆盖；转向时不考虑车辆跟踪精度。
[0090]
其中，转向路径中半径矢量的旋转具有方向性，实施例的方向通过叉乘判定，计算当前路径的方向矢量与半径矢量1的叉乘，如果叉乘结果为正，则进行顺时针旋转，否则进行逆时针旋转；旋转方向的判定方法不限于此。
[0091]
仿真中只考虑到矩形田的路径生成，平行四边形地块的路径生成方式类似，一般四边形地块的路径生成不在考虑范围内。
[0092]
步骤(1)中所述的作业线数量line_num不定的持续作业时；可将line_num设置为一个足够大的数(或者设置为无穷大)，此时车辆将持续跟踪路径，直到用户手动切出自动状态；以ab矢量所指方向为第一条作业线的方向，则所有作业线将生成在第一条作业线的一侧。
[0093]
在步骤(2)中：
[0094]
所述的单个单元的作业线数量n满足以下条件：n＝2*ceil(2*r_min/line_width)
+a；其中ceil函数表示向上取整，r_min表示最小转向半径，line_width表示作业线间距，a为≥1的整数。
[0095]
根据实际情况，每个单元的作业线数量n的下限(n＝1时)可以适当增加，但不能减少，减少将导致路径无法生成(前轮最大转向角将超过30度，无法根据规划路径进行转向)。
[0096]
所述的数个相连接的单元，具体的单元的个数＝floor(line_num/n)，其中floor函数为向下取整；line_num为作业线总数量；n为单个单元的作业线数量。
[0097]
单元的数量确定后，初始化一个标识单元的指标index_block，用来标识当前车辆所在单元。
[0098]
车辆走完一个单元，就进入下一个单元；设置单元是因为一次性不能作业完大量(例如100条)作业线，且作业线数量过多，旅行商(tsp)算法生成的作业线跨度大、算法计算时间长；储存作业序列也需要较大空间；为此拆分大量作业线，生成多个小单元，逐个作业。这样符合实际工况，降低了tsp作业序列的计算时间，降低了作业序列的存储成本。实施例使用的作业序列是作业线的编号，例如当前作业线是第5条，下一条作业线是第12条，下下条是第4条，储存的序列是(5,12,4)。但也可以使用差分序列，在上面同样的例子下，如果使用差分序列，则当前作业线是第5条，下一条作业线是第5+7条，下下条作业线是12
‑
8条，储存的差分序列是(5,7,
‑
8)。两种方案可以产生同样的技术效果，均在本发明的保护范围内。
[0099]
步骤(2)中关于作业地块的最后一个单元；因为总的作业线条数不一定是单元中作业线条数的整数倍，因此最后一个单元应该和剩下的作业线合并。比如“100条300米作业线”的例子，如果单元的作业线是11条，则前8个单元具有相同的结构，但第9个单元的作业线是12条，这样才能遍历完所有作业线(100＝11*8+12*1)。步骤(2)生成单元时，实际上生成的是第一个单元的作业次序，在上面举的100条作业线的例子中，第2到8个单元和第1个单元是一样的，第9个单元独立生成作业次序。
[0100]
步骤(3)中：
[0101]
所述的局部路径点在农机最近的全局路径点附近生成，通过3次b样条生成，农机附近的数个全局路径点均参与局部路径点的生成，这些全局路径点作为局部路径点的节点，算法通过计算这些节点的连线的斜率，在每个节点旁插入两个控制点；这样生成的b样条将通过节点，但不会通过控制点。现有技术的算法中没有将路径点设置为样条节点，因此生成的样条不通过路径点；通过插入控制点将路径点变为节点后便解决了这个问题，增加了插值的精度；局部路径初始化后，初始化一个表示局部路径点位置的指标index_local，用来标识距离当前车辆最近的局部路径点。
[0102]
所述跟踪的算法采用纯跟踪控制，所述纯跟踪控制需要输入3个参数，分别为控制周期dt、车辆轴距l、纵向控制反馈速率，状态矢量为[state_pos_e,state_pos_n,state_heading,state_speed]；其中，state_pos_e为局部坐标系下车辆位置的东向坐标，state_pos_n为局部坐标系下车辆位置的北向坐标，state_heading为车辆航向角，state_speed为车辆速率。
[0103]
初始化时将state_pos_e设置为a点(作业线的始点标记)的横坐标，state_pos_n设置为a点的纵坐标，state_heading设置为atan2(b(2)
‑
a(2),b(1)
‑
a(1))，state_speed设置为纵向控制反馈速率；其中b点为作业线的末点。
[0104]
所述局部路径点的生成过程示意如图4所示；车辆行驶过程中动态判定距离车辆
实际位置最近的全局路径点n，然后在n附近的全局路径点n
‑
1到n+2(实施例不限于四个点，增加全局路径点数会增加局部路径点数，依具体工况进行配置)之间生成局部路径，并判定距离车辆实际位置最近的局部路径点+。所有点在车辆运动过程中实时更新。
[0105]
本实施例中的局部路径点是一套3阶b样条插值点，本发明实施方式不限于样条阶数以及样条生成方法。生成一条样条，有两种点，一种是节点，样条将通过节点；一种是控制点，样条不通过控制点，控制点可以控制样条的弯曲方向、弯曲程度。直接用全局路径点作为样条控制点，则误差较大；为了让b样条通过全局路径点，就是要让全局路径点成为节点，这需要在每个全局路径点附近额外插2个控制点，这2个控制点到局部路径点的距离相同，且三点在同一直线上。
[0106]
所述控制点的算法生成方法为：
[0107]
1)检查当前节点是否是第一个全局路径点，如果是，则用下一全局路径点减当前全局路径点的差值作为切矢量；
[0108]
2)检查当前节点是否是最后一个全局路径点，如果是，则用当前全局路径点减前一全局路径点的差值作为切矢量；
[0109]
3)如果当前节点介于起始路径点与终止路径点之间，则用上一路径点减前一路径点的半差值作为切矢量；
[0110]
4)用当前节点坐标加减切矢量生成两个控制点。
[0111]
实施例中切矢量的长度是0.25米，但不限于0.25米，根据具体工况确定。通过这种方式生成的局部路径点是光滑的，不会产生扭结或锯齿，如图5和图6所示。
[0112]
在步骤(4)中：
[0113]
所述的旅行商算法(tsp)给出了单元内n条作业线的切换顺序，使得车辆的行驶路程局部最优。在tsp算法中，把间距小于2*r_min的作业线当作无穷大距离，这样生成的路线便符合阿克曼转向原理，此时前轮转角的变化率光滑，并且在不另加约束条件的情况下可以自动限制最大前轮转角在合理的范围内；作业线的切换顺序确定后，初始化一个标识作业线位置的指标index_rank，用来标识当前车辆所在作业线。
[0114]
上述所有指标在车辆跟踪路径过程中实时更新。
[0115]
所述的路径更新，具体为：主循环开始时，判定当前车辆状态下需不需要更新单元或作业线单元，如果需要，则更新单元index_block＝index_block+1，或更新作业线index_rank＝index_rank+1；在更新作业线时重置全局路径点指标index_global。
[0116]
当车辆在作业线上，给全局路径点一个标签(例如采用标签0)；当车辆在转向路径上，给全局路径点另一个标签(例如采用标签1)；更新全局路径时，通过判定当前标签来判定下一条全局路径的类型；如果当前全局路径是作业线，则下一条全局路径更新为转向路径；如果当前全局路径是转向路径，则下一条全局路径更新为作业线。
[0117]
查询距离当前车辆状态最近的全局路径点index_transition，如果当前全局路径点发生变化index_global！＝index_transition，则更新局部路径；更新后查询距离当前车辆状态最近的局部路径点index_local，将这一点作为路径起点，设置合适的预瞄距离，在局部路径上对路径进行积分，得到预瞄点,并通过纯跟踪控制跟踪预瞄点，通过纯跟踪控制输出的前轮转角对车辆进行横向控制，完成车辆状态更新。
[0118]
本发明实施例中作业地块分的单元和全局路径点的示意图如图7和图8所示。
[0119]
本发明实施例中所述农机自动作业的路径规划方法的流程示意图如图9所示，具体作业逻辑流程步骤如下：
[0120]
1)初始化车辆位置，并且初始化全局路径，选择初始化全局路径类型为作业线；
[0121]
2)通过对距离车辆位置最近的全局路径点靠近全局路径末尾进行判断，
[0122]
2.1)当距离车辆位置最近的全局路径点未靠近全局路径末尾，查询距离当前车辆位置最近的全局路径点，在查询到的全局路径点附近用b样条生成局部路径点，查询距离当前车辆位置最近的局部路径点，轨迹跟踪控制器对车辆状态进行更新，重新进行步骤2)的判断；
[0123]
2.2)当距离车辆位置最近的全局路径点靠近全局路径末尾，判断当前路径类型；
[0124]
2.2.1)判断当前路径类型为转向路径时，生成作业线作为全局路径，切换全局路径类型为作业线；查询距离当前车辆位置最近的全局路径点；
[0125]
2.2.2)判断当前路径类型为作业线时，判断当前单元是否是最后一个单元的最后一条作业线；
[0126]
2.2.2.1)判断当前单元是最后一个单元的最后一条作业线，结束跟踪；
[0127]
2.2.2.2)判断当前单元不是最后一个单元的最后一条作业线，判断当前作业线是否是当前单元的最后一条作业线；
[0128]
2.2.2.2.1)判断当前作业线是当前单元的最后一条作业线；更新重复单元；
[0129]
2.2.2.2.2)判断当前作业线不是当前单元的最后一条作业线，生成转向路径作为全局路径，切换全局路径类型为转向路径。
[0130]
以上所述仅为本发明的优选实施例，并非因此限制本发明的专利范围，凡是在本发明的发明构思下，利用本发明说明书及附图内容所作的等效结构变换，或直接/间接运用在其他相关的技术领域均包括在本发明的专利保护范围内。