CN116382306A - 全覆盖作业农机的轨迹跟踪控制方法、装置、设备及介质 - Google Patents
全覆盖作业农机的轨迹跟踪控制方法、装置、设备及介质 Download PDFInfo
- Publication number
- CN116382306A CN116382306A CN202310653090.3A CN202310653090A CN116382306A CN 116382306 A CN116382306 A CN 116382306A CN 202310653090 A CN202310653090 A CN 202310653090A CN 116382306 A CN116382306 A CN 116382306A
- Authority
- CN
- China
- Prior art keywords
- agricultural machine
- path
- point
- full
- coverage
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Granted
Links
- 238000000034 method Methods 0.000 title claims abstract description 34
- 238000003860 storage Methods 0.000 claims abstract description 21
- 230000001419 dependent effect Effects 0.000 claims description 28
- 238000004590 computer program Methods 0.000 claims description 21
- 238000005070 sampling Methods 0.000 claims description 21
- 238000005452 bending Methods 0.000 claims description 14
- 230000000295 complement effect Effects 0.000 claims description 10
- 230000003044 adaptive effect Effects 0.000 claims description 3
- 238000012163 sequencing technique Methods 0.000 claims description 3
- WYTGDNHDOZPMIW-RCBQFDQVSA-N alstonine Natural products C1=CC2=C3C=CC=CC3=NC2=C2N1C[C@H]1[C@H](C)OC=C(C(=O)OC)[C@H]1C2 WYTGDNHDOZPMIW-RCBQFDQVSA-N 0.000 description 7
- 238000004422 calculation algorithm Methods 0.000 description 6
- 230000006870 function Effects 0.000 description 6
- 230000004044 response Effects 0.000 description 4
- 238000001514 detection method Methods 0.000 description 3
- 238000012545 processing Methods 0.000 description 3
- 238000003491 array Methods 0.000 description 2
- 238000004891 communication Methods 0.000 description 2
- 238000010586 diagram Methods 0.000 description 2
- 206010063385 Intellectualisation Diseases 0.000 description 1
- 238000012271 agricultural production Methods 0.000 description 1
- 238000011166 aliquoting Methods 0.000 description 1
- 238000004458 analytical method Methods 0.000 description 1
- 238000004364 calculation method Methods 0.000 description 1
- 230000001413 cellular effect Effects 0.000 description 1
- 238000006243 chemical reaction Methods 0.000 description 1
- 238000011161 development Methods 0.000 description 1
- 238000005516 engineering process Methods 0.000 description 1
- 238000009313 farming Methods 0.000 description 1
- 238000003306 harvesting Methods 0.000 description 1
- 230000006872 improvement Effects 0.000 description 1
- 230000003287 optical effect Effects 0.000 description 1
- 230000008569 process Effects 0.000 description 1
- 239000000047 product Substances 0.000 description 1
- 238000006467 substitution reaction Methods 0.000 description 1
- 239000013589 supplement Substances 0.000 description 1
- 230000009466 transformation Effects 0.000 description 1
Images
Classifications
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
- G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
- G05D1/02—Control of position or course in two dimensions
- G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
- G05D1/0212—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
- G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
- G05D1/02—Control of position or course in two dimensions
- G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
- G05D1/0268—Control of position or course in two dimensions specially adapted to land vehicles using internal positioning means
- G05D1/0274—Control of position or course in two dimensions specially adapted to land vehicles using internal positioning means using mapping information stored in a memory device
-
- Y—GENERAL TAGGING OF NEW TECHNOLOGICAL DEVELOPMENTS; GENERAL TAGGING OF CROSS-SECTIONAL TECHNOLOGIES SPANNING OVER SEVERAL SECTIONS OF THE IPC; TECHNICAL SUBJECTS COVERED BY FORMER USPC CROSS-REFERENCE ART COLLECTIONS [XRACs] AND DIGESTS
- Y02—TECHNOLOGIES OR APPLICATIONS FOR MITIGATION OR ADAPTATION AGAINST CLIMATE CHANGE
- Y02A—TECHNOLOGIES FOR ADAPTATION TO CLIMATE CHANGE
- Y02A40/00—Adaptation technologies in agriculture, forestry, livestock or agroalimentary production
- Y02A40/10—Adaptation technologies in agriculture, forestry, livestock or agroalimentary production in agriculture
Landscapes
- Engineering & Computer Science (AREA)
- Radar, Positioning & Navigation (AREA)
- Aviation & Aerospace Engineering (AREA)
- Remote Sensing (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Automation & Control Theory (AREA)
- Management, Administration, Business Operations System, And Electronic Commerce (AREA)
- Harvester Elements (AREA)
- Lifting Devices For Agricultural Implements (AREA)
Abstract
本发明提供了一种全覆盖作业农机的轨迹跟踪控制方法、装置、设备及可读存储介质，先通过获取农机作业区域的栅格地图，并基于栅格地图上的多个关键角点生成拓扑地图，接着，获取农机参数根据所述农机参数和所述拓扑地图进行自适应全覆盖路径规划，以生成期望路径，最后，获取农机位姿信息，根据农机位姿信息与全覆盖路径点集，控制所述农机在期望路径上运行，解决了现有的轨迹跟踪算法可能造成作业覆盖的重叠和遗漏的问题。
Description
技术领域
本发明涉农业机械自动化领域，特别涉及一种全覆盖作业农机的轨迹跟踪控制方法、装置、设备及介质。
背景技术
随着自动驾驶技术的发展以及人工成本的不断提升，农业机械自动化和智能化已成为现代农业生产的必然趋势，目前有很多地区在农收或农耕时期为了提高效率都采用自动驾驶的智慧农机进行作物的收种，相比于传统的人力作业，无人化农场的出现不仅大幅降低人力成本，同时也提升了作业效率。
对于全自动驾驶农机，路径规划与轨迹跟踪模块尤为重要。全覆盖作业农机的路径规划与轨迹跟踪问题，就是要在其作业的农田区域中，规划出一条能够遍历整个工作区域且没有碰撞的一条安全行驶路径，然后控制农机底层驱动进行跟踪作业。目前，现有的全覆盖路径跟踪算法往往难以适配不同车型与实际工况，无法根据农机实际的作业模式与需求规划出最优全覆盖路径，且现有的轨迹跟踪方法通常并不适用于跟踪全覆盖路径，无法完整准确跟踪蛇形或螺旋形作业路径，造成作业覆盖的重叠和遗漏。
有鉴于此，提出本申请。
发明内容
本发明公开了一种全覆盖作业农机的轨迹跟踪控制方法、装置、设备及介质，旨在解决现有的轨迹跟踪算法可能造成作业覆盖的重叠和遗漏的问题。
本发明第一实施例提供了一种全覆盖作业农机的轨迹跟踪控制方法，包括：
获取农机作业区域的栅格地图，并根据所述栅格地图边缘上的多个关键角点生成拓扑地图，其中，拓扑地图包括内部拓扑地图和外部拓扑地图；
获取农机参数，根据所述农机参数和所述拓扑地图进行自适应全覆盖路径规划，以生成期望路径，其中，农机参数包括农机搭载的农具宽度、农机的转弯半径、农机的作业模式；
获取农机位姿信息，根据农机位姿信息与全覆盖路径点集，控制所述农机在期望路径上运行。
优选地，所述获取农机位姿信息，根据农机位姿信息与全覆盖路径点集，控制所述农机在期望路径上运行，具体为：
通过GNSS获取农机的当前位姿信息，比较所述当前位姿信息和全覆盖路径点集的位姿信息对比，生成比较信息，其中，所述农机位姿信息包括农机在参考坐标系内坐标以及农机当前航向角；
根据所述比较信息判断到农机当前坐标不与预测工作点坐标重合，则调整农机航向角，转向预测工作点后，以固定步进向预测工作点行驶，行驶至预测工作点坐标后，调整农机航向角，转向预测航向角；
根据所述比较信息判断到农机当前坐标与预测工作点坐标重合，则检测农机当前航向角是否为预测航向角，若与预测航向角一致，则向下一个预测工作点行驶；若与预测航向角不一致，则调整农机航向角后，向下一个预测工作点行驶；检测下一个预测工作点是否为路径终点，若为路径终点，则行驶至终点后停止作业。
优选地，所述农机的作业模式包括平行式路径模式、蛇形路径模式、以及螺旋形路径模式。
优选地，所述期望路径包括平行直线路径与将平行直线路径首尾相连的转弯路径，其中，所述转弯路径为Reeds_shepp曲线，由半径固定的圆弧和直线段拼接而成。
优选地，所述获取农机作业区域的栅格地图，并根据所述栅格地图边缘上的多个关键角点生成拓扑地图，具体为：
获取农机作业区域栅格地图，取所述栅格地图边缘上数个关键角点的坐标，并存储为边缘关键角点链表形式；
在关键角点之间以一定步长采样，生成补点填充边缘线，存储于链表，形成的拓扑地图。
优选地，所述获取农机参数，根据所述农机参数和所述拓扑地图进行自适应全覆盖路径规划，以生成期望路径，具体为：
生成路径的平行依赖线簇，并以经过给定起始工作点坐标的为依赖线簇母线，以所述依赖线簇母线与不同边缘线段有交时的斜率值范围作为划分，形成斜率基数链表；
在所述拓扑地图的边缘线内部生成与边缘线平行的转弯区域，根据所述斜率基数链表，确定最小转弯区域宽度；
根据所述拓扑地图的边缘线和最小转弯区域宽度，生成与边缘线平行的内部拓扑地图的边框线，取其交点作为内部拓扑地图的关键角点，在相邻关系的关键角点之间以一定步长采样，获得补点以填充内部边框线生成内部拓扑地图；
从所述依赖线簇母线开始循环求解平行依赖线与内部边框线交点，若交点存在，则继续向母线两侧延伸循环求解；若交点不存在，则结束循环遍历，根据覆盖作业方向沿平行依赖线簇垂直方向对交点进行排序，并根据所述农机的作业模式确定进弯点与出弯点，
根据所述进弯点和所述出弯点，生成转弯路径，将平行直线路径起点与终点相连接，生成期望路径。
优选地，所述根据所述进弯点和所述出弯点，生成转弯路径，将平行直线路径起点与终点相连接，生成期望路径，具体为：
根据所述进弯点和所述出弯点在所述外部拓扑地图转弯所需Reeds_shepp曲线，并以连接两条平行直线路径；
对所述Reeds_shepp曲线进行定距离离散化采样，生成转弯路径点，对平行直线路径进行等步长采样，在完整覆盖路径点集后生成期望路径。
本发明第二实施例提供了一种全覆盖作业农机的轨迹跟踪控制装置，包括：
拓扑地图生成单元，用于获取农机作业区域的栅格地图，并根据所述栅格地图边缘上的多个关键角点生成拓扑地图，其中，拓扑地图包括内部拓扑地图和外部拓扑地图；
期望路径生成单元，用于获取农机参数，根据所述农机参数和所述拓扑地图进行自适应全覆盖路径规划，以生成期望路径，其中，农机参数包括农机搭载的农具宽度、农机的转弯半径、农机的作业模式；
控制单元，用于获取农机位姿信息，根据农机位姿信息与全覆盖路径点集，控制所述农机在期望路径上运行。
本发明第三实施例提供了一种全覆盖作业农机的轨迹跟踪控制设备，其特征在于，包括存储器以及处理器，所述存储器内存储有计算机程序，所述计算机程序能够被所述处理器执行，以实现如上任意一项所述的一种全覆盖作业农机的轨迹跟踪控制方法。
本发明第四实施例提供了一种计算机可读存储介质，其特征在于，存储有计算机程序，所述计算机程序能够被所述计算机可读存储介质所在设备的处理器执行，以实现如上任意一项所述的一种全覆盖作业农机的轨迹跟踪控制方法。
基于本发明提供的一种全覆盖作业农机的轨迹跟踪控制方法、装置、设备及可读存储介质，先通过获取农机作业区域的栅格地图，并基于栅格地图上的多个关键角点生成拓扑地图，接着，获取农机参数根据所述农机参数和所述拓扑地图进行自适应全覆盖路径规划，以生成期望路径，最后，获取农机位姿信息，根据农机位姿信息与全覆盖路径点集，控制所述农机在期望路径上运行，解决了现有的轨迹跟踪算法可能造成作业覆盖的重叠和遗漏的问题。
附图说明
图1是本发明第一实施例提供的一种全覆盖作业农机的轨迹跟踪控制方法的流程示意图；
图2是本发明提供的自适应全覆盖路径规划方法示意图；
图3是本发明第二实施例提供的一种全覆盖作业农机的轨迹跟踪控制装置的模块示意图。
具体实施方式
下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。
为了更好的理解本发明的技术方案，下面结合附图对本发明实施例进行详细描述。
应当明确，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有作出创造性劳动前提下所获得的所有其它实施例，都属于本发明保护的范围。
在本发明实施例中使用的术语是仅仅出于描述特定实施例的目的，而非旨在限制本发明。在本发明实施例和所附权利要求书中所使用的单数形式的“一种”、“所述”和“该”也旨在包括多数形式，除非上下文清楚地表示其他含义。
应当理解，本文中使用的术语“和/或”仅仅是一种描述关联对象的关联关系，表示可以存在三种关系，例如，A和/或B，可以表示：单独存在A，同时存在A和B，单独存在B这三种情况。另外，本文中字符“/”，一般表示前后关联对象是一种“或”的关系。
取决于语境，如在此所使用的词语“如果”可以被解释成为“在……时”或“当……时”或“响应于确定”或“响应于检测”。类似地，取决于语境，短语“如果确定”或“如果检测（陈述的条件或事件）”可以被解释成为“当确定时”或“响应于确定”或“当检测（陈述的条件或事件）时”或“响应于检测（陈述的条件或事件）”。
实施例中提及的“第一\第二”仅仅是是区别类似的对象，不代表针对对象的特定排序，可以理解地，“第一\第二”在允许的情况下可以互换特定的顺序或先后次序。应该理解“第一\第二”区分的对象在适当情况下可以互换，以使这里描述的实施例能够以除了在这里图示或描述的那些以外的顺序实施。
以下结合附图对本发明的具体实施例做详细说明。
本发明公开了一种全覆盖作业农机的轨迹跟踪控制方法、装置、设备及介质，旨在解决现有的轨迹跟踪算法可能造成作业覆盖的重叠和遗漏的问题。
本发明第一实施例提供了一种全覆盖作业农机的轨迹跟踪控制方法，其可由全覆盖作业农机的轨迹跟踪控制设备（以下简称控制设备）来执行，特别的，由控制设备内的一个或者多个处理器来执行，以至少实现如下步骤：
S101，获取农机作业区域的栅格地图，并根据所述栅格地图边缘上的多个关键角点生成拓扑地图，其中，拓扑地图包括内部拓扑地图和外部拓扑地图；
在本实施例中，所述控制设备可为台式电脑、笔记本电脑、服务器、工作站等具有数据处理分析能力的终端，其中，所述控制设备内可安装有相应的操作系统以及应用软件，并通过操作系统以及应用软件的结合来实现本实施例所需的功能。
具体地，在本实施例中，获取农机作业区域栅格地图，取所述栅格地图边缘上数个关键角点的坐标，并存储为边缘关键角点链表形式
；由相邻关系形成拓扑地图，在关键角点之间以一定步长采样，生成补点填充边缘线，存储于链表/>
，形成的拓扑地图。
S102，获取农机参数，根据所述农机参数和所述拓扑地图进行自适应全覆盖路径规划（如图2所示），以生成期望路径，其中，农机参数包括但不仅限于农机搭载的农具宽度、农机的转弯半径、农机的作业模式，其中，所述农机的作业模式包括但不仅限于平行式路径模式、蛇形路径模式、以及螺旋形路径模式；
在本实施例中，平行式运行模式可以为沿着工作方向按顺序遍历每一条平行直线路径；
蛇形运行模式可以为每次都跳过一条平行直线路径，直至沿工作方向穿过地图后，再通过未覆盖的路径返回，以覆盖全部路径；
螺旋形运行模式可以为蛇形运行模式的变体，每次跳过固定条平行直线路径。
具体地，在本实施例中，所述期望路径包括平行直线路径与将平行直线路径首尾相连的转弯路径，其中，所述转弯路径为Reeds_shepp曲线，由半径固定的圆弧和直线段拼接而成。
进一步地，生成路径的平行依赖线簇，并以经过给定起始工作点坐标的为依赖线簇母线，以所述依赖线簇母线与不同边缘线段有交时的斜率值范围作为划分，形成斜率基数链表；
在所述拓扑地图的边缘线内部生成与边缘线平行的转弯区域，根据所述斜率基数链表，确定最小转弯区域宽度；
根据所述拓扑地图的边缘线和最小转弯区域宽度，生成与边缘线平行的内部拓扑地图的边框线，取其交点作为内部拓扑地图的关键角点，在相邻关系的关键角点之间以一定步长采样，获得补点以填充内部边框线生成内部拓扑地图；
从所述依赖线簇母线开始循环求解平行依赖线与内部边框线交点，若交点存在，则继续向母线两侧延伸循环求解；若交点不存在，则结束循环遍历，根据覆盖作业方向沿平行依赖线簇垂直方向对交点进行排序，并根据所述农机的作业模式确定进弯点与出弯点，
根据所述进弯点和所述出弯点在所述外部拓扑地图转弯所需Reeds_shepp曲线，并以连接两条平行直线路径；
对所述Reeds_shepp曲线进行定距离离散化采样，生成转弯路径点，对平行直线路径进行等步长采样，在完整覆盖路径点集后生成期望路径
更具体地：
S1021，生成路径的平行依赖线簇：
其中，
代表第/>
条路径依赖线，/>
为全局坐标斜率，/>
为全局坐标偏置量，/>
为依赖线簇间距；
对依赖线簇相对坐标系的倾角以0.1°细分，将0到180度分为1800份，建立数组
，先设定以最少路径数为最优，则路径计数数组
的第i项可由如下方式求得，由于/>
，带入依赖线簇方程，与边界线段联立解得区域边界交点数n，则/>
，当结果不唯一时，加入路径覆盖面积最大为第二参考，若结果仍不唯一，则在前两条基础上取路径总长度最短为最优；
为简化后续计算过程，先根据工作区中心间距
确定/>
与/>
的关系。由于工作区中心轴线为平行关系，且相邻两工作区间距/>
与平行依赖线簇的相邻两线垂直距离相等，故有：
由上可得：
由于依赖线簇中必有一条经过给定起始工作点坐标
，令其为依赖线簇母线，可得其方程：
由上可得，依赖线簇方程简化为：
再根据所述拓扑地图的边缘关键点链表
，两两相连形成n条边缘，记为第j个边缘线段参数/>
，存入边缘数组
。
遍历边缘数组，以母线与不同边缘线段有交时的k值范围作为划分，形成斜率基数链表
，在斜率基数链表中，对每个斜率阶段分别计算，每条边缘线段，从依赖线簇母线开始向两侧延展计算，求交点坐标，直到依赖线与边缘线段无交结束,此时，第p条线段对应的交点结果为/>
。
S1022，在拓扑地图边缘线内部生成与边缘线平行的转弯区域，在此区域中，农机在完成直线路径作业后转弯进入下一条直线路径，为使转弯区域面积占总作业区域面积最小，覆盖作业面积最大化，使用自适应算法，根据农机宽度
以及农机转弯半径R，针对平行线簇所依赖的边缘线，动态生成转弯区域，以确定最小转弯区域宽度，首先，根据农机转弯运动学特性，可知其转弯半径最小值为其宽度的一半，由此可得：
其转弯半径最大值为：
求取当前平行线簇与当前依赖边缘线的夹角
，将/>
与/>
之间的差值线性映射到其夹角/>
，得到差值与夹角间的线性关系/>
：
由此可得，当前依赖边缘线所对应的最小转弯区域宽度：
S1023，根据农机作业区域拓扑地图边缘线及其对应的最小转弯区域宽度，得到分别与边缘线平行的内部拓扑地图边框线，取其交点作为内部拓扑地图的关键角点，存储为内部边缘角点链表形式
，由相邻关系在内部关键角点之间以一定步长采样，获得补点补充填充内部边框线，生成内部拓扑地图，存储于内部边框列表/>
；
S1024从所述母线开始循环求解平行依赖线与内部边框线交点，若交点存在，则继续向母线两侧延伸循环求解，并将交点存入链表
；若交点不存在，则结束循环遍历，根据覆盖作业方向沿平行依赖线簇垂直方向对交点进行排序，由用户输入确定运行模式，可选择平行式、蛇形或螺旋形路径，由此确定进弯点与出弯点，并将平行直线路径起点与终点相连接；其中，平行式运行模式为为沿着工作方向按顺序遍历每一条平行直线路径；蛇形运行模式为每次都跳过一条平行直线路径，直至沿工作方向穿过地图后，再通过未覆盖的路径返回，以覆盖全部路径；螺旋形运行模式为蛇形运行模式的变体，每次跳过固定条平行直线路径。
S1025，根据所述平行直线路径及进弯点与出弯点，在转弯区域中，生成Reeds_shepp曲线，以连接两条平行直线路径，所述Reeds_shepp曲线由半径固定的圆弧和直线段拼接而成，其中，固定半径指农机转弯半径R，令农机当前行驶所在的平行直线路径为直线路径1，直线路径1的起点为点1
，终点为点2/>
，其中点2为出弯点，目标下一条平行直线路径为直线路径2，其起点为点3/>
，终点为点4/>
，其中点3为进弯点，即Reeds_shepp曲线起点为点2，终点为点3；
根据k、b、d可知，农机在点2处的出射角度为：
过点2作相切于直线路径1且半径为R的圆：由斜率k与点2坐标，求得与直线路径1垂直且经过点2的直线公式：
由于圆心在
上且距离点2有R的距离，可求得圆心坐标/>
，根据圆心坐标以及圆的半径R，可求得圆的方程，过点3作与圆相切的直线方程，求得圆上切点q坐标
，与点3相连为直线段，即可获得转弯所需Reeds_shepp曲线；
S1026，在生成的Reeds_shepp曲线进行定距离离散化采样，生成转弯路径点，生成转弯路径点，取从点2到圆上切点q
之间的圆弧，用极坐标表示：
其中，
为圆心和点2的连线与/>
轴的夹角，/>
为圆心和切点的连线与/>
轴的夹角，离散采样时以固定步长采样，可以解决大转弯半径下路径点稀疏的问题，假设固定步长为s，由于弧长为：
则采样点的数量应为：
对
到/>
之间角度进行n等分，可求得角度步长：
将等分角度存入角采样序列：
对极坐标进行坐标变换，将角采样序列转换为全局直角坐标系下的路径点：
对点3与圆上切点q之间的直线段进行定步长采样，由线段总长l与步距s算得采样点数：
则此直线段路径点：
S1027，对平行直线路径进行等步长采样，对点1与点2之间的直线路径1、点3与点4之间的直线路径2分别进行离散采样，获得路径点集
与/>
，至此完整全覆盖路径点集已全部获取。
S103，获取农机位姿信息，根据农机位姿信息与全覆盖路径点集，控制所述农机在期望路径上运行。
具体地，在本实施例中：
通过GNSS获取农机的当前位姿信息，比较所述当前位姿信息和全覆盖路径点集的位姿信息对比，生成比较信息，其中，所述农机位姿信息包括农机在参考坐标系内坐标以及农机当前航向角；
根据所述比较信息判断到农机当前坐标不与预测工作点坐标重合，则调整农机航向角，转向预测工作点后，以固定步进向预测工作点行驶，行驶至预测工作点坐标后，调整农机航向角，转向预测航向角；
根据所述比较信息判断到农机当前坐标与预测工作点坐标重合，则检测农机当前航向角是否为预测航向角，若与预测航向角一致，则向下一个预测工作点行驶；若与预测航向角不一致，则调整农机航向角后，向下一个预测工作点行驶；检测下一个预测工作点是否为路径终点，若为路径终点，则行驶至终点后停止作业。
需要说明的是，其主要包括以下三个模块：
1)在轨检测模块：通过GNSS获取位姿，跟预测工作状态相比较，用来检测当前坐标是否在最近预测工作点，若农机正常工作在作业路径，则坐标应与预测工作点相重合，执行在轨运行模块；若两坐标相差较大，则认为农机脱轨，未按照预定结果运行，需要执行脱轨复位模块；
2)脱轨复位模块：将当前位姿与目标位姿做对比，在存在距离误差时，计算当前农机航向角与位置误差连线的差角，调整航向角，驶向最近预测工作点；当不存在距离误差时，计算农机的航向角与目标姿态航向角的差值，调整航向角，驶向预测工作点，未达到目标位姿前，循环调用直到归位；
3)在轨运行模块：隔一点读取后续路径点，取3个后续路径点作为预选点作转弯圆，令当前工作点为k，则取k+2、k+4，k+6点作为预选点，判断预选点是否存在路径终点，求其三角形外接圆半径，判断半径是否在可运行阈值内，当半径大于500米时都认定后几米是直线，当半径小于最小转弯半径时要让运行半径等于最小转弯半径，根据半径的大小来调节农机的速度与航向角，对农机进行横向与纵向控制，判断预选点是否存在路径终点，若存在路径终点，则行驶至路径终点后停止作业。
本发明第二实施例提供了一种全覆盖作业农机的轨迹跟踪控制装置，包括：
拓扑地图生成单元，用于获取农机作业区域的栅格地图，并根据所述栅格地图边缘上的多个关键角点生成拓扑地图，其中，拓扑地图包括内部拓扑地图和外部拓扑地图；
期望路径生成单元，用于获取农机参数，根据所述农机参数和所述拓扑地图进行自适应全覆盖路径规划，以生成期望路径，其中，农机参数包括农机搭载的农具宽度、农机的转弯半径、农机的作业模式；
控制单元，用于获取农机位姿信息，根据农机位姿信息与全覆盖路径点集，控制所述农机在期望路径上运行。
本发明第三实施例提供了一种全覆盖作业农机的轨迹跟踪控制设备，其特征在于，包括存储器以及处理器，所述存储器内存储有计算机程序，所述计算机程序能够被所述处理器执行，以实现如上任意一项所述的一种全覆盖作业农机的轨迹跟踪控制方法。
本发明第四实施例提供了一种计算机可读存储介质，其特征在于，存储有计算机程序，所述计算机程序能够被所述计算机可读存储介质所在设备的处理器执行，以实现如上任意一项所述的一种全覆盖作业农机的轨迹跟踪控制方法。
基于本发明提供的一种全覆盖作业农机的轨迹跟踪控制方法、装置、设备及可读存储介质，先通过获取农机作业区域的栅格地图，并基于栅格地图上的多个关键角点生成拓扑地图，接着，获取农机参数根据所述农机参数和所述拓扑地图进行自适应全覆盖路径规划，以生成期望路径，最后，获取农机位姿信息，根据农机位姿信息与全覆盖路径点集，控制所述农机在期望路径上运行，解决了现有的轨迹跟踪算法可能造成作业覆盖的重叠和遗漏的问题。
示例性地，本发明第三实施例和第四实施例中所述的计算机程序可以被分割成一个或多个模块，所述一个或者多个模块被存储在所述存储器中，并由所述处理器执行，以完成本发明。所述一个或多个模块可以是能够完成特定功能的一系列计算机程序指令段，该指令段用于描述所述计算机程序在所述实现一种全覆盖作业农机的轨迹跟踪控制设备中的执行过程。例如，本发明第二实施例中所述的装置。
所称处理器可以是中央处理单元(Central Processing Unit，CPU)，还可以是其他通用处理器、数字信号处理器 (Digital Signal Processor，DSP)、专用集成电路(Application Specific Integrated Circuit，ASIC)、现成可编程门阵列 (Field-Programmable Gate Array，FPGA)或者其他可编程逻辑器件、分立门或者晶体管逻辑器件、分立硬件组件等。通用处理器可以是微处理器或者该处理器也可以是任何常规的处理器等，所述处理器是所述一种全覆盖作业农机的轨迹跟踪控制方法的控制中心，利用各种接口和线路连接整个所述实现一种全覆盖作业农机的轨迹跟踪控制方法的各个部分。
所述存储器可用于存储所述计算机程序和/或模块，所述处理器通过运行或执行存储在所述存储器内的计算机程序和/或模块，以及调用存储在存储器内的数据，实现一种全覆盖作业农机的轨迹跟踪控制方法的各种功能。所述存储器可主要包括存储程序区和存储数据区，其中，存储程序区可存储操作系统、至少一个功能所需的应用程序（比如声音播放功能、文字转换功能等）等；存储数据区可存储根据手机的使用所创建的数据（比如音频数据、文字消息数据等）等。此外，存储器可以包括高速随机存取存储器，还可以包括非易失性存储器，例如硬盘、内存、插接式硬盘、智能存储卡（Smart Media Card, SMC）、安全数字（Secure Digital, SD）卡、闪存卡（Flash Card）、至少一个磁盘存储器件、闪存器件、或其他易失性固态存储器件。
其中，所述实现的模块如果以软件功能单元的形式实现并作为独立的产品销售或使用时，可以存储在一个计算机可读取存储介质中。基于这样的理解，本发明实现上述实施例方法中的全部或部分流程，也可以通过计算机程序来指令相关的硬件来完成，所述的计算机程序可存储于一个计算机可读存储介质中，该计算机程序在被处理器执行时，可实现上述各个方法实施例的步骤。其中，所述计算机程序包括计算机程序代码，所述计算机程序代码可以为源代码形式、对象代码形式、可执行文件或某些中间形式等。所述计算机可读介质可以包括：能够携带所述计算机程序代码的任何实体或装置、记录介质、U盘、移动硬盘、磁碟、光盘、计算机存储器、只读存储器（ROM，Read-Only Memory）、随机存取存储器（RAM，Random Access Memory）、电载波信号、电信信号以及软件分发介质等。需要说明的是，所述计算机可读介质包含的内容可以根据司法管辖区内立法和专利实践的要求进行适当的增减，例如在某些司法管辖区，根据立法和专利实践，计算机可读介质不包括电载波信号和电信信号。
需说明的是，以上所描述的装置实施例仅仅是示意性的，其中所述作为分离部件说明的单元可以是或者也可以不是物理上分开的，作为单元显示的部件可以是或者也可以不是物理单元，即可以位于一个地方，或者也可以分布到多个网络单元上。可以根据实际的需要选择其中的部分或者全部模块来实现本实施例方案的目的。另外，本发明提供的装置实施例附图中，模块之间的连接关系表示它们之间具有通信连接，具体可以实现为一条或多条通信总线或信号线。本领域普通技术人员在不付出创造性劳动的情况下，即可以理解并实施。
以上所述，仅为本发明较佳的具体实施方式，但本发明的保护范围并不局限于此，任何熟悉本技术领域的技术人员在本发明揭露的技术范围内，可轻易想到的变化或替换，都应涵盖在本发明的保护范围之内。因此，本发明的保护范围应该以权利要求的保护范围为准。
Claims (10)
1.一种全覆盖作业农机的轨迹跟踪控制方法，其特征在于，包括：
获取农机作业区域的栅格地图，并根据所述栅格地图边缘上的多个关键角点生成拓扑地图，其中，拓扑地图包括内部拓扑地图和外部拓扑地图；
获取农机参数，根据所述农机参数和所述拓扑地图进行自适应全覆盖路径规划，以生成期望路径，其中，农机参数包括农机搭载的农具宽度、农机的转弯半径、农机的作业模式；
获取农机位姿信息，根据农机位姿信息与全覆盖路径点集，控制所述农机在期望路径上运行。
2.根据权利要求1所述的一种全覆盖作业农机的轨迹跟踪控制方法，其特征在于，所述获取农机位姿信息，根据农机位姿信息与全覆盖路径点集，控制所述农机在期望路径上运行，具体为：
通过GNSS获取农机的当前位姿信息，比较所述当前位姿信息和全覆盖路径点集的位姿信息对比，生成比较信息，其中，所述农机位姿信息包括农机在参考坐标系内坐标以及农机当前航向角；
根据所述比较信息判断到农机当前坐标不与预测工作点坐标重合，则调整农机航向角，转向预测工作点后，以固定步进向预测工作点行驶，行驶至预测工作点坐标后，调整农机航向角，转向预测航向角；
根据所述比较信息判断到农机当前坐标与预测工作点坐标重合，则检测农机当前航向角是否为预测航向角，若与预测航向角一致，则向下一个预测工作点行驶；若与预测航向角不一致，则调整农机航向角后，向下一个预测工作点行驶；检测下一个预测工作点是否为路径终点，若为路径终点，则行驶至终点后停止作业。
3.根据权利要求1所述的一种全覆盖作业农机的轨迹跟踪控制方法，其特征在于，所述农机的作业模式包括平行式路径模式、蛇形路径模式、以及螺旋形路径模式。
4.根据权利要求1所述的一种全覆盖作业农机的轨迹跟踪控制方法，其特征在于，所述期望路径包括平行直线路径与将平行直线路径首尾相连的转弯路径，其中，所述转弯路径为Reeds_shepp曲线，由半径固定的圆弧和直线段拼接而成。
5.根据权利要求1所述的一种全覆盖作业农机的轨迹跟踪控制方法，其特征在于，所述获取农机作业区域的栅格地图，并根据所述栅格地图边缘上的多个关键角点生成拓扑地图，具体为：
获取农机作业区域栅格地图，取所述栅格地图边缘上数个关键角点的坐标，并存储为边缘关键角点链表形式；
在关键角点之间以一定步长采样，生成补点填充边缘线，存储于链表，形成的拓扑地图。
6.根据权利要求1所述的一种全覆盖作业农机的轨迹跟踪控制方法，其特征在于，所述获取农机参数，根据所述农机参数和所述拓扑地图进行自适应全覆盖路径规划，以生成期望路径，具体为：
生成路径的平行依赖线簇，并以经过给定起始工作点坐标的为依赖线簇母线，以所述依赖线簇母线与不同边缘线段有交点时的斜率值范围作为划分，形成斜率基数链表；
在所述拓扑地图的边缘线内部生成与边缘线平行的转弯区域，根据所述斜率基数链表，确定最小转弯区域宽度；
根据所述拓扑地图的边缘线和最小转弯区域宽度，生成与边缘线平行的内部拓扑地图的边框线，取其交点作为内部拓扑地图的关键角点，在相邻关系的关键角点之间以一定步长采样，获得补点以填充内部边框线生成内部拓扑地图；
从所述依赖线簇母线开始循环求解平行依赖线与内部边框线交点，若交点存在，则继续向母线两侧延伸循环求解；若交点不存在，则结束循环遍历，根据覆盖作业方向沿平行依赖线簇垂直方向对交点进行排序，并根据所述农机的作业模式确定进弯点与出弯点，
根据所述进弯点和所述出弯点，生成转弯路径，将平行直线路径起点与终点相连接，生成期望路径。
7.根据权利要求1所述的一种全覆盖作业农机的轨迹跟踪控制方法，其特征在于，所述根据所述进弯点和所述出弯点，生成转弯路径，将平行直线路径起点与终点相连接，生成期望路径，具体为：
根据所述进弯点和所述出弯点在所述外部拓扑地图转弯所述Reeds_shepp曲线，并以连接两条平行直线路径；
对所述Reeds_shepp曲线进行定距离离散化采样，生成转弯路径点，对平行直线路径进行等步长采样，在完整覆盖路径点集后生成期望路径。
8.一种全覆盖作业农机的轨迹跟踪控制装置，其特征在于，包括：
拓扑地图生成单元，用于获取农机作业区域的栅格地图，并根据所述栅格地图边缘上的多个关键角点生成拓扑地图，其中，拓扑地图包括内部拓扑地图和外部拓扑地图；
期望路径生成单元，用于获取农机参数，根据所述农机参数和所述拓扑地图进行自适应全覆盖路径规划，以生成期望路径，其中，农机参数包括农机搭载的农具宽度、农机的转弯半径、农机的作业模式；
控制单元，用于获取农机位姿信息，根据农机位姿信息与全覆盖路径点集，控制所述农机在期望路径上运行。
9.一种全覆盖作业农机的轨迹跟踪控制设备，其特征在于，包括存储器以及处理器，所述存储器内存储有计算机程序，所述计算机程序能够被所述处理器执行，以实现如权利要求1至7任意一项所述的一种全覆盖作业农机的轨迹跟踪控制方法。
10.一种计算机可读存储介质，其特征在于，存储有计算机程序，所述计算机程序能够被所述计算机可读存储介质所在设备的处理器执行，以实现如权利要求1至7任意一项所述的一种全覆盖作业农机的轨迹跟踪控制方法。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202310653090.3A CN116382306B (zh) | 2023-06-05 | 2023-06-05 | 全覆盖作业农机的轨迹跟踪控制方法、装置、设备及介质 |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202310653090.3A CN116382306B (zh) | 2023-06-05 | 2023-06-05 | 全覆盖作业农机的轨迹跟踪控制方法、装置、设备及介质 |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN116382306A true CN116382306A (zh) | 2023-07-04 |
| CN116382306B CN116382306B (zh) | 2023-09-05 |
Family
ID=86967888
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202310653090.3A Active CN116382306B (zh) | 2023-06-05 | 2023-06-05 | 全覆盖作业农机的轨迹跟踪控制方法、装置、设备及介质 |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN116382306B (zh) |
Cited By (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN117268401A (zh) * | 2023-11-16 | 2023-12-22 | 广东碧然美景观艺术有限公司 | 一种动态围栏的园艺路径生成方法 |
Citations (5)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| US20120194535A1 (en) * | 2011-01-31 | 2012-08-02 | Autodesk, Inc. | Two-dimensional vector fills using topological recipes |
| CN103611646A (zh) * | 2013-12-09 | 2014-03-05 | 江苏科技大学 | 喷涂机器人空间路径规划方法 |
| CN109541634A (zh) * | 2018-12-28 | 2019-03-29 | 歌尔股份有限公司 | 一种路径规划方法、装置和移动设备 |
| CN113534823A (zh) * | 2021-09-16 | 2021-10-22 | 季华实验室 | 种植机器人路径规划方法、装置、电子设备和存储介质 |
| CN115933751A (zh) * | 2023-01-10 | 2023-04-07 | 潍柴动力股份有限公司 | 无人农机的全覆盖路径规划方法、装置及电子设备 |
-
2023
- 2023-06-05 CN CN202310653090.3A patent/CN116382306B/zh active Active
Patent Citations (6)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| US20120194535A1 (en) * | 2011-01-31 | 2012-08-02 | Autodesk, Inc. | Two-dimensional vector fills using topological recipes |
| CN103611646A (zh) * | 2013-12-09 | 2014-03-05 | 江苏科技大学 | 喷涂机器人空间路径规划方法 |
| CN109541634A (zh) * | 2018-12-28 | 2019-03-29 | 歌尔股份有限公司 | 一种路径规划方法、装置和移动设备 |
| US20210333108A1 (en) * | 2018-12-28 | 2021-10-28 | Goertek Inc. | Path Planning Method And Device And Mobile Device |
| CN113534823A (zh) * | 2021-09-16 | 2021-10-22 | 季华实验室 | 种植机器人路径规划方法、装置、电子设备和存储介质 |
| CN115933751A (zh) * | 2023-01-10 | 2023-04-07 | 潍柴动力股份有限公司 | 无人农机的全覆盖路径规划方法、装置及电子设备 |
Non-Patent Citations (1)
| Title |
|---|
| 李彩虹, 李贻斌: "移动机器人运动路径的搜索策略", 计算机应用研究, no. 12 * |
Cited By (2)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN117268401A (zh) * | 2023-11-16 | 2023-12-22 | 广东碧然美景观艺术有限公司 | 一种动态围栏的园艺路径生成方法 |
| CN117268401B (zh) * | 2023-11-16 | 2024-02-20 | 广东碧然美景观艺术有限公司 | 一种动态围栏的园艺路径生成方法 |
Also Published As
| Publication number | Publication date |
|---|---|
| CN116382306B (zh) | 2023-09-05 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| US20200327680A1 (en) | Visual target tracking method and apparatus based on deep adversarial training | |
| CN110316193B (zh) | 预瞄距离的设置方法、装置、设备及计算机可读存储介质 | |
| CN116382306B (zh) | 全覆盖作业农机的轨迹跟踪控制方法、装置、设备及介质 | |
| CN110260867A (zh) | 一种机器人导航中位姿确定、纠正的方法、设备及装置 | |
| US20200201339A1 (en) | Robot movement control method and apparatus and robot using the same | |
| CN110936383A (zh) | 一种机器人的障碍物避让方法、介质、终端和装置 | |
| CN109814575B (zh) | 自动驾驶车辆变道路线规划方法、装置以及终端 | |
| CN109683617B (zh) | 一种自动驾驶方法、装置及电子设备 | |
| KR20200109275A (ko) | 차량 궤적 계획 방법, 장치, 컴퓨터 장치, 컴퓨터 저장 매체 | |
| CN106774393B (zh) | 一种任务进度计算方法、装置及无人机 | |
| CN110389341A (zh) | 充电桩识别方法、装置、机器人及计算机可读存储介质 | |
| CN114063612B (zh) | 一种路径规划方法、路径规划装置及电子设备 | |
| US20210200223A1 (en) | Robotic motion control method and apparatus and robot using the same | |
| CN113954836B (zh) | 分段式导航换道方法及其系统、计算机设备、存储介质 | |
| CN114427866A (zh) | 路径规划方法、电子设备以及存储介质 | |
| CN111824131B (zh) | 一种自动泊车的方法和车辆 | |
| CN112912894A (zh) | 道路边界识别方法和装置 | |
| CN115933751A (zh) | 无人农机的全覆盖路径规划方法、装置及电子设备 | |
| CN111413986A (zh) | 一种基于农机的自动驾驶控制方法及系统 | |
| CN116576859A (zh) | 路径导航方法、作业控制方法及相关装置 | |
| CN116070903A (zh) | 通过障碍物区域的风险确定方法、装置以及电子设备 | |
| CN115837921B (zh) | 车辆轨迹碰撞检测方法、装置、设备及存储介质 | |
| CN114296452B (zh) | 自动泊车路径规划方法、设备及计算机可读存储介质 | |
| CN115540895B (zh) | 掉头路径规划方法、装置、电子设备及存储介质 | |
| CN114355917B (zh) | 超参确定方法、路径规划方法、装置、电子设备和可读存储介质 |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| PB01 | Publication | ||
| PB01 | Publication | ||
| SE01 | Entry into force of request for substantive examination | ||
| SE01 | Entry into force of request for substantive examination | ||
| GR01 | Patent grant | ||
| GR01 | Patent grant |