CN114415670B - 一种局部路径规划方法、装置、机器人及存储介质 - Google Patents
一种局部路径规划方法、装置、机器人及存储介质 Download PDFInfo
- Publication number
- CN114415670B CN114415670B CN202111623005.6A CN202111623005A CN114415670B CN 114415670 B CN114415670 B CN 114415670B CN 202111623005 A CN202111623005 A CN 202111623005A CN 114415670 B CN114415670 B CN 114415670B
- Authority
- CN
- China
- Prior art keywords
- path
- speed
- local
- paths
- robot
- 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
- 238000000034 method Methods 0.000 title claims abstract description 96
- 238000005070 sampling Methods 0.000 claims description 46
- 238000010606 normalization Methods 0.000 claims description 22
- 230000001133 acceleration Effects 0.000 claims description 17
- 230000007613 environmental effect Effects 0.000 claims description 8
- 238000004590 computer program Methods 0.000 claims description 3
- 230000011218 segmentation Effects 0.000 claims description 3
- 238000010586 diagram Methods 0.000 description 12
- 230000006870 function Effects 0.000 description 9
- 230000033001 locomotion Effects 0.000 description 8
- 238000012545 processing Methods 0.000 description 8
- 238000004422 calculation algorithm Methods 0.000 description 6
- 230000008569 process Effects 0.000 description 6
- 238000004364 calculation method Methods 0.000 description 4
- 238000004891 communication Methods 0.000 description 4
- 238000011156 evaluation Methods 0.000 description 4
- 241000282414 Homo sapiens Species 0.000 description 2
- 238000003491 array Methods 0.000 description 2
- 230000009286 beneficial effect Effects 0.000 description 2
- 239000000463 material Substances 0.000 description 2
- 238000013473 artificial intelligence Methods 0.000 description 1
- 230000008901 benefit Effects 0.000 description 1
- 238000013500 data storage Methods 0.000 description 1
- 238000011161 development Methods 0.000 description 1
- 230000018109 developmental process Effects 0.000 description 1
- 238000005516 engineering process Methods 0.000 description 1
- 238000012854 evaluation process Methods 0.000 description 1
- 230000003203 everyday effect Effects 0.000 description 1
- 230000003993 interaction Effects 0.000 description 1
- 238000010295 mobile communication Methods 0.000 description 1
- 230000003287 optical effect Effects 0.000 description 1
- 230000008707 rearrangement Effects 0.000 description 1
- 238000011160 research Methods 0.000 description 1
- 238000004088 simulation Methods 0.000 description 1
- 238000006467 substitution reaction Methods 0.000 description 1
- 238000010408 sweeping Methods 0.000 description 1
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/0221—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory involving a learning process
-
- 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
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)
- Numerical Control (AREA)
Abstract
本发明公开了一种局部路径规划方法、装置、机器人及存储介质。动态窗口法预测机器人局部路径的预测时长切分为至少两个子段,在每一子段内,以前一子段内的每一路径的终点为起点,采用动态窗口法生成至少两条新的路径,对各子段的路径组合成的多条局部路径进行评价,从多条局部路径中选择得分最高的局部路径作为目标路径。通过在每一子段内,以前一子段内的每一路径的终点为起点,采用动态窗口法生成至少两条新的路径,每个子段内的路径的数量相对于前一子段内的路径的数量成倍数增加,增加了预测时长内局部路径的总数量,提高了路径末端的空间覆盖率,从而能够选择出最符合实际环境的最优路径,提高机器人的避障性能,提高机器人的运行安全性。
Description
技术领域
本发明实施例涉及机器人技术,尤其涉及一种局部路径规划方法、装置、机器人及存储介质。
背景技术
移动机器人是一种智能控制移动以进行各种任务的设备。随着人工智能的不断发展,移动机器人已经逐渐能够代替人类做一部分工作,相比人类而言,机器人具有能长时间、高效率工作的优势。移动机器人,尤其是服务型的移动机器人,例如,扫地机器人、送货机器人等,在日常生活中逐渐变得很常见。
在移动机器人的技术领域中,规划控制是一个重要的研究领域,较为常用的规划算法有A*算法、D*算法、DWA(动态窗口法)算法等。DWA算法的原理主要是对速度空间采样多组速度,并仿真这些速度在一定时间的运动轨迹,将这些运动轨迹通过多层评价函数进行打分,从而输出最优速度样本。
现有的DWA算法存在机器人避障性能较差的问题,导致机器人运行安全性差。
发明内容
本发明提供一种局部路径规划方法、装置、机器人及存储介质,以提高路径末端的空间覆盖率,提高机器人的避障性能。
第一方面,本发明实施例提供了一种局部路径规划方法,包括:
确定动态窗口法预测机器人局部路径的预测时长;
将所述预测时长切分为至少两个子段;
在第i个所述子段内,针对第i-1个所述子段的至少两条第一路径中的每一条所述第一路径,以所述第一路径的终点为起点,采用动态窗口法生成至少两条第二路径;
对所述第一路径和所述第二路径组成的多条局部路径进行评价,从多条所述局部路径中选择得分最高的局部路径作为目标路径。
可选的,各所述子段的时长相等。
可选的,在第i个所述子段内,针对第i-1个所述子段的至少两条第一路径中的每一条所述第一路径,以所述第一路径的终点为起点,采用动态窗口法生成至少两条第二路径,包括:
针对第i-1个所述子段的至少两条第一路径中的每一条所述第一路径,基于所述机器人自身参数和环境参数的制约确定所述机器人在所述第一路径的终点时的速度采样窗口;
在第i个所述子段内,在所述速度采样窗口内选取至少两个目标速度,所述目标速度包括线速度和角速度;
基于所述机器人在所述第一路径的终点的位姿和所述至少两个目标速度,生成以所述第一路径的终点为起点的至少两条第二路径。
可选的,针对第i-1个所述子段的至少两条第一路径中的每一条所述第一路径,基于所述机器人自身参数和环境参数的制约确定所述机器人在所述第一路径的终点时的速度采样窗口,包括:
基于所述机器人的最大速度限制和最小速度限制确定第一速度空间;
基于所述机器人在所述第一路径的终点的速度和所述机器人的驱动电机的加速度限制确定第二速度空间;
基于所述机器人采集到的障碍物信息和所述机器人的驱动电机的加速度限制确定第三速度空间;
计算所述第一速度空间、所述第二速度空间和所述第三速度空间的交集,得到在所述第一路径的终点时的速度采样窗口。
可选的,在第i个所述子段内,在所述速度采样窗口内选取至少两个目标速度,包括:
获取所述机器人在所述第一路径的终点的位姿和速度;
创建至少二维的速度调整向量;
将所述速度调整向量与所述机器人在所述第一路径的终点的速度相乘,得到至少两个调整速度;
判断所述至少两个调整速度是否均属于所述速度采样空间;
若所述至少两个调整速度均属于所述速度采样空间,则将所述调整速度作为目标速度。
可选的,创建至少二维的速度调整向量,包括:
创建三维的速度调整向量,其中,所述速度调整向量中的每维元素包括线速度调整参数和角速度调整参数,所述线速度调整参数为1,三个所述角速度调整参数中其中两个为绝对值大于1的相反数,另一个为1。
可选的,对所述第一路径和所述第二路径组成的多条局部路径进行评价,从多条所述局部路径中选择得分最高的局部路径作为目标路径,包括:
针对每一所述局部路径,计算在所述局部路径终点时所述机器人的朝向与目标之间的角度差距;
针对每一所述局部路径,计算所述机器人在所述局部路径上与最近的障碍物之间的距离;
针对每一所述局部路径,确定所述局部路径对应的速度;
基于所述局部路径对应的所述角度差距、所述距离和所述速度计算所述局部路径的得分;
将得分最高的所述局部路径作为目标路径。
可选的,基于所述局部路径对应的所述角度差距、所述距离和所述速度计算所述局部路径的得分,包括:
对所有所述局部路径对应的所述角度差距进行归一化处理,得到各所述局部路径对应的角度差距归一化值;
对所有所述局部路径对应的所述距离进行归一化处理,得到各所述局部路径对应的距离归一化值;
对所有所述局部路径对应的所述速度进行归一化处理,得到各所述局部路径对应的速度归一化值;
针对每一所述局部路径,为所述局部路径对应的角度差距归一化值、距离归一化值和速度归一化值分别赋予不同的权重,并相加,得到所述局部路径的得分。
第二方面,本发明实施例还提供了一种局部路径规划装置,包括:
预测时长确定模块,用于确定动态窗口法预测机器人局部路径的预测时长;
切分模块,用于将所述预测时长切分为至少两个子段;
路径生成模块,用于在第i个所述子段内,针对第i-1个所述子段的至少两条第一路径中的每一条所述第一路径,以所述第一路径的终点为起点,采用动态窗口法生成至少两条第二路径;
目标路径确定模块,用于对所述第一路径和所述第二路径组成的多条局部路径进行评价,从多条所述局部路径中选择得分最高的局部路径作为目标路径。
第三方面,本发明实施例还提供了一种机器人,包括:
一个或多个处理器;
存储装置,用于存储一个或多个程序;
当所述一个或多个程序被所述一个或多个处理器执行,使得所述一个或多个处理器实现如本发明第一方面提供的局部路径规划方法。
第四方面,本发明实施例还提供了一种计算机可读存储介质,其上存储有计算机程序,该程序被处理器执行时实现如本发明第一方面提供的局部路径规划方法。
本发明实施例提供的局部路径规划方法,在确定动态窗口法预测机器人局部路径的预测时长后,将预测时长切分为至少两个子段,在每一子段内,以前一子段内的每一路径的终点为起点,采用动态窗口法生成至少两条新的路径,然后,对各子段的路径组合成的多条局部路径进行评价,从多条局部路径中选择得分最高的局部路径作为目标路径。通过在每一子段内,以前一子段内的每一路径的终点为起点,采用动态窗口法生成至少两条新的路径,每个子段内的路径的数量相对于前一子段内的路径的数量成倍数增加,增加了预测时长内局部路径的总数量,提高了路径末端的空间覆盖率,从而能够选择出最符合实际环境的最优路径,提高机器人的避障性能,提高机器人的运行安全性。
附图说明
图1A为本发明实施例一提供的一种局部路径规划方法的流程图;
图1B为一种采用动态窗口法生成的局部路径的示意图;
图1C为本发明实施例提供的一种采用动态窗口法生成的局部路径的示意图;
图1D为本发明实施例提供的一种在第一路径的终点生成第二路径的示意图;
图1E为本发明实施例提供的另一种采用动态窗口法生成的局部路径的示意图;
图2为本发明实施例二提供的一种局部路径规划方法的流程图;
图3为本发明实施例三提供的一种局部路径规划装置的结构示意图;
图4为本发明实施例四提供的一种机器人的结构示意图。
具体实施方式
下面结合附图和实施例对本发明作进一步的详细说明。可以理解的是,此处所描述的具体实施例仅仅用于解释本发明,而非对本发明的限定。另外还需要说明的是,为了便于描述,附图中仅示出了与本发明相关的部分而非全部结构。
实施例一
图1A为本发明实施例一提供的一种局部路径规划方法的流程图,本实施例可适用于机器人局部路径规划的情况,该方法可以由本发明实施例提供的局部路径规划装置来执行,该装置可以由软件和/或硬件的方式实现,并配置于本发明实施例提供的机器人中,如图1A所示,该方法具体包括如下步骤:
S101、确定动态窗口法预测机器人局部路径的预测时长。
如前文所述,动态窗口法是指在约束的速度空间内以预设的采样频率多次进行采样,得到多组采样速度(包括线速度和角速度),并模拟机器人在这些采样速度在一定时间(即预测时长)的运动轨迹,并对这些运动轨迹进行评价,选取最优运动轨迹对应的采样速度来驱动机器人运动。在本发明实施例中,预先确定动态窗口法预测机器人局部路径的预测时长。
预测时长通常在机器人出厂调试过程中已经设定好,并以配置参数的形式存储在机器人的存储器中。示例性的,机器人可以在每次启动过程中读取配置参数,获取并确定动态窗口法预测机器人局部路径的预测时长。
S102、将预测时长切分为至少两个子段。
在本发明实施例中,将预测时长切分为至少两个子段。在本发明的一些实施例中,采用均分法将预测时长切分为至少两个等时长的子段,在本发明的另一些实施例中,各子段的时长也可以不相等,本发明实施例在此不做限定。
S103、在第i个子段内,针对第i-1个子段的至少两条第一路径中的每一条第一路径,以第一路径的终点为起点,采用动态窗口法生成至少两条第二路径。
在本发明实施例中,在第i个子段内,针对第i-1个子段的至少两条第一路径中的每一条第一路径,以第一路径的终点为起点,采用动态窗口法生成至少两条第二路径。即在每一子段内,以前一子段内的每一路径的终点为起点,采用动态窗口法生成至少两条新的路径。
示例性的,在第一个子段内,以当前机器人所在的位置为起点,采用动态窗口法生成多条路径,通常,在第一个子段内会生成上百条路径。接着,在第二个子段内,针对第一个子段内的每一条路径,以每一条路径的终点为起点,采用动态窗口法生成至少两条路径。以此类推,在最后一个子段内,针对前一子段内的每一条路径,以每一条路径的终点为起点,采用动态窗口法生成至少两条路径。如此,每个子段内的路径的数量相对于前一子段内的路径的数量成倍数增加,增加了预测时长内预测的局部路径的总数量,提高了路径末端的空间覆盖率。
S104、对第一路径和第二路径组成的多条局部路径进行评价,从多条局部路径中选择得分最高的局部路径作为目标路径。
在本发明实施例中,采用评价函数对第一路径和第二路径组成的多条局部路径进行评价,从多条局部路径中选择得分最高的局部路径作为目标路径。然后,以该目标路径对应的速度来驱动机器人运动。需要说明的是,在本发明实施例中,由于目标路径是由至少两条子路径构成,在每一子段内的路径(即子路径)均通过动态窗口法生成,因此,在每一子段内,以该子段对应的速度驱动机器人运动。
图1B为一种采用动态窗口法生成的局部路径的示意图,如图1B所示,采用现有的动态窗口法生成以预测时长的起始时刻下机器人当前位置O为起点的多条局部路径,在路径末端,局部路径较为稀疏,空间覆盖率较低,因此,即使动态窗口法从多条局部路径中选择出的最优路径,也并非符合实际环境的最优路径,从而导致机器人避障性能较差,运行安全性较差。
图1C为本发明实施例提供的一种采用动态窗口法生成的局部路径的示意图,图1D为本发明实施例提供的一种在第一路径的终点生成第二路径的示意图,图1E为本发明实施例提供的另一种采用动态窗口法生成的局部路径的示意图,示例性的,如图1C和图1D所示,在本发明实施例中,将预测时长切分为等时长的两个子段t1和t2,在第一个子段t1内,以当前机器人所在的位置O为起点生成多条(上百条)第一路径。如图1D所示,在第二个子段t2内,针对第一个子段t1内的每一条第一路径,以每一条第一路径的终点O1为起点生成三条第二路径。如此,在预测时长内总的局部路径的数量比现有技术的多3倍,路径末端的空间覆盖率也提高了3倍,从而能够选择出最符合实际环境的最优路径,提高机器人的避障性能,提高机器人的运行安全性。
当然,在本发明的其他实施例中,子段的数量和第二路径的数量可以根据实际环境以及机器人的数据处理能力进行设定,本发明实施例在此不做限定。如图1E所示,在该实施例中,预测时长被切分为3个子段t1、t2和t3,以第一路径的终点为起点生成3条第二路径。
本发明实施例提供的局部路径规划方法,在确定动态窗口法预测机器人局部路径的预测时长后,将预测时长切分为至少两个子段,在每一子段内,以前一子段内的每一路径的终点为起点,采用动态窗口法生成至少两条新的路径,然后,对各子段的路径组合成的多条局部路径进行评价,从多条局部路径中选择得分最高的局部路径作为目标路径。通过在每一子段内,以前一子段内的每一路径的终点为起点,采用动态窗口法生成至少两条新的路径,每个子段内的路径的数量相对于前一子段内的路径的数量成倍数增加,增加了预测时长内局部路径的总数量,提高了路径末端的空间覆盖率,从而能够选择出最符合实际环境的最优路径,提高机器人的避障性能,提高机器人的运行安全性。
实施例二
图2为本发明实施例二提供的一种局部路径规划方法的流程图,本实施例在上述实施例一的基础上进一步细化,详细描述了局部路径规划方法中各步骤的具体过程,如图2所示,该方法包括:
S201、确定动态窗口法预测机器人局部路径的预测时长。
示例性的,如前述实施例所述,机器人可以在每次启动过程中读取配置参数,获取并确定动态窗口法预测机器人局部路径的预测时长。
S202、将预测时长切分为至少两个子段。
示例性的,在本发明实施例中,采用均分法将预测时长切分为至少两个等时长的子段。
S203、针对第i-1个子段的至少两条第一路径中的每一条第一路径,基于机器人自身参数和环境参数的制约确定机器人在第一路径的终点时的速度采样窗口。
在本发明实施例中,机器人自身参数可以包括速度(包括线速度和角速度)、加速度(包括线加速度和角加速度)中的至少一种,本发明实施例在此不做限定。环境参数可以包括障碍物与机器人之间的距离等。
在本发明实施例中,基于机器人自身参数和环境参数的制约确定机器人在第一路径的终点时的速度采样窗口。将第一路径的终点时可取的速度限制在一定范围内,即速度采样窗口。
示例性的,在本发明实施例中,基于机器人自身的速度限制和机器人的驱动电机的加速度(包括正向加速度和负向加速度)限制,以及机器人采集的障碍物信息共同确定速度采样空间。具体过程如下:
1、基于机器人的最大速度限制和最小速度限制确定第一速度空间。
由于机器人的速度是不可能无限增加的,所以应该考虑其最大速度。设机器人的最大线速度为vmax,最小线速度为vmin,最大角速度为wmax,最小角速度为wmin,则第一速度空间可表示为:
Vm={v∈[vmin,vmax],w∈[wmin,wmax]}
其中,最小线速度通常设为0,考虑到为了增加评价函数的区分度,可以考虑把最小线速度设置为1,因为当线速度为0时,评价函数就无法发挥作用。最小角速度和最大角速度互为相反数。
2、基于机器人在第一路径的终点的速度和机器人的驱动电机的加速度限制确定第二速度空间。
由于机器人的驱动电机的转矩有限,因此存在一个最大的加速度限制。假设机器人在第一路径的终点的速度为[vc,wc],经过时间间隔Δt后可达到的速度在第二速度空间Vd内,第二速度空间Vd满足如下条件:
其中,表示最大线加速度,/>表示最大线减速度,/>表示最大角速度,/>表示最大角减速度。在一般情况下,最大加速度和最大减速度为相反数。
3、基于机器人采集到的障碍物信息和机器人的驱动电机的加速度限制确定第三速度空间。
当机器人检测到障碍物时,应该留有一段刹车距离用于减速。当机器人与最近障碍物的距离小于刹车距离时,机器人就会与障碍物发生碰撞,这是我们不愿看到的情况。因此,我们应该考虑一个允许速度,也就是可以安全停止下来的速度。显然,这个安全停止速度应该与机器人与最近障碍物的距离成正比。当距离越小时,这个安全停止速度也应该相应减小。因此,在最大减速度条件下,机器人的速度应当在第三速度空间Va内,第三速度空间Va满足如下条件:
其中,dist(v,w)表示在速度(v,w)对应的路径上离障碍物最近的距离。
4、计算第一速度空间、第二速度空间和第三速度空间的交集,得到在第一路径的终点时的速度采样窗口。
在计算三种速度限制后,我们得到了三个速度空间,它们的交集就是我们在当前状态下可以取到的速度范围,即速度采样窗口Vr。在第一路径的终点时的速度采样窗口Vr可表示为:
Vr=Vm∩Vd∩Va
S204、在第i个子段内,在速度采样窗口内选取至少两个目标速度。
在本发明实施例中,在第i个子段内,在速度采样窗口内选取至少两个目标速度。
在本发明一具体实施例中,为了避免在速度采样窗口中随机选取造成生成的第二路径与第一路径之间的衔接不平滑的问题,以第一路径的终点时机器人的速度为基准速度,在基准速度上进行微调,得到新的目标速度,保证第二路径与第一路径之间的平滑性,改善机器人的运动平滑性。具体过程如下:
1、获取机器人在第一路径的终点的位姿和速度。
在本发明实施例中,第一路径的终点的位姿和速度可以由第i-1个子段内的动态窗口法预测得出。
2、创建至少二维的速度调整向量。
创建一个至少二维的速度调整向量,速度调整向量中的每维元素包括线速度调整参数和角速度调整参数。示例性的,在本发明实施例中,创建一个三维的速度调整向量,每维元素中,线速度调整参数为1,三个角速度调整参数中其中两个为绝对值大于1的相反数,另一个为1。
3、将速度调整向量与机器人在第一路径的终点的速度相乘,得到至少两个目标速度。
在本发明实施例中,将速度调整向量与机器人在第一路径的终点的速度相乘,得到至少两个目标速度。示例性的,在本发明一具体实施例中,速度调整向量为三维,每维元素中,线速度调整参数为1,三个角速度调整参数中其中两个为绝对值大于1的相反数,另一个为1,即本次采样的三个速度中,三个线速度均与第一路径终点的线速度相同,三个角速度中,其中一个与第一路径终点的角速度相同,另外两个分别向不同的方向调整,如图1E所示。
4、判断至少两个目标速度是否均属于速度采样空间。
在得到目标速度之后,需要判断目标速度是否在速度采样空间内。
5、若至少两个调整速度均属于速度采样空间,则将调整速度作为目标速度。
在本发明实施例中,只有当所有调整速度都属于速度采样空间时,则将调整速度作为目标速度。若其中有一个或一个以上的调整速度不属于速度采样空间,则重新创建速度调整向量。
S205、基于机器人在第一路径的终点的位姿和采集的至少两个目标速度,生成以第一路径的终点为起点的至少两条第二路径。
假设机器人在某一条第一路径的终点的位姿为(xr,yr,θr),其中,xr,yr,θr分别表示机器人距离原点(起始点)的X轴距离、Y轴距离和机器人的偏转角度,其中一个目标速度为(v1,w1),且认为在一端无穷小的时长Δt内,机器人的速度保持不变,则该以速度模拟出来的路径点坐标可通过下式迭代计算:
xnew=xr+v1*cos(θr)*Δt
ynew=yr+v1*sin(θr)*Δt
θnew=θr+w1*Δt
如此,生成以第一路径的终点为起点的至少两条第二路径。
经过上述路径模拟过程,得到由各子段内的路径组成的多条局部路径。接下来,将对各局部路径进行评价,计算各局部路径的得分,并从多条局部路径中选择得分最高的局部路径作为目标路径。得分用于表示机器人的避障能力和快速通过能力,得分最高表示以目标路径对应的速度运行时,机器人能够避障,且以最快速度通过。具体的评价过程如下:
S206、针对每一局部路径,计算在局部路径终点时机器人的朝向与目标之间的角度差距。
示例性的,针对每一局部路径,计算在局部路径终点时机器人的朝向与目标之间的角度差距。其中,机器人的朝向可以由该点时机器人的偏转角度计算得到。在本发明实施例中,记第j个局部路径终点时机器人的朝向与目标之间的角度差距为head(j)。
S207、针对每一局部路径,计算机器人在局部路径上与最近的障碍物之间的距离。
示例性的,针对每一局部路径,计算机器人在局部路径上与最近的障碍物之间的距离。记第j个局部路径上机器人与最近的障碍物的距离为dist(j)。
S208、针对每一局部路径,确定局部路径对应的速度。
记第j个局部路径对应的速度为velocity(j),包括线速度和角速度。
S209、基于局部路径对应的角度差距、距离和速度计算局部路径的得分。
示例性的,在本发明实施例中,基于局部路径对应的角度差距、距离和速度计算局部路径的得分的具体过程如下:
1、对所有局部路径对应的角度差距进行归一化处理,得到各局部路径对应的角度差距归一化值。第j个局部路径对应的角度差距归一化值可表示为:
其中,n为局部路径的总数量。
2、对所有局部路径对应的距离进行归一化处理,得到各局部路径对应的距离归一化值。第j个局部路径对应的距离归一化值可表示为:
3、对所有局部路径对应的速度进行归一化处理,得到各局部路径对应的速度归一化值。第j个局部路径对应的速度归一化值可表示为:
4、针对每一局部路径,为局部路径对应的角度差距归一化值、距离归一化值和速度归一化值分别赋予不同的权重,并相加,得到局部路径的得分。
在本发明实施例中,针对每一局部路径,为局部路径对应的角度差距归一化值、距离归一化值和速度归一化值分别赋予不同的权重,并相加,得到局部路径的得分。第j个局部路径的得分的计算公式如下:
G(j)=α·normhead(j)+β·normdist(j)+γ·norm_velocity(j)
如此,计算得到各局部路径的得分。
S210、将得分最高的局部路径作为目标路径。
示例性的,在本发明实施例中,从多个得分中找到最高的得分,并将得分最高的局部路径作为目标路径。然后,以该目标路径对应的速度来驱动机器人运动。需要说明的是,在本发明实施例中,由于目标路径是由至少两条子路径构成,在每一子段内的路径(即子路径)均通过动态窗口法生成,因此,在每一子段内,以该子段对应的速度驱动机器人运动。
本发明实施例提供的局部路径规划方法,在确定动态窗口法预测机器人局部路径的预测时长后,将预测时长切分为至少两个子段,在每一子段内,以前一子段内的每一路径的终点为起点,采用动态窗口法生成至少两条新的路径,每个子段内的路径的数量相对于前一子段内的路径的数量成倍数增加,增加了预测时长内局部路径的总数量,提高了路径末端的空间覆盖率,从而能够选择出最符合实际环境的最优路径,提高机器人的避障性能,提高机器人的运行安全性。此外,以第一路径的终点时机器人的速度为基准速度,在基准速度上进行微调,得到新的目标速度,保证第二路径与第一路径之间的平滑性,改善机器人的运动平滑性。
实施例三
图3为本发明实施例三提供的一种局部路径规划装置的结构示意图,如图3所示,该装置包括:
预测时长确定模块301,用于确定动态窗口法预测机器人局部路径的预测时长;
切分模块302,用于将所述预测时长切分为至少两个子段;
路径生成模块303,用于在第i个所述子段内,针对第i-1个所述子段的至少两条第一路径中的每一条所述第一路径,以所述第一路径的终点为起点,采用动态窗口法生成至少两条第二路径;
目标路径确定模块304,用于对所述第一路径和所述第二路径组成的多条局部路径进行评价,从多条所述局部路径中选择得分最高的局部路径作为目标路径。
在本发明的一些实施例中,各所述子段的时长相等。
在本发明的一些实施例中,路径生成模块303包括:
速度采样窗口确定子模块,用于针对第i-1个所述子段的至少两条第一路径中的每一条所述第一路径,基于所述机器人自身参数和环境参数的制约确定所述机器人在所述第一路径的终点时的速度采样窗口;
目标速度选取子模块,用于在第i个所述子段内,在所述速度采样窗口内选取至少两个目标速度,所述目标速度包括线速度和角速度;
路劲生成子模块,用于基于所述机器人在所述第一路径的终点的位姿和所述至少两个目标速度,生成以所述第一路径的终点为起点的至少两条第二路径。
在本发明的一些实施例中,速度采样窗口确定子模块包括:
第一速度空间确定单元,用于基于所述机器人的最大速度限制和最小速度限制确定第一速度空间;
第二速度空间确定单元,用于基于所述机器人在所述第一路径的终点的速度和所述机器人的驱动电机的加速度限制确定第二速度空间;
第三速度空间确定单元,用于基于所述机器人采集到的障碍物信息和所述机器人的驱动电机的加速度限制确定第三速度空间;
交集计算单元,用于计算所述第一速度空间、所述第二速度空间和所述第三速度空间的交集,得到在所述第一路径的终点时的速度采样窗口。
在本发明的一些实施例中,目标速度选取子模块包括:
获取单元,用于获取所述机器人在所述第一路径的终点的位姿和速度;
向量创建单元,用于创建至少二维的速度调整向量;
乘积单元,用于将所述速度调整向量与所述机器人在所述第一路径的终点的速度相乘,得到至少两个调整速度;
判断单元,用于判断所述至少两个调整速度是否均属于所述速度采样空间;
目标速度确定单元,用于在所述至少两个调整速度均属于所述速度采样空间时,将所述调整速度作为目标速度。
在本发明的一些实施例中,向量创建单元包括:
向量创建子单元,用于创建三维的速度调整向量,其中,所述速度调整向量中的每维元素包括线速度调整参数和角速度调整参数,所述线速度调整参数为1,三个所述角速度调整参数中其中两个为绝对值大于1的相反数,另一个为1。
在本发明的一些实施例中,目标路径确定模块304包括:
角度差距计算子模块,用于针对每一所述局部路径,计算在所述局部路径终点时所述机器人的朝向与目标之间的角度差距;
距离计算子模块,用于针对每一所述局部路径,计算所述机器人在所述局部路径上与最近的障碍物之间的距离;
速度确定子模块,用于针对每一所述局部路径,确定所述局部路径对应的速度;
得分计算子模块,用于基于所述局部路径对应的所述角度差距、所述距离和所述速度计算所述局部路径的得分;
目标路径确定子模块,用于将得分最高的所述局部路径作为目标路径。
在本发明的一些实施例中,得分计算子模块包括:
第一归一化单元,用于对所有所述局部路径对应的所述角度差距进行归一化处理,得到各所述局部路径对应的角度差距归一化值;
第二归一化单元,用于对所有所述局部路径对应的所述距离进行归一化处理,得到各所述局部路径对应的距离归一化值;
第三归一化单元,用于对所有所述局部路径对应的所述速度进行归一化处理,得到各所述局部路径对应的速度归一化值;
加权单元,用于针对每一所述局部路径,为所述局部路径对应的角度差距归一化值、距离归一化值和速度归一化值分别赋予不同的权重,并相加,得到所述局部路径的得分。
上述局部路径规划装置可执行本发明任意实施例所提供的局部路径规划方法,具备执行方法相应的功能模块和有益效果。
实施例四
本发明实施例四提供了一种机器人,图4为本发明实施例四提供的一种机器人的结构示意图,如图4所示,该机器人包括:
处理器401、存储器402、通信模块403、输入装置404和输出装置405;移动终端中处理器401的数量可以是一个或多个,图4中以一个处理器401为例;移动终端中的处理器401、存储器402、通信模块403、输入装置404和输出装置405可以通过总线或其他方式连接,图4中以通过总线连接为例。上述处理器401、存储器402、通信模块403、输入装置404和输出装置405可以集成在机器人上。
存储器402作为一种计算机可读存储介质,可用于存储软件程序、计算机可执行程序以及模块,如上述实施例中的局部路径规划方法对应的模块。处理器401通过运行存储在存储器402中的软件程序、指令以及模块,从而执行机器人的各种功能应用以及数据处理,即实现上述的局部路径规划方法。
存储器402可主要包括存储程序区和存储数据区,其中,存储程序区可存储操作系统、至少一个功能所需的应用程序;存储数据区可存储根据微型计算机的使用所创建的数据等。此外,存储器402可以包括高速随机存取存储器,还可以包括非易失性存储器,例如至少一个磁盘存储器件、闪存器件、或其他非易失性固态存储器件。在一些实例中,存储器402可进一步包括相对于处理器401远程设置的存储器,这些远程存储器可以通过网络连接至机器人。上述网络的实例包括但不限于互联网、企业内部网、局域网、移动通信网及其组合。
通信模块403,用于与外界设备(例如智能终端)建立连接,并实现与外界设备的数据交互。输入装置404可用于接收输入的数字或字符信息,以及产生与机器人的用户设置以及功能控制有关的键信号输入。
本实施例提供的一种机器人,可执行本发明上述任意实施例提供的局部路径规划方法,具有相应的功能和有益效果。
实施例五
本发明实施例五提供了一种包含计算机可执行指令的存储介质,其上存储有计算机程序,该程序被处理器执行时实现如本发明上述任意实施例提供的局部路径规划方法,该方法包括:
确定动态窗口法预测机器人局部路径的预测时长;
将所述预测时长切分为至少两个子段;
在第i个所述子段内,针对第i-1个所述子段的至少两条第一路径中的每一条所述第一路径,以所述第一路径的终点为起点,采用动态窗口法生成至少两条第二路径;
对所述第一路径和所述第二路径组成的多条局部路径进行评价,从多条所述局部路径中选择得分最高的局部路径作为目标路径。
需要说明的是,对于装置、机器人和存储介质实施例而言,由于其与方法实施例基本相似,所以描述的比较简单,相关之处参见方法实施例的部分说明即可。
通过以上关于实施方式的描述,所属领域的技术人员可以清楚地了解到,本发明可借助软件及必需的通用硬件来实现,当然也可以通过硬件实现,但很多情况下前者是更佳的实施方式。基于这样的理解,本发明的技术方案本质上或者说对现有技术做出贡献的部分可以以软件产品的形式体现出来,该计算机软件产品可以存储在计算机可读存储介质中,如计算机的软盘、只读存储器(Read-Only Memory,ROM)、随机存取存储器(RandomAccess Memory,RAM)、闪存(FLASH)、硬盘或光盘等,包括若干指令用以使得一台机器人(可以是机器人,个人计算机,服务器,或者网络设备等)执行本发明任意实施例所述的局部路径规划方法。
值得注意的是,上述装置中,所包括的各个模块、子模块、单元只是按照功能逻辑进行划分的,但并不局限于上述的划分,只要能够实现相应的功能即可;另外,各功能模块的具体名称也只是为了便于相互区分,并不用于限制本发明的保护范围。
应当理解,本发明的各部分可以用硬件、软件、固件或它们的组合来实现。在上述实施方式中,多个步骤或方法可以用存储在存储器中且由合适的指令执行装置执行的软件或固件来实现。例如,如果用硬件来实现,和在另一实施方式中一样,可用本领域公知的下列技术中的任一项或他们的组合来实现:具有用于对数据信号实现逻辑功能的逻辑门电路的离散逻辑电路,具有合适的组合逻辑门电路的专用集成电路,可编程门阵列(PGA),现场可编程门阵列(FPGA)等。
在本说明书的描述中,参考术语“一个实施例”、“一些实施例”、“示例”、“具体示例”、或“一些示例”等的描述意指结合该实施例或示例描述的具体特征、结构、材料或者特点包含于本发明的至少一个实施例或示例中。在本说明书中,对上述术语的示意性表述不一定指的是相同的实施例或示例。而且,描述的具体特征、结构、材料或者特点可以在任何的一个或多个实施例或示例中以合适的方式结合。
注意,上述仅为本发明的较佳实施例及所运用技术原理。本领域技术人员会理解,本发明不限于这里所述的特定实施例,对本领域技术人员来说能够进行各种明显的变化、重新调整和替代而不会脱离本发明的保护范围。因此,虽然通过以上实施例对本发明进行了较为详细的说明,但是本发明不仅仅限于以上实施例,在不脱离本发明构思的情况下,还可以包括更多其他等效实施例,而本发明的范围由所附的权利要求范围决定。
Claims (9)
1.一种局部路径规划方法,其特征在于,包括:
确定动态窗口法预测机器人局部路径的预测时长;
将所述预测时长切分为至少两个子段;
在第i个所述子段内,针对第i-1个所述子段的至少两条第一路径中的每一条所述第一路径,以所述第一路径的终点为起点,采用动态窗口法生成至少两条第二路径;
对所述第一路径和所述第二路径组成的多条局部路径进行评价,从多条所述局部路径中选择得分最高的局部路径作为目标路径;
所述在第i个所述子段内,针对第i-1个所述子段的至少两条第一路径中的每一条所述第一路径,以所述第一路径的终点为起点,采用动态窗口法生成至少两条第二路径,包括:
针对第i-1个所述子段的至少两条第一路径中的每一条所述第一路径,基于所述机器人自身参数和环境参数的制约确定所述机器人在所述第一路径的终点时的速度采样窗口;
在第i个所述子段内,在所述速度采样窗口内选取至少两个目标速度,所述目标速度包括线速度和角速度;
基于所述机器人在所述第一路径的终点的位姿和所述至少两个目标速度,生成以所述第一路径的终点为起点的至少两条第二路径;
所述在第i个所述子段内,在所述速度采样窗口内选取至少两个目标速度,包括:
获取所述机器人在所述第一路径的终点的位姿和速度;
创建至少二维的速度调整向量;
将所述速度调整向量与所述机器人在所述第一路径的终点的速度相乘,得到至少两个调整速度;
判断所述至少两个调整速度是否均属于所述速度采样窗口;
若所述至少两个调整速度均属于所述速度采样窗口,则将所述调整速度作为目标速度。
2.根据权利要求1所述的局部路径规划方法,其特征在于,各所述子段的时长相等。
3.根据权利要求1所述的局部路径规划方法,其特征在于,针对第i-1个所述子段的至少两条第一路径中的每一条所述第一路径,基于所述机器人自身参数和环境参数的制约确定所述机器人在所述第一路径的终点时的速度采样窗口,包括:
基于所述机器人的最大速度限制和最小速度限制确定第一速度空间;
基于所述机器人在所述第一路径的终点的速度和所述机器人的驱动电机的加速度限制确定第二速度空间;
基于所述机器人采集到的障碍物信息和所述机器人的驱动电机的加速度限制确定第三速度空间;
计算所述第一速度空间、所述第二速度空间和所述第三速度空间的交集,得到在所述第一路径的终点时的速度采样窗口。
4.根据权利要求1所述的局部路径规划方法,其特征在于,创建至少二维的速度调整向量,包括:
创建三维的速度调整向量,其中,所述速度调整向量中的每维元素包括线速度调整参数和角速度调整参数,所述线速度调整参数为1,三个所述角速度调整参数中其中两个为绝对值大于1的相反数,另一个为1。
5.根据权利要求1、2、3-4任一所述的局部路径规划方法,其特征在于,对所述第一路径和所述第二路径组成的多条局部路径进行评价,从多条所述局部路径中选择得分最高的局部路径作为目标路径,包括:
针对每一所述局部路径,计算在所述局部路径终点时所述机器人的朝向与目标之间的角度差距;
针对每一所述局部路径,计算所述机器人在所述局部路径上与最近的障碍物之间的距离;
针对每一所述局部路径,确定所述局部路径对应的速度;
基于所述局部路径对应的所述角度差距、所述距离和所述速度计算所述局部路径的得分;
将得分最高的所述局部路径作为目标路径。
6.根据权利要求5所述的局部路径规划方法,其特征在于,基于所述局部路径对应的所述角度差距、所述距离和所述速度计算所述局部路径的得分,包括:
对所有所述局部路径对应的所述角度差距进行归一化处理,得到各所述局部路径对应的角度差距归一化值;
对所有所述局部路径对应的所述距离进行归一化处理,得到各所述局部路径对应的距离归一化值;
对所有所述局部路径对应的所述速度进行归一化处理,得到各所述局部路径对应的速度归一化值;
针对每一所述局部路径,为所述局部路径对应的角度差距归一化值、距离归一化值和速度归一化值分别赋予不同的权重,并相加,得到所述局部路径的得分。
7.一种局部路径规划装置,其特征在于,包括:
预测时长确定模块,用于确定动态窗口法预测机器人局部路径的预测时长;
切分模块,用于将所述预测时长切分为至少两个子段;
路径生成模块,用于在第i个所述子段内,针对第i-1个所述子段的至少两条第一路径中的每一条所述第一路径,以所述第一路径的终点为起点,采用动态窗口法生成至少两条第二路径;
目标路径确定模块,用于对所述第一路径和所述第二路径组成的多条局部路径进行评价,从多条所述局部路径中选择得分最高的局部路径作为目标路径;
速度采样窗口确定子模块,用于针对第i-1个所述子段的至少两条第一路径中的每一条所述第一路径,基于所述机器人自身参数和环境参数的制约确定所述机器人在所述第一路径的终点时的速度采样窗口;
目标速度选取子模块,用于在第i个所述子段内,在所述速度采样窗口内选取至少两个目标速度,所述目标速度包括线速度和角速度;
路径生成子模块,用于基于所述机器人在所述第一路径的终点的位姿和所述至少两个目标速度,生成以所述第一路径的终点为起点的至少两条第二路径;获取单元,用于获取所述机器人在所述第一路径的终点的位姿和速度;
向量创建单元,用于创建至少二维的速度调整向量;
乘积单元,用于将所述速度调整向量与所述机器人在所述第一路径的终点的速度相乘,得到至少两个调整速度;
判断单元,用于判断所述至少两个调整速度是否均属于所述速度采样窗口;
目标速度确定单元,用于在所述至少两个调整速度均属于所述速度采样窗口时,将所述调整速度作为目标速度。
8.一种机器人,其特征在于,包括:
一个或多个处理器;
存储装置,用于存储一个或多个程序;
当所述一个或多个程序被所述一个或多个处理器执行,使得所述一个或多个处理器实现如权利要求1-6中任一所述的局部路径规划方法。
9.一种计算机可读存储介质,其上存储有计算机程序,其特征在于,该程序被处理器执行时实现如权利要求1-6中任一所述的局部路径规划方法。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202111623005.6A CN114415670B (zh) | 2021-12-28 | 2021-12-28 | 一种局部路径规划方法、装置、机器人及存储介质 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202111623005.6A CN114415670B (zh) | 2021-12-28 | 2021-12-28 | 一种局部路径规划方法、装置、机器人及存储介质 |
Publications (2)
Publication Number | Publication Date |
---|---|
CN114415670A CN114415670A (zh) | 2022-04-29 |
CN114415670B true CN114415670B (zh) | 2024-04-16 |
Family
ID=81270328
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN202111623005.6A Active CN114415670B (zh) | 2021-12-28 | 2021-12-28 | 一种局部路径规划方法、装置、机器人及存储介质 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN114415670B (zh) |
Families Citing this family (1)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN114995464B (zh) * | 2022-07-19 | 2022-11-25 | 佛山市星曼信息科技有限公司 | 局部路径规划的控制方法及装置、机器人、存储介质 |
Citations (3)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN107703948A (zh) * | 2017-11-14 | 2018-02-16 | 上海理工大学 | 基于自适应动态窗口的移动机器人局部动态路径规划方法 |
CN109062227A (zh) * | 2018-09-25 | 2018-12-21 | 电子科技大学 | 一种基于动态窗口的多曲线局部路径规划方法 |
CN111352416A (zh) * | 2019-12-29 | 2020-06-30 | 的卢技术有限公司 | 一种基于运动模型的动态窗口局部轨迹规划方法及系统 |
-
2021
- 2021-12-28 CN CN202111623005.6A patent/CN114415670B/zh active Active
Patent Citations (3)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN107703948A (zh) * | 2017-11-14 | 2018-02-16 | 上海理工大学 | 基于自适应动态窗口的移动机器人局部动态路径规划方法 |
CN109062227A (zh) * | 2018-09-25 | 2018-12-21 | 电子科技大学 | 一种基于动态窗口的多曲线局部路径规划方法 |
CN111352416A (zh) * | 2019-12-29 | 2020-06-30 | 的卢技术有限公司 | 一种基于运动模型的动态窗口局部轨迹规划方法及系统 |
Non-Patent Citations (1)
Title |
---|
基于改进型DWA的移动机器人避障路径规划;卞永明等;《中国工程机械学报》;第19卷(第1期);第44-49页 * |
Also Published As
Publication number | Publication date |
---|---|
CN114415670A (zh) | 2022-04-29 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
US5570285A (en) | Method and apparatus for avoiding obstacles by a robot | |
Godoy et al. | Adaptive learning for multi-agent navigation | |
Wen et al. | MRPB 1.0: A unified benchmark for the evaluation of mobile robot local planning approaches | |
CN112526999B (zh) | 速度规划方法、装置、电子设备和存储介质 | |
CN114415670B (zh) | 一种局部路径规划方法、装置、机器人及存储介质 | |
CN110362081B (zh) | 一种移动机器人路径规划方法 | |
Hong et al. | A modified dynamic window approach to obstacle avoidance combined with fuzzy logic | |
Hovland et al. | Dynamic sensor selection for robotic systems | |
CN107309873B (zh) | 机械臂运动控制方法和系统 | |
CN114578808A (zh) | 路径规划方法、电子设备、计算机程序产品及存储介质 | |
Nampoothiri et al. | Real time terrain identification of autonomous robots using machine learning | |
Forootaninia et al. | Uncertainty Models for TTC-Based Collision-Avoidance. | |
Li et al. | Controller optimization using data-driven constrained bat algorithm with gradient-based depth-first search strategy | |
CN109088452B (zh) | 机器人充电方法及机器人 | |
Im et al. | An extended virtual force field based behavioral fusion with neural networks and evolutionary programming for mobile robot navigation | |
EP2898996A1 (en) | Method of controlling a robotic system and a robotic system controller for implementing this method | |
Pozna et al. | Potential field-based approach for obstacle avoidance trajectories | |
Boufera et al. | Fuzzy inference system optimization by evolutionary approach for mobile robot navigation | |
CN115454061B (zh) | 一种基于3d技术的机器人路径避障方法及系统 | |
Regier et al. | Improving navigation with the social force model by learning a neural network controller in pedestrian crowds | |
Chang et al. | A two-phase navigation system for mobile robots in dynamic environments | |
JP2016224680A (ja) | 自己位置推定装置及び自己位置推定装置を備えた移動体 | |
Feng et al. | Mobile robot obstacle avoidance based on deep reinforcement learning | |
US11392092B2 (en) | Information processing device and information processing method | |
Rahmani et al. | Data-driven Koopman fractional order PID control of a MEMS gyroscope using bat algorithm |
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 |