(19)国家知识产权局 (12)发明 专利申请 (10)申请公布号 (43)申请公布日 (21)申请 号 202211326333.4 (22)申请日 2022.10.27 (71)申请人 联友智连科技有限公司 地址 510800 广东省广州市花都区风神大 道15号1栋一楼1 17房 (72)发明人 谢浪雄　 (74)专利代理 机构 深圳市顺天达专利商标代理 有限公司 4 4217 专利代理师 高占元 (51)Int.Cl. G01C 21/00(2006.01) G01C 21/32(2006.01) G06T 7/73(2017.01) G06T 7/80(2017.01) (54)发明名称 一种低成本记忆泊车建图与定位方法、 装置 及电子设备 (57)摘要 本发明公开了一种低成本记忆泊车建图与 定位方法、 装置及电子设备， 涉及记忆泊车定位 技术领域。 本发明包括： 基于车身轮速里程计输 出的后轴轮速脉冲数据进行轮速里程计计算， 将 轮速里程计数据与前视鱼眼数据进行时间同步； 通过前视鱼眼图片数据构建SLAM稀疏点 云地图， 并利用部分的关键帧轨迹和轮速里程计轨迹对 稀疏点云地图进行尺度恢复， 建图过程利用车身 轮速里程计信息进行误回环检测， 提升建图鲁棒 性， 利用SLAM定位位姿与轮速里程计进行融合定 位， 在保证定位精度的同时， 提高定位频率。 上述 方法只利用了环视中的前视鱼眼和车身轮速里 程计两个常规传感器， 并实现了1公里 以上的记 忆距离， 具有成本低、 实现简单、 泊车距离长等特 点。 权利要求书3页 说明书12页 附图3页 CN 115388880 A 2022.11.25 CN 115388880 A 1.一种低成本记 忆泊车建图与定位方法， 其特 征在于， 包括以下步骤： 步骤1： 参数 标定： 对环视中的前视鱼眼摄 像头进行参数 标定； 步骤2： 地图构建： 利用图像以及轮速里程计数据的时间戳进行时间 同步后再进行图像 帧筛选， 再对关键帧的位姿及其生成的地图点进行局部BA优化， 根据当前关键帧与回环关 键帧的轮速里程计数据进行误回环检测； 对稀疏点云地图进行尺度恢复后再分块保存地 图； 步骤3： 实时定位： 对初始位置匹配成功后， 结合前两帧位姿及特征点进行下一帧位姿 预测、 追踪， 通过加载当前帧的局部地图来对前面追踪到的位姿 进行优化； 步骤4： 融合定位： 利用扩展卡尔曼 滤波算法对SLAM 输出位姿与轮速里程计进行融合。 2.根据权利要求1所述的一种低成本记忆泊车建图与定位方法， 其特征在于， 所述 时间 同步的方法为： 利用图像时间戳找到与其最相邻 两个的轮速里程计数据， 当图像时间与最相邻的两个 轮速里程计数据时间差均小于预设阈值时， 通过线性插值的方式得到每一帧图片同步的轮 速里程计数据。 3.根据权利要求2所述的一种低成本记忆泊车建图与定位方法， 其特征在于， 所述线性 插值的方法为： 令( )和( )分别为 时刻和 时刻轮速里程计对应的位姿数据， 则当前 时刻t图像对应的位姿数据 为： ； 其中， 位姿数据组成形式为： （车辆的所在位置的横坐标， 车辆所在位置的纵坐标， 车辆 航向角） ； 所述轮速里程计对应的位姿数据通过轮速里程计输出的后轴左右轮脉冲数值计算得 到， 计算方式为： 权　利　要　求　书 1/3 页 2 CN 115388880 A 2式中： sl： 为左侧车轮移动距离； sr： 为右侧车轮移动距离； wheelbase： 为车辆轴距； 分别为车辆上一时刻的对应位置的横坐标， 纵坐标， 车辆航向角； 分别为车辆当前时刻 对应位置的横坐标的预测值， 纵坐标的预测值， 车辆航向 角的预测值。 4.根据权利要求2所述的一种低成本记忆泊车建图与定位方法， 其特征在于， 所述图像 帧筛选的步骤为： 利用相邻 两帧图像同步的轮速里程计数据来计算相邻 两帧图像对应的车辆行走距离， 当距离小于预设阈值时， 则 删除当前的图像帧。 5.根据权利要求1所述的一种低成本记忆泊车建图与定位方法， 其特征在于， 所述步骤 2中， 稀疏点云地图的尺度恢复方法为： 利用SLAM算法计算得到的部分关键帧轨迹数据及与其同步的轮速里程计轨迹共 同构 建Umeyama模型； 求解得到部分关键帧图像位姿轨 迹与轮速里程计轨 迹之间的最优尺度因子； 稀疏点云地图乘最优尺度因子即完成稀疏点云地图的尺度恢复； 所述的部分 关键帧轨 迹数据是指起始位置一定范围内对应的图像关键帧数据。 6.根据权利要求1所述的一种低成本记忆泊车建图与定位方法， 其特征在于， 所述步骤 2中， 对地图进行分块保存的方法为： 通过将地图分成多个数据块， 分别对数据块进行二进制转换并且通过索引文件的方式 记录多个数据块在文件中的字节位置； 所述数据块由多个关键帧及稀疏点 地图云构成。 7.根据权利要求1所述的一种低成本记忆泊车建图与定位方法， 其特征在于， 所述步骤 3中实时定位的方法为： Step31： 地图加载： 加载构建好的稀疏点云地图； Step32： 特 征点提取： 对实时进来的图片数据帧进行角点信息的检测； Step33： 初始位置匹配： 利用词袋信息对起始位置进来的图片进行位置匹配， 如果匹配 的位置超过建图起始点 一定范围， 则被认为初始位置匹配失败； Step34： 恒速模型追踪： 当初始位置匹配成功以后， 则利用前两帧图像对应的位姿进行 下一帧图像位姿的预测， 并通过 特征点追踪下一帧的位姿； Step35： 位姿优化： 通过加载当前帧的局部地图来对Step34中追踪到的位姿 进行优化。 8.根据权利要求1所述的一种低成本记忆泊车建图与定位方法， 其特征在于， 所述步骤 4中融合定位的方法为： Step1： 首先通过线性插值方式获得与当前SLAM帧对应的轮速数据， 将其作为预测， 将权　利　要　求　书 2/3 页 3 CN 115388880 A 3