Part I : PD Control with Gravity CompensationIn a common robotics textbook such as , robot control is classified to two types: 1. independent joint control, and 2. multivariable control. For the first category, a controller, either a PID or custom-designed one, is connected with each joint of the robot to form an SISO feedback loop. The interaction between adjacent links is modelled as a disturbance signal entering at some point, normally at the plant input, which can be attenuated using feedback. We discussed this approach in our H-inf synthesis articles. The second category, on the other hand, takes into account the robot dynamics and uses that information to craft a multivariable controller that tackles all robot joints as a whole. Since the robot dynamics is time-varying and configuration-dependent, the resulting controller is governed by a system of nonlinear differential equations.
In this article, we discuss some basics of robot nonlinear control and show how to compute and simulate with Scilab and Xcos. To focus only on the essence, a simple 2-link planar manipulator in Figure 1.1 is used as our robot model. We will see that even for this 2D robot consisting of 2 revolute joints, the control laws can be quite complicated.
- PD with gravity compensation
- inverse dynamics
- adaptive control
(1)with the corresponding terms
(2)This is just an example of a general robot equation of motion in matrix form
(3)with in this case (neglecting actuator inertia), and no friction term . So for compactness, we can write (1) in matrix form as
PD Control Scheme with Gravity CompensationA PD control is a subset of PID (Proportional Integral Derivative), which is a linear, general purpose controller used in various industrial applications. A PD control law results when the integral term of PID is turned off. When a gravity compensation term is added, the control law becomes nonlinear. A block diagram for PD control with gravity compensation is shown in Figure 1.2, with the control law described as
Enter these variables to Scilab workspace. Now we can run mdl_2link.zcos to see an open-loop responses to sinusoidal commands at the two joints. The position, velocity, and acceleration for each of the two joints are plotted. Try changing the magnitude and frequency of the commands, and turn on/off either of them to see the effect on the responses. You can also vary the robot parameters and evaluate whether the responses change as expected. After getting a bit comfortable with the 2-link manipulator plant, we move on to add the feedback controller to the model, as depicted in Figure 1.5, which conforms to the block diagram in Figure 1.2. The step blocks are used as command inputs, with switches to turn them on/off. The PD feedback part is trivial. The gravity compensation terms need some more clarification.
m1 = 5; m2 = 5; l1 = 1; l2 = 1; lc1 = l1/2; lc2 = l2/2; I1 = 2; I2 = 2; g = 9.81;
where m1_e, m2_e, lc1_e, lc2_e, I1_e, I2_e represent the estimated plant data. These values are used by the nonlinear expression blocks which are a part of the controller. Note: when parameters vary randomly, the simulation result is slightly different between each run. To see clearly the effect from interaction between the robot joints, we set the step command activation time of joint 1 and 2 to and second, respectively. The proportional and derivative gains can be set to your desired values by clicking on the corresponding gain blocks. To simplify the discussion, we choose the and gains for each joint identical; i.e., and with I a 2 x 2 identity matrix and kp, kd are scalars. Figure 1.6 results from setting and varying to 3 values: . As expected, the response is faster with increasing , though it also worsens the overshoots and disturbance responses
et = 0.7; m1_e = m1 + et*m1*(rand()-0.5); m2_e = m2 + et*m2*(rand()-0.5); lc1_e = lc1 + et*lc1*(rand()-0.5);lc2_e = lc2 + et*lc2*(rand()-0.5); I1_e = I1 + et*I1*(rand()-0.5); I2_e = I2 + et*I2*(rand()-0.5);
Now, put this trajectory data to a format that Xcos could read. Suppose we name the variables in From Workspace blocks for the first and second joints as Q1_d and Q2_d, respectively. Issue the following commands
t=0:0.01:5; [q1_d,qd1_d,qdd1_d]=lspb(0,30,t); [q2_d,qd2_d,qdd2_d]=lspb(0,-10,t);
Running the model pd_2link_track.zcos with 3 different gain values yields the tracking responses in Figure 1.8. Tracking performance can be determined more clearly by plotting the tracking errors as in Figure 1.9 and computing their RMS values.
Q1_d.time = t'; Q1_d.values = q1_d; Q2_d.time = t'; Q2_d.values = q2_d;
Scilab/Xcos FilesFor your convenience, here are the model and script files used in this article.
- mdl_2link.zcos open-loop 2-link manipulator with sinusoidal commands
- pd_2link_step.zcos step response simulation
- pd_2link_track.zcos tracking response simulation (requires RTSX)
- M.W.Spong, S. Hutchinson and M. Vidyasagar, Robot Modeling and Control. John Wiley & Sons. 2006.
- V. Toochinda, Robot Analysis and Control with Scilab and RTSX, Mushin Dynamics, 2014.