【自動運転AIチャレンジ2024】壁を考慮した障害物回避

自動運転AIチャレンジ2024へ参加し、車両が障害物を回避するコードの開発を行ったので実装方法をご紹介します。

大会の詳細は以下のリンクでご確認いただけます。

www.jsae.or.jp

前提
障害物回避の概要
壁の位置を考慮した障害物回避
- 処理の追加
- 障害物回避の全体像
最後に

前提

この大会の予選は、Autowareと呼ばれるオープンソースの自動運転プラットフォームで開発を行い、AWSIMと呼ばれるオープンソースの自動運転シミュレーター環境でその性能を評価します。

自動運転AIチャレンジ2024はコース6週のタイムを競うもので、素早くコースを周回するためにランダムに配置された障害物をよける必要があります。

車両はtrajectory(経路)に沿って走行するように制御されており、障害物を避けるtrajectoryへ修正することで障害物回避を行うことが可能となっております。

Trajectoryと障害物の位置情報は(x,y)の座標情報として受け取ることができ、その情報をもとにtrajectoryを変更します。

障害物回避の概要

今回ご紹介する障害物回避は、以下の記事の方法をもとに作成しております。ベースとなるコードの詳しい内容はリンク先でご確認ください。

blog.jbs.co.jp

元となる障害物回避では、trajectoryと障害物の距離を計算し、指定した距離より近かった場合に以下の処理を行います。

障害物とtrajectoryの距離から車両のステアリング（操舵角度）を調整し、障害物を避けるためのオフセットを設定する
障害物とtrajectoryの位置関係から回避の方向を決定する
新しい座標を適用し、障害物を回避する座標に修正する

壁の位置を考慮した障害物回避

処理の追加

上記の方法で障害物を回避するtrajectoryの生成は可能となりますが、障害物回避前のtrajectoryがコースの中心ではなく壁寄りの場合、壁の方向へ回避してしまうことがありました。

そのため、コースの中心方向へ回避をするために次の処理を追加しました。

左右の壁の情報を取得する

module autoware_auto_planning_msgs {
  module msg {
    @verbatim (language = "comment", text=
      "Contains a PathPointWithLaneId path and left and right bound.")
    struct PathWithLaneId {
      std_msgs::msg::Header header;
      sequence<autoware_auto_planning_msgs::msg::PathPointWithLaneId> points;
      sequence<geometry_msgs::msg::Point> left_bound;
      sequence<geometry_msgs::msg::Point> right_bound;
    };
  };
};

PathWithLaneIdのメッセージはすでにサブスクライブしているので、trajectoryと類似した方法で取得が可能です。

// Copy left_bound and right_bound from the message
  left_bound_ = msg->left_bound;
  right_bound_ = msg->right_bound;

障害物から、指定した距離以内の壁のポイントを取得し座標の平均値を計算する

double sum_x = 0.0;
double sum_y = 0.0;
int count = 0;

// NEAR_OBSTACLE_DISTANCEは適切な値を選択
for (const auto &left_point : left_bound_) {
    if (std::hypot(left_point.x - obj_x, left_point.y - obj_y) < NEAR_OBSTACLE_DISTANCE) {
    sum_x += left_point.x;
    sum_y += left_point.y;
    count++;
    }
}

for (const auto &right_point : right_bound_) {
    if (std::hypot(right_point.x - obj_x, right_point.y - obj_y) < NEAR_OBSTACLE_DISTANCE) {
    sum_x += right_point.x;
    sum_y += right_point.y;
    count++;
    }
}

if (count > 0) {
    sum_x /= count;
    sum_y /= count;
} else {
// 壁のポイントを取得できなかった場合
    sum_x = obj_x;
    sum_y = obj_y;
}

障害物とtrajectoryの距離から車両のステアリング（操舵角度）を調整し、障害物を避けるためのオフセットを設定する(元のコードを利用)

// Apply a larger steering offset based on proximity to the obstacle
double steering_offset = MAX_STEERING_OFFSET * (SAFETY_DISTANCE - distance) / SAFETY_DISTANCE;

算出した壁の平均点とtrajectoreyの位置関係から回避方向を決定する

// Calculate direction toward the center
double direction_x = -(sum_x - car_x);
double direction_y = -(sum_y - car_y);
// Normalize direction vector
double length = std::sqrt(direction_x * direction_x + direction_y * direction_y);
direction_x /= length;
direction_y /= length;

新しい座標を適用し、障害物を回避する座標に修正する(元のコードを利用)

// Proposed new position toward the center
double new_x = car_x + direction_x * steering_offset;
double new_y = car_y + direction_y * steering_offset;
traj_point.pose.position.x = new_x;
traj_point.pose.position.y = new_y;

障害物回避の全体像

void PathToTrajectory::avoidObstacles(Trajectory &trajectory) {
  // Check if objects are available
  if (!objects_) {
    return;
  }

  //以下の値は適当な値に調整します。
  constexpr double SAFETY_DISTANCE = 3.4; // Increased safety distance in meters
  constexpr double MAX_STEERING_OFFSET = 2.5; // Maximum steering offset in meters
  constexpr double NEAR_OBSTACLE_DISTANCE = 3.2; // Distance to consider points near the obstacle


  // Iterate through the trajectory points
  for (auto &traj_point : trajectory.points) {
    for (const auto &marker : objects_->markers) {
      double obj_x = marker.pose.position.x;
      double obj_y = marker.pose.position.y;
      double car_x = traj_point.pose.position.x;
      double car_y = traj_point.pose.position.y;

      // Check if the object is close to the trajectory point
      double distance = std::hypot(car_x - obj_x, car_y - obj_y);
      if (distance < SAFETY_DISTANCE) {

        // Calculate center of the lane considering points around the obstacle
        double sum_x = 0.0;
        double sum_y = 0.0;
        int count = 0;

        for (const auto &left_point : left_bound_) {
          if (std::hypot(left_point.x - obj_x, left_point.y - obj_y) < NEAR_OBSTACLE_DISTANCE) {
            sum_x += left_point.x;
            sum_y += left_point.y;
            count++;
          }
        }

        for (const auto &right_point : right_bound_) {
          if (std::hypot(right_point.x - obj_x, right_point.y - obj_y) < NEAR_OBSTACLE_DISTANCE) {
            sum_x += right_point.x;
            sum_y += right_point.y;
            count++;
          }
        }

        if (count > 0) {
          sum_x /= count;
          sum_y /= count;
        } else {
          sum_x = obj_x;
          sum_y = obj_y;
        }

        // Apply a larger steering offset based on proximity to the obstacle
        double steering_offset = MAX_STEERING_OFFSET * (SAFETY_DISTANCE - distance) / SAFETY_DISTANCE;

        // Calculate direction toward the center
        double direction_x = -(sum_x - car_x);
        double direction_y = -(sum_y - car_y);
        // Normalize direction vector
        double length = std::sqrt(direction_x * direction_x + direction_y * direction_y);
        direction_x /= length;
        direction_y /= length;

        // Proposed new position toward the center
        double new_x = car_x + direction_x * steering_offset;
        double new_y = car_y + direction_y * steering_offset;

        traj_point.pose.position.x = new_x;
        traj_point.pose.position.y = new_y;
      }
    }
  }
}