CN113701739B - 一种地图更新方法以及装置 - Google Patents

Abstract

本发明公开了一种地图更新方法，该方法包括，建立从第一有效地图节点经过至少一个待更新地图节点至第二有效地图节点的更新路径，控制移动机器人按照所述更新路径移动，当移动机器人当前位置与任一待更新地图节点的距离小于设定的触发采集图像的距离阈值时，触发当前位置的运动信息采集，并将当前位置作为更新后的地图节点位置；且在移动过程中至少采集第二有效地图节点位置的运动信息，基于所采集的第一有效地图节点位置、第二有效地图节点位置的运动信息，求解更新后的地图节点的全局位姿，删除地图中的待更新地图节点的地图信息，保存更新后的地图节点的地图信息。本发明能够实时自动完成地图节点更新处理，无需人工介入。

Description

一种地图更新方法以及装置

技术领域

本发明涉及机器人技术领域，特别地，涉及一种地图更新方法。

背景技术

在机器人技术领域里，地图提供了用于机器视觉定位的环境描述数据。例如，用于在即时定位与建图(SLAM)的系统中，移动机器人依靠预先建立的地图进行定位。由于构成地图信息的环境会产生损坏等不可恢复的变化，如果还用原来的地图进行定位，会导致定位效果下降甚至失败。

以基于地面纹理地图的视觉导航为例。基于地面纹理的视觉导航，需要对地面纹理进行建图。建图的过程是用摄像头将地面的有效纹理信息记录下来，并且和当前的位置坐标对应，形成一张有若干节点构成的纹理地图；每个节点包括位姿信息和纹理点的纹理信息。建图过程中，可能由于人工实施的疏忽等引入错误的信息；在长期的使用过程中，地面会产生一些磨损、破裂等不可恢复的变化。如果还用原来的纹理地图进行定位，则会产生定位效果下降的问题，因此需要对出错、污损的纹理点的纹理信息进行更新处理。

上述这种某些地图节点出现受损的情形也存在于其他类似的基于地图的视觉导航中。

发明内容

本发明提供了一种地图更新方法，以更新地图中需要更新的地图节点的节点数据。

本发明提供的一种地图更新方法，是这样实现的：

建立从第一有效地图节点经过待更新地图节点至第二有效地图节点的更新路径，

控制移动机器人按照所述更新路径移动，当移动机器人当前位置与待更新地图节点的距离小于设定的距离阈值时，触发当前位置的运动信息采集，并将当前位置作为更新后的地图节点位置；且在移动过程中至少采集第二有效地图节点位置的运动信息，

基于所采集的第一有效地图节点位置、第二有效地图节点位置的运动信息，求解更新后的地图节点的全局位姿，

删除地图中的待更新地图节点的地图信息，保存更新后的地图节点的地图信息，

其中，

地图信息至少包括更新后的地图节点的全局位姿。较佳地，所述更新路径为第一有效地图节点、待更新地图节点、第二有效地图节点在一条直线上的路径或者具有一定曲率的近似直线的路径；

所述用于解算移动机器人全局位姿信息的数据为安装于移动机器人本体的惯性传感器所采集的里程计数据、以及安装于移动机器人底部的摄像装置所采集的地面纹理图像数据；

所述当移动机器人当前位置与待更新地图节点的距离小于设定的触发采集图像的距离阈值时，触发当前位置的运动信息采集，进一步包括，

根据里程计数据获取当前位置信息，

当当前位置与待更新地图节点的距离小于所述距离阈值时，控制移动机器人暂停移动，触发所述摄像装置采集当前地面纹理图像；

所述基于所采集的第一有效地图节点位置、第二有效地图节点位置的运动信息，求解更新后的地图节点的全局位姿包括，

以所采集的第一有效地图节点位置、第二有效地图节点位置的运动信息、以及待更新地图节点的更新后的地图节点位置所采集的运动信息为约束，基于从第一有效地图节点至第二有效地图节点的测量误差，求解更新后的地图节点的全局位姿。

所述以第一有效地图节点、所采集的第二有效地图节点的运动信息、以及更新后的地图节点位置所采集的运动信息为约束，基于从第一有效地图节点至第二有效地图节点的测量误差，求解更新后的地图节点的全局位姿，求解更新后的地图节点的全局位姿，包括，

至少以任意一对地图节点之间的相对位姿为约束，其中，所述一对地图节点包括两个地图节点，且两个地图节点中至少包括一个更新后的地图节点；

根据移动机器人基于地图确定的有效地图节点的位姿与基于移动机器人所采集有效地图节点位置的运动信息所获得的位姿之间的误差，利用最小二乘法，求解出所述更新后的地图节点的位姿。

较佳地，所述至少以任意一对地图节点之间的相对位姿为约束包括，至少以两两地图节点之间的距离和角度的测量值为约束；

所述根据移动机器人基于地图确定的有效地图节点的位姿与基于移动机器人所采集有效地图节点位置的运动信息所获得的位姿之间的误差，利用最小二乘法，求解出所述更新后的地图节点的位姿，包括，

将更新后的地图节点处所采集的里程计数据作为更新后的地图节点的位姿初始值；

根据移动机器人基于地图确定的有效地图节点的位姿与基于移动机器人所采集有效地图节点位置的运动信息所获得的位姿之间的误差，通过迭代，求解出所述两两地图节点之间的距离和角度；

根据求解的距离和角度，确定待更新地图节点的位姿。

较佳地，所述待更新地图节点的数量为1，

所述至少以两两地图节点之间的距离和角度的测量值为约束包括，以更新后的地图节点分别与第一有效地图节点、第二有效地图节点之间的位姿作为约束，

根据第一有效地图节点位置的位姿信息和里程计数据，确定更新后地图节点和第二有效地图节点的位姿分别为：

更新后的地图节点的x轴坐标为：里程计记录的从第一有效地图节点移动到所述更新后地图节点的第一距离以第一角度在x轴上的投影，与第一有效地图节点的x轴坐标之和，

更新后的地图节点的y轴坐标为：所述第一距离以第一角度在y轴上的投影，与在第一有效地图节点的y轴坐标之和，

更新后的地图节点姿态为：第一角度与第一有效地图节点的姿态之和；

第二有效地图节点的x轴坐标为：里程计记录的从更新后的地图节点移动到第二有效地图节点的第二距离与所述第一距离之和，以第一角度和第二角度之和为角度在x轴上的投影，与第一有效地图节点的x轴坐标之和，

第二有效地图节点的y轴坐标为：所述第二距离与所述第一距离之和，以第一角度和第二角度之和为角度在y轴上的投影，与第一有效地图节点的y轴坐标之和，

第二有效地图节点姿态为：第一角度、第二角度、以及第一有效地图节点的姿态之和；

其中，所述第一角度为从第一有效地图节点移动到所述更新后的地图节点的转角，所述第二角度为从所述更新后的地图节点移动到第二有效地图节点的转角，

所述第一有效地图节点的位姿根据地图确定。

较佳地，所述根据移动机器人基于地图确定的有效地图节点的位姿与基于移动机器人采集该有效地图节点位置的运动信息所获得的位姿之间的误差，利用最小二乘法，求解出所述更新后的地图节点的位姿，包括，

根据更新后的地图节点的位姿初始值，设定第一距离和第一角度的初始值；基于第二有效地图节点的运动信息所解算的位姿信息，设定第二距离和第二角度的初始值；将所述第一距离、第一角度、第二距离和第二角度的初始值作为当前值，

根据第一距离、第一角度、第二距离和第二角度的当前值，计算第二有效地图节点的位姿当前值，

计算移动机器人基于地图确定的第二有效地图节点的位姿与所述第二有效地图节点的位姿当前值之间的误差，

判断是否满足迭代结束条件，

如果是，则将当前第一距离、和当前第一角度作为最终结果，按照约束计算更新后的地图节点的位姿，

否则，计算一修正量，将修正量分别与当前第一距离、第一角度、第二距离、以及第二角度相加，将修正后的第一距离、第一角度、第二距离、以及第二角度作为各自的当前值，然后返回执行所述根据第一距离、第一角度、第二距离和第二角度的当前值，计算第二有效地图节点的位姿当前值的步骤；

迭代条件可以是，达到设定的迭代次数、所述误差小于设定的误差阈值、修正量小于设定的修正阈值之一或其任意的组合；

所述保存更新后的地图节点的地图信息包括，将更新后的地图节点的位姿作为节点位姿保存，将基于采集的地面纹理图像所提取的纹理点的纹理信息作为更新后的地图节点的纹理信息保存。

较佳地，所述修正量通过以下方式计算的：

对当前误差求第一距离、第一角度、第二距离、第二角度的偏导，得到雅克比矩阵，

根据雅克比矩阵的转置矩阵、基于第二地图节点的运动信息进行纹理点图像匹配时的协方差矩阵的逆矩阵、雅克比矩阵、以及修正量的乘积等于雅克比矩阵的转置矩阵与当前误差乘积的负数，求得当前修正量；

其中，所述雅克比矩阵根据第一距离、第一角度、第二距离和第二角度的当前值确定。

所述建立至少从第一有效地图节点经过待更新地图节点至第二有效地图节点的更新路径，包括，

基于地图确定经过待更新地图节点、且与待更新地图节点在具有一定曲率的近似直线上的至少两个有效地图节点，分别作为第一有效地图节点和第二有效地图节点；

根据第一有效地图节点、待更新地图节点、第二有效地图节点建立至少从第一有效地图节点出发、经过待更新地图节点、到第二有效地图节点的更新路径。

较佳地，所述基于地图确定经过待更新地图节点、且与待更新地图节点在具有一定曲率的近似直线上的至少两个有效地图节点，包括，

确定与待更新地图节点的距离满足设定的第一阈值的第一有效地图节点，所述第一阈值至少满足从第一有效地图节点作两条射线与以待更新地图节点为圆心、以触发采集图像的距离阈值为半径的第一圆相切、且所述两条射线所形成夹角为锐角；

在所述两射线所形成夹角范围内，至少确定与待更新地图节点的距离满足设定的第二阈值的第二有效地图节点；

其中，所述第一阈值和第二阈值在设定的限制范围内。

较佳地，所述基于地图确定经过待更新地图节点、且与待更新地图节点在具有一定曲率的近似直线上的至少两个有效地图节点，包括，

待更新地图节点与除待更新地图节点之外的两有效地图节点所确定的直线的距离不大于设定阈值、且待更新地图节点位于两有效地图节点之间；

所述更新地图节点分别与所述两有效地图节点之间的距离满足设定的限制范围；

或者，

选择与待更新地图节点具有一定距离的第一有效地图节点，在从第一有效地图节点经待更新地图节点所确定的延长线上，确定距离该延长线不大于设定阈值的第二有效地图节点，待更新地图节点分别与第一有效地图节点、第二有效地图节点的距离满足设定的限制范围。

较佳地，控制移动机器人按照所述更新路径移动，包括，

控制移动机器人按照更新路径从第一有效地图节点出发，采集第一有效地图节点的运动信息，

根据里程计数据确定当前位置，判断当前位置与待更新地图节点的距离是否小于触发采集图像的距离阈值，如果是，判断是否已采集过当前位置的图像信息，如果已经采集过，则不触发当前位置的图像采集，如果尚未采集过，则触发采集当前图像信息；如果不是，则不触发当前位置的图像采集，按照更新路径继续移动，返回所述根据里程计数据确定当前位置的步骤；

判断当前位置是否到达第二有效地图节点，如果是，则采集当前位置的运动信息，否则，按照更新路径继续移动，直至所移动的更新路径结束；

判断当前更新路径的采集数据是否符合要求，如果不符合要求，则判断在该更新路径重复移动的次数是否大于设定的次数阈值，如果是，则重新建立更新路径，否则，将第二有效地图节点作为下一次按照更新路径移动时的第一有效地图节点，将第一有效地图节点作为下一次按照更新路径移动时的第二有效地图节点，返回至所述控制移动机器人按照更新路径从第一有效地图节点出发的步骤。

本发明提供的一种地图更新装置，该装置包括，

更新路径建立模块，用于建立从第一有效地图节点经过待更新地图节点至第二有效地图节点的更新路径；

控制模块，控制移动机器人按照所述更新路径移动，当移动机器人当前位置与待更新地图节点的距离小于设定的距离阈值时，触发当前位置的运动信息采集，并将当前位置作为更新后的地图节点位置；且在移动过程中至少采集第二有效地图节点位置的运动信息，

计算模块，基于所采集的第一有效地图节点位置、第二有效地图节点位置的运动信息，求解更新后的地图节点的全局位姿，删除地图中的待更新地图节点的地图信息，保存更新后的地图节点的地图信息，

其中，

地图信息至少包括更新后的地图节点的全局位姿。

本发明提供的一种移动机器人，包括，存储器和处理器，所述存储器存储有可被处理器执行的指令，所述指令被处理器执行，以使所述处理器执行以上任一所述的地图更新方法的步骤。

本发明提供的一种计算机可读存储介质，所述存储介质内存储有计算机程序，所述计算机程序被处理器执行时实现以上任一所述地图更新方法的步骤。

本发明通过更新路径上与待更新节点一定距离的位置、以及有效地图节点的运动信息采集，以所采集的第一有效地图节点位置、第二有效地图节点位置的运动信息、以及所述更新后的地图节点位置的运动信息为约束，求解更新后的地图节点的全局位姿，无需人工介入，实现了受损节点的地图实时自动更新；地图更新简单高效，改善了地图中部分地图节点失效受损时对移动机器人所导致的定位效果下降的影响。

附图说明

图1为本发明实施例一的地图更新方法的一种流程示意图。

图2为该待更新地图节点、至少两个有效地图节点位置关系的一种示意图。

图3为从a点开始经由b点运行至c点结束的过程中触发图像采集的一种示意图。

图4为本发明实施例二的地图更新方法的一种流程示意图。

图5为基于地图确定有效地图节点的一种示意图。

图6为待更新路径上有多个待更新地图节点的一种示意。

图7为本发明实施例的地图更新的装置示意图。

具体实施方式

为了使本申请的目的、技术手段和优点更加清楚明白，以下结合附图对本申请做进一步详细说明。

本发明的构思是，基于移动机器人在两有效地图节点之间经过m个待更新地图节点移动时的第一测量误差，可分配至由所述m个待更新地图节点将两有效地图节点之间所分割的m+1段上的测量误差，建立从第一有效地图节点经过待更新地图节点至第二有效地图节点的更新路径，通过移动机器人在该更新路径上所采集的两两地图节点之间的距离和角度的测量值，根据第一测量误差来解算待更新地图节点的位姿。其中，测量误差为移动机器人基于采集的运动信息数据而获得的位姿与有效地图节点根据地图确定的位姿之间的差异；运动信息包括用于解算移动机器人全局位姿信息的数据。例如，图像数据；m为大于等于1的自然数。

为便于理解本发明，以下将以待更新地图节点的数量为1为例。

实施例一

为便于理解，以下将以地面纹理地图为例来说明，所应理解的是，其也适用于除地面纹理地图之外的其他地图。

参见图1所示，图1为本发明实施例一的地图更新方法的一种流程示意图。当移动机器人接收到更新地图的触发指令时，解析触发指令中所包括的待更新地图节点的ID，然后移动机器人执行如下步骤：

步骤101，根据待更新地图节点的ID，基于地图确定经过该待更新地图节点、且与待更新地图节点在具有一定曲率的近似直线或者直线上的至少两个有效地图节点，分别作为第一有效地图节点和第二有效地图节点；

参见图2所示，图2为该待更新地图节点、至少两个有效地图节点位置关系的一种示意图。b点为待更新地图节点，地图节点a为第一有效地图节点，地图节点c为第二有效地图节点，a、b、c三点在一条直线上。鉴于实际应用中，根据两点较为容易地确定一直线，故而三点中可以允许任意一点偏离所确定的直线在设定的阈值范围；例如，选择与待更新地图节点具有一定距离的第一有效地图节点，在从第一有效地图节点经待更新地图节点机所确定的延长线上，确定距离该延长线不大于设定阈值的第二有效地图节点，待更新地图节点分别与第一有效地图节点、第二有效地图节点的距离满足设定的限制范围。

较佳地，可以根据除待更新地图节点之外的两有效地图节点确定直线，待更新地图节点偏离所述直线一定的范围，使得第一有效地图节点、待更新地图节点、第二有效地图节点形成具有一定曲率的近似直线。

其中，第一有效地图节点的地图坐标为(x_a,y_a,θ_a)，待更新地图节点的地图坐标为(x_b,y_b,θ_b)，第二有效地图节点的地图坐标为(x_c,y_c,θ_c)。这些地图坐标为地图节点的全局位姿，其中，x、y为地图节点的全局位置信息，θ为地图节点的全局姿态信息。

步骤102，移动机器人根据第一有效地图节点、待更新地图节点、第二有效地图节点建立至少从第一有效地图节点出发、经过待更新地图节点、到第二有效地图节点的更新路径；移动机器人按照更新路径控制移动，移动过程中获取至少第一有效地图节点和第二有效地图节点的运动信息，并且当基于里程计数据所确定的当前位置与待更新地图节点的距离小于设定的触发采集图像的距离阈值时，触发采集当前地面纹理图像信息，并将当前位置作为待更新地图节点更新后的地图节点；

在该步骤中，为提高更新的效率，较佳地，该更新路径为直线路径或者为具有一定曲率的近似直线路径，更新路径也可以是人工配置后加载至移动机器人中，并由人工控制移动机器人按照更新路径移动。

在本实施例中，运动信息包括，里程计数据和用于解算移动机器人的全局位姿信息的地面纹理图像。

以图2为例，控制移动机器人按照更新路径至少移动一次：从a点开始经由b点运行至c点结束。如果更新路径上有障碍物，可以绕行，其对待更新地图节点的更新影响不大。

如图3所示，图3为从a点开始经由b点运行至c点结束的过程中触发图像采集的一种示意图。在移动过程中，移动机器人利用轮式里程计等惯性传感器获取当前位置信息，当当前位置的地图坐标(x，y)与待更新地图节点(xb,yb)之间的距离小于设定的距离阈值时，让移动机器人暂停，触发摄像头采集当前位置的地面纹理点图像，以获得该位置的纹理信息；并且，通过惯性传感器获取a点、c点的里程计数据，通过所采集a点、c点的纹理点图像与有效地图节点中的纹理信息进行匹配，从而基于地图节点信息获取移动机器人在a点、c点的当前位姿。

步骤103，移动机器人将更新后的地图节点处的位姿作为待更新地图节点更新后的节点位姿初始值，以更新后的地图节点分别与第一有效地图节点、第二有效地图节点之间的位姿作为约束，以用于进行位姿图优化。

鉴于更新后的地图节点处所采集的纹理点图像与地图能够成功匹配的概率较低，这使得更新后的地图节点处的位姿难以通过图像配准的方式而解算出移动机器人在更新后的地图节点处的位姿，故而，在该步骤中，将待更新地图节点更新后的位置b′处所采集的里程计数据作为初始值，这样，更新后的位置b′地图坐标的初始值，记为

移动机器人由a点运动到b′点，里程计记录移动机器人移动的第一距离为r₁，转过的第一角度θ₁，再由b′点移动至c点，里程计记录移动的第二距离为r₂，转过的第二角度为θ₂。显然，由a点运动至c点移动的距离为r＝r₁+r₂，转过的角度为θ＝θ₁＋θ₂。其中，距离可由里程计通过记录轮胎转过的圈数和已知的轮胎半径获得，转过的角度可通过记录左右轮转过的圈数和已知的轮胎半径获得。

根据移动机器人在a点的位姿信息(根据地图确定的位姿信息)和里程计数据推算出b′点和c点的位姿信息：

b′点的x轴坐标为：第一距离以第一角度在x轴上的投影与a点的x轴坐标之和；b′点的y轴坐标为：所述第一距离以第一角度在y轴上的投影与在a点的y轴坐标之和；b′点的姿态为：第一角度与a点的姿态之和；

c点的x轴坐标为：第二距离与第一距离之和以第一角度和第二角度之和为角度在x轴上的投影，与a的x轴坐标之和；c点的y轴坐标为：第二距离与第一距离之和以第一角度和第二角度之和为角度在y轴上的投影，与a点的y轴坐标之和；c点的姿态为：第一角度、第二角度、以及a点的姿态之和；

数学式表达为：

其中，ξ_c′表示通过采集的运动信息获得的测量值。

由此得到当前位置分别与第一有效地图节点、第二有效地图节点之间运动信息的约束关系。

步骤104，移动机器人根据基于地图信息所确定的有效地图节点的位姿与移动机器人在该有效地图节点处基于所采集运动信息获得的位姿之间的误差，利用最小二乘法求解出待更新地图节点的位姿信息，以对位置b′的初始值进行图优化；保存位置b′所采集的图像信息，得到待更新地图节点的节点数据。

在如图2从a移动到c时，由于c点仍有有效的纹理地图节点的信息，因此，根据地图有效位姿信息ξ_c＝(x_c,y_c,θ_c)。鉴于里程计测量值包含一定误差，地图节点的信息是准确的，故而ξ_c和ξ_c′可能并不会相等，即，里程计测量值与有效地图节点可能存在误差：

e＝ξ_c-ξ_c′ (式3)

其中，e是关于r₁,θ₁和r₂,θ₂的函数。由于角度信息的存在，因此，上式中是非线性的。

为求解出r₁,θ₁和r₂,θ₂，可以采用最小二乘迭代法。以高斯-牛顿法为例。

步骤1041，根据位置b′的初始值设定r₁,θ₁的初始值，基于c点的运动信息ξ_c′所解算的位姿信息设定r₂,θ₂的初始值；将r₁,θ₁的初始值、r₂,θ₂的初始值作为当前值，记为P；

步骤1042，按照式2计算出ξ_c′的当前测量值，

按照c点处里程计测量值与基于有效地图节点c的地图信息所确定的位姿存在误差关系计算出当前误差，即，按照式3计算出误差，

步骤1043，判断是否满足迭代结束条件，

如果是，则将r₁、θ₁的当前值作为最终结果，然后按照式1计算出位姿更新后的地图节点位姿，

否则，为了减小误差，计算基于当前P的修正量δP，以得到修正后的估计值P₁，其中，P₁＝P+δP，

所述修正量是这样计算的：

对当前e求r₁,θ₁和r₂,θ₂的偏导数，得到雅克比矩阵

根据高斯-牛顿最小二乘法，有：雅克比矩阵的转置矩阵、基于第二地图节点的运动信息进行纹理点图像匹配时的协方差矩阵的逆矩阵、雅克比矩阵、以及修正量的乘积等于雅克比矩阵的转置矩阵与当前误差乘积的负数，用数学式表达为：

J⁷∑^-1J·δP＝-J^T·e

其中，J为雅克比矩阵，根据当前估计值计算得到，∑为基于c点采集的运动信息进行纹理点图像匹配时的协方差矩阵，其为3×3的矩阵，e为按照当前估计值得到的当前误差，通过上式可求得用于迭代的修正量δP。

将修正后的估计值P₁作为当前值，即第一距离、第一角度、第二距离、以及第二角度的当前值，返回执行步骤1042。

迭代条件可以是，达到设定的迭代次数、误差e小于设定的误差阈值、修正量小于设定的修正阈值之一或其任意的组合。

通过上述步骤可以求解出r₁、θ₁、r₂、θ₂，根据其中r₁、θ₁利用式1得到b′的地图坐标，将该地图坐标和获取的纹理点图像信息作为待更新地图节点更新后的节点数据，并保存，即，将更新后的地图节点的位姿作为节点位姿保存，将基于采集的地面纹理图像所提取的纹理点的纹理信息作为更新后的地图节点的纹理信息保存。

从本实施例可见，由于里程计本身存在累计误差，从a经过b到c的距离越大，累计误差就越大，因此，为提高迭代求解的效率，有效地图节点之间的距离可以限制在设定的范围，例如最大不超过5～8米。

本发明实施例提供的地图更新方法，移动机器人在接收到地图更新触发指令后，能够实时自动完成更新处理，无需人工介入，地图更新简单且高效，极大地改善了地图中部分地图节点失效受损时对移动机器人所导致的定位效果下降的影响。通过最小二乘法求解出待更新地图节点的位姿信息，能够有效地去除噪声，获得高精度的位姿信息。

实施例二

参见图4所示，图4为本发明实施例二的地图更新方法的一种流程示意图。当移动机器人接收到更新地图的触发指令时，解析触发指令中所包括的待更新地图节点的ID，然后移动机器人执行如下步骤：

步骤401，根据待更新地图节点的ID，基于地图确定经过该待更新地图节点、且与待更新地图节点在具有一定曲率的近似直线上的至少两个有效地图节点，分别作为第一有效地图节点和第二有效地图节点；

参见如5所示，图5为基于地图确定有效地图节点的一种示意图。基于地图在待更新地图节点位置附近具有多个有效地图节点。这种情形下，以待更新地图节点为圆心、以触发采集图像的距离阈值为半径，确定第一圆；以待更新地图节点为中心，确定与待更新地图节点的距离满足设定的第一阈值的第一有效地图节点，所述第一阈值至少满足从第一有效地图节点作两条射线与所述第一圆相切，两射线所形成夹角为锐角；从所述第一有效地图节点作两条射线与所述第一圆相切，在两射线所形成夹角范围内至少确定与待更新地图节点的距离满足设定的第二阈值的第二有效地图节点；进一步地，还可以确定在两射线所形成夹角范围内确定分别满足设定阈值的多个待更新地图节点，以使得更新路径可以包括多个待更新地图节点，从而提高整体的更新效率。其中，所述第一阈值和第二阈值在设定的限制范围内。

较佳地，第一有效地图节点、待更新地图节点、第二有效地图节点在一直线上。

步骤402，根据第一有效地图节点、待更新地图节点、第二有效地图节点，建立至少从第一有效地图节点出发、经过待更新地图节点、到第二有效地图节点的更新路径。

步骤403，控制移动机器人按照更新路径从第一有效地图节点出发，采集第一有效地图节点的运动信息，

本实施例中，所述运动信息包括里程计数据和用于解算移动机器人全局位姿数据的图像。

步骤404，根据里程计数据确定当前位置，判断当前位置与待更新地图节点的距离是否小于所述触发采集图像的距离阈值，

如果是，较佳地，为了避免在当前位置重复地采集图像，判断是否已采集过当前位置的图像信息，如果已经采集过，则不触发当前位置的图像采集，如果尚未采集过，则触发当前图像信息的采集，并将当前位置作为待更新地图节点更新后的位置b′，即更新后的节点位置；

如果不是，则不触发采集当前图像信息，并按照更新路径继续移动；并返回步骤404，

步骤405，判断当前位置是否到达第二有效地图节点，如果是，则采集当前位置的运动信息并执行步骤406，否则，按照更新路径继续移动。

步骤406，根据所采集图像信息的质量以及数量，判断当前更新路径的采集数据是否符合要求，如果不符合要求，则判断在该更新路径重复移动的次数是否大于设定的次数阈值，如果是，则返回步骤401，以重新建立更新路径，否则，则将所述第二有效地图节点作为下一次按照更新路径移动时的第一有效地图节点，将所述第一有效地图节点作为下一次按照更新路径移动时的第二有效地图节点，返回至步骤403；如果符合要求，则执行步骤407；

步骤407，移动机器人将待更新地图节点更新后的位姿信息(里程计数据)作为待更新地图节点更新后的节点位姿的初始值，以待更新地图节点更新后的位置分别与第一有效地图节点、第二有效地图节点之间的位姿作为约束，以用于进行位姿图优化。

在该步骤中，将待更新地图节点更新后的位置b′的地图坐标作为位置b′的初始值，记为

移动机器人由第一有效地图节点运动到b′点，里程计记录移动机器人移动的距离为r₁，转过的角度θ₁，再由b′点移动至第二有效地图节点，里程计记录移动的距离为r₂，转过的角度为θ₂。显然，由第一有效地图节点运动至第二有效地图节点移动的距离为r＝r₁+r₂，转过的角度为θ＝θ₁+θ₂。

根据移动机器人在第一有效地图节点的位姿信息(根据地图确定的位姿信息)和里程计数据推算出b′点和第二有效地图节点的位姿信息：

ξ_c′＝(x_c,y_cθ_c)＝(x_a+r·cosθ，y_a+r·sinθ，θ_a+θ) (式2)。

其中，下标a表示第一有效地图节点，ξ_c′表示通过采集的运动信息获得第二有效地图节点的测量值。

由此得到位置b′分别与第一有效地图节点、第二有效地图节点之间运动信息的约束关系。

步骤408，移动机器人根据基于地图信息所确定的有效地图节点的位姿与移动机器人在该有效地图节点处基于所采集运动信息获得的位姿之间的误差，利用最小二乘法求解出待更新地图节点的位姿信息，并保存位置b′所采集的图像信息，得到待更新地图节点的节点数据。该步骤与实施例一中的步骤104相同。

本发明实施例通过控制按照更新路径移动的次数，能够及时地调整有效地图节点，提高了地图更新的鲁棒性。

尽管上述实施例是以一个待更新地图节点来说明的，所应理解的是，在更新路径上可以包括多个待更新地图节点。参见图6所示，图6为待更新路径上有多个待更新地图节点的一种示意，两个有效地图节点之间包括了b1,b2,…,b4个节点，例如，有效地图节点之间的间距为5米，这5米中可以每隔1米都有个地图节点，当然，各个节点之间的间距可以不相等。作为一种变形，该更新路径还可以包括多个有效地图节点。

在更新路径上，任意两两节点之间都有距离和角度的测量值，累积误差仍然由c点计算。考虑里程计精度，距离越大，里程计的累计误差越大，但从a点到c点之间总的误差不会因待更新地图节点而增加，即，里程计记录的误差不因为中间有几个节点而改变。地图节点越多，需要优化计算的量也越多，计算量越大。求解优化的过程相当于是将里程计的累计误差分配到各个分段路径上，地图节点越多，分配越困难，优化就难以收敛，优化后的残差越大。

参见图6所示，图6为本发明实施例的地图更新的装置示意图。该地图更新装置包括，

更新路径建立模块，用于建立从第一有效地图节点经过待更新地图节点至第二有效地图节点的更新路径；

控制模块，控制移动机器人按照所述更新路径移动，当移动机器人当前位置与待更新地图节点的距离小于设定的距离阈值时，触发当前位置的运动信息采集，并将当前位置作为更新后的地图节点位置；且在移动过程中至少采集第二有效地图节点位置的运动信息；

计算模块，基于所采集的第一有效地图节点位置、第二有效地图节点位置的运动信息，求解更新后的地图节点的全局位姿，删除地图中的待更新地图节点的地图信息，保存更新后的地图节点的地图信息，

其中，

地图信息至少包括更新后的地图节点的全局位姿。。

本发明提供的一种移动机器人，包括，存储器和处理器，所述存储器存储有可被处理器执行的指令，所述指令被处理器执行，以使所述处理器执行所述的地图更新方法的步骤。

对于装置/网络侧设备/存储介质实施例而言，由于其基本相似于方法实施例，所以描述的比较简单，相关之处参见方法实施例的部分说明即可。

在本文中，诸如第一和第二等之类的关系术语仅仅用来将一个实体或者操作与另一个实体或操作区分开来，而不一定要求或者暗示这些实体或操作之间存在任何这种实际的关系或者顺序。而且，术语“包括”、“包含”或者其任何其他变体意在涵盖非排他性的包含，从而使得包括一系列要素的过程、方法、物品或者设备不仅包括那些要素，而且还包括没有明确列出的其他要素，或者是还包括为这种过程、方法、物品或者设备所固有的要素。在没有更多限制的情况下，由语句“包括一个……”限定的要素，并不排除在包括所述要素的过程、方法、物品或者设备中还存在另外的相同要素。

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

Claims (14)

1.一种地图更新方法，其特征在于，该方法包括，

建立从第一有效地图节点经过待更新地图节点至第二有效地图节点的更新路径，

控制移动机器人按照所述更新路径移动，当移动机器人当前位置与待更新地图节点的距离小于设定的距离阈值时，触发当前位置的运动信息采集，并将当前位置作为更新后的地图节点位置；且在移动过程中采集第一有效节点位置和第二有效节点位置的运动信息，

基于所采集的第一有效地图节点位置、第二有效地图节点位置的运动信息，求解更新后的地图节点的全局位姿，

删除地图中的待更新地图节点的地图信息，保存更新后的地图节点的地图信息，

其中，

地图信息至少包括更新后的地图节点的全局位姿；

所述基于所采集的第一有效地图节点位置、第二有效地图节点位置的运动信息，求解更新后的地图节点的全局位姿，包括：

以所采集的第一有效地图节点位置、第二有效地图节点位置的运动信息、以及待更新地图节点的更新后的地图节点位置所采集的运动信息为约束，

基于从第一有效地图节点至第二有效地图节点的测量误差，求解更新后的地图节点的全局位姿。

2.如权利要求1所述的方法，其特征在于，所述更新路径为第一有效地图节点、待更新地图节点、第二有效地图节点在一条直线上的路径或者具有一定曲率的近似直线的路径；

用于解算移动机器人全局位姿信息的数据为安装于移动机器人本体的惯性传感器所采集的里程计数据、以及安装于移动机器人底部的摄像装置所采集的地面纹理图像数据；

所述当移动机器人当前位置与待更新地图节点的距离小于设定的触发采集图像的距离阈值时，触发当前位置的运动信息采集，进一步包括，

根据里程计数据获取当前位置信息，

当当前位置与待更新地图节点的距离小于所述距离阈值时，控制移动机器人暂停移动，触发所述摄像装置采集当前地面纹理图像。

3.如权利要求2所述的方法，其特征在于，所述以所采集的第一有效地图节点、第二有效地图节点的运动信息、以及更新后的地图节点位置所采集的运动信息为约束，基于从第一有效地图节点至第二有效地图节点的测量误差，求解更新后的地图节点的全局位姿，包括，

至少以任意一对地图节点之间的相对位姿为约束，其中，所述一对地图节点包括两个地图节点，且两个地图节点中至少包括一个更新后的地图节点；

根据移动机器人基于地图确定的有效地图节点的位姿与基于移动机器人所采集有效地图节点位置的运动信息所获得的位姿之间的误差，利用最小二乘法，求解出所述更新后的地图节点的位姿。

4.如权利要求3所述的方法，其特征在于，所述里程计数据包括，地图节点之间的距离和转过的角度，

所述至少以任意一对地图节点之间的相对位姿为约束包括，至少以两两地图节点之间的距离和角度的测量值为约束；

所述根据移动机器人基于地图确定的有效地图节点的位姿与基于移动机器人所采集有效地图节点位置的运动信息所获得的位姿之间的误差，利用最小二乘法，求解出所述更新后的地图节点的位姿，包括，

将更新后的地图节点处所采集的里程计数据作为更新后的地图节点的位姿初始值；

根据移动机器人基于地图确定的有效地图节点的位姿与基于移动机器人所采集有效地图节点位置的运动信息所获得的位姿之间的误差，通过迭代，求解出所述两两地图节点之间的距离和角度；

根据求解的距离和角度，确定待更新地图节点的位姿；

其中，基于地图确定的有效地图节点的位姿通过所采集的地面纹理图像数据与该有效地图节点中的纹理信息进行匹配而获取。

5.如权利要求4所述的方法，其特征在于，所述待更新地图节点的数量为1，

所述至少以两两地图节点之间的距离和角度的测量值为约束包括，以更新后的地图节点分别与第一有效地图节点、第二有效地图节点之间的位姿作为约束，

根据第一有效地图节点位置的位姿信息和里程计数据，确定更新后地图节点和第二有效地图节点的位姿分别为：

更新后的地图节点的x轴坐标为：里程计记录的从第一有效地图节点移动到所述更新后地图节点的第一距离以第一角度在x轴上的投影，与第一有效地图节点的x轴坐标之和，

更新后的地图节点的y轴坐标为：所述第一距离以第一角度在y轴上的投影，与在第一有效地图节点的y轴坐标之和，

更新后的地图节点姿态为：第一角度与第一有效地图节点的姿态之和；

第二有效地图节点的x轴坐标为：里程计记录的从更新后的地图节点移动到第二有效地图节点的第二距离与所述第一距离之和，以第一角度和第二角度之和为角度在x轴上的投影，与第一有效地图节点的x轴坐标之和，

第二有效地图节点的y轴坐标为：所述第二距离与所述第一距离之和，以第一角度和第二角度之和为角度在y轴上的投影，与第一有效地图节点的y轴坐标之和，

第二有效地图节点姿态为：第一角度、第二角度、以及第一有效地图节点的姿态之和；

其中，所述第一角度为从第一有效地图节点移动到所述更新后的地图节点的转角，

所述第二角度为从所述更新后的地图节点移动到第二有效地图节点的转角，

所述第一有效地图节点的位姿根据地图确定。

6.如权利要求5所述的方法，其特征在于，所述根据移动机器人基于地图确定的有效地图节点的位姿与基于移动机器人采集该有效地图节点位置的运动信息所获得的位姿之间的误差，利用最小二乘法，求解出所述更新后的地图节点的位姿，包括，

根据更新后的地图节点的位姿初始值，设定第一距离和第一角度的初始值；基于第二有效地图节点的运动信息所解算的位姿信息，设定第二距离和第二角度的初始值；将所述第一距离、第一角度、第二距离和第二角度的初始值作为当前值，

根据第一距离、第一角度、第二距离和第二角度的当前值，计算第二有效地图节点的位姿当前值，

计算移动机器人基于地图确定的第二有效地图节点的位姿与所述第二有效地图节点的位姿当前值之间的误差，

判断是否满足迭代结束条件，

如果是，则将当前第一距离、和当前第一角度作为最终结果，按照约束计算更新后的地图节点的位姿，

否则，计算一修正量，将修正量分别与当前第一距离、第一角度、第二距离、以及第二角度相加，将修正后的第一距离、第一角度、第二距离、以及第二角度作为各自的当前值，然后返回执行所述根据第一距离、第一角度、第二距离和第二角度的当前值，计算第二有效地图节点的位姿当前值的步骤；

迭代条件包括以下之一或任意组合：达到设定的迭代次数、所述误差小于设定的误差阈值、修正量小于设定的修正阈值；

所述保存更新后的地图节点的地图信息包括，将更新后的地图节点的位姿作为节点位姿保存，将基于采集的地面纹理图像所提取的纹理点的纹理信息作为更新后的地图节点的纹理信息保存。

7.如权利要求6所述的方法，其特征在于，所述修正量通过以下方式计算：

对当前误差求第一距离、第一角度、第二距离、第二角度的偏导，得到雅克比矩阵，

根据雅克比矩阵的转置矩阵、基于第二地图节点的运动信息进行纹理信息图像匹配时的协方差矩阵的逆矩阵、雅克比矩阵、以及修正量的乘积等于雅克比矩阵的转置矩阵与当前误差乘积的负数，求得当前修正量；

其中，所述雅克比矩阵根据第一距离、第一角度、第二距离和第二角度的当前值确定。

8.如权利要求1所述的方法，其特征在于，所述建立至少从第一有效地图节点经过待更新地图节点至第二有效地图节点的更新路径，包括，

基于地图确定经过待更新地图节点、且与待更新地图节点在具有一定曲率的近似直线上的至少两个有效地图节点，分别作为第一有效地图节点和第二有效地图节点；

根据第一有效地图节点、待更新地图节点、第二有效地图节点建立至少从第一有效地图节点出发、经过待更新地图节点、到第二有效地图节点的更新路径。

9.如权利要求8所述的方法，其特征在于，所述基于地图确定经过待更新地图节点、且与待更新地图节点在具有一定曲率的近似直线上的至少两个有效地图节点，包括，

确定与待更新地图节点的距离满足设定的第一阈值的第一有效地图节点，所述第一阈值至少满足从第一有效地图节点作两条射线与以待更新地图节点为圆心、以触发采集图像的距离阈值为半径的第一圆相切、且所述两条射线所形成夹角为锐角；

在所述两射线所形成夹角范围内，至少确定与待更新地图节点的距离满足设定的第二阈值的第二有效地图节点；

其中，所述第一阈值和第二阈值在设定的限制范围内。

10.如权利要求8所述的方法，其特征在于，所述基于地图确定经过待更新地图节点、且与待更新地图节点在具有一定曲率的近似直线上的至少两个有效地图节点，包括，

待更新地图节点与除待更新地图节点之外的两有效地图节点所确定的直线的距离不大于设定阈值、且待更新地图节点位于两有效地图节点之间；

所述待更新地图节点分别与所述两有效地图节点之间的距离满足设定的限制范围；

或者，

选择与待更新地图节点具有一定距离的第一有效地图节点，在从第一有效地图节点经待更新地图节点所确定的延长线上，确定距离该延长线不大于设定阈值的第二有效地图节点，待更新地图节点分别与第一有效地图节点、第二有效地图节点的距离满足设定的限制范围。

11.如权利要求1至7任一项所述的方法，其特征在于，控制移动机器人按照所述更新路径移动，包括，

控制移动机器人按照更新路径从第一有效地图节点出发，采集第一有效地图节点的运动信息，

根据里程计数据确定当前位置，判断当前位置与待更新地图节点的距离是否小于触发采集图像的距离阈值，如果是，判断是否已采集过当前位置的图像信息，如果已经采集过，则不触发当前位置的图像采集，如果尚未采集过，则触发采集当前图像信息；如果不是，则不触发当前位置的图像采集，按照更新路径继续移动，返回所述根据里程计数据确定当前位置的步骤；

判断当前位置是否到达第二有效地图节点，如果是，则采集当前位置的运动信息，否则，按照更新路径继续移动，直至所移动的更新路径结束；

判断当前更新路径的采集数据是否符合要求，如果不符合要求，则判断在该更新路径重复移动的次数是否大于设定的次数阈值，如果是，则重新建立更新路径，否则，将第二有效地图节点作为下一次按照更新路径移动时的第一有效地图节点，将第一有效地图节点作为下一次按照更新路径移动时的第二有效地图节点，返回至所述控制移动机器人按照更新路径从第一有效地图节点出发的步骤。

12.一种地图更新装置，其特征在于，该装置包括，

更新路径建立模块，用于建立从第一有效地图节点经过待更新地图节点至第二有效地图节点的更新路径；

控制模块，控制移动机器人按照所述更新路径移动，当移动机器人当前位置与待更新地图节点的距离小于设定的距离阈值时，触发当前位置的运动信息采集，并将当前位置作为更新后的地图节点位置；且在移动过程中采集第一有效节点位置和第二有效地图节点位置的运动信息，

计算模块，基于所采集的第一有效地图节点位置、第二有效地图节点位置的运动信息，求解更新后的地图节点的全局位姿，删除地图中的待更新地图节点的地图信息，保存更新后的地图节点的地图信息，

其中，

地图信息至少包括更新后的地图节点的全局位姿；

所述基于所采集的第一有效地图节点位置、第二有效地图节点位置的运动信息，求解更新后的地图节点的全局位姿，包括：

以所采集的第一有效地图节点位置、第二有效地图节点位置的运动信息、以及待更新地图节点的更新后的地图节点位置所采集的运动信息为约束，

基于从第一有效地图节点至第二有效地图节点的测量误差，求解更新后的地图节点的全局位姿。

13.一种移动机器人，其特征在于，包括，存储器和处理器，所述存储器存储有可被处理器执行的指令，所述指令被处理器执行，以使所述处理器执行如权利要求1至11任一所述的地图更新方法的步骤。

14.一种计算机可读存储介质，其特征在于，所述存储介质内存储有计算机程序，所述计算机程序被处理器执行时实现如权利要求1至11任一所述地图更新方法的步骤。