对视觉导航系统进行处理，主要包括三部分：
1.图像处理（获取双目视觉图像，标定相机参数，对图像进行矫正）
2.三维重建（双目摄像头获取图片，通过立体匹配建立两个图像之间的关系，使用Matlab软件进行三维重建仿真）
3.导航仿真（使用SLAM算法进行仿真，分为两种不同算法对比仿真）
1.图像处理
使用 张正友标定法（原理基础）
内参标定模型
采集图像
使用“谷客HD91S”双目相机采集棋盘图像，存储到电脑中
利用MATLAB标定工具箱对摄像机的内部参数进行标定，采集靶标图像，在棋盘格靶标中选定一个区域，对每一幅靶标图片进行角点提取，分别计算出左右摄像机的内部参数。
首先输入标定板的长和宽均为28mm，将选择的图片都在MATLAB的窗口中显示出来，然后在操作面板中进行角点的提取。最终将提取出角点的图像在标定工具箱操作面板点击“Calibration”键，完成摄像机的内参标定。
完成内参的标定后，可以得到各畸变参数，随后对图像进行畸变校正，重新提取网格角点。可以看到图像有了很大的改善。
分别标定机器人系统中的左右两个摄像机的参数，按照畸变参数对图像进行处理，可到到消除畸变后的图像，并且确定两个摄像头相对于棋盘的距离和位置。
2三维重建
双目视觉系统理论基础
三维重建技术的模型结构如下图18所示，对于一个空间中的物体或者其表面的任意一点 P ，通过C_1摄像机进行拍摄得到的图片对应点为P_1，但没办法由P_1确定P的位置坐标。在O_1 P连线上任意一点P^'在所拍摄的照片中对应点都是P_1。所以通过 P_1点的相对位置，这样仅能够得到O_1 P_1 与O_2 P_2两条垂直线的相交点，即它们在这两个空间中的三维相对位置也是唯一可以确定的。
三维重建Matlab仿真
首先拍摄了一组人物图像，下面是原始图像和校正后得到的图像
对校正后的图像进行特征点的匹配，发现噪声过大，标注了150个明显特征点，再去除背景的部分特征点后得到较为清晰的三维图像。
根据对人像三维重建的效果可以分明显看出人与建筑物或者相机的距离，通过视差和三维重建原理推算出人与摄像机的距离，本次推算出1米，大致准确。
其次，对路面进行三维重建，拍摄图像，处理过程与前者类似。
地面的三维重建也比较成功。
3.导航算法研究
SLAM导航算法原理
建立机器人模型
EKF-SLAM
使用MTALAB对上述EKF-SLAM算法进行仿真，创建35个路标障碍物，17个位姿，在用户界面上创建环境特征和运动控制信息，如图4-6所示。使用一个三角形的机器人进行导航模拟，具体运行过程如图4-7所示，计算预测路标与实际路标距离的误差如图4-8所示。
FAST-SLAM
两种算法比较
看出FASTSLAM算法导航较为精确。
