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 rust_robotics_core::{ControlInput, Path2D, PathTracker, Point2D, State2D};
11use std::f64::consts::PI;
12
13/// Vehicle state for Stanley Controller
14#[derive(Debug, Clone, Copy)]
15pub struct VehicleState {
16    pub x: f64,
17    pub y: f64,
18    pub yaw: f64,
19    pub v: f64,
20    pub wheelbase: f64,
21}
22
23impl VehicleState {
24    pub fn new(x: f64, y: f64, yaw: f64, v: f64, wheelbase: f64) -> Self {
25        VehicleState {
26            x,
27            y,
28            yaw,
29            v,
30            wheelbase,
31        }
32    }
33
34    pub fn update(&mut self, a: f64, delta: f64, dt: f64) {
35        self.x += self.v * self.yaw.cos() * dt;
36        self.y += self.v * self.yaw.sin() * dt;
37        self.yaw += self.v / self.wheelbase * delta.tan() * dt;
38        self.v += a * dt;
39    }
40
41    /// Get front axle position
42    pub fn front_axle(&self) -> (f64, f64) {
43        let fx = self.x + self.wheelbase * self.yaw.cos();
44        let fy = self.y + self.wheelbase * self.yaw.sin();
45        (fx, fy)
46    }
47
48    pub fn to_state2d(&self) -> State2D {
49        State2D::new(self.x, self.y, self.yaw, self.v)
50    }
51}
52
53impl From<State2D> for VehicleState {
54    fn from(s: State2D) -> Self {
55        VehicleState::new(s.x, s.y, s.yaw, s.v, 2.9) // default wheelbase
56    }
57}
58
59/// Configuration for Stanley Controller
60#[derive(Debug, Clone)]
61pub struct StanleyConfig {
62    /// Cross-track error gain (k)
63    pub k: f64,
64    /// Vehicle wheelbase
65    pub wheelbase: f64,
66    /// Speed proportional gain
67    pub kp: f64,
68    /// Goal distance threshold
69    pub goal_threshold: f64,
70}
71
72impl Default for StanleyConfig {
73    fn default() -> Self {
74        Self {
75            k: 0.5,
76            wheelbase: 2.9,
77            kp: 1.0,
78            goal_threshold: 3.0,
79        }
80    }
81}
82
83/// Stanley path tracking controller
84pub struct StanleyController {
85    config: StanleyConfig,
86    path: Path2D,
87    path_yaw: Vec<f64>,
88    last_target_idx: usize,
89}
90
91impl StanleyController {
92    /// Create a new Stanley controller
93    pub fn new(config: StanleyConfig) -> Self {
94        StanleyController {
95            config,
96            path: Path2D::new(),
97            path_yaw: Vec::new(),
98            last_target_idx: 0,
99        }
100    }
101
102    /// Create with simplified parameters (legacy interface)
103    pub fn with_params(k: f64, wheelbase: f64) -> Self {
104        let config = StanleyConfig {
105            k,
106            wheelbase,
107            ..Default::default()
108        };
109        Self::new(config)
110    }
111
112    /// Set the reference path
113    pub fn set_path(&mut self, path: Path2D) {
114        self.path_yaw = self.compute_path_yaw(&path);
115        self.path = path;
116        self.last_target_idx = 0;
117    }
118
119    /// Set the reference path with pre-computed yaw angles
120    pub fn set_path_with_yaw(&mut self, path: Path2D, yaw: Vec<f64>) {
121        self.path = path;
122        self.path_yaw = yaw;
123        self.last_target_idx = 0;
124    }
125
126    /// Get the current reference path
127    pub fn get_path(&self) -> &Path2D {
128        &self.path
129    }
130
131    /// Compute yaw angles from path points
132    fn compute_path_yaw(&self, path: &Path2D) -> Vec<f64> {
133        path.yaw_profile()
134    }
135
136    /// Normalize angle to [-PI, PI]
137    fn normalize_angle(mut angle: f64) -> f64 {
138        while angle > PI {
139            angle -= 2.0 * PI;
140        }
141        while angle < -PI {
142            angle += 2.0 * PI;
143        }
144        angle
145    }
146
147    /// Find target index and cross-track error
148    fn calc_target_index(&self, state: &VehicleState) -> (usize, f64) {
149        let (fx, fy) = state.front_axle();
150        let query = Point2D::new(fx, fy);
151        let min_idx = self
152            .path
153            .nearest_point_index_from(query, self.last_target_idx)
154            .unwrap_or(0);
155
156        // Calculate cross-track error
157        let target_point = &self.path.points[min_idx];
158        let diff_x = fx - target_point.x;
159        let diff_y = fy - target_point.y;
160        let error_front_axle =
161            -(state.yaw + 0.5 * PI).cos() * diff_x - (state.yaw + 0.5 * PI).sin() * diff_y;
162
163        (min_idx, error_front_axle)
164    }
165
166    /// Compute steering angle using Stanley control law
167    pub fn compute_steering(&mut self, state: &VehicleState) -> f64 {
168        if self.path.is_empty() {
169            return 0.0;
170        }
171
172        let (mut target_idx, error_front_axle) = self.calc_target_index(state);
173
174        if self.last_target_idx >= target_idx {
175            target_idx = self.last_target_idx;
176        }
177        self.last_target_idx = target_idx;
178
179        // Heading error
180        let theta_e = Self::normalize_angle(self.path_yaw[target_idx] - state.yaw);
181
182        // Cross-track error correction
183        let theta_d = (self.config.k * error_front_axle).atan2(state.v.max(0.1));
184
185        // Total steering angle
186        theta_e + theta_d
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        // Set path if different
265        if self.path.len() != path.len() {
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// Cubic spline helper functions for legacy interface
297
298fn calc_spline_course(x: &[f64], y: &[f64], ds: f64) -> SplineCourse {
299    let sp = CubicSpline2D::new(x, y);
300    let mut s = 0.0;
301    let mut course_x = Vec::new();
302    let mut course_y = Vec::new();
303    let mut course_yaw = Vec::new();
304    let mut course_k = Vec::new();
305    let mut course_s = Vec::new();
306
307    // sp.s is always non-empty: CubicSpline2D::new initializes s with at least one element
308    let s_max = *sp
309        .s
310        .last()
311        .expect("spline s is non-empty after construction")
312        - ds;
313    while s < s_max {
314        let (ix, iy) = sp.calc_position(s);
315        let iyaw = sp.calc_yaw(s);
316        let ik = sp.calc_curvature(s);
317        course_x.push(ix);
318        course_y.push(iy);
319        course_yaw.push(iyaw);
320        course_k.push(ik);
321        course_s.push(s);
322        s += ds;
323    }
324
325    (course_x, course_y, course_yaw, course_k, course_s)
326}
327
328struct CubicSpline {
329    a: Vec<f64>,
330    b: Vec<f64>,
331    c: Vec<f64>,
332    d: Vec<f64>,
333    x: Vec<f64>,
334}
335
336type SplineCourse = (Vec<f64>, Vec<f64>, Vec<f64>, Vec<f64>, Vec<f64>);
337
338impl CubicSpline {
339    fn new(x: &[f64], y: &[f64]) -> Self {
340        let n = x.len();
341        let mut h = vec![0.0; n - 1];
342        for i in 0..n - 1 {
343            h[i] = x[i + 1] - x[i];
344        }
345
346        let mut a = vec![0.0; n];
347        let mut b = vec![0.0; n];
348        let mut c = vec![0.0; n];
349        let mut d = vec![0.0; n];
350
351        a[..n].copy_from_slice(&y[..n]);
352
353        let mut alpha = vec![0.0; n - 1];
354        for i in 1..n - 1 {
355            alpha[i] = 3.0 * (a[i + 1] - a[i]) / h[i] - 3.0 * (a[i] - a[i - 1]) / h[i - 1];
356        }
357
358        let mut l = vec![1.0; n];
359        let mut mu = vec![0.0; n];
360        let mut z = vec![0.0; n];
361
362        for i in 1..n - 1 {
363            l[i] = 2.0 * (x[i + 1] - x[i - 1]) - h[i - 1] * mu[i - 1];
364            mu[i] = h[i] / l[i];
365            z[i] = (alpha[i] - h[i - 1] * z[i - 1]) / l[i];
366        }
367
368        for j in (0..n - 1).rev() {
369            c[j] = z[j] - mu[j] * c[j + 1];
370            b[j] = (a[j + 1] - a[j]) / h[j] - h[j] * (c[j + 1] + 2.0 * c[j]) / 3.0;
371            d[j] = (c[j + 1] - c[j]) / (3.0 * h[j]);
372        }
373
374        CubicSpline {
375            a,
376            b,
377            c,
378            d,
379            x: x.to_vec(),
380        }
381    }
382
383    fn calc(&self, t: f64) -> f64 {
384        if t < self.x[0] {
385            return self.a[0];
386        } else if t > self.x[self.x.len() - 1] {
387            return self.a[self.a.len() - 1];
388        }
389
390        let mut i = self.search_index(t);
391        if i >= self.x.len() - 1 {
392            i = self.x.len() - 2;
393        }
394
395        let dx = t - self.x[i];
396        self.a[i] + self.b[i] * dx + self.c[i] * dx * dx + self.d[i] * dx * dx * dx
397    }
398
399    fn calc_d(&self, t: f64) -> f64 {
400        if t < self.x[0] {
401            return self.b[0];
402        } else if t > self.x[self.x.len() - 1] {
403            return self.b[self.b.len() - 1];
404        }
405
406        let mut i = self.search_index(t);
407        if i >= self.x.len() - 1 {
408            i = self.x.len() - 2;
409        }
410
411        let dx = t - self.x[i];
412        self.b[i] + 2.0 * self.c[i] * dx + 3.0 * self.d[i] * dx * dx
413    }
414
415    fn calc_dd(&self, t: f64) -> f64 {
416        if t < self.x[0] {
417            return 2.0 * self.c[0];
418        } else if t > self.x[self.x.len() - 1] {
419            return 2.0 * self.c[self.c.len() - 1];
420        }
421
422        let mut i = self.search_index(t);
423        if i >= self.x.len() - 1 {
424            i = self.x.len() - 2;
425        }
426
427        let dx = t - self.x[i];
428        2.0 * self.c[i] + 6.0 * self.d[i] * dx
429    }
430
431    fn search_index(&self, x: f64) -> usize {
432        for i in 0..self.x.len() - 1 {
433            if self.x[i] <= x && x <= self.x[i + 1] {
434                return i;
435            }
436        }
437        self.x.len() - 2
438    }
439}
440
441struct CubicSpline2D {
442    s: Vec<f64>,
443    sx: CubicSpline,
444    sy: CubicSpline,
445}
446
447impl CubicSpline2D {
448    fn new(x: &[f64], y: &[f64]) -> Self {
449        let mut s = vec![0.0];
450        for i in 1..x.len() {
451            let dx = x[i] - x[i - 1];
452            let dy = y[i] - y[i - 1];
453            s.push(s[i - 1] + (dx * dx + dy * dy).sqrt());
454        }
455
456        let sx = CubicSpline::new(&s, x);
457        let sy = CubicSpline::new(&s, y);
458
459        CubicSpline2D { s, sx, sy }
460    }
461
462    fn calc_position(&self, s: f64) -> (f64, f64) {
463        let x = self.sx.calc(s);
464        let y = self.sy.calc(s);
465        (x, y)
466    }
467
468    fn calc_curvature(&self, s: f64) -> f64 {
469        let dx = self.sx.calc_d(s);
470        let ddx = self.sx.calc_dd(s);
471        let dy = self.sy.calc_d(s);
472        let ddy = self.sy.calc_dd(s);
473        (ddy * dx - ddx * dy) / (dx * dx + dy * dy).powf(1.5)
474    }
475
476    fn calc_yaw(&self, s: f64) -> f64 {
477        let dx = self.sx.calc_d(s);
478        let dy = self.sy.calc_d(s);
479        dy.atan2(dx)
480    }
481}
482
483#[cfg(test)]
484mod tests {
485    use super::*;
486
487    #[test]
488    fn test_stanley_creation() {
489        let config = StanleyConfig::default();
490        let controller = StanleyController::new(config);
491        assert!(controller.path.is_empty());
492    }
493
494    #[test]
495    fn test_stanley_set_path() {
496        let mut controller = StanleyController::with_params(0.5, 2.9);
497        let path = Path2D::from_points(vec![
498            Point2D::new(0.0, 0.0),
499            Point2D::new(1.0, 0.0),
500            Point2D::new(2.0, 0.0),
501        ]);
502        controller.set_path(path);
503        assert_eq!(controller.get_path().len(), 3);
504        assert_eq!(controller.path_yaw.len(), 3);
505    }
506
507    #[test]
508    fn test_stanley_normalize_angle() {
509        assert!((StanleyController::normalize_angle(3.0 * PI) - PI).abs() < 0.01);
510        assert!((StanleyController::normalize_angle(-3.0 * PI) + PI).abs() < 0.01);
511    }
512
513    #[test]
514    fn test_stanley_planning() {
515        let mut controller = StanleyController::with_params(0.5, 2.9);
516        let waypoints = vec![(0.0, 0.0), (50.0, 0.0), (100.0, 0.0)];
517
518        let result = controller.planning(waypoints, 5.0, 0.5);
519        assert!(result.is_some());
520        assert!(!result.unwrap().is_empty());
521    }
522}
rust_robotics_control/stanley_controller.rs

rust_robotics_control/
stanley_controller.rs
