无人机合速度和航捷转速度分量

假设你现在的输入为：

合速度 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）的完整数学推导和工程实现。

关键要点：

1. 坐标系：使用 ENU（东-北-天）坐标系，方位角以北为 0°，顺时针为正
2. 运动假设：无人机默认水平运动，天向速度 vu = 0
3. 航捷定义：航捷 D 表示目标水平航线到雷达的最短水平距离，右侧为正，左侧为负
4. 核心公式：通过航捷比例 q = D/Rh 和径向比例 c = √(1 - q²) 计算速度方向
5. 完整实现：提供了 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

内部建议统一使用东北天坐标系：

E：East，东向，向东为正
N：North，北向，向北为正
U：Up，天向，向上为正

方位角定义为：

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)
输出斜距   = 输入斜距 R

---

4. 水平运动：天向速度 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 = 0

---

7. 用例验证

示例 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
    北向速度 vn

---

9. Qt 转换函数代码

下面代码按你的当前约定实现：

1. 高度是相对雷达高度
2. 无人机水平运动
3. 天向速度 vu = 0
4. 目标默认接近雷达
5. 方位角北向为 0°，顺时针为正
6. 航捷右侧为正，左侧为负

代码不依赖 QVector3D，只使用 QtCore/QtMath，适合放在协议解析层、数据转换层或目标模型工具类里。

#ifndef DRONE_CONVERT_UTILS_H
#define DRONE_CONVERT_UTILS_H

#include <QtMath>
#include <QString>
#include <algorithm>

/**
 * @brief 雷达原始输入数据
 *
 * 当前协议输入含义：
 * speed       : 合速度，单位 m/s。当前版本默认目标水平运动，因此这里按水平速度处理。
 * height      : 相对雷达高度，单位 m。不是海拔高度。
 * range       : 斜距，单位 m。
 * azimuthDeg  : 方位角，单位度。北向为 0°，顺时针为正。
 * hangjie     : 航捷，单位 m。右侧为正，左侧为负。
 */
struct RadarDroneInput
{
    double speed = 0.0;       // 合速度 V，m/s
    double height = 0.0;      // 相对雷达高度 H，m
    double range = 0.0;       // 斜距 R，m
    double azimuthDeg = 0.0;  // 方位角 az，度，0°为北，90°为东
    double hangjie = 0.0;     // 航捷 D，m，右正左负
};

/**
 * @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。
 */
struct DroneConvertedState
{
    double azimuthDeg = 0.0;
    double elevationDeg = 0.0;
    double range = 0.0;

    double east = 0.0;
    double north = 0.0;
    double up = 0.0;

    double ve = 0.0;
    double vn = 0.0;
    double vu = 0.0;
};

/**
 * @brief 将数值限制到 [-1, 1]，用于避免浮点误差导致 asin/sqrt 出现异常。
 */
static inline double clampToUnit(double value)
{
    if (value > 1.0) {
        return 1.0;
    }

    if (value < -1.0) {
        return -1.0;
    }

    return value;
}

/**
 * @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 数据不合法
 */
static inline bool convertRadarDroneInput(const RadarDroneInput &input,
                                          DroneConvertedState &output,
                                          QString *errorMessage = nullptr)
{
    /*
     * =========================
     * 1. 基本合法性检查
     * =========================
     */

    if (input.range <= 0.0) {
        if (errorMessage) {
            *errorMessage = QStringLiteral("斜距 range 必须大于 0");
        }
        return false;
    }

    if (input.speed < 0.0) {
        if (errorMessage) {
            *errorMessage = QStringLiteral("合速度 speed 不能为负数");
        }
        return false;
    }

    /*
     * height 是相对雷达高度。
     *
     * 几何上：
     *     R² = Rh² + H²
     *
     * 所以必须满足：
     *     |H| <= R
     *
     * 如果 |H| > R，说明高度差已经大于斜距，数据不可能成立。
     */
    if (qAbs(input.height) > input.range) {
        if (errorMessage) {
            *errorMessage = QStringLiteral("高度 height 的绝对值不能大于斜距 range");
        }
        return false;
    }

    /*
     * =========================
     * 2. 计算水平距离 Rh
     * =========================
     *
     * R  : 斜距
     * H  : 相对雷达高度
     * Rh : 水平距离
     *
     * Rh = sqrt(R² - H²)
     */
    const double horizontalRangeSquared =
            input.range * input.range - input.height * input.height;

    const double horizontalRange =
            qSqrt(std::max(0.0, horizontalRangeSquared));

    /*
     * 如果 horizontalRange 为 0，说明目标几乎在雷达正上方或正下方。
     *
     * 此时方位角在水平面上没有有效意义，航捷也无法用于水平速度分解。
     */
    if (horizontalRange <= 0.0) {
        if (errorMessage) {
            *errorMessage = QStringLiteral("水平距离为 0，无法根据航捷分解水平速度");
        }
        return false;
    }

    /*
     * =========================
     * 3. 检查航捷是否合理
     * =========================
     *
     * 航捷 D 是目标水平航线到雷达的最短水平距离。
     *
     * 对于当前目标位置来说：
     *     |D| <= Rh
     *
     * 如果 |D| > Rh，说明"航线最近距离"比"当前水平距离"还大，
     * 这在几何上不成立。
     */
    if (qAbs(input.hangjie) > horizontalRange) {
        if (errorMessage) {
            *errorMessage = QStringLiteral("航捷 hangjie 的绝对值不能大于当前水平距离");
        }
        return false;
    }

    /*
     * =========================
     * 4. 角度转换
     * =========================
     *
     * Qt 的三角函数使用弧度，因此方位角需要由度转为弧度。
     *
     * azimuthDeg:
     *     0°   = 北
     *     90°  = 东
     *     180° = 南
     *     270° = 西
     */
    const double azimuthRad = qDegreesToRadians(input.azimuthDeg);

    /*
     * =========================
     * 5. 计算高低角 elevation
     * =========================
     *
     * 高低角可以写成：
     *     elevation = asin(H / R)
     *
     * 也可以写成：
     *     elevation = atan2(H, Rh)
     *
     * 这里使用 atan2：
     *     elevationRad = atan2(相对高度, 水平距离)
     *
     * 好处：
     *     1. 几何意义直观；
     *     2. 数值稳定；
     *     3. 可以自然处理负高度。
     */
    const double elevationRad = qAtan2(input.height, horizontalRange);
    const double elevationDeg = qRadiansToDegrees(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
     *         目标在正东
     */
    const double east = horizontalRange * qSin(azimuthRad);
    const double north = horizontalRange * qCos(azimuthRad);
    const double up = input.height;

    /*
     * =========================
     * 7. 默认水平运动
     * =========================
     *
     * 当前约定：
     *     无人机只做水平运动。
     *
     * 因此：
     *     vu = 0
     *
     * 同时：
     *     输入合速度 speed 按水平速度处理。
     */
    const double vu = 0.0;
    const double horizontalSpeed = input.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
     */
    const double q = input.hangjie / horizontalRange;

    /*
     * q 理论上已经满足 [-1, 1]。
     * 这里再 clamp 一下，避免极小浮点误差导致 sqrt(1 - q²) 出现负数。
     */
    const double qSafe = clampToUnit(q);

    /*
     * c 是径向方向的比例。
     *
     * 如果 D = 0：
     *     q = 0
     *     c = 1
     *     目标正对雷达飞来
     *
     * 如果 |D| 接近 Rh：
     *     |q| 接近 1
     *     c 接近 0
     *     目标接近最近通过点，径向速度接近 0
     */
    const double c = qSqrt(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)，指向东
     */
    const double erE = qSin(azimuthRad);
    const double erN = qCos(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)，右侧是南
     */
    const double etE = qCos(azimuthRad);
    const double etN = -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
     */
    const double dirE = -c * erE + qSafe * etE;
    const double dirN = -c * erN + qSafe * etN;

    /*
     * =========================
     * 12. 合速度分解为东向、北向速度
     * =========================
     *
     * 因为 v_dir 是单位方向向量，
     * 所以：
     *
     *     ve = V * dirE
     *     vn = V * dirN
     *
     * 当前水平运动：
     *
     *     vu = 0
     */
    const double ve = horizontalSpeed * dirE;
    const double vn = horizontalSpeed * dirN;

    /*
     * =========================
     * 13. 填充输出
     * =========================
     */
    output.azimuthDeg = input.azimuthDeg;
    output.elevationDeg = elevationDeg;
    output.range = input.range;

    output.east = east;
    output.north = north;
    output.up = up;

    output.ve = ve;
    output.vn = vn;
    output.vu = vu;

    return true;
}

#endif // DRONE_CONVERT_UTILS_H

---

10. 调用示例

RadarDroneInput input;
input.speed = 20.0;       // 合速度 20 m/s
input.height = 0.0;       // 相对雷达高度 0 m
input.range = 1000.0;     // 斜距 1000 m
input.azimuthDeg = 0.0;   // 目标在正北方向
input.hangjie = 20.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

这套逻辑可以作为协议解析层的标准转换逻辑。