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