一种基于码头石墩的建图方法、装置、设备以及介质与流程

1.本发明涉及自动驾驶技术领域，尤其涉及一种基于码头石墩的建图方法、装置、设备以及介质。


背景技术：

2.随着无人驾驶技术的发展，自动化的码头运输逐渐变得可能。在运输过程中，对于车辆的定位技术提出了更高的要求。传统方案是基于gps(全球卫星定位系统)的方案，直接获取定位信息。此外也有基于已有地图的定位方案，通常利用视觉和激光传感器对场地进行预先建图，然后在作业时利用已有地图和实时观测进行位置估计。
3.基于gps的方案需要与卫星保持良好的通信，在某些建筑物遮挡的情况下，其定位结果会显著受到影响。此外，目前的民用gps的定位精度普遍在米级别，无法满足高精度的定位需要。基于已有地图的定位方案中，由于激光传感器的精度和稳定性优于视觉传感器，所以是目前的主流方案。但也存在着计算复杂、实时性低的情况。尤其是多线激光雷达，因为数据量大，所以计算耗时更高。


技术实现要素：

4.本发明的目的在于提供一种基于码头石墩的建图方法、装置、设备以及介质，可以实时高效地进对码头石墩进行建图，以便于后续的导航控制。
5.为实现上述目的，本发明提供如下技术方案：
6.一种基于码头石墩的建图方法，其特征在于，步骤1-初始化石墩地图m和因子图g；步骤2-推算当前车辆的位姿；步骤3-获取激光雷达观测数据，并提取石墩数据；步骤4-对各个石墩数据对应到石墩地图m，建立数据关联；步骤5-根据车辆位姿的推算值和各个石墩的位置值，在因子图g中添加相关的因子；步骤6-对因子图g进行非线性优化求解，得到各个石墩的位置，以此更新石墩地图m；步骤7-待有新的激光雷达观测数据后，重复执行3-6步骤；若无，则结束流程；步骤8-流程结束后，最终的石墩地图m即为所求结果。
7.本发明进一步设置，初始状态下，石墩地图m为空，因子图g为空。
8.本发明进一步设置，推算当前车辆的位姿时，根据gps和车辆的imu数据推算，并在t时刻，推算出车辆在全局地图下的位姿为r
t
。
9.本发明进一步设置，各个石墩的位置为s＝{s1，s2，...，sk}，其中石墩的位置通过车辆坐标系确定。
10.本发明进一步设置，对于每个石墩si，如果在石墩地图m中找到距离它在h米之内的石墩mj，则称si和mj发生了数据关联。如果没有找到，则把si插入到石墩地图m中，设新插入的石墩为mu，则称si与mu产生了数据关联。
11.本发明进一步设置，在因子图g中添加相关的因子时，因子图插入先验因子x＝(x
t
，r
t
)，以及与石墩相关联的观测因子集合：l＝{(x
t
，l
u(i)
，si)|i＝1，2，3，...，k}，其中u(i)表示m中与si发生数据关联的石墩编号。
12.本发明还提供了一种基于码头石墩的建图装置，包括：
13.建图模块，初始化石墩地图m、因子图g，更新石墩地图m，并形成最终的石墩地图；
14.推算模块，根据gps和车辆的imu数据，推算当前车辆的位姿，推算出车辆在全局地图下的位姿为r
t
；
15.激光雷达观测模块，用于获取观测数据；
16.数据提取模块，获取激光雷达观测模块中的观测数据，并提取石墩数据，将各个石墩的位置记为s＝{s1,s2,...,sk}；
17.数据关联模块，用于将各个石墩数据对应到石墩地图m，建立数据关联；因子添加模块，车辆位姿的推算值和各个石墩的位置值，在因子图g中添加相关的因子；
18.数据优化模块；对因子图g进行非线性优化求解，得到各个石墩的位置，以此更新石墩地图m。
19.本发明还提供了一种电子设备，其特征在于，包括：处理器，以及与所述处理器通信连接的存储器；所述存储器存储计算机执行指令；所述处理器执行所述存储器存储的计算机执行指令，以实现建图方法。
20.本发明还提供了一种计算机可读存储介质，其特征在于，所述计算机可读存储介质中存储有计算机执行指令，当所述指令被执行时，使得计算机所述的建图方法。
21.本发明的有益效果：本算法使用了因子图来进行优化求解，其中的因子分为两类，一类是对车辆自身的位姿估计作为对车辆的先验位姿因子，一类是对石墩的观测和提取得到的数据作为对石墩的观测位置因子，然后对因子图进行优化求解。
附图说明
22.此处所说明的附图用来提供对本技术的进一步理解，构成本技术的一部分，本技术的示意性实施例及其说明用于解释本技术，并不构成对本技术的不当限定。在附图中：
23.图1为本发明实施例的流程图。
24.图2为本发明因子图结构示例。
具体实施方式
25.以下将配合附图及实施例来详细说明本技术的实施方式，借此对本技术如何应用技术手段来解决技术问题并达成技术功效的实现过程能充分理解并据以实施。
26.用深度学习在每一帧激光观测中识别出各个石墩的位置，然后整合所有激光帧的石墩位置数据，建立整个码头的石墩地图。
27.在整个过程中维护一个石墩地图m，它记录了每个石墩在全局地图下的位置。此外，我们还维护了一个因子图g，具体是一个gtsam库的nonlinearfactorgraph(非线性因子图)实例，用以进行求解。整体步骤如下，流程图可以参看图1：
28.1.初始时刻，石墩地图m为空，因子图g为空。
29.2.根据gps和车辆的imu等数据，推算当前车辆的位姿。我们记为：在t时刻，记推算出车辆在全局地图下的位姿为r
t
。
30.3.获取激光雷达的观测数据，并从雷达的观测数据中提取各个石墩数据。我们记为：各个石墩的位置为s＝{s1，s2，...，sk}，其中石墩的位置是相对于车辆坐标系来说的。
31.4.对各个石墩数据对应到石墩地图m，建立数据关联。具体为：对每个石墩si，如果在石墩地图m中找到距离它在h米之内的石墩mj，则称si和mj发生了数据关联。如果没有找到，则把si插入到石墩地图m中，设新插入的石墩为mu，则称si与mu产生了数据关联。
32.5.根据步骤2中车辆位姿的推测值和步骤3中各个石墩的位置值，在因子图g中添加相关的因子。具体为：我们对因子图插入先验因子x＝(x
t
，r
t
)，以及与石墩相关联的观测因子集合：l＝{(x
t
，l
u(i)
，si)|i＝1，2，3，...，k}，其中u(i)表示m中与si发生数据关联的石墩编号。
33.6.对因子图g进行非线性优化求解，得到各个石墩的位置，以此更新石墩地图m。
34.7.待有新的激光观测数据后，重复执行3-6步骤.否则，结束流程。
35.8.流程结束后，最终的石墩地图m即为所求结果。
36.本发明还提供了一种基于码头石墩的建图装置，包括：
37.建图模块，初始化石墩地图m、因子图g，更新石墩地图m，并形成最终的石墩地图；
38.推算模块，根据gps和车辆的imu数据，推算当前车辆的位姿，推算出车辆在全局地图下的位姿为r
t
；
39.激光雷达观测模块，用于获取观测数据；
40.数据提取模块，获取激光雷达观测模块中的观测数据，并提取石墩数据，将各个石墩的位置记为s＝{s1，s2，...，sk}；
41.数据关联模块，用于将各个石墩数据对应到石墩地图m，建立数据关联；因子添加模块，车辆位姿的推算值和各个石墩的位置值，在因子图g中添加相关的因子；
42.数据优化模块；对因子图g进行非线性优化求解，得到各个石墩的位置，以此更新石墩地图m。
43.本发明还提供了一种电子设备，包括：处理器，以及与所述处理器通信连接的存储器；所述存储器存储计算机执行指令；所述处理器执行所述存储器存储的计算机执行指令，以实现建图方法。
44.本发明还提供了一种计算机可读存储介质，所述计算机可读存储介质中存储有计算机执行指令，当所述指令被执行时，使得计算机所述的建图方法。
45.本发明使用了因子图来进行优化求解，其中的因子分为两类，一类是对车辆自身的位姿估计作为对车辆的先验位姿因子，一类是对石墩的观测和提取得到的数据作为对石墩的观测位置因子。然后对因子图进行优化求解，因子图目前的常用库为gtsam，也可以利用某些优化库例如ceres进行自主实现；做数据管理的时候以位置作为判断标准，也可以用其他方案例如三角匹配之类的。识别石墩不限于某种深度学习算法。
46.如在说明书及权利要求当中使用了某些词汇来指称特定组件。本领域技术人员应可理解，硬件制造商可能会用不同名词来称呼同一个组件。本说明书及权利要求并不以名称的差异来作为区分组件的方式，而是以组件在功能上的差异来作为区分的准则。如在通篇说明书及权利要求当中所提及的“包含”为一开放式用语，故应解释成“包含但不限定于”。“大致”是指在可接收的误差范围内，本领域技术人员能够在一定误差范围内解决所述技术问题，基本达到所述技术效果。
47.需要说明的是，术语“包括”、“包含”或者其任何其他变体意在涵盖非排他性的包含，从而使得包括一系列要素的商品或者系统不仅包括那些要素，而且还包括没有明确列
出的其他要素，或者是还包括为这种商品或者系统所固有的要素。在没有更多限制的情况下，由语句“包括一个
……”
限定的要素，并不排除在包括所述要素的商品或者系统中还存在另外的相同要素。
48.上述说明示出并描述了本发明的若干优选实施例，但如前所述，应当理解本发明并非局限于本文所披露的形式，不应看作是对其他实施例的排除，而可用于各种其他组合、修改和环境，并能够在本文所述发明构想范围内，通过上述教导或相关领域的技术或知识进行改动。而本领域人员所进行的改动和变化不脱离本发明的精神和范围，则都应在本发明所附权利要求的保护范围内。