《移动机器人原理与技术》_第六章.pptVIP

****************************6.7.3ORB-SLAM3算法ORB-SLAM3结构图*******************************6.5移动机器人卡尔曼滤波SLAM算法步骤2：应用传感器获得k时刻的对环境特征的实际观测值Yk；步骤3：根据移动机器人位姿预测与实际观测计算得到观测预测；步骤4：计算新息和增益Kk；步骤5：状态更新；6.5移动机器人卡尔曼滤波SLAM算法澳大利亚机器人中心提供了“CarParkDataset”标准数据集，用以进行EKF-SLAM算法实验。该数据集可以在其网站.au下载得到。实验用的智能车上安装了GPS、激光雷达和惯导三种传感器，惯导测量车辆左后轮线速度及车辆舵角，激光雷达则测量路标的距离和方向，智能车在运动过程中的经纬度通过GPS记录。“CarParkDataset”数据集纪录了智能车在实验场地东北部45m*30m的区域中行驶约2分钟的过程中各传感器的测量值。数据由三部分组成，一部分是智能车在运动过程中6.5移动机器人卡尔曼滤波SLAM算法的GPS信息，记录智能车的运动路径，虽然有一定的误差，一般仍被作为从总体上对SLAM算法的性能进行评估的参考标准。第二部分是惯导传感器在各个时刻的测量值，代入智能车的运动方程则可预测智能车在下一个时刻的位姿，第三部分是激光雷达在各个时刻对人工路标的观测数据，通过提取观测特征并进行数据关联，最终根据观测数据值和数据关联结果修正地图估计和智能车的位姿估计。6.5移动机器人卡尔曼滤波SLAM算法“CarParkDataset”数据集的EKF-SLAM结果6.6移动机器人粒子滤波SLAM算法6.6.1蒙特卡罗采样对于要解决的问题，蒙特卡罗方法首先建立一个概率模型，使概率模型的参数等于问题的解，所求参数的统计特征通过对模型的观察或抽样试验来计算，最后给出所求解的近似值。蒙特卡罗方法通过随机试验求解积分问题，将服从分布密度函数p(r)的随机变量g(r)的数学期望E(g(r))作为所要求解的积分：6.6.1蒙特卡罗采样从分布密度函数p(r)中通过某种试验获得采样N个观测值和N个相应的随机变量值g(ri)i=1,...,N的算术平均值作为积分的估计值:当采样点N足够大时，根据大数定律，趋于数学期望E(g)。6.6.2重要性采样蒙特卡罗采样需要从后验概率分布p(r)中采样N个点，对于移动机器人系统，移动机器人位置估计E(g(X0:k))可以表示为：在移动机器人系统中，系统的后验分布密度函数无法直接获得，对系统状态的估计无法通过蒙特卡罗方法采样后验分布密度函数而进行。但可以对系统中一个已知的并且容易采样的概率分布函数进行采样，该概率分布函数被称为重要性采样分布或者重要性函数。通过对由重要性函数的采样点进行加权来逼近：6.6.2重要性采样具体推导如下：6.6.2重要性采样称为未归一化的重要性权值。另外，可以表示为：6.6.2重要性采样如果能够从重要性函数式采样到样本点，则通过蒙特卡罗采样后的表示为：称为归一化权值。6.6.3序列重要性采样重要性函数可以分解为：实际动态系统服从一阶马尔可夫过程并且满足系统观测独立，其数学表达式可以分别描述为：p(X0)表示该动态系统的初始先验密度。6.6.3序列重要性采样如果重要性分布函数只依赖于前一状态Xk-1和当前的观测值Zk，即上式的重要性采样函数采样得到的样本，样本的最优的选择方法是：当前时刻的粒子由前一时刻粒子Xk-1和当前的观测值Zk采样得到，这种采样方法称为序列重要性采样。将一定的权值wik赋予给每一个样本Xik，则带权重的粒子集表示Xk的概率分布，形式如下：6.6.3序列重要性采样可以近似地表达为：式中，是狄拉克函数。6.6.4退化问题与重采样序列重要性采样算法中，当前时刻的粒子权值是由上一时刻的粒子权值递推得到，存在误差的权值的误差会随着时间的传播而进一步的积累，导致只有少数粒子的权值比较大，而大多数的粒子因权值太小以至于最终被忽略不计，即序列重要性采