Verbesserte Kamerahalterung

Im ersten Teil der Ingenieurpraxis wurde eine Kamerahalterung auf auf Arduino Basis konstruiert. Später wurden dann Möglichkeiten zur Optimierung gefunden. Im Folgenden sollen die vorgenommenen  Veränderungen beschrieben werden.

Es gab hauptsächlich zwei Modifikationen:  Der Inertialsensor wurde gegen ein anderes Modell ausgetauscht und ein anderer Regel-Algorithmus wurde verwendet.

1. Hardwareänderungen

Bisher wurden zwei dedizierte Sensoren (Accelerometer und Gyroskop) zu einer logischen Einheit zusammengefasst. Über einen gemeinsamen Bus war es möglich auf sie zuzugreifen.

Dies machte es jedoch schwierig, die Sensoren gleichzeitig mit der gleichen Frequenz abzutasten. Das hat zur Folge, dass die Daten die ausgelesen werden nicht zur selben Zeit gemessen wurden.  Fehlerhafte Berechnungen sind die Folge.

Deswegen wurde entschieden, auf den Inertialsensor MPU6050 umzusteigen. Für diesen sprechen neben der Tatsache, dass die genutzten Sensortypen (Accelerometer und Gyroskop) auf einem Chip untergebracht sind und somit gleichzeitig ausgelesen werden können, auch der integrierte digitale Hardware-Tiefpass-Filter. Auch eine auf Interrupts basierende Lösung ist mit diesem Modell möglich.

Das von 5 bis 200Hz konfigurierbare Hardware-Filter entlastet den Mikrocontroller und arbeitet aufgrund der internen Abtastrate von 1khz genauer und schneller als ein Software Filter.

Die Elektronik wurde erneut auf eine Lochrasterplatine steckbar gelötet. Im Vergleich zur alten Platine wurde darauf geachtet, weniger Lot und Draht zu verwenden, indem die Komponenten möglichst optimal positioniert wurden. Der Rest der Hardware ist gleich geblieben.

 

 Links: alt, rechts: neu

1.2 Pinbelegungen

 

MPU6050 (IMU):

Pin Ziel Pin
VCC VCC 3V3
SDC CONVERTER  TXI_C2
SDA CONVERTER  TXI_C1
AD0 VCC 3V3
INT ARDUINO 3
XCL NC NC
XDA NC NC

 

Level-Converter:

Pin Ziel Pin
HV VCC 5V
LV VCC 3V3
TXO_C1 ARDUINO A4
TXO_C2 ARDUINO A5
TXI_C2 IMU SCL
TXI_C1 IMU SDA

 

NRF24L01+:

Pin Ziel Pin
SCK ARDUINO 13
MISO ARDUINO 12
MOSI ARDUINO 11
CSN ARDUINO 10
CE ARDUINO  8
INT ARDUINO  2
VCC VCC 3V3

 

Servo Signalleitung

Pin Ziel Pin
SIG_X ARDUINO  5
SIG_Y ARDUINO  9
SIG_Z ARDUINO  6

 

 2. Software

Die Steuerungssoftware wurde im Wesentlichen um den Treiber für den Inertialsensor und dem  Regelalgorithmus ergänzt.

Da die Nutzung der MPU6050-Library bereits gut beim I2cDev-Projekt (http://www.i2cdevlib.com/usage) dokumentiert ist soll nun auf die Funktionsweise des Regelalgorithmus eingegangen werden:

Der Algorithmus basiert auf der Implementierung von Robert Mahony's DCM Algorithmus durch Sebastian Madgwick (http://www.x-io.co.uk/open-source-imu-and-ahrs-algorithms/ ) und wurde im Rahmen des FreeIMU-Projekts (http://www.varesano.net/projects/hardware/FreeIMU) auf die Arduino-Plattform portiert.

Die Funktionsweise soll kurz dargestellt werden:

Das Ziel des Algorithmus ist die Fusion der Sensordaten von Accelerometer und Gyrometer. Genauer gesagt gilt es die Drehwinkel zwischen einer Referenz Orientierung und der Orientierung des Sensor zu bestimmen. Dazu wird die Richtung der Gravitation gemessen und die Abweichung zu einem Referenzvektor ermittelt.

Hier wird diese Aufgabe als Minimierungsproblem formuliert. Und zwar gilt es ein Quaternion (oder alternativ eine Drehmatrix) zu finden, so dass die Abweichung vom gemessenen Vektor zum gedrehten Referenzvektor minimal wird.

Es empfiehlt sich für ein besseres Verständnis nach nachfolgenden Codes, die Arbeiten von Magdwick und Mahony zu lesen.

Implementiert sieht es dann wie folgt aus:

   float halfvx, halfvy, halfvz;

    // Normalise accelerometer measurement
    recipNorm = invSqrt(ax * ax + ay * ay + az * az);
    ax *= recipNorm;
    ay *= recipNorm;
    az *= recipNorm;

    // Estimated direction of gravity
    halfvx = q1q3 - q0q2;
    halfvy = q0q1 + q2q3;
    halfvz = q0q0 - 0.5f + q3q3;

 // Error is sum of cross product between estimated direction and measured direction of field vectors
    halfex = (ay * halfvz - az * halfvy);
    halfey = (az * halfvx - ax * halfvz);
    halfez = (ax * halfvy - ay * halfvx);

 

 Zunächst werden die Messwerte vom Accelerometer normiert. Danach wird mit den vorangegangenen Werten die Richtung der Gravitation geschätzt um damit anschließend  die Abweichung zu den Messwerten zu berechnen

 

  // Apply feedback only when valid data has been gathered from the accelerometer or magnetometer
  if(halfex != 0.0f && halfey != 0.0f && halfez != 0.0f) {
    // Compute and apply integral feedback if enabled
    if(twoKi > 0.0f) {
      integralFBx += twoKi * halfex * (1.0f / sampleFreq);  // integral error scaled by Ki
      integralFBy += twoKi * halfey * (1.0f / sampleFreq);
      integralFBz += twoKi * halfez * (1.0f / sampleFreq);
      gx += integralFBx;  // apply integral feedback
      gy += integralFBy;
      gz += integralFBz;
    }
    else {
      integralFBx = 0.0f; // prevent integral windup
      integralFBy = 0.0f;
      integralFBz = 0.0f;
    }

    // Apply proportional feedback
    gx += twoKp * halfex;
    gy += twoKp * halfey;
    gz += twoKp * halfez;
  }

Hier wird die soeben ausgerechnete Abweichung einem PI-Regler mit den Verstärkungsfaktoren KP und KI zugeführt um damit die gemessenen Winkelgeschwindigkeiten in (gx,gy,gz) zu korrigieren.

 

Nun wir die Ableitung des Orientierungs-Quarternions durch das Quaternionprodukt mit (gx,gy,gz) anschließender Integration berechnet und normiert:

  // Integrate rate of change of quaternion
  gx *= (0.5f * (1.0f / sampleFreq));   // pre-multiply common factors
  gy *= (0.5f * (1.0f / sampleFreq));
  gz *= (0.5f * (1.0f / sampleFreq));
  qa = q0;
  qb = q1;
  qc = q2;
  q0 += (-qb * gx - qc * gy - q3 * gz);
  q1 += (qa * gx + qc * gz - q3 * gy);
  q2 += (qa * gy - qb * gz + q3 * gx);
  q3 += (qa * gz + qb * gy - qc * gx);

  // Normalise quaternion
  recipNorm = invSqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3);
  q0 *= recipNorm;
  q1 *= recipNorm;
  q2 *= recipNorm;
  q3 *= recipNorm;

 

Um nun die Servos verstellen zu können, muss nochmal die Richtung der Gravitationaus dem Quaternion geschätzt werden um aus diesem Vektor die Winkel berechnen zu können. Die entsprechenden Winkel (hier ypr[1] und ypr[2])  um die Y-Achse bzw. X-Achse entsprechen dem Winkel, die der Vektor mit der Y-Z- bzw. der X-Z-Ebene einschließt. Dies stellt für Winkel unter 90° eine gute Approximation dar:

  
 gx = 2* (q[1]*q[3] - q[0]*q[2]); 
 gy = 2 * (q[0]*q[1] + q[2]*q[3]); 
 gz = q[0]*q[0] - q[1]*q[1] - q[2]*q[2] + q[3]*q[3];

 ypr[1] = atan(gx * invSqrt(gy*gy + gz*gz));
 ypr[2] = atan(gy * invSqrt(gx*gx + gz*gz));

 

Nach der Berechnung, werden von den Winkeln die Offsets abgezogen und an die Servos übermittelt.

 

3. Inbetriebnahme

 

Montiert wird die Halterung, indem sie zunächst an den Rahmen des Quadrocopters geklemmt wird und anschließend zur Stabilisierung die Gegenstücke festgeschraubt werden.

Danach wird das Stromkabel, das von einem der Motorregler 5V abzweigt, angeschlossen.

 

Dabei ist es wichtig auf die Markierung an der Halterung zu achten. Rot steht für 5V, schwarz für GND.

Alternativ kann ein 7.4V Akku über einen Spannungswandler angeschlossen werden (Polarität beachten).

Danach sollte die Halterung die Kamera stabilisieren und Befehle von der Fernbedienung empfangen. Sollte dies nicht geschehen, so ist ein Reset des Arduino ratsam.

Anhänge:
Diese Datei herunterladen (Halterung.zip)Halterung.zip[ ]%2013-%10-%31 %0:%Okt%+01:00