基于機(jī)器視覺的最大似然位姿估計算法

屈也頻，張超然，呂余海

(海軍研究院，上海 200235)

引言

利用相機(jī)對位置已知的控制點(diǎn)進(jìn)行拍攝，通過拍攝得到的二維圖像解算相機(jī)在世界坐標(biāo)系下的位置和姿態(tài)，稱為位姿估計問題，是機(jī)器人導(dǎo)航、計算機(jī)視覺等領(lǐng)域[1-3]的核心問題之一。位姿估計根據(jù)計算方法分為2種方式[4]：第一種一般通過解算一組與旋轉(zhuǎn)參數(shù)和平移參數(shù)相關(guān)的多項式，進(jìn)而獲得位姿估計結(jié)果[5]，這類方法對于噪聲敏感[3]，估計性能有限；另一種通常稱為迭代類算法，這種算法通過建立代價函數(shù)[6]，將位姿估計問題轉(zhuǎn)換為非線性最小二乘優(yōu)化問題，然后利用Gauss-Newton[7]、Levenberg-Marquardt等方法進(jìn)行位姿參數(shù)的估計，這類算法能夠綜合利用多點(diǎn)的冗余信息[8]，增加了估計的魯棒性，迭代算法中還有一種稱為正交迭代(orthogonal iteration, OI)算法，該算法將物空間共線性誤差的最小化視為求解絕對定向問題，然后利用迭代方式進(jìn)行參數(shù)估計[9]，這種方法計算量較小，速度較快[10]，并保證了全局收斂性，但是性能略遜于傳統(tǒng)迭代算法。

傳統(tǒng)迭代算法是從信號的測量誤差出發(fā)[11]，利用誤差最小化的原則建立代價函數(shù)，從而得到估計值。這一類算法是一種最小二乘估計，試圖使采樣得到的信號和無噪聲情況下的數(shù)據(jù)之差的平方達(dá)到最小，這一類方法對采樣數(shù)據(jù)不做任何的統(tǒng)計假設(shè)，性能取決于噪聲的特性，并且往往不是最佳估計，而由于沒有對信號做任何統(tǒng)計假設(shè)，估計的統(tǒng)計性能是無法評價的[12]。由于相機(jī)拍攝的不確定性，圖片采樣所得的像素信號應(yīng)視為隨機(jī)信號，本文從隨機(jī)信號的統(tǒng)計特性出發(fā)，根據(jù)采樣信號的概率密度推導(dǎo)了最大似然位姿估計的一般形式。從理論上證明了利用單幅圖像，在各向同性高斯噪聲情況下傳統(tǒng)迭代算法等效于最大似然估計，因此在理論上傳統(tǒng)迭代算法只能應(yīng)用于各向同性高斯噪聲，對于復(fù)雜噪聲，將會產(chǎn)生模型誤差，導(dǎo)致算法失效。本文的最大似然算法將是推導(dǎo)復(fù)雜噪聲下的位姿估計的基礎(chǔ)；另外，根據(jù)信號的Fisher信息陣推導(dǎo)了位姿估計的克拉美-羅界(Cramér-Rao bound, CRB)，作為任何無偏估計的方差下界，克拉美-羅界可以作為位姿估計方法有效性的評價標(biāo)準(zhǔn)，并且根據(jù)所得結(jié)果可知，通過適當(dāng)增加采樣個數(shù)可以有效提高估計的性能。這是由于通過采樣信號個數(shù)的增加，可以更好地估計隨機(jī)噪聲的統(tǒng)計特性，從而減小隨機(jī)噪聲對于估計的影響。

1 信號模型

(1)

式中：k=1,2,…,K,其中：

m(k)= [u1(k),v1(k),u2(k),v2(k),…,

un(k),vn(k)]T

(2)

圖1 位姿估計問題的幾何示意圖Fig.1 Geometric sketch of pose estimation

世界坐標(biāo)與相機(jī)坐標(biāo)存在以下關(guān)系：

(3)

由共線方程可以得到：

(4)

(5)

式中：tx(k)、ty(k)、tz(k)分別為t(k)從上到下的3個元素；r1(k)、r2(k)、r3(k)分別為R(k)的從上到下的3個橫向量；R可以用3個歐拉角表示。假設(shè)在K次拍攝過程中，相機(jī)的姿態(tài)、控制點(diǎn)的世界坐標(biāo)保持不變，即m不隨時間變化，則采樣信號可以表示為

s(k)=m+n(k)

(6)

2 最大似然估計

最大似然估計是一種使用了信號概率模型的方法，其基本思想是參數(shù)的估計值應(yīng)是使觀測信號概率最大的值。假設(shè)信號為高斯隨機(jī)變量，噪聲的均值為零，則m即為觀測量s(k)的均值，根據(jù)概率論可知，k時刻觀測量的概率密度為[13]

(7)

式中：det(·)表示矩陣的行列式；Qs為隨機(jī)信號s的方差。與待估計參數(shù)無關(guān)，待估計參數(shù)表示為

θ=[α,β,γ,tx,ty,tz]T

(8)

則m(θ)表示為θ的函數(shù)，假設(shè)K次拍攝是統(tǒng)計獨(dú)立的，則K次拍攝觀測值的聯(lián)合密度函數(shù)為

(9)

對等號兩邊求自然對數(shù)，去掉常數(shù)項并除以K，得到似然函數(shù)為

(10)

(10)式的第二項可以表示為

(11)

式中：tr{·}表示取矩陣的跡；Cs(θ)定義為信號相關(guān)矩陣：

(12)

于是得到似然函數(shù)：

(13)

(13)式為最大似然估計的一般形式，使似然函數(shù)最大的參數(shù)θ即為位姿估計的最大似然估計，可以通過搜索或者更高效的迭代算法計算。

(14)

由于似然函數(shù)取最大值的必要非充分條件為

(15)

得到：

(16)

將上式代入似然函數(shù)，去除常數(shù)項并除以2n，似然函數(shù)化簡為

L(θ)=-lntr{Cs(θ)}

(17)

根據(jù)Cs(θ)的定義，求似然函數(shù)的最大值等效于求下式的最小值：

(18)

利用單幅圖像時，上式與傳統(tǒng)的迭代算法等效，因此，利用單幅圖像時，在各向同性高斯噪聲情況下傳統(tǒng)迭代算法與最大似然算法等效。值得注意的是，上述結(jié)果是在(16)式的情況下得到的，即位姿估計是在噪聲功率估計的基礎(chǔ)上實(shí)現(xiàn)的，所以增加信號的采樣，提高對噪聲統(tǒng)計特性的估計，有利于提高參數(shù)估計的性能。

3 克拉美-羅界分析

為了了解參數(shù)估計的潛在性能，可以利用克拉美-羅界作為評價標(biāo)準(zhǔn)?？死?羅界給出了參數(shù)所有無偏估計的方差極限[14]，將估計誤差的方差表示為C(θ)，對于所有無偏估計，有關(guān)系式：

C(θ)≥CCR(θ)

(19)

式中CCR(θ)表示克拉美-羅界，克拉美-羅界可以通過求解Fisher信息陣J的逆矩陣求得：

CCR(θ)=J-1

(20)

Fisher信息陣J中的元素為

(21)

通過對(13)式求偏導(dǎo)并取期望可以得到[J]i,j通用的形式(推導(dǎo)可參見文獻(xiàn)[15])：

(22)

(22)式為Fisher信息陣的一般化形式，是許多克拉美-羅界推導(dǎo)的起點(diǎn)，需要注意的是，通常方差Qs是一個未知矩陣，因此上式應(yīng)該包含方差的未知參數(shù)。

在位姿估計問題中，由于(22)式的第1項只與方差所含未知參數(shù)有關(guān)，第2項只與待估計參數(shù)有關(guān)，可以將Fisher信息陣分塊：

(23)

(24)

(25)

(26)

根據(jù)矩陣分塊求逆原理，噪聲功率估計的克拉美-羅界為

(27)

(28)

定義

(29)

由于θ=[α,β,γ,tx,ty,tz]T，待估計參數(shù)的克拉美-羅界為

(30)

從(30)式中可以看出，參數(shù)估計的克拉美-羅界隨拍攝數(shù)量的增加線性下降，隨噪聲功率的增加線性增加。

4 仿真

利用計算機(jī)仿真對最大似然估計和克拉美-羅界進(jìn)行分析，并與傳統(tǒng)迭代算法以及OI算法進(jìn)行比較。

仿真參數(shù)：相機(jī)焦距f=35 mm，單個像素尺寸dx=dy=62.5 μm，主點(diǎn)在圖像中心，控制點(diǎn)從相機(jī)坐標(biāo)系的[-2 2]×[-2 2]×[4 9] m3的長方形區(qū)域內(nèi)隨機(jī)選取，相機(jī)坐標(biāo)系相對于世界坐標(biāo)系的歐拉角為[20 10 30]°，平移矩陣為[2 3 10] m。圖2(a)是隨機(jī)選取的控制點(diǎn)在相機(jī)坐標(biāo)系下的坐標(biāo)，圖2(b)中圓圈表示相機(jī)拍攝控制點(diǎn)時實(shí)際成像位置(像素坐標(biāo)系)，圓圈旁邊的10個點(diǎn)為噪聲功率為1 dB時，由拍攝的10張圖片提取的坐標(biāo)點(diǎn)。

圖2 控制點(diǎn)在相機(jī)坐標(biāo)系和成像平面上的位置Fig.2 Positions of control points in camera coordinate system and image plane

圖3是傳統(tǒng)迭代算法、OI算法、最大似然方法在不同的噪聲功率下的參數(shù)估計均方根誤差(root-mean-square error, RMSE)，在各噪聲級下完成500次獨(dú)立實(shí)驗(yàn)，其中最大似然方法利用了10張圖片。從仿真結(jié)果中可以看出，3種方法在噪聲功率較小的情況下都趨向于克拉美-羅界，傳統(tǒng)迭代算法性能略優(yōu)于OI算法，而最大似然方法的性能明顯優(yōu)于其他兩種方法。

圖3 不同噪聲功率下各算法的參數(shù)估計均方根誤差Fig.3 RMSEs of different methods with different noise powers

圖4是最大似然估計在不同的圖片數(shù)下的參數(shù)估計均方根誤差，噪聲功率為2 dB，每個圖片數(shù)完成500次獨(dú)立實(shí)驗(yàn)，可以看出參數(shù)估計的RMSE隨著圖片數(shù)的增加而減小，說明適當(dāng)?shù)卦黾优臄z數(shù)量可有效提高估計的性能。

圖4 不同圖片數(shù)下參數(shù)估計的均方根誤差Fig.4 RMSEs with different snapshot numbers

5 結(jié)論

給出機(jī)器視覺位姿估計的信號模型，并從隨機(jī)信號的角度出發(fā)，推導(dǎo)了基于機(jī)器視覺的最大似然位姿估計以及相應(yīng)的克拉美-羅界，從理論上證明了利用單幅圖像時，在各向同性高斯噪聲情況下傳統(tǒng)迭代算法與最大似然算法等效，通過仿真分析，可知在圖像數(shù)量適當(dāng)增加的情況下，估計性能得到了明顯改進(jìn)，適用于更高精度的位姿估計。并且推導(dǎo)的位姿估計克拉美-羅界作為任何無偏估計的方差下界，是位姿估計的性能極限，可以作為位姿估計方法有效性的評價標(biāo)準(zhǔn)；下一步，針對復(fù)雜的噪聲情況，應(yīng)在此基礎(chǔ)上討論各向異性噪聲以及相關(guān)噪聲情況下的位姿估計方法及其克拉美-羅界。