---

前言

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]$$

二、主要代码实现

首先，需要发布路径，话题名为"/rc_path_pub",发布路径实现可参考博客读取机器人移动轨迹并在RVIZ界面中显示。
其次，作为测试仅仅将获取到的导航角偏差 δ ×一个比例系数作为角速度，预设的速度 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();}}

三、效果

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

---