一種用于移動平臺下視覺著艦導航的位姿估計系統(tǒng)及方法
【技術領域】
[0001] 本發(fā)明屬于無人機自主著艦領域，具體涉及一種用于移動平臺下視覺著艦導航的 位姿估計系統(tǒng)及方法。
【背景技術】
[0002] 無人機自主著降能力是自主飛行能力的核心部分。在現(xiàn)有的著陸導航方式中，慣 性導航由于需要通過積分運算來得到飛行器大概位置和速度信息，使得誤差會隨著時間 的推移不斷增加；GPS(Global Positioning System)衛(wèi)星導航容易受到電子干擾；INS/ GPS(INS:Inertial Navigation System)組合導航系統(tǒng)只能定位自身的位姿信息，若在特 定地點實施降落，即便在提前知曉降落點經(jīng)煒度的情況下，也難以保證降落的精度。當需 要在移動平臺上實施自主降落時，除非能獲知平臺的實時姿態(tài)及位置信息，否則無法完成 降落任務。由此可知，單獨使用INS/GPS組合導航系統(tǒng)并不能提供無人機自主降落所需的 導航彳目息。
[0003] 現(xiàn)有的基于計算機視覺著陸導航技術主要解決無人機在靜止平臺上的著陸問題， 很少涉及到移動平臺上的著艦問題；并且對于獲取全部三維位姿信息的情況，現(xiàn)有技術一 般采用雙目立體視覺對場景進行三維重建而獲得目標的位姿信息，采用雙目立體視覺方法 可以處理大部分的場景，但重建后獲得的深度信息可能有深度丟失的情況從而導致位姿信 息計算不完整或錯誤；同時基于雙目視覺的三維重建計算量大，實時性不好。

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

[0004] 本發(fā)明的目的是提供一種用于移動平臺下視覺著艦導航的位姿估計系統(tǒng)及方法， 以便有效的降低開發(fā)成本，縮短視覺導航技術的研發(fā)周期，實現(xiàn)無人機在移動平臺上安全 著陸。
[0005] 本發(fā)明的技術方案是：一種用于移動平臺下視覺著艦導航的位姿估計方法，具體 步驟是：
[0006] 步驟101 :開始對無人機視覺著艦導航進行位姿估計；
[0007] 步驟102 :設計一個由兩個長3米、寬1米的矩形組成的黑色"T"型地標；正視地 標時，左上角角點定為地標坐標系原點，坐標為（〇,〇);
[0008] 步驟103 :由全球定位系統(tǒng)導航和慣性導航組成的組合導航方式引導無人機飛 行；
[0009] 步驟104 :通過目標匹配判斷無人機是否到達著艦地標上空，若是則執(zhí)行步驟 105,否則執(zhí)行步驟103,繼續(xù)引導飛行；
[0010] 步驟105 :攝像機獲取圖像；
[0011] 步驟106 :對獲取的圖像進行二值化、形態(tài)學運算、輪廓提取處理，為步驟108提取 角點做準備；
[0012] 步驟107 :運用直方圖匹配算法進行目標匹配；
[0013] 步驟108 :對匹配成功的圖像提取8個Harris角點，結果為像素坐標系下y坐標 最小的點為初始點，其余各點依輪廓順時針排列，并保留每個角點的像素坐標；
[0014] 步驟109 :跟蹤步驟108提取的角點；
[0015] 步驟110 :判斷步驟108得到的角點排列方式，當a、b兩點之間距離大于a、h兩點 之間距離時步驟109所得排序即為正確順序，當a、b兩點之間距離小于a、h兩點之間距離 時需要對所有點重新排序，遍歷輪廓，取x坐標最小的點為初始點，其余各點按輪廓順時針 依次排序；
[0016] 步驟111 :使用步驟110得到的角點像素坐標、初始化得到的角點世界坐標、攝像 機參數(shù)矩陣、各坐標系之間的關系求解位姿估計值；
[0017] 步驟112 :對由慣性傳感器和全球定位系統(tǒng)估計到的位姿數(shù)據(jù)和視覺算法解算的 位姿數(shù)據(jù)進行融合，得到最終結果；
[0018] 步驟113 :判斷位姿估計算法是否完成，如果是，則視覺著艦導航位姿估計算法結 束，否則轉至步驟103;
[0019] 步驟114 :視覺著艦導航位姿估計算法結束。
[0020] 一種用于移動平臺下視覺著艦導航的位姿估計系統(tǒng)，其特征是包括：
[0021] 地標設計模塊，用于提供需要的地標；
[0022] 圖像處理模塊，用于對當前幀圖像進行圖像處理，使用多邊形擬合算法對著陸地 標進行多邊形擬合并進行直方圖匹配，匹配成功后提取8個Harris角點，對所得角點順時 針排序與著陸地標對應后將結果傳至位姿解算模塊；
[0023] 數(shù)據(jù)融合模塊，用于提高位姿估計的準確性；
[0024] 位姿解算模塊，用于計算移動艦船相對于無人機的位姿數(shù)據(jù)；首先計算無人機相 對于著陸地標的旋轉矩陣和平移矩陣，然后通過歐拉角與旋轉矩陣關系得到姿態(tài)角估計 值，通過平移矩陣和系統(tǒng)坐標系轉換關系計算得到位置估計值，最后，用求得的位姿數(shù)據(jù)減 去由慣性傳感器和全球定位系統(tǒng)得到的無人機的位姿數(shù)據(jù)即可得到船相對于無人機的位 姿數(shù)據(jù)，實時估計位姿數(shù)據(jù)；
[0025] 角點提取模塊，對攝像機模塊傳送的圖像數(shù)據(jù)進行角點提取，把提取到的8個角 點送至角點跟蹤模塊；
[0026] 角點跟蹤模塊，把跟蹤的角點送至角點排序模塊；
[0027] 角點排序模塊，對所得到地標角點進行排序，與物理坐標系下著陸地標各角點 一一對應并將角點序列傳送至位姿解算模塊。
[0028] 所述的位姿解算模塊使用攝像機內(nèi)參矩陣、三維空間到二維平面的投影關系計算 攝像機坐標系相對于著艦地標坐標系的旋轉平移矩陣RT，并利用歐拉角與旋轉矩陣關系得 到姿態(tài)角估計值；使用姿態(tài)角估計所計算得到的平移矩陣和系統(tǒng)坐標系轉換關系計算得到 位置估計值，再用求得的位姿數(shù)據(jù)減去由慣性傳感器和全球定位系統(tǒng)得到的無人機的位姿 數(shù)據(jù)即可得到船相對于無人機的位姿數(shù)據(jù)，然后經(jīng)過數(shù)據(jù)融合模塊得到最終數(shù)據(jù)。
[0029] 所述的地標設計模塊設計了由兩個長3米、寬1米的矩形組成的"T"型地標，地標 的顏色為黑色，比值長3米、寬1米可以保證8個角點的提取和排序；正視地標時，左上角 角點定為地標坐標系原點，坐標為（〇, 〇)，其余各點順時針排序，坐標依次為（3, 0)、（3, 1)、 (2, 1)、（2, 4)、（1，4)、（1，1)、（0, 1)〇
[0030] 所述圖像處理模塊，包括圖像預處理模塊、角點提取模塊、角點跟蹤模塊和角點排 序模塊；圖像預處理模塊首先對攝像機拍攝到的圖像進行灰度化、二值化、形態(tài)學運算、輪 廓提取、直方圖匹配處理；角點提取模塊用于提取"T"型地標的8個Harris角點，從左上角 開始順時針依次標記為a，b，c，d，e，f，g，h;角點跟蹤模塊是運用高斯光流法跟蹤運動中的 角點，防止目標丟失；角點排序模塊用于將所保留的角點順序為像素坐標系下y坐標最小 的點為初始點，其余各點順時針依次排列；當角點排序模塊所得圖像角點a、b兩點之間距 離大于a、h兩點之間距離時的角點排序即為正確順序；當角點排序所得圖像角點a、b兩點 之間距離小于a、h兩點之間距離時需要對所有點重新排序，遍歷輪廓，取圖像像素坐標系 下x坐標最小的點為初始點，其余各點按順時針依次排序；角點排序模塊所得圖像角點最 終排序結果和物理坐標系下地標各角點一一對應。
[0031] 所述的位姿解算模塊，規(guī)定無人機的俯仰角、滾轉角、偏航角分別記為、Q2、 a2，船的俯仰角、滾轉角、偏航角分別記為:約、h、ai，其中無人機自身的姿態(tài)角可通過 陀螺儀和加速度計這樣的傳感器得到，船相對于無人機的姿態(tài)角通過位姿解算模塊計算得 至IJ;可通過根據(jù)圖像坐標系、攝像機坐標系、機體坐標系和世界坐標系之間的關系可推導出 像素坐標系和世界坐標系之間的關系：
[0032] 如下式
[0033] XYt=xd
[0034] 式中X=[xjdywydzwydyd-xwxd-ywxd-zwxd]是已知的，Y= [Pi/typ2/tyP3/tytx/ tyP4/tyP5/tyP6/ty]是待求參數(shù)；(xd,yd)是角點像素坐標，（xw,yw,zw)為角點世界坐標；當 存在N> = 7個已知點時，方程具有唯一解或最小二乘解；視覺算法中提取了8個角點進行 解算，在保證有解的前提下計算量最??；因此把攝像機內(nèi)參矩陣M、8個地標角點物理坐標 (xd，yd)和世界坐標（\，7"，2")，并帶入公式從而得到式子￥中的未知參數(shù)；從而得到旋轉平 移矩陣RT;
[0035]
[0036] 式中T= [txtytz]T為3X1的平移向量，tx、ty、tj別是x、y、z方向的平移量；p =[PxPyPJ為 3X3 的旋轉矩陣，其中px=[PiP4P7]T，Py= [P2P5PS]T，PZ= [P3P6P9] T分別為世界坐標系Xw軸，Yw軸，Zw軸在攝像機坐標系中的方向向量，其中pi、p4、p7對應世 界坐標系Xw軸在攝像機坐標系中方向向量的x、y、z坐標值，p2、p5、ps對應世界坐標系丫"軸 在攝像機坐標系中方向向量的x、y、z坐標值，p3、p6、p9對應世界坐標系Z"軸在攝像機坐標 系中方向向量的x、y、z坐標值；根據(jù)各約束對應關系可得
[0037]
[0038] 式中，求得的總俯仰角辦、滾轉角Q、偏航角a ;由相對關系知船的姿態(tài)角為 (Q-Q2a-a2妒-％ )，位置估計用于計算艦船相對于無人機的位置，已知著艦地標的 原點在世界坐標系中的坐標為（x，y，z)和姿態(tài)估計所求得的平移向量（tx，ty，tz)