---
title: LQR Controller Design with 2DOF Robot Arm
image: https://i.imgur.com/zJ97YeF.jpg
---

# LQR Controller Design with 2DOF Robot Arm
>This is an experiment for ELEC 4631 Continuous-time control system at UNSW.

## Introduction
The purpose of this experiment is to design an LQR controller and control a 2 DOF robot arm (Flexible Joint Robot, shown in figure below) [1] with it. Thus article will first start with a Matlab simulation and then implement the result of LQR on the robot.


## Linear Quadratic Regulator (LQR)
In this section, the complex calculation of LQR would **not** be discussed. I will explain it in a straightforward way.

### What is LQR?
Linear Quadratic Regulator is a feedback controller that optimizes the feedback gain. It is useful when designing a controller in the real world since there are always some constraints for a physical system. Before explaining more about LQR, an equation of it need to be introduced.

Consider a state-space model:
:::success
$\begin{gather*}
\dot{x}(t)=Ax(t)+Bu(t)
\end{gather*}$

$A\in\mathbb{R}^{n \times n}$, $B\in\mathbb{R}^{n \times n}$, $u(t)\in\mathbb{R}$
:::
A linear quadratic (LQ) perfomance index $J_{t_{f}}$ (also called cost function) is introduced below.
:::success
$\begin{gather*}
J_{t_{f}}(x_{0},u)=\int_{0}^{t_{f}}(x(t)^{T}Qx(t)+Ru(t)^2)dt+x(t_{f})^{T}Px(t_{f})
\end{gather*}$
:::
$x(t)$ is state and $u(t)$ is input. $t_{f}$ is control horizon (It's the time).

>A good video for explanation of state-space:
[Introduction to State-Space Equations | State Space, Part 1](https:// "[title](https://www.youtube.com/watch?v=hpeKrMG-WP0&ab_channel=MATLAB)")

### What's the meaning of the cost function
The purpose of LQR is to minimize the cost function by setting a proper Q and R.

For Q, it is the penalty value of states. The bigger the Q, the bigger the value of the cost function. It results in a faster response of a system.

When Q is large, it means that even a small change of states $x(t)$ will cause the value of the cost function to increase a lot. Thus, the system would try to reach the reference point as soon as possible (less change of states), resulting in a fast response.

It is the opposite for R. The bigger the R, the slower the system responds. It can be explained that a larger R means more penalty when receiving external energy (control input $u(t)$). The system would prefer not to obtain a large input value. A small value of input would cause the system to be slower.

| | result|
|-----|--------|
|Q $\uparrow$ |faster|
|Q $\downarrow$|slower|
|R $\uparrow$ |slower|
|R $\downarrow$|faster|

## Simulation
In this section, the effect of Q and R will be observed by the Matlab simulation.

### Build Model
In this experiment, we only control the first stage of the arm.
(1)
Fisrt Stage ↑
(2)
Free body diagram ↑

The dynamic equation can be obtained:
:::success
$J_{11}\ddot{\theta}_{11}-K_{s1}\theta_{12}+K_{s1}\theta_{11}=K_{t1}I_{m1}-B_{11}\dot{\theta}_{11}$

$J_{12}\ddot{\theta}_{12}+K_{s1}\theta_{12}-K_{s1}\theta_{11}=-B_{12}\dot{\theta}_{12}$
:::
With Matlab, the system can be easy to be illustrated, we define
$x = [x_1 x_2 x_3 x_4]^T=[\theta_{11} \theta_{12} \dot{\theta}_{11} \dot{\theta}_{12}]^T$
$u=I_{m1}$

The state space model is built in following code:

% parameters (given by lab tutor)
j_11 = 0.06373091;
j_12 = 0.23041858;
k_s1 = 9;
k_t1 = 8.925;
b_11 = 7.5;
b_12 = 0.070364;

% model
A = [0 0 1 0;
     0 0 0 1;
    -k_s1/j_11 k_s1/j_11 -b_11/j_11 0;
     k_s1/j_12 -k_s1/j_12 0 -b_12/j_12];
B = [0;0;k_t1/j_11;0];
C = eye(4);
D = 0;
sys = ss(A,B,C,D);

### LQR
The goal is to move the first stage of arm by $15^{\circ}$

By Bryson's rule, the Q and R can be determined. Since we are trying to let $\theta_{11}$ and $\theta_{12}$ go to zero with non-zero initial condition and $\dot{\theta}_{11}$, $\dot{\theta}_{12}$ are zero in the beginning. The Q would be

$Q=\left[
\begin{array}{cccc}
\frac{\bar{q}_1}{(\pi/12)^2} & 0 & 0 & 0\\
0 & \frac{\bar{q}_2}{(\pi/12)^2} & 0 & 0\\
0 & 0 & 0 & 0 \\
0 & 0 & 0 & 0
\end{array}
\right]$

$R = \frac{\bar{R}}{0.944^2}$

The MATLAB code is shown below.

q1_bar = 2.5;
q2_bar = 20;
R_bar = 2;
Q = [q1_bar/((pi/12)^2) 0 0 0;0 q2_bar/((pi/12)^2) 0 0;0 0 0 0;0 0 0 0];
R = R_bar/(0.944^2);

Q_sqrt = sqrt(Q);

obsv_m = [Q_sqrt; Q_sqrt*A; Q_sqrt*A*A; Q_sqrt*A*A*A];
disp('rank of (Q^(1/2), A)pair:');
disp(rank(obsv_m));

[K1,S1,e1] = lqr(sys,Q,R);

## Implementation
{%youtube OB1OZ1UpxNw %}

##### Fast Response (Large Q Small R)
##### Slow Response (Small Q Large R)

## Reference
[1] Quanser. 2022. 2 DOF Serial Flexible Joint - Quanser. Available at: <https://www.quanser.com/products/2-dof-serial-flexible-joint/attachment/dscn9132/>.
[2] Hendra Nurdin, “Lab3 Note,” 2022.
[3] Linear-Quadratic Regulator (LQR) design - MATLAB lqr. 2022. Available at: <https://au.mathworks.com/help/control/ref/lqr.html>.