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

0 专栏介绍

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

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

1 纯追踪算法原理推导

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

给定路径点 $\mathcal{P} =\left\{ \boldsymbol{p}_0,\boldsymbol{p}_1,\cdots ,\boldsymbol{p}_g \right\}$ 。根据纯追踪算法设置的固定预瞄距离 $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$

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

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

在这里插入图片描述

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

$\frac{L}{\sin 2\alpha}=\frac{R}{\sin \left( {{\pi}/{2}}-\alpha \right)}\Rightarrow R=\frac{L}{2\sin \alpha}$

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

$\begin{cases} v_t=v_d\\ \omega _t={{v_t}/{R}}\\\end{cases}$

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

$R=\frac{L^2}{2e_y}\Leftrightarrow \kappa =\frac{2}{L^2}\cdot e_y$

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

在这里插入图片描述

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

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

$L_t=l_tv_t+L_0$

其中 $l_t$ 是前瞻增益，表示将 $v_t$ 向前投影的时间增量； $L_0$ 是最小预瞄距离。

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

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

$v_{t}^{'}=\begin{cases} v_t\,\, , \kappa \leqslant \kappa _{\max}\\ \frac{\kappa _{\max}}{\kappa}v_t\,\, , \kappa >\kappa _{\max}\\\end{cases}$

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

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