Selbstbalancierender Roboter im Prototypen Stadium

**djdune** · 24.07.2012, 22:31

Hallo liebe Robotergemeinde,

ich möchte hier mal den Testaufbau meines Selbstbalacierenden Roboters vorstellen. Mein Plan ist es eigentlich einen Segway nachzubauen aber ich dachte mir dass ich erstmal ein kleines Modell baue, bevor ich viel Geld für Motoren und Elektronik rauswerfe. Mittlerweile bin ich ganz froh darüber weil er nicht so will wie ich.
Es gibt genügend Codes und Pläne für Segway Nachbauten im Netz, aber ich will es eben auf meine Weise bauen

Nun zum eigentlichen Projekt:
Alles ist aus Breakout Boards zusammengestöpselt
Eckdaten:

Controller: STM32F103VE 32bit ARM Cortex M3
Sensoren: 5DOF Board mit IDG300 und ADXL330
Bluetooth Modul zur Datenübertragung
Auf durchdrehen modifizierte Modellbau Servos als Antriebe
L298 Board für die Motoren
12V LiPo Akku mit 1800mAh
Räder von einem Modellflugzeug

So sieht er aus:

Klicke auf die Grafik für eine größere Ansicht

Name: seg01.jpg
Hits: 57
Größe: 110,3 KB
ID: 22888

Klicke auf die Grafik für eine größere Ansicht

Name: seg02.jpg
Hits: 76
Größe: 90,5 KB
ID: 22889

Ja, er sieht etwas wild aus, aber das Testobjekt zum üben reicht das komplett.
Und in Betrieb kommt das dabei heraus: http://www.youtube.com/watch?v=CkQ29...&feature=g-upl
In der Regelung sorgt ein Kalman Filter für eine schöne und saubere Winkelangabe. Für die Motoren habe ich einen PID Regler eingebaut, wobei I=0 ist, da das ja für diese Anwendung nicht notwendig ist.
Nun bin ich aber regelungstechnisch am Ende meines Wissens und habe keine Ahnung wie ich den Regler anpassen soll. Durch Try and Error habe ich zumindest schon das am Video zu sehende Ergebnis bekommen.
Vielleicht hat ja noch jemand eine Idee dazu.

**RP6conrad** · 25.07.2012, 08:20

Schwertepunkt erhohen ! Damit wird das ganse System langsamer und is einfacher zu regelen.

**oberallgeier** · 25.07.2012, 08:33

Zitat von djdune

... PID ... wie ... den Regler anpassen ... Try and ... schon ... Ergebnis ...

Klar, mit einem halbwegs ordentlich eingestellten Regler bekommt man schon ein halbwegs passendes Ergebnis. Bereits vor Jahren hatte waste ein ausführliches Regelun gstechnik-Tutorial im RNW issen erstellt, sehr ausführlich, sehr sauber aufgebaut, sehr langweilig durchzuarbeiten ABER mit sehr praktischen Hinweisen gerade zu digitalen Reglern. Dort gibt es die Beschreibung Einstellung nach der Sprungantwort (klick hier). Diesem Verfahren kann wegen seines Kochbuchaufbaues auch mit wenig regelungstechnischer Erfahrung gut abgearbeitet werden. Hier habe ich kurz dargestellt, oder auch mal hier, wie ich das für die Drehzahlregelung in meinen Zweirädrern angewendet hatte (zwei unabhängig angesteuerte Getriebemotoren aus gehackten Servos mit Zeitkonstanten "im System" von acht und zwölf Millisekunden). Mittlerweile arbeitet diese Regelung (>ausschließlich integer< , aber ich habe eben auch nur nen ATmega168/328-20MHz und keinen ARM) sehr befriedigend, kleines Beispiel siehe hier.

Achso - mit EINEM Sharp und einem P-Regler kann man auch balancieren!

Viel Erfolg.

**Inventor76** · 25.07.2012, 13:08

Na das sieht ja schon ganz nett aus für den Anfang. Ich bin selbst gerade dabei mir so einen "Zweiradler" zu bauen. Ich verwende 2 Getriebemotoren und hab gestern mal erfolgreich den Motortreiber dazu probehalber aufgebaut. Bezüglich der Regelung und der Sensorauswertung hab ich zwar schon so meine theoretischen Ideen, ob diese allerdings in der Praxis dann auch so klappen werden wird sich erst herausstellen. Ich hoffe das mein Balancierer in den nächsten 14 Tagen auch so weit sein wird wie deiner. Aber ich werde hier im Forum bestimmt noch darüber berichten wenns soweit ist.

Werde deinen Beitrag aber sicher im Auge behalten und wünsch dir schon mal viel Erfolg!

**mausi_mick** · 08.01.2014, 16:01

Hallo,

hab jetzt auch mal kurz an einem balancierenden Robot gaearbeitet,
aber nich wie waste mit optischen Sensoren, sondern mit einem 6DOF: MPU6050 (3xacc + 3x gyro).

Wollte aber mal was anderes machen:

- nur einen Motor, eine H-Brücke verwenden,
- ohne Getriebe auskommen.

Letzteres war das Problem, da bei Drehzahl kurz über 0 auch kaum Drehmoment da ist, bei höherer Drehzahl das Fahrzeug aber Geschwindigkeit aufnimmt ...

Die Regelung musste daher recht feinfühlig sein. Den Sensor hab ich ca 2,5 cm oberhalb der Motorachse angebracht.
Der Motor ist eine Billiigmodell, an dem die Achsen an beiden Seiten herausgeführt sind, die Räder sitzen direkt auf der Motorachse.

Hab das dann - für kleine Kippwinkel - auch hinbekommen, musste aber dazu die Motorspannung auf ca 11 V (3 Lion-Akkus) erhöhen
Den Code hab ich zum grössten Teil aus diversen Foren (Kalman-Filter, PD-Regler), letzteren hab ich aber etwas angepasst, da die Kippmoment / Winkelkurve recht nichtlinera ist .

Das Video ist auf Youtube:

http://www.youtube.com/watch?v=tKy0-RIf0yQ&google_comment_id=z13gctlzyvqhgzcdo22ov1di3 ny4upq1l&google_view_type#gpluscomments

/ Balance Robot mit 6DOF MPU6050 und FastPWM_Timer2 (Pin D3 (OC2B/PD3)
// 20.12.2013 angepasst an einen Motor mit Anfahr-PWM 40
// 28.12.2013 aus Balance_22 mit GLCD-ST7735 Ausgabe zum Test
// 29.12.2013 Ausgabe auf GLCD ST7735 deaktiviert
// 29.12.2013 stabilste Version mit einem Motor an 3 Lipos
//
// PID nit komst.-Wert bei 0°
/* Copyright (C) 2012 Kristian Lauszus, TKJ Electronics. All rights reserved.
This software may be distributed and modified under the terms of the GNU
General Public License version 2 (GPL2) as published by the Free Software
Foundation and appearing in the file GPL2.TXT included in the packaging of
this file. Please note that GPL2 Section 2[b] requires that all works based
on this software must also be made publicly available under the terms of
the GPL2 ("Copyleft").
Contact information
-------------------
Kristian Lauszus, TKJ Electronics
Web : http://www.tkjelectronics.com
e-mail : kristianl@tkjelectronics.com
*/
/*
//################################################## ##############
#define cs 10
#define dc 9
#define rst 8 // you can also connect this to the Arduino reset

#include <Adafruit_GFX.h> // Core graphics library
#include <Adafruit_ST7735.h> // Hardware-specific library
#include <SPI.h>

Adafruit_ST7735 tft = Adafruit_ST7735(cs, dc, rst);

//################################################## ##############
*/

#include <Wire.h>
#include "Kalman.h" // Source: https://github.com/TKJElectronics/KalmanFilter

#ifndef PI

# define PI 3.141592653589793f

#endif

#ifndef M_PI_2

# define M_PI_2 1.570796326794897f

#endif

#ifndef atanf

# define atanf atan

#endif

#ifndef atan2f

# define atan2f atan2

#endif

Kalman kalmanX; // Create the Kalman instances
Kalman kalmanY;
Kalman kalmanZ;
char letter;
char buf[5];
int dirPin = 4; //
int pwmPin = 3; // PWM-Pins: 3(OC2B) (servo),11(OC2A,MOSI); 5(OC0B),6 (OC0A)(delay); 9(OC1A),10 (SS)
//int val = 0; //

int ir,il,ipwm;
int pwma= 0,pwmn=0;
/* IMU Data */
int16_t accX, accY, accZ;
int16_t tempRaw;
int16_t gyroX, gyroY, gyroZ;
//int16_t ikal=0;
float /*double*/ accXangle, accYangle,accZangle; // Angle calculate using the accelerometer
float /*double*/ temp; // Temperature
float /*double*/ gyroXangle, gyroYangle,gyroZangle; // Angle calculate using the gyro
float /*double*/ compAngleX, compAngleY,compAngleZ; // Calculate the angle using a complementary filter
float /*double*/ kalAngleX, kalAngleY,kalAngleZ; // Calculate the angle using a Kalman filter
int ita,ikal,ikam,ikala=0,ikaln,iacc;
uint32_t timer;
uint8_t i2cData[14]; // Buffer for I2C data
uint8_t itap;
const uint8_t IMUAddress = 0x68; // AD0 is logic low on the PCB
const uint16_t I2C_TIMEOUT = 1000; // Used to check for errors in I2C communication

// PID
const float Kc = 0; //8,10,20,40,60,0
const float Kp = 5; //10,8,19;16,14,12,4,5,7,5,6,8,20,30,50,30,54,60,50, 51,48,53,50
const float Ki = 4; //8,5,14,17,8,15,12,10
const float Kd = 2;//5,3,4,10,6,3,,8,11,4,9,7,5
float pTerm, iTerm, dTerm, integrated_error, last_error, error;
const float K = 1.9*1.12;
#define GUARD_GAIN 10.0

void setup()
{
/*
//############################
tft.initR(INITR_BLACKTAB);
tft.fillScreen(VGA_SILVER);
//############################
*/
pinMode(dirPin, OUTPUT); // setzt den Pin als Output
pinMode(pwmPin, OUTPUT); // setzt den Pin als Output
//================================================== ===========
//==================== Fast-PWM Timer2 ========================
//================================================== ===========
TCCR2A = _BV(COM2A1) | _BV(COM2B1) | _BV(WGM21) | _BV(WGM20); // A frequency: 16/64/256 = 976Hz
// TCCR2B = _BV(CS22); // A frequency: 16MHz/64/256 = 976Hz
TCCR2B &= ~(1<<CS01); // Invert that to "11111101" // 4kHz
// TCCR2B &= ~(1<<CS00); // Invert that to "11111110" // 32kHz
// OCR2A = 180; // Output A/D11 Duty Cycle: (180+1) / 256 = 70.7%
OCR2B = 40; // Anfahr-PWM
// Serial.begin(9600);
Wire.begin();
i2cData[0] = 7; // Set the sample rate to 1000Hz - 8kHz/(7+1) = 1000Hz
i2cData[1] = 0x00; // Disable FSYNC and set 260 Hz Acc filtering, 256 Hz Gyro filtering, 8 KHz sampling
i2cData[2] = 0x00; // Set Gyro Full Scale Range to ±250deg/s
i2cData[3] = 0x00; // Set Accelerometer Full Scale Range to ±2g
while(i2cWrite(0x19,i2cData,4,false)); // Write to all four registers at once
while(i2cWrite(0x6B,0x01,true)); // PLL with X axis gyroscope reference and disable sleep mode

while(i2cRead(0x75,i2cData,1));
if(i2cData[0] != 0x6

{ // Read "WHO_AM_I" register
// Serial.print(F("Error reading sensor"));
while(1);
}

delay(100); // Wait for sensor to stabilize

/* Set kalman and gyro starting angle */
while(i2cRead(0x3B,i2cData,6));
accX = ((i2cData[0] <<

| i2cData[1]);
accY = ((i2cData[2] <<

| i2cData[3]);
accZ = ((i2cData[4] <<

| i2cData[5]);
// atan2 outputs the value of -π to π (radians) - see http://en.wikipedia.org/wiki/Atan2
// We then convert it to 0 to 2π and then from radians to degrees
accYangle = (atan2(accX,accZ)+PI)*RAD_TO_DEG;
accXangle = (atan2(accY,accZ)+PI)*RAD_TO_DEG;
accZangle = (atan2(accX,accY)+PI)*RAD_TO_DEG;
kalmanX.setAngle(accXangle); // Set starting angle
kalmanY.setAngle(accYangle);
kalmanZ.setAngle(accZangle);
gyroXangle = accXangle;
gyroYangle = accYangle;
gyroZangle = accZangle;
compAngleX = accXangle;
compAngleY = accYangle;
compAngleZ = accZangle;
timer = micros();

}

int Pid(){
int ispeed,it;
// error = error - 180; // 180 = level??
it = (int) error;
if (it < 0) it = -it;
// weil das Kippmoment sehr nichtlinear ist (Sinuskurve), hab ich de bei grössern Winkeln
// den Fehler vergröösert:
if (it <= 1) ;
else if (it <= 2) error = error * 1.1; // 1.1;// 1.1;
else if (it == 3) error = error * 1.2; //1.3; //1,2;
else if (it == 4) error = error * 1.4; //1.6; //1.4;
else if (it == 5) error = error * 1.7; //2.0; //1.7;
else if (it == 6) error = error * 2.0; //2.5; //2.0;
else if (it == 7) error = error * 2.4; //3.1; //2.4;
else if (it <= 10) error = error * 4.0; //5.5; //4.0
else if (it <= 20) error = error * 10.0;
// else error = error * 40.0;
pTerm = Kp * error; // + Kc;
integrated_error += error;
iTerm = Ki * constrain(integrated_error, -GUARD_GAIN, GUARD_GAIN);
dTerm = Kd * (error - last_error);
last_error = error;
ispeed = constrain(K*(pTerm + iTerm + dTerm), -215, 215);
return(ispeed);
}

uint8_t i2cWrite(uint8_t registerAddress, uint8_t data, bool sendStop)
{
return i2cWrite(registerAddress,&data,1,sendStop); // Returns 0 on success
}

uint8_t i2cWrite(uint8_t registerAddress, uint8_t *data, uint8_t length, bool sendStop)
{
Wire.beginTransmission(IMUAddress);
Wire.write(registerAddress);
Wire.write(data, length);
uint8_t rcode = Wire.endTransmission(sendStop); // Returns 0 on success
if (rcode) {
// Serial.print(F("i2cWrite failed: "));
// Serial.println(rcode);
}
return rcode; // See: http://arduino.cc/en/Reference/WireEndTransmission
}

uint8_t i2cRead(uint8_t registerAddress, uint8_t *data, uint8_t nbytes) {
uint32_t timeOutTimer;
Wire.beginTransmission(IMUAddress);
Wire.write(registerAddress);
uint8_t rcode = Wire.endTransmission(false); // Don't release the bus
if (rcode) {
// Serial.print(F("i2cRead failed: "));
// Serial.println(rcode);
return rcode; // See: http://arduino.cc/en/Reference/WireEndTransmission
}
Wire.requestFrom(IMUAddress, nbytes,(uint8_t)true); // Send a repeated start and then release the bus after reading
for (uint8_t i = 0; i < nbytes; i++) {
if (Wire.available())
data[i] = Wire.read();
else {
timeOutTimer = micros();
while (((micros() - timeOutTimer) < I2C_TIMEOUT) && !Wire.available());
if (Wire.available())
data[i] = Wire.read();
else {
// Serial.println(F("i2cRead timeout"));
return 5; // This error value is not already taken by endTransmission
}
}
}
return 0; // Success
}

void loop()
{

/* Update all the values */

while(i2cRead(0x3B,i2cData,14));

accX = ((i2cData[0] <<

| i2cData[1]);
accY = ((i2cData[2] <<

| i2cData[3]);
accZ = ((i2cData[4] <<

| i2cData[5]);
// tempRaw = ((i2cData[6] <<

| i2cData[7]);
gyroX = ((i2cData[8] <<

| i2cData[9]);
gyroY = ((i2cData[10] <<

| i2cData[11]);
gyroZ = ((i2cData[12] <<

| i2cData[13]);

// atan2 outputs the value of -π to π (radians) - see http://en.wikipedia.org/wiki/Atan2
// We then convert it to 0 to 2π and then from radians to degrees
accXangle = (atan2(accY,accZ)+PI)*RAD_TO_DEG;
// accYangle = (atan2(accX,accZ)+PI)*RAD_TO_DEG;
// accZangle = (atan2(accX,accY)+PI)*RAD_TO_DEG;

float /*double*/ gyroXrate = (float/*double*/)gyroX/131.0;
// double gyroYrate = -((double)gyroY/131.0);
// double gyroZrate = -((double)gyroZ/131.0); // ??
gyroXangle += gyroXrate*((float/*double*/)(micros()-timer)/1000000); // Calculate gyro angle without any filter
// gyroYangle += gyroYrate*((double)(micros()-timer)/1000000);
// gyroZangle += gyroZrate*((double)(micros()-timer)/1000000); // ???
//gyroXangle += kalmanX.getRate()*((double)(micros()-timer)/1000000); // Calculate gyro angle using the unbiased rate
//gyroYangle += kalmanY.getRate()*((double)(micros()-timer)/1000000);

compAngleX = (0.93*(compAngleX+(gyroXrate*(double)(micros()-timer)/1000000)))+(0.07*accXangle); // Calculate the angle using a Complimentary filter
// compAngleY = (0.93*(compAngleY+(gyroYrate*(double)(micros()-timer)/1000000)))+(0.07*accYangle);
// compAngleZ = (0.93*(compAngleZ+(gyroZrate*(double)(micros()-timer)/1000000)))+(0.07*accZangle);
kalAngleX = kalmanX.getAngle(accXangle, gyroXrate, (float/*double*/)(micros()-timer)/1000000); // Calculate the angle using a Kalman filter
// kalAngleY = kalmanY.getAngle(accYangle, gyroYrate, (double)(micros()-timer)/1000000);
// kalAngleZ = kalmanZ.getAngle(accZangle, gyroZrate, (double)(micros()-timer)/1000000);
timer = micros();

// temp = ((double)tempRaw + 12412.0) / 340.0;

/*
Serial.print(" xAcc: "); Serial.print(accXangle);
Serial.print(" xGyr: "); Serial.print(gyroXangle);
Serial.print(" xCom: "); Serial.print(compAngleX);
Serial.print(" xKal: "); Serial.print(kalAngleX);
// Serial.print(" yAcc: "); Serial.print(accYangle);
// Serial.print(" yGyr: "); Serial.print(gyroYangle);
// Serial.print(" yCom: "); Serial.print(compAngleY);
// Serial.print(" yKal: "); Serial.println(kalAngleY);

// Serial.print(" zAcc: "); Serial.print(accZangle);
// Serial.print(" zGyr: "); Serial.print(gyrZAngle);
// Serial.print(" zCom: "); Serial.print(compAngleZ);
// Serial.print(" zKal: "); Serial.print(kalAngleZ);

*/
error = kalAngleX;//accXangle;//kalAngleX; // laAngleX = 180° (Fahrzeug senkrecht)
error = error - 180;
ikal = (int)error; //accXangle;//kalAngleX; // kal ??
// Serial.print(" ikal: "); Serial.print(ikal);
// ikal = ikal - 180;
// Serial.print(" ikal-180: "); Serial.println(ikal);
pwma = pwmn;

if (ikal > 0)
{
ikaln = ikal;
digitalWrite(dirPin, LOW);
ipwm = Pid();//mot_pwm();
//ipwm = ipwm; // PWM: 0-255

}
else
{
if (ikal < 0)
{
// ikal = -ikal;
ikaln = ikal;
digitalWrite(dirPin, HIGH);
ipwm = Pid(); //mot_pwm();
ipwm = -ipwm; // PWM: 0-255

} // ikal < 0
else ipwm = 0;//205; //0; //0;
// if ((iacc - ikal) > 5 ) OCR2B = OCR2B + 3;

}
// if (ipwm > 215) ipwm = 215;
ipwm = ipwm + 40; // Anfahr-PWM
pwmn = ipwm;

if (ikal > 70) ipwm = 0;
if (ikal < -70) ipwm = 0;
OCR2B = (int)ipwm;

//delay(2);//1),//+ikal);//10,12,16/15/18/10/20delay(2); //10
ikala = ikaln;
//================================================== =====================
/*
Serial.print(" xAcc: "); Serial.print(accXangle);
// Serial.print(" xGyr: "); Serial.print(gyroXangle);
// Serial.print(" xCom: "); Serial.print(compAngleX);
Serial.print(" xKal: "); Serial.print(kalAngleX);
Serial.print(" error: "); Serial.print(error);
Serial.print(" iKal: "); Serial.print(ikal);
Serial.print(" OCR2B: "); Serial.println(OCR2B);
// delay(200);
*/
//
//################################################## ######################################
/*
tft.fillRect(52,5,53,114,VGA_SILVER);//GRAY);
tft.setTextColor(ST7735_BLACK,VGA_SILVER);
tft.setCursor(2,5); tft.print("x-accel:"); tft.setCursor(52, 5); tft.print(accXangle,2);
tft.setCursor(2,20); tft.print("x-gyro:"); tft.setCursor(52,20); tft.print(gyroXangle,2);
tft.setCursor(2,35); tft.print("x-comp:"); tft.setCursor(52,35); tft.print(compAngleX,2);
tft.setCursor(2,50); tft.print("x-kalm:"); tft.setCursor(52,50); tft.print(kalAngleX,2);
tft.setCursor(2,70); tft.print("error:"); tft.setCursor(52,70); tft.print(error,2);
tft.setCursor(2,90); tft.print("ikal:") ; tft.setCursor(52,90); tft.print(ikal);

tft.setCursor(2,110); tft.print("OCR2B:"); tft.setCursor(52,110);
if (OCR2B > 200) tft.setTextColor(ST7735_RED,ST7735_WHITE);
tft.print(0,10);tft.setCursor(52,110); tft.print(OCR2B,10);

// delay(1);
//################################################## ####################
*/

}

**djdune** · 08.01.2014, 16:48

Da hast du ja einen schönen Balancer gebaut!
Mittlerweile verwende ich auch den MPU6050. Interessant ist die DMP Berechnung die im Sensor inbegriffen ist. Damit kann man die Filterung direkt am Sensor machen und spart das dann am Controller ein.
Welchen Motor hast du da verbaut? Das erkennt man am Video so schlecht.