1.基于激光雷达的移动机器人同步定位与地图构建方法,其特征在于,包括以下步骤:步骤1、点云匹配:当机器人在移动过程中获得来自激光雷达的扫描数据时,采用点到线的改进ICP配准算法,对机器人位姿进行精确估计;
步骤2、抽样:对上一代第i个粒子 估计得到当前时刻粒子 选取激光雷达匹配结果作为优化提议分布;
步骤3、更新权重:根据权重计算公式计算出每个粒子 的权值步骤4、重采样:当Neff
步骤5、更新地图:根据第i个粒子表示的移动机器人位姿和当前观测情况,对第i个粒子构建的地图进行更新。
2.如权利要求1所述的基于激光雷达的移动机器人同步定位与地图构建方法,其特征在于,所述步骤1中点到线的改进ICP配准算法的具体步骤为:步骤1.1、设置距离误差阈值δ;
步骤1.2、将当前帧的点云数据根据初始位姿投影到参考坐标系中;
步骤1.3、对于点云中的每一个点pi,在参考帧中找到与其最近的两个点 和步骤1.4、通过下式(4)计算配准误差:式(4)中,索引i和j分别代表点云p和q中的点,R和T分别为旋转和平移矩阵,ni为线段的法向量;
步骤1.5、当配准误差E小于误差阈值δ,停止配准迭代,否则返回步骤1.2。
3.如权利要求1所述的基于激光雷达的移动机器人同步定位与地图构建方法,其特征在于,所述步骤2中对上一代第i个粒子 估计得到当前时刻粒子 选取激光雷达匹配结果作为优化提议分布,优化提议分布如式(5)所示:式(5)中, 表示环境地图m和机器人轨迹x1:t=x1,x2,…,xt的联合后(i)
验概率密度, 表示利用里程计信息估计机器人位姿,p(zt|m ,xt)表示用传感器信息z1:t=z1,z2,…,zt‑1更新地图m, 表示用里程计信息和传感器信息z1:t=z1,z2,…,zt‑1更新地图m。
4.如权利要求1所述的基于激光雷达的移动机器人同步定位与地图构建方法,其特征在于,所述步骤3中根据权重计算公式式(6)计算出每个粒子 的权值式(6)中, 为前一时刻粒子权重, 表示用里程计信息和传感器信息z1:t=z1,z2,…,zt‑1更新地图m。
5.如权利要求1所述的基于激光雷达的移动机器人同步定位与地图构建方法,其特征在于,所述步骤4具体为:
步骤4.1、设定重采样门限阈值Nth,当有效粒子数 小于阈值Nth时,进行重采样步骤4.2,否则进行下一时刻的滤波过程,其中 表示归一化权值;
步骤4.2、对权重大于阈值wt的粒子进行重采样,将高权值粒子进行复制生成M个粒子,复制次数为粒子总数N与粒子权重的乘积取整floor(N*w),高权值粒子集记为 其中r=
1,2,…,M表示粒子个数;
步骤4.3、对权重小于阈值wt的粒子,进行随机重采样生成低权值粒子集记为 其中j=1,2,…,N‑M表示粒子个数;
步骤4.4、分别对步骤4.2和步骤4.3产生的两部分粒子进行排序,得到新的粒子集步骤4.5、对步骤4.4得到的新的粒子集进行权重归一化后对其进行后验均值估计,即得到粒子滤波估计的最终状态。