球面几何计算：基于经纬度的距离（Great-Circle）推导

背景

最近在做一个基于当前 GPS 坐标，通过输入目标经纬度来计算相对位置与距离的硬件装置。

在查阅相关算法时，我发现网上的推导过程大多较为碎片化或直接给出结论。为了彻底理解其底层的数学逻辑，我决定从零开始重新推导一遍，并将其系统地记录下来。

本文将完整呈现这一推导过程：从建立三维空间直角坐标系开始，利用向量点积导出球面余弦定律（Law of Cosines）。此外，我们还会深入探讨该公式在短距离场景下产生“精度丢失”的物理本质，以及如何利用半角恒等式引入 Haversine 公式 来修正误差。最后，我将分享如何通过数学化简，精简公式降低运算量，从而实现更高的运算效能。

简化模型(平面模型)

我们在高中数学中学习过欧几里得距离公式， 用于计算平面上两点之间的距离.
同样的，当地球表面两点距离相对较近时， 我们可以将这一局部地区视为一个平面， 此时直接使用欧几里得距离公式即可

$$L = \sqrt{(x_b - x_a)^2 + (y_b - y_a)^2}$$

但是当地球表面两点距离相对较远时， 我们就需要考虑地球的曲率了.下面将介绍如何计算球面两点之间的距离。

球面模型

真实的地球是椭球体， 考虑地球的曲率会增加计算的复杂度。
在大多数情况下， 我们可以将地球简化为球体， 得到如下图所示的几何模型。

图 1：基于球面三角学的方位角计算几何模型

其中，$O$ 为球心，$A$ 和 $B$ 为球面上任意两点，$j_a， w_a$ 和 $j_b， w_b$ 分别为两点的坐标。

计算任意两点间的距离

要在球面上精确锁定 $A(j_a， w_a)$ 与 $B(j_b， w_b)$ 两点间的路径，我们追求的是大圆距离 (Great Circle Distance)，即球面上最短的弧长。

从几何模型来看，这段距离本质上是球心角 $\angle AOB$ 所截得的弧长： $$L = R \cdot \theta$$ （其中 $R$ 为地球半径，$\theta$ 为以弧度计的夹角 $\angle AOB$）。

具体的求解思路如下：

三维坐标转换：先通过经纬度将 $A$、$B$ 两点转换至空间直角坐标系。
计算弦长：求出三维空间中直线段 $|AB|$ 的长度（即三角形 $AOB$ 的底边）。
反求夹角：利用余弦定理 (Law of Cosines)，根据三边长度反推球心角$\angle AOB$ (以下记作$\theta$)，进而锁定最终的球面距离。

$$ A = \left\{ \begin{aligned} x_a &= R \cos w_a \cos j_a \\ y_a &= R \cos w_a \sin j_a \\ z_a &= R \sin w_a \end{aligned} \right. $$

$$ B = \left\{ \begin{aligned} x_b &= R \cos w_b \cos j_b \\ y_b &= R \cos w_b \sin j_b \\ z_b &= R \sin w_b \end{aligned} \right. $$

可得AB两点之间的直线距离为
$$|AB| = \sqrt{(x_a - x_b)^2 + (y_a - y_b)^2 + (z_a - z_b)^2}$$
根据余弦定理求出$\angle AOB$
$$|AB|^2 = OA^2 + OB^2 - 2 \cdot OA \cdot OB \cdot \cos\theta$$
代入半径 $R$ 后的形式
$$|AB|^2 = R^2 + R^2 - 2 \cdot R \cdot R \cdot \cos\theta$$
反求 $\cos\theta$ 的公式
$$\cos\theta = \frac{2R^2 - |AB|^2}{2R^2}$$
根据弧长公式
$$L = R \cdot \theta$$
带入可得
$$弧长AB=R \cdot arccos(\frac{2R^2 - |AB|^2}{2R^2})$$
前面已经推导过$|AB|$的公式， 那么
$$弧长AB=R \cdot arccos(\frac{2R^2 - (x_a - x_b)^2 - (y_a - y_b)^2 - (z_a - z_b)^2}{2R^2})$$
又因为 $x_a^2 + y_a^2 + z_a^2 = R^2$ 且 $x_b^2 + y_b^2 + z_b^2 = R^2$，分子部分可以展开并简化为：
$$
2R^2 - (x_a^2 - 2x_ax_b + x_b^2) - (y_a^2 - 2y_ay_b + y_b^2) - (z_a^2 - 2z_az_b + z_b^2)
$$
$$
= 2R^2 - (x_a^2 + y_a^2 + z_a^2) + (x_b^2 + y_b^2 + z_b^2) + 2(x_ax_b + y_ay_b + z_az_b)
$$
$$
= 2(x_ax_b + y_ay_b + z_az_b)
$$
代入原公式
$$
弧长AB=R \cdot arccos(\frac{2(x_ax_b + y_ay_b + z_az_b)}{2R^2})
$$
$$
弧长AB=R \cdot arccos(\frac{x_ax_b + y_ay_b + z_az_b}{R^2})
$$
到这里， 可以带入上面参数方程中的$x_a， y_a， z_a， x_b， y_b， z_b$

代入后得到
$$
弧长AB=R \cdot arccos(\frac{R^2 \cos w_a \cos j_a \cos w_b \cos j_b + R^2 \cos w_a \sin j_a \cos w_b \sin j_b + R^2 \sin w_a \sin w_b}{R^2})
$$
$$
弧长AB=R \cdot arccos(\cos w_a \cos j_a \cos w_b \cos j_b + \cos w_a \sin j_a \cos w_b \sin j_b + \sin w_a \sin w_b)
$$

提取 $\cos w_a \cos w_b$，得到$$\cos w_a \cos w_b (\cos j_a \cos j_b + \sin j_a \sin j_b) + \sin w_a \sin w_b$$
利用公式
$$\cos(j_b - j_a) = \cos j_a \cos j_b + \sin j_a \sin j_b$$

括号内的部分可以被替换为经度差的余弦：$$\cos w_a \cos w_b \cos(j_b - j_a) + \sin w_a \sin w_b$$

最终公式
$$L = R \cdot \arccos(\cos w_a \cos w_b \cos(j_b - j_a) + \sin w_a \sin w_b)$$

这个公式就是著名的球面余弦定律 (Law of Cosines)。化简后的形式逻辑清晰，运算量从推导过程中的 9 次三角函数精简到了 6 次，非常适合嵌入式 MCU 运行。

踩坑笔记：数值稳定性的陷阱 (Numerical Stability)

虽然这个公式在数学上严丝合缝，但在计算机的浮点运算世界里，它存在一个致命的“短板”：两点距离极近（如小于几十米）时，精度会产生剧烈波动甚至完全失效。

究其原因，这主要源于计算机处理数字的物理局限：

1. 有效精度被“挤”出去了：当 A、B 两点靠得非常近时，球心角 $\theta$ 趋于 $0$，此时 $\cos \theta$ 的值会无限趋近于 $1$。由于双精度浮点数 (double) 的精度上限仅有 15-17 位，用“1”去承载一个极其微小的偏移量时，代表精确偏移量的最后几位有效数字会被直接截断。
2. 反余弦函数的“死亡陡坡”：观察 $y = \arccos(x)$ 的图像可以发现（arccos 函数图像），它在 $x = 1$ 附近的导数趋向无穷大。这意味着在 $1$ 附近，自变量哪怕只有极其微小的抖动（比如仅仅是最后一位的舍入误差），都会被 $\arccos$ 函数无限放大，导致计算出的距离在 $0$ 和一个巨大的错误值之间“反复横跳”。

简而言之，对于需要处理近距离（如几米到几十米）的定位应用，球面余弦定律可能会直接罢工。为了搞定这个坑，我们需要引入基于半角恒等式推导出的 Haversine 公式。

图 2：arccos 函数图像

Haversine公式

在介绍Haversine公式之前， 我们先介绍一下正矢函数

图 3：versine 函数图像

如上图所示，versine函数中文叫正矢函数， 定义为
$$\text{versin}(\theta) = 1 - \cos(\theta)$$

红色线段为$\cos(\theta)$， 橙色为$\text{versin}(\theta)$

基于高中学过的半角公式
$$\sin(\frac{\theta}{2}) = \pm \sqrt{\frac{1 - \cos\theta}{2}}$$

将其两边平方得到
$$\sin^2\left(\frac{\theta}{2}\right) = \frac{1 - \cos\theta}{2}$$
不难发现等式右边刚好和正矢函数的定义非常相似， 只是除以了2， 所以
$$\text{hav}(\theta) = \sin^2\left(\frac{\theta}{2}\right) = \frac{1 - \cos\theta}{2}$$

基于高中学过的三角恒等式变形

$$\cos A - \cos B = -2 \sin\left(\frac{A+B}{2}\right) \sin\left(\frac{A-B}{2}\right)$$

通过变形，我们可以得到积化和差的形式：
$$\sin A \sin B = \cos(A - B) - \cos A \cos B$$

现在回到球面余弦定律的公式
$$L = R \cdot \arccos(\cos w_a \cos w_b \cos(j_b - j_a) + \sin w_a \sin w_b)$$
不难发现其中有 $\sin w_a \sin w_b$ 的形式， 我们可以将其展开得到
$$\sin w_a \sin w_b = \cos(w_b - w_a) - \cos w_a \cos w_b$$
代入公式，得到
$$
L = R \cdot \arccos(\cos w_a \cos w_b \cos(j_b - j_a) + \cos(w_b - w_a) - \cos w_a \cos w_b)
$$
提取其中的公因式 $\cos w_a \cos w_b$， 得到
$$
L = R \cdot \arccos(\cos w_a \cos w_b (\cos(j_b - j_a) - 1) + \cos(w_b - w_a))
$$

根据弧长公式
$$L = R \cdot \theta$$
我们知道其实右边那一长串就是 $\theta$， 所以
$$
\theta = \arccos(\cos w_a \cos w_b (\cos(j_b - j_a) - 1) + \cos(w_b - w_a))
$$
则有
$$
\cos\theta = \cos w_a \cos w_b (\cos(j_b - j_a) - 1) + \cos(w_b - w_a)
$$
此时两边同时被1减去，得到
$$
1 - \cos\theta = 1 - \cos w_a \cos w_b (\cos(j_b - j_a) - 1) - \cos(w_b - w_a)
$$
稍作整理
$$
1 - \cos\theta = 1 - \cos(w_b - w_a) - \cos w_a \cos w_b (\cos(j_b - j_a) - 1)
$$
注意到 $-\cos w_a \cos w_b (\cos(j_b - j_a) - 1)$ 可以重写为 $+\cos w_a \cos w_b (1 - \cos(j_b - j_a))$， 则有
$$1 - \cos(\theta) = (1 - \cos(w_b - w_a)) + \cos w_a \cos w_b (1 - \cos(j_b - j_a))$$
现在将两边同时除以2， 得到
$$\frac{1 - \cos(\theta)}{2} = \frac{1 - \cos(w_b - w_a)}{2} + \cos w_a \cos w_b \frac{1 - \cos(j_b - j_a)}{2}$$
代入正矢函数定义
$$\text{hav}(\theta) = \text{hav}(w_b - w_a) + \cos w_a \cos w_b \cdot \text{hav}(j_b - j_a)$$

根据半正矢函数定义
$$\text{hav}(\theta) = \sin^2\left(\frac{\theta}{2}\right)$$
则上面的式子可以写为
$$\sin^2\left(\frac{\theta}{2}\right) = \sin^2\left(\frac{w_b-w_a}{2}\right) + \cos w_a \cos w_b \cdot \sin^2\left(\frac{j_b-j_a}{2}\right)$$
两边开平方得到
$$\sin\left(\frac{\theta}{2}\right) = \sqrt{\sin^2\left(\frac{w_b-w_a}{2}\right) + \cos w_a \cos w_b \cdot \sin^2\left(\frac{j_b-j_a}{2}\right)}$$
则
$$\theta = 2 \arcsin\left(\sqrt{\sin^2\left(\frac{w_b-w_a}{2}\right) + \cos w_a \cos w_b \cdot \sin^2\left(\frac{j_b-j_a}{2}\right)}\right)$$

再次代入弧长公式
$$L = R \cdot \theta$$
得到
$$L = 2R \cdot \arcsin\left(\sqrt{\sin^2\left(\frac{w_b-w_a}{2}\right) + \cos w_a \cos w_b \sin^2\left(\frac{j_b-j_a}{2}\right)}\right)$$

经过一系列三角函数变换， 最终得到的这一公式被称为Haversine公式
它解决了球面余弦定律在近距离计算时的数值稳定性问题

下一篇， 我们将介绍如何计算任意两点之间的方位角 (Bearing)

参考资料

使用GPS坐标来计算距离和方位角 by Johnny Qian

球面余弦定理

半正矢公式