Modellierung eines Sumo-Robots
(EN google-translate)
(PL google-translate)
Auf der Grundlage der zuvor gefundenen Parameter soll nun ein dynamisches Modell eines Sumo-Robots erstellt werden.
Das Modell wird eben sein (von oben betrachtet).
Die Raupenketten werden über vier an den Enden der Auflagefläche der Raupenketten liegenden Kräfte angenähert.
Die Kräfte in den vier Angriffspunkten A, B, C, D setzen sich zusammen aus einem konstanten entgegen der aktuellen Orientierung der Geschwindigkeit wirksamen Reibterm FR, einem Dämpfungsterm FD und einem Term, der der Antriebskraft FA entspricht.
Der Reibterm muß zu der Tatsache passend gewählt werden, dass sowohl in, als auch quer zur Fahrtrichtung es mindestens ca. 6N bedarf, um das stehende Fahrzeug wegschieben zu können. Dies wurde mit einer Federwaage gemessen. Auf die vier Kraftangriffspunkte entfallen folglich jeweils 1,5N dieser Gesamtkraft als Reibkraft.
Neben den Kraftangriffspunkten A, B, C, D, gibt es noch weitere ausgezeichnete Punkte: Den Schwerpunkt S, der im geometrischen Zentrum von A, B, C, D liegend angenommen wird,sowie die durch die Punkte E, F, G, H bestimmte rechteckige umlaufende Stoßstange, welche leicht nach hinten versetzt angebracht ist und die Grenzen des Fahrzeugs darstellt. Jeder Punkt dieser Stoßstange kann im Verlauf der Simulation Angriffspunkt einer aus einer Kollision herrührenden äußeren Kraft werden.
Identifiziert werden muß vor allem der Dämpfungsfaktor D, der für alle vier Punkte A, B, C, D als gleich angenommen wird und ein Verstärkungsfaktor K, der bei dem zunächst als linear angenommenen Zusammenhang zwischen PWM-Signal und Antriebskraft auftritt.
Zur Orientierung zeigt die folgende Grafik die Lage der ausgezeichneten Punkte eines Sumo-Robots, wobei ein körperfestes Koordinatensystem mit dem Ursprung im Schwerpunkt eingeführt wird, dessen x-Achse in positive Fahrtrichtung (vorwärts, kürzeres Ende) zeigt.
Bild 0-1: Geometrie des Sumo-Robots mit den Kraftangriffspunkten, die Ersatz für die Ketten sind.
Die Koordinaten der ausgezeichneten Punkte im körpereigenen Koordinatensystem sind dann folgende:
Punkt x/m y/m S 0.0 0.0 A 0.0565 -0.054 B 0.0565 0.054 C -0.0565 0.054 D -0.0565 -0.054 E 0.1065 -0.085 F 0.1065 0.085 G -0.1325 0.085 H -0.1325 -0.085
Code 0-1: Koordinaten der ausgezeichneten Punkte eines Sumo-Robots.
Die Identifikation und erste Tests des Modells erfolgen mit Scilab, da hier geeignete Werkzeuge schon zur Verfügung stehen.
Visualisierung mit Scilab
Bild 0-2: Visualisierung des Fahrzeugs über die plot-Funktion von Scilab.
modell001_visualisierung.zip - Test-Script zur Visualisierung.
modell002_identifikation.zip - Modell und Testsimulation
modell003_fehlerfunktion.zip - Fertiges Skript zur Identifikation von "PWMFAKTOR" und "DAMP".
//Definition aller wichtigen Parameter des Sumo-Modells:
//Punkte:
PS=[0;0];
PA=[ 0.0565 ; -0.054];
PB=[ 0.0565 ; 0.054];
PC=[-0.0565 ; 0.054];
PD=[-0.0565 ; -0.054];
PE=[ 0.1065 ; -0.085];
PF=[ 0.1065 ; 0.085];
PG=[-0.1325 ; 0.085];
PH=[-0.1325 ; -0.085];
//Fahrzeug an einer Stelle x,y in einem Winkel phi zeigen:
//Koordinatentransformation
function vek = tr(P,x,y,phi)
T = [cos(phi),-sin(phi);sin(phi),cos(phi)];
vek = [x;y]+T*P;
endfunction
function zeigeFahrzeug(x,y,phi)
fahrzeug1 = [tr(PS,x,y,phi),tr(PS+[0.1;0],x,y,phi)];
fahrzeug2 = [tr(PA,x,y,phi),tr(PB,x,y,phi),tr(PC,x,y,phi),tr(PD,x,y,phi),tr(PA,x,y,phi)];
fahrzeug3 = [tr(PE,x,y,phi),tr(PF,x,y,phi),tr(PG,x,y,phi),tr(PH,x,y,phi),tr(PE,x,y,phi)];
plot(fahrzeug1(1,:),fahrzeug1(2,:),fahrzeug2(1,:),fahrzeug2(2,:),fahrzeug3(1,:),fahrzeug3(2,:));
endfunction
function zeigeTisch()
r = 0.85/2;
p = 0:0.01:2.0*%pi;
x = r*cos(p);
y = r*sin(p);
plot(x,y);
endfunction
function zeigeRahmen()
{
plot([0.5,0.5,-0.5,-0.5,0.5],[-0.5,0.5,0.5,-0.5,-0.5]);
}
clf(); //Grafik löschen
zeigeRahmen();
zeigeTisch();
x = 0.25;
y = 0.0;
phi = %pi*0.25;
zeigeFahrzeug(x,y,phi);
ax = gca();
ax.auto_scale = "off";
Code 0-2: Test-Script zur Visualisierung.
Mechanisches Modell
modell002_identifikation.zip - Erste Tests mit mechanischem Modell ohne Parameteroptimierung.
//Punkte:
PS=[0;0];
PA=[ 0.0565 ; -0.054];
PB=[ 0.0565 ; 0.054];
PC=[-0.0565 ; 0.054];
PD=[-0.0565 ; -0.054];
PE=[ 0.1065 ; -0.085];
PF=[ 0.1065 ; 0.085];
PG=[-0.1325 ; 0.085];
PH=[-0.1325 ; -0.085];
//Masse des Fahrzeugs:
m = 0.871; //kg
//Masseträgheitsmoment:
//Korpus als homogen angenommen,
//Stoßstange als masselos:
J = (1/12)*m*(0.108*0.108 + 0.113*0.113); //kg*m/s**2
//Verstärkungsfaktor, mit dem die pwm-Werte multipliziert werden,
//um die Antriebskraft zu erhalten:
PWMFAKTOR = 0.008; //gesuchter Parameter. Dies ist der Startwert.
//Linearfaktor für den Dämpfungsterm in jedem Punkt A,B,C,D:
DAMP = 15; //gesuchter Parameter. Dies ist der Startwert.
//Koordinatentransformation
function vek = tr(P,x,y,phi)
T = [cos(phi),-sin(phi);sin(phi),cos(phi)];
vek = [x;y]+T*P;
endfunction
//Nur Drehung
function vek = dr(P,phi)
T = [cos(phi),-sin(phi);sin(phi),cos(phi)];
vek = T*P;
endfunction
//Kreuzprodukt, liefert 3. komponente als Skalar
function w = kr(P,Q)
w = P(1)*Q(2)-P(2)*Q(1);
endfunction
//Kreuzprodukt speziell um v aus omega zu bestimmen
function v = kv(w,P)
v = [-w*P(2);w*P(1)];
endfunction
function f = rechteSeite(t,yy)
x = yy(1,1);
y = yy(2,1);
phi = yy(3,1);
vx = yy(4,1);
vy = yy(5,1);
om = yy(6,1);
//Kollisionskräfte sind zunächst keine vorhanden:
Fkollisionx = 0;
Fkollisiony = 0;
Mkollision = 0;
//Die linke und rechte Antriebskraft wird aus den PWM-Signalen
//mit einem Verstärkungsfaktor gebildet:
pwm_links = 799;
// pwm_links = 400;
pwm_rechts = 799;
Fantrieb_links = pwm_links * PWMFAKTOR;
Fantrieb_rechts = pwm_rechts * PWMFAKTOR;
//Die Antriebskräfte wirken in die Richtung, in die das Fahrzeug gerade orientiert ist:
FanL = dr([Fantrieb_links;0], phi);
FanR = dr([Fantrieb_rechts;0], phi);
//Vorbereitung der Dämpfungskräfte:
//a) Verbindungsvektoren von S zu ABCD im Inertialkoordinatensystem
rA = dr(PA, phi);
rB = dr(PB, phi);
rC = dr(PC, phi);
rD = dr(PD, phi);
//b) Aktuelle Geschwindigkeit in jedem Punkt ABCD bestimmen:
// vp = v + w x r
vA = [vx;vy] + kv(om,rA);
vB = [vx;vy] + kv(om,rB);
vC = [vx;vy] + kv(om,rC);
vD = [vx;vy] + kv(om,rD);
//c) Dämpfungskräfte entgegen aktueller Geschw. im jeweiligen Punkt:
FDampA = - DAMP*vA;
FDampB = - DAMP*vB;
FDampC = - DAMP*vC;
FDampD = - DAMP*vD;
//Reibkraft vorbereiten:
//6N insgesamt, 1.5 pro Kraftangriffspunkt.
//a) Skalare der Geschwindigkeitsanteile vorwärts und zur Seite bestimmen
evorn = dr([1;0], phi);
eseit = dr([0;1], phi);
vAvorn = vA'*evorn;
vAseit = vA'*eseit;
vBvorn = vB'*evorn;
vBseit = vB'*eseit;
vCvorn = vC'*evorn;
vCseit = vC'*eseit;
vDvorn = vD'*evorn;
vDseit = vD'*eseit;
//Skalar zwischen +/-1.5 mit schmalem Übergang:
FReibA = -1.5*( -1/(1+exp(-1000*vAvorn))*2+1 )*evorn -1.5*( -1/(1+exp(-1000*vAseit))*2+1 )*eseit;
FReibB = -1.5*( -1/(1+exp(-1000*vBvorn))*2+1 )*evorn -1.5*( -1/(1+exp(-1000*vBseit))*2+1 )*eseit;
FReibC = -1.5*( -1/(1+exp(-1000*vCvorn))*2+1 )*evorn -1.5*( -1/(1+exp(-1000*vCseit))*2+1 )*eseit;
FReibD = -1.5*( -1/(1+exp(-1000*vDvorn))*2+1 )*evorn -1.5*( -1/(1+exp(-1000*vDseit))*2+1 )*eseit;
//Die Kräfte setzen sich aus den einzelnen Komponenten zusammen:
FA = 0.5*FanR + FDampA + FReibA;
FB = 0.5*FanL + FDampB + FReibB;
FC = 0.5*FanL + FDampC + FReibC;
FD = 0.5*FanR + FDampD + FReibD;
//Momente, die durch die Kräfte in ABCD wirken bestimmen:
MA = kr(rA,FA);
MB = kr(rB,FB);
MC = kr(rC,FC);
MD = kr(rD,FD);
//Aufspaltung der Kräfte in x/y-Komponenten:
FAx = FA(1);
FAy = FA(2);
FBx = FB(1);
FBy = FB(2);
FCx = FC(1);
FCy = FC(2);
FDx = FD(1);
FDy = FD(2);
f(1,1) = vx;
f(2,1) = vy;
f(3,1) = om;
f(4,1) = (FAx+FBx+FCx+FDx + Fkollisionx)/m;
f(5,1) = (FAy+FBy+FCy+FDy + Fkollisiony)/m;
f(6,1) = (MA +MB +MC +MD + Mkollision)/J;
endfunction
//Geradeausfahrt testen:
t = 0:0.01:10;
y0 = [0,0,0,0,0,0]';
t0 = 0;
y = ode(y0,t0,t,rechteSeite);
plot(t,y(4,:)',t,y(5,:)'); //t,vx,t,vy plotten
//plot(y(1,:)',y(2,:)'); //x,y plotten
Code 0-3: Mechanisches Modell.