Genetischer Algorithmus für QuadroCopter

[Che Guevara]

Oh... ok  
Ist zwar jetzt sowieso nicht mehr relevant, da ich meinen eigenen Algorithmus geschrieben habe, aber trotzdem danke, Wissen ist nie verkehrt!

Hm also eigentlich soll der Algorithmus später dann die ganze Zeit auf dem Quadrocopter mitlaufen, damit die Parameter ständig angepasst werden können, falls nötig. Deswegen muss ich auf einen PC usw.. verzichten. Trotzdem danke für den Tipp.

Heute werde ich mal, sofern ich Zeit finde, meinen Algorithmus testen und sehen, was dabei rauskommt. Sobald ich Ergebnisse habe, poste ich sie hier.

Gruß
Chris

EDIT:
Hier ist jetzt mal mein aktueller Code:

$regfile = "m328pdef.dat"
$crystal = 16000000
$framesize = 100
$hwstack = 100
$swstack = 100
$baud = 38400


Declare Sub Serial0charmatch()

Config Serialin = Buffered , Size = 30 , Bytematch = 13


Declare Sub Simulated_annealing()



Dim T As Single
Dim T_a As Single

Dim Wi_kp As Single
Dim Wi_ki As Single
Dim Wi_kd As Single

Dim W0_kp As Single
Dim W0_ki As Single
Dim W0_kd As Single

Dim Rp As Single
Dim Ri As Single
Dim Rd As Single
Dim Rp_i As Integer
Dim Ri_i As Integer
Dim Rd_i As Integer

Dim Fp As Single
Dim Fi As Single
Dim Fd As Single

Dim F As Double
Dim F0 As Double

'------evtl. als long!!!------------------------------
Dim Extmp As Double
Dim Eytmp As Double
Dim Eztmp As Double
Dim Ax(50) As Double
Dim Ay(50) As Double
Dim Az(50) As Double
Dim Ex As Double
Dim Ey As Double
Dim Ez As Double

Dim Atmp As Word
Dim A As Word

Dim Log_step As Byte
Dim Log_cnt As Word
Dim Max_log_cnt As Word

Dim Tries As Word
Dim Max_tries As Word

Dim Test_anz As Long


Dim X As Integer



'################################################################################################################################################################
'################################################################################################################################################################
'################################################################################################################################################################
Declare Sub Init_system()

Declare Sub Filter_gyro_data()
Declare Sub Filter_rx_data()
Declare Sub Mixer()

Declare Sub Pid_regulator()

Declare Sub Wmp_init()
Declare Sub Send_zero()
Declare Sub Read_wmp_data()
Declare Sub Set_wmp_offset()

Declare Sub Failsave()

Declare Sub Set_pwm()



Config Portc.2 = Output
Config Portc.3 = Output
Portc.2 = 0
Portc.3 = 0


Config Portb.1 = Output
Config Portb.2 = Output
Config Portd.5 = Output
Config Portd.6 = Output


Config Timer1 = Pwm , Pwm = 8 , Compare A Pwm = Clear Down , Compare B Pwm = Clear Down , Prescale = 64
Pwm1a = 113
Pwm1b = 113

Config Timer2 = Pwm , Compare A Pwm = Clear Down , Compare B Pwm = Clear Down , Prescale = 64
Pwm2a = 113
Pwm2b = 113


Config Pind.2 = Input
Portd.2 = 0

Config Int0 = Falling
On Int0 Measure
Enable Int0

Config Timer0 = Timer , Prescale = 256
On Timer0 Pausedetect
Enable Timer0


$lib "I2C_TWI.LBX"
Config Scl = Portc.5
Config Sda = Portc.4
Config Twi = 100000
I2cinit


Const Minthrottle = 30
Const Maxchannel = 8
Const _throttlechannel = 1
Const _rollchannel = 2
Const _pitchchannel = 3
Const _yawchannel = 4
Const _statechannel = 5
Const _datachannel = 6


Dim I As Byte
Dim J As Byte
Dim Newvalsreceived As Bit
Dim _blink As Byte

Dim _yaw_kp As Single
Dim _roll_kp As Single
Dim _pitch_kp As Single

Dim _yaw_ki As Single
Dim _roll_ki As Single
Dim _pitch_ki As Single

Dim _yaw_kd As Single
Dim _roll_kd As Single
Dim _pitch_kd As Single

_yaw_kp = 0.4                                               '0.32
_roll_kp = 0.4                                              '0.32
_pitch_kp = 0.4                                             '0.32

_yaw_ki = 0.0008                                            '0.0008
_roll_ki = 0.0015                                           '0.0018
_pitch_ki = 0.0015                                          '0.0018

_yaw_kd = 0
_roll_kd = 0
_pitch_kd = 0

'_yaw_ki = 0.0001                                            '0.000135
'_roll_ki = 0.0001                                           '0.000135
'_pitch_ki = 0.0001                                          '0.000135

'_yaw_kd = 0.000007                                          '0.0000096
'_roll_kd = 0.000007                                         '0.0000096
'_pitch_kd = 0.000007                                        '0.0000096


'###################################
'#########RC-EMPFÄNGER##############
'###################################
Dim Bufferbyte As Byte
Dim Kanal(maxchannel) As Word
Dim Fkanal(maxchannel) As Word
Dim Channel As Byte
Dim _bl(4) As Word
Dim _sbl(maxchannel) As Integer
Dim State As Byte
Dim Oldstate As Byte
'###################################
'###################################
'###################################


'###################################
'###########I2C-Inputs##############
'###################################
Dim Wmplus_buffer(6) As Byte
Dim Ar(6) As Byte
'###################################
'###################################
'###################################


'###################################
'#########GYRO######################
'###################################
Dim Yaw As Word
Dim Yaw0 As Byte At Yaw + 1 Overlay
Dim Yaw1 As Byte At Yaw Overlay

Dim Roll As Word
Dim Roll0 As Byte At Roll + 1 Overlay
Dim Roll1 As Byte At Roll Overlay

Dim Pitch As Word
Dim Pitch0 As Byte At Pitch + 1 Overlay
Dim Pitch1 As Byte At Pitch Overlay

Dim _yawoffset As Long
Dim _rolloffset As Long
Dim _pitchoffset As Long

Dim _yawoffset_int As Integer
Dim _rolloffset_int As Integer
Dim _pitchoffset_int As Integer

Dim _yawnow As Integer
Dim _rollnow As Integer
Dim _pitchnow As Integer
'###################################
'###################################
'###################################

'##################################
'#########PID-REGLER###############
'##################################
Dim _yaw_kp_err As Single
Dim _roll_kp_err As Single
Dim _pitch_kp_err As Single

Dim _yaw_ki_err As Single
Dim _roll_ki_err As Single
Dim _pitch_ki_err As Single

Dim _yaw_ki_sum As Single
Dim _roll_ki_sum As Single
Dim _pitch_ki_sum As Single

Dim _yaw_kd_err As Single
Dim _roll_kd_err As Single
Dim _pitch_kd_err As Single

Dim _yaw_kd_old As Single
Dim _roll_kd_old As Single
Dim _pitch_kd_old As Single

Dim _yaw_pid_int As Integer
Dim _roll_pid_int As Integer
Dim _pitch_pid_int As Integer

Dim _yaw_pid As Single
Dim _roll_pid As Single
Dim _pitch_pid As Single

Dim _yaw_err_int As Integer
Dim _roll_err_int As Integer
Dim _pitch_err_int As Integer

Dim _yaw_err As Single
Dim _roll_err As Single
Dim _pitch_err As Single
'#################################
'#################################
'#################################

Dim Setpoint_yaw As Single
Dim Setpoint_roll As Single
Dim Setpoint_pitch As Single

Dim Yawstickvel As Integer
Dim Rollstickvel As Integer
Dim Pitchstickvel As Integer

Dim Yawstickold As Integer
Dim Rollstickold As Integer
Dim Pitchstickold As Integer

Dim Hempf(maxchannel) As Word
Dim Lempf(maxchannel) As Word

Dim _x1 As Single
Dim _x2 As Single

Dim Rc_on As Word

Dim Failure As Byte


'_bl(1) = rechts hinten  (US)
'_bl(2) = links hinten   (GUS)
'_bl(3) = rechts vorne   (GUS)
'_bl(4) = links vorne    (US)

'PWM = 113 --> 0.900us
'PWM = 250 --> 2.000ms


'################################################################################################################################################################
'################################################################################################################################################################
'################################################################################################################################################################




'-------initialize the values

'_roll_kp = 0.4
'_roll_ki = 0.0015

T = 1.0
T_a = 0.75
F0 = 500
F = F0
W0_kp = 0.3
W0_ki = 0.001
W0_kd = 0.000005
Fp = 0.2
Fi = 0.0008
Fd = 0.00001
Log_step = 3
Max_log_cnt = 90
Max_tries = 30



Call Init_system()
Waitms 500
Enable Interrupts
Waitms 500



Rc_test:
If Kanal(_throttlechannel) <= 65 And Kanal(_statechannel) < 80 Then
   If Rc_on < 65000 Then Incr Rc_on
Else
   If Rc_on > 0 Then Decr Rc_on
End If
If Rc_on < 500 Then
   Goto Rc_test
End If




Do


   Call Filter_rx_data()
   Call Filter_gyro_data()
   Call Pid_regulator()
   Call Mixer()
   Call Failsave()
   '#####
   _bl(1) = 113
   _bl(4) = 113
   '#####
   Call Set_pwm()
   Call Simulated_annealing()


Loop


Sub Simulated_annealing()

   If State > 0 Then

      Incr Test_anz

      '-------randomize according to the current temperature--------
      Rp_i = Rnd(200) - 100
      Rp = Rp_i * 0.01
      Rp = Rp * T
      Ri_i = Rnd(200) - 100
      Ri = Ri_i * 0.01
      Ri = Ri * T
      Rd_i = Rnd(200) - 100
      Rd = Rd_i * 0.01
      Rd = Rd * T

      '-------generate new parameters------------------------------
      Wi_kp = Rp * Fp
      Wi_kp = Wi_kp + W0_kp
      Wi_ki = Ri * Fi
      Wi_ki = Wi_ki + W0_ki
      Wi_kd = Rd * Fd
      Wi_kd = Wi_kd + W0_kd

      '-------cut off parameters-----------------------------------
      If Wi_kp < 0 Then Wi_kp = 0
      If Wi_ki < 0 Then Wi_ki = 0

      '-------test current parameters------------------------------
      _roll_kp = Wi_kp
      _roll_ki = Wi_ki
      _roll_kd = Wi_kd

      '-------log data and get fitness-----------------------------
      Incr Log_cnt
      If Log_cnt < Max_log_cnt Then
         Ax(log_cnt) = _roll_err
         '####
         If Log_cnt > Log_step Then
            Atmp = Log_cnt - Log_step
         Else
            Atmp = 1
         End If
         Extmp = Ax(log_cnt) - Ax(atmp)
         Extmp = Extmp ^ 2
         Eytmp = Ay(log_cnt) - Ay(atmp)
         Eytmp = Eytmp ^ 2
         Eztmp = Az(log_cnt) - Az(atmp)
         Eztmp = Eztmp ^ 2
         Ex = Ex + Extmp
         Ey = Ey + Eytmp
         Ez = Ez + Eztmp
         '####
      Elseif Log_cnt = Max_log_cnt Then
         F = Ex + Ey
         F = F + Ez
         Ex = 0
         Ey = 0
         Ez = 0
         Log_cnt = 0
         Incr Tries
      End If

      '-------better then current solution?------------------------
      If F < F0 Then
         F0 = F
         W0_kp = Wi_kp
         W0_ki = Wi_ki
         W0_kd = Wi_kd
      End If

      '-------counter for max tries---------------------------------
      If Tries = Max_tries Then
         Toggle Portc.2
         T = T * T_a
         Tries = 0
      End If

   End If

End Sub


Sub Serial0charmatch()
   Clear Serialin
   If State = 0 Then
      'Print F
      Print Test_anz ; " " ; T ; " " ; F0 ; " " ; W0_kp ; " " ; W0_ki ; " " ; W0_kd
   End If
End Sub


Sub Set_pwm()
   If _bl(1) > 250 Then _bl(1) = 250
   If _bl(1) < 113 Then _bl(1) = 113
   If _bl(2) > 250 Then _bl(2) = 250
   If _bl(2) < 113 Then _bl(2) = 113
   If _bl(3) > 250 Then _bl(3) = 250
   If _bl(3) < 113 Then _bl(3) = 113
   If _bl(4) > 250 Then _bl(4) = 250
   If _bl(4) < 113 Then _bl(4) = 113
   Pwm2a = _bl(4)                                           'links vorne (US)
   Pwm2b = _bl(2)                                           'links hinten (GUS)
   Pwm1a = _bl(3)                                           'rechts vorne (GUS)
   Pwm1b = _bl(1)                                           'rechts hinten (US)
End Sub


Sub Failsave()
  If Channel >= 11 Then
    If Failure < 255 Then
      Incr Failure
    End If
  End If
  If Channel < 11 Then
    If Failure > 0 Then
      Decr Failure
    End If
  End If
End Sub


Measure:
 If Channel > 0 And Channel < 9 Then
   Kanal(channel) = Timer0
 End If
 Timer0 = 6
 Incr Channel
Return


Pausedetect:
  If Channel <> 0 Then
    Newvalsreceived = 1
  End If
  Channel = 0
Return


Sub Filter_gyro_data()
 Call Read_wmp_data()

 _yawnow = Yaw - _yawoffset_int
 _rollnow = Roll - _rolloffset_int
 _pitchnow = Pitch - _pitchoffset_int
End Sub


Sub Filter_rx_data()

 If Newvalsreceived = 1 Then
   Newvalsreceived = 0

   For I = 1 To Maxchannel
     Fkanal(i) = Fkanal(i) * 3
     Fkanal(i) = Fkanal(i) + Kanal(i)
     Shift Fkanal(i) , Right , 2
     Hempf(i) = Fkanal(i) + 17
     Lempf(i) = Fkanal(i) - 17
     If Kanal(i) < Lempf(i) Or Kanal(i) > Hempf(i) Then
        Kanal(i) = Fkanal(i)
     End If
   Next I

   If Kanal(_throttlechannel) >= 60 And Kanal(_throttlechannel) <= 140 Then
     _sbl(_throttlechannel) = Kanal(_throttlechannel) - 100
     _sbl(_throttlechannel) = _sbl(_throttlechannel) * 2
     If _sbl(_throttlechannel) => 60 Then _sbl(_throttlechannel) = 60
     If _sbl(_throttlechannel) < -68 Then _sbl(_throttlechannel) = -68
   End If

   For I = 2 To Maxchannel
     If Kanal(i) >= 62 And Kanal(i) < 139 Then
       _sbl(i) = Kanal(i) - 100
     End If
   Next I

   If Kanal(5) < 80 Then State = 0
   If Kanal(5) >= 80 And Kanal(5) < 115 Then State = 1
   If Kanal(5) >= 115 Then State = 2


   Setpoint_yaw = _sbl(_yawchannel) * 1.5

   If State < 2 Then
      Setpoint_roll = _sbl(_rollchannel) * 0.6
      Setpoint_pitch = _sbl(_pitchchannel) * 0.6
   Else
      Setpoint_roll = _sbl(_rollchannel) * 1.5
      Setpoint_pitch = _sbl(_pitchchannel) * 1.5
   End If


   If Kanal(3) > 120 And Kanal(4) > 120 And Kanal(1) < 70 And State = 0 Then
     Call Set_wmp_offset()
   End If

   Oldstate = State

 End If

End Sub


Sub Wmp_init()
   I2cstart
   I2cwbyte &HA6
   I2cwbyte &HFE
   I2cwbyte &H04
   I2cstop
End Sub


Sub Send_zero()
   I2cstart
   I2cwbyte &HA4
   I2cwbyte &H00
   I2cstop
End Sub


Sub Read_wmp_data()
   Gosub Send_zero
   I2creceive &HA4 , Wmplus_buffer(1) , 0 , 6
   Yaw1 = Wmplus_buffer(1)
   Roll1 = Wmplus_buffer(2)
   Pitch1 = Wmplus_buffer(3)
   Shift Wmplus_buffer(4) , Right , 2 : Yaw0 = Wmplus_buffer(4)
   Shift Wmplus_buffer(5) , Right , 2 : Roll0 = Wmplus_buffer(5)
   Shift Wmplus_buffer(6) , Right , 2 : Pitch0 = Wmplus_buffer(6)
   Shift Yaw , Right , 4
   Shift Roll , Right , 4
   Shift Pitch , Right , 4
End Sub


Sub Set_wmp_offset()
   _yawoffset = 0
   _rolloffset = 0
   _pitchoffset = 0
   For I = 1 To 100
      Call Read_wmp_data()
      _yawoffset = _yawoffset + Yaw
      _rolloffset = _rolloffset + Roll
      _pitchoffset = _pitchoffset + Pitch
   Next I
   _yawoffset = _yawoffset / 100
   _rolloffset = _rolloffset / 100
   _pitchoffset = _pitchoffset / 100
   _yawoffset_int = _yawoffset
   _rolloffset_int = _rolloffset
   _pitchoffset_int = _pitchoffset
End Sub


Sub Pid_regulator()

'-------------YAW---------------
 _yaw_err = Setpoint_yaw - _yawnow
 _yaw_kp_err = _yaw_err * _yaw_kp
 _yaw_ki_sum = _yaw_ki_sum + _yaw_err
 If _yaw_ki_sum < -10000 Then _yaw_ki_sum = -10000
 If _yaw_ki_sum > 10000 Then _yaw_ki_sum = 10000
 _yaw_ki_err = _yaw_ki_sum * _yaw_ki
 _yaw_kd_err = _yaw_err - _yaw_kd_old
 _yaw_kd_old = _yaw_err
 _yaw_kd_err = _yaw_kd_err * _yaw_kd

'-------------ROLL--------------
 _roll_err = Setpoint_roll - _rollnow
 _roll_kp_err = _roll_err * _roll_kp
 _roll_ki_sum = _roll_ki_sum + _roll_err
 If _roll_ki_sum < -10000 Then _roll_ki_sum = -10000
 If _roll_ki_sum > 10000 Then _roll_ki_sum = 10000
 _roll_ki_err = _roll_ki_sum * _roll_ki
 _roll_kd_err = _roll_err - _roll_kd_old
 _roll_kd_old = _roll_err
 _roll_kd_err = _roll_kd_err * _roll_kd

'-------------PITCH-------------
 _pitch_err = Setpoint_pitch - _pitchnow
 _pitch_kp_err = _pitch_err * _pitch_kp
 _pitch_ki_sum = _pitch_ki_sum + _pitch_err
 If _pitch_ki_sum < -10000 Then _pitch_ki_sum = -10000
 If _pitch_ki_sum > 10000 Then _pitch_ki_sum = 10000
 _pitch_ki_err = _pitch_ki_sum * _pitch_ki
 _pitch_kd_err = _pitch_err - _pitch_kd_old
 _pitch_kd_old = _pitch_err
 _pitch_kd_err = _pitch_kd_err * _pitch_kd


 _yaw_pid = _yaw_kp_err + _yaw_ki_err
 _yaw_pid = _yaw_pid + _yaw_kd_err
 _yaw_pid_int = _yaw_pid

 _roll_pid = _roll_kp_err + _roll_ki_err
 _roll_pid = _roll_pid + _roll_kd_err
 _roll_pid_int = _roll_pid

 _pitch_pid = _pitch_kp_err + _pitch_ki_err
 _pitch_pid = _pitch_pid + _pitch_kd_err
 _pitch_pid_int = _pitch_pid

 If _yaw_pid_int < -68 Then _yaw_pid_int = -68
 If _yaw_pid_int > 68 Then _yaw_pid_int = 68
 If _roll_pid_int < -68 Then _roll_pid_int = -68
 If _roll_pid_int > 68 Then _roll_pid_int = 68
 If _pitch_pid_int < -68 Then _pitch_pid_int = -68
 If _pitch_pid_int > 68 Then _pitch_pid_int = 68

End Sub


Sub Mixer()

  '_bl(1) = rechts hinten  (US)
  '_bl(2) = links hinten   (GUS)
  '_bl(3) = rechts vorne   (GUS)
  '_bl(4) = links vorne    (US)

 _bl(1) = 182 + _sbl(_throttlechannel)
 _bl(2) = _bl(1)
 _bl(3) = _bl(1)
 _bl(4) = _bl(1)

 If State <> 0 And Failure < 20 Then

   _bl(1) = _bl(1) + Minthrottle
   _bl(2) = _bl(2) + Minthrottle
   _bl(3) = _bl(3) + Minthrottle
   _bl(4) = _bl(4) + Minthrottle

   _bl(1) = _bl(1) - _pitch_pid_int
   _bl(4) = _bl(4) + _pitch_pid_int

   _bl(2) = _bl(2) - _roll_pid_int
   _bl(3) = _bl(3) + _roll_pid_int

   _bl(1) = _bl(1) - _yaw_pid_int
   _bl(2) = _bl(2) + _yaw_pid_int
   _bl(3) = _bl(3) + _yaw_pid_int
   _bl(4) = _bl(4) - _yaw_pid_int

 Else

   _bl(1) = 113
   _bl(2) = 113
   _bl(3) = 113
   _bl(4) = 113

   _yaw_kp_err = 0
   _yaw_ki_sum = 0
   _yaw_kd_err = 0
   _yaw_ki_err = 0
   _roll_kp_err = 0
   _roll_ki_sum = 0
   _roll_kd_err = 0
   _roll_ki_err = 0
   _pitch_kp_err = 0
   _pitch_ki_sum = 0
   _pitch_kd_err = 0
   _pitch_ki_err = 0
   _yaw_pid_int = 0
   _roll_pid_int = 0
   _pitch_pid_int = 0
   _yaw_pid = 0
   _roll_pid = 0
   _pitch_pid = 0
   Yawstickvel = 0
   Rollstickvel = 0
   Pitchstickvel = 0
   Yawstickold = 0
   Rollstickold = 0
   Pitchstickold = 0

 End If

End Sub


Sub Init_system()

_bl(1) = 113
_bl(2) = 113
_bl(3) = 113
_bl(4) = 113

_sbl(1) = -68
_sbl(2) = 0
_sbl(3) = 0
_sbl(4) = 0
_sbl(5) = -600
_sbl(6) = -600
_sbl(7) = 0
_sbl(8) = 0

Fkanal(1) = 63
Fkanal(2) = 100
Fkanal(3) = 100
Fkanal(4) = 100
Fkanal(5) = 63
Fkanal(6) = 63
Fkanal(7) = 100
Fkanal(8) = 100

Reset Newvalsreceived

State = 0
Oldstate = 0

_yawnow = 0
_rollnow = 0
_pitchnow = 0

Call Wmp_init()
Waitms 500
For I = 1 To 10
Call Read_wmp_data()
Next I
Waitms 500
Call Set_wmp_offset()

Rc_on = 0

End Sub

End

Im Unterschied zu meiner vorherigen Version ist hier die Fitness-Funtkion anders aufgebaut. Es wird nicht, wie vorher, die Fitness der geloggten Daten auf einmal bestimmt, sondern es wird nach jedem Datenloggen wieder ein Teil der Fitness bestimmt. Das hat den Vorteil, dass die Rechenzeit für die Fitness-Funktion gleichmäßiger verteilt ist und nicht mehr auf einen kurzen Zeitpunkt fixiert. Somit kann die Regelung besser funktionieren.
Das einzige, was jetzt noch fehlt, ist die "kontrollierte unkontrollierte" Abweichung im System, z.b. eine Windböe in der Software nachgebildet ^^
Das möchte ich über die Variable Log_cnt realisieren: Wenn diese in dem Bereich von x1 bis x2 ist, wird eine RC-Eingabe simuliert (z.b. Vollausschlag links)... Hier kann ich dann versch. beliebige Muster einprogrammieren, evtl. sogar über eine GUI (wird sicherlich noch folgen).
Habe mir jetzt auch zwei Bücher bestellt, welche das Thema naturanaloge Algorithmen behandeln (also auch Simulated Annealing, genet. Algos, usw...). Vielleicht finde ich ja darin dann noch ein paar Tipps zur besseren Umsetzung... Es gäbe da noch einiges, z.b. die Möglichkeit, einen Kandidaten mit schlechterer Fitness trotzdem zu übernehmen, um lokale Minima zu vermeiden, usw... Das kommt aber erst, wenn der Grundalgo mal läuft. Aber so wies aussieht, läuft er schon  
Wenn Interresse besteht, kann ich ja mal ein Video von meinem 5 Minuten Teststand reinstellen...