|
| 1 | +.. serest_controller: |
| 2 | +
|
| 3 | +SeReST Controller Fine-Tuning |
| 4 | +----------------------------- |
| 5 | + |
| 6 | +This guide helps you tune the SeReST (Safe Reparameterized Time) controller to |
| 7 | +track discrete paths (e.g., A* polylines) crisply, slow down gracefully near |
| 8 | +the goal, and behave safely around dynamic obstacles. |
| 9 | + |
| 10 | +The parameters below are grouped by purpose. Typical starting values and |
| 11 | +actionable “if you see X, change Y” advice are included. |
| 12 | + |
| 13 | +.. contents:: |
| 14 | + :local: |
| 15 | + :depth: 2 |
| 16 | + |
| 17 | +Quick-Start Defaults |
| 18 | +^^^^^^^^^^^^^^^^^^^^ |
| 19 | + |
| 20 | +Put these into your YAML (adjust namespace to your setup): |
| 21 | + |
| 22 | +.. code-block:: yaml |
| 23 | +
|
| 24 | + serest_controller: |
| 25 | + ros__parameters: |
| 26 | + # motion policy |
| 27 | + v_ref: 0.6 |
| 28 | + max_linear_speed: 0.6 |
| 29 | + max_angular_speed: 1.5 |
| 30 | + max_linear_acc: 0.8 |
| 31 | + max_angular_acc: 2.0 |
| 32 | + allow_reverse: false |
| 33 | +
|
| 34 | + # tracking gains |
| 35 | + k_theta: 2.0 |
| 36 | + k_y: 1.2 |
| 37 | + k_s: 0.8 |
| 38 | + ell: 0.3 |
| 39 | +
|
| 40 | + # safety governor |
| 41 | + a_brake: 1.2 |
| 42 | + a_lat_max: 1.5 |
| 43 | + d0_margin: 0.20 |
| 44 | + tau_latency: 0.10 |
| 45 | + d_hard: 0.15 |
| 46 | + t_emerg: 0.25 |
| 47 | +
|
| 48 | + # local heading smoothing |
| 49 | + blend_base: 0.6 |
| 50 | + blend_k_per_v: 0.6 |
| 51 | + kappa_max: 2.5 |
| 52 | +
|
| 53 | + # progress robustness |
| 54 | + v_progress_min: 0.05 |
| 55 | + k_s_share_max: 0.5 |
| 56 | +
|
| 57 | + # goal behavior |
| 58 | + goal_pos_tol: 0.07 |
| 59 | + goal_yaw_tol_deg: 6.0 |
| 60 | + slow_radius: 0.60 |
| 61 | + slow_min_speed: 0.03 |
| 62 | + final_align_k: 2.0 |
| 63 | + final_align_wmax: 0.6 |
| 64 | +
|
| 65 | + # corner guard (tight turns) |
| 66 | + corner_guard_enable: true |
| 67 | + a_lat_soft: 1.1 |
| 68 | + corner_gain_ey: 1.5 |
| 69 | + corner_gain_eth: 0.7 |
| 70 | + corner_gain_kappa: 0.4 |
| 71 | + corner_min_alpha: 0.35 |
| 72 | + corner_boost_omega: 0.8 |
| 73 | + apex_ey_des: 0.05 |
| 74 | +
|
| 75 | +Core Concepts |
| 76 | +^^^^^^^^^^^^^ |
| 77 | + |
| 78 | +* **Tracking controller** keeps the robot close to the path using heading and |
| 79 | + lateral error feedback (``k_theta``, ``k_y``, ``ell``) and a small longitudinal |
| 80 | + “suction” (``k_s``). |
| 81 | + |
| 82 | +* **Safety governor** re-parameterizes time (reduces forward progress) according |
| 83 | + to distance to the nearest obstacle and curvature-based limits. It can also |
| 84 | + trigger an emergency stop via TTC / distance thresholds. |
| 85 | + |
| 86 | +* **Local heading smoothing** blends headings around polyline corners to avoid |
| 87 | + discontinuities; it does *not* deform the path, preserving path fidelity. |
| 88 | + |
| 89 | +* **Goal behavior** slows down within a radius and finishes with an in-place |
| 90 | + alignment to the final yaw when position tolerance is met. |
| 91 | + |
| 92 | +* **Corner guard** reduces speed and boosts yaw when you drift to the *outside* |
| 93 | + of a tight curve, preventing wide arcs. |
| 94 | + |
| 95 | +Parameter-by-Parameter Guidance |
| 96 | +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ |
| 97 | + |
| 98 | +Tracking Gains |
| 99 | +"""""""""""""" |
| 100 | + |
| 101 | +- ``k_theta`` (typical: 1.5–3.0; default 2.0) |
| 102 | + *Effect:* heading stiffness. |
| 103 | + *Increase if:* turns feel sluggish or heading error decays slowly. |
| 104 | + *Decrease if:* oscillatory heading (wiggle) at speed. |
| 105 | + |
| 106 | +- ``k_y`` (typical: 0.8–2.0; default 1.2) |
| 107 | + *Effect:* lateral correction authority. |
| 108 | + *Increase if:* robot rides outside of the “lane” or cuts corners. |
| 109 | + *Decrease if:* lateral oscillations, especially on coarse grids. |
| 110 | + |
| 111 | +- ``ell`` (typical: 0.25–0.5 m; default 0.3) |
| 112 | + *Effect:* lateral nonlinearity scale (like an effective lookahead). |
| 113 | + *Increase if:* controller is too twitchy laterally. |
| 114 | + *Decrease if:* lateral convergence is too slow. |
| 115 | + |
| 116 | +- ``k_s`` (typical: 0.4–1.2; default 0.8) |
| 117 | + *Effect:* longitudinal “suction” to centerline in Frenet dynamics. |
| 118 | + *Increase if:* robot tends to drift along the segment instead of re-centering. |
| 119 | + *Decrease if:* tendency to fight forward progress from standstill. |
| 120 | + |
| 121 | +Motion Limits & Reference |
| 122 | +""""""""""""""""""""""""" |
| 123 | + |
| 124 | +- ``v_ref`` (typical: 0.4–0.8 m/s; default 0.6) |
| 125 | + Desired cruise speed before safety/curvature scaling. |
| 126 | + |
| 127 | +- ``max_linear_speed``, ``max_angular_speed`` |
| 128 | + Respect the robot’s true capabilities; don’t set these higher than your platform can achieve. |
| 129 | + |
| 130 | +- ``max_linear_acc``, ``max_angular_acc`` |
| 131 | + *Increase if:* robot feels lethargic on starts/stops. |
| 132 | + *Decrease if:* traction limited, wheel slip, or tipping risk. |
| 133 | + |
| 134 | +Progress Robustness |
| 135 | +""""""""""""""""""" |
| 136 | + |
| 137 | +- ``v_progress_min`` (typical: 0.03–0.10 m/s; default 0.05) |
| 138 | + Minimum forward speed when reasonably aligned; prevents “stalls” at zero. |
| 139 | + |
| 140 | +- ``k_s_share_max`` (typical: 0.3–0.7; default 0.5) |
| 141 | + Caps how much the lateral suction term can eat into forward progress. |
| 142 | + *Increase if:* robot still stalls near corners. |
| 143 | + *Decrease if:* it pushes too hard and overshoots laterally. |
| 144 | + |
| 145 | +Safety Governor |
| 146 | +""""""""""""""" |
| 147 | + |
| 148 | +- ``a_brake`` (typical: 1.0–2.0 m/s²; default 1.2) |
| 149 | + Used in safe-speed-from-distance; set below physical max for robustness. |
| 150 | + |
| 151 | +- ``a_lat_max`` (typical: 1.2–2.5 m/s²; default 1.5) |
| 152 | + Global lateral accel cap for curvature speed limit (non-corner scenarios). |
| 153 | + |
| 154 | +- ``d0_margin`` (typical: 0.15–0.30 m; default 0.20) |
| 155 | + Geometric safety buffer; increase on larger robots or noisy sensing. |
| 156 | + |
| 157 | +- ``tau_latency`` (typical: 0.08–0.15 s; default 0.10) |
| 158 | + Accounts for sensing + control delay; increase if you observe late braking. |
| 159 | + |
| 160 | +- ``d_hard`` (typical: 0.10–0.25 m; default 0.15), ``t_emerg`` (0.2–0.4 s; default 0.25) |
| 161 | + Emergency stop triggers. Make more conservative (larger ``d_hard``, smaller ``t_emerg``) |
| 162 | + if near-collisions are observed. |
| 163 | + |
| 164 | +Local Heading Smoothing (Corners on Polylines) |
| 165 | +"""""""""""""""""""""""""""""""""""""""""""""" |
| 166 | + |
| 167 | +- ``blend_base`` (typical: 0.4–0.9 m; default 0.6) |
| 168 | + Base length over which to blend headings around vertices. |
| 169 | + |
| 170 | +- ``blend_k_per_v`` (typical: 0.4–0.9 s; default 0.6) |
| 171 | + Additional blend proportional to speed; higher = smoother at high speed. |
| 172 | + |
| 173 | +- ``kappa_max`` (typical: 2.0–3.5 1/m; default 2.5) |
| 174 | + Caps surrogate curvature used for feed-forward/limits. |
| 175 | + |
| 176 | +Goal Behavior (Smooth Arrival) |
| 177 | +"""""""""""""""""""""""""""""" |
| 178 | + |
| 179 | +- ``slow_radius`` (typical: 0.5–1.0 m; default 0.6) |
| 180 | + Start slowing down here. Increase to remove overshoot near goals. |
| 181 | + |
| 182 | +- ``goal_pos_tol`` (typical: 0.05–0.10 m; default 0.07) |
| 183 | + Consider position achieved within this radius. |
| 184 | + |
| 185 | +- ``goal_yaw_tol_deg`` (typical: 4–10 deg; default 6) |
| 186 | + Orientation tolerance to declare final arrival. |
| 187 | + |
| 188 | +- ``slow_min_speed`` (typical: 0.02–0.05 m/s; default 0.03) |
| 189 | + Keeps progress inside slow zone; reduce if you see micro-oscillations. |
| 190 | + |
| 191 | +- ``final_align_k`` (typical: 1.5–3.0; default 2.0), ``final_align_wmax`` (0.4–0.8 rad/s; default 0.6) |
| 192 | + Controls the in-place final yaw alignment. Increase ``final_align_k`` or |
| 193 | + ``final_align_wmax`` if final rotation is sluggish; decrease if it overshoots. |
| 194 | + |
| 195 | +Corner Guard (Tight Turns) |
| 196 | +"""""""""""""""""""""""""" |
| 197 | + |
| 198 | +- ``corner_guard_enable`` (default: true) |
| 199 | + Enables the whole cornering safety scheme. |
| 200 | + |
| 201 | +- ``a_lat_soft`` (typical: 0.9–1.4 m/s²; default 1.1) |
| 202 | + *Corner-specific* lateral accel cap (more conservative than ``a_lat_max``). |
| 203 | + Reduce if you still drift wide; increase if it brakes too much in curves. |
| 204 | + |
| 205 | +- ``corner_gain_ey`` (typical: 1.0–2.0; default 1.5) |
| 206 | + Penalizes “being outside” the curve (positive signed lateral error). |
| 207 | + Increase to cut speed harder when you drift wide. |
| 208 | + |
| 209 | +- ``corner_gain_eth`` (typical: 0.5–1.0; default 0.7) |
| 210 | + Penalizes large heading error in turns. |
| 211 | + Increase if you understeer; decrease if it brakes for minor angular noise. |
| 212 | + |
| 213 | +- ``corner_gain_kappa`` (typical: 0.3–0.7; default 0.4) |
| 214 | + Penalizes high curvature itself. Increase to slow more in very tight corners. |
| 215 | + |
| 216 | +- ``corner_min_alpha`` (typical: 0.30–0.50; default 0.35) |
| 217 | + Lower bound on speed scaling in corners. Raise to prevent excessive slowdown. |
| 218 | + |
| 219 | +- ``corner_boost_omega`` (typical: 0.6–1.0; default 0.8) |
| 220 | + Multiplier that boosts yaw authority when outside the curve. |
| 221 | + Increase if you still open the turn; decrease if over-rotating at apex. |
| 222 | + |
| 223 | +- ``apex_ey_des`` (typical: 0.03–0.08 m; default 0.05) |
| 224 | + Desired small inward bias at apex to avoid riding the outside. |
| 225 | + |
| 226 | +Troubleshooting by Symptom |
| 227 | +^^^^^^^^^^^^^^^^^^^^^^^^^^ |
| 228 | + |
| 229 | +Robot “moonwalks” (tries to go backwards) |
| 230 | +""""""""""""""""""""""""""""""""""""""""" |
| 231 | +*Set ``allow_reverse: false``* unless you intend reverse. |
| 232 | +Ensure forward-only clamps are active (already default) and reduce ``k_s`` if |
| 233 | +longitudinal suction is pulling progress negative at standstill. |
| 234 | +Increase ``v_progress_min`` to push out of zero-speed stiction. |
| 235 | + |
| 236 | +Stalls near corners or from standstill |
| 237 | +"""""""""""""""""""""""""""""""""""""" |
| 238 | +Increase ``v_progress_min``; increase ``k_s_share_max`` so lateral suction |
| 239 | +cannot cancel too much forward progress. Consider a slightly larger ``blend_base`` |
| 240 | +for smoother heading transitions. |
| 241 | + |
| 242 | +Oscillates near goal |
| 243 | +"""""""""""""""""""" |
| 244 | +Increase ``slow_radius`` and/or ``goal_pos_tol`` slightly. Lower |
| 245 | +``slow_min_speed`` (but keep > 0). Reduce ``final_align_wmax`` or |
| 246 | +``final_align_k`` if in-place alignment overshoots. |
| 247 | + |
| 248 | +Cuts corners / rides outside in tight turns |
| 249 | +""""""""""""""""""""""""""""""""""""""""""" |
| 250 | +Enable corner guard (default true). Decrease ``a_lat_soft``. Increase |
| 251 | +``corner_gain_ey`` and/or ``corner_boost_omega``. Increase ``apex_ey_des`` a bit. |
| 252 | +If still wide, reduce ``v_ref`` or raise ``blend_base`` to smooth heading earlier. |
| 253 | + |
| 254 | +Overreacts laterally (snappy, “nervous” path hugging) |
| 255 | +""""""""""""""""""""""""""""""""""""""""""""""""""""" |
| 256 | +Increase ``ell``. Reduce ``k_y`` slightly. Reduce ``corner_boost_omega`` if only |
| 257 | +in corners. If on very coarse grids, increase ``blend_base`` to avoid rapid |
| 258 | +heading changes. |
| 259 | + |
| 260 | +Feels sluggish to turn on straights |
| 261 | +""""""""""""""""""""""""""""""""""" |
| 262 | +Increase ``k_theta``. Ensure ``max_angular_speed`` and ``max_angular_acc`` are |
| 263 | +not too low. If corner guard reduces yaw authority too much, lower |
| 264 | +``corner_gain_eth`` and ``corner_gain_kappa``. |
| 265 | + |
| 266 | +Emergency stops too often |
| 267 | +""""""""""""""""""""""""" |
| 268 | +Reduce conservatism: increase ``t_emerg`` (e.g., 0.35), reduce ``d_hard``, |
| 269 | +reduce ``d0_margin``, or increase ``v_ref`` a bit. **Only** do this if your |
| 270 | +sensing/latency is trustworthy. |
| 271 | + |
| 272 | +Abstacle distance seems too pessimistic |
| 273 | +""""""""""""""""""""""""""""""""""""""" |
| 274 | +Lower ``lethal_cost_threshold`` (if exposed), reduce ``dist_search_radius``, or |
| 275 | +prefer publishing a measured ``closest_obstacle_distance`` into the NavState. |
| 276 | + |
| 277 | +Tuning Order of Operations |
| 278 | +^^^^^^^^^^^^^^^^^^^^^^^^^^ |
| 279 | + |
| 280 | +1. **Set kinematics**: ``max_*``, ``v_ref`` to platform-safe values. |
| 281 | +2. **Basic tracking**: tune ``k_theta``, ``k_y``, ``ell`` for stable path hugging. |
| 282 | +3. **Safety**: verify emergency stop triggers with test obstacles. |
| 283 | +4. **Corners**: set ``blend_*``, ``a_lat_soft``, and corner guard gains. |
| 284 | +5. **Goal behavior**: adjust ``slow_radius``, ``goal_pos_tol``, |
| 285 | + ``final_align_*`` for a smooth, precise finish. |
| 286 | +6. **Robust progress**: tweak ``v_progress_min``, ``k_s_share_max`` if stalls occur. |
| 287 | + |
| 288 | +FAQ |
| 289 | +^^^ |
| 290 | + |
| 291 | +**Q: Can I allow reverse?** |
| 292 | +A: Yes, set ``allow_reverse: true``. Use with care; tune goal/slow behaviors to |
| 293 | +avoid reverse within the final meters unless desired. |
| 294 | + |
| 295 | +**Q: What if my path is very jagged (low map resolution)?** |
| 296 | +A: Increase ``blend_base`` and/or ``blend_k_per_v``. Consider lowering |
| 297 | +``kappa_max`` and reducing ``v_ref`` slightly. |
| 298 | + |
| 299 | +**Q: How aggressive should ``a_lat_soft`` be?** |
| 300 | +A: For indoor AMRs with solid traction, 1.0–1.4 m/s² is typical. On slippery or |
| 301 | +uneven surfaces, go lower (0.8–1.0). |
| 302 | + |
0 commit comments