Mathematik rund um einen Quadrokopter

Berechnungen, die ein µProzessor durchführen muss, um einen Quadro in der Luft zu halten.

Dass ein Quadrokopter nicht einfach so in der Luft bleibt und schon gar nicht eigenständig fliegen kann, wissen wir spätestens, seit wir die Funktion eines Gyros kennen. Die Motoren müssen auch wissen, wann sie wie schnell drehen müssen, um z.B. vorwärts zu fliegen, oder um keine Überschläge zu machen. Das alles übernimmt bei der Flight-Ctrl der kleine schwarze Käfer namens ATMEGA644(P). Was geht aber in diesem Käfer vor sich und wie errechnet er die Drehzahlen, damit das Ufo in der Luft bleibt? Das werde ich heute mal mit ein paar einfachen mathematischen Formeln versuchen, zu erklären.

Dazu erstmal ein paar Worte zu den verwendeten Kurzbezeichnungen in der Formelübersicht:

Null_Gyro_x => Bezeichnet den Nullwert des Gyros. Also der Wert, den der ATMEGA als Wert kennt, wenn sich der Gyro in Neutrallage befindet (keine Bewegung)
Mess_Gyro_x => Bezeichnet den Messwert, der vom Analog-Digitalwandler des ATMEGA geliefert wird
Diff_Gyro_x => Bezeichnet den Differnzwert des Gyros, wenn er sich in Bewegung befindet (Relativ zur Neutrallage)
Verstärkung_Gyro_x => Bezeichnet den Verstärkungsfaktor des Gyros, mit dem der Mikrokopter härter oder weicher gemacht werden kann

Stick_x => Geberwert, der von der Fernsteuerung übergeben wird
Gyromix_x => Berechneter Wert, aus Stickvorgabe und Gyro-Differenzwert
Motorx => Motor-Nummerierung (1 - 4 für 4 Motore)

Alle Formeln in diesem Beispiel bauen aufeinander auf.

Gyrowerte berechnen (Nick/Roll/Yaw) im Differential-Modus:

Diff_Gyro_nick = (Null_Gyro_nick - Mess_Gyro_nick) * Verstärkung_Gyro_nick
Diff_Gyro_roll = (Null_Gyro_roll - Mess_Gyro_roll) * Verstärkung_Gyro_roll
Diff_Gyro_yaw = (Null_Gyro_yaw - Mess_Gyro_yaw) * Verstärkung_Gyro_yaw

Gyros auf die Kanäle Mischen:

Gyromix_nick = Stick_nick - Diff_Gyro_nick (Wirkrichtungsumkehr: Gyro_nick = Stick_nick + Diff_Gyro_nick)
Gyromix_roll = Stick_roll - Diff_Gyro_roll (Wirkrichtungsumkehr: Gyro_roll = Stick_roll + Diff_Gyro_roll)
Gyromix_yaw = Stick_yaw - Diff_Gyro_yaw (Wirkrichtungsumkehr: Gyro_yaw = Stick_yaw + Diff_Gyro_yaw)

Kanalmischung auf die Motoren:

Pitch:

Motor1 = Stick_pitch
Motor2 = Stick_pitch
Motor3 = Stick_pitch
Motor4 = Stick_pitch

Yaw:

Motor1 = Motor1 - Gyromix_yaw
Motor2 = Motor2 + Gyromix_yaw
Motor3 = Motor3 - Gyromix_yaw
Motor4 = Motor4 + Gyromix_yaw

Nick:

Motor1 = Motor1 - Gyromix_nick
Motor3 = Motor3 + Gyromix_nick

Roll:

Motor2 = Motor2 - Gyromix_roll
Motor4 = Motor4 + Gyromix_roll

Bei diesem Berechnungsbeispiel wurde folgende Motoranordnung angenommen:

Motor1 = vorn
Motor2 = rechts
Motor3 = hinten
Motor4 = links

Mit Hilfe dieser Formeln ist es möglich in nahezu jeder Programmiersprache einen flugfähigen Quadrokopter zu bekommen.

$/!\$ ToDo: hier gibt es noch etwas zu tun. ACC-Berechnungen einfügen. Formelbeispiel Kalman-Filter. Formelbeispiel PID-Regelung für Heading-Hold.

KategorieTheorie