CN111750859B - 过渡路径规划方法及相关装置 - Google Patents
过渡路径规划方法及相关装置 Download PDFInfo
- Publication number
- CN111750859B CN111750859B CN202010478768.5A CN202010478768A CN111750859B CN 111750859 B CN111750859 B CN 111750859B CN 202010478768 A CN202010478768 A CN 202010478768A CN 111750859 B CN111750859 B CN 111750859B
- Authority
- CN
- China
- Prior art keywords
- trajectory
- track
- feasible
- transition path
- pose
- 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.)
- Active
Links
Images
Classifications
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
- G01C21/20—Instruments for performing navigational calculations
Abstract
本申请的实施例提供了一种过渡路径规划方法及相关装置,涉及设备导引领域。该方法包括:获取作业设备的当前位姿、目标位姿、预设的转弯半径和待作业的作业地块的信息;根据当前位姿、转弯半径以及作业地块的信息,生成位于作业地块内且以当前位姿为起点的至少一个第一安全轨迹;根据目标位姿、转弯半径以及作业地块的信息,生成位于作业地块内且以目标位姿为终点的至少一个第二安全轨迹;根据至少一个第一安全轨迹、至少一个第二安全轨迹生成过渡路径,以便作业设备沿过渡路径从当前位姿移动到目标位姿。本申请能够高效安全地规划出过渡路径,以使作业设备沿该过渡路径从当前位姿移动到目标位姿。
Description
技术领域
本申请涉及设备导引领域,具体而言,涉及一种过渡路径规划方法及相关装置。
背景技术
当前的作业设备(例如,农机、无人车、无人机等)在沿作业路径行驶进行作业之前,需要先从当前的位置移动到作业路径的起点位置,并调整好方向,才能开始沿作业路径行驶进行作业。
换句话说,为了使得作业设备能够成功的沿作业路径行驶进行作业,在当作业设备当前位姿(包括位置和方向)不符合作业路径的起点的位姿时,需要规划出一条过渡路径,以使作业设备沿该过渡路径从当前位姿行驶到作业路径的起点的位姿。
而目前,只能通过人工规划的方式或者通过RLR、RR等路径规划方式实现上述过渡路径的规划。这些规划方式很可能越出作业地块的边界,且规划效率低下。即,现有的过渡路径的规划不安全且效率低下。
发明内容
本申请的目的包括,例如,提供了一种过渡路径规划方法及相关装置,其能够高效安全地规划出过渡路径,以使作业设备沿该过渡路径从当前位姿移动到目标位姿。
本申请的实施例可以这样实现:
第一方面,本申请实施例提供一种过渡路径规划方法,包括:
获取作业设备的当前位姿、目标位姿、预设的转弯半径和待作业的作业地块的信息;
根据所述当前位姿、所述转弯半径以及所述作业地块的信息,生成位于所述作业地块内且以所述当前位姿为起点的至少一个第一安全轨迹;
根据所述目标位姿、所述转弯半径以及所述作业地块的信息,生成位于所述作业地块内且以所述目标位姿为终点的至少一个第二安全轨迹;
根据所述至少一个第一安全轨迹、所述至少一个第二安全轨迹生成过渡路径,以便所述作业设备沿所述过渡路径从所述当前位姿移动到所述目标位姿。
在可选的实施方式中,所述根据所述至少一个第一安全轨迹、所述至少一个第二安全轨迹生成过渡路径,以便所述作业设备沿所述过渡路径从所述当前位姿移动到所述目标位姿的步骤包括:
分别从所述至少一个第一安全轨迹和所述至少一个第二安全轨迹中确定出一个轨迹,得到第一可行轨迹和第二可行轨迹;所述第一可行轨迹与所述第二可行轨迹不相交;
根据所述第一可行轨迹和所述第二可行轨迹生成过渡路径,以便所述作业设备沿所述过渡路径从所述当前位姿移动到所述目标位姿。
在可选的实施方式中,所述分别从所述至少一个第一安全轨迹和所述至少一个第二安全轨迹中确定出一个轨迹,得到第一可行轨迹和第二可行轨迹的步骤包括:
从所述至少一个第一安全轨迹和所述至少一个第二安全轨迹中确定出距离最小且不相交的第一安全轨迹和第二安全轨迹,作为第一可行轨迹和第二可行轨迹。
在可选的实施方式中,在所述至少一个第一安全轨迹和所述至少一个第二安全轨迹中,所述第一安全轨迹与所述第二安全轨迹均相交时,所述分别从所述至少一个第一安全轨迹和所述至少一个第二安全轨迹中确定出一个轨迹,得到第一可行轨迹和第二可行轨迹的步骤还包括:
移动所述当前位姿,并返回执行所述根据所述当前位姿、所述转弯半径以及所述作业地块的信息,生成位于所述作业地块内且以所述当前位姿为起点的至少一个第一安全轨迹的步骤。
在可选的实施方式中,当所述第一可行轨迹和所述第二可行轨迹均为圆弧时,所述根据所述第一可行轨迹、所述第二可行轨迹生成过渡路径,以便所述作业设备沿所述过渡路径从所述当前位姿移动到所述目标位姿的步骤包括:
在预设坐标系中对所述第一可行轨迹和所述第二可行轨迹进行旋转平移变换,分别得到与所述第一可行轨迹对应的当前标准轨迹和与所述第二可行轨迹对应的目标标准轨迹;
其中,所述当前标准轨迹的圆心和所述目标标准轨迹的圆心关于所述预设坐标系的原点对称,且均位于所述预设坐标系的坐标轴上;
根据所述当前标准轨迹和所述目标标准轨迹生成标准过渡路径;
对所述标准过渡路径进行逆旋转平移变换,得到过渡路径,以便所述作业设备沿所述过渡路径从所述当前位姿移动到所述目标位姿。
在可选的实施方式中,当所述作业地块为多边形地块时,所述根据所述当前位姿、所述转弯半径以及所述作业地块的信息,生成位于所述作业地块内且以所述当前位姿为起点的至少一个第一安全轨迹的步骤之前,所述方法还包括:
根据所述多边形地块的所有顶点的坐标集,确定所述多边形地块的每条边的边界参数,其中,所述边界参数包括斜率、截距、起点坐标以及终点坐标。
第二方面,本申请实施例提供一种过渡路径规划装置,包括:
获取模块,用于获取作业设备的当前位姿、目标位姿、预设的转弯半径和待作业的作业地块的信息;
规划模块,用于根据所述当前位姿、所述转弯半径以及所述作业地块的信息,生成位于所述作业地块内且以所述当前位姿为起点的至少一个第一安全轨迹;
所述规划模块,还用于根据所述目标位姿、所述转弯半径以及所述作业地块的信息,生成位于所述作业地块内且以所述目标位姿为终点的至少一个第二安全轨迹;
所述规划模块,还用于根据所述至少一个第一安全轨迹、所述至少一个第二安全轨迹生成过渡路径,以便所述作业设备沿所述过渡路径从所述当前位姿移动到所述目标位姿。
在可选的实施方式中,所述规划模块用于分别从所述至少一个第一安全轨迹和所述至少一个第二安全轨迹中确定出一个轨迹,得到第一可行轨迹和第二可行轨迹;所述第一可行轨迹与所述第二可行轨迹不相交;
所述规划模块还用于根据所述第一可行轨迹和所述第二可行轨迹生成过渡路径,以便所述作业设备沿所述过渡路径从所述当前位姿移动到所述目标位姿。
在可选的实施方式中,所述规划模块用于从所述至少一个第一安全轨迹和所述至少一个第二安全轨迹中确定出距离最小且不相交的第一安全轨迹和第二安全轨迹,作为第一可行轨迹和第二可行轨迹。
在可选的实施方式中,在所述至少一个第一安全轨迹和所述至少一个第二安全轨迹中,所述第一安全轨迹与所述第二安全轨迹均相交时,所述规划模块用于移动所述当前位姿,并返回执行所述根据所述当前位姿、所述转弯半径以及所述作业地块的信息,生成位于所述作业地块内且以所述当前位姿为起点的至少一个第一安全轨迹的步骤。
在可选的实施方式中,当所述第一可行轨迹和所述第二可行轨迹均为圆弧时,所述规划模块用于在预设坐标系中对所述第一可行轨迹和所述第二可行轨迹进行旋转平移变换,分别得到与所述第一可行轨迹对应的当前标准轨迹和与所述第二可行轨迹对应的目标标准轨迹;
其中,所述当前标准轨迹的圆心和所述目标标准轨迹的圆心关于所述预设坐标系的原点对称,且均位于所述预设坐标系的坐标轴上;
所述规划模块还用于根据所述当前标准轨迹和所述目标标准轨迹生成标准过渡路径;
所述规划模块还用于对所述标准过渡路径进行逆旋转平移变换,得到过渡路径,以便所述作业设备沿所述过渡路径从所述当前位姿移动到所述目标位姿。
在可选的实施方式中,当所述作业地块为多边形地块时,所述规划模块用于根据所述多边形地块的所有顶点的坐标集,确定所述多边形地块的每条边的边界参数,其中,所述边界参数包括斜率、截距、起点坐标以及终点坐标。
第三方面,本申请实施例提供一种计算机可读存储介质,其上存储有计算机程序,所述计算机程序被处理器执行时实现如前述实施方式中任一项所述的过渡路径规划方法。
第四方面,本申请实施例提供一种作业设备控制单元,包括处理器和存储器,所述存储器存储有机器可读指令,所述处理器用于执行所述机器可读指令,以实现如前述实施方式中任一项所述的过渡路径规划方法。
第五方面,本申请实施例提供一种作业设备,包括:
机体;
动力设备,安装在所述机体,用于为所述作业设备提供动力;
以及作业设备控制单元;所述作业设备控制单元包括处理器和存储器,所述存储器存储有机器可读指令,所述处理器用于执行所述机器可读指令,以实现如前述实施方式中任一项所述的过渡路径规划方法。
本申请实施例的有益效果包括,例如:通过本申请所提供的方法,能够使得所生成的至少一个第一安全轨迹和至少一个第二安全轨迹均位于作业地块内,进而能够确保所生成的过渡路径也在多变形地块内,避免作业设备在沿过渡路径行驶时超出作业地块,确保作业设备的安全。即,本申请能够高效安全地规划出过渡路径,以使作业设备沿该过渡路径从当前位姿移动到目标位姿。
附图说明
为了更清楚地说明本申请实施例的技术方案,下面将对实施例中所需要使用的附图作简单地介绍,应当理解,以下附图仅示出了本申请的某些实施例,因此不应被看作是对范围的限定,对于本领域普通技术人员来讲,在不付出创造性劳动的前提下,还可以根据这些附图获得其他相关的附图。
图1为本申请实施例所提供的作业设备控制单元的结构框图;
图2为本申请实施例所提供的作业设备的结构框图;
图3为本申请实施例提供的过渡路径规划方法的一种流程图;
图4为本申请实施例提供的过渡路径的一种场景示意图;
图5为本申请实施例提供的过渡路径规划方法的S110步骤的流程图;
图6为本申请实施例提供的当前位姿的转弯圆弧的规划示意图;
图7为本申请实施例提供的过渡路径规划方法的S110B步骤的流程图;
图8为本申请实施例提供的过渡路径规划方法的S120步骤的流程图;
图9为本申请实施例提供的过渡路径规划方法的S120B步骤的流程图;
图10为本申请实施例提供的过渡路径规划方法的另一种流程图;
图11为本申请实施例提供的过渡路径规划方法的S130B步骤的流程图;
图12为本申请实施例提供的过渡路径的另一种场景示意图;
图13为本申请实施例提供的完整过渡路径规划方法的流程示意图;
图14为本申请实施例提供的过渡路径规划装置的一种功能模块图。
图标:100-作业设备控制单元;110-存储器;120-处理器;130-总线;140-通信接口;200-作业设备;210-机体;220-动力设备;300-过渡路径规划装置;310-获取模块;320-规划模块。
具体实施方式
为使本申请实施例的目的、技术方案和优点更加清楚,下面将结合本申请实施例中的附图,对本申请实施例中的技术方案进行清楚、完整地描述,显然,所描述的实施例是本申请一部分实施例,而不是全部的实施例。通常在此处附图中描述和示出的本申请实施例的组件可以以各种不同的配置来布置和设计。
因此,以下对在附图中提供的本申请的实施例的详细描述并非旨在限制要求保护的本申请的范围,而是仅仅表示本申请的选定实施例。基于本申请中的实施例,本领域普通技术人员在没有作出创造性劳动前提下所获得的所有其他实施例,都属于本申请保护的范围。
应注意到:相似的标号和字母在下面的附图中表示类似项,因此,一旦某一项在一个附图中被定义,则在随后的附图中不需要对其进行进一步定义和解释。
需要说明的是,在不冲突的情况下,本申请的实施例中的特征可以相互结合。
在实现本申请实施例的技术方案的过程中,本申请发明人发现:
在对农机进行路径规划以便农机沿规划的路径进行作业时,目前通常关注的都是所规划出的工作路径如何避障、如何能够划出效率最高的作业路径。而对于农机在沿作业路径进行作业之前,农机如何从当前的位姿移动到作业路径的起始位姿,目前还没有很好的解决方法。
当农机的当前位姿(包括位置和方向)不符合作业路径的起点的位姿时,目前通常采用的方法是人工规划,即,由人手动规划从农机的当前位姿到作业路径的起点位姿的过渡路径,这种方式效率低下,并且人工规划并不精准,所规划出的过渡路径很可能会超出作业地块的边界,不能确保农机在作业地块内安全地进行作业。
而在路径规划领域中,从一个位姿到另一个位姿之间的过渡路径可以通过RLR、RR方法实现,但是RLR、RR方法在针对不同的情况时,计算复杂,并且所规划出的过渡路径可能超出作业地块的边界,甚至存在无法规划出过渡路径的情况。因此,RLR、RR方法规划出的过渡路径往往不适用于农机当前的作业地块,不能确保生成能够使得农机在作业地块内安全地进行作业的过渡路径。
换句话说,目前的规划方式很可能越出作业地块的边界,且规划效率低下。现有的过渡路径的规划不安全且效率低下。
因此,为了改善上述缺陷,本申请实施例提出一种过渡路径规划方法及相关装置,其能够高效安全地规划出过渡路径,以使作业设备沿该过渡路径从当前位姿移动到目标位姿。需要说明的是,以上现有技术中的方案所存在的缺陷,均是发明人在经过实践并仔细研究后得出的结果,因此,上述问题的发现过程以及下文中本申请实施例针对上述问题所提出的解决方案,都应该是发明人在本申请过程中对本申请做出的贡献。
请参照图1,为本申请实施例所提供的作业设备控制单元100的结构框图。该作业设备控制单元100可以包括:存储器110、处理器120、总线130和通信接口140,该存储器110、处理器120和通信接口140相互之间直接或间接地电性连接,以实现数据的传输或交互。例如,这些元件相互之间可通过一条或多条总线130或信号线实现电性连接。
处理器120可以处理与过渡路径规划有关的信息和/或数据,以执行本申请中描述的一个或多个功能。例如,处理器120可以获取作业设备的当前位姿、目标位姿、预设的转弯半径和待作业的作业地块的信息,并根据上述数据进行过渡路径规划,能够高效安全地规划出过渡路径,以使作业设备沿该过渡路径从当前位姿移动到目标位姿,实现本申请提供的过渡路径规划方法。
其中,存储器110可以是但不限于,随机存取存储器(Random Access Memory,RAM),只读存储器(Read Only Memory,ROM),可编程只读存储器(Programmable Read-OnlyMemory,PROM),可擦除只读存储器(Erasable Programmable Read-Only Memory,EPROM),电可擦除只读存储器(Electric Erasable Programmable Read-Only Memory,EEPROM)等。
处理器120可以是一种集成电路芯片,具有信号处理能力。该处理器120可以是通用处理器,包括中央处理器(Central Processing Unit,CPU)、网络处理器(NetworkProcessor,NP)等;还可以是数字信号处理器(Digital Signal Processing,DSP)、专用集成电路(Application Specific Integrated Circuit,ASIC)、现场可编程门阵列(Field-Programmable Gate Array,FPGA)或者其他可编程逻辑器件、分立门或者晶体管逻辑器件、分立硬件组件。
在一些可能的实施例中,当本申请所提供的作业设备控制单元100是作业设备的控制核心时,本申请还提供了一种作业设备。请参照图2,为本申请实施例所提供的作业设备200的结构框图,该作业设备200可以包括机体210、动力设备220以及上述的作业设备控制单元100。
其中,动力设备220安装在上述的机体210,用于为作业设备200提供动力。作业设备控制单元100的存储器110存储有与过渡路径规划方法相关的机器可读指令,处理器120可以执行该机器可读指令,进而获取作业设备的当前位姿、目标位姿、预设的转弯半径和待作业的作业地块的信息,并根据上述数据进行过渡路径规划,以及根据规划出的过渡路径控制作业设备200高效安全地根据过渡路径从当前位姿移动到目标位姿,实现本申请提供的过渡路径规划方法。
应理解,本申请提供的作业设备200可以根据作业需求采用不同的构造,例如,本申请所提供的作业设备200的可以是农机、无人机、无人车、无人船、载具、自动驾驶汽车等。也即是说,图2所示的结构仅为示意,该作业设备200还可包括比图2中所示更多或者更少的组件,或者具有与图2所示不同的配置。图2中所示的各组件可以采用硬件、软件或其组合实现。
在另一些可能的实施例中,当本申请所提供的作业设备控制单元100是具有通信功能和计算存储功能的电子设备时,这些电子设备可以获取作业设备的当前位姿、目标位姿、预设的转弯半径和待作业的作业地块的信息,并根据上述数据进行过渡路径规划,并将规划出的过渡路径通过网络传送至作业设备(例如农机、无人车、无人机等),进而使得这些作业设备能够高效安全地根据接收到的过渡路径从当前位姿移动到目标位姿,实现本申请提供的过渡路径规划方法。
下面,为了便于理解,本申请以下实施例将以图2所示的作业设备200为例,结合附图,对本申请实施例提供的过渡路径规划方法进行阐述。
请参照图3,图3示出了本申请实施例提供的过渡路径规划方法的一种流程图。该过渡路径规划方法可以应用于上述的作业设备200,该过渡路径规划方法可以包括以下步骤:
S100,获取作业设备的当前位姿、目标位姿、预设的转弯半径和待作业的作业地块的信息。
在一些可能的实施例中,作业设备200可以从其他设备(例如,后台服务器、云服务器等设备)的存储介质处获取到作业设备的当前位姿、目标位姿、预设的转弯半径和待作业的作业地块的信息,或者也可以从自身的存储介质处获取到作业设备的当前位姿、目标位姿、预设的转弯半径和待作业的作业地块的信息,并且作业设备200还可以通过GPS等定位方式获取到作业设备200的当前位姿。应理解,本申请对于上述数据的获取方式不作限定。
当前位姿包括作业设备200当前的位置信息以及作业设备的当前方向信息,目标位姿包括目标位置信息和目标方向信息。如图4所示,作业设备当前位于A点,且方向(作业设备的朝向)与A点处的箭头方向一致,作业设备将要移动到的点为B点,作业设备在移动到B点时的方向需要与B点处的箭头方向一致。则A点即为作业设备当前的位置信息,A点处的箭头方向即为作业设备的当前方向信息;B点即为目标位置信息,B点处的箭头方向即为目标方向信息。
上述的转弯半径为作业设备在进行转弯时所行驶的轨迹的半径,并且上述的地块可以是规则的地块或是不规则的地块,其中规则的地块可以包括多边形地块、圆形地块、扇形地块等。
S110,根据当前位姿、转弯半径以及作业地块的信息,生成位于作业地块内且以当前位姿为起点的至少一个第一安全轨迹。
在一些可能的实施例中,为了确保作业设备的安全,可以先根据当前位姿、转弯半径生成以当前位姿为起点的多个试探轨迹。其中,所生成的多个试探轨迹的形式可以是圆弧、椭圆弧等,本申请对此不作限定。
为了避免所规划出的过渡路径超出作业地块的边界,在根据当前位姿、转弯半径生成多个试探轨迹之后,可以从这多个试探轨迹中选出位于作业地块内的至少一个轨迹,进而得到至少一个第一安全轨迹。
S120,根据目标位姿、转弯半径以及作业地块的信息,生成位于作业地块内且以目标位姿为终点的至少一个第二安全轨迹。
在一些可能的实施例中,为了确保作业设备的安全,可以先根据目标位姿、转弯半径生成以目标位姿为终点的多个试探轨迹。其中,所生成的多个试探轨迹的形式可以是圆弧、椭圆弧等,本申请对此不作限定。
为了避免所规划出的过渡路径超出作业地块的边界,在根据目标位姿、转弯半径生成多个试探轨迹之后,可以从这多个试探轨迹中选出位于作业地块内的至少一个轨迹,进而得到至少一个第二安全轨迹。
可以理解的是,上述的S110、S120在执行顺序上没有先后之分,在实际应用中,既可以先执行S110再执行S120,也可以先执行S120再执行S110,甚至还可以两者同时执行,本申请对此不作限定。
S130,根据至少一个第一安全轨迹、至少一个第二安全轨迹生成过渡路径,以便作业设备沿过渡路径从当前位姿移动到目标位姿。
在得到至少一个第一安全轨迹和至少一个第二安全轨迹后,可以根据至少一个第一安全轨迹、至少一个第二安全轨迹生成过渡路径。例如,可以先从至少一个第一安全轨迹随机挑选一条第一安全轨迹,从至少一个第二安全轨迹随机挑选一条第二安全轨迹,然后将这挑选出的第一安全轨迹和第二安全轨迹相连,得到作业设备的过渡路径,以便作业设备沿过渡路径从当前位姿移动到目标位姿。
需要说明的是,该过渡路径一般包括以第一安全轨迹部分、第二安全轨迹部分以及相连路径部分。但是在实际应用上述S110、S120以及S130来生成过渡路径时,由于不同的应用场景下,当前位姿和起点位姿的位置关系很可能不相同,所生成的过渡路径可能存在以下情形:
一,所生成的过渡路径仅包括相连路径部分,而不包括以第一安全轨迹部分和第二安全轨迹部分;
二,所生成的过渡路径仅包括相连路径部分和第一安全轨迹部分,而不包括第二安全轨迹部分;
三,所生成的过渡路径仅包括相连路径部分和的第二安全轨迹部分,而不包括第一安全轨迹部分;
四,所生成的过渡路径仅包括第一安全轨迹部分和第二安全轨迹部分,而不包括相连路径部分;
五,所生成的过渡路径仅包括第一安全轨迹部分,而不包括相连路径部分和第二安全轨迹部分;
六,所生成的过渡路径仅包括第二安全轨迹部分,而不包括相连路径部分和第一安全轨迹部分;
七,当前位姿和完整起点位姿完全相同时,所生成的过渡路径不存在。
进而,上述S110、S120以及S130可以理解为一种规划过渡路径的完整流程,在规划从当前位姿到完整起点位姿之间的过渡路径时,会尝试规划出第一安全轨迹部分、第二安全轨迹部分以及相连路径部分,但是最后规划出的过渡路径可以包括第一安全轨迹部分、第二安全轨迹部分以及相连路径部分中的至少一种,或者所规划出的过渡路径可以不存在。
应理解,通过本申请所提供的方法,能够使得所生成的至少一个第一安全轨迹和至少一个第二安全轨迹均位于作业地块内,进而能够确保所生成的过渡路径也在多变形地块内,避免作业设备在沿过渡路径行驶时超出作业地块,确保作业设备的安全。即,本申请能够高效安全地规划出过渡路径,以使作业设备沿该过渡路径从当前位姿移动到目标位姿。
在一些可能的实施例中,上述的作业地块可以为多边形地块。为了便于S110和S120的执行,在执行S110和S120之前,还可以将多边形地块的边界特征化。即,本申请所提供的方法还可以包括:根据所述多边形地块的所有顶点的坐标集,确定所述多边形地块的每条边的边界参数,其中,所述边界参数包括斜率、截距、起点坐标以及终点坐标。
其中,将多边形地块的边界特征化,是指可以根据多边形地块每条边的顶点坐标得到每条边的斜率,截距,起点坐标和终点坐标。
例如,对于N边形地块的边界特征化,可以首先将N边形地块各个边的顶点的坐标(即多边形地块的所有顶点的坐标集)按逆时针或顺时针的顺序进行排列,得到顺序排列的多个顶点坐标。其中,假设多个顶点坐标中的第i个点坐标为(xi,yi),i=1,2,…N。然后按顺序排列的多个顶点坐标确定每条边的起点坐标以及终点坐标,其中,当i=N时,最后一条边(即第N条边)的起点坐标为(xN,yN),最后一条边的终点坐标为(x1,y1),当i≠N时,第i条边的起点坐标为(xi,yi),第i条边的终点坐标为(xi+1,yi+1)。
最后可以根据如下公式(1)、(2)确定各个边界的斜率以及截距;
bi=yi-kixi (2);
其中,ki和bi分别是N边形地块的第i条边的斜率和截距。
根据以上的“将多边形地块的边界特征化”的步骤,可以得到如下表1所示的多边形地块的每条边的边界参数。
表1
可以理解,将多边形地块的边界特征化之后,在执行S110和S120时,仅需将生成的试探轨迹与多边形地块的每条边的边界参数进行比较,就可判断出该生成的试探轨迹是否位于多边形地块内,进而降低计算量,提高方法的计算效率。
在一些可能的实施例中,当生成的试探轨迹为圆弧(即,生成的第一安全轨迹为圆弧)时,对于如何根据当前位姿、转弯半径以及作业地块的信息,生成位于作业地块内且以当前位姿为起点的至少一个第一安全轨迹,请参照图5,S110可以包括:
S110A,根据所述当前位姿和所述转弯半径生成以所述当前位姿为起点的当前转弯圆弧集合。
假设当前位姿的坐标为(xs,ys),方向为θ,则对于该当前位姿,分别有顺时针和逆时针两种方向的圆弧,即,根据当前位姿和转弯半径所生成的当前转弯圆弧集合包括顺时针当前转弯圆弧和逆时针当前转弯圆弧(如图6中的逆时针当前转弯圆弧C1和顺时针当前转弯圆弧C2)。
为了确保作业设备的安全,需要判断顺时针当前转弯圆弧和逆时针当前转弯圆弧中是否存在位于作业地块内的圆弧,当存在位于作业地块内的圆弧时,则能够确保作业设备的安全。如图6,逆时针当前转弯圆弧C1位于作业地块内,而顺时针当前转弯圆弧C2超出了作业地块。
S110B,将所述当前转弯圆弧集合中位于所述作业地块内的当前转弯圆弧加入至当前安全转弯圆弧集合,得到至少一个第一安全轨迹。
可以理解,当前转弯圆弧集合包括上述的顺时针当前转弯圆弧和逆时针当前转弯圆弧。进而,可以将这两者中位于作业地块内的圆弧加入至当前安全转弯圆弧集合,得到至少一个第一安全轨迹。如图6所示,可以将逆时针当前转弯圆弧C1加入至当前安全转弯圆弧集合。
进一步的,当上述的作业地块为多边形地块时,对于如何将所述当前转弯圆弧集合中位于所述作业地块内的当前转弯圆弧加入至当前安全转弯圆弧集合,得到至少一个第一安全轨迹,请参照图7,S110B可以包括:
S110B-1,确定所述当前转弯圆弧集合中每个圆弧的圆心坐标。
假设多边形地块的信息基于X-Y轴坐标系建立,该坐标系的原点为(0,0),上述的转弯半径为R,当前位姿的坐标为(xs,ys),方向为θ。则可以先获得标准圆心坐标O1=(-R,0)和O2=(0,R)。然后根据公式Oj`=Rotate(Oj,θ),j=1,2对标准圆心坐标进行逆时针旋转。其中,Rotate为以逆时针方向为正方向,绕原点的旋转变换函数。最后根据公式Oj``=Oj`+S对旋转过后的圆心坐标进行偏移,即可得到当前转弯圆弧集合中每个圆弧的圆心坐标。
S110B-2,根据所述圆心坐标判断所述当前转弯圆弧集合中每个圆弧是否位于多边形地块内。
假设多边形地块的信息基于X-Y轴坐标系建立,该坐标系的原点为(0,0),上述的转弯半径为R,并以确定目标圆弧为例(该目标圆弧可以为当前转弯圆弧集合中的任一个圆弧),则根据圆心坐标判断目标圆弧是否位于多边形地块内可以包括如下步骤:
步骤1,确定出目标圆弧的圆心与多边形地块的每条边的距离,得到多个距离值{d1j,d2j,…,dij,…,dNj},其中,1≤i≤N,i为多边形地块的边的序号;
步骤2,判断{d1j,d2j,…,dij,…,dNj}中的任一距离值是否满足dij≥R+C(C为一常数);
步骤3,当{d1j,d2j,…,dij,…,dNj}中的任一距离值均满足dij≥R+C时,确定该目标圆弧位于多边形地块内;否则执行步骤4;
步骤4,从{d1j,d2j,…,dij,…,dNj}中获取所有不满足dij≥R+C的距离值所对应的多边形地块的边,得到至少一条边;
步骤5,确定目标圆弧的圆心与所述至少一条边的位置参数值,得到至少一个位置参数值;若位置参数值均大于等于0且小于等于1,则确定该目标圆弧不位于多边形地块内;否则执行步骤6;
步骤6,确定目标圆弧的圆心与所述至少一条边的两个端点的最小距离,得到至少一个最小距离值,若所有的最小距离值均大于等于R+C,则确定该目标圆弧位于多边形地块内;否则确定该目标圆弧不位于多边形地块内。
根据以上述的步骤1-6类推,即可判断出当前转弯圆弧集合中每个圆弧是否位于多边形地块内。
其中,可以根据公式计算出目标圆弧的圆心坐标到多边形地块的第i条边的距离。根据公式可以计算出目标圆弧的圆心坐标到多边形地块的第i条边的垂足横坐标,以及根据公式可以计算出目标圆弧的圆心与第i条边的位置参数值。多边形地块的多个顶点坐标中的第i个点坐标为(xi,yi),i=1,2,…N,ki和bi分别是N边形地块的第i条边的斜率和截距。
在一些可能的实施例中,当生成的试探轨迹为圆弧(即,生成的第二安全轨迹为目标安全转弯圆弧)时,对于如何根据目标位姿、转弯半径以及作业地块的信息,生成位于作业地块内且以目标位姿为终点的至少一个第二安全轨迹,请参照图8,S120可以包括:
S120A,根据所述目标位姿和所述转弯半径生成以所述目标位姿为终点的目标转弯圆弧集合。
S120B,将所述目标转弯圆弧集合中位于所述作业地块内的目标转弯圆弧加入至目标安全转弯圆弧集合,得到至少一个第二安全轨迹。
进一步的,当上述的作业地块可以为多边形地块时,对于如何将所述目标转弯圆弧集合中位于所述作业地块内的目标转弯圆弧加入至目标安全转弯圆弧集合,得到至少一个第二安全轨迹,请参照图9,S120B可以包括:
S120B-1,确定所述当前转弯圆弧集合中每个圆弧的圆心坐标。
S120B-2,根据所述圆心坐标判断所述当前转弯圆弧集合中每个圆弧是否位于多边形地块内。
可以理解的是,上述的S120A、S120B、S120B-1、S120B-2的具体过程可以分别参照上述的S110A、S110B、S110B-1、S110B-2,在此不再赘述。
进一步的,由于在根据两个轨迹生成过渡路径时,若这两个轨迹相交则会导致无法生成可行的过渡路径。因此,为了确保在根据至少一个第一安全轨迹、至少一个第二安全轨迹生成过渡路径时可行,对于如何根据至少一个第一安全轨迹、至少一个第二安全轨迹生成过渡路径,以便作业设备沿过渡路径从当前位姿移动到目标位姿,在图3的基础上,请参照图10,S130可以包括:
S130A,分别从至少一个第一安全轨迹和至少一个第二安全轨迹中确定出一个轨迹,得到第一可行轨迹和第二可行轨迹;第一可行轨迹与第二可行轨迹不相交。
可以理解的是,第一可行轨迹与第二可行轨迹不相交可以指的是:第一可行轨迹与第二可行轨迹相离或相切。
例如,可以先判断所述至少一个第一安全轨迹和所述至少一个第二安全轨迹中,是否存在有不相交的轨迹,若存在,则将不相交的这两个轨迹确定为第一可行轨迹和第二可行轨迹。应理解,此时所确定出来的不相交的两个轨迹能够确保在根据至少一个第一安全轨迹、至少一个第二安全轨迹生成过渡路径时可行。
在一些可能的实施例中,为了找出最合适的第一可行轨迹和第二可行轨迹,对于如何分别从至少一个第一安全轨迹和至少一个第二安全轨迹中确定出一个轨迹,得到第一可行轨迹和第二可行轨迹,S130A可以包括:从至少一个第一安全轨迹和至少一个第二安全轨迹中确定出距离最小且不相交的第一安全轨迹和第二安全轨迹,作为第一可行轨迹和第二可行轨迹。
例如,可以先从至少一个第一安全轨迹和至少一个第二安全轨迹中确定出所有不相交的第一安全轨迹和第二安全轨迹,得到第一安全轨迹集合和第二安全轨迹集合。然后从第一安全轨迹集合和第二安全轨迹集合确定出中心距离最小的第一安全轨迹和第二安全轨迹,作为第一可行轨迹和第二可行轨迹。
进一步的,当所述第一可行轨迹和所述第二可行轨迹均为圆弧时,S130A还可以包括:从至少一个第一安全轨迹和至少一个第二安全轨迹中确定出圆心距离最小且不相交的第一安全轨迹和第二安全轨迹,作为第一可行轨迹和第二可行轨迹。
在一些可能的实施例中,在所述至少一个第一安全轨迹和所述至少一个第二安全轨迹中,所述第一安全轨迹与所述第二安全轨迹均相交时,对于上述的S130A,该步骤还可以包括:移动当前位姿,并返回执行S110。
其中,假设转弯半径为R,当前位姿的坐标为(xs,ys),方向为θ,则可以根据公式(xs,ys)`=(xs,ys)+(Rcosθ,Rsinθ),对当前位姿进行平移,得到平移后的位姿(xs,ys)`作为新的当前位姿,并返回执行S110。
应理解,由于在所述至少一个第一安全轨迹和所述至少一个第二安全轨迹中,所述第一安全轨迹与所述第二安全轨迹均相交时,可以采用试探的方式对当前位姿进行移动,进而再次应用本申请提供的方法重新进行过渡路径的规划,达到了自动进行多次过渡路径规划,提高自动化程度以及确保过渡路径能够成功生成的效果。
S130B,根据第一可行轨迹和第二可行轨迹生成过渡路径,以便作业设备沿过渡路径从当前位姿移动到目标位姿。
由于实际应用中的位姿信息各种各样,每次都直接进行过渡路径的规划会比较复杂。因此,为了简化过渡路径的规划,在一些可能的实施例中,当所述第一可行轨迹和所述第二可行轨迹均为圆弧时,对于如何根据第一可行轨迹和第二可行轨迹生成过渡路径,以便作业设备沿过渡路径从当前位姿移动到目标位姿,在图3的基础上,请参照图11,S130B可以包括:
S130B-1,在预设坐标系中对第一可行轨迹和第二可行轨迹进行旋转平移变换,分别得到与第一可行轨迹对应的当前标准轨迹和与第二可行轨迹对应的目标标准轨迹;
其中,当前标准轨迹的圆心和目标标准轨迹的圆心关于预设坐标系的原点对称,且均位于预设坐标系的坐标轴上。
如图12所示,假设预设坐标系为X-Y坐标系,第一可行轨迹和第二可行轨迹分别为S1和S2,则在X-Y坐标系中对第一可行轨迹和第二可行轨迹进行旋转平移变换,得到当前标准轨迹和与第二可行轨迹对应的目标标准轨迹的过程可以是:
首先,假设第一可行轨迹S1的圆心坐标为(XS1,YS1),第二可行轨迹S2的圆心坐标为(XS2,YS2),两圆心坐标的中点坐标为Omid=(xmid,ymid)。根据如下公式:即可实现对第一可行轨迹的圆心和第二可行轨迹的圆心进行旋转平移变换,该旋转平移变换的平移量为(-xmid,-ymid),旋转量为α;其中X、Y分别为轨迹圆心的坐标,(x″,y″)为轨迹圆心进行旋转平移变换后的坐标,(xc,yc)为旋转平移变换前S1的圆心坐标,(xt,yt)为旋转平移变换前S2的圆心坐标,(x″,y″)为被操作坐标的最终变换结果。
例如对于第一可行轨迹S1的圆心进行旋转平移,则代入上述公式可得第一可行轨迹的圆心坐标的最终变换结果。
在对第一可行轨迹S1的圆心和第二可行轨迹S2的圆心进行旋转平移变换后,还需要根据如下公式:对当前位姿和目标位姿的方向进行平移旋转,进而实现对第一可行轨迹和第二可行轨迹进行旋转平移变换;其中,θ∈[0,2π],是位姿的方向角,θ′为更新后的方向角。
例如,假设当前位姿的方向角为θ1,对于当前位姿进行旋转平移,则代入上述公式可得更新后的当前位姿的方向角。
S130B-2,根据当前标准轨迹和目标标准轨迹生成标准过渡路径。
S130B-3,对标准过渡路径进行逆旋转平移变换,得到过渡路径,以便作业设备沿过渡路径从当前位姿移动到目标位姿。
可以理解,逆旋转平移变换时所需要的角度旋转量为-α,坐标平移量为(xmid,ymid)。
应理解,本申请所提供的方法既可以在三维空间中实现,也可以在二维平面上实现,本申请所提供的示意图所展示的二维平面上的过渡路径规划过程仅仅只是示意过程,本领域技术人员在本申请的基础上可以容易地将过渡路径的规划方法应用至三维空间中。
下面将结合实际应用对上述S100-S130做进一步解释。
请参照图13,为本申请实施例所提供的一种完整过渡路径规划方法的流程示意图,该方法包括如下步骤:
S1,获取多边形地块的顶点坐标集、作业设备的当前位姿、目标位姿和转弯半径。
S2,根据多边形地块的顶点坐标集对多边形地块进行地块边界特征化。
S3,对当前位姿和目标位姿进行安全性评估。
S4,当安全性评估结果为安全时,对过渡路径规划进行可行性评估;当安全性评估结果为不安全时,输出无法生成安全过渡路径信息。
S5,当可行性评估结果为可行时,计算当前位姿和目标位姿对应的转弯轨迹的当前圆心和目标圆心;当可行性评估结果为不可行时,重设当前位姿,并返回执行S3。
S6,对当前圆心和目标圆心进行旋转平移变换,得到标准当前圆心和标准目标圆心;对当前位姿和目标位姿进行旋转平移变换,得到标准当前位姿和标准目标位姿。
S7,根据标准当前圆心、标准目标圆心、标准当前位姿、标准目标位姿、转弯半径生成标准过渡路径,并对标准过渡路径进行逆旋转平移变换,得到过渡路径,以便作业设备沿过渡路径从当前位姿移动到目标位姿。
可以理解的是,上述S1-S7可以参照上述方法步骤,在此不再赘述。
为了执行上述实施例及各个可能的方式中的相应步骤,下面给出一种过渡路径规划装置的实现方式,请参阅图14,图14示出了本申请实施例提供的过渡路径规划装置的一种功能模块图。需要说明的是,本实施例所提供的过渡路径规划装置300,其基本原理及产生的技术效果和上述实施例相同,为简要描述,本实施例部分未提及之处,可参考上述的实施例中相应内容。该过渡路径规划装置300包括:获取模块310、规划模块320。
可选地,上述模块可以软件或固件(Firmware)的形式存储于存储器中或固化于本申请提供的作业设备的操作系统(Operating System,OS)中,并可由作业设备中的处理器执行。同时,执行上述模块所需的数据、程序的代码等可以存储在存储器中。
基于上述方法实施例,本申请实施例还提供了一种计算机可读存储介质,该计算机可读存储介质上存储有计算机程序,该计算机程序被处理器运行时执行上述过渡路径规划方法的步骤。
获取模块310可以用于获取作业设备的当前位姿、目标位姿、预设的转弯半径和待作业的作业地块的信息。
可以理解的是,获取模块310可以用于支持作业设备执行上述S100等,和/或用于本文所描述的技术的其他过程。
规划模块320可以用于根据当前位姿、转弯半径以及作业地块的信息,生成位于作业地块内且以当前位姿为起点的至少一个第一安全轨迹。
可以理解的是,规划模块320可以用于支持作业设备执行上述S110等,和/或用于本文所描述的技术的其他过程。
规划模块320可以用于根据目标位姿、转弯半径以及作业地块的信息,生成位于作业地块内且以目标位姿为终点的至少一个第二安全轨迹。
可以理解的是,规划模块320可以用于支持作业设备执行上述S120等,和/或用于本文所描述的技术的其他过程。
规划模块320可以用于根据至少一个第一安全轨迹、至少一个第二安全轨迹生成过渡路径,以便作业设备沿过渡路径从当前位姿移动到目标位姿。
可以理解的是,规划模块320可以用于支持作业设备执行上述S130等,和/或用于本文所描述的技术的其他过程。
在一些可能的实施例中,当生成的试探轨迹为圆弧(即,生成的第一安全轨迹为圆弧)时,对于如何根据当前位姿、转弯半径以及作业地块的信息,生成位于作业地块内且以当前位姿为起点的至少一个第一安全轨迹,规划模块320可以用于根据所述当前位姿和所述转弯半径生成以所述当前位姿为起点的当前转弯圆弧集合;将所述当前转弯圆弧集合中位于所述作业地块内的当前转弯圆弧加入至当前安全转弯圆弧集合,得到至少一个第一安全轨迹。
可以理解的是,规划模块320可以用于支持作业设备执行上述S110A、S110B等,和/或用于本文所描述的技术的其他过程。
当上述的作业地块为多边形地块时,对于如何将所述当前转弯圆弧集合中位于所述作业地块内的当前转弯圆弧加入至当前安全转弯圆弧集合,得到至少一个第一安全轨迹,规划模块320可以用于确定所述当前转弯圆弧集合中每个圆弧的圆心坐标;根据所述圆心坐标判断所述当前转弯圆弧集合中每个圆弧是否位于多边形地块内。
可以理解的是,规划模块320可以用于支持作业设备执行上述S110B-1、S110B-2等,和/或用于本文所描述的技术的其他过程。
在一些可能的实施例中,当生成的试探轨迹为圆弧(即,生成的第二安全轨迹为目标安全转弯圆弧)时,对于如何根据目标位姿、转弯半径以及作业地块的信息,生成位于作业地块内且以目标位姿为终点的至少一个第二安全轨迹,规划模块320可以用于根据所述目标位姿和所述转弯半径生成以所述目标位姿为终点的目标转弯圆弧集合;将所述目标转弯圆弧集合中位于所述作业地块内的目标转弯圆弧加入至目标安全转弯圆弧集合,得到至少一个第二安全轨迹。
可以理解的是,规划模块320可以用于支持作业设备执行上述S120A、S120B等,和/或用于本文所描述的技术的其他过程。
当上述的作业地块可以为多边形地块时,对于如何将所述目标转弯圆弧集合中位于所述作业地块内的目标转弯圆弧加入至目标安全转弯圆弧集合,得到至少一个第二安全轨迹,规划模块320可以用于确定所述当前转弯圆弧集合中每个圆弧的圆心坐标;根据所述圆心坐标判断所述当前转弯圆弧集合中每个圆弧是否位于多边形地块内。
可以理解的是,规划模块320可以用于支持作业设备执行上述S120B-1、S120B-2等,和/或用于本文所描述的技术的其他过程。
进一步的,由于在根据两个轨迹生成过渡路径时,若这两个轨迹相交则会导致无法生成可行的过渡路径。因此,为了确保在根据至少一个第一安全轨迹、至少一个第二安全轨迹生成过渡路径时可行,对于如何根据至少一个第一安全轨迹、至少一个第二安全轨迹生成过渡路径,以便作业设备沿过渡路径从当前位姿移动到目标位姿,规划模块320可以用于分别从至少一个第一安全轨迹和至少一个第二安全轨迹中确定出一个轨迹,得到第一可行轨迹和第二可行轨迹;第一可行轨迹与第二可行轨迹不相交;根据第一可行轨迹和第二可行轨迹生成过渡路径,以便作业设备沿过渡路径从当前位姿移动到目标位姿。
可以理解的是,规划模块320可以用于支持作业设备执行上述S130A、S130B等,和/或用于本文所描述的技术的其他过程。
由于实际应用中的位姿信息各种各样,每次都直接进行过渡路径的规划会比较复杂。因此,为了简化过渡路径的规划,在一些可能的实施例中,当所述第一可行轨迹和所述第二可行轨迹均为圆弧时,对于如何根据第一可行轨迹和第二可行轨迹生成过渡路径,以便作业设备沿过渡路径从当前位姿移动到目标位姿,规划模块320可以用于在预设坐标系中对第一可行轨迹和第二可行轨迹进行旋转平移变换,分别得到与第一可行轨迹对应的当前标准轨迹和与第二可行轨迹对应的目标标准轨迹;其中,当前标准轨迹的圆心和目标标准轨迹的圆心关于预设坐标系的原点对称,且均位于预设坐标系的坐标轴上;根据当前标准轨迹和目标标准轨迹生成标准过渡路径;对标准过渡路径进行逆旋转平移变换,得到过渡路径,以便作业设备沿过渡路径从当前位姿移动到目标位姿。
可以理解的是,规划模块320可以用于支持作业设备执行上述S130B-1、S130B-2、S130B-3等,和/或用于本文所描述的技术的其他过程。
具体地,该存储介质可以为通用的存储介质,如移动磁盘、硬盘等,该存储介质上的计算机程序被运行时,能够执行上述过渡路径规划方法,从而解决现有的过渡路径的规划不安全且效率低下的问题,实现高效安全地规划出过渡路径,以使作业设备沿该过渡路径从当前位姿移动到目标位姿的目的。
综上所述,本申请实施例提供了一种过渡路径规划方法及相关装置,该方法包括:获取作业设备的当前位姿、目标位姿、预设的转弯半径和待作业的作业地块的信息;根据当前位姿、转弯半径以及作业地块的信息,生成位于作业地块内且以当前位姿为起点的至少一个第一安全轨迹;根据目标位姿、转弯半径以及作业地块的信息,生成位于作业地块内且以目标位姿为终点的至少一个第二安全轨迹;根据至少一个第一安全轨迹、至少一个第二安全轨迹生成过渡路径,以便作业设备沿过渡路径从当前位姿移动到目标位姿。通过本申请所提供的方法,能够使得所生成的至少一个第一安全轨迹和至少一个第二安全轨迹均位于作业地块内,进而能够确保所生成的过渡路径也在多变形地块内,避免作业设备在沿过渡路径行驶时超出作业地块,确保作业设备的安全。即,本申请能够高效安全地规划出过渡路径,以使作业设备沿该过渡路径从当前位姿移动到目标位姿。
以上所述,仅为本申请的具体实施方式,但本申请的保护范围并不局限于此,任何熟悉本技术领域的技术人员在本申请揭露的技术范围内,可轻易想到的变化或替换,都应涵盖在本申请的保护范围之内。因此,本申请的保护范围应以所述权利要求的保护范围为准。
Claims (15)
1.一种过渡路径规划方法,其特征在于,包括:
获取作业设备的当前位姿、目标位姿、预设的转弯半径和待作业的作业地块的信息;
根据所述当前位姿、所述转弯半径以及所述作业地块的信息,生成位于所述作业地块内且以所述当前位姿为起点的至少一个第一安全轨迹;
根据所述目标位姿、所述转弯半径以及所述作业地块的信息,生成位于所述作业地块内且以所述目标位姿为终点的至少一个第二安全轨迹;
根据所述至少一个第一安全轨迹、所述至少一个第二安全轨迹生成过渡路径,以便所述作业设备沿所述过渡路径从所述当前位姿移动到所述目标位姿。
2.根据权利要求1所述的方法,其特征在于,所述根据所述至少一个第一安全轨迹、所述至少一个第二安全轨迹生成过渡路径,以便所述作业设备沿所述过渡路径从所述当前位姿移动到所述目标位姿的步骤包括:
分别从所述至少一个第一安全轨迹和所述至少一个第二安全轨迹中确定出一个轨迹,得到第一可行轨迹和第二可行轨迹;所述第一可行轨迹与所述第二可行轨迹不相交;
根据所述第一可行轨迹和所述第二可行轨迹生成过渡路径,以便所述作业设备沿所述过渡路径从所述当前位姿移动到所述目标位姿。
3.根据权利要求2所述的方法,其特征在于,所述分别从所述至少一个第一安全轨迹和所述至少一个第二安全轨迹中确定出一个轨迹,得到第一可行轨迹和第二可行轨迹的步骤包括:
从所述至少一个第一安全轨迹和所述至少一个第二安全轨迹中确定出距离最小且不相交的第一安全轨迹和第二安全轨迹,作为第一可行轨迹和第二可行轨迹。
4.根据权利要求2所述的方法,其特征在于,在所述至少一个第一安全轨迹和所述至少一个第二安全轨迹中,所述第一安全轨迹与所述第二安全轨迹均相交时,所述分别从所述至少一个第一安全轨迹和所述至少一个第二安全轨迹中确定出一个轨迹,得到第一可行轨迹和第二可行轨迹的步骤还包括:
移动所述当前位姿,并返回执行所述根据所述当前位姿、所述转弯半径以及所述作业地块的信息,生成位于所述作业地块内且以所述当前位姿为起点的至少一个第一安全轨迹的步骤。
5.根据权利要求2所述的方法,其特征在于,当所述第一可行轨迹和所述第二可行轨迹均为圆弧时,所述根据所述第一可行轨迹、所述第二可行轨迹生成过渡路径,以便所述作业设备沿所述过渡路径从所述当前位姿移动到所述目标位姿的步骤包括:
在预设坐标系中对所述第一可行轨迹和所述第二可行轨迹进行旋转平移变换,分别得到与所述第一可行轨迹对应的当前标准轨迹和与所述第二可行轨迹对应的目标标准轨迹;
其中,所述当前标准轨迹的圆心和所述目标标准轨迹的圆心关于所述预设坐标系的原点对称,且均位于所述预设坐标系的坐标轴上;
根据所述当前标准轨迹和所述目标标准轨迹生成标准过渡路径;
对所述标准过渡路径进行逆旋转平移变换,得到过渡路径,以便所述作业设备沿所述过渡路径从所述当前位姿移动到所述目标位姿。
6.根据权利要求2所述的方法,其特征在于,当所述作业地块为多边形地块时,所述根据所述当前位姿、所述转弯半径以及所述作业地块的信息,生成位于所述作业地块内且以所述当前位姿为起点的至少一个第一安全轨迹的步骤之前,所述方法还包括:
根据所述多边形地块的所有顶点的坐标集,确定所述多边形地块的每条边的边界参数,其中,所述边界参数包括斜率、截距、起点坐标以及终点坐标。
7.一种过渡路径规划装置,其特征在于,包括:
获取模块,用于获取作业设备的当前位姿、目标位姿、预设的转弯半径和待作业的作业地块的信息;
规划模块,用于根据所述当前位姿、所述转弯半径以及所述作业地块的信息,生成位于所述作业地块内且以所述当前位姿为起点的至少一个第一安全轨迹;
所述规划模块,还用于根据所述目标位姿、所述转弯半径以及所述作业地块的信息,生成位于所述作业地块内且以所述目标位姿为终点的至少一个第二安全轨迹;
所述规划模块,还用于根据所述至少一个第一安全轨迹、所述至少一个第二安全轨迹生成过渡路径,以便所述作业设备沿所述过渡路径从所述当前位姿移动到所述目标位姿。
8.根据权利要求7所述的装置,其特征在于,所述规划模块用于分别从所述至少一个第一安全轨迹和所述至少一个第二安全轨迹中确定出一个轨迹,得到第一可行轨迹和第二可行轨迹;所述第一可行轨迹与所述第二可行轨迹不相交;
所述规划模块还用于根据所述第一可行轨迹和所述第二可行轨迹生成过渡路径,以便所述作业设备沿所述过渡路径从所述当前位姿移动到所述目标位姿。
9.根据权利要求8所述的装置,其特征在于,所述规划模块用于从所述至少一个第一安全轨迹和所述至少一个第二安全轨迹中确定出距离最小且不相交的第一安全轨迹和第二安全轨迹,作为第一可行轨迹和第二可行轨迹。
10.根据权利要求8所述的装置,其特征在于,在所述至少一个第一安全轨迹和所述至少一个第二安全轨迹中,所述第一安全轨迹与所述第二安全轨迹均相交时,所述规划模块用于移动所述当前位姿,并返回执行所述根据所述当前位姿、所述转弯半径以及所述作业地块的信息,生成位于所述作业地块内且以所述当前位姿为起点的至少一个第一安全轨迹的步骤。
11.根据权利要求8所述的装置,其特征在于,当所述第一可行轨迹和所述第二可行轨迹均为圆弧时,所述规划模块用于在预设坐标系中对所述第一可行轨迹和所述第二可行轨迹进行旋转平移变换,分别得到与所述第一可行轨迹对应的当前标准轨迹和与所述第二可行轨迹对应的目标标准轨迹;
其中,所述当前标准轨迹的圆心和所述目标标准轨迹的圆心关于所述预设坐标系的原点对称,且均位于所述预设坐标系的坐标轴上;
所述规划模块还用于根据所述当前标准轨迹和所述目标标准轨迹生成标准过渡路径;
所述规划模块还用于对所述标准过渡路径进行逆旋转平移变换,得到过渡路径,以便所述作业设备沿所述过渡路径从所述当前位姿移动到所述目标位姿。
12.根据权利要求8所述的装置,其特征在于,当所述作业地块为多边形地块时,所述规划模块用于根据所述多边形地块的所有顶点的坐标集,确定所述多边形地块的每条边的边界参数,其中,所述边界参数包括斜率、截距、起点坐标以及终点坐标。
13.一种计算机可读存储介质,其上存储有计算机程序,其特征在于,所述计算机程序被处理器执行时实现如权利要求1-6中任一项所述的过渡路径规划方法。
14.一种作业设备控制单元,其特征在于,包括处理器和存储器,所述存储器存储有机器可读指令,所述处理器用于执行所述机器可读指令,以实现如权利要求1-6中任一项所述的过渡路径规划方法。
15.一种作业设备,其特征在于,包括:
机体;
动力设备,安装在所述机体,用于为所述作业设备提供动力;
以及作业设备控制单元;所述作业设备控制单元包括处理器和存储器,所述存储器存储有机器可读指令,所述处理器用于执行所述机器可读指令,以实现如权利要求1-6中任一项所述的过渡路径规划方法。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202010478768.5A CN111750859B (zh) | 2020-05-29 | 2020-05-29 | 过渡路径规划方法及相关装置 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202010478768.5A CN111750859B (zh) | 2020-05-29 | 2020-05-29 | 过渡路径规划方法及相关装置 |
Publications (2)
Publication Number | Publication Date |
---|---|
CN111750859A CN111750859A (zh) | 2020-10-09 |
CN111750859B true CN111750859B (zh) | 2021-11-05 |
Family
ID=72674340
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN202010478768.5A Active CN111750859B (zh) | 2020-05-29 | 2020-05-29 | 过渡路径规划方法及相关装置 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN111750859B (zh) |
Families Citing this family (2)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN112686424A (zh) * | 2020-12-03 | 2021-04-20 | 广州极飞科技有限公司 | 平地路径范围确定方法、平地路径规划方法及相关装置 |
US20220397914A1 (en) * | 2021-06-04 | 2022-12-15 | Ubtech North America Research And Development Center Corp | Fast continuous regulation of nonholonomic mobile robots |
Citations (13)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
US5029088A (en) * | 1988-10-14 | 1991-07-02 | Tsubakimoto Chain Co. | Method of and apparatus for guiding a moving object |
US5615116A (en) * | 1990-02-05 | 1997-03-25 | Caterpillar Inc. | Apparatus and method for autonomous vehicle navigation using path data |
CN105751196A (zh) * | 2016-04-12 | 2016-07-13 | 华南理工大学 | 一种基于主从式的工业机器人协作作业方法 |
CN106643733A (zh) * | 2016-12-30 | 2017-05-10 | 北京理工大学 | 面向多导航站接力导航的运动体路径规划方法 |
CN107168305A (zh) * | 2017-04-01 | 2017-09-15 | 西安交通大学 | 路口场景下基于Bezier和VFH的无人车轨迹规划方法 |
CN107918388A (zh) * | 2017-11-08 | 2018-04-17 | 深圳市招科智控科技有限公司 | 港口集装箱搬运无人车轨迹控制方法及装置 |
WO2018143958A1 (en) * | 2017-01-31 | 2018-08-09 | Halliburton Energy Services, Inc. | Curvature-based feedback control techniques for directional drilling |
CN208329765U (zh) * | 2018-06-19 | 2019-01-04 | 方城龙昊液压设备有限责任公司 | 一种行走机构及无避让立体停车设备 |
CN109144097A (zh) * | 2018-08-15 | 2019-01-04 | 广州极飞科技有限公司 | 障碍物或地面识别及飞行控制方法、装置、设备及介质 |
CN109496287A (zh) * | 2017-10-31 | 2019-03-19 | 深圳市大疆创新科技有限公司 | 可移动设备作业控制方法及装置、路径规划方法及装置 |
CN110300940A (zh) * | 2017-02-28 | 2019-10-01 | 本田技研工业株式会社 | 作业系统以及作业机 |
CN110383191A (zh) * | 2017-03-09 | 2019-10-25 | 洋马株式会社 | 路径生成系统 |
CN110440823A (zh) * | 2019-08-27 | 2019-11-12 | 广州小鹏汽车科技有限公司 | 一种路径规划方法及路径规划系统 |
Family Cites Families (11)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
US10378896B2 (en) * | 2006-02-27 | 2019-08-13 | Trimble Inc. | Method and system for planning the path of an agricultural vehicle |
US9321458B2 (en) * | 2013-12-17 | 2016-04-26 | Automotive Research & Testing Center | Sliding mode trajectory voting strategy module and driving control system and method thereof |
JP2017127290A (ja) * | 2016-01-22 | 2017-07-27 | ヤンマー株式会社 | 農業用作業車両 |
JP6727944B2 (ja) * | 2016-06-17 | 2020-07-22 | 株式会社クボタ | 圃場走行経路生成システム及び圃場作業車 |
EP3549419B1 (en) * | 2016-12-02 | 2024-04-17 | Kubota Corporation | Travel route management system and travel route determination device |
CN107702716B (zh) * | 2017-08-31 | 2021-04-13 | 广州小鹏汽车科技有限公司 | 一种无人驾驶路径规划方法、系统和装置 |
CN107943072B (zh) * | 2017-11-13 | 2021-04-09 | 深圳大学 | 无人机飞行路径生成方法、装置、存储介质及设备 |
CN109540159B (zh) * | 2018-10-11 | 2020-11-27 | 同济大学 | 一种快速完备的自动驾驶轨迹规划方法 |
CN109670719A (zh) * | 2018-12-25 | 2019-04-23 | 雷沃重工股份有限公司 | 一种农业设备的作业规划方法、装置及电子设备 |
CN110658846B (zh) * | 2019-09-30 | 2023-05-02 | 广州极飞科技股份有限公司 | 路径规划方法、装置、电子设备及存储介质 |
CN111060108B (zh) * | 2019-12-31 | 2021-10-12 | 江苏徐工工程机械研究院有限公司 | 路径规划方法和装置、工程车辆 |
-
2020
- 2020-05-29 CN CN202010478768.5A patent/CN111750859B/zh active Active
Patent Citations (13)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
US5029088A (en) * | 1988-10-14 | 1991-07-02 | Tsubakimoto Chain Co. | Method of and apparatus for guiding a moving object |
US5615116A (en) * | 1990-02-05 | 1997-03-25 | Caterpillar Inc. | Apparatus and method for autonomous vehicle navigation using path data |
CN105751196A (zh) * | 2016-04-12 | 2016-07-13 | 华南理工大学 | 一种基于主从式的工业机器人协作作业方法 |
CN106643733A (zh) * | 2016-12-30 | 2017-05-10 | 北京理工大学 | 面向多导航站接力导航的运动体路径规划方法 |
WO2018143958A1 (en) * | 2017-01-31 | 2018-08-09 | Halliburton Energy Services, Inc. | Curvature-based feedback control techniques for directional drilling |
CN110300940A (zh) * | 2017-02-28 | 2019-10-01 | 本田技研工业株式会社 | 作业系统以及作业机 |
CN110383191A (zh) * | 2017-03-09 | 2019-10-25 | 洋马株式会社 | 路径生成系统 |
CN107168305A (zh) * | 2017-04-01 | 2017-09-15 | 西安交通大学 | 路口场景下基于Bezier和VFH的无人车轨迹规划方法 |
CN109496287A (zh) * | 2017-10-31 | 2019-03-19 | 深圳市大疆创新科技有限公司 | 可移动设备作业控制方法及装置、路径规划方法及装置 |
CN107918388A (zh) * | 2017-11-08 | 2018-04-17 | 深圳市招科智控科技有限公司 | 港口集装箱搬运无人车轨迹控制方法及装置 |
CN208329765U (zh) * | 2018-06-19 | 2019-01-04 | 方城龙昊液压设备有限责任公司 | 一种行走机构及无避让立体停车设备 |
CN109144097A (zh) * | 2018-08-15 | 2019-01-04 | 广州极飞科技有限公司 | 障碍物或地面识别及飞行控制方法、装置、设备及介质 |
CN110440823A (zh) * | 2019-08-27 | 2019-11-12 | 广州小鹏汽车科技有限公司 | 一种路径规划方法及路径规划系统 |
Non-Patent Citations (2)
Title |
---|
"多路径段平滑过渡的自适应前瞻位姿插补算法";王斌锐等;《控制与决策》;20190630;第34卷(第6期);1211-1218 * |
"小半径曲线上大直径盾构精准始发技术";张斌;《铁道建筑》;20171130;第57卷(第11期);58-61 * |
Also Published As
Publication number | Publication date |
---|---|
CN111750859A (zh) | 2020-10-09 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN110927740B (zh) | 一种移动机器人定位方法 | |
CN110609290B (zh) | 激光雷达匹配定位方法及装置 | |
US10614592B2 (en) | Three-dimensional posture estimating method and apparatus, device and computer storage medium | |
CN111750859B (zh) | 过渡路径规划方法及相关装置 | |
US10875178B2 (en) | Motion target direction angle obtaining method, apparatus and robot using the same | |
WO2020216315A1 (zh) | 一种参考行驶线快速生成方法、系统、终端和存储介质 | |
CN111766870B (zh) | 过渡路径和作业路径的规划方法及相关装置 | |
CN112729320A (zh) | 一种障碍物地图构建方法、装置、设备及存储介质 | |
CN111795699A (zh) | 无人车的路径规划方法、装置和计算机可读存储介质 | |
CN113866747A (zh) | 一种多激光雷达的标定方法及装置 | |
CN112154429B (zh) | 高精度地图定位方法、系统、平台及计算机可读存储介质 | |
CN115617042A (zh) | 碰撞检测方法及装置、终端设备、计算机可读存储介质 | |
CN112148033A (zh) | 无人机航线的确定方法、装置、设备及存储介质 | |
CN116339347B (zh) | 一种无人车运行路径规划方法、装置及设备 | |
CN115432007A (zh) | 用于车辆自动驾驶系统的碰撞检测方法、装置、电子设备 | |
CN116215517A (zh) | 碰撞检测方法、装置、设备、存储介质和自动驾驶车辆 | |
CN115542301A (zh) | 激光雷达外部参数的校准方法、装置、设备及存储介质 | |
CN114119684B (zh) | 基于四面体结构的标记点配准方法 | |
CN116934851A (zh) | 机器人定位方法、装置、设备及存储介质 | |
CN116430906B (zh) | 一种基于凸点平移的无人机动态避障方法、系统、设备及介质 | |
CN114429492A (zh) | 数据配准方法、电子设备及计算机存储介质 | |
CN112966059B (zh) | 针对定位数据的数据处理方法、装置、电子设备和介质 | |
CN114055449B (zh) | 高冗余度蛇形机械臂的末端跟随运动控制方法及装置 | |
Yang et al. | Polygon Formation in Distributed Multi-Agent Systems. | |
CN117109597B (zh) | 基于改进a星算法的无人机路径规划方法和装置 |
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 | ||
CB02 | Change of applicant information | ||
CB02 | Change of applicant information |
Address after: 510000 Block C, 115 Gaopu Road, Tianhe District, Guangzhou City, Guangdong Province Applicant after: Guangzhou Jifei Technology Co.,Ltd. Address before: 510000 Block C, 115 Gaopu Road, Tianhe District, Guangzhou City, Guangdong Province Applicant before: Guangzhou Xaircraft Technology Co.,Ltd. |
|
GR01 | Patent grant | ||
GR01 | Patent grant |