一种适用于机器人路径搜索的改进的A*算法的制作方法与工艺

文档序号:13108881
技术领域该发明涉及机器人路径搜索领域,特别是机器人路径搜索技术中一种适用于机器人路径搜索的改进的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表用于存放最终的路径节点ni;其中,i=1,2,3...;所述的OPEN表用于存放待检测的候选节点mj;其中,j=1,2,3...;所述的OPEN表的作用还在于将当前节点ni的邻域Nn内的所有候选节点mj按估价值从小到大排列;其中,ni表示最终的路径节点,i是最终路径依次按照初始节点s到目标节点g的顺序排列的编号;mj表示路径节点ni的邻域Nn内的候选节点,j为路径节点ni的邻域Nn内所有候选节点的编号;每遍历一个路径节点ni,i的数值加1;每遍历一个路径节点ni邻域Nn内的候选节点mj,j的数值加1;所述的估价值是A*算法搜索路径过程中评价路径长短与否的重要尺标,估价值越小,则该段路径越短;所述的路径节点ni的邻域Nn是以路径节点ni为球心,以一固定邻域阈值ρ为半径设定的球型空间,该球型空间中包含候选节点mj;即在此球型空间内所有候选节点mj与路径节点ni间的距离均小于等于邻域阈值ρ;(2)将初始节点s直接添加进入CLOSED表的末尾处;(3)用局部路径规划器判断机器人沿初始节点s与目标节点g之间的局部路径运动是否发生碰撞;若碰撞,则顺序执行步骤(4);否则将目标节点g移入CLOSED表末尾,执行步骤(18);所述的局部路径规划器是将两个节点之间的局部路径离散成若干离散点,并对各离散点进行判断,判断机器人末端位于该局部路径间的各离散点时是否与周围环境物体存在碰撞;(4)以初始节点s为当前节点nl,建立当前节点nl的邻域Nn,分别计算该节点邻域Nn内的各候选节点mj的估价值,将OPEN表中最小估价值f(mj)的候选节点mj转移到OPEN表的表头;所述的估价值函数公式表示为:f(x)=g(x)+h(x);f(x)是从初始节点s经由节点x到目标节点g的估价函数,g(x)是从初始节点s到节点x的实际代价,h(x)是从节点x到目标节点g的最佳路径的估计代价;其中x为节点表达的通项形式,计算路径节点时x为ni,即路径节点ni的估价值函数为f(ni);计算邻域Nn内的候选节点时x为mj,即邻域Nn内的候选节点mj的估价值函数为f(mj);(5)将OPEN表表头的候选节点mj移入CLOSED表的末尾,并以移入CLOSED表的末尾的候选节点mj为路径搜索的当前节点n2;(6)对转移到CLOSED表中的当前节点n2进行判断,确定其是否为目标节点g;若节点n2为目标节点g,则执行步骤(18);否则顺序执行步骤(7);为表达方便,遍历搜索过程中所有当前节点的表述均采用通项ni的形式表示,即当前节点为ni;每遍历一个节点,i的数值加1;(7)用局部路径规划器将当前节点ni与目标节点g之间的局部路径离散;(8)判断机器人沿着当前节点ni到目标节点g之间的局部路径运动时是否会发生碰撞;若机器人沿着当前节点ni到目标节点g之间的局部路径运动时发生碰撞,则顺序执行步骤(9);否则将目标节点g移入CLOSED表的末尾,执行步骤(18);所述的判断机器人沿路径运动时是否发生碰撞,本质是判断机器人末端位于该路径中的任一节点时,是否与周围环境物体发生碰撞。(9)建立当前节点ni的邻域Nn;(10)遍历当前节点ni的邻域Nn中的候选节点mj;(11)计算当前遍历的候选节点mj的估价值,并将当前遍历的候选节点mj添加进OPEN表中;(12)将添加进入OPEN表中当前遍历的候选节点mj的估价值f(mj)与之前遍历的候选节点mk的估价值f(mk)进行比较;其中,k=1,2,3...;若当前遍历的候选节点mj的估价值f(mi)小于之前遍历的候选节点mk的估价值f(mk),则顺序执行步骤(13);否则跳转到步骤(14)执行;邻域Nn内的候选节点mj添加进入OPEN表中的过程有先后之分,所述的当前遍历的候选节点mj的估价值f(mj)必须与之前遍历的候选节点mk的估价值f(mk)进行比较;所述的之前遍历的候选节点mk与当前遍历的候选节点mj中的编号j≠k,即j和k表示不同的候选节点;(13)更新OPEN表,将当前遍历的候选节点mj保留在OPEN表表头;(14)判断当前节点ni的邻域Nn中的遍历是否完成;若没有,返回执行步骤(10),直至当前节点ni的邻域Nn中所有节点遍历完成;否则顺序执行步骤(15);(15)判断OPEN表是否为空;若为空,则路径搜索失败,结束本次路径搜索;否则顺序执行步骤(16);OPEN表为空,表示遍历的当前节点ni的邻域Nn内不包含其它候选节点mj,路径搜索失败,本次路径搜索结束;(16)邻域Nn遍历结束后,将OPEN表中的候选节点mj按照估价值f(mj)从小到大排列;将OPEN表中位于表头的候选节点mj转移至CLOSED表末尾,并以该候选节点mj为下一搜索过程的当前节点ni;以候选节点mj为下一搜索过程的当前节点ni,开始下一轮搜索,即i的数值加1;开始下一轮搜索,OPEN表中的信息将被覆盖,以用于存放下一路径节点ni的邻域Nn中其它候选节点mj的信息;(17)判断当前路径节点ni是否为目标节点g;若当前节点ni不是目标节点g,返回执行步骤(7);若当前节点ni为目标节点g,则顺序执行步骤(18);(18)顺序连接CLOSED表中的所有路径节点ni,获得机器人路径。其中,所述的一种适用于机器人路径搜索的改进的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(n)=||p(g)-p(n)||=(xg-xn)2+(yg-yn)2+(zg-zn)2---(3)]]>式中p(g),p(n)为节点g,n的末端位置信息;xg,yg,zg为目标节点g的坐标;xn,yn,zn为当前节点n的坐标。其中,所述的一种适用于机器人路径搜索的改进的A*算法具体流程的步骤(3)中采用“二分法”形式的局部路径规划器。所述的“二分法”形式的局部路径规划器采用如公式4所示的形式对局部路径进行离散化。引入约束条件为局部路径检测阈值,其定义如公式5所示。初始时离散程度k为1,以l为插补步长对局部路径进行离散,再对所有离散点处的机器人进行碰撞检测,若均不发生碰撞,则离散程度k加1,即k为2,重新计算插补步长l后再对局部路径进行离散,此时只需对新生成的离散点处的机器人进行碰撞检测,若新生成的离散点处均不发生碰撞,则离散程度k继续加1,并重复上述步骤,直至发生碰撞或当插补步长l小于局部路径检测阈值ε时还未发生过碰撞才结束操作。在采用局部路径规划器对该局部路径进行处理的过程中机器人会发生碰撞,则认为该路径是不可行的,反之则认为该路径是可行的。l=L2k---(4)]]>l<ε(5)式中,l为离散线段长度;L为局部路径长度;k为离散程度;ε为局部路径检测阈值。输入需要判断的局部路径的起点和终点转角信息后,所述的“二分法”形式的局部路径规划器的具体操作处理过程分为以下几步:(1)求取局部路径的起点位姿与终点位姿及其长度;因为起点和终点是以机器人关节转角值的形式表示,要通过机器人正运动学求出对应的欧氏空间中的位置信息,以便于计算出两点之间的距离值。(2)判断局部路径长度L是否小于局部路径检测阈值ε;当局部路径长度L小于局部路径检测阈值ε时,则认为该局部路径是不会发生碰撞的,即判断其为可行路径,并结束局部路径规划器的操作处理,否则需要继续步骤(3)的操作。其中局部路径检测阈值ε需要通过反复实验来选取合适的值或凭借相关经验进行确定。(3)初始化离散程度k,即让k=1,并通过公式4计算出位置插补步长l;(4)对局部路径进行插补,将其离散化;确定插补步数n(n=2k+1),并以此计算出姿态插补步长l′。(5)对新生成的离散插补点进行碰撞检测;为避免做无用功,无需对之前采用二分法得到的离散点重复进行碰撞检测,只需对新生成的离散点进行碰撞检测。若存在发生碰撞的离散点,则判断该局部路径为不可行路径,并结束局部路径规划器的操作处理,否则需要继续步骤(6)的操作。(6)将离散程度k加1(k=k+1),并重新计算位置插补步长l;(7)判断位置插补步长l是否小于局部路径检测阈值ε;当位置插补步长l小于局部路径检测阈值ε时,则认为该局部路径是不会发生碰撞的,即判断其为可行路径,并结束局部路径规划器的操作处理,否则返回步骤(4)。其中,所述的一种适用于机器人路径搜索的改进的A*算法具体流程的步骤(8)中所述的机器人沿当前节点ni到目标节点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为当前节点nl,建立当前节点nl的邻域Nn,分别计算该节点邻域Nn内的各候选节点mj的估价值,将OPEN表中最小估价值f(mj)的候选节点mj转移到OPEN表的表头;(5)将OPEN表表头的候选节点mj移入CLOSED表的末尾,并以移入CLOSED表的末尾的候选节点mj为路径搜索的当前节点n2;(6)对转移到CLOSED表中的当前节点n2进行判断,确定其是否为目标节点g;若节点n2为目标节点g,则执行步骤(18);否则顺序执行步骤(7);(7)用局部路径规划器将当前节点ni与目标节点g之间的局部路径离散;(8)判断机器人沿着当前节点ni到目标节点g之间的局部路径运动时是否会发生碰撞;若机器人沿着当前节点ni到目标节点g之间的局部路径运动时发生碰撞,则顺序执行步骤(9);否则将目标节点g移入CLOSED表的末尾,执行步骤(18);(9)建立当前节点ni的邻域Nn;(10)遍历当前节点ni的邻域Nn中的候选节点mj;(11)计算当前遍历的候选节点mj的估价值,并将当前遍历的候选节点mj添加进OPEN表中;(12)将添加进入OPEN表中当前遍历的候选节点mj的估价值f(mj)与之前遍历的候选节点mk的估价值f(mk)进行比较;其中,k=1,2,3...;若当前遍历的候选节点mj的估价值f(mi)小于之前遍历的候选节点mk的估价值f(mk),则顺序执行步骤(13);否则跳转到步骤(14)执行;(13)更新OPEN表,将当前遍历的候选节点mj保留在OPEN表表头;(14)判断当前节点ni的邻域Nn中的遍历是否完成;若没有,返回执行步骤(10),直至当前节点ni的邻域Nn中所有节点遍历完成;否则顺序执行步骤(15);(15)判断OPEN表是否为空;若为空,则路径搜索失败,结束本次路径搜索;否则顺序执行步骤(16);(16)邻域Nn遍历结束后,将OPEN表中的候选节点mj按照估价值f(mj)从小到大排列;将OPEN表中位于表头的候选节点mj转移至CLOSED表末尾,并以该候选节点mj为下一搜索过程的当前节点ni;(17)判断当前路径节点ni是否为目标节点g;若当前节点ni不是目标节点g,返回执行步骤(7);若当前节点ni为目标节点g,则顺序执行步骤(18);(18)顺序连接CLOSED表中的所有路径节点ni,获得机器人路径。其中,所述的具体步骤的步骤(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中S7表示基座,S1到S6分别代表连杆1-6,Si-j表示由刚体i-j构成的包围体(i∈A={1,2,3,4,5,6...
当前第1页1 2 3 
网友询问留言 已有0条留言
  • 还没有人留言评论。精彩留言会获得点赞!
1