【正文】 對于C空間中存在狹小通道的問題，可在路徑必經(jīng)的狹小區(qū)域插入位姿點；對于雙機動作復雜的任務(wù)可在動作輸入集變化的區(qū)域插入路徑位姿點。本節(jié)是對論文第三章的深入研究，這類問題中環(huán)境復雜，或者雙機動作復雜度高，或者行走轉(zhuǎn)彎同時進行，會出現(xiàn)尋路成功概率低、路徑迂回質(zhì)量差或者耗費時間長等問題。C空間中存在狹小區(qū)域，而尋找路徑成功又必須經(jīng)過此區(qū)域，由于算法本質(zhì)的隨機性因素，很可能錯過此區(qū)域，導致尋路失敗。吊裝路徑尋找困難的原因可以歸結(jié)為兩個：（1） 雙機動作復雜度高，造成搜索空間巨大。案例實現(xiàn)的典型吊裝任務(wù)表明由用戶根據(jù)具體項目限制可活動自由度、設(shè)置C空間范圍便可在短時間內(nèi)得到較短路徑，大大減少了自由度增多和算法隨機性引起路徑出現(xiàn)迂回現(xiàn)象的出現(xiàn)，滿足無碰撞、不超載、起重機運動特性以及最小繩偏角等約束，證實了該方法的靈活性、高效性和可控性。 本章小結(jié)本章應(yīng)用RRTConnect算法，解決了雙機只能進行平移被吊設(shè)備以及不可以行走、轉(zhuǎn)彎動作兩個問題。因此，所做案例表明本文提出的算法既能協(xié)同翻轉(zhuǎn)設(shè)備使之豎直，又能用于協(xié)同抬起設(shè)備越過障礙物，并且雙機均具有行走、轉(zhuǎn)彎、起升、回轉(zhuǎn)和變幅等動作，可以在較短時間內(nèi)找到從初始位姿到目標位姿的雙機協(xié)同作業(yè)方案，并且該路徑滿足閉環(huán)、無碰撞、不超載和行走非完整約束。三個案例完成了三種典型吊裝任務(wù)。通過C空間上下限的設(shè)置，使搜索路徑空間減小或降維。由于轉(zhuǎn)彎動方向不定且RRTConnect算法具有隨機性，造成了一定的路徑迂回，%?？梢钥吹匠晒β蔬_到100%，平均每規(guī)劃一次路徑需要進行6541次碰撞檢測計算，兩棵樹相遇時樹上節(jié)點總數(shù)為194個。a）吊點軌跡 b） 初始位姿a） Trajectories of linking points b） The initial configuration c） 吊裝中位姿1 d） 吊裝中位姿2c） Configuration 1 of dualcrane d） Configuration 2 of dualcraneg）吊裝中位姿3 h） 吊裝結(jié)束g） Configuration 3 of dualcrane h） The target configuration 雙機吊裝仿真截圖Fig. The screenshots of dualcrane lifting simulation。（3） 結(jié)果輸出，可以看到應(yīng)用RRTConnect算法可以成功找到一條滿足約束的可行路徑，圖中細線為吊點A和吊點B劃過的軌跡。輸入集中有三個可選的輸入形式。但是我們設(shè)定該吊裝任務(wù)中通過兩臺起重機的同時轉(zhuǎn)彎避開通道中部突出的障礙物，所以C空間相應(yīng)維度的上下限不能相同，該案例中表示下車轉(zhuǎn)角的維度區(qū)間設(shè)為[0, ]。 雙機初始和目標位姿信息Tab. Initial and target configuration of dualcrane項目() (m)(rad)(rad)(rad)(m)起重機A初始(, )58目標(, )58起重機B初始(75, )58目標(75, 30)58。 路徑規(guī)劃結(jié)果Tab. Results of path planning項目成功率碰撞檢測（次）樹節(jié)點（個）規(guī)劃時間(s)路徑長度(m)優(yōu)化時間(s)優(yōu)化后長度(m)RRTConnect100%2404110 案例三（1） 任務(wù)描述，起重機A和B把被吊設(shè)備從狹長通道的一端搬運到另一端，并且需要在一定位置避開突出的障礙物。規(guī)劃時間和路徑優(yōu)化時間都很短。由于本案例中雙機均沒有行走和轉(zhuǎn)彎動作，為輸入可控的完整約束問題。雙機協(xié)同吊裝過程中起重機A和B吊起設(shè)備依次越過三個鋼架。 雙機動作輸入集Tab. Motion inputs起重機A起重機B行走(m)轉(zhuǎn)彎(rad)回轉(zhuǎn)(rad)變幅(rad)起升(m)行走(m)轉(zhuǎn)彎(rad)回轉(zhuǎn)(rad)變幅(rad)起升(m)兩棵樹相遇的位姿誤差為（, , , , , ；, , , , , ）。 C空間范圍Tab. Range of C space項目起重機A起重機B最小31003最大5410054，每行表示一個輸入向量。C空間設(shè)置時將某些一定不會發(fā)生變化的自由度區(qū)間設(shè)為零。可以看出雙機的位姿具有對稱性，起重機A轉(zhuǎn)臺相對下車回轉(zhuǎn)角增大；起重機B轉(zhuǎn)臺相對下車回轉(zhuǎn)角減小。 路徑規(guī)劃結(jié)果Tab. Results of path planning項目成功率碰撞檢測（次）樹節(jié)點（個）規(guī)劃時間(s)路徑長度(m)優(yōu)化時間(s)優(yōu)化后長度(m)RRTConnect100%8420201 案例二（1） 任務(wù)描述，吊裝任務(wù)為起重機A和B通過回轉(zhuǎn)并配合其他動作將被吊設(shè)備由實線所示的初始位置轉(zhuǎn)移到三個不同高度、并排矗立的鋼架另一側(cè)，即虛線所示的目標位置。規(guī)劃時間和路徑優(yōu)化時間都很短。由于本案例中只有起重機B直線行走，并且無轉(zhuǎn)彎動作，為輸入可控的完整約束問題。雙機協(xié)同吊裝過程中起重機A起升、變幅配合起重機B回轉(zhuǎn)、沿直線行走。 , , , , , ）。輸入集中有四個可選的輸入形式。例如，不允許起重機B沿Z向行走，與起重機B初始和目標位姿信息的Z坐標值相同。 雙機初始和目標位姿信息Table Initial and target configuration of dualcrane項目() (m)(rad)(rad)(rad)(m)起重機A初始(, )目標(, )起重機B初始(78, )目標(32, )。 雙機吊裝起始時刻布局圖 雙擊吊裝終止時刻布局圖 Scene of heavy lifing at starting time Scene of heavy lifing at finishing time（2） 參數(shù)輸入，為了方便查看。 案例一（1） 任務(wù)描述本案例吊裝任務(wù)為雙機協(xié)同將被吊設(shè)備豎起，之后由起重機A運輸并安裝到基礎(chǔ)上。案例一中起重機A臂長63m，起重機B臂長42m，案例二和三中兩臺起重機的臂長均為63m。、（尺寸參照LIEBHERR1750）；被吊設(shè)備長50m，直徑4m。本文對得到的可行路徑進行了平滑處理，即在得到的路徑上隨機選擇兩個位姿點，將這兩個位姿作為初始和目標位姿點用RRTConnect算法再次進行路徑規(guī)劃，若新生成的路徑比原始路徑短，則替換并生成更短路徑，不斷迭代該過程到設(shè)定迭代次數(shù)結(jié)束。給出的雙機始末位姿屬于吊裝路徑的一部分，為C空間中滿足約束的點，算法對其是否滿足路徑約束做出反饋，若不滿足需進行適當調(diào)整。臂架鉸點到起升滑輪組連線與水平面夾角 （）臂架仰角 （）（3） 起重機A起升滑輪組到地面距離 （）式中，——起重機A的下車高；——臂架鉸點到轉(zhuǎn)臺回轉(zhuǎn)軸的豎直距離。（1） 起重機A轉(zhuǎn)臺相對下車轉(zhuǎn)角轉(zhuǎn)臺轉(zhuǎn)角為 （）轉(zhuǎn)臺相對下車轉(zhuǎn)角 （）（2） 起重機A臂架仰角臂架鉸點到起升滑輪組連線與臂架軸線夾角 （）式中，——起升滑輪組中心到臂架軸線距離；——起重機A臂架長度。本文通過輸入被吊設(shè)備始末位姿，并根據(jù)經(jīng)驗給定雙機回轉(zhuǎn)中心X、Z坐標以及下車轉(zhuǎn)角，采用逆向運動學原理，自動計算出雙機其余自由度值，即得到雙機起始和終止時刻的全部位姿信息。由于兩棵樹通過控制輸入集進行擴展，兩棵樹很難嚴格相遇，為此需要為兩個位姿點各個維度允許誤差進行設(shè)置，允許誤差可以用與位姿點對應(yīng)的十二維向量表示。步驟6 相遇判定用相應(yīng)的最佳輸入進行多步擴展，直到與隨機點的距離不在變小，或者不滿足上述三點要求的一點，則將最后滿足約束的位姿生成新節(jié)點加入到樹中，并記錄相鄰位姿點的距離，接著以新節(jié)點作為另一棵的隨機點，進行同樣的多步擴展，如此反復直到兩棵樹相遇。遍歷控制輸入集中所有設(shè)定的動作形式，按照公式（）計算動作后的雙機新位姿點，并對該位姿點進行三方面的檢測：（1） 該位姿點是否在雙機C空間中；（2） 兩吊點的距離與被吊設(shè)備實際長度誤差是否使繩偏角限定在設(shè)定范圍內(nèi)；（3） 碰撞檢測，包括起重機和障礙物、被吊設(shè)備和障礙物、起重機和被吊設(shè)備以及被吊設(shè)備和地面之間的碰撞，本文采用PQP方法?？紤]滿足履帶行走的非完整約束，需要對最佳輸入作以下處理，設(shè)動作前和動作后兩個位姿點和為和 ，則有： （）步驟5 最佳輸入判定首先要構(gòu)建輸入集，然后設(shè)置相應(yīng)的可選輸入組成輸入集。步驟4 輸入處理選擇好最佳輸入，則通過積分生成新節(jié)點。步驟2 隨機點生成雙機的初始位姿和目標位姿為根節(jié)點初始化兩棵樹，樹擴展時，隨機點在C空間隨機選取。(a) 樹擴展 （a） Expanding of (b) 樹擴展（b） Expanding of 兩棵樹交替擴展Fig. Two expanding in turns對于雙機路徑規(guī)劃問題，該算法的執(zhí)行步驟如下所示：步驟1 建立C空間，但為了提高路徑規(guī)劃的效率，一般由方案制作人員根據(jù)具體的吊裝任務(wù)縮小C空間各個維度的上下限，極端情況為某個維度起始和終止時刻的值一致，并且在吊裝過程中不發(fā)生變化，則該維度的最大值和最小值可設(shè)為相等的并與始末位姿點中一樣的值。再次判斷是否相遇。之后判斷兩棵樹的最新節(jié)點距離是否滿足誤差要求，即是否相遇。 路徑規(guī)劃流程圖Fig. Flow chart of path planning RRTConnect算法RRTConnect算法分別以初始化兩棵樹和，和在C空間向著對方交替擴展，反復迭代，直到兩棵樹相遇，則成功找到一條從始態(tài)到終態(tài)的路徑，若達到迭代次數(shù)仍未相遇則尋路失敗。首先參數(shù)輸入部分，主要設(shè)置雙機位姿信息以及環(huán)境的位置和尺寸信息，確定雙機的初始和目標位姿、控制輸入集等，輸入雙機起重性能、被吊設(shè)備分配到兩臺起重機的最大重量等；其次處理部分，根據(jù)輸入信息建立C空間，RRTConnect算法將在構(gòu)建的輸入集中不斷選擇最佳輸入并在C空間中進行樹的擴展，以找到滿足約束的路徑，然后對得到的路徑進行處理使路徑更短更平滑；最后輸出部分，可以保存雙機先后執(zhí)行的動作形式、動作量，指導吊裝方案制定和執(zhí)行，同時可以將吊裝過程可視化。為此，本章解決可行走的雙機協(xié)同吊裝路徑規(guī)劃問題，吊裝任務(wù)可以是避開障礙物的平移被吊設(shè)備，也可以是翻轉(zhuǎn)被吊設(shè)備使其豎起的吊裝。然而，目前關(guān)于雙機路徑規(guī)劃的研究一般只考慮起升、回轉(zhuǎn)和變幅三個動作，且只針對平移搬運設(shè)備的吊裝任務(wù)。但是，此種方法中雙機不可行走，并且只針對被吊設(shè)備的平移任務(wù)。3 基于RRTConnect算法的雙機路徑規(guī)劃方法 問題描述YuCheng Chang于2012年提出一種基于概率路標法（PRM，Probabilistic roadmaps Method）規(guī)劃單臺起重機和兩臺起重機協(xié)同吊裝的路徑[22]，該方法先將起重機的回轉(zhuǎn)、變幅自由度作為位形的變量（此時暫未考慮起升自由度），在此位形空間中采用PRM規(guī)劃出一條無碰撞的吊裝路徑，然后在此路徑的基礎(chǔ)上進行起重機起升動作的規(guī)劃，從而最終得到一條可行的無碰撞吊裝路徑。例如臂架和下車不可能發(fā)生碰撞，側(cè)無需設(shè)置此碰撞對。本文中碰撞檢測包括兩層，一是雙機和設(shè)備系統(tǒng)的各個剛體模型是否與環(huán)境模型碰撞；二是雙機和設(shè)備系統(tǒng)各個剛體之間是否發(fā)生碰撞。 碰撞檢測碰撞檢測采用PQP （Proximity Queries Package）方法，PQP是Larsen（1999）等所提出算法的實現(xiàn)。 雙機和設(shè)備系統(tǒng)機構(gòu)簡圖 允許繩偏角Fig. Diagram of dualcrane system Structure Fig. Incline angle allowed此誤差在實際吊裝中表現(xiàn)為繩索偏擺，設(shè)實際吊裝要求繩偏角在以內(nèi)，允許的繩偏角與兩吊鉤間距與被吊設(shè)備長度誤差有以下對應(yīng)關(guān)系。王維等[62]將操控對象的配置空間作為求解空間，利用手臂快速逆向運動學方法獲得的虛擬人手臂配置作為判斷操控對象配置是否可行的約束，結(jié)合雙向平衡的啟發(fā)式RRT擴展算法，可快速求解虛擬人雙臂的操控規(guī)劃問題。雙機、被吊設(shè)備與地面構(gòu)成了一個閉環(huán)，不同于機器人中研究的鉸鏈連接