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 nalgebra::{Matrix1, Matrix1x4, Matrix4, Vector4};
10use rust_robotics_core::{ControlInput, Path2D, PathTracker, Point2D, State2D};
11use std::f64::consts::PI;
12
13/// Vehicle state for LQR controller
14#[derive(Debug, Clone, Copy)]
15pub struct LQRVehicleState {
16    pub x: f64,
17    pub y: f64,
18    pub yaw: f64,
19    pub v: f64,
20    pub wheelbase: f64,
21    pub max_steer: f64,
22}
23
24impl LQRVehicleState {
25    pub fn new(x: f64, y: f64, yaw: f64, v: f64, wheelbase: f64, max_steer: f64) -> Self {
26        LQRVehicleState {
27            x,
28            y,
29            yaw,
30            v,
31            wheelbase,
32            max_steer,
33        }
34    }
35
36    pub fn update(&mut self, a: f64, mut delta: f64, dt: f64) {
37        delta = delta.clamp(-self.max_steer, self.max_steer);
38        self.x += self.v * self.yaw.cos() * dt;
39        self.y += self.v * self.yaw.sin() * dt;
40        self.yaw += self.v / self.wheelbase * delta.tan() * dt;
41        self.v += a * dt;
42    }
43
44    pub fn to_state2d(&self) -> State2D {
45        State2D::new(self.x, self.y, self.yaw, self.v)
46    }
47}
48
49impl From<State2D> for LQRVehicleState {
50    fn from(s: State2D) -> Self {
51        LQRVehicleState::new(s.x, s.y, s.yaw, s.v, 0.5, 45.0_f64.to_radians())
52    }
53}
54
55/// Configuration for LQR Steer Controller
56#[derive(Debug, Clone)]
57pub struct LQRSteerConfig {
58    /// Vehicle wheelbase
59    pub wheelbase: f64,
60    /// Maximum steering angle \[rad\]
61    pub max_steer: f64,
62    /// Speed proportional gain
63    pub kp: f64,
64    /// State cost matrix Q (4x4 diagonal)
65    pub q_diag: [f64; 4],
66    /// Control cost R
67    pub r: f64,
68    /// Time step
69    pub dt: f64,
70    /// Goal distance threshold
71    pub goal_threshold: f64,
72}
73
74impl Default for LQRSteerConfig {
75    fn default() -> Self {
76        Self {
77            wheelbase: 0.5,
78            max_steer: 45.0_f64.to_radians(),
79            kp: 1.0,
80            q_diag: [1.0, 1.0, 1.0, 1.0],
81            r: 1.0,
82            dt: 0.1,
83            goal_threshold: 0.3,
84        }
85    }
86}
87
88/// LQR Steer path tracking controller
89pub struct LQRSteerController {
90    config: LQRSteerConfig,
91    path: Path2D,
92    path_yaw: Vec<f64>,
93    path_curvature: Vec<f64>,
94    speed_profile: Vec<f64>,
95    prev_error: f64,
96    prev_theta_error: f64,
97}
98
99impl LQRSteerController {
100    /// Create a new LQR Steer controller
101    pub fn new(config: LQRSteerConfig) -> Self {
102        LQRSteerController {
103            config,
104            path: Path2D::new(),
105            path_yaw: Vec::new(),
106            path_curvature: Vec::new(),
107            speed_profile: Vec::new(),
108            prev_error: 0.0,
109            prev_theta_error: 0.0,
110        }
111    }
112
113    /// Create with default configuration
114    pub fn with_defaults() -> Self {
115        Self::new(LQRSteerConfig::default())
116    }
117
118    /// Set the reference path
119    pub fn set_path(&mut self, path: Path2D) {
120        let (yaw, curvature) = self.compute_path_derivatives(&path);
121        self.path_yaw = yaw;
122        self.path_curvature = curvature;
123        self.speed_profile = self.calc_speed_profile(&path, 2.78); // default ~10 km/h
124        self.path = path;
125        self.prev_error = 0.0;
126        self.prev_theta_error = 0.0;
127    }
128
129    /// Set the reference path with speed
130    pub fn set_path_with_speed(&mut self, path: Path2D, target_speed: f64) {
131        let (yaw, curvature) = self.compute_path_derivatives(&path);
132        self.path_yaw = yaw;
133        self.path_curvature = curvature;
134        self.speed_profile = self.calc_speed_profile(&path, target_speed);
135        self.path = path;
136        self.prev_error = 0.0;
137        self.prev_theta_error = 0.0;
138    }
139
140    /// Get the current reference path
141    pub fn get_path(&self) -> &Path2D {
142        &self.path
143    }
144
145    /// Compute yaw and curvature from path points
146    fn compute_path_derivatives(&self, path: &Path2D) -> (Vec<f64>, Vec<f64>) {
147        let n = path.len();
148        let yaw = path.yaw_profile();
149        let mut curvature = Vec::with_capacity(n);
150
151        for i in 0..n {
152            // Simple curvature approximation
153            if i > 0 && i < n - 1 {
154                let dyaw = Self::normalize_angle(yaw[i] - yaw[i - 1]);
155                let dx = path.points[i].x - path.points[i - 1].x;
156                let dy = path.points[i].y - path.points[i - 1].y;
157                let ds = (dx * dx + dy * dy).sqrt();
158                curvature.push(if ds > 0.001 { dyaw / ds } else { 0.0 });
159            } else {
160                curvature.push(0.0);
161            }
162        }
163
164        (yaw, curvature)
165    }
166
167    /// Calculate speed profile based on path curvature
168    fn calc_speed_profile(&self, path: &Path2D, target_speed: f64) -> Vec<f64> {
169        let n = path.len();
170        let mut profile = Vec::with_capacity(n);
171        let mut direction = 1.0;
172
173        for i in 0..n {
174            if i < n - 1 && i < self.path_yaw.len() - 1 {
175                let dyaw = (self.path_yaw[i + 1] - self.path_yaw[i]).abs();
176                if (PI / 4.0..PI / 2.0).contains(&dyaw) {
177                    direction *= -1.0;
178                    profile.push(0.0);
179                } else {
180                    profile.push(direction * target_speed);
181                }
182            } else {
183                profile.push(0.0);
184            }
185        }
186
187        profile
188    }
189
190    /// Normalize angle to [-PI, PI]
191    fn normalize_angle(mut angle: f64) -> f64 {
192        while angle > PI {
193            angle -= 2.0 * PI;
194        }
195        while angle < -PI {
196            angle += 2.0 * PI;
197        }
198        angle
199    }
200
201    /// Find target index and cross-track error
202    fn calc_target_index(&self, state: &LQRVehicleState) -> (usize, f64) {
203        let query = Point2D::new(state.x, state.y);
204        let min_idx = self.path.nearest_point_index(query).unwrap_or(0);
205        let target = &self.path.points[min_idx];
206        let min_dist = ((state.x - target.x).powi(2) + (state.y - target.y).powi(2)).sqrt();
207
208        // Calculate signed cross-track error
209        let diff_x = target.x - state.x;
210        let diff_y = target.y - state.y;
211        let arcang = self.path_yaw[min_idx] - diff_y.atan2(diff_x);
212        let angle = Self::normalize_angle(arcang);
213
214        let error = if angle < 0.0 { -min_dist } else { min_dist };
215        (min_idx, error)
216    }
217
218    /// Solve Discrete Algebraic Riccati Equation
219    fn solve_dare(
220        a: Matrix4<f64>,
221        b: Vector4<f64>,
222        q: Matrix4<f64>,
223        r: Matrix1<f64>,
224    ) -> Matrix4<f64> {
225        let mut x = q;
226        let max_iter = 150;
227        let eps = 0.01;
228
229        for _ in 0..max_iter {
230            let bt_x_b = b.transpose() * x * b;
231            let inv = (r + bt_x_b).try_inverse().unwrap_or(Matrix1::identity());
232            let xn =
233                a.transpose() * x * a - a.transpose() * x * b * inv * b.transpose() * x * a + q;
234
235            if (xn - x).abs().max() < eps {
236                break;
237            }
238            x = xn;
239        }
240        x
241    }
242
243    /// Compute LQR gain
244    fn dlqr(a: Matrix4<f64>, b: Vector4<f64>, q: Matrix4<f64>, r: Matrix1<f64>) -> Matrix1x4<f64> {
245        let x = Self::solve_dare(a, b, q, r);
246        let bt_x_b = b.transpose() * x * b;
247        let inv = (bt_x_b + r).try_inverse().unwrap_or(Matrix1::identity());
248        inv * (b.transpose() * x * a)
249    }
250
251    /// Compute LQR steering control
252    pub fn compute_steering(&mut self, state: &LQRVehicleState) -> f64 {
253        if self.path.is_empty() {
254            return 0.0;
255        }
256
257        let (ind, e) = self.calc_target_index(state);
258        let k = if ind < self.path_curvature.len() {
259            self.path_curvature[ind]
260        } else {
261            0.0
262        };
263
264        let th_e = Self::normalize_angle(state.yaw - self.path_yaw[ind]);
265        let dt = self.config.dt;
266
267        // Build state-space matrices
268        let mut a = Matrix4::zeros();
269        a[(0, 0)] = 1.0;
270        a[(0, 1)] = dt;
271        a[(1, 2)] = state.v;
272        a[(2, 2)] = 1.0;
273        a[(2, 3)] = dt;
274
275        let mut b = Vector4::zeros();
276        b[3] = state.v / state.wheelbase;
277
278        // Build Q and R matrices
279        let q = Matrix4::from_diagonal(&Vector4::new(
280            self.config.q_diag[0],
281            self.config.q_diag[1],
282            self.config.q_diag[2],
283            self.config.q_diag[3],
284        ));
285        let r = Matrix1::new(self.config.r);
286
287        let gain = Self::dlqr(a, b, q, r);
288
289        // State error vector
290        let x_err = Vector4::new(
291            e,
292            if dt > 0.0 {
293                (e - self.prev_error) / dt
294            } else {
295                0.0
296            },
297            th_e,
298            if dt > 0.0 {
299                (th_e - self.prev_theta_error) / dt
300            } else {
301                0.0
302            },
303        );
304
305        // Feed-forward + feedback
306        let ff = (state.wheelbase * k).atan2(1.0);
307        let fb = Self::normalize_angle((-gain * x_err)[0]);
308        let delta = ff + fb;
309
310        self.prev_error = e;
311        self.prev_theta_error = th_e;
312
313        delta.clamp(-state.max_steer, state.max_steer)
314    }
315
316    /// Proportional speed control
317    pub fn compute_acceleration(&self, target_speed: f64, current_speed: f64) -> f64 {
318        self.config.kp * (target_speed - current_speed)
319    }
320
321    /// Get target speed for current index
322    pub fn get_target_speed(&self, index: usize) -> f64 {
323        if index < self.speed_profile.len() {
324            self.speed_profile[index].abs()
325        } else {
326            0.0
327        }
328    }
329
330    /// Check if goal is reached
331    pub fn is_goal_reached_vehicle(&self, state: &LQRVehicleState) -> bool {
332        if let Some(goal) = self.path.points.last() {
333            let dx = state.x - goal.x;
334            let dy = state.y - goal.y;
335            (dx * dx + dy * dy).sqrt() < self.config.goal_threshold
336        } else {
337            true
338        }
339    }
340
341    /// Legacy planning interface
342    pub fn planning(
343        &mut self,
344        waypoints: Vec<(f64, f64)>,
345        target_speed: f64,
346        ds: f64,
347    ) -> Option<Vec<(f64, f64)>> {
348        if waypoints.len() < 2 {
349            return None;
350        }
351
352        // Generate spline path
353        let ax: Vec<f64> = waypoints.iter().map(|p| p.0).collect();
354        let ay: Vec<f64> = waypoints.iter().map(|p| p.1).collect();
355        let (cx, cy, cyaw, ck, _) = calc_spline_course(&ax, &ay, ds);
356
357        // Set path
358        let path = Path2D::from_points(
359            cx.iter()
360                .zip(cy.iter())
361                .map(|(&x, &y)| Point2D::new(x, y))
362                .collect(),
363        );
364        self.path_yaw = cyaw;
365        self.path_curvature = ck;
366        self.speed_profile = self.calc_speed_profile(&path, target_speed);
367        self.path = path;
368
369        // Simulate tracking
370        let mut state = LQRVehicleState::new(
371            0.0,
372            0.0,
373            0.0,
374            0.0,
375            self.config.wheelbase,
376            self.config.max_steer,
377        );
378
379        let mut trajectory = vec![(state.x, state.y)];
380        let dt = self.config.dt;
381        let t_max = 500.0;
382        let mut time = 0.0;
383
384        while time < t_max {
385            let (target_idx, _) = self.calc_target_index(&state);
386            let target_v = self.get_target_speed(target_idx);
387
388            let ai = self.compute_acceleration(target_v, state.v);
389            let di = self.compute_steering(&state);
390            state.update(ai, di, dt);
391            time += dt;
392
393            trajectory.push((state.x, state.y));
394
395            if self.is_goal_reached_vehicle(&state) {
396                break;
397            }
398        }
399
400        Some(trajectory)
401    }
402}
403
404impl PathTracker for LQRSteerController {
405    fn compute_control(&mut self, current_state: &State2D, path: &Path2D) -> ControlInput {
406        // Set path if different
407        if self.path.len() != path.len() {
408            self.set_path(path.clone());
409        }
410
411        let vehicle_state = LQRVehicleState::new(
412            current_state.x,
413            current_state.y,
414            current_state.yaw,
415            current_state.v,
416            self.config.wheelbase,
417            self.config.max_steer,
418        );
419
420        let delta = self.compute_steering(&vehicle_state);
421        let (target_idx, _) = self.calc_target_index(&vehicle_state);
422        let target_v = self.get_target_speed(target_idx);
423
424        let v =
425            current_state.v + self.compute_acceleration(target_v, current_state.v) * self.config.dt;
426        let omega = v * delta.tan() / self.config.wheelbase;
427
428        ControlInput::new(v, omega)
429    }
430
431    fn is_goal_reached(&self, current_state: &State2D, goal: Point2D) -> bool {
432        let dx = current_state.x - goal.x;
433        let dy = current_state.y - goal.y;
434        (dx * dx + dy * dy).sqrt() < self.config.goal_threshold
435    }
436}
437
438// Cubic spline helper functions for legacy interface
439
440fn calc_spline_course(x: &[f64], y: &[f64], ds: f64) -> SplineCourse {
441    let sp = CubicSpline2D::new(x, y);
442    let mut s = 0.0;
443    let mut course_x = Vec::new();
444    let mut course_y = Vec::new();
445    let mut course_yaw = Vec::new();
446    let mut course_k = Vec::new();
447    let mut course_s = Vec::new();
448
449    // sp.s is always non-empty: CubicSpline2D::new initializes s with at least one element
450    let s_max = *sp
451        .s
452        .last()
453        .expect("spline s is non-empty after construction")
454        - ds;
455    while s < s_max {
456        let (ix, iy) = sp.calc_position(s);
457        let iyaw = sp.calc_yaw(s);
458        let ik = sp.calc_curvature(s);
459        course_x.push(ix);
460        course_y.push(iy);
461        course_yaw.push(iyaw);
462        course_k.push(ik);
463        course_s.push(s);
464        s += ds;
465    }
466
467    (course_x, course_y, course_yaw, course_k, course_s)
468}
469
470struct CubicSpline {
471    a: Vec<f64>,
472    b: Vec<f64>,
473    c: Vec<f64>,
474    d: Vec<f64>,
475    x: Vec<f64>,
476}
477
478type SplineCourse = (Vec<f64>, Vec<f64>, Vec<f64>, Vec<f64>, Vec<f64>);
479
480impl CubicSpline {
481    fn new(x: &[f64], y: &[f64]) -> Self {
482        let n = x.len();
483        let mut h = vec![0.0; n - 1];
484        for i in 0..n - 1 {
485            h[i] = x[i + 1] - x[i];
486        }
487
488        let mut a = vec![0.0; n];
489        let mut b = vec![0.0; n];
490        let mut c = vec![0.0; n];
491        let mut d = vec![0.0; n];
492
493        a[..n].copy_from_slice(&y[..n]);
494
495        let mut alpha = vec![0.0; n - 1];
496        for i in 1..n - 1 {
497            alpha[i] = 3.0 * (a[i + 1] - a[i]) / h[i] - 3.0 * (a[i] - a[i - 1]) / h[i - 1];
498        }
499
500        let mut l = vec![1.0; n];
501        let mut mu = vec![0.0; n];
502        let mut z = vec![0.0; n];
503
504        for i in 1..n - 1 {
505            l[i] = 2.0 * (x[i + 1] - x[i - 1]) - h[i - 1] * mu[i - 1];
506            mu[i] = h[i] / l[i];
507            z[i] = (alpha[i] - h[i - 1] * z[i - 1]) / l[i];
508        }
509
510        for j in (0..n - 1).rev() {
511            c[j] = z[j] - mu[j] * c[j + 1];
512            b[j] = (a[j + 1] - a[j]) / h[j] - h[j] * (c[j + 1] + 2.0 * c[j]) / 3.0;
513            d[j] = (c[j + 1] - c[j]) / (3.0 * h[j]);
514        }
515
516        CubicSpline {
517            a,
518            b,
519            c,
520            d,
521            x: x.to_vec(),
522        }
523    }
524
525    fn calc(&self, t: f64) -> f64 {
526        if t < self.x[0] {
527            return self.a[0];
528        } else if t > self.x[self.x.len() - 1] {
529            return self.a[self.a.len() - 1];
530        }
531
532        let mut i = self.search_index(t);
533        if i >= self.x.len() - 1 {
534            i = self.x.len() - 2;
535        }
536
537        let dx = t - self.x[i];
538        self.a[i] + self.b[i] * dx + self.c[i] * dx * dx + self.d[i] * dx * dx * dx
539    }
540
541    fn calc_d(&self, t: f64) -> f64 {
542        if t < self.x[0] {
543            return self.b[0];
544        } else if t > self.x[self.x.len() - 1] {
545            return self.b[self.b.len() - 1];
546        }
547
548        let mut i = self.search_index(t);
549        if i >= self.x.len() - 1 {
550            i = self.x.len() - 2;
551        }
552
553        let dx = t - self.x[i];
554        self.b[i] + 2.0 * self.c[i] * dx + 3.0 * self.d[i] * dx * dx
555    }
556
557    fn calc_dd(&self, t: f64) -> f64 {
558        if t < self.x[0] {
559            return 2.0 * self.c[0];
560        } else if t > self.x[self.x.len() - 1] {
561            return 2.0 * self.c[self.c.len() - 1];
562        }
563
564        let mut i = self.search_index(t);
565        if i >= self.x.len() - 1 {
566            i = self.x.len() - 2;
567        }
568
569        let dx = t - self.x[i];
570        2.0 * self.c[i] + 6.0 * self.d[i] * dx
571    }
572
573    fn search_index(&self, x: f64) -> usize {
574        for i in 0..self.x.len() - 1 {
575            if self.x[i] <= x && x <= self.x[i + 1] {
576                return i;
577            }
578        }
579        self.x.len() - 2
580    }
581}
582
583struct CubicSpline2D {
584    s: Vec<f64>,
585    sx: CubicSpline,
586    sy: CubicSpline,
587}
588
589impl CubicSpline2D {
590    fn new(x: &[f64], y: &[f64]) -> Self {
591        let mut s = vec![0.0];
592        for i in 1..x.len() {
593            let dx = x[i] - x[i - 1];
594            let dy = y[i] - y[i - 1];
595            s.push(s[i - 1] + (dx * dx + dy * dy).sqrt());
596        }
597
598        let sx = CubicSpline::new(&s, x);
599        let sy = CubicSpline::new(&s, y);
600
601        CubicSpline2D { s, sx, sy }
602    }
603
604    fn calc_position(&self, s: f64) -> (f64, f64) {
605        let x = self.sx.calc(s);
606        let y = self.sy.calc(s);
607        (x, y)
608    }
609
610    fn calc_curvature(&self, s: f64) -> f64 {
611        let dx = self.sx.calc_d(s);
612        let ddx = self.sx.calc_dd(s);
613        let dy = self.sy.calc_d(s);
614        let ddy = self.sy.calc_dd(s);
615        let denom = (dx * dx + dy * dy).powf(1.5);
616        if denom.abs() > 1e-6 {
617            (ddy * dx - ddx * dy) / denom
618        } else {
619            0.0
620        }
621    }
622
623    fn calc_yaw(&self, s: f64) -> f64 {
624        let dx = self.sx.calc_d(s);
625        let dy = self.sy.calc_d(s);
626        dy.atan2(dx)
627    }
628}
629
630#[cfg(test)]
631mod tests {
632    use super::*;
633
634    #[test]
635    fn test_lqr_creation() {
636        let config = LQRSteerConfig::default();
637        let controller = LQRSteerController::new(config);
638        assert!(controller.path.is_empty());
639    }
640
641    #[test]
642    fn test_lqr_set_path() {
643        let mut controller = LQRSteerController::with_defaults();
644        let path = Path2D::from_points(vec![
645            Point2D::new(0.0, 0.0),
646            Point2D::new(1.0, 0.0),
647            Point2D::new(2.0, 0.0),
648        ]);
649        controller.set_path(path);
650        assert_eq!(controller.get_path().len(), 3);
651    }
652
653    #[test]
654    fn test_lqr_normalize_angle() {
655        assert!((LQRSteerController::normalize_angle(3.0 * PI) - PI).abs() < 0.01);
656        assert!((LQRSteerController::normalize_angle(-3.0 * PI) + PI).abs() < 0.01);
657    }
658
659    #[test]
660    fn test_lqr_planning() {
661        let mut controller = LQRSteerController::with_defaults();
662        let waypoints = vec![(0.0, 0.0), (5.0, 0.0), (10.0, 0.0)];
663
664        let result = controller.planning(waypoints, 2.0, 0.5);
665        assert!(result.is_some());
666        assert!(!result.unwrap().is_empty());
667    }
668
669    #[test]
670    fn test_lqr_solve_dare() {
671        let a = Matrix4::identity();
672        let b = Vector4::new(0.0, 0.0, 0.0, 1.0);
673        let q = Matrix4::identity();
674        let r = Matrix1::new(1.0);
675
676        let x = LQRSteerController::solve_dare(a, b, q, r);
677        // Should return a positive definite matrix
678        assert!(x[(0, 0)] >= 0.0);
679    }
680}
rust_robotics_control/lqr_steer_control.rs

rust_robotics_control/
lqr_steer_control.rs
