基于雙目視覺的無人機(jī)懸停精度測(cè)量方法
【技術(shù)領(lǐng)域】
[0001] 本發(fā)明涉及一種基于雙目視覺的無人機(jī)懸停精度測(cè)量方法。
【背景技術(shù)】
[0002] 無人機(jī)懸停精度是無人機(jī)性能的一個(gè)重要指標(biāo)，反映了無人機(jī)的核心一一飛行控 制系統(tǒng)的穩(wěn)定性和精確性。目前，在對(duì)無人機(jī)飛行功能的檢驗(yàn)檢測(cè)過程中，懸停精度的測(cè)量 方法為人工觀察的方法，不能保證其安全性、客觀性和規(guī)范性。
[0003] 無人機(jī)三維空間定位技術(shù)主要有兩種方式：基于機(jī)載設(shè)備的定位方式和基于地面 設(shè)備的定位方式。
[0004] 根據(jù)機(jī)載設(shè)備類型的不同，基于機(jī)載設(shè)備的定位技術(shù)主要有3種：圖4(a)基于 GPS設(shè)備、圖4(b)基于機(jī)載視頻和圖4(c)基于慣性導(dǎo)航裝置的定位技術(shù)。機(jī)載設(shè)備集成在 每個(gè)無人機(jī)的飛行控制系統(tǒng)中，而不是獨(dú)立于無人機(jī)之外，因此靈活性較差，不適用于針對(duì) 不同無人機(jī)進(jìn)行定位的任務(wù)。
[0005] 基于地面設(shè)備的定位方式可以避免上述問題。根據(jù)地面設(shè)備類型的不同，基于地 面設(shè)備的定位技術(shù)可以分為3種：圖4(d)超聲波測(cè)距儀、圖4(e)激光測(cè)距儀、和圖4(f)基 于機(jī)器視覺的定位技術(shù)。其中，超聲波或激光測(cè)距儀多用于目標(biāo)距離的測(cè)量，而三維激光測(cè) 距儀的掃描速度較慢，主要應(yīng)用于靜態(tài)三維場(chǎng)景的重建，無法計(jì)算目標(biāo)的運(yùn)動(dòng)軌跡。

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

[0006] 本發(fā)明的目的就是為了解決上述問題，提供一種基于雙目視覺的無人機(jī)懸停精度 測(cè)量方法，它能夠?qū)崟r(shí)計(jì)算無人機(jī)的三維飛行軌跡，并自動(dòng)計(jì)算懸停精度，提高了測(cè)量的準(zhǔn) 確性與規(guī)范性。
[0007] 為了實(shí)現(xiàn)上述目的，本發(fā)明采用如下技術(shù)方案：
[0008] 基于雙目視覺的無人機(jī)懸停精度測(cè)量方法，包括如下步驟：
[0009] 步驟（1):標(biāo)定階段：利用張正友棋盤標(biāo)定法對(duì)相機(jī)進(jìn)行標(biāo)定，從而確定標(biāo)定參 數(shù)，定義標(biāo)定結(jié)果參數(shù)；
[0010] 步驟（2):定位階段：在進(jìn)行無人機(jī)懸停精度測(cè)量時(shí)，將滑軌放置于無人機(jī)懸停點(diǎn) 正下方，將雙目相機(jī)按照設(shè)定距離平行固定于滑軌上，且雙目相機(jī)能夠沿滑軌方向移動(dòng)，相 機(jī)鏡頭垂直向上放置，雙目相機(jī)成像平面應(yīng)位于同一平面，光軸相互平行；左目相機(jī)和右目 相機(jī)分別采集無人機(jī)圖像，傳輸至計(jì)算機(jī)；計(jì)算機(jī)根據(jù)采集到的左目圖像和右目圖像，結(jié)合 步驟（1)得到的標(biāo)定結(jié)果參數(shù)，計(jì)算無人機(jī)的三維位置坐標(biāo)；懸停結(jié)束后，根據(jù)無人機(jī)三維 軌跡計(jì)算懸停精度。
[0011] 所述步驟⑴的步驟為：
[0012] 步驟（1-1):將兩個(gè)相機(jī)固定在同一個(gè)滑軌上，定義距離L，調(diào)整滑軌上兩相機(jī)位 置，使其中心點(diǎn)之間的距離為L(zhǎng);
[0013] 步驟（1-2):采用張正友棋盤標(biāo)定法對(duì)相機(jī)進(jìn)行標(biāo)定，并記錄標(biāo)定結(jié)果參數(shù) result= {Mleft，Dleft，Mflght，Dfl氣R，T}。result表示標(biāo)定結(jié)果參數(shù)，Mleft和Dleft分別表示左 目相機(jī)的相機(jī)矩陣和畸變系數(shù)向量，M~ht和D~ht分別表示右目相機(jī)的相機(jī)矩陣和畸變系 數(shù)向量，R和T分別表示兩相機(jī)之間的旋轉(zhuǎn)矩陣和平移向量。對(duì)于每個(gè)相機(jī)，
[0014]
[0015] 其中，Μ為相機(jī)矩陣，fx，fy是以像素為單位的焦距。
[0016] 所述步驟（2)的步驟為：
[0017] 步驟（2-1):在進(jìn)行無人機(jī)懸停精度測(cè)量時(shí)，將滑軌放置于無人機(jī)懸停點(diǎn)正下方， 將雙目相機(jī)平行固定于滑軌上，相機(jī)鏡頭垂直向上放置，兩相機(jī)成像平面應(yīng)位于同一平面， 光軸相互平行；左目相機(jī)采集無人機(jī)左目圖像，右目相機(jī)采集無人機(jī)右目圖像；
[0018] 步驟（2-2):目標(biāo)區(qū)域定位：采用手動(dòng)選取的方式，選定左目圖像中的目標(biāo)區(qū)域；
[0019] 步驟（2-3):目標(biāo)跟蹤：利用TLD算法，對(duì)左目圖像中的目標(biāo)進(jìn)行跟蹤；
[0020] 步驟（2-4):目標(biāo)匹配：在右目圖像中，尋找與左目圖像的目標(biāo)區(qū)域最相似的匹配 區(qū)域；
[0021] 步驟（2-5):同名點(diǎn)匹配：分別利用左目和右目圖像中矩形目標(biāo)區(qū)域的中心點(diǎn)，作 為同名點(diǎn)；
[0022] 步驟（2-6):三維坐標(biāo)計(jì)算：建立相機(jī)的坐標(biāo)系，結(jié)合步驟（1)得到的標(biāo)定結(jié)果參 數(shù)，計(jì)算目標(biāo)點(diǎn)在相機(jī)坐標(biāo)系中的三維坐標(biāo)；
[0023] 步驟（2-7):懸停精度評(píng)估，根據(jù)目標(biāo)點(diǎn)的三維坐標(biāo)軌跡，計(jì)算無人機(jī)的懸停精 度。
[0024] 所述步驟（2-2)的步驟為：令開始對(duì)目標(biāo)進(jìn)行定位的時(shí)刻t=0。首先手動(dòng)框取 目標(biāo)區(qū)域，目標(biāo)區(qū)域?yàn)橐?amp;為左上角點(diǎn)，高h(yuǎn)、寬w的矩形區(qū)域。
[0025] 所述步驟（2-3)的步驟為：根據(jù)t=0時(shí)刻確定的左目圖像目標(biāo)區(qū)域，在t= 1及 其以后的時(shí)刻，采用TLD算法對(duì)左目圖像的目標(biāo)進(jìn)行跟蹤。
[0026] 所述步驟（2-4)的步驟為：每次得到左目圖像中的目標(biāo)區(qū)域后，在右目圖像中，尋 找與左目圖像的目標(biāo)區(qū)域最相似的匹配區(qū)域，匹配區(qū)域?yàn)橐訠R為左上角點(diǎn)，高h(yuǎn)、寬w的矩 形區(qū)域；
[0027] 則目標(biāo)匹配表不為：
[0028]
[0029] 其中，Ilrft表示左目圖像灰度值，Iright表示右目圖像灰度值，（Xt，h)表示點(diǎn)郵勺 坐標(biāo)，（Χ?，yd表示點(diǎn)隊(duì)的坐標(biāo)；此時(shí)搜尋范圍XRe[0,XJ，_yRe[_yL -含，,vL +f，其 中&為搜尋區(qū)域的高度。這樣，得到使得公式（1)最小的BR點(diǎn)坐標(biāo)（xR，yR)后，則視差d=xl-Xr；
[0030]在t彡1時(shí)刻，左目圖像通過TLD算法得到新的目標(biāo)區(qū)域后，對(duì)右目圖像的搜尋 范圍進(jìn)行更新
根據(jù)公式（1) 確定右目圖像中的目標(biāo)區(qū)域；以此類推，來計(jì)算每一幀左目圖像中的目標(biāo)區(qū)域，以及同一目 標(biāo)在右目圖像中的對(duì)應(yīng)區(qū)域。
[0031] 所述步驟（2-5)的步驟為：
[0032] 利用左目圖像中目標(biāo)區(qū)域的中心?
乍為左目目標(biāo)的同 名點(diǎn)，利用右目圖像中的目標(biāo)區(qū)域的中心點(diǎn)]
隹為右目目標(biāo)的同 名點(diǎn)。
[0033] 所述同名點(diǎn)即左目圖像和右目圖像中，對(duì)應(yīng)實(shí)際目標(biāo)同一部位的像素點(diǎn)，必須保 證前后幀、左右目圖像中的同名點(diǎn)對(duì)應(yīng)實(shí)際目標(biāo)的同一位置。
[0034] 所述步驟（2-6)的步驟為：
[0035] 相機(jī)坐標(biāo)系是以左目相機(jī)的光心(\為原點(diǎn)，X0J平面平行于成像平面，光軸方向 為Ζ軸，根據(jù)標(biāo)定的相機(jī)參數(shù)result，得到重投影矩陣
[0036]
[0037] 其中，(c丨.,為左相機(jī)的主點(diǎn)坐標(biāo)，(^，為右相機(jī)的主點(diǎn)坐標(biāo)；L為兩相機(jī)之 間平移矩陣的X軸分量；f1為左側(cè)相機(jī)焦距；
[0038] 在左右目光軸相互平行情況下，已知左目圖像同名點(diǎn)坐標(biāo)Pjk，yj和右目圖像同 名點(diǎn)坐標(biāo)PR (xR,yR)，計(jì)算目標(biāo)點(diǎn)在左右視圖的視差d= ，則令
[0039] _
' 'Λ _
[0040] 得到目標(biāo)點(diǎn)在相機(jī)坐標(biāo)系中的三維坐標(biāo)：
[0041 ]
[0042] 其中元、九、之和6為中間結(jié)果變量，X。、y。和ζ。分別為目標(biāo)點(diǎn)Ρ。在相機(jī)坐標(biāo)系 中的X、Y和Z軸坐標(biāo)。
[0043] 所述步驟（2-7)的步驟為：
[0044] 在無人機(jī)懸停時(shí)，采用步驟（2-5)實(shí)時(shí)計(jì)算無人機(jī)的三維位置坐標(biāo)，得到其飛行 軌跡；
[0045] 假設(shè)得到軌跡點(diǎn)的集合為P=防，P2, . . .，PN]，共包含N個(gè)點(diǎn)，其中
[0046] Pn=[xn,yn,zn]T,n= 1, 2, . . . ,N；
[0047] 飛行軌跡點(diǎn)集合的質(zhì)心
[0048] 進(jìn)行無人機(jī)懸停精度測(cè)試時(shí)，規(guī)定無人機(jī)懸停的離地高度記作Η。；
[0049] 在進(jìn)行懸停精度檢測(cè)時(shí)，將雙目相機(jī)放置于懸停的正下方，具體來說，是將左目相 機(jī)放置于懸停點(diǎn)并使光軸垂直于水平面；懸停精度分為定點(diǎn)懸停精度的水平偏差和 垂直偏差懸?？刂凭鹊乃狡瞵i和垂直偏差
[0050] 由于為坐標(biāo)系原點(diǎn)，計(jì)算公式為：
[0051]
[0052]
[0053]
[0054]
[0055] 其中，^和e￥分別為無人機(jī)懸停過程中，在以左目相機(jī)為原點(diǎn)的相機(jī)坐標(biāo)系下，X 軸方向和Y軸方向的運(yùn)動(dòng)范圍，2"表示無人機(jī)飛行軌跡點(diǎn)P"的Z軸坐標(biāo)。<