假设你现在的输入为合速度 V(m/s) 相对高度 H(m) 斜距 R(m) 方位角 az(度) 航捷 D(m)需要转换为方位角 az(度) 高低角 el(度) 斜距 R(m) 东向速度 ve(m/s) 天向速度 vu(m/s) 北向速度 vn(m/s) 文章摘要本文详细讲解了如何将雷达输入的无人机数据转换为标准的目标状态输出。核心转换涉及从输入参数合速度 V、相对高度 H、斜距 R、方位角 az、航捷 D到输出参数方位角 az、高低角 el、斜距 R、东向速度 ve、天向速度 vu、北向速度 vn的完整数学推导和工程实现。关键要点坐标系使用 ENU东-北-天坐标系方位角以北为 0°顺时针为正运动假设无人机默认水平运动天向速度 vu 0航捷定义航捷 D 表示目标水平航线到雷达的最短水平距离右侧为正左侧为负核心公式通过航捷比例 q D/Rh 和径向比例 c √(1 - q²) 计算速度方向完整实现提供了 Qt/C 的完整转换函数包含数据验证和错误处理转换流程计算水平距离 Rh √(R² - H²)计算高低角 el atan2(H, Rh)计算航捷比例 q D/Rh计算径向比例 c √(1 - q²)分解速度ve V × (-c × sin(az) q × cos(az)), vn V × (-c × cos(az) - q × sin(az)), vu 0文章包含详细的几何解释、公式推导、用例验证和完整的代码实现适合雷达数据处理和无人机跟踪系统的开发人员参考。1. 工程约定你现在已经明确了 4 个关键前提1. 无人机默认只做水平运动 天向速度 vu 0 2. 无人机默认正在接近雷达 速度的径向分量指向雷达站 3. 雷达图中北向为 0° 方位角 az 0° 表示目标在雷达正北方向 方位角 az 90° 表示目标在雷达正东方向 4. 航捷右边为正左边为负 D 0 表示目标从雷达右侧通过 D 0 表示目标从雷达左侧通过这里我采用的“右侧 / 左侧”定义是站在雷达站沿“雷达指向目标当前位置”的径向方向看出去 右边为正 左边为负。例如目标当前在雷达正北方az 0° 雷达看向目标的方向是向北。 此时右边是东左边是西。 D 0目标从雷达东侧通过 D 0目标从雷达西侧通过2. 坐标系统一为 ENU内部建议统一使用东北天坐标系EEast东向向东为正 NNorth北向向北为正 UUp天向向上为正方位角定义为az 0° 正北 az 90° 正东 az 180° 正南 az 270° 正西这个定义下目标当前位置的水平径向单位向量为er (sin az, cos az)其中er_E sin az er_N cos az例如az 0° er (0, 1) 目标在正北方向 az 90° er (1, 0) 目标在正东方向3. 由高度和斜距求高低角高度是相对雷达高度所以可以直接使用H 输入高度斜距为R 输入斜距水平距离为Rh sqrt(R² - H²)高低角为el asin(H / R)或者el atan2(H, Rh)工程上更推荐用el atan2(H, Rh)因为atan2在数值上更稳定也更符合“高度 / 水平距离”的几何含义。所以位置角度部分为输出方位角 输入方位角 az 输出高低角 atan2(H, Rh) 输出斜距 输入斜距 R4. 水平运动天向速度 vu 0你当前默认无人机水平运动因此vu 0此时输入的合速度V可以按水平速度处理Vh V也就是说V² ve² vn²不再考虑V² ve² vn² vu²5. 航捷的几何意义航捷D表示目标水平航线到雷达站点的最短水平距离。也就是D 目标航线在水平面内距离雷达最近时的距离带符号后D 0右侧通过 D 0左侧通过注意航捷不是角度也不是速度。它是一个水平距离约束。假设目标当前水平距离为Rh航捷为D则目标航线与雷达径向方向之间的偏角满足sin(beta) |D| / Rh其中beta速度方向相对“朝向雷达方向”的偏转角如果D 0说明目标正对雷达飞来。如果|D| 很小说明目标几乎正对雷达飞来只是略微偏一点。如果|D| 接近 Rh说明目标已经接近最近通过点此时速度方向几乎与雷达径向方向垂直。如果|D| Rh说明数据不合理。因为航线到雷达的最短距离不应该大于目标当前到雷达的水平距离。6. 航捷如何决定速度方向这是最关键的一步。6.1 径向方向 er雷达到目标当前位置的方向为er (sin az, cos az)它表示雷达站 - 目标例如目标在正北az 0° er (0, 1)6.2 接近雷达所以速度径向分量是 -er目标正在接近雷达说明速度的大方向应该指向雷达。而er是雷达 - 目标所以朝向雷达的方向就是-er例如目标在正北er (0, 1) -er (0, -1)这表示目标向南飞也就是接近雷达。6.3 右侧切向方向 et定义et为er的右侧方向。在 ENU 坐标下et (cos az, -sin az)例如az 0° er (0, 1) // 正北 et (1, 0) // 正东此时正东就是“看向正北时的右侧”。再例如az 90° er (1, 0) // 正东 et (0, -1) // 正南看向正东时右侧就是正南。6.4 航捷转成侧向比例 q定义q D / Rh其中D带符号航捷 Rh目标当前水平距离因为sin(beta) |D| / Rh所以q D / Rh可以理解为q 的绝对值速度方向侧向偏转比例 q 的正负号向右偏还是向左偏然后径向比例为c sqrt(1 - q²)其中c cos(beta)6.5 速度方向单位向量因为目标正在接近雷达所以径向分量为-c * er因为航捷右正左负所以侧向分量为q * et最终速度方向单位向量为v_dir -c * er q * et这就是航捷转换速度分量的核心公式。展开后v_dir_E -c * sin(az) q * cos(az) v_dir_N -c * cos(az) - q * sin(az)因为v_dir是单位向量所以最终速度分量为ve V * v_dir_E vn V * v_dir_N vu 0也就是ve V * (-c * sin(az) q * cos(az)) vn V * (-c * cos(az) - q * sin(az)) vu 07. 用例验证示例 1目标在正北右侧通过输入V 20 m/s H 0 m R 1000 m az 0° D 20 m计算Rh sqrt(1000² - 0²) 1000 m q D / Rh 20 / 1000 0.02 c sqrt(1 - q²) sqrt(1 - 0.0004) ≈ 0.9998方位角az 0° sin az 0 cos az 1速度ve 20 * (-0.9998 * 0 0.02 * 1) 0.4 m/s vn 20 * (-0.9998 * 1 - 0.02 * 0) -19.996 m/s vu 0结果ve ≈ 0.4 m/s vn ≈ -20 m/s vu 0含义目标在雷达正北方向 正在向南接近雷达 航捷为正表示从右侧通过 正北方向的右侧是东侧 所以东向速度 ve 为正。示例 2目标在正北左侧通过输入只改航捷D -20 m则q -0.02速度ve -0.4 m/s vn ≈ -19.996 m/s vu 0含义目标在雷达正北方向 正在向南接近雷达 航捷为负表示从左侧通过 正北方向的左侧是西侧 所以东向速度 ve 为负。示例 3目标在正东方右侧通过输入az 90° D 20 m目标在雷达正东方向。此时er (1, 0) // 雷达到目标正东 -et? 不需要 et (0, -1) // 看向正东时右侧是正南目标接近雷达径向方向应该向西。如果航捷为正表示从右侧通过也就是向南偏。所以速度结果应该是ve 0 vn 0也就是向西 向南这和公式是一致的。8. 完整转换流程输入V合速度 H相对高度 R斜距 az方位角北向 0°顺时针为正 D航捷右正左负转换步骤1. 检查 R 0 2. 检查 |H| R 3. 计算水平距离 Rh sqrt(R² - H²) 4. 计算高低角 el atan2(H, Rh) 5. 水平运动 vu 0 6. 检查 |D| Rh 7. 计算航捷比例 q D / Rh 8. 计算径向比例 c sqrt(1 - q²) 9. 计算径向单位向量 er (sin az, cos az) 10. 计算右侧切向单位向量 et (cos az, -sin az) 11. 因为目标接近雷达 v_dir -c * er q * et 12. 分解速度 ve V * v_dir_E vn V * v_dir_N vu 0 13. 输出 方位角 az 高低角 el 斜距 R 东向速度 ve 天向速度 vu 北向速度 vn9. Qt 转换函数代码下面代码按你的当前约定实现1. 高度是相对雷达高度 2. 无人机水平运动 3. 天向速度 vu 0 4. 目标默认接近雷达 5. 方位角北向为 0°顺时针为正 6. 航捷右侧为正左侧为负代码不依赖QVector3D只使用 QtCore/QtMath适合放在协议解析层、数据转换层或目标模型工具类里。#ifndefDRONE_CONVERT_UTILS_H#defineDRONE_CONVERT_UTILS_H#includeQtMath#includeQString#includealgorithm/** * brief 雷达原始输入数据 * * 当前协议输入含义 * speed : 合速度单位 m/s。当前版本默认目标水平运动因此这里按水平速度处理。 * height : 相对雷达高度单位 m。不是海拔高度。 * range : 斜距单位 m。 * azimuthDeg : 方位角单位度。北向为 0°顺时针为正。 * hangjie : 航捷单位 m。右侧为正左侧为负。 */structRadarDroneInput{doublespeed0.0;// 合速度 Vm/sdoubleheight0.0;// 相对雷达高度 Hmdoublerange0.0;// 斜距 RmdoubleazimuthDeg0.0;// 方位角 az度0°为北90°为东doublehangjie0.0;// 航捷 Dm右正左负};/** * brief 转换后的目标状态 * * 输出目标 * azimuthDeg : 方位角直接继承输入方位角。 * elevationDeg : 高低角由 height 和 range 计算得到。 * range : 斜距直接继承输入斜距。 * * east : 目标当前位置东向坐标 E单位 m。 * north : 目标当前位置北向坐标 N单位 m。 * up : 目标当前位置天向坐标 U单位 m。 * * ve : 东向速度单位 m/s。 * vn : 北向速度单位 m/s。 * vu : 天向速度单位 m/s。当前默认水平运动因此为 0。 */structDroneConvertedState{doubleazimuthDeg0.0;doubleelevationDeg0.0;doublerange0.0;doubleeast0.0;doublenorth0.0;doubleup0.0;doubleve0.0;doublevn0.0;doublevu0.0;};/** * brief 将数值限制到 [-1, 1]用于避免浮点误差导致 asin/sqrt 出现异常。 */staticinlinedoubleclampToUnit(doublevalue){if(value1.0){return1.0;}if(value-1.0){return-1.0;}returnvalue;}/** * brief 根据“合速度、高度、斜距、方位角、航捷”转换为 * “方位角、高低角、斜距、东向速度、天向速度、北向速度”。 * * 核心约定 * 1. 坐标系使用 ENU * E东向向东为正 * N北向向北为正 * U天向向上为正。 * * 2. 方位角 azimuthDeg * 北向为 0° * 东向为 90° * 顺时针为正。 * * 3. 高度 height * 是相对雷达高度不是海拔高度。 * * 4. 目标水平运动 * vu 0 * 输入 speed 按水平速度处理。 * * 5. 目标默认接近雷达 * 速度径向分量指向雷达站。 * * 6. 航捷 hangjie * 是目标水平航线到雷达站点的最短水平距离 * 右侧为正 * 左侧为负。 * * param input 输入雷达数据 * param output 输出转换后的目标状态 * param errorMessage 如果转换失败返回错误信息可以传 nullptr * return true 转换成功false 数据不合法 */staticinlineboolconvertRadarDroneInput(constRadarDroneInputinput,DroneConvertedStateoutput,QString*errorMessagenullptr){/* * * 1. 基本合法性检查 * */if(input.range0.0){if(errorMessage){*errorMessageQStringLiteral(斜距 range 必须大于 0);}returnfalse;}if(input.speed0.0){if(errorMessage){*errorMessageQStringLiteral(合速度 speed 不能为负数);}returnfalse;}/* * height 是相对雷达高度。 * * 几何上 * R² Rh² H² * * 所以必须满足 * |H| R * * 如果 |H| R说明高度差已经大于斜距数据不可能成立。 */if(qAbs(input.height)input.range){if(errorMessage){*errorMessageQStringLiteral(高度 height 的绝对值不能大于斜距 range);}returnfalse;}/* * * 2. 计算水平距离 Rh * * * R : 斜距 * H : 相对雷达高度 * Rh : 水平距离 * * Rh sqrt(R² - H²) */constdoublehorizontalRangeSquaredinput.range*input.range-input.height*input.height;constdoublehorizontalRangeqSqrt(std::max(0.0,horizontalRangeSquared));/* * 如果 horizontalRange 为 0说明目标几乎在雷达正上方或正下方。 * * 此时方位角在水平面上没有有效意义航捷也无法用于水平速度分解。 */if(horizontalRange0.0){if(errorMessage){*errorMessageQStringLiteral(水平距离为 0无法根据航捷分解水平速度);}returnfalse;}/* * * 3. 检查航捷是否合理 * * * 航捷 D 是目标水平航线到雷达的最短水平距离。 * * 对于当前目标位置来说 * |D| Rh * * 如果 |D| Rh说明“航线最近距离”比“当前水平距离”还大 * 这在几何上不成立。 */if(qAbs(input.hangjie)horizontalRange){if(errorMessage){*errorMessageQStringLiteral(航捷 hangjie 的绝对值不能大于当前水平距离);}returnfalse;}/* * * 4. 角度转换 * * * Qt 的三角函数使用弧度因此方位角需要由度转为弧度。 * * azimuthDeg: * 0° 北 * 90° 东 * 180° 南 * 270° 西 */constdoubleazimuthRadqDegreesToRadians(input.azimuthDeg);/* * * 5. 计算高低角 elevation * * * 高低角可以写成 * elevation asin(H / R) * * 也可以写成 * elevation atan2(H, Rh) * * 这里使用 atan2 * elevationRad atan2(相对高度, 水平距离) * * 好处 * 1. 几何意义直观 * 2. 数值稳定 * 3. 可以自然处理负高度。 */constdoubleelevationRadqAtan2(input.height,horizontalRange);constdoubleelevationDegqRadiansToDegrees(elevationRad);/* * * 6. 计算目标当前位置 ENU * * * 水平距离 Rh 已知方位角 az 已知。 * * 因为方位角是“北向为 0°顺时针为正”所以 * * E Rh * sin(az) * N Rh * cos(az) * U H * * 例如 * az 0° * E 0 * N Rh * 目标在正北 * * az 90° * E Rh * N 0 * 目标在正东 */constdoubleeasthorizontalRange*qSin(azimuthRad);constdoublenorthhorizontalRange*qCos(azimuthRad);constdoubleupinput.height;/* * * 7. 默认水平运动 * * * 当前约定 * 无人机只做水平运动。 * * 因此 * vu 0 * * 同时 * 输入合速度 speed 按水平速度处理。 */constdoublevu0.0;constdoublehorizontalSpeedinput.speed;/* * * 8. 航捷转换为速度方向偏转比例 * * * 这是整个转换的关键。 * * 航捷 D 表示目标水平航线到雷达站的最短水平距离。 * * 当前目标水平距离为 Rh。 * * 目标航线和雷达径向方向之间存在一个偏角 beta。 * * 几何关系 * * sin(beta) |D| / Rh * * 由于 D 是带符号的 * * D 0右侧通过 * D 0左侧通过 * * 因此定义 * * q D / Rh * * q 的意义 * 1. |q| sin(beta)表示侧向偏转比例 * 2. q 0向右侧偏 * 3. q 0向左侧偏。 * * 径向比例为 * * c cos(beta) sqrt(1 - q²) * * 因为目标默认接近雷达所以径向方向取负号 * * 径向分量 -c * er * * 航捷右正左负所以侧向分量为 * * 侧向分量 q * et * * 最终速度方向单位向量 * * v_dir -c * er q * et */constdoubleqinput.hangjie/horizontalRange;/* * q 理论上已经满足 [-1, 1]。 * 这里再 clamp 一下避免极小浮点误差导致 sqrt(1 - q²) 出现负数。 */constdoubleqSafeclampToUnit(q);/* * c 是径向方向的比例。 * * 如果 D 0 * q 0 * c 1 * 目标正对雷达飞来 * * 如果 |D| 接近 Rh * |q| 接近 1 * c 接近 0 * 目标接近最近通过点径向速度接近 0 */constdoublecqSqrt(std::max(0.0,1.0-qSafe*qSafe));/* * * 9. 计算径向单位向量 er * * * er 表示 * 雷达站 - 目标当前位置 * * 在 ENU 坐标下 * * er_E sin(az) * er_N cos(az) * * 示例 * az 0° * er (0, 1)指向北 * * az 90° * er (1, 0)指向东 */constdoubleerEqSin(azimuthRad);constdoubleerNqCos(azimuthRad);/* * * 10. 计算右侧切向单位向量 et * * * et 表示 * 沿 er 方向看出去时右手边的方向。 * * 在 ENU 坐标下 * * et_E cos(az) * et_N -sin(az) * * 示例 * az 0° * er (0, 1)看向北 * et (1, 0)右侧是东 * * az 90° * er (1, 0)看向东 * et (0, -1)右侧是南 */constdoubleetEqCos(azimuthRad);constdoubleetN-qSin(azimuthRad);/* * * 11. 根据“接近雷达 航捷正负”得到速度方向 * * * 目标接近雷达 * 速度径向分量指向雷达 * 即方向为 -er * * 航捷右正左负 * D 0速度带有 et 方向分量 * D 0速度带有 -et 方向分量 * * 所以 * * v_dir -c * er q * et * * 展开 * * v_dir_E -c * erE q * etE * v_dir_N -c * erN q * etN */constdoubledirE-c*erEqSafe*etE;constdoubledirN-c*erNqSafe*etN;/* * * 12. 合速度分解为东向、北向速度 * * * 因为 v_dir 是单位方向向量 * 所以 * * ve V * dirE * vn V * dirN * * 当前水平运动 * * vu 0 */constdoublevehorizontalSpeed*dirE;constdoublevnhorizontalSpeed*dirN;/* * * 13. 填充输出 * */output.azimuthDeginput.azimuthDeg;output.elevationDegelevationDeg;output.rangeinput.range;output.easteast;output.northnorth;output.upup;output.veve;output.vnvn;output.vuvu;returntrue;}#endif// DRONE_CONVERT_UTILS_H10. 调用示例RadarDroneInput input;input.speed20.0;// 合速度 20 m/sinput.height0.0;// 相对雷达高度 0 minput.range1000.0;// 斜距 1000 minput.azimuthDeg0.0;// 目标在正北方向input.hangjie20.0;// 航捷 20 m右侧通过DroneConvertedState output;QString error;if(convertRadarDroneInput(input,output,error)){qDebug()方位角:output.azimuthDeg;qDebug()高低角:output.elevationDeg;qDebug()斜距:output.range;qDebug()东向速度 ve:output.ve;qDebug()北向速度 vn:output.vn;qDebug()天向速度 vu:output.vu;qDebug()E:output.east;qDebug()N:output.north;qDebug()U:output.up;}else{qDebug()转换失败:error;}这个例子的输出大致为东向速度 ve ≈ 0.4 m/s 北向速度 vn ≈ -19.996 m/s 天向速度 vu 0 m/s含义是目标在正北方向 正在向南接近雷达 航捷为正表示右侧通过 正北方向的右侧是东侧 所以东向速度 ve 为正。11. 最终公式汇总输入V合速度 H相对雷达高度 R斜距 az方位角北向 0°顺时针为正 D航捷右正左负计算Rh sqrt(R² - H²) el atan2(H, Rh) q D / Rh c sqrt(1 - q²) ve V * (-c * sin(az) q * cos(az)) vn V * (-c * cos(az) - q * sin(az)) vu 0输出方位角 az 高低角 el 斜距 R 东向速度 ve 天向速度 0 北向速度 vn这套逻辑可以作为协议解析层的标准转换逻辑。