CN105955254A - 一种适用于机器人路径搜索的改进的a＊算法 - Google Patents

Abstract

本发明公开一种适用于机器人路径搜索的改进的A*算法。输入初始节点s与目标节点g，以初始节点s为第一次搜索的当前节点n_i；所述算法是用局部路径规划器将当前节点n_i与目标节点g之间的局部路径离散并判断其是否碰撞；若该局部路径无碰撞，则直接将当前节点n_i和目标节点g导入CLOSED表中作为最终路径节点；否则按照传统的A*算法执行；轮到将下一节点视为当前节点n_i时，再按照所述算法的步骤执行，以此往复；直至搜索到的当前节点n_i为目标节点g；将目标节点g导入CLOSED表中，顺序连接CLOSED表中的路径节点，获得机器人路径；所述算法克服传统A*算法因邻域的限制而导致搜索易失败和局部路径过于曲折的问题，有效应用于工业机器人末端运动路径的搜索。

Description

一种适用于机器人路径搜索的改进的A＊算法

技术领域

该发明涉及机器人路径搜索领域，特别是机器人路径搜索技术中一种适用于机器人路径搜索的改进的A*算法。

背景技术

工业机器人在自动化领域十分常见，而自动路径搜索是机器人研究领域中用于提高机器人自治性的一个基本问题，这对于确保工业机器人安全、高效和有序的现场操作极其重要。在工业机器人路径规划控制中，机器人路径搜索是十分重要的一个环节。

现有的路径搜索算法可分为盲目式搜索算法和启发式搜索算法。其中盲目式搜索算法有广度优先算法、深度优先算法和Dijkstra算法等，这些算法不关心路径搜索问题本身的特点，在搜索过程中均按照事先设定的策略进行搜索，不会根据所得的信息对搜索过程加以优化。而A*搜索算法作为启发式搜索算法，更为关注路径搜索问题的本身，即从初始节点开始，每次搜索都寻找相对当前节点估价值最小的节点作为下次搜索的当前节点，直至搜索到目标节点为止。

传统的A*算法通过建立OPEN表与CLOSED表来分别存放待检测的候选节点与最终路径的节点，然后从初始节点开始建立当前节点的邻域，将邻域内节点放入OPEN表中，按估价值的大小从小到大依次排列，接着将OPEN表中估价值最小的节点转移至CLOSED表中，再以此节点为当前节点重复上述过程，即每次搜索都寻找相对当前节点估价值最小的节点作为下次搜索的当前节点，直至搜索到目标节点为止。但是采用传统的A*算法将各局部路径连接起来，会导致最终路径是由曲折的多段局部路径连接而成，同时由于A*算法邻域大小的限制，采用传统的A*算法搜索路径，会使得路径搜索的成功率很低，这限制A*算法搜索到较为优化的路径。

发明内容

本发明针对A*算法因邻域的限制而导致搜索易失败和局部路径过多的问题，提出一种适用于机器人路径搜索的改进的A*算法。该算法在按照传统的A*算法流程进行每次搜索前，先对当前节点与目标节点之间的局部路径进行判断，如果该局部路径是无碰撞的安全路径，那么直接采用该局部路径，否则再按照传统的A*算法流程进行搜索。

为实现上述目标，本发明所采用的技术方案如下：

针对传统的A*算法固有的一些缺陷，提出的一种适用于机器人路径搜索的改进的A*算法在对当前节点的邻域进行下一轮搜索前，先对当前节点与目标节点之间的局部路径进行规划，如果它们间的局部路径是无碰撞的安全路径，那么将直接采用该局部路径，并且停止后续的搜索操作；否则再按照传统的A*算法的搜索方法进行下一步操作。一种适用于机器人路径搜索的改进的A*算法，其搜索流程分为以下几步：

(1)建立OPEN表与CLOSED表，且两个都为空表，表中没有存放任何节点信息；

所述的CLOSED表用于存放最终的路径节点n_i；其中，i＝1，2，3...；所述的OPEN表用于存放待检测的候选节点m_j；其中，j＝1，2，3...；所述的OPEN表的作用还在于将当前节点n_i的邻域N_n内的所有候选节点m_j按估价值从小到大排列；其中，n_i表示最终的路径节点，i是最终路径依次按照初始节点s到目标节点g的顺序排列的编号；m_j表示路径节点n_i的邻域N_n内的候选节点，j为路径节点n_i的邻域N_n内所有候选节点的编号；每遍历一个路径节点n_i，i的数值加1；每遍历一个路径节点n_i邻域N_n内的候选节点m_j，j的数值加1；所述的估价值是A*算法搜索路径过程中评价路径长短与否的重要尺标，估价值越小，则该段路径越短；所述的路径节点n_i的邻域N_n是以路径节点n_i为球心，以一固定邻域阈值ρ为半径设定的球型空间，该球型空间中包含候选节点m_j；即在此球型空间内所有候选节点m_j与路径节点n_i间的距离均小于等于邻域阈值ρ；

(2)将初始节点s直接添加进入CLOSED表的末尾处；

(3)用局部路径规划器判断机器人沿初始节点s与目标节点g之间的局部路径运动是否发生碰撞；若碰撞，则顺序执行步骤(4)；否则将目标节点g移入CLOSED表末尾，执行步骤(18)；

所述的局部路径规划器是将两个节点之间的局部路径离散成若干离散点，并对各离散点进行判断，判断机器人末端位于该局部路径间的各离散点时是否与周围环境物体存在碰撞；

(4)以初始节点s为当前节点n_l，建立当前节点n_l的邻域N_n，分别计算该节点邻域N_n内的各候选节点m_j的估价值，将OPEN表中最小估价值f(m_j)的候选节点m_j转移到OPEN表的表头；

所述的估价值函数公式表示为：f(x)＝g(x)+h(x)；f(x)是从初始节点s经由节点x到目标节点g的估价函数，g(x)是从初始节点s到节点x的实际代价，h(x)是从节点x到目标节点g的最佳路径的估计代价；其中x为节点表达的通项形式，计算路径节点时x为n_i，即路径节点n_i的估价值函数为f(n_i)；计算邻域N_n内的候选节点时x为m_j，即邻域N_n内的候选节点m_j的估价值函数为f(m_j)；

(5)将OPEN表表头的候选节点m_j移入CLOSED表的末尾，并以移入CLOSED表的末尾的候选节点m_j为路径搜索的当前节点n₂；

(6)对转移到CLOSED表中的当前节点n₂进行判断，确定其是否为目标节点g；若节点n₂为目标节点g，则执行步骤(18)；否则顺序执行步骤(7)；

为表达方便，遍历搜索过程中所有当前节点的表述均采用通项n_i的形式表示，即当前节点为n_i；每遍历一个节点，i的数值加1；

(7)用局部路径规划器将当前节点n_i与目标节点g之间的局部路径离散；

(8)判断机器人沿着当前节点n_i到目标节点g之间的局部路径运动时是否会发生碰撞；若机器人沿着当前节点n_i到目标节点g之间的局部路径运动时发生碰撞，则顺序执行步骤(9)；否则将目标节点g移入CLOSED表的末尾，执行步骤(18)；

所述的判断机器人沿路径运动时是否发生碰撞，本质是判断机器人末端位于该路径中的任一节点时，是否与周围环境物体发生碰撞。

(9)建立当前节点n_i的邻域N_n；

(10)遍历当前节点n_i的邻域N_n中的候选节点m_j；

(11)计算当前遍历的候选节点m_j的估价值，并将当前遍历的候选节点m_j添加进OPEN表中；

(12)将添加进入OPEN表中当前遍历的候选节点m_j的估价值f(m_j)与之前遍历的候选节点m_k的估价值f(m_k)进行比较；其中，k＝1，2，3...；若当前遍历的候选节点m_j的估价值f(m_i)小于之前遍历的候选节点m_k的估价值f(m_k)，则顺序执行步骤(13)；否则跳转到步骤(14)执行；

邻域N_n内的候选节点m_j添加进入OPEN表中的过程有先后之分，所述的当前遍历的候选节点m_j的估价值f(m_j)必须与之前遍历的候选节点m_k的估价值f(m_k)进行比较；所述的之前遍历的候选节点m_k与当前遍历的候选节点m_j中的编号j≠k，即j和k表示不同的候选节点；

(13)更新OPEN表，将当前遍历的候选节点m_j保留在OPEN表表头；

(14)判断当前节点n_i的邻域N_n中的遍历是否完成；若没有，返回执行步骤(10)，直至当前节点n_i的邻域N_n中所有节点遍历完成；否则顺序执行步骤(15)；

(15)判断OPEN表是否为空；若为空，则路径搜索失败，结束本次路径搜索；否则顺序执行步骤(16)；

OPEN表为空，表示遍历的当前节点n_i的邻域N_n内不包含其它候选节点m_j，路径搜索失败，本次路径搜索结束；

(16)邻域N_n遍历结束后，将OPEN表中的候选节点m_j按照估价值f(m_j)从小到大排列；将OPEN表中位于表头的候选节点m_j转移至CLOSED表末尾，并以该候选节点m_j为下一搜索过程的当前节点n_i；

以候选节点m_j为下一搜索过程的当前节点n_i，开始下一轮搜索，即i的数值加1；开始下一轮搜索，OPEN表中的信息将被覆盖，以用于存放下一路径节点n_i的邻域N_n中其它候选节点m_j的信息；

(17)判断当前路径节点n_i是否为目标节点g；若当前节点n_i不是目标节点g，返回执行步骤(7)；若当前节点n_i为目标节点g，则顺序执行步骤(18)；

(18)顺序连接CLOSED表中的所有路径节点n_i，获得机器人路径。

其中，所述的一种适用于机器人路径搜索的改进的A*算法的估价函数f(n)用如公式1所示的形式进行计算，它代表一条从初始节点s出发，经过当前节点n，然后到达目标节点g的路径所需要花费的代价，它包括两个部分：从初始节点s到当前节点n所花费的实际代价g(n)和从当前节点n到目标节点g的最优路径估计代价h(n)。在判断当前节点n时，g(n)是已经确定的实际代价，是一个已知量，而h(n)是未确定的估计代价，是一个未知量，所以h(n)对整个估价函数f(n)的优劣起着决定性的作用。

采用如公式2所示的形式进行计算g(n)，g(n)为当前节点n所在的搜索深度，即从初始节点s到当前节点n实际搜索过多少次(经历过多少个节点)；采用如公式3所示的形式进行计算h(n)，h(n)为从当前节点n到目标节点g的欧氏距离。

f(n)＝g(n)+h(n) (1)

式中，f(n)为当前节点n的估价函数；g(n)为从初始节点s到当前节点n所花费的实际代价；h(n)为从当前节点n到目标节点g的最优路径估计代价。

g(n)＝d (2)

式中，d为搜索深度。

$h\left(n\right)=\left|\right|p\left(g\right)-p\left(n\right)\left|\right|=\sqrt{{(x_g-x_n)}^2+{(y_g-y_n)}^2+{(z_g-z_n)}^2}---\left(3\right)$

式中p(g)，p(n)为节点g，n的末端位置信息；x_g，y_g，z_g为目标节点g的坐标；x_n，y_n，z_n为当前节点n的坐标。

其中，所述的一种适用于机器人路径搜索的改进的A*算法具体流程的步骤(3)中采用“二分法”形式的局部路径规划器。所述的“二分法”形式的局部路径规划器采用如公式4所示的形式对局部路径进行离散化。引入约束条件为局部路径检测阈值，其定义如公式5所示。初始时离散程度k为1，以l为插补步长对局部路径进行离散，再对所有离散点处的机器人进行碰撞检测，若均不发生碰撞，则离散程度k加1，即k为2，重新计算插补步长l后再对局部路径进行离散，此时只需对新生成的离散点处的机器人进行碰撞检测，若新生成的离散点处均不发生碰撞，则离散程度k继续加1，并重复上述步骤，直至发生碰撞或当插补步长l小于局部路径检测阈值ε时还未发生过碰撞才结束操作。在采用局部路径规划器对该局部路径进行处理的过程中机器人会发生碰撞，则认为该路径是不可行的，反之则认为该路径是可行的。

$l=\frac{L}{2^k}---\left(4\right)$

l＜ε (5)

式中，l为离散线段长度；L为局部路径长度；k为离散程度；ε为局部路径检测阈值。

输入需要判断的局部路径的起点和终点转角信息后，所述的“二分法”形式的局部路径规划器的具体操作处理过程分为以下几步：

(1)求取局部路径的起点位姿与终点位姿及其长度；

因为起点和终点是以机器人关节转角值的形式表示，要通过机器人正运动学求出对应的欧氏空间中的位置信息，以便于计算出两点之间的距离值。

(2)判断局部路径长度L是否小于局部路径检测阈值ε；

当局部路径长度L小于局部路径检测阈值ε时，则认为该局部路径是不会发生碰撞的，即判断其为可行路径，并结束局部路径规划器的操作处理，否则需要继续步骤(3)的操作。其中局部路径检测阈值ε需要通过反复实验来选取合适的值或凭借相关经验进行确定。

(3)初始化离散程度k，即让k＝1，并通过公式4计算出位置插补步长l；

(4)对局部路径进行插补，将其离散化；

确定插补步数n(n＝2^k+1)，并以此计算出姿态插补步长l′。

(5)对新生成的离散插补点进行碰撞检测；

为避免做无用功，无需对之前采用二分法得到的离散点重复进行碰撞检测，只需对新生成的离散点进行碰撞检测。若存在发生碰撞的离散点，则判断该局部路径为不可行路径，并结束局部路径规划器的操作处理，否则需要继续步骤(6)的操作。

(6)将离散程度k加1(k＝k+1)，并重新计算位置插补步长l；

(7)判断位置插补步长l是否小于局部路径检测阈值ε；

当位置插补步长l小于局部路径检测阈值ε时，则认为该局部路径是不会发生碰撞的，即判断其为可行路径，并结束局部路径规划器的操作处理，否则返回步骤(4)。

其中，所述的一种适用于机器人路径搜索的改进的A*算法具体流程的步骤(8)中所述的机器人沿当前节点n_i到目标节点g之间的局部路径是否会发生碰撞的判断方法，是采用碰撞检测算法对局部路径碰撞与否进行判断的；所述的碰撞检测算法为OBB包围盒碰撞检测算法，或为RAPID碰撞检测算法，或为混合包围体层次树碰撞检测算法等。所述的混合包围体层次树算法采用由顶层、中间层和底层3层结构构成的包围体层次树技术；如果所述的混合包围体层次树中父结点包围体不存在碰撞，则无须对子结点包围体进行碰撞检测，以此加快碰撞检测速度；所述的混合包围体层次树随着机器人实际运动时各连杆间相对位置的变化而动态更新，适用于机器人的路径搜索中判断节点或离散点是否发生碰撞。对节点或离散点的碰撞检测，实质上就是对当机器人末端位置位于该节点或离散点时，机器人是否与周围环境中存在的障碍物发生碰撞的判断。

所述的混合包围体层次树碰撞检测算法操作时，先构建机器人和周围环境障碍物的混合包围体层次树顶层、中间层和底层包围体结构，再依次按照顶层，中间层，底层的顺序进行机器人的碰撞检测；如果顶层中没有发生顶-底层球包围体相交，则不进行中间层和底层的碰撞检测；如果中间层中没有中-底层OBB包围体相交，则不进行底层碰撞检测。机器人的碰撞检测的执行过程按照以下3步：

(1)进行顶层层次树包围体相交测试；若包围体与包围体之间相交，得到顶层中相交的顶-底层球包围体，从而获得需要执行相交测试的中间层OBB包围体层次树的中-顶层OBB包围体，执行步骤(2)；否则输出结果为不发生碰撞；

(2)进行中间层OBB包围体相交测试；若包围体与包围体之间相交，获取OBB包围体层次树中相交的中-底层OBB包围体，执行步骤(3)；否则输出结果为不发生碰撞；

(3)进行底层中三角面片的相交测试得到相交三角面片对；若三角面片之间相交，则输出结果为碰撞；否则输出结果为不发生碰撞。

本发明的特点和有益效果在于：

(1)采用改进的A*算法搜索路径，首先的步骤不再是从当前节点连接到下一节点，而是先判断当前节点与目标节点之间的路径是否安全、无碰撞，若是安全无碰撞的，则直接连接，形成最终路径；若不是安全无碰撞的，则按传统的A*算法执行。

(2)采用改进的A*算法搜索路径时，由于其搜索原理是首先将当前节点与目标节点相连接，判断其局部路径是否安全无碰撞，如果该局部路径是安全无碰撞的，则直接采用为最终路径。故能有效避免传统的A*算法邻域大小对路径搜索成功率的影响。

(3)在操作时只需输入机器人运动的起始点与终止点的位姿信息，即可搜索出一条无碰撞的优化路径。

附图说明

图1一种适用于机器人路径搜索的改进的A*算法的操作流程

图2一种适用于机器人路径搜索的改进的A*算法的具体操作流程

图3“二分法”形式的局部路径规划器操作处理流程

图4顶层多层层次树结构图

图5中间层多层层次树结构图——偶数个三角面片

图6中间层多层层次树结构图——奇数个三角面片

图7混合包围体层次树的执行过程

具体实施方式

在下文中将结合附图对本发明的示范性实施例进行描述。

所述的一种适用于机器人路径搜索的改进的A*算法的搜索流程如图1所示。因以6自由度关节型机器人为示范对象，设定机器人路径运行起始点的关节转角值(-41.6，-47.7，-0.2，0，47.9，-41.6)与机器人路径运行终止点的关节转角值(54.9，-76.4，43.1，0，33.2，54.9)，设定节点的邻域阈值ρ为200mm，局部路径检测阈值ε为0.5mm。所述的起始点与终止点分别对应A*算法搜索路径的初始节点s与目标节点g。一种适用于机器人路径搜索的改进的A*算法，其具体操作流程如图2所示，其中虚线框中的内容为相对于传统的A*路径搜索算法的改进部分。所述的一种适用于机器人路径搜索的改进的A*算法，其详细路径搜索过程分为以下几步：

(1)建立OPEN表与CLOSED表，且两个都为空表，表中没有存放任何节点信息；

(2)将初始节点s直接添加进入CLOSED表的末尾处；

(3)用局部路径规划器判断机器人沿初始节点s与目标节点g之间的局部路径运动是否发生碰撞；若碰撞，则顺序执行步骤(4)；否则将目标节点g移入CLOSED表末尾，执行步骤(18)；

(4)以初始节点s为当前节点n_l，建立当前节点n_l的邻域N_n，分别计算该节点邻域N_n内的各候选节点m_j的估价值，将OPEN表中最小估价值f(m_j)的候选节点m_j转移到OPEN表的表头；

(5)将OPEN表表头的候选节点m_j移入CLOSED表的末尾，并以移入CLOSED表的末尾的候选节点m_j为路径搜索的当前节点n₂；

(6)对转移到CLOSED表中的当前节点n₂进行判断，确定其是否为目标节点g；若节点n₂为目标节点g，则执行步骤(18)；否则顺序执行步骤(7)；

(7)用局部路径规划器将当前节点n_i与目标节点g之间的局部路径离散；

(8)判断机器人沿着当前节点n_i到目标节点g之间的局部路径运动时是否会发生碰撞；若机器人沿着当前节点n_i到目标节点g之间的局部路径运动时发生碰撞，则顺序执行步骤(9)；否则将目标节点g移入CLOSED表的末尾，执行步骤(18)；

(9)建立当前节点n_i的邻域N_n；

(10)遍历当前节点n_i的邻域N_n中的候选节点m_j；

(11)计算当前遍历的候选节点m_j的估价值，并将当前遍历的候选节点m_j添加进OPEN表中；

(12)将添加进入OPEN表中当前遍历的候选节点m_j的估价值f(m_j)与之前遍历的候选节点m_k的估价值f(m_k)进行比较；其中，k＝1，2，3...；若当前遍历的候选节点m_j的估价值f(m_i)小于之前遍历的候选节点m_k的估价值f(m_k)，则顺序执行步骤(13)；否则跳转到步骤(14)执行；

(13)更新OPEN表，将当前遍历的候选节点m_j保留在OPEN表表头；

(14)判断当前节点n_i的邻域N_n中的遍历是否完成；若没有，返回执行步骤(10)，直至当前节点n_i的邻域N_n中所有节点遍历完成；否则顺序执行步骤(15)；

(15)判断OPEN表是否为空；若为空，则路径搜索失败，结束本次路径搜索；否则顺序执行步骤(16)；

(16)邻域N_n遍历结束后，将OPEN表中的候选节点m_j按照估价值f(m_j)从小到大排列；将OPEN表中位于表头的候选节点m_j转移至CLOSED表末尾，并以该候选节点m_j为下一搜索过程的当前节点n_i；

(17)判断当前路径节点n_i是否为目标节点g；若当前节点n_i不是目标节点g，返回执行步骤(7)；若当前节点n_i为目标节点g，则顺序执行步骤(18)；

(18)顺序连接CLOSED表中的所有路径节点n_i，获得机器人路径。

其中，所述的具体步骤的步骤(3)中所述的局部路径规划器为“二分法”形式的局部路径规划器；输入需要判断的局部路径的起点和终点转角信息后，所述的“二分法”形式的局部路径规划器的具体操作处理流程如图3所示，其详细操作处理过程分为以下几步：

(1)获取局部路径的起点位置与终点位置，并求出局部路径的长度；

(2)判断局部路径长度L是否小于局部路径检测阈值ε；

(3)初始化离散程度k，即让k＝1，并通过公式4计算出位置插补步长l；

(4)对局部路径进行插补，将其离散化；

(5)对新生成的离散插补点进行碰撞检测；

(6)将离散程度k加1(k＝k+1)，并重新计算位置插补步长l；

(7)判断位置插补步长l是否小于局部路径检测阈值ε；

其中，路径内各节点或离散点间碰撞检测的方法是利用混合包围体层次树碰撞检测算法进行碰撞检测的；所述的混合包围体层次树碰撞检测算法操作时，先构建机器人的混合包围体层次树顶层、中间层和底层包围体结构，再依次按照顶层，中间层，底层的顺序进行机器人的碰撞检测；如果顶层中没有发生顶-底层球包围体相交，则不进行中间层和底层的碰撞检测；如果中间层中没有中-底层OBB包围体相交，则不进行底层碰撞检测。其操作流程如下：

(1)构建机器人的混合包围体层次树顶层；

混合包围体层次树的顶层选用OBB包围机器人连杆模型，并作为球包围体的构建基元。采用多层层次树，顶层的构建采用自底向上方式和二叉树数据结构，如图4表示机器人刚体数是7个时顶层层次树的结构形式，它由球包围体作为顶层包围体。图4中S₇表示基座，S₁到S₆分别代表连杆1-6，S_i-j表示由刚体i-j构成的包围体(i∈A＝{1，2，3，4，5，6}，j∈B＝{b|i＜b≤7，i∈A})，S_1-7表示整个机器人。图4中每个结点S_i-j均被1个以OBB为基元的球包围体；构建环境中障碍物的混合包围体的方法也是一样。

(2)构建机器人的混合包围体层次树中间层；

机器人连杆模型是刚性的细长杆，采用OBB包围体来建立混合包围体层次树的中间层。OBB包围体层次树的数量对应于连杆个数，其采用自顶而下方法和二叉树的数据结构进行构建。当连杆坐标发生变化后，只需要更新中间层层次树中顶层OBB包围体的坐标，其大小形状不变，无须重新对中间层层次树的中间层与底层进行构建，因而实现快速更新中间层层次树；构建环境中障碍物的混合包围体的方法也是一样。

当构成连杆的三角面片数量m为偶数时，中间层层次树如图5所示，该图是以m＝10为例的结构形式；当构成连杆的三角面片数量m为奇数时，中间层层次树为如图6所示，该图是以m＝11为例的结构形式；图5、图6中T_i-j(i∈C＝{c|1≤c＜m}，j∈D＝{d|i＜d≤m，i∈C})表示由三角面片i-j构成的包围体，顶层根结点T_1-10和T_1-11∈{S₁，S₂，S₃，S₄，S₅，S₆，S₇}，图5、图6中每个结点T_i-j均被1个OBB包围。

(3)构建机器人的混合包围体层次树底层；

混合包围体层次树的底层是由中间层层次树的底层中发生碰撞的OBB包围体所包围的三角面片构成，其中中间层层次树中底层的三角面片坐标是在机器人的基坐标系下，而混合包围体层次树的底层的三角面片坐标是在世界坐标系下。而且其构建过程比较特殊，它是在完成中间层碰撞检测后，当且仅当中间层中有底层OBB包围体发生碰撞时才需要建立。建立时将中间层层次树底层中发生碰撞的OBB包围体其所包围的三角面片顶点坐标分别转换到世界坐标系下，转换后得到的三角面片构成混合包围体层次树的底层。

(4)混合包围体层次树的执行过程。如果顶层中没有发生顶-底层球包围体相交，则不进行中间层和底层的碰撞检测；如果中间层中没有中-底层OBB包围体相交，则不进行底层碰撞检测。

混合包围体层次树每个离散点的碰撞检测依次按照顶层，中间层，底层进行，如图7所示。机器人与周边环境进行碰撞检测的执行过程按照以下3步：

(1)进行顶层层次树包围体相交测试；若包围体间相交，得到顶层中相交的顶-底层球包围体，从而获得需要执行相交测试的中间层OBB包围体层次树的中-顶层OBB包围体，执行步骤(2)；否则输出结果为不发生碰撞；

(2)进行中间层OBB包围体相交测试；若包围体间相交，获取OBB包围体层次树中相交的中-底层OBB包围体，执行步骤(3)；否则输出结果为不发生碰撞；

(3)进行底层中三角面片的相交测试得到相交三角面片对；若三角面片间相交，则输出结果为碰撞；否则输出结果为不发生碰撞。

对于一条包含多个节点或离散点的路径，整个碰撞检测过程按照以下5个步骤进行：

(1)构建包围连杆的OBB包围体基元；

(2)构建整个机器人的混合包围体层次树；

(3)在一个离散点处执行混合包围体层次树碰撞检测；

(4)重新计算混合包围体层次树顶层；

(5)返回执行(3)和(4)，直到完成该段路径内所有离散点碰撞检测。

根据本示范例中设定的机器人运动路径的起始点与终止点，以及节点的邻域阈值ρ和局部路径检测阈值ε的具体信息，采用改进的A*路径搜索算法所得的最终路径长度为1679.01mm。而在相同设定条件下，采用未加以改进的A*路径搜索算法所得的最终路径长度为1765.43mm。其中，邻域阈值ρ和局部路径检测阈值ε通常需要操作人员通过实验来选取合适的值，或者操作人员凭借相关经验进行确定。

最后说明的是，本发明的一种适用于机器人路径搜索的改进的A*算法不局限于上述具体实施例。上述实施例仅是本发明较有代表性的例子，还能做出各种修改、变换和变形。因此，说明书和附图应该被认为是说明性的而非限制性的。凡是依据本发明的技术实质对以上实施例所作的任何简单修改、等同变化与修饰，均应认为属于本发明的保护范围。

Claims (5)

1.一种适用于机器人路径搜索的改进的A*算法，其特征在于，只需输入机器人运动路径的初始节点s和目标节点g，改进的A*算法能够自动搜索出一条更短且全局无碰撞的优化路径；改进的A*算法是一种搜索路径的最直接有效的方法，具体包含以下步骤：

(1)建立OPEN表与CLOSED表，且两个都为空表，表中没有存放任何节点信息；

所述的CLOSED表用于存放最终的路径节点n_i；其中，i＝1，2，3...；所述的OPEN表用于存放待检测的候选节点m_j；其中，j＝1，2，3...；所述的OPEN表的作用还在于将当前节点n_i的邻域N_n内的所有候选节点m_j按估价值从小到大排列；其中，n_i表示最终的路径节点，i是最终路径依次按照初始节点s到目标节点g的顺序排列的编号；m_j表示路径节点n_i的邻域N_n内的候选节点，j为路径节点n_i的邻域N_n内所有候选节点的编号；每遍历一个路径节点n_i，i的数值加1；每遍历一个路径节点n_i邻域N_n内的候选节点m_j，j的数值加1；所述的估价值是A*算法搜索路径过程中评价路径长短与否的重要尺标，估价值越小，则该段路径越短；所述的路径节点n_i的邻域N_n是以路径节点n_i为球心，以一固定邻域阈值ρ为半径设定的球型空间，该球型空间中包含候选节点m_j；即在此球型空间内所有候选节点m_j与路径节点n_i间的距离均小于等于邻域阈值ρ；

(2)将初始节点s直接添加进入CLOSED表的末尾处；

(3)用局部路径规划器判断机器人沿初始节点s与目标节点g之间的局部路径运动是否发生碰撞；若碰撞，则顺序执行步骤(4)；否则将目标节点g移入CLOSED表末尾，执行步骤(18)；

所述的局部路径规划器是将两个节点之间的局部路径离散成若干离散点，并对各离散点进行判断，判断机器人末端位于该局部路径间的各离散点时是否与周围环境物体存在碰撞；

(4)以初始节点s为当前节点n_l，建立当前节点n_l的邻域N_n，分别计算该节点邻域N_n内的各候选节点m_j的估价值，将OPEN表中最小估价值f(m_j)的候选节点m_j转移到OPEN表的表头；

所述的估价值函数公式表示为：f(x)＝g(x)+h(x)；f(x)是从初始节点s经由节点x到目标节点g的估价函数，g(x)是从初始节点s到节点x的实际代价，h(x)是从节点x到目标节点g的最佳路径的估计代价；其中x为节点表达的通项形式，计算路径节点时x为n_i，即路径节点n_i的估价值函数为f(n_i)；计算邻域N_n内的候选节点时x为m_j，即邻域N_n内的候选节点m_j的估价值函数为f(m_j)；

(5)将OPEN表表头的候选节点m_j移入CLOSED表的末尾，并以移入CLOSED表的末尾的候选节点m_j为路径搜索的当前节点n₂；

(6)对转移到CLOSED表中的当前节点n₂进行判断，确定其是否为目标节点g；若节点n₂为目标节点g，则执行步骤(18)；否则顺序执行步骤(7)；

为表达方便，遍历搜索过程中所有当前节点的表述均采用通项n_i的形式表示，即当前节点为n_i；每遍历一个节点，i的数值加1；

(7)用局部路径规划器将当前节点n_i与目标节点g之间的局部路径离散；

(8)判断机器人沿着当前节点n_i到目标节点g之间的局部路径运动时是否会发生碰撞；若机器人沿着当前节点n_i到目标节点g之间的局部路径运动时发生碰撞，则顺序执行步骤(9)；否则将目标节点g移入CLOSED表的末尾，执行步骤(18)；

所述的判断机器人沿路径运动时是否发生碰撞，本质是判断机器人末端位于该路径中的任一节点时，是否与周围环境物体发生碰撞；

(9)建立当前节点n_i的邻域N_n；

(10)遍历当前节点n_i的邻域N_n中的候选节点m_j；

(11)计算当前遍历的候选节点m_j的估价值，并将当前遍历的候选节点m_j添加进OPEN表中；

(12)将添加进入OPEN表中当前遍历的候选节点m_j的估价值f(m_j)与之前遍历的候选节点m_k的估价值f(m_k)进行比较；其中，k＝1，2，3...；若当前遍历的候选节点m_j的估价值f(m_i)小于之前遍历的候选节点m_k的估价值f(m_k)，则顺序执行步骤(13)；否则跳转到步骤(14)执行；

邻域N_n内的候选节点m_j添加进入OPEN表中的过程有先后之分，所述的当前遍历的候选节点m_j的估价值f(m_j)必须与之前遍历的候选节点m_k的估价值f(m_k)进行比较；所述的之前遍历的候选节点m_k与当前遍历的候选节点m_j中的编号j≠k，即j和k表示不同的候选节点；

(13)更新OPEN表，将当前遍历的候选节点m_j保留在OPEN表表头；

(14)判断当前节点n_i的邻域N_n中的遍历是否完成；若没有，返回执行步骤(10)，直至当前节点n_i的邻域N_n中所有节点遍历完成；否则顺序执行步骤(15)；

(15)判断OPEN表是否为空；若为空，则路径搜索失败，结束本次路径搜索；否则顺序执行步骤(16)；

OPEN表为空，表示遍历的当前节点n_i的邻域N_n内不包含其它候选节点m_j，路径搜索失败，本次路径搜索结束；

(16)邻域N_n遍历结束后，将OPEN表中的候选节点m_j按照估价值f(m_j)从小到大排列；将OPEN表中位于表头的候选节点m_j转移至CLOSED表末尾，并以该候选节点m_j为下一搜索过程的当前节点n_i；

以候选节点m_j为下一搜索过程的当前节点n_i，开始下一轮搜索，即i的数值加1；开始下一轮搜索，OPEN表中的信息将被覆盖，以用于存放下一路径节点n_i的邻域N_n中其它候选节点m_j的信息；

(17)判断当前路径节点n_i是否为目标节点g；若当前节点n_i不是目标节点g，返回执行步骤(7)；若当前节点n_i为目标节点g，则顺序执行步骤(18)；

(18)顺序连接CLOSED表中的所有路径节点n_i，获得机器人路径。

2.根据权利要求1所述的一种适用于机器人路径搜索的改进的A*算法，其特征在于，所述的改进的A*算法不同于传统的A*算法，所述的改进的A*算法是用局部路径规划器直接将当前节点n_i与目标节点g之间的局部路径离散并判断其是否碰撞；若该段局部路径有效，则直接将当前节点n_i和目标节点g导入CLOSED表中作为最终的路径节点；否则按照传统的A*算法执行；轮到将下一节点视为当前节点时，再按照改进的A*算法的步骤执行，以此往复，直至搜索到当前节点n_i与目标节点g之间的局部路径有效，将目标节点g导入CLOSED表中，改进的A*算法搜索路径完成。

3.根据权利要求1所述的一种适用于机器人路径搜索的改进的A*算法，其特征在于，所述的具体步骤中的步骤(3)中所述的局部路径规划器为“二分法”形式的局部路径规划器；所述的“二分法”形式的局部路径规划器是将两点之间的局部路径离散成各个离散点；每一次“二分法”形式的局部路径规划器的操作，是在每两个离散点间等距的插入一个新的离散点，判断插入后的新的离散点是否发生碰撞；若不发生碰撞，则再在每两个离散点间等距的插入一个新的离散点，并对其进行碰撞检测，以此往复操作，直至两个离散点之间的距离小于设定的局部路径检测阈值ε，局部路径规划器的操作终止，认为两点之间的局部路径为局部安全路径，两点之间的局部路径不发生碰撞；否则认为两点之间的局部路径碰撞，不为局部安全路径。

4.根据权利要求1所述的一种适用于机器人路径搜索的改进的A*算法，其特征在于，所述的具体步骤中的步骤(8)中所述的机器人沿当前节点n_i到目标节点g之间的局部路径是否会发生碰撞的判断方法，是采用碰撞检测算法对局部路径碰撞与否进行判断的；所述的碰撞检测算法为OBB包围盒的碰撞检测算法，或为RAPID碰撞检测算法，或为混合包围体层次树碰撞检测算法等碰撞检测算法。

5.根据权利要求4所述的碰撞检测算法，其特征在于，所述的碰撞检测算法是利用混合包围体层次树碰撞检测算法对各节点或离散点之间进行碰撞检测；所述的混合包围体层次树算法采用由顶层、中间层和底层3层结构构成的包围体层次树技术；如果所述的混合包围体层次树中父结点包围体不存在碰撞，则无须对子结点包围体进行碰撞检测，以此加快碰撞检测速度；所述的混合包围体层次树算法计算简化和耗时较少，随着机器人实际运动时各连杆间相对位置的变化而动态更新，适用于对机器人的碰撞检测。