一种基于机器人VSLAM技术的泊车位自动搜索方法和系统
技术领域
本发明涉及泊车位检测的技术领域,尤其涉及一种基于机器人VSLAM技术的泊车位自动搜索方法及其系统。
背景技术
近年来汽车自动驾驶系统,又称自动驾驶汽车;也称无人驾驶汽车、电脑驾驶汽车、或轮式移动机器人,是一种通过车载电脑系统实现无人驾驶的智能汽车系统,备受关注。
对于自动驾驶系统的自动泊车系统来说,除了需要准确检测出空的泊车位位置及其坐标之外,对泊车位周围环境的检测也必不可少,尤其需要确定停车位内是否存在障碍物、水坑等不利条件,以及车位的大小、方向和邻近车位的状态,以便于规划最优的泊车轨迹,提高泊车的安全性和用户体验。
发明内容
本部分的目的在于概述本发明的实施例的一些方面以及简要介绍一些较佳实施例。在本部分以及本申请的说明书摘要和发明名称中可能会做些简化或省略以避免使本部分、说明书摘要和发明名称的目的模糊,而这种简化或省略不能用于限制本发明的范围。
鉴于上述现有存在的问题,提出了本发明。
因此,本发明解决的一个技术问题是:兼顾考虑车辆寻找泊车位的路径规划需求,提出了一种基于机器人VSLAM技术的泊车位自动搜索方法。
为解决上述技术问题,本发明提供如下技术方案:一种基于机器人VSLAM技术的泊车位自动搜索方法,包括如下步骤,运用VSLAM算法模块对停车场环境进行地图构建;构建完地图之后,将地图和泊车位显示出来供车主进行人工确认,如果存在无标识线的车位或需要新增加规划车位,提供人工手动划定泊车区域,生成泊车地图;车辆根据生成的所述泊车地图进行泊车位搜寻的路径规划。
作为本发明所述的基于机器人VSLAM技术的泊车位自动搜索方法的一种优选方案,其中:所述路径规划包括,检测车辆在所述泊车地图内有固定车位,则直接规划由当前位置至所述固定车位的轨迹并控制车辆行驶,导航到所述固定车位进行自动泊车。
作为本发明所述的基于机器人VSLAM技术的泊车位自动搜索方法的一种优选方案,其中:所述路径规划还包括,检测出车辆在所述泊车地图内无固定车位;在所述泊车地图中搜索能够使用车位;根据历史数据在所述泊车地图中预测规划一条最优的路径;导航控制车辆的行驶,在行驶过程中每遇到可用车位就检测车位上是否有不适合停车的状况,直到检测到能够泊入的车位并自动泊入。
作为本发明所述的基于机器人VSLAM技术的泊车位自动搜索方法的一种优选方案,其中:所述不适合停车的状况为预留车位、专用车位和车位尺寸不适合车辆大小的可用车位。
作为本发明所述的基于机器人VSLAM技术的泊车位自动搜索方法的一种优选方案,其中:所述最优路径筛选考虑因素,包括以下规划步骤,根据历史泊车数据采用数据挖掘算法计算当前时间段各个车位的空闲概率,优先选择空闲概率较大车位作为目标车位进行轨迹规划;如果空闲概率均不足够大,则优先选择具有高概率且高概率车位密集分布的区域进行轨迹规划;每次泊车完成后根据所经过路径上各车位的实际状况对该概率进行更新。
作为本发明所述的基于机器人VSLAM技术的泊车位自动搜索方法的一种优选方案,其中:所述地图构建包括以下步骤,通过采集模块采集环境数据,包括图像数据、雷达数据;将当前采集到的雷达数据与上一次采集到的数据进行特征提取和特征匹配;基于匹配的结果计算出两帧之间车辆行驶的方向和距离,计算并存储车辆实际行驶的轨迹。
作为本发明所述的基于机器人VSLAM技术的泊车位自动搜索方法的一种优选方案,其中:所述构建地图包括以下步骤,将采集到图像和上一次采集到的图像进行拼接;对当前图像It和上一次的图像It-1分别提取SIFT特征并进行特征匹配;根据匹配出的特征对图像进行旋转和平移,对旋转平移后的图像进行重叠区域裁剪和不同区域的拼接。
作为本发明所述的基于机器人VSLAM技术的泊车位自动搜索方法的一种优选方案,其中:所述构建地图还包括以下步骤,将采集到的所述雷达数据和所述图像数据进行融合;根据所采集的数据、车辆的实际行驶轨迹和存储的地图特征数据进行车辆的判读定位;如果当前位置是已经走过的位置,则更新地图上当前位置的信息,否则将融合后的深度数据叠加到拼接好的图像上,形成三维栅格地图;所述图像数据进行泊车位标识线检测,识别图像中泊车位的方向、距离、坐标以及是否为预留车位和专用车位信息,并将识别结果在地图上存储相应的标志信息;最后根据障碍物状况重新规划轨迹,并控制车辆行驶,直到所有能走的地方都走完为止。
作为本发明所述的基于机器人VSLAM技术的泊车位自动搜索方法的一种优选方案,其中:其中,所述特征提取使用K-均值聚类算法,根据最小化将S和P的数据分别聚成几个目标类和其中S′和P′中包含的是各个类的中心;所述特征的匹配使用ICP算法:最小化函数根据匹配得到旋转矩阵R和平移向量T,对P′进行旋转和平移后根据欧式距离得到两组数据的对应关系:
其中的T=[Tx Ty Tz]分别表示汽车在三个坐标轴方向上的偏移量/行驶距离,而R则对应这车在三个方向上的旋转量,累积一段时间的{R,T}即形成车辆的实际行驶轨迹。
本发明解决的另一个技术问题是:提出了一种基于机器人VSLAM技术的泊车位自动搜索系统,上述方案基于该系统实现。
为解决上述技术问题,本发明提供如下技术方案:一种基于机器人VSLAM技术的泊车位自动搜索系统,其特征在于:包括设置于车辆上的采集模块,以及设置于车载主机内的VSLAM算法模块、处理模块和控制模块;所述采集模块与所述VSLAM算法模块相连接,将采集车辆的图像数据和雷达数据传输至所述VSLAM算法模块通过特征提取和特征匹配构建含有泊车位的地图;所述处理模块将所述VSLAM算法模块的数据分析处理,根据需求规划路线数据并发送至控制模块控制车辆行驶。
本发明的有益效果:通过多传感器融合技术,根据多个毫米波雷达、超声波雷达、单目相机和双目相机的数据,运用相应的信号处理技术和图像处理技术进行泊车位的检测及其大小、方向、坐标等信息的准确计算,同时对其周边环境中的障碍物等进行检测分析。
附图说明
为了更清楚地说明本发明实施例的技术方案,下面将对实施例描述中所需要使用的附图作简单地介绍,显而易见地,下面描述中的附图仅仅是本发明的一些实施例,对于本领域普通技术人员来讲,在不付出创造性劳动的前提下,还可以根据这些附图获得其它的附图。其中:
图1为本发明第一种实施例所述基于机器人VSLAM技术的泊车位自动搜索方法的整体流程示意图;
图2为本发明第一种实施例所述VSLAM技术架构的示意图;
图3为本发明第一种实施例所述设备结构示意图;
图4为本发明第二种实施例所述基于机器人VSLAM技术的泊车位自动搜索系统的整体原理结构示意图;
图5为本发明所述SIFT算法特征检测实际效果图;
图6为本发明所述SURF算法特征检测实际效果图;
图7为本发明所述基于VSLAM算法特征检测实际效果图。
具体实施方式
为使本发明的上述目的、特征和优点能够更加明显易懂,下面结合说明书附图对本发明的具体实施方式做详细的说明,显然所描述的实施例是本发明的一部分实施例,而不是全部实施例。基于本发明中的实施例,本领域普通人员在没有做出创造性劳动前提下所获得的所有其他实施例,都应当属于本发明保护的范围。
在下面的描述中阐述了很多具体细节以便于充分理解本发明,但是本发明还可以采用其他不同于在此描述的其它方式来实施,本领域技术人员可以在不违背本发明内涵的情况下做类似推广,因此本发明不受下面公开的具体实施例的限制。
其次,此处所称的“一个实施例”或“实施例”是指可包含于本发明至少一个实现方式中的特定特征、结构或特性。在本说明书中不同地方出现的“在一个实施例中”并非均指同一个实施例,也不是单独的或选择性的与其他实施例互相排斥的实施例。
本发明结合示意图进行详细描述,在详述本发明实施例时,为便于说明,表示器件结构的剖面图会不依一般比例作局部放大,而且所述示意图只是示例,其在此不应限制本发明保护的范围。此外,在实际制作中应包含长度、宽度及深度的三维空间尺寸。
同时在本发明的描述中,需要说明的是,术语中的“上、下、内和外”等指示的方位或位置关系为基于附图所示的方位或位置关系,仅是为了便于描述本发明和简化描述,而不是指示或暗示所指的装置或元件必须具有特定的方位、以特定的方位构造和操作,因此不能理解为对本发明的限制。此外,术语“第一、第二或第三”仅用于描述目的,而不能理解为指示或暗示相对重要性。
本发明中除非另有明确的规定和限定,术语“安装、相连、连接”应做广义理解,例如:可以是固定连接、可拆卸连接或一体式连接;同样可以是机械连接、电连接或直接连接,也可以通过中间媒介间接相连,也可以是两个元件内部的连通。对于本领域的普通技术人员而言,可依具体情况理解上述术语在本发明中的具体含义。
实施例1
参照图1~3的示意,本实施例提出一种基于机器人VSLAM技术的泊车位自动搜索方法,目前自动泊车系统的泊车位检测方法主要分为两大类:一是基于超声波雷达等测距传感器,该方法受限于测距传感器的自身特性,只能找到空闲位置,其实并不能知道是否是真正的停车位,更不知道停车位的方向;二是基于可见光成像的各种相机,该方法可以通过泊车位的标识线及周边车辆的停放状况客服前一类技术的不足,但其对于空间距离计算的精度和障碍物的检测技术难度较大,尤其当车道线不清晰或者不存在时该方法的可行性将迅速降低。另外,更重要的是,上述两种方法在寻找泊车位的过程中,没有考虑到车辆的行驶路径规划问题。
因此本实施例提出泊车位自动搜索方法,具体的包括以下步骤:
S1:运用VSLAM算法模块100对停车场环境进行地图构建;地图构建包括以下步骤,通过采集模块200采集环境数据,包括图像数据、雷达数据;将当前采集到的雷达数据与上一次采集到的数据进行特征提取和特征匹配;基于匹配的结果计算出两帧之间车辆行驶的方向和距离,计算并存储车辆实际行驶的轨迹。
接着将采集到图像和上一次采集到的图像进行拼接;对当前图像I_t和上一次的图像I_(t-1)分别提取SIFT特征并进行特征匹配;根据匹配出的特征对图像进行旋转和平移,对旋转平移后的图像进行重叠区域裁剪和不同区域的拼接。其中,
特征提取使用K-均值聚类算法,根据最小化将S和P的数据分别聚成几个目标类和其中S′和P′中包含的是各个类的中心;
特征的匹配使用ICP算法:最小化函数根据匹配得到旋转矩阵R和平移向量T,对P′进行旋转和平移后根据欧式距离得到两组数据的对应关系:
其中的T=[Tx Ty Tz]分别表示汽车在三个坐标轴方向上的偏移量/行驶距离,而R则对应这车在三个方向上的旋转量,累积一段时间的{R,T}即形成车辆的实际行驶轨迹。
再具体的,采集到的雷达数据和图像数据进行融合;根据所采集的数据、车辆的实际行驶轨迹和存储的地图特征数据进行车辆的判读定位;如果当前位置是已经走过的位置,则更新地图上当前位置的信息,否则将融合后的深度数据叠加到拼接好的图像上,形成三维栅格地图;图像数据进行泊车位标识线检测,识别图像中泊车位的方向、距离、坐标以及是否为预留车位和专用车位信息,并将识别结果在地图上存储相应的标志信息;最后根据障碍物状况重新规划轨迹,并控制车辆行驶,直到所有能走的地方都走完为止。
本实施例中用采集数据对三维栅格地图进行更新还包括以下步骤,
通行区域的点云投影到地面平面,并对平面中各栅格的占用状态进行计算与更新,可以构建出栅格地图。
例如若用F表示某栅格中有障碍,用E表示某栅格没有障碍,用P(F)、P(E)分别表示某栅格有障碍、没有障碍的概率,那么对于某栅格,有P(F)+P(E)=1。假设某栅格中有n个点,nmax为当前探测范围内栅格中地图点数目的最大值,那么该栅格的占用状态为:
P(F)=1-P(E)。
在下一次探测到同一个栅格时,需要对该栅格的占用状态进行更新,若将某栅格上一次的占用状态表示为E1、F1,将当前的占用状态表示为E2,F2,采用贝叶斯公式有:
式中,P(E1|E2)及P(F1|E2)为先验概率。那么设定参数λ,采用线性插值的方法对该栅格占用状态进行更新,有:
在进行栅格占用状态的计算与更新之后,能够在线进行栅格地图构建,然而由于在路径规划等任务中使用0-1栅格地图有助于减少计算量,使用效率更高,所以还需进行栅格地图决策。
S2:构建完地图之后,将地图和泊车位显示出来供车主进行人工确认,如果存在无标识线的车位或需要新增加规划车位,提供人工手动划定泊车区域,生成泊车地图。
S3:车辆根据生成的泊车地图进行泊车位搜寻的路径规划。本步骤中路径规划包括,
检测车辆在泊车地图内有固定车位,则直接规划由当前位置至固定车位的轨迹并控制车辆行驶,导航到固定车位进行自动泊车。
检测出车辆在泊车地图内无固定车位;
在泊车地图中搜索能够使用车位;
根据历史数据在泊车地图中预测规划一条最优的路径;
控制导航车辆的行驶,在行驶过程中每遇到可用车位就检测车位上是否不适合停车的状况,直到检测到能够泊入的车位并自动泊入。该不适合停车的状况为预留车位、专用车位和车位尺寸不适合车辆大小的可用车位。
进一步的,最优路径筛选考虑因素,包括以下规划步骤,
根据历史经验采用数据挖掘算法计算当前时间段各个能够用车位的空闲概率,优先选择空闲概率较大车位作为目标车位进行轨迹规划;如果空闲概率均不足够大,则优先选择具有高概率且高概率车位密集分布的区域进行轨迹规划;每次泊车完成后根据所经过路径上各车位的实际状况对该概率进行更新。
场景一:
本实施例中对本方法中采用的技术效果加以验证说明,对关键步骤中可选择的不同方法,以科学论证的手段多次对比试验结果,实验结论依据试验结果和数据对比的方式加以论证。本文应用场景为露天的停车场,软件开发平台涉及树莓派微型计算机和PC电脑平台,开发软件包括VisualStudio2013、Eclipse、Notepad++。本实施例使用的实验数据为某建筑外露天非字型停车场。实验图像拍摄方法为每连续三张为一组照片,每个循环拍摄两组。
本测试实验中先对Opencv3提供的图像拼接类库做性能测试,这是自Opencv2.4.0版本之后添加的又一个新的类库。通过实验对比特征点检测数量和计算速度来选择适合本方法的最佳方案。在此之前,获取的原图片是通过云台摄像头循环拍摄实景照片所得。
本测试中以2张为一组进行拍摄,且拍摄两组获得两组车位图像,分别使用本方法基于VSLAM和传统方法(SIFT和SURF算法)进行识别的对比测试,以及并对SIFT和SURF算法进行FLANN匹配。照片拍摄使用的是树莓派平台,得到原图像后的所有工作进行的环境在电脑PC端。这一阶段是对SIFT和SURF算法在特征点提取上的性能做测试。实验设置如下:以获得的任意一组照片为实验对象,每连续相邻两张分别做SIFT和SURF特征提取和FLANN匹配。实验目的是对比非特殊样本下SIFT和SURF的特征点提取数量,匹配效果以及最终耗时。软件输出测试数据如下表。
表1:第一组实验结果统计表。
表2:第二组实验结果统计表。
对同一组图片特征提取结果可以看出,SURF算法的特征点检测数量较多,数量大概是SIFT数量的3到4倍;SURF算法有效匹配点数(Match)比SIFT的有效匹配点数高出近3倍。从同一组图片进行特征点检测的时间上看,SURF算法的时间也相对较短。而搭配同一匹配算法FLANN的运行时间,SURF算法所花时间高出近1s;但是SURF-FLANN的匹配效率比SIFT-FLANN高很多。但本实施例提出的基于VSLAM算法无论是特征点检测的数据和匹配耗时上都们明显优于SIFT和SURF算法。其中可参数图5~6的效果示意,分别为SIFT和SURF算法最终输出的特征检测实际效果图,图7为本方法基于VSLAM算法识别车位输出的实际效果图,明显优于传统方法。
实施例2
参照图4的示意,本实施例提出一种基于机器人VSLAM技术的泊车位自动搜索系统,上述实施例提出的方法依托于本实施例的系统实现。具体的,该系统包括设置于车辆上的采集模块200,以及设置于车载主机内的VSLAM算法模块100、处理模块300和控制模块400;其中采集模块200与VSLAM算法模块100相连接,将采集车辆的图像数据和雷达数据传输至VSLAM算法模块100通过特征提取和特征匹配构建含有泊车位的地图;处理模块300将VSLAM算法模块100的数据分析处理,根据需求规划路线数据并发送至控制模块400控制车辆行驶。
需要说明的是,VSLAM算法模块采用的VSLAM的技术框架主要包括传感器数据预处理、前端、后端、回环检测和建图。其中传感器数据预处理这里的传感器包括摄像头、惯性测量单元等,涉及传感器选型、标定、多传感器数据同步等技术。前度又称为视觉里程计,主要是研究如何根据相邻帧图像定量估算帧间相机的运动,通过把相邻帧的运动轨迹串起来,就构成了相机载体的运动轨迹,解决了定位的问题,然后根据估算的每个时刻相机的位置,计算出各像素的空间点的位置,就得到了地图。
VSLAM中前端主要涉及计算机视觉相关的算法,本实施例首先提取每帧图像特征点,对相邻帧进行特征点粗匹配,然后利用RANSAC(随机抽样一致)算法去除不合理的匹配对,然后得到位置和姿态信息。整个过程涉及到特征提取、特征匹配、对极几何、PnP、刚体运动、李代数等多视图几何知识。
建立地图为SLAM根据不同的传感器类型和应用需求建立不同的地图。常见的有2D栅格地图、2D拓扑地图、3D点云地图,其中3D点云地图在VSLAM中用的比较多,主要用于真实场景的视觉重建,重建的地图非常直观漂亮。但是点云地图通常规模很大,比如一张VGA分辨率(640x 480)的点云图像,就会产生30万个空间点,这会占据非常大的存储空间,而且存在很多冗余信息。
基于视觉的SLAM技术使用视觉传感器进行三维地图构建,能够对环境中的复杂结构完整表示,但是三维地图存在占用存储空间大、路径规划时对计算平台的性能要求高等局限性,因此,通过结合三维地图完整描述环境与二维地图占用存储空间小的优势进行地图制备。基于本实施例所制备的二维导航地图中,能够包含环境的三维结构信息,有助于地图在应用于路径规划等任务时可提供完整的环境信息,使得执行路径规划等任务时能够考虑环境的三维结构,路径规划结果的精确性更高、鲁棒性更好。
进一步的,采集模块200为设置于车上的毫米波雷达、超声波雷达、单双目相机以及距离传感器等,用于采集停车场内车辆周围的数据,处理模块300和控制模块400设置于车载主机内的内集成芯片,芯片内写入程序,用于数据的处理,例如处理模块300为中央处理器模块,其是一块超大规模的集成电路,是一台计算机的运算核心和控制核心。它的功能主要是解释计算机指令以及处理计算机软件中的数据,中央处理器主要包括运算器、算术逻辑运算单元和高速缓冲存储器及实现它们之间联系的数据、控制及状态的总线。它与内部存储器和输入/输出设备合称为计算机三大核心部件,可以执行定点或浮点算术运算操作、移位操作以及逻辑操作,也可执行地址运算和转换。本实施例中主要用于负责对指令译码,并且发出为完成每条指令所要执行的各个操作的控制信号,例如控制车载的变色。是以微存储为核心的微程序控制方式或是以逻辑硬布线结构为主的控制方式实现。
控制模块400为控制芯片组或车身控制器,其中控制芯片组作为主板的一部分,控制芯片组在主板上起着核心作用,控制芯片组一般有两块用于控制和协调主机系统各部件的运行,主要控制着内存I/O、总线I/O,集成了硬盘控制器,负责处理中断请求和直接内存访问等。
而车身控制器例如采用ZLG大集成主要芯片,其集成了BCM、PEPS、TPMS、IMMO功能,能够实现UDS诊断、网络管理、FBL功能,方案包括mcu:采用NXPS32K148(176PIN),单芯片解决方案,常规应用无需额外多余逻辑扩展芯片,同时该芯片可扩展1路以太网接口;防盗与高频接收:遥控器采用NCF29A1芯片,基站低频发射和IMMO采用NXP公司的NJJ29C0芯片,高频接收采用NXP公司的NCK2910芯片,通过3个芯片实现PEPS、IMMO、TPMS等接收相关功能;外围:采用NXP的UJA1078或者UJA1132等SBC芯片,实现CAN、LIN、看门狗、基本控制与诊断。
在本实施例中处理模块300主要用于VSLAM算法模块100发送的数据处理并存储,以及根据采集模块200实时采集的数据与存储数据进行比对,寻找可以用的车位、分析出路面障碍物、规划出最优路径,由处理模块300发送导航指令至控制模块400内进行车辆的控制驾驶至目标车位。
应说明的是,以上实施例仅用以说明本发明的技术方案而非限制,尽管参照较佳实施例对本发明进行了详细说明,本领域的普通技术人员应当理解,可以对本发明的技术方案进行修改或者等同替换,而不脱离本发明技术方案的精神和范围,其均应涵盖在本发明的权利要求范围当中。