自動運転AIチャレンジに向けてPythonで自作プランナーを作成してみた

本記事の概要

2023年の6月から11月まで実施された自動運転AIチャレンジ（インテグレーション大会）に出場した際に、Pythonを使った自作プランナーを作成したのでその内容について記載します。

Autoware

自動運転AIチャレンジではAutowareという自動運転システム向けソフトウェアを使用します。Autowareの詳細は公式ページをご参照ください。
　AutowareにはPlanning, Localization, Control, Perception　など複数の自動運転向けモジュールが含まれており、今回は経路計画のための Planning モジュール（図の黄色部分）向けに追加機能を開発することを目指します。

大会用リポジトリ

自動運転AIチャレンジ（インテグレーション大会）では、参加者は大会用リポジトリをカスタマイズして、競技に挑みました。私もこちらの環境をもとに開発を行っております。

開発方針

　Autoware のソフトの完成度は高く、直接コードを変更して、機能をカスタマイズすることは高度な知識と理解が必要になり、初心者の私には困難だと考えたため、今回はあきらめました。一方、Planningモジュール内は機能ごとに、ROS2のノードが分けられており、推定した経路情報をトピック通信により、共有されています。そのため、推定された経路情報のトピックを受信し、新たに推定した経路情報のトピックを発信するノードを開発することで、簡易的に経路計画をカスタマイズすることができます。

CrankDriving Planner

　大会において、狭路のコースを走行する必要があったため、今回は狭路コースを走行するノード(CrankDriviPlanner）を作成しました。

CrankDriving Plannerの概要

　今回は 図1のように behavior_velocity_planner から出力された経路情報(Path)を受信し、道幅や自己位置に応じて経路を修正することを考えました。

　まず、CrankDriving Plannerでは走行中の道が狭路か狭路でないかを判断し、狭路の場合は受信したPath データを修正し、トピックとして発信します。その際、その他のノードによって、経路を再修正されることを防ぐため、そのまま Control モジュールが受信できるトピック名で発信します。
逆に、狭路でない場合は受信した経路情報を変更せず、そのままのPath データを発信します。この場合は、障害物の回避などが必要になることがあるため、obstacle_avoidance_planner（物体回避機能ノード）が受信できるトピック名で発信します。

CrankDriving Plannerの入出力

開発するノードでは以下の情報を入出力データとします。

• Input

トピック名	型	説明
~/input/path	autoware_auto_planning_msgs/msg/Path	変更前の経路情報
~/input/acceleration	geometry_msgs/AccelWithCovarianceStamped	アクセル情報
~/input/odometry	nav_msgs/msg/Odometry	オドメトリ情報
~/input/perception	autoware_auto_perception_msgs/msg/PredictedObjects	検知した物体情報

ここで、狭路か狭路ではないかの判断のためにアクセル情報、オドメトリ情報、検知した物体情報も同時に受信します。

• Output

トピック名	型	説明
~/output/path	autoware_auto_planning_msgs/msg/Path	変更後または元の経路情報

PublisherとSubscriberの定義

上記の入出力データに従って、PublisherとSubscriber を定義します。

CrankDriviPlanner.py

class CrankDrivingPlanner(Node):
    def __init__(self):
        super().__init__('CrankDrigingPlanner')
        self.get_logger().info("Start CrankDrigingPlanner")
        
        ## 更新前の経路情報向けの subscriber##
        self.create_subscription(Path ,"~/input/path", self.onTrigger, 10)

        ## Accel情報向けの subscriber ##
        self.create_subscription(AccelWithCovarianceStamped, "~/input/acceleration", self.onAcceleration, 10)

        ## オドメトリ情報向けの subscriber ##
        self.create_subscription(Odometry, "~/input/odometry", self.onOdometry, 10)

        ## 検知した物体情報向けの subscriber ## 
        self.create_subscription(PredictedObjects, "~/input/perception", self.onPerception, 10)

        ## 更新後のPath のpublisher ##
        self.pub_path_ = self.create_publisher(Path, 
                                               "~/output/path", 
                                                10)

    ## オドメトリ情報向けのコールバック関数 ##
    def onOdometry(self, msg: Odometry):
        self.current_odometry = msg
        self.ego_pose = self.current_odometry.pose.pose
        self.crrent_vel_x = self.current_odometry.twist.twist.linear.x
        self.crrent_vel_y = self.current_odometry.twist.twist.linear.y

    ## Accel情報向けのコールバック関数 ##
    def onAcceleration(self, msg: AccelWithCovarianceStamped):
        self.current_accel = [msg.accel.accel.linear.x, msg.accel.accel.linear.y, msg.accel.accel.linear.z]

    ## 検知した物体情報向けのコールバック関数 ##
    def onPerception(self, msg: PredictedObjects):
        self.dynamic_objects = msg

onOdometry, onAcceleration, onPerception ではそれぞれ受信したmsg データを、クラス変数に格納しています。

狭路の判断

　狭路の判断は地図情報やカメラ情報から行う方法があると思いますが、今回は走行するコースが決まっていたため、ヒューリスティックになりますが道幅と曲がり角の路肩の角度から判断させることにしました。

① 道幅とカーブの内側の角度を取得

まず、受信したPath データにはleft_boud と right_bound という推定した走行可能範囲の情報が含まれています。今回はそれらの情報から、カーブ後の道幅とカーブの角度を導出します。Path のデータ構造についてはこちらをご参照ください。

② 道幅と角度を閾値と比較

次に、あらかじめ設定した閾値と比較し、狭路かどうかを決定します。今回のコースでは３パターンの狭路がコース内に存在したので、それぞれ閾値を設定して判別しました。

道幅と角度の取得部分のコードは下記になります。

CrankDriviPlanner.py

import numpy as np
from geometry_msgs.msg import Quaternion
from autoware_auto_planning_msgs.msg import Path

# カーブの角度計算
def getCosFromLines(p1: np.array, p2: np.array, p3: np.array) -> float:
    vec_1 = p1 - p2
    vec_2= p3 - p2
    d1  = np.hypot(vec_1[0], vec_1[1])
    d2  = np.hypot(vec_2[0], vec_2[1])
    cos = np.dot(vec_1, vec_2) / (d1 * d2)
    return cos

# 正規ベクトルの取得
def getNormVec(p: np.array, q: np.array) -> np.array:

    vec = q - p
    vec /= np.hypot(vec[0], vec[1])
    return vec

# 交点の取得
def getCrossPoint(p1: np.array, vec1: np.array, p2: np.array, vec2: np.array) -> np.array:
    d = vec1[0] * vec2[1] - vec2[0] * vec1[1]
    if d == 0:
         return None
    sn = vec2[1] * (p2[0] - p1[0]) - vec2[0] * (p2[1] - p1[1])
    print(sn / d)
    return np.array([p1[0] + vec1[0] * (sn / d), p1[1] + vec1[1] * (sn / d)])

# 2点の距離計算
def calcDistancePoits(point_a: list, point_b: list) -> float:
    if len(point_a) != len(point_b):
        return None
    return np.linalg.norm(np.array(point_a) - np.array(point_b))

# 道幅の計算
def getRoadWidth(inner_bound: np.array, outer_bound: np.array, diag_idx: int, sharp_idx: int) -> float:
    inner_backward_vec = getNormVec(inner_bound[diag_idx], inner_bound[diag_idx - 1])
    outer_forward_vec = getNormVec(outer_bound[sharp_idx], outer_bound[sharp_idx + 1])
    inner_finish_point = inner_bound[diag_idx + 1]
    outer_finish_point = getCrossPoint(inner_bound[diag_idx + 1], 
                                            -1 *inner_backward_vec,
                                            outer_bound[sharp_idx],
                                            outer_forward_vec)
    return calcDistancePoits(outer_finish_point, inner_finish_point)

# 複数の点の距を計算
def calcDistancePoitsFromArray(point_a: np.array, points: np.array) -> np.array:
    dist = points[:, 0:2] - point_a[0:2]
    dist = np.hypot(dist[:, 0], dist[:, 1])
    return dist

# QuaternionをYawに変換
def getYawFromQuaternion(orientation: Quaternion):
    siny_cosp = 2 * (orientation.w * orientation.z + orientation.x * orientation.y)
    cosy_cosp = 1 - 2 * (orientation.y * orientation.y + orientation.z * orientation.z)
    return np.arctan2(siny_cosp, cosy_cosp)

# 現在と次のパスのindex を取得
def get_nearest_path_idx(ego_pose, bound, next_path_threshold=5.0):
    diff = calcDistancePoitsFromArray(ego_pose, bound)
    current_path_index = diff.argmin()
    
    next_path_index = None
    for idx in range(len(diff)):
        if diff[idx] > next_path_threshold:
            next_path_index = idx
            break

    if next_path_index is None or next_path_index == 0:
        next_path_index = current_path_index + 1
    
    return current_path_index, next_path_index

# Points型をnumpyに変換
def ConvertPointSeq2Array(points: list) -> np.array:
    k = []
    for i in range(len(points)):
        k.append([points[i].x, points[i].y])
    return np.array(k)

# Point型をnumpyに変換
def ConvertPoint2List(p) -> np.array:
    yaw = getYawFromQuaternion(p.orientation)
    return np.array([p.position.x, p.position.y, yaw])

# カーブの角の点の取得
def get_sharp_point(bound):
    sharp_index = None
    if len(bound) < 3:
        return sharp_index

    for idx in range(2, len(bound)):
        cos= getCosFromLines(bound[idx - 2], bound[idx - 1], bound[idx])
        if (cos < 0.2 and cos > -0.2):
            sharp_index = idx -1
            break
    return sharp_index

# カーブの内側の曲がり始めの点
def get_diag_point(bound):
    diag_idx = None
    if len(bound) < 3:
        return diag_idx
    for idx in range(2, len(bound)):
        cos= getCosFromLines(bound[idx - 2], bound[idx - 1], bound[idx])
        if (cos < 0.85 and cos > 0.4) or (cos < -0.4 and cos > -0.85):
            diag_idx = idx -1 
            break
    return diag_idx

# 経路情報向けのコールバック関数
def onTrigger(self, msg: Path):
    self.reference_path = msg
    # パスをnumpy 型に変換
    self.left_bound = ConvertPointSeq2Array(self.reference_path.left_bound)
    self.right_bound = ConvertPointSeq2Array(self.reference_path.right_bound)

    # 自己情報を numpy 型に変換
    ego_pose_array = ConvertPoint2List(self.ego_pose)

    # 現在のパスのindex を取得
    self.current_left_path_index ,self.next_left_path_index = \
        get_nearest_path_idx(ego_pose_array, self.left_bound, next_path_threshold=5.0)

    self.current_right_path_index ,self.next_right_path_index = \
        get_nearest_path_idx(ego_pose_array, self.right_bound, next_path_threshold=5.0)

    ### 角度の取得 ####
    cos_left  = getCosFromLines(self.left_bound[self.current_left_path_index - 1], 
                                self.left_bound[self.current_left_path_index],
                                self.left_bound[self.current_left_path_index + 1])

    cos_right = getCosFromLines(self.right_bound[self.current_right_path_index - 1],
                                self.right_bound[self.current_right_path_index],
                                self.right_bound[self.current_right_path_index + 1])

    cos_left_next = getCosFromLines(self.left_bound[self.current_left_path_index], 
                                    self.left_bound[self.current_left_path_index + 1],
                                    self.left_bound[self.current_left_path_index + 2])
                
    cos_right_next = getCosFromLines(self.right_bound[self.current_right_path_index],
                                     self.right_bound[self.current_right_path_index + 1],
                                     self.right_bound[self.current_right_path_index  + 2])
    
    # 右回りか左回りかの判定
    if(cos_left_next < 0.2 and  cos_left_next> -0.2) or (cos_left < 0.2 and  cos_left > -0.2):
        self.vehicle_state = "S-crank-right"

    elif (cos_right_next < 0.2 and cos_right_next > -0.2) or  (cos_right < 0.2 and cos_right > -0.2):
        self.vehicle_state = "S-crank-left"


    if self.vehicle_state == "S-crank-right":
        outer_bound= self.left_bound
        inner_bound = self.right_bound
    elif self.vehicle_state ==  "S-crank-left":
        outer_bound = self.right_bound
        inner_bound= self.left_bound

    # カーブの内側の曲がり始めの点
    diag_idx = get_diag_point(inner_bound)
    
    # カーブの角の点を取得
    sharp_index = get_sharp_point(outer_bound)
    
    ### 道幅の取得 ###
    road_width = getRoadWidth(inner_bound, outer_bound, diag_idx, sharp_index)

狭路用経路の生成

　最後に狭路を発見したら、新しく走行経路を生成します。経路生成のアルゴリズムについては、DynamicWindowApproacy(DWA)やRRTなどを検討したのですが、今回の環境に適応することに苦戦したため、最初はシンプルな軌道の経路を生成させることを目指しました。(DWAやRRTについては再度チャレンジする予定です）
　今回は図３のように円軌道の経路を生成させるようにしました。

経路生成の流れは以下になります。

1. まず、狭路判定時に取得したカーブの曲がり角の点と自己位置の距離を算出します。
2. 1. で計算した距離が設定した閾値より小さければ、自己位置と曲がった先の道幅から扇形の中心位置を決定します。道幅は狭路判定時に取得した結果を使用します。同時に道幅に応じて、扇形の角度を決定します。こちらはあらかじめ道幅ごとに設定したパラメータを使用します。
3. 円の半径と扇形の角度に応じて、新たに経路を生成します。経路生成はPath データの points という変数に座標を表す点を加えることで可能です。元の経路から円軌道の始点を見つけ、その点から一定間隔ごとに生成した円軌道に沿って点を追加していきます。
4. 内側の曲がり角の点から進行方向に延長した線と追加した円軌道の交点を目標点とします。
5. 元の経路のうち目標点から設定した距離分進行した値点にある点を再接続先として、円軌道の終点と再接続先を繋ぐように新たに経路を生成します。

1~5のステップで新たに経路を生成し、再接続先まで走行できれば再度、狭路の判定を再会して走行することを繰り返します。

開発コード

　今回開発したコードの全体は以下のリポジトリに公開しております。不十分な点が多々あると思いますが、ご容赦ください。 https://github.com/bushio/crank_driving_planner

結果

　今回開発したプランナーで走行した結果がこちらになります。動画では、matplotlib を使用し、生成した経路情報などを可視化しています。元の経路を黄色線、新たに生成した経路をピンクの線で表しています。

まとめ

　今回は自動運転AIチャレンジ2023（インテグレーション大会）に向けて開発した自作のプランナーについて記載しました。今回開発したものは単なる円軌道で走行するだけで、経路生成と言えるような大したものではないですが、大会の参加を通じてAutowareやROS2の仕組み、経路計画のアルゴリズムなど多くのことを学ぶことができました。今後は経路計画アルゴリズムと組み合わせて、より高度な経路生成ができるよう改善していきます。2023年12月より自動運転AIチャレンジ２０２３（シミュレーション）が開催されるので、引き続きチャレンジしていきたいと思います。