stanley 控制算法

公式推导

上图借用网上的示例，stanley 公式：

其中  是机器当前航向与跟踪点航向的偏差，用于修正机器的航向误差；

e 是前轮与轨迹曲线的最近距离，此时有存在 

 表示当前轮角度为 δ，在  时间内，前轮可以抵达最轨迹点的切线位置。当 变小，表示需要更快速地收缩横向误差，此时输出的  也更大。

通过上面知道前轮控制角度和前轮朝向的速度是成反比关系，和横向误差成正比关系。官网公式的横向误差最后简化用   来控制，实质上是 PID 控制跟踪，而非严格的几何跟踪。

当速度较低的时候，为防止出现控制较大的抖动，为给分母增加一个 ks 常数。

注意项

1. 公式里的速度 v(t) 是前轮方向上的线速度，如果是后驱阿克曼底盘机器，传入的速度需要将后轮速度转为前轮方向上的线速度。

2. 横向控制公式成立需要轨迹是连续的曲线，曲线外一点到曲线上最近距离才是垂直于曲线的。工程上轨迹一般都是一系列离散的轨迹点，这里就需要留意轨迹不能过于稀疏。

3. 横向偏差需要根据前轮位姿与最近轨迹点的方位来选取正负。

ROS C++ 实现

/**
 * @brief 计算从 <angle_start> 到 <angle_end> 的航向偏差
 *        逆时针偏差为正，顺时针偏差为负
 * @param  angle_start      开始的航向角
 * @param  angle_end        结束的航向角
 * @return float            返回的航向角偏差
 */
float GetDiffAngle(const float angle_start, const float angle_end)
{
    float diff_angle = angle_end - angle_start;
    if (diff_angle > M_PI) {
        diff_angle = diff_angle - 2 * M_PI;
    } else if (diff_angle < -M_PI) {
        diff_angle = 2 * M_PI + diff_angle;
    }
    return diff_angle;
}

/**
 * @brief stanley 前轮反馈控制
 * @param  steering_pose    前轮位姿
 * @param  target           跟踪轨迹上与前轮距离最近的轨迹点
 * @param  wheel_base       阿克曼底盘后轮中心到前轮的距离
 * @param  mu               航向误差的比例系数，根据轨迹平滑程度设置，默认为 1.0
 * @param  lambda           横向误差的比例系数
 * @param  ks               减小低速行驶下的控制抖动
 * @param  linear_x         前轮朝向的线速度，如果是后驱车，需要转换下速度
 * @return float            返回前轮控制角度
 */
double StanleyFollow(
  const geometry_msgs::PoseStamped steering_pose,
  const geometry_msgs::PoseStamped target,
  const float wheel_base, const float mu, const float lambda,
  const float ks, const float linear_x)
{
  // 航向角偏差
  float steer_yaw = tf2::getYaw(steering_pose.pose.orientation);
  float target_yaw = tf2::getYaw(target.pose.orientation);
  float yaw_err =  mu * GetDiffAngle(steer_yaw, target_yaw);
  
  // 横向偏差
  float dist_diff = std::hypot(
    target.pose.position.y - steering_pose.pose.position.y,
    target.pose.position.x - steering_pose.pose.position.x);
  float relative_yaw = atan2(target.pose.position.y - steering_pose.pose.position.y,
                             target.pose.position.x - steering_pose.pose.position.x);
  if (GetDiffAngle(steer_yaw, relative_yaw) < 0) dist_diff = -dist_diff;
  float dist_err = atan(lambda * dist_diff / (linear_x + ks));
  
  float steering = yaw_err + dist_err;
  steering = clamp(steering, kSteerMin, kSteerMax);
  return steering;
}

参考

斯坦福 stanley 论文