# Optimal Control of a Two-Link Manipulator - CodeProject

## Table of Contents

## Problem Statement

In this blog, I'd like to apply the technique and software presented in my CodeProject article *Simple Software for Optimal Control* [1] to governance of a two-link manipulator. The mathematic model of the manipulator is taken from write-up *A two-link manipulator: simulation and control design* [2] by Stephen Dodds (advisor Dr. Mahboub Baccouch). It is expected that the reader is familiar with [1] since its terminology, notation and formulas are widely used here.

The simplified model of the two-link vertically oriented manipulator is depicted below [2].

Here *M _{i}* are lumped masses,

*L*are lengths of the links and

_{i}*Θ*are links' displacement angles. Math description of the model is given with formulas (1).

_{i}The system is essentially nonlinear. For its optimal control, we will apply method of linear-quadratic control combined with quasilinearization and appropriate software provided in [1].

According to notation used in [1], state vector **x** corresponds to vector **z** in (1). The system is actuated with two motors producing torques *T _{1}* and

*T*rotating the links. For the sake of simplicity, it is assumed that the motors are inertialess. Control vector

_{2}**m**of dimension 2 corresponds to the torques. The torques are denotes as

*f*and

_{Θ1}*f*in formulas (1).

_{Θ2}Our goal is to suggest such control of the system which provides transition from its initial position to a prescribed final position and its fixation in the final position. The transition should be fast. However, during this transition values of angular velocities and torques should be kept within some reasonable limits to avoid damage of system's mechanics.

For numerical sample, it is assumed masses in *kg*, lengths in *m*, time in *s* and torques in *N⋅m*. Therefore *g* = 9.81 *m/s ^{2}*. Angles are assumed in

*degree*s for a better reading. The following parameters are taken for numeric sample:

Δt = 0.01, N = 1001.

## Optimization Policies

The goal spoken out above should be formalized. The suggested approach is to minimize integral quadratic cost function described in [1]. Below, we demonstrate application of two optimization policies to the system. The **case 1** is characterized with:

- permanent desirable final values for
*Θ*and_{1}*Θ*angles which are desirable coordinates_{2}*r*and_{0}*r*respectively,_{2} - permanent weight factors for the
*Θ*-s as*Q*and_{00}*Q*, and_{22} - permanent limits to external control parameters (torques) implemented with unit matrix
**Z**and zero vector**u**.

The **case 2** implies:

- variable desirable values for
*Θ*and_{1}*Θ*angles (_{2}*r*and_{0}*r*respectively) calculating as_{2}

where**r**=_{k}*if*k < s⋅N*then***x**_{init}+ (r_{final}- x_{init}) / (s⋅N)*else***r**(2)_{final}*k*is timestep, and*s*is threshold value,*s < 1*. - permanent desirable derivatives from angles
*Θ*-s equal to zero, - unit weight matrix
**Q**, and - torques are not controlled, i. e. matrix
**Z**=**0**.

In both cases, we confine ourselves with just one iteration enabling in conjunction with constant weight matrices to achieve constant values of feedbacks.

These two optimization policies are applied to the following two movements.

## Movements

**The first movement**

x = _{init} |
$ \begin{bmatrix} 0 \\ 0 \\ 0 \\ 0 \end{bmatrix} $ | , | x = _{final} |
$\begin{bmatrix} 45 \\ 0 \\ 30 \\ 0 \end{bmatrix}$ |

and

**The second movement**

x = _{init} |
$ \begin{bmatrix} 45 \\ 0 \\ 30 \\ 0 \end{bmatrix} $ | , | x = _{final} |
$\begin{bmatrix} 60 \\ 0 \\ -30 \\ 0 \end{bmatrix}$ |

## The First Movement

*Input*

**Case 1**

$ Q = \begin{bmatrix} 5000 &0 &0 &0 \\ 0 & 0 & 0 & 0 \\ 0 & 0 & 5000 & 0 \\ 0 & 0 & 0 & 0 \end{bmatrix} , Z = \begin{bmatrix} 1 & 0 \\ 0 & 1 \end{bmatrix} , r = \begin{bmatrix} 48.6 \\ 0 \\ 32.9 \\ 0 \end{bmatrix} , u = 0$

**Case 2**

$ Q = \begin{bmatrix} 1 & 0 & 0 & 0 \\ 0 & 1 & 0 & 0 \\ 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 1 \end{bmatrix} , Z = 0 , r = \begin{bmatrix} (2)where \hspace{2 mm} s=0.6, r_{final}=48.6\\ 0 \\ (2)where \hspace{2 mm} s=0.6, r_{final}=32\\ 0 \end{bmatrix} $

*Result Transients*

## The Second Movement

*Input*

**Case 1**

$ Q = \begin{bmatrix} 5000 &0 &0 &0 \\ 0 & 0 & 0 & 0 \\ 0 & 0 & 5000 & 0 \\ 0 & 0 & 0 & 0 \end{bmatrix} , Z = \begin{bmatrix} 1 & 0 \\ 0 & 1 \end{bmatrix} , r = \begin{bmatrix} 66.15 \\ 0 \\ -16.05 \\ 0 \end{bmatrix} , u = 0$

**Case 2**

$ Q = \begin{bmatrix} 1 & 0 & 0 & 0 \\ 0 & 1 & 0 & 0 \\ 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 1 \end{bmatrix} , Z = 0 , r = \begin{bmatrix} (2) where \hspace{2 mm} s=0.6, r_{final}=66.5\\ 0 \\ (2) where \hspace{2 mm} s=0.6, r_{final}=-20.5\\ 0 \end{bmatrix} $

*Result Transients*

## Calculated Feedbacks and Permanent Inputs of Close-Loop System

*Case 1*

**Feedback**

46.7 | 7.06 | -34.6 | -4.42 |

46.6 | 7.06 | 34.6 | 4.42 |

**Input for the First Movement**

**Input for the Second Movement**

*Case 2*

**Feedback**

49.9 | 50.5 | -49.8 | -50.5 |

49.7 | 50.5 | 49.8 | 50.5 |

Although formally in the case 2 input of close-loop system is not constant, calculations show results very close to the constant values below.

**Input for the First Movement**

**Input for the Second Movement**

## Discussion

Problem of two-link manipulator optimal control is not a simple one. The system is essentially nonlinear. Its solution requires effective coordinated control by two channels (torques). As it is shown above, the technique for minimization of integral quadratic cost function presented in [1] allows designer to easily calculate various control policies. In fact, I performed a lot of calculations with various values of desirable vectors and weight matrices members, and the above results constitute just a small part of them. For both discussed sets of input data, we managed to obtain reasonable control policies with just one iteration ensuring constant value of feedbacks. It is interesting that obtained control policies include positive feedbacks from angle and angular velocity of the second link to the first actuator in both control cases. Both control channels have similar feedback values because masses *M _{1}* and

*M*and link lengths

_{2}*L*and

_{1}*L*are equal.

_{2}For further research, check the system controlled with suggested policies for stability and sensitivity to model inaccuracy is required. And of course, it is always interesting to generate more control policies, e.g., with more that one iterations process. For technical implementation of control will be useful to provide effective measurement / estimation of feedback parameters (that are links angles and angular velocities). For the real life system dynamics of actuators should be included to mathematical model of controlled object.