DWPP: Dynamic Windowを用いた速度・加速度制約を考慮したPure Pursuitの提案とNav2プラグインの実装公開【ROSConJP 2025】

DWPP: Dynamic Windowを用いた 速度・加速度制約を考慮したPure Pursuitの提案と Nav2プラグインの実装公開 2025年9月9日 慶應義塾大学大学院 理工学研究科 髙橋正樹研究室 〇大西 史弥、髙橋 正樹

目次 2/18 ◼ Dynamic Window Pure Pursuit (DWPP) の紹介 – Pure Pursuitとは • 概要 • 使用例 • 課題 – Dynamic Window Pure Pursuit (DWPP) • 概要 • メリット • 得意なシナリオ ◼ 導入方法 – Nav2への導入 – テスト環境の導入 ◼ デモンストレーション ◼ まとめ Dynamic Window Pure Pursuitの提案とNav2プラグインの実装公開 2025/9/16

目次 3/18 ◼ Dynamic Window Pure Pursuit (DWPP) の紹介 – Pure Pursuitとは • 概要 • 使用例 • 課題 – Dynamic Window Pure Pursuit (DWPP) • 概要 • メリット • 得意なシナリオ ◼ 導入方法 – Nav2への導入 – テスト環境の導入 ◼ デモンストレーション ◼ まとめ Dynamic Window Pure Pursuitの提案とNav2プラグインの実装公開 2025/9/16

Pure Pursuitの概要 4/18 ◼ 自律移動ロボットのナビゲーションの仕組み goal – グローバルプランナー • スタートとゴールを結ぶグローバルな経路のプランナー – ローカルプランナー • 経路追従や障害物回避するローカルな経路のプランナー • ローカルな経路へ追従するための速度指令値も算出 start (v, ω) ◼ Pure Pursuitとは – 経路追従のみ行うローカルプランナー 名前 ベースのアルゴリズム 機能 DWB Controller Dynamic Window Approach 経路追従+障害物回避 TEB Controller Model Predictive Control 経路追従+障害物回避 Model Predictive Path Integral 経路追従+障害物回避 経路追従のみ Regulated Pure Pursuit Pure Pursuit MPPI Controller Nav2で使用可能な主なローカルプランナー一覧 Dynamic Window Pure Pursuitの提案とNav2プラグインの実装公開 2025/9/16

Pure Pursuitの使用例 ◼ 設定した経路上を走行 5/18 ◼ 障害物回避するように リアルタイムに更新される経路上を走行 障害物 AGV（無人搬送車） 経路 自動運転サービス[1] 搬送ロボット（歩行者回避機能付き） [1] 「WHILL自動運転サービス」空港での利用方法, https://youtu.be/5tXsCfeWbso?si=H6xT8fXvoyT5kSaW Dynamic Window Pure Pursuitの提案とNav2プラグインの実装公開 2025/9/16

Pure Pursuitの課題 6/18 Adaptive Regulated 速度指令値 Pure Pursuit (PP) Pure Pursuit (APP) Pure Pursuit (RPP) [Coulter, 1992] [Campbell, 2007] [Macenski et. al., 2023] vmax ローカルな経路の計画 v 前方注視点 pL の算出 前方注視距離 L = 定数 Lt=gLvt （並進速度に比例, APP） t 前方注視点へ至る（円弧）経路曲率 φ の算出 速度指令値算出 制限並進速度 vreg 算出 (RPP) 並進速度 vref の算出（最高速度で一定，ゴール付近で減速） vref を vreg で制限 (RPP) ωmax ω 回転速度 ωref の算出 ωref = φvref（曲率φの経路を追従する式） 速度指令値 (vref, ωref) 実現速度 速度指令値算出時に速度・加速度制約を未考慮 t PPの速度プロファイル例 Dynamic Window Pure Pursuitの提案とNav2プラグインの実装公開 2025/9/16

Pure Pursuitの課題 7/18 Adaptive Regulated 速度指令値 Pure Pursuit (PP) Pure Pursuit (APP) Pure Pursuit (RPP) [Coulter, 1992] [Campbell, 2007] [Macenski et. al., 2023] vmax ローカルな経路の計画 v 前方注視点 pL の算出 前方注視距離 L = 定数 Lt=gLvt （並進速度に比例, APP） t 前方注視点へ至る（円弧）経路曲率 φ の算出 速度指令値算出 ωmax PPによる経路追従 制限並進速度 vreg 算出 (RPP) 並進速度 vref の算出（最高速度で一定，ゴール付近で減速） vref を vreg で制限 (RPP) ω 回転速度 ωref の算出 ωref = φvref（曲率φの経路を追従する式） 速度指令値 (vref, ωref) 実現速度 速度指令値算出時に速度・加速度制約を未考慮 t PPの速度プロファイル例 Dynamic Window Pure Pursuitの提案とNav2プラグインの実装公開 2025/9/16

Dynamic Window Pure Pursuit (DWPP)の概要 Adaptive Regulated Pure Pursuit (PP) Pure Pursuit (APP) Pure Pursuit (RPP) [Coulter, 1992] [Campbell, 2007] [Macenski et. al., 2023] 8/18 Dynamic Window Pure Pursuit (DWPP) [Ohnishi et. al., 2025] ローカルな経路の計画 前方注視点 pL の算出 前方注視距離 L = 定数 Lt=gLvt （並進速度に比例, APP） 前方注視点へ至る（円弧）経路曲率 φ の算出 速度指令値算出 制限並進速度 vreg 算出 (RPP) 並進速度 vref の算出（最高速度で一定，ゴール付近で減速） vref を vreg で制限 (RPP) 回転速度 ωref の算出 ωref = φvref（曲率φの経路を追従する式） 速度指令値 (vref, ωref) Dynamic Windowの 算出 (DWPP) Dynamic Windowを 用いた速度算出 (DWPP) DWPP: 速度・加速度制約の下で最適に経路追従する速度を算出可能 Dynamic Window Pure Pursuitの提案とNav2プラグインの実装公開 2025/9/16

Dynamic Window Pure Pursuit (DWPP)の概要 速度指令値 実現速度 vmax vmax v v t ωmax ωmax ω ω ωmin t PPの速度プロファイル例 9/18 t t DWPPによる経路追従 DWPPの速度プロファイル例 DWPP: 速度・加速度制約の下で最適に経路追従する速度を算出可能 Dynamic Window Pure Pursuitの提案とNav2プラグインの実装公開 2025/9/16

DWPPのメリット 10/18 評価に使用した経路 ◼ 正確な経路追従 – 追従誤差の低減 ◼ 乗り心地の向上 – 設定した速度・加速度制約下での運転 – オーバーシュートの低減 45 θpath [deg] 90 3.0 m θpath θpath 135 θpath 3.0 m y 1.0 m PP APP RPP DWPP x 経路追従誤差 (RMSE of cross-track error [m]) θpath PP 45 0.035 90 Failed 135 0.38 APP 0.032 0.15 0.35 Dynamic Window Pure Pursuitの提案とNav2プラグインの実装公開 RPP DWPP 0.024 0.023 0.040 0.029 0.10 0.039 2025/9/16 誤差 低減

DWPPの得意なシナリオ 11/18 評価に使用した経路 ◼ 曲率が大きな経路 45 θpath [deg] 90 3.0 m θpath θpath 135 θpath y 3.0 m PP APP RPP DWPP x 1.0 m 経路追従誤差 (RMSE of cross-track error [m]) θpath PP 45 0.035 90 Failed 135 0.38 APP 0.032 0.15 0.35 Dynamic Window Pure Pursuitの提案とNav2プラグインの実装公開 RPP DWPP 0.024 0.023 0.040 0.029 0.10 0.039 2025/9/16 誤差 低減

目次 12/18 ◼ Dynamic Window Pure Pursuit (DWPP) の紹介 – Pure Pursuitとは • 概要 • 使用例 • 課題 – Dynamic Window Pure Pursuit (DWPP) • 概要 • メリット • 得意なシナリオ ◼ 導入方法 – Nav2への導入 – テスト環境の導入 ◼ デモンストレーション ◼ まとめ Dynamic Window Pure Pursuitの提案とNav2プラグインの実装公開 2025/9/16

Nav2への導入 13/18 ◼ DWPP Nav2プラグイン GitHubリポジトリ (ROS2 Humble) – https://github.com/Decwest/nav2_dynamic_window_pure_pursuit_controller ◼ 導入手順 1. ROS2環境のsrcディレクトリにコピー git clone https://github.com/Decwest/nav2_dynamic_window_pure_pursuit_controller.git 2. パッケージをビルド colcon build --symlink-install 3. Nav2のパラメータファイルのプラグインを変更 controller_server: ros__parameters: controller_plugins: ["FollowPath"] FollowPath: plugin: "nav2_dynamic_window_pure_pursuit_controller::DynamicWindowPurePursuitController コピってビルド→プラグイン変更！ Dynamic Window Pure Pursuitの提案とNav2プラグインの実装公開 2025/9/16

• https://github.com/Decwest/nav2_dynamic_window_pure_pursuit_controller

テスト環境の導入 14/18 ◼ DWPP テスト環境 GitHubリポジトリ – https://github.com/Decwest/dwpp_test_environment ◼ テスト環境 デモを実施します！ – 手法間比較シミュレーション – Nav2シミュレーション with DWPP ◼ 導入に必要なもの – Docker, Docker compose • 仮想環境の作成 手法間比較シミュレーション – Task • コマンドの管理 – GPUを使う場合：nvidia-container-toolkit ◼ 導入方法 – README参照 Nav2シミュレーション with DWPP Dynamic Window Pure Pursuitの提案とNav2プラグインの実装公開 2025/9/16

• https://github.com/Decwest/dwpp_test_environment

目次 15/18 ◼ Dynamic Window Pure Pursuit (DWPP) の紹介 – Pure Pursuitとは • 概要 • 使用例 • 課題 – Dynamic Window Pure Pursuit (DWPP) • 概要 • メリット • 得意なシナリオ ◼ 導入方法 – Nav2への導入 – テスト環境の導入 ◼ デモンストレーション ◼ まとめ Dynamic Window Pure Pursuitの提案とNav2プラグインの実装公開 2025/9/16

デモンストレーション：手法間比較シミュレーション Dynamic Window Pure Pursuitの提案とNav2プラグインの実装公開 16/18 2025/9/16

デモンストレーション：Nav2シミュレーション with DWPP Dynamic Window Pure Pursuitの提案とNav2プラグインの実装公開 17/18 2025/9/16

まとめ 18/18 ◼ Dynamic Window Pure Pursuit (DWPP)とは – 速度・加速度制約の下で最適に経路追従する速度を算出可能なローカルプランナー ◼ 使用場面：経路追従のみ行う場合 – 設定した経路上を走行 – 障害物回避するようにリアルタイムに更新される経路上を走行 ◼ 導入のメリット – 正確な経路追従 – 乗り心地の向上 ◼ 得意なシナリオ – 曲率が大きな経路 ◼ 導入方法（簡単！） – コピってビルド→プラグイン変更！ – テスト環境も用意 DWPP Nav2プラグイン DWPP テスト環境 ぜひお使いください！ Dynamic Window Pure Pursuitの提案とNav2プラグインの実装公開 2025/9/16