一种低成本记忆泊车建图与定位方法、装置及电子设备与流程

1.本发明属于记忆泊车定位技术领域，特别是涉及一种低成本记忆泊车建图与定位方法、装置及电子设备。

背景技术：

2.随着汽车技术的发展，对自动驾驶的需求越来越大。在自动驾驶的应用中，精准定位是最重要的基础技术。感知、预测、规划和控制都基于精确的定位结果。为了实现更加鲁棒的定位，智能驾驶汽车通常都会装备各式各样的传感器，如gps、摄像头、激光雷达、imu、车轮里程计等。现有的定位算法，比如基于视觉、基于激光雷达等等。从商业的角度来考虑，像摄像头、轮速里程计等低成本的传感器方案更加容易被市场接受。
3.记忆泊车系统作为自动驾驶中的一种特殊应用，也越来越受到的市场的关注。记忆泊车主要包括两大核心模块，首先是对要泊车的停车场场景进行学习记忆，然后基于记忆的环境自动的将汽车泊到指定的车位上。由于停车场环境通常都有狭窄、拥挤和范围小的特点，任何的定位误差都有可能引起汽车的碰撞，因此精准的定位显得更加重要，仅依赖轮速的定位方式显然不能满足当前定位的需求。
4.然而，智能驾驶汽车除了车身轮速里程计外，基本上都安装了环视等低成本传感器，因此，在不需要额外增加其他昂贵传感器的情况下，如果仅利用环视中的前视鱼眼摄像头和车身轮速里程计即可完成高精度、长距离的slam建图与定位，则具有更高的商业价值。如专利公布号为cn109887053a的专利公开一种基于单目视觉slam的车辆定位方法及系统：在建图时，单目视觉slam初始化过程中，以车辆的实际移动距离来确定单目视觉slam地图的尺度，并在后续以车辆实际移动距离来不断优化slam地图的尺度等信息；基于地图的定位时，匹配目标图像以及slam地图中的特征点，从而确定出车辆在slam地图中的重定位位姿，以获得车辆定位模块测量到的车身位姿与单目摄像头的视觉位姿之间的转换关系，并且利用多个车身位姿和视觉位姿来不断优化该转换关系。此专利的缺陷在于在slam定位过程中，当停车场存在两个非常相似的场景时，重定位会出现错误匹配的风险，最终导致整个系统运行异常；其次，当系统中获取得到的每一张图像数据都拿来建图时，整个系统的建图速度会较慢，因此用户体验感也会较差；另外，由于停车场存在大量的相似场景，此发明提出的系统在建图时存在误回环的风险。
5.又如专利公布号为cn110132280a的专利名公开一种室内场景下的车辆定位方法、车辆定位装置和车辆，该发明公开了一种室内场景的车辆定位方法及系统。获取摄像头拍摄的前方道路的当前帧图像，并从惯性测量单元imu采集车辆的惯导姿态信息；获取前方道路的当前帧图像中的主消失点；根据惯导姿态信息判断当前帧图像中的主消失点是否存在异常；如果判断当前帧图像中的主消失点未存在异常，则根据当前帧图像中的主消失点计算摄像头的全局姿态信息；根据全局姿态信息对slam算法进行校正；以及根据校正后的slam算法对所述车辆进行定位。此发明能够在室内场景下，利用主消失点和惯导融合的方式辅助slam算法矫正其积累误差，从而提高车辆定位的准确性。此发明的缺陷在于当采用
低精度imu进行车辆位姿计算时，由于收到零偏以及温漂等因素的影响，计算的车辆位姿误差较大，若使用高精度imu时，则会导致系统成本的增加；其次，面对停车楼这种室内场景，如果摄像头对着室外，有可能一直无法计算图像中的主消失点，可能存在算法失效的风险；另外，此发明只能用于室内场景，因此，用户使用此功能的过程中会有较大的局限性。
6.因此，本发明提供一种一种低成本记忆泊车建图与定位方法、装置及电子设备。

技术实现要素：

7.本发明的目的在于提供一种低成本记忆泊车建图与定位方法、装置及电子设备，通过利用摄像头以及车身轮速里程计等低成本的传感器进行建图与定位、利用相邻两帧图像同步的轮速里程计来计算相邻两帧之间的距离来筛选图片、利用当前关键帧与回环关键帧的轮速里程计信息来实现误回环的检测、利用部分的关键帧轨迹和轮速里程计轨迹基于umeyama模型对构建好的slam地图进行尺度恢复，利用slam位姿信息与轮速里程计信息进行融合定位，解决了现有的问题。
8.为解决上述技术问题，本发明是通过以下技术方案实现的：作为本发明提供的第一个方面，本发明为一种低成本记忆泊车建图与定位方法，包括以下步骤：步骤1：参数标定：以车身后轴中心为坐标原点、采用右手定则建立坐标系，对环视中的前视鱼眼摄像头进行参数标定；步骤2：地图构建：利用图像以及轮速里程计数据的时间戳进行时间同步后再进行图像帧筛选，再对关键帧的位姿及其生成的地图点进行局部ba优化，根据关键帧及相邻关键帧的位姿及相关地图点进行相似矩阵的变换，完成回环纠正，并利用轮速里程计数据进行误回环检测；进行稀疏点云地图的尺度恢复后再进行地图的分块保存；步骤3：实时定位：对初始位置匹配成功后，结合前两帧位姿及特征点进行下一帧位姿预测、追踪，通过加载当前帧的局部地图来对前面追踪到的位姿进行优化；步骤4：融合定位：利用扩展卡尔曼滤波算法对slam输出位姿与轮速里程计进行融合，并且在远离更新点的时刻，使用最近两帧的slam方向作为参考方向，将轮速轨迹投影到参考方向上进行修正，并将修正后的轨迹作为融合轨迹发布。
9.进一步地，所述时间同步的方法为：利用图像时间戳到与其最相邻两个的轮速里程计数据，当图像时间与最相邻的两个轮速里程计数据时间差均小于某一固定的阈值时，通过线性插值的方式得到每一帧图片同步的轮速里程计数据，即前一时刻和后一时刻两个轮速里程计数据都进行比较，两者都要小于某一阈值；其中，最相邻两个的轮速里程计数据即指前一时刻以及后一时刻的轮速里程计数据。
10.进一步地，所述线性插值的方法为：令()和()分别为时刻和时刻轮速里程计对应的位姿数据，则当前时刻t图像对应的位姿数据为：
；其中，位姿数据组成形式为：（车辆的所在位置的横坐标，车辆所在位置的纵坐标，车辆航向角）；所述轮速里程计对应的位姿数据通过轮速里程计输出的后轴左右轮脉冲数值计算得到，计算方式为：算得到，计算方式为：算得到，计算方式为：式中：s
l
：为左侧车轮移动距离；sr：为右侧车轮移动距离；wheelbase：为车辆轴距；分别为车辆上一时刻的对应位置的横坐标，纵坐标，车辆航向角；分别为车辆当前时刻对应位置的横坐标的预测值，纵坐标的预测值，车辆航向角的预测值。
11.进一步地，所述所述图像帧筛选的步骤为：利用相邻两帧图像同步的轮速里程计数据来计算相邻两帧图像对应的车辆行走距离，当距离小于某一个固定的阈值时，则删除当前的图像帧，以防止车辆行走缓慢或者静止时产生的大量冗余图像。
12.进一步地，所述步骤2中，稀疏点云地图的尺度恢复方法为：利用slam算法计算得到的部分关键帧轨迹数据及与其同步的轮速里程计轨迹共同构建umeyama模型；求解得到部分关键帧图像位姿轨迹与轮速里程计轨迹之间的最优尺度因子；稀疏点云地图乘最优尺度因子即完成稀疏点云地图的尺度恢复；
所述的部分关键帧轨迹数据是指起始位置一定范围内对应的图像关键帧数据，这样可以降低因为轮速里程计累积偏差造成尺度因子不准的情况。
13.进一步地，所述步骤2中，对地图进行分块保存的方法为：通过将地图分成多个数据块，分别对数据块进行二进制转换并且通过索引文件的方式记录多个数据块在文件中的字节位置；所述数据块由多个关键帧及稀疏点地图云构成。
14.进一步地，所述步骤3中实时定位的方法为：step31：地图加载：加载构建好的稀疏点云地图；step32：特征点提取：对实时进来的图片数据帧进行角点信息的检测；step33：初始位置匹配：利用词袋信息对起始位置进来的图片进行位置匹配，如果匹配的位置超过建图起始点一定范围，则被认为初始位置匹配失败，这样可以防止停车场环境过度相似引起的重定位位置匹配错误的风险；step34：恒速模型追踪：当初始位置匹配成功以后，则利用前两帧位姿进行下一帧位姿预测，并通过特征点追踪下一帧的位姿；step35：位姿优化：通过加载当前帧的局部地图来对step34中追踪到的位姿进行优化，提升当前帧的位姿精度。
15.进一步地，所述步骤4中融合定位的方法为：step1：首先通过线性插值方式获得与当前slam帧对应的轮速数据，将其作为预测，将slam定位信息作为测量，输入到扩展卡尔曼滤波器中进行融合得到后验位姿；由于时间可能不同步，使用融合得到的后验位姿重新预测上一帧轮速位姿，在实时解算线程中基于上一帧轮速预测当前帧位姿，从而将融合产生的更新量传递至当前时刻；step2：在上一帧轮速位姿的基础上，使用轮速积分对位置和航向角进行预测；在远离更新点的时刻，积分预测的误差会逐渐累积，为减少这种累积误差，使用最近两帧的slam方向作为参考方向，将轮速轨迹投影到参考方向上进行修正，并将修正后的轨迹作为融合轨迹发布。
16.作为本发明提供的第二个方面，本发明为一种低成本记忆泊车建图与定位装置，所述装置用于实现第一个方面所述的记忆泊车建图与定位方法，所述装置包括：参数标定模块，其用于对环视中的前视鱼眼摄像头进行的内参和外参标定；地图构建模块，其包括时间同步子模块、图像帧筛选子模块、误回环检测子模块、尺度恢复子模块、地图分块保存子模块；地图定位模块，其用于初始位置匹配，且初始重定位的位置被限定在起始点一定范围内；融合定位模块，其用于进行轨迹修正，并将修正后的轨迹作为融合轨迹发布。
17.作为本发明提供的第三个方面，本发明为一种电子设备，所述电子设备包括存储器和处理器，所述存储器中存储有可在所述处理器上运行的计算机程序，所述处理器执行所述计算机程序时实现第一个方面提供的方法。
18.本发明具有以下有益效果：本发明利用摄像头以及车身轮速里程计等低成本的传感器进行建图与定位，具有更高的商业价值；利用相邻两帧图像同步的轮速里程计来计算相邻两帧之间的距离来筛选
图片，从而加快lapa系统的建图速度；由于停车场中存在大量的相似场景，因此在建图时，利用当前关键帧与回环关键帧的轮速里程计信息来实现误回环的检测，提升系统的建图鲁棒性；利用slam位姿信息与轮速里程计信息进行融合定位，最终提升整个系统的定位频率，较好的解决了域控性能较低时无法实时slam定位的问题。
19.当然，实施本发明的任一产品并不一定需要同时达到以上所述的所有优点。
附图说明
20.为了更清楚地说明本发明实施例的技术方案，下面将对实施例描述所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他的附图。
21.图1为本发明低成本记忆泊车建图与定位方法的流程图；图2为本发明中车身后轴中心坐标系示意图；图3为本发明中地图构建方法的流程图；图4为本发明中实时定位方法的流程图。
具体实施方式
22.下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有作出创造性劳动前提下所获得的所有其它实施例，都属于本发明保护的范围。
23.在本发明的描述中，需要理解的是，术语“前”、“后”、“左”、“右”、“中心”等指示方位或位置关系，仅是为了便于描述本发明和简化描述，而不是指示或暗示所指的组件或元件必须具有特定的方位，以特定的方位构造和操作，因此不能理解为对本发明的限制。
24.实施例一：作为本发明提供的一个实施例，本发明为一种低成本记忆泊车建图与定位方法，如图1所示，所述的方法包括以下步骤：步骤1：参数标定：对环视中的前视鱼眼摄像头进行参数标定，作为本发明提供的一个实施例，优选的，参数标定的步骤如下所示：step11：内参标定：对环视中的前视鱼眼摄像头进行内参标定；作为本发明提供的一个实施例，优选的，内参标定中标定的参数有fx、fy、cx和cy，其中fx=f/dx fy=f/dyf是指前视鱼眼摄像头焦距的长度；dx是指水平x方向一个像素的长度；dy是指水平y方向一个像素的长度；cx 、cy是指图像的中心像素坐标和图像原点像素坐标之间相差的横向和纵向像素数；step12：外参标定：对环视中的前视鱼眼摄像头的参数，也即摄像头坐标系到车身后轴中心坐标系进行外参计算，其中，车身后轴中心坐标系采用右手坐标系，以车身后轴中心为坐标原点，前方是x轴，左边是y轴，上方是z轴，具体如下图所示2；
步骤2：地图构建：利用图像（前视鱼眼摄像头采集的图像）以及轮速里程计数据的时间戳进行时间同步后再进行图像帧筛选，再对关键帧的位姿及其生成的地图点进行局部ba优化，根据当前关键帧与回环关键帧的轮速里程计数据进行误回环检测；对稀疏点云地图进行尺度恢复后再分块保存地图；ba英文全称为bundle adjustment，也称为光束法平差优化模型；作为本发明提供的一个实施例，优选的，如图3所示，地图构建的步骤如下：step21：追踪：首先对提取的图像（前视鱼眼摄像头采集的图像）进行灰度化，并对灰度化后的图像进行特征角点的提取；然后利用图像以及轮速里程计数据的时间戳进行时间同步；接着利用图像同步上的轮速里程计数据进行图像帧筛选，以加快lapa系统建图的速度；最后筛选出特征点丰富、共视程度高的普通帧作为关键帧提供给局部建图模块以及回环检测模块；step22：局部建图：首先利用关键帧的位姿及一级和二级相邻关键帧之间的共视点三角化生成新的地图点，并对一些冗余地图点进行融合；接着对关键帧的位姿及其生成的地图点进行局部ba优化，提升关键帧位姿及其地图点的精度；最后如果关键帧中80%以上的特征点都能被其他关键帧观测到，则对此冗余关键帧进行剔除；step23：回环检测：首先通过词袋的方式检测当前关键帧与历史关键帧的描述子共视程度，如果检测到共视程度大于某一个阈值时，则当前关键帧放到后续关键帧队列中；接着对后续关键帧进行场景相似度识别，当场景相似度大于某一些预先设定好的条件时，此关键帧则被认为是回环关键帧，并对此关键帧及相邻关键帧的位姿及相关地图点进行相似矩阵的变换，也即回环纠正；除此之外，由于停车场存在大量的相似场景，因此系统还会利用轮速里程计信息来实现误回环检测，从而提升整个系统的建图鲁棒性。
25.step24:尺度恢复：利用slam算法计算得到的部分关键帧图像轨迹数据与其同步的轮速里程计轨迹共同构建umeyama模型，并求解得到关键帧位姿轨迹与轮速里程计轨迹之间的最优尺度因子，最终稀疏点云地图乘于最优尺度因子即可完成稀疏点云地图的尺度恢复。由于轮速里程计在积分的过程中随着距离的增加其累积偏差会越来越大，因此，所述的部分关键帧图像位姿数据是指起始位置一定范围内对应的图像关键帧数据，这样可以降低因为轮速里程计累积偏差造成尺度因子不准的情况；slam的全称为simultane ous localization and mapping，也称为同时定位与建图;step25：地图保存：在嵌入式系统中，由于内存资源有限，当建图距离较长时，如果直接将整块地图保存成二进制数据时，会出现内存剧增，甚至可能存在内存溢出的风险，因此，在此发明中通过将地图分成多个数据块，分别对数据块进行二进制转换并且通过索引文件的方式记录多个数据块在文件中的字节位置，以防止整块地图保存时内存溢出导致系统异常退出的风险；所述数据块是指由多个关键帧及其稀疏点云构成的数据模块；步骤3：实时定位：对初始位置匹配成功后，结合前两帧位姿及特征点进行下一帧位姿预测、追踪，通过加载当前帧的局部地图来对前面追踪到的位姿进行优化；如图4所示，实时定位的步骤为：step31:地图加载：加载前面构建好的稀疏点云地图；step32:特征点提取：对实时进来的图片数据帧进行角点信息的检测，即fast角点；
step33:初始位置匹配：利用词袋信息对起始位置进来的图片进行位置匹配，如果匹配的位置超过建图起始点一定范围，则被认为初始匹配失败，这样可以防止停车场环境过度相似引起的重定位位置匹配错误的风险；起始位置（建图起始点）即指记忆泊车过程中的学习起点；step34:恒速模型追踪：当初始位置匹配成功以后，则利用前两帧位姿进行下一帧位姿预测，并通过特征点追踪下一帧的位姿；step35:位姿优化：通过加载当前帧的局部地图来对前面追踪到的位姿进行优化，提升当前帧的位姿精度，即通过图优化方式对位姿进行优化；作为本发明提供的一个实施例，优选的，追踪下一帧的位姿的步骤为：ss1、将上一帧某一个地图点投影到当前帧中；ss2、如果能投影到当前帧中，则以当前帧的投影点为中心，在窗口范围内寻与当前帧中所有相关的特征点；ss3、将上一帧地图点的描述子与当前帧所有相关的特征点描述子进行比较，到最相似描述子对应的特征点，然后将上一帧地图点作为此特征点对应的地图点；ss4、对上一帧中所有的地图点都重复ss1-ss3的过程，最终得到当前帧特征点与其对应的地图点对集合；ss5、利用地图点对集合构建图优化模型，便可对预测的位姿进行进一步优化，得到一个更加精准的当前帧位姿；步骤4：融合定位：利用扩展卡尔曼滤波算法对slam输出位姿与轮速里程计进行融合，融合定位的方法为：step1：首先通过线性插值方式获得与当前slam帧对应的轮速数据，将其作为预测，将slam定位信息作为测量，输入到扩展卡尔曼滤波器中进行融合得到后验位姿。由于时间可能不同步，使用融合得到的后验位姿重新预测上一帧轮速位姿，在实时解算线程中基于上一帧轮速预测当前帧位姿，从而将融合产生的更新量传递至当前时刻；step2：在上一帧轮速位姿的基础上，使用轮速积分对位置和航向角进行预测。在远离更新点的时刻，积分预测的误差会逐渐累积。为减少这种累积误差，使用最近两帧的slam方向作为参考方向，此处参考方向只有一个，就是车辆前进的方向，将轮速轨迹投影到参考方向上进行修正，并将修正后的轨迹作为融合轨迹发布。
26.作为本发明提供的一个实施例，优选的，所述时间同步的方法为：利用图像时间戳到与其最相邻两个的轮速里程计数据，当图像时间与最相邻的两个轮速里程计数据时间差均小于预设阈值时，通过线性插值的方式得到每一帧图片同步的轮速里程计数据。
27.作为本发明提供的一个实施例，优选的，所述线性插值的方法为：令()和()分别为时刻和时刻轮速里程计对应的位姿数据，则当前时刻t图像对应的位姿数据为：
；其中，位姿数据组成形式为：（车辆的所在位置的横坐标，车辆所在位置的纵坐标，车辆航向角）；即()中的分别为时刻车辆的所在位置的横坐标，车辆所在位置的纵坐标，车辆航向角；()中的分别为时刻车辆的所在位置的横坐标，车辆所在位置的纵坐标，车辆航向角；中的分别为当前时刻t车辆的所在位置的横坐标，车辆所在位置的纵坐标，车辆航向角；所述轮速里程计对应的位姿数据通过轮速里程计输出的后轴左右轮脉冲数值计算得到，计算方式为：算得到，计算方式为：算得到，计算方式为：式中：s
l
：为左侧车轮移动距离；sr：为右侧车轮移动距离；wheelbase：为车辆轴距；分别为车辆上一时刻的对应位置的横坐标，纵坐标，车辆航向角；分别为车辆当前时刻对应位置的横坐标的预测值，纵坐标的预测值，车辆航向角的预测值。
28.作为本发明提供的一个实施例，优选的，所述图像帧筛选的步骤为：利用相邻两帧图像同步的轮速里程计数据来计算相邻两帧图像对应的车辆行走距离，当距离小于预设阈值时，则删除当前的图像帧。
29.作为本发明提供的一个实施例，优选的，所述一级相邻关键帧是指与当前关键帧的共视地图点数量超过一定阈值的相邻关键帧；所述二级相邻关键帧是指与一级相邻关键帧有共视地图点并且数量超过一定阈值的相邻关键帧。
30.作为本发明提供的一个实施例，优选的，ba优化的模型为：
其中，表示从相机坐标系到像素坐标系的映射函数，具体如下所示：其中，分别表示图像中x方向和y方向的像素坐标值；x、y、z是指空间中某一个点在世界坐标系下的一个空间坐标；、、、分别是前面所述标定的相机内参，其中：=f/dx
ꢀꢀꢀꢀꢀ
=f/dyf是指焦距的长度；dx是指水平x方向一个像素的长度；dy是指水平y方向一个像素的长度；cx 、cy是指图像的中心像素坐标和图像原点像素坐标之间相差的横向和纵向像素数；k
l
表示当前关键帧的一级相邻关键帧列表；表示当前关键帧的一级相邻关键帧对应的局部地图点列表；kf表示当前关键帧的二级相邻关键帧列表，但不包含一级相邻关键帧列表中的关键帧；表示第k个关键帧第j个地图点对应的像素坐标值；表示一级局部地图点中的第p个地图点的坐标值；表示第k个关键帧的旋转矩阵，表示第k个关键帧的平移矢量，具体度的，之所以用不同的字符l和k来表示r
cw 和cp的上标，是因为两个位置表示的含义是不一样的，等式左边l表示的是集合中的元素，此集合中的元素是指需要被优化的变量，等式右边k表示的是集合的元素，此集合中的元素分为两部分，一部分是需要被用于优化的变量，一部分是只参与计算不进行优化的变量。由于集合不同，所以用不同的元素来表示。
31.作为本发明提供的一个实施例，优选的，所述回环，是指slam系统在运行过程中会出现累积误差，为了消除这种累积误差，slam系统会执行回环检测，也即通过图片特征点的语义来判断slam系统是否之前来过此位置，如果确定来过，则对有累积误差的关键帧及其特征点云进行相似变换矩阵的纠正，从而消除累积误差。但值得注意的时，当存在相似场景时，slam系统可能会将错误的位置认为是回环位置，则执行回环纠正，导致整个建图系统出错，因此误回环检测很重要。在此发明中，主要是通过图像同步上的轮速里程计数据进行辅助检测，也即当slam系统检测到回环关键帧时，则对当前关键帧与回环关键帧同步的轮速里程计数据进行比较，如果他们之间的距离大于某些阈值时，则认为是误回环。
32.作为本发明提供的一个实施例，优选的，所述步骤2中，稀疏点云地图的尺度恢复方法为：利用slam算法计算得到的部分关键帧轨迹数据及与其同步的轮速里程计轨迹共同构建umeyama模型；求解得到部分关键帧图像位姿轨迹与轮速里程计轨迹之间的最优尺度因子；稀疏点云地图乘最优尺度因子即完成稀疏点云地图的尺度恢复；所述的部分关键帧轨迹数据是指起始位置一定范围内对应的图像关键帧数据；作为本发明提供的一个实施例，优选的，所述umeyama模型主要是用于对齐两条轨迹，具体原理如下所示：设x是需要评估的轨迹的位置坐标，y是轮速里程计轨迹的位置坐标，此时，需要到一个尺度s、旋转矩阵r、平移向量t，使得待评估的轨迹和轮速里程计轨迹在尺度上可以对齐，其误差模型如下所示：其中和分别表示轮速里程计轨迹和待评估的轨迹的第i个位置的坐标值；将所有轨迹点写成最小二乘的形式可得到：通过求解上述最小二乘模型即可得到slam点云地图的最优尺度因子。
33.一种低成本记忆泊车建图与定位方法，对环视中的前视鱼眼摄像头进行内参和外参标定，其中外参是指摄像头坐标系到车身坐标系的变换矩阵；基于车身轮速里程计输出的后轴轮速脉冲数据进行轮速里程计计算；将轮速里程计数据与前视鱼眼数据进行时间同步；通过前视鱼眼图片数据构建slam稀疏点云地图，并利用部分的关键帧轨迹和轮速里程计轨迹对稀疏点云地图进行尺度恢复，除此之外，建图过程还会利用车身轮速里程计信息进行误回环检测，提升建图鲁棒性；在自动泊车过程中，基于构建好的稀疏点云地图，对获取实时图片数据的特征点进行重定位或者恒速模型追踪，实现定位功能；值得注意的是，在重定位阶段，如果匹配的位置超过建图起始点一定范围，则被认为初始重定位失败，这样可以降低停车场环境过度相似引起的重定位位置匹配错误的风险；由于域控性能有限，算法实现的定位频率较低，因此需要利用slam定位位姿与轮速里程计进行融合定位，在保证定位精度的同时，提高定位频率。上述方法只利用了环视中的前视鱼眼和车身轮速里程计两个常规传感器，并实现了1公里以上的记忆距离，因此，上述方法具有成本低、实现简单、泊车距离长等特点。
34.实施例二：作为本发明提供的再一个实施例，本发明为一种低成本记忆泊车建图与定位装置，所述装置用于实现实施例一所提供的记忆泊车建图与定位方法，所述装置包括：参数标定模块，其用于对环视中的前视鱼眼摄像头进行的内参和外参标定，对外参进行标定即指摄像头坐标系到车身后轴中心坐标系的变换矩阵；地图构建模块，其包括：时间同步子模块，利用图像时间戳到与其最相邻两个的轮速里程计数据，当图
像时间与相邻的轮速里程计数据时间差小于某一固定的阈值时，通过线性插值的方式得到每一帧图片同步的轮速里程计数据，轮速里程计数据通过轮速里程计输出的后轴左右轮脉冲数值基于阿克曼公式计算得来；图像帧筛选子模块，利用相邻两帧图像同步的轮速里程计来计算相邻两帧之间的距离，当距离小于某一个固定的阈值时，则删除当前的图像帧；误回环检测子模块，通过图像同步上的轮速里程计数据进行误回环检测，也即当slam系统检测到回环关键帧时，则对当前关键帧与回环关键帧同步的轮速里程计数据进行比较，如果他们之间的距离大于某些阈值时，则认为是误回环；尺度恢复子模块，利用slam算法计算得到的部分关键帧轨迹数据与其同步的轮速里程计轨迹共同构建umeyama模型，并求解得到关键帧图像位姿轨迹与轮速里程计轨迹之间的最优尺度因子，最终稀疏点云地图乘于最优尺度因子即可完成稀疏点云地图的尺度恢复。所述的部分关键帧轨迹数据是指起始位置一定范围内对应的图像关键帧数据，这样可以降低因为轮速里程计累积偏差造成尺度因子不准的情况；地图分块保存子模块，通过将地图分成多个数据块，分别对数据块进行二进制转换并且通过索引文件的方式记录多个数据块在文件中的字节位置，所述数据块是指由多个关键帧及其稀疏点云构成的数据模块；地图定位模块，其用于初始位置匹配，且初始重定位的位置被限定在起始点（起始点与上述起点位置一致，是指记忆泊车过程中的学习起点）一定范围内，这样可以防止停车场环境过度相似引起的重定位位置匹配错误的风险。所述初始重定位是指slam系统在定位模式下刚刚启动的时间点，此时需要对当前车在slam地图下的初始位置进行确定；作为本发明提供的一个实施例，优选的，记忆泊车过程分为两个阶段，分别是学习阶段和记忆阶段，学习阶段是指用户驾车绕某一路线走一遍，把路线的环境记忆下来；记忆阶段是指根据记忆的环境自己能时刻知道自己在所学环境的哪个位置；所以学习的时候车辆会有一个起点和终点位置，然后记忆的时候车辆会先驶入到学习起点的附近，车辆会根据摄像头传话的画面自动识别出自己在记忆环境中的哪个位置，这个起点附近的位置就是指初始重定位的位置；融合定位模块，其用于进行轨迹修正，并将修正后的轨迹作为融合轨迹发布；具体的，利用扩展卡尔曼滤波算法对slam输出位姿与轮速里程计进行融合，并且在远离更新点的时刻，轮速积分预测的误差会逐渐累积。为减少这种累积误差，使用最近两帧的slam方向作为参考方向，将轮速轨迹投影到参考方向上进行修正，并将修正后的轨迹作为融合轨迹发布。
35.实施例三：作为本发明提供的再一个实施例，本发明为一种电子设备，所述电子设备包括存储器和处理器，所述存储器中存储有可在所述处理器上运行的计算机程序，所述处理器执行所述计算机程序时实现实施例一所提供的方法。
36.实施例四：作为本发明提供的再一个实施例，本发明为一种计算机可读存储介质，所述计算机可读存储介质存储有计算机程序，所述计算机程序被处理器执行时实现实施例一所提供的方法。
37.在本说明书的描述中，参考术语“一个实施例”、“示例”、“具体示例”等的描述意指结合该实施例或示例描述的具体特征、结构、材料或者特点包含于本发明的至少一个实施例或示例中。在本说明书中，对上述术语的示意性表述不一定指的是相同的实施例或示例。而且，描述的具体特征、结构、材料或者特点可以在任何的一个或多个实施例或示例中以合适的方式结合。
38.以上公开的本发明优选实施例只是用于帮助阐述本发明。优选实施例并没有详尽叙述所有的细节，也不限制该发明仅为所述的具体实施方式。显然，根据本说明书的内容，可作很多的修改和变化。本说明书选取并具体描述这些实施例，是为了更好地解释本发明的原理和实际应用，从而使所属技术领域技术人员能很好地理解和利用本发明。本发明仅受权利要求书及其全部范围和等效物的限制。

技术特征：

1.一种低成本记忆泊车建图与定位方法，其特征在于，包括以下步骤：步骤1：参数标定：对环视中的前视鱼眼摄像头进行参数标定；步骤2：地图构建：利用图像以及轮速里程计数据的时间戳进行时间同步后再进行图像帧筛选，再对关键帧的位姿及其生成的地图点进行局部ba优化，根据当前关键帧与回环关键帧的轮速里程计数据进行误回环检测；对稀疏点云地图进行尺度恢复后再分块保存地图；步骤3：实时定位：对初始位置匹配成功后，结合前两帧位姿及特征点进行下一帧位姿预测、追踪，通过加载当前帧的局部地图来对前面追踪到的位姿进行优化；步骤4：融合定位：利用扩展卡尔曼滤波算法对slam输出位姿与轮速里程计进行融合。2.根据权利要求1所述的一种低成本记忆泊车建图与定位方法，其特征在于，所述时间同步的方法为：利用图像时间戳到与其最相邻两个的轮速里程计数据，当图像时间与最相邻的两个轮速里程计数据时间差均小于预设阈值时，通过线性插值的方式得到每一帧图片同步的轮速里程计数据。3.根据权利要求2所述的一种低成本记忆泊车建图与定位方法，其特征在于，所述线性插值的方法为：令()和()分别为时刻和时刻轮速里程计对应的位姿数据，则当前时刻t图像对应的位姿数据为：为：为：；其中，位姿数据组成形式为：（车辆的所在位置的横坐标，车辆所在位置的纵坐标，车辆航向角）；所述轮速里程计对应的位姿数据通过轮速里程计输出的后轴左右轮脉冲数值计算得到，计算方式为：到，计算方式为：
式中：s
l
：为左侧车轮移动距离；s
r
：为右侧车轮移动距离；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帧对应的轮速数据，将其作为预测，将
slam定位信息作为测量，输入到扩展卡尔曼滤波器中进行融合得到后验位姿；step2：在上一帧轮速位姿的基础上，使用轮速积分对位置和航向角进行预测；在远离更新点的时刻，使用最近两帧的slam方向作为参考方向，将轮速轨迹投影到参考方向上进行修正，并将修正后的轨迹作为融合轨迹发布。9.一种低成本记忆泊车建图与定位装置，其特征在于，所述装置用于实现如权利要求1-8任意一项所述的记忆泊车建图与定位方法，所述装置包括：参数标定模块，其用于对环视中的前视鱼眼摄像头进行的内参和外参标定；地图构建模块，其包括时间同步子模块、图像帧筛选子模块、误回环检测子模块、尺度恢复子模块、地图分块保存子模块；地图定位模块，其用于初始位置匹配，且初始重定位的位置被限定在起始点一定范围内；融合定位模块，其用于进行轨迹修正，并将修正后的轨迹作为融合轨迹发布。10.一种电子设备，所述电子设备包括存储器和处理器，所述存储器中存储有可在所述处理器上运行的计算机程序，其特征在于，所述处理器执行所述计算机程序时实现如权利要求1-8任意一项所述的方法。

技术总结

本发明公开了一种低成本记忆泊车建图与定位方法、装置及电子设备，涉及记忆泊车定位技术领域。本发明包括：基于车身轮速里程计输出的后轴轮速脉冲数据进行轮速里程计计算，将轮速里程计数据与前视鱼眼数据进行时间同步；通过前视鱼眼图片数据构建SLAM稀疏点云地图，并利用部分的关键帧轨迹和轮速里程计轨迹对稀疏点云地图进行尺度恢复，建图过程利用车身轮速里程计信息进行误回环检测，提升建图鲁棒性，利用SLAM定位位姿与轮速里程计进行融合定位，在保证定位精度的同时，提高定位频率。上述方法只利用了环视中的前视鱼眼和车身轮速里程计两个常规传感器，并实现了1公里以上的记忆距离，具有成本低、实现简单、泊车距离长等特点。点。点。