Skip to main content

rust_robotics_control/
stanley_controller.rs

1//! Stanley Controller path tracking algorithm
2//!
3//! A path tracking controller based on Stanley control law that uses
4//! heading error and cross-track error for steering computation.
5//!
6//! Ref:
7//!     - [Stanley: The robot that won the DARPA grand challenge](http://isl.ecst.csuchico.edu/DOCS/darpa2005/DARPA%202005%20Stanley.pdf)
8//!     - [Autonomous Automobile Path Tracking](https://www.ri.cmu.edu/pub_files/2009/2/Automatic_Steering_Methods_for_Autonomous_Automobile_Path_Tracking.pdf)
9
10use crate::spline_course::calc_spline_course;
11use alloc::vec::Vec;
12use core::f64::consts::PI;
13#[cfg(not(feature = "std"))]
14#[allow(unused_imports)]
15// f64 math via libm on no_std targets; on std hosts the inherent methods win
16use num_traits::Float;
17use rust_robotics_core::normalize_angle;
18use rust_robotics_core::{ControlInput, Path2D, PathTracker, Point2D, State2D};
19
20/// Vehicle state for Stanley Controller
21#[derive(Debug, Clone, Copy)]
22pub struct VehicleState {
23    pub x: f64,
24    pub y: f64,
25    pub yaw: f64,
26    pub v: f64,
27    pub wheelbase: f64,
28}
29
30impl VehicleState {
31    pub fn new(x: f64, y: f64, yaw: f64, v: f64, wheelbase: f64) -> Self {
32        VehicleState {
33            x,
34            y,
35            yaw,
36            v,
37            wheelbase,
38        }
39    }
40
41    pub fn update(&mut self, a: f64, delta: f64, dt: f64) {
42        self.x += self.v * self.yaw.cos() * dt;
43        self.y += self.v * self.yaw.sin() * dt;
44        self.yaw += self.v / self.wheelbase * delta.tan() * dt;
45        self.v += a * dt;
46    }
47
48    /// Get front axle position
49    pub fn front_axle(&self) -> (f64, f64) {
50        let fx = self.x + self.wheelbase * self.yaw.cos();
51        let fy = self.y + self.wheelbase * self.yaw.sin();
52        (fx, fy)
53    }
54
55    pub fn to_state2d(&self) -> State2D {
56        State2D::new(self.x, self.y, self.yaw, self.v)
57    }
58}
59
60impl From<State2D> for VehicleState {
61    fn from(s: State2D) -> Self {
62        VehicleState::new(s.x, s.y, s.yaw, s.v, 2.9) // default wheelbase
63    }
64}
65
66/// Largest steering angle the controller commands \[rad\] (about 84°).
67const MAX_STEER: f64 = 0.5 * PI - 0.1;
68
69/// Configuration for Stanley Controller
70#[derive(Debug, Clone)]
71pub struct StanleyConfig {
72    /// Cross-track error gain (k)
73    pub k: f64,
74    /// Vehicle wheelbase
75    pub wheelbase: f64,
76    /// Speed proportional gain
77    pub kp: f64,
78    /// Goal distance threshold
79    pub goal_threshold: f64,
80}
81
82impl Default for StanleyConfig {
83    fn default() -> Self {
84        Self {
85            k: 0.5,
86            wheelbase: 2.9,
87            kp: 1.0,
88            goal_threshold: 3.0,
89        }
90    }
91}
92
93/// Stanley path tracking controller
94pub struct StanleyController {
95    config: StanleyConfig,
96    path: Path2D,
97    path_yaw: Vec<f64>,
98    last_target_idx: usize,
99}
100
101impl StanleyController {
102    /// Create a new Stanley controller
103    pub fn new(config: StanleyConfig) -> Self {
104        StanleyController {
105            config,
106            path: Path2D::new(),
107            path_yaw: Vec::new(),
108            last_target_idx: 0,
109        }
110    }
111
112    /// Create with simplified parameters (legacy interface)
113    pub fn with_params(k: f64, wheelbase: f64) -> Self {
114        let config = StanleyConfig {
115            k,
116            wheelbase,
117            ..Default::default()
118        };
119        Self::new(config)
120    }
121
122    /// Set the reference path
123    pub fn set_path(&mut self, path: Path2D) {
124        self.path_yaw = self.compute_path_yaw(&path);
125        self.path = path;
126        self.last_target_idx = 0;
127    }
128
129    /// Set the reference path with pre-computed yaw angles
130    pub fn set_path_with_yaw(&mut self, path: Path2D, yaw: Vec<f64>) {
131        self.path = path;
132        self.path_yaw = yaw;
133        self.last_target_idx = 0;
134    }
135
136    /// Get the current reference path
137    pub fn get_path(&self) -> &Path2D {
138        &self.path
139    }
140
141    /// Compute yaw angles from path points
142    fn compute_path_yaw(&self, path: &Path2D) -> Vec<f64> {
143        path.yaw_profile()
144    }
145
146    /// Find target index and cross-track error
147    fn calc_target_index(&self, state: &VehicleState) -> (usize, f64) {
148        let (fx, fy) = state.front_axle();
149        let query = Point2D::new(fx, fy);
150        let min_idx = self
151            .path
152            .nearest_point_index_forward(query, self.last_target_idx)
153            .unwrap_or(0);
154
155        // Calculate cross-track error
156        let target_point = &self.path.points[min_idx];
157        let diff_x = fx - target_point.x;
158        let diff_y = fy - target_point.y;
159        let error_front_axle =
160            -(state.yaw + 0.5 * PI).cos() * diff_x - (state.yaw + 0.5 * PI).sin() * diff_y;
161
162        (min_idx, error_front_axle)
163    }
164
165    /// Compute steering angle using Stanley control law
166    pub fn compute_steering(&mut self, state: &VehicleState) -> f64 {
167        if self.path.is_empty() {
168            return 0.0;
169        }
170
171        let (mut target_idx, error_front_axle) = self.calc_target_index(state);
172
173        if self.last_target_idx >= target_idx {
174            target_idx = self.last_target_idx;
175        }
176        self.last_target_idx = target_idx;
177
178        // Heading error
179        let theta_e = normalize_angle(self.path_yaw[target_idx] - state.yaw);
180
181        // Cross-track error correction
182        let theta_d = (self.config.k * error_front_axle).atan2(state.v.max(0.1));
183
184        // Total steering angle, kept short of ±90°: beyond it tan(δ) (and so
185        // the turn rate) flips sign and the vehicle turns away from the path.
186        (theta_e + theta_d).clamp(-MAX_STEER, MAX_STEER)
187    }
188
189    /// Proportional speed control
190    pub fn compute_acceleration(&self, target_speed: f64, current_speed: f64) -> f64 {
191        self.config.kp * (target_speed - current_speed)
192    }
193
194    /// Check if goal is reached
195    pub fn is_goal_reached_vehicle(&self, state: &VehicleState) -> bool {
196        if let Some(goal) = self.path.points.last() {
197            let dx = state.x - goal.x;
198            let dy = state.y - goal.y;
199            (dx * dx + dy * dy).sqrt() < self.config.goal_threshold
200        } else {
201            true
202        }
203    }
204
205    /// Legacy planning interface
206    pub fn planning(
207        &mut self,
208        waypoints: Vec<(f64, f64)>,
209        target_speed: f64,
210        ds: f64,
211    ) -> Option<Vec<(f64, f64)>> {
212        if waypoints.len() < 2 {
213            return None;
214        }
215
216        // Generate spline path
217        let ax: Vec<f64> = waypoints.iter().map(|p| p.0).collect();
218        let ay: Vec<f64> = waypoints.iter().map(|p| p.1).collect();
219        let (cx, cy, cyaw, _, _) = calc_spline_course(&ax, &ay, ds);
220
221        // Set path
222        let path = Path2D::from_points(
223            cx.iter()
224                .zip(cy.iter())
225                .map(|(&x, &y)| Point2D::new(x, y))
226                .collect(),
227        );
228        self.set_path_with_yaw(path, cyaw);
229
230        // Initialize state
231        let init_yaw = 20.0_f64.to_radians();
232        let mut state = VehicleState::new(
233            waypoints[0].0,
234            waypoints[0].1 + 5.0,
235            init_yaw,
236            0.0,
237            self.config.wheelbase,
238        );
239
240        let mut trajectory = vec![(state.x, state.y)];
241        let dt = 0.1;
242        let t_max = 100.0;
243        let mut time = 0.0;
244
245        while time < t_max {
246            let ai = self.compute_acceleration(target_speed, state.v);
247            let di = self.compute_steering(&state);
248            state.update(ai, di, dt);
249            time += dt;
250
251            trajectory.push((state.x, state.y));
252
253            if self.is_goal_reached_vehicle(&state) {
254                break;
255            }
256        }
257
258        Some(trajectory)
259    }
260}
261
262impl PathTracker for StanleyController {
263    fn compute_control(&mut self, current_state: &State2D, path: &Path2D) -> ControlInput {
264        // Adopt a new reference path (comparing contents, not just length)
265        if self.path != *path {
266            self.set_path(path.clone());
267        }
268
269        let vehicle_state = VehicleState::new(
270            current_state.x,
271            current_state.y,
272            current_state.yaw,
273            current_state.v,
274            self.config.wheelbase,
275        );
276
277        let delta = self.compute_steering(&vehicle_state);
278
279        // Compute speed control (assume constant target speed)
280        let target_speed = 5.0; // m/s
281        let v = current_state.v + self.compute_acceleration(target_speed, current_state.v) * 0.1;
282
283        // Convert steering angle to angular velocity
284        let omega = v * delta.tan() / self.config.wheelbase;
285
286        ControlInput::new(v, omega)
287    }
288
289    fn is_goal_reached(&self, current_state: &State2D, goal: Point2D) -> bool {
290        let dx = current_state.x - goal.x;
291        let dy = current_state.y - goal.y;
292        (dx * dx + dy * dy).sqrt() < self.config.goal_threshold
293    }
294}
295
296#[cfg(test)]
297mod tests {
298    use super::*;
299
300    #[test]
301    fn a_large_heading_error_still_steers_back_toward_the_path() {
302        // Path along +x; the vehicle points almost straight up or down.
303        // Past ±90° of steering tan() flipped sign and it turned away.
304        let path = Path2D::from_points((0..=40).map(|i| Point2D::new(i as f64, 0.0)).collect());
305        for (yaw, expected_sign) in [(1.6, -1.0), (1.8, -1.0), (-1.6, 1.0), (-1.8, 1.0)] {
306            let mut controller = StanleyController::new(StanleyConfig::default());
307            let command = controller.compute_control(&State2D::new(5.0, 0.0, yaw, 2.0), &path);
308            assert!(
309                command.omega * expected_sign > 0.0,
310                "yaw {yaw}: omega {}",
311                command.omega
312            );
313        }
314    }
315
316    #[test]
317    fn test_stanley_creation() {
318        let config = StanleyConfig::default();
319        let controller = StanleyController::new(config);
320        assert!(controller.path.is_empty());
321    }
322
323    #[test]
324    fn test_stanley_set_path() {
325        let mut controller = StanleyController::with_params(0.5, 2.9);
326        let path = Path2D::from_points(vec![
327            Point2D::new(0.0, 0.0),
328            Point2D::new(1.0, 0.0),
329            Point2D::new(2.0, 0.0),
330        ]);
331        controller.set_path(path);
332        assert_eq!(controller.get_path().len(), 3);
333        assert_eq!(controller.path_yaw.len(), 3);
334    }
335
336    #[test]
337    fn test_stanley_normalize_angle() {
338        assert!((normalize_angle(3.0 * PI) - PI).abs() < 0.01);
339        assert!((normalize_angle(-3.0 * PI) + PI).abs() < 0.01);
340    }
341
342    #[test]
343    fn test_stanley_planning() {
344        let mut controller = StanleyController::with_params(0.5, 2.9);
345        let waypoints = vec![(0.0, 0.0), (50.0, 0.0), (100.0, 0.0)];
346
347        let result = controller.planning(waypoints, 5.0, 0.5);
348        assert!(result.is_some());
349        assert!(!result.unwrap().is_empty());
350    }
351}