1. 项目概述与核心挑战在无人机自主执行任务的众多场景中对地面移动或静止目标进行稳定、持续的跟踪是一项极具价值且充满挑战的核心能力。无论是用于边境巡逻、灾害救援现场监视还是农业植保中的定点喷洒精准的目标跟踪都是任务成功的关键。对于多旋翼无人机而言凭借其出色的悬停和全向机动能力实现这类跟踪相对直观。然而当我们把目光转向固定翼无人机时情况就变得复杂得多。固定翼无人机依靠机翼产生的升力飞行这带来了两个根本性的约束第一它必须维持高于失速速度的前飞速度无法像多旋翼一样在空中悬停第二其机动完全依赖于改变飞行姿态主要是偏航、滚转和俯仰飞行方向与机头指向强相关无法进行侧向平移。这些动力学特性使得传统的、基于位置反馈的控制策略在固定翼平台上实施起来困难重重。更棘手的是为了控制成本和系统复杂度许多应用场景倾向于使用固定在机身上的单目摄像头而非昂贵的云台相机。这意味着相机的光轴会随着无人机姿态的剧烈变化例如转弯时的滚转而大幅摆动导致目标在图像中的位置发生非预期的、快速的跳跃严重干扰了基于图像的控制器。因此本项目的核心目标就是设计一套专门针对“固定翼无人机固定摄像头”这一苛刻组合的视觉伺服目标跟踪系统。它不依赖GPS等外部绝对定位信息仅凭机载摄像头捕获的图像流就能引导无人机以恒定半径环绕地面目标飞行并将目标稳定在图像中心附近。这其中的核心矛盾在于如何从剧烈晃动的图像中剥离出因目标相对位置变化而产生的“有效运动”并滤除因自身姿态变化引起的“干扰运动”从而生成准确、稳定的飞行控制指令。2. 系统核心原理从图像像素到控制指令的映射要理解整个系统首先需要厘清从二维图像信息到三维空间控制指令的转换链条。这个过程的核心是图像雅可比矩阵它建立了图像特征点速度与相机进而到无人机运动速度之间的数学关系。2.1 坐标系定义与成像模型我们首先需要统一描述空间的“语言”即定义几个关键的坐标系世界坐标系一个固定于地面的参考系用于描述目标的绝对位置和无人机的全局轨迹。机体坐标系原点在无人机质心X轴指向机头Y轴指向右翼Z轴垂直向下遵循右手定则。这是飞行控制器“理解”姿态和角速度的坐标系。相机坐标系原点在相机光心Z轴沿光轴指向拍摄方向X轴和Y轴平行于成像平面。对于固定相机其与机体坐标系的相对关系是固定的通常相机安装在机头下方光轴与机体X轴有一个固定的俯仰夹角例如45度。图像坐标系与像素坐标系图像坐标系原点在图像中心光轴与成像平面的交点单位是物理长度如毫米。像素坐标系原点在图像左上角单位是像素。两者通过相机的内参焦距、像素尺寸、主点坐标进行转换。成像过程遵循小孔成像模型。空间中的一个点其相机坐标系下的坐标与其在图像坐标系下的坐标通过焦距f和深度z该点在相机坐标系下的Z坐标联系起来。这是所有视觉几何计算的基础。2.2 图像雅可比矩阵连接图像与运动的桥梁假设我们跟踪的目标在图像上体现为一个特征点例如目标的质心其像素坐标为(u, v)。当无人机相机运动时这个点在图像上的移动速度(du/dt, dv/dt)与相机的运动速度线速度[Vx, Vy, Vz]和角速度[ωx, ωy, ωz]之间存在线性关系这个关系就是图像雅可比矩阵J[du/dt; dv/dt] J * [Vx, Vy, Vz, ωx, ωy, ωz]^T矩阵J的具体形式包含了特征点坐标(u, v)、焦距f和目标的深度z。它的物理意义非常直观矩阵的每一列代表了相机沿某一个自由度例如沿X轴平移单位速度运动时会导致特征点在图像上产生多大的像素位移。核心难点我们的目标是解算控制指令即已知特征点的期望运动速度我们希望它向图像中心移动反求无人机的运动速度。这需要求解J的逆矩阵。但这里J是一个2x6的矩阵我们只有u, v两个观测值却要解算6个运动自由度方程是欠定的无法直接求逆。传统IBVS方案通过跟踪多个至少3个特征点来构造一个6x6或更大的方阵但这在固定翼单目标跟踪中并不总是可行且计算量大。2.3 “理想相机”模型姿态干扰补偿的关键创新这是本文方案的精髓所在用于解决固定相机因姿态变化带来的图像干扰。其核心思想是进行一种“视角归一化”。问题无人机在转弯时会发生滚转在爬升/下降时会有俯仰。这些姿态变化会导致相机光轴指向改变即使目标在地面的实际相对位置没变它在图像上的位置也会突然跳动。如果直接将这个跳动的像素坐标送给控制器控制器会误认为是目标在移动从而发出错误的修正指令导致系统振荡甚至失稳。解决方案“理想相机”模型。我们设想存在一个虚拟的相机它与真实相机固连在无人机上的位置相同但它的姿态始终与一个“理想状态”的无人机绑定即无人机的滚转角和俯仰角都为零只有偏航角与当前真实无人机相同。具体操作获取当前状态通过飞控读取无人机当前的滚转角φ和俯仰角θ。坐标变换首先假设目标点P在真实相机图像上的坐标为(x_P, y_P)。根据小孔成像模型反推可以计算出在真实相机坐标系下从光心指向目标点P的三维射线方向。然后对这个三维方向矢量依次施加一个反向的滚转旋转矩阵R1(-φ)和一个反向的俯仰旋转矩阵R2(-θ)。这个操作在几何上等价于“如果无人机把当前的滚转和俯仰都回正到零那么目标点P在相机图像上应该在哪里”最后再通过一个固定的旋转矩阵Rα来补偿相机相对机身的固定安装角。得到补偿后坐标经过上述旋转后我们得到了目标点P在“理想相机”图像平面上的投影坐标(x_P, y_P)。这个坐标滤除了当前姿态扰动的影响只反映了目标相对于“机头水平朝前飞行”的理想无人机的位置关系。一个生活化的比喻想象你坐在一辆高速转弯车身倾斜的赛车里透过侧窗看一个路标路标在窗框里的位置会快速移动。但如果你能在脑海中瞬间“摆正”车身想象自己是在平直道路上行驶再判断路标相对于车头方向的位置这个位置才是对你决定方向盘转角真正有用的信息。“理想相机”模型做的就是这件事。经过“理想相机”补偿后我们用于控制律计算的误差e (x_P, y_P)就只包含我们希望消除的、目标偏离期望位置图像中心的信息而剔除了自身姿态扰动带来的噪声。控制器将基于这个“纯净”的误差信号来工作。3. 优化控制律设计与实现步骤在通过“理想相机”模型获得稳定的特征点误差信号后下一步就是设计控制律将这个二维图像误差转换为无人机的一维偏航角速度指令。3.1 控制律推导从欠定方程到最小二乘解我们的控制目标是让无人机以恒定空速V_t和恒定高度绕目标做圆周运动。在这种稳态下无人机在“理想相机”对应的机体坐标系即滚转俯仰为零的坐标系下的运动是高度简化的线速度只有沿机体X轴前进方向的速度V_tY和Z方向速度为0。角速度只有绕机体Z轴偏航方向的角速度ω_z即\dot{\psi}。将这个简化后的运动模型[V_t, 0, 0, 0, 0, ω_z]^T代入图像雅可比矩阵公式并与经过“理想相机”补偿后的特征点速度[\dot{u}_1, \dot{v}_1]^T建立等式我们会得到如下形式的方程[\dot{u}_1; \dot{v}_1] [M1; M2] * ω_z [C; 0]其中M1,M2是由图像雅可比矩阵元素、相机安装角α等构成的已知量C是一个与V_t、焦距f、目标深度z相关的常数项。现在我们有一个未知数ω_z却有两个方程来自u和v两个方向。这是一个超定方程组。最优的解法是采用最小二乘法寻找一个ω_z使得方程两边的误差平方和最小。通过简单的矩阵运算可以得到ω_z的解析解ω_z (M1*( \dot{u}_1 - C) M2* \dot{v}_1) / (M1^2 M2^2)这个公式就是最终的控制律。它巧妙地规避了直接求解图像雅可比矩阵伪逆的复杂性和数值不稳定性直接利用特征点质心的速度信息通过一次计算就得到了偏航角速度指令。3.2 深度信息z的处理细心的读者会发现常数项C中包含目标深度z。在单目视觉中深度z是无法直接测量的。这里有几种处理策略设为常数在环绕跟踪的稳态下无人机与目标的相对距离即深度z在光轴方向的分量近似恒定。可以将其设为一个经验值或初始估计值。这种方法最简单虽然会影响环绕半径的精确性但不影响跟踪的稳定性。控制器依然会努力将目标保持在图像中心只是最终稳定的圆周半径会因z的设定值不同而变化。在线估计可以通过其他传感器如超声波、激光雷达融合估计或者利用单目SLAM、SFM等视觉方法进行粗略估计。这能提升半径控制精度但增加了系统复杂度。自适应调节可以设计一个慢速外环根据图像中目标的大小像素面积来微调z的估计值使其能适应目标高度的缓慢变化。在实际工程中对于很多巡检类应用只要无人机能稳定地以某个半径环绕目标飞行半径的绝对精度并非首要要求因此策略1设为常数因其鲁棒性和简单性而被广泛采用。3.3 完整控制算法流程将上述所有模块串联起来就得到了从图像输入到舵机输出的完整闭环算法。其在线运行的伪代码如下所示初始化设定无人机巡航速度V_t期望高度相机内参(f, u0, v0)安装角α深度估计值z控制器增益λ。 循环每收到一帧图像 1. 目标检测与定位 a. 对当前图像进行处理识别并跟踪目标。 b. 计算目标在图像像素坐标系中的质心坐标 (u, v)。 2. 姿态补偿“理想相机”模型 a. 从飞控读取当前无人机的滚转角 φ 和俯仰角 θ。 b. 根据公式(15)将原始像素坐标 (u, v) 转换为补偿后的“理想图像”坐标 (u, v)。 3. 误差与特征点速度计算 a. 计算图像误差 e [u - u0; v - v0] 假设期望位置在图像中心(u0, v0)。 b. 计算特征点速度 s_dot -λ * e λ为对角正定矩阵控制收敛速度。 4. 控制量解算 a. 根据当前 (u, v) 和已知的 f, z, α计算图像雅可比矩阵 Jv 中的相关元素。 b. 计算中间量 M1, M2 和常数项 C (f/z) * V_t。 c. 代入控制律公式ω_z (M1*(s_dot_u - C) M2*s_dot_v) / (M1^2 M2^2)。 5. 指令输出 a. 将计算得到的偏航角速度指令 ω_z 发送给飞控的角速度控制环。 b. 飞控综合空速、高度保持等指令最终生成舵面偏转信号。 结束循环这个流程清晰地展示了如何将复杂的数学模型转化为一步步可执行的代码。其中步骤2的坐标转换和步骤4的控制律解算是实现稳定跟踪的核心。4. 系统稳定性分析与参数整定任何一个控制系统尤其是用于安全关键场景的无人机控制系统其稳定性必须得到严格的理论保证。本文采用李雅普诺夫直接法对设计的控制律进行了稳定性证明。4.1 稳定性证明思路控制器的目标是双重的图像误差收敛目标在“理想图像”中的位置误差e应趋于零。运动状态收敛无人机应收敛到以恒定角速度ω_z绕圈的状态且该角速度与当前滚转角φ满足固定翼的协调转弯关系ω_z - (g / V_t) * tan(φ)g为重力加速度。我们定义了一个李雅普诺夫函数V 1/2 * e^T * e 1/2 * δ^2其中δ是实际偏航角速度与协调转弯所需角速度的偏差。通过求导并代入我们设计的控制律和误差动态方程可以证明V的导数负定。根据李雅普诺夫稳定性定理这保证了系统是渐近稳定的即随着时间推移图像误差e和运动偏差δ都会趋于零。这意味着无人机最终能平滑、稳定地进入并保持环绕跟踪状态。4.2 关键参数整定与实操心得理论证明保证了“可以稳定”但“如何稳定得好、响应快、超调小”则依赖于参数调试。以下是几个关键参数的整定经验控制器增益λ作用直接决定特征点误差的收敛速度。λ越大控制器对误差的反应越激进。调试技巧从小值开始如0.1。过大的λ会导致控制指令高频抖动无人机飞行轨迹振荡过小则响应迟钝跟踪滞后严重。建议在仿真中先观察图像误差的收敛曲线使其呈指数衰减且无明显超调为佳。实际飞行中还需考虑图像处理延时λ值通常比仿真设置要小一些。深度估计值z作用影响图像雅可比矩阵的计算进而影响控制量的大小和环绕半径。调试技巧这是一个先验参数。如果你知道无人机的大致任务高度H和相机倾角α则初始z ≈ H / cos(α)。在仿真中可以故意设置一个偏离真实值20%-30%的z观察系统是否依然能稳定跟踪理论上应能稳定但半径会变。这可以测试控制器的鲁棒性。相机安装角α作用决定了相机的前视视野与下视视野的比例直接影响目标在图像中的位置和可跟踪范围。实操心得α通常设置在30°到60°之间。角度太小接近0°则主要前视不利于观察正下方目标角度太大接近90°则主要下视前方视野太小无人机在接近目标时容易丢失目标。45°是一个常用的折中值。务必在机体上精确测量此角度并输入算法角度误差会直接带来稳态误差。图像处理频率与控制频率注意这是工程实现中的关键瓶颈。视觉处理目标检测、质心计算耗时较长可能只能达到10-30Hz。而飞控的控制环频率通常在50Hz以上。解决方案采用“视觉感知”与“控制决策”异步运行的架构。控制循环以固定高频率运行每次迭代使用最新的、有效的视觉测量结果。如果当前周期没有新的视觉数据到来则使用上一周期的数据或进行短时预测如匀速模型。这需要在代码中做好数据的时间戳管理和有效性判断。5. 仿真验证与问题排查实录理论设计和参数整定完成后必须在高保真仿真环境中进行充分验证才能考虑实机飞行。本文采用硬件在环仿真进行验证这是目前无人机算法开发的标准流程。5.1 仿真环境搭建要点仿真平台选择GazeboROS是机器人仿真的黄金组合。Gazebo提供高精度的物理引擎模拟空气动力学、传感器噪声ROS提供模块化的通信框架。飞机模型需要导入或编写一个动力学特性准确的固定翼无人机模型。关键参数包括质量、翼展、翼面积、气动系数、发动机/推进器模型等。可以使用开源的模型如ArduPilot的Gazebo插件提供的Typhoon H480固定翼模型。传感器模拟在无人机模型上添加一个相机传感器插件配置其内参焦距、分辨率、帧率、安装位置和角度α。Gazebo会实时渲染出仿真世界的图像。硬件在环使用真实的飞控硬件如Pixhawk通过MAVLink协议与Gazebo中的飞机模型连接。飞控运行真实的飞行控制固件如PX4或ArduPilot接收来自仿真环境的传感器数据IMU、空速计等并输出舵机控制信号给仿真模型。这样除了飞机和环境是虚拟的整个控制链路与真实情况完全一致。视觉模块在ROS中创建一个节点订阅Gazebo相机的话题对图像进行目标检测与跟踪算法处理计算出目标质心(u, v)。伺服控制器节点创建另一个ROS节点订阅视觉节点的质心话题和飞控的姿态话题运行我们上述设计的“理想相机”补偿和优化控制律算法并将计算出的偏航角速度指令通过ROS或MAVLink发送给飞控。5.2 典型问题与排查技巧在仿真调试中你几乎一定会遇到以下问题。这里提供我的排查思路和解决记录问题1无人机进入环绕状态后目标在图像中持续小幅度振荡无法稳定在中心点。可能原因A控制器增益λ过大。排查观察指令波形图。如果发送给飞控的偏航角速度指令ω_z高频上下抖动基本就是增益过大。解决逐步减小λ直到振荡幅度在可接受范围内。同时检查图像处理的输出是否平滑有时检测框抖动也会带来输入噪声。可能原因B“理想相机”补偿中的姿态角φ,θ噪声大。排查记录并绘制从飞控获取的滚转角和俯仰角曲线。如果噪声明显在补偿前对姿态角进行低通滤波。解决设计一个一阶低通滤波器。angle_filtered 0.9 * angle_filtered_prev 0.1 * angle_raw。滤波系数需要权衡系数太大会引入相位滞后影响动态响应太小则滤波效果不佳。可能原因C深度z估计不准。排查在仿真中我们可以直接获取目标的真实深度。将算法中使用的z_estimated与z_truth打印出来对比。解决如果z是常数尝试微调其值。或者实现一个简单的自适应律z z_initial * sqrt(area_initial / area_current)其中area是目标在图像中的像素面积假设目标实际大小不变。问题2无人机在接近目标时控制指令突然发散导致飞机失控或丢失目标。可能原因A特征点靠近图像边缘导致图像雅可比矩阵J中的某些项变得非常大或非常小引起数值不稳定。排查打印计算控制律时的中间变量M1,M2观察在目标靠近边缘时是否出现异常值。解决在算法中增加安全边界。当目标质心(u, v)距离图像边界小于一定像素如50像素时不再使用视觉伺服控制而是切换到一个保守的“恢复模式”例如让无人机向图像中心方向缓慢偏航直到目标回到视野安全区。可能原因B姿态角变化过快超出“理想相机”模型的小角度假设范围。排查固定翼在急转弯时滚转角可能达到30-45度。检查在大角度滚转时补偿后的坐标(u, v)计算是否出现异常例如除零错误。解决确保在计算旋转矩阵和三角函数时使用数值稳定的库函数。对于tan(φ)当φ接近 ±90° 时需要特别处理但固定翼正常飞行不会达到此角度。问题3跟踪移动目标时存在明显的滞后无人机总是跟在目标后面。可能原因纯比例控制的固有缺陷。分析我们的控制律本质上是比例控制P控制误差e大控制量ω_z就大。但对于匀速运动的目标需要无人机提前产生一个恒定的角速度才能形成同心圆。比例控制为了消除稳态误差必须保留一个固定的误差e这就导致了滞后。解决引入微分项或前馈项。微分项在误差e的基础上加入误差变化率de/dt作为微分项D项可以预测误差的变化趋势减少超调改善动态响应。前馈项如果我们能估计目标的运动速度例如通过图像序列计算光流或使用滤波器预测可以将这个预估速度作为前馈量加入到控制律中提前补偿目标的运动。这是提升跟踪移动目标性能的最有效方法。5.3 硬件在环仿真结果分析在完成上述调试后我在Gazebo中设置了静态目标和匀速直线运动目标两种场景进行测试。仿真结果表明静态目标无人机能从任意初始位置和航向平滑地切入到以目标为中心的圆形轨道上。稳态时目标在“理想图像”中的位置误差在±5像素以内实际飞行轨迹的半径波动小于5%。匀速运动目标无人机能成功跟踪并保持环绕。在仅使用比例控制时存在约1-2秒的相位滞后。加入基于目标速度估计的前馈后滞后显著减小跟踪性能大幅提升。整个仿真系统稳定运行CPU占用率主要来自图像处理和控制器计算在搭载Intel i7的机器上低于15%证明了算法的实时性。通过HIL仿真我们不仅验证了算法的功能更重要的是提前暴露并解决了大量工程实现中的细节问题为后续的实机飞行打下了坚实的基础。
