[發(fā)明專利]基于視覺SLAM的復(fù)雜環(huán)境下無人機(jī)自主降落區(qū)域選擇方法

在審

• 申請?zhí)枺?/td>: 201710535595.4
• 申請日: 2017-07-04
• 公開（公告）號: CN107291093A
• 公開（公告）日: 2017-10-24
• 發(fā)明（設(shè)計(jì)）人: 楊濤;李珮琦;張慧明;張艷寧
• 申請（專利權(quán)）人: 西北工業(yè)大學(xué);西北工業(yè)大學(xué)深圳研究院
• 主分類號: G05D1/08
• 分類號: G05D1/08;G05D1/10
• 代理公司: 西北工業(yè)大學(xué)專利中心61204
• 代理人: 王鮮凱

1.一種基于視覺SLAM的復(fù)雜環(huán)境下無人機(jī)自主降落區(qū)域選擇方法，其特征在于包括以下步驟：

步驟一、用無人機(jī)機(jī)載的單目相機(jī)采集20張標(biāo)定板的圖像，采用張氏標(biāo)定法，用Matlab工具包對相機(jī)內(nèi)參進(jìn)行標(biāo)定；

步驟二、估計(jì)基礎(chǔ)矩陣F和單應(yīng)矩陣H，通過比較重投影誤差選擇模型；

地圖初始化算法步驟：

提取參考幀和當(dāng)前幀的特征點(diǎn)p_c，然后對兩幀之間進(jìn)行特征匹配如果匹配點(diǎn)數(shù)不夠，則重置參考幀；通過對極約束計(jì)算出單應(yīng)矩陣H

p_c＝Hp_r (1)

其中p_c和p_r是兩幀檢測出的特征點(diǎn)對，H是單應(yīng)矩陣；

計(jì)算基礎(chǔ)矩陣F

pcTFpr=0E=KTFK---(2)]]>

其中，K為相機(jī)的內(nèi)參矩陣，E為本質(zhì)矩陣；從本質(zhì)矩陣恢復(fù)運(yùn)動，其中本質(zhì)矩陣E的自由度為8，通過經(jīng)典的八點(diǎn)法計(jì)算得到，對參考幀和當(dāng)前幀進(jìn)行特征匹配，通過匹配上的特征點(diǎn)解算出本質(zhì)矩陣E；

u1v11e1e2e3e4e5e6e7e8e9u2v21=0---(3)]]>

兩幀圖片匹配上特征點(diǎn)滿足公式(3)，將公式(3)轉(zhuǎn)換成線性方程組的形式，求解出本質(zhì)矩陣E的參數(shù)；得到本質(zhì)矩陣E之后，通過奇異值分解進(jìn)行求解：

E＝UΣV^T(4)

其中，U,V為正交陣，Σ為奇異值矩陣；通過SVD分解得到t，R；通過4對匹配特征點(diǎn)計(jì)算出單應(yīng)矩陣H；將單應(yīng)矩陣H看成是一個(gè)向量，通過直接線性變換法恢復(fù)單應(yīng)矩陣H；通過分解得t，R；通過對極約束得到相機(jī)的位姿；然后求解得到這些匹配點(diǎn)的3D坐標(biāo)；這些3D坐標(biāo)就是地圖初始化的地圖點(diǎn)；

在地圖初始化成功之后，用PnP求解3D到2D點(diǎn)對的運(yùn)動；通過參考幀的3D地圖點(diǎn)和當(dāng)前幀的2D特征點(diǎn)匹配，通過PnP解算出3D到2D點(diǎn)的運(yùn)動，獲取當(dāng)前幀的位姿；提取三維的地圖點(diǎn)用于后續(xù)的聚類算法；

采用閉環(huán)檢測對相似的場景進(jìn)行檢測；在閉環(huán)檢測部分采用詞袋模型DBoW2來實(shí)現(xiàn)；詞袋模型通過提取特征并通過聚類的方法將特征詞進(jìn)行分類，然后利用視覺詞袋量化圖像特征，利用統(tǒng)計(jì)的詞頻直方圖將圖像進(jìn)行歸類；當(dāng)相機(jī)來到之前到過的地方，用過閉環(huán)檢測部分檢測閉環(huán)，同時(shí)為了有效的完成閉環(huán)，使用本質(zhì)圖優(yōu)化位姿圖，將閉環(huán)的誤差分散到整個(gè)圖中；

步驟三、通過相似變換的求解估計(jì)單目視覺SLAM尺度因子，單目視覺SLAM系統(tǒng)的導(dǎo)航參考坐標(biāo)系為地圖初始化時(shí)的相機(jī)坐標(biāo)系O_CX_CY_CZ_C，GPS的導(dǎo)航坐標(biāo)系為O_tX_tY_tZ_t；假設(shè)SLAM系統(tǒng)輸出的相機(jī)位置為對應(yīng)的GPS輸出真實(shí)的位置信息根據(jù)圖像變換的原理；

t＝sRC+t₀(5)

其中，R為O_CX_CY_CZ_C坐標(biāo)系到坐標(biāo)系O_tX_tY_tZ_t的旋轉(zhuǎn)矩陣，t₀平移向量；R和t₀通過組合導(dǎo)航系統(tǒng)的標(biāo)定參數(shù)直接獲得，s為SLAM系統(tǒng)的尺度因子，在初始化階段需要估計(jì)此參數(shù)的值；由于系統(tǒng)測量誤差的存在，式(5)的左右兩邊是不等的，記殘差為e_i＝t_i-sR·C_i-t₀；尺度因子s的估計(jì)問題轉(zhuǎn)化為求取最優(yōu)值s使得

minsΣi=1n||ei||2---(6)]]>

最小，最后解算出最優(yōu)解s；

步驟四、將地平面劃分成一個(gè)個(gè)小網(wǎng)格，將SLAM算法檢測得到的世界坐標(biāo)系下的地圖點(diǎn)投影到二維的網(wǎng)格地圖中；

定義每個(gè)網(wǎng)格高度如下式：

h(i,j)=Σk∈N(i,j)hk-hmin-hmaxN(i,j)-2---(7)]]>

其中，h_k表示網(wǎng)格Grid(i,j)中的地圖點(diǎn)，h_min和h_max表示網(wǎng)格中高度值最大和最小的地圖點(diǎn)，去除掉這兩個(gè)點(diǎn)之后將落在網(wǎng)格中的地圖點(diǎn)的高度均值賦值給網(wǎng)格高度h(i,j)；

步驟五、對步驟四得到的二維網(wǎng)格高度地圖進(jìn)行平滑與劃分，輸入二維網(wǎng)格高度地圖的大小與每一個(gè)網(wǎng)格的高度，將每一個(gè)網(wǎng)格作為最小單元；先使用Mean shift算法對網(wǎng)格高度進(jìn)行聚類，確定網(wǎng)格高度分類的類別總數(shù)和各類別的類心，然后以類別數(shù)和類心為輸入，使用Mean shift算法得到網(wǎng)格地圖最終的劃分結(jié)果；具體步驟如下：

運(yùn)用Mean shift算法對網(wǎng)格地圖進(jìn)行平滑，對于每一個(gè)網(wǎng)格初始化j＝1，并且使模值點(diǎn)y_i,1＝x_i；

運(yùn)用公式計(jì)算y_i,j+1直到收斂，記收斂后的值為y_i,c；

對圖像進(jìn)行mean shift平滑，將收斂結(jié)果存儲在z_i中，z_i＝y(tǒng)_i,c；

將z_i中所有空間距離小于h_s，高度值距離小于h_r的網(wǎng)格劃分到不同的類別{C_p}_p＝1,…,m中去；

對于每一個(gè)網(wǎng)格i＝1,2…,n，類別標(biāo)志L _i＝{p|z_i∈C_p}；

{x_i}_{i＝1,2,…,n},{z_i}_i＝1,2…n分別表示原始地圖中的網(wǎng)格和平滑后的網(wǎng)格，上標(biāo)r,s分別表示網(wǎng)格的高度信息和坐標(biāo)，{L_i}_i＝1,2,…n表示分割后每一個(gè)網(wǎng)格的類別標(biāo)簽；

步驟六、由步驟五得到的網(wǎng)格高度類別，選擇出合適降落的高度h_UAV，根據(jù)無人機(jī)大小設(shè)定合適降落的區(qū)域面積s_UAV，通過兩部篩選，確定無人機(jī)最佳降落地點(diǎn)；

定義網(wǎng)格是否適合降落公式：

T(i,j)=Σm,n∈R(i,j,r)||h(m,n)-h(i,j)||2---(8)]]>

其中，h(m,n)義為二維網(wǎng)格地圖上地圖點(diǎn)(m,n)的高度值，h(i,j)表示網(wǎng)格坐標(biāo)下(i,j)網(wǎng)格的高度值；其中r表示搜索的半徑；根據(jù)實(shí)際無人機(jī)的大小搜索以坐標(biāo)h(i,j)為原點(diǎn)，半徑為r的區(qū)域；遍歷每一個(gè)網(wǎng)格，篩選出適合無人機(jī)降落的區(qū)域；將沒有投影點(diǎn)的網(wǎng)格區(qū)域標(biāo)志為不可降落區(qū)域；最后根據(jù)實(shí)際的應(yīng)用設(shè)定公式T(i,j)的一個(gè)閾值用于判斷適合無人機(jī)降落的網(wǎng)格，同時(shí)將適合無人機(jī)降落的網(wǎng)格標(biāo)注出來；

對標(biāo)志過適合降落區(qū)域的網(wǎng)格地圖進(jìn)行遍歷，選擇出滿足條件：每一個(gè)網(wǎng)格標(biāo)簽為h_UAV對應(yīng)的類別，且面積大于等于s_UAV的每一個(gè)區(qū)域，并對其進(jìn)行編號area_i；

計(jì)算出每一個(gè)區(qū)域area_i距離高度值大于h_UAV的區(qū)域的距離，選擇距離最大的area_i為降落點(diǎn)。