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
336/// Normalizes an angle into `[-pi, pi]`.
337pub fn normalize_angle(mut angle: f64) -> f64 {
338    while angle > PI {
339        angle -= 2.0 * PI;
340    }
341    while angle < -PI {
342        angle += 2.0 * PI;
343    }
344    angle
345}
346
347fn integrate_pose(pose: Pose2D, control: ControlInput, dt: f64) -> Pose2D {
348    let x = pose.x + control.v * pose.yaw.cos() * dt;
349    let y = pose.y + control.v * pose.yaw.sin() * dt;
350    let yaw = normalize_angle(pose.yaw + control.omega * dt);
351    Pose2D::new(x, y, yaw)
352}
353
354fn distance_to_goal(pose: Pose2D, goal: Pose2D) -> f64 {
355    ((goal.x - pose.x).powi(2) + (goal.y - pose.y).powi(2)).sqrt()
356}
357
358#[cfg(test)]
359mod tests {
360    use super::*;
361
362    #[test]
363    fn test_backstepping_config_defaults() {
364        let config = BacksteppingConfig::default();
365        assert_eq!(config.k1, 3.0);
366        assert_eq!(config.k2, 8.0);
367        assert_eq!(config.k3, 3.0);
368        assert_eq!(config.dt, 0.01);
369        assert_eq!(config.goal_tolerance, 0.05);
370        assert_eq!(config.max_steps, 10_000);
371    }
372
373    #[test]
374    fn test_backstepping_converges_to_goal() {
375        let start = Pose2D::origin();
376        let goal = Pose2D::new(2.0, 1.5, PI / 4.0);
377
378        let result = backstepping_control(start, goal, BacksteppingConfig::default());
379        let final_pose = result.final_pose();
380
381        assert!(result.converged);
382        assert!(distance_to_goal(final_pose, goal) < 0.05);
383        assert!(normalize_angle(final_pose.yaw - goal.yaw).abs() < 0.05);
384    }
385
386    #[test]
387    fn test_backstepping_returns_non_empty_path() {
388        let controller = BacksteppingController::new(BacksteppingConfig::default());
389        let result = controller.simulate(Pose2D::origin(), Pose2D::new(1.5, 0.5, 0.0));
390        let path = result.path();
391
392        assert!(path.points.len() > 1);
393        assert_eq!(path.points[0].x, 0.0);
394        assert_eq!(path.points[0].y, 0.0);
395    }
396
397    #[test]
398    fn test_time_varying_backstepping_config_defaults() {
399        let config = TimeVaryingBacksteppingConfig::default();
400        assert_eq!(config.base.goal_tolerance, 0.05);
401        assert_eq!(config.orbit_radius_scale, 0.75);
402        assert_eq!(config.orbit_frequency, 2.0);
403        assert_eq!(config.orbit_decay_rate, 0.8);
404    }
405
406    #[test]
407    fn test_time_varying_backstepping_converges_to_goal() {
408        let start = Pose2D::origin();
409        let goal = Pose2D::new(1.5, 1.0, PI / 2.0);
410        let controller = TimeVaryingBacksteppingController::new(TimeVaryingBacksteppingConfig {
411            base: BacksteppingConfig {
412                k1: 2.5,
413                k2: 6.0,
414                k3: 3.0,
415                dt: 0.01,
416                goal_tolerance: 0.05,
417                max_steps: 12_000,
418            },
419            orbit_radius_scale: 0.75,
420            orbit_frequency: 2.0,
421            orbit_decay_rate: 0.8,
422        });
423
424        let result = controller.simulate(start, goal);
425        let final_pose = result.final_pose();
426
427        assert!(result.converged);
428        assert!(distance_to_goal(final_pose, goal) < 0.05);
429        assert!(normalize_angle(final_pose.yaw - goal.yaw).abs() < 0.05);
430    }
431}
rust_robotics_control/backstepping_control.rs

rust_robotics_control/
backstepping_control.rs
