轨迹规划 | 图解纯追踪算法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 commandsif (shouldRotateToGoal(robot_pose_map, global_plan_.back())){...}else{double e_theta = regularizeAngle(_dphi(lookahead_pt, robot_pose_map));// large angle, turn firstif (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 constraintselse{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 reachedif 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 distancelookahead_pt, _, _ = self.getLookaheadPoint()# get the tracking curvature with goalahead pointlookahead_k = 2 * math.sin(self.angle(self.robot.position, lookahead_pt) - self.robot.theta) / self.lookahead_dist# calculate velocity commande_theta = self.regularizeAngle(self.robot.theta - self.goal[2]) / 10if 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) / 10if self.shouldRotateToPath(abs(e_theta), np.pi / 4):u = np.array([[0], [self.angularRegularization(e_theta / dt)]])else:# apply constraintscurv_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 pointslookahead_pts.append(lookahead_pt)# feed into robotic kinematicself.robot.kinematic(u, dt)return False, None, None

在这里插入图片描述

4.3 Matlab仿真

核心代码如下所示

while iter < param.max_iterationiter = iter + 1;% break until goal reachedif 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 pointlookahead_k = 2 * sin( ...atan2(lookahead_pt(2) - robot.y, lookahead_pt(1) - robot.x) - robot.theta ...) / getLookaheadDistance(robot, param);% calculate velocity commande_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];elseu = [0, angularRegularization(robot, e_theta / param.dt, param)];endelsee_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 constraintscurv_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) ...];endend% input into robotic kinematicrobot = f(robot, u, param.dt);pose = [pose; robot.x, robot.y, robot.theta];
end