專利名稱：工程機(jī)械入場路徑規(guī)劃方法和路徑規(guī)劃裝置的制作方法
技術(shù)領(lǐng)域：
本發(fā)明涉及工程機(jī)械領(lǐng)域，具體而言，涉及一種工程機(jī)械入場路徑規(guī)劃方法和路徑規(guī)劃裝置。
背景技術(shù)：
一般工程機(jī)械體積龐大，而在施工環(huán)境較為復(fù)雜，存在各種障礙物。因此將例如起重機(jī)等大型工程機(jī)械布置到工位上，需要對工程機(jī)械的入場路徑進(jìn)行規(guī)劃。在工程機(jī)械入場路徑規(guī)劃中，必須保證工程機(jī)械安全地通過障礙物之間的縫隙，防止出現(xiàn)工程機(jī)械與障礙物發(fā)生碰撞，另外，由于工程機(jī)械的重量和體積都較大，進(jìn)行路徑規(guī)劃還需要對行進(jìn)的距離和轉(zhuǎn)向次數(shù)進(jìn)行綜合考慮?，F(xiàn)有的工程機(jī)械入場一般依靠人工勘查，通過操作人員的個人經(jīng)驗(yàn)進(jìn)行操作，不能保證是否是最優(yōu)方案，另外也無法保證順利通過所有障礙物，有時甚至出現(xiàn)退回重新選擇路徑的情況。目前有文獻(xiàn)中提出使用遍歷的方法進(jìn)行計(jì)算距離，首先將障礙物轉(zhuǎn)為凸形物體，對障礙物間的間距進(jìn)行遍歷距離計(jì)算，這種方法計(jì)算量大，計(jì)算耗時長，而且沒有充分考慮工程機(jī)械的轉(zhuǎn)彎情況，不滿足實(shí)際工程機(jī)械在三維場景中進(jìn)行規(guī)劃需求。以上現(xiàn)有技術(shù)工程機(jī)械入場路徑規(guī)劃中沒有結(jié)合工程機(jī)械實(shí)際轉(zhuǎn)彎特點(diǎn)的問題，尚未提出有效的解決方案。

發(fā)明內(nèi)容
本發(fā)明旨在提供一種工程機(jī)械入場路徑規(guī)劃方法和路徑規(guī)劃裝置，以解決現(xiàn)有技術(shù)中入場路徑規(guī)劃中沒有結(jié)合工程機(jī)械實(shí)際轉(zhuǎn)彎特點(diǎn)的問題。為了實(shí)現(xiàn)上述目的，根據(jù)本發(fā)明的一個方面，提供了一種工程機(jī)械入場路徑規(guī)劃方法。本發(fā)明提供的工程機(jī)械入場路徑規(guī)劃方法包括將入場區(qū)域內(nèi)的障礙物的三維模型投影到水平面上，得到障礙物的二維投影輪廓；利用二維投影輪廓分別計(jì)算每兩個相鄰障礙物之間的最短連線，并標(biāo)記最短連線的中點(diǎn)；計(jì)算工程機(jī)械經(jīng)由最短連線的中點(diǎn)到達(dá)目標(biāo)位置的各條路徑，并對各條路徑進(jìn)行路徑規(guī)劃計(jì)算；分別計(jì)算工程機(jī)械按照經(jīng)過路徑計(jì)算的各條路徑入場需要行進(jìn)的距離、轉(zhuǎn)彎次數(shù)、和工程機(jī)械通過障礙物時與障礙物的最小距離；按照需要行進(jìn)的距離、轉(zhuǎn)彎次數(shù)、和工程機(jī)械通過障礙物時與障礙物的最小距離對經(jīng)過路徑計(jì)算的各條路徑進(jìn)行排序，并根據(jù)排序結(jié)果輸出各條入場路徑。進(jìn)一步地，利用二維投影輪廓分別計(jì)算每兩個相鄰障礙物之間的最短連線包括使用層次包圍盒法分別對每個障礙物的二維投影輪廓進(jìn)行計(jì)算，得到每個障礙物的包圍盒輪廓；分別計(jì)算每兩個相鄰障礙物的包圍盒輪廓之間的最短連線，作為每兩個相鄰障礙物之間的最短連線。進(jìn)一步地，使用層次包圍盒法分別對每個障礙物的二維投影輪廓進(jìn)行計(jì)算，得到每個障礙物的包圍盒輪廓包括使用方向包圍盒對每個障礙物的二維投影輪廓進(jìn)行計(jì)算，得到每個障礙物的矩形輪廓；分別計(jì)算每兩個相鄰障礙物的包圍盒輪廓之間的最短連線包括分別計(jì)算每兩個相鄰障礙物的矩形輪廓之間的最短連線。進(jìn)一步地，計(jì)算工程機(jī)械經(jīng)由最短連線的中點(diǎn)到達(dá)目標(biāo)位置的各條路徑包括沿當(dāng)前點(diǎn)向目標(biāo)點(diǎn)的前進(jìn)方向進(jìn)行路徑探索；當(dāng)前進(jìn)方向上遇到障礙物時，判斷遇到的障礙物的至少一側(cè)是否存在該障礙物到與該障礙物相鄰的障礙物的最短連線；當(dāng)遇到的障礙物的至少一側(cè)存在該障礙物到與該障礙物相鄰的障礙物的最短連線時，通過最短連線的中點(diǎn)向目標(biāo)點(diǎn)搜索路徑，并記錄當(dāng)前點(diǎn)到該中點(diǎn)的路徑。進(jìn)一步地，判斷遇到的障礙物的至少一側(cè)是否存在該障礙物到與該障礙物相鄰的障礙物的最短連線之后還包括當(dāng)障礙物的至少一側(cè)不存在該障礙物到與該障礙物相鄰的障礙物的最短連線時，根據(jù)工程機(jī)械繞行障礙物的安全距離查找距離最短的轉(zhuǎn)向點(diǎn)，通過轉(zhuǎn)向點(diǎn)向目標(biāo)點(diǎn)搜索路徑，并記錄當(dāng)前點(diǎn)到轉(zhuǎn)向點(diǎn)的路徑。進(jìn)一步地，對各條路徑進(jìn)行路徑規(guī)劃計(jì)算之后還包括分別判斷經(jīng)過路徑計(jì)算的各條路徑中所有最短連線的距離是否都大于工程機(jī)械的轉(zhuǎn)彎安全距離；刪除出現(xiàn)最短連線的距離小于工程機(jī)械的轉(zhuǎn)彎安全距離的路徑。進(jìn)一步地，工程機(jī)械的轉(zhuǎn)彎安全距離為工程機(jī)械外輪廓的最大轉(zhuǎn)彎半徑與內(nèi)輪廓的最小半徑之差加上預(yù)設(shè)裕值。進(jìn)一步地，分別計(jì)算工程機(jī)械按照經(jīng)過路徑計(jì)算的各條路徑入場需要行進(jìn)的距離包括計(jì)算經(jīng)過路徑計(jì)算的各條路徑中工程機(jī)械轉(zhuǎn)彎的中心點(diǎn)、半徑、和轉(zhuǎn)向位置；分別對經(jīng)過路徑計(jì)算的各條路徑中工程機(jī)械轉(zhuǎn)彎的弧線距離進(jìn)行累加得到各條路徑入場需要行進(jìn)的距離。進(jìn)一步地，計(jì)算工程機(jī)械經(jīng)由最短連線的中點(diǎn)到達(dá)目標(biāo)位置的各條路徑包括使用二叉樹算法遍歷計(jì)算工程機(jī)械經(jīng)由最短連線的中點(diǎn)到達(dá)目標(biāo)位置的各條路徑。進(jìn)一步地，對二叉樹各條路徑進(jìn)行路徑規(guī)劃計(jì)算包括使用迪杰斯特拉算法對二叉樹各條路徑分別進(jìn)行路徑規(guī)劃計(jì)算。根據(jù)本發(fā)明的另一個方面，提供了一種工程機(jī)械入場路徑規(guī)劃裝置。該工程機(jī)械入場路徑規(guī)劃裝置包括投影模塊，用于將入場區(qū)域內(nèi)的障礙物的三維模型投影到水平面上，得到障礙物的二維投影輪廓；最短距離中點(diǎn)計(jì)算模塊，用于利用二維投影輪廓分別計(jì)算每兩個相鄰障礙物之間的最短連線，并標(biāo)記最短連線的中點(diǎn)；路徑搜索模塊，用于計(jì)算工程機(jī)械經(jīng)由最短連線的中點(diǎn)到達(dá)目標(biāo)位置的各條路徑，并對各條路徑進(jìn)行路徑規(guī)劃計(jì)算；路徑分析模塊，用于分別計(jì)算工程機(jī)械按照經(jīng)過路徑計(jì)算的各條路徑入場需要行進(jìn)的距離、轉(zhuǎn)彎次數(shù)、和工程機(jī)械通過障礙物時與障礙物的最小距離；路徑優(yōu)化模塊，用于按照需要行進(jìn)的距離、轉(zhuǎn)彎次數(shù)、和工程機(jī)械通過障礙物時與障礙物的最小距離對經(jīng)過路徑計(jì)算的各條路徑進(jìn)行排序，并根據(jù)排序結(jié)果輸出各條入場路徑。進(jìn)一步地，本發(fā)明提供的工程機(jī)械入場路徑規(guī)劃裝置還包括路徑安全評價模塊，用于分別判斷各條路徑中所有最短連線的距離是否都大于工程機(jī)械的轉(zhuǎn)彎安全距離，并刪除出現(xiàn)最短連線的距離小于工程機(jī)械的轉(zhuǎn)彎安全距離的路徑應(yīng)用本發(fā)明的技術(shù)方案，通過計(jì)算障礙物二維投影輪廓的最短連線，找出最短連線的中點(diǎn)，在出現(xiàn)障礙物情況下探索從出發(fā)點(diǎn)通過上述最短連線終點(diǎn)到達(dá)目的施工地點(diǎn)的路線，并在進(jìn)行路線路徑計(jì)算后，按照實(shí)際工程機(jī)械的行進(jìn)距離計(jì)算路徑距離，統(tǒng)計(jì)轉(zhuǎn)彎次數(shù)，安全間距，從而可以綜合各項(xiàng)條件對各條路徑進(jìn)行考察，最終輸出最優(yōu)路徑方案。解決了已知施工環(huán)境的三維場景后的工程機(jī)械入場路徑規(guī)劃問題，計(jì)算量小，可以實(shí)現(xiàn)實(shí)時進(jìn)行路徑規(guī)劃的要求，符合工程機(jī)械的自身移動特性。


構(gòu)成本申請的一部分的說明書附圖用來提供對本發(fā)明的進(jìn)一步理解，本發(fā)明的示意性實(shí)施例及其說明用于解釋本發(fā)明，并不構(gòu)成對本發(fā)明的不當(dāng)限定。在附圖中圖1是根據(jù)本發(fā)明實(shí)施例的工程機(jī)械入場路徑規(guī)劃裝置的示意圖；圖2是根據(jù)本發(fā)明實(shí)施例的工程機(jī)械入場路徑規(guī)劃方法的示意圖；圖3A是根據(jù)本發(fā)明實(shí)施例的工程機(jī)械入場路徑規(guī)劃方法對障礙物的三維模型投影到水平面的示意圖；圖3B是根據(jù)本發(fā)明實(shí)施例的工程機(jī)械入場路徑規(guī)劃方法使用方向包圍盒對每個障礙物的二維投影輪廓進(jìn)行計(jì)算的結(jié)果示意圖；圖4A是根據(jù)本發(fā)明實(shí)施例的工程機(jī)械入場路徑規(guī)劃方法矩形輪廓最短距離的第一種情況；圖4B是根據(jù)本發(fā)明實(shí)施例的工程機(jī)械入場路徑規(guī)劃方法矩形輪廓最短距離的第二種情況；圖4C是根據(jù)本發(fā)明實(shí)施例的工程機(jī)械入場路徑規(guī)劃方法矩形輪廓最短距離的第三種情況；圖5是根據(jù)本發(fā)明實(shí)施例的工程機(jī)械入場路徑規(guī)劃方法的具體場景的示意圖；圖6是根據(jù)本發(fā)明實(shí)施例的工程機(jī)械入場路徑規(guī)劃方法的穿越障礙物的具體路徑的不意圖；圖7是根據(jù)本發(fā)明實(shí)施例的工程機(jī)械入場路徑規(guī)劃方法的穿越障礙物的具體路徑的流程圖；圖8是根據(jù)本發(fā)明實(shí)施例的工程機(jī)械入場路徑規(guī)劃方法得出的最佳路徑的示意圖。
具體實(shí)施例方式需要說明的是，在不沖突的情況下，本申請中的實(shí)施例及實(shí)施例中的特征可以相互組合。下面將參考附圖并結(jié)合實(shí)施例來詳細(xì)說明本發(fā)明。本發(fā)明實(shí)施例提供了一種工程機(jī)械入場路徑規(guī)劃裝置，圖1是根據(jù)本發(fā)明實(shí)施例的工程機(jī)械入場路徑規(guī)劃裝置的示意圖，如圖1所示，本發(fā)明實(shí)施例的工程機(jī)械入場路徑規(guī)劃裝置包括投影模塊11，用于將入場區(qū)域內(nèi)的障礙物的三維模型投影到水平面上，得到障礙物的二維投影輪廓；最短距離中點(diǎn)計(jì)算模塊13，用于利用二維投影輪廓分別計(jì)算每兩個相鄰障礙物之間的最短連線，并標(biāo)記最短連線的中點(diǎn)；路徑搜索模塊15，用于計(jì)算工程機(jī)械經(jīng)由最短連線的中點(diǎn)到達(dá)目標(biāo)位置的各條路徑，并對各條路徑進(jìn)行路徑規(guī)劃計(jì)算；路徑分析模塊17，用于分別計(jì)算工程機(jī)械按照經(jīng)過路徑計(jì)算的各條路徑入場需要行進(jìn)的距離、轉(zhuǎn)彎次數(shù)、和工程機(jī)械通過障礙物時與障礙物的最小距離；路徑優(yōu)化模塊19，用于按照需要行進(jìn)的距離、轉(zhuǎn)彎次數(shù)、和工程機(jī)械通過障礙物時與障礙物的最小距離對各條路徑進(jìn)行排序，并根據(jù)排序結(jié)果輸出各條入場路徑。
使用本發(fā)明的工程機(jī)械入場路徑規(guī)劃裝置，計(jì)算障礙物二維投影輪廓的最短連線，找出最短連線的中點(diǎn)，探索從出發(fā)點(diǎn)通過上述最短連線終點(diǎn)到達(dá)目的施工地點(diǎn)的路線并進(jìn)行路徑規(guī)劃計(jì)算，對經(jīng)過路徑計(jì)算的路線按照實(shí)際工程機(jī)械的行進(jìn)距離計(jì)算路徑距離，統(tǒng)計(jì)轉(zhuǎn)彎次數(shù)，安全間距，從而可以綜合各項(xiàng)條件對各條路徑進(jìn)行考察，最終輸出最優(yōu)路徑方案。解決了已知施工環(huán)境的三維場景后的工程機(jī)械入場路徑規(guī)劃問題，計(jì)算量小，可以實(shí)現(xiàn)實(shí)時進(jìn)行路徑規(guī)劃的要求，符合工程機(jī)械的自身移動特性。為了驗(yàn)證工程機(jī)械是否安全順利通過障礙物之間的空間，本發(fā)明可應(yīng)用實(shí)施的工程機(jī)械入場路徑規(guī)劃裝置，還可以包括路徑安全評價模塊(圖中未示出)，用于分別判斷各條路徑中所有最短連線的距離是否都大于工程機(jī)械的轉(zhuǎn)彎安全距離，并刪除出現(xiàn)最短連線的距離小于工程機(jī)械的轉(zhuǎn)彎安全距離的路徑。使保留下來的所有路徑都符合工程機(jī)械的安全通過要求。其中，工程機(jī)械的轉(zhuǎn)彎安全距離為工程機(jī)械外輪廓的最大轉(zhuǎn)彎半徑與內(nèi)輪廓的最小半徑之差加上預(yù)設(shè)裕值。具體地，在轉(zhuǎn)彎過程中，障礙物之間最短距離必須大于外輪廓的最大轉(zhuǎn)彎半徑Rmax與內(nèi)輪廓的最小半徑Rmin之差(Rmax-Rmin) +X,其中X為可以預(yù)先設(shè)置的轉(zhuǎn)彎安全距離裕值。當(dāng)障礙物物之間的距離小于(Rmax-Rmin) +X時，則說明工程機(jī)械不能通過這兩個障礙物之間的空間，該條路徑需要拋棄。如果通過比較，該路徑上所有障礙物之間的距離都大于(Rmax-Rmin) +X，則可以認(rèn)為該路徑暢通。施工場地中的障礙物可能是建筑、堆放材料、已經(jīng)入場的工程機(jī)械、或者自然存在的物體，形狀是不規(guī)則的，因此直接計(jì)算障礙物之間的最短距離計(jì)算量大。因此投影模塊11的具體執(zhí)行步驟可以是使用層次包圍盒法分別對每個障礙物的二維投影輪廓進(jìn)行計(jì)算，得到每個障礙物的包圍盒輪廓；分別計(jì)算每兩個相鄰障礙物的包圍盒輪廓之間的最短連線，作為每兩個相鄰障礙物之間的最短連線。層次包圍盒法是用體積略大而幾何特性簡單的包圍盒來近似地描述復(fù)雜的對象，從而只對包圍盒重疊的對象進(jìn)行進(jìn)一步的相交測試，常見的包圍盒類型有沿坐標(biāo)軸的包圍盒AABB、包圍球、方向包圍盒0BB、固定方向凸包等。由于方向包圍盒(Oriented boundingbox)算法是利用最接近的矩形覆蓋障礙物的投影輪廓，而矩形之間的最短距離計(jì)算相對簡單，因此可以優(yōu)選使用方向包圍盒對每個障礙物的二維投影輪廓進(jìn)行計(jì)算，得到每個障礙物的矩形輪廓；然后分別計(jì)算每兩個相鄰障礙物的矩形輪廓之間的最短連線。路徑搜索模塊15的具體執(zhí)行步驟為沿當(dāng)前點(diǎn)向目標(biāo)點(diǎn)的前進(jìn)方向進(jìn)行路徑探索；當(dāng)前進(jìn)方向上遇到障礙物時，判斷遇到的障礙物的至少一側(cè)是否存在該障礙物到與該障礙物相鄰的障礙物的最短連線；當(dāng)遇到的障礙物的至少一側(cè)存在該障礙物到與該障礙物相鄰的障礙物的最短連線時，通過最短連線的中點(diǎn)向目標(biāo)點(diǎn)搜索路徑，并記錄當(dāng)前點(diǎn)到中點(diǎn)的路徑。當(dāng)障礙物的至少一側(cè)不存在該障礙物到與該障礙物相鄰的障礙物的最短連線時，根據(jù)工程機(jī)械繞行障礙物的安全距離查找距離最短的轉(zhuǎn)向點(diǎn)，通過轉(zhuǎn)向點(diǎn)向目標(biāo)點(diǎn)搜索路徑,并記錄當(dāng)前點(diǎn)到轉(zhuǎn)向點(diǎn)的路徑。依次各條連接搜索中點(diǎn)或轉(zhuǎn)向點(diǎn)直到目標(biāo)點(diǎn)，就可以得到從當(dāng)前點(diǎn)到目標(biāo)點(diǎn)可能經(jīng)過的所有路徑。也就是當(dāng)前進(jìn)方向上遇到障礙物時，就從障礙物兩側(cè)的中點(diǎn)或者轉(zhuǎn)向點(diǎn)繞行，從而分出兩條可能的路徑，然后從該中點(diǎn)或轉(zhuǎn)向點(diǎn)繼續(xù)向前探索。直至到達(dá)目標(biāo)點(diǎn)，然后整理出所有可能經(jīng)過的路徑供下一步優(yōu)化。路徑分析模塊17，計(jì)算各條計(jì)算得出的路徑中工程機(jī)械轉(zhuǎn)彎的中心點(diǎn)、半徑、和轉(zhuǎn)向位置的步驟可以是分別對各條路徑中工程機(jī)械轉(zhuǎn)彎的弧線距離進(jìn)行累加得到各條路徑入場需要行進(jìn)的距離。就是計(jì)算工程機(jī)械行進(jìn)的弧線距離而不是直線距離，使各條路徑入場需要行進(jìn)的距離更加符合工程機(jī)械入場的要求。路徑優(yōu)化模塊19的排序條件包括需要行進(jìn)的距離、轉(zhuǎn)彎次數(shù)、和工程機(jī)械通過障礙物時與障礙物的最小距離。其中，需要行進(jìn)的距離反映了工程機(jī)械行進(jìn)的距離長短-M彎次數(shù)反映了工程機(jī)械的路線控制難度；工程機(jī)械通過障礙物時與障礙物的最小距離反映了安全程度。用戶可以根據(jù)需要選擇排序條件，或者對以上排序設(shè)置權(quán)重值進(jìn)行綜合計(jì)算，得到最符合用戶要求的入場路徑。以上路徑搜索模塊15計(jì)算路徑以及進(jìn)行路徑規(guī)劃的具體算法可以為用于利用二叉樹算法遍歷計(jì)算工程機(jī)械經(jīng)由最短連線的中點(diǎn)到達(dá)目標(biāo)位置的各條路徑，并使用迪杰斯特拉算法(Di jkstra)對各條路徑進(jìn)行路徑規(guī)劃計(jì)算。迪杰斯特拉(Di jkstra)算法是現(xiàn)有典型的單源最短路徑算法，用于計(jì)算一個節(jié)點(diǎn)到其他所有節(jié)點(diǎn)的最短路徑。以起始點(diǎn)為中心向外層層擴(kuò)展，直到擴(kuò)展到終點(diǎn)為止。本發(fā)明實(shí)施例還提供了一種工程機(jī)械入場路徑規(guī)劃方法，該工程機(jī)械入場路徑規(guī)劃方法可以通過本發(fā)明上述實(shí)施例所提供的任一種工程機(jī)械入場路徑規(guī)劃裝置來執(zhí)行。圖2是根據(jù)本發(fā)明實(shí)施例的工程機(jī)械入場路徑規(guī)劃方法的示意圖，如圖所示，該工程機(jī)械入場路徑規(guī)劃方法包括步驟S21，將入場區(qū)域內(nèi)的障礙物的三維模型投影到水平面上，得到障礙物的二維投影輪廓；步驟S22，利用二維投影輪廓分別計(jì)算每兩個相鄰障礙物之間的最短連線，并標(biāo)記最短連線的中點(diǎn)；步驟S23，計(jì)算工程機(jī)械經(jīng)由最短連線的中點(diǎn)到達(dá)目標(biāo)位置的各條路徑，并對各條路徑進(jìn)行路徑規(guī)劃計(jì)算；步驟S24，分別計(jì)算工程機(jī)械按照經(jīng)過路徑計(jì)算的各條路徑入場需要行進(jìn)的距離、轉(zhuǎn)彎次數(shù)、和工程機(jī)械通過障礙物時與障礙物的最小距離；步驟S25，按照需要行進(jìn)的距離、轉(zhuǎn)彎次數(shù)、和工程機(jī)械通過障礙物時與障礙物的最小距離對各條路徑進(jìn)行排序，并根據(jù)排序結(jié)果輸出各條入場路徑。利用該方法，計(jì)算障礙物二維投影輪廓的最短連線，找出最短連線的中點(diǎn)，探索從出發(fā)點(diǎn)通過上述最短連線終點(diǎn)到達(dá)目的施工地點(diǎn)的路線，并按照實(shí)際工程機(jī)械的行進(jìn)距離計(jì)算路徑距離，統(tǒng)計(jì)轉(zhuǎn)彎次數(shù)，安全間距，從而可以綜合各項(xiàng)條件對各條路徑進(jìn)行考察，最終輸出最優(yōu)路徑方案。解決了已知施工環(huán)境的三維場景后的工程機(jī)械入場路徑規(guī)劃問題，計(jì)算量小，可以實(shí)現(xiàn)實(shí)時進(jìn)行路徑規(guī)劃的要求，符合工程機(jī)械的自身移動特性。采用投影的思路將三維物體之間最短距離計(jì)算轉(zhuǎn)化二維物體間的距離的求解，簡化求解，降低計(jì)算量；采用包圍盒方法將物體進(jìn)行封裝來求解，將物體進(jìn)行封裝方式求解，簡化求解方法，同時求解精度損失較少；采用從障礙物中心點(diǎn)處入手來尋找最安全的入場路徑，符合實(shí)際路徑規(guī)劃需求；提出解決完整的工程機(jī)械入場路徑實(shí)現(xiàn)方法，并分步驟給出求解實(shí)現(xiàn)的方法。其中步驟S21具體可以包括使用層次包圍盒法分別對每個障礙物的二維投影輪廓進(jìn)行計(jì)算，得到每個障礙物的包圍盒輪廓；分別計(jì)算每兩個相鄰障礙物的包圍盒輪廓之間的最短連線，作為每兩個相鄰障礙物之間的最短連線。層次包圍盒法是用體積略大而幾何特性簡單的包圍盒來近似地描述復(fù)雜的對象，從而只對包圍盒重疊的對象進(jìn)行進(jìn)一步的相交測試，常見的包圍盒類型有沿坐標(biāo)軸的包圍盒AABB、包圍球、方向包圍盒OBB、固定方向凸包等。由于方向包圍盒(Oriented boundingbox)算法是利用最接近的矩形覆蓋障礙物的投影輪廓，而矩形之間的最短距離計(jì)算相對簡單。因此可以優(yōu)選使用方向包圍盒的算法，這種情況下，步驟S21中使用層次包圍盒法分別對每個障礙物的二維投影輪廓進(jìn)行計(jì)算，得到每個障礙物的包圍盒輪廓可以具體包括使用方向包圍盒對每個障礙物的二維投影輪廓進(jìn)行計(jì)算，得到所述每個障礙物的矩形輪廓；分別計(jì)算每兩個相鄰所述障礙物的包圍盒輪廓之間的最短連線可以具體包括分別計(jì)算每兩個相鄰所述障礙物的矩形輪廓之間的最短連線。圖3A是根據(jù)本發(fā)明實(shí)施例的工程機(jī)械入場路徑規(guī)劃方法對障礙物的三維模型投影到水平面的示意圖，圖3B是根據(jù)本發(fā)明實(shí)施例的工程機(jī)械入場路徑規(guī)劃方法使用方向包圍盒對每個障礙物的二維投影輪廓進(jìn)行計(jì)算的結(jié)果示意圖，如圖所示，一般是同場地上的障礙物位置和障礙物的形狀是不規(guī)則的，對其直接進(jìn)行距離計(jì)算，計(jì)算量大而且對求解難度大，使用方向包圍和計(jì)算后，將不規(guī)則的輪廓使用最接近的矩形代替。圖4A是根據(jù)本發(fā)明實(shí)施例的工程機(jī)械入場路徑規(guī)劃方法矩形輪廓最短距離的第一種情況，圖4B是根據(jù)本發(fā)明實(shí)施例的工程機(jī)械入場路徑規(guī)劃方法矩形輪廓最短距離的第二種情況，圖4C是根據(jù)本發(fā)明實(shí)施例的工程機(jī)械入場路徑規(guī)劃方法矩形輪廓最短距離的第三種情況，在圖4A中，兩個障礙物的矩形輪廓之間的最短距離為與第一障礙物的一條邊的垂線到第二障礙物一個頂點(diǎn)的距離；在圖4B中，兩個障礙物的矩形輪廓之間的最短距離為第一障礙物的一個頂點(diǎn)到第二障礙物一條邊的垂線的長度。在圖4C中，兩個障礙物的矩形輪廓之間的最短距離為一個矩形頂點(diǎn)到另一個障礙物矩形頂點(diǎn)。因此，可以通過比較矩形輪廓各頂點(diǎn)到矩形各點(diǎn)、線之間的距離的方式求解兩矩形間的最短距離。步驟S23具體可以包括沿當(dāng)前點(diǎn)向目標(biāo)點(diǎn)的前進(jìn)方向進(jìn)行路徑探索；當(dāng)前進(jìn)方向上遇到障礙物時用二叉樹方法來規(guī)劃路徑，具體為使用二叉樹法向障礙物兩側(cè)分叉，判斷遇到的障礙物的至少一側(cè)是否存在該障礙物到與該障礙物相鄰的障礙物的最短連線；當(dāng)遇到的障礙物的至少一側(cè)存在該障礙物到與該障礙物相鄰的障礙物的最短連線時，通過最短連線的中點(diǎn)向目標(biāo)點(diǎn)搜索路徑，并記錄當(dāng)前點(diǎn)到中點(diǎn)的路徑。當(dāng)障礙物的至少一側(cè)不存在該障礙物到與該障礙物相鄰的障礙物的最短連線時，根據(jù)工程機(jī)械繞行障礙物的安全距離查找距離最短的轉(zhuǎn)向點(diǎn)，通過轉(zhuǎn)向點(diǎn)向目標(biāo)點(diǎn)搜索路徑，并記錄當(dāng)前點(diǎn)到轉(zhuǎn)向點(diǎn)的路徑。從而，步驟S23計(jì)算工程機(jī)械經(jīng)由最短連線的中點(diǎn)到達(dá)目標(biāo)位置的各條路徑的具體算法為使用二叉樹算法進(jìn)行遍歷計(jì)算。步驟S23中對各條路徑進(jìn)行路徑規(guī)劃計(jì)算的算法可以具體為使用迪杰斯特拉算法對所述各條路徑分別進(jìn)行路徑規(guī)劃計(jì)算。通過步驟S23依次各條連接搜索中點(diǎn)或轉(zhuǎn)向點(diǎn)直到目標(biāo)點(diǎn)，就可以得到從當(dāng)前點(diǎn)到目標(biāo)點(diǎn)可能經(jīng)過的所有路徑。也就是當(dāng)前進(jìn)方向上遇到障礙物時，就從障礙物兩側(cè)的中點(diǎn)或者轉(zhuǎn)向點(diǎn)繞行，從而分出兩條可能的路徑，然后從該中點(diǎn)或轉(zhuǎn)向點(diǎn)繼續(xù)向前探索。直至到達(dá)目標(biāo)點(diǎn)，然后整理出所有可能經(jīng)過的路徑供下一步分析。下面結(jié)合附圖對一個具體的路徑規(guī)劃方法進(jìn)行說明圖5是根據(jù)本發(fā)明實(shí)施例的工程機(jī)械入場路徑規(guī)劃方法的具體場景的示意圖，如圖5所示，A點(diǎn)是工程機(jī)械的當(dāng)前點(diǎn)，B點(diǎn)是目標(biāo)點(diǎn)，在A至B的路徑當(dāng)中，存在著多個障礙物，圖中已經(jīng)按照方向包圍盒方法將障礙物的輪廓處理為矩形，C、D、E、F點(diǎn)為障礙物之間最短連線的中點(diǎn)。路徑搜索的具體步驟為以A點(diǎn)到B點(diǎn)為直徑的圓形區(qū)域劃定為入場區(qū)域，得出區(qū)域內(nèi)相鄰障礙物之間最短連線，并得出最短連線的中點(diǎn)坐標(biāo)，將結(jié)果進(jìn)行保存。保存的數(shù)據(jù)結(jié)構(gòu)可以是[Zi，Zj，Lij，Mid[X，Y]，Lbj]，其中，Zi 為第 i 個障礙物，Zj 為第 j 個障礙物；Lij 障礙物Zi與Zj之間的最短距離；Mid[X，Y]:最短距離線段的中點(diǎn)坐標(biāo)；Lbj B點(diǎn)到各中心點(diǎn)之間的距離。從A點(diǎn)向B點(diǎn)進(jìn)行路徑搜索，當(dāng)A到B的方向上遇到障礙物Zx時，獲取該障礙物與其他障礙物之間的最短距離中心點(diǎn)Mid[X，Y]，并計(jì)算到在AB連線左邊的最短中心點(diǎn)C[XI，Yl]，在AB連線右邊的最短中心點(diǎn)D[X2, Y2]。使用二叉樹法，分別以左分叉點(diǎn)C為起始點(diǎn)，以垂直與C所在的最短連線的方向繼續(xù)搜索路徑；采用同樣的方法，計(jì)算兩條分支線上E、F，前進(jìn)方向遇到的障礙物，并進(jìn)行標(biāo)識，計(jì)算X3、Y3，和X4、Y4的坐標(biāo)值。如果前進(jìn)方向上不存在無障礙物，則一直行駛到入場區(qū)域的邊緣，如圖中的a，d兩點(diǎn)，然后在連線該點(diǎn)與B點(diǎn)，依據(jù)以上方法進(jìn)行探索并繼續(xù)搜索路徑。如果前進(jìn)方向存在障礙物，但障礙物只有一側(cè)與其他障礙物有最短距離，如圖中點(diǎn)a，其前進(jìn)方向只有障礙物只有一側(cè)存在最短距離中點(diǎn)，此時采用以下三角形計(jì)算方法求解從a到障礙物一側(cè)c的路徑。點(diǎn)a到障礙物端點(diǎn)b的距離已知，而be為繞行障礙物時的安全距離，由當(dāng)ac與be垂直時,此時繞行距離最短，因此ac與ab的夾角為acrsin (bc/ab)，由此可以求解獲得c點(diǎn)的坐標(biāo)，獲得a處繞行障礙物的路徑。當(dāng)路徑到達(dá)目的點(diǎn)時，認(rèn)定搜索完畢一條路徑，保存本條路徑，然后回到上一個分叉節(jié)點(diǎn)，搜索其他該節(jié)點(diǎn)右分叉點(diǎn)后的其他路徑，一直搜索全部節(jié)點(diǎn)為止。記錄每條路徑經(jīng)過的中心點(diǎn)坐標(biāo),并保存所有坐標(biāo)。在工程機(jī)械轉(zhuǎn)彎過程中，障礙物之間最短距離必須大于外輪廓的最大轉(zhuǎn)彎半徑Rmax與內(nèi)輪廓的最小半徑Rmin之差(Rmax-Rmin)+X,其中X為可以預(yù)先設(shè)置的轉(zhuǎn)彎安全距離裕值。當(dāng)障礙物物之間的距離小于(Rmax-Rmin) +X時，則說明工程機(jī)械不能通過這兩個障礙物之間的空間，該條路徑需要拋棄。如果通過比較，該路徑上所有障礙物之間的距離都大于(Rmax-Rmin) +X，則可以認(rèn)為該路徑暢通。并對所有路徑的距離值進(jìn)行排序輸出。步驟S24計(jì)算工程機(jī)械按照各條路徑入場需要行進(jìn)的距離可以具體包括計(jì)算各條路徑中工程機(jī)械轉(zhuǎn)彎的中心點(diǎn)、半徑、和轉(zhuǎn)向位置；分別對各條路徑中工程機(jī)械轉(zhuǎn)彎的弧線距離進(jìn)行累加得到各條路徑入場需要行進(jìn)的距離。下面結(jié)合附圖6和圖7，對工程機(jī)械穿越障礙物的具體路徑及行進(jìn)距離進(jìn)行說明，圖6是根據(jù)本發(fā)明實(shí)施例的工程機(jī)械入場路徑規(guī)劃方法的穿越障礙物的具體路徑的示意圖，圖7是根據(jù)本發(fā)明實(shí)施例的工程機(jī)械入場路徑規(guī)劃方法的穿越障礙物的具體路徑的流程圖，如圖所示，目前路徑中，工程機(jī)械需要從A點(diǎn)首先穿過障礙物Zl和障礙物Z2之間的間隙，然后穿過障礙物Z2和障礙物Z3之間的間隙，障礙物Zl和障礙物Z2之間最短連線BC線段的中點(diǎn)為F，障礙物Z2和障礙物Z3之間最短連線DE線段的中點(diǎn)為G。步驟S71，求取ED線段延長線與BC線段延長線的交點(diǎn)01，比較線段OlG和線段OlF長度，記錄兩者較大的數(shù)值為Rl，較小數(shù)值記錄為R2 ；
步驟S72，判斷Rl是否大于工程機(jī)械的轉(zhuǎn)彎半徑，如果R小于工程機(jī)械的轉(zhuǎn)彎半徑，則可以確定工程機(jī)械無法通過這三個障礙之間的縫隙，該條路徑需要放棄；步驟S73，如果Rl大于工程機(jī)械的轉(zhuǎn)彎半徑，延長OlF至02，使得0102=R1+R2，分別以01和02為圓心，以Rl為半徑做弧，獲取兩條弧線的交點(diǎn)H ；步驟S74，判斷角度Z GHF的補(bǔ)角是否大于工程機(jī)械的最小轉(zhuǎn)彎角度，如果Z GHF的補(bǔ)角大于工程機(jī)械的最小轉(zhuǎn)彎角度，則認(rèn)為從FHG之間的弧線為工程機(jī)械穿越障礙物的具體路徑；步驟S75，如果Z GHF的補(bǔ)角小于工程機(jī)械的最小轉(zhuǎn)彎角度，判斷當(dāng)前的轉(zhuǎn)彎半徑是否大于5倍的R1，如果當(dāng)前的轉(zhuǎn)彎半徑大于5倍的R1，則可以確定工程機(jī)械無法通過這三個障礙之間的縫隙，該條路徑需要放棄；步驟S76，如果當(dāng)前的轉(zhuǎn)彎半徑小于5倍的Rl，分別沿⑶的方向和FC的方向，以預(yù)設(shè)的增值A(chǔ) R尋找新的01點(diǎn)和02點(diǎn)，使得GOl=GOl+ A R，F(xiàn)02=G01, A R為預(yù)先設(shè)定的
增值；重復(fù)步驟S73，直至得到弧線路徑或者得出工程機(jī)械無法通過的結(jié)論。通過以上步驟得到的FHG之間的弧線為工程機(jī)械穿越障礙物的具體路徑，對以上圓弧、圓心、轉(zhuǎn)向交點(diǎn)、以及行進(jìn)距離進(jìn)行保存，可以為工程機(jī)械入場時提供參考。得出所有可能行進(jìn)的路徑后，統(tǒng)計(jì)各條路徑的行進(jìn)距離，轉(zhuǎn)向次數(shù)，以及工程機(jī)械通過障礙物時與障礙物的最小距離。制作成路徑選擇表，表I是本根據(jù)本發(fā)明實(shí)施例的工程機(jī)械入場路徑規(guī)劃方法的路徑選擇表。表I路徑選擇表
權(quán)利要求
1.一種工程機(jī)械入場路徑規(guī)劃方法，其特征在于，包括 將入場區(qū)域內(nèi)的障礙物的三維模型投影到水平面上，得到所述障礙物的二維投影輪廓； 利用所述二維投影輪廓分別計(jì)算每兩個相鄰所述障礙物之間的最短連線，并標(biāo)記所述最短連線的中點(diǎn)； 計(jì)算工程機(jī)械經(jīng)由所述最短連線的中點(diǎn)到達(dá)目標(biāo)位置的各條路徑，并對所述各條路徑進(jìn)行路徑規(guī)劃計(jì)算； 分別計(jì)算工程機(jī)械按照經(jīng)過路徑計(jì)算的各條路徑入場需要行進(jìn)的距離、轉(zhuǎn)彎次數(shù)、和所述工程機(jī)械通過障礙物時與障礙物的最小距離； 按照所述需要行進(jìn)的距離、轉(zhuǎn)彎次數(shù)、和所述工程機(jī)械通過障礙物時與障礙物的最小距離對經(jīng)過路徑計(jì)算的各條路徑進(jìn)行排序，并根據(jù)排序結(jié)果輸出各條入場路徑。
2.根據(jù)權(quán)利要求1所述的工程機(jī)械入場路徑規(guī)劃方法，其特征在于，利用所述二維投影輪廓分別計(jì)算每兩個相鄰所述障礙物之間的最短連線包括 使用層次包圍盒法分別對每個障礙物的二維投影輪廓進(jìn)行計(jì)算，得到每個障礙物的包圍盒輪廓； 分別計(jì)算每兩個相鄰所述障礙物的包圍盒輪廓之間的最短連線，作為每兩個相鄰所述障礙物之間的最短連線。
3.根據(jù)權(quán)利要求2所述的工程機(jī)械入場路徑規(guī)劃方法，其特征在于， 使用層次包圍盒法分別對每個障礙物的二維投影輪廓進(jìn)行計(jì)算，得到每個障礙物的包圍盒輪廓包括使用方向包圍盒對每個障礙物的二維投影輪廓進(jìn)行計(jì)算，得到所述每個障礙物的矩形輪廓； 分別計(jì)算每兩個相鄰所述障礙物的包圍盒輪廓之間的最短連線包括分別計(jì)算每兩個相鄰所述障礙物的矩形輪廓之間的最短連線。
4.根據(jù)權(quán)利要求1所述的工程機(jī)械入場路徑規(guī)劃方法，其特征在于，計(jì)算工程機(jī)械經(jīng)由所述最短連線的中點(diǎn)到達(dá)目標(biāo)位置的各條路徑包括 沿當(dāng)前點(diǎn)向目標(biāo)點(diǎn)的前進(jìn)方向進(jìn)行路徑探索； 當(dāng)前進(jìn)方向上遇到障礙物時，判斷遇到的障礙物的至少一側(cè)是否存在該障礙物到與該障礙物相鄰的障礙物的最短連線； 當(dāng)遇到的障礙物的至少一側(cè)存在該障礙物到與該障礙物相鄰的障礙物的最短連線時，通過所述最短連線的中點(diǎn)向目標(biāo)點(diǎn)搜索路徑，并記錄當(dāng)前點(diǎn)到所述中點(diǎn)的路徑。
5.根據(jù)權(quán)利要求4所述的工程機(jī)械入場路徑規(guī)劃方法，其特征在于，判斷遇到的障礙物的至少一側(cè)是否存在該障礙物到與該障礙物相鄰的障礙物的最短連線之后還包括 當(dāng)所述障礙物的至少一側(cè)不存在該障礙物到與該障礙物相鄰的障礙物的最短連線時，根據(jù)所述工程機(jī)械繞行障礙物的安全距離查找距離最短的轉(zhuǎn)向點(diǎn)，通過所述轉(zhuǎn)向點(diǎn)向目標(biāo)點(diǎn)搜索路徑，并記錄當(dāng)前點(diǎn)到轉(zhuǎn)向點(diǎn)的路徑。
6.根據(jù)權(quán)利要求1所述的工程機(jī)械入場路徑規(guī)劃方法，其特征在于，對所述各條路徑進(jìn)行路徑規(guī)劃計(jì)算之后還包括 分別判斷經(jīng)過路徑計(jì)算的各條路徑中所有最短連線的距離是否都大于所述工程機(jī)械的轉(zhuǎn)彎安全距離；刪除出現(xiàn)最短連線的距離小于所述工程機(jī)械的轉(zhuǎn)彎安全距離的路徑。
7.根據(jù)權(quán)利要求6所述的工程機(jī)械入場路徑規(guī)劃方法，其特征在于，所述工程機(jī)械的轉(zhuǎn)彎安全距離為工程機(jī)械外輪廓的最大轉(zhuǎn)彎半徑與內(nèi)輪廓的最小半徑之差加上預(yù)設(shè)裕值。
8.根據(jù)權(quán)利要求1所述的工程機(jī)械入場路徑規(guī)劃方法，其特征在于，分別計(jì)算工程機(jī)械按照經(jīng)過路徑計(jì)算的各條路徑入場需要行進(jìn)的距離包括 計(jì)算經(jīng)過路徑計(jì)算的各條路徑中工程機(jī)械轉(zhuǎn)彎的中心點(diǎn)、半徑、和轉(zhuǎn)向位置； 分別對經(jīng)過路徑計(jì)算的各條路徑中工程機(jī)械轉(zhuǎn)彎的弧線距離進(jìn)行累加得到所述各條路徑入場需要行進(jìn)的距離。
9.根據(jù)權(quán)利要求1至8中任一項(xiàng)所述的工程機(jī)械入場路徑規(guī)劃方法，其特征在于，計(jì)算工程機(jī)械經(jīng)由所述最短連線的中點(diǎn)到達(dá)目標(biāo)位置的各條路徑包括 使用二叉樹算法遍歷計(jì)算工程機(jī)械經(jīng)由所述最短連線的中點(diǎn)到達(dá)目標(biāo)位置的各條路徑。
10.根據(jù)權(quán)利要求1至8中任一項(xiàng)所述的工程機(jī)械入場路徑規(guī)劃方法，其特征在于，對所述各條路徑進(jìn)行路徑規(guī)劃計(jì)算包括 使用迪杰斯特拉算法對所述各條路徑分別進(jìn)行路徑規(guī)劃計(jì)算。
11.一種工程機(jī)械入場路徑規(guī)劃裝置，其特征在于，包括 投影模塊，用于將入場區(qū)域內(nèi)的障礙物的三維模型投影到水平面上，得到所述障礙物的二維投影輪廓； 最短距離中點(diǎn)計(jì)算模塊，用于利用所述二維投影輪廓分別計(jì)算每兩個相鄰所述障礙物之間的最短連線，并標(biāo)記所述最短連線的中點(diǎn)； 路徑搜索模塊，用于計(jì)算工程機(jī)械經(jīng)由所述最短連線的中點(diǎn)到達(dá)目標(biāo)位置的各條路徑，并對所述各條路徑進(jìn)行路徑規(guī)劃計(jì)算； 路徑分析模塊，用于分別計(jì)算工程機(jī)械按照經(jīng)過路徑計(jì)算的各條路徑入場需要行進(jìn)的距離、轉(zhuǎn)彎次數(shù)、和所述工程機(jī)械通過障礙物時與障礙物的最小距離； 路徑優(yōu)化模塊，用于按照所述需要行進(jìn)的距離、轉(zhuǎn)彎次數(shù)、和和所述工程機(jī)械通過障礙物時與障礙物的最小距離對經(jīng)過路徑計(jì)算的各條路徑進(jìn)行排序，并根據(jù)排序結(jié)果輸出各條入場路徑。
12.根據(jù)權(quán)利要求11所述的工程機(jī)械入場路徑規(guī)劃裝置，其特征在于，還包括 路徑安全評價模塊，用于分別判斷各條路徑中所有最短連線的距離是否都大于所述工程機(jī)械的轉(zhuǎn)彎安全距離，并刪除出現(xiàn)最短連線的距離小于所述工程機(jī)械的轉(zhuǎn)彎安全距離的路徑。
全文摘要
本發(fā)明提供了一種工程機(jī)械入場路徑規(guī)劃方法和路徑規(guī)劃裝置。該工程機(jī)械的入場路徑規(guī)劃方法包括將入場區(qū)域內(nèi)的障礙物的三維模型投影到水平面上，得到障礙物的二維投影輪廓；利用二維投影輪廓分別計(jì)算每兩個相鄰障礙物之間的最短連線，并標(biāo)記最短連線的中點(diǎn)；計(jì)算工程機(jī)械經(jīng)由最短連線的中點(diǎn)到達(dá)目標(biāo)位置的各條路徑，并對所述各條路徑進(jìn)行路徑規(guī)劃計(jì)算；分別計(jì)算工程機(jī)械按照經(jīng)過路徑計(jì)算的各條路徑入場需要行進(jìn)的距離、轉(zhuǎn)彎次數(shù)、和工程機(jī)械通過障礙物時與障礙物的最小距離；按照需要行進(jìn)的距離、轉(zhuǎn)彎次數(shù)、和工程機(jī)械通過障礙物時與障礙物的最小距離對經(jīng)過路徑計(jì)算的各條路徑進(jìn)行排序，并輸出結(jié)果。從而最終得出最優(yōu)路徑方案。
文檔編號G01C21/00GK103017757SQ20121052028
公開日2013年4月3日 申請日期2012年12月6日 優(yōu)先權(quán)日2012年12月6日
發(fā)明者曾楊, 黃文華, 范卿, 李義 申請人:中聯(lián)重科股份有限公司