一种基于概率地图的工业机器人路径搜索优化算法
【专利摘要】本发明公开一种基于概率地图的工业机器人路径搜索优化算法，所述算法包括以下步骤：(1)在工业机器人末端运动的位姿空间中，运用碰撞检测算法选取若干无碰撞的采样位姿点，放入采样位姿点集V；(2)采用局部路径规划器选取采样位姿点之间的局部安全路径，放入边集E；(3)采样位姿点集V和边集E构成所需的概率地图G；(4)输入工业机器人末端运动的起始点与终止点；(5)采用A*算法搜索路径，获取全局无碰撞路径；(6)采用路径优化算法对所得路径进行优化，获得全局优化路径；所述算法在已知环境下，只输入机器人末端运动的起始点和终止点，即搜索得出一条全局优化路径，有效应用于工业机器人末端运动路径的规划。
【专利说明】
一种基于概率地图的工业机器人路径搜索优化算法
技术领域
[0001] 该发明涉及机器人路径搜索领域，特别是机器人路径搜索技术中一种基于概率地 图的工业机器人路径搜索优化算法。
【背景技术】
[0002] 在工业机器人进行装配、焊接、喷涂等作业时，采用良好的路径搜索方法不仅可以 节省机器人作业时间、节约人力资源，而且可以避免机器人损坏，保证机器人安全作业。
[0003] 传统的工业机器人路径搜索方法通过在线示教的方式实现，为保证机器人作业过 程中和周边环境的障碍物不发生碰撞，路径搜索时需要进行多次调试，这样不仅费时、费 力、效率低，而且在搜索完成后不便于进行优化处理。所以无碰撞路径的自动搜索算法是保 证工业机器人安全、高效作业的关键技术。
[0004] 现有的路径搜索算法可分为盲目式搜索算法和启发式搜索算法。其中盲目式搜索 算法有广度优先算法、深度优先算法和Dijkstra算法等，这些算法不关心路径搜索问题本 身的特点，在搜索过程中均按照事先设定的策略进行搜索，不会根据所得的信息对搜索过 程加以优化。而A*搜索算法作为启发式搜索算法，更为关注路径搜索问题的本身，即从初始 节点开始，每次搜索都寻找相对当前节点估价值最小的节点作为下次搜索的当前节点，直 至搜索到目标节点为止。将A*算法应用于机器人的路径搜索，本发明提出一种基于概率地 图的工业机器人路径搜索优化算法，引入概率地图选取机器人可达的路径搜索节点，再采 用路径优化算法优化A*算法搜索所得的路径。
【发明内容】
[0005] 本发明旨在提供一种基于概率地图的工业机器人路径搜索优化算法，首先引入概 率地图构建包含机器人所有可行路径的无向图，再将A*算法用于机器人的路径搜索，最后 采用路径优化算法对所得路径进行优化，使得在路径搜索过程当中，操作者只需要输入机 器人末端路径运动的起始点q start和终止点qg〇ai，工业机器人便可自动搜索出一条无碰撞的 全局优化路径。为实现在复杂环境下的工业机器人路径搜索与优化，本发明采用一种基于 概率地图的工业机器人路径搜索优化算法，其技术方案如下：
[0006] -种基于概率地图的工业机器人路径搜索优化算法分为两个阶段:学习阶段与查 询阶段。在学习阶段时，在机器人位姿空间中进行随机采样，选取无碰撞的采样位姿点后， 再用局部路径规划器选取无碰撞采样位姿点间的局部安全路径，得到无碰撞的采样位姿点 与局部安全路径则构成所需的概率地图。在查询阶段时，输入机器人运动的起始点q start与 终止点，在所构建的概率地图中采用A*算法进行路径搜索，得到一条连接起始点 qstart与终止Aqg〇al的全局无碰撞路径，最后采用路径优化算法对该路径进行优化，获得全 局优化路径，具体包括以下步骤：
[0007] 学习阶段：
[0008] (1)在OpenGL仿真环境内以实际尺寸与虚拟尺寸按比例建立机器人及其周边环境 障碍物的三维模型；
[0009] (2)在OpenGL仿真环境中，运用碰撞检测算法对工业机器人末端的位姿空间进行 随机采样，选取无碰撞的采样位姿点q;
[0010] 所述的碰撞检测算法为选取工业机器人位姿空间中无碰撞的采样位姿点q的碰撞 检测算法;所述的碰撞检测的本质为判断机器人末端位于该采样位姿点、离散点或节点时， 是否与周围环境物体发生碰撞；
[0011] (3)将无碰撞的采样位姿点q加入采样位姿点集V中；
[0012] 所述的采样位姿点集V是概率地图数学表达式G的两个重要组成部分之一，而概率 地图数学表达式G作为描述概率地图学习阶段中路径搜索空间的重要模型；
[0013] (4)采用局部路径规划器，判断步骤(2)中所述的无碰撞的采样位姿点q与该采样 位姿点q的邻域N q内其他采样位姿点q'之间的局部路径是否为局部安全路径;若是局部安 全路径，则顺序执行学习阶段中的步骤(5);否则舍弃该段局部路径，执行学习阶段中的步 骤⑻；
[0014] 所述的局部路径规划器是将局部路径离散成若干离散点，并对各离散点进行判 断，若局部路径间的各离散点均与周围环境物体不发生碰撞，则视其为无碰撞的采样位姿 点q与采样位姿点q'之间的局部安全路径;否则舍弃掉该局部路径，接着判断无碰撞的采样 位姿点q与剩余的采样位姿点q'间的局部路径；
[0015] (5)将学习阶段步骤(4)中所得的局部安全路径保存在边集E中；
[0016] 所述的边集E是概率地图数学表达式G的另一个重要组成部分；
[0017] (6)判断学习阶段步骤(3)中所得的采样位姿点q的邻域Nq内所有采样位姿点q'间 的局部路径是否都用局部路径规划器判断完毕;若判断完毕，则顺序执行学习阶段中的步 骤(7);若没有判断完毕，则返回学习阶段中的步骤(4)执行，判断步骤(3)中所述的采样位 姿点q与其邻域Nq内剩余采样位姿点q'之间的局部路径；
[0018] (7)判断当前采样位姿点数s是否小于设定的最大采样位姿点数smax;若小于最大 采样位姿点数smax，则返回学习阶段中的步骤（2)执行；否则顺序执行学习阶段中的步骤 (8)；
[0019]所述的当前采样位姿点数s表示已加入采样位姿点集V中的采样位姿点个数;最大 采样位姿点数smax表示在构建概率地图之初，根据需要设定的采样位姿点q的个数;在机器 人位姿空间中设置的最大采样位姿点q的个数越多，即采样位姿点q分布得越密集，路径搜 索成功率越高;而设定过多的采样位姿点q的个数会影响算法执行效率；
[0020] (8)保存所有得到的无碰撞的采样位姿点集V与局部安全路径边集E，构成所需的 概率地图G的表达形式；
[0021] 查询阶段：
[0022] (1)在学习阶段构建成功的概率地图中，输入工业机器人末端路径运动的起始点 Qstart-^=3^§'_lll/l7?NQgoal ；
[0023]输入的起始点qstart与终止点qgoal是用于搜索所需的安全无碰撞路径；
[0024] (2)判断起始点qstart是否在已构建的概率地图中；若起始点qstart在已构建的概率 地图中，则执行查询阶段中的步骤(4);否则顺序执行查询阶段中的步骤(3);
[0025] (3)将起始点qstart移入概率地图G中；
[0026]确保起始点qstart在概率地图内，将起始点qstart等同于采样位姿点q保存在概率地 图G中；
[0027] (4)判断终止点qg。al是否在已构建的概率地图中；若终止Aqgcial在已构建的概率地 图中，则执行查询阶段中的步骤(6);否则顺序执行查询阶段中的步骤(5);
[0028] (5)将终止点qg〇ai移入概率地图G中；
[0029]确保终止点qgcial在概率地图内，将终止点qgcial等同于采样位姿点q保存在概率地图 G中；
[0030] (6)采用A*算法进行全局路径搜索，得到一条连接起始点qstart与终止点如。31的全 局安全路径；
[0031] 所述的A*算法是一种求解路径的最直接有效的搜索方法，应用于路径搜索;A*算 法从起始点qstart开始第一轮路径搜索，以起始点qstart作为当前节点，通过在当前节点邻域 仏里寻找最小估价值的其它节点，确保他们之间的局部路径无碰撞;接着以搜索得到的最 小估价值节点为下一轮路径搜索的当前节点，再从下一轮路径搜索的当前节点开始，搜索 最小估价值节点，以此往复操作，逐渐向终止点qgoal逼近;依次连接起始点qstart到终止点 qg〇al间的各搜索所得节点，最终得到一条全局无碰撞路径;所述的节点，即为学习阶段中的 采样位姿点q;所述的估价值是A*算法搜索路径过程中评价路径长短与否的重要尺标，越小 的估价值，表明该段路径越短；
[0032] (7)采用路径优化算法对A*算法搜索所得的全局路径进行优化，以获得全局优化 路径；
[0033]其中，所述的学习阶段的步骤（1)中机器人与周围环境障碍物的虚拟模型在 OpenGL仿真环境内，以实际尺寸与虚拟尺寸按照1:1的比例建立，或根据用户需要的比例建 立。
[0034] 其中，所述的概率地图的各参数表达形式如下描述：
[0035] 机器人概率地图以无向图G的形式进行表示，它由采样位姿点集V与边集E两部分 组成，如公式1所示。采样位姿点集V由无碰撞的随机采样位姿点组成，其采样点数为最大采 样点数s max。边集E由经过局部路径规划器检测过的无碰撞局部安全路径组成，其总边数在 概率地图构建完成前是无法获知的。
[0036] G=(V，E) (1)
[0037] 机器人的位姿一般机器人的位姿一般用如公式2所示的方式在欧氏空间中进行表 示，但也用如公式3所示的方式在位姿空间中进行表示。机器人的采样位姿点q作为构成概 率地图的一个基本元素，最大采样位姿点数s max根据需要设定。采样位姿点q定义为
[0038] q=(x，y，z，a，b，c) (2)
[0039] q=(0i, 02,03,04,05,06) (3)
[0040] 式中，q为机器人采样位姿点;x，y，z分别为沿机器人直角坐标系x，y，z轴方向的坐 标;a，b，c分别为绕机器人直角坐标系X，y，z轴转动的角度;为机器人各关节转动角度值。 [0041]采样位姿点间的距离以机器人处于不同位姿时其末端之间的欧氏距离来表示，其 定义如公式4所示
[0043] 式中，DU，^)为机器人采样位姿点间的欧氏距离;Y为机器人采样位姿点q的邻 域中某个采样位姿点;pUhpUO为机器人在采样位姿点q，^时末端的位置; Xq，yq，Zq为采 样位姿点q的坐标;Xq'，yq'，Zq'为采样位姿点q'的坐标。
[0044] 采样位姿点q邻域的定义如公式5所示，即以采样位姿点q为球心，邻域阈值P为半 径，构建一个球型空间，在此空间内的所有采样位姿点与球心间的距离均小于等于邻域阈 值
[0045] Nq={qeV,q/ ) <p} (5)
[0046] 式中，Nq为采样位姿点q的邻域;P为邻域阈值。
[0047] 采样位姿点间的局部路径作为构成概率地图的另一个基本元素，其定义为
[0048] τ = (qi,qi) (6)
[0049] 式中，τ为局部路径;qi，qj为采样位姿点。
[0050] 其中，学习阶段步骤(4)中所述的局部路径规划器采用"二分法"形式的局部路径 规划器;所述的"二分法"形式的局部路径规划器采用如公式7所示方法对局部路径进行离 散化。引入约束条件为局部路径检测阈值，其定义如公式8所示。初始时离散程度k为1，以1 为插补步长对局部路径进行离散，再对所有离散点处的机器人进行碰撞检测，若均不发生 碰撞，则离散程度k加1，即k为2,重新计算插补步长1后再对局部路径进行离散，此时只需对 新生成的离散点处的机器人进行碰撞检测，若新生成的离散点处均不发生碰撞，则离散程 度k继续加1，并重复上述步骤，直至发生碰撞或当插补步长1小于局部路径检测阈值ε时还 未发生过碰撞才结束操作。在采用局部路径规划器对该局部路径进行处理的过程中机器人 会发生碰撞，则认为该路径是不可行的，反之则认为该路径是可行的。
[0052] 1<ε (8)
[0053] 式中，1为离散线段长度;L为局部路径长度;k为离散程度;ε为局部路径检测阈值。
[0054] 所述的"二分法"形式的局部路径规划器是将两点之间的局部路径离散成各个离 散点；每一次"二分法"形式的局部路径规划器的操作，是在每两个离散点间等距的插入一 个新的离散点，判断插入后的新的离散点是否发生碰撞;若不发生碰撞，则再在每两个离散 点间等距的插入一个新的离散点，并对其进行碰撞检测，以此往复操作，直至两个离散点之 间的距离小于设定的局部路径检测阈值ε，局部路径规划器的操作终止，认为两点之间的局 部路径为局部安全路径，两点之间的局部路径不发生碰撞；否则认为两点之间的局部路径 碰撞，不为局部安全路径。输入需要判断的局部路径的起点和终点转角信息后，所述的局部 路径规划器的具体操作处理流程分为以下几步：
[0055] (1)求欧氏空间内局部路径的起点位姿与终点位姿及其长度；
[0056] 因为起点和终点是在机器人的位姿空间内进行采样得到的，其表示形式如公式3 所示，通过机器人正运动学求出欧氏空间内局部路径的起点位姿与终点位姿，转换得到的 点的信息如公式2所示，同时通过公式4计算出局部路径的长度，为之后局部路径的离散化 做准备。
[0057] (2)判断局部路径长度L是否小于局部路径检测阈值ε ;
[0058] 当局部路径长度L小于局部路径检测阈值ε时，则认为该局部路径是不会发生碰撞 的，即判断其为可行路径，并结束局部路径规划器的操作处理，否则需要继续步骤(3)的操 作。
[0059] (3)初始化离散程度k，即让k = l，并通过公式7计算出位置插补步长1;
[0060] (4)对局部路径进行插补，将其离散化；
[0061] 确定插补步数n(n = 2k+l)，并以此计算出姿态插补步长]/，然后通过起点位姿、位 置插补步长和姿态插补步长求出每个插补点的位姿矩阵，最后通过机器人逆运动学求解出 机器人位姿空间内的每个插补点。
[0062] (5)对新生成的离散插补点进行碰撞检测；
[0063] 为避免做无用功，无需对之前采用二分法得到的离散点重复进行碰撞检测，只需 对新生成的离散点进行碰撞检测。若存在发生碰撞的离散点，则判断该局部路径为不可行 路径，并结束局部路径规划器的操作处理，否则需要继续步骤(6)的操作。
[0064] (6)将离散程度k加 l(k = k+l)，并重新计算位置插补步长1;
[0065] (7)判断位置插补步长1是否小于局部路径检测阈值ε ;
[0066] 当位置插补步长1小于局部路径检测阈值ε时，则认为该局部路径是不会发生碰撞 的，即判断其为可行路径，并结束局部路径规划器的操作处理，否则返回步骤(4)。
[0067]其中，所述的学习阶段的步骤(2)中所述的碰撞检测算法为0ΒΒ包围盒的碰撞检测 算法，或为RAPID碰撞检测算法，或为混合包围体层次树碰撞检测算法等。所述的混合包围 体层次树算法采用由顶层、中间层和底层3层结构构成的包围体层次树技术;如果所述的混 合包围体层次树中父结点包围体不存在碰撞，则无须对子结点包围体进行碰撞检测，以此 加快碰撞检测速度;所述的混合包围体层次树随着机器人实际运动时各连杆间相对位置的 变化而动态更新，适用于机器人的路径搜索中判断采样位姿点或节点或离散点是否发生碰 撞。对采样位姿点或节点或离散点的碰撞检测，实质上就是对当机器人末端位置位于该采 样位姿点或节点或离散点时，机器人是否与周围环境中存在的障碍物发生碰撞的判断。
[0068] 所述的混合包围体层次树碰撞检测算法操作时，先构建机器人和周围环境障碍物 的混合包围体层次树顶层、中间层和底层包围体结构，再依次按照顶层，中间层，底层的顺 序进行机器人的碰撞检测;如果顶层中没有发生顶-底层球包围体相交，则不进行中间层和 底层的碰撞检测；如果中间层中没有中-底层0ΒΒ包围体相交，则不进行底层碰撞检测。机器 人的碰撞检测的执行过程按照以下3步：
[0069] (1)进行顶层层次树包围体相交测试;若包围体与包围体之间相交，得到顶层中相 交的顶-底层球包围体，从而获得需要执行相交测试的中间层0ΒΒ包围体层次树的中-顶层 0ΒΒ包围体，执行步骤(2);否则输出结果为不发生碰撞；
[0070] (2)进行中间层0ΒΒ包围体相交测试;若包围体与包围体之间相交，获取0ΒΒ包围体 层次树中相交的中-底层0ΒΒ包围体，执行步骤(3);否则输出结果为不发生碰撞；
[0071] (3)进行底层中三角面片的相交测试得到相交三角面片对;若三角面片之间相交， 则输出结果为碰撞;否则输出结果为不发生碰撞。
[0072] 其中，所述的查询阶段的步骤(7)中所述的路径优化算法运用反向遍历的处理方 法，从目标节点到初始节点的方向，采用学习阶段步骤(4)中所述的局部路径规划器对目标 节点与初始节点之间的路径进行判断，若该路径为安全路径，则直接采用该路径为最终路 径；否则以目标节点的前一节点作为当前节点，用局部路径规划器对当前节点与初始节点 之间的路径进行判断;若该路径安全，则采用该路径;否则再用局部路径规划器判断当前节 点的前一节点与初始节点之间的路径；以此按照反向遍历的顺序重复上述步骤，直至遍历 完所有节点为止;所述的初始节点对应查询阶段中的步骤⑴所述的起始点q start，即在A*算 法里将起始点qstart表述为初始节点;所述的目标节点对应查询阶段中步骤(1)所述的终止 点q g〇ai，即在A*算法里将终止点&。31表述为目标节点。所述的路径优化算法具体流程分为以 下几步：
[0073] (1)将CLOSED表的表头节点m与表尾节点nn分别赋予遍历目标节点n st〇P与遍历当 前节点η current ；
[0074] 该算法的遍历顺序是反向遍历，即按从CLOSED表的表尾节点nn到表头节点m的方 向进行遍历，所以进行初始化时有遍历目标节点nst〇P = ni，遍历当前节点ncmmntinn。所述 的CLOSED表用于存放最终的路径节点m;其中，i = l，2,3.;
[0075] (2)将遍历目标节点nstop与遍历当前节点nmnt作为局部优化路径的起点与终点；
[0076] 节点遍历的过程为反向遍历，路径运行的方向为正向运行，故遍历目标节点nstop 对应局部优化路径的起点，遍历当前节点n?rrent对应局部优化路径的终点。
[0077] (3)用局部路径规划器将该局部优化路径离散化，并对其进行碰撞检测；
[0078] 若机器人按照该局部优化路径进行运动时发生碰撞，则继续执行步骤(4);否则转 到步骤(5)。
[0079] (4)将遍历当前节点n〇jrre3nt的前一个节点赋予遍历当前节点nmnt;
[0080] 假设之前n〇jrrent = m，那么现在令ntiurrent = m-1，然后转到步骤(6 )。
[0081] (5)保存该局部优化路径，然后将遍历当前节点n?rre3nt赋予遍历目标节点n stcipd# CLOSED表的表尾节点nn赋予遍历当前节点nmnt;
[0082] 假设之前 nst〇P = nj ，Ilcurrent - Π? j iSl^Tlstop - Ilcurrent，Ilcurrent - Πη〇
[0083] (6)遍历目标节点nstcip是否为CLOSED表的表尾节点nn;
[0084] 若遍历目标节点nstcip是为CLOSED表的表尾节点nn，则继续执行步骤(7);否则返回 步骤(2)。
[0085] (7)将之前保存的局部优化路径按顺序进行连接得到全局优化路径。
[0086]本发明的特点和有益效果在于：
[0087] (1)在操作时只需输入机器人运动的起始点与终止点的位姿信息，即成功搜索出 一条无碰撞的优化路径。
[0088] (2)采用碰撞检测算法能快速、高效的构建概率地图和搜索路径。
[0089] (3)引入路径优化算法对搜索所得的路径进行优化，使最终路径曲折更少。
【附图说明】
[0090] 图1 一种基于概率地图的工业机器人路径搜索优化算法的操作流程 [0091 ]图2-种基于概率地图的工业机器人路径搜索优化算法的具体操作流程 [0092]图3 "二分法"形式的局部路径规划器操作处理流程
[0093]图4顶层多层层次树结构图
[0094]图5中间层多层层次树结构图一一偶数个三角面片
[0095]图6中间层多层层次树结构图一一奇数个三角面片
[0096]图7混合包围体层次树的执行过程 [0097]图8路径优化算法流程
【具体实施方式】
[0098]在下文中将结合附图对本发明的示范性实施例进行描述。
[0099]所述的一种基于概率地图的工业机器人路径搜索优化算法的操作流程如图1所 示。因以6自由度关节型机器人为示范对象，设定机器人路径运行起始点qstart的关节转角值 (-41.6，-47.7，-0.2,0,47.9，-41.6)与机器人路径运行终止点9哪1的关节转角值（3.3，_ 42.2，-27.1，91.2,86.9，-2〇.9)，设定采样位姿点、节点的邻域阈值0为2〇〇臟，最大采样位 姿点数 Smax为5000，局部路径检测阈值ε为0.5mm。所述的一种基于概率地图的工业机器人路 径搜索优化算法的具体操作流程如图2所示，具体包括以下步骤：
[0100] 学习阶段：
[0101] (1)在OpenGL仿真环境内以实际与虚拟尺寸1:1的比例建立机器人及其周边环境 的三维模型；
[0102] (2)在OpenGL仿真环境中，运用混合包围体层次树碰撞检测算法对工业机器人末 端的位姿空间进行随机采样，选取无碰撞的采样位姿点q;
[0103] (3)将无碰撞的采样位姿点q加入采样位姿点集V中；
[0104] (4)采用局部路径规划器，判断步骤(2)中所述的无碰撞的采样位姿点q与该采样 位姿点q的邻域Nq内其他采样位姿点q'之间的局部路径是否为局部安全路径;若是局部安 全路径，则顺序执行学习阶段中的步骤(5);否则舍弃该段局部路径，执行学习阶段中的步 骤⑻；
[0105] (5)将学习阶段步骤(4)中所得的局部安全路径保存在边集E中；
[0106] (6)判断学习阶段步骤(3)中所得的采样位姿点q的邻域Nq内所有采样位姿点q'间 的局部路径是否都用局部路径规划器判断完毕;若判断完成，则顺序执行学习阶段中的步 骤(7);若没有判断完成，则返回学习阶段中的步骤(4)执行，判断步骤(3)中所述的采样位 姿点q与其邻域N q内剩余采样位姿点q'之间的局部路径；
[0107] (7)判断当前采样位姿点数s是否小于设定的最大采样位姿点数Smax;若小于最大 采样位姿点数smax，则返回学习阶段中的步骤（2)执行；否则顺序执行学习阶段中的步骤 (8)；
[0108] (8)保存所有得到的无碰撞的采样位姿点集V与局部安全路径边集E，构成所需的 概率地图G的表达形式；
[0109] 查询阶段：
[0110] (1)在学习阶段构建成功的概率地图中，输入工业机器人末端路径运动的起始点 Qstart-^=3^§'_lll/l7?NQgoal ；
[01 1 1 ]输入的起始点qstart与终止点qgoal是用于搜索所需的安全无碰撞路径；
[0112] (2)判断起始点qstart是否在已构建的概率地图中；若起始点qstart在已构建的概率 地图中，则执行查询阶段中的步骤(4);否则顺序执行查询阶段中的步骤(3);
[0113] (3)将起始点qstart移入概率地图G中；
[0114]确保起始点qstart在概率地图内，将起始点qstart等同于采样位姿点q保存在概率地 图G中；
[0115] (4)判断终止点qg。al是否在已构建的概率地图中；若终止Aqgcial在已构建的概率地 图中，则执行查询阶段中的步骤(6);否则顺序执行查询阶段中的步骤(5);
[0116] (5)将终止点qg〇ai移入概率地图G中；
[0117]确保终止点qgciai在概率地图内，将终止点qgciai等同于采样位姿点q保存在概率地图 G中；
[0118] (6)采用A*算法进行全局路径搜索，得到一条连接起始点qstart与终止点如。31的全 局安全路径；
[0119] (7)采用路径优化算法对A*算法搜索所得的全局路径进行优化，以获得全局优化 路径；
[0120] 其中，所述的学习阶段的步骤(4)中采用所述的局部路径规划器为"二分法"形式 的局部路径规划器;输入需要判断的局部路径的起点和终点转角信息后，所述的"二分法" 形式的局部路径规划器的具体操作处理流程如图3所示，其详细操作处理过程分为以下几 步：
[0121] (1)求欧氏空间内局部路径的起点位姿与终点位姿及其长度；
[0122] (2)判断局部路径长度L是否小于局部路径检测阈值ε;
[0123] (3)初始化离散程度k，即让k = l，并通过公式7计算出位置插补步长1;
[0124] (4)对局部路径进行插补，将其离散化；
[0125] (5)对新生成的离散插补点进行碰撞检测；
[0126] (6)将离散程度k加 l(k = k+l)，并重新计算位置插补步长1;
[0127] (7)判断位置插补步长1是否小于局部路径检测阈值ε;
[0128] 其中，各采样位姿点和路径内各节点或离散点间碰撞检测的方法是利用混合包围 体层次树碰撞检测算法进行碰撞检测的；所述的混合包围体层次树碰撞检测算法操作时， 先构建机器人的混合包围体层次树顶层、中间层和底层包围体结构，再依次按照顶层，中间 层，底层的顺序进行机器人的碰撞检测；如果顶层中没有发生顶-底层球包围体相交，则不 进行中间层和底层的碰撞检测；如果中间层中没有中-底层0ΒΒ包围体相交，则不进行底层 碰撞检测。其操作流程如下：
[0129] (1)构建机器人的混合包围体层次树顶层；
[0130] 混合包围体层次树的顶层选用0ΒΒ包围机器人连杆模型，并作为球包围体的构建 基元。采用多层层次树，顶层的构建采用自底向上方式和二叉树数据结构，如图4表示机器 人刚体数是7个时顶层层次树的结构形式，它由球包围体作为顶层包围体。图4中S 7表示基 座，S^ljS6分别代表连杆l-6，Si-j表示由刚体i-j构成的包围体(ieA={l，2,3,4,5,6}，jeB ={b | i<b < 7，i eA})，Si-7表示整个机器人。图4中每个结点Si-j均被1个以0ΒΒ为基元的球 包围；构建环境中障碍物的混合包围体的方法也是一样。
[0131] (2)构建机器人的混合包围体层次树中间层；
[0132] 机器人连杆模型是刚性的细长杆，采用0ΒΒ包围体来建立混合包围体层次树的中 间层。0ΒΒ包围体层次树的数量对应于连杆个数，其采用自顶而下方法和二叉树的数据结构 进行构建。当连杆坐标发生变化后，只需要更新中间层层次树中顶层0ΒΒ包围体的坐标，其 大小形状不变，无须重新对中间层层次树的中间层与底层进行构建，因而实现快速更新中 间层层次树;构建环境中障碍物的混合包围体的方法也是一样。
[0133] 当构成连杆的三角面片数量m为偶数时，中间层层次树如图5所示，该图是以m=10 为例的结构形式；当构成连杆的三角面片数量m为奇数时，中间层层次树为如图6所示，该图 是以m= 11为例的结构形式；图5、图6中Ti-j(i￡C={c|l< c<m}，jeD={d|i<d<m，ie (^表示由三角面片^构成的包围体"页层根结点^-^和^^卢设士义义义々々}. 5、图6中每个结点IV j均被1个OBB包围。
[0134] (3)构建机器人的混合包围体层次树底层；
[0135] 混合包围体层次树的底层是由中间层层次树的底层中发生碰撞的0ΒΒ包围体所包 围的三角面片构成，其中中间层层次树中底层的三角面片坐标是在机器人的基坐标系下， 而混合包围体层次树的底层的三角面片坐标是在世界坐标系下。而且其构建过程比较特 殊，它是在完成中间层碰撞检测后，当且仅当中间层中有中-底层0ΒΒ包围体发生碰撞时才 需要建立。建立时将中间层层次树的底层中发生碰撞的0ΒΒ包围体其所包围的三角面片顶 点坐标分别转换到世界坐标系下，转换后得到的三角面片构成混合包围体层次树的底层。
[0136] (4)混合包围体层次树的执行过程。如果顶层中没有发生顶-底层球包围体相交， 则不进行中间层和底层的碰撞检测；如果中间层中没有中-底层0ΒΒ包围体相交，则不进行 底层碰撞检测。
[0137]混合包围体层次树每个离散点的碰撞检测依次按照顶层，中间层，底层进行，如图 7所示。机器人与周边环境进行碰撞检测的执行过程按照以下3步：
[0138] (1)进行顶层层次树包围体相交测试;若包围体与包围体之间相交，得到顶层中相 交的顶-底层球包围体，从而获得需要执行相交测试的中间层0ΒΒ包围体层次树的中-顶层 0ΒΒ包围体，执行步骤(2);否则输出结果为不发生碰撞；
[0139] (2)进行中间层0ΒΒ包围体相交测试;若包围体与包围体之间相交，获取0ΒΒ包围体 层次树中相交的中-底层0ΒΒ包围体，执行步骤(3);否则输出结果为不发生碰撞；
[0140] (3)进行底层中三角面片的相交测试得到相交三角面片对;若三角面片之间相交， 则输出结果为碰撞;否则输出结果为不发生碰撞。
[0141] 对于一条包含多个采样位姿点或节点或离散点的路径，整个碰撞检测过程按照以 下5个步骤进行：
[0142] (1)构建包围连杆的0ΒΒ包围体基元；
[0143] (2)构建整个机器人的混合包围体层次树；
[0144] (3)在一个离散点处执行混合包围体层次树碰撞检测；
[0145] (4)重新计算混合包围体层次树顶层；
[0146] (5)返回执行(3)和(4)，直到完成该段路径内所有离散点碰撞检测。
[0147] 其中，所述的查询阶段步骤(7)中所述的路径优化算法具体流程如图8所示。所述 的路径优化算法的执行过程分为以下几步：
[0148] (1)将CLOSED表的表头节点m与表尾节点nn分别赋予遍历目标节点nstop与遍历当 前节点η current ；
[0149] (2)将遍历目标节点仏^与遍历当前节点n?rre3nt作为局部优化路径的起点与终点；
[0150] (3)用局部路径规划器将该局部优化路径离散化，并对其进行碰撞检测;若机器人 按照该局部优化路径进行运动时发生碰撞，则继续执行步骤(4);否则转到步骤(5)。
[0151] (4)将遍历当前节点ncmrrent的前一个节点赋予遍历当前节点nc^rrent;假设之前 ncurrent = ΓΗ，那么现在令ncurrent = ΓΗ-1，然后转到步骤(6 )。
[0152] (5)保存该局部优化路径，然后将遍历当前节点ncurrent赋予遍历目标节点n stcipd# CLOSED表的表尾节点nn赋予遍历当前节点nmnt;
[0?53] (6)遍历目标节点nstop是否为CLOSED表的表尾节点nn;若遍历目标节点nstop是为 CLOSED表的表尾节点nn，则继续执行步骤(7);否则返回步骤(2)。
[0154] (7)将之前保存的局部优化路径按顺序进行连接得到全局优化路径。
[0155] 依次连接CLOSED表中的所有节点，最终获得以起始点qstart为运动路径的起点和以 终止点q g〇ai为运动路径的终点的全局优化路径。
[0156] 根据本示范例中设定的机器人运动路径的起始点qstart与终止点qgrai，以及采样位 姿点、节点的邻域阈值P和局部路径检测阈值ε的具体信息，采用一种基于概率地图的工业 机器人路径搜索优化算法所得的最终路径长度为1364.22mm，且该路径为一条直线路径。其 中，邻域阈值P、最大采样位姿点数s max和局部路径检测阈值ε通常需要操作人员通过实验来 选取合适的值，或者操作人员凭借相关经验进行确定。
[0157] 最后说明的是，本发明的一种基于概率地图的工业机器人路径搜索优化算法不局 限于上述具体实施例。上述实施例仅是本发明较有代表性的例子，还能做出各种修改、变换 和变形。因此，说明书和附图应该被认为是说明性的而非限制性的。凡是依据本发明的技术 实质对以上实施例所作的任何简单修改、等同变化与修饰，均应认为属于本发明的保护范 围。
【主权项】
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) 判断终止点&。31是否在已构建的概率地图中；若终止点&。31在已构建的概率地图 中，则执行查询阶段中的步骤(6);否则顺序执行查询阶段中的步骤(5); (5) 将终止点qgrai移入概率地图G中； 确保终止点qg〇al在概率地图内，将终止点qg〇al等同于采样位姿点q保存在概率地图G中； (6) 采用A*算法进行全局路径搜索，得到一条连接起始点qstart与终止点&。31的全局安 全路径； 所述的A*算法是一种求解路径的最直接有效的搜索方法，应用于路径搜索;A*算法从 起始点qstart开始第一轮路径搜索，以起始点qstart作为当前节点，通过在当前节点邻域Nn里 寻找最小估价值的其它节点，确保他们之间的局部路径无碰撞;接着以搜索得到的最小估 价值节点为下一轮路径搜索的当前节点，再从下一轮路径搜索的当前节点开始，搜索最小 估价值节点，以此往复操作，逐渐向终止点qgoal逼近;依次连接起始点qstart到终止点qgoal间 的各搜索所得节点，最终得到一条全局无碰撞路径;所述的节点，即为学习阶段中的采样位 姿点q;所述的估价值是A*算法搜索路径过程中评价路径长短与否的重要尺标，越小的估价 值，表明该段路径越短； (7) 采用路径优化算法对A*算法搜索所得的全局路径进行优化，以获得全局优化路径； 所述的路径优化算法运用反向遍历的处理方法，从目标节点到初始节点的方向，采用 学习阶段步骤(4)中所述的局部路径规划器对目标节点与初始节点之间的路径进行判断， 若该路径为安全路径，则直接采用该路径为最终路径；否则以目标节点的前一节点作为当 前节点，用局部路径规划器对当前节点与初始节点之间的路径进行判断;若该路径安全，则 采用该路径；否则再用局部路径规划器判断当前节点的前一节点与初始节点之间的路径； 以此按照反向遍历的顺序重复上述步骤，直至遍历完所有节点为止;所述的初始节点对应 查询阶段中的步骤⑴所述的起始点qstart，即在A*算法里将起始点qstart表述为初始节点； 所述的目标节点对应查询阶段中步骤⑴所述的终止点q g〇ai，即在A*算法里将终止点qg-表 述为目标节点。2. 根据权利要求1所述的一种基于概率地图的工业机器人路径搜索优化算法，其特征 在于，所述的学习阶段的步骤（1)中机器人与周围环境障碍物的虚拟模型在OpenGL仿真环 境内，以实际尺寸与虚拟尺寸按照1:1的比例建立，或根据用户需要的比例建立。3. 根据权利要求1所述的一种基于概率地图的工业机器人路径搜索优化算法，其特征 在于，所述的学习阶段的步骤(2)中所述的碰撞检测算法为0BB包围盒的碰撞检测算法，或 为RAPID碰撞检测算法，或为混合包围体层次树碰撞检测算法等。4. 根据权利要求1所述的一种基于概率地图的工业机器人路径搜索优化算法，其特征 在于，所述的学习阶段的步骤(4)中所述的局部路径规划器为"二分法"形式的局部路径规 划器;所述的"二分法"形式的局部路径规划器是将两点之间的局部路径离散成各个离散 点；每一次"二分法"形式的局部路径规划器的操作，是在每两个离散点间等距的插入一个 新的离散点，判断插入后的新的离散点是否发生碰撞;若不发生碰撞，则再在每两个离散点 间等距的插入一个新的离散点，并对其进行碰撞检测，以此往复操作，直至两个离散点之间 的距离小于设定的局部路径检测阈值ε，局部路径规划器的操作终止，认为两点之间的局部 路径为局部安全路径，两点之间的局部路径不发生碰撞；否则认为两点之间的局部路径碰 撞，不为局部安全路径。5. 根据权利要求1所述的一种基于概率地图的工业机器人路径搜索优化算法，其特征 在于，所述的查询阶段的步骤(7)中所述的路径优化算法能改善Α*算法搜索得到的路径的 不够优化之处;所述的路径优化算法反向遍历目标节点与初始节点之间的路径是否可行； 若可行，则采用该路径，直接连接目标节点与初始节点之间的路径;否则以目标节点的前一 节点为当前节点，接着判断当前节点与初始节点之间的路径是否可行，重复上述步骤，直至 遍历完所有节点为止;最终使得所得路径曲折更少，路程更短。6. 根据权利要求3所述的碰撞检测算法，其特征在于，所述的碰撞检测算法是利用混合 包围体层次树碰撞检测算法对各采样位姿点、节点或离散点之间进行碰撞检测;所述的混 合包围体层次树算法采用由顶层、中间层和底层3层结构构成的包围体层次树技术;如果所 述的混合包围体层次树中父结点包围体不存在碰撞，则无须对子结点包围体进行碰撞检 测，以此加快碰撞检测速度;所述的混合包围体层次树算法计算简化和耗时较少，随着机器 人实际运动时各连杆间相对位置的变化而动态更新，适用于对机器人的碰撞检测。
【文档编号】G05D1/02GK105867381SQ201610257825
【公开日】2016年8月17日
【申请日】2016年4月25日
【发明人】陈琳, 韦志琪, 潘海鸿
【申请人】广西大学