GPS坐标夹角计算

常飞无人机的需求里有时候会有一个需求，知道无人机当前坐标点，知道一个兴趣点，想要飞机在飞行过程中始终朝向兴趣点，当然大疆的waypoint里自带添加兴趣点，但是我们有时候不执行航线，只执行一个旋转，那么这个旋转角度就需要自己计算了。

一、计算原理：（想要方法直接看第二个）

要计算无人机从当前位置调整机头指向特定目标点的角度，你可以使用以下方法来计算两点间的方位角。这里提供了一个基于地球坐标系（经纬度）的简单算法：
1、定义坐标点：假设你有无人机当前的GPS坐标（纬度1, 经度1）和目标点的GPS 坐标（纬度2, 经度2）。
2、计算经纬度差异：计算两点间的经度差和纬度差。
Δ经度 = 经度2 – 经度1
Δ纬度 = 纬度2 – 纬度1
3、计算方位角：使用以下公式计算方位角（以弧度表示）：
θ=arctan2(Δ经度,Δ纬度)
这里使用的 arctan⁡2arctan2 函数能够根据经度差和纬度差的符号返回正确的角度（从北方向顺时针测量），其值域为 [−π,π]。
4、转换角度为度：
角度=θ×(π180​)
如果需要角度值在 00 至 360360 度之间，可以按需调整：
角度=(角度+360)%360
5、调整无人机机头朝向：根据计算出的角度调整无人机机头朝向。如果你有无人机的当前朝向，你还需要计算从当前朝向转向目标朝向需要旋转的角度。
这是一个理论上的计算，实际应用中可能需要考虑其他因素，例如地形、无人机的飞行性能等。

二、实现方法（这里以飞机坐标点为原点，正北为0度）

public class DroneNavigation {

    /**
     * 计算从无人机当前位置到目标位置的朝向角度。
     *
     * @param lat1 无人机当前位置纬度
     * @param lon1 无人机当前位置经度
     * @param lat2 目标位置纬度
     * @param lon2 目标位置经度
     * @return 从正北方向顺时针到目标方向的角度
     */
    public static double calculateBearing(double lat1, double lon1, double lat2, double lon2) {
        // 将角度转换为弧度
        double latitude1 = Math.toRadians(lat1);
        double longitude1 = Math.toRadians(lon1);
        double latitude2 = Math.toRadians(lat2);
        double longitude2 = Math.toRadians(lon2);

        // 计算经度差
        double deltaLongitude = longitude2 - longitude1;

        // 计算X, Y组件
        double y = Math.sin(deltaLongitude) * Math.cos(latitude2);
        double x = Math.cos(latitude1) * Math.sin(latitude2) - Math.sin(latitude1) * Math.cos(latitude2) * Math.cos(deltaLongitude);

        // 计算初始方位角
        double bearingRadians = Math.atan2(y, x);

        // 将弧度转换为角度
        double bearingDegrees = Math.toDegrees(bearingRadians);

        // 确保角度为正数
        double bearing = (bearingDegrees + 360) % 360;

        return bearing;
    }

    public static void main(String[] args) {
        // 示例坐标
        double lat1 = 39.90921; // 无人机当前纬度
        double lon1 = 116.397434; // 无人机当前经度
        double lat2 = 39.910247; // 目标纬度
        double lon2 = 116.398094; // 目标经度

        double bearing = calculateBearing(lat1, lon1, lat2, lon2);
        System.out.println("无人机需要旋转的角度: " + bearing + "度");
    }
}

上面用坐标拾取器，采集了几个点测试后，精度挺不错。

自己的站里欢迎大家光顾：GPS坐标夹角计算 – 付的技术小窝