Package Summary
Tags | No category tags. |
Version | 0.1.0 |
License | Apache License 2.0 |
Build type | AMENT_CMAKE |
Use | RECOMMENDED |
Repository Summary
Description | |
Checkout URI | https://github.com/ieiauto/autodrrt.git |
VCS Type | git |
VCS Version | main |
Last Updated | 2025-05-30 |
Dev Status | UNKNOWN |
Released | UNRELEASED |
Tags | No category tags. |
Contributing |
Help Wanted (-)
Good First Issues (-) Pull Requests to Review (-) |
Package Description
Additional Links
Maintainers
- Fumiya Watanabe
- Takamasa Horibe
- Makoto Kurihara
Authors
- Takamasa Horibe
- Fumiya Watanabe
- Yutaka Shimizu
- Makoto Kurihara
Motion Velocity Smoother
Purpose
motion_velocity_smoother
は目標軌道上の各点における望ましい車速を計画して出力するモジュールである。
このモジュールは、速度の最大化と乗り心地の良さを両立するために、事前に指定された制限速度、制限加速度および制限躍度の範囲で車速を計画する。
加速度や躍度の制限を与えることは車速の変化を滑らかにすることに対応するため、このモジュールをmotion_velocity_smoother
と呼んでいる。
Inner-workings / Algorithms
Flow chart
Extract trajectory
自車後輪軸中心位置に最も近い参照経路上の点に対し、extract_behind_dist
だけ戻った点からextract_ahead_dist
だけ進んだ点までの参照経路を抜き出す。
Apply external velocity limit
モジュール外部から指定された速度制限を適用する。
ここで扱う外部の速度制限は/planning/scenario_planning/max_velocity
の topic で渡されるもので、地図上で設定された速度制限など、参照経路にすでに設定されている制限速度とは別である。
外部から指定される速度制限は、パラメータで指定されている減速度および躍度の制限の範囲で減速可能な位置から速度制限を適用する。
Apply stop approaching velocity
停止点に近づいたときの速度を設定する。障害物近傍まで近づく場合や、正着精度向上などの目的に用いる。
Apply lateral acceleration limit
経路の曲率に応じて、指定された最大横加速度max_lateral_accel
を超えない速度を制限速度として設定する。ただし、制限速度はmin_curve_velocity
を下回らないように設定する。
Resample trajectory
指定された時間間隔で経路の点を再サンプルする。ただし、経路全体の長さはmin_trajectory_length
からmax_trajectory_length
の間となるように再サンプルを行い、点の間隔はmin_trajectory_interval_distance
より小さくならないようにする。
現在車速でresample_time
の間進む距離までは密にサンプリングし、それ以降は疎にサンプリングする。
この方法でサンプリングすることで、低速時は密に、高速時は疎にサンプルされるため、停止精度と計算負荷軽減の両立を図っている。
Calculate initial state
速度計画のための初期値を計算する。初期値は状況に応じて下表のように計算する。
状況 | 初期速度 | 初期加速度 |
---|---|---|
最初の計算時 | 現在車速 | 0.0 |
発進時 | engage_velocity |
engage_acceleration |
現在車速と計画車速が乖離 | 現在車速 | 前回計画値 |
通常時 | 前回計画値 | 前回計画値 |
Smooth velocity
速度の計画を行う。速度計画のアルゴリズムはJerkFiltered
, L2
, Linf
の 3 種類のうちからコンフィグで指定する。
最適化のソルバは OSQP[1]を利用する。
JerkFiltered
速度の 2 乗(最小化で表すため負値で表現)、制限速度逸脱量の 2 乗、制限加度逸脱量の 2 乗、制限ジャーク逸脱量の 2 乗、ジャークの 2 乗の総和を最小化する。
L2
速度の 2 乗(最小化で表すため負値で表現)、制限速度逸脱量の 2 乗、制限加度逸脱量の 2 乗、疑似ジャーク[2]の 2 乗の総和を最小化する。
Linf
速度の 2 乗(最小化で表すため負値で表現)、制限速度逸脱量の 2 乗、制限加度逸脱量の 2 乗の総和と疑似ジャーク[2]の絶対最大値の和の最小化する。
Post process
計画された軌道の後処理を行う。
- 停止点より先の速度を 0 に設定
- 速度がパラメータで与えられる
max_velocity
以下となるように設定 - 自車位置より手前の点における速度を設定
- Trajectory の仕様に合わせてリサンプリング(
post resampling
) - デバッグデータの出力
最適化の計算が終わったあと、次のノードに経路を渡す前にpost resampling
と呼ばれるリサンプリングを行う。ここで再度リサンプリングを行っている理由としては、最適化前で必要な経路間隔と後段のモジュールに渡す経路間隔が必ずしも一致していないからであり、その差を埋めるために再度サンプリングを行っている。そのため、post resampling
では後段モジュールの経路仕様を確認してパラメータを決める必要がある。なお、最適化アルゴリズムの計算負荷が高く、最初のリサンプリングで経路間隔が後段モジュールの経路仕様より疎になっている場合、post resampling
で経路を蜜にリサンプリングする。逆に最適化アルゴリズムの計算負荷が小さく、最初のリサンプリングで経路間隔が後段の経路仕様より蜜になっている場合は、post resampling
で経路をその仕様に合わせて疎にリサンプリングする。
Inputs / Outputs
Input
Name | Type | Description |
---|---|---|
~/input/trajectory |
autoware_auto_planning_msgs/Trajectory |
Reference trajectory |
/planning/scenario_planning/max_velocity |
std_msgs/Float32 |
External velocity limit [m/s] |
/localization/kinematic_state |
nav_msgs/Odometry |
Current odometry |
/tf |
tf2_msgs/TFMessage |
TF |
/tf_static |
tf2_msgs/TFMessage |
TF static |
Output
Name | Type | Description |
---|---|---|
~/output/trajectory |
autoware_auto_planning_msgs/Trajectory |
Modified trajectory |
/planning/scenario_planning/current_max_velocity |
std_msgs/Float32 |
Current external velocity limit [m/s] |
~/closest_velocity |
std_msgs/Float32 |
Planned velocity closest to ego base_link (for debug) |
~/closest_acceleration |
std_msgs/Float32 |
Planned acceleration closest to ego base_link (for debug) |
~/closest_jerk |
std_msgs/Float32 |
Planned jerk closest to ego base_link (for debug) |
~/debug/trajectory_raw |
autoware_auto_planning_msgs/Trajectory |
Extracted trajectory (for debug) |
File truncated at 100 lines see the full file
Package Dependencies
System Dependencies
Launch files
- launch/motion_velocity_smoother.launch.xml
-
- common_param_path [default: $(find-pkg-share motion_velocity_smoother)/config/default_common.param.yaml]
- nearest_search_param_path
- input_trajectory [default: /planning/scenario_planning/scenario_selector/trajectory]
- output_trajectory [default: /planning/scenario_planning/trajectory]
- publish_debug_trajs [default: false]
- velocity_smoother_type [default: JerkFiltered]
- param_path [default: $(find-pkg-share motion_velocity_smoother)/config/default_motion_velocity_smoother.param.yaml]
- velocity_smoother_param_path [default: $(find-pkg-share motion_velocity_smoother)/config/$(var velocity_smoother_type).param.yaml]
Messages
Services
Plugins
Recent questions tagged motion_velocity_smoother at Robotics Stack Exchange
Package Summary
Tags | No category tags. |
Version | 0.1.0 |
License | Apache License 2.0 |
Build type | AMENT_CMAKE |
Use | RECOMMENDED |
Repository Summary
Description | |
Checkout URI | https://github.com/ieiauto/autodrrt.git |
VCS Type | git |
VCS Version | main |
Last Updated | 2025-05-30 |
Dev Status | UNKNOWN |
Released | UNRELEASED |
Tags | No category tags. |
Contributing |
Help Wanted (-)
Good First Issues (-) Pull Requests to Review (-) |
Package Description
Additional Links
Maintainers
- Fumiya Watanabe
- Takamasa Horibe
- Makoto Kurihara
Authors
- Takamasa Horibe
- Fumiya Watanabe
- Yutaka Shimizu
- Makoto Kurihara
Motion Velocity Smoother
Purpose
motion_velocity_smoother
は目標軌道上の各点における望ましい車速を計画して出力するモジュールである。
このモジュールは、速度の最大化と乗り心地の良さを両立するために、事前に指定された制限速度、制限加速度および制限躍度の範囲で車速を計画する。
加速度や躍度の制限を与えることは車速の変化を滑らかにすることに対応するため、このモジュールをmotion_velocity_smoother
と呼んでいる。
Inner-workings / Algorithms
Flow chart
Extract trajectory
自車後輪軸中心位置に最も近い参照経路上の点に対し、extract_behind_dist
だけ戻った点からextract_ahead_dist
だけ進んだ点までの参照経路を抜き出す。
Apply external velocity limit
モジュール外部から指定された速度制限を適用する。
ここで扱う外部の速度制限は/planning/scenario_planning/max_velocity
の topic で渡されるもので、地図上で設定された速度制限など、参照経路にすでに設定されている制限速度とは別である。
外部から指定される速度制限は、パラメータで指定されている減速度および躍度の制限の範囲で減速可能な位置から速度制限を適用する。
Apply stop approaching velocity
停止点に近づいたときの速度を設定する。障害物近傍まで近づく場合や、正着精度向上などの目的に用いる。
Apply lateral acceleration limit
経路の曲率に応じて、指定された最大横加速度max_lateral_accel
を超えない速度を制限速度として設定する。ただし、制限速度はmin_curve_velocity
を下回らないように設定する。
Resample trajectory
指定された時間間隔で経路の点を再サンプルする。ただし、経路全体の長さはmin_trajectory_length
からmax_trajectory_length
の間となるように再サンプルを行い、点の間隔はmin_trajectory_interval_distance
より小さくならないようにする。
現在車速でresample_time
の間進む距離までは密にサンプリングし、それ以降は疎にサンプリングする。
この方法でサンプリングすることで、低速時は密に、高速時は疎にサンプルされるため、停止精度と計算負荷軽減の両立を図っている。
Calculate initial state
速度計画のための初期値を計算する。初期値は状況に応じて下表のように計算する。
状況 | 初期速度 | 初期加速度 |
---|---|---|
最初の計算時 | 現在車速 | 0.0 |
発進時 | engage_velocity |
engage_acceleration |
現在車速と計画車速が乖離 | 現在車速 | 前回計画値 |
通常時 | 前回計画値 | 前回計画値 |
Smooth velocity
速度の計画を行う。速度計画のアルゴリズムはJerkFiltered
, L2
, Linf
の 3 種類のうちからコンフィグで指定する。
最適化のソルバは OSQP[1]を利用する。
JerkFiltered
速度の 2 乗(最小化で表すため負値で表現)、制限速度逸脱量の 2 乗、制限加度逸脱量の 2 乗、制限ジャーク逸脱量の 2 乗、ジャークの 2 乗の総和を最小化する。
L2
速度の 2 乗(最小化で表すため負値で表現)、制限速度逸脱量の 2 乗、制限加度逸脱量の 2 乗、疑似ジャーク[2]の 2 乗の総和を最小化する。
Linf
速度の 2 乗(最小化で表すため負値で表現)、制限速度逸脱量の 2 乗、制限加度逸脱量の 2 乗の総和と疑似ジャーク[2]の絶対最大値の和の最小化する。
Post process
計画された軌道の後処理を行う。
- 停止点より先の速度を 0 に設定
- 速度がパラメータで与えられる
max_velocity
以下となるように設定 - 自車位置より手前の点における速度を設定
- Trajectory の仕様に合わせてリサンプリング(
post resampling
) - デバッグデータの出力
最適化の計算が終わったあと、次のノードに経路を渡す前にpost resampling
と呼ばれるリサンプリングを行う。ここで再度リサンプリングを行っている理由としては、最適化前で必要な経路間隔と後段のモジュールに渡す経路間隔が必ずしも一致していないからであり、その差を埋めるために再度サンプリングを行っている。そのため、post resampling
では後段モジュールの経路仕様を確認してパラメータを決める必要がある。なお、最適化アルゴリズムの計算負荷が高く、最初のリサンプリングで経路間隔が後段モジュールの経路仕様より疎になっている場合、post resampling
で経路を蜜にリサンプリングする。逆に最適化アルゴリズムの計算負荷が小さく、最初のリサンプリングで経路間隔が後段の経路仕様より蜜になっている場合は、post resampling
で経路をその仕様に合わせて疎にリサンプリングする。
Inputs / Outputs
Input
Name | Type | Description |
---|---|---|
~/input/trajectory |
autoware_auto_planning_msgs/Trajectory |
Reference trajectory |
/planning/scenario_planning/max_velocity |
std_msgs/Float32 |
External velocity limit [m/s] |
/localization/kinematic_state |
nav_msgs/Odometry |
Current odometry |
/tf |
tf2_msgs/TFMessage |
TF |
/tf_static |
tf2_msgs/TFMessage |
TF static |
Output
Name | Type | Description |
---|---|---|
~/output/trajectory |
autoware_auto_planning_msgs/Trajectory |
Modified trajectory |
/planning/scenario_planning/current_max_velocity |
std_msgs/Float32 |
Current external velocity limit [m/s] |
~/closest_velocity |
std_msgs/Float32 |
Planned velocity closest to ego base_link (for debug) |
~/closest_acceleration |
std_msgs/Float32 |
Planned acceleration closest to ego base_link (for debug) |
~/closest_jerk |
std_msgs/Float32 |
Planned jerk closest to ego base_link (for debug) |
~/debug/trajectory_raw |
autoware_auto_planning_msgs/Trajectory |
Extracted trajectory (for debug) |
File truncated at 100 lines see the full file
Package Dependencies
System Dependencies
Launch files
- launch/motion_velocity_smoother.launch.xml
-
- common_param_path [default: $(find-pkg-share motion_velocity_smoother)/config/default_common.param.yaml]
- nearest_search_param_path
- input_trajectory [default: /planning/scenario_planning/scenario_selector/trajectory]
- output_trajectory [default: /planning/scenario_planning/trajectory]
- publish_debug_trajs [default: false]
- velocity_smoother_type [default: JerkFiltered]
- param_path [default: $(find-pkg-share motion_velocity_smoother)/config/default_motion_velocity_smoother.param.yaml]
- velocity_smoother_param_path [default: $(find-pkg-share motion_velocity_smoother)/config/$(var velocity_smoother_type).param.yaml]
Messages
Services
Plugins
Recent questions tagged motion_velocity_smoother at Robotics Stack Exchange
Package Summary
Tags | No category tags. |
Version | 0.1.0 |
License | Apache License 2.0 |
Build type | AMENT_CMAKE |
Use | RECOMMENDED |
Repository Summary
Description | |
Checkout URI | https://github.com/ieiauto/autodrrt.git |
VCS Type | git |
VCS Version | main |
Last Updated | 2025-05-30 |
Dev Status | UNKNOWN |
Released | UNRELEASED |
Tags | No category tags. |
Contributing |
Help Wanted (-)
Good First Issues (-) Pull Requests to Review (-) |
Package Description
Additional Links
Maintainers
- Fumiya Watanabe
- Takamasa Horibe
- Makoto Kurihara
Authors
- Takamasa Horibe
- Fumiya Watanabe
- Yutaka Shimizu
- Makoto Kurihara
Motion Velocity Smoother
Purpose
motion_velocity_smoother
は目標軌道上の各点における望ましい車速を計画して出力するモジュールである。
このモジュールは、速度の最大化と乗り心地の良さを両立するために、事前に指定された制限速度、制限加速度および制限躍度の範囲で車速を計画する。
加速度や躍度の制限を与えることは車速の変化を滑らかにすることに対応するため、このモジュールをmotion_velocity_smoother
と呼んでいる。
Inner-workings / Algorithms
Flow chart
Extract trajectory
自車後輪軸中心位置に最も近い参照経路上の点に対し、extract_behind_dist
だけ戻った点からextract_ahead_dist
だけ進んだ点までの参照経路を抜き出す。
Apply external velocity limit
モジュール外部から指定された速度制限を適用する。
ここで扱う外部の速度制限は/planning/scenario_planning/max_velocity
の topic で渡されるもので、地図上で設定された速度制限など、参照経路にすでに設定されている制限速度とは別である。
外部から指定される速度制限は、パラメータで指定されている減速度および躍度の制限の範囲で減速可能な位置から速度制限を適用する。
Apply stop approaching velocity
停止点に近づいたときの速度を設定する。障害物近傍まで近づく場合や、正着精度向上などの目的に用いる。
Apply lateral acceleration limit
経路の曲率に応じて、指定された最大横加速度max_lateral_accel
を超えない速度を制限速度として設定する。ただし、制限速度はmin_curve_velocity
を下回らないように設定する。
Resample trajectory
指定された時間間隔で経路の点を再サンプルする。ただし、経路全体の長さはmin_trajectory_length
からmax_trajectory_length
の間となるように再サンプルを行い、点の間隔はmin_trajectory_interval_distance
より小さくならないようにする。
現在車速でresample_time
の間進む距離までは密にサンプリングし、それ以降は疎にサンプリングする。
この方法でサンプリングすることで、低速時は密に、高速時は疎にサンプルされるため、停止精度と計算負荷軽減の両立を図っている。
Calculate initial state
速度計画のための初期値を計算する。初期値は状況に応じて下表のように計算する。
状況 | 初期速度 | 初期加速度 |
---|---|---|
最初の計算時 | 現在車速 | 0.0 |
発進時 | engage_velocity |
engage_acceleration |
現在車速と計画車速が乖離 | 現在車速 | 前回計画値 |
通常時 | 前回計画値 | 前回計画値 |
Smooth velocity
速度の計画を行う。速度計画のアルゴリズムはJerkFiltered
, L2
, Linf
の 3 種類のうちからコンフィグで指定する。
最適化のソルバは OSQP[1]を利用する。
JerkFiltered
速度の 2 乗(最小化で表すため負値で表現)、制限速度逸脱量の 2 乗、制限加度逸脱量の 2 乗、制限ジャーク逸脱量の 2 乗、ジャークの 2 乗の総和を最小化する。
L2
速度の 2 乗(最小化で表すため負値で表現)、制限速度逸脱量の 2 乗、制限加度逸脱量の 2 乗、疑似ジャーク[2]の 2 乗の総和を最小化する。
Linf
速度の 2 乗(最小化で表すため負値で表現)、制限速度逸脱量の 2 乗、制限加度逸脱量の 2 乗の総和と疑似ジャーク[2]の絶対最大値の和の最小化する。
Post process
計画された軌道の後処理を行う。
- 停止点より先の速度を 0 に設定
- 速度がパラメータで与えられる
max_velocity
以下となるように設定 - 自車位置より手前の点における速度を設定
- Trajectory の仕様に合わせてリサンプリング(
post resampling
) - デバッグデータの出力
最適化の計算が終わったあと、次のノードに経路を渡す前にpost resampling
と呼ばれるリサンプリングを行う。ここで再度リサンプリングを行っている理由としては、最適化前で必要な経路間隔と後段のモジュールに渡す経路間隔が必ずしも一致していないからであり、その差を埋めるために再度サンプリングを行っている。そのため、post resampling
では後段モジュールの経路仕様を確認してパラメータを決める必要がある。なお、最適化アルゴリズムの計算負荷が高く、最初のリサンプリングで経路間隔が後段モジュールの経路仕様より疎になっている場合、post resampling
で経路を蜜にリサンプリングする。逆に最適化アルゴリズムの計算負荷が小さく、最初のリサンプリングで経路間隔が後段の経路仕様より蜜になっている場合は、post resampling
で経路をその仕様に合わせて疎にリサンプリングする。
Inputs / Outputs
Input
Name | Type | Description |
---|---|---|
~/input/trajectory |
autoware_auto_planning_msgs/Trajectory |
Reference trajectory |
/planning/scenario_planning/max_velocity |
std_msgs/Float32 |
External velocity limit [m/s] |
/localization/kinematic_state |
nav_msgs/Odometry |
Current odometry |
/tf |
tf2_msgs/TFMessage |
TF |
/tf_static |
tf2_msgs/TFMessage |
TF static |
Output
Name | Type | Description |
---|---|---|
~/output/trajectory |
autoware_auto_planning_msgs/Trajectory |
Modified trajectory |
/planning/scenario_planning/current_max_velocity |
std_msgs/Float32 |
Current external velocity limit [m/s] |
~/closest_velocity |
std_msgs/Float32 |
Planned velocity closest to ego base_link (for debug) |
~/closest_acceleration |
std_msgs/Float32 |
Planned acceleration closest to ego base_link (for debug) |
~/closest_jerk |
std_msgs/Float32 |
Planned jerk closest to ego base_link (for debug) |
~/debug/trajectory_raw |
autoware_auto_planning_msgs/Trajectory |
Extracted trajectory (for debug) |
File truncated at 100 lines see the full file
Package Dependencies
System Dependencies
Launch files
- launch/motion_velocity_smoother.launch.xml
-
- common_param_path [default: $(find-pkg-share motion_velocity_smoother)/config/default_common.param.yaml]
- nearest_search_param_path
- input_trajectory [default: /planning/scenario_planning/scenario_selector/trajectory]
- output_trajectory [default: /planning/scenario_planning/trajectory]
- publish_debug_trajs [default: false]
- velocity_smoother_type [default: JerkFiltered]
- param_path [default: $(find-pkg-share motion_velocity_smoother)/config/default_motion_velocity_smoother.param.yaml]
- velocity_smoother_param_path [default: $(find-pkg-share motion_velocity_smoother)/config/$(var velocity_smoother_type).param.yaml]
Messages
Services
Plugins
Recent questions tagged motion_velocity_smoother at Robotics Stack Exchange
Package Summary
Tags | No category tags. |
Version | 0.1.0 |
License | Apache License 2.0 |
Build type | AMENT_CMAKE |
Use | RECOMMENDED |
Repository Summary
Description | |
Checkout URI | https://github.com/ieiauto/autodrrt.git |
VCS Type | git |
VCS Version | main |
Last Updated | 2025-05-30 |
Dev Status | UNKNOWN |
Released | UNRELEASED |
Tags | No category tags. |
Contributing |
Help Wanted (-)
Good First Issues (-) Pull Requests to Review (-) |
Package Description
Additional Links
Maintainers
- Fumiya Watanabe
- Takamasa Horibe
- Makoto Kurihara
Authors
- Takamasa Horibe
- Fumiya Watanabe
- Yutaka Shimizu
- Makoto Kurihara
Motion Velocity Smoother
Purpose
motion_velocity_smoother
は目標軌道上の各点における望ましい車速を計画して出力するモジュールである。
このモジュールは、速度の最大化と乗り心地の良さを両立するために、事前に指定された制限速度、制限加速度および制限躍度の範囲で車速を計画する。
加速度や躍度の制限を与えることは車速の変化を滑らかにすることに対応するため、このモジュールをmotion_velocity_smoother
と呼んでいる。
Inner-workings / Algorithms
Flow chart
Extract trajectory
自車後輪軸中心位置に最も近い参照経路上の点に対し、extract_behind_dist
だけ戻った点からextract_ahead_dist
だけ進んだ点までの参照経路を抜き出す。
Apply external velocity limit
モジュール外部から指定された速度制限を適用する。
ここで扱う外部の速度制限は/planning/scenario_planning/max_velocity
の topic で渡されるもので、地図上で設定された速度制限など、参照経路にすでに設定されている制限速度とは別である。
外部から指定される速度制限は、パラメータで指定されている減速度および躍度の制限の範囲で減速可能な位置から速度制限を適用する。
Apply stop approaching velocity
停止点に近づいたときの速度を設定する。障害物近傍まで近づく場合や、正着精度向上などの目的に用いる。
Apply lateral acceleration limit
経路の曲率に応じて、指定された最大横加速度max_lateral_accel
を超えない速度を制限速度として設定する。ただし、制限速度はmin_curve_velocity
を下回らないように設定する。
Resample trajectory
指定された時間間隔で経路の点を再サンプルする。ただし、経路全体の長さはmin_trajectory_length
からmax_trajectory_length
の間となるように再サンプルを行い、点の間隔はmin_trajectory_interval_distance
より小さくならないようにする。
現在車速でresample_time
の間進む距離までは密にサンプリングし、それ以降は疎にサンプリングする。
この方法でサンプリングすることで、低速時は密に、高速時は疎にサンプルされるため、停止精度と計算負荷軽減の両立を図っている。
Calculate initial state
速度計画のための初期値を計算する。初期値は状況に応じて下表のように計算する。
状況 | 初期速度 | 初期加速度 |
---|---|---|
最初の計算時 | 現在車速 | 0.0 |
発進時 | engage_velocity |
engage_acceleration |
現在車速と計画車速が乖離 | 現在車速 | 前回計画値 |
通常時 | 前回計画値 | 前回計画値 |
Smooth velocity
速度の計画を行う。速度計画のアルゴリズムはJerkFiltered
, L2
, Linf
の 3 種類のうちからコンフィグで指定する。
最適化のソルバは OSQP[1]を利用する。
JerkFiltered
速度の 2 乗(最小化で表すため負値で表現)、制限速度逸脱量の 2 乗、制限加度逸脱量の 2 乗、制限ジャーク逸脱量の 2 乗、ジャークの 2 乗の総和を最小化する。
L2
速度の 2 乗(最小化で表すため負値で表現)、制限速度逸脱量の 2 乗、制限加度逸脱量の 2 乗、疑似ジャーク[2]の 2 乗の総和を最小化する。
Linf
速度の 2 乗(最小化で表すため負値で表現)、制限速度逸脱量の 2 乗、制限加度逸脱量の 2 乗の総和と疑似ジャーク[2]の絶対最大値の和の最小化する。
Post process
計画された軌道の後処理を行う。
- 停止点より先の速度を 0 に設定
- 速度がパラメータで与えられる
max_velocity
以下となるように設定 - 自車位置より手前の点における速度を設定
- Trajectory の仕様に合わせてリサンプリング(
post resampling
) - デバッグデータの出力
最適化の計算が終わったあと、次のノードに経路を渡す前にpost resampling
と呼ばれるリサンプリングを行う。ここで再度リサンプリングを行っている理由としては、最適化前で必要な経路間隔と後段のモジュールに渡す経路間隔が必ずしも一致していないからであり、その差を埋めるために再度サンプリングを行っている。そのため、post resampling
では後段モジュールの経路仕様を確認してパラメータを決める必要がある。なお、最適化アルゴリズムの計算負荷が高く、最初のリサンプリングで経路間隔が後段モジュールの経路仕様より疎になっている場合、post resampling
で経路を蜜にリサンプリングする。逆に最適化アルゴリズムの計算負荷が小さく、最初のリサンプリングで経路間隔が後段の経路仕様より蜜になっている場合は、post resampling
で経路をその仕様に合わせて疎にリサンプリングする。
Inputs / Outputs
Input
Name | Type | Description |
---|---|---|
~/input/trajectory |
autoware_auto_planning_msgs/Trajectory |
Reference trajectory |
/planning/scenario_planning/max_velocity |
std_msgs/Float32 |
External velocity limit [m/s] |
/localization/kinematic_state |
nav_msgs/Odometry |
Current odometry |
/tf |
tf2_msgs/TFMessage |
TF |
/tf_static |
tf2_msgs/TFMessage |
TF static |
Output
Name | Type | Description |
---|---|---|
~/output/trajectory |
autoware_auto_planning_msgs/Trajectory |
Modified trajectory |
/planning/scenario_planning/current_max_velocity |
std_msgs/Float32 |
Current external velocity limit [m/s] |
~/closest_velocity |
std_msgs/Float32 |
Planned velocity closest to ego base_link (for debug) |
~/closest_acceleration |
std_msgs/Float32 |
Planned acceleration closest to ego base_link (for debug) |
~/closest_jerk |
std_msgs/Float32 |
Planned jerk closest to ego base_link (for debug) |
~/debug/trajectory_raw |
autoware_auto_planning_msgs/Trajectory |
Extracted trajectory (for debug) |
File truncated at 100 lines see the full file
Package Dependencies
System Dependencies
Launch files
- launch/motion_velocity_smoother.launch.xml
-
- common_param_path [default: $(find-pkg-share motion_velocity_smoother)/config/default_common.param.yaml]
- nearest_search_param_path
- input_trajectory [default: /planning/scenario_planning/scenario_selector/trajectory]
- output_trajectory [default: /planning/scenario_planning/trajectory]
- publish_debug_trajs [default: false]
- velocity_smoother_type [default: JerkFiltered]
- param_path [default: $(find-pkg-share motion_velocity_smoother)/config/default_motion_velocity_smoother.param.yaml]
- velocity_smoother_param_path [default: $(find-pkg-share motion_velocity_smoother)/config/$(var velocity_smoother_type).param.yaml]
Messages
Services
Plugins
Recent questions tagged motion_velocity_smoother at Robotics Stack Exchange
Package Summary
Tags | No category tags. |
Version | 0.1.0 |
License | Apache License 2.0 |
Build type | AMENT_CMAKE |
Use | RECOMMENDED |
Repository Summary
Description | |
Checkout URI | https://github.com/ieiauto/autodrrt.git |
VCS Type | git |
VCS Version | main |
Last Updated | 2025-05-30 |
Dev Status | UNKNOWN |
Released | UNRELEASED |
Tags | No category tags. |
Contributing |
Help Wanted (-)
Good First Issues (-) Pull Requests to Review (-) |
Package Description
Additional Links
Maintainers
- Fumiya Watanabe
- Takamasa Horibe
- Makoto Kurihara
Authors
- Takamasa Horibe
- Fumiya Watanabe
- Yutaka Shimizu
- Makoto Kurihara
Motion Velocity Smoother
Purpose
motion_velocity_smoother
は目標軌道上の各点における望ましい車速を計画して出力するモジュールである。
このモジュールは、速度の最大化と乗り心地の良さを両立するために、事前に指定された制限速度、制限加速度および制限躍度の範囲で車速を計画する。
加速度や躍度の制限を与えることは車速の変化を滑らかにすることに対応するため、このモジュールをmotion_velocity_smoother
と呼んでいる。
Inner-workings / Algorithms
Flow chart
Extract trajectory
自車後輪軸中心位置に最も近い参照経路上の点に対し、extract_behind_dist
だけ戻った点からextract_ahead_dist
だけ進んだ点までの参照経路を抜き出す。
Apply external velocity limit
モジュール外部から指定された速度制限を適用する。
ここで扱う外部の速度制限は/planning/scenario_planning/max_velocity
の topic で渡されるもので、地図上で設定された速度制限など、参照経路にすでに設定されている制限速度とは別である。
外部から指定される速度制限は、パラメータで指定されている減速度および躍度の制限の範囲で減速可能な位置から速度制限を適用する。
Apply stop approaching velocity
停止点に近づいたときの速度を設定する。障害物近傍まで近づく場合や、正着精度向上などの目的に用いる。
Apply lateral acceleration limit
経路の曲率に応じて、指定された最大横加速度max_lateral_accel
を超えない速度を制限速度として設定する。ただし、制限速度はmin_curve_velocity
を下回らないように設定する。
Resample trajectory
指定された時間間隔で経路の点を再サンプルする。ただし、経路全体の長さはmin_trajectory_length
からmax_trajectory_length
の間となるように再サンプルを行い、点の間隔はmin_trajectory_interval_distance
より小さくならないようにする。
現在車速でresample_time
の間進む距離までは密にサンプリングし、それ以降は疎にサンプリングする。
この方法でサンプリングすることで、低速時は密に、高速時は疎にサンプルされるため、停止精度と計算負荷軽減の両立を図っている。
Calculate initial state
速度計画のための初期値を計算する。初期値は状況に応じて下表のように計算する。
状況 | 初期速度 | 初期加速度 |
---|---|---|
最初の計算時 | 現在車速 | 0.0 |
発進時 | engage_velocity |
engage_acceleration |
現在車速と計画車速が乖離 | 現在車速 | 前回計画値 |
通常時 | 前回計画値 | 前回計画値 |
Smooth velocity
速度の計画を行う。速度計画のアルゴリズムはJerkFiltered
, L2
, Linf
の 3 種類のうちからコンフィグで指定する。
最適化のソルバは OSQP[1]を利用する。
JerkFiltered
速度の 2 乗(最小化で表すため負値で表現)、制限速度逸脱量の 2 乗、制限加度逸脱量の 2 乗、制限ジャーク逸脱量の 2 乗、ジャークの 2 乗の総和を最小化する。
L2
速度の 2 乗(最小化で表すため負値で表現)、制限速度逸脱量の 2 乗、制限加度逸脱量の 2 乗、疑似ジャーク[2]の 2 乗の総和を最小化する。
Linf
速度の 2 乗(最小化で表すため負値で表現)、制限速度逸脱量の 2 乗、制限加度逸脱量の 2 乗の総和と疑似ジャーク[2]の絶対最大値の和の最小化する。
Post process
計画された軌道の後処理を行う。
- 停止点より先の速度を 0 に設定
- 速度がパラメータで与えられる
max_velocity
以下となるように設定 - 自車位置より手前の点における速度を設定
- Trajectory の仕様に合わせてリサンプリング(
post resampling
) - デバッグデータの出力
最適化の計算が終わったあと、次のノードに経路を渡す前にpost resampling
と呼ばれるリサンプリングを行う。ここで再度リサンプリングを行っている理由としては、最適化前で必要な経路間隔と後段のモジュールに渡す経路間隔が必ずしも一致していないからであり、その差を埋めるために再度サンプリングを行っている。そのため、post resampling
では後段モジュールの経路仕様を確認してパラメータを決める必要がある。なお、最適化アルゴリズムの計算負荷が高く、最初のリサンプリングで経路間隔が後段モジュールの経路仕様より疎になっている場合、post resampling
で経路を蜜にリサンプリングする。逆に最適化アルゴリズムの計算負荷が小さく、最初のリサンプリングで経路間隔が後段の経路仕様より蜜になっている場合は、post resampling
で経路をその仕様に合わせて疎にリサンプリングする。
Inputs / Outputs
Input
Name | Type | Description |
---|---|---|
~/input/trajectory |
autoware_auto_planning_msgs/Trajectory |
Reference trajectory |
/planning/scenario_planning/max_velocity |
std_msgs/Float32 |
External velocity limit [m/s] |
/localization/kinematic_state |
nav_msgs/Odometry |
Current odometry |
/tf |
tf2_msgs/TFMessage |
TF |
/tf_static |
tf2_msgs/TFMessage |
TF static |
Output
Name | Type | Description |
---|---|---|
~/output/trajectory |
autoware_auto_planning_msgs/Trajectory |
Modified trajectory |
/planning/scenario_planning/current_max_velocity |
std_msgs/Float32 |
Current external velocity limit [m/s] |
~/closest_velocity |
std_msgs/Float32 |
Planned velocity closest to ego base_link (for debug) |
~/closest_acceleration |
std_msgs/Float32 |
Planned acceleration closest to ego base_link (for debug) |
~/closest_jerk |
std_msgs/Float32 |
Planned jerk closest to ego base_link (for debug) |
~/debug/trajectory_raw |
autoware_auto_planning_msgs/Trajectory |
Extracted trajectory (for debug) |
File truncated at 100 lines see the full file
Package Dependencies
System Dependencies
Launch files
- launch/motion_velocity_smoother.launch.xml
-
- common_param_path [default: $(find-pkg-share motion_velocity_smoother)/config/default_common.param.yaml]
- nearest_search_param_path
- input_trajectory [default: /planning/scenario_planning/scenario_selector/trajectory]
- output_trajectory [default: /planning/scenario_planning/trajectory]
- publish_debug_trajs [default: false]
- velocity_smoother_type [default: JerkFiltered]
- param_path [default: $(find-pkg-share motion_velocity_smoother)/config/default_motion_velocity_smoother.param.yaml]
- velocity_smoother_param_path [default: $(find-pkg-share motion_velocity_smoother)/config/$(var velocity_smoother_type).param.yaml]
Messages
Services
Plugins
Recent questions tagged motion_velocity_smoother at Robotics Stack Exchange
Package Summary
Tags | No category tags. |
Version | 0.1.0 |
License | Apache License 2.0 |
Build type | AMENT_CMAKE |
Use | RECOMMENDED |
Repository Summary
Description | |
Checkout URI | https://github.com/ieiauto/autodrrt.git |
VCS Type | git |
VCS Version | main |
Last Updated | 2025-05-30 |
Dev Status | UNKNOWN |
Released | UNRELEASED |
Tags | No category tags. |
Contributing |
Help Wanted (-)
Good First Issues (-) Pull Requests to Review (-) |
Package Description
Additional Links
Maintainers
- Fumiya Watanabe
- Takamasa Horibe
- Makoto Kurihara
Authors
- Takamasa Horibe
- Fumiya Watanabe
- Yutaka Shimizu
- Makoto Kurihara
Motion Velocity Smoother
Purpose
motion_velocity_smoother
は目標軌道上の各点における望ましい車速を計画して出力するモジュールである。
このモジュールは、速度の最大化と乗り心地の良さを両立するために、事前に指定された制限速度、制限加速度および制限躍度の範囲で車速を計画する。
加速度や躍度の制限を与えることは車速の変化を滑らかにすることに対応するため、このモジュールをmotion_velocity_smoother
と呼んでいる。
Inner-workings / Algorithms
Flow chart
Extract trajectory
自車後輪軸中心位置に最も近い参照経路上の点に対し、extract_behind_dist
だけ戻った点からextract_ahead_dist
だけ進んだ点までの参照経路を抜き出す。
Apply external velocity limit
モジュール外部から指定された速度制限を適用する。
ここで扱う外部の速度制限は/planning/scenario_planning/max_velocity
の topic で渡されるもので、地図上で設定された速度制限など、参照経路にすでに設定されている制限速度とは別である。
外部から指定される速度制限は、パラメータで指定されている減速度および躍度の制限の範囲で減速可能な位置から速度制限を適用する。
Apply stop approaching velocity
停止点に近づいたときの速度を設定する。障害物近傍まで近づく場合や、正着精度向上などの目的に用いる。
Apply lateral acceleration limit
経路の曲率に応じて、指定された最大横加速度max_lateral_accel
を超えない速度を制限速度として設定する。ただし、制限速度はmin_curve_velocity
を下回らないように設定する。
Resample trajectory
指定された時間間隔で経路の点を再サンプルする。ただし、経路全体の長さはmin_trajectory_length
からmax_trajectory_length
の間となるように再サンプルを行い、点の間隔はmin_trajectory_interval_distance
より小さくならないようにする。
現在車速でresample_time
の間進む距離までは密にサンプリングし、それ以降は疎にサンプリングする。
この方法でサンプリングすることで、低速時は密に、高速時は疎にサンプルされるため、停止精度と計算負荷軽減の両立を図っている。
Calculate initial state
速度計画のための初期値を計算する。初期値は状況に応じて下表のように計算する。
状況 | 初期速度 | 初期加速度 |
---|---|---|
最初の計算時 | 現在車速 | 0.0 |
発進時 | engage_velocity |
engage_acceleration |
現在車速と計画車速が乖離 | 現在車速 | 前回計画値 |
通常時 | 前回計画値 | 前回計画値 |
Smooth velocity
速度の計画を行う。速度計画のアルゴリズムはJerkFiltered
, L2
, Linf
の 3 種類のうちからコンフィグで指定する。
最適化のソルバは OSQP[1]を利用する。
JerkFiltered
速度の 2 乗(最小化で表すため負値で表現)、制限速度逸脱量の 2 乗、制限加度逸脱量の 2 乗、制限ジャーク逸脱量の 2 乗、ジャークの 2 乗の総和を最小化する。
L2
速度の 2 乗(最小化で表すため負値で表現)、制限速度逸脱量の 2 乗、制限加度逸脱量の 2 乗、疑似ジャーク[2]の 2 乗の総和を最小化する。
Linf
速度の 2 乗(最小化で表すため負値で表現)、制限速度逸脱量の 2 乗、制限加度逸脱量の 2 乗の総和と疑似ジャーク[2]の絶対最大値の和の最小化する。
Post process
計画された軌道の後処理を行う。
- 停止点より先の速度を 0 に設定
- 速度がパラメータで与えられる
max_velocity
以下となるように設定 - 自車位置より手前の点における速度を設定
- Trajectory の仕様に合わせてリサンプリング(
post resampling
) - デバッグデータの出力
最適化の計算が終わったあと、次のノードに経路を渡す前にpost resampling
と呼ばれるリサンプリングを行う。ここで再度リサンプリングを行っている理由としては、最適化前で必要な経路間隔と後段のモジュールに渡す経路間隔が必ずしも一致していないからであり、その差を埋めるために再度サンプリングを行っている。そのため、post resampling
では後段モジュールの経路仕様を確認してパラメータを決める必要がある。なお、最適化アルゴリズムの計算負荷が高く、最初のリサンプリングで経路間隔が後段モジュールの経路仕様より疎になっている場合、post resampling
で経路を蜜にリサンプリングする。逆に最適化アルゴリズムの計算負荷が小さく、最初のリサンプリングで経路間隔が後段の経路仕様より蜜になっている場合は、post resampling
で経路をその仕様に合わせて疎にリサンプリングする。
Inputs / Outputs
Input
Name | Type | Description |
---|---|---|
~/input/trajectory |
autoware_auto_planning_msgs/Trajectory |
Reference trajectory |
/planning/scenario_planning/max_velocity |
std_msgs/Float32 |
External velocity limit [m/s] |
/localization/kinematic_state |
nav_msgs/Odometry |
Current odometry |
/tf |
tf2_msgs/TFMessage |
TF |
/tf_static |
tf2_msgs/TFMessage |
TF static |
Output
Name | Type | Description |
---|---|---|
~/output/trajectory |
autoware_auto_planning_msgs/Trajectory |
Modified trajectory |
/planning/scenario_planning/current_max_velocity |
std_msgs/Float32 |
Current external velocity limit [m/s] |
~/closest_velocity |
std_msgs/Float32 |
Planned velocity closest to ego base_link (for debug) |
~/closest_acceleration |
std_msgs/Float32 |
Planned acceleration closest to ego base_link (for debug) |
~/closest_jerk |
std_msgs/Float32 |
Planned jerk closest to ego base_link (for debug) |
~/debug/trajectory_raw |
autoware_auto_planning_msgs/Trajectory |
Extracted trajectory (for debug) |
File truncated at 100 lines see the full file
Package Dependencies
System Dependencies
Launch files
- launch/motion_velocity_smoother.launch.xml
-
- common_param_path [default: $(find-pkg-share motion_velocity_smoother)/config/default_common.param.yaml]
- nearest_search_param_path
- input_trajectory [default: /planning/scenario_planning/scenario_selector/trajectory]
- output_trajectory [default: /planning/scenario_planning/trajectory]
- publish_debug_trajs [default: false]
- velocity_smoother_type [default: JerkFiltered]
- param_path [default: $(find-pkg-share motion_velocity_smoother)/config/default_motion_velocity_smoother.param.yaml]
- velocity_smoother_param_path [default: $(find-pkg-share motion_velocity_smoother)/config/$(var velocity_smoother_type).param.yaml]
Messages
Services
Plugins
Recent questions tagged motion_velocity_smoother at Robotics Stack Exchange
Package Summary
Tags | No category tags. |
Version | 0.1.0 |
License | Apache License 2.0 |
Build type | AMENT_CMAKE |
Use | RECOMMENDED |
Repository Summary
Description | |
Checkout URI | https://github.com/ieiauto/autodrrt.git |
VCS Type | git |
VCS Version | main |
Last Updated | 2025-05-30 |
Dev Status | UNKNOWN |
Released | UNRELEASED |
Tags | No category tags. |
Contributing |
Help Wanted (-)
Good First Issues (-) Pull Requests to Review (-) |
Package Description
Additional Links
Maintainers
- Fumiya Watanabe
- Takamasa Horibe
- Makoto Kurihara
Authors
- Takamasa Horibe
- Fumiya Watanabe
- Yutaka Shimizu
- Makoto Kurihara
Motion Velocity Smoother
Purpose
motion_velocity_smoother
は目標軌道上の各点における望ましい車速を計画して出力するモジュールである。
このモジュールは、速度の最大化と乗り心地の良さを両立するために、事前に指定された制限速度、制限加速度および制限躍度の範囲で車速を計画する。
加速度や躍度の制限を与えることは車速の変化を滑らかにすることに対応するため、このモジュールをmotion_velocity_smoother
と呼んでいる。
Inner-workings / Algorithms
Flow chart
Extract trajectory
自車後輪軸中心位置に最も近い参照経路上の点に対し、extract_behind_dist
だけ戻った点からextract_ahead_dist
だけ進んだ点までの参照経路を抜き出す。
Apply external velocity limit
モジュール外部から指定された速度制限を適用する。
ここで扱う外部の速度制限は/planning/scenario_planning/max_velocity
の topic で渡されるもので、地図上で設定された速度制限など、参照経路にすでに設定されている制限速度とは別である。
外部から指定される速度制限は、パラメータで指定されている減速度および躍度の制限の範囲で減速可能な位置から速度制限を適用する。
Apply stop approaching velocity
停止点に近づいたときの速度を設定する。障害物近傍まで近づく場合や、正着精度向上などの目的に用いる。
Apply lateral acceleration limit
経路の曲率に応じて、指定された最大横加速度max_lateral_accel
を超えない速度を制限速度として設定する。ただし、制限速度はmin_curve_velocity
を下回らないように設定する。
Resample trajectory
指定された時間間隔で経路の点を再サンプルする。ただし、経路全体の長さはmin_trajectory_length
からmax_trajectory_length
の間となるように再サンプルを行い、点の間隔はmin_trajectory_interval_distance
より小さくならないようにする。
現在車速でresample_time
の間進む距離までは密にサンプリングし、それ以降は疎にサンプリングする。
この方法でサンプリングすることで、低速時は密に、高速時は疎にサンプルされるため、停止精度と計算負荷軽減の両立を図っている。
Calculate initial state
速度計画のための初期値を計算する。初期値は状況に応じて下表のように計算する。
状況 | 初期速度 | 初期加速度 |
---|---|---|
最初の計算時 | 現在車速 | 0.0 |
発進時 | engage_velocity |
engage_acceleration |
現在車速と計画車速が乖離 | 現在車速 | 前回計画値 |
通常時 | 前回計画値 | 前回計画値 |
Smooth velocity
速度の計画を行う。速度計画のアルゴリズムはJerkFiltered
, L2
, Linf
の 3 種類のうちからコンフィグで指定する。
最適化のソルバは OSQP[1]を利用する。
JerkFiltered
速度の 2 乗(最小化で表すため負値で表現)、制限速度逸脱量の 2 乗、制限加度逸脱量の 2 乗、制限ジャーク逸脱量の 2 乗、ジャークの 2 乗の総和を最小化する。
L2
速度の 2 乗(最小化で表すため負値で表現)、制限速度逸脱量の 2 乗、制限加度逸脱量の 2 乗、疑似ジャーク[2]の 2 乗の総和を最小化する。
Linf
速度の 2 乗(最小化で表すため負値で表現)、制限速度逸脱量の 2 乗、制限加度逸脱量の 2 乗の総和と疑似ジャーク[2]の絶対最大値の和の最小化する。
Post process
計画された軌道の後処理を行う。
- 停止点より先の速度を 0 に設定
- 速度がパラメータで与えられる
max_velocity
以下となるように設定 - 自車位置より手前の点における速度を設定
- Trajectory の仕様に合わせてリサンプリング(
post resampling
) - デバッグデータの出力
最適化の計算が終わったあと、次のノードに経路を渡す前にpost resampling
と呼ばれるリサンプリングを行う。ここで再度リサンプリングを行っている理由としては、最適化前で必要な経路間隔と後段のモジュールに渡す経路間隔が必ずしも一致していないからであり、その差を埋めるために再度サンプリングを行っている。そのため、post resampling
では後段モジュールの経路仕様を確認してパラメータを決める必要がある。なお、最適化アルゴリズムの計算負荷が高く、最初のリサンプリングで経路間隔が後段モジュールの経路仕様より疎になっている場合、post resampling
で経路を蜜にリサンプリングする。逆に最適化アルゴリズムの計算負荷が小さく、最初のリサンプリングで経路間隔が後段の経路仕様より蜜になっている場合は、post resampling
で経路をその仕様に合わせて疎にリサンプリングする。
Inputs / Outputs
Input
Name | Type | Description |
---|---|---|
~/input/trajectory |
autoware_auto_planning_msgs/Trajectory |
Reference trajectory |
/planning/scenario_planning/max_velocity |
std_msgs/Float32 |
External velocity limit [m/s] |
/localization/kinematic_state |
nav_msgs/Odometry |
Current odometry |
/tf |
tf2_msgs/TFMessage |
TF |
/tf_static |
tf2_msgs/TFMessage |
TF static |
Output
Name | Type | Description |
---|---|---|
~/output/trajectory |
autoware_auto_planning_msgs/Trajectory |
Modified trajectory |
/planning/scenario_planning/current_max_velocity |
std_msgs/Float32 |
Current external velocity limit [m/s] |
~/closest_velocity |
std_msgs/Float32 |
Planned velocity closest to ego base_link (for debug) |
~/closest_acceleration |
std_msgs/Float32 |
Planned acceleration closest to ego base_link (for debug) |
~/closest_jerk |
std_msgs/Float32 |
Planned jerk closest to ego base_link (for debug) |
~/debug/trajectory_raw |
autoware_auto_planning_msgs/Trajectory |
Extracted trajectory (for debug) |
File truncated at 100 lines see the full file
Package Dependencies
System Dependencies
Launch files
- launch/motion_velocity_smoother.launch.xml
-
- common_param_path [default: $(find-pkg-share motion_velocity_smoother)/config/default_common.param.yaml]
- nearest_search_param_path
- input_trajectory [default: /planning/scenario_planning/scenario_selector/trajectory]
- output_trajectory [default: /planning/scenario_planning/trajectory]
- publish_debug_trajs [default: false]
- velocity_smoother_type [default: JerkFiltered]
- param_path [default: $(find-pkg-share motion_velocity_smoother)/config/default_motion_velocity_smoother.param.yaml]
- velocity_smoother_param_path [default: $(find-pkg-share motion_velocity_smoother)/config/$(var velocity_smoother_type).param.yaml]
Messages
Services
Plugins
Recent questions tagged motion_velocity_smoother at Robotics Stack Exchange
Package Summary
Tags | No category tags. |
Version | 0.1.0 |
License | Apache License 2.0 |
Build type | AMENT_CMAKE |
Use | RECOMMENDED |
Repository Summary
Description | |
Checkout URI | https://github.com/ieiauto/autodrrt.git |
VCS Type | git |
VCS Version | main |
Last Updated | 2025-05-30 |
Dev Status | UNKNOWN |
Released | UNRELEASED |
Tags | No category tags. |
Contributing |
Help Wanted (-)
Good First Issues (-) Pull Requests to Review (-) |
Package Description
Additional Links
Maintainers
- Fumiya Watanabe
- Takamasa Horibe
- Makoto Kurihara
Authors
- Takamasa Horibe
- Fumiya Watanabe
- Yutaka Shimizu
- Makoto Kurihara
Motion Velocity Smoother
Purpose
motion_velocity_smoother
は目標軌道上の各点における望ましい車速を計画して出力するモジュールである。
このモジュールは、速度の最大化と乗り心地の良さを両立するために、事前に指定された制限速度、制限加速度および制限躍度の範囲で車速を計画する。
加速度や躍度の制限を与えることは車速の変化を滑らかにすることに対応するため、このモジュールをmotion_velocity_smoother
と呼んでいる。
Inner-workings / Algorithms
Flow chart
Extract trajectory
自車後輪軸中心位置に最も近い参照経路上の点に対し、extract_behind_dist
だけ戻った点からextract_ahead_dist
だけ進んだ点までの参照経路を抜き出す。
Apply external velocity limit
モジュール外部から指定された速度制限を適用する。
ここで扱う外部の速度制限は/planning/scenario_planning/max_velocity
の topic で渡されるもので、地図上で設定された速度制限など、参照経路にすでに設定されている制限速度とは別である。
外部から指定される速度制限は、パラメータで指定されている減速度および躍度の制限の範囲で減速可能な位置から速度制限を適用する。
Apply stop approaching velocity
停止点に近づいたときの速度を設定する。障害物近傍まで近づく場合や、正着精度向上などの目的に用いる。
Apply lateral acceleration limit
経路の曲率に応じて、指定された最大横加速度max_lateral_accel
を超えない速度を制限速度として設定する。ただし、制限速度はmin_curve_velocity
を下回らないように設定する。
Resample trajectory
指定された時間間隔で経路の点を再サンプルする。ただし、経路全体の長さはmin_trajectory_length
からmax_trajectory_length
の間となるように再サンプルを行い、点の間隔はmin_trajectory_interval_distance
より小さくならないようにする。
現在車速でresample_time
の間進む距離までは密にサンプリングし、それ以降は疎にサンプリングする。
この方法でサンプリングすることで、低速時は密に、高速時は疎にサンプルされるため、停止精度と計算負荷軽減の両立を図っている。
Calculate initial state
速度計画のための初期値を計算する。初期値は状況に応じて下表のように計算する。
状況 | 初期速度 | 初期加速度 |
---|---|---|
最初の計算時 | 現在車速 | 0.0 |
発進時 | engage_velocity |
engage_acceleration |
現在車速と計画車速が乖離 | 現在車速 | 前回計画値 |
通常時 | 前回計画値 | 前回計画値 |
Smooth velocity
速度の計画を行う。速度計画のアルゴリズムはJerkFiltered
, L2
, Linf
の 3 種類のうちからコンフィグで指定する。
最適化のソルバは OSQP[1]を利用する。
JerkFiltered
速度の 2 乗(最小化で表すため負値で表現)、制限速度逸脱量の 2 乗、制限加度逸脱量の 2 乗、制限ジャーク逸脱量の 2 乗、ジャークの 2 乗の総和を最小化する。
L2
速度の 2 乗(最小化で表すため負値で表現)、制限速度逸脱量の 2 乗、制限加度逸脱量の 2 乗、疑似ジャーク[2]の 2 乗の総和を最小化する。
Linf
速度の 2 乗(最小化で表すため負値で表現)、制限速度逸脱量の 2 乗、制限加度逸脱量の 2 乗の総和と疑似ジャーク[2]の絶対最大値の和の最小化する。
Post process
計画された軌道の後処理を行う。
- 停止点より先の速度を 0 に設定
- 速度がパラメータで与えられる
max_velocity
以下となるように設定 - 自車位置より手前の点における速度を設定
- Trajectory の仕様に合わせてリサンプリング(
post resampling
) - デバッグデータの出力
最適化の計算が終わったあと、次のノードに経路を渡す前にpost resampling
と呼ばれるリサンプリングを行う。ここで再度リサンプリングを行っている理由としては、最適化前で必要な経路間隔と後段のモジュールに渡す経路間隔が必ずしも一致していないからであり、その差を埋めるために再度サンプリングを行っている。そのため、post resampling
では後段モジュールの経路仕様を確認してパラメータを決める必要がある。なお、最適化アルゴリズムの計算負荷が高く、最初のリサンプリングで経路間隔が後段モジュールの経路仕様より疎になっている場合、post resampling
で経路を蜜にリサンプリングする。逆に最適化アルゴリズムの計算負荷が小さく、最初のリサンプリングで経路間隔が後段の経路仕様より蜜になっている場合は、post resampling
で経路をその仕様に合わせて疎にリサンプリングする。
Inputs / Outputs
Input
Name | Type | Description |
---|---|---|
~/input/trajectory |
autoware_auto_planning_msgs/Trajectory |
Reference trajectory |
/planning/scenario_planning/max_velocity |
std_msgs/Float32 |
External velocity limit [m/s] |
/localization/kinematic_state |
nav_msgs/Odometry |
Current odometry |
/tf |
tf2_msgs/TFMessage |
TF |
/tf_static |
tf2_msgs/TFMessage |
TF static |
Output
Name | Type | Description |
---|---|---|
~/output/trajectory |
autoware_auto_planning_msgs/Trajectory |
Modified trajectory |
/planning/scenario_planning/current_max_velocity |
std_msgs/Float32 |
Current external velocity limit [m/s] |
~/closest_velocity |
std_msgs/Float32 |
Planned velocity closest to ego base_link (for debug) |
~/closest_acceleration |
std_msgs/Float32 |
Planned acceleration closest to ego base_link (for debug) |
~/closest_jerk |
std_msgs/Float32 |
Planned jerk closest to ego base_link (for debug) |
~/debug/trajectory_raw |
autoware_auto_planning_msgs/Trajectory |
Extracted trajectory (for debug) |
File truncated at 100 lines see the full file
Package Dependencies
System Dependencies
Launch files
- launch/motion_velocity_smoother.launch.xml
-
- common_param_path [default: $(find-pkg-share motion_velocity_smoother)/config/default_common.param.yaml]
- nearest_search_param_path
- input_trajectory [default: /planning/scenario_planning/scenario_selector/trajectory]
- output_trajectory [default: /planning/scenario_planning/trajectory]
- publish_debug_trajs [default: false]
- velocity_smoother_type [default: JerkFiltered]
- param_path [default: $(find-pkg-share motion_velocity_smoother)/config/default_motion_velocity_smoother.param.yaml]
- velocity_smoother_param_path [default: $(find-pkg-share motion_velocity_smoother)/config/$(var velocity_smoother_type).param.yaml]
Messages
Services
Plugins
Recent questions tagged motion_velocity_smoother at Robotics Stack Exchange
Package Summary
Tags | No category tags. |
Version | 0.1.0 |
License | Apache License 2.0 |
Build type | AMENT_CMAKE |
Use | RECOMMENDED |
Repository Summary
Description | |
Checkout URI | https://github.com/ieiauto/autodrrt.git |
VCS Type | git |
VCS Version | main |
Last Updated | 2025-05-30 |
Dev Status | UNKNOWN |
Released | UNRELEASED |
Tags | No category tags. |
Contributing |
Help Wanted (-)
Good First Issues (-) Pull Requests to Review (-) |
Package Description
Additional Links
Maintainers
- Fumiya Watanabe
- Takamasa Horibe
- Makoto Kurihara
Authors
- Takamasa Horibe
- Fumiya Watanabe
- Yutaka Shimizu
- Makoto Kurihara
Motion Velocity Smoother
Purpose
motion_velocity_smoother
は目標軌道上の各点における望ましい車速を計画して出力するモジュールである。
このモジュールは、速度の最大化と乗り心地の良さを両立するために、事前に指定された制限速度、制限加速度および制限躍度の範囲で車速を計画する。
加速度や躍度の制限を与えることは車速の変化を滑らかにすることに対応するため、このモジュールをmotion_velocity_smoother
と呼んでいる。
Inner-workings / Algorithms
Flow chart
Extract trajectory
自車後輪軸中心位置に最も近い参照経路上の点に対し、extract_behind_dist
だけ戻った点からextract_ahead_dist
だけ進んだ点までの参照経路を抜き出す。
Apply external velocity limit
モジュール外部から指定された速度制限を適用する。
ここで扱う外部の速度制限は/planning/scenario_planning/max_velocity
の topic で渡されるもので、地図上で設定された速度制限など、参照経路にすでに設定されている制限速度とは別である。
外部から指定される速度制限は、パラメータで指定されている減速度および躍度の制限の範囲で減速可能な位置から速度制限を適用する。
Apply stop approaching velocity
停止点に近づいたときの速度を設定する。障害物近傍まで近づく場合や、正着精度向上などの目的に用いる。
Apply lateral acceleration limit
経路の曲率に応じて、指定された最大横加速度max_lateral_accel
を超えない速度を制限速度として設定する。ただし、制限速度はmin_curve_velocity
を下回らないように設定する。
Resample trajectory
指定された時間間隔で経路の点を再サンプルする。ただし、経路全体の長さはmin_trajectory_length
からmax_trajectory_length
の間となるように再サンプルを行い、点の間隔はmin_trajectory_interval_distance
より小さくならないようにする。
現在車速でresample_time
の間進む距離までは密にサンプリングし、それ以降は疎にサンプリングする。
この方法でサンプリングすることで、低速時は密に、高速時は疎にサンプルされるため、停止精度と計算負荷軽減の両立を図っている。
Calculate initial state
速度計画のための初期値を計算する。初期値は状況に応じて下表のように計算する。
状況 | 初期速度 | 初期加速度 |
---|---|---|
最初の計算時 | 現在車速 | 0.0 |
発進時 | engage_velocity |
engage_acceleration |
現在車速と計画車速が乖離 | 現在車速 | 前回計画値 |
通常時 | 前回計画値 | 前回計画値 |
Smooth velocity
速度の計画を行う。速度計画のアルゴリズムはJerkFiltered
, L2
, Linf
の 3 種類のうちからコンフィグで指定する。
最適化のソルバは OSQP[1]を利用する。
JerkFiltered
速度の 2 乗(最小化で表すため負値で表現)、制限速度逸脱量の 2 乗、制限加度逸脱量の 2 乗、制限ジャーク逸脱量の 2 乗、ジャークの 2 乗の総和を最小化する。
L2
速度の 2 乗(最小化で表すため負値で表現)、制限速度逸脱量の 2 乗、制限加度逸脱量の 2 乗、疑似ジャーク[2]の 2 乗の総和を最小化する。
Linf
速度の 2 乗(最小化で表すため負値で表現)、制限速度逸脱量の 2 乗、制限加度逸脱量の 2 乗の総和と疑似ジャーク[2]の絶対最大値の和の最小化する。
Post process
計画された軌道の後処理を行う。
- 停止点より先の速度を 0 に設定
- 速度がパラメータで与えられる
max_velocity
以下となるように設定 - 自車位置より手前の点における速度を設定
- Trajectory の仕様に合わせてリサンプリング(
post resampling
) - デバッグデータの出力
最適化の計算が終わったあと、次のノードに経路を渡す前にpost resampling
と呼ばれるリサンプリングを行う。ここで再度リサンプリングを行っている理由としては、最適化前で必要な経路間隔と後段のモジュールに渡す経路間隔が必ずしも一致していないからであり、その差を埋めるために再度サンプリングを行っている。そのため、post resampling
では後段モジュールの経路仕様を確認してパラメータを決める必要がある。なお、最適化アルゴリズムの計算負荷が高く、最初のリサンプリングで経路間隔が後段モジュールの経路仕様より疎になっている場合、post resampling
で経路を蜜にリサンプリングする。逆に最適化アルゴリズムの計算負荷が小さく、最初のリサンプリングで経路間隔が後段の経路仕様より蜜になっている場合は、post resampling
で経路をその仕様に合わせて疎にリサンプリングする。
Inputs / Outputs
Input
Name | Type | Description |
---|---|---|
~/input/trajectory |
autoware_auto_planning_msgs/Trajectory |
Reference trajectory |
/planning/scenario_planning/max_velocity |
std_msgs/Float32 |
External velocity limit [m/s] |
/localization/kinematic_state |
nav_msgs/Odometry |
Current odometry |
/tf |
tf2_msgs/TFMessage |
TF |
/tf_static |
tf2_msgs/TFMessage |
TF static |
Output
Name | Type | Description |
---|---|---|
~/output/trajectory |
autoware_auto_planning_msgs/Trajectory |
Modified trajectory |
/planning/scenario_planning/current_max_velocity |
std_msgs/Float32 |
Current external velocity limit [m/s] |
~/closest_velocity |
std_msgs/Float32 |
Planned velocity closest to ego base_link (for debug) |
~/closest_acceleration |
std_msgs/Float32 |
Planned acceleration closest to ego base_link (for debug) |
~/closest_jerk |
std_msgs/Float32 |
Planned jerk closest to ego base_link (for debug) |
~/debug/trajectory_raw |
autoware_auto_planning_msgs/Trajectory |
Extracted trajectory (for debug) |
File truncated at 100 lines see the full file
Package Dependencies
System Dependencies
Launch files
- launch/motion_velocity_smoother.launch.xml
-
- common_param_path [default: $(find-pkg-share motion_velocity_smoother)/config/default_common.param.yaml]
- nearest_search_param_path
- input_trajectory [default: /planning/scenario_planning/scenario_selector/trajectory]
- output_trajectory [default: /planning/scenario_planning/trajectory]
- publish_debug_trajs [default: false]
- velocity_smoother_type [default: JerkFiltered]
- param_path [default: $(find-pkg-share motion_velocity_smoother)/config/default_motion_velocity_smoother.param.yaml]
- velocity_smoother_param_path [default: $(find-pkg-share motion_velocity_smoother)/config/$(var velocity_smoother_type).param.yaml]