rust_robotics_planning/
lqr_planner.rs

1//! LQR-based path planner
2//!
3//! Uses a discrete-time double-integrator model with LQR control
4//! to generate a smooth path from start to goal.
5//! The state is expressed relative to the goal so the LQR drives
6//! the error to zero.
7//!
8//! Reference: PythonRobotics LQRPlanner by Atsushi Sakai
9
10use nalgebra::{Matrix1, Matrix2, Matrix2x1, Vector2};
11use rust_robotics_core::{Path2D, Point2D, RoboticsError, RoboticsResult};
12
13/// Configuration for the LQR path planner.
14#[derive(Debug, Clone)]
15pub struct LqrPlannerConfig {
16    /// Maximum simulation time \[s\]
17    pub max_time: f64,
18    /// Time step \[s\]
19    pub dt: f64,
20    /// Goal tolerance distance \[m\]
21    pub goal_dist: f64,
22    /// Maximum DARE iterations
23    pub max_iter: usize,
24    /// DARE convergence threshold
25    pub eps: f64,
26}
27
28impl Default for LqrPlannerConfig {
29    fn default() -> Self {
30        Self {
31            max_time: 100.0,
32            dt: 0.1,
33            goal_dist: 0.1,
34            max_iter: 150,
35            eps: 0.01,
36        }
37    }
38}
39
40/// LQR-based path planner.
41///
42/// Plans a path by formulating each axis (x, y) as an independent
43/// double-integrator and using discrete-time LQR to compute the
44/// control input that drives the state towards the goal.
45pub struct LqrPlanner {
46    pub config: LqrPlannerConfig,
47}
48
49impl LqrPlanner {
50    pub fn new(config: LqrPlannerConfig) -> Self {
51        Self { config }
52    }
53
54    /// Plan a path from `start` to `goal`, returning a [`Path2D`].
55    pub fn plan(&self, start: Point2D, goal: Point2D) -> RoboticsResult<Path2D> {
56        let (rx, ry) = self.planning(start.x, start.y, goal.x, goal.y)?;
57        Path2D::try_from_xy(&rx, &ry)
58    }
59
60    /// Plan a path returning raw coordinate vectors.
61    pub fn planning(
62        &self,
63        sx: f64,
64        sy: f64,
65        gx: f64,
66        gy: f64,
67    ) -> RoboticsResult<(Vec<f64>, Vec<f64>)> {
68        let mut rx = vec![sx];
69        let mut ry = vec![sy];
70
71        // State: error relative to goal for each axis
72        let mut x = Vector2::new(sx - gx, sy - gy);
73
74        let (a, b) = self.get_system_model();
75
76        let mut time = 0.0;
77        while time <= self.config.max_time {
78            time += self.config.dt;
79
80            let u = self.lqr_control(&a, &b, &x);
81            x = a * x + b * u;
82
83            rx.push(x[0] + gx);
84            ry.push(x[1] + gy);
85
86            let d =
87                ((gx - *rx.last().unwrap()).powi(2) + (gy - *ry.last().unwrap()).powi(2)).sqrt();
88            if d <= self.config.goal_dist {
89                return Ok((rx, ry));
90            }
91        }
92
93        Err(RoboticsError::PlanningError(
94            "LqrPlanner: could not reach goal within max_time".to_string(),
95        ))
96    }
97
98    /// Build the discrete-time double-integrator model.
99    ///
100    /// State: \[position, velocity\]
101    /// The A matrix encodes `position += dt * position + velocity`
102    /// and `velocity += dt * velocity`.
103    fn get_system_model(&self) -> (Matrix2<f64>, Matrix2x1<f64>) {
104        let a = Matrix2::new(self.config.dt, 1.0, 0.0, self.config.dt);
105        let b = Matrix2x1::new(0.0, 1.0);
106        (a, b)
107    }
108
109    /// Compute the LQR control input `u = -K * x`.
110    fn lqr_control(&self, a: &Matrix2<f64>, b: &Matrix2x1<f64>, x: &Vector2<f64>) -> f64 {
111        let q = Matrix2::<f64>::identity();
112        let r = Matrix1::<f64>::identity();
113        let (k, _, _) = self.dlqr(a, b, &q, &r);
114        let u = -(k * x);
115        u[0]
116    }
117
118    /// Solve a discrete-time Algebraic Riccati Equation (DARE).
119    fn solve_dare(
120        &self,
121        a: &Matrix2<f64>,
122        b: &Matrix2x1<f64>,
123        q: &Matrix2<f64>,
124        r: &Matrix1<f64>,
125    ) -> Matrix2<f64> {
126        let mut p = *q;
127
128        for _ in 0..self.config.max_iter {
129            let bt_p = b.transpose() * p;
130            let denom = r + bt_p * b;
131            if denom[0].abs() < 1e-10 {
132                break;
133            }
134
135            let pn =
136                a.transpose() * p * a - a.transpose() * p * b * (1.0 / denom[0]) * bt_p * a + q;
137
138            if (pn - p).abs().max() < self.config.eps {
139                break;
140            }
141            p = pn;
142        }
143        p
144    }
145
146    /// Solve the discrete-time LQR controller.
147    ///
148    /// Returns `(K, P, eigenvalues)` where K is the gain matrix.
149    fn dlqr(
150        &self,
151        a: &Matrix2<f64>,
152        b: &Matrix2x1<f64>,
153        q: &Matrix2<f64>,
154        r: &Matrix1<f64>,
155    ) -> (nalgebra::Matrix1x2<f64>, Matrix2<f64>, Vector2<f64>) {
156        let p = self.solve_dare(a, b, q, r);
157
158        let bt_p = b.transpose() * p;
159        let denom = r + bt_p * b;
160        let k = (1.0 / denom[0]) * bt_p * a;
161
162        let closed_loop = a - b * k;
163        let eigenvalues = closed_loop.eigenvalues().unwrap_or_else(Vector2::zeros);
164
165        (k, p, eigenvalues)
166    }
167}
168
169#[cfg(test)]
170mod tests {
171    use super::*;
172
173    #[test]
174    fn test_lqr_planner_reaches_goal() {
175        let planner = LqrPlanner::new(LqrPlannerConfig::default());
176        let (rx, ry) = planner.planning(0.0, 0.0, 10.0, 10.0).unwrap();
177
178        assert!(rx.len() > 2);
179        assert_eq!(rx.len(), ry.len());
180
181        // Final point should be close to goal
182        let last_x = *rx.last().unwrap();
183        let last_y = *ry.last().unwrap();
184        let d = ((last_x - 10.0).powi(2) + (last_y - 10.0).powi(2)).sqrt();
185        assert!(d <= 0.1, "Final distance to goal: {d}");
186    }
187
188    #[test]
189    fn test_lqr_planner_negative_goal() {
190        let planner = LqrPlanner::new(LqrPlannerConfig::default());
191        let (rx, ry) = planner.planning(5.0, 5.0, -10.0, -20.0).unwrap();
192
193        let last_x = *rx.last().unwrap();
194        let last_y = *ry.last().unwrap();
195        let d = ((last_x + 10.0).powi(2) + (last_y + 20.0).powi(2)).sqrt();
196        assert!(d <= 0.1, "Final distance to goal: {d}");
197    }
198
199    #[test]
200    fn test_lqr_planner_same_start_goal() {
201        let planner = LqrPlanner::new(LqrPlannerConfig::default());
202        let (rx, _ry) = planner.planning(5.0, 5.0, 5.0, 5.0).unwrap();
203
204        // Should reach goal immediately (start == goal)
205        assert!(!rx.is_empty());
206    }
207
208    #[test]
209    fn test_lqr_planner_plan_returns_path2d() {
210        let planner = LqrPlanner::new(LqrPlannerConfig::default());
211        let path = planner
212            .plan(Point2D::new(0.0, 0.0), Point2D::new(10.0, 10.0))
213            .unwrap();
214
215        assert!(path.points.len() > 2);
216    }
217
218    #[test]
219    fn test_lqr_planner_starts_at_start() {
220        let planner = LqrPlanner::new(LqrPlannerConfig::default());
221        let (rx, ry) = planner.planning(3.0, 7.0, 20.0, 15.0).unwrap();
222
223        assert!((rx[0] - 3.0).abs() < 1e-10);
224        assert!((ry[0] - 7.0).abs() < 1e-10);
225    }
226
227    #[test]
228    fn test_lqr_planner_config_custom() {
229        let config = LqrPlannerConfig {
230            max_time: 50.0,
231            dt: 0.05,
232            goal_dist: 0.5,
233            max_iter: 200,
234            eps: 0.001,
235        };
236        let planner = LqrPlanner::new(config);
237        let result = planner.planning(0.0, 0.0, 5.0, 5.0);
238        assert!(result.is_ok());
239    }
240
241    #[test]
242    fn test_lqr_planner_unreachable_returns_error() {
243        let config = LqrPlannerConfig {
244            max_time: 0.01, // Very short time
245            goal_dist: 0.001,
246            ..LqrPlannerConfig::default()
247        };
248        let planner = LqrPlanner::new(config);
249        let result = planner.planning(0.0, 0.0, 1000.0, 1000.0);
250        assert!(result.is_err());
251    }
252}
rust_robotics_planning/lqr_planner.rs

rust_robotics_planning/
lqr_planner.rs
