基于因子圖的無人噴霧機多源融合定位

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

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

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

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

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

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

1 系統(tǒng)組成

1.1 系統(tǒng)框架

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

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

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

圖1 算法整體框架

1.2 系統(tǒng)建模

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

(1)

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

圖2 噴霧機坐標系定義

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

φ=θa，

(2)

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

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

(3)

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

2 融合定位

2.1 提取Harris角點

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

圖3 Harris角點原理圖

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

(4)

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

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

(5)

其中二階矩M表示為

(6)

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

τ=detM-α(trM)2

.

(7)

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

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

2.2 金字塔LK光流跟蹤

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

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

圖4 金字塔LK光流原理圖

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

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

(8)

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

(9)

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

2.3 相機運動估計

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

圖5 3D迭代最近鄰點原理圖

由于相機位姿未知，且觀測到的三維空間點存在噪聲，所以定義匹配點的坐標誤差為

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

(10)

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

(11)

計算每個匹配點與質(zhì)心的相對坐標，

(12)

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

(13)

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

p=P-AP′.

(14)

2.4 因子圖優(yōu)化

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

傳統(tǒng)的圖優(yōu)化算法是對整個圖進行優(yōu)化，隨著特征點的增加，圖規(guī)模逐漸變大，優(yōu)化會耗費大量計算時間，而因子圖保留了優(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為角點的觀測因子向量.

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

xk=(Abk,Pbk).

(16)

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

(17)

對誤差項線性化可得

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

(18)

式中：J為雅克比矩陣.

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

3 試驗與分析

3.1 試驗平臺

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

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

3.2 改進的Harris角點

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

圖8 Harris角點提取效果圖

3.3 改進的金字塔光流

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

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

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

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

3.4 相機運動狀態(tài)求解

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

圖10 圖像信息采集效果圖

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

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

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

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

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

圖11 位姿實時更新效果圖

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

圖12 定位效果圖

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

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

表2 定位試驗位置誤差分析 m

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

4 結(jié) 論

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

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

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