kinematische Funktionen für Modelle enthalten oft die trigonometrischen Funktionen sin(x) und cos(x). Die Periodizität dieser Funktionen (x und x+2*n*pi liefert das Gleiche) überträgt sich dann oft auf die kinematischen Ausdrücke und bei der Invertierung dieser Ausdrücke in Näherungsverfahren (z.B. Newton) bekommt man dann oft Winkelwerte für x, die außerhalb des für das Modell sinnvollen Bereiches liegen.
Ersetzt man x durch eine Funktion s(x) , die das Argument im gewünschten Definitionsbereich hält, kann man die Numerik in gewisser Weise überlisten.
Ein Beispiel einer geeigneten Funktion ist die logistische Funktion sig(x), die man "tunen" kann, so dass sie nur Werte liefert. Weil man für das Newton-Verfahren die Ableitungen (i.a. Jacobi-Matrix) benötigt, braucht man dann auch zum "Nachdifferenzieren" die Ableitung der sig(x)-Funktion.
Code: Alles auswählen
double sig(double t, double a, double b) {
return a + (b - a) / (1 + exp(-((t - a) * 8 / (b - a) - 4)));
}
double dsig(double t, double a, double b) {
double expTerm = exp(-((t - a) * 8 / (b - a) - 4));
return (8 * (b - a) * expTerm) / pow((1 + expTerm), 2);
}
Hier ein Solver für Backward-Kinematik eines Scara mit Newton-Verfahren mit der sig-Funktion.
Zu gegebener Position (x,z) werden die Inklinationswinkel alpha und beta bestimmt:
(Scara kann man auch ohne Newton-Verfahren lösen. Aber es gibt Kinematiken, bei denen das nicht geht)
Code: Alles auswählen
int solve_x_z_to_alpha_beta(float x,float z,float *alpha, float * beta)
{
for (int iter = 0; iter < max_iter; iter++) {
// Funktionswerte
float f1 = a * cos(sig(*alpha,alpha0,alpha1)) + b * cos(sig(*beta,beta0,beta1)) - x;
float f2 = a * sin(sig(*alpha,alpha0,alpha1)) + b * sin(sig(*beta,beta0,beta1)) - z;
// Ableitungen (Jacobi-Matrix)
float df1_dalpha = -a * sin(sig(*alpha,alpha0,alpha1))*dsig(*alpha,alpha0,alpha1);
float df1_dbeta = -b * sin(sig(*beta,beta0,beta1))*dsig(*beta,beta0,beta1);
float df2_dalpha = a * cos(sig(*alpha,alpha0,alpha1))*dsig(*alpha,alpha0,alpha1);
float df2_dbeta = b * cos(sig(*beta,beta0,beta1))*dsig(*beta,beta0,beta1);
// Determinante der Jacobi-Matrix
float det = df1_dalpha * df2_dbeta - df1_dbeta * df2_dalpha;
if (fabs(det) < 1e-8) {
printf("Jacobi-Matrix ist singulär!");
return 0;
}
// Inverse der Jacobi-Matrix mal Funktionsvektor (delta = -J⁻¹·f)
float delta_alpha = (-f1 * df2_dbeta + f2 * df1_dbeta) / det;
float delta_beta = (-f2 * df1_dalpha + f1 * df2_dalpha) / det;
*alpha += delta_alpha;
*beta += delta_bet
// Konvergenzprüfung
if (fabs(delta_alpha) < tol && fabs(delta_beta) < tol) {
break;
}
}
// die Winkel, die wir ausrechnen wollen:
*alpha = sig(*alpha,alpha0,alpha1);
*beta= sig(*beta,beta0,beta1);
return 1;
}
Code: Alles auswählen
/**
* Funktion, um t auf [t0-t1] zu bescrhänken
* t0,t1
* phase: 0 .. M_PI um die Richtung der Abbildung zu beeinflussen
*/
float sin_(float t,float t0, float t1,float phase)
{
float tt = (sin( (t-t0)/(t1-t0)*2.0*M_PI-M_PI/2.0 +phase)+1)* (t1-t0)*0.5 + t0;
if (tt < t0 || tt > t1)
{
printf("Range Error: %f not in [%f,%f]\n" ,tt,t0,t1);
}
return tt;
}