stop

役割

stop は simple_pure_pursuit の終点接近時に並進目標速度を生成し、停止完了を判定する StopController です。独立した ROS node ではなく、rogi_control/stop/stop.hpp を include して使用します。

停止方式は path.follow.stop.mode で選択します。

mode	概要	主に使う情報
`distance_interpolation`	終点距離による速度補間	終点距離
`velocity_ff`	制動距離から速度を先に決める速度 FF	終点距離、最大減速度
`bang_bang`	一定速度保持から終点への P 制御へ切り替える	終点距離、切替後の経過時間

既定値は distance_interpolation です。mode は起動時に読み込まれ、存在しない文字列を指定すると node の初期化を失敗させます。

記号

以降では次の記号を使います。

記号	意味	対応 parameter
\(d_g\)	現在位置から経路終点までの距離	―
\(d_s\)	選択した mode へ切り替える終点距離	各 mode の `switching_distance`
\(d_{goal}\)	停止完了とみなす位置許容差	`path.follow.goal.dist_threshold`
\(d\)	速度計算に使う残り制動距離	―
\(v_{max}\)	並進速度上限	`path.follow.velocity.max_linear`
\(v_k\)	odometry から得た現在並進速度	―
\(v_{cmd,k}\)	今回出力する並進目標速度	―
\(\Delta t\)	制御周期	`control_period_ms / 1000`
\(a_{dec}\)	速度 FF の最大減速度の大きさ	`path.follow.stop.velocity_ff.max_deceleration`

distance_interpolation と velocity_ff、および未切替の bang_bang では、終点距離が切替距離以上なら pure pursuit の並進速度上限をそのまま返します。

\[v_{cmd}=v_{max}\qquad(d_g\ge d_s)\]

\(d_g<d_s\) になったときだけ、選択した停止アルゴリズムへ切り替えます。velocity_ff と bang_bang の P 制御段階では、位置許容差の内側で速度入力が 0 になるよう、次の残り制動距離を使います。

\[d=\max(0,d_g-d_{goal})\]

現在並進速度は停止完了判定に使います。現行の3方式の目標速度式は現在速度を参照しません。/odom の body velocity から計算します。

\[v_k=\sqrt{v_x^2+v_y^2}\]

distance_interpolation

従来の停止ロジックです。減速開始距離を \(d_s\)、停止 gain を \(k_s\) とします。

\[\begin{split}v_{cmd}=\begin{cases} v_{max} & (d_g\ge d_s)\\ \min\left(v_{max},\ v_{max}\alpha+k_s d_g(1-\alpha)\right) & (d_g<d_s) \end{cases}\end{split}\]

\[\alpha=\operatorname{clamp}\left(\frac{d_g}{d_s},0,1\right)\]

\(d_s\) は path.follow.stop.distance_interpolation.switching_distance、\(k_s\) は path.follow.stop.distance_interpolation.gain です。現在速度をフィードバックしないため単純ですが、実機の応答遅れや慣性によって停止位置が変わります。

velocity_ff

一定減速度 \(a_{dec}\) で残り距離 \(d\) を使い切る速度を、運動方程式から計算します。

\[v^2-v_{end}^2=2a_{dec}d\]

終端速度を \(v_{end}=0\) とすると、目標速度は次になります。

\[v_{cmd}=\min\left(v_{max},\sqrt{2a_{dec}d}\right)\]

path.follow.stop.velocity_ff.max_deceleration を小さくすると早い位置から緩やかに減速し、大きくすると終点近くまで高速を保ちます。この方式は現在速度を式に使わないため、速度計測のノイズに強い一方、指令どおりの減速度が実機で出ない場合は停止位置に誤差が残ります。

bang_bang

現行実装は、切替距離内に入った時刻を保持し、一定速度保持と P 制御の2段階で停止します。加速度を積分する方式ではありません。

初めて \(d_g<d_s\) になった時点を \(t_0\) とし、\(T_h\) を hold_duration_sec、\(v_h\) を hold_speed、\(k_p\) を p_gain とします。

\[\begin{split}v_{cmd}=\begin{cases} v_{max} & (\text{未切替かつ }d_g\ge d_s)\\ \min(v_{max},v_h) & (t-t_0<T_h)\\ \operatorname{clamp}(k_p\max(0,d_g-d_{goal}),0,\min(v_{max},v_h)) & (t-t_0\ge T_h) \end{cases}\end{split}\]

一度切り替わると、距離が再び switching_distance 以上になっても保持・P 制御を継続します。経過時間には steady_clock を使うため、Gazebo のシミュレーション時刻ではなく実時間です。新しい FollowPath goal の開始時に履歴をリセットします。

P 制御段階では simple_pure_pursuit が並進方向を現在位置から終点へ向け直します。終点を通り過ぎた場合も終点側へ戻る指令になります。一定速度保持中には action の停止完了判定を行わず、P 制御へ移った後に下記の4条件で判定します。角速度は引き続き pure pursuit 側で計算します。

pseudo_acceleration_ff は削除済みです。この mode を指定すると初期化に失敗します。また、旧 bang_bang.max_acceleration / max_deceleration は現行制御で使用しません。古い example YAML にこれらのキーが残っている場合も、以下の現行キーに置き換えて調整してください。

停止完了判定

速度指令が小さくなっただけでは停止完了にしません。次の4条件をすべて満たしたときに FollowPath action を success にし、/cmd_vel を 0 にします。

\[d_g<d_{goal}\]

\[|\operatorname{wrap}(\theta_g-\theta_r)|<\theta_{goal}\]

\[|v_k|<v_{goal}\]

\[|\omega_k|<\omega_{goal}\]

角度しきい値の ROS parameter は degree、内部計算は radian です。速度判定には /odom の twist を使います。

Parameter

parameter	default	単位	使用 mode	説明
`path.follow.stop.mode`	`distance_interpolation`	―	全体	停止方式を選択
`path.follow.stop.distance_interpolation.gain`	`1.0`	1/s	`distance_interpolation`	終点距離に掛ける停止 gain \(k_s\)
`path.follow.stop.distance_interpolation.switching_distance`	`0.1`	m	`distance_interpolation`	停止制御への切替距離 \(d_s\)
`path.follow.stop.velocity_ff.switching_distance`	`1.0`	m	`velocity_ff`	停止制御への切替距離 \(d_s\)
`path.follow.stop.velocity_ff.max_deceleration`	`1.0`	m/s²	`velocity_ff`	最大減速度の大きさ \(a_{dec}\)
`path.follow.stop.bang_bang.switching_distance`	`1.0`	m	`bang_bang`	停止制御への切替距離 \(d_s\)
`path.follow.goal.dist_threshold`	`0.01`	m	全体	位置の停止完了しきい値 \(d_{goal}\)
`path.follow.goal.yaw_threshold_deg`	`1.0`	deg	全体	yaw の停止完了しきい値 \(\theta_{goal}\)
`path.follow.goal.linear_velocity_threshold`	`0.05`	m/s	全体	並進速度の停止完了しきい値 \(v_{goal}\)
`path.follow.goal.angular_velocity_threshold`	`0.05`	rad/s	全体	角速度の停止完了しきい値 \(\omega_{goal}\)
`path.follow.velocity.max_linear`	`1.0`	m/s	全体	出力速度上限 \(v_{max}\)
`path.follow.stop.bang_bang.hold_duration_sec`	`0.5`	s	`bang_bang`	一定速度の保持時間
`path.follow.stop.bang_bang.hold_speed`	`0.5`	m/s	`bang_bang`	保持速度・P 制御の速度上限
`path.follow.stop.bang_bang.p_gain`	`1.0`	1/s	`bang_bang`	残り制動距離に掛ける P gain
`control_period_ms`	`10`	ms	全体	pure pursuit の制御周期

切替距離、速度 FF の減速度、保持速度、P gain には 0 より大きい有限値が必要です。距離補間 gain、保持時間、各完了しきい値は 0 以上の有限値、制御周期は正の値にします。未選択方式の設定も検証されます。

設定例

control/config.yaml または control/<profile>/config.yaml に記述します。

simple_pure_pursuit:
  ros__parameters:
    path:
      follow:
        stop:
          mode: bang_bang
          bang_bang:
            switching_distance: 1.0
            hold_duration_sec: 0.5
            hold_speed: 0.5
            p_gain: 2.0
        goal:
          dist_threshold: 0.01
          yaw_threshold_deg: 1.0
          linear_velocity_threshold: 0.05
          angular_velocity_threshold: 0.05
    control_period_ms: 10

これは example/test/config/control/real/config.yaml の停止設定です。sample の既定方式は distance_interpolation です。test 実機設定の max_linear は 2.0 m/s、モータ上限は 1000 rad/s で、sample の 1.0 m/s、200 rad/s とは異なります。表の default はノードの宣言値であり、example ごとの調整値ではありません。