Python 机器人大脑构建指南：路径规划与决策算法深度解析

1. 引言：从扫地机到自动驾驶

在人工智能与机器人技术飞速发展的今天，**“移动”**是智能体最基础的能力之一。无论是家中的扫地机器人，仓库里的 AGV 小车，还是在大街上行驶的自动驾驶汽车，它们的核心技术栈中都离不开两个关键模块：

1. 路径规划 (Path Planning)： 解决“如何从 A 点到达 B 点”的问题，侧重于几何路径的生成与避障。
2. 决策系统 (Decision Making)： 解决“当前应该做什么”的问题（例如：是超车、跟车还是停车等待），侧重于高层逻辑与行为仲裁。

Python 凭借其强大的科学计算库（NumPy, SciPy）和简洁的语法，成为了算法原型验证和教学的首选语言。本文将深入探讨这两大领域的经典算法，并提供基于 Python 的实战代码。

---

2. 路径规划：寻找最优解

路径规划通常分为全局规划（Global Planning）和局部规划（Local Planning）。全局规划在已知地图上寻找最优路径，而局部规划则处理动态避障和轨迹平滑。

2.1 基于搜索的算法：A* (A-Star)

A* 是最经典的图搜索算法，它结合了 Dijkstra 的广度优先搜索和贪婪最佳优先搜索（Greedy Best-First Search）的优点。

核心原理

A* 通过维护一个代价函数  来评估节点  的优先级：

• ：从起点到当前节点  的实际移动代价。
• ：从当前节点  到终点的预估代价（启发式函数，Heuristic）。

启发式函数的选择：

• 曼哈顿距离 (Manhattan)： 适用于只能上下左右移动的网格地图。
• 欧几里得距离 (Euclidean)： 适用于允许任意角度移动的空间。

Python 代码实现 (A* 算法)

下面是一个基于网格地图的 A* 算法实现。

import heapq
import math

class Node:
    def __init__(self, x, y, cost, parent_index):
        self.x = x
        self.y = y
        self.cost = cost  # g(n)
        self.parent_index = parent_index

    def __lt__(self, other):
        return self.cost < other.cost

class AStarPlanner:
    def __init__(self, ox, oy, resolution, rr):
        """
        ox: 障碍物 x 坐标列表
        oy: 障碍物 y 坐标列表
        resolution: 网格大小
        rr: 机器人半径
        """
        self.resolution = resolution
        self.rr = rr
        self.min_x, self.min_y = 0, 0
        self.max_x, self.max_y = 0, 0
        self.x_width, self.y_width = 0, 0
        self.obstacle_map = None
        self.calc_obstacle_map(ox, oy)
        self.motion = self.get_motion_model()

    class Node:
        def __init__(self, x, y, cost, parent_index):
            self.x = x  # 网格索引
            self.y = y  # 网格索引
            self.cost = cost
            self.parent_index = parent_index

        def __str__(self):
            return str(self.x) + "," + str(self.y) + "," + str(self.cost) + "," + str(self.parent_index)

    def planning(self, sx, sy, gx, gy):
        """
        sx, sy: 起点坐标
        gx, gy: 终点坐标
        """
        start_node = self.Node(self.calc_xy_index(sx, self.min_x),
                               self.calc_xy_index(sy, self.min_y), 0.0, -1)
        goal_node = self.Node(self.calc_xy_index(gx, self.min_x),
                              self.calc_xy_index(gy, self.min_y), 0.0, -1)

        open_set, closed_set = dict(), dict()
        open_set[self.calc_grid_index(start_node)] = start_node
        
        # 优先队列 (cost + heuristic, grid_index)
        pq = [] 
        heapq.heappush(pq, (self.calc_heuristic(start_node, goal_node), self.calc_grid_index(start_node)))

        whileTrue:
            ifnot open_set:
                print("Open set is empty..")
                break

            _, c_id = heapq.heappop(pq)
            current = open_set[c_id]

            # 到达目标
            if current.x == goal_node.x and current.y == goal_node.y:
                goal_node.parent_index = current.parent_index
                goal_node.cost = current.cost
                print("Goal Found!")
                return self.calc_final_path(goal_node, closed_set)

            # 将当前点移出 open_set，加入 closed_set
            del open_set[c_id]
            closed_set[c_id] = current

            # 扩展邻居节点
            for i, _ in enumerate(self.motion):
                node = self.Node(current.x + self.motion[i][0],
                                 current.y + self.motion[i][1],
                                 current.cost + self.motion[i][2], c_id)
                n_id = self.calc_grid_index(node)

                if n_id in closed_set:
                    continue

                ifnot self.verify_node(node):
                    continue

                if n_id notin open_set:
                    open_set[n_id] = node
                    priority = node.cost + self.calc_heuristic(node, goal_node)
                    heapq.heappush(pq, (priority, n_id))
                else:
                    if open_set[n_id].cost > node.cost:
                        open_set[n_id] = node
                        # 更新优先队列稍显复杂，这里简化处理，实际工程需优化更新逻辑

        returnNone, None

    def calc_heuristic(self, n1, n2):
        w = 1.0# 启发式权重
        d = w * math.hypot(n1.x - n2.x, n1.y - n2.y)
        return d

    def calc_obstacle_map(self, ox, oy):
        self.min_x = round(min(ox))
        self.min_y = round(min(oy))
        self.max_x = round(max(ox))
        self.max_y = round(max(oy))
        
        self.x_width = round((self.max_x - self.min_x) / self.resolution)
        self.y_width = round((self.max_y - self.min_y) / self.resolution)

        self.obstacle_map = [[Falsefor _ in range(self.y_width)]
                             for _ in range(self.x_width)]
        
        for ix in range(self.x_width):
            x = self.calc_grid_position(ix, self.min_x)
            for iy in range(self.y_width):
                y = self.calc_grid_position(iy, self.min_y)
                for iox, ioy in zip(ox, oy):
                    d = math.hypot(iox - x, ioy - y)
                    if d <= self.rr:
                        self.obstacle_map[ix][iy] = True
                        break

    def get_motion_model(self):
        # dx, dy, cost
        return [[1, 0, 1], [0, 1, 1], [-1, 0, 1], [0, -1, 1],
                [-1, -1, math.sqrt(2)], [-1, 1, math.sqrt(2)],
                [1, -1, math.sqrt(2)], [1, 1, math.sqrt(2)]]

    def calc_grid_position(self, index, min_pos):
        return index * self.resolution + min_pos

    def calc_xy_index(self, position, min_pos):
        return round((position - min_pos) / self.resolution)

    def calc_grid_index(self, node):
        return (node.y - self.min_y) * self.x_width + (node.x - self.min_x)

    def verify_node(self, node):
        px = self.calc_grid_position(node.x, self.min_x)
        py = self.calc_grid_position(node.y, self.min_y)
        if px < self.min_x or py < self.min_y or px >= self.max_x or py >= self.max_y:
            returnFalse
        if self.obstacle_map[node.x][node.y]:
            returnFalse
        returnTrue

    def calc_final_path(self, goal_node, closed_set):
        rx, ry = [self.calc_grid_position(goal_node.x, self.min_x)], [
            self.calc_grid_position(goal_node.y, self.min_y)]
        parent_index = goal_node.parent_index
        while parent_index != -1:
            n = closed_set[parent_index]
            rx.append(self.calc_grid_position(n.x, self.min_x))
            ry.append(self.calc_grid_position(n.y, self.min_y))
            parent_index = n.parent_index
        return rx, ry

# 使用示例（伪代码）：
# ox, oy = [障碍物坐标列表...]
# a_star = AStarPlanner(ox, oy, resolution=2.0, rr=1.0)
# rx, ry = a_star.planning(sx=10, sy=10, gx=50, gy=50)

2.2 基于采样的算法：RRT (Rapidly-exploring Random Tree)

对于高维空间（如机械臂）或非完整约束（如汽车不能原地自旋），A* 效率较低。RRT 及其变种 RRT* 通过随机采样快速覆盖空间。

RRT 的基本步骤：

1. 在空间中随机采样一个点 。
2. 在已生成的树中找到距离  最近的节点 。
3. 从  向  延伸一段固定步长，生成新节点 。
4. 如果  到  之间无碰撞，则将  加入树中。

---

3. 决策算法：赋予机器人逻辑

路径规划解决了“几何”问题，而决策算法解决“行为”问题。决策层通常位于规划层之上。

3.1 有限状态机 (Finite State Machine, FSM)

FSM 是最基础的决策模型。系统处于某一特定状态，当满足特定条件（Event）时，跳转到另一状态。

优点： 逻辑简单，易于实现。缺点： 当状态过多时，状态转换图会变得像“意大利面条”一样复杂，难以维护（State Explosion）。

Python FSM 代码示例

模拟一个简单的巡逻机器人：

import time
import random

# 定义状态基类
class State:
    def run(self, robot):
        pass
    def next(self, robot):
        pass

# 1. 巡逻状态
class PatrolState(State):
    def run(self, robot):
        print(f"[{robot.name}] 正在巡逻中... 电量: {robot.battery}%")
        robot.battery -= 10
        time.sleep(0.5)

    def next(self, robot):
        if robot.battery < 20:
            return ChargeState()
        if robot.detect_enemy():
            return AttackState()
        return self

# 2. 充电状态
class ChargeState(State):
    def run(self, robot):
        print(f"[{robot.name}] 正在充电... 电量: {robot.battery}%")
        robot.battery += 20
        time.sleep(0.5)

    def next(self, robot):
        if robot.battery >= 100:
            robot.battery = 100
            return PatrolState()
        return self

# 3. 攻击状态
class AttackState(State):
    def run(self, robot):
        print(f"[{robot.name}] 发现敌人！开火！")
        robot.battery -= 5
        time.sleep(0.5)

    def next(self, robot):
        ifnot robot.detect_enemy():
            print(f"[{robot.name}] 敌人已消除，继续巡逻。")
            return PatrolState()
        if robot.battery < 10:
            return ChargeState() # 紧急充电
        return self

# 机器人主体
class Robot:
    def __init__(self, name):
        self.name = name
        self.battery = 100
        self.state = PatrolState()
        
    def detect_enemy(self):
        # 模拟随机遇到敌人
        return random.random() < 0.1

    def update(self):
        self.state.run(self)
        self.state = self.state.next(self)

# 运行模拟
bot = Robot("Wall-E")
for _ in range(15):
    bot.update()

3.2 行为树 (Behavior Tree, BT)

为了解决 FSM 的扩展性问题，现代游戏 AI 和机器人系统（如 ROS 2 Navigation Stack）广泛采用行为树。

BT 是一棵树状结构，由控制节点（Control Nodes）和执行节点（Execution Nodes）组成。

• Sequence (->): 依次执行子节点，直到有一个失败。
• Selector (?): 依次执行子节点，直到有一个成功。

BT 的优势在于模块化，可以像搭积木一样组合复杂的逻辑。

---

4. 融合与应用：规划与决策的协同

在实际工程中，决策与规划是紧密耦合的闭环系统。

4.1 典型数据流

1. 感知层：激光雷达发现前方 5 米有障碍物。
2. 决策层：
• 当前状态：高速巡航。
• FSM/BT 逻辑：检测到障碍 -> 触发“换道”行为。
• 输出指令：目标点变更为左侧车道。
3. 规划层：
• 接收到新的目标点。
• 运行局部规划算法（如多项式曲线生成），生成一条平滑的换道轨迹。
4. 控制层：
• 使用 PID 或 MPC（模型预测控制）算法，控制方向盘和油门跟踪轨迹。

4.2 进阶：轨迹优化 (Trajectory Optimization)

单纯的 A* 生成的是几何路径（Path），是一系列坐标点。机器人需要的是轨迹（Trajectory），即包含时间、速度、加速度信息的路径。

Python 中常用 scipy.optimize 或 cvxpy 来进行轨迹优化，最常见的方法是最小化加加速度 (Minimum Jerk) 多项式轨迹。

这保证了机器人的运动平滑，不会出现剧烈的抖动，保护电机并提高控制精度。

---

5. Python 生态中的神器

如果不想从零造轮子，以下 Python 库是构建路径规划与决策系统的必备神器：

1. PythonRobotics: (GitHub Star 数极高) 收录了几乎所有主流的机器人算法（A*, RRT, D*, SLAM, MPC 等）的 Python 实现，非常适合学习。
2. NetworkX: 强大的图论库，虽然主要用于网络分析，但处理拓扑地图的路径搜索非常方便。
3. OMPL (Open Motion Planning Library): C++ 库，但有 Python 绑定。工业级的采样规划库（RRT, EST, KPIECE）。
4. Gym (OpenAI): 强化学习（Reinforcement Learning）的标准环境，如果你想尝试用 DQN 或 PPO 来做端到端的路径规划，这是必经之路。

6. 总结

路径规划赋予了机器“行走的智慧”，而决策算法赋予了机器“判断的灵魂”。

• 对于静态环境，A* 是首选。
• 对于复杂高维环境，RRT/RRT* 必不可少。
• 对于简单逻辑，FSM 足够好用。
• 对于复杂交互，请拥抱行为树。

Python 提供了极低的门槛让我们接触这些复杂的逻辑。无论是为了面试算法岗，还是开发自己的机器人 Demo，掌握这些核心算法都将是你技术武库中极具分量的一环。

⭐️ 好书推荐

《Python路径规划与决策算法》

在这里插入图片描述

【内容简介】

本书详细讲解了基于Python语言实现自动驾驶路径规划与决策算法的知识，并通过具体实例展示了这些算法的使用方法。本书共10章，依次讲解了Dijkstra算法、A算法、Bellman-Ford算法、Floyd-Warshall算法、D算法、D* Lite算法、RRT算法、其他路径规划算法、智能行为决策算法、基于SLAM的自主路径导航系统。