特征信息和地图信息进行过滤，W生成整合观测数据，从而减少了在较低级别执行SLAM 时的开销W及兀余信息。
[0103] 接下来，当在较低级别中过滤掉当前普通节点和历史普通节点所包含的重复的特 征信息之后，可W使用整合观测数据来替换在普通节点1中原始包括的观测数据，从而将 所选择的普通节点1转换为当前的关键节点1。
[0104] 优选地，在进行特征过滤的过程中，还可W对各个特征信息进行增强地图识别操 作，W便减少在特征信息中可能存在的误差，并且提高地图识别的精度。
[0105] 具体地，可W向当前普通节点和历史普通节点所包含的重复的特征信息分别赋予 权重值，例如，权重值的分配规则可W是：如果机器人距离该特征点的距离越近，则权重值 越大。送里，假设该特征点是机器人在行进过程中拍摄到的一盆花，并且机器人在普通节点 1中距离送盆花9米，在普通节点2中距离送盆花6米，在普通节点3中距离送盆花3米，贝U 例如，可W向普通节点1赋予较低权重值（例如，2)，向普通节点2赋予中等权重值（例如， 3)，并且向普通节点3赋予较高权重值（例如，5)。然后，可W根据所述权重值来对多个节 点所包含的重复的特征信息进行加权平均，并且生成包括加权平均后的特征信息的整合观 测数据，W生成单一关键节点。
[0106] 然后，可W在存储器中存储当前生成的关键节点1，W供稍后使用。
[0107] 需要说明的是，关键节点与其他的普通节点基本相似，其区别仅仅在于，关键节点 进一步包括了一个局部区域中的网格地图信息（或称之为子图），而普通节点仅仅包括全 局中的网格地图信息。构建关键节点的动机在于，可W通过局部子图进行匹配、而不是通过 单顿观测数据进行匹配来判断是否出现闭合环路，从而增加环路检测的准确性。
[0108] 送样，在地图构建的过程中，可W在较低级别的普通节点中，构造出全局网格地 图；并且可W在较高级别的关键节点中，同时构造出全局网格地图和局部网格地图。维护该 全局网格地图，并且应用马尔科夫蒙特卡洛（MCMC)理论来估计机器人在世界坐标系中的 位置；而维护该局部网格地图，W用于验证是否存在闭合环路。
[0109] 在步骤S250中，判断当前普通节点与先前关键节点是否满足匹配检测条件。
[0110] 在生成当前关键节点（即，关键节点1)之后，可W判断当前的普通节点与先前的 关键节点是否满足匹配检测（即，闭合环路检测）的条件。如果当前普通节点与先前关键 节点满足匹配检测条件，则本信息处理方法可W前进到步骤S260, W进行匹配检测（即，闭 合环路）检测。否则，本信息处理方法返回到步骤S210继续执行，W获取新的一顿观测数 据。
[0111] 可W基于各种不同的检测条件来触发闭合环路的检测操作。例如，闭合环路检测 条件可W基于两个节点之间的距离来触发检测操作。具体地，如果发现当前普通节点与先 前关键节点处于预定距离之内，则认为两个节点符合闭合环路检测条件。也就是说，当机器 人的当前位姿接近于先前的关键节点时，可W认为两个节点符合闭合环路检测条件。
[011引如图3和图4所示，由于此时，该当前普通节点（即，普通节点4)归属于当前关键 节点（即，关键节点1)，所W可W判断出该普通节点并不满足闭合环路检测条件。
[0113] 故此，如上所述，本信息处理方法返回到步骤S210继续执行，继而依次地根据机 器人在位置5到位置8的观测数据来生成并存储四个普通节点（即，普通节点5-8)，对每两 个相邻的普通节点进行顿间匹配；判断是否满足关键节点生成条件，从而通过对普通节点 5-8的观测数据进行整合来将普通节点5转换为关键节点2,并且判断普通节点8是否与关 键节点1是否符合匹配检测条件。如图3和图4所示，显然，当机器人在位置8时，根据当 前普通节点8和先前关键节点1的位姿信息可W判断出两者之间的间隔距离显然大于预定 阔值（例如，5米），也就是说，两者之间并不存在产生闭合环路的可能性，因此，仍然判断出 该普通节点8与关键节点1不符合匹配检测条件。
[0114] 如此类推，随着机器人从位置9经过位置10向位置11进行运动，继而又生成并存 储H个普通节点（即，普通节点9-11)，对每两个相邻的普通节点进行顿间匹配；判断是否 满足关键节点生成条件，从而通过对普通节点9-11的观测数据进行整合来将普通节点9转 换为关键节点3,并且判断普通节点11是否与关键节点1和关键节点2是否符合匹配检测 条件。如图3和图4所示，显然，当机器人在运行到位置11时，根据当前普通节点11和先前 关键节点1的位姿信息可W判断出两者之间的间隔距离已经小于预定阔值（例如，5米）， 也就是说，可W知道两者之间出现存在闭合环路的可能性，因此，可W判断出由于该普通节 点11与关键节点1距离很近，所W已经符合匹配检测条件。
[0115] 在继续描述后续的步骤S260之前，为了进一步清楚地说明拓扑图中上述各个节 点的生成过程，下面，参考图5A到图5F来详细描述在根据本发明实施例的具体示例中增量 式地生成一张包括普通节点和关键节点的拓扑图的过程。
[0116] 图5A到图5F图示了根据本发明实施例具体示例的综合拓扑图。在该综合拓扑图 中，既包括较低级别的普通节点，也包括经过规约所生成的较高级别的关键节点。
[0117] 在图5A到图5F中，白色空必圆表示普通节点，黑色实必圆表示关键节点，而灰色 填充圆表示当前正在处理的节点。每个关键节点包含若干个相邻普通节点中的观测信息， 从而构成局部的子图。
[0118] 如图5A所示，通过重复步骤S210到S230,在该拓扑图中生成了四个普通节点 1-4。接下来，由于在步骤S230中判断出包括当前普通节点（即，普通节点4)的多个普通 节点（即，普通节点1-4)满足关键节点生成条件，所W在步骤S240中，通过对普通节点1-4 中的观测数据进行整合，从而将普通节点1转换为关键节点1，该关键节点1表示出局部子 图1 (在图中通过Ml来表示），如图5B所示。然后，由于在步骤S250中判断出没有先前关 键节点，所W本信息处理方法返回继续执行。
[0119] 如图5C所示，通过重复步骤S210到S230,在该拓扑图中又生成了四个普通节点 5-8。接下来，由于在步骤S230中判断出包括当前普通节点（即，普通节点8)的多个普通 节点（即，普通节点5-8)满足关键节点生成条件，所W在步骤S240中，通过对普通节点5-8 中的观测数据进行整合，从而将普通节点5转换为关键节点2,该关键节点2表示出局部子 图2 (在图中通过M2来表示），如图抓所示。然后，由于在步骤S250中判断出当前普通节 点（即，普通节点8)与先前关键节点（即，关键节点1)不满足匹配检测条件，所W本信息 处理方法返回继续执行。
[0120] 如图祀所示，通过重复步骤S210到S230,在该拓扑图中又生成了H个普通节点 9-11。接下来，由于在步骤S230中判断出包括当前普通节点（即，普通节点8)的多个普通 节点（即，普通节点9-11)满足关键节点生成条件，所W在步骤S240中，通过对普通节点 9-11中的观测数据进行整合，从而将普通节点9转换为关键节点3,该关键节点3表示出局 部子图3(在图中通过M3来表示），如图5F所示。然后，可W在步骤S250中，判断出当前普 通节点（即，普通节点11)与一个先前关键节点（即，关键节点1)满足匹配检测条件，并且 当前普通节点（即，普通节点11)与另一先前关键节点（即，关键节点2)不满足匹配检测 条件。本信息处理方法可W前进到步骤S260, W判断当前普通节点（即，普通节点11)与先 前关键节点（即，关键节点1)是否数据匹配。
[0121] 在步骤S260中，判断当前普通节点与先前关键节点是否数据匹配。
[0122] 当判断出当前普通节点11与先前关键节点1之间满足所述匹配检测条件时，为了 进行数据匹配检测，可W首先，读取在先前关键节点1中包括的整合观测数据。然后，可W 判断在当前普通节点11中包括的一顿观测数据与在先前关键节点1中包括的整合观测数 据是否匹配。如果当前普通节点11与先前关键节点之间数据匹配，则本信息处理方法可W 前进到步骤S270, W进行图优化过程。否则，本信息处理方法返回到步骤S210继续执行，W 获取新的一顿观测数据，直到无法再次获取到新的一顿观测数据为止。
[0123] 如图5F所示，通过在黑色实必圆表示的关键节点1中包括普通节点1-4中的观测 数据，可W构成局部的子图Ml。在通过灰色填充圆表示的当前普通节点11寻找闭环的时 候，只要与该关键节点1进行匹配就可W 了。也就是说，在搜索环路连接的时候，可W仅仅 选择关键节点作为潜在的环路匹配目标。送样做的原因在于，与在普通节点中包括的单顿 观测数据不同，在局部子图中包括大量的观测数据，送样在例如通过特征点来进行观测数 据匹配时，可W设置较高的匹配阔值，W增加环路检测的准确性。
[0124] 在一个示例中，可W直接基于当前普通节点11的位姿信息和先前关键节点1的位 姿信息来确定关于两者之间位姿变换的一个初始猜想U。例如，可W根据机器人的码盘运 动、测距模型、W及高斯噪声来确定送个初始猜想U。
[01巧]接下来，可W利用送个初始猜想U，并且使用误差方程，来判断在所述当前普通节 点11中包括的一顿观测数据与在所述先前关键节点1中包括的整合观测数据是否匹配。
[0126] 在上述示例中，提供了用于进行环路检测的一种方式。然而，当机器人沿着很长的 路径而行进的时候，累计的运动误差变得非常大。在送种情况下，送个初始猜想可能距离可 W获得全局最优解的收敛区间很远。
[0127] 由于在顿间匹配算法中涉及的误差方程是一种求局部最优解的过程，所W如果送 个初始猜想U位于不理想的收敛区间时，即使在当前普通节点11中包括的当前观测数据与 在先前关键节点1中包括的子图之间具有很大的相似性，该顿间匹配算法也可能落入性能 不佳的局部最优解，而无法得到全局最优解，从而不能准确地估计出环路匹配中的变换，最 终影响正常地形成闭环。
[012引图6图示了根据本发明实施例具体示例的用于闭环检测的局部搜索算法。
[0129] 如图6所示，在局部搜索算法中包括4个收敛区间。如图6所示，如果初始猜想位 于UO的位置，则该局部搜索算法关于误差方式最终得到的最优解仅仅是局部最优解SO。对 于初始猜想Ul和u3,该局部搜索算法关于误差方式最终得到的最优解也仅仅是局部最优 解Sl和S3。只有当初始猜想位于u2的位置时，该局部搜索算法关于误差方式最终得到的 最优解才可能是全局最优解s2。
[0130] 为了获得全局最优解W准确地形成闭环，在另一示例中，可W通过在当前普通节 点11与先前关键节点1之间自适应地内插多个初始猜想，使得尽量在每一个收敛区间中都 具有一个初始猜想，W便保证能够在局部搜索算法中获得一全局最优解，从而精确地检测 闭合环路。
[0131] 图7图示了根据本发明实施例具体示例的基于内插方式的环路检测算法的原理。
[0132] 如图7所示，例如，可W在当前机器人的位姿（如灰色大H角所示）与潜在可能形 成闭环的关键节点位姿（如黑色大H角所示）之间线性地插入多个位姿（如灰色小H角所 示），W获得多个初始猜想。
[0133] 也就是说，可W根据当前普通节点11的位姿信息和先前关键节点1的位姿信息来 生成多个初始估计值。例如，可W首先，根据当前普通节点11的位姿信息和先前关键节点 1的位姿信息来确定所述当前普通节点11与所述先前关键节点1之间的间隔距离；然后， 在一个预定值与所述间隔距离之间选择多个值，作为所述多个初始估计值。
[0134] 具体地，可W在预定值0与所述间隔距离之间按照相等间隔地插入N个值，即N个 位姿，
[0135]
[0136] 其中，N是待插入的初始估计值的个数，「1是取上限函数，Dis是当前普通节点11 与先前关键节点1之间的间隔距离，并且1；。。。是所述误差方程的平均收敛区间，即如图6所 示的收敛区间1-4的平均值。
[0137] 接下来，可W利用所述多个初始估计值，并且使用误差方程，来判断在所述当前普 通节点11中包括的一顿观测数据与在所述先前关键节点1中包括的整合观测数据是否匹 配。
[0138] 送样，通过在当前普通节点与先前关键节点之间自适应地内插多个初始猜想，来 保证能够在局部搜索算法中获得一全局最优解，从而精确地检测闭合环路。
[0139] 优选地，为了减小可移动设备或区域服务器的计算量，可W对初始猜想的个数进 行限制。
[0140] 在一个示例中，可W通过考虑该当前节点的误差范围来限制参加顿间匹配算法的 初始猜想。为此，可W在生成每个普通节点的时候，进一步确定当前普通节点的误差范围。
[0141] 具体地，可W在步骤S220中使用例如扫描匹配方法来对该当前普通节点进行顿 间匹配的同时，通过该扫描匹配方法的运算结果来计算当前普通节点的方差和/或协方 差。该方差和协方差可W指示出所计算得到的当前普通节点在地图中的坐标（x，y)与该当 前节点在未知环境中的真实坐标之间的误差范围。
[0142] 对于普通节点1来说，由于当前机器人处于初始位置（即，位置1)处，所W-般情 况下，所得到的该节点1的坐标信息往往并不存在任何误差，所W在对该节点1的误差范围 进行确定之后，很可能得知该节点1的误差范围为0。对于普通节点2来说，由于此时机器 人已经离开自己的初始位置而到达未知环境中的一不确定位置，所W通过SLAM而计算得 到的该机器人的坐标位置必须存在一定误差，并且该误差也将随着机器人的不断运动，计 算的不断执行而逐渐累加。换言之，当前普通节点的误差范围必然大于历史普通节点中前 面第一个