amcl算法原理

AMCL算法Ros代码结构：AmclNode::AmclNode()updatePoseFromServer//从参数服务器获取机器人初始位置requestMap//从参数服务器获取地图信息，初始化粒子滤波器handleMapMessagefreeMapDependentMemory//清空map_,pf_,odom_,laser_convertMap//保存载入的地图信息pf_alloc//创建粒子滤波器updatePoseFromServer//载入预设的均值和方差pf_init//初始化概率密度函数pf_kdtree_clearpf_pdf_gaussian_alloc//根据设置的均值和方差初始化概率密度函数For(sample_count)pf_pdf_gaussian_sample//根据概率密度函数设置粒子的采样位置pf_kdtree_insert//将采样的粒子插入kdtreepf_pdf_gaussian_free//清空概率密度函数pf_cluster_statspf_kdtree_cluster//对kdtree中的采样粒子进行聚类将粒子滤波器中的粒子集和粒子群集初始化为0For(sample_count)pf_kdtree_get_cluster//得到当前粒子所属的集群编号计算x,y,cos(),sin(),xx,xy,yx,yy的累计值For(cluster_count)对每个集群的累计值进行归一化，得到均值和方差对每个粒子集的累计值进行归一化，得到均值和方差pf_init_converged//将粒子滤波器和粒子集的收敛度置为0newAMCLOdomAMCLOdom：：SetModel//设置里程计模型参数newAMCLLaser//载入参数和地图map_AMCLLaser：：SetModelLikelihoodField//设置laser模型参数map_update_cspace//根据离障碍物的距离设置map_-cells的值laserReceivedtf订阅laser在base_footprint下的相对位置SetLaserPose//设置lasers_[idx]的laser_pose得到odom在base_footprint下的相对位置将当前odom的位置与上一次更新时odom的位置比较，大于设定阈值，则将更新标记置为trueUpdateAction//利用里程计模型和当前数据对pf_采样粒子的位置进行更新将laser数据转换到base_footprint下UpdateSensor//用laser模型和当前数据更新pf_pf_update_sensorLikelihoodFieldModelFor(sample_count)pf_vector_coord_addFor(range_count)计算laser映射到map中的位置，得到该位置离障碍物的距离根据距离得到一个权值，更新采样粒子的权重，并得到total_weight对粒子权重归一化更新w_slow和w_fastpf_update_resample//对粒子进行重采样分别获取当前粒子采样集和待存放新的采样集的sets指针遍历当前粒子采样集，得到权重积分列表清空待存放新的采样集For(max_samples)If(drand48()w_diff)随机产生一个位置作为粒子位置Else将当前粒子权值累计值小于随机数的粒子保留为新的粒子每个粒子权值相同，1/sample_count插入kdtreepf_cluster_statspf_kdtree_cluster//对kdtree中的采样粒子进行聚类将粒子滤波器中的粒子集和粒子群集初始化为0For(sample_count)pf_kdtree_get_cluster//得到当前粒子所属的集群编号计算x,y,cos(),sin(),xx,xy,yx,yy的累计值For(cluster_count)对每个集群的累计值进行归一化，得到均值和方差对每个粒子集的累计值进行归一化，得到均值和方差pf_update_converged//判断粒子滤波器是否收敛pf_get_cluster_stats//获取粒子集群的均值方差权重将权重最大的集群的均值作为估计值