Rigid Body Dynamics

Now, we are going to talk about joint control with disturbances. And the most obvious disturbance, the most common disturbance on a robotic arm is the disturbance due to gravity. Imagine a PUMA robot manipulator in this particular configuration. It’s got its arm stretched out horizontally and can you imagine that arm weighs quite a lot. So, there is a large torque on the shoulder joint and there’s also a large torque on the elbow joint that are required in order to keep the arm in that horizontal pose.

We can look at a more simplified version of that particular scenario. Consider here we have an electric motor which is connected to a horizontal bar. The bar has a centre of mass and the centre of mass is a distance r from the axis of the motor. And the bar has got a mass of m and the bar has a weight force, that’s the force due to gravity acting on the centre of mass and that is a force of mg acting in the downward direction.

Now, a force of mg with a lever arm length of r results in a torque acting on the motor shaft and the magnitude of that torque is given by the mass times the gravitational acceleration times the distance of the centre of mass from the axis of the motor. In this particular case, the torque is acting in a counter clockwise direction. The centre of mass wants to move vertically downward. If we actually do the maths for the case we looked at a moment ago, the PUMA robot with its arm in a horizontal configuration, the torque on the shoulder joint is 14 Newton meters.

If I assume that my arm is one meter long, that’s equivalent to holding four kilograms with my arm outstretched; that’s a pretty difficult thing to do, though there’s a lot of torque acting on the shoulder joint of the PUMA robot. And clearly, this is going to have an effect on the control of the shoulder joint. So, let’s look now in some more detail at our robot joint controller. And this is a block diagram we introduced in the last lecture. It’s a feedback controller. The input to the robot joint controller is the desired position, theta star. And the output of the joint controller is the actual joint position denoted by theta. And as we discussed in the last lecture, I’m using the Laplace transform of many of these variables; so not their response in the time domain but their response in the Laplace domain. And I denote the Laplace domain by using capital letters. So, that’s why I’ve used capital theta here and here. Now within this control loop, we have a control law, it might be a proportional derivative controller, it might be a proportional integral derivative controller known as a PID controller and we have the gain of the motor driver and we have the gain of the sensor and we have the transfer function of the actuator, that’s the electric motor which is driving the joint. And that has parameters such as the motor torque constant, KM; the inertia, J; and the viscous friction, B; and S is of course the Laplace operator. Now this is a very idealistic representation. There is no disturbance such as the force of gravity represented in this block diagram. So, let’s do something about that.

What I’ve done is I’ve moved the motor torque constant out of the actuator transfer function and made it an explicit gain block which you can see here. The output of that gain block is the actual motor torque.

Now what I’ve done is actually separated the motor into two components. I have a component which generates a torque and I have another component which represents the inertia and viscous friction of the motor.

Now, I’m going to introduce another summing junction which is a where we inject the disturbance torque. So at this particular summing junction, we have the sum of the torque which the motor creates and I add to that the torque which is coming from the disturbance source. It might be due to gravity. It might be due to something like Coulomb friction which we talked about in the last lecture and TD is the Laplace transform of that disturbance torque. 

The introduction of a disturbance, even a constant disturbance, will cause an error in the. If we now look a gain at that simplified schematic, we have the actual angle of the robot link and we have the desired angle of the robot link. And we can see that the actual angle is a little bit different, it’s downwards because the force of gravity is pulling it downwards and the motor controller hasn’t fully opposed that force of gravity. The controller that we just used is a proportional-plus-derivative controller often known as a PD controller and it’s very very commonly used in robotic systems. So the torque produced by the PD controller is a gain P multiplied by the error between where I want the joint angle to be, theta star, and the actual joint angle theta plus another gain termed D multiplied by the velocity of the joint that’s theta dot.

If we consider the steady state case where the joint is not moving, it’s deflected downwards by gravity; but it’s not moving any further downwards; it’s in equilibrium; then theta dot will be equal to zero. So, we can eliminate that term. And if the joint is in equilibrium, then the torque generated by the motor and its controller must be equal to the torque due to gravity. So, we can write this relationship. And if we do a little bit of rearrangement, we can see that the error between where we want the joint to be and where the joint actually is, is equal to MGR divided by P, our proportional gain.

Now, there are some very interesting consequences of that. And one is it if we want to reduce this error, we need to increase the value of P. If P is bigger because it’s in the denominator, then it makes the error much much smaller.
We are going to revisit the robot joint model that we built in Simulink in the last lecture. We’ve put the values of some of the parameters into the workspace and this is our model and we’re going to bring in my palette of useful Simulink objects as well. The first thing I’m going to do is to move all of this stuff across here and I’m going to introduce a new gain block which I’m going to drop in here. And what I’m going to do is to remove the gain termed KM from the numerator of the transfer function and I’m going to put it into this block here. So functionally, this is exactly the same. We have a gain KM multiplying this transfer function here. The output of this gain block is the torque that’s applied to the armature of the electric motor.

Now what I’m going to do is to introduce a disturbance torque. To do that, I need to find a summing junction and there’s a summing junction and I’m going to edit this so that it has got a plus in the upward direction.

I think that will do the trick and I’m going to remove that wire, put the summing junction in here, draw a wire from there to there. And the disturbance force is going to come from a step function, that’s the source. Here we go. And I want my step to occur at time five in the simulation. The simulation runs for 10 seconds, so the disturbance will occur at five seconds. Hence, it has got a value of one.
So, I’m going to connect that into the summing junction. All right, so what we have now is a model like what we had before. But now, I can introduce a disturbance torque which also acts on the armature of the electric motor. There are now two torques acting on the armature. There is the torque generated by the controller and there is the torque by this external disturbance. Now let’s run that. I’ll bring the scope into view, so we can see that, just across a little bit, and run my simulation. And the result looks like this. Let’s zoom in a little bit, let's auto-scale that and we can see that the motor has been following the square wave moving between minus one and plus one. But when the external disturbance is applied, we see that that has got a really big influence on the position of the motor. We could see that it has moved up and is now moving between the 3-1/2 and 5-1/2. So clearly this external force has had a really big impact on the performance of the motor controller and it’s not doing the job that we want it to do.

So, we’ve seen that there are some consequences of increasing the proportional gain P. Another strategy that we can use is to add an integral term and that turns our controller into a proportional integral and derivative controller very commonly known as a PID controller and what we’ve added here is the integral term.
We have another gain I which is the integral gain and that multiplies the time integral of the position error, theta star minus theta. What this means, in intuitive terms, is if there is a position error, the integral gradually increases over time and that increases the torque that’s applied to the joint so as to counter that particular error.

Now what we are going to do is to increase the proportional gain of the system and see if we can improve things, so here are the parameters of our controller. The proportional gain is equal to one. I’m going to double that. I’m going to make it equal to two. Close that and we’ll run our simulation again and see what happens to the step response. And now we see that the effect of the disturbance has been attenuated. We can see now that the effect of the disturbance is less because we’ve increased the proportional gain.

Let’s increase it some more. I’m going to increase it to a value of four, run that simulation again. Now we see that the effect of disturbance is even further reduced, but we’ve also noticed that the amount over shoot on our square wave response is much, much higher and this is a consequence of increasing the proportional gain and we can get around that to some extent by increasing the amount of derivative term, increase the amount of damping that will reduce the amount of overshoot. But there are other consequences besides.

What I am going to do is to add another scope to this diagram. And bring in the scope here and what it’s going to do is to show the torque that’s generated by the motor and let’s run this. 

So, I have a new scope, let's open that up and what we see here is the torque that we're asking the motor to generate. Now, we can see that there is spikes in the torque between about plus five to minus five Newton meters of torque for asking the motor to produce. As we increase the proportional gain, the maximum torque that we ask the motor to produce will keep increasing.

Any real motor has got a finite torque capability. It can't produce an infinite amount of torque. Now, ultimately, this is where the strategy of increasing the proportional gain will come unstuck. So, increase the proportional gain we’re asking the motor provide more torque and eventually, we’re be asking it to produce more torque than it actually can do.

So, we’ve seen the effects of increasing the proportional gain. We’ve seen the effect of adding an integral term. So, third strategy, that’s often use to eliminate this kind of error and that’s to introduce what’s called a feed forward term.

Here's our block diagram from earlier and we are going to introduce a feed forward control term. And the key part of feed forward control, is that in many cases, we actually can compute what the disturbance force will be. So, in the case of gravity, if we know the joint angles, we can compute what the gravity force is on each of the joints.

So, we can come up with an estimate of what the gravity disturbance is and this is got to be really helpful to us. This is extra knowledge about the disturbance in we should be able to use it to cancel the disturbance out. The way we do that is we add another summing junction into our control law and it's got a negative sign on it and so, this has got a negative gain or it's a subtraction. And then to make it to all work, to balance out, we have to add a gain in here. This is a gain of one on KdKm.

If you think about it, if we have our estimated torque here, we divided by KD and KM here. We multiply by KD and KM here. So, the overall gain is now one and then we add that to the actual disturbance and because of the negative sign here, the disturbance is cancelled out. This is the essence of feed forward control. It is a really neat trick.

Another strategy that we can adopt is to increase the integral gain at the moment is equal to zero. So, I’m going to put the proportional gain back to one and I’m going to increase the integral gain to a value of one. We are going to simulate that and see what happens. What we see is that the over shoot on the square wave response has been reduced. It's back to what it was previously when we had the proportional gain equal to one.

And here we can see the effect of the integral term coming into play. When the external disturbance is applied at time five, the error in the control system increases. That is then integrated over time to produce a torque which increases over time so as to push the error towards zero. We can see here the effect of the integrated term ramping up. It’s pushing the error down. From what we can see is an after short amount of time, the upward of the integrated has become equal to the value of the external disturbance. And it has effectively cancelled it out.
So, we can see towards the end of the trace that the square wave response is pretty much what we want it to be. The external disturbance has been rejected. Let’s summarize.

We've looked at a number of different techniques to reduce the effect of disturbance on the position of a robot joint. We started off looking at a very standard proportional derivative typically known as a PD controller which got a control law like this, a proportional gain multiplied by the position error plus a derivative gain multiplied by the velocity which helps to control over shoot when the joint is moving.

We saw that the amount of error is inversely proportional to the gain P. Now, we can increase P to reduce the error, but that has a number of adverse consequences such as increasing over shoot or providing signals to the motor that it cannot possibly achieve.

We then looked at an adding an integral term. The intuition is that, with the constant error, the integral over time is a signal that ramps up which increases the torque so as to oppose the torque that’s causing the disturbance in the first place.
The third approach, we looked at, is feed forward control which attempts to predict or estimate the disturbance and then cancel it out. A great advantage of feed forward control is even if you have a not perfect estimate of what the disturbance is, some cancellation is certainly better than no cancellation at all. In practice, we often use a mixture of feedback and feed forward control. They each have particular strengths and advantages.