rust_robotics_control/
sliding_mode_control.rs

1//
2// Sliding Mode Control for unicycle mobile robot
3// Forces the system state onto a sliding surface s=0,
4// providing robust control against disturbances.
5use rust_robotics_core::{ControlInput, Path2D, Point2D, Pose2D};
6use std::f64::consts::PI;
7
8#[derive(Debug, Clone, Copy)]
9pub struct SlidingModeConfig {
10    /// Sliding surface slope weight on heading error.
11    pub lambda: f64,
12    /// Reaching gain (control aggressiveness).
13    pub eta: f64,
14    /// Boundary layer thickness for chattering reduction.
15    pub phi: f64,
16    pub dt: f64,
17    pub goal_tolerance: f64,
18    pub max_steps: usize,
19}
20
21impl Default for SlidingModeConfig {
22    fn default() -> Self {
23        Self {
24            lambda: 1.0,
25            eta: 0.5,
26            phi: 0.1,
27            dt: 0.01,
28            goal_tolerance: 0.05,
29            max_steps: 10_000,
30        }
31    }
32}
33
34#[derive(Debug, Clone, Copy)]
35pub struct SlidingModeStep {
36    pub pose: Pose2D,
37    pub control: ControlInput,
38    pub distance_to_goal: f64,
39}
40
41#[derive(Debug, Clone)]
42pub struct SlidingModeResult {
43    pub steps: Vec<SlidingModeStep>,
44    pub converged: bool,
45}
46
47impl SlidingModeResult {
48    pub fn final_pose(&self) -> Pose2D {
49        self.steps
50            .last()
51            .map(|s| s.pose)
52            .unwrap_or_else(Pose2D::origin)
53    }
54
55    pub fn iterations(&self) -> usize {
56        self.steps.len().saturating_sub(1)
57    }
58
59    pub fn path(&self) -> Path2D {
60        Path2D::from_points(
61            self.steps
62                .iter()
63                .map(|s| Point2D::new(s.pose.x, s.pose.y))
64                .collect(),
65        )
66    }
67}
68
69pub struct SlidingModeController {
70    config: SlidingModeConfig,
71}
72
73impl SlidingModeController {
74    pub fn new(config: SlidingModeConfig) -> Self {
75        Self { config }
76    }
77
78    pub fn config(&self) -> SlidingModeConfig {
79        self.config
80    }
81
82    pub fn simulate(&self, start: Pose2D, goal: Pose2D) -> SlidingModeResult {
83        let mut pose = start;
84        let mut steps = vec![SlidingModeStep {
85            pose,
86            control: ControlInput::zero(),
87            distance_to_goal: dist(pose, goal),
88        }];
89
90        for _ in 0..self.config.max_steps {
91            if dist(pose, goal) <= self.config.goal_tolerance {
92                return SlidingModeResult {
93                    steps,
94                    converged: true,
95                };
96            }
97
98            let control = self.compute_control(pose, goal);
99            pose = integrate_pose(pose, control, self.config.dt);
100            steps.push(SlidingModeStep {
101                pose,
102                control,
103                distance_to_goal: dist(pose, goal),
104            });
105        }
106
107        SlidingModeResult {
108            steps,
109            converged: false,
110        }
111    }
112
113    fn compute_control(&self, pose: Pose2D, goal: Pose2D) -> ControlInput {
114        let x_diff = goal.x - pose.x;
115        let y_diff = goal.y - pose.y;
116        let rho = (x_diff.powi(2) + y_diff.powi(2)).sqrt();
117        let alpha = normalize_angle(y_diff.atan2(x_diff) - pose.yaw);
118
119        // Sliding surface: s = alpha + lambda * rho_dot_desired
120        // Use alpha as the primary surface; lambda couples the distance error.
121        // s = alpha  (heading error); we want s -> 0
122        let s = alpha;
123
124        // Forward velocity: proportional to distance, sign corrected so robot
125        // always drives towards the goal (back up if facing away by > 90 deg).
126        let v = if alpha.abs() <= PI / 2.0 {
127            self.config.eta * rho
128        } else {
129            -self.config.eta * rho
130        };
131
132        // Angular velocity: sliding mode reaching law u = -eta * sat(s / phi)
133        // plus a proportional term lambda*rho to pull rho down while on surface.
134        let omega =
135            -self.config.eta * sat(s / self.config.phi) - self.config.lambda * rho * alpha.signum();
136
137        ControlInput::new(v, omega)
138    }
139}
140
141pub fn sliding_mode_control(
142    start: Pose2D,
143    goal: Pose2D,
144    config: SlidingModeConfig,
145) -> SlidingModeResult {
146    SlidingModeController::new(config).simulate(start, goal)
147}
148
149/// Smooth saturation function (boundary layer approximation of sign).
150fn sat(x: f64) -> f64 {
151    x.clamp(-1.0, 1.0)
152}
153
154pub fn normalize_angle(mut angle: f64) -> f64 {
155    while angle > PI {
156        angle -= 2.0 * PI;
157    }
158    while angle < -PI {
159        angle += 2.0 * PI;
160    }
161    angle
162}
163
164fn integrate_pose(pose: Pose2D, control: ControlInput, dt: f64) -> Pose2D {
165    let yaw = normalize_angle(pose.yaw + control.omega * dt);
166    Pose2D::new(
167        pose.x + control.v * yaw.cos() * dt,
168        pose.y + control.v * yaw.sin() * dt,
169        yaw,
170    )
171}
172
173fn dist(pose: Pose2D, goal: Pose2D) -> f64 {
174    ((goal.x - pose.x).powi(2) + (goal.y - pose.y).powi(2)).sqrt()
175}
176
177#[cfg(test)]
178mod tests {
179    use super::*;
180
181    #[test]
182    fn test_sliding_mode_config_defaults() {
183        let cfg = SlidingModeConfig::default();
184        assert_eq!(cfg.lambda, 1.0);
185        assert_eq!(cfg.eta, 0.5);
186        assert_eq!(cfg.phi, 0.1);
187        assert_eq!(cfg.dt, 0.01);
188        assert_eq!(cfg.goal_tolerance, 0.05);
189        assert_eq!(cfg.max_steps, 10_000);
190    }
191
192    #[test]
193    fn test_sliding_mode_converges() {
194        let start = Pose2D::origin();
195        let goal = Pose2D::new(3.0, 3.0, 0.0);
196
197        let result = sliding_mode_control(start, goal, SlidingModeConfig::default());
198
199        println!(
200            "converged: {}, steps: {}, final: ({:.3}, {:.3})",
201            result.converged,
202            result.iterations(),
203            result.final_pose().x,
204            result.final_pose().y,
205        );
206
207        assert!(result.converged);
208        assert!(dist(result.final_pose(), goal) <= SlidingModeConfig::default().goal_tolerance);
209    }
210
211    #[test]
212    fn test_sliding_mode_returns_non_empty_path() {
213        let controller = SlidingModeController::new(SlidingModeConfig::default());
214        let result = controller.simulate(Pose2D::origin(), Pose2D::new(2.0, 1.0, 0.0));
215        let path = result.path();
216
217        assert!(path.points.len() > 1);
218        assert_eq!(path.points[0].x, 0.0);
219        assert_eq!(path.points[0].y, 0.0);
220    }
221}
rust_robotics_control/sliding_mode_control.rs

rust_robotics_control/
sliding_mode_control.rs
