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 slTuner interface to specify which blocks must be tuned and to locate the plant/controller boundary.

```% Linearize the plant at t=3s
tLinearize = 3;

% Create slTuner interface
TunedBlocks = {'TurntablePID','BicepPID','ForearmPID','WristPID'};
ST0 = slTuner('rct_robotarm',TunedBlocks,tLinearize);

% Mark outputs of PID blocks as plant inputs

% Mark joint angles as plant outputs
```

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
Controls = TunedBlocks;      % actuator commands
Measurements = 'Robot Arm';  % joint angle measurements
ST1 = looptune(ST0,Controls,Measurements,wc);
```
```Final: Peak gain = 1, Iterations = 57
Achieved target gain value TargetGain=1.
```

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,Controls,Measurements,TR);
```
```Final: Peak gain = 1.06, Iterations = 65
```
```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 1: rct_robotarm/Controller/TurntablePID =

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

with Kp = 13.5017, Ki = 12.9021, Kd = 0.54464, Tf = 0.030154, b = 0.85779, c = 2.0038.

Continuous-time 2-DOF PID controller.

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

Block 2: rct_robotarm/Controller/BicepPID =

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

with Kp = 13.2708, Ki = 6.8389, Kd = 1.5882, Tf = 0.61258, b = 0.72416, c = 1.5889.

Continuous-time 2-DOF PID controller.

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

Block 3: rct_robotarm/Controller/ForearmPID =

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

with Kp = 18.9643, Ki = 42.2787, Kd = 1.1583, Tf = 0.017729, b = 0.58255, c = 1.4574.

Continuous-time 2-DOF PID controller.

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

Block 4: rct_robotarm/Controller/WristPID =

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

with Kp = 6.6085, Ki = 14.552, Kd = 1.6992, Tf = 0.01376, b = 0.80141, c = 2.3179.

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 is in fact due to 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'};
H2.y = {'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.

Refining The Design

To improve the Bicep response for this specific arm maneuver, we must keep the cross-couplings effects small relative to the final angular displacements in each joint. To do this, scale the cross-coupling terms in the tracking requirement by the reference angle amplitudes.

```JointDisp = [60 10 60 90];  % commanded angular displacements, in degrees
TR.InputScaling = JointDisp;
```

```s = tf('s');
RO = TuningGoal.MaxGain(RefSignals,'Robot Arm',20/s);
```

Finally, explicitly limit the overshoot to 5% and increase the desired phase margin from its default value of 45 degrees to 60 degrees.

```OS = TuningGoal.Overshoot(RefSignals,'Robot Arm',5);
Options = looptuneOptions('PhaseMargin',60);
```

Retune the controller with the additional requirements in force

```ST3 = looptune(ST0,Controls,Measurements,TR,RO,OS,Options);
```
```Final: Peak gain = 1.06, Iterations = 83
```

Compare the scaled responses with the previous design. Notice the significant reduction of the coupling between Forearm/Wrist and Bicep motion, both in peak value and total energy.

```T2s = diag(1./JointDisp) * T2 * diag(JointDisp);
T3s = diag(1./JointDisp) * getIOTransfer(ST3,RefSignals,'Robot Arm') * diag(JointDisp);
stepplot(T2s,'g--',T3s,'m',4,opt)
legend('Initial 2-DOF','Refined 2-DOF','location','SouthEast')
```

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 smooth transient.

Figure 6: Angular response with refined controller.