[0001] 本申请涉及智能驾驶技术领域，尤其涉及一种基于港口环境的无缝定位系统及方法。

[0002] 随着科技的发展，车辆，集装箱的自动识别，对位技术的提升，无人集卡在我国港口，码头应用越来越普及，极大的减少人力成本，加快工作效率，提升工作准确率。目前常用的车辆定位技术是基于车上的GPS（全球定位系统）装置。然而，由于岸桥特别是连续岸桥大船和桥吊的遮挡，信号强度不够，GPS装置无法实现定位功能，因此，基于传统的GPS装置无法在该环境下对车辆进行有效定位。而且，岸桥上的车道线有别于普通公路上的车道线，非常混乱，基于正常道路上的车道保持在港口也无法实现稳定运行。现有的技术方案，基于GPS定位数据前往岸桥区域，并进行持续空间扫描，建立岸桥区域中货轮的点云地图数据，实时检测GPS定位信号，并将点云地图数据中的点云对象与GPS定位数据进行匹配，当GPS定位信号不满足预设信号强度阈值，则生成新路径进行装卸作业。但现有技术方案只能在规划控制中进行导航切换，增加了规划模块算法设计难度。另外，在港口运行中，平台下发岸桥和堆场站点位置，车辆通过站点位置，依靠导航路径导航到目的地，岸桥位置一般通过安装在港机设备上的RTK设备获得，堆场一般通过打点确认贝位号坐标获得，通过上述方案，无法准确下发岸桥站点坐标，极大影响作业效率。而且，港口作业多为无人集卡编队作业，单台车建立的局部点云无法共享其它车端，影响作业效率。

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

[0017] 实施例一：本发明提供一种基于港口环境的无缝定位的系统及方法，如图1所示，所述系统包括云平台、传感器组件、智能驾驶装置。如图3所示，所述方法步骤如下。

[0018] 步骤1，云平台下发采图指令至智能驾驶装置；步骤2，智能驾驶装置接收到采图指令后，控制传感器组件获取传感器数据；
步骤3，智能驾驶装置根据传感器数据和图优化理论，得到点云地图；
步骤4，对点云地图进行匹配定位，采用点云配准算法得到车辆位姿，从而实现无缝定位。

[0019] 本发明提供一种基于港口环境的无缝定位系统及方法，该技术方案通过云平台下发采图指令，使得智能驾驶装置控制传感器组件收集传感器数据，根据数据进行实时建图，全局图优化，匹配定位，解决现有技术算法设计难度大，无法准确下发安桥站点坐标，无法共享其它车辆，进而影响工作效率等技术问题。

[0020] 实施例二：本发明提供一种基于港口环境的无缝定位的系统及方法，如图1所示，所述系统包括云平台、传感器组件、智能驾驶装置。如图2所示，所述方法步骤如下。

[0021] 步骤1，云平台下发采图指令至智能驾驶装置；步骤2，智能驾驶装置接收到采图指令后，控制传感器组件获取传感器数据；
步骤3，智能驾驶装置根据传感器数据和图优化理论，得到点云地图；
如图4所示，本发明提供的基于港口环境的无缝定位的系统主要用于完成这三个任务：采图任务、地图下发任务、作业任务。所述步骤1、2、3为用于执行采图任务的步骤，其步骤3的具体实施过程如下。

[0022] 步骤31，利用激光雷达传感器同步局部建图，在利用惯性导航估算车辆位姿；步骤32，将局部建图与组合导航位姿进行联合优化建图；
步骤33，通过激光雷达估算车辆位姿，并与惯性导航位姿进行整合，构建位姿；
步骤34，根据构建的子地图和位姿的约束，采用图优化对雷达相关帧进行优化，得到点云地图。

[0023] 执行采图任务时，车端（即车辆上的智能驾驶装置，AD端）收到采图命令后，开启建图任务，如图7所示，利用激光slam（Simultaneous Localization and Mapping，即时定位与地图构建）局部建图与组合导航位姿进行联合优化建图。如图5所示，通过激光雷达位姿估计结果直接与INS（惯性导航定位）位姿进行整合，构建位姿，基于经典位姿图优化理论优化位姿图，继而输出最终位姿，图优化是求解大规模定位建图问题的有效方法，当不考虑已收敛观测点而只优化位姿构成图时，称之为位姿图优化算法。所述采用图优化的具体方法为：通过目标函数 ，以n条边求和的形式完成优化，其中，xk和zk为优化变量，Ω为信息矩阵，e函数为误差。由前文可知，系统因子图构建包含了IMU（惯性传感器，又称惯性测量单元）测量约束、子地图与雷达帧的位姿约束以及全局位姿测量的约束，采用G2O图优化库对雷达相关帧进行优化，进而得到点云地图。由于图优化为现有技术，故此处不再就细节进行展开说明。

[0024] 步骤4，对点云地图进行匹配定位，采用点云配准算法得到车辆位姿，从而实现无缝定位；步骤5，智能驾驶装置上传点云地图至云平台；
步骤6，云平台将点云地图下发至其它车辆；
步骤7，点云地图下发完成后，其它车辆完成点云地图更新。

[0025] 本发明提供一种基于港口环境的无缝定位系统及方法，该技术方案通过云平台下发采图指令，使得智能驾驶装置控制传感器组件收集传感器数据，根据数据进行实时建图，全局图优化，匹配定位，解决现有技术算法设计难度大，无法准确下发安桥站点坐标，无法共享其它车辆，进而影响工作效率等技术问题。

[0026] 实施例三：本发明提供一种基于港口环境的无缝定位的系统及方法，如图1所示，所述系统包括云平台、传感器组件、智能驾驶装置。如图2所示，所述方法步骤如下。

[0027] 步骤1，云平台下发采图指令至智能驾驶装置；步骤2，智能驾驶装置接收到采图指令后，控制传感器组件获取传感器数据；
步骤3，智能驾驶装置根据传感器数据和图优化理论，得到点云地图；
步骤4，对点云地图进行匹配定位，采用点云配准算法得到车辆位姿，从而实现无缝定位；
由前文可知，本发明提供的系统主要用于完成三项任务，而步骤4为执行作业任务的步骤，其具体实施过程如下。

[0028] 步骤40，云平台发送任务作业至车辆，车辆在接收到任务作业后，行驶至作业区域；步骤41，对点云地图进行匹配定位，判断车辆是否位于作业区域，若是转步骤42，若否转步骤43；
步骤42，采用点云配准算法得到车辆位姿；
如图6所示，点云匹配定位，采用ICP匹配算法，即点云配准算法得到车辆位姿，具体步骤如下。

[0029] 步骤421，确定目标点云和源点云；步骤422，根据函数 ，确定对应点云，构
建初始点对集合，其中pi为源点云，qj为目标点云；
步骤423，根据对应点和目标函数 ，计算旋转矩阵R和平移
矩阵T；
步骤424，根据矩阵R和向量T，得到变化后的点云坐标，计算当前点云与目标点云距离误差是否小于预设阈值；
步骤425，若小于，则匹配完成，若小于，则匹配完成。

[0030] 由于ICP匹配算法为现有技术，故此处不再赘述。

[0031] 步骤43，在车辆移动后，转步骤41。

[0032] 在连续匹配定位后，连续岸桥匹配轨迹如图8所示。另外，由前文可知，本发明提供的基于港口环境的无缝定位的系统主要用于完成这三个任务：采图任务、地图下发任务、作业任务。所述步骤5、6、7为用于执行地图下发任务的步骤。

[0033] 步骤5，智能驾驶装置上传点云地图至云平台；步骤6，云平台将点云地图下发至其它车辆；
步骤7，点云地图下发完成后，其它车辆完成点云地图更新。

[0034] 本发明提供一种基于港口环境的无缝定位系统及方法，该技术方案通过云平台下发采图指令，使得智能驾驶装置控制传感器组件收集传感器数据，根据数据进行实时建图，全局图优化，匹配定位，解决现有技术算法设计难度大，无法准确下发安桥站点坐标，无法共享其它车辆，进而影响工作效率等技术问题。

[0035] 综上所述，本发明实施例提供一种基于港口环境的无缝定位系统及方法，该技术方案无需进行局部坐标和全局坐标的切换，降低了规划控制模块算法设计的难度，而且该方案通过点云地图进行匹配定位，得到车辆位姿来实现无缝定位，岸桥站点坐标，极大的提高了作业效率，同时该方案还可以实现多车共享点云地图，可进行编队作业。解决现有技术算法设计难度大，无法准确下发安桥站点坐标，无法共享其它车辆，进而影响工作效率等技术问题。

[0036] 以上所述仅为本发明的较佳实施例而已，并不用以限制本发明，凡在本发明的精神和原则之内，所做的任何修改、等同替换、改进等，均应包含在本发明保护的范围之内。