CN112639502A - 机器人位姿估计 - Google Patents

Abstract

本发明涉及一种用于估计机器人位姿的装置和方法，其中，基于第一位姿估计或第二位姿估计或所述第一位姿估计和所述第二位姿估计的组合确定所述机器人位姿的当前位姿估计，其中，所述第一位姿估计是基于当前位姿分布；其中，基于所述当前位姿分布确定所述第一位姿估计对所述当前位姿估计的贡献和所述第二位姿估计对所述当前位姿估计的贡献。因此，将每个位姿估计的强度合并到所述当前位姿估计中。此外，描述了改进的粒子滤波方法和系统，其中，基于一组参考特征与一组观察特征之间的相似度评分更新所述粒子的权重，其中，在所述机器人的环境中根据所述机器人的一个或多个传感器的传感器数据检测该组观察特征。利用相似度评分解决了机器人环境中有关动态对象的问题，提高了特征可辨别性。

Description

机器人位姿估计

本发明涉及稳健和有效地估计机器人，特别是车辆的位姿。所公开的系统和方法可用于车辆的实时定位。

背景技术

随着机器人系统在日益增加的非结构化环境中运行，移动机器人定位在机器人学中变得越来越重要。目前，移动机器人系统的应用包括以不同应用作为以下各项的移动平台：行星探测、用于深海探测的潜水器、空中或密闭空间(如矿井)中的机器人车辆、在城市环境中自主行驶的汽车，以及在涉及与人类互动的高度动态环境中工作的安卓机器人。

这些以及其他应用中的移动机器人需要在本质上不可预测的环境中运行，在这些环境中，移动机器人通常必须在由静态和动态对象构成的环境中导航。此外，即使是静态对象的位置也常常是未知的，或者所知晓的位置也具有不确定性。因此，对机器人进行十分精准的定位至关重要，通常使用机器人传感器和/或外部传感器的传感器数据。定位问题涉及根据传感器数据，通常使用环境地图，在外部参考框架或全局坐标系中估计机器人的坐标及其一般方向，共同形成所谓的位姿。

为了说明该定位过程固有的不确定性，包括不可避免的测量误差和传感器噪声，通常采用概率方法，其中，机器人瞬时位姿的估计(也称为置信度)通过所有位置和潜在方向的空间(即所谓的状态空间)内的概率密度函数来表示。

针对定位问题常用的概率方法涉及递归贝叶斯估计，也称为贝叶斯滤波器。利用贝叶斯滤波器，基于最新获得的传感器数据或观察值持续更新机器人的概率密度函数。递归算法由两部分组成：预测和更新。假设机器人的真实状态X是未被观察的马尔可夫过程，测量值Z是隐马尔可夫模型的观察状态。

预测步骤使用系统模型p(X_t|X_t–1)(也称为运动模型)，在根据时间t–1的先前概率分布函数p(X_t–1|Z_1:t–1)(所谓的先前后验)给定先前观察值Z_1:t–1的条件下，预测在时间t的概率分布函数p(X_t|Z_1:t–1)(所谓的当前先验)，其中，由于噪声，预测的概率分布函数是扩散的。更新步骤根据新观察数据更新预测，以在给定当前时刻的观察值Z_1:t的条件下，计算当前概率分布函数p(X_t|Z_1:t)(所谓的当前后验)。

当前后验与测量似然函数p(Z_t|X_t)和证据p(Z_t|Z_1:t–1)归一化的当前先验p(X_t|Z_1:t-1)的乘积成正比。通过似然函数p(Z_t|X_t)，测量模型进入计算阶段，其中，该测量模型表示在给定时间t的真实状态X_t的条件下观察值Z_t的条件概率。根据当前后验p(X_t|Z_1:t)，可以通过确定当前概率分布函数的最大值或应用最小均方误差(minimum mean-square error，MMSE)方法等，确定在时间t下真实状态X_t的最优估计

即机器人位姿的估计。然后，可以使用位姿估计在机器人的环境中操作机器人。

如果系统模型和测量模型都是线性的，且后验是高斯型的，则贝叶斯滤波器变成卡尔曼滤波器。对于具有加性噪声的非线性系统，可采用使用一阶泰勒级数展开的局部线性化来提供扩展卡尔曼滤波器(extended Kalman filter，EKF)。

例如，S.Thrun、W.Burgard和D.Fox于2018年在麻省理工出版社发表的《Probabilistic Robotics》的第7章中描述了扩展卡尔曼滤波定位算法。使用速度运动模型(其中，控制数据u_t由速度给出)或使用里程计运动模型(其中，控制数据u_t由传感器测量值代替)来实现给定先前状态X_t–1和控制数据u_t的状态转换概率的运动模型，即系统模型，p(X_t|u_t,X_t–1)。此外，通过机器人的环境地图m扩展所述运动模型，以创建可近似分解为如等式(1)中的基于地图的运动模型p(X_t|u_t,X_t–1,m)：

p(X_t|u_t,X_t–1,m)＝η p(X_t|u_t,X_t–1)p(X_t|m) (1)

其中，η为归一化因子。第二个术语p(X_t|m)表示位姿或状态X_t与地图m的“一致性”。

还通过环境地图m扩展描述了形成过程的测量模型，通过该形成过程在物理世界中生成传感器测量值，以定义条件概率分布函数p(Z_t|X_t,m)，其中，X_t是机器人位姿，Z_t是时间t下的测量值。

速度运动模型和里程计运动模型都受到噪声的影响，当机器人移动时，噪声会导致不确定性增加。此外，机器人里程计通常受漂移和滑移的影响，使得机器人的内部里程计使用的坐标与物理世界坐标之间没有固定的坐标转换。确定机器人相对于给定环境地图的位姿通常会增加位姿估计的确定性，但也会大大增加底层算法的计算复杂性。因此，用于解决移动机器人定位问题的一般已知的算法通常无法实时执行，并且常常会因为机器人移动而导致精度丧失的问题。

这对于自动驾驶车辆而言尤其成问题，出于安全原因，车辆定位的高精度或高准确性至关重要。此外，经济因素在确保车辆可行商用部署方面也起到重要作用，所述可行商用部署一般作为高级驾驶辅助系统(Advanced Driver-Assistance Systems，ADAS)解决方案的一部分实现。

S.Kuutti、S.Fallah、K.Katsaros、M.Dianati、F.Mccullough和A.Mouzakitis在2018年4月于IEEE Internet of Things Journal，第5卷，第2期，第829-846页发表的最新综述文章《A Survey of the State-of-the-Art Localization Techniques and TheirPotentials for Autonomous Vehicle Applications》对支持自动驾驶的最新车辆定位系统进行了综述。低成本系统大多使用全球定位系统(Global Positioning System，GPS)、惯性测量单元(Inertial Measurement Unit，IMU)和(单目或立体)摄像机等传感器。这些低成本系统的问题在于其准确性相当低。例如，在1m以下的绝对平均误差百分比为80％，平均绝对平均误差在纵向或驾驶方向为1.43m，在横向为0.58m。车联网(Internet ofVehicles，IoV)规划及控制部门要求纵向最大误差为1m，横向最大误差为0.5m。虽然低成本系统很吸引人，因为在大多数情况下，所涉及的传感器已经嵌入如今的车辆中，但缺乏准确性阻碍了在自动驾驶车辆中广泛实施。

此外，目前的最新系统仅估计车辆相对于二维平面坐标以及可能车辆方位的位姿，即三个最大自由度。在潜在的未知三维地形中，期望估计全部6个自由度(degrees offreedom，DoF)位姿，即三维位置和三维方向，包括滚转、俯仰和偏航等。

最后，由于建筑物或树木阻挡了GPS信号，因此GPS传感器的传感器数据经常不可用，而使用IMU传感器获得的里程计测量值存在固有的漂移问题。因此，需要一种可靠的车辆定位方法和系统来满足自动驾驶车辆的安全要求。

发明内容

本发明提供了一种显著提高上述低成本系统的性能的方法，从而解决上述技术问题。所公开的方法和系统特别对贝叶斯滤波框架进行了一些改进，增强了定位过程的鲁棒性。

所公开的方法和系统不仅允许以几个(例如，六个)自由度(DoF)进行定位，还可以用于以10Hz或更高速率运行，因此适用于实时实施。这些方法和系统可用于确定自动或非自动车辆或其他类型机器人的位姿。因此，它们的潜在应用领域并不限于自动驾驶。本文中称为“装置”的设备都可以是合作设备的系统。所述装置可以包括处理电路，用于执行与相应装置相关联的各种数据或信号处理操作。下文详细描述这些操作。所述处理电路可以是软件和硬件的组合。例如，所述处理电路可以包括一个或多个处理器和非易失性存储器，其中，所述非易失性存储器中存储有可由所述一个或多个处理器执行的程序代码。所述程序代码使得所述处理电路在由所述一个或多个处理器执行时执行相应操作。

根据本发明的一个方面，提供了一种用于估计机器人位姿的装置，其中，所述装置用于基于第一位姿估计或第二位姿估计或所述第一位姿估计和所述第二位姿估计的组合确定所述机器人的当前位姿估计，其中，所述第一位姿估计是基于所述机器人的当前位姿分布；其中，所述第一位姿估计对所述当前位姿估计的贡献和所述第二位姿估计对所述当前位姿估计的贡献是基于所述当前位姿分布确定的。因此，可以获得更精确、更可靠的位姿估计。在一个实施例中，所述装置用于将当前位姿估计作为多个位姿估计的加权和确定，其中，所述多个位姿估计中的每个位姿估计具有所述加权和中的相应权重；其中，所述多个位姿估计包括基于当前位姿分布的第一位姿估计和一个或多个其他位姿估计；其中，所述多个位姿估计的所述权重是基于所述当前位姿分布。

所述第二位姿估计可以基于以下一项或多项：根据一个或多个先前位姿估计，或从位置传感器的传感器数据和定向传感器的传感器数据中的至少一个导出的全局位姿估计进行预测。所述预测可以包括航位推算。

所述第一位姿估计的所述贡献和所述第二位姿估计的所述贡献可以基于所述当前位姿分布，特别是关于所述第一位姿估计的置信度度量值来确定。

在确定所述当前位姿分布的所述置信度度量值超过阈值时，只有所述第一位姿估计对所述当前位姿估计贡献。

根据另一个方面，所述装置还可以用于基于所述当前位姿分布的所述置信度度量值来调整所述阈值。可以重复(例如，周期性或连续地)调整所述阈值。

根据另一个方面，响应于所述当前位姿分布的所述置信度度量值明显高于所述阈值，增大所述阈值；或者响应于所述当前位姿分布的所述置信度度量值明显低于所述阈值，减小所述阈值。当所述置信度度量值超过所述阈值加上非负第一偏移量时，可以认为所述置信度度量值明显高于所述阈值。所述第一偏移量可以为零。类似地，当所述置信度度量值低于所述阈值减去非负第二偏移量时，可以认为所述置信度度量值明显低于所述阈值。所述第二偏移量可以为零。

从增加所述阈值到减少所述阈值的转换以及从减少所述阈值到增加所述阈值的转化可以延迟相应的延迟时间。

或者，所述第一位姿估计的所述贡献和所述第二位姿估计的所述贡献可以基于相应位姿估计的置信度度量值确定。

根据本发明的另一个方面，提供了一种用于估计机器人位姿的装置，其中，所述装置用于：确定所述机器人的多个当前假设位姿，特别使用预测；确定所述多个当前假设位姿中的每个当前假设位姿的权重；基于所述多个当前假设位姿及其权重确定所述机器人的当前位姿估计；其中，确定所述多个当前假设位姿中的每个当前假设位姿的所述权重包括计算相似度评分，其中，所述相似度评分是一组参考特征与一组观察特征之间的相似度度量值。所述一组观察特征可以包括在所述机器人的环境中检测到的特征。所述特征可以由所述机器人的一个或多个传感器检测。所述传感器可包括遥感装置，例如摄像机、雷达传感器、声纳传感器或其组合。所述装置可以包括用于进行所述位姿估计的处理电路。因此，可以获得可靠的位姿估计。

每个参考特征和每个观察特征均可包括一个或多个特征描述符。

每个参考特征和每个观察特征均可包括一个或多个特征类，并且每个特征类包括概率值；可以基于所述参考特征的所述一个或多个特征类及其概率值和所述观察特征的所述一个或多个特征类及其概率值，计算所述相似度评分。

根据另一个方面，每个特征类可以与现实世界元素的类别相关联。所述现实世界元素的类别可以是“树”、“天空”、“人”、“车辆”或“建筑物”等。

根据另一个方面，每个参考特征还可包括空间固定(space-fixed，SF)位置坐标，每个检测特征还可包括物体固定(body-fixed，BF)位置坐标，其中，相对于所述机器人定义所述BF位置坐标；其中，计算所述相似度评分包括：基于当前假设位姿，在所述SF位置坐标与所述BF位置坐标之间进行映射。坐标可以是多维的。例如，该坐标可以是二维空间(例如，对应于地球表面)，或三维空间(例如，地球表面上的三维空间)中的一个点。

当所述分布满足可靠性条件时，可以基于所述相似度评分的分布确定所述当前假设位姿的所述权重。所述分布可以是所述相似度评分的频率分布或归一化频率分布。

根据另一个方面，当所述分布不满足可靠性条件时，可以不基于所述相似度评分的所述分布确定所述当前假设位姿的所述权重。

根据另一个方面，所述装置还可包括位置传感器和定向传感器中的至少一个，其中，基于从所述位置传感器的传感器数据和所述定向传感器的传感器数据中的至少一个导出的全局位姿估计，进一步调整所述当前假设位姿的所述权重。例如，所述位置传感器或所述定向传感器或两者可以基于视觉、声音、雷达、卫星信号、惯性或其组合。

根据本发明的另一个方面，用于估计机器人位姿的装置用于：基于一个或多个第一导航测量值生成所述机器人的第一位姿分布；基于所述第一位姿分布和精细化位姿分布的当前实例生成所述机器人的第二位姿分布；基于所述第二位姿分布和一个或多个第二导航测量值生成所述精细化位姿分布的下一个实例；基于所述精细化位姿分布的所述下一个实例确定所述机器人的位姿估计。因此，可以将新的分布峰值添加到现有位姿分布中。在现有位姿分布(即，所述精化位姿分布的当前实例)是错误的(例如，由于传感器读数错误或缺少传感器读数，例如在缺少摄像机或卫星数据一段时间之后)情况下，在所述精化分布的所述下一个实例中存在新增加的峰值可以使所述装置能够“回收”，即，找到准确的新位姿估计。因此，可以获得可靠的位姿估计。

根据另一个方面，所述精细化位姿分布的所述当前实例和所述下一个实例均由一组假设位姿和关联权重表示；其中，表示所述当前实例的一组假设位姿和关联权重和表示所述下一个实例的一组假设位姿和关联权重包括相同数量的所述假设位姿。

根据另一个方面，在生成所述第二位姿分布时，所述精细化位姿分布的所述当前实例对所述第二位姿分布的贡献大于所述第一位姿分布。例如，所述第二位姿分布可以是所述第一位姿分布和所述精细化位姿分布的所述当前实例的加权和，其中，所述精细化位姿分布的所述当前实例的权重大于所述第一位姿分布。例如，所述精细化位姿分布的所述当前实例和所述第一位姿分布的相对权重可以分别为1减X(例如，0.95)和X(例如，0.05)，其中X小于0.5。例如，所述精细化位姿分布的所述当前实例可以由一组样本(例如，95个样本)(即，95个假设位姿)表示，所述第一位姿分布由一组样本(例如5个样本)表示，其中，所有样本(在该示例中共有100个样本)具有相同的样本权重(例如0.01)。在该示例中，然后可以将所述第二位姿分布作为由来自所述两个组的所有样本组成的组。

根据另一个方面，所述装置用于不基于所述精细化位姿分布生成所述第一位姿分布。

根据另一个方面，所述装置用于通过以下一项或多项生成所述一个或多个第一导航测量值：基于卫星的位姿估计、基于惯性的位姿估计、基于视觉的位姿估计或用户输入。所述第一导航测量值可包括全局位姿估计。例如，可以从位置传感器的传感器数据和定向传感器的传感器数据中的至少一个导出所述全局位姿估计。

根据另一个方面，所述装置用于通过以下一项或多项生成所述一个或多个第二导航测量值：基于卫星的位姿估计、基于惯性的位姿估计、基于视觉的位姿估计或里程计位姿估计。

根据本发明的一个方面，提供了一种机器人，特别是车辆，尤其是自动驾驶车辆，包括根据上述任一方面所述的装置。

根据本发明的另一个方面，提供了一种用于估计机器人位姿的方法，其中，所述方法包括：基于第一位姿估计或第二位姿估计或所述第一位姿估计和所述第二位姿估计的组合确定所述机器人的当前位姿估计，其中，所述第一位姿估计是基于所述机器人的当前位姿分布；其中，所述第一位姿估计对所述当前位姿估计的贡献和所述第二位姿估计对所述当前位姿估计的贡献是基于所述当前位姿分布确定的。所述方法还可包括确定所述当前位姿分布，特别使用粒子滤波。

所述第二位姿估计可以通过以下一项或多项确定：根据一个或多个先前位姿估计进行预测，或从位置传感器的传感器数据和定向传感器的传感器数据中的至少一个导出全局位姿估计。

所述第一位姿估计的所述贡献和所述第二位姿估计的所述贡献可以基于所述当前位姿分布，特别是关于所述第一位姿估计的置信度度量值来确定。

在确定所述当前位姿分布的所述置信度度量值超过阈值时，只有所述第一位姿估计对所述当前位姿估计贡献。

根据另一个方面，所述装置还可包括：基于所述当前位姿分布的所述置信度度量值来调整所述阈值。可以重复(例如，周期性或连续地)调整所述阈值。

根据另一个方面，响应于所述当前位姿分布的所述置信度度量值明显高于所述阈值，增大所述阈值；或者响应于所述当前位姿分布的所述置信度度量值明显低于所述阈值，减小所述阈值。当所述置信度度量值超过所述阈值加上非负第一偏移量时，可以认为所述置信度度量值明显高于所述阈值。所述第一偏移量可以为零。类似地，当所述置信度度量值低于所述阈值减去非负第二偏移量时，可以认为所述置信度度量值明显低于所述阈值。所述第二偏移量可以为零。

从增加所述阈值到减少所述阈值的转换以及从减少所述阈值到增加所述阈值的转换可以延迟相应的延迟时间。

或者，所述第一位姿估计的所述贡献和所述第二位姿估计的所述贡献可以基于相应位姿估计的置信度度量值确定。

根据本发明的一个方面，提供了一种用于估计机器人位姿的装置，其中，所述方法包括：确定所述机器人的多个当前假设位姿，特别使用预测；确定所述多个当前假设位姿中的每个当前假设位姿的权重；基于所述多个当前假设位姿及其权重确定所述机器人的当前位姿估计；其中，确定所述多个当前假设位姿中的每个当前假设位姿的所述权重包括计算相似度评分，其中，所述相似度评分是一组参考特征与一组观察特征之间的相似度度量值。所述一组观察特征可以包括在所述机器人的环境中检测到的特征。所述特征可以由所述机器人的一个或多个传感器检测。所述传感器可包括遥感装置，例如摄像机、雷达传感器、声纳传感器或其组合。

每个参考特征和每个观察特征均可包括一个或多个特征描述符。

每个参考特征和每个观察特征均可包括一个或多个特征类，并且每个特征类包括概率值；可以基于所述参考特征的所述一个或多个特征类及其概率值和所述观察特征的所述一个或多个特征类及其概率值，计算所述相似度评分。

根据另一个方面，每个特征类可以与现实世界元素的类别相关联。所述现实世界元素的类别可以是“树”、“天空”、“人”、“车辆”或“建筑物”等。

根据另一个方面，每个参考特征还可包括空间固定(SF)位置坐标，每个检测特征还可包括物体固定(BF)位置坐标，其中，相对于所述机器人定义所述BF位置坐标；其中，计算所述相似度评分包括：基于当前假设位姿，在所述SF位置坐标与所述BF位置坐标之间进行映射。坐标可以是多维的。例如，该坐标可以是二维空间(例如，对应于地球表面)，或三维空间(例如，地球表面上的三维空间)中的一个点。

当所述分布满足可靠性条件时，可以基于所述相似度评分的分布确定所述当前假设位姿的所述权重。所述分布可以是所述相似度评分的频率分布或归一化频率分布。

根据另一个方面，当所述分布不满足可靠性条件时，可以不基于所述相似度评分的所述分布确定所述当前假设位姿的所述权重。

根据另一个方面，所述方法还可包括：基于从位置传感器的传感器数据和定向传感器的传感器数据中的至少一个导出的全局位姿估计，进一步调整所述当前假设位姿的所述权重。例如，所述位置传感器或所述定向传感器或两者可以基于视觉、声音、雷达、卫星信号、惯性或其组合。

根据一个方面，估计机器人位姿的方法包括：基于一个或多个第一导航测量值生成所述机器人的第一位姿分布；基于所述第一位姿分布和精细化位姿分布的当前实例生成所述机器人的第二位姿分布；基于所述第二位姿分布和一个或多个第二导航测量值生成所述精细化位姿分布的下一个实例；基于所述精细化位姿分布的所述下一个实例确定所述机器人的位姿估计。

根据本发明的一个方面，提供了一种用于存储指令的计算机可读介质，其中，当所述指令在处理器上执行时，使得所述处理器执行根据上述任一方面所述的方法。

附图说明

下文参考附图和图式详细描述示例性实施例，其中：

图1示出了用于介绍本发明的基本粒子滤波过程；

图2示出了图1的基本粒子滤波过程的相关步骤；

图3示出了根据本发明的经改进粒子滤波过程，作为本发明的基本框架；

图4示出了根据图3的经修改粒子滤波过程的主要步骤，包括根据本发明第一实施例的位姿估计；

图5示出了根据图3的经改进粒子滤波过程的主要步骤，包括根据本发明第二实施例的位姿估计；

图6描绘了对于基于真实数据的测试用例，当前位姿分布的置信度度量值的阈值的时间行为；

图7示出了图4和图5的校正块的权重更新的第一阶段细节；

图8示出了根据权重更新处理的第一实施例的校正块的第二阶段细节；

图9示出了根据权重更新处理的第二实施例的校正块的第二阶段细节；

图10示出了根据权重更新处理的第三实施例的校正块的第二阶段细节；

图11示出了根据本发明的具有定位系统的车辆。

具体实施方式

本发明涉及移动机器人定位的一般技术领域，特别涉及车辆，尤其是自动驾驶车辆的实时定位。通过改进映射步长、稳定位姿估计和使底层算法准备好实时应用，本发明提供了一种显著提高低成本系统性能的方法。

更具体地，本发明对上文关于移动机器人定位问题描述的贝叶斯滤波框架进行了几项改进。

图1中示出了用于实现贝叶斯滤波器的基本粒子滤波过程，以对本发明进行描述。所描述的粒子滤波过程是基于众所周知的蒙特卡罗定位(也称为粒子滤波定位)，使用粒子滤波定位移动机器人。该过程使用粒子滤波器来表示可能状态的分布，其中每个粒子表示一种可能状态，即机器人所在位置的假设，也称为机器人的假设位姿。

后验概率分布函数(也称为概率密度函数)或后验信念bel(X_t)＝p(X_t|Z_1:t)由随机选取的加权样本(粒子)的集合

表示，具有M个假设位姿

及其相应的权重

对于M个样本，表征成为真实概率分布函数的等效表示。粒子滤波方法可以表示任意分布，并且可以同时追踪多个假设位姿。基于递归贝叶斯估计对粒子进行重采样。

在预测步骤110中，通过将模拟运动应用于时间t–1的粒子

中的每个粒子来确定当前先验

根据本发明，使用基于在时间t所述机器人的至少一个对应传感器的里程计测量值O_t的里程计运动模型p(X_t|O_t,X_t–1)，基于上一组样本

获得一组预测样本

从所述机器人的一个或多个里程计传感器获得里程计测量值O_t，以估计所述机器人的位置和/或方向随时间的变化。可提供一个或多个里程计传感器以测量至少一个位置坐标的变化和/或至少一个角坐标(如所述机器人的俯仰、滚转和偏航)的变化。里程计传感器的典型示例是运动传感器，如车轮编码器、旋转编码器、线性编码器、速度计、加速度计、陀螺仪和惯性测量单元(IMU)。IMU可用于同时确定最多6个DOF，即机器人位姿的全部三维位置和全部三维方向。此外，可以使用基于视觉的传感器，包括摄像机、雷达传感器、声纳传感器或其组合等遥感装置，以使用称为视觉里程计的技术计算里程。通常，可提供里程计传感器，以确定机器人位姿中与该位姿维度相同的变化。里程计传感器可包括与机器人一起提供的不需要环境测量的内部传感器。

除了运动传感器之外，里程计测量值O_t还可以包括：使用机器人的一个或多个基于卫星的传感器(即外部传感器)确定机器人的至少一个位置坐标。这类基于卫星的传感器利用全球导航卫星系统(global navigation satellite system，GNSS)(如GPS、GLONASS、北斗和伽利略)确定机器人的全局位置，即机器人相对于全局坐标系的位置。

在更新步骤120中，基于测量模型

根据先前后验的先前权重

更新当前权重

也称为当前重要性权重。对于机器人的每个当前假设位姿，即每个预测的粒子

计算机器人处于粒子状态时机器人将感知其传感器实际感测到的内容的概率

然后，将与所述概率成比例的当前权重

分配给每个预测粒子，其中应用归一化常数α来归一化权重。

在根据图1的基本过程中，所述测量模型完全基于一组观察特征Y_t与根据环境地图确定的一组参考特征

之间的映射。下面将结合图7至图10描述该地图匹配过程的细节。从所述机器人的至少一个基于视觉的传感器的传感器数据中提取所述观察特征Y_t，这在下文更详细地描述。基于视觉的传感器(在下文中也称为遥感装置)的典型示例是单目和立体摄像机、雷达传感器、光探测和测距(light detection and ranging，LiDAR)传感器(例如使用脉冲激光)、超声波传感器、红外传感器、或任何其他适于提供机器人环境成像测量的传感器。可以分析此类基于视觉的传感器输出的传感器数据以提取上述特征Y_t。特别地，地图可以包括关于地标、车道标记、建筑物、路缘和道路形的信息。如果使用多个基于视觉的传感器，则可以使用基于不同频率范围(如，光学和无线电)的不同地图。

在步骤130中，还可以对当前假设位姿

及其对应的当前权重

进行重采样，以避免概率分布函数的发生简并。在图1的重要性重采样步骤130中，基于当前信念bel(X_t)(即当前位姿分布)或当前后验p(X_t|Z_1:t)对粒子进行重采样，从而生成下一迭代或帧t+1的新样本集合

如本领域所知，在重采样期间，增加具有高重要性权重的粒子，消除具有低重要性权重的粒子，从而生成一组新样本。因此，在定位过程中，粒子数目M保持恒定，粒子权重保持有限。在这里以及下文中，适用时，术语“当前假设位姿”和“当前权重”是指在重要性重采样之前和之后的粒子及其对应的权重，因为重采样步骤在很大程度上维持了概率分布函数。

基于当前假设位姿和相应当前权重，可以通过应用(全局或局部)最大后验估计方法来估计帧t的当前迭代时机器人的当前位姿。或者，可以使用最小均方误差准则。此外，可以使用简单平均数(均值)和最大后验估计周围的窗口内的均值(鲁棒均值)来确定当前位姿估计

图2示出了图1的基本粒子滤波过程的相关步骤。块210、220和230与图1中的概念相同，表示为预测、校正和重采样。针对基于视觉的传感器数据的每个时间步骤或每一帧t，将预测210、校正220和重采样230的序列迭代。每次迭代的输入1至4由图2中的虚线框突出显示。图中还示出了块210、220、230和250的输出11、12、13和15。输出13表示精细化位姿分布。所述精细化位姿分布用于计算机器人的当前位姿。在本发明中，不同时间点的位姿分布值可称为位姿分布的实例。所述精细化位姿分布的当前实例(即，最新可用实例)也可在本文中称为当前位姿分布。

从先前后验概率分布函数的粒子滤波表示开始，即先前粒子

和相应的先前权重

在预测步骤210中应用使用里程计测量值O_t的运动模型，以确定预测样本

使用预测样本

和先前权重

作为输入，校正步骤220在观察特征Y_t与地图或参考特征

之间进行地图匹配以确定更新权重

然后，在重采样步骤230中对预测样本和相应的更新权重进行重采样，以产生重采样粒子

和相应的平衡权重

基于这些重采样的当前假设位姿和相应的当前权重，在位姿估计步骤250中确定当前位姿估计

通常必须在动态环境中进行移动机器人定位，在所述动态环境中，除所述机器人之外的其他对象和/或主体可能随着时间改变其位置或配置。可能影响位姿估计的更持久变化的示例是人、日光变化、可移动家具、其他车辆(特别是停泊车辆)、门等。这些动态对象通常不由通常静态参考地图中的参考特征表示，因此，在执行上述更新步骤时，可能会导致映射错误。此外，桌子或椅子的边缘等不同对象的特征可能无法使用标准特征向量进行区分。在现实世界环境中存在动态对象和观察可辨别性的一般问题导致在地图匹配过程中出现错配。因此，上述过程可能导致位姿估计不正确。

此外，上文结合图1和图2描述的粒子滤波过程可能由于其校正机制在某些情况下不稳定。最后，可能并非任何地方都可获得基于卫星的传感器信号。例如，高楼大厦、隧道和树木可能屏蔽车辆的GPS传感器与至少一些GPS卫星的连接。因此，至少在一些城市地区，通常无法获得充足的GPS信号。因此，基于里程计的预测容易产生漂移误差。因此，期望在无法获得基于卫星的传感器信号的区域，也能够使用上述粒子过滤过程。

为了解决上述技术问题，本发明对粒子滤波过程进行了修改，如图3所示。预测步骤310基于时间t机器人的至少一个对应里程计传感器的里程计测量值O_t和上一组样本

来确定一组预测样本，该预测步骤与图1中的预测步骤110相同，因此，为了清楚起见省去重复描述。

然而，与更新步骤120相比，通过在确定更新权重

时考虑在时间t的额外测量值Z_t，扩展了更新步骤320。特别地，可以考虑至少一个基于卫星的传感器和/或至少一个基于惯性的传感器的观察数据Z_t，以包括相应的概率评分。如图3所示，可以根据如下等式(2)确定当前权重：

其中，α表示归一化因子，Y_t表示一组观察特征，

表示一组参考特征。此外，机器人位姿写为矢量

其中T表示转置；

表示全局坐标系中的一个、两个或三个位置坐标，如x、y和z坐标；

表示相对于全局坐标系方向的一个、两个或三个旋转坐标，如俯仰、滚转和偏航。同样，

表示在全局坐标系中对应数量位置坐标的测量值；

表示在全局坐标系中对应数量旋转坐标的测量值。因此，测量矢量可以写为

测量值Z_t是指使用至少一个基于卫星的传感器和/或至少一个基于惯性的传感器获得的全局位姿测量值。例如，可以使用位置传感器(如GPS传感器和/或加速度计)测量位置

类似地，可以使用旋转传感器(如陀螺仪)测量方向

使用惯性测量单元，可测量完整的全局位姿Z_t，多达6个DOF。此外，在确定更新权重时可以包括不同传感器的多个测量值。在用于确定之前，可以将测量的传感器数据提交到滤波过程。

如图3的重采样步骤330所示进一步修改图1的重采样步骤130：仅由当前信念bel(X_t)生成M个粒子总数的一小部分(如95％)，使用不基于基于卫星的传感器数据的全局位姿测量值Z_t和/或全局位姿测量值G_t对一些粒子(如5％)进行重采样。例如，全局位姿测量值G_t可以基于使用基于视觉的传感器数据的图像处理和/或基于惯性的传感器数据。特别地，全局位姿测量值G_t可以与全局位姿测量值Z_t无关，即基于未包括在全局位姿测量值Z_t中的传感器数据。由于全局位姿G_t不是由基于卫星的传感器数据导出的，因此如果机器人的基于卫星的传感器无法接收GPS信号，仍可以确定全局位姿和相应的重采样粒子。

图4示出了根据图3的经修改粒子滤波过程的主要步骤，包括根据本发明第一实施例的位姿估计。如根据图2的基本粒子滤波过程所示，根据图4的经修改粒子滤波过程包括随时间或帧数t的循环迭代。该迭代的输入1到5在图4中以虚线框所示。此外，提供时间t–1的先前位姿估计

作为位姿预测步骤460的输入6。根据本发明第一实施例的经修改过程产生输出11至14、15a、15b、18和19，如图4所示。

从先前后验概率分布函数的粒子滤波表示开始，即先前粒子

和相应的先前权重

如同预测步骤210在预测步骤410中应用使用里程计测量值O_t的运动模型，以确定预测样本

使用预测样本

和先前权重

作为输入，校正步骤420在观察特征Y_t与地图或参考特征

之间进行地图匹配以确定更新权重

然而，除了地图匹配之外，校正步骤420还考虑了上文结合图3描述的全局位姿测量值

其使用至少一个基于卫星的传感器和/或至少一个基于惯性的传感器；和/或不使用基于卫星的传感器进行的全局位姿测量值

然后，在重采样步骤230中对预测样本和相应的更新权重进行重采样，以产生重采样粒子

和相应的平衡权重

然而，与基本粒子滤波过程的重采样步骤230不同，重采样步骤430仅由当前信念生成减少数量的(M–N个)粒子。如图4所示，使用回收粒子生成步骤480，不基于当前位姿分布确定剩余N个粒子。由全局位姿测量值

和/或不使用基于卫星的传感器进行的全局位姿测量值

产生这些回收粒子

及其各自的权重

如上所述，可以使用基于基于视觉的传感器数据的图像处理和/或使用基于惯性的传感器数据来获取全局位姿测量值G_t。根据一个特定实施例，全局位姿测量值G_t可以完全基于基于视觉的传感器数据。

在增强步骤485中，使用在步骤480中从一个或多个全局位姿测量值中回收的粒子

补充所述一组重采样粒子

以生成当前假设位姿

及其各自的当前权重

该当前假设位姿及其各自的当前权重将作为输入1提供给循环的下一个迭代。通过首先根据预测样本的权重

对预测样本进行排序，然后舍弃权重最小的样本，可以将所述一组预测样本

减少到M–N个样本。

添加N个粒子，这些粒子是从具有相应重要性权重

的其他可靠的全局位姿Z_t和/或G_t取样的，其中所述相应重要性权重基于这些位姿的可靠性，特别是协方差确定，从而确保将不基于粒子滤波过程的位姿估计纳入考虑。此类位姿估计的存在可以提高整个定位过程的准确度，并有助于处理重新定位、GPS信号丢失或不良等情况。

根据重采样430之前的当前位姿分布

或重采样430之后的当前位姿分布

在步骤440中计算当前位姿分布的置信度度量值，表示可以根据位姿分布明确确定机器人位姿的置信度。所述置信度度量值可以表示为作为当前信念的函数f导出的当前位姿估计

的后验概率，即后验概率(X_t)。如上所述，所述函数可以是最大后验估计。函数f的另一种可能性是识别表示后验概率的局部最大值的几个簇，其中位姿估计被选择为最可能局部最大簇的加权平均值。

当使用这种聚类方法时，可以根据等式(3)将当前位姿估计的置信度度量值或后验概率

作为最可能局部最大聚类的累积权重

计算：

其中M_c是属于局部最大簇的当前位姿分布的权重

的数量。

基于当前位姿分布的置信度度量值，图4所示的根据第一实施例的定位过程可以输出基于当前位姿分布的第一位姿估计15a；特别地可以不基于当前位姿分布确定的第二位姿估计；或第一位姿估计和第二位姿估计的组合。

根据如上所述的当前位姿分布

在位姿估计步骤450中确定第一位姿估计

其中，可以颠倒置信度度量值440和位姿估计450的计算顺序。在步骤455中，除了输出当前位姿估计15a之外，将从当前位姿分布导出的当前位姿估计存储在存储器单元等存储空间中。

根据第一实施例的当前定位过程还可以通过步骤460中的预测确定独立的第二位姿估计15b。可以基于一个或多个先前位姿估计和/或使用其他全局位姿估计进行预测。例如，可以根据两个或两个以上先前位姿估计外推第二位姿估计15b。在一个特定实施例中，可以进行航位推算以基于里程计测量值O_t和存储的先前位姿估计

确定第二位姿估计

在这里，函数g表示确定性运动模型，该确定性运动模型通过应用从里程计测量值O_t导出的位姿变化，根据在时间t–1机器人的位姿

预测在时间t机器人的位姿

也可以将第二位姿估计15b存储在存储器455中。

根据图4所示的具体实施例，输出第一位姿估计15a或第二位姿估计15b作为当前位姿估计

然而，本发明不限于该替代输出，而是可以输出第一位姿估计15a和第二位姿估计15b的组合，其中所述第一位姿估计和所述第二位姿估计对该组合的贡献可以基于当前位姿分布的置信度度量值确定。例如，可以根据等式(4)确定组合的位姿估计

其中，

表示第一位姿估计15a，

表示第二位姿估计15b。

在最新技术中，航位推算已用于粒子滤波器的预测步骤，例如E.J.Krakiwsky、C.B.Harris和R.V.C.Wong，《A Kalman filter for integrating dead reckoning,mapmatching and GPS positioning》，Position Location and Navigation Symposium，1988，Record.Navigation into the 21^st Century，IEEE PLANS’88，IEEE，Orlando，FL，1988，第39-46页中所述。然而，仅仅在预测步骤中使用航位推算不能解决上述第一位姿估计的不稳定性问题。

因此，根据第一实施例的定位过程基于当前位姿分布的置信度度量值将独立的第二位姿估计15b与第一位姿估计15a结合，或将第一位姿估计15a替换为第二位姿估计15b。根据图4中描绘的具体实施例，如果当前位姿分布的置信度度量值

超过可能与时间相关的阈值

则使用第一位姿估计15a作为当前位姿估计

否则，可使用第二位姿估计15b作为当前位姿估计

此外，如果置信度度量值小于或等于阈值，则可以使用从航位推算导出的第二位姿估计15b与可以独立导出的其他位姿估计的组合。在此处和下文中，假设置信度度量值和阈值均为正标量。对于一般标量或偶向量，可以通过在评估条件之前计算绝对值或应用范数来实现上述条件。

如上所述，阈值

可以是时间相关的，并在时间t＝0使用预定值初始化。然后，可以根据函数

特别基于当前位姿分布的置信度度量值，重复(例如周期性地或连续地)调整阈值。例如，可以根据当前位姿分布的置信度度量值是否超过阈值来增大或减小阈值。例如，阈值可根据等式(5)变化：

其中，

是时间t^升高的单调递增函数，

是时间t^降低的单调递减函数。两个函数的可能结构可以是等式(6)中的指数函数形式：

其中

表示固定偏移量，

表示变化大小，c表示衰减因子。还可以只有在置信度度量值比阈值高非负第一偏移值，或者比阈值低非负第二偏移值时调整阈值，否则该阈值保持恒定。

从增加所述阈值到减少所述阈值的转换以及从减少所述阈值到增加所述阈值的转换可以延迟相应的延迟时间。应用于调整阈值函数的这种增添的延迟是为了避免位姿估计之间的假瞬时变化，从而可能导致与机器人位姿有关的跳跃。延迟将迫使一个估计在切换到另一个位姿估计之前在时段Δt内连续使用。

在等式(7)中给出了将延迟时间应用于阈值调整的示例：

其中当基于航位推算的位姿估计激活时，将t^升高重置为0；当基于粒子滤波的位姿估计激活时，将t^降低重置为0。当满足

时，t^升高增加，否则t^降低增加。

图6示出了对于基于真实数据的测试用例，当前位姿分布的置信度度量值的阈值

的时间行为。图中用竖直虚线表示了基于粒子滤波和基于航位推算确定位姿估计的两种典型阶段。在粒子滤波阶段，根据函数

提高阈值

等效地，在航位推算阶段，根据函数

减小阈值

根据当前位姿分布的置信度度量值确定可互换地应用基于粒子滤波的第一位姿估计和基于航位推算或不基于粒子滤波的另一种估计过程的第二位姿估计将这两种估计的强度结合起来。已知，航位推算在短时间段内是稳定的位姿估计，但如果在较长的时间段内应用，则容易出现“漂移”现象。另一方面，粒子滤波没有出现这种漂移现象，但由于校正步骤中的测量或观察更新不可靠，遭受跳跃或不稳定，即位姿估计显著变化。如在本实施例中，结合来自不同方法的两个位姿估计增加了定位过程的整体稳定性。

图5示出了根据图3的经改进粒子滤波过程的主要步骤，包括根据本发明第二实施例的位姿估计。图4中用相同参考符号标注的方法步骤与上文关于第一实施例所述的方法步骤相同，因此不再赘述。然而，与图4所示的第一实施例不同，根据图5的第二实施例始终在位姿估计步骤450中基于粒子滤波确定第一位姿估计15a并在预测步骤460中基于预测，特别是航位推算确定第二位姿估计15b。所述第一位姿估计和第二位姿估计在此处如上文结合图4所述确定。

根据所述第二实施例，在步骤470中，将当前位姿估计16计算为第一位姿估计15a和第二位姿估计15b的组合并输出。在步骤470中，基于各自位姿估计的置信度度量值，确定所述第一位姿估计和所述第二位姿估计对该组合的各自贡献。例如，可以根据等式(8)确定当前位姿估计

其中，

表示第一位姿估计15a，

表示第二位姿估计15b，并根据相对于相应位姿估计的协方差估计值

和值

确定各自的贡献。根据一个特定示例，可以根据如下等式(9)确定各自贡献：

和

协方差估计值

和

可以看作是相应位姿估计的置信度度量值。

例如，位姿估计的协方差可以通过如下方式估计：首先取重投影误差代价函数e在其收敛点周围的雅可比矩阵来估计，如等式(10)所示：

其次，对雅可比矩阵的逆和残差r(即其收敛点(x,y,z,滚转,俯仰,偏航)处的误差值)应用单调增加函数h，以确定位姿估计

的协方差，如等式(11)所示：

如上所述，粒子滤波受到机器人环境中动态对象的影响，存在观察可辨识性问题。根据本发明的定位过程通过修改基本粒子滤波过程的图2中的更新步骤220来解决这些问题，如图7至图10所示。

图7示出了根据本发明的图4和图5的校正块420的权重更新的第一阶段细节。最新技术在进行地图匹配时，根据最近邻原则，基于地图中一组观察特征点与一组参考特征点之间的距离评分计算似然函数。根据本发明的图7至图10的更新过程通过采用关于观察特征和参考特征的额外知识来显著扩展和修改该概念。

图7所示的更新过程的第一阶段使用三个嵌套循环，其中初始将各自循环中的指数m、p和q设置为1。外层循环对一组预测粒子

进行迭代，所述一组预测粒子从预测步骤410作为输入11提供到转换步骤421，其中M表示粒子总数。中间循环对一组观察特征

进行迭代，其中P表示观察特征的总数。最后，内层循环对一组参考特征

进行迭代，其中Q表示从地图中获取的参考特征总数。

通过对机器人的至少一个传感器的传感器数据的至少一个帧(也称为关键帧)进行特征检测和描述，从所述传感器数据中提取所述一组参考特征。所述传感器可以基于遥感，在本发明中称为基于视觉的传感器。如上所述，基于视觉的传感器可涉及摄像机，特别是立体摄像机、雷达传感器、光探测和测距(LiDAR)传感器(例如，使用脉冲激光)、超声波传感器、红外传感器、或任何其他适于对机器人环境提供成像测量(也称为测距)的传感器。可以根据时间索引将所得传感器数据组织成帧。可以分析这些帧中的一些或全部以检测和提取特征，然后可以将这些特征与通常从机器人环境的一个或多个参考地图离线提取的参考特征进行比较。

由于特征检测和匹配是机器视觉和机器人学中的两个重要问题，因此，本领域已知大量特征提取方法。一些比较普遍的方法是尺度不变量特征变换(Scale-InvariantFeature Transform，SIFT)、加速鲁棒特征(Speed Up Robust Feature，SURF)技术、二进制鲁棒独立基本特征(Binary Robust Independent Elementary Feature，BRIEF)方法，及面向FAST和旋转BRIEF(Oriented FAST and Rotated BRIEF，ORB)方法。例如，R.Mur-Artal和J.D.Tardós在IEEE Transactions on Robotics，第33卷，第5期，第1255-1262页，2017年10月发表的文章《ORB-SLAM2:an Open-Source SLAM System for Monocular,Stereo andRGB-D Cameras》中描述了还涉及同步定位与地图构建(Simultaneous Localization andMapping，SLAM)的先进ORB技术。

上述提取方法通常对传感器数据进行处理，以提取突出关键点位置处单个特征点或聚类特征点形式的特征。作为该过程的一部分，确定描述相应特征点的属性的特征描述符。因此，上述每个观察特征都可包括一个或多个特征点及其对应的特征描述符。此外，根据本发明的具体实施例的更新过程根据特征类利用与特征相关的语义信息。因此，每个特征可另外包括如下文更详细描述的一个或多个特征类。特别地，所述语义信息可用于区分汽车、交通标志、道路、建筑物等现实世界元素的特定类型或对象类别。相比之下，典型的特征可能仅识别线条、角落、边缘、不同的图案或外观不同的对象。通常，特征描述符提供为可具有高维度的特征向量。所述观察特征和所述参考特征两者的结构通常包括相同的信息，即特征点和对应的特征描述符，以及可能特征类，从而允许观察特征与参考特征之间匹配，产生与似然函数

关联的对应关系，这在此处和下文称为地图匹配。

所述一组参考特征

通常从一组参考图像中提取，与机器人环境相关；其中，使用与至少一个基于视觉的传感器所使用的成像方法相同的成像方法获取所述一组参考图像。然后，可以将所提取的参考特征作为所谓的基于特征的地图存储在数据库或存储介质中。

一般，地图基于特征和基于位置。基于位置的地图体积较大，因为它们包括地图区域内任何位置的特征描述符，而基于特征的地图仅指定特定位置的特征，特别是地图中所含关键对象的具体位置。根据本实施例的更新过程在基于特征的地图的上下文中进行描述，但是易于修改以应用于基于位置的地图。虽然基于特征的模型提取的信息相对较少，但由于特征提取器将高维传感器测量投射到低维空间，这种优势被基于特征的表示的高级计算属性抵消。

如上所述，移动机器人一般位于相对于其他对象或主体的位置和相对于日光变化等环境条件随时间变化的动态环境中。考虑到动态对象，观察特征与参考特征之间的匹配过程的一个潜在问题在于，用于检测参考特征的参考图像中存在的动态对象可能不在机器人的局部环境中存在，反之亦然。因此，基于这类动态对象进行特征提取和匹配会在更新过程中引入错误。因此，最新方法建议在进行特征提取和匹配之前，对机器人的至少一个基于视觉的传感器的传感器数据应用专用滤波器。然而，这种滤波进一步加重了底层处理已经很繁重的计算负载。

因此，根据本发明的特征/地图匹配过程设计一种替代方法，其中参考特征，更具体地说，参考图像经离线滤波以移除行人和其他车辆等动态对象。例如，可以基于上述特征类等语义信息来识别动态对象。

理想地，在执行定位过程之前已创建参考特征图，所述参考特征图可从存储介质或数据库获得以用于下文描述的匹配过程。然而，可能会存在移动机器人进入没有这种地图的地形或区域的情况。在这些情况下，可以修改所描述的方法以执行本领域中通常已知的同步定位与地图构建(SLAM)。在SLAM中，机器人在获取其环境地图的同时，也相对于该地图进行定位。当在机器人环境中检测到对象时，SLAM算法必须判断该对象与先前检测到的对象的关系。通过地图传播有助于对机器人进行定位的信息，从而改进了对地图中其他特征的定位。

可以针对SLAM问题应用根据本实施例的更新过程。在这种情况下，所述一组参考特征包括已经从机器人环境的当前地图中提取的那些特征。因此，所述参考特征可以包括可基于上述语义信息从所述组中移除的动态对象。或者，在假设动态对象在SLAM过程中保留在机器人环境中的情况下，所述动态对象可以保持为参考特征。所述算法还可以选择性移除一些基于相应的语义信息或特征类高度动态的动态对象，如行人和移动车辆，同时保留其他动态性较低的对象，如停泊车辆。

为了在所述一组观察特征与所述一组参考特征之间进行匹配，根据本发明的过程首先在图7中的步骤421中将必须相对于机器人的局部坐标系观察的观察特征Y_t转换为参考特征

的全局坐标系。对每个当前假设位姿

进行这种转换，以产生相应的经转换观察特征

换句话说，每个参考特征包括至少一个全局，即空间固定位置坐标，每个观察特征包括至少一个相对于机器人定义的物体固定位置坐标，其中，在转换步骤421中，基于当前假设位姿

进行空间固定位置坐标与物体固定位置坐标之间的映射。当然，这种转换也可能涉及旋转，以在局部坐标系与全局坐标系的旋转坐标之间进行映射。此外，还可以反方向进行转换，以将参考特征映射到机器人的局部坐标系中。

在进行坐标转换之后，根据本发明在步骤422中计算似然距离评分和相似度评分。例如，可以使用本领域中已知的基于最近邻的似然评分方法计算每个粒子m、每个观察特征p和每个参考特征q的距离评分

例如，可以使用ORB特征描述符的汉明距离计算相似度评分

然后，在步骤423中，如果特征不相似，则可以根据如下等式(12)使用相似度评分惩罚距离评分：

其中D^MIN是所述相似度评分超过所述相似度评分的阈值θ_S的情况下的最小距离评分，例如，可以在所述汉明距离的范围内选择所述相似度评分。

根据一个替代性实施例，每个特征包括一个或多个特征类，每个特征类包括传递语义信息的概率值。计算相似度评分考虑参考特征和观察特征的特征类及其各自的概率。特别地，每个特征类可以与现实世界元素的类别相关联，每个概率值表示特征属于相应特征类的概率。在这种情况下，相似度评分

可以确定为参考特征q和观察特征p具有相同关联关系(即语义标签)的概率。这种情况的阈值θ_S可能仅表示特定概率值。

可以分别对特征描述符和特征类或对特征描述符和特征类的组合计算相似度评分。特别地，可以将所述特征类集成到所述特征描述符中。由于距离评分仅对共享一定程度相似度的那些特征对(p,q)有意义，因此，可以根据等式(12)在步骤423中以类似方式对特征描述符和特征类应用距离评分的惩罚。如果在基于所述特征类计算所述相似度评分中包括所述语义信息，则如果对参考特征图进行预处理以移除动态对象，不需要分别从至少一个基于视觉的传感器的传感器数据中移除动态对象。例如，与行人有关的观察特征不会在地图中找到具有足够相似度的匹配，从而产生有意义的配对。因此，根据本实施例的基于相似度的方法高效且适用于实时实施。

在步骤424中，针对每个粒子m和每个观察特征p，确定和存储根据等式(13)与最近参考特征

相关的评分

和

然后，在步骤425中，根据等式(14)，累积每个粒子m的所得距离和相似度评分

和

作为图7所示的更新过程的第一阶段的结果，输出当前假设位姿

的距离和相似度评分的集合

可以以不同方式处理该距离和相似度评分的集合以更新先前权重

图8中示出了根据权重更新过程的第一实施例对先前权重进行更新。首先，将距离和相似度评分的集合分为距离评分的集合

和相似度评分的集合

然后，在步骤426中，通过计算相似度评分分布，例如通过将相似度评分集合对给定分布模型(如单边分布)拟合，进一步处理相似度评分，获得所述分布的

具有均值

和标准差

所述分布可以是所述评分的频率分布或归一化频率分布。基于相似度评分分布的参数

和

分布的可靠性条件可以定义为等式(15)：

其中，θ_R是可确定为相似度分布的尾阈值的阈值。

对于当前粒子集合

在步骤427中确定所述相似度评分的分布是否满足可靠性条件。如果所述分布不满足所述可靠性条件，则舍弃计算的距离评分，并且不基于相似度评分的分布确定更新的重要性权重

在这种情况下，可以在步骤429b中使用先前权重

或分配均匀分布以产生更新权重12b。通过使用先前权重或均匀分布，舍弃与不可靠相似度评分相关联的距离评分。这提高了底层更新过程的稳定性。

如果相似度评分的分布满足可靠性条件，则基于相似度评分的分布

确定当前权重

根据图8所示的实施例，首先在步骤428中将权重

分配给每个粒子，然后在步骤429a中利用相似度评分

进行加权，以产生更新的、尚未归一化的权重

因此，粒子的加权函数可以作为相似度评分的单边分布的概率值。如果满足所述可靠性条件，则通过所述过程输出所述更新权重12a。因此，根据图8所示的实施例，根据等式(16)确定当前权重12a：

其中，α为归一化因子。

根据图9所示的更新过程的第二实施例，可以通过进一步应用基于全局旋转估计的加权来获得当前重要性权重，所述全局旋转估计可以来自至少一个旋转传感器，如基于惯性的传感器。步骤426至步骤428和步骤429a与图8中的这些步骤相同，因此不再重复描述。当在步骤427中所述相似度评分的分布满足所述可靠性条件时，在步骤429a中应用相似度评分加权，除此之外，在步骤529c中，基于来自全局位姿测量值Z_t的全局旋转估计

进一步调整权重，以根据等式(17)生成更新的重要性权重12a：

此外，当不满足上述可靠性条件时，在步骤529b中，通过基于来自全局位姿测量值Z_t的全局位置估计

分配权重

来根据本实施例进行重置，所述全局位姿测量值可以来自至少一个位置传感器，如基于卫星的传感器，特别是GPS传感器。然后，在步骤529d中，可以基于来自全局位姿测量值Z_t的全局旋转估计

进一步调整所分配的权重，以根据等式(17)产生更新的重要性权重12b。

与图8所示的第一实施例相比，根据图9中的第二实施例的更新过程具有两个优点。首先，在不满足所述可靠性条件的情况下，由能够提供一定可靠度的全局位置估计确定权重。其次，无论是否满足所述可靠性条件，基于全局旋转估计的进一步加权都进一步提高所述更新权重的可靠性。根据所述第二实施例，使用另外两个单独的全局位置和旋转估计(例如从GPS传感器和IMU传感器导出)以增加更新权重的可靠性，从而增加当前位姿估计的可靠性。根据所述第二实施例，当满足所述可靠性条件时，根据等式(18)确定所述更新的重要性权重：

最后，根据图10所示的更新过程的第三实施例，除了来自基于卫星的传感器和/或基于惯性的传感器的全局位姿测量值Z_t之外，可以使用来自除基于卫星的传感器以外的位置和/或旋转传感器，以及可能基于惯性的传感器，例如来自基于视觉的传感器的全局位姿测量值G_t以进一步提高所述更新的重要性权重的可靠性。

由于图10中的步骤426至步骤428和步骤429a与图8中的这些步骤相同，因此此处不再重复描述。当不满足所述可靠性条件时，可以在步骤629b中基于全局位姿估计Z_t和G_t分配权重

从而产生更新的重要性权重12b。在所述相似度评分的分布满足所述可靠性条件的情况下，基于全局位姿估计Z_t和G_t进一步调整在步骤429a中确定的权重，以根据等式(19)产生更新的重要性权重12a：

在这种情况下，因此根据如下等式(20)确定更新的重要性权重：

根据位姿估计的准确度，可以在第三实施例的步骤629b和步骤629c中将全局位置估计或全局旋转估计或两者用于全局位姿估计Z_t和G_t。

总之，可以基于从位置传感器的传感器数据和旋转传感器的传感器数据中的至少一个，例如从基于卫星的传感器的传感器数据、基于惰性的传感器的传感器数据和至少一个基于视觉的传感器的传感器数据中的至少一个导出的全局位姿估计进一步调整当前权重。

在所述更新过程的上述实施例中，包括关于观察特征和参考特征的特征描述符和可选的特征类的知识，以增加在给定搜索区域内找到正确的最近邻的概率，并调整粒子权重计算。不满足相似度评分的阈值准则的特征点受到惩罚，其中，所述相似度评分是针对这些特征描述符和特征类计算的。此外，如果对应的分布满足可靠性条件，则基于所述相似度评分的分布对预测粒子进行加权；如果所述分布不满足所述可靠性条件，则舍弃预测粒子的距离评分，并用一个或多个全局位姿估计替换这些距离评分。

对所述观察特征和参考特征使用特征描述符和可选特征类进一步增加针对经过车辆、行人等动态对象的滤波器准确度，并增加特征可辨别性。因此，所得当前位姿估计变得更加可靠。

图11最后示出了实现根据上述任何实施例的本发明的车辆。不受限制地，车辆700在前轮上配备轮编码器792，作为测量前轮旋转的里程计传感器，由此可以确定车辆位置的变化。车辆700还包括惯性测量单元(IMU)790，作为基于惯性的传感器，用于确定6个DOF的变化，即车辆的位置坐标和方向的变化。因此，IMU 790由位置传感器和旋转传感器构成。此外，所述车辆配备有GPS传感器796，作为基于卫星的传感器或位置传感器，用于基于GPS信号测量全局位姿

最后，车辆700配备有立体摄像机794，作为基于视觉的传感器，记录车辆环境的立体图像。然后，如上文所述处理摄像机794记录的图像，以提取所述车辆环境中的观察特征。

为了执行上文结合本发明实施例描述的定位过程，所述车辆配备有处理电路780，用于执行上述任一种方法。经由电缆或以无线方式将传感器信号从里程计传感器792、IMU790、GPS传感器796和摄像机794传输到处理电路780。然后，处理电路780处理如上所述的传感器数据，以在图11中使用虚线指示的全局坐标系中对车辆700进行定位。

图11示出了全局坐标系的x轴和y轴，其中，z轴与车辆700的局部坐标系的z轴重合。全局坐标也称为空间固定坐标，而由x'轴、y'轴和z轴表示的局部坐标也称为物体固定坐标。车辆700的航向由图中的x'轴指示。一种方便的方式是，该航向可用于定义所述局部坐标系的x'轴，其中，在所述车辆固定局部坐标系中示出了与滚转、俯仰和偏航有关的旋转角度。如上所述，处理电路780用于在物体固定局部坐标系中的位置坐标和旋转坐标与空间固定全局坐标系中的位置坐标和旋转坐标之间进行转换。因此，在本发明中，全局位姿和全局位姿估计始终参考空间固定全局坐标。图11进一步示意性地指示车辆700的速度，作为方向可能由于车辆的侧滑而不同于车辆航向的矢量。这种侧滑可能是定位过程中的错误源之一，因为轮编码器792通常不会考虑所述侧滑。

本发明中描述的过程和方法，特别是结合图1至10描述的流程和方法，可以在包括用于执行所描述的过程和方法的处理电路的系统中实现。所述系统可包括软件和硬件的组合。例如，根据图4和图5的粒子滤波过程的预测、校正和重采样步骤可以作为软件模块或作为处理电路的单独单元实现。实际上，图2、图4、图5和图7至图10中的任何块都可以实现作为硬件单元或软件模块实现。其中，所描述的处理可以由通用处理器、CPU、GPU、数字信号处理器(digital signal processor，DSP)、现场可编程门阵列(field programmable gatearray，FPGA)等芯片执行。然而，本发明不限于在可编程硬件上实现。它可以在专用集成电路(application-specific integrated circuit，ASIC)上实现，或者通过上述硬件组件的组合实现。

图4和图5中的存储器455可以使用本领域已知的任何存储单元实现，如内存单元，特别是RAM、ROM、EEPROM等；存储介质，特别是DVD、CD、USB(闪存)驱动器、硬盘等；经由网络可用的服务器存储器等。

特别地，处理电路780可用于：在预测步骤410中使用里程计测量值O_t确定机器人(特别是车辆700)的多个当前假设位姿

在校正步骤420中，特别基于如图7至图10所示的相似度评分，确定相应的更新权重；在重采样步骤430中对粒子进行重采样；可能在步骤480中回收粒子；在增强步骤485中补充粒子。此外，处理电路718可用于确定图4的步骤440中当前位姿分布的置信度度量值，或者用于确定图5的步骤470中独立确定的位姿估计的置信度度量值。此外，所述处理电路可用于在步骤450中基于粒子滤波进行位姿估计，并在步骤460中基于预测进行独立位姿估计。

上述定位过程和子过程还可以由包括指令的程序实施，其中，所述指令存储在计算机可读介质上。这些指令在处理器上执行时，使处理器执行上述过程和方法。所述计算机可读介质可以是存储指令的任何介质，如DVD、CD、USB(闪存)驱动器、硬盘、经由网络可用的服务器存储器等。

总之，本发明提供了提高用于移动机器人定位的低成本系统的性能的方法。根据本发明的过程已经使用具有GPS、IMU和立体摄像机传感器的汽车原型进行了广泛的实时测试。初步结果显示，1m以下的绝对平均误差百分比约为90％，纵向平均绝对平均误差为0.75m或以下，横向平均绝对误差小于0.4m。这些错误在IoV规划及控制部门的要求规范内，因此适于商用部署。

本发明的方法和系统在保持低成本特征的同时显著提高定位准确度性能。它们可以在具有立体摄像机、GPS传感器和IMU传感器的低成本系统中实现。所描述的方法和系统为车辆位姿包括高度和旋转(滚转、俯仰和偏航)的所有6个DOF提供高准确度。由于其对处理能力的要求较低，因此所公开的过程适合于实时实施。第一测试表明，位姿估计可以在约10Hz下进行。

所描述的方法和系统通过如下方式解决了低成本车辆定位系统的许多问题：通过在粒子滤波过程中利用扩展的特征描述符解决动态对象和观察可辨别性的问题；通过可交换地使用粒子滤波和航位推算解决位姿估计不稳定性的问题；通过加入基于非GPS传感器的全局位姿估计解决GPS信号的时断时续问题。

Claims (27)

1.一种用于估计机器人位姿的装置(700)，其特征在于，用于：

基于第一位姿估计(15a)或第二位姿估计(15b)或所述第一位姿估计和所述第二位姿估计的组合(16)确定(450、460、470)所述机器人的当前位姿估计；

其中，所述第一位姿估计是基于所述机器人的当前位姿分布(13)；

其中，基于所述当前位姿分布确定所述第一位姿估计对所述当前位姿估计的贡献和所述第二位姿估计对所述当前位姿估计的贡献。

2.根据权利要求1所述的装置，其特征在于，所述第二位姿估计(15b)是基于以下一项或多项：

根据一个或多个先前位姿估计(6)进行的预测(460)，或

从位置传感器(796)的传感器数据和定向传感器(790)的传感器数据中的至少一个导出的全局位姿估计。

3.根据权利要求1或2所述的装置，其特征在于，所述第一位姿估计(15a)的所述贡献和所述第二位姿估计(15b)的所述贡献是基于所述当前位姿分布的置信度度量值(440)确定的。

4.根据权利要求3所述的装置，其特征在于，在确定所述当前位姿分布的所述置信度度量值(440)超过阈值时，只有所述第一位姿估计(15a)对所述当前位姿估计贡献。

5.根据权利要求4所述的装置，其特征在于，还用于基于所述当前位姿分布的所述置信度度量值(440)调整所述阈值。

6.根据权利要求5所述的装置，其特征在于，响应于所述当前位姿分布的所述置信度度量值(440)明显高于所述阈值，增大所述阈值；或者响应于所述当前位姿分布的所述置信度度量值(440)明显低于所述阈值，减小所述阈值。

7.根据权利要求6所述的装置，其特征在于，从增加所述阈值到减少所述阈值的转换以及从减少所述阈值到增加所述阈值的转换会延迟相应的延迟时间。

8.根据权利要求1或2所述的装置，其特征在于，所述第一位姿估计(15a)的所述贡献和所述第二位姿估计(15b)的所述贡献是基于各自的位姿估计的置信度度量值(470)确定的。

9.一种用于估计机器人位姿的装置(700)，其特征在于，用于：

确定(410)所述机器人的多个当前假设位姿(11)；

确定(420)所述多个当前假设位姿中的每个当前假设位姿的权重(12)；

基于所述多个当前假设位姿及其权重，确定(450)所述机器人的当前位姿估计(15a)；

其中，确定(420)所述多个当前假设位姿中的每个当前假设位姿的所述权重(12)包括：计算(422-425)相似度评分，所述相似度评分是一组参考特征(4)与一组观察特征(3)之间的相似度度量值。

10.根据权利要求9所述的装置，其特征在于，每个参考特征和每个观察特征均包括一个或多个特征描述符。

11.根据权利要求9或10所述的装置，其特征在于

每个参考特征和每个观察特征均包括一个或多个特征类，并且每个特征类包括一个概率值；其中，基于所述参考特征的所述一个或多个特征类及其概率值和所述观察特征的所述一个或多个特征类及其概率值，计算(422-425)所述相似度评分。

12.根据权利要求11所述的装置，其特征在于，每个特征类与现实世界元素的类别相关联。

13.根据权利要求10至12中任一项所述的装置，其特征在于

每个参考特征还包括空间固定(SF)位置坐标，每个观察特征还包括物体固定(BF)位置坐标，其中，相对于所述机器人定义所述BF位置坐标；

其中，计算(422-425)所述相似度评分包括：基于当前假设位姿(11)，在所述SF位置坐标与所述BF位置坐标之间进行映射(421)。

14.根据权利要求9至13中任一项所述的装置，其特征在于，当所述分布满足可靠性条件(427)时，基于所述相似度评分的分布确定(429a)所述当前假设位姿的所述权重。

15.根据权利要求14所述的装置，其特征在于，当所述分布不满足所述可靠性条件(427)时，不基于所述相似度评分的所述分布确定(429b、529b、629b)所述当前假设位姿的所述权重。

16.根据权利要求14或15所述的装置，其特征在于，还包括位置传感器(796)和定向传感器(790)中的至少一个，

其中，基于从所述位置传感器(796)的传感器数据和所述定向传感器(790)的传感器数据中的至少一个导出的全局位姿估计，进一步调整(529c、529d、629c)所述当前假设位姿的所述权重。

17.一种用于估计机器人位姿的装置(700)，其特征在于，用于：

基于一个或多个第一导航测量值(5)生成所述机器人的第一位姿分布(18)；

基于所述第一位姿分布(18)和精细化位姿分布(13)的当前实例生成所述机器人的第二位姿分布(1)；

基于所述第二位姿分布(1)和一个或多个第二导航测量值(2、3)生成所述精细化位姿分布(13)的下一个实例；

基于所述精细化位姿分布(13)的所述下一个实例确定所述机器人的位姿估计。

18.根据权利要求17所述的装置，其特征在于，所述精细化位姿分布(13)的所述当前实例和所述下一个实例均由一组假设位姿和关联权重表示；其中，表示所述当前实例的一组假设位姿和关联权重和表示所述下一个实例的一组假设位姿和关联权重包括相同数量的假设位姿。

19.根据权利要求17或18所述的装置，其特征在于，在生成所述第二位姿分布(1)时，所述精细化位姿分布(13)的所述当前实例对所述第二位姿分布(1)的贡献大于所述第一位姿分布(18)。

20.根据权利要求17至19中任一项所述的装置，其特征在于，用于不基于所述精细化位姿分布生成所述第一位姿分布(18)。

21.根据权利要求17至20中任一项所述的装置，其特征在于，用于通过以下一项或多项生成所述一个或多个第一导航测量值：基于卫星的位姿估计、基于惯性的位姿估计、基于视觉的位姿估计或用户输入。

22.根据权利要求17至21中任一项所述的装置，其特征在于，用于通过以下一项或多项生成所述一个或多个第二导航测量值：基于卫星的位姿估计、基于惯性的位姿估计、基于视觉的位姿估计或里程计位姿估计。

23.一种机器人(700)，特别是车辆，其特征在于，包括根据权利要求1至22中任一项所述的装置。

24.一种用于估计机器人(700)位姿的方法，其特征在于，所述方法包括：

基于第一位姿估计(15a)或第二位姿估计(15b)或所述第一位姿估计和所述第二位姿估计的组合(16)确定(450、460、470)所述机器人的当前位姿估计；

其中，所述第一位姿估计是基于所述机器人的当前位姿分布(13)；

其中，基于所述当前位姿分布确定所述第一位姿估计对所述当前位姿估计的贡献和所述第二位姿估计对所述当前位姿估计的贡献。

25.一种用于估计机器人(700)位姿的方法，其特征在于，所述方法包括：

确定(410)所述机器人的多个当前假设位姿(11)；

确定(420)所述多个当前假设位姿中的每个当前假设位姿的权重(12)；

基于所述多个当前假设位姿及其权重，确定(450)所述机器人的当前位姿估计(15a)；

其中，确定(420)所述多个当前假设位姿中的每个当前假设位姿的所述权重(12)包括：计算(422-425)相似度评分，所述相似度评分是一组参考特征(4)与一组观察特征(3)之间的相似度度量值。

26.一种估计机器人(700)位姿的方法，其特征在于，包括：

基于一个或多个第一导航测量值(5)生成所述机器人的第一位姿分布(18)；

基于所述第一位姿分布(18)和精细化位姿分布(13)的当前实例生成所述机器人的第二位姿分布(1)；

基于所述第二位姿分布(1)和一个或多个第二导航测量值(2、3)生成所述精细化位姿分布(13)的下一个实例；

基于所述精细化位姿分布(13)的所述下一个实例确定所述机器人的位姿估计。

27.一种存储指令的计算机可读介质，其特征在于，当所述指令在处理器上执行时，使得所述处理器执行根据权利要求24至26中任一项所述的方法。

Images