機器人研究的核心就是：導航定位、路徑規劃、避障、多傳感器融合。定位技術有幾種，不關心，只關心視覺的。視覺技術用到“眼睛”可以分為：單目，雙目，多目、RGB-D，后三種可以使圖像有深度，這些眼睛亦可稱為VO（視覺里程計：單目or立體），維基百科給出的介紹：在機器人和計算機視覺問題中，視覺里程計就是一個通過分析處理相關圖像序列來確定機器人的位置和姿態。

當今,由于數字圖像處理和計算機視覺技術的迅速發展,越來越多的研究者采用攝像機作為全自主用移動機器人的感知傳感器。這主要是因為原來的超聲或紅外傳感器感知信息量有限,魯棒性差,而視覺系統則可以彌補這些缺點。而現實世界是三維的，而投射于攝像鏡頭（CCD/CMOS）上的圖像則是二維的，視覺處理的最終目的就是要從感知到的二維圖像中提取有關的三維世界信息。

系統基本組成：CCD、PCI、PC及其外設等。

CCD/CMOS

一行硅成像元素，在一個襯底上配置光敏元件和電荷轉移器件，通過電荷的依次轉移，將多個象素的視頻信號分時、順序地取出來，如面陣CCD傳感器采集的圖像的分辨率可以從32×32到1024×1024像素等。

視頻數字信號處理器

圖像信號一般是二維信號，一幅圖像通常由512×512個像素組成(當然有時也有256×256，或者1024×1024個像素)，每個像素有256級灰度，或者是3×8bit，紅黃蘭16M種顏色，一幅圖像就有256KB或者768KB(對于彩色)個數據。為了完成視覺處理的傳感、預處理、分割、描述、識別和解釋，上述前幾項主要完成的數學運算可歸納為：

(1)點處理常用于對比度增強、密度非線性較正、閾值處理、偽彩色處理等。每個像素的輸入數據經過一定關系映射成像素的輸出數據，例如對數變換可實現暗區對比度擴張。

(2)二維卷積的運算常用于圖像平滑、尖銳化、輪廓增強、空間濾波、標準模板匹配計算等。若用M×M卷積核矩陣對整幅圖像進行卷積時，要得到每個像素的輸出結果就需要作M2次乘法和(M2-1)次加法，由于圖像像素一般很多，即使用較小的卷積和，也需要進行大量的乘加運算和訪問存儲器。

(3)二維正交變換常用二維正交變換有FFT、Walsh、Haar和K-L變換等，常用于圖像增強、復原、二維濾波、數據壓縮等。

(4)坐標變換常用于圖像的放大縮小、旋轉、移動、配準、幾何校正和由攝影值重建圖像等。

(5)統計量計算如計算密度直方圖分布、平均值和協方差矩陣等。在進行直方圖均衡器化、面積計算、分類和K-L變換時，常常要進行這些統計量計算。

視覺導航定位系統的工作原理

簡單說來就是對機器人周邊的環境進行光學處理，先用攝像頭進行圖像信息采集，將采集的信息進行壓縮，然后將它反饋到一個由神經網絡和統計學方法構成的學習子系統，再由學習子系統將采集到的圖像信息和機器人的實際位置聯系起來，完成機器人的自主導航定位功能。

1）攝像頭標定算法：2D-3D映射求參。

傳統攝像機標定主要有 Faugeras 標定法、Tscai 兩步法、直接線性變換方法、張正友平面標定法和 Weng迭代法。自標定包括基于 Kruppa 方程自標定法、分層逐步自標定法、基于絕對二次曲面的自標定法和 Pollefeys 的模約束法。視覺標定有馬頌德的三正交平移法、李華的平面正交標定法和 Hartley 旋轉求內參數標定法。

2）機器視覺與圖像處理：

a.預處理：灰化、降噪、濾波、二值化、邊緣檢測。。。

b.特征提取：特征空間到參數空間映射。算法有HOUGH、SIFT、SURF。

c.圖像分割：RGB-HIS。

d.圖像描述識別

3）定位算法：基于濾波器的定位算法主要有KF、SEIF、PF、EKF、UKF等。

也可以使用單目視覺和里程計融合的方法。以里程計讀數作為輔助信息,利用三角法計算特征點在當前機器人坐標系中的坐標位置,這里的三維坐標計算需要在延遲一個時間步的基礎上進行。根據特征點在當前攝像頭坐標系中的三維坐標以及它在地圖中的世界坐標,來估計攝像頭在世界坐標系中的位姿。這種降低了傳感器成本,消除了里程計的累積誤差,使得定位的結果更加精確。此外,相對于立體視覺中攝像機間的標定,這種方法只需對攝像機內參數進行標定,提高了系統的效率。

定位算法基本過程：

簡單的算法過程，可基于OpenCV進行簡單實現。

輸入

通過攝像頭獲取的視頻流（主要為灰度圖像，stereo VO中圖像既可以是彩色的，也可以是灰度的 ），記錄攝像頭在t和t+1時刻獲得的圖像為It和It+1，相機的內參,通過相機標定獲得，可以通過matlab或者opencv計算為固定量。

輸出

計算每一幀相機的位置+姿態

基本過程

● 獲得圖像It,It+1

● 對獲得圖像進行畸變處理

●通過FAST算法對圖像It進行特征檢測，通過KLT算法跟蹤這些特征到圖像It+1中，如果跟蹤特征有所丟失，特征數小于某個閾值，則重新進行特征檢測

● 通過帶RANSAC的5點算法來估計兩幅圖像的本質矩陣

● 通過計算的本質矩陣進行估計R,t

● 對尺度信息進行估計，最終確定旋轉矩陣和平移向量