坐標矩陣奇異值分解下的視覺導航點對應關系計算方法

孫永榮，黃 斌，曾慶化，趙 偉

（南京航空航天大學 自動化學院導航研究中心，南京 210016）

視覺傳感器以其近距測量精度高廣泛應用于空中加油對接、著陸著艦、水下導航等小范圍定位場景中[1-5]。點是視覺導航最為常用的幾何特征，例如利用特征點之間的匹配來計算光流速度或載體的六自由度運動被廣泛應用于視覺里程計與同步定位與制圖（SLAM）系統中[6-9]。

已知目標處自帶或布置的點狀特征來進行相對定位是視覺導航的另一種主要方式[10-13]。這種方式主要應用在受控環境下，可利用容易檢測的人工特征實現視覺高精度實時定位。例如，自動空中加油系統中的LED標志燈[14]或者無人機自主起降系統中的特征光點[15]等是輔助視覺導航的關鍵特征。

特征點視覺定位系統主要包括特征點三維空間定位、特征點二維圖像定位、特征點對應關系計算以及基于特征點的視覺位置姿態估計等幾個部分[16-19]。其中，基于特征點的視覺位置姿態估計方法已經相對成熟，采用“線性粗估計”與“非線性優化”兩步策略能夠獲得絕大部分情況下的相對位置和姿態。而在求解視覺位置姿態參數時需要先將圖像特征點與空間特征點一一對齊，即特征點對應關系計算。以VisNav系統為例，它采用了特征點按照一定頻率閃爍的方式來確定特征點的對應關系；而直升機著陸或著艦系統中則利用標志物自身特點人工設計用于區分不同特征點的幾何特征。

現有視覺特征點對應關系計算方法需要根據特征點的分布來人工設計用于描述點位置信息的角度、長度等特征，而后利用這些人工設計特征來區分不同位置處的特征點，算法對人工設計特征的依賴性強，缺乏在不同特征點配置下的通用性。

針對特征點對應關系計算的普適性，本文考慮一般幾何分布下的特征點配置情況，并提出了基于坐標矩陣奇異值分解的特征點對應關系計算方法，它以矩陣奇異值分解為工具，分析特征點三維坐標的內蘊特點，構建與旋轉、尺度以及平移無關的不變特征，并計算特征點對應關系。該方法避免了傳統方法中特征點對應關系的指數級搜索或者在搜索特征點對應關系時需要人工設計較為復雜的匹配策略。

本文先建立了視覺點對應關系計算問題的數學模型，而后推導并提出了坐標矩陣奇異值分解下的點對應關系計算方法，最后利用所研制的視覺定位系統進行了實驗和分析。

1 視覺導航點對應關系計算問題的數學模型

基于小孔成像原理可建立視覺傳感器成像模型，對某一空間點X具有如下變換關系：

式中，λ表示該空間點的深度，表示該空間點對應的規范化圖像坐標，Xw表示空間點的世界坐標系下的坐標，(R,t)表示相機坐標系與世界坐標系之間的旋轉變換矩陣與平移向量。

定義點對應關系矩陣MN×N，它是一個0-1矩陣，并且每一行每一列僅有1個元素為1，M(i,j)=1表示第i個圖像點對應第j個空間點，即

在上述成像模型的基礎上，可描述視覺點對應關系計算問題如下：已知多個空間點的世界系坐標點集為以及規范化圖像坐標點集計算點集Sn與點集Sw之間的點對應關系矩陣M，使得如下的誤差指標最小：

其中，λ(k)、R和t是未知的常值標量、矩陣或向量。

2 坐標矩陣奇異值分解下的點對應關系計算方法

本節首先建立了坐標矩陣奇異值分解關系等式，而后針對兩種奇異值分布情況分別提出了相應的點對應關系計算方法，并針對其中的旋轉變換下的二維點集匹配方法進行介紹，最后對本文方法進行總結。

2.1 坐標矩陣奇異值分解關系等式

根據式(1)并結合點對應關系矩陣的定義可知：

考慮“相機與物體之間的距離遠大于物體在光軸方向上的尺寸”情況，即特征點深度相差較小，可認為為常量。對式(3)左右兩邊按行求和可得：

對式(3)左右兩邊的每一列向量，與式(4)左右兩邊作差，可得：

式中，Zn和Zw分別表示兩個點集對應的中心化坐標矩陣，即至此，消除了表達式中的平移向量t。

進一步對式(5)中的Zn、Zw進行奇異值分解得到其中奇異值按降序排列，展開可得：

將奇異值分解結果代入式(5)中，可得：

式(7)即為坐標矩陣奇異值分解關系等式，其左右兩邊均表示矩陣Zw的奇異值分解。對于確定矩陣，其奇異值是確定的，考慮到Xn的第三個分量是常值 1，即Zn是不滿秩的，因此Zn的第三個奇異值是0。只考慮前兩個奇異值，由式(7)可知：

可以看出，以上坐標矩陣的規范化奇異值是與旋轉、尺度、平移以及點對應關系無關的不變特征量。另外，從式(7)可知，兩個中心化坐標矩陣的奇異值特征向量是與旋轉、尺度以及平移無關的不變特征量，僅與點匹配對應關系相關，因此可利用該不變量為的匹配關系來計算點集匹配關系。

坐標矩陣的奇異值描述了特征點集的分布情況，需要根據奇異值的特點進行分類處理，下面分別對“所有奇異值均不相等且相差較大”、“某兩個較大的奇異值相等或者相近”等兩種情況進行分析。

2.2 所有奇異值均不相等且相差較大

若考慮矩陣Zw具有 3個不等且相差較大的奇異值，則某個奇異值對應的特征向量之間相差一個正負號，從而根據式(7)有

式中，Φ為對角矩陣，其對角線上的元素φ1、φ2、φ3只能取±1。

選擇某一組φ1、φ2值進行計算。由于上述推導是在λ(k)λ的情況下進行的，即式中的等號并不嚴格成立，特別是對于Zn的奇異值分解，其最小的奇異值為 0，即通常滿足條件ηn1≥ηn2＞ηn3=0，因此對于式(10)僅提取前兩個奇異值對應的特征向量，即

對于式(11)中第三行的兩個等式（其中只有M為未知量），進一步整理可得：

將式(12)左邊和右邊的左半部分看成是兩個二維點集，則式(12)描述了兩個二維點集之間的對應關系，可通過搜索各個點的最近鄰點來確定。建立距離矩陣DN×N={dij}，用以描述兩個二維點集之間的距離，距離矩陣的各個元素的計算方式如下：

式中，ei和ej分別表示第i個或第j個元素為1、其他元素為0的N×1列向量。

通過搜索距離矩陣D每一行或者每一列的最小元素，可確定二維點集中離某個點最近的元素。為了避免單向搜索可能帶來的不一致性，采用了雙向搜索策略，即搜索每一行的最小元素，可確定一組從左到右的點對應關系，若滿足雙射條件（即兩個點集中的點是一一對應的），則認為已搜索到有效的點對應關系，否則，繼續搜索每一列的最小元素，同樣可確定一組點對應關系，并判斷其是否滿足雙射條件，若滿足則認為搜索到有效的點對應關系，若還不滿足，則認為沒有找到有效的點對應關系，即所選擇的φ1、φ2是無效的。

若上一步能夠找到有效的點對應關系，計算這N組對應點距離的平均平方和ε(φ1,φ2)，用以評價點對應關系的質量。選擇φ3=1，并根據式(10)中的第一個等式計算出旋轉矩陣：

由于旋轉矩陣的行列式為1，若計算出的det(R)=-1，則選擇φ3=-1并重新計算R，以保證det(R)=1。

在實際情況下，可對旋轉矩陣進行一定的約束。由于目標通常處于相機前方，且相機坐標系的Z軸定義為相機光心到相機拍攝場景的中心，若定義的世界坐標系Z軸與相機坐標系的Z軸之間的夾角不超過90°，則R的最后一個元素滿足r33＞0。利用這一條件可消除從物體背面進行拍攝的情況。

上述從式(11)開始的計算是對某一組φ1、φ2取值進行的，一共存在4種情況。對這4種情況下得到的距離平均平方和ε(φ1,φ2)進行比較，選擇最小值所對應的點對應關系作為最終計算結果。

2.3 某兩個較大的奇異值相等或者相近

式(11)說明了在計算對應關系時主要利用了前兩個較大的奇異值進行計算。若考慮這2個奇異值相等或者相近，即式(11)中的等式并不是唯一的，還可能存在交叉的對應關系，即同時還可能存在旋轉變換。因此，需要對式(12)重新進行編排：

式中，Τ表示二維旋轉變換，θ為旋轉角度，其取值范圍為0≤θ＜2π，Φ12的可能取值如下：

進一步可修改式(13)為

由于此處存在二維旋轉變換，因此不能直接采用前面的最近鄰點搜索方法。可通過比較模值來選擇可能的候選匹配關系，而后對這些可能的候選匹配關系進行檢驗，具體方法參考2.4節。

2.4 旋轉變換下的二維點集匹配方法

1）計算兩個匹配點集的模值LA=sqrt和LB=sqrt，其中，sqrt(·)表示對矩陣中各個元素求平方根。

3）計算模值距離矩陣D(LL)所有元素的均值，記為。比較模值距離矩陣D(LL)每一個元素與之間的大小，若小于則認為該元素所表示的匹配關系滿足模值距離條件。對于模值距離矩陣D(LL)第i行，統計該行滿足模值距離條件的元素個數n(i)。選擇n(i)最小的行im，記錄滿足模值距離條件的元素的列索引 {j1,j2,…,jn(im)}，該索引值說明了點集PA第im個點與點集PA第j1、j2、…、jn(im)個點的模值相近。

4）計算旋轉角度候選值：

式中，θ(·)表示對應向量的極坐標角度，對每一種可能的旋轉角度候選值計算旋轉矩陣T，從而可利用最近鄰點搜索方法檢驗及計算點對應關系。若獲得有效的點對應關系，則記錄距離平均平方和ε(T·Φ12)。

5）選擇最小距離平均平方和ε(Φ12)所對應的點對應關系作為最終計算結果。

2.5 點對應關系計算方法總結及特點分析

總結上述方法的推導過程，同時將“所有奇異值均不相等且相差較大”的情況合并到“某兩個較大的奇異值相等或者相近”情況中，則在視覺投影變換約束下的點對應關系計算過程如下：

已知空間點的世界系坐標點集Sw以及規范化圖像坐標點集Sn。

1）計算點集坐標均值，并對坐標矩陣進行奇異值分解。

2）遍歷符號矩陣Φ12與旋轉變換矩陣T所有可能取值，計算有效的點對應關系。

遍歷符號矩陣Φ12的可能取值（見式(16)），對于符號矩陣Φ12的每一種取值，判斷Zw的2個較大奇異值是否相差較大，若相差較大，則旋轉變換矩陣T的可能取值為

若相差較小，則利用 2.4節的二維點集匹配方法確定旋轉變換矩陣T的可能取值。

3）遍歷符號矩陣Φ12與旋轉變換矩陣T的每一種可能取值，計算奇異值特征向量二維點集對應的距離矩陣D，其元素計算公式參考式(17)。進一步根據距離矩陣D雙向搜索最近鄰點，以確定點對應關系，并判斷是否滿足雙射條件，若滿足，根據式計算旋轉矩陣，并判斷r33是否大于0，若大于0則認為已找到有效的點對應關系，并記錄距離平均平方和ε(Φ12)。

4）確定最優點對應關系。

選擇最小距離平均平方和ε(T·Φ12)所對應的點對應關系作為最終計算結果。當存在多個距離平均平方和ε(T·Φ12)與最小值相近時，說明特征點的分布滿足旋轉對稱的特點，此時有效的點對應關系不止一組，可根據實際場景（如旋轉角度約束）進行選擇。

從上述計算過程可以看出，本文所提出的點對應關系計算方法需要進行檢驗的點對應關系個數為max(4,2× min(n(i) ) )≤ max(4,2× (N-1))，即最多需要檢驗的點對應關系與點集中點個數成正比。相比指數級的搜索次數，本文方法能夠極大提高搜索效率。另外，本文方法能夠處理存在旋轉對稱特點的點對應關系計算問題。

需要注意的是，本文方法采用了深度近似相等（即式(4)中λ(k)λ）的假設，即本文方法只適用于光軸方向上目標特征點分布較為密集的場景。例如在軟管式空中加油中在加油錐套上按照前后兩個環形布置特征點、或者無人機著陸著艦時采用的小范圍人工標記等情況。

3 試驗結果與分析

采用FL3-U3-20E4M-C近紅外工業灰度相機，拍攝圖像像素個數為800×600，其在OpenCV下的標定參數為fu= 9 18.34,fv= 9 18.34,u0= 3 99.5,v0=299.5。以空中加油為應用場景，在加油錐套上布置了8個近紅外敏感材料小球作為特征點，這些小球在感光近紅外波段的光線時較為敏感，并且分布在2個圓環上。對于這 8個特征點，采用文獻[20]的方法進行三維空間點坐標標定，獲得的特征點世界系坐標如表1所示。

表1 目標特征點世界系三維坐標Tab.1 3D coordinate of points in world frame

為了檢驗點對應關系計算方法結果的正確性，定義點對應關系向量為p=(p1,p2,,pN)，其中pi表示點集Zw中第i個世界系坐標對應于點集Zn中第pi個圖像坐標。通過人工方式標記圖像特征點的順序，預設某個隨機的點對應關系向量，打亂圖像特征點的順序，利用點對應關系計算方法計算點對應關系向量，與預設值進行比較。若一致，則結果正確；若存在多個可能解，即判斷存在旋轉對稱的情況；若預設值與某個可能解相同，則同樣認為結果正確。

3.1 單幀圖像計算過程及結果分析

對某一幀圖像提取特征點圖像坐標，采用人工標記的方式，通過對局部區域放大可獲得亞像素級別的特征點圖像定位精度，圖像特征點記錄的順序與上述一致。對于某一幀圖像所記錄的特征點如表2所示。

表2 目標特征點二維圖像坐標Tab.2 2D coordinates of points in image frame

定義點對應關系向量為p=(p1,p2,,pN)，其中pi表示點集Zw中第i個世界系坐標對應于點集Zn中第pi個圖像坐標。打亂圖像坐標點集的順序，預設點對應關系向量為p=(2,7,6,3,4,5,1,8)，以檢驗點對應關系計算方法是否能夠獲得正確的點對應關系。

首先來驗證本文所設計不變特征量的有效性，對三維世界系點集、排列正確的二維圖像點集、打亂順序后的二維圖像點集分別計算規范化奇異值，如表3所示。可以看出，它們的規范化奇異值基本一致，驗證了“規范化奇異值是旋轉平移尺度以及點對應關系下的不變量”。

表3 規范化奇異值對比Tab.3 Comparison on invariants between pointsets

對點對應關系的計算過程如下：

1）對坐標矩陣進行奇異值分解后，按式(16)依次遍歷Φ12的所有可能取值，由于Λw中前2個奇異值較為相近，因此需要根據 2.4節方法來計算可能的旋轉角度。對于Φ12=diag(1,1)，有 209.0°、120.5°、29.5°、296.8°等4種可能；對于Φ12= d iag(1,-1)，有 286.7°、15.2°、106.2°、198.9°等 4 種可能。對這 8 種可能取值，分別計算距離矩陣D，保留滿足雙射條件和r33＞0的點對應關系，并記錄距離平均平方和ε(T·Φ12)。對于該幀圖像數據，8組不同的Φ12均能獲得滿足雙射條件的點對應關系。進一步利用r33＞0約束條件可剔除掉第5、6、7、8組點對應關系，保留的點對應關系所對應的距離平均平方和ε(T·Φ12)記錄如表4所示。

表4 可能的點對應關系向量與距離平均平方和Tab.4 Possible point correspondence vectors and MSEs

3）從表4可以看出，第3組的距離平均平方和最小，然而其他幾組數據的距離平均平方和與最小值也較為相近，可判斷特征點具有旋轉對稱特性。可保留上述4組解作為點對應關系最終的可能解。這4組解與“特征點的旋轉對稱角度為 90°”相一致。通過對比預設值可以看出，第2組可能解與預設的點對應關系向量一致。

為了驗證本文方法在奇異值相差較大情況下的有效性，選擇前7個特征點參與計算，并預設點對應關系向量為p=(3,1,6,5,7,4,2)，此時點集Zw、Zn對應的規范化奇異值分別為(0.5606,0.4394)和(0.5655,0.4345)。由于2個奇異值相差較大，只需要搜索T·Φ12的4種可能取值。對這4種可能取值進行遍歷，計算出滿足雙射條件和r33＞0約束條件的點對應關系只有1種，即p^ = (3,1,6,5,7,4,2)，其距離平均平方和為0.0303。可以看出結果與預設值一致，驗證了本文方法的有效性。

3.2 多幀圖像下不同點個數的計算結果分析

對多種相對位姿參數下的圖像進行特征點提取，共采集了17張圖像，部分圖像如圖1所示。這些圖像均通過人工標記的方式來確定亞像素精度的圖像特征點坐標，用于點對應關系計算以及相對位姿參數計算。

為了驗證本文點對應關系計算方法的穩定性，對不同特征點個數、不同相對位姿參數情況下的算法有效性進行檢驗，分別選擇8個與7個特征點進行試驗，以測試本文算法在有/無旋轉對稱分布情況下的性能。

圖1 多種相對位姿參數下的近紅外圖像Fig.1 Near-infrared images with various relative poses

1）選擇所有 8個特征點參與計算，隨機設置預設點對應關系向量，對每一張圖像進行100次隨機試驗，試驗結果顯示了每一次試驗均能找到點對應關系向量的4組可能解。通過與預設值進行比較，驗證了每一次試驗結果中某一組可能解與預設值相同。

2）考慮7個特征點的試驗。對于每張圖像，在8個特征點中隨機選擇7個特征點，一共進行100次隨機試驗，并隨機打亂圖像特征點的順序，每一次試驗結果顯示了點對應關系的唯一解，分析原因可知這是由于7個特征點的幾何分布不具備旋轉對稱的特點。進一步與預設值進行對比發現，每一次試驗結果給出的點對應關系向量與預設值完全一致。

距離平均平方和的均值和標準差度量了規范化空間下兩個點集匹配后的距離誤差分布情況，能夠間接說明算法匹配結果的有效性。如圖2所示，8個特征點對應的距離1σ最大值不超過0.045，而7個特征點情況下則不超過0.05，間接說明了本文方法每一次試驗都能夠獲得正確的點對應關系。

圖2 距離平均平方和均值標準差曲線Fig.2 Mean-Std curve of MSE

3.3 與其他算法的對比分析

選擇窮舉搜索算法、RANSAC搜索算法與本文算法進行對比，其中 RANSAC是一種隨機選擇算法，該算法可能找不到正確的點對應關系。以上述7個特征點的點對應關系計算問題為例，3種算法的搜索次數、1000次計算時間以及成功率如表5所示，其中計算時間是在i5-8250U計算機的MATLAB平臺下計算與記錄所得，RANSAC搜索次數分別設置為2500次、1000次和100次。

表5 與其他算法的對比Tab.5 Comparison to other methods

從表5可以看出，本文算法具有最少的搜索次數和計算耗時，同時本文算法在1000次計算過程中均找到了正確的點對應關系，其準確度與窮舉搜索方法一致，優于RANSAC隨機搜索算法。

以上試驗結果表明：本文方法能夠處理較遠距離光軸方向上特征點集較為密集場景下不同距離、不同特征點個數以及一般幾何分布情況時的特征點對應關系計算問題。相比窮舉搜索算法和 RANSAC搜索算法，本文方法在保證準確度的同時具有最少的搜索次數與計算量。

對于無旋轉對稱特點的場景，本文方法能夠準確可靠地獲得點對應關系的唯一解；而對于有旋轉對稱特點的場景，本文方法能夠準確可靠地獲得包含正確點對應關系的多個可能解，且可能解的個數等于360°除以旋轉對稱的最小角度。

4 結 論

針對視覺導航系統中點對應關系計算問題，提出了基于坐標矩陣奇異值分解的一般特征點對應關系計算方法，避免了特征點對應關系的指數級搜索或者在搜索特征點對應關系時需要人工設計較為復雜的匹配策略。試驗結果驗證了本文方法應用于較遠距離光軸方向上特征點集較為密集場景時具有較高的準確性及可靠性。