一种三维高精度地图生成方法及装置
技术领域
本发明涉及地图构建技术领域,尤其涉及一种三维高精度地图生成方法及装置。
背景技术
园区环境中混合着结构化和非结构化道路,移动机器人无法像在普通的道路上一样完全按照红绿灯、车道线等规则行驶。面对园区中路灯、垃圾桶、树木、建筑物等障碍物,移动机器人需要更加“聪明”地感知和避障,防止不安全状况的发生。在这种情况下,一张包含了丰富的环境信息的三维高精度地图,可以为移动机器人的感知定位提供足够的参考,有效降低执行自动行驶任务的难度。
传统的三维高精度地图的生成方式需要昂贵的数据采集设备,例如高频多线束激光雷达、高清摄像头、高精度组合导航定位系统等,这种方式数据采集量大,算法处理复杂,需要消耗大量的计算资源,地图更新慢,不能及时响应场景的变化,因此,在大规模空间场景中的应用较多,例如百度、高德等地图服务提供商大都采用这种方式。
另外,构建高精度地图的点云数据利用基于双目相机采集时,容易受到光照变化的干扰,测量距离短,精度低,图像畸变大,需要经过复杂的标定和校正过程,在室外环境中受到各种限制。
发明内容
为了解决传统的三维高精度地图的生成方式成本高、效率低的问题,本发明实施例提供了一种三维高精度地图生成方法及装置。
第一方面,本发明实施例提供了一种三维高精度地图生成方法,包括:
实时采集移动机器人在移动过程中周围的环境信息,所述环境信息包括环境点云数据,所述环境点云数据为利用安装在所述移动机器人上的激光雷达获取的;
分别从各帧环境点云数据中提取障碍物点云数据和路面点云数据;
根据所述障碍物点云数据生成地图点云层和地图语义层,所述地图点云层为用于表征障碍物信息的图层,所述地图语义层为用于表征障碍物的语义信息的图层;
根据所述路面点云数据生成地图拓扑层,所述地图拓扑层为用于表征道路拓扑信息的图层;
根据所述地图点云层、所述地图语义层和所述地图拓扑层生成三维高精度地图。
本发明实施例提供的三维高精度地图生成方法中,移动机器人实施采集其在移动过程中周围的环境信息,所述环境信息包括利用安装在所述移动机器人上的激光雷达获取的环境点云数据,分别从各帧环境点云数据中提取障碍物点云数据和路面点云数据,进而,根据提取的障碍物点云数据生成地图点云层和地图语义层,其中,地图点云层为用于表征障碍物信息的图层,地图语义层为用于表征障碍物的语义信息的图层,根据提取的路面点云数据生成地图拓扑层,地图拓扑层为用于表征道路拓扑信息的图层,进一步地,根据生成的地图点云层、地图语义层和地图拓扑层共同生成三维高精度地图。相比于现有技术,本申请在构建三维高精度地图采集数据时,所使用的传感器可以仅仅依赖一个激光雷达,在成本可控的情况下生成三维高精度地图,有效降低了地图构建的成本,提高了地图生成效率。并且,本申请构建的三维高精度地图采用分层的模式,分别包含了环境的点云信息、语义信息和拓扑信息,三维高精度地图的层次结构分明,不同层次的信息耦合度低,便于后期在地图中修改和添加新的数据。
较佳地,所述环境信息还包括惯导信息;在从各帧环境点云数据中提取障碍物点云数据和路面点云数据之前,还包括:
针对每一帧的环境点云数据,执行如下操作:
对所述环境点云数据进行滤波处理,去除所述环境点云数据中的噪声和离群点;
融合滤波后的环境点云数据和所述惯导信息,消除所述滤波后的环境点云数据的变形。
上述较佳的实施方式表征,还可以在从各帧环境点云数据中提取障碍物点云数据和路面点云数据之前,对每一帧的环境点云数据做如下预处理:对环境点云数据进行滤波处理,以去除噪声和离群点,利用惯导传感器获取移动机器人在移动过程中的惯导信息,融合滤波后的环境点云数据和所述惯导信息,消除滤波后环境点云数据的变形,经过预处理后的环境点云数据更加精确,从而,使得生成的三维高精度地图更加精确。
较佳地,所述环境点云数据包括所述移动机器人周围环境的三维数据点坐标,所述惯导信息包括所述移动机器人的加速度。
较佳地,针对每一帧的环境点云数据,从所述环境点云数据中提取障碍物点云数据,具体包括:
根据预设的第一神经网络模型对所述环境点云数据进行物体检测和语义分割,确定并提取障碍物点云数据。
上述较佳的实施方式中,基于深度学习的物体检测和语义分割过程,算法运行速度快,生成的三维高精度地图更加稳定可靠。
较佳地,针对每一帧的环境点云数据,从所述环境点云数据中提取路面点云数据,具体包括:
以所述移动机器人为中心,根据预设步长生成所述环境点云数据的障碍物栅格图;
根据环境点云数据的各三维数据点坐标投影到所述障碍物栅格图上的x轴坐标和y轴坐标所对应的z轴坐标,计算在每个栅格内的最大高度差;
将所述最大高度差小于预设阈值的三维数据点坐标标记为路面点;
提取所述路面点对应的三维数据点。
较佳地,根据所述障碍物点云数据生成地图点云层,具体包括:
针对每一帧的障碍物点云数据,执行如下操作:
根据第一预设算法将所述障碍物点云数据和存储的上一帧的障碍物点云数据进行点云配准;
根据进行了点云配准后的障碍物点云数据完成地图点云层的一次增量更新。
较佳地,根据所述障碍物点云数据生成地图语义层,具体包括:
针对每一帧的障碍物点云数据,执行如下操作:
针对由所述障碍物点云数据中组成的结构化障碍物,根据预设的第二神经网络模型对所述障碍物点云数据进行语义分割,确定所述结构化障碍物的语义并标注;
对由所述障碍物点云数据中组成的非结构化障碍物的语义进行手工标注;
根据标注的语义完成地图语义层的一次增量更新。
上述较佳的是实施方式中,针对由障碍物点云数据中组成的结构化障碍物,利用基于深度学习的神经网络模型自动提取语义信息并标注,针对由障碍物点云数据中组成的非结构化障碍物的语义进行手工标注,降低自动标注过程中出现错误的几率,保证了地图的准确性。
具体地,根据所述路面点云数据生成地图拓扑层,具体包括:
针对每一帧的路面点云数据,执行如下操作:
将所述路面点对应的三维数据点坐标投影到二维平面;
在所述二维平面上提取道路边缘点;
根据所述提取的道路边缘点利用第二预设算法拟合道路曲线;
根据拟合的各道路曲线,获取所述各道路的属性信息并标注,所述道路的属性信息包括道路的长度、宽带、起点和终点信息,其中,所述道路曲线的起点和终点由人工指定;
根据所述起点和终点信息确定各道路的连接信息;
根据所述各道路曲线、所述道路的连接信息、标注的属性信息和存储的上一帧的道路的连接信息完成地图拓扑层的一次增量更新。
上述较佳的实施方式中,针对每一帧的路面点云数据,从路面点云数据中提取出道路边界,获取各道路的属性信息并进行标注,并根据各道路曲线、道路的连接信息和标注信息和存储的上一帧的道路的连接信息完成地图拓扑层的一次增量更新,直至处理完最后一帧路面点云数据,生成地图拓扑层。
第二方面,本发明实施例提供了一种三维高精度地图生成装置,包括:
采集单元,用于实时采集移动机器人在移动过程中周围的环境信息,所述环境信息包括环境点云数据,所述环境点云数据为利用安装在所述移动机器人上的激光雷达获取的;
提取单元,用于分别从各帧环境点云数据中提取障碍物点云数据和路面点云数据;
第一生成单元,用于根据所述障碍物点云数据生成地图点云层和地图语义层,所述地图点云层为用于表征障碍物信息的图层,所述地图语义层为用于表征障碍物的语义信息的图层;
第二生成单元,用于根据所述路面点云数据生成地图拓扑层,所述地图拓扑层为用于表征道路拓扑信息的图层;
第三生成单元,用于根据所述地图点云层、所述地图语义层和所述地图拓扑层生成三维高精度地图。
较佳地,所述环境信息还包括惯导信息;
所述装置,还包括:
预处理单元,用于在从各帧环境点云数据中提取障碍物点云数据和路面点云数据之前,针对每一帧的环境点云数据,执行如下操作:对所述环境点云数据进行滤波处理,去除所述环境点云数据中的噪声和离群点;融合滤波后的环境点云数据和所述惯导信息,消除所述滤波后的环境点云数据的变形。
较佳地,所述环境点云数据包括所述移动机器人周围环境的三维数据点坐标,所述惯导信息包括所述移动机器人的加速度。
较佳地,所述提取单元,具体用于针对每一帧的环境点云数据,根据预设的第一神经网络模型对所述环境点云数据进行物体检测和语义分割,确定并提取障碍物点云数据。
较佳地,所述提取单元,具体用于针对每一帧的环境点云数据,以所述移动机器人为中心,根据预设步长生成所述环境点云数据的障碍物栅格图;根据环境点云数据的各三维数据点坐标投影到所述障碍物栅格图上的x轴坐标和y轴坐标所对应的z轴坐标,计算在每个栅格内的最大高度差;将所述最大高度差小于预设阈值的三维数据点坐标标记为路面点;提取所述路面点对应的三维数据点。
较佳地,所述第一生成单元,具体用于针对每一帧的障碍物点云数据,执行如下操作:根据第一预设算法将所述障碍物点云数据和存储的上一帧的障碍物点云数据进行点云配准;根据进行了点云配准后的障碍物点云数据完成地图点云层的一次增量更新。
较佳地,所述第一生成单元,具体用于针对每一帧的障碍物点云数据,执行如下操作:针对由所述障碍物点云数据中组成的结构化障碍物,根据预设的第二神经网络模型对所述障碍物点云数据进行语义分割,确定所述结构化障碍物的语义并标注;对由所述障碍物点云数据中组成的非结构化障碍物的语义进行手工标注;根据标注的语义完成地图语义层的一次增量更新。
较佳地,所述第二生成单元,具体用于针对每一帧的路面点云数据,执行如下操作:将所述路面点对应的三维数据点坐标投影到二维平面;在所述二维平面上提取道路边缘点;根据所述提取的道路边缘点利用第二预设算法拟合道路曲线;根据拟合的各道路曲线,获取所述各道路的属性信息并标注,所述道路的属性信息包括道路的长度、宽带、起点和终点信息,其中,所述道路曲线的起点和终点由人工指定;根据所述起点和终点信息确定各道路的连接信息;根据所述各道路曲线、所述道路的连接信息、标注的属性信息和存储的上一帧的道路的连接信息完成地图拓扑层的一次增量更新。
本发明提供的三维高精度地图生成装置的技术效果可以参见上述第一方面或第一方面的各个实现方式的技术效果,此处不再赘述。
第三方面,本发明实施例提供了一种电子设备,包括存储器、处理器及存储在所述存储器上并可在所述处理器上运行的计算机程序,所述处理器执行所述程序时实现本发明所述的三维高精度地图生成方法。
第四方面,本发明实施例提供了一种计算机可读存储介质,其上存储有计算机程序,该程序被处理器执行时实现本发明所述的三维高精度地图生成方法中的步骤。
本发明的其它特征和优点将在随后的说明书中阐述,并且,部分地从说明书中变得显而易见,或者通过实施本发明而了解。本发明的目的和其他优点可通过在所写的说明书、权利要求书、以及附图中所特别指出的结构来实现和获得。
附图说明
此处所说明的附图用来提供对本发明的进一步理解,构成本发明的一部分,本发明的示意性实施例及其说明用于解释本发明,并不构成对本发明的不当限定。在附图中:
图1为本发明实施例提供的三维高精度地图生成方法的实施流程示意图;
图2为本发明实施例中,对环境点云数据进行预处理的实施流程示意图;
图3为本发明实施例中,从环境点云数据中提取路面点云数据的实施流程示意图;
图4为本发明实施例中,生成地图点云层的实施流程示意图;
图5为本发明实施例中,生成地图语义层的实施流程示意图;
图6为本发明实施例中,生成地图拓扑层的实施流程示意图;
图7为本发明实施例提供的三维高精度地图生成装置的结构示意图;
图8为本发明实施例提供的电子设备的结构示意图。
具体实施方式
为了解决传统的三维高精度地图的生成方式成本高、效率低的问题,本发明实施例提供了一种三维高精度地图生成方法及装置。
以下结合说明书附图对本发明的优选实施例进行说明,应当理解,此处所描述的优选实施例仅用于说明和解释本发明,并不用于限定本发明,并且在不冲突的情况下,本发明中的实施例及实施例中的特征可以相互组合。
如图1所示,其为本发明实施例提供的三维高精度地图生成方法的实施流程示意图,可以包括以下步骤:
S11、实时采集移动机器人在移动过程中周围的环境信息,所述环境信息包括环境点云数据。
具体实施时,移动机器人实时采集其在移动过程中周围的环境信息,所述环境信息包括环境点云数据,所述环境点云数据为利用安装在所述移动机器人上的激光雷达获取的。
具体地,本发明实施例中,移动机器人配备有激光雷达,利用激光雷达获取环境点云数据,所述环境点云数据包括所述移动机器人周围环境的三维数据点坐标。
较佳地,所述环境信息还可以包括惯导信息,移动机器人还可以配备IMU(Inertial Measurement Unit,惯性测量单元)或者惯性导航传感器,利用IMU或者惯性导航传感器获取移动机器人的惯导信息,所述惯导信息包括所述移动机器人的加速度。
较佳地,在采集到所述环境信息后,可以对所述环境点云数据进行预处理。
具体地,针对每一帧的环境点云数据,对所述环境点云数据进行滤波和消除变形处理。
具体实施时,针对每一帧的环境点云数据,可以通过如图2所示的流程对所述环境点云数据进行预处理,包括以下步骤:
S21、对所述环境点云数据进行滤波处理,去除所述环境点云数据中的噪声和离群点。
本步骤中,可以但不限于采用以下滤波方法对所述环境点云数据进行滤波处理:半径滤波、条件滤波、体素滤波或统计滤波,以去除所述环境点云数据中的噪声和离群点。
S22、融合滤波后的环境点云数据和所述惯导信息,消除所述滤波后的环境点云数据的变形。
具体实施时,激光雷达匀速运转,生成每帧环境点云数据扫描经过的时间(即扫描周期)固定为Tperiod,扫描周期Tperiod的起始时间分别记为Tstart和Tend,在一个扫描周期内,激光雷达在Tstart时刻接收到第一个三维数据点Pstart,在Tend时刻接收到最后一个三维数据点Pend,假设点Pstart的水平扫描角度为Angfirst,点Pend的水平扫描角度为Anglast,则当前帧在Tperiod时间内扫描过的总角度Angscan=Anglast-Angfirst。在扫描周期Tperiod内的任意一三维数据点P的水平扫描角度为AngP,则点P相对于Pstart的时间偏移通过以下公式计算获得:
则点P的时间戳为:TP=Tstart+TPscanshift。
进而,将惯导信息数据与P点根据时间戳大小进行排序,找到点P的时间戳之前和之后最近的两个惯导信息数据,时间戳在点P的时间戳之前的惯导信息数据记为gyro(k),时间戳在点P的时间戳之后的惯导信息数据记为gyro(k+1),其中,k表示个数,gyro(k)表示第k个惯导信息数据,gyro(k+1)表示第k+1个惯导信息数据。
进一步地,计算gyro(k)和gyro(k+1)之间移动机器人发生的位移。假设移动机器人做匀速运动,gyro(k)的加速度为a(k),gyro(k)和gyro(k+1)之间的时间差为t(k),则移动机器人从gyro(k)时刻移动到gyro(k+1)时,速度v(k+1)=v(k)+a(k)*t(k),初始时,v(0)=0,其中,v(k+1)表示移动机器人在gyro(k+1)时刻的速度,v(k)表示移动机器人在gyro(k)时刻的速度,则gyro(k+1)相对于gyro(k)的位置偏移为:Shift(k+1)=v(k)*t(k)+0.5*a(k)*t2(k),其中,表示Shift(k+1)gyro(k+1)相对于gyro(k)的位置偏移。
进一步地,以点Pstart作为基准点,它的位置偏移为0,则可以计算出gyro(k)数据到达时移动机器人相对于点Pstart的位置偏移Shiftstart(k):
其中,shift(i)表示第i个惯导信息数据相对于点Pstart的位置偏移。
通过线性插值法计算点P相对于点Pstart的位置偏移Shiftstart(P):
其中,Shiftstart(P)表示点P相对于点Pstart的位置偏移;
Shiftstart(k)表示gyro(k)数据到达时移动机器人相对于点Pstart的位置偏移;
Shiftstart(k+1)表示gyro(k+1)数据到达时移动机器人相对于点Pstart的位置偏移;
Tp表示点P的时间戳;
Tgyro(k)表示gyro(k)的时间戳;
Tgyro(k+1)表示gyro(k+1)的时间戳。
进而,重新调整点P的三维坐标,消除移动机器人运动导致的环境点云数据的变形:
P’.pos=P.pos+Shiftstart(P)
其中,P’.pos表示消除变形后的点P的三维坐标;
P.pos表示点P的初始三维坐标。
S12、分别从各帧环境点云数据中提取障碍物点云数据和路面点云数据。
本步骤中,针对每一帧的环境点云数据,通过以下方法从所述环境点云数据中提取障碍物点云数据:根据预设的第一神经网络模型对所述环境点云数据进行物体检测和语义分割,确定并提取障碍物点云数据。
具体实施时,障碍物包括结构化障碍物和非结构化障碍物。其中,结构化障碍物为具有相对规则的点云形状的障碍物,例如园区中的标志牌、告示牌、固定垃圾桶、路灯杆和树木等,非结构化障碍物为具有不规则的点云形状的障碍物,例如园区入口、园区中的商店、运动场和居民楼等。
具体地,第一神经网络模型可以但不限于采用PointNet2深度学习神经网络模型,进行物体检测和语义分割,本发明实施例中,预先采集机器人移动区域内的环境点云数据,人工为环境点云数据的三维数据点添加属性标签,其中,属性标签包括上述各结构化障碍物和非结构化障碍物,将添加过标签的环境点云数据的三维数据点分为训练集和测试集,训练PointNet2深度学习神经网络直至收敛,保存网络参数,得到训练好的神经网络模型。
具体实施时,将所述环境点云数据作为所述预先训练好的第一神经网络模型的输入,进行物体检测和语义分割,输出组成各障碍物对应的三维数据点坐标。
本发明实施例中,针对每一帧的环境点云数据,可以根据如图3所示的流程从所述环境点云数据中提取路面点云数据,包括以下步骤:
S31、以所述移动机器人为中心,根据预设步长生成所述环境点云数据的障碍物栅格图。
本步骤中,预设步长可以根据实际经验自行设定,本发明实施例对此不作限定。例如,预设步长可以为10cm,则栅格大小为10cm*10cm。
S32、根据环境点云数据的各三维数据点坐标投影到所述障碍物栅格图上的x轴坐标和y轴坐标所对应的z轴坐标,计算在每个栅格内的最大高度差。
本步骤中,根据所述环境点云数据的各三维数据点坐标投影到所述障碍物栅格图上的x轴坐标和y轴坐标所对应的z轴坐标,计算在每个栅格内的最大高度差。
S33、将所述最大高度差小于预设阈值的三维数据点坐标标记为路面点。
本步骤中,预设阈值可以根据实际经验设定,本发明实施例对此不作限定。
S34、提取所述路面点对应的三维数据点。
具体地,提取所述路面点对应的三维数据点坐标,即路面点云数据。
S13、根据所述障碍物点云数据生成地图点云层和地图语义层。
所述地图点云层为用于表征障碍物信息的图层,所述地图语义层为用于表征障碍物的语义信息的图层。
本步骤中,可以根据如图4所示的流程生成地图点云层,包括以下步骤:
针对每一帧的障碍物点云数据,执行如下操作:
S41、根据第一预设算法将所述障碍物点云数据和存储的上一帧的障碍物点云数据进行点云配准。
点云配准就是计算获得两点集之间的旋转变换矩阵和三维平移变量,使得两点集重叠区域的点对齐。要获得旋转变换矩阵和三维平移变量,须至少知道三个非共线点的对应关系。
本发明实施例中,预设算法可以但不限于使用正态分布变换(NormalDistribution Transform,NDT)算法。初始时,地图点云层为空,针对从激光雷达获取的第二帧环境点云数据提取出的障碍物点云数据,可以根据从激光雷达获取的第一帧环境点云数据提取出的障碍物点云数据进行点云配准,获得旋转变换矩阵和三维平移变量,根据获得的旋转变换矩阵和三维平移变量进行点云配准。后续以同样的方式,从第三帧环境点云数据提取出的障碍物点云数据开始,均根据从上一帧环境点云数据提取出的障碍物点云数据进行点云配准。
具体地,将当前帧障碍物点云数据和存储的上一帧障碍物点云数据进行点云配准,获得坐标变换矩阵M,已知坐标变换由旋转和平移组成,假设当前帧障碍物点云数据中三维数据点P的坐标是(x,y,z)T,记为通过旋转和平移,可获得点P在地图点云层中对应点P’的坐标(x’,y’,z’)T,记为用R表示3*3旋转变换矩阵,T表示3*1平移矩阵,则:记坐标变换矩阵则公式变为如下形式:
本发明实施例中,可以采用PCL(Point Cloud Library,点云库)提供的NDT算法工具库计算所述坐标变换矩阵M,仅需将当前帧障碍物点云数据和存储的包含了上一帧的障碍物点云数据的地图点云层作为NDT算法的数据输入,设置算法参数,经过NDT计算,即可自动输出坐标变换矩阵M。NDT算法的参数主要包括连续两次变换之间增量变化阈值trans_epsilon,最大搜索步长step_size,网格体素分辨率resolution,最大迭代次数max_iterations等,具体参数大小可以通过调试确定。
S42、根据进行了点云配准后的障碍物点云数据完成地图点云层的一次增量更新。
具体实施时,针对每一帧的障碍物点云数据,根据坐标变换矩阵M,将当前帧障碍物点云数据和存储的上一帧的障碍物点云数据进行坐标系转换,根据上述公式以地图点云层坐标系为基准,将进行了点云配准后的障碍物点云数据投影到地图点云层中,完成地图点云层的一次增量更新,直至移动机器人遍历完整个园区停止移动为止。
较佳地,在进行点云配准之前,还可以对障碍物点云数据和已生成的地图点云层中的点云数据进行下采样,本发明实施例中,可以但不限于采用体素滤波方法进行下采样,以加快配准速度。
本发明实施例中,可以根据如图5所示的流程生成地图语义层,包括以下步骤:
针对每一帧的障碍物点云数据,执行如下操作:
S51、针对由所述障碍物点云数据中组成的结构化障碍物,根据预设的第二神经网络模型对所述障碍物点云数据进行语义分割,确定所述结构化障碍物的语义并标注。
本步骤中,针对由所述障碍物点云数据中组成的结构化障碍物,根据预设的第二神经网络模型对所述障碍物点云数据进行语义分割,确定所述结构化障碍物的语义并标注。第二神经网络模型可以但不限于采用PointNet深度学习神经网络模型,对障碍物点云数据进行语义分割。具体地,将障碍物点云数据作为所述预先训练好的第二神经网络模型的输入,进行语义分割,输出各结构化障碍物的位置坐标和各结构化障碍物对应的语义,将所述结构化障碍物对应的语义标注在其位置坐标的对应位置处。
较佳地,第二神经网络模型的输出还可以包括语义准确度信息。一般地,语义准确度在0.5~1.0之间。
S52、对由所述障碍物点云数据中组成的非结构化障碍物的语义进行手工标注。
本步骤中,对由所述障碍物点云数据中组成的非结构化障碍物的语义进行手工标注。手工标注的语义信息准确度为1。
S53、根据标注的语义完成地图语义层的一次增量更新。
本步骤中,比较当前语义元素列表和地图语义层语义元素列表,通过障碍物位置坐标和障碍物类型,判断是否为新出现的语义元素,或者是已经出现在地图语义层中的元素,如果是新出现的语义元素,则直接加入历史地图语义层,完成地图语义层的一次增量更新,初始时,地图语义层为空。如果新出现的语义元素已经在相同的位置处出现,则比较两个语义元素的准确度大小,在地图语义层中保留准确度大的语义元素。
本发明实施例采用语义自动标注配合人工标注的方式,排除自动标记过程中容易出现错误的地方,保证了地图的准确性。
S14、根据所述路面点云数据生成地图拓扑层。
所述地图拓扑层为用于表征道路拓扑信息的图层。
具体实施时,可以按照如图6所示的流程生成地图拓扑层,包括以下步骤:
针对每一帧的路面点云数据,执行如下操作:
S61、将所述路面点对应的三维数据点坐标投影到二维平面。
本步骤中,建立以移动机器人为参考的二维平面坐标系,令x轴正向指向移动机器人右侧,y轴指向移动机器人前方,将路面点云数据中的各三维数据点坐标投影到二维平面坐标系,生成距离移动机器人左右各第一预设距离、前方第二预设距离、后方第三预设距离的局部区域二维平面栅格地图,其中,第一预设距离、第二预设距离、第三预设距离以及栅格的大小可以根据经验值自行设定,本发明实施例对此不作限定。
S62、在所述二维平面上提取道路边缘点。
具体实施时,例设第一预设距离取值为6米,第二预设距离取值为15米,第三预设距离取值为5米,栅格的大小设置为10cm*10cm,则局部地图大小为120×200,移动机器人的中心点处于二维平面坐标系的原点(0,0),y轴范围为-50~150。在二维平面坐标系上,从左至右射出一条平行于x轴的直线:y=yi,i=-50,-49,...,149,150,y=yi与投影到二维平面的路面点云数据相交,第一个交点记为道路左边缘点lk=(lxk,lyk),最后一个交点记为道路右边缘点rk=(rxk,ryk),保证lk和rk不重合,且横向距离大于最小阈值MinDis,将lk点加入左边缘点集合Left,rk加入右边缘点集合Right,获得n对道路边缘点,记Left={(lxk,lyk)|k=1,2,......,n},Right={(rxk,ryk)|k=1,2,......,n}。其中,最小阈值可以根据经验值设定,本发明实施例对此不作限定。
S63、根据所述提取的道路边缘点利用第二预设算法拟合道路曲线。
本步骤中,第二预设算法可以但不限于使用最小二乘法,本发明实施例对此不作限定。
具体实施时,道路边界可以使用二次曲线y=a0+a1x+a2x2进行拟合。
记A=(a0 a1 a2)T,Y(y1 y2......yn)T
Y=XA
则:A=(XTX)-1XTY
其中,X矩阵为横坐标点矩阵,Y矩阵为纵坐标点矩阵,A矩阵为参数矩阵。
分别将左边缘点集合Left={(lxk,lyk)|k=1,2,......,n}和右边缘点集合Right={(rxk,ryk)|k=1,2,......,n}代入公式A=(XTX)-1XTY中,计算得出道路左边界曲线和道路右边界曲线。
S64、根据拟合的各道路曲线,获取所述各道路的属性信息并标注,所述道路的属性信息包括道路的长度、宽带、起点和终点信息。
本步骤中,根据拟合的各道路对应的左边界道路曲线和右边界道路曲线,获取所述各道路的属性信息并标注,所述道路的属性信息包括道路的长度、宽带、起点和终点信息,其中,所述道路曲线的起点和终点由人工指定。
S65、根据所述起点和终点信息确定各道路的连接信息。
本步骤中,当一条道路曲线的终点与另一条道路曲线的终点的位置相同,则将该点确定为一个连接点,用于连接这两条道路。
S66、根据所述各道路曲线、所述道路的连接信息、标注的属性信息和存储的上一帧的道路的连接信息完成地图拓扑层的一次增量更新。
初始时,地图拓扑层为空。将当前帧的道路曲线的起点与上一帧的道路曲线的终点相同的道路曲线进行连接,并在连接后的道路曲线上相应地标注属性信息,完成地图拓扑层的一次增量更新。
S15、根据所述地图点云层、所述地图语义层和所述地图拓扑层生成三维高精度地图。
本发明实施例提供的三维高精度地图生成方法中,移动机器人实施采集其在移动过程中周围的环境信息,所述环境信息包括利用安装在所述移动机器人上的激光雷达获取的环境点云数据,分别从各帧环境点云数据中提取障碍物点云数据和路面点云数据,进而,根据提取的障碍物点云数据生成地图点云层和地图语义层,其中,地图点云层为用于表征障碍物信息的图层,地图语义层为用于表征障碍物的语义信息的图层,根据提取的路面点云数据生成地图拓扑层,地图拓扑层为用于表征道路拓扑信息的图层,进一步地,根据生成的地图点云层、地图语义层和地图拓扑层共同生成三维高精度地图。相比于现有技术,本申请在构建三维高精度地图采集数据时,所使用的传感器可以仅仅依赖一个激光雷达,在成本可控的情况下生成三维高精度地图,有效降低了地图构建的成本,提高了地图生成效率。并且,本申请构建的三维高精度地图采用分层的模式,分别包含了环境的点云信息、语义信息和拓扑信息,三维高精度地图的层次结构分明,不同层次的信息耦合度低,便于后期在地图中修改和添加新的数据。
基于同一发明构思,本发明实施例还提供了一种三维高精度地图生成装置,由于上述三维高精度地图生成装置解决问题的原理与三维高精度地图生成方法相似,因此上述系统的实施可以参见方法的实施,重复之处不再赘述。
如图7所示,其为本发明实施例提供的三维高精度地图生成装置的结构示意图,可以包括:
采集单元71,用于实时采集移动机器人在移动过程中周围的环境信息,所述环境信息包括环境点云数据,所述环境点云数据为利用安装在所述移动机器人上的激光雷达获取的;
提取单元72,用于分别从各帧环境点云数据中提取障碍物点云数据和路面点云数据;
第一生成单元73,用于根据所述障碍物点云数据生成地图点云层和地图语义层,所述地图点云层为用于表征障碍物信息的图层,所述地图语义层为用于表征障碍物的语义信息的图层;
第二生成单元74,用于根据所述路面点云数据生成地图拓扑层,所述地图拓扑层为用于表征道路拓扑信息的图层;
第三生成单元75,用于根据所述地图点云层、所述地图语义层和所述地图拓扑层生成三维高精度地图。
较佳地,所述环境信息还包括惯导信息;
所述装置,还包括:
预处理单元,用于在从各帧环境点云数据中提取障碍物点云数据和路面点云数据之前,针对每一帧的环境点云数据,执行如下操作:对所述环境点云数据进行滤波处理,去除所述环境点云数据中的噪声和离群点;融合滤波后的环境点云数据和所述惯导信息,消除所述滤波后的环境点云数据的变形。
较佳地,所述环境点云数据包括所述移动机器人周围环境的三维数据点坐标,所述惯导信息包括所述移动机器人的加速度。
较佳地,所述提取单元72,具体用于针对每一帧的环境点云数据,根据预设的第一神经网络模型对所述环境点云数据进行物体检测和语义分割,确定并提取障碍物点云数据。
较佳地,所述提取单元72,具体用于针对每一帧的环境点云数据,以所述移动机器人为中心,根据预设步长生成所述环境点云数据的障碍物栅格图;根据环境点云数据的各三维数据点坐标投影到所述障碍物栅格图上的x轴坐标和y轴坐标所对应的z轴坐标,计算在每个栅格内的最大高度差;将所述最大高度差小于预设阈值的三维数据点坐标标记为路面点;提取所述路面点对应的三维数据点。
较佳地,所述第一生成单元73,具体用于针对每一帧的障碍物点云数据,执行如下操作:根据第一预设算法将所述障碍物点云数据和存储的上一帧的障碍物点云数据进行点云配准;根据进行了点云配准后的障碍物点云数据完成地图点云层的一次增量更新。
较佳地,所述第一生成单元73,具体用于针对每一帧的障碍物点云数据,执行如下操作:针对由所述障碍物点云数据中组成的结构化障碍物,根据预设的第二神经网络模型对所述障碍物点云数据进行语义分割,确定所述结构化障碍物的语义并标注;对由所述障碍物点云数据中组成的非结构化障碍物的语义进行手工标注;根据标注的语义完成地图语义层的一次增量更新。
较佳地,所述第二生成单元74,具体用于针对每一帧的路面点云数据,执行如下操作:将所述路面点对应的三维数据点坐标投影到二维平面;在所述二维平面上提取道路边缘点;根据所述提取的道路边缘点利用第二预设算法拟合道路曲线;根据拟合的各道路曲线,获取所述各道路的属性信息并标注,所述道路的属性信息包括道路的长度、宽带、起点和终点信息,其中,所述道路曲线的起点和终点由人工指定;根据所述起点和终点信息确定各道路的连接信息;根据所述各道路曲线、所述道路的连接信息、标注的属性信息和存储的上一帧的道路的连接信息完成地图拓扑层的一次增量更新。
基于同一技术构思,本发明实施例还提供了一种电子设备800,参照图8所示,电子设备800用于实施上述方法实施例记载的三维高精度地图生成方法,该实施例的电子设备800可以包括:存储器801、处理器802以及存储在所述存储器中并可在所述处理器上运行的计算机程序,例如三维高精度地图生成程序。所述处理器执行所述计算机程序时实现上述各个三维高精度地图生成方法实施例中的步骤,例如图1所示的步骤S11。或者,所述处理器执行所述计算机程序时实现上述各装置实施例中各模块/单元的功能,例如71。
本发明实施例中不限定上述存储器801、处理器802之间的具体连接介质。本申请实施例在图8中以存储器801、处理器802之间通过总线803连接,总线803在图8中以粗线表示,其它部件之间的连接方式,仅是进行示意性说明,并不引以为限。所述总线803可以分为地址总线、数据总线、控制总线等。为便于表示,图8中仅用一条粗线表示,但并不表示仅有一根总线或一种类型的总线。
存储器801可以是易失性存储器(volatile memory),例如随机存取存储器(random-access memory,RAM);存储器801也可以是非易失性存储器(non-volatilememory),例如只读存储器,快闪存储器(flash memory),硬盘(hard disk drive,HDD)或固态硬盘(solid-state drive,SSD)、或者存储器801是能够用于携带或存储具有指令或数据结构形式的期望的程序代码并能够由计算机存取的任何其他介质,但不限于此。存储器801可以是上述存储器的组合。
处理器802,用于实现如图1所示的一种三维高精度地图生成方法,包括:
所述处理器802,用于调用所述存储器801中存储的计算机程序执行如图1中所示的步骤S11~步骤15。
本申请实施例还提供了一种计算机可读存储介质,存储为执行上述处理器所需执行的计算机可执行指令,其包含用于执行上述处理器所需执行的程序。
在一些可能的实施方式中,本发明提供的三维高精度地图生成方法的各个方面还可以实现为一种程序产品的形式,其包括程序代码,当所述程序产品在电子设备上运行时,所述程序代码用于使所述电子设备执行本说明书上述描述的根据本发明各种示例性实施方式的三维高精度地图生成方法中的步骤,例如,所述电子设备可以执行如图1中所示的步骤S11~步骤15。
所述程序产品可以采用一个或多个可读介质的任意组合。可读介质可以是可读信号介质或者可读存储介质。可读存储介质例如可以是——但不限于——电、磁、光、电磁、红外线、或半导体的系统、装置或器件,或者任意以上的组合。可读存储介质的更具体的例子(非穷举的列表)包括:具有一个或多个导线的电连接、便携式盘、硬盘、随机存取存储器(RAM)、只读存储器(ROM)、可擦式可编程只读存储器(EPROM或闪存)、光纤、便携式紧凑盘只读存储器(CD-ROM)、光存储器件、磁存储器件、或者上述的任意合适的组合。
本发明的实施方式的用于三维高精度地图生成的程序产品可以采用便携式紧凑盘只读存储器(CD-ROM)并包括程序代码,并可以在计算设备上运行。然而,本发明的程序产品不限于此,在本文件中,可读存储介质可以是任何包含或存储程序的有形介质,该程序可以被指令执行系统、装置或者器件使用或者与其结合使用。
可读信号介质可以包括在基带中或者作为载波一部分传播的数据信号,其中承载了可读程序代码。这种传播的数据信号可以采用多种形式,包括——但不限于——电磁信号、光信号或上述的任意合适的组合。可读信号介质还可以是可读存储介质以外的任何可读介质,该可读介质可以发送、传播或者传输用于由指令执行系统、装置或者器件使用或者与其结合使用的程序。
可读介质上包含的程序代码可以用任何适当的介质传输,包括——但不限于——无线、有线、光缆、RF等等,或者上述的任意合适的组合。
可以以一种或多种程序设计语言的任意组合来编写用于执行本发明操作的程序代码,所述程序设计语言包括面向对象的程序设计语言-诸如Java、C++等,还包括常规的过程式程序设计语言-诸如“C”语言或类似的程序设计语言。程序代码可以完全地在用户计算设备上执行、部分地在用户设备上执行、作为一个独立的软件包执行、部分在用户计算设备上部分在远程计算设备上执行、或者完全在远程计算设备或服务器上执行。在涉及远程计算设备的情形中,远程计算设备可以通过任意种类的网络——包括局域网(LAN)或广域网(WAN)-连接到用户计算设备,或者,可以连接到外部计算设备(例如利用因特网服务提供商来通过因特网连接)。
应当注意,尽管在上文详细描述中提及了装置的若干单元或子单元,但是这种划分仅仅是示例性的并非强制性的。实际上,根据本发明的实施方式,上文描述的两个或更多单元的特征和功能可以在一个单元中具体化。反之,上文描述的一个单元的特征和功能可以进一步划分为由多个单元来具体化。
此外,尽管在附图中以特定顺序描述了本发明方法的操作,但是,这并非要求或者暗示必须按照该特定顺序来执行这些操作,或是必须执行全部所示的操作才能实现期望的结果。附加地或备选地,可以省略某些步骤,将多个步骤合并为一个步骤执行,和/或将一个步骤分解为多个步骤执行。
本领域内的技术人员应明白,本发明的实施例可提供为方法、装置、或计算机程序产品。因此,本发明可采用完全硬件实施例、完全软件实施例、或结合软件和硬件方面的实施例的形式。而且,本发明可采用在一个或多个其中包含有计算机可用程序代码的计算机可用存储介质(包括但不限于磁盘存储器、CD-ROM、光学存储器等)上实施的计算机程序产品的形式。
本发明是参照根据本发明实施例的方法、设备(装置)、和计算机程序产品的流程图和/或方框图来描述的。应理解可由计算机程序指令实现流程图和/或方框图中的每一流程和/或方框、以及流程图和/或方框图中的流程和/或方框的结合。可提供这些计算机程序指令到通用计算机、专用计算机、嵌入式处理机或其他可编程数据处理设备的处理器以产生一个机器,使得通过计算机或其他可编程数据处理设备的处理器执行的指令产生用于实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能的装置。
这些计算机程序指令也可存储在能引导计算机或其他可编程数据处理设备以特定方式工作的计算机可读存储器中,使得存储在该计算机可读存储器中的指令产生包括指令装置的制造品,该指令装置实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能。
这些计算机程序指令也可装载到计算机或其他可编程数据处理设备上,使得在计算机或其他可编程设备上执行一系列操作步骤以产生计算机实现的处理,从而在计算机或其他可编程设备上执行的指令提供用于实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能的步骤。
尽管已描述了本发明的优选实施例,但本领域内的技术人员一旦得知了基本创造性概念,则可对这些实施例做出另外的变更和修改。所以,所附权利要求意欲解释为包括优选实施例以及落入本发明范围的所有变更和修改。
显然,本领域的技术人员可以对本发明进行各种改动和变型而不脱离本发明的精神和范围。这样,倘若本发明的这些修改和变型属于本发明权利要求及其等同技术的范围之内,则本发明也意图包含这些改动和变型在内。