路径跟踪算法---Stanley Method实现

前言

Stanley Controller也是基于几何追踪的轨迹跟踪控制器，和Pure Pursuit不同的是，其基于前轮中心点为参考点进行控制，没有预瞄距离，以前轮中心点与最近参考轨迹点进行横向误差与导航角误差的计算。

一、Stanley原理介绍

如图，黑色曲线表示预设路径，红色方框表示车辆当前运动状态，偏离预设路径，使用Stanley Controller的目的是让车辆追踪预设路径，通过控制车的转向，使其靠近预设路径，图中黑色方框代表期望状态。图中红色箭头表示车辆实际运动方向，黑色箭头表示期望车辆运动方向，绿色虚线表示距离车辆最近路径点的切线，e_y 表示车与路径的最短距离，即横向偏差，θ e 为切线与实际运动方向的夹角，即航向偏差，δ e 表示期望运动方向与切线的夹角。

假定车辆实际位置在预设路径上，此时没有横向偏差，只需要转过 θe 角度即可，期望车辆转过的角度为：
$$\begin{gathered} \delta =\theta e \\ \theta e=\theta r-\theta x \end{gathered}$$
假定车辆实际运动方向与切线方向一致，此时没有航向偏差，只有横向偏差。车辆当前速度为v，t时间内运动距离为d，和速度存在一个系数k。
$$\begin{gathered} \delta =\delta e \\ d=v/k \\ tan\delta e=e_y/d=k\ast e_y/v \\ \delta e=ta{n}^{-1}(k{e}_y/v) \end{gathered}$$
同时，转向角不能任意，需要进行限幅处理，此时期望车转过的角度为（加入时间变化）：
$$\delta (t)=\delta e(t)+\theta e(t)=\theta e(t)+ta{n}^{-1}(k{e}_y(t)/v(t)),\delta (t)\in [\delta min,\delta max]$$

二、关键代码实现

作为测试,将获取到的导航角偏差 δ ×一个比例系数作为角速度，预设的速度 v 作为线速度发送到底盘进行控制，因为用的模型是差速模型，没有进行角度限幅处理，实际想达到好的实验结果，可能需要将导航角偏差送到 pid 控制器进行实际的速度计算。代码参考知乎：Raiden

using path_type = std::pair<std::pair<double, double>, double>;

	//用amcl定位获取机器人位姿效果不好，故采用odom位姿，也可用其他定位方式
    void StanleyController::robotPoseCallBack(const geometry_msgs::PoseWithCovarianceStampedConstPtr& msg){

        // yaw =tf::getYaw(msg->pose.pose.orientation);
        // tf::pointMsgToTF(msg->pose.pose.position,current_pose);
    }
    void StanleyController::OdomCallback(const nav_msgs::Odometry::ConstPtr& odom_msg) {
        yaw =tf::getYaw(odom_msg->pose.pose.orientation);
        tf::pointMsgToTF(odom_msg->pose.pose.position,current_pose);
    }
    //两点间距离
    auto StanleyController::distance(path_type p1,path_type p2) -> double{
        return sqrt(pow(p1.first.first-p2.first.first,2) +
                    pow(p1.first.second-p2.first.second,2));
    }
    //最近距离路径点索引
    auto StanleyController::GetMinDisIndex(path_type current_pose, std::vector<path_type> path) -> int{
        double min_dis = INFINITY;
        int idx = 0;
        for(int i = 0;i<path.size();i++){
            double dis  = distance(path[i],current_pose);
            if(dis < min_dis){
                min_dis = dis;
                idx = i;
            }
        }
        return idx;
    }
    void StanleyController::robotPathCallBack(const nav_msgs::PathConstPtr& msg){
        if(recv_path_flag){
            nav_path_.poses = msg->poses;
            recv_path_flag = false;
            start_pose_.position.x = nav_path_.poses[0].pose.position.x;
            start_pose_.position.y = nav_path_.poses[0].pose.position.y;
            path_vector.clear();
            for(int i = 0;i<nav_path_.poses.size();i++){
                path_type temp_path = std::make_pair(std::make_pair(nav_path_.poses[i].pose.position.x,nav_path_.poses[i].pose.position.y),
                                        tf::getYaw(nav_path_.poses[i].pose.orientation));
                path_vector.push_back(temp_path);
            }
        }
    }
    void StanleyController::run(){
        cmd_vel_pub = n_.advertise<geometry_msgs::Twist>("/cmd_vel",1);
        pose_sub = n_.subscribe<geometry_msgs::PoseWithCovarianceStamped>("amcl_pose",10,
                            std::bind(&StanleyController::robotPoseCallBack,this,std::placeholders::_1));
        path_sub = n_.subscribe<nav_msgs::Path>("/rc_path_pub",10,std::bind(&StanleyController::robotPathCallBack,this,std::placeholders::_1));
        real_path_pub = n_.advertise<nav_msgs::Path>("rvizpath", 100, true);

        odom_pose_sub = n_.subscribe<nav_msgs::Odometry>("odom", 10, 
                                 std::bind(&StanleyController::OdomCallback, this, std::placeholders::_1));
                                 
        ros::Rate loop_rate(10);
        recv_path_flag = true;
        path.header.frame_id = "/map";
        path.header.stamp = ros::Time::now();
        
        path_type init_pose = std::make_pair(std::make_pair(0.0,0.0),0.0);
        path_type now_pose = init_pose;
		while(ros::ok()){

            now_pose.first.first = current_pose.x();
            now_pose.first.second = current_pose.y();
            now_pose.second = yaw;
            //判断获取到路径
            if(path_vector.size()>0){
                int current_idx = GetMinDisIndex(now_pose,path_vector);
                path_type cur_path_point = path_vector[current_idx];

                //横向偏差
                double e_y = distance(now_pose,cur_path_point);
                e_y = (now_pose.first.second - cur_path_point.first.second) * cos(cur_path_point.second) - 
                        (now_pose.first.first - cur_path_point.first.first) * sin(cur_path_point.second) <=0 ? e_y : -e_y;
                //航向偏差,转到-PI 到 +PI之间
                double theta_e =  cur_path_point.second -now_pose.second ;  //参考点航向 - 自车航向角
                if(theta_e > M_PI){ 
                    theta_e = theta_e - 2* M_PI;
                } else if(theta_e < -M_PI){
                    theta_e = theta_e + 2* M_PI;
                }
                //转角，限幅
                double delta =( theta_e + atan2(factor * e_y, speed) );
                // if(delta > M_PI/3){
                //     delta = M_PI/3;
                // } else if(delta < -M_PI/3){
                //     delta = -M_PI/3;
                // }
                //直接把转角当作角速度
                vel.linear.x = speed;
                vel.angular.z = delta * 2.5;
                cmd_vel_pub.publish(vel);
            }

            ros::spinOnce();
            loop_rate.sleep();
        }
    }

三、效果

最后，放一张效果图~~~