Skip to main content

rust_robotics_control/
lqr_steer_control.rs

1//! LQR Steer Control path tracking algorithm
2//!
3//! A path tracking controller using Linear Quadratic Regulator (LQR)
4//! for steering control combined with PID speed control.
5//!
6//! author: Atsushi Sakai (@Atsushi_twi)
7//!         Ryohei Sasaki (@rsasaki0109)
8
9use crate::spline_course::calc_spline_course;
10use alloc::vec::Vec;
11use core::f64::consts::PI;
12use nalgebra::{Matrix1, Matrix1x4, Matrix4, Vector4};
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 LQR controller
21#[derive(Debug, Clone, Copy)]
22pub struct LQRVehicleState {
23    pub x: f64,
24    pub y: f64,
25    pub yaw: f64,
26    pub v: f64,
27    pub wheelbase: f64,
28    pub max_steer: f64,
29}
30
31impl LQRVehicleState {
32    pub fn new(x: f64, y: f64, yaw: f64, v: f64, wheelbase: f64, max_steer: f64) -> Self {
33        LQRVehicleState {
34            x,
35            y,
36            yaw,
37            v,
38            wheelbase,
39            max_steer,
40        }
41    }
42
43    pub fn update(&mut self, a: f64, mut delta: f64, dt: f64) {
44        delta = delta.clamp(-self.max_steer, self.max_steer);
45        self.x += self.v * self.yaw.cos() * dt;
46        self.y += self.v * self.yaw.sin() * dt;
47        self.yaw += self.v / self.wheelbase * delta.tan() * dt;
48        self.v += a * dt;
49    }
50
51    pub fn to_state2d(&self) -> State2D {
52        State2D::new(self.x, self.y, self.yaw, self.v)
53    }
54}
55
56impl From<State2D> for LQRVehicleState {
57    fn from(s: State2D) -> Self {
58        LQRVehicleState::new(s.x, s.y, s.yaw, s.v, 0.5, 45.0_f64.to_radians())
59    }
60}
61
62/// Configuration for LQR Steer Controller
63#[derive(Debug, Clone)]
64pub struct LQRSteerConfig {
65    /// Vehicle wheelbase
66    pub wheelbase: f64,
67    /// Maximum steering angle \[rad\]
68    pub max_steer: f64,
69    /// Speed proportional gain
70    pub kp: f64,
71    /// State cost matrix Q (4x4 diagonal)
72    pub q_diag: [f64; 4],
73    /// Control cost R
74    pub r: f64,
75    /// Time step
76    pub dt: f64,
77    /// Goal distance threshold
78    pub goal_threshold: f64,
79}
80
81impl Default for LQRSteerConfig {
82    fn default() -> Self {
83        Self {
84            wheelbase: 0.5,
85            max_steer: 45.0_f64.to_radians(),
86            kp: 1.0,
87            q_diag: [1.0, 1.0, 1.0, 1.0],
88            r: 1.0,
89            dt: 0.1,
90            goal_threshold: 0.3,
91        }
92    }
93}
94
95/// LQR Steer path tracking controller
96pub struct LQRSteerController {
97    config: LQRSteerConfig,
98    path: Path2D,
99    path_yaw: Vec<f64>,
100    path_curvature: Vec<f64>,
101    speed_profile: Vec<f64>,
102    prev_error: f64,
103    prev_theta_error: f64,
104    /// Nearest path index found last step; the search continues from it.
105    nearest_index: Option<usize>,
106}
107
108impl LQRSteerController {
109    /// Create a new LQR Steer controller
110    pub fn new(config: LQRSteerConfig) -> Self {
111        LQRSteerController {
112            config,
113            path: Path2D::new(),
114            path_yaw: Vec::new(),
115            path_curvature: Vec::new(),
116            speed_profile: Vec::new(),
117            prev_error: 0.0,
118            prev_theta_error: 0.0,
119            nearest_index: None,
120        }
121    }
122
123    /// Create with default configuration
124    pub fn with_defaults() -> Self {
125        Self::new(LQRSteerConfig::default())
126    }
127
128    /// Set the reference path
129    pub fn set_path(&mut self, path: Path2D) {
130        let (yaw, curvature) = self.compute_path_derivatives(&path);
131        self.path_yaw = yaw;
132        self.path_curvature = curvature;
133        self.speed_profile = self.calc_speed_profile(&path, 2.78); // default ~10 km/h
134        self.path = path;
135        self.prev_error = 0.0;
136        self.prev_theta_error = 0.0;
137        self.nearest_index = None;
138    }
139
140    /// Set the reference path with speed
141    pub fn set_path_with_speed(&mut self, path: Path2D, target_speed: f64) {
142        let (yaw, curvature) = self.compute_path_derivatives(&path);
143        self.path_yaw = yaw;
144        self.path_curvature = curvature;
145        self.speed_profile = self.calc_speed_profile(&path, target_speed);
146        self.path = path;
147        self.prev_error = 0.0;
148        self.prev_theta_error = 0.0;
149        self.nearest_index = None;
150    }
151
152    /// Get the current reference path
153    pub fn get_path(&self) -> &Path2D {
154        &self.path
155    }
156
157    /// Compute yaw and curvature from path points
158    fn compute_path_derivatives(&self, path: &Path2D) -> (Vec<f64>, Vec<f64>) {
159        let n = path.len();
160        let yaw = path.yaw_profile();
161        let mut curvature = Vec::with_capacity(n);
162
163        for i in 0..n {
164            // Simple curvature approximation
165            if i > 0 && i < n - 1 {
166                let dyaw = normalize_angle(yaw[i] - yaw[i - 1]);
167                let dx = path.points[i].x - path.points[i - 1].x;
168                let dy = path.points[i].y - path.points[i - 1].y;
169                let ds = (dx * dx + dy * dy).sqrt();
170                curvature.push(if ds > 0.001 { dyaw / ds } else { 0.0 });
171            } else {
172                curvature.push(0.0);
173            }
174        }
175
176        (yaw, curvature)
177    }
178
179    /// Calculate speed profile based on path curvature
180    fn calc_speed_profile(&self, path: &Path2D, target_speed: f64) -> Vec<f64> {
181        let n = path.len();
182        let mut profile = Vec::with_capacity(n);
183        let mut direction = 1.0;
184
185        for i in 0..n {
186            if i < n - 1 && i < self.path_yaw.len() - 1 {
187                let dyaw = (self.path_yaw[i + 1] - self.path_yaw[i]).abs();
188                if (PI / 4.0..PI / 2.0).contains(&dyaw) {
189                    direction *= -1.0;
190                    profile.push(0.0);
191                } else {
192                    profile.push(direction * target_speed);
193                }
194            } else {
195                profile.push(0.0);
196            }
197        }
198
199        profile
200    }
201
202    /// Find target index and cross-track error
203    ///
204    /// Follows the path forward from the previous nearest point (from the
205    /// start on the first call), so loops and crossings are not skipped.
206    fn calc_target_index(&mut self, state: &LQRVehicleState) -> (usize, f64) {
207        if self.path.is_empty() || self.path_yaw.is_empty() {
208            return (0, 0.0);
209        }
210        let query = Point2D::new(state.x, state.y);
211        let from = self.nearest_index.unwrap_or(0);
212        let min_idx = self
213            .path
214            .nearest_point_index_forward(query, from)
215            .unwrap_or(from);
216        self.nearest_index = Some(min_idx);
217        let target = &self.path.points[min_idx];
218        let min_dist = ((state.x - target.x).powi(2) + (state.y - target.y).powi(2)).sqrt();
219
220        // Calculate signed cross-track error
221        let diff_x = target.x - state.x;
222        let diff_y = target.y - state.y;
223        let arcang = self.path_yaw[min_idx] - diff_y.atan2(diff_x);
224        let angle = normalize_angle(arcang);
225
226        let error = if angle < 0.0 { -min_dist } else { min_dist };
227        (min_idx, error)
228    }
229
230    /// Solve Discrete Algebraic Riccati Equation
231    fn solve_dare(
232        a: Matrix4<f64>,
233        b: Vector4<f64>,
234        q: Matrix4<f64>,
235        r: Matrix1<f64>,
236    ) -> Matrix4<f64> {
237        let mut x = q;
238        let max_iter = 150;
239        let eps = 0.01;
240
241        for _ in 0..max_iter {
242            let bt_x_b = b.transpose() * x * b;
243            let inv = (r + bt_x_b).try_inverse().unwrap_or(Matrix1::identity());
244            let xn =
245                a.transpose() * x * a - a.transpose() * x * b * inv * b.transpose() * x * a + q;
246
247            if (xn - x).abs().max() < eps {
248                break;
249            }
250            x = xn;
251        }
252        x
253    }
254
255    /// Compute LQR gain
256    fn dlqr(a: Matrix4<f64>, b: Vector4<f64>, q: Matrix4<f64>, r: Matrix1<f64>) -> Matrix1x4<f64> {
257        let x = Self::solve_dare(a, b, q, r);
258        let bt_x_b = b.transpose() * x * b;
259        let inv = (bt_x_b + r).try_inverse().unwrap_or(Matrix1::identity());
260        inv * (b.transpose() * x * a)
261    }
262
263    /// Compute LQR steering control
264    pub fn compute_steering(&mut self, state: &LQRVehicleState) -> f64 {
265        if self.path.is_empty() {
266            return 0.0;
267        }
268
269        let (ind, e) = self.calc_target_index(state);
270        let k = if ind < self.path_curvature.len() {
271            self.path_curvature[ind]
272        } else {
273            0.0
274        };
275
276        let th_e = normalize_angle(state.yaw - self.path_yaw[ind]);
277        let dt = self.config.dt;
278
279        // Build state-space matrices
280        let mut a = Matrix4::zeros();
281        a[(0, 0)] = 1.0;
282        a[(0, 1)] = dt;
283        a[(1, 2)] = state.v;
284        a[(2, 2)] = 1.0;
285        a[(2, 3)] = dt;
286
287        let mut b = Vector4::zeros();
288        b[3] = state.v / state.wheelbase;
289
290        // Build Q and R matrices
291        let q = Matrix4::from_diagonal(&Vector4::new(
292            self.config.q_diag[0],
293            self.config.q_diag[1],
294            self.config.q_diag[2],
295            self.config.q_diag[3],
296        ));
297        let r = Matrix1::new(self.config.r);
298
299        let gain = Self::dlqr(a, b, q, r);
300
301        // State error vector
302        let x_err = Vector4::new(
303            e,
304            if dt > 0.0 {
305                (e - self.prev_error) / dt
306            } else {
307                0.0
308            },
309            th_e,
310            if dt > 0.0 {
311                (th_e - self.prev_theta_error) / dt
312            } else {
313                0.0
314            },
315        );
316
317        // Feed-forward + feedback
318        let ff = (state.wheelbase * k).atan2(1.0);
319        let fb = normalize_angle((-gain * x_err)[0]);
320        let delta = ff + fb;
321
322        self.prev_error = e;
323        self.prev_theta_error = th_e;
324
325        delta.clamp(-state.max_steer, state.max_steer)
326    }
327
328    /// Proportional speed control
329    pub fn compute_acceleration(&self, target_speed: f64, current_speed: f64) -> f64 {
330        self.config.kp * (target_speed - current_speed)
331    }
332
333    /// Get target speed for current index
334    pub fn get_target_speed(&self, index: usize) -> f64 {
335        if index < self.speed_profile.len() {
336            self.speed_profile[index].abs()
337        } else {
338            0.0
339        }
340    }
341
342    /// Check if goal is reached
343    pub fn is_goal_reached_vehicle(&self, state: &LQRVehicleState) -> bool {
344        if let Some(goal) = self.path.points.last() {
345            let dx = state.x - goal.x;
346            let dy = state.y - goal.y;
347            (dx * dx + dy * dy).sqrt() < self.config.goal_threshold
348        } else {
349            true
350        }
351    }
352
353    /// Legacy planning interface
354    pub fn planning(
355        &mut self,
356        waypoints: Vec<(f64, f64)>,
357        target_speed: f64,
358        ds: f64,
359    ) -> Option<Vec<(f64, f64)>> {
360        if waypoints.len() < 2 {
361            return None;
362        }
363
364        // Generate spline path
365        let ax: Vec<f64> = waypoints.iter().map(|p| p.0).collect();
366        let ay: Vec<f64> = waypoints.iter().map(|p| p.1).collect();
367        let (cx, cy, cyaw, ck, _) = calc_spline_course(&ax, &ay, ds);
368        // Waypoints closer together than `ds` give an empty course.
369        if cx.is_empty() {
370            return None;
371        }
372
373        // Set path
374        let path = Path2D::from_points(
375            cx.iter()
376                .zip(cy.iter())
377                .map(|(&x, &y)| Point2D::new(x, y))
378                .collect(),
379        );
380        self.path_yaw = cyaw;
381        self.path_curvature = ck;
382        self.speed_profile = self.calc_speed_profile(&path, target_speed);
383        self.path = path;
384        self.nearest_index = None;
385
386        // Simulate tracking
387        let mut state = LQRVehicleState::new(
388            0.0,
389            0.0,
390            0.0,
391            0.0,
392            self.config.wheelbase,
393            self.config.max_steer,
394        );
395
396        let mut trajectory = vec![(state.x, state.y)];
397        let dt = self.config.dt;
398        let t_max = 500.0;
399        let mut time = 0.0;
400
401        while time < t_max {
402            let (target_idx, _) = self.calc_target_index(&state);
403            let target_v = self.get_target_speed(target_idx);
404
405            let ai = self.compute_acceleration(target_v, state.v);
406            let di = self.compute_steering(&state);
407            state.update(ai, di, dt);
408            time += dt;
409
410            trajectory.push((state.x, state.y));
411
412            if self.is_goal_reached_vehicle(&state) {
413                break;
414            }
415        }
416
417        Some(trajectory)
418    }
419}
420
421impl PathTracker for LQRSteerController {
422    fn compute_control(&mut self, current_state: &State2D, path: &Path2D) -> ControlInput {
423        // Adopt a new reference path (comparing contents, not just length)
424        if self.path != *path {
425            self.set_path(path.clone());
426        }
427
428        let vehicle_state = LQRVehicleState::new(
429            current_state.x,
430            current_state.y,
431            current_state.yaw,
432            current_state.v,
433            self.config.wheelbase,
434            self.config.max_steer,
435        );
436
437        if self.path.is_empty() {
438            // Nothing to track: stop.
439            return ControlInput::new(0.0, 0.0);
440        }
441        let delta = self.compute_steering(&vehicle_state);
442        let (target_idx, _) = self.calc_target_index(&vehicle_state);
443        let target_v = self.get_target_speed(target_idx);
444
445        let v =
446            current_state.v + self.compute_acceleration(target_v, current_state.v) * self.config.dt;
447        let omega = v * delta.tan() / self.config.wheelbase;
448
449        ControlInput::new(v, omega)
450    }
451
452    fn is_goal_reached(&self, current_state: &State2D, goal: Point2D) -> bool {
453        let dx = current_state.x - goal.x;
454        let dy = current_state.y - goal.y;
455        (dx * dx + dy * dy).sqrt() < self.config.goal_threshold
456    }
457}
458
459#[cfg(test)]
460mod tests {
461    use super::*;
462
463    #[test]
464    fn the_nearest_point_search_stays_on_the_leg_being_driven() {
465        // A hairpin: out along y = 0, back along y = 0.5.
466        let mut points: Vec<Point2D> = (0..=100)
467            .map(|i| Point2D::new(i as f64 * 0.1, 0.0))
468            .collect();
469        points.extend((0..=100).map(|i| Point2D::new(10.0 - i as f64 * 0.1, 0.5)));
470        let mut controller = LQRSteerController::with_defaults();
471        controller.set_path(Path2D::from_points(points));
472        let at = |x: f64, y: f64| LQRVehicleState::new(x, y, 0.0, 1.0, 2.5, 0.6);
473
474        let (first, _) = controller.calc_target_index(&at(2.0, 0.1));
475        assert_eq!(first, 20);
476        // Drifted toward the return leg: still the outbound leg, further on.
477        let (next, _) = controller.calc_target_index(&at(3.0, 0.3));
478        assert_eq!(next, 30);
479    }
480
481    #[test]
482    fn an_empty_or_too_short_path_does_not_panic() {
483        let mut controller = LQRSteerController::new(LQRSteerConfig::default());
484        let command = controller.compute_control(&State2D::new(0.0, 0.0, 0.0, 1.0), &Path2D::new());
485        assert_eq!((command.v, command.omega), (0.0, 0.0));
486        let mut controller = LQRSteerController::new(LQRSteerConfig::default());
487        assert!(controller
488            .planning(vec![(0.0, 0.0), (0.3, 0.0)], 1.0, 0.5)
489            .is_none());
490    }
491
492    #[test]
493    fn test_lqr_creation() {
494        let config = LQRSteerConfig::default();
495        let controller = LQRSteerController::new(config);
496        assert!(controller.path.is_empty());
497    }
498
499    #[test]
500    fn test_lqr_set_path() {
501        let mut controller = LQRSteerController::with_defaults();
502        let path = Path2D::from_points(vec![
503            Point2D::new(0.0, 0.0),
504            Point2D::new(1.0, 0.0),
505            Point2D::new(2.0, 0.0),
506        ]);
507        controller.set_path(path);
508        assert_eq!(controller.get_path().len(), 3);
509    }
510
511    #[test]
512    fn test_lqr_normalize_angle() {
513        assert!((normalize_angle(3.0 * PI) - PI).abs() < 0.01);
514        assert!((normalize_angle(-3.0 * PI) + PI).abs() < 0.01);
515    }
516
517    #[test]
518    fn test_lqr_planning() {
519        let mut controller = LQRSteerController::with_defaults();
520        let waypoints = vec![(0.0, 0.0), (5.0, 0.0), (10.0, 0.0)];
521
522        let result = controller.planning(waypoints, 2.0, 0.5);
523        assert!(result.is_some());
524        assert!(!result.unwrap().is_empty());
525    }
526
527    #[test]
528    fn test_lqr_solve_dare() {
529        let a = Matrix4::identity();
530        let b = Vector4::new(0.0, 0.0, 0.0, 1.0);
531        let q = Matrix4::identity();
532        let r = Matrix1::new(1.0);
533
534        let x = LQRSteerController::solve_dare(a, b, q, r);
535        // Should return a positive definite matrix
536        assert!(x[(0, 0)] >= 0.0);
537    }
538}