www.gusucode.com > control_featured 案例源码程序 matlab代码 > control_featured/RobotArmExample.m

    %% 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.

% Copyright 1986-2012 The MathWorks, Inc.

%% 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.
%
% <<../robotarm1.png>>
%
% *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).
%
% <<../robotarm2.png>>
%
% *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.
% 
% <<../robotarm3.png>>
%
% *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)

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

getPeakGain(G(:,:,1))

%%
% 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
addPoint(ST0,TunedBlocks)

% Mark joint angles as plant outputs
addPoint(ST0,'Robot Arm')

%%
% 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);

%%
% 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.
% 
% <<../robotarm4.png>>
%
% *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 $b$ and $c$
% parameters of the PID controllers remain set to their initial value $b=c=1$
% (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);

%%

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 $b$ and $c$ 
% parameters as confirmed by inspecting the tuned PID gains:

showTunable(ST2)

%% 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)

%%
% 
% <<../robotarm5.png>>
%
% *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;

%%
% To prevent jerky transients and avoid overloading the motors, limit the
% control bandwidth by imposing -20 dB/decade roll-off past 20 rad/s.

s = tf('s');
RO = TuningGoal.Gain(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);

%%
% 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.
% 
% <<../robotarm6.png>>
%
% *Figure 6: Angular response with refined controller.*