根据两点的经纬度求方位角和距离
转自：http://blog.sina.com.cn/s/blog_5e7960620101vi0d.html 拿过来学习下；
这里主要解决四个问题：
1、已知两点经纬度，求两点间距离；
2、已知两点经纬度，求一点相对于另一点航向；
3、已知一点经纬度及与另一点距离和航向，求另一点经纬度；
总声明：
因为地球偏心率极低，所以此处将地球看做球体，以下所有公式在适用范围内使用可以保证计算结果与WGS84坐标系统下测量的偏差最大不超过0.5%。关于度娘知道上有人说的需要将WGS84坐标转换成北京54、西安80后再计算的高深言论，我的观点是：只要不是搞大地测量、土木工程、导弹发射或者载人飞船回收，就根本就没有必要那么做，请这帮满嘴专业词汇自己却连WGS84坐标系统下距离求算公式都给不出的砖家们“地平线有多远，就给我滚多远”。关于方法的名称，是为便于编写自己起的，请勿当做正式名称与人讨论。因本人才浅手拙，不善算数，故错误难免，如有发现，定及时改正，请见谅。
有关WGS84坐标系统下的距离及航向求算方法，需要精确计算的请点击下列查看
距离及航向1 http://www.geomidpoint.com/destination/calculation.html
距离及航向2 http://www.movable-type.co.uk/scripts/latlong-vincenty.html
Vincenty公式 https://en.wikipedia.org/wiki/Vincenty
更多信息，请谷歌 Vincenty's formula
关于WGS84坐标系统，请点下列查看：
WGS84 1 https://en.wikipedia.org/wiki/World_Geodetic_System
WGS84 2 https://confluence.qps.nl/qinsy/en/world-geodetic-system-1984-wgs84-29855173.html
在求算前我们先对符号及单位进行约定：
此处设定求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度。
（注：在之前的文章中我写的是Azimuth方位角，但是因为这个词容易产生误解，所以这次修改为Bearing，也就是此处所要求算的角度非“地理相对方位”，而是“航行方向”也就是“圆弧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点经纬度后，要用到第一个公式，球面余弦公式，
这里，角C等于角A~OC~B，也就是面AOC与面BOC的二面角，也就是Bj-Aj
这里我们将已知数据代入，公式便写成：
求得c的余弦值后用反余弦函数便可求得角c
将角度化为弧度后求取距离
这里要注意，L的单位与R的单位时一致的，单位不同的不要忘记换算。另外最后的Bj-Aj的顺序在这里就无所谓，因为cos函数的Y轴对称性值都是一样的。
方法2、Haversine法
适用范围：此方法适用于球面上任意两点间的距离求算，用高中数学知识即可证明此方法是球面余弦函数的一个变换，因为其中换掉了cos项，因此不存在短距离求算时对系统计算精度的过多顾虑的问题。
已知：Aj，Aw，Bj，Bw，R
求算：
这里为书写方便默认上式反正弦函数求取度数为弧度制。
方法3、直角坐标系法
适用范围：因为将球坐标转化为直角坐标并运用勾股定理，所以只能在两点相聚较近时使用，纬度越高，使用范围越窄。
已知：Aj，Aw，Bj，Bw，R
求算：
简化算法的基本思路就是将以经纬度表示的球坐标转换成三维直角坐标，再利用立体几何知识去解决。
设：Xa、Ya、Za为三维直角坐标下A点的坐标，B点坐标同样式，
Ha为A点海拔高度，Hb为b点海拔高度
则
Xa=(R+Ha)×cos(Aw)×cos(Aj)
Ya=(R+Ha)×cos(AW)×sin(Aj)
Za=(R+Ha)×sin(Aw)
Xb=(R+Hb)×cos(Bw)×cos(Bj)
Yb=(R+Hb)×cos(Bw)×sin(Bj)
Zb=(R+Hb)×sin(Bw)
（注：此处坐标转换为诱导公式化简后的形式，关于球坐标转直角坐标的公式可点此查看：球坐标转直角坐标（https://zh.wikipedia.org/wiki/%E7%90%83%E5%BA%A7%E6%A8%99%E7%B3%BB））
这里说明一下，海拔高度H可有可无，如果有的话，注意H与R单位要统一。普通GPS接收机给出的海拔一般很不准确，所以不推荐使用。另外NMEA规范报文中有两个量涉及到海拔，注意计算。
二、航向的求算
方法1、球面正弦公式法
适用范围：理论上，这个公式可以用于地球上任意两点间航向的求算，但是，这个方法求出的角度在“后处理”上存在缺陷（关于缺陷的原因及解决方法会在后面说明），而且因为使用了球面正弦公式的余弦值，同样对系统浮点运算精度有较高要求，因此不推荐使用此方法。如果要使用，则这个方法只能在短距离内使用，低纬度地区建议300km以下，中纬度地区建议100km以下，高纬度地区建议40km以下。
已知：Aj，Aw，Bj，Bw
求解：
在“距离的求算”的“方法1”中求得了角c的余弦值，要求得它的正弦值，所用的公式就是三角函数里最基本正弦余弦转换函数的一个变形
求得正弦后，接下来我们要用一个不太常用的公式，球面正弦公式
将已知数据代入并稍微变形一下，公式写为：
用反正弦函数求角度，于是上式就写成
这里需要注意一点，我们一开始的假设便是求B点相对于A点的航向，因此这里是Bj-Aj，不要写反，否则后面得不到正确结果。
算到这里，还没有完，得到的结果并不总符合我们对航向的定义，因此要根据B相对于A的位置在四个象限两个轴上进行讨论，依据不同情况对计算结果进行不同处理。假设A点固定于原点，则:
B点在第一象限，Bearing=A；
B在第二象限，Bearing=360+A;
B在第三四象限，Bearing=180-A。
这里只说了象限的讨论结果，因为轴上的讨论更复杂些，要结合程序运行环境一起考虑，考虑的主要因素是系统的计算精度。譬如，在三面角余弦公式中，当AB点纬度值相同时，对公式的值起决定作用的就是cos(Bj-Aj)这一项，当Bj-Aj的值比较小时，例如0.0001（这在赤道地区对应的长度为11米左右），用一般的计算器计算时值为1，这样，后面的计算便不可能完成。但是，如果用计算机计算则为0.999999999998476913…………。所以，基于以上原因，需要对轴的“范围进项扩充”，要用单片机、手机运算的尤其要注意。
经过一系列计算，最后，就得到了最终结果。
--------------------------------------------------------------------------------------------------
到这里，上面是我最开始写的一个方法，还对最后结果进行了分情况讨论，似乎没什么不妥，其实就是这一段分象限讨论使这个方法不再完美，因为对一个极坐标下的结果用直角坐标系来对它讨论就是错误。依旧举个栗子：
（白色线为经纬线，黄色线为AB最短路径）
A点经纬度：40°N，80°E。B点经纬度：37.55°N，120°E。
按照上面的方法，B点应该在第四象限，也就是航向应当为98.53°，但是实际上在A点向B点走，要先朝81.4651°的方向走，因此，可以说，上面的象限讨论完全就是个错误（虽然这个错误在短距离内与实际值偏差微乎其微），对轴的范围进行扩充跟加剧了这个错误。目前，可以使用“距离的求算”中“Haversine法”的反正弦结果进行稍微处理以代替球面余弦公式所得的余弦结果，从而省去对轴的扩充，但是到目前，我还没有找到一个将上述结果转化成符合航向定义的合理方法。
方法2、平面直角坐标系法
适用范围：这个方法我的基本思路就是将经度差和纬度差转化成地面距离再运用平面几何知识求解，所以只能用于短距离求算，中纬度地区建议40km以下。因为计算更简便，所以相对来说有优势。
已知：Aj，Aw，Bj，Bw
求解：
当
B点在第一象限及Y轴正半轴，Bearing=A；
B在第二象限，Bearing=360+A；
B在第三四象限及Y轴负半轴，Bearing=180+A。
对于某些系统，再单独设定B位于X正负半轴上的值就可以了，有些系统可以返回arctan（X/0）=90。
方法3、极坐标法
适用范围：这个方法可以用于地球上任意两点间航向的求算，不存在任何限制及缺陷。（至少目前我还没发现）
已知：Aj，Aw，Bj，Bw
求解：
这个方法是我从别处学来的，关于这个方法的证明，请点此：求算航向（http://mathforum.org/library/drmath/view/55417.html）
因为输入自然公式对我这个懒人来说太累了，所以这里只说一下他证明的思路，他的思路就是将地球放在一个球坐标系中并适当调整三个参数的起始点以减少后面的运算量，然后将各点由球坐标转化为直角坐标，之后依据平面法向量的定理求得二面夹角也就是航向，之后转化为符合航向定义的度数。
求算前先定义3个临时变量x，y，A
其中
将求得的x，y值代入下面这个函数中
atan2这个函数比较少用，其实它就是直角坐标转化成极坐标方法的集合，如果程序的函数库中有这个函数，可以省下时间去做更有意义的事情，如果不幸没有找到这个函数名称，请点此：atan2函数，按照当中所列的方法自行编写实现这个函数的功能。
求得A后下面要进行非常关键的一步，MOD函数求余。
这样，就得到了符合航向定义的结果。
(注：在调用MOD函数时一定要看清所用语言对MOD的定义，这里需要其定义符合下式
如果不符合就需要自行编写语句实现上述MOD功能，在自行编写语句时，使用INT函数时还要看清所用语言对INT函数的定义，此处INT函数应当为向下取整，而非四舍五入或者向上取整。）
三、第二点经纬度的求算
最近在网上看到不少人在问第二点经纬度的求算，所以，这里也附加说一下求算方法。
都应该能想到一个最最最笨的方法，就是将前面两部分用的公式联立解方程。
言归正传，解方程的方法可以，但是运算量极大，费时，对于一些系统不现实。
方法1、球面三角函数法
适用范围：这个方法其实就是“航向的求算”“球面正弦公式法”的再次运用。目前未发现有任何限制，因为这当中没有象限讨论，因此不会发生两种坐标系冲突的低级错误。
已知：Aj，Aw，L，R，Bearing
求解：
首先求算c，
（注意此处L、R的单位要统一）
之后求解a，将已知量代入，公式为：
求得a之后我们求解C，
写到这里应该都恍然大悟了吧，
Bw=90-a
Bj=Aj+C
PS：对于上面两个三角公式的理解，可以想成把A点作为北极点。