(19)国家知识产权局 (12)发明 专利申请 (10)申请公布号 (43)申请公布日 (21)申请 号 202211163166.6 (22)申请日 2022.09.23 (71)申请人 福州大学 地址 350108 福建省福州市闽侯县福州大 学城乌龙江北 大道2号福州大 学 (72)发明人 张卫波　温珍林　黄晓军　黄赐坤　 封士宇　 (74)专利代理 机构 福州元创专利商标代理有限 公司 35100 专利代理师 丘鸿超　蔡学俊 (51)Int.Cl. G01C 21/00(2006.01) G01C 21/16(2006.01) G01C 21/20(2006.01) G06T 7/277(2017.01)G06T 7/73(2017.01) (54)发明名称 基于拓展卡尔曼滤波的多传感器融合SLAM 方法 (57)摘要 本发明公开了一种基于拓展卡尔曼滤波的 多传感器融合SLAM方法， 针对走廊相似场景下， 激光SLAM算法由于观测数据的相似性， 算法性能 将严重劣化， 甚至完全失效的问题， 本发明将里 程计和IMU的数据进行预处理， 通过拓展卡尔曼 滤波将两者的位姿信息融合， 作为激光雷达扫描 匹配更精确的迭代初始位姿； 为验证本算法的性 能， 在Melodic版本的ROS(Robot  Operating   System)搭建了Gazebo仿真实验环境， 通过仿真 实验对比， 验证 了算法的鲁棒 性和有效性。 权利要求书4页 说明书10页 附图6页 CN 115435775 A 2022.12.06 CN 115435775 A 1.一种基于 拓展卡尔曼 滤波的多传感器融合SLAM方法， 其特 征在于， 包括以下步骤： 步骤S1： 读取IMU、 轮速计的数据信息并进行 预处理； 步骤S2： 采用拓展卡尔曼 滤波融合 IMU、 轮速计预处 理的位姿得到融合 位姿； 步骤S3： 激光雷达数据预处理， 将融合后的位姿为激光雷达数据做线性插值去运动畸 变； 步骤S4： 将融合处理之后的位姿作为激光雷达扫描匹配的迭代初始位姿， 开始SLAM前 端建立局部 子图； 步骤S5： 对于构建完成的子图判断是否形成回环， 进行SLAM后端回环检测以减少累计 误差， 并将当前时刻对应的位姿约束加入到非线性优化中； 步骤S6： 当没有新的激光雷达Scan产生， 输出全局优化后的轨迹位姿， 并将产生的全局 地图可视化。 2.根据权利要求1所述的基于拓展卡尔曼滤波的多传感器 融合SLAM方法， 其特征在于： 步骤S1具体包括： 对惯性测量单 元IMU预积分求得惯性里程计； IMU每个时刻进行自身3轴加速度和角速度的测量， 角速度 与加速度 的测量 模型分别为： 其中 为实际角速度， bg是角速度随时间缓慢变化的偏差bias, ηg是角速度白噪声， 为旋转矩阵， aw为实际加速度， gw表示重力矢量， w表示世界坐标系作为惯性系， ba是加速 度随时间变化的偏差， ηa是加速度白噪声； 运动方程 为： 公式中∧表示反对称矩阵； 在区间[t,t+Δt]里进行欧拉积分有： vw(t+Δt)＝vw(t)+aw(t)·Δt 为所求旋转矩阵， pw(t+Δt)为所求 位移， 从而求得惯性里程计； 计算轮速计数据求得轮边里程计信息： 通过轮速计得到的两轮角速度和两轮线速度求 解横向位移、 纵向位移和航向角； 根据运动模型计算出小车的速度v和角速度ω：权　利　要　求　书 1/4 页 2 CN 115435775 A 2公式vL、 vR分别表示左右轮的线速度， d为轮胎离底盘中心的距离； 对时间积分得到里程信息： θ(t)＝ ∫ω(t)dt x(t)＝ ∫v(t)cos( θ(t) )dt y(t)＝ ∫v(t)si n( θ(t))dt 式中θ(t)为 航行角， x(t)为横轴位移， y(t)为纵轴位移； 进行位姿数据融合前时间上和空间上的统一； 以激光雷达发布 的时间为基准进行时间上的统一： 当每一帧激光传感器数据到来时， 各传感器的时间取比基准时间小且离的最近的时间作为初始 时间， 当下一帧数据到来时， 取比基准时间大且最近的时间作为终止时间， 计算 这两个时刻的时间差作为积分时间； 将得到的数据进行坐标系的变换以得到空间上的统一： 根据各传感器的安装位姿， 利 用Eigen库进 行坐标系的转换， 得到变换的平移矩阵和旋转矩阵， 再将其他传感器坐标系统 一到小车的基座标系， W为基座标系， C为需要对齐的坐标系， X、 Y、 Z分别为空间坐标系下的 坐标； 变换公式为： 为了方便计算改写成增广矩阵： 公式中Rc为3*3的旋转矩阵， Tc为3*1的平移矩阵： 公式中RX( α )为绕X轴旋转α角度的旋转矩阵， 公式中RY(β )为绕Y轴旋转β角度的旋转矩 阵， 公式中RZ( θ )为绕Z轴旋转θ角度的旋转矩阵。 3.根据权利要求2所述的基于拓展卡尔曼滤波的多传感器 融合SLAM方法， 其特征在于： 步骤S2当中： 所述拓展卡尔曼滤波是标准卡尔曼滤波EKF在非线性情形下的一种扩展形式， 即， 将非 线性函数通过一阶泰勒展开近似线性函数， 然后采用卡尔曼滤波框架得出准确的下一时刻 状态真值；权　利　要　求　书 2/4 页 3 CN 115435775 A 3