lqr

使用nalgebra进行大量优化的Rust语言通用的线性-二次调节器（LQR）控制器。它可以作为各种动态系统的反馈控制器/轨迹跟踪器使用，并在汽车和四旋翼机上进行了实际测试。

最小示例

使用简单的运动自行车模型控制汽车

// Define state
let x = 2.0;
let y = 2.0;
let theta = 0.34;
let v = 3.0;

// Define controls
let delta = 0.0;
let acc = 0.0;

// Model parameters
let l = 2.0; // wheelbase

// compute matrices for the LQR controller
let a = na::Matrix4::<f64>::new(0.0, 0.0, -v * theta.sin(), theta.cos(),
                                0.0, 0.0, v * theta.cos(),  theta.sin(),
                                0.0, 0.0, 0.0,              delta.tan() / l,
                                0.0, 0.0, 0.0,              0.0);

let b = na::Matrix4x2::<f64>::new(0.0, 0.0,
                                  0.0, 0.0,
                                  v / (l * delta.cos().powf(2.0)), 0.0,
                                  0.0, 1.0);

let q = na::Matrix4::identity();
let r = na::Matrix2::identity();

let mut controller = LQRController::new()?;
controller.compute_gain(&a, &b, &q, &r, 1e-6)?;

// Set states for the optimal control computation
let current_state = na::Vector4::<f64>::new(x, y, theta, v);
let desired_state = na::Vector4::<f64>::new(x + 1.0, y, theta + 0.01, 3.0);

let u_feedforward = na::Vector2::<f64>::new(acc, delta);
let u_feedback = controller.compute_optimal_controls(&current_state, &desired_state)?;
let u = u_feedforward + u_feedback;