梯度下降法在雙目視覺導航中的一個運用

廣東工業大學 袁文波 陳慶瑜 高洪兵 王欽若

1 引言

本文將討論的是一種基于雙目視覺且路線固定的視覺導航方式，如無人搬運機器人，無人快遞小車的導航。其導航特點是路線已知且相對固定，機器只需沿著設定好的路線前進就能達到目的地。

2 無人快遞車導航原理概述

要實現自動導航，那么這就要求機器必須知道自己的實時位姿與正確位姿的相對偏差。由于快遞小車的運行路線固定，我們可以事先人為控制讓安裝有雙目攝像頭的小車沿著正確的道路行走一遍，雙目攝像頭相對小車位置固定，并對前進路線環境進行拍攝，我們每一幀將有兩張照片，左攝像頭及右攝像頭所拍攝的照片，我們對兩張照片進行特征點匹配，本文選用的是SIFT特征點匹配方式，并將匹配上的特征點的特征向量及其在小車坐標系（一般選小車的中心點或者小車的某個頂點作為小車坐標系的原點，小車的正前基線、水平基線、豎直方向基線分別與小車坐標系的三個坐標軸平行）的坐標按照片先后順序進行存儲。雙目視覺確定匹配上的特征點在小車坐標系的坐標方法已經很成熟在這里將不再贅述。當小車在正常運作時，不斷拍攝前方環境，并在左右幀照片中進行特征點匹配，將匹配上的特征點在與之前保存的特征點匹配，選取匹配度最高的一幀作為此次調整的正確位姿。并根據匹配上的特征點的坐標計算出小車實時位姿與正確位姿的偏差，從而達到導航的目的。本文將要詳細闡述的是在小車正常運行時如何檢測小車實時位姿與正確位姿的偏差。

圖1 SIFT特征點匹配效果

3 實時位姿與正確位姿偏差計算

圖2 實時位姿與正確位姿偏差示意圖

當小車正常工作時，進行左右兩張照片特征點的匹配，及計算出匹配點相對小車坐標系的坐標。將匹配上的特征點與系統里事先掃描路線預存的特征點以幀為跨度進行匹配，選取匹配點數最高的一幀，接下來就是要根據這些匹配點在其各自坐標系的坐標求出這兩個坐標系的變換矩陣，有了變換矩陣也就知道了小車實際位姿與正確位姿的偏差。有了這個偏差便能對小車位姿進行調控。其實這就轉化成了一個數學問題，已知同一組點在兩個三維坐標系的坐標求兩三維坐標系之間的變換矩陣。首先我們來觀察一下，兩個空間三維直角坐標系能夠通過怎樣的變換使得兩個坐標系重合，很明顯我們可以先進行平移使得兩個坐標系原點重合，重合后分別先后繞三個坐標軸進行三次旋轉，一定能使兩坐標系重合。以下四個變換矩陣分別對應以上的四個步驟，平移，繞X軸旋轉，繞Y軸旋轉，繞Z軸旋轉。

最終變換矩陣R計算如下：

隨機選取五個點，在坐標坐標系1的坐標構成矩陣Y1則Y1矩陣如下,每列代表一個點的坐標x,y,z，添1為了計算方便。

則這五點在第二個坐標系的坐標計算公式如下：

實際情況是我們有了五個點在兩個坐標系的坐標，要求坐標系的變換矩陣,即求那六個參數，如果這些坐標都是精確的，那就是一個解方程的問題，但實際運用時測量都是有誤差的，本文介紹一種在利用梯度下降的方法在坐標有誤差的情況下求出精度符合要求的變換矩陣。我們假設xt=50，yt=-80zt=200，xr=3.1416/2，yr=3.1416/3，zr=3.1416/5帶入各變換矩陣，利用公式(7)求得該五點在坐標系二的坐標如下：

再以xt,yt,zt,xr,yr,zr為未知數通過公式(9)計算，

則在坐標系二的五點的坐標為包含xt,yt,zt,xr,yr,zr的表達式，再通過公式（10）計算Loss：

此時Loss為包含xt，yt，zt，xr，yr，zr的表達式，將Loss分別對xt,yt,zt,xr,yr,zr求偏導，利用梯度下降法，不斷迭代，使Loss下降到一個可以接受的范圍，則便可得到各個參數較為準確的值。之后再在Y2中加入隨機的正態分布誤差來模仿實際測量帶來的誤差，用同樣的方法得到各個參數較為準確的值。如下兩矩陣分別是以u=0，σ=1，u=0，σ=2，隨機生成的干擾數據。

noise將以如下形式加載到數據中：

表1所示反應了在不加噪聲及加不同噪聲時梯度下降法的計算精度。圖2為Loss，及其中一個參數xr在無噪聲情況下，隨著迭代次數增加而收斂的圖像，其余參數及在有噪聲情況下的收斂趨勢與下圖類似。

表1 梯度下降法求的結果

圖2 Loss及xr收斂圖