人總是對未知充滿了好奇，人的一生是不斷髮現，不斷探索未知的過程。哥倫布發現新大陸，月球探索，火星探索，人類的腳步將永不停止。探索是一個充滿未知的旅程，人類根據自身的知識，藉助外部工具，利用自己的智慧，發現或者發明新的事物，這是一個非常有意思的過程。

　　不識廬山真面目，只緣身在此山中，如何克服認知的侷限性，擴大我們的視野是一個很重要的問題。為了實現定位導航，人類發明了GPS，北斗導航，我們姑且認為是一種外部定位的技術，北斗為我們提供了外部座標參考。對於未知環境，我們希望機器人能夠幫助我們完成探索，這是一個很複雜的過程。而機器人在某些環境下無法獲得外部參考，只能依賴於自身進行定位，同時需要感知周圍的環境，為自身的活動規劃路徑。這裡涉及了兩個問題，一個是定位，一個是環境感知（構建地圖），這是SLAM的基本問題。

　　小豆包是本文的主人公，它是一個機器人。

　　星球大戰裡面有2個非常出色的機器人一個是3PO，一個是R2。我們也給這個機器人取了個名字，叫小豆包，代號FR2。不許侵犯它的署名哦。

　　小豆包是個多好的孩子啊，外在熱情開朗，充滿了好奇心和求知慾。這不，他開始了自己的SLAM之旅。

小豆包的新裝備

　　小豆包這下高興啦，因為它終於從研究人員的設計圖紙中走出來了，成為了一個真實的存在，一個有意義的存在。一身裝備也算是價格不菲了，先鋒Pioneer3-AT，帶工控i5處理器，8G記憶體，120G硬碟。北陽鐳射UTM-LX30，Kinect2.0深度相機。這樣小豆包算是煥然一新了，大腦可以運轉，眼睛一直撲靈撲靈的眨著。多討人喜歡啊。

　　可是問題來了，只是有了大腦和眼睛，小豆包仍然是白紙一張，什麼也幹不了啊。“你動動啊，你自己動啊。”小豆包也傻了，大哥（水哥），你讓我怎麼動啊！

　　想讓機器人動，必須給機器人傳送指令。如果想實現機器人的自主運動，則需要機器人感知周圍的環境，同時規劃運動路徑。SLAM問題成為小豆包亟待解決的問題。

機器人移動平臺Pioneer3-AT

　　小豆包是四輪驅動，運動控制是機器人的最基本的功能了。移動平臺車的基本硬體結構，簡單來說分為三部分：

（1）底層的硬體及各類感測器，包括電機、聲吶、鐳射
（2）微控制器，ArCOS，控制底層的各種硬體，同時負責與上位機進行通訊
（3）上位機，可以是工控計算機或者是普通的PC

　　（2）和（3）通過232協議——串列埠通訊(COM)或者網口進行連線；

　　小豆包主要基於Windows7作業系統，也支援ROS機器人作業系統。先鋒機器人提供了很詳細的開發類庫SDK，很容易上手。

小豆包的眼睛（感測器）

　　北陽鐳射Hokuyo UTM-30LX，12VDC直流供電，測距30m。

　　深度相機Kinect V2.0

幾個座標系

(1) 地圖座標系O-XYZ：全域性座標系

(2) 機器人中心座標系O'-X'Y'Z'：區域性座標系，一般以機器人的Heading方向作為O'-X'的方向。

(3) 感測器測量中心座標系o-xyz：區域性座標系，鐳射測量的距離是相對於該座標系的中心，鐳射的旋轉是相對於o-x軸。

感測器安裝在機器人上，存在著偏移$(x_{trans} y_{trans})$。一般o-x和機器人的Heading方向一致。

機器人運動的位姿是相對於地圖座標系的$(x,y,\theta)^T$.

不確定性

　　機器人對周圍環境的感知依賴於硬體裝置，而不管是輪編碼器，IMU或者是鐳射測距掃描器，測量過程中都會產生誤差。隨著機器人的不斷運動，誤差也會傳播和累積。比如里程計模型，單次測量誤差可能很小，但是機器人不斷運動，如果輪子再打滑，誤差累積會越來越大。