王平，付輝，徐貴力

1.蘭州理工大學(xué) 電氣工程與信息工程學(xué)院，蘭州 730050

2.甘肅省工業(yè)過程先進(jìn)控制重點(diǎn)實(shí)驗(yàn)室，蘭州 730050

3.南京航空航天大學(xué) 自動化學(xué)院，南京 210016

基于視覺的位置和姿態(tài)（簡稱位姿）估計問題又稱為n點(diǎn)透視成像（Perspective-n-Point，PnP）問題。該問題是在相機(jī)內(nèi)參數(shù)已知的情況下，利用n個3D 空間已知的特征點(diǎn)和其對應(yīng)的2D 圖像點(diǎn)來估計相機(jī)和目標(biāo)之間的位姿參數(shù)。PnP 問題的解決可廣泛應(yīng)用于視覺導(dǎo)航［1］、目標(biāo)位置和姿態(tài)的監(jiān)測［2］、無人機(jī)的視覺著陸［3］、航天器的自主交會對接［4］等領(lǐng)域。

目前，PnP 算法的研究已經(jīng)取得了豐碩成果［5-13］。然而，PnP 問題中需要知道3D 空間特征點(diǎn)與其2D 圖像點(diǎn)的對應(yīng)匹配關(guān)系，這是PnP 問題能夠準(zhǔn)確求解的必要條件。在一些實(shí)際應(yīng)用中，可以通過人工的方式尋找3D 物點(diǎn)與2D 像點(diǎn)的匹配關(guān)系［14］，但這無法滿足實(shí)時且自主估算位姿的要求。為了解決以上問題，可以在3D 空間按照一定規(guī)則編碼目標(biāo)（即物點(diǎn)）［15］，然后通過圖像處理的方法提取編碼信息，確定相應(yīng)的3D 物點(diǎn)和2D 像點(diǎn)對應(yīng)匹配關(guān)系。然而，采用圖像編碼方式識別3D/2D 對應(yīng)匹配關(guān)系的方法容易受到成像質(zhì)量的影響，空間目標(biāo)成像不清晰或圖像存在較大畸變時，將無法從圖像中正確識別出編碼信息，進(jìn)而導(dǎo)致3D/2D 對應(yīng)點(diǎn)匹配失敗。為克服采用圖像識別方式匹配3D/2D 對應(yīng)點(diǎn)方法的缺點(diǎn)，David 等［16］提出了一種可以實(shí)現(xiàn)同步位姿估計和對應(yīng)點(diǎn)匹配（Simultaneous Pose and Correspondence Determination，SPCD）的 Soft-POSIT 算法。該算法基于弱透視成像模型，同時結(jié) 合Softassign［17］和POSIT（Pose from Orthography and Scaling with Iterations）［18］2 種算法，采用交替迭代的方式實(shí)現(xiàn)估算位姿的同時確定對應(yīng)點(diǎn)匹配關(guān)系。張鑫等［19］、勞達(dá)寶等［20］對Soft-POSIT 算法進(jìn)行了應(yīng)用研究，表明SoftPOSIT算法局部收斂能力好，但無法全局收斂。為了提升SoftPOSIT 算法的抗噪能力，Moreno-Noguer等［21］提出了BlindPnP 算法。該算法采用混合高斯模型建模姿態(tài)集，并將其用來初始化算法中的卡爾曼濾波器。存在大量噪聲的情況下，Blind-PnP 算法性能優(yōu)于SoftPOSIT 算法，在常規(guī)情況下性能和SoftPOSIT 算法相當(dāng)，且都無法收斂于全局最優(yōu)值。為了得到SPCD 問題的最優(yōu)解，Brown 等［22-23］提出了BnBDA 算法，該算法將SPCD 問題轉(zhuǎn)換為優(yōu)化問題，并采用分支定界的方法在旋轉(zhuǎn)和位置空間上交替搜索位姿參數(shù)的最優(yōu)值?；谝陨纤悸?，Campbell 等［24］在隨后的工作中研究了空間存在孤立點(diǎn)時的SPCD 問題。最近的工作中，Liu 等［25］基于分支定界的思路，研究了基于直線特征的同步位姿估計和對應(yīng)直線匹配問題，取得了良好的效果。

相比于SoftPOSIT 算法，采用分支定界思路解決SPCD 問題的方法可以在全局空間上得到最優(yōu)解，因此收斂率和精度都較高。然而，分支定界方法求解時需要預(yù)先給定旋轉(zhuǎn)和位置參數(shù)的搜索空間。對于旋轉(zhuǎn)參數(shù)，利用歐拉角表示時取值范圍為[-π，π]。而對于位置參數(shù)，實(shí)際問題中的初始位置往往是無法確定的，為了保證搜索到較為準(zhǔn)確的位置值，通常需要給定一個很大范圍，這增加了分支定界迭代搜索的復(fù)雜度，影響了效率，限制了這類算法的實(shí)際應(yīng)用。事實(shí)上，在SPCD 問題中，除了3D/2D 點(diǎn)特征可以提供約束信息構(gòu)建優(yōu)化函數(shù)以外，點(diǎn)和點(diǎn)之間也可以提供約束信息，但現(xiàn)有算法中都未曾考慮這些信息?；谝陨纤悸?，本文在SPCD 問題中引入點(diǎn)和點(diǎn)之間的約束模型，該約束模型中僅含有旋轉(zhuǎn)參數(shù)。并基于以上約束模型，從幾何意義上出發(fā)構(gòu)建優(yōu)化求解SPCD 問題的目標(biāo)函數(shù)，采用分支定界的方式在旋轉(zhuǎn)空間上搜索最優(yōu)旋轉(zhuǎn)參數(shù)和最佳3D/2D 匹配對應(yīng)關(guān)系，以高精度實(shí)現(xiàn)相機(jī)同步位姿估計和對應(yīng)點(diǎn)匹配。

1 SPCD 問題描述

如圖1 所示，相機(jī)內(nèi)參數(shù)標(biāo)定的情況下，在3D 空間給定一組世界坐標(biāo)已知的空間目標(biāo)參考點(diǎn)P={Pi|i=1，2，…，m}，其在2D 圖像上的投影點(diǎn) 為p={pj|j=1，2，…，n}，并且P和p之間的一一對應(yīng)關(guān)系未知。SPCD 問題就是在計算相機(jī)位姿（即旋轉(zhuǎn)矩陣R∈SO(3)和平移矩陣t∈R3）的同時來確定P和p之間的對應(yīng)關(guān)系。在一些位姿估算任務(wù)中，如點(diǎn)云匹配［26］，由于目標(biāo)遮擋的原因，目標(biāo)參考點(diǎn)集P和其2D 投影點(diǎn)集p的大小不相等，即m≠n。但在大多數(shù)視覺位姿任務(wù)中，P和p的大小是相同的，即m=n?；谙鄼C(jī)透視成像模型［8］，SPCD 問題可以表示為

圖1 SPCD 問題描述Fig.1 Description of SPCD problem

式中：F(·)為相機(jī)坐標(biāo)系下3D 參考點(diǎn)到2D 圖像點(diǎn)的投影變換。SPCD 問題的本質(zhì)就是在3D 特征點(diǎn)和2D 投影點(diǎn)對應(yīng)關(guān)系未知的情況下，尋找最優(yōu)的R*、t*使得式（1）的誤差最小。在誤差最小的情況下，通過估算位姿R和t反向重構(gòu)的模型能夠和原始模型實(shí)現(xiàn)最佳的匹配。

2 基于點(diǎn)對點(diǎn)約束SPCD 問題求解

2.1 點(diǎn)對點(diǎn)約束模型推導(dǎo)

現(xiàn)有的SoftPOSIT 算法［14］或優(yōu)化求解SPCD 問題的方法［20-22］都是以式（1）誤差最小為思路求解。從式（1）中可以看出，求解過程中涉及的變量有R和t，因此迭代優(yōu)化過程中需要對R和t交替求解，無疑增加了算法實(shí)現(xiàn)的復(fù)雜度。除此之外，旋轉(zhuǎn)R和平移t自身的量綱不同，同時優(yōu)化求解時容易造成算法的不穩(wěn)定。事實(shí)上，在求解SPCD 問題時，除了3D/2D 點(diǎn)特征可以提供約束外，點(diǎn)和點(diǎn)之間也可以提供約束信息，現(xiàn)有的算法中并未有效利用這些點(diǎn)和點(diǎn)之間的約束信息。基于以上思路，本文提出一種基于點(diǎn)和點(diǎn)之間約束信息，并從幾何意義上構(gòu)建優(yōu)化目標(biāo)函數(shù)求解SPCD 問題的方法。

如 圖2 所 示，Pi，Pj∈P為3D 空間中 的任意2 點(diǎn)，假設(shè)其對應(yīng)圖像平面上的投影點(diǎn)為pi，pj∈p。相機(jī)光心O和pi、pj共同形成一平面，其平面法向量為Nij。當(dāng)相機(jī)內(nèi)參數(shù)標(biāo)定的情況下，Nij可以很容易由O和pi、pj形成的向量得到

利用旋轉(zhuǎn)矩陣R和平移向量t可以將世界坐標(biāo)系下的Pi、Pj轉(zhuǎn)換為相機(jī)坐標(biāo)系下的Ci、Cj，轉(zhuǎn)換關(guān)系為

式（3）中上下兩式相減，得

式中：Mij為相機(jī)坐標(biāo)系下的向量，且在平面OCiCj內(nèi)。從圖2 可以看出，Opi pj、OCiCj為同一個平面，因此Nij與Mij互相垂直，即Nij與RVij之間的夾角為。對于空間目標(biāo)參考點(diǎn)集P，按式（4）的定義可以生成個關(guān)于V的方向向量V={Vx|x=1，2，...，K1}。同理，投影點(diǎn)集p可按照式（2）生成平面法向量集合N={Ny|y=1，2，...，K2}，其中。由于P和p的對應(yīng)關(guān)系未知，因此生成的V和N對應(yīng)關(guān)系也未知。此時，基于新生成的方向向量集合V和法向量集N，SPCD 問題可以轉(zhuǎn)換為如下優(yōu)化問題：

圖2 點(diǎn)對點(diǎn)的約束Fig.2 Point-to-point constraint

式中：arccos(·)為反余弦函數(shù)，表示Ny和RVx的夾角，||NyTRVx||則限定了該夾角為銳角。當(dāng)3D參考點(diǎn)生成的方向向量V與其2D 投影點(diǎn)生成的平面法向量N一一對應(yīng)，且計算的姿態(tài)參數(shù)R*最優(yōu)時，式（5）誤差最小。相比于式（1），式（5）中僅含有旋轉(zhuǎn)參數(shù)R待求解，形式上更為簡單且待求解參數(shù)量綱唯一，能夠有效提升求解SPCD 問題時收斂到全局最優(yōu)解的概率，并且在優(yōu)化求解時更容易實(shí)現(xiàn)。由于R是一個旋轉(zhuǎn)矩陣，具有正交性。當(dāng)Ny和Vx單位化的時候，||Ny||||RVx||=1，此時式（5）可以簡化為

2.2 目標(biāo)函數(shù)上界和下界確定

分支定界法能夠在變量取值空間上迭代搜素全局最優(yōu)值且不會陷入局部最優(yōu)，因此本文采用分支定界的方式求解式（6）的最優(yōu)值。采用歐拉角r=[α，β，γ]表示旋轉(zhuǎn)R

式中：α、β、γ的取值范圍為-π～π，因此r=的取值空間形成半徑為π 的球體，如圖3（a）所示。為了在分支定界過程中取值空間的劃分方便，將r=[α，β，γ]的取值空間從半徑為π 的球體擴(kuò)充為半徑為π 的立方體，記作Cr。對式（6）分支定界的過程，就是對Cr按照3D 八叉樹結(jié)構(gòu)不斷迭代劃分使得式（6）誤差最小的過程，在這一過程中需要確定式（6）在每個劃分子空間的上界和下界

圖3 旋轉(zhuǎn)參數(shù)幾何邊界的表示Fig.3 An illustration of geometric bounds of rotation parameters

對于式（6）下界的推導(dǎo)，需要使用到文獻(xiàn)［27］中的引理。

引理1 采用歐拉角的形式給定2 個旋轉(zhuǎn)r、r0，其對應(yīng)旋轉(zhuǎn)矩陣為R(r)、R(r0)，則對于空間任意向量Vx∈R3，都存在不等式關(guān)系

式中：∠(·，·)為兩向量的夾角。

綜合式（9）、式（10）可以得出

式（6）在分支定界過程中主要計算Ny與R(r)Vx的夾角，即∠(Ny，R(r)Vx)，利用三角不等式的關(guān)系可以得到

式（13）兩端取模代入式（6），最終得到式（6）在子空間上的下界

式中：∠(Ny，R(r0)Vx)可以由計算得到。

2.3 分支定界優(yōu)化求解位姿參數(shù)

圖4 一個劃分子空間Fig.4 A branched subspace

根據(jù)式（6）的定義，得到最優(yōu)旋轉(zhuǎn)R*的同時也得到了V和N之間的最佳匹配關(guān)系。假設(shè)總共得到了k對匹配關(guān)系，從圖2 可以看出，當(dāng)Vij∈V和Nij∈N匹配時，Pi和Pj的中點(diǎn)同樣垂直于Nij。記Pi和Pj的中點(diǎn)為，則有

將式（15）展開，進(jìn)一步寫為關(guān)于t的表達(dá)式

式（15）、式（16）對k對匹配關(guān)系都滿足，因此有

3 實(shí)驗(yàn)分析

將本文提出的方法命名為BnB-P2P 算法，并采用仿真和真實(shí)實(shí)驗(yàn)驗(yàn)證的方法與SoftPOSIT算法［14］進(jìn)行對比。為了測試公平，所有代碼都采用MATLAB2018a 實(shí)現(xiàn)，本文實(shí)驗(yàn)代碼可以從https：//github.com/pingwangsky 下載。

3.1 仿真實(shí)驗(yàn)

合成焦距f=800 像素，分辨率為640×480像素的虛擬相機(jī)。在相機(jī)坐標(biāo)系下隨機(jī)產(chǎn)生3D空間參考點(diǎn)，分布在［-2，2］m×［-2，2］m×［4，8］m 的范圍內(nèi)。旋轉(zhuǎn)矩陣R按照式（7）生成，平移向量t取相機(jī)坐標(biāo)系下3D 參考點(diǎn)的平均值。由于相機(jī)拍攝時目標(biāo)物體在相機(jī)前方，因此式（7）中的角度r=[α，β，γ]取值在的 范圍。利用生成的R、t將相機(jī)坐標(biāo)系下的3D 參考點(diǎn)轉(zhuǎn)換到世界坐標(biāo)系下，得到世界坐標(biāo)系下的3D參考點(diǎn)集P。采用合成的虛擬相機(jī)，將P投影到2D 圖像上，得到其投影點(diǎn)集合p，并隨機(jī)打亂P與p的對應(yīng)匹配關(guān)系。

3.1.1 收斂成功率

SoftPOSIT 算法對初值的選取比較敏感，初值選擇不合理將導(dǎo)致SoftPOSIT 算法無法收斂。設(shè)r=[α，β，γ]為仿真時生成的真值，SoftPOSIT算法的初始值分別在范圍內(nèi)選取。對于本文提出的BnB-P2P算法，由于其可以全局尋優(yōu)，初始值范圍只影響收斂速率，不影響最終收斂結(jié)果，因此初始值范圍直接選取。改變測試點(diǎn)的數(shù)目從4～10，每個點(diǎn)處程序執(zhí)行50 次，分別統(tǒng)計Soft-POSIT 算法、BnB-P2P 算法的收斂成功率，結(jié)果如圖5 所示。

從圖5 中可以看出，隨著點(diǎn)數(shù)目的增加，Soft-POSIT 算法能夠收斂的次數(shù)逐漸增加，這是由于點(diǎn)數(shù)目增加時提供的有效約束信息也隨之增多，因此SoftPOSIT 算法能夠收斂的概率隨之增大。但隨著初始角選取范圍的擴(kuò)大，SoftPOSIT 算法能夠收斂的次數(shù)減少，即使點(diǎn)數(shù)目增加時也不能保證其完全收斂。相比之下，由于本文提出的BnB-P2P 算法采用了全局尋優(yōu)的方式，在以上測試情況下的收斂成功率均可達(dá)100%，遠(yuǎn)高于SoftPOSIT 算法。

圖5 收斂成功率Fig.5 Success rate of convergence

3.1.2 求解精度

算法收斂成功并不能保證算法收斂到正確的結(jié)果，為此需求取第3.1.1 節(jié)中收斂成功的結(jié)果與仿真真值之間的誤差，并計算誤差的平均值，旋轉(zhuǎn)矩陣、平移向量的誤差分別如圖6 和圖7所示。值得注意的是，為了誤差評估的統(tǒng)一性，本文誤差計算公式采用文獻(xiàn)［8，11-13］中的表示形式。即

式中：Erot、Etran分別為旋轉(zhuǎn)矩陣誤差、平移向量誤差；rk，true、rk分別為旋轉(zhuǎn)矩陣真值和計算得到旋轉(zhuǎn)矩陣的第k列；ttrue、t分別為平移向量真值和計算得到的平移向量。

從 圖6 和 圖7 可以看出，SoftPOSIT 算法雖然能夠收斂成功，但是并不能保證每次都收斂到正確的結(jié)果，因此誤差均值波動較大。本文提出的BnB-P2P 算法將SPCD 問題轉(zhuǎn)換為了最優(yōu)化問題，并且采用分支定界的方法在整個旋轉(zhuǎn)空間上搜索，因此能夠保證搜索到全局最優(yōu)值，可靠性和準(zhǔn)確率遠(yuǎn)高于SoftPOSIT 算法。進(jìn)一步做出BnB-P2P 算法旋轉(zhuǎn)矩陣誤差和平移向量誤差的箱型圖，如圖8 所示（由于SoftPOSIT 算法誤差波動太大，無法繪制其箱型圖）。從圖中可以看出，BnB-P2P 算法平均誤差數(shù)量級在10-4以內(nèi)，并且波動范圍不大，收斂可靠性遠(yuǎn)高于Soft-POSIT 算法。

3.1.3 求解魯棒性

為進(jìn)一步驗(yàn)證算法的魯棒性，初始仿真數(shù)據(jù)中增加方差為2 的零均值高斯白噪聲，并改變測試點(diǎn)的數(shù)目從4～10，采用式（19）分別計算Soft-POSIT、BnB-P2P 算法的旋轉(zhuǎn)矩陣、平移向量誤差，結(jié)果如圖9 所示（SoftPOSIT 算法的初始值在真值的范圍內(nèi)選?。?。從圖9 中可以看出，存在噪聲的情況下SoftPOSIT、BnB-P2P 算法的求解誤差要顯著大于不存在噪聲時的情況（參見圖6）。類似于圖6 和圖7 所得結(jié)論，由于本文BnB-P2P 算法由于采用了全局搜索最優(yōu)值的方法，因此在存在干擾噪聲的情況下，求解精度、穩(wěn)定性要高于SoftPOSIT 算法。

圖6 旋轉(zhuǎn)矩陣誤差Fig.6 Rotation matrix errors

圖7 平移向量誤差Fig.7 Translation vector errors

圖8 旋轉(zhuǎn)和平移誤差箱型圖Fig.8 Rotation and translation errors box-plot

圖9 噪聲情況下的旋轉(zhuǎn)和平移誤差Fig.9 Rotation and translation errors in the case of noise

3.1.4 計算效率

對2 種算法設(shè)置不同的初始角度范圍，并從4～10 改變空間3D 參考點(diǎn)的數(shù)目，每個參考點(diǎn)處各算法分別執(zhí)行100 次，統(tǒng)計各算法的平均運(yùn)行時間，結(jié)果如圖10 所示。可以看出，SoftPOSIT由于是局部尋優(yōu)的算法，因此求解速度快。本文的BnB-P2P 算法需要在整個解的空間上搜索最優(yōu)值，因此效率低于SoftPOSIT 算法。在明確初始位姿范圍且空間參考點(diǎn)數(shù)目較少的情況下，本文BnB-P2P 算法和SoftPOSIT 算法所消耗的時間相差不大。但隨著初始位姿范圍的擴(kuò)大和空間參考點(diǎn)數(shù)目的增加，本文BnB-P2P 算法的尋優(yōu)時間顯著增加，原因在于本文算法的復(fù)雜度是O(n2)。綜合以上考慮，本文BnB-P2P 算法適用于空間點(diǎn)數(shù)目較少且要求位姿求解精度較高的場合（如基于視覺的空間交匯對接、視覺輔助無人機(jī)降落等任務(wù)）。在不需要考慮運(yùn)行效率的位姿求解任務(wù)中，相比于SoftPOSIT 算法，本文BnB-P2P 算法有更可靠的收斂率、更高的求解精度，具有明顯的優(yōu)勢。

圖10 計算效率Fig.10 Computational efficiency

3.2 實(shí)物實(shí)驗(yàn)

采用現(xiàn)實(shí)增強(qiáng)的方式驗(yàn)證對比算法，實(shí)驗(yàn)中使用的相機(jī)分辨率為1 292×964 像素，像元尺寸為3.75 μm。采用張正友法［28］標(biāo)定相機(jī)內(nèi)參數(shù)，結(jié)果如表1 所示。

表1 相機(jī)標(biāo)定結(jié)果Table 1 Camera calibration result

以118 mm×118 mm×82 mm 的長方體為模型，對其7 個可以目視的三維頂點(diǎn)提取2D 圖像坐標(biāo)，如圖11（a）中綠點(diǎn)所示。三維頂點(diǎn)在3D 空間的真實(shí)坐標(biāo)可以由長方體的尺寸計算得到。長方體頂點(diǎn)的2D 圖像坐標(biāo)和其3D 頂點(diǎn)坐標(biāo)的對應(yīng)匹配關(guān)系未知，分別采用SoftPOSIT 算法和BnB-P2P 算法求解長方體模型在相機(jī)坐標(biāo)系下的位姿。對于SoftPOSIT 算法，初始值首先選擇在真值較近處，通過迭代計算得到長方體在相機(jī)下的旋轉(zhuǎn)矩陣和平移向量，并利用該計算值將長方體模型的3D 頂點(diǎn)投影到圖像平面，結(jié)果如圖11（b）所示，可見SoftPOSIT 算法投影模型能夠和真實(shí)模型較好重合，證明當(dāng)初始值離真值范圍較近時，SoftPOSIT 算法能夠有效收斂到真值。然后，將SoftPOSIT 算法的初始值設(shè)置為r=[0，0，0 ]，同樣使用迭代結(jié)果得到投影長方體模型的3D 頂點(diǎn)，如圖11（c）所示，可見當(dāng)初始值離真值范圍較遠(yuǎn)時，SoftPOSIT 算法不能有效收斂于真實(shí)值，容易導(dǎo)致位姿求解出現(xiàn)錯誤。對于本文BnB-P2P 算法，無需選擇迭代初始值，直接給定旋轉(zhuǎn)參數(shù)的搜索空間為[-π，π]。同樣利用最終搜索的位姿參數(shù)將長方體模型的3D 頂點(diǎn)投影到圖像平面，如圖11（d）所示?？梢钥闯觯疚腂nB-P2P 算法能夠在旋轉(zhuǎn)參數(shù)的整個定義域空間上搜索并收斂于真實(shí)值，迭代求解位姿真值的能力明顯好于SoftPOSIT 算法。圖12 為BnBP2P 算法搜尋最優(yōu)旋轉(zhuǎn)參數(shù)時上界和下界的收斂曲線，大約經(jīng)過5 000 次迭代以后，BnB-P2P 算法的上界和下界偏差小于預(yù)先設(shè)定的精度，最終收斂于最優(yōu)值。

圖11 使用3D 模型的投影增強(qiáng)圖像Fig.11 Images augmented by using projections of 3D model

圖12 收斂曲線Fig.12 Convergence curves

4 結(jié)論

1）本文推導(dǎo)出了一種新的基于點(diǎn)和點(diǎn)約束的SPCD 問題求解模型，該模型中僅含有旋轉(zhuǎn)參數(shù)，不含有平移參數(shù)，優(yōu)化求解時無需采用交替迭代的方式。

2）基于推導(dǎo)的新模型，從角度距離出發(fā)構(gòu)建了求解SPCD 問題的優(yōu)化目標(biāo)函數(shù)，并采用分支定界搜索的方法在旋轉(zhuǎn)參數(shù)的全局空間上搜尋最優(yōu)的相機(jī)位姿和最佳的3D/2D 匹配關(guān)系。

3）實(shí)驗(yàn)表明，相比于傳統(tǒng)SoftPOSIT 算法，本文算法的收斂率成功率更高，位姿求解的精度更好，并且求解時無需精確的初始值，可以在旋轉(zhuǎn)參數(shù)的整個定義域空間上搜索尋優(yōu)，具有更好的適應(yīng)性。