(19)国家知识产权局 (12)发明 专利申请 (10)申请公布号 (43)申请公布日 (21)申请 号 202211003293.X (22)申请日 2022.08.19 (71)申请人 杭州电子科技大 学 地址 310018 浙江省杭州市下沙高教园区2 号大街 申请人 国网河南省电力公司电力科 学研究 院　 国家电网有限公司 (72)发明人 屠铭扬　曾平良　吴秋轩　郭志民　 田杨阳　孟秦源　经韬　毛万登　 刘昊　库永恒　张波涛　袁少光　 刘善峰　梁允　 (74)专利代理 机构 杭州君度专利代理事务所 (特殊普通 合伙) 33240 专利代理师 朱月芬(51)Int.Cl. G01S 17/42(2006.01) G01S 17/86(2020.01) G01S 17/89(2020.01) G01C 22/00(2006.01) G06T 7/73(2017.01) G06T 7/246(2017.01) (54)发明名称 基于粒子滤波和图优化的机器人定位和建 图系统及方法 (57)摘要 本发明公开了基于粒子滤波和图优化的机 器人定位和建图系统及方法。 基于SIR滤波器进 行重采样策略优化， 使用最小采样方差重采样方 法代替简单判断权重大小的重采样方法， 有效提 高粒子多样性。 引入子图系统， 同时维护两张活 跃子图， 一帧激光数据与两张子图构建约束， 并 通过相关性扫描匹配构建回环约束， 进行后端优 化。 利用非线性最小二乘法最小化误差， 优化所 有历史位姿与子图位姿， 并在优化后的位姿集中 继续添加新位姿。 本发明使用粒子滤波器作为前 端里程计， 估计良好的位姿初值， 生成局部子图， 并结合图优化利用所有时刻信息最小化误差的 优势， 在满足实时性要求的前提下， 获得高精度 的位姿和栅格地图， 为后续变电站巡检工作提供 参考。 权利要求书5页 说明书7页 附图3页 CN 115407352 A 2022.11.29 CN 115407352 A 1.基于粒子滤波和图优化的机器人定位和建图系统， 其特征在于， 包括局部SLAM模块 和全局SLAM模块； 所述的局部SLAM模块包括粒子集合生成模块、 重采样模块和子图系统； 所述的全局 SLAM模块包括约束构建模块和位姿优化模块； 所述的粒子集合生成模块用于进行扫描匹配和粒子采样， 生成粒子集合， 其中每一个 粒子都会得到一个里程计位姿， 生成并携带一幅当前子图； 所述的重采样模块采用改进后的重采样方法， 粒子集合生成模块通过重采样模块不断 重采样直至粒子集的权 重离散程度高于设定阈值， 得到优化后的粒子集 合； 所述的子图系统用于同时维护两张活跃子图， 保持足够多的激光帧间位姿约束关系； 所述的约束构建模块， 使用CSM匹配与分支定界加速获得回环约束； 所述的位姿优化模块， 根据局部SLAM模块计算得到的里程计约束和约束构建模块得到 的回环约束构建图优化问题， 最小二乘求 解最优位姿和地图。 2.基于粒子滤波和图优化的机器人定位和建图方法， 其特 征在于， 步骤如下： 步骤1： 通过 粒子集合生成模块进行扫描匹配和粒子采样， 生成粒子集 合； 步骤2： 改进重采样， 不断重采样直至粒子集的权重离散程度高于设定阈值， 得到优化 后的粒子集 合； 步骤3： 通过子图系统完成子图维护； 步骤4： 回环检测， 使用CSM匹配与分支定界加速获得回环约束； 步骤5： 根据计算得到的里程计约束和约束构建模块得到的回环约束构建图优化问题， 最小二乘求 解最优位姿和地图。 3.根据权利要求2所述的基于粒子滤波和图优化的机器人定位和建图方法， 其特征在 于， 步骤1具体方法如下； 一帧新激光数据进入机器人定位和建图系统后， 机器人定位和建图系统初始化当前时 刻粒子群St， 循环历遍上一时刻的粒子群St‑1， 获取上一时刻粒子 的位姿 权重 和携带 子图 并 利 用轮 速 计的 运 动模型 状 态 传播 预 测t时 刻位 姿 其中ut‑1为t‑1时刻轮速计数据； 由于粒子 不唯一， 同一时刻将估 计出 数条轨迹， 每条轨迹的权重至关重要， 采用 表示该粒子的轨迹， 其重要性权重可定义为 式(3)： 其中， 分子为目标分布， 分母为建议分布， 每进行一次观测， 如计算完整轨迹权重， 则轨 迹的重要性权重计算量会随时间 增长 ； 在已 知粒子轨迹 的条件下 ， 通过 计算轨迹对应子图 并对其进行展开和化简， 可得到重要性权重递 归式(4)；权　利　要　求　书 1/5 页 2 CN 115407352 A 2当激光帧与现有地图匹配度高时， 观测的可靠性高， 观测分布L(i)区间小， 使用激光匹 配采样生成的粒子落在区间L(i)的数量更多， 能够用更少的粒子覆盖位姿的概 率分布； 当激光匹配度低时， 观测可靠性低， 则使用运动模型 采样生成 粒子； 观测可靠性通过极大似然估计地图匹配度判断， 首先在预测位姿 附近进行搜索， 计 算当前帧的观测zt与子图 的匹配度， 若匹配度超过设定阈值， 则认为观测可靠性高， 如式(5)所示： 若在预测位姿 周围搜索到匹配度超过阈值的极值点 极值点 即代表观测区 间L(i)中概率峰值区域， 以 为中心， Δ为半径随机采样固定数量的k个点{ x1， ...， xk}， 即 完成采样； 因激光匹配模型方差较小， 令采样粒子集服从高斯分布， 并结合激光观测和轮速计信 息， 计算其矢量均值 和协方差 如式(6)所示： 其中归一 化参数 T为矩阵转置符； 新的粒子点集 将从 中使用多元正态分布公式进行概率计算， 其权重中的建议分布π改进为 则新粒子对应的权重计算方 式为式(7)； 若通过式 (5) 计算不存在与地图匹配度超过阈值的 则使用运动模型 进行新粒子的采样， 建议分布π使用运动模型， 其重要性权重计算 方式为式(8)；权　利　要　求　书 2/5 页 3 CN 115407352 A 3