How many times have you seen a video of a robot that moves in a jerky motion? This is probably due to an on/off approach to speed control. This is an easy trap when controlling a servo. You have to move from A to B. One second you are setting it to position A and the next you set it to position B. The servo responds with a jack-rabbit start and gets to the new position as quickly as possible.
A better approach is to accelerate slowly till you reach full speed and then decelerate to stop. The need for this was first illustrated when a motor being controlled jumped and fell off my table. Since then, the speed profile has been tested by keeping the motor on its own on a flat surface. If it falls over, it has failed the test.
First for some motion basics. The position of the servo is determined by the pulse width of the signal to the servo motor. Vary the pulse width and the servo motor will move at a rate as fast as it can - and this is often too fast. The rate of change of position is velocity. The rate of change of velocity is acceleration. The rate of change of acceleration is jerk, a term that is encountered less often. Even rarer are the further derivatives snap, crackle and pop. No, really.
The key to reduce the jerkiness of motion is to keep the value of jerk as close to zero as possible. This means the acceleration has to be constant. The only control the servo controller has over the motor is the position. This is done by varying the pulse width of the signal to the servo motor.
The concepts of position, velocity and acceleration can be applied to their rotational counterparts angular position, angular velocity and angular acceleration in the same way. Instead of distance (metres), just use angle (degrees). Apart from the change in units, the laws of motion are the same. Let us look at a typical robot servo, the RDS3115.
Max. Speed = 0.16s/60° Pulse Width = 500us to 2500us Angle of Travel = 270° (estimated)
Let us start with an assumption that the motor reaches its top speed from a standing start in 30°.
Vmax = 60° / 0.16s = 375°/s
s = 30°
v2 = u2 + 2as
u = 0
V2max = 2 * amax * 30
amax = (375 * 375) / (2 * 30)
amax = 2343°/s2 ≈ 2500°/s2
s = v2 / 2a = (375 * 375) / (2 * 2500) = 28°
So we have values of an acceleration of 2500°/s2 and it takes 28° to reach the top speed of 375°/s.
The signal to the servo motor has a period of 20ms. So every 20ms, a change in position can be sent. This 20ms is handled using a timer interrupt. The time can also be expressed in interrupt cycles or ic. In addition, the change in angular position of 270° is achieved by changing the signal width from 500us to 2500us - in other words a change of 2000us. So the angular position can also be expressed as a pulse width in us. So, let us convert the time to units of ic and position to ussig.
1s = 1000/20 ms = 50 ic
1° = 2000/270 = 7.4ussig
Now let us convert the vmax and amax to their new units.
Vmax = 375°/s = 375 * 7.4/50 = 55.5 ≈ 55ussig/ic amax = 2500°/s2 = 2500 * 7.4 / (50 * 50) = 7.4 ≈ 7 ussig/ic2
So for the RDS3115, the Vmax is 55 ussig/ic. This means the pulse width should ideally change by 55us with each interrupt cycle for the servo motor to run at the max speed i.e. each successive interrupt cycle, the pulse width increases as 500, 555, 610, 665 and so on. The amax is 7ussig/ic2. This means to provide the given acceleration, the V should be increased by 7us each ic i.e. if you are increasing the v at the max. acceleration from 0, with each successive interrupt cycle, the V increases as 0, 7, 14, 21 and so on till it reached 55. So what angular distance is travelled (in terms of ussig) before the motor goes from a standing start to full speed?
v = 55, a = 7, s = ?
v2 = u2 + 2as
u = 0
s = v2 / 2a = (55 * 55) / (2 * 7) = 216 ussig
So to go from A to B without being a complete jerk, you ramp up the velocity by 7 from the current ussig every interrupt cycle. You keep doing this till the signal has increased by 216ussig. Then you stop accelerating and stay at this max speed. Then when you are 216ussig from the destination, you start decelerating at the rate of 7. So you will have 216ussig to accelerate to max speed, a certain time at max speed and then 216ussig to decelerate to a stop. For each interrupt cycle, increase the speed by the amax value. Then add the speed to the position to give the new position. Do this till max speed is reached and then keep the speed constant. At some point, reduce speed by amax till the final position is reached. The values will be as below.
| Interrupt Cycle | Acceleration (ussig/ic2) |
Velocity (ussig/ic) |
Signal width (ussig) |
| 1 | 0 | 0 | x |
| 2 | 7 | 7 | x + 7 |
| 3 | 7 | 14 | x + 21 |
| 3 | 7 | 21 | x + 42 |
| Accelerating... | |||
| 8 | 0 | 55 | x + 251 |
| Constant speed... | |||
| 12 | 0 | 55 | x + 471 |
| 13 | -7 | 48 | x + 519 |
| Deaccelerating... | |||
| 20 | -7 | 0 | x + 660 |
Or to see it all graphically...
From the graph, we can see the key is to keep a constant acceleration (green line) till max speed (red line) is reached. Then maintain max speed. After that, keep a constant deceleration till it reaches the final position.
So what happens when the distance to be travelled is so small that there is no time to reach max speed? Accelerate till you reach the halfway point and then start decelerating. You will not reach max speed in this case.
So to implement this, the startPos holds the start position of the motor, the endPos is set to the desired position. The currPos is the current position that is updated each interrupt cycle and sent out as a pulse width. The rampDist is calculated based on the max speed and acceleration. These values are accessed from EEPROM as fields of a structure called configSave. The code, implemented on an ESP8266 NodeMCU, is given below.
int rampDist = (configSave.confMaxSpeed * configSave.confMaxSpeed) / (2 * configSave.confAccel);
...
if (currPos != endPos) {
fwdFlag = (unsigned char)(endPos > currPos);
if (fwdFlag) {
if (rampUpFlag != 0) {
if (2 * currPos > startPos + endPos
&& currPos + rampDist > endPos) {
// Halfway there - start decelarating
rampUpFlag = 0;
}
else {
speed += configSave.confAccel;
if (speed > configSave.confMaxSpeed) {
// Max speed reached, start cruising
speed = configSave.confMaxSpeed;
}
else {
// Still accelerating to reach max speed
}
currPos += speed;
}
}
if (rampUpFlag == 0) {
if (currPos + rampDist > endPos) {
// Start decelerating
speed -= configSave.confAccel;
if (speed < configSave.confAccel) {
speed = configSave.confAccel;
}
}
currPos += speed;
}
if (currPos > endPos) {
currPos = endPos;
}
}
else {
// Similar logic when it is going in reverse
if (rampUpFlag != 0) {
if (2 * currPos < startPos + endPos
&& currPos - rampDist < endPos) {
rampUpFlag = 0;
}
else {
speed += configSave.confAccel;
if (speed > configSave.confMaxSpeed) {
speed = configSave.confMaxSpeed;
}
currPos -= speed;
}
}
if (rampUpFlag == 0) {
if (currPos - rampDist < endPos) {
speed -= configSave.confAccel;
if (speed < configSave.confAccel) {
speed = configSave.confAccel;
}
}
currPos -= speed;
}
if (currPos < endPos) {
currPos = endPos;
}
}
}
This approach of changing the speed is called the trapezoidal profile. This is because the speed is the shape of a trapezoid. There are more complex profiles, like the S profile. A good description of various profiles can be seen here. Looking at the graph, the acceleration is constant except for the interrupt cycle when it reaches max speed where it drops to 0 and then when it decelerates where it drops to the negative maximum. The jerk is zero except for these two points. The S profile is more complex but minimizes that jerk.
The values for max speed (55) and acceleration (7) are approximate values for an RDS3115 with almost no load. In servo control, as in life, you have to find a happy medium between getting there fast and at the same time not coming across as a jerk. These values will have to be tweaked depending on the power of the motor and the load attached to it. Ideally, these will be individually set for each servo motor in a chain and will probably be saved in EEPROM.
No comments:
Post a Comment