确定性退火算法的TOF相机自主导引叉车/托盘相对位姿测量

李 鹏，王鑫鑫，陈立久，邓甘霖，陈杰勇

确定性退火算法的TOF相机自主导引叉车/托盘相对位姿测量

李 鹏，王鑫鑫，陈立久，邓甘霖，陈杰勇

（湘潭大学自动化与电子信息学院，湖南 湘潭 411100）

针对自主搬运车与物料托盘之间相对位置、相对姿态解算问题，利用基于飞行时间（TOF）的深度相机采集的灰度和深度信息进行托盘关键点提取和匹配，并基于此确定双方相对位置和姿态。首先，根据相机灰度数据进行阈值分割和边缘检测确定目标区域，并使用加速分割检测特征角点检测（FAST）算法对目标区域提取关键点；
进而根据无向图理论，结合马尔科夫随机场和确定性退火算法，匹配关键点集和模板点集；
最后，根据匹配点进行基于奇异值分解（SVD）的迭代临近点（ICP）估计位姿，确定搬运车与托盘之间的相对位姿。仿真实验表明，关键点集匹配精度达80%以上，位置精度15 mm，姿态精度2°，算法精度和实时性均满足工程需求。

深度相机；
位姿估计；
确定性退火算法；
马尔科夫随机场

自动引导车（automated guided vehicle, AGV）是一种用于搬运物料的无人驾驶导航车，目前已在智能仓储系统中发挥着重要的作用。AGV小车从复杂的场景中检测出物料托盘并估计自身与托盘的相对位姿，实现搬运小车与托盘的对接，因此对目标进行精确定位是移动机器人系统的关键。对于室内AGV的定位有多种方式，基于视觉的相对位姿估计一般通过特征提取、特征匹配、位姿估计得出智能车运动的平移信息和方向信息，由于运动前后的特征点可以通过立体匹配的视差或深度数据计算得到其三维坐标，所以目前视觉运动估计算法一般都采用三维特征对应的运动估计算法。

使用激光雷达定位也是常用的定位方式[1]，其具有定位精度高、测量距离远、对光照不敏感等优点，但实现较为复杂，且在有透明或反射物体的环境下应用仍具有巨大挑战。相机成本较低，近年来利用相机进行室内定位的技术已成为AGV小车的主要研究方向。融合无线节点、惯性导航、视觉引导的室内定位系统[2]，虽然能满足定位精度要求，但多种传感器信息融合复杂，无法保证实时性。基于标签作为人工路标的视觉检测方法虽能实现定位需求[3-5]，但标签易受损，影响检测。文献[6]使用深度相机通过检测托盘点云数据进行轮廓提取与模板匹配来测量相对位姿，达到定位的效果，且不易受光照影响，但点云信息量较为庞大，不易处理。近年来，深度学习的方法也逐渐应用至相对位姿的测量中。文献[7-9]采用深度学习方法进行托盘检测，求取搬运小车与目标之间的位姿实现对接，但深度学习方法需要大量的模型训练才能达到一定精度与效率要求，对于简单场景的视觉定位，实现过程相对复杂。

由此，针对自主搬运车与物料托盘之间相对位置、相对姿态解算问题，提出精确搬运小车的局部定位，以实现小车与物料托盘之间的精确对接。利用TOF 相机采集的灰度和深度信息进行托盘关键点提取和匹配，并基于此确定双方相对位置和姿态。首先对目标区域进行分割提取，对目标区域使用加速分割检测特征角点检测算法（features from accelerated segment test, FAST）提取其关键角点信息，进而基于无向图理论，使用马尔可夫随机场与确定性退火算法，使得关键点集与模板点集相匹配，最后对匹配点进行基于奇异值分解（singular value decomposition, SVD）的迭代临近点（iterative closest point, ICP）位姿估计，确定搬运小车位置与姿态。

图1 相机成像模型

由相似三角形可得相机坐标系与物理成像坐标系中坐标关系为

式中为相机的焦距，即图1中相机坐标系原点到物理成像坐标系原点的距离。

图2 物理成像坐标系与像素坐标系关系

因此，世界坐标系中一点在相机观测坐标系中的坐标与像素坐标系中坐标之间的关系为

对相机拍摄图片通过目标灰度阈值分割、边缘提取定位目标边缘位置，在其边缘进行角点检测来提取所需关键点进行位姿估计与局部定位。

2.1 目标区域分割

图像边缘是图像的基本特征之一，实验所需关键角点集中在目标边缘。对目标所在区域进行边缘提取，提取出图像灰度值变化显著的部分，减少角点检测时的运算压力，同时可辅助验证角点提取的完整性与准确性。边缘提取首先使用高斯滤波器对原图像进行降噪平滑处理，减少噪声对边缘定位精度的影响，距离越近权重越高，对该像素点的灰度值的贡献也就越大，从而可以很好地抑制噪声。常用高斯函数为式（5），使用高斯函数对图像进行卷积得到平滑后的图像（如式（6）所示）。

2.2 目标区域分割关键角点提取

角点可以保留图像的重要特征，减少信息数据量提高运行速度，有利于实时匹配。FAST角点检测算法是计算机视觉领域的一种经典算法，在运行效率方面具有巨大优势，当目标像素点邻域范围内有足够数量的像素点与之不相似，则认为该像素点为角点[10]。图3为FAST角点检测模板。

图3 FAST角点检测模板[10]

如图4所示，通过灰度阈值分割进行目标区域提取；
然后对目标区域进行边缘检测，提取出目标物体边缘，如图5所示；
最后使用FAST角点检测提取出所需关键点，如图6所示。对所检测出的关键点进行下文中的关键点匹配。

图4 灰度阈值分割目标区域提取

图5 边缘位置提取

图6 FAST角点检测与筛选

3.1 确定性退火进行全局优化

3.2 基于SVD的ICP位姿估计

物体位姿是指相机当前坐标系相对于原始物体所在的世界坐标系发生的平移和旋转变换。原始物体可以在世界坐标系任何位置，通常将原始物体的重心和朝向与世界坐标系对齐，即将物体坐标系与世界坐标系重合，通过求取世界坐标系到相机坐标系的变换求取相对位姿。

对于ICP的求解可用奇异值分解方法，SVD是线性代数中一种矩阵分解方法，可用来求解ICP中的旋转矩阵和平移矢量进行位姿估计。其方法如下：

4.1 关键点匹配对比实验

为了验证本文中关键点匹配算法的准确性与时效性，实验中设定相机高度为0.5 m，在相机与托盘前平面相距1.5～4 m、相机旋转角度为–30°～30°范围内，调整相机位姿并记录当前帧的灰度数据与深度数据，共测得100组数据。对所测数据提取关键点后采用确定性退火算法、ICP配准算法、相干点漂移算法（coherent point drift, CPD）进行点集匹配，测得3种方法匹配精度与平均匹配时间（如表1所示）。

表1 配准方法性能比较

ICP算法对待配对点集的初始位置比较敏感，且容易被外点影响导致点配准失败。CPD算法实现复杂度高，匹配所需时间长，不利于实时位姿解算。本文采用基于马尔可夫的确定性退火算法最终得到的匹配点对是一一映射的，具有一定的抗干扰能力，可以有效地去除外点影响，匹配准确率较高，且在匹配时间上也具有一定优势。

4.2 相对位姿解算实验

为验证位姿解算精度，实验采用软件接口openGL创建托盘模型，根据相机内参设定虚拟相机的投影矩阵模拟真实相机，使用openGL中的缓冲模拟深度相机深度数据。实验中固定托盘位置与朝向不变，托盘前平面底边中心与世界坐标系原点重合。调整相机在世界坐标系中位置与朝向，记录灰度数据与深度数据，进而根据上述原理解算相机位姿，计算测量误差。

世界坐标系使用右手坐标系，水平方向为轴，垂直地面方向为轴。托盘位置保持不变。考虑实际应用情况，相机高度固定为0.5 m，即相机轴坐标固定为0.5 m，相机从距托盘1.4 m位置向远离托盘方向移动，即从世界坐标系中(0，0.5，-1.5)位置平行于轴向负方向移动，每移动0.2 m记录相机一帧数据。使用文中算法解算平移矢量，测得轴方向平移误差如图7所示。

图7 z轴平移误差分析

测量水平方向位移误差时，相机距托盘前平面垂直距离为3 m，沿水平方向移动，即相机从世界坐标系中(0，0.5，3)位置沿平行于轴方向移动。考虑到相机视野问题，只测量位移1 m之内数据。测得水平方向平移误差如图8所示。

从平移实验中可以看出，轴平均位移误差为12 mm，测量误差随距离增加也有增大趋势，但最大误差不超过15 mm，在测试范围内能满足实验要求。

图8 x轴平移误差分析

图9 旋转角测量示意图

如图10所示，由旋转角度误差图可看出，旋转角度误差控制在2°之内，满足实际检测需要。

图10 旋转角测量分析

本文采用TOF相机对搬运叉车和物料托盘之间的相对位置和相对姿态进行测量，实现自主搬运叉车无人作业需求。根据物料托盘灰度图像和深度图像，进行关键点提取与匹配，利用基于SVD的ICP算法解算搬运车和托盘之间的相对位置和相对姿态。仿真实验表明，该算法能够满足自主搬运叉车/物料托盘相对位姿测量精度和实时性的要求，并具有一定的鲁棒性。

[1] KIM J, CHUNG W. Localization of a mobile robot using a laser range finder in a Glass-Walled environment[J]. IEEE Transactions on Industrial Electronics, 2016, 63(6): 3616-3627.

[2] 吴平, 刘刚军, 牛志朝, 等. 室内AGV导引和定位系统设计[J]. 计算机工程与设计, 2019, 40(6): 1782-1787.

[3] 张浩悦, 程晓琦, 刘畅, 等. 基于全局稀疏地图的AGV视觉定位技术[J]. 北京航空航天大学学报, 2019, 45(1): 218-226.

[4] HU X, LUO Z, JIANG W. AGV localization system based on ultra-wide band and vision guidance[J]. Electronics, 2020, 9(3): 448.

[5] 李照, 舒志兵. 一种改进二维码视觉精定位AGV技术研究[J]. 控制工程, 2019, 26(6): 1049-1054.

[6] 武文汉, 杨明, 王冰, 等. 一种基于轮廓匹配的仓储机器人托盘检测方法[J]. 上海交通大学学报, 2019, 53(2): 197-202.

[7] LI Y, WANG G, JI X, et al. Deepim: deep iterative matching for 6d pose estimation[C]// European Conference on Computer Vision (ECCV). Proceedings of the European Conference on Computer Vision. Munich, Germany: ECCV, 2018: 683-698[2022-07-16].

[8] JIA F, TAO Z, WANG F. Pallet detection based on Halcon and AlexNet network for autonomous forklifts[C]// The Institute of Electrical and Electronic Engineers（IEEE）. Proceedings of 2021 International Conference on Internet, Education and Information Technology (IEIT). Suzhou, China: IEEE, 2021: 86-89[2022-07-16].

[9] 张亚辉, 杨林, 白雪. 基于Faster RCNN网络的仓储托盘检测方法[J]. 现代信息科技, 2021, 5(2): 57-62, 65.

[10] ROSTEN E, DRUMMOND T. Machine learning for high-speed corner detection[C]//European Conference on Computer Vision (ECCV). Proceedings of 9th European Conference on Computer Vision. Graz, Austria : ECCV, 2006: 430-443[2022-07-16].

[11] 曲寒冰, 王加强, 李彬, 等. 基于概率图模型的点集匹配方法研究[J]. 自动化学报, 2015, 41(4): 694-710.

[12] 张祥德, 唐青松. 确定性退火技术及其在点匹配中的应用[J]. 东北大学学报, 2003, 24(11): 1119-1122.

[13] ARUN K S, HUANG T S, BLOSTEIN S D. Least-squares fitting of two 3-D point sets[J]. IEEE Transactions on Pattern Analysis and Machine Intelligence, 1987 (5): 698-700.

[14] POMERLEAU F, COLAS F, SIEGWART R. A review of point cloud registration algorithms for mobile robotics[J]. Foundations and Trends in Robotics, 2015, 4(1): 1-104.

Relative pose measurement of AGV and pallet based on deterministic annealing using TOF cameras

LI Peng, WANG Xinxin, CHEN Lijiu, DENG Ganlin, CHEN Jieyong

（College of Automation and Electronic Information, Xiangtan University, Xiangtan, Hunan 411100, China）

Aiming at the problem of solving the relative position and relative oritention between the automated guided vehicle (AGV) and the material pallet, the gray and depth information collected by the time-of-flight (TOF) camera is used to extract and match the key points of the pallet, based on which the relative position and oritention of both sides will be determined. Firstly, recognize the target area through the threshold segmentation and edge detection according to the camera gray data, and use the features from accelerated segment test (FAST) algorithm to extract the key points of the target area. Then, match the key point set with the template point set on the basis of the undirected graph theory,combined with Markov Random Field and deterministic annealing algorithm, the key point set and the template point set are matched. Finally, the singular value decomposition (SVD) based iterative closest point (ICP) method is performed to estimate the pose based on the matching point, and the relative pose between the AGV and the pallet is determined. Simulation experiments show that the matching accuracy of the key point set is up to 80%, with the position accuracy 15 mm, the oritention accuracy 2°. Meanwhile, the algorithm accuracy and real-time performance both meet the engineering requirements.

time-of-flight camera; pose estimation; deterministic annealing; Markov random field

P228

A

2095-4999(2022)06-0081-07

李鹏，王鑫鑫，陈立久，等. 确定性退火算法的TOF相机自主导引叉车/托盘相对位姿测[J]. 导航定位学报, 2022, 10(6): 81-87.（LI Peng，WANG Xinxin，CHEN Lijiu，et al. Relative pose measurement of AGV and pallet based on deterministic annealing using TOF cameras[J]. Journal of Navigation and Positioning, 2022, 10(6): 81-87.）

10.16547/j.cnki.10-1096.20220611.

2020-07-13

国家自然科学基金项目（61773330）；
国家重点研发计划项目（2020YFA0713501）；
湖南省自然科学基金项目（2021JJ50126）；
湖南省教育厅重点项目（21A0083）。

李鹏（1978—），男，山东青岛人，博士，教授，研究方向为室内外导航定位。

王鑫鑫（1994—），男，河南开封人，硕士研究生，研究方向为视觉定位。

猜你喜欢 角点位姿关键点 论建筑工程管理关键点建材发展导向(2022年3期)2022-04-19肉兔育肥抓好七个关键点今日农业(2021年8期)2021-11-28多支撑区域模式化融合角点检测算法仿真计算机仿真(2021年8期)2021-11-17建筑设计中的防火技术关键点建材发展导向(2021年11期)2021-07-28无人机动平台着陆惯性/视觉位姿歧义校正算法中国惯性技术学报(2020年4期)2020-12-14船舶清理机器人定位基准位姿测量技术研究浙江海洋大学学报(自然科学版)(2020年5期)2020-06-19角点检测技术综述①计算机系统应用(2020年1期)2020-01-15基于灰度差预处理的改进Harris角点检测算法电子技术与软件工程(2019年9期)2019-07-12优化ORB 特征的视觉SLAM电子技术与软件工程(2019年6期)2019-04-26基于FAST角点检测算法上对Y型与X型角点的检测电子技术与软件工程(2018年10期)2018-07-16

相关热词搜索：退火 导引 叉车