CN112484740B - 用于港口无人物流车的自动建图与自动地图更新系统 - Google Patents

Abstract

本发明提供一种用于港口无人物流车的自动建图与自动地图更新系统，包括软件和硬件，软件包括车载数据采集单元、数据传输单元、数据存储单元、计算单元和矢量地图绘制单元，硬件包括激光雷达、GPS、IMU，车联网模块和建图服务器，通过定时或人工触发完成港口先验地图的自动更新，解决了港口卸货区环境缓慢变化导致无人车辆定位失败的问题。

Description

用于港口无人物流车的自动建图与自动地图更新系统

技术领域

本发明涉及自动驾驶与地图数据处理技术领域，具体涉及一种用于港口无人物流车的自动建图与自动地图更新系统。

背景技术

随着无人驾驶技术的日趋成熟，无人驾驶技术率先在物流园区，集装箱港口卸货区这类封闭无人场景落地。

在港口卸货区这个封闭场景下，由于存在着车道线不完整以及部分行车区域没有车道线的情况，传统的依靠车道线识别和车道线保持的的自动驾驶解决方案无法完成任务。目前主流的港口无人驾驶解决方案是通过车辆当前的准确位置和期望目的地计算油门开度和方向盘转角的形式，控制车辆向目的地移动。目前无人驾驶车辆获得当前准确位置的方式主要有两种，第一种是使用高精度GPS（厘米级精度），另一种是使用精光雷达当前的扫描帧在先验地图中匹配出当前车辆的位置。第一种方式依赖于价格高昂的组合导航设备，而且需要在使用过程中持续购买高精度RTK服务，但是第一种方式获取当前位置的方式简单，第一种方式使用的设备可以直接输出无人车辆的当前位置，不需要消耗额外的算力来计算无人车辆的位置。第二种方式使用的主要设备是激光雷达，激光雷达的价格低廉，经过车载电脑的计算就可以输出无人车辆当前的准确位置，位置的精度与第一种方式相当，但是第二种方式依赖于准确的先验地图，如果先验地图错误，那么无人驾驶车辆将无法通过计算得到准确的当前位置。

从成本的角度出发，越来越多的港口卸货区无人驾驶解决方案开始采用第二种方式获取无人驾驶车辆的准确位置。但是，港口卸货区大多数时间环境都在发生缓慢变化（如：集装箱堆码位置的改变，码头吊装机的移动）。这些位置缓慢变化的物体短时间内并不会影响激光雷达的定位精度，但是当时间积累的足够久导致卸货区环境与先验地图产生了较大的出入时，车载电脑将无法计算出准确的无人车辆位置（也就是无人车辆定位失败）。

发明内容

发明目的：为了解决港口卸货区环境缓慢变化导致无人车辆定位失败这个问题，本发明提出了一种无人值守自动建图与自动地图更新系统，通过定时或人工触发完成港口先验地图的自动更新。

技术方案：为解决上述技术问题，本发明提供的用于港口无人物流车的自动建图与自动地图更新系统，包括软件和硬件，所述软件包括车载数据采集单元、数据传输单元、数据存储单元、计算单元和矢量地图绘制单元，所述硬件包括激光雷达、GPS、IMU，车联网模块和建图服务器。

具体地，所述车载数据采集单元部署在建图数据采集车上，用于采集数据，所述数据包括激光雷达点云帧、车辆速度、GPS坐标，以及IMU的方向角、加速度和角速度。

具体地，所述数据传输单元同时部署在建图数据采集车和建图服务器上，用于将车载数据采集单元采集到的数据传输到数据存储单元，将计算单元下发的建图指令和建图路径传输到建图数据采集车，向现场的港口无人物流车传输更新后的高精度点云地图。

所述数据存储输单元部署在建图服务器上，用于存储计算单元生成的高精度点云地图，存储数据传输单元传输的数据供计算单元取用。

所述计算单元部署在建图服务器上，用于生成高精度点云地图，根据预设触发地图更新功能。

所述矢量地图绘制单元部署在建图服务器上，为操作员提供绘制矢量地图的软件平台。

所述激光雷达是水平安装在采集车顶的32线激光雷达，所述GPS是安装在车顶的米级精度的消费级GPS，所述IMU水平安装在车身上，所述车联网模块安装在车顶。

具体地，该系统的实施包括以下步骤：

步骤1，采用人工遥控建图数据采集车采集港口环境数据，生成高精度点云地图，采集完成后数据传输至建图服务器；

步骤2，建图服务器根据采集到的数据首次生成港口高精度点云地图；

步骤3，对比首次生成的高精度点云地图是否与港口的真实环境相符，若相符则使用这一幅高精度点云地图作为初始高精度点云地图，若不符则重复步骤1-2；

步骤4，根据高精度点云地图中的地面点的强度信息绘制港口高精度矢量地图；

步骤5，将高精度矢量地图和高精度点云地图传输至所有的港口无人物流车，开始港口集装箱装卸作业。

步骤6，当满足地图更新触发条件时，启动地图更新流程；

步骤7，建图服务器根据步骤1中数据采集车走过的轨迹，规划出建图数据采集车的自动建图路线：根据遥控路点在矢量地图中位置，最靠近哪些道路的车道线的中心点，则自动路线使用哪条车道；服务器将规划好的自动建图路线发送给建图数据采集车；

步骤8，建图数据采集车根据规划好的自动建图路线行驶，当建图车辆进入匀速直线行驶阶段后开始采集数据；数据采集完成后，将数据发送到建图服务器；

步骤9，建图服务器生成新的高精度点云地图；

步骤10，建图服务器自动评估新的高精度点云地图中是否包含错误点云帧匹配结果；若存在错误匹配则执行步骤8-9；若不存在错误匹配，则执行步骤11；

步骤11，建图服务器自动评估新的高精度点云地图的精度，若精度不满足要求则执行步骤8-9；若精度满足要求，则执行步骤12；

步骤12，将存储在建图服务器上的步骤9生成的高精度点云地图传输至所有的港口无人物流车，覆盖车辆上原有的高精度点云地图。

具体地，所述步骤6中的触发条件包括人工触发、定时触发或本轮卸货完毕，作业车辆调度系统向建图服务器发送重新建图请求信号。

具体地，所述步骤10中评估新的点云地图中是否包含错误点云帧匹配结果的计算过程包括：

步骤10.1，使用CTRV作为车辆的运动学模型，使用IMU的姿态信息中的Yaw角作为CTRV模型中Yaw的观测量，车辆速度信息作为CTRV模型中v的观测量，点云帧匹配结果累积出的位置作为pose的观测量，使用UKF方法预测车辆的位置；

步骤10.2，UKF状态向量中的初始值v设置为车辆进入匀速直线行驶阶段后的车速，Yaw为车辆进入匀速直线行驶阶段后IMU输出的航向，角速度为0，位置为新地图中第1帧激光雷达关键帧的位置；

步骤10.3，从生成的高精度点云地图的第2帧激光雷达关键帧开始，比较UKF的预测结果和当前关键帧的位置，使用UKF的位置预测结果和当前关键帧的位置之间的欧式距离衡量位置偏差，使用UKF的Yaw预测结果和当前关键帧的Yaw角度的差的绝对值衡量Yaw的偏差，当位置偏差或者Yaw的偏差二者中的任意一个超过了预先设定的阈值，则建图服务器认定新的高精度点云地图中包含错误点云帧匹配结果，算法到此结束；反之则将当前关键帧的位置作为UKF的位置观测量，继续预测下一帧点云的位置，然后执行步骤10.4；

步骤10.4，重复步骤10.3，直到所有点云帧被遍历，则建图服务器认定新的高精度点云地图中不包含错误点云帧匹配结果。

具体地，所述步骤11中评估新的高精度点云地图的精度的计算过程包括：

步骤11.1，提取新地图中的地面点；

步骤11.2，在地面点中根据预先设置的强度阈值过滤出颜色符合要求的地面点；

步骤11.3，在步骤11.2得到的点中提取车道线；

步骤11.4，求解平移矩阵T（x，y）和旋转角Yaw（θ），使得步骤11.3得到的车道线在施加了平移T和旋转Yaw之后与实施步骤4中的矢量地图的交并比最大；

步骤11.5，若x，y和θ均小于预先设定的阈值，则建图服务器认定新的高精度点云地图的精度满足要求；若x，y和θ中任意一个值大于等于预先设定的阈值，则建图服务器认定新的高精度点云地图的精度不满足要求。

使用时，在地图坐标系下，港口第一次建图时人工绘制的矢量地图，与步骤11.3提取出的车道线进行比较，对车道线施加平移T(x,y)和旋转Yaw(θ)后得到新车道线，计新车道线和港口第一次建图时人工绘制的矢量地图的交并比，交并比等于以上交集的值与并集的值的比值。

有益效果：本发明的无人值守自动建图与自动地图更新系统，通过定时或人工触发完成港口先验地图的自动更新，解决了港口卸货区环境缓慢变化导致无人车辆定位失败的问题。

除以上所述的本发明解决的技术问题、构成技术方案的技术特征以及由这些技术方案的技术特征所带来的优点外。为使本发明目的、技术方案和有益效果更加清楚，下面将结合本发明实施例中的附图，对本发明所能解决的其他技术问题、技术方案中包含的其他技术特征以及这些技术特征带来的优点做更为清楚、完整的描述。

附图说明

图1是本发明自动建图与自动地图更新系统的系统架构图；

图2是本发明的自动建图与自动地图更新系统的系统流程图；

图3是评估新地图是否包含错误点云帧匹配结果的算法流程图；

图4是评估新地图的精度的算法的算法流程图；

图5是人工绘制的矢量地图和算法生成车道线相互比较的示意；

图6是对图5中车道线施加平移的示意；

图7是对图6中施加了平移的车道线继续施加旋转的示意；

图8是计算施加了平移和旋转的车道线与人工绘制的矢量地图交并比的示意。

具体实施方式

本发明的自动建图与自动地图更新系统由软件和硬件组成：

软件方面如图1所示，由车载数据采集单元、数据传输单元、数据存储单元、计算单元和矢量地图绘制单元五部分构成，其中：

车载数据采集单元部署在建图数据采集车上，车载数据采集单元的作用是采集激光雷达点云帧，采集车辆速度数据，采集IMU的方向角（Yaw）数据，采集GPS（消费级GPS，米级精度）数据，采集IMU的加速度和角速度数据。

数据传输单元同时部署在建图数据采集车和建图服务器上，数据传输单元的作用有三个，第一个作用是将车载数据采集单元采集到的数据传输到数据存储单元，第二个作用是将计算单元下发的建图指令和建图路径传输到建图数据采集车，第三个作用是向现场的作业车辆（无人驾驶集装箱运输车）传输更新后的高精度点云地图（先验地图）。

数据存储输单元部署在建图服务器上，数据存储单元的作用有两个，第一个作用是将数据传输单元传输的数据存储起来，供计算单元取用；第二个作用是存储计算单元生成的高精度点云地图（先验地图）。

计算单元部署在建图服务器上，计算单元的作用有两个，第一个作用是生成高精度点云地图（建图），第二个作用是根据预设触发地图更新功能。

矢量地图绘制单元部署在建图服务器上，矢量地图绘制单元的作用是为操作员提供绘制矢量地图的软件平台。

硬件方面如图2所示，使用水平安装在车顶的32线激光雷达，安装在车顶的GPS（消费级GPS，米级精度），水平安装在车身的IMU，安装在车顶的T-BOX（车联网模块），建图服务器使用处理器为Intel酷睿I7的工控机。

该系统的实施包括以下步骤：

步骤1，对于还没有高精度点云地图的港口，第一次生成高精度点云地图，需要人工遥控建图数据采集车采集港口环境数据。采集完成后数据传输至建图服务器。

步骤2，建图服务器根据采集到的数据首次生成港口高精度点云地图。

步骤3，工作人员对比首次生成的高精度点云地图是否与港口的真实环境相符，若相符则使用这一幅高精度点云地图作为初始高精度点云地图，若不符则重复步骤1-2。

步骤4，工作人员根据高精度点云地图中的地面点的强度信息绘制港口高精度矢量地图。

步骤5，将高精度矢量地图和高精度点云地图传输至所有的港口作业车辆，开始港口集装箱装卸作业。

步骤6，当满足地图更新触发条件时，启动地图更新流程。触发条件可以但不仅限于以下几种条件：

6.1，人工触发。

6.2，本轮卸货完毕，作业车辆调度系统向建图服务器发送重新建图请求信号。

6.3，定时触发。

步骤7，建图服务器根据第一次建图时人工遥控建图数据采集车走过的轨迹规划出建图数据采集车的自动建图路线：根据遥控路点在矢量地图中位置，最靠近哪些道路的车道线的中心点，则自动路线使用哪条车道。服务器将规划好的自动建图路线发送给建图数据采集车。

步骤8，建图数据采集车根据规划好的自动建图路线行驶，当建图车辆进入匀速直线行驶阶段后开始采集数据。数据采集完成后，将数据发送到建图服务器。

步骤9，建图服务器生成新的高精度点云地图。

步骤10，建图服务器自动评估新的高精度点云地图中是否包含错误点云帧匹配结果。若存在错误匹配则执行步骤8-9。若不存在错误匹配，则执行步骤11。

步骤11，建图服务器自动评估新的高精度点云地图的精度。若精度不满足要求则执行步骤8-9。若精度满足要求，则执行步骤12。

步骤12，将存储在建图服务器上的步骤9生成的高精度点云地图传输至所有的港口作业车辆，覆盖车辆上原有的高精度点云地图。

步骤13，重复步骤6-12。

如图3所示，步骤10中评估新的点云地图中是否包含错误点云帧匹配结果的计算过程（算法）具体为：

步骤10.1，使用CTRV作为车辆的运动学模型，使用IMU的姿态信息中的Yaw角作为CTRV模型中Yaw的观测量，车辆速度信息作为CTRV模型中v的观测量，点云帧匹配结果累积出的位置作为pose的观测量，使用UKF方法预测车辆的位置。

步骤10.2，UKF的初始值设置。因为建图车辆进入匀速直线行驶阶段后才开始采集数据，因此UKF状态向量中的初始值v即为车辆进入匀速直线行驶阶段后的车速，Yaw为车辆进入匀速直线行驶阶段后IMU输出的Yaw，角速度为0，位置为新地图中第1帧激光雷达关键帧的位置。UKF的协方差矩阵的初始值根据传感器的不同而不同。

步骤10.3，从生成的高精度点云地图的第2帧激光雷达关键帧开始，比较UKF的预测结果和当前关键帧的位置。使用UKF的位置预测结果和当前关键帧的位置之间的欧式距离衡量位置偏差。使用UKF的Yaw预测结果和当前关键帧的Yaw角度的差的绝对值衡量Yaw的偏差。当位置偏差或者Yaw的偏差二者中的任意一个超过了预先设定的阈值，则建图服务器认定新的高精度点云地图中包含错误点云帧匹配结果，算法到此结束，反之则将当前关键帧的位置作为UKF的位置观测量，继续预测下一帧点云的位置，然后执行步骤10.4。

步骤10.4，重复步骤10.3，直到所有点云帧被遍历，则建图服务器认定新的高精度点云地图中不包含错误点云帧匹配结果。

如图4所示，步骤11中评估新的高精度点云地图的精度的计算过程（算法）具体为：

步骤11.1，提取新地图中的地面点。

步骤11.2，在地面点中根据预先设置的强度阈值过滤出颜色符合要求的地面点（地面点的强度与地面车道线的颜色有关，通过强度值的范围过滤出特定颜色的点，阈值的设置根据车道线颜色的不同而不同）。

步骤11.3，在步骤11.2得到的点中提取车道线。

步骤11.4，求解平移矩阵T（x，y）和旋转角Yaw（θ），使得步骤11.3得到的车道线在施加了平移T和旋转Yaw之后与实施步骤4中的矢量地图的交并比最大。（求解方法包括但不仅限于粒子滤波算法，如图5-图8）。

步骤11.5，若x，y和θ均小于预先设定的阈值（阈值的选定与激光雷达的距离精度，和IMU的AHRS功能的精度有关，传感器的精度越高则阈值越小），则建图服务器认定新的高精度点云地图的精度满足要求。若x，y和θ中任意一个值大于等于预先设定的阈值，则建图服务器认定新的高精度点云地图的精度不满足要求。

如图5所示，地图坐标系下，人工绘制的矢量地图和步骤11.3提取出的车道线的示意图。其中数字1标注的是港口第一次建图时人工绘制的矢量地图，数字2标注的是从步骤11.3提取出的车道线。

如图6所示，地图坐标系下，对步骤11.3提取出的车道线施加平移T(x,y)的示意图。其中数字3标注的是对步骤11.3提取出的车道线施加平移T(x,y)之后得到的新的车道线。数字4标注的直角三角形，直角三角形的斜边是施加平移T(x,y)时车道线发生的位移。数字5标注的直角边的长度是平移T(x,y)中y的值。数字6标注的直角边的长度是平移T(x,y)中x的值。

如图7所示，地图坐标系下，对施加了平移T(x,y)的车道线继续施加旋转Yaw(θ)的示意图。其中，数字7标注的粗体虚线是对施加了平移T(x,y)的车道线继续施加旋转Yaw(θ)之后得到的新的车道线。数字8标注的是对施加了平移T(x,y)的车道线继续施加的旋转Yaw(θ)的旋转角度θ。

如图8所示，地图坐标系下，计算施加了平移T(x,y)和旋转Yaw(θ)的车道线和港口第一次建图时人工绘制的矢量地图的交并比的示意图。数字9标注的是计算交并比时用到的单元格，单元格为正方形，单元格的两条边分别平行于地图坐标系的两条坐标轴，单元格的大小可以根据激光雷达测距的精度进行设置，激光雷达测距的精度越高单元格的大小可以设置的越小。数字10标注的是同时包含矢量地图和车道线的单元格。数字11标注的是仅包含矢量地图的单元格。数字1标注的矢量地图和数字7标注的车道线的交集的值为同时包含矢量地图和车道线的单元格的数量。数字1标注的矢量地图和数字7标注的车道线的并集的值为同时包含矢量地图和车道线的单元格的数量与仅包含矢量地图的单元格的数量与仅包含车道线的单元格的数量的和。交并比等于以上交集的值与并集的值的比值。

Claims (4)

1.一种用于港口无人物流车的自动建图与自动地图更新系统，包括软件和硬件，所述软件包括车载数据采集单元、数据传输单元、数据存储单元、计算单元和矢量地图绘制单元，所述硬件包括激光雷达、GPS、IMU、车联网模块和建图服务器；其特征在于：

所述车载数据采集单元部署在建图数据采集车上，用于采集数据，所述数据包括激光雷达点云帧、车辆速度、GPS坐标以及IMU的方向角、加速度和角速度；

所述数据传输单元同时部署在建图数据采集车和建图服务器上，用于将车载数据采集单元采集到的数据传输到数据存储单元，将计算单元下发的建图指令和建图路径传输到建图数据采集车，向现场的港口无人物流车传输更新后的高精度点云地图；

所述数据存储输单元部署在建图服务器上，用于存储计算单元生成的高精度点云地图，存储数据传输单元传输的数据供计算单元取用；

所述计算单元部署在建图服务器上，用于生成高精度点云地图，根据预设触发地图更新功能；

所述矢量地图绘制单元部署在建图服务器上，为操作员提供绘制矢量地图的软件平台；

所述激光雷达是水平安装在车顶的32线激光雷达，所述GPS是安装在车顶的米级精度的消费级GPS，所述IMU水平安装在车身上，所述车联网模块安装在车顶；

建图与地图更新系统的工作执行以下步骤：

步骤1，采用人工遥控建图数据采集车采集港口环境数据，生成高精度点云地图，采集完成后数据传输至建图服务器；

步骤2，建图服务器根据采集到的数据首次生成港口高精度点云地图；

步骤3，对比首次生成的高精度点云地图是否与港口的真实环境相符，若相符则使用这一幅高精度点云地图作为初始高精度点云地图，若不符则重复步骤1-步骤2；

步骤4，根据高精度点云地图中的地面点的强度信息绘制港口高精度矢量地图；

步骤5，将高精度矢量地图和高精度点云地图传输至所有的港口无人物流车，开始港口集装箱装卸作业；

步骤6，当满足地图更新触发条件时，启动地图更新流程；

步骤7，建图服务器根据步骤1中建图数据采集车走过的轨迹，规划出建图数据采集车的自动建图路线：根据遥控路点在矢量地图中位置，最靠近哪些道路的车道线的中心点，则自动路线使用哪条车道；服务器将规划好的自动建图路线发送给建图数据采集车；

步骤8，建图数据采集车根据规划好的自动建图路线行驶，当建图数据采集车进入匀速直线行驶阶段后开始采集数据；数据采集完成后，将数据发送到建图服务器；

步骤9，建图服务器生成新的高精度点云地图；

步骤10，建图服务器自动评估新的高精度点云地图中是否包含错误点云帧匹配结果；若存在错误匹配则执行步骤8-步骤9；若不存在错误匹配，则执行步骤11；

步骤11，建图服务器自动评估新的高精度点云地图的精度，若精度不满足要求则执行步骤8-步骤9；若精度满足要求，则执行步骤12；

步骤12，将存储在建图服务器上的步骤9生成的高精度点云地图传输至所有的港口无人物流车，覆盖港口无人物流车上原有的高精度点云地图。

2.根据权利要求1所述的用于港口无人物流车的自动建图与自动地图更新系统，其特征在于：所述步骤6中的触发条件包括人工触发、定时触发或本轮卸货完毕，港口无人物流车调度系统向建图服务器发送重新建图请求信号。

3.根据权利要求1所述的用于港口无人物流车的自动建图与自动地图更新系统，其特征在于：所述步骤10中评估新的点云地图中是否包含错误点云帧匹配结果的计算过程包括：

步骤10.1，使用CTRV作为建图数据采集车的运动学模型，使用IMU的姿态信息中的Yaw角作为CTRV模型中Yaw的观测量，车辆速度信息作为CTRV模型中v的观测量，点云帧匹配结果累积出的位置作为pose的观测量，使用UKF方法预测建图数据采集车的位置；

步骤10.2，UKF状态向量中的初始值v设置为建图数据采集车进入匀速直线行驶阶段后的车速，Yaw为建图数据采集车进入匀速直线行驶阶段后IMU输出的航向旋转，角速度为0，位置为新地图中第1帧激光雷达关键帧的位置；

步骤10.3，从生成的高精度点云地图的第2帧激光雷达关键帧开始，比较UKF的预测结果和当前关键帧的位置，使用UKF的位置预测结果和当前关键帧的位置之间的欧式距离衡量位置偏差，使用UKF的Yaw预测结果和当前关键帧的Yaw角度的差的绝对值衡量Yaw的偏差，当位置偏差或者Yaw的偏差二者中的任意一个超过了预先设定的阈值，则建图服务器认定新的高精度点云地图中包含错误点云帧匹配结果，算法到此结束；反之则将当前关键帧的位置作为UKF的位置观测量，继续预测下一帧点云的位置，然后执行步骤10.4；

步骤10.4，重复步骤10.3，直到所有点云帧被遍历，则建图服务器认定新的高精度点云地图中不包含错误点云帧匹配结果。

4.根据权利要求1所述的用于港口无人物流车的自动建图与自动地图更新系统，其特征在于：所述步骤11中评估新的高精度点云地图的精度的计算过程包括：

步骤11.1，提取新地图中的地面点；

步骤11.2，在地面点中根据预先设置的强度阈值过滤出颜色符合要求的地面点；

步骤11.3，在步骤11.2得到的点中提取车道线；

步骤11.4，求解平移矩阵T（x，y）和旋转角Yaw（θ），使得步骤11.3得到的车道线在施加了平移T和旋转Yaw之后与实施步骤4中的矢量地图的交并比最大；

步骤11.5，若x，y和θ均小于预先设定的阈值，则建图服务器认定新的高精度点云地图的精度满足要求；若x，y和θ中任意一个值大于等于预先设定的阈值，则建图服务器认定新的高精度点云地图的精度不满足要求。

Images