Quelles sont les méthodes pour gérer le décalage de la boussole (hystérésis dépendante de la vitesse)?

J'ai un robot à bande de roulement, avec des encodeurs de roue de faible précision pour suivre la distance et une boussole électronique pour déterminer le cap. La boussole a un retard important (> 1 seconde) lorsque le robot tourne rapidement, par exemple après avoir atteint un waypoint - pivotant en place pour pointer vers son nouveau cap.

Quels sont les moyens de gérer le décalage? Je pense que l'on pourrait prendre beaucoup de mesures et modéliser la réponse de la boussole. Cependant, cela semble problématique car il dépend du taux et je ne connais pas le taux instantané.

En tant qu'approche simple mais lente, je fais tourner le robot jusqu'à ce qu'il soit très grossièrement pointé dans la bonne direction, puis je fais de très petits virages incrémentiels avec de brèves pauses de mesure jusqu'à ce qu'il soit pointé dans le bon sens. Y a-t-il d'autres façons de gérer cela?

sensors compass

— VienneMike
source

Le décalage dans la boussole est dû à un filtre passe-bas, pour supprimer le bruit haute fréquence.

Il existe des magnétomètres plus chers qui ont moins de bruit et donc moins de décalage.
Il est également possible d'utiliser un gyroscope pour améliorer la précision. En fait, c'est ce que font les unités de mesure inertielle (IMU). Cela peut être accompli en utilisant un filtre de Kalman. L'amélioration de la précision permet de réduire le décalage, car une précision accrue réduit la dépendance à l'égard d'un filtre passe-bas pour supprimer le bruit. Le filtre de Kalman fusionne les données du magnétomètre et du gyroscope (qui mesure le taux de changement de cap).

Si vous vous en tenez à votre boussole actuelle, il y a deux solutions possibles (Attention, cela devient de plus en plus avancé, mais l'option 1 devrait être accessible à la plupart des gens sans trop de travail).

Vous pouvez essayer d'annuler le filtre. Cela peut éliminer le décalage, mais augmente également le bruit haute fréquence. Après cela, vous pouvez essayer de contrôler le robot en fonction de la nouvelle estimation du cap. Pour ce faire, vous devez expérimenter pour déterminer les paramètres du filtre passe-bas. Par exemple, en temps discret, vous pourriez trouver:

$\hat{θ} (t) = a_{0} θ (t) + a_{1} θ (t - 1) + \dots + a_{k} θ (t - k)$ $\hat\theta(t)=a_0\theta(t)+a_1\theta(t-1)+\cdots+a_k\theta(t-k)$ où est le cap estimé (sortie de la boussole) à le temps , est le cap réel (vérité fondamentale) au temps . $\hat\theta(t)$ $t$ $\theta$ $t$
Vous pouvez trouver les paramètres en faisant une expérience où vous mesurez la vérité du sol en utilisant d'autres moyens externes. Étant donné échantillons, vous avez cette équation: $a_i$ $n+k+1$
$[\begin{matrix} \hat{θ} (k) \\ ⋮ \\ \hat{θ} (k + n) \end{matrix}] = [\begin{matrix} θ (k) & θ (k - 1) & \dots & θ (0) \\ ⋮ & ⋮ & ⋮ \\ θ (k + n) & θ (k + n - 1) & \dots & θ (n) \end{matrix}] [\begin{matrix} a_{0} \\ a_{1} \\ ⋮ \\ a_{k} \end{matrix}]$ $\left[\matrix{\hat\theta(k)\\\vdots\\\hat\theta(k+n)}\right]=\left[\matrix{\theta(k)&\theta(k-1)&\cdots&\theta(0)\\\vdots&\vdots&&\vdots\\\theta(k+n)&\theta(k+n-1)&\cdots&\theta(n)}\right]\left[\matrix{a_0\\a_1\\\vdots\\a_k}\right]$
Et vous pouvez résoudre en trouvant: où est la matrice pseudo-inverse de . Il n'y a aucun moyen définitif de calculer , vous devinerez donc probablement. Pour les points bonus, cela suppose que le bruit est blanc et indépendant, mais vous pouvez d'abord le blanchir pour supprimer le biais, et donc améliorer votre estimation des paramètres.
$[\begin{matrix} a_{0} \\ a_{1} \\ ⋮ \\ a_{k} \end{matrix}] = {[\begin{matrix} θ (k) & θ (k - 1) & \dots & θ (0) \\ ⋮ & ⋮ & ⋮ \\ θ (k + n) & θ (k + n - 1) & \dots & θ (n) \end{matrix}]}^{+} [\begin{matrix} \hat{θ} (k) \\ ⋮ \\ \hat{θ} (k + n) \end{matrix}]$ $\left[\matrix{a_0\\a_1\\\vdots\\a_k}\right]=\left[\matrix{\theta(k)&\theta(k-1)&\cdots&\theta(0)\\\vdots&\vdots&&\vdots\\\theta(k+n)&\theta(k+n-1)&\cdots&\theta(n)}\right]^{+}\left[\matrix{\hat\theta(k)\\\vdots\\\hat\theta(k+n)}\right]$ $M^+$ $M$ $k$
Vous pouvez le convertir en une fonction de transfert (également connue sous le nom de transformée en Z dans le domaine temporel discret):

$\frac{\hat{Θ} (z)}{Θ (z)} = a_{0} + a_{1} z^{- 1} + . . . + a_{k} z^{- k}$ $\frac{\hat\Theta(z)}{\Theta(z)}=a_0+a_1 z^{-1}+...+a_k z^{-k}$
Pour annuler cela, nous pouvons prendre l'inverse (où est notre nouvelle estimation de cap): $\bar\theta(t)$

$\frac{\bar{Θ} (z)}{\hat{Θ} (z)} = \frac{1}{a_{0} + a_{1} z^{- 1} + \dots + a_{k} z^{- k}}$ $\frac{\bar\Theta(z)}{\hat\Theta(z)}=\frac{1}{a_0+a_1 z^{-1}+\cdots+a_k z^{-k}}$
Reconversion dans le domaine temporel:

$a_{0} \bar{θ} (t) + a_{1} \bar{θ} (t - 1) + \dots + a_{k} \bar{θ} (t - k) = \hat{θ} (t)$ $a_0\bar\theta(t)+a_1\bar\theta(t-1)+\cdots+a_k \bar\theta(t-k)=\hat\theta(t)$
$\bar{θ} (t) = \frac{\hat{θ} (t) - a_{1} \bar{θ} (t - 1) - \dots - a_{k} \bar{θ} (t - k)}{a_{0}}$ $\bar\theta(t)=\frac{\hat\theta(t)-a_1\bar\theta(t-1)-\cdots-a_k \bar\theta(t-k)}{a_0}$
nous pouvons alors utiliser pour contrôler le robot. $\bar\theta$

Ce sera très bruyant, donc vous voudrez peut-être toujours mettre travers un filtre passe-bas avant utilisation (bien que peut-être celui avec moins de décalage). $\bar\theta$
La solution ci-dessus n'est toujours pas la meilleure solution. L'estimation bruyante peut ne pas être très utile. Si nous mettons cela dans une équation de l'espace d'états, nous pouvons concevoir un filtre de Kalman et un contrôleur de rétroaction d'état complet en utilisant LQR (régulateur quadratique linéaire). La combinaison d'un filtre de Kalman et d'un contrôleur LQR est également connue sous le nom de contrôleur LQG (gaussien linéaire quadratique) et utilise la récupération par transfert de boucle pour obtenir un bon contrôleur.

Pour ce faire, imaginez les équations espace-état (temps discret):

$\vec{x}(t)=A\vec{x}(t-1)+B\vec{u}(t-1)$ , $\vec{y}(t)=C\vec{x}(t)$

ou:
$\vec{x} (t) = [\begin{matrix} θ (t) \\ θ (t - 1) \\ \dots \\ θ (t - k) \end{matrix}] = [\begin{matrix} A_{1} & A_{2} & \dots & 0 & 0 & 0 \\ 1 & 0 & \dots & 0 & 0 & 0 \\ 0 & 1 & \dots & 0 & 0 & 0 \\ ⋮ & ⋮ & ⋮ & ⋮ & ⋮ \\ 0 & 0 & \dots & 1 & 0 & 0 \\ 0 & 0 & \dots & 0 & 1 & 0 \end{matrix}] \vec{x} (t - 1) + [\begin{matrix} B_{0} \\ B_{1} \\ 0 \\ ⋮ \\ 0 \\ 0 \end{matrix}] \vec{u} (t - 1)$ $\vec{x}(t)=\left[\matrix{\theta(t)\\\theta(t-1)\\\cdots\\\theta(t-k)}\right]= \left[\matrix{ A_1&A_2&\cdots&0&0&0\\ 1&0&\cdots&0&0&0\\ 0&1&\cdots&0&0&0\\ \vdots&\vdots&&\vdots&\vdots&\vdots\\ 0&0&\cdots&1&0&0\\ 0&0&\cdots&0&1&0 }\right] \vec{x}(t-1) + \left[\matrix{B_0\\B_1\\0\\\vdots\\0\\0}\right]\vec{u}(t-1)$
$\vec{y} (t) = [\begin{matrix} \hat{θ} (t) \end{matrix}] = [\begin{matrix} a_{0} \\ a_{1} \\ ⋮ \\ a_{k} \end{matrix}] \vec{x} (t)$ $\vec{y}(t)=\left[\matrix{\hat\theta(t)}\right]=\left[\matrix{a_0\\a_1\\\vdots\\a_k}\right]\vec{x}(t)$
où représente la puissance dans les moteurs pour faire tourner le robot, et , , , est la mesure dans laquelle elle affecte le cap en fonction de la position et de la vitesse (vous pouvez choisir des valeurs non nulles pour les autres éléments de la matrice , et la première ligne de la matrice également). $\vec{u}(t-1)$ $A_0$ $A_1$ $B_0$ $B_1$ $B$ $A$

Ensuite, vous pouvez construire votre observateur (filtre de Kalman), en choisissant les estimations de bruit et pour le bruit de processus et le bruit de mesure. Le filtre de Kalman peut alors trouver l'estimation optimale du cap, compte tenu de ces hypothèses sur le bruit. Après avoir choisi les estimations de bruit, la mise en œuvre dépend simplement de la mise en œuvre du code pour le filtre de Kalman (les équations peuvent être trouvées sur Wikipedia, donc je ne vais pas le passer en revue ici). $Q_o$ $R_o$

Après cela, vous pouvez concevoir un contrôleur LQR, cette fois, en choisissant et représentant la pondération accordée à la régulation du cap, et en essayant de limiter l'utilisation des actionneurs. Dans ce cas, vous pouvez choisir et . Cela est dû au fait que LQR trouve le contrôleur optimal pour minimiser une fonction de coût: $Q_c$ $R_c$ $Q_c = \left[\matrix{1&0&0&\cdots&0\\0&0&0&\cdots&0\\\vdots&\vdots&\vdots&&\vdots\\0&0&0&\cdots&0}\right]$ $R_c = \left[1\right]$ $J = \sum{(\vec{x}^T Q\vec{x} + \vec{u}^T R \vec{u})}$

Ensuite, vous venez de le passer par l'équation algébrique de Riccati à temps discret:

$P = Q + A^{T} (P - P B {(R + B^{T} P B)}^{- 1} B^{T} P) A$ $P = Q + A^T \left( P - P B \left( R + B^T P B \right)^{-1} B^T P \right) A$
et résoudre pour une matrice définie positive . $P$

Ainsi, votre loi de contrôle peut être donnée par:

$\vec{u} (t) = - K (\vec{x} (t) - {\vec{x}}_{r e f} (t))$ $\vec{u}(t)=-K(\vec{x}(t)-\vec{x}_{ref}(t))$
où $K = (R + B^T P B)^{-1}(B^T P A)$

Enfin, le faire ne fonctionnera pas très bien et risque d'être instable à cause du bruit. En effet, cela signifie que l'option 1 ne fonctionnera probablement pas à moins que vous ayez d'abord mis travers un filtre passe-bas (bien que pas nécessairement avec un temps de retard effectif aussi long). En effet, alors que LQR est garanti stable, dès que vous utilisez un filtre Kalman, la garantie est perdue. $\bar\theta$

Pour résoudre ce problème, nous utilisons la technique de récupération par transfert de boucle, où vous ajustez le filtre de Kalman, et choisissez à la place un nouveau , où est votre matrice origine , réglé pour que le filtre de Kalman soit optimal . est une matrice symétrique définie positive, que vous pouvez simplement choisir comme matrice d'identité ( ). Ensuite, choisissez simplement un scalaire . Le contrôleur résultant devrait devenir (plus) stable sous la forme , bien que la matrice devienne , ce qui signifie qu'elle devient moins optimale. $Q_o = Q_0 + q^2BVB^T$ $Q_0$ $Q$ $V$ $V=I$ $q$ $q \rightarrow \infty$ $Q_o$

Par conséquent, vous augmentez simplement jusqu'à ce qu'il soit stable. Une autre façon d'essayer de le rendre stable est d'augmenter (ou de diminuer ) pour le contrôleur LQR. $q$ $R_c$ $Q_c$

Les concepts de cet article sont assez avancés, mais si vous avez besoin de résoudre des choses comme l'équation de Riccati, vous pouvez utiliser MATLAB ou un autre logiciel pour le faire. Il peut également y avoir des bibliothèques implémentant déjà le filtre de Kalman (encore une fois, je pense que MATLAB le fait également).

Pour une application intégrée, vous devrez peut-être implémenter le filtre Kalman vous-même, bien qu'il existe probablement une implémentation C.

— ronalchn
source

Merci pour votre réponse excellente et approfondie. Je suis l'essentiel de votre première solution et je suis sûr que je peux y travailler. Le deuxième, comme vous le dites, est plus difficile et je devrai travailler pour voir si je peux tout suivre.

— ViennaMike

Un gyroscope est la réponse simple. J'ai toujours entendu, gyroscope pour les mesures courtes, boussole pour les longues. Et de façon réaliste, une tasse de filtre kallman entre les deux la plupart du temps. Le prix d'une carte gyroscope / acc 6DOF est inférieur à 20 $ de nos jours, beaucoup trop bon marché pour ne pas en utiliser une.

À un moment donné, j'ai travaillé sur le filtre Kallman de quelqu'un d'autre . et l'a fait fonctionner. Un filtre kallman est en fait plus une approche, pas une implémentation exacte, et dans le cas du gyroscope, le résultat final n'a pas besoin d'utiliser des mathématiques matricielles. Cela rend le code beaucoup plus simple.

— Spiked3
source