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

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

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

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

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

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

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

0 引言

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

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

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

1 相機(jī)成像模型

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

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

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

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

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

2 關(guān)鍵點提取

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

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

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

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

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

圖3 FAST角點檢測模板[10]

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

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

圖5 邊緣位置提取

圖6 FAST角點檢測與篩選

3 關(guān)鍵點匹配

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

3.2 基于SVD的ICP位姿估計

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

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

4 實驗與結(jié)果分析

4.1 關(guān)鍵點匹配對比實驗

為了驗證本文中關(guān)鍵點匹配算法的準(zhǔ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ù)，共測得100組數(shù)據(jù)。對所測數(shù)據(jù)提取關(guān)鍵點后采用確定性退火算法、ICP配準(zhǔn)算法、相干點漂移算法（coherent point drift, CPD）進(jìn)行點集匹配，測得3種方法匹配精度與平均匹配時間（如表1所示）。

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

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

4.2 相對位姿解算實驗

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

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

圖7 z軸平移誤差分析

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

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

圖8 x軸平移誤差分析

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

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

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

5 結(jié)束語

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

[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ī)工程與設(shè)計, 2019, 40(6): 1782-1787.

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

[6] 武文漢, 楊明, 王冰, 等. 一種基于輪廓匹配的倉儲機(jī)器人托盤檢測方法[J]. 上海交通大學(xué)學(xué)報, 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ò)的倉儲托盤檢測方法[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), 李彬, 等. 基于概率圖模型的點集匹配方法研究[J]. 自動化學(xué)報, 2015, 41(4): 694-710.

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

國家自然科學(xué)基金項目（61773330）；國家重點研發(fā)計劃項目（2020YFA0713501）；湖南省自然科學(xué)基金項目（2021JJ50126）；湖南省教育廳重點項目（21A0083）。

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

王鑫鑫（1994—），男，河南開封人，碩士研究生，研究方向為視覺定位。