RuyiSDK Board Docs

RISC-V ROS2 机器人操作系统编程技术

Source repository

ch18 · 全局路径规划

TextbookProgramming languageEnvironmentLessonLab
RISC-VC++17SpacemiT K3 CoM260 Kit / Bianbu 4.0.6 / Humble, with an x86 Ubuntu 22.04 / Humble / Harmonic course containerRead lessonStart lab
x86PythonUbuntu 22.04 / Humble or Ubuntu 24.04 / JazzyRead lessonStart lab

This document is not yet available in English. The Chinese original is shown below.

第18章 全局路径规划

课程:ROS2 Python 编程
章节:第18章
课时:2 课时(90 分钟)
教学方式:讲授 + 演示


学习目标

本章学习目标包括:理解Dijkstra算法和A*算法的原理与实现,掌握Nav2 Smac Planner的工作原理,熟悉全局路径规划的代价函数设计,能够配置和切换不同的全局规划器,掌握路径优化和平滑技术。

18.1 全局路径规划概述

18.1.1 问题定义

全局路径规划是在已知环境地图中,寻找从起点到终点的无碰撞最优路径。

**输入:**环境地图(代价地图)、起点位姿 (x_start, y_start, θ_start) 和终点位姿 (x_goal, y_goal, θ_goal)。

**输出:**路径,即一系列连续的位姿点 {(x₁,y₁,θ₁), ..., (xₙ,yₙ,θₙ)}。

优化目标:

path* = argmin f(path)

其中 f(path) 通常为路径总长度,也可包含其他约束(如转弯代价、安全距离)。

18.1.2 路径规划的评价指标

指标 含义 重要性
路径长度 从起点到终点的总距离 高
计算时间 规划所需时间 高(实时性)
平滑度 路径曲率变化 中
安全距离 距离障碍物的最近距离 高
完备性 是否存在可行解 高

18.2 Dijkstra算法

18.2.1 算法原理

Dijkstra算法是一种广度优先的最短路径搜索算法,它从起点开始,逐步扩展,保证找到最短路径。

**核心思想:**维护从起点到每个节点的最短距离 G(n),每次选择 G(n) 最小的节点进行扩展,直到扩展到目标点。

伪代码:

1. 初始化:
   - open_list = {start}
   - closed_list = {}
   - g(start) = 0, g(其他) = ∞
   
2. while open_list 不为空:
   3. 从 open_list 中取出 g 值最小的节点 n
   4. if n == goal: 回溯路径,返回成功
   5. 将 n 移入 closed_list
   6. 遍历 n 的相邻节点 m:
     7. if m in closed_list: continue
     8. if m 是障碍物: continue
     9. new_g = g(n) + cost(n, m)
     10. if new_g < g(m):
         11. g(m) = new_g
         12. parent(m) = n
         13. if m not in open_list: 加入 open_list
         
3. return 失败 (无可行路径)

18.2.2 Python实现

import numpy as np
import heapq
from enum import Enum
 
class CellState(Enum):
    FREE = 0
    OBSTACLE = 1
    START = 2
    GOAL = 3
    PATH = 4
 
class DijkstraPlanner:
    """Dijkstra全局路径规划器"""
    def __init__(self, costmap: np.ndarray, resolution: float = 0.05):
        self.costmap = costmap
        self.resolution = resolution
        self.height, self.width = costmap.shape
        
        # 8邻域连接
        self.neighbors = [
            (-1, -1), (-1, 0), (-1, 1),
            (0, -1),           (0, 1),
            (1, -1),  (1, 0),  (1, 1)
        ]
        self.movement_costs = [
            np.sqrt(2), 1.0, np.sqrt(2),
            1.0,              1.0,
            np.sqrt(2), 1.0, np.sqrt(2)
        ]
        
    def world_to_grid(self, wx: float, wy: float) -> tuple:
        """世界坐标转栅格坐标"""
        # 假设地图原点在 (0, 0)
        return int(wy / self.resolution), int(wx / self.resolution)
    
    def grid_to_world(self, row: int, col: int) -> tuple:
        """栅格坐标转世界坐标"""
        return col * self.resolution, row * self.resolution
    
    def plan(self, start_world: tuple, goal_world: tuple) -> list:
        """
        执行Dijkstra路径规划
        
        参数:
            start_world: (x, y) 起点世界坐标
            goal_world: (x, y) 终点世界坐标
            
        返回:
            path: [(x1,y1), (x2,y2), ...] 世界坐标路径点
        """
        start = self.world_to_grid(*start_world)
        goal = self.world_to_grid(*goal_world)
        
        if not self._is_valid(start) or not self._is_valid(goal):
            print("起点或终点无效")
            return []
            
        # 初始化
        g_score = np.full((self.height, self.width), np.inf)
        g_score[start] = 0
        
        parent = {}
        
        # 优先队列: (g_score, (row, col))
        open_heap = [(0, start)]
        closed_set = set()
        
        while open_heap:
            # 取出g值最小的节点
            current_g, current = heapq.heappop(open_heap)
            
            if current in closed_set:
                continue
                
            # 到达目标
            if current == goal:
                return self._reconstruct_path(parent, start, goal)
                
            closed_set.add(current)
            
            # 扩展邻居
            for i, (dr, dc) in enumerate(self.neighbors):
                neighbor = (current[0] + dr, current[1] + dc)
                
                if neighbor in closed_set:
                    continue
                    
                if not self._is_valid(neighbor):
                    continue
                    
                # 代价 = 移动代价 + 地图代价
                movement_cost = self.movement_costs[i]
                map_cost = self._get_map_cost(neighbor)
                step_cost = movement_cost * (1.0 + map_cost / 50.0)
                
                tentative_g = current_g + step_cost
                
                if tentative_g < g_score[neighbor]:
                    g_score[neighbor] = tentative_g
                    parent[neighbor] = current
                    heapq.heappush(open_heap, (tentative_g, neighbor))
                    
        return []  # 无可行路径
    
    def _is_valid(self, cell: tuple) -> bool:
        """检查栅格是否有效"""
        r, c = cell
        if not (0 <= r < self.height and 0 <= c < self.width):
            return False
        return self.costmap[r, c] < 254  # 非障碍物
    
    def _get_map_cost(self, cell: tuple) -> float:
        """获取地图代价值"""
        r, c = cell
        cost = self.costmap[r, c]
        if cost >= 254:
            return float('inf')
        return float(cost)
    
    def _reconstruct_path(self, parent: dict, start: tuple, 
                           goal: tuple) -> list:
        """回溯重建路径"""
        path_grid = []
        current = goal
        
        while current != start:
            path_grid.append(current)
            current = parent.get(current)
            if current is None:
                return []
        path_grid.append(start)
        path_grid.reverse()
        
        # 转换为世界坐标
        path_world = [self.grid_to_world(r, c) for r, c in path_grid]
        return path_world

18.2.3 Dijkstra的特点

**优点:**保证找到最短路径,算法完备(有解必能找到),实现简单。

**缺点:**搜索范围大、效率低,不考虑目标位置而向所有方向等距离扩展,在大地图上计算时间过长。

18.3 A*算法

18.3.1 算法原理

A*算法在Dijkstra的基础上加入启发式函数,引导搜索方向,提高效率。

评价函数:

F(n) = G(n) + H(n)

其中 G(n) 表示起点到当前节点n的实际代价,H(n) 表示当前节点n到目标点的启发式估计代价。

启发式函数的选择:

def manhattan_distance(a: tuple, b: tuple) -> float:
    """曼哈顿距离(四邻域连接时使用)"""
    return abs(a[0] - b[0]) + abs(a[1] - b[1])
 
def euclidean_distance(a: tuple, b: tuple) -> float:
    """欧氏距离(八邻域连接时使用)"""
    return np.sqrt((a[0] - b[0])**2 + (a[1] - b[1])**2)
 
def chebyshev_distance(a: tuple, b: tuple) -> float:
    """切比雪夫距离"""
    return max(abs(a[0] - b[0]), abs(a[1] - b[1]))
 
def diagonal_distance(a: tuple, b: tuple, d1: float = 1.0, 
                       d2: float = np.sqrt(2)) -> float:
    """对角线距离(考虑对角线移动)"""
    dx = abs(a[0] - b[0])
    dy = abs(a[1] - b[1])
    return d1 * (dx + dy) + (d2 - 2 * d1) * min(dx, dy)

18.3.2 Python实现

import heapq
import numpy as np
 
class AStarPlanner:
    """A*路径规划器"""
    def __init__(self, costmap: np.ndarray, resolution: float = 0.05):
        self.costmap = costmap
        self.resolution = resolution
        self.height, self.width = costmap.shape
        
        # 8邻域
        self.neighbors = [
            (-1, -1), (-1, 0), (-1, 1),
            (0, -1),           (0, 1),
            (1, -1),  (1, 0),  (1, 1)
        ]
        self.movement_costs = [
            np.sqrt(2), 1.0, np.sqrt(2),
            1.0,              1.0,
            np.sqrt(2), 1.0, np.sqrt(2)
        ]
        
    def plan(self, start_world: tuple, goal_world: tuple) -> list:
        """执行A*路径规划"""
        start = self._world_to_grid(*start_world)
        goal = self._world_to_grid(*goal_world)
        
        if not self._is_valid(start) or not self._is_valid(goal):
            return []
            
        g_score = {start: 0.0}
        f_score = {start: self._heuristic(start, goal)}
        
        open_heap = [(f_score[start], start)]
        open_set = {start}
        closed_set = set()
        parent = {}
        
        while open_heap:
            _, current = heapq.heappop(open_heap)
            
            if current == goal:
                return self._reconstruct_path(parent, start, goal)
                
            open_set.discard(current)
            closed_set.add(current)
            
            for i, (dr, dc) in enumerate(self.neighbors):
                neighbor = (current[0] + dr, current[1] + dc)
                
                if neighbor in closed_set:
                    continue
                    
                if not self._is_valid(neighbor):
                    continue
                    
                move_cost = self.movement_costs[i]
                map_cost = self._get_map_cost(neighbor)
                step_cost = move_cost * (1.0 + map_cost / 50.0)
                
                tentative_g = g_score[current] + step_cost
                
                if neighbor not in g_score or tentative_g < g_score[neighbor]:
                    parent[neighbor] = current
                    g_score[neighbor] = tentative_g
                    f_score[neighbor] = tentative_g + self._heuristic(neighbor, goal)
                    
                    if neighbor not in open_set:
                        heapq.heappush(open_heap, (f_score[neighbor], neighbor))
                        open_set.add(neighbor)
                        
        return []
    
    def _heuristic(self, cell: tuple, goal: tuple) -> float:
        """启发式函数:对角线距离"""
        dr = abs(cell[0] - goal[0])
        dc = abs(cell[1] - goal[1])
        return (1.0 * (dr + dc) + (np.sqrt(2) - 2 * 1.0) * min(dr, dc))
    
    def _world_to_grid(self, wx: float, wy: float) -> tuple:
        return int(wy / self.resolution), int(wx / self.resolution)
    
    def _grid_to_world(self, row: int, col: int) -> tuple:
        return col * self.resolution, row * self.resolution
    
    def _is_valid(self, cell: tuple) -> bool:
        r, c = cell
        if not (0 <= r < self.height and 0 <= c < self.width):
            return False
        return self.costmap[r, c] < 254
    
    def _get_map_cost(self, cell: tuple) -> float:
        r, c = cell
        cost = self.costmap[r, c]
        return float('inf') if cost >= 254 else float(cost)
    
    def _reconstruct_path(self, parent: dict, start: tuple, 
                           goal: tuple) -> list:
        path_grid = []
        current = goal
        while current != start:
            path_grid.append(current)
            current = parent.get(current)
            if current is None:
                return []
        path_grid.append(start)
        path_grid.reverse()
        
        return [self._grid_to_world(r, c) for r, c in path_grid]

18.3.3 A* vs Dijkstra

def compare_algorithms(costmap: np.ndarray, 
                        start: tuple, goal: tuple):
    """对比Dijkstra和A*的性能"""
    import time
    
    # Dijkstra
    dijkstra = DijkstraPlanner(costmap)
    t1 = time.time()
    path_d = dijkstra.plan(start, goal)
    t_d = time.time() - t1
    
    # A*
    astar = AStarPlanner(costmap)
    t1 = time.time()
    path_a = astar.plan(start, goal)
    t_a = time.time() - t1
    
    print(f"Dijkstra: {len(path_d)}点, {t_d*1000:.1f}ms")
    print(f"A*:       {len(path_a)}点, {t_a*1000:.1f}ms")
    
    if path_d and path_a:
        len_d = sum(np.linalg.norm(np.array(path_d[i+1]) - np.array(path_d[i]))
                    for i in range(len(path_d)-1))
        len_a = sum(np.linalg.norm(np.array(path_a[i+1]) - np.array(path_a[i]))
                    for i in range(len(path_a)-1))
        print(f"Dijkstra路径长度: {len_d:.2f}m")
        print(f"A*路径长度:       {len_a:.2f}m")

18.3.4 官方要点——A* 的理论根基与启发式可采纳性

A* 出自 Hart、Nilsson 与 Raphael 1968 年在 SRI 的论文《A Formal Basis for the Heuristic Determination of Minimum Cost Paths》,其核心结论是:启发式 h(n) 若"可采纳"(不高估真实代价,admissible),A* 保证找到最优路径;若再满足一致性,则首次扩展节点即为最优。欧氏距离是 8 邻域可采纳启发式,而切比雪夫距离或对角线加权(如本章 18.3.2 的 √2 对角代价)在 8 邻域中更贴合实际步长,能显著减少无效扩展——这正是练习第 1 题的理论支撑。

官方文档同时提醒:在代价地图上 A* 的"最优"只是相对栅格代价的最优,若膨胀层 cost_scaling_factor 配置不当,规划器会为绕开低代价区走出怪异长路;调路径形状应先调代价地图,再考虑换规划器。

18.4 Nav2 Smac Planner

18.4.1 Smac Planner概述

Smac Planner 是Nav2内置的高级规划器,支持多种规划算法:

规划器 算法 特点 适用场景
NavfnPlanner 导航函数 经典算法,路径平滑 简单环境
SmacPlanner2D A* 标准A*在2D代价地图 通用场景
SmacPlannerHybrid Hybrid-A* 带运动学约束 非全向机器人
SmacPlannerLattice 状态格 满足动力学约束 阿克曼转向
ThetaStarPlanner Theta* 任意角度路径 全向机器人

18.4.2 Hybrid-A*算法

Hybrid-A* 在标准A*基础上加入运动学约束,适用于非全向移动机器人。

**与A*的区别:**状态空间从 (x, y) 扩展到 (x, y, θ),考虑最小转弯半径,支持前进和后退,并使用Reeds-Shepp曲线连接。

import numpy as np
import heapq
 
class HybridAStarNode:
    """Hybrid-A*节点"""
    def __init__(self, x: float, y: float, theta: float):
        self.x = x
        self.y = y
        self.theta = theta
        self.g = float('inf')
        self.f = float('inf')
        self.parent = None
        
    def __lt__(self, other):
        return self.f < other.f
 
class HybridAStarPlanner:
    """Hybrid-A*路径规划器(简化版)"""
    def __init__(self, costmap: np.ndarray, 
                 min_turning_radius: float = 0.5,
                 resolution: float = 0.05):
        self.costmap = costmap
        self.resolution = resolution
        self.min_turning_radius = min_turning_radius
        
        # 运动控制采样(速度+转角)
        self.motion_primitives = self._generate_motion_primitives()
        
    def _generate_motion_primitives(self) -> list:
        """生成运动基元"""
        primitives = []
        
        # 前进
        for steer in [-0.4, -0.2, 0.0, 0.2, 0.4]:
            primitives.append({
                'v': 0.3,            # 前进速度
                'steer': steer,       # 转角
                'dt': 0.5,           # 时间步长
                'gear': 1            # 前进
            })
            
        # 后退
        for steer in [-0.2, 0.0, 0.2]:
            primitives.append({
                'v': -0.2,
                'steer': steer,
                'dt': 0.3,
                'gear': -1
            })
            
        return primitives
    
    def _simulate_motion(self, x: float, y: float, theta: float,
                          primitive: dict) -> tuple:
        """模拟运动基元"""
        v = primitive['v']
        steer = primitive['steer']
        dt = primitive['dt']
        
        # 自行车模型
        new_x = x + v * np.cos(theta) * dt
        new_y = y + v * np.sin(theta) * dt
        new_theta = theta + v * np.tan(steer) / 0.5 * dt
        
        return new_x, new_y, new_theta
    
    def _is_collision_free(self, x: float, y: float) -> bool:
        """检查位姿是否无碰撞"""
        gx = int(y / self.resolution)
        gy = int(x / self.resolution)
        
        if not (0 <= gx < self.costmap.shape[0] and 
                0 <= gy < self.costmap.shape[1]):
            return False
            
        # 检查足迹范围内是否有障碍物
        robot_radius_cells = int(0.22 / self.resolution)
        for dr in range(-robot_radius_cells, robot_radius_cells + 1):
            for dc in range(-robot_radius_cells, robot_radius_cells + 1):
                if dr*dr + dc*dc <= robot_radius_cells*robot_radius_cells:
                    nr, nc = gx + dr, gy + dc
                    if (0 <= nr < self.costmap.shape[0] and 
                        0 <= nc < self.costmap.shape[1]):
                        if self.costmap[nr, nc] >= 254:
                            return False
        return True
    
    def plan(self, start: tuple, goal: tuple) -> list:
        """
        执行Hybrid-A*规划
        
        参数:
            start: (x, y, theta)
            goal: (x, y, theta)
        """
        start_node = HybridAStarNode(*start)
        goal_node = HybridAStarNode(*goal)
        
        # 离散化分辨率
        xy_resolution = 0.2  # 比代价地图粗
        theta_resolution = np.radians(15)
        
        # 离散化状态空间
        def discretize(x, y, theta):
            dx = int(x / xy_resolution)
            dy = int(y / xy_resolution)
            dt = int(theta / theta_resolution)
            return (dx, dy, dt)
        
        start_node.g = 0.0
        start_node.f = self._heuristic(start, goal)
        
        open_heap = [(start_node.f, id(start_node), start_node)]
        open_dict = {discretize(*start): start_node}
        closed_set = set()
        
        while open_heap:
            _, _, current = heapq.heappop(open_heap)
            current_key = discretize(current.x, current.y, current.theta)
            
            if current_key in closed_set:
                continue
                
            # 检查是否到达目标
            dist_to_goal = np.sqrt((current.x - goal[0])**2 + 
                                   (current.y - goal[1])**2)
            if dist_to_goal < 0.5:
                # 使用Reeds-Shepp曲线连接
                return self._reconstruct_path(current)
                
            closed_set.add(current_key)
            
            # 扩展运动基元
            for primitive in self.motion_primitives:
                new_x, new_y, new_theta = self._simulate_motion(
                    current.x, current.y, current.theta, primitive
                )
                
                # 碰撞检测
                if not self._is_collision_free(new_x, new_y):
                    continue
                    
                new_key = discretize(new_x, new_y, new_theta)
                
                if new_key in closed_set:
                    continue
                    
                # 计算代价
                motion_cost = abs(primitive['v']) * primitive['dt']
                steering_penalty = abs(primitive['steer']) * 0.5
                step_cost = motion_cost + steering_penalty
                
                new_g = current.g + step_cost
                
                if new_key not in open_dict or new_g < open_dict[new_key].g:
                    new_node = HybridAStarNode(new_x, new_y, new_theta)
                    new_node.g = new_g
                    new_node.f = new_g + self._heuristic(
                        (new_x, new_y), (goal[0], goal[1])
                    )
                    new_node.parent = current
                    
                    open_dict[new_key] = new_node
                    heapq.heappush(open_heap, (new_node.f, id(new_node), new_node))
                    
        return []
    
    def _heuristic(self, a: tuple, b: tuple) -> float:
        """启发式函数:欧氏距离"""
        return np.sqrt((a[0] - b[0])**2 + (a[1] - b[1])**2)
    
    def _reconstruct_path(self, node: HybridAStarNode) -> list:
        """重建路径"""
        path = []
        current = node
        while current:
            path.append((current.x, current.y, current.theta))
            current = current.parent
        path.reverse()
        return path

18.4.3 Nav2规划器配置

# 规划器配置
planner_server:
  ros__parameters:
    planner_plugins: ["GridBased"]
    
    GridBased:
      plugin: "nav2_navfn_planner/NavfnPlanner"
      tolerance: 0.5                # 目标容忍距离
      use_astar: true               # 使用A*加速
      allow_unknown: false          # 不允许未知区域
      
    # Smac Planner Hybrid可选配置
    # GridBased:
    #   plugin: "nav2_smac_planner/SmacPlannerHybrid"
    #   tolerance: 0.5
    #   max_planning_time: 2.0       # 最大规划时间(s)
    #   minimum_turning_radius: 0.2  # 最小转弯半径(m)
    #   reverse_penalty: 2.0         # 后退惩罚
    #   change_penalty: 0.5          # 方向变化惩罚
    #   non_straight_penalty: 1.1    # 非直线惩罚

18.4.4 规划器切换

# 查看当前使用的规划器
ros2 param get /planner_server planner_plugins
 
# 动态切换规划器
ros2 param set /planner_server planner_plugins "['SmacPlanner']"
 
# 重新配置后需要重新规划

18.4.5 官方要点——Nav2 官方规划器全家福与选型指引

官方 Planner Tables 页面对比了四类内置全局规划器:NavFn(Dijkstra/A* 的代价地图搜索实现,速度优先、质量一般)、Smac 2D(栅格 A*,支持 4/8 邻域,比 NavFn 更省内存)、Smac Hybrid-A*(运动学约束,供阿克曼/有最小转弯半径的底盘使用)、Smac Lattice(状态栅格法,用预生成运动基元搜索,适合差速与全向机器人)。文档给出的选型原则与本章 18.4 一致:圆形差速底盘用 NavFn/Smac 2D 足够;需考虑朝向与转弯半径时才引入 Hybrid-A*;Lattice 在要求路径可精确复现时最合适。

nav2_navfn_planner 文档还说明了 tolerance 参数的语义:目标不可达时按该半径搜索"最近可达点"并直接规划过去,这与本章 18.6.2 的 GridBased.tolerance 命令行示例对应。

18.4.6 官方要点——Smac Hybrid-A* 的运动学细节

nav2_smac_planner 文档对 Hybrid-A* 给出了与本章 18.4.2 呼应的参数语义:motion_model_for_search_type 选择 DUBIN(前向 Only)/REEDS_SHEPP(允许倒车)等运动模型;minimum_turning_radius 决定可行运动基元的曲率下限,设置过小会产生底盘物理上转不出来的急弯;reverse_penalty、change_penalty、non_straight_penalty 分别惩罚倒车、朝向突变与非直行段,是"路径像不像人开车"的直接旋钮。文档建议的调参顺序是:先把 analytic_expansion_ratio 与 analytic_expansion_max_length 调到能稳定生成解析端点(Dubins/Reeds-Shepp 直连段),再逐步收紧其余惩罚项。

Smac Lattice 则依赖离线生成的运动基元(Motion Primitives)文件,基元分辨率必须与 costmap 分辨率匹配,这是配置时最常见的坑。

18.5 路径优化

18.5.1 路径平滑

路径平滑去除路径中的锐角转折:

import numpy as np
from scipy import interpolate
 
class PathSmoother:
    """路径平滑器"""
    def __init__(self, weight_data: float = 0.5,
                 weight_smooth: float = 0.3,
                 weight_length: float = 0.2):
        self.weight_data = weight_data
        self.weight_smooth = weight_smooth
        self.weight_length = weight_length
        
    def smooth(self, path: np.ndarray, 
               num_iterations: int = 100) -> np.ndarray:
        """
        梯度下降路径平滑
        
        参数:
            path: (N, 2) 原始路径点
            num_iterations: 迭代次数
        """
        new_path = path.copy()
        n = len(path)
        
        for _ in range(num_iterations):
            for i in range(1, n - 1):
                # 保持接近原始点
                d_data = self.weight_data * (path[i] - new_path[i])
                
                # 平滑项(与邻居的差距)
                d_smooth = self.weight_smooth * (
                    new_path[i-1] + new_path[i+1] - 2 * new_path[i]
                )
                
                # 长度约束(均匀间距)
                d_length = self.weight_length * (
                    new_path[i-1] - 2 * new_path[i] + new_path[i+1]
                )
                
                new_path[i] += d_data + d_smooth + d_length
                
        return new_path
    
    def b_spline_smooth(self, path: np.ndarray, 
                         num_points: int = 100) -> np.ndarray:
        """B样条平滑"""
        n = len(path)
        t = np.linspace(0, 1, n)
        t_new = np.linspace(0, 1, num_points)
        
        # B样条插值
        tck, _ = interpolate.splprep([path[:, 0], path[:, 1]], s=0)
        smooth_points = interpolate.splev(t_new, tck)
        
        return np.column_stack(smooth_points)

18.5.2 路径修剪

class PathOptimizer:
    """路径优化器"""
    def __init__(self):
        pass
        
    def remove_redundant_points(self, path: np.ndarray, 
                                  epsilon: float = 0.1) -> np.ndarray:
        """
        移除冗余路径点(Douglas-Peucker算法)
        """
        if len(path) < 3:
            return path
            
        # 找最远点
        start, end = path[0], path[-1]
        line_vec = end - start
        line_len = np.linalg.norm(line_vec)
        
        if line_len == 0:
            return np.array([start, end])
            
        line_unit = line_vec / line_len
        
        max_dist = 0
        max_idx = 0
        
        for i in range(1, len(path) - 1):
            vec = path[i] - start
            proj_len = np.dot(vec, line_unit)
            proj_point = start + proj_len * line_unit
            dist = np.linalg.norm(path[i] - proj_point)
            
            if dist > max_dist:
                max_dist = dist
                max_idx = i
                
        if max_dist > epsilon:
            # 递归
            left = self.remove_redundant_points(path[:max_idx + 1], epsilon)
            right = self.remove_redundant_points(path[max_idx:], epsilon)
            return np.vstack((left[:-1], right))
        else:
            return np.array([start, end])
    
    def interpolate_path(self, path: np.ndarray, 
                          step_size: float = 0.1) -> np.ndarray:
        """路径插值(均匀间距)"""
        if len(path) < 2:
            return path
            
        new_path = [path[0]]
        remaining = 0.0
        
        for i in range(len(path) - 1):
            segment = path[i+1] - path[i]
            seg_len = np.linalg.norm(segment)
            
            if seg_len == 0:
                continue
                
            seg_unit = segment / seg_len
            pos = path[i]
            
            while remaining < seg_len:
                pos = pos + seg_unit * step_size
                new_path.append(pos.copy())
                remaining += step_size
                
            remaining -= seg_len
            
        new_path.append(path[-1])
        return np.array(new_path)

18.5.3 代价感知路径优化

class CostAwarePathOptimizer:
    """代价感知路径优化"""
    def __init__(self, costmap: np.ndarray, resolution: float = 0.05):
        self.costmap = costmap
        self.resolution = resolution
        
    def optimize_path(self, path: np.ndarray, 
                       safety_margin: float = 0.3) -> np.ndarray:
        """
        调整路径点,使其远离高代价区域
        """
        optimized = path.copy()
        
        for iteration in range(50):
            max_change = 0
            
            for i in range(1, len(path) - 1):
                # 获取当前点的代价
                cost = self._get_cost(path[i])
                
                # 如果处于高代价区域,向外推
                if cost > 50:
                    # 计算法线方向
                    prev_vec = path[i] - path[i-1]
                    next_vec = path[i+1] - path[i]
                    
                    # 法线 = 垂直路径方向
                    tangent = (next_vec + prev_vec) / 2
                    if np.linalg.norm(tangent) > 0:
                        normal = np.array([-tangent[1], tangent[0]])
                        normal = normal / np.linalg.norm(normal)
                    else:
                        normal = np.zeros(2)
                        
                    # 朝低代价方向移动
                    push_force = min(cost / 100.0, 0.5) * 0.05
                    optimized[i] += normal * push_force
                    
                    change = np.linalg.norm(optimized[i] - path[i])
                    max_change = max(max_change, change)
                    
            if max_change < 0.001:
                break
                
        return optimized
    
    def _get_cost(self, point: np.ndarray) -> float:
        """获取栅格代价值"""
        gx = int(point[1] / self.resolution)
        gy = int(point[0] / self.resolution)
        
        if (0 <= gx < self.costmap.shape[0] and 
            0 <= gy < self.costmap.shape[1]):
            return float(self.costmap[gx, gy])
        return 255

18.5.4 官方要点——路径平滑与规划器协同

Nav2 官方文档把后处理交给独立的 smoother_server:内置 Simple Smoother(基于共轭梯度的迭代平滑,避免路径"抄近道"穿墙,通过 smoother.w_smooth/smoother.w_data 权衡平滑度与贴合度)、Conjugate Gradient Smoother 与可选的 TH*-like 换代实现。官方默认行为树中 ComputePathToPose 之后常串联 SmoothPath Action,使"折线路径 + 平滑输出"成为标准管线——练习第 6 题的三层框架(A* 粗规划 → Hybrid-A* 细化 → 平滑器后处理)在 Nav2 中可以直接用"planner_server + smoother_server"两个服务拼接实现。Robotics Back-End 的对比实验还指出:先平滑后跟踪时,控制器(DWB/RPP/MPPI)的横向误差显著降低,平滑器不是可选装饰而是规划-控制之间的质量缓冲。

18.6 实战案例

18.6.1 路径规划在Nav2中的完整流程

import rclpy
from rclpy.node import Node
from nav_msgs.msg import Path, OccupancyGrid
from geometry_msgs.msg import PoseStamped
import numpy as np
 
class GlobalPathPlannerDemo(Node):
    """全局路径规划演示节点"""
    def __init__(self):
        super().__init__('global_planner_demo')
        
        self.costmap_sub = self.create_subscription(
            OccupancyGrid, '/global_costmap/costmap',
            self.costmap_callback, 10)
        self.path_pub = self.create_publisher(Path, '/planned_path', 10)
        
        self.costmap = None
        self.planner = None
        
    def costmap_callback(self, msg: OccupancyGrid):
        """接收代价地图并执行规划"""
        if self.costmap is None:
            h, w = msg.info.height, msg.info.width
            data = np.array(msg.data).reshape(h, w)
            self.costmap = data
            self.planner = AStarPlanner(self.costmap, msg.info.resolution)
            
            # 示例规划
            start = (0.0, 0.0)
            goal = (3.0, 2.0)
            
            path = self.planner.plan(start, goal)
            
            if path:
                self.publish_path(path, msg.header.frame_id)
                self.get_logger().info(f'路径规划成功: {len(path)}点')
                
    def publish_path(self, path_world: list, frame_id: str):
        """发布规划路径"""
        msg = Path()
        msg.header.frame_id = frame_id
        msg.header.stamp = self.get_clock().now().to_msg()
        
        for x, y in path_world:
            pose = PoseStamped()
            pose.header = msg.header
            pose.pose.position.x = x
            pose.pose.position.y = y
            pose.pose.orientation.w = 1.0
            msg.poses.append(pose)
            
        self.path_pub.publish(msg)

18.6.2 命令行调试

# 查看规划路径
ros2 topic echo /plan --once
 
# 查看代价地图
ros2 topic echo /global_costmap/costmap --once
 
# 查看当前规划器参数
ros2 param describe /planner_server GridBased.plugin
 
# 设置规划容忍度
ros2 param set /planner_server GridBased.tolerance 0.3

18.6.3 常见问题

问题1:路径规划超时:可降低代价地图分辨率、减小搜索范围,或使用更高效的规划器。

问题2:路径穿过障碍物:多为膨胀半径过小,应检查代价地图是否正确更新,并增加规划容忍度。

问题3:路径不平滑:可使用路径平滑器后处理、改用Hybrid-A*规划器,或降低代价地图分辨率。

课后练习

  1. 原理题: 比较Dijkstra算法和A*算法的异同,说明启发式函数H(n)如何影响搜索效率和路径质量。

  2. 编程题: 实现A*路径规划算法,在给定代价地图上搜索从起点到终点的最短路径,并可视化路径。

  3. 分析题: 分析Hybrid-A与标准A的区别,说明为什么非全向机器人需要Hybrid-A*规划器。

  4. 配置题: 在Nav2中配置Smac Planner Hybrid,调整最小转弯半径、后退惩罚等参数以适应差速机器人。

  5. 操作题: 在仿真环境中使用不同的全局规划器进行导航,对比各规划器生成路径的平滑度和计算时间。

  6. 设计题: 设计一个三层路径规划框架:上层(全局搜索)使用A在中分辨率地图规划,中层使用Hybrid-A细化路径,下层使用路径平滑器后处理。


仿真结合实例(当前仓库):Nav2 GridBased 规划器生成路径

目标与知识点对应

使用预置 Museum 地图和 Nav2 的 GridBased/NavFn 全局规划器,从地图坐标目标生成无碰撞路径,观察规划器、地图服务器与 BT Navigator 的连接。

运行步骤

source /opt/ros/jazzy/setup.bash
source install/setup.bash
ros2 launch navigation_sim_demo_ros2 nav2_demo.launch.py \
  use_gazebo:=true use_rviz:=true gz_headless:=false
ros2 run navigation_sim_demo_ros2 nav_goal_runner \
  --ros-args -p goal_x:=1.0 -p goal_y:=0.0
ros2 topic echo /plan --once

观察结果

RViz 中可观察全局路径;目标是否被接受、是否产生 /plan 以及机器人是否运动,要分别看 Action、Path 和 /odom 输出。

源码与边界

Nav2 参数位于 src/navigation_sim_demo_ros2/params/nav2_params.yaml,目标节点位于 src/navigation_sim_demo_ros2/navigation_sim_demo_ros2/nav_goal_runner.py,地图位于 src/navigation_sim_demo_ros2/maps/Software_Museum.yaml。

路径规划结果受初始位姿、TF 和仿真启动时序影响,不能只凭启动成功判断规划完成。

学习材料: