概要
「初心者がAI Challengeやってみた」シリーズの第10弾です。
第9弾では障害物回避のための軌道生成を目指して目的地まで経路を生成するモジュールを作成しました。
今回は障害物の位置も取得して障害物を回避しながら目的地に向かうプログラムを作成します。
本シリーズでは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)：障害物回避モジュールの作成①
筆者はautoware初心者です。
説明等が正確でない可能性があるので本記事だけではなく他の記事やautowareのドキュメントも確認するようにしてください。
目標
課題を細分化すると以下のようになります。
前回の作業で最初の2点はクリアしているので、今回は３つ目の障害物位置の設定を目標に作業をします。
- 目的地の設定
- 走行可能エリアの設定
- 障害物の位置の設定
- その他交通ルールの設定
Occupancy Grid Map の表示
障害物の位置を取得する方法は色々あると思いますが、今回はOccupancy Grid Mapを使用します。
Occupancy Grid Mapとは障害物等により占有されている場所とそうでない場所を示すグリッドマップで、自立走行ロボット等の経路生成の際によく使われます。
通常バージョンのAutoware（Autoware-Miniじゃない方）だと Occupancy Grid Mapを配信するノードが走っています。
aichallenge_submit.launch.xmlで通常バージョンのAutowareを起動するように設定し、RVizで Perception > OccupancyGridにチェックをすると以下のように確認することができます。
黒＝障害物、暗い灰色＝未知、白（明るい灰色）＝走行可能、という感じです。
あくまでセンシングをベースとしているので走行不可エリア等の交通ルールは加味されていません。
Autoware-Mini で表示する
Autoware-Miniではデフォルトではoccupancy grid mapを配信するノードが設定されていないので、上記方法でRVizを変更しても何も表示されません。
autoware_mini_awsim.launch.xmlに以下の"occupancy grid map module"部分を追加するとOccupancy Grid Mapを配信するノードの設定ができます。
（perception.launch.xmlやpointcloud_based_occupancy_grid_map.launch.pyを参考にしました。）
<!-- Perception -->
<group>
<push-ros-namespace namespace="perception"/>
<!-- obstacle_segmentation -->
<group>
<!-- 割愛 -->
</group>
<!-- occupancy grid map module -->
<group>
<push-ros-namespace namespace="occupancy_grid_map"/>
<node_container pkg="rclcpp_components" exec="component_container" name="occupancy_grid_map_container" namespace="">
<composable_node pkg="probabilistic_occupancy_grid_map" plugin="occupancy_grid_map::PointcloudBasedOccupancyGridMapNode" name="occupancy_grid_map_node" namespace="">
<remap from="~/input/obstacle_pointcloud" to="/perception/obstacle_segmentation/pointcloud" />
<remap from="~/input/raw_pointcloud" to="/perception/obstacle_segmentation/range_cropped/pointcloud" />
<remap from="~/output/occupancy_grid_map" to="/perception/occupancy_grid_map/map" />
<param name="map_resolution" value="0.5" />
<param name="use_height_filter" value="true" />
</composable_node>
</node_container>
</group>
Autoware-Miniを起動してRVizを設定するとOccupancy Grid Mapを表示することができました。
障害物回避経路の生成
まずはOccupancy Grid Mapにサブスクライブして障害物位置を取得します。Autowareの Occupancy Grid Map についてはここに書いてありますが、今回はあまり深く考えずにやってみました。
Occupancy Gridのセルの値が一定以上の場合そこに障害物があると判断します。Autowareではセルの最大値は50らしく、これが占有されていることを示しているようです。実際には障害物がないけど障害物の影になっていて観測できていない部分も占有されているという判定がされているっぽいです。（これはちゃんと確認してないので間違ってるかもしれないです。）
経路の生成
次に各障害物から反発する力を計算して、第９弾で作った目的地からの引き寄せる力を合わせてポテンシャル場を計算、そこから経路を作成しました。全部の障害物ではなく、車両から一定以内の距離にある障害物のみ反発する力を計算しています。（コードは最後の部分に貼ってあります。）
中心の緑色の点がスタート地点、赤色の点が目標地点、青色の点が経路、そして黒い点が障害物です。
これを実際のマップと重ねてみると、それっぽい感じのものが作れていることが分かります。
私のコードの書き方の問題かもしれませんが、目的地からの引き寄せる力と障害物からの反発する力の重みの調整が大事になってきます。
障害物が多い分、重みを下げてあげないとゴールに近づくよりも障害物から遠ざかるような経路になってしまいます。
逆に目的地の重みを上げすぎると障害物にかなり接近しながらほぼ一直線にゴールに向かうような経路が生成されます。
目的地や障害物の位置などによるので一概には言えませんが、いくつかの例を下に表示します。ヘッダーは「引き寄せる力:反発する力」の比率です。
| 1:1 | 1000:1 | 5000:1 | 10000:1 |
|---|---|---|---|
 |
 |
 |
 |
1:1は障害物に囲まれているためかその場にとどまるような経路です。
1000:1は障害物を迂回しながら目的地に向かっています。
5000:1は勢いよく障害物の隙間を狙うも、そこから動けなくなっています。
10000:1は障害物の細かい隙間をそって目的地に向かっています。
前述したようにマップの状況等によってベストな値は変わってきます。
以下で説明する方法では比率は500:1でした。
Autoware-Miniに組み込む
せっかくそれっぽい経路が生成できたのでこれで車両を動かしてみたいと思います。
本来であればTrajectoryを配信してコントロール部分に指令を出すのですが、Trajectoryには色々な情報が必要で作るのが大変そうです。
なので、上記で作った経路をPathとして配信、Obstacle Avoidance PlannerにこれをTrajectoryに変換してもらうという形を取ろうと思います。Obstacle Avoidance Plannerはパラメータによっては障害物回避機能とかをほぼ無効にできるっぽいので単純なPath-Trajectory変換器として使用できそうです（実際Autoware-Miniではパラメータ調整なしだと障害物回避を一切してくれません）。
autoware_mini_awsim.launch.xmlでトピックをremapしたりしてうまい具合に組み込みます。
まず従来ですと以下のような流れてPath→Trajectoryが生成されています：
-
behavior path plannerが/planning/scenario_planning/lane_driving/behavior_planning/pathトピックを配信 -
obstacle avoidance plannerが上記トピックにサブスクライブして交通可能エリア等を考慮した軌道に変換し、obstacle_avoidance_planner/trajectoryトピックを配信する -
obstacle stop plannerが上記トピックにサブスクライブして/planning/scenario_planning/trajectoryトピックを配信する - 上記トピックがコントロール部分にサブスクライブされて車両が動く
上記のステップ１と２の間に今回作成したポテンシャル場モジュールを組み込みます。また、ステップ３の役割があまりよく分からなかったのでこれをスキップしちゃいます。
-
behavior path plannerが/planning/scenario_planning/lane_driving/behavior_planning/pathトピックを配信 -
potential field plannerが上記トピックにサブスクライブして、障害物位置等を考慮した経路を生成、/potential_planner/pathを配信する -
obstacle avoidance plannerが上記トピックにサブスクライブして交通可能エリア等を考慮した軌道に変換し、/planning/scenario_planning/trajectoryトピックを配信する - 上記トピックがコントロール部分にサブスクライブされて車両が動く
autoware_mini_awsim.launch.xmlの変更点は以下の通りです：
<!-- motion_planning -->
<group>
<push-ros-namespace namespace="motion_planning"/>
<!-- motion_planning_container -->
<node_container pkg="rclcpp_components" exec="component_container" name="motion_planning_container" namespace="">
<!-- obstacle_avoidance_planner::ObstacleAvoidancePlanner -->
<composable_node pkg="obstacle_avoidance_planner" plugin="obstacle_avoidance_planner::ObstacleAvoidancePlanner" name="obstacle_avoidance_planner" namespace="">
- <remap from="~/input/path" to="/planning/scenario_planning/lane_driving/behavior_planning/path" />
+ <remap from="~/input/path" to="/potential_planner/path" />
<remap from="~/input/odometry" to="/localization/kinematic_state" />
- <remap from="~/output/path" to="obstacle_avoidance_planner/trajectory" />
+ <remap from="~/output/path" to="/planning/scenario_planning/trajectory" />
<!-- 割愛 -->
</composable_node>
<!-- 割愛 -->
</node_container> <!-- motion_planning_container -->
</group> <!-- motion_planning -->
+<!-- Potential Field Planner -->
+<group>
+ <node pkg="potential_field_planner_py" exec="potential_field_planner"/>
+</group> <!-- Potential Field Planner -->
結果
パラメータチューニングを一切せずに障害物をある程度避けてくれます。
ただし障害物に近づきすぎるとOccupancy Grid Map上からその障害物が消えてしまうらしく、障害物に激突してしまいます。
車酔いしそうなクネクネした動きなので自動運転車両としてはあまりいい動きではないかもしれませんね。
比較として、今回作成した作成したモジュールを外した状態でも動かしてみます。
こちらも同様にパラメータチューニングは一切していません。
この場合は最初の障害物の前で一切動かなくなり、しっかりとポテンシャル法のおかげで障害物回避（？）ができていたことが分かります。
コード
import numpy as np
import matplotlib.pyplot as plt
import rclpy
from rclpy.node import Node
from autoware_auto_planning_msgs.msg import Path
from geometry_msgs.msg import Pose, PoseStamped, PoseWithCovarianceStamped
from nav_msgs.msg import OccupancyGrid
class PotentialFieldPlanner(Node):
def __init__(self) -> None:
super().__init__("potential_field_planner")
self.potential_calculated = False
self.position_sub = self.create_subscription(
PoseWithCovarianceStamped,
"/localization/pose_with_covariance",
self.position_callback,
10,
)
self.path_sub = self.create_subscription(
Path,
"/planning/scenario_planning/lane_driving/behavior_planning/path",
self.path_callback,
10,
)
self.occupancy_grid_sub = self.create_subscription(
OccupancyGrid,
"/perception/occupancy_grid_map/map",
self.occupancy_grid_callback,
10,
)
self.path_pub = self.create_publisher(
Path,
"/potential_planner/path",
10,
)
def position_callback(self, msg: PoseWithCovarianceStamped):
self.current_x = msg.pose.pose.position.x
self.current_y = msg.pose.pose.position.y
def path_callback(self, msg: Path):
new_path = msg
tmp_goals = msg.points
last_goal = tmp_goals[-1]
sim_x = self.current_x
sim_y = self.current_y
ox, oy = [], []
for i in range(len(self.occupancy_data)):
if self.occupancy_data[i] > 25.0:
x = self.occupancy_info.origin.position.x + (i % self.occupancy_info.width) * self.occupancy_info.resolution
y = self.occupancy_info.origin.position.y + (i // self.occupancy_info.width) * self.occupancy_info.resolution
#self.get_logger().info(f"{self.occupancy_data[i]}")
ox.append(x)
oy.append(y)
direction = self.compute_grad(sim_x, sim_y, last_goal.pose.position.x, last_goal.pose.position.y, ox, oy)
traj_x, traj_y = [], []
for step in range(len(tmp_goals)):
tmp_goals[step].pose.position.x = sim_x
tmp_goals[step].pose.position.y = sim_y
traj_x.append(sim_x)
traj_y.append(sim_y)
direction = self.compute_grad(sim_x, sim_y, last_goal.pose.position.x, last_goal.pose.position.y, ox, oy)
sim_x += direction[0] * 0.2
sim_y += direction[1] * 0.2
new_path.points = tmp_goals
self.path_pub.publish(new_path)
def occupancy_grid_callback(self, msg: OccupancyGrid):
self.occupancy_info = msg.info
self.occupancy_data = msg.data
return
def compute_grad(self, x, y, gx, gy, ox, oy):
dx, dy = 0.01, 0.01
curr_pot = self.compute_potential(x, y, gx, gy, ox, oy)
dp_dx = (self.compute_potential(x + dx, y, gx, gy, ox, oy) - curr_pot) / dx
dp_dy = (self.compute_potential(x, y + dy, gx, gy, ox, oy) - curr_pot) / dy
grad = np.array([dp_dx, dp_dy])
norm_grad = grad / np.linalg.norm(grad)
return norm_grad
def compute_potential(self, x, y, gx, gy, ox, oy):
w_attract, w_repulse = 500, 1
attract = self.compute_attractive_potential(x, y, gx, gy)
repulse = self.compute_repulsive_potential(x, y, ox, oy)
return w_attract * attract + w_repulse * repulse
def compute_attractive_potential(self, x, y, gx, gy):
return 1.0 / np.hypot(x - gx, y - gy)
def compute_repulsive_potential(self, x, y, ox, oy):
repulse = 0.0
for i in range(len(ox)):
dist = np.hypot(x - ox[i], y - oy[i])
if dist < 10.0:
repulse += -1.0 / dist
return repulse
def main(args=None):
rclpy.init(args=args)
potential_field_planner = PotentialFieldPlanner()
rclpy.spin(potential_field_planner)
potential_field_planner.destroy_node()
rclpy.shutdown()
if __name__ == '__main__':
main()
今後の予定
今回の実装で最初の３つの課題はある程度クリアできたと思います。
- 目的地の設定
- 走行可能エリアの設定
- 障害物の位置の設定
- その他交通ルールの設定
次回以降の取り組む課題としては以下のことを考えています：
- 障害物に接近したらOccupancy Grid Mapから障害物が消える問題を解決する：接近して障害物が消えるとそのまま衝突してしまうため
- 障害物の前で一時停止する機能を追加する：最初のダンボールが動くのを待たずに回避しようとしてしまうため