一种2D激光雷达SLAM地图动态加载方法及系统与流程

本发明涉及自动驾驶技术领域，特别是关于一种2d激光雷达slam(英文全称为：simultaneouslocalizationandmapping，中文全称为：即时定位与地图构建)地图动态加载方法及系统。

背景技术：

目前，已经有很多场景实现了自动驾驶和移动机器人的应用，例如：餐厅/酒店无人送餐车、无人巡逻车等。通常，在无gps或gps信号较差的环境下，尤其是室内，定位与导航主要通过slam技术实现。但是，当前主要应用的slam技术，在得到地图进行定位的时候，存在一个开销过大的问题，这个问题在地图面积大的场景下尤为突出，计算资源消耗过度，导致整个系统运行不流畅。

技术实现要素：

本发明的目的在于提供一种2d激光雷达slam大型地图动态加载方法及系统，其对2d激光雷达slam地图进行划分与管理，实现小规模动态加载，从而能够解决大型场景中slam地图过大导致的系统资源消耗大的问题。

为实现上述目的，本发明提供一种2d激光雷达slam地图动态加载方法，该方法包括：

步骤1，根据2d激光雷达slam地图存储的地图图片上所包含的坐标系信息、比例尺分辨率与选定角点像素信息，得到全局坐标x-y与图像坐标u-v中任意点的转换矩阵m；

步骤2，将所述地图图片按照n行m列划分为若干子地图gi，每个子地图gi的图像宽度和高度分别为wi、hi；并利用转换矩阵m，子地图gi中点j的像素坐标对应的全局坐标由式(4)获得：

其中，col(gi)表示子地图gi在划分好的所述地图图片所对应的列的值，row(gi)表示子地图gi在划分好的所述地图图片所对应行的值，wk表示第k个子地图gk的图像宽度，k小于col(gi)，hl表示第l个子地图gl的图像高度，l小于row(gi)；

步骤3，根据划分好的子地图，动态加载子地图。

进一步地，所述步骤3具体包括：

步骤31，通过图结构描述步骤2划分好的各所述子地图之间的相对位置和连接关系，所述图结构包括节点和边，其中，一个所述节点对应一个所述子地图，记为子地图节点，一条所述边对应两个所述子地图之间的存在连通道路；

步骤32，通过邻接矩阵方式描述所述图结构，所述邻接矩阵中的(n×m)行和(n×m)列的每一行和每一列对应设置为一个所述子地图节点的通行状态向量，每一所述通行状态向量包括预设数量的元素，每一所述元素通过两种可区分的符号，来表征其所在所述通行状态向量对应的行和列的两个所述子地图节点之间是否存在所述边；

步骤33，依据当前车辆行驶方向，搜索车辆当前处的第一子地图节点所在所述邻接矩阵的行或列中是否存在满足当前行驶方向的元素及该元素对应的子地图节点，如果存在，则进行预加载；如果不存在，则不加载。

进一步地，所述两种可区分的符号为0和1，其中，0表示该元素所在所述通行状态向量对应的行和列的两个所述子地图节点之间不存在所述边，1表示该元素所在所述通行状态向量对应的行和列的两个所述子地图节点之间存在所述边。

进一步地，所述步骤3具体还包括：

步骤34，当由于行驶方向的变化导致可能的后继子地图节点变化时，将释放掉之前的预加载地图节点，再加载此时可能的后继子地图节点，并返回步骤33。

进一步地，所述步骤3具体还包括：

步骤35，当车辆由所述第一子地图节点进入到所述后继子地图节点后，切换并加载所述后继子地图节点为当前的所述子地图节点，并释放掉所述第一子地图节点。

进一步地，所述步骤3具体还包括：

步骤36，在步骤33同时预加载多个子地图节点的情形下，选取车辆的当前行驶朝向θ与预加载多个子地图节点所对应的预设联通方向夹角最小的子地图节点，作为后续可能行驶的子地图节点。

本发明还提供一种2d激光雷达slam地图动态加载系统，该系统包括：

转换矩阵获取装置，其用于根据2d激光雷达slam地图存储的地图图片上所包含的坐标系信息、比例尺分辨率与选定角点像素信息，得到全局坐标x-y与图像坐标u-v中任意点的转换矩阵m；

地图划分装置，其用于将地图图片按照n行m列划分为若干子地图gi，每个子地图gi尺寸为(wi，hi)，其中，wi表示第i个子地图gi的图像宽度，hi表示第i个子地图gi的图像高度，并利用转换矩阵m，对于任意子地图gi中任意一点j的像素坐标对应的全局坐标计算式描述为式(4)：

其中，col(gi)表示子地图gi在划分好的所述地图图片所对应的列的值，row(gi)表示子地图gi在划分好的所述地图图片所对应行的值，wk表示第k个子地图gk的图像宽度，k小于col(gi)，hl表示第l个子地图gl的图像高度，l小于row(gi)；

动态加载子地图装置，其用于根据划分好的子地图，动态加载子地图。

进一步地，所述动态加载子地图装置具体包括：

图结构描述单元，其用于通过图结构描述划分好的各所述子地图之间的相对位置和连接关系，所述图结构包括节点和边，其中，一个所述节点对应一个所述子地图，记为子地图节点，一条所述边对应两个所述子地图之间的存在连通道路；

邻接矩阵描述单元，其用于通过邻接矩阵方式描述所述图结构，所述邻接矩阵中的(n×m)行和(n×m)列的每一行和每一列对应设置为一个所述子地图节点的通行状态向量，每一所述通行状态向量包括预设数量的元素，每一所述元素通过两种可区分的符号，来表征其所在所述通行状态向量对应的行和列的两个所述子地图节点之间是否存在所述边；

子地图加载单元，其用于依据当前车辆行驶方向，搜索车辆当前处的第一子地图节点所在所述邻接矩阵的行或列中是否存在满足当前行驶方向的元素及该元素对应的子地图节点，如果存在，则进行预加载；如果不存在，则不加载；当由于行驶方向的变化导致可能的后继子地图节点变化时，将释放掉之前的预加载地图节点，再加载此时可能的后继子地图节点；当车辆由所述第一子地图节点进入到所述后继子地图节点后，切换并加载所述后继子地图节点为当前的所述子地图节点，并释放掉所述第一子地图节点；同时预加载多个子地图节点的情形下，选取车辆的当前行驶朝向θ与预加载多个子地图节点所对应的预设联通方向夹角最小的子地图节点，作为后续可能行驶的子地图节点。

进一步地，所述两种可区分的符号为0和1，其中，0表示该元素所在所述通行状态向量对应的行和列的两个所述子地图节点之间不存在所述边，1表示该元素所在所述通行状态向量对应的行和列的两个所述子地图节点之间存在所述边。

本发明由于采取以上技术方案，其具有以下优点：

1.降低了系统资源，降低了计算开销，提高了定位导航的实时。

2.扩大了基于2d激光雷达slam技术的自动驾驶车、移动机器人等设备的应用范围，不再受到场景大小的约束。

3.使得大场景下，不再需要高性能的设备，降低了类似应用场景的设备的成本。

附图说明

图1为本发明一实施例中的2d激光雷达slam地图的原图示意图。

图2为图1中原图被划分后的子地图示意图。

图3为图2抽象为图结构的示意图。

图4为图3用0-1编码表示为邻接矩阵方式的示意图。

图5为本发明实施例中的搜索邻接矩阵中是否存在满足当前行驶方向的后继子地图节点的示意图。

图6为本发明灵一实施例中的2d激光雷达slam地图的原图被划分后的子地图示意图。

图7为斜向联通的示意图。

图8为斜向联通时预加载子地图的示意图。

具体实施方式

下面结合附图和实施例对本发明进行详细的描述。

本发明实施例结合图1提供的示例，对2d激光雷达slam地图的结构进行描述，与该示例同类型方式组织的地图，均包含在本发明方法的保护范围之内。

如图1所示，2d激光雷达slam地图通常通过一张w×h分辨率图片进行存储，其中，w表示2d激光雷达slam地图的宽度，h表示2d激光雷达slam地图的高度。

下文将“2d激光雷达slam地图”简称为“地图图片”。为了描述方便，以地图图片本身的“上、下、左、右”对下面的方向进行描述，也可以理解为是在地图图片的平面内平行于子地图之间的划分线的方向。

地图图片上包含有各种障碍物的概率信息、坐标系信息、比例尺分辨率和选定点的像素信息。其中：

本实施例中的各种障碍物的概率信息采用深浅不同的颜色进行表示，比如：图1中，黑色区域表示不可通行区域(障碍物、墙壁等)，白色区域为可通行区域，灰色区域为无法明确为不可通行区域和可通行区域的未知区域。图1仅仅是一个示例，本领域技术人员可以根据实际需求，用其它颜色或标记符号来表示各种障碍物的概率信息。

本实施例中的坐标系信息包括由u-v坐标系表示的图像坐标系及由x-坐标系表示的全局坐标系。

本实施例中的比例尺分辨率采用参数r表示，单位例如可以是：“米/每像素”。

本实施例中的选定点的像素信息可以记为(x0，y0，yaw)，x0、y0分别为选定点在x-y坐标系中的横坐标、纵坐标，yaw表示选定点在全局坐标系x-中相对于图像坐标系u-v的朝向。为了方便应用，选定点通常选择地图图片的最左下角角点的像素点，但不限于此。

通过比例尺分辨率r与选定点的像素信息(x0，y0，yaw)，有如下方程(1)和方程(2)：

x0＝(cos(yaw)ru0+sin(yaw)rv0+px)(1)

y0＝(-sin(yaw)ru0+cos(yaw)rv0+py)(2)

其中，(u0，v0)表示选定点的像素坐标。本实施例中视(u0，v0)为图像坐标系u-v的原点，(x0，y0，yaw)中的三个参数分别表示选定点在全局坐标系中的坐标，以及坐标轴的朝向，r为比例尺分辨率。(px，py)为全局坐标系x-和图像坐标系u-v之间的平移量，其可通过方程(1)和方程(2)解出，进而可以得到全局坐标x-与图像坐标u-v中任意点的转换矩阵m，其表示为式(3)：

步骤2，将地图图片按照n行m列划分为若干子地图gi，每个子地图gi尺寸为(wi，hi)，其中，wi表示第i个子地图gi的图像宽度，hi表示第i个子地图gi的图像高度。

由于划分并不改变地图的比例尺分辨率，因此比例尺分辨率r不变。因此，步骤2还利用转换矩阵m，对于任意子地图gi中任意一点j的像素坐标对应的全局坐标计算式描述为式(4)：

其中，col(gi)表示子地图gi所在的划分列的值，row(gi)表示子地图gi所在的划分行的值，k表示第k个小于col(gi)的子地图，表示第l个小于row(gi)的子地图，wk表示第k个子地图gk的图像宽度，hl表示第l个子地图gl的图像高度。

在一个实施例中，如图2所示，图2显示的是采用均分地图图片的方式，将地图图片按照n行m列划分为若干子地图gi，也就是说，每个子地图gi的宽度wi和高度hi相同。此时式(4)中的表示为式(5)，表示为式(6)：

式中，wi表示第i个子地图gi的图像宽度，hi表示第i个子地图gi的图像高度。上述实施例提供的方法，能够完成对于一个大型2d激光雷达slam地图划分的过程，同时保障了划分好的每一个子地图与原地图坐标关系的转换。

在一个实施例中，还可以根据实际情况，采用非均分的形式对地图图片进行划分，如图6所示，此时划分出的子地图的宽度和高度不相同，原则只需要尽可能满足每一子地图中的信息均衡，同时满足每一个子地图中的道路完整性。一般情形下，面积大的场景可以划分为更多的子地图，这样可以保障运行效率。

步骤3，根据划分好的子地图，动态加载子地图。

作为步骤3的一种实现方式，其具体包括：

步骤31，通过图结构描述步骤2划分好的各所述子地图之间的相对位置和连接关系，所述图结构包括节点和边，其中，一个所述节点对应一个所述子地图，记为子地图节点，一条所述边对应两个所述子地图之间的存在连通道路。

也就是说，两个子地图节点之间的边对应于二节点的联通情况和联通方向。其中，联通情况分为有边和无边两种情形。有边，则表示两个子地图节点之间的道路是连通的；无边，则表示两个子地图节点之间无道路，无法连通，如图2所示，g4并不能通往g5。

预设联通方向一般根据实际场景情况选取，在图2所示实施例中，由于道路相对方正，可以取左联通、右联通、上联通、下联通即可。在图6所示实施例中，由于存在一定程度的斜向行驶，可以预设更多联通可能，例如：左联通、右联通、上联通、下联通、左上联通、左下联通、右上联通和右下联通。“斜向”意味着在子地图平面内且不平行于子地图的划分线的方向，可以参考图7中示出的斜向箭头。更为复杂的道路结构，可以进一步划分更细的预设联通方向。

步骤32，通过邻接矩阵方式描述所述图结构，所述邻接矩阵中的(n×m)行和(n×m)列的每一行和每一列对应设置为一个所述子地图节点的通行状态向量，每一所述通行状态向量包括预设数量的元素，每一所述元素通过两种可区分的符号，来表征其所在所述通行状态向量对应的行和列的两个所述子地图节点之间是否存在所述边。

步骤33，依据当前车辆行驶方向，判断车辆可能的下一个行驶状态，并搜索车辆当前所处的第一子地图节点所在所述邻接矩阵的行或列中是否存在满足当前行驶方向的元素及该元素对应的子地图节点，如果存在，则进行预加载；如果不存在，则不加载。

在一个实施例中，所述两种可区分的符号为0和1，其中，0表示该元素所在所述通行状态向量对应的行和列的两个所述子地图节点之间不存在所述边，1表示该元素所在所述通行状态向量对应的行和列的两个所述子地图节点之间存在所述边。以图2所示实施例，其邻接矩阵如图4所示。

如图5中的(a)所示的一种情形：当前车辆在g5中，此时的g5为第一子地图节点。车辆欲向上行驶，结合图4，搜索邻接矩阵中g5所在行，是否存在向上联通的边，搜索结果是：g5-g8为(0，0，1，0)，此时，可将g8作为预加载子地图节点。

如图5中的(c)示出的又一种情形：前车辆在g5中，此时的g5为第一子地图节点。车辆欲向左行驶，结合图4，搜索邻接矩阵中g5所在行，是否存在向左联通的边，搜索结果是：不存在左联通的边，因此不会进行地图预加载。

在一个实施例中，步骤3还包括：

步骤34，当由于行驶方向的变化导致可能的后继子地图节点变化时，将释放掉之前的预加载地图节点，再加载此时可能的后继子地图节点，并返回步骤33。

如图5中的(b)示出的是基于图5中的(a)所示情形外的另一种情形：在预加载子地图节点g8后，车辆欲调头行驶，即向下行驶，此时g5-g8的边不满足当前行驶方向，释放预加载子地图节点g8，结合图4，重新搜索邻接矩阵中g5所在行，是否存在向下联通的边，搜索结果是：g5-g2边等于(0，0，0，1)，此时，可将g2作为后继子地图节点。

在一个实施例中，步骤3还包括：

步骤35，当车辆由所述第一子地图节点进入到所述后继子地图节点后，切换并加载所述后继子地图节点为当前的所述子地图节点，并释放掉所述第一子地图节点。

结合图5中的(b)和图2示出的情形，当车辆由所述第一子地图节点g5进入到所述后继子地图节点g2后，切换并加载后继子地图节点g2为当前的所述子地图节点，并释放掉第一子地图节点g5，以节省系统资源。

自动驾驶车在某一子地图节点中行驶，根据行驶方向，可以动态加载后续可能行驶的子地图，而不必整个地图都加载，这样为减少系统计算开销提供条件。

在一个实施例中，所述步骤3具体还包括：

步骤36，在步骤33同时预加载多个子地图节点的情形下，选取车辆的当前行驶朝向θ与预加载多个子地图节点所对应的预设联通方向夹角最小的子地图节点，作为后续可能行驶的子地图节点。

例如图8，车辆当前在子地图ga中，按照当前箭头方向行驶，箭头方向与左下方向夹角最小，符合当前的行驶状态，因此，在定义的邻接矩阵中搜索是否存在左联通、下联通和左下联通三个位置的联通子地图，并将此三个子地图同时进行预加载。即在图8中，gb、gc、gd同时是预加载的子地图节点。因此，需要在gb、gc、gd中选取车辆的当前行驶朝向θ与预加载多个子地图节点所对应的预设联通方向夹角最小的子地图节点，作为后续可能行驶的子地图节点。

最后需要指出的是：以上实施例仅用以说明本发明的技术方案，而非对其限制。本领域的普通技术人员应当理解：可以对前述各实施例所记载的技术方案进行修改，或者对其中部分技术特征进行等同替换；这些修改或者替换，并不使相应技术方案的本质脱离本发明各实施例技术方案的精神和范围。