Pure Pursuit算法

Pure pursuit算法是一种基于几何方式的基本路径追踪算法，下面将从：

  • RTK与imu融合
  • 车辆运动学模型
  • ROS实现进行讲解

imu_gps_localization包

该项目（ROS包）是：利用EKF（扩展卡尔曼滤波）实现IMU与GPS信号的融合。

为什么要进行融合呢？

  • IMU数据具有：高频率、灵敏，能有效提供姿态变化，但存在漂移等问题。
  • GPS数据具有：位置精度高（RTK 厘米级误差）、可靠，但存在更新频率慢。

综上所述，仅仅使用GPS进行对车辆的完全控制显然是不可靠的。

下载包后，分析项目结构有：

├─doc ## 存放地图信息
├─imu_gps_localizer ## imu、GPS融合算法
│  ├─include
│  │  └─imu_gps_localizer
│  ├─src
│  └─third_party
│      └─GeographicLib ## 几何库
│          ├─include
│          └─src
└─ros_wrapper ## 封装
    ├─include
    ├─launch
    ├─rviz
    └─src

进入ros_wrapper目录中，首先观察启动文件的设计：

<launch>
    <param name="acc_noise"       type="double" value="1e-2" />
    <param name="gyro_noise"      type="double" value="1e-4" />
    <param name="acc_bias_noise"  type="double" value="1e-6" />
    <param name="gyro_bias_noise" type="double" value="1e-8" /> // imu噪声

    <param name="I_p_Gps_x"       type="double" value="0.0" />
    <param name="I_p_Gps_y"       type="double" value="0.0" />
    <param name="I_p_Gps_z"       type="double" value="0.0" /> // GPS在IMU坐标系下的位置

    <param name="log_folder"      type="string" value="$(find imu_gps_localization)" /> // 日志目录

    <node name="nmea_topic_driver" pkg="nmea_navsat_driver" type="nmea_topic_driver" output="screen" /> // nmea驱动
    <node name="imu_gps_localization_node" pkg="imu_gps_localization" type="imu_gps_localization_node" output="screen" /> // 融合

    <node pkg="rviz" type="rviz" name="rviz" output="screen"
      args="-d $(find imu_gps_localization)/ros_wrapper/rviz/default.rviz" required="true"> // 打开rviz配置，失败关闭整个进程
    </node>

</launch>

综上所述，只需在新建的work_ws/src下，添加imu_gps_localization和nmea_navsat_driver包进行编译即可。

而对于nmea_navsat_driver包的使用，其launch文件：

<launch>
    
  <arg name="port" default="/dev/ttyUSB0" />
  <arg name="baud" default="4800" />
  <arg name="frame_id" default="gps" />
  <arg name="use_GNSS_time" default="False" />
  <arg name="time_ref_source" default="gps" />
  <arg name="useRMC" default="False" />

  <node name="nmea_serial_driver_node" pkg="nmea_navsat_driver" type="nmea_serial_driver" output="screen">
    <param name="port" value="$(arg port)"/>
    <param name="baud" value="$(arg baud)" />
    <param name="frame_id" value="$(arg frame_id)" />
    <param name="use_GNSS_time" value="$(arg use_GNSS_time)" />
    <param name="time_ref_source" value="$(arg time_ref_source)" />
    <param name="useRMC" value="$(arg useRMC)" />
  </node>

</launch>
  • 发布的ROS话题：启动 nmea_navsat_driver 后，它会解析 NMEA 数据并发布以下 ROS 话题：
    • /fix：标准的 sensor_msgs/NavSatFix 消息，包含位置信息。
    • /vel：标准的 geometry_msgs/TwistStamped 消息，包含速度信息（如果可用）。
    • /time_reference：标准的 sensor_msgs/TimeReference 消息，包含时间参考信息（如果可用）。

则通过以上文件的配置，我们就可以实现imu与GPS信号的融合，完整完整操作为：

sudo chmod 666 /dev/ttyUSB0
roscore
roslaunch imu_gps_localization imu_gps_localization.launch

值得注意的是：该项目需要自己设计imu话题发布对象，话题名：/imu/data，为标准sensor_msgs格式。

当我们得到理想的IMU与GPS融合数据后，现在需要进行控制车辆对目标点的追踪。

阿克曼运动学模型

阿克曼模型即四轮模型，可简化成自行车（二轮模型）进行运动学分析。

Pure Pursuit是一种几何跟踪控制算法，也被称为纯跟踪控制算法。该算法最早由R. Wallace在1985年提出，其思想是基于当前车辆的后轮中心位置（车辆质心），在参考路径上向ldl_d（称为前视距离）的距离匹配一个预瞄点，假设车辆后轮中心可以按照一定的转弯半径RR行驶至该预瞄点，然后根据前视距离ldl_d、转弯半径RR、车辆坐标系下预瞄点的朝向角𝛼之间的几何关系来计算前轮转角δ\delta。

image-20240718144543164
  • 如上图所示，在三角形OAC中，根据正弦定理可得：

ldsin⁡(2α)=Rsin⁡(π2−α)⇒R=ld2sin⁡α\frac{l_d}{\sin(2\alpha)}=\frac{R}{\sin(\frac{\pi}{2}-\alpha)}\Rightarrow R=\frac{l_d}{2\sin\alpha}

  • 根据车辆的运动学方程，有：

δ=arctan⁡LR\delta=\arctan\frac LR

  • 所以，前轮转角为：

δ=arctan⁡2Lsin⁡αld\delta=\arctan\frac{2L\sin\alpha}{l_d}

  • 车辆坐标系下预瞄点的朝向角α\alpha可以由AC的方向角减去车身航向角获得。

  • 定义横向误差为后轮中心位置和预瞄点在横向上的误差，如上图中的eye_y：

ey=ldsin⁡αe_y=l_d\sin\alpha

  • 当前轮转角很小时（<π3\frac{\pi}{3}），横向误差可以近似为：

ey=ldsin⁡α≈ld2δ2L⇒δ=2Lld2eye_{y}=l_{d}\sin\alpha\approx\frac{l_{d}^{ 2}\delta}{2L}\Rightarrow\delta=\frac{2L}{l_{d}^{ 2}}e_{y}

所以，该算法近似于一个P控制器，跟踪效果由前视距离ldl_d决定。

纯追踪ROS实现

  1. 获取小车的当前坐标（订阅odom获取或者map到base_link坐标变换获取）
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 !");
// }
    }
}
  1. 寻找与小车最近的路径点
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;
}
  1. 从该路径点出发，沿着路径寻找大于前视距离的第一个点作为目标点
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;
}
  1. 将路径点转到小车坐标系
// 转到小车坐标系
    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);
  1. 计算α或e(航偏角或横向误差)
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);
}
  1. 计算δ
double PurePursuit::getSteering(double eta)
{
    return atan2(2*(this->base_shape_L*sin(eta)),(this->Lfw));
}
  1. 发布角度，速度控制
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_);
}