畢業(yè)答辯-視覺航向測量與視覺慣性組合導航算法設(shè)計.pptx

視覺航向測量與視覺/慣性組合導航算法設(shè)計,報告人：指導老師：,2019/12/6,1,2019/12/6,2,研究背景,組合導航,視覺/慣性組合導航：無需考慮電磁信號的屏蔽、遮擋問題，在復雜環(huán)境中有顯著優(yōu)勢。,完成內(nèi)容,單目視覺航向測量,1,慣性航向測量,2,組合導航算法,3,導航軟件設(shè)計,4,1.0單目視覺測量原理,2019/12/6,4,視角1,視角2,特征匹配,相機運動,幾何約束,1.1攝像機內(nèi)參數(shù)標定,2019/12/6,5,1）張正友標定法的基本思想,角點檢測結(jié)果,1.1攝像機內(nèi)參數(shù)標定,2019/12/6,6,2）張正友標定法的數(shù)學模型,攝像機有5個內(nèi)參需要求解，則當采集的圖像數(shù)大于3時，可以線性唯一求解出內(nèi)參數(shù)矩陣K。,內(nèi)參數(shù)矩陣：,f為攝像機的焦距，為攝相機成像平面坐標軸互不正交引出的傾斜因子，是攝像機光軸與圖像平面的交點的像素坐標，為每個像素點的尺寸。,單應(yīng)性約束：,內(nèi)參數(shù)矩陣基本約束：,1.1攝像機內(nèi)參數(shù)標定,2019/12/6,7,3）相機標定結(jié)果,標定模板圖像,所用攝像機,大恒MER-310-12UC數(shù)字攝像機,標定結(jié)果,內(nèi)參數(shù)矩陣：,1.2特征點獲取及匹配,2019/12/6,8,SURF算法是對SIFT算法的改進SIFT算法中用差分高斯金字塔（DOG）代替高斯金字塔（LOG）來提高算法性能；SURF算法對特征點的檢測基于Hessian矩陣,SURF算法中用方框濾波近似代替高斯濾波，用積分圖像加速卷積提高運算速度，以較小的精度損失獲得更快的特征點檢測速度。,1）SURF算法基本原理,1.2特征點獲取及匹配,2019/12/6,9,SURF算法檢測的特征點,FLANN算法匹配結(jié)果,2）檢測和匹配結(jié)果,采用FLANN（快速最近鄰逼近搜索）匹配SURF特征矢量。可以提高匹配的速度，同時精度的減小在可接受范圍內(nèi)。,SURF特征點描述算子,1.3攝像機的運動描述,2019/12/6,10,C與C在世界坐標系下存在旋轉(zhuǎn)矩陣R和位移t。,1）對極幾何關(guān)系,定義基礎(chǔ)矩陣：,當對應(yīng)點滿足歸一化情況,即本質(zhì)矩陣：,基本矩陣存在特殊形式,本質(zhì)矩陣與基礎(chǔ)矩陣存在轉(zhuǎn)換關(guān)系：,1.3攝像機的運動描述,2019/12/6,11,基本矩陣F是秩為2，自由度為7的33齊次矩陣；對兩幅圖像中任一對對應(yīng)點m,m，都滿足,RANSAC算法求解基礎(chǔ)矩陣的具體步驟為：,對所有特征點進行歸一化處理；已知基礎(chǔ)矩陣自由度為7，隨機選取其中7組對應(yīng)點，利用7點法求解基礎(chǔ)矩陣；通過上述計算，若F陣有3個解，計算匹配點對應(yīng)極線的距離小于設(shè)定閾值的點作為內(nèi)點，以內(nèi)點數(shù)最多的解作為基礎(chǔ)矩陣；由劃為內(nèi)點的點對重新估計基礎(chǔ)矩陣F，用估計的F陣定義極線附近的搜索區(qū)域。重復上述（2）（4）步，進行反復迭代，直至特征點數(shù)目穩(wěn)定為止，則得到最終的基礎(chǔ)矩陣及內(nèi)點信息。,2）RANSAC算法求解基礎(chǔ)矩陣,1.3攝像機的運動描述,2019/12/6,12,RANSAC篩選的內(nèi)點,基礎(chǔ)矩陣：,1.3攝像機的運動描述,2019/12/6,13,利用內(nèi)參矩陣和基礎(chǔ)矩陣求本質(zhì)矩陣：,E的SVD分解為：,旋轉(zhuǎn)矩陣,位移向量,3）本質(zhì)矩陣及其分解,本質(zhì)矩陣的四種分解情況,本質(zhì)矩陣分解對應(yīng)多個可能的旋轉(zhuǎn)矩陣和位移向量：,1.3攝像機的運動描述,2019/12/6,14,圖像平面二維點的齊次坐標：,世界坐標系三維點維點的齊次坐標：,歸一化攝像機矩陣：,可以得到三維空間中的點和攝像機的相對位置關(guān)系。根據(jù)所有點都在攝像機前方，即,對于所有的點有：,可以判斷得到唯一正確的攝像機矩陣,4）三維重構(gòu)確定旋轉(zhuǎn)矩陣,通過旋轉(zhuǎn)矩陣和位移向量可以得到歸一化的攝像機矩陣：,從二維到三維：,得到正確的旋轉(zhuǎn)矩陣：,1.3攝像機的運動描述,2019/12/6,15,四種情況的三維重構(gòu),特征點云,特征點云,特征點云,特征點云,1.4航向角求解,2019/12/6,16,航向角主值,攝像機坐標系的X,Y,Z軸分別指向載體的右，下，前方向。先繞X軸，再繞Y軸，最后繞Z軸的歐拉角變換：,由本質(zhì)矩陣經(jīng)過分解和三維重構(gòu)可以得到攝像機正確的旋轉(zhuǎn)矩陣R：,1.4航向角求解,2019/12/6,17,每組水平偏轉(zhuǎn)2,圖片分辨率：20481536，拍攝地點新主樓E座11層過道，由轉(zhuǎn)臺得到角度真值,每組水平偏轉(zhuǎn)5,1.4航向角求解,2019/12/6,18,（單位：）,完成內(nèi)容,單目視覺航向測量,1,慣性航向測量,2,組合導航算法,3,導航軟件設(shè)計,4,2019/12/6,20,Z軸陀螺儀測量值gz不等于航向角速度，需要投影到導航坐標系：,慣性器件的安裝誤差,地面的起伏,載體的不平整,俯仰角,橫滾角,2.1捷聯(lián)航姿測量,2019/12/6,21,1）載體姿態(tài)描述,載體系到導航系的坐標變換矩陣為,按照歐拉角定義：,矩陣中包含載體三軸姿態(tài)信息，因此又稱為姿態(tài)矩陣。,2.1捷聯(lián)航姿測量,2019/12/6,22,載體姿態(tài)角可從姿態(tài)矩陣中提取：,3）姿態(tài)角提取,只使用捷聯(lián)測量姿態(tài)角誤差很快發(fā)散,姿態(tài)矩陣的微分方程為：,地球自轉(zhuǎn)角速度，陀螺儀精度無法測量,載體系相對地球角速度，可忽略,陀螺儀測量值可直接視為載體角速度,2）捷聯(lián)解算及其簡化,2.2加速度計校正姿態(tài)角,2019/12/6,當載體平穩(wěn)時，認為重力方向與水平面垂直，有：,加速度計測量俯仰角和滾轉(zhuǎn)角：,1）加速度計傾角儀,23,2.2加速度計校正姿態(tài)角,2019/12/6,24,融合前后效果對比,定義加權(quán)系數(shù)和為衰減系數(shù)K：,加權(quán)融合加表和陀螺儀測量的俯仰、滾轉(zhuǎn)角為：,2）加權(quán)融合加表與陀螺儀數(shù)據(jù),完成內(nèi)容,單目視覺航向測量,1,慣性航向測量,2,組合導航算法,3,導航軟件設(shè)計,4,3.1組合方式,2019/12/6,26,3.2系統(tǒng)模型推導,2019/12/6,27,1）卡爾曼濾波,3.2系統(tǒng)模型推導,2019/12/6,28,系統(tǒng)狀態(tài)量：,系統(tǒng)狀態(tài)方程：,系統(tǒng)觀測量：,系統(tǒng)量測方程：,dT為角速度的更新時間,編碼器測得的速度大小為velocity(k)：,2）方案一：航向融合+航位推算,一步狀態(tài)轉(zhuǎn)移矩陣：,量測矩陣：,3.2系統(tǒng)模型推導,2019/12/6,29,系統(tǒng)狀態(tài)量：,系統(tǒng)狀態(tài)方程：,系統(tǒng)觀測量：,系統(tǒng)量測方程：,dT為角速度的更新時間,3）方案二：航向、位置、速度融合估計,一步狀態(tài)轉(zhuǎn)移矩陣：,量測矩陣：,3.3實驗測試,2019/12/6,30,大恒MER-310-12UC數(shù)字攝像機,MEMS捷聯(lián)慣性模塊CrossbowIMU440,FLASHLIDARF4激光雷達,安裝有組合導航軟件的微型計算機,在內(nèi)部：光電編碼器4個STM32控制芯片一個,1）實驗測試平臺,3.3實驗測試,2019/12/6,31,航向角及其誤差,2）軟件仿真,3）靜態(tài)試驗,視覺航向誤差,慣性與濾波航向誤差,3.3實驗測試,2019/12/6,32,經(jīng)過靜態(tài)試驗測試，可以得到：視覺解算求得的航向角誤差平均值為0.274，方差為3.98。陀螺儀求得的航向角速度誤差平均值為0.2086/s，方差為0.0087。根據(jù)經(jīng)驗估計編碼器的方差為1cm/s可得到量測噪聲方差陣：,4）參數(shù)與模型選擇,3.3實驗測試,2019/12/6,33,不同模型軌跡比較,方案一,方案二,3.3實驗測試,2019/12/6,34,不同系統(tǒng)噪聲方差陣Q的參數(shù)設(shè)置對導航結(jié)果的影響,3.3實驗測試,2019/12/6,35,軌跡對比圖,5）最終濾波結(jié)果,航向角對比圖,完成內(nèi)容,單目視覺航向測量,1,慣性航向測量,2,組合導航算法,3,導航軟件設(shè)計,4,4.1軟件總體結(jié)構(gòu),2019/12/6,37,利用多線程的方式實現(xiàn)了同步采集多路傳感器數(shù)據(jù)，并且實時進行導航計算。,4.2數(shù)據(jù)采集解碼,2019/12/