🎯 第18课：路径规划(局部)

导航进阶 ✅ Docker验证通过

📋 课程目标

🧠 什么是局部路径规划？

局部路径规划(Local Planner/Controller)负责实时跟踪全局路径，同时动态避开传感器检测到的障碍物。它以10-20Hz的高频率运行，输出/cmd_vel速度命令控制机器人运动。

🔑 局部规划器核心任务

全局+局部规划器协作： ┌────────────┐ 全局路径 ┌────────────────┐ │ Global │──/plan────►│ Local Planner │ │ Planner │ │ 输入:/plan │ └────────────┘ │ /costmap │ ┌────────────┐ 局部代价 │ /odom │ │ Costmap2D │───────────►│ 输出: /cmd_vel │ └────────────┘ └───────┬────────┘ ▼ ┌──────────────┐ │ 机器人底盘 │ └──────────────┘

📊 Nav2局部规划器对比

规划器算法特点适用场景
DWB动态窗口法简单稳定、参数少室内差速机器人
TEB时间弹性带时间最优、轨迹平滑非完整约束机器人
MPPI模型预测路径积分考虑动力学、多目标高速/复杂动力学
Regulated Pure Pursuit纯追踪简单高效、跟踪好简单路径跟踪

🔧 DWB控制器配置

# nav2_params.yaml - DWB配置
controller_server:
  ros__parameters:
    controller_frequency: 10.0
    controller_plugins: ["FollowPath"]
    FollowPath:
      plugin: "dwb_core::DWBLocalPlanner"
      min_vel_x: 0.0
      max_vel_x: 0.26
      max_vel_theta: 1.0
      acc_lim_x: 2.5
      acc_lim_theta: 3.2
      vx_samples: 20
      vtheta_samples: 40
      sim_time: 1.2
      critics: ["RotateToGoal","Oscillation","BaseObstacle",
                "GoalAlign","PathAlign","PathDist","GoalDist"]
      BaseObstacle.scale: 0.02
      PathAlign.scale: 32.0
      GoalAlign.scale: 24.0
      PathDist.scale: 32.0
      GoalDist.scale: 24.0
      RotateToGoal.scale: 32.0

🔧 TEB控制器配置

# TEB - 时间最优弹性带
controller_server:
  ros__parameters:
    controller_plugins: ["FollowPath"]
    FollowPath:
      plugin: "teb_local_planner/TebLocalPlannerROS"
      teb_autosize: true
      dt_ref: 0.3
      max_vel_x: 0.26
      max_vel_theta: 1.0
      acc_lim_x: 2.5
      acc_lim_theta: 3.2
      xy_goal_tolerance: 0.25
      yaw_goal_tolerance: 0.25
      min_obstacle_dist: 0.25
      weight_kinematics_nh: 1000.0
      weight_obstacle: 50.0
      weight_dynamic_obstacle: 100.0

🔧 MPPI控制器配置

# MPPI - 模型预测路径积分
controller_server:
  ros__parameters:
    controller_plugins: ["FollowPath"]
    FollowPath:
      plugin: "nav2_mppi_controller::MPPIController"
      time_steps: 56
      model_dt: 0.05
      batch_size: 2000
      vx_std: 0.2
      wz_std: 0.4
      temperature: 0.3
      gamma: 0.015
      motion_model: "DiffDrive"
      critics: ["ConstraintCritic","CostCritic","GoalCritic",
                "PathAlignCritic","PathFollowCritic"]
      ConstraintCritic.cost_weight: 4.0
      CostCritic.cost_weight: 3.0
      GoalCritic.cost_weight: 5.0
      PathAlignCritic.cost_weight: 14.0

🐍 Python：纯追踪控制器

#!/usr/bin/env python3
# 纯追踪(Pure Pursuit)路径跟踪控制器
import math, rclpy
from rclpy.node import Node
from nav_msgs.msg import Path
from geometry_msgs.msg import Twist
from tf2_ros import Buffer, TransformListener

class PurePursuitController(Node):
    def __init__(self):
        super().__init__('pure_pursuit')
        self.declare_parameter('lookahead_dist', 0.6)
        self.declare_parameter('max_v', 0.26)
        self.declare_parameter('max_w', 1.0)
        self.declare_parameter('goal_tol', 0.3)
        self.la = self.get_parameter('lookahead_dist').value
        self.mv = self.get_parameter('max_v').value
        self.mw = self.get_parameter('max_w').value
        self.gt = self.get_parameter('goal_tol').value
        self.tf_buf = Buffer()
        TransformListener(self.tf_buf, self)
        self.path_sub = self.create_subscription(Path,'/plan',self._pcb,10)
        self.cmd_pub = self.create_publisher(Twist,'/cmd_vel',10)
        self.create_timer(0.1, self._ctrl)
        self.path = None
        self.get_logger().info('🚗 Pure Pursuit已启动')

    def _pcb(self, msg): self.path = msg

    def _ctrl(self):
        if not self.path or not self.path.poses: return
        try: t = self.tf_buf.lookup_transform('map','base_link',rclpy.time.Time())
        except: return
        rx, ry = t.transform.translation.x, t.transform.translation.y
        q = t.transform.rotation
        ryaw = math.atan2(2*(q.w*q.z+q.x*q.y),1-2*(q.y*q.y+q.z*q.z))
        # 找前瞻点
        lp = None
        for p in reversed(self.path.poses):
            dx, dy = p.pose.position.x-rx, p.pose.position.y-ry
            if math.hypot(dx,dy) >= self.la:
                lp = (p.pose.position.x, p.pose.position.y); break
        if not lp:
            last = self.path.poses[-1].pose.position
            if math.hypot(last.x-rx,last.y-ry) < self.gt:
                self.cmd_pub.publish(Twist()); return
            lp = (last.x, last.y)
        # 纯追踪几何：curvature = 2*local_y / L^2
        dx, dy = lp[0]-rx, lp[1]-ry
        L = math.hypot(dx, dy)
        ly = -dx*math.sin(ryaw) + dy*math.cos(ryaw)
        k = 2*ly/(L*L) if L > 0.01 else 0
        v = min(self.mv, self.mv/(1+abs(k)*5)) if abs(k)>0.001 else self.mv
        w = max(-self.mw, min(self.mw, k*v))
        cmd = Twist(); cmd.linear.x = v; cmd.angular.z = w
        self.cmd_pub.publish(cmd)

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

🐍 Python：DWA核心算法

#!/usr/bin/env python3
# 动态窗口法(DWA)核心实现
import math, numpy as np

class DWAPlanner:
    def __init__(self, max_v=0.5, max_w=1.0, acc_v=2.5, acc_w=3.2,
                 v_res=0.01, w_res=0.1, dt=0.1, pred_t=1.2):
        self.max_v=max_v; self.max_w=max_w; self.acc_v=acc_v
        self.acc_w=acc_w; self.v_res=v_res; self.w_res=w_res
        self.dt=dt; self.pred_t=pred_t

    def dyn_window(self, v, w):
        return [(max(-self.max_v, v-self.acc_v*self.dt),
                 min(self.max_v, v+self.acc_v*self.dt)),
                (max(-self.max_w, w-self.acc_w*self.dt),
                 min(self.max_w, w+self.acc_w*self.dt))]

    def predict(self, x, y, th, v, w):
        traj = [(x,y,th)]; t=0
        while t <= self.pred_t:
            x += v*math.cos(th)*self.dt
            y += v*math.sin(th)*self.dt
            th += w*self.dt; traj.append((x,y,th)); t += self.dt
        return traj

    def cost(self, traj, goal, obs):
        x,y,th = traj[-1]
        hd = abs(math.atan2(goal[1]-y,goal[0]-x)-th)
        dg = math.hypot(goal[0]-x,goal[1]-y)
        mo = min((math.hypot(px-ox,py-oy) for px,py,_ in traj
                  for ox,oy in obs), default=10)
        return hd + 0.5*dg - 2.0*mo

    def plan(self, state, goal, obs, v0, w0):
        dw = self.dyn_window(v0, w0)
        best, best_c = (0,0), float('inf')
        for v in np.arange(dw[0][0], dw[0][1], self.v_res):
            for w in np.arange(dw[1][0], dw[1][1], self.w_res):
                tr = self.predict(*state, v, w)
                if any(math.hypot(px-ox,py-oy)<0.2
                       for px,py,_ in tr for ox,oy in obs): continue
                c = self.cost(tr, goal, obs)
                if c < best_c: best_c=c; best=(v,w)
        return best

dwa = DWAPlanner()
v,w = dwa.plan((0.,0.,0.),(3.,2.),[(1.,.5),(1.5,1.5)],0.,0.)
print(f"DWA: v={v:.2f} w={w:.2f}")

🔧 C++：MPPI轨迹采样

// mppi_core.cpp
#include <cmath><vector><random>
struct State{double x,y,theta,v,w;};
class MPPICore {
    int batch_,steps_; double dt_,vs_,ws_;
    std::mt19937 rng_{std::random_device{}()};
public:
    MPPICore(int b,int s,double d,double vx,double wz)
        :batch_(b),steps_(s),dt_(d),vs_(vx),ws_(wz){}
    auto sample(){
        std::normal_distribution<double> nv(0,vs_),nw(0,ws_);
        std::vector<std::vector<std::pair<double,double>>> seqs(batch_);
        for(auto& sq:seqs) for(int t=0;t<steps_;t++)
            sq.emplace_back(nv(rng_),nw(rng_));
        return seqs;
    }
    std::vector<State> rollout(State s,
        const std::vector<std::pair<double,double>>& sq){
        std::vector<State> tr; tr.push_back(s);
        for(auto&[dv,dw]:sq){
            s.v+=dv;s.w+=dw;
            s.x+=s.v*std::cos(s.theta)*dt_;
            s.y+=s.v*std::sin(s.theta)*dt_;
            s.theta+=s.w*dt_; tr.push_back(s);
        }
        return tr;
    }
};

🎯 练习题

📝 练习1：DWB参数调优

调整sim_time和vx_samples，观察轨迹质量变化。

📝 练习2：纯追踪前瞻距离

分别设lookahead为0.3、0.6、1.2，观察转弯行为差异。

📝 练习3：TEB vs DWB对比

相同场景对比路径跟踪误差和计算时间。

🏆 成就解锁

🏅 局部规划专家

经验值：+250 XP

← 上一课 📖 目录 下一课 →
💡 控制器调试技巧：在rviz2中添加/plan和/cmd_vel显示，可以实时观察全局路径和速度命令的对应关系。如果路径跟踪有振荡，通常是sim_time过大或critic权重不当。
⚠️ 常见问题：TEB控制器计算量大，在低性能嵌入式设备上可能无法达到控制频率。如果出现控制频率下降，考虑减少no_inner_iterations和no_outer_iterations，或切换到DWB控制器。

📚 扩展阅读