跳转至

控制器

控制器栈负责将高层目标转换为与后端无关的执行器级命令。具体仿真器或硬件集成层负责把这些命令施加到各自的 actuator 模型。

在包结构层面，具体控制器统一组织在公共接口 ControllerBase 之下。它定义了 PID、几何控制、以及自适应控制器变体共用的生命周期与目标管理模式。

flowchart LR
  Target[目标或策略输出] --> Mapping[映射]
  Mapping --> Controller[控制器]
  Controller --> Mixer[Mixer]
  Mixer --> Rotor[旋翼动力学]
  Rotor --> FlightAdapter[飞行执行器适配层]

  Mapping --> Differential[差速控制器]
  Differential --> GroundAdapter[平台驱动适配层]
  GroundAdapter --> GroundActuators[地面执行器]

控制流程

飞行控制路径按如下顺序执行：

  1. 从当前状态提供方读取位姿、速度、姿态和机体系角速度。
  2. 构造 FlightController 所期望的控制器状态字典。所有状态条目都带有前导环境维度，例如 pos(N, 3)、vel(N, 3)、att_euler(N, 3)、ang_vel(N, 3)。
  3. 将目标与状态转换为总推力和机体系力矩。
  4. 通过 Mixer 将该 wrench 分配为旋翼 RPM 命令。
  5. 将旋翼 RPM 命令输入 RotorDynamics。
  6. 将得到的推力与扭矩交给后端 actuator 适配层。

地面控制路径使用 DifferentialDriveController 生成左右驱动输出轴速度目标。平台适配层再将这两个目标映射到平台的实际传动结构与 actuator 模型。

PID 控制器

lav2.controllers.pid 中包含可复用的 PID 基础块与组合式 FlightController。差速地面控制器位于独立模块：

飞行控制器

飞行控制器采用级联 PID 结构：

  • 位置环生成速度目标
  • 速度环生成加速度目标
  • 加速度环生成横滚 / 俯仰命令与总推力
  • 姿态环生成角速度目标
  • 角速度环生成机体系力矩
flowchart LR
  TPos[位置目标]
  TVel[速度目标]
  TAcc[加速度目标]
  TAtt[姿态目标]
  TRate[角速度目标]
  TThr[总推力目标]

  subgraph Cascade[飞行控制级联]
    direction LR
    Pos[位置环] --> Vel[速度环] --> Acc[加速度环] --> Att[姿态环] --> Rate[角速度环] --> Mixer[Mixer] --> Rotor[旋翼动力学] --> Act[执行器适配层]
  end

  TPos --> Pos
  TVel --> Vel
  TAcc --> Acc
  TAtt --> Att
  TRate --> Rate
  TThr --> Mixer

control_mask 决定了哪些目标通道由外部直接提供，哪些通道则在级联内部生成。这正是同一个控制器能够同时支持位置式与低层控制模式的关键。

对于 cmd_ctatt 与 cmd_ctbr，推力相关目标会先通过 apply_thrust_curve(...)，再经由 Mixer 直接注入 mixer 输入，而不是由外层的位置环和速度环生成。因此，这些模式表现为“嵌套的姿态 / 角速度控制 + 油门直通”，而不是完整的外环位置控制。

差速控制器

差速控制器遵循与飞行控制器相同的批量状态、生命周期和 control_mask 约定。它读取 pos(N, 3)、att_euler(N, 3)、vel_b(N, 3) 与 ang_vel(N, 3)，支持三个级联入口：

  • cmd_pos：位置误差生成前向速度，目标方位误差生成偏航角速度。
  • cmd_ctatt：外部机体系前向速度加外部航向目标。
  • cmd_ctbr：外部机体系前向速度加外部偏航角速度目标。

这两个名称复用飞行控制器的姿态与机体系角速度入口。cmd_ctbr 对应 PX4 Rover Acro 中的偏航角速度控制层，cmd_ctatt 暴露与 Stabilized 对应的航向控制层。不过， 当前仿真的前向通道仍是输出轴速度目标，而 PX4 Acro/Stabilized 使用直接电机命令； cmd_ctatt 也直接接收航向目标，不包含 PX4“偏航角速度摇杆回中后锁定当前航向”的状态机。

LAV2 限值取自 PX4 手动控制链路：底盘最大速度 2.5 m/s、Acro 偏航角速度输入 上限 7.0 rad/s、航点接受半径 0.5 m。现有 ULog 全部是 manual 模式，因此 实机数据只验证了 cmd_ctbr、差速分配和下层传动响应，不能验证上层 cmd_ctatt 或 cmd_pos PID。在接受半径外，位置环生成前向速度；进入接受半径后前向速度归零， 但最终航向目标仍可触发原地转向。

唯一一套默认增益直接采用飞控日志中的参数快照：位置环 Kp/Ki/Kd = 5.0/0/0， 航向环 Kp/Ki/Kd = 5.0/0.15/0.001。它们是对齐默认值，并不表示 manual 日志已经 辨识了当时未激活的上层控制环。

差速分配使用

\[ v_L = v - \frac{B}{2}\dot\psi, \qquad v_R = v + \frac{B}{2}\dot\psi \]

并返回输出轴单位为 rad/s 的 [omega_left, omega_right]。LAV2 使用按日志偏航响应 拟合的有效分配宽度，它不等于履带中心的物理间距。某侧超过限值时，分配器会同时 平移左右目标，在牺牲前向速度前优先保留可实现的偏航速度差。该效果与 PX4 diff_wheel_ctrl 一致，但不会把实物电机反向或减速比约定带入仿真坐标系。

控制器本身不绑定平台。LAV2 将每侧目标映射到一个主动轮 actuator；ATMO 将每侧目标映射到一台电机输出，并由同步带约束同侧前后轮。几何、限值和增益属于机器人参数；电机、传动与 actuator 动态属于平台适配层。

其它控制器家族

包内还提供了建立在 ControllerBase 之上的其它控制器结构：

即使它们内部的控制律不同于 PID 级联，在包级组织方式上仍遵循同一模式。

Mixer 与控制分配

Mixer 仅用于旋翼飞行器路径。它通过求逆一个基于 VehicleParams 派生出的几何分配矩阵，把总推力与横滚 / 俯仰 / 偏航力矩分配到四个旋翼命令上。

Mixer 支持两种分配策略，通过 allocation_mode 构造函数参数选择：

  • pinv（默认）：执行直接的伪逆分配，然后进行裁剪。这是一条更简单的路径，适用于最大吞吐量的批量工作负载。
  • saturation：在分配偏航之前优先保留横滚与俯仰控制裕度。当单个旋翼命令超出物理限制时，mixer 会先围绕平均旋翼转速修剪横滚和俯仰，然后将剩余裕度用于偏航。

两种模式都从同一个求逆后的分配矩阵计算旋翼转速平方目标，裁剪到 [0, max_rpm^2] 范围内，并返回平方根后的旋翼转速大小。

Mixer 还承担了与 PX4 对齐的归一化推力转换功能。PX4 本质上工作在归一化推力与力矩风格的输入上，因此 mixer 在这一层完成归一化推力约定与牛顿制总推力之间的转换。

辅助模块

当处理范围不只涉及控制律本身时，还有两个辅助模块值得了解：

  • lav2.controllers.utils：提供控制器响应、执行器行为的日志、可视化与数据导出能力。
  • lav2.controllers.mapping：负责将策略输出对齐到对应控制环期望的输入形式，这对分层 RL 控制尤其重要，因为不同 RL agent 可能驱动级联中的不同层。

API 交叉引用