陳 策，岳繼光，董延超，沈潤杰

(同濟大學電子與信息工程學院，上海 201804)

0 引言

目標位姿追蹤作為工業(yè)零部件無約束抓取等應用的關鍵技術，最初通過確定物體表面至少4 個特征點的圖像坐標以解算目標位姿[1-3]。2008 年Choi 等人使用SIFT 特征點匹配定位方法估計物體位姿[4]，平移誤差在5 cm 以內(nèi)，姿態(tài)角度誤差在6°以內(nèi)。之后Youngmin 等人使用關鍵幀的特征點提取與匹配方法實現(xiàn)了多目標的位姿追蹤[5]，該算法運行效率高。2014 年Karl 等人使用SIFT 特征點檢測目標并實現(xiàn)分割，之后利用深度相機提取的深度圖信息匹配物體模型[6]，實現(xiàn)了多目標的檢測與位姿追蹤，實驗中對機械臂抓取場景進行了實際測試，算法每幀耗時30 ms，平移誤差在3 cm 以內(nèi)，姿態(tài)角度誤差在4°以內(nèi)。至此，基于特征點的追蹤方法已在實時性以及精度上滿足一定要求。

基于圖像邊緣或輪廓的匹配方法，最早于1995 年就被提出，該方法通過尋找模型邊緣與圖像邊緣的匹配關系，將物體模型對齊于場景的邊緣圖像，以解算目標相對相機的位置與姿態(tài)，實現(xiàn)位姿追蹤[7]。2015 年Wang 等人研究邊緣匹配的全局優(yōu)化方法[8]，使用圖模型進行全局尋優(yōu)，該方法在復雜背景情況中也取得了不錯的效果，位置誤差在2 cm 以內(nèi)，姿態(tài)誤差在4°以內(nèi)。基于隨機森林的空間點匹配方法[9-11]，能夠檢測物體并實現(xiàn)追蹤，但算法耗時較長且誤差較大；基于深度學習的RGB-D 位姿判別算法使用深度神經(jīng)網(wǎng)絡提取姿態(tài)描述子，之后回歸出物體位置和姿態(tài)[12-14]，該類方法仍然存在算法耗時過長，前期成本較高，精度不理想等缺陷。

所提算法主要解決復雜背景[15,16]下的貧紋理物體位姿追蹤問題?；诂F(xiàn)有算法，改進場景灰度圖像[17]與物體三維模型的尋優(yōu)過程，提高配準的精度以及魯棒性。通過模型邊緣方向與場景梯度方向構造自適應遮擋權重，有效降低誤匹配點對整體尋優(yōu)的影響，提高追蹤算法在復雜情況下的追蹤精度。

1 目標追蹤算法

本文利用物體三維CAD 模型、目標初始位姿，將模型邊緣點投影于圖像平面，利用方向倒角匹配構建優(yōu)化函數(shù)，尋找模型邊緣點與場景邊緣圖像的最優(yōu)匹配，獲得當前幀中目標物體的精確位姿。該方法具有追蹤的連續(xù)性，相比使用檢測的追蹤方法實時性和連續(xù)性更高，不易產(chǎn)生突變。所提方案的基本流程如圖1 所示，主要分為三個部分：邊緣圖像方向倒角距離（DCM）計算；模型邊緣點投影并構建目標函數(shù)；以及最后的解析尋優(yōu)過程。

圖1 追蹤算法流程圖 Fig.1 Tracking algorithm flowchart

1.1 方向倒角距離匹配

為實現(xiàn)物體CAD 模型邊緣與圖像邊緣的對齊，常用的方法是倒角距離匹配（Chamfer Matching，簡稱CM）。假設M={mi}表示物體模型中的邊緣經(jīng)過離散采樣光柵化后映射于圖像坐標系中的點，其中 i=1,2,… , | M |；N ={nj}代表場景邊緣圖像中的邊緣點，其中j=1,2,…,| N|。為實現(xiàn)物體模型與邊緣圖像的匹配，構造倒角距離誤差函數(shù)如式（1）所示：

其中w 代表物體模型中的邊緣點數(shù)量，w=| M|。倒角匹配距離為二維圖像距離，使用該方法進行匹配魯棒性較差，易受背景影響。

Tuzel 等人[18]基于倒角距離匹配方案提出了方向倒角匹配（Directional Chamfer Matching，簡稱DCM）方法。在原有圖像距離基礎上，增加方向匹配信息，使匹配過程不僅考慮二維圖像距離，而且加入方向誤差。該方法提高了模型與圖像邊緣的匹配精度。假設 φ(?)代表二維圖像中的方向運算符，即 φ(mi)代表模型點 mi在相機成像平面中對應的模型邊緣方向，φ (nj)代表邊緣圖中的點 nj對應的邊緣方向，構造方向倒角距離誤差函數(shù)如下：

其中λ 代表方向誤差權重?；贒CM 的匹配方法正確率更高，能夠有效降低匹配誤差，并且對于遮擋以及復雜背景情況魯棒性更強，本文將使用DCM 構造目標函數(shù)，尋找模型邊緣與圖像邊緣的最優(yōu)匹配關系，實現(xiàn)物體的位姿追蹤。

1.2 目標函數(shù)的構建

基于DCM 構建目標函數(shù)需要計算物體邊緣點在相機成像平面的圖像坐標；并通過場景邊緣圖像計算對應DCM 張量。進而得到所有模型邊緣光柵點對應的優(yōu)化殘差，以得到整體尋優(yōu)的目標函數(shù)。首先通過物體三維CAD 模型以及目標初始位姿，利用計算機提取物體深度邊緣。并將邊緣直線光柵化為三維離散點集P ={o1, … , om}∈?3，離散邊緣點位于物體模型邊緣之上，離散邊緣點的數(shù)量直接影響匹配的精度以及算法的效率。通過試驗，對于本文所用的目標模型，光柵點間距選擇在1-2 mm。繼而提取點={, …,}∈?3，使得：

其次是場景邊緣圖像提取。本文使用LSD 算法提取場景邊緣，該算法的目標在于檢測圖像中局部直線輪廓，并將輪廓構成直線段；相比其他邊緣提取算法，能夠有效抑制長度較短的邊緣提取誤差，如圖2 所示，LSD 邊緣提取算法更加適合于DCM 三維張量的計算。

圖2 邊緣提取圖 Fig.2 Edge extraction

為加速DCM 張量的方向誤差計算，在場景邊緣圖像中，按一定角度間隔ε 對邊緣方向進行離散化，使得各角度范圍內(nèi)的邊緣單獨成圖，如圖3(a)所示。ε 的取值將對邊緣匹配精度以及算法運行時間造成影響。通過測試，權衡精度與耗時的關系，選定ε 為3°，即將邊緣圖像分為60 個離散的方向。為計算DCM 張量，首先根據(jù)各方向?qū)倪吘増D像計算倒角匹配距離，尋找所有光柵點與當前離散角度邊緣中的最近邊緣點距離。該距離即是倒角匹配距離DTV{φ?i}，其中φ?i, i = {1,…,60}代表離散的邊緣方向，V代表當前場景圖像。計算結果如圖3(b)所示，其中像素點的灰度代表當前像素點距離邊緣的距離，灰度值越高，距離越遠。通過各角度對應的倒角匹配距離計算方向倒角距離DT3V{φ?i}，結果如圖3(c)所示。方向倒角匹配張量為三維距離張量，代表不同離散角度中，任一像素點的圖像距離與角度距離之和最小的邊緣距離。根據(jù)雙向動態(tài)規(guī)劃算法，首先將所有角度的方向倒角距離初始化為圖像二維距離，根據(jù)前向遞推以及后向遞推以計算各點對應的距離最小值：

其中λ 代表方向誤差權重，該方法計算速度快，能夠在最多1.5 次前向與后向循環(huán)之后得到各角度邊緣圖像中的所有像素點對應的DT3V{φ?i}，時間復雜度控制在 Ο ( q)以內(nèi)，其中q 代表圖像的像素個數(shù)。

完成模型邊緣光柵點的提取、映射以及場景圖像DCM 距離計算之后，通過光柵點的圖像坐標ix 以及該模型邊緣點的方向構造方向倒角匹配殘差函數(shù)：

圖3 離散方向殘差示意圖 Fig.3 Discrete direction residuals

其中n 代表模型邊緣點的數(shù)量， φ?(xi)代表對應邊緣點在二維圖像中的邊緣切向方向，該方向即點集P 與P 映射到圖像坐標系中的位置連線方向。通過構造后的殘差函數(shù)作為優(yōu)化的目標函數(shù)，將匹配問題轉(zhuǎn)化為尋優(yōu)問題，尋找使得所有邊緣光柵點匹配殘差之和最小的相對位姿。設立物體相對于相機的旋轉(zhuǎn)向量R =[ rxryrz]T以及平移向量T =[ txtytz]T，由此根據(jù)殘差函數(shù)得到優(yōu)化的目標函數(shù)：

其中， oi代表三維模型邊緣光柵點， π(?)代表相機投影模型，g (T, R )∈?6代表物體相對于相機的三維空間位姿變換關系。通過優(yōu)化目標函數(shù) E (T, R )以獲得當前幀中物體相對于相機的精確位姿變換關系，并將該幀位姿作為下一幀圖像的初始位姿，以實現(xiàn)姿態(tài)追蹤。

1.3 解析尋優(yōu)過程

待優(yōu)化目標函數(shù)由模型邊緣光柵點的方向倒角距離(DT3V)構成，而優(yōu)化變量為目標相對于相機的位姿變換關系，即g (T, R )∈?6。優(yōu)化目標函數(shù)，關鍵在于尋找DT3V相對于待優(yōu)化變量的導數(shù)，Imperoli等人[19]通過直接計算數(shù)值導數(shù)的方法進行尋優(yōu)，該方法在優(yōu)化變量上進行微小攝動以計算 ΔD T3V，以獲取對應優(yōu)化變量相對于目標函數(shù)的數(shù)值導數(shù)，該方法在設定合適步長的情況下能夠使得目標函數(shù)降低到最小值，但是在初始位姿與實際位姿相差較大或者背景較為復雜的情況下容易匹配失敗，陷入局部最小值。為提高匹配精度以及系統(tǒng)魯棒性，本文將利用解析方法，計算導數(shù)以提高匹配的魯棒性與精度。

目標函數(shù) E (T, R )是由場景DT3V張量以及模型映射到圖像中的二維圖像坐標點 xi(ui, vi)及該點邊緣方向φ?(xi)確定，因此尋找位姿變換關系 g (T, R )相對于二維圖像坐標(ui, vi)的雅克比矩陣能夠解析地表示物體相對相機的位姿與物體在圖像坐標系中映射點的坐標變化關系。當場景DT3V張量確定之后，模型映射點的坐標變換將直接影響目標函數(shù)，因此通過解耦位姿變換與模型映射點的圖像坐標關系，能夠優(yōu)化尋優(yōu)過程，提高配準精度。

假設目標坐標系為 Cobj，相機坐標系 Ccam， Cobj相對于 Ccam的一次變換為T（對應李代數(shù)為ξ ，對應旋轉(zhuǎn)矩陣R 以及平移向量t），現(xiàn)存在目標坐標系 Cobj上一點p，其位于 Ccam中的位置為Tp。給定T 左乘一個擾動Δ T=exp(δξ∧)，∧算符代表求取向量的反對稱矩陣，并設定擾動項的李代數(shù)為δξ =[δ ρ ,δ φ ]T，可得：

根據(jù)相機投影模型，對于點p′：

使用第三行消去點p′距離相機坐標系的距離s，得：

由此計算偏導數(shù)可得：

根據(jù)式（11）以及（14）可得到物體上一點投影到圖像平面內(nèi)的坐標相對于坐標系變換關系的雅克比矩陣：

使用該雅克比矩陣替代數(shù)值導數(shù)優(yōu)化方法，經(jīng)過測試，在復雜背景情況下，算法的匹配精度以及魯棒性均有提升。

2 自適應權重優(yōu)化方法

為提高追蹤算法對復雜背景的魯棒性，本文提出自適應權重優(yōu)化算法，動態(tài)改變光柵點優(yōu)化權重，降低擾動點的優(yōu)化占比，提高追蹤的精確度。

在完成當前幀邊緣圖像與物體三維模型的匹配尋優(yōu)后，獲得了當前時刻物體相對于相機的位姿變換關系 g (T, R )，通過該位姿變換關系能夠?qū)⒛Ｐ瓦吘壒鈻劈c集P ={o1, … , om}∈?3及用于求取模型邊緣方向的臨近點集={,… ,}∈?3映射為圖像坐標系中的二維模型邊緣點集X={ x1,…, xm} ={π ( oi, g), i=0,…, m}∈?2以及={, …,} ={π (, g ), i=0,…, m}∈?2。計算邊緣光柵映射點以及對應臨近點的連線方向，即為相應模型邊緣在圖像中的方向Dmodel(xi)。在完成模型邊緣點方向計算后，對場景圖像的梯度方向進行提取。對于圖像函數(shù) f ( x,y )，其在點( x , y )處的梯度是一個具有大小和方向的矢量，設Gx和 Gy分別代表x 和y 方向的梯度，該梯度的矢量可以表示為：

本文使用Sobel 算子對梯度進行提取，該算子使用梯度矩陣與圖像矩陣作平面卷積，如式（17,18）所示，以計算圖像灰度函數(shù)的梯度近似值，所需運算資源相對較少，并且能有效降低僅對單個像素點進行梯度計算的誤差。

其中A 表示原始圖像矩陣中以計算點為中心的3*3 圖像矩陣，由此即可獲得圖像中所有像素的梯度大小，以及方向

根據(jù)模型光柵點的圖像坐標 xi( ui, vi)獲得場景圖像中對應該點的梯度大小Mg( xi)以及方向 Dg( xi)，進而結合該點對應的模型邊緣方向Dm(xi)，計算該點的優(yōu)化權重。首先對梯度大小進行篩選，若對應點所在圖像梯度過小，證明該點所處圖像位置邊緣信息不明顯，導致該問題的原因可能是：（1）該處邊緣由于光照等外部原因，在圖像中顯示不明顯，邊緣提取算法未能成功提??；（2）該處邊緣被遮擋。在這兩種情況下，使用該光柵點的殘差作位姿優(yōu)化都會引入誤差，因盡量降低該點對下一幀位姿優(yōu)化的權值，防止誤匹配點對物體位姿追蹤的影響。其次，當梯度大小大于設定閾值后，證明該匹配點已匹配到場景圖像的邊緣，但模型邊緣點與場景邊緣的匹配正確與否以及匹配的精度需要使用模型邊緣的方向以及場景梯度方向進一步確定。通過計算邊緣方向與梯度方向之差的余弦值作為該光柵點的優(yōu)化權值，使得兩者方向垂直情況下的權重更高，由此可得：

將優(yōu)化權重加入目標優(yōu)化函數(shù)，得：

將圖像坐標點 xi轉(zhuǎn)換為三維空間點 oi：

使用該方法不僅能夠降低圖像噪音以及邊緣提取誤差對系統(tǒng)帶來的擾動，而且在復雜背景中也能夠及時降低干擾點對于整體尋優(yōu)的影響，保證系統(tǒng)的魯棒性。

3 實驗驗證

本文所提算法系統(tǒng)使用C++構建，上位機配置為Intel i7 CPU、8G RAM，算法平均每幀優(yōu)化配準過程耗時57 ms。由于位姿數(shù)據(jù)庫的建立成本過高，并且精確度難以保證，因此本文使用計算機圖形學（簡稱：CG）渲染視頻作追蹤精度測試，并由渲染軟件得到目標相對相機的位姿真值[20]，CG 渲染視頻追蹤效果如圖4 所示。

根據(jù)軟件生成真值用于精度測試，本文將相機放置據(jù)物體不同的距離，以測試物體在圖像平面內(nèi)不同尺度的追蹤精度，如表1 所示，其中S 表示物體在圖像中的平均尺度大小，單位為pix，R 表示平均角度偏差，T 表示平均平移向量偏差：

圖4 CG 渲染視頻追蹤 Fig.4 CG render video tracking

表1 CG 渲染視頻追蹤精度表 Tab.1 CG rendering video tracking accuracy table

由實驗可知，當物體平均尺度在200～300 像素時的追蹤精度明顯優(yōu)于100 像素左右的情況，主要原因為物體尺度過小后邊緣匹配點減少，并且邊緣提取精度也較低，容易受到擾動的影響。

真實場景測試視頻由單目灰度相機提取，分辨率為1280*1024，真實場景測試主要考察算法對背景復雜、環(huán)境光照變化以及噪音較大時的表現(xiàn)，追蹤效果如圖6 所示。進而使用公開數(shù)據(jù)庫Rigid Pose[21]對算法精度進行測試，追蹤效果如圖5 所示。將追蹤結果與數(shù)據(jù)庫真值進行對比，如圖7 所示為每一幀圖像中目標相對于相機的位姿關系，子圖a-f 分別對應相機坐標系中x，y，z 軸的旋轉(zhuǎn)以及平移關系，其中黃色線代表數(shù)據(jù)庫真值，藍色線代表追蹤算法實際追蹤結果。本文對比了Pauwels[22]等人提出的追蹤算法，并測試了相同數(shù)據(jù)集上的追蹤精度。如表2 所示，本文所提方法的角度追蹤平均誤差為1.835°，平移向量平均誤差為0.989 mm，相比Pauwels 所提的稀疏點追蹤方法，優(yōu)勢明顯；而與深度稠密追蹤方法相比，僅在平移向量誤差上稍大，但本文算法僅使用灰度圖像，在算法復雜度與計算耗時上有較大優(yōu)勢。根據(jù)測試，本文所提方法在CG 渲染視頻、真實灰度相機拍攝視頻以及公開數(shù)據(jù)集上的追蹤效果良好，平均角度誤差控制在2°以內(nèi)，平均平移向量誤差在1 mm 以內(nèi)，并且隨視頻質(zhì)量的提高，精度仍有一定程度提高。

圖5 Rigid Pose 追蹤結果圖 Fig.5 Rigid Pose tracking results

圖6 真實視頻追蹤 Fig.6 Real video tracking

圖7 Rigid Pose 數(shù)據(jù)集追蹤精度 Fig.7 Rigid Pose dataset tracking accuracy

表2 追蹤算法平均誤差 Tab.2 Average error of tracking algorithm

4 結論

本文給出了基于物體三維CAD 模型的追蹤方法，構建方向倒角殘差函數(shù)，將追蹤問題轉(zhuǎn)換為尋優(yōu)問題，進而尋找模型與場景邊緣圖像的最優(yōu)匹配，以解算目標物體相對相機的位姿關系。算法精度高、魯棒性強，標準測試數(shù)據(jù)庫實驗表明角度誤差在2°以內(nèi)，平移向量誤差在1 mm 以內(nèi)，均已達到現(xiàn)有算法較高水平；提出基于自適應權重優(yōu)化的解決方案，該方法不僅能夠提高物體在復雜背景情況下的追蹤精度，還有效降低了系統(tǒng)噪音對追蹤效果的影響，進一步提高算法的適用性。并且該算法計算耗時較低，便于移植到移動設備平臺，對于工業(yè)視覺的應用具有重要應用價值。