{"id":105,"date":"2024-08-16T16:37:32","date_gmt":"2024-08-16T16:37:32","guid":{"rendered":"https:\/\/libraryresources.nse.org.ng\/robotics\/chapter\/chapter-13\/"},"modified":"2026-03-16T14:23:50","modified_gmt":"2026-03-16T14:23:50","slug":"chapter-13","status":"publish","type":"chapter","link":"https:\/\/libraryresources.nse.org.ng\/robotics\/chapter\/chapter-13\/","title":{"raw":"Linear Quadratic Controller","rendered":"Linear Quadratic Controller"},"content":{"raw":"[latexpage]\n

1) Motivation for the Linear Quadratic Controller<\/strong><\/h1>\nThe Linear Quadratic Controller (LQR) and the Model Predicative Controller (MPC) are classical optimal controllers. They are effective and efficient in controlling linear systems. Both of them have been proven by theoretical and real-world control applications. It has been shown that the two controllers offer the best performance for real-world control systems.\n\nLQR needs to work in the discrete state space because it is required to optimize the control function.\u00a0 So before we discuss the LQR controller, we need to first convert the state space to the discrete state space as shown in Figure 13.1.\u00a0 Why do we need to convert continuous state space to discrete state space? The basic idea is to be able to optimize the solution in the discrete state space domain.\u00a0 The discrete values of the solution to the state space allows one to optimize the controller.\n\n \n\n[caption id=\"attachment_104\" align=\"aligncenter\" width=\"662\"]\"Example Figure 13.1: Example calculation of state space using discrete state space. Note that, for this discrete state space, we used $x[k+1]= Ax[k] + Bu[k]$.[\/caption]The discrete state space example shows that iterations are needed for the computation and the optimization of state space. The optimization of discrete state space is well illustrated in the previous chapter.\u00a0 In this current chapter, we will use another simpler example to better illustrate the importance of understanding the numerical computation of the state space.\n\nThe following example shows an example of discrete state space:\n\n$\nx(k+1) = Ax(k) + Bu(k)\ny(k) = Cx(k) + Du(k)\n$\n\nWhere\n\n$\n\nA = \\begin{bmatrix}\n0.5 & 0.1 \\\\\n0.2 & 0.4\n\\end{bmatrix}\n\nB = \\begin{bmatrix}\n0.1 \\\\\n0.2\n\\end{bmatrix}\n\nx(0) = \\begin{bmatrix}\n1 \\\\\n1\n\\end{bmatrix}\n\n$\n\nThe state space will be iterated until the solution converges as shown in Figure 13.2. It is shown that following 22 iterations, the x value converges, yielding the value of\n\n$\nX_{Steady} = \\begin{bmatrix}\n0.285 \\\\\n0.4282\n\\end{bmatrix}\n$\n\n[caption id=\"attachment_104\" align=\"aligncenter\" width=\"546\"]\"Example Figure 13.2: Example of calculation of state space using discrete state space. Note that, for this discrete state space, we used $x[k+1]= Ax[k] + Bu[k]$.[\/caption]With this example, you may wonder how we can use state space for a meaningful control task.\u00a0 Now, it is evident that the state space is not only an equation; rather, it it actually corresponds to a solution. It means that given a state space with $A, B, C,$ and $ D $ matrices, we can find the solution for the system of equations.\u00a0 Such a property naturally inspires us to use the discrete state space to find the control gain function to ensure the solution results in the desired values. By doing this, we can utilize the state space domain for meaningful control.\n\nNow, we are not the only ones who have thought about this.\u00a0 As early as the 1950s, engineers have been inspired to adopt the state space techniques for solving difficult control problems. The development of the Linear Quadratic Controller (LQR) was heavily influenced by the work on dynamic programming by Richard Bellman<\/a> and the optimal control theory by Lev Pontryagin<\/a>. The optimal control law and the Linear Quadratic Controller have significant applications and are thus worth of our attention and investigation to learn how to use them to solve real-world control problems.\n\n \n

2) Overview of the Linear Quadratic Controller<\/strong><\/h1>\n

2.1) Fundamentals of the Linear Quadratic Controller<\/strong><\/h2>\nThe Linear Quadratic Controller is inspired by the optimal control law.\u00a0 Optimal control theory believes it is important to formulate a control problem as an optimization problem, where a cost function will guide the process to solve for the best solution of the underlying control problems.\u00a0 Rudolf Kalman<\/a> played a significant role in determining an approach to formulate the LQR using optimal control theory.\u00a0 He proposed the use of the Riccati equation to practically solve the LQR problem.\u00a0 Specifically, the Riccati equation can compute the optimal feedback gains that minimize a quadratic cost function. The quadratic cost function helps achieve the goal of the control, which is to ensure that a state converges to the desired state with minimal overshoot while ensuring that the control command to the actuator is not excessively large. As we know, a large control input to motors will damage the motors.\u00a0 Therefore, the optimization function needs to consider the optimization of both state and input variables.\n\nLQR controllers are very robust for real-world applications and often able to stabilize systems with guaranteed specifications and performance. Such properties have made it widely used in various engineering applications, including aerospace, robotics, and industrial automation.\n\n[caption id=\"attachment_104\" align=\"aligncenter\" width=\"397\"]\"LQR Figure 13.3: LQR cost objective function based on the discrete state space[\/caption]\n\n \n\n\\begin{equation}\nJ = sum(x(k)'Qx(k) + u(k)'Ru(k))\n\\end{equation}\n\nwhere:\n\nQ<\/em> is a positive semi-definite<\/em> cost matrix. The goal is to make $x(k)'Qx(k)$ always greater than or equal to zero regardless of negative values appearing in the X<\/em> state matrix.\n\nR<\/em> is a positive definite<\/em> control cost matrix. It means $u(k)'Ru(k)$ is always greater than zero regardless of negative values appearing in the X<\/em> state matrix.\n\nAmong this cost objective function, Q<\/em> and R<\/em> are two the most important variables used to achieve LQR control. An example of a semi-definite state cost matrix Q<\/em> is:\n\n$$ Q = \\begin{bmatrix}\n2 & -1 & 0 \\\\\n-1 & 2 & -1 \\\\\n0 & -1 & 2\n\n\\end{bmatrix} $$\n\nThis matrix is symmetric and positive semi-definite, meaning that for any non-zero vector x<\/em>, the quadratic form\n\n$ x^T Q x \\geq 0 . $\n\nWe should note that it is possible to have negative elements in the Q<\/em> matrix.\u00a0 Such negative elements have a physical meaning. For example, for a mechanical system, the negative values in the Q<\/em> matrix can represents damping effects or forces that act in opposition to the motion of the system. In electrical circuits, the negative elements represent resistances or impedances of the circuit.\n\nSimilarly, R<\/em>, the control cost matrix, is also important. A representative control cost matrix R<\/em> is as follows.\n\n$$ R = \\begin{bmatrix}\n1 & 0 \\\\\n0 & 1\n\\end{bmatrix} $$\n\nThe goal of the matrix R<\/em> is to ensure\n\n$ x^T Q x > 0 . $\n\nIn contrast to the Q<\/em> matrix, the R<\/em> matrix needs to be symmetric and positive. Therefore, there are never negative terms in the R<\/em> matrix.\n\nIt is important to note that the selection of values for the Q<\/em> and R<\/em> matrices are important for LQR. Here are practical guidelines for the selection of Q<\/em> and R<\/em>.\n\nNote 1:<\/strong> The goal of the Q<\/em> matrix is to penalize the control states, so it is weighted accordingly. Higher values in the Q<\/em> matrix correspond to states that you want to keep small.\u00a0 This means that for a given state, if you want to minimize the difference between the actual state and the desired state for a particular state, you can simply assign a larger corresponding weight to that state in the corresponding element of\u00a0Q.<\/em>\n\nThe design should be initiated using just the diagonal elements of the Q matrix and then expanding to use the non-diagonal elements if necessary. Using negative values is feasible if the corresponding state has damping and resistance associated with it.\n\nNote 2:<\/strong> The goal of the R<\/em> matrix is to penalize the control inputs, so the elements are weighted accordingly. Higher values in the R<\/em> matrix correspond to higher penalties on the control inputs, which can help to prevent excessive control inputs.\n\nSimilar to the Q<\/em> matrix, the choice of R<\/em> matrix weight elements should also start with the diagonal elements of the R<\/em> matrix. Once the diagonal elements are confirmed, one can consider using the non diagonal elements.\n\nNote 3:<\/strong>\u00a0 Bryson's rule is often used to select the values of the Q<\/em> and R<\/em> matrices. The general guideline based on Bryson's rule is to set\n\n$Q_{ii} = 1 \/ (\\text{max acceptable } x_i)^2 $\n$R_{jj} = 1 \/ (\\text{max acceptable } u_j)^2 $\n\nNote 4:<\/strong>\u00a0 The selection of Q and R matrices needs to be performed in an iterative fashion. Start with initial values and iteratively choose other values until the states and control inputs meet the control design goal.\n\n \n

3) Control Gain Computation of the Linear Quadratic Controller<\/strong><\/h1>\nAnother goal of the LQR is to obtain the control gain. The LQR control gain is denoted as $K$.\u00a0 The relationship between control input, state, and LQR control gain $K$ is described below. Please note that we need to use negative ($ -K $) here for a negative feedback system.\n\\begin{equation}\nu(k) = -Kx(k)\n\\end{equation}\n\nTo solve for the LQR control gain (feedback gain) $K$, we need to follow a few steps introduced below:\n\nStep 1 to Compute $K$:<\/strong> To solve for the feedback gain K<\/em>, we need introduce the Bellman equation.\u00a0 The Bellman equation was born from research dating back to the 1950s.\u00a0 The Bellman equation is based on Bellman's principle of optimality. The principle of optimality emphasizes the use of state space to solve an optimization problem. The optimization policy states that regardless of the initial state and decision, the following decisions must constitute an optimal policy with respect to the state resulting from the first decision. (Each current decision must be chosen to optimize the remaining problem regardless of what was chosen for previous states.)\n\nSpecifically, the Bellman equation starts by introducing the \"value function\" $V(x)$, also known as the \"cost-to-go function\".\n\n\\begin{equation}\nV(x) = \\min_u \\int_0^\\infty \\left( x(t)^T Q x(t) + u(t)^T R u(t) \\right) dt\n\\end{equation}\n\nBased on the Bellman equation optimality principle, it states that the optimal policy from any state $x$ must minimize the immediate cost plus the cost-to-go from the next state. For the LQR problem, this is written as:\n\n\\begin{equation}\nV(x) = \\min_u (\\left{ x^T Q x + u^T R u + V(\\dot{x})) \\right}\n\\end{equation}\n\n\\begin{equation}\n\\dot{x} = Ax + BU\n\\end{equation}\n\nAs such,\u00a0 $V(\\dot{x})$ representing the cost-to-go from the next state $\\dot{x}$ can be represented as\n\n\\begin{equation}\nV(x) = \\min_u \\left{ x^T Q x + u^T R u + V(A x + B u) \\right}\n\\end{equation}\n\nWe also know that\n\\begin{equation}\nx(k+1) = Ax(k) + Bu(k)\n\\end{equation}\n\nHence, the Bellman equation for the discrete-time LQR problem is written as:\n\n\\begin{equation}\nV(x_k) = \\min_{u_k} \\left[ x_k^T Q x_k + u_k^T R u_k + V(x_{k+1}) \\right]\n\\end{equation}\n\nwhere $ V(x_k) $ is the value function representing the minimum cost-to-go from state $x_k$.\n\nStep 2 to Compute $K$:\u00a0 <\/strong>It is typically safe to assume that the value function is of a positive quadratic form (because a positive quadratic function has a global minimum). The quadratic form of the $V(\\mathbf{x}[k])$ is:\n\n\\begin{equation}\nV(\\mathbf{x}[k]) = \\mathbf{x}[k]^T \\mathbf{P} \\mathbf{x}[k]\n\\end{equation}\n\nwhere $ (\\mathbf{P})$\u00a0 is a symmetric positive semi-definite matrix.\u00a0 So basically P<\/strong> is the positive (+) coefficient, and $x[k]$ is squared, thus you have a positive quadratic.\n\nCombining the two equations above, substitute the quadratic form into the Bellman equation, we can obtain the following equation\n\n\\begin{equation}\n\\mathbf{x}[k]^T \\mathbf{P} \\mathbf{x}[k] = \\mathbf{x}[k]^T \\mathbf{Q} \\mathbf{x}[k] + \\min_{\\mathbf{u}[k]} \\left( \\mathbf{u}[k]^T \\mathbf{R} \\mathbf{u}[k] + (\\mathbf{A} \\mathbf{x}[k] + \\mathbf{B} \\mathbf{u}[k])^T \\mathbf{P} (\\mathbf{A} \\mathbf{x}[k] + \\mathbf{B} \\mathbf{u}[k]) \\right)\n\\end{equation}\n\nNow, we know this Bellman equation: $$ x^T P x = \\min_u \\left{ x^T Q x + u^T R u + (A x + B u)^T P (A x + B u) \\right} $$\n\nWe can expand the terms inside the minimization: $$ x^T P x = \\min_u \\left{ x^T Q x + u^T R u + x^T A^T P A x + x^T A^T P B u + u^T B^T P A x + u^T B^T P B u \\right} $$\n\nSubsequently, we can group the terms involving u<\/em> : $$ x^T P x = \\min_u \\left{ x^T Q x + x^T A^T P A x + u^T (R + B^T P B) u + 2 x^T A^T P B u \\right} $$\n\nTo find the optimal control matrix u<\/em>, take the derivative with respect to u<\/em> and set it equal to zero: $$ \\frac{\\partial}{\\partial u} \\left( x^T Q x + x^T A^T P A x + u^T (R + B^T P B) u + 2 x^T A^T P B u \\right) = 0 $$ This yields: $$ 2 (R + B^T P B) u + 2 B^T P A x = 0 $$\n\nBased on the above equation, we can find the input u <\/em>that optimizes the control law. In order to obtain the optimal control input $\\mathbf{u}[k]$, we can take the derivative with respect to $\\mathbf{u}[k]$ and set it equal to zero:\n\n\\begin{equation}\n2 \\mathbf{R} \\mathbf{u}[k] + 2 \\mathbf{B}^T \\mathbf{P} (\\mathbf{A} \\mathbf{x}[k] + \\mathbf{B} \\mathbf{u}[k]) = 0\n\\end{equation}\n\nFinally, solve for $\\mathbf{u}[k]$.\u00a0 This is the most important step for obtaining the control input $\\mathbf{u}[k]$ !!<\/strong>\n\n\\begin{equation}\n\\mathbf{u}[k] = -(\\mathbf{R} + \\mathbf{B}^T \\mathbf{P} \\mathbf{B})^{-1} \\mathbf{B}^T \\mathbf{P} \\mathbf{A} \\mathbf{x}[k]\n\\end{equation}\n\nWe also know that control the input is equal to $$ \\mathbf{u}[k] = -\\mathbf{K} \\mathbf{x}[k] $$\u00a0 Therefore, the control gain $K$ becomes\n\n\\begin{equation}\n\\mathbf{K} = (\\mathbf{R} + \\mathbf{B}^T \\mathbf{P} \\mathbf{B})^{-1} \\mathbf{B}^T \\mathbf{P} \\mathbf{A}\n\\end{equation}\n\nStep 3 to Compute $K$:<\/strong> In order to calculate the LQR control gain $K$, we need to first calculate the\u00a0P <\/em>matrix. In order to solve for P<\/em>, we must rely on the Bellman equation again. For now, we need to substitute $\\mathbf{u}[k]$, so we can obtain\n\n\\begin{equation}\nx_k^T P x_k = x_k^T Q x_k + x_k^T A^T P A x_k - x_k^T A^T P B (R + B^T P B)^{-1} B^T P A x_k\n\\end{equation}\n\nRewrite the equation in terms of the quadratic form: $$ x_k^T (P - Q - A^T P A + A^T P B (R + B^T P B)^{-1} B^T P A) x_k = 0 $$\n\nFor the equation to hold for all $x_k$, the matrix inside the quadratic form must equal zero: $$ P - Q - A^T P A + A^T P B (R + B^T P B)^{-1} B^T P A = 0 $$\n\nSimplify the term using the matrix inverse: $ A^T P B (R + B^T P B)^{-1} B^T P A $\n\nUsing the Woodbury matrix identity, we can simplify this term. The Woodbury identity states: $$ (R + B^T P B)^{-1} = R^{-1} - R^{-1} B^T (P^{-1} + B R^{-1} B^T)^{-1} B R^{-1} $$\n\nHowever, in many control theory contexts, we assume R<\/em>\u00a0is invertible and simplify directly: $$ (R + B^T P B)^{-1} \\approx R^{-1} $$\n\nSubstitute the simplified inverse term back into the equation to obtain the discrete-time algebraic Riccati equation (DARE)<\/strong> : $$ P - Q - A^T P A + A^T P B R^{-1} B^T P A = 0 $$\n\nWith the substitution of $$ (R + B^T P B)^{-1} \\approx R^{-1} $$ we are able to rearrange to obtain the discrete-time algebraic Riccati equation (DARE):\n\n\\begin{equation}\n\\mathbf{P} = \\mathbf{Q} + \\mathbf{A}^T \\mathbf{P} \\mathbf{A} - \\mathbf{A}^T \\mathbf{P} \\mathbf{B} (\\mathbf{R} + \\mathbf{B}^T \\mathbf{P} \\mathbf{B})^{-1} \\mathbf{B}^T \\mathbf{P} \\mathbf{A}\n\\end{equation}\n\n \n\nStep 4 to Compute K:<\/strong> In order to solve for K<\/em>, we must first solve for P<\/em>. In the DARE equation, it is difficult to isolate P<\/em>.\u00a0 P<\/em> cannot be solved for easily, as shown in the following DARE equation:\n\n\\begin{equation}\nA^T P A - P - A^T P B R^{-1} B^T PA + Q = 0\n\\end{equation}\n\nHowever, it is still feasible for us to solve this equation using iterative numerical methods. One common method involves the use of the Schur method<\/em>\u00a0or the Newton-Kleinman iteration method<\/em>. These methods iteratively update the matrix P<\/em> until convergence.\n\nThe Python SciPy<\/em> package has built-in functions to solve the DARE. Specifically, the package below is able to do this work: scipy.linalg.solve_discrete_are<\/em> function from the SciPy library.\u00a0 Example of using Python to compute P<\/em> and K<\/em> are shown in Figures 13.4 and 13.5.\n\n \n\n[caption id=\"attachment_104\" align=\"aligncenter\" width=\"409\"]\"Example Figure 13.4: Example of using Python to compute the P matrix.[\/caption]\n\n[caption id=\"attachment_104\" align=\"aligncenter\" width=\"453\"]\"Example Figure 13.5: Example of using Python to compute the LQR control gain K.[\/caption]\n\n \n

4) Class Engagement<\/strong><\/h1>\nThere are a few interesting example showing you the implementation of the LQR involving Arduino. Among them, the use of LQR to control the inverted pendulum is an interesting application.\n\nFor this problem, an inverted pendulum sits on a cart moving along the x-axis direction.\u00a0 The inverted pendulum can move along the X and Y direction. It means the inverted pendulum can swing back and forth shown.\n\n \n\n\"\"\n

Figure 13.6: Example of using Python to compute the LQR control gain K.<\/p>\nIn order to control the inverted pendulum using LQR , we need to model the system.\u00a0 The followings show the process of the modeling\n\nhttps:\/\/mtmailmtsu-my.sharepoint.com\/:b:\/g\/personal\/hbzhang_mtsu_edu\/Ee-RhYhDtIRJqXFsQKpTBWoBq0Z9ZpVDy6PuTrdSbQzXYg?e=MPBRwJ<\/a>\n\n \n\nThrough the derivation above, you can find the system state space as the following.\n\n\\[\n\\begin{bmatrix}\n\\dot{x} \\\\\n\\ddot{x} \\\\\n\\dot{\\theta} \\\\\n\\ddot{\\theta}\n\\end{bmatrix}\n=\n\\begin{bmatrix}\n0 & 1 & 0 & 0 \\\\\n0 & 0 & -\\frac{gm}{M} & 0 \\\\\n0 & 0 & 0 & 1 \\\\\n0 & 0 & \\frac{g(M+m)}{lM} & 0\n\\end{bmatrix}\n\\begin{bmatrix}\nx \\\\\n\\dot{x} \\\\\n\\theta \\\\\n\\dot{\\theta}\n\\end{bmatrix}\n+\n\\begin{bmatrix}\n0 \\\\\n\\frac{1}{M} \\\\\n0 \\\\\n\\frac{1}{lM}\n\\end{bmatrix}\nF\n\\]\n\nWith this state space modeling, it becomes feasible for us to control the inverted pendulum using LQR with the python code as shown in below\n\nhttps:\/\/mtmailmtsu-my.sharepoint.com\/:u:\/g\/personal\/hbzhang_mtsu_edu\/ER09ECG2UU1Crz7kMu7chegB0wn5fwLCJcHyNWGPB9AMgQ?e=nM6AHf<\/a>\n\n \n\nFollowing running the code, results show that the use of LQR is able to stabilize the cart motion as shown in Figure 13.7. Specifically, when the cart is moving from one side to another, the inverted pendulum stays upright.\n\n\"\"\n

Figure 13.7:<\/p>\n

Figure 13.7:\u00a0 The results of running MPC python code showing that MPC is able to stabilize the inverted pendulum on the moving cart.<\/p>\n \n\n \n\n \n\n \n\n \n\n \n\n \n\n ","rendered":"

[latexpage]<\/p>\n

1) Motivation for the Linear Quadratic Controller<\/strong><\/h1>\n

The Linear Quadratic Controller (LQR) and the Model Predicative Controller (MPC) are classical optimal controllers. They are effective and efficient in controlling linear systems. Both of them have been proven by theoretical and real-world control applications. It has been shown that the two controllers offer the best performance for real-world control systems.<\/p>\n

LQR needs to work in the discrete state space because it is required to optimize the control function.\u00a0 So before we discuss the LQR controller, we need to first convert the state space to the discrete state space as shown in Figure 13.1.\u00a0 Why do we need to convert continuous state space to discrete state space? The basic idea is to be able to optimize the solution in the discrete state space domain.\u00a0 The discrete values of the solution to the state space allows one to optimize the controller.<\/p>\n

 <\/p>\n

\"Example
Figure 13.1: Example calculation of state space using discrete state space. Note that, for this discrete state space, we used $x[k+1]= Ax[k] + Bu[k]$.<\/figcaption><\/figure>\n

The discrete state space example shows that iterations are needed for the computation and the optimization of state space. The optimization of discrete state space is well illustrated in the previous chapter.\u00a0 In this current chapter, we will use another simpler example to better illustrate the importance of understanding the numerical computation of the state space.<\/p>\n

The following example shows an example of discrete state space:<\/p>\n

$
\nx(k+1) = Ax(k) + Bu(k)
\ny(k) = Cx(k) + Du(k)
\n$<\/p>\n

Where<\/p>\n

$<\/p>\n

A = \\begin{bmatrix}
\n0.5 & 0.1 \\\\
\n0.2 & 0.4
\n\\end{bmatrix}<\/p>\n

B = \\begin{bmatrix}
\n0.1 \\\\
\n0.2
\n\\end{bmatrix}<\/p>\n

x(0) = \\begin{bmatrix}
\n1 \\\\
\n1
\n\\end{bmatrix}<\/p>\n

$<\/p>\n

The state space will be iterated until the solution converges as shown in Figure 13.2. It is shown that following 22 iterations, the x value converges, yielding the value of<\/p>\n

$
\nX_{Steady} = \\begin{bmatrix}
\n0.285 \\\\
\n0.4282
\n\\end{bmatrix}
\n$<\/p>\n

\"Example
Figure 13.2: Example of calculation of state space using discrete state space. Note that, for this discrete state space, we used $x[k+1]= Ax[k] + Bu[k]$.<\/figcaption><\/figure>\n

With this example, you may wonder how we can use state space for a meaningful control task.\u00a0 Now, it is evident that the state space is not only an equation; rather, it it actually corresponds to a solution. It means that given a state space with $A, B, C,$ and $ D $ matrices, we can find the solution for the system of equations.\u00a0 Such a property naturally inspires us to use the discrete state space to find the control gain function to ensure the solution results in the desired values. By doing this, we can utilize the state space domain for meaningful control.<\/p>\n

Now, we are not the only ones who have thought about this.\u00a0 As early as the 1950s, engineers have been inspired to adopt the state space techniques for solving difficult control problems. The development of the Linear Quadratic Controller (LQR) was heavily influenced by the work on dynamic programming by Richard Bellman<\/a> and the optimal control theory by Lev Pontryagin<\/a>. The optimal control law and the Linear Quadratic Controller have significant applications and are thus worth of our attention and investigation to learn how to use them to solve real-world control problems.<\/p>\n

 <\/p>\n

2) Overview of the Linear Quadratic Controller<\/strong><\/h1>\n

2.1) Fundamentals of the Linear Quadratic Controller<\/strong><\/h2>\n

The Linear Quadratic Controller is inspired by the optimal control law.\u00a0 Optimal control theory believes it is important to formulate a control problem as an optimization problem, where a cost function will guide the process to solve for the best solution of the underlying control problems.\u00a0 Rudolf Kalman<\/a> played a significant role in determining an approach to formulate the LQR using optimal control theory.\u00a0 He proposed the use of the Riccati equation to practically solve the LQR problem.\u00a0 Specifically, the Riccati equation can compute the optimal feedback gains that minimize a quadratic cost function. The quadratic cost function helps achieve the goal of the control, which is to ensure that a state converges to the desired state with minimal overshoot while ensuring that the control command to the actuator is not excessively large. As we know, a large control input to motors will damage the motors.\u00a0 Therefore, the optimization function needs to consider the optimization of both state and input variables.<\/p>\n

LQR controllers are very robust for real-world applications and often able to stabilize systems with guaranteed specifications and performance. Such properties have made it widely used in various engineering applications, including aerospace, robotics, and industrial automation.<\/p>\n

\"LQR
Figure 13.3: LQR cost objective function based on the discrete state space<\/figcaption><\/figure>\n

 <\/p>\n

\\begin{equation}
\nJ = sum(x(k)’Qx(k) + u(k)’Ru(k))
\n\\end{equation}<\/p>\n

where:<\/p>\n

Q<\/em> is a positive semi-definite<\/em> cost matrix. The goal is to make $x(k)’Qx(k)$ always greater than or equal to zero regardless of negative values appearing in the X<\/em> state matrix.<\/p>\n

R<\/em> is a positive definite<\/em> control cost matrix. It means $u(k)’Ru(k)$ is always greater than zero regardless of negative values appearing in the X<\/em> state matrix.<\/p>\n

Among this cost objective function, Q<\/em> and R<\/em> are two the most important variables used to achieve LQR control. An example of a semi-definite state cost matrix Q<\/em> is:<\/p>\n

$$ Q = \\begin{bmatrix}
\n2 & -1 & 0 \\\\
\n-1 & 2 & -1 \\\\
\n0 & -1 & 2<\/p>\n

\\end{bmatrix} $$<\/p>\n

This matrix is symmetric and positive semi-definite, meaning that for any non-zero vector x<\/em>, the quadratic form<\/p>\n

$ x^T Q x \\geq 0 . $<\/p>\n

We should note that it is possible to have negative elements in the Q<\/em> matrix.\u00a0 Such negative elements have a physical meaning. For example, for a mechanical system, the negative values in the Q<\/em> matrix can represents damping effects or forces that act in opposition to the motion of the system. In electrical circuits, the negative elements represent resistances or impedances of the circuit.<\/p>\n

Similarly, R<\/em>, the control cost matrix, is also important. A representative control cost matrix R<\/em> is as follows.<\/p>\n

$$ R = \\begin{bmatrix}
\n1 & 0 \\\\
\n0 & 1
\n\\end{bmatrix} $$<\/p>\n

The goal of the matrix R<\/em> is to ensure<\/p>\n

$ x^T Q x > 0 . $<\/p>\n

In contrast to the Q<\/em> matrix, the R<\/em> matrix needs to be symmetric and positive. Therefore, there are never negative terms in the R<\/em> matrix.<\/p>\n

It is important to note that the selection of values for the Q<\/em> and R<\/em> matrices are important for LQR. Here are practical guidelines for the selection of Q<\/em> and R<\/em>.<\/p>\n

Note 1:<\/strong> The goal of the Q<\/em> matrix is to penalize the control states, so it is weighted accordingly. Higher values in the Q<\/em> matrix correspond to states that you want to keep small.\u00a0 This means that for a given state, if you want to minimize the difference between the actual state and the desired state for a particular state, you can simply assign a larger corresponding weight to that state in the corresponding element of\u00a0Q.<\/em><\/p>\n

The design should be initiated using just the diagonal elements of the Q matrix and then expanding to use the non-diagonal elements if necessary. Using negative values is feasible if the corresponding state has damping and resistance associated with it.<\/p>\n

Note 2:<\/strong> The goal of the R<\/em> matrix is to penalize the control inputs, so the elements are weighted accordingly. Higher values in the R<\/em> matrix correspond to higher penalties on the control inputs, which can help to prevent excessive control inputs.<\/p>\n

Similar to the Q<\/em> matrix, the choice of R<\/em> matrix weight elements should also start with the diagonal elements of the R<\/em> matrix. Once the diagonal elements are confirmed, one can consider using the non diagonal elements.<\/p>\n

Note 3:<\/strong>\u00a0 Bryson’s rule is often used to select the values of the Q<\/em> and R<\/em> matrices. The general guideline based on Bryson’s rule is to set<\/p>\n

$Q_{ii} = 1 \/ (\\text{max acceptable } x_i)^2 $
\n$R_{jj} = 1 \/ (\\text{max acceptable } u_j)^2 $<\/p>\n

Note 4:<\/strong>\u00a0 The selection of Q and R matrices needs to be performed in an iterative fashion. Start with initial values and iteratively choose other values until the states and control inputs meet the control design goal.<\/p>\n

 <\/p>\n

3) Control Gain Computation of the Linear Quadratic Controller<\/strong><\/h1>\n

Another goal of the LQR is to obtain the control gain. The LQR control gain is denoted as $K$.\u00a0 The relationship between control input, state, and LQR control gain $K$ is described below. Please note that we need to use negative ($ -K $) here for a negative feedback system.
\n\\begin{equation}
\nu(k) = -Kx(k)
\n\\end{equation}<\/p>\n

To solve for the LQR control gain (feedback gain) $K$, we need to follow a few steps introduced below:<\/p>\n

Step 1 to Compute $K$:<\/strong> To solve for the feedback gain K<\/em>, we need introduce the Bellman equation.\u00a0 The Bellman equation was born from research dating back to the 1950s.\u00a0 The Bellman equation is based on Bellman’s principle of optimality. The principle of optimality emphasizes the use of state space to solve an optimization problem. The optimization policy states that regardless of the initial state and decision, the following decisions must constitute an optimal policy with respect to the state resulting from the first decision. (Each current decision must be chosen to optimize the remaining problem regardless of what was chosen for previous states.)<\/p>\n

Specifically, the Bellman equation starts by introducing the “value function” $V(x)$, also known as the “cost-to-go function”.<\/p>\n

\\begin{equation}
\nV(x) = \\min_u \\int_0^\\infty \\left( x(t)^T Q x(t) + u(t)^T R u(t) \\right) dt
\n\\end{equation}<\/p>\n

Based on the Bellman equation optimality principle, it states that the optimal policy from any state $x$ must minimize the immediate cost plus the cost-to-go from the next state. For the LQR problem, this is written as:<\/p>\n

\\begin{equation}
\nV(x) = \\min_u (\\left{ x^T Q x + u^T R u + V(\\dot{x})) \\right}
\n\\end{equation}<\/p>\n

\\begin{equation}
\n\\dot{x} = Ax + BU
\n\\end{equation}<\/p>\n

As such,\u00a0 $V(\\dot{x})$ representing the cost-to-go from the next state $\\dot{x}$ can be represented as<\/p>\n

\\begin{equation}
\nV(x) = \\min_u \\left{ x^T Q x + u^T R u + V(A x + B u) \\right}
\n\\end{equation}<\/p>\n

We also know that
\n\\begin{equation}
\nx(k+1) = Ax(k) + Bu(k)
\n\\end{equation}<\/p>\n

Hence, the Bellman equation for the discrete-time LQR problem is written as:<\/p>\n

\\begin{equation}
\nV(x_k) = \\min_{u_k} \\left[ x_k^T Q x_k + u_k^T R u_k + V(x_{k+1}) \\right]
\n\\end{equation}<\/p>\n

where $ V(x_k) $ is the value function representing the minimum cost-to-go from state $x_k$.<\/p>\n

Step 2 to Compute $K$:\u00a0 <\/strong>It is typically safe to assume that the value function is of a positive quadratic form (because a positive quadratic function has a global minimum). The quadratic form of the $V(\\mathbf{x}[k])$ is:<\/p>\n

\\begin{equation}
\nV(\\mathbf{x}[k]) = \\mathbf{x}[k]^T \\mathbf{P} \\mathbf{x}[k]
\n\\end{equation}<\/p>\n

where $ (\\mathbf{P})$\u00a0 is a symmetric positive semi-definite matrix.\u00a0 So basically P<\/strong> is the positive (+) coefficient, and $x[k]$ is squared, thus you have a positive quadratic.<\/p>\n

Combining the two equations above, substitute the quadratic form into the Bellman equation, we can obtain the following equation<\/p>\n

\\begin{equation}
\n\\mathbf{x}[k]^T \\mathbf{P} \\mathbf{x}[k] = \\mathbf{x}[k]^T \\mathbf{Q} \\mathbf{x}[k] + \\min_{\\mathbf{u}[k]} \\left( \\mathbf{u}[k]^T \\mathbf{R} \\mathbf{u}[k] + (\\mathbf{A} \\mathbf{x}[k] + \\mathbf{B} \\mathbf{u}[k])^T \\mathbf{P} (\\mathbf{A} \\mathbf{x}[k] + \\mathbf{B} \\mathbf{u}[k]) \\right)
\n\\end{equation}<\/p>\n

Now, we know this Bellman equation: $$ x^T P x = \\min_u \\left{ x^T Q x + u^T R u + (A x + B u)^T P (A x + B u) \\right} $$<\/p>\n

We can expand the terms inside the minimization: $$ x^T P x = \\min_u \\left{ x^T Q x + u^T R u + x^T A^T P A x + x^T A^T P B u + u^T B^T P A x + u^T B^T P B u \\right} $$<\/p>\n

Subsequently, we can group the terms involving u<\/em> : $$ x^T P x = \\min_u \\left{ x^T Q x + x^T A^T P A x + u^T (R + B^T P B) u + 2 x^T A^T P B u \\right} $$<\/p>\n

To find the optimal control matrix u<\/em>, take the derivative with respect to u<\/em> and set it equal to zero: $$ \\frac{\\partial}{\\partial u} \\left( x^T Q x + x^T A^T P A x + u^T (R + B^T P B) u + 2 x^T A^T P B u \\right) = 0 $$ This yields: $$ 2 (R + B^T P B) u + 2 B^T P A x = 0 $$<\/p>\n

Based on the above equation, we can find the input u <\/em>that optimizes the control law. In order to obtain the optimal control input $\\mathbf{u}[k]$, we can take the derivative with respect to $\\mathbf{u}[k]$ and set it equal to zero:<\/p>\n

\\begin{equation}
\n2 \\mathbf{R} \\mathbf{u}[k] + 2 \\mathbf{B}^T \\mathbf{P} (\\mathbf{A} \\mathbf{x}[k] + \\mathbf{B} \\mathbf{u}[k]) = 0
\n\\end{equation}<\/p>\n

Finally, solve for $\\mathbf{u}[k]$.\u00a0 This is the most important step for obtaining the control input $\\mathbf{u}[k]$ !!<\/strong><\/p>\n

\\begin{equation}
\n\\mathbf{u}[k] = -(\\mathbf{R} + \\mathbf{B}^T \\mathbf{P} \\mathbf{B})^{-1} \\mathbf{B}^T \\mathbf{P} \\mathbf{A} \\mathbf{x}[k]
\n\\end{equation}<\/p>\n

We also know that control the input is equal to $$ \\mathbf{u}[k] = -\\mathbf{K} \\mathbf{x}[k] $$\u00a0 Therefore, the control gain $K$ becomes<\/p>\n

\\begin{equation}
\n\\mathbf{K} = (\\mathbf{R} + \\mathbf{B}^T \\mathbf{P} \\mathbf{B})^{-1} \\mathbf{B}^T \\mathbf{P} \\mathbf{A}
\n\\end{equation}<\/p>\n

Step 3 to Compute $K$:<\/strong> In order to calculate the LQR control gain $K$, we need to first calculate the\u00a0P <\/em>matrix. In order to solve for P<\/em>, we must rely on the Bellman equation again. For now, we need to substitute $\\mathbf{u}[k]$, so we can obtain<\/p>\n

\\begin{equation}
\nx_k^T P x_k = x_k^T Q x_k + x_k^T A^T P A x_k – x_k^T A^T P B (R + B^T P B)^{-1} B^T P A x_k
\n\\end{equation}<\/p>\n

Rewrite the equation in terms of the quadratic form: $$ x_k^T (P – Q – A^T P A + A^T P B (R + B^T P B)^{-1} B^T P A) x_k = 0 $$<\/p>\n

For the equation to hold for all $x_k$, the matrix inside the quadratic form must equal zero: $$ P – Q – A^T P A + A^T P B (R + B^T P B)^{-1} B^T P A = 0 $$<\/p>\n

Simplify the term using the matrix inverse: $ A^T P B (R + B^T P B)^{-1} B^T P A $<\/p>\n

Using the Woodbury matrix identity, we can simplify this term. The Woodbury identity states: $$ (R + B^T P B)^{-1} = R^{-1} – R^{-1} B^T (P^{-1} + B R^{-1} B^T)^{-1} B R^{-1} $$<\/p>\n

However, in many control theory contexts, we assume R<\/em>\u00a0is invertible and simplify directly: $$ (R + B^T P B)^{-1} \\approx R^{-1} $$<\/p>\n

Substitute the simplified inverse term back into the equation to obtain the discrete-time algebraic Riccati equation (DARE)<\/strong> : $$ P – Q – A^T P A + A^T P B R^{-1} B^T P A = 0 $$<\/p>\n

With the substitution of $$ (R + B^T P B)^{-1} \\approx R^{-1} $$ we are able to rearrange to obtain the discrete-time algebraic Riccati equation (DARE):<\/p>\n

\\begin{equation}
\n\\mathbf{P} = \\mathbf{Q} + \\mathbf{A}^T \\mathbf{P} \\mathbf{A} – \\mathbf{A}^T \\mathbf{P} \\mathbf{B} (\\mathbf{R} + \\mathbf{B}^T \\mathbf{P} \\mathbf{B})^{-1} \\mathbf{B}^T \\mathbf{P} \\mathbf{A}
\n\\end{equation}<\/p>\n

 <\/p>\n

Step 4 to Compute K:<\/strong> In order to solve for K<\/em>, we must first solve for P<\/em>. In the DARE equation, it is difficult to isolate P<\/em>.\u00a0 P<\/em> cannot be solved for easily, as shown in the following DARE equation:<\/p>\n

\\begin{equation}
\nA^T P A – P – A^T P B R^{-1} B^T PA + Q = 0
\n\\end{equation}<\/p>\n

However, it is still feasible for us to solve this equation using iterative numerical methods. One common method involves the use of the Schur method<\/em>\u00a0or the Newton-Kleinman iteration method<\/em>. These methods iteratively update the matrix P<\/em> until convergence.<\/p>\n

The Python SciPy<\/em> package has built-in functions to solve the DARE. Specifically, the package below is able to do this work: scipy.linalg.solve_discrete_are<\/em> function from the SciPy library.\u00a0 Example of using Python to compute P<\/em> and K<\/em> are shown in Figures 13.4 and 13.5.<\/p>\n

 <\/p>\n

\"Example
Figure 13.4: Example of using Python to compute the P matrix.<\/figcaption><\/figure>\n
\"Example
Figure 13.5: Example of using Python to compute the LQR control gain K.<\/figcaption><\/figure>\n

 <\/p>\n

4) Class Engagement<\/strong><\/h1>\n

There are a few interesting example showing you the implementation of the LQR involving Arduino. Among them, the use of LQR to control the inverted pendulum is an interesting application.<\/p>\n

For this problem, an inverted pendulum sits on a cart moving along the x-axis direction.\u00a0 The inverted pendulum can move along the X and Y direction. It means the inverted pendulum can swing back and forth shown.<\/p>\n

 <\/p>\n

\"\"<\/p>\n

Figure 13.6: Example of using Python to compute the LQR control gain K.<\/p>\n

In order to control the inverted pendulum using LQR , we need to model the system.\u00a0 The followings show the process of the modeling<\/p>\n

https:\/\/mtmailmtsu-my.sharepoint.com\/:b:\/g\/personal\/hbzhang_mtsu_edu\/Ee-RhYhDtIRJqXFsQKpTBWoBq0Z9ZpVDy6PuTrdSbQzXYg?e=MPBRwJ<\/a><\/p>\n

 <\/p>\n

Through the derivation above, you can find the system state space as the following.<\/p>\n

\\[
\n\\begin{bmatrix}
\n\\dot{x} \\\\
\n\\ddot{x} \\\\
\n\\dot{\\theta} \\\\
\n\\ddot{\\theta}
\n\\end{bmatrix}
\n=
\n\\begin{bmatrix}
\n0 & 1 & 0 & 0 \\\\
\n0 & 0 & -\\frac{gm}{M} & 0 \\\\
\n0 & 0 & 0 & 1 \\\\
\n0 & 0 & \\frac{g(M+m)}{lM} & 0
\n\\end{bmatrix}
\n\\begin{bmatrix}
\nx \\\\
\n\\dot{x} \\\\
\n\\theta \\\\
\n\\dot{\\theta}
\n\\end{bmatrix}
\n+
\n\\begin{bmatrix}
\n0 \\\\
\n\\frac{1}{M} \\\\
\n0 \\\\
\n\\frac{1}{lM}
\n\\end{bmatrix}
\nF
\n\\]<\/p>\n

With this state space modeling, it becomes feasible for us to control the inverted pendulum using LQR with the python code as shown in below<\/p>\n

https:\/\/mtmailmtsu-my.sharepoint.com\/:u:\/g\/personal\/hbzhang_mtsu_edu\/ER09ECG2UU1Crz7kMu7chegB0wn5fwLCJcHyNWGPB9AMgQ?e=nM6AHf<\/a><\/p>\n

 <\/p>\n

Following running the code, results show that the use of LQR is able to stabilize the cart motion as shown in Figure 13.7. Specifically, when the cart is moving from one side to another, the inverted pendulum stays upright.<\/p>\n

\"\"<\/p>\n

Figure 13.7:<\/p>\n

Figure 13.7:\u00a0 The results of running MPC python code showing that MPC is able to stabilize the inverted pendulum on the moving cart.<\/p>\n

 <\/p>\n

 <\/p>\n

 <\/p>\n

 <\/p>\n

 <\/p>\n

 <\/p>\n

 <\/p>\n

 <\/p>\n","protected":false},"author":1,"menu_order":13,"template":"","meta":{"pb_show_title":"","pb_short_title":"Ch. 13: Linear Quadratic Controller","pb_subtitle":"","pb_authors":["hbzhang"],"pb_section_license":""},"chapter-type":[],"contributor":[61],"license":[],"class_list":["post-105","chapter","type-chapter","status-publish","hentry","contributor-hbzhang"],"part":66,"_links":{"self":[{"href":"https:\/\/libraryresources.nse.org.ng\/robotics\/wp-json\/pressbooks\/v2\/chapters\/105","targetHints":{"allow":["GET"]}}],"collection":[{"href":"https:\/\/libraryresources.nse.org.ng\/robotics\/wp-json\/pressbooks\/v2\/chapters"}],"about":[{"href":"https:\/\/libraryresources.nse.org.ng\/robotics\/wp-json\/wp\/v2\/types\/chapter"}],"author":[{"embeddable":true,"href":"https:\/\/libraryresources.nse.org.ng\/robotics\/wp-json\/wp\/v2\/users\/1"}],"version-history":[{"count":1,"href":"https:\/\/libraryresources.nse.org.ng\/robotics\/wp-json\/pressbooks\/v2\/chapters\/105\/revisions"}],"predecessor-version":[{"id":106,"href":"https:\/\/libraryresources.nse.org.ng\/robotics\/wp-json\/pressbooks\/v2\/chapters\/105\/revisions\/106"}],"part":[{"href":"https:\/\/libraryresources.nse.org.ng\/robotics\/wp-json\/pressbooks\/v2\/parts\/66"}],"metadata":[{"href":"https:\/\/libraryresources.nse.org.ng\/robotics\/wp-json\/pressbooks\/v2\/chapters\/105\/metadata\/"}],"wp:attachment":[{"href":"https:\/\/libraryresources.nse.org.ng\/robotics\/wp-json\/wp\/v2\/media?parent=105"}],"wp:term":[{"taxonomy":"chapter-type","embeddable":true,"href":"https:\/\/libraryresources.nse.org.ng\/robotics\/wp-json\/pressbooks\/v2\/chapter-type?post=105"},{"taxonomy":"contributor","embeddable":true,"href":"https:\/\/libraryresources.nse.org.ng\/robotics\/wp-json\/wp\/v2\/contributor?post=105"},{"taxonomy":"license","embeddable":true,"href":"https:\/\/libraryresources.nse.org.ng\/robotics\/wp-json\/wp\/v2\/license?post=105"}],"curies":[{"name":"wp","href":"https:\/\/api.w.org\/{rel}","templated":true}]}}