基于Harris和卡爾曼濾波的農業機器人田間穩像算法

王 杰 經俊森,2 陳正偉 徐照勝 王少平

(1.中國科學院合肥物質科學研究院， 合肥 230031； 2.安徽大學物質科學與信息技術研究院， 合肥 230601)

0 引言

隨著智慧農業[1-2]發展，農業機器人輔助或者替代人工參與農業作業逐漸成為研究熱點，其中計算機視覺是重要的機器人環境感知研究內容，如植物工廠、溫室大棚以及丘陵果園中的植株病蟲害[3-4]巡檢機器人[5-6]、高附加值果蔬精準施藥機器人[7-8]以及各類采摘作業機器人[9-10]等。通常，研究人員需要機器人行走運動同時進行高速圖像采集處理工作。例如，在農業巡檢機器人病蟲害巡檢方面，機器人通過前端圖像采集，利用專家數據庫或深度學習等方法實現特定病蟲害自動識別，實時向后端傳輸采集圖像，交付人工遠程復檢或完成機器無法自動識別的內容，提升巡檢效率，并將圖像信息作為后期植保、施肥等管護作業基礎數據支持[11]；在農業機器人自主行走方面，針對無衛星或者衛星信號嚴重遮擋情況下的機器人定位導航問題，基于視覺或視覺與激光雷達融合技術的定位導航方法是區別于UWB、藍牙等技術的重要研究內容[12]。由于田間路面起伏，易造成機器人視覺圖像抖動，且機器人運動速度越高，抖動越嚴重，不利于類似前述圖像采集數據進一步應用，譬如通過實時畫面對作物表型進行人工精準識別，以及基于視覺或視覺與激光雷達融合的田間自主行走導航控制。因此，針對農業機器人運動圖像采集工作，研究實時穩像方法具有重要意義。

電子穩像技術作為圖像處理的一個重要發展方向，在軍事、航空等領域研究較多。相比云臺等專業穩像設備，電子穩像不需額外硬件，具有一定成本優勢，且可以針對圖像感興趣區域進行局部穩像，盡可能提高穩像實時性，減小甚至消除抖動，獲得平穩圖像序列，因此，農業領域相關研究也在逐漸增多。電子穩像方法[13-15]中最關鍵技術是運動估計。根據運動估計方法差異，分為塊匹配[16]、位平面匹配法[17]、灰度投影法[18]、特征匹配法[19]和光流法[20]。光流法具有跟蹤精度高和應用范圍廣等優勢，是電子穩像研究熱點。由于傳統光流法是計算整幅圖像光流矢量，存在計算量大和實時性差缺陷，所以本文對每幀圖像進行角點檢測，僅計算角點處光流，達到降低計算量和提高整體算法實時性目的。因此，當使用光流法作為運動估計方法時，角點檢測必不可少。研究表明，利用SIFT[21]和SURF[22]提取特征點進行穩像的速度相較于使用基于Harris角點檢測的穩像算法慢很多。而傳統Harris角點檢測算法[23]存在角點提取分布不均勻，角點數目少問題，尤其在農業領域，由于環境復雜，該問題更為突出。針對以上問題，本文通過改進Harris角點檢測算法和卡爾曼濾波算法，設計一種適用于農業機器人顛簸田間環境的實時電子穩像方法。

1 研究方法

1.1 傳統Harris角點檢測原理

HARRIS等[24]首次提出Harris角點檢測方法，其主要思路是利用圖像自相關性，并結合微分運算來提取圖像特征點，具有很好魯棒性。

Harris角點檢測核心是描述像素鄰域梯度分布的二階矩陣M，其具體形式為

(1)

式中Ix——圖像在x方向上梯度

Iy——圖像在y方向上梯度

w(x,y)——以點(x,y)為中心的窗口函數，通常取高斯函數

二階矩陣M存在2個特征值λ1和λ2，特征值反映了像素點的位置關系和突出程度。通常利用兩個特征值構建角點響應函數re

re=detM-ktr(M)2

(2)

式中 detM——矩陣M的行列式

tr(M)——矩陣M的跡

k——系數，取0.04～0.06

角點響應函數re值越大的點，表明該點突出程度越高，代表性越強。如果re大于設定閾值T，則該像素點為角點，反之不為角點。

1.2 改進的Harris角點檢測算法

傳統Harris角點檢測算法基于閾值T進行檢測，如果整幅圖像采用相同閾值T，當圖像灰度差異過大時，將導致特征點分布不均勻，易造成特征點聚簇且影響算法實時性。因此，本文設計了一種自適應閾值選取方法：將圖像分割為無重合子區域，通過計算每塊區域的灰度均方差進行自適應閾值構建。

當某塊圖像區域灰度值具有差異明顯，反映該區域存在大量的特征角點，反之特征角點較少。圖像灰度差異通常由圖像灰度均方差計算獲得，灰度均方差較大區域，應設置較大閾值，灰度均方差較小區域，應設置較小閾值。通過不同區域設置不同閾值，保持每塊區域的特征角點分布更加均勻。

具體的步驟如下：

(1)將每幀圖像進行等比例分割，分割成3×3同等尺寸子區域。

(2)計算每個子區域平均灰度，計算式為

(3)

式中m、n——子區域的尺寸參數

I(i,j)——圖像在點(i,j)的灰度

(3)計算每塊子區域灰度均方差P，計算式為

(4)

(4)計算子區域中最大灰度均方差Pmax。

(5)根據各子區域灰度均方差與Pmax的比例設定自適應閾值T，計算式為

(5)

式中T0——初始設置最大的閾值參數

該閾值設置方法能夠實現圖像灰度差異較大時，提取到的特征角點分布均勻。

為了克服局部角點過于聚簇的問題，需要對提取后的角點增加距離約束。具體角點選取步驟：先通過選取每塊子區域的角點響應函數re值大于該區域自適應閾值T的像素點作為特征角點，并避免選取的點距離過近，設定距離約束條件d，圖像尺寸與d相關，本文選取5個像素；然后對特征角點依次遍歷，若在特征角點鄰域內存在其他特征角點，比較兩個點之間的距離，若距離小于約束條件d，再判斷兩點測量噪聲協方差矩陣R值，保留R值更大的點作為角點。

針對田間路況，采用基于圖像金字塔的L-K光流法[25]對提取的角點進行跟蹤，再利用RANSAC[26]算法去除Harris角點檢測中存在的誤匹配點，最終得到水平偏移量dx、垂直偏移量dy和旋轉量dθ。

1.3 自適應卡爾曼濾波算法

前述運動估計參數包含相機的主動運動和隨機運動，可利用卡爾曼濾波算法[27]對相機的運動進行分離，對相機的隨機運動進行抑制或消除，僅保留相機的主動運動，從而實現對得到的運動估計量的平滑操作。

卡爾曼濾波的預測模型計算式為

(6)

式中X(k|k-1)——第k幀對應的狀態量預估值

P(k|k-1)——第k幀對應的狀態協方差

A——系統的狀態轉移矩陣

Q——過程噪聲方差矩陣

卡爾曼濾波的更新模型計算式為

(7)

式中K(k)——第k幀卡爾曼增益

Z(k)——測量值

H——觀測矩陣

P(k|k)——平滑濾波后協方差

I——單位矩陣

田間抖動視頻運動估計參數進行平滑過程中，R和Q取值既要考慮濾除抖動分量也要保留主動運動分量，經試驗可取Q=0.01和R=1對運動參數進行濾波，能夠達到較好濾波效果。

當田間路面起伏較大時，運動估計變化會很劇烈，如果全局均設置單一Q和R進行濾波操作，輸出結果將會跟原曲線存在較大偏差，造成輸出的視頻圖像存在大范圍未定義區域，增加運動補償的難度且降低圖像質量。為了解決全局單一濾波參數問題，本文研究了自適應調整濾波參數的卡爾曼濾波算法。

由于Q取值較小，難以根據運動量進行準確調節，故本文通過自適應調節參數R，提高整體算法跟蹤及平滑效果。當視頻序列中存在大幅度主動運動時，應減小R，提高系統跟隨性能。

先設一個長度為L的隊列，將得到的運動估計參數(dx,dy,dθ)存入隊列；當隊列已滿時，計算當前幀運動估計量及整個隊列運動估計量總和D(dx,dy,dθ)，之后刪除隊首元素，當前幀運動估計量入隊，根據總和D自適應修正R值。具體計算方式為

(8)

(9)

式中R0——初始值參數

Dx、Dy、Dθ——x、y、θ每個方向估計量單獨總和

當存在主動運動時，D中各方向累積量將會增加，此時R將會減小，卡爾曼濾波器增益K增加，整體跟隨性能提高，保留了主動運動；反之，累積量較小，則相機主動運動量較小，濾波器平滑性能提升，有效去除隨機抖動。

經過卡爾曼濾波后得到抖動運動估計量，對前后幀圖像進行仿射變換，利用雙線性插值[28]對變換后圖像中未定義區域進行校正，最終輸出穩像后圖像。

2 試驗

2.1 試驗設計

本文基于中國科學院合肥物質科學研究院智能機械研究所研制的履帶式農業機器人平臺進行試驗。如圖1所示，機器人運動速度1.8～5.4 km/h，采用的相機傳感器為森云智能公司SG2-IMX390C型低噪聲卷簾快門攝像機，幀頻率為30 f/s，快門時間約5 ms，水平視場角為90°，垂直視場角為47°，有效焦距為4 mm，曝光時間充足，不易出現單幀圖像晃動、斜坡圖形和部分曝光，可以近乎實時地捕獲和處理高分辨率圖像。攝像機水平安裝于農業機器人平臺正前方。

圖1 試驗平臺及場景Fig.1 Experiment platform and scene1.信號接收器 2.農業機器人平臺 3.攝像頭

為檢驗改進后Harris角點檢測穩像算法對田間起伏路面適用性和穩像效果，本文選取長豐縣崗集鎮皖信家庭農莊葡萄樹、火龍果和橘樹種植試驗場景。通過農業機器人平臺以分辨率1 920像素×1 080像素、幀率30 f/s采集機器人田間運動期間起伏路況視頻圖像。

2.2 特征點提取試驗

圖2a為農業機器人平臺在橘樹田間路況場景，圖2b為傳統Harris角點檢測算法提取的角點示意圖。角點檢測過程中，計算出角點響應函數中det(M)與tr(M)值關系如圖2c所示。在設置初始閾值時，通常設為角點響應函數re最大值的k倍，k一般取0.005～0.015，為了提取足夠多角點，本文k取0.005。

圖2 傳統Harris角點檢測算法Fig.2 Traditional Harris corner detection algorithm

通過圖2b可知，傳統Harris角點檢測算法提取到的角點全部集中在圖像上半部分，分布非常不均勻，且提取的角點數目較少。由圖2c可見，傳統Harris角點檢測算法提取的角點呈現聚簇現象，導致提取結果不均勻。

由圖3a可見，與傳統Harris角點檢測算法相比，改進后提取的角點分布更加均勻。由圖3b可見，由于采用子區域自適應閾值設定，角點呈現均勻分布。通過圖3c和圖3d對比可見，改進前后設置的特征角點尺寸相同，改進前角點間易發生聚簇現象，導致同一塊區域的特征角點連在一起難以區分，角點誤匹配率高，不利于后續光流跟蹤；改進后同一區域的角點間隙明顯，避免了局部角點過于聚集問題。

圖3 改進Harris角點檢測算法Fig.3 Improved Harris corner detection algorithm

為了驗證改進后Harris角點檢測的分布情況，在VS2013+Inter(R)Core(TM)i5-8300H CPU試驗設備上分別對300幅圖像使用改進前后的Harris角點檢測方法，進行角點提取。將每幅圖像劃分成3×3子區域，統計每一子區域的角點數，最終計算整幅圖像的子區域角點分布標準差。為確保單一變量原則，保持改進前后角點檢測方法角點數提取一致，將300幅圖像平均分成3份，每份角點數分別為500、800和1 000，計算平均標準差。通常角點數可根據圖像尺寸確定，本文起始角點數設置500，即可較好觀測角點分布情況。為了分析角點數對平均分布標準差影響，需要增加角點數，故設置了800和1 000。表1 為改進前后Harris角點的區域平均分布。

表1 改進前后Harris角點的區域平均分布Tab.1 Regional average distribution of Harris corner points before and after improvement

通過表1可得，改進后Harris角點檢測方法的分布標準差均小于改進前，表明提取的角點分布比原先更加均勻。隨著角點數的增加，改進后標準差的降低率將增大，分布將更加均勻。

2.3 運動平滑試驗

針對不同Q、R參數在卡爾曼濾波中影響，本文研究了參數Q、R在實際運動參數平滑中的特點。圖4為未改進前卡爾曼濾波器中不同Q、R值的濾波效果。由圖4可知，Q越小、R越大曲線越平滑。保留相機主動運動情況下，濾波后的曲線越平滑表明高頻抖動分量消除效果越好。綜合圖4a、4b可知，Q=0.01，R=1時，平滑效果最好。

圖4 R和Q取值不同時的濾波結果Fig.4 Filter results with different values of R and Q

針對卡爾曼濾波器全局單一Q、R參數問題，當相機主動運動分量較大時，恒定系數不能很好保持相機主動運動，需要設置自適應卡爾曼濾波系數進行平滑操作。由于Q系數較小，改進卡爾曼濾波器僅改變R。圖5為改進前后卡爾曼濾波器對旋轉量平滑效果，改進前后Q設置均為0.01，而改進前R設置為1，改進后R設置為自適應，其值同累積量相關?？梢钥闯?，當主動運動較小時，改進前后都能保證較好平滑效果；當主動運動較大時，改進前濾波后曲線明顯不能跟蹤相機運動。

圖5 改進前后卡爾曼濾波器的平滑結果Fig.5 Smoothing results of Kalman filter before and after improvement

圖5a為跟蹤存在單峰主動運動時改進前后卡爾曼濾波平滑結果，改進前其峰值僅為原始參數25.4%，跟蹤效果差，改進后其峰值為原始參數80.5%，在保證隨機運動平滑效果前提下，其跟蹤效果提升55.1個百分點，極大提升了跟蹤性能。

由圖5b可見，跟蹤第1個單峰時，改進后跟蹤效果明顯優于改進前，跟蹤第2個單峰，改進前其峰值為原始參數18.4%，而改進后其雙峰值為原始參數24.8%，在保證隨機運動平滑效果的前提下，其跟蹤效果提升6.4個百分點。綜合單雙峰跟蹤情況，改進后平均跟蹤性能比改進前提升30.75個百分點。

2.4 穩像效果試驗

本文以常用的峰值信噪比(PSNR)[29]和單幀處理時間指標評估測試算法有效性。PSNR越大，說明前后兩幀圖像重合度越高，圖像穩定效果越好；單幀處理時間越短，實時性越好。

取試驗平臺在田間采集的任意200幀抖動視頻圖像計算穩像前后的PSNR。圖6為穩像前后圖像序列的PSNR，可見針對同一幀圖像：穩像后的PSNR普遍大于穩像前，穩像前圖像平均PSNR為18.52 dB，穩像后圖像平均PSNR為21.47 dB，提高2.95 dB，相對于穩像前提高15.93%。

圖6 穩像前后圖像PSNR對比結果Fig.6 PSNR comparison results before and after image stabilization

為更加直觀表明穩像前后效果，如圖7所示，設置相鄰幀圖像穩像前后對照，其中紅色直線為參考輔助線，便于觀察基于參照物圖框中線選取。通過對比穩像前后背景參照物離輔助線的距離，可直觀評估穩像效果：穩像前同一水平線下，景物位置發生了明顯偏移，而穩像后景物位置幾乎在同一位置，穩像后視頻抖動干擾去除明顯。

圖7 穩像前后相鄰幀對比結果Fig.7 Comparison results before and after image stabilization

為驗證本文算法改進效果，選擇基于Harris角點檢測的常用穩像算法開展對比測試。測試圖像數據均來源于試驗平臺采集的田間抖動視頻。測試中設置特征點采集個數為200，進一步提高穩像算法實時性。試驗仿真環境為ubuntu 16.04操作系統及ROS架構，系統配置為Intel(R)Core(TM)i7-8700CPU, 3.20 GHz。為驗證改進后的Harris角點檢測算法與自適應卡爾曼濾波算法對穩像效果影響，分別進行了測試。

通過表2可知，本文算法穩像后圖像的PSNR相比于穩像前提高15.93%，單幀處理時間為25.66 ms；改進后Harris角點檢測算法相對于改進前，PSNR提高3.51個百分點；自適應卡爾曼濾波算法相對于改進前，PSNR提高1.56個百分點；本文算法PSNR總體提高6.64個百分點。自適應卡爾曼濾波算法對實時性影響很小，本文單幀處理時間為25.66 ms，相對于未改進前縮短44.42%，能夠滿足幀速率30 f/s下的實時處理。

表2 穩像效果和速度Tab.2 Image stabilization effect and speed

3 結論

(1)分析了農業機器人平臺采集的田間抖動視頻圖像序列，利用改進Harris角點檢測算法，解決了傳統Harris角點檢測算法提取角點分布不均勻和局部角點聚簇問題；采用自適應卡爾曼濾波算法，解決了傳統卡爾曼濾波算法無法很好跟蹤相機主動運動問題；最后經過穩像處理抑制了相機隨機抖動對視頻圖像影響。

(2)試驗表明，改進后Harris角點檢測算法能有效解決角點聚簇問題，角點區域平均分布標準差隨著角點數目的增加而下降；自適應卡爾曼濾波算法在保證平滑隨機運動前提下，跟蹤主動運動性能提升30.75個百分點；本文算法同傳統基于Harris角點檢測的穩像算法相比，PSNR提高6.64個百分點，單幀處理時間縮短44.42%，且穩像后圖像PSNR相比于穩像前提高15.93%，單幀處理時間為25.66 ms，滿足農業機器人以運動速度1.8～5.4 km/h運動及圖像采集幀速率30 f/s的實時處理要求。