double_point_pure_pursuit を実装する

# はじめに

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

https://www.jsae.or.jp/jaaic/2024ver/summary/

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

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

https://qiita.com/porizou1/items/d7574f651116cdcca269

https://tech.tier4.jp/entry/2019/03/29/173543

# Multiple Look ahead points への拡張

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

https://www.jstage.jst.go.jp/article/jsaeronbun/45/2/45_20144284/_pdf

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

https://www.jstage.jst.go.jp/article/jsmetld/2016.25/0/2016.25_1131/_pdf/-char/ja

https://www.sciencedirect.com/science/article/abs/pii/S1537511012001298

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

## 実装

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

<details><summary>double_point_pure_persuit.cpp</summary>

```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;
}


```
</details>

## シミュレーション

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

![ezgif-2-7e78219939.gif](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/3628135/c401d8b7-9460-4639-4b3a-9555e92d5010.gif)

## 所感

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

https://qiita.com/soyaoki/items/cf1b6573d6130ccdf5ab

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

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

https://github.com/AutomotiveAIChallenge/autoware.universe/blob/awsim-stable/control/pure_pursuit/src/pure_pursuit/pure_pursuit_lateral_controller.cpp#L109

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

https://github.com/ros-navigation/navigation2/tree/main/nav2_regulated_pure_pursuit_controller

# おわりに

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

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

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