專利名稱：產(chǎn)品零部件的拆卸路徑求解方法及裝置的制作方法
技術(shù)領(lǐng)域：
本發(fā)明涉及機(jī)械工程領(lǐng)域，特別是指一種產(chǎn)品零部件的拆卸路徑求解方法及裝置。
背景技術(shù)：
產(chǎn)品零部件的裝配路徑求解是以避障或滿足作業(yè)需要為目的而進(jìn)行的以安全性為主要指標(biāo)的零部件裝配路徑搜索，同時(shí)也是驗(yàn)證產(chǎn)品裝配順序規(guī)劃是否合理，保證產(chǎn)品可裝配性的重要手段。裝配路徑求解技術(shù)是面向裝配的設(shè)計(jì)(DFA)中的一項(xiàng)重要關(guān)鍵技術(shù)。裝配路徑的可行性直接關(guān)系到裝配方案的可行性，在現(xiàn)代產(chǎn)品的裝配規(guī)劃過(guò)程中，裝配路徑規(guī)劃已成為關(guān)鍵步驟之一。在飛機(jī)、船舶、汽車、航天器等復(fù)雜產(chǎn)品的裝配中，由于產(chǎn)品零部件多、結(jié)構(gòu)復(fù)雜， 其拆卸/裝配路徑的規(guī)劃是在一個(gè)由產(chǎn)品零部件、工夾具等組成的復(fù)雜環(huán)境中進(jìn)行。完全依靠經(jīng)驗(yàn)和交互式來(lái)實(shí)現(xiàn)裝配路徑規(guī)劃，需要花費(fèi)大量的時(shí)間且很難實(shí)現(xiàn)路徑優(yōu)化，甚至出現(xiàn)由于交互式方法的局限性導(dǎo)致部分零部件甚至無(wú)法進(jìn)行裝配規(guī)劃。利用計(jì)算機(jī)工具進(jìn)行拆卸/裝配路徑的自動(dòng)規(guī)劃是一項(xiàng)有廣闊應(yīng)用背景的技術(shù)。目前國(guó)內(nèi)、國(guó)外對(duì)于裝配路徑自動(dòng)求解技術(shù)已經(jīng)有了一定的研究，但仍主要停留在理論探索的階段，相關(guān)的實(shí)用性工具較少，其性能也有待改進(jìn)。

發(fā)明內(nèi)容
本發(fā)明要解決的技術(shù)問(wèn)題是提供一種產(chǎn)品零部件的拆卸路徑求解方法及裝置，可以實(shí)現(xiàn)對(duì)復(fù)雜環(huán)境中對(duì)產(chǎn)品拆卸路徑進(jìn)行求解，提高產(chǎn)品拆卸路徑的求解效率。為解決上述技術(shù)問(wèn)題，本發(fā)明的實(shí)施例提供一種產(chǎn)品零部件的拆卸路徑求解方法，包括設(shè)置產(chǎn)品零部件的已裝配位姿；將所述已裝配位姿作為第一求解階段的第一起始位姿，對(duì)所述第一求解階段的路徑進(jìn)行求解，得到所述產(chǎn)品零部件在所述第一求解階段的第一終止位姿；將所述第一終止位姿作為第二求解階段的第二起始位姿，對(duì)所述第二求解階段的路徑進(jìn)行求解，得到所述產(chǎn)品零部件在所述第二求解階段的第二終止位姿；將所述第二終止位姿作為第三求解階段的第三起始位姿，對(duì)所述第三求解階段的路徑進(jìn)行求解，得到所述產(chǎn)品零部件在所述第三求解階段的第三終止位姿；將所述第一起始位姿到達(dá)所述第一終止位姿的第一路徑，所述第二起始位姿到達(dá)所述第二終止位姿的第二路徑以及所述第三起始位姿到達(dá)所述第三終止位姿的第三路徑首尾相連，得到所述零部件的全局拆卸路徑。其中，所述對(duì)所述第一求解階段的路徑進(jìn)行求解的過(guò)程包括將所述第一起始位姿設(shè)置為樹(shù)的根結(jié)點(diǎn)；隨機(jī)產(chǎn)生一第一空間位姿，所述第一空間位姿對(duì)應(yīng)第一結(jié)點(diǎn)；
獲得樹(shù)中離所述第一結(jié)點(diǎn)最近的第二結(jié)點(diǎn)；將所述零部件沿所述第二結(jié)點(diǎn)到所述第一結(jié)點(diǎn)方向按照預(yù)設(shè)第一步長(zhǎng)運(yùn)動(dòng)，得到運(yùn)動(dòng)所述第一步長(zhǎng)后的新位姿對(duì)應(yīng)的第三結(jié)點(diǎn)；對(duì)所述零部件在所述第三結(jié)點(diǎn)處，以及從所述第二結(jié)點(diǎn)到所述第三結(jié)點(diǎn)的路徑上的關(guān)鍵插值點(diǎn)處進(jìn)行碰撞檢測(cè)；若所述零部件在所述第三結(jié)點(diǎn)處和所述關(guān)鍵插值點(diǎn)處與面片模型有碰撞，則擴(kuò)展所述第二結(jié)點(diǎn)，若擴(kuò)展失敗，則返回隨機(jī)產(chǎn)生一第一空間位姿的步驟重新執(zhí)行，若擴(kuò)展成功，判斷所述零部件與所述面片模型是否不存在面片級(jí)別的碰撞或者所述第三結(jié)點(diǎn)到達(dá)所述第一起始位姿對(duì)應(yīng)的根結(jié)點(diǎn)的距離是否達(dá)到預(yù)設(shè)的第一局部拆卸路徑長(zhǎng)度，若是，終止所述第一求解階段的求解，否則返回隨機(jī)產(chǎn)生一第一空間位姿的步驟重新執(zhí)行；若無(wú)碰撞， 則連接所述第二結(jié)點(diǎn)和所述第三結(jié)點(diǎn)所形成的路徑作為所述第一路徑。其中，所述碰撞檢測(cè)的方法包括獲得一測(cè)試射線；將所述測(cè)試射線分別與所述零部件以及與所述零部件進(jìn)行碰撞測(cè)試的面片模型進(jìn)行相交測(cè)試；獲得所述測(cè)試射線同時(shí)位于所述零部件以及所述面片模型中的線段；多次獲得不同的測(cè)試射線分別對(duì)所述零部件以及所述面片模型進(jìn)行相交測(cè)試，獲得多個(gè)線段；若所述多個(gè)線段中長(zhǎng)度大于預(yù)設(shè)長(zhǎng)度閾值的線段個(gè)數(shù)與所述多個(gè)線段的總數(shù)的比值大于預(yù)設(shè)比率閾值，則認(rèn)為所述零部件與所述面片模型產(chǎn)生了碰撞，否則認(rèn)為無(wú)碰撞。其中，所述擴(kuò)展所述第二結(jié)點(diǎn)的步驟包括查找所述樹(shù)中所述第二結(jié)點(diǎn)下最遠(yuǎn)的葉結(jié)點(diǎn)為第四結(jié)點(diǎn)；依次獲得所述葉結(jié)點(diǎn)的第一父輩結(jié)點(diǎn)，第二父輩結(jié)點(diǎn)，直到所獲得的父輩結(jié)點(diǎn)的數(shù)量達(dá)到一預(yù)設(shè)值或者獲得了所述樹(shù)的根結(jié)點(diǎn)；獲得所述預(yù)設(shè)值數(shù)量的所述父輩結(jié)點(diǎn)的平均位姿結(jié)點(diǎn)，或者從所述葉結(jié)點(diǎn)到所述樹(shù)的根結(jié)點(diǎn)之間的包括所述第一父輩結(jié)點(diǎn)、第二父輩結(jié)點(diǎn)的多個(gè)父輩結(jié)點(diǎn)的平均位姿結(jié)用所述第四結(jié)點(diǎn)代替所述第二結(jié)點(diǎn)，并確定新的方向?yàn)閺乃銎骄蛔私Y(jié)點(diǎn)到所述第四結(jié)點(diǎn)。其中，所述對(duì)所述第二求解階段的路徑進(jìn)行求解的過(guò)程包括將所述第二起始位姿設(shè)置為樹(shù)的根結(jié)點(diǎn)；隨機(jī)產(chǎn)生一第二空間位姿，所述第二空間位姿對(duì)應(yīng)第五結(jié)點(diǎn)；獲得樹(shù)中離所述第五結(jié)點(diǎn)最近的第六結(jié)點(diǎn)；在區(qū)間
獲取隨機(jī)數(shù)r，將所述隨機(jī)數(shù)r與所述第六結(jié)點(diǎn)上的擴(kuò)展概率ρ比較，如果r < p，則對(duì)所述第六結(jié)點(diǎn)進(jìn)行擴(kuò)展；如果r > = p，則令所述第六結(jié)點(diǎn)下最遠(yuǎn)的葉結(jié)點(diǎn)取代所述第六結(jié)點(diǎn)進(jìn)行擴(kuò)展，若擴(kuò)展失敗，返回隨機(jī)產(chǎn)生一第二空間位姿的步驟重新執(zhí)行，若擴(kuò)展成功，判斷新擴(kuò)展得到的所述第六結(jié)點(diǎn)到達(dá)所述第一起始位姿對(duì)應(yīng)的根結(jié)點(diǎn)的距離是否達(dá)到預(yù)設(shè)的第二局部拆卸路徑長(zhǎng)度，若達(dá)到，終止所述第一求解階段，否則返回隨機(jī)產(chǎn)生一第二空間位姿的步驟重新執(zhí)行。
其中，對(duì)所述六結(jié)點(diǎn)進(jìn)行擴(kuò)展的過(guò)程包括將所述零部件沿所述第六結(jié)點(diǎn)到所述第五結(jié)點(diǎn)的方向按照一定步長(zhǎng)運(yùn)動(dòng)，計(jì)算運(yùn)動(dòng)該步長(zhǎng)后的新位姿結(jié)點(diǎn)；對(duì)所述零部件在所述新位姿結(jié)點(diǎn)處及從所述第六結(jié)點(diǎn)到所述新位姿結(jié)點(diǎn)的路徑的關(guān)鍵插值點(diǎn)處進(jìn)行碰撞檢測(cè)，如果無(wú)碰撞，則連接所述第六結(jié)點(diǎn)和所述新位姿結(jié)點(diǎn)；如果存在碰撞，將擴(kuò)展失敗結(jié)點(diǎn)的擴(kuò)展概率減小，并確定一擴(kuò)展方式，按照選取的所述擴(kuò)展方式計(jì)算所述新位姿結(jié)點(diǎn)，將所述第六結(jié)點(diǎn)向所述新位姿結(jié)點(diǎn)進(jìn)行擴(kuò)展。其中，所述擴(kuò)展方式包括向采樣點(diǎn)方向擴(kuò)展，向該結(jié)點(diǎn)的父親結(jié)點(diǎn)到該結(jié)點(diǎn)的方向擴(kuò)展，隨機(jī)選擇方向擴(kuò)展和/或沿空間一維度方向擴(kuò)展。其中，所述葉結(jié)點(diǎn)的擴(kuò)展權(quán)值保持為1。其中，所述對(duì)所述第三求解階段的路徑進(jìn)行求解的步驟具體為采用雙向的快速擴(kuò)展隨機(jī)樹(shù)算法對(duì)所述第三求解階段的路徑進(jìn)行求解。其中，上述方法還包括根據(jù)設(shè)定的全局路徑步長(zhǎng)，對(duì)所述全局拆卸路徑中的路徑點(diǎn)序列進(jìn)行插值和順滑。其中，上述方法還包括對(duì)所述全局拆卸路徑上的路徑點(diǎn)序列反序，得到所述零部件的裝配路徑。本發(fā)明的實(shí)施例還提供一種產(chǎn)品零部件的拆卸路徑求解裝置，包括設(shè)置模塊，用于設(shè)置產(chǎn)品零部件的已裝配位姿；第一求解模塊，用于將所述已裝配位姿作為第一求解階段的第一起始位姿，對(duì)所述第一求解階段的路徑進(jìn)行求解，得到所述產(chǎn)品零部件在所述第一求解階段的第一終止位姿；第二求解模塊，用于將所述第一終止位姿作為第二求解階段的第二起始位姿，對(duì)所述第二求解階段的路徑進(jìn)行求解，得到所述產(chǎn)品零部件在所述第二求解階段的第二終止位姿；第三求解模塊，用于將所述第二終止位姿作為第三求解階段的第三起始位姿，對(duì)所述第三求解階段的路徑進(jìn)行求解，得到所述產(chǎn)品零部件在所述第三求解階段的第三終止位姿；全局拆卸路徑生成模塊，用于將所述第一起始位姿到達(dá)所述第一終止位姿的第一路徑，所述第二起始位姿到達(dá)所述第二終止位姿的第二路徑以及所述第三起始位姿到達(dá)所述第三終止位姿的第三路徑首尾相連，得到所述零部件的全局拆卸路徑。本發(fā)明的上述技術(shù)方案的有益效果如下上述方案中，將全局拆卸路徑的求解分解為多個(gè)階段(包括上述的第一求解階段，第二求解階段和第三求解階段)進(jìn)行，對(duì)每個(gè)階段使用適應(yīng)其特征的求解算法，能夠穩(wěn)定快速的對(duì)復(fù)雜環(huán)境中的拆卸/裝配路徑進(jìn)行求解，且該方法是一個(gè)通用、可行的復(fù)雜環(huán)境中產(chǎn)品拆卸/裝配路徑求解方法，以產(chǎn)品的數(shù)字化幾何模型為基礎(chǔ)，通過(guò)相應(yīng)算法，利用計(jì)算機(jī)對(duì)產(chǎn)品拆卸/裝配路徑進(jìn)行求解，可以穩(wěn)定快速的求解環(huán)境未知且?guī)в歇M窄通道、 大量障礙物等復(fù)雜環(huán)境中的拆卸/裝配路徑。


圖1為本發(fā)明的產(chǎn)品零部件的拆卸路徑求解方法流程示意圖；圖2為圖1所示方法中，零部件從已裝配位姿開(kāi)始經(jīng)過(guò)第一路徑、第二路徑以及第三路徑，得到全局路徑的過(guò)程示意圖；圖3為本發(fā)明的方法的第一求解階段過(guò)程中采用的基于歷史引導(dǎo)的快速擴(kuò)展隨機(jī)樹(shù)算法的擴(kuò)展過(guò)程示意圖；圖4為本發(fā)明的方法的第一求解階段過(guò)程中采用的碰撞算法示意圖；圖5為圖4所示的碰撞算法中多次進(jìn)行射線檢測(cè)的示意圖；圖6為本發(fā)明的方法的第二求解階段過(guò)程中采用的自適應(yīng)的快速擴(kuò)展隨機(jī)樹(shù)算法中的結(jié)點(diǎn)擴(kuò)展方式中向采樣點(diǎn)方向擴(kuò)展的示意圖；圖7為本發(fā)明的方法的第二求解階段過(guò)程中采用的自適應(yīng)的快速擴(kuò)展隨機(jī)樹(shù)算法中的結(jié)點(diǎn)擴(kuò)展方式中向該結(jié)點(diǎn)的父結(jié)點(diǎn)到該結(jié)點(diǎn)的擴(kuò)展方向擴(kuò)展的示意圖；圖8為本發(fā)明的方法的第二求解階段過(guò)程中采用的自適應(yīng)的快速擴(kuò)展隨機(jī)樹(shù)算法中的結(jié)點(diǎn)擴(kuò)展方式中隨機(jī)選擇方向擴(kuò)展的示意圖；圖9為本發(fā)明的方法的第二求解階段過(guò)程中采用的自適應(yīng)的快速擴(kuò)展隨機(jī)樹(shù)算法中的結(jié)點(diǎn)擴(kuò)展方式中沿空間某一維度方向擴(kuò)展的示意圖。
具體實(shí)施例方式為使本發(fā)明要解決的技術(shù)問(wèn)題、技術(shù)方案和優(yōu)點(diǎn)更加清楚，下面將結(jié)合附圖及具體實(shí)施例進(jìn)行詳細(xì)描述。如圖1、圖2所示，本發(fā)明的實(shí)施例一種產(chǎn)品零部件的拆卸路徑求解方法，包括步驟11，設(shè)置產(chǎn)品零部件的已裝配位姿；步驟12，將所述已裝配位姿作為第一求解階段的第一起始位姿，對(duì)所述第一求解階段的路徑進(jìn)行求解，得到所述產(chǎn)品零部件在所述第一求解階段的第一終止位姿；步驟13，將所述第一終止位姿作為第二求解階段的第二起始位姿，對(duì)所述第二求解階段的路徑進(jìn)行求解，得到所述產(chǎn)品零部件在所述第二求解階段的第二終止位姿；步驟14，將所述第二終止位姿作為第三求解階段的第三起始位姿，對(duì)所述第三求解階段的路徑進(jìn)行求解，得到所述產(chǎn)品零部件在所述第三求解階段的第三終止位姿；步驟15，將所述第一起始位姿到達(dá)所述第一終止位姿的第一路徑，所述第二起始位姿到達(dá)所述第二終止位姿的第二路徑以及所述第三起始位姿到達(dá)所述第三終止位姿的第三路徑首尾相連，得到所述零部件的全局拆卸路徑(如圖2所示，零部件21經(jīng)過(guò)第一路徑a，第二路徑b以及第三路徑c，路徑a，b，c首尾相連，得到零部件的全局拆卸路徑)。該實(shí)施例主要用于在復(fù)雜環(huán)境中對(duì)產(chǎn)品零部件裝配過(guò)程中，產(chǎn)品零部件的拆卸/ 裝配路徑的求解，復(fù)雜環(huán)境是指由于產(chǎn)品結(jié)構(gòu)復(fù)雜、零部件數(shù)量繁多、工作環(huán)境具有不確定性等而造成拆卸/裝配路徑求解的障礙物環(huán)境可能具有狹窄通道、密集障礙物、場(chǎng)景易變動(dòng)等難題，已有的算法和工具求解效率低或無(wú)法解決，本發(fā)明的該實(shí)施例將全局拆卸路徑的求解分解為多個(gè)階段(包括上述的第一求解階段，第二求解階段和第三求解階段)進(jìn)行， 對(duì)每個(gè)階段使用適應(yīng)其特征的求解算法，能夠穩(wěn)定快速的對(duì)復(fù)雜環(huán)境中的拆卸/裝配路徑進(jìn)行求解，且該方法是一個(gè)通用、可行的復(fù)雜環(huán)境中產(chǎn)品拆卸/裝配路徑求解方法，以產(chǎn)品的數(shù)字化幾何模型為基礎(chǔ)，通過(guò)相應(yīng)算法，利用計(jì)算機(jī)對(duì)產(chǎn)品拆卸/裝配路徑進(jìn)行求解，可以穩(wěn)定快速的求解環(huán)境未知且?guī)в歇M窄通道、大量障礙物等復(fù)雜環(huán)境中的拆卸/裝配路徑。在本發(fā)明的另一實(shí)施例中，對(duì)所述第一求解階段的路徑進(jìn)行求解的過(guò)程采用本發(fā)明提出的基于歷史引導(dǎo)的快速擴(kuò)展隨機(jī)樹(shù)算法，其中，該算法中的物體碰撞檢測(cè)采用基于隨機(jī)射線測(cè)試的碰撞檢測(cè)與評(píng)估方法，其中，基于歷史引導(dǎo)的快速擴(kuò)展隨機(jī)樹(shù)算法包括將所述第一起始位姿設(shè)置為樹(shù)的根結(jié)點(diǎn)；隨機(jī)產(chǎn)生一第一空間位姿，所述第一空間位姿對(duì)應(yīng)第一結(jié)點(diǎn)；獲得樹(shù)中離所述第一結(jié)點(diǎn)最近的第二結(jié)點(diǎn)；將所述零部件沿所述第二結(jié)點(diǎn)到所述第一結(jié)點(diǎn)方向按照預(yù)設(shè)第一步長(zhǎng)運(yùn)動(dòng)，得到運(yùn)動(dòng)所述第一步長(zhǎng)后的新位姿對(duì)應(yīng)的第三結(jié)點(diǎn)；對(duì)所述零部件在所述第三結(jié)點(diǎn)處，以及從所述第二結(jié)點(diǎn)到所述第三結(jié)點(diǎn)的路徑上的關(guān)鍵插值點(diǎn)處進(jìn)行碰撞檢測(cè)；若所述零部件在所述第三結(jié)點(diǎn)處和所述關(guān)鍵插值點(diǎn)處與面片模型有碰撞(其中， 這里的面片模型可以包括當(dāng)前裝配環(huán)境中其他所有零部件和環(huán)境模型的面片模型，其中， 其他零部件在被作為障礙物時(shí)，也可以被稱為面片模型)，則擴(kuò)展所述第二結(jié)點(diǎn)，若擴(kuò)展失敗，則返回隨機(jī)產(chǎn)生一第一空間位姿的步驟重新執(zhí)行，若擴(kuò)展成功，判斷所述零部件與所述面片模型是否不存在面片級(jí)別的碰撞或者第三結(jié)點(diǎn)到達(dá)所述第一起始位姿對(duì)應(yīng)的根結(jié)點(diǎn)的距離是否達(dá)到預(yù)設(shè)的第一局部拆卸路徑長(zhǎng)度，若是，終止所述第一求解階段的求解，否則返回隨機(jī)產(chǎn)生一第一空間位姿的步驟重新執(zhí)行；若無(wú)碰撞，則連接所述第二結(jié)點(diǎn)和所述第三結(jié)點(diǎn)所形成的路徑作為所述第一路徑。其中，所述擴(kuò)展所述第二結(jié)點(diǎn)的步驟包括查找所述樹(shù)中所述第二結(jié)點(diǎn)下最遠(yuǎn)的葉結(jié)點(diǎn)為第四結(jié)點(diǎn)；依次獲得所述葉結(jié)點(diǎn)的第一父輩結(jié)點(diǎn)，第二父輩結(jié)點(diǎn)，直到所獲得的父輩結(jié)點(diǎn)數(shù)量達(dá)到一預(yù)設(shè)值N(如N = 8)或者獲得了所述樹(shù)的根結(jié)點(diǎn)；獲得所述預(yù)設(shè)值數(shù)量的所述父輩結(jié)點(diǎn)的平均位姿結(jié)點(diǎn)，或者從所述葉結(jié)點(diǎn)到所述樹(shù)的根結(jié)點(diǎn)之間的包括所述第一父輩結(jié)點(diǎn)、所述第二父輩結(jié)點(diǎn)的多個(gè)父輩結(jié)點(diǎn)的平均位姿用所述第四結(jié)點(diǎn)代替所述第二結(jié)點(diǎn)，并確定新的方向?yàn)閺乃銎骄蛔私Y(jié)點(diǎn)到所述第四結(jié)點(diǎn)。具體實(shí)現(xiàn)過(guò)程可參考如下步驟21，將待求解路徑(上述第一求解階段)的起始點(diǎn)設(shè)置為樹(shù)的根結(jié)點(diǎn)；步驟22，隨機(jī)產(chǎn)生一個(gè)空間位姿QMnd(上述第一空間位姿)；步驟23，獲得樹(shù)中離隨機(jī)空間位姿最近的結(jié)點(diǎn)qnea,(上述第二結(jié)點(diǎn))；步驟M，沿qn_到qMnd方向按照一定步長(zhǎng)(其中該步長(zhǎng)可預(yù)先設(shè)定)運(yùn)動(dòng)，計(jì)算運(yùn)動(dòng)該步長(zhǎng)后的新位姿qnOT(上述第三結(jié)點(diǎn))；步驟25，對(duì)零部件在新位姿qnew處及qnea,到qnew的路徑關(guān)鍵插值點(diǎn)處進(jìn)行碰撞檢測(cè)，如果無(wú)碰撞，則連接qnea,和qnew ；如果存在碰撞，則擴(kuò)展qnea,，執(zhí)行以下步驟251-254，返回最終結(jié)果；
步驟251，尋找樹(shù)中qnear結(jié)點(diǎn)下最遠(yuǎn)的葉結(jié)點(diǎn)qfr。nt (上述第四結(jié)點(diǎn))；步驟252，依次尋找葉結(jié)點(diǎn)的N個(gè)父輩結(jié)點(diǎn)，直到所獲得的父輩結(jié)點(diǎn)數(shù)量達(dá)到一預(yù)設(shè)值N(如N = 8)或者獲得了所述樹(shù)的根結(jié)點(diǎn)；步驟253，計(jì)算N個(gè)父輩結(jié)點(diǎn)的平均位姿信息qavCT(平均位姿結(jié)點(diǎn))或者從所述葉結(jié)點(diǎn)到所述樹(shù)的根結(jié)點(diǎn)之間的包括所述第一父輩結(jié)點(diǎn)、第二父輩結(jié)點(diǎn)的多個(gè)父輩結(jié)點(diǎn)的平均位姿結(jié)點(diǎn)qavCT ；步驟254，確定新的擴(kuò)展結(jié)點(diǎn)為qft。nt，代替qnea,；新的擴(kuò)展方向應(yīng)為從qavCT到 Qfront ；執(zhí)行步驟24-25 ；如圖3所示，qdi,在圖中用于展示從結(jié)點(diǎn)qfront以向量(qfrant_qavJ為
方向進(jìn)行擴(kuò)展，qdil"q&。nt = Qfront-Qaver ；步驟沈，如果步驟25的結(jié)果為擴(kuò)展失敗，則返回步驟22 ；如果擴(kuò)展成功，則判斷新位姿qnew到達(dá)起始點(diǎn)(上述第一起始位姿)的距離是否超過(guò)預(yù)設(shè)的第一局部拆卸路徑長(zhǎng)度或者零部件與所述面片模型是否完全不存在面片級(jí)別的碰撞，如果是，則停止計(jì)算；如果否，則返回步驟22。在上述基于歷史引導(dǎo)的快速擴(kuò)展隨機(jī)樹(shù)算法中，碰撞檢測(cè)算法采用基于隨機(jī)射線測(cè)試的碰撞檢測(cè)與評(píng)估方法，該方法的實(shí)現(xiàn)過(guò)程包括獲得一測(cè)試射線；將所述測(cè)試射線分別與所述零部件以及與所述零部件進(jìn)行碰撞測(cè)試的面片模型進(jìn)行相交測(cè)試；獲得所述測(cè)試射線同時(shí)位于所述零部件以及所述面片模型中的線段；多次獲得不同的測(cè)試射線分別對(duì)所述零部件以及所述面片模型進(jìn)行相交測(cè)試，獲得多個(gè)線段；若所述多個(gè)線段中長(zhǎng)度大于預(yù)設(shè)長(zhǎng)度閾值的線段個(gè)數(shù)與所述多個(gè)線段的總數(shù)的比值大于預(yù)設(shè)比率閾值，則認(rèn)為所述零部件與所述面片模型產(chǎn)生了碰撞，否則認(rèn)為無(wú)碰撞。如圖4和圖5所示，該基于隨機(jī)射線測(cè)試的碰撞檢測(cè)與評(píng)估方法的具體實(shí)現(xiàn)過(guò)程可參考如下若利用該方法對(duì)物體A和物體B的碰撞情況進(jìn)行檢測(cè)和評(píng)估，默認(rèn)已獲得A和B 的面片模型(其中，該面片模型是指路徑求解過(guò)程中所涉及的產(chǎn)品零部件、工夾具、工作環(huán)境等，其中工作環(huán)境包括工作臺(tái)、車間等)，這里的產(chǎn)品零部件、工夾具和工作環(huán)境等都可以稱為幾何面片模型(一般為無(wú)序的三角面片模型)的前提下，算法描述如下步驟31，設(shè)定長(zhǎng)度閾值t > 0，設(shè)定比率閾值0 < ρ < 1 ；步驟32，在空間中定義一條隨機(jī)射線O1D1 ；步驟33，將射線分別與物體A和物體B做相交測(cè)試，測(cè)試中遍歷A和B的所有面片， 并按順序記錄射線與物體的交點(diǎn)，分別為集合Xm = IA1, A2, A3. . . }，Xob = (B1, B2, B3. . . }； 如果射線與物體A無(wú)交點(diǎn)，或與物體B無(wú)交點(diǎn)，則返回步驟32 ；如圖4所示；步驟34，分別獲取射線位于物體A和物體B內(nèi)部的線段，其中，位于物體A 中的線段如A1A2，A2A3以及A3A4，位于物體B內(nèi)部的線段如B1B2,若集合Xm元素個(gè)數(shù)為偶數(shù)，則有 SoA={A)A)+\\A),A) + IeJTcw,/= 1,3,5^(^0^-1)}，若為奇數(shù)個(gè)，則有
L={^TT|4，4+leX。W = 2，4，6，...，(|Xm|-l)}。同樣的方式定義
步驟35，遍歷 S0JP S。B，&仙={1 = 1^。!^ I VZa4 GScw，VZos e Sos},即同時(shí)位于物體A和物體B內(nèi)部的線段即位于A和B的交集中，即線段B1A3和A3B2 ；步驟36，將射線按照光線反射規(guī)則在A和B的交集內(nèi)進(jìn)行反射，定義新的射線或者重新定義隨機(jī)射線(如圖5中所示的多個(gè)箭頭所示的隨機(jī)射線)；重復(fù)步驟33-36N次，記錄檢測(cè)結(jié)果于S-中；步驟37，分析和評(píng)估檢測(cè)結(jié)果，如果S-中長(zhǎng)度大于t的元素?cái)?shù)比率大于p，則認(rèn)為A和B產(chǎn)生了深度碰撞；否則認(rèn)為產(chǎn)生了淺碰撞或無(wú)碰撞。在本發(fā)明的另一實(shí)施例中，對(duì)所述第二求解階段的路徑進(jìn)行求解的過(guò)程采用本發(fā)明提出的自適應(yīng)的快速擴(kuò)展隨機(jī)樹(shù)算法，其中，該算法中的物體碰撞檢測(cè)采用傳統(tǒng)的碰撞檢測(cè)方法，其中，自適應(yīng)的快速擴(kuò)展隨機(jī)樹(shù)算法包括將所述第二起始位姿設(shè)置為樹(shù)的根結(jié)點(diǎn)；隨機(jī)產(chǎn)生一第二空間位姿，所述第二空間位姿對(duì)應(yīng)第五結(jié)點(diǎn)；獲得樹(shù)中離所述第五結(jié)點(diǎn)最近的第六結(jié)點(diǎn)；在區(qū)間
獲取隨機(jī)數(shù)r，將所述隨機(jī)數(shù)r與所述第六結(jié)點(diǎn)上的擴(kuò)展概率ρ比較，如果r < p，則對(duì)所述第六結(jié)點(diǎn)進(jìn)行擴(kuò)展；如果r > = p，則令所述第六結(jié)點(diǎn)下最遠(yuǎn)的葉結(jié)點(diǎn)取代所述第六結(jié)點(diǎn)進(jìn)行擴(kuò)展，若擴(kuò)展失敗，返回隨機(jī)產(chǎn)生一第二空間位姿的步驟重新執(zhí)行，若擴(kuò)展成功，判斷新擴(kuò)展得到的所述第六結(jié)點(diǎn)到達(dá)所述第一起始位姿對(duì)應(yīng)的根結(jié)點(diǎn)的距離是否達(dá)到預(yù)設(shè)的第二局部拆卸路徑長(zhǎng)度，若達(dá)到，終止所述第一求解階段，否則返回隨機(jī)產(chǎn)生一第二空間位姿的步驟重新執(zhí)行。其中，這里的預(yù)設(shè)的第二局部拆卸路徑長(zhǎng)度與上述預(yù)設(shè)的第一局部拆卸路徑長(zhǎng)度可以相同，也可以不相同。其中，對(duì)所述六結(jié)點(diǎn)進(jìn)行擴(kuò)展的過(guò)程包括將所述零部件沿所述第六結(jié)點(diǎn)到所述第五結(jié)點(diǎn)的方向按照一定步長(zhǎng)運(yùn)動(dòng)，計(jì)算運(yùn)動(dòng)該步長(zhǎng)后的新位姿結(jié)點(diǎn)；對(duì)所述零部件在所述新位姿結(jié)點(diǎn)處及從所述第六結(jié)點(diǎn)到所述新位姿結(jié)點(diǎn)的路徑的關(guān)鍵插值點(diǎn)處進(jìn)行碰撞檢測(cè)，如果無(wú)碰撞，則連接所述第六結(jié)點(diǎn)和所述新位姿結(jié)點(diǎn)；如果存在碰撞，將擴(kuò)展失敗結(jié)點(diǎn)的擴(kuò)展概率減小，并確定一擴(kuò)展方式，按照選取的所述擴(kuò)展方式計(jì)算所述新位姿結(jié)點(diǎn)，將所述第六結(jié)點(diǎn)向所述新位姿結(jié)點(diǎn)進(jìn)行擴(kuò)展。該算法中，所述擴(kuò)展方式可以包括向采樣點(diǎn)方向擴(kuò)展，向該結(jié)點(diǎn)的父親結(jié)點(diǎn)到該結(jié)點(diǎn)的方向擴(kuò)展，隨機(jī)選擇方向擴(kuò)展和/或沿空間一維度方向擴(kuò)展。優(yōu)選的，該算法中的所述葉結(jié)點(diǎn)的擴(kuò)展權(quán)值保持為1。該自適應(yīng)快速擴(kuò)展隨機(jī)樹(shù)算法的具體實(shí)現(xiàn)過(guò)程可參考如下該算法中所涉及的四種結(jié)點(diǎn)擴(kuò)展方式包括向采樣點(diǎn)方向擴(kuò)展；向該結(jié)點(diǎn)的父結(jié)點(diǎn)到該結(jié)點(diǎn)的擴(kuò)展方向擴(kuò)展；隨機(jī)選擇方向擴(kuò)展；沿空間某一維度方向擴(kuò)展，分別如圖6、 7、8、9所示。算法流程描述如下步驟41，將待求解路徑的起始點(diǎn)設(shè)置為樹(shù)的根結(jié)點(diǎn)，至多重復(fù)K次執(zhí)行以下步驟42，產(chǎn)生一個(gè)受終點(diǎn)引導(dǎo)的隨機(jī)空間位姿QMnd(即上述第二空間位姿)；步驟43，獲得樹(shù)中離隨機(jī)空間位姿最近的結(jié)點(diǎn)qnea,(即上述第六結(jié)點(diǎn))；步驟44，在區(qū)間
獲取隨機(jī)數(shù)r，與qn_上的擴(kuò)展概率ρ比較，如果r <p，則對(duì)qnea,進(jìn)行擴(kuò)展；如果r > = p，則令qnea,下最遠(yuǎn)的葉結(jié)點(diǎn)qfrant取代qnea,進(jìn)行擴(kuò)展步驟45，沿qnea,到q—方向按照一定步長(zhǎng)運(yùn)動(dòng)，計(jì)算運(yùn)動(dòng)該步長(zhǎng)后的新位姿qnew ；步驟46，對(duì)物體在新位姿qnew處及qnea,到qnew的路徑關(guān)鍵插值點(diǎn)處進(jìn)行碰撞檢測(cè)， 如果無(wú)碰撞，則連接qn_和；如果存在碰撞，將擴(kuò)展失敗結(jié)點(diǎn)的擴(kuò)展概率減小，并至多重復(fù)N次執(zhí)行以下；a，選取擴(kuò)展方式；b，按照擴(kuò)展方式計(jì)算qnew ；C，將 qne 向 qnew 擴(kuò)展；d，如果成功，返回結(jié)果；如果失敗，返回到a;步驟47，如果步驟44的結(jié)果為擴(kuò)展失敗，則返回步驟42;如果擴(kuò)展成功，則判斷新結(jié)點(diǎn)是否靠近終點(diǎn)小于閾值，如果否，則返回步驟42，如果是，則停止計(jì)算。其中，該擴(kuò)展過(guò)程中，在每個(gè)節(jié)點(diǎn)上賦權(quán)值，通過(guò)設(shè)定權(quán)值大小表明該點(diǎn)是否易于擴(kuò)展(也稱擴(kuò)展概率)， 并控制樹(shù)在該節(jié)點(diǎn)生長(zhǎng)的概率；在樹(shù)的擴(kuò)展中，如果某結(jié)點(diǎn)被選為待擴(kuò)展結(jié)點(diǎn)，進(jìn)一步根據(jù)權(quán)值大小決定是否在該節(jié)點(diǎn)進(jìn)行擴(kuò)展；優(yōu)選的，所有葉節(jié)點(diǎn)的擴(kuò)展權(quán)值總是保持為1，保證其擴(kuò)展概率不被減弱；如果在某一非葉節(jié)點(diǎn)擴(kuò)展失敗，則降低該節(jié)點(diǎn)的權(quán)值；如果某節(jié)點(diǎn)多次擴(kuò)展失敗，則設(shè)置其擴(kuò)展權(quán)值為0，關(guān)閉該節(jié)點(diǎn)的擴(kuò)展；在樹(shù)擴(kuò)展遇到障礙物時(shí)，多次嘗試采用不同的擴(kuò)展方式(如在上述a，b，c，d四種擴(kuò)展方式中進(jìn)行嘗試)進(jìn)行擴(kuò)展，強(qiáng)化樹(shù)在障礙物附近的擴(kuò)展；不同的擴(kuò)展方式中，選擇相應(yīng)的擴(kuò)展方向，并以一定步長(zhǎng)擴(kuò)展。在本發(fā)明的另一實(shí)施例中，上述對(duì)所述第三求解階段的路徑進(jìn)行求解的步驟具體為采用傳統(tǒng)的雙向的快速擴(kuò)展隨機(jī)樹(shù)算法對(duì)所述第三求解階段的路徑進(jìn)行求解。進(jìn)一步地，上述圖1所示的方法還可以包括根據(jù)設(shè)定的全局路徑步長(zhǎng)，對(duì)所述全局拆卸路徑中的路徑點(diǎn)序列進(jìn)行插值和順滑。進(jìn)一步地，上述圖1所示的方法還可以包括對(duì)所述全局拆卸路徑上的路徑點(diǎn)序列反序，得到所述零部件的裝配路徑。綜上，本發(fā)明的實(shí)施例提出一種通用、可行的復(fù)雜環(huán)境中產(chǎn)品拆卸/裝配路徑求解方法，以產(chǎn)品的數(shù)字化幾何模型為基礎(chǔ)，通過(guò)相應(yīng)算法，利用計(jì)算機(jī)對(duì)產(chǎn)品拆卸/裝配路徑進(jìn)行求解。本發(fā)明的主要特點(diǎn)和優(yōu)勢(shì)在于(1)創(chuàng)新性的提出了基于歷史引導(dǎo)的快速擴(kuò)展隨機(jī)樹(shù)算法(RRT-H)算法、自適應(yīng)快速擴(kuò)展隨機(jī)樹(shù)算法(RRT-A算法)以及在RRT-H算法中采用的基于隨機(jī)射線測(cè)試的碰撞檢測(cè)與評(píng)估方法(RTSColDet算法)，該RTSColDet算法可以解決傳統(tǒng)碰撞檢測(cè)算法對(duì)具有面接觸的兩物體間的碰撞檢測(cè)偽碰撞問(wèn)題；RRT-H算法可以快速穩(wěn)定求解極狹窄空間中簡(jiǎn)單運(yùn)動(dòng)的路徑；RRT-A算法可以穩(wěn)定快速的求解環(huán)境未知且?guī)в歇M窄通道、大量障礙物等復(fù)雜環(huán)境中的拆卸/裝配路徑。本發(fā)明的方法將路徑求解過(guò)程所涉及的產(chǎn)品零部件、工夾具、工作環(huán)境包括工作臺(tái)、車間等所有的幾何面片模型 (一般為無(wú)序的三角面片模型)輸入路徑求解裝置，并根據(jù)其位姿信息定義每個(gè)模型在空間中的位置和姿態(tài)；設(shè)定要進(jìn)行路徑求解的零部件，設(shè)置其已裝配位姿和未裝配位姿，則待求路徑為已裝配位姿到未裝配位姿的路徑，其余所有模型則作為拆卸/裝配過(guò)程中的障礙物對(duì)待；設(shè)置全局路徑的步長(zhǎng)；分別設(shè)置RRT-H/RRT-A算法中的路徑步長(zhǎng)；設(shè)置RTSColDet 的碰撞深度閾值；接著進(jìn)行路徑求解過(guò)程，該路徑求解過(guò)程求解所指定零部件的拆卸路徑，分三個(gè)階段實(shí)現(xiàn)，第一求解階段、第二求解階段以及第三求解階段，如任一階段計(jì)算失敗， 則退出該次求解；首先使用RRT-H算法進(jìn)行第一路徑的求解，其中碰撞檢測(cè)利用RTSColDet 算法實(shí)現(xiàn)；該第一求解階段中設(shè)置RRT-H的起始位姿狀態(tài)為全局路徑中的零部件裝配位姿，不設(shè)定RRT-H的終止位姿狀態(tài)；當(dāng)RTSColDet碰撞檢測(cè)結(jié)果返回的碰撞面片數(shù)為O時(shí)， 用傳統(tǒng)碰撞檢測(cè)算法再次檢測(cè)，如無(wú)碰撞，則終止該階段的路徑求解；否則判斷新擴(kuò)展的結(jié)點(diǎn)到達(dá)所述第一階段樹(shù)的根結(jié)點(diǎn)的距離是否達(dá)到預(yù)設(shè)的第一局部拆卸路徑長(zhǎng)度，如果是， 則終止該階段的路徑求解；該第二求解階段使用RRT-A進(jìn)行路徑求解，使用傳統(tǒng)的碰撞檢測(cè)算法進(jìn)行碰撞檢測(cè)；設(shè)定上一階段(即第一求解階段)求解到的路徑終止位姿設(shè)定為 RRT-A的起始位姿，當(dāng)RRT-A生成的樹(shù)中某一結(jié)點(diǎn)距離全局路徑起始點(diǎn)達(dá)到設(shè)定的距離，則終止該第二求解階段的求解；該第三求解階段使用RRT-ExtCon(雙向的快速擴(kuò)展隨機(jī)樹(shù)算法)進(jìn)行路徑求解，使用傳統(tǒng)的碰撞檢測(cè)算法進(jìn)行碰撞檢測(cè)；設(shè)定上一階段(即上述第二求解階段)求解到的路徑終止位姿設(shè)定為RRT-ExtCon的起始位姿，全局路徑的終止位姿為RRT-ExtCon的終止位姿，當(dāng)求解出該起始位姿到終止位姿的路徑后終止路徑求解；將上述三個(gè)求解階段所求解的三段路徑依照順序首尾連接，則獲得從零部件裝配位姿到未裝配位姿的全局拆卸路徑，實(shí)質(zhì)上是一系列的有序位姿路徑點(diǎn)；根據(jù)需求，如需要裝配路徑，則將該路徑點(diǎn)序列反序；根據(jù)設(shè)定的全局路徑步長(zhǎng)，對(duì)路徑點(diǎn)序列進(jìn)行插值(優(yōu)選為線性插值)，以獲得更精細(xì)順滑的全局拆卸路徑。在本發(fā)明的另一方面，本發(fā)明的實(shí)施例還提供一種與上述方法相應(yīng)的產(chǎn)品零部件的拆卸路徑求解裝置(也可以稱為求解器)，包括設(shè)置模塊，用于設(shè)置產(chǎn)品零部件的已裝配位姿；第一求解模塊，用于將所述已裝配位姿作為第一求解階段的第一起始位姿，對(duì)所述第一求解階段的路徑進(jìn)行求解，得到所述產(chǎn)品零部件在所述第一求解階段的第一終止位姿；第二求解模塊，用于將所述第一終止位姿作為第二求解階段的第二起始位姿，對(duì)所述第二求解階段的路徑進(jìn)行求解，得到所述產(chǎn)品零部件在所述第二求解階段的第二終止位姿；第三求解模塊，用于將所述第二終止位姿作為第三求解階段的第三起始位姿，對(duì)所述第三求解階段的路徑進(jìn)行求解，得到所述產(chǎn)品零部件在所述第三求解階段的第三終止位姿；全局拆卸路徑生成模塊，用于將所述第一起始位姿到達(dá)所述第一終止位姿的第一路徑，所述第二起始位姿到達(dá)所述第二終止位姿的第二路徑以及所述第三起始位姿到達(dá)所述第三終止位姿的第三路徑首尾相連，得到所述零部件的全局拆卸路徑。上述方法中所有實(shí)現(xiàn)手段和應(yīng)用場(chǎng)景均適用于該裝置的實(shí)施例中，也能達(dá)到相同的技術(shù)效果，在此不再贅述。以上所述是本發(fā)明的優(yōu)選實(shí)施方式，應(yīng)當(dāng)指出，對(duì)于本技術(shù)領(lǐng)域的普通技術(shù)人員來(lái)說(shuō)，在不脫離本發(fā)明所述原理的前提下，還可以作出若干改進(jìn)和潤(rùn)飾，這些改進(jìn)和潤(rùn)飾也應(yīng)視為本發(fā)明的保護(hù)范圍。
權(quán)利要求
1.一種產(chǎn)品零部件的拆卸路徑求解方法，其特征在于，包括 設(shè)置產(chǎn)品零部件的已裝配位姿；將所述已裝配位姿作為第一求解階段的第一起始位姿，對(duì)所述第一求解階段的路徑進(jìn)行求解，得到所述產(chǎn)品零部件在所述第一求解階段的第一終止位姿；將所述第一終止位姿作為第二求解階段的第二起始位姿，對(duì)所述第二求解階段的路徑進(jìn)行求解，得到所述產(chǎn)品零部件在所述第二求解階段的第二終止位姿；將所述第二終止位姿作為第三求解階段的第三起始位姿，對(duì)所述第三求解階段的路徑進(jìn)行求解，得到所述產(chǎn)品零部件在所述第三求解階段的第三終止位姿；將所述第一起始位姿到達(dá)所述第一終止位姿的第一路徑，所述第二起始位姿到達(dá)所述第二終止位姿的第二路徑以及所述第三起始位姿到達(dá)所述第三終止位姿的第三路徑首尾相連，得到所述零部件的全局拆卸路徑。
2.根據(jù)權(quán)利要求1所述的產(chǎn)品零部件的拆卸路徑求解方法，其特征在于，所述對(duì)所述第一求解階段的路徑進(jìn)行求解的過(guò)程包括將所述第一起始位姿設(shè)置為樹(shù)的根結(jié)點(diǎn)；隨機(jī)產(chǎn)生一第一空間位姿，所述第一空間位姿對(duì)應(yīng)第一結(jié)點(diǎn)；獲得樹(shù)中離所述第一結(jié)點(diǎn)最近的第二結(jié)點(diǎn)；將所述零部件沿所述第二結(jié)點(diǎn)到所述第一結(jié)點(diǎn)方向按照預(yù)設(shè)第一步長(zhǎng)運(yùn)動(dòng)，得到運(yùn)動(dòng)所述第一步長(zhǎng)后的新位姿對(duì)應(yīng)的第三結(jié)點(diǎn)；對(duì)所述零部件在所述第三結(jié)點(diǎn)處，以及從所述第二結(jié)點(diǎn)到所述第三結(jié)點(diǎn)的路徑上的關(guān)鍵插值點(diǎn)處進(jìn)行碰撞檢測(cè)；若所述零部件在所述第三結(jié)點(diǎn)處和所述關(guān)鍵插值點(diǎn)處與面片模型有碰撞，則擴(kuò)展所述第二結(jié)點(diǎn)，若擴(kuò)展失敗，則返回隨機(jī)產(chǎn)生一第一空間位姿的步驟重新執(zhí)行，若擴(kuò)展成功，判斷所述零部件與所述面片模型是否不存在面片級(jí)別的碰撞或者所述第三結(jié)點(diǎn)到達(dá)所述第一起始位姿對(duì)應(yīng)的根結(jié)點(diǎn)的距離是否達(dá)到預(yù)設(shè)的第一局部拆卸路徑長(zhǎng)度，若是，終止所述第一求解階段的求解，否則返回隨機(jī)產(chǎn)生一第一空間位姿的步驟重新執(zhí)行；若無(wú)碰撞，則連接所述第二結(jié)點(diǎn)和所述第三結(jié)點(diǎn)所形成的路徑作為所述第一路徑。
3.根據(jù)權(quán)利要求2所述的產(chǎn)品零部件的拆卸路徑求解方法，其特征在于，所述碰撞檢測(cè)的方法包括獲得一測(cè)試射線；將所述測(cè)試射線分別與所述零部件以及與所述零部件進(jìn)行碰撞測(cè)試的面片模型進(jìn)行相交測(cè)試；獲得所述測(cè)試射線同時(shí)位于所述零部件以及所述面片模型中的線段； 多次獲得不同的測(cè)試射線分別對(duì)所述零部件以及所述面片模型進(jìn)行相交測(cè)試，獲得多個(gè)線段；若所述多個(gè)線段中長(zhǎng)度大于預(yù)設(shè)長(zhǎng)度閾值的線段個(gè)數(shù)與所述多個(gè)線段的總數(shù)的比值大于預(yù)設(shè)比率閾值，則認(rèn)為所述零部件與所述面片模型產(chǎn)生了碰撞，否則認(rèn)為無(wú)碰撞。
4.根據(jù)權(quán)利要求2所述的產(chǎn)品零部件的拆卸路徑求解方法，其特征在于，所述擴(kuò)展所述第二結(jié)點(diǎn)的步驟包括查找所述樹(shù)中所述第二結(jié)點(diǎn)下最遠(yuǎn)的葉結(jié)點(diǎn)為第四結(jié)點(diǎn)；依次獲得所述葉結(jié)點(diǎn)的第一父輩結(jié)點(diǎn)，第二父輩結(jié)點(diǎn)，直到所獲得的父輩結(jié)點(diǎn)數(shù)量達(dá)到一預(yù)設(shè)值或者獲得了所述樹(shù)的根結(jié)點(diǎn)；獲得所述預(yù)設(shè)值數(shù)量的所述父輩結(jié)點(diǎn)的平均位姿結(jié)點(diǎn)，或者從所述葉結(jié)點(diǎn)到所述樹(shù)的根結(jié)點(diǎn)之間的包括所述第一父輩結(jié)點(diǎn)、第二父輩結(jié)點(diǎn)的多個(gè)父輩結(jié)點(diǎn)的平均位姿結(jié)點(diǎn)；用所述第四結(jié)點(diǎn)代替所述第二結(jié)點(diǎn)，并確定新的方向?yàn)閺乃銎骄蛔私Y(jié)點(diǎn)到所述第四結(jié)點(diǎn)。
5.根據(jù)權(quán)利要求1所述的產(chǎn)品零部件的拆卸路徑求解方法，其特征在于，所述對(duì)所述第二求解階段的路徑進(jìn)行求解的過(guò)程包括將所述第二起始位姿設(shè)置為樹(shù)的根結(jié)點(diǎn)；隨機(jī)產(chǎn)生一第二空間位姿，所述第二空間位姿對(duì)應(yīng)第五結(jié)點(diǎn)；獲得樹(shù)中離所述第五結(jié)點(diǎn)最近的第六結(jié)點(diǎn)；在區(qū)間W，l]獲取隨機(jī)數(shù)r，將所述隨機(jī)數(shù)r與所述第六結(jié)點(diǎn)上的擴(kuò)展概率ρ比較，如果！· < p，則對(duì)所述第六結(jié)點(diǎn)進(jìn)行擴(kuò)展；如果r > = p，則令所述第六結(jié)點(diǎn)下最遠(yuǎn)的葉結(jié)點(diǎn)取代所述第六結(jié)點(diǎn)進(jìn)行擴(kuò)展，若擴(kuò)展失敗，返回隨機(jī)產(chǎn)生一第二空間位姿的步驟重新執(zhí)行，若擴(kuò)展成功，判斷新擴(kuò)展得到的所述第六結(jié)點(diǎn)到達(dá)所述第一起始位姿對(duì)應(yīng)的根結(jié)點(diǎn)的距離是否達(dá)到預(yù)設(shè)的第二局部拆卸路徑長(zhǎng)度，若達(dá)到，終止所述第一求解階段，否則返回隨機(jī)產(chǎn)生一第二空間位姿的步驟重新執(zhí)行。
6.根據(jù)權(quán)利要求5所述的產(chǎn)品零部件的拆卸路徑求解方法，其特征在于，對(duì)所述六結(jié)點(diǎn)進(jìn)行擴(kuò)展的過(guò)程包括將所述零部件沿所述第六結(jié)點(diǎn)到所述第五結(jié)點(diǎn)的方向按照一定步長(zhǎng)運(yùn)動(dòng)，計(jì)算運(yùn)動(dòng)該步長(zhǎng)后的新位姿結(jié)點(diǎn)；對(duì)所述零部件在所述新位姿結(jié)點(diǎn)處及從所述第六結(jié)點(diǎn)到所述新位姿結(jié)點(diǎn)的路徑的關(guān)鍵插值點(diǎn)處進(jìn)行碰撞檢測(cè)，如果無(wú)碰撞，則連接所述第六結(jié)點(diǎn)和所述新位姿結(jié)點(diǎn)；如果存在碰撞，將擴(kuò)展失敗結(jié)點(diǎn)的擴(kuò)展概率減小，并確定一擴(kuò)展方式，按照選取的所述擴(kuò)展方式計(jì)算所述新位姿結(jié)點(diǎn)，將所述第六結(jié)點(diǎn)向所述新位姿結(jié)點(diǎn)進(jìn)行擴(kuò)展。
7.根據(jù)權(quán)利要求6所述的產(chǎn)品零部件的拆卸路徑求解方法，其特征在于，所述擴(kuò)展方式包括向采樣點(diǎn)方向擴(kuò)展，向該結(jié)點(diǎn)的父親結(jié)點(diǎn)到該結(jié)點(diǎn)的方向擴(kuò)展，隨機(jī)選擇方向擴(kuò)展和/或沿空間一維度方向擴(kuò)展。
8.根據(jù)權(quán)利要求6所述的產(chǎn)品零部件的拆卸路徑求解方法，其特征在于，所述葉結(jié)點(diǎn)的擴(kuò)展權(quán)值保持為1。
9.根據(jù)權(quán)利要求1所述的產(chǎn)品零部件的拆卸路徑求解方法，其特征在于，所述對(duì)所述第三求解階段的路徑進(jìn)行求解的步驟具體為采用雙向的快速擴(kuò)展隨機(jī)樹(shù)算法對(duì)所述第三求解階段的路徑進(jìn)行求解。
10.根據(jù)權(quán)利要求1-9任一項(xiàng)所述的產(chǎn)品零部件的拆卸路徑求解方法，其特征在于，還包括根據(jù)設(shè)定的全局路徑步長(zhǎng)，對(duì)所述全局拆卸路徑中的路徑點(diǎn)序列進(jìn)行插值和順滑。
11.根據(jù)權(quán)利要求10所述的產(chǎn)品零部件的拆卸路徑求解方法，其特征在于，還包括對(duì)所述全局拆卸路徑上的路徑點(diǎn)序列反序，得到所述零部件的裝配路徑。
12.—種產(chǎn)品零部件的拆卸路徑求解裝置，其特征在于，包括設(shè)置模塊，用于設(shè)置產(chǎn)品零部件的已裝配位姿；第一求解模塊，用于將所述已裝配位姿作為第一求解階段的第一起始位姿，對(duì)所述第一求解階段的路徑進(jìn)行求解，得到所述產(chǎn)品零部件在所述第一求解階段的第一終止位姿；第二求解模塊，用于將所述第一終止位姿作為第二求解階段的第二起始位姿，對(duì)所述第二求解階段的路徑進(jìn)行求解，得到所述產(chǎn)品零部件在所述第二求解階段的第二終止位姿；第三求解模塊，用于將所述第二終止位姿作為第三求解階段的第三起始位姿，對(duì)所述第三求解階段的路徑進(jìn)行求解，得到所述產(chǎn)品零部件在所述第三求解階段的第三終止位姿；全局拆卸路徑生成模塊，用于將所述第一起始位姿到達(dá)所述第一終止位姿的第一路徑，所述第二起始位姿到達(dá)所述第二終止位姿的第二路徑以及所述第三起始位姿到達(dá)所述第三終止位姿的第三路徑首尾相連，得到所述零部件的全局拆卸路徑。
全文摘要
本發(fā)明提供一種產(chǎn)品零部件的拆卸路徑求解方法及裝置，方法包括將零部件的已裝配位姿作為第一求解階段的第一起始位姿，對(duì)第一求解階段的路徑進(jìn)行求解，得到第一求解階段的第一終止位姿；將第一終止位姿作為第二求解階段的第二起始位姿，對(duì)第二求解階段的路徑進(jìn)行求解，得到第二求解階段的第二終止位姿；將第二終止位姿作為第三求解階段的第三起始位姿，對(duì)第三求解階段的路徑進(jìn)行求解，得到第三求解階段的第三終止位姿；將第一起始位姿到達(dá)第一終止位姿的第一路徑，第二起始位姿到達(dá)第二終止位姿的第二路徑以及第三起始位姿到達(dá)第三終止位姿的第三路徑首尾相連，得到零部件的全局拆卸路徑。本發(fā)明可以實(shí)現(xiàn)對(duì)復(fù)雜環(huán)境中對(duì)產(chǎn)品拆卸路徑進(jìn)行求解。
文檔編號(hào)G06F17/50GK102289553SQ20111028874
公開(kāi)日2011年12月21日 申請(qǐng)日期2011年9月26日 優(yōu)先權(quán)日2011年9月26日
發(fā)明者劉密, 劉檢華, 寧汝新, 尚煒 申請(qǐng)人:北京理工大學(xué)