基于視覺導(dǎo)航的農(nóng)機(jī)多智體相對導(dǎo)航算法研究

孟福軍，岳勝如，胡雪菲

（塔里木大學(xué)水利與建筑工程學(xué)院，新疆 阿拉爾 843300）

在精準(zhǔn)農(nóng)業(yè)技術(shù)驅(qū)動(dòng)下，農(nóng)業(yè)機(jī)械自主導(dǎo)航的現(xiàn)代化有了十足的發(fā)展［1，2］。作為精準(zhǔn)農(nóng)業(yè)的一項(xiàng)核心關(guān)鍵技術(shù)，利用農(nóng)機(jī)多智體（即多個(gè)農(nóng)業(yè)機(jī)械智能機(jī)器人）的協(xié)同作業(yè)去完成田間的智能播種、噴藥、施肥、收割等或在收割機(jī)提出卸糧請求時(shí)，運(yùn)糧農(nóng)機(jī)跟隨收割機(jī)智能行進(jìn)并配合收割機(jī)完成卸糧作業(yè)，既能降低能源消耗，又能提高田間作業(yè)效率。在田間作業(yè)中，還可減少人員的直接參與。而同類型農(nóng)機(jī)多智體之間、各農(nóng)機(jī)智能體與被操作目標(biāo)（如運(yùn)糧農(nóng)機(jī)、農(nóng)作物等）之間的相對導(dǎo)航問題必須首先解決。

目前，無線電導(dǎo)航、慣性導(dǎo)航（INS）、視覺導(dǎo)航、衛(wèi)星導(dǎo)航等技術(shù)是農(nóng)業(yè)機(jī)械導(dǎo)航定位的主要技術(shù)［3-6］。視覺自主相對導(dǎo)航具有低成本、探測信息范圍寬、完整的目標(biāo)信息、高精度、強(qiáng)自主性、易于實(shí)現(xiàn)等優(yōu)勢，非常適用于農(nóng)機(jī)多智體相對導(dǎo)航需求。在目標(biāo)農(nóng)機(jī)智能體上安裝形狀和大小已知的光學(xué)特征裝置，在主動(dòng)農(nóng)機(jī)智能體上安裝CCD相機(jī)，要進(jìn)行主動(dòng)農(nóng)機(jī)智能體和目標(biāo)農(nóng)機(jī)智能體之間的相對導(dǎo)航，就需通過對特征裝置在CCD相機(jī)上成像的分析和計(jì)算。因此，本研究提出基于視覺導(dǎo)航的農(nóng)機(jī)多智體相對導(dǎo)航算法，以機(jī)器視覺中的四元數(shù)模型作為農(nóng)機(jī)多智體相對導(dǎo)航算法，四元數(shù)位姿確定參數(shù)只有7個(gè)，相比較Hall算法12個(gè)參數(shù)［6］降低了相關(guān)矩陣的階數(shù)，減輕了計(jì)算量。

1 坐標(biāo)系定義及其坐標(biāo)變換

1.1 坐標(biāo)系定義

如圖1所表示，本研究假設(shè)相機(jī)坐標(biāo)系的三軸分別與主動(dòng)農(nóng)機(jī)智能體的三軸方向一致，其中，OB-xByBzB為主動(dòng)農(nóng)機(jī)智能體本體坐標(biāo)系、OW-xWyWzW為被動(dòng)農(nóng)機(jī)智能體本體坐標(biāo)系、OC-xCyCzC為相機(jī)坐標(biāo)系，在基于機(jī)器視覺的相對導(dǎo)航中，目標(biāo)與相機(jī)的相對位姿關(guān)系一般通過求解所選的目標(biāo)參考坐標(biāo)系OW-xWyWzW與相機(jī)坐標(biāo)系的相對關(guān)系得到。根據(jù)實(shí)際要將目標(biāo)農(nóng)機(jī)智能體的坐標(biāo)系定義為目標(biāo)參考坐標(biāo)系。像素坐標(biāo)系為o-uv，u為列數(shù)，v為行數(shù)。OI-xIyI為圖像物理坐標(biāo)系。

圖1 各坐標(biāo)系及其相互關(guān)系

1.2 四元數(shù)對三維物體旋轉(zhuǎn)變換的描述

四元數(shù)在描述目標(biāo)的姿態(tài)方面具有特有的優(yōu)勢，它既能避免歐拉角所產(chǎn)生的奇異問題，又具有簡單、系統(tǒng)的運(yùn)算法則［7-10］。

假設(shè)x為空間三維向量，經(jīng)過旋轉(zhuǎn)矩陣R變換為x′，則有如下關(guān)系：

式中，旋轉(zhuǎn)矩陣R可由四元數(shù)Q的元素表示為：

式中，mij(i=1,2,3;j=1,2,3)為：

2 基于視覺導(dǎo)航的農(nóng)機(jī)多智體相對導(dǎo)航算法

2.1 確定農(nóng)機(jī)多智體相對位姿關(guān)系

基于機(jī)器視覺的農(nóng)機(jī)多智體相對位姿關(guān)系確定是確定主動(dòng)農(nóng)機(jī)智能體相機(jī)與被動(dòng)農(nóng)機(jī)智能體目標(biāo)間的相對位姿，即根據(jù)目標(biāo)特征點(diǎn)在目標(biāo)參考坐標(biāo)系的坐標(biāo)與其相應(yīng)的像平面坐標(biāo)系求解相機(jī)的6個(gè)外方位元素。即：

將式（5）進(jìn)一步轉(zhuǎn)化得：

式中，（xWi,yWi,zWi）為空間第i個(gè)點(diǎn)的坐標(biāo)；（xi,yi,1）為第i個(gè)點(diǎn)的像平面坐標(biāo)；mij為姿態(tài)矩陣M的第i行第j列元素，f為相機(jī)的焦距。

用xˉ表示(xi,yi)T，矢量xˉ′表示齊次坐標(biāo)(xi,yi,1)T，矢 量Xˉ表 示(xWi,yWi,zWi)T，矢 量Xˉ′表 示 齊 次 坐 標(biāo)(xWi,yWi,zWi,1)T，式（6）可以表示為：

將式（7）兩邊同除以zCi就可得到共線方程：

基于機(jī)器視覺農(nóng)機(jī)多智體位姿確定的基本方程，mij(i=1,2,3;j=1,2,3)表示相對姿態(tài)，ti(i=1,2,3)表示相對位置。

2.2 農(nóng)機(jī)多智體機(jī)器視覺相對位姿確定模型

式（8）中，獨(dú)立的參數(shù)有6個(gè)：3個(gè)旋轉(zhuǎn)和3個(gè)平移參數(shù)。傳統(tǒng)的Hall算法將mij(i=1,2,3;j=1,2,3)和ti(i=1,2,3)全部作為參數(shù)，這樣就有12個(gè)參數(shù)，只有當(dāng)觀測點(diǎn)數(shù)n≥6時(shí)，方程才有解。引入四元數(shù)算法，利用式（4）將mij(i=1,2,3;j=1,2,3)換為四元數(shù)的元素，則由Hall算法的12個(gè)參數(shù)減少為7個(gè)參數(shù)。令：

則式（8）可表示為：

將上式非線性函數(shù)按泰勒級數(shù)展開，取小值一次項(xiàng)得到線性化方程：

式中，F(xiàn)x0、Fy0是將q0,q1,q2,q3,t1,t2,t3的初始值代入式（10）所得，ΔFx、ΔFy的表達(dá)式為：

式中，Δq0,Δq1,Δq2,Δq3,Δt1,Δt2,Δt3為q0,q1,q2,q3,t1,t2,t3初始值的相應(yīng)改正值。式（11）、式（12）解算必須要進(jìn)行反復(fù)迭代趨近，直至改正數(shù)ΔFx、ΔFy小于某一允許值為止。獲得的參數(shù)減少，但當(dāng)觀測點(diǎn)的個(gè)數(shù)n＞4時(shí)，應(yīng)按最小二乘原理解算參數(shù)，參數(shù)的最小二乘解為：

為減少觀測點(diǎn)數(shù)量和解算參數(shù)，先通過采用基于四元數(shù)的最小二乘法求解農(nóng)機(jī)多智體相對位置和姿態(tài)參數(shù)，接著利用求差法計(jì)算農(nóng)機(jī)智能體和相機(jī)的相對位置，再根據(jù)四元數(shù)和農(nóng)機(jī)智能體坐標(biāo)系的轉(zhuǎn)換關(guān)系求得相對姿態(tài)，將得到的數(shù)據(jù)作為真實(shí)值，采用式（10）計(jì)算15個(gè)特征點(diǎn)位的像平面坐標(biāo)，并給這些點(diǎn)位的像平面坐標(biāo)加上白噪聲誤差作為模擬觀測值，最后利用式（11）和式（12）計(jì)算主動(dòng)農(nóng)機(jī)智能體和被動(dòng)農(nóng)機(jī)智能體的相對位置和姿態(tài)，可以進(jìn)行農(nóng)機(jī)多智體的相對位姿確定。

3 地面實(shí)物仿真與分析

3.1 數(shù)據(jù)處理結(jié)果

將目標(biāo)架設(shè)在機(jī)器視覺云臺MV-5959上，通過計(jì)算機(jī)串口實(shí)現(xiàn)對目標(biāo)的運(yùn)動(dòng)控制，目標(biāo)由圖2所示，設(shè)置了15個(gè)特征點(diǎn)位、1個(gè)相機(jī)三腳架，計(jì)算機(jī)由云臺控制計(jì)算機(jī)和一臺專門的數(shù)據(jù)處理計(jì)算機(jī)（數(shù)據(jù)的離線處理，其參數(shù)為Intel Celeron Dual-Core T3000 1.8 G，2.0 GB內(nèi)存）。MV-5959云臺上的目標(biāo)設(shè)置特征線，并給隨動(dòng)云臺跟蹤系統(tǒng)MV-5959設(shè)置一定的運(yùn)動(dòng)規(guī)律，利用云臺系統(tǒng)自帶的控制軟件控制云臺水平勻速轉(zhuǎn)動(dòng)8 000步。同時(shí)，在預(yù)先定位好的三腳架上架設(shè)專業(yè)單目視CCD相機(jī)（MVVEM500SM最高分辨率為640×480；焦距為27 mm），并對云臺目標(biāo)進(jìn)行跟蹤拍照，每1秒記錄1次數(shù)據(jù)，產(chǎn)生一系列CCD觀測影像。最后，對采集的數(shù)據(jù)進(jìn)行離線處理，進(jìn)行農(nóng)機(jī)主動(dòng)智能體相機(jī)與被動(dòng)智能體目標(biāo)之間的相對位置和姿態(tài)的確定。

圖2 MV-5959云臺上的目標(biāo)特征點(diǎn)

1）目標(biāo)坐標(biāo)系的建立及目標(biāo)特征點(diǎn)的精確量測。以MV-5959底座中心為坐標(biāo)原點(diǎn)OW，xW軸平行于目標(biāo)特征點(diǎn)11和12的連線，zW軸平行于特征點(diǎn)1方向垂直向上，yW垂直于xWOWzW平面，并與xW、zW軸組成左手坐標(biāo)系。經(jīng)測量得到在云臺旋轉(zhuǎn)0、4 000、8 000步時(shí)，特征標(biāo)記十字中心在目標(biāo)坐標(biāo)系中的坐標(biāo)見表1。

表1 y=0，x=0、4 000、8 000步目標(biāo)坐標(biāo)系中特征點(diǎn)坐標(biāo)

2）基于四元數(shù)的視覺導(dǎo)航位姿確定模型計(jì)算目標(biāo)和相機(jī)的相對位置和姿態(tài)關(guān)系。初始值位置參數(shù)為：平移矢量t=［0 0 7.269m］T，初始值位置參數(shù)為：q=［1 0 0 0］T，計(jì)算相對導(dǎo)航算法中誤差m0量級達(dá)到10-4，其結(jié)果如圖3、圖4所示。

圖3 農(nóng)機(jī)多智體視覺導(dǎo)航算法各時(shí)刻中誤差

圖4 基于視覺導(dǎo)航的農(nóng)機(jī)多智體導(dǎo)航算法計(jì)算的相對位姿成果

3.2 結(jié)果與分析

在仿真時(shí)間內(nèi)，計(jì)算中誤差上下變化區(qū)間不大。隨著仿真時(shí)間的增加，相對姿態(tài)均呈下滑趨勢。同時(shí)，x、y軸相對坐標(biāo)有一定的上下浮動(dòng)，總體來看均呈下滑趨向。而z軸相對坐標(biāo)逐漸升高?？梢钥闯鲛r(nóng)機(jī)多智體視覺導(dǎo)航模型進(jìn)行位置和姿態(tài)確定時(shí)，計(jì)算誤差在允許的范圍內(nèi)，并且誤差在很小的范圍（10-4）內(nèi)，說明該模型高效、準(zhǔn)確。

4 小結(jié)

通過采用基于四元數(shù)的視覺導(dǎo)航位姿確定模型，進(jìn)行農(nóng)機(jī)多智能體相對位姿估計(jì)，計(jì)算量小，并且減少了迭代次數(shù)，提高了相對位置姿態(tài)的計(jì)算效率和計(jì)算精度，能夠滿足農(nóng)機(jī)自主相對導(dǎo)航精準(zhǔn)性和實(shí)時(shí)性的要求。該算法模型為中國農(nóng)田作業(yè)的農(nóng)機(jī)自動(dòng)化耕種、收割、施肥、灌溉等工作的精準(zhǔn)化理論研究提供一定的參考價(jià)值。