专利名称：一种基于虚拟场景的机器人避障路径规划方法
技术领域：
本发明涉及机器人避障路径规划技术。本方法主要涉及基于虚拟机器人的机器人避障路径规划方法，也可以用于实体机器人的避障路径规划。
背景技术：
避障路径规划是指给定环境的障碍条件以及起始位姿和目标位姿，要求选择一条从起始点到目标点的路径，使运动物体(机器人)能安全、无碰撞地通过所有的障碍。这种自主地躲避障碍物并完成作业任务是机器人研究的一个重要内容。
目前，地面机械臂的障方法有基于C空间的自由空间法、基于直角坐标空间的人工势场法和关节空间法。
Lazona-Perze提出了基于C空间的自由空间法。以机械臂的关节轴为坐标系建立C空间(Configuration pace)，将障碍物映射到C空间，形成C空间障碍，那么，在C空间内C空间障碍的补集，则对应自由空间，自由空间内的点代表不与障碍物干涩的机器人构型，而C空间障碍内的点代表与障碍物干涩的机器人构型。应用启发搜索算法，避障规划就转化为在自由空间内寻找连接初始点和目标点的路径问题。缺点是C空间的建立比较困难，且计算量大。
Khatib在罚函数的基础上引入了人工势场(Artificial PotentialField)的概念，对障碍定义一个排斥场，此势场在机器人上一点的梯度定义为排斥力；对目标定义一个吸引势场，机器人的运动是由两种力的互相作用引起的，且受到运动学的约束。关节空间法是指将障碍物映射到关节空间，那么关节空间形成了一个可行域。优点是比较直观。缺点是根据关节空间规划出的路径点手爪的轨迹不规则。
与传统规划方法相比，空间机器人避碰路径规划算法还应从以下几方面考虑(1)安全性机器人整个手臂都有可能与障碍物体发生碰撞，因此对机器人的手臂进行规划时，应与障碍物体保持一定距离，并对已经规划好的路径，进行机器人与障碍物体的碰撞检验检查，以免发生碰撞。(2)规划效率为提高路径规划的效率，应限制规划算法搜索的空间，从而缩短机器人轨迹规划的时间。
目前对空间机器人的避障路径规划极少。已有的方法也都只能进行离线规划，达不到在线规划的实时性要求。
发明内容
为了解决上述传统问题，因此本发明的一个目的就是提出了一种基于虚拟机器人的机器人避障路径规划方法。
在本发明的一个方面中，基于虚拟场景的机器人避障路径规划方法，包括步骤(1)确定机械臂的初始状态 和目标状态 (2)确定时间步长h和 (3)计算障碍物在水平面内的障碍域HSSmin和HSSmax以及垂直平面内的障碍域；(4)在由 确定的垂直平面内顺序调节 (5)按靠近原则将 调节到180、0、-180度；(6)判断若 而QQ1∈(HSSmin，HSSmax)或 或 时，进入下一步，否则进入第十步；(7)计算 的准目标位置QQ1；(8)将 由当前位置调节到QQ1；(9)再由QQ1确定的垂直平面内调节 使得当 后L1，L2，L3将不会与障碍物相撞；(10)将 由当前位置调到QQ1；(11)计算由QQ1确定的垂直平面下基座的障碍域；(12)按 的顺序调节关节角到目标关节角QQ5，QQ3，QQ2。若成功，则进入下一步；(13)按 的顺序调节关节角到目标关节角QQ2，QQ3，QQ5。若成功，则进入下一步；(14)调节 至目标关节角QQ4；(15)调节 至目标关节角QQ6；(16)规划结束。
根据这个方面，在步骤(13)中，若不成功，则规划失败，进入第十四步。
根据这个方面，在步骤(14)中，若不成功，则规划失败，进入第十六步。
结合随后的附图，从下面的详细说明中可显而易见的得出本发明的上述及其他目的、特征及优点。在附图中图1给出了对障碍物用规则体的包络近似建模；图2给出了投影到两个平面示意图；图3给出了平面避障示意图；图4给出了平面避障示意图；图5给出了平面内的障碍物；图6给出了机械臂避障路径规划的两岔树；图7给出了V平面内zjojvj坐标系下障碍物Wi确定的障碍域；图8给出了由基座，六关节机械臂、以及方形障碍物组成的机器人系统的示意图；图9给出了机械臂避障流程图。
具体实施例方式
针对目前C空间法的C空间的建立困难，而且计算量大，人工势场法只是局部规划，而空间机器人避障路径规划研究极少等特点，为了达到规划的实时性和高效性的要求，我们提出了把三维问题转化为二维问题解决六自由度空间机械臂避障路径规划问题。
本方法利用规则体的包络对障碍物建模，并充分结合了关节空间法和C空间法的思想，在保持较高精度的前提下，通过在H和V平面上的投影确定障碍域，将三维避障路径规划问题转化成两个平面内的二维问题。该方法计算量小、简单、直观、易实现。
(1)利用规则体的包络对障碍物建模三维障碍物一般具有不规则的几何形状，因此它在C空间法等方法中的精确的障碍域难以获得。王伟[7]等人提出通过利用边界点和特征点来确定障碍物在C空间下的障碍域，但是对于不规则几何形状的障碍物的边界点过多，并且特征点不易确定，即使能确定，其计算量也相当大，这将直接影响规划速度，难以满足实时性要求。因此本发明提出了利用障碍物规则体的包络来近似建模，这种近似虽然扩大了障碍域，但是使障碍域的描述大大简化，有效地提高了规划的效率，并满足安全性的要求。图1(a)为在满足精度的条件下障碍物采用最小外接球的近似，图1(b)是对障碍物采用长方体近似。
(2)把三维问题转化为二维问题目前对障碍物的工作空间的建模主要利用C空间法，但是由于C空间法的建立非常困难，而且计算量随着机械臂自由度的增加成指数被增长，因此我们利用C空间法的思想，把三维的工作空间投影到两个垂直的平面(H平面、V平面)内解决。然后分别在H平面和V平面内进行路径搜索。图2为把机械臂和障碍物投影到H和V平面的示意图。图3和图4为在V平面内和H平面内进行搜索的示意图。
(3)障碍域内障碍物合并如图2所示平面内的障碍物可以描述为Wi(x，y，z，r)，其中i为障碍物序号，x，y，z为球心在基坐标系中的坐标，r为球的半径。障碍物在平面上形成的障碍域为(αi，αi+δi)(αi+1，αi+1+δi+1)(αi+2，αi+2+δi+2)Λ。其中当多个障碍域有重叠时，将这些障碍域进行合并，经过整理后的平面的障碍域为Yt=1(αt,αt+δt).]]>(4)搜索算法利用两岔树逆向寻优方法，如图6所示。机械臂避障路径规划的两岔树共由T段组成，每段又分为g级。第i段通过调解每一级的关节角，避开第i段的障碍域，形成第i个中间规划点 依此类推，最终到达机械臂目标位姿 其中ΔθjL和ΔθjU为本节点到下一级节点的连接权值(在整个两岔树中，虽然表述相同，但由于父节点不同，所以其值不同)。在每一级的规划中，若这个节点满足机械臂关节角的硬约束时，此节点为实心节点，否则为空心节点。空心节点没有子节点，规划时返回到父节点并沿另一岔规划。
两岔树逆向寻优方法从最后一级子节点开始逆向返回到根节点，同时计算每一条路径(每一条都是可行的避障规划路径)的权值 最后根据罚函数 确定一条“路径最短”的最优路径。其中Eg×1＝[1，1，Λ，1]T。
如图8所示，机器人系统由基座，六关节机械臂以及方形障碍物组成。
其中，A，B，C，D，E，F分别为基座及机械臂的安装位置参数；L1，L2，L3为简化后机械臂参数；g为外延尺寸；LWH为障碍物参数，r为障碍物的外接球半径(r=L2+W2+H2);]]>此外，机械臂的6关节角的硬约束条件为1)各个关节角的调节范围是〔-180~180〕；根据当前t时刻1)障碍物相对于基座的位置SSx，SSy，SSz；2)机械臂的6个初始关节角Q1，Q2，Q3，Q4，Q5，Q6；3)机械臂的6个目标关节角QQ1，QQ2，QQ3，QQ4，QQ5，QQ6；4)规划输出的节点的时间步长为0.25s；作如下规定规划时间步长为h＝0.25s；机械臂各个关节角在单位时间内的运动步长为 接下来，结合图9，对基于虚拟机器人的机器人避障路径规划方法进行描述。
第一步，确定机械臂的初始状态 和目标状态 第二步，确定时间步长h和 第三步，计算障碍物在水平面内的障碍域(HSSmin，HSSmax)，以及垂直平面内的障碍域。
第四步，在由 确定的垂直平面内顺序调节 直到11～13都在水平平面之上。
第五步，按靠近原则将调节到180、0、-180度。
第六步，判断若 而QQ1∈(HSSmin，HSSmax)或 QQ1>HSSmax]]>或 HSSmax时，进入下一步，否则进入第十步。
第七步，计算 的准目标位置Q Q1。
第八步，将 由当前位置调节到QQ1。
第九步，再由QQ1确定的垂直平面内调节 使得当 后L1，L2，L3将不会与障碍物相撞。
第十步，将 由当前位置调到QQ1。
第十一步，计算由QQ1确定的垂直平面下基座的障碍域。
第十二步，按 的顺序调节关节角到目标关节角QQ5，QQ3，QQ2。若成功，则进入下一步，否则，规划失败，进入第十四步。
第十三步，按 的顺序调节关节角到目标关节角QQ2，QQ3，QQ5。若成功，则进入下一步，否则，规划失败，进入第十六步。
第十四步，调节 至目标关节角QQ4。
第十五步，调节 至目标关节角QQ6。
第十六步，规划结束。
因此，基于虚拟场景的机器人避障相对于直接的机器人避障有消除远距离时延，为苛刻环境提供现场状况等优点。该规划方法可提高规划的安全性，实时性和高效性。
对于本领域的普通技术人员来说可显而易见的得出其他优点和修改。因此，具有更广方面的本发明并不局限于这里所示出的并且所描述的具体说明及示例性实施例。因此，在不脱离由随后权利要求及其等价体所定义的一般发明构思的精神和范围的情况下，可对其作出各种修改。
权利要求
1.一种基于虚拟场景的机器人避障路径规划方法，包括步骤(1)确定机械臂的初始状态 和目标状态 (2)确定时间步长h和 (3)计算障碍物在水平面内的障碍域HSSmin和HSSmax以及垂直平面内的障碍域。(4)在由 确定的垂直平面内顺序调节 (5)按靠近原则将 调节到180、0、-180度；(6)判断若 而QQ1∈(HSSmin，HSSmax)或 QQ1>HSSmax]]>或 时，进入下一步，否则进入第十步；(7)计算 的准目标位置QQ1；(8)将 由当前位置调节到QQ1；(9)再由QQ1确定的垂直平面内调节 使得当 后L1，L2，L3将不会与障碍物相撞；(10)将 由当前位置调到QQ1；(11)计算由QQ1确定的垂直平面下基座的障碍域；(12)按 的顺序调节关节角到目标关节角QQ5，QQ3，QQ2。若成功，则进入下一步；(13)按 的顺序调节关节角到目标关节角QQ2，QQ3，QQ5。若成功，则进入下一步；(14)调节 至目标关节角QQ4；(15)调节 至目标关节角QQ6；(16)规划结束。
2.根据权利要求1的方法，在步骤(13)中，若不成功，则规划失败，进入第十四步。
3.根据权利要求1的方法，在步骤(14)中，若不成功，则规划失败，进入第十六步。
全文摘要
本方法主要用于基于虚拟机器人的机器人避障路径规划，其利用规则体的包络对障碍物建模，并充分结合了关节空间法和C空间法的思想，在保持较高精度的前提下，通过在H和V平面上的投影确定障碍域，将三维避障路径规划问题转化成两个平面内的二维问题。该规划方法可提高规划的安全性，实时性和高效性。
文档编号B25J9/16GK1883887SQ20061009100
公开日2006年12月27日 申请日期2006年7月7日 优先权日2006年7月7日
发明者张珩, 陈靖波, 赵猛 申请人:中国科学院力学研究所