【AutowareAuto】Localization Design

高度总结

Autoware.Auto 定位模块应符合 REP105[3]，并定义了以下坐标系：

• /earth
• /map
• /odom
• /base_link
• 额外的坐标系可以附加到 /base_link 坐标系来表示传感器坐标系

定位模块应具有以下组件：

• 地图管理器（输出 /earth-/map 变换）
• tf 管理器（输出完整的转换链）
• 绝对定位器（例如 GPS，输出 /earth-/base_link 变换）
• 相对定位器（例如 NDT 匹配，输出 /map-/base_link 变换）
• 局部定位器（例如视觉里程计、IMU、输出 /odom-/base_link 变换）

建图被认为是与定位密切相关但独立的相关的问题。
此外，下游组件应使用以下组件，该组件理解这一文档中的语义：

• LocalBufferCore（更新 /odom-/base_link 转换，允许 /base_link-/base_link 跨时间转换）

具有以下接口：

• 参考地图（输入到相对定位器）：PointCloud2
• 相对定位状态：DiagnosticStatus
• 所有定位组件的输出（为了一致性）：TFMessage

根据用例，可能需要某些组件子集。

引言

定位是任何更高级别的自动驾驶的重要组成部分。 虽然通过纯反应控制可以实现最小的 L3 级功能，但任何更高级别的自动驾驶或更高级的 L3 级功能都需要进行规划。 为了使用计划，载体必须在计划执行期间知道它相对于计划的位置。 这就是定位所提供的。

此外，定位提供了解决自动驾驶中较难问题所需的基础功能，例如需要地图的问题。 这些问题包括处理复杂的道路结构，例如交叉路口，以及全局规划问题。

使用案例

在高级别自动驾驶上，定位算法必须做一件事：给定一个观测，该算法必须提供观测坐标系相对于某个参考坐标系的变换

变换是完整的 6 自由度变换，用于表示 3 维笛卡尔空间中的任意刚体平移和旋转。

一般来说，我们可以将用例细分为三种定位，具有不同的优势或保证：

• 绝对定位
• 相对定位
• 里程计（如视觉里程计）

然后可以将此相对定位用例应用于或分解为各种特定用例：

• 建图/SLAM，即相对于动态地图的定位
• 相对于先验地图的定位，即相对于静态地图的定位

此外，还可以考虑各种输入方式，包括：

• 激光雷达
• 相机
• GPS/IMU/INSS

定位的输出或结果的用途也可以广泛考虑：

• 全局算法：
  • 行为规划
  • 路径/全局规划
• 局部算法：
  • 运动控制
  • 运动规划
  • 对象跟踪/栅格地图
  • 场景理解（例如车道检测）
  • 运动估计（例如传感器融合）
• 航位推算

最后，可以考虑在大篷车中长途旅行（即越野）的具体用例。

高级使用案例

下面描述了每个高级用例。

绝对定位

绝对定位涉及提供相对于不变的绝对惯性系的车辆姿态，例如，提供相对于地球中心的 LLA（或 ECEF，首选）坐标中的车辆姿态。

在这种情况下，GPS 可以提供绝对定位。通过使用相对于固定的独特特征（例如道路上的独特标志）的自我位置，也可以实现绝对定位。

这种定位算法通常会以至少双精度提供 LLA 坐标中的估计姿态。

相对定位

相对定位涉及计算自我坐标系中的观测变换到局部地图坐标系的变换。本地地图通常采用 ENU 坐标，带有一些 ECEF 参考点。

自动驾驶中使用的大多数定位算法都属于这一类。

这些算法通常解决非凸优化问题，这意味着解决方案是局部最小值。这意味着这些算法通常需要一个很好的初始猜测，这可以由绝对定位组件提供，或者从以前的解决方案热启动。

里程计

里程计涉及计算自我车辆相对于局部特征（例如车道、道路边界和标志）的变换。

该部分中的算法与 SLaM 算法和其他特征提取方法重叠。

这种定位模式通常用于无地图驾驶，例如在 L3 级高速公路自动驾驶场景中，或者在没有更高形式定位的情况下，例如穿越 GPS 死区或地图质量不佳的空间时不足以实现良好的相对定位（即隧道）。

地图类型

定位器操作模式的一个根本区别是参考地图是静态的还是动态的。

静态地图

如果提供了静态地图，那么定位组件应该为观测坐标系相对于地图坐标系产生一些变换。

一般来说，地图应该在 ENU 坐标系中，以兼容简单的笛卡尔推理和规划。

在这种情况下，地图的原点应该有一些与之关联的 ECEF 坐标。地图的大小应该有合理的界限（例如 < 83 公里 [3]），以避免由于行星的曲率引起的错误。

因此，可能需要地理围栏或实施一种允许定位器切换静态地图的机制。当车辆仍在当前地图的范围内时，应该进行这种切换。后续地图应与当前地图重叠，并且地图框中引用的任何对象或其任何衍生物都应具有更新的姿态。

通常，此类静态地图的处理、平铺、切换和表示是特定实现的。

动态地图

一般来说，如果打算进行建图，则定位器的地图是动态的，无论是作为核心功能（即稍后生成静态地图），还是作为更大用例的一部分（如 SLAM：同时定位和建图） ）。

在这种情况下，观测结果通常必须独立存储。在存在新的观察结果的情况下，聚合图可能会发生变化，例如由于束调整或位姿图优化的影响。

通常，地图的表示不应影响定位组件的外部行为。

输入用例/传感器

可以使用各种传感器来实现定位。本节将考虑其中的一些用例和注意事项。

一般来说，定位算法的工作原理是从当前观测和参考中提取独特的特征，并确定最能将观测到的特征与参考特征对齐的变换。

激光雷达

存在许多局部配准算法，它们操作点、线、平面或分布特征。

该领域常用的算法包括ICP和NDT。

也存在全局匹配算法。

雷达

许多适用于 LiDAR 的技术也适用于 RADAR，因为两者都是在 3D 笛卡尔空间中产生回波的有源传感器。

适用于 LiDAR 和 RADAR 的算法示例是 ICP [1]。

相机

通过相机定位的工作方式与 LiDAR 或 RADAR 类似，一般来说，特征是从观察到的图像中提取的，并与参考特征进行匹配。

相机定位技术与 3D 定位技术的不同之处在于必须推断深度，通常是用来解决优化问题。

INSS (GPS, IMU)

作为一种定位方式的 GPS 与其他传感方式的不同之处在于 GPS 的参考采用 GPS 卫星的形式。

GPS（和 INSS）传感器产生绝对位置估计，通常在地球相对 LLA 坐标系中。要使用这些结果，应以与其他定位实现一致的方式将这些 LLA 坐标通过参考 ECEF 点转换为本地 ENU 框架。

相比之下，IMU 提供线性加速度和旋转速度观测。这些观测通常非常频繁地到达（例如，以 1 kHz 的频率），并且可以在一段时间内使用运动模型（简单的或更明智的）累积以产生位置更新。这些位置更新可以与可以提供相对于参考系的位置的定位源相结合。

通常，不同输入或传感器模式的处理是单独实现的。一个兼容的实现应该在目标坐标系和一些固定的地图或参考坐标系之间提供一个 6 DoF 变换。

输出用例

目标坐标系相对于固定或参考坐标系的变换的输出可以被自动驾驶中的许多算法使用。

全局算法

全局算法相对于世界上固定的坐标系进行操作。这些算法一般涉及一些更远距离的规划。

行为规划

行为规划器通常根据当前姿态、当地道路网络以及感兴趣区域内或附近的障碍物来设置运动规划问题。

行为规划器可能不需要车辆状态的全保真表示，但它需要转换，以便该组件的所有输入都在兼容的坐标系中。

一般来说，行为规划器会在 /map 坐标系中操作。

全局规划

全局规划器计算一系列lanelet 集合，这些集合可以将目标车辆从其在世界中的当前姿态转换到目标姿态。

全局规划器只需要粗略表示车辆的状态（即位置、航向、速度）。定位组件需要在与世界坐标系兼容的坐标系中提供姿态。

通常，全局规划器将在 /earth 或 /map 框架中运行，具体取决于用例。

局部算法

局部算法通常在目标车辆的局部坐标系中运行。这些算法通常直接使用观测并在 /odom 框架中运行。如果没有可用的里程计源，则这些算法可以在 /map 坐标系中交替运行。

运动估计

目标车辆相对于固定（惯性）坐标系的姿态或变换可以随时间累积，以产生关于车辆动力学的动力学估计。一个简单的例子可能涉及对位置随时间的变化进行数值微分，以推导出车辆的速度。

可以使用更高级的运动估计形式，例如使用运动模型和状态估计器的形式。

运动或状态估计应与定位分开，可以将产生的变换作为输入，并生成车辆的完整运动学或动动力学状态作为输出。状态估计器可以从对观测到的姿态或变换的概率处理中受益。在这种情况下，表示关于变换的高斯噪声和关于旋转四元数的 Bingham[2] 噪声可能是有益的，从而在消息表示中产生额外的 9 + 16 = 25 双倍。

自动驾驶的规划和控制组件可以使用这种动态估计。

运动控制

给定轨迹和当前状态，运动控制器产生控制命令，该控制命令限定后续状态相对于参考轨迹的误差。

在每次迭代中，必须提供一个状态，该状态表示车辆相对于参考轨迹的位置。定位的结果可用于填充状态，或将状态转换为与参考轨迹兼容的坐标系。

在这种情况下，鲁棒控制可能受益于不确定性参数的添加。

运动规划

给定当前状态、目标状态、障碍物和可行驶空间，运动规划器生成与当前状态兼容的轨迹，在可行驶空间内，不与障碍物碰撞，并向目标状态前进。

定位的结果可用于隐式生成当前状态，并可用于确保运动规划器的所有输入都在兼容的坐标系中。

类似地，稳健的规划算法可能受益于不确定性参数的添加。

对象跟踪/栅格地图/场景理解

对象跟踪、栅格地图和场景理解算法通常遵循类似的模式。这些算法涉及通过（处理过的）传感器输入接收观察结果，并使用这些观察结果更新某些世界属性的后验状态估计。

从根本上说，这涉及确保算法的状态与观察结果处于一致的坐标系中。例如，这将涉及确保在观察对象的目标或自身坐标系与存储对象轨迹的某些惯性坐标系（例如地图框）之间存在变换。

航位推算

另一个常见的用例是航位推算，当更强的定位信号不可用时。例如，如果 RTK GPS 用作定位的主要形式，则当本车位于没有 GPS 的位置（例如隧道中）时，可能需要进行一些航位推算。

处理此类情况的一些方法包括使用里程计、IMU 或其他局部观察，例如车道标记。

总的来说，航位推算可以被认为是与定位分开的一个问题，因为它不能提供绝对位置，而只能提供相对位置。 航位推算可以用作在定位实现中结合传感模式以提高更新率的一种方式（例如，使用 GPS 和 IMU）。 或者，航位推算可以作为运动估计的一部分（即作为时间预测步骤的一部分）来实现。

具体用例

考虑了在大篷车中进行长途旅行的具体用例。这将涉及高速行驶至少三个小时。

根据硬件设置的详细信息，可能需要所有三种形式的定位。

如果使用高精度 RTK GPS，并且在旅途中 GPS 始终可用，那么除了对局部坐标系进行周期性更新之外，除了将绝对坐标转换为局部坐标系的机制之外，不需要额外的机制（即地图切换），因为规划通常发生在局部坐标系中。

如果将穿越某些缺乏 GPS 的区域，则必须在缺乏 GPS 的区域中使用某种形式的里程计或相对定位。这可能涉及维护可实现定位的参考地图，或维护可支持局部规划和跟踪过程的局部更新。

如果使用的主要定位方式是相对定位，则必须在启动时使用绝对定位的形式来确定哪个是最相关的参考地图，以及目标在该参考地图中的初始估计。此外，必须以不中断从属流程（例如跟踪和计划）的方式处理地图切换。在穿越 GPS 匮乏的地区时，与在地图匮乏的地区中一样，同样的普遍问题也适用。

要求

定位实现必须满足一些要求才能与上述用例兼容：

• 在目标坐标系和指定的惯性坐标系之间提供刚体变换
• 提供与 /earth 坐标系相关的变换
• 如果使用相对定位，则必须提供管理参考地图的机制
• 相对定位器应该具有一种机制，允许从启动或参考剥夺状态、绝对定位或本地源进行初始化
• 定位模块的实现应符合 REP105[3]
• /odom-/base_link 转换不得无限期地增长。

提供自身和惯性坐标系之间的转换

定位算法的核心功能满足了这一要求。此要求满足以下用例：

• 全局规划（如果惯性坐标系与路网的世界坐标系兼容）
• 行为规划
• 运动规划（可以根据目标帧使用局部或相对定位）
• 运动控制（可根据目标帧使用局部或相对定位）
• 运动估计
• 对象跟踪（可以根据目标坐标系使用局部或相对定位）

如果使用提供绝对定位的方法，例如 GPS，则必须提供某种机制来将此绝对位置转换为局部坐标系。

该要求可能有额外的子条款，例如与结果的属性或机制的性能有关的子条款，例如：

• 有界误差
• 有界运行时间

注：规划、跟踪和运动估计通常在笛卡尔/ENU/局部坐标系中进行

提供与 /earth 坐标系相关的变换

对于长期规划用例以及涉及使用高清地图的用例，必须提供相对于 /earth 坐标系的绝对变换。 因此，必须提供某种机制将局部惯性坐标系与绝对/地球坐标系连接起来。

注： /earth 框架中提供了一些有用的功能，通过 LLA 或 ECEF 坐标。

更新局部惯性系

规划、跟踪和运动估计通常发生在局部笛卡尔/ENU 坐标系中。此外，这些算法通常会做出平面运动假设，因为道路参与者通常会固定在地面上。鉴于地球是圆的，随着二维坐标离本地坐标系的原点越来越远，误差就会累积。

因此，重要的是局部坐标系的影响区域是有界的，并且当目标车辆移动到局部坐标系的边界附近时，它应该切换到新的局部坐标系。

这对于防止将无限错误引入本地过程（即规划、跟踪、运动估计）是必要的。

注：地球是圆的，许多算法做出平面假设。需要地图切换来控制近似误差的涌入。

注：内存不是无限的，所以不太可能存储整个世界的参考地图。

参考定位器初始化

为确保参考定位器的最佳操作，应提供初始姿态估计。

注：大多数相对定位算法需要合理的初始猜测才能收敛到良好的估计。

REP105 合规性

REP105[3] 是为机器人指定的标准坐标系。 在坐标系的选择和设计中考虑了各种问题，包括：

• 地图切换
• 定位漂移/校正
• 地图坐标系和初始化约定

注：遵守此标准将使外部专家立即理解此定位模块

注：遵守此标准允许在此定位模块中使用并非为自动驾驶用例专门构建的实现

注：该标准规定了可以解释各种失败案例并编码社区专业知识和期望的行为

/odom-/base_link 变换必须有界

局部算法涉及关于不同时间和空间的变换结构。一个隐含的假设是这些变换将随着时间平滑。因此，本地算法必须针对 /odom 坐标系进行操作，该坐标系具有平滑演化的内置假设。这通常假设 /odom 框架原点固定在空间中而不移动。

如果车辆长距离行驶，则 /odom-base_link 变换的幅度将增加，可能会导致浮点精度成为问题[3]。因此，我们需要一种机制来确保此变换的幅度保持在合理的范围内，以避免在大幅度下出现浮点运算错误。

注：浮点算术会产生错误，这些错误与算术中涉及的值的大小成正比。这些误差应该被最小化，因此意味着应该限制或最小化变换的幅度。

机制

满足这一要求的主要机制是选择合适的定位算法。

• 根据用例，必须提供一些定位算法的子集，作为满足基本要求的机制
• 如果使用绝对定位，则必须通过软件组件将此信号转换为局部坐标系以满足基本要求
• 必须提供切换地图的软件组件，以满足惯性系的更新
• 必须提供一种机制，可以为相关定位算法提供初始猜测
• 一种批量更新框架图的机制，使得/odom-/base_link变换可以保持很小，并确保平滑，并且需要全局一致性

这些机制在如下所述的通用本地化架构中部分实例化。

设计

为了支持上述用例和衍生需求，下面提出了定位的分解。

组件

在最抽象的情况下，我们在定位模块中定义以下组件。根据各种组件的用例和功能，可能只需要组件的一些子集。

• 地图管理器
• TF管理器
• 绝对定位器（例如 GPS）
• 相对定位器（例如 NDT 匹配）
• 里程计

为了支持对 /odom-/base_link 变换幅度的控制，需要一种嵌入下游算法 LocalBufferCore 的附加机制。

地图管理器

地图管理器保持对局部惯性系的控制。地图管理器负责为局部惯性系的相关定位器提供参考地图，并为局部坐标系提供适当的 ECEF 参考点。该组件还必须处理本地地图的更新以及这些更改与相关定位器和其他可能受影响的组件的适当通信。

输入：给定时间步长的完整tf树（即包含一个或多个时间步长以下变换的消息：/earth-/map、/map-/odom、/odom-/base_link）

输出：参考地图（定位）

输出：配对的 /earth-/map 和 /map-/odom 变换

输出：其他地图特征（如 规划）

行为：这个组件应该管理地图的切换。当车辆接近地图的边界时，该地图应从库加载新地图，并使地图的特征可用于更大的系统。地图的边界可以定义为距离地图物理边界的 10 秒以内，或者距离可以使车辆保持在参考地图范围内的最后一条道路序列的 10 秒以内。如果用例不需要地图切换，那么可能不需要地图切换，只需要简单的加载和发布数据。

行为：如果已解决靠近参考地图的边界的变换，则应向更大的堆栈传达警告。 “接近”可以定义为距离地图的物理边界在 10 秒内（在这种情况下系统可能会停止），或者距离最后一条道路序列在 10 秒内，可以将车辆保持在参考地图的边界（在这种情况下，系统可能会在没有新地图的情况下重新规划车辆）。

行为：应该使用 /earth-/base_link 转换来确定何时应该发生地图切换

行为：当 /map 坐标系更新时，应将 /earth-/map 应用变换的逆应用到最新的 /map-/odom 变换并随更新一起发布

注：使用完整的tf树更新作为输入简化了该组件针对各种用例的初始化

注：需要成对更新以保持绝对坐标系或全局坐标系中 /base_link 坐标系的一致性。

注：定位丢失是一个严重的错误。应通过适当通知更大的系统来主动避免这种情况。

TF 管理器

每个定位模式都提供了自我车辆相对于它们的参考坐标系的姿态。这些变换通常与定义的变换树不兼容。

因此，这个组件是必要的，它可以组合所有定位模态并确保它们以与定义的变换树一致的方式输出。

该组件协调定位模块的各个部分，并代表定位模块的主要输出及其与自动驾驶系统其余部分的接口。

输入：绝对定位（/earth-/base_link）

输入：相对定位（/map-/base_link）

输入：里程计（/odom-/base_link）

输入：地图更新（/earth-/map）

输出：给定时间步长的完整变换树（即包含一个或多个时间步长以下变换的消息：/earth-/map、/map-/odom、/odom-/base_link）

• 此消息有时也可能包含 /odom-/odom 变换以表示 /odom 坐标系和子变换的位置更新

行为：TF 管理器只有在收到 /earth-/map 变换后才能被视为已初始化。这可能涉及该组件的一些初步输出

行为：当接收到里程计输入时，此变换被添加到最新的 /odom-/base_link 变换中，产生的变换被添加到坐标系图中，并添加到包含缓存地图和里程计坐标系变换并发布的消息中

行为：当接收到相对定位输入时，/map-/odom 框架以实现定义的方式更新，例如：

1. 从中减去适当时间戳的里程计变换以提供 /map- /odom 变换
2. 解决了最小不确定性优化问题以确定对 /map-/odom 和 /odom-/base_link 坐标系的更新贡献
3. /map-/odom 被认为是固定的，/odom-/base_link 通过状态估计器更新。如果状态估计器创新太大，则使用更新通过（1）更新 /map-/odom

行为：当接收到绝对定位输入时，可以减去变换的地图和里程计分量以提供 /map-/odom 变换

行为：当接收到地图更新输入时，应将新的变换消息输入到输出消息中，并且应在新变换之前包含前一地图变换的时间

行为：如果绝对和相对定位输入可用于同一时间范围，则对 /map-/odom 框架的更新是实现定义的。这些实现可能包括：

• 通过传感器融合算法融合变换
• 更精确的变换
• 更快到达的更多变换

行为：定期（即每 N 秒，或每 N 次里程计更新），TF 管理器应通过在输出消息的开头添加适当的 /odom-/odom 变换来通知 /map-/base_link 帧的更新。剩余的输出可能：

• 是给定时刻的名义更新
• 包含一个完整的变换树（带有 /odom-/base_link 历史）

行为：在启动时，每个变换都被初始化为恒等变换

注：该组件对于隔离每个单独的定位模式的关注点是必要的，从而简化了它们的实现。

注：所有变换都初始化为恒等变换以简化其他组件的初始化。当适当的地图被初始化时，可以在不影响 /odom-/base_link 变换的情况下引入一个步进变换来更新 /earth-/map 变换，确保连续性

注：处理多种定位模式是特定于用例的。本文档仅概述了更新相应坐标系的基本行为

注：里程计坐标系的更新是必要的，以确保变换在范围上有界，并最大限度地减少浮点运算中的错误。需要对历史变换进行逆更新以确保坐标系的全局一致性。更新可以采用不同的形式以适应通信-计算的权衡。

绝对定位器

绝对定位器（例如 GPS 驱动程序）在绝对坐标系中提供自我的当前姿势。

输入：N/A（实现定义，例如传感器输入）

输出：变换 /earth-/base_link，其中旋转使得 z 轴远离地球，x 轴指向东方

注：这是一种独立的传感模式

笔记
如果使用生成 NavSatFix 消息的 GPS 驱动程序，则可能需要附加组件将点转换为适当的 ECEF 变换。

相对定位器

相对定位算法涉及生成变换以最佳匹配或注册关于地面实况的观察。这些算法通常需要一个良好的初始猜测，除了基本事实参考。根据架构假设和给定用例的参数，也可以将运动约束编码到实现中。

输入：N/A（实现定义，一些传感器输入）

输入：参考地图

输入：变换树，其中 /map-/base_link 变换用于初始化

输出：提供相对于参考地图的 6 DoF 刚体变换（平移、旋转）（/map-/base_link 之间的变换）

输出：应该提供某种形式的诊断，指定算法的状态

行为：必须验证参考地图以确保存在所有必要的特征

行为：如果 /map-/base_link 之间的转换在传感器输入的时间戳可用，则应使用此转换来初始化算法。

行为：如果没有初始位姿估计可用于传感器输入的时间戳，则行为是实现定义的。

注：许多方法可用于冷启动初始化，包括零初始化，或读取关闭时写入的文件。这些方法的适用性取决于用例，因此不能为所有实现预先指定

注：类似地，可以使用多种方法进行外推，例如使用最后一次变换，或使用运动模型预测当前姿势。任何方法的适用性取决于用例。

（相对/局部）定位算法的输入是实现定义的。这些输入可能包括但不限于：

• LiDAR 点云（原始或下采样）
• 雷达斑点
• 相机图像
• IMU 读数
• 里程计读数

里程计

里程计的工作原理类似于相对定位。 在这种情况下，算法提供关于先前观察的变换。 实现该组件行为的算法包括（视觉/激光雷达）里程计算法和 SLAM 算法。

输入：N/A（实现定义，一些传感器输入）

输出：提供相对于先前观察的 6 DoF 刚体变换（平移、旋转），特征为变换 /ego-/base_link

LocalBufferCore

为了保持 /odom-/base_link 转换的大小有界，需要批量更新转换。 这些更新包括用给定的变换更新 /map-/odom 变换，并用上述变换的逆更新 /odom-/base_link 变换的历史，以确保保持全局一致性。

该组件将是 tf2::BufferCore 的封装。

建议使用以下 API：

class LocalBufferCore : protected tf2::BufferCore
{
public:
  // Handles whole frame graph update when /odom-/odom message is received; also handles queueing of
  // /odom-/base_link transforms to support this
  void addTransforms(const TFMessage & msgs);
  // Handles transforms from /base_link (past) to /base_link (present), or vice versa
  bool canTransform(const std::string & frame_id, time from, time to);
  // Handles transforms from /base_link (past) to /base_link (present), or vice versa
  TransformStamped lookupTransform(const std::string & frame_id, time from, time to);
  using BufferCore::canTransform;
  using BufferCore::lookupTransform;
};  // class tf2::BufferCore

注：需要此组件来确保对 /odom 框架的更新在分布式系统中同步

注：该组件允许本地算法在 /base_link 坐标系中运行，并在需要时更新其内部状态以与观察结果一致

注：如果局部算法在 /base_link 坐标系中计算跨时间的变换，则 /odom-/odom 更新前后两个时间点之间的累积变换应该相同

接口定义

针对上述设计提出了具体的接口定义。

输出

所有组件都输出标准的 TFMessage。

注：某些组件可能会输出一个或多个变换

注：这是一条标准消息

注：所有组件都应该使用它来保持类型一致性

注：对于通常输出一个变换的组件，使用这种类型允许扩展性或批处理，视情况而定

扩展

如果算法开发者需要使用不确定性语义，那么建议如下消息：

# TransformWithCovariance
geometry_msgs/TransformStamped 变换
 
float64[9] translation_covariance
float64[16] rotation_covariance
uint8 position_covariance_type
uint8 rotation_covariance_type
 
uint8 COVARIANCE_TYPE_UNKNOWN = 0
uint8 COVARIANCE_TYPE_APPROXIMATED = 1
uint8 COVARIANCE_TYPE_DIAGONAL_KNOWN = 2
uint8 COVARIANCE_TYPE_KNOWN = 3

# TFCovarianceMessage
 
TransformWtihCovariance tranforms[]

其中 rotation_covariance 可以具有 Bingham[2] 语义。

注：变换消息是标准消息，表示标准笛卡尔空间中的刚体变换

注：添加了不确定性以反映 NavSatFix 中的不确定性。此附加信息可能对下游使用的概率或稳健算法有用，例如运动估计或规划算法

笔记
由于开销和破坏标准消息，此时不建议使用这些自定义消息

参考地图表示

PointCloud2 被提议作为参考地图表示的一种方式。

注：这是一种标准的消息类型

注：它可以扩展到可能特定于实现的任意表示和布局

相对定位诊断

相关定位算法的健康状态可以与 DiagnosticStatus 消息一起报告。

注：这是标准消息类型

注：这种形式的消息的语义和预期结果与具有报告一般算法性能的语义的 DiagnosticHeader 的语义和预期结果不同

示例架构

为了证明设计的有效性，并为实施者提供指导，提出了一些示例用例和架构。

仅 GPS

仅 GPS 模式将由以下组件组成：

• 绝对定位器（GPS 驱动程序）
• 地图管理器（仅定期更新本地参考系）
• TF 管理器

由于没有里程计，使用此输出的局部算法可能必须设置为针对 /map 坐标系进行操作以确保一致性。

基于NDT

基于 NDT（或任何其他相关定位器）的模式至少需要以下内容：

• 相对定位器（NDT匹配）
• 地图管理器
• TF 管理器

如果地图适当地覆盖了服务区，地图管理器可以简单地生成单个地图。或者，如果使用适当的地图适当地初始化相对定位器，则地图管理器可以基于相对于参考地图的位置知道切换到哪个地图。

如果没有其他形式的初始化可用，或由用例指定，则需要以下附加组件来确保模块可以正确初始化：

• 绝对定位器

同样，在此设置中，由于没有里程计，使用此输出的局部算法可能必须设置为针对 /map 坐标系进行操作以确保一致性。

高速公路自动驾驶

对于最小的高速公路自动驾驶，车辆只需要知道它相对于局部特征（例如车道标记）的位置，或者车辆之前所在的位置。因此，这样的模块只需要：

• 里程计
• TF 管理器

全栈/全部模式

完整的冗余定位模块将需要提出的定位模块中的每个组件：

• 地图管理器
• TF 管理器
• 绝对定位器
• 相对定位器
• 里程计

绝对定位可用于初始化地图管理和相对定位。在一般运行时，观测里程计可用于运动估计。在剥夺场景中，可以使用相对和/或里程计来维持最小功能，并在恢复更高级别的传感功能时重新启动绝对或相对定位。

SLAM

同时定位和建图涉及在运行时生成地图，并针对该地图进行定位。这些算法通常涉及“内部”问题，在此期间定位算法将当前观察与最近的历史相匹配，以及“外部”问题，其中针对全局一致性（即束调整、姿态图优化）校正整体地图。

在提出的定位堆栈的上下文中，执行 SLAM 任务的组件可以被视为等效于里程计组件。如果外部问题产生关于已知地图坐标系的全局估计，那么它也可以被视为相对定位器组件。

建图

建图通常涉及聚合观察结果并解决全局优化问题，以确保各种优化之间的一致性，如 SLAM 案例中所述。在建图算法与注册/匹配算法分开的情况下，单独的映射组件可以简单地接收对显着类型和主题以及给定时间步长的变换树的观察。

然后可以转换观察结果以生成对全局地图的初始猜测，在此基础上可以在非实时循环中进行细化。结果可能存储在某处。

通常不建议在安全关键用例中使用异步建图过程的结果。

参考

• [1] Vehicle Localization with low cost radar sensors, Ward and Folkesson
• [2] The Bingham Distribution of Quaternions and its Spherical Radon Transform in Texture Analysis, Kunze and Schaeben
• [3] REP105 - Coordinate Frames for Mobile Platforms