初心者がAIチャレンジやってみた（13）：障害物回避モジュールの改良③

概要

「初心者がAI Challengeやってみた」シリーズの第13弾です。
今回も前回と同様に第10弾で作成した障害物回避モジュールを改良していきます。

本シリーズではJapan Automotive AI Challenge 2023にautoware初心者の筆者が試行錯誤しながら挑戦する記録を公開しています。自動運転に興味があるけどプログラミングに自信が無い方などの参考になれば幸いです。

前の記事はこちら：

• 第1弾：初心者がAIチャレンジやってみた(1)：Autowareを動かしてみる
• 第2弾：初心者がAIチャレンジやってみた(2)：1つ目の障害物の回避成功
• 第3弾：初心者がAIチャレンジやってみた(3)：2つ目の障害物の回避成功
• 第4弾：初心者がAIチャレンジやってみた(4)：全障害物回避達成！（全パラメータ公開）
• 第5弾：初心者がAIチャレンジやってみた(5)：開発環境の再構築（番外編）
• 第6弾：初心者がAIチャレンジやってみた(6)：Autoware-Miniを使ってみる
• 第7弾：初心者がAIチャレンジやってみた(7)：外部モジュール導入（失敗）
• 第8弾：初心者がAIチャレンジやってみた(8)：自作パッケージの導入
• 第9弾：初心者がAIチャレンジやってみた(9)：障害物回避モジュールの作成①
• 第10弾：初心者がAIチャレンジやってみた(10)：障害物回避モジュールの作成②（成功？）
• 第11弾：初心者がAIチャレンジやってみた(11)：障害物回避モジュールの改良①
• 第12弾：初心者がAIチャレンジやってみた(12)：障害物回避モジュールの改良②

筆者はautoware初心者です。
説明等が正確でない可能性があるので本記事だけではなく他の記事やautowareのドキュメントも確認するようにしてください。

目標

• ポテンシャル場を可視化
• 最後の障害物を回避
• 最初の障害物を待機

ポテンシャル場を可視化

前回の記事にも書いたように、最後の障害物がうまく回避できない理由はポテンシャル場に問題があると考えました。

これを検証するためにポテンシャル場を可視化するプログラムを書きました。簡単に説明すると車両周囲の各ポイントのポテンシャル場を計算し、それらをヒートマップで表示するようなものです。

コードの一部抜粋：

def plot_potential_field(self, gx, gy, path_points):
    num_points = 50
    dist_range = 10
    x = np.linspace(self.current_x - dist_range, self.current_x + dist_range, num_points)
    y = np.linspace(self.current_y - dist_range, self.current_y + dist_range, num_points)
    X, Y = np.meshgrid(x, y)
    pot_field = np.zeros((num_points, num_points))

    for x_idx in range(len(x)):
        for y_idx in range(len(y)):
            pot_field[x_idx][y_idx] = self.compute_potential(x[x_idx], y[y_idx], gx, gy)

    norm = colors.Normalize(vmin=np.min(pot_field), vmax=np.max(pot_field))
    norm_pot_field = norm(pot_field)

    im = self.ax2.imshow(norm_pot_field, cmap='gnuplot')
    if len(plt.gcf().axes) <= 1:
        self.fig2.colorbar(im, ax=self.ax2)
    
    path_x, path_y = [], []
    for i in range(len(path_points)):
        # Confusing but x and y axis must be flipped
        path_x.append(((path_points[i].pose.position.y - self.current_y) / dist_range * num_points / 2) + num_points / 2)
        path_y.append(((path_points[i].pose.position.x - self.current_x) / dist_range * num_points / 2) + num_points / 2)
    self.sc2.set_offsets(np.c_[path_x, path_y])

    plt.pause(0.1)

結果

以下のようにヒートマップでポテンシャル場を表示することができました。

明るい箇所ほどポテンシャル場の値が大きく、暗い場所ほど値が小さいです。
ポテンシャル法ではより暗い場所を目指すように経路が生成されます。
実際に障害物や走行レーン（明るい箇所）を避けながら目的地（暗い場所）に向かうように経路が生成されていることが分かります。

デバッグ

今回ポテンシャル場を可視化するに当たり、車両が一切移動していないのに時間経過とともにポテンシャル場の値が全体的に増加していく現象が見られました。

調べてみたところ障害物の位置を登録する際に、コールバックが呼ばれるたびに同じ障害物が重複して何回も登録されてしまっていました。そのせいで同じ障害物による反発する力が何度も計算されてしまい、ポテンシャル場の数値に影響していました。

新たに障害物を登録する際はすでに同じ座標上に障害物が登録されていないかを確認し、登録されていない場合のみ新規登録をするようにしました。

コードの一部抜粋：

def occupancy_grid_callback(self, msg: OccupancyGrid):
    self.ogm_info = msg.info
    self.ogm_data = msg.data

    new_ox, new_oy = [], []
    for i in range(len(self.obstacle_ox)):
        dist = np.hypot(self.current_x - self.obstacle_ox[i], self.current_y - self.obstacle_oy[i])
        if dist < OBSTACLE_DIST_THRESHOLD:
            new_ox.append(self.obstacle_ox[i])
            new_oy.append(self.obstacle_oy[i])
    self.obstacle_ox = new_ox
    self.obstacle_oy = new_oy

    for i in range(len(self.ogm_data)):
        if self.ogm_data[i] > 90.0:
            x = self.ogm_info.origin.position.x + (i % self.ogm_info.width) * self.ogm_info.resolution
            y = self.ogm_info.origin.position.y + (i // self.ogm_info.width) * self.ogm_info.resolution

+           # 登録する障害物の位置座標がすでに登録済みかを確認
+           dup_x = [i for i, tmp_x in enumerate(self.obstacle_ox) if tmp_x == x]
+           dup_y = [i for i, tmp_y in enumerate(self.obstacle_oy) if tmp_y == y]
+           dup_idx = set(dup_x) & set(dup_y)
+           if (dup_idx):
+               pass
+           else:
                dist = np.hypot(self.current_x - x, self.current_y - y)
                if dist < OBSTACLE_DIST_THRESHOLD:
                    self.obstacle_ox.append(x)
                    self.obstacle_oy.append(y)

最後の障害物の回避

ヒートマップで最後の障害物付近のポテンシャル場を可視化すると以下のようになりました。
障害物はもちろんですが、その横の走行可能なスペースの値も車両の現在地の値よりも少し高くなってしまっています（色の違いが微妙で分かりづらいですが）。

ポテンシャル法では値が小さい場所を目指して経路が生成されるので、その場でとどまるような経路になってしまうのはこれが原因です。

恐らくは障害物が大きく、障害物の点群の数が多い（密集している）影響でそこからの反発力も強くなり、走行可能な場所までもが高いポテンシャルを持ってしまっているのだと思います。
そこで、以下の変更を加えました：

• ポテンシャルに上限をつける
  →今回のように障害物が密集している場所の影響を抑える
• 引き寄せる力を強くする
  →障害物の反発力を上回る引力を発生させる

結果

無事に障害物に一切衝突することなく進行することができました！
（前回同様最初の障害物は無視しています。）

ただし回避をする時はレーンギリギリを走行することが多く、最後の障害物を回避する際は少しだけレーンからはみ出てしまいました。
また、最後の障害物を回避したはいいものの、その直後のカーブを曲がることができませんでした。

ヒートマップを見る限り走行レーンからの反発力が弱そうなので今後はここを修正してみます。