CN115979248A - 基于定位位姿为约束的地图更新方法及系统、存储介质 - Google Patents

Abstract

本发明提供的该基于定位位姿为约束的地图更新方法及系统、存储介质，其方法步骤包括：导航地图中将半静态物体区域设为地图待更新区域，令移动机器人到达后启动建图算法；采集导航地图中的定位位姿A及局部地图中的定位位姿B，做时间戳对齐；将定位位姿A作为局部地图的建图原点，将局部地图与导航地图进行时间粗对齐；采集数组定位位姿A和定位位姿B，将局部地图和导航地图做空间粗对齐；将局部地图中的定位位姿B作为顶点，相邻两个定位位姿B之间的位姿变换关系作为双边，以时间戳对齐后的定位位姿A作为单边，构建图模型进行图优化，获取局部建图定位位姿构建待更新区域栅格地图。籍此减少地图更新计算资源，消除建图过程累计误差。

Description

基于定位位姿为约束的地图更新方法及系统、存储介质

技术领域

本发明涉及移动机器人的地图更新技术，尤其涉及在预设区域内以定位位姿为约束实施地图自动更新的方法及系统、存储介质。

背景技术

目前工厂以及仓储物流领域智能化程度越来越高，利用激光SLAM技术构建环境地图然后进行移动机器人导航的方式越来越普及，由于其实施成本低，部署简单快捷得到越来越多的关注。

但是在部分工厂环境中，由于环境中的物体经常发生移动变化，导致实际环境与事先扫描的激光轮廓地图有很大的偏差，因此在移动机器人实际运行过程中参考该激光地图时会产生偏差，导致移动机器人的运行路线以及位置会与预期不符。

为此现有技术曾提出了各种地图更新技术，例如《激光地图更新方法、机器人和集群机器人系统》（专利公开号：CN112629518A）中提出了一种地图更新方案，其主要采用的是将当前的激光点云根据当前的定位点映射到地图上，然后根据当前的扫描点和地图点之间的总体偏差确定是否更新地图。

然而此类方法一方面在使用采样率较高的激光雷达时，其计算量会比较大，通常会造成需要时时刻刻都在计算，因此在大地图场景中会持续占用计算资源。另一方面，在确认是否更新地图的步骤上，该现有技术通过对比扫描点和地图点之间的总体偏差来进行判断，这需要对当前扫描中的每个激光点和地图上的点进行计算比较，非常浪费时间和计算资源。

发明内容

为此，本发明的主要目的在于提供一种基于定位位姿为约束的地图更新方法及系统、存储介质，以减少地图更新时所占的计算资源，并消除建图过程中的累计误差，使得地图更新更加精准。

为了实现上述目的，根据本发明的第一个方面，提供了一种基于定位位姿为约束的地图更新方法，其步骤包括：

步骤S100 在栅格化的导航地图中，将半静态物体所处区域设置为地图待更新区域，以令移动机器人到达后，启动建图算法；

步骤S200 采集移动机器人在导航地图中的定位位姿A及局部地图中的定位位姿B，做时间戳对齐；

步骤S300 将定位位姿A作为局部地图的建图原点，将局部地图与导航地图进行时间粗对齐；

步骤S400 采集数组定位位姿A和定位位姿B做ICP，以求出对应的旋转平移转换矩阵，以供将局部地图和导航地图进行空间粗对齐；

步骤S500 将局部地图中的定位位姿B作为顶点，相邻的两个定位位姿B之间的位姿变换关系作为双边，以时间戳对齐后的定位位姿A作为单边，构建图模型进行图优化，获取优化后的局部建图定位位姿来构建待更新区域栅格地图。

在可能的优选实施方式中，步骤S200中还包括：步骤S210做时间戳对齐过程中，采用线性插值对定位位姿A及定位位姿B进行补偿。

在可能的优选实施方式中，步骤S400中所述空间粗对齐的步骤包括：

步骤S410 以预设行驶距离为间隔，记录数组建图定位点和导航定位点后，做ICP求出旋转R和平移t矩阵；

步骤S420 将局部地图的起始部分轨迹通过R和t的变换与导航地图做空间粗对齐。

在可能的优选实施方式中，步骤S500中采用局部地图中的关键帧的定位位姿B作为顶点，相邻的两个关键帧的定位位姿B之间的位姿变换关系作为双边，以时间戳对齐后的定位位姿A作为单边，构建图模型进行图优化。

在可能的优选实施方式中，步骤S500中所述关键帧为，当移动机器人移动超过0.5m或旋转超过20°中任一时，记录当前定位位姿作为局部地图中的关键帧的定位位姿B。

在可能的优选实施方式中，该基于定位位姿为约束的地图更新方法的步骤还包括：

步骤S600 将经过时间戳对齐的导航地图的定位位姿A点集合和局部地图的定位位姿B点的集合分别做均方根误差计算

，

当判断满足预设精度值时允许进行地图更新。

为了实现上述目的，对应上述方法本发明的第二个方面，还提供了一种基于定位位姿为约束的地图更新系统，其包括：

存储单元，用于存储包括如上任一所述基于定位位姿为约束的地图更新方法步骤的程序，以供导航单元，数据采集单元，建图单元，处理单元，适时调取执行；

导航单元，用于在导航地图中将半静态物体所处区域设置为地图待更新区域，以令移动机器人前往；

建图单元，用于在地图待更新区域启动建图算法；

数据采集单元，用于采集移动机器人在导航地图中的定位位姿A，局部地图中的定位位姿B；

处理单元，用于将定位位姿A与定位位姿B经线性插值补偿做时间戳对齐；及将定位位姿A作为局部地图的建图原点，将局部地图与导航地图进行时间粗对齐；根据获取的数组定位位姿A和定位位姿B做ICP，以求出对应的旋转平移转换矩阵，以供将局部地图和导航地图进行空间粗对齐；及将局部地图中的定位位姿B作为顶点，相邻的两个定位位姿B之间的位姿变换关系作为双边，以时间戳对齐后的定位位姿A作为单边，构建图模型进行图优化，获取局部建图定位位姿；

建图单元，还用于根据局部建图定位位姿来构建待更新区域栅格地图。

在可能的优选实施方式中，所述处理单元，还用于将导航地图的定位点集合和局部地图的建图定位点的集合分别做均方根误差计算

，

当判断满足预设精度值时允许建图单元进行地图更新。

为了实现上述目的，对应上述方法本发明的第三个方面，还提供了一种计算机可读存储介质，其上存储有计算机程序，其特征在于，所述计算机程序被处理器执行时实现上述方法中任一项所述基于定位位姿为约束的地图更新方法的步骤。

通过本发明提供的该一种基于定位位姿为约束的地图更新方法及系统、存储介质，巧妙的将地图场景分为三部分：静态地图、半静态地图和动态地图，通过分析使用场景，仅针对半静态地图区域进行处理划定特定区域，进一步减少移动机器人运动过程中不必要的计算量。

另一方面，在部分优选方案中，由于现有技术主要通过对比扫描点和地图点之间的总体偏差来判断是否更新地图，这需要对当前扫描中的每个激光点和地图上的点进行计算比较，非常浪费时间，而本发明通过记录数个局部地图轨迹点和导航定位点，只需要计算几十个甚至几个点即可完成对比计算，大大减少了计算资源。

此外，现有技术对当前机器人的定位点的使用上，也只是对当前点云做位置映射，将点云位姿转换到当前定位点上，因此对当前定位点的利用不充分。本发明则将当前定位点引入到局部地图的建图中，将当前定位点作为局部建图的图优化中的单边，给当前地图一个强约束，从而在大场景的更新中，可以消除建图过程中的累计误差，使得地图更加精准。

附图说明

构成本申请的一部分的附图用来提供对本发明的进一步理解，本发明的示意性实施例及其说明用于解释本发明，并不构成对本发明的不当限定。在附图中：

图1为本发明基于定位位姿为约束的地图更新方法的步骤示意图；

图2为本发明基于定位位姿为约束的地图更新方法的逻辑示意图；

图3为本发明基于定位位姿为约束的地图更新方法中的线性插值的原理示意图；

图4为本发明基于定位位姿为约束的地图更新方法中的图优化模型示意图；

图5为本发明基于定位位姿为约束的地图更新方法中的占据栅格地图建立过程示意图；

图6为本发明基于定位位姿为约束的地图更新系统结构示意图。

具体实施方式

为了使本领域的技术人员能够更好的理解本发明的技术方案，下面将结合实施例来对本发明的具体技术方案进行清楚、完整地描述，以助于本领域的技术人员进一步理解本发明。显然，本案所描述的实施例仅仅是本发明一部分的实施例，而不是全部的实施例。应当指出的是，对本领域的普通技术人员来说，在不脱离本发明构思及相互不冲突的前提下，本申请中的实施例及实施例中的特征可以相互组合。基于本发明中的实施例，在本领域普通技术人员没有做出创造性劳动前提下所获得的所有其他实施例，都应当属于本发明的揭露及保护范围。

此外本发明的说明书和权利要求书及附图中的术语“第一”、“第二”、“S100”、“S200”等是用于区别类似的对象，而不必用于描述特定的顺序或先后次序。应该理解这样使用的数据在适当情况下可以互换，以便这里描述的本发明的实施例能够以除了在这里描述的那些以外的顺序实施。同时本发明中的术语“包括”和“具有”以及他们的任何变形，意图在于覆盖不排他的包含。除非另有明确的规定和限定，术语“设置”、“布设”、“安装”、“相连”、“连接”应做广义理解，例如，可以是固定连接，也可以是可拆卸连接，或一体地连接；可以是机械连接，也可以是电连接；可以是直接相连，也可以通过中间媒介间接相连，可以是两个元件内部的连通。对于本领域的普通技术人员而言，可以根据具体情况，结合现有技术来理解上述术语在本案中的具体含义。

在本发明的示例中，对于工业场景，一般将环境分为三种，静态环境、半静态环境和动态环境，其中静态环境是指墙体柱子等一系列不会移动的物体，动态环境是指工人车辆等不停在移动的物体，半静态环境指货物这种位置会变化，但是不会一直变化的物体。

在工业场景中，当实现了自动化之后动态的物体就会减少，而静态环境本来就是定位的主要参照物，因此本专利重点解决半静态的物体所处区域的地图更新。其中由于移动机器人的定位主要依赖于静态物体，而半静态物体和静态物体都会干扰定位，考虑到在自动化的工厂中，动态物体存在较少，更多的是半静态的物体。目前行业内主要采用激光雷达传感器作为主要的定位传感器，而对于半静态物体的处理方案，通常会采用反光柱作为定位参照物，或者额外添加固定参照物，使得激光能够扫描到更多的静态物体，而前者需要对场景进行大量的反光柱安装，后者需要在场景中安装较多的固定物体作为静态参照物。

为此本发明意图采用自然轮廓建图和导航，从而不需要在场景中部署反光柱，且对于半静态的场景，不需要加装额外的静态参照物，而是依照预先设置的场景进行地图更新，籍此还能有效的降低AGV的cpu运算资源，只有在进入到区域内才调用局部建图算法。最终在占用较少的cpu运算资源的同时能够实时对地图进行更新，保证在半静态区域内的定位精度，只需要在导航地图中设置好自动更新地图区域，就不用再进行额外的操作即可实现。

为此，在优选实施方式中，如图1至图5所示，本发明提供了一种基于定位位姿为约束的地图更新方法，其步骤包括：

步骤S100 在栅格化的导航地图中，将半静态物体所处区域设置为地图待更新区域，以令移动机器人到达后，启动建图算法。

具体的来说在示例中，该步骤S100包括：

步骤S110 首先需要获取导航定位地图，因此需要对场景进行地图构建，本示例中所采用的建图方案为karto slam算法，地图构建完成之后，得到一张场景的2D栅格地图。

步骤S120 使用仙工智能的上位机软件RoboShop对步骤S110得到的栅格地图进行编辑，将半静态物体所处区域设置为地图待更新区域。

步骤S130 当前述工作完成后，便可令移动机器人（以下简称AGV）根据导航定位地图，行驶到地图中的待更新区域内处，此时再令AGV的建图单元启用建图算法，读取激光雷达数据和轮式里程数据，开始构建局部地图。

步骤S200 采集移动机器人在导航地图中的定位位姿A及局部地图中的定位位姿B，做时间戳对齐。

其中在优选实施方式中，还包括步骤S210对齐采用线性插值的方式对位姿进行补偿，最终得到相同时间戳下两条轨迹，且轨迹上的点一一对应，线性插值的原理如图3所示。

假设t-1时刻定位位姿的为，t时刻的定位位姿为，s时刻局部建图定位位姿为，满足，那么根据线性插值，可以得到s时刻定位的位姿

。

步骤S300 将定位位姿A作为局部地图的建图原点，将局部地图与导航地图进行时间粗对齐。

步骤S400 采集数组定位位姿A和定位位姿B做ICP，以求出对应的旋转平移转换矩阵，以供将局部地图和导航地图进行空间粗对齐。

其中在本示例中所述空间粗对齐的步骤包括：

步骤S410 以预设行驶距离为间隔，记录数组建图定位点和导航定位点后，做ICP求出旋转R和平移t矩阵；

步骤S420 将局部地图的起始部分轨迹通过R和t的变换与导航地图做空间粗对齐。

具体的，例如在建图的一段时间内，令AGV每行驶0.5m记录一个点，一共记录三组点，当记录完成之后，对这三组点做ICP求出这三组点的旋转（R）和平移（t）矩阵，然后将局部地图的起始部分轨迹通过R和t的变换与导航地图做空间粗对齐。

籍此通过上述步骤S300-S400的操作，使得局部地图和导航地图在空间上能够进行粗对齐，这样是为了给定位位姿作为单边提供一个先决条件。

步骤S500 将局部地图中的定位位姿B作为顶点，相邻的两个定位位姿B之间的位姿变换关系作为双边，以时间戳对齐后的定位位姿A作为单边，构建图模型进行图优化，获取局部建图定位位姿来构建待更新区域栅格地图。

具体来说，当AGV驶离地图更新区域后，AGV的建图单元关闭建图算法，然后对局部地图进行后端优化，以获取局部建图的定位位姿，其中本示例中，优选使用图优化作为后端优化方式，图优化是把优化问题表现为图（Graph）的一种方式，一个图由若干个顶点（Vertex）以及连接着这些节点的边（Edge）组成。其中用顶点表示待优化量（机器人的位姿），用边表示顶点的约束（两个节点之间的空间约束）。目的是为了通过构建图，通过最优化各节点的位姿，使得预测和观测的误差最小。

其中本示例下，在局部建图中，并不要求每个建图位姿都要进行计算，因此优选采用关键帧进行计算，例如当机器人平移超过0.5m或者旋转超过20度时，则选取一个关键帧。

由此将局部地图的每个时间戳对应的局部地图中的关键帧的定位位姿B作为顶点，局部地图中相邻的两个关键帧的定位位姿B之间的位姿变换关系作为双边，以时间戳对齐之后的导航地图中的定位位姿A作为单边，便可构建如图4所示的图模型，以供进行图优化。

具体来说，在建图过程中，系统会记录局部建图的关键帧的位姿（顶点）、和关键帧对齐时间戳之后的导航地图定位位姿（单边）、局部建图相邻关键帧之间的位姿变换关系（双边）以及关键帧对应的扫描点云信息。因此在优化的过程中，待优化量为局部地图中关键帧的位姿（顶点），约束为单边和双边。

而前述步骤S300-S400中，已经对局部地图初始位姿和导航定位位姿进行时间和空间的对齐，其目的在于优化的时候能够提供比较精确的顶点，从而可在优化的过程中也能减少计算的时间和算力资源。

待优化完成之后，得到优化完之后的顶点以及每个顶点所对应的激光扫描点，接下来需要进行地图构建，如图5所示，本示例中所使用的地图为二维占据栅格地图。首先根据优化之后的顶点构造原始栅格地图，将地图根据栅格的大小划分为一个个小栅格，原始状态每个栅格都是未知的，然后将顶点以此放入到地图中去，将顶点的扫描激光点的位置设置为占据（黑色），将激光光线传播的中间栅格设置为空闲（白色），这样将所有的顶点以及对应的激光扫描点依次放入栅格地图中，按照黑色和白色对栅格地图进行更新，直到所有的点位都放入地图中。（过程如图5所示）最终便可根据得到的局部建图定位位姿，来构建栅格地图，以替换地图待更新区域，完成地图的更新。

另一方面，为了提高整体地图更新精度，则需要在地图更新前，评判局部地图是否满足偏差要求，为此在优选实施方式中，该基于定位位姿为约束的地图更新方法，还包括：

步骤S600 采用均方根误差作为轨迹对齐的评判标准，假设轨迹对齐之后，将经过时间戳对齐的导航地图的定位位姿A点集合和局部地图的定位位姿B点的集合分别做均方根误差计算。

则均方根误差

此时RMSE越小，说明两条轨迹的偏差越小，本示例中优选当RMSE小于0.1m时，认为两条轨迹对齐比较好，以允许进行地图更新。

另一方面，对应上述方法示例，如图6所示，本发明还提供了一种基于定位位姿为约束的地图更新系统，其包括：

存储单元，用于存储包括如上任一所述基于定位位姿为约束的地图更新方法步骤的程序，以供导航单元，数据采集单元，建图单元，处理单元，适时调取执行；

导航单元，用于在导航地图中将半静态物体所处区域设置为地图待更新区域，以令移动机器人前往；

建图单元，用于在地图待更新区域启动建图算法；

数据采集单元，用于采集移动机器人在导航地图中的定位位姿A，局部地图中的定位位姿B；

处理单元，用于将定位位姿A与定位位姿B经线性插值补偿做时间戳对齐；及将定位位姿A作为局部地图的建图原点，将局部地图与导航地图进行时间粗对齐；根据获取的数组定位位姿A和定位位姿B做ICP，以求出对应的旋转平移转换矩阵，以供将局部地图和导航地图进行空间粗对齐；及将局部地图中的定位位姿B作为顶点，相邻的两个定位位姿B之间的位姿变换关系作为双边，以时间戳对齐后的定位位姿A作为单边，构建图模型进行图优化，获取局部建图定位位姿；

建图单元，还用于根据局部建图定位位姿来构建待更新区域栅格地图。

进一步的，为了提高整体地图更新精度，所述处理单元，还用于将经过时间戳对齐的导航地图的定位位姿A点集合和局部地图的定位位姿B点的集合分别做均方根误差计算

，

当判断满足预设精度值时允许建图单元进行地图更新。

另一方面，对应上述方法，本发明还提供了一种计算机可读存储介质，其上存储有计算机程序，其特征在于，所述计算机程序被处理器执行时实现上述方法中任一项所述基于定位位姿为约束的地图更新方法的步骤。

综上所述，通过本发明提供的该一种基于定位位姿为约束的地图更新方法及系统、存储介质，巧妙的将地图场景分为三部分：静态地图、半静态地图和动态地图，通过分析使用场景，仅针对半静态地图区域进行处理划定特定区域，进一步减少移动机器人运动过程中不必要的计算量。

另一方面，在部分优选方案中，由于现有技术主要通过对比扫描点和地图点之间的总体偏差来判断是否更新地图，这需要对当前扫描中的每个激光点和地图上的点进行计算比较，非常浪费时间，而本发明通过记录数个局部地图轨迹点和导航定位点，只需要计算几十个甚至几个点即可完成对比计算，大大减少了计算资源。

此外，现有技术对当前机器人的定位点的使用上，也只是对当前点云做位置映射，将点云位姿转换到当前定位点上，因此对当前定位点的利用不充分。本发明则将当前定位点引入到局部地图的建图中，将当前定位点作为局部建图的图优化中的单边，给当前地图一个强约束，从而在大场景的更新中，可以消除建图过程中的累计误差，使得地图更加精准。

以上公开的本发明优选实施例只是用于帮助阐述本发明。优选实施例并没有详尽叙述所有的细节，也不限制该发明仅为所述的具体实施方式。显然，根据本说明书的内容，可作很多的修改和变化。本说明书选取并具体描述这些实施例，是为了更好地解释本发明的原理和实际应用，从而使所属技术领域技术人员能很好地理解和利用本发明。本发明仅受权利要求书及其全部范围和等效物的限制，凡在本发明的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。

本领域技术人员可以理解，除了以纯计算机可读程序代码方式实现本发明提供的系统、装置及其各个模块以外，完全可以通过将方法步骤进行逻辑编程来使得本发明提供的系统、装置及其各个模块以逻辑门、开关、专用集成电路、可编程逻辑控制器以及嵌入式微控制器等的形式来实现相同程序。所以，本发明提供的系统、装置及其各个模块可以被认为是一种硬件部件，而对其内包括的用于实现各种程序的模块也可以视为硬件部件内的结构；也可以将用于实现各种功能的模块视为既可以是实现方法的软件程序又可以是硬件部件内的结构。

此外实现上述实施例方法中的全部或部分步骤是可以通过程序来指令相关的硬件来完成，该程序存储在一个存储介质中，包括若干指令用以使得单片机、芯片或处理器（processor）执行本申请各个实施例所述方法的全部或部分步骤。而前述的存储介质包括：U盘、移动硬盘、只读存储器（ROM，Read-Only Memory）、随机存取存储器（RAM，RandomAccess Memory）、磁碟或者光盘等各种可以存储程序代码的介质。

此外，本发明实施例的各种不同的实施方式之间也可以进行任意组合，只要其不违背本发明实施例的思想，其同样应当视为本发明实施例所公开的内容。

Claims (9)

1.一种基于定位位姿为约束的地图更新方法，其特征在于步骤包括：

步骤S100 在栅格化的导航地图中，将半静态物体所处区域设置为地图待更新区域，以令移动机器人到达后，启动建图算法；

步骤S200 采集移动机器人在导航地图中的定位位姿A及局部地图中的定位位姿B，做时间戳对齐；

步骤S300 将定位位姿A作为局部地图的建图原点，将局部地图与导航地图进行时间粗对齐；

步骤S400 采集数组定位位姿A和定位位姿B做ICP，以求出对应的旋转平移转换矩阵，以供将局部地图和导航地图进行空间粗对齐；

步骤S500 将局部地图中的定位位姿B作为顶点，相邻的两个定位位姿B之间的位姿变换关系作为双边，以时间戳对齐后的定位位姿A作为单边，构建图模型进行图优化，获取局部建图定位位姿来构建待更新区域栅格地图。

2.根据权利要求1所述的基于定位位姿为约束的地图更新方法，其特征在于，步骤S200中还包括：

步骤S210做时间戳对齐过程中，采用线性插值对定位位姿A及定位位姿B进行补偿。

3.根据权利要求1所述的基于定位位姿为约束的地图更新方法，其特征在于，步骤S400中所述空间粗对齐的步骤包括：

步骤S410 以预设行驶距离为间隔，记录数组建图定位点和导航定位点后，做ICP求出旋转R和平移t矩阵；

步骤S420 将局部地图的起始部分轨迹通过R和t的变换与导航地图做空间粗对齐。

4.根据权利要求1所述的基于定位位姿为约束的地图更新方法，其特征在于，步骤S500中采用局部地图中的关键帧的定位位姿B作为顶点，相邻的两个关键帧的定位位姿B之间的位姿变换关系作为双边，以时间戳对齐后的定位位姿A作为单边，构建图模型进行图优化。

5.根据权利要求4所述的基于定位位姿为约束的地图更新方法，其特征在于，步骤S500中所述关键帧为，当移动机器人移动超过0.5m或旋转超过20°中任一时，记录当前定位位姿作为局部地图中的关键帧的定位位姿B。

6.根据权利要求1至5中任一所述的基于定位位姿为约束的地图更新方法，其特征在于，还包括：

步骤S600 将经过时间戳对齐的导航地图的定位位姿A点集合和局部地图的定位位姿B点的集合分别做均方根误差计算

，

当判断满足预设精度值时允许进行地图更新。

7.一种基于定位位姿为约束的地图更新系统，其特征在于包括：

存储单元，用于存储包括如权利要求1至6中任一所述基于定位位姿为约束的地图更新方法步骤的程序，以供导航单元，数据采集单元，建图单元，处理单元，适时调取执行；

导航单元，用于在导航地图中将半静态物体所处区域设置为地图待更新区域，以令移动机器人前往；

建图单元，用于在地图待更新区域启动建图算法；

数据采集单元，用于采集移动机器人在导航地图中的定位位姿A，局部地图中的定位位姿B；

处理单元，用于将定位位姿A与定位位姿B经线性插值补偿做时间戳对齐；及将定位位姿A作为局部地图的建图原点，将局部地图与导航地图进行时间粗对齐；根据获取的数组定位位姿A和定位位姿B做ICP，以求出对应的旋转平移转换矩阵，以供将局部地图和导航地图进行空间粗对齐；及将局部地图中的定位位姿B作为顶点，相邻的两个定位位姿B之间的位姿变换关系作为双边，以时间戳对齐后的定位位姿A作为单边，构建图模型进行图优化，获取局部建图定位位姿；

建图单元，还用于根据局部建图定位位姿来构建待更新区域栅格地图。

8.根据权利要求7所述的基于定位位姿为约束的地图更新系统，其特征在于，所述处理单元，还用于将经过时间戳对齐的导航地图的定位位姿A点集合和局部地图的定位位姿B点的集合分别做均方根误差计算

，

当判断满足预设精度值时允许建图单元进行地图更新。

9.一种计算机可读存储介质，其上存储有计算机程序，其特征在于，所述计算机程序被处理器执行时实现权利要求1至6中任一项所述基于定位位姿为约束的地图更新方法的步骤。

Images