一種用于自主空中加油的圓形錐套視覺位姿解算方法
【專利摘要】本發明公開了一種用于自主空中加油的圓形錐套視覺位姿解算方法，該方法利用加油錐套的結構特點，先建立原點為攝像機光心、XY平面與加油口平面平行的世界坐標系，然后分別在攝像機坐標系和世界坐標系下建立圓形投影錐面方程，并根據“旋轉變換不改變系數矩陣特征值”這一特性，計算世界坐標系下圓形投影錐面方程系數矩陣中的未知參數，進而可求解受油插頭和加油錐套之間的位置和姿態信息。該方法思路清晰，計算過程簡單，通用性和便利性好，可獲得位姿參數的精確解。
【專利說明】—種用于自主空中加油的圓形錐套視覺位姿解算方法
【技術領域】
[0001]本發明涉及一種用于自主空中加油的圓形錐套視覺位姿解算方法。
【背景技術】
[0002]空中加油能夠延長航空器的飛行時間和作用范圍，而自主空中加油(AerialAutonomous Refueling,AAR)可為有人機空中加油降低風險,還可用于無人機,是現代航空領域的重要挑戰之一。目前我國主要采用插頭-錐套式(軟式)空中加油方式，即加油機拖曳一根軟管，軟管末端掛載傘狀的加油錐套，受油機通過飛行控制使得安裝在機頭區域的受油插頭與加油錐套完成對接，進而實現油料的空中傳送。
[0003]在此過程中，較為關鍵的一步是受油機在近距(距離在Im?20m之間)的情況下如何獲得加油錐套與受油插頭之間的精確相對位置和姿態信息，一般至少要求在對接時(即距離為Im以內)的位置誤差要小于10cm，加油錐套端面中心與受油插頭頂點的連線和加油口中軸線之間的角度偏差要小于10°。考慮到加油機尾流和大氣紊流的影響，INS (Inertial Navigat1n System,慣性導航系統)和 GPS (Global Posit1ning System,全球定位系統)等傳統的導航方式定位精度較低，通常采用近距定位精度高、價格便宜、重量輕的視覺導航系統。為了提高系統的可靠性和精度，也會利用INS與視覺導航相組合的方式。
[0004]現有的大多數AAR視覺方法常采用特殊的光學標記安裝在加油錐套上，包括LED光標、人工特殊顏色標記等。如美國國防預研局和NASA Dryden飛行研究中心早期聯合發起的自主空中加油項目(AAR)中就使用了一套VisNav系統，該VisNav系統就需要在加油錐套上安裝多個LED光標，而后利用半導體位置探測器PSD進行圖像定位(見董新民，徐躍鑒，陳博，《自動空中加油技術研究進展與關鍵問題》，《空軍工程大學學報(自然科學版)》，2008(12),9(6):1-5)。另外，國內空軍工程大學的王旭峰等建立了自主空中加油的一個視覺相對導航半物理地面試驗平臺，并在加油錐套斷面區域加裝了紅色標識環帶，以增強特征區域與背景圖像的對比度，從而可通過色彩識別獲得圖像定位信息(見王旭峰，董新民，孔星煒，《機器視覺輔助的插頭錐套式無人機自主空中加油仿真》，《科學技術與工程》，2013 (6)，13 (18) =5245-5250)。而北航的解洪文等則在加油錐管端面上放置多個光學標記，并采用雙目視覺的方式求解相對位置和姿態參數(見解洪文，王宏倫，《基于雙目視覺的自動空中加油近距導航方法》，《北京航空航天大學學報》，2011，37 (2) =206-209)。
[0005]上述這些方法要求加油錐套安裝額外的光學標記，特別是需要供電的LED光標，增加了空中加油操作的風險。因此，一種不依靠額外標記、僅利用加油錐套本身特征的AAR視覺方法就具有更好的通用性、便利性和安全性。
[0006]通過對加油錐套形狀特征的分析，可知其內部加油口為圓形的，半徑約為13cm，成像后在圖像中呈現明顯的黑色圓塊或橢圓塊，利用這一明顯的自身形狀特征可降低圖像定位處理的計算量，精確的圖像定位結果能夠為進一步的圓形特征視覺位姿解算提高精度。隨著無人機的發展，自主空中加油的需求也就越來越迫切，而不依靠額外光學標記的自主空中加油視覺導航方法具有較高的通用性和便利性，特別是基于加油錐套內部圓形加油口的計算機視覺方法特征明顯，圖像處理速度快，后續的視覺位姿解算過程計算量小。
【發明內容】
[0007]發明目的:為了克服現有技術中存在的自主空中加油方法中需要安裝額外光學標記的不足、額外的帶電裝置帶來更高的空中加油危險性的不足等，本發明從僅依賴加油錐套自身特征出發，提供一種用于自主空中加油的圓形錐套視覺位姿解算方法，該方法從投影變換關系出發，分別在攝像機坐標系和世界坐標系下建立圓形投影錐面方程，并根據“旋轉變換不改變系數矩陣特征值”這一特性，計算世界坐標系下圓形投影錐面方程系數矩陣中的未知參數，進而可求解攝像機和加油錐套之間的位置和姿態信息，進一步可獲得受油機受油插頭和加油機加油錐套之間的相對位姿參數。
[0008]技術方案:為實現上述目的，本發明采用的技術方案為:
[0009]一種用于自主空中加油的圓形錐套視覺位姿解算方法，僅利用加油錐套自身內部圓形加油口特征，分別在攝像機坐標系和世界坐標系下建立圓形投影錐面方程，并根據旋轉變換不改變系數矩陣特征值這一特性，計算世界坐標系下圓形投影錐面方程系數矩陣中的未知參數，進而可求解受油插頭和加油錐套之間的位置和姿態信息，具體包括如下步驟:
[0010]步驟1、載入內部加油口成像橢圓在圖像坐標系下的平面方程、攝像機標定矩陣和圓形特征的半徑，具體包括:
[0011]步驟101、根據加油錐套內部加油口的圖像定位結果，提取內部加油口成像橢圓的邊緣像素點坐標，并利用最小二乘法確定橢圓平面方程系數:
[0012]f (px, Py) = (P1)tA(Pj) = O
[0013]式中，P= (px, py)T為圖像坐標系下內部加油口成像橢圓的邊緣像素點坐標，上標I 表示齊次坐標，即 P1 = (pT, 1)t = (px, Py, 1)T, A = (a^-)、i, j = 1，2，3, A 為 3X3 系數矩陣；
[0014]步驟102、進行攝像機標定實驗，并載入攝像機標定矩陣K，形式如下:
【權利要求】
1.一種用于自主空中加油的圓形錐套視覺位姿解算方法，其特征在于:僅利用加油錐套自身內部圓形加油口特征，分別在攝像機坐標系和世界坐標系下建立圓形投影錐面方程，并根據旋轉變換不改變系數矩陣特征值這一特性，計算世界坐標系下圓形投影錐面方程系數矩陣中的未知參數，進而可求解受油插頭和加油錐套之間的位置和姿態信息，具體包括如下步驟: 步驟1、載入內部加油口成像橢圓在圖像坐標系下的平面方程、攝像機標定矩陣和圓形特征的半徑，具體包括: 步驟101、根據加油錐套內部加油口的圖像定位結果，提取內部加油口成像橢圓的邊緣像素點坐標，并利用最小二乘法確定橢圓平面方程系數:f (Px, Py) = (P1)tA (p1) = O 式中，P = (Px, Py) τ為圖像坐標系下內部加油口成像橢圓的邊緣像素點坐標，上標I表示齊次坐標，即 P1 = (PT, 1)t = (px, Py, 1)T, A = (aij)、i, j = 1，2，3, A 為 3X3 系數矩陣；步驟102、進行攝像機標定實驗，并載入攝像機標定矩陣K，形式如下:
式中，(fcx, fcy)為焦距長度,其單位為像素，(ccx, ccy)為中心點坐標,其單位為像素，a c為偏尚率，無量綱； 步驟103、載入圓形特征的半徑r，單位為m ； 步驟2、計算圓形射影錐面在攝像機坐標系下的方程系數P，具體包括: 步驟201、利用攝像機標定矩陣，計算在單位焦距情況下成像橢圓的平面方程: g(qx, qy) = (qOWq1) = O, B = K1AK 式中，q= (qx，qy)T為單位焦距對應的圖像坐標系下內部加油口成像橢圓的邊緣像素點坐標； 步驟202、根據攝像機坐標系與單位焦距圖像坐標系之間點的投影變換關系: c = czqJ 式中，C= (cx，cy，cz)T為攝像機坐標系下內部加油口的圓形投影錐面上除原點以外的所有點，可確定圓形投影錐面在攝像機坐標系下的方程如下:
步驟3、計算圓形投影錐面在世界坐標系下的方程系數M，具體包括: 步驟301、建立世界坐標系Ow-XwYwZw,使得XY平面與圓形加油口平面平行，坐標原點在攝像機光心位置，圓形加油口圓心的Y坐標為O，且X坐標為非負值； 步驟302、確定圓形加油口邊緣點在世界坐標系下的方程:
式中，w = (wx, wy, wz)T為世界坐標系下圓形加油口邊緣點坐標，w0 = (m，0，h)T是世界坐標系下內部加油口圓心坐標，其中m和h是未知數，且m3 O ； 步驟303、根據投影變換關系，計算圓形投影錐面在世界坐標系下的方程: ('.V1+ U./ = C-^r)1 ' h ' h 寫成矩陣形式，即:
IO—m / h wT eMeW = O,M= OIO
-m / hO(nr -r2)/ Ir 式中，W= (Wx，Wy，Wz)T為世界坐標系下內部加油口的圓形投影錐面上除原點以外的所有點； 步驟4、確定世界坐標系下的內部加油口圓心坐標，具體包括: 步驟401、計算系數矩陣M的特征值，有如下關系:
99I
nr -r ,r
Ili = I, !h+ηλ=~Γ?~ + \,η?η,=~—
"h~" Ir 即:(1-%)(卜-fso
"Zr 式中，(H1, η 2) η3)為M的特征值，且根據n 2 η 3〈0和(1- n 2) (1- n 3)彡O設η 2 彡 η!>ο> η 3 ； 步驟402、計算系數矩陣B的特征值，并按照如下順序排列: 入I，入2，入3且入2 $入I〉入3 式中，A1,入2，入3 SB的特征值； 步驟403、根據旋轉變換不改變系數矩陣的特征值，可知系數矩陣M和系數矩陣B的特征值滿足如下關系， '11 Fzl1' "2 — H ，即 A7 /L _"3_ _Λ_ΛΛ 其中，μ為常數因子； 步驟404，根據步驟401中的關系式可確定矩陣M中的未知數m和h: h = r /, in = ι^{\-ηζ){\-η,)Ι(η2η,) 進而可確定世界坐標系下的內部加油口圓心坐標Wtl = (m, 0，h)τ ; 步驟5、確定世界坐標系與攝像機坐標系之間的旋轉變換矩陣R，具體包括: 步驟501、對系數矩陣M對角化，
>1 "
M = U1A1U11, A1 - //
_ %_ 式中，U1為正交矩陣,其行列式為I ; 步驟502、對系數矩陣P對角化:
式中，U2為正交矩陣,其彳」A為I ; 步驟503、確定世界坐標系與攝像機坐標系之間的旋轉變換矩陣R: w = Re, R = U1UU21" 式中，U依次取以下4個矩陣，以得到R的四個解:
步驟6、計算攝像機坐標系下內部加油口的圓心坐標和所在平面的法線向量，具體包括: 步驟601、計算攝像機坐標系下內部加油口的圓心坐標:
cO = (Cx0) cy0? CZ0) T = RtWo 由步驟503可確定R的四個解，根據約束cz(l〈0以剔除其中兩個解，保留兩個解； 步驟602、確定攝像機坐標系下內部加油口所在平面的法線向量:
U0 = (ux0, uy0, uz0)T = Rt.(O, O, 1)τ 此時Utl有兩個解，考慮到加油機處于平穩飛行階段，進一步可根據當前飛機飛行姿態可剔除掉其中一個解，即保留唯一確定的R ; 步驟603、根據唯一確定的R，確定攝像機坐標系下內部加油口的圓心坐標:
cO = (Cx0) cy0? CZ0) T = RtWo 步驟7、輸出受油插頭與加油錐套之間的相對位姿參數，具體包括: 步驟701、預先標定攝像機坐標系下攝像機光心與受油插頭之間的三維相對位置:
Cb0 一 (Cxb0，CybO，Czbo) 以及世界坐標系下圓形加油口圓心與加油錐套端面中心之間的三維相對位置:
Wb0 = (Wxb0.Wyb0，wZbO) T 步驟702、根據位置偏移關系，輸出攝像機坐標系下受油插頭與加油錐套之間的相對位置:
cr = (cxr，cyr, czr)T = c0+RTwb0-cb0 步驟703、定義受油機機體坐標系如下:飛機質心為原點、X軸指向機頭方向、Z軸在機體對稱面內并指向地面； 定義加油錐套固連坐標系如下:加油口圓心為原點、Z軸平行于加油口平面法向量并指向軟管、Y軸在加油口平面內并與當地水平面平行； 定義加油錐套相對于受油機的姿態角如下:受油機機體坐標系先繞Z軸旋轉α，接著繞Y軸旋轉β，最后繞X軸旋轉Y，使得機體坐標系三個軸與加油錐套固連坐標系平行；預先標定攝像機坐標系與受油機機體坐標系之間的旋轉變換關系Rc*,計算受油機機體坐標系下內部加油口的圓心坐標和所在平面的法線向量:
Ub0 — (Ubxtl，uby0) %ζθ) — ^cb.U0步驟704、輸出受油機與加油錐套之間的相對姿態，包括航向角α和俯仰角β，
【文檔編號】G01C21/20GK104180808SQ201410382576
【公開日】2014年12月3日 申請日期:2014年8月5日 優先權日:2014年8月5日
【發明者】黃斌, 孫永榮, 王瀟瀟, 熊智, 楊博文, 曾慶化, 單堯 申請人:南京航空航天大學