1use 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)]
15use num_traits::Float;
17use rust_robotics_core::normalize_angle;
18use rust_robotics_core::{ControlInput, Path2D, PathTracker, Point2D, State2D};
19
20#[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#[derive(Debug, Clone)]
64pub struct LQRSteerConfig {
65 pub wheelbase: f64,
67 pub max_steer: f64,
69 pub kp: f64,
71 pub q_diag: [f64; 4],
73 pub r: f64,
75 pub dt: f64,
77 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
95pub 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_index: Option<usize>,
106}
107
108impl LQRSteerController {
109 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 pub fn with_defaults() -> Self {
125 Self::new(LQRSteerConfig::default())
126 }
127
128 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); self.path = path;
135 self.prev_error = 0.0;
136 self.prev_theta_error = 0.0;
137 self.nearest_index = None;
138 }
139
140 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 pub fn get_path(&self) -> &Path2D {
154 &self.path
155 }
156
157 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 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 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 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 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 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 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 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 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 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 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 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 pub fn compute_acceleration(&self, target_speed: f64, current_speed: f64) -> f64 {
330 self.config.kp * (target_speed - current_speed)
331 }
332
333 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 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 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 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 if cx.is_empty() {
370 return None;
371 }
372
373 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 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 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 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 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 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 assert!(x[(0, 0)] >= 0.0);
537 }
538}