double_point_pure_pursuit を実装する

はじめに

自動運転AIチャレンジ2024決勝まで、残りわずかとなりました。

今回は経路追従コントローラであるsimple_pure_pursuitの改良をしてみます。

simple_pure_pursuitやそもそもコントロールとは？の解説は以下が詳しいです。

Multiple Look ahead points への拡張

世の中には、複数の前方注視点を用いた制御則が存在します。

Pure Pursuit にも複数ポイントへの拡張例が存在し、追従性能が向上するようです。

ということで、simple_pure_pursuitをベースに、Look ahead pointを2点に拡張してみます！

実装

以下のように愚直に実装しました。double_point_pure_pursuitと名付けます。

double_point_pure_persuit.cpp

double_point_pure_persuit.cpp

#include "simple_pure_pursuit/simple_pure_pursuit.hpp"

#include <motion_utils/motion_utils.hpp>
#include <tier4_autoware_utils/tier4_autoware_utils.hpp>

#include <tf2/utils.h>

#include <algorithm>

namespace simple_pure_pursuit
{

using motion_utils::findNearestIndex;
using tier4_autoware_utils::calcLateralDeviation;
using tier4_autoware_utils::calcYawDeviation;

SimplePurePursuit::SimplePurePursuit()
: Node("simple_pure_pursuit"),
  // initialize parameters
  wheel_base_(declare_parameter<float>("wheel_base", 2.14)),
  lookahead_gain_(declare_parameter<float>("lookahead_gain", 4.0)),
  lookahead_min_distance_(declare_parameter<float>("lookahead_min_distance", 1.0)),
  lookahead_gain_2_(declare_parameter<float>("lookahead_gain_2", 2.0)),
  lookahead_min_distance_2_(declare_parameter<float>("lookahead_min_distance_2", 1.0)),
  speed_proportional_gain_(declare_parameter<float>("speed_proportional_gain", 1.0)),
  use_external_target_vel_(declare_parameter<bool>("use_external_target_vel", false)),
  external_target_vel_(declare_parameter<float>("external_target_vel", 0.0)),
  steering_tire_angle_gain_(declare_parameter<float>("steering_tire_angle_gain", 1.0)),
  w_1_(declare_parameter<float>("w_1", -0.0966)),
  w_2_(declare_parameter<float>("w_2", 1.09666))
{
  pub_cmd_ = create_publisher<AckermannControlCommand>("output/control_cmd", 1);
  pub_raw_cmd_ = create_publisher<AckermannControlCommand>("output/raw_control_cmd", 1);
  pub_lookahead_point_1_ = create_publisher<PointStamped>("/control/debug/lookahead_point_1", 1);
  pub_lookahead_point_2_ = create_publisher<PointStamped>("/control/debug/lookahead_point_2", 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(&SimplePurePursuit::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 SimplePurePursuit::onTimer()
{
  // check data
  if (!subscribeMessageAvailable()) {
    return;
  }

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

  // publish zero command
  AckermannControlCommand cmd = zeroAckermannControlCommand(get_clock()->now());

  if ((closet_traj_point_idx == trajectory_->points.size() - 1) || 
      (trajectory_->points.size() <= 2)) {
    cmd.longitudinal.speed = 0.0;
    cmd.longitudinal.acceleration = -10.0;
    RCLCPP_INFO_THROTTLE(get_logger(), *get_clock(), 1000 /*ms*/, "reached to the goal");
  } else {
    TrajectoryPoint closet_traj_point = trajectory_->points.at(closet_traj_point_idx);

    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);

    double lookahead_distance_1 = lookahead_gain_ * target_longitudinal_vel + lookahead_min_distance_;
    double lookahead_distance_2 = lookahead_gain_2_ * target_longitudinal_vel + lookahead_min_distance_2_;

    double rear_x = odometry_->pose.pose.position.x - 
                    wheel_base_ / 2.0 * std::cos(tf2::getYaw(odometry_->pose.pose.orientation));
    double rear_y = odometry_->pose.pose.position.y - 
                    wheel_base_ / 2.0 * std::sin(tf2::getYaw(odometry_->pose.pose.orientation));

    auto lookahead_point_itr_1 = std::find_if(
      trajectory_->points.begin() + closet_traj_point_idx, trajectory_->points.end(),
      [&](const TrajectoryPoint & point) {
        return std::hypot(point.pose.position.x - rear_x, point.pose.position.y - rear_y) >=
               lookahead_distance_1;
      });

    auto lookahead_point_itr_2 = std::find_if(
      lookahead_point_itr_1, trajectory_->points.end(),
      [&](const TrajectoryPoint & point) {
        return std::hypot(point.pose.position.x - rear_x, point.pose.position.y - rear_y) >=
               lookahead_distance_2;
      });

    if (lookahead_point_itr_1 == trajectory_->points.end()) {
      lookahead_point_itr_1 = trajectory_->points.end() - 1;
    }
    if (lookahead_point_itr_2 == trajectory_->points.end()) {
      lookahead_point_itr_2 = trajectory_->points.end() - 1;
    }

    double x1 = lookahead_point_itr_1->pose.position.x;
    double y1 = lookahead_point_itr_1->pose.position.y;
    double x2 = lookahead_point_itr_2->pose.position.x;
    double y2 = lookahead_point_itr_2->pose.position.y;

    geometry_msgs::msg::PointStamped lookahead_point_msg_1;
    lookahead_point_msg_1.header.stamp = get_clock()->now();
    lookahead_point_msg_1.header.frame_id = "map";
    lookahead_point_msg_1.point.x = x1;
    lookahead_point_msg_1.point.y = y1;
    lookahead_point_msg_1.point.z = 0.0;
    pub_lookahead_point_1_->publish(lookahead_point_msg_1);

    geometry_msgs::msg::PointStamped lookahead_point_msg_2;
    lookahead_point_msg_2.header.stamp = get_clock()->now();
    lookahead_point_msg_2.header.frame_id = "map";
    lookahead_point_msg_2.point.x = x2;
    lookahead_point_msg_2.point.y = y2;
    lookahead_point_msg_2.point.z = 0.0;
    pub_lookahead_point_2_->publish(lookahead_point_msg_2);

    double alpha_1 = std::atan2(y1 - rear_y, x1 - rear_x) - tf2::getYaw(odometry_->pose.pose.orientation);
    double alpha_2 = std::atan2(y2 - rear_y, x2 - rear_x) - tf2::getYaw(odometry_->pose.pose.orientation);

    double steering_angle_1 = std::atan2(2.0 * wheel_base_ * std::sin(alpha_1), lookahead_distance_1);
    double steering_angle_2 = std::atan2(2.0 * wheel_base_ * std::sin(alpha_2), lookahead_distance_2);

    cmd.lateral.steering_tire_angle =
      steering_tire_angle_gain_ * (w_1_ * steering_angle_1 + w_2_ * steering_angle_2);
  }

  pub_cmd_->publish(cmd);
  cmd.lateral.steering_tire_angle /= steering_tire_angle_gain_;
  pub_raw_cmd_->publish(cmd);
}

bool SimplePurePursuit::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;
}

}  // namespace simple_pure_pursuit

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

シミュレーション

とりあえず動きました。
デフォルトのパラメータだとオーバーシュートしてしまいますので調整が必要です。

所感

実装は簡単でした。パラメータ増える分、自由度が上がりますが調整が大変です。
以下の自動調整が有効かもしれません。

単純にカーブ時の外側への膨らみを抑制したいのであれば、曲率に応じたフィードフォワード成分を追加することも選択肢としてはアリな気がしてきました。

※ ちなみに、Autoware.universe の lateral_controller - pure_pursuitは、曲率に応じてlook ahead distanceを延ばしているようです。

※ ROS 2 Nav2にも実装があるのでご参考まで。

おわりに

完全に思いつきでやってみました。理論的な解析が不足しているのは申し訳ありません。
Controlに課題がある場合の一助になれば幸いです。

またDARPAグランドチャレンジから20年ほど経ちますが、今なお使われているPure Pursuitってすごいなと改めて思いました。

この記事は、AI文章校正ツール「ちゅらいと」で校正されています。