【自動運転AIチャレンジ2024】障害物を回避する経路を生成する

先端技術部テクノロジーリサーチグループの色部です。

テクノロジーリサーチグループでは、2024年7月2日-9月2日の期間で、自動運転AIチャレンジ2024の予選に参加しています。自動運転AIチャレンジの詳細については、以下リンクを参照してください。

www.jsae.or.jp

本大会の予選は、AWSIMと呼ばれるオープンソース自動運転シミュレーターを用いた環境で行われます。ランダムで配置される障害物の点在するコースをカートが6周する時間をチームで競う、といった趣向となっています。

実装環境として、Autowareと呼ばれる自動運転ソフトウェアを使用します。運営より提供されるコードとワークスペースを利用することで、周回可能なカートを最初から取り扱うことができるようになっていますが、ランダムで出現する障害物を避けるためのパッケージまでは提供されていません。

そこで、こちらの記事では、障害物回避のための経路を生成させる方法について紹介します。

前提
path_to_trajectoryの修正
avoidObstacles関数の追加
smoothTrajectory関数の追加
おわりに

前提

前提として、大会用のリポジトリを実行できる環境設定が必要です。

環境構築の詳細については、以下リンクをご確認ください。*1

automotiveaichallenge.github.io

path_to_trajectoryの修正

まず、自動運転のための経路（Trajectory）のデータがどのように送信されているかについて確認します。

以下は、開発の手を加える前の、Trajectoryデータの流れをまとめた図になります。

図から分かるように、Trajectoryデータのメッセージは、「planning/scenario_planning/trajectory」のトピックとして、「/planning/scenario_planning/path_to_trajectory」ノードから、「/simple_pure_pursuit」ノードにパブリッシュされていることが分かります。

よって、障害物を避けるTrajectoryを生成する方法の一つとして、path_to_trajectoryパッケージ内で障害物を避ける処理を加えてみたいと思います。

avoidObstacles関数の追加

それでは実際に、path_to_trajectoryのスクリプトに、障害物を回避する機能を追加しましょう。

最初に、障害物の情報をサブスクライブする必要があるため、サブスクライバーを下記のように追加します。

sub_objects_ = this->create_subscription<MarkerArray>(
    "/aichallenge/objects_marker", 1, [this](const MarkerArray::SharedPtr msg) { objects_ = msg; });

そして、関数の最初のステップとして、障害物の情報が存在しなければ処理を終了します。これにより、無駄な計算を避けることができます。

if (!objects_) {
    return;
}

次に、障害物を回避するための安全距離と最大ステアリングオフセットを設定します。

安全距離は、障害物から車両が保持するべき最低限の距離を指します。この距離内に障害物が存在する場合、障害物を避けるための経路を調整します。

最大ステアリングオフセットは、車両が障害物を避けるためにどれだけ距離および方向を変更できるかの値を指します。具体的には、障害物に近づいたとき、どれだけの角度でハンドルを回すことができるかを制御します。

これらのパラメータは、壁との衝突有無や、障害物回避の精度によって調整してください。

constexpr double SAFETY_DISTANCE = 4.0; // 障害物からの安全距離
constexpr double MAX_STEERING_OFFSET = 3.4; // 最大ステアリングオフセット

また、経路の各ポイントに対して、障害物との距離を測定します。

for (auto & traj_point : trajectory.points) {
    ...
}

障害物のマーカーに対して、現在の距離を計算します。この距離が安全距離よりも小さければ、以降の回避処理を行います。

double distance = std::hypot(car_x - obj_x, car_y - obj_y);
if (distance < SAFETY_DISTANCE) {
    ...
}

障害物に近づくほど大きなステアリングオフセットを設定し、より急角度で回避します。

double steering_offset = MAX_STEERING_OFFSET * (SAFETY_DISTANCE - distance) / SAFETY_DISTANCE;

障害物の位置に基づいて、右または左に曲がる方向を決定します。

double direction = (obj_y > car_y) ? -1.0 : 1.0;

その他、障害物を避けるための経路ポイントを調整する処理を追加しますが、そちらはお好みで調整ください。

上記をまとめた、avoidObstacles関数の全体像は、以下の通りです。

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

  constexpr double SAFETY_DISTANCE = 4.0; // Increased safety distance in meters
  constexpr double MAX_STEERING_OFFSET = 3.4; // Maximum steering offset in meters

  // 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) {
        // Apply a larger steering offset based on proximity to the obstacle
        double steering_offset = MAX_STEERING_OFFSET * (SAFETY_DISTANCE - distance) / SAFETY_DISTANCE;
        
        // Determine direction to steer
        double direction = (obj_y > car_y) ? -1.0 : 1.0; // Steer right if object is on the left, otherwise steer left

        // Apply the steering offset
        traj_point.pose.position.y += direction * steering_offset;

        // Adjust the x position to ensure smooth transition
        traj_point.pose.position.x += (obj_x > car_x) ? -steering_offset / 2 : steering_offset / 2;

        // Adjust velocity and other parameters for smoother transition
        traj_point.longitudinal_velocity_mps *= (1.0 - distance / SAFETY_DISTANCE);
        traj_point.lateral_velocity_mps = direction * steering_offset;

        // Adjust acceleration to be higher when closer to the obstacle
        traj_point.acceleration_mps2 = (distance < 1.0) ? MAX_STEERING_OFFSET : MAX_STEERING_OFFSET / distance;

        traj_point.heading_rate_rps = direction * std::atan2(traj_point.pose.position.y - car_y, traj_point.pose.position.x - car_x);
        traj_point.front_wheel_angle_rad = direction * steering_offset / 2;
        traj_point.rear_wheel_angle_rad = -direction * steering_offset / 2;

        // Set time_from_start for smoother transition
        traj_point.time_from_start.sec = static_cast<int32_t>(distance / traj_point.longitudinal_velocity_mps);
        traj_point.time_from_start.nanosec = static_cast<uint32_t>((distance / traj_point.longitudinal_velocity_mps - traj_point.time_from_start.sec) * 1e9);
      }
    }
  }
}

smoothTrajectory関数の追加

avoidObstacles関数をpath_to_trajectoryに追加するだけでも障害物を回避することは可能になりますが、以下の図のように、ジグザグとしたTrajectoryが生成されるようになってしまいます。

そのため、障害物を回避するTrajectoryを滑らかにするための関数を追加します。

平滑化（スムージング）を行う前に、経路内のポイント数を確認します。ポイントが5未満の場合、スムージング処理を行う意味がないため、関数を早期に終了します。

if (trajectory.points.size() < 5) {
    return;
}

次に、スライディングウィンドウのサイズを設定します。ここでは、7つのポイントを考慮して、移動平均を計算します。*2

int window_size = 7; // Number of points to consider for moving average

そして、スムージング処理のループを行います。この部分では、指定したウィンドウサイズ内の各ポイントについて、x座標とy座標の移動平均を計算します。

for (size_t i = window_size / 2; i < trajectory.points.size() - window_size / 2; ++i) {
    double sum_x = 0.0;
    double sum_y = 0.0;
    for (int j = -window_size / 2; j <= window_size / 2; ++j) {
        sum_x += trajectory.points[i + j].pose.position.x;
        sum_y += trajectory.points[i + j].pose.position.y;
    }
    smoothed_trajectory.points[i].pose.position.x = sum_x / window_size;
    smoothed_trajectory.points[i].pose.position.y = sum_y / window_size;
}

その他、スムージングされた経路に対して、他のパラメータ（速度、加速度、角速度など）も同様の手順でスムーズ化の調整をすることもできますが、こちらについてもお好みで追加/削除してください。

上記をまとめた、smoothTrajectory関数の全体像は、以下の通りです。

void PathToTrajectory::smoothTrajectory(Trajectory &trajectory) {
    if (trajectory.points.size() < 5) {
        // Not enough points to smooth
        return;
    }
    Trajectory smoothed_trajectory = trajectory;
    int window_size = 7; // Number of points to consider for moving average
    for (size_t i = window_size / 2; i < trajectory.points.size() - window_size / 2; ++i) {
        double sum_x = 0.0;
        double sum_y = 0.0;
        for (int j = -window_size / 2; j <= window_size / 2; ++j) {
            sum_x += trajectory.points[i + j].pose.position.x;
            sum_y += trajectory.points[i + j].pose.position.y;
        }
        smoothed_trajectory.points[i].pose.position.x = sum_x / window_size;
        smoothed_trajectory.points[i].pose.position.y = sum_y / window_size;
    }
     // Directly assign the points to the trajectory
    trajectory.points = smoothed_trajectory.points;
}