域描述

该 common 子目录包含节点和库，其功能由 Autoware.Auto 系统中的许多包共享。

中心页面

• autoware_auto_tf2
• Autoware rviz 插件
• 协方差插入节点
• 时间间隔
• 空间散列
• 比较
• 测量转换
• 监控节点
• More-Thuente 线搜索
• 运动模型

autoware_auto_tf2

这是 autoware_auto_tf2 包装的设计文件。

目的/用例

通常，ROS 的用户依赖 tf（及其后继者 tf2 ）来发布和使用坐标系变换。 即使在 tf2 包含包 tf2_geometry_msgs 并且 tf2_sensor_msgs 允许在 geometry_msgs 和 sensor_msgs 中定义的消息类型之间轻松转换的情况下也是如此。 但是，AutowareAuto 包含一些特殊的消息类型，这些消息类型不能使用 ROS2 库在帧之间进行转换。 该 autoware_auto_tf2 软件包旨在为开发人员提供转换适用 autoware_auto_geometry_msgs 和 autoware_auto_perception_msgs 类型的工具。 除此之外，该包还 geometry_msgs 为 tf2_geometry_msgs .

设计

在编写 tf2_some_msgs 或贡献时，现有 tf2 框架 tf2_geometry_msgs 中的以下文件确保了兼容性和设计意图：

• tf2/convert.h
• tf2_ros/buffer_interface.h

例如：

该方法 tf2::convert 取决于以下内容：

反过来，这取决于以下内容：

tf2_geometry_msgs 的当前实现

在 ROS1 和 ROS2 标记的消息中，如 Vector3Stamped ， QuaternionStamped 具有相关的功能，如：

• getTimestamp
• getFrameId
• doTransform
• toMsg
• fromMsg

在 ROS1 中，为了支持 tf2::convert 和需要 doTransform 被标记的数据，非标记的底层数据，如 Vector3 ， Point ，具有以下功能的实现：

• toMsg
• fromMsg

在 ROS2 中，大部分 doTransform 方法都没有使用 toMsg ，并且没有使用来自 tf2 fromMsg 的数据类型 。 而是 使用 完成 ，因此未添加与基础数据相关的函数； 例如 , , 或移植到这个提交 ros/geometry2/commit/6f2a82 中。 带有和 的非标记数据 是 , 。 已修改 且未被 . doTransform KDL Vector3 Point toMsg fromMsg Quaternion Transform Pose toMsg PoseStamped

计划 autoware_auto_perception_msgs::msg::BoundingBoxArray

最初 的 粗略 计划 toMsg 是 根据 fromMsg . 数据类型的示例包括： 、 和 。 此外，应该进行实施，以便上游贡献也可以对 . doTransform BoundingBoxArray BoundingBox Quaternion32 Point32 geometry_msgs

假设/已知限制

由于函数签名中的冲突， 不遵循 convert.h /的预定义模板，并且兼容性 被破坏并且 编写方式不同。 transform_functions.h tf2::convert(..) toMsg

输入/输出/API

该库为 doTransform 以下数据类型提供 API，这些数据类型要么在其中不可用， tf2_geometry_msgs 要么消息类型是其中的一部分， autoware_auto_geometry_msgs 因此 autoware_auto_perception_msgs 是自定义的，并且不受任何 tf2 库的固有支持。 为以下数据类型提供了以下 API：

• Point32
• 内联无效doTransform（ const geometry_msgs::msg::Point32 & t_in, geometry_msgs::msg::Point32 & t_out, const geometry_msgs::msg::TransformStamped & transform)
• Quarternion32 ( autoware_auto_geometry_msgs )
• 内联无效doTransform（ 常量 autoware_auto_geometry_msgs::msg::Quaternion32 & t_in, autoware_auto_geometry_msgs::msg::Quaternion32 & t_out, const geometry_msgs::msg::TransformStamped & transform)
• BoundingBox ( autoware_auto_perception_msgs )
• 内联无效doTransform（ const BoundingBox & t_in, BoundingBox & t_out, const geometry_msgs::msg::TransformStamped & transform)
• BoundingBoxArray
• 内联无效doTransform（ const BoundingBoxArray & t_in, BoundingBoxArray & t_out, const geometry_msgs::msg::TransformStamped & transform)

此外，还添加了以下辅助方法：

• BoundingBoxArray
• 内联 tf2::TimePoint getTimestamp(const BoundingBoxArray & t)
• 内联 std::string getFrameId(const BoundingBoxArray & t)

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

挑战

• tf2_geometry_msgs 不 doTransform 为任何非标记数据类型实现，但可以使用相同的函数模板。 转换子数据时需要它，主数据确实有标记，并且可以对具有相同转换的子数据调用 doTransform。 这是有用的上游贡献吗？
• tf2_geometry_msgs 没有 Point , Point32 , 似乎不需要一个，非标准的实施 toMsg 也无助于转换。
• BoundingBox Quaternion32 像和 一样使用 32 位浮点数 Point32 来节省空间，因为它们在 BoundingBoxArray . 转换时最好转换为 64 位 Quaternion , Point 或 PoseStamped , 以重新使用 的现有实现 doTransform ，还是需要实现？ 模板化可能并不简单。

Autoware rviz 插件

目的/用例

需要支持插件来可视化 autoware.auto 中各种包的输出

设计

边界框阵列显示

这个类实现 rviz_common::RosTopicDisplay<autoware_auto_perception_msgs::msg::BoundingBoxArray> 了 BoundingBoxArray 消息的可视化。 它是一个很薄的包装器 rviz_default_plugins::displays::MarkerCommon ，通常功能与 default_rviz_plugins::MarkerArrayDisplay . 每当一个新的 BoundingBoxArray 到达时，每个边界框都被转换为一个标记并添加到 的内部显示队列中 rviz_default_plugins::displays::MarkerCommon ，该队列反过来处理队列中的标记及其可视化。

轨迹显示

此类实现 rviz_common::RosTopicDisplay<autoware_auto_planning_msgs::msg::Trajectory> 可视化轨迹消息。 它是一个很薄的包装器 rviz_default_plugins::displays::MarkerCommon ，通常功能与 default_rviz_plugins::MarkerArrayDisplay . 每当有新的 Trajectory 到达时，每个轨迹点都会转换为两个标记，并添加到 的内部显示队列中 rviz_default_plugins::displays::MarkerCommon ，该队列反过来处理队列中的标记及其可视化。 这两个标记包括 Arrow 轨迹航点的姿态和 Text 同一点的速度。 您还可以为标记选择颜色。 两种标记的比例和 Alpa 值可以独立选择。

ObjectPolygonDisplayBase

autoware_auto_perception_msgs::msg::Shape 这是一个抽象类，有助于为包含和 autoware_auto_perception_msgs:msg::ObjectClassification 字段 的类型创建插件。 这个基类为插件创建属性，允许基于类标签自定义形状的颜色和透明度值。 它还创建一个属性来打开或关闭形状的 3D 可视化。 除了属性之外，这个类还有一个 get_marker_ptr 函数，可以 visualization_msgs::msg::Marker::SharedPtr 为提供的形状和分类标签生成一个。
从这个类派生的子类必须定义 processMessage 函数，该函数将作为正在实现插件的消息的回调。

DetectedObjectsDisplayBase

此类派生自 ObjectPolygonDisplayBase 实现 RosTopicDisplay for autoware_auto_perception_msgs::msg::DetectedObjects 。 它将形状字段可视化为凸的 2D 或 3D 多边形，并根据 msg 中的类标签对其进行着色。

TrackedObjectsDisplayBase

此类派生自 ObjectPolygonDisplayBase 实现 RosTopicDisplay for autoware_auto_perception_msgs::msg::TrackedObjects 。 它将形状字段可视化为凸的 2D 或 3D 多边形，并根据 msg 中的类标签对其进行着色。 此类还创建文本标记以可视化 id msg 中的对象。

相关问题

• #152 - 创建用于显示 BoundingBoxArray.msg 的 rviz2 插件
• #900 - 为 TrackedObjects.msg 实现 rviz2 插件
• #1110 - 为 DetectedObjects.msg 实现 rviz2 插件

协方差插入节点

目的/用例

此节点旨在订阅提供的主题（ messages 默认情况下），向现有字段添加协方差将消息重新发布到主题 <input_topic>_with_overriden_covariance ，默认情况下为 messages_with_overriden_covariance . 如果输入消息没有协方差字段，则将填充具有协方差的等效消息。 例如：

• 如果 input_msg_type 是 Odometry ，它将重新发布，并覆盖协方差。 这同样适用于已经包含协方差的类型，例如 PoseWithCovariance or TwistWithCovariance 和它们的标记版本。
• 如果其中 input_msg_type 没有协方差，like Pose or PoseStamped 它将分别重新发布为 PoseWithCovariance or PoseWithCovarianceStamped 消息。 这同样适用于 Twist 它的衍生物。

当在 StateEstimatorNode 中使用此消息且此消息的原始提供者未填充协方差时，此类节点的一个可能用例是填充 Odometry 消息的协方差值。 StateEstimationNode 要求协方差有效才能发挥作用，因此我们可以使用此节点将其填充为对我们有意义的值作为显式工程因素。

设计

该节点旨在允许将其扩展到任何消息类型。 它需要一个主题、它的输入类型和一个指定协方差所有条目的条目。

例如，对于 Odometry 上面的消息示例，这可以是：

假设/已知限制

这仅适用于向消息添加协方差并适用于有限的消息集。 可以使它更通用，但这需要大量的工程设计，因此目前还没有以最通用的方式实现。 话虽如此，应该可以为任何可以保存协方差的消息扩展代码。

内部工作/算法

节点将首先猜测输出类型，然后为正确的输入类型和输出类型创建发布者和订阅者。 从此时起，每条传入的消息都将使用协方差进行更新并作为输出类型发布。

错误检测和处理

大多数错误可能来自缺少（未实现）的类型特征或使用尚未存在的类型的节点。 这些错误应该使节点崩溃，以通知用户该节点无法使用指定的类型或参数。

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

该节点可以扩展为使用更多类型。 见 covariance_insertion::convert.hpp ， covariance_insertion::add_covariance.hpp 。

相关问题

• #505 - 将状态估计器与 NDT 集成

时间间隔

该区间是标准的一维实数值区间。 该类实现了区间类型的表示和操作，并保证了构造的区间有效性。 实现了基本操作和访问器，以及其他常见操作。 请参阅下面的“示例用法”。

目标用例

• 范围或遏制检查。 区间类简化了涉及检查某个值是否属于某个范围或两个范围相交的代码。 它还提供一致的行为和对边缘情况的一致处理。

特性

• empty ：一个空区间相当于一个空集。 它不包含任何元素。 这是一个有效的区间，但因为它是空的，所以度量（长度）的概念是未定义的； 空区间的度量 不 为零。 该实现表示对空区间的度量，其中 NaN .
• 零度量 ：具有零度量的区间是边界完全相等的区间。 度量为零，因为区间仅包含一个点，并且点的度量为零。 但是，因为它确实包含单个元素，所以间隔 不是 空的。
• 有效 ：有效间隔为空或具有最小/最大界限，例如 (min <= max)。 在构造时，区间对象保证是有效的。 尝试构造无效间隔会导致抛出 runtime_error 异常。
• 伪不可变 ：一旦构造，更改间隔值的唯一方法就是用新的值覆盖它； 无法修改现有对象。

约定

• 对区间对象的所有操作都定义为区间类上的静态类方法。 这是一种函数式编程，基本上将类转换为命名空间，授予函数访问它们所操作对象的私有成员变量的权限。

假设

• 间隔仅适用于浮点类型。 这是通过静态断言强制执行的。
• 非空区间的构造函数接受两个参数“min”和“max”，它们必须是有序的（即，min <= max）。 如果违反此假设，则会发出异常并且构造失败。

示例用法

空间散列

空间哈希是一种数据结构，专为低维高效的固定半径近邻查询而设计。

固定半径近邻问题定义如下：

'对于点 p，找到所有点 p' st d(p, p') < r`

在这种情况下，'d(p, p') is euclidean distance, and r` 是固定半径。

对于每个 n 具有平均 k 邻居的点，该数据结构可以及时执行 m 近邻查询（为 m 不同的点生成近邻列表） O(mk) 。

相比之下，使用 kd 树进行连续最近邻查询会导致运行时间为 O(m log n) .

空间哈希的工作原理如下：

• 每个点都分配给预定义的边界区域中的一个 bin，由 x_min/x_max 和 y_min/y_max
• 这可以通过将 x 和 y 位置分别转换为 x 和 y 索引来完成
  • 例如使用包含 x_min 和 y_min 作为索引的 bin (0, 0)
  • 然后可以将两个（或更多）索引转换为单个索引
• 一旦每个兴趣点都被插入到哈希中，就可以开始近邻查询：
  • 首先计算参考点的 bin
  • 对于每个相邻 bin 中的每个点，在所述点和参考点之间执行显式距离计算。 如果距离低于给定半径，则认为所述点是近邻

在引擎盖下， std::unordered_multimap 使用了 an，其中键是 bin/voxel 索引。 bin 大小被计算为与查找距离相同。

此外，这种数据结构可以支持 2D 或 3D 查询。 这是在配置期间确定的，并通过配置类烘焙到数据结构中。 这样做的目的是避免紧密循环中的 if 语句。 配置类专业化本身使用 CRTP（Curiously Recurring Template Patterns）来执行“静态多态性”，并避免调度调用。

性能表征

时间

插入是 O(n) 因为底层哈希图的查找时间是 O(n) 针对哈希图的。 在实践中，hashmaps 的查找时间和插入时间应该是 O(1) .

删除点是 O(1) 因为当前 API 仅支持通过直接引用节点来删除。

在对抗性示例的情况下， 寻找 k 近邻是最坏的情况，但在实践中 。 O(n) O(k)

空间

该模块由以下组件组成：

• 内部哈希图是 O(n + n + A * n) ，其中 A 是任意常数（负载因子）
• 空间散列的其他组件是 O(n + n)

这导致 O(n) 空间复杂度。

状态

空间散列的状态由底层 unordered_multimap 的状态决定。

数据结构完全由 配置 类配置。 类的构造函数在数据结构中确定是否接受严格的 2D 或严格的 3D 查询。

输入

将数据引入数据结构的主要方法是通过 插入 方法。

输出

从数据结构中检索数据的主要方法是通过 近 （2D 配置）或 近 （3D 配置）方法。

也可以使用标准常量迭代器遍历整个数据结构。

未来的工作

• 性能调优和优化

相关问题

• #28: 到 autoware.Auto 的端口

比较

该 float_comparisons.hpp 库是一组用于执行近似数值比较的简单函数。 有单独的函数用于使用绝对边界和相对边界执行比较。 绝对比较检查以 . 为前缀 abs_ ，相对检查以 . 为前缀 rel_ 。

该 bool_comparisons.hpp 库还包含一个 XOR 运算符。

该库的目的是提高代码的可读性，并在使用数值和布尔比较时减少印刷错误的可能性。

目标用例

近似比较旨在用于检查两个数字是否位于某个绝对或相对区间内。 该 exclusive_or 函数将测试两个值是否转换为不同的布尔值。

假设

• 近似比较都带一个 epsilon 参数。 此参数的值必须 >= 0。
• 该库仅用于浮点类型。 如果库与非浮点类型一起使用，将引发静态断言。

示例用法

测量转换

这是 measurement_conversion 包装的设计文件。

目的/用例

这个包提供从消息类型到测量类型的转换，即继承自 MeasurementInterface . 它们包含值及其协方差。

设计

from 转换实现为模板类 的静态模板函数 autoware::common::state_estimation::convert_to 。 还有一个额外的模板化函数 autoware::common::state_estimation::transform_measurement ，它将转换作为输入，应用于测量。

实际的转换将通过专门化这些类和函数来实现。 通用界面将始终遵循以下原则：

假设/已知限制

假定消息对象是有效的，例如，在 PoseWithCovariance 消息中，协方差矩阵必须是对称的和半正定的，四元数必须是单位四元数等。

输入/输出/API

在撰写本文时，API 包含两种模板化方法：

请在 API 文档中搜索此函数以查看可用专业化列表。

对于两个模板都专用的消息和测量类型，组合功能 message_to_transformed_measurement 自动可用。

内部工作/算法

不适用

错误检测和处理

不适用

安全注意事项

不适用

参考/外部链接

不适用

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

可以添加产生双精度测量类型的变体。

相关问题

不适用

监控节点

这是 monitored_node 包装的设计文件。

目的/用例

运行 Autoware 时，需要确保所有节点都获得了足够的资源，并且能够以预期的频率运行。 任何涉及多个节点的处理管道也应遵守总体延迟配额，以确保成功运行。 因此，需要某种监控功能来检测节点何时执行低于容差并做出适当反应。

设计

该 monitored_node 软件包提供了一个 API，用于构建具有内置回调时间监控功能的 ROS2 节点。

受监控的节点将使用 ROS 计时器 API 监控自己的性能，并在超过时间限制时将消息记录到 ROS2 记录器和诊断主题。 然后使用专用的错误监视器节点监视诊断主题。 这个专用节点负责检测故障并采取适当的措施。

每个被监控的发布节点应与主话题一起发布该话题的预期发布间隔。 受监视的订阅者节点可以使用此信息来确保以预期的速率调用其订阅者回调。 订阅者回调所花费的时间应该少于最小消息间隔，以便跟上系统的其余部分。 可以自动检查此条件。 程序员可以对订阅者回调施加进一步的时间限制。 这是为了处理处理链中的端到端延迟受到限制并且链中的每个步骤都获得配额的情况。

假设/已知限制

假设：

• ROS 计时器是准确的。
• DDS 实施的延迟最小

输入/输出/API

用户不会从类继承 rclcpp::Node 来构建节点，而是使用：

发布者和订阅者是使用 create_monitored_<> API 创建的。 MonitoredSubscription 并且 MonitoredPublisher 是 ROS 基类的包装器，以透明地实现时序检查。 MonitoredSubscription 对象包含有关订阅主题的预期发布间隔的信息 。 此信息可用于确定当前节点中发布者的预期发布间隔。 例如，过滤器节点订阅传入主题并在传出主题上发布。 传出主题的预期速率取决于传入主题的速率。

除了在 cpp API 中指定间隔和延迟之外，用户还可以从启动文件中的 ROS2 参数中覆盖它们。 参数名称为：

发布间隔应该在发布者端设置，回调持续时间在订阅者端设置。

内部工作/算法

检测异常

1. 监控订阅回调频率：

   在回调开始时，会启动一个计时器。 计时器设置为在 API 中指定的 max_interval 到期。 如果在计时器到期之前第二次回调进入，则检查计时器上的已用时间。 如果节点运行正常，则经过的时间应该大于 min_interval。 如果计时器在第二次回调发生之前到期或 min_interval 检查失败，则触发处理程序。

2. 监控订阅回调持续时间：同样的方法适用。

   在调用回调之前启动计时器，并在回调结束时重置。 计时器设置为在 API 中指定的 max_duration 到期。

沟通

受监控节点通过单个诊断主题与外部错误监控器进行通信。 每次发生事件时，例如。 callback_started, callback_ended，包含事件名称和时间戳的消息被发送到主题。 监控节点分析此事件流并输出指示车辆能力的信号。 错误监视器节点可以在专用的安全强化硬件上运行，以确保其正确运行。

区间传播

受监控订阅者的 min/max_interval_ms 成员作为期货持有。 仅在从上游被监控节点接收到相关信息后才设置该值。 使用定义为期货的参数创建的受监控发布者将依次向其订阅者宣布这些值。

当间隔值通过发布者传播到下游受监控节点时，设置期货的值应始终发生在从当前受监控节点发布第一条消息之前。 否则，回调的监视将保持不活动状态，直到设置了间隔； 回调本身正在正常处理。

错误检测和处理

安全监视器功能围绕正常的 rclcpp 节点，不应干扰节点的正常错误处理机制。

安全注意事项

• 专用的错误监控节点是一个高价值的目标，因为它可以确保整个系统的正常运行，从而防止某些类型的攻击。 因此，应该采取通常的步骤来保护 Linux 和 ROS 以防止攻击。 安全监视器应在高价值部署的专用安全硬件上运行。
• 安全监控功能依赖于 ROS2 API 工作。 因此，适用于保护 ROS2 和 Linux 的常规做法。

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

总体规划见相关问题。

相关问题

• #821 - 检测节点的传入消息何时被跳过
• #1233 - Autoware 监控系统：实施 Autoware 错误监视器

More-Thuente 线搜索

我们实现了 More-Thuente 线搜索方法作为线搜索方法之一。 它实现了 LineSearch 来自 line_search.hpp .

该实现紧跟 Jorge J. More 和 David J. Thuente的论文 “保证足够减少的线搜索算法” 。 我们在以下部分之一中对该论文进行了简短摘要。

目标用例

线搜索算法通常用于帮助优化问题，例如牛顿法。

More-Thuente 线搜索是一种比固定步长更强大的线搜索版本。 它保证选择一个使目标函数充分减小的步骤，同时旨在使目标函数的结果值接近最优值。

我们的目标是使用这种方法作为 NDT 实施中用于定位的牛顿方法的辅助。

假设

我们在这里假设，以下两种情况之一成立：

• 我们从某个点 x_0 开始搜索 函数 f的 最小值 。 然后 \phi^\prime(0) = f^\prime(x_0) < 0 。 F X 0 φ ' ( 0 ) = F ' ( X 0 ) < 0
• 我们从某个点 x_0 开始搜索 函数 f的 最大值 。 然后 \phi^\prime(0) = f^\prime(x_0) > 0 。 F X 0 φ ' ( 0 ) = F ' ( X 0 ) > 0

该实现将基于导数 F ' ( X 0 ) 。

此外，步长 α 及其边界必须是非负的。

简短的论文回顾

定义

假设 定义在[ 0 , \infty] 上 的目标函数 一个步 α > 0 ，即所谓的 强 Wolfe 条件 （论文中的方程 1.1 和 1.2）： φ : R → R [ 0 , ∞ ] φ ' ( 0 ) < 0 α > 0

φ ( α ) ≤ φ ( 0 ) + μ φ ' ( 0 ) α ∣ φ ' ( α ) ∣ ≤ η ∣ φ ' ( 0 ) ∣

通常， 1/2 的小值 ， \eta 是接近 1 的值。 请注意 \mu \le \eta 。 μ 1 / 2 η 1 μ ≤ η

在我们的例子中，使用优化函数 F ( × ) ，起点 X 0 ，优化方向 d 和步长 α ，我们定义函数 φ 如下（论文中的公式 1.3）：

φ ( α ) ≡ f ( X 0 + α p ) , α≥0 _ _

在此过程中，我们使用如下定义的辅助函数 ψ ( α ) （就在论文中的公式 2.1 之前）：

ψ ( α ) ≡ ϕ ( α ) - μ φ ' ( 0 ) α

迭代搜索步长

该算法可以总结如下（遵循论文第2节中的 搜索算法 ）。

ψ ( α 吨 ) ≤ 0 , φ ' ( α 吨 ) > 0

对于给定的步骤 α 吨 和值的区间 [ α l , α 你 ] ：

1. 检查强沃尔夫条件是否适用于 α 吨 。 如果他们这样做 - 以 α 吨 作为结果终止过程。
2. 使用函数 \psi 或 \phi从区间 [\alpha_l, \alpha_u] 和当前步骤生成下一步长度 。 这部分显示在论文的第 4 节“试值选择”中。 [ α l , α 你 ] ψ φ
3. 使用函数 \psi 或 \phi 和当前步骤 \alpha_t 更新区间 [\alpha_l, \alpha_u] 。 这部分包含两种算法：当 仍然使用 \psi函数时的 更新算法 （在论文中的定理 2.1 之后）和 在我们切换到使用函数 \phi之后使用的 修改更新算法 （在论文中的定理 3.2 之后显示） 。 这些算法的不同之处仅在于它们使用的函数。 [ α l , α 你 ] ψ φ α 吨 ψ φ
   

运动模型

动机

为了预测任何物体向前的运动，需要运动模型的概念。 在这个包中，我们为不同的运动模型（包括线性和非线性模型）提供了一个接口和具体实现。

提议的设计

所有运动模型都实现 autoware::prediction::MotionModelInterface ，文件中定义的 CRTP 接口 motion_model_interface.hpp ，即，它们应实现以下功能：

符合此接口的运动模型可以称为：

假设/已知限制

运动模型可以有多种实现方式，例如线性模型或差动驱动模型。 这些是提议的 autoware::prediction::MotionModelInterface . 其中一些实现可以是通用的，而另一些则必须针对特定状态进行定制。 一般来说，更复杂的非线性模型需要针对特定​​状态进行专门化。

内部工作/算法

静止运动模型

在 autoware::common::motion_model::StationaryMotionModel 类中实现的静止运动模型是确保状态保持静止的运动模型，即状态没有变化。

因此，该运动模型的雅可比矩阵是一个大小与状态向量维数匹配的简单单位矩阵。

线性运动模型

类中实现的线性运动模型 autoware::common::motion_model::LinearMotionModel 是期望所有维度（例如，变量 X 、 Y 、 Z 、参见 common_variables.hpp ）都是独立的运动模型，因此这种模型的转移函数可以用矩阵乘法来表示。 因此，预测发生在公式 s 1 = J s 0 之后。

该运动模型的雅可比行列式专门针对每个状态。 如果给定状态的变量在一行中出现，则可以使用为给定 的位置、速度和加速度变量构建块矩阵的效用函数，例如 。 这种状态的转换矩阵示例如下所示： Δt _

FloatState<X, X_SPEED, X_ACCELERATION, Y, Y_SPEED, Y_ACCELERATION>

差动驱动运动模型

差动驱动运动模型对沿其面向的方向移动的对象建模，而不是在二维中自由移动。 这个方向（偏航）是可变的。 这是一个非线性运动模型，提供了两个特定的变体：CVTR（恒定速度和转弯率）模型和 CATR（恒定加速度和转弯率）模型。

这些模型的推导可以在 motion_model.ipynb 文件中找到。

CATR（恒定加速度和转弯率）

有关详细信息，请参见 autoware::common::motion_model::CatrMotionModel typedef。

该模型假设一个状态：

中 X 和 是的 是物体的 2D 坐标， θ 是它的偏航角（方向，从 X 轴逆时针测量）， v 是物体面对方向的线速度， ω 是角速度（又名转率）， 一个 是沿定向方向的线加速度。

假设常数 ω 和 一个 ，下一个状态可以重写如下：

将其放入 SymPy 中，预测状态可以计算如下：

ω 为 0 和不为 0 的情况有所不同。

在非零情况下，雅可比将是：

而如果 ω 为零 ，则它简化为以下形式：

CVTR（恒速和转弯率）

有关详细信息，请参见 autoware::common::motion_model::CvtrMotionModel typedef。

这种运动模型与 CATR 非常相似，只是稍微简单一些。 它假设状态：

其中 X 和 是的 是物体的 2D 坐标， θ 是它的偏航角（方向，从 X 轴逆时针测量）， v 是物体面对方向的线速度，而 ω 是角速度（又名转弯率）。

假设常数 ω 和 v ，下一个状态可以重写如下：

和以前一样，将其插入 SymPy，我们得到：

对 s取 s_{\mathrm{next}} 的雅可比矩阵 得到： s n e x t s

参考

#865 - 重新设计卡尔曼滤波器类层次结构