基于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的实时处理要求。