基于Kinect相机的多视角RGB-D信息融合的甜椒姿态估计研究*

牟向伟，孙国奇，陈林涛，于新业，彭柱菁，伍倩

(1.广西师范大学电子工程学院,广西桂林,541004;2.广西师范大学职业技术师范学院,广西桂林,541004)

0 引言

果蔬采摘作业,在果蔬生产过程中是一个至关重要的环节,传统人工采摘方式劳动强度大,生产效率低[1]。利用机器人采摘果蔬,可有效降低成本提高采摘效率。果蔬采摘机器人实现自动采摘主要由果实识别、安全抓取和果实与植株分离3个步骤组成[2],任何一个环节出现问题都将导致采摘失败。对果实识别常用的方法有转化不同颜色空间的方法[3-5]进行分割,以及使用神经网络[6-7]等方法。针对不同的果蔬,采摘方式也有所不同。近年来,机械臂式采摘机器人研究日益受到国内外学者的重视[8]。研究发现,以接近果实生长姿态的采摘方式进行采摘作业比一般的以水平方式直接抓取的采摘成功率有明显提升[9]。另外,通过剪断果实的果梗也可实现果实与植株的分离,利用果实的生长方向有助于研究人员在果梗存在遮挡情况下预测果梗的位置[10-11]。由此可见,果实在空间中三维姿态的识别对果实采摘有着重要的意义,能为果实成功抓取与分离提供有效的信息。果实生长姿态确定方法的研究:文献[12-13]通过对二维图像处理确定苹果的姿态,并未给出果实的三维空间姿态。在三维点云的处理上,Yin等[14]将分割出葡萄目标利用圆柱模型拟合,由于该模型与葡萄形状相差较大,只能提供一个大致的预估姿态。Lin等[15]在提取番石榴果实和树枝后使用一种三维线段检测方法对分割后的番石榴分支进行重构,将果实中心位置和最近分支信息估计果实的三维位姿;Li等[16]提出一种对称轴的水果姿态估计方法,取得了较好的效果,但此方法只适用于成对称状的水果姿态估计;Bac等[17]开发的甜椒采摘机器人,通过双目相机确定果实位置,在甜椒预采摘点通过单目相机获取果柄方位信息,从而确定采摘姿态,但此方法只能用于果柄无遮挡情况。

综上,早期工作在果实姿态提取上取得一定进展,但有仍然存在一定局限性。例如,所提出的算法只能适用于某一种果实或某一类果实,或者精确度太低,难以满足实际的生产需求,尤其是在存在遮挡的情况下无法准确估计出果实的姿态。

为解决现有研究存在的问题,在点云配准阶段提高两视角下甜椒点云的配准精度,对点云配准后的甜椒姿态进行估计,并计算检测精度。本研究以成熟甜椒为研究对象,提出一种基于多视角下RGB-D信息的甜椒姿态估计方法,通过点云配准融合获取的两视角下的点云信息,弥补因叶子或枝干遮挡导致甜椒点云信息残缺导致的姿态估计偏差大、精度低等关键技术问题,使用视觉算法来确定甜椒生长姿态,为果蔬采摘机器人的智能化发展提供理论依据和技术支撑。

1 材料与方法

1.1 图像采集与相机标定

试验地点在室内,研究对象为成熟甜椒(含水率为78.8%,室内温度为28 ℃);研究对象距离图像采集设备为0.8 m;日光灯照射下,使用Azure Kinect DK作为图像采集设备。工作时如图1所示,Kinect相机水平安装在三脚架上,距离甜椒姿态固定装置底部垂直高度15 cm,通过间隔60°的双视角方式获取目标点云的信息,并以30帧/s成像获取1 920像素×1 080像素分辨率的彩色图像与512像素×421像素分辨率的深度图像;试验平台所用计算机为Windows10操作系统、Intel(R) Core(TM) i5-7200U CPU @ 2.5 GHZ、NVIDA GeForce 940MX显卡、12 G内存,在Visual Studio 2019平台上配置Microsoft.Azure.Kinect.Sensor1.4.1、OpenCV3.4.1、PCL1.11.1、Open3D编写算法。

图1 点云采集视角布局图

在获取甜椒彩色图像与深度图像后,需将彩色图与深度图融合生成彩色点云,建立从图像坐标系到世界坐标系之间的数学关系。使用Azure Kinect DK相机对5 mm×7 mm×30 mm的黑白棋盘格,采集不同角度下标定板(共30张),使用MATLAB软件中的Camera Calibrator(相机标定工具箱)对该相机进行参数标定,得到其内部参数,计算初始点云,如图2所示。

图2 初始点云数据图

1.2 图像预处理

相机空间坐标系下包含了原始场景的点云集合,点云中包含了多余的背景信息。因此需要对原始点云进行预处理,将所需的甜椒点云数据从背景中分割出来。基于颜色信息的分割是常用的一种分割方式,由于甜椒RGB信息与背景RGB信息差别较明显,因此根据获取的点云RGB信息,设置点云的颜色阈值范围,将果实从背景中分割出来。同时对点云数据进行下采样与滤波处理,提高后续配准速度与精度。本文经过对果实与背景颜色信息分析,设定当R与G分量的差值大于阈值t时,该点被判定为果实上一点,反之则视为无用点删掉,这样可以很好地将有用数据从庞大的背景信息中筛选出来。本文经过对果实与背景颜色信息分析,将有用数据从庞大的背景信息中筛选出来,使用统计滤波的方式剔除存在的噪声。

1.3 点云配准算法

常见的配准算法有迭代最近点(ICP)算法[18]、正态分布法(NDT)以及奇异值分解法(SVD)。ICP算法配准效果显著,但存在一些缺点,如对配准点云初始状态敏感,计算速度慢,在求解最优目标函数时易陷入局部最优解[19]。由于本文采集的两视角下甜椒点云距离较远,重叠度低,直接使用ICP算法会陷入局部最优解,因此在精配准前需对点云进行粗配准,提供给ICP配准较好的初始状态。首先利用SIFT算法[20]提取甜椒点云的关键特征点,通过FPFH(快速点特征直方图)[21]建立特征点的特征向量描述,之后经过SAC-IA(采样一致性)算法[22]完成两视角下的甜椒点云粗配准,经过粗配准后源点云和目标点云有了一定的重叠度,最后对点云进行精配准。

1.3.1 法向量阈值约束

ICP精配准时,对于目标点云p与源点云中距离最近的点形成对应点对记作q,利用该ICP算法寻找对应点对并形成点集,使距离公式趋于最小值,由于相对应的两匹配点具有相似的局部特征,因此两点的法向量相似。在点云表面处处光滑的情况下,任何点的局部邻域特征都可由曲面重建的方式或基于与其邻近的K个点计算可得,本文利用邻域信息的方法计算点的法向量,对于qi邻域中的点,建立协方差矩阵

(1)

C·Vj=λj·Vjj∈{1,2,3}

(2)

式中:Pi——所要求的法向量特征的点;

λj——矩阵C的特征值;

Vj——对应的特征向量。

通过计算矩阵C的特征值[λ1,λ2,λ3]和其对应的特征向量V1、V2、V3,所求的三个特征向量的最大值即为该点的法向量。计算出法向量特征后,在实际情况下,两向量之间会存在一定夹角,记作θ。通过限制对应点对p1与p2的法向量夹角的阈值删除错误点对,如图3所示,若θ小于设定的阈值ε1,则认为是正确点对并保留,两向量之间会存在的一定夹角θ计算如式(3)所示。

图3 对应点夹角阈值示意图

cosθ=(e1·e2)/(|e1||e2|)

(3)

式中:e1、e2——对应点对p1、p2处的法向量。

1.3.2 曲率阈值约束

经法向量阈值约束后,具有相似法向量特征值的错误点对仍会被保留。因此,本文加入点云的曲率约束条件进一步提纯其对应关系。在式(2)中通过计算点云邻域的协方差矩阵的特征值后,得到λ1、λ2、λ3这3个特征值,若特征值满足λ1<λ2<λ3,则pi的曲率

(4)

同样,两片点云中公共部分对应点p1与p2理论上是相同的,实际上仍会存在一定偏差。故将两对应点曲率的差值记作β,若β小于设定的阈值ε2则认为是错误点对并删除,反之则保留。最终得到剔除错误点后的最近邻点云对,其中两对应点曲率的差值

β=|gi-gj|

(5)

式中:gi、gj——对应点对pi、pj处的曲率。

1.4 双约束项的ICP算法

经过粗配准后,源点云和目标点云有了一定的重叠度,接着使用ICP配准算法进行精配准。构建KD-tree加速搜索速度,并对相对应的匹配点的法向量夹角与曲率阈值双约束条件删除错误配准点,提高配准精度。双约束项的ICP算法步骤如下。

1) 分别构建点云P与Q的KD-tree。

2) 从目标点云P中搜索与源点云Q中的点qi,相应的距离最近的点pi,并使用双约束条件确定对应点对。

3) 根据筛选后的对应点对,求出点云Q和P之间的变换关系R和T。

4) 更新源点云Q,求出Q′=Rqi+T。根据新点云集Q′重复上述3个步骤。同时计算点云P与点云Q′间的距离平方和f如式(6)所示,若平方和小于所设置的阈值则停止迭代,并返回最终的[R,T]矩阵。

(6)

1.5 基于PCA的姿态估计算法

本文整体流程如图4所示。

图4 整体技术流程图

对甜椒点云进行配准,解决因遮挡导致甜椒点云缺失的问题后,利用配准后的点云信息对甜椒姿态进行估计。主成分分析法(PCA)原理是将原有的一组变量组合成一组新的相互无关的几个综合变量,在代数上表现为将原随机向量的协方差矩阵转换成对角阵,使数据指向样本点散布最开的n个方向,对多维数据进行降维处理。PCA主成分分析应用于点云平面检测,法向量特征、降维等方面[23]。PCA算法处理点云步骤如下。

1) 假设点云P={p1,p2,…,pi},点云数目为N,计算其均值,求得该组点云中心点如式(7)所示。

(7)

2) 计算点云样本集的协方差矩阵M=PPT,并将矩阵进行特征分解由于点云是三维数据,计算出三个最大的特征值对应的特征向量,求得的特征向量即为点云3个主方向。

3) 将求解的点云中心点和三个主方向,建立点云朝向的最小包围盒。

4) 通过点云最小包围盒上表面中心点与下表面中心点连线确定点云姿态轴向,如图5所示,直线即为估算出甜椒的生长方向。

图5 基于PCA的姿态估计算法求解的姿态结果

2 试验设计与结果

2.1 配准参数分析

为验证本文所提出的算法的稳定性与可行性,在经由预处理后得到目标甜椒点云,将两片不同视角下点云进行配准,配准过程中各参数的设置影响着甜椒点云的配准效果。其中,SIFT算法中参数的设置影响着提取特征点的个数,进而影响SAC-IA的效果。同时,SAC-IA算法中误差阈值与迭代次数也都影响着最终的配准效果。对于本文所提出的双约束项的ICP配准算法中法向量夹角与曲率的阈值的设置,若阈值设置过大,则仍然会存在错误的匹配点对,若阈值过小,会让对应点不足,则会使配准误差更大,甚至无法完成配准。因此设置合理的参数对最终的配准效果起着至关重要的作用。

2.2 SIFT特征提取算法参数整定

为研究SIFT特征提取算法对点云配准的效果,本文通过改变sift特征提取算法中每层尺度空间的计算尺数octaver,变化区间为3～12,步长为1,研究参数变化对配准效果的影响,并将10次数据变化记录,SIFT参数对甜椒配准效果的影响如图6所示。

图6 Octaver参数对配准效果的影响

分析试验结果可知SIFI算法所提取的关键点个数影响后期配准所消耗的时间,当提取关键点较少时,虽然配准所消耗的时间短,但配准的精度得不到保障。当octaver的值逐渐变大时,所提取的甜椒特征点逐渐增多,点云匹配消耗的时间相应越长,配准精度也在提升,当octaver数值大于一定范围后点云配准效果反而在一定范围内波动,而不是趋于收敛。由图6可以看出当octaver设置为8时,在点云配准的时间较短情况下,仍然有着较好的配准效果。

2.3 其他算法参数配置

通过颜色阈值分割算法用来将配准后的甜椒果实点云从环境中分割出来,好的分割效果能将无用点云都滤除,只保留有用的甜椒信息,提供给甜椒姿态估计一个好的环境。经过反复试验发现,将R-G的值设置为140,B-G的值设置为20时能很好地分割效果。FPFH特征描述算子用来描述特征点邻域空间的几何信息,邻域范围选择的合理性,一定程度上决定了特征点空间信息能否准确描述,试验发现当邻域数设置为20时,能保证配准具有较好的效果。ICP算法中迭代的次数,点云对法向量夹角的阈值、曲率的阈值都影响着ICP配准算法的效果。试验发现当ICP迭代次数设置为25时,法向量夹角阈值设置为10°时,曲率阈值设置为0.005可实现较高的配准速度与精度。

3 结果分析与讨论

3.1 基于双约束项的ICP配准算法误差分析

为验证本文所提出的改进的配准算法的可行性,本文使用斯坦福数据集中Bunny模型与Dragon模型的点云数据。通过比较点云配准消耗的时间和均方根误差RMES。

(8)

本文算法与SAC-IA+ICP算法比较结果,如表1所示。

表1 本文算法与其他算法效果的对比表

从表1中可以看出,在耗时上,由于本文的算法在配准时引入两项条件约束,即使在构建双向KD-tree加速搜索点云对,本文算法在时间上仅仅略快于SAC-IA+ICP算法;在配准误差上,本文算法在Bunny和Dragon数据集上都得到了明显的改善。因此本文所提出的点云配准算法,在点云配准总体效果更优。配准效果图如图7所示。

(a) Bunny配准前效果图

3.2 基于PCA算法的对称轴检测结果

3.2.1 甜椒姿态确定方法与装置误差分析

为验证本文甜椒姿态检测算法的检测准确性,分别在无遮挡情况与遮挡情况下,用本文算法所提出的方法对估计甜椒点云姿态。在试验过程中,使用物理装置将甜椒固定在某一姿态,装置结构示意图如图8所示。

该装置由一个基座,2个U型连接器和一个尾部调节器组成,装置各部分可绕定位孔旋转实现绕x-y-z这3个坐标轴旋转形成不同的姿态,装置尾部调节器上有一笔直的圆筒型长杆,用于穿过甜椒尾部中心点并使之固定,使甜椒的生长方向与长杆朝向一致,本文将圆筒长杆点云拟合成的直线的方向向量作为真实空间甜椒的朝向,并与使用本文算法估计出来的甜椒点云轴向进行对比,验证本文所提出方法用于估计甜椒姿态的精确度。

为了验证装置中细圆柱形长杆点云拟合出来的直线的方向向量能够作为甜椒真实的朝向,对此,本文在试验前先针对长杆点云所拟合出的直线与空间中长杆的真实姿态的误差进行试验分析,将装置尾部调节器旋转不同的角度,用长杆点云使用直线拟合算法求出方向向量并求出其绕Z轴旋转的度数,实际度数与测量出的度数特征曲线如图9所示。由于线性拟合度R2>0.99,因此可以认为尾部长杆点云拟合出来的直线的方向向量可以代表甜椒点云的真实朝向。

图9 测量度数与实际度数的拟合曲线

在验证姿态调整装置上尾部长杆方向向量可代表甜椒真实生长姿态后,通过尾部长杆点云拟合出的方向向量(白线)与本文方法计算出的甜椒点云姿态向量(绿线)的夹角如图10所示,来评价本文算法的精确度。

图10 长杆方向向量与计算的姿态向量夹角示意图

为便于观察,将计算出的甜椒姿态向量移动到长杆方向向量同一顶点上。

3.2.2 无遮挡情况试验结果分析

首先分析了无遮情况下本文算法的稳定性,在无遮挡情况下,调整不同姿态使用本文方法计算甜椒姿态向量与甜椒姿态调整装置长杆方向向量的夹角作为相对误差值,并进行20次试验。结果显示,在无遮挡情况下,误差最小值为4.22°,最大误差为7.11,平均误差为5.15°,相较于文献[17]的姿态误差夹角精度可提高1.85°,具体误差结果如图11所示。

图11 无遮挡情况下甜椒姿态误差值

3.2.3 遮挡情况下试验结果分析

为具体研究遮挡情况下本文方法估计甜椒姿态的误差,随机在甜椒前方不同位置放置叶子若干,并通过Kinect相机采集甜椒图像,使获取的甜椒图像不同部位造成不同程度的遮挡,本文在试验过程中为界定甜椒受枝叶遮挡程度,并将遮挡成为分为轻微遮挡(遮挡30%以下)、一般遮挡(遮挡30%～50%)和严重遮挡(遮挡50%以上)3种情形,如图12所示,通过直接观察获取的甜椒区域的点云图像并综合多人意见将不同视角采集的点云划入以上三种遮挡情形之中。

(a) 轻微遮挡

本文将遮挡试验分为2组,第1组试验为单视角不同遮挡情况,第2组为双视角不同遮挡情况。第1组试验为90次,且两视角下只有一视角存在不同遮挡情况,另一视角则无遮挡情况,每种情况各试验30次,记录每组数据误差的平均值δ1与标准差σ1,试验结果如表2所示。

表2 单视角不同遮挡情况下姿态估计误差值

由第1组试验可以得出,当轻微遮挡情况下时,误差最小值为4.27°,平均误差为5.31°;一般遮挡情况下时,误差最小值为4.66°,平均误差为5.71°;严重遮挡情况下时,误差最小值为4.72°,平均误差为5.98°,综合3种情况下的平均误差为5.67°。由此可见,由于存在另一视角对甜椒点云补充,即使某一视角存在遮挡问题对甜椒姿态的估计也无过大的影响。

第2组试验为30次,一共180次试验。在甜椒前方随机放置叶子若干,模拟甜椒真实环境下的遮挡情况,使甜椒两组视角都有上述三种不同程度的遮挡,使用本文方法对甜椒姿态进行计算得到的平均误差值δ2与标准差σ2和直接从左或右任意单一视角计算出的最小平均误差值δ3与标准差σ3进行对比分析,试验结果如表3所示。

表3 双视角不同遮挡情况下姿态估计误差值

由第2组试验可以得出当视角B存在轻微遮挡时,视角A在三种遮挡情况下(轻微、一般、严重)的平均误差为5.71°、6.05°、5.71°,与单视角计算出的姿态误差值相比都有着较大的改善效果;当视角B存在一般遮挡时,视角A在一般遮挡与严重遮挡时的误差值为7.92°与8.61°,相比单视角下的15.72°与15.49°,本文方法识别结果误差小,在可行性范围之内。

在两视角都存在严重遮挡情况下,若两视角遮挡面积过大,即使融合两视角下点云也会有较大的识别误差,因此,在两视角都严重遮挡时,将严重遮挡面积上限设置在70%,通过本文方法融合两视角下的甜椒点云如图13所示。

可见当存在严重遮挡时,A和B视角下获取的甜椒点云大部分缺失,点云配准时A与B视角下公共区域视角的点较少,本文算法在ICP算法中加入双约束的条件提纯了公共区域对应点对关系,提高了点云配准时的精度,在严重遮挡条件下,两片点云融合度仍然较高,由于配准的点云信息仍然有限,在使用PCA算法对点云姿态估计时精度上有所降低,最终在双视角严重遮挡情况下识别结果平均误差为13.18°。

3.3 算法效率分析

本文进行了算法性能分析,模拟一株甜椒植株(共4个甜椒)的生长环境,如图14所示,并随机对植株上的甜椒造成不同情况的遮挡,对两个视角下获取的甜椒点云信息进行融合,并分割出单个甜椒对象,使用基于PCA的姿态估计算法对每个甜椒姿态进行估计,并进行5次试验。

图14 模拟的甜椒植株示意图

其中对整个甜椒植株中的甜椒进行识别、配准、分割的平均总运行时间为7.613 s,单个甜椒的姿态估计时间均低于0.4 s,能够满足果实采摘机器人的实时性能要求,并能移植到采摘机器人系统上。

4 结论

1) 针对在复杂环境下由于存在遮挡,获取的甜椒点云不完整,导致甜椒姿态识别精度低,甚至无法识别的问题,提出一种融合多视角下的甜椒RGB-D信息,利用点云配准算法将甜椒点云转化到同一角度下,扩充甜椒点云信息,并提高配准精度,运用PCA算法对甜椒姿态进行解析,得到计算出的甜椒姿态向量。

2) 对原始的ICP配准算法进行改进,提出基于点云法向量与曲率约束的ICP配准算法,并在不同数据集与拍摄的甜椒点云进行验证,与传统的ICP算法相比,配准精度均有所提高,在略短的配准时间下降低了甜椒点云的配准误差,提高了两视角下甜椒点云的融合度,进一步提高甜椒姿态向量的计算精度。

3) 为验证本文所提出方法的可行性,本文在不同情况下进行试验(无遮挡、单视角遮挡,双视角遮挡)进行试验。结果表明,在无遮挡情况下,本文算法误差均值为5.15°,在双视角中其中某一视角有遮挡情况下三种遮挡情况综合均值误差为5.67°,双视角均有遮挡情况下综合平均误差为7.86°,完成两视角甜椒点云信息配准后,单个甜椒的姿态估计时间均小于0.4 s。