根据两点的经纬度求方位角和距离等问题

自动驾驶中利用gps算距离和方位角的问题是非常基本的问题也很常见。主要涉及到的就是球面三角学。

本文参考这篇文章做一个说明，这一篇其实也是转载，原创的博文已经不见了。原来转载的那篇文章有一些错误，格式排版也不是很好，所以此篇博客稍微整理了一下。
在这篇博客中主要解决三个问题：

1. 已知两点经纬度，求两点间距离；
2. 已知两点经纬度，求一点相对于另一点航向；
3. 已知一点经纬度及与另一点距离和航向，求另一点经纬度；

总声明：

因为地球偏心率极低，所以此处将地球看做球体，以下所有公式在适用范围内使用可以保证计算结果与WGS84坐标系统下测量的偏差最大不超过0.5%。关于度娘知道上有人说的需要将WGS84坐标转换成北京54、西安80后再计算的高深言论，我的观点是：只要不是搞大地测量、土木工程、导弹发射或者载人飞船回收，就根本就没有必要那么做，请这帮满嘴专业词汇自己却连WGS84坐标系统下距离求算公式都给不出的砖家们“地平线有多远，就给我滚多远”。关于方法的名称，是为便于编写自己起的，请勿当做正式名称与人讨论。因本人才浅手拙，不善算数，故错误难免，如有发现，定及时改正，请见谅。

有关WGS84坐标系统下的距离及航向求算方法，需要精确计算的请点击下列查看
距离及航向1
距离及航向2
Vincenty公式
更多信息，请谷歌 Vincenty’s formula

关于WGS84坐标系统，请点下列查看：
WGS84 1
WGS84 2

在求算前我们先对符号及单位进行约定：

$此处设定求B相对于A的航向，即A为当前位置，B为目标位置$

$Aj：A点经度$

$Aw：A点纬度$

$Bj：B点经度$

$Bw：B点纬度$

北纬为正，南纬为负；东经为正，西经为负

经纬度使用十进制度，i.e.DDD.DDDDDD°，非度分或度分秒。

度数未加说明均采用角度制

$A,B,C表示球面上的三个点及球面上“弧线”在该点处所夹的角，C所处位置为北极点$

$a,b,c表示A,B,C三点的对“弧”两端点与地心连线所夹的角（其实这里解释成ABC三点对弧的弧度更方便）$

$O$：球心

$L：AB两点间球面距离，也叫做大圆距离，即过AOB三点的平面与球相交所产生的圆弧中劣弧AB的长度$

$R：地球平均半径$

$Bearing：起始真航向，也叫大圆始航向。以真北为0度起点，由东向南向西顺时针旋转360度。$

此处所要求算的角度非“地理相对方位”，而是“航行方向”也就是“圆弧L在A点处的切线与真北方向的夹角”。举个栗子：设A点经纬度：40°N,10°E；B点经纬度：40°N，100°E。显然B点在A点的正东方90°，但是如果某人自A点以最短距离去B点，则在A点需要先向57.2676°方向走，之后逐渐变化航向（只是所测得的航向发生了变化，但他仍然在那一个大圆平面上），最终到达B点时的行走方向为122.7324°。这里，57.2676°就是我所说“起始真航向”。与这个概念相对的另一个航向叫做恒向线航向，更多详情请谷歌：Rhumb line，目前暂不讨论恒向线距离及恒向线航向求算的问题.

（注：因我考虑欠缺，没有注意字母C大小写较难分辨，所以此处提醒读者在后面的公式中注意C的大小写。

对于一些GPS接收机，其数据格式为NMEA-0183，经纬度报文样式为DDDMM.MMMM，需要将它转换为十进制度，公式为：

经纬度（度）=DDD+MM.MMMM/60）

一、距离的求算

方法1、球面余弦公式法

适用范围：理论上，此方法适用于球面上任意两点间的距离求算，但是因为公式中有cos项，当系统的浮点运算精度不高时，在求算较近的两点间的距离时会有较大误差（64位系统一般无需担心此问题）。

已知：$Aj$，$Aw$，$Bj$，$Bw$，$R$

求算：

第一步：在知道AB点经纬度后，要用到第一个公式，球面余弦公式，

$$cos(c)=cos(a)\ast cos(b)+sin(a)\ast sin(b)\ast cos(c)$$

这里，角C等于角A-OC-B，也就是面AOC与面BOC的二面角，也就是 B j −