So verschmelzen Sie lineare und winklige Daten von Sensoren

Mein Team und ich bauen einen Außenroboter mit Encodern, einer kommerziellen IMU und einem GPS- Sensor auf. Der Roboter verfügt über einen einfachen Panzerantrieb, sodass die Encoder ausreichend Ticks vom linken und rechten Rad liefern. Die IMU gibt Roll-, Nick-, Gier- und Linearbeschleunigungen in x, y und z an. Wir könnten später weitere IMUs hinzufügen, die Redundanz bieten, aber zusätzlich auch Roll-, Nick- und Gierwinkelraten bereitstellen. Das GPS veröffentlicht globale x-, y- und z-Koordinaten.

Wenn der Roboter die xy-Position und den Kurs des Roboters kennt, kann er die zu navigierende Umgebung lokalisieren und zuordnen. Die Geschwindigkeit des Roboters kann auch nützlich sein, um reibungslose Bewegungsentscheidungen zu treffen. Es ist ein bodengebundener Roboter, daher ist uns die z-Achse nicht besonders wichtig. Der Roboter verfügt außerdem über einen Lidar- Sensor und eine Kamera. Durch Drehen und Neigen können die Lidar- und Kameradaten zur besseren Orientierung umgewandelt werden.

Ich versuche herauszufinden, wie alle diese Zahlen auf eine Weise zusammengeführt werden können, die die Genauigkeit aller Sensoren optimal ausnutzt. Im Moment verwenden wir einen Kalman-Filter , um eine Schätzung [x, x-vel, x-accel, y, y-vel, y-accel]mit der einfachen Übergangsmatrix zu generieren :

[[1, dt, .5*dt*dt, 0,  0,        0],
 [0,  1,       dt, 0,  0,        0],
 [0,  0,        1, 0,  0,        0],
 [0,  0,        0, 1, dt, .5*dt*dt],
 [0,  0,        0, 0,  1,       dt],
 [0,  0,        0, 0,  0,        1]]

Der Filter schätzt den Zustand ausschließlich anhand der von der IMU bereitgestellten Beschleunigungen. (Die IMU ist nicht die beste Qualität. Innerhalb von 30 Sekunden zeigt sie den Roboter (in Ruhe), der gut 20 Meter von seiner ursprünglichen Position entfernt ist.) Ich möchte wissen, wie man Roll-, Nick- und Gierbewegungen von der verwendet IMU- und möglicherweise Roll-, Nick- und Gierraten, Encoderdaten von den Rädern und GPS-Daten zur Verbesserung der Zustandsschätzung.

Mit etwas Mathematik können wir die beiden Encoder verwenden, um X-, Y- und Steuerkursinformationen des Roboters sowie lineare und Winkelgeschwindigkeiten zu generieren. Die Encoder sind sehr genau, können jedoch auf Außenfeldern leicht verrutschen.

Es scheint mir, dass es hier zwei separate Datensätze gibt, die schwer zu verschmelzen sind:

1. Schätzungen von x, x-vel, x-accel, y, y-vel, y-accel
2. Schätzungen von Rollen, Neigen, Gieren und Raten von Rollen, Neigen und Gieren

Obwohl es eine Überkreuzung zwischen diesen beiden Sätzen gibt, habe ich Probleme damit, sie zusammenzusetzen. Wenn der Roboter zum Beispiel mit einer konstanten Geschwindigkeit fährt, ist die Richtung des Roboters, die durch seine x-vel und y-vel bestimmt wird, dieselbe wie sein Gieren. Wenn der Roboter in Ruhe ist, kann das Gieren jedoch nicht genau durch die x- und y-Geschwindigkeiten bestimmt werden. Die von den Encodern gelieferten Daten können in Winkelgeschwindigkeit umgerechnet eine Aktualisierung der Gierrate sein. Aber wie kann eine Aktualisierung der Gierrate letztendlich zu besseren Positionsschätzungen führen?

Ist es sinnvoll, alle 12 Zahlen in einem Filter zu speichern, oder werden sie normalerweise getrennt aufbewahrt? Gibt es bereits einen gut entwickelten Weg, um mit dieser Art von Problem umzugehen?

Zwei Dinge.

1. Wenn Sie eine Zuordnung vornehmen möchten, benötigen Sie einen vollständigen SLAM-Algorithmus (Simultaneous Localization and Mapping). Siehe: Gleichzeitige Lokalisierung und Zuordnung (SLAM): Teil I Die wesentlichen Algorithmen . In SLAM ist das Schätzen des Roboterzustands nur das halbe Problem. Wie das geht, ist eine größere Frage, als hier beantwortet werden kann.

2. In Bezug auf die Lokalisierung (Schätzung des Roboterzustands) ist dies keine Aufgabe für einen Kalman-Filter. Der Übergang von zu ist keine lineare Funktion aufgrund der Winkelbeschleunigungen und Geschwindigkeiten. Daher müssen Sie für diese Aufgabe nichtlineare Schätzer berücksichtigen. Ja, es gibt Standardmethoden dafür. Ja, sie sind in der Literatur verfügbar. Ja, normalerweise werden alle Eingänge in den gleichen Filter gelegt. Position, Geschwindigkeit, Orientierung und Winkelgeschwindigkeit des Roboters werden als Ausgaben verwendet. Und ja, ich werde hier eine kurze Einführung in ihre gemeinsamen Themen geben. Die Haupt-Take-Aways sind$x(t)=[x,y,\dot{x},\dot{y},\theta,\dot{\theta}]$$x(t+1)$

   1. Beziehen Sie die Gyro- und IMU-Voreingenommenheit in Ihren Bundesstaat ein, da sonst Ihre Schätzungen abweichen
   2. Ein erweiterter Kalman-Filter (EKF) wird häufig für dieses Problem verwendet
   3. Implementierungen können von Grund auf neu erstellt werden und müssen im Allgemeinen nicht "nachgeschlagen" werden.
   4. Für die meisten Lokalisierungs- und SLAM-Probleme gibt es Implementierungen. Machen Sie also nicht mehr Arbeit als nötig. Siehe: Roboterbetriebssystem ROS

Erklären Sie nun die EKF im Kontext Ihres Systems. Wir haben eine IMU + Gyro, GPS und Kilometerzähler. Der betreffende Roboter ist wie erwähnt ein Differentialantrieb. Die Filterung Aufgabe ist es, die aktuelle Pose Schätzung des Roboters zu nehmen , die Steuereingänge , und die Messungen von jedem Sensor, , und produziert die Schätzung bei der nächsten Zeitschritt . Wir nennen die IMU-Messungen , GPS ist und .${\hat{x}_t}$$u_t$$z_t$$\hat{x}_{t+1}$$I_t$$G_t$$O_t$

Ich gehe davon aus, dass wir daran interessiert sind, die Roboterhaltung als . Das Problem mit IMU und Gyros ist Drift. Die Beschleunigungen, die Sie im EKF berücksichtigen müssen, sind instationär. Dies geschieht (normalerweise), indem die Vorspannung in den geschätzten Zustand versetzt wird. Auf diese Weise können Sie die Abweichung bei jedem Zeitschritt direkt abschätzen. für einen Vektor von Verzerrungen .$x_t={x,y,\dot{x},\dot{y},\theta,\dot{\theta}}$$x_t={x,y,\dot{x},\dot{y},\theta,\dot{\theta},b}$$b$

Ich gehe davon aus:

1. $O_t$ = zwei Distanzmessungen, die die Distanz darstellen, die die Laufflächen in einem kleinen haben
2. $I_t$ = drei Orientierungsmessungen und drei Beschleunigungsmessungen .${\alpha, \beta, \theta}$${\ddot{x},\ddot{y},\ddot{z}}$
3. $G_t$ = die Position des Roboters im globalen Frame, .$^Gx_t,^Gy_t$

In der Regel ist es schwierig, das Ergebnis der Steuereingaben (gewünschte Geschwindigkeiten für jede Lauffläche) auf die Ausgaben abzubilden (die Änderung der Haltung des Roboters). Anstelle von ist es üblich (siehe Thrun , Odometry Question ), den Kilometerzähler als "Ergebnis" der Kontrolle zu verwenden. Diese Annahme funktioniert gut, wenn Sie sich nicht auf einer nahezu reibungslosen Oberfläche befinden. Wie wir sehen werden, können die IMU und das GPS dabei helfen, Schlupf zu korrigieren.$u$

Die erste Aufgabe besteht also darin, den nächsten Zustand aus dem aktuellen Zustand vorherzusagen: . Im Fall eines Differentialantriebsroboters kann diese Vorhersage direkt aus der Literatur (siehe Über die Kinematik mobiler Radroboter oder die präzisere Behandlung in einem modernen Robotiklehrbuch) oder von Grund auf wie hier gezeigt abgeleitet werden: Odometry Question .$\hat{x}_{t+1} = f(\hat{x}_t,u_t)$

Wir können also jetzt . Dies ist der Ausbreitungs- oder Vorhersageschritt. Sie können einen Roboter durch einfaches Propagieren bedienen. Wenn die Werte vollständig genau sind, haben Sie niemals eine Schätzung die nicht genau Ihrem wahren Zustand entspricht. Das passiert in der Praxis nie.$\hat{x}_{t+1} = f(\hat{x}_t, O_t)$$O_t$$\hat{x}$

Dies gibt nur einen vorhergesagten Wert aus der vorherigen Schätzung an und sagt uns nicht, wie sich die Genauigkeit der Schätzung mit der Zeit verschlechtert. Um die Unsicherheit zu propagieren, müssen Sie die EKF-Gleichungen (die die Unsicherheit in geschlossener Form unter Gaußschen Rauschannahmen propagieren), einen Partikelfilter (der einen stichprobenbasierten Ansatz verwendet) * und den UKF (der punktweise verwendet) verwenden Approximation der Unsicherheit) oder eine von vielen anderen Varianten.

Im Fall der EKF gehen wir wie folgt vor. Sei die Kovarianzmatrix des Roboterzustands. Wir linearisieren die Funktion Verwendung der Taylor-Reihenexpansion, um ein lineares System zu erhalten. Ein lineares System kann mit dem Kalman-Filter leicht gelöst werden. Angenommen, die Kovarianz der Schätzung zum Zeitpunkt ist , und die angenommene Kovarianz des Rauschens in der Kilometerzahl wird als die Matrix (normalerweise eine diagonale Matrix, wie ). . Im Fall der Funktion erhalten wir die Jacobi und$P_t$$f$$t$$P_t$$U_t$$2\times2$$.1\times I_{2\times 2}$$f$ $F_x=\frac{\partial f}{\partial x}$$F_u=\frac{\partial f}{\partial u}$, dann verbreiten die Unsicherheit als,

Jetzt können wir die Schätzung und die Unsicherheit verbreiten. Beachten Sie, dass die Unsicherheit mit der Zeit monoton zunimmt. Dies wird erwartet. Um dies zu beheben, wird normalerweise der vorhergesagte Zustand mit und aktualisiert. Dies wird als Messschritt des Filterprozesses bezeichnet, da die Sensoren eine indirekte Messung des Roboterzustands liefern.$I_t$$G_t$

Verwenden Sie zunächst jeden Sensor, um einen Teil des Roboterzustands als Funktion und für GPS, IMU zu schätzen . Bilden Sie das Residuum oder die Innovation, bei der es sich um die Differenz zwischen den vorhergesagten und den gemessenen Werten handelt. Schätzen Sie dann die Genauigkeit für jede Sensorschätzung in Form einer Kovarianzmatrix für alle Sensoren ( in diesem Fall , ). Suchen Sie schließlich die Jakobiner von und aktualisieren Sie die Zustandsschätzung wie folgt:$h_g()$$h_i()$$R$$R_g$$R_i$$h$

Für jeden Sensor mit Schätzung Zustand ( wikipedias Eintritt Nach )$s$$z_s$

Im Fall von GPS ist die Messung wahrscheinlich nur eine Transformation von Breite und Länge zum lokalen Rahmen des Roboters, sodass der Jacobian nahezu wird. wird in den meisten Fällen direkt vom GPS-Gerät gemeldet.$z_g=h_g()$$H_g$$R_g$

Im Fall der IMU + Gyro ist die Funktion eine Integration von Beschleunigungen und ein additiver Bias-Term. Eine Möglichkeit, mit der IMU umzugehen, besteht darin, die Beschleunigungen numerisch zu integrieren, um eine Positions- und Geschwindigkeitsschätzung zum gewünschten Zeitpunkt zu finden. Wenn Ihre IMU für jede Beschleunigungsschätzung einen kleinen additiven Rauschterm , müssen Sie dieses Rauschen integrieren, um die Genauigkeit der Positionsschätzung zu ermitteln. Dann ist die Kovarianz die Integration aller kleinen additiven Rauschausdrücke . Das Update für die Verzerrung einzubauen ist schwieriger und aus meiner Erfahrung heraus. Da Sie jedoch an planaren Bewegungen interessiert sind, können Sie das Problem wahrscheinlich vereinfachen. Dafür musst du in der Literatur nachsehen.$z_i=h_i()$$p_i$$R_i$$p_i$

Einige außergewöhnliche Referenzen:

1. Verbesserung der Genauigkeit der EKF-basierten visuellen Inertial-Kilometerzähler

2. Beobachtbarkeitsbasierte konsistente EKF-Schätzer für die kooperative Lokalisierung mehrerer Roboter

3. Adaptives zweistufiges EKF für lose gekoppeltes INS-GPS-System mit unbekannter Fehlervorspannung

Dieses Feld ist ausgereift genug, dass Google (Gelehrter) Sie wahrscheinlich eine funktionierende Implementierung finden könnte. Wenn Sie in diesem Bereich viel arbeiten werden, empfehle ich Ihnen, sich ein solides Lehrbuch zuzulegen. Vielleicht so etwas wie Probablistic Robotics von S. Thrun von Google Car. (Ich fand es eine nützliche Referenz für diese Late-Night-Implementierungen).

* Im Robot Operating System (ROS) sind mehrere PF-basierte Schätzer verfügbar . Diese wurden jedoch für den Innenbereich optimiert. Partikelfilter befassen sich mit den multimodalen PDFs, die sich aus der kartenbasierten Lokalisierung ergeben können (bin ich in der Nähe dieser Tür oder jener Tür). Ich glaube, dass die meisten Outdoor-Implementierungen (insbesondere diejenigen, die zumindest zeitweise GPS verwenden können) den Extended Kalman Filter (EKF) verwenden. Ich habe den erweiterten Kalman-Filter erfolgreich für einen Außenrover mit Differentialantrieb eingesetzt.

Sie können das Problem in den meisten Fällen erheblich vereinfachen:

• Viele IMus "kommerzieller Qualität" (z. B. Xsens) haben sehr laute Beschleunigungsmesser. Machen Sie sich nicht einmal die Mühe, sie zu verschmelzen, um Geschwindigkeit zu erreichen. Die Kilometerzahl ist bereits um Größenordnungen besser. Die einzigen verwendbaren Daten, die die IMU liefern wird, sind das Pitch and Roll und in gewissem Maße die Überschrift (siehe nächster Punkt).
• Überschrift von IMUs ist nicht so vertrauenswürdig. Es verwendet Magetometer und zeigt enorme Drifts (in unserem Fall bis zu 25 Grad über 2 m) in der Nähe von ferromagnetischen Massen, wie sie in Gebäudewänden zu finden sind. Um dies zu lösen, haben wir die IMU-Überschrift verwendet, aber eine Überschriftenverschiebung geschätzt.
• Wenn Sie sich im Freien aufhalten, vergessen Sie nicht, dass eine Fahrt mit 10 m bei einer Neigung von 10 Grad nicht zu denselben Änderungen in X und Y führt wie eine Fahrt mit 10 m auf ebenem Gelände. Dies wird normalerweise durch Schätzen von Z berücksichtigt, aber ich denke, es kann anders geschätzt werden.
• GPS ist auch eine lügende Schlampe, typischerweise in Umgebungen mit vielen Wegen. Außerdem neigen GPS-Geräte mit niedriger Qualität (und sogar unter bestimmten Bedingungen mit hoher Qualität) dazu, sehr falsche Standardabweichungen zu melden. Wir haben einige einfache Chi-Quadrat-Tests verwendet, um zu überprüfen, ob eine bestimmte GPS-Messung integriert werden sollte (dh zu überprüfen, ob sie bis zu einem bestimmten Punkt mit der aktuellen Filterschätzung übereinstimmt), was zu anständigen Ergebnissen führte.

Die "typische" Lösung für uns ist die Verwendung von Odometrie + IMU, um eine Ego-Bewegungsschätzung zu erhalten, und dann die Verwendung von GPS, um die X-, Y-, Z- und Steuerkursverschiebung zu korrigieren.

Hier ist eine EKF-Implementierung, die wir ausgiebig verwendet haben. Wenn Sie die Ausrichtung der IMU schätzen müssen (dh wenn sie noch keinen integrierten Filter hat), können Sie auch einen dieser beiden Filter verwenden: UKF und EKF .