[發(fā)明專利]一種基于視覺的室內(nèi)無人機(jī)路徑規(guī)劃方法

有效

• 申請(qǐng)?zhí)枺?/td>: 202011542716.6
• 申請(qǐng)日: 2020-12-22
• 公開（公告）號(hào): CN112747736B
• 公開（公告）日: 2022-07-08
• 發(fā)明（設(shè)計(jì)）人: 劉小雄;黃劍雄;高鵬程;張興旺;梁晨
• 申請(qǐng)（專利權(quán)）人: 西北工業(yè)大學(xué)
• 主分類號(hào): G01C21/00
• 分類號(hào): G01C21/00;G06T7/50;G06T7/73
• 代理公司: 西安凱多思知識(shí)產(chǎn)權(quán)代理事務(wù)所(普通合伙) 61290
• 代理人: 劉新瓊

1.一種基于視覺的室內(nèi)無人機(jī)路徑規(guī)劃方法，其特征在于，包括以下步驟：

步驟1：構(gòu)建點(diǎn)云地圖；

假設(shè)點(diǎn)云中的點(diǎn)P_w來自無人機(jī)相機(jī)獲取圖像，點(diǎn)P_w在世界坐標(biāo)系的坐標(biāo)為(x_w，y_w，z_w)，點(diǎn)P_w在圖像像素坐標(biāo)系對(duì)應(yīng)的坐標(biāo)為(u，v)，點(diǎn)P_w在世界坐標(biāo)系的rgb信息等于點(diǎn)P_w在圖像像素坐標(biāo)系中的投影點(diǎn)對(duì)應(yīng)的rgb信息，即：

假設(shè)無人機(jī)相機(jī)獲取圖像位姿為R_cw、t_cw，其中R_cw表示無人機(jī)相機(jī)獲取圖像在世界坐標(biāo)系中的位姿矩陣，t_cw表示無人機(jī)相機(jī)獲取圖像的平移矩陣，則點(diǎn)P_w在相機(jī)坐標(biāo)系下的投影P_c為：

P_c＝R_cwP_w+t_cw

假設(shè)相機(jī)內(nèi)參為K：

其中，f_x、f_y分別表示相機(jī)坐標(biāo)系x、y軸與圖像像素坐標(biāo)系x、y軸之間對(duì)應(yīng)的縮放倍率，c_x、c_y代表相機(jī)坐標(biāo)系與圖像像素坐標(biāo)系原點(diǎn)之間的偏移量；則P_c與(u，v)之間的關(guān)系為：

d[u，v，1]^T＝KP_c

其中，d代表點(diǎn)P_w在圖像像素坐標(biāo)系對(duì)應(yīng)的深度信息，[u，v，1]^T代表點(diǎn)P_w在圖像像素坐標(biāo)系對(duì)應(yīng)的齊次坐標(biāo)；

將點(diǎn)P_w在圖像像素坐標(biāo)系的坐標(biāo)還原到世界坐標(biāo)系，如下：

步驟2：點(diǎn)云濾波及降采樣；

點(diǎn)云共包括n個(gè)點(diǎn)，對(duì)各個(gè)點(diǎn)計(jì)算與其最鄰近的50個(gè)點(diǎn)到該點(diǎn)距離的平均值，即：

μ_i＝∑_ml_i/m，m＝50

其中，l_i為第i個(gè)點(diǎn)與其最鄰近的50個(gè)點(diǎn)的距離，μ_i為第i個(gè)點(diǎn)與其最鄰近的50個(gè)點(diǎn)的距離的平均距離，i＝1，...，n，由此得到n個(gè)平均距離；假設(shè)平均距離符合正態(tài)分布N(μ_a，σ_a)，則將n個(gè)平均距離作為正態(tài)分布N(μ_a，σ_a)的樣本，估計(jì)正態(tài)分布N(μ_a，σ_a) 的期望與方差，期望計(jì)算公式如下：

μ_a＝∑_nμ_i/(n-1)

方差的計(jì)算公式如下：

σ_a＝∑_n(μ_i-μ_a)²/(n-1)

若任一點(diǎn)的平均距離滿足：

μ_i＞μ_a+ασ_a

則認(rèn)為該點(diǎn)為離群點(diǎn)，即外點(diǎn)，需要從點(diǎn)云中剔除；其中，α為設(shè)定的閾值；

將點(diǎn)云劃分為多個(gè)相同長(zhǎng)寬高的體素空間，每個(gè)體素空間中存在k個(gè)點(diǎn)，坐標(biāo)分別為(x_i，y_i，z_i)，i＝1，...，k，則該體素空間的坐標(biāo)表示為：(∑_kx_i/k，∑_ky_i/k，∑_kz_i/k)；

即體素空間的坐標(biāo)為這個(gè)體素空間中包含的所有點(diǎn)的位置重心，并用位置重心近似表達(dá)這個(gè)體素空間中包含的所有點(diǎn)的空間位置；求取所有體素空間的重心，將點(diǎn)云降采樣為由各體素空間的重心組成；

步驟3：構(gòu)建八叉樹地圖；

將能夠恰好覆蓋完整點(diǎn)云的正方體空間不斷八等分，直至達(dá)到預(yù)設(shè)的分辨率，得到一個(gè)完整的八叉樹；

假定初始時(shí)刻任一葉子節(jié)點(diǎn)的占據(jù)次數(shù)為用y表示，即初始時(shí)刻：

y＝0

當(dāng)觀測(cè)到該葉子節(jié)點(diǎn)被占據(jù)時(shí)，即該葉子節(jié)點(diǎn)包含的空間中存在點(diǎn)云，則將y加1；反之，如果觀測(cè)到該葉子節(jié)點(diǎn)空閑，即該葉子節(jié)點(diǎn)處于當(dāng)前相機(jī)光心到被占據(jù)葉子節(jié)點(diǎn)的連線上時(shí)，則將y減1；

用概率的方式表示該葉子節(jié)點(diǎn)的占據(jù)情況，用x表示占據(jù)情況，則有：

x＝e^y/(1+e^y)

在初始時(shí)刻，x的值為0.5，表示當(dāng)前該葉子節(jié)點(diǎn)的狀態(tài)為未知，即為探索區(qū)域；當(dāng)觀測(cè)到該葉子節(jié)點(diǎn)被占據(jù)的次數(shù)增多時(shí)，則x逐漸增大，當(dāng)大于設(shè)定的高閾值，則判斷該葉子節(jié)點(diǎn)被占據(jù)；當(dāng)觀測(cè)到該葉子節(jié)點(diǎn)為空閑的次數(shù)增多時(shí)，則x逐漸減小，當(dāng)小于設(shè)定的低閾值時(shí)，則判斷該葉子節(jié)點(diǎn)為空閑，完成點(diǎn)云八叉樹地圖構(gòu)建；

步驟4：構(gòu)建探索地圖；

以無人機(jī)為球心構(gòu)建半徑可變的三維探索球，并將三維探索球分成多個(gè)大小相等的正方體，將這些正方體稱為三維柵格；

三維柵格分為普通柵格和障礙物柵格；所述普通柵格為既不包含障礙物信息，也不包含目標(biāo)信息的柵格；

障礙物柵格由八叉樹地圖中節(jié)點(diǎn)的占據(jù)信息確定，具體如下：如果八叉樹節(jié)點(diǎn)被占據(jù)，說明該八叉樹節(jié)點(diǎn)在八叉樹地圖中對(duì)應(yīng)的三維柵格被障礙物占據(jù)，由于八叉樹地圖中的三維柵格與三維探索球中的三維柵格存在空間不匹配現(xiàn)象，當(dāng)確定該三維柵格被障礙物占據(jù)后，按照公式(1)將八叉樹地圖中的三維柵格與三維探索球中的三維柵格進(jìn)行匹配，找到三維探索球中的障礙物柵格P_os(x_os，y_os，z_os)：

其中，P_d(x_d，y_d，z_d)為當(dāng)前無人機(jī)在導(dǎo)航坐標(biāo)系的三維位置坐標(biāo)，P_o(x_o，y_o，z_o)為障礙物在導(dǎo)航坐標(biāo)系的三維位置坐標(biāo)，r為八叉樹分辨率；

定義目標(biāo)點(diǎn)在導(dǎo)航坐標(biāo)系下的位置坐標(biāo)為P_t(x_t，y_t，z_t)，三維探索球半徑為R，得到目標(biāo)點(diǎn)到當(dāng)前無人機(jī)間的距離如下：

當(dāng)l＞R時(shí)，則目標(biāo)點(diǎn)在探索球外部，通過公式(2)得到目標(biāo)點(diǎn)在球面的投影，即虛擬目標(biāo)點(diǎn)柵格，虛擬目標(biāo)點(diǎn)柵格的三維位置為P_ts(x_ts，y_ts，z_ts)：

當(dāng)l≤R時(shí)，則目標(biāo)點(diǎn)在探索球內(nèi)部，通過公式(3)得到包含目標(biāo)點(diǎn)的三維柵格，即虛擬目標(biāo)點(diǎn)柵格，虛擬目標(biāo)點(diǎn)柵格的三維位置為P_ts(x_ts，y_ts，z_ts)：

由此得到室內(nèi)路徑規(guī)劃的導(dǎo)航地圖，定義為探索地圖；

步驟5：計(jì)算地勢(shì)值；

由虛擬目標(biāo)點(diǎn)柵格在探索地圖中的位置信息，求得探索地圖內(nèi)距離虛擬目標(biāo)點(diǎn)柵格最遠(yuǎn)的柵格點(diǎn)，及兩者之間的直線距離d_max，得到VH_MAX：

VH_MAX＝λ_vh×d_max

其中，λ_vh是預(yù)設(shè)的常數(shù)，λ_vh≥2；VH_MAX是探索地圖中的最大地勢(shì)值；

計(jì)算虛擬目標(biāo)點(diǎn)柵格對(duì)特定柵格點(diǎn)(x，y，z)的地勢(shì)影響為：

VH_g(x，y，z)＝K_αd_x+K_βd_y+K_γd_z

其中，d_x、d_y、d_z分別為特定柵格點(diǎn)到虛擬目標(biāo)點(diǎn)柵格的三軸距離，K_α、K_β、K_γ為預(yù)設(shè)參數(shù)，取值為：

計(jì)算障礙物柵格對(duì)特定柵格點(diǎn)(x，y，z)的地勢(shì)影響為：

其中，K_obs為障礙物柵格點(diǎn)的地勢(shì)值；λ_obs為預(yù)設(shè)值，用于調(diào)整受障礙物柵格點(diǎn)影響的地勢(shì)下降坡度；K_obs和λ_obs通由公式(4)確定：

其中，α、β、d_β均為預(yù)設(shè)常數(shù)，其中λ_obs的物理含義為：在d_β的距離范圍內(nèi)，均勻的下落β×K_obs的地勢(shì)量時(shí)，λ_obs代表下落坡度；

則特定柵格點(diǎn)(x，y，z)的最終地勢(shì)值為：

完成對(duì)探索地圖中各個(gè)柵格點(diǎn)的地勢(shì)值的計(jì)算；

步驟6：實(shí)時(shí)路徑規(guī)劃；

步驟6-1：將當(dāng)前路徑點(diǎn)位置(R_x，R_y，R_z)作為參考單元，從(R_x，R_y，R_z)同平面內(nèi)的八個(gè)相鄰單元以及豎直方向上、下兩個(gè)相鄰單元，共計(jì)十個(gè)單元中選取VH值最小的單元作為下一參考單元(N_x，N_y，N_z)，如果有多于一個(gè)單元的VH值同為最小，則根據(jù)VH值同為最小的單元數(shù)目，在無人機(jī)當(dāng)前路徑的基礎(chǔ)上創(chuàng)建相同數(shù)目新路徑；

步驟6-2：若VH(N_x，N_y，N_z)≥K_VH，則不存在無人機(jī)新路徑，刪除無人機(jī)當(dāng)前路徑；否則，若VH(N_x，N_y，N_z)＜K_VH，繼續(xù)執(zhí)行當(dāng)前路徑；其中，K_VH表示危險(xiǎn)臨界值，K_VH＝αVH_MAX(0＜α＜1)，即旋翼無人機(jī)禁止進(jìn)入的區(qū)域；

步驟6-3：若VH(N_x，N_y，N_y)≥VH(R_x，R_y，R_z)，則令VH(R_x，R_y，R_z)加上K_W，使虛擬地勢(shì)地圖VTM動(dòng)態(tài)可變：其中，K_W為預(yù)設(shè)的常數(shù)，K_W＝2；

步驟6-4：對(duì)當(dāng)前路徑及創(chuàng)建的新路徑進(jìn)行如下判定：

步驟6-4-1：如果當(dāng)前路徑長(zhǎng)度超出允許長(zhǎng)度，則將該路徑刪除；

步驟6-4-2：如果無人機(jī)已抵達(dá)目標(biāo)柵格，則判定創(chuàng)建的新路徑已成功規(guī)劃，并將創(chuàng)建的新路徑記錄到備選路徑內(nèi)；

步驟6-4-3：如果無人機(jī)已抵達(dá)探索地圖邊界，則判定創(chuàng)建的新路徑已成功規(guī)劃，并將創(chuàng)建的新路徑記錄到備選路徑內(nèi)；

步驟6-5：如果生成了無人機(jī)的有效新路徑，則進(jìn)行如下判定：

步驟6-5-1：如果當(dāng)前正在規(guī)劃的路徑，包括成功完成規(guī)劃的路徑小于5條，則填充有效新路徑至當(dāng)前規(guī)劃路徑為5條；

步驟6-5-2：如果當(dāng)前正在規(guī)劃的路徑已達(dá)到5條，則刪除本輪規(guī)劃生成的有效新路徑，并繼續(xù)規(guī)劃當(dāng)前路徑；

步驟6-6：若當(dāng)前已規(guī)劃成功的路徑達(dá)到5條，或當(dāng)前不存在正在規(guī)劃且尚未規(guī)劃成功的路徑，則判定路徑規(guī)劃已完成；

步驟6-7：從規(guī)劃完成的備選路徑中，將按照順序執(zhí)行如下判定，選取最終有效路徑一條：

步驟6-7-1：判定各路徑終點(diǎn)距離目標(biāo)柵格的距離，并從中選取出最短的三檔距離d_min1＜d_min2＜d_min3，并將這三檔距離對(duì)應(yīng)的路徑作為下一判定的備選路徑；每求出一檔最短距離，將該最短距離對(duì)應(yīng)的路徑記錄到備選路徑，直到找到三檔最短距離或已不存在未選擇的路徑，完成此次判定；

步驟6-7-2：判定路徑安全性，對(duì)備選路徑中的每一個(gè)路徑點(diǎn)執(zhí)行如下判定：

其中，代表障礙柵格到該路徑點(diǎn)的距離，d_check是預(yù)設(shè)常數(shù)，代表以障礙物柵格為球心的檢測(cè)區(qū)域，如果路徑點(diǎn)進(jìn)入該區(qū)域，則計(jì)算路徑點(diǎn)的危險(xiǎn)值

路徑的危險(xiǎn)值則為其所有路徑點(diǎn)危險(xiǎn)值之和，即危險(xiǎn)值計(jì)算完后，從備選路徑中選取兩檔危險(xiǎn)值最小的路徑作為下一判定的備選路徑；

步驟6-7-3：判定備選路徑長(zhǎng)度，從中選擇路徑最短的一條，作為最終路徑；

步驟7：實(shí)時(shí)路徑平滑；

假設(shè)當(dāng)前探索地圖的中心柵格在探索地圖中的坐標(biāo)為P_ds(x_ds，y_ds，z_ds)，任一路徑點(diǎn)在探索地圖中的坐標(biāo)為P_ps(x_ps，y_ps，z_ps)，則該路徑點(diǎn)在導(dǎo)航坐標(biāo)系下的坐標(biāo)P_p(x_p，y_p，z_p)如下：

將所有路徑點(diǎn)的坐標(biāo)值計(jì)算出來，表示為y_old(n)，n＝1，2，...，平滑后的最終路徑的所有路徑點(diǎn)表示為：y_new(n)，n＝1，2，...，則有：

最終得到y(tǒng)_new(n)，n＝1，2，...，即經(jīng)過平滑后的路徑。