适用于城市内小范围

# lon1:：位置1经度；lat1：位置1纬度；lon2：位置2经度；lat2：位置2纬度
def gcj02_sphere_distance(lon1, lat1, lon2, lat2):
    # 地球平均半径
    # R = 6371.0 # （单位：千米）
    R = 6371000 # （单位：米）6378137.0 ?

    # 转为弧度
    phi1 = math.radians(lat1)
    lambda1 = math.radians(lon1)
    phi2 = math.radians(lat2)
    lambda2 = math.radians(lon2)

    # 计算差值
    delta_phi = phi2 - phi1
    delta_lambda = lambda2 - lambda1

    # Haversine公式
    a = math.sin(delta_phi / 2)**2 + math.cos(phi1) * math.cos(phi2) * math.sin(delta_lambda / 2)**2
    c = 2 * math.atan2(math.sqrt(a), math.sqrt(1 - a))
    distance = R * c

    return distance

大多车辆的坐标系是以后轴中心点为原点的，坐标系如下：

Note：

智驾一般使用上诉坐标系作为自车坐标系。

其中：航向角（Yaw1）正东为0，顺时针为负，逆时针为正

而地图导航，如高德的坐标系也是东北天坐标系，但基准一般为：航向角（Yaw2）正北为0，顺时针为正，逆时针为负

所以不同坐标系下的GCJ-02经纬度的计算还需考虑坐标系的基准，如上自车坐标系和高德坐标系举例，计算ADAS和高德两个系统给的GCJ-02经纬度信息的相对坐标则如下（转为自车坐标系）：

//角度转弧度
Radian = degree * M_PI / 180 
// 弧度转角度则为：Degree = radian * 180 / M_PI，其中M_PI≈3.1415926

//计算经纬度差值
delta_Longitude = Longitude2 - Longitude1
delta_Latitude = Latitude2 - Latitude1

# Haversine公式
a = math.sin(delta_Latitude / 2)**2 + math.cos(Longitude1) * math.cos(Longitude2) * math.sin(delta_Longitude / 2)**2
c = 2 * math.atan2(math.sqrt(a), math.sqrt(1 - a))
distance = R * c	// R是地球平均半径，约为6371公里

//将高德坐标系转为ADAS（自车）坐标系，计算相对坐标xy
//计算X坐标，正北方向为正
double positionX = distance * (cos(Latitude1) * sin(Latitude2) - 
            sin(Latitude1) * cos(Latitude2) * cos(delta_Longitude));  
//计算Y坐标，正东方向为正      
double positionY = distance * cos(Latitude2) * sin(delta_Longitude);                                                                                
//计算X坐标，车体方向为正
*calPositionX = positionY * sin(Yaw1) + positionX * cos(Yaw1);  
//计算Y坐标，车体切线左方为正
*calPositionY = positionY * cos(Yaw1) - positionX * sin(Yaw1);  

其他：

地图上查看GCJ-02坐标系下点位置：https://lbs.amap.com/demo/javascript-api-v2/example/marker/delete-multiple-markers
坐标系转换：https://www.lddgo.net/convert/coordinate
经纬度距离计算：https://www.lddgo.net/convert/distance

对标网页https://www.lddgo.net/convert/coordinate结果，实现WGS-84转GCJ-02：

python实现：

import math

# 判断是否在中国范围内
def out_of_china(lng, lat):
    return (lng < 72.004 or lng > 137.8347) or (lat < 0.8293 or lat > 55.8271)

# 转换函数
def transform_lat(x, y):
    ret = -100.0 + 2.0 * x + 3.0 * y + 0.2 * y * y + 0.1 * x * y + 0.2 * math.sqrt(abs(x))
    ret += (20.0 * math.sin(6.0 * x * math.pi) + 20.0 * math.sin(2.0 * x * math.pi)) * 2.0 / 3.0
    ret += (20.0 * math.sin(y * math.pi) + 40.0 * math.sin(y / 3.0 * math.pi)) * 2.0 / 3.0
    ret += (160.0 * math.sin(y / 12.0 * math.pi) + 320 * math.sin(y * math.pi / 30.0)) * 2.0 / 3.0
    return ret

def transform_lng(x, y):
    ret = 300.0 + x + 2.0 * y + 0.1 * x * x + 0.1 * x * y + 0.1 * math.sqrt(abs(x))
    ret += (20.0 * math.sin(6.0 * x * math.pi) + 20.0 * math.sin(2.0 * x * math.pi)) * 2.0 / 3.0
    ret += (20.0 * math.sin(x * math.pi) + 40.0 * math.sin(x / 3.0 * math.pi)) * 2.0 / 3.0
    ret += (150.0 * math.sin(x / 12.0 * math.pi) + 300.0 * math.sin(x * math.pi / 30.0)) * 2.0 / 3.0
    return ret

def delta(lng, lat):
    a = 6378245.0  # 地球长半轴
    ee = 0.00669342162296594323  # 扁率
    dLat = transform_lat(lng - 105.0, lat - 35.0)
    dLng = transform_lng(lng - 105.0, lat - 35.0)
    radLat = lat / 180.0 * math.pi
    magic = math.sin(radLat)
    magic = 1 - ee * magic * magic
    sqrtMagic = math.sqrt(magic)
    dLat = (dLat * 180.0) / ((a * (1 - ee)) / (magic * sqrtMagic) * math.pi)
    dLng = (dLng * 180.0) / (a / sqrtMagic * math.cos(radLat) * math.pi)
    return dLng, dLat

def wgs84_to_gcj02(lng, lat):
    if out_of_china(lng, lat):
        return lng, lat
    dLng, dLat = delta(lng, lat)
    mgLng = lng + dLng
    mgLat = lat + dLat
    return mgLng, mgLat

# 示例
wgs84_lng, wgs84_lat = 116.391349, 39.907375
gcj02_lng, gcj02_lat = wgs84_to_gcj02(wgs84_lng, wgs84_lat)
print(f"示例 北京天安门坐标：")
print(f"WGS-84 坐标: {wgs84_lng}, {wgs84_lat}")
print(f"GCJ-02 坐标: {gcj02_lng}, {gcj02_lat}")
# 参考网页：https://www.lddgo.net/convert/coordinate，期望输出：116.39759019123527, 39.90877629414095

C++实现：

#include <iostream>
#include <cmath>
    
    // 判断是否在中国范围内
    bool out_of_china(double lng, double lat) {
        return (lng < 72.004 || lng > 137.8347) || (lat < 0.8293 || lat > 55.8271);
    }
    
    // 转换函数
    double transform_lat(double x, double y) {
        double ret = -100.0 + 2.0 * x + 3.0 * y + 0.2 * y * y + 0.1 * x * y + 0.2 * std::sqrt(std::abs(x));
        ret += (20.0 * std::sin(6.0 * x * M_PI) + 20.0 * std::sin(2.0 * x * M_PI)) * 2.0 / 3.0;
        ret += (20.0 * std::sin(y * M_PI) + 40.0 * std::sin(y / 3.0 * M_PI)) * 2.0 / 3.0;
        ret += (160.0 * std::sin(y / 12.0 * M_PI) + 320 * std::sin(y * M_PI / 30.0)) * 2.0 / 3.0;
        return ret;
    }
    
    double transform_lng(double x, double y) {
        double ret = 300.0 + x + 2.0 * y + 0.1 * x * x + 0.1 * x * y + 0.1 * std::sqrt(std::abs(x));
        ret += (20.0 * std::sin(6.0 * x * M_PI) + 20.0 * std::sin(2.0 * x * M_PI)) * 2.0 / 3.0;
        ret += (20.0 * std::sin(x * M_PI) + 40.0 * std::sin(x / 3.0 * M_PI)) * 2.0 / 3.0;
        ret += (150.0 * std::sin(x / 12.0 * M_PI) + 300.0 * std::sin(x * M_PI / 30.0)) * 2.0 / 3.0;
        return ret;
    }
    
    std::pair<double, double> delta(double lng, double lat) {
        double a = 6378245.0;  // 地球长半轴
        double ee = 0.00669342162296594323;  // 扁率
        double dLat = transform_lat(lng - 105.0, lat - 35.0);
        double dLng = transform_lng(lng - 105.0, lat - 35.0);
        double radLat = lat / 180.0 * M_PI;
        double magic = std::sin(radLat);
        magic = 1 - ee * magic * magic;
        double sqrtMagic = std::sqrt(magic);
        dLat = (dLat * 180.0) / ((a * (1 - ee)) / (magic * sqrtMagic) * M_PI);
        dLng = (dLng * 180.0) / (a / sqrtMagic * std::cos(radLat) * M_PI);
        return std::make_pair(dLng, dLat);
    }
    
    std::pair<double, double> wgs84_to_gcj02(double lng, double lat) {
        if (out_of_china(lng, lat)) {
            return std::make_pair(lng, lat);
        }
        auto [dLng, dLat] = delta(lng, lat);
        double mgLng = lng + dLng;
        double mgLat = lat + dLat;
        return std::make_pair(mgLng, mgLat);
    }
    
    int main() {
        double wgs84_lng = 116.391349;
        double wgs84_lat = 39.907375;
        auto [gcj02_lng, gcj02_lat] = wgs84_to_gcj02(wgs84_lng, wgs84_lat);
        std::cout << "GCJ - 02 坐标: " << gcj02_lng << ", " << gcj02_lat << std::endl;
        return 0;
    }