Aversive/Modules/Control system/Filters/Quadramp derivate

De Wikidroids

Why a second quadramp ?

The utility of the quadramp_derivate is very similar to the normal quadramp. Hovewer, the functionnal principle is very different.

The goal achieved by this module is to provide a more robust algorithm against mechanical problems.


While the quadramp is a pure trajectory generator, which generates a trajectory to follow, the derivate version generates a speed, and the traj can be affected by the physical behaviour of the controlled process.


For example, if you block the motor (obstacle, mechanical problem, whatever...) the traditionnal quadramp will continue to generate the (then not useful anymore) trajectory exactly the same way. When recovering, the PID loop will be open (saturation), and then the speed and acceleration imits will be overlooked for a while. The process will probably come with exessive speed at the goal, and deccelerate too quickly!!

In the same situation, the derivate version will recover very quickly, and will not overlook the goal, because it takes into account the instaneous speed and position for generation of the trajectory.


Briefly said, the quadramp_derivate is far more robust against mechnanical defects than the quadramp.

A second advantage over the quadramp is the gain in computationnal speed (because no square root is needed).

operating principle : Pivot

what is the pivot ?

The basic principle is to generate an acceleration. This acceleration will have two values only : a positive and a negative one, for example +2 and -2.

To determine which value to use, we calculate a position called "pivot position". This pivot is in fact the position at which we must begin decceleration, to atteign the goal with a null speed, and considering the actual speed (not the consign !!).

The pivot position can be calculated by considering a constant decceleration, which gives :

P(t) = speed(t)^2 / (2* acceleration)
where speed(t) is the actual speed, and acceleration is the maximal acceleration.


and then ?

The resulting acceleration (+2 or -2) is then integrated (and limited to max) in order to obtain a useful speed consign. This speed consign will then be fed to a speed PID, typically.

The resulting total algorithm can be seen below :

Quadramp derivate principle.png

You can compare this with the traditionnal quadramp structure


Pivot tracking

Joining a given position is done in 3 phases (4 in fact)

  • A : acceleration  : the goal is far away, so we accelerate
  • C : constant speed : the maximal speed is atteined, so the acceleration is ignored(this phase may not exist if the position is not far away)
  • D : Decceleration  : the pivot has been crossed, so we reverse the acceleration. As the speed decreases, the pivot decreases. In fact we keep the pivot nearby. This is called "Pivot tracking".
  • G : Goal atteined  : the pivot algorithm is shut off to avoid little vibrations when the goal is atteined.

You can see a typical action of the algorith in the following chart :

Quadramp derivate tracking.png

At T=20 we change the goal position from 0 to 400 You can see the evolution of the pivot which reflects the actual speed (the square of it)


Some anticipation please

The main problem with that algorithm is that the speed PID will need some time to react. We then need to anticipate a little bit.

This is done by adding a pondered speed to the pivot the resulting equation is :

Pivot(t) = speed(t)^2 / (2* acceleration) + AnticipGain * abs(speed(t))
where speed(t) is the actual speed, and acceleration is the maximal acceleration.


This will make some little peaks visible in the D phase, if the anticipation gain is correctly set :

Quadramp anticipation.png

You can here see the difference between 3 typical situations. This shows that it is very easy to adjust the anticipation gain value.

It is possible to use an over-anticipation for smoothing the arrival.

TBD : test anticipation with more resolution on coders.

Interface and code example

For a complete code example done for brushless motor, look at the test

You need to declare two cs : one for the speed PID, and one for the position. we also need a pid and a biquad as derivation. Of course a quadramp_derivate is also needed:

struct cs                          speed;
struct pid_filter                  speed_pid;
struct biquad_filter               speed_derivation;

struct cs                          position;
struct quadramp_derivate_filter    position_quadr_derv;

Now we can initialize all this stuff. First the speed CS :

 // PID
 pid_init(&speed_pid);
 
 pid_set_gains(&speed_pid, xx,xx,xx);
 pid_set_maximums(&speed_pid,  xx,xx,xx);
 pid_set_out_shift(&speed_pid, xx);
 
 biquad_init(&speed_derivation);
 biquad_set_numerator_coeffs(&speed_derivation, 1,-1,0); // this is a discrete derivation : O(n) = I(n) - I(n-1)
 
 // control system speed
 cs_init(&speed);
 
 cs_set_correct_filter(&speed, pid_do_filter, &speed_pid);
 cs_set_process_in(&speed,  /* motor set torque command */);
 cs_set_process_out(&speed, /* motor get position command */);
 cs_set_feedback_filter(&speed, biquad_do_filter, &speed_derivation);
 cs_set_consign(&speed, 0);

And then the position CS :

 /** ramp generator parameters */  
 quadramp_derivate_init(&position_quadr_derv);
 
 quadramp_derivate_set_gain_anticipation(&position_quadr_derv, 256 *xx);// some anticipation (this is a 
                                                                 // fixed point value divided by 256!)
 
 quadramp_derivate_set_goal_window(&position_quadr_derv, xx);          // algorithm shut down window
 
 quadramp_derivate_set_2nd_order_vars(&position_quadr_derv,  xx , xx); // max acceleration
 quadramp_derivate_set_1st_order_vars(&position_quadr_derv,  xx,  xx); // max speed
 
 quadramp_derivate_set_divisor(&position_quadr_derv, xx);               // divisor, for precision


 // control system position 
 cs_init(&position);
 cs_set_correct_filter(&position, quadramp_derivate_do_filter, &position_quadr_derv); 
 cs_set_process_in(&position, (void *)cs_set_consign, &speed ); // interface with the speed
 cs_set_process_out(&position,  /* motor get position command */);
 cs_set_consign(&position, 0);

Now we must execute periodically only the following functions :

 cs_manage(&position);
 cs_manage(&speed);

This can be done directly over scheduler, or in all functions that can be called periodically. Hovewer it is preferable to use this order (position, and then speed) to minimize the loop lag.

Boîte à outils
LANGUAGES