Accelerating the pace of engineering and science

# Robust Control Toolbox

## Multi-Loop PID Control of a Robot Arm

This example shows how to use looptune to tune a multi-loop controller for a 4-DOF robotic arm manipulator.

Robotic Arm Model and Controller

This example uses the four degree-of-freedom robotic arm shown below. This arm consists of four joints labeled from base to tip: "Turntable", "Bicep", "Forearm", and "Wrist". Each joint is actuated by a DC motor except for the Bicep joint which uses two DC motors in tandem.

Figure 1: Robotic arm manipulator.

Open the Simulink model of the robot arm.

```open_system('rct_robotarm')
```

The controller consists of four PID controllers (one per joint). Each PID controller is implemented using the "2-DOF PID Controller" block from the Simulink library (see PID Tuning for Setpoint Tracking vs. Disturbance Rejection example for motivation).

Figure 2: Controller structure.

Typically, such multi-loop controllers are tuned sequentially by tuning one PID loop at a time and cycling through the loops until the overall behavior is satisfactory. This process can be time consuming and is not guaranteed to converge to the best overall tuning. Alternatively, you can use systune or looptune to jointly tune all four PID loops subject to system-level requirements such as response time and minimum cross-coupling.

In this example, the arm must move to a particular configuration in about 1 second with smooth angular motion at each joint. The arm starts in a fully extended vertical position with all joint angles at zero. The end configuration is specified by the angular positions: Turntable = 60 deg, Bicep = -10 deg, Forearm = 60 deg, Wrist = 90 deg. The angular trajectories for the original PID settings are shown below. Clearly the response is too sluggish and the forearm is wobbling.

Figure 3: Untuned angular response.

Linearizing the Plant

The robot arm dynamics are nonlinear. To understand whether the arm can be controlled with one set of PID gains, linearize the plant at various points (snapshot times) along the trajectory of interest. Here "plant" refers to the dynamics between the control signals (outputs of PID blocks) and the measurement signals (output of "Robot Arm" block).

```SnapshotTimes = 0:1:5;
% Plant is from PID outputs to Robot Arm outputs
LinIOs = [...
linio('rct_robotarm/Controller/TurntablePID',1,'openinput'),...
linio('rct_robotarm/Controller/BicepPID',1,'openinput'),...
linio('rct_robotarm/Controller/ForearmPID',1,'openinput'),...
linio('rct_robotarm/Controller/WristPID',1,'openinput'),...
linio('rct_robotarm/Robot Arm',1,'output')];
LinOpt = linearizeOptions('SampleTime',0);  % seek continuous-time model
G = linearize('rct_robotarm',LinIOs,SnapshotTimes,LinOpt);

size(G)
```
```6x1 array of state-space models.
Each model has 4 outputs, 4 inputs, and between 0 and 13 states.
```

The robot arm model linearizes to zero at t=0 due to the Bicep and Forearm joints hitting their mechanical limits:

```getPeakGain(G(:,:,1))
```
```ans =

0

```

Plot the gap between the linearized models at t=1,2,3,4 seconds and the final model at t=5 seconds.

```G5 = G(:,:,end);  % t=5
G5.SamplingGrid = [];
sigma(G5,G(:,:,2:5)-G5,{1e-3,1e3}), grid
title('Variation of linearized dynamics along trajectory')
legend('Linearization at t=5 s','Absolute variation',...
'location','SouthWest')
```

While the dynamics vary significantly at low and high frequency, the variation drops to less than 10% near 10 rad/s, which is roughly the desired control bandwidth. Small plant variations near the target gain crossover frequency suggest that we can control the arm with a single set of PID gains and need not resort to gain scheduling.

Tuning the PID Controllers with LOOPTUNE

With looptune, you can directly tune all four PID loops to achieve the desired response time with minimal loop interaction and adequate MIMO stability margins. The controller is tuned in continuous time and automatically discretized when writing the PID gains back to Simulink. Use the slTunable interface to specify which blocks must be tuned and where the plant/controller boundary is.

```% Use linearization at t=3s for plant model
op = findop('rct_robotarm',3);
TunedBlocks = {'TurntablePID','BicepPID','ForearmPID','WristPID'};
ST0 = slTunable('rct_robotarm',TunedBlocks,op);

% Control signals are the PID outputs
% Measurement signals at the joint angles
```

In its simplest use, looptune only needs to know the target control bandwidth, which should be at least twice the reciprocal of the desired response time. Here the desired response time is 1 second so try a target bandwidth of 5 rad/s (bearing in mind that the plant dynamics vary least near 10 rad/s).

```wc = 5;  % target gain crossover frequency
ST1 = looptune(ST0,wc);
```
```Final: Peak gain = 1, Iterations = 54
Min decay rate 1e-07 is close to lower bound 1e-07
```

A final value near or below 1 indicates that looptune achieved the requested bandwidth. Compare the responses to a step command in angular position for the initial and tuned controllers.

```RefSignals = {'tREF','bREF','fREF','wREF'};
T0 = getIOTransfer(ST0,RefSignals,'Robot Arm');
T1 = getIOTransfer(ST1,RefSignals,'Robot Arm');

opt = timeoptions; opt.IOGrouping = 'all'; opt.Grid = 'on';
stepplot(T0,'b--',T1,'r',4,opt)
legend('Initial','Tuned','location','SouthEast')
```

The four curves settling near y=1 represent the step responses of each joint, and the curves settling near y=0 represent the cross-coupling terms. The tuned controller is a clear improvement but should ideally settle faster with less overshoot.

Exploiting the Second Degree of Freedom

The 2-DOF PID controllers have a feedforward and a feedback component.

Figure 4: Two degree-of-freedom PID controllers.

By default, looptune only tunes the feedback loop and does not "see" the feedforward component. This can be confirmed by verifying that the and parameters of the PID controllers remain set to their initial value (use showTunable for this purpose). To take advantage of the feedforward action and reduce overshoot, replace the bandwidth target by an explicit tracking requirement from reference angles to joint angles.

```TR = TuningGoal.Tracking(RefSignals,'Robot Arm',0.5);
ST2 = looptune(ST0,TR);
```
```Final: Peak gain = 1.06, Iterations = 72
```
```T2 = getIOTransfer(ST2,RefSignals,'Robot Arm');
stepplot(T1,'r--',T2,'g',4,opt)
legend('1-DOF tuning','2-DOF tuning','location','SouthEast')
```

The 2-DOF tuning reduces overshoot and takes advantage of the and parameters as confirmed by inspecting the tuned PID gains:

```showTunable(ST2)
```
```Block "rct_robotarm/Controller/TurntablePID" =

1                s
u = Kp (b*r-y) + Ki --- (r-y) + Kd -------- (c*r-y)
s              Tf*s+1

with Kp = 12.427, Ki = 7.3436, Kd = 0.65591, Tf = 0.024911, b = 0.90521, c = 1.8123.

Continuous-time 2-DOF PID controller.

-----------------------------------

Block "rct_robotarm/Controller/BicepPID" =

1                s
u = Kp (b*r-y) + Ki --- (r-y) + Kd -------- (c*r-y)
s              Tf*s+1

with Kp = 12.3814, Ki = 7.1887, Kd = 1.5736, Tf = 0.48625, b = 0.7019, c = 1.7493.

Continuous-time 2-DOF PID controller.

-----------------------------------

Block "rct_robotarm/Controller/ForearmPID" =

1                s
u = Kp (b*r-y) + Ki --- (r-y) + Kd -------- (c*r-y)
s              Tf*s+1

with Kp = 20.2349, Ki = 40.4771, Kd = 1.0118, Tf = 0.013114, b = 0.57636, c = 1.2497.

Continuous-time 2-DOF PID controller.

-----------------------------------

Block "rct_robotarm/Controller/WristPID" =

1                s
u = Kp (b*r-y) + Ki --- (r-y) + Kd -------- (c*r-y)
s              Tf*s+1

with Kp = 28.8414, Ki = 6.4949, Kd = 0.83132, Tf = 0.013636, b = 0.9661, c = 1.4881.

Continuous-time 2-DOF PID controller.

```

Validating the Tuned Controller

The tuned linear responses look satisfactory so write the tuned values of the PID gains back to the Simulink blocks and simulate the overall maneuver. The simulation results appear in Figure 5.

```writeBlockValue(ST2)
```

Figure 5: Tuned angular response.

The responses look good except for the Bicep joint whose response is somewhat sluggish and jerky. It is tempting to blame this discrepancy on nonlinear effects, but this turns out to be just cross-coupling effects between the Forearm and Bicep joints. To see this, plot the step response of these two joints for the actual step changes occurring during the maneuver (-10 deg for the Bicep joint and 60 deg for the Forearm joint).

```H2 = T2(2:3,2:3) * diag([-10 60]);  % scale by step amplitude
H2.u = {'Bicep','Forearm'};
step(H2,5), grid
```

When brought to scale, the first row of plots show that a 60-degree step change in Forearm position has a sizeable and lasting impact on the Bicep position. This explains the sluggish Bicep response observed when simultaneously moving all four joints.

Eliminating Undesirable Cross-Couplings

Seen from the Bicep joint, a step change in Forearm position acts as an external disturbance. Better rejection of this disturbance is the key to improving the Bicep response. Plot the magnitude of the closed-loop transfer function from fREF (Forearm reference angle) to the Bicep angle.

```s = tf('s');
bodemag(T2('Robot Arm(2)','fREF'),0.02*s,'g--',{1e-1,1e3}), grid
```

Notice the hump near 1 rad/s. This is responsible for the lasting (low frequency) offset in the Bicep position. To mitigate this, you need to lower the gain below 1 rad/s and ideally keep it below the dashed green line. Add a requirement to enforce this gain limit and keep overshoot in check by also limiting the peak gain from reference to actual angles.

```% Reduce cross-coupling forearm -> bicep
MG = TuningGoal.Gain('fREF','Robot Arm(2)',0.02*s);
MG.Focus = [0 1];

% Limit overshoot to 5%
OS = TuningGoal.Overshoot(RefSignals,'Robot Arm',5);

% Retune controller with all three requirements in force
ST3 = looptune(ST0,TR,MG,OS);
```
```Final: Peak gain = 1.01, Iterations = 115
```

Compare results with the previous design.

```T3 = getIOTransfer(ST3,RefSignals,'Robot Arm');
stepplot(T2,'g--',T3,'m',4,opt)
legend('Tracking only','Tracking and rejection','location','SouthEast')
```

The cross-coupling terms settle faster now. Push the retuned values to Simulink for further validation.

```writeBlockValue(ST3)
```

The simulation results appear in Figure 6. The Bicep response is now on par with the other joints in terms of settling time and smoothness.

Figure 6: Tuned angular response with reduced Bicep-Forearm coupling.