(19)国家知识产权局 (12)发明 专利申请 (10)申请公布号 (43)申请公布日 (21)申请 号 202210813501.6 (22)申请日 2022.07.11 (71)申请人 东北大学 地址 110819 辽宁省沈阳市和平区文化路 三巷11号 (72)发明人 张云洲　曹振中　田瑞　胡兴刚　 邓志强　孙文恺　 (74)专利代理 机构 大连理工大 学专利中心 21200 专利代理师 梅洪玉 (51)Int.Cl. G06T 7/73(2017.01) G06T 7/246(2017.01) G06T 17/05(2011.01) G01C 21/20(2006.01) (54)发明名称 基于二次曲面初始化和联合数据关联的物 体感知SLAM算法 (57)摘要 本发明属于物体感知算法领域， 提出了一种 基于二次曲面初始化和联合数据关联的物体感 知SLAM算法。 首先提取图像中的特征点、 平面和 物体信息； 然后根据特征点和平 面信息计算相机 位姿； 然后对图像的物体信息和地图中的物体信 息进行联合数据关联， 进而完成物体二次曲面参 数的高效初始化； 然后联合优化相机位姿和地图 中构建的物体二次曲面， 最终构建出完整的几何 语义地图； 本发 明提出的方法解决了以往二次曲 面级SLAM算法中存在的二次曲面初始化困难、 数 据关联不鲁棒等问题； 提高了物体 建模精度和相 机定位精度。 权利要求书3页 说明书6页 附图1页 CN 115239809 A 2022.10.25 CN 115239809 A 1.一种基于二次曲面初始化和联合数据关联的物体感知SLAM算法， 其特征在于， 首先 提取图像中的特征点、 平面信息和物体信息； 然后根据特征点和平 面信息计算相机位姿； 然 后对图像的物体信息和地图中的物体信息进 行联合数据关联， 进而完成物体二次 曲面参数 的高效初始化； 然后联合优化相 机位姿和地图中构建的物体二次曲面， 最终构建出完整的 几何语义 地图； 包括步骤如下： 步骤1： 输入RGB图像和深度图像； 步骤2： 特 征提取； 从RGB图像上提取特征点信息和语义物体信息； 从深度图像上提取几何平面信息； 语义 物体信息包括物体目标检测结果和物体表面稀疏 特征点； 步骤3： 位姿估计； 将当前帧数据提取到的特征点信息、 几何平面信息与包含点云、 平面和二次曲面的地 图中已经构建的数据进 行联合数据关联， 使用关联后的信息对相机位姿进 行基于3D信息到 2D信息的最小重投影误差的求 解； 步骤4： 高效二次曲面初始化； 从步骤2中得到的物体目标检测结果和物体表面稀疏特征点出发构建物体表面的切 平 面， 即物体的包络平面； 每个包络平面为物体二次曲面参数求解提供一个线性约束， 基于得到的所有包络平 面， 构建出对应的线性方程组； 将待求解的二次曲面 参数问题转换为线性 最小二乘问题； 步骤5： 基于g2o技术的多约束联合优化因子图； 结合几何平面信息和语义物体信息， 利 用地图中构建的点云、 平面和二次曲面在图像平面上投影的特征点、 区域和 二次曲线的误 差对相机位姿以及构建的物体二次曲面进行基于g2o非线性优化 技术的联合优化； 步骤6： 算法输出； 输出包 含平面和二次曲面的几何语义 地图。 2.根据权利要求1所述的基于二次曲面初始化和联合数据关联的物体感知SLAM算法， 其特征在于， 所述联合数据关联从几何和概率的双角度出发， 采用四种关联距离进行鲁棒 关联当前帧检测的物体和地图已经构建的物体； 四种关联距离 分别为图像2D ‑IoU关联距离 二次曲面投影IoU关联距离 地图点投影频数关联距离 和非参数概率关联距离 基于以上四种关联距离， 设定当前帧存在M个目标检测框， 地图中存在N个二次曲面， 构 建矩阵AM×N＝[aij]M×N来表示当前帧目标检测框与地图中二次曲面的数据关联距离矩阵， 其 中aij表示第i个检测框和第j个物体的关联距离(0≤aij≤1)， 采用上述不同层面的关联距 离加权求和来计算矩阵A的值； 其中， kq+kw+ke+kr＝1， 是各项关联距离的权 重值。 3.根据权利要求2所述的基于二次曲面初始化和联合数据关联的物体感知SLAM算法， 其特征在于， 所述图像2D ‑IoU关联距离具体为； 根据上一帧所关联的二次曲面得到当前帧目标检测与二次曲面的关联结果； 记 为第权　利　要　求　书 1/3 页 2 CN 115239809 A 2i个检测框和第j个二次曲面的2D图像平面IoU关联距离 表示当前帧第i 个检测框的边界框， 为第j个二次曲面对应的上一帧中的检测框的边界框； 4.根据权利要求2所述的基于二次曲面初始化和联合数据关联的物体感知SLAM算法， 其特征在于， 所述 二次曲面投影I oU关联距离具体为； 将地图中已经构建的二次曲面投影到当前帧， 得到投影的检测框， 然后和当前帧的检 测框计算IoU； 记 为第i个检测框和第j个二次曲面的投影IoU关联距离 表示当前帧第i个检测框的边界框， 为第j个二次曲面投影到当前帧的检测框的边界 框； 5.根据权利要求2所述的基于二次曲面初始化和联合数据关联的物体感知SLAM算法， 其特征在于， 所述 地图点投影频 数关联距离具体为； 投影不同的物体所关联的地图点到当前帧的图像平面上， 计算和当前帧每一个检测框 所包含的物体地图点频数； 记 为第i个检测框和第j个二次曲面的投影地图点频数关联 距离 Numij表示当前帧第i个检测框所包含的第j个二次曲面关联的地图点投 影的频数； 6.根据权利要求2所述的基于二次曲面初始化和联合数据关联的物体感知SLAM算法， 其特征在于， 所述非参数概 率关联距离具体为； 从概率分布的角度出发， 得出目标检测结果和地图中路标的数据关联的后验概率正比 于狄利克 雷过程先验概 率、 目标检测类别测量似然概 率以及路标测量似然概 率的乘积； 记 为第i个检测框和第j个二次曲面的非参数概率关联距离 DP_prior (Oj)表示属于第j个二次曲面的狄利克雷过程先验概率， Label_likelihood(Di)表示第i个 检测框的类别似然概率， Pos_ likelihood(Di,Oj)表示第i个检测框和第j个 二次曲面的位置 似然概率， 则 7.根据权利要求2所述的基于二次曲面初始化和联合数据关联的物体感知SLAM算法， 其特征在于， 所述各项关联距离的权 重值， 分别为 kq＝0.2， kw＝0.2， ke＝0.3， kr＝0.3。权　利　要　求　书 2/3 页 3 CN 115239809 A 3