CN104833370A

CN104833370A - 用于映射、定位和位姿校正的系统和方法

Info

Publication number: CN104833370A
Application number: CN201410826371.5A
Authority: CN
Inventors: J·梅纳什; A·穆拉卡; A·兰加纳坦
Original assignee: Honda Motor Co Ltd
Current assignee: Honda Motor Co Ltd
Priority date: 2014-02-08
Filing date: 2014-12-25
Publication date: 2015-08-12
Anticipated expiration: 2034-12-25
Also published as: US20150228077A1; US9443309B2; CN104833370B; JP6595182B2; US9342888B2; JP2015148601A; DE102015201951A1; US20160203373A1

Abstract

用于映射、定位和位姿校正的系统和方法。本发明提供一种用于映射、定位和位姿校正的系统和方法，包括：沿着行进路线确定车辆的当前位置以及沿着相对于所述当前位置的所述行进路线确定当前可观测地标集合，所述当前可观测地标集合从得自成像设备的一个或多个立体图像中提取；以及查询勘测地标数据库以识别相对于所述车辆的所述当前位置的已勘测地标子集。所述方法包括在所述当前可观测地标集合与所述已勘测地标子集之间确定一个或多个二维变换估算，以及从所述一个或多个二维变换估算中识别最佳变换估算，所述最佳变换估算使所述当前可观测地标集合与所述已勘测地标子集之间的距离最小化。所述方法包括基于所述最佳变换估算校正所述车辆的位姿。

Description

用于映射、定位和位姿校正的系统和方法

技术领域

本发明涉及用于映射、定位和位姿校正的系统和方法。

背景技术

尝试在全球定位的智能代理(例如，车辆，机器人)的一个重要能力是辨识其位置。一般来讲，只要提供其局部传感器信息和全球的某个型号，代理就可确定其位置。通常，其自身的局部传感器信息提供对其位置的某种测量，但该测量常常不精确。配备有摄像头的智能车辆可使用视觉测程法跟踪其相对于空间中给定起始点的位置。然而，视觉测程法可提供往往会漂移的位姿。可靠的视觉测程法允许车辆更精确地估算其位置。

发明内容

根据一个方面，用于映射、定位和位姿校正的计算机实现的方法包括沿着行驶路线确定车辆的当前位置以及沿着相对于当前位置的行驶路线确定当前可观测地标集合。从得自成像设备的一个或多个立体图像中提取该当前可观测地标集合。该方法包括查询勘测地标数据库以识别相对于车辆当前位置的已勘测地标子集，所述勘测地标数据库包括先前存储的已勘测地标的集合，并且在当前可观测地标集合与已勘测地标子集之间确定一个或多个二维变换估算。该方法包括：从所述一个或多个二维变换估算中识别最佳变换估算，该最佳变换估算使当前可观测地标集合与已勘测地标子集之间的距离最小化；以及基于最佳变换估算校正车辆的位姿。

根据另一个方面，用于映射、定位和位姿校正的系统包括处理器和视觉测程模块，该视觉测程模块使处理器沿着行进路线确定车辆的当前位置以及沿着相对于当前位置的行进路线确定当前可观测地标集合。从得自通信地连接到处理器的成像设备的一个或多个立体图像中提取当前可观测地标集合。定位模块使处理器查询通信地连接到处理器的勘测地标数据库以检索相对于车辆当前位置的已勘测地标子集，所述勘测地标数据库包括先前存储的已勘测地标集合。位姿校正模块使处理器：在当前可观测地标集合与已勘测地标子集之间确定一个或多个二维变换估算；从所述一个或多个二维变换估算中识别最佳变换估算，该最佳变换估算使当前可观测地标集合与已勘测地标子集之间的距离最小化；以及基于最佳变换估算校正车辆的位姿。

根据又一个方面，一种包括指令的非临时性计算机可读介质，所述指令当由处理器执行时执行用于映射、定位和位姿校正的方法，所述方法包括沿着行进路线确定车辆的当前位置以及沿着相对于当前位置的行进路线确定当前可观测地标集合。从得自成像设备的一个或多个立体图像中提取该当前可观测地标集合。该方法包括：基于识别当前位置的查询接收来自勘测地标数据库的已勘测地标子集，该查询在勘测地标数据库中执行，所述勘测地标数据库包括先前存储的已勘测地标集合；以及在当前可观测地标集合与已勘测地标子集之间确定一个或多个二维变换估算。该方法包括：从所述一个或多个二维变换估算中识别最佳变换估算，该最佳变换估算使当前可观测地标集合与已勘测地标子集之间的距离最小化；以及基于最佳变换估算校正车辆的位姿。

附图说明

被认为是本发明的特征的一些新结构在所附权利要求书中有所陈述。在以下描述中，相同部分在整个说明书和附图中分别使用相同的数字标记。附图不一定按比例绘制，并且为了清晰和简洁起见，某些图形可以放大或一般化的形式示出。然而，当结合附图阅读时，通过参考以下对于示例性实施例的详细描述，将最好地理解本发明本身以及优选的使用方式、更多的目标和进步，其中：

图1是根据一个方面的用于映射、定位和位姿校正的示例性系统的示意图；

图2是根据一个方面的由服务提供的勘测模式的示例性数据流的示意图；

图3是根据一个方面的定位模式的示例性数据流的示意图；

图4是根据一个方面的从图3继续的定位模式的示例性数据流的示意图；

图5是根据一个方面的示例性对象、类别、功能和文件的UML图；

图6是根据一个方面的用于映射、定位和位姿校正的示例性方法的过程流程图；

图7是根据一个方面的示例性方法的过程流程图，该方法用于确定图6的方法的一个或多个二维变换；

图8A是根据一个方面的示例性栅格搜索的示意图，其中对于第一深度水平处的最佳估算而言，分支因子为1；

图8B是根据一个方面的示例性栅格搜索的示意图，其中对于图8A的第一深度水平之后的第二深度水平处的最佳估算而言，分支因子为1；

图8C是根据一个方面的示例性栅格搜索的示意图，其中对于图8B的第二深度水平之后的第三深度水平处的最佳估算而言，分支因子为1；

图8D是根据一个方面的示例性栅格搜索的示意图，其中对于第一深度水平下的不止一个最佳估算而言，分支因子为2；

图8E是根据一个方面的示例性栅格搜索的示意图，其中对于图8D的第一深度水平之后的第二深度水平下的不止一个最佳估算而言，分支因子为2；

图8F是根据一个方面的示例性栅格搜索的示意图，其中对于图8E的第二深度水平之后的第三深度水平下的不止一个最佳估算而言，分支因子为2；

图9A是根据一个方面的示例性排序列表的示意图；

图9B是根据一个方面的示例性群集集合的示意图；

图10A是根据一个方面的在应用位姿校正之前的地图的示意图；

图10B是图10A的地图的简化视图，其示出了沿着真实路径所观察到的地标；

图10C是图10A的地图的简化视图，其示出了沿着由车辆的视觉测程系统确定的行进路径所观察到的地标；

图11是图10A的地图的视图，其示出了对沿着由车辆的视觉测程系统确定的行进路径所观察到的地标进行查询的结果；

图12A是图10A的地图的简化视图，其示出了一个或多个2D变换；

图12B是图10A的地图的简化视图，其示出了最佳变换估算；

图12C是图10A的地图的简化视图，其示出了具有地标的最佳变换估算；以及

图13是图10A的地图的视图，其中将最佳变换估算应用于车辆的当前位置。

具体实施方式

以下包括本文所用的所选术语的定义。这些定义包括落在术语范围内并可用于实施的组件的各种例子和/或形式。本文无意用这些例子进行限制。此外，本领域的普通技术人员将认识到，可将本文所讨论的组件结合、省略或与其他组件组织起来，或组织成不同的体系结构。

如本文所用，“总线”是指可操作地连接至在计算机内部或在计算机之间的其他计算机组件的互联体系结构。总线可在计算机组件之间传输数据。因此，总线可使用其他总线与各种设备、模块、逻辑器和外围设备进行通信。总线可为单个内部总线互联体系结构和/或其他总线或网状体系结构(例如，外部)。总线可为存储器总线、存储器控制器、外围总线、外部总线、纵横开关和/或局部总线等等。在一些实施例中，总线可为车载总线等等，该车载总线使用诸如控制器局域网(CAN)、局域互联网(LIN)等的协议来使车内的组件互连。

如本文所用，“计算机通信”是指在两个或更多个计算设备(例如，计算机、便携式设备、个人数字助理、移动电话、网络设备)之间的通信，并且可为例如网络传输、文件传输、小程序传输、电子邮件、超文本传输协议(HTTP)传输等。计算机通信可在例如无线系统(例如，IEEE 802.11)、以太网系统(例如，IEEE 802.3)、令牌环系统(例如，IEEE 802.5)、局域网(LAN)、广域网(WAN)、点到点系统、电路交换系统、分组交换系统等等上进行。

如本文所用，“盘”可为例如磁盘驱动器、固态磁盘驱动器、软盘驱动器、磁带驱动器、极碟驱动器、闪存卡和/或记忆棒。此外，盘可为CD-ROM(光盘ROM)、CD可记录驱动器(CD-R驱动器)、CD可再写驱动器(CD-RW驱动器)和/或数字视频ROM驱动器(DVD ROM)。盘可存储用于控制或分配计算设备的资源的操作系统。

如本文所用，“数据库”可指表、表集合、数据存储装置(例如，盘)的集合和/或用于访问和/或操纵那些数据存储装置的方法。

如本文所用，“存储器”可包括易失性存储器和/或非易失性存储器。非易失性存储器可包括例如ROM(只读存储器)、PROM(可编程只读存储器)、EPROM(可擦除PROM)和EEPROM(电可擦除PROM)。易失性存储器可包括例如RAM(随机存取存储器)、同步RAM(SRAM)、动态RAM(DRAM)、同步DRAM(SDRAM)、双数据速率SDRAM(DDRSDRAM)和直接RAM总线RAM(DRRAM)。所述存储器可存储用于控制或分配计算装置的资源的操作系统。

如本文所用，“模块”包括但不限于存储指令的非临时性计算机可读介质、在机器上执行的指令、硬件、固件、在机器上执行的软件和/或每一者的组合以执行功能或者动作，和/或引起来自另一个模块、方法和/或系统的功能或操作。模块还可包括逻辑器、软件控制的微处理器、分立逻辑电路、模拟电路、数字电路、可编程逻辑设备、包含执行指令的存储器设备、逻辑门、门组合和/或其他电路组件。可将多个模块组合成一个模块并且可在多个模块间分配单个模块。

“可操作连接部”或将实体“可操作地连接”的连接部是其中可发送和/或接收信号、物理通信和/或逻辑通信的连接部。可操作连接部可包括物理接口、数据接口和/或电接口。

如本文所用，“处理器”处理信号并执行一般计算和算术功能。经处理器处理的信号可包括数字信号、数据信号、计算机指令、处理器指令、消息、比特、比特流或其他可被接收、传输和/或检测的对象。通常，处理器可以是各种处理器，包括多个单核和多核处理器和协处理器以及其他多个单核和多核处理器和协处理器体系结构。处理器可包括各种模块以执行各种功能。

如本文所用，“车辆”指能够承载一个或多个乘客并以任何形式的能源提供动力的任何移动车辆。术语“车辆”包括但不限于汽车、卡车、厢式货车、小型货车、SUV、摩托车、踏板车、小船、私人船艇和航空器。在一些情况下，机动车辆包括一个或多个引擎。另外，术语“车辆”可指能够承载一个或多个乘客并由一个或多个电动机完全或部分提供动力的电动车辆(EV)，所述电动机由电池供电。EV可包括纯电动车辆(BEV)和插电式混合电动车辆(PHEV)。另外，术语“车辆”可指由任何形式的能源提供动力的自动车辆和/或无人驾驶车辆。自动车辆可能或可能不承载一个或多个乘客。

如本文所用，“车辆系统”可包括但不限于可用于增强车辆、驾驶和/或安全的任何自动或手动系统。示例性车辆系统包括但不限于：电子稳定性控制系统、防抱死制动系统、制动辅助系统、自动制动预充系统、低速跟踪系统、巡航控制系统、碰撞预警系统、碰撞缓解制动系统、自动巡航控制系统、车道偏离预警系统、盲点指示系统、车道保持辅助系统、导航系统、传动系统、制动踏板系统、电动转向系统、可视设备(例如，摄像头系统、接近传感器系统)、气候控制系统、电子预拉伸系统等等。

I.系统概述

通常，本文所公开的系统和方法提供基于立体的映射和定位方法，以用于校正沿着路线行进的代理(例如，车辆、自动车辆、机器人、具有便携式设备的用户)的位姿。通过利用借助密集立体深度数据、两个地标集合和细化的变换估算而构建的代理环境的轻质3D局部地图，代理路径的位姿校正精确又快速。所述系统和方法在本文描述为包括两种操作模式：勘测模式和定位模式。然而，可将两种模式的功能和组件重新组织成不同体系结构和/或在其他体系结构、模式和实施例中实施。

现在参见附图，其中图示是为了示出一个或多个示例性实施例，而不是为了对这些实施例进行限制，图1示出了根据一个方面的用于映射、定位和位姿校正的示例性系统100的示意图。可将系统100的组件以及本文所述的其他系统、硬件体系结构和软件体系结构的组件结合、省略或组织成不同的体系结构以用于各种实施例。另外，在一些实施例中，可利用车辆(未示出)、一个或多个车辆系统(未示出)或便携式设备(未示出)来实施系统100和/或系统100的一个或多个组件。此外，应当理解，在一些实施例中，系统100和/或系统100的一个或多个组件可用于实施勘测模式或定位模式，如本文将更详细地讨论。

在图1的例示实施例中，系统100包括计算设备102，该计算设备具备对系统100的各组件进行处理并且与各组件进行通信和交互的配置。通常，计算机102包括处理器104、存储器106、盘108、位置测定单元110和通信接口112，它们各自可操作地连接以用于经由总线114进行计算机通信。计算机102可与车辆(未示出)，例如ECU、车辆音响主机以及成像系统等等，集成或相关联。通信接口112提供软件和硬件以促进计算设备102的组件与将在本文描述的其他组件、网络和数据源之间的数据输入和输出。另外，如将利用本文所述的系统和方法更详细地讨论，处理器104包括视觉测程模块116、定位模块118和位姿校正模块120。

计算设备102还可操作地连接以用于与成像设备122、网络124和勘测地标数据库126(DB)进行计算机通信。应当理解，可以各种方式促进与成像设备122、网络124和勘测地标DB 126的连接，例如，通过来自通信接口112的网络连接(例如，有线或无线)、来自连接到计算设备102的便携式设备(未示出)的蜂窝数据网络、车辆到车辆自组织网络(未示出)、车载网络等等。

成像设备122可包括一个或多个摄像头或者其他成像设备和感测设备。例如，成像设备122可为一个或多个立体摄像头、三维摄像头、远程感测设备(例如，LIDAR、激光器、传感器)等等。在一个实施例中，成像设备122采集图像场景(例如，行进路线、道路和车辆周围环境的场景)的立体图像(例如，左侧图像、右侧图像)。网络124为例如数据网络、互联网、广域网或局域网。网络124用作各种远程设备(例如，Web服务器、远程服务器、应用程序服务器、中间服务器、客户机、其他便携式设备)的通信介质。

在图1中，网络124包括勘测地标DB 126。构建勘测地标DB 126是为了进行地标匹配和定位，这将在本文中更详细地讨论。勘测地标DB 126可包括先前存储的已勘测地标集合128(例如，L_i…L_n)。另外，勘测地标DB 126可包括描述文件130和密钥文件132。密钥文件包含每个地标(例如，L_i…L_n)的形心位置。描述文件130包含有关每个地标的附加存储信息。描述文件130和密钥文件132将参照图5在本文中更详细地描述。

通常，利用位置测定单元(例如，图1中的位置测定单元110)构建勘测地标DB 126。在一些实施例中，位置测定单元110可包括高品质位置测定单元140，其带有例如全球定位系统(GPS)136和/或惯性测量单元(IMU)138。高品质位置测定单元140可为确定车辆全局位姿的高品质昂贵设备。在一些实施例中，高品质位置测定单元140具有10-15厘米的位置精确度。在勘测模式期间利用高品质位置测定单元140获得车辆全局位姿。

在另一个实施例中，位置测定单元110可包括低品质位置测定单元142，其带有全球定位系统(GPS)136或另一种类型的传感器或技术，以确定二维(2D)车辆位姿。低品质位置测定单元142可为确定车辆全局位姿的较便宜的设备。在一些实施例中，低品质位置测定单元142为标准车辆导航系统和/或便携式设备中的GPS设备。在一些实施例中，低品质位置测定单元142具有10-15米的位置精确度。在定位模式期间利用低品质位置测定单元142获得二维形式的相对车辆位姿。

如上所述，应当理解，可将位置测定单元110与另一个设备，例如车辆(未示出)中的便携式设备，相关联。另外，可从其他设备例如视觉测程系统(例如，视觉测程模块116)或通过网络例如网络124获得位置和全局位姿信息。可在勘测模式或定位模式期间使用自位置测定单元110获得的位置，所述模式将在本文更详细地讨论。

II.勘测模式

在勘测模式期间，沿着车辆的当前行进路线提取地标并且保存至勘测地标DB126。勘测模式可实施为来自提供商(例如，第三方)的服务或在消费者侧(例如，要定位的代理)实施。在定位期间使用勘测地标DB 126。现在将描述用于通过第三方服务构建图1的勘测地标数据库(DB)126的勘测模式。简言之，在勘测模式过程期间，通过将多个帧上的密集立体视差图像组合，由车辆的周围环境构成局部三维(3D)地图。通过识别和移除地表面来从局部3D地图中提取对象(例如，地标)。随后将这些对象和有关这些对象的信息保存至盘(例如，图1的勘测地标DB 126)以进一步用于定位。

图2是根据一个方面的由服务所提供的勘测模式的示例性数据流的示意图。在该实施例中，将勘测地标DB构建作为来自提供商(例如，第三方)的服务。因此，在定位期间，计算设备可访问来自提供商的勘测地标DB的数据。在该实施例中，勘测地标DB可经由网络、互联网、云提供并且由提供商维护。例如，参见图1，计算设备102可经由网络124访问勘测地标DB 126，其中勘测地标DB 126可由第三方提供商维护。应当理解，图1的组件可用于促进和执行图2中所示的数据流方框。

再次参见图2，将立体图像202(例如，从图1的成像设备122采集)和车辆全局位姿204(例如，由位置测定单元110确定)用作输入以生成局部三维(3D)地图206。在一个实施例中，在勘测模式期间，通过高品质位置测定单元140(例如，GPS 136、IMU 138)确定车辆全局位姿204。在一个实施例中，立体图像202还可包括来自例如成像设备122的传感器数据，例如，摄像头跟踪数据、摄像头轨线数据。局部3D地图206提供了车辆外部环境的视图并且以车辆为中心(即，其随车辆移动)。

在一个实施例中，使用基于体积和关键帧的方法来构建局部3D地图206。在该实施例中，生成立体图像202的体积表示。如果当前帧与前一个关键帧之间有足够的摄像头运动，则指定当前帧为关键帧。估算关键帧之间的相对位姿。在一个实施例中，使用KLT跟踪器估算相对位姿以跟踪立体图像(例如，左侧和右侧图像)中的哈里斯(Harris)角点特征。使用归一化SSD(差值平方和)来使特征在左侧和右侧图像之间匹配并且基于其视差计算特征的3D位置。可使用嵌入RANSAC中的三点算法来移除外点并且找到与运动假设相一致的特征匹配。使用非线性优化来处理内点以找到摄像头帧的增强位姿估算并且找到新的内点集合。使最后的关键帧穿过局部束调整(LBA)模块(未示出)以改善关键帧的相对位姿估算并且确保当前关键帧位置周围的局部度量图。

使用关键帧及其位姿来构建车辆周围环境的局部3D地图206。具体地讲，为了生成局部3D地图206，对于每个关键帧而言，在初始立体图像202和Sobel过滤图像的加权组合上，使用具有SAD区块匹配的半全局匹配算法的变异体来计算密集立体视差/深度图像。另外，为了节省计算时间，与8个或16个方向相比，可在例如仅仅两个方向上执行扫描线优化(Scanline Optimization)。

通过组合多个关键帧上的密集深度图像，在每个关键帧处生成和/或更新以车辆为中心的局部3D地图206，从而利用关键帧位姿将深度图像注册到单个坐标帧内。具体地讲，在一个实施例中，3D占用栅格用于构建局部3D地图并移除图像中的虚假视差值。组合多个深度图像并使用3D占用栅格提供了一种用以处理外点和缺失深度的快速又有效的方式，从而以精确方式生成局部3D地图206。利用局部3D地图206进行地标分段和跟踪208，这将在下面详细描述。

A.地标分段

3D局部图206经处理用于地标分段210以移除地表面，诸如道路和人行道，并且提取车辆环境中的对象(例如，地标)，诸如树木和交通标志。将对象过滤并产生当前可观测地标集合212。利用当前可观测地标集合212进行地标跟踪214以随着时间跟踪地标。

地标分段210可以各种方式进行。在一个实施例中，使用基于地面和对象高度创建群集的区域生长算法来提取地表面。在该实施例中，区域生长算法对3D占用栅格(上文所讨论)中的竖体素列操作并且将竖体素列分类为地面或对象。区域生长算法使用已知在地面上的车辆当前位置(例如，车辆全局位姿204)，并且将车辆占用的所有体素列分类为地面。将分类的列添加至体素列的列表以备将来检查。还计算车辆占用的每个体素列的高度。该高度为车辆下方的最高占用体素的离散高度，或如果是未知的，则该高度为使用车辆已知维度和摄像头校准信息的地面的估算离散高度。

区域生长算法检查与车辆列相邻的体素列并计算它们的高度。具体地讲，弹出体素列列表并检查弹出列的相邻列。体素列的高度为该位置处的地面的高度，或者如果空间被对象占用，则为该对象的高度。在存在外伸物体的情况下，其中外伸部分高于车顶，使用地面高度作为体素列的高度。不具有数据的列使其高度用相邻列高度填充。

如果相邻体素列的高度在弹出列的阈值内，则将相邻体素列分类为地面。如果高度相差较大的量，则不对体素列进行分类。将标记为地面的体素列添加至列列表，使得也对所述体素列的相邻对象进行检查。以该方式继续进行，区域生长算法扫过3D占用栅格，从而基于高度差值计算体素列高度和对体素列分类。

在区域生长算法过程结束之后，将所有未分类的体素列分类为对象。提取属于对象的所有体素，从而使用例如连接组件算法生成属于对象的体素的列表。连接组件算法使用分类为对象的列中的占用体素来初始化新对象列表。连接组件算法识别所占用体素的也被占用的所有相邻体素。如果将相邻体素列分类为对象，则将其添加至对象列表。如果将列分类为地面，则只有在体素高度大于其列的高度(例如，指示其为外伸物体)时才将该体素添加至对象列表。连接组件算法对新添加体素的相邻体素进行检查，直至检查完所有连接体素。对象为连接占用体素的列表。

对象基于高度和尺寸进行过滤。如果对象过短或如果对象的边界框的横截面尺寸过大，则将对象从对象列表中移除。基于高度的过滤会移除可能难以持续观察到的较小对象，或者我们对其可能不具有足够信息的对象。另外，基于尺寸的过滤产生具有较小基部的对象，该对象更易用于定位。另外，过滤会移除大多数停放车辆，使得可通过提供错误地标而使位姿校正过程混淆。对象的这个最终集合为3D局部地图206中的当前可观测地标集合212。当前可观测地标集合212可包括地标的体素列表以及与地标相关联的起始点和结束点。当前可观测地标集合212可存储在例如图1的存储器106和/或盘108中。作为另外一种选择，当前可观测地标集合212可存储在例如经由网络124访问的远程数据库中。

B.地标跟踪

通过地标跟踪214过程随着时间来跟踪当前可观测地标集合212。地标跟踪214可在本文所讨论的方法和系统的勘测模式和定位模式中执行。在勘测模式中，地标跟踪214对已将哪些地标写入盘(例如，写入勘测地标DB 126)保持跟踪。在定位模式中，地标跟踪214在大于局部3D地图206尺寸的距离范围内对地标保持跟踪。

为了跟踪地标，将可观测地标的当前集合212与勘测地标DB 216中的现有地标集合匹配。具体地讲，现有地标集合为先前存储的已勘测地标集合128。勘测地标DB 216中的每个地标(例如，先前存储的已勘测地标集合128)包括相关联的信息。例如，在一个实施例中，每个地标具有以下属性：(i)ID：唯一的地标ID；(ii)Updated：在当前关键帧中是否更新了地标；(iii)InFOV：地标是否在摄像头的视野内；(iv)InOuter：地标是否在局部3D地图的外部区域中；(v)Saved：是否已将地标保存至盘。图5示出了示例性地标对象502(例如，先前存储的已勘测地标集合128中的地标)的UML图，包括属性、属性类型和相关联的功能。图5的UML图实质上是示例性的，应当理解，还可使用地标对象502来实施其他属性、属性类型和功能。

如上所述，地标跟踪214将当前可观测地标集合212与勘测地标DB 216中先前存储在的已勘测地标集合128匹配。在一个实施例中，在进行匹配之前将先前存储的已勘测地标集合128中的每个地标(例如，L_i…L_n)的位置转换成当前局部3D地图206坐标帧。可通过利用当前可观测地标集合212和先前存储的已勘测地标集合128构造二分图(Bipartite Graph)，来执行匹配，从而形成不相交的两组顶点。如果当前地标与现有地标体积重叠，则在这两个地标之间添加图中的边缘。具体地讲，如果，基于二分图，包括两个地标的体素集合相交，则基于每一者中的现有和当前地标的数目来识别和分类当前可观测地标集合212与先前存储的已勘测地标集合128之间的等同类。基于等同类的类型更新先前存储的已勘测地标集合128。

现在将描述等同类的类型及其相关联的更新。在一个当前和一个现有等同类中，已经发现匹配。因此，现有地标的属性更新如下：(i)ID：无改变；(ii)Updated：如果当前和现有地标在其体素列表或体素中的结束点中不同，则设置为TRUE；(iii)InFOV：如果当前地标在摄像头视野中，则设置为TRUE；(iv)InOuter：如果当前地标在局部3D地图的外部区域中，则设置为TRUE；(v)Saved：设置为现有地标的值。

在一个当前和许多现有的等同类中，若干现有地标合并成单个当前地标，当新深度数据变得可用时可发生这种情况。将具有最大重叠的现有地标的体素和点列表更新至当前地标的体素列表，并且更新其属性，如在上文所讨论的一个当前和一个现有的等同类的情况中一样。删除剩余的现有地标。

在许多当前和一个现有的等同类中，单个现有地标产生若干当前地标，当先前占用的体素成为可用空间或当分段改变时可发生这种情况。在这种情况下，将具有最大重叠的现有地标的体素和点列表更新至当前地标的体素列表，并且更新其属性，如在上文所讨论的一个当前和一个现有的等同类的情况中一样。为剩余当前地标分配新的唯一地标ID并将它们添加至现有地标列表，其中它们的属性以类似方式更新。将Saved属性设置为FALSE。

在许多当前和许多现有的等同类中，将每个当前地标与具有最大重叠的现有地标匹配。如果尚未将现有地标的ID分配给另一个现有地标，则为当前地标分配该ID。如在上文所讨论的一个当前和一个现有的等同类的情况中一样完成剩余更新。如果已将现有ID分配给当前地标，则为当前地标生成新的唯一ID。如在上文所讨论的一个当前和一个现有的等同类的情况中一样完成剩余更新，然而，属性Saved设置为FALSE。

在一个当前和零个现有的等同类中，将当前地标添加至现有地标列表并且为其分配新的唯一ID。属性Updated设置为TRUE，并且属性Saved设置为False。如在上文所讨论的一个当前和一个现有的等同类的情况中一样完成剩余更新。在零个当前和一个现有的等同类中，除非属性Updated设置为FALSE，否则保留现有地标。

因此，在地标分段和跟踪208的匹配过程结束时，先前存储的已勘测地标集合128及其属性已用当前地标(例如，来自当前可观测地标集合212)更新。

C.计算地标特性并保存到盘上

定位涉及将当前可观测地标集合与存储地标(即，先前存储的已勘测地标集合128)匹配。为了有助于匹配过程，计算存储地标的若干特性，例如，形心、主轴和维度。在计算特性之前，可在比局部3D占用栅格更精细的分辨率下对地标建模以更精确地计算特性。在一个实施例中，仅针对新地标(即，具有新ID的那些)或使属性Updated设置为TRUE以及属性Saved设置为False的那些地标计算特性。

为了计算特性，在一个实施例中，使用带符号距离场对地标建模。创建了使地标封闭的新3D栅格，该3D栅格具有比3D占用栅格更精细的分辨率。针对与地标相关联的每个起始点，将光线投射到其结束点。为结束点落在其中的体素分配距离值零。为沿着光线并且在起始点与结束点体素之间的体素分配正距离，从而测量体素距结束点的距离。类似地，为超过结束点体素(远离起始点)沿着光线的体素分配负距离。由于针对每个起始结束点对执行该过程，因此如果体素恰好落在若干起始结束点对之间的光线上，则可为该体素分配多个距离值。

体素中的距离可提供体素距真实地标表面的多个估算。将距离平均以获取体素距真实地标表面的单个估算。具体地讲，在一个实施例中，将距离截短成最大值、分组为二进制并且计算加权平均数。针对所有体素一起考虑的这些平均距离构成带符号距离场，并且地标表面作为其零水平集而获得。即，采用带符号距离接近零的所有体素来构成地标表面。

一旦识别了地标表面体素，就使用体素中心的坐标来计算每个地标的以下特性：(i)形心：计算为所有表面体素的形心；(ii)主轴：通过对体素中心坐标进行主成分分析(PCA)而获得；(iii)轴维度：与沿着每个轴的变化成比例并且还作为PCA的结果而获得；(iv)半径：通过使立式气缸与地标表面配合并且使用气缸的半径来确定。将这些特性连同描述地标表面的体素坐标一起视为地标描述。

在勘测模式中，当地标不再位于摄像头视野中并且地标位于局部3D地图的外部区域中时，将地标描述写入盘(即，勘测地标DB 216)。换句话讲，当属性InOuter为TRUE并且属性InFOV和Saved为FALSE时，将地标描述写入盘。一旦将地标写入盘，属性Saved就设置为TRUE，因此就不再将地标写入盘。在一个实施例中，将地标描述写入描述文件130。图5示出了包括属性、属性类型和相关联功能的示例性描述文件506的UML图。

在一个实施例中，将地标描述写入盘之前，使用可从高品质位置测定单元140(即，GPS 136和IMU 138；车辆全局位姿204)获得的车辆六自由度(即，6DOF)位姿将所有三维坐标转换成纬度、经度和海拔。例如，让(x_p，y_p，z_p)^T作为车辆坐标帧中的点的位置。让作为车辆的纬度、经度、海拔、偏航、俯仰和横滚，如GPS 136和IMU 138所提供。随后通过以下方式来估算点(l_p，λ_p，α_p)的GPS坐标：

\begin{matrix} (\begin{matrix} {x^{l}}_{p} \\ {y^{l}}_{p} \end{matrix}) = (\begin{matrix} \cos θ & - \sin θ \\ \sin θ & \cos θ \end{matrix}) (\begin{matrix} x_{p} \\ y_{p} \end{matrix}) \\ l_{p} = l + {x^{l}}_{p} / 111111 \\ λ_{p} = λ + {y^{l}}_{p} / (111111 * \cos l) \\ α_{p} = z_{p} \end{matrix} - - - (1)

通过地标形心的纬度和经度对描述文件130作标记(例如，通过处理器104)。另外，生成密钥文件132，该文件包含每个地标的纬度和经度形心。在定位模式中通过处理器104将密钥文件132读取到存储器106中并且允许容易地识别车辆附近的地标以及快速读取其相关联的文件。图5示出了包括属性、属性类型和关联功能的示例性密钥文件504的UML图。

如上所讨论，勘测模式可实施为来自提供商(例如，第三方)或消费者侧的服务。在消费者的情况下，可在具有高品质GPS和IMU设备(例如，图1的高品质位置测定单元140)的车辆中实施勘测模式。因此，勘测模式将遵循如图2针对利用高品质位置测定单元140的消费者侧所示的类似步骤。

III.定位模式

在定位模式中，车辆定位涉及地标匹配和地标变换以计算位姿校正并将位姿校正应用于车辆路径。为了在全局帧中定位车辆，将定位模式期间观察到的地标与勘测模式期间保存到盘上的地标匹配。

A.定位模式概述

图3和4是根据一个方面的在定位模式期间的示例性数据流的示意图。现在将参照图1-2详细地描述图3和4。在一个实施例中，视觉测程模块116沿着行驶路线确定车辆相对位姿以及沿着相对于车辆相对位姿的行进路线确定当前可观测地标集合。例如，如以上通过图1和2所讨论，使用立体图像302和车辆相对位姿304(例如，由视觉测程模块116确定))来生成局部3D地图306。局部3D地图306使用地标分段310(例如，如通过图2的地标分段210所讨论)进行处理，并且产生当前可观测地标集合312。因此，从得自成像设备112的一个或多个立体图像302中提取当前可观测地标集合312。

现在参见图4，可使用例如低品质位置测定单元142将车辆的当前位置(例如，当前的弱全局位置)确定为二维(2D)车辆位姿316。2D车辆位姿316至少提供车辆位置的维度和经度。在一些实施例中，2D车辆位姿316提供车辆位置，其中具有约10m的标准偏差。

在一个实施例中，在地标匹配318处，通过2D车辆位姿316查询勘测地标DB 126以识别相对于车辆当前位置(即，2D车辆位姿316)的已勘测地标子集320。例如，定位模块118可生成包括车辆当前位置的查询并且在勘测地标DB 126处执行查询以检索相对于车辆当前位置的已勘测地标子集320。具体地讲，在一个实施例中，定位模块118可通过将线路与若干过去的GPS读数配合来估算全局GPS帧中的车辆大概航向(偏航)。在一个实施例中，从低品质位置测定单元142获得若干过去的GPS读数并且保存在例如低品质位置测定单元142的存储器(未示出)和/或存储器106或盘108中。

使(l_v，λ_v，θ_v)利用分别表示维度、经度和通过线路配合计算的航向的符号来给出车辆的当前2D GPS位姿316。通过利用勘测模式期间生成的密钥文件132，识别靠近车辆当前位置的已勘测地标子集320。还读取来自描述文件130的与已勘测地标子集320相关联的信息。使用车辆的当前2D位姿316，根据车辆的帧将来自描述文件130的GPS坐标转换成笛卡尔坐标，如下所述：

\begin{matrix} (\begin{matrix} x_{p} \\ y_{p} \end{matrix}) = (\begin{matrix} \cos 0_{v} & - \sin 0_{v} \\ \sin θ_{v} & \cos θ_{v} \end{matrix}) (\begin{matrix} {x^{l}}_{p} \\ {y^{l}}_{p} \end{matrix}) \\ {x^{l}}_{p} = (l_{v} - l_{p}) * 111111 \\ {y^{l}}_{p} = (λ_{v} - λ_{p}) * (111111 * \cos l_{v}) \\ z_{p} = α_{p} \end{matrix} - - - (2)

其中(l_p，λ_p，α_p)是已勘测地标子集320的地标的GPS坐标并且(x_p，y_p，z_p)^T是车辆坐标帧中已勘测地标子集320的地标的位置。

为了计算要应用于车辆位姿的位姿校正326，在地标变换322处，估算已勘测地标子集320与当前可观测地标集合402之间的一个或多个二维(2D)变换。在一个实施例中，位姿校正模块120在当前可观测地标集合与已勘测地标子集之间确定一个或多个二维变换估算。

以二维方式估算变换，然而，在其他实施例中，可使用地标表面来估算三维变换。为了计算2D变换估算，将地标形心和表面投影至2D(例如，移除z坐标)。这些投影定义为描述地标集合的布局和结构的地标标记。地标标记(未示出)实时生成并且允许进行快速和轻质的2D匹配。

另外，在地标变换322处，使用群集搜索324自一个或多个2D变换估算来确定最佳变换估算，将在下面进一步详细讨论。具体地讲，位姿校正模块120可识别一个或多个2D变换估算中的最佳变换。在一些实施例中，最佳变换估算使当前可观测地标集合312与已勘测地标子集320之间的距离最小化。

在将最佳变换估算计算并且选择为一个或多个2D变换估算中的(x_2D，y_2D，θ_2D)^T之后，应用车辆的当前2D位姿316以获取校正位姿，如下所述：

\begin{matrix} {l^{l}}_{v} = (l_{v} + x_{2 D}) / 111111 \\ {λ^{l}}_{v} = (λ_{v} - y_{2 D}) / (111111 * \cos l_{v}) \\ {θ^{l}}_{v} = θ_{v} + θ_{2 D} \end{matrix} - - - (3)

其中(l^l _v，λ^l _v，θ^l _v)^T为车辆最终校正位姿326。因此，通过利用借助密集立体深度数据、两个地标集合和细化的变换估算而构建的车辆环境的轻质3D局部地图，车辆路径的位姿校正精确又快速。

B.确定一个或多个2D变换估算并识别最佳变换估算

如上所述，为了计算位姿校正，在地标变换322处确定当前可观测地标集合312与已勘测地标子集320之间的一个或多个二维(2D)变换估算。所述一个或多个二维变换估算为2D刚体变换。2D刚体变换由单个旋转和2D平移组成。具体地讲，是X偏移、Y偏移和旋转方向上的变换。

通常，2D变换估算通过以下方式得出：在深度水平N的离散有界空间中的一个或多个变换标度下估算一个或多个2D变换估算，选择深度水平N的一个或多个最佳变换估算，以及基于所述一个或多个最佳变换估算修改离散有界空间。该过程可通过在最大深度水平N之外的每个深度水平N的离散有界空间中的一个或多个变换标度下估算一个或多个2D变换来继续。

所述一个或多个变换标度从粗略变换标度增进为精细变换标度。因此，在一些实施例中，与前一个变换标度相比，所述一个或多个变换标度处于更精细的细节水平(例如，更精细的移动水平)。例如，在第一变换标度下，所述一个或多个2D变换估算可开始为粗略移动(例如，1m增量)。在第二变换标度下，2D变换估算是比第一移动标度的粗略移动更精细的移动(例如，1/2m增量)。应当理解，可使用两个以上的变换标度来估算一个或多个2D变换估算。

现在将参照图8A-8F详细地描述地标变换322和群集搜索324。图8A、8B和8C是在每个深度水平N具有单个估算(即，分支因子为1)的不同变换标度下进行示例性栅格和树木搜索以实现最佳变换估算的示意图，其中最大深度水平N为3(即，图8A，N＝1；图8B，N＝2；图8C，N＝3)。应当理解，可实施任何数值的分支因子和深度水平。现在参见图8A，在由栅格800定义的离散有界空间内估算一个或多个2D变换估算。在一个或多个变换标度下估算所述一个或多个2D变换估算。变换标度从粗略变换标度增进为精细变换标度。在一个实施例中，变换标度以设置数值增进，例如，以45度增量以及以1米增量平移。

如一示例性例子，可在第一变换标度下，例如使用粗略移动，估算所述一个或多个2D变换估算。例如，粗略移动可包括以45度增量旋转和以1米增量平移。在另一个例子中，可在第二变换标度下估算所述一个或多个2D变换估算。第二变换标度例如为比第一变换标度更精细的标度或更精细的移动。因此，例如，第二变换标度下的所述一个或多个2D变换估算可包括以25度增量旋转和以1/2米增量平移。

在每个深度水平N下，确定最佳变换估算。在一个实施例中，确定所述一个或多个2D变换估算中每一者的变换评分和/或变换成本。在一个实施例中，评分和/或成本基于在应用2D变换之后当前可观测地标集合312与已勘测地标子集320之间的距离测量。例如，在一个实施例中，在应用2D变换之后，当前可观测地标集合312与已勘测地标子集320之间越靠近(即，地标之间的距离最小化，形心之间的距离最小化)，变换估算就越好，并且成本和/或评分就越低。例如，评分或成本为零将为完美变换。

在另一个实施例中，距离测量可为应用于距离集合的任何标准。例如，在一个实施例中，在应用2D变换之后基于当前可观测地标集合312与已勘测地标子集320的对应点之间的均方差的平方根来确定评分和/或成本。因此，针对每个2D变换估算，确定评分并将评分分配给2D变换估算。

因此，可基于与所述一个或多个2D变换估算中每一者相关联的变换成本确定深度水平K的最佳变换估算。在一个实施例中，基于深度水平K的最佳变换估算修改离散有界空间。再次参见图8A，在栅格800的区域804中选择最佳估算802。因此，在图8B中，区域804成为用于确定一个或多个2D变换估算的离散有界空间。

与图8A的讨论类似，在深度水平K++(2)的一个或多个变换标度下估算图8B中的一个或多个2D变换。可基于与所述一个或多个2D变换估算中每一者相关联的变换成本确定深度水平K++(2)的最佳变换估算。在一个实施例中，基于深度水平K++(2)的最佳变换估算修改离散有界空间。

在图8B中，在离散有界空间804中选择最佳估算806。因此，将离散有界空间重定义为最佳估算806周围的区域810。因此，在图8C中，将一个或多个2D变换估算限于区域810(即，重定义的离散有界空间)。在深度水平K++(3)的一个或多个变换标度下估算图8C中的所述一个或多个2D变换。可基于与所述一个或多个2D变换估算中每一者相关联的变换成本确定深度水平K++(3)的最佳变换估算。在图8C中，在区域810(例如，离散有界空间)中选择最佳估算812。

应当理解，可针对预定数目的深度水平K和/或2D变换估算重新进行上述过程。另外，可针对不止一个最佳变换估算进行该过程。例如，图8A、8B和8C是具有不止一个估算(例如，分支因子大于1)而不是具有如图8D、8E和8F所示的单个估算的不同水平下的最佳变换估算的示例性栅格搜索的示意图。

在一个实施例中，一旦为每个深度水平N至最大深度水平N确定最佳变换估算，就生成排序列表，该排序列表包含每个深度水平N的最佳变换估算并且基于每个深度水平N的最佳变换估算中每一者的变换成本排序。因此，参见最大深度水平N为3并且分支因子为1的图8A、8B和8C，排序列表将包括三个最佳变换估算，每个深度水平N一个。换句话说，所估算的最佳变换估算包括分支因子N乘以最大深度水平K。参见最大深度水平为3并且分支因子为2的图8D、8E和8F，排序列表将包括九个最佳变换估算，每个深度水平N两个。

图9A示出了通过每个深度水平N的最佳变换估算生成的示例性排序列表900。在图9A中，排序列表900的第一列识别深度水平N的最佳估算，并且第二列识别与深度水平N的最佳估算相关联的变换成本。图9A中的最大深度水平为6，并且分支因子为1；然而，应当理解，可实现任何最大深度水平或任何数值的分支因子。在图9A中，将最佳变换估算从最低变换成本至最高变换成本排序。

在一个实施例中，将排序列表中的每个最佳变换估算与相邻最佳变换估算进行比较。这可通过从最低变换成本至最高变换成本以迭代方式处理排序列表而实现。如果确定变换成本之间有显著差值(例如，差值>预定数值)，则生成包括最佳变换估算的群集。换句话说，当评分中存在显著差值(例如，两个评分相差50％)时，可指定新群集。

再次参见图9A，B_N1将与B_N5进行比较，B_N3将与B_N6进行比较等等，直至到达排序列表的末端。在一个示例性例子中，预定差值设置为5。因此，在图9A中，如果最佳变换估算与相邻变换估算之间的差值大于5，则生成群集。如一示例性例子，将B_N3与B_N6进行比较(即，20-6＝14)。由于B_N3与B_N6之间的差值为14，因此在B_N3与B_N6之间插入群集分界线。因此，第一群集包括B_N1、B_N5和B_N3。假定未确定另一个群集分界线，则第二群集包括B_N6、B_N3和B_N4。

图9B是根据一个方面的示例性群集集合的示意图。例如，群集902、904和906各自包括基于上述迭代比较的多个2D变换估算。参见图9A的例子和图9B的图，第一群集可为群集902并且第二群集可为群集904。

在一个实施例中，基于群集的最佳估算为群集分配评分。在一个实施例中，最佳估算为具有最低评分的估算。再次参见图9A的例子，由于2为第一群集中的最低变换评分，因此将为第一群集分配与B_N1相关联的评分。类似地，由于20为第二群集中的最低变换评分，因为将为第二群集分配与B_N6相关联的评分。参见图9B，基于每个群集902、904和906中的所述多个2D变换的最佳估算，为该群集分配评分。

因此，在一个实施例中，基于如每个群集所确定的最佳变换估算来应用位姿校正。从而，基于每个群集的最佳变换估算选择最佳变换估算(例如，最佳群集变换估算)。该最佳群集变换估算可用于将位姿校正应用于车辆。如一示例性例子，如果群集A中的最佳估算优于(例如，低于)群集B中的最佳估算，则群集A优于群集B。因此，参见图9A的例子，由于第一群集的最佳变换估算为2(更低)，第二群集中的最佳变换估算为20，因此最佳变换估算为2并且可将该变换估算应用于车辆的位姿校正。

C.用于映射、定位和位姿校正的方法

现在将参照图6的方法描述上述图1-5中示出的系统100。应当理解，以上在图1-5中所讨论的系统和组件可通过图6的方法类似地实施。图6的方法示出了根据一个方面的用于映射、定位和位姿校正的示例性方法。在方框602处，该方法包括沿着行进路线确定车辆的当前位置以及沿着相对于当前位置的行进路线确定当前可观测地标集合。从得自成像设备的一个或多个立体图像中提取该当前可观测地标集合。例如，图1的位置测定单元110，具体地讲，低品质位置测定单元142，可沿着行进路线获得车辆的当前位置。车辆的当前位置可为二维(2D)车辆位姿(例如，至少车辆位置的维度和经度)。当前可观测地标集合例如由视觉测程模块116通过从得自成像设备122和3D局部地图的一个或多个立体图像中提取当前可观测地标集合来确定，如上文通过图3所讨论。

现在参见方框604，该方法包括查询勘测地标DB以识别相对于车辆当前位置的已勘测地标子集。例如，在一个实施例中，定位模块118生成查询并将查询传输(和/或执行)至勘测地标DB 126。查询可包括车辆的当前位置。勘测地标DB 126包括先前存储的已勘测地标集合(例如，图4的先前存储的已勘测地标集合417)。

在一个实施例中，在接收和执行查询之后，勘测地标DB 126向定位模块118返回相对于车辆当前位置的已勘测地标子集。在一个实施例中，勘测地标DB 126可使用读取自描述文件的数据来执行查询。描述文件可包括先前存储的已勘测地标集合中的每个地标的坐标。从而，将每个地标的坐标与车辆的当前位置进行比较以确定已勘测地标子集。因此，已勘测地标子集包括存储在车辆当前位置附近的勘测地标DB 416中的地标。应当理解，在一些实施例中，定位模块118可从勘测地标DB 416读取描述文件并且在定位模块118处执行查询。

在方框606处，该方法包括在当前可观测地标集合与已勘测地标子集之间确定一个或多个二维变换估算。确定一个或多个二维变换估算将在下面通过图7更进一步地详细描述。在一个实施例中，位姿校正模块120在当前可观测地标集合与已勘测地标子集之间确定一个或多个二维变换估算。

另外，在一个实施例中，在方框608处，该方法包括确定当前可观测地标集合的每个地标的形心。如上所讨论，已勘测地标子集还包括已勘测地标子集中每个地标的形心。例如，如上所讨论，描述文件130和/或密钥文件132可包括已勘测地标子集中每个地标的形心的信息(例如，位置)。

因此，在一个实施例中，确定一个或多个二维变换估算包括将已勘测地标子集的每个地标的形心以及当前可观测地标集合的每个地标的形心投影成二维。例如，如上所讨论，在一个实施例中，通过移除例如z坐标来将地标形心和表面投影成2D。这些投影可定义为描述地标集合的布局和结构的地标标记。

在方框608处，该方法包括从所述一个或多个二维变换估算中识别最佳变换估算。在一个实施例中，最佳变换估算使当前可观测地标集合与已勘测地标子集之间的距离最小化。在另一个实施例中，最佳变换估算使当前可观测地标集合与已勘测地标子集之间的对应形心距离最小化。

在方框610处，该方法包括基于最佳变换估算校正车辆的位姿。例如，如上通过公式(3)所讨论，将最佳变换估算应用于车辆当前位置，例如，沿着在方框602处确定的行进路线的车辆当前位置。在一些实施例中，车辆当前位置为2D位姿。在图1的系统100中，位姿校正模块120基于最佳变换估算校正车辆的位姿。

确定一个或多个二维变换估算以及识别最佳变换估算现在将参照图7进行详细的讨论。图7是根据一个方面的用于确定图6的一个或多个二维变换的示例性方法的过程流程图。现在将参照图1-5的组件和功能描述图7。

在方框702处，并且如上所讨论，该方法包括确定一个或多个2D变换估算。在一个实施例中，处理器104保持图6的方法中深度水平N的每次迭代的计数器i。因此，在方框702处，该方法可包括使计数器i增量。在一个实施例中，确定一个或多个二维变换估算包括在深度N的离散有界空间中的当前可观测地标集合与已勘测地标子集之间的一个或多个变换标度下确定一个或多个二维变换估算。如上面通过图8A-8F所讨论，所述一个或多个变换标度从粗略变换标度增进为精细变换标度。

在另一个实施例中，确定一个或多个二维变换估算包括在离散有界空间中的当前可观测地标集合与已勘测地标子集之间的第一变换标度下确定一个或多个二维变换估算。如上所讨论，第一变换标度可为粗略标度变换。另外，在该实施例中，确定一个或多个二维变换估算包括在离散有界空间中的当前可观测地标集合与已勘测地标子集之间的第二变换标度下确定一个或多个二维变换估算。第二变换标度为比前一个标度(例如，第一变换标度)更精细的细节水平下的变换标度。

在方框704处，该方法可包括确定深度水平K下的所述一个或多个2D变换估算中每一者的变换成本。另外，可基于深度水平K下的所述一个或多个2D变换估算中每一者的变换成本确定最佳变换估算。如上所讨论，在一个实施例中，最佳变换估算可为具有最低变换成本的2D变换估算。在方框706处，将离散有界搜索空间修改为最佳变换估算周围的区域。在一些实施例中，在方框708处，可确定其是否等于深度水平的最大数目K。如果为否，则该方法继续回到方框702并且在下一个深度水平K下确定一个或多个变换标度下的一个或多个2D变换估算。

如果在方框708处确定为是，则该方法进行到方框709。在方框709处，该方法包括基于每个深度水平N下的所述一个或多个二维变换估算的变换成本生成每个深度水平N下的最佳变换估算的排序列表。如以上通过图9A所讨论，排序列表可从最低至最高变换评分排序。

将排序列表迭代以确定一个或多个群集。具体地讲，该方法包括基于将排序列表中的每个最佳变换估算与排序列表中的相邻最佳变换估算进行比较来确定最佳变换估算的一个或多个群集。更具体地讲，在方框710处，针对排序列表中的每个最佳变换估算，将最佳变换估算与相邻最佳变换估算进行比较(例如，将评分s与评分s++进行比较)。

在方框712处，确定最佳变换估算与相邻最佳变换估算之间的差值是否为显著差值(例如，差值>预定数值)。换句话说，当变换成本中存在显著差值(例如，两个变换成本相差50％)时，可指定新群集。因此，如果在方框712处确定为是，则在方框714处生成新群集。在一些实施例中，在方框712处，通过处理器104保持计数器c以保持多个群集。如果在方框712处确定为否，则该方法继续进行至方框716。在方框716处，确定是否已达到排序列表的末端。如果在方框716处确定为否，则该方法继续在方框710处以迭代方式处理排序列表。如果在方框716处确定为是，则该方法继续进行到方框720。

在方框720处，该方法包括基于所述一个或多个群集中每一者的最佳变换估算确定最佳群集变换估算。如上通过图9A和9B所讨论，基于群集的最佳估算为群集分配评分。在一个实施例中，最佳估算为具有最低评分的估算。从而，基于每个群集的最佳变换估算选择最佳变换估算(例如，最佳群集变换估算)。该最佳群集变换估算可用于将位姿校正应用于车辆。如一示例性例子，如果群集A中的最佳估算优于(例如，低于)群集B中的最佳估算，则群集A优于群集B。因此，基于最佳群集变换估算，在方框718处应用位姿校正。

IV.示例性例子

现在将描述实施用于映射、定位和位姿校正的系统和方法的示例性例子。图10A是在应用位姿校正之前的地图1000的示意图。线1002示出了由车辆的视觉测程系统(例如，视觉测程模块116)确定的行进路径。线1004示出了车辆实际行驶的行进路线(即，真实路径)。通过利用上述用于映射、定位和位姿校正的方法和系统，可协调行进路径1002和行进路线1004以确定车辆的正确行进路径。

图10B是图10A的地图1000的详细视图，其示出了沿真实行进路线(线1004)观察到的地标1006。这些地标1006是当前可观测地标集合。图10C是图10A的地图1000的详细视图，其示出了沿着由视觉测程系统确定的行进路径(线1002)观察到的地标1008。

图11示出了地图1000的视图，其示出了对勘测地标数据库(例如，勘测地标数据库126)执行的查询的结果，以识别相对于车辆当前位置的已勘测地标子集。查询结果，勘测地标子集，各自由图11中的圆角方形表示。

图12A是确定当前可观测地标集合1006(即，来自图10B)与已勘测地标子集(例如，由圆角方形表示)之间的一个或多个2D变换的示例性例子。在图12B中，选择最佳变换估算1014。在图12C中，相对于图10A的地图1000示出最佳变换估算。在图13中，将最佳变换估算应用于车辆当前位置，从而产生新路线1012。

本文所讨论的实施例也可在存储计算机可执行指令的非临时性计算机可读存储介质的背景中描述和实施。非临时性计算机可读存储介质包括计算机存储介质和通信介质。例如，闪存驱动器、数字通用光盘(DVD)、光盘(CD)、软盘和盒式磁带。非临时性计算机可读存储介质可包括以用于存储信息(诸如，计算机可读指令、数据结构、模块或其他数据)的以任何方法或技术实施的易失性和非易失性、可移除和不可移除介质。非临时性计算机可读存储介质不包括暂态和传播的数据信号。

应当理解，以上公开的以及其他的结构和功能的各种具体实施或其替代形式或变型可有利地结合到许多其他的不同系统或应用中。另外，随后可由本领域技术人员作出各种当前未预见或意料之外的替代形式、修改、变型或其中的改进，本文旨在将这些涵盖于以下权利要求书范围内。

Claims

1.一种用于映射、定位和位姿校正的计算机实现的方法，该方法包括以下步骤：

沿着行进路线确定车辆的当前位置以及沿着相对于所述当前位置的所述行进路线确定当前可观测地标集合，所述当前可观测地标集合从得自成像设备的一个或多个立体图像中提取；

查询勘测地标数据库以识别相对于所述车辆的所述当前位置的已勘测地标子集，所述勘测地标数据库包括先前存储的已勘测地标集合；

在所述当前可观测地标集合与所述已勘测地标子集之间确定一个或多个二维变换估算；

从所述一个或多个二维变换估算中识别最佳变换估算，所述最佳变换估算使所述当前可观测地标集合与所述已勘测地标子集之间的距离最小化；以及

基于所述最佳变换估算校正所述车辆的位姿。

2.根据权利要求1所述的计算机实现的方法，该方法包括：确定所述当前可观测地标集合中的每个地标的形心，并且其中所述已勘测地标子集包括所述已勘测地标子集中的每个地标的形心。

3.根据权利要求2所述的计算机实现的方法，其中，确定一个或多个二维变换估算包括将所述已勘测地标子集中的每个地标的所述形心以及所述当前可观测地标集合中的每个地标的所述形心投影成二维。

4.根据权利要求3所述的计算机实现的方法，其中，所述最佳变换估算使所述当前可观测地标集合与所述已勘测地标子集之间的对应形心距离最小化。

5.根据权利要求1所述的计算机实现的方法，其中，确定一个或多个二维变换估算包括在深度水平N的离散有界空间中的所述当前可观测地标集合与所述已勘测地标子集之间的一个或多个变换标度下确定一个或多个二维变换估算。

6.根据权利要求5所述的计算机实现的方法，其中，所述一个或多个变换标度从粗略变换标度增进为精细变换标度。

7.根据权利要求5所述的计算机实现的方法，该方法包括：基于每个深度水平N下的所述一个或多个二维变换估算的变换成本生成每个深度水平N下的最佳变换估算的排序列表。

8.根据权利要求7所述的计算机实现的方法，该方法包括：基于将所述排序列表中的每个最佳变换估算与所述排序列表中的相邻最佳变换估算进行比较来确定最佳变换估算的一个或多个群集。

9.根据权利要求8所述的计算机实现的方法，该方法包括：基于所述一个或多个群集中的所述最佳变换估算确定最佳群集变换估算，并且其中校正所述车辆的位姿基于所述最佳群集变换估算。

10.根据权利要求1所述的计算机实现的方法，该方法包括：在所述当前可观测地标集合与所述已勘测地标子集之间的一个或多个变换标度下确定一个或多个二维变换标度，基于所述一个或多个二维变换估算中每一者的变换成本确定一个或多个群集，以及从所述一个或多个群集中识别最佳变换估算。

11.一种用于映射、定位和位姿校正的系统，该系统包括：

处理器；

视觉测程模块，其使所述处理器沿着行进路线确定车辆的当前位置以及沿着相对于所述当前位置的所述行进路线确定当前可观测地标集合，所述当前可观测地标集合从得自通信地连接到所述处理器的成像设备的一个或多个立体图像中提取；

定位模块，其使所述处理器查询通信地连接到所述处理器的勘测地标数据库以检索相对于所述车辆所述当前位置的已勘测地标子集，所述勘测地标数据库包括先前存储的已勘测地标集合；以及

位姿校正模块，其使所述处理器在所述当前可观测地标集合与所述已勘测地标子集之间确定一个或多个二维变换估算；从所述一个或多个二维变换估算中识别最佳变换估算，所述最佳变换估算使所述当前可观测地标集合与所述已勘测地标子集之间的距离最小化；并且基于所述最佳变换估算校正所述车辆的位姿。

12.根据权利要求11所述的系统，其中，所述定位模块确定所述当前可观测地标集合的每个地标的形心并且其中所述已勘测地标子集包括所述已勘测地标子集中的每个地标的形心。

13.根据权利要求12所述的系统，其中，所述位姿校正模块将所述最佳变换估算识别为使所述当前可观测地标集合与所述已勘测地标子集之间的对应形心距离最小化的变换估算。

14.根据权利要求11所述的系统，其中，所述位姿校正模块基于一个或多个变换标度下的二维变换估算确定所述当前可观测地标集合与所述已勘测地标子集之间的变换估算集合，其中所述一个或多个变换标度从粗略变换标度增进为精细变换标度。

15.根据权利要求14所述的系统，其中，所述位姿校正模块基于每个估算的变换成本生成所述变换估算集合中的最佳变换估算的排序列表，并且基于将所述排序列表中的每个最佳变换估算与所述排序列表中的相邻最佳变换估算进行比较来确定最佳变换估算的一个或多个群集。

16.根据权利要求15所述的系统，其中，所述位姿校正模块基于所述一个或多个群集中每一者的所述最佳变换估算确定最佳群集变换估算，并且基于所述最佳变换估算校正所述车辆的位姿。