CN115963510A - 一种基于激光点云的物流园区车辆精准停车方法和系统 - Google Patents

Abstract

本发明涉及一种基于激光点云的物流园区车辆精准停车方法，该方法首先获取库位信息并将库位信息转化为粗停坐标；车辆依据预设路径到达粗停坐标；采集车辆尾部的点云数据通过最小二乘法投影至XY二维平面，平面与精停模板信息比对得到车辆与卸货门之间的距离D；D与车辆最佳停放位置

比较以确定车辆是否需要调整，若无需调整则结束操作，否则将车辆调整信息发送给车辆规划模块；车辆规划模块根据车辆调整信息规划车辆移动，并将规划好的车辆移动信息发送给车辆控制模块，以控制车辆移动一定距离；车辆移动一定距离后，检查车辆与卸货门及其周边区域之间的相对位置是否在车辆最佳停放范围内。

Description

一种基于激光点云的物流园区车辆精准停车方法和系统

技术领域

本发明属于汽车自动驾驶技术领域，具体涉及一种基于激光点云的物流园区车辆精准停车方法和系统。

背景技术

目前，随着科学技术的发展，传统的人工物流园区正逐步向智慧物流、无人物流转型，基于无人驾驶技术的物流车辆因其高效、便捷和全天候等特性而备受青睐。由于物流车辆在园区内进行装卸货物作业时，会经常遇到顶点精准停车的问题，要求车辆的货箱门与车间的卸货门对齐，方便叉车及搬运人员进入车辆货箱搬运货物。园区的车辆调度管理系统会下发重点目标库位编号信息，车辆在进入目标库位后仍需调整，直至货箱门与卸货门对齐。

现有技术中，为了提高人工停车的精准度，最常见的方案是采用机械式的限位装置，即在卸货门左右安放橡胶限位块，当车辆进入目标车库后向后倒车，直到货箱与橡胶块接触，驾驶员以此判断车辆位置是否停止到位。由此可见，目前的精准停车方案中，人为经验因素高，对驾驶员依赖程度大，且定位胶块在使用过程中会产生磨损，磨损后不仅对精度产生影响，还与可能会导致无人驾驶物流车辆后部传感器损坏。

为了避免人工停车所存在的问题，进一步提高停车精度，现在大多采用自动停车方式，自动驾驶汽车通过传感器识别参考点来判断汽车所处的位置，从而实现一次性精准停车。公开号为CN109941274B的中国专利公开了基于雷达测距识别岸桥的停车状态方法及系统、服务器及介质，该方法利用激光雷达获取车辆附近确定的障碍物点云来计算确定车辆行进位置，通过识别车辆正前方横梁特征点云并对其进行跟踪测距，结合识别点云中横梁特征测距及车身CAN总线获取的速度计算目标停车距离，达到实时描述车辆行进位置的目的。该方法在计算距离时，需要对目标点云（即岸桥横梁）和自车点云进行拟合计算，每次停车时都需要重新进行计算，一旦遇到无法提供足够特征点云的岸桥横梁就无法进行精停操作；同时该方法一直实时采集并存储激光雷达扫描得到的点云数据，在“激光雷达检测到岸桥点云预估区域出现几根较均匀的线条，则判断激光雷达检测到准确的岸桥点云区域”，导致该方法存在一定的误判风险，即在不需要进行停车的时候，系统根据采集到的点云误触发判断车辆进入停车状态，且一直运行的程序也会增大系统负担；该方法通过激光雷达获取岸桥横梁的三维点云并投影至XY二维平面，利用最小二乘原理将点云拟合成直线，再根据直线的斜率计算横梁相对车身的倾斜角进而计算车辆与横梁之间的距离。

发明内容

本发明的目的在于，针对现有技术所存在的问题，提出一种基于激光点云的物流园区车辆精准停车方法和系统。

为了达到以上目的，本发明提供一种基于激光点云的物流园区车辆精准停车方法，包括以下步骤：

步骤1、获取库位信息并将库位信息转化为终点坐标，终点坐标设为粗停坐标；转至步骤2；

步骤2、车辆依据预设路径到达粗停坐标，调整库位内位姿使车身平行于库位线；转至步骤3；

步骤3、在车辆尾部安装激光雷达，实时采集激光雷达扫描得到的车辆尾部的点云数据，将点云数据通过最小二乘法投影至XY二维平面，以便将投影在XY二维平面的点云数据拟合成平面，然后将平面与依照步骤1的库位信息从数据库中调出的精停模板信息进行比对，得到车辆与卸货门之间的距离 D；转至步骤4；

步骤4、将车辆与卸货门之间的距离 D与车辆最佳停放位置进行比较，以确定车辆是否需要调整，若车辆无需调整则结束操作，若车辆需要调整则将车辆调整信息发送给车辆规划模块，所述车辆调整信息包括车辆调整方向及调整距离；转至步骤5；

步骤5、车辆规划模块根据车辆调整信息规划车辆移动，并将规划好的车辆移动信息发送给车辆控制模块，以控制车辆移动一定距离；转至步骤6；

步骤6、车辆移动一定距离后，检查车辆与卸货门及其周边区域之间的相对位置，若该位置在车辆最佳停放范围内则操作结束，否则返回步骤3。

本发明提供一种基于激光雷达点云强泛化能力的精准停车方法和系统，采用该方法无需进行人工路标的铺设，就能实现无人物流车倒车时的精准对位和精准停车，精度更高。同时，增加了粗停模块进行判断，只有完成粗停任务，到达设定的粗停坐标时才会触发精停车，对车后点云进行处理，这种方法既避免了误判又降低了整体系统的负载。

本发明进一步的采用如下技术方案：

所述步骤3中，所述精停模板信息是预先对待精停的卸货门及其周边区域进行激光点云采样，再利用最小二乘法将采样得到的三维点云投影在XY二维平面上，拟合成为平面精停模板信息。

上述方法中，可将采集的点云数据拟合成平面，通过比较平面上各点云之间的距离，经过运算得出车辆与卸货门的距离，理论上精度更高。另外，本发明预先构建各库位精停激光点云模板，由于模板的存在可以预先提取足够多的特征，满足停车的激光点云特征需求，每次停车时只要对其中一些特征点云进行对比即可，需要采集的点云数量减少，鲁棒性更好。同时，较少的点云提取处理数量也使系统运行速度更快。

所述步骤3中，在车辆后方安装补盲激光雷达，用于对卸货门及周边区域进行激光点云采样；以车后补盲激光雷达为中心，卸货门周边区域的选取标准为。

所述步骤4中，设车辆的最佳停放位置为，通过下式计算的值，

式中，为车辆距离卸货门的最大距离，为车辆距离卸货门的最小距离。

所述步骤4中，根据与 D之间的关系判断车辆是否需要调整，当时说明车辆进入最佳停车位置，车辆无需调整结束操作；当时说明车辆位置过于靠后，需要向前调整，将包含车辆需要向前调整以及向前调整距离的信息发送给车辆规划模块；当时说明车辆位置过于靠前，需要向后调整，将包含车辆需要向后调整以及向后调整距离的信息发送给车辆规划模块。

相应的，车辆向前调整的距离为；车辆向后调整的距离为。

所述步骤5中，所述车辆移动信息是根据车辆调整信息生成的，包含期望速度、车辆移动方向、车辆移动距离等信息，其中所述车辆移动方向为步骤4中的车辆调整方向，所述车辆移动距离为步骤4中的车辆移动距离，所述规划模块可根据车辆移动距离规划期望速度；设车辆移动距离为s，那么还可以通过下式计算s的值，

式中，为车辆实时速度， 为车辆开始运动的时间，为车辆结束运动的时间，dt是积分公式的一部分。

上述步骤中的调整距离就是车辆要移动的距离s，车辆移动是由控制模块执行的，规划下发了期望移动距离s，但控制模块控制的是车辆速度，即s=vt，即距离=速度*时间，由于车辆速度在移动的时间中是变化的，车辆不是匀速行驶的，所以车辆实际移动距离是车辆移动期间的速度积分，即。那么，规划模块通过车辆移动距离s可以进一步规划车辆的期望速度和加速度，进而控制车辆移动。

本发明还提供基于激光点云的物流园区车辆精准停车系统，该系统包括以下模块：

车队管理系统，用于接收目标库位信息并根据目标库位信息生成途径车道、任务终点坐标、目标库位、目标车道等信息；

任务管理模块，用于接收车队管理系统发送的途径车道、任务终点坐标、目标库位、目标车道等信息，并通过内部转换生成路径点、粗停任务终点坐标、粗停信息和库位信息；

地图模块，用于接收任务管理模块发送的路径点、粗停任务终点坐标，并通过内部运算后生成地图引导线信息；

激光定位模块，用于接收任务管理模块发送的粗停信息、库位信息以及激光雷达实时采集的激光点云数据，将采集的激光点云数据投影至XY二维平面生成车辆粗停位置信息，然后根据库位信息调取精停模板信息，并与车辆粗停位置进行比较，计算车辆需要移动距离信息；

规划模块，用于接收地图模块发送的地图引导线信息以及激光定位模块发送的移动距离信息，并根据地图引导线信息对车辆行驶路径进行规划后将规划路径发送给控制模块，同时根据移动距离信息生成路径点、加速度和速度请求信息；

控制模块，用于接收规划模块发送的规划路径并根据规划路径控制车辆行驶至粗停任务终点；用于接收路径点、加速度和速度请求信息，并根据路径点、加速度和速度请求信息控制位于粗停位置的车辆转向、加减速、前进和后退。

本发明所提出的基于激光雷达点云的精准停车方法，是利用激光雷达获取车辆后方物流装卸门及周围感兴趣区域点云，计算车辆相对装卸门位置，综合所收集到的点云信息及底盘CAN线上获取的车辆速度对移动距离进行精准控制，以达到精准停车的目的。相较于目前物流园区普遍使用的停车对准方法，该方法不会由于物理定位装置的磨损而产生误差，且不需要对场地进行改造，可以精准控制停车位置。

附图说明

下面结合附图对本发明作进一步的说明。

图1为本发明车辆精准停车方法的流程图。

图2为本发明车辆精准停车系统的原理框图。

图3为本发明车辆精准停车系统的信息交互时序图。

图4为本发明中一个实施例的平面模板和直线模板数据图。

具体实施方式

实施例一

如图1所示，一种基于激光点云的物流园区车辆精准停车方法，包括以下步骤：

步骤1、获取调度系统下发的库位信息，库位信息包括由调度系统下发的库位编号以及终点经纬度坐标，依据调度系统下发的库位信息转化为停车终点坐标信息，将停车终点坐标信息设为粗停坐标。

步骤2、车辆依据规划模块下发的预设路径到达粗停坐标，并调整库位内位姿使车身平行于库位线，进入精停阶段。其中，车辆位姿指车辆的位置及姿态（包括航向、车辆的GPS坐标等），库位内位姿就是指车辆在库位内的位姿，也包括车辆在库位内的航向和GPS坐标等信息；库位线是指停车位周围的白线，一般认为车子平行于白线的时候是粗停到位的必要条件。然后，依据步骤1中的库位信息调出存储在数据库中的精停模板信息。所述精停模板是预先对精停的卸货门及其周边区域进行激光点云采样，即通过激光雷达提取卸货门及周边感兴趣区域（主要是指墙体）的点云数据，并利用最小二乘法将提取的点云数据投影到XY二维平面，拟合成直线或平面，即得到精停模板信息，然后将精停模板信息存储在数据库中，每个库位对应一个精停模板信息。在车辆后方安装补盲激光雷达，用于对卸货门及周边区域进行激光点云采样。由于每个卸货门及周围的感兴趣区域都存在细微差异，因此需要提前对卸货门及其周边区域进行激光点云建模，以适配后续不同库位的精准泊车需求。感兴趣区域的选取标准为，即以车后补盲激光雷达为中心，后方区域3米，左、右两侧各1.5米，高1.5至3.5米的区域为感兴趣区域，既能采集到足够多的点云，又可以避免杂草等对测距的影响。选取感兴趣区域时，选取的区域可以根据不同的场景进行变动，而非一成不变。

步骤3、进入精停阶段后，激活安装在车辆尾部的激光雷达，实时获取车辆尾部点云，并通过最小二乘法将得到的点云数据投影至XY二维平面，拟合成一条直线或平面（见图4），该直线或平面与步骤2中的精停模板进行比对，得到车辆与卸货门之间的距离 D。实际操作时，对比拟合平面和精停模板中点云的距离，先将特征相同的点云进行匹配和筛选，然后计算对应点云间的距离，得到两平面的距离即为 D。

步骤4、设车辆距离卸货门的最小距离为，车辆距离卸货门最大的距离为。其中，最小距离为车辆距离卸货门最近的距离，小于这个距离会使车辆接触物理限位装置，有损坏车辆尾部传感器的风险；最大距离为车辆距离卸货门最大的距离，超过这个距离会导致物流人员无法进入货箱卸货。

可得出车辆最佳停放位置 ，即。

那么，有以下三种情况：

a.当时，即 D位于时，车辆进入最佳停车位置，结束操作；

b.当时时，车辆位置过于靠后，需要向前调整，向前调整的距离为；

c.当时，车辆位置过于靠前，需要向后调整，向后调整的距离为。

以上过程的系统调优可以依据经验值进行，至此完成了对车辆与卸货门距离的估计和调整距离的计算。

步骤5、将步骤4中计算出的调整距离发送给规划模块，规划模块根据调整距离获得车辆移动信息，该车辆移动信息包括车辆移动距离s、移动方向和期望速度、期望加速度等信息，且s=或，即车辆移动距离为前述车辆调整距离，移动方向即为调整方向，规划模块根据车辆移动距离规划期望速度；然后将车辆移动信息发送给车辆控制模块，车辆控制模块根据期望速度控制车辆移动，其中车辆移动的距离s还可以通过下式计算，

其中，是车辆实时速度，可以由底盘CAN线上获得；为车辆开始运动的时间，为车辆结束运动的时间。

步骤6、车辆移动一定距离s后，再通过步骤3的方法获得此时车辆与卸货门之间的距离 D′（即车辆移动一定距离s后先通过激光雷达实时获取车辆尾部点云，再通过最小二乘法将得到的点云数据投影至XY二维平面拟合成一条直线或平面，最后将该拟合的直线或平面与精停模板进行比对得到车辆与卸货门之间的距离 D′），以检查车辆与装卸门及感兴趣区域之间的相对位置，若在最佳停车范围内（即 D′=），则退出精准泊车任务，结束操作。若不在最佳停车范围内，则返回步骤3。

如图2和图3所示，本实施例还提供一种基于激光点云的物流园区车辆精准停车系统，主要包括以下几个模块：

FMS（Fleet Management System）模块，即车队管理系统，其作用是在收到目标库位信息后生成途径车道、任务终点坐标、目标库位、目标车道等信息，并将途径车道、任务终点坐标、目标库位、目标车道等信息传递给Taskmanager车辆任务管理模块。该模块可以对多车发送命令信息，起到多车调度协同作业的功能。

Taskmanager模块，即任务管理模块，主要是接收上游FMS模块的信息，并将接收的信息通过内部转换生成路径点、粗停任务终点坐标、粗停信息、库位信息，然后将路径点、粗停任务终点坐标发送给MAP地图模块，同时将粗停信息、库位信息给到Lidar_locat激光定位模块。

MAP模块，即地图模块，主要是接收Taskmanager模块的信息，将接收的信息通过内部运算后生成地图引导线信息，将地图引导线信息给到Planning模块。

Planning模块，即规划模块，用于依据MAP模块生成的地图引导线，规划对车辆行驶路径，同时接收Lidar_locat模块的移动距离信息，生成路径点、加速度和速度请求等信息，将路径点、加速度和速度请求等信息发送给下游Control控制模块。

Control模块，即控制模块，负责控制车辆转向、加减速、前进和后退。

Lidar_locat模块，即激光定位模块，精停功能包含在该模块中。该模块通过车位部安装的激光雷达，实时采集存储激光点云数据，同时将采集到的数据通过最小二乘法被投影到XY平面上，根据Taskmanager模块提供的库位信息调取相应精停模板，并与车辆粗停位置进行比较，以计算车辆需要移动的距离，并将该距离发送给Planning模块。

综上，本发明公开的一种基于激光点云的物流园区车辆精准停车方法和系统，包括FMS模块、Taskmanager模块、MAP模块、Planning模块、Control模块和Lidar_locate模块，通过FMS模块下发目标库位信息、途径车道、终点坐标等信息给到Taskmanager模块，Taskmanager模块再将转换后的粗停任务终点、路径点发给MAP模块，MAP将以上信息转化为Planning模块可以直接使用的引导线信息，Planning模块依据引导线信息生成车辆可行驶路径，再由下游Control模块控制车辆行驶到粗停任务终点，车辆到达粗停任务终点后，激活精停任务，Lidar_locat模块通过激光点云计算车辆相对装卸门距离后，将车辆距离信息发送给Planning模块，Planning模块将车辆移动信息发送给Control模块，再由Control模块控制车辆移动到精停任务终点。

本发明提出了一种利用激光雷达采集激光点云在物流园区场景对车辆和物流装卸门的距离进行精准测距的方法，用以实现车辆的精准停车，这种方法不需要依赖机械式限位装置，避免了装置磨损对停止精度的影响，同时不需要依赖司机，实现全自动化的精准停车。

本发明还提供一种基于激光点云的物流园区车辆精准停车系统，可以方便的进行不同库位的精准停车，并且可以方便的对期望停车距离进行修改。由于只需要采集不同库位的模板，因此可以对不同类型的库位进行适配，并且只需要修改算法中的参数即可以对停车距离进行修改。另外，本发明的系统可以方便的在不同的技术方案间进行移植来实现功能，只需要更改上游数据接口即可。

应当说明的是，以上步骤的执行顺序以其内在逻辑和功能确定，其执行的次序只要能够完成本专利公开的技术方案所期望完成的结果即可，不应对本发明及实施例的实施造成任何的限制和约束。除上述实施例外，本发明还可以有其他实施方式。凡采用等同替换或等效变换形成的技术方案，均落在本发明要求的保护范围。

Claims (8)

1.一种基于激光点云的物流园区车辆精准停车方法，其特征在于，包括以下步骤：

步骤1、获取库位信息并将库位信息转化为终点坐标，终点坐标设为粗停坐标；转至步骤2；

步骤2、车辆依据预设路径到达粗停坐标，调整库位内位姿使车身平行于库位线；转至步骤3；

步骤3、在车辆尾部安装激光雷达，实时采集激光雷达扫描得到的车辆尾部的点云数据，将点云数据通过最小二乘法投影至XY二维平面，然后与依照步骤1的库位信息从数据库中调出的精停模板信息进行比对，得到车辆与卸货门之间的距离D；转至步骤4；

步骤4、将车辆与卸货门之间的距离D与车辆最佳停放位置进行比较，以确定车辆是否需要调整，若车辆无需调整则结束操作，若车辆需要调整则将车辆调整信息发送给车辆规划模块，所述车辆调整信息包括车辆调整方向及调整距离；转至步骤5；

步骤5、车辆规划模块根据车辆调整信息规划车辆移动，并将规划好的车辆移动信息发送给车辆控制模块，以控制车辆移动一定距离；转至步骤6；

步骤6、车辆移动一定距离后，检查车辆与卸货门及其周边区域之间的相对位置，若该位置在车辆最佳停放范围内则操作结束，否则返回步骤3。

2.根据权利要求1所述一种基于激光点云的物流园区车辆精准停车方法，其特征在于，所述步骤3中，所述精停模板信息是预先对待精停的卸货门及其周边区域进行激光点云采样，再利用最小二乘法将采样得到的三维点云投影在XY二维平面上，拟合成为平面精停模板信息。

3.根据权利要求2所述一种基于激光点云的物流园区车辆精准停车方法，其特征在于，所述步骤3中，在车辆后方安装补盲激光雷达，用于对卸货门及周边区域进行激光点云采样；以车后补盲激光雷达为中心，卸货门周边区域的选取标准为。

4.根据权利要求3所述一种基于激光点云的物流园区车辆精准停车方法，其特征在于，所述步骤4中，设车辆的最佳停放位置为，通过下式计算的值，

式中，为车辆距离卸货门的最大距离，为车辆距离卸货门的最小距离。

5.根据权利要求4所述一种基于激光点云的物流园区车辆精准停车方法，其特征在于，所述步骤4中，根据与D之间的关系判断车辆是否需要调整，当时说明车辆进入最佳停车位置，车辆无需调整结束操作；当时说明车辆位置过于靠后，需要向前调整，将包含车辆需要向前调整以及向前调整距离的信息发送给车辆规划模块；当时说明车辆位置过于靠前，需要向后调整，将包含车辆需要向后调整以及向后调整距离的信息发送给车辆规划模块。

6.根据权利要求5所述一种基于激光点云的物流园区车辆精准停车方法，其特征在于，车辆向前调整的距离为；车辆向后调整的距离为。

7.根据权利要求6所述一种基于激光点云的物流园区车辆精准停车方法，其特征在于，所述步骤5中，所述车辆移动信息是根据车辆调整信息生成的，包含期望速度、期望加速度、车辆移动方向、车辆移动距离，其中所述车辆移动方向为步骤4中的车辆调整方向，所述车辆移动距离为步骤4中的车辆移动距离，所述规划模块可根据车辆移动距离规划期望速度；设车辆移动距离为s，还可以通过下式计算s的值，

式中，为车辆实时速度， 为车辆开始运动的时间，为车辆结束运动的时间。

8.一种基于激光点云的物流园区车辆精准停车系统，其特征在于，包括以下模块：

车队管理系统，用于接收目标库位信息并根据目标库位信息生成途径车道、任务终点坐标、目标库位、目标车道；

任务管理模块，用于接收车队管理系统发送的途径车道、任务终点坐标、目标库位、目标车道，并通过内部转换生成路径点、粗停任务终点坐标、粗停信息和库位信息；

地图模块，用于接收任务管理模块发送的路径点、粗停任务终点坐标，并通过内部运算后生成地图引导线信息；

激光定位模块，用于接收任务管理模块发送的粗停信息、库位信息以及激光雷达实时采集的激光点云数据，将采集的激光点云数据投影至XY二维平面生成车辆粗停位置信息，然后根据库位信息调取精停模板信息，并与车辆粗停位置进行比较，计算车辆需要移动的距离信息；

规划模块，用于接收地图模块发送的地图引导线信息以及激光定位模块发送的移动距离信息，并根据地图引导线信息对车辆行驶路径进行规划后将规划路径发送给控制模块，同时根据移动距离信息生成路径点、加速度和速度请求信息；

控制模块，用于接收规划模块发送的规划路径并根据规划路径控制车辆行驶至粗停任务终点；用于接收路径点、加速度和速度请求信息，并根据路径点、加速度和速度请求信息控制位于粗停位置的车辆转向、加减速、前进和后退。

Images