🧭 第17课：路径规划(全局)

📋 课程目标

• 理解全局路径规划的原理与分类
• 掌握NavFn、SmacPlanner2D、Hybrid-A*的使用
• 学会自定义全局规划器插件
• 理解不同规划器的适用场景与性能差异
• 掌握规划器参数调优方法

🧠 什么是全局路径规划？

全局路径规划(Global Planner)是在已知地图上，从起点到终点计算出一条无碰撞的最优路径。它是导航系统的"大脑"，决定了机器人要走的大方向。

📐 经典路径规划算法

🔧 Nav2全局规划器配置

1. NavFn Planner（经典A*）

# nav2_params.yaml - NavFn配置
planner_server:
  ros__parameters:
    expected_planner_frequency: 1.0
    planner_plugins: ["GridBased"]
    GridBased:
      plugin: "nav2_navfn_planner/NavfnPlanner"
      tolerance: 0.5
      use_astar: true
      allow_unknown: true

2. SmacPlannerHybrid（Hybrid-A*）

# Hybrid-A*配置 - 考虑运动学约束
planner_server:
  ros__parameters:
    planner_plugins: ["GridBased"]
    GridBased:
      plugin: "nav2_smac_planner/SmacPlannerHybrid"
      allow_unknown: true
      max_iterations: 1000000
      max_planning_time: 5.0
      motion_model_for_search: "REEDS_SHEPP"
      minimum_turning_radius: 0.4
      reverse_penalty: 2.0
      non_straight_penalty: 1.2
      cost_penalty: 2.0
      analytic_expansion_max_length: 3.0

🐍 Python：A*规划器实现

#!/usr/bin/env python3
# A*路径规划器 - 2D栅格地图
import heapq, math, numpy as np

class AStarPlanner:
    def __init__(self, grid, resolution, origin):
        self.grid = grid
        self.resolution = resolution
        self.origin = origin
        self.h, self.w = grid.shape
        self.occ_thresh = 50

    def w2g(self, wx, wy):
        return int((wx-self.origin[0])/self.resolution), int((wy-self.origin[1])/self.resolution)

    def g2w(self, gx, gy):
        return gx*self.resolution+self.origin[0]+self.resolution/2, \
               gy*self.resolution+self.origin[1]+self.resolution/2

    def heuristic(self, a, b):
        return math.hypot(a[0]-b[0], a[1]-b[1])

    def neighbors(self, node):
        dirs = [((0,1),1.),((0,-1),1.),((1,0),1.),((-1,0),1.),
                ((1,1),1.414),((1,-1),1.414),((-1,1),1.414),((-1,-1),1.414)]
        result = []
        for (dx,dy),c in dirs:
            nx,ny = node[0]+dx, node[1]+dy
            if 0<=nx<self.w and 0<=ny<self.h and self.grid[ny][nx]<self.occ_thresh:
                result.append(((nx,ny), c*self.resolution))
        return result

    def plan(self, start_w, goal_w):
        start, goal = self.w2g(*start_w), self.w2g(*goal_w)
        if not (0<=start[0]<self.w and 0<=start[1]<self.h): return None
        if not (0<=goal[0]<self.w and 0<=goal[1]<self.h): return None
        open_set = [(0., start)]; came_from = {}; g = {start: 0.}; closed = set()
        while open_set:
            _, cur = heapq.heappop(open_set)
            if cur == goal:
                path = []
                while cur in came_from: path.append(self.g2w(*cur)); cur = came_from[cur]
                path.append(self.g2w(*start)); path.reverse(); return path
            closed.add(cur)
            for nb, mc in self.neighbors(cur):
                if nb in closed: continue
                tg = g[cur] + mc
                if tg < g.get(nb, float('inf')):
                    came_from[nb] = cur; g[nb] = tg
                    heapq.heappush(open_set, (tg+self.heuristic(nb,goal), nb))
        return None

# 测试
grid = np.zeros((20,20), dtype=np.int8)
grid[8:12,5:15] = 100
p = AStarPlanner(grid, 0.05, (-0.5,-0.5))
path = p.plan((0.,0.), (0.5,0.5))
print(f"{'✅ Found' if path else '❌ Failed'}: {len(path) if path else 0} points")

🐍 Python：ROS2全局规划器节点

#!/usr/bin/env python3
# 自定义Nav2全局规划器节点
import rclpy, numpy as np
from rclpy.node import Node
from nav_msgs.msg import Path, OccupancyGrid
from geometry_msgs.msg import PoseStamped

class GlobalPlannerNode(Node):
    def __init__(self):
        super().__init__('custom_global_planner')
        self.declare_parameter('tolerance', 0.5)
        self.map_sub = self.create_subscription(
            OccupancyGrid, '/global_costmap/costmap', self._map_cb, 10)
        self.path_pub = self.create_publisher(Path, '/plan', 10)
        self.goal_sub = self.create_subscription(
            PoseStamped, '/goal_pose', self._goal_cb, 10)
        self.current_map = None
        self.get_logger().info('🧭 自定义全局规划器已启动')

    def _map_cb(self, msg): self.current_map = msg

    def _goal_cb(self, msg):
        if not self.current_map: return
        grid = np.array(self.current_map.data).reshape(
            self.current_map.info.height, self.current_map.info.width)
        planner = AStarPlanner(grid, self.current_map.info.resolution,
            (self.current_map.info.origin.position.x,
             self.current_map.info.origin.position.y))
        path_pts = planner.plan((0.,0.), (msg.pose.position.x, msg.pose.position.y))
        if not path_pts:
            self.get_logger().error('规划失败'); return
        pm = Path(); pm.header.frame_id = 'map'
        pm.header.stamp = self.get_clock().now().to_msg()
        for px,py in path_pts:
            ps = PoseStamped(); ps.header = pm.header
            ps.pose.position.x = px; ps.pose.position.y = py
            ps.pose.orientation.w = 1.0; pm.poses.append(ps)
        self.path_pub.publish(pm)
        self.get_logger().info(f'✅ 规划完成: {len(pm.poses)}点')

def main(args=None):
    rclpy.init(args=args); rclpy.spin(GlobalPlannerNode()); rclpy.shutdown()

🔧 C++：Hybrid-A*运动学核心

// hybrid_astar_core.cpp
#include <cmath><vector><memory>
struct Pose2D { double x,y,theta; };
class HybridAStarCore {
    double min_r_, step_, max_steer_=0.5;
    std::vector<double> steers_;
public:
    HybridAStarCore(double r, double s) : min_r_(r), step_(s) {
        steers_ = {-max_steer_,-max_steer_/2,0.,max_steer_/2,max_steer_};
    }
    std::vector<Pose2D> successors(const Pose2D& c) {
        std::vector<Pose2D> res;
        for(auto st:steers_) for(bool fwd:{true,false})
            res.push_back(simulate(c,st,fwd));
        return res;
    }
private:
    Pose2D simulate(const Pose2D& s, double steer, bool fwd) {
        Pose2D e; double d=step_*(fwd?1:-1);
        if(std::abs(steer)<1e-6) {
            e.x=s.x+d*std::cos(s.theta); e.y=s.y+d*std::sin(s.theta);
            e.theta=s.theta;
        } else {
            double tr=min_r_/std::tan(std::abs(steer)), a=d/tr;
            if(steer>0){e.x=s.x+tr*(std::sin(s.theta+a)-std::sin(s.theta));
                        e.y=s.y-tr*(std::cos(s.theta+a)-std::cos(s.theta));}
            else{e.x=s.x-tr*(std::sin(s.theta-a)+std::sin(s.theta));
                 e.y=s.y+tr*(std::cos(s.theta-a)-std::cos(s.theta));}
            e.theta=s.theta+(steer>0?a:-a);
        }
        return e;
    }
};

📊 规划器选型与调优

🎯 练习题

🏆 成就解锁