CN114791730A - 机器人路径规划方法、运行方法、机器人以及介质 - Google Patents
机器人路径规划方法、运行方法、机器人以及介质 Download PDFInfo
- Publication number
- CN114791730A CN114791730A CN202110106346.XA CN202110106346A CN114791730A CN 114791730 A CN114791730 A CN 114791730A CN 202110106346 A CN202110106346 A CN 202110106346A CN 114791730 A CN114791730 A CN 114791730A
- Authority
- CN
- China
- Prior art keywords
- cost value
- distance
- calculating
- reference track
- information
- 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.)
- Pending
Links
- 238000000034 method Methods 0.000 title claims abstract description 54
- 230000001133 acceleration Effects 0.000 claims description 85
- 230000006870 function Effects 0.000 claims description 72
- 238000004590 computer program Methods 0.000 claims description 15
- 230000007613 environmental effect Effects 0.000 claims description 14
- 230000008859 change Effects 0.000 claims description 13
- 238000004364 calculation method Methods 0.000 claims description 7
- 230000004888 barrier function Effects 0.000 claims description 2
- 230000008569 process Effects 0.000 description 12
- 238000004422 calculation algorithm Methods 0.000 description 3
- 238000010586 diagram Methods 0.000 description 3
- 230000000694 effects Effects 0.000 description 3
- 238000012163 sequencing technique Methods 0.000 description 2
- 230000004048 modification Effects 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
- 238000005457 optimization Methods 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
- G05D1/0223—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory involving speed control of the vehicle
-
- 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
Landscapes
- Engineering & Computer Science (AREA)
- Aviation & Aerospace Engineering (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Automation & Control Theory (AREA)
- Manipulator (AREA)
Abstract
本发明公开了一种机器人路径规划方法、运行方法、机器人以及介质,路径规划方法包括:对参考路径进行轨迹的初始化,得到参考轨迹,所述参考轨迹包括多个参考轨迹点;基于所述多个参考轨迹点的参数信息计算所述参考轨迹的代价值,所述参数信息包括每一参考轨迹点的坐标信息、角度信息以及相邻两个所述参考轨迹点的时间信息;对所述多个参考轨迹点的参数信息进行动态调整以使得所述代价值达到最优值。本发明通过构建约束,限制解空间,并且下一帧是基于上一帧的状态求解,因此计算得到的轨迹和上一时刻是连续且满足机器人物理运动能力的,从而提升机器人运动的流畅性。
Description
技术领域:
本发明涉及机器人运动控制技术领域,尤其涉及一种机器人路径规划方法、运行方法、机器人以及介质。
背景技术:
在机器人运行过程中,在机器人的拓扑路径上会出现一些障碍物,机器人会基于这些障碍物对应生成对应的避障路径,但是现有的避障路径往往没有考虑到机器人的运动特性。
发明内容:
本发明的目的在于提供一种机器人路径规划方法、运行方法、机器人以及介质,以解决现有技术生成的避障路径没有考虑到机器人的运动特性的问题。
本发明由如下技术方案实施:本发明第一方面提供一种机器人路径规划方法,包括以下步骤:
对参考路径进行轨迹的初始化,得到参考轨迹,所述参考轨迹包括多个参考轨迹点;
基于所述多个参考轨迹点的参数信息计算所述参考轨迹的代价值,所述参数信息包括每一参考轨迹点的坐标信息、角度信息以及相邻两个所述参考轨迹点的时间信息;
对所述多个参考轨迹点的参数信息进行动态调整以使得所述代价值达到最优值。
进一步的,所述基于所述多个参考轨迹点的参数信息计算所述参考轨迹的代价值,包括:
基于所述多个参考轨迹点的参数信息计算所述参考轨迹的速度代价值和/或加速度代价值和/或角速度代价值和/或时间代价值和/或动力学代价值和/或第一距离代价值和/或第二距离代价值;
计算所述速度代价值、加速度代价值、角速度代价值、时间代价值、动力学代价值、第一距离代价值、第二距离代价值中至少两个的和,并将所述和作为所述代价值。
进一步的,所述基于所述多个参考轨迹点的参数信息计算所述参考轨迹的速度代价值和/或加速度代价值和/或角速度代价值和/或时间代价值和/或动力学代价值和/或第一距离代价值和/或第二距离代价值,包括:
基于所述坐标信息和所述时间信息确定相邻两个所述参考轨迹点的线速度;
基于所述角度信息和所述时间信息确定相邻两个所述参考轨迹点的角速度;
基于所述线速度、所述角速度以及所述时间信息确定相邻两点所述参考轨迹点的加速度;
基于所述坐标信息计算每一所述参考轨迹点和环境障碍物的第一距离;
基于所述坐标信息计算每一所述参考轨迹点与所述参考路径的第二距离;
基于所述线速度来计算所述参考轨迹的速度代价值和/或基于所述角速度来计算所述参考轨迹的角速度代价值和/或基于所述加速度来计算所述参考轨迹的加速度代价值和/或基于所述第一距离来计算所述参考轨迹的第一距离代价值和/或基于所述第二距离来计算所述参考轨迹的第二距离代价值和/或基于所述时间信息来计算所述参考轨迹的时间代价值和/或基于所述坐标信息和角度信息计算所述参考轨迹的动力学代价值。
进一步的,所述基于所述线速度、角速度、加速度、第一距离、第二距离以及所述时间信息来计算所述参考轨迹的速度代价值和/或加速度代价值和/或角速度代价值和/或时间代价值和/或动力学代价值和/或第一距离代价值和/或第二距离代价值,包括:
采用速度代价函数计算多个所述线速度得到所述速度代价值;
采用加速度代价函数计算多个所述加速度得到所述加速度代价值;
采用角速度代价函数计算多个角速度得到所述角速度代价值;
采用第一距离代价函数计算所述第一距离得到所述第一距离代价值;
采用第二距离代价函数计算多个所述第二距离得到所述第二距离代价值;
采用时间代价函数计算所述多个时间得到时间距离代价值;
采用动力学代价函数计算所述坐标信息、角度信息得到动力学代价值。
进一步的,所述采用速度代价函数计算多个所述线速度得到所述速度代价值,包括:
判断所述线速度是否位于所述速度代价函数中规定的速度预设范围内;
如果是,则所述速度代价值为0;
如果不是,则计算所述线速度与所述速度预设范围的速度偏移量,并利用所述速度代价函数的规则来计算所述速度偏移量得到所述速度代价值;
所述采用加速度代价函数计算多个所述加速度得到所述加速度代价值,包括:
判断所述加速度是否位于所述加速度代价函数中规定的加速度预设范围内;
如果是,则所述加速度代价值为0;
如果不是,则计算所述加速度与所述加速度预设范围的加速度偏移量,并利用所述加速度代价函数的规则来计算所述加速度偏移量得到所述加速度代价值;
所述采用角速度代价函数计算多个角速度得到所述角速度代价值,包括:
判断所述角速度是否位于所述角速度代价函数中规定的角速度预设范围内;
如果是,则所述角速度代价值为0;
如果不是,则计算所述角速度与所述角速度预设范围的角速度偏移量,并利用所述角速度代价函数的规则来计算所述角速度偏移量得到所述角速度代价值;
所述采用第一距离代价函数计算所述第一距离得到所述第一距离代价值,包括:
判断所述第一距离是否位于所述第一距离代价函数中规定的第一距离预设范围内;
如果是,则所述第一距离代价值为0;
如果不是,则计算所述第一距离与所述第一距离预设范围的第一距离偏移量,并利用所述第一距离代价函数的规则来计算所述第一距离偏移量得到所述第一距离代价值;
所述采用第二距离代价函数计算多个所述第二距离得到所述第二距离代价值,包括:
判断所述第二距离是否位于所述第二距离代价函数中规定的第二距离预设范围内;
如果是,则所述第二距离代价值为0;
如果不是,则计算所述第二距离与所述第二距离预设范围的第二距离偏移量,并利用所述第二距离代价函数的规则来计算所述第二距离偏移量得到所述第二距离代价值。
进一步的,所述采用动力学代价函数计算所述坐标信息、角度信息得到动力学代价值,包括:
根据坐标信息计算得到相邻两个所述参考轨迹点的横坐标差值和竖坐标差值;
根据角度信息以及横坐标差值计算得到相邻两个所述参考轨迹点的横坐标变化信息;
根据角度信息以及竖坐标差值计算得到相邻两个所述参考轨迹点的竖坐标变化信息;
基于所述横坐标变化信息和所述竖坐标变化信息的差值确定所述动力学代价值。
进一步的,所述对所述多个参考轨迹点的参数信息进行动态调整以使得所述代价值达到最优值,包括:
动态对所述多个参考轨迹点的坐标信息进行调整,以获取使得所述代价值达到最小值的参考轨迹的坐标信息。
本发明第二方面提供一种机器人的运行方法,所述方法包括上述的机器人路径规划方法,还包括:
按照预设间隔获取对应时刻所述代价值达到最优值的参考轨迹;
根据所述参考轨迹获取运行信息;
基于所述运行信息控制机器人进行运行。
本发明第三方面提供一种机器人,所述机器人包括:
初始化模块,用于对参考路径进行轨迹的初始化,得到参考轨迹,所述参考轨迹包括多个参考轨迹点;
计算模块,用于基于所述多个参考轨迹点的参数信息计算所述参考轨迹的代价值,所述参数信息包括每一参考轨迹点的坐标信息、角度信息以及相邻两个所述参考轨迹点的时间信息;
调整模块,用于对所述多个参考轨迹点的参数信息进行动态调整以使得所述代价值达到最优值。
本发明第四方面提供一种机器人,包括存储器、处理器以及存储在所述存储器中并可在所述处理器上运行的计算机程序,其特征在于,所述处理器执行所述计算机程序时实现如上述机器人路径规划方法的步骤或所述机器人运行方法的步骤。
本发明第五方面提供一种计算机可读存储介质,所述计算机可读存储介质存储有计算机程序,其特征在于,所述计算机程序被处理器执行时实现如上述机器人路径规划方法的步骤或机器人运行方法的步骤。
本发明的优点:
本发明中只要机器人运动轨迹存在可行解,便可以得到一条满足要求的轨迹。从而在狭窄、复杂的区域能提升运行效果,提升流畅度,符合动力学。通过构建约束,限制解空间,并且同时下一帧是基于上一帧的状态求解,因此计算得到的轨迹和上一时刻是连续且满足机器人物理运动能力的,从而提升机器人运动的流畅性。
附图说明:
为了更清楚地说明本发明实施例或现有技术中的技术方案,下面将对实施例或现有技术描述中所需要使用的附图作简单地介绍,显而易见地,下面描述中的附图仅仅是本发明的一些实施例,对于本领域普通技术人员来讲,在不付出创造性劳动的前提下,还可以根据这些附图获得其他的附图。
图1为本发明实施例的一种机器人路径规划方法的流程图;
图2为本发明实施例的基于所述多个参考轨迹点的参数信息计算所述参考轨迹的代价值的方法具体流程图;
图3是本发明实施例提供的机器人的运行方法流程图;
图4是本发明实施例提供的机器人原理模块图;
图5是本发明实施例提供的机器人控制与存储架构示意图。
具体实施方式:
下面将结合本发明实施例中的附图,对本发明实施例中的技术方案进行清楚、完整地描述,显然,所描述的实施例仅仅是本发明一部分实施例,而不是全部的实施例。基于本发明中的实施例,本领域普通技术人员在没有作出创造性劳动前提下所获得的所有其他实施例,都属于本发明保护的范围。
实施例1
如图1所示,本发明提供一种机器人路径规划方法,该方法具体包括如下步骤。
S1,对参考路径进行轨迹的初始化,得到参考轨迹,所述参考轨迹包括多个参考轨迹点。
可选地,机器人在运行过程中,如果其拓扑路径(运行路径)上出现环境障碍物,则机器人会根据环境障碍物和拓扑路径对应生成一条参考路径,用于规避该环境障碍物并回到拓扑路径上继续运行,但是参考路径是一条简单的路径信息,没有包括具体的坐标信息。
可选地,可以先对参考路径进行归集的初始化,从而得到参考轨迹,该参考轨迹可以包括有多个参考轨迹点,每一参考轨迹均包括有参数信息,该参数信息具体为参考轨迹点的坐标信息、角度信息以及相邻两个参考轨迹点的时间信息。
在可选实施例中,参考路径是由多个栅格组成的,通过进行初始化,参考路径上对应的栅格则会形成一个参考轨迹点。具体地,将该栅格的中心点作为参考轨迹点,并将中心点的坐标作为参考轨迹点的坐标信息。角度信息则是相邻参考轨迹点之间的连线与整个坐标系的X轴或Y轴的夹角信息,是随着坐标信息的变化而变化的,时间信息则可以是提前进行预设的,如可以是0.3S,在其他实施例中,也可以说0.4S等等,这里不做限定。
S2,基于所述多个参考轨迹点的参数信息计算所述参考轨迹的代价值。
可选的,基于多个参考轨迹点的参数信息计算该参考轨迹的代价值。即可以通过整个参考轨迹所有的参考轨迹点的参考信息来对该参考轨迹的代价值进行计算。
请参阅图2,图2是图1步骤S2的子步骤流程示意图,具体包括如下步骤:
S21,基于多个参考轨迹点的参数信息计算参考轨迹的速度代价值和/或加速度代价值和/或角速度代价值和/或时间代价值和/或动力学代价值和/或第一距离代价值和/或第二距离代价值。
通过多个参考轨迹点的参数信息来计算该参考轨迹的速度代价值和/或加速度代价值和/或角速度代价值和/或时间代价值和/或动力学代价值和/或第一距离代价值和/或第二距离代价值。
具体地,可以通过坐标信息和时间信息确定两个相邻的参考轨迹点的线速度,即任意的两个相邻的参考轨迹点通过坐标信息可以计算其距离,随后通过距离与时间的比值可以得到对应的线速度。该线速度是指前一个参考轨迹点到当前参考轨迹点的线速度。通过对每两个相邻的参考轨迹点进行计算,则可以获取多个线速度。
类似的,可以通过相邻两个参考轨迹点的角度信息与时间信息的比值来确定对应的角速度。类似的,该角速度是指前一个参考轨迹到当前参考轨迹点的角速度。通过对每两个相邻的参考轨迹点进行计算,则可以获取多个角速度。
基于所述线速度、所述角速度和时间信息确定相邻两点所述参考轨迹点的加速度,可选地,加速度包括线加速度和角加速度,该线加速度通过相邻的两个线速度差与时间的比值获得,角速度通过相邻的两个角速度差与实际的比值获得。
基于坐标信息计算每一参考轨迹点和环境障碍物的第一距离。类似的,通过每一参考轨迹点的坐标信息,计算该参考轨迹点与环境障碍物的距离值,并将该距离值作为第一距离。
基于坐标信息计算每一参考轨迹点和参考路径的距离值,并将该距离值作为第二距离。
可选的,在获取到坐标信息、角度信息、时间信息、线速度信息、角速度信息、加速度信息、第一距离以及第二距离后,可以通过代价值的计算从而获得速度代价值、角速度代价值、加速度代价值、时间代价值、第一距离代价值、第二距离代价值以及动力学代价值。
在可选实施例中,可以通过线速度来计算参考轨迹的速度代价值,即采用速度代价函数来计算多个线速度得到该速度代价值。
可选的,基于机器人的运动特色,机器人能够运行的速度应该是在一个阈值范围内,例如小于1.2m/s,如果偏移这个速度,这表明这个速度的可行性很低,即达到的代价很高,因此,可以判断线速度是否是位于线速度代价函数中规定的速度预设范围内,如果是,则该速度代价值为0,如果不是,则计算该线速度和速度预设范围的速度偏移量,并利用速度代价函数的规则来计算所述速度偏移量得到所述速度代价值,可选的,如果该速度偏移量越大,则所得到的速度代价值也越大。
类似的,考虑到机器人所允许的加速度范围,也可以采用加速度代价函数计算多个加速度得到加速度代价值,具体表现为,判断加速度是否位于加速度代价函数中规定的加速度预设范围内,如果是,则加速度代价值为0,如果不是,则计算加速度与加速度预设范围的加速度偏移量,并利用加速度代价函数的规则来计算所述加速度偏移量得到所述加速度代价值,类似的,如果加速度偏移量越大,则加速度代价值也越大。
类似的,考虑到机器人的所允许的角速度范围,采用角速度代价函数来计算多个角速度得到角速度代价值,类似的,如果角速度位于角速度代价函数中规定的角速度预设范围内,则该角速度代价值为0,如果不是,则计算角速度与所述角速度预设范围的角速度偏移量,并利用角速度代价函数的规则来计算所述角速度偏移量得到所述角速度代价值。
在其他实施例中,考虑到参考路径点和环境障碍物的距离,如一般来说,参考路径点与环境障碍物越远,则表明该参考路径点越安全,即参考路径点离环境障碍物越远越好,如果小于某一个阈值,例如0.5m,则表面该参考路径点与环境障碍物可能比较容易相碰撞。基于该思路,则可以利用第一距离代价函数来计算第一距离得到第一距离代价值,具体也是判断该第一距离是否位于第一距离代价函数中规定的第一距离预设范围内,如果是,则第一距离代价值为0,如果不是,则计算第一距离与所述第一距离预设范围的第一距离偏移量,并利用第一距离代价函数的规则来计算所述第一距离偏移量得到所述第一距离代价值。
可选的,如果是多个环境障碍物,则选取与该参考轨迹点最近的一个环境障碍物的距离作为第一距离。
在其他考量中,还需要考虑到每一个参考轨迹点和原来的参考路径的距离,一般来说,参考轨迹点不能太过于偏离参考路径,即参考轨迹点应该离参考路径越近越好,类似的,采用第二距离代价函数计算多个所述第二距离得到所述第二距离代价值,判断第二距离是否位于第二距离代价函数中规定的第二距离预设范围内;如果是,则第二距离代价值为0;如果不是,则计算第二距离与所述第二距离预设范围的第二距离偏移量,并利用第二距离代价函数的规则来计算所述第二距离偏移量得到所述第二距离代价值。
还有包括时间上的考量,在本实施例中,时间信息也是预设,即为0.3S,在其他实施例中,时间信息也可以是机器人通过预设算法计算得到的,即每相邻的两个参考轨迹点的时间信息是不同的,因此还需要考虑到时间,即不能过长也不能过短。类似的,采用时间代价函数计算多个所述时间信息得到所述时间代价值,具体为判断事件是否位于时间代价函数中规定的时间预设范围内;如果是,则时间代价值为0;如果不是,则计算时间信息与所述时间预设范围的时间偏移量,并利用时间代价函数的规则来计算所述时间偏移量得到所述时间代价值。
在其他场景上,还需要着重考虑到整个参考轨迹的平滑性,即处于机器人运行的特色,更希望机器人是以弧形段来运行,而非多个直线段所组成的运行轨迹。此时则需要考虑到参考轨迹的动力学代价值,具体可以采用动力学代价函数来计算坐标信息、角速度信息得到动力代价值,具体可以通过坐标信息来计算相邻的两个参考轨迹点的横坐标差值和竖坐标差值,随后根据加速度信息以及横坐标差值计算得到向量两个参考轨迹点的横坐标变化信息,根据角速度信息以及竖坐标差值计算得到向量两个参考轨迹点的竖坐标变化信息,随后基于所述横坐标变化信息和所述竖坐标变化信息的差值确定所述动力学代价值。
具体地,通过如下公式进行计算:
dx=p2.x-p1.x;
dy=p2.y-p1.y;
c1=[cos(p1.t)+cos(p2.t)]*dy;
c2=[sin(p1.t)+sin(p2.t)]*dx;
d=fabs(c1-c2);
cost=f(d)。
其中,dx是指横坐标变化信息,p1和p2是指相邻的两个参考轨迹点,p2.x是指参考轨迹点p2的横坐标,p1.x是指参考轨迹点p1的横坐标,dy是指竖坐标变化信息,p2.y是指参考轨迹点p2的竖坐标,p1.y是指参考轨迹点p1的竖坐标,p1.t则是指参考轨迹点p1的角度信息,p2.t则是指参考轨迹点p2的角度信息。cost则是动力学代价值,其和d成正相关的关系,即d越大则cost越大。
S22,计算所述速度代价值、加速度代价值、角速度代价值、时间代价值、动力学代价值、第一距离代价值、第二距离代价值中至少两个的和,并将所述和作为所述代价值。
在获取到速度代价值、加速度代价值、角速度代价值、时间代价值、动力学代价值、第一距离代价值、第二距离代价值后,则可以选取至少任意两个的和作为代价值。
在最优选的实施例中,则将速度代价值、加速度代价值、角速度代价值、时间代价值、动力学代价值、第一距离代价值、第二距离代价值的和作为代价值。
可选的,每一个代价值均是对参考轨迹的一个约束,在考虑到多个代价值后,对参考轨迹的约束也会越来越大。
S3,对多个参考轨迹点的参数信息进行动态调整以使得代价值达到最优值。
考虑到多个因素(反映在各个代价值)对参考轨迹的影响后,可以对参考轨迹进行动态调整,具体是对多个参考轨迹点的参考信息进行动态调整,从而使得该参考轨迹的代价值趋向于优化,即代价值变小,直到该代价值达到最优(代价值达到可实现的最小值)。
具体地,对多个参考轨迹点的坐标信息进行调整,随后基于调整后的坐标信息,重新计算整个参考轨迹的代价值,一直循环该过程,直到获取代价值达到最小值的参考轨迹的坐标信息。随后这些坐标信息即为新的参考轨迹。
上述实施例中,通过速度代价值、加速度代价值、角速度代价值、时间代价值、动力学代价值、第一距离代价值、第二距离代价值等不同考量的因素,一起来对参考轨迹来进行约束,并在对参考轨迹的调整过程中,使得参考轨迹逐渐趋向于达到一个最优的情况,即综合性最好的情况。
如图3,本发明还提供一种机器人的运行方法,该方法包括上述实施例中任一项所述的机器人的路径规划方法,且具体还包括如下步骤:
S4,按照预设间隔获取对应时刻所述代价值达到最优值的参考轨迹。
按照预设间隔获取对应所述代价值达到最优值的参考轨迹,例如每隔0.1S,则获取对应时刻的衣柜最优参考轨迹。
S5,根据所述参考轨迹获取运行信息。
随后根据参考轨迹获取运行信息,这里的运行信息具体是指该参考轨迹对应的速度、加速度以及时间等等信息。
S6,基于所述运行信息控制机器人进行运行。
随后基于运行信息控制机器人进行运行。即将速度、加速度以及时间等信息下发给下位机,从而使得机器人的电机输出对应的扭矩,并带动机器人可以按照参考轨迹的路径进行运行。
在可选场景中,机器人在整个运行过程中,会按照预设的时间间隔计算一次最优的参考轨迹,并生成对应的运行信息控制机器人进行运行。如当计算出一条最优的参考轨迹后,机器人基于运行信息控制运行在该参考轨迹的第一个参考轨迹点到第二个参考轨迹点的过程中,机器人又会计算出新的一个最优参考轨迹,并重新下发运行信息,控制机器人进行运动,从而可以保持机器人在整个运行过程中,一直会寻求当前最优的一个参考轨迹,而不是仅仅基于一个参考轨迹进行运行。从而可以使得真个机器人的运行过程中流畅,并可以在复杂的多障碍物环境中实时进行调整运行,从而达到良好的运行效果。
实施例2
本实施例提供一种机器人路径规划方法,包括以下步骤:
对参考路径进行轨迹的初始化,得到参考轨迹;
计算参考轨迹的参数信息,参数信息至少包括轨迹点速度、加速度和每一个轨迹点与最近障碍物的距离;
根据实时环境,构建代价函数,代价函数包含对参数信息的约束,通过参数信息的约束,计算求解出一条平滑的符合机器人运动能力的轨迹;
下一时刻,基于上一时刻的轨迹和当前时刻的环境,重新构建基于参数信息的约束,再次求解得到可运行的新的轨迹,之后依次循环,得出整个平滑的符合机器人运动能力的轨迹,从而保证轨迹点多帧之间的连续。
其中,对参考路径进行轨迹的初始化,得到参考轨迹,具体为:根据参考路径的相邻两个路径点(x1,y1)、(x2,y2),得到相邻两个路径点之间的角度theta,其中theta=arctan(y2-y1)/(x2-x1),再设定相邻两个路径点的运动时间dt,得到参考轨迹基本信息(x,y,theta,dt),比如,可以假设dt=0.25s。因此路径只包含坐标,轨迹在此基础上包含坐标以及时间,对初始轨迹进行初始化可以避免后续机器人控制算法优化过程中的局部最优解导致机器卡顿的问题。
其中,计算轨迹点速度具体为:通过任意2个相邻轨迹点的距离ds,以及运动时间dt,计算2点之间的速度v=ds/dt,其中,所得到的v满足机器人的运动性能,即v(min)<v<v(max),其中v(min)、v(max)机器人运动性能上下限。
其中,计算轨迹点加速度具体为:通过任意3个相邻轨迹点,计算前2个相邻点的距离为ds1,后2个相邻点的距离为ds2,则计算加速度a=((ds2/dt)-(ds1/dt))/dt,所得到的的a满足a(min)<a<a(max),其中a(min)、a(max)为机器人运动性能上下限。
其中,计算每一个轨迹点与最近障碍物的距离ds,满足ds>d,其中d为期望的机器人与障碍物的最近距离。先遍历所有障碍物,计算轨迹点距离每个障碍物的距离ds1、ds2……dsN后,然后通过排序算法排序后,得到最近障碍物的距离ds。
其中,通过参数信息的约束,计算求解出一条平滑的符合机器人运动能力的轨迹,具体为:将参数信息轨迹点速度v、加速度a和每一个轨迹点与最近障碍物的距离ds,使用工具g2o进行求解,求解的结果为每一个轨迹点的坐标。g2o是一个开源的库,把上述约束输入进去,通过这个g2o工具,就能计算得到轨迹点坐标。可以理解成,轨迹点(x,y,theta,dt)为待求解变量,g2o是一个求解多方程式的工具,把参数信息轨迹点速度v、加速度a和每一个轨迹点与最近障碍物的距离ds输入方程式,就能求解出轨迹点(x,y,theta,dt)。后续通过判断,来检查求解的轨迹是否满足设定的所有约束。
综上,本实施例的路径规划方法,是一个求解最优值得过程,而非传统的状态空间搜索过程。因此只要存在可行解,便可以得到一条满足要求的轨迹。从而在狭窄、复杂的区域能提升运行效果,提升流畅度,符合动力学。通过构建约束,限制解空间,并且同时下一帧是基于上一帧的状态求解,因此计算得到的轨迹和上一时刻是连续且满足机器人物理运动能力的,从而提升机器人运动的流畅性。
实施例3
如图4所示,本发明还提供一种机器人,所述机器人包括:
初始化模块101,用于对参考路径进行轨迹的初始化,得到参考轨迹,所述参考轨迹包括多个参考轨迹点;
计算模块102,用于基于所述多个参考轨迹点的参数信息计算所述参考轨迹的代价值,所述参数信息包括每一参考轨迹点的坐标信息、角度信息以及相邻两个所述参考轨迹点的时间信息;
调整模块103,用于对所述多个参考轨迹点的参数信息进行动态调整以使得所述代价值达到最优值。
图5是本发明一实施例提供的机器人的控制与存储系统架构图。如图5所示,该实施例的机器人6包括:处理器60、存储器61以及存储在所述存储器61中并可在所述处理器60上运行的计算机程序62,例如路径规划方法程序。所述处理器60执行所述计算机程序62时实现上述各个机器人路径规划方法实施例中的步骤,例如图1所示的步骤S1至S3。或者,所述处理器60执行所述计算机程序62时实现上述各装置实施例中各模块/单元的功能,例如图4所示模块101至103的功能。
示例性的,所述计算机程序62可以被分割成一个或多个模块/单元,所述一个或者多个模块/单元被存储在所述存储器61中,并由所述处理器60执行,以完成本发明。所述一个或多个模块/单元可以是能够完成特定功能的一系列计算机程序指令段,该指令段用于描述所述计算机程序62在所述机器人6中的执行过程。例如,所述计算机程序62可以被分割成初始化模块101、计算模块102、调整模块103。
所述机器人6可以是桌上型计算机、笔记本、掌上电脑及云端服务器等计算设备。所述机器人可包括,但不仅限于,处理器60、存储器61。本领域技术人员可以理解,图5仅仅是机器人6的示例,并不构成对机器人6的限定,可以包括比图示更多或更少的部件,或者组合某些部件,或者不同的部件,例如所述机器人还可以包括输入输出设备、网络接入设备、总线等。
所称处理器60可以是中央处理单元(Central Processing Unit,CPU),还可以是其它通用处理器、数字信号处理器(Digital Signal Processor,DSP)、专用集成电路(Application Specific Integrated Circuit,ASIC)、现场可编程门阵列(Field-Programmable Gate Array,FPGA)或者其它可编程逻辑器件、分立门或者晶体管逻辑器件、分立硬件组件等。通用处理器可以是微处理器或者该处理器也可以是任何常规的处理器等。
所述存储器61可以是所述机器人6的内部存储单元,例如机器人6的硬盘或内存。所述存储器61也可以是所述机器人6的外部存储设备,例如所述机器人6上配备的插接式硬盘,智能存储卡(Smart Media Card,SMC),安全数字(Secure Digital,SD)卡,闪存卡(Flash Card)等。进一步地,所述存储器61还可以既包括所述机器人6的内部存储单元也包括外部存储设备。所述存储器61用于存储所述计算机程序以及所述机器人所需的其它程序和数据。所述存储器61还可以用于暂时地存储已经输出或者将要输出的数据。
所属领域的技术人员可以清楚地了解到,为了描述的方便和简洁,仅以上述各功能单元、模块的划分进行举例说明,实际应用中,可以根据需要而将上述功能分配由不同的功能单元、模块完成,即将所述装置的内部结构划分成不同的功能单元或模块,以完成以上描述的全部或者部分功能。实施例中的各功能单元、模块可以集成在一个处理单元中,也可以是各个单元单独物理存在,也可以两个或两个以上单元集成在一个单元中,上述集成的单元既可以采用硬件的形式实现,也可以采用软件功能单元的形式实现。另外,各功能单元、模块的具体名称也只是为了便于相互区分,并不用于限制本申请的保护范围。上述系统中单元、模块的具体工作过程,可以参考前述方法实施例中的对应过程,在此不再赘述。
以上所述仅为本发明的较佳实施例而已,并不用以限制本发明,凡在本发明的精神和原则之内,所作的任何修改、等同替换、改进等,均应包含在本发明的保护范围之内。
Claims (11)
1.一种机器人路径规划方法,其特征在于,包括以下步骤:
对参考路径进行轨迹的初始化,得到参考轨迹,所述参考轨迹包括多个参考轨迹点;
基于所述多个参考轨迹点的参数信息计算所述参考轨迹的代价值,所述参数信息包括每一参考轨迹点的坐标信息、角度信息以及相邻两个所述参考轨迹点的时间信息;
对所述多个参考轨迹点的参数信息进行动态调整以使得所述代价值达到最优值。
2.根据权利要求1所述的路径规划方法,其特征在于,
所述基于所述多个参考轨迹点的参数信息计算所述参考轨迹的代价值,包括:
基于所述多个参考轨迹点的参数信息计算所述参考轨迹的速度代价值和/或加速度代价值和/或角速度代价值和/或时间代价值和/或动力学代价值和/或第一距离代价值和/或第二距离代价值;
计算所述速度代价值、加速度代价值、角速度代价值、时间代价值、动力学代价值、第一距离代价值、第二距离代价值中至少两个的和,并将所述和作为所述代价值。
3.根据权利要求2所述的路径规划方法,其特征在于,所述基于所述多个参考轨迹点的参数信息计算所述参考轨迹的速度代价值和/或加速度代价值和/或角速度代价值和/或时间代价值和/或动力学代价值和/或第一距离代价值和/或第二距离代价值,包括:
基于所述坐标信息和所述时间信息确定相邻两个所述参考轨迹点的线速度;
基于所述角度信息和所述时间信息确定相邻两个所述参考轨迹点的角速度;
基于所述线速度、所述角速度以及所述时间信息确定相邻两点所述参考轨迹点的加速度;
基于所述坐标信息计算每一所述参考轨迹点和环境障碍物的第一距离;
基于所述坐标信息计算每一所述参考轨迹点与所述参考路径的第二距离;
基于所述线速度来计算所述参考轨迹的速度代价值和/或基于所述角速度来计算所述参考轨迹的角速度代价值和/或基于所述加速度来计算所述参考轨迹的加速度代价值和/或基于所述第一距离来计算所述参考轨迹的第一距离代价值和/或基于所述第二距离来计算所述参考轨迹的第二距离代价值和/或基于所述时间信息来计算所述参考轨迹的时间代价值和/或基于所述坐标信息和角度信息计算所述参考轨迹的动力学代价值。
4.根据权利要求3所述的路径规划方法,其特征在于,所述基于所述线速度、角速度、加速度、第一距离、第二距离以及所述时间信息来计算所述参考轨迹的速度代价值和/或加速度代价值和/或角速度代价值和/或时间代价值和/或动力学代价值和/或第一距离代价值和/或第二距离代价值,包括:
采用速度代价函数计算多个所述线速度得到所述速度代价值;
采用加速度代价函数计算多个所述加速度得到所述加速度代价值;
采用角速度代价函数计算多个角速度得到所述角速度代价值;
采用第一距离代价函数计算所述第一距离得到所述第一距离代价值;
采用第二距离代价函数计算多个所述第二距离得到所述第二距离代价值;
采用时间代价函数计算所述多个时间得到时间距离代价值;
采用动力学代价函数计算所述坐标信息、角度信息得到动力学代价值。
5.根据权利要求4所述的路径规划方法,其特征在于,
所述采用速度代价函数计算多个所述线速度得到所述速度代价值,包括:
判断所述线速度是否位于所述速度代价函数中规定的速度预设范围内;
如果是,则所述速度代价值为0;
如果不是,则计算所述线速度与所述速度预设范围的速度偏移量,并利用所述速度代价函数的规则来计算所述速度偏移量得到所述速度代价值;
所述采用加速度代价函数计算多个所述加速度得到所述加速度代价值,包括:
判断所述加速度是否位于所述加速度代价函数中规定的加速度预设范围内;
如果是,则所述加速度代价值为0;
如果不是,则计算所述加速度与所述加速度预设范围的加速度偏移量,并利用所述加速度代价函数的规则来计算所述加速度偏移量得到所述加速度代价值;
所述采用角速度代价函数计算多个角速度得到所述角速度代价值,包括:
判断所述角速度是否位于所述角速度代价函数中规定的角速度预设范围内;
如果是,则所述角速度代价值为0;
如果不是,则计算所述角速度与所述角速度预设范围的角速度偏移量,并利用所述角速度代价函数的规则来计算所述角速度偏移量得到所述角速度代价值;
所述采用第一距离代价函数计算所述第一距离得到所述第一距离代价值,包括:
判断所述第一距离是否位于所述第一距离代价函数中规定的第一距离预设范围内;
如果是,则所述第一距离代价值为0;
如果不是,则计算所述第一距离与所述第一距离预设范围的第一距离偏移量,并利用所述第一距离代价函数的规则来计算所述第一距离偏移量得到所述第一距离代价值;
所述采用第二距离代价函数计算多个所述第二距离得到所述第二距离代价值,包括:
判断所述第二距离是否位于所述第二距离代价函数中规定的第二距离预设范围内;
如果是,则所述第二距离代价值为0;
如果不是,则计算所述第二距离与所述第二距离预设范围的第二距离偏移量,并利用所述第二距离代价函数的规则来计算所述第二距离偏移量得到所述第二距离代价值。
6.根据权利要求4所述的路径规划方法,其特征在于,
所述采用动力学代价函数计算所述坐标信息、角度信息得到动力学代价值,包括:
根据坐标信息计算得到相邻两个所述参考轨迹点的横坐标差值和竖坐标差值;
根据角度信息以及横坐标差值计算得到相邻两个所述参考轨迹点的横坐标变化信息;
根据角度信息以及竖坐标差值计算得到相邻两个所述参考轨迹点的竖坐标变化信息;
基于所述横坐标变化信息和所述竖坐标变化信息的差值确定所述动力学代价值。
7.根据权利要求1所述的路径规划方法,其特征在于,所述对所述多个参考轨迹点的参数信息进行动态调整以使得所述代价值达到最优值,包括:
动态对所述多个参考轨迹点的坐标信息进行调整,以获取使得所述代价值达到最小值的参考轨迹的坐标信息。
8.一种机器人的运行方法,其特征在于,所述方法包括权利要求1-7中任一项所述的机器人路径规划方法,还包括:
按照预设间隔获取对应时刻所述代价值达到最优值的参考轨迹;
根据所述参考轨迹获取运行信息;
基于所述运行信息控制机器人进行运行。
9.一种机器人,其特征在于,所述机器人包括:
初始化模块,用于对参考路径进行轨迹的初始化,得到参考轨迹,所述参考轨迹包括多个参考轨迹点;
计算模块,用于基于所述多个参考轨迹点的参数信息计算所述参考轨迹的代价值,所述参数信息包括每一参考轨迹点的坐标信息、角度信息以及相邻两个所述参考轨迹点的时间信息;
调整模块,用于对所述多个参考轨迹点的参数信息进行动态调整以使得所述代价值达到最优值。
10.一种机器人,包括存储器、处理器以及存储在所述存储器中并可在所述处理器上运行的计算机程序,其特征在于,所述处理器执行所述计算机程序时实现如权利要求1至7任一项所述机器人路径规划方法的步骤或权利要求8所述机器人运行方法的步骤。
11.一种计算机可读存储介质,所述计算机可读存储介质存储有计算机程序,其特征在于,所述计算机程序被处理器执行时实现如权利要求1至7任一项所述机器人路径规划方法的步骤或权利要求8所述的机器人运行方法的步骤。
Priority Applications (2)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202110106346.XA CN114791730A (zh) | 2021-01-26 | 2021-01-26 | 机器人路径规划方法、运行方法、机器人以及介质 |
PCT/CN2022/073496 WO2022161315A1 (zh) | 2021-01-26 | 2022-01-24 | 机器人路径规划方法、运行方法、机器人以及介质 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202110106346.XA CN114791730A (zh) | 2021-01-26 | 2021-01-26 | 机器人路径规划方法、运行方法、机器人以及介质 |
Publications (1)
Publication Number | Publication Date |
---|---|
CN114791730A true CN114791730A (zh) | 2022-07-26 |
Family
ID=82460668
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN202110106346.XA Pending CN114791730A (zh) | 2021-01-26 | 2021-01-26 | 机器人路径规划方法、运行方法、机器人以及介质 |
Country Status (2)
Country | Link |
---|---|
CN (1) | CN114791730A (zh) |
WO (1) | WO2022161315A1 (zh) |
Families Citing this family (2)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN116372927B (zh) * | 2023-04-06 | 2024-03-22 | 深圳市康士达科技有限公司 | 一种机器人示教的轨迹生成方法、装置、设备和介质 |
CN116449767B (zh) * | 2023-04-12 | 2023-09-15 | 武汉金顿激光科技有限公司 | 一种半自动激光清洗轨迹修正方法及系统 |
Family Cites Families (4)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
US8700190B2 (en) * | 2011-08-05 | 2014-04-15 | Mitsubishi Electric Research Labs. | Method for generating trajectories for motor controlled actuators |
CN106774329B (zh) * | 2016-12-29 | 2019-08-13 | 大连理工大学 | 一种基于椭圆切线构造的机器人路径规划方法 |
CN110375753B (zh) * | 2019-07-04 | 2022-04-29 | 丰图科技(深圳)有限公司 | 地图匹配方法、装置、服务器及存储介质 |
CN111121812B (zh) * | 2019-12-31 | 2022-04-08 | 达闼机器人有限公司 | 一种路径优化方法、电子设备及存储介质 |
-
2021
- 2021-01-26 CN CN202110106346.XA patent/CN114791730A/zh active Pending
-
2022
- 2022-01-24 WO PCT/CN2022/073496 patent/WO2022161315A1/zh active Application Filing
Also Published As
Publication number | Publication date |
---|---|
WO2022161315A1 (zh) | 2022-08-04 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN107980108B (zh) | 机器人运动轨迹规划方法及相关装置 | |
CN106651987B (zh) | 路径规划方法及装置 | |
CN107980109B (zh) | 机器人运动轨迹规划方法及相关装置 | |
WO2017138664A1 (en) | Method for controlling motion of vehicle and control system of vehicle | |
CN114791730A (zh) | 机器人路径规划方法、运行方法、机器人以及介质 | |
EP4296133A1 (en) | Intelligent driving method and apparatus, and storage medium and computer program | |
EP4134285A1 (en) | Vehicle obstacle avoidance method and apparatus, electronic device, and storage medium | |
WO2020098551A1 (zh) | 机器人运行方法、装置、机器人、电子设备及可读介质 | |
CN114647248B (zh) | 机器人避障方法、电子设备及可读存储介质 | |
CN110238841B (zh) | 障碍物的避让方法及装置 | |
JP7197550B2 (ja) | ビジュアルローカリゼーションとオドメトリに基づく経路追跡方法およびシステム | |
Yang et al. | Generation of dynamically feasible and collision free trajectory by applying six-order Bezier curve and local optimal reshaping | |
CN113110423B (zh) | 步态轨迹规划方法、装置、计算机可读存储介质及机器人 | |
CN113960995A (zh) | 一种避障规划方法、系统及设备 | |
Ahmad et al. | 3D safe and intelligent trajectory generation for multi-axis machine tools using machine vision | |
CN112540602A (zh) | 一种机器人局部路径的优化方法及机器人 | |
CN112540609A (zh) | 路径规划方法、装置、终端设备及存储介质 | |
CN114690767A (zh) | 一种机器人轨迹规划方法、系统以及机器人 | |
Xiong et al. | Surrounding vehicle trajectory prediction and dynamic speed planning for autonomous vehicle in cut-in scenarios | |
Jansch-Porto et al. | Decentralized control with moving-horizon linear switched systems: Synthesis and testbed implementation | |
CN113495565A (zh) | 一种机器人及其轨迹规划和平滑过渡方法、系统及介质 | |
CN112506203B (zh) | 一种机器人运动动态反馈方法及系统 | |
Fujii et al. | Realtime trajectory smoothing with neural nets | |
Wyffels et al. | Priority-based tracking of extended objects | |
Zhang et al. | Intelligent path planning by an improved RRT algorithm with dual grid map |
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 |