はじめに

自動運転AIチャレンジ2023で自分で経路追従モジュールを作成するにあたりデフォルトのsimple_pure_pursuitを理解する。

simple_pure_pursuitのソースコードはこちら

入出力トピック

車両の位置情報と追従する経路情報をSubscribeして制御指令を出力するシンプルなものになっています。

トピックの種類	トピック名	メッセージタイプ	説明
入力	"input/kinematics"	nav_msgs/msg/Odometry	車両の状態。位置、速度、向きなど
入力	"input/trajectory"	autoware_auto_planning_msgs/msg/Trajectory	車両が追従する経路情報
出力	"output/control_cmd"	autoware_auto_control_msgs/msg/AckermannControlCommand	車両の操舵と速度の制御命令を含む制御指令値

Autoware固有のメッセージ型の中身はこちらで見れます。

アルゴリズムの概要

以下のtimer callbackの中身の計算について

details

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

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

    // calc lateral control
    //// calc lookahead distance
    double lookahead_distance = lookahead_gain_ * target_longitudinal_vel + lookahead_min_distance_;
    //// calc center coordinate of rear wheel
    double rear_x = odometry_->pose.pose.position.x -
                    wheel_base_ / 2.0 * std::cos(odometry_->pose.pose.orientation.z);
    double rear_y = odometry_->pose.pose.position.y -
                    wheel_base_ / 2.0 * std::sin(odometry_->pose.pose.orientation.z);
    //// search lookahead point
    auto lookahead_point_itr = 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;
      });
    if (lookahead_point_itr == trajectory_->points.end()) {
      lookahead_point_itr = trajectory_->points.end() - 1;
    }
    double lookahead_point_x = lookahead_point_itr->pose.position.x;
    double lookahead_point_y = lookahead_point_itr->pose.position.y;

    // calc steering angle for lateral control
    double alpha = std::atan2(lookahead_point_y - rear_y, lookahead_point_x - rear_x) -
                   tf2::getYaw(odometry_->pose.pose.orientation);
    cmd.lateral.steering_tire_angle =
      std::atan2(2.0 * wheel_base_ * std::sin(alpha), lookahead_distance);
  }
  pub_cmd_->publish(cmd);
}

こちらの資料のPure pursuitとほぼ同じ。
違いは速度0のときにlookahead distanceが0にならないようにlookahead_min_distanceを足している。

画像は上記記事から引用

1. 縦方向制御:

$v_{target} $ : 縦方向速度指令値
$v_{trajectry}$ : 最も近い経路の速度[Trajectoryトピックから参照]
$v_{current}$ : 現在の縦方向速度[Odometryトピックから参照]
$a$ : 縦方向加速度指令値
$K_p$ : 比例ゲイン

縦方向速度指令値は車体の位置に最も近い経路点のlongitudinal_velocity_mpsの値と同じにする

v_{target} = v_{trajectry}

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

2. 横方向制御（操舵角）

$l_d$ : Look ahead distance
$l_{min}$ : Look ahead distance の最小値
$k$ : Look ahead ゲイン
$L$ : Wheel Base 距離
$(x, y)$ : 現在の車体中心の位置[Odometryトピックから参照]
$(x_r, y_r)$ : 現在の車体後輪中心の位置
$(x_{lookahead}, y_{lookahead})$ : Target Pointの位置
$\psi$ : 現在の車体中心のYaw角 [Odometryトピックから参照]
$\delta$ : steering 角度指令値
$\alpha$ : $\psi$と前方参照距離との角度

Look ahead distance の計算、速度が早いほど遠い位置を見るようにする

l_d = k \cdot v_{target} + l_{min}

後輪の中心位置を計算

(x_r, y_r) = \left( x - \frac{L}{2} \cos(\psi), y - \frac{L}{2} \sin(\psi) \right)

操舵角の計算

\delta = \arctan\left(\frac{2 \cdot L \cdot \sin(\alpha)}{l_d}\right)

\alpha = \arctan\left(\frac{y_{lookahead} - y_r}{x_{lookahead} - x_r}\right) - \psi

最終的に縦方向の速度$v_{target} $と加速度$a$、操舵角$\delta$をAckermannControlCommandトピックに入れて出力する。

おわりに

simple_pure_pursuitのコードを数式にして参考資料と比較してみました。
最初はOSS連携としてPythonRoboticsのコードを移植しようとしてたのですが大変そうなので、このsimple_pure_pursuitを改造していこうと思います。
走行の様子を見ていると直線でゆっくり走っていたり、高速のままシケインに突っ込んだりしていたのでタイム短縮のためには縦方向の速度と加速度の制御が非常に重要だと感じました。

aichallenge2023-racingのsimple_pure_pursuitについてのメモ