Hallo zusammen und wieder einmal habe ich eine Frage,
ich habe einen Kalmanfilter zur Sensordatenfilterung zusammengestellt.
Die Daten kommen von folgenden Sensoren:
-> Gyroskop: L3G4200D
-> Accelerometer: BMA180
-> Magnetometer: HMC5883
Die Kurven auf dem Bild(siehe Anhang) entsprechen folgendermaßen den
Drehmomenten(Mx,My,Mz) - bzw. den Winkeln:
Rot -> Mx (Angabe des Winkels(in Grad) zwischen +Z-Achse und Y-Achse)
Blau -> My (Angabe des Winkels(in Grad) zwischen +Z-Achse und X-Achse)
Grün -> Mz (Angabe des Winkels(in Grad) zwischen NORDEN und X-Achse)
(Rot und Blau sind anfangs auf 0Grad weil ich in horizontaler Stellung
auf Null sein möchte. Ich habe dafür einfach 90Grad auf die Achsen
addiert.)
Kann man anhand dieses Bildes erkennen, ob die gefilterten Daten zur
Regelung eines Multicopters ausreichen?
Leider gibt es immer wieder ein Rucken an den Motoren und allgemein
wirkt die Ansprache der Motoren nicht sauber und ich weiß nicht ob meine
PD-Regelung das Problem ist oder die gefilterten Daten.
Mein versuche einen PD-Regler zu programmieren sieht folgendermaßen aus:
1 | float errorX = -anglesOutput.y/90; //anglesOutput.y = Winkel(inGrad) zwischen DownVektor und y-Achse ->RollWert
|
2 | float errorY = anglesOutput.x/90; //anglesOutput.x = Winkel(inGrad) zwischen DownVektor und x-Achse ->PitchWert
|
3 | float errorZ = (anglesOutput.z-globalstartingZ)/90; //anglesOutput.z = Winkel(inGrad) zwischen NorthVektor und rotem Ausleger(x-Achse) ->YawWert
|
4 |
|
5 | float DerrorX = (errorX - lasterrorX)/0.003;
|
6 | float DerrorY = (errorY - lasterrorY)/0.003;
|
7 | float DerrorZ = (errorZ - lasterrorZ)/0.003;
|
8 |
|
9 | float pParamXY = remoteData.ctrl6 * 10; //ctrl6 Poti
|
10 | float pParamZ = remoteData.ctrl8; //ctrl8 Poti
|
11 | float dParamXY = remoteData.ctrl7 * -2.5; //ctrl7 Poti
|
12 | float dParamZ = -0.1;
|
13 |
|
14 | if (remoteData.isValid && remoteData.g > 0.2){
|
15 | float x = remoteData.r; //zwischen -1 und 1
|
16 | float y = remoteData.p; //zwischen -1 und 1
|
17 | float z = remoteData.y; //zwischen -1 und 1
|
18 | errorX += x*0.2;
|
19 | errorY += y*0.2;
|
20 | errorZ += z*0.4;
|
21 |
|
22 |
|
23 | float uF = (remoteData.g - 0.2)/(1-0.2)*40;
|
24 | float uX = pParamXY * errorX + dParamXY*DerrorX;
|
25 | float uY = pParamXY * errorY + dParamXY*DerrorY;
|
26 | float uZ = pParamZ * errorZ + dParamZ*DerrorZ;
|
27 |
|
28 | actualMoments.uF = uF;
|
29 | actualMoments.uX = uX;
|
30 | actualMoments.uY = uY;
|
31 | actualMoments.uZ = uZ;
|
32 |
|
33 | // now calculate motor values from u (see limitMotorValue for limits)
|
34 | MotorValues motorValues(Motors::calculateMotorValues(actualMoments));
|
35 | actualMotorValues = motorValues;
|
36 | }
|
37 | else {
|
38 | actualMoments.uF = 0;
|
39 | actualMoments.uX = 0;
|
40 | actualMoments.uY = 0;
|
41 | actualMoments.uZ = 0;
|
42 | MotorValues motorValues = motorvalues_ZERO;
|
43 | actualMotorValues = motorValues;
|
44 | }
|
45 |
|
46 | lasterrorX = errorX;
|
47 | lasterrorY = errorY;
|
48 | lasterrorZ = errorZ;
|
Wäre Klasse wenn mir jemand weiterhelfen könnte, ich weiß nicht so recht
wo ich nun Probleme ausfindigmachen könnte...
bild2.gif
(41 KB)
