狹窄空間內(nèi)基于隨機(jī)采樣的無(wú)人機(jī)軌跡規(guī)劃方法
【技術(shù)領(lǐng)域】
[0001] 本發(fā)明涉及一種狹窄空間內(nèi)基于隨機(jī)采樣的無(wú)人機(jī)軌跡規(guī)劃方法，屬于無(wú)人機(jī)軌 跡規(guī)劃領(lǐng)域。
【背景技術(shù)】
[0002] 無(wú)人機(jī)軌跡規(guī)劃的目的在于根據(jù)無(wú)人機(jī)的任務(wù)目標(biāo)、環(huán)境信息和約束，在環(huán)境中 尋找滿足某種最優(yōu)性能指標(biāo)和約束的、從無(wú)人機(jī)的起始狀態(tài)到任務(wù)目標(biāo)的連續(xù)飛行軌跡。 軌跡規(guī)劃是無(wú)人機(jī)導(dǎo)航與控制領(lǐng)域的重要問(wèn)題，也是實(shí)現(xiàn)無(wú)人機(jī)自主飛行的關(guān)鍵技術(shù)之 〇
[0003] 非結(jié)構(gòu)化復(fù)雜三維環(huán)境廣泛存在于無(wú)人機(jī)環(huán)境探索、目標(biāo)搜尋和監(jiān)測(cè)等等許多實(shí) 際應(yīng)用中?，F(xiàn)實(shí)中的眾多任務(wù)場(chǎng)景（如復(fù)雜室內(nèi)空間、倒塌的建筑物內(nèi)部、礦井等）均屬于 此類(lèi)非結(jié)構(gòu)化復(fù)雜三維環(huán)境。該類(lèi)環(huán)境一般存在狹窄空間、非凸結(jié)構(gòu)障礙物以及復(fù)雜的環(huán) 境結(jié)構(gòu)，因此任務(wù)環(huán)境中自由空間與障礙物區(qū)域的相對(duì)關(guān)系較為復(fù)雜，空間的連通性較弱。 無(wú)人機(jī)在存在狹窄空間的非結(jié)構(gòu)化復(fù)雜三維環(huán)境下執(zhí)行任務(wù)，要求無(wú)人機(jī)系統(tǒng)必須能夠有 效地識(shí)別自由空間與障礙物，并規(guī)劃出避開(kāi)障礙物的連續(xù)軌跡。
[0004] 傳統(tǒng)的基于隨機(jī)采樣的軌跡規(guī)劃方法（如概率路徑圖方法（PRM)和快速擴(kuò)展隨機(jī) 樹(shù)（RRT)等）雖然在解決存在動(dòng)力學(xué)/運(yùn)動(dòng)學(xué)約束和高維空間的規(guī)劃問(wèn)題中應(yīng)用較為廣 泛，但由于該類(lèi)方法一般在空間中采用均勻采樣策略，在存在狹窄空間的復(fù)雜三維環(huán)境中 往往無(wú)法從復(fù)雜的結(jié)構(gòu)中有效辨識(shí)出自由空間；且難以在連通性較差的復(fù)雜空間中實(shí)現(xiàn)軌 跡的擴(kuò)展，軌跡規(guī)劃的整體計(jì)算效率較低。

【發(fā)明內(nèi)容】

[0005] 本發(fā)明的目的在于解決上述技術(shù)的不足之處，提出一種狹窄空間內(nèi)基于隨機(jī)采樣 的無(wú)人機(jī)軌跡規(guī)劃方法，可有效提高狹窄空間內(nèi)無(wú)人機(jī)軌跡規(guī)劃的計(jì)算效率。
[0006] 本發(fā)明提出的一種狹窄空間內(nèi)基于隨機(jī)采樣的無(wú)人機(jī)軌跡規(guī)劃方法，其特征在 于，該方法具體包括以下步驟：
[0007] S101 :建立飛行環(huán)境三維模型和設(shè)置軌跡規(guī)劃初始參數(shù)：
[0008] 獲取無(wú)人機(jī)飛行環(huán)境信息，并根據(jù)所述飛行環(huán)境信息建立以多面體形式表達(dá)的飛 行環(huán)境的三維模型；所述飛行環(huán)境包含自由空間和障礙物；
[0009] 確定無(wú)人機(jī)的初始狀態(tài)和目標(biāo)狀態(tài)，無(wú)人機(jī)的初始狀態(tài)和目標(biāo)狀態(tài)包括無(wú)人機(jī)的 位置、速度和相對(duì)于飛行環(huán)境的姿態(tài)；
[0010] 并將所述無(wú)人機(jī)的初始狀態(tài)、目標(biāo)狀態(tài)和飛行環(huán)境的三維模型作為軌跡規(guī)劃的初 始參數(shù)；
[0011] S102 :根據(jù)所述飛行環(huán)境的三維模型進(jìn)行路標(biāo)點(diǎn)采樣：
[0012] 在步驟S101建立的飛行環(huán)境的三維模型中通過(guò)隨機(jī)星型-均勻混合采樣策略判 斷并生成路標(biāo)點(diǎn)，并將所述路標(biāo)點(diǎn)分別存入狹窄空間N和空曠區(qū)域U所對(duì)應(yīng)的路標(biāo)點(diǎn)列表 NList 和 UList ;
[0013] S103 :以生成的路標(biāo)點(diǎn)和無(wú)人機(jī)的初始狀態(tài)、目標(biāo)狀態(tài)為根節(jié)點(diǎn)，采用多快速擴(kuò)展 隨機(jī)樹(shù)進(jìn)行軌跡擴(kuò)展：
[0014] 以步驟S102中生成的路標(biāo)點(diǎn)列表NList和UList中的每一路標(biāo)點(diǎn)和所述無(wú)人機(jī) 的初始狀態(tài)、目標(biāo)狀態(tài)為RRT軌跡樹(shù)的根節(jié)點(diǎn)，在所述飛行環(huán)境的三維模型中進(jìn)行多條RRT 軌跡樹(shù)的擴(kuò)展，并將多條RRT軌跡樹(shù)兩兩連接成為一條融合后的RRT軌跡樹(shù)；
[0015] S104:生成完整軌跡
[0016] 將步驟103中融合后的RRT軌跡樹(shù)與所述無(wú)人機(jī)的初始狀態(tài)、目標(biāo)狀態(tài)相連接， 從而生成一條連接所述無(wú)人機(jī)的初始狀態(tài)和目標(biāo)狀態(tài)的完整軌跡，作為該無(wú)人機(jī)的飛行軌 跡。
[0017] 本發(fā)明提出的狹窄空間內(nèi)基于隨機(jī)采樣的無(wú)人機(jī)軌跡規(guī)劃方法，其特點(diǎn)和有益效 果在于：
[0018] 1.本發(fā)明采用的隨機(jī)星型-均勻混合采樣策略結(jié)合了隨機(jī)星型采樣和均勻采樣 的優(yōu)勢(shì)，一方面通過(guò)隨機(jī)星型采樣，可有效辨識(shí)非結(jié)構(gòu)化復(fù)雜三維環(huán)境中的狹窄空間，提高 狹窄空間內(nèi)的路標(biāo)點(diǎn)密度，并可區(qū)分環(huán)境中的障礙物死區(qū)和拐角結(jié)構(gòu)；另一方面，通過(guò)均勻 采樣，可同時(shí)在環(huán)境中的空曠區(qū)域產(chǎn)生一定數(shù)量的路標(biāo)點(diǎn)。因此，采用隨機(jī)星型-均勻混合 采樣策略能夠生成全局分布更為合理的路標(biāo)點(diǎn)，可適應(yīng)無(wú)人機(jī)飛行環(huán)境結(jié)構(gòu)的復(fù)雜性和多 樣性，有利于軌跡的擴(kuò)展。
[0019] 2.本發(fā)明基于多RRT軌跡擴(kuò)展實(shí)現(xiàn)無(wú)人機(jī)軌跡規(guī)劃，以分布在無(wú)人機(jī)飛行環(huán)境中 不同區(qū)域的多個(gè)路標(biāo)點(diǎn)為根節(jié)點(diǎn)同時(shí)進(jìn)行多條RRT軌跡樹(shù)的擴(kuò)展，可有效提高狹窄空間內(nèi) 無(wú)人機(jī)軌跡規(guī)劃的計(jì)算效率。
【附圖說(shuō)明】
[0020] 圖1為根據(jù)本發(fā)明提出的狹窄空間內(nèi)基于隨機(jī)采樣的無(wú)人機(jī)軌跡規(guī)劃方法的總 體流程框圖；
[0021] 圖2為本發(fā)明一個(gè)實(shí)施例的路標(biāo)點(diǎn)采樣的流程框圖；
[0022] 圖3為本實(shí)施例方法中所述隨機(jī)星型采樣策略對(duì)位于狹窄空間內(nèi)的位形點(diǎn)（左a) 和位于非狹窄空間內(nèi)的位形點(diǎn)（右b)的判斷示意圖；
[0023] 圖4為本實(shí)施例的多RRT軌跡擴(kuò)展示意圖；
[0024] 圖5為本實(shí)施例的軌跡生成示意圖。
【具體實(shí)施方式】
[0025] 下面結(jié)合附圖和實(shí)施例詳細(xì)描述本發(fā)明的【具體實(shí)施方式】。
[0026] 本發(fā)明提出的一種狹窄空間內(nèi)基于隨機(jī)采樣的無(wú)人機(jī)軌跡規(guī)劃方法總體流程如 圖1所示，具體包括以下步驟：
[0027] S101 :建立飛行環(huán)境三維模型和設(shè)置軌跡規(guī)劃初始參數(shù)：
[0028] 獲取無(wú)人機(jī)飛行環(huán)境信息，并根據(jù)所述飛行環(huán)境信息建立以多面體形式表達(dá)的飛 行環(huán)境的三維模型；所述飛行環(huán)境包含自由空間和障礙物；
[0029] 確定無(wú)人機(jī)的初始狀態(tài)和目標(biāo)狀態(tài)，無(wú)人機(jī)的初始狀態(tài)和目標(biāo)狀態(tài)包括無(wú)人機(jī)的 位置、速度和相對(duì)于飛行環(huán)境的姿態(tài)；
[0030] 并將所述無(wú)人機(jī)的初始狀態(tài)、目標(biāo)狀態(tài)和飛行環(huán)境的三維模型作為軌跡規(guī)劃的初 始參數(shù)；
[0031] S102 :根據(jù)所述飛行環(huán)境的三維模型進(jìn)行路標(biāo)點(diǎn)采樣：
[0032] 在步驟S101建立的飛行環(huán)境的三維模型中通過(guò)隨機(jī)星型-均勻混合采樣策略生 成路標(biāo)點(diǎn)，判斷并將所述路標(biāo)點(diǎn)分別存入狹窄空間N和空曠區(qū)域U所對(duì)應(yīng)的路標(biāo)點(diǎn)列表 NList 和 UList ;
[0033] S103 :以生成的路標(biāo)點(diǎn)和無(wú)人機(jī)的初始狀態(tài)、目標(biāo)狀態(tài)為根節(jié)點(diǎn)，采用多RRT(快 速擴(kuò)展隨機(jī)樹(shù)）進(jìn)行軌跡擴(kuò)展：
[0034] 以步驟S102中生成的路標(biāo)點(diǎn)列表NList和UList中的每一路標(biāo)點(diǎn)和所述無(wú)人機(jī) 的初始狀態(tài)、目標(biāo)狀態(tài)為RRT軌跡樹(shù)的根節(jié)點(diǎn)，在所述飛行環(huán)境的三維模型中進(jìn)行多條RRT 軌跡樹(shù)的擴(kuò)展，并將多條RRT軌跡樹(shù)兩兩連接成為一條融合后的RRT軌跡樹(shù)；
[0035] S104 :生成完整軌跡
[0036] 將步驟103中融合后的RRT軌跡樹(shù)與所述無(wú)人機(jī)的初始狀態(tài)、目標(biāo)狀態(tài)相連接， 從而生成一條連接所述無(wú)人機(jī)的初始狀態(tài)和目標(biāo)狀態(tài)的完整軌跡，作為該無(wú)人機(jī)的飛行軌 跡。
[0037] 上述方法的各步驟具體實(shí)現(xiàn)方法說(shuō)明如下；
[0038] 在步驟S101中，無(wú)人機(jī)通過(guò)機(jī)載RGB-D相機(jī)或激光測(cè)距儀等器件獲得無(wú)人機(jī)飛行 環(huán)境的深度信息，并以三維點(diǎn)云數(shù)組的形式存儲(chǔ)。其中三維點(diǎn)云數(shù)組中的每一點(diǎn)表征飛行 環(huán)境中的某一點(diǎn)相對(duì)于無(wú)人機(jī)的三維坐標(biāo)信息，該三維點(diǎn)云數(shù)組即包含了無(wú)人機(jī)飛行環(huán) 境中的各障礙物和環(huán)境結(jié)構(gòu)的信息。進(jìn)一步將三維點(diǎn)云數(shù)組轉(zhuǎn)換為以多面體形式表示的飛 行環(huán)境的三維模型，其中每個(gè)多面體以構(gòu)成該多面體的所有平面的形式存儲(chǔ)。每個(gè)多面體 即代表了無(wú)人機(jī)飛行環(huán)境中的不同障礙物和環(huán)境結(jié)構(gòu)。
[0039] 步驟102中通過(guò)隨機(jī)星型-均勻混合采樣策略生成路標(biāo)點(diǎn)的實(shí)現(xiàn)流程如圖2所 示，具體步驟包括：
[0040] S1021設(shè)定所需的總路標(biāo)點(diǎn)數(shù)ntotal，和權(quán)重系數(shù)ωΝ, ωμ，且ωΝ+ωμ= 1 ;
[0041] 在本實(shí)施例中，可根據(jù)無(wú)人機(jī)飛行環(huán)境的尺寸設(shè)置和計(jì)算時(shí)間要求設(shè)置總路標(biāo)點(diǎn) 數(shù)ntotal (本實(shí)施例中無(wú)人機(jī)的飛行環(huán)境尺寸為18mX6. 9mX8m，路標(biāo)點(diǎn)采樣步驟的計(jì)算時(shí) 間設(shè)置為80秒，則設(shè)置總路標(biāo)點(diǎn)數(shù)ntotal= 30)，并