无人机常用计算指标：已知两点经纬度，求解距离

创作时间:

作者:

@小白创作中心

无人机常用计算指标：已知两点经纬度，求解距离

引用

CSDN

https://blog.csdn.net/qq_44157835/article/details/140019181

在无人机应用中，计算两点间的距离是一个常见的需求。本文将详细介绍如何根据两点的经纬度坐标计算它们之间的距离，并提供具体的C++代码实现。

原理

如下一个半径为1 的圆，O是圆心，A、B是弦(chord)。角度AOB=theta。则角度AOC=theta/2。OC是垂直于AB的垂线(perpendicular)。AC长度是sin(theta/2)，AB长度是2*sin(theta/2)。

如下地球图所示，假设半径R为1，O是球心，A (lat1,lon1) 和 B (lat2,lon2) 是我们感兴趣的2个点。2跟经度线 lon1，lon2相交于北极(north pole)N。EF所在的线是赤道(equator)。ACBD是平面上的等腰梯形的四个顶点(vertice)。AC和DB的弦（直线）在图上没有画出。CD的位置是：C (lat2,lon1) and D (lat1,lon2)。角度AOC是A点与C点的纬度差 dlat。角度EOF是经度E点和经度F点的差dlon。

弦AC的长度，参照图1的方式，那么是AC=2sin(dlat/2)，弦BD也是一样的长度。E、F 2个点是赤道上的2个点，它们的纬度是0。EF的距离是EF=2sin(dlon/2)。A、D2个点所在的纬度是lat1。AD所在纬度的圆平面的半径是cos(lat1)。从A作一条垂线(perpendicular)到OE为AG，AO是球半径，则OG=cos(lat1)，即A、D所在纬度圆圈的半径（AO`）。

这时候，AD的弦长AD= 2*sin(dlon/2)cos(lat1)，类似的可以推出CB的长度= CB=2sin(dlon/2)*cos(lat2)。

下面看一下如何求AB的长度，回到平面等腰梯形，如下图：

AH是到CB的垂线(perpendicular)，CH= (CB-AD)/2。根据勾股定理(Pythagorean theorem): 【^2表示2的平方】

AH^2 = AC^2 - CH^2
= AC^2 - (CB-AD)^2/4

HB 的长度是HB=AD+CH = AD+(CB-AD)/2 = (CB+AD)/2，根据勾股定理得到：

AB^2 = AH^2 + HB^2
= AC^2 - (CB-AD)^2/4 + (CB+AD)^2/4
= AC^2 + CB*AD

根据前面球面上的求经纬距离的方式，我们已经得到 AC、AD和CB的长度，代入公式得到：

AB^2 = 4*(sin^2(dlat/2) + 4*cos(lat1)*cos(lat2)*sin^2(dlon/2))

假设中间值h 是AB长度一半的平方，如下

h = (AB/2)^2
= (sin^2(dlat/2)) + cos(lat1) * cos(lat2) * sin^2(dlon/2)

最后一步，是求得代表AB长度的角度AOB。参照图1的方式，我们可以知道

设AC=sqrt(a), 根据勾股定理(Pythagorean theorem)得到：oc = sqrt(oa^2 - ac^2) = sqrt(oa^2 - a) = sqrt(1-a).

如果设c是角AOB的度数值。

tan(<AOC) = tan(c)= AC/OC = sqrt(a)/sqrt(1-a)

则：

c = 2 * arctan(sqrt(a)/sqrt(1-a))，

最后的AB真实距离，把地球半径带上就可以了。

distance = 2 * EARTH_RADIUS * c。

代码

double deg2rad(double deg) {
    return deg * (std::acos(-1.0) / 180.0);
}

// 计算两点之间的地理距离（单位：公里）
double calculateDistance(double lon1, double lat1, double lon2, double lat2) {
    double R = 6371000.0; // 地球半径，单位：m
    double dLat = deg2rad(lat2 - lat1);
    double dLon = deg2rad(lon2 - lon1);

    double a = std::sin(dLat / 2) * std::sin(dLat / 2) +
               std::cos(deg2rad(lat1)) * std::cos(deg2rad(lat2)) *
               std::sin(dLon / 2) * std::sin(dLon / 2);
    double c = 2 * std::atan2(std::sqrt(a), std::sqrt(1 - a));
    double distance = R * c;

    return distance;
}