引言

对于导航系统来说，在规划好全局路径后，使机器人根据路径行驶这部分被称为轨迹跟踪。轨迹跟踪主要分为两类：基于几何追踪的方法和基于模型预测的方法。而pure pursuit算法就是最基本的基于几何的控制算法，因其鲁棒性高，对路径的要求低而广泛使用，也为后续的standly、LQR、MPC算法打好基础。

车辆运动学模型

因为纯追踪算法是基于几何模型的，因此，需要推导出车辆的运动学模型，本文采用的是阿克曼模型（差速模型也会提及）。将四轮模型转换为自行车模型方便后续计算。
对于阿克曼转向模型来说，只需要驱动电机控制线速度，以及前轮舵机转角控制转向，因此，需要计算前轮转向角的求导公式。

如图，L为车辆的轴距（前轮到后轮的距离），R为转弯半径，δ为前轮的舵机转角。根据三角函数关系可得：tan(δ)=\frac{L}{R}

纯追踪算法

从自行车模型出发，纯跟踪算法以车后轴为原点, 车辆与路径最近的点为起点，在路径中寻找一个大于前视距离的点作为目标点，通过控制前轮转角，使车辆可以沿着一条经过目标路点的圆弧行驶，如下图所示：

其中α为车辆与目标点之间的夹角，可以用α=arctan(\frac{x}{y})计算，x,y为车辆后轮到目标点x,y方向上的距离。具体推导如下图所示。

从上图推导可知，车辆的转角和轴距、前视距离、以及横向误差有关。其中最重要的是前视距离的选取。

ROS中的实现步骤

1.获取小车的当前坐标（订阅odom获取或者map到base_link坐标变换获取）

/*!
 *
 * @param  odomMsg odom传感器数据
 * @author 测试
 * @author odom数据会出现漂移导致不准，可尝试使用tf变换 计算出base_link在map下的坐标
 */
void PurePursuit::odomCallback(const nav_msgs::Odometry::ConstPtr &odomMsg) {
    this->odom_ = *odomMsg;
    if(this->goal_received_)
    {
        double car2goal_x = this->goal_pos_.x - odomMsg->pose.pose.position.x;
        double car2goal_y = this->goal_pos_.y - odomMsg->pose.pose.position.y;
        double dist2goal = sqrt(car2goal_x*car2goal_x + car2goal_y*car2goal_y);
        /// 当小车位置距离终点小于阈值时，说明到达目的地了
       ///和move_base一起使用时这段注释解除
//        if(dist2goal < this->goal_radius)
//        {
//            this->goal_reached_ = true;
//            this->goal_received_ = false;
//            ROS_INFO("Goal Reached !");
//        }
    }
}

2.寻找与小车最近的路径点

/*!
 *
 * @param wayPt 路径点
 * @param carPose 机器人位姿
 * @return 判断路径点是否在机器人的前方
 */
int PurePursuit::minIndex(const geometry_msgs::Point& carPose){
    int index_min = 0;
    // 先将d_min设置为最大
    int d_min = INT16_MAX;
    for(int i =0; i< map_path_.poses.size(); i++)
    {
        // 读取路径点坐标
        geometry_msgs::PoseStamped map_path_pose = map_path_.poses[i];
        // 计算路径点到小车的距离，取最小的距离
        double d_temp = PointDistanceSquare(map_path_pose,carPose);
        if (d_temp < d_min) {
            d_min = d_temp;
            index_min = i;
        }
    }
    return index_min;
}

3.从该路径点出发，沿着路径寻找大于前视距离的第一个点作为目标点

/*!
 *
 * @param wayPt 寻找到的预选点
 * @param car_pos 位姿
 * @return 是否为前预瞄点
 */
bool PurePursuit::isWayPtAwayFromLfwDist(const geometry_msgs::Point& wayPt, const geometry_msgs::Point& car_pos)
{
    double dx = wayPt.x - car_pos.x;
    double dy = wayPt.y - car_pos.y;
    double dist = sqrt(dx*dx + dy*dy);
     // 当大于设置的预瞄距离则为目标点
    if(dist < Lfw)
        return false;
    else if(dist >= Lfw)
        return true;
}

4.将路径点转到小车坐标系

// 转到小车坐标系
    odom_car2WayPtVec.x = cos(carPose_yaw)*(forwardPt.x - carPose_pos.x) + sin(carPose_yaw)*(forwardPt.y - carPose_pos.y);
    odom_car2WayPtVec.y = -sin(carPose_yaw)*(forwardPt.x - carPose_pos.x) + cos(carPose_yaw)*(forwardPt.y - carPose_pos.y);

5.计算α或e(航偏角或横向误差)

/*!
 *
 * @param carPose 当前位姿
 * @return 获取航向角误差
 */
double PurePursuit::getEta(const geometry_msgs::Pose& carPose)
{
    // 获取目标点在小车坐标系下的坐标
    geometry_msgs::Point odom_car2WayPtVec = get_odom_car2WayPtVec(carPose);
    // 返回偏航角
    return atan2(odom_car2WayPtVec.y,odom_car2WayPtVec.x);
}

6.计算δ

/*!
 *
 * @param eta 获取航向角误差
 * @return 机器人舵机转角
 */
double PurePursuit::getSteering(double eta)
{
    return atan2(2*(this->base_shape_L*sin(eta)),(this->Lfw));
}

7.发布角度，速度控制

void PurePursuit::controlLoopCallback(const ros::TimerEvent&)
{

    geometry_msgs::Pose carPose = this->odom_.pose.pose;
    geometry_msgs::Twist carVel = this->odom_.twist.twist;

    if(this->goal_received_)
    {
        // 获取航向角偏差
        double eta = getEta(carPose);
//        cout<<eta<<endl;
        if(foundForwardPt_)
        {
            this->steering_ = this->base_angle_ + getSteering(eta);
            if(!this->goal_reached_)
            {
                this->velocity = this->reference_v;
            }
        }
    }

    if(goal_reached_)
    {
        velocity = 0.0;
        steering_ = 0.0;
    }

    this->ackermann_cmd_.drive.steering_angle = this->steering_;
    this->ackermann_cmd_.drive.speed = this->velocity;
    this->ackermann_pub_.publish(this->ackermann_cmd_);
}

优缺点

优点

1.对于路径的要求不高
2.鲁棒性强
3.低速下性能较好

缺点

1.对路径追踪程度不够
2.过于依赖前视距离的选取，可以采用动态前视距离ld = l+kv其中 l、k为系数，根据速度调整前视距离
3.高速时不适用

代码

基于tianbot_racecar仿真代码，经过魔改而来，代码地址为
https://github.com/lzw12138/pure_pursuit.git

视频参考

https://www.bilibili.com/video/BV1R34y1H7nf/?vd_source=ff55bd8dde4763fcbe54d332225ad729