Numerischer Tipp

[atzensepp]

Hallo,

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);
}

Wollen wir die Argumente der trigonometrischen Funktionen auf ]pi/2,pi[ beschränken, setzen wir a=pi/2 und b=pi.

sig.png (23.57 KiB) 528 mal betrachtet

Für sin(x) setzen wir sin(sig(x)) usw.

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;
}

Es kann natürlich sein, dass das Verfahren in den Ausläufern der sig-Funktion mal auf eine singuläre Jacobi-Matrix kommt. Dann gibt es keine Lösung oder man muss andere Startwerte probieren. Ggf. kann man auch die sig-funktion "invertieren", um die Suchrichtung zu verändern, so dass sie vom großen zum kleinen Wert läuft. Die sig-Funktion ist nur ein Beispiel. Man kann die Beschränkung auch durch sin- oder cos-Funktionen erreichen wie diese hier:

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;
}

Hier hat man seltener den Effekt, dass sich das Newton-Verfahren totläuft. Wenn es mehrere Lösungen im Definitionsbereich gibt, ist es Zufall bzw. hängt von den Startwerten ab, welche Lösung gefunden wird. Bei der logistischen Funktion steigt das Verfahren aus, wenn die Startwerte unpassend sind.