[发明专利]一种三维避障的室内机器人导航方法和系统

摘要

本发明公开了一种三维避障的室内机器人导航方法和系统，其中方法的实现包括：采集机器人的三维模型、初始位置和终点位置，全局三维地图并进行全局路径规划，生成一系列全局路径点，在每一全局路径点处进行三维障碍物碰撞检测，得到最优的全局路径规划；机器人按最优的全局路径规划行进，在行进过程中对下一全局路径点进行三维障碍物碰撞检测，若会发生碰撞，则以实时三维地图为全局三维地图，以实时位置为初始位置重新进行全局路径规划，若不会发生碰撞，则继续行进直至达到终点位置，实现了机器人的自主导航与三维避障。本发明在拥挤的室内环境下也能顺利求解出可行的机器人路径，完成了机器人从初始位置到终点位置的自主导航和三维避障任务。

主权项

1.一种三维避障的室内机器人导航方法，其特征在于，包括：(1)采集机器人的三维模型、机器人行进的初始位置和终点位置，以及包含初始位置和终点位置的全局三维地图；(2)根据初始位置、终点位置和全局三维地图进行全局路径规划，生成一系列全局路径点，在每一全局路径点处进行三维障碍物碰撞检测，若不会发生碰撞，则得到最优的全局路径规划，若会发生碰撞，则根据初始位置、终点位置和全局三维地图重新进行全局路径规划直至避开障碍物，得到最优的全局路径规划；(3)机器人按照最优的全局路径规划行进，采集机器人的实时位置，拍摄包含实时位置和下一全局路径点的局部三维地图，利用局部三维地图更新全局三维地图得到实时三维地图；(4)在行进过程中对下一全局路径点进行三维障碍物碰撞检测，若会发生碰撞，则以实时三维地图为全局三维地图，以机器人实时位置为初始位置，返回步骤(2)，若不会发生碰撞，则继续行进直至达到终点位置，实现了机器人的自主导航与三维避障；所述三维障碍物碰撞检测的具体实现方式为：(2‑1)机器人在每一全局路径点处的三维模型表面由N个二维多边形组成，遍历机器人三维模型表面的N个多边形，对N个多边形使用启发式的算法得到N个基元球体；(2‑2)使用启发式的算法得到一个能将N个基元球体包围的大球体；(2‑3)使用宽度优先算法构建一个模型二叉树结构，使用大球体作为模型二叉树结构的根节点，父节点表示一个能将n个基元球体包围的父球体，子节点表示一个能将n/2个基元球体包围的子球体，最底层节点表示一个能将1个基元球体包围的最底层球体，n≤N；(2‑4)全局三维地图表面由N个全局二维多边形组成，遍历全局三维地图表面的N个全局多边形，对N个全局多边形使用启发式的算法得到N个全局基元球体；使用启发式的算法得到一个能将N个全局基元球体包围的全局大球体；(2‑5)使用宽度优先算法构建一个全局二叉树结构，使用全局大球体作为全局二叉树结构的全局根节点，全局父节点表示一个能将n个全局基元球体包围的全局父球体，全局子节点表示一个能将n/2个全局基元球体包围的全局子球体，全局最底层节点表示一个能将1个全局基元球体包围的全局最底层球体，n≤N；(2‑6)设置模型二叉树结构和全局二叉树结构之间的距离为无穷大，从模型二叉树结构和全局二叉树结构中分别选取节点；(2‑7)计算选取节点对应的球体之间的球心距离，若该距离大于等于模型二叉树结构和全局二叉树结构之间的距离，则三维模型和全局三维地图没有相交，机器人不会发生碰撞，流程结束，否则检查两个节点是否都是最底层节点，若是，则进入步骤(2‑8)，若不是，则进入步骤(2‑10)；(2‑8)检查选定两个节点代表的球体所包覆的多边形是否在三维空间相交，若是，则确认机器人和全局三维地图将发生碰撞，否则设置两个节点代表的球体所包覆的多边形之间的距离为机器人模型二叉树结构和全局二叉树结构之间的距离；(2‑9)从机器人模型二叉树结构和全局二叉树结构中未检索过的节点中各取一个节点并选定，返回步骤(2‑7)；(2‑10)比较两个节点在模型二叉树结构和全局二叉树结构中的层级，若相同，则随机选取一个节点，否则选取层级较高的节点，选取节点的两个子节点并比较两个子节点与另一选定节点代表的球体之间的距离，选取距离较近的子节点代替其父节点，返回步骤(2‑7)。