简介面向无人机视觉开发者、吊舱算法工程师及目标定位方向学习者这份压缩包围绕“无人机吊舱单目相机目标定位”提供一套可运行、易扩展的C工程实现。工程采用模块化结构含src、include、demo及CMakeLists构建配置并附带使用说明文本便于快速理解相机成像模型、坐标系转换与目标测距定位流程图片资料展示了相机坐标系、图像坐标系和归一化平面坐标系等关键概念可辅助算法推演。压缩包共15个文件以cpp/h源码、txt说明、png示意图为主另含json、in等配置文件整体仅179KB轻量实用。目前已有334人学习下载适合希望对照代码掌握单目视觉定位原理并完成工程落地的读者。1. 吊舱单目相机定位本质上是一场坐标系接力赛无人机吊舱挂一颗单目相机想实时报出地面目标的经纬度是光电吊舱最早接触到的需求之一。很多第一次写目标定位算法的工程师会栽在一个地方目标检测模型跑通了像素坐标拿到了下一步却不知道拿这个像素点怎么办。单目相机只有一张图像素坐标只能确定一条射线无法确定目标距离。工程里破局的办法是把无人机位置、吊舱姿态角和地面假设串起来让像素坐标经过相机系、吊舱系、机体系、导航系层层变换最后落到大地坐标。这条链路里最隐蔽的坑不是矩阵公式而是坐标系定义、旋转顺序、测距模型里高度和俯仰角的符号。下面按这条链路从原理讲到可编译的 C 代码适合正在写无人机目标定位、目标跟踪或者要把视觉结果接入地面站显示的工程师。2. 吊舱单目定位的坐标变换链从像素系到 NED 导航系2.1 四个关键坐标系像素、相机、机体、NED目标定位算法开始前先把坐标系约定写死在注释里。像素坐标系(u,v)原点在图像左上角u向右、v向下相机坐标系原点在光心Z轴沿光轴向前X轴向右Y轴向下这与OpenCV约定一致。机体坐标系采用NED风格原点在飞控中心X轴机头、Y轴右翼、Z轴向下。导航坐标系即NED或ENU但无人机领域绝大多数用NED原点是起飞点或RTK基准点。吊舱坐标系其实指稳定平台坐标系它与相机系之间往往只差一个固定外参因为吊舱出厂时会把相机安放在平台坐标系的固定位置。要注意两种常见歧义。吊舱输出的“方位角”有的厂家是相对机头有的是相对地理北甚至磁北俯仰角有的定义为光轴与水平面夹角有的是与机体Z轴夹角。拿到任何一份吊舱协议第一件事就是把角度参考系写进配置文件否则后面所有坐标都是错的。我一般会在代码里加一个枚举字段标明“方位角参考机头 / 真北 / 磁北”。2.2 姿态角与旋转矩阵先统一 YPR 旋转顺序无论用欧拉角还是四元数要先确定旋转顺序。很多飞控默认Z-Y-X也就是先偏航、再俯仰、最后横滚。把欧拉角转成旋转矩阵时矩阵乘法顺序写反结果是整个坐标系的轴都错位。以一个惯用约定为例R_ned_body Rz(yaw) * Ry(pitch) * Rx(roll)这里的写法是列向量左乘向量从机体坐标系转到NED。旋转矩阵是正交矩阵反向变换直接转置。欧拉角在俯仰接近±90°时有万向锁问题工程上建议用Eigen::Quaterniond保存姿态只在日志或调试时转成欧拉角。不同飞控对姿态方向的定义可能有差异需要以飞控文档为准不能只看矩阵形式。2.3 吊舱安装角与杆臂被漏掉的外参吊舱安装在机体下方时吊舱的“机头方向”和机体机头之间有一个固定偏置。常见做法是把安装角作为外参传入算法而不要把它和吊舱实时角度混在一起。如果吊舱输出的是相对机体的角度变换顺序是R_ned_cam R_ned_body * R_body_gimbal * R_gimbal_cam其中R_body_gimbal就是安装角构成的固定旋转。另一个容易被漏掉的是杆臂向量吊舱光心到GPS天线相位中心有几十厘米的平移。当目标距离几百米以上时杆臂可以忽略但近距离10米以内或者云台偏转大时平移项会造成数米偏置。工程处理办法是在变换链里补一个t_body_gimbal和t_ned_body单位米通常实测一次就能标定。现在很多吊舱在稳定平台内已经融合了自身姿态直接对外输出“相对地理北的方位角 相对水平面的俯仰角”。这种情况不用再经过机体可以直接构造R_ned_gimbal见下节。2.4 变换链公式与工程参数表目标定位算法最核心的变换链可以写成下面五步p_cam_norm inv(K) * [u, v, 1]^T p_cam p_cam_norm * s p_gimbal R_gimbal_cam * p_cam t_gimbal_cam p_body R_body_gimbal * p_gimbal t_body_gimbal p_ned R_ned_body * p_body t_ned_bodys是尺度因子由第3章的测距模型求出t_ned_body是无人机当前位置在NED下的坐标。实际代码里第1行到第4行经常合并成一个单位方向向量最后用射线与地平面求交替代显式计算s这样更稳。下面这张参数表建议直接做成配置文件字段参数含义来源单位fx, fy, cx, cy相机内参标定板标定像素dist镜头畸变系数标定-gimbal_yaw_offset吊舱相对机体航向安装角厂测或自标定度gimbal_pitch_offset吊舱相对机体俯仰安装角厂测或自标定度gimbal_yaw吊舱当前方位角吊舱串口/网络协议度gimbal_pitch吊舱当前俯仰角吊舱串口/网络协议度uav_yaw/pitch/roll无人机姿态飞控度或四元数uav_lat/lon/height无人机位置GPS/RTK度/米表中每一项都可能因为协议差异改变符号所以调试时要先把单点验证跑通再上连续测试。2.5 用 Eigen 实现姿态矩阵与坐标变换的代码骨架坐标变换代码建议用 Eigen头文件即可不引入额外依赖。下面是一个生成旋转矩阵并做向量变换的最小骨架#include Eigen/Core #include Eigen/Geometry // 按 Z-Y-X 顺序生成旋转矩阵角度单位弧度 Eigen::Matrix3d RotationFromYPR(double yaw, double pitch, double roll) { return (Eigen::AngleAxisd(yaw, Eigen::Vector3d::UnitZ()) * Eigen::AngleAxisd(pitch, Eigen::Vector3d::UnitY()) * Eigen::AngleAxisd(roll, Eigen::Vector3d::UnitX())).toRotationMatrix(); } // 把相机系下的方向向量转到 NED Eigen::Vector3d CameraRayToNed( const Eigen::Vector3d ray_cam, const Eigen::Matrix3d R_gimbal_cam, const Eigen::Matrix3d R_body_gimbal, const Eigen::Matrix3d R_ned_body) { Eigen::Vector3d ray_gimbal R_gimbal_cam * ray_cam; Eigen::Vector3d ray_body R_body_gimbal * ray_gimbal; return R_ned_body * ray_body; }RotationFromYPR中用三个AngleAxisd连续相乘得到的就是矩阵乘积Rz * Ry * Rx顺序是先偏航、再俯仰、最后横滚。CameraRayToNed完成相机系到NED的方向旋转平移项没有包含因为计算单位方向向量时平移可以最后再加。注意这里的ray_cam不要求单位向量归一化可以在求交时一起做避免多次归一化引入误差。3. 单目测距模型地平面假设下的目标距离计算与修正3.1 单目为什么测不准距离一条射线对应无穷多个点单目相机成像是一个降维过程像素坐标只能反过来确定一条射线射线上所有点都落在同一个像素上。没有深度传感器也没有双目视差就必须引入外部约束来固定射线上的点。无人机吊舱场景里最常见的约束是地平面假设目标在地面上或者已知目标相对地面高度就可以用光心高度、光轴俯仰角和像素位置算交点。这个假设在平坦地面、水面、机场等场景下非常有效山区、楼顶目标、地形起伏大的场景需要配合地表高程模型或目标高度先验。在没有已知地标的大范围场景里这是唯一不需要额外传感器就能定尺度的办法。3.2 地平面假设测距公式高度除以角正切假设光心到地面高度为 h吊舱光轴俯仰角 pitch 向下为正。目标像素纵坐标 v先转成相机归一化坐标y (v - cy) / fy alpha atan(y) beta pitch alphaalpha是目标射线相对光轴在垂直方向上的夹角图像v轴向下为正。beta就是目标射线相对水平面的俯仰角。当beta 0时射线指向上方不会与地面相交该像素点无效。当beta 0时水平距离为D h / tan(beta)这个 D 是目标在水平面上的投影到光心正下方投影点的距离不是斜距。要得到NED下的北向和东向分量需要再结合吊舱方位角 yawnorth D * cos(yaw) east D * sin(yaw)注意上面的 beta 推导只适用于无滚转或滚转很小的吊舱。如果吊舱有滚转就不能再单独拆 y而是要把归一化方向向量 (x, y, 1) 通过旋转矩阵转到 NED再与地平面求交。实际吊舱有稳定平台滚转通常很小但算法里保留旋转矩阵路径更通用。3.3 目标高度不为零底部中点优先于中心点地面上的行人、车辆有实际高度。如果你用目标框中心点做测距等价于假设目标中心贴在地面上结果会系统性偏近。原因很简单中心点对应的射线与地面交点比目标真实底部点要近。对于1.8米高的人在100米高度、斜视条件下中心点测距可能偏近10米以上。解决办法有两个。首选目标检测器输出目标框底边中点这个点在图像上是目标与地面接触点的投影直接参与求交不需要高度假设。次选只能拿到中心点时根据目标类别给一个先验高度 H然后让射线与“目标中心所在高度平面”求交。在NED中这个平面的 z 坐标是-H/2注意NED向下为正目标中心在地面之上所以是负值。工程上可以写一个TargetAnchor枚举默认用BottomCenter只有拿不到底边时才退化为高度修正。3.4 误差灵敏度俯仰角误差与像素误差如何被放大测距公式对俯仰角非常敏感。对 D h / tan(beta) 求导dD/dβ -h / sin^2(β)当目标较远时beta 变小sin^2(beta)急剧减小同样的俯仰角误差会带来更大的距离误差。以100米高度为例目标距离300米时 beta 约为18.4°俯仰角误差0.1°会引起约4米距离误差目标距离800米时 beta 约7.1°同样0.1°误差会放大到约30米可见远距离定位的误差主要来自姿态角而不是图像像素。像素误差的影响由焦距决定长焦吊舱的像素角分辨率更高对测距更有利。因此实际项目中远距离测距优先用长焦并对吊舱俯仰角做低通滤波或与飞控IMU融合。像素层面用亚像素检测或跟踪滤波能明显改善近距离精度。3.5 用 C 实现单目测距函数并做敏感性计算把上面的公式落成一个可复用的函数#include cmath struct CameraIntrinsics { double fx, fy, cx, cy; }; // pitch: 光轴与水平面夹角向下为正单位弧度 // v: 目标像素纵坐标图像原点在左上角 // h: 光心相对地面高度单位米 double PixelToGroundDistance(const CameraIntrinsics K, double pitch, double v, double h) { double alpha std::atan((v - K.cy) / K.fy); double beta pitch alpha; if (beta 0.02) { return -1.0; // 射线不指向地面返回负值表示无效 } return h / std::tan(beta); }beta 0.02是为了避免射线接近水平时算出极大的不可信距离阈值可以根据实际任务调整一般取1度左右。调用时如果吊舱协议给的是仰角向上为正要先取负再传入。这个函数只算水平距离NED坐标换算在下一章的类里完成。敏感性代码可以用几行循环打印不同 v 下的距离CameraIntrinsics K{2000.0, 2000.0, 960.0, 540.0}; double h 100.0; double pitch 11.0 * M_PI / 180.0; for (double v : {600.0, 700.0, 800.0}) { double D PixelToGroundDistance(K, pitch, v, h); std::printf(pixel v%.0f - D%.1f m\n, v, D); }这个例子中 v 越大目标在图像中越靠下测距结果越小符合观察直觉。你也可以把同样的像素偏移比如1个像素换成不同距离区间的 v观察距离跳变量从而预估当前吊舱在哪个距离上开始不可用。4. 目标定位 C 实现GimbalLocator 类、配置参数与常见坑4.1 项目结构与依赖OpenCV、Eigen、C17目标定位代码不需要复杂框架。常见的做法是单独抽一个GimbalLocator类声明和实现分离include/gimbal_locator.hpp放接口src/gimbal_locator.cpp放实现app/main_demo.cpp做一个最小可运行示例。构建用CMake依赖OpenCV矩阵、畸变矫正、projectPoints和Eigen旋转矩阵。CMakeLists.txt 可以这样写cmake_minimum_required(VERSION 3.16) project(gimbal_locator LANGUAGES CXX) set(CMAKE_CXX_STANDARD 17) find_package(OpenCV REQUIRED COMPONENTS core imgproc) find_package(Eigen3 REQUIRED) add_library(gimbal_locator src/gimbal_locator.cpp) target_include_directories(gimbal_locator PUBLIC include) target_link_libraries(gimbal_locator PRIVATE ${OpenCV_LIBS} Eigen3::Eigen)Eigen3只需要头文件编译很快。OpenCV建议保留imgproc后面做图像可视化调试要用。如果只做纯几何解算不用OpenCV也可以但项目里一旦涉及“把解算坐标画回图像”OpenCV就省不掉。4.2 GimbalLocator 接口设计输入像素与姿态输出 NED 坐标类的核心接口围绕“输入一帧图像中的目标像素 当前无人机和吊舱状态输出NED坐标”设计。头文件骨架#pragma once #include Eigen/Core #include Eigen/Geometry #include opencv2/core.hpp struct CameraIntrinsics { double fx, fy, cx, cy; }; enum class TargetAnchor { BottomCenter, // 目标框底边中点推荐 Center // 目标框中心需要目标高度修正 }; struct TargetResult { double north 0.0; // NED 北向米 double east 0.0; // NED 东向米 double down 0.0; // NED 地向地面通常为0 double range 0.0; // 从相机光心到目标的斜距米 }; class GimbalLocator { public: void SetCameraIntrinsics(const CameraIntrinsics K); // 吊舱输出相对地理北的方位角、相对水平面的俯仰角 void SetGimbalState(double yaw_deg, double pitch_deg); void SetUavPosition(const Eigen::Vector3d pos_ned); TargetResult LocateGroundTarget(const cv::Point2d pixel, TargetAnchor anchor, double target_height_m 0.0); };SetCameraIntrinsics在标定完成后调用一次SetGimbalState每帧更新SetUavPosition接收GPS/RTK解算出的NED位置。LocateGroundTarget是每帧的入口。如果要从经纬度输入可以在外层把经纬度转成NED常见做法是用起飞点作为原点用WGS84椭球把经纬度差转成米简单场景也可以用等距圆柱近似。4.3 LocateGroundTarget 完整实现先反投影、再求交实现时把像素坐标转成相机归一化方向向量再用吊舱姿态旋转到NED最后与目标高度平面求交#include gimbal_locator.hpp #include cmath void GimbalLocator::SetGimbalState(double yaw_deg, double pitch_deg) { double yaw yaw_deg * M_PI / 180.0; double pitch pitch_deg * M_PI / 180.0; // 吊舱坐标系到NED先方位角再俯仰角俯仰角向下为正旋转时取负 R_ned_gimbal_ (Eigen::AngleAxisd(yaw, Eigen::Vector3d::UnitZ()) * Eigen::AngleAxisd(-pitch, Eigen::Vector3d::UnitY())).toRotationMatrix(); } TargetResult GimbalLocator::LocateGroundTarget( const cv::Point2d pixel, TargetAnchor anchor, double target_height_m) { Eigen::Vector3d p_cam_norm; p_cam_norm (pixel.x - K_.cx) / K_.fx, (pixel.y - K_.cy) / K_.fy, 1.0; Eigen::Vector3d dir_ned R_ned_gimbal_ * p_cam_norm.normalized(); if (dir_ned.z() 0.0) { return TargetResult{}; // 射线不指向下方无法与地面求交 } // NED中 z 向下为正目标中心高度在地面以上所以是负值 double plane_z (anchor TargetAnchor::BottomCenter) ? 0.0 : -target_height_m * 0.5; double t (pos_ned_.z() - plane_z) / (-dir_ned.z()); TargetResult r; r.north pos_ned_.x() dir_ned.x() * t; r.east pos_ned_.y() dir_ned.y() * t; r.down plane_z; r.range t; return r; }dir_ned.z() 0的检查很关键很多无效像素会在这里被挡住。目标框中心点必须传target_height_m否则交点落在目标前面的地面上结果偏近。如果你的吊舱输出的是相对机体的方位角修改SetGimbalState把R_ned_body * R_body_gimbal换成R_ned_gimbal_即可具体公式在第2章已经给出。这个类的优势是像素、姿态、位置三者彻底解耦单测时可以直接喂固定值。4.4 参数配置与标定流程内参、外参、时间戳对齐参数配置最容易出问题的有三处。第一是相机内参用OpenCV棋盘格标定时建议收集20张以上不同角度图像重投影误差控制在0.15像素以内标定完把内参写进JSON文件。第二是吊舱安装角可以在无人机起飞前把吊舱对准正前方的一个已知地标用反投影残差估计安装角也可以用多次空中拍摄已知点做最小二乘求解标定结果应满足重投影误差小于2个像素。第三是时间戳对齐吊舱的图像、云台姿态、飞控GPS往往来自三个不同时钟云台快速转动时图像和姿态相差50毫秒就能造成几十米误差。常见做法是给每帧图像打上主机接收时间把吊舱姿态和GPS按时间戳做线性插值或样条插值再参与解算。姿态数据可以先用滑动窗口滤波去掉突变但滤波延迟必须小于100毫秒否则跟踪目标时会有明显拖尾。4.5 常见问题与排查现象、原因、对策下面三个问题在实飞中几乎一定会遇到按我的排查顺序列出来现象可能原因排查方法目标点始终偏北或偏南一个固定角度吊舱方位角零点偏移、磁偏角未扣除、安装角错误对正北地标检查对比吊舱方位角输出与真实方位角近距离准、远距离偏差越来越大测距模型 beta 符号错、高度用错、俯仰角时延大做第5章仿真回环先排除算法符号再看时延云台转动时结果剧烈跳动图像与姿态时间戳不同步记录两路时间戳打印差值做插值对齐横滚较大时结果左右偏滚转未参与旋转矩阵确认旋转矩阵包含 roll且符号与飞控一致排查时每步只改一个变量不要同时改符号和高度的基准否则很难定位问题。我习惯先在静态悬停时验证一个固定地标再稍微偏转云台观察解算目标是否保持在地标附近如果保持不住问题大概率在姿态变换如果保持得住再测不同距离。5. 精度验证与调试仿真回环、误差统计与 YOLO 目标点5.1 仿真回环从NED坐标反投影再正解算法写完先做仿真回环不要直接上飞机。思路是构造一个地面目标NED坐标用当前的无人机位置、吊舱姿态、相机内参通过cv::projectPoints把目标投影到图像得到像素再用这个像素调用LocateGroundTarget解算比较解算结果和真值。这个回环能在5分钟内发现旋转顺序、符号、高度基准三处最常见的bug。5.2 静态悬停误差统计CEP50 怎么算实飞验证时让无人机悬停吊舱固定指向目标点连续记录300帧解算坐标。用RTK或全站仪测出目标真值统计每个点的北向、东向误差画散点图并计算CEP50以真值为圆心覆盖50%误差点的圆半径就是CEP50。建议同时记录每个目标点的距离画出误差随距离变化的曲线这条曲线决定了这套吊舱单目目标定位算法在哪个距离内可用。5.3 把解算坐标投影回图像的闭环验证别看输出坐标直接把解算出的NED点用cv::projectPoints投影回图像看是否落在目标框底部。投影时把相机到NED的旋转矩阵转成rvec相机位置取吊舱光心在NED的坐标作为tveccv::Mat rvec, tvec; cv::Rodrigues(R_ned_cam_.t(), rvec); tvec -R_ned_cam_.t() * pos_ned_; // 相机系下的位置向量杆臂明显时替换为光心坐标 std::vectorcv::Point3d object_pts {cv::Point3d(r.north, r.east, r.down)}; std::vectorcv::Point2d image_pts; cv::projectPoints(object_pts, rvec, tvec, K_, dist_, image_pts);如果投影点与目标像素距离小于3个像素说明整条链自洽如果差异很大优先检查R_ned_cam_是否写成了转置。这是最快定位问题在旋转还是测距的手段。5.4 与 YOLO 等检测器对接时目标点的选择最后落到实际部署。检测器给的矩形框中心点不适合直接参与测距要把框底边中点作为目标锚点。YOLO系列输出里底边中点就是(x1x2)/2, y2不需要额外改模型。如果只有中心点可用就根据目标类别给一个先验身高或车高再传入LocateGroundTarget的高度参数。整个目标定位算法链路里这一步改动最小、收益最大把锚点从中心换成底边中点通常比调三个滤波参数都管用。本文还有配套的精品资源点击获取 无人机吊舱单目相机目标定位算法：坐标变换与测距的C++工程实践
简介面向无人机视觉开发者、吊舱算法工程师及目标定位方向学习者这份压缩包围绕“无人机吊舱单目相机目标定位”提供一套可运行、易扩展的C工程实现。工程采用模块化结构含src、include、demo及CMakeLists构建配置并附带使用说明文本便于快速理解相机成像模型、坐标系转换与目标测距定位流程图片资料展示了相机坐标系、图像坐标系和归一化平面坐标系等关键概念可辅助算法推演。压缩包共15个文件以cpp/h源码、txt说明、png示意图为主另含json、in等配置文件整体仅179KB轻量实用。目前已有334人学习下载适合希望对照代码掌握单目视觉定位原理并完成工程落地的读者。1. 吊舱单目相机定位本质上是一场坐标系接力赛无人机吊舱挂一颗单目相机想实时报出地面目标的经纬度是光电吊舱最早接触到的需求之一。很多第一次写目标定位算法的工程师会栽在一个地方目标检测模型跑通了像素坐标拿到了下一步却不知道拿这个像素点怎么办。单目相机只有一张图像素坐标只能确定一条射线无法确定目标距离。工程里破局的办法是把无人机位置、吊舱姿态角和地面假设串起来让像素坐标经过相机系、吊舱系、机体系、导航系层层变换最后落到大地坐标。这条链路里最隐蔽的坑不是矩阵公式而是坐标系定义、旋转顺序、测距模型里高度和俯仰角的符号。下面按这条链路从原理讲到可编译的 C 代码适合正在写无人机目标定位、目标跟踪或者要把视觉结果接入地面站显示的工程师。2. 吊舱单目定位的坐标变换链从像素系到 NED 导航系2.1 四个关键坐标系像素、相机、机体、NED目标定位算法开始前先把坐标系约定写死在注释里。像素坐标系(u,v)原点在图像左上角u向右、v向下相机坐标系原点在光心Z轴沿光轴向前X轴向右Y轴向下这与OpenCV约定一致。机体坐标系采用NED风格原点在飞控中心X轴机头、Y轴右翼、Z轴向下。导航坐标系即NED或ENU但无人机领域绝大多数用NED原点是起飞点或RTK基准点。吊舱坐标系其实指稳定平台坐标系它与相机系之间往往只差一个固定外参因为吊舱出厂时会把相机安放在平台坐标系的固定位置。要注意两种常见歧义。吊舱输出的“方位角”有的厂家是相对机头有的是相对地理北甚至磁北俯仰角有的定义为光轴与水平面夹角有的是与机体Z轴夹角。拿到任何一份吊舱协议第一件事就是把角度参考系写进配置文件否则后面所有坐标都是错的。我一般会在代码里加一个枚举字段标明“方位角参考机头 / 真北 / 磁北”。2.2 姿态角与旋转矩阵先统一 YPR 旋转顺序无论用欧拉角还是四元数要先确定旋转顺序。很多飞控默认Z-Y-X也就是先偏航、再俯仰、最后横滚。把欧拉角转成旋转矩阵时矩阵乘法顺序写反结果是整个坐标系的轴都错位。以一个惯用约定为例R_ned_body Rz(yaw) * Ry(pitch) * Rx(roll)这里的写法是列向量左乘向量从机体坐标系转到NED。旋转矩阵是正交矩阵反向变换直接转置。欧拉角在俯仰接近±90°时有万向锁问题工程上建议用Eigen::Quaterniond保存姿态只在日志或调试时转成欧拉角。不同飞控对姿态方向的定义可能有差异需要以飞控文档为准不能只看矩阵形式。2.3 吊舱安装角与杆臂被漏掉的外参吊舱安装在机体下方时吊舱的“机头方向”和机体机头之间有一个固定偏置。常见做法是把安装角作为外参传入算法而不要把它和吊舱实时角度混在一起。如果吊舱输出的是相对机体的角度变换顺序是R_ned_cam R_ned_body * R_body_gimbal * R_gimbal_cam其中R_body_gimbal就是安装角构成的固定旋转。另一个容易被漏掉的是杆臂向量吊舱光心到GPS天线相位中心有几十厘米的平移。当目标距离几百米以上时杆臂可以忽略但近距离10米以内或者云台偏转大时平移项会造成数米偏置。工程处理办法是在变换链里补一个t_body_gimbal和t_ned_body单位米通常实测一次就能标定。现在很多吊舱在稳定平台内已经融合了自身姿态直接对外输出“相对地理北的方位角 相对水平面的俯仰角”。这种情况不用再经过机体可以直接构造R_ned_gimbal见下节。2.4 变换链公式与工程参数表目标定位算法最核心的变换链可以写成下面五步p_cam_norm inv(K) * [u, v, 1]^T p_cam p_cam_norm * s p_gimbal R_gimbal_cam * p_cam t_gimbal_cam p_body R_body_gimbal * p_gimbal t_body_gimbal p_ned R_ned_body * p_body t_ned_bodys是尺度因子由第3章的测距模型求出t_ned_body是无人机当前位置在NED下的坐标。实际代码里第1行到第4行经常合并成一个单位方向向量最后用射线与地平面求交替代显式计算s这样更稳。下面这张参数表建议直接做成配置文件字段参数含义来源单位fx, fy, cx, cy相机内参标定板标定像素dist镜头畸变系数标定-gimbal_yaw_offset吊舱相对机体航向安装角厂测或自标定度gimbal_pitch_offset吊舱相对机体俯仰安装角厂测或自标定度gimbal_yaw吊舱当前方位角吊舱串口/网络协议度gimbal_pitch吊舱当前俯仰角吊舱串口/网络协议度uav_yaw/pitch/roll无人机姿态飞控度或四元数uav_lat/lon/height无人机位置GPS/RTK度/米表中每一项都可能因为协议差异改变符号所以调试时要先把单点验证跑通再上连续测试。2.5 用 Eigen 实现姿态矩阵与坐标变换的代码骨架坐标变换代码建议用 Eigen头文件即可不引入额外依赖。下面是一个生成旋转矩阵并做向量变换的最小骨架#include Eigen/Core #include Eigen/Geometry // 按 Z-Y-X 顺序生成旋转矩阵角度单位弧度 Eigen::Matrix3d RotationFromYPR(double yaw, double pitch, double roll) { return (Eigen::AngleAxisd(yaw, Eigen::Vector3d::UnitZ()) * Eigen::AngleAxisd(pitch, Eigen::Vector3d::UnitY()) * Eigen::AngleAxisd(roll, Eigen::Vector3d::UnitX())).toRotationMatrix(); } // 把相机系下的方向向量转到 NED Eigen::Vector3d CameraRayToNed( const Eigen::Vector3d ray_cam, const Eigen::Matrix3d R_gimbal_cam, const Eigen::Matrix3d R_body_gimbal, const Eigen::Matrix3d R_ned_body) { Eigen::Vector3d ray_gimbal R_gimbal_cam * ray_cam; Eigen::Vector3d ray_body R_body_gimbal * ray_gimbal; return R_ned_body * ray_body; }RotationFromYPR中用三个AngleAxisd连续相乘得到的就是矩阵乘积Rz * Ry * Rx顺序是先偏航、再俯仰、最后横滚。CameraRayToNed完成相机系到NED的方向旋转平移项没有包含因为计算单位方向向量时平移可以最后再加。注意这里的ray_cam不要求单位向量归一化可以在求交时一起做避免多次归一化引入误差。3. 单目测距模型地平面假设下的目标距离计算与修正3.1 单目为什么测不准距离一条射线对应无穷多个点单目相机成像是一个降维过程像素坐标只能反过来确定一条射线射线上所有点都落在同一个像素上。没有深度传感器也没有双目视差就必须引入外部约束来固定射线上的点。无人机吊舱场景里最常见的约束是地平面假设目标在地面上或者已知目标相对地面高度就可以用光心高度、光轴俯仰角和像素位置算交点。这个假设在平坦地面、水面、机场等场景下非常有效山区、楼顶目标、地形起伏大的场景需要配合地表高程模型或目标高度先验。在没有已知地标的大范围场景里这是唯一不需要额外传感器就能定尺度的办法。3.2 地平面假设测距公式高度除以角正切假设光心到地面高度为 h吊舱光轴俯仰角 pitch 向下为正。目标像素纵坐标 v先转成相机归一化坐标y (v - cy) / fy alpha atan(y) beta pitch alphaalpha是目标射线相对光轴在垂直方向上的夹角图像v轴向下为正。beta就是目标射线相对水平面的俯仰角。当beta 0时射线指向上方不会与地面相交该像素点无效。当beta 0时水平距离为D h / tan(beta)这个 D 是目标在水平面上的投影到光心正下方投影点的距离不是斜距。要得到NED下的北向和东向分量需要再结合吊舱方位角 yawnorth D * cos(yaw) east D * sin(yaw)注意上面的 beta 推导只适用于无滚转或滚转很小的吊舱。如果吊舱有滚转就不能再单独拆 y而是要把归一化方向向量 (x, y, 1) 通过旋转矩阵转到 NED再与地平面求交。实际吊舱有稳定平台滚转通常很小但算法里保留旋转矩阵路径更通用。3.3 目标高度不为零底部中点优先于中心点地面上的行人、车辆有实际高度。如果你用目标框中心点做测距等价于假设目标中心贴在地面上结果会系统性偏近。原因很简单中心点对应的射线与地面交点比目标真实底部点要近。对于1.8米高的人在100米高度、斜视条件下中心点测距可能偏近10米以上。解决办法有两个。首选目标检测器输出目标框底边中点这个点在图像上是目标与地面接触点的投影直接参与求交不需要高度假设。次选只能拿到中心点时根据目标类别给一个先验高度 H然后让射线与“目标中心所在高度平面”求交。在NED中这个平面的 z 坐标是-H/2注意NED向下为正目标中心在地面之上所以是负值。工程上可以写一个TargetAnchor枚举默认用BottomCenter只有拿不到底边时才退化为高度修正。3.4 误差灵敏度俯仰角误差与像素误差如何被放大测距公式对俯仰角非常敏感。对 D h / tan(beta) 求导dD/dβ -h / sin^2(β)当目标较远时beta 变小sin^2(beta)急剧减小同样的俯仰角误差会带来更大的距离误差。以100米高度为例目标距离300米时 beta 约为18.4°俯仰角误差0.1°会引起约4米距离误差目标距离800米时 beta 约7.1°同样0.1°误差会放大到约30米可见远距离定位的误差主要来自姿态角而不是图像像素。像素误差的影响由焦距决定长焦吊舱的像素角分辨率更高对测距更有利。因此实际项目中远距离测距优先用长焦并对吊舱俯仰角做低通滤波或与飞控IMU融合。像素层面用亚像素检测或跟踪滤波能明显改善近距离精度。3.5 用 C 实现单目测距函数并做敏感性计算把上面的公式落成一个可复用的函数#include cmath struct CameraIntrinsics { double fx, fy, cx, cy; }; // pitch: 光轴与水平面夹角向下为正单位弧度 // v: 目标像素纵坐标图像原点在左上角 // h: 光心相对地面高度单位米 double PixelToGroundDistance(const CameraIntrinsics K, double pitch, double v, double h) { double alpha std::atan((v - K.cy) / K.fy); double beta pitch alpha; if (beta 0.02) { return -1.0; // 射线不指向地面返回负值表示无效 } return h / std::tan(beta); }beta 0.02是为了避免射线接近水平时算出极大的不可信距离阈值可以根据实际任务调整一般取1度左右。调用时如果吊舱协议给的是仰角向上为正要先取负再传入。这个函数只算水平距离NED坐标换算在下一章的类里完成。敏感性代码可以用几行循环打印不同 v 下的距离CameraIntrinsics K{2000.0, 2000.0, 960.0, 540.0}; double h 100.0; double pitch 11.0 * M_PI / 180.0; for (double v : {600.0, 700.0, 800.0}) { double D PixelToGroundDistance(K, pitch, v, h); std::printf(pixel v%.0f - D%.1f m\n, v, D); }这个例子中 v 越大目标在图像中越靠下测距结果越小符合观察直觉。你也可以把同样的像素偏移比如1个像素换成不同距离区间的 v观察距离跳变量从而预估当前吊舱在哪个距离上开始不可用。4. 目标定位 C 实现GimbalLocator 类、配置参数与常见坑4.1 项目结构与依赖OpenCV、Eigen、C17目标定位代码不需要复杂框架。常见的做法是单独抽一个GimbalLocator类声明和实现分离include/gimbal_locator.hpp放接口src/gimbal_locator.cpp放实现app/main_demo.cpp做一个最小可运行示例。构建用CMake依赖OpenCV矩阵、畸变矫正、projectPoints和Eigen旋转矩阵。CMakeLists.txt 可以这样写cmake_minimum_required(VERSION 3.16) project(gimbal_locator LANGUAGES CXX) set(CMAKE_CXX_STANDARD 17) find_package(OpenCV REQUIRED COMPONENTS core imgproc) find_package(Eigen3 REQUIRED) add_library(gimbal_locator src/gimbal_locator.cpp) target_include_directories(gimbal_locator PUBLIC include) target_link_libraries(gimbal_locator PRIVATE ${OpenCV_LIBS} Eigen3::Eigen)Eigen3只需要头文件编译很快。OpenCV建议保留imgproc后面做图像可视化调试要用。如果只做纯几何解算不用OpenCV也可以但项目里一旦涉及“把解算坐标画回图像”OpenCV就省不掉。4.2 GimbalLocator 接口设计输入像素与姿态输出 NED 坐标类的核心接口围绕“输入一帧图像中的目标像素 当前无人机和吊舱状态输出NED坐标”设计。头文件骨架#pragma once #include Eigen/Core #include Eigen/Geometry #include opencv2/core.hpp struct CameraIntrinsics { double fx, fy, cx, cy; }; enum class TargetAnchor { BottomCenter, // 目标框底边中点推荐 Center // 目标框中心需要目标高度修正 }; struct TargetResult { double north 0.0; // NED 北向米 double east 0.0; // NED 东向米 double down 0.0; // NED 地向地面通常为0 double range 0.0; // 从相机光心到目标的斜距米 }; class GimbalLocator { public: void SetCameraIntrinsics(const CameraIntrinsics K); // 吊舱输出相对地理北的方位角、相对水平面的俯仰角 void SetGimbalState(double yaw_deg, double pitch_deg); void SetUavPosition(const Eigen::Vector3d pos_ned); TargetResult LocateGroundTarget(const cv::Point2d pixel, TargetAnchor anchor, double target_height_m 0.0); };SetCameraIntrinsics在标定完成后调用一次SetGimbalState每帧更新SetUavPosition接收GPS/RTK解算出的NED位置。LocateGroundTarget是每帧的入口。如果要从经纬度输入可以在外层把经纬度转成NED常见做法是用起飞点作为原点用WGS84椭球把经纬度差转成米简单场景也可以用等距圆柱近似。4.3 LocateGroundTarget 完整实现先反投影、再求交实现时把像素坐标转成相机归一化方向向量再用吊舱姿态旋转到NED最后与目标高度平面求交#include gimbal_locator.hpp #include cmath void GimbalLocator::SetGimbalState(double yaw_deg, double pitch_deg) { double yaw yaw_deg * M_PI / 180.0; double pitch pitch_deg * M_PI / 180.0; // 吊舱坐标系到NED先方位角再俯仰角俯仰角向下为正旋转时取负 R_ned_gimbal_ (Eigen::AngleAxisd(yaw, Eigen::Vector3d::UnitZ()) * Eigen::AngleAxisd(-pitch, Eigen::Vector3d::UnitY())).toRotationMatrix(); } TargetResult GimbalLocator::LocateGroundTarget( const cv::Point2d pixel, TargetAnchor anchor, double target_height_m) { Eigen::Vector3d p_cam_norm; p_cam_norm (pixel.x - K_.cx) / K_.fx, (pixel.y - K_.cy) / K_.fy, 1.0; Eigen::Vector3d dir_ned R_ned_gimbal_ * p_cam_norm.normalized(); if (dir_ned.z() 0.0) { return TargetResult{}; // 射线不指向下方无法与地面求交 } // NED中 z 向下为正目标中心高度在地面以上所以是负值 double plane_z (anchor TargetAnchor::BottomCenter) ? 0.0 : -target_height_m * 0.5; double t (pos_ned_.z() - plane_z) / (-dir_ned.z()); TargetResult r; r.north pos_ned_.x() dir_ned.x() * t; r.east pos_ned_.y() dir_ned.y() * t; r.down plane_z; r.range t; return r; }dir_ned.z() 0的检查很关键很多无效像素会在这里被挡住。目标框中心点必须传target_height_m否则交点落在目标前面的地面上结果偏近。如果你的吊舱输出的是相对机体的方位角修改SetGimbalState把R_ned_body * R_body_gimbal换成R_ned_gimbal_即可具体公式在第2章已经给出。这个类的优势是像素、姿态、位置三者彻底解耦单测时可以直接喂固定值。4.4 参数配置与标定流程内参、外参、时间戳对齐参数配置最容易出问题的有三处。第一是相机内参用OpenCV棋盘格标定时建议收集20张以上不同角度图像重投影误差控制在0.15像素以内标定完把内参写进JSON文件。第二是吊舱安装角可以在无人机起飞前把吊舱对准正前方的一个已知地标用反投影残差估计安装角也可以用多次空中拍摄已知点做最小二乘求解标定结果应满足重投影误差小于2个像素。第三是时间戳对齐吊舱的图像、云台姿态、飞控GPS往往来自三个不同时钟云台快速转动时图像和姿态相差50毫秒就能造成几十米误差。常见做法是给每帧图像打上主机接收时间把吊舱姿态和GPS按时间戳做线性插值或样条插值再参与解算。姿态数据可以先用滑动窗口滤波去掉突变但滤波延迟必须小于100毫秒否则跟踪目标时会有明显拖尾。4.5 常见问题与排查现象、原因、对策下面三个问题在实飞中几乎一定会遇到按我的排查顺序列出来现象可能原因排查方法目标点始终偏北或偏南一个固定角度吊舱方位角零点偏移、磁偏角未扣除、安装角错误对正北地标检查对比吊舱方位角输出与真实方位角近距离准、远距离偏差越来越大测距模型 beta 符号错、高度用错、俯仰角时延大做第5章仿真回环先排除算法符号再看时延云台转动时结果剧烈跳动图像与姿态时间戳不同步记录两路时间戳打印差值做插值对齐横滚较大时结果左右偏滚转未参与旋转矩阵确认旋转矩阵包含 roll且符号与飞控一致排查时每步只改一个变量不要同时改符号和高度的基准否则很难定位问题。我习惯先在静态悬停时验证一个固定地标再稍微偏转云台观察解算目标是否保持在地标附近如果保持不住问题大概率在姿态变换如果保持得住再测不同距离。5. 精度验证与调试仿真回环、误差统计与 YOLO 目标点5.1 仿真回环从NED坐标反投影再正解算法写完先做仿真回环不要直接上飞机。思路是构造一个地面目标NED坐标用当前的无人机位置、吊舱姿态、相机内参通过cv::projectPoints把目标投影到图像得到像素再用这个像素调用LocateGroundTarget解算比较解算结果和真值。这个回环能在5分钟内发现旋转顺序、符号、高度基准三处最常见的bug。5.2 静态悬停误差统计CEP50 怎么算实飞验证时让无人机悬停吊舱固定指向目标点连续记录300帧解算坐标。用RTK或全站仪测出目标真值统计每个点的北向、东向误差画散点图并计算CEP50以真值为圆心覆盖50%误差点的圆半径就是CEP50。建议同时记录每个目标点的距离画出误差随距离变化的曲线这条曲线决定了这套吊舱单目目标定位算法在哪个距离内可用。5.3 把解算坐标投影回图像的闭环验证别看输出坐标直接把解算出的NED点用cv::projectPoints投影回图像看是否落在目标框底部。投影时把相机到NED的旋转矩阵转成rvec相机位置取吊舱光心在NED的坐标作为tveccv::Mat rvec, tvec; cv::Rodrigues(R_ned_cam_.t(), rvec); tvec -R_ned_cam_.t() * pos_ned_; // 相机系下的位置向量杆臂明显时替换为光心坐标 std::vectorcv::Point3d object_pts {cv::Point3d(r.north, r.east, r.down)}; std::vectorcv::Point2d image_pts; cv::projectPoints(object_pts, rvec, tvec, K_, dist_, image_pts);如果投影点与目标像素距离小于3个像素说明整条链自洽如果差异很大优先检查R_ned_cam_是否写成了转置。这是最快定位问题在旋转还是测距的手段。5.4 与 YOLO 等检测器对接时目标点的选择最后落到实际部署。检测器给的矩形框中心点不适合直接参与测距要把框底边中点作为目标锚点。YOLO系列输出里底边中点就是(x1x2)/2, y2不需要额外改模型。如果只有中心点可用就根据目标类别给一个先验身高或车高再传入LocateGroundTarget的高度参数。整个目标定位算法链路里这一步改动最小、收益最大把锚点从中心换成底边中点通常比调三个滤波参数都管用。本文还有配套的精品资源点击获取