Autoware.Auto 本地化（Localization）

全局状态估计器
- 输入：全球参考的传感器数据（例如 GPS）
- 输出： sensor_msgs/PoseWithCovarianceStamped 帧内 /earth
姿势变压器
- 输入： sensor_msgs/PoseWithCovarianceStamped 在源框架中
- 输入：Transform，其中包含在 sensor_msgs/PoseWithCovarianceStamped
- 输入：变换的目标帧
- 输出： sensor_msgs/PoseWithCovarianceStamped 在目标帧中
环境传感器定位器
- 示例：NDT 匹配
- 输入：具有固定外部定位环境（例如激光雷达、雷达）的本地参考传感器数据
- 输入：全球参考的传感器数据图（例如点云）
- 输入： sensor_msgs/PoseWithCovarianceStamped 在 /map 来自 Global State Estimator 的帧中，通过 Pose Transformer 从 /earth ->转换 /map
- 输出： sensor_msgs/PoseWithCovarianceStamped 帧内 /map
里程状态估计器
- 输入：本地参考的传感器数据，没有固定的外部定位环境（例如车轮编码器、视觉里程计、加速度计、陀螺仪）
- 输出： /odom -> /base_link 变换
地图状态估计器
- 输入： /odom -> /base_link 变换
- 输入： sensor_msgs/PoseWithCovarianceStamped 在 /map 环境传感器定位器的框架内
- 输入： sensor_msgs/PoseWithCovarianceStamped 在 /map 来自 Global State Estimator 的帧中，通过 Pose Transformer 从 /earth ->转换 /map
- 输出： /map -> /odom 变换

变换

AVP 2020 演示的最终本地化堆栈应具有以下可用转换：

/base_link -> <sensor_frame> ：静态。由 URDF 文件发布 tf2::static_transform_publisher 或 robot_description 在使用 URDF 文件时发布。
- 解释：假设所有连接到移动机器人主体的传感器都是刚性完成的，不能自由移动。作为 /base_link 被定义为绑定到移动机器人身体上的固定点的框架，传感器和传感器之间的任何转换 /base_link 都是静态的。
/base_link -> /nav_base ：静态。由 URDF 文件发布 tf2::static_transform_publisher 或 robot_description 在使用 URDF 文件时发布。
- 解释：虽然 /base_link 框架的位置对某些车辆模型和长期路径规划很有用，但车辆的质心在短期路径规划和估计车辆动力学中更有用。然而，在某些操作设计领域，以足够的特异性来测量车辆的质心是不值得的，这就是为什么这个框架是可选的。
/odom -> /base_link ：动态。由 Odometry State Estimator 发布。
- 解释：虽然本地地图的定位提供了全局固定的位置估计，但没有任何传感器或算法可以定位到本地地图，不能被认为是绝对可靠的，并且不能始终提供连续、无差错的位置。此外，当尚未计算本地地图中的初始位置时，不能期望环境传感器定位器提供准确的定位估计。由于导航基于完整 /map -> /base_link 变换，因此即使在全局相对或地图相对位置尚不可用时，此估计也可用于导航。
/map -> /odom ：动态。由 Map State Estimator 发布。
- 解释：地图状态估计器采用环境传感器定位器提供的位置估计（a /map -> /base_link 关系）以及全局状态估计器提供的位置估计（a /map ->使用 -> /base_link 通过姿势变换器后的关系 transform) 并将它们融合成一个 -> 估计。然后，它将这个估计与 odom 框架的原点联系起来，作为 -> 变换，它会发布。这允许将里程计定位数据和环境传感器定位数据集成到 -> ->的变换树中，这应该被视为单个变换（即短暂的 -> /earth /map /map /base_link /map /odom /map /odom /base_link /map /base_link ) 通过导航和规划组件。
/map -> /semantic_map ：静态。发布者 tf2::static_transform_publisher 或地图加载器
- 解释：由于语义图的原点和传感器图的原点之间经常存在偏移，因此每个图的不同帧是必要的。此外，由于本地化通常比使用语义地图的函数对时间更敏感，因此典型的 /map 框架与传感器地图相关联，这避免了定位中的变换查找，但在使用语义地图的功能中需要一个，例如路径规划。使这种转换成为必需而不是可选可以避免传感器和语义地图之间不必要的依赖。此外，省略此转换将需要假设语义图和传感器图具有重合的起源，这将需要另一个不必要的相互依赖或对该假设的运行时验证，这会导致操作顺序依赖。
/earth -> /map ：如果使用地图服务器，则为动态但不连续并由地图服务器发布。如果未使用地图服务器，则为静态并由 tf2::static_transform_publisher
- 解释：由于传感器地图（在 /map 帧中发布）被定义为全局固定的，因此此变换将传感器地图的原点与 /earth 称为 ECEF 的帧的原点相关联。发布此变换对于环境传感器定位器使用全局状态估计器的输出创建初始位置估计是必要的。这种变换也是 Map State Estimator 能够利用 Global State Estimator 的输出作为融合输入所必需的。

图表

下图描述了这种架构：

AVPTFT树

本地化常见

构成初始化

相对定位器需要对姿态进行良好的初始估计，以便在合理的时间内收敛到一个好的解决方案。出于这个原因，需要一个位姿初始化器来估计车辆的位姿。

算法设计

替代定位方式为姿势初始化提供了可靠的来源。这就是为什么初始化器会在可能的情况下尝试在给定时间从变换树中查找变换。

如果给定时间的转换在给定的转换树中不可用，则初始化程序将尝试使用实现定义的策略推断给定时间的转换。当前可用的实现如下：

BestEfforInitializer ：需要外推时返回最新的可用变换。

相关问题

#142 实现RelativeLocalizerIntializerBase，以及一个简单的实现

本地化设计

概要

Autoware.Auto 本地化堆栈应符合 REP105[3]，并定义了以下帧：

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

本地化堆栈应具有以下组件：

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

地图被认为是一个独立的，但与本地化密切相关的关注点。

此外，下游组件应使用以下组件，该组件理解本文档中列出的语义：

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

使用以下接口：

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

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

介绍

本地化是任何更高级别的自动驾驶堆栈的重要组成部分。虽然纯粹的反应控制可以实现最小的 3 级功能，但任何更高级别的自治或更高级的 3 级功能都需要规划。为了使用计划，代理必须在计划执行期间知道其相对于计划的位置。这就是本地化提供的。

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

用例

在高层次上，定位算法必须做一件事：给定一个观察，该算法必须提供观察的坐标系相对于某个参考坐标系的变换。

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

一般来说，我们可以将用例细分为三种本地化，具有不同的强度或保证：

绝对本地化
相对本地化
里程计（即视觉里程计）

然后可以将这种相对本地化用例应用于或分解为各种特定用例：

Mapping/SLaM，即相对于动态地图的定位
相对于先验地图的定位，即相对于静态地图的定位

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

激光雷达
相机
GPS/IMU/INSS

也可以广泛考虑本地化输出或结果的用法：

全局算法：
- 行为规划
- 路由/全球规划
本地算法：
- 运动控制
- 运动规划
- 对象跟踪/占用地图
- 场景理解（例如车道检测）
- 运动估计（例如传感器融合）
航位推算

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

高级用例

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

绝对本地化

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

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

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

相对本地化

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

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

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

里程计

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

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

这种定位模式通常用于无地图驾驶，例如在 3 级高速公路自动驾驶场景中，或者在更高形式的定位被剥夺的情况下，例如在穿越 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/局部坐标系中进行

提供与 <tt>/earth</tt> 框架兼容的变换

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

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

更新局部惯性系

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

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

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

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

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

参考定位器初始化

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

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

REP105 合规性

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

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

理由：遵守此标准将使外部专家立即理解此本地化堆栈

理由：遵守此标准允许在此本地化堆栈中使用不是为自动驾驶用例专门构建的实现

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

/odom-/base_link 变换必须有界

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

如果车辆长距离行驶，则 /odom - base_link 变换的幅度将增大，可能到浮点精度成为问题的地步[3]。因此，我们需要一种机制来确保这种变换的幅度保持相当小，以避免大量浮点运算产生的错误。

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

机制

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

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

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

设计

为了支持上述用例和衍生需求，下面提出了本地化堆栈的分解。

成分

在最抽象的情况下，我们在本地化堆栈中定义以下组件。根据各种组件的用例和功能，可能只需要组件的某些子集。

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

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

地图管理器

地图管理器保持对局部惯性框架的控制。地图管理器的职责是为本地惯性坐标系的相对定位器提供参考地图，并为本地坐标坐标系提供适当的 ECEF 参考点。该组件还必须处理本地地图的更新，并将这些更改适当地传达给相关定位器和其他可能受到影响的组件。

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

输出：参考地图（本地化）

输出：成对 /earth 的变换 /map _ /map /odom

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

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

行为：如果已解决的变换靠近参考地图的边界，则应将警告传达给较大的堆栈。 “近”可以定义为距离地图的物理限制在 10 秒内（在这种情况下，系统可能会停止），或者距离可以使车辆保持在参考地图的边界（在这种情况下，系统可能会在没有新地图的情况下重新安排车辆路线）。

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

行为：当 /map 帧更新时，应用变换的逆 /earth 应 /map 应用于最新的 /map 变换 /odom 并与更新一起发布

基本原理 ：使用完整的变换树更新作为输入简化了该组件针对各种用例的初始化

基本原理 ：需要成对更新来保持 /base_link 绝对坐标系或全局坐标系中坐标系的一致性。

基本原理 ：本地化丢失是一个严重错误。应该通过适当地通知更大的系统来主动避免这种情况。

TF经理

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

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

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

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

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

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

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

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

此消息有时也可能包含一个 /odom - /odom 变换以表示 /odom 帧和子变换的位置更新

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

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

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

从中减去适当时间戳的里程表变换以提供 /map - /odom 变换
求解最小不确定性优化问题以确定更新对 /map - /odom 和 /odom - /base_link 帧的贡献
/map - /odom 被认为是固定的， /odom - /base_link 通过状态估计器更新。更新用于更新 /map - /odom 如果状态估计器创新太大，则通过 (1)

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

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

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

通过传感器融合算法融合变换
更喜欢更精确的变换
更喜欢越早到达的变换

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

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

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

基本原理 ：这个组件对于隔离每个单独的本地化模式的关注点是必要的，从而简化了它们的实现。

基本原理 ：所有转换都初始化为恒等转换，以简化其他组件的初始化。当初始化合适的map时，可以在不影响-transform的情况下，诱导一个step transform来更新 /earth -transforms ，保证连续性 /map /odom /base_link

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

基本原理 ：里程计框架的更新是必要的，以确保变换在幅度上是有界的，并最大限度地减少浮点运算中的误差。需要对历史变换进行逆更新以确保坐标系的全局一致性。更新可能有不同的形式以适应通信计算的权衡。

绝对定位器

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

输入：不适用（定义的实现，例如传感器输入）

输出：变换 /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

本地缓冲区核心

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

该组件将是 tf2::BufferCore .

建议使用以下 API：

类 LocalBufferCore : 受保护的 tf2::BufferCore { 公开： // 当收到/odom-/odom 消息时，处理整个帧图的更新；还处理排队 // /odom-/base_link 转换来支持这个 void addTransforms( const TFMessage & msgs); // 处理从 /base_link（过去）到 /base_link（现在）的转换，反之亦然 bool canTransform( const std::string & frame_id, time from, time to); // 处理从 /base_link（过去）到 /base_link（现在）的转换，反之亦然 TransformStamped lookupTransform( const std::string & frame_id, time from, time to); 使用 BufferCore::canTransform；使用 BufferCore::lookupTransform； }; // 类 tf2::BufferCore

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

基本原理 ：此组件允许本地算法在 /base_link 框架中运行，并在需要时更新其内部状态以与观察结果保持一致

基本原理 ：如果本地算法正在计算帧中跨时间的变换，那么在更新之前和之后的 /base_link 两个时间点之间累积的变换应该是相同的 /odom /odom

接口定义

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

输出

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

基本原理 ：某些组件可能会输出一个或多个变换

理由：这是一条标准消息

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

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

扩展

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

# 变换协方差
geometry_msgs/TransformStamped 变换
float64[9] 翻译协方差
float64[16] 旋转协方差
uint8 position_covariance_type
uint8 旋转协方差类型
uint8 COVARIANCE_TYPE_UNKNOWN = 0
uint8 COVARIANCE_TYPE_APPROXIMATED = 1
uint8 COVARIANCE_TYPE_DIAGONAL_KNOWN = 2
uint8 COVARIANCE_TYPE_KNOWN = 3

# TFCovarianceMessage
TransformWtihCovariance 变换[]

哪里 rotation_covariance 可以有 Bingham[2] 语义。

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

理由：添加不确定性以反映 NavSatFix 中的情况。此附加信息可用于下游使用的概率或稳健算法，例如运动估计或规划算法

笔记由于开销和标准消息中断，目前不建议使用这些自定义消息

参考地图表示

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

理由：这是一个标准的消息类型

基本原理 ：它可以扩展到可能是特定于实现的任意表示和布局

相对定位诊断

可以使用 DiagnosticStatus 消息报告相对定位算法的健康状态。

基本原理 ：这是标准消息类型

基本原理 ：这种形式的消息的语义和预期结果与DiagnosticHeader的不同，后者具有报告一般算法性能的语义

示例架构

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

仅限 GPS

仅 GPS 堆栈将包含以下组件：

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

因为没有里程计，使用此输出的本地算法可能必须设置为相对于 /map 框架进行操作以确保一致性。

基于无损检测

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

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

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

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

绝对定位器

同样，在此设置中，由于没有里程计，因此可能必须设置使用此输出的本地算法以相对于 /map 框架进行操作以确保一致性。

高速公路自动驾驶仪

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

里程计
TF经理

全栈

一个完整的、冗余的本地化堆栈将需要提议的本地化堆栈中的每个组件：

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

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

SLAM

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

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

地图

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

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

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

本地化指标

介绍

具体度量（即那些生成数值的度量）对于软件组件的一般开发和算法的开发很重要。

度量标准允许定义通用软件组件的验收标准，并提供一种通用语言来比较某些标准的实现，在这种情况下是本地化算法。

指标

这些指标分为四类：

软件质量指标
嵌入式软件指标
一般算法指标
定位算法指标

应该提到两个一般警告：

应该了解对度量标准的需求来自哪里。需求可能并非普遍正确或正确
对于软件质量指标以外的所有指标，指标值是特定于用例的，这意味着指定的值仅对给定的环境和硬件（即传感器、计算）设置有效

软件质量指标

定义了以下软件质量指标：

测试覆盖率（按照严格程度递增的顺序）：
1. 线路覆盖
2. 功能覆盖
3. 分行覆盖
4. MC/DC 覆盖范围
5. 基路径覆盖
通用软件质量度量，例如 CppDepend 工具所涵盖的那些
代码行
外部项目依赖的数量

应该提到两个警告：

只有当测试有意义时，测试覆盖率才有意义。实现测试覆盖率的随机 API 调用不如由需求和边缘案例驱动的测试有价值
软件质量指标对气味测试很有用，但不能替代人工审查和设计。这些指标可以定义建议，但不能定义硬性规则

嵌入式软件指标

为通用嵌入式软件定义了以下指标：

二进制大小
最大内存使用量
给定用例（输入、硬件、操作系统）的平均 CPU 负载

一般算法指标

定义了以下指标：

平均/最大/95% 算法运行时间
平均/最大/95% 算法迭代
平均值/最大值/95% 算法运行时间/迭代
算法复杂度（例如 Big O）

定位算法指标

定义了以下本地化指标：

平均/最大/最小/95% 平移误差（米）
平均/最大/最小/95% 旋转误差（弧度）
成功率作为初始猜测误差的函数
最小输入密度/尺寸

还可以定义进一步的非定性指标：

输入需求
保证（例如最优性、收敛性等）

本地化节点

目的/用例

需要一个带有样板的节点来操作本地化器并管理输入和输出数据流。

设计

RelativeLocalizerNode 是一个通用的相对定位节点模板，它使用 PoseInitializerBase 实现来管理分别符合 LocalizerConstraint 和 MapConstraint 中指定的接口的定位器实现和地图实现以及符合 inter 的地图实现。

类型约束被实现为封装一系列静态断言的结构，如果给定的算法实现不提供所需的接口，则编译失败并显示通知消息 RelativeLocalizerNode 。请参阅这些约束结构的文档以了解本地化和地图算法的新实现的接口需求。

在每个接收到的观察消息中，接收到的消息在获取的初始估计值的帮助下在定位器中注册并发布。
在每个接收到的地图消息中，定位器中的地图都会更新。

假设/已知限制

由于有多个回调，因此节点在任何阶段都应该在单个线程中运行。

输入/输出/API

输入：

地图讯息
观察消息
转换消息以进行初始估计

输出：

输出姿势信息

错误检测和处理

安全注意事项

未来的扩展/未实现的部分