(19)国家知识产权局 (12)发明 专利申请 (10)申请公布号 (43)申请公布日 (21)申请 号 202211041346.7 (22)申请日 2022.08.29 (71)申请人 江苏科技大学 地址 212100 江苏省镇江市丹徒区长晖路 666号 (72)发明人 章飞　张子菁　姬传堂　 (74)专利代理 机构 南京经纬专利商标代理有限 公司 32200 专利代理师 袁姝 (51)Int.Cl. G01C 21/20(2006.01) G06T 7/277(2017.01) G06T 7/73(2017.01) (54)发明名称 一种基于多伯努利滤波器的同时定位与建 图方法 (57)摘要 本发明公开一种基于多伯努利滤波器的同 时定位与建图方法， 具体步骤包括(1)参数初始 化； (2)获得输入数据； (3)获得机器人位置预测 值； (4)获得机器人的观测集； (5)通过势均衡多 伯努利滤波方法获得机器人在第k时刻用于表示 地图特征的伯努利项； (6)对获得伯努利项进行 目标提取； (7)记录k时刻得到的地图特征数目及 位姿； (8)通过自适应信息控制法， 判断是否执行 图优化过程； (9)通过图优化方法t 个时刻所对应 的机器人位姿， 然后执行步骤(2)； (10)判断是否 达到最大运行时刻数， 来决定是否输出机器人位 姿和地图特征的状态估计值。 本发明的方法， 改 善了同时定位与建图方法中的机器人位姿估计 精度， 提高了实时性。 权利要求书6页 说明书12页 附图1页 CN 115307645 A 2022.11.08 CN 115307645 A 1.一种基于多伯努利滤波器的同时定位与建图方法， 其特 征在于： 包括以下步骤： (1)对运行时刻数参数、 优化时刻数参数、 多伯努利存在参数、 最大运行时刻数参数进 行初始化； (2)通过机器人所携带的传感器获得机器人的运动速度v和方向角度θ、 地图特征与机 器人的直接距离d和方位角 所述传感器为惯导元件或激光雷达； (3)基于步骤(2)获得的机器人运行时的运动 速度v和方向角度θ， 通过机器人的运动方 程f(v, θ,k)计算获得第k时刻的机器人位姿预测值； (4)基于步骤(3)获得第k时刻的机器人位姿预测值， 以及基于步骤(2)的直接距离d和 方位角 通过观测方程 获得机器人在第k时刻所对应的观测集； (5)基于步骤(3)、 步骤(4)所获得的信息， 通过势均衡多伯努利滤波方法对地图特征进 行状态估计， 获得机器人在第k时刻用于表示 地图特征的伯努利项； (6)对步骤(5)中获得的伯努利 项， 根据其存在概率参数r的值来进行目标提取， 其结果 作为地图特征的状态估计值， 该状态估计值包括 地图特征的数目和地图特 征的位姿； (7)基于步骤(6)所获得的地 图特征的状态估计值， 记录k时刻所得到的地 图特征的数 目和地图特 征的位姿； (8)基于步骤(7)获得的地 图特征状态估计值， 通过自适应信息控制法， 判断先验信息 是否满足阈值， 当不满足， 则执 行步骤(2)， 令k ＝k+1， t＝t+1； 当满足， 则执 行步骤(9)； (9)基于步骤(8)， 当满足自适应信息控制法的条件， 则通过图优化方法对该t个时刻的 机器人位姿进行估计， 以更新该t个时刻所对应的机器人位姿， 令k＝k+1,t＝0， 执行步骤 (2)； (10)基于步骤(8)和步骤(9)的k， 判断是否达到最大运行时刻数， 当满足， 则完成最后 一次步骤(9)的图优化过程， 并输出机器人位姿和地图特征的状态估计值后结束， 否则执行 步骤(2)。 2.根据权利要求1所述的基于多伯努利滤波器的同时定位与建图方法， 其特征在于： 步 骤(1)中所述对运行时刻数参数、 优化时刻数参数、 多伯努利存在参数、 最大运行时刻数参 数进行初始化的方法和步骤是： (11)运行时刻数参数初始化， 令k ＝0； (12)优化时刻数参数初始化， 令t＝0； (13)多伯努利存在参数初始化， 令r＝0.9 9； (14)最大运行时刻数参数初始化， 令kmax＝500。 3.根据权利要求1所述的基于多伯努利滤波器的同时定位与建图方法， 其特征在于： 步 骤(2)中所述通过机器人所携带的传感器获得机器人的运动速度v和方向角度θ、 地图特征 与机器人的直接距离d和方位角 的方法和步骤是： (21)通过机器人 所携带的传感器获得机器人的运动速度v和方向角度 θ； (22)通过机器人 所携带的传感器获得地图特 征与机器人直接的距离d和方位角 4.根据权利要求1所述的基于多伯努利滤波器的同时定位与建图方法， 其特征在于： 步 骤(3)中所述 通过机器人的运动方程f(v, θ,k)来预测机器人的位姿的方法和步骤是： (31)以初始时刻即k ＝0的位姿作为原点；权　利　要　求　书 1/6 页 2 CN 115307645 A 2(32)以初始时刻的运动方向为y轴建立笛卡尔坐标系； (33)机器人运动方程f(v, θ,t)由公式(1)确定； 其中， vk表示机器人在第k时刻时的移动速度， θk表示机器人在第k时刻时的前进角度， Rk表示机器人运行 过程的过程噪声。 5.根据权利要求1所述的基于多伯努利滤波器的同时定位与建图方法， 其特征在于： 步 骤(4)中所述 通过观测方程 获得机器人在第k时刻所获得观测集的方法是由 观测方程 由公式(2)确定： 上式中， 是机器人的观测集， dk, 分别代表传感器观测到的地图特征与 移动机器人自身的距离和方向夹角， Xl和Yl分别是第l个 地图特征的X轴和Y轴坐 标， Rz,k是观 测噪声协方差矩阵。 6.根据权利要求1所述的基于多伯努利滤波器的同时定位与建图方法， 其特征在于： 步 骤(5)中所述通过势均衡多伯努利滤波 方法对地图特征进 行状态估计， 获得机器人在第k时 刻用于表示 地图特征的伯努利项 的方法和步骤是： (51)基于步骤(3)所获得机器人位姿预测值， 将机器人在时k时所观测范围内的地图特 征进行描述， 其形式采用随即有限集形式进行表示， 通过公式(3)获得其随机有限集模型； 其中， 表示机器人0到k时刻地图特征的随机有限集， Mk‑1， 表示机 器人0到k ‑1时刻地图特征的随机有限集， M表示整个地图特征的随机有限集； Xk表示机器人 在k时刻的机器人的位姿， 表示k时刻机器人新 生地图特征； (52)基于步骤(4)所获得机器人在第k时刻时对地图特征的观测集， 将其通过随即有限 集形式进行表示， 通过公式(4)获得其随机有限集模型； 其中， 集合Zk表示机器人在第k时刻观测集， Xk为机器人位姿， Dk(m,Xk)表示机器人在Xk 处对地图特 征的真实观测， Ck(Xk)表示机器人在其 位姿为Xk观测到的杂波误观测集； (53)通过条件贝叶斯公式， 构建同时定位与建图问题， 即机器 人在观测集合Zk条件下估 计地图特征Mk和机器人位姿X1:k的联合后验概率密度的过程； 该同时定位与建图问题通过 公式(5)进行表示； πk|k(Mk,X1:k|Z1:k,u1:k,X0)                       (5)权　利　要　求　书 2/6 页 3 CN 115307645 A 3