本發(fā)明屬于視覺測(cè)量領(lǐng)域，具體涉及一種繞飛軌跡下的旋翼無人機(jī)三維目標(biāo)定位方法。
背景技術(shù)：
：旋翼無人機(jī)以成本低、垂直起降和空中懸停等特點(diǎn)，在偵查、農(nóng)業(yè)保險(xiǎn)、環(huán)保和災(zāi)后救援等領(lǐng)域獲得了廣泛應(yīng)用。而基于視覺的旋翼無人機(jī)目標(biāo)定位已是目前的研究熱點(diǎn)問題之一，采用視覺方法對(duì)目標(biāo)進(jìn)行三維定位，首先通過三角定位方法確定無人機(jī)與目標(biāo)的相對(duì)高度，然后才能進(jìn)行目標(biāo)的定位?？紤]到旋翼無人機(jī)配備的低精度ahrs航姿參考系統(tǒng)所帶來的航向偏差較大，利用無人機(jī)拍攝的圖像進(jìn)行視覺測(cè)量時(shí)，圖像中投射的光線均發(fā)生一定的偏移。若采用傳統(tǒng)的三角定位方法,從左右視圖投射的兩組光線由于發(fā)生了偏移，求解得到的相對(duì)高度將產(chǎn)生較大的誤差，故無法準(zhǔn)確計(jì)算無人機(jī)與物體間的相對(duì)高度，從而不能對(duì)目標(biāo)進(jìn)行有效的三維定位。技術(shù)實(shí)現(xiàn)要素：有鑒于此，本發(fā)明提供了一種繞飛軌跡下的旋翼無人機(jī)三維目標(biāo)定位方法，能夠計(jì)算得到航向偏差，減小對(duì)相對(duì)高度的計(jì)算誤差，從而提高旋翼無人機(jī)對(duì)目標(biāo)的三維定位能力。有益效果：(1)本發(fā)明所提供的方法針對(duì)配備低精度ahrs航姿參考系統(tǒng)系統(tǒng)的旋翼無人機(jī)，能精確地計(jì)算ahrs航姿參考系統(tǒng)存在的航向偏差，進(jìn)而計(jì)算繞飛軌跡下旋翼無人機(jī)與目標(biāo)所在地形之間的高度，從而實(shí)現(xiàn)旋翼無人機(jī)對(duì)目標(biāo)的三維視覺定位。附圖說明圖1為本發(fā)明的旋翼無人機(jī)目標(biāo)三維定位系統(tǒng)結(jié)構(gòu)圖；圖2為本發(fā)明所提供方法的流程圖；圖3為本發(fā)明所使用的旋轉(zhuǎn)視圖雙目視覺模型示意圖；圖4為本發(fā)明所使用的單目攝像機(jī)測(cè)距模型示意圖；圖5為本發(fā)明所提供方法中的迭代過程流程圖；圖6為本發(fā)明所提供方法中的的數(shù)據(jù)擬合曲線；圖7為本發(fā)明所提供方法中的的數(shù)據(jù)擬合曲線；圖8為本發(fā)明所提供方法的定位效果圖。具體實(shí)施方式下面結(jié)合附圖并舉實(shí)施例，對(duì)本發(fā)明進(jìn)行詳細(xì)描述。搭建如下實(shí)驗(yàn)平臺(tái)對(duì)本發(fā)明的有效性進(jìn)行驗(yàn)證，使用一架t650四旋翼無人機(jī)，一臺(tái)筆記本作為地面站，無人機(jī)與地面站之間可進(jìn)行實(shí)時(shí)通信，系統(tǒng)結(jié)構(gòu)如圖1所示。對(duì)于無人機(jī)，機(jī)上帶有g(shù)ps定位系統(tǒng)、ahrs航姿參考系統(tǒng)、高程計(jì)、無線圖像傳輸模塊和無線數(shù)據(jù)收發(fā)模塊，使3drobotics公司的apm飛行控制系統(tǒng)工作在自穩(wěn)模式來保證無人機(jī)的穩(wěn)定飛行。在無人機(jī)的機(jī)頭位置安裝攝像機(jī)，俯視角β為45°，并通過無線圖像傳輸模塊回傳圖像到地面站，而由gps定位系統(tǒng)、ahrs航姿參考系統(tǒng)和高程計(jì)分別獲得的無人機(jī)的位置、姿態(tài)和高程信息則通過無線數(shù)據(jù)收發(fā)模塊傳輸?shù)降孛嬲?。地面站以?jì)算機(jī)為主體，運(yùn)行無人機(jī)視覺定位等算法，使用usb接口連接無線數(shù)據(jù)收發(fā)模塊，實(shí)現(xiàn)無人機(jī)與地面站的相互通信?；谠搶?shí)驗(yàn)平臺(tái)，如圖2所示，一種繞飛軌跡下的旋翼無人機(jī)三維目標(biāo)定位方法，包括以下步驟：步驟一、系統(tǒng)啟動(dòng)后，利用搭載在無人機(jī)上的攝像機(jī)拍攝圖像，并將圖像回傳到地面站；步驟二、從回傳的圖像中選擇具有清晰輪廓的靜態(tài)物體作為標(biāo)志物，并對(duì)標(biāo)志物進(jìn)行視覺識(shí)別；步驟二中對(duì)于標(biāo)志物進(jìn)行視覺識(shí)別的具體過程如下：運(yùn)用sift算法對(duì)標(biāo)志物進(jìn)行識(shí)別，得到m個(gè)特征點(diǎn)p1,p2...pm-1,pm，并將這些特征點(diǎn)進(jìn)行存儲(chǔ)作為模板，m為整數(shù)；步驟三、旋翼無人機(jī)以該標(biāo)志物為中心繞飛，并利用視覺識(shí)別的結(jié)果對(duì)標(biāo)志物進(jìn)行多點(diǎn)圖像測(cè)量，基于雙目視覺模型和線性回歸模型相互迭代的方法計(jì)算無人機(jī)相對(duì)于目標(biāo)所在地形的高度和航向偏差；步驟三的流程圖如圖5所示，具體過程如下：步驟3.1、旋翼無人機(jī)在繞飛軌跡下利用視覺識(shí)別分別對(duì)n個(gè)圖像按時(shí)間順序進(jìn)行測(cè)量，采用sift算法對(duì)當(dāng)前第i個(gè)圖像進(jìn)行特征提取(1≤i≤n)，然后利用模板中的特征點(diǎn)與當(dāng)前圖像的特征點(diǎn)進(jìn)行匹配，得到w組匹配點(diǎn)p1,p2...pw-1,pw(w≤m)，最后取這些匹配點(diǎn)的幾何中心pf(f≤w)代表標(biāo)志物在圖像中的像素位置,記為并記錄在對(duì)第i個(gè)圖像測(cè)量時(shí)的測(cè)量值，包括：無人機(jī)拍攝點(diǎn)oi在慣性參考系{i}的位置和姿態(tài)(ψi,θi,φi)，ψi,θi,φi分別為方位角，俯仰角和橫滾角。步驟3.2、選取n個(gè)圖像中的任意兩個(gè)圖像，共有n組，其中把在先測(cè)量的圖像作為左視圖l，在后測(cè)量的圖像作為右視圖r，構(gòu)成旋轉(zhuǎn)視圖的雙目視覺模型，如圖3所示。計(jì)算無人機(jī)相對(duì)于標(biāo)志物的相對(duì)高度hj，1≤j≤n其中，標(biāo)志物在左、右視圖的像素位置分別為rl，tl分別為左視圖對(duì)應(yīng)的無人機(jī)拍攝點(diǎn)ol相對(duì)于慣性參考坐標(biāo)系的旋轉(zhuǎn)矩陣和平移矩陣，其中，ψl,θl,φl分別為左視圖無人機(jī)拍攝點(diǎn)ol的航向角，俯仰角和橫滾角，ψr,θr,φr分別為右視圖無人機(jī)拍攝點(diǎn)ol航向角，俯仰角和橫滾角，δψ為航向偏差，有ψl＝ψi-δψ(k),k為迭代次數(shù)，θl＝θi，φl＝φi(1≤i＜n),設(shè)初始值δψ(0)＝0；rr，tr分別為右視圖對(duì)應(yīng)的無人機(jī)拍攝點(diǎn)or相對(duì)于慣性參考坐標(biāo)系的旋轉(zhuǎn)矩陣和平移矩陣，其中，ψr＝ψm-δψ(k)，θr＝θm，φr＝φm(i＜m≤n)左視圖和右視圖對(duì)應(yīng)的無人機(jī)拍攝點(diǎn)在慣性參考坐標(biāo)系下的坐標(biāo)分別為和r，t為右視圖對(duì)應(yīng)的攝像機(jī)坐標(biāo)系相對(duì)于左視圖對(duì)應(yīng)的攝像機(jī)坐標(biāo)系的旋轉(zhuǎn)矩陣、平移矩陣，r＝rrrlt，t＝tl-rttr＝rl(or-ol)；m＝[pl-rtprpl×rtpr]-1t。步驟3.3、針對(duì)計(jì)算得到的n組相對(duì)高度hj，用3σ準(zhǔn)則剔除粗大誤差，然后求n組平均值步驟3.4、獲得相對(duì)高度后，利用步驟3.2中的n點(diǎn)測(cè)量值并基于線性回歸模型計(jì)算航向偏差δψ(k)；一般地，[xyz]t，[xpypzp]t分別表示無人機(jī)和物體在慣性參考坐標(biāo)系{i}的坐標(biāo)，(xf′,yf′)表示物體在圖像中的像素位置，f為攝像機(jī)的焦距，攝像機(jī)的測(cè)距模型為姿態(tài)矩陣為其中，h'為無人機(jī)與物體之間的相對(duì)高度，(ψ′,θ′,φ′)表示無人機(jī)在某個(gè)測(cè)量點(diǎn)的航向角、俯仰角和橫滾角，其中，俯仰角θ′、橫滾角φ′的測(cè)量精度高，其誤差忽略不計(jì)，而航向角ψ′的測(cè)量存在較大的航向偏差。在本實(shí)施例中，為了計(jì)算航向角的航向偏差，利用無人機(jī)在不同位置拍攝的標(biāo)志物的n點(diǎn)測(cè)量值，并通過線性回歸方法進(jìn)行求解，具體計(jì)算過程如下：[xgygzg]t表示標(biāo)志物在慣性參考坐標(biāo)系{i}的坐標(biāo)，令[xpypzp]t＝[xgygzg]t，為無人機(jī)與標(biāo)志物的相對(duì)高度的平均值，令代入公式(4)，可得設(shè)參數(shù)θ＝[θa,θb]t，θa＝[xg,yg]t，θb＝δψ(k)，位置和姿態(tài)的量測(cè)方程分別為式(6)和式(7)：z1(i)＝y(tǒng)1(i)+v1,v1～n(0,r1)(6)其中v1，v2為測(cè)量噪聲，r1，r2為實(shí)對(duì)稱正定陣。則式(5)變形為其中,為姿態(tài)偏差,運(yùn)用泰勒展開，式(8)變?yōu)橛墒?8)和式(9)，得設(shè)矩陣其中a1,3～a2,5表示矩陣ai中對(duì)應(yīng)的元素；矩陣其中b1,1～b2,3表示在矩陣bi中對(duì)應(yīng)的元素。在本實(shí)施例中，對(duì)同一標(biāo)志物進(jìn)行n點(diǎn)視覺測(cè)量，故對(duì)應(yīng)的矩陣為a1,…,an，b1,…,bn，通過這些測(cè)量值得到如下線性回歸模型，其中，i2為2×2的單位矩陣，噪聲為v～n(0,r)協(xié)方差矩陣為參數(shù)θ的估計(jì)值為通過式(12)可求解航向偏差δψ(k)。步驟3.5、設(shè)e為常數(shù)，若|δψ(k)-δψ(k-1)|＜e，則得到最終的相對(duì)高度的估計(jì)值和航向偏差的估計(jì)值并執(zhí)行步驟四；否則，,轉(zhuǎn)到步驟3.2，將當(dāng)前的δψ(k)代入左右視圖航向角的計(jì)算公式中，求出從而進(jìn)行迭代計(jì)算。步驟四、在相對(duì)高度和航向偏差均有效估計(jì)的條件下，選擇攝像機(jī)視野里的任一目標(biāo)并獲得該目標(biāo)的測(cè)量值，利用得出的航向偏差計(jì)算得到無人機(jī)的真實(shí)航向，進(jìn)而根據(jù)真實(shí)航向和高度估計(jì)值實(shí)現(xiàn)對(duì)目標(biāo)的三維精確定位。具體地，假設(shè)選定的目標(biāo)與標(biāo)志物處于同一平面，即估計(jì)的相對(duì)高度可以認(rèn)為無人機(jī)與目標(biāo)的相對(duì)高度，[xtytzt]t表示目標(biāo)在慣性參考坐標(biāo)系{i}的坐標(biāo)，有令[xpypzp]t＝[xtytzt]t，將目標(biāo)的測(cè)量值以及無人機(jī)的真實(shí)航向代入公式(4)，計(jì)算得到目標(biāo)的坐標(biāo)，從而實(shí)現(xiàn)對(duì)目標(biāo)的三維定位。下面具體說明該迭代過程的有效性，以繞飛軌跡是圓周為例，半徑r＝73m，弧度rad＝1.5π，δψ＝0,1,...,59,60deg,由式(1)得到對(duì)應(yīng)的61組然后運(yùn)用數(shù)據(jù)擬合的方法，以為因變量，δψ為自變量，如圖6所示，得到的數(shù)學(xué)關(guān)系表達(dá)式：相同地，令由式(10)求解獲得36組δψ，然后運(yùn)用數(shù)據(jù)擬合的方法，以δψ為因變量，為自變量，如圖7所示，得到的數(shù)學(xué)關(guān)系表達(dá)式：設(shè)eδψ＝δψ-δψt，其中ht，δψt為相對(duì)高度和航向偏差的真實(shí)值，由式(9)得，由式(10)得，其中，k1，k2為相關(guān)參數(shù)。雙目視覺模型計(jì)算的相對(duì)高度代入線性回歸方程，可有效地計(jì)算航向偏差。然后，將航向偏差的估計(jì)值回代到雙目視覺模型，可精確地計(jì)算相對(duì)高度。一般地，ahrs系統(tǒng)的航向偏差不超過30deg，所以有|k2|＞k1＞0，而由于k2＜0，根據(jù)式(15)，(16)可知，經(jīng)過有限步迭代后，相對(duì)高度的估計(jì)值和航向偏差的估計(jì)值將收斂到真實(shí)值。在無人機(jī)搭載相機(jī)的條件下，進(jìn)行了飛行試驗(yàn)，無人機(jī)在繞飛軌跡下對(duì)標(biāo)志物進(jìn)行圖像測(cè)量，其中繞飛軌跡的半徑r＝73m，弧度rad＝1.5π，無人機(jī)相對(duì)于標(biāo)志物的真實(shí)高度ht＝45m，飛行速度v＝3.44m/s，fgps＝4hz，航向偏差的真實(shí)值δψt＝30deg，設(shè)e＝0.02deg，本發(fā)明所提供方法的效果如表1，表2，如圖8所示。其中表中所列誤差eh，eδψ，exy，ez都指的是均方根誤差。表1迭代過程表2目標(biāo)定位結(jié)果指標(biāo)本發(fā)明的三維定位相對(duì)高度估計(jì)誤差eh/m0.93航向估計(jì)誤差eδψ/deg1.89定位誤差exy/m10.89定位誤差ez/m0.43綜上所述，以上僅為本發(fā)明的較佳實(shí)施例而已，并非用于限定本發(fā)明的保護(hù)范圍。凡在本發(fā)明的精神和原則之內(nèi)，所作的任何修改、等同替換、改進(jìn)等，均應(yīng)包含在本發(fā)明的保護(hù)范圍之內(nèi)。當(dāng)前第1頁12