基于視覺導航的農機多智體相對導航算法研究

孟福軍，岳勝如，胡雪菲

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

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

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

1 坐標系定義及其坐標變換

1.1 坐標系定義

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

圖1 各坐標系及其相互關系

1.2 四元數對三維物體旋轉變換的描述

四元數在描述目標的姿態方面具有特有的優勢，它既能避免歐拉角所產生的奇異問題，又具有簡單、系統的運算法則［7-10］。

假設x為空間三維向量，經過旋轉矩陣R變換為x′，則有如下關系：

式中，旋轉矩陣R可由四元數Q的元素表示為：

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

2 基于視覺導航的農機多智體相對導航算法

2.1 確定農機多智體相對位姿關系

基于機器視覺的農機多智體相對位姿關系確定是確定主動農機智能體相機與被動農機智能體目標間的相對位姿，即根據目標特征點在目標參考坐標系的坐標與其相應的像平面坐標系求解相機的6個外方位元素。即：

將式（5）進一步轉化得：

式中，（xWi,yWi,zWi）為空間第i個點的坐標；（xi,yi,1）為第i個點的像平面坐標；mij為姿態矩陣M的第i行第j列元素，f為相機的焦距。

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

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

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

2.2 農機多智體機器視覺相對位姿確定模型

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

則式（8）可表示為：

將上式非線性函數按泰勒級數展開，取小值一次項得到線性化方程：

式中，Fx0、Fy0是將q0,q1,q2,q3,t1,t2,t3的初始值代入式（10）所得，ΔFx、ΔFy的表達式為：

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

為減少觀測點數量和解算參數，先通過采用基于四元數的最小二乘法求解農機多智體相對位置和姿態參數，接著利用求差法計算農機智能體和相機的相對位置，再根據四元數和農機智能體坐標系的轉換關系求得相對姿態，將得到的數據作為真實值，采用式（10）計算15個特征點位的像平面坐標，并給這些點位的像平面坐標加上白噪聲誤差作為模擬觀測值，最后利用式（11）和式（12）計算主動農機智能體和被動農機智能體的相對位置和姿態，可以進行農機多智體的相對位姿確定。

3 地面實物仿真與分析

3.1 數據處理結果

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

圖2 MV-5959云臺上的目標特征點

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

表1 y=0，x=0、4 000、8 000步目標坐標系中特征點坐標

2）基于四元數的視覺導航位姿確定模型計算目標和相機的相對位置和姿態關系。初始值位置參數為：平移矢量t=［0 0 7.269m］T，初始值位置參數為：q=［1 0 0 0］T，計算相對導航算法中誤差m0量級達到10-4，其結果如圖3、圖4所示。

圖3 農機多智體視覺導航算法各時刻中誤差

圖4 基于視覺導航的農機多智體導航算法計算的相對位姿成果

3.2 結果與分析

在仿真時間內，計算中誤差上下變化區間不大。隨著仿真時間的增加，相對姿態均呈下滑趨勢。同時，x、y軸相對坐標有一定的上下浮動，總體來看均呈下滑趨向。而z軸相對坐標逐漸升高。可以看出農機多智體視覺導航模型進行位置和姿態確定時，計算誤差在允許的范圍內，并且誤差在很小的范圍（10-4）內，說明該模型高效、準確。

4 小結

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