Consider an \( n \)-DOF robot manipulator with joint coordinates \( q \in \mathbb{R}^n \). The standard rigid-body dynamics are

\[ M(q)\ddot{q} + C(q,\dot{q})\dot{q} + g(q) = u, \]

where \( M(q) \) is the inertia matrix, \( C(q,\dot{q}) \) the Coriolis/centrifugal matrix, \( g(q) \) gravity, and \( u \in \mathbb{R}^n \) the joint torque input. We define the state

\[ x = \begin{bmatrix} q \\ \dot{q} \end{bmatrix} \in \mathbb{R}^{2n}. \]

Suppose we are given a smooth reference trajectory \( x_{\mathrm{ref}}(t) = [q_{\mathrm{ref}}(t)^\top, \dot{q}_{\mathrm{ref}}(t)^\top]^\top \) with associated nominal input \( u_{\mathrm{ref}}(t) \) that exactly satisfies the nonlinear dynamics. Our objective is to design a finite-horizon, time-varying state feedback \( u(t) = u_{\mathrm{ref}}(t) + \delta u(t) \) that minimizes a quadratic cost on tracking errors over \( t \in [0,T] \).

We linearize the dynamics around the trajectory and obtain an LTV (linear time-varying) model in the tracking-error coordinates \( \delta x(t) = x(t) - x_{\mathrm{ref}}(t) \), \( \delta u(t) = u(t) - u_{\mathrm{ref}}(t) \). The overall design flow is summarized in the following diagram.

flowchart TD
  N["Nonlinear manipulator dynamics M(q) qddot + C(q,qdot) qdot + g(q) = u"]
    --> R["Reference trajectory (q_ref(t), qdot_ref(t), u_ref(t))"]
  R --> L["Linearize around trajectory: x_dot = A(t) x + B(t) u in error coords"]
  L --> C["Choose Q(t), R(t), S_T for finite-horizon quadratic cost"]
  C --> P["Solve backward Riccati equation for P(t)"]
  P --> K["Compute time-varying gain K(t) = R(t)^(-1) B(t)^T P(t)"]
  K --> CL["Implement u(t) = u_ref(t) - K(t) (x(t) - x_ref(t))"]
        

In what follows we formalize the linearization, state the finite-horizon LQR problem, derive the Riccati equation, and then implement the resulting controller for a simple joint model.

From earlier chapters we know that the manipulator dynamics can be written as a first-order, nonlinear state-space system

\[ \dot{x}(t) = f(x(t),u(t),t) = \begin{bmatrix} \dot{q}(t) \\ M(q(t))^{-1}\big(u(t) - C(q(t),\dot{q}(t))\dot{q}(t) - g(q(t))\big) \end{bmatrix}. \]

Let \( x_{\mathrm{ref}}(t) \), \( u_{\mathrm{ref}}(t) \) be a nominal solution of this nonlinear system. The error dynamics in \( \delta x(t) = x(t) - x_{\mathrm{ref}}(t) \), \( \delta u(t) = u(t) - u_{\mathrm{ref}}(t) \) can be approximated by the first-order Taylor expansion:

\[ \delta \dot{x}(t) \approx A(t)\,\delta x(t) + B(t)\,\delta u(t), \]

where

\[ A(t) = \left. \frac{\partial f(x,u,t)}{\partial x} \right|_{x = x_{\mathrm{ref}}(t),\,u = u_{\mathrm{ref}}(t)},\quad B(t) = \left. \frac{\partial f(x,u,t)}{\partial u} \right|_{x = x_{\mathrm{ref}}(t),\,u = u_{\mathrm{ref}}(t)}. \]

For manipulator dynamics the expressions for \( A(t) \) and \( B(t) \) can be obtained analytically for simple arms, or numerically using robotics dynamics libraries (e.g., spatial-algebra based libraries that provide \( M(q) \), \( C(q,\dot{q}) \), \( g(q) \) and their partial derivatives).

The finite-horizon LQR problem will be formulated for these linearized error coordinates, so the controller is locally optimal around the chosen trajectory or equilibrium.

We consider the linearized error dynamics

\[ \delta \dot{x}(t) = A(t)\,\delta x(t) + B(t)\,\delta u(t), \quad t \in [0,T]. \]

The quadratic performance index for a finite horizon is

\[ J(\delta x_0,\delta u(\cdot)) = \delta x(T)^\top S_T \delta x(T) + \int_{0}^{T} \Big( \delta x(t)^\top Q(t)\,\delta x(t) + \delta u(t)^\top R(t)\,\delta u(t) \Big)\,dt, \]

where

• \( Q(t) \in \mathbb{R}^{2n \times 2n} \) is symmetric positive semidefinite,
• \( R(t) \in \mathbb{R}^{n \times n} \) is symmetric positive definite,
• \( S_T \in \mathbb{R}^{2n \times 2n} \) is the terminal cost matrix, symmetric positive semidefinite.

We seek the control policy \( \delta u^{\star} \) that minimizes \( J \) subject to the linear dynamics. The finite-horizon LQR solution gives an optimal time-varying linear feedback of the form

\[ \delta u^{\star}(t) = -K(t)\,\delta x(t),\quad K(t) = R(t)^{-1} B(t)^\top P(t), \]

where \( P(t) \) solves a matrix Riccati differential equation backward in time from \( t = T \) to \( t = 0 \). The closed-loop error dynamics become

\[ \delta \dot{x}(t) = \big(A(t) - B(t)K(t)\big)\,\delta x(t). \]

In terms of the original variables, \( u(t) = u_{\mathrm{ref}}(t) - K(t)\big(x(t) - x_{\mathrm{ref}}(t)\big) \) yields a locally optimal tracking controller over the finite time interval.

Let \( V(\delta x,t) \) denote the optimal cost-to-go starting from state \( \delta x \) at time \( t \). Dynamic programming gives the Hamilton–Jacobi–Bellman (HJB) equation

\[ 0 = \min_{\delta u} \left\{ \delta x^\top Q(t)\,\delta x + \delta u^\top R(t)\,\delta u + \frac{\partial V}{\partial x}(\delta x,t)^\top \big(A(t)\delta x + B(t)\delta u\big) + \frac{\partial V}{\partial t}(\delta x,t) \right\}, \]

with terminal condition \( V(\delta x,T) = \delta x^\top S_T \delta x \). Motivated by the quadratic structure of \( J \), we postulate the quadratic value function

\[ V(\delta x,t) = \delta x^\top P(t)\,\delta x, \]

where \( P(t) \) is a symmetric matrix to be determined. Then

\[ \frac{\partial V}{\partial x}(\delta x,t) = 2P(t)\,\delta x,\quad \frac{\partial V}{\partial t}(\delta x,t) = \delta x^\top \dot{P}(t)\,\delta x. \]

Substituting into the HJB equation, we obtain

\[ \begin{aligned} 0 &= \min_{\delta u} \Big\{ \delta x^\top Q(t)\,\delta x + \delta u^\top R(t)\,\delta u + 2\delta x^\top P(t) \big(A(t)\delta x + B(t)\delta u\big) + \delta x^\top \dot{P}(t)\,\delta x \Big\} \\ &= \min_{\delta u} \Big\{ \delta x^\top\big( Q(t) + A(t)^\top P(t) + P(t)A(t) + \dot{P}(t) \big)\delta x \\ &\qquad\qquad + 2\delta u^\top \big(R(t)\delta u + B(t)^\top P(t)\delta x\big) \Big\}. \end{aligned} \]

The minimizer with respect to \( \delta u \) is found from the first-order optimality condition

\[ \frac{\partial}{\partial \delta u} \Big( \delta u^\top R(t)\,\delta u + 2\delta u^\top B(t)^\top P(t)\delta x \Big) = 2R(t)\,\delta u + 2B(t)^\top P(t)\delta x = 0, \]

so that

\[ \delta u^{\star}(t) = -R(t)^{-1} B(t)^\top P(t)\,\delta x(t) = -K(t)\,\delta x(t). \]

Substituting back into the HJB equation eliminates the minimization and yields the Riccati differential equation for \( P(t) \):

\[ -\dot{P}(t) = A(t)^\top P(t) + P(t)A(t) - P(t)B(t)R(t)^{-1}B(t)^\top P(t) + Q(t),\quad P(T) = S_T. \]

Once \( P(t) \) is found by integrating this equation backward from \( t = T \) to \( t = 0 \), the optimal gain \( K(t) \) is obtained as \( K(t) = R(t)^{-1}B(t)^\top P(t) \).

In practice, robot controllers are implemented digitally with sampling period \( \Delta t \). A standard approach is to discretize the linearized dynamics to obtain

\[ x_{k+1} = A_k x_k + B_k u_k,\quad k = 0,\dots,N-1, \]

where \( A_k \), \( B_k \) are the discrete-time system matrices (possibly time-varying), and \( T = N\Delta t \). The discrete finite-horizon quadratic cost is

\[ J(x_0,\{u_k\}) = x_N^\top S_N x_N + \sum_{k=0}^{N-1} \big( x_k^\top Q_k x_k + u_k^\top R_k u_k \big), \]

with \( Q_k \succeq 0 \), \( R_k \succ 0 \), \( S_N \succeq 0 \). The optimal control has the form \( u_k^{\star} = -K_k x_k \) with gains obtained from the backward Riccati recursion:

\[ \begin{aligned} P_N &= S_N, \\ K_k &= \big(R_k + B_k^\top P_{k+1} B_k\big)^{-1} B_k^\top P_{k+1} A_k, \\ P_k &= Q_k + A_k^\top P_{k+1} A_k - A_k^\top P_{k+1} B_k K_k, \quad k = N-1,\dots,0. \end{aligned} \]

This recursion is numerically robust and well suited for trajectory optimization and time-varying LQR on manipulators. The algorithmic structure is illustrated below.

flowchart TD
  ST["Choose N, Q_k, R_k, S_N, and discrete A_k, B_k"] --> I["Initialize P_N = S_N"]
  I --> L["For k = N-1 down to 0"]
  L --> G["Compute K_k = (R_k + B_k^T P_{k+1} B_k)^(-1) B_k^T P_{k+1} A_k"]
  G --> U["Update P_k = Q_k + A_k^T P_{k+1} A_k - A_k^T P_{k+1} B_k K_k"]
  U --> L
  U --> OUT["Store K_k sequence for on-line control"]
        

In the simplest case of an equilibrium control problem, \( A_k = A \), \( B_k = B \), \( Q_k = Q \), \( R_k = R \) are constant, but the gain \( K_k \) still depends on the remaining horizon \( N-k \).

To make the implementation concrete, consider a single revolute joint of a manipulator modeled as a damped rigid rotor driven by torque \( u \):

\[ I\,\ddot{q} + b\,\dot{q} = u, \]

where \( I \) is the equivalent inertia and \( b \) the viscous damping coefficient. Let

\[ x = \begin{bmatrix} q \\ \dot{q} \end{bmatrix}, \quad \dot{x} = \begin{bmatrix} \dot{q} \\ \ddot{q} \end{bmatrix} = \begin{bmatrix} \dot{q} \\ -\frac{b}{I}\dot{q} + \frac{1}{I}u \end{bmatrix}. \]

Around the equilibrium \( q = 0, \dot{q} = 0, u = 0 \), the system is already linear, with

\[ A = \begin{bmatrix} 0 & 1 \\ 0 & -\frac{b}{I} \end{bmatrix}, \quad B = \begin{bmatrix} 0 \\ \frac{1}{I} \end{bmatrix}. \]

For a finite horizon \( T \), choose \( Q = \mathrm{diag}(q_w,\dot{q}_w) \), \( R = r_w \), and terminal cost \( S_T = Q \). Discretize with sampling period \( \Delta t \) to obtain \( A_d \), \( B_d \) (e.g. via forward Euler or more accurate matrix exponentials), then run the Riccati recursion to compute \( K_k \). The resulting control \( u_k = -K_k x_k \) stabilizes the joint and shapes its transient response over the finite horizon.

We implement the discrete-time Riccati recursion for the single-joint model using NumPy. For multi-DOF manipulators, one would first obtain \( A_k \), \( B_k \) along a reference trajectory using a robotics dynamics library (e.g., via functions that return \( M(q) \), \( C(q,\dot{q}) \), \( g(q) \) and then numerically approximate derivatives for the linearization).

import numpy as np

# Single-joint parameters
I = 0.5     # inertia
b = 0.1     # viscous damping

# Continuous-time A, B matrices
A = np.array([[0.0,      1.0],
              [0.0, -b / I]])
B = np.array([[0.0],
              [1.0 / I]])

# LQR weights
q_w = 100.0
qd_w = 10.0
r_w = 1.0
Q = np.diag([q_w, qd_w])
R = np.array([[r_w]])

# Horizon and discretization
T = 2.0           # total time [s]
dt = 0.002        # sampling period [s]
N = int(T / dt)   # number of steps

# Simple forward-Euler discretization (for illustration)
n = A.shape[0]
Ad = np.eye(n) + A * dt
Bd = B * dt

# Backward Riccati recursion
P = np.zeros((N + 1, n, n))
K = np.zeros((N, B.shape[1], n))

# Terminal cost
P[N] = Q.copy()

for k in range(N - 1, -1, -1):
    P_next = P[k + 1]
    # Compute gain K_k
    S = R + Bd.T @ P_next @ Bd
    K[k] = np.linalg.solve(S, Bd.T @ P_next @ Ad)
    # Update P_k
    P[k] = Q + Ad.T @ P_next @ Ad - Ad.T @ P_next @ Bd @ K[k]

# Simulation of closed-loop system
x = np.array([[0.3],   # initial position error [rad]
              [0.0]])  # initial velocity error [rad/s]

traj = []
for k in range(N):
    u = -K[k] @ x
    dx = A @ x + B @ u
    x = x + dt * dx
    traj.append((k * dt, float(x[0, 0]), float(x[1, 0]), float(u[0, 0])))

# 'traj' holds time, position, velocity, torque; can be plotted or exported
      

For a full manipulator, one would replace the constant A, B by time-varying arrays A_k, B_k obtained from linearization along a reference trajectory (q_ref[k], qd_ref[k], u_ref[k]) computed with a robotics dynamics toolbox.

In C++, it is natural to use Eigen for linear algebra and a robot dynamics library (e.g., based on Featherstone's spatial vector algebra) to obtain \( A_k \), \( B_k \) for multi-DOF arms. Below is a minimal implementation of the backward Riccati recursion for the single-joint example.

#include <Eigen/Dense>
#include <vector>

struct LQRResult {
    std::vector<Eigen::MatrixXd> P; // P_k, size N+1
    std::vector<Eigen::MatrixXd> K; // K_k, size N
};

LQRResult finiteHorizonLQR(const Eigen::MatrixXd& Ad,
                           const Eigen::MatrixXd& Bd,
                           const Eigen::MatrixXd& Q,
                           const Eigen::MatrixXd& R,
                           int N)
{
    const int n = Ad.rows();
    const int m = Bd.cols();

    LQRResult res;
    res.P.resize(N + 1);
    res.K.resize(N);

    // Terminal cost
    res.P[N] = Q;

    for (int k = N - 1; k >= 0; --k) {
        const Eigen::MatrixXd& Pnext = res.P[k + 1];
        Eigen::MatrixXd S = R + Bd.transpose() * Pnext * Bd;
        Eigen::MatrixXd Kk = S.ldlt().solve(Bd.transpose() * Pnext * Ad);
        res.K[k] = Kk;

        res.P[k] = Q + Ad.transpose() * Pnext * Ad
                     - Ad.transpose() * Pnext * Bd * Kk;
    }

    return res;
}

int main()
{
    double I = 0.5;
    double b = 0.1;
    double dt = 0.002;
    double T  = 2.0;
    int N = static_cast<int>(T / dt);

    Eigen::Matrix2d A;
    A << 0.0, 1.0,
          0.0, -b / I;
    Eigen::Vector2d B;
    B << 0.0, 1.0 / I;

    Eigen::Matrix2d Ad = Eigen::Matrix2d::Identity() + A * dt;
    Eigen::MatrixXd Bd(2,1);
    Bd.col(0) = B * dt;

    Eigen::Matrix2d Q;
    Q.setZero();
    Q(0,0) = 100.0;
    Q(1,1) = 10.0;

    Eigen::MatrixXd R(1,1);
    R(0,0) = 1.0;

    LQRResult res = finiteHorizonLQR(Ad, Bd, Q, R, N);

    // Example closed-loop simulation
    Eigen::Vector2d x(0.3, 0.0); // initial state
    for (int k = 0; k < N; ++k) {
        Eigen::MatrixXd u = -res.K[k] * x;
        Eigen::Vector2d dx = A * x + B * u(0,0);
        x += dt * dx;
        // store or log x, u as needed
    }
    return 0;
}
      

For a full manipulator, A and B would be generated from the dynamics library at each linearization point along the reference trajectory, and the same Riccati routine can be reused.

In Java, a convenient choice for matrix computations is EJML. We again implement the backward Riccati recursion for the single-joint model.

import org.ejml.data.DMatrixRMaj;
import org.ejml.dense.row.CommonOps_DDRM;
import org.ejml.dense.row.decomposition.chol.CholeskyDecomposition_DDRM;
import org.ejml.dense.row.decomposition.chol.CholeskyDecompositionInner_DDRM;

public class FiniteHorizonLQR {

    public static class Result {
        public DMatrixRMaj[] P;
        public DMatrixRMaj[] K;
    }

    public static Result lqr(DMatrixRMaj Ad,
                             DMatrixRMaj Bd,
                             DMatrixRMaj Q,
                             DMatrixRMaj R,
                             int N) {

        int n = Ad.numRows;
        int m = Bd.numCols;

        Result res = new Result();
        res.P = new DMatrixRMaj[N + 1];
        res.K = new DMatrixRMaj[N];

        res.P[N] = Q.copy();

        DMatrixRMaj Bt = new DMatrixRMaj(m, n);
        DMatrixRMaj At = new DMatrixRMaj(n, n);
        CommonOps_DDRM.transpose(Bd, Bt);
        CommonOps_DDRM.transpose(Ad, At);

        for (int k = N - 1; k >= 0; --k) {
            DMatrixRMaj Pnext = res.P[k + 1];
            DMatrixRMaj BtPnext = new DMatrixRMaj(m, n);
            CommonOps_DDRM.mult(Bt, Pnext, BtPnext);

            DMatrixRMaj S = new DMatrixRMaj(m, m);
            DMatrixRMaj BtPnextB = new DMatrixRMaj(m, m);
            CommonOps_DDRM.mult(BtPnext, Bd, BtPnextB);
            CommonOps_DDRM.add(R, BtPnextB, S);

            // Solve S * Kk = BtPnext * Ad
            DMatrixRMaj BtPnextAd = new DMatrixRMaj(m, n);
            CommonOps_DDRM.mult(BtPnext, Ad, BtPnextAd);

            DMatrixRMaj Kk = BtPnextAd.copy();
            CholeskyDecomposition_DDRM chol = new CholeskyDecompositionInner_DDRM();
            chol.decompose(S);
            chol.solve(Kk); // Kk = S^{-1} * BtPnextAd

            res.K[k] = Kk;

            // P_k = Q + A^T P_{k+1} A - A^T P_{k+1} B Kk
            DMatrixRMaj APnext = new DMatrixRMaj(n, n);
            CommonOps_DDRM.mult(Pnext, Ad, APnext);
            DMatrixRMaj AtPnextAd = new DMatrixRMaj(n, n);
            CommonOps_DDRM.mult(At, APnext, AtPnextAd);

            DMatrixRMaj BK = new DMatrixRMaj(n, n);
            CommonOps_DDRM.mult(Bd, Kk, BK);
            DMatrixRMaj PnextBK = new DMatrixRMaj(n, n);
            CommonOps_DDRM.mult(Pnext, BK, PnextBK);
            DMatrixRMaj AtPnextBK = new DMatrixRMaj(n, n);
            CommonOps_DDRM.mult(At, PnextBK, AtPnextBK);

            DMatrixRMaj Pk = Q.copy();
            CommonOps_DDRM.addEquals(Pk, AtPnextAd);
            CommonOps_DDRM.subtractEquals(Pk, AtPnextBK);
            res.P[k] = Pk;
        }

        return res;
    }

    public static void main(String[] args) {
        double I = 0.5;
        double b = 0.1;
        double dt = 0.002;
        double T = 2.0;
        int N = (int)(T / dt);

        DMatrixRMaj A = new DMatrixRMaj(2, 2, true,
                0.0, 1.0,
                0.0, -b / I);
        DMatrixRMaj B = new DMatrixRMaj(2, 1, true,
                0.0,
                1.0 / I);

        DMatrixRMaj Ad = A.copy();
        CommonOps_DDRM.scale(dt, Ad);
        CommonOps_DDRM.addEquals(Ad, 1.0, CommonOps_DDRM.identity(2));

        DMatrixRMaj Bd = B.copy();
        CommonOps_DDRM.scale(dt, Bd);

        DMatrixRMaj Q = new DMatrixRMaj(2, 2, true,
                100.0, 0.0,
                0.0, 10.0);
        DMatrixRMaj R = new DMatrixRMaj(1, 1, true, 1.0);

        Result res = lqr(Ad, Bd, Q, R, N);

        // Closed-loop simulation can then be carried out as in the C++ example.
    }
}
      

Again, an industrial manipulator control stack would obtain Ad, Bd along a trajectory using a robot dynamics layer, and then perform the same Riccati recursion in Java if the control logic is implemented on a JVM-based platform.

10.1 MATLAB / Simulink

MATLAB provides excellent support for LQR design. For finite-horizon LQR, we typically implement the discrete backward Riccati recursion explicitly (in contrast to the standard infinite-horizon lqr command).

% Single-joint parameters
I = 0.5;
b = 0.1;

A = [0 1;
     0 -b/I];
B = [0;
     1/I];

% Discretization
dt = 0.002;
T  = 2.0;
N  = round(T/dt);

Ad = eye(2) + A*dt;
Bd = B*dt;

% Weights
Q = diag([100 10]);
R = 1;

% Backward Riccati recursion
P = zeros(2,2,N+1);
K = zeros(1,2,N);

P(:,:,N+1) = Q;   % terminal cost

for k = N:-1:1
    Pnext = P(:,:,k+1);
    S = R + Bd'*Pnext*Bd;
    K(:,:,k) = (S \ (Bd'*Pnext*Ad));
    P(:,:,k) = Q + Ad'*Pnext*Ad - Ad'*Pnext*Bd*K(:,:,k);
end

% Closed-loop simulation
x = [0.3; 0.0];   % initial state
x_log = zeros(2,N);
u_log = zeros(1,N);

for k = 1:N
    u = -squeeze(K(:,:,k)) * x;
    x = x + dt*(A*x + B*u);
    x_log(:,k) = x;
    u_log(:,k) = u;
end
      

In Simulink, a convenient implementation strategy is:

• Precompute the gain sequence \( K_k \) offline in MATLAB and store it in a workspace variable.
• Use a Discrete-Time Integrator or State-Space block to simulate the plant dynamics.
• Feed the state measurement into a custom block that:
  • Reads the current time step index \( k \),
  • Fetches \( K_k \) from the precomputed array (e.g. via a Lookup Table or MATLAB Function block),
  • Outputs \( u_k = -K_k x_k \).

This structure directly realizes the finite-horizon time-varying feedback for the discrete model.

10.2 Wolfram Mathematica

In Wolfram Mathematica, we can implement the Riccati recursion using standard list and matrix operations. Below is an example for the single-joint model using forward-Euler discretization as before.

(* Parameters *)
I  = 0.5;
b  = 0.1;
dt = 0.002;
T  = 2.0;
nSteps = Round[T/dt];

(* Continuous-time system *)
A = { {0, 1},
     {0, -b/I} };
B = { {0},
     {1/I} };

(* Discrete-time system (Euler) *)
Ad = IdentityMatrix[2] + dt*A;
Bd = dt*B;

Q = DiagonalMatrix[{100, 10}];
R = {{1}};

(* Initialize arrays *)
P = ConstantArray[0, {nSteps + 1, 2, 2}];
K = ConstantArray[0, {nSteps, 1, 2}];

(* Terminal cost *)
P[[nSteps + 1]] = Q;

Do[
  Module[{Pnext, S, Kk},
    Pnext = P[[k + 1]];
    S = R + Transpose[Bd].Pnext.Bd;
    Kk = Inverse[S].Transpose[Bd].Pnext.Ad;
    K[[k]] = Kk;
    P[[k]] = Q + Transpose[Ad].Pnext.Ad - Transpose[Ad].Pnext.Bd.Kk;
  ],
  {k, nSteps, 1, -1}
];

(* Closed-loop simulation *)
x0   = {0.3, 0.0};
xCur = x0;
traj = Table[
   Module[{u, dx},
     u  = -K[[k]].xCur;
     dx = A.xCur + B.u;
     xCur = xCur + dt*dx;
     {k*dt, xCur[[1]], xCur[[2]], u[[1,1]]}
   ],
   {k, 1, nSteps}
];
      

For more complex manipulators, one can couple this Riccati routine with symbolic or numerical models of \( M(q) \), \( C(q,\dot{q}) \), \( g(q) \) constructed in Mathematica, and generate \( A_k \), \( B_k \) via automatic differentiation or finite differences.

Problem 1 (Scalar finite-horizon LQR): Consider the scalar system \( \dot{x}(t) = a x(t) + b u(t) \) on \( t \in [0,T] \) with cost

\[ J(x_0,u(\cdot)) = s_T x(T)^2 + \int_{0}^{T} \big( q x(t)^2 + r u(t)^2 \big)\,dt, \]

where \( q \ge 0 \), \( r > 0 \), \( s_T \ge 0 \). Derive the scalar Riccati equation for \( p(t) \) such that \( V(x,t) = p(t)x^2 \) is the optimal cost-to-go, and show that the optimal feedback is \( u(t) = -\frac{b}{r}p(t)x(t) \).

Solution:

Postulate \( V(x,t) = p(t)x^2 \), so that \( \frac{\partial V}{\partial x} = 2p(t)x \) and \( \frac{\partial V}{\partial t} = \dot{p}(t)x^2 \). The HJB equation is

\[ 0 = \min_{u} \big\{ qx^2 + r u^2 + 2p(t)x(a x + b u) + \dot{p}(t)x^2 \big\}. \]

Differentiating with respect to \( u \) gives \( 2r u + 2p(t)b x = 0 \), hence \( u^{\star}(t) = -\frac{b}{r}p(t)x(t) \). Substituting back,

\[ \begin{aligned} 0 &= qx^2 + r\left(\frac{b^2}{r^2}p(t)^2 x^2\right) + 2p(t)x\big(a x - \frac{b^2}{r}p(t)x\big) + \dot{p}(t)x^2 \\ &= x^2\Big( q + \dot{p}(t) + 2a p(t) - \frac{b^2}{r}p(t)^2 \Big). \end{aligned} \]

Thus \( p(t) \) satisfies the scalar Riccati differential equation

\[ -\dot{p}(t) = 2a p(t) - \frac{b^2}{r}p(t)^2 + q,\quad p(T) = s_T. \]

Together with the feedback expression \( u^{\star}(t) = -\frac{b}{r}p(t)x(t) \), this solves the scalar finite-horizon LQR problem.

Problem 2 (Monotonicity of Riccati solution): Let \( A,B,Q,R \) be constant matrices with \( Q \succeq 0 \), \( R \succ 0 \). Consider the continuous Riccati equation on \( [0,T] \) with terminal condition \( P(T) = S_T \succeq 0 \). Show that if we decrease the horizon (replace \( T \) by \( \hat{T} < T \) while keeping \( S_T \) fixed as the terminal condition at \( \hat{T} \)), then the solution \( P(t) \) at any fixed intermediate time increases in the Loewner order (i.e., shorter horizon yields larger matrices).

Solution (sketch):

The value function \( V(x,t) = x^\top P(t)x \) represents the minimal cost from time \( t \) to the terminal time. If we shorten the horizon, fewer stages of the running cost contribute, hence the cost-to-go can only increase. Formally, for any fixed \( t \), the cost for the shortened horizon \( \hat{T} \) is

\[ V_{\hat{T}}(x,t) = \min_{u(\cdot)} \left[ x(\hat{T})^\top S_T x(\hat{T}) + \int_{t}^{\hat{T}} \big(x(\sigma)^\top Q x(\sigma) + u(\sigma)^\top R u(\sigma)\big)\,d\sigma \right], \]

whereas for horizon \( T \) we add a nonnegative integral from \( \hat{T} \) to \( T \). Therefore \( V_{\hat{T}}(x,t) \ge V_{T}(x,t) \) for all \( x \), which implies \( x^\top P_{\hat{T}}(t) x \ge x^\top P_T(t) x \) for all \( x \), hence \( P_{\hat{T}}(t) - P_T(t) \succeq 0 \).

Problem 3 (Decoupled joints and block-diagonal LQR): Suppose a manipulator has dynamics that decouple per joint around an operating point so that the linearized state-space matrices have block-diagonal form \( A = \mathrm{diag}(A_1,\dots,A_n) \), \( B = \mathrm{diag}(B_1,\dots,B_n) \). Take \( Q = \mathrm{diag}(Q_1,\dots,Q_n) \) and \( R = \mathrm{diag}(R_1,\dots,R_n) \). Show that the finite-horizon LQR gains are likewise block-diagonal and can be computed by solving \( n \) independent LQR problems.

Solution:

Because the system and costs decompose across joints, the total cost is the sum

\[ J = \sum_{i=1}^{n} \left( x_{i,N}^\top S_{i,N} x_{i,N} + \sum_{k=0}^{N-1} \big( x_{i,k}^\top Q_i x_{i,k} + u_{i,k}^\top R_i u_{i,k} \big) \right), \]

where \( x_{i,k} \), \( u_{i,k} \) denote the state and input of joint \( i \). The dynamics \( x_{i,k+1} = A_i x_{i,k} + B_i u_{i,k} \) are uncoupled across joints. Therefore, the optimal control problem splits into \( n \) independent LQR problems, each with its own Riccati recursion. The resulting \( P_k \) and \( K_k \) are block-diagonal, with joint-wise blocks \( P_{i,k} \), \( K_{i,k} \). Hence for decoupled joints, finite-horizon LQR can be designed joint-by-joint.

Problem 4 (Continuous vs discrete Riccati for small sampling time): Consider a time-invariant system discretized with sampling period \( \Delta t \) using \( A_d = I + A\Delta t \), \( B_d = B\Delta t \), and discrete-time Riccati recursion with \( Q_d = Q\Delta t \), \( R_d = R\Delta t \). Show formally (using first-order expansions) that as \( \Delta t \to 0 \), the discrete Riccati recursion approaches the continuous-time Riccati differential equation.

Solution (outline):

Write the discrete recursion for constant matrices as

\[ P_k = Q_d + A_d^\top P_{k+1} A_d - A_d^\top P_{k+1} B_d \big(R_d + B_d^\top P_{k+1}B_d\big)^{-1} B_d^\top P_{k+1}A_d. \]

Substitute \( A_d = I + A\Delta t \), \( B_d = B\Delta t \), \( Q_d = Q\Delta t \), \( R_d = R\Delta t \) and expand all terms up to first order in \( \Delta t \). Using \( P_k - P_{k+1} \approx -\dot{P}(t_k)\Delta t \) and rearranging terms, one recovers

\[ -\dot{P}(t) = A^\top P(t) + P(t)A - P(t)B R^{-1} B^\top P(t) + Q, \]

which is exactly the continuous-time Riccati equation. Thus the discrete recursion can be interpreted as an Euler integration of the continuous Riccati differential equation.

In this lesson we formulated the finite-horizon LQR problem for manipulator control by linearizing the nonlinear dynamics around a reference trajectory and penalizing tracking errors via a quadratic functional. Using dynamic programming, we derived the Riccati differential equation and its discrete-time counterpart, and we implemented the resulting time-varying feedback gains in several programming environments. These constructions provide the foundation for nonlinear optimal control methods such as DDP and iLQR, which iteratively refine trajectories using repeated LQR subproblems.