基于深度相机的汽车转向节位姿估计研究

刘学超，张波，郑魁敬

(1.燕山大学，先进锻压成形技术与科学教育部重点实验室，河北秦皇岛 066004；2.燕山大学，河北省并联机器人与机电系统实验室，河北秦皇岛 066004)

0 前言

目前消费者对产品质量要求越来越高，而高级技工数量有限，因此希望机器人能代替一部分劳动力。在人工智能推动下，机器人开始智能化，并逐渐进入生产一线代替工人工作，但其应用范围及深度还有待挖掘和拓展。张翰博等利用视觉关系推理实现了多物体堆叠机器人抓取。韩冬等人利用解耦控制实现了腱驱动多指机械手灵巧抓取。WENG等利用机器学习和彩色摄像头指导机器人抓取透明物体。卢道华等利用多个摄像头采集图像，通过PnP算法计算集装箱姿态。MATEO等实现了刚体六自由度位姿估计。TEJANI等研究了基于霍夫森林的6D位姿估计。XIANG等提出了PoseCNN的位姿估计方法。

针对汽车转向节自动化加工中的上下料场景，以转向节内孔为基准点，通过视觉引导机器人实现抓取。利用2D图像获取转向节位置并结合点云确定转向节姿态。转向节位姿检测方案分3个部分：相机标定与点云产生、转向节抓取点检测和姿态检测。

1 RGBD相机标定与点云产生

1.1 RGBD相机

Intel RealSense D435i相机如图1所示，可以输出彩色图和深度图。深度相机坐标系以左侧红外相机为基准，深度信息的获取由左红外相机、右红外相机和红外投射器联合工作完成。左右红外相机捕获场景并将成像器数据发送到深度成像处理器，通过左右图像上匹配的关联点来计算图像中每个像素的深度。

图1 Intel RealSense D435i相机

1.2 RGBD相机标定

由于相机透镜误差给图像带来了畸变，需对相机内、外参数进行标定。采用一个长度为5的向量=[,,,,]描述相机畸变模型。

所选标定板为9×7黑白相间边长为25 mm的正方形棋盘格标定板，如图2所示。

图2 棋盘格

RGB相机内参数如表1所示，使用RGBD相机对不同位置、不同角度下的标定板拍摄15张不同姿态棋盘格图片进行标定，RGBD相机映射误差为0.14像素。

表1 Intel RealSense D435i彩色相机内参 单位:像素

彩色相机畸变系数为

[-0.051 99,2.092 44,-0.002 004,-0.000 357,-7.778 8]

深度相机内参数如表2所示，同时使用深度相机对不同位置、不同角度下的标定板拍摄15张不同姿态棋盘格图片进行标定，深度相机映射误差为0.12像素。

表2 Intel RealSense D435i深度相机内参 单位:像素

深度相机畸变系数为

[-0.028 22,0.309 87,0.002 92,-0.001 14,-0.881 4]

1.3 彩色图和深度图对齐

由于Intel RealSense D435i相机获取的彩色图与深度图之间存在一定偏移，为确保通过图像拟合确定的转向节抓取基准点能映射到准确的深度信息，需将深度值和彩色图像素坐标匹配，同时通过一一对应生成(,,,,,)格式的信息，转化为点云以估计转向节姿态。需要求出深度相机坐标系与彩色相机坐标系之间的变换关系如图3所示，空间中一点在世界坐标系中为(,,)，在深度相机坐标系中为(,,)，在RGB相机坐标系中为(,,)。

图3 坐标系转换原理

彩色像素坐标表示为(,)，深度图像素坐标表示为(,)，则存在以下关系：

=

(1)

=

(2)

式中：为彩色相机内参；为深度相机内参。

和通过旋转矩阵和平移向量关联：

=+

(3)

点与和之间关系可以通过RGB相机和深度相机的外参矩阵求得：

=+

(4)

=+

(5)

式中:和分别为世界坐标系到RGB相机坐标系和深度相机坐标系的旋转矩阵；和分别为世界坐标系到RGB相机坐标系和深度相机坐标系的平移向量。

可求出和：

(6)

(7)

RGB相机和深度相机之间的旋转矩阵和平移矩阵确定如下：

=[0.014 736 0.000 029 0.000 316]

根据和进行深度图与彩色图对齐，并对转换的像素坐标系的点按式(8)进行过滤，剔除不合理的点。

(8)

彩色图与深度图对齐的结果如图4(a)所示。根据对齐后的信息，每一个彩色像素点对应的3D坐标，通过遍历所设置的分辨率为1 280像素×720像素图像的每个像素的(,,,,,)格式的信息，并利用PCL点云库Point(x y z R G B)格式的数据结构生成点云，如图4(b)所示。

图4 对齐的结果与点云

2 转向节抓取点定位

2.1 图像预处理

选取转向节中心孔上表面圆心为抓取基准点，确定转向节基准点在机器人坐标系和相机坐标系下的坐标。在彩色图中把转向节单独区分出来，并对原图像中的噪声和图像二值化后出现的小杂点进行图像预处理，如图5所示。

图5 图像预处理对比图

2.2 转向节区域轮廓提取

使用边缘检测对转向节进行像素级提取非常关键。在对图像的边缘检测处理后，需要对转向节内孔边缘信息进行搜索及提取，以便拟合基准点像素坐标。边缘检测结果如图6所示。

图6 边缘检测结果

边缘检测会出现多条轮廓，需要设定条件将转向节中心孔的轮廓提取出来。轮廓形状各异，可以很好地进行区分，且轮廓面积周长有显著差异，可设定轮廓周长为390～420像素，面积为10 200～10 800像素，以提取转向节中心孔轮廓。

提取转向节中心孔的一组数据点如图7所示，这组数据点近似地落在一个圆上，采用最小二乘法来估计这个圆的参数。

图7 轮廓提取

设要拟合的转向节中心圆孔曲线方程为

=(-)+(-)

(9)

改写为

=-2++-2+

(10)

令=-2、=-2、=+-，可得圆曲线方程的另一个形式：

++++=0

(11)

提取的中心孔轮廓点集(，)(∈(1,2,3，…,))到圆心的距离为

(12)

点(,)到圆心距离的平方与半径平方的差为

(13)

要求距离的平方和最小：

(14)

计算使(,,)具有最小值的、、,以确定拟合圆的参数,对(,,)求导可得：

(15)

则：

(16)

式中：

将轮廓操作提取的转向节中心孔的轮廓点集代入式(16)，求得、、、、的值，并求出、、的值，得到通过最小二乘法拟合的转向节中心圆孔的参数、和。其中，和为转向节抓取基准点的像素坐标，分别为565、320像素，执行结果如图8所示。

图8 拟合圆结果

2.3 相机坐标系下抓取点坐标确定

为获取转向节中心孔上表面圆心深度，可以计算转向节中心孔上表面圆的一组对应点深度的平均值作为深度坐标。根据确定出来的圆心，设置过圆心与水平线成0°、±45°、90°的8组方向。根据拟合求得的半径，设置搜索区域[-3,+7]进行坐标搜索，搜索区域如图9红线部分所示。

图9 设置搜索半径

根据对齐后的深度图，获取搜索区域深度，按照由内向外的顺序组成列表。依照顺序计算相邻两值的差值，并定位最大差值的位置。利用该位置之后的值计算的平均值代替该搜索方向确定的深度。以同样方法获取8组搜索区域的深度。由于转向节内孔部分有小凹槽，还需去除对应方向上深度值相差超过5 mm的搜索方向，将剩余搜索方向的平均值作为抓取基准点的深度。根据内参矩阵、转向节抓取基准点像素坐标及深度值得到基准点在彩色相机坐标系下的三维坐标，结果如图10所示。

图10 转向节抓取基准点定位坐标

3 转向节姿态估计

3.1 点云预处理

由于转向节会倾斜，不能保持同一姿态，需要确定转向节姿态从而实现机器人精准抓取。转向节材质为铝，表面存在一定的反光、光线干扰等导致获取点云时出现噪声和离群点。如果不去除离群点，会影响转向节特征描述子计算，导致点云配准出现误差，影响转向节姿态估计精度。在对点云处理之前，需对转向节部分数据去除离群点。如果直接把整个原始点云进行配准计算会带来很大干扰，需从原始点云中将转向节部分单独分离出来。算法流程如图11所示，处理结果如图12所示。

图11 点云预处理算法流程

图12 点云预处理结果图

实际应用中，转向节孔口朝上特征明显，无须利用完整的转向节点云，通过局部点云模型法可以构建出满足转向节姿态估计的模板点云。点云预处理获取转向节部分点云后采用ICP(Iterative Closest Point)算法进行拼接，得到相对完整的转向节点云模板。点云生成流程如图13所示，转向节模板点云如图14所示。

图13 模板点云生成流程

图14 模板点云

3.2 基于特征描述子的转向节姿态初步估计

对转向节进行姿态估计是通过模板转向节点云与通过点云预处理得到的待估计转向节点云的配准实现的。区分转向节曲面空间的分布情况和寻找相对应的点，引入局部特征描述子，通过计算邻域内相应特征，使特征描述子能够描述转向节表面，通过表面的几何特征寻找对应点，找到对应点之后再基于ICP算法计算转换矩阵。采样一致性初始配准算法(SAC-IA)可以通过几种不同的特征描述子来寻找对应点进行初始配准，因此用来作为转向节姿态初始估计，其算法流程如图15所示。

图15 SAC-IA算法流程

提取模板转向节点云特征点和FPFH(Fast Point Feature Histograms)特征描述子建立特征集合=[,,,]，其中，(,,)表示SIFT3D特征点坐标、表示FPFH局部特征。以同样方式建立待检测转向节点云的特征集合=[,,,]，用SAC-IA算法对模板转向节和待检测转向节进行配准，获取初始位姿信息。

配准过程中，通过Huber函数计算转向节匹配点变换后的距离误差和，判断待检测转向节点云与模板转向节点云间配准变换的精度。Huber函数对噪声有好的鲁棒性，表示为

(17)

式中:为Huber的参数，需要预先设定；为第组匹配区域的距离误差。

利用式(17)对误差进行评价，在所有的匹配中寻找一组最优匹配，使两转向节间距离误差和最小。获得的转换矩阵作为粗配准的结果，不同姿态下FPFH配准结果如图16所示。

图16 FPFH配准结果

3.3 基于NDT的转向节姿态精准估计

利用粗配准算法估计的转向节姿态结果达不到精度要求，还需要细配准来提高转向节姿态估计精度。利用NDT(Normal Distributions Transform)算法对待检测转向节与模板转向节进行细配准，流程如图17所示。

图17 NDT算法流程

统计模板转向节与待检测转向节的点云分布信息，利用转换矩阵将待检测转向节的点云转换到模板转向节坐标系中，如果其点云数据点在模板转向节点云中的概率密度很大，则该矩阵参数较好。细配准结果如图18所示。

图18 多姿态NDT配准结果

4 软件开发与测试

为验证转向节基准点定位和转向节姿态估计算法，在VS平台上利用C++多线程技术实现多窗口同步显示，数据流程如图19所示。

图19 姿态估计数据流程

图20所示为直观显示转向节位姿信息的系统界面。该系统采用MFC设计，调用OpenCV图像处理库和PCL点云库等对图像及点云进行处理。

图20 系统界面

根据深度误差与深度的关系，设定相机距离采集平面为800 mm，采集转向节不同姿态下的点云图像，估计不同姿态下的姿态。图21所示为采用SAC-IA算法得到的结果，可以看出粗配准的结果还有一定的误差，不能用于最终的姿态估计结果。图22所示为NDT算法的结果，NDT算法的初始转换矩阵设置为SAC-IA算法得到的转换矩阵，可以看出：配准后获得了更精确的姿态估计结果；转向节在不同位置及姿态下，都能准确识别姿态信息。

图21 粗配准结果

图22 细配准结果

不同姿态下，姿态1、姿态2粗配准分别对应的旋转矩阵如、所示:

不同姿态下,姿态1、姿态2细配准分别对应的旋转矩阵如下、所示：

为配准实现数值化，计算30次转向节间点云匹配中getFitnessScore()函数的平均值，该值是所有对应点间的距离平方和，值越小配准效果越好。设置了对照组测试，选取模板转向节的点云进行配准来作为对照组，粗、细配准结果分别如图23、图24所示。最终getFitnessScore得分如表3所示。

图23 对照组粗配准结果

图24 对照组细配准结果

表3 对照点云与粗细配准得分 单位：mm2

单纯地基于特征描述子的姿态估计难以获得良好结果，从对照组的结果看，同一转向节获得的分数更低，配准效果更好，不同转向节间有微小差别，对配准有一定影响。NDT算法在粗配准基础上进行细配准，取得了更好效果，getFitnessScore(最近对应点距离平方和)得分为27 mm，表明转向节姿态估计算法有效。

5 结论

对转向节进行基准点定位，通过轮廓检测过滤及轮廓拟合确定抓取点的像素坐标，设置搜索区域中的突变值取平均值获取深度，利用相机参数获取相机坐标系下的三维坐标。通过去除背景将转向节从原始点云中分离出来，利用滤波算法去除噪声及离群点。提取转向节特征点及特征描述子，用SAC-IA算法粗配准，用NDT算法细配准，getFitnessScore得分为27 mm，实现了转向节姿态的精确估计。