基于直線特征的合作目標(biāo)加權(quán)位姿估計(jì)*

邢亞斌 王汝童 譚振國 郭永奇 于 濤

（北京化工大學(xué)信息科學(xué)與技術(shù)學(xué)院 北京 100029）

1 引言

隨著我國高端制造業(yè)的快速發(fā)展，迫切需要對(duì)大型構(gòu)件的三維幾何參量進(jìn)行高精度測(cè)量，以確保裝配精度。當(dāng)使用三維測(cè)量儀器對(duì)裝配在大型構(gòu)件表面的合作目標(biāo)進(jìn)行瞄準(zhǔn)時(shí)，大型構(gòu)件所處外界環(huán)境會(huì)影響合作目標(biāo)的成像質(zhì)量，導(dǎo)致其位姿估計(jì)精度較低，從而使測(cè)量精度降低。因此，研究合作目標(biāo)加權(quán)位姿估計(jì)方法，減少外界環(huán)境造成的干擾，對(duì)提高合作目標(biāo)位姿估計(jì)的精度具有重要意義和應(yīng)用價(jià)值。

合作目標(biāo)具有較為簡單、明顯的幾何特征，且在3D世界坐標(biāo)系中的坐標(biāo)精確已知，有利于建立2D圖像特征與3D合作目標(biāo)之間的對(duì)應(yīng)關(guān)系，位姿估計(jì)的復(fù)雜度低［1］。合作目標(biāo)位姿估計(jì)方法主要可以分為基于n點(diǎn)透視模型（Perspective-n-Point，PnP）［2］和基于n線透視模型（Perspective-n-Line，PnL）［3］的方法。相比于點(diǎn)特征，直線特征所包含的屬性更為豐富，而且提取較為穩(wěn)定，受噪聲的影響較小，可靠性高［4］，因此PnL問題受到了更為廣泛的關(guān)注和研究。

根據(jù)PnL模型求解方法的不同，可分為非迭代方法和迭代方法［5］。其中非迭代方法主要有Mirzaei等［6］提出的全局最優(yōu)線性求解方法，該方法將優(yōu)化問題轉(zhuǎn)化為對(duì)三個(gè)三次方程組的求解問題，簡化了優(yōu)化過程，但是由于麥考利矩陣的構(gòu)建緩慢，總計(jì)算時(shí)間仍然較高；Zhang等［7］提出的RPnL方法，采用構(gòu)建局部坐標(biāo)系間接求取目標(biāo)位姿的方法，簡化了計(jì)算過程，其時(shí)間復(fù)雜度為O()n，位姿解算效率高，有效解決了直線共面情況下的位姿估計(jì)問題，能夠得到較為準(zhǔn)確的目標(biāo)位姿初值。雖然非迭代方法的位姿解算速度較快，但是對(duì)直線特征提取的精度要求較高［8］。當(dāng)合作目標(biāo)圖像受到測(cè)量環(huán)境影響發(fā)生質(zhì)量退化時(shí)，會(huì)導(dǎo)致特征提取精度下降，非迭代方法的精度與穩(wěn)定性都會(huì)受到較大影響［9］。

迭代法通過迭代的方式最小化特定的目標(biāo)函數(shù)［10］，從而完成對(duì)位姿估計(jì)結(jié)果的優(yōu)化，提高位姿估計(jì)精度［11］。此類方法能夠通過準(zhǔn)確的初值和層層的迭代提高位姿估計(jì)精度［12］。但是由于其迭代優(yōu)化方法多為無差別優(yōu)化［13］，并不能有效地解決由圖像質(zhì)量退化問題造成的直線定位偏差為位姿估計(jì)精度帶來的影響［14～15］。張躍強(qiáng)等［16］提出的基于直線間積分距離度量的位姿估計(jì)方法屬于無差別優(yōu)化方法，認(rèn)為每一條直線估計(jì)出的結(jié)果對(duì)誤差的貢獻(xiàn)相同來優(yōu)化目標(biāo)函數(shù)，會(huì)加大提取的相對(duì)不準(zhǔn)確的直線對(duì)位姿估計(jì)結(jié)果的影響。針對(duì)該問題，張振杰等［17］為提高位姿估計(jì)的抗噪性，對(duì)每條直線進(jìn)行加權(quán)，該方法假設(shè)權(quán)值服從高斯分布，并對(duì)其進(jìn)行迭代優(yōu)化，但是該權(quán)值具有一定隨機(jī)性，并不能準(zhǔn)確表示每條直線提取的精度。

本文在現(xiàn)有的PnL位姿估計(jì)方法的基礎(chǔ)上，針對(duì)大型構(gòu)件所處環(huán)境對(duì)合作目標(biāo)圖像成像質(zhì)量造成的影響，在位姿估計(jì)過程中引入了直線端點(diǎn)特征的約束，提出了一種合作目標(biāo)加權(quán)位姿估計(jì)方法。該方法根據(jù)直接提取的直線端點(diǎn)與其對(duì)應(yīng)的通過幾何約束關(guān)系計(jì)算的直線端點(diǎn)的距離賦予每條直線權(quán)值，在位姿優(yōu)化過程中，根據(jù)每條直線的權(quán)值對(duì)其重投影誤差進(jìn)行加權(quán)，提高合作目標(biāo)位姿估計(jì)的準(zhǔn)確性，并保持較快的位姿估計(jì)速度。

2 基于直線特征的合作目標(biāo)加權(quán)位姿估計(jì)

2.1 RPnL位姿估計(jì)方法

合作目標(biāo)中普遍存在直線特征，采用RPnL方法［7］可以對(duì)其位姿初值進(jìn)行求解，位姿估計(jì)的主要原理如下。

首先構(gòu)建局部坐標(biāo)系Om-XmYmZm，選擇圖像中具有最長投影直線長度的參考直線L0作為Zm軸，Xm軸為參考直線L0與相機(jī)光心Oc構(gòu)成的投影平面Π的法向量n0，則Ym軸也可確定。世界坐標(biāo)系Ow-XwYwZw旋轉(zhuǎn)到Om-XmYmZm的旋轉(zhuǎn)矩陣Rmw，可由V w0與V m0確定。其坐標(biāo)系轉(zhuǎn)換關(guān)系如圖1所示。

圖1 坐標(biāo)系轉(zhuǎn)換關(guān)系

另外選擇圖像中具有第二長投影直線長度的參考直線L1為輔助線，將線組{Li}分為n-2個(gè)三元組{L0L1Lj|j=2…n-2}，根 據(jù) 相 機(jī) 坐 標(biāo) 系Oc-XcYcZc下直線li方向向量Vic垂直于法向量ni可得約束函數(shù)

式 中，Rcm=R′Rot(Xm，α)Rot(Zm，β)表 示 由Om-XmYmZm旋轉(zhuǎn)到Oc-XcYcZc的旋轉(zhuǎn)矩陣；R′為以n0為首列的隨機(jī)正交旋轉(zhuǎn)矩陣。由式（1）可得

式中，系數(shù)Ak、Bk可由計(jì)算得到。將sin2α=1-cos2α代入式（2），并令x=cosα可得

式中，系數(shù)λk可由n1、n2、V1m、V2m計(jì)算得到。

使用最小二乘殘差法來求取系統(tǒng)的局部最小值可解得繞Xm軸旋轉(zhuǎn)的角度α，則Rcm可簡化為式中=R′Rot(Xm，α)，根據(jù)直線透視投影關(guān)系可得

根據(jù)式（5），對(duì)于n條直線，可以得到2n個(gè)參數(shù)向量為[cosβ，sinβ，，1]T的齊次線性方程組如下所示：

通過對(duì)齊次方程組進(jìn)行SVD分解可以求出β和，從而可以計(jì)算出平移向量T=，最后可得到由Ow-XwYwZw旋轉(zhuǎn)到Oc-XcYcZc的旋轉(zhuǎn)矩陣=，即可求得目標(biāo)對(duì)應(yīng)的位姿參數(shù)矩陣M。

RPnL位姿估計(jì)方法對(duì)合作目標(biāo)位姿初值進(jìn)行了線性求解，但是該方法未經(jīng)迭代優(yōu)化，位姿估計(jì)精度有限，且易受干擾。

2.2 基于直線特征的合作目標(biāo)加權(quán)位姿估計(jì)方法

采用RPnL方法獲得合作目標(biāo)的位姿初值之后，通過迭代方法對(duì)直線重投影誤差進(jìn)行優(yōu)化，可以獲得最優(yōu)的位姿參數(shù)矩陣M。

本文所采用的合作目標(biāo)為方形靶標(biāo)（單個(gè)網(wǎng)格尺寸為62.5mm×62.5mm）和圓形靶標(biāo)（內(nèi)刻圓形直徑為125mm），方形靶標(biāo)中同時(shí)存在點(diǎn)、線幾何特征，圓形靶標(biāo)中同時(shí)存在點(diǎn)、線、圓幾何特征，通過特征提取方法直接提取點(diǎn)特征，可以獲得各直線的端點(diǎn)坐標(biāo)；通過直線之間或直線與圓之間的幾何約束關(guān)系，可以計(jì)算得到直線端點(diǎn)坐標(biāo)。計(jì)算直接提取的直線端點(diǎn)到利用幾何約束關(guān)系計(jì)算的對(duì)應(yīng)直線端點(diǎn)的距離，可以確定直線特征提取的置信度。提取的直線和提取的點(diǎn)越接近則認(rèn)為直線提取的置信度越高，所以在位姿估計(jì)的優(yōu)化過程中，該條直線對(duì)應(yīng)的權(quán)重大；而對(duì)于直線和點(diǎn)距離誤差較大的情況，說明這部分提取的不好，那么則需抑制這部分對(duì)最終結(jié)果的影響，減少它的權(quán)重。

假設(shè)合作目標(biāo)中有m條模型直線，表示為{L1，L2，…Li，…Lm}，直線的重投影距離可表示為模型直線Li的端點(diǎn)到投影平面Π的距離［16］，如圖2所示。

圖2 模型直線段端點(diǎn)到投影平面的距離

合作目標(biāo)圖像中的直線方程用極坐標(biāo)表示為

將圖像直線端點(diǎn)坐標(biāo)代入式（7）可得

式中，γx、γy分別表示相機(jī)焦距與x軸、y軸方向像元尺寸的比值；式（8）可以簡化為

式中，n0=(γxcosθ，γysinθ，-ρ)T，其單位法向量可以表示為，則L在Π上的約束為

則L的端點(diǎn)到Π的距離為

以最小化該距離為目標(biāo)構(gòu)建誤差函數(shù)如下

采用引入直線權(quán)值的方法對(duì)誤差函數(shù)進(jìn)行改進(jìn)，計(jì)算提取的直線端點(diǎn)到利用幾何約束關(guān)系獲得的端點(diǎn)的距離di，可確定直線權(quán)值，如下式所示。

式中，(pi1，pi2)為圖像中直接提取得到的直線端點(diǎn)坐標(biāo)；()為利用幾何約束關(guān)系計(jì)算得到的對(duì)應(yīng)直線端點(diǎn)坐標(biāo)。di越大則說明該條直線的提取置信度越低，則對(duì)應(yīng)的權(quán)值越小。將di取倒數(shù)后歸一化作為權(quán)值ωi，可得

則引入權(quán)值后的直線重投影誤差優(yōu)化的誤差函數(shù)為

通過對(duì)其進(jìn)行迭代求解，可得到最優(yōu)的直線重投影誤差，從而求解出對(duì)應(yīng)的合作目標(biāo)位姿參數(shù)矩陣M。

在合作目標(biāo)的實(shí)際成像過程中，由于測(cè)量環(huán)境的影響會(huì)導(dǎo)致被測(cè)合作目標(biāo)圖像中可能存在噪聲、模糊、光照變化等多種影響直線特征提取的干擾因素，而直線特征提取的精度會(huì)直接影響到基于直線特征的位姿估計(jì)的精度。所提合作目標(biāo)加權(quán)位姿估計(jì)方法在優(yōu)化重投影誤差的過程中采用加權(quán)的方式引入每條直線的權(quán)值，可以提高合作目標(biāo)位姿估計(jì)的準(zhǔn)確性。

2.3 基于直線特征的合作目標(biāo)加權(quán)位姿估計(jì)算法流程

本文提出的基于直線特征的合作目標(biāo)加權(quán)位姿估計(jì)算法流程如圖3所示，實(shí)現(xiàn)步驟如下。

圖3 合作目標(biāo)加權(quán)位姿估計(jì)算法流程框圖

步驟1輸入合作目標(biāo)圖像，相機(jī)內(nèi)參數(shù)矩陣及畸變系數(shù)，設(shè)置最大迭代次數(shù)與誤差閾值ε*。

步驟2提取合作目標(biāo)圖像中的幾何特征，根據(jù)幾何約束關(guān)系獲取直線端點(diǎn)坐標(biāo)，計(jì)算其與直接提取的對(duì)應(yīng)直線端點(diǎn)的距離di，并將di的倒數(shù)歸一化作為權(quán)值ωi。

步驟3構(gòu)建局部坐標(biāo)系，將每3條直線分為一組，計(jì)算由局部坐標(biāo)系到相機(jī)坐標(biāo)系的旋轉(zhuǎn)矩陣Rcm，進(jìn)而確定由世界坐標(biāo)系到相機(jī)坐標(biāo)系的旋轉(zhuǎn)矩陣Rcw和平移向量T。

步驟4引入權(quán)值ωi構(gòu)造新的直線重投影誤差優(yōu)化的誤差函數(shù)E2。

步驟5將位姿初值代入目標(biāo)函數(shù)，判斷計(jì)算結(jié)果是否小于誤差閾值ε*，若不滿足，則繼續(xù)迭代。

步驟6輸出優(yōu)化后的位姿參數(shù)。

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

實(shí)驗(yàn)所用圖像為相機(jī)在真實(shí)環(huán)境下拍攝得到的多幅方形靶標(biāo)圖像（單個(gè)網(wǎng)格尺寸為62.5mm×62.5mm）和圓形靶標(biāo)圖像（內(nèi)刻圓形直徑為125mm），圖像尺寸為3024×4032像素。對(duì)比實(shí)驗(yàn)為文獻(xiàn)［7］的位姿估計(jì)方法。

實(shí)驗(yàn)采用重投影特征點(diǎn)平均距離誤差與位姿估計(jì)時(shí)間作為位姿估計(jì)結(jié)果的質(zhì)量評(píng)價(jià)指標(biāo)，其中重投影特征點(diǎn)平均距離誤差的計(jì)算公式為

式中，pIi表示原2D特征點(diǎn)的坐標(biāo)；表示3D特征點(diǎn)的坐標(biāo)；M表示位姿參數(shù)；m表示特征點(diǎn)數(shù)量；λi表示任意的比例系數(shù)。

使用本文方法分別對(duì)3幅實(shí)拍的方形靶標(biāo)圖像與圓形靶標(biāo)圖像進(jìn)行位姿估計(jì)處理，將所得到的位姿估計(jì)結(jié)果與文獻(xiàn)［7］方法的結(jié)果進(jìn)行對(duì)比，驗(yàn)證方法的有效性，圖4為實(shí)驗(yàn)所用的合作目標(biāo)圖像。

圖4 合作目標(biāo)圖像

合作目標(biāo)圖像的位姿估計(jì)結(jié)果如表1所示，對(duì)比位姿估計(jì)結(jié)果可以看出，本文位姿估計(jì)方法較文獻(xiàn)［7］方法的精度有所提高，位姿估計(jì)時(shí)間與其相當(dāng)。3幅方形靶標(biāo)圖像重投影特征點(diǎn)平均距離誤差的平均值為4.9798pixel，位姿估計(jì)時(shí)間的平均值為0.193s；與文獻(xiàn)［7］方法相比，重投影特征點(diǎn)平均距離誤差的平均值減小0.3253pixel，位姿估計(jì)精度提高6.13%。3幅圓形靶標(biāo)圖像重投影特征點(diǎn)平均距離誤差的平均值為3.4317pixel，位姿估計(jì)時(shí)間的平均值為0.189s；與文獻(xiàn)［7］方法相比，重投影特征點(diǎn)平均距離誤差的平均值減小0.3719pixel，位姿估計(jì)精度提高9.78%。

表1 合作目標(biāo)圖像位姿估計(jì)實(shí)驗(yàn)結(jié)果

通過對(duì)比實(shí)驗(yàn)所得到的位姿估計(jì)結(jié)果可以得出，本文方法的位姿估計(jì)精度在總體上優(yōu)于文獻(xiàn)［7］方法，能夠提高合作目標(biāo)的位姿估計(jì)精度，并保持較快的位姿估計(jì)速度。

4 結(jié)語

為了提高合作目標(biāo)的位姿估計(jì)精度，實(shí)現(xiàn)三維測(cè)量儀器對(duì)合作目標(biāo)的高精度瞄準(zhǔn)，提出了一種基于直線特征的合作目標(biāo)加權(quán)位姿估計(jì)方法。首先提取圖像中的合作目標(biāo)關(guān)鍵幾何特征，通過幾何約束關(guān)系獲取各直線端點(diǎn)；然后計(jì)算其與直接提取的對(duì)應(yīng)直線端點(diǎn)的距離，將距離的倒數(shù)歸一化作為權(quán)值；最后將所得權(quán)值引入直線重投影誤差的計(jì)算過程中構(gòu)建新的誤差函數(shù)，完成對(duì)合作目標(biāo)位姿參數(shù)的優(yōu)化。實(shí)驗(yàn)結(jié)果表明，所提方法的位姿估計(jì)精度優(yōu)于文獻(xiàn)［7］方法，方形靶標(biāo)的位姿估計(jì)精度提高6.13%，圓形靶標(biāo)的位姿估計(jì)精度提高9.78%，位姿估計(jì)時(shí)間與其相當(dāng)，能夠有效減小外界環(huán)境對(duì)位姿估計(jì)結(jié)果帶來的影響，提高合作目標(biāo)的位姿估計(jì)精度。