发明内容
本发明的目的在于提供一种基于激光雷达的轻量化环境感知地图构建方法,以解决现有的环境感知地图构建方法中,由于采用多传感器融合感知方式,而导致的操作复杂、成本高、性价比低等问题。
本发明是采用以下技术方案实现的:
一种基于激光雷达的轻量化环境感知地图构建方法,包括如下步骤:
S1:基于激光雷达采集并生成障碍物数据;
S2:计算障碍物的膨胀系数k,并基于该膨胀系数k对感知地图进行更新,生成轻量化感知地图;
所述S1具体包括如下子步骤:
S11:启动安装在自主导航车辆上的激光雷达;
S12:接收激光雷达数据;
S13:提取感兴趣区域数据;
S14:地面分离;
S15:去除离群点;
S16:维度压缩;
所述S2具体包括如下子步骤:
S21:重置感知地图;
S22:障碍物坐标转换;
S23:获取地图索引值;
S24:计算膨胀系数k;
S25:更新感知地图;
其中,所述S24中,根据自主导航车辆的大小L以及感知地图的分辨率res,计算障碍物的膨胀系数k。
本方法中,通过上述步骤,可将三维空间感知压缩转化为带有高度信息的二维平面地图,并且该二维平面地图为数据量小、处理速度快的轻量化地图;在此过程中,使用的是单一的激光雷达,不需要多种传感器进行融合感知。基于此,与现有的多传感器融合感知方式相比,在使用同等传感器个数的情况下,根据分辨率的设置及检测高度的需求不同,本方法能够实现大幅度的算力缩减,并且对感知系统的搭建操作较为简单,还可以在较大程度上降低传感器和处理器的成本。特别是对于一些小型低速的自主导航车辆,它们的运行环境固定明确、障碍物信息不复杂,所以更加适合使用本方法。
进一步地,所述S11中,根据激光雷达的型号及驱动代码,启动激光雷达,若启动成功则执行步骤S12,若启动失败则退出程序;所述S12中,获取激光雷达所采集到的数据集P,并将格式为ROS点云数据的数据集P转换成格式为PCL点云数据的数据集Q;所述S13中,根据感知需求范围大小对数据集Q进行感兴趣区域数据截取,得到数据集Qr’;所述S14中,根据激光雷达的安装姿态,计算地面所在平面的方程式,并根据该方程式将数据集Qr’中的地面数据去除掉,得到数据集Qr;所述S15中,使用滤波器将数据集Qr中的异常离散点过滤去除,得到数据集Qf’;所述S16中,对数据集Qf’进行数据压缩,得到障碍物数据集Qf。
进一步地,所述S21中,在收到障碍物数据集Qf前,先将感知地图进行重置,将感知地图中原有的障碍物信息清除;所述S22中,将障碍物数据集Qf从传感器坐标系T2下转到车体坐标系T1下;所述S23中,计算障碍物数据集Qf中每个点所对应的感知地图的索引值i;所述S24中,障碍物的膨胀系数k= L/res,其中L为自主导航车辆的大小,res为感知地图的分辨率;所述S25中,根据索引值i和障碍物的膨胀系数k,对感知地图进行更新。
进一步地,所述S21中,根据感知需求范围{[-x,x][-y,y]}和感知地图的分辨率res,生成大小为(2x/res*2y/res)的二维数组,并将其中的每个值初始化为0;其中,x、y分别为车辆坐标系中车头方向、车正左方向的感知范围,车辆坐标系以自主导航车辆的车体回转中心为坐标原点,以自主导航车辆的车头方向为X轴正方向,以自主导航车辆的车正左方向为Y轴正方向;二维数组的中心为自主导航车辆的车体回转中心。
进一步地,所述S22中,以自主导航车辆的车轮所接触地面在的平面的形心作为车体坐标系T1的原点,以激光雷达的中心作为传感器坐标系T2的原点,将激光雷达在车体坐标系T1下的姿态记为(x,y,z,roll,pitch,yaw),其中(roll,pitch,yaw)为三轴坐标(x,y,z)所对应的三轴偏角;则传感器坐标系T2相对于车体坐标系T1的位置转换关系即为平移加旋转{V,W},V=(x,y,z),W=(roll,pitch,yaw)。
进一步地,所述S23中,先对障碍物数据集Qf中的每个点进行遍历,判断当前点是否在感知需求范围内;若是,则计算该点所对应的感应地图中的索引值i。
进一步地,所述索引值i的具体计算方式为:定义ix、iy分别为感知地图的两个方向的栅格坐标,ix=(X-Xp)/res,iy=(Y-Yp)/res,索引值i=ix*Xnum+iy;其中,Xnum为X方向的数据大小,X、Y为障碍物的坐标值,Xp、Yp为障碍物在感知地图中的偏移量。
进一步地,所述S14中,地面所在平面方程式的计算步骤如下:
(1)取点(x,y,z)和该点的三轴偏角(roll,pitch,yaw),建立一个以点(x,y,z)为原点的坐标系,分别以roll、pitch、yaw方向为X、Y、Z轴;
(2)对于坐标系中的X、Y、Z轴,分别计算单位向量;
(3)单位向量即为垂直于轴的平面的法向量,通过法向量得到轴上的投影的向量,将X、Y、Z轴上的投影的向量相加即得到坐标系中的向量表示;
(4)根据平面方程的通式,代入向量表示和地面点坐标,得到地面所在平面的方程式。
进一步地,所述S16中,沿Z轴方向对数据集Qf’进行数据压缩,具体操作为:遍历数据集Qf’中的所有点,将每个点的Z坐标的数值均设置为0。
本发明实现的有益效果是:
本发明提供一种基于激光雷达的轻量化环境感知地图构建方法,通过使用激光雷达采集生成障碍物数据,并基于算法生成轻量化感知地图。与采用多传感器融合感知方式,而导致存在操作复杂、成本高、性价比低等问题的现有环境感知方法相比,本发明仅使用单一的激光雷达进行障碍物数据采集,而非多种传感器进行融合感知;所以不需要对多种传感器进行复杂的单一校准和联合校准工作,也不必要在感知系统中配置足以支撑较高算力的硬软件;从而能够简化感知系统搭建的工作量和难度,可以有效节约成本,并且更加适用于小型低速的自主导航车辆,具有较高的性价比和实用性。
具体实施方式
为清楚说明本发明中的方案,下面结合附图做进一步说明:
实施例1
本实施例提供一种基于激光雷达的轻量化环境感知地图构建方法,如图1所示,包括如下步骤:
S1:生成障碍物数据;
S2:生成轻量化感知地图。
其中:
S1具体包括如下子步骤:
S11:启动激光雷达;
激光雷达安装在自主导航车辆上,根据激光雷达的型号及驱动代码,启动激光雷达,若启动成功则执行步骤S12,若启动失败则退出程序。
S12:接收激光雷达数据;
获取激光雷达所采集到的数据集P,使用PCL库将格式为ROS点云数据的数据集P转换成格式为PCL点云数据的数据集Q。
S13:提取感兴趣区域数据;
根据感知需求范围大小(XY方向范围)对数据集Q进行感兴趣区域数据截取,得到数据集Qr’。
S14:地面分离;
根据激光雷达的安装姿态,计算地面所在平面的方程式,并根据该方程式将数据集Qr’中的地面数据去除掉,得到数据集Qr。
地面所在平面方程式的具体计算步骤如下:
(1)取已知点(x,y,z)和该点的三轴偏角(roll,pitch,yaw),建立一个以点(x,y,z)为原点的坐标系,分别以roll、pitch、yaw方向为X、Y、Z轴;
(2)对于坐标系中的X、Y、Z轴,分别计算单位向量;
具体地:
对于roll方向(X轴),可以计算出在xz平面上的单位向量;对于pitch方向(Y轴),可以计算出在yz平面上的单位向量;对于yaw方向(Z轴),可以计算出在xy平面上的单位向量;
计算单位向量的公式如下:
其中,θr、θp、θy分别为roll、pitch、yaw的角度;
(3)单位向量即为垂直于轴的平面的法向量,通过法向量可得到轴上的投影的向量,将X、Y、Z轴上的投影的向量相加即得到坐标系中的向量表示;
具体地:
对于垂直于roll方向(X轴)的平面,其法向量为e_r;对于垂直于pitch方向(Y轴)的平面,其法向量为e_p;对于垂直于yaw方向(Z轴)的平面,其法向量为e_y;将X、Y、Z轴上的投影分别记为Pr、Pp、Py,则:
其中,“”表示点积,“/>”表示向量的模长;
则可得到向量表示 ,并将向量简化表示为n=(a,b,c);
(4)根据平面方程的通式,代入向量表示和地面点坐标,即可得到地面所在平面的方程式;
例如:假设所取地面点为P1 (x1,y1,z1),则根据平面方程的通式ax + by +cz +d= 0,可以计算出d=-n·P1=-ax1-by1-cz1;所以,过地面点P1(x1,y1,z1)且法向量为n=(a,b,c)的平面方程式为:ax+by+cz-(ax1+by1+cz1)=0。
而后,将数据集Qr’中位于平面方程式ax+by+cz-(ax1+by1+cz1) =0上方的点筛选出,以存入数据集Qr。
S15:去除离群点;
使用StatisticalOutlierRemoval滤波器将数据集Qr中的异常离散点过滤去除,得到数据集Qf’。
S16:维度压缩;
为了减小运算量、达到轻量化感知的效果,遍历数据集Qf’中的所有点,将每个点的Z坐标的数值均设置为0,从而实现沿Z轴方向对数据集Qf’进行数据压缩,以得到障碍物数据集Qf;障碍物数据集Qf中的点均在XOY平面内。
S2具体包括如下子步骤:
S21:重置感知地图;
在收到障碍物数据集Qf前,先将感知地图进行重置,以将感知地图中原有的障碍物信息清除,以防干扰后边最新障碍物数据的更新。
具体地:
根据感知需求范围{[-x,x][-y,y]}和感知地图的分辨率res=0.1m,生成大小为(2x/res*2y/res)的二维数组,并将其中的每个值初始化为0。其中:
x、y分别为车辆坐标系中车头方向、车正左方向的感知范围,车辆坐标系以车体回转中心为坐标原点,以车头方向为X轴正方向,以车正左方向为Y轴正方向;例如当x=3、y=2,即感知需求范围为{[-3,3][-2,2]},便代表检测车前和车后各3m内、车左和车右各2m内。二维数组用于表示所感知的周围环境,当将某点所对应的二维数组的值置为0,则代表该点位置不存在障碍物,为1则代表存在障碍物;本实施例中,将二维数组的中心设置为自主导航车辆的车体回转中心,以方便计算障碍物与车回转中心的位置关系。
S22:障碍物坐标转换;
将障碍物数据集Qf从传感器坐标系T2下转到车体坐标系T1下。
具体地:
以自主导航车辆的车轮所接触地面在的平面的形心作为车体坐标系T1的原点,以激光雷达的中心作为传感器坐标系T2的原点,将激光雷达在车体坐标系T1下的姿态记为(x,y,z,roll,pitch,yaw),其中(roll,pitch,yaw)为三轴坐标(x,y,z)所对应的三轴偏角;则传感器坐标系T2相对于车体坐标系T1的位置转换关系为平移加旋转{V,W},其中V=(x,y,z),W=(roll,pitch,yaw);通过此关系,便可以将障碍物数据集Qf从传感器坐标系T2下转到车体坐标系T1下。
S23:获取地图索引值;
计算障碍物数据集Qf中每个点所对应的感知地图的索引值i。
具体地:
先对障碍物数据集Qf中的每个点进行遍历,判断当前点是否在感知需求范围内;若是,则计算该点所对应的感应地图中的索引值i。索引值i的具体计算方式如下:
定义ix、iy分别为感知地图的两个方向的栅格坐标,ix =(X-Xp)/res,iy =(Y-Yp)/res,res=0.1m,索引值i=ix*Xnum+iy;其中,Xnum为X方向的数据大小,X、Y为障碍物的坐标值,Xp、Yp为障碍物在感知地图中的偏移量。
具体地,感知地图的索引值计算方式中,需要沿着X轴方向依次递增;即当X值满值(达到360)时,需要从下一排重新递增。比如若当前坐标为(34,0),则索引值为0*360+34=34;若当前坐标为(34,1),则索引值为1*360+34=394,这是因为Y值为1,说明前边有一排X值满值了,因此(Xn,Yn)的索引值为Yn*360+Xn;例如,点(5,16)的索引值为16*360+5=5765。
例如,对于360*480大小的感知地图图片,则Xnum=360、Ynum=480,可代表感知需求范围为X方向36m、Y方向48m;则自主导航车辆在感知需求范围的中心位置,即坐标(18,24)处;激光雷达安装在自主导航车辆上,障碍物坐标是以车体回转中心为原点的;假设当前有个障碍物在自主导航车辆的车前方3.6m、右方1.7m处,则障碍物坐标在车辆坐标系中的坐标为(3.6,-1.7);由于自主导航车辆在感知地图中的坐标为(18,24),所以障碍物在感知地图中的坐标为(18+3.6,-1.7+24)=(21.6,22.3);在上述情景下,Xp=-18,Yp=-14。
S24:计算膨胀系数;
根据自主导航车辆的大小L以及感知地图的分辨率res,计算障碍物的膨胀系数k。
具体地:
k= L/res,L通常为自主导航车辆的外接圆半径。
S25:更新感知地图;
根据索引值i和障碍物的膨胀系数k,将障碍物数据集Qf中的点依次进行膨胀,以对感知地图进行更新。
具体地:
假设障碍物数据集Qf中存在点m(x,y),点m(x,y)对应的索引为Imx、Imy,将[Imx,Imy] 的k范围,即[Imx-k,Imx+k]-[Imy-k,Imy+k]中的每对(ix,iy)点带入索引值i的计算公式中以得到i,再将二维数组中i对应的值赋值为1。
综上所述,通过上述步骤,可将三维空间感知压缩转化为带有高度信息的二维平面地图,并且该二维平面地图为数据量小、处理速度快的轻量化地图;本发明仅使用单一的激光雷达进行障碍物数据采集,而非多种传感器进行融合感知;所以不需要对多种传感器进行复杂的单一校准和联合校准工作,也不必要在感知系统中配置足以支撑较高算力的硬软件;从而能够简化感知系统搭建的工作量和难度,可以有效节约成本,并且更加适用于小型低速的自主导航车辆,具有较高的性价比和实用性。
当然,上述内容仅为本发明的较佳实施例,不能被认为用于限定对本发明的实施例范围。本发明也并不仅限于上述举例,本技术领域的普通技术人员在本发明的实质范围内所做出的均等变化与改进等,均应归属于本发明的专利涵盖范围内。