PT0003_对SLAM的理解-----计算机学习实战.ppt

* * * 对SLAM的理解 About SLAM Simultaneous Localization and Mapping SLAM is that a mobile robot makes self localization and incremental mapping in the known or completely unknown environment by measuring on-line and position estimating using sensors installed on the robot.That is to say robot navigates itself using the incremental map. 在理想情况下，忽略噪声的影响，系统运动模型和观测模型。Xtruek是机器人在k时刻的位姿，zok是k时刻的观测量,控制量 。 观测模型 运动模型 路标点 为了模拟真实的环境，往往将噪声和误差视为服从均值为零的高斯分布，叠加在控制量uk和观测量zk上。 . 1.预测 根据系统运动模型 ，得到xvk|k-1 的均 值和协方差为（ F*表示F在*处的雅克比矩阵）： 因为机器人的状态向量与控制量相互独立，所以 pxvk-1uk=pukxvk-1=0，因此协方差表达式可简化为 由于所有的路标点为静止的，在预测阶段，只有机 器人的状态向量发生变化，特征点与机器人之间的 互协方差会因机器人位姿的变化而随之改变: 最后整个状态向量对应的协方差为： 2.路标点的提取 确定一个范围排除那些不希望出现的点，这个范围用bounding box来表示； 判断路标点是否落在激光扫描器扫描的范围内，右图用一个 圆形区域表示； （3）最后判断机器人能否观测 到路标点。机器人的观测范 围在图 中用浅棕色的半圆 表示。 3.数据关联 1>SLAM 数据关联是在机器人移动过程中，依据对周围环 境特征的观测，将每个观测量值与系统中已有特征相比较， 若为新特征，则增加到地图中；若为已存特征，则利用新 观测量信息修正相应特征位置，更新地图。 2>过程：首先将传感器送过来的观测进行门限过滤，利用 先验统计知识过滤掉那些门限以外的所不希望的观测。在 该关联门的输出形成可行或有效路标点和观测值值对，然 后形成关联矩阵，用以度量各个路标点和观测值的接近的 程度，最后将最接近预测值的观测值与路标点匹配。 图3.1 数据关联的过程 3>方法：数据关联的方法有最近邻数据关联、概率数 据关联、联合数据关联等。其中最近邻数据关联是一 种最简单的数据算法。 1>>最近邻数据算法选择落在相关跟踪门之内的观测 点，且与该观测点位置最近的目标点作为关联的对象。 跟踪门的确定依靠新息向量的加权范数或规化新息平 方： nis = vTS?1v，v、S分别表示新息和新息的协方差 （ V=zk-zk|k1 ）。当观测量和目标源的nis 小于某个门 限时，视观测点落在了跟踪门内。然后在所有的目标 点中，找出与跟踪门内观测点位置(规范化距离:nd = nis + log|S|) 最近的目标点。该目标点就是当前观测量 要关联的对象。 2>>概率数据关联的运用条件是是单个目标源。概率数 据关联认为只要在关联门内的观测值就都可能源于某一 个目标，它把门内所有测量值的加权平均作为关联对象， 各测量值对应的概率就是加权系数。 3>>联合概率数据关联是概率数据关联的一种延伸，它能 用于多个目标点的情况。与概率数据关联不同的是：目 标关联概率的测量值的计算是多个目标点联合计算出来 的。 4.匹配 当观测量和目标点的对应关系已知时，将这种对应的关 系存放于更新列表中。当前的观测量分成两类：一是在 地图中存在与当前观测量对应的目标点，将此观测量加 入到已有特征点(old feature)行列；另一种视为不存在与 之有对应关系的目标点，将这类观测量加入到新特征点 (new feature) 中。 5.更新 假设经过数据关联后，某个观测量zk与状态向量Xk|k-1 中第i个特征点mi =[mix miy]T有对应关系。由观测模型求 出特征点的预测值： 1>由观测值和预测值之差求得新息vk=zk-h(xk|k-1), 其中zk|k?1=h(xk|k?1)； 2>由观测模型计算出h(·)关于机器人状态和第i个特征点 的雅克比矩阵： * * *