平码五不中公式规律
  • / 22
  • 下载费用:30 金币  

一种机器人避碰路径的规划方法、装置.pdf

关 键 ?#21097;?/dt>
一种 机器人 路径 规划 方法 装置
  专利查询网所有资源均是用户自行上传分享,仅供网友学习交流,未经上传用户书面授权,请勿作他用。
摘要
申请专利号:

CN201310586189.2

申请日:

2013.11.19

公开号:

CN104154917A

公开日:

2014.11.19

当前法律状态:

授权

有效性:

有权

法?#19978;?#24773;: 授权|||实质审查的生效IPC(主分类):G01C 21/20申请日:20131119|||公开
IPC分类号: G01C21/20 主分类号: G01C21/20
申请人: 深圳信息职业技术学院; 李华忠
发明人: 李华忠; 梁永生; 但唐仁; 唐飞
地址: 518172 广东省深圳市龙岗区龙翔大道2188号
优?#28909;ǎ?/td>
专利代理机构: 深圳中一专利商标事务所 44237 代理人: 张全文
PDF完整版下载: PDF下载
法律状态
申请(专利)号:

CN201310586189.2

授权公告号:

||||||

法律状态公告日:

2017.02.08|||2014.12.17|||2014.11.19

法律状态类型:

授权|||实质审查的生效|||公开

摘要

本发明适用于机器人避碰和操控领域,提供了一种机器人避碰路径的规划方法、装置,所述方法包括:先根据机器人初始位形qs和目标位形qg计算得到碰?#24067;?#27979;距离参数的初始值rcol;再根据qs和qg构造双向快速密集搜索树,并设置所述双向快速密集搜索树的碰?#24067;?#27979;距离参数的值为rcol;然后根据qs、qg生成一条机器人规划路径;最后对机器人规划路径上的离散采样位形点分别进行碰?#24067;?#27979;,如果有离散位形点与障碍物发生碰撞,则更新rcol的值为rcol/2;并根据该更新后的rcol生成新的机器人规划路径,直至生成的新的机器人规划路径上的所有离散位形点均没有与障碍物发生碰撞为止。本发明,若机器人与障碍物发生碰撞会自适应将碰?#24067;?#27979;距离参数rcol减半,以便于成功通过狭窄通道或障碍物密集空间。

权利要求书

权利要求书
1.  一种机器人避碰路径的规划方法,其特征在于,所述方法包括:
步骤1、根据机器人的初始位形qs和目标位形qg计算得到碰?#24067;?#27979;距离参数的初始值rcol;
步骤2、根据所述初始位形qs、目标位形qg构造双向快速密集搜索树,所述双向快速密集搜索树包括正向快速密集搜索树和逆向快速密集搜索树;
步骤3、设置所述双向快速密集搜索树的碰?#24067;?#27979;距离参数的值为rcol;
步骤4、根据所述初始位形qs、目标位形qg采用自适应双向快速密集搜索树动态扩展方法生成一条机器人规划路径;
步骤5、?#36816;?#36848;机器人规划路径上的离散采样位形点分别进行碰?#24067;?#27979;,如果所有离散位形点均没有与障碍物发生碰撞,则将由所有离散采样位形点组成的机器人规划路径作为有效路径,如果有离散位形点与障碍物发生碰撞,则执行步骤6;
步骤6、更新rcol的值为rcol/2;
步骤7、循?#20998;?#34892;步骤3至6,直至生成的机器人规划路径上的所有离散位形点均没有与障碍物发生碰撞为止。

2.  如权利要求1所述的方法,其特征在于,所述步骤4包括:
根据所述初始位形qs、目标位形qg计算得到扩?#20849;?#38271;的初始值rext;
根据所述初始位形qs、目标位形qg和所述扩?#20849;?#38271;的初始值rext配置所述双向快速密集搜索树;
局部调整所述扩?#20849;?#38271;的初始值rext,获得一条机器人规划路径。

3.  如权利要求2所述的方法,其特征在于,所述局部调整所述扩?#20849;?#38271;的初始值rext,获得一条机器人规划路径包括:
从位形空间随机采样获得一个随机采样位形点qrand;
搜索所述正向快速密集搜索树上与所述随机采样位形qrand最近邻的边e1;
在边e1上搜索与所述随机采样位形qrand最近邻的位形点
根据所述qrand、以及所述扩?#20849;?#38271;rext计算得到不与障碍物发生碰撞的新位形点qnew;
根据所述最近邻的位形点以及所述新位形点qnew做碰?#24067;?#27979;;
如果连接所述最近邻的位形点和所述新位形点qnew之间的路径没有与障碍物发生碰撞,则搜索所述逆向快速密集搜索树上与所述新位形点qnew最近邻的边e2;
在边e2上搜索与所述新位形点qnew最近邻的位形点
根据所述最近邻的位形点以及所述新位形点qnew做碰?#24067;?#27979;;
若连接所述最近邻的位形点和所述新位形点qnew之间的路径没有与障碍物发生碰撞时,将所述边e2、所述最近邻的位形点和所述新位形点qnew添加至所述逆向快速密集搜索树中,并获得新边enew,同时分别使与所述边e2和所述新边enew对应的扩?#20849;?#38271;rext加倍;
若连接所述最近邻的位形点和所述新位形点qnew之间的路径与障碍物发生碰撞时,计算机器人从向qnew移动而不与障碍物发生碰撞的最合适的位形点,将所述最适合的位形点、所述边e2和所述新位形点qnew添加至所述逆向快速密集搜索树中,并获得新边enew,同时分别使与所述边e2和所述新边enew对应的扩?#20849;?#38271;rext减半,直至连接所述最近邻的位形点和所述新位形点qnew之间的路径没有与障碍物发生碰撞。

4.  如权利要求3所述的方法,其特征在于,利用加权采样方法,从位形空间随机采样获得一个随机采样位形点qrand。

5.  一种机器人避碰路径的规划装置,其特征在于,所述装置包括:
碰?#24067;?#27979;距离初始化单元,用于根据机器人的初始位形qs和目标位形qg计算得到碰?#24067;?#27979;距离参数的初始值rcol;
搜索树构建单元,用于根据所述初始位形qs、目标位形qg构造双向快速密集搜索树,所述双向快速密集搜索树包括正向快速密集搜索树和逆向快速密集搜索树;
碰?#24067;?#27979;距离设置单元,用于设置所述双向快速密集搜索树的碰?#24067;?#27979;距离参数的值为rcol;
规划路径生成单元,用于根据所述初始位形qs、目标位形qg采用自适应双向快速密集搜索树动态扩展方法生成一条机器人规划路径;
碰?#24067;?#27979;单元,用于?#36816;?#36848;机器人规划路径上的离散采样位形点分别进行碰?#24067;?#27979;;
碰?#24067;?#27979;距离更新单元,用于如果所有离散位形点均没有与障碍物发生碰撞,则将由所有离散采样位形点组成的机器人规划路径作为有效路径,如果有离散位形点与障碍物发生碰撞,则更新rcol的值为rcol/2;
规划路径更新单元,用于依次循环调用规划路径生成单元、规划路径生成单元、碰?#24067;?#27979;单元和碰?#24067;?#27979;距离更新单元,直至生成的机器人规划路径上的所有离散位形点均没有与障碍物发生碰撞为止。

6.  如权利要求5所述的装置,其特征在于,所述规划路径生成单元包括:
扩?#20849;?#38271;初始化模块,用于根据所述初始位形qs、目标位形qg计算得到扩?#20849;?#38271;的初始值rext;
搜索树配置模块,用于根据所述初始位形qs、目标位形qg和所述扩?#20849;?#38271;的初始值rext配置所述双向快速密集搜索树;
扩?#20849;?#38271;调整模块,用于局部调整所述扩?#20849;?#38271;的初始值rext,获得一条机器人规划路径。

7.  如权利要求6所述的装置,其特征在于,所述扩?#20849;?#38271;调整模块包括:
随机采样位?#20301;?#21462;子模块,用于从位形空间随机采样获得一个随机采样位形点qrand;
第一最近邻边获取子模块,用于搜索所述正向快速密集搜索树上与所述随机采样位形qrand最近邻的边e1;
第一最近邻位形点获取子模块,用于在边e1上搜索与所述随机采样位形qrand最近邻的位形点
新位形点获取子模块,用于根据所述qrand、以及所述扩?#20849;?#38271;rext计算得到不与障碍物发生碰撞的新位形点qnew;
第一碰?#24067;?#27979;子模块,用于根据所述最近邻的位形点以及所述新位形点qnew做碰?#24067;?#27979;;
第二最近邻边获取子模块,用于如果连接所述最近邻的位形点和所述新位形点qnew之间的路径没有与障碍物发生碰撞,则搜索所述逆向快速密集搜索树上与所述新位形点qnew最近邻的边e2;
第二最近邻位形点获取子模块,用于在边e2上搜索与所述新位形点qnew最近邻的位形点
第二碰?#24067;?#27979;子模块,用于根据所述最近邻的位形点以及所述新位形点qnew做碰?#24067;?#27979;;
扩?#20849;?#38271;加倍子模块,用于若连接所述最近邻的位形点和所述新位形点qnew之间的路径没有与障碍物发生碰撞时,将所述边e2、所述最近邻的位形点和所述新位形点qnew添加至所述逆向快速密集搜索树中,并获得新边enew,同时分别使与所述边e2和所述新边enew对应的扩?#20849;?#38271;加倍;
扩?#20849;?#38271;减半子模块,用于若连接所述最近邻的位形点和所述新位形点qnew之间的路径与障碍物发生碰撞时,计算机器人从向qnew移动而不与障碍物发生碰撞的最合适的位形点,将所述最适合的位形点、所述边e2和所述新位形点qnew添加至所述逆向快速密集搜索树中,并获得新边enew,同时分别使与所述边e2和所述新边enew对应的扩?#20849;?#38271;减半,直至连接所述最近邻的位形点和所述新位形点qnew之间的路径没有与障碍物发生碰撞。

8.  如权利要求7所述的装置,其特征在于,所述随机采样位?#20301;?#21462;子模块利用加权采样方法,从位形空间随机采样获得一个随机采样位形点qrand。

说明书

说明书一种机器人避碰路径的规划方法、装置
技术领域
本发明属于机器人避碰和操控领域,尤其涉及一种机器人避碰路径的规划方法、装置。
背景技术
目前,基于采样的规划技术已被广泛应用于高自由度智能机器人避碰和操控等领域,成为国内外普遍关注的研究热点。该方法的显著优点是基于随机采样只需近似构造自由位形空间,从而避免了采用骨架网格和栅格分解等方法需完整和精确构造全部有效C-空间模型,以寻找精?#26041;?#26512;所带来的PSPACE难题,规避了高自由?#20154;?#24341;起的存储空间和计算量呈指数爆炸增长的NP问题。最典型的两类基于采样的运动规划方法为:快速随机搜索树法?#36879;?#29575;地图法。它们均利用采样技术在自由C-空间中构造树或图,然后迭代细化,直到?#19994;?#36830;接初始位形和目标位形的无碰撞路径解。然而,该类方法的有效性,尤其在处理高维空间?#36879;?#26434;障碍环境等复杂问题时,严重依赖于所选的采样参数和离散碰?#24067;?#27979;距离参数。
发明内容
本发明实施例提供了一种机器人避碰路径的规划方法、装置,旨在解决现有技术在处理高维空间?#36879;?#26434;障碍环境等复杂问题时,严重依赖于所选的采样参数和离散碰?#24067;?#27979;距离参数的问题。
一方面,提供一种机器人避碰路径的规划方法,所述方法包括:
步骤1、根据机器人的初始位形qs和目标位形qg计算得到碰?#24067;?#27979;距离参数的初始值rcol;
步骤2、根据所述初始位形qs、目标位形qg构造双向快速密集搜索树,所述双向快速密集搜索树包括正向快速密集搜索树和逆向快速密集搜索树;
步骤3、设置所述双向快速密集搜索树的碰?#24067;?#27979;距离参数的值为rcol;
步骤4、根据所述初始位形qs、目标位形qg采用自适应双向快速密集搜索树动态扩展方法生成一条机器人规划路径;
步骤5、?#36816;?#36848;机器人规划路径上的离散采样位形点分别进行碰?#24067;?#27979;,如果所有离散位形点均没有与障碍物发生碰撞,则将由所有离散采样位形点组成的机器人规划路径作为有效路径,如果有离散位形点与障碍物发生碰撞,则执行步骤6;
步骤6、更新rcol的值为rcol/2;
步骤7、循?#20998;?#34892;步骤3至6,直至生成的机器人规划路径上的所有离散位形点均没有与障碍物发生碰撞为止。
进一步地,所述步骤4包括:
根据所述初始位形qs、目标位形qg计算得到扩?#20849;?#38271;的初始值rext;
根据所述初始位形qs、目标位形qg和所述扩?#20849;?#38271;的初始值rext配置所述双向快速密集搜索树;
局部调整所述扩?#20849;?#38271;的初始值rext,获得一条机器人规划路径。
进一步地,所述局部调整所述扩?#20849;?#38271;的初始值rext,获得一条机器人规划路径包括:
从位形空间随机采样获得一个随机采样位形点qrand;
搜索所述正向快速密集搜索树上与所述随机采样位形qrand最近邻的边e1;
在边e1上搜索与所述随机采样位形qrand最近邻的位形点
根据所述qrand、以及所述扩?#20849;?#38271;计算得到不与障碍物发生碰撞的新位 形点qnew;
根据所述最近邻的位形点以及所述新位形点qnew做碰?#24067;?#27979;;
如果连接所述最近邻的位形点和所述新位形点qnew之间的路径没有与障碍物发生碰撞,则搜索所述逆向快速密集搜索树上与所述新位形点qnew最近邻的边e2;
在边e2上搜索与所述新位形点qnew最近邻的位形点
根据所述最近邻的位形点以及所述新位形点qnew做碰?#24067;?#27979;;
若连接所述最近邻的位形点和所述新位形点qnew之间的路径没有与障碍物发生碰撞时,将所述边e2、所述最近邻的位形点和所述新位形点qnew添加至所述逆向快速密集搜索树中,并获得新边enew,同时分别使与所述边e2和所述新边enew对应的扩?#20849;?#38271;加倍;
若连接所述最近邻的位形点和所述新位形点qnew之间的路径与障碍物发生碰撞时,计算机器人从向qnew移动而不与障碍物发生碰撞的最合适的位形点,将所述最适合的位形点、所述边e2和所述新位形点qnew添加至所述逆向快速密集搜索树中,并获得新边enew,同时分别使与所述边e2和所述新边enew对应的扩?#20849;?#38271;减半,直至连接所述最近邻的位形点和所述新位形点qnew之间的路径没有与障碍物发生碰撞。
进一步地,利用加权采样方法,从位形空间随机采样获得一个随机采样位形点qrand。
另一方面,提供一种机器人避碰路径的规划装置,所述装置包括:
碰?#24067;?#27979;距离初始化单元,用于根据机器人的初始位形qs和目标位形qg计算得到碰?#24067;?#27979;距离参数的初始值rcol;
搜索树构建单元,用于根据所述初始位形qs、目标位形qg构造双向快速密集搜索树,所述双向快速密集搜索树包括正向快速密集搜索树和逆向快速密集搜索树;
碰?#24067;?#27979;距离设置单元,用于设置所述双向快速密集搜索树的碰?#24067;?#27979;距离参数的值为rcol;
规划路径生成单元,用于根据所述初始位形qs、目标位形qg采用自适应双向快速密集搜索树动态扩展方法生成一条机器人规划路径;
碰?#24067;?#27979;单元,用于?#36816;?#36848;机器人规划路径上的离散采样位形点分别进行碰?#24067;?#27979;;
碰?#24067;?#27979;距离更新单元,用于如果所有离散位形点均没有与障碍物发生碰撞,则将由所有离散采样位形点组成的机器人规划路径作为有效路径,如果有离散位形点与障碍物发生碰撞,则更新rcol的值为rcol/2;
规划路径更新单元,用于依次循环调用规划路径生成单元、规划路径生成单元、碰?#24067;?#27979;单元和碰?#24067;?#27979;距离更新单元,直至生成的机器人规划路径上的所有离散位形点均没有与障碍物发生碰撞为止。
进一步地,所述规划路径生成单元包括:
扩?#20849;?#38271;初始化模块,用于根据所述初始位形qs、目标位形qg计算得到扩?#20849;?#38271;的初始值rext;
搜索树配置模块,用于根据所述初始位形qs、目标位形qg和所述扩?#20849;?#38271;的初始值rext配置所述双向快速密集搜索树;
扩?#20849;?#38271;调整模块,用于局部调整所述扩?#20849;?#38271;的初始值rext,获得一条机器人规划路径。
进一步地,所述扩?#20849;?#38271;调整模块包括:
随机采样位?#20301;?#21462;子模块,用于从位形空间随机采样获得一个随机采样位形点qrand;
第一最近邻边获取子模块,用于搜索所述正向快速密集搜索树上与所述随机采样位形qrand最近邻的边e1;
第一最近邻位形点获取子模块,用于在边e1上搜索与所述随机采样位形 qrand最近邻的位形点
新位形点获取子模块,用于根据所述qrand、以及所述扩?#20849;?#38271;计算得到不与障碍物发生碰撞的新位形点qnew;
第一碰?#24067;?#27979;子模块,用于根据所述最近邻的位形点以及所述新位形点qnew做碰?#24067;?#27979;;
第二最近邻边获取子模块,用于如果连接所述最近邻的位形点和所述新位形点qnew之间的路径没有与障碍物发生碰撞,则搜索所述逆向快速密集搜索树上与所述新位形点qnew最近邻的边e2;
第二最近邻位形点获取子模块,用于在边e2上搜索与所述新位形点qnew最近邻的位形点
第二碰?#24067;?#27979;子模块,用于根据所述最近邻的位形点以及所述新位形点qnew做碰?#24067;?#27979;;
扩?#20849;?#38271;加倍子模块,用于若连接所述最近邻的位形点和所述新位形点qnew之间的路径没有与障碍物发生碰撞时,将所述边e2、所述最近邻的位形点和所述新位形点qnew添加至所述逆向快速密集搜索树中,并获得新边enew,同时分别使与所述边e2和所述新边enew对应的扩?#20849;?#38271;加倍;
扩?#20849;?#38271;减半子模块,用于若连接所述最近邻的位形点和所述新位形点qnew之间的路径与障碍物发生碰撞时,计算机器人从向qnew移动而不与障碍物发生碰撞的最合适的位形点,将所述最适合的位形点、所述边e2和所述新位形点qnew添加至所述逆向快速密集搜索树中,并获得新边enew,同时分别使与所述边e2和所述新边enew对应的扩?#20849;?#38271;减半,直至连接所述最近邻的位形点和所述新位形点qnew之间的路径没有与障碍物发生碰撞。
进一步地,所述随机采样位?#20301;?#21462;子模块利用加权采样方法,从位形空间随机采样获得一个随机采样位形点qrand。
在本发明实施例,先根据机器人初始位形qs和目标位形qg计算得到碰撞 检测距离参数的初始值rcol;再根据qs和qg构造双向快速密集搜索树,并设置所述双向快速密集搜索树的碰?#24067;?#27979;距离参数的值为rcol;然后根据qs、qg采用自适应双向快速密集搜索树动态扩展方法生成一条机器人规划路径;最后?#36816;?#36848;机器人规划路径上的离散采样位形点分别进行碰?#24067;?#27979;,如果所有离散位形点均没有与障碍物发生碰撞,则将由所有离散采样位形点组成的机器人规划路径作为有效路径,如果有离散位形点与障碍物发生碰撞,则更新rcol的值为rcol/2;并根据该更新后的rcol生成新的机器人规划路径,直至生成的新的机器人规划路径上的所有离散位形点均没有与障碍物发生碰撞为止。实现了若机器人与障碍物发生碰撞会自适应将碰?#24067;?#27979;距离参数rcol减半,以便于成功通过狭窄通道或障碍物密集空间。
附图说明
图1是本发明实施例一提供的机器人避碰路径的规划方法的实?#33267;?#31243;图;
图2是本发明实施例一提供的通过BiRDT对象调用自适应双向快速密集搜索树动态扩展方法ABiRDTDExtend(qs,qg)来生成一条机器人规划路径的实?#33267;?#31243;图;
图3是本发明实施例一提供的通过局部调整所述扩?#20849;?#38271;的初始值rext,获得一条机器人规划路径的实?#33267;?#31243;图;
图4是本发明实施例一提供的快速密集搜索树从随机采样位形qrand向目标位形qg扩展,获得不与障碍物发生碰撞的新位形qnew的过程示意图;
图5是本发明实施例一提供的RDTExtend(T,qrand,qnew)的实?#33267;?#31243;图;
图6是本发明实施例一提供的构建快速密集搜索树(RDT)的新边时,扩?#20849;?#38271;减半的过程示意图;
图7是本发明实施例一提供的RDTConnect(T,q)算法的实?#33267;?#31243;图;
图8是本发明实施例二提供的机器人避碰路径的规划装置的结构框图。
具体实施方式
为了使本发明的目的、技术方案及优点更加清楚明白,以下结合附?#25216;?#23454;施例,对本发明进行进一步详细说明。应当理解,此处所描述的具体实施例仅仅用以解释本发明,并不用于限定本发明。
在本发明实施例中,先根据机器人初始位形qs和目标位形qg计算得到碰?#24067;?#27979;距离参数的初始值rcol;再根据qs和qg构造双向快速密集搜索树,并设置所述双向快速密集搜索树的碰?#24067;?#27979;距离参数的值为rcol;然后根据qs、qg采用自适应双向快速密集搜索树动态扩展方法生成一条机器人规划路径;最后?#36816;?#36848;机器人规划路径上的离散采样位形点分别进行碰?#24067;?#27979;,如果所有离散位形点均没有与障碍物发生碰撞,则将由所有离散采样位形点组成的机器人规划路径作为有效路径,如果有离散位形点与障碍物发生碰撞,则更新rcol的值为rcol/2;并根据该更新后的rcol生成新的机器人规划路径,直至生成的新的机器人规划路径上的所有离散位形点均没有与障碍物发生碰撞为止。
以下结合具体实施例对本发明的实?#32440;?#34892;详?#35813;?#36848;:
实施例一
图1?#22659;?#20102;本发明实施例一提供的机器人避碰路径的规划方法的实?#33267;?#31243;,详述如下:
在步骤S101中,根据机器人的初始位形qs和目标位形qg计算得到碰?#24067;?#27979;距离参数的初始值rcol。
本发明实施例中,先初始化机器人的初始位形qs和目标位形qg,再根据初始位形qs和目标位形qg计算得到碰?#24067;?#27979;距离的初始值rcol,其中,计算得到的rcol满足:
rcol=||qg-qs||。
在步骤S102中,根据所述初始位形qs、目标位形qg构造双向快速密集搜 索树,所述双向快速密集搜索树包括正向快速密集搜索树和逆向快速密集搜索树。
在本发明实施例中,构造双向快速密集搜索树,所述双向快速密集搜索树包括正向快速密集搜索树Ta和逆向快速密集搜索树Tb,Ta是从初始位形qs开始,递增地构造的正向快速密集搜索树,Tb是从目标位形qg开始,递增地构造的逆向快速密集搜索树。
其中,具体实现时,双向快速密集搜索树以CBiRDT(qs,qg)对象?#36947;鼴iRDT对象表示,后续在介绍具体实现时,都以BiRDT对象来进行说明,不再赘述。
在步骤S103中,设置所述双向快速密集搜索树的碰?#24067;?#27979;距离参数的值为rcol。
在本发明实施例中,BiRDT对象调用SetColPara(rcol)方法设置双向快速密集搜索树的碰?#24067;?#27979;距离参数的值为rcol。
在步骤S104中,根据所述初始位形qs、目标位形qg采用自适应双向快速密集搜索树动态扩展方法生成一条机器人规划路径。
本发明实施例中,获得机器人的初始位形qs、目标位形qg后,可以通过BiRDT对象调用自适应双向快速密集搜索树动态扩展方法ABiRDTDExtend(qs,qg)来生成一条机器人规划路径,具体实?#32844;?#25324;的步骤如图2所示,详述如下:
步骤1、根据机器人初始位形qs、目标位形qg计算得到扩?#20849;?#38271;的初始值rext。
本步骤中,根据目标位形qg和初始位形qs,计算后续将要构造的双向快速密集搜索树的扩?#20849;?#38271;的初始值rext,其中rext满足:
rext=||qg-qs||。
扩?#20849;?#38271;初始值的计算采用位形空间距离测度方法,即是:rext=||qg-qs||=Σi=1n(qgi-qsi)2,]]>其中,n为位形空间的维度。
步骤2、根据初始位形qs、目标位形qg和扩?#20849;?#38271;的初始值rext配置所述双向快速密集搜索树。
本步骤中,可以根据Config函数配置正向快速密集搜索树和逆向快速密集搜索树的参数,具体通过Ta.Config(qs,rext)和Tb.Config(qg,rext)函数配置正向快速密集搜索树和逆向快速密集搜索树的参数,其中双向快速密集搜索树的每个节点中均存储有扩?#20849;?#38271;的初始值rext。
步骤3、局部调整所述扩?#20849;?#38271;的初始值rext,获得一条机器人规划路径。
本步骤中,BiRDT对象通过调用自适应双向快速密集树动态扩展方法ABiRDTDExtend(qs,qg)中的RDTExtend(T,qrand,qnew)和RDTConnect(T,qnew)方法扩展双向搜索树,通过局部自适应调整存储在双向搜索树每个节点中存储的扩?#20849;?#38271;的初始值rext,能够处理狭窄通道运动规划问题。通过调用RDTExtend(Ta,qrand,qnew)和RDTConnect(Ta,qnew)方法处理完狭窄通道运动规划问题后,BiRDT对象可以调用Execute()方法,执行自适应双向快速密集树动态扩展方法,获得一条局部可行路径Path。
其中,步骤3的实?#33267;?#31243;如图3所示,具体包括以下步骤:
步骤11、利用加权采样方法,从位形空间随机采样获得一个随机采样位形点qrand。
本步骤中,通过调用函数qrand=RandConfig()从位形空间随机采样获得一个随机采样位形点qrand。
其中,RandConfig()法主要处理C-空间随机采样,本发明实施例采用加权采样方法,生成分辨率均匀的采样,以消除C-空间不同维度在工作空间对机器人影响的差异性。为了在C-空间路径两个位形点q1和q2之间进行加权均匀采样,其单位步长矢量计算公式如(1)式:
vstep(q1,q2)=(q2-q1)/ρws(q2-q1)     (1)
其中,ρws(qc)计算C-空间路径的最大工作空间位移量,其公式如(2)式;
ρws(qc)=Σi=0n-1wiqci---(2)]]>
在(2)式中,权重系数wi可通过求解从机器人第i个关节开始的所有运动链K的最大距离。对运动链K,属于K所有关节k∈K在工作空间的最大扩展是通过查找在相应表面Sk上两点和之间最大距离和获得,其计算公式如(3)式:
wi=maxK(Σk∈Kmaxqci,qcj∈Sk|qci-qcj|)---(3)]]>
因此,在两个位形q1和q2之间的路径上可均匀产生m=[ρws/rext]-1个中间采样位形qt,其计算公式如(4)式:
qt=q1+krextvstep(q1,q2),t=(1,...,m)     (4)
若将wi设置为机器人工作空间位移的最大上限,则简化计算,从而避免依赖人工设置归一化C-空间维度参数,提高C-空间采样的自适应性。
步骤12、搜索所述正向快速密集搜索树上与所述随机采样位形qrand最近邻的边e1。
本步骤中,通过NNeighborEdge(T,q)函数来搜索正向快速密集搜索树上与所述随机采样位形qrand最近邻的边e1。
步骤13、在边e1上搜索与所述随机采样位形qrand最近邻的位形点
步骤14、根据所述qrand、以及所述扩?#20849;?#38271;的初始值rext计算得到不与障碍物发生碰撞的新位形点qnew。
步骤15、根据所述最近邻的位形点以及所述新位形点qnew做碰?#24067;?#27979;。
步骤16、如果连接所述最近邻的位形点和所述新位形点qnew之间的路径没有与障碍物发生碰撞,则搜索所述逆向快速密集搜索树上与所述新位形点 qnew最近邻的边e2。
步骤17、在边e2上搜索与所述新位形点qnew最近邻的位形点
步骤18、根据所述最近邻的位形点以及所述新位形点qnew做碰?#24067;?#27979;,如果发生碰撞,执行步骤19,否则,执行步骤20。
步骤19、如果连接所述最近邻的位形点和所述新位形点qnew之间的路径没有与障碍物发生碰撞,则将所述边e2、所述最近邻的位形点和所述新位形点qnew添加至所述逆向快速密集搜索树中,并获得新边enew,同时分别使与所述边e2和所述新边enew对应的扩?#20849;?#38271;加倍。
步骤20、否则,计算机器人从向qnew移动而不与障碍物发生碰撞的最合适的位形点,将所述最适合的位形点、所述边e2和所述新位形点qnew添加至所述逆向快速密集搜索树中,并获得新边enew,同时分别使与所述边e2和所述新边enew对应的扩?#20849;?#38271;减半,直至连接所述最近邻的位形点和所述新位形点qnew之间的路径没有与障碍物发生碰撞。
其中,RDTExtend(T,qrand,qnew)可以使得正向快速密集搜索树Ta从随机采样位形qrand向目标位形qg扩展,获得不与障碍物发生碰撞的新位形qnew,?#37096;?#20197;使得逆向快速密集搜索树Tb从随机采样位形qrand向目标位形qs扩展,获得不与障碍物发生碰撞的新位形qnew并返回扩展状态ResE,若扩展成功,则ResE返回TURE,否则返回FALSE;RDTConnect(T,qnew)可用于搜索逆向快速密集搜索树上与所述新位形点qnew最近邻的边e2,其中,?#37096;?#29992;于搜索正向快速密集搜索树上与所述新位形点qnew最近邻的边e1并返回扩展状态ResC,若扩展成功,则ResC返回TURE,否则返回FALSE;如果ResE和ResC均扩展成功,则获得一条局部可行路径Path?#29615;?#21017;,为了保证扩展的平衡性,通过函数Swap(Ta,Tb)交换正向快速密集搜索树和逆向快速密集搜索树,然后再调用RDTExtend(T,qrand,qnew)和RDTConnect(T,qnew),直至ResE和ResC均扩展成功为止。
具体的,RDTExtend(T,qrand,qnew)的基本思路是:确定在边e上与随机采样 位形qrand的最近邻位形qNN,然后通过考虑边e的局部扩?#20849;?#38271;rext,将扩展生成的新位形qnew,限制在rext以内(如图4所示)。图中黑色的qrand情况表示,当随机采样位形qrand位于扩?#20849;?#38271;rext以内,则qnew=qrand?#25442;?#33394;qrand情况表示,当随机采样位形qrand位于扩?#20849;?#38271;rext以外,则需要裁减为:qnew=qNN+(qrand-qNN)rext|qrand-qNN|。具体的实?#33267;?#31243;如图5所示,详述如下:
步骤21:搜索正向快速密集搜索树T中与位形点qrand最近邻的边e=NNeighborEdge(T,qrand),即是遍历搜索RDT树T中的每条边,求每条边与位形点qrand的距离最短的作为最近邻边e。
步骤22:在RDT树的边e上寻找与位形点qrand最近邻的位形:qNN=NNeighborOnEdge(T,e,qrand),即是求位形点qrand与最近邻边e的垂足。
步骤23:当rext≥|qrand-qNN|时,qnew=qrand;当rext<|qrand-qNN|时,qnew=qNN+(qrand-qNN)rext|qrand-qNN|。即是说:计算新的位形点,当rext≥|qrand-qNN|(最近qNN与随机采样位形qrand之间的距离小于等于rext邻的位形)时,qnew=qrand;其他,即是当rext<|qrand-qNN|(最近qNN与随机采样位形qrand之间的距离大于rext的位形),qnew=qNN+(qrand-qNN)rext|qrand-qNN|(裁剪超过的部分,使其约束在边界上,详见图4)。
步骤24:调用isCollision(qNN,qnew)做碰?#24067;?#27979;,判?#29421;?#25509;位形点qNN和qnew之间的路径是否与障碍物发生碰撞;若发生碰撞,则输出false?#29615;?#21017;调用RDTConnect(T,qnew)搜索逆向快速密集搜索树上的新边。
具体的,RDTConnect(T,q)主要用于构建快速密集搜索树(RDT)的新边。如果在位形qNN和q之间的直线段路径是避碰的,则新边被添加到RDT中,且与边e对应的扩?#20849;?#38271;rext加倍。否则,在从qNN到q路径上最大有效位形的边将被添加到树中,且将扩?#20849;?#38271;rext减半。该方法的扩?#20849;?#38271;减半的示意图如图6所示。
RDTConnect(T,q)算法的流程如图7所示,具体步骤描述如下:
步骤31:搜索RDT上与位形q最近邻的边e=NNeighborEdge(T,q);
步骤32:在RDT树的边e上寻找与位形点q最近邻的位形:qNN=NNeighborOnEdge(T,e,q);
步骤33:调用isCollision(qNN,q)判定从qNN和q的路径是否与障碍物发生碰撞;如发生碰撞,则转步骤34;若不发生碰撞,则转步骤37;
步骤34:增加最有效位形点:qnew=GetMValidConfig(qNN,q);
步骤35:增?#26377;?#30340;扩展边:enew=AddEdge(T,e,qNN,qnew)
步骤36:扩?#20849;?#38271;减半:e.q1.rext=e.q1.rext/2;e.q2.rext=e.q2.rext/2;enew.q1.rext=rext/2;enew.q2.rext=rext/2,输出false,转步骤39;
步骤37:向RDT树增?#26377;?#36793;和位形点enew=AddEdge(T,e,qNN,q);
步骤38:扩?#20849;?#38271;加倍:e.q1.rext=2*e.q1.rext;e.q2.rext=2*e.q2.rext;enew.q1.rext=2*rext;enew.q2.rext=2*rext;输出true。
步骤39:RDTConnect(T,q)结束。
具体的,函数isCollision(qNN,q)用于判定从qNN和q的路径是否与障碍物发生碰撞,isCollision(qNN,q)利用方向包围盒(OBB)的紧密?#38498;?#29699;状包围盒计算简单的优点来构造机器人和环境的混合包围体层次树结构,快速排除不相交的对象,并利用OpenMP模型并行遍历混合包围体层次树,从而加快碰?#24067;?#27979;速度。该方法的主要实现步骤如下:
步骤41:预处理,构建基于方向包围盒和球状包围盒的混合包围盒层次树。
步骤42:先采用基于OpenMP的混合包围盒并行相交检测进行粗略相交检测。
步骤43?#21495;?#26029;混合包围盒间存在相交,若不相交,则表明机器人和障碍物之间不发生碰撞;输出false,转步骤45;若相交,则转步骤44。
步骤44:精确相交检测,基于OpenMP相交包围盒所对应的三角形间的相 交检测,若相交,则判定机器人与障碍物发生碰撞,输出true;否则,判定机器人不与障碍物发生碰撞,输出false;转步骤45。
步骤45:碰?#24067;?#27979;算法结束。
在步骤S105中,?#36816;?#36848;机器人规划路径上的离散采样位形点分别进行碰?#24067;?#27979;,如果所有离散位形点均没有与障碍物发生碰撞,则将由所有离散采样位形点组成的机器人规划路径作为有效路径,如果有离散位形点与障碍物发生碰撞,则执行步骤S106。
在步骤S106中,更新rcol的值为rcol/2。
在步骤S107中,循?#20998;?#34892;步骤S103至S106,直至生成的机器人规划路径上的所有离散位形点均没有与障碍物发生碰撞为止。
本实施例,先根据机器人初始位形qs和目标位形qg计算得到碰?#24067;?#27979;距离参数的初始值rcol;再根据qs和qg构造双向快速密集搜索树,并设置所述双向快速密集搜索树的碰?#24067;?#27979;距离参数的值为rcol;然后根据qs、qg采用自适应双向快速密集搜索树扩展方法生成一条机器人规划路径;最后?#36816;?#36848;机器人规划路径上的离散采样位形点分别进行碰?#24067;?#27979;,如果所有离散位形点均没有与障碍物发生碰撞,则将由所有离散采样位形点组成的机器人规划路径作为有效路径,如果有离散位形点与障碍物发生碰撞,则更新rcol的值为rcol/2;并根据该更新后的rcol生成新的机器人规划路径,直至生成的新的机器人规划路径上的所有离散位形点均没有与障碍物发生碰撞为止。实现了若机器人与障碍物发生碰撞会自适应将碰?#24067;?#27979;距离参数rcol减半,以便于成功通过狭窄通道或障碍物密集空间。
另外,在生成机器人规划路径的过程中,可以扩展搜索树,通过局部自适应调整存储在搜索树每个节点的扩展参数rext,能够处理狭窄通道运动规划问题。?#28909;紓?#33509;扩展搜索树失败,则该rext减半;若扩展成功,则该rext加倍,从而对环?#25345;?#30340;障碍物做出条件反射,自适应调整扩大或减小扩?#20849;?#38271;,在障碍物较少的空间加快扩展速度,在狭窄通道或障碍物较多的地方,采用较小扩展 步长,确保不与障碍物发生碰撞。
本领域普通技术人员可以理解实现上述各实施例方法中的全部或部分步骤是可以通过程序来指令相关的?#24067;?#26469;完成,相应的程序可?#28304;?#20648;于一计算机可读取存储介质中,所述的存储介?#21097;?#22914;ROM/RAM、?#25490;?#25110;光盘?#21462;?
实施例二
图8?#22659;?#20102;本发明实施例二提供的机器人避碰路径的规划装置的具体结构框图,为了便于说明,仅?#22659;?#20102;与本发明实施例相关的部分。该装置8包括:碰?#24067;?#27979;距离初始化单元41、搜索树构建单元42、碰?#24067;?#27979;距离设置单元43、规划路径生成单元44、碰?#24067;?#27979;单元45、碰?#24067;?#27979;距离更新单元46和规划路径更新单元47。
其中,碰?#24067;?#27979;距离初始化单元41,用于根据机器人的初始位形qs和目标位形qg计算得到碰?#24067;?#27979;距离参数的初始值rcol;
搜索树构建单元42,用于根据所述初始位形qs、目标位形qg构造双向快速密集搜索树,所述双向快速密集搜索树包括正向快速密集搜索树和逆向快速密集搜索树;
碰?#24067;?#27979;距离设置单元43,用于设置所述双向快速密集搜索树的碰?#24067;?#27979;距离参数的值为rcol;
规划路径生成单元44,用于根据所述初始位形qs、目标位形qg采用自适应双向快速密集搜索树动态扩展方法生成一条机器人规划路径;
碰?#24067;?#27979;单元45,用于?#36816;?#36848;机器人规划路径上的离散采样位形点分别进行碰?#24067;?#27979;;
碰?#24067;?#27979;距离更新单元46,用于如果所有离散位形点均没有与障碍物发生碰撞,则将由所有离散采样位形点组成的机器人规划路径作为有效路径,如果有离散位形点与障碍物发生碰撞,则更新rcol的值为rcol/2;
规划路径更新单元47,用于依次循环调用规划路径生成单元、规划路径生成单元、碰?#24067;?#27979;单元和碰?#24067;?#27979;距离更新单元,直至生成的机器人规划路径上的所有离散位形点均没有与障碍物发生碰撞为止。
进一步地,所述规划路径生成单元44包括:
扩?#20849;?#38271;初始化模块,用于根据所述初始位形qs、目标位形qg计算得到扩?#20849;?#38271;的初始值rext;
搜索树配置模块,用于根据所述初始位形qs、目标位形qg和所述扩?#20849;?#38271;的初始值rext配置所述双向快速密集搜索树;
扩?#20849;?#38271;调整模块,用于局部调整所述扩?#20849;?#38271;的初始值rext,获得一条机器人规划路径。
进一步地,所述扩?#20849;?#38271;调整模块包括:
随机采样位?#20301;?#21462;子模块,用于从位形空间随机采样获得一个随机采样位形点qrand;
第一最近邻边获取子模块,用于搜索所述正向快速密集搜索树上与所述随机采样位形qrand最近邻的边e1;
第一最近邻位形点获取子模块,用于在边e1上搜索与所述随机采样位形qrand最近邻的位形点
新位形点获取子模块,用于根据所述qrand、以及所述扩?#20849;?#38271;计算得到不与障碍物发生碰撞的新位形点qnew;
第一碰?#24067;?#27979;子模块,用于根据所述最近邻的位形点以及所述新位形点qnew做碰?#24067;?#27979;;
第二最近邻边获取子模块,用于如果连接所述最近邻的位形点和所述新位形点qnew之间的路径没有与障碍物发生碰撞,则搜索所述逆向快速密集搜索树上与所述新位形点qnew最近邻的边e2;
第二最近邻位形点获取子模块,用于在边e2上搜索与所述新位形点qnew最 近邻的位形点
第二碰?#24067;?#27979;子模块,用于根据所述最近邻的位形点以及所述新位形点qnew做碰?#24067;?#27979;;
扩?#20849;?#38271;加倍子模块,用于若连接所述最近邻的位形点和所述新位形点qnew之间的路径没有与障碍物发生碰撞时,将所述边e2、所述最近邻的位形点和所述新位形点qnew添加至所述逆向快速密集搜索树中,并获得新边enew,同时分别使与所述边e2和所述新边enew对应的扩?#20849;?#38271;加倍;
扩?#20849;?#38271;减半子模块,用于若连接所述最近邻的位形点和所述新位形点qnew之间的路径与障碍物发生碰撞时,计算机器人从向qnew移动而不与障碍物发生碰撞的最合适的位形点,将所述最适合的位形点、所述边e2和所述新位形点qnew添加至所述逆向快速密集搜索树中,并获得新边enew,同时分别使与所述边e2和所述新边enew对应的扩?#20849;?#38271;减半,直至连接所述最近邻的位形点和所述新位形点qnew之间的路径没有与障碍物发生碰撞。
进一步地,所述随机采样位?#20301;?#21462;子模块利用加权采样方法,从位形空间随机采样获得一个随机采样位形点qrand。
本发明实施例提供的机器人避碰路径的规划装置可以应用在前述对应的方法实施例一中,详情参见上述实施例一的描述,在此不再赘述。
?#26723;?#27880;意的是,上述系?#21576;?#26045;例中,所包括的各个单元只?#21069;?#29031;功能逻辑进行划分的,但并不局限于上述的划分,只要能够实现相应的功能即可;另外,各功能单元的具体名称也只是为了便于相互区分,并不用于限制本发明的保护?#27573;А?
以上所述仅为本发明的较佳实施例而已,并不用以限制本发明,凡在本发明的精神和原则之内所作的任何修改、等同替换?#36879;?#36827;等,均应包含在本发明的保护?#27573;?#20043;内。

关于本文
本文标题:一种机器人避碰路径的规划方法、装置.pdf
链接地址:http://www.pqiex.tw/p-6124312.html
关于我们 - 网站声明 - 网站地图 - 资源地图 - 友情链接 - 网站客服 - 联系我们

[email protected] 2017-2018 zhuanlichaxun.net网站版权所有
经营许可证编号:粤ICP备17046363号-1 
 


收起
展开
平码五不中公式规律 22选5选号方法大全(一) 爱玩棋牌游戏中心下载 海南环岛赛车彩票网站下载 新疆时时彩开奖直播现场 上海时时乐代理 pk10教你快速看会走势图 山东11选5走势图前三走势图 海南环岛赛指标统计 京东方a股票 怎么五分彩的技巧