簡介面向無人機視覺開發(fā)者、吊艙算法工程師及目標定位方向學習者這份壓縮包圍繞“無人機吊艙單目相機目標定位”提供一套可運行、易擴展的C工程實現(xiàn)。工程采用模塊化結構含src、include、demo及CMakeLists構建配置并附帶使用說明文本便于快速理解相機成像模型、坐標系轉換與目標測距定位流程圖片資料展示了相機坐標系、圖像坐標系和歸一化平面坐標系等關鍵概念可輔助算法推演。壓縮包共15個文件以cpp/h源碼、txt說明、png示意圖為主另含json、in等配置文件整體僅179KB輕量實用。目前已有334人學習下載適合希望對照代碼掌握單目視覺定位原理并完成工程落地的讀者。1. 吊艙單目相機定位本質上是一場坐標系接力賽無人機吊艙掛一顆單目相機想實時報出地面目標的經緯度是光電吊艙最早接觸到的需求之一。很多第一次寫目標定位算法的工程師會栽在一個地方目標檢測模型跑通了像素坐標拿到了下一步卻不知道拿這個像素點怎么辦。單目相機只有一張圖像素坐標只能確定一條射線無法確定目標距離。工程里破局的辦法是把無人機位置、吊艙姿態(tài)角和地面假設串起來讓像素坐標經過相機系、吊艙系、機體系、導航系層層變換最后落到大地坐標。這條鏈路里最隱蔽的坑不是矩陣公式而是坐標系定義、旋轉順序、測距模型里高度和俯仰角的符號。下面按這條鏈路從原理講到可編譯的 C 代碼適合正在寫無人機目標定位、目標跟蹤或者要把視覺結果接入地面站顯示的工程師。2. 吊艙單目定位的坐標變換鏈從像素系到 NED 導航系2.1 四個關鍵坐標系像素、相機、機體、NED目標定位算法開始前先把坐標系約定寫死在注釋里。像素坐標系(u,v)原點在圖像左上角u向右、v向下相機坐標系原點在光心Z軸沿光軸向前X軸向右Y軸向下這與OpenCV約定一致。機體坐標系采用NED風格原點在飛控中心X軸機頭、Y軸右翼、Z軸向下。導航坐標系即NED或ENU但無人機領域絕大多數用NED原點是起飛點或RTK基準點。吊艙坐標系其實指穩(wěn)定平臺坐標系它與相機系之間往往只差一個固定外參因為吊艙出廠時會把相機安放在平臺坐標系的固定位置。要注意兩種常見歧義。吊艙輸出的“方位角”有的廠家是相對機頭有的是相對地理北甚至磁北俯仰角有的定義為光軸與水平面夾角有的是與機體Z軸夾角。拿到任何一份吊艙協(xié)議第一件事就是把角度參考系寫進配置文件否則后面所有坐標都是錯的。我一般會在代碼里加一個枚舉字段標明“方位角參考機頭 / 真北 / 磁北”。2.2 姿態(tài)角與旋轉矩陣先統(tǒng)一 YPR 旋轉順序無論用歐拉角還是四元數要先確定旋轉順序。很多飛控默認Z-Y-X也就是先偏航、再俯仰、最后橫滾。把歐拉角轉成旋轉矩陣時矩陣乘法順序寫反結果是整個坐標系的軸都錯位。以一個慣用約定為例R_ned_body Rz(yaw) * Ry(pitch) * Rx(roll)這里的寫法是列向量左乘向量從機體坐標系轉到NED。旋轉矩陣是正交矩陣反向變換直接轉置。歐拉角在俯仰接近±90°時有萬向鎖問題工程上建議用Eigen::Quaterniond保存姿態(tài)只在日志或調試時轉成歐拉角。不同飛控對姿態(tài)方向的定義可能有差異需要以飛控文檔為準不能只看矩陣形式。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單位米通常實測一次就能標定。現(xiàn)在很多吊艙在穩(wěn)定平臺內已經融合了自身姿態(tài)直接對外輸出“相對地理北的方位角 相對水平面的俯仰角”。這種情況不用再經過機體可以直接構造R_ned_gimbal見下節(jié)。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這樣更穩(wěn)。下面這張參數表建議直接做成配置文件字段參數含義來源單位fx, fy, cx, cy相機內參標定板標定像素dist鏡頭畸變系數標定-gimbal_yaw_offset吊艙相對機體航向安裝角廠測或自標定度gimbal_pitch_offset吊艙相對機體俯仰安裝角廠測或自標定度gimbal_yaw吊艙當前方位角吊艙串口/網絡協(xié)議度gimbal_pitch吊艙當前俯仰角吊艙串口/網絡協(xié)議度uav_yaw/pitch/roll無人機姿態(tài)飛控度或四元數uav_lat/lon/height無人機位置GPS/RTK度/米表中每一項都可能因為協(xié)議差異改變符號所以調試時要先把單點驗證跑通再上連續(xù)測試。2.5 用 Eigen 實現(xiàn)姿態(tài)矩陣與坐標變換的代碼骨架坐標變換代碼建議用 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連續(xù)相乘得到的就是矩陣乘積Rz * Ry * Rx順序是先偏航、再俯仰、最后橫滾。CameraRayToNed完成相機系到NED的方向旋轉平移項沒有包含因為計算單位方向向量時平移可以最后再加。注意這里的ray_cam不要求單位向量歸一化可以在求交時一起做避免多次歸一化引入誤差。3. 單目測距模型地平面假設下的目標距離計算與修正3.1 單目為什么測不準距離一條射線對應無窮多個點單目相機成像是一個降維過程像素坐標只能反過來確定一條射線射線上所有點都落在同一個像素上。沒有深度傳感器也沒有雙目視差就必須引入外部約束來固定射線上的點。無人機吊艙場景里最常見的約束是地平面假設目標在地面上或者已知目標相對地面高度就可以用光心高度、光軸俯仰角和像素位置算交點。這個假設在平坦地面、水面、機場等場景下非常有效山區(qū)、樓頂目標、地形起伏大的場景需要配合地表高程模型或目標高度先驗。在沒有已知地標的大范圍場景里這是唯一不需要額外傳感器就能定尺度的辦法。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再與地平面求交。實際吊艙有穩(wěn)定平臺滾轉通常很小但算法里保留旋轉矩陣路徑更通用。3.3 目標高度不為零底部中點優(yōu)先于中心點地面上的行人、車輛有實際高度。如果你用目標框中心點做測距等價于假設目標中心貼在地面上結果會系統(tǒng)性偏近。原因很簡單中心點對應的射線與地面交點比目標真實底部點要近。對于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米可見遠距離定位的誤差主要來自姿態(tài)角而不是圖像像素。像素誤差的影響由焦距決定長焦吊艙的像素角分辨率更高對測距更有利。因此實際項目中遠距離測距優(yōu)先用長焦并對吊艙俯仰角做低通濾波或與飛控IMU融合。像素層面用亞像素檢測或跟蹤濾波能明顯改善近距離精度。3.5 用 C 實現(xiàn)單目測距函數并做敏感性計算把上面的公式落成一個可復用的函數#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度左右。調用時如果吊艙協(xié)議給的是仰角向上為正要先取負再傳入。這個函數只算水平距離NED坐標換算在下一章的類里完成。敏感性代碼可以用幾行循環(huán)打印不同 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個像素換成不同距離區(qū)間的 v觀察距離跳變量從而預估當前吊艙在哪個距離上開始不可用。4. 目標定位 C 實現(xiàn)GimbalLocator 類、配置參數與常見坑4.1 項目結構與依賴OpenCV、Eigen、C17目標定位代碼不需要復雜框架。常見的做法是單獨抽一個GimbalLocator類聲明和實現(xiàn)分離include/gimbal_locator.hpp放接口src/gimbal_locator.cpp放實現(xiàn)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也可以但項目里一旦涉及“把解算坐標畫回圖像”O(jiān)penCV就省不掉。4.2 GimbalLocator 接口設計輸入像素與姿態(tài)輸出 NED 坐標類的核心接口圍繞“輸入一幀圖像中的目標像素 當前無人機和吊艙狀態(tài)輸出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 完整實現(xiàn)先反投影、再求交實現(xiàn)時把像素坐標轉成相機歸一化方向向量再用吊艙姿態(tài)旋轉到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章已經給出。這個類的優(yōu)勢是像素、姿態(tài)、位置三者徹底解耦單測時可以直接喂固定值。4.4 參數配置與標定流程內參、外參、時間戳對齊參數配置最容易出問題的有三處。第一是相機內參用OpenCV棋盤格標定時建議收集20張以上不同角度圖像重投影誤差控制在0.15像素以內標定完把內參寫進JSON文件。第二是吊艙安裝角可以在無人機起飛前把吊艙對準正前方的一個已知地標用反投影殘差估計安裝角也可以用多次空中拍攝已知點做最小二乘求解標定結果應滿足重投影誤差小于2個像素。第三是時間戳對齊吊艙的圖像、云臺姿態(tài)、飛控GPS往往來自三個不同時鐘云臺快速轉動時圖像和姿態(tài)相差50毫秒就能造成幾十米誤差。常見做法是給每幀圖像打上主機接收時間把吊艙姿態(tài)和GPS按時間戳做線性插值或樣條插值再參與解算。姿態(tài)數據可以先用滑動窗口濾波去掉突變但濾波延遲必須小于100毫秒否則跟蹤目標時會有明顯拖尾。4.5 常見問題與排查現(xiàn)象、原因、對策下面三個問題在實飛中幾乎一定會遇到按我的排查順序列出來現(xiàn)象可能原因排查方法目標點始終偏北或偏南一個固定角度吊艙方位角零點偏移、磁偏角未扣除、安裝角錯誤對正北地標檢查對比吊艙方位角輸出與真實方位角近距離準、遠距離偏差越來越大測距模型 beta 符號錯、高度用錯、俯仰角時延大做第5章仿真回環(huán)先排除算法符號再看時延云臺轉動時結果劇烈跳動圖像與姿態(tài)時間戳不同步記錄兩路時間戳打印差值做插值對齊橫滾較大時結果左右偏滾轉未參與旋轉矩陣確認旋轉矩陣包含 roll且符號與飛控一致排查時每步只改一個變量不要同時改符號和高度的基準否則很難定位問題。我習慣先在靜態(tài)懸停時驗證一個固定地標再稍微偏轉云臺觀察解算目標是否保持在地標附近如果保持不住問題大概率在姿態(tài)變換如果保持得住再測不同距離。5. 精度驗證與調試仿真回環(huán)、誤差統(tǒng)計與 YOLO 目標點5.1 仿真回環(huán)從NED坐標反投影再正解算法寫完先做仿真回環(huán)不要直接上飛機。思路是構造一個地面目標NED坐標用當前的無人機位置、吊艙姿態(tài)、相機內參通過cv::projectPoints把目標投影到圖像得到像素再用這個像素調用LocateGroundTarget解算比較解算結果和真值。這個回環(huán)能在5分鐘內發(fā)現(xiàn)旋轉順序、符號、高度基準三處最常見的bug。5.2 靜態(tài)懸停誤差統(tǒng)計CEP50 怎么算實飛驗證時讓無人機懸停吊艙固定指向目標點連續(xù)記錄300幀解算坐標。用RTK或全站儀測出目標真值統(tǒng)計每個點的北向、東向誤差畫散點圖并計算CEP50以真值為圓心覆蓋50%誤差點的圓半徑就是CEP50。建議同時記錄每個目標點的距離畫出誤差隨距離變化的曲線這條曲線決定了這套吊艙單目目標定位算法在哪個距離內可用。5.3 把解算坐標投影回圖像的閉環(huán)驗證別看輸出坐標直接把解算出的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個像素說明整條鏈自洽如果差異很大優(yōu)先檢查R_ned_cam_是否寫成了轉置。這是最快定位問題在旋轉還是測距的手段。5.4 與 YOLO 等檢測器對接時目標點的選擇最后落到實際部署。檢測器給的矩形框中心點不適合直接參與測距要把框底邊中點作為目標錨點。YOLO系列輸出里底邊中點就是(x1x2)/2, y2不需要額外改模型。如果只有中心點可用就根據目標類別給一個先驗身高或車高再傳入LocateGroundTarget的高度參數。整個目標定位算法鏈路里這一步改動最小、收益最大把錨點從中心換成底邊中點通常比調三個濾波參數都管用。本文還有配套的精品資源點擊獲取 無人機吊艙單目相機目標定位算法：坐標變換與測距的C++工程實踐
簡介面向無人機視覺開發(fā)者、吊艙算法工程師及目標定位方向學習者這份壓縮包圍繞“無人機吊艙單目相機目標定位”提供一套可運行、易擴展的C工程實現(xiàn)。工程采用模塊化結構含src、include、demo及CMakeLists構建配置并附帶使用說明文本便于快速理解相機成像模型、坐標系轉換與目標測距定位流程圖片資料展示了相機坐標系、圖像坐標系和歸一化平面坐標系等關鍵概念可輔助算法推演。壓縮包共15個文件以cpp/h源碼、txt說明、png示意圖為主另含json、in等配置文件整體僅179KB輕量實用。目前已有334人學習下載適合希望對照代碼掌握單目視覺定位原理并完成工程落地的讀者。1. 吊艙單目相機定位本質上是一場坐標系接力賽無人機吊艙掛一顆單目相機想實時報出地面目標的經緯度是光電吊艙最早接觸到的需求之一。很多第一次寫目標定位算法的工程師會栽在一個地方目標檢測模型跑通了像素坐標拿到了下一步卻不知道拿這個像素點怎么辦。單目相機只有一張圖像素坐標只能確定一條射線無法確定目標距離。工程里破局的辦法是把無人機位置、吊艙姿態(tài)角和地面假設串起來讓像素坐標經過相機系、吊艙系、機體系、導航系層層變換最后落到大地坐標。這條鏈路里最隱蔽的坑不是矩陣公式而是坐標系定義、旋轉順序、測距模型里高度和俯仰角的符號。下面按這條鏈路從原理講到可編譯的 C 代碼適合正在寫無人機目標定位、目標跟蹤或者要把視覺結果接入地面站顯示的工程師。2. 吊艙單目定位的坐標變換鏈從像素系到 NED 導航系2.1 四個關鍵坐標系像素、相機、機體、NED目標定位算法開始前先把坐標系約定寫死在注釋里。像素坐標系(u,v)原點在圖像左上角u向右、v向下相機坐標系原點在光心Z軸沿光軸向前X軸向右Y軸向下這與OpenCV約定一致。機體坐標系采用NED風格原點在飛控中心X軸機頭、Y軸右翼、Z軸向下。導航坐標系即NED或ENU但無人機領域絕大多數用NED原點是起飛點或RTK基準點。吊艙坐標系其實指穩(wěn)定平臺坐標系它與相機系之間往往只差一個固定外參因為吊艙出廠時會把相機安放在平臺坐標系的固定位置。要注意兩種常見歧義。吊艙輸出的“方位角”有的廠家是相對機頭有的是相對地理北甚至磁北俯仰角有的定義為光軸與水平面夾角有的是與機體Z軸夾角。拿到任何一份吊艙協(xié)議第一件事就是把角度參考系寫進配置文件否則后面所有坐標都是錯的。我一般會在代碼里加一個枚舉字段標明“方位角參考機頭 / 真北 / 磁北”。2.2 姿態(tài)角與旋轉矩陣先統(tǒng)一 YPR 旋轉順序無論用歐拉角還是四元數要先確定旋轉順序。很多飛控默認Z-Y-X也就是先偏航、再俯仰、最后橫滾。把歐拉角轉成旋轉矩陣時矩陣乘法順序寫反結果是整個坐標系的軸都錯位。以一個慣用約定為例R_ned_body Rz(yaw) * Ry(pitch) * Rx(roll)這里的寫法是列向量左乘向量從機體坐標系轉到NED。旋轉矩陣是正交矩陣反向變換直接轉置。歐拉角在俯仰接近±90°時有萬向鎖問題工程上建議用Eigen::Quaterniond保存姿態(tài)只在日志或調試時轉成歐拉角。不同飛控對姿態(tài)方向的定義可能有差異需要以飛控文檔為準不能只看矩陣形式。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單位米通常實測一次就能標定。現(xiàn)在很多吊艙在穩(wěn)定平臺內已經融合了自身姿態(tài)直接對外輸出“相對地理北的方位角 相對水平面的俯仰角”。這種情況不用再經過機體可以直接構造R_ned_gimbal見下節(jié)。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這樣更穩(wěn)。下面這張參數表建議直接做成配置文件字段參數含義來源單位fx, fy, cx, cy相機內參標定板標定像素dist鏡頭畸變系數標定-gimbal_yaw_offset吊艙相對機體航向安裝角廠測或自標定度gimbal_pitch_offset吊艙相對機體俯仰安裝角廠測或自標定度gimbal_yaw吊艙當前方位角吊艙串口/網絡協(xié)議度gimbal_pitch吊艙當前俯仰角吊艙串口/網絡協(xié)議度uav_yaw/pitch/roll無人機姿態(tài)飛控度或四元數uav_lat/lon/height無人機位置GPS/RTK度/米表中每一項都可能因為協(xié)議差異改變符號所以調試時要先把單點驗證跑通再上連續(xù)測試。2.5 用 Eigen 實現(xiàn)姿態(tài)矩陣與坐標變換的代碼骨架坐標變換代碼建議用 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連續(xù)相乘得到的就是矩陣乘積Rz * Ry * Rx順序是先偏航、再俯仰、最后橫滾。CameraRayToNed完成相機系到NED的方向旋轉平移項沒有包含因為計算單位方向向量時平移可以最后再加。注意這里的ray_cam不要求單位向量歸一化可以在求交時一起做避免多次歸一化引入誤差。3. 單目測距模型地平面假設下的目標距離計算與修正3.1 單目為什么測不準距離一條射線對應無窮多個點單目相機成像是一個降維過程像素坐標只能反過來確定一條射線射線上所有點都落在同一個像素上。沒有深度傳感器也沒有雙目視差就必須引入外部約束來固定射線上的點。無人機吊艙場景里最常見的約束是地平面假設目標在地面上或者已知目標相對地面高度就可以用光心高度、光軸俯仰角和像素位置算交點。這個假設在平坦地面、水面、機場等場景下非常有效山區(qū)、樓頂目標、地形起伏大的場景需要配合地表高程模型或目標高度先驗。在沒有已知地標的大范圍場景里這是唯一不需要額外傳感器就能定尺度的辦法。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再與地平面求交。實際吊艙有穩(wěn)定平臺滾轉通常很小但算法里保留旋轉矩陣路徑更通用。3.3 目標高度不為零底部中點優(yōu)先于中心點地面上的行人、車輛有實際高度。如果你用目標框中心點做測距等價于假設目標中心貼在地面上結果會系統(tǒng)性偏近。原因很簡單中心點對應的射線與地面交點比目標真實底部點要近。對于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米可見遠距離定位的誤差主要來自姿態(tài)角而不是圖像像素。像素誤差的影響由焦距決定長焦吊艙的像素角分辨率更高對測距更有利。因此實際項目中遠距離測距優(yōu)先用長焦并對吊艙俯仰角做低通濾波或與飛控IMU融合。像素層面用亞像素檢測或跟蹤濾波能明顯改善近距離精度。3.5 用 C 實現(xiàn)單目測距函數并做敏感性計算把上面的公式落成一個可復用的函數#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度左右。調用時如果吊艙協(xié)議給的是仰角向上為正要先取負再傳入。這個函數只算水平距離NED坐標換算在下一章的類里完成。敏感性代碼可以用幾行循環(huán)打印不同 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個像素換成不同距離區(qū)間的 v觀察距離跳變量從而預估當前吊艙在哪個距離上開始不可用。4. 目標定位 C 實現(xiàn)GimbalLocator 類、配置參數與常見坑4.1 項目結構與依賴OpenCV、Eigen、C17目標定位代碼不需要復雜框架。常見的做法是單獨抽一個GimbalLocator類聲明和實現(xiàn)分離include/gimbal_locator.hpp放接口src/gimbal_locator.cpp放實現(xiàn)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也可以但項目里一旦涉及“把解算坐標畫回圖像”O(jiān)penCV就省不掉。4.2 GimbalLocator 接口設計輸入像素與姿態(tài)輸出 NED 坐標類的核心接口圍繞“輸入一幀圖像中的目標像素 當前無人機和吊艙狀態(tài)輸出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 完整實現(xiàn)先反投影、再求交實現(xiàn)時把像素坐標轉成相機歸一化方向向量再用吊艙姿態(tài)旋轉到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章已經給出。這個類的優(yōu)勢是像素、姿態(tài)、位置三者徹底解耦單測時可以直接喂固定值。4.4 參數配置與標定流程內參、外參、時間戳對齊參數配置最容易出問題的有三處。第一是相機內參用OpenCV棋盤格標定時建議收集20張以上不同角度圖像重投影誤差控制在0.15像素以內標定完把內參寫進JSON文件。第二是吊艙安裝角可以在無人機起飛前把吊艙對準正前方的一個已知地標用反投影殘差估計安裝角也可以用多次空中拍攝已知點做最小二乘求解標定結果應滿足重投影誤差小于2個像素。第三是時間戳對齊吊艙的圖像、云臺姿態(tài)、飛控GPS往往來自三個不同時鐘云臺快速轉動時圖像和姿態(tài)相差50毫秒就能造成幾十米誤差。常見做法是給每幀圖像打上主機接收時間把吊艙姿態(tài)和GPS按時間戳做線性插值或樣條插值再參與解算。姿態(tài)數據可以先用滑動窗口濾波去掉突變但濾波延遲必須小于100毫秒否則跟蹤目標時會有明顯拖尾。4.5 常見問題與排查現(xiàn)象、原因、對策下面三個問題在實飛中幾乎一定會遇到按我的排查順序列出來現(xiàn)象可能原因排查方法目標點始終偏北或偏南一個固定角度吊艙方位角零點偏移、磁偏角未扣除、安裝角錯誤對正北地標檢查對比吊艙方位角輸出與真實方位角近距離準、遠距離偏差越來越大測距模型 beta 符號錯、高度用錯、俯仰角時延大做第5章仿真回環(huán)先排除算法符號再看時延云臺轉動時結果劇烈跳動圖像與姿態(tài)時間戳不同步記錄兩路時間戳打印差值做插值對齊橫滾較大時結果左右偏滾轉未參與旋轉矩陣確認旋轉矩陣包含 roll且符號與飛控一致排查時每步只改一個變量不要同時改符號和高度的基準否則很難定位問題。我習慣先在靜態(tài)懸停時驗證一個固定地標再稍微偏轉云臺觀察解算目標是否保持在地標附近如果保持不住問題大概率在姿態(tài)變換如果保持得住再測不同距離。5. 精度驗證與調試仿真回環(huán)、誤差統(tǒng)計與 YOLO 目標點5.1 仿真回環(huán)從NED坐標反投影再正解算法寫完先做仿真回環(huán)不要直接上飛機。思路是構造一個地面目標NED坐標用當前的無人機位置、吊艙姿態(tài)、相機內參通過cv::projectPoints把目標投影到圖像得到像素再用這個像素調用LocateGroundTarget解算比較解算結果和真值。這個回環(huán)能在5分鐘內發(fā)現(xiàn)旋轉順序、符號、高度基準三處最常見的bug。5.2 靜態(tài)懸停誤差統(tǒng)計CEP50 怎么算實飛驗證時讓無人機懸停吊艙固定指向目標點連續(xù)記錄300幀解算坐標。用RTK或全站儀測出目標真值統(tǒng)計每個點的北向、東向誤差畫散點圖并計算CEP50以真值為圓心覆蓋50%誤差點的圓半徑就是CEP50。建議同時記錄每個目標點的距離畫出誤差隨距離變化的曲線這條曲線決定了這套吊艙單目目標定位算法在哪個距離內可用。5.3 把解算坐標投影回圖像的閉環(huán)驗證別看輸出坐標直接把解算出的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個像素說明整條鏈自洽如果差異很大優(yōu)先檢查R_ned_cam_是否寫成了轉置。這是最快定位問題在旋轉還是測距的手段。5.4 與 YOLO 等檢測器對接時目標點的選擇最后落到實際部署。檢測器給的矩形框中心點不適合直接參與測距要把框底邊中點作為目標錨點。YOLO系列輸出里底邊中點就是(x1x2)/2, y2不需要額外改模型。如果只有中心點可用就根據目標類別給一個先驗身高或車高再傳入LocateGroundTarget的高度參數。整個目標定位算法鏈路里這一步改動最小、收益最大把錨點從中心換成底邊中點通常比調三個濾波參數都管用。本文還有配套的精品資源點擊獲取