[发明专利]一种基于概率地图的工业机器人路径搜索优化算法

摘要

本发明公开一种基于概率地图的工业机器人路径搜索优化算法，所述算法包括以下步骤：(1)在工业机器人末端运动的位姿空间中，运用碰撞检测算法选取若干无碰撞的采样位姿点，放入采样位姿点集V；(2)采用局部路径规划器选取采样位姿点之间的局部安全路径，放入边集E；(3)采样位姿点集V和边集E构成所需的概率地图G；(4)输入工业机器人末端运动的起始点与终止点；(5)采用A*算法搜索路径，获取全局无碰撞路径；(6)采用路径优化算法对所得路径进行优化，获得全局优化路径；所述算法在已知环境下，只输入机器人末端运动的起始点和终止点，即搜索得出一条全局优化路径，有效应用于工业机器人末端运动路径的规划。

主权项

1.一种基于概率地图的工业机器人路径搜索优化算法，其特征在于，只需输入机器人运动路径的起始点和终止点，一种基于概率地图的工业机器人路径搜索优化算法便自动搜索出一条全局优化路径；一种基于概率地图的工业机器人路径搜索优化算法分为学习阶段和查询阶段，分别包含以下几个步骤：学习阶段：(1)在OpenGL仿真环境内以实际尺寸与虚拟尺寸按比例建立机器人及其周边环境障碍物的三维模型；(2)在OpenGL仿真环境中，运用碰撞检测算法对工业机器人末端的位姿空间进行随机采样，选取无碰撞的采样位姿点q；所述的碰撞检测算法为选取工业机器人位姿空间中无碰撞的采样位姿点q的碰撞检测算法；所述的碰撞检测的本质为判断机器人末端位于该采样位姿点、离散点或节点时，是否与周围环境物体发生碰撞；(3)将无碰撞的采样位姿点q加入采样位姿点集V中；所述的采样位姿点集V是概率地图数学表达式G的重要组成部分，而概率地图数学表达式G作为描述概率地图学习阶段中路径搜索空间的重要模型；(4)采用局部路径规划器，判断步骤(2)中所述的无碰撞的采样位姿点q与该采样位姿点q的邻域Nq内其他采样位姿点q`之间的局部路径是否为局部安全路径；若是局部安全路径，则顺序执行学习阶段中的步骤(5)；否则舍弃该段局部路径，执行学习阶段中的步骤(6)；所述的局部路径规划器是将局部路径离散成若干离散点，并对各离散点进行判断，若局部路径间的各离散点均与周围环境物体不发生碰撞，则视其为无碰撞的采样位姿点q与采样位姿点q`之间的局部安全路径；否则舍弃掉该局部路径，接着判断无碰撞的采样位姿点q与剩余的采样位姿点q`之间的局部路径；(5)将学习阶段步骤(4)中所得的局部安全路径保存在边集E中；所述的边集E是概率地图数学表达式G的另一个重要组成部分；(6)判断学习阶段步骤(3)中所得的采样位姿点q的邻域Nq内所有采样位姿点q`间的局部路径是否都用局部路径规划器判断完毕；若判断完毕，则顺序执行学习阶段中的步骤(7)；若没有判断完毕，则返回学习阶段中的步骤(4)执行，判断步骤(3)中所述的采样位姿点q与其邻域Nq内剩余的采样位姿点q`之间的局部路径；(7)判断当前采样位姿点数s是否小于设定的最大采样位姿点数smax；若小于最大采样位姿点数smax，则返回学习阶段中的步骤(2)执行；否则顺序执行学习阶段中的步骤(8)；所述的当前采样位姿点数s表示已加入采样位姿点集V中的采样位姿点个数；最大采样位姿点数smax表示在构建概率地图之初，根据需要设定的采样位姿点q的个数；在机器人位姿空间中设置的最大采样位姿点q的个数越多，即采样位姿点q分布得越密集，路径搜索成功率越高；而设定过多的采样位姿点q的个数会影响算法执行效率；(8)保存所有得到的无碰撞的采样位姿点集V与局部安全路径边集E，构成所需的概率地图G的表达形式；查询阶段：(1)在学习阶段构建成功的概率地图中，输入工业机器人末端路径运动的起始点qstart与终止点qgoal；输入的起始点qstart与终止点qgoal是用于搜索所需的安全无碰撞路径；(2)判断起始点qstart是否在已构建的概率地图中；若起始点qstart在已构建的概率地图中，则执行查询阶段中的步骤(4)；否则顺序执行查询阶段中的步骤(3)；(3)将起始点qstart移入概率地图G中；确保起始点qstart在概率地图内，将起始点qstart等同于采样位姿点q保存在概率地图G中；(4)判断终止点qgoal是否在已构建的概率地图中；若终止点qgoal在已构建的概率地图中，则执行查询阶段中的步骤(6)；否则顺序执行查询阶段中的步骤(5)；(5)将终止点qgoal移入概率地图G中；确保终止点qgoal在概率地图内，将终止点qgoal等同于采样位姿点q保存在概率地图G中；(6)采用A*算法进行全局路径搜索，得到一条连接起始点qstart与终止点qgoal的全局安全路径；所述的A*算法是一种求解路径的最直接有效的搜索方法，应用于路径搜索；A*算法从起始点qstart开始第一轮路径搜索，以起始点qstart作为当前节点，通过在当前节点邻域Nn里寻找最小估价值的其它节点，确保他们之间的局部路径无碰撞；接着以搜索得到的最小估价值节点为下一轮路径搜索的当前节点，再从下一轮路径搜索的当前节点开始，搜索最小估价值节点，以此往复操作，逐渐向终止点qgoal逼近；依次连接起始点qstart到终止点qgoal间的各搜索所得节点，最终得到一条全局无碰撞路径；所述的节点，即为学习阶段中的采样位姿点q；所述的估价值是A*算法搜索路径过程中评价路径长短与否的重要尺标，越小的估价值，表明该段路径越短；(7)采用路径优化算法对A*算法搜索所得的全局路径进行优化，以获得全局优化路径；所述的路径优化算法运用反向遍历的处理方法，从目标节点到初始节点的方向，采用学习阶段步骤(4)中所述的局部路径规划器对目标节点与初始节点之间的路径进行判断，若该路径为安全路径，则直接采用该路径为最终路径；否则以目标节点的前一节点作为当前节点，用局部路径规划器对当前节点与初始节点之间的路径进行判断；若该路径安全，则采用该路径；否则再用局部路径规划器判断当前节点的前一节点与初始节点之间的路径；以此按照反向遍历的顺序重复上述步骤，直至遍历完所有节点为止；所述的初始节点对应查询阶段中的步骤(1)所述的起始点qstart，即在A*算法里将起始点qstart表述为初始节点；所述的目标节点对应查询阶段中步骤(1)所述的终止点qgoal，即在A*算法里将终止点qgoal表述为目标节点。