[發(fā)明專利]一種三維環(huán)境下的無人機(jī)路徑規(guī)劃方法

有效

• 申請?zhí)枺?/td>: 202010579045.4
• 申請日: 2020-06-23
• 公開（公告）號: CN111707269B
• 公開（公告）日: 2022-04-05
• 發(fā)明（設(shè)計(jì)）人: 程向紅;王彤;吳峻;馬自應(yīng);李丹若;常興國;梁平昊;麻寧杰;王森
• 申請（專利權(quán)）人: 東南大學(xué)
• 主分類號: G01C21/20
• 分類號: G01C21/20;G05D1/10
• 代理公司: 南京眾聯(lián)專利代理有限公司 32206
• 代理人: 許小莉

1.一種三維環(huán)境下的無人機(jī)路徑規(guī)劃方法，其特征在于，該方法包含以下步驟：

(1)環(huán)境建模：對無人機(jī)飛行的三維空間劃分柵格并使用唯一柵格索引標(biāo)識，對空間中的障礙物建立唯一索引標(biāo)識，并計(jì)算每一障礙物的包圍盒作為確定受影響柵格的依據(jù)，將對應(yīng)的障礙索引保存至所有與包圍盒相交的柵格中；

(2)路徑搜索：分別將無人機(jī)當(dāng)前位置及目標(biāo)位置設(shè)為起始點(diǎn)和目標(biāo)點(diǎn)，建立一條由起始點(diǎn)到目標(biāo)點(diǎn)不與障礙物包圍盒相碰撞的生成路徑；

(3)對步驟(2)中生成路徑進(jìn)行簡化并使用樣條曲線對簡化后生成路徑進(jìn)行平滑即得到用于無人機(jī)控制的最終飛行軌跡；

步驟(2)中所述路徑搜索的具體方法是：

(21)初始化生成路徑為空，將起始點(diǎn)作為當(dāng)前點(diǎn)并保存于生成路徑中；

(22)設(shè)置最大擴(kuò)展步長S：

(23)若當(dāng)前點(diǎn)與目標(biāo)點(diǎn)距離小于S，將目標(biāo)點(diǎn)作為擴(kuò)展點(diǎn)；

若當(dāng)前點(diǎn)與目標(biāo)點(diǎn)距離大于或等于S，則將規(guī)劃路徑以概率p向目標(biāo)點(diǎn)進(jìn)行擴(kuò)展，概率1-p隨機(jī)選擇擴(kuò)展方向，其中擴(kuò)展距離小于或等于S，計(jì)算擴(kuò)展點(diǎn)可能到達(dá)的柵格并依據(jù)該柵格中保存的障礙物索引進(jìn)行擴(kuò)展路徑的碰撞檢測；

(24)倘若不發(fā)生碰撞，將生成路徑沿所選節(jié)點(diǎn)擴(kuò)展，選擇新加入的生成路徑點(diǎn)作為當(dāng)前點(diǎn)，倘若發(fā)生碰撞，優(yōu)先選擇上次隨機(jī)擴(kuò)展方向，直至有新節(jié)點(diǎn)加入；

(25)在限定時(shí)間T內(nèi)重復(fù)步驟(23)-步驟(24)，直至生成路徑擴(kuò)展至目標(biāo)點(diǎn)；

步驟(2)中所述碰撞檢測的具體方法是：

對于球形障礙物，只需判斷點(diǎn)到探索路徑P₁ P₂的最短距離是否小于球體半徑r，依次判斷當(dāng)前點(diǎn)和目標(biāo)點(diǎn)到球心的歐式距離是否小于半徑，對于當(dāng)前點(diǎn)P₁(x₁，y₁，z₁)，目標(biāo)點(diǎn)P₂(x₂，y₂，z₂)和球心P₀(x₀，y₀，z₀)，判斷探索路徑P₁ P₂發(fā)生碰撞需要計(jì)算式(1-4)：

若P₁與P₀的距離l₁或P₂與P₀的距離l₂小于r，可以判斷線段P₁ P₂與球體碰撞；若l₁與l₂都大于r但P₁與P₂所在直線到球心的距離d小于r，比較P₁ P₀在線段P₁ P₂上的投影h長度是否小于P₁ P₂的長度，當(dāng)且僅當(dāng)小于時(shí)線段P₁ P₂才與球體碰撞；其余情況線段P₁ P₂不與球體碰撞；

對于空間多面體，除采用上述方法用包圍球進(jìn)行快速碰撞檢測外，還要對每個(gè)面進(jìn)行線段和面的相交檢測，具體過程如下：

對于當(dāng)前點(diǎn)P₁(x₁，y₁，z₁)，目標(biāo)點(diǎn)P₂(x₂，y₂，z₂)，根據(jù)中心點(diǎn)，尺寸和姿態(tài)信息計(jì)算障礙物頂點(diǎn)坐標(biāo)，對于頂點(diǎn)圍成的每個(gè)外表面，倘若中心點(diǎn)始終與其中一個(gè)點(diǎn)在面的同一側(cè)，則其中一點(diǎn)位于障礙物內(nèi)，線段P₁ P₂與其必然發(fā)生碰撞。假設(shè)平面三角形由三點(diǎn)P₃(x₃，y₃，z₃)，P₄(x₄，y₄，z₄)，P₅(x₅，y₅，z₅)確定，計(jì)算式(5-6)：

若混合積s₁，s₂的符號相同，說明P₁，P₂位于面同一側(cè)；

繼續(xù)判斷當(dāng)前點(diǎn)和目標(biāo)點(diǎn)的同側(cè)性，對兩點(diǎn)處于異側(cè)的三角形進(jìn)行相交檢測，可用重心法進(jìn)行相交判斷，假設(shè)三角形內(nèi)點(diǎn)O為線段P₁ P₂與三角形P₃P₄P₅的交點(diǎn)，則點(diǎn)O可表示為式(7)：

O＝(1-u-v)P₃+uP₄+vP₅ (7)

其中，u、v為變量且0≤u≤1，0≤v≤1，0≤u+v≤1，線段P₁ P₂可表示為式(8)：

其中，t為變量且0≤t≤1；

則線段是否與三角形相交轉(zhuǎn)換為聯(lián)立式(9)是否有解的問題，由矩陣計(jì)算得：

若計(jì)算結(jié)果u，v取值范圍在0-1之間且0≤u+v≤1，則發(fā)生碰撞；

步驟(1)中所述環(huán)境建模具體包括如下步驟：

(11)對尺寸為(x，y，z)的三維空間進(jìn)行柵格大小G的劃分，對每一柵格使用唯一柵格索引標(biāo)識；

(12)將空間中的障礙物抽象為由位姿，縮放向量和形狀信息表示的幾何物體并保存，每一障礙物使用唯一障礙索引標(biāo)識；

(13)增加每一障礙物的縮放向量使其尺寸擴(kuò)大一定值L并計(jì)算其包圍盒，將對應(yīng)的障礙索引保存至所有與包圍盒相交的柵格中；

步驟(13)中對于球體障礙物利用球心坐標(biāo)加減半徑計(jì)算軸向包圍盒；對立方體障礙物，比較各個(gè)頂點(diǎn)在三個(gè)方向上的極值獲取軸向包圍盒，根據(jù)軸向包圍盒的坐標(biāo)范圍確定其影響的柵格范圍，將障礙物索引保存到對應(yīng)的柵格中。