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

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

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

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

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

1.3 運(yùn)動(dòng)估計(jì)

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

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

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

小車t時(shí)刻位于位置A時(shí)，特征點(diǎn)P在坐標(biāo)系ΣBt為參考位置的三維坐標(biāo)是[x1],y1,z1T，當(dāng)經(jīng)過t+1時(shí)刻后移動(dòng)到空間位置B時(shí)，同一特征點(diǎn)P在以B為參考位置的三維坐標(biāo)為[x]t+1,yt+1,zt+1T，所以得到：

當(dāng)這些位置滿足3個(gè)及以上相同的匹配特征點(diǎn)時(shí)，小車所在坐標(biāo)系間的平移與旋轉(zhuǎn)向量就可以通過以上方程獲得。

2 車體的轉(zhuǎn)彎控制

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

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

2.2 彎道控制策略

2.2.1 控制策略

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