轨迹规划 | 图解纯追踪算法Pure Pursuit(附ROS C++/Python/Matlab仿真)

本篇文章分享轨迹规划 | 图解纯追踪算法Pure Pursuit(附ROS C++/Python/Matlab仿真)，对你有帮助的话记得收藏一下，看Python资料网收获更多编程知识

0 专栏介绍

🔥附C++/Python/Matlab全套代码🔥课程设计、毕业设计、创新竞赛必备！详细介绍全局规划(图搜索、采样法、智能算法等)；局部规划(DWA、APF等)；曲线优化(贝塞尔曲线、B样条曲线等)。

🚀详情：图解自动驾驶中的运动规划(Motion Planning)，附几十种规划算法

---

1 纯追踪算法原理推导

纯追踪算法(Pure Pursuit, PP)参考了人类驾驶行为，其基本思想是：在待跟踪路径上设置预瞄点(goal-ahead)，通过简单的几何方法驱动机器人跟踪预瞄点，随着机器人运动，预瞄点动态移动直至抵达目标位置。

给定路径点 P = { p 0 , p 1 , ⋯   , p g } \mathcal{P} =\left\{ \boldsymbol{p}_0,\boldsymbol{p}_1,\cdots ,\boldsymbol{p}_g \right\} P={p0​,p1​,⋯,pg​}。根据纯追踪算法设置的固定预瞄距离$L$选择预瞄点

p l = p i ∈ P    s . t . ∥ p i − 1 − p r ∥ 2 2 < L    a n d ∥ p i − p r ∥ 2 2 ⩾ L \boldsymbol{p}_l=\boldsymbol{p}_i\in \mathcal{P} \,\,\mathrm{s}.\mathrm{t}. \left\| \boldsymbol{p}_{i-1}-\boldsymbol{p}_r \right\| _{2}^{2}<L\,\,\mathrm{and} \left\| \boldsymbol{p}_i-\boldsymbol{p}_r \right\| _{2}^{2}\geqslant L pl​=pi​∈Ps.t.∥pi−1​−pr​∥22​<Land∥pi​−pr​∥22​⩾L

其中 p r \boldsymbol{p}_r pr​是最接近机器人当前位置的路径点。

当规划时间间隔$\Delta t\to 0$时，可认为机器人线速度$v$和角速度$\omega$不变，因此其转向半径$R=v/\omega$是定值，即机器人进行圆周运动。

如图所示，根据几何关系可得

L sin ⁡ 2 α = R sin ⁡ ( π / 2 − α ) ⇒ R = L 2 sin ⁡ α \frac{L}{\sin 2\alpha}=\frac{R}{\sin \left( {{\pi}/{2}}-\alpha \right)}\Rightarrow R=\frac{L}{2\sin \alpha} sin2αL​=sin(π/2−α)R​⇒R=2sinαL​

确定曲率半径后，对于差速轮式移动机器人而言，可根据下式计算当前的控制指令

{ v t = v d ω t = v t / R \begin{cases} v_t=v_d\\ \omega _t={{v_t}/{R}}\\\end{cases} {vt​=vd​ωt​=vt​/R​

在机器人局部坐标系中，设机器人与预瞄点的纵向误差为 e y e_y ey​，则

R = L 2 2 e y ⇔ κ = 2 L 2 ⋅ e y R=\frac{L^2}{2e_y}\Leftrightarrow \kappa =\frac{2}{L^2}\cdot e_y R=2ey​L2​⇔κ=L22​⋅ey​

相当于一个以横向跟踪误差为系统误差的比例控制器，如图所示。增大$L$有利于降低超调，但会产生稳态误差；减小$L$能够加快动态响应速度，但容易引起振荡。

2 自适应纯追踪算法(APP)

为了在跟踪振荡和较慢收敛间取得可接受的权衡，自适应纯追踪算法(Adaptive Pure Pursuit, APP)根据运动速度自适应调整预瞄距离

L t = l t v t + L 0 L_t=l_tv_t+L_0 Lt​=lt​vt​+L0​

其中 l t l_t lt​是前瞻增益，表示将 v t v_t vt​向前投影的时间增量； L 0 L_0 L0​是最小预瞄距离。

3 规范化纯追踪算法(RPP)

考虑到机器人始终以期望速度 运动并不合理，尤其是在狭窄区域、急转弯等不完全可见工作空间，动态调整机器人速度有利于提供更高质量的行为表现。规范化纯追踪算法(Regulated Pure Pursuit, RPP)引入了修正启发式等进行自适应调整。举例而言，曲率启发式，目的是放慢机器人在部分可观察环境的速度，以提高盲转弯时的安全性。其中最大曲率阈值 提供了急转弯位置的速度缩放。

v t ′ = { v t    , κ ⩽ κ max ⁡ κ max ⁡ κ v t    , κ > κ max ⁡ v_{t}^{'}=\begin{cases} v_t\,\, , \kappa \leqslant \kappa _{\max}\\ \frac{\kappa _{\max}}{\kappa}v_t\,\, , \kappa >\kappa _{\max}\\\end{cases} vt′​={vt​,κ⩽κmax​κκmax​​vt​,κ>κmax​​

可以根据应用需求设计更多启发式

4 仿真实现

4.1 ROS C++仿真

核心代码如下所示

bool RPPPlanner::computeVelocityCommands(geometry_msgs::Twist& cmd_vel)
{
  ...
  
  double vt = std::hypot(base_odom.twist.twist.linear.x, base_odom.twist.twist.linear.y);
  double L = getLookAheadDistance(vt);

  getLookAheadPoint(L, robot_pose_map, prune_plan, lookahead_pt, theta, kappa);
  double lookahead_k = 2 * sin(_dphi(lookahead_pt, robot_pose_map)) / L;

  // calculate commands
  if (shouldRotateToGoal(robot_pose_map, global_plan_.back()))
  {
    ...
  }
  else
  {
    double e_theta = regularizeAngle(_dphi(lookahead_pt, robot_pose_map));

    // large angle, turn first
    if (shouldRotateToPath(std::fabs(e_theta), M_PI_2))
    {
      cmd_vel.linear.x = 0.0;
      cmd_vel.angular.z = angularRegularization(base_odom, e_theta / d_t_);
    }

    // apply constraints
    else
    {
      double curv_vel = _applyCurvatureConstraint(max_v_, lookahead_k);
      double cost_vel = _applyObstacleConstraint(max_v_);
      double v_d = std::min(curv_vel, cost_vel);
      v_d = _applyApproachConstraint(v_d, robot_pose_map, prune_plan);

      cmd_vel.linear.x = linearRegularization(base_odom, v_d);
      cmd_vel.angular.z = angularRegularization(base_odom, v_d * lookahead_k);
    }
  }

  return true;
}

4.2 Python仿真

核心代码如下所示

def plan(self):
    lookahead_pts = []
    dt = self.params["TIME_STEP"]
    for _ in range(self.params["MAX_ITERATION"]):
        # break until goal reached
        if self.shouldRotateToGoal(self.robot.position, self.goal):
            return True, self.robot.history_pose, lookahead_pts

        # get the particular point on the path at the lookahead distance
        lookahead_pt, _, _ = self.getLookaheadPoint()

        # get the tracking curvature with goalahead point
        lookahead_k = 2 * math.sin(
            self.angle(self.robot.position, lookahead_pt) - self.robot.theta
        ) / self.lookahead_dist

        # calculate velocity command
        e_theta = self.regularizeAngle(self.robot.theta - self.goal[2]) / 10
        if self.shouldRotateToGoal(self.robot.position, self.goal):
            if not self.shouldRotateToPath(abs(e_theta)):
                u = np.array([[0], [0]])
            else:
                u = np.array([[0], [self.angularRegularization(e_theta / dt)]])
        else:
            e_theta = self.regularizeAngle(
                self.angle(self.robot.position, lookahead_pt) - self.robot.theta
            ) / 10
            if self.shouldRotateToPath(abs(e_theta), np.pi / 4):
                u = np.array([[0], [self.angularRegularization(e_theta / dt)]])
            else:
                # apply constraints
                curv_vel = self.applyCurvatureConstraint(self.params["MAX_V"], lookahead_k)
                cost_vel = self.applyObstacleConstraint(self.params["MAX_V"])
                v_d = min(curv_vel, cost_vel)
                u = np.array([[self.linearRegularization(v_d)], [self.angularRegularization(v_d * lookahead_k)]])
        
        # update lookahead points
        lookahead_pts.append(lookahead_pt)

        # feed into robotic kinematic
        self.robot.kinematic(u, dt)
    
    return False, None, None

4.3 Matlab仿真

核心代码如下所示

while iter < param.max_iteration
	iter = iter + 1;
	
	% break until goal reached
	if shouldRotateToGoal([robot.x, robot.y], goal, param)
	    flag = true;
	    break;
	end
	
	% get the particular point on the path at the lookahead distance
	[lookahead_pt, ~, ~] = getLookaheadPoint(robot, path, param);
	
	% get the tracking curvature with goalahead point
	lookahead_k = 2 * sin( ...
	    atan2(lookahead_pt(2) - robot.y, lookahead_pt(1) - robot.x) - robot.theta ...
	) / getLookaheadDistance(robot, param);
	
	% calculate velocity command
	e_theta = regularizeAngle(robot.theta - goal(3)) / 10;
	if shouldRotateToGoal([robot.x, robot.y], goal, param)
	    if ~shouldRotateToPath(abs(e_theta), 0.0, param)
	        u = [0, 0];
	    else
	        u = [0, angularRegularization(robot, e_theta / param.dt, param)];
	    end
	else
	    e_theta = regularizeAngle( ...
	        atan2(lookahead_pt(2) - robot.y, lookahead_pt(1) - robot.x) - robot.theta ...
	    ) / 10;
	    if shouldRotateToPath(abs(e_theta), pi / 4, param)
	        u = [0, angularRegularization(robot, e_theta / param.dt, param)];
	    else
	        % apply constraints
	        curv_vel = applyCurvatureConstraint(param.max_v, lookahead_k, param);
	        cost_vel = applyObstacleConstraint(param.max_v, map, robot, param);
	        v_d = min(curv_vel, cost_vel);
	        u = [
	                linearRegularization(robot, v_d, param), ...
	                angularRegularization(robot, v_d * lookahead_k, param) ...
	        ];
	    end
	end
	
	% input into robotic kinematic
	robot = f(robot, u, param.dt);
	pose = [pose; robot.x, robot.y, robot.theta];
end

完整工程代码请联系下方博主名片获取

---

🔥 更多精彩专栏：

• 《ROS从入门到精通》
• 《Pytorch深度学习实战》
• 《机器学习强基计划》
• 《运动规划实战精讲》
• …

👇源码获取 · 技术交流 · 抱团学习 · 咨询分享 请联系👇

版权声明：本文来自互联网用户投稿，该文观点仅代表作者本人，不代表本站立场。本站仅提供信息存储空间服务，不拥有所有权，不承担相关法律责任。如若内容造成侵权/违法违规/事实不符，请联系邮箱：jacktools123@163.com进行投诉反馈，一经查实，立即删除！