Implementierung eines Zustandreglers mit Beobachter

Buell24 Gast #2454079
Lesenswert?
• ▲
▼ 
Ich habe in Matlab-Simulink einen Zustandregler mit Beobachter für ein 
inverses Pendel ausgelegt und diskretisiert.
http://www.library.cmu.edu/ctms/ctms/state/obs.gif
Ich bin dabei analog zu:
http://www.library.cmu.edu/ctms/ctms/examples/pend/diginvss.htm
vorgegangen.

Nun möchte ich diesen Zustandsregler auf einem Mikrocontroller (Arduino 
Mega) implementieren.
Folgende Gleichungen habe ich aufgestellt:

x[n + 1] = Ax[n] + Bu[n]
wobei u[n] = - Kx[n]  (negativ rückgekoppelt)
x[n + 1] = Ax[n] - BKx[n] = (A-BK)x[n]
Mit dem Beobachter sieht das ganze so aus:
x^[n + 1] = (A-BK)x^[n] - L(y^-y)
x^ = predicted state, y^ = predicted output, y= measured outputs

Implementiert habe ich das ganze volgendermassen:

   .
   .
   .
   .

  //matrix A-B*K
const double matr_A_BK[4][4] =       {
  1.0013,    0.0112,   -0.0061,   -0.0020,
  0.2633,    1.2461,   -1.2260,   -0.4024,
  0.0012,    0.0012,    0.9947,    0.0081,
  0.2489,    0.2326,   -1.0661,    0.6201 };

const double matr_L[2][4] =       {
  2.6319,     -0.0108,
  173.1476,     -1.3975,
  -0.0086,      2.6289,
  -1.1521,    172.8517   };

const double matr_K[4] = {
-65.9759,  -61.7565,  314.0219,  100.8610 };

double state[4] = {
  0, 0, 0, 0};   //state vector

double nstate[4] = {
  0, 0, 0, 0};  //temporarly stores new state

   .
   .
   .
   .

    p[0] = state[0];  //two predicted states (x^ and theta^)
    p[1] = state[2];
    read_current_distance_m1();
    s[1] = (distanceM1-initDistanceM1)/6573.75; //two sensors (x and 
theta)
    s[2] = (analogRead(10)-513.0);

    for(int i = 0; i < 4; i++) {
      for(int j = 0; j < 4; j++) {   // (A-BK)*state
        nstate[i] = nstate[i] + matr_A_BK[i][j]*state[j];
      }
      for(int j = 0; j < 2; j++) { //-L*pred + L*measures
        nstate[i] = nstate[i] + matr_L[i][j]*( s[j] - p[j] );
      }
    }

    //update states
    for(int i = 0; i<4; i++) {
      state[i] = nstate[i];
      nstate[i] = 0; //init for following cycle
    }

   .
   .
   .
   .


Leider muss aber ein Fehler im Code vorhanden sein, die Variable 
nstate[i] ist nämlich immer 0.00. Könnt Ihr mir sagen, ob dieser Ansatz 
der Implementierung überhaupt richtig ist?
Antwort Zitieren Bearbeiten Thread verschieben Thread sperren Anmeldepflicht aktivieren Anpinnen Thread löschen Thread mit anderem zusammenführen Beitrag melden
Buell24 Gast #2461658
Lesenswert?
• ▲
▼ 
Der Code wurde für einen Arduino Mega geschrieben und besteht aus zwei 
Dateien:

StateSpaceControl.ino:
Beinhaltet den Regel Algorithmus

RoboClaw.ino:
Beinhaltet die Kommunikation zum Motor Controller.

StateSpaceControl.ino:
1
#define SERIAL_BAUD 57600

2
#define INPUT_COUNT 5     //number of analog inputs

3
#define VDD 5000.0f       //Analog reference voltage in mV

4
#define PI 3.14159265358979f

5


6
//DIGITAL OUTPUT

7
int ledGREENpin = 2;

8
int ledREDpin = 3;

9
int oscilloscopePin = 8;  //oscilloscope pin to work out the program loop time

10
boolean oscilloscopeState = false;

11


12
//Analog Input

13
float poti8 = 0;

14
float poti9 = 0;

15


16


17
//Global variables Motor Controller-----------------------------------------------------------------------------------------------

18
float level = 0;

19
int speedMotor1;

20
long distanceM1=0;

21
long initDistanceM1=0;

22
  

23
  //matrix A-B*K

24
const double matr_A_BK[4][4] =       { 

25
  1.0013,    0.0112,   -0.0061,   -0.0020,

26
  0.2633,    1.2461,   -1.2260,   -0.4024,  

27
  0.0012,    0.0012,    0.9947,    0.0081,  

28
  0.2489,    0.2326,   -1.0661,    0.6201 };

29


30
const double matr_L[2][4] =       {    

31
  2.6319,     -0.0108,

32
  173.1476,     -1.3975,

33
  -0.0086,      2.6289,

34
  -1.1521,    172.8517   };

35


36
const double matr_K[4] = {      

37
-65.9759,  -61.7565,  314.0219,  100.8610 };

38


39
double state[4] = {

40
  0, 0, 0, 0};   //state vector

41


42
double nstate[4] = {

43
  0, 0, 0, 0};  //temporarly stores new state

44


45


46
void setup() {

47
  Serial.begin(SERIAL_BAUD);

48
  setupRoboClaw();

49


50
  //digital outputs

51
  pinMode(ledREDpin, OUTPUT);

52
  pinMode(ledGREENpin, OUTPUT);

53
  pinMode(oscilloscopePin, OUTPUT);

54


55
  digitalWrite(ledREDpin, HIGH);

56
  digitalWrite(ledGREENpin, LOW);

57
  Serial.println("start");

58
  read_current_distance_m1();

59
  initDistanceM1=distanceM1;

60
}

61


62
void loop() {

63
  

64
  double p[2] = {

65
    0,0    }; //two predicted state

66
  double s[2] = {

67
    0,0    }; //two outputs

68
  double force = 0;     //force that must be applied to motor

69
  long now = millis();

70
  long last = millis();

71


72


73
  while(1) {     //-------------CYCLE

74
    digitalWrite(ledREDpin, LOW);

75
    digitalWrite(ledGREENpin, HIGH);

76
    now = millis();

77


78
    while(last == now) {   //wait until 1 microsecond elapses

79
      // sample time = 1 ms

80
      now = millis();

81
    }

82
    last = now;

83


84
    // -L*pred + L*measures

85
    p[0] = state[0];  //two predicted states (x^ and theta^)

86
    p[1] = state[2];

87
    read_current_distance_m1();

88
    s[0] = (distanceM1-initDistanceM1)/6573.75;     //two sensors (x and theta)

89
    s[1] = (analogRead(10)-511.0)*0.00405890523719604651162790697674;

90


91


92
    for(int i = 0; i < 4; i++) {

93
      for(int j = 0; j < 4; j++) {   // _________(A-BK)*state

94
        nstate[i] = nstate[i] + matr_A_BK[i][j]*state[j];

95
      }

96
      for(int j = 0; j < 2; j++) { //-L*pred + L*measures

97
        nstate[i] = nstate[i] + matr_L[i][j]*( s[j] - p[j] );

98
      }

99
      Serial.print(" nstate[");

100
      Serial.print(i);

101
      Serial.print("]= ");

102
      printDouble(nstate[i],10);

103
    }

104
    Serial.println(" end for");

105


106


107
    //update states

108
    for(int i = 0; i<4; i++) {

109
      state[i] = nstate[i];

110
      nstate[i] = 0; //init for following cycle

111
    }

112


113


114
    // controller action

115
    force = 0;

116
    for(int i = 0; i < 4; i++) { //controller action

117
      force =  force + (state[i]*matr_K[i]);

118
    }

119
    level = force * 6666.6;

120
    set_motor();

121


122
  }//end of while(1)

123
} 

124


125
void printDouble( double val, byte precision){

126
  // prints val with number of decimal places determine by precision

127
  // precision is a number from 0 to 6 indicating the desired decimial places

128
  // example: printDouble( 3.1415, 2); // prints 3.14 (two decimal places)

129


130
  Serial.print (int(val));  //prints the int part

131
  if( precision > 0) {

132
    Serial.print("."); // print the decimal point

133
    unsigned long frac;

134
    unsigned long mult = 1;

135
    byte padding = precision -1;

136
    while(precision--)

137
   mult *=10;

138


139
    if(val >= 0)

140
  frac = (val - int(val)) * mult;

141
    else

142
  frac = (int(val)- val ) * mult;

143
    unsigned long frac1 = frac;

144
    while( frac1 /= 10 )

145
  padding--;

146
    while(  padding--)

147
  Serial.print("0");

148
    Serial.print(frac,DEC) ;

149
  }

150
}


RoboClaw.ino:
1
#define address 128

2
#define ROBOCLAW_BAUD 38400

3


4
signed char Motor1percent = 0;

5
signed char Motor2percent = 0;

6
int incomingByte= 0;

7


8
void setupRoboClaw()

9
{

10
  Serial1.begin(ROBOCLAW_BAUD);

11
  delay(100);

12
  set_min_main_voltage_value(15);   //9V   formula: (voltage-6)*5

13
  set_max_main_voltage_value(82);   //16V  formula: voltage*5.25

14
  set_min_logic_voltage_value(15);  //9V   formula: (voltage-6)*5

15
  set_max_logic_voltage_value(82);  //16V  formula: voltage*5.25

16
  delay(100);

17
  set_speed_m1_m2(0,0);

18


19
}

20


21
/*

22
** 2 - Set Minimum Main Voltage

23
 ** Sets main battery (B- / B+) minimum voltage level. If the battery voltages drops below the set

24
 ** voltage level Robo Claw will shut down. The value is cleared at start up and must set after each

25
 ** power up. The voltage is set in .2 volt increments. A value of 0 sets the minimum value allowed which

26
 ** is 6V. The valid data range is 0 - 120 (6V - 30V). The formula for calculating the voltage is: (Desired

27
 ** Volts - 6) x 5 = Value. Examples of valid values are 6V = 0, 8V = 10 and 11V = 25. Example with

28
 ** Robo Claw address set to 128:

29
 **

30
 ** Serout P15, i19200, [128, 2, 25, (165 & 0X7F)]

31
 */

32


33
void set_min_main_voltage_value(byte min_main_battery_voltage)

34
{

35
  byte serial_data[4];

36
  serial_data[0]=address;

37
  serial_data[1]=2;

38
  serial_data[2]=min_main_battery_voltage;

39
  serial_data[3]=(serial_data[0] + serial_data[1] + serial_data[2]) & 0x7F;

40


41
  Serial1.write(serial_data, sizeof(serial_data));

42
}

43


44
/*

45
** 3 - Set Maximum Main Voltage

46
 ** Sets main battery (B- / B+) maximum voltage level. The valid data range is 0 - 154 (0V - 30V). If

47
 ** you are using a battery of any type you can ignore this setting. During regenerative breaking a back

48
 ** voltage is applied to charge the battery. When using an ATX type power supply if it senses anything

49
 ** over 16V it will shut down. By setting the maximum voltage level, Robo Claw before exceeding it will

50
 ** go into hard breaking mode until the voltage drops below the maximum value set. The formula for

51
 ** calculating the voltage is: Desired Volts x 5.12 = Value. Examples of valid values are 12V = 62, 16V

52
 ** = 82 and 24V = 123. Example with Robo Claw address set to 128:

53
 **

54
 ** Serout P15, i19200, [128, 3, 82, (213 & 0X7F)]

55
 */

56


57
void set_max_main_voltage_value(byte max_main_battery_voltage)

58
{

59


60
  byte serial_data[4];

61
  serial_data[0]=address;

62
  serial_data[1]=3;

63
  serial_data[2]=max_main_battery_voltage;

64
  serial_data[3]=(serial_data[0] + serial_data[1] + serial_data[2]) & 0x7F;

65


66
  Serial1.write(serial_data, sizeof(serial_data));

67
}

68


69


70
void set_min_logic_voltage_value(byte min_logic_battery_voltage)

71
{

72
  byte serial_data[4];

73
  serial_data[0]=address;

74
  serial_data[1]=26;

75
  serial_data[2]=min_logic_battery_voltage;

76
  serial_data[3]=(serial_data[0] + serial_data[1] + serial_data[2]) & 0x7F;

77


78
  Serial1.write(serial_data, sizeof(serial_data));

79
}

80


81
void set_max_logic_voltage_value(byte max_logic_battery_voltage)

82
{

83
  byte serial_data[4];

84
  serial_data[0]=address;

85
  serial_data[1]=27;

86
  serial_data[2]=max_logic_battery_voltage;

87
  serial_data[3]=(serial_data[0] + serial_data[1] + serial_data[2]) & 0x7F;

88


89
  Serial1.write(serial_data, sizeof(serial_data));

90
}

91


92
void set_speed_m1_m2(long speedM1, long speedM2) {

93
  byte ab[11];

94
  ab[0] = address;

95
  ab[1] = 37;

96
  ab[2] = (speedM1 >> 24) & 0xff;

97
  ab[3] = (speedM1 >> 16) & 0xff;

98
  ab[4] = (speedM1 >> 8) & 0xff;

99
  ab[5] = (speedM1 >> 0) & 0xff;

100
  ab[6] = (speedM2 >> 24) & 0xff;

101
  ab[7] = (speedM2 >> 16) & 0xff;

102
  ab[8] = (speedM2 >> 8) & 0xff;

103
  ab[9] = (speedM2 >> 0) & 0xff;

104
  ab[10] = (ab[0]+ab[1]+ab[2]+ab[3]+ab[4]+ab[5]+ab[6]+ab[7]+ab[8]+ab[9]) & 0x7f;

105
  Serial1.write(ab, sizeof(ab));

106
}

107


108
void read_current_distance_m1()

109
{

110
  byte ab[2];

111
  ab[0] = address;

112
  ab[1] = 16;

113
  Serial1.write(ab, sizeof(ab));

114
  delay(10);

115
  

116
  byte distanceM1_byte[4];

117
    // send data only when you receive data:

118
  int i=0;

119
  while (Serial1.available() > 0) {

120
    if (i<4){

121
      distanceM1_byte[i] = Serial1.read();

122
    }

123
    i++;

124
  }

125
    distanceM1 = ((long )distanceM1_byte[0]) << 24;

126
    distanceM1 |= ((long )distanceM1_byte[1]) << 16;

127
    distanceM1 |= ((long )distanceM1_byte[2]) << 8;

128
    distanceM1 |= distanceM1_byte[3];

129
}


Der debug output sieht wie folgt aus:

1
start

2
 nstate[0]= 0.0003028782 nstate[1]= 0.0737256000 nstate[2]= -12.0398380160 nstate[3]= 0.0000000000 end for

3
 nstate[0]= 0.0072283688 nstate[1]= 48.0991425344 nstate[2]= -787.0603735872 nstate[3]= 13.0303843360 end for

4
 nstate[0]= -3.0112114600 nstate[1]= 3091.0715704576 nstate[2]= 16104.0000000000 nstate[3]= 858.1392766592 end for

5
 nstate[0]= -194.0648187712 nstate[1]= -2546.0000000000 nstate[2]= -21931.0000000000 nstate[3]= -11586.0000000000 end for

6
 nstate[0]= -12197.0159733968 nstate[1]= -11260.0000000000 nstate[2]= -32240.0000000000 nstate[3]= -22153.0000000000 end for

7
 nstate[0]= 20980.0000000000 nstate[1]= -21632.0000000000 nstate[2]= 0.0000000000 nstate[3]= 3568.0000000000 end for

8
 nstate[0]= 1824.0000000000 nstate[1]= 0.0000000000 nstate[2]= 0.0000000000 nstate[3]= 0.0000000000 end for

9
 nstate[0]= 0.0000000000 nstate[1]= 0.0000000000 nstate[2]= 0.0000000000 nstate[3]= 0.0000000000 end for

10
 nstate[0]= 0.0000000000 nstate[1]= 0.0000000000 nstate[2]= 0.0000000000 nstate[3]= 0.0000000000 end for

11
 nstate[0]= 0.0000000000 nstate[1]= 0.0000000000 nstate[2]= 0.0000000000 nstate[3]= 0.0000000000 end for

12
 nstate[0]= 0.0000000000 nstate[1]= 0.0000000000 nstate[2]= 0.0000000000 nstate[3]= 0.0000000000 end for

13
 nstate[0]= 0.0000000000 nstate[1]= 0.0000000000 nstate[2]= 0.0000000000 nstate[3]= 0.0000000000 end for

14
 nstate[0]= 0.0000000000 nstate[1]= 0.0000000000 nstate[2]= 0.0000000000 nstate[3]= 0.0000000000 end for

15
 nstate[0]= 0.0000000000 nstate[1]= 0.0000000000 nstate[2]= 0.0000000000 nstate[3]= 0.0000000000 end for

16
 nstate[0]= 0.0000000000 nstate[1]= 0.0000000000 nstate[2]= 0.0000000000 nstate[3]= 0.0000000000 end for

17
 nstate[0]= 0.0000000000 nstate[1]= 0.0000000000 nstate[2]= 0.0000000000 nstate[3]= 0.0000000000 end for

18
 nstate[0]= 0.0000000000 nstate[1]= 0.0000000000 nstate[2]= 0.0000000000 nstate[3]= 0.0000000000 end for

19
 nstate[0]= 0.0000000000 nstate[1]= 0.0000000000 nstate[2]= 0.0000000000 nstate[3]= 0.0000000000 end for
Antwort Zitieren Bearbeiten Löschen Beitrag melden
Thread beobachten

Antwort schreiben

Bitte melde dich an, um einen Beitrag zu schreiben.

Bestehender Account

oder

Mit Google-Account einloggen

Noch kein Account?

Die Registrierung ist kostenlos und dauert nur eine Minute.

Jetzt registrieren