import math
# from math import radians,cos,sin,asin,sqrt,pi,atan,tan,atan2
# 这个算法和 geographiclib 算的几乎一样,相差<0.2米
def distVincenty(lat1, lon1, lat2, lon2):
'精度更高的椭球计算,计算两个 WGS84 经纬度点的距离'
a = 6378137.0 # vincentyConstantA(WGS84) ##单位:米
b = 6356752.3142451 # vincentyConstantB(WGS84) ##单位:米
f = 1 / 298.257223563 # vincentyConstantF(WGS84)
L = math.radians(lon2 - lon1)
U1 = math.atan((1 - f) * math.tan(math.radians(lat1)))
U2 = math.atan((1 - f) * math.tan(math.radians(lat2)))
sinU1 = math.sin(U1)
cosU1 = math.cos(U1)
sinU2 = math.sin(U2)
cosU2 = math.cos(U2)
lambda1 = L
lambdaP = 2 * math.pi
iterLimit = 20
sinLambda = 0.0
cosLambda = 0.0
sinSigma = 0.0
cosSigma = 0.0
sigma = 0.0
alpha = 0.0
cosSqAlpha = 0.0
cos2SigmaM = 0.0
C = 0.0
while (abs(lambda1 - lambdaP) > 1e-12 and --iterLimit > 0):
sinLambda = math.sin(lambda1)
cosLambda = math.cos(lambda1)
sinSigma = math.sqrt((cosU2 * sinLambda) * (cosU2 * sinLambda) + (cosU1 * sinU2 - sinU1 * cosU2 * cosLambda) * (
cosU1 * sinU2 - sinU1 * cosU2 * cosLambda))
if (sinSigma == 0):
return 0
cosSigma = sinU1 * sinU2 + cosU1 * cosU2 * cosLambda
sigma = math.atan2(sinSigma, cosSigma)
alpha = math.asin(cosU1 * cosU2 * sinLambda / sinSigma)
cosSqAlpha = math.cos(alpha) * math.cos(alpha)
cos2SigmaM = cosSigma - 2 * sinU1 * sinU2 / cosSqAlpha
C = f / 16 * cosSqAlpha * (4 + f * (4 - 3 * cosSqAlpha))
lambdaP = lambda1
lambda1 = L + (1 - C) * f * math.sin(alpha) * (
sigma + C * sinSigma * (cos2SigmaM + C * cosSigma * (-1 + 2 * cos2SigmaM * cos2SigmaM)))
if iterLimit == 0:
return 0.0
uSq = cosSqAlpha * (a * a - b * b) / (b * b)
A = 1 + uSq / 16384 * (4096 + uSq * (-768 + uSq * (320 - 175 * uSq)))
B = uSq / 1024 * (256 + uSq * (-128 + uSq * (74 - 47 * uSq)))
deltaSigma = B * sinSigma * (cos2SigmaM + B / 4 * (
cosSigma * (-1 + 2 * cos2SigmaM * cos2SigmaM) - B / 6 * cos2SigmaM * (-3 + 4 * sinSigma * sinSigma) * (
-3 + 4 * cos2SigmaM * cos2SigmaM)))
s = b * A * (sigma - deltaSigma)
d = s ##单位:米
return d
if __name__ == '__main__':
# file_name = 'E:/231025/YT'
file_name = 'E:/10.27/GGA_20231027_015005'
'''
YT文件内容如下:
$GPZDA,063052.00,16,10,2023,,*61
$GPGGA,063052.00,4349.7377413,N,12509.8354912,E,4,40,0.6,222.928,M,0.00,M,01,2445*69
$GPZDA,063053.00,16,10,2023,,*60
$GPGGA,063053.00,4349.7377412,N,12509.8354914,E,4,40,0.6,222.926,M,0.00,M,01,2445*61
$GPZDA,063054.00,16,10,2023,,*67
$GPGGA,063054.00,4349.7377415,N,12509.8354924,E,4,40,0.6,222.926,M,0.00,M,01,2445*62
'''
with open(file_name, 'r') as yt:
# 临时存放
tmp = []
for line in yt.readlines():
tmpSort = line.strip().split(",")
# 筛选以GPGGA开头,经纬度不为空的行
if (tmpSort[0] == '$GPGGA') and tmpSort[2] and tmpSort[4]:
tmp.append(tmpSort)
# print(tmp)
sum = 0
l = 0
for i in range(len(tmp) - 1):
# print(tmp[i][2], tmp[i][4])
# 计算相邻两行的第3个值(lat)和第5个(lon),并累加
lat1 = float(tmp[i][2][:2]) + float(tmp[i][2][2:]) / 60
lon1 = float(tmp[i][4][:3]) + float(tmp[i][4][3:]) / 60
lat2 = float(tmp[i + 1][2][:2]) + float(tmp[i + 1][2][2:]) / 60
lon2 = float(tmp[i + 1][4][:3]) + float(tmp[i + 1][4][3:]) / 60
sum += distVincenty(lat1, lon1, lat2, lon2)
l += 1
print("第%s次:" % (l) + "统计路程:" + str(sum) + "米")
print("总路程:%s" % str(sum) + "米")