前言
各位同学好！新一届智能车竞赛的"飞跃雷区"组别，是一道典型的"空地协同"题。环境感知必须放在悬停飞机上，地面执行平台不能直接感知场地，两者通过线缆连接。这意味着，想要完成比赛，核心不只是"让飞机飞起来"或"让车跑起来"，而是要把飞行稳定、信标车辆识别、Y车协同执行三部分真正串成一个系统。
写这篇手册，是希望帮助新同学快速了解这个赛项，并分享我们今年摸索出来的一套方案。我们深知，没有绝对的最优解，但在手册里，我们会毫无保留地分享我们的方案经验与调车心得。希望这些内容能为大家提供一条清晰的探索思路，让大家在备赛过程中少走弯路，更加从容地应对比赛挑战。
如果大家想要探讨智能车相关话题，欢迎加入我们的 QQ 讨论群。在这里，无论是智能车的硬件设计、软件编程，还是比赛策略、技术难题，都能展开交流。
1. 组别简介
1.1 赛题要求
根据飞跃雷区组当前公开说明，和我们做这套系统时最相关的约束主要有下面几条：
- 环境感知传感器必须安装在悬停飞机上，地面执行平台不能直接依靠自身传感器去搜索信标。
- 飞机与车模之间通过不超过 1.5 米的线缆连接，这根线既可以供电，也可以传输数据，但不能使用无线通信。
- 飞机的主要任务不是像穿越机一样大范围机动，而是在空中稳定悬停，为感知系统提供持续、清晰、稳定的俯视视野。
- 地面平台需要根据飞机端给出的信息，快速接近目标并完成压灯动作。
- 在最新的规则中，增加了可以使用英飞凌硅麦传感器检测飞机位置的说明，这意味着大家也可以围绕"飞机位置感知"继续尝试别的完赛路线，例如用车辆跟随飞机。
所以从参赛队伍的任务分解来看，至少要完成下面三件事：
- 让飞机稳定悬停，做到定高、定点飞行可用。
- 让飞机上的摄像头能够稳定识别信标和 Y 车。
- 让 Y 车根据飞机给出的信息完成接近和压灯。
这样拆开理解以后，这个组别的任务链路会清晰很多。难点不在某一项技术本身，而在三条链路能不能最后接成一个可靠闭环。
相关规则说明可参考（推荐经常浏览，或许每次都有新发现）：
- 竞速比赛规则：第二十一届全国大学生智能汽车竞赛比赛规则
- 比赛场地与车模说明：第21届智能车竞赛飞跃雷区组比赛车模与比赛场地说明
- 组别问答说明：第21届全国大学生智能汽车竞赛提问与回答：飞跃雷区组别
1.2 赛项解析
建议不要一上来就想着"整车系统"，而是先按任务流拆成三个小闭环：
- 飞机先飞稳
如果飞机悬停不稳，摄像头看到的图像就会一直晃。图像一晃，阈值分割会不稳定，中心点会跳，后面的车辆跟随和地面执行也都会跟着乱。所以这道题里，飞控不是辅助模块，而是整个系统的地基。
- 飞机能看清
飞机稳定以后，第二件事就是识别。这里至少要同时解决两个问题：
- 识别信标在哪里。
- 识别 Y 车在哪里，车头朝哪边。
如果只知道信标位置，不知道 Y 车位置和朝向，就没法告诉车该往前走还是往侧面移；如果只知道 Y 车，不知道信标，也没法给执行层目标。所以这一步一定是"双目标识别"。
- Y 车要能执行到位
当地面 Y 车拿到目标误差以后，剩下的事情就不再是"视觉题"，而是"控制题"。它需要把误差转换成全向底盘的运动指令，再通过电机闭环把动作真正做出来。只要方向正确、速度合理、闭环稳定，压灯就是水到渠成的结果。
- 参赛队具体要做什么
站在参赛队的角度，这个组别通常至少需要有人分别负责下面几块：
- 飞机端上位机：摄像头、图像处理、串口通信
- 飞机端飞控：姿态、定高、定点、外环接入
- Y 车执行层：电机闭环、全向运动解算、任务执行
- 硬件整合：主板、电源、线束、安装结构
如果按这个思路分工，整个项目就会清楚很多。每个人可以先把自己负责的小闭环调顺，最后再做整机联调。
1.3 本方案的总体思路
我们采用三控制器协同架构，各司其职：
- 飞机端上位机 (CYT系列)：负责"看"和"算"。处理摄像头图像，识别信标和Y车，然后分别把控制信息发给飞控和Y车。
- 飞机端飞控 (TC系列)：负责"稳"。保持飞机姿态稳定，实现定高、定点悬停，为上位机提供稳定的拍摄平台。
- Y车下位机 (CYT系列)：负责"动"。接收上位机发来的运动指令，驱动全向轮，让车快速、准确地到达目标位置。
这种分工方式不仅让系统职责更加清晰，也让调试过程更符合"先分模块跑通，再做整机联调"的备赛逻辑。同时，对于团队来说可以根据能力有一个很好的分工合作，比如会视觉算法的负责上位机识别，适合做控制的调整飞控参数，基础相对薄弱的做Y车闭环。
2. 硬件清单
想复刻这套方案，建议准备以下硬件。这是我们在调试中验证过的组合，可以把更多精力放在代码和调参上。
2.1 飞行平台选型
机架 ：我们提供的是整机5mm厚碳板机架，平衡重量与耐炸性，同时大面积的中板也便于安装主板与扩展模块，其他比赛也可以用上。
动力 ：STC32mini电调配合51466三叶大桨，在提供大推力的同时平衡功耗，效率高。
核心：
飞机端上位机 ：英飞凌 TRAVEO T2G系列（CYT）系列
开始调试的时候可以使用我们的CYTmini母板
主板大家自制的时候只需要保留自己需要用到的，尤其是屏幕接口，摄像头接口，和两路串口 。
飞机端飞控 ：
在飞控端卓晴老师在规则中放宽了要求 可使用的微控制器（MCU）型号包括Infineon， STC， NXP单片机
对比下来推荐Infineon AURIX 系列，俗称TC系列，TC297的库到目前为止已经相对完善了。
开始调试的时候可以使用我们的TC通用母板
主板大家自制的时候只需要保留自己需要用到的，如四路无刷接口，屏幕接口，陀螺仪接口，光流（SPI），TOF（IIC）
2.2 地面执行平台选型
车模 ：Y车模（全向轮），可以实现全向运动，侧向修正效率高，接近目标时动作更直接。
今年也修改了很多Y车相关的"BUG"，使其更适合今年的飞跃雷区。例如用上了行星齿轮减速电机，配套的一体式磁编码器。
核心 ：Y车下位机：同为英飞凌 TRAVEO T2G系列 （CYT）核心板。
开始调试的时候可以使用我们的CYT通用母板
主板大家自制的时候只需要保留自己需要用到的，如屏幕接口，三路电机驱动接口，3路编码器接口
2.3 核心硬件模块
MT9V034 神眼摄像头 (160°带滤光片) ：空中感知的"眼睛"，全局快门，我们用它获取原始灰度图。再进行算法处理去识别信标灯和V型灯板。
PWM3901 + VL53X 光流TOF一体板 ：
低功耗光学追踪传感器+高精度TOF传感器、可有效测量2m以内光流变化
为飞控提供高度和平面速度反馈，是定高定点飞行的关键。
ICM42688 陀螺仪 ：
作为公认的高精度 低噪音 模块，常用于飞控中。
为飞控提供姿态和角速度数据。
显示模块 (IPS200) ：高分辨率彩屏，用于实时查看图像和处理结果，调试常备。
3. 开发工具
3.1 硬件设计软件
需要自制主板时，使用立创EDA软件。我们提供的资料中包含PDF版原理图和封装库，可以直接参考使用。
需要注意：自制的电路板需在正面覆铜面展示队伍信息，包括学校名称、队伍名称、参赛年份。
规则原话：
嘉立创制作流程规范：第二十一届智能车竞赛电路板嘉立创制作流程规范
3.2 搭建开发环境
对很多新同学来说，真正开始做不是从算法开始，而是从"工程能不能编译、板子能不能点灯、传感器有没有数据"开始，所以接下来先手把手带着大家过一遍。
3.2.1 TC 系列开发环境
第一步：安装开发环境
下载、安装 ADS，和下载器驱动DAS。
ADS编译环境资料：网盘链接：https://pan.baidu.com/s/1Wg7uZ-PFW63A-0p1yOg5jQ 提取码：eawu
第二步：下载仓库
下载龙邱科技 TC297 软件库工程，并解压到任意目录（推荐统一放在一个代码文件夹中）
- Gitee 仓库：TC297_SoftWare_LIB
第三步：导入到工程目录
选择导入外部工程到工作区
找到TC297 软件库工程解压出来的目录并导入
右键导入的工程，点击激活工程
这一步建议不要急着改代码，先把原始工程完整编译一遍，看看有无报错，再打开点灯例程。
打开点灯例程后，再点击小锤子🔨编译工程，再进行下一步。
第四步：先跑最简单的例程
将DAPLink下载器与核心板和电脑连接（推荐核心板插在母板上，母板接上电池单独供电），点击下载图标
按照上面的流程走：
- 程序能成功下载。
- 复位后 LED 按预期闪烁。
- 说明烧录、时钟、最基础的运行环境已经没问题。
到这一步，TC 工程的基础环境算真正搭好了。
第五步：读传感器原始值
点灯通过后，下一步建议测试读取陀螺仪原始值。
（记得把原来的点灯程序Ctrl+/注释掉，将陀螺仪和屏幕插到母板陀螺仪SPI接口和屏幕SPI接口上）
这里不要急着上姿态解算，先看原始数据是不是能稳定变化。因为姿态解算只是建立在原始传感器数据可靠的前提上。
得到的结果：
- 串口或屏幕上能看到陀螺仪原始值。
- 手动转动板子时，三个轴数据会有对应变化。
- 变化方向基本符合安装方向。
3.2.2 CYT 系列开发环境
CYT 这边的思路和 TC 一样，也建议按"先点灯，再出图"的顺序走。
第一步：安装开发环境
下载安装 IAR9.40 for ARM编译环境
大家可以自行到网上搜索保姆级的IAR下载安装注册教程，这里就不多赘述了。
安装注意事项：
如果安装在系统盘以外，最好新建一个不含有中文的文件夹进行安装
安装时会弹出对应驱动的安装，建议不要拉下了。
第二步：下载仓库
下载龙邱科技 CYT4BB7 软件库工程：（目前库文件还在不断更新中，更新会在群里通知大家，新库使用更方便）
- Gitee 仓库：CYT4BB7_Soft_LIB
这里同样是下载并解压到任意目录（推荐统一放在一个代码文件夹中）
第三步：先跑 LED 例程
解压好后根目录附有使用手册，其中有更详细的工程介绍使用。（随着工程更新，打开目录会更简短，可提高效率。）
IAR编译器直接双击打开对应工程路径下的文件即可打开工程。
第一件事仍然是点灯，目的不是为了"会点灯"，而是为了确认板子、下载器、工程配置和最基本的运行环境都没有问题。
打开工程后，首先找到M0核的主函数，左键点击选择Test_LED例程那一行，Ctrl+shift+k取消Test_LED的例程注释。
（注释默认快捷键是Ctrl+K，大家可以使用VS code打开工程目录，编些代码会方便一些，在VS code编写完了记得ctrl+S保存一下）
使用IAR编译器最好养成习惯 ，每次下载前把三个核的程序都编译一遍 ：
右键M0核工程，点击Make，M7_0核,M7_1核的工程，用同样的方法，都编译一遍再进行下载。
三个核的工程都编译完成后，将ARMLINK下载器与电脑与核心板相连，点击 下载并进行调试 图标
下载后会自动进入debug模式，如果大家不需要进行调试，点击 停止调试 图标后，系统会自动复位重新开始跑程序。
经过以上步骤，核心板上的灯能按程序设定频率闪烁。
到这一步，CYT 工程的基础环境算真正搭好了。
第四步：跑摄像头例程
LED 通过后，先将M0核的LED示例程序重新注释，找到M7_1核的主函数，找到Test_MT9V034例程，Ctrl+shift+k取消注释。
如果使用IPS200的屏幕，找到LQ_IPSLCD.h将宏定义修改为IPS200即可，这里还能修改屏幕方向，默认是横屏（设置为1或2为竖屏）
重新编译三个核的工程，
将IPS200屏幕和摄像头接线连接到母板上。下载，再点击停止调试。如果装了红外滤光片，将摄像头对着阳光，或者对着我们的红外灯板，可以看到如下画面。
到这里都没问题的话，后面再开始做阈值分割、连通域和目标识别就水到渠成了。
4. 系统总体方案
4.1 整体系统架构与数据流
从数据流的角度看，这套系统其实只有两条核心链路：
- 视觉链路：摄像头采图，上位机识别信标和 Y 车，输出位置、方向和误差。
- 控制链路：飞控根据信标偏差修正飞机位置，Y 车根据车体误差执行接近和压灯。
把它展开来看，整个过程是这样的：
- 飞机端上位机从摄像头获取灰度图像。
- 通过亮度区分，分别提取信标灯和 Y 车 V 型灯板。
- 对信标灯求中心位置，对 Y 车求中心位置和朝向。
- 将信标相对图像中心的偏差
(dx, dy)发给飞控。 - 将信标相对 Y 车的前向误差和侧向误差
(err_fwd, err_lat)发给 Y 车。 - 飞控根据
(dx, dy)做位置修正，让信标尽量保持在图像中心。 - Y 车根据
(err_fwd, err_lat)做全向运动，逐步接近并压灭信标灯。
这样分的好处是，三部分边界很清楚：
- 上位机负责把图像变成控制量
- 飞控负责把控制量变成稳定悬停
- Y 车负责把误差变成地面执行动作
只要每一段都先单独跑通，最后联调时问题就会好定位很多。
4.2 通信协议
这套系统里，通信没有做得很复杂，而是尽量采用最直接的串口帧格式。原因很简单：这个组别真正难的是识别、控制和联调，通信层能简单就尽量简单，后面排错也更方便。
我们一共用了两条串口链路。
1. 上位机发给飞控
这条链路只负责一件事：把信标在图像中的偏差发给飞控。
飞控拿到偏差后，再结合当前高度，把像素偏差换成实际平面误差，送入位置环。
数据格式如下：
- 帧头：
0xA0 dx：信标 X 方向偏差（2 字节）dy：信标 Y 方向偏差（2 字节）valid：识别有效位（1 字节）- 校验：异或校验（1 字节）
- 帧尾：
0x55
2. 上位机发给 Y 车
这条链路负责把 Y 车需要执行的误差直接发过去。
也就是说，上位机已经帮 Y 车算好了"往前多少、往侧面多少"，Y 车只需要做运动解算和电机闭环。
数据格式如下：
- 帧头：
0xA1 err_fwd：车体前向误差（2 字节）err_lat：车体侧向误差（2 字节）flags：状态标志位（1 字节）- 校验：异或校验（1 字节）
- 帧尾：
0x55
为什么建议大家一开始也采用这种思路？
因为这样一来：
- 飞控不用关心车辆怎么走
- Y 车也不用关心飞控怎么稳
- 上位机只负责"看"和"算"
- 每条链路职责单一，调试时很容易抓包确认
5. 空中感知方案（核心！）
5.1 核心思路：亮度区分
这套方案的核心，不是先去想复杂识别，而是先把目标拆开。
在摄像头看到的原始灰度图里，场地里的两类目标亮度并不一样：
- 信标灯：亮度中等，带一圈红外灯，整体更像一个亮环
- Y 车上的 V 型灯板：亮度更高，几乎是整张图里最亮的一类目标
所以最直接的办法就是：
- 先用一组"中亮度阈值"把信标灯提出来
- 再用一组"高亮阈值"把 Y 车灯板单独提出来
这样做的好处很明显：
- 信标和车辆先被拆开处理，后面就不容易互相干扰
- 算法整体比较轻，适合单片机实时跑
- 调试的时候可以直接盯着屏幕看，每一步改了什么一眼就知道
如果用一句话概括，就是：
先把"目标"和"车"分开，再分别去求位置和方向。
5.2 图像设置
空中感知的第一步不是写算法，而是先把图像调顺。
这里我们使用的是 MT9V034 神眼摄像头（带红外滤光片） 。
建议一开始就把曝光改成手动，不要用自动曝光。原因很简单：自动曝光会让阈值跟着漂，今天能识别，明天灯光稍微一变就不稳定。
建议起始值：
- 曝光：
80
先把原始图像显示到屏幕上，只做一件事：观察信标灯和 Y 车灯板的亮度差异是否足够明显。
做到下面这几点，就说明图像输入这一关过了：
- 屏幕能稳定显示灰度图
- 对着红外灯时，亮区明显高于背景
- 调整曝光后，亮区大小和亮度会有明显变化
- 画面不会时亮时暗、频繁漂移
这一步做不好，后面的阈值分割基本都会跟着乱。
5.3 信标识别
信标识别建议按四步来理解：
- 阈值分割
- 连通域提取
- 几何约束筛选
- 输出中心坐标并发送给飞控
5.3.1 先用阈值把信标提出来
信标在图里是"中亮度目标"，所以先给它设一个亮度区间。
代码可参考这样定义：
c
#define TH_LOW 80 // 信标灯下阈值
uint8_t TL_HIGH = 225; // 信标灯上阈值
uint8_t Car_LOW = 242; // 车辆高亮阈值
#define AREA_MIN 8 // 信标连通域面积最小值
#define AREA_MAX 80 // 信标连通域面积最大值然后直接扫整张图，把落在阈值区间里的像素保留下来：
c
for(int y = 0; y < IMG_H; y++)
{
for(int x = 0; x < IMG_W; x++)
{
uint8_t v = img_copy[y][x];
if(v > TH_LOW && v < TL_HIGH)
{
binImg[y][x] = 1;
debugImg[y][x] = 255;
}
else
{
binImg[y][x] = 0;
debugImg[y][x] = 0;
}
if(v > Car_LOW)
{
CarImg[y][x] = 1;
CardebugImg[y][x] = 255;
}
else
{
CarImg[y][x] = 0;
CardebugImg[y][x] = 0;
}
}
}上面这一段代码其实同时做了两件事：
- 用 TH_LOW ~ TL_HIGH 提取信标
- 用 Car_LOW 提取 Y 车灯板
也就是说，一次扫图，直接得到两张二值图：
- binImg：信标二值图
- CarImg：车辆二值图
这一步的目的就是先把原始灰度图变成"可判断的目标区域"。
5.3.2 再做连通域，把亮点变成目标块
阈值分割以后，屏幕上看到的是一堆白点或者白块。
单片机并不知道哪一块是真信标，所以接下来要做连通域。
连通域的作用就是：把"连在一起的白色像素"归成一个目标。
你可以把它理解成这样：
- 一圈连在一起的白像素，算一个目标
- 另一个不相连的亮块，算另一个目标
- 后面就可以对每个目标分别做面积、形状、位置判断
这一层做完以后，图像就不再是"像素集合"，而是"目标集合"了。
5.3.3 最后加几何约束，筛掉假目标
做完连通域以后，不是所有亮块都是真信标。
比如 Y 车灯板的光晕、反光、小噪点，都可能形成连通域。所以还要加一层几何约束：
面积不能太小，也不能太大
长宽比不能太夸张
填充率要落在合理范围内
参考：
c
for(uint8_t i = 1; i < labels; i++)
{
uint16_t a = area_table[i];
if(a < AREA_MIN || a > AREA_MAX) continue;
uint16_t w = maxx[i] - minx[i] + 1;
uint16_t h = maxy[i] - miny[i] + 1;
float ratio = (float)w / h;
float fill = (float)a / (w * h);
if(ratio > 0.4f && ratio < 1.7f && fill < 0.95f && fill > 0.5f)
{
beacon_list[beacon_count].cx = (minx[i] + maxx[i]) / 2;
beacon_list[beacon_count].cy = (miny[i] + maxy[i]) / 2;
beacon_list[beacon_count].area = a;
beacon_list[beacon_count].valid = 1;
beacon_count++;
if(beacon_count >= MAX_BLOB) break;
}
}这里三组条件分别在做什么：
- （a < AREA_MIN || a > AREA_MAX）把太小的噪声点、太大的干扰块筛掉
- （ratio > 0.4f && ratio < 1.7f ）信标看起来大致接近圆环，不会细得像一条线，也不会扁得像一块板子
- （fill < 0.95f && fill > 0.5f ）信标不是完全实心，也不是特别稀疏，所以填充率会落在一个中间区间
为什么这一步很关键？
因为如果不加这些限制，Y 车上的 V 型灯板光晕和其他点状干扰也可能会被错当成信标。
5.3.4 输出信标中心并发给飞控
通过筛选以后，信标中心点就有了。
接下来只需要把它换成相对图像中心的偏差（如果你需要做综合调度，比如信标在飞机一定范围内不需要移动，或者不需要完全靠近，在这里做算法判断后改变发送的dx、dy值即可）：
c
int16_t dx = beacon_list[0].cx - IMG_W / 2;
int16_t dy = beacon_list[0].cy - IMG_H / 2;再通过串口发给飞控：
c
void Beacon_Send(int16_t dx, int16_t dy, uint8_t valid)
{
uint8_t frame[FRAME_SIZE];
frame[0] = HEAD;
frame[1] = (dx >> 8) & 0xFF;
frame[2] = dx & 0xFF;
frame[3] = (dy >> 8) & 0xFF;
frame[4] = dy & 0xFF;
frame[5] = valid;
frame[6] = CalcChecksum(frame);
frame[7] = TAIL;
for(int i = 0; i < FRAME_SIZE; i++)
UART_PutChar(SCB7, frame[i]);
}这一步做完以后，飞控拿到的就已经不是图像了，而是：
- dx
- dy
- valid
也就是"目标偏哪边"和"这一帧识别有没有成功"。
5.3.5 屏幕调试怎么做
调试时最直接的办法，就是把识别到的中心点画到屏幕上。
c
LCD_Draw_Rectangle(x - 10, y - 10, x + 10, y + 10, RED_IPS);这样你一眼就能看到：
- 有没有框到信标
- 框的位置是否稳定
- 多个信标是否都能识别出来
做到这一步时，信标识别基本就已经跑通了。
5.4 Y 车识别
Y 车识别的目标和信标不一样。
这里不仅要知道"车在哪"，还要知道"车头朝哪"。所以流程会比信标多一步"方向估计"。
整条链路可以拆成下面五步：
- 高亮阈值提取
- 连通域保留主体
- 计算质心
- PCA 求主方向
- 结合 V 型结构，判断车头朝向
5.4.1 先提取高亮灯板
Y 车上的 V 型灯板亮度比信标更高，所以直接用更高的阈值把它提出来（之前提取信标时同时提取即可，这里只做演示）：
c
if(v > Car_LOW)
{
CarImg[y][x] = 1;
CardebugImg[y][x] = 255;
}
else
{
CarImg[y][x] = 0;
CardebugImg[y][x] = 0;
}这一步做完以后，CarImg 里保留的就是最亮的那部分目标。
如果阈值选得合适，最终看到的会是一块比较干净的 V 型主体，而不是一整片发白的灯光区域。
5.4.2 在利用PCA算法前先排除干扰
如果直接把 CarImg 丢给 PCA，然后发现方向乱跳。
原因是：图里除了 V 型主体，往往还有零散亮点，比如信标灯旁边的红外、噪点、边缘反光。如果这些点也一起参与计算，质心和方向都会被带偏。
所以在 PCA 之前，先要做一次"主体保留"。
我们用的是这个思路：
- 先找出最大连通域
- 再把距离它很近的小连通域并进来
- 其他都删掉
核心代码如下：
c
uint8_t labels = Car_Labeling();
uint16_t max_area = 0;
uint8_t max_label = 0;
uint8_t keep_label[MAX_BLOB] = {0};
for(uint8_t i = 1; i < labels; i++)
{
if(CarArea[i] > max_area)
{
max_area = CarArea[i];
max_label = i;
}
}
if(max_label == 0)
{
memset(CardebugImg, 0, sizeof(CardebugImg));
car_result.valid = 0;
return;
}
keep_label[max_label] = 1u;接着再把离主体足够近的小块并进来：
c
for(int changed = 1; changed; )
{
changed = 0;
for(uint8_t i = 1; i < labels; i++)
{
if(keep_label[i] || CarArea[i] < CAR_BLOB_MIN_AREA) continue;
for(uint8_t j = 1; j < labels; j++)
{
if(!keep_label[j]) continue;
if(CarBlobNear(i, j, CAR_BLOB_MERGE_GAP))
{
keep_label[i] = 1u;
changed = 1;
break;
}
}
}
}这样做完以后，PCA 看到的基本就是完整的 V 型灯板，而不是一堆无关亮点。
5.4.3 先求中心：质心怎么来的
PCA 之前，先求出灯板的中心，也就是质心。
做法很简单：把所有白色像素点的坐标加起来，再除以点数。
c
int32_t sum_x = 0, sum_y = 0;
Count_n = 0;
for(int y = 0; y < IMG_H; y++)
for(int x = 0; x < IMG_W; x++)
if(CarImg[y][x]) { sum_x += x; sum_y += y; Count_n++; }
if(Count_n < CAR_MIN_PIX || Count_n > CAR_MAX_PIX)
{
car_result.valid = 0;
return;
}
float mx = (float)sum_x / Count_n;
float my = (float)sum_y / Count_n;这里的 (mx, my) 就是 V 型灯板的中心点。
为什么先求中心？
因为后面的"主方向"不是直接看图像左上角到右下角怎么摆，而是要看所有点围绕中心是怎么展开的。
5.4.4 PCA 是怎么求方向的
PCA 在这里的作用可以直接理解为：求出灯板的主方向 。
它本质上是在回答一个问题：
这堆亮点围绕中心，主要朝哪个方向拉得最开？
先计算 2×2 协方差矩阵：
c
float cxx = 0.0f, cyy = 0.0f, cxy = 0.0f;
for(int y = 0; y < IMG_H; y++)
for(int x = 0; x < IMG_W; x++)
{
if(CarImg[y][x])
{
float dx = (float)x - mx;
float dy = (float)y - my;
cxx += dx * dx;
cyy += dy * dy;
cxy += dx * dy;
}
}
cxx /= Count_n;
cyy /= Count_n;
cxy /= Count_n;再求出主特征向量，也就是主方向：
c
float e1x, e1y;
if(fabsf(cxy) < 0.5f)
{
e1x = (cxx >= cyy) ? 1.0f : 0.0f;
e1y = (cxx >= cyy) ? 0.0f : 1.0f;
}
else
{
float d = (cxx - cyy) * 0.5f;
float r = sqrtf(d * d + cxy * cxy);
float nx = cxy;
float ny = r - d;
float norm = sqrtf(nx * nx + ny * ny);
e1x = nx / norm;
e1y = ny / norm;
}
float e2x = -e1y;
float e2y = e1x;这段代码算出来两条互相垂直的方向：
- e1：横向测量方向
- e2：主方向 / 分半方向
可以用下面这个图来理解：
开口端（展开更大）
←────── w_pos ──────→
● ●
\ /
\ /
\ /
\ /
\ /
\ /
\ /
───── C ───── e1（横向测量方向）
│
│
│ e2（PCA主方向）
▼
/ \
/ \
/ \
● ●
←── w_neg ──→
尖端（展开更小）图中各个量的含义如下：
- C：V 型灯板的中心点
- e2：PCA 求出来的主方向，用来把图形分成两半
- e1：和 e2 垂直，用来量"这一半张开多宽"
- w_pos / w_neg：主方向两侧各自的横向展开程度
5.4.5 比较两侧宽度得到车辆朝向
PCA 只能告诉我们"这块灯板整体朝哪条轴摆着"，但还不能直接区分车头和车尾。
因为一个方向向量本身有两个朝向，可能向前，也可能反过来向后。
这时就要利用 V 型结构本身的几何特征：
- V 型张开的那一端更宽，是开口端
- V 型收拢的那一端更窄，是尖端
代码里的做法是：
- 先沿 e2 把图形分成两半
- 再分别统计两半在 e1 方向上的宽度
- 哪边更宽，哪边就是开口端
- 对侧就是尖端，也就是车头方向
对应代码如下：
c
float p1_pos_max = -1e9f, p1_pos_min = 1e9f;
float p1_neg_max = -1e9f, p1_neg_min = 1e9f;
int cnt_pos = 0, cnt_neg = 0;
for(int y = 0; y < IMG_H; y++)
for(int x = 0; x < IMG_W; x++)
{
if(CarImg[y][x])
{
float dx = (float)x - mx;
float dy = (float)y - my;
float p1 = dx * e1x + dy * e1y;
float p2 = dx * e2x + dy * e2y;
if(p2 >= 0.0f)
{
if(p1 > p1_pos_max) p1_pos_max = p1;
if(p1 < p1_pos_min) p1_pos_min = p1;
cnt_pos++;
}
else
{
if(p1 > p1_neg_max) p1_neg_max = p1;
if(p1 < p1_neg_min) p1_neg_min = p1;
cnt_neg++;
}
}
}
float w_pos = (cnt_pos > 1) ? (p1_pos_max - p1_pos_min) : 0.0f;
float w_neg = (cnt_neg > 1) ? (p1_neg_max - p1_neg_min) : 0.0f;然后根据宽窄关系，定出真正的车头方向：
c
float hx = (w_pos >= w_neg) ? -e2x : e2x;
float hy = (w_pos >= w_neg) ? -e2y : e2y;如果两边宽度差不多，说明这一帧图像有退化、像素太少或者形状不明显，这时就用上一帧方向来消除跳变：
c
if(fabsf(w_pos - w_neg) < M3_MIN_THRES && car_result.valid)
{
if(hx * car_result.hx + hy * car_result.hy < 0.0f)
{
hx = -hx;
hy = -hy;
}
}最后把结果存起来：
c
car_result.cx = (int16_t)(mx + 0.5f);
car_result.cy = (int16_t)(my + 0.5f);
car_result.hx = hx;
car_result.hy = hy;
car_result.valid = 1;到这里，车辆识别输出的就不只是中心点了，还包括一个朝向单位向量 (hx, hy)。
5.4.6 车体误差怎么生成
识别出信标位置和车辆位置后，下一步就是告诉 Y 车"你该怎么走"。
先对车辆中心做一次修正。因为 V 型灯板一般不在车辆几何中心，所以需要沿朝向反向偏一点：
c
float real_cx = (float)car_result.cx - car_result.hx * CAR_CENTER_OFFSET;
float real_cy = (float)car_result.cy - car_result.hy * CAR_CENTER_OFFSET;然后计算"信标 - 车辆"的图像坐标误差：
c
float edx = (float)beacon_list[0].cx - real_cx;
float edy = (float)beacon_list[0].cy - real_cy;最后把这个误差投影到车体坐标系，得到：
- err_fwd：车前后方向误差
- err_lat：车左右方向误差
c
err_fwd = (int16_t)(edx * car_result.hx + edy * car_result.hy);
err_lat = (int16_t)(edx * car_result.hy - edy * car_result.hx);如果你的摄像头安装方向和我们的不一致，这里正负号可能要跟着改，但原理不变：
就是把图像中的误差，投影到车自己的前后左右坐标系里。
5.4.7 把误差发给 Y 车
最后把 err_fwd 和 err_lat 打包发给 Y 车：
c
void Car_Ctrl_Send(int16_t err_fwd, int16_t err_lat,
uint8_t car_valid, uint8_t beacon_valid)
{
uint8_t frame[CAR_CTRL_FRAME];
frame[0] = CAR_CTRL_HEAD;
frame[1] = (uint8_t)((err_fwd >> 8) & 0xFF);
frame[2] = (uint8_t)( err_fwd & 0xFF);
frame[3] = (uint8_t)((err_lat >> 8) & 0xFF);
frame[4] = (uint8_t)( err_lat & 0xFF);
frame[5] = (uint8_t)((car_valid ? 0x01u : 0x00u) |
(beacon_valid ? 0x02u : 0x00u));
frame[6] = CalcChecksum(frame);
frame[7] = CAR_CTRL_TAIL;
for(int i = 0; i < CAR_CTRL_FRAME; i++)
UART_PutChar(SCB9, frame[i]);
}到这一步，空中感知这部分就真正完成了它的任务：
- 给飞控发 dx/dy
- 给 Y 车发 err_fwd/err_lat
5.5 双核分工
CYT的双核，我们把它用得很"粗暴"，但非常有效：
M7_1 (干活核)：负责摄像头采集、图像处理（信标+Y车识别）、串口通信（发送数据给飞控和Y车）。
在M7_1核中的IPC发送显示函数中，如果M7_0核没有释放信号量，可以不用发送这一针的图片，提高处理速度：
M7_0 (显示核)：负责LCD显示，把M7_1处理的结果（原图、二值图、标注）画到屏幕上。
如果 M7_0 这一帧还没处理完，M7_1 可以直接跳过这一帧显示发送，不影响识别主流程。
这样做很实用，因为真正比赛时，识别速度比屏幕好不好看更重要。
5.6 空中感知调试建议
按这个顺序检查，哪里出问题一目了然：
首先看原图：首先曝光需要手动调整到合适的，要能压住亮起的信标灯"高光"，同时不能太暗，太暗需要更低的阈值筛选，容易引入不必要的干扰。
（我这边的图片没有那么明显，实际上左边的信标在屏幕上看会比旁边V型灯板要暗一倍。）
看信标二值图：调好亮度下限和亮度上限，亮度上限一般比Y车高亮提取阈值低，确保亮起的红外信标是一个完整的圆环。
看Y车二值图：Y车灯板是否被干净地提取出来？提取车辆的高亮阈值一般是偏高的，尽量只保留能连续的V型最简图形即可。
看识别结果：屏幕上标注的中心点（信标的中心、车辆的中心）和车辆的方向需要相对稳定。如不稳定首先检查信标的形态学限制，或修正Y车上V型信标灯识别算法。
最后看串口数据：上位机发出的 dx/dy 和 err_fwd/err_lat 数值是否合理？
6. 飞控方案
6.1 飞控任务说明
飞控在这套方案里的任务很明确，不是做大范围机动，而是把飞机稳定地停在空中，给上位机提供一个可用的观察平台。具体要完成下面四件事：
- 姿态稳定，让飞机能飞得住。
- 定高控制，让飞机能稳定停在合适高度。
- 定点控制，让飞机尽量不要在平面上乱漂。
- 接收视觉偏差，把信标位置误差接到位置外环里。
如果这四件事拆开理解，飞控并不复杂。真正难的是调试顺序不能乱。
6.2 控制架构
飞控整体采用多层闭环结构：
- 姿态环：角度环 + 角速度环
- 定高环：高度环 + 垂直速度环
- 定点环：位置环 + 速度环
- 视觉外环 ：把
(dx, dy)接入位置环
可以先把这套结构理解成三层：
- 最里面一层是姿态稳定，负责把飞机本体稳住。
- 中间一层是定高和定点，负责让飞机停在想要的位置。
- 最外面一层是视觉目标偏差，负责让飞机围绕信标做修正。
6.3 飞控输入来自哪里
飞控侧真正用到的输入量主要有三类：
6.3.1 姿态输入
来自陀螺仪和姿态解算，得到：
- Pitch
- Roll
- Yaw
- 三轴角速度
基于姿态的控制请严格参考：四轴飞行器飞控编写教程
之后的各项数据输入处理，例如级联PID（双环控制），是基于这个飞控中的内容进行。
6.3.2 高度输入
来自 TOF 测距模块。
TOF 读出来的是测距值，但飞机飞行时会有俯仰和横滚，所以要先做一次姿态补偿，才能得到更接近真实垂直方向的高度：
c
true_height = tof_distance * cos(pitch) * cos(roll)这一步做完以后，飞控用来定高的就不是原始斜距，而是真实垂直高度，并且后续平面光流的速度获取，需要高度作为参数纠正。
6.3.3 平面速度输入
平面速度这一层，目的很明确：
把 PMW3901 读到的光流增量，结合 VL53X 测到的高度，整理成飞控能直接使用的平面速度反馈，最后送到定点飞行的速度环内环。
这一部分不需要重新编写底层驱动。库中已经提供了 PMW3901 的初始化、寄存器配置和运动数据读取函数，也提供了 VL53X 的 IIC 读写函数。使用时直接调用即可。
1. 先定义需要用到的量
平面速度换算时，至少要先明确下面几个量：
c
#define FLOW_DT_SEC 0.02f // 固定读取周期，建议 50Hz
#define FLOW_QUALITY_MIN 20 // 光流最小质量阈值
#define FLOW_HEIGHT_MIN_MM 80 // 最低有效高度
#define FLOW_HEIGHT_MAX_MM 2000 // 最高有效高度
#define FLOW_MM_PER_COUNT_AT_1M 2.13f // 1m高度下，每1count约对应的位移(mm)，起始标定值
#define DEG2RAD(x) ((x) * 3.14159265f / 180.0f)其中：
- FLOW_DT_SEC：表示固定读取光流的周期
- FLOW_QUALITY_MIN：图像质量太低时，这一帧不参与速度更新
- FLOW_HEIGHT_MIN_MM / FLOW_HEIGHT_MAX_MM：高度超范围时，不使用这一帧光流
- FLOW_MM_PER_COUNT_AT_1M：把 PMW3901 的 count
换成实际距离时用到的比例尺，前期先给一个起始值，后面再按实测修正
2. 初始化流程直接沿用测试例程
初始化顺序可以直接沿用 Test_PMW3901() 的思路：
c
void Flow_TOF_Init(void)
{
SPI_PMW3901_Init();
if(PMW3901_Init_Chip() != 0)
{
while(1); // 光流初始化失败，停住调试
}
PMW3901_Set_Sensitivity(1); // 起步可先用 1，环境差时再调到 2
IIC_Init();
VL53_Write_Byte(VL53ADDR, VL53L0X_REG_SYSRANGE_START, 0x02); // VL53连续测距
s_tof_data.flow_x_prev = 0.0f;
s_tof_data.flow_y_prev = 0.0f;
s_tof_data.filter_alpha = 0.2f;
}做到这一步时，说明两件事已经成立：
- PMW3901 已经能稳定输出 delta_x / delta_y
- VL53X 已经能稳定输出距离值
3. 先取原始值
在定时器中，先读取 PMW3901 的运动数据和 VL53X 的距离值：
c
PMW3901_Data_t motion_data;
uint8_t no_motion;
uint8_t dis_buff[2];
uint16_t tof_distance;
no_motion = PMW3901_Read_Motion(&motion_data);
VL53_Read_nByte(VL53ADDR, VL53_REG_DIS, 2, dis_buff);
tof_distance = ((uint16_t)dis_buff[0] << 8) | dis_buff[1];此时拿到的原始量包括：
- motion_data.delta_x
- motion_data.delta_y
- motion_data.quality
- tof_distance
这里要先说明一点：
- delta_x / delta_y 是两次读取之间累计的光流增量，它不是直接的速度值后面还需要结合高度和采样周期，才能换成真实平面速度
4. 先把测距值换成真实高度
飞行器处于俯仰、横滚状态时，TOF 测到的是斜距，不是真正的垂直高度。
所以要先利用姿态做一次补偿：
c
s_tof_data.tof_distance = tof_distance;
if (s_tof_data.tof_distance > 0)
{
float pitch_rad = DEG2RAD(angles.pitch);
float roll_rad = DEG2RAD(angles.roll);
s_tof_data.true_height = (uint16_t)(
s_tof_data.tof_distance * cosf(pitch_rad) * cosf(roll_rad) + 0.5f
);
}这样处理以后，后面做光流换算时用到的就是离地真实高度 true_height。
5. 区分"无运动"和"无效帧"
这里建议把两种情况分开处理：
- 无运动 ：说明这一帧检测到了稳定图像，但没有明显平移，这时平面速度应当回到
0 - 无效帧：例如图像质量太差，或者高度超出有效范围，这时不使用这一帧去更新速度
对应代码可参考：
c
if (s_tof_data.true_height < FLOW_HEIGHT_MIN_MM ||
s_tof_data.true_height > FLOW_HEIGHT_MAX_MM ||
motion_data.quality < FLOW_QUALITY_MIN)
{
return;
}
if (no_motion)
{
flow_x_vel = 0.0f;
flow_y_vel = 0.0f;
}这样处理更稳一些。
因为"没有运动"不等于"这帧数据无效"。如果在无运动时直接保留上一帧非零速度，速度环内环就可能一直看到旧速度，不利于悬停和定点收敛。
6. 再把光流增量换成平面速度
同样一个 delta_x / delta_y，在不同高度下对应的实际平面位移并不一样，所以换算时一定要把高度带进去。
先算当前高度下，每个 count 对应多少实际位移：
c
float mm_per_count = FLOW_MM_PER_COUNT_AT_1M *
((float)s_tof_data.true_height / 1000.0f);再把增量换成位移，并除以固定周期 dt，得到速度：
c
float disp_x_mm = -(float)motion_data.delta_x * mm_per_count;
float disp_y_mm = -(float)motion_data.delta_y * mm_per_count;
float flow_x_vel = disp_x_mm / FLOW_DT_SEC;
float flow_y_vel = disp_y_mm / FLOW_DT_SEC;这里前面的负号，是因为光流传感器看到的是"图像往哪边滑"，与飞行器真实平移方向通常相反。
如果后面接到飞控里发现方向不对，优先检查这里是否需要：
- 某一轴取反
- x / y 交换
方向关系先在这里理顺，不要先去改 PID。
7. 再做陀螺仪补偿
光流除了能看到平移，也会看到姿态变化带来的画面移动。
所以还要利用陀螺仪，把这部分"假速度"补回来。
先把陀螺仪原始值换成角速度：
c
static float gyro_to_rad_per_sec(short gyro_raw)
{
return (gyro_raw / GYRO_SENSITIVITY) * M_PI / 180.0f;
}然后计算姿态补偿量：
c
float roll_rate = gyro_to_rad_per_sec(ICM42688_data.gyrox);
float pitch_rate = gyro_to_rad_per_sec(ICM42688_data.gyroy);
s_tof_data.comp_x_from_pitch = pitch_rate * s_tof_data.true_height;
s_tof_data.comp_y_from_roll = roll_rate * s_tof_data.true_height;
float raw_x_vel = flow_x_vel + s_tof_data.comp_y_from_roll;
float raw_y_vel = flow_y_vel - s_tof_data.comp_x_from_pitch;这一层做完以后，raw_x_vel / raw_y_vel 才更接近平面真实速度。
8. 最后做滤波，作为速度环内环输入
平面速度本身会有抖动，所以最后再做一次一阶低通：
c
static float low_pass_filter(float input, float prev, float alpha)
{
return alpha * input + (1.0f - alpha) * prev;
}
s_tof_data.flow_x_filtered = low_pass_filter(raw_x_vel, s_tof_data.flow_x_prev,
s_tof_data.filter_alpha);
s_tof_data.flow_y_filtered = low_pass_filter(raw_y_vel, s_tof_data.flow_y_prev,
s_tof_data.filter_alpha);
s_tof_data.flow_x_prev = s_tof_data.flow_x_filtered;
s_tof_data.flow_y_prev = s_tof_data.flow_y_filtered;
s_tof_data.flow_x_mm = s_tof_data.flow_x_filtered;
s_tof_data.flow_y_mm = s_tof_data.flow_y_filtered;虽然变量名里写的是 mm，但经过上面的换算和除以 dt 以后，这里保存的已经是平面速度量，后面可以直接送给速度环内环使用。
9. 整理成一个完整更新函数
上面的步骤合起来，可以直接整理成下面这个固定周期更新函数：
c
void Flow_TOF_Update(void)
{
PMW3901_Data_t motion_data;
uint8_t no_motion;
uint8_t dis_buff[2];
uint16_t tof_distance;
float flow_x_vel = 0.0f;
float flow_y_vel = 0.0f;
// 1. 读取 PMW3901 光流增量
no_motion = PMW3901_Read_Motion(&motion_data);
// 2. 读取 VL53X 距离
VL53_Read_nByte(VL53ADDR, VL53_REG_DIS, 2, dis_buff);
tof_distance = ((uint16_t)dis_buff[0] << 8) | dis_buff[1];
s_tof_data.tof_distance = tof_distance;
// 3. 计算真实高度
if (s_tof_data.tof_distance <= 0)
{
return;
}
{
float pitch_rad = DEG2RAD(angles.pitch);
float roll_rad = DEG2RAD(angles.roll);
s_tof_data.true_height = (uint16_t)(
s_tof_data.tof_distance * cosf(pitch_rad) * cosf(roll_rad) + 0.5f
);
}
// 4. 质量或高度异常时，本帧不更新
if (motion_data.quality < FLOW_QUALITY_MIN ||
s_tof_data.true_height < FLOW_HEIGHT_MIN_MM ||
s_tof_data.true_height > FLOW_HEIGHT_MAX_MM)
{
return;
}
// 5. 无运动时，速度直接记为0；有运动时再做换算
if (!no_motion)
{
float mm_per_count = FLOW_MM_PER_COUNT_AT_1M *
((float)s_tof_data.true_height / 1000.0f);
float disp_x_mm = -(float)motion_data.delta_x * mm_per_count;
float disp_y_mm = -(float)motion_data.delta_y * mm_per_count;
flow_x_vel = disp_x_mm / FLOW_DT_SEC;
flow_y_vel = disp_y_mm / FLOW_DT_SEC;
}
// 6. 姿态补偿
float roll_rate = gyro_to_rad_per_sec(ICM42688_data.gyrox);
float pitch_rate = gyro_to_rad_per_sec(ICM42688_data.gyroy);
s_tof_data.comp_x_from_pitch = pitch_rate * s_tof_data.true_height;
s_tof_data.comp_y_from_roll = roll_rate * s_tof_data.true_height;
{
float raw_x_vel = flow_x_vel + s_tof_data.comp_y_from_roll;
float raw_y_vel = flow_y_vel - s_tof_data.comp_x_from_pitch;
// 7. 低通滤波
s_tof_data.flow_x_filtered = low_pass_filter(raw_x_vel, s_tof_data.flow_x_prev,
s_tof_data.filter_alpha);
s_tof_data.flow_y_filtered = low_pass_filter(raw_y_vel, s_tof_data.flow_y_prev,
s_tof_data.filter_alpha);
}
s_tof_data.flow_x_prev = s_tof_data.flow_x_filtered;
s_tof_data.flow_y_prev = s_tof_data.flow_y_filtered;
// 8. 作为速度环内环测量值
s_tof_data.flow_x_mm = s_tof_data.flow_x_filtered;
s_tof_data.flow_y_mm = s_tof_data.flow_y_filtered;
}这样整个链路就连起来了：
- PMW3901 提供光流增量
- VL53X 提供高度
- 两者一起换算出真实平面速度
- 平面速度作为定点飞行速度环内环的测量值使用
这一层跑通以后，后面的定点飞行就只剩下位置外环和姿态输出的事情了。
6.4 定高怎么做
定高这一层，要解决的问题很直接：
让飞机在起飞后稳定停在某个目标高度，而不是忽高忽低。
这一层真正用到的量有 3 个：
- tof_distance：TOF 原始测距值
- true_height：经过姿态补偿后的真实垂直高度
- fix_t_distance：目标高度
前面已经说过，true_height 是由 tof_distance 结合姿态补偿得到的：
c
true_height = tof_distance * cos(pitch) * cos(roll)所以定高控制真正拿来做反馈的，不是原始斜距，而是真实垂直高度。
1. 先确定目标高度
进入定高模式以后，通常先把当前高度锁成目标高度。
如果需要通过遥控器微调目标高度，也可以直接这样写：
c
if(abs(rc_info.LEFTH) > 8)
s_tof_data.fix_t_distance += rc_info.LEFTH / 40;
if(s_tof_data.fix_t_distance >= 1500)
s_tof_data.fix_t_distance = 1500;
if(s_tof_data.fix_t_distance <= 80)
s_tof_data.fix_t_distance = 80;
s_tof_data.fix_distance = s_tof_data.fix_t_distance;这几行代码分别在做三件事：
- 左手油门杆不再直接控制电机，而是改成微调目标高度
- 目标高度限制在合理范围内，避免过高或过低
- 把最终目标值同步到定高控制使用的变量里
这样做以后，定高模式下飞手的感觉就会变成"调高度目标"，而不是"直接推油门"。
2. 高度外环先算目标垂直速度
定高不能直接从"高度差"一步跳到"电机加多少油"，中间还要先过一层速度环。
所以这里先用高度环，把"离目标高度还差多少"变成"应该以多快的速度升降"。
c
pid_height.desire = fix_height;
pid_height.measure = flight_height;也就是：
- desire：目标高度
- measure：当前真实高度
高度环的输出，不是直接控电机，而是作为垂直速度环的目标值。
3. 垂直速度内环再算油门修正
垂直速度这一层，我们没有只用一种速度来源，而是做了一个融合：
- 一部分来自 TOF 高度微分
- 一部分来自 Z 轴加速度积分
c
float speed_z1 = s_tof_data.tof_speed;
float acc_z = EarthAccel_GetZAcceleration();
float speed_z2 = last_speed_z + acc_z * dt;
pid_z_speed.measure = 0.02 * speed_z1 + 0.98 * speed_z2;
last_speed_z = pid_z_speed.measure;这里这样处理的原因是：
- 只用 TOF 微分，速度容易抖
- 只用加速度积分，时间长了容易漂
- 两者做一个简单融合，得到的垂直速度会更稳一点
接下来再做级联控制：
c
Com_PID_Cascade(&pid_height, &pid_z_speed, dt);这一句的意思是：
- 高度环先算出目标垂直速度
- 垂直速度环再根据目标速度和当前速度，算出油门修正量
4. 最后把油门修正叠加到四个电机
定高环最后输出的是油门修正量。
再往后，就和姿态控制一起进入混控，把这部分修正叠加到四个电机基础油门上。
所以定高这一整条链路可以直接概括成：
- TOF 测距
- 姿态补偿得到真实高度
- 高度环根据高度误差算目标垂直速度
- 垂直速度环根据速度误差算油门修正
- 油门修正进入电机混控
如果定高飞起来不稳，排查顺序建议按下面来：
- 先看 TOF 原始距离是否稳定
- 再看姿态补偿后的 true_height 是否合理
- 再看目标高度是否在合理范围内
- 最后再调高度环和垂直速度环参数
6.5 定点怎么做
定点这一层，要解决的问题是：
让飞机在平面上尽量停住，或者朝目标点慢慢修正。
在这套方案里，目标点不是预先写死的坐标，而是由上位机实时识别出来的信标中心。
也就是说，只要让信标尽量回到图像中心，就等于让飞机尽量飞在信标上方。
这一层真正用到的输入量有两类：
- 上位机发来的信标偏差：dx / dy
- 光流和高度整理出来的平面速度：flow_x_mm / flow_y_mm
前者负责告诉飞控"目标偏了多少"，后者负责告诉飞控"自己现在平面上动得有多快"。
1. 先把图像偏差换成实际平面误差
位置环的目标值直接设为 0，意思就是希望信标始终处于图像中心：
c
pid_Point_Y.desire = 0;
pid_Point_X.desire = 0;上位机发来的 beacon_dx / beacon_dy 是像素偏差，不能直接拿来当实际位置误差，所以先结合当前真实高度做换算：
c
#define BEACON_FOCAL_PX 150.0f
float scale = (float)s_tof_data.true_height / BEACON_FOCAL_PX;
pid_Point_Y.measure = (float)beacon_dx * scale;
pid_Point_X.measure = (float)beacon_dy * scale;这里的 scale 可以理解成"像素偏差 -> 实际平面误差"的比例系数。
同样偏 20 个像素，飞机飞得高和飞得低，对应到地面上的实际距离完全不同，所以这里必须把高度乘进去。
如果这一帧没有识别到信标，就不继续追旧目标，而是把位置测量值回到 0 附近：
c
if(beacon_valid)
{
float scale = (float)s_tof_data.true_height / BEACON_FOCAL_PX;
pid_Point_Y.measure = (float)beacon_dx * scale;
pid_Point_X.measure = (float)beacon_dy * scale;
}
else
{
pid_Point_Y.measure = 0;
pid_Point_X.measure = 0;
}2. 位置环只负责"应该多快地靠过去"
位置环不直接输出姿态，而是先输出一个目标速度。
这一步的作用就是把"偏了多远"变成"应该以多快的速度去修正"。
所以位置环解决的是：
- 飞机应不应该往前修
- 应不应该往左修
- 修正动作应该快一点还是慢一点
但位置环本身不直接控电机，还要再套一层速度环。
3. 速度环内环直接用平面速度反馈
前一节已经把 PMW3901 + VL53X 的数据整理成了平面速度量：
- s_tof_data.flow_x_mm
- s_tof_data.flow_y_mm
这两个量就直接作为速度环内环的测量值使用：
c
pid_Speed_X.measure = s_tof_data.flow_x_mm;
pid_Speed_Y.measure = s_tof_data.flow_y_mm;如果希望速度环更稳一点，也可以在进入内环前再做一次简单平滑：
c
static float filt_x = 0.0f, filt_y = 0.0f;
filt_x += 0.2f * (s_tof_data.flow_x_mm - filt_x);
filt_y += 0.2f * (s_tof_data.flow_y_mm - filt_y);
pid_Speed_X.measure = filt_x;
pid_Speed_Y.measure = filt_y;这一层的意义很直接：
- 位置环说"应该跑多快"
- 速度环看"现在实际跑多快"
- 二者一比较，就知道还要不要继续给姿态修正
4. 用串级控制把位置误差变成姿态目标
位置环和速度环之间的关系，就是标准串级：
c
Com_PID_Cascade(&pid_Point_Y, &pid_Speed_Y, dt);
Com_PID_Cascade(&pid_Point_X, &pid_Speed_X, dt);这一步可以直接理解成：
- 外层位置环先算目标速度
- 内层速度环再算控制输出
最后再把速度环输出转成姿态目标：
c
pid_pitch.desire = LIMIT(pid_Speed_Y.output, -5, 5);
pid_roll.desire = LIMIT(pid_Speed_X.output, -5, 5);
pid_yaw.desire = 0;这里有三个重点：
- pitch / roll 不再由摇杆直接决定，而是由定点控制算出来
- 最终姿态目标被限制在 ±5°，防止修正过猛
- yaw 默认不主动参与定点，只做平面修正
5.把整条定点链路串起来看
定点控制整条链路其实就是下面这个过程：
- 上位机识别信标，发来 dx / dy
- 飞控根据当前真实高度，把像素偏差换成实际平面误差
- 位置环根据平面误差算出目标速度
- 光流整理出的平面速度作为速度环反馈
- 速度环根据"目标速度 - 当前速度"算出姿态修正量
- 姿态环和混控再把这些修正量分配到四个电机
所以定点飞行的关键不在某一个单独环节，而在这几层能不能顺着接起来：
- 高度值要准
- 光流速度方向要对
- 外环和内环的参数要匹配
- 姿态限幅不能太死也不能太松
6.定点不好用时先查什么
如果定点效果不理想，建议按下面顺序排查：
- 先看 dx / dy 方向对不对
- 再看像素换算比例是否合理
- 再看 flow_x_mm / flow_y_mm 方向对不对、稳不稳
- 再调位置环和速度环参数
- 最后再看 ±5° 的姿态限幅是否过紧
只要方向对、速度反馈稳、位置环和速度环参数匹配，定点这一块一般都能比较快收住。
6.6 电机输出怎么叠加
飞控算完姿态环、定高环和定点环以后，最后都要落到四个电机 PWM 上。
这一步本质上就是混控：
- Pitch 修正叠加到前后电机
- Roll 修正叠加到左右电机
- Yaw 修正叠加到对角电机
- 定高修正统一叠加到四个电机
6.7 飞控调参建议
飞控调试顺序非常关键，建议严格按下面的顺序走。
第一步：先调姿态环
手动模式下起飞，先把 Pitch 和 Roll 的角速度环、角度环调稳。
这一阶段不追求花哨动作，只看三件事：
- 打杆有响应
- 松杆能回正
- 不抖、不慢、不乱飘
第二步：再调定高环
打开定高模式，调整高度环和垂直速度环参数，让飞机能稳定停在一个高度。
建议从中低高度开始调，比如 200 mm 左右。
第三步：然后调定点环
打开光流，先让飞机学会"站住"。
这一步不要一开始就追求跟踪速度，先把悬停平面稳定性调出来。
第四步：最后接视觉外环
接入上位机发来的 dx/dy，观察飞机能否根据目标偏差自动修正位置。
如果方向对但反应慢，优先检查：
- 像素换算比例是否合理
- 位置环和速度环参数是否偏小
- 光流速度滤波是否太重
- 姿态限幅是否过紧
7. Y车执行方案
Y车是"腿"，让它跑得又快又准。它的任务很清晰：
- 接收指令：接收上位机发来的 err_fwd 和 err_lat。
- 运动解算：将误差指令转换为全向轮的速度指令 (vx, vy, vw)。
- 电机闭环：每个轮子用增量式PID做速度闭环，确保运动精准。
7.1 执行层任务说明
我们把底盘定义成一个倒 Y 型三全向轮结构：
- 第一个轮子在左前方
- 第二个轮子在右前方
- 第三个轮子在后方
这样布置以后，三个轮子的受力方向并不一样，所以同一个 vx、vy 指令投到三个轮子上，速度系数也不会一样。
运动解算+速度闭环可直接简化为：
c
void OmniMove(float vx, float vy, float omega)
{
float wheel_speeds[3];
wheel_speeds[0] = vx * cosf_30 - vy * 0.5 + omega;
wheel_speeds[1] = vx * -cosf_30 - vy * 0.5 + omega;
wheel_speeds[2] = vy + omega;
Target_speed1 = (int16_t)wheel_speeds[0];
Target_speed2 = (int16_t)wheel_speeds[1];
Target_speed3 = (int16_t)wheel_speeds[2];
motor1 = (int16_t)PidIncCtrl(&motor1PID, Target_speed1 - ECPULSE1);
motor2 = (int16_t)PidIncCtrl(&motor2PID, Target_speed2 - ECPULSE2);
motor3 = (int16_t)PidIncCtrl(&motor3PID, Target_speed3 - ECPULSE3);
MotorCtrl(motor1,motor3,motor2);
}7.1.1 车辆直行向前时为什么前两个轮子起主要作用
如果希望车辆直行向前，那么左前轮和右前轮同时向前转。
因为这两个轮子相对车体中轴线夹角相同，所以它们在横向上的分力一左一右，正好抵消；而在前向上的分力方向相同，可以合成为向前的推力。
这时后方轮子不需要额外提供横向补偿，所以主要由前两个轮子共同完成前进。
这就是代码里 vx 项会分别乘上 cos30° 的原因，本质上是在取前向分量。
7.1.2 车辆侧向移动时为什么前两个轮子只取 Vy 的一半
如果希望车辆纯侧移，那么前两个轮子会同时往同一个方向转动。
由于它们和车体中心线夹角相同，这时两个轮子的侧向分力方向相同，都会指向左侧或右侧；而后方轮子的受力方向本身就是沿着车辆侧向的，所以它对侧移的贡献最直接。
从力的合成上看：
- 前两个轮子的侧向分量会叠加
- 后方轮子直接提供一整份侧向分量
- 为了让车辆只侧移、不自转，三个轮子的速度比例就必须匹配
这也是代码里前两个轮子的 vy 前面是 0.5，而后轮是 1.0 的原因。
不是因为这个数"经验上好用"，而是因为底盘几何关系本身就决定了它应该这么分配。
7.1.3 为什么最后还要做电机闭环
运动解算只解决了"每个轮子应该转多快"，但实际电机不会自动按这个速度工作。
所以还要给每个轮子加速度闭环，一般用增量式 PID 就够了。
它的作用是：
- 让编码器反馈速度跟目标速度对齐
- 减少地面摩擦、供电波动带来的影响
- 让底盘动作更稳定
总结下来就是先做运动解算，把 err_fwd/err_lat 变成三个轮子的目标速度，再做电机闭环，把这三个目标速度真正跑出来。
7.2 调试建议
Y 车这部分建议按下面的顺序调，不要一上来就接上位机。
第一步：先调单轮
先让三个电机分别单独转起来，确认下面三件事：
- 编码器计数方向和电机转动方向一致
- 目标速度给正值时，轮子转向符合预期
- PID 输出方向正确，不会出现一给目标就越调越偏
如果一上来就狂转，通常不是 PID 太差，而是方向反了。
这时先不要继续调参数，直接把对应轮子的 kp/ki 符号调整过来。
第二步：再调速度闭环
把三个轮子的增量式 PID 分别调顺。
这里的判断标准很简单：
- 给一个固定速度，轮子能比较快地跟上
- 松掉目标后不会拖太久
- 不会出现来回抖动
第三步：再测全向运动
闭环稳定以后，再开始测底盘整体解算是否正确。
可以按下面的顺序测：
- 给
(vx, 0, 0)，看车是否直行向前 - 给
(0, vy, 0)，看车是否纯侧移 - 给
(0, 0, omega)，看车是否原地旋转 - 再给混合速度，看整体方向是否符合预期
第四步：最后再接误差输入
确认底盘解算和闭环都没问题以后，再把 err_fwd/err_lat 接进来。
到这一步，Y 车要做的事情就变得很简单：上位机给多少误差，它就把误差变成运动。
这样调的好处是，后面一旦联调出问题，你能很快判断到底是：
- 上位机误差算错了
- 还是 Y 车底盘执行错了
8. 系统联调
当三个子系统都单独调通后，就可以联调了。强烈推荐的顺序：
- 先接飞控：打开上位机识别和飞控的视觉跟随，观察飞机是否能稳定追踪信标。然后再打开Y车上的灯板，确保不会相互干扰。
- 再接Y车：打开上位机识别和Y车接收，可以在在锁住桨叶的情况手动举起飞行器，看Y车是否按预期移动。
- 最后全闭环：让飞机在空中悬停，Y车在地面。当飞机识别到信标和Y车后，Y车应当能自动向信标移动并压灯。
9. 结语
飞跃雷区组真正考验的，不是某一个单独模块做得多复杂，而是整套系统能不能形成稳定协同。
从工程角度看，这套方案的主线其实很清楚：
- 上位机负责把图像变成可用的控制量
- 飞控负责把飞机稳稳停在空中
- Y 车负责把误差变成实际执行动作
所以备赛时最重要的事情，不是急着整机联调，而是先把每一个小模块单独调顺。
只要视觉识别稳定、飞控悬停可靠、Y 车执行干净，最后把三部分串起来，整套系统自然会收敛很多。
当然，这篇手册分享的仍然只是一种实现思路，而不是唯一答案。
也欢迎大家在这个基础上继续尝试更适合自己团队的感知方式、控制结构和执行策略，找到属于自己的完赛路线。
祝大家备赛顺利，调试少走弯路，也期待在赛场上看到更多有意思的方案。