基于單應(yīng)性估計(jì)和擴(kuò)展卡爾曼濾波的無(wú)人機(jī)位置估計(jì)方法
【技術(shù)領(lǐng)域】
[0001] 本發(fā)明屬于無(wú)人機(jī)位置估計(jì)方法，設(shè)及一種基于單應(yīng)性估計(jì)和擴(kuò)展卡爾曼濾波的 無(wú)人機(jī)位置估計(jì)（A Homography Estimaiton and Extended Kalman Filter-based Localization Method for Unmanned Aerial Vehicle,肥-EKF-LM)方法，該方法能夠消除 無(wú)人機(jī)位置估計(jì)的累積誤差，并實(shí)時(shí)構(gòu)建環(huán)境地圖，實(shí)現(xiàn)無(wú)人機(jī)位置參數(shù)的精確估計(jì)功能。
【背景技術(shù)】
[0002] 在無(wú)人機(jī)視覺(jué)導(dǎo)航領(lǐng)域，無(wú)人機(jī)位置估計(jì)與環(huán)境構(gòu)圖至關(guān)重要，無(wú)人機(jī)位置參數(shù) 的估計(jì)是實(shí)現(xiàn)自主導(dǎo)航的核屯、，研究可靠、高精度的視覺(jué)位置估計(jì)方法是提高無(wú)人機(jī)位置 參數(shù)估計(jì)精度的重要手段，具有重要的理論意義和應(yīng)用價(jià)值。
[0003] 在線圖像拼接技術(shù)，當(dāng)無(wú)人機(jī)對(duì)某一區(qū)域重訪時(shí)，認(rèn)為其航跡形成閉環(huán)，并利用該 區(qū)域的位置信息在擴(kuò)展卡爾曼濾波器化Xtended Kalman FiIter,邸F)的框架下檢測(cè)并消 除無(wú)人機(jī)位置估計(jì)的累積漂移，從而為無(wú)人機(jī)提供精確的位置信息。一般情況下，計(jì)算圖像 之間的單應(yīng)關(guān)系，可W對(duì)無(wú)人機(jī)的位置進(jìn)行實(shí)時(shí)估計(jì)，然而，隨著時(shí)間增加，傳統(tǒng)的靜態(tài)圖 像拼接方法對(duì)位置估計(jì)的累積誤差會(huì)不可避免的增大，位置參數(shù)估計(jì)結(jié)果并不準(zhǔn)確。需要 考慮拼接過(guò)程中的不確定性因素，能夠完成在線圖像拼接，在無(wú)人機(jī)位置估計(jì)過(guò)程中保持 一定的可靠性。與傳統(tǒng)方法相比，基于在線圖像拼接的位置估計(jì)技術(shù)在位置估計(jì)精度和可 靠性方面具有較大優(yōu)勢(shì)。

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

[0004] 要解決的技術(shù)問(wèn)題
[0005] 為了避免現(xiàn)有技術(shù)的不足之處，本發(fā)明提出一種基于單應(yīng)性估計(jì)和擴(kuò)展卡爾曼濾 波的無(wú)人機(jī)位置估計(jì)方法。
[0006] 技術(shù)方案
[0007] -種基于單應(yīng)性估計(jì)和擴(kuò)展卡爾曼濾波的無(wú)人機(jī)位置估計(jì)方法，其特征在于步驟 如下：
[000引步驟1、對(duì)無(wú)人機(jī)捕獲的圖像序列中相鄰的兩帖圖像進(jìn)行單應(yīng)矩陣的魯棒估計(jì):首 先提取相鄰的兩帖圖像的特征點(diǎn)，然后進(jìn)行匹配：
[0009] 1)當(dāng)匹配點(diǎn)被追蹤的成功率超過(guò)65%時(shí)，采用全單應(yīng)模型，利用LMeDS剔除誤匹配 點(diǎn)，并采用M估計(jì)器計(jì)算單應(yīng)矩陣；
[0010] 2)當(dāng)匹配點(diǎn)被追蹤的成功率在40%到65%之間時(shí)，采用仿射單應(yīng)模型，并利用松 弛M估計(jì)器估算單應(yīng)矩陣；
[0011] 3)當(dāng)匹配點(diǎn)被追蹤的成功率低于40%時(shí)，采用歐式單應(yīng)模型，并利用最小二乘計(jì) 算單應(yīng)矩陣；
[0012] 步驟2、計(jì)算單應(yīng)矩陣估計(jì)的協(xié)方差：
[001；3]給定一個(gè)具有n對(duì)匹配點(diǎn)的集合Sm= {{mi，n/ i}，{m2，n/ 2}，…，{mn，n/n}}，其中，Hii和 n/I分別表示兩個(gè)匹配點(diǎn)的像素位置，則描述不確定性的協(xié)方差矩陣Ch的計(jì)算過(guò)程為：
[0014] 1)計(jì)算從m到心的單應(yīng)性變換的雅克比矩陣J;
[001引2)計(jì)算每對(duì)匹配點(diǎn)匹配誤差的協(xié)方差即m' 1和放;二撤之間殘差的協(xié)方差，假 設(shè)各對(duì)匹配點(diǎn)的匹配誤差之間不相關(guān)，則總協(xié)方差巧
[0016] 3)計(jì)算單應(yīng)估計(jì)的協(xié)方差矩陣，良[
[0017] 步驟3、基于單應(yīng)關(guān)系的運(yùn)動(dòng)估計(jì)：
[0018] 韋人化壇動(dòng)參數(shù)Ri2、t2和ni與相鄰的兩帖圖像之間的單應(yīng)矩陣滿足關(guān)系式
，其中Ru是將第一帖圖像相機(jī)坐標(biāo)系中的向量變換到第二帖圖像 相機(jī)坐標(biāo)系中向量的旋轉(zhuǎn)矩陣，t2是第二帖圖像相對(duì)于第一帖圖像的相機(jī)坐標(biāo)系的相對(duì)位 移，m為第一帖圖像相機(jī)坐標(biāo)系中垂直于圖像平面的單位向量（取沿相機(jī)向外的方向?yàn)?正），I為單位矩陣，A為相機(jī)標(biāo)定矩陣，山是第一帖圖像相機(jī)位置到圖像平面之間的距離?；?于奇異值分解，關(guān)系式
巧W表示為
已知出2、A和山時(shí)，根據(jù)該關(guān)系式可W由無(wú)人機(jī)捕獲的圖像序列對(duì)無(wú)人機(jī)運(yùn)動(dòng)參數(shù)Rl2、t2和 m進(jìn)行求解；
[0019] 步驟4:更新圖像拼接數(shù)據(jù)庫(kù)：
[0020] 每當(dāng)UAV采集到一帖新的圖像，圖像拼接數(shù)據(jù)庫(kù)的更新參數(shù)包括：
[0021] 1)當(dāng)前圖像與前一帖圖像之間的單應(yīng)矩陣及其協(xié)方差；
[0022] 2)將該單應(yīng)矩陣乘W拼接數(shù)據(jù)庫(kù)內(nèi)之前所有的單應(yīng)矩陣，即計(jì)算組合單應(yīng)矩陣， 更新該圖像在拼接中相對(duì)參考帖的位置；
[0023] 3)將該圖像與前一帖圖像之間的單應(yīng)矩陣與參考帖圖像Xi的組合單應(yīng)矩 陣、單應(yīng)矩陣的協(xié)方差矩陣及相對(duì)參考帖的位置信息插入到拼接數(shù)據(jù)庫(kù)中；
[0024] 步驟5:基于擴(kuò)展卡爾曼濾波的位置估計(jì)：
[0025] 若未檢測(cè)到當(dāng)前圖像與圖像拼接數(shù)據(jù)庫(kù)航跡形成閉環(huán)則跳過(guò)此步驟，繼續(xù)采集下 一帖圖像，重復(fù)步驟1;
[0026] 若檢測(cè)到航跡閉環(huán)的終點(diǎn)即圖像序列的第i帖圖像和第j帖圖像重疊時(shí)，啟動(dòng)特征 匹配過(guò)程校準(zhǔn)重疊圖像Ii與圖像。，更新位置估計(jì)，其步驟包括：
[0027] 1)建立動(dòng)態(tài)模型：狀態(tài)方程為X化）=f(x(k-l))+w(k-l)，量測(cè)方程為z(k)=g(x 化-l))+n化），w化-1)和n化)分別是零均值高斯的過(guò)程噪聲和量測(cè)噪聲，Q和R為其各自的協(xié) 方差矩陣，記
分別為狀態(tài)方程和量測(cè)方程關(guān)于狀態(tài)的雅可比行列 式；
[0028] 2)計(jì)算協(xié)方差矩陣:設(shè)。和Ii之間共包含n+1張圖片，而校準(zhǔn)重疊圖像的單應(yīng)矩陣 為町1，協(xié)方差矩陣為（\;,假定^' = 0，1=11，則有先驗(yàn)狀態(tài)向量義-=[山^，。'，^^=比01， Xl ? hl2，…，Xn-I ? h(n-l)n]T，狀態(tài)可W通過(guò)迭代得到；
[0029] 3)計(jì)算狀態(tài)向量:根據(jù)預(yù)測(cè)方程Xi = Xi-I ? h(i-i)i逐步完成對(duì)n個(gè)狀態(tài)的預(yù)測(cè)，易得 該式關(guān)于狀態(tài)和h ( 1 - 1 ) 1的雅可比行列式A和W，則X 1的協(xié)方差矩陣表示為
[0030] 4)計(jì)算卡爾曼增益:將狀態(tài)向量按序排列，取h日n = Gx=[09X(9n)，I9X9]X，根據(jù)卡爾 曼濾波經(jīng)典方程式計(jì)算卡爾曼增益；
[003。 5)更新?tīng)顟B(tài)向量和協(xié)方差矩陣:狀態(tài)向量按照式x = x>K(h0n-G。進(jìn)行更新，協(xié)方 差矩陣通過(guò)求解式恥-1)1 =化-1廠吃的雅克比行列式獲得；
[0032 ] 6)修正閉環(huán)內(nèi)所有圖像的位置:完成對(duì)閉環(huán)內(nèi)所有相鄰兩帖圖像間單應(yīng)關(guān)系的更 新，并將更新后的信息插入到圖像拼接數(shù)據(jù)庫(kù)中，從而實(shí)現(xiàn)無(wú)人機(jī)位置的精確估計(jì)；
[0033] 繼續(xù)采集下一帖圖像，重復(fù)步驟1，直至飛行結(jié)束。
[0034] 有益效果
[0035] 本發(fā)明提出的一種基于單應(yīng)性估計(jì)和擴(kuò)展卡爾曼濾波的無(wú)人機(jī)位置估計(jì)方法，利 用在線圖像拼接來(lái)消除無(wú)人機(jī)位置估計(jì)的累積誤差，并實(shí)時(shí)構(gòu)建環(huán)境地圖，可W有效提高 無(wú)人機(jī)位置參數(shù)估計(jì)的精確性。該方法利用在線圖像拼接消除無(wú)人機(jī)位置估計(jì)的累積誤 差，基于圖像帖間單應(yīng)矩陣魯棒估計(jì)方法，并考慮圖像間單應(yīng)關(guān)系和不確定性，實(shí)現(xiàn)對(duì)無(wú)人 機(jī)位置參數(shù)的精確估計(jì)，并利用擴(kuò)展卡爾曼濾波對(duì)無(wú)人機(jī)位置估計(jì)結(jié)果進(jìn)行預(yù)測(cè)與更新， 從而顯著提高無(wú)人機(jī)的位置估計(jì)精度。
【附圖說(shuō)明】
[0036] 圖1為原理框圖
【具體實(shí)施方式】
[0037] 現(xiàn)結(jié)合實(shí)施例、附圖對(duì)本發(fā)明作進(jìn)一步描述：
[0038] 本發(fā)明主要包括兩個(gè)核屯、部分:A、單應(yīng)矩陣的估計(jì)及估計(jì)不確定性的描述。B、基 于擴(kuò)展卡爾曼濾波的位置估計(jì)。其實(shí)現(xiàn)步驟如下：
[0039] 第1步，單應(yīng)矩陣的魯棒估計(jì)；
[0040] 第2步，單應(yīng)矩陣的估計(jì)不確定性的描述；
[0041] 第3步，基于單應(yīng)關(guān)系的運(yùn)動(dòng)估計(jì)；
[0042] 第4步，更新圖像拼接數(shù)據(jù)庫(kù)；
[0043] 第5步，基于擴(kuò)展卡爾曼濾波的位置估計(jì)。
[0044] 具體實(shí)施例流程如圖1所示。
[0045] 1、單應(yīng)矩陣的魯棒估計(jì)
[0046] 在齊次坐標(biāo)中，任何單應(yīng)關(guān)系都可W表示為如下可逆線性變換：
[0047]
(!)
[004引其中，H為單應(yīng)矩陣，[x，y，U哺巧哀皆分別為同一點(diǎn)在兩個(gè)不同投影空間內(nèi)的坐 標(biāo)向量。
[0049]無(wú)人機(jī)捕獲的圖像序列中相鄰的兩帖圖像互相匹配的特征點(diǎn)之間不僅存在相機(jī) 運(yùn)動(dòng)引起的特征點(diǎn)位置偏移，還存在由監(jiān)視區(qū)域非平面引起的不確定性。因此，單應(yīng)性估計(jì) 過(guò)程一般可W分為剔除誤匹配點(diǎn)和估計(jì)單應(yīng)矩陣兩個(gè)部分。其中，剔除誤匹配點(diǎn)通常采用 最小中值算法(LMeDS)或隨機(jī)采樣一致性(RANSAC)方法，對(duì)于單應(yīng)性擬合問(wèn)題，LMeDS具有 更優(yōu)的性能;估計(jì)單應(yīng)矩陣時(shí)通常利用M估計(jì)器。
[0050] 匹配點(diǎn)較少時(shí)，單應(yīng)性估計(jì)的結(jié)果并不理想，為了得到單應(yīng)矩陣的魯棒估計(jì)，可W 自適應(yīng)地降低單應(yīng)模型的復(fù)雜度，采用歐式、仿射和全單應(yīng)矩陣等形式，其自由度分別為4、 6、8d
[0051] 根據(jù)點(diǎn)追蹤器對(duì)匹配點(diǎn)集被成功追蹤的比例，確定應(yīng)當(dāng)采用的單應(yīng)模型、誤匹配 點(diǎn)剔除方法及單應(yīng)矩陣估計(jì)方法。W下給出的比例闊值是通過(guò)對(duì)數(shù)百?gòu)埡娇請(qǐng)D片處理后得 到的經(jīng)驗(yàn)值。
[0052] 1)全單應(yīng)模型：該模型適用于匹配點(diǎn)被追蹤的成功率超過(guò)65 %的情況，采用LMeDS 剔除誤匹配點(diǎn)，并利用M估計(jì)器計(jì)算單應(yīng)矩陣；
[0053] 2)仿射單應(yīng)模型：如果匹配點(diǎn)被追蹤的成功率在40%到65%之間，需要采用松弛M 估計(jì)器估算單應(yīng)矩陣；
[0054] 3)歐式單應(yīng)模型：如果匹配點(diǎn)被追蹤的成功率低于40%，即數(shù)據(jù)集受干擾嚴(yán)重或 者數(shù)據(jù)量過(guò)小，則利用最小二乘計(jì)算單應(yīng)矩陣。
[0055] 2、單應(yīng)矩陣的估計(jì)不確定性的描述
[0056] 計(jì)算出單應(yīng)矩陣后，由協(xié)方差矩陣衡量估計(jì)精度。（1)中的單應(yīng)矩陣H可W寫(xiě)成如 下向量：
[0057] h=比 11 hi2...h33]T (2)
[005引則其協(xié)方差矩陣Ch是一個(gè)9X9的矩陣，對(duì)角線元素為變量的方差，上、下=角矩陣 元素為互協(xié)方差。
[0059] 給定一個(gè)具有n對(duì)匹配點(diǎn)的集合Sm:
[0060]
(1)
[0061] 其中，mi = [Xi，yi]T和n/I = 分別表示兩個(gè)匹配點(diǎn)的像素位置。則描述不確 定性的協(xié)方差矩陣Ch的計(jì)算步驟為：
[0062] 1)計(jì)算從m到n/的單應(yīng)性變換h的雅克比矩陣J，其中mi=[Xi，yi]T和n/ i=[x/ i，y ^1]滿足：
[0063]
[0064] (4)
[00 化]
[0066] 2)計(jì)算每對(duì)匹配點(diǎn)匹配誤差的協(xié)方差運(yùn)里通過(guò)計(jì)算m'l和成;:::撤之間殘差 的協(xié)方差并將其作為相應(yīng)的匹配誤差協(xié)方差。假設(shè)各對(duì)匹配點(diǎn)的匹配誤差之間不相關(guān)，貝U 可得到如下對(duì)角矩陣形式的總協(xié)方差Cm:
[0067]
(5)
[0068] 3)計(jì)算單應(yīng)性估計(jì)