专利名称：产品零部件的拆卸路径求解方法及装置的制作方法
技术领域：
本发明涉及机械工程领域，特别是指一种产品零部件的拆卸路径求解方法及装置。
背景技术：
产品零部件的装配路径求解是以避障或满足作业需要为目的而进行的以安全性为主要指标的零部件装配路径搜索，同时也是验证产品装配顺序规划是否合理，保证产品可装配性的重要手段。装配路径求解技术是面向装配的设计(DFA)中的一项重要关键技术。装配路径的可行性直接关系到装配方案的可行性，在现代产品的装配规划过程中，装配路径规划已成为关键步骤之一。在飞机、船舶、汽车、航天器等复杂产品的装配中，由于产品零部件多、结构复杂， 其拆卸/装配路径的规划是在一个由产品零部件、工夹具等组成的复杂环境中进行。完全依靠经验和交互式来实现装配路径规划，需要花费大量的时间且很难实现路径优化，甚至出现由于交互式方法的局限性导致部分零部件甚至无法进行装配规划。利用计算机工具进行拆卸/装配路径的自动规划是一项有广阔应用背景的技术。目前国内、国外对于装配路径自动求解技术已经有了一定的研究，但仍主要停留在理论探索的阶段，相关的实用性工具较少，其性能也有待改进。
发明内容
本发明要解决的技术问题是提供一种产品零部件的拆卸路径求解方法及装置，可以实现对复杂环境中对产品拆卸路径进行求解，提高产品拆卸路径的求解效率。为解决上述技术问题，本发明的实施例提供一种产品零部件的拆卸路径求解方法，包括设置产品零部件的已装配位姿；将所述已装配位姿作为第一求解阶段的第一起始位姿，对所述第一求解阶段的路径进行求解，得到所述产品零部件在所述第一求解阶段的第一终止位姿；将所述第一终止位姿作为第二求解阶段的第二起始位姿，对所述第二求解阶段的路径进行求解，得到所述产品零部件在所述第二求解阶段的第二终止位姿；将所述第二终止位姿作为第三求解阶段的第三起始位姿，对所述第三求解阶段的路径进行求解，得到所述产品零部件在所述第三求解阶段的第三终止位姿；将所述第一起始位姿到达所述第一终止位姿的第一路径，所述第二起始位姿到达所述第二终止位姿的第二路径以及所述第三起始位姿到达所述第三终止位姿的第三路径首尾相连，得到所述零部件的全局拆卸路径。其中，所述对所述第一求解阶段的路径进行求解的过程包括将所述第一起始位姿设置为树的根结点；随机产生一第一空间位姿，所述第一空间位姿对应第一结点；
获得树中离所述第一结点最近的第二结点；将所述零部件沿所述第二结点到所述第一结点方向按照预设第一步长运动，得到运动所述第一步长后的新位姿对应的第三结点；对所述零部件在所述第三结点处，以及从所述第二结点到所述第三结点的路径上的关键插值点处进行碰撞检测；若所述零部件在所述第三结点处和所述关键插值点处与面片模型有碰撞，则扩展所述第二结点，若扩展失败，则返回随机产生一第一空间位姿的步骤重新执行，若扩展成功，判断所述零部件与所述面片模型是否不存在面片级别的碰撞或者所述第三结点到达所述第一起始位姿对应的根结点的距离是否达到预设的第一局部拆卸路径长度，若是，终止所述第一求解阶段的求解，否则返回随机产生一第一空间位姿的步骤重新执行；若无碰撞， 则连接所述第二结点和所述第三结点所形成的路径作为所述第一路径。其中，所述碰撞检测的方法包括获得一测试射线；将所述测试射线分别与所述零部件以及与所述零部件进行碰撞测试的面片模型进行相交测试；获得所述测试射线同时位于所述零部件以及所述面片模型中的线段；多次获得不同的测试射线分别对所述零部件以及所述面片模型进行相交测试，获得多个线段；若所述多个线段中长度大于预设长度阈值的线段个数与所述多个线段的总数的比值大于预设比率阈值，则认为所述零部件与所述面片模型产生了碰撞，否则认为无碰撞。其中，所述扩展所述第二结点的步骤包括查找所述树中所述第二结点下最远的叶结点为第四结点；依次获得所述叶结点的第一父辈结点，第二父辈结点，直到所获得的父辈结点的数量达到一预设值或者获得了所述树的根结点；获得所述预设值数量的所述父辈结点的平均位姿结点，或者从所述叶结点到所述树的根结点之间的包括所述第一父辈结点、第二父辈结点的多个父辈结点的平均位姿结用所述第四结点代替所述第二结点，并确定新的方向为从所述平均位姿结点到所述第四结点。其中，所述对所述第二求解阶段的路径进行求解的过程包括将所述第二起始位姿设置为树的根结点；随机产生一第二空间位姿，所述第二空间位姿对应第五结点；获得树中离所述第五结点最近的第六结点；在区间
获取随机数r，将所述随机数r与所述第六结点上的扩展概率ρ比较，如果r < p，则对所述第六结点进行扩展；如果r > = p，则令所述第六结点下最远的叶结点取代所述第六结点进行扩展，若扩展失败，返回随机产生一第二空间位姿的步骤重新执行，若扩展成功，判断新扩展得到的所述第六结点到达所述第一起始位姿对应的根结点的距离是否达到预设的第二局部拆卸路径长度，若达到，终止所述第一求解阶段，否则返回随机产生一第二空间位姿的步骤重新执行。
其中，对所述六结点进行扩展的过程包括将所述零部件沿所述第六结点到所述第五结点的方向按照一定步长运动，计算运动该步长后的新位姿结点；对所述零部件在所述新位姿结点处及从所述第六结点到所述新位姿结点的路径的关键插值点处进行碰撞检测，如果无碰撞，则连接所述第六结点和所述新位姿结点；如果存在碰撞，将扩展失败结点的扩展概率减小，并确定一扩展方式，按照选取的所述扩展方式计算所述新位姿结点，将所述第六结点向所述新位姿结点进行扩展。其中，所述扩展方式包括向采样点方向扩展，向该结点的父亲结点到该结点的方向扩展，随机选择方向扩展和/或沿空间一维度方向扩展。其中，所述叶结点的扩展权值保持为1。其中，所述对所述第三求解阶段的路径进行求解的步骤具体为采用双向的快速扩展随机树算法对所述第三求解阶段的路径进行求解。其中，上述方法还包括根据设定的全局路径步长，对所述全局拆卸路径中的路径点序列进行插值和顺滑。其中，上述方法还包括对所述全局拆卸路径上的路径点序列反序，得到所述零部件的装配路径。本发明的实施例还提供一种产品零部件的拆卸路径求解装置，包括设置模块，用于设置产品零部件的已装配位姿；第一求解模块，用于将所述已装配位姿作为第一求解阶段的第一起始位姿，对所述第一求解阶段的路径进行求解，得到所述产品零部件在所述第一求解阶段的第一终止位姿；第二求解模块，用于将所述第一终止位姿作为第二求解阶段的第二起始位姿，对所述第二求解阶段的路径进行求解，得到所述产品零部件在所述第二求解阶段的第二终止位姿；第三求解模块，用于将所述第二终止位姿作为第三求解阶段的第三起始位姿，对所述第三求解阶段的路径进行求解，得到所述产品零部件在所述第三求解阶段的第三终止位姿；全局拆卸路径生成模块，用于将所述第一起始位姿到达所述第一终止位姿的第一路径，所述第二起始位姿到达所述第二终止位姿的第二路径以及所述第三起始位姿到达所述第三终止位姿的第三路径首尾相连，得到所述零部件的全局拆卸路径。本发明的上述技术方案的有益效果如下上述方案中，将全局拆卸路径的求解分解为多个阶段(包括上述的第一求解阶段，第二求解阶段和第三求解阶段)进行，对每个阶段使用适应其特征的求解算法，能够稳定快速的对复杂环境中的拆卸/装配路径进行求解，且该方法是一个通用、可行的复杂环境中产品拆卸/装配路径求解方法，以产品的数字化几何模型为基础，通过相应算法，利用计算机对产品拆卸/装配路径进行求解，可以稳定快速的求解环境未知且带有狭窄通道、 大量障碍物等复杂环境中的拆卸/装配路径。
图1为本发明的产品零部件的拆卸路径求解方法流程示意图；图2为图1所示方法中，零部件从已装配位姿开始经过第一路径、第二路径以及第三路径，得到全局路径的过程示意图；图3为本发明的方法的第一求解阶段过程中采用的基于历史引导的快速扩展随机树算法的扩展过程示意图；图4为本发明的方法的第一求解阶段过程中采用的碰撞算法示意图；图5为图4所示的碰撞算法中多次进行射线检测的示意图；图6为本发明的方法的第二求解阶段过程中采用的自适应的快速扩展随机树算法中的结点扩展方式中向采样点方向扩展的示意图；图7为本发明的方法的第二求解阶段过程中采用的自适应的快速扩展随机树算法中的结点扩展方式中向该结点的父结点到该结点的扩展方向扩展的示意图；图8为本发明的方法的第二求解阶段过程中采用的自适应的快速扩展随机树算法中的结点扩展方式中随机选择方向扩展的示意图；图9为本发明的方法的第二求解阶段过程中采用的自适应的快速扩展随机树算法中的结点扩展方式中沿空间某一维度方向扩展的示意图。
具体实施例方式为使本发明要解决的技术问题、技术方案和优点更加清楚，下面将结合附图及具体实施例进行详细描述。如图1、图2所示，本发明的实施例一种产品零部件的拆卸路径求解方法，包括步骤11，设置产品零部件的已装配位姿；步骤12，将所述已装配位姿作为第一求解阶段的第一起始位姿，对所述第一求解阶段的路径进行求解，得到所述产品零部件在所述第一求解阶段的第一终止位姿；步骤13，将所述第一终止位姿作为第二求解阶段的第二起始位姿，对所述第二求解阶段的路径进行求解，得到所述产品零部件在所述第二求解阶段的第二终止位姿；步骤14，将所述第二终止位姿作为第三求解阶段的第三起始位姿，对所述第三求解阶段的路径进行求解，得到所述产品零部件在所述第三求解阶段的第三终止位姿；步骤15，将所述第一起始位姿到达所述第一终止位姿的第一路径，所述第二起始位姿到达所述第二终止位姿的第二路径以及所述第三起始位姿到达所述第三终止位姿的第三路径首尾相连，得到所述零部件的全局拆卸路径(如图2所示，零部件21经过第一路径a，第二路径b以及第三路径c，路径a，b，c首尾相连，得到零部件的全局拆卸路径)。该实施例主要用于在复杂环境中对产品零部件装配过程中，产品零部件的拆卸/ 装配路径的求解，复杂环境是指由于产品结构复杂、零部件数量繁多、工作环境具有不确定性等而造成拆卸/装配路径求解的障碍物环境可能具有狭窄通道、密集障碍物、场景易变动等难题，已有的算法和工具求解效率低或无法解决，本发明的该实施例将全局拆卸路径的求解分解为多个阶段(包括上述的第一求解阶段，第二求解阶段和第三求解阶段)进行， 对每个阶段使用适应其特征的求解算法，能够稳定快速的对复杂环境中的拆卸/装配路径进行求解，且该方法是一个通用、可行的复杂环境中产品拆卸/装配路径求解方法，以产品的数字化几何模型为基础，通过相应算法，利用计算机对产品拆卸/装配路径进行求解，可以稳定快速的求解环境未知且带有狭窄通道、大量障碍物等复杂环境中的拆卸/装配路径。在本发明的另一实施例中，对所述第一求解阶段的路径进行求解的过程采用本发明提出的基于历史引导的快速扩展随机树算法，其中，该算法中的物体碰撞检测采用基于随机射线测试的碰撞检测与评估方法，其中，基于历史引导的快速扩展随机树算法包括将所述第一起始位姿设置为树的根结点；随机产生一第一空间位姿，所述第一空间位姿对应第一结点；获得树中离所述第一结点最近的第二结点；将所述零部件沿所述第二结点到所述第一结点方向按照预设第一步长运动，得到运动所述第一步长后的新位姿对应的第三结点；对所述零部件在所述第三结点处，以及从所述第二结点到所述第三结点的路径上的关键插值点处进行碰撞检测；若所述零部件在所述第三结点处和所述关键插值点处与面片模型有碰撞(其中， 这里的面片模型可以包括当前装配环境中其他所有零部件和环境模型的面片模型，其中， 其他零部件在被作为障碍物时，也可以被称为面片模型)，则扩展所述第二结点，若扩展失败，则返回随机产生一第一空间位姿的步骤重新执行，若扩展成功，判断所述零部件与所述面片模型是否不存在面片级别的碰撞或者第三结点到达所述第一起始位姿对应的根结点的距离是否达到预设的第一局部拆卸路径长度，若是，终止所述第一求解阶段的求解，否则返回随机产生一第一空间位姿的步骤重新执行；若无碰撞，则连接所述第二结点和所述第三结点所形成的路径作为所述第一路径。其中，所述扩展所述第二结点的步骤包括查找所述树中所述第二结点下最远的叶结点为第四结点；依次获得所述叶结点的第一父辈结点，第二父辈结点，直到所获得的父辈结点数量达到一预设值N(如N = 8)或者获得了所述树的根结点；获得所述预设值数量的所述父辈结点的平均位姿结点，或者从所述叶结点到所述树的根结点之间的包括所述第一父辈结点、所述第二父辈结点的多个父辈结点的平均位姿用所述第四结点代替所述第二结点，并确定新的方向为从所述平均位姿结点到所述第四结点。具体实现过程可参考如下步骤21，将待求解路径(上述第一求解阶段)的起始点设置为树的根结点；步骤22，随机产生一个空间位姿QMnd(上述第一空间位姿)；步骤23，获得树中离随机空间位姿最近的结点qnea,(上述第二结点)；步骤M，沿qn_到qMnd方向按照一定步长(其中该步长可预先设定)运动，计算运动该步长后的新位姿qnOT(上述第三结点)；步骤25，对零部件在新位姿qnew处及qnea,到qnew的路径关键插值点处进行碰撞检测，如果无碰撞，则连接qnea,和qnew ；如果存在碰撞，则扩展qnea,，执行以下步骤251-254，返回最终结果；
步骤251，寻找树中qnear结点下最远的叶结点qfr。nt (上述第四结点)；步骤252，依次寻找叶结点的N个父辈结点，直到所获得的父辈结点数量达到一预设值N(如N = 8)或者获得了所述树的根结点；步骤253，计算N个父辈结点的平均位姿信息qavCT(平均位姿结点)或者从所述叶结点到所述树的根结点之间的包括所述第一父辈结点、第二父辈结点的多个父辈结点的平均位姿结点qavCT ；步骤254，确定新的扩展结点为qft。nt，代替qnea,；新的扩展方向应为从qavCT到 Qfront ；执行步骤24-25 ；如图3所示，qdi,在图中用于展示从结点qfront以向量(qfrant_qavJ为
方向进行扩展，qdil"q&。nt = Qfront-Qaver ；步骤沈，如果步骤25的结果为扩展失败，则返回步骤22 ；如果扩展成功，则判断新位姿qnew到达起始点(上述第一起始位姿)的距离是否超过预设的第一局部拆卸路径长度或者零部件与所述面片模型是否完全不存在面片级别的碰撞，如果是，则停止计算；如果否，则返回步骤22。在上述基于历史引导的快速扩展随机树算法中，碰撞检测算法采用基于随机射线测试的碰撞检测与评估方法，该方法的实现过程包括获得一测试射线；将所述测试射线分别与所述零部件以及与所述零部件进行碰撞测试的面片模型进行相交测试；获得所述测试射线同时位于所述零部件以及所述面片模型中的线段；多次获得不同的测试射线分别对所述零部件以及所述面片模型进行相交测试，获得多个线段；若所述多个线段中长度大于预设长度阈值的线段个数与所述多个线段的总数的比值大于预设比率阈值，则认为所述零部件与所述面片模型产生了碰撞，否则认为无碰撞。如图4和图5所示，该基于随机射线测试的碰撞检测与评估方法的具体实现过程可参考如下若利用该方法对物体A和物体B的碰撞情况进行检测和评估，默认已获得A和B 的面片模型(其中，该面片模型是指路径求解过程中所涉及的产品零部件、工夹具、工作环境等，其中工作环境包括工作台、车间等)，这里的产品零部件、工夹具和工作环境等都可以称为几何面片模型(一般为无序的三角面片模型)的前提下，算法描述如下步骤31，设定长度阈值t > 0，设定比率阈值0 < ρ < 1 ；步骤32，在空间中定义一条随机射线O1D1 ；步骤33，将射线分别与物体A和物体B做相交测试，测试中遍历A和B的所有面片， 并按顺序记录射线与物体的交点，分别为集合Xm = IA1, A2, A3. . . }，Xob = (B1, B2, B3. . . }； 如果射线与物体A无交点，或与物体B无交点，则返回步骤32 ；如图4所示；步骤34，分别获取射线位于物体A和物体B内部的线段，其中，位于物体A 中的线段如A1A2，A2A3以及A3A4，位于物体B内部的线段如B1B2,若集合Xm元素个数为偶数，则有 SoA={A)A)+\\A),A) + IeJTcw,/= 1,3,5^(^0^-1)}，若为奇数个，则有
L={^TT|4，4+leX。W = 2，4，6，...，(|Xm|-l)}。同样的方式定义
步骤35，遍历 S0JP S。B，&仙={1 = 1^。!^ I VZa4 GScw，VZos e Sos},即同时位于物体A和物体B内部的线段即位于A和B的交集中，即线段B1A3和A3B2 ；步骤36，将射线按照光线反射规则在A和B的交集内进行反射，定义新的射线或者重新定义随机射线(如图5中所示的多个箭头所示的随机射线)；重复步骤33-36N次，记录检测结果于S-中；步骤37，分析和评估检测结果，如果S-中长度大于t的元素数比率大于p，则认为A和B产生了深度碰撞；否则认为产生了浅碰撞或无碰撞。在本发明的另一实施例中，对所述第二求解阶段的路径进行求解的过程采用本发明提出的自适应的快速扩展随机树算法，其中，该算法中的物体碰撞检测采用传统的碰撞检测方法，其中，自适应的快速扩展随机树算法包括将所述第二起始位姿设置为树的根结点；随机产生一第二空间位姿，所述第二空间位姿对应第五结点；获得树中离所述第五结点最近的第六结点；在区间
获取随机数r，将所述随机数r与所述第六结点上的扩展概率ρ比较，如果r < p，则对所述第六结点进行扩展；如果r > = p，则令所述第六结点下最远的叶结点取代所述第六结点进行扩展，若扩展失败，返回随机产生一第二空间位姿的步骤重新执行，若扩展成功，判断新扩展得到的所述第六结点到达所述第一起始位姿对应的根结点的距离是否达到预设的第二局部拆卸路径长度，若达到，终止所述第一求解阶段，否则返回随机产生一第二空间位姿的步骤重新执行。其中，这里的预设的第二局部拆卸路径长度与上述预设的第一局部拆卸路径长度可以相同，也可以不相同。其中，对所述六结点进行扩展的过程包括将所述零部件沿所述第六结点到所述第五结点的方向按照一定步长运动，计算运动该步长后的新位姿结点；对所述零部件在所述新位姿结点处及从所述第六结点到所述新位姿结点的路径的关键插值点处进行碰撞检测，如果无碰撞，则连接所述第六结点和所述新位姿结点；如果存在碰撞，将扩展失败结点的扩展概率减小，并确定一扩展方式，按照选取的所述扩展方式计算所述新位姿结点，将所述第六结点向所述新位姿结点进行扩展。该算法中，所述扩展方式可以包括向采样点方向扩展，向该结点的父亲结点到该结点的方向扩展，随机选择方向扩展和/或沿空间一维度方向扩展。优选的，该算法中的所述叶结点的扩展权值保持为1。该自适应快速扩展随机树算法的具体实现过程可参考如下该算法中所涉及的四种结点扩展方式包括向采样点方向扩展；向该结点的父结点到该结点的扩展方向扩展；随机选择方向扩展；沿空间某一维度方向扩展，分别如图6、 7、8、9所示。算法流程描述如下步骤41，将待求解路径的起始点设置为树的根结点，至多重复K次执行以下步骤42，产生一个受终点引导的随机空间位姿QMnd(即上述第二空间位姿)；步骤43，获得树中离随机空间位姿最近的结点qnea,(即上述第六结点)；步骤44，在区间
获取随机数r，与qn_上的扩展概率ρ比较，如果r <p，则对qnea,进行扩展；如果r > = p，则令qnea,下最远的叶结点qfrant取代qnea,进行扩展步骤45，沿qnea,到q—方向按照一定步长运动，计算运动该步长后的新位姿qnew ；步骤46，对物体在新位姿qnew处及qnea,到qnew的路径关键插值点处进行碰撞检测， 如果无碰撞，则连接qn_和；如果存在碰撞，将扩展失败结点的扩展概率减小，并至多重复N次执行以下；a，选取扩展方式；b，按照扩展方式计算qnew ；C，将 qne 向 qnew 扩展；d，如果成功，返回结果；如果失败，返回到a;步骤47，如果步骤44的结果为扩展失败，则返回步骤42;如果扩展成功，则判断新结点是否靠近终点小于阈值，如果否，则返回步骤42，如果是，则停止计算。其中，该扩展过程中，在每个节点上赋权值，通过设定权值大小表明该点是否易于扩展(也称扩展概率)， 并控制树在该节点生长的概率；在树的扩展中，如果某结点被选为待扩展结点，进一步根据权值大小决定是否在该节点进行扩展；优选的，所有叶节点的扩展权值总是保持为1，保证其扩展概率不被减弱；如果在某一非叶节点扩展失败，则降低该节点的权值；如果某节点多次扩展失败，则设置其扩展权值为0，关闭该节点的扩展；在树扩展遇到障碍物时，多次尝试采用不同的扩展方式(如在上述a，b，c，d四种扩展方式中进行尝试)进行扩展，强化树在障碍物附近的扩展；不同的扩展方式中，选择相应的扩展方向，并以一定步长扩展。在本发明的另一实施例中，上述对所述第三求解阶段的路径进行求解的步骤具体为采用传统的双向的快速扩展随机树算法对所述第三求解阶段的路径进行求解。进一步地，上述图1所示的方法还可以包括根据设定的全局路径步长，对所述全局拆卸路径中的路径点序列进行插值和顺滑。进一步地，上述图1所示的方法还可以包括对所述全局拆卸路径上的路径点序列反序，得到所述零部件的装配路径。综上，本发明的实施例提出一种通用、可行的复杂环境中产品拆卸/装配路径求解方法，以产品的数字化几何模型为基础，通过相应算法，利用计算机对产品拆卸/装配路径进行求解。本发明的主要特点和优势在于(1)创新性的提出了基于历史引导的快速扩展随机树算法(RRT-H)算法、自适应快速扩展随机树算法(RRT-A算法)以及在RRT-H算法中采用的基于随机射线测试的碰撞检测与评估方法(RTSColDet算法)，该RTSColDet算法可以解决传统碰撞检测算法对具有面接触的两物体间的碰撞检测伪碰撞问题；RRT-H算法可以快速稳定求解极狭窄空间中简单运动的路径；RRT-A算法可以稳定快速的求解环境未知且带有狭窄通道、大量障碍物等复杂环境中的拆卸/装配路径。本发明的方法将路径求解过程所涉及的产品零部件、工夹具、工作环境包括工作台、车间等所有的几何面片模型 (一般为无序的三角面片模型)输入路径求解装置，并根据其位姿信息定义每个模型在空间中的位置和姿态；设定要进行路径求解的零部件，设置其已装配位姿和未装配位姿，则待求路径为已装配位姿到未装配位姿的路径，其余所有模型则作为拆卸/装配过程中的障碍物对待；设置全局路径的步长；分别设置RRT-H/RRT-A算法中的路径步长；设置RTSColDet 的碰撞深度阈值；接着进行路径求解过程，该路径求解过程求解所指定零部件的拆卸路径，分三个阶段实现，第一求解阶段、第二求解阶段以及第三求解阶段，如任一阶段计算失败， 则退出该次求解；首先使用RRT-H算法进行第一路径的求解，其中碰撞检测利用RTSColDet 算法实现；该第一求解阶段中设置RRT-H的起始位姿状态为全局路径中的零部件装配位姿，不设定RRT-H的终止位姿状态；当RTSColDet碰撞检测结果返回的碰撞面片数为O时， 用传统碰撞检测算法再次检测，如无碰撞，则终止该阶段的路径求解；否则判断新扩展的结点到达所述第一阶段树的根结点的距离是否达到预设的第一局部拆卸路径长度，如果是， 则终止该阶段的路径求解；该第二求解阶段使用RRT-A进行路径求解，使用传统的碰撞检测算法进行碰撞检测；设定上一阶段(即第一求解阶段)求解到的路径终止位姿设定为 RRT-A的起始位姿，当RRT-A生成的树中某一结点距离全局路径起始点达到设定的距离，则终止该第二求解阶段的求解；该第三求解阶段使用RRT-ExtCon(双向的快速扩展随机树算法)进行路径求解，使用传统的碰撞检测算法进行碰撞检测；设定上一阶段(即上述第二求解阶段)求解到的路径终止位姿设定为RRT-ExtCon的起始位姿，全局路径的终止位姿为RRT-ExtCon的终止位姿，当求解出该起始位姿到终止位姿的路径后终止路径求解；将上述三个求解阶段所求解的三段路径依照顺序首尾连接，则获得从零部件装配位姿到未装配位姿的全局拆卸路径，实质上是一系列的有序位姿路径点；根据需求，如需要装配路径，则将该路径点序列反序；根据设定的全局路径步长，对路径点序列进行插值(优选为线性插值)，以获得更精细顺滑的全局拆卸路径。在本发明的另一方面，本发明的实施例还提供一种与上述方法相应的产品零部件的拆卸路径求解装置(也可以称为求解器)，包括设置模块，用于设置产品零部件的已装配位姿；第一求解模块，用于将所述已装配位姿作为第一求解阶段的第一起始位姿，对所述第一求解阶段的路径进行求解，得到所述产品零部件在所述第一求解阶段的第一终止位姿；第二求解模块，用于将所述第一终止位姿作为第二求解阶段的第二起始位姿，对所述第二求解阶段的路径进行求解，得到所述产品零部件在所述第二求解阶段的第二终止位姿；第三求解模块，用于将所述第二终止位姿作为第三求解阶段的第三起始位姿，对所述第三求解阶段的路径进行求解，得到所述产品零部件在所述第三求解阶段的第三终止位姿；全局拆卸路径生成模块，用于将所述第一起始位姿到达所述第一终止位姿的第一路径，所述第二起始位姿到达所述第二终止位姿的第二路径以及所述第三起始位姿到达所述第三终止位姿的第三路径首尾相连，得到所述零部件的全局拆卸路径。上述方法中所有实现手段和应用场景均适用于该装置的实施例中，也能达到相同的技术效果，在此不再赘述。以上所述是本发明的优选实施方式，应当指出，对于本技术领域的普通技术人员来说，在不脱离本发明所述原理的前提下，还可以作出若干改进和润饰，这些改进和润饰也应视为本发明的保护范围。
权利要求
1.一种产品零部件的拆卸路径求解方法，其特征在于，包括 设置产品零部件的已装配位姿；将所述已装配位姿作为第一求解阶段的第一起始位姿，对所述第一求解阶段的路径进行求解，得到所述产品零部件在所述第一求解阶段的第一终止位姿；将所述第一终止位姿作为第二求解阶段的第二起始位姿，对所述第二求解阶段的路径进行求解，得到所述产品零部件在所述第二求解阶段的第二终止位姿；将所述第二终止位姿作为第三求解阶段的第三起始位姿，对所述第三求解阶段的路径进行求解，得到所述产品零部件在所述第三求解阶段的第三终止位姿；将所述第一起始位姿到达所述第一终止位姿的第一路径，所述第二起始位姿到达所述第二终止位姿的第二路径以及所述第三起始位姿到达所述第三终止位姿的第三路径首尾相连，得到所述零部件的全局拆卸路径。
2.根据权利要求1所述的产品零部件的拆卸路径求解方法，其特征在于，所述对所述第一求解阶段的路径进行求解的过程包括将所述第一起始位姿设置为树的根结点；随机产生一第一空间位姿，所述第一空间位姿对应第一结点；获得树中离所述第一结点最近的第二结点；将所述零部件沿所述第二结点到所述第一结点方向按照预设第一步长运动，得到运动所述第一步长后的新位姿对应的第三结点；对所述零部件在所述第三结点处，以及从所述第二结点到所述第三结点的路径上的关键插值点处进行碰撞检测；若所述零部件在所述第三结点处和所述关键插值点处与面片模型有碰撞，则扩展所述第二结点，若扩展失败，则返回随机产生一第一空间位姿的步骤重新执行，若扩展成功，判断所述零部件与所述面片模型是否不存在面片级别的碰撞或者所述第三结点到达所述第一起始位姿对应的根结点的距离是否达到预设的第一局部拆卸路径长度，若是，终止所述第一求解阶段的求解，否则返回随机产生一第一空间位姿的步骤重新执行；若无碰撞，则连接所述第二结点和所述第三结点所形成的路径作为所述第一路径。
3.根据权利要求2所述的产品零部件的拆卸路径求解方法，其特征在于，所述碰撞检测的方法包括获得一测试射线；将所述测试射线分别与所述零部件以及与所述零部件进行碰撞测试的面片模型进行相交测试；获得所述测试射线同时位于所述零部件以及所述面片模型中的线段； 多次获得不同的测试射线分别对所述零部件以及所述面片模型进行相交测试，获得多个线段；若所述多个线段中长度大于预设长度阈值的线段个数与所述多个线段的总数的比值大于预设比率阈值，则认为所述零部件与所述面片模型产生了碰撞，否则认为无碰撞。
4.根据权利要求2所述的产品零部件的拆卸路径求解方法，其特征在于，所述扩展所述第二结点的步骤包括查找所述树中所述第二结点下最远的叶结点为第四结点；依次获得所述叶结点的第一父辈结点，第二父辈结点，直到所获得的父辈结点数量达到一预设值或者获得了所述树的根结点；获得所述预设值数量的所述父辈结点的平均位姿结点，或者从所述叶结点到所述树的根结点之间的包括所述第一父辈结点、第二父辈结点的多个父辈结点的平均位姿结点；用所述第四结点代替所述第二结点，并确定新的方向为从所述平均位姿结点到所述第四结点。
5.根据权利要求1所述的产品零部件的拆卸路径求解方法，其特征在于，所述对所述第二求解阶段的路径进行求解的过程包括将所述第二起始位姿设置为树的根结点；随机产生一第二空间位姿，所述第二空间位姿对应第五结点；获得树中离所述第五结点最近的第六结点；在区间W，l]获取随机数r，将所述随机数r与所述第六结点上的扩展概率ρ比较，如果！· < p，则对所述第六结点进行扩展；如果r > = p，则令所述第六结点下最远的叶结点取代所述第六结点进行扩展，若扩展失败，返回随机产生一第二空间位姿的步骤重新执行，若扩展成功，判断新扩展得到的所述第六结点到达所述第一起始位姿对应的根结点的距离是否达到预设的第二局部拆卸路径长度，若达到，终止所述第一求解阶段，否则返回随机产生一第二空间位姿的步骤重新执行。
6.根据权利要求5所述的产品零部件的拆卸路径求解方法，其特征在于，对所述六结点进行扩展的过程包括将所述零部件沿所述第六结点到所述第五结点的方向按照一定步长运动，计算运动该步长后的新位姿结点；对所述零部件在所述新位姿结点处及从所述第六结点到所述新位姿结点的路径的关键插值点处进行碰撞检测，如果无碰撞，则连接所述第六结点和所述新位姿结点；如果存在碰撞，将扩展失败结点的扩展概率减小，并确定一扩展方式，按照选取的所述扩展方式计算所述新位姿结点，将所述第六结点向所述新位姿结点进行扩展。
7.根据权利要求6所述的产品零部件的拆卸路径求解方法，其特征在于，所述扩展方式包括向采样点方向扩展，向该结点的父亲结点到该结点的方向扩展，随机选择方向扩展和/或沿空间一维度方向扩展。
8.根据权利要求6所述的产品零部件的拆卸路径求解方法，其特征在于，所述叶结点的扩展权值保持为1。
9.根据权利要求1所述的产品零部件的拆卸路径求解方法，其特征在于，所述对所述第三求解阶段的路径进行求解的步骤具体为采用双向的快速扩展随机树算法对所述第三求解阶段的路径进行求解。
10.根据权利要求1-9任一项所述的产品零部件的拆卸路径求解方法，其特征在于，还包括根据设定的全局路径步长，对所述全局拆卸路径中的路径点序列进行插值和顺滑。
11.根据权利要求10所述的产品零部件的拆卸路径求解方法，其特征在于，还包括对所述全局拆卸路径上的路径点序列反序，得到所述零部件的装配路径。
12.—种产品零部件的拆卸路径求解装置，其特征在于，包括设置模块，用于设置产品零部件的已装配位姿；第一求解模块，用于将所述已装配位姿作为第一求解阶段的第一起始位姿，对所述第一求解阶段的路径进行求解，得到所述产品零部件在所述第一求解阶段的第一终止位姿；第二求解模块，用于将所述第一终止位姿作为第二求解阶段的第二起始位姿，对所述第二求解阶段的路径进行求解，得到所述产品零部件在所述第二求解阶段的第二终止位姿；第三求解模块，用于将所述第二终止位姿作为第三求解阶段的第三起始位姿，对所述第三求解阶段的路径进行求解，得到所述产品零部件在所述第三求解阶段的第三终止位姿；全局拆卸路径生成模块，用于将所述第一起始位姿到达所述第一终止位姿的第一路径，所述第二起始位姿到达所述第二终止位姿的第二路径以及所述第三起始位姿到达所述第三终止位姿的第三路径首尾相连，得到所述零部件的全局拆卸路径。
全文摘要
本发明提供一种产品零部件的拆卸路径求解方法及装置，方法包括将零部件的已装配位姿作为第一求解阶段的第一起始位姿，对第一求解阶段的路径进行求解，得到第一求解阶段的第一终止位姿；将第一终止位姿作为第二求解阶段的第二起始位姿，对第二求解阶段的路径进行求解，得到第二求解阶段的第二终止位姿；将第二终止位姿作为第三求解阶段的第三起始位姿，对第三求解阶段的路径进行求解，得到第三求解阶段的第三终止位姿；将第一起始位姿到达第一终止位姿的第一路径，第二起始位姿到达第二终止位姿的第二路径以及第三起始位姿到达第三终止位姿的第三路径首尾相连，得到零部件的全局拆卸路径。本发明可以实现对复杂环境中对产品拆卸路径进行求解。
文档编号G06F17/50GK102289553SQ20111028874
公开日2011年12月21日 申请日期2011年9月26日 优先权日2011年9月26日
发明者刘密, 刘检华, 宁汝新, 尚炜 申请人:北京理工大学