无人机吊舱单目相机目标定位算法：坐标变换与测距的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_body

s是尺度因子，由第3章的测距模型求出；t_ned_body是无人机当前位置在NED下的坐标。实际代码里，第1行到第4行经常合并成一个单位方向向量，最后用射线与地平面求交替代显式计算s，这样更稳。

下面这张参数表建议直接做成配置文件字段：

表中每一项都可能因为协议差异改变符号，所以调试时要先把单点验证跑通，再上连续测试。

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 + alpha

alpha是目标射线相对光轴在垂直方向上的夹角，图像v轴向下为正。beta就是目标射线相对水平面的俯仰角。当beta <= 0时，射线指向上方，不会与地面相交，该像素点无效。当beta > 0时，水平距离为：

D = h / tan(beta)

这个 D 是目标在水平面上的投影到光心正下方投影点的距离，不是斜距。要得到NED下的北向和东向分量，需要再结合吊舱方位角 yaw：

north = 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、C++17

目标定位代码不需要复杂框架。常见的做法是单独抽一个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 常见问题与排查：现象、原因、对策

下面三个问题在实飞中几乎一定会遇到，按我的排查顺序列出来：

排查时每步只改一个变量，不要同时改符号和高度的基准，否则很难定位问题。我习惯先在静态悬停时验证一个固定地标，再稍微偏转云台，观察解算目标是否保持在地标附近；如果保持不住，问题大概率在姿态变换；如果保持得住，再测不同距离。

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的坐标作为tvec：

cv::Mat rvec, tvec; cv::Rodrigues(R_ned_cam_.t(), rvec); tvec = -R_ned_cam_.t() * pos_ned_; // 相机系下的位置向量，杆臂明显时替换为光心坐标 std::vector<cv::Point3d> object_pts = {cv::Point3d(r.north, r.east, r.down)}; std::vector<cv::Point2d> image_pts; cv::projectPoints(object_pts, rvec, tvec, K_, dist_, image_pts);

如果投影点与目标像素距离小于3个像素，说明整条链自洽；如果差异很大，优先检查R_ned_cam_是否写成了转置。这是最快定位问题在旋转还是测距的手段。

5.4 与 YOLO 等检测器对接时目标点的选择

最后落到实际部署。检测器给的矩形框中心点不适合直接参与测距，要把框底边中点作为目标锚点。YOLO系列输出里，底边中点就是(x1+x2)/2, y2，不需要额外改模型。如果只有中心点可用，就根据目标类别给一个先验身高或车高，再传入LocateGroundTarget的高度参数。整个目标定位算法链路里，这一步改动最小、收益最大：把锚点从中心换成底边中点，通常比调三个滤波参数都管用。

本文还有配套的精品资源，点击获取