1use crate::spline_course::calc_spline_course;
15use nalgebra::{Matrix2, Matrix5, Matrix5x2, SMatrix, SVector, Vector2, Vector5};
16use rust_robotics_core::normalize_angle;
17use rust_robotics_core::{ControlInput, Path2D, PathTracker, Point2D, State2D};
18use std::f64::consts::PI;
19
20#[derive(Debug, Clone, Copy)]
22pub struct LQRSpeedSteerVehicleState {
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 LQRSpeedSteerVehicleState {
32 pub fn new(x: f64, y: f64, yaw: f64, v: f64, wheelbase: f64, max_steer: f64) -> Self {
33 Self {
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) {
45 delta = delta.clamp(-self.max_steer, self.max_steer);
46 self.x += self.v * self.yaw.cos() * dt;
47 self.y += self.v * self.yaw.sin() * dt;
48 self.yaw += self.v / self.wheelbase * delta.tan() * dt;
49 self.v += a * dt;
50 }
51
52 pub fn to_state2d(&self) -> State2D {
53 State2D::new(self.x, self.y, self.yaw, self.v)
54 }
55}
56
57impl From<State2D> for LQRSpeedSteerVehicleState {
58 fn from(s: State2D) -> Self {
59 Self::new(s.x, s.y, s.yaw, s.v, 0.5, 45.0_f64.to_radians())
60 }
61}
62
63#[derive(Debug, Clone)]
65pub struct LQRSpeedSteerConfig {
66 pub wheelbase: f64,
68 pub max_steer: f64,
70 pub q_diag: [f64; 5],
72 pub r_diag: [f64; 2],
74 pub dt: f64,
76 pub goal_threshold: f64,
78}
79
80impl Default for LQRSpeedSteerConfig {
81 fn default() -> Self {
82 Self {
83 wheelbase: 0.5,
84 max_steer: 45.0_f64.to_radians(),
85 q_diag: [1.0, 1.0, 1.0, 1.0, 1.0],
86 r_diag: [1.0, 1.0],
87 dt: 0.1,
88 goal_threshold: 0.3,
89 }
90 }
91}
92
93pub struct LQRSpeedSteerController {
99 config: LQRSpeedSteerConfig,
100 path: Path2D,
101 path_yaw: Vec<f64>,
102 path_curvature: Vec<f64>,
103 speed_profile: Vec<f64>,
104 prev_error: f64,
105 prev_theta_error: f64,
106}
107
108impl LQRSpeedSteerController {
109 pub fn new(config: LQRSpeedSteerConfig) -> Self {
111 Self {
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 }
120 }
121
122 pub fn with_defaults() -> Self {
124 Self::new(LQRSpeedSteerConfig::default())
125 }
126
127 pub fn set_path(&mut self, path: Path2D) {
129 let (yaw, curvature) = self.compute_path_derivatives(&path);
130 self.path_yaw = yaw;
131 self.path_curvature = curvature;
132 self.speed_profile = self.calc_speed_profile(&path, 10.0 / 3.6);
133 self.path = path;
134 self.prev_error = 0.0;
135 self.prev_theta_error = 0.0;
136 }
137
138 pub fn set_path_with_speed(&mut self, path: Path2D, target_speed: f64) {
140 let (yaw, curvature) = self.compute_path_derivatives(&path);
141 self.path_yaw = yaw;
142 self.path_curvature = curvature;
143 self.speed_profile = self.calc_speed_profile(&path, target_speed);
144 self.path = path;
145 self.prev_error = 0.0;
146 self.prev_theta_error = 0.0;
147 }
148
149 pub fn get_path(&self) -> &Path2D {
151 &self.path
152 }
153
154 fn compute_path_derivatives(&self, path: &Path2D) -> (Vec<f64>, Vec<f64>) {
156 let n = path.len();
157 let yaw = path.yaw_profile();
158 let mut curvature = Vec::with_capacity(n);
159
160 for i in 0..n {
161 if i > 0 && i < n - 1 {
162 let dyaw = normalize_angle(yaw[i] - yaw[i - 1]);
163 let dx = path.points[i].x - path.points[i - 1].x;
164 let dy = path.points[i].y - path.points[i - 1].y;
165 let ds = (dx * dx + dy * dy).sqrt();
166 curvature.push(if ds > 0.001 { dyaw / ds } else { 0.0 });
167 } else {
168 curvature.push(0.0);
169 }
170 }
171
172 (yaw, curvature)
173 }
174
175 fn calc_speed_profile(&self, path: &Path2D, target_speed: f64) -> Vec<f64> {
177 let n = path.len();
178 let mut profile = vec![target_speed; n];
179 let mut direction = 1.0;
180
181 #[allow(clippy::needless_range_loop)]
182 for i in 0..n.saturating_sub(1) {
183 if i < self.path_yaw.len() - 1 {
184 let dyaw = (self.path_yaw[i + 1] - self.path_yaw[i]).abs();
185 let switch = (PI / 4.0..PI / 2.0).contains(&dyaw);
186
187 if switch {
188 direction *= -1.0;
189 }
190
191 if direction != 1.0 {
192 profile[i] = -target_speed;
193 } else {
194 profile[i] = target_speed;
195 }
196
197 if switch {
198 profile[i] = 0.0;
199 }
200 }
201 }
202
203 let slowdown_len = 40.min(n);
205 for i in 0..slowdown_len {
206 let idx = n - 1 - i;
207 profile[idx] = target_speed / (50 - i) as f64;
208 if profile[idx] <= 1.0 / 3.6 {
209 profile[idx] = 1.0 / 3.6;
210 }
211 }
212
213 profile
214 }
215
216 fn calc_nearest_index(&self, state: &LQRSpeedSteerVehicleState) -> (usize, f64) {
218 let query = Point2D::new(state.x, state.y);
219 let min_idx = self.path.nearest_point_index(query).unwrap_or(0);
220 let target = &self.path.points[min_idx];
221 let min_dist = ((state.x - target.x).powi(2) + (state.y - target.y).powi(2)).sqrt();
222
223 let diff_x = target.x - state.x;
224 let diff_y = target.y - state.y;
225 let arcang = self.path_yaw[min_idx] - diff_y.atan2(diff_x);
226 let angle = normalize_angle(arcang);
227
228 let error = if angle < 0.0 { -min_dist } else { min_dist };
229 (min_idx, error)
230 }
231
232 fn solve_dare(
234 a: &Matrix5<f64>,
235 b: &Matrix5x2<f64>,
236 q: &Matrix5<f64>,
237 r: &Matrix2<f64>,
238 ) -> Matrix5<f64> {
239 let mut x = *q;
240 let max_iter = 150;
241 let eps = 0.01;
242
243 for _ in 0..max_iter {
244 let at = a.transpose();
246 let bt = b.transpose();
247 let at_x = at * x;
248 let at_x_a = at_x * a;
249 let at_x_b = at_x * b;
250 let bt_x_b: Matrix2<f64> = bt * x * b;
251 let inv = (r + bt_x_b).try_inverse().unwrap_or(Matrix2::identity());
252 let bt_x_a: SMatrix<f64, 2, 5> = bt * x * a;
253 let x_next = at_x_a - at_x_b * inv * bt_x_a + q;
254
255 let diff: f64 = (x_next - x).abs().max();
256 if diff < eps {
257 return x_next;
258 }
259 x = x_next;
260 }
261 x
262 }
263
264 fn dlqr(
266 a: &Matrix5<f64>,
267 b: &Matrix5x2<f64>,
268 q: &Matrix5<f64>,
269 r: &Matrix2<f64>,
270 ) -> SMatrix<f64, 2, 5> {
271 let x = Self::solve_dare(a, b, q, r);
272 let bt = b.transpose();
273 let bt_x_b: Matrix2<f64> = bt * x * b;
274 let inv = (bt_x_b + r).try_inverse().unwrap_or(Matrix2::identity());
275 inv * (bt * x * a)
276 }
277
278 pub fn compute_control_full(
282 &mut self,
283 state: &LQRSpeedSteerVehicleState,
284 ) -> (f64, usize, f64, f64, f64) {
285 if self.path.is_empty() {
286 return (0.0, 0, 0.0, 0.0, 0.0);
287 }
288
289 let (ind, e) = self.calc_nearest_index(state);
290 let tv = if ind < self.speed_profile.len() {
291 self.speed_profile[ind]
292 } else {
293 0.0
294 };
295 let k = if ind < self.path_curvature.len() {
296 self.path_curvature[ind]
297 } else {
298 0.0
299 };
300
301 let v = state.v;
302 let th_e = normalize_angle(state.yaw - self.path_yaw[ind]);
303 let dt = self.config.dt;
304 let l = self.config.wheelbase;
305
306 let mut a = Matrix5::zeros();
308 a[(0, 0)] = 1.0;
309 a[(0, 1)] = dt;
310 a[(1, 2)] = v;
311 a[(2, 2)] = 1.0;
312 a[(2, 3)] = dt;
313 a[(4, 4)] = 1.0;
314
315 let mut b = Matrix5x2::zeros();
317 b[(3, 0)] = v / l;
318 b[(4, 1)] = dt;
319
320 let q = Matrix5::from_diagonal(&Vector5::new(
322 self.config.q_diag[0],
323 self.config.q_diag[1],
324 self.config.q_diag[2],
325 self.config.q_diag[3],
326 self.config.q_diag[4],
327 ));
328 let r = Matrix2::from_diagonal(&Vector2::new(self.config.r_diag[0], self.config.r_diag[1]));
329
330 let gain = Self::dlqr(&a, &b, &q, &r);
331
332 let x_err = Vector5::new(
334 e,
335 if dt > 0.0 {
336 (e - self.prev_error) / dt
337 } else {
338 0.0
339 },
340 th_e,
341 if dt > 0.0 {
342 (th_e - self.prev_theta_error) / dt
343 } else {
344 0.0
345 },
346 v - tv,
347 );
348
349 let ustar: SVector<f64, 2> = -gain * x_err;
351
352 let ff = (l * k).atan2(1.0);
354 let fb = normalize_angle(ustar[0]);
355 let delta = ff + fb;
356
357 let accel = ustar[1];
359
360 self.prev_error = e;
361 self.prev_theta_error = th_e;
362
363 let delta_clamped = delta.clamp(-state.max_steer, state.max_steer);
364 (delta_clamped, ind, e, th_e, accel)
365 }
366
367 pub fn compute_steering(&mut self, state: &LQRSpeedSteerVehicleState) -> f64 {
369 self.compute_control_full(state).0
370 }
371
372 pub fn compute_acceleration(&mut self, state: &LQRSpeedSteerVehicleState) -> f64 {
374 self.compute_control_full(state).4
375 }
376
377 pub fn get_target_speed(&self, index: usize) -> f64 {
379 if index < self.speed_profile.len() {
380 self.speed_profile[index]
381 } else {
382 0.0
383 }
384 }
385
386 pub fn is_goal_reached(&self, state: &LQRSpeedSteerVehicleState) -> bool {
388 if let Some(goal) = self.path.points.last() {
389 let dx = state.x - goal.x;
390 let dy = state.y - goal.y;
391 (dx * dx + dy * dy).sqrt() < self.config.goal_threshold
392 } else {
393 true
394 }
395 }
396
397 pub fn planning(
399 &mut self,
400 waypoints: Vec<(f64, f64)>,
401 target_speed: f64,
402 ds: f64,
403 ) -> Option<Vec<(f64, f64)>> {
404 if waypoints.len() < 2 {
405 return None;
406 }
407
408 let ax: Vec<f64> = waypoints.iter().map(|p| p.0).collect();
409 let ay: Vec<f64> = waypoints.iter().map(|p| p.1).collect();
410 let (cx, cy, cyaw, ck, _) = calc_spline_course(&ax, &ay, ds);
411
412 let path = Path2D::from_points(
413 cx.iter()
414 .zip(cy.iter())
415 .map(|(&x, &y)| Point2D::new(x, y))
416 .collect(),
417 );
418 self.path_yaw = cyaw;
419 self.path_curvature = ck;
420 self.speed_profile = self.calc_speed_profile(&path, target_speed);
421 self.path = path;
422 self.prev_error = 0.0;
423 self.prev_theta_error = 0.0;
424
425 let mut state = LQRSpeedSteerVehicleState::new(
426 0.0,
427 0.0,
428 0.0,
429 0.0,
430 self.config.wheelbase,
431 self.config.max_steer,
432 );
433
434 let mut trajectory = vec![(state.x, state.y)];
435 let dt = self.config.dt;
436 let t_max = 500.0;
437 let stop_speed = 0.05;
438 let mut time = 0.0;
439
440 while time < t_max {
441 let (delta, target_ind, _, _, accel) = self.compute_control_full(&state);
442
443 state.update(accel, delta, dt);
444 time += dt;
445
446 if state.v.abs() <= stop_speed && target_ind + 1 < self.path.len() {
447 }
449
450 trajectory.push((state.x, state.y));
451
452 if self.is_goal_reached(&state) {
453 break;
454 }
455 }
456
457 Some(trajectory)
458 }
459}
460
461impl PathTracker for LQRSpeedSteerController {
462 fn compute_control(&mut self, current_state: &State2D, path: &Path2D) -> ControlInput {
463 if self.path != *path {
465 self.set_path(path.clone());
466 }
467
468 let vehicle_state = LQRSpeedSteerVehicleState::new(
469 current_state.x,
470 current_state.y,
471 current_state.yaw,
472 current_state.v,
473 self.config.wheelbase,
474 self.config.max_steer,
475 );
476
477 let (delta, _, _, _, accel) = self.compute_control_full(&vehicle_state);
478
479 let v = current_state.v + accel * self.config.dt;
480 let omega = v * delta.tan() / self.config.wheelbase;
481
482 ControlInput::new(v, omega)
483 }
484
485 fn is_goal_reached(&self, current_state: &State2D, goal: Point2D) -> bool {
486 let dx = current_state.x - goal.x;
487 let dy = current_state.y - goal.y;
488 (dx * dx + dy * dy).sqrt() < self.config.goal_threshold
489 }
490}
491
492#[cfg(test)]
493mod tests {
494 use super::*;
495
496 #[test]
497 fn test_creation() {
498 let config = LQRSpeedSteerConfig::default();
499 let controller = LQRSpeedSteerController::new(config);
500 assert!(controller.path.is_empty());
501 }
502
503 #[test]
504 fn test_default_config() {
505 let config = LQRSpeedSteerConfig::default();
506 assert_eq!(config.q_diag.len(), 5);
507 assert_eq!(config.r_diag.len(), 2);
508 assert!((config.wheelbase - 0.5).abs() < 1e-9);
509 assert!((config.dt - 0.1).abs() < 1e-9);
510 }
511
512 #[test]
513 fn test_set_path() {
514 let mut controller = LQRSpeedSteerController::with_defaults();
515 let path = Path2D::from_points(vec![
516 Point2D::new(0.0, 0.0),
517 Point2D::new(1.0, 0.0),
518 Point2D::new(2.0, 0.0),
519 Point2D::new(3.0, 0.0),
520 ]);
521 controller.set_path(path);
522 assert_eq!(controller.get_path().len(), 4);
523 assert_eq!(controller.path_yaw.len(), 4);
524 assert_eq!(controller.path_curvature.len(), 4);
525 assert_eq!(controller.speed_profile.len(), 4);
526 }
527
528 #[test]
529 fn test_set_path_with_speed() {
530 let mut controller = LQRSpeedSteerController::with_defaults();
531 let path = Path2D::from_points(vec![
532 Point2D::new(0.0, 0.0),
533 Point2D::new(1.0, 0.0),
534 Point2D::new(2.0, 0.0),
535 ]);
536 controller.set_path_with_speed(path, 5.0);
537 assert_eq!(controller.get_path().len(), 3);
538 }
539
540 #[test]
541 fn test_normalize_angle() {
542 let a = normalize_angle(3.0 * PI);
543 assert!((a - PI).abs() < 0.01);
544 let b = normalize_angle(-3.0 * PI);
545 assert!((b + PI).abs() < 0.01);
546 let c = normalize_angle(0.5);
547 assert!((c - 0.5).abs() < 1e-9);
548 }
549
550 #[test]
551 fn test_solve_dare_identity() {
552 let a = Matrix5::identity();
553 let mut b = Matrix5x2::zeros();
554 b[(3, 0)] = 1.0;
555 b[(4, 1)] = 0.1;
556 let q = Matrix5::identity();
557 let r = Matrix2::identity();
558
559 let x = LQRSpeedSteerController::solve_dare(&a, &b, &q, &r);
560 for i in 0..5 {
562 assert!(x[(i, i)] >= 0.0);
563 }
564 }
565
566 #[test]
567 fn test_dlqr_gain() {
568 let a = Matrix5::identity();
569 let mut b = Matrix5x2::zeros();
570 b[(3, 0)] = 1.0;
571 b[(4, 1)] = 0.1;
572 let q = Matrix5::identity();
573 let r = Matrix2::identity();
574
575 let k = LQRSpeedSteerController::dlqr(&a, &b, &q, &r);
576 assert_eq!(k.nrows(), 2);
578 assert_eq!(k.ncols(), 5);
579 }
580
581 #[test]
582 fn test_compute_control_empty_path() {
583 let mut controller = LQRSpeedSteerController::with_defaults();
584 let state = LQRSpeedSteerVehicleState::new(0.0, 0.0, 0.0, 1.0, 0.5, 0.78);
585 let (delta, ind, e, th_e, accel) = controller.compute_control_full(&state);
586 assert_eq!(delta, 0.0);
587 assert_eq!(ind, 0);
588 assert_eq!(e, 0.0);
589 assert_eq!(th_e, 0.0);
590 assert_eq!(accel, 0.0);
591 }
592
593 #[test]
594 fn test_compute_control_straight_path() {
595 let mut controller = LQRSpeedSteerController::with_defaults();
596 let path =
597 Path2D::from_points((0..50).map(|i| Point2D::new(i as f64 * 0.5, 0.0)).collect());
598 controller.set_path_with_speed(path, 2.0);
599
600 let state = LQRSpeedSteerVehicleState::new(0.0, 0.0, 0.0, 1.0, 0.5, 0.78);
601 let (delta, _ind, _e, _th_e, _accel) = controller.compute_control_full(&state);
602 assert!(delta.abs() < 0.5);
604 }
605
606 #[test]
607 fn test_vehicle_state_update() {
608 let mut state = LQRSpeedSteerVehicleState::new(0.0, 0.0, 0.0, 1.0, 0.5, 0.78);
609 state.update(0.0, 0.0, 0.1);
610 assert!((state.x - 0.1).abs() < 1e-9);
611 assert!(state.y.abs() < 1e-9);
612 }
613
614 #[test]
615 fn test_vehicle_state_update_with_steer() {
616 let mut state = LQRSpeedSteerVehicleState::new(0.0, 0.0, 0.0, 1.0, 0.5, 0.78);
617 state.update(0.0, 0.1, 0.1);
618 assert!(state.x > 0.0);
620 assert!(state.yaw.abs() > 0.0);
621 }
622
623 #[test]
624 fn test_vehicle_state_steer_clamping() {
625 let mut state = LQRSpeedSteerVehicleState::new(0.0, 0.0, 0.0, 1.0, 0.5, 0.1);
626 state.update(0.0, 1.0, 0.1); let expected_yaw = 1.0 / 0.5 * (0.1_f64).tan() * 0.1;
629 assert!((state.yaw - expected_yaw).abs() < 1e-9);
630 }
631
632 #[test]
633 fn test_from_state2d() {
634 let s = State2D::new(1.0, 2.0, 0.5, 3.0);
635 let vs = LQRSpeedSteerVehicleState::from(s);
636 assert!((vs.x - 1.0).abs() < 1e-9);
637 assert!((vs.y - 2.0).abs() < 1e-9);
638 assert!((vs.yaw - 0.5).abs() < 1e-9);
639 assert!((vs.v - 3.0).abs() < 1e-9);
640 }
641
642 #[test]
643 fn test_to_state2d() {
644 let vs = LQRSpeedSteerVehicleState::new(1.0, 2.0, 0.5, 3.0, 0.5, 0.78);
645 let s = vs.to_state2d();
646 assert!((s.x - 1.0).abs() < 1e-9);
647 assert!((s.y - 2.0).abs() < 1e-9);
648 }
649
650 #[test]
651 fn test_is_goal_reached() {
652 let mut controller = LQRSpeedSteerController::with_defaults();
653 let path = Path2D::from_points(vec![Point2D::new(0.0, 0.0), Point2D::new(5.0, 0.0)]);
654 controller.set_path(path);
655
656 let far = LQRSpeedSteerVehicleState::new(0.0, 0.0, 0.0, 1.0, 0.5, 0.78);
657 assert!(!controller.is_goal_reached(&far));
658
659 let near = LQRSpeedSteerVehicleState::new(5.0, 0.1, 0.0, 0.0, 0.5, 0.78);
660 assert!(controller.is_goal_reached(&near));
661 }
662
663 #[test]
664 fn test_planning_straight_line() {
665 let mut controller = LQRSpeedSteerController::with_defaults();
666 let waypoints = vec![(0.0, 0.0), (5.0, 0.0), (10.0, 0.0)];
667 let result = controller.planning(waypoints, 2.0, 0.5);
668 assert!(result.is_some());
669 let traj = result.unwrap();
670 assert!(!traj.is_empty());
671 let (fx, fy) = traj.last().unwrap();
673 let dist = ((fx - 10.0).powi(2) + fy.powi(2)).sqrt();
674 assert!(dist < 2.0, "Final distance to goal: {}", dist);
675 }
676
677 #[test]
678 fn test_planning_with_curve() {
679 let mut controller = LQRSpeedSteerController::with_defaults();
680 let waypoints = vec![
681 (0.0, 0.0),
682 (6.0, -3.0),
683 (12.5, -5.0),
684 (10.0, 6.5),
685 (17.5, 3.0),
686 (20.0, 0.0),
687 (25.0, 0.0),
688 ];
689 let result = controller.planning(waypoints, 10.0 / 3.6, 0.1);
690 assert!(result.is_some());
691 let traj = result.unwrap();
692 assert!(traj.len() > 10);
693 }
694
695 #[test]
696 fn test_planning_too_few_waypoints() {
697 let mut controller = LQRSpeedSteerController::with_defaults();
698 let result = controller.planning(vec![(0.0, 0.0)], 2.0, 0.5);
699 assert!(result.is_none());
700 }
701
702 #[test]
703 fn test_path_tracker_trait() {
704 let mut controller = LQRSpeedSteerController::with_defaults();
705 let path =
706 Path2D::from_points((0..50).map(|i| Point2D::new(i as f64 * 0.5, 0.0)).collect());
707 let state = State2D::new(0.0, 0.0, 0.0, 1.0);
708 let input = controller.compute_control(&state, &path);
709 assert!(input.v.is_finite());
711 assert!(input.omega.is_finite());
712 }
713
714 #[test]
715 fn test_speed_profile_slowdown() {
716 let mut controller = LQRSpeedSteerController::with_defaults();
717 let path = Path2D::from_points(
718 (0..100)
719 .map(|i| Point2D::new(i as f64 * 0.1, 0.0))
720 .collect(),
721 );
722 controller.set_path_with_speed(path, 3.0);
723 let n = controller.speed_profile.len();
725 assert!(controller.speed_profile[n - 1] < 3.0);
726 assert!(controller.speed_profile[n - 1] > 0.0);
727 }
728
729 #[test]
730 fn test_get_target_speed() {
731 let mut controller = LQRSpeedSteerController::with_defaults();
732 let path = Path2D::from_points(vec![Point2D::new(0.0, 0.0), Point2D::new(1.0, 0.0)]);
733 controller.set_path_with_speed(path, 2.0);
734 let s = controller.get_target_speed(0);
736 assert!(s.is_finite());
737 assert_eq!(controller.get_target_speed(1000), 0.0);
739 }
740}