域描述
该 control 子目录包含与车辆控制器功能相关的节点和库。 此目录下的软件包使自动驾驶汽车能够通过将输入轨迹转换为纵向和横向命令来适当地遵循参考轨迹。 为了确保安全，报告错误和有关控制器的系统统计信息的测试框架模块和库也可能是该目录下的包。 中心页面
控制器基础设计
目的
该基类的目的是封装控制器的共同功能。
然后，该通用功能旨在简化特定控制器算法的实现。
API 实现子类
compute_command_impl(State) 必须实施。 这定义了在标称轨迹以下条件下控制器的控制行为。
check_new_trajectory(Trajectory) 可以覆盖以允许用户定义是否接受还是拒绝轨迹。 一些示例可以包括检查提供的轨迹是否符合控制算法所需的某些假设，例如指定的采样率。
handle_new_trajectory(Trajectory) 可以覆盖以允许用户更新簿记，例如该轨迹的算法特定表示。
设计
一些输入验证已烘烤到设计中。 compute_command 如果状态与轨迹不在同一框架中，则将引发异常。 用户有责任确保输入兼容。
行为
基本控制器类的许多基本行为旨在实施，以便算法实施者不必担心实施。 此外，这将确保所有实施此接口的控制器在边缘情况下都表现出相同的行为。
实施以下行为：
安全停止行为
如果不存在轨迹或其他不可用的轨迹，车辆应平稳地停止。
将在以下条件下生成算法计算算法的控制命令将生成平稳停止的算法：
- 如果没有提供轨迹
- 提供了一个空轨迹
- 如果提供的车辆状态超出了时间或空间的轨迹
参考点跟踪
在每个呼叫中 compute_command(State) ，都更新了有关轨迹的参考索引。 参考索引是给定状态过去 （或等于）的最后一点。
- 参考索引只能随后对 compute_command(State)
- 参考索引已重置为呼叫 set_trajectory(Trajectory) 变体的0
不同的算法适用于空间或时间参考。 因此，提供了在时间和空间感知中跟踪当前状态的API。
控制器设计
控制器是自动驾驶堆栈不可或缺的一部分。 控制器以高速率运行，并确保适当的车辆遵循参考轨迹，从而最大程度地减少相对于预先指定的路径或轨迹的误差。
本文档旨在适当地定义自动驾驶堆栈控制器组件的用例和需求。 由此，可以得出控制器的适当设计和体系结构。
用例
本节旨在定义控制器软件组件必须满足的用例。 本节分为高级和原子用例。 高级用例是整个自动驾驶汽车（AV）堆栈必须能够满足的用例。 然后，这些高级别用例被用于得出原子用例，该案例中的控制器必须孤立地满足。 高级用例
以下用例以不同级别的自主权呈现，从2（纵向和横向控制）最多5（完全自治）开始。
以下存在说明性用例的集合（非排量）：
- 车辆必须能够以低速安全（<= 20 kph）安全导航（2-5）
- 该车辆必须能够高速行驶（> 20 kph）（2-5）
- 车辆必须能够操纵狭窄的空间（即引进，平行停车手）（3-5）
- 上述用例必须在不同的车辆平台上实现
然后安全导航的能力，然后（非详尽地）组成以下用例：
- 该车辆必须能够留在特定的可驾驶空间（例如车道）（2-5）
- 车辆必须能够在其驾驶信封内停止对物体（实际或虚拟，例如停止线）（2-5）
- 车辆必须能够避免在其可驱动空间内（3-5）
原子用例
然后，每个高级用例都会适当分解。
可以实现与安全导航有关的每个陈述的用例，基于可以计划适当的轨迹，以便满足上述所有案例。
控制者对安全操纵的责任归结为以下用例：
- 给定一组输入，控制器必须能够实现具有界限级别的动态状态的指定序列
更具体地说，鉴于参考轨迹是一系列时期的动态状态（例如姿势，速度，加速度），控制器必须能够生成一系列的致动命令，以便在每个时间步骤，车辆的实际动态状态下在某些相对于参考动态状态的误差范围内。
请注意，未指定使用哪种错误度量，或使用了哪种错误界限（在哪个时间步长）。 这些界限和指标的具体需求留给最终系统集成器。
然后，操纵狭窄空间的能力是（非急切）的以下用例组成：
- 该车辆必须能够在所有配置状态的实现级别的实现级子集上，并具有有界的误差级别
需求
然后，每个原子用例通常都可以分解为一组需求。
一般的假设是，控制器不打算实现车辆性能限制的操作。 做出此假设是为了简化接口，并使控制器更容易支持多个车辆平台。
另一个假设是，将车辆平台限制为在平面上运行。 也就是说，出于计划和控制的目的，车辆被限制在SE（2）空间。
有限的动态误差
以下假设是针对车辆轨迹的一般情况以下的：
低速
以下假设是针对低速轨迹的情况。
高速
以下假设是针对高速轨迹的情况。
基于上述假设，必须对车辆动力学进行建模。 至少这意味着必须对轮胎进行建模。 车辆的其他要素可能会或可能不会建模，例如：
为了使控制器在动态车辆模型的假设下成功控制车辆，必须实时获得以下信息：
- 纵向速度
- 横向速度
- 执行器姿势（例如车轮角度）
- 偏航率
可以使用其他实时信息，例如：
最后，可以实时使用一些附加信息，尽管它们可能被假定为（运行时）静态：
- 车辆质量
- 车辆重心
- 轮胎特性（即转向坡度等）
- 转动惯量（相对于 z 轴）
实现配置状态
针对实现配置状态的情况做出以下假设：
- 车辆可以向前和向后移动
- 车辆可能是 N-link
- 配置状态至少包含位置（x，y）和航向
来自任何（静态）状态的最极端的配置是与当前姿势直接横向的配置（即平行停车，余量很小）。 此设置将用作以下派生需求的灵感。
请注意，N 点转弯的等效设置也可以用作灵感。
要实现半任意配置状态，需要满足以下需求：
- 车辆的旋转必须能够在有或没有纵向速度的情况下驱动到极端位置
- 车辆必须能够同时满足正负纵向速度/位移（例如，车辆必须能够在驱动和倒档设置之间切换）
- 车辆必须能够产生一些小的、离散的纵向位移单位，并以有界误差准确检测该位移的满意度
支持不同的车辆平台
针对支持不同车辆平台的情况做出以下假设：
- 可以控制的驱动都在单个刚体平台上（即对于 N 连杆车辆，可以控制转向和油门的所有车轮都位于单个刚性连杆上）
- 单轨近似适用于车辆平台（即，前轮和/或后轮使用 Ackerman 形式）
- 油门或加速度被假定为可以作为一个整体应用于车辆平台的体积参数（即，与作为每个车轮的扭矩应用相反）
这些假设通常是为了简化建模假设，从而通过相当简单的接口促进运动规划器、控制器和车辆接口的解耦。
如果必须打破这些假设中的任何一个，则以下情况之一可能是正确的：
- 车辆是一个高度非标准动力学的平台
- 用户希望车辆在接近或处于其动态极限的情况下运行
在所有这些情况下，可能更适合使用特定于特定用例的定制规划架构（例如，涉及规划器、控制器、车辆接口和车辆平台的更紧密耦合）。
机制
为了满足需求，控制器需要以下输入、输出。 此外，控制器还必须能够满足某一组行为。 行为
控制器必须遵循的基本行为是：
- 生成一个控制序列，该序列产生一系列（动态）状态，这些状态相对于参考轨迹有误差
可以通过选择满足错误拒绝的适当控制器算法，并在理论上或充分测试以建立错误界限来实现这一需求。
满足这种基本行为可能需要的一些额外行为需求可能包括：
- 获取和排序转换，以便可以将自我状态转换为与提供的轨迹兼容的框架
请注意，这些需求是可选的，并由系统集成商最终决定。
控制器可能支持的其他行为包括：
- 重心的系统识别，其他假定的运行时常数； 误差估计
- 动态状态或系统模型错误级别超过某个阈值时的诊断（即这些错误的保护级别）
输入
需要以下输入：
- 自我状态信息：
- 可转换为轨迹框架的位置 (x, y, heading)（所有用例）
- 纵向速度（安全机动用例）
- 横向速度（高速安全机动）
- 偏航率（高速机动）
- 车轮角度（所有用例）
- 参考轨迹至少：
以下输入是可选的：
- 纵向加速度（高速机动）
- 横向加速度（高速机动）
- 轮角率（高速机动）
需要以下输入，但可以将其视为运行时静态（例如配置参数）：
- 车辆质量/惯性
- 车辆重心
- 车辆轮胎特性
- 偏航/转动惯量
输出
需要以下输出：
可以提供以下可选输出来支持可选行为：
设计
然后提出以下设计考虑： 输入
控制器的输入应为三种类型：
- 车辆状态（高频率，触发主题）
- 轨迹（以较低的速率，次要主题）
- 转换消息
基本原理
- 输入 (1) 和 (2) 是控制器核心功能所必需的
- 需要输入 (3) 以确保输入 (1) 可以与输入 (2) 处于兼容状态
车辆状态
车辆状态类型应 至少 包含以下信息：
- x 位置（ 相对于某个坐标系 的重心）
- y 位置（ 相对于某个坐标系 的重心）
- 方向（作为虚数）
- 参考坐标系的标识符（例如 string frame_id ）
- 纵向速度（ 重心 相对于一些 假设的静态 坐标系）
- 横向速度（ 重心 相对于一些 假设的静态 坐标系）
- 车轮角度（前轮和后轮，假设为单轨车型）
- 时间戳，即原始传感器数据生成时间对应的时间点
- 偏航率
所有参数均采用标准 ISO 单位（如适用）（米、秒、千克、弧度）。
基本原理
- 车辆姿态（x、y 位置、方向）是相对于重心的，因为这些坐标可以很容易地转换为用于运动学模型（参见 [1]），而相反的情况则不适用于运动学模型转换为动力学模型。 高速机动用例需要动态车辆模型。
- 方向 (3) 表示为一个虚数，以简化正弦和余弦的计算，并减少 SO(2) 中状态之间的歧义。
- 姿态是相对于坐标系给出的，以便它可以明确地与轨迹给出的动态状态兼容。 这是基本功能所必需的
- 考虑到纵向控制是加速度的控制（下一个导数），纵向速度 (5) 是更直接地闭合控制回路的最低需求
- 需要横向速度 (6) 来估计车辆的动态效应和/或在动态操纵期间完全指定车辆的状态。 这是高速机动用例以及与动态车辆模型之间的转换所必需的。
- 指定车轮角度 (7) 而不是路径曲率，因为高速机动情况需要指定车轮/轮胎动力学。 虽然曲率可以通过车轮角度计算，但在相反方向上并非严格如此。
- 如果在动态和运动学控制情况下都有等效表示（例如参考坐标、曲率表示等），则优先考虑动态情况。 这是因为动态情况发生在较高的速度，较低的延迟，因此较少的中间计算可能是首选。
- 车轮角度 (7) 用于状态表示，因为车辆状态估计的实现可能使用车辆里程计。 进一步使用车轮角度代替曲率，因为车辆的转向动力学可能具有多于一个的自由度，这不能用曲率明确表示
- 假设速度 (5)、(6) 是相对于静态坐标系的，因为出于规划和控制的目的，大多数相关坐标系（例如 /world ） /map 可以被认为是静态的。 这个假设进一步简化了处理并允许我们使用标准的转换消息
- 需要时间戳 (8) 以确保辅助值可以及时正确对齐（例如通过插值）
- 时间戳与原始传感器数据相同，因此当结果以不同延迟到达时可以正确对齐数据
- 仅预期前轮和后轮角度以提供某种程度的可扩展性，例如后轮转向平台，同时保持仅驱动刚体平台的假设。 平台上所有车轮的独立转向超出了车辆控制器的范围，因为这将需要可能接近真实车辆平台极限的复杂动力学
轨迹
轨迹类型应 至少 包含以下信息：
- 参考坐标系的标识符（例如 string frame_id ）
- 轨迹点的有界序列， 至少 包含以下信息：
- 车辆状态中表示的所有信息（冗余帧标识符和时间戳除外），具有相同的语义
- 表示给定动态状态的目标时间的持续时间
- 时间戳
基本原理
- 需要一个坐标系标识符（1），以便将观察到的车辆状态放入与轨迹一致的坐标系中
- 需要时间戳 (3)，以便轨迹中的每个点都植根于时间。 这可以帮助在接收新轨迹之间进行路径跟踪。
- 轨迹点 (2) 由车辆状态 (2a) 形成，以确保为测量、限制和拒绝错误的目的而进行兼容的表示。 此外，这减少了表示开销。 可以应用类似的推理来需求相同的语义
- 每个轨迹点都需要一个持续时间（2b），以便轨迹的每个点都可以明确地植根于空间和时间。
- 需要轨迹的有界表示来支持静态内存需求，这是一种满足安全关键用例的机制。
- 时间戳与原始传感器数据相同，因此当结果以不同延迟到达时，数据可以正确对齐。 此外，如果参考框架是非静态的（例如 3 级高速公路驾驶案例），此信息可用于“根”轨迹
变换
转换类型应至少包含以下信息：
- 父坐标系
- 子坐标系
- 时间戳
- 翻译（米）
- 回转
该 geometry_msgs::TransformStamped 类型或由该类型组成的任何更高级别的消息都满足这些需求。
基本原理
- 刚体变换允许我们在坐标系之间转换车辆状态估计
- 时间戳允许转换移动帧中的坐标
输出
控制器的输出应为以下类型：
- 控制指令（参考值）
为了满足可选行为，可能会输出其他类型：
- 错误统计
- 系统识别参数，其中可能包含假定为运行时静态的参数（如质量），以及这些相关参数的误差
控制命令
控制命令类型应至少包含以下信息：
- 车轮角度（前后，假设单轨模型）
- 加速
这些参数应该具有被视为 参考值 的语义。
基本原理
- 控制命令被认为是参考值，因为底层车辆平台可能包含嵌入式 ECU 上的控制器。 如果没有关于车辆接口和车辆平台本身的强假设，这些值不能被视为前馈值，这在一般情况下可能无法满足。
- 车轮角度采用具有类似阿克曼转向的单轨道模型，以简化建模假设。
- 提供前后转向角以处理前、后和独立的转向平台。 没有考虑额外的转向自由度，因为我们假设驱动只发生在单个刚体平台上。 包含额外的自由度会引入显着的轮胎打滑，而有效使用这种自由度可能需要车辆在其机动能力附近运行。
- 提供所需的整体加速度而不是单独的车轮角度，以简化建模假设和接口。 如果需要单独的车轮扭矩，可以将其委托给车辆接口级别的内部控制器或外部 ECU。 引入单个车轮扭矩将使控制器和运动规划层所需的建模大大复杂化。
错误统计
错误统计类型应包含以下信息：
- 每个相关车辆状态字段的错误
基本原理
- 误差统计可以提供控制器性能的度量。 这可用于比较实现之间的控制器性能。
- 可以在运行时使用错误统计来确定车辆是否在预期的性能范围内运行。 如果错误超出某些范围，则可能会发出警告或接管警报。
- 车辆状态的所有元素的误差统计都是相关的，因为控制器的核心功能是拒绝参考状态和自我状态（的所有元素）之间的误差。 所有错误信息都是可用的。 例如，较高位置导数中的误差可能是领先的误差指标。
系统识别统计
系统标识统计类型应包含以下信息：
- 估计（假定的）运行时静态参数，例如：
- 车辆质量
- 车辆重心
- 轮胎特性（即转向坡度等）
- 基于估计参数与使用参数的偏差的误差度量
基本原理
- 控制器错误统计数据可用于估计动态模型规范中的自由度
- 此信息可用于关闭循环以进行高度动态的规划
参考
[1]使用线性时间变化的模型预测控制对多孔紧急情况做出反应。 耆那教等人。
控制器参考实现
本文档定义了控制器组件的参考实现。 至少满足本文档中定义的输入、输出和行为的软件组件（即库、可执行文件、可执行文件集或软件黑盒）应能够与任何足够兼容的 Autoware 一起运行。自动衍生自动驾驶堆。
介绍
控制器可以被认为是自动驾驶规划堆栈的底部。 它为车辆接口生成命令，以便车辆遵循给定的轨迹。
范围
本文档为控制器列出了一组最小的输入、输出和行为。
特别是，本文档定义了输入和输出的消息类型，以及它们定义背后的基本原理。 行为是根据预期结果定义的，并且类似地概述了基本原理。
符合这些输入、输出和行为的实现将能够与任何足够符合 Autoware.Auto 的自动驾驶堆栈一起运行。
本文档特别没有概述控制器算法的任何实现细节（例如，是否使用运动模型预测当前车辆状态，使用什么算法等）。 这些细节留给算法开发人员。
其他行为和输出可用于控制器实现。 这些应适当记录。 如果使用了额外的输入，那么在不包含额外输入的情况下，本文档中概述的最小行为应该仍然是可能的。
参考实现
本节概述了最小控制器实现所需的输入、输出和行为 输入
控制器具有三个输入：
转换消息
标准 tf2_msgs/TFMessage 应该是辅助输入。
基本原理 ：控制器可以接收不同坐标系中的轨迹和车辆运动状态。 有了可用的转换，就可以将两个对象转换为兼容的坐标系，而无需注入辅助节点来执行此操作。
轨迹消息
轨迹消息表示控制器应尝试满足的一系列运动状态。
轨迹消息具有以下形式：
std_msgs/标头标头 TrajectoryPoint[100] 个点 uint32 大小
其中轨迹点具有以下形式：
std_msgs/持续时间 time_from_start geometry_msgs/Pose 姿势 float32 速度_mps float32 加速_mps2 float32 heading_rate_rps
默认值 ：所有轨迹点和轨迹字段应默认为 0
扩展 ：将来，此消息可能包含更高的衍生信息。
基本原理 ：提供有限数量的点，因为应该以固定速率提供新轨迹。 由于具有有限的大小，有界数据结构还意味着更具确定性的通信时间。
基本原理 ：轨迹点镜像，但与标准的 JointTrajectory 消息不同。 这是因为标准消息允许有歧义的空间，而此消息则不允许。
基本原理 ：更高的衍生信息（例如加速度、航向速率）可用于为控制器和轨迹规划器提供更具表现力的语言。 如果不需要该信息，则提供零值，这对于控制器和运动规划器在语义上是一致的
有关消息的更多详细信息，请参阅包中的正式消息定义 autoware_auto_planning_msgs 。 虽然为方便起见，此处重复此消息及其基本原理，但应将消息包视为事实来源。
车辆运动状态消息
车辆运动状态表示车辆的当前运动状态，并用于通知将导致车辆遵循当前轨迹的控制命令。
车辆运动状态消息具有以下形式：
std_msgs/标头标头 轨迹点状态 geometry_msgs/变换增量
该 TrajectoryPoint::time_from_start 字段还应填写当前状态和先前状态消息之间的时间差。
如果可用，成员的位置字段 TrajectoryPoint 应包含自我车辆相对于固定框架的位置。 如果帧是动态帧（例如 base_link ），则该位置字段应视为不可用或无效。 该构件的进一步运动学应该始终可用，并根据适当的坐标系进行填充，并额外假设坐标系是静态的。
delta 成员是消息的变换，应该表示自我车辆的坐标系相对于它的最后位置和方向的位置更新。
默认值 ：轨迹点应适当地默认初始化
默认值 ：默认情况下，增量变换字段应具有零平移和单位四元数。
扩展 ：状态和变换不确定性可以作为方差或协方差矩阵添加。 这对于概率算法（例如稳健的 MPC）可能是必需的
基本原理 ：使用底层轨迹点是因为控制器可能需要运动学信息。 此外，以反映轨迹的方式表示车辆状态符合控制器的语义目的：将当前状态与状态序列匹配
基本原理 ：提供了相对于自我车辆先前位置和方向的变换，因为虽然绝对位置可能并不总是准确可用，但这种相对变换应该具有一定程度的准确度（例如，通过 IMU 信息、里程计信息等） .)。 该信息的可用性允许在局部坐标系内进行规划和跟踪。 例如，在 3 级高速公路驾驶用例中，轨迹可能植根于自我车辆在某个时间戳的位置。 这些相对变换的累积将允许控制器正确地跟随轨迹。
基本原理 ：运动学由固定框架假设填充，以简化该字段的更新和使用。
有关消息的更多详细信息，请参阅包中的正式消息定义 autoware_auto_vehicle_msgs 。 虽然为方便起见，此处重复此消息及其基本原理，但应将消息包视为事实来源。 输出
控制器有两个输出：
车辆控制命令消息
车辆控制命令消息具有以下形式：
builtin_interfaces/时间戳 float32 long_accel_mps2 float32 front_wheel_angle_rad float32 后轮角度弧度
此消息类型旨在为油门踏板和制动踏板上的脚以及方向盘上的双手提供代理，同时还为控制器开发人员提供方便的表示。
这些车轮角度是逆时针正的，其中 0 对应于中性或前向方向。
默认值 ：所有字段的默认值为 0
基本原理 ：提供时间戳字段是因为应生成控制命令以响应车辆运动状态的输入。 保留触发数据的时间应该有助于诊断和可追溯性。
基本原理 ：提供单个加速度场是因为控制器通常会为刚体规划纵向加速度和转向角。 为了简化控制器开发，制动被包含在这个单一命令中，并且因为通常不希望同时踩下制动器和加速器。
基本原理 ：提供车轮转向角，因为它允许控制器只需要车辆平台的简化视图，例如知道轴距长度。 根据线控接口的实现，产生给定的车轮角度可能需要了解方向盘角度与车轮角度之间的映射关系。
基本原理 ：还提供后轮角度以支持后驱动车辆，例如叉车。
理由 : 没有提供框架，因为它暗示它是固定在车架上的。
有关消息的更多详细信息，请参阅包中的正式消息定义 autoware_auto_vehicle_msgs 。 虽然为方便起见，此处重复此消息及其基本原理，但应将消息包视为事实来源。
控制器诊断信息
控制器诊断消息旨在报告控制器算法的运行统计数据。 该消息由以下字段组成：
诊断标头 布尔新轨迹 字符串轨迹源 字符串姿势源 float32lateral_error_m float32 纵向错误_m float32 velocity_error_mps float32 加速错误_mps2 float32 yaw_error_rad float32 yaw_rate_error_rps
Where DiagnosticHeader 有以下字段：
字符串名称 builtin_interfaces/时间 data_stamp builtin_interfaces/时间计算_start builtin_interfaces/Duration 运行时 uint32 迭代
基本原理 ：诊断信息有助于检测是否发生故障或导致故障的某些不正确行为。 它也可用于信息娱乐应用。
有关消息的更多详细信息，请参阅包中的正式消息定义 autoware_auto_system_msgs 。 虽然为方便起见，此处重复此消息及其基本原理，但应将消息包视为事实来源。 行为
兼容的控制器实现至少需要以下行为：
- 生成控制命令
- 生成诊断消息
- 在固定坐标系中操作
- 在局部坐标系中操作
生成控制命令
给定一个新的车辆运动状态，控制器应该输出一个控制命令。
基本原理 ：这是控制器的基本功能
生成诊断消息
给定一个新的车辆运动状态，控制器应该输出一个诊断信息。
基本原理 ：这是控制器的基本功能，可用于记录和诊断。
在固定坐标系中操作
控制器应该能够接收在固定框架（例如 map ）中的轨迹并正确操作。
这意味着应该可以使用后续的车辆运动学状态消息来确定车辆的当前状态（即通过变换绝对位置或累积相对变换），以便底层算法可以生成结果。
基本原理 ：根据自动驾驶堆栈的设置，相对于固定坐标系的绝对位置可能是唯一可用的位置信息来源。 根据运动规划算法，生成的轨迹也可以植根于绝对帧。
在局部坐标系中操作
控制器应该能够在特定时间戳处接收位于局部坐标系中的轨迹并正确操作。
这意味着控制器应该能够接收车辆运动学状态，并且可以将其相对于轨迹框架（植根于特定时间戳）的位姿进行变换，或者通过累积相对变换来计算其位姿，然后适当地进行计算。
基本原理 ：运动规划器可以在局部坐标系中输出轨迹。 此外，强大的本地化保证不一定总是可用的（例如，在农村环境中的高速公路行驶，或使用 3 级堆栈）。 在这种情况下，仍然可以生成局部轨迹，并且应该仍然可以遵循该轨迹。
参考
相关消息类型
活性氧：
汽车件：
Apollo ：
配置消息
算法状态消息
控制器测试
该软件包包含用于运行动态车辆模型同时运行的控制器测试的启动文件，参数和脚本，并生成图
参数
| mpc_controller_nodes/param/defaults.yaml |
| controller_testing/param/defaults.param.yaml |
用法
| ROS2启动Controller_testing Controller_testing_node_mpc.launch.py real_time_sim：= true with_rviz：= true |
| ROS2启动Controller_testing Controller_testing_node_pure_pursuit.launch.py real_time_sim：= true with_rviz：= true |
- 对于比实时使用更快的速度，请将参数更改为 real_time_sim:=False 和 with_rviz=False 。
MPC 控制器设计
目的
模型预测控制 (MPC) 涉及按需解决优化问题，并使用生成的解决方案的初始部分来计划或控制接下来几个时间步长的系统。 这种技术有利于确定控制输入，因为它了解建模系统的动态，因此可以选择不仅对当前状态最佳的控制动作，而且还可以设置未来状态以获得成功。
该软件包旨在为单轨车辆平台实现 MPC 控制器。
设计 车辆动力学模型
使用了基本的运动学自行车模型。 行为
实现了后退水平模型预测控制。
当接收到一个新的轨迹时，下一个命令是通过一个冷启动优化问题来计算的。 这意味着初始猜测是没有控制输入的参考轨迹。
随着车辆沿参考轨迹前进，先前的解决方案会向前滚动（热启动），并且参考点从参考轨迹回填。
随着参考轨迹接近终点，参考权重被适当地归零以实现后退水平行为。 API
该控制器使用 ControllerBase 接口。 因此，如果当前状态在某种意义上“超过”当前轨迹，将提供停止减速命令。
在所有其他情况下，库将解决优化问题并使用计划的第一个元素作为控制命令。 实施
static_asserts 用作粗略的气味测试，以检测生成的代码是否已从外观更改。 它们通常放置在可能受到影响的代码周围。
未来的改进
动态车辆模型
应该使用动态车辆模型来更准确地表示车辆在更高速度下的运动。
该模型改编自[1]。
进行了以下调整：
- 纵向速度不假定为常数
- 相对于（固定）内部坐标系的航向角包含在状态向量中
- 加速度作为状态变量包含在内，并假定直接作用于纵向加速度
- 加速度和车轮角度用作控制输入
进行这些调整的原因如下：
- 支持加速和减速轨迹
- 支持任意轨迹方向
- 提高最终控制计划的平滑度
Pure Pursuit(纯追踪算法)
目的/用例
Autoware.Auto 需要基本的运动控制器。 运动控制器必须遵循参考轨迹并正确保持车道。
控制器将参考轨迹和当前车辆状态（姿态和速度）作为输入。 主要输出是车辆接口的控制命令。
纯追踪是轨迹跟踪的基本算法，广泛应用于自主机器人应用中。 纯追踪算法从轨迹中找到合适的目标点并计算曲率半径。 纵向加速度是使用当前车辆和目标点的速度计算的。
设计
纯追踪算法的过程如下所述。
- 计算当前车辆状态和目标状态之间的误差。 首先，使用最近邻搜索从轨迹中计算目标候选。 然后对第一个和第二个最近邻点进行插值，并根据当前车辆位置（线点距离）计算插值点。
- 检查更新的轨迹，如果其大小为 0，则控制器尝试根据给定的停止距离进行紧急停止。
- 如果延迟补偿的布尔值为真，则使用当前姿势状态计算姿势时间戳和计算时间戳之间的位置差
- 根据当前车辆速度和从速度到距离的转换比计算前瞻距离。
- 根据当前位置和前瞻距离从轨迹计算目标点。
- 如果轨迹更新，则搜索目标点的起始索引为 0
- 如果轨迹没有更新，则搜索目标点的起始索引是从最后一个目标索引开始
- 从起始索引到满足以下条件的最终索引搜索目标点。 满足这两个条件的第一个索引被选为目标索引。 如果没有满足第二个条件的点，则选择满足第一个条件的最远索引（最终索引）作为目标点。
- 候选目标点在车辆行驶方向
- 当前车辆与候选目标点之间的距离大于计算的前瞻距离
- 如果插值模式打开，则使用目标点及其前一个点（索引）计算插值点。 从当前车辆位置到插值目标点的距离等于前瞻距离。
- 计算当前位置和目标点之间的曲率（半径）。 从目标点提取目标速度
- 从目标点提取目标速度。 如果没有目标点，则执行紧急停止（加速/减速）。 如果汽车停下来（低于 0.001mps），什么也不做。
更新了基础文件中的内容
- 使用 CATR 模型添加延迟补偿
- 前瞻距离由当前车辆速度估计
- 添加了行进方向策略来确定目标点。 这使控制器能够采用固定轨迹的顺序姿势输入。
- 添加了目标点插值以跟随前瞻距离
假设/已知限制
坐标信息如下。 θ 为逆时针角度，取值范围为 [-pi, pi]。
x
^
\ θ |
\ |
\ |
<---+--- y
|
存在以下限制：
- 这个实现假设速度的变化是平滑完成的。 如果速度的符号突然以 (30.0, 20.0, -10.0) 之类的大值改变（例如由于不稳定的定位）并且轨迹没有更新，则此实现可能不会从轨迹中提取目标点，因为行进方向策略。
- 如果pose和controller节点的时间戳相差大于21秒，就会因为int32的溢出导致延迟补偿被破坏。
输入/输出/API
输入：
- Trajectory.msg 是具有 2D 位置和姿态的轨迹点序列
- TrajectoryPointStamped.msg 是当前车辆状态（姿态或速度）
输出：
- VehicleControlCommand.msg 是车辆接口的控制命令
- ControllerDiagnostic.msg 是控制器的诊断结果
配置状态
下面定义了配置状态：
- minimum_lookahead_distance (float32_t)：纯粹追击的最小前瞻距离（米）。 这可以防止车辆受到强烈振动
- maximum_lookahead_distance (float32_t)：纯追击的最大前瞻距离（米）。 这可以防止车辆受到强烈振动
- speed_to_lookahead_ratio (float32_t)：从速度到前瞻距离的转换比率。 该比率等于持续时间 (s)。
- is_interpolate_lookahead_point (bool8_t): 是否使用插值来确定目标位置
- is_delay_compensation (bool8_t)：是否使用延迟补偿在当前时间戳估计当前车辆位置
- emergency_stop_distance (float32_t)：紧急停止的距离。 该值在控制器没有参考轨迹时使用，或者当轨迹未更新且行进方向（180 度）上没有前点时使用。
- speed_thres_traveling_direction (float32_t)：确定行进方向的速度阈值。 如果车辆速度的绝对值大于阈值，则基于速度的符号确定行驶方向。 如果小于阈值，则使用前瞻距离确定目标点。
性能表征
时间
从轨迹（长度 n ）中搜索目标点，即 O(n) ，是纯追踪算法的计算复杂度。
空间
纯追踪算法的空间复杂度以轨迹为主，轨迹 O(n) 在空间中。
安全注意事项
由安全专家待定。
参考/外部链接
未来的扩展/未实现的部分
相关问题
Pure pursuit nodes（纯追踪算法节点）
目的/用例
这个包是一个连接计算（pure_pursuit）和通信（DDS/ROS 2）的薄样板包。
这是必要的，以便在与通信隔离的情况下轻松测试计算。 同样，这个包分离了这个包的集成/通信测试。
设计
这是对 pure_pursuit 的薄包装。
使用相应的 tf 主题对当前位姿进行变换，然后计算控制命令。 假设/已知限制
该节点假设全局 tf 图在轨迹帧和姿势帧之间具有转换。
有关 apex_tf 转换 API 限制的更多详细信息，请参阅。
有关更多详细信息，请参阅 PurePursuit 设计文档。 输入/输出/API
输入：
- tf 主题：源帧是姿势，目标帧是轨迹（通过 apex_tf ）
- 轨迹主题（在轨迹框架中）
- 车辆姿势主题（在姿势框架中）
- pure_pursuit 参数
输出：
- 车辆运动控制器主题
- pure_pursuit 的诊断主题
最重要的是，可以通过编程方式或通过构造参数文件配置节点。 内部工作/算法
有关 pure_pursuit 更多详细信息，请参阅。
错误检测和处理
大多数错误处理发生在 rclcpp 内部、 PurePursuit 自身内部或关联的配置类中。
安全注意事项
Node 这些组件从、 BufferCore 和 核心 PurePursuit 类 继承安全注意事项。
未来的扩展/未实现的部分
- tf 姿势变换的内部库
- 订阅 tf 主题的方式应该是确定性的。
相关问题
- #2642：初始实施
- #2838: apex_app -> apex_auto
轨迹跟踪
这是 trajectory_follower 包装的设计文件。
目的/用例
这个包提供了 trajectory_follower_nodes 包的节点使用的库代码。 主要实现了两种算法：
- 用于计算横向转向命令的模型预测控制 (MPC)。
- 用于计算速度和加速度命令的 PID 控制。
相关问题
轨迹跟随节点
目的
生成控制命令以遵循给定的轨迹。
设计
此功能被分解为三个节点。
核心功能在 Trajectory Follower 包中实现。
Trajectory Follower 包概述
调试
调试信息由横向和纵向控制器使用 autoware_auto_system_msgs/Float32MultiArrayDiagnostic 消息发布。
文件夹中提供了 PlotJuggler 的配置文件 config ，加载时允许自动订阅和可视化对调试有用的信息。
此外，预测的 MPC 轨迹在主题上发布， output/lateral/predicted_trajectory 并且可以在 Rviz 中可视化。
|
