18 KiB
视觉跟踪控制律设计（FlightController 实现期规格）
1. 文档状态与范围
1.1 文档状态
- 本文档为实现期规格文档，由早期草稿《物理追踪实现路线 v1.0》重写而来： 原文档中与既有设计体系冲突的表述（摄像头型号、飞控直连、控制时机、 拦截设备绑定、雷达距离语义等）已全部修正，合理的控制思路 （水平角控制、垂直控制、距离控制）保留并纳入本规格。
- 当前相关代码均为骨架占位实现，与
docs/开发进度.md（第 6 步未开始）一致：src/control/flight_controller.cpp（IFlightController Stub）src/state_machine/mission_state_machine.cpp（IMissionStateMachine Stub）src/perception/target_estimator.cpp（ITargetEstimator Stub）
- 本文档中所有阈值、限幅值、PID 参数只列占位，不填具体数值， 一律标注"待定/配置化"（需求分析.md NFR-007）。
1.2 范围与状态机对应
本文档定义视觉跟踪阶段（位置→速度控制切换之后）的控制律：
| 状态机阶段 | 本文档覆盖内容 |
|---|---|
| S5 GPS_APPROACH | 仅提供上下文（位置引导阶段，控制律不在本文档范围） |
| S6 VISUAL_HANDOVER | 切换后首条限幅指令的产生要求（切换条件本身见 状态机设计.md S6） |
| S7 VISUAL_TRACKING | 本文档核心：水平航向、垂直/高度、距离三路控制律 |
| S8 OBSTACLE_HOLD | 控制律输出被障碍策略接管（限速/刹停），见第 9 章 |
1.3 文档定位与引用关系
本文档定位为视觉跟踪控制律设计（FlightController 实现期规格）。 与以下内容重叠的部分一律引用、不复述：
| 引用文档 | 引用章节 | 本文档不重复的内容 |
|---|---|---|
docs/状态机设计.md |
S6、S7、§4.2、§7 | 切换守卫条件、目标丢失转移、可替换策略接口 |
docs/感知与避障设计.md |
§3、§5、§6、§9 | 标定流程、稳定锁定、雷达关联条件、降级原则 |
docs/视觉追踪技术路线.md |
§1.2、§2 | TargetTracker 算法细节、Coast/判丢行为约定 |
docs/系统架构设计.md |
§6、§8 | 数据流、控制单出口、时间与控制周期 |
docs/数据接口文档.md |
§2.2、§3、§4.9~§4.12、§5 | 消息字段定义、接口签名、Topic 一览 |
docs/需求分析.md |
FR-033、FR-041、FR-051、NFR-007、NFR-008、§7 | 需求条文本身 |
include/common/types.h |
全文 | 消息类型与 Topic 常量的权威定义 |
target_tracker/target_tracker.md |
§2、§7 | 追踪库接口与未来接线指引 |
2. 输入输出与数据流
2.1 输入
控制律在状态机与控制线程中按固定周期消费以下快照（Topic 名称常量见
include/common/types.h 的 drone::common::topics 命名空间）：
| Topic 常量 | 消息类型 | 用途 |
|---|---|---|
kEstimatedTarget |
TargetState |
目标估计状态（位置/速度/置信度/最近观测时间），控制律主输入 |
kFlightState |
FlightStateSnapshot |
本机姿态、速度、局部位置，用于坐标转换与反馈 |
kLaserRange |
LaserRangeSample |
机头前向单点距离；仅在相机—雷达标定完成且空间关联成立时才可作为目标距离，否则仅为前向安全距离（需求分析.md FR-041） |
kGroundTarget |
GroundStationTarget |
地面站目标状态，用于丢失校验与降级回退（非视觉控制主输入） |
2.2 输出与控制链路
控制律不直接写飞控。链路严格遵循 系统架构设计.md §6 的控制单出口约束：
状态机与控制线程（控制律，本文档范围）
-> ControlIntent（控制意图，kControlIntent）
-> IFlightController（意图 -> 设定值转换与限幅）
-> Px4Setpoint（kPx4Setpoint）
-> IPx4Link（PX4 通信线程）
-> 飞控串口
- 只有 PX4 通信线程可以写飞控串口；控制律只提交意图 （系统架构设计.md §6、数据接口文档.md §4.9）。
- 视觉跟踪阶段主要使用：
ControlIntentType::kVelocityHeading（速度与航向意图）：target_x/target_y/target_z为速度分量（坐标系由header.frame_id给出），yaw_deg为目标航向，max_speed_mps携带速度限制；ControlIntentType::kBrakeHover（刹停/悬停意图）：目标丢失判丢、 障碍制动等场景；ControlIntentType::kManualOverride（禁止自动输出/人工接管意图）： 遥控器接管时停止任务控制输出。
Px4Setpoint使用SetpointType::kVelocity（速度设定值，分量写入x、y、z；垂直通道不再使用SetpointType::kAttitude，理由见 5.2 设计修订）；航向写入yaw_deg或yaw_rate_dps； 无效设定值（如刹停完成）也需发送，避免过期指令（valid标志语义见include/common/types.h）。
2.3 坐标系编码
所有空间数据消息显式使用 MessageHeader.frame_id 编码
（数据接口文档.md §2.2、include/common/types.h 文件头约定）：
0 = 未知
1 = WGS84 经纬度
2 = 机体（前右下）
3 = 局部NED
4 = 局部ENU
控制律内部计算统一在**机体坐标系（frame_id = 2）**下进行
（视线角→机体前/右/下分量），输出的速度意图若采用局部系
（frame_id = 3），必须随 FlightStateSnapshot 的姿态/航向完成旋转，
并在 header.frame_id 中显式标注；禁止混用未标注坐标系的数据。
3. 坐标与标定前提
3.1 转换链路
目标像素位置 (u, v)
->（相机内参 + 畸变校正）-> 归一化像平面方向
->（相机→机体固定外参）-> 机体视线方向
->（结合当前机体姿态）-> 水平偏航误差 / 垂直俯仰误差
->（雷达关联成立时叠加距离）-> 机体系相对位置 (Δ前, Δ右, Δ下)
视觉像素只有结合相机模型和对应时刻的机体姿态，才能转换为目标相对机体 的视线方向（感知与避障设计.md §3）。
3.2 标定前提与待定项
实现视觉控制前必须完成（感知与避障设计.md §3、需求分析.md §7）：
- 相机内参与畸变标定（待定：相机内参、畸变参数未标定）；
- 相机坐标系到机体坐标系的固定外参（待定：外参未标定）；
- 激光波束在相机图像中的投影位置或区域（待定：相机—雷达标定未完成）；
- 图像、检测、光流、雷达和 PX4 姿态的时间对齐；
- 目标和本机高度基准（待定，需求分析.md §7）。
摄像头为 SIYI A8mini，经网口以 RTSP 输出 H.265、1080p、25 FPS 视频
（需求分析.md §3、开发思路.md）；画面宽度 W、高度 H 与帧率以实际
解码配置为准。标定完成前，控制律只允许在 SITL/台架中以录制数据回放
验证数学链路，不得上实飞。
4. 水平航向控制
4.1 目标与原理
目标：让机头在水平方向对准目标。由目标像素水平偏移反求偏航修正量， 经限幅后作为航向指令。
设目标中心像素横坐标偏移为 x_offset（目标中心相对画面中心，右正），
图像宽度为 W，相机水平视场角为 θ_h（注意：θ_h 仅表示水平
视场角，与俯仰姿态角无关，不得复用同一符号）。
针孔模型下的精确表述：
β = atan( x_offset * 2 * tan(θ_h / 2) / W )
当目标靠近画面中心（|x_offset| 相对 W 较小）时，可用线性近似：
β ≈ x_offset * ( θ_h / W )
线性近似的适用条件：
- 偏移量处于画面中心附近（具体适用范围待定，由标定与实测确定）；
- 已完成畸变校正（畸变在画面边缘放大近似误差）；
- 大偏移场景应使用针孔模型表述，或直接采用归一化像平面方向。
机体姿态变化时，像素偏移同时包含本机转动与目标运动两部分，
控制律需结合 FlightStateSnapshot 的姿态与角速度做补偿
（实现期细化，补偿结构待定）。
4.2 输出映射
- 航向指令映射到
ControlIntentType::kVelocityHeading的yaw_deg， 或由Px4Setpoint的yaw_deg/yaw_rate_dps表达； - 采用位置式（β→目标航向）还是速率式（β→偏航角速度）由实现期 按 PX4 1.17.0 Offboard 行为确定（待定）；
- 偏航角速度与方向变化率必须限幅（需求分析.md FR-033）， 限幅值见第 8 章。
5. 垂直 / 高度控制
5.1 误差来源
垂直误差由两个来源合成：
-
像素垂直偏移：目标中心相对画面中心的纵坐标偏移
y_offset（下正）， 图像高度H，相机垂直视场角θ_v。与第 4 章同理，垂直视线角：α = atan( y_offset * 2 * tan(θ_v / 2) / H ) （针孔模型） α ≈ y_offset * ( θ_v / H ) （中心附近线性近似） -
距离信息：只有雷达与目标完成空间关联（见 3.2 与第 6.3 节）时， 才可由测距距离
d换算高度差：Δh = d * sin(α)（等价于早期草稿中的
Δh = d × cos(90° − α)；此处 α 为垂直视线角， 与水平视场角 θ_h、垂直视场角 θ_v 明确区分命名。）
雷达未关联时，垂直通道仍由角度误差驱动（统一速度支路，见 5.2）， 只是缺少高度差校核，不得假设已知高度差。
5.2 控制策略（设计修订：双支路已合并为统一速度支路）
现行设计：垂直控制采用统一速度支路——垂直视线角 α 直接映射为
垂直速度设定值，全程保持 ControlIntentType::kVelocityHeading 的
SetpointType::kVelocity z 分量输出，无误差分支切换：
vz = clamp( slew( k_vz * α ), -vz_limit, +vz_limit )
其中 k_vz 为角度-速度增益，slew 为垂直加速度限幅（第 8 章），
clamp 为垂直速度限幅。小角度误差自然映射为小垂直速度，大小误差
表现为连续的增益、饱和与限速，不产生模式跳变。
设计修订记录：原草稿按角度误差大小分两支——小误差微调姿态角
（SetpointType::kAttitude，姿态限幅要求见需求分析.md FR-033），
大误差用垂直速度调整高度，两支按角度误差切换阈值切换。参考实现
reference-c++/visual_track_control（tracking_control_law.cpp
垂直速度通道）已将两支合并为上述统一速度支路，Python 移植
（utils/visual_track_control）沿用同一实现。合并原因：输出切换为
姿态设定值时，同一周期的距离通道前向速度会随输出模式切换被丢弃，
导致水平逼近停滞；统一速度支路消除了该问题，且无需维护切换阈值。
垂直角度-速度增益 k_vz 与垂直速度限幅均待定/配置化（第 8 章）。
6. 距离控制
6.1 目标
距离控制使目标进入拦截射程。射程进入条件由拦截设备接口
（IInterceptionDevice）与配置给出，本文档不假设任何具体拦截方式；
具体捕网或其他拦截方式不得进入通用逻辑（需求分析.md FR-051、
状态机设计.md §7）。射程 d_exp 为配置项，数值待定。
6.2 控制律
设目标距离为 d（来源见 6.3），射程为 d_exp：
距离误差：Δd = d - d_exp
前向速度指令 = PID(Δd)
- 对
Δd做 PID 得到机体前向速度指令，采用速度指令实现 （ControlIntentType::kVelocityHeading的速度分量 →SetpointType::kVelocity）； d > d_exp时前向接近，d < d_exp时减速/后退，接近末端的速度 必须按限幅约束收敛；- PID 参数（Kp/Ki/Kd、积分限幅、微分滤波）全部待定/配置化。
6.3 距离来源与降级
- 雷达距离不得直接当作目标距离：
LaserRangeSample表达机头前向 单点距离，不得自动解释为任意视觉目标的距离 （include/common/types.h注释、通信与数据定义.md §7）。 仅当相机—雷达标定完成、视觉区域覆盖雷达投影点、时间足够接近且 测量质量有效时，才可关联为目标距离（需求分析.md FR-041、 感知与避障设计.md §3）； - 关联失败时，雷达距离仅作为前向安全距离，供障碍策略 （第 9 章）使用；距离通道此时按配置降级：保持当前速度、 限速接近或退出近距操作（具体降级动作待定）；
- 视觉估计的距离趋势（目标估计器输出）可作为辅助输入， 融合权重待定。
6.4 与追踪库的行为衔接
沿用 视觉追踪技术路线.md §1.2 的上游行为约定：
- 追踪库输出
is_predicted == true（Coast 预测中）时，距离与航向 PID 的积分项清空，防止积分饱和； - 追踪库输出
tracked == false（判丢）时，执行零速刹车 （ControlIntentType::kBrakeHover），并按 状态机设计.md §4.2 的目标丢失转移处理； - 停止视觉闭环时调用追踪库
Reset()清空状态。
7. 控制周期与时序
7.1 固定控制周期
- 控制线程按固定周期持续运行，不等待任意传感器消息后被动执行； 视觉跟踪控制律在每个控制周期内持续执行，而非"检测完成后才进行" （系统架构设计.md §8）；
- 每次控制计算使用同一时刻附近的 PX4、视觉、雷达和目标数据快照；
- 超龄数据只能触发降级或告警，不能继续生成正常控制指令
（超龄判定使用
MessageHeader.receive_time_ms+valid_for_ms）； - 控制周期频率与各输入超龄阈值待定/配置化（系统架构设计.md §8）。
7.2 与 target_tracker 的衔接
目标追踪库（target_tracker/，已实现，见 target_tracker/target_tracker.md）
承担像素级平滑、Coast 与主目标锁定：
- 推理侧每帧调用
TargetTracker::Update()，控制线程（约 20Hz 速度闭环） 读取TrackResult（LastResult()线程安全拷贝）； - 控制线程消费
TrackResult的平滑中心点计算第 4/5 章角度误差， 并执行 6.4 的 Coast/判丢约定； TrackResult属于追踪库自有类型，尚未进入include/common/types.h；target_tracker/target_tracker.md§7 提议新增TrackedTarget消息与kTrackedTarget主题， 该提议当前未实现，接入主工程时需先在数据接口文档与 types.h 中正式定义。
8. 限幅、阈值与配置化
所有阈值集中于此并进入配置文件，不得散落在代码中 （需求分析.md NFR-007）。视觉跟踪的速度、加速度和方向变化 必须限幅（需求分析.md FR-033）。
| 项目 | 说明 | 状态 |
|---|---|---|
射程 d_exp |
由拦截设备接口与配置给出 | 待定 |
| 距离通道 PID 参数 | Kp / Ki / Kd、积分限幅 | 待定 |
| 航向通道增益与限幅 | 偏航角速度上限、方向变化率上限 | 待定 |
| 垂直角度-速度增益 | 垂直视线角到垂直速度的映射增益（5.2 统一速度支路） | 待定 |
| 垂直速度限幅 | 上升/下降速度上限 | 待定 |
| 前向速度限幅 | 接近/后退速度上限，随距离收敛 | 待定 |
| 加速度限幅 | 各轴加速度上限 | 待定 |
| 控制周期频率 | 控制线程固定周期 | 待定 |
| 各输入超龄阈值 | 视觉/雷达/姿态/目标估计 | 待定 |
| 线性近似适用范围 | 画面中心偏移比例 | 待定 |
首条视觉速度设定值必须经过限幅，避免位置→速度切换产生突变 （状态机设计.md S6）。
9. 异常与降级
9.1 目标丢失
- 判丢后执行零速刹车并调用
ITargetLostStrategy：短时预测 （预测时长与不确定度有上限）、最新有效kGroundTarget校验、 整机转向搜索或退回GPS_APPROACH（状态机设计.md §4.2、 感知与避障设计.md §5）； - 卡尔曼预测不得无限期生成控制指令（需求分析.md FR-034）。
9.2 传感器降级
按 感知与避障设计.md §9 的降级原则，与本文档控制律相关者：
| 故障 | 视觉跟踪阶段动作 |
|---|---|
| 摄像头失效 | 退出视觉跟踪 |
| 光流失效 | 降低跟踪可信度和速度 |
| 雷达失效 | 降低速度或退出近距操作；距离通道仅保留角度级控制 |
| 地面站目标过期 | 可按策略短时视觉跟踪 |
| PX4 姿态过期 | 禁止空间转换和控制，立即退出正常控制 |
9.3 障碍物规避
前向危险时控制律输出被障碍策略接管：清除视觉速度意图，限速、刹停并 悬停（S8 OBSTACLE_HOLD，状态机设计.md S8、感知与避障设计.md §7）。 雷达未关联时的前向安全距离即该策略的主输入。危险解除后必须重新 验证入口条件才可恢复视觉控制（需求分析.md FR-044）。
10. 待定项与验证计划
10.1 待定项清单
| 待定项 | 依据 |
|---|---|
| 相机内参、畸变参数、相机—机体—雷达外参 | 需求分析.md §7 |
| 目标和本机高度基准 | 需求分析.md §7 |
| 第 8 章全部阈值、限幅与 PID 参数 | 需求分析.md §7、NFR-007 |
| 雷达—目标空间关联的具体判据参数 | 感知与避障设计.md §3 |
| 航向控制位置式/速率式选择 | PX4 1.17.0 Offboard 实测 |
| 机体姿态补偿结构 | 实现期细化 |
TrackedTarget / kTrackedTarget 消息定义 |
target_tracker.md §7 提议，未入库 |
10.2 验证路径
按 需求分析.md NFR-008 与 系统架构设计.md §11 逐级验证：
- 开发机单元测试：控制律纯函数（角度换算、PID、限幅）用 Google Test 覆盖；录制数据回放验证像素→视线角数学链路；
- PX4 SITL：在仿真中验证设定值流、限幅行为、丢失/降级分支；
- 串口台架：真实飞控 + 模拟数据，验证时序、超龄判定与 控制权仲裁；
- 受控实飞：按状态机设计验证 S6→S7 切换与跟踪效果。
必测故障项包含旧目标位置、旧视频帧、雷达异常、遥控器接管、 算力板控制输出中断（系统架构设计.md §11）。