初心者がAIチャレンジやってみた (4)：全障害物の回避成功！（全パラメータ公開）

概要

「初心者がAI Challengeやってみた」シリーズの第4弾です。
今回は全ての障害物を突破することができたので、その時のパラメータを共有します。

前の記事はこちら：

• 第1弾：初心者がAIチャレンジやってみた(1)：Autowareを動かしてみる
• 第2弾：初心者がAIチャレンジやってみた(2)：1つ目の障害物の回避成功
• 第3弾：初心者がAIチャレンジやってみた(3)：2つ目の障害物の回避成功

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

大会レポジトリの更新

AI Challengeの大会レポジトリが更新されたそうなので、git pullで更新しました。
git pullをすると今まで調整したパラメータファイルも更新されてしまうので、git stashなどをして後からパラメータファイルを元に戻せるようにしておきました。

他にもAWSIMのデータが更新されたそうなので、こちらは最新バージョンのものをダウンロード、解凍して使えるようにしました。（詳しくは公式セットアップドキュメントを参照。）

主な変化は以下の3点でしょうか：

• 新たな障害物が序盤に追加
• 2つ目と3つ目の障害物の間の距離が長くなり、回避が簡単になった
• カーブの形状の変化、レーンの幅に統一感が加わりコースがきれいになった

新たな障害物が追加。車両が箱に近づくと人が箱をレーン外に引いてくれる。

道路中央に箱が！	車両が箱に近づくと…

障害物の位置が変化。回避が簡単になった？

更新前	更新後

コースの形状の変化。より洗練された形になった印象。

更新前	更新後

レポジトリの更新に伴ってrockerの起動などの手順も少し変わったようなので、手順を確認しながら実行しました。

パラメータ調整

object_envelope_buffer

avoidance.param.yamlのobject_envelope_bufferを調整しました。

avoidance.param.yaml

object_envelope_buffer: 1.1   # 元々0.3

公式ドキュメントに説明が載っています。
実際の障害物の大きさにobject_envelope_buffer分の大きさを足して、擬似的に障害物を大きくします。その輪郭から一定距離の位置にshift pointを生成して回避経路を生成するイメージです。

本来の目的はノイズによる回避経路のチャタリング防止のようですが、今回このパラメータを大きい値に設定した理由は別にあります。シミュレーションを実行すると、障害物が本来よりも大きいサイズで表示されてしまうという現象があります。この影響で経路生成がうまく行っていないという仮説を立て、object_envelope_bufferを大きくすることでいい感じに回避経路を生成しようという理由です。本記事の最後にも書いてありますが、この仮説は間違っている可能性もあるので詳しいことが分かったら更新します。

backward/forward_path_length

behavior_path_planner.param.yamlの一部パラメータを以下のように変更しました。

behavior_path_planner.param.yaml

backward_path_length: 0.0    # 元々5.0
forward_path_length: 20.0    # 元々300.0

これらを調整すると経路生成の際にどれくらい先までの経路を生成するかを変えることができます。元々のパラメータでは最大で300m先を見据えた経路を生成していました。ただ、ここで生成される経路は障害物の存在を考慮しておらず、マップ情報を元に生成されるグローバルプランの様なものです。

障害物を避けたいときにこのグローバルプランと障害物回避用のローカルプランが「喧嘩」してうまく障害物をよけれてないのではないかと仮説をたてて、いっそのことグローバルプランを短くしてしまえばどうかと思って実行したら下の動画のようにうまく行きました。
（初めて最後の障害物まで回避できたのでテンションが上がってその後のカーブ走行のテストもしたので動画がいつもより長めです。）

まだ動作はぎこちない感じですが、このパラメータで数回うまく行きました。ただし毎回うまく行くわけではなく、途中の障害物にぶつかってしまったり障害物前で停車したりすることもあるのでまだ色々な調整が必要そうです。

この時に使っていたパラメータファイルを載せておきますので、よろしければ参考にしてみてください。

behavior_path_planner.param.yaml

/**:
  ros__parameters:
    verbose: false

    planning_hz: 10.0
    backward_path_length: 0.0
    forward_path_length: 20.0
    backward_length_buffer_for_end_of_pull_over: 5.0
    backward_length_buffer_for_end_of_pull_out: 5.0
    minimum_pull_over_length: 16.0

    refine_goal_search_radius_range: 7.5

    # parameters for turn signal decider
    turn_signal_intersection_search_distance: 30.0
    turn_signal_intersection_angle_threshold_deg: 15.0
    turn_signal_minimum_search_distance: 1.0
    turn_signal_search_time: 1.0
    turn_signal_shift_length_threshold: 0.1
    turn_signal_on_swerving: true

    path_interval: 0.1

    visualize_maximum_drivable_area: true

    lateral_distance_max_threshold: 1.5
    longitudinal_distance_min_threshold: 1.0

    expected_front_deceleration: -1.0
    expected_rear_deceleration: -1.0

    expected_front_deceleration_for_abort: -1.0
    expected_rear_deceleration_for_abort: -2.0

    rear_vehicle_reaction_time: 2.0
    rear_vehicle_safety_time_margin: 1.0

    # USE ONLY WHEN THE OPTION COMPILE_WITH_OLD_ARCHITECTURE IS SET TO FALSE.
    # https://github.com/autowarefoundation/autoware.universe/blob/main/planning/behavior_path_planner/CMakeLists.txt
    # NOTE: The smaller the priority number is, the higher the module priority is.
    lane_change:
      enable_module: true
      enable_simultaneous_execution: false
      priority: 4
      max_module_size: 1

    pull_out:
      enable_module: true
      enable_simultaneous_execution: false
      priority: 1
      max_module_size: 1

    side_shift:
      enable_module: true
      enable_simultaneous_execution: false
      priority: 3
      max_module_size: 1

    pull_over:
      enable_module: true
      enable_simultaneous_execution: false
      priority: 2
      max_module_size: 1

    avoidance:
      enable_module: true
      enable_simultaneous_execution: false
      priority: 0
      max_module_size: 1

avoidance.param.yaml

# see AvoidanceParameters description in avoidance_module_data.hpp for description.
/**:
    ros__parameters:
        avoidance:
            resample_interval_for_planning: 0.05 # [m]
            resample_interval_for_output: 0.1 # [m]
            detection_area_right_expand_dist: 0.0 # [m]
            detection_area_left_expand_dist: 0.0 # [m]
            drivable_area_right_bound_offset: 0.0 # [m]
            drivable_area_left_bound_offset: 0.0 # [m]
            object_envelope_buffer: 1.1 # [m]
    
    
    # avoidance module common setting
    enable_bound_clipping: false
    enable_avoidance_over_same_direction: true
    enable_avoidance_over_opposite_direction: true
    enable_update_path_when_object_is_gone: false
    enable_force_avoidance_for_stopped_vehicle: false
    enable_safety_check: false
    enable_yield_maneuver: false
    disable_path_update: false
    
    
    # for debug
    publish_debug_marker: true
    print_debug_info: false
    
    
    # avoidance is performed for the object type with true
    target_object:
        car: true
        truck: true
        bus: true
        trailer: true
        unknown: true
        bicycle: false
        motorcycle: false
        pedestrian: false
    
    
    # For target object filtering
    target_filtering:
        # filtering moving objects
        threshold_speed_object_is_stopped: 1.0 # [m/s]
        threshold_time_object_is_moving: 1.0 # [s]
        threshold_time_force_avoidance_for_stopped_vehicle: 10.0 # [s]
        # detection range
        object_check_force_avoidance_clearance: 1.0 # [m]
        object_check_forward_distance: 10.0 # [m]
        object_check_backward_distance: 0.0 # [m]
        object_check_goal_distance: 20.0 # [m]
        # filtering parking objects
        threshold_distance_object_is_on_center: 0.0 # [m]
        object_check_shiftable_ratio: 0.0 # [-]
        object_check_min_road_shoulder_width: 0.0 # [m]
        # lost object compensation
        object_last_seen_threshold: 2.0
    
    
    # For safety check
    safety_check:
        safety_check_backward_distance: 100.0 # [m]
        safety_check_time_horizon: 10.0 # [s]
        safety_check_idling_time: 1.5 # [s]
        safety_check_accel_for_rss: 2.5 # [m/ss]
        safety_check_hysteresis_factor: 2.0 # [-]
    
    
    # For avoidance maneuver
    avoidance:
        # avoidance lateral parameters
        lateral:
            lateral_collision_margin: 0.1 # [m]
            lateral_collision_safety_buffer: 0.1 # [m]
            lateral_passable_safety_buffer: 0.0 # [m]
            road_shoulder_safety_margin: 0.0 # [m]
            avoidance_execution_lateral_threshold: 0.0
            max_right_shift_length: 5.0
            max_left_shift_length: 5.0
        # avoidance distance parameters
        longitudinal:
            prepare_time: 0.0 # [s]
            longitudinal_collision_safety_buffer: 0.0 # [m]
            min_prepare_distance: 0.0 # [m]
            min_avoidance_distance: 0.0 # [m]
            min_nominal_avoidance_speed: 1.0 # [m/s]
            min_sharp_avoidance_speed: 0.8 # [m/s]
    
    
    # For yield maneuver
    yield:
        yield_velocity: 2.78 # [m/s]
    
    
    # For stop maneuver
    stop:
        min_distance: 10.0 # [m]
        max_distance: 20.0 # [m]
    
    
    constraints:
        # vehicle slows down under longitudinal constraints
        use_constraints_for_decel: false # [-]
        
        
        # lateral constraints
        lateral:
            nominal_lateral_jerk: 10.0 # [m/s3]
            max_lateral_jerk: 15.0 # [m/s3]
        
        
        # longitudinal constraints
        longitudinal:
            nominal_deceleration: -1.0 # [m/ss]
            nominal_jerk: 0.5 # [m/sss]
            max_deceleration: -2.0 # [m/ss]
            max_jerk: 1.0
            # For prevention of large acceleration while avoidance
            min_avoidance_speed_for_acc_prevention: 3.0 # [m/s]
            max_avoidance_acceleration: 0.5 # [m/ss]
    
    
    target_velocity_matrix:
        col_size: 2
        matrix:
            [2.78, 13.9, # velocity [m/s]
            0.50, 1.00] # margin [m]

Motion Planner

今まで記述するのを忘れていましたが、motion plannerの以下のパラメータファイルも調整していました。
ここのsteer_input_weightなどを調整しないと車両がハンドルを切ってくれないかもしれません。

obsacle_avoidance_planner.param.yaml

/**:
  ros__parameters:
    option:
      enable_smoothing: true                                       # enable path smoothing by elastic band
      enable_skip_optimization: false                              # skip elastic band and model predictive trajectory
      enable_reset_prev_optimization: false                        # If true, optimization has no fix constraint to the previous result.
      enable_outside_drivable_area_stop: true                      # stop if the ego's trajectory footprint is outside the drivable area
      use_footprint_polygon_for_outside_drivable_area_check: false # If false, only the footprint's corner points are considered.

      debug:
        # flag to publish
        enable_pub_debug_marker: true           # publish debug marker

        # flag to show
        enable_debug_info: false
        enable_calculation_time_info: false

    common:
      # output
      output_delta_arc_length: 0.5     #  delta arc length for output trajectory [m]
      output_backward_traj_length: 5.0  # backward length for backward trajectory from base_link [m]

    # replanning & trimming trajectory param outside algorithm
    replan:
      max_path_shape_around_ego_lat_dist: 2.0  # threshold of path shape change around ego [m]
      max_ego_moving_dist: 5.0                 # threshold of ego's moving distance for replan [m]
      # make max_goal_moving_dist long to keep start point fixed for pull over
      max_goal_moving_dist: 15.0               # threshold of goal's moving distance for replan [m]
      max_delta_time_sec: 1.0                  # threshold of delta time for replan [second]

    # eb param
    eb:
      option:
        enable_warm_start: true
        enable_optimization_validation: false

      common:
        num_points: 100        # number of points for optimization [-]
        delta_arc_length: 1.0  # delta arc length for optimization [m]

      clearance:
        num_joint_points: 3  # number of joint points (joint means connecting fixing and smoothing)

        clearance_for_fix: 0.0     # maximum optimizing range when applying fixing
        clearance_for_joint: 0.1   # maximum optimizing range when applying jointing
        clearance_for_smooth: 0.1  # maximum optimizing range when applying smoothing

      weight:
        smooth_weight: 1.0
        lat_error_weight: 0.001

      qp:
        max_iteration: 10000  # max iteration when solving QP
        eps_abs: 1.0e-7       # eps abs when solving OSQP
        eps_rel: 1.0e-7       # eps rel when solving OSQP

      validation: # if enable_optimization_validation is true
        max_error: 3.0 # [m]

    # mpt param
    mpt:
      option:
        # TODO(murooka) enable the following. Currently enabling the flag makes QP so heavy
        steer_limit_constraint: false
        visualize_sampling_num: 1
        enable_manual_warm_start: false
        enable_warm_start: true
        enable_optimization_validation: false

      common:
        num_points: 100        # number of points for optimization [-]
        delta_arc_length: 1.0  # delta arc length for optimization [m]

      # kinematics:
        # If this parameter is commented out, the parameter is set as below by default.
        # The logic could be `optimization_center_offset = vehicle_info.wheelbase * 0.8`
        # The 0.8 scale is adopted as it performed the best.
        # optimization_center_offset: 2.3 # optimization center offset from base link

      clearance:  # clearance(distance) between vehicle and roads/objects when generating trajectory
        # if collision_free_constraints.option.hard_constraint is true
        hard_clearance_from_road: 0.0 # clearance from road boundary[m]
        # if collision_free_constraints.option.soft_constraint is true
        soft_clearance_from_road: 0.1 # clearance from road boundary[m]

      # weight parameter for optimization
      weight:
        # collision free
        soft_collision_free_weight: 1000.0        # soft weight for lateral error around the middle point

        # tracking error
        lat_error_weight: 0.0       # weight for lateral error
        yaw_error_weight: 0.0       # weight for yaw error
        yaw_error_rate_weight: 0.0  # weight for yaw error rate
        steer_input_weight: 0.0     # weight for steering input
        steer_rate_weight: 0.0      # weight for steering rate

        terminal_lat_error_weight: 100.0  # weight for lateral error at terminal point
        terminal_yaw_error_weight: 100.0  # weight for yaw error at terminal point
        goal_lat_error_weight: 1000.0     # weight for lateral error at path end point
        goal_yaw_error_weight: 1000.0     # weight for yaw error at path end point

      # avoidance
      avoidance:
        max_avoidance_cost: 0.5            # [m]
        avoidance_cost_margin: 0.0         # [m]
        avoidance_cost_band_length: 5.0    # [m]
        avoidance_cost_decrease_rate: 0.05 # decreased cost per point interval

        weight:
          lat_error_weight: 0.0     # weight for lateral error
          yaw_error_weight: 0.0    # weight for yaw error
          steer_input_weight: 0.0 # weight for yaw error

      # collision free constraint for optimization
      collision_free_constraints:
        option:
          l_inf_norm: true
          soft_constraint: true
          hard_constraint: false

        # how to represent footprint as circles
        vehicle_circles:
          method: "fitting_uniform_circle"

          bicycle_model:
            num_for_calculation: 3
            front_radius_ratio: 1.0
            rear_radius_ratio: 1.0

          uniform_circle:
            num: 3
            radius_ratio: 1.0

          fitting_uniform_circle:
            num: 3

      validation: # if enable_optimization_validation is true
        max_lat_error: 5.0 # [m]
        max_yaw_error: 1.046 # [rad]

今後の予定

なんとかパラメータ調整だけで全障害物回避を達成することができました！
まだ動作がぎこちない感じで成功よりも失敗のほうが多いので、もっとパラメータを改善できるかを探求しようかなと考えています。

ただ、いくつかソースコードをいじればもっと良くなるんじゃないか、パラメータ調整だけでは限界があるのではないかと思う点があるので共有しようと思います。

1. 障害物が大きく表示される
   ダンボール箱の障害物が、奥行き方向に大きくなっているように表示されています。
   あくまでRViz上の表示のされ方の話なので、これが実際にoccupancy gridなどに影響をしているかは分かりませんが、シミュレーションを動かしているとこの大きい奥行きのせいで経路生成が失敗しているのではないかと感じることが多くありました。
   RVizのマーカー表示やoccupancy gridあたりのコードを読んでみます。

2. 障害物を見失う？
   車両が障害物に近づくとそれを見失う（LiDARセンサによって検知されない）ことが多々あります。特に新たに追加された障害物に関しては、動画上ではうまく回避しているように見えますが、実際には
   「箱に近づく→箱を見失う→障害物なしと判断しそのまま直進→いいタイミングで人が箱をどけてくれる→ぶつからない」
   というような流れで運良く成功しているように思います。例えば箱をどかすタイミングがもう少し遅ければそのままぶつかってしまうのではないかと思います。
   まだPerception関連のコードやパラメータはしっかりと確認できていないので、まずはそこら辺を探ってみようと思います。

3. 回避する障害物と回避しない(?)障害物がある
   上の動画を見てわかるように、障害物によってRVizに表示されるものとされないものがあります。障害物が表示されていない場合（＝認識していない？）、回避経路などを生成せずにそのまま進行するような動作をします。上記したように、新たに追加された障害物に遭遇した時にこの様な動作をしている気がします。
   どの様な条件で表示さるのかが分からないのでこれについても調べてみます。