盛晨航，沈 躍，劉 慧，崔業(yè)民，龍友能

(1.江蘇大學(xué) 電氣信息工程學(xué)院，江蘇 鎮(zhèn)江 212013；2.南通市廣益機(jī)電有限責(zé)任公司，江蘇 南通 226000)

隨著農(nóng)業(yè)作業(yè)方式向著自動化、無人化方向發(fā)展，大量的自主導(dǎo)航輪式、履帶式農(nóng)業(yè)機(jī)器人被用于果園等復(fù)雜的作業(yè)環(huán)境中[1]，為了能在這種環(huán)境下執(zhí)行任務(wù)，實(shí)現(xiàn)機(jī)器人的運(yùn)動控制和自主導(dǎo)航，連續(xù)精確的定位顯得尤為重要[2].

目前國內(nèi)外關(guān)于自主導(dǎo)航機(jī)器人的定位主要采用慣性元件和激光雷達(dá)來提高航跡推算[3]的準(zhǔn)確性.激光雷達(dá)廣泛使用于自動駕駛噴霧機(jī)中，測距精度高，但是價格昂貴，難以推廣應(yīng)用[4].相反，通過視覺進(jìn)行定位具有很多優(yōu)點(diǎn)，是一種低成本高性能的導(dǎo)航方案[5].

近些年，視覺狀態(tài)估計技術(shù)發(fā)展迅速，科研人員提出了多種采用單目、雙目、RGB-D相機(jī)的算法.其中單目相機(jī)不能直接獲得深度信息，需要復(fù)雜的初始化過程[6]，雙目立體視覺根據(jù)圖像特征計算視差得到目標(biāo)的深度信息，需要高性能處理器，實(shí)時性較低[7]，而RGB-D相機(jī)可以直接獲取目標(biāo)的深度信息，極大地減少了計算量[8].早期的視覺狀態(tài)估計算法主要基于擴(kuò)展卡爾曼濾波降低估計誤差，但是基于濾波的算法不能消除過去時刻的累積誤差[9].

筆者針對果林環(huán)境GPS信號容易丟失造成農(nóng)機(jī)位姿估計誤差較大的問題，融合RGB-D相機(jī)、IMU(inertial measurement unit)和GPS(global positioning system)信息提出噴霧機(jī)多源融合定位因子圖優(yōu)化算法，構(gòu)建因子圖優(yōu)化模型，迭代優(yōu)化噴霧機(jī)定位數(shù)據(jù)序列.

1 系統(tǒng)組成

1.1 系統(tǒng)框架

RGB-D相機(jī)、IMU和GPS多源融合噴霧機(jī)定位算法框架如圖1所示.RGB-D相機(jī)型號為Real-Sense D435i，可以獲得同步的彩色圖和深度圖，同時捕捉其傳感范圍內(nèi)的彩色圖像和周圍環(huán)境結(jié)構(gòu)的深度數(shù)據(jù).相機(jī)內(nèi)置IMU單元，可檢測x,y,z三軸的旋轉(zhuǎn)和平移以及俯仰、橫搖等動作.該RGB-D相機(jī)基于主動紅外技術(shù)，適用于戶外環(huán)境使用.

首先通過GPS和IMU初始化相機(jī)當(dāng)前時刻的位姿狀態(tài)，然后采集彩色圖像和深度數(shù)據(jù)序列，利用改進(jìn)的自適應(yīng)閾值均分算法提取Harris角點(diǎn)，通過金字塔LK光流跟蹤匹配角點(diǎn)，估計噴霧機(jī)相對運(yùn)動狀態(tài)序列，再以奇異值分解方法計算兩幀間相機(jī)相對運(yùn)動量，得到局部較為精確的定位.

計算出的噴霧機(jī)相對運(yùn)動狀態(tài)序列最終通過因子圖優(yōu)化，最小化累積誤差，從而得到噴霧機(jī)更為精確的全局定位.

圖1 算法整體框架

1.2 系統(tǒng)建模

噴霧機(jī)坐標(biāo)系定義如圖2所示.相機(jī)到噴霧機(jī)的位姿變換矩陣TBC固定不變，通過初始化校準(zhǔn)獲得，噴霧機(jī)到地面坐標(biāo)系的位姿變換矩陣TWB表示噴霧機(jī)相對于初始位置的位姿變換.噴霧機(jī)的狀態(tài)由位姿變換矩陣T，速度向量v表示.

(1)

式中：A為正交旋轉(zhuǎn)矩陣；p為三維平移向量；O為三維零向量.

圖2 噴霧機(jī)坐標(biāo)系定義

為了采用因子圖優(yōu)化狀態(tài)變量，對旋轉(zhuǎn)量求導(dǎo)進(jìn)行優(yōu)化，采用特殊正交群SO(3)表示旋轉(zhuǎn)矩陣A，對應(yīng)的李代數(shù)φ是SO(3)群上的三維切向量[10].

φ=θa，

(2)

式中：a為旋轉(zhuǎn)坐標(biāo)軸；θ為繞坐標(biāo)軸旋轉(zhuǎn)的角度.

李代數(shù)到李群采用指數(shù)映射轉(zhuǎn)化，即

(3)

式中：E為3×3的單位矩陣；

2 融合定位

2.1 提取Harris角點(diǎn)

通過光流法跟蹤Harris角點(diǎn)估計噴霧機(jī)位姿變換.Harris角點(diǎn)假設(shè)相鄰幀圖像灰度不變，計算灰度圖像某一像素為中心的窗口中水平和豎直方向的梯度，并將2個方向的梯度值相乘生成二階矩M，通過判斷二階矩M的特征值的大小來選擇角點(diǎn).由于采用微分運(yùn)算，Harris角點(diǎn)對圖像旋轉(zhuǎn)、亮度和對比度的變化不敏感[11]，適合用于室外果林間圖像的特征角點(diǎn)提取，同時Harris角點(diǎn)檢測效率較高，能夠保證定位的實(shí)時性.Harris角點(diǎn)原理如圖3所示.

圖3 Harris角點(diǎn)原理圖

對于圖像I(x,y)，通過自相關(guān)函數(shù)給出點(diǎn)(x,y)處平移(Δx,Δy)灰度差異：

(4)

式中：W(x,y)是以點(diǎn)(x,y)為中心的滑動窗口；ω(u,v)為高斯加權(quán)函數(shù).

根據(jù)泰勒展開，對圖像I(x,y)在平移(Δx,Δy)后進(jìn)行一階近似：

(5)

其中二階矩M表示為

(6)

當(dāng)檢測到角點(diǎn)時，二階矩M具有2個較大且接近相等的特征值.通過式(7)計算像素點(diǎn)的響應(yīng)值τ，調(diào)節(jié)角點(diǎn)響應(yīng)閾值來改變角點(diǎn)檢測的靈敏度，從而控制檢測角點(diǎn)的數(shù)量.

τ=detM-α(trM)2

.

(7)

但是提取更多的角點(diǎn)并不能提高相機(jī)狀態(tài)估計的準(zhǔn)確度，因此需要預(yù)先設(shè)定合適的閾值，保留適量的角點(diǎn)，但是在室外環(huán)境中，固定的閾值不能適應(yīng)環(huán)境光線的變化，而且Harris算法檢測到的角點(diǎn)非常容易聚集在圖像紋理豐富的區(qū)域.

因此，采用了自適應(yīng)閾值的均勻Harris角點(diǎn)檢測策略.具體步驟如下：① 對灰度圖提取Harris角點(diǎn)，找出角點(diǎn)響應(yīng)函數(shù)的最大值τmax；② 定義閾值η為角點(diǎn)最大響應(yīng)值τmax的λ倍；③ 設(shè)定2個角點(diǎn)的最小間距，只保留局部區(qū)域內(nèi)角點(diǎn)響應(yīng)值最大的角點(diǎn)，使得角點(diǎn)均勻分布，提高光流跟蹤的準(zhǔn)確度.

2.2 金字塔LK光流跟蹤

傳統(tǒng)的特征點(diǎn)匹配方法需要計算描述子，耗費(fèi)了大量時間，而用LK光流法跟蹤特征點(diǎn)進(jìn)行匹配省去了計算描述子的過程，極大地減少了特征點(diǎn)匹配的時間.傳統(tǒng)的LK光流跟蹤算法只能在兩幀圖像間發(fā)生微小運(yùn)動時有效，當(dāng)兩幀圖像間噴霧機(jī)產(chǎn)生較大的運(yùn)動時，傳統(tǒng)的LK光流跟蹤準(zhǔn)確度會降低，為此，采用改進(jìn)后的金字塔LK光流跟蹤算法.

金字塔LK光流原理如圖4所示，Lm中L表示圖像金字塔層，下標(biāo)m表示第m層.構(gòu)造圖像金字塔，將原圖像逐層縮小，金字塔的頂層是最低分辨率圖像，底層是原圖，定義光流值為dL=(dx,dy)T，遞歸估計每一層光流[12].

圖4 金字塔LK光流原理圖

頂層光流估計初始值設(shè)為gLm=(0 0)T，頂層gLm光流計算出的運(yùn)動量反饋給下一層gLm-1，通過最小化每個點(diǎn)鄰域內(nèi)匹配誤差和，得到頂層圖像中每個點(diǎn)的光流作為該層初始值時的光流估計值.

gLm-1=2(gLm+dLm)=2(0+dLm)=2dLm.

(8)

沿著圖像金字塔向下反饋，重復(fù)利用LK光流跟蹤，直到到達(dá)金字塔底層的原圖像，最終圖像的光流值d就是所有層分段光流的疊加：

(9)

由于構(gòu)造圖像金字塔對原圖進(jìn)行縮放，減小了物體在圖像上的位移，使光流估計能夠處理大幅度運(yùn)動，同時保持局部亞像素精度.

2.3 相機(jī)運(yùn)動估計

因?yàn)槭褂肦GB-D相機(jī)可以直接獲得深度信息，所以角點(diǎn)的三維坐標(biāo)可以通過一一對應(yīng)的灰度圖像和深度圖像獲得.對光流跟蹤后的n對匹配好的三維空間點(diǎn)坐標(biāo)向量P(x，y，z)和P′(x′，y′，z′)，采用迭代最近點(diǎn)算法求解相機(jī)運(yùn)動狀態(tài)，通過奇異值分解方法求解相機(jī)旋轉(zhuǎn)矩陣和平移向量，如圖5所示.

圖5 3D迭代最近鄰點(diǎn)原理圖

由于相機(jī)位姿未知，且觀測到的三維空間點(diǎn)存在噪聲，所以定義匹配點(diǎn)的坐標(biāo)誤差為

ei=Pi-(APi′+p)，i=1,2,…,n.

(10)

定義前后幀匹配角點(diǎn)的質(zhì)心分別為

(11)

計算每個匹配點(diǎn)與質(zhì)心的相對坐標(biāo)，

(12)

構(gòu)建最小二乘問題，求使得誤差平方和最小的位姿A，定義目標(biāo)函數(shù)為

(13)

根據(jù)計算出來的A,可以求出p,即

p=P-AP′.

(14)

2.4 因子圖優(yōu)化

因子圖[13]是由與狀態(tài)變量相關(guān)的因子組成的無向圖模型，非常適合于建模復(fù)雜的估計問題.狀態(tài)變量表示估計問題中的未知隨機(jī)變量，而因子表示這些變量的測量值的概率信息.

傳統(tǒng)的圖優(yōu)化算法是對整個圖進(jìn)行優(yōu)化，隨著特征點(diǎn)的增加，圖規(guī)模逐漸變大，優(yōu)化會耗費(fèi)大量計算時間，而因子圖保留了優(yōu)化的中間結(jié)果，增量處理優(yōu)化問題，不需要重新計算之前的中間結(jié)果[14]，避免冗余計算，提高了計算速度，減少計算資源開銷.融合定位的因子圖模型如圖6所示.

圖6 多源融合因子圖優(yōu)化模型

系統(tǒng)的優(yōu)化變量為

X=(x0,x1,…，xn,P0,P1,…,Pn)，

(15)

式中：x0，x1，…，xn為系統(tǒng)的狀態(tài)向量；P0，P1，…，Pn為角點(diǎn)的觀測因子向量.

IMU先驗(yàn)因子提供初始姿態(tài)三維向量，GPS先驗(yàn)因子提供初始位置三維向量.關(guān)鍵幀bk時刻系統(tǒng)的狀態(tài)表示為

xk=(Abk,Pbk).

(16)

通常，因子只與少數(shù)幾個變量相關(guān)，所以因子圖是稀疏連接的，利用GTSAM構(gòu)建因子圖，在GTSAM中實(shí)現(xiàn)的算法利用稀疏性來降低計算復(fù)雜度，提高計算效率.對于大規(guī)模的因子圖，GTSAM提供了迭代優(yōu)化方法，可以方便地優(yōu)化圖網(wǎng)絡(luò).針對融合定位因子圖，定義最小化目標(biāo)函數(shù)為

(17)

對誤差項(xiàng)線性化可得

e(x+Δx)≈e(x)+JΔx,

(18)

式中：J為雅克比矩陣.

前三維為旋轉(zhuǎn)，后三維為平移，描述了重投影誤差關(guān)于相機(jī)位姿李代數(shù)的一階導(dǎo)數(shù)關(guān)系，然后使用列文伯格-馬爾夸特方法進(jìn)行迭代優(yōu)化求解位姿.

3 試驗(yàn)與分析

3.1 試驗(yàn)平臺

試驗(yàn)平臺為履帶式自主導(dǎo)航果園噴霧機(jī)，如圖7所示，RTK-GPS為噴霧機(jī)提供精確的初始坐標(biāo)，噴霧機(jī)頂端距離地面145 cm，安裝了RealSense D435i傳感器，相機(jī)的水平視角87°，垂直視角58°，最遠(yuǎn)深度感知距離為10 m，圖像分辨率為640×480像素.采用Intel微型電腦進(jìn)行數(shù)據(jù)采集和處理，微型電腦的處理器型號為i5-5250U.試驗(yàn)數(shù)據(jù)在室外光照充足的高大果林間采集.

圖7 自主導(dǎo)航果園噴霧機(jī)

3.2 改進(jìn)的Harris角點(diǎn)

將RealSense采集到的彩色圖像二值化后，采用自適應(yīng)閾值算法提取合適數(shù)量的角點(diǎn)，試驗(yàn)中λ取經(jīng)驗(yàn)值0.01，然后根據(jù)最小角點(diǎn)間距進(jìn)行過濾使得提取的角點(diǎn)均勻分布.如圖8a所示，紅色標(biāo)注的Harris角點(diǎn)聚集在圖像紋理豐富區(qū)域，兩幀圖像間該區(qū)域運(yùn)動的距離過小，不利于采用光流法跟蹤.如圖8b所示，通過設(shè)定最小間距重新提取Harris角點(diǎn)，試驗(yàn)中，設(shè)定角點(diǎn)最小間距為20像素，只保留局部區(qū)域角點(diǎn)響應(yīng)值最大的角點(diǎn)，使得角點(diǎn)分布更加均勻，有利于提高后續(xù)光流跟蹤的準(zhǔn)確度.

圖8 Harris角點(diǎn)提取效果圖

3.3 改進(jìn)的金字塔光流

不同層數(shù)金字塔光流跟蹤效果對比如圖9所示，紅色圓點(diǎn)標(biāo)記出檢測到的Harris角點(diǎn)，綠色線段標(biāo)記出光流法跟蹤匹配角點(diǎn)的運(yùn)動距離.圖9a中，傳統(tǒng)的LK光流跟蹤在較快的運(yùn)動中會跟蹤錯誤，試驗(yàn)中對比了多層金字塔LK光流跟蹤的準(zhǔn)確度，并與傳統(tǒng)的描述子特征點(diǎn)匹配算法進(jìn)行對比.

傳統(tǒng)描述子匹配算法和改進(jìn)光流匹配算法對比如表1所示，改進(jìn)后的金字塔光流跟蹤匹配算法計算效率比傳統(tǒng)的描述子匹配方法更高，同時，通過試驗(yàn)，發(fā)現(xiàn)單純增加圖像金字塔的層數(shù)并不能獲得更好的匹配效果，如圖9b所示，經(jīng)過多次試驗(yàn)后，選取了跟蹤準(zhǔn)確率較高的四層光流金字塔進(jìn)行Harris角點(diǎn)的匹配.

圖9 不同層數(shù)金字塔光流跟蹤效果對比

表1 傳統(tǒng)描述子匹配算法和改進(jìn)光流匹配算法對比

3.4 相機(jī)運(yùn)動狀態(tài)求解

相機(jī)在運(yùn)動過程中采集到的連續(xù)兩幀彩色圖像與深度圖像如圖10所示.通過改進(jìn)后的金字塔LK光流匹配好Harris角點(diǎn)后，結(jié)合相機(jī)獲得的深度圖像，計算出匹配點(diǎn)在相機(jī)坐標(biāo)系下的三維坐標(biāo)信息.

圖10 圖像信息采集效果圖

然后通過迭代最近點(diǎn)算法，利用奇異值SVD分解求解，相機(jī)定位結(jié)果最終收斂，求解相機(jī)位姿變換矩陣耗時共1.39 ms，計算耗時較短.求解的相機(jī)旋轉(zhuǎn)矩陣A和平移向量p分別為

p=[0.084 442 0.007 554 0.080 206]T.

可計算出相機(jī)在x，y，z這3個方向的旋轉(zhuǎn)角度分別為0.330°，1.057°，0.176°，平移量分別為0.084，0.008，0.080 m.

3.5 因子圖優(yōu)化位姿

噴霧機(jī)定位試驗(yàn)包括彎道與直線路段，如圖11所示，噴霧機(jī)運(yùn)動過程中通過視覺實(shí)時估計位姿，圖中，x軸為噴霧機(jī)滾轉(zhuǎn)角旋轉(zhuǎn)軸，y軸為噴霧機(jī)偏航角旋轉(zhuǎn)軸，z軸為噴霧機(jī)俯仰角旋轉(zhuǎn)軸.經(jīng)試驗(yàn)測得改進(jìn)的融合定位算法位姿更新頻率約為50 Hz，滿足了噴霧機(jī)自主導(dǎo)航過程中的運(yùn)動控制的姿態(tài)估計的實(shí)時性要求.

圖11 位姿實(shí)時更新效果圖

噴霧機(jī)在高大果林間以約0.5 m·s-1的速度行駛，定位效果如圖12所示，藍(lán)色點(diǎn)線為噴霧機(jī)在GPS信號丟失后的位置狀態(tài)更新軌跡，改進(jìn)的視覺定位算法持續(xù)地更新噴霧機(jī)的位姿狀態(tài)，從而自主導(dǎo)航噴霧機(jī)得以沿著規(guī)劃路徑繼續(xù)平穩(wěn)行駛.

圖12 定位效果圖

試驗(yàn)采集了視覺與慣導(dǎo)的噴霧機(jī)定位數(shù)據(jù)，并與噴霧機(jī)真實(shí)的運(yùn)動軌跡數(shù)據(jù)進(jìn)行對比，本次測試距離為112.39 m，改進(jìn)后的定位算法有效減少了位置估計誤差.

為了驗(yàn)證算法的魯棒性，經(jīng)過多次試驗(yàn)，并與單獨(dú)慣性導(dǎo)航定位結(jié)果進(jìn)行比較，結(jié)果如表2，3所示，改進(jìn)的因子圖融合定位算法有效提高了定位的準(zhǔn)確度，使得定位的均方根誤差減少了0.816 m，最大誤差減少了4.613 m，姿態(tài)角度估計最大誤差減少了2.713°.

表2 定位試驗(yàn)位置誤差分析 m

表3 定位試驗(yàn)姿態(tài)誤差分析 (°)

4 結(jié) 論

1) 改進(jìn)了自適應(yīng)閾值算法提取Harris角點(diǎn)，適應(yīng)果林環(huán)境，使角點(diǎn)均勻分布，提高光流跟蹤的準(zhǔn)確度.針對試驗(yàn)中相機(jī)采集的圖像，設(shè)定角點(diǎn)最小間距為20像素時，提取角點(diǎn)效果最好.

2) 通過多層圖像金字塔LK光流跟蹤算法實(shí)時跟蹤匹配運(yùn)動角點(diǎn)，保證噴霧機(jī)在較快運(yùn)動速度下角點(diǎn)跟蹤匹配的準(zhǔn)確度.多次對比試驗(yàn)結(jié)果表明，4層金字塔光流匹配的準(zhǔn)確度最高.

3) 最終構(gòu)建多源融合因子圖模型，增量優(yōu)化相機(jī)運(yùn)動估計序列，減少了圖優(yōu)化過程中的重復(fù)計算.試驗(yàn)結(jié)果表明：位姿更新頻率為50 Hz，減少了噴霧機(jī)定位的累積誤差，改進(jìn)的多源融合定位算法精度更高.