Skip to main content

rust_robotics_control/
backstepping_control.rs

1//! Backstepping-style controller for a unicycle robot.
2//!
3//! Uses a recursive Lyapunov-based control law in the robot body frame to
4//! drive the robot to a target pose.
5//!
6//! References:
7//! - Claude Samson, "Control of chained systems application to path following
8//!   and time-varying point-stabilization of mobile robots":
9//!   <https://ieeexplore.ieee.org/document/362899>
10//! - R. W. Brockett, "Asymptotic Stability and Feedback Stabilization":
11//!   <https://hrl.harvard.edu/publications/brockett83asymptotic.pdf>
12//!
13//! Notes:
14//! A stationary unicycle posture cannot in general be asymptotically
15//! stabilized by smooth time-invariant pure-state feedback alone. To keep the
16//! API posture-regulation oriented while preserving robust convergence in
17//! practice, this module uses the backstepping tracking-form control law away
18//! from the goal and a terminal pose regulator close to the target.
19
20use rust_robotics_core::{ControlInput, Path2D, Point2D, Pose2D};
21use std::f64::consts::PI;
22
23/// Configuration for the backstepping controller.
24#[derive(Debug, Clone, Copy)]
25pub struct BacksteppingConfig {
26    /// Position gain.
27    pub k1: f64,
28    /// Lateral error gain.
29    pub k2: f64,
30    /// Heading error gain.
31    pub k3: f64,
32    /// Integration time-step \[s\].
33    pub dt: f64,
34    /// Convergence tolerance for position \[m\] and yaw \[rad\].
35    pub goal_tolerance: f64,
36    /// Maximum number of simulation steps.
37    pub max_steps: usize,
38}
39
40impl Default for BacksteppingConfig {
41    fn default() -> Self {
42        Self {
43            k1: 3.0,
44            k2: 8.0,
45            k3: 3.0,
46            dt: 0.01,
47            goal_tolerance: 0.05,
48            max_steps: 10_000,
49        }
50    }
51}
52
53/// Configuration for the time-varying pose stabilizer.
54#[derive(Debug, Clone, Copy)]
55pub struct TimeVaryingBacksteppingConfig {
56    /// Base gains and integration settings shared with the hybrid controller.
57    pub base: BacksteppingConfig,
58    /// Initial orbit radius as a multiple of the start-goal distance.
59    pub orbit_radius_scale: f64,
60    /// Angular speed of the virtual goal orbit \[rad/s\].
61    pub orbit_frequency: f64,
62    /// Exponential decay rate for the virtual goal orbit \[1/s\].
63    pub orbit_decay_rate: f64,
64}
65
66impl Default for TimeVaryingBacksteppingConfig {
67    fn default() -> Self {
68        Self {
69            base: BacksteppingConfig::default(),
70            orbit_radius_scale: 0.75,
71            orbit_frequency: 2.0,
72            orbit_decay_rate: 0.8,
73        }
74    }
75}
76
77/// One recorded simulation step.
78#[derive(Debug, Clone, Copy)]
79pub struct BacksteppingStep {
80    pub pose: Pose2D,
81    pub control: ControlInput,
82    pub distance_to_goal: f64,
83}
84
85/// Full backstepping simulation result.
86#[derive(Debug, Clone)]
87pub struct BacksteppingResult {
88    pub steps: Vec<BacksteppingStep>,
89    pub converged: bool,
90}
91
92impl BacksteppingResult {
93    /// Returns the final pose in the recorded rollout.
94    pub fn final_pose(&self) -> Pose2D {
95        self.steps
96            .last()
97            .map(|step| step.pose)
98            .unwrap_or_else(Pose2D::origin)
99    }
100
101    /// Returns the number of applied control steps.
102    pub fn iterations(&self) -> usize {
103        self.steps.len().saturating_sub(1)
104    }
105
106    /// Returns the traced path of the robot center.
107    pub fn path(&self) -> Path2D {
108        Path2D::from_points(
109            self.steps
110                .iter()
111                .map(|step| Point2D::new(step.pose.x, step.pose.y))
112                .collect(),
113        )
114    }
115}
116
117/// Backstepping controller.
118pub struct BacksteppingController {
119    config: BacksteppingConfig,
120}
121
122impl BacksteppingController {
123    /// Creates a new controller from the provided configuration.
124    pub fn new(config: BacksteppingConfig) -> Self {
125        Self { config }
126    }
127
128    /// Returns the controller configuration.
129    pub fn config(&self) -> BacksteppingConfig {
130        self.config
131    }
132
133    /// Simulates the controller from `start` to `goal`.
134    pub fn simulate(&self, start: Pose2D, goal: Pose2D) -> BacksteppingResult {
135        let mut pose = start;
136        let mut steps = vec![BacksteppingStep {
137            pose,
138            control: ControlInput::zero(),
139            distance_to_goal: distance_to_goal(pose, goal),
140        }];
141
142        for _ in 0..self.config.max_steps {
143            if self.is_converged(pose, goal) {
144                return BacksteppingResult {
145                    steps,
146                    converged: true,
147                };
148            }
149
150            let control = self.compute_control(pose, goal);
151            pose = integrate_pose(pose, control, self.config.dt);
152            steps.push(BacksteppingStep {
153                pose,
154                control,
155                distance_to_goal: distance_to_goal(pose, goal),
156            });
157        }
158
159        BacksteppingResult {
160            steps,
161            converged: false,
162        }
163    }
164
165    /// Convenience wrapper returning only the traced `x`/`y` points.
166    pub fn planning(&self, start: (f64, f64, f64), goal: (f64, f64, f64)) -> Vec<(f64, f64)> {
167        self.simulate(
168            Pose2D::new(start.0, start.1, start.2),
169            Pose2D::new(goal.0, goal.1, goal.2),
170        )
171        .path()
172        .points
173        .into_iter()
174        .map(|point| (point.x, point.y))
175        .collect()
176    }
177
178    fn is_converged(&self, pose: Pose2D, goal: Pose2D) -> bool {
179        let position_ok = distance_to_goal(pose, goal) <= self.config.goal_tolerance;
180        let yaw_ok = normalize_angle(goal.yaw - pose.yaw).abs() <= self.config.goal_tolerance;
181        position_ok && yaw_ok
182    }
183
184    fn compute_control(&self, pose: Pose2D, goal: Pose2D) -> ControlInput {
185        let dx = goal.x - pose.x;
186        let dy = goal.y - pose.y;
187        let distance = (dx.powi(2) + dy.powi(2)).sqrt();
188
189        if distance <= self.terminal_region_radius() {
190            return self.compute_terminal_control(pose, goal, distance);
191        }
192
193        self.compute_tracking_control(pose, goal)
194    }
195
196    fn compute_tracking_control(&self, pose: Pose2D, goal: Pose2D) -> ControlInput {
197        let dx = goal.x - pose.x;
198        let dy = goal.y - pose.y;
199
200        let cos_theta = pose.yaw.cos();
201        let sin_theta = pose.yaw.sin();
202
203        let e1 = cos_theta * dx + sin_theta * dy;
204        let e2 = -sin_theta * dx + cos_theta * dy;
205        let e3 = normalize_angle(goal.yaw - pose.yaw);
206
207        let vr = self.config.k1 * (e1.powi(2) + e2.powi(2)).sqrt();
208        let v = vr * e3.cos() + self.config.k1 * e1;
209        let omega = vr * (self.config.k2 * e2 + self.config.k3 * e3.sin());
210
211        ControlInput::new(v, omega)
212    }
213
214    fn compute_terminal_control(&self, pose: Pose2D, goal: Pose2D, distance: f64) -> ControlInput {
215        let alpha = normalize_angle((goal.y - pose.y).atan2(goal.x - pose.x) - pose.yaw);
216        let beta = normalize_angle(goal.yaw - pose.yaw - alpha);
217
218        let mut v = self.config.k1 * distance;
219        if !(-PI / 2.0..=PI / 2.0).contains(&alpha) {
220            v = -v;
221        }
222
223        let omega = self.config.k2 * alpha - self.config.k3 * beta;
224        ControlInput::new(v, omega)
225    }
226
227    fn terminal_region_radius(&self) -> f64 {
228        10.0 * self.config.goal_tolerance
229    }
230}
231
232/// Time-varying pose stabilizer based on a shrinking virtual-goal orbit.
233///
234/// This controller keeps the target explicitly time-varying by asking the
235/// backstepping tracking law to follow a virtual goal that spirals into the
236/// desired pose. The construction is Samson-inspired, but remains an
237/// engineering approximation rather than a direct transcription of the chained
238/// coordinates feedback law.
239pub struct TimeVaryingBacksteppingController {
240    config: TimeVaryingBacksteppingConfig,
241    tracker: BacksteppingController,
242}
243
244impl TimeVaryingBacksteppingController {
245    /// Creates a new time-varying stabilizer.
246    pub fn new(config: TimeVaryingBacksteppingConfig) -> Self {
247        Self {
248            tracker: BacksteppingController::new(config.base),
249            config,
250        }
251    }
252
253    /// Simulates the controller from `start` to `goal`.
254    pub fn simulate(&self, start: Pose2D, goal: Pose2D) -> BacksteppingResult {
255        let mut pose = start;
256        let mut steps = vec![BacksteppingStep {
257            pose,
258            control: ControlInput::zero(),
259            distance_to_goal: distance_to_goal(pose, goal),
260        }];
261        let reference_radius = distance_to_goal(start, goal).max(self.config.base.goal_tolerance)
262            * self.config.orbit_radius_scale;
263        let initial_phase = (start.y - goal.y).atan2(start.x - goal.x);
264
265        for step_index in 0..self.config.base.max_steps {
266            if self.is_converged(pose, goal) {
267                return BacksteppingResult {
268                    steps,
269                    converged: true,
270                };
271            }
272
273            let time = step_index as f64 * self.config.base.dt;
274            let distance = distance_to_goal(pose, goal);
275            let control = if distance <= self.tracker.terminal_region_radius() {
276                self.tracker.compute_terminal_control(pose, goal, distance)
277            } else {
278                let virtual_goal = self.virtual_goal(goal, reference_radius, initial_phase, time);
279                self.tracker.compute_tracking_control(pose, virtual_goal)
280            };
281            pose = integrate_pose(pose, control, self.config.base.dt);
282            steps.push(BacksteppingStep {
283                pose,
284                control,
285                distance_to_goal: distance_to_goal(pose, goal),
286            });
287        }
288
289        BacksteppingResult {
290            steps,
291            converged: false,
292        }
293    }
294
295    fn is_converged(&self, pose: Pose2D, goal: Pose2D) -> bool {
296        let position_ok = distance_to_goal(pose, goal) <= self.config.base.goal_tolerance;
297        let yaw_ok = normalize_angle(goal.yaw - pose.yaw).abs() <= self.config.base.goal_tolerance;
298        position_ok && yaw_ok
299    }
300
301    fn virtual_goal(
302        &self,
303        goal: Pose2D,
304        reference_radius: f64,
305        initial_phase: f64,
306        time: f64,
307    ) -> Pose2D {
308        let amplitude = reference_radius * (-self.config.orbit_decay_rate * time).exp();
309        let phase = initial_phase + self.config.orbit_frequency * time;
310        Pose2D::new(
311            goal.x + amplitude * phase.cos(),
312            goal.y + amplitude * phase.sin(),
313            goal.yaw,
314        )
315    }
316}
317
318/// Runs the backstepping controller from `start` to `goal`.
319pub fn backstepping_control(
320    start: Pose2D,
321    goal: Pose2D,
322    config: BacksteppingConfig,
323) -> BacksteppingResult {
324    BacksteppingController::new(config).simulate(start, goal)
325}
326
327/// Runs the Samson-inspired time-varying backstepping controller.
328pub fn time_varying_backstepping_control(
329    start: Pose2D,
330    goal: Pose2D,
331    config: TimeVaryingBacksteppingConfig,
332) -> BacksteppingResult {
333    TimeVaryingBacksteppingController::new(config).simulate(start, goal)
334}
335
336pub use rust_robotics_core::normalize_angle;
337
338fn integrate_pose(pose: Pose2D, control: ControlInput, dt: f64) -> Pose2D {
339    let x = pose.x + control.v * pose.yaw.cos() * dt;
340    let y = pose.y + control.v * pose.yaw.sin() * dt;
341    let yaw = normalize_angle(pose.yaw + control.omega * dt);
342    Pose2D::new(x, y, yaw)
343}
344
345fn distance_to_goal(pose: Pose2D, goal: Pose2D) -> f64 {
346    ((goal.x - pose.x).powi(2) + (goal.y - pose.y).powi(2)).sqrt()
347}
348
349#[cfg(test)]
350mod tests {
351    use super::*;
352
353    #[test]
354    fn test_backstepping_config_defaults() {
355        let config = BacksteppingConfig::default();
356        assert_eq!(config.k1, 3.0);
357        assert_eq!(config.k2, 8.0);
358        assert_eq!(config.k3, 3.0);
359        assert_eq!(config.dt, 0.01);
360        assert_eq!(config.goal_tolerance, 0.05);
361        assert_eq!(config.max_steps, 10_000);
362    }
363
364    #[test]
365    fn test_backstepping_converges_to_goal() {
366        let start = Pose2D::origin();
367        let goal = Pose2D::new(2.0, 1.5, PI / 4.0);
368
369        let result = backstepping_control(start, goal, BacksteppingConfig::default());
370        let final_pose = result.final_pose();
371
372        assert!(result.converged);
373        assert!(distance_to_goal(final_pose, goal) < 0.05);
374        assert!(normalize_angle(final_pose.yaw - goal.yaw).abs() < 0.05);
375    }
376
377    #[test]
378    fn test_backstepping_returns_non_empty_path() {
379        let controller = BacksteppingController::new(BacksteppingConfig::default());
380        let result = controller.simulate(Pose2D::origin(), Pose2D::new(1.5, 0.5, 0.0));
381        let path = result.path();
382
383        assert!(path.points.len() > 1);
384        assert_eq!(path.points[0].x, 0.0);
385        assert_eq!(path.points[0].y, 0.0);
386    }
387
388    #[test]
389    fn test_time_varying_backstepping_config_defaults() {
390        let config = TimeVaryingBacksteppingConfig::default();
391        assert_eq!(config.base.goal_tolerance, 0.05);
392        assert_eq!(config.orbit_radius_scale, 0.75);
393        assert_eq!(config.orbit_frequency, 2.0);
394        assert_eq!(config.orbit_decay_rate, 0.8);
395    }
396
397    #[test]
398    fn test_time_varying_backstepping_converges_to_goal() {
399        let start = Pose2D::origin();
400        let goal = Pose2D::new(1.5, 1.0, PI / 2.0);
401        let controller = TimeVaryingBacksteppingController::new(TimeVaryingBacksteppingConfig {
402            base: BacksteppingConfig {
403                k1: 2.5,
404                k2: 6.0,
405                k3: 3.0,
406                dt: 0.01,
407                goal_tolerance: 0.05,
408                max_steps: 12_000,
409            },
410            orbit_radius_scale: 0.75,
411            orbit_frequency: 2.0,
412            orbit_decay_rate: 0.8,
413        });
414
415        let result = controller.simulate(start, goal);
416        let final_pose = result.final_pose();
417
418        assert!(result.converged);
419        assert!(distance_to_goal(final_pose, goal) < 0.05);
420        assert!(normalize_angle(final_pose.yaw - goal.yaw).abs() < 0.05);
421    }
422}