核心代码示例

以下通过几个关键模块的代码片段，展示如何用Python实现自主导航的基础功能。

1. 路径规划：A* 算法实现

A*算法是解决栅格地图上全局路径规划的经典方法。

import numpy as np
from queue import PriorityQueue

def heuristic(a, b):
    """启发式函数（曼哈顿距离）"""
    return abs(a[0] - b[0]) + abs(a[1] - b[1])

def a_star_search(grid, start, goal):
    """
    在二维栅格网格上执行A*搜索。
    :param grid: 2D numpy数组，0表示可通行，1表示障碍物。
    :param start: 起始坐标 (x, y)。
    :param goal: 目标坐标 (x, y)。
    :return: 路径列表（从起点到终点），如果找不到路径则返回空列表。
    """
    rows, cols = grid.shape
    # 可能的移动方向：上、下、左、右、四对角线
    neighbors = [(-1,0), (1,0), (0,-1), (0,1),
                 (-1,-1), (-1,1), (1,-1), (1,1)]

    close_set = set()
    came_from = {}
    gscore = {start: 0}
    fscore = {start: heuristic(start, goal)}
    open_set = PriorityQueue()
    open_set.put((fscore[start], start))

    while not open_set.empty():
        _, current = open_set.get()

        if current == goal:
            # 重构路径
            path = []
            while current in came_from:
                path.append(current)
                current = came_from[current]
            path.append(start)
            return path[::-1]

        close_set.add(current)

        for dx, dy in neighbors:
            neighbor = (current[0] + dx, current[1] + dy)
            # 检查邻居是否在地图范围内且可通行
            if 0 <= neighbor[0] < rows and 0 <= neighbor[1] < cols:
                if grid[neighbor[0], neighbor[1]] == 1:
                    continue
            else:
                continue

            tentative_g_score = gscore[current] + ((dx**2 + dy**2) ** 0.5) # 欧氏距离作为代价

            if neighbor in close_set and tentative_g_score >= gscore.get(neighbor, float('inf')):
                continue

            if tentative_g_score < gscore.get(neighbor, float('inf')):
                came_from[neighbor] = current
                gscore[neighbor] = tentative_g_score
                fscore[neighbor] = tentative_g_score + heuristic(neighbor, goal)
                if neighbor not in [i[1] for i in open_set.queue]:
                    open_set.put((fscore[neighbor], neighbor))
    return [] # 未找到路径

# 使用示例
if __name__ == "__main__":
    # 创建一个10x10的地图，1表示障碍物
    grid_map = np.array([
        [0,0,0,0,0,0,0,0,0,0],
        [0,1,1,0,0,0,0,0,0,0],
        [0,1,1,0,1,1,1,1,0,0],
        [0,0,0,0,0,0,0,1,0,0],
        [0,0,0,1,1,1,0,1,0,0],
        [0,0,0,0,0,0,0,0,0,0]
    ])
    start_point = (0, 0)
    goal_point = (5, 9)
    path = a_star_search(grid_map, start_point, goal_point)
    print("规划路径坐标:", path)

2. 路径跟踪：纯追踪算法 (Pure Pursuit)

纯追踪算法通过计算前视距离和曲率来控制机器人跟踪路径。

import numpy as np
import math

def pure_pursuit_control(current_pose, path, lookahead_distance):
    """
    纯追踪控制器。
    :param current_pose: 机器人当前位姿 [x, y, theta]。
    :param path: 要跟踪的路径，Nx2数组，每行是路径点[x, y]。
    :param lookahead_distance: 前视距离。
    :return: 期望的曲率 (curvature)，用于计算角速度。
    """
    x, y, theta = current_pose
    # 1. 寻找路径上距离机器人最近的点
    distances = np.sqrt((path[:,0] - x)**2 + (path[:,1] - y)**2)
    closest_idx = np.argmin(distances)

    # 2. 在前视距离范围内，寻找目标点
    target_idx = closest_idx
    for i in range(closest_idx, len(path)):
        if np.linalg.norm(path[i] - np.array([x, y])) >= lookahead_distance:
            target_idx = i
            break
    # 如果循环结束仍未找到，则取最后一个点
    if target_idx == closest_idx and closest_idx < len(path) - 1:
        target_idx = len(path) - 1

    target_point = path[target_idx]
    # 3. 计算目标点在机器人坐标系下的位置
    dx = target_point[0] - x
    dy = target_point[1] - y
    target_local_x = dx * math.cos(theta) + dy * math.sin(theta)
    target_local_y = -dx * math.sin(theta) + dy * math.cos(theta)

    # 4. 计算曲率 = 2 * y / L^2，其中L是前视距离，y是横向误差
    curvature = 2.0 * target_local_y / (lookahead_distance ** 2)
    return curvature, target_point

# 使用示例
if __name__ == "__main__":
    # 假设一条直线路径
    reference_path = np.array([[i, 0.5] for i in range(20)])
    robot_pose = [2.0, 0.2, 0.1] # x, y, yaw
    lookahead = 1.5
    curvature, target = pure_pursuit_control(robot_pose, reference_path, lookahead)
    print(f"计算得到曲率: {curvature:.4f}, 目标点: {target}")
    # 通常，线速度v是固定的或由其他逻辑给出，角速度 w = v * curvature
    v = 0.5
    w = v * curvature
    print(f"建议控制量 - 线速度: {v}, 角速度: {w:.4f}")

3. 与ROS集成：发送导航目标点

在ROS1中，自主导航通常使用move_base节点。可以通过Python发送一个PoseStamped消息到/move_base_simple/goal话题来指令机器人前往某个目标点。

#!/usr/bin/env python3
import rospy
from geometry_msgs.msg import PoseStamped, Quaternion
from tf.transformations import quaternion_from_euler

def send_navigation_goal(x, y, yaw):
    """
    发送一个2D导航目标点到ROS的move_base。
    """
    rospy.init_node('send_goal_py', anonymous=True)
    pub = rospy.Publisher('/move_base_simple/goal', PoseStamped, queue_size=10)
    rospy.sleep(1) # 等待发布者建立连接

    goal = PoseStamped()
    goal.header.stamp = rospy.Time.now()
    goal.header.frame_id = "map" # 目标点相对于地图坐标系

    goal.pose.position.x = x
    goal.pose.position.y = y
    goal.pose.position.z = 0.0

    # 将偏航角yaw转换为四元数
    q = quaternion_from_euler(0, 0, yaw)
    goal.pose.orientation = Quaternion(*q)

    pub.publish(goal)
    rospy.loginfo(f"导航目标已发送: ({x}, {y}, {yaw})")
    rospy.spin() # 保持节点运行，直到Ctrl+C

if __name__ == '__main__':
    try:
        # 发送目标点：x=1.0, y=2.0, 朝向yaw=1.57弧度（90度）
        send_navigation_goal(1.0, 2.0, 1.57)
    except rospy.ROSInterruptException:
        pass

实战项目流程与资源

1. 环境搭建与仿真：对于初学者，强烈建议在仿真环境中开始。可以使用 ROS + Gazebo 搭建机器人仿真环境。例如，使用TurtleBot3或自主设计的URDF模型在Gazebo中构建世界，并在Rviz中可视化。
2. 获取与处理传感器数据：在ROS中，通过订阅话题（如/scan激光雷达、/odom里程计）获取数据。Python脚本可以使用rospy库来接收和处理这些消息。

   import rospy
   from sensor_msgs.msg import LaserScan
   def scan_callback(data):
       ranges = list(data.ranges) # 获取激光雷达测距数据
       # ... 进行数据处理，例如障碍物检测
   rospy.Subscriber("/scan", LaserScan, scan_callback)
   

3. 调用成熟的导航栈：最直接的方式是利用ROS的move_base导航栈。你需要为其提供costmap参数（全局和局部代价地图）、global_planner（如global_planner/Planner）和local_planner（如dwa_local_planner/DWAPlannerROS）的配置。你的Python代码主要扮演高层任务调度和与move_base交互的角色。
4. 学习与参考代码库：
   • GitHub 资源：参考日本工程师坂井敦（Atsushi Sakai）整理的 PythonRobotics 代码库，它几乎包含了表格中所有算法的清晰Python实现，并配有可视化动图。这是学习算法原理和实现的绝佳资源。
   • SLAM实战：对于完整的SLAM和导航系统，可以参考详细的实战博客，其中涵盖了从环境配置、Gmapping建图、move_base参数调试到系统优化的全流程。

总结，使用Python实现机器人自主导航是一个系统工程，涉及感知、定位、规划、控制多个模块。从单个算法（如A*路径规划）的Python实现入手，再学习在ROS框架下利用Python进行消息通信、任务调度，并集成成熟的C++算法模块（如move_base），是高效可行的开发路径。

参考来源

• SLAM实战：机器人自主导航系统开发（附完整代码+数据集）
• Python实现DroNet训练与评估代码实战
• 机器人实用Python代码合集，帮你搞定自主导航 |GitHub高热
• 机器人实用Python代码合集，帮你搞定自主导航 |GitHub高热
• ROS1 调试 —— 雷达相关话题在自主导航中的应用
• ROS智能车定位导航仿真（原赛道自主导航，构建地图再导航）