Linear-Quadratic Regulator Rapidly-Exploring Random Tree
The Linear-Quadratic Regulator Rapidly-Exploring Random Tree (LQR-RRT) is a sampling-based kinodynamic motion planning algorithm that integrates asymptotic optimality with linear-quadratic regulator feedback controllers as local steering functions. By accounting for differential equations of motion, actuator limits, and state constraints directly within the tree expansion phase, LQR-RRT effectively navigates complex high-dimensional non-linear dynamical systems while minimizing a quadratic cost functional.
HOW-TO IMPLEMENT & CONFIGURE
- Modern C++ compiler supporting C++17 (GCC 9+, Clang 10+, MSVC 2019+)
- Eigen 3.3+ linear algebra library for matrix operations and Riccati equation solvers
- Boost 1.70+ for optional spatial indexing and graph structures
- Python 3.10+ with NumPy and Matplotlib for rapid offline validation and visualization
Architectural Setup & Environment Initialization
Define the system dynamics matrices (A, B) for the linear or linearized plant, the quadratic cost weights (Q, R) for the LQR tracking error and control effort, and state space boundaries with obstacle geometry structures.
#include <Eigen/Dense>
#include <vector>
struct SystemMatrices {
Eigen::MatrixXd A;
Eigen::MatrixXd B;
Eigen::MatrixXd Q;
Eigen::MatrixXd R;
};
struct Node {
Eigen::VectorXd state;
Eigen::VectorXd control;
double cost_to_come;
int parent_index;
};
void initialize_environment(SystemMatrices& sys, int state_dim, int control_dim) {
sys.A = Eigen::MatrixXd::Identity(state_dim, state_dim);
sys.B = Eigen::MatrixXd::Zero(state_dim, control_dim);
sys.Q = Eigen::MatrixXd::Identity(state_dim, state_dim);
sys.R = Eigen::MatrixXd::Identity(control_dim, control_dim);
}Core LQR-RRT Algorithm Implementation
Implement the iterative sampling loop: sample a random state, find the nearest node in the tree using a distance metric, solve the LQR finite-horizon boundary value problem to steer towards the sample, validate collision constraints, and append the node to the tree.
#include <iostream>
#include <vector>
#include <Eigen/Dense>
class LQRRRTPlanner {
public:
Eigen::VectorXd sample_random_state() {
// Uniform random sampling within state bounds
return Eigen::VectorXd::Random(4);
}
int find_nearest_neighbor(const std::vector<Node>& tree, const Eigen::VectorXd& sample) {
int best_idx = 0;
double min_dist = std::numeric_limits<double>::max();
for (size_t i = 0; i < tree.size(); ++i) {
double dist = (tree[i].state - sample).norm();
if (dist < min_dist) {
min_dist = dist;
best_idx = static_cast<int>(i);
}
}
return best_idx;
}
bool steer_with_lqr(const Eigen::VectorXd& x_near, const Eigen::VectorXd& x_rand, Eigen::VectorXd& u_out) {
// Simplified LQR steering approximation
u_out = -0.1 * (x_near - x_rand).head(2);
return true;
}
};
Verification, Profiling & Unit Testing
Write automated unit tests verifying LQR gain matrix convergence via the Algebraic Riccati Equation (ARE) solver, trajectory boundary condition satisfaction, and memory consumption bounds under high iteration limits.
#include <Eigen/Dense>
#include <iostream>
bool verify_are_convergence(const Eigen::MatrixXd& A, const Eigen::MatrixXd& B, const Eigen::MatrixXd& Q, const Eigen::MatrixXd& R) {
// Simple check for stability or placeholder fordare solver verification
Eigen::MatrixXd P = Q;
for(int i = 0; i < 100; ++i) {
P = Q + A.transpose() * P * A - (A.transpose() * P * B) * (R + B.transpose() * P * B).inverse() * (B.transpose() * P * A);
}
return P.allFinite();
}
int main() {
Eigen::MatrixXd A(2,2); A << 1, 1, 0, 1;
Eigen::MatrixXd B(2,1); B << 0, 1;
Eigen::MatrixXd Q(2,2); Q << 1, 0, 0, 1;
Eigen::MatrixXd R(1,1); R << 1;
bool ok = verify_are_convergence(A, B, Q, R);
std::cout << "ARE Verification: " << (ok ? "PASSED" : "FAILED") << std::endl;
return 0;
}Production Hardening & Common Failure Modes
Mitigate numerical instability in LQR matrix inversions by adding Tikhonov regularization (ridge regression), guard against degenerate state transitions, and enforce strict execution time caps for real-time robotic loops.
Eigen::MatrixXd safe_inverse(const Eigen::MatrixXd& M, double lambda = 1e-6) {
Eigen::MatrixXd reg_M = M + lambda * Eigen::MatrixXd::Identity(M.rows(), M.cols());
return reg_M.inverse();
}
Mathematical Formulation of LQR-RRT
LQR-RRT addresses the kinodynamic motion planning problem for linear time-invariant (LTI) systems governed by the differential equation: dx/dt = Ax + Bu, subject to state constraints x in X_free and control constraints u in U_adm. The cost functional minimized along trajectories is defined as J = integral from 0 to T of (x^T Q x + u^T R u) dt + x(T)^T P_f x(T). The local steering procedure solves the finite-horizon optimal control problem using the Linear-Quadratic Regulator framework, generating dynamically feasible trajectories that respect the system's inertia and actuation limits.
Asymptotic Optimality and Convergence Properties
Similar to RRT*, LQR-RRT can be extended to LQR-RRT* to ensure probabilistic completeness and asymptotic optimality. As the number of samples N approaches infinity, the cost of the best path found by LQR-RRT* converges almost surely to the global optimum of the constrained optimal control problem, provided that the steering function is locally controllable and metric space properties are satisfied.
Computational Complexity & Optimization Strategies
Naively implemented, finding the nearest neighbor via linear scan incurs O(N) time per iteration, leading to O(N^2) total building time. Integrating spatial partitioning data structures such as KD-Trees reduces nearest-neighbor lookup to O(log N), yielding an overall expected runtime complexity of O(N log N). However, constructing valid distance metrics (metrics that account for LQR cost-to-go rather than simple Euclidean distance) remains computationally demanding and often requires pre-computing LQR cost lookup tables.
📚 STANDARDS & ACADEMIC RFC CITATIONS
- Karaman, S., & Frazzoli, E. (2011). Sampling-based algorithms for optimal motion planning. The International Journal of Robotics Research, 30(7), 846-894.
- Webb, J., & van der Berg, J. (2013). Kinodynamic RRT*: Asymptotically optimal motion planning for systems with linear differential constraints. IEEE International Conference on Robotics and Automation (ICRA).
- LaValle, S. M. (2006). Planning Algorithms. Cambridge University Press.