【control】pure pursuit纯追踪算法

系列文章目录

提示：这里可以添加系列文章的所有文章的目录，目录需要自己手动添加
TODO:写完再整理

---

前言

认知有限，望大家多多包涵，有什么问题也希望能够与大家多交流，共同成长！

在项目和平时的学习中，我对机器人/无人驾驶的决策规划模块进行了划分，当然划分的方法有很多，我的划分方式仅供参考
（1）动态障碍物行为预测模块（Behavior prediction）–结合感知和高精度地图信息，估计周围障碍物未来运动状态
（2）执行机构的轨迹规划模块（Trajectory_planning）–执行机构如机器人载体上的机械臂、串行云台等的运动轨迹规划
（3）任务决策模块（Mission_planning）–任务决策模块比较偏业务层了，处理机器人/无人驾驶的各种任务，主要分为三个方面：车底盘航线业务决策（交规、横向换道等等）、执行应用机构业务决策（机械臂、人机交互等等）、不同场景的导航方案切换决策（组合导航、融合导航）
（4）前端路径探索模块（path_finding）–全局路径规划算法难度不算复杂，找到一条可通行的（必须满足）、考虑动力学的（尽可能满足）、可以是稀疏的路径base_waypoints【由于其只考虑了环境几何信息，往往忽略了无人机本身的运动学与动力学模型。因此，其得到的轨迹往往显得比较“突兀”，并不适合直接作为无人机的控制指令】
（5）后端轨迹处理模块（motion_planning）–我主要归纳整理为三个方向：（1）对base_waypoints进行简单处理及生成方向、（2）对base_waypoints轨迹优化方向【一般是二次优化，这里用的较多的事优化方面的知识】、（3）进行对应功能的replan方向（replan之前的预处理、进入replan的条件、停障replan、避障replan、纠偏replan、换道replan、自动泊车replan、穿过狭窄道路replan等等），这部分内容使用的方法比较专
（6）路径跟踪模块（trajectory_following）–这个模块就得针对机器人载体了，如无人驾驶使用得阿克曼模型可以采用几何的pure pursuit纯追踪算法，更好的可以用模型预测控制MPC方法，还有强化学习做的（效果怎样我就没验证过了）；当然也可能事麦克纳姆轮车、差速车PID、还有无人机的三维轨迹跟踪等等
（7）碰撞检测模块
（8）集群多机器人规划模块

当然，这种划分方式是我权衡了原理和功能粗略划分的，在实际产品研发过程中，需要理解了各个算法的功能和定位的基础上融汇贯通，不能生搬硬套，如全段通过hybrid A探索出来的路径与A、RRT*探索出来的路径更平滑，后端轨迹优化的任务就不用这么重了；又如，机器人/无人驾驶项目研发的需求业务还没发展到能响应很多功能阶段，任务决策使用简单的状态机（fsm）就可以对现有任务进行状态转移了

本文先对pure pursuit纯追踪算法做个简单的介绍，具体内容后续再更，其他模块可以参考去我其他文章

---

提示：以下是本篇文章正文内容
对于无人车辆来说，在规划好路径以后（这个路径我们通常称为全局路径），全局路径由一系列路径点构成，这些路径点只要包含空间位置信息即可，也可以包含姿态信息，但是不需要与时间相关，这些路径点被称为全局路径点（Global Waypoint），路径（Path）和轨迹（Trajectory）的区别就在于，轨迹还包含了时间信息，轨迹点也是一种路径点，它在路径点的基础上加入了时间约束，通产我们将这些轨迹点称为局部路径点（Local Waypoints）

一、pure_pursuit纯追踪算法几何分析图例

（gx,gy）是我们下一个要追踪的路点，它位于我们已经规划好的全局路径上，ld表示车辆当前位置（即后轴位置）到目标路点的距离（前视距离），表示目前车身姿态和目标路点的夹角

二、pure_pursuit纯追踪算法输入输出

（1）输入：当前车辆的位置、速度和当前跟随路径【防盗标记–盒子君hzj】
（2）输出：车辆的目标线速度、线加速度、转角角度、转角角速度的控制信息

科普一下~
（1）Twist速度指令
1）线速度

linear.x指向前方
linear.y指向左方【平面移动机器人常常linear.y为0】
linear.z垂直向上【满足右手系，平面移动机器人常常linear.z为0】

2）角速度

angular.z代表平面机器人的角速度【z轴为旋转轴】

对于四轮四转车底盘
1）线速度v=0，角速度w=0：车辆原地不动
2）线速度v=0，角速度w≠0：车辆直线运动【防盗标记–盒子君hzj】
3）线速度v≠0，角速度w=0：车辆原地旋转
4）线速度v≠0，角速度w≠0：车辆直线+旋转运动
.
.

三、pure_pursuit纯追踪算法原理

Pure Pursuit是一种用于路径跟踪的控制算法。它通过计算角速度控制机器人从当前位置移到机器人前方的某个预瞄点。假定线速度是恒定的，当然可以随意更改机器人的线速度。该算法会根据机器人的当前位置在路径上移动预瞄点，直到路径的终点。可以想象成机器人不断追逐它前面的一个点。参数LookAheadDistance就决定将预瞄点放置有多远。

.
.

1.车底盘模型科普

（1）质点模型(x,y)

（2）自行车模型（x,y,yaw）

纯跟踪算法以车后轴为切点, 车辆纵向车身为切线, 通过控制前轮转角 ， 【防盗标记–盒子君hzj】使车辆后轴可以沿着一条已经规划好的全局路径，经过目标路点（goal point）进行圆弧行驶

自行车模型的运动学与动力学
https://blog.csdn.net/qq_35635374/article/details/124210275

四、pure_pursuit纯追踪算法公式推导及源码分析【核心：后轮速度v -> 前视距离ld -> 转弯曲率 -> 前轮转角及角速度】

1.距离目标路点的前视距离 ld公式

通常来说，ld 被认为是车速的函数，在不同的车速下需要选择不同的前视距离，【防盗标记–盒子君hzj】将前视距离表示成车辆纵向速度的线形函数：

通常来说，会使用最大/最小前视距离来约束前视距离，越大的前视距离意味着轨迹的追踪越平滑，小的前视距离会使得追踪更加精确（当然也会带来控制的震荡）

注意：在程序中,线速度v是自己设定的或者用waypoints自带的速度，用过标志位velocity_source进行选择

// 计算预瞄距离
double PurePursuit::computeLookaheadDistance()
{
  if (velocity_source_ == 1) //0代表参照waypoints设定的速度, 1则是自己设定需要的速度
  {
    return const_lookahead_distance_;//若是设置为1，则使用yaml文件规定的前视距离
  }
  double maximum_lookahead_distance = current_linear_velocity_ * 10; //最大前视距离为当前速度的10倍
  double ld = current_linear_velocity_ * lookahead_ratio_;           //计算前视距离，线性比率k为2    [前视距离公式：前视距离=k*当前车速]

  // 根据预瞄距离的范围，限幅选择合适的值
  return ld < minimum_lookahead_distance_ ? minimum_lookahead_distance_ : //若当前前视距离小于最小前视距离，则限幅为最小前视距离
             ld > maximum_lookahead_distance ? maximum_lookahead_distance
                                             : ld; //若当前前视距离大于最大前视距离，则限幅为最大前视距离
}

2.车身和目标路点的夹角 α(t)公式

可以通过几何关系直接计算出来，程序上下面用公式绕开了

3.圆弧的曲率kappa公式

方式一：

方式二：
定义一个新的量：el车辆当前姿态和目标路点在横向上的误差，【防盗标记–盒子君hzj】可知：

可得：

从这条公式可以理解到，圆弧的曲率是一个横向转角的p控制器，【防盗标记–盒子君hzj】其P系数为

// 计算曲率
double PurePursuit::calcCurvature(xag_nav_msgs::Point target)
{
  double kappa;
  double denominator = pow(getPlaneDistance(target, PoseCovertPoint(current_pose_)), 2);  //计算前视距离的平方
  //double denominator = pow(3, 2);
  double numerator = 2 * calcRelativeCoordinate(target, current_pose_.pose).y;            //计算两点的横向偏差

  if (denominator != 0)
  {
    kappa = numerator / denominator;//计算曲率的公式
  }
  else
  {
    if (numerator > 0)
    {
      kappa = KAPPA_MIN_;
    }
    else
    {
      kappa = -KAPPA_MIN_;
    }
  }
  return kappa;
}

4.前轮转角的角速度公式

方式一：角速度=线速度/曲率半径
方式二：角速度=线速度*曲率
注意：曲率=1/曲率半径

这个公式怎么来的呢？记得高中学习的扇形公式吗？角度=扇形周长L/扇形半径R
扇形公式求导就是前轮转角的角速度公式了

void PurePursuit::publishTwistStamped(const bool &can_get_curvature, const double &kappa)
{
  xag_nav_msgs::TwistStamped ts;
  //ts.header.stamp = ros::Time::now();
  ts.twist.linear.x = can_get_curvature ? computeCommandVelocity() : 0;         //计算线速度  [can_get_curvature时计算结果标志位。能计算出来的就用计算出来的线速度，反之输出0]
  ts.twist.angular.z = can_get_curvature ? 1.0 * kappa * ts.twist.linear.x : 0; //计算角速度  [参考公式w=V/R   即角速度=线速度/曲率半径    角速度=线速度*曲率     曲率=1/曲率半径]
  cmd_vel_writer_->Transmit(ts);
  std::cout << "velocity:   " << ts.twist.linear.x << "angular:  " << ts.twist.angular.z  << std::endl;
}

5.前轮转角 δ公式

表达式一：

---

表达式二：  把时间考虑进来，在知道t时刻车身和目标路点的夹角 α(t) 和距离目标路点的前视距离 ld 的情况下，由于车辆轴距 L 固定，我们可以利用上式估计出应该作出的前轮转角 δ

---

表达式三：

将前视距离表示成车辆纵向速度的线形函数

从这个角度，在知道t时刻车身和目标路点的夹角 α(t)、前视距离系数k、的情况下，【防盗标记–盒子君hzj】由于车辆轴距 L 固定，车辆的纵向速度vx可测，我们可以利用上式估计出应该作出的前轮转角 δ

void PurePursuit::publishControlCommandStamped(
    const bool &can_get_curvature, const double &kappa)
{
  if (!publishes_for_steering_robot_) //这是用于切换小车的控制指令的，有的小车用角速度控制，有的小车用转角位置控制
  {
    return;
  }

  xag_nav_msgs::CotrolCmd ccs;
  //ccs.header.stamp = ros::Time::now();

  ccs.linear_velocity = can_get_curvature ? computeCommandVelocity() : 0;                           //线速度指令
  ccs.linear_acceleration = can_get_curvature ? computeCommandAccel() : 0;                          //加速度指令
  ccs.steering_angle = can_get_curvature ? convertCurvatureToSteeringAngle(wheel_base_, kappa) : 0; //发布角度指令，将曲率转换成转角【steering_angle=atan(wheel_base * kappa)】
  ctrl_raw_writer_->Transmit(ccs);
}

autoware的pure pursuit中的源码框架分析介绍

温馨提示：
autoware中有pure_persuit算法的源码
（0）源文件分类

pure_pursuit_node.cpp
pure_pursuit_core.cpp
pure_pursuit.cpp
pure_pursuit_viz.cpp

（1）构造函数initForROS()
1）设置ros的参数

is_linear_interpolation
publishes_for_steering_robot
wheel_base

2）创建订阅器

current_pose		#当前位置
current_velocity  #当前速度
final_waypoints		#目标位置
config/waypoint_follower#纯跟踪算法参数

3）创建发布器

twist_raw
ctrl_cmd
next_waypoint_mark
next_target_mark
search_circle_mark
line_point_mark
trajectory_circle_mark
angular_gravity
deviation_of_current_position

（2）回调函数
1）callbackFromCurrentPose（）
2）callbackFromCurrentVelocity（）
3）callbackFromWayPoints
若有路径，用路径点内的速度信息设置目标的当前车辆线速度
4）callbackFromConfig

#获取认为设置的参数，获取成功则挂起标志位
param_flag_
const_lookahead_distance_
const_velocity_
lookahead_distance_ratio_
minimum_lookahead_distance_

（3）循环函数
当机器人的速度、位置、路径点订阅函数都接收到数据后，先算前视距离，【防盗标记–盒子君hzj】进而计算曲率kappa，再进而计算线速度v(保持之前的订阅的线速度)、角速度数据，发布线速度、线加速度、转角角度控制命令，最后rviz可视化的话题发布

五、pure pursuit部署（其他车型）的经验

（1）pure_persuit航线跟随的思路

-> 路径规划给航线waypoints点的速度和位姿
-> 计算线速度
-> 计算前视距离
-> 计算曲率
-> 计算角速度（角度）
-> 线速度和角速度根据实际情况增加阻尼项（根据实车模型）

（2）pure_persuit纯跟踪适配差速底盘的方法

在纯跟踪算法输出的角速度乘一个增益，这样输出的控制量既不是阿克曼模型，【防盗标记–盒子君hzj】也不是差速模型，但是是这两个模型之间

这样就可以实现差速车方向快速调节的效果了

（3）pure_persuit部署经验

（1）根据实际车况情况在阻尼大的地方添加增益项

理论模型输出没错，实际物理模型可能执行不到，要根据实际车况情况在阻尼大的地方多增益项

（2）调节预瞄距离

预瞄距离的调整需要根据机器人运行的实际情况来进行调整。

LookAheadDistance参数是整个Pure Pursuit控制器的重要参数。【防盗标记–盒子君hzj】往前看的距离是机器人从当前位置应沿着路径观察的距离，以计算角速度命令。下图显示了机器人和预瞄点。如图所示，实际的路径与追踪路径是不匹配的。

更改预瞄距离可以改变机器人路径追踪的方式。较小的预瞄距离可以使机器人更加精确地追踪实际的路径，但是可能会使机器人在追踪路径时产生振荡，较大的预瞄距离可以减少机器人沿路的振荡，【防盗标记–盒子君hzj】但最追踪的路径可能不够精确，也会导致拐角处附近的曲率变大。

Pure Pursuit在低速小在情况下合理调整LookAheadDistance参数可以实现较好的路径跟踪效果，较小的预瞄距离能使机器人更加精确地追踪路径，但可能会引起机器人控制的不稳定甚至震荡；【防盗标记–盒子君hzj】较大的预瞄距离可以使机器人跟踪路径更加平滑，但不能精确地跟踪原始的路径，在大转角处会出现转向不足的情况。

（3）适当的删除不合理（已经走过的全局航线）

搜索点的时候是搜前视距离以外的点！这样路径删除的点可以是4米左右之后，稳定性好很多

删除的点个前视距离理论上一致比较好

.
.

---

总结

（1）优点

（1）以纯追踪控制器为代表的几何路径跟踪器很容易理解和实现。
（2）这种方法优点，线速度严格按照航线执行（路径规划包括了车辆的位姿和速度），同时角速度不会震荡（飞思卡尔这种把后轮线速度看成一个闭环，方向角度看成一个闭环的方法，打方向会震荡的，大型车辆不能震荡）

（2）缺点

（1）存在显著的速度改变时会影响纯追踪算法的前视距离计算（前视距离是控制的会很飘）
（2）在低速情况下，路径情况变化几乎不影响转角，【防盗标记–盒子君hzj】低速路径情况变化比较大时，可以把k参数调大试试
（3）效果好不好，却决于车辆底盘长度L、路径变化情况、车辆速度设置和参数k（实在速度太低，可以把前视距离直接设置为一个常数）
（4）效果好不好完全靠调试的参数，不太智能化

（3）Pure_pursuit纯跟踪算法的思考

（1）从不同的角度理解算法

（1）从模型的角度去理解

1）车辆外部整体的模型(自行车模型)
使用车辆外部整体的模型(自行车模型)，用于根据路径点（x,y）解算出车辆的xy方向的速度和方向盘的偏航角（xv,yv,yaw）指令

---

（2）车辆内部机械结构的模型（底盘类型+轮子大小模型） 使用车辆内部机械结构的模型（底盘类型+轮子大小模型），【防盗标记--盒子君hzj】用于根据车辆的xy方向的速度和方向盘的偏航角（xv,yv,yaw）指令，通过逆运动学解算出来每个轮子的速度

---

（2）从闭环控制去理解
纯跟踪算法也是一个闭环算法，一个纯跟踪算法相当于代替方向闭环和速度闭环两个分立的PID算法，区别在在于PID闭环是不基于模型的，而纯跟踪算法是基于自行车模型的，相同点在于两种方法都是根据轨迹的位置速度要求控制转角和后轮速度
1）飞思卡尔用的是独立双PID闭环实现【注意这个闭环控制对象是输出的指令，不是执行器（如实验室的编码器版本，舵机硬件闭环这个）】
2）无人车用的是纯跟踪算法实现

（2）Pure_pursuit纯跟踪算法的模型适配及解决办法

1）模型适配
Pure_pursuit纯跟踪算法比较适合阿克曼底盘模型，【防盗标记–盒子君hzj】麦克纳姆轮底盘模型和四轮差速底盘可以跟踪到的极端航线，Pure_pursuit纯跟踪算法不一定能跟踪到（输出的线速度v和角速度w不一定准确）麦克纳姆轮底盘模型和四轮差速底盘的极端航线，但是通过调参，在没有折线的情况能达到一个较好的效果

---

2）模型适配本质
Pure_pursuit纯跟踪算法是把车看成一个阿克曼模型进行几何解算的，本质是不适合差速车或者麦轮车的

---

3）解决办法
（1）通过路径规划，让得到的执行路径更平滑【治标】
（2）通过调参（调前视距离），使得pure_suit有更好的的跟踪效果【治标】
（3）不用Pure_pursuit纯跟踪算法，直接给定航线用差速车用PID的方法进行跟踪（找无人车部门来做）【治本】
（4）不用Pure_pursuit纯跟踪算法，【防盗标记–盒子君hzj】直接用DWA_planner算法做规划，DWA_planner是为四轮差速车设计的

参考资料

pure_pursuit纯追踪算法论文参考

《implement of the pure pursuit path tracking algorithm》

视频展示

https://www.bilibili.com/video/BV1eq4y1Z7BM?spm_id_from=333.999.0.0
https://www.bilibili.com/video/BV1AK411j74q?spm_id_from=333.999.0.0
https://www.bilibili.com/video/BV1AK411j74q?spm_id_from=333.999.0.0

https://adamshan.blog.csdn.net/article/details/80555174