基于Python与U-blox7的GPS导航程序问题排查咨询

项目背景 #

我是一名大学生，正在研发自主漫游车项目：

• 已实现键盘控制漫游车，通过Intel RealSense摄像头完成障碍物检测
• 下一步目标：开发接收航点坐标输入、引导漫游车沿路径抵达目的地的程序
• 测试方式：携带笔记本步行测试，用left/right/straight指令替代车轮控制指令
• 核心逻辑：
  1. 通过U-blox7 GPS/GLONASS模块获取实时坐标
  2. 确定起始位置与目的地，计算初始距离和初始方位角（正北到源点-目标点连线的夹角）
  3. 移动中更新当前方位角，与初始方位角对比：当前<初始则左转，反之右转，直至抵达目的地

测试代码 #

import math
import time
from collections import namedtuple
import gps

coordinates = namedtuple("coordinates", ["latitude", "longitude"])
session = gps.gps(mode=gps.WATCH_ENABLE)


def get_bearing(x1, y1, x2, y2):
    bearing = math.atan2(y2 - y1, x2 - x1) * (180 / math.pi)
    return (bearing + 360) % 360

try:
    waypoints = [
        coordinates(
            float(input("Enter waypoint 1 latitude: ")),
            float(input("Enter waypoint 1 longitude: ")),
        ),
        coordinates(
            float(input("Enter waypoint 2 latitude: ")),
            float(input("Enter waypoint 2 longitude: ")),
        ),
        coordinates(
            float(input("Enter waypoint 3 latitude: ")),
            float(input("Enter waypoint 3 longitude: ")),
        ),
    ]

    flag = 0
    current_waypoint_index = 0
    current_waypoint = waypoints[current_waypoint_index]

    while True:
        if not (session.valid):
            print('TPV')
            time.sleep(2)
            # not useful, probably not a TPV message
            continue

        if flag == 0:
            if gps.isfinite(session.fix.latitude) and gps.isfinite(session.fix.longitude):
                current_position = coordinates(session.fix.latitude, session.fix.longitude)

                x1 = current_position.latitude
                y1 = current_position.longitude
                x2 = current_waypoint.latitude
                y2 = current_waypoint.longitude

                initial_distance = math.sqrt((x2 - x1) ** 2 + (y2 - y1) ** 2) * 100000
                initial_bearing = get_bearing(x1, y1, x2, y2)

                print("Initial Distance:", initial_distance)
                print("Initial Bearing:", initial_bearing)
                flag = 1


        if gps.isfinite(session.fix.latitude) and gps.isfinite(session.fix.longitude):
            current_position = coordinates(session.fix.latitude, session.fix.longitude)

            x1 = current_position.latitude
            y1 = current_position.longitude
            x2 = current_waypoint.latitude
            y2 = current_waypoint.longitude


            current_distance = math.sqrt((x2 - x1) ** 2 + (y2 - y1) ** 2) * 100000
            current_bearing = get_bearing(x1, y1, x2, y2)

            print("Current Distance:", current_distance)
            print("Current Bearing:", current_bearing)
            

            if current_distance <= 0:
                print("Reached waypoint", current_waypoint_index + 1)
                current_waypoint_index += 1
                if current_waypoint_index < len(waypoints):
                    current_waypoint = waypoints[current_waypoint_index]
                    print("Navigating to waypoint", current_waypoint_index + 1)
                    x2 = current_waypoint.latitude
                    y2 = current_waypoint.longitude
                    flag = 0
            else:
                if not gps.isfinite(current_bearing):
                    print("Current bearing not available")
                else:
                    if current_bearing > initial_bearing:
                        print("Turn RIGHT")
                    elif current_bearing < initial_bearing:
                        print("Turn LEFT")
                    else:
                        print("Move STRAIGHT")
        else:
            print("Lat n/a Lon n/a")

        time.sleep(2)

except KeyboardInterrupt:
    pass

session.close()

遇到的问题 #

1. 抵达目的地后，当前距离值未更新
2. 转向指令与对应的转向角度存在不一致性

运行环境：Ubuntu 18.04，Python 3.6.9

问题排查思路 #

针对问题1：抵达目的地后距离值未更新 #

• 阈值设置问题：代码中判断抵达的条件是current_distance <= 0，但GPS坐标存在精度误差（U-blox7定位精度约2-3米），实际中几乎不可能出现距离为0的情况。这会导致程序一直判定未抵达目标点，无法切换航点，距离值停留在接近0的数值。
  • 修正方向：将判断阈值改为合理范围，比如current_distance <= 5（对应5米误差范围，可根据GPS实际精度调整）。
• 航点切换验证：切换到下一个航点后，添加日志打印current_waypoint的坐标，确认是否正确更新为下一个航点值，同时检查flag=0是否触发了新航点的初始距离和方位角计算。
• GPS数据有效性检查：抵达目标点后，持续打印current_position的经纬度，确认GPS模块是否仍在输出实时更新的有效坐标。

针对问题2：转向指令与角度不一致 #

• 方位角计算逻辑错误：当前get_bearing函数将经纬度当作平面直角坐标计算，不符合地理球面坐标的特性，会导致方位角偏差（高纬度地区误差更明显）。正确的地理方位角（正北顺时针计算）应该使用球面坐标公式：

  def get_bearing(lat1, lon1, lat2, lon2):
      lat1_rad = math.radians(lat1)
      lat2_rad = math.radians(lat2)
      delta_lon_rad = math.radians(lon2 - lon1)
      
      y = math.sin(delta_lon_rad) * math.cos(lat2_rad)
      x = math.cos(lat1_rad)*math.sin(lat2_rad) - math.sin(lat1_rad)*math.cos(lat2_rad)*math.cos(delta_lon_rad)
      bearing_rad = math.atan2(y, x)
      bearing_deg = math.degrees(bearing_rad)
      return (bearing_deg + 360) % 360
  

• 初始方位角逻辑缺陷：当前代码仅在开始时计算一次initial_bearing，但漫游车偏离路径后，初始方位角已不是当前应朝向的目标方位角。正确逻辑应实时计算当前位置到目标航点的方位角，并与漫游车的实际航向角（可通过GPS的track字段或IMU模块获取）对比，而非固定初始值。
• 环形角度判断问题：方位角是0-360度的环形值，比如初始方位角350度、当前方位角10度时，直接用current_bearing > initial_bearing会判定左转，但实际最短路径是右转。需处理环形角度差值：

  # 计算最短路径角度差
  angle_diff = (current_bearing - initial_bearing) % 360
  if angle_diff > 180:
      angle_diff -= 360
  # 基于差值判断转向
  if abs(angle_diff) < 2:  # 设置小阈值避免频繁转向
      print("Move STRAIGHT")
  elif angle_diff > 0:
      print("Turn RIGHT")
  else:
      print("Turn LEFT")
  

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