[發(fā)明專利]一種無(wú)人機(jī)室內(nèi)自主導(dǎo)航方法

在審

• 申請(qǐng)?zhí)枺?/td>: 201811054136.5
• 申請(qǐng)日: 2018-09-10
• 公開（公告）號(hào): CN109238288A
• 公開（公告）日: 2019-01-18
• 發(fā)明（設(shè)計(jì)）人: 武德安;劉億;吳磊;馮江遠(yuǎn);劉杰
• 申請(qǐng)（專利權(quán)）人: 電子科技大學(xué);四川維德之星教育科技有限公司
• 主分類號(hào): G01C21/20
• 分類號(hào): G01C21/20
• 代理公司: 電子科技大學(xué)專利中心 51203
• 代理人: 鄒裕蓉

1.一種無(wú)人機(jī)室內(nèi)自主導(dǎo)航方法，其特征在于，包括以下步驟：

步驟1.搭建雙目立體視覺系統(tǒng)

步驟1.1相機(jī)標(biāo)定，獲取左右兩個(gè)相機(jī)的內(nèi)往外參數(shù)和畸變模型；

步驟1.2圖像矯正

首先利用相機(jī)的內(nèi)往外參數(shù)完成世界坐標(biāo)系到相機(jī)坐標(biāo)系的轉(zhuǎn)換，然后利用相機(jī)畸變系數(shù)對(duì)相機(jī)坐標(biāo)進(jìn)行矯正后得到較為準(zhǔn)確的像素坐標(biāo)；

步驟1.3三角測(cè)距

獲取該點(diǎn)到相機(jī)間的距離z：

其中，u₁和u₂分別為空間中同一點(diǎn)在左右相機(jī)成像平面上的橫坐標(biāo)，d為視差，f為相機(jī)焦距，b為兩個(gè)相機(jī)光心之間的距離；

步驟2.環(huán)境分類

卷積神經(jīng)網(wǎng)絡(luò)包括依次連接的卷積層1、池化層1、看、卷積層2、池化層2、卷積層3、池化層3、卷積層4、池化層4和隱層9；其中，網(wǎng)絡(luò)輸入為32×32圖像，輸出為3×1向量，用于表示當(dāng)前圖像對(duì)應(yīng)分類于走廊、樓梯、拐角等三類環(huán)境的概率；

其中，各神經(jīng)元激活函數(shù)為ReLu函數(shù)：

f(v)＝max(0，v) (4)

其中，v為神經(jīng)元激活前的輸出，max表示求最大值，f(v)為經(jīng)激活后的輸出；

選取交叉熵作為網(wǎng)絡(luò)輸出的損失函數(shù)：

其中，C為損失函數(shù)，n為樣本數(shù)，a為神經(jīng)元的實(shí)際輸出，a＝f(z)，z＝∑w_ix_i+b，w_i為權(quán)重，x_i為x的*第i個(gè)元素，i為正整數(shù)，b為偏置，x^*為每一層神經(jīng)元的輸入，y^*為真實(shí)標(biāo)簽；

在反向傳播過程中，使用Momentum進(jìn)行優(yōu)化，如下式：

其中，l為當(dāng)前迭代次數(shù)，d_l為第l次更新方向，θ_l為待估計(jì)參數(shù)，g(θ_l-1)為當(dāng)前梯度方向，E為誤差，α為學(xué)習(xí)率，β為上次更新方向衰減系數(shù)；

步驟3.當(dāng)通過卷積神經(jīng)網(wǎng)絡(luò)確定無(wú)人機(jī)所處環(huán)境后，針對(duì)不同的環(huán)境設(shè)計(jì)不同飛行策略，用于完成無(wú)人機(jī)室內(nèi)自主導(dǎo)航；

情形1.無(wú)人機(jī)位于室內(nèi)走廊環(huán)境；

首先對(duì)走廊圖像進(jìn)行Canny邊緣檢測(cè)與概率霍夫線變換獲取圖像中的直線，將圖像所檢測(cè)到的直線中斜率接近于0或接近于無(wú)窮的直線剔除，直線交點(diǎn)坐標(biāo)：

其中，(k₁，b₁)，(k₂，b₂)分別為兩直線的斜率與截距，；x和y分別為二維成像平面上任一點(diǎn)的X軸和Y軸坐標(biāo)；

使用劃分圖像網(wǎng)格的方法，通過尋找直線交點(diǎn)數(shù)目最多的網(wǎng)格點(diǎn)，作為消失點(diǎn)的初始估計(jì)位置；假設(shè)在預(yù)處理后的尺寸大小為W×H的圖像I上劃分N×N的網(wǎng)格，每個(gè)網(wǎng)格點(diǎn)的尺寸大小為(W/N)×(H/N)，則每個(gè)網(wǎng)格點(diǎn)中直線交點(diǎn)的個(gè)數(shù)I_{(m′，n′)}如下式所示：

其中，(x_k，y_k)為圖像上直線焦點(diǎn)坐標(biāo)，K為直線焦點(diǎn)個(gè)數(shù)，1≤k≤K，(m′，n′)為圖像上的格點(diǎn)坐標(biāo)，1≤m′≤M，1≤n′≤N；

消失點(diǎn)的初始估計(jì)位置(x^*，y^*)為：

其中，(m^*，n^*)為直線交點(diǎn)數(shù)最多的網(wǎng)格點(diǎn)：

采用鄰域求平均的方法進(jìn)一步估計(jì)消失點(diǎn)的位置

其中，U為該領(lǐng)域內(nèi)所有點(diǎn)u組合成的集合，|U|為集合內(nèi)點(diǎn)的個(gè)數(shù)：

U＝{k∈[0，K)：||(x_u，y_u)-(x^*，y^*)||²≤δ} (13)

其中，δ為點(diǎn)(x^*，y^*)的鄰域半徑；

假設(shè)消失點(diǎn)P在左右圖像上的相機(jī)坐標(biāo)分別為p_l(x_l，y₀)、p_r(x_r，y₀)，則無(wú)人機(jī)與消失點(diǎn)水平方向的位置關(guān)系分類三類：

(1)無(wú)人機(jī)正對(duì)消失點(diǎn)，x_l≥0，x_r≤0，無(wú)人機(jī)保持當(dāng)前飛行狀態(tài)；

(2)無(wú)人機(jī)在消失點(diǎn)左邊，x_l＞0，x_r＞0，無(wú)人機(jī)應(yīng)向右調(diào)整；

(3)無(wú)人機(jī)在消失點(diǎn)右邊，x_l＜0，x_r＜0，無(wú)人機(jī)應(yīng)向左調(diào)整。

情形2.無(wú)人機(jī)位于室內(nèi)樓梯環(huán)境；

經(jīng)過Canny邊緣檢測(cè)與概率霍夫線變換預(yù)處理并過濾后，代表樓梯的直線簇相互間往往并不絕對(duì)平行，則斜率與k_i相近的近似平行直線數(shù)目如下：

其中T為近似平行直線的斜率差值，為不同直線的斜率；1≤l₁≤L，1≤l₂≤L，L為直線簇的直線數(shù)量；

圖像中的樓梯由近似平行直線簇中數(shù)量最多的直線簇表示，其數(shù)量為：

其中，下標(biāo)k為直線簇的平均斜率，下標(biāo)k_ll為直線ll的斜率，1≤ll≤L；

無(wú)人機(jī)開始上樓飛行前矯正其機(jī)頭的朝向：

(1)當(dāng)樓梯直線平均斜率k＜-ε時(shí)，無(wú)人機(jī)機(jī)頭偏左，應(yīng)向右轉(zhuǎn)動(dòng)；

(2)當(dāng)樓梯直線平均斜率k＞ε時(shí)，無(wú)人機(jī)機(jī)頭偏左，應(yīng)向左轉(zhuǎn)動(dòng)；

(3)當(dāng)樓梯直線平均斜率-ε≤k≤ε時(shí)，無(wú)人機(jī)機(jī)頭正對(duì)樓梯，保持不動(dòng)；

其中，閾值ε為接近于0的極小正數(shù)；

令點(diǎn)與點(diǎn)B(x_j，y_j)為圖像上檢測(cè)到的樓梯段簇左右兩個(gè)端點(diǎn)，則樓梯中心的橫坐標(biāo)為：

其中，Q為樓梯的線段數(shù)，1≤q₁≤Q，1≤q₂≤Q；

由無(wú)人機(jī)與樓梯中心的相對(duì)位置可知：

(1)當(dāng)時(shí)，無(wú)人機(jī)位于樓梯中心偏左，此時(shí)應(yīng)向右移動(dòng)；

(2)當(dāng)時(shí)，無(wú)人機(jī)位于樓梯中心偏右，此時(shí)應(yīng)向左移動(dòng)；

(3)當(dāng)時(shí)，無(wú)人機(jī)位于樓梯中心，可保持不動(dòng)；

其中，閾值δ^*為接近于0的極小正數(shù)，x，分別為無(wú)人機(jī)與樓梯的水平坐標(biāo)；

情形3.無(wú)人機(jī)位于室內(nèi)拐角

首先通過雙目視覺系統(tǒng)獲取當(dāng)前場(chǎng)景的左右圖像，并嘗試通過立體匹配后獲取當(dāng)前場(chǎng)景的深度圖，采用非視覺傳感器控制無(wú)人機(jī)距離正前方墻壁的最小距離0.5m。