| View previous topic :: View next topic |
| Author |
Message |
stefanengelke
Joined: 03 Jan 2006
Posts: 59
Location: Stuttgart, Germany
|
 Posted: Sat Jan 14, 2006 6:31 pm Post subject: Motion Control: State Regulator |
 |
|
The PID and IPD algorithms are very well implemented and should be suitable for a wide range of applications. The PID is a good choice for everyone who don't want the get too proficient in automatic control theory because it is easy to find information about how the parameters of a PID can be tuned intuitive. But even among control engineers it is the most popular controler because its behavior is very well understood. Nevertheless the OpenServo has the potential for more sophisticated concepts. One of them is the State Regulator I want to introduce here.
I will try to explain the mathematical background because one of the biggest advantages of the the State Regulator is that there are methods to directly calculate the control parameters without the need of a iterative tunig process. My far aim is to build a tool to calculate the parameters so those of you who don't want to get to much in detail with the mathematics are although be able to use the advantages of a state regulator.
First we need a mathematical model of the servo. Here we look at the input-output-behavior to get a connection between the PWM we give to the motor and the position of the servo.
| Code: | _______
PWM | | Position
---->| Motor |----->
|_______| |
One of Mike's measurements shows this behavior:
To descripe the behavior of a DC-motor there are different approaches. The most common one splits the dynamics into two parts:
1. The electro-magnetic dynamics wich descripes the connection between the voltage at the motor connectors and the torque of the motor and
2. the mechanical dynamics wich descripes the connection between motor torque and postion of the servo.
To keep the mathematical model simple I assume a proportional behavior for the first dynamic. I have experimented a litte bit with more complex models, for example a first order model for the dynamic of the magnetic field and the induced voltage to the armature coil. This model takes also a backward induced electro-magnetic-force into account wich occurs in high speed motion of the motor. Perhaps we can use such a model later to analyse the current drained by the motor because the current is propotianal to the state of the magnetic field. But for the development of the controler the simpler model will be good enough. So we can descripe the first dynamics with a algebraic expression like this:
| Code: | | torque = k_motor * PWM |
where k_motor is a motor constant.
The second dynamics can be derived with the Principle of Conservation of Angular Momentum. It says that the change of the angular momentum is equal to the sum of moments:
| Code: | | J p(t)'' = sum( M_i ) |
where J is the moment of inertia, p(t)'' the second derivation of the servo position (or the acceleration) and M_i the moments to the shaft. In our chase we have three moments:
- The motor torque,
- a viscose friction and
- a external load.
So we can descripe the mechanical dynamics of the servo with the differential equation:
| Code: | | J p(t)'' = torque - d p(t)' + load |
where d is the viscose friction constant, p(t)' the speed and load the external load.
You can illustrate the differential equation with this diagram:
| Code: | external load
| _____ _____
motor torque | accelr.| | speed | | position
----->(+)->[ 1/J ]----->| int |---o-->| int |---------->
| moment of |_____| | |_____|
| inertia |
| |
+-----[ -d ]<--------------+
viscose friction
|
Last edited by stefanengelke on Sat Jan 14, 2006 7:47 pm; edited 1 time in total |
|
| Back to top |
|
 |
stefanengelke
Joined: 03 Jan 2006
Posts: 59
Location: Stuttgart, Germany
|
| Posted: Sat Jan 14, 2006 6:33 pm Post subject: |
|
|
With this model you can simulate the input-output-behavior to identify the system parameters J, d and k_motor. But with only one measurement it is not possible to identify all 3 parameters, because there are infinity posibilities. Imagin a small servo with a small torque but therfore a small moment of inertia and a small friction parameter. It will have the same behavior like a bigger servo with all parameters raised in the same proportion. To determine the true set of parameters we have to compare two measurements, one without a load and one with a load where we know the moment of inertia. To see the quality of the result I have assumed a motor constant k_motor=1 and have identified the other two parameters. With this set of parameters
| Code: | k_motor = 1
J = 0.0077 Nms?
d = 0.0689 Nm/s |
the simulated servo behavior looks like this:
To finalize the modeling part of the controler development we transform the dynamical equations in a common used form, called the state-space-form. Therefore we transform the second-order differential equation
| Code: | | p(t)'' = - d/J p(t)' + k_motor/J PWM + load/J |
into two first-order differential equations:
| Code: | p(t)' = v(t)
v(t)' = - d/J v + k_motor/J PWM + load/J |
If we define the state vector x, the input u and the output y like this
| Code: | x(t) = | p(t) | u(t) = PWM y(t) = Position
| v(t) | |
we can write the two differential equations in a matrix notation:
| Code: | x(t)' = | 0 1 | x(t) + | 0 | u(t) + | 0 | load
| 0 -d/J | | k_motor/J | | 1/J |
^^^^^^^^ ^^^^^^^^^^^
A B
y(t) = | 0 1 | x(t)
^^^^^^^
C |
Because we don't know anything about the load at this point, we take it completly as a disturbance to our controler. If the controler is robust enough, it will compensate the influence of the disturbance. Later we can use knowledge about the load to adapt the control parameters.
The equation for the servo dynamics without load can be illustration with this diagram:
| Code: | _____
PWM x' | | x Position
--->[ B ]-->(+)--->| int |----o-->[ C ]--->
Input ^ |_____| | Output
Matrix | | Matrix
| |
+------[ A ]<----+
System
Matrix |
to be continued.. |
|
| Back to top |
|
|
stefanengelke
Joined: 03 Jan 2006
Posts: 59
Location: Stuttgart, Germany
|
| Posted: Wed Jan 18, 2006 9:55 am Post subject: |
|
|
The state regulator assume, that we know every state of the system. In our case this means we have to know the current position and the current speed. How to reconstruct states we can't measure is another task but for now we assume that we know every state. This means the output-matrix C vanishes.
The state regulator itself is very simple now. It just takes the difference between the current state and a command state and calculates the control signal as weighted sum:
| Code: | | u = K ( x_command - x ) |
where K is a row vektor with the control parameter (in the case we have more then one input, it is a matrix).
In our case this yields to
| Code: | | u = K1 ( p_command - p ) + K2 ( v_command - v ) |
The closed control loop now looks like this:
| Code: | x' = A x + B K ( x_command - x )
_____
x_command u x' | | x
----->(+)->[ K ]--->[ B ]-->(+)-->| int |----o---+
^- ^ |_____| | |
| | | |
| | | |
| +-----[ A ]<----+ |
| |
+-----------------------------------------+
|
If we want to look at the dynamical behavior of the closed loop we can rewrite
| Code: | x' = (A - BK) x + BK x_command
_____
x_command x' | | x
------>[ BK ]-->(+)-->| int |-----o
^ |_____| |
| |
| |
+---[ A - BK ]<--+
|
where (A-BK) is the system matrix and (BK) the input matrix of the closed loop.
Now we have a expression that descripes the behavior of the closed loop in dependency to the control parameter K. At this point there are different design methods like pole placement and LQR to calculate K to reach a certain behavior.
One big problem of our current control structure is, that if we have a affine load, like the gravity in Mike's load-lifting scenario, there will be a steady control error. To solve this problem we virtualy extend our system with an integrator:
| Code: | _____ _______
PWM' | | PWM | | Position
u ----->| int |---->| Servo |----> y
|_____| |_______|
|
The integrator is later implemented in the controler but for the calculation of the control parameter we look at the merged system:
| Code: | p' = v
v' = -d/J v + 1/J PWM
PWM' = u |
The way how to calculate the control parameters is the same, only the state vector x, the system matrix A and the input matrix B are extended by one dimension. For the OpenServo this yields to
| Code: | | p | | 0 1 0 | | 0 |
x = | v | A = | 0 -d/J 1/J | B = | 0 |
| PWM | | 0 0 0 | | 1 | |
Futhermore we now calculating 3 instead of 2 control parameters. |
|
| Back to top |
|
|
guru
Joined: 03 Jan 2006
Posts: 128
Location: St Pete Beach, FL
|
| Posted: Sat Jan 21, 2006 4:00 am Post subject: |
|
|
Thanks stefanen,
I enjoyed your mathmatical explaination. The first graph triggered the DSP part of my brain. This is essentially much the same mathmatics for processing impulse responses, in fact, it is exactly, just a different application of the same theory. I understand what the math is doing, I understood some of the math itself, some!, but I like it. I like how it handle all the variances within the system. I wonder if a PIC/AVR is capable of implementing the algorithm.
C |
|
| Back to top |
|
|
stefanengelke
Joined: 03 Jan 2006
Posts: 59
Location: Stuttgart, Germany
|
| Posted: Sat Jan 21, 2006 2:24 pm Post subject: |
|
|
Colin,
You're right, processing impulse responses is just another way to analyse dynamic systems because a single impulse response characterizes the whole behavior of a linear system.
My description is still very theoretical, but I hope I can show how to implement soon. Mike has send me one of his OpenServos and I'm looking forward to tinkering with it.
The state regulator itself should be very easy to implement because it's quite similar to the PID in our case. The only thing I worry about is the speed estimator. But even if we use a numerical derivation like the PID does, the new control concept should provide some advantages.
If you have any questions with the mathmatics, feel free to send me a message. I benefit from your and Mikes electronic experience very much so I would be glad if I can give something back.
Stefan |
|
| Back to top |
|
|
stefanengelke
Joined: 03 Jan 2006
Posts: 59
Location: Stuttgart, Germany
|
| Posted: Sun Jan 29, 2006 11:06 pm Post subject: |
|
|
Yeah, after two sleepless nights it works 
I have realy underestimated the litte ATTiny45.
I have written a new motion algorithm wich controls the servo with a state regulator. To get the current speed of the servo I have implemented a Luenberg state estimator. It is a realtime simulation of the servo behavior with a internal controler wich keeps track of the difference between the simulated position and the measured one. A few milliseconds after starting the simulation, it runs parallel to the real states of the servo motion. But because it is a simulation we can read every state we want to know. With the knowledge of every state (in our case the position and the speed) the state regulator is easy to implement.
Here is a measurement of the new algorithm:
As you can see, we now have a very clear speed signal. Even if it looks like these curves are simulated on a pc, they aren't! They are captured from the OpenServo every few milliseconds and measured/calculated in realtime.
At the moment there is no additional integrator in the control scheme. Like I have mentioned above this will yield to a steady control error on a affine load. The characteristic hysteresis behavior of friction is such a affine load if we are near zero speed. You can clearly see the remaining non zero PWM signal after the servo has reached the new command position. This is because the controler sees the remaining little difference between the current and the command position. It produces a small PWM Signal to correct this error, but the signal is totally canceld by the friction. The solution to this problem is an additional integrator wich integrates this small error so that the correcting PWM is raised if nothing changes.
The remaining non zero PWM signal while standing still is confusing the estimator as well. Because the simulation don't take the friction into account it thinks the servo must accelerate. But the internal controler is trying to hold it back, because it sees that then the difference between the simulated and the measured position will raise. So the the simulation will remain in this steady error.
But nevertheless it's impressive to watch the servo moving with the new algorithm. There are absolute no virbrations while the servo holds the position and there is no overshooting even on big steps to the command position. ( I have calculated the current set of parameters with the pole placement methode, so suppressing overshooting was my main control target )
And here is a video of the OpenServo moving a improvisational load with the new motion control:
http://hal.wh-stuttgart.de/OpenServo/StateRegulator.wmv
My webcam produces a lot of motion blur, but I need the batteries of my digital camera to drive the servo and as weight
Stefan |
|
| Back to top |
|
|
mpthompson
Joined: 02 Jan 2006
Posts: 650
Location: San Carlos, CA
|
| Posted: Mon Jan 30, 2006 1:38 am Post subject: |
|
|
Stefan,
Very cool. I'll probably be asking many questions about the state regulator over the coming days.
One initial question I have is if the Luenberg state estimator you use to determine servo velocity can be made independent of the state regulator code in motion.c? For instance, just as position handled in the adc.c file, I think it would be useful to have a velocity.c file that handles velocity determination independent of the motion.c file. This will make it easier for the different motion algorithms to more easily share common velocity code -- hopefully the best velocity algorithm we can create.
-Mike |
|
| Back to top |
|
|
stefanengelke
Joined: 03 Jan 2006
Posts: 59
Location: Stuttgart, Germany
|
| Posted: Mon Jan 30, 2006 5:14 pm Post subject: |
|
|
Yes, the state estimator is independent of the regulator. It just needs the current position and the current PWM signal. I have seperated it in a own method by now.
I like the idea spending a new c-file for it but I would prefer another name. If we think about further extensions it could estimate the back-emf as well, for example. So why we don't call it estimator.c ?
Stefan |
|
| Back to top |
|
|
mpthompson
Joined: 02 Jan 2006
Posts: 650
Location: San Carlos, CA
|
| Posted: Mon Jan 30, 2006 7:16 pm Post subject: |
|
|
Sure, estimator.c would be fine. It should write it's output to REG_VELOCITY_HI/LO register pair which can be used by the other modules.
-Mike |
|
| Back to top |
|
|
|
FAQ
Search
Memberlist
Usergroups
Register
Profile
Log in to check your private messages
Log in
