Suite et Fin
Vous connaissez presque tout déjà... Reste (outre le code de traitement des messages I2C que je ne décris pas ici) le filtrage Kalman et le choix des infos envoyées à l'asservissement....
Kalman
Pourquoi faire ???
Le gyroscope est excellent pour son temps de réponse et son bruitage quasi nul. Mais il ne donne pas l'angle... juste la vitesse angulaire. Il dérive
L'accéléromètre fournit un angle absolu. Mais il est très bruité (ne serait-ce que par les vibrations du moteur) et, en raison de l'intégration nécessaire pour diminuer ce bruit, a un temps de réponse trop lent, trop mou.
Il faut donc "mélanger" ces deux données.... un filtre eût été bien (passe bas pour l'accéléromètre et passe haut pour le gyro) mais j'ai voulu suivre un peu la mode en utilisant un filtre de Kalman.
Le source C de référence est rotomation.c que vous trouverez en fouillant un peu sur internet. Il est superbement commenté et n'a posé aucun problème à intégrer. Il faut simplement songer à l'alimenter avec la vitesse angulaire de manière bien récurrente et lui injecter l'angle d'accéléromètre pour y récupérer l'angle calculé par le filtre.
Voici le code de la fonction
/*
* Kalman.c : source tirée de tilt.c de rotomation
*
* 1 dimensional tilt sensor using a dual axis accelerometer
* and single axis angular rate gyro. The two sensors are fused
* via a two state Kalman filter, with one state being the angle
* and the other state being the gyro bias.
*
* Gyro bias is automatically tracked by the filter. This seems like magic.
*
*/
/********************************************************************
*
* state_update (float Pitch gyro measurement)
*
********************************************************************
* state_update is called every dt with a biased gyro measurement
* by the user of the module. It updates the current angle and
* rate estimate.
*
* The pitch gyro measurement should be scaled into real units, but
* does not need any bias removal. The filter will track the bias.
*
* Our state vector is:
* X = [ angle, gyro_bias ]
* It runs the state estimation forward via the state functions:
* Xdot = [ angle_dot, gyro_bias_dot ]
* angle_dot = gyro - gyro_bias
* gyro_bias_dot = 0
* And updates the covariance matrix via the function:
* Pdot = A*P + P*A' + Q
* A is the Jacobian of Xdot with respect to the states:
* A = [ d(angle_dot)/d(angle) d(angle_dot)/d(gyro_bias) ]
* [ d(gyro_bias_dot)/d(angle) d(gyro_bias_dot)/d(gyro_bias) ]
* = [ 0 -1 ]
* [ 0 0 ]
*
*
* ENTREE : q_m = Pitch gyro measurement
*/
void state_update(float q_m) {
float q;
float Pdot[2*2];
q = q_m - Gyro_bias; /* Unbias our gyro */
// Compute the derivative of the covariance matrix
// Pdot = A*P + P*A' + Q
// We've hand computed the expansion of A = [ 0 -1, 0 0 ] multiplied
// by P and P multiplied by A' = [ 0 0, -1, 0 ]. This is then added
// to the diagonal elements of Q, which are Q_angle and Q_gyro.
Pdot[0]= Q_angle - P[0][1] - P[1][0]; /* 0,0 */
Pdot[1]= - P[1][1]; /* 0,1 */
Pdot[2]= - P[1][1]; /* 1,0 */
Pdot[3]= Q_gyro; /* 1,1 */
Gyro_rate = q; // Store our unbias gyro estimate
/* Update our angle estimate
* Angle_Kalman += angle_dot * dt
* += (gyro - gyro_bias) * dt
* += q * dt
*/
Angle_Kalman += q * Filtrage_Period;
/* Update the covariance matrix */
P[0][0] += Pdot[0] * Filtrage_Period;
P[0][1] += Pdot[1] * Filtrage_Period;
P[1][0] += Pdot[2] * Filtrage_Period;
P[1][1] += Pdot[3] * Filtrage_Period;
}
/********************************************************************
*
* kalman_update
*
********************************************************************
* kalman_update is called by a user of the module when a new
* accelerometer measurement is available. ax_m and az_m do not
* need to be scaled into actual units, but must be zeroed and have
* the same scale.
*
* This does not need to be called every time step, but can be if
* the accelerometer data are available at the same rate as the
* rate gyro measurement.
*
* For a two-axis accelerometer mounted perpendicular to the rotation
* axis, we can compute the angle for the full 360 degree rotation
* with no linearization errors by using the arctangent of the two readings.
*
* The C matrix is a 1x2 (measurements x states) matrix that
* is the Jacobian matrix of the measurement value with respect
* to the states. In this case, C is:
* C = [ d(angle_m)/d(angle) d(angle_m)/d(gyro_bias) ]
* = [ 1 0 ]
* because the angle measurement directly corresponds to the angle
* estimate and the angle measurement has no relation to the gyro bias.
*
*/
/*
void kalman_update(float ax_m, float az_m) {
float angle_m;
*/
// routine lancée dès que le résultat de l'accéléromètre est connu
// paramètre d'entrée = l'angle mesuré par l'accéléromètre
float kalman_update(float angle_m){
float angle_err;
float C_0;
float PCt_0;
float PCt_1;
float E;
float K_0;
float K_1;
float t_0;
float t_1;
//Compute our measured angle and the error in our estimate
// angle_m = atan2( -az_m, ax_m );
angle_err = angle_m - Angle_Kalman;
//C_0 shows how the state measurement directly relates to the state estimate.
// The C_1 shows that the state measurement does not relate
// to the gyro bias estimate. We don't actually use this, so we comment it out.
C_0 = 1;
/* float C_1 = 0; */
// PCt<2,1> = P<2,2> * C'<2,1>, which we use twice. This makes
// it worthwhile to precompute and store the two values.
// Note that C[0,1] = C_1 is zero, so we do not compute that term.
PCt_0 = C_0 * P[0][0]; /* + C_1 * P[0][1] = 0 */
PCt_1 = C_0 * P[1][0]; /* + C_1 * P[1][1] = 0 */
// Compute the error estimate. From the Kalman filter paper:
// E = C P C' + R
// Dimensionally,
// E<1,1> = C<1,2> P<2,2> C'<2,1> + R<1,1>
// Again, note that C_1 is zero, so we do not compute the term.
E = R_angle + C_0 * PCt_0 /* + C_1 * PCt_1 = 0 */ ;
// Compute the Kalman filter gains. From the Kalman paper:
// K = P C' inv(E)
// Dimensionally:
// K<2,1> = P<2,2> C'<2,1> inv(E)<1,1>
// Luckilly, E is <1,1>, so the inverse of E is just 1/E.
K_0 = PCt_0 / E;
K_1 = PCt_1 / E;
/* Update covariance matrix. Again, from the Kalman filter paper:
* P = P - K C P
* Dimensionally:
* P<2,2> -= K<2,1> C<1,2> P<2,2>
* We first compute t<1,2> = C P. Note that:
* t[0,0] = C[0,0] * P[0,0] + C[0,1] * P[1,0]
* But, since C_1 is zero, we have:
* t[0,0] = C[0,0] * P[0,0] = PCt[0,0]
* This saves us a floating point multiply.
*/
t_0 = PCt_0; /* C_0 * P[0][0] + C_1 * P[1][0] */
t_1 = C_0 * P[0][1]; /* + C_1 * P[1][1] = 0 */
P[0][0] -= K_0 * t_0;
P[0][1] -= K_0 * t_1;
P[1][0] -= K_1 * t_0;
P[1][1] -= K_1 * t_1;
//Update our state estimate. Again, from the Kalman paper:
// X += K * err
//And, dimensionally,
// X<2> = X<2> + K<2,1> * err<1,1>
// err is a measurement of the difference in the measured state
// and the estimate state. In our case, it is just the difference
// between the two accelerometer measured angle and our estimated angle.
Angle_Kalman += K_0 * angle_err;
Gyro_bias += K_1 * angle_err;
return Angle_Kalman;
}
Les appels à cette fonction ont lieu à chaque calcul :
appel à state_update à chaque calcule de la vitesse du gyro
appel à kalman_update (argument = angle mesuré ) pour calculer l'angle réel
Choix de la réponse envoyée à l'asservissement
En fonction du type de mesure demandée (avec la télécommande), on envoit à la carte d'asservissement
- Une vitesse angulaire toujours issue du gyroscope
-Un angle_K fonction de la demande :
Gyro + accéléromètre :
Angle_K= kalman_update(Moy_Angle_Accel);
Gyro + Opto
Angle_K= kalman_update(Angle_Opto);
Gyro seulement
Angle_K=Angle_Gyro +Trim_Angle_Gyro;
Notez qu'on n'utilise pas le filtre de Kalman dans ce dernier cas.
Conclusion
Voci terminé ce petit tour au milieu du code...Il manque les fonctions de messagerie I2C et l'assemblage final avec les initialisations, les cadencements, les divers tests d'état de fonctionnement mais ce n'était pas le but de ces articles. Si toutefois vous voulez le code complet (.C et .H) demandez le moi.
Actuellement, le mode gyro+accéléromètre est très bruité par les vibration des moteurs et pratiquement inutilié. Le mode Gyro+Opto est d'un fonctionnent irréprochable et le mode Gyro seul est très souvent utilisé en démonstration... Il n'est pas encore complet : il faut en effet travailler les situations où le robot est non vertical et bloqué sur un obstacle (table, chaise) : en effet, l'algorithme qui dit qu'il est vertical va peut à peu dériver vers la situation réelle, ce qui provoque un retour au balancement périlleux. Encore un effort ....
Enfin, il faut noter une fonction (non décrite ici) qui permet d'enregistrer 4 variables durant 3 secondes.... En branchant l'ICD en debug, on peut alors uploader ces données et , par export vers un tableur, on visualise les courbes : ceci m'a servi à comparer , au cours d'un balancement normal, les valeurs brutes, lissées de l'accéléromètre, de l'opto et du gyro... Une fonction qui vaut la peine d'être écrite car elle procure , en différé, un équivalent d'oscilloscope multicanaux...