一种用于无人车的城市环境构图方法.pdf

本发明公开了一种用于无人车的城市环境构图方法，不依赖于里程计、GPS以及惯导等外部定位传感器，仅采用车载激光雷达返回的3D激光点云数据用较少的粒子完成无人车轨迹跟踪与环境地图构建，为无人地面车辆在未知环境下的自主行驶提供依据；本发明对相邻两帧数据应用ICP算法得到了车辆真实位姿的粗估计，然后在此粗估计附近根据高斯分布撒点。该粗估计虽然不是无人车真实位姿，却是无人车真实位姿的高概率区域，在后续撒点过程用少量的粒子便实现了较准确的定位与构图，避免了传统方法使用大量粒子拟合车辆轨迹，提高了算法的效率，并有效抑制了由于粒子估计不好带来的粒子匮乏现象。

权利要求书
1.  一种用于无人车的城市环境构图方法，其特征在于，包括如下步骤：
步骤1、采用无人车搭载激光雷达获取周围环境的点云数据，并对该点云数据进行预处理；
步骤2、根据上一时刻以及当前时刻的预处理后的点云数据，采用I CP算法，获得当前时刻相对于上一时刻的旋转变换矩阵R和平移变换矩阵T；根据该旋转变换矩阵R和平移变换矩阵T以及上一时刻无人车所在位姿，得到当前时刻无人车所在位姿，作为无人车的粗估计位姿；
步骤3、根据无人车所在环境的复杂度确定粒子个数，然后将所有粒子撒在以所述粗估计位姿为中心的周围区域，最后确定各粒子的位姿；
步骤4、针对步骤3中的所有粒子中的任一粒子N，根据粒子N的位姿，将所述步骤1中预处理后的当前时刻的点云数据中各点云点坐标按该粒子N的位置与姿态进行偏移，得到任一粒子N对应的点云数据；
步骤5、根据各粒子对应的点云数据成像获得各粒子对应的环境的局部地图；对于上一时刻更新后的全局地图，找到全局地图与各局部地图的重合区域，并确定其中重合区域匹配度最大的局部地图，该局部地图对应的粒子位姿即为当前时刻无人车位姿；将重合区域匹配度最大的局部地图融合到所述全局地图中，作为当前时刻的全局地图，实现全局地图的更新。

2.  如权利要求1所述的一种用于无人车的城市环境构图方法，其特征在于,所述预处理包括选取有效点云帧,即对于激光雷达获取的周围环境的点云数据，每隔1s选取一帧点云数据作为当前时刻的点云数据。

3.  如权利要求1所述的一种用于无人车的城市环境构图方法，其特征在于，所述预处理包括截取有效数据，即删除点云数据中远处点云稀疏部分对应的点云数据以及地面对应的点云数据。

4.  如权利要求1所述的一种用于无人车的城市环境构图方法，其特征在于，所述预处理包括对点云数据的下采样，即：对所述点云数据进行三维体素栅格化，将每个体素栅格内所有点云点的重心作为一个点云点。

5.  如权利要求4所述的一种用于无人车的城市环境构图方法，其特征在于， 所述三维体素栅格大小为20cm×20cm×20cm。

6.  如权利要求1所述的一种用于无人车的城市环境构图方法，其特征在于，所述步骤3中粒子撒点的规律服从高斯分布。

7.  如权利要求1所述的一种用于无人车的城市环境构图方法，其特征在于，所述粒子个数与环境复杂度成正比。

8.  如权利要求1所述的一种用于无人车的城市环境构图方法，其特征在于，所述步骤3中的周围区域表示为：以粗估计位姿为中心，水平范围为1m，竖直范围为-0.3m至0.3m，姿态中的横滚角、俯仰角和偏航角的范围均为-5度至5度。

9.  如权利要求1所述的一种用于无人车的城市环境构图方法，其特征在于，所述步骤2中采用ICP算法获得旋转变换矩阵R和平移变换矩阵T的具体方法为：
S20、对于当前时刻的点云数据Pi中每个点(xj,yj,zj)，在上一时刻的点云数据中，采用最近邻域方法搜索距离最近点，得到在上一时刻的点云数据Pi-1中的对应点(x'j,y'j,z'j)；
S21、根据平均距离最小原则，通过S20得到的对应点对求得刚体变换参数旋转变换矩阵R与平移变换矩阵T；
S22、将点云Pi按照P′i＝RPi+T进行转换，得到新的点云数据P′i，同时，迭代次数累计一次；
S23、判断新的点云数据P′i与Pi-1距离是否小于或等于设定的平均距离阈值:如果满足，输出旋转变换矩阵R和平移变换矩阵T；如果不满足，判断是否达到最高迭代次数：若没有达到，用点云数据P′i更新当前时刻的点云数据，并返回S20；若达到，输出当前求得的旋转变换矩阵R和平移变换矩阵T。

10.  如权利要求1所述的一种用于无人车的城市环境构图方法，其特征在 于，所述步骤5中重合区域匹配度的获得方法为：对所述各重合区域进行三维体素栅格化，累计体素栅格中同时包含全局地图的点云点和局部地图点云点的体素栅格个数，该体素栅格个数与重合区域中体素栅格总个数之比即为重合区域匹配度。

说明书一种用于无人车的城市环境构图方法
技术领域
本发明涉及基于无人地面车辆的定位和构图技术领域，尤其涉及一种用于无人车的城市环境构图方法。
背景技术
无人地面车辆作为智能机器人领域的研究热点，在智能交通系统和军事安全上都有重要应用前景。无人车具有智能性，可以在司机醉酒和疲劳时辅助驾驶，在操作失误时及时提醒，在检测到车辆软硬件故障时及时发出警报，做到减少交通事故，提高交通安全。军事领域无人车可以代替人在危险场合执行任务和完成野外物资运输等工作，保障士兵的生命安全，减少人员伤亡。
定位与构图是无人车领域的重要研究内容，为了在未知环境中准确、安全地实现预定目标，无人车需要实时确定周围环境地图以及自身在地图中的位姿。同时实现载体的自身定位与周围环境地图构建问题通常被称为即时定位与地图构建，即SLAM。SLAM问题的复杂性在于定位与构图的相互依赖性。无人车的精确定位依赖于周围环境地图的一致性，同样，高度一致性环境地图的构建也以准确的定位为前提。通常，SLAM问题被分为位姿估计算法和地图表示两部分。地图表示主要包括特征地图、栅格地图、拓扑地图以及混合地图。其中特征地图和栅格地图在SLAM中应用较早。位姿估计算法的研究是解决SLAM问题的关键。目前，已有大量位姿估计算法。这些算法大致可分为以下六类：扩展卡尔曼滤波(EKF)、最大似然方法、稀疏的扩展信息滤波器(SEIFs)、平滑技术、Rao-Blackwellized粒子滤波(RBPF)以及图的优化。EKF与RBPF作为SLAM领域的经典算法得到广泛应用，但是EKF算法以机器人运动模型和传感器噪声均为高斯分布的强假设为前提，当不满足这种假设时会导致滤波器发散，而实际应用中大多数情况均不满足此假设，限制了算法的应用范围。RBPF算法以大量 粒子拟合移动机器人运动轨迹，不依赖任何外部假设，但存在粒子数量大、计算复杂、粒子匮乏以及闭环问题等。
发明内容
有鉴于此，本发明提供了一种用于无人车的城市环境构图方法，不依赖于里程计、GPS以及惯导等外部定位传感器，仅采用车载激光雷达返回的3D激光点云数据用较少的粒子完成无人车轨迹跟踪与环境地图构建，为无人地面车辆在未知环境下的自主行驶提供依据。
本发明的一种用于无人车的城市环境构图方法，包括如下步骤：
步骤1、采用无人车搭载激光雷达获取周围环境的点云数据，并对该点云数据进行预处理；
步骤2、根据上一时刻以及当前时刻的预处理后的点云数据，采用ICP算法，获得当前时刻相对于上一时刻的旋转变换矩阵R和平移变换矩阵T；根据该旋转变换矩阵R和平移变换矩阵T以及上一时刻无人车所在位姿，得到当前时刻无人车所在位姿，作为无人车的粗估计位姿；
步骤3、根据无人车所在环境的复杂度确定粒子个数，然后将所有粒子撒在以所述粗估计位姿为中心的周围区域，最后确定各粒子的位姿；
步骤4、针对步骤3中的所有粒子中的任一粒子N，根据粒子N的位姿，将所述步骤1中预处理后的当前时刻的点云数据中各点云点坐标按该粒子N的位置与姿态进行偏移，得到任一粒子N对应的点云数据；
步骤5、根据各粒子对应的点云数据成像获得各粒子对应的环境的局部地图；对于上一时刻更新后的全局地图，找到全局地图与各局部地图的重合区域，并确定其中重合区域匹配度最大的局部地图，该局部地图对应的粒子位姿即为当前时刻无人车位姿；将重合区域匹配度最大的局部地图融合到所述全局地图中，作为当前时刻的全局地图，实现全局地图的更新。
进一步的，所述预处理包括选取有效点云帧,即对于激光雷达获取的周围环 境的点云数据，每隔1s选取一帧点云数据作为当前时刻的点云数据。
进一步的，所述预处理包括截取有效数据，即删除点云数据中远处点云稀疏部分对应的点云数据以及地面对应的点云数据。
进一步的，所述预处理包括对点云数据的下采样，即：对所述点云数据进行三维体素栅格化，将每个体素栅格内所有点云点的重心作为一个点云点。
较佳的，所述三维体素栅格大小为20cm×20cm×20cm。
较佳的，所述步骤3中粒子撒点的规律服从高斯分布。
较佳的，所述粒子个数与环境复杂度成正比。
较佳的，所述步骤3中的周围区域表示为：以粗估计位姿为中心，水平范围为1m，竖直范围为-0.3m至0.3m，姿态中的横滚角、俯仰角和偏航角的范围均为-5度至5度。
较佳的，所述步骤2中采用ICP算法获得旋转变换矩阵R和平移变换矩阵T的具体方法为：
S20、对于当前时刻的点云数据Pi中每个点(xj,yj,zj)，在上一时刻的点云数据中，采用最近邻域方法搜索距离最近点，得到在上一时刻的点云数据Pi-1中的对应点(x'j,y'j,z'j)；
S21、根据平均距离最小原则，通过S20得到的对应点对求得刚体变换参数旋转变换矩阵R与平移变换矩阵T；
S22、将点云Pi按照Pi′＝RPi+T进行转换，得到新的点云数据Pi'，同时，迭代次数累计一次；
S23、判断新的点云数据Pi'与Pi-1距离是否小于或等于设定的平均距离阈值:如果满足，输出旋转变换矩阵R和平移变换矩阵T；如果不满足，判断是否达到最高迭代次数：若没有达到，用点云数据Pi'更新当前时刻的点云数据，并返回S20；若达到，输出当前求得的旋转变换矩阵R和平移变换矩阵T。
较佳的，所述步骤5中重合区域匹配度的获得方法为：对所述各重合区域进行三维体素栅格化，累计体素栅格中同时包含全局地图的点云点和局部地图点云点的体素栅格个数，该体素栅格个数与重合区域中体素栅格总个数之比即为重合区域匹配度。
本发明具有如下有益效果：
(1)本发明采用ICP算法与高斯分布相结合的方式完成运动更新。传统RBPF-SLAM算法采用里程计运动模型实现运动更新，里程计运动模型精度较低，为了覆盖无人车位姿有效区域，需要大量粒子来拟合无人车运动轨迹，导致计算量增大，并且大量粒子还会导致粒子匮乏现象，影响算法有效性。本发明对相邻两帧数据应用ICP算法得到了车辆真实位姿的粗估计，然后在此粗估计附近根据高斯分布撒点。该粗估计虽然不是无人车真实位姿，却是无人车真实位姿的高概率区域，在后续撒点过程用少量的粒子便实现了较准确的定位与构图，避免了传统方法使用大量粒子拟合车辆轨迹，提高了算法的效率，并有效抑制了由于粒子估计不好带来的粒子匮乏现象。
(2)本发明在传感器应用上充分考虑自主性，不依赖于里程计、GPS和惯导等外部传感器，仅采用车载激光雷达返回3D激光点云作为原始数据，具有较高的自主性和抗干扰能力。原始数据仅来源于单一传感器，在应用上降低了成本也避免了多传感器融合所带来的校准和误差等问题。
(3)本发明数据选择上采用等时间间隔采样法。所用激光雷达频率为10Hz,即每秒可返回10帧数据，大量的冗余数据会对给算法带来沉重的负担。采用等时间间隔采样数据帧，其中选择采样时间间隔为1s,此时无人车运动距离大概为5m,使得相邻两帧数据间的重叠区域较大，保证了ICP算法的有效性，并使得无需在ICP算法初值问题上做过多研究。
(4)本发明在数据预处理阶段充分考虑激光雷达返回的3D激光点云数据特性，采用截取方式选择有效数据，解决激光点云远处点量稀疏不能有效表示环境信息问题以及去掉不具备结构特性的大量地面数据的影响，提高算法准确 度和效率。同时，3D激光点云单帧数据量高达13万，虽然密集的点云数据能尽可能保留环境信息，却要付出高昂的时间代价，本发明采用VoxelGrid滤波器对点云进行下采样，减少点数的同时保持环境点云数据，提高算法效率，其中可根据实际应用需要通过调节三维体素栅格大小确定点云稀疏程度。
(5)本发明观测更新阶段采用当前时刻粒子所带局部地图与全局地图的匹配度来量化权值。其中匹配度的计算采用粒子所带局部地图与全局地图重叠区域栅格属性值相匹配的方法，计算属性一致栅格占总栅格比例作为重要性权值，这样的权值计算方法简单有效。
附图说明
图1为本发明的构图方法流程图；
图2(a)为本发明的非结构化城市环境实验中无人车运动轨迹图；
图2(b)为本发明的非结构化城市环境实验中环境地图；
图3(a)为本发明的结构化城市环境实验中无人车运动轨迹图；
图3(b)为本发明的结构化城市环境实验中环境地图。
具体实施方式
下面结合附图并举实施例，对本发明进行详细描述。
本发明公开了一种基于3D激光点云的用于无人车的城市环境构图方法，如图1所示，包括数据预处理、运动更新和观测更新三部分。其中运动更新采用ICP算法实现粗估计，位姿估计则基于高斯分布。观测更新包括粒子赋权值与全局地图更新两部分。
所述数据预处理中首先选取有效点云帧。激光雷达的频率为10Hz,即每秒返回10帧数据，如果对每帧数据进行处理，大量的冗余数据会给算法带来沉重的负担。采用等时间间隔采样数据帧，后续点云匹配算法ICP对初值较敏感，初值不好或者迭代次数过多都可能使算法陷入局部最优导致匹配失败。在两帧点云重叠区域较大的情况下，将ICP的初值R与T设为单位阵和零矩阵即可，因 而取时间间隔为1s，此时无人车行驶路程约为5m使相邻两帧点云具有较大的重叠区域，保证ICP算法的有效性。
然后采用截取有效数据方式解决远处点云稀疏问题和地面影响。本发明所采用原始数据来自于安装于无人车顶部的车载64位Velodyne激光雷达。由于激光雷达激光束射线分布，因而在距离激光雷达较远处点云稀疏，难以较准确地表达环境特征，这样的稀疏点云会影响算法后续地图匹配和环境地图表示。激光点云数据含有大量地面信息，但是地面不具备结构特性，影响算法后续地图匹配。采用截取有效数据方式即宽度范围选择与高度范围选择可虑除以上稀疏点集与地面点集，其中数据截取中宽度范围和高度范围选择可视实际需要而定，图2与图3中的示例宽度范围为25m，高度范围为-1.6-1m(车载激光雷达距离地面约2.1m)。
最后采用VoxelGrid滤波器(体素化网络方法)对点云进行下采样，即减少点的数量，并同时保持点云的形状特征。基本思路是将点云数据三维体素栅格化，然后在每个体素内用体素中所有点的重心来近似表示体素中其他点。对于一个存在M个点的三维体素，经过VoxelGrid滤波器处理后得到点的计算公式如式(1)所示。
x=1MΣi=1Mxi,y=1MΣi=1Myi,z=1MΣi=1Mzi---(1)]]>
其中体素栅格大小视具体需要和实际情况而定，图2与图3中的示例中体素栅格大小为20cm×20cm×20cm。
本发明所述运动更新包括位姿粗估计与粒子更新。粗估计即对经过数据预处理后的相邻两帧数据采用点云匹配算法——ICP算法实现无人车位姿初步估计。3D激光雷达返回的点云数据均为以当时激光雷达所在位姿为原点的局部坐标，对于t-1时刻点云数据和t时刻点云数据应用ICP算法计算出两个点集的对应旋转变换矩阵R与平移变换矩阵T，使得满足式(2)所示平均距离阈值要求。
f(R,T)=Σt=1N||Pt-(RPt-1+T)||2=min---(2)]]>
ICP具体算法步骤可表示为：
S20、对于当前时刻的点云数据Pi中每个点(xj,yj,zj)，在上一时刻的点云数据中，采用最近邻域方法搜索距离最近点，得到在上一时刻的点云数据Pi-1中的对应点(x'j,y'j,z'j)；
S21、根据平均距离最小原则，通过S20得到的对应点对求得刚体变换参数旋转变换矩阵R与平移变换矩阵T；
S22、将点云Pi按照Pi′＝RPi+T进行转换，得到新的点云数据Pi'，同时，迭代次数累计一次；
S23、判断新的点云数据Pi'与Pi-1是否小于或等于设定的平均距离阈值(公式2所示):如果满足，输出旋转变换矩阵R和平移变换矩阵T；如果不满足，判断是否达到最高迭代次数：若没有达到，用点云数据Pi'更新当前时刻的点云数据，并返回S20；若达到，输出当前求得的旋转变换矩阵R和平移变换矩阵T。
利用上述两帧矩阵间的旋转平移矩阵，可得到两帧数据间的位姿转换矩阵M，如式(4)所示。
M=RT0001---(4)]]>
显然，R为一个3×3矩阵，T为一个3×1矩阵。则M为一个4×4矩阵。
从而可根据上一t-1时刻的位姿与位姿转换矩阵得到无人车t时刻在全局坐标系下的粗估计Ct，如式(5)所示。
Ct=xt-1M=RtTt0001---(5)]]>
其中xt-1表示t-1时刻无人车的位姿。Rt为无人车t时刻粗估计姿态，Tt为t时刻粗估计位姿。显然，无人车初始位姿x0的姿态矩阵R0为3×3的单位阵，位置矩阵T0为3×1的零阵。
由于收敛条件和误差等原因，ICP算法所得位姿估计并不是无人车真实位 姿，但却位于无人车真实位姿的高概率区域。为了得到更准确的位姿估计，需要进一步粒子更新拟合。
粒子更新通过高斯模型在粗估计位姿附近撒点，其中位置坐标X、Y轴代表平水位置，撒点范围为半径1m的区域；Z轴代表竖直位置，由于针对城市环境，竖直位置变化很小，取范围为-0.3m-0.3m；姿态中的横滚、俯仰、偏航三个角，取范围为-5度-5度。从而可由粗估计位姿更新得到当前时刻粒子的位姿，如式(6)所示。
xt(i)=Ct+Wt(i)=Rt(i)Tt(i)0001---(6)]]>
其中，表示第i个粒子在t时刻的位姿。是第i个粒子对应的高斯噪声。为第i个粒子在t时刻的姿态，为第i个粒子在t时刻的位置。粒子个数与环境复杂度成正比，其中,图2非结构化城市环境构图示例采用25个粒子，而图3结构化城市环境构图示例仅采用5个粒子。
由于在位姿概率较大区域生成粒子，因而可有效覆盖高概率区域，减少粒子数，提高算法效率，同时也能有效抑制粒子匮乏现象。
本发明所述观测更新包括粒子赋权值与全局地图更新。粒子赋权值是对运动更新得到的粒子群分别赋予重要性权值。用粒子当前时刻所带的局部地图与全局地图的匹配度来具体量化权值，匹配度越好，权值越大，反之，匹配度越差，权值越小，权值计算可表示为如式(5)所示。

表示t时刻第i个粒子的权值。理想情况下，当得到的估计位姿与真实位姿完全一致时，权值为1。实际应用中，两者几乎不可能完全重合，但是估计位姿距离真实位姿越近，权值越高，即估计位姿的优劣可由粒子权值量化表示。本文采用局部地图与全局地图的匹配度来近似公式(5)中的权值。
由于雷达得到的当前时刻点云数据是在雷达本体坐标系为参考系的局部坐标系中获得的数据，即当前时刻点云数据以雷达本体坐标的原点为参考点。而 根据上述原理可知，粗估计位姿并不是无人车真实位姿，需要从撒点粒子中进行优选，从而作为当前时刻无人车的位姿，因此，需要将当前时刻粒子所带局部坐标系下的点云数据变换成全局坐标系下的点云数据，具体方法为：根据任一粒子N的位姿，将当前时刻的点云数据中各点云点坐标按该粒子N位姿进行转换，得到任一粒子N对应的全局坐标系下的点云数据，即：即由t时刻点云和第i个粒子在t时刻的粒子位姿得到t时刻第i个粒子在全局坐标系下的点云如式(6)所示。
xgk(i)ygk(j)zgk(i)=Rt(i)xLkxLkzLk+Tt(i)---(6)]]>
由于此时粒子的坐标为全局坐标系，则上述处理将各粒子对应的点云数据转换为全局坐标下的点云数据。经过转换，粒子所带局部地图与上一时刻全局地图坐标系一致，方便地图匹配度的计算。
本文地图表示选用栅格地图方式。将环境划分为大小的栅格，对于每一个栅格赋属性，被点云点占用为1，未被点云点占用为0。如式(7)所示。

这样的二值属性存贮方式在后续地图匹配时使用方便。对于每一个粒子，选取其全局坐标下的地图与全局地图的重叠区域并栅格化，分别对全局地图和局部地图的该重叠区域赋权值。然后将两地图进行匹配操作，计算相同属性值的栅格数占总栅格数的比例，即为该局部地图与全局地图的匹配度。计算公式如式(8)所示。
ωt(i)=nt(i)Nt(i)---(8)]]>
其中表示t时刻第i个粒子的局部地图与全局地图重叠区域属性相同的栅格数。表示t时刻第i个粒子的局部地图与全局地图进行匹配时所选择重 叠区域栅格的总数。
全局地图更新是在完成赋权值后将当前时刻粒子所带新的环境信息加入全局地图。具体操作为每个粒子赋完权值后选择当前时刻最优粒子即权值最高粒子作为当前时刻无人车位姿，将该粒子所带局部地图加入全局地图，即可完成地图更新。其中，当获得第一帧数据时，以该第一帧数据的坐标作为全局坐标，以第一帧数据所带地图为最初的全局地图，当下一时刻新的点云数据到来时，重复上述过程，递增构图。上述步骤是一次构图的迭代过程，当下一时刻激光雷达有新的环境数据返回时，重复上述步骤，直到不再有新的数据返回，完成整个环境的完整构图。
综上所述，以上仅为本发明的较佳实施例而已，并非用于限定本发明的保护范围。凡在本发明的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。