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

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

【技术实现步骤摘要】

本专利技术涉及基于无人地面车辆的定位和构图
，尤其涉及一种用于无人车 的城市环境构图方法。
技术介绍
无人地面车辆作为智能机器人领域的研宄热点，在智能交通系统和军事安全上都 有重要应用前景。无人车具有智能性，可以在司机醉酒和疲劳时辅助驾驶，在操作失误时及 时提醒，在检测到车辆软硬件故障时及时发出警报，做到减少交通事故，提高交通安全。军 事领域无人车可以代替人在危险场合执行任务和完成野外物资运输等工作，保障士兵的生 命安全，减少人员伤亡。 定位与构图是无人车领域的重要研宄内容，为了在未知环境中准确、安全地实现 预定目标，无人车需要实时确定周围环境地图以及自身在地图中的位姿。同时实现载体的 自身定位与周围环境地图构建问题通常被称为即时定位与地图构建，即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、根据各粒子对应的点云数据成像获得各粒子对应的环境的局部地图；对 于上一时刻更新后的全局地图，找到全局地图与各局部地图的重合区域，并确定其中重合 区域匹配度最大的局部地图，该局部地图对应的粒子位姿即为当前时刻无人车位姿；将重 合区域匹配度最大的局部地图融合到所述全局地图中，作为当前时刻的全局地图，实现全 局地图的更新。 进一步的，所述预处理包括选取有效点云帧，即对于激光雷达获取的周围环境的 点云数据，每隔Is选取一帧点云数据作为当前时刻的点云数据。 进一步的，所述预处理包括截取有效数据，即删除点云数据中远处点云稀疏部分 对应的点云数据以及地面对应的点云数据。 进一步的，所述预处理包括对点云数据的下采样，即：对所述点云数据进行三维体 素栅格化，将每个体素栅格内所有点云点的重心作为一个点云点。 较佳的，所述三维体素栅格大小为20cmX 20cmX 20cm。 较佳的，所述步骤3中粒子撒点的规律服从高斯分布。 较佳的，所述粒子个数与环境复杂度成正比。 较佳的，所述步骤3中的周围区域表示为：以粗估计位姿为中心，水平范围为lm， 竖直范围为-0. 3m至0. 3m，姿态中的横滚角、俯仰角和偏航角的范围均为-5度至5度。 较佳的，所述步骤2中采用ICP算法获得旋转变换矩阵R和平移变换矩阵T的具 体方法为： S20、对于当前时刻的点云数据Pi中每个点（X j，y」，Zj)，在上一时刻的点云数 据中，采用最近邻域方法搜索距离最近点，得到在上一时刻的点云数据Ph中的对应点 (χ' j, y' j, z' j)； S21、根据平均距离最小原则，通过S20得到的对应点对求得刚体变换参数旋转变 换矩阵R与平移变换矩阵T ; S22、将点云Pi按照Pi' =RPi+T进行转换，得到新的点云数据P/，同时，迭代次数 累计一次； S23、判断新的点云数据P/与Pp1距离是否小于或等于设定的平均距离阈值：如 果满足，输出旋转变换矩阵R和平移变换矩阵T ;如果不满足，判断是否达到最高迭代次数： 若没有达到，用点云数据P/更新当前时刻的点云数据，并返回S20 ;若达到，输出当前求得 的旋转变换矩阵R和平移变换矩阵T。 较佳的，所述步骤5中重合区域匹配度的获得方法为：对所述各重合区域进行三 维体素栅格化，累计体素栅格中同时包含全局地图的点云点和局部地图点云点的体素栅格 个数，该体素栅格个数与重合区域中体素栅格总个数之比即为重合区域匹配度。 本专利技术具有如下有益效果： (1)本专利技术采用ICP算法与高斯分布相结合的方式完成运动更新。传统RBPF-SLAM 算法采用里程计运动模型实现运动更新，里程计运动模型精度较低，为了覆盖无人车位姿 有效区域，需要大量粒子来拟合无人车运动轨迹，导致计算量增大，并且大量粒子还会导致 粒子匮乏现象，影响算法有效性。本专利技术对相邻两帧数据应用ICP算法得到了车辆真实位 姿的粗估计，然后在此粗估计附近根据高斯分布撒点。该粗估计虽然不是无人车真实位姿， 却是无人车真实位姿的高概率区域，在后续撒点过程用少量的粒子便实现了较准确的定位 与构图，避免了传统方法使用大量粒子拟合车辆轨迹，提高了算法的效率，并有效抑制了由 于粒子估计不好带来的粒子匮乏现象。 (2)本专利技术在传感器应用上充分考虑自主性，不依赖于里程计、GPS和惯导等外部 传感器，仅采用车载激光雷达返回3D激光点云作为原始数据，具有较高的自主性和抗干扰 能力。原始数据仅来源于单一传感器，在应用上降低了成本也避免了多传感器融合所带来 的校准和误差等问题。 (3)本专利技术数据选择上采用等时间间隔采样法。所用激光雷达频率为10Hz，即每 秒可返回10帧数据，大量的冗余数据会对给算法带来沉重的负担。采用等时间间隔采样数 据帧，其中选择采样时间间隔为ls，此时无人车运动距离大概为5m，使得相邻两帧数据间 的重叠区域较大，保证了 ICP算法的有效性，并使得无需在ICP算法初值问题上做过多研 宄。 (4)本专利技术在数据预处理阶段充分考虑激光雷达返回的3D激光点云数据特性，采 用截取方式选择有效数据，解决激光点云远处点量稀疏不能有效表示环境信息问题以及去 掉不具备结构特性的大量地面数据的影响，提高算法准确度和效率本文档来自技高网...

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

【技术特征摘要】

【专利技术属性】
技术研发人员：王美玲，李玉，杨毅，朱昊，吕宪伟，
申请(专利权)人：北京理工大学，
类型：发明
国别省市：北京;11