simple_pure_pursuitを改造してStanleyを実装する [aichallenge2023-racing ]

はじめに

前回の記事はこちら

前回はデフォルトの経路追従ノードであるsimple_pure_pursuitについてまとめました。
参考記事でStanleyという手法について知ったのでsimple_pure_pursuitを変更してStanleyを実装してみました。

Stanleyについての参考資料

• 参考資料の授業のもと動画

• Python Roboticsのコード

メジャーな手法ではないのか参考資料が少ない...

作成したコード

.hファイル [折りたたみ]

details

stanley_controller.hpp

#ifndef STANLEY_CONTROLLER_HPP_
#define STANLEY_CONTROLLER_HPP_

#include <autoware_auto_control_msgs/msg/ackermann_control_command.hpp>
#include <autoware_auto_planning_msgs/msg/trajectory.hpp>
#include <autoware_auto_planning_msgs/msg/trajectory_point.hpp>
#include <geometry_msgs/msg/pose.hpp>
#include <geometry_msgs/msg/twist.hpp>
#include <nav_msgs/msg/odometry.hpp>
#include <optional>
#include <rclcpp/rclcpp.hpp>

namespace stanley_controller {

using autoware_auto_control_msgs::msg::AckermannControlCommand;
using autoware_auto_planning_msgs::msg::Trajectory;
using autoware_auto_planning_msgs::msg::TrajectoryPoint;
using geometry_msgs::msg::Pose;
using geometry_msgs::msg::Twist;
using nav_msgs::msg::Odometry;

class StanleyController : public rclcpp::Node {
public:
  explicit StanleyController();

  // subscribers
  rclcpp::Subscription<Odometry>::SharedPtr sub_kinematics_;
  rclcpp::Subscription<Trajectory>::SharedPtr sub_trajectory_;

  // publishers
  rclcpp::Publisher<AckermannControlCommand>::SharedPtr pub_cmd_;

  // timer
  rclcpp::TimerBase::SharedPtr timer_;

  // updated by subscribers
  Trajectory::SharedPtr trajectory_;
  Odometry::SharedPtr odometry_;

  // pure pursuit parameters
  double wheel_base_;
  double cte_proportional_gain_;
  double speed_proportional_gain_;
  bool use_external_target_vel_;
  double external_target_vel_;

private:
  void onTimer();
  bool subscribeMessageAvailable();
  double computeCrossTrackError(const TrajectoryPoint &traj_point,
                                const geometry_msgs::msg::Pose &pose);
  double computeHeadingError(const TrajectoryPoint &traj_point,
                             const TrajectoryPoint &prev_traj_point,
                             const geometry_msgs::msg::Pose &pose);
};

} // namespace stanley_controller

#endif // SIMPLE_PURE_PURSUIT_HPP_

.cppファイル [折りたたみ]
onTimerの中身を変更し、Cross Track ErrorとHeading Errorを計算する関数を追加

details

stanley_controller.cpp

#include "stanley_controller/stanley_controller.hpp"
#include <algorithm>
#include <cmath>
#include <motion_utils/motion_utils.hpp>
#include <tf2/utils.h>
#include <tier4_autoware_utils/tier4_autoware_utils.hpp>

namespace stanley_controller {

using motion_utils::findNearestIndex;

StanleyController::StanleyController() : Node("stanley_controller") {
  // Initialize parameters
  this->declare_parameter<float>("wheel_base", 2.14);
  wheel_base_ = this->get_parameter("wheel_base").as_double();

  this->declare_parameter<float>("cte_proportional_gain", 1.0);
  cte_proportional_gain_ =
      this->get_parameter("cte_proportional_gain").as_double();

  this->declare_parameter<float>("speed_proportional_gain", 1.0);
  speed_proportional_gain_ =
      this->get_parameter("speed_proportional_gain").as_double();

  this->declare_parameter<bool>("use_external_target_vel", false);
  use_external_target_vel_ =
      this->get_parameter("use_external_target_vel").as_bool();

  this->declare_parameter<float>("external_target_vel", 0.0);
  external_target_vel_ = this->get_parameter("external_target_vel").as_double();

  pub_cmd_ = create_publisher<AckermannControlCommand>("output/control_cmd", 1);

  sub_kinematics_ = create_subscription<Odometry>(
      "input/kinematics", 1,
      [this](const Odometry::SharedPtr msg) { odometry_ = msg; });
  sub_trajectory_ = create_subscription<Trajectory>(
      "input/trajectory", 1,
      [this](const Trajectory::SharedPtr msg) { trajectory_ = msg; });

  using namespace std::literals::chrono_literals;
  timer_ = rclcpp::create_timer(this, get_clock(), 30ms,
                                std::bind(&StanleyController::onTimer, this));
}

AckermannControlCommand zeroAckermannControlCommand(rclcpp::Time stamp) {
  AckermannControlCommand cmd;
  cmd.stamp = stamp;
  cmd.longitudinal.stamp = stamp;
  cmd.longitudinal.speed = 0.0;
  cmd.longitudinal.acceleration = 0.0;
  cmd.lateral.stamp = stamp;
  cmd.lateral.steering_tire_angle = 0.0;
  return cmd;
}

void StanleyController::onTimer() {
  // check data
  if (!subscribeMessageAvailable()) {
    return;
  }

  // calc center coordinate of front wheel
  double front_x =
      odometry_->pose.pose.position.x +
      wheel_base_ / 2.0 * std::cos(odometry_->pose.pose.orientation.z);
  double front_y =
      odometry_->pose.pose.position.y +
      wheel_base_ / 2.0 * std::sin(odometry_->pose.pose.orientation.z);
  geometry_msgs::msg::Pose front_wheel_pos = odometry_->pose.pose;
  front_wheel_pos.position.x = front_x;
  front_wheel_pos.position.y = front_y;

  size_t closet_traj_point_idx =
      findNearestIndex(trajectory_->points, front_wheel_pos.position);

  AckermannControlCommand cmd = zeroAckermannControlCommand(get_clock()->now());

  if ((closet_traj_point_idx == trajectory_->points.size() - 1) ||
      (trajectory_->points.size() <= 5)) {
    cmd.longitudinal.speed = 0.0;
    cmd.longitudinal.acceleration = -10.0;
    RCLCPP_INFO_THROTTLE(get_logger(), *get_clock(), 1000 /*ms*/,
                         "reached to the goal");
  } else {
    // get closest trajectory point from current position
    TrajectoryPoint closet_traj_point =
        trajectory_->points.at(closet_traj_point_idx);
    // closet_traj_point の1つ前の点を取得
    TrajectoryPoint previous_traj_point;
    if (closet_traj_point_idx > 0) {
      size_t previous_traj_point_idx = closet_traj_point_idx - 1;
      previous_traj_point = trajectory_->points.at(previous_traj_point_idx);
    } else {
      previous_traj_point = closet_traj_point;
    }
    // calc longitudinal speed and acceleration
    double target_longitudinal_vel =
        use_external_target_vel_ ? external_target_vel_
                                 : closet_traj_point.longitudinal_velocity_mps;
    double current_longitudinal_vel = odometry_->twist.twist.linear.x;

    cmd.longitudinal.speed = target_longitudinal_vel;
    cmd.longitudinal.acceleration =
        speed_proportional_gain_ *
        (target_longitudinal_vel - current_longitudinal_vel);

    // Stanley lateral control algorithm implementation
    // Compute the cross track error (cte)
    double cte =
        computeCrossTrackError(closet_traj_point, odometry_->pose.pose);

    // Compute the heading error (he)
    double he = computeHeadingError(closet_traj_point, previous_traj_point,
                                    front_wheel_pos);

    // Steering angle based on Stanley method
    double steering_angle =
        he + std::atan2(cte_proportional_gain_ * cte, current_longitudinal_vel);
    double max_steering_angle_ = 0.64;
    // within feasible limits of the vehicle
    steering_angle = std::max(std::min(steering_angle, max_steering_angle_),
                              -max_steering_angle_);

    // Set the computed steering angle in the command message
    cmd.lateral.steering_tire_angle = steering_angle;
    // Log Cross Track Error, Heading Error, and Steering Angle
    RCLCPP_INFO_THROTTLE(this->get_logger(), *this->get_clock(),
                         1000, // Log every 1000 milliseconds (1 second)
                         "Cross Track Error (CTE): %f, Heading Error (HE): "
                         "%f, Steering Angle: "
                         "%f degrees",
                         cte, he,
                         steering_angle * 180 /
                             M_PI); // Convert steering_angle from radians to
                                    // degrees for logging

    pub_cmd_->publish(cmd);
  }
}

bool StanleyController::subscribeMessageAvailable() {
  if (!odometry_) {
    RCLCPP_INFO_THROTTLE(get_logger(), *get_clock(), 1000 /*ms*/,
                         "odometry is not available");
    return false;
  }
  if (!trajectory_) {
    RCLCPP_INFO_THROTTLE(get_logger(), *get_clock(), 1000 /*ms*/,
                         "trajectory is not available");
    return false;
  }
  return true;
}

double StanleyController::computeCrossTrackError(
    const TrajectoryPoint &traj_point, const geometry_msgs::msg::Pose &pose) {

  // Vector from vehicle to trajectory point
  double dx = traj_point.pose.position.x - pose.position.x;
  double dy = traj_point.pose.position.y - pose.position.y;

  // Orientation of the vehicle
  double vehicle_theta = tf2::getYaw(pose.orientation);

  // Compute the cross track error
  double cross_track_error =
      dy * std::cos(vehicle_theta) - dx * std::sin(vehicle_theta);

  return cross_track_error;
}

double
StanleyController::computeHeadingError(const TrajectoryPoint &traj_point,
                                       const TrajectoryPoint &prev_traj_point,
                                       const geometry_msgs::msg::Pose &pose) {
  // Orientation of the vehicle
  double vehicle_theta = tf2::getYaw(pose.orientation);

  // Orientation towards the trajectory point
  double traj_theta =
      std::atan2(traj_point.pose.position.y - prev_traj_point.pose.position.y,
                 traj_point.pose.position.x - prev_traj_point.pose.position.x);

  // Compute the heading error
  double heading_error = traj_theta - vehicle_theta;
  // Normalize the heading error to be within [-pi, pi]
  heading_error = std::atan2(std::sin(heading_error), std::cos(heading_error));

  return heading_error;
}

} // namespace stanley_controller

int main(int argc, char const *argv[]) {
  rclcpp::init(argc, argv);
  rclcpp::spin(std::make_shared<stanley_controller::StanleyController>());
  rclcpp::shutdown();
  return 0;
}

以下 コードの説明

=============================

前輪中心位置を計算し、経路の中から前輪中心位置と一番近い経路点を探す。

(x_f, y_f) = \left( x + \frac{L}{2} \cos(\psi), y + \frac{L}{2} \sin(\psi) \right)

  // calc center coordinate of front wheel
  double front_x =
      odometry_->pose.pose.position.x +
      wheel_base_ / 2.0 * std::cos(odometry_->pose.pose.orientation.z);
  double front_y =
      odometry_->pose.pose.position.y +
      wheel_base_ / 2.0 * std::sin(odometry_->pose.pose.orientation.z);
  geometry_msgs::msg::Pose front_wheel_pos = odometry_->pose.pose;
  front_wheel_pos.position.x = front_x;
  front_wheel_pos.position.y = front_y;

  size_t closet_traj_point_idx =
      findNearestIndex(trajectory_->points, front_wheel_pos.position);

縦方向の速度と加速度指令値の計算、途中でCross Track Errorの計算に使うため一つ前の経路の点を取得してる

v_{target} = v_{trajectry}

a = K_p \cdot (v_{target} - v_{current})

    // get closest trajectory point from current position
    TrajectoryPoint closet_traj_point =
        trajectory_->points.at(closet_traj_point_idx);
    // closet_traj_point の1つ前の点を取得
    TrajectoryPoint previous_traj_point;
    if (closet_traj_point_idx > 0) {
      size_t previous_traj_point_idx = closet_traj_point_idx - 1;
      previous_traj_point = trajectory_->points.at(previous_traj_point_idx);
    } else {
      previous_traj_point = closet_traj_point;
    }
    // calc longitudinal speed and acceleration
    double target_longitudinal_vel =
        use_external_target_vel_ ? external_target_vel_
                                 : closet_traj_point.longitudinal_velocity_mps;
    double current_longitudinal_vel = odometry_->twist.twist.linear.x;

    cmd.longitudinal.speed = target_longitudinal_vel;
    cmd.longitudinal.acceleration =
        speed_proportional_gain_ *
        (target_longitudinal_vel - current_longitudinal_vel);

Cross Track Errorの計算
経路に対して左にずれる方向がマイナス

    // Compute the cross track error (cte)
    double cte =
        computeCrossTrackError(closet_traj_point, odometry_->pose.pose);

double StanleyController::computeCrossTrackError(
    const TrajectoryPoint &traj_point, const geometry_msgs::msg::Pose &pose) {

  // Vector from vehicle to trajectory point
  double dx = traj_point.pose.position.x - pose.position.x;
  double dy = traj_point.pose.position.y - pose.position.y;

  // Orientation of the vehicle
  double vehicle_theta = tf2::getYaw(pose.orientation);

  // Compute the cross track error
  double cross_track_error =
      dy * std::cos(vehicle_theta) - dx * std::sin(vehicle_theta);

  return cross_track_error;
}

Heading Errorの計算
前輪中心位置に最も近い経路点とそれの一つ前の経路点を結ぶ直線に対する現在の車体の角度をHeading Errorとしています。

    // Compute the heading error (he)
    double he = computeHeadingError(closet_traj_point, previous_traj_point,
                                    front_wheel_pos);

double
StanleyController::computeHeadingError(const TrajectoryPoint &traj_point,
                                       const TrajectoryPoint &prev_traj_point,
                                       const geometry_msgs::msg::Pose &pose) {
  // Orientation of the vehicle
  double vehicle_theta = tf2::getYaw(pose.orientation);

  // Orientation towards the trajectory point
  double traj_theta =
      std::atan2(traj_point.pose.position.y - prev_traj_point.pose.position.y,
                 traj_point.pose.position.x - prev_traj_point.pose.position.x);

  // Compute the heading error
  double heading_error = traj_theta - vehicle_theta;
  // Normalize the heading error to be within [-pi, pi]
  heading_error = std::atan2(std::sin(heading_error), std::cos(heading_error));

  return heading_error;
}

操舵角指令値の計算
計算した値は最大のsteering角度 -0.64[rad]~ +0.64[rad]に制限する。

\delta= h_e + \tan^{-1}\left(\frac{k \cdot C_{te}}{v_f}\right)

    // Steering angle based on Stanley method
    double steering_angle =
        he + std::atan2(cte_proportional_gain_ * cte, current_longitudinal_vel);
    double max_steering_angle_ = 0.64;
    // within feasible limits of the vehicle
    steering_angle = std::max(std::min(steering_angle, max_steering_angle_),
                              -max_steering_angle_);

    // Set the computed steering angle in the command message
    cmd.lateral.steering_tire_angle = steering_angle;

動かしてみる

autowre_micro_awsin.launch.xmlのsimple_pure_pursuitをコメントアウトして作成したstanley_controllerノードを起動する。

autowre_micro_awsin.launch.xml

  <node pkg="stanley_controller" exec="stanley_controller" name="stanley_controller_node" output="screen">
    <param name="use_external_target_vel" value="false"/>
    <param name="external_target_vel" value="100.0"/>
    <param name="cte_proportional_gain" value="0.2"/>
    <param name="speed_proportional_gain" value="0.8"/>
    
    <remap from="input/kinematics" to="/localization/kinematic_state"/>
    <remap from="input/trajectory" to="/planning/scenario_planning/trajectory"/>
    <remap from="output/control_cmd" to="/control/command/control_cmd"/>
  </node>

※この動画の時はCross Track Errorの計算ミスってますが修正してゲイン適当に調整したら完走できるようになりました。
（なおシケイン走行中にNPCに遭遇するとNPCに衝突して盛大にクラッシュします..）

おわりに

OSSを変更して動くようにするのは大変だったのでデフォルトのsimple_pure_pursuitを変更してStanleyアルゴリズムを実装して動かしてみました。
実装正しいかどうか分かりませんが一応適当にゲイン調整して完走できたのでよしとします。
走行してみてStenleyのようなP制御っぽい制御だと誤差が発生してからハンドルを切ることになり、シケインなどの曲率が急な箇所ではハンドルが間に合わないので、pure pursuitのような先の経路を予測して先にハンドル切るようなD制御っぽい制御は必要だと感じました。
次はVector Mapの方をいじっていきます。