[发明专利]一种机器人避碰路径的规划方法、装置

权利要求书

1.一种机器人避碰路径的规划方法，其特征在于，所述方法包括：

步骤1、根据机器人的初始位形q_s和目标位形q_g计算得到碰撞检测距离参数的初始值r_col；

步骤2、根据所述初始位形q_s、目标位形q_g构造双向快速密集搜索树，所述双向快速密集搜索树包括正向快速密集搜索树和逆向快速密集搜索树；

步骤3、设置所述双向快速密集搜索树的碰撞检测距离参数的值为r_col；

步骤4、根据所述初始位形q_s、目标位形q_g采用自适应双向快速密集搜索树动态扩展方法生成一条机器人规划路径；

步骤5、对所述机器人规划路径上的离散采样位形点分别进行碰撞检测，如果所有离散位形点均没有与障碍物发生碰撞，则将由所有离散采样位形点组成的机器人规划路径作为有效路径，如果有离散位形点与障碍物发生碰撞，则执行步骤6；

步骤6、更新r_col的值为r_col/2；

步骤7、循环执行步骤3至6，直至生成的机器人规划路径上的所有离散位形点均没有与障碍物发生碰撞为止。

2.如权利要求1所述的方法，其特征在于，所述步骤4包括：

根据所述初始位形q_s、目标位形q_g计算得到扩展步长的初始值r_ext；

根据所述初始位形q_s、目标位形q_g和所述扩展步长的初始值r_ext配置所述双向快速密集搜索树；

局部调整所述扩展步长的初始值r_ext，获得一条机器人规划路径。

3.如权利要求2所述的方法，其特征在于，所述局部调整所述扩展步长的初始值r_ext，获得一条机器人规划路径包括：

从位形空间随机采样获得一个随机采样位形点q_rand；

搜索所述正向快速密集搜索树上与所述随机采样位形q_rand最近邻的边e₁；

在边e₁上搜索与所述随机采样位形q_rand最近邻的位形点

根据所述q_rand、以及所述扩展步长r_ext计算得到不与障碍物发生碰撞的新位形点q_new；

根据所述最近邻的位形点以及所述新位形点q_new做碰撞检测；

如果连接所述最近邻的位形点和所述新位形点q_new之间的路径没有与障碍物发生碰撞，则搜索所述逆向快速密集搜索树上与所述新位形点q_new最近邻的边e₂；

在边e₂上搜索与所述新位形点q_new最近邻的位形点

根据所述最近邻的位形点以及所述新位形点q_new做碰撞检测；

若连接所述最近邻的位形点和所述新位形点q_new之间的路径没有与障碍物发生碰撞时，将所述边e₂、所述最近邻的位形点和所述新位形点q_new添加至所述逆向快速密集搜索树中，并获得新边e_new，同时分别使与所述边e₂和所述新边e_new对应的扩展步长r_ext加倍；

若连接所述最近邻的位形点和所述新位形点q_new之间的路径与障碍物发生碰撞时，计算机器人从向q_new移动而不与障碍物发生碰撞的最合适的位形点，将所述最适合的位形点、所述边e₂和所述新位形点q_new添加至所述逆向快速密集搜索树中，并获得新边e_new，同时分别使与所述边e₂和所述新边e_new对应的扩展步长r_ext减半，直至连接所述最近邻的位形点和所述新位形点q_new之间的路径没有与障碍物发生碰撞。