CN110345946A - 一种室内车辆地图构建方法 - Google Patents

Abstract

本发明公开一种室内车辆地图构建方法，这是一种基于室内激光雷达覆盖栅格地图构建方法的改进算法。本发明通过建图前校准、建图以构建地图环境，将整个地图环境用栅格地图表示；统计每个栅格的被激光束自由通过的次数以及栅格被激光束击中的次数；对地图进行估计，确定优化函数，完成建图。本发明利用多次激光雷达测量的数据，提高了稳定性与准确性，也消除了覆盖栅格地图的边界不稳定性。

Description

一种室内车辆地图构建方法

技术领域

本发明属于无人驾驶技术领域，尤其涉及一种室内车辆地图构建方法。

背景技术

地图一般有两种表示方式，第一种基于特征，地图列表中每个信息表示特征 的位置以及属性。第二种基于位置的地图，地图中表示的是在某个坐标系下的属 性。常见的地图一般分为三类，尺度地图、拓扑地图、语义地图。尺度地图包括 特征地图与栅格地图。特征地图用数学的几何点、线表示二维环境，常用于视觉 SLAM，一般与摄像头相结合。栅格地图把整个环境用一些列栅格表示，栅格中有 一个值，这个值表示有障碍物的概率，栅格地图是目前最流行的室内地图构建类 型，这里也选择栅格地图。

地图构建有三种不同方式，基于已知位姿的构图、未知位姿的构图、同时定 位与构图(SLAM)。第一种方式，是最简单的，但是前提是要确定车辆的准确位 姿，这一步骤为这个方式增加了更多的步骤。第二种方式，本课题选用的就是这 种，在学校实验室控制车辆采集激光雷达数据，用此数据建图，这种构图方式不 需要提前准确定位，总的来说所花精力时间更少。第三种方式SLAM构图，SLAM 是目前的无人驾驶建图与定位的重难点，比其它的几种方式都要难很多，其是一 种定位与地图的后验概率检验问题P(x_t，m|z_1：t，u_1：t)。SLAM问题需同时估计x_t、 m两个变量，x_t为t时测量的位姿，m为车辆所在环境地图。可以看出SLAM是最 复杂最难的方式，该课题选用第二种方式。具体实验过程将在第五章介绍，主 要通过控制车辆在室内采集室内环境激光雷达数据，利用此数据进行构图。

目前主流的栅格地图构建方法，是覆盖栅格地图构建算法，虽然这种算法只 用到加减法因为构图速度非常快，但是因为其是覆盖栅格算法，每次的扫描都会 对整个地图产生影响，有可能一两次错误的信息，也会被采集到，并且地图的边 界因为每个栅格都是覆盖产生，因此边界栅格重合度不高，边界信息不准确。

发明内容

针对上述缺陷，本发明解决的技术问题在于，提供一种室内车辆地图构建方 法，主要采用计数法构建栅格地图，以解决现在技术所存在的边界信息不准确的 难题，因为其利用的是多次的数据并不会因为一次的错误测量就会对地图产生影 响。

本发明方法的技术方案为一种室内车辆地图构建方法，具体包括以下步骤：

步骤1：通过建图前校准、建图以构建地图环境，将整个地图环境用栅格地 图表示；

步骤2：统计每个栅格的被激光束自由通过的次数以及栅格被激光束击中的 次数；

步骤3：对地图进行估计，确定优化函数，完成建图。

作为优选，步骤1中所述建图前校准包括：

车辆底盘运行精度校准、激光雷达运动畸变矫正、根据里程计辅助方法进行 激光雷达数据矫正三部分；

所述车辆底盘运行精度校准具体方法是：

通过先让车辆走1米直线，观察构建地图的距离与实际车辆走过路程构建地 图的距离直接在上位机上读取，实际车辆走过路程直接测量，若误差在1％内， 则再控制车辆原地旋转360°，再次观察地图上车辆是否转过360°，若误差在 1％内，则整个车辆底盘运行精度调节完毕；假如直线误差过大则通过调整车轮直 径即更换不同直径轮胎来解决，若转动角度误差过大，则通过调整车轮间距来完 成。所述激光雷达运动畸变矫正具体为：

采用里程计辅助方法进行激光雷达数据矫正，主要方法为给车辆安装里程计， 里程计可以直接测量车辆位移角度，有较高的局部测量精度，较快的更新速度， 具体方法为：已知激光雷达两个起始时刻时间t_s，t_e，两个激光数据之间间隔Δt， 里程计的数据按顺序在队列中存储，时间间隔在t_s和t_e之间，把每个时刻激光雷 达对应的里程计数据，再把所有的位姿转换到同一个坐标系中，然后将激光雷达 数据和里程计数据利用二次线性牛顿插值法确定实际的运动轨迹，从而消除运动 畸变，具体过程如下：

里程计队列中位姿和激光数据同步，假设第i和第j数据对应的时刻分别为 t_s和t_e

p_s＝OdomList[i]

p_j＝OdomList[j]

若在t_s时刻没有对应的里程计位姿，则进行线性插值，设l，k时刻有位姿， 且l＜s＜k，则：

p_l＝OdomList[l]

p_k＝OdomList[k]

二次插值

在一帧数据之间，认为机器人做匀加速运动，由于机器人的位姿是关于时间 t的二次函数，设且l＜m＜k，则：

已知p_s，p_m，p_e，可以插值一条二次曲线：

p(t)＝At²+Bt+C

t_s≤t≤t_e

对二次曲线进行近似

利用分段线性函数对二次曲线进行近似，在t_s和t_e时间段内，一共取k个位 姿{p_s，p_s+1，…，p_s(n-2)，p_s+1}

则：

坐标统一及激光数据的发布

一帧激光数据n个激光点，每个激光点对应的位姿{p₁，p₂，…，p_n}通过上述介 绍的方法插值得到

x_i为转化之前的坐标，x_i′为转换之后的坐标，则：

把转换之后的坐标转换成激光数据发布出去：

x_i′＝(p_x，p_y)

angle＝atan2(p_y，p_x)

步骤1中所述建图为：

控制车辆在所需要建图的环境中进行运动，启动车辆的建图指令，然后控制 车辆在所需要建图的环境中进行运动，此时激光雷达会将采集到的数据会呈现出 当前环境的二维地图，即构图完成；

过程中实时观察车辆的直线状态、转弯状态、地图构建的完成度；

步骤1中所述将整个地图环境用栅格地图表示具体过程为：

栅格地图的计算公式为：

其中，P为栅格地图的后验概率，m为车辆所在环境地图，x_1：t为用所有机 器人位姿定义的路径，z_1：t为直到时刻t的所有测量值；

式中栅格地图的后验概率P为：

P(m|x_1:t,z_1:t)

并且该概率由激光雷达传感器的数据data＝{x₁…x_t，z₁…z_t}计算：

data＝{x₁…x_t，z₁…z_t}带入P(m|x_1∶t，z_1∶t)

即可，其中x₁…x_t对应x_1∶t即为直到时刻t的所有机器人位姿定义的路径， z₁…z_t对应z_1：t即为直到时刻t的所有测量值；

作为优选，步骤2中所述第i个栅格被激光束自由通过的次数为misses(i)即 这个栅格为空闲的次数，步骤2中所述第i个栅格被激光束击中的次数为hits(i) 即这个栅格为占用的次数；

hits(j)为第j个栅格被激光束击中的次数，即这个栅格为占用的次数，具体 为：

其中，x_t表示t时刻机器人的位姿，t时刻激光雷达数据为z_t，第n个激光 束为z_t，n，c_t，n表示t时刻的第n个激光束是否为最大值，c_t，n＝1表示最大值， c_t，n＝0表示正常值，f(x_t，n，z_t，n)表示t时刻的第n个激光束击中的栅格的下标；

misses(j)为第j个栅格被激光束自由通过的次数，即这个栅格为空闲的次数， 具体为：

其中，x_t表示t时刻机器人的位姿，t时刻激光雷达数据为z_t，第n个激光 束为z_t，n，c_t，n表示t时刻的第n个激光束是否为最大值，c_t，n＝1表示最大值， c_t，n＝0表示正常值，f(x_t，n，z_t，n)表示t时刻的第n个激光束击中的栅格的下标；

作为优选，步骤3中所述对地图进行估计，因此建立地图的极大似然估计模 型为：

并以此为优化函数，其中，a_j表示栅格j被激光击中的次数，即步骤2中所述 上述hits(j)，b_j表示栅格j被激光通过的次数，即步骤2中所述misses(j)；

步骤3中所述完成建图，以所述优化函数为极大似然估计模型求其极值便可 得到最优化地图。

本发明提供的室内车辆地图构建方法，在现有栅格地图质量方面，改善了局 部栅格精度，栅格地图的边界栅格过多问题得以解决。在构图之前进行了车辆的 直线与角度校准使得数据匹配更为准确，还进行了构图所用传感器的数据校准， 这个也是提高地图精度一大措施。

附图说明

为了更清楚地说明本发明实施例，下面将对实施例或现有技术描述中所需要 使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明的一些 实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以 根据这些附图获得其他的附图。

图1：为该发明所用所有设备实体图；

图2：为车辆矫正之前构建的室内地图；

图3：为进行校准后传统覆盖栅格地图构建算法构建的室内栅格地图。

图4：为本发明构建的室内栅格地图；

图5：为两种算法的边界比较图；

图6：为本发明方法流程图。

具体实施方式

为使本发明实施例的目的、技术方案和优点更加清楚，下面将结合本发明实 施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述。面将结合 本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述， 显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于 本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的 所有其他实施例，都属于本发明保护的范围。

如图1所示，用于室内地图构建的车辆1-1里包括一个1GB的RAM，一个 USB-UART接口、蓝牙模块、激光雷达、IMU模块、轮速传感器、以及三相直流无 刷电机。

本发明具体实施方式中系统包括车辆1-1，激光雷达传感器1-2，上位机1-3。 其中车辆为带底层驱动的小型可移动机器人，激光雷达安装在车体上面，上位机 主要用来控制车辆以及可视化显示构图过程。

具体的，如图1所示，用上位机连接车辆内部单片机，控制车辆的运动，在 所需构图环境中运行，人工规划路径。

下面结合图1至图6介绍本发明的具体实施方式为：

步骤1：通过建图前校准、建图以构建地图环境，将整个地图环境用栅格地 图表示；

步骤1中所述建图前校准包括：

车辆底盘运行精度校准、激光雷达运动畸变矫正、根据里程计辅助方法进行 激光雷达数据矫正三部分；

所述车辆底盘运行精度校准具体方法是：

通过先让车辆走1米直线，观察构建地图的距离与实际车辆走过路程构建地 图的距离直接在上位机上读取，实际车辆走过路程直接测量，若误差在1％内， 则再控制车辆原地旋转360°，再次观察地图上车辆是否转过360°，若误差在 1％内，则整个车辆底盘运行精度调节完毕；假如直线误差过大则通过调整车轮直 径即更换不同直径轮胎来解决，若转动角度误差过大，则通过调整车轮间距来完 成。所述激光雷达运动畸变矫正具体为：

采用里程计辅助方法进行激光雷达数据矫正，主要方法为给车辆安装里程计， 里程计可以直接测量车辆位移角度，有较高的局部测量精度，较快的更新速度， 具体方法为：已知激光雷达两个起始时刻时间t_s，t_e，两个激光数据之间间隔Δt， 里程计的数据按顺序在队列中存储，时间间隔在t_s和t_e之间，把每个时刻激光雷 达对应的里程计数据，再把所有的位姿转换到同一个坐标系中，然后将激光雷达 数据和里程计数据利用二次线性牛顿插值法确定实际的运动轨迹，从而消除运动 畸变，具体过程如下：

里程计队列中位姿和激光数据同步，假设第i和第j数据对应的时刻分别为 t_s和t_e

p_s＝OdomList[i]

p_j＝OdomList[j]

若在t_s时刻没有对应的里程计位姿，则进行线性插值，设l，k时刻有位姿， 且l＜s＜k，则：

p_l＝OdomList[l]

p_k＝OdomList[k]

二次插值

在一帧数据之间，认为机器人做匀加速运动，由于机器人的位姿是关于时间 t的二次函数，设且l＜m＜k，则：

已知p_s，p_m，p_e，可以插值一条二次曲线：

p(t)＝At²+Bt+C

t_s≤t≤t_e

对二次曲线进行近似

利用分段线性函数对二次曲线进行近似，在t_s和t_e时间段内，一共取k个位 姿{p_s，p_s+1，…，p_s(n-2)，p_s+1}

则：

坐标统一及激光数据的发布

一帧激光数据n个激光点，每个激光点对应的位姿{p₁，p₂，…，p_n}通过上述介 绍的方法插值得到

x_i为转化之前的坐标，x_i′为转换之后的坐标，则：

把转换之后的坐标转换成激光数据发布出去：

x_i′＝(p_x，p_y)

angle＝atan2(p_y，p_x)

步骤1中所述建图为：

控制车辆在所需要建图的环境中进行运动，启动车辆的建图指令，然后控制 车辆在所需要建图的环境中进行运动，此时激光雷达会将采集到的数据会呈现出 当前环境的二维地图，即构图完成；

过程中实时观察车辆的直线状态、转弯状态、地图构建的完成度；

步骤1中所述将整个地图环境用栅格地图表示具体过程为：

栅格地图的计算公式为：

其中，P为栅格地图的后验概率，m为车辆所在环境地图，x_1∶t为用所有机 器人位姿定义的路径，z_1：t为直到时刻t的所有测量值；

式中栅格地图的后验概率P为：

P(m|x_1:t,z_1:t)

并且该概率由激光雷达传感器的数据data＝{x₁…x_t，z₁…z_t}计算：

data＝{x₁…x_t，z₁…z_t}带入P(m|x_1：t，z_1：t)

即可，其中x₁…x_t对应x_1∶t即为直到时刻t的所有机器人位姿定义的路径， z₁…z_t对应z_1：t即为直到时刻t的所有测量值；

步骤2：统计每个栅格的被激光束自由通过的次数以及栅格被激光束击中的 次数；

步骤2中所述第i个栅格被激光束自由通过的次数为misses(i)即这个栅格为 空闲的次数，步骤2中所述第i个栅格被激光束击中的次数为hits(i)即这个栅格 为占用的次数；

hits(j)为第j个栅格被激光束击中的次数，即这个栅格为占用的次数，具体 为：

其中，x_t表示t时刻机器人的位姿，t时刻激光雷达数据为z_t，第n个激光束 为z_t，n，c_t，n表示t时刻的第n个激光束是否为最大值，c_t，n＝1表示最大值，c_t，n＝0表 示正常值，f(x_t，n，z_t，n)表示t时刻的第n个激光束击中的栅格的下标；

misses(j)为第j个栅格被激光束自由通过的次数，即这个栅格为空闲的次数， 具体为：

其中，x_t表示t时刻机器人的位姿，t时刻激光雷达数据为z_t，第n个激光束 为z_t，n，c_t，n表示t时刻的第n个激光束是否为最大值，c_t，n＝1表示最大值，c_t，n＝0表 示正常值，f(x_t，n，z_t，n)表示t时刻的第n个激光束击中的栅格的下标；

步骤3：对地图进行估计，确定优化函数，完成建图。

步骤3中所述对地图进行估计，因此建立地图的极大似然估计模型为：

并以此为优化函数，其中，a_j表示栅格j被激光击中的次数，即步骤2中所述上 述hits(j)，b_j表示栅格j被激光通过的次数，即步骤2中所述misses(j)；

步骤3中所述完成建图，以所述优化函数为极大似然估计模型求其极值便可 得到最优化地图。

进行车辆底盘运行精度校准，进行车辆的校准，因为车辆的安装精度问题， 在安装的过程中的一些小的误差，导致坐标系的转换出现误差，引起数据匹配问 题。因此车辆要想获得理想的地图，取得好的效果，对室内车辆的校准是必不可 少的一步。

主要关注走直线误差以及转动角度误差。第一步在ROS操作系统打开底盘控 制端口，控制车辆前进一米和原地转动360°两个指令。要保证误差在1％之内， 假如误差过大，通过调整车辆底板车轮直径、两个车轮间距、动力系数这三个参 数。准确校准过程为首先校准走1米直线，这一步完成后误差在允许范围内，进 行原地转动校准。因为直线校准仅仅与车轮直径有关，而不与其它三个参数有关， 假如先校准转动角度再校准直线，直线矫正会再影响角度矫正。直线矫正过程中， 当车辆运行的距离大于一米，通过调大车轮直径修正；当车辆实际运行距离小于 一米，通过调小车轮直径。实际建图过程中假如还不理想，可以再通过校准3 米、5米、10米，以期获得理想效果。原地转动360°校准，首先控制车辆底盘原地转动360°，如果实际车辆转动角度大于360°，通过调小车轮间距修正； 实际过程中如果转动角度小于360°，通过调大车轮间距修正。在校准过程中， 如果车轮直径与车轮间距看起来与正常车辆的直径与间距区别较大，这里就需要 调整动力系数来帮助矫正。

进行激光雷达运动畸变矫正，运动畸变产生的原因主要是因为激光雷达数据 的获取是需要时间，采集某一个特征并不是瞬时完成的，选用的TOF激光雷达在 几微米之间数据都会有一点小的偏移，最后导致一个瞬时数据采集过程中不是闭 环，有一个大的偏差，激光雷达测量的过程中伴随着车辆的运动，加剧了畸变的 产生。

选择里程计辅助方法进行数据矫正，里程计可以直接测量车辆位移角度，有 较高的局部测量精度，较快的更新速度。具体实施过程介绍如下：已知激光雷达 两个起始时刻时间t_s，t_e，两个激光数据之间间隔Δt，里程计的数据按顺序在队 列中存储，时间间隔在t_s和t_e之间，把每个时刻激光雷达数据对应的车辆位姿计 算出来，再把所有的位姿转换到同一个坐标系中，用二次线性牛顿插值法对里程 计数据进行标定。

图5中A、B分别为用传统覆盖栅格建图法构建的两面墙边的边界，a、b分 别为本发明方法优化后的墙边的边界，其中A优化后为a，B优化后为b。

综上所述本发明提供的室内车辆地图构建方法，在现有栅格地图质量方面， 改善了局部栅格精度，栅格地图的边界栅格过多问题得以解决。在构图之前进行 了车辆的直线与角度校准使得数据匹配更为准确，还进行了构图所用传感器的数 据校准，这个也是提高地图精度一大措施。

对所公开的实施例的上述说明，使本领域专业技术人员能够实现或使用本发 明。对这些实施例的多种修改对本领域的专业技术人员来说将是显而易见的，本 文中所定义的一般原理可以在不脱离本发明的精神或范围的情况下，在其它实施 例中实现。

Claims (4)

1.一种室内车辆地图构建方法，其特征在于，包括以下步骤：

步骤1：通过建图前校准、建图以构建地图环境，将整个地图环境用栅格地图表示；

步骤2：统计每个栅格的被激光束自由通过的次数以及栅格被激光束击中的次数；

步骤3：对地图进行估计，确定优化函数，完成建图。

2.根据权利要求1所述的室内车辆地图构建方法，其特征在于，步骤1中所述建图前校准包括：

车辆底盘运行精度校准、激光雷达运动畸变矫正、根据里程计辅助方法进行激光雷达数据矫正三部分；

所述车辆底盘运行精度校准具体方法是：

通过先让车辆走1米直线，观察构建地图的距离与实际车辆走过路程构建地图的距离直接在上位机上读取，实际车辆走过路程直接测量，若误差在1％内，则再控制车辆原地旋转360°，再次观察地图上车辆是否转过360°，若误差在1％内，则整个车辆底盘运行精度调节完毕；假如直线误差过大则通过调整车轮直径即更换不同直径轮胎来解决，若转动角度误差过大，则通过调整车轮间距来完成，所述激光雷达运动畸变矫正具体为：

采用里程计辅助方法进行激光雷达数据矫正，主要方法为给车辆安装里程计，里程计可以直接测量车辆位移角度，有较高的局部测量精度，较快的更新速度，具体方法为：已知激光雷达两个起始时刻时间t_s，t_e，两个激光数据之间间隔Δt，里程计的数据按顺序在队列中存储，时间间隔在t_s和t_e之间，把每个时刻激光雷达对应的里程计数据，再把所有的位姿转换到同一个坐标系中，然后将激光雷达数据和里程计数据利用二次线性牛顿插值法确定实际的运动轨迹，从而消除运动畸变，具体过程如下：

里程计队列中位姿和激光数据同步，假设第i和第j数据对应的时刻分别为t_s和t_e

p_s＝OdomList[i]

p_j＝OdomList[j]

若在t_s时刻没有对应的里程计位姿，则进行线性插值，设l，k时刻有位姿，且l<s<k,则：

p_l＝OdomList[l]

p_k＝OdomList[k]

二次插值

在一帧数据之间，认为机器人做匀加速运动，由于机器人的位姿是关于时间t的二次函数，设且l<m<k，则：

已知p_s，p_m，p_e，可以插值一条二次曲线：

p(t)＝At²+Bt+G

t_s≤t≤t_e

对二次曲线进行近似

利用分段线性函数对二次曲线进行近似，在t_s和t_e时间段内，一共取k个位姿{p_s,p_s+1,…，p_s(n-2)，p_s+1}

则：

坐标统一及激光数据的发布

一帧激光数据n个激光点，每个激光点对应的位姿{p₁，p₂，…，p_n}通过上述介绍的方法插值得到

x_i为转化之前的坐标，x_i′为转换之后的坐标，则：

把转换之后的坐标转换成激光数据发布出去：

x_i′＝(p_x，p_y)

angle＝atan2(p_y，p_x)

步骤1中所述建图为：

控制车辆在所需要建图的环境中进行运动，启动车辆的建图指令，然后控制车辆在所需要建图的环境中进行运动，此时激光雷达会将采集到的数据会呈现出当前环境的二维地图，即构图完成；

过程中实时观察车辆的直线状态、转弯状态、地图构建的完成度；

步骤1中所述将整个地图环境用栅格地图表示具体过程为：

栅格地图的计算公式为：

其中，P为栅格地图的后验概率，m为车辆所在环境地图，x_1∶t为用所有机器人位姿定义的路径，z_1∶t为直到时刻t的所有测量值；

式中栅格地图的后验概率P为：

P(m|x_1∶t，z_1∶t)

并且该概率由激光雷达传感器的数据data＝{x₁…x_t，z₁…z_t}计算：

data＝{x₁…x_t，z₁…z_t}带入P(m|x_1：t，z_1：t)

即可，其中x₁…x_t对应x_1：t即为直到时刻t的所有机器人位姿定义的路径，z₁…z_t对应z_1：t即为直到时刻t的所有测量值。

3.根据权利要求1所述的室内车辆地图构建方法，其特征在于，步骤2中所述第i个栅格被激光束自由通过的次数为misses(i)即这个栅格为空闲的次数，步骤2中所述第i个栅格被激光束击中的次数为hits(i)即这个栅格为占用的次数；

hits(j)为第j个栅格被激光束击中的次数，即这个栅格为占用的次数，具体为：

其中，x_t表示t时刻机器人的位姿，t时刻激光雷达数据为zt，第n个激光束为z_t，n，c_t，n表示t时刻的第n个激光束是否为最大值，c_t，n＝1表示最大值，c_t，n＝0表示正常值，f(x_t，n，z_t，n)表示t时刻的第n个激光束击中的栅格的下标；

misses(j)为第j个栅格被激光束自由通过的次数，即这个栅格为空闲的次数，具体为：

其中，x_t表示t时刻机器人的位姿，t时刻激光雷达数据为z_t，第n个激光束为z_t，n，c_t，n表示t时刻的第n个激光束是否为最大值，c_t，n＝1表示最大值，c_t，n＝0表示正常值，f(x_t，n，z_t，n)表示t时刻的第n个激光束击中的栅格的下标。

4.根据权利要求1所述的室内车辆地图构建方法，其特征在于，步骤3中所述对地图进行估计，因此建立地图的极大似然估计模型为：

并以此为优化函数，其中，a_j表示栅格j被激光击中的次数，即步骤2中所述上述hits(j)，b_j表示栅格j被激光通过的次数，即步骤2中所述misses(j)；步骤3中所述完成建图，以所述优化函数为极大似然估计模型求其极值便可得到最优化地图。