,, ,

(浙江工業(yè)大學 信息工程學院，浙江 杭州 310023)

基于視覺傳感器的移動目標位姿估計在視覺伺服[1]、移動目標跟蹤[2]和視覺定位[3]等領域有著廣泛的應用。目前, 大多數(shù)研究都是基于相機(視覺傳感器)內參已知的前提下, 通過相機獲取圖像并加以處理, 得到圖像中的2D特征信息, 利用2D-3D坐標系間的對應關系來設計位姿估計方法以實現(xiàn)對目標位姿的估計。基于單個視覺傳感器的目標位姿估計方法按所提取圖像特征的不同主要分為以下幾類：1) 基于點特征的位姿估計方法; 2) 基于線特征的位姿估計方法; 3) 基于輪廓的位姿估計方法。其中, 第一類方法是利用移動目標上的一系列已知的特征標記, 通過基于點特征的位姿估計方法求解目標的位姿[4-6]。第二類方法通常都是由第一類方法演變而來, 尤其是針對某些線特征易于提取的目標, 通過建立所提取的線特征與3D 空間中目標線段之間的對應關系, 采用相應的位姿估計算法估計目標位姿[7-8]。第三類是基于目標的3D模型已知的情況下, 利用2D圖像提取的目標輪廓構建與3D 模型輪廓的對應關系, 并采取某些優(yōu)化方法不斷減小估計誤差[9]。在上述的三類方法中, 第一類方法適用性最廣, 究其原因有二： 其一, 相對于第二和第三類方法, 第1)類方法可更直接更方便地建立2D-3D 對應關系; 其二, 便于人為地創(chuàng)造一些mark點以加速點特征的提取。在現(xiàn)有的研究中, 大多數(shù)研究都是基于點特征的, 并存在一些相對成熟的點特征位姿估計方法, 例如OI正交迭代法[10]、Epnp算法[11]及自適應擴展卡爾曼方法(AEKF)[13]與迭代自適應擴展卡爾曼方法(IAEKF)[12]。在上述的四種方法中, 前兩者的估計精度過分依賴觀測噪聲, 當噪聲過大時, 它們的估計精度便會急劇下降, 在這種情況下, AEKF 方法[13]被提出, 它通過估計每一個采樣步長中的過程噪聲和觀測噪聲來克服環(huán)境中噪聲過大或者環(huán)境發(fā)生異變時產生的影響, 增加了系統(tǒng)的穩(wěn)定性, 相對于AEKF, IAEKF將AEKF 的結果進行迭代, 使其估計方差最小以達到提高精度的目的, 但付出的計算量卻遠超AEKF。在某些極端狀況下, 如相機標定誤差過大、初始值誤差過大, 基于單個視覺傳感器的估計方法所得到的估計結果誤差過大甚至發(fā)散?；诙鄠€視覺傳感器的集中式融合估計方法[14], 結合多個視覺傳感器提供的圖像信息, 采用AEKF或者IAEKF估計目標的位姿, 在一定程度上彌補了單視覺傳感器方法的不足, 提高了系統(tǒng)的估計精度, 但隨著觀測信息的增加, 觀測矩陣維數(shù)上升, 集中式融合方法消耗的計算資源成平方增長,導致系統(tǒng)的響應速度大大降低。

為此, 筆者提出了一種應用于多視覺位姿跟蹤系統(tǒng)的分布式融合方法, 以此來提高系統(tǒng)的運行效率。分布式融合方法[15]是對各個局部估計結果按照一定的方法進行加權得到全局最優(yōu)值, 避免了觀測矩陣擴維, 而且可采用多線程編程方式同時計算多個局部估計結果, 大大降低了運算時間。主要內容描述如下：首先, 根據(jù)實驗環(huán)境建立觀測模型與剛體運動學模型; 其次, 以AEKF為局部濾波器, 提出了應用于多視覺傳感器位姿估計系統(tǒng)的分布式融合方法; 最后, 通過對比實驗, 以精度和系統(tǒng)的響應時間為指標, 驗證基于AEKF的分布式融合方法應用于目標位姿估計問題的有效性和優(yōu)越性。

1 平臺簡介

如圖1所示, 本實驗平臺包括一臺PI公司生產的6 足位移臺和兩個固焦相機(型號為今貴 S9)。

圖1 實驗平臺Fig.1 Experimental platform

兩個相機分別固定在平臺一旁, 被跟蹤目標固定在6 足位移臺上部, 隨6 足位移臺而運動, 6 足位移臺實時反饋狀態(tài)信息(位置和姿態(tài)), 如圖2所示, 在6 足位移臺初始化狀態(tài)下, 6 足位移臺坐標系Ow′-Xw′Yw′Zw′與世界坐標系Ow-XwYwZw重合, 6 足平臺實時反饋的位姿信息即是6 足位移臺坐標系原點在世界坐標系的位姿信息, 6 足位移臺反饋位姿精度可達微米級，所以可將該反饋位姿信息作為真實位姿。

圖2 世界坐標系與位移臺坐標系Fig.2 World coordinate system & 6 axis hexapodcoordinate system

2 系統(tǒng)模型

為了將所提出方法的估計結果與真實結果進行對比, 設定系統(tǒng)的狀態(tài)變量為基于世界坐標系下的6 足位移臺坐標系原點的位置、姿態(tài)及對應的速度、加速度, 系統(tǒng)的觀測是二維像素坐標系中特征點的像素坐標, 它與狀態(tài)變量存在著一定的約束關系。因此, 在下面的描述中, 首先介紹了像素坐標系與世界坐標系之間的轉換關系,并以此建立系統(tǒng)的觀測模型; 其次, 利用加速度模型描述了移動目標的運動情況。

2.1 觀測模型的建立

如圖3所示, 本系統(tǒng)涉及的坐標系包括：像素坐標系Opix-upixvpix, 圖像坐標系Op-xpyp, 相機坐標系Oc-XcYcZc, 物體坐標系Oob-XobYobZob,6 足位移臺坐標系Ow′-Xw′Yw′Zw′及世界坐標系Ow-XwYwZw。

圖3 6 個坐標系之間的關系示意圖Fig.3 The relationship between the six coordinate systems

像素坐標系與圖像坐標系之間的關系為

(1)

圖像坐標系與相機坐標系之間的關系為

(2)

式中f為相機的焦距。

相機坐標系與物體坐標系之間的關系為

(3)

式中supTsub為sup坐標系與sub坐標系之間的轉換矩陣, 由3×3的旋轉矩陣supRsub與3×1的平移矩陣supTsub組成, 且supTsub= (subTsup)-1。結合上述坐標系之間的轉換關系, 可以得到像素坐標系與物體坐標系之間的轉換為

(4)

綜上, 系統(tǒng)的觀測方程可表示為

z(k)=h(x(k))+r(k)

(5)

2.2 跟蹤目標的加速度運動模型

記k時刻運動目標狀態(tài)為x(k), 其具體形式為

x(k)=Fx(k-1)+q(k-1)

(6)

式中：δt為采樣時間；I3×3為3×3的單位矩陣；q(k-1)為過程噪聲,服從均值為零、協(xié)方差為Q(k-1)的高斯分布。

3 基于多視覺傳感器的分布式融合方法

3.1 AEKF局部估計器

EKF方法通過對非線性函數(shù)的泰勒展開式進行一階線性化截斷, 來處理非線性問題。為了進一步提高估計精度, 提出了基于自適應方法的擴展卡爾曼濾波器，自適應方法包括基于相關性方法(Correlation based algorithms)、極大似然方法(Maximum likelihood algorithms)和方差匹配方法(Covariance matching algorithms)[12]等等， 其中方差匹配方法較其他兩種方法具有更高的計算效率?；诜讲钇ヅ涞臄U展卡爾曼濾波方法可描述如下：

首先, 根據(jù)系統(tǒng)模型計算狀態(tài)的預測值和觀測的預測值分別為

(7)

(8)

P(k|k-1)=FP(k-1|k-1)FT+Q(k-1)

(9)

式中P(k-1|k-1)為k- 1時刻的狀態(tài)誤差協(xié)方差矩陣。由于系統(tǒng)的觀測方程是非線性的, 對觀測方程求關于狀態(tài)預測值的一階偏導作為觀測矩陣,即

(10)

為了計算觀測噪聲的協(xié)方差, 根據(jù)觀測殘差向量

(11)

并通過前N個時刻可計算當前時刻的觀測噪聲協(xié)方差方差矩陣為

(12)

再根據(jù)上述計算得到的狀態(tài)預測誤差協(xié)方差矩陣、觀測矩陣及觀測噪聲方差矩陣計算當前時刻的卡爾曼增益, 進而計算狀態(tài)的估計值并更新狀態(tài)誤差協(xié)方差矩陣為

(13)

(14)

P(k|k)=P(k|k-1)-K(k)H(k)P(k|k-1)

(15)

最后，更新過程噪聲協(xié)方差矩陣, 計算方法與求取觀測誤差方差矩陣類似，即

(16)

(17)

3.2 分布式融合算法

在卡爾曼濾波器中, 計算濾波增益時, 涉及新息矩陣的求逆, 若采用觀測擴維的方式, 將很大程度上增加系統(tǒng)的計算量。而分布式融合算法是對各個局部估計結果按照一定的方法進行加權得到全局最優(yōu)值, 避免了觀測擴維, 從而達到減小計算負擔的目的。下列所述的分布式融合算法已被證明等價于集中式算法[16]，描述如下：

融合中心根據(jù)先驗信息計算k時刻狀態(tài)預測值及其對應的誤差協(xié)方差分別為

(18)

P(d)(k|k-1)=FP(d)(k-1|k-1)FT+Q(d)

(19)

式中：x(d)(k-1|k-1)，P(d)(k-1|k-1)分別為k-1時刻分布式融合狀態(tài)的估計值和誤差協(xié)方差矩陣, 然后結合各個局部濾波器的狀態(tài)和誤差協(xié)方差的預測值與估計值, 更新融合中心的誤差協(xié)方差矩陣及融合后的狀態(tài)估計值分別為

(20)

(21)

3.3 基于多視覺傳感器的分布式融合方法

結合3.1，3.2所述,基于多視覺傳感器的分布式融合方法可描述如下：

4 實驗及分析

物體坐標系與6 足位移臺坐標系之間的轉換矩陣為

在被跟蹤目標上標記8 個特征點以便實時提取, 8 個特征點在物體坐標系下的坐標分別為(-35,-80,0),(35,-80,0),(35,-10,0),(-35,-10,0),(-20,-65,0),(20,-65,0),(20,-25,0),(-20,-25,0),單位為mm。需要說明的是, 在理論上, 每個采樣時刻的局部過程噪聲方差與融合中的過程噪聲方差是統(tǒng)一的, 但是由于存在數(shù)值計算誤差, AEKF局部估計的兩個過程噪聲方差可能會不同, 在此, 令融合過程噪聲協(xié)方差Q(d)為局部過程噪聲協(xié)方差的均值。

實驗時, 為了便于分析實驗結果及算法的運行時間, 采用以下步驟進行實驗: 首先, 利用設計的上位機系統(tǒng)及實驗平臺, 以20 Hz的采樣頻率實時提取特征點的像素坐標及記錄當前時刻6 足位移臺所反饋的姿態(tài)信息, 并保存; 其次, 將保存的數(shù)據(jù)代入已編寫好的Matlab程序中, 得到估計結果及算法運行時間。

圖 4，5 給出了基于AEKF的集中式融合方法和基于AEKF的分布式融合方法對目標位置和姿態(tài)的估計結果曲線, 直觀地看, 分布式融合方法與集中式融合方法的估計結果曲線幾乎重合, 說明分布式融合方法的精度接近甚至等于集中式融合方法的精度。

圖4 AEKF位置估計結果Fig.4 Position estimation results based AEKF

圖5 AEKF姿態(tài)估計結果Fig.5 Posture estimation results based AEKF

為了更加清晰、直觀的分析各個濾波器的估計精度, 表1給出了基于AEKF的局部估計誤差和融合估計誤差的統(tǒng)計信息, 包括最大誤差、誤差均值及誤差的方差。從表1中可看出：融合估計誤差均值及方差都要小于對應的局部估計結果, 表明通過信息融合提高了估計精度, 分布式融合方法與集中式融合方法的估計誤差均值及方差相近, 再次證明基于AEKF的分布式融合方法能保證系統(tǒng)的估計精度。

表1 基于AEKF的各個濾波器誤差統(tǒng)計Table 1 Error statistics of each filter based AEKF

表2給出了各個濾波器每次估計狀態(tài)信息的運行總時間, 這個實驗結果是基于Core i5-2450 2。5 GHz雙核CPU及6 GB RAM的硬件平臺所得到的, 從表2中可以看出: 基于AEKF 的分布式融合方法的運行時間要明顯小于集中式融合方法的運行時間。結合上述實驗結果表明：基于多傳感器的分布式融合估計方法在保證精度的前提下, 能大大提高系統(tǒng)的運行效率, 證明了所提出方法的有效性和優(yōu)越性。

表2 各個濾波器運行時間Table 2 Running time of each filters

5 結 論

在基于多視覺傳感器的移動剛體估計系統(tǒng)中, 針對于集中式融合方法消耗過多的計算資源的弊端, 提出了一種基于多視覺傳感器的分布式融合方法。筆者將自適應擴展卡爾曼濾波器作為局部估計器, 得到各個視覺傳感器的局部估計結果, 再運用分布式融合方法得到融合估計結果。通過實際實驗驗證：與自適應擴展卡爾曼集中式融合方法進行相比, 所提出的方法在保證估計精度的同時, 能有效提升系統(tǒng)的運行效率, 降低了系統(tǒng)對高性能硬件的依賴程度。