基于雙目立體視覺伺服的智能車室內彎道控制

假定參考圖像P的SIFT特征點集合為：FP ={FP (1),FP (2),-,FP (m)}，m為圖像P的特征點的個數(shù)；待匹配圖像Q的特征點的個數(shù)為N，SIFT特征點集合為FQ ={FQ (1)，F(xiàn)Q (2)，-,FQ (m)}。當在建立參考圖像中的特征點和待匹配圖像的特征篩選對應匹配關系時，應按照圖2所示的算法進行對稱性測試，只有當兩個匹配集中的對應點完全一致時，才視為有效匹配。

SIFT特征向量生成后，利用特征點向量的歐式距離來作為兩幅圖像中特征點的相似性判定度量。最近鄰法是一種有效的為每個特征點尋找匹配點的方法。

最近鄰點被定義為與特征點的不變描述子向量之間的歐氏距離最短的點。對特征點集合FP中的每個點，逐一計算其與特征點集合FQ中每個點的距離，得到特征點之間的距離集合D。將距離集合D中的元素進行排序，得到最近鄰距離dmin和次近鄰距離dn–nim。SIFT算法通過判斷最近鄰和次近鄰距離的比值：

來區(qū)分正確匹配對和錯誤匹配對。

對于正確的匹配對，其最近鄰距離dmin要遠遠小于次近鄰距離dn-nim，即DistanceRatio-1；而錯誤的匹配對，由于特征空間的維數(shù)很高，其最近鄰距離dmin與次近鄰距離dn-nim差距不大，即DistanceRatio≈1。所以可取一個距離比閾值Threh∈(0,1)來區(qū)分正確匹配對和錯誤匹配對。

1.3 運動估計

進行特征點立體匹配和跟蹤匹配如圖2所示，對當前時刻t獲得圖像對(ImgL1和ImgL2)進行SIFT特征匹配之后獲得道路環(huán)境匹配特征點Ni個，再對相鄰時刻t+1獲得的Ni+1個特征點計算歐式距離，尋找t與t+1時刻獲取圖像中相同的特征點，得到一系列匹配點在車體運動前后的三維坐標。

本文實驗環(huán)境是在室內，在假定室內環(huán)境為理想的水平面的基礎上，同一特征點三維坐標中y坐標保持基本不變，即車體運動參數(shù)僅在x-z坐標平面內發(fā)生變化。

圖3中點P(x1,y1,z1)為所提取環(huán)境特征點，在t時刻立體匹配后得到其在攝像頭坐標系下的三維坐標，由于攝像頭固定安裝在小車上，因此通過坐標系的旋轉與平移變換可將點P的三維坐標轉換到小車坐標系ΣBt下表示。同理，t+1時刻所獲取的圖像中相同點P的(u,v)二維坐標發(fā)生變化，經坐標轉換后可得到小車坐標系ΣBt+1下表示。

小車t時刻位于位置A時，特征點P在坐標系ΣBt為參考位置的三維坐標是[x1],y1,z1T，當經過t+1時刻后移動到空間位置B時，同一特征點P在以B為參考位置的三維坐標為[x]t+1,yt+1,zt+1T，所以得到：

當這些位置滿足3個及以上相同的匹配特征點時，小車所在坐標系間的平移與旋轉向量就可以通過以上方程獲得。

2 車體的轉彎控制

2.1 車體定位參數(shù)計算

環(huán)境特征點投影到車體坐標系下表示后，即為所有的環(huán)境坐標均統(tǒng)一在世界坐標系下的表示。如圖4所示，其中小車前進方向為zw軸，xw水平垂直于zw，方向向右，其交點為原點Ow。直線為lmid為道路中間線，lmid與zw的夾角為φ，即為車輛的偏航角。點Ow到左車道線lL的距離為DL，到右車道線lR的距離為DR，可行駛區(qū)域的道路寬度為w = DL+DR。

2.2 彎道控制策略

2.2.1 控制策略

機器人的底層運動控制是非常重要的一個環(huán)節(jié)，機器人運動控制的好壞直接決定了能否有效執(zhí)行決策意圖，準確無誤地沿著預定軌跡行進，完成導航任務。傳統(tǒng)的定位控制中，PI、PID是廣泛采用的控制方法，這些方法比較成熟，但卻具有一定的局限性。對被控系統(tǒng)的參數(shù)變化比較敏感，難以克服系統(tǒng)中非線性因素的影響。本文采用BP神經網(wǎng)絡控制器在行使過程中經學習對網(wǎng)絡進行訓練，以適應當前的環(huán)境路況來調整車的左右輪速控制量。智能車左右輪速的大小，和車體偏離道路中間線lmid的距離d和車運動時方向偏離中間線的角度φ有關。小車控制輸入為d,φ，輸出為VL,VR。