如何获取GPS_velocity？基于经纬坐标计算0.1s速度并对比IMU数据

完整实现代码 #

from __future__ import print_function
import time
import math
import csv
from dronekit import connect, VehicleMode, LocationGlobalRelative
from pymavlink import mavutil  # 修正原代码拼写错误

# 连接载具
vehicle = connect('/dev/ttyACM0', wait_ready=True, baud=115200)

def haversine_distance(lat1, lon1, lat2, lon2):
    """用Haversine公式计算两点地面距离（单位：米），适配球面地球特性"""
    # 经纬度转为弧度
    lat1, lon1, lat2, lon2 = map(math.radians, [lat1, lon1, lat2, lon2])
    
    # Haversine公式核心计算逻辑
    dlat = lat2 - lat1
    dlon = lon2 - lon1
    a = math.sin(dlat/2)**2 + math.cos(lat1) * math.cos(lat2) * math.sin(dlon/2)**2
    c = 2 * math.asin(math.sqrt(a))
    earth_radius = 6378137.0  # 地球半径（米）
    return c * earth_radius

# 初始化变量
prev_location = None
prev_time = None
csv_file = open('velocity_comparison.csv', 'w', newline='')
csv_writer = csv.writer(csv_file)
# 写入CSV表头
csv_writer.writerow(['采样时间(UTC)', 'GPS速度(m/s)', 'IMU_X速度(m/s)', 'IMU_Y速度(m/s)', 'IMU_Z速度(m/s)'])

try:
    while True:
        current_time = time.time()
        # 每0.1秒执行一次采样与计算
        if prev_time is None or (current_time - prev_time) >= 0.1:
            current_location = vehicle.location.global_relative_frame
            # 仅当GPS返回有效经纬度时计算
            if current_location.lat is not None and current_location.lon is not None:
                if prev_location is not None:
                    # 计算前后位置距离
                    distance = haversine_distance(
                        prev_location.lat, prev_location.lon,
                        current_location.lat, current_location.lon
                    )
                    # 计算时间间隔与GPS速度
                    time_interval = current_time - prev_time
                    gps_velocity = distance / time_interval if time_interval > 0 else 0.0
                    
                    # 获取IMU三轴速度数据
                    imu_velocity = vehicle.velocity
                    
                    # 格式化UTC时间
                    utc_time = time.strftime('%Y-%m-%d %H:%M:%S', time.gmtime())
                    
                    # 输出数据并写入CSV
                    print(f"UTC时间: {utc_time}, GPS速度: {gps_velocity:.2f} m/s, IMU速度: {imu_velocity}")
                    csv_writer.writerow([
                        utc_time,
                        round(gps_velocity, 2),
                        round(imu_velocity[0], 2),
                        round(imu_velocity[1], 2),
                        round(imu_velocity[2], 2)
                    ])
                
                # 更新上一次采样的位置与时间
                prev_location = current_location
                prev_time = current_time
        # 降低CPU占用
        time.sleep(0.01)
except KeyboardInterrupt:
    print("程序终止")
finally:
    csv_file.close()
    vehicle.close()

关键逻辑说明 #

• 高精度距离计算：用Haversine公式替代原平面近似计算，考虑地球球面特性，GPS坐标距离结果更准确。
• 0.1秒采样控制：通过时间戳判断采样间隔，确保每次计算的时间差稳定在0.1秒左右，避免高频重复采样导致的无效计算。
• 数据对比与保存：同时采集GPS计算速度和IMU原生速度，写入CSV文件后可直接用表格工具做差值分析。
• 异常处理：判断GPS信号有效性，避免无信号时的报错；捕获终止信号，确保文件和连接正常关闭。

注意事项 #

1. 测试需在开阔场地进行，避免GPS信号遮挡导致坐标跳变，影响速度计算精度。
2. 若需要更精准的时间同步，可替换为载具的GPS时间戳，而非系统本地时间。
3. vehicle.velocity返回的是载具三轴速度（单位：m/s），GPS速度为地面二维速度，对比时可根据需求取IMU对应轴数据。

内容的提问来源于stack exchange，提问作者YUKI