李 鵬，王鑫鑫，陳立久，鄧甘霖，陳杰勇

確定性退火算法的TOF相機(jī)自主導(dǎo)引叉車/托盤相對(duì)位姿測(cè)量

李 鵬，王鑫鑫，陳立久，鄧甘霖，陳杰勇

（湘潭大學(xué)自動(dòng)化與電子信息學(xué)院，湖南 湘潭 411100）

針對(duì)自主搬運(yùn)車與物料托盤之間相對(duì)位置、相對(duì)姿態(tài)解算問(wèn)題，利用基于飛行時(shí)間（TOF）的深度相機(jī)采集的灰度和深度信息進(jìn)行托盤關(guān)鍵點(diǎn)提取和匹配，并基于此確定雙方相對(duì)位置和姿態(tài)。首先，根據(jù)相機(jī)灰度數(shù)據(jù)進(jìn)行閾值分割和邊緣檢測(cè)確定目標(biāo)區(qū)域，并使用加速分割檢測(cè)特征角點(diǎn)檢測(cè)（FAST）算法對(duì)目標(biāo)區(qū)域提取關(guān)鍵點(diǎn)；進(jìn)而根據(jù)無(wú)向圖理論，結(jié)合馬爾科夫隨機(jī)場(chǎng)和確定性退火算法，匹配關(guān)鍵點(diǎn)集和模板點(diǎn)集；最后，根據(jù)匹配點(diǎn)進(jìn)行基于奇異值分解（SVD）的迭代臨近點(diǎn)（ICP）估計(jì)位姿，確定搬運(yùn)車與托盤之間的相對(duì)位姿。仿真實(shí)驗(yàn)表明，關(guān)鍵點(diǎn)集匹配精度達(dá)80%以上，位置精度15 mm，姿態(tài)精度2°，算法精度和實(shí)時(shí)性均滿足工程需求。

深度相機(jī)；位姿估計(jì)；確定性退火算法；馬爾科夫隨機(jī)場(chǎng)

0 引言

自動(dòng)引導(dǎo)車（automated guided vehicle, AGV）是一種用于搬運(yùn)物料的無(wú)人駕駛導(dǎo)航車，目前已在智能倉(cāng)儲(chǔ)系統(tǒng)中發(fā)揮著重要的作用。AGV小車從復(fù)雜的場(chǎng)景中檢測(cè)出物料托盤并估計(jì)自身與托盤的相對(duì)位姿，實(shí)現(xiàn)搬運(yùn)小車與托盤的對(duì)接，因此對(duì)目標(biāo)進(jìn)行精確定位是移動(dòng)機(jī)器人系統(tǒng)的關(guān)鍵。對(duì)于室內(nèi)AGV的定位有多種方式，基于視覺(jué)的相對(duì)位姿估計(jì)一般通過(guò)特征提取、特征匹配、位姿估計(jì)得出智能車運(yùn)動(dòng)的平移信息和方向信息，由于運(yùn)動(dòng)前后的特征點(diǎn)可以通過(guò)立體匹配的視差或深度數(shù)據(jù)計(jì)算得到其三維坐標(biāo)，所以目前視覺(jué)運(yùn)動(dòng)估計(jì)算法一般都采用三維特征對(duì)應(yīng)的運(yùn)動(dòng)估計(jì)算法。

使用激光雷達(dá)定位也是常用的定位方式[1]，其具有定位精度高、測(cè)量距離遠(yuǎn)、對(duì)光照不敏感等優(yōu)點(diǎn)，但實(shí)現(xiàn)較為復(fù)雜，且在有透明或反射物體的環(huán)境下應(yīng)用仍具有巨大挑戰(zhàn)。相機(jī)成本較低，近年來(lái)利用相機(jī)進(jìn)行室內(nèi)定位的技術(shù)已成為AGV小車的主要研究方向。融合無(wú)線節(jié)點(diǎn)、慣性導(dǎo)航、視覺(jué)引導(dǎo)的室內(nèi)定位系統(tǒng)[2]，雖然能滿足定位精度要求，但多種傳感器信息融合復(fù)雜，無(wú)法保證實(shí)時(shí)性?；跇?biāo)簽作為人工路標(biāo)的視覺(jué)檢測(cè)方法雖能實(shí)現(xiàn)定位需求[3-5]，但標(biāo)簽易受損，影響檢測(cè)。文獻(xiàn)[6]使用深度相機(jī)通過(guò)檢測(cè)托盤點(diǎn)云數(shù)據(jù)進(jìn)行輪廓提取與模板匹配來(lái)測(cè)量相對(duì)位姿，達(dá)到定位的效果，且不易受光照影響，但點(diǎn)云信息量較為龐大，不易處理。近年來(lái)，深度學(xué)習(xí)的方法也逐漸應(yīng)用至相對(duì)位姿的測(cè)量中。文獻(xiàn)[7-9]采用深度學(xué)習(xí)方法進(jìn)行托盤檢測(cè)，求取搬運(yùn)小車與目標(biāo)之間的位姿實(shí)現(xiàn)對(duì)接，但深度學(xué)習(xí)方法需要大量的模型訓(xùn)練才能達(dá)到一定精度與效率要求，對(duì)于簡(jiǎn)單場(chǎng)景的視覺(jué)定位，實(shí)現(xiàn)過(guò)程相對(duì)復(fù)雜。

由此，針對(duì)自主搬運(yùn)車與物料托盤之間相對(duì)位置、相對(duì)姿態(tài)解算問(wèn)題，提出精確搬運(yùn)小車的局部定位，以實(shí)現(xiàn)小車與物料托盤之間的精確對(duì)接。利用TOF 相機(jī)采集的灰度和深度信息進(jìn)行托盤關(guān)鍵點(diǎn)提取和匹配，并基于此確定雙方相對(duì)位置和姿態(tài)。首先對(duì)目標(biāo)區(qū)域進(jìn)行分割提取，對(duì)目標(biāo)區(qū)域使用加速分割檢測(cè)特征角點(diǎn)檢測(cè)算法（features from accelerated segment test, FAST）提取其關(guān)鍵角點(diǎn)信息，進(jìn)而基于無(wú)向圖理論，使用馬爾可夫隨機(jī)場(chǎng)與確定性退火算法，使得關(guān)鍵點(diǎn)集與模板點(diǎn)集相匹配，最后對(duì)匹配點(diǎn)進(jìn)行基于奇異值分解（singular value decomposition, SVD）的迭代臨近點(diǎn)（iterative closest point, ICP）位姿估計(jì)，確定搬運(yùn)小車位置與姿態(tài)。

1 相機(jī)成像模型

圖1 相機(jī)成像模型

由相似三角形可得相機(jī)坐標(biāo)系與物理成像坐標(biāo)系中坐標(biāo)關(guān)系為

式中為相機(jī)的焦距，即圖1中相機(jī)坐標(biāo)系原點(diǎn)到物理成像坐標(biāo)系原點(diǎn)的距離。

圖2 物理成像坐標(biāo)系與像素坐標(biāo)系關(guān)系

因此，世界坐標(biāo)系中一點(diǎn)在相機(jī)觀測(cè)坐標(biāo)系中的坐標(biāo)與像素坐標(biāo)系中坐標(biāo)之間的關(guān)系為

2 關(guān)鍵點(diǎn)提取

對(duì)相機(jī)拍攝圖片通過(guò)目標(biāo)灰度閾值分割、邊緣提取定位目標(biāo)邊緣位置，在其邊緣進(jìn)行角點(diǎn)檢測(cè)來(lái)提取所需關(guān)鍵點(diǎn)進(jìn)行位姿估計(jì)與局部定位。

2.1 目標(biāo)區(qū)域分割

圖像邊緣是圖像的基本特征之一，實(shí)驗(yàn)所需關(guān)鍵角點(diǎn)集中在目標(biāo)邊緣。對(duì)目標(biāo)所在區(qū)域進(jìn)行邊緣提取，提取出圖像灰度值變化顯著的部分，減少角點(diǎn)檢測(cè)時(shí)的運(yùn)算壓力，同時(shí)可輔助驗(yàn)證角點(diǎn)提取的完整性與準(zhǔn)確性。邊緣提取首先使用高斯濾波器對(duì)原圖像進(jìn)行降噪平滑處理，減少噪聲對(duì)邊緣定位精度的影響，距離越近權(quán)重越高，對(duì)該像素點(diǎn)的灰度值的貢獻(xiàn)也就越大，從而可以很好地抑制噪聲。常用高斯函數(shù)為式（5），使用高斯函數(shù)對(duì)圖像進(jìn)行卷積得到平滑后的圖像（如式（6）所示）。

2.2 目標(biāo)區(qū)域分割關(guān)鍵角點(diǎn)提取

角點(diǎn)可以保留圖像的重要特征，減少信息數(shù)據(jù)量提高運(yùn)行速度，有利于實(shí)時(shí)匹配。FAST角點(diǎn)檢測(cè)算法是計(jì)算機(jī)視覺(jué)領(lǐng)域的一種經(jīng)典算法，在運(yùn)行效率方面具有巨大優(yōu)勢(shì)，當(dāng)目標(biāo)像素點(diǎn)鄰域范圍內(nèi)有足夠數(shù)量的像素點(diǎn)與之不相似，則認(rèn)為該像素點(diǎn)為角點(diǎn)[10]。圖3為FAST角點(diǎn)檢測(cè)模板。

圖3 FAST角點(diǎn)檢測(cè)模板[10]

如圖4所示，通過(guò)灰度閾值分割進(jìn)行目標(biāo)區(qū)域提??；然后對(duì)目標(biāo)區(qū)域進(jìn)行邊緣檢測(cè)，提取出目標(biāo)物體邊緣，如圖5所示；最后使用FAST角點(diǎn)檢測(cè)提取出所需關(guān)鍵點(diǎn)，如圖6所示。對(duì)所檢測(cè)出的關(guān)鍵點(diǎn)進(jìn)行下文中的關(guān)鍵點(diǎn)匹配。

圖4 灰度閾值分割目標(biāo)區(qū)域提取

圖5 邊緣位置提取

圖6 FAST角點(diǎn)檢測(cè)與篩選

3 關(guān)鍵點(diǎn)匹配

3.1 確定性退火進(jìn)行全局優(yōu)化

3.2 基于SVD的ICP位姿估計(jì)

物體位姿是指相機(jī)當(dāng)前坐標(biāo)系相對(duì)于原始物體所在的世界坐標(biāo)系發(fā)生的平移和旋轉(zhuǎn)變換。原始物體可以在世界坐標(biāo)系任何位置，通常將原始物體的重心和朝向與世界坐標(biāo)系對(duì)齊，即將物體坐標(biāo)系與世界坐標(biāo)系重合，通過(guò)求取世界坐標(biāo)系到相機(jī)坐標(biāo)系的變換求取相對(duì)位姿。

對(duì)于ICP的求解可用奇異值分解方法，SVD是線性代數(shù)中一種矩陣分解方法，可用來(lái)求解ICP中的旋轉(zhuǎn)矩陣和平移矢量進(jìn)行位姿估計(jì)。其方法如下：

4 實(shí)驗(yàn)與結(jié)果分析

4.1 關(guān)鍵點(diǎn)匹配對(duì)比實(shí)驗(yàn)

為了驗(yàn)證本文中關(guān)鍵點(diǎn)匹配算法的準(zhǔn)確性與時(shí)效性，實(shí)驗(yàn)中設(shè)定相機(jī)高度為0.5 m，在相機(jī)與托盤前平面相距1.5～4 m、相機(jī)旋轉(zhuǎn)角度為–30°～30°范圍內(nèi)，調(diào)整相機(jī)位姿并記錄當(dāng)前幀的灰度數(shù)據(jù)與深度數(shù)據(jù)，共測(cè)得100組數(shù)據(jù)。對(duì)所測(cè)數(shù)據(jù)提取關(guān)鍵點(diǎn)后采用確定性退火算法、ICP配準(zhǔn)算法、相干點(diǎn)漂移算法（coherent point drift, CPD）進(jìn)行點(diǎn)集匹配，測(cè)得3種方法匹配精度與平均匹配時(shí)間（如表1所示）。

表1 配準(zhǔn)方法性能比較

ICP算法對(duì)待配對(duì)點(diǎn)集的初始位置比較敏感，且容易被外點(diǎn)影響導(dǎo)致點(diǎn)配準(zhǔn)失敗。CPD算法實(shí)現(xiàn)復(fù)雜度高，匹配所需時(shí)間長(zhǎng)，不利于實(shí)時(shí)位姿解算。本文采用基于馬爾可夫的確定性退火算法最終得到的匹配點(diǎn)對(duì)是一一映射的，具有一定的抗干擾能力，可以有效地去除外點(diǎn)影響，匹配準(zhǔn)確率較高，且在匹配時(shí)間上也具有一定優(yōu)勢(shì)。

4.2 相對(duì)位姿解算實(shí)驗(yàn)

為驗(yàn)證位姿解算精度，實(shí)驗(yàn)采用軟件接口openGL創(chuàng)建托盤模型，根據(jù)相機(jī)內(nèi)參設(shè)定虛擬相機(jī)的投影矩陣模擬真實(shí)相機(jī)，使用openGL中的緩沖模擬深度相機(jī)深度數(shù)據(jù)。實(shí)驗(yàn)中固定托盤位置與朝向不變，托盤前平面底邊中心與世界坐標(biāo)系原點(diǎn)重合。調(diào)整相機(jī)在世界坐標(biāo)系中位置與朝向，記錄灰度數(shù)據(jù)與深度數(shù)據(jù)，進(jìn)而根據(jù)上述原理解算相機(jī)位姿，計(jì)算測(cè)量誤差。

世界坐標(biāo)系使用右手坐標(biāo)系，水平方向?yàn)檩S，垂直地面方向?yàn)檩S。托盤位置保持不變?？紤]實(shí)際應(yīng)用情況，相機(jī)高度固定為0.5 m，即相機(jī)軸坐標(biāo)固定為0.5 m，相機(jī)從距托盤1.4 m位置向遠(yuǎn)離托盤方向移動(dòng)，即從世界坐標(biāo)系中(0，0.5，-1.5)位置平行于軸向負(fù)方向移動(dòng)，每移動(dòng)0.2 m記錄相機(jī)一幀數(shù)據(jù)。使用文中算法解算平移矢量，測(cè)得軸方向平移誤差如圖7所示。

圖7 z軸平移誤差分析

測(cè)量水平方向位移誤差時(shí)，相機(jī)距托盤前平面垂直距離為3 m，沿水平方向移動(dòng)，即相機(jī)從世界坐標(biāo)系中(0，0.5，3)位置沿平行于軸方向移動(dòng)?？紤]到相機(jī)視野問(wèn)題，只測(cè)量位移1 m之內(nèi)數(shù)據(jù)。測(cè)得水平方向平移誤差如圖8所示。

從平移實(shí)驗(yàn)中可以看出，軸平均位移誤差為12 mm，測(cè)量誤差隨距離增加也有增大趨勢(shì)，但最大誤差不超過(guò)15 mm，在測(cè)試范圍內(nèi)能滿足實(shí)驗(yàn)要求。

圖8 x軸平移誤差分析

圖9 旋轉(zhuǎn)角測(cè)量示意圖

如圖10所示，由旋轉(zhuǎn)角度誤差圖可看出，旋轉(zhuǎn)角度誤差控制在2°之內(nèi)，滿足實(shí)際檢測(cè)需要。

圖10 旋轉(zhuǎn)角測(cè)量分析

5 結(jié)束語(yǔ)

本文采用TOF相機(jī)對(duì)搬運(yùn)叉車和物料托盤之間的相對(duì)位置和相對(duì)姿態(tài)進(jìn)行測(cè)量，實(shí)現(xiàn)自主搬運(yùn)叉車無(wú)人作業(yè)需求。根據(jù)物料托盤灰度圖像和深度圖像，進(jìn)行關(guān)鍵點(diǎn)提取與匹配，利用基于SVD的ICP算法解算搬運(yùn)車和托盤之間的相對(duì)位置和相對(duì)姿態(tài)。仿真實(shí)驗(yàn)表明，該算法能夠滿足自主搬運(yùn)叉車/物料托盤相對(duì)位姿測(cè)量精度和實(shí)時(shí)性的要求，并具有一定的魯棒性。

[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] 吳平, 劉剛軍, 牛志朝, 等. 室內(nèi)AGV導(dǎo)引和定位系統(tǒng)設(shè)計(jì)[J]. 計(jì)算機(jī)工程與設(shè)計(jì), 2019, 40(6): 1782-1787.

[3] 張浩悅, 程曉琦, 劉暢, 等. 基于全局稀疏地圖的AGV視覺(jué)定位技術(shù)[J]. 北京航空航天大學(xué)學(xué)報(bào), 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] 李照, 舒志兵. 一種改進(jìn)二維碼視覺(jué)精定位AGV技術(shù)研究[J]. 控制工程, 2019, 26(6): 1049-1054.

[6] 武文漢, 楊明, 王冰, 等. 一種基于輪廓匹配的倉(cāng)儲(chǔ)機(jī)器人托盤檢測(cè)方法[J]. 上海交通大學(xué)學(xué)報(bào), 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網(wǎng)絡(luò)的倉(cāng)儲(chǔ)托盤檢測(cè)方法[J]. 現(xiàn)代信息科技, 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] 曲寒冰, 王加強(qiáng), 李彬, 等. 基于概率圖模型的點(diǎn)集匹配方法研究[J]. 自動(dòng)化學(xué)報(bào), 2015, 41(4): 694-710.

[12] 張祥德, 唐青松. 確定性退火技術(shù)及其在點(diǎn)匹配中的應(yīng)用[J]. 東北大學(xué)學(xué)報(bào), 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ī)自主導(dǎo)引叉車/托盤相對(duì)位姿測(cè)[J]. 導(dǎo)航定位學(xué)報(bào), 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

國(guó)家自然科學(xué)基金項(xiàng)目（61773330）；國(guó)家重點(diǎn)研發(fā)計(jì)劃項(xiàng)目（2020YFA0713501）；湖南省自然科學(xué)基金項(xiàng)目（2021JJ50126）；湖南省教育廳重點(diǎn)項(xiàng)目（21A0083）。

李鵬（1978—），男，山東青島人，博士，教授，研究方向?yàn)槭覂?nèi)外導(dǎo)航定位。

王鑫鑫（1994—），男，河南開封人，碩士研究生，研究方向?yàn)橐曈X(jué)定位。