ROS实现无人驾驶控制算法（一）——pure pursuit_纯追踪ros代码

引言

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

车辆运动学模型

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

纯追踪算法

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

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

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.从该路径点出发，沿着路径寻找大于前视距离的第一个点作为目标点

点击ROS实现无人驾驶控制算法（一）——pure pursuit - 古月居可查看全文