Hallo Hannes,

ich habe noch keinen Aufbau
Die Teile dafür sollten im Laufe der nächsten Woche bei mir eintreffen. Radbruch hat es aber von der Kinematik her exakt so umgesetzt, wie ich es vorhabe. In diesen Fred haben wir uns zusammengerottet
Trotzdem wollte ich die Ansteuerung schonmal soweit wie möglich fertig machen. Konstruktionsmerkmale wie zB den Abstand der Arm-Anfänge vom Mittelpunkt des Bots habe ich als Variable implementiert, die kann ich nachträglich anpassen.

Mein Gedankengang zur Rückwärtskinematik ist folgender:
Gesucht ist der Winkel zwischen dem Oberen Armteil und der Ebene, in der der Bot befestigt ist (welche ich fix als parallele zur xy-Ebene definiert habe).
Um den Winkel zu finden, muss man den Ort des mittleren Armgelenkes ("Ellenbogen") kennen. Von diesem kenne ich folgende Eigenschaften:
- Abstand zum Anfang des Armes
- Abstand zum Ende des Armes (Ende des Armes ist abhängig von der zu erreichenden Koordinate)
- Liegt auf der Ebene E1, welche alle Punkte beinhaltet, die der "Ellenbogen" durch Rotation am Armanfangs-Gelenk erreichen kann.

Ich habe nun zwei Kugeln generiert, eine um den Armanfang, eine um das Armende, auf dessen Schnittkreis der Ellenbogen liegen muss. Der Schnittkreis liegt in Ebene E2, welche durch Gleichsetzen der Kugelgleichungen entstanden ist.

Das Gelenk muss also auf der Schnittgeraden von E1 und E2 liegen. Diese sei G.

Auf G finde man nun die Punkte, die zu Arm-Anfang und Arm-Ende mit den tatsächlichen Ausmaßen des Arms übereinstimmende Abstände haben. Von diesen ist nun jener zu Wählen, der vom Mittelpunkt der Konstruktion weiter entfernt ist (Damit der Arm nach aussen und nicht nach innen geknickt ist).

Ja, und nach G Suche ich nun.

Falls du vor einem Herzinfarkt nicht zurückschreckst, kannst du dir auch gerne mal den Code anschauen, den ich mir bisher gebraut habe:

Code:
'******************************************************************************
'***                      Delta-Ansteuerung3.BAS                            ***
'******************************************************************************
'***                                                                        ***
'***  Stellt eine Rückwärtskinematik für einen 3-Armigen Roboter nach       ***
'***  Delta-Art zur Verfügung.                                              ***
'***                                                                        ***
'***  Programm wurde für AtMega32 auf RN_Control V1.4 konzipiert.           ***
'***                                                                        ***
'******************************************************************************
$regfile = "m32def.dat"
$framesize = 32
$swstack = 32
$hwstack = 32
$crystal = 16000000
$baud = 9600

'Für Tastenabfrage:
Config Adc = Single , Prescaler = Auto
Config Pina.7 = Input
Porta.7 = 1

'******************************************************************************
'***  Deklaration von Unterprogrammen                                       ***
'******************************************************************************
Declare Function Tastenabfrage() As Byte
Declare Sub Update_ae()
Declare Sub Update_abst()
Declare Function Check_abst() As Byte
Declare Sub Update_ebene1()
Declare Sub Init_calc()
Declare Sub Clr_hilf()
Declare Sub Change_t()
Declare Sub Nix()

'******************************************************************************
'***  Deklaration von Variablen                                             ***
'******************************************************************************
Dim Taste As Byte                                           'Für die Onboard-Tasten
Dim Abst_ok As Byte                                         'Für die Rückgabe von Check_abst()
Dim I As Integer
Dim J As Integer
Dim K As Integer
Dim Single_hilf(5) As Single                                'Hilfsvariablen für Zwischenergebnisse
Dim Long_hilf(5) As Long

Dim B_x As Long                                             'Drei Koordinaten des Punktes B = Befestigungspunkt des Bots im Raum
Dim B_y As Long
Dim B_z As Long
Dim Oa As Long                                              'Länge des Servosnahen Armteils
Dim Ua As Long                                              'Länge des servofernen Armteils
Dim Abst_b_aa_xy As Long                                    'Abstand der Arm-Anfänge vom Befestigungspunkt B, gemessen in Projektion auf die xy-Ebene
Dim Abst_b_aa_z As Long                                     'Abstand der Arm-Anfänge vom Befestigungspunkt B, gemessen nur in z-Richtung
Dim Abst_t_ae_xy As Long                                    'Abstand der Arm-Enden vom Tool-Punkt T, gemessen in Projektion auf die xy-Ebene
Dim Abst_t_ae_z As Long                                     'Abstand der Arm-Enden vom Tool-Punkt T, gemessen nur in z-Richtung

Dim Aa_x(3) As Long                                         'x-Koordinate des Armanfangs für jeden Arm
Dim Aa_y(3) As Long                                         'y-Koordinate des Armanfangs für jeden Arm
Dim Aa_z(3) As Long                                         'z-Koordinate des Armanfangs für jeden Arm
Dim Ae_x(3) As Long                                         'x-Koordinate des Armendes für jeden Arm
Dim Ae_y(3) As Long                                         'y-Koordinate des Armendes für jeden Arm
Dim Ae_z(3) As Long                                         'z-Koordinate des Armendes für jeden Arm

Dim Abst_min As Long                                        'Minimaler Abstand, den Armanfang zu Armende haben muss
Dim Abst_max As Long                                        'Maximaler Abstand, den Armanfang zu Armende haben darf

Dim T_x As Long                                             'Drei Koordinaten des Punktes T = Tool-Punkt.
Dim T_y As Long                                             'Das ist der Punkt, des der Bot erreichen soll.
Dim T_z As Long

Dim Abst(3) As Long                                         'Abstand von Armanfang zu Armende für jeden Arm.

Dim D(12) As Single                                         'Parameter der Schnittebene E2. Siehe Dokumentation ######################### TODO: ERKLÄRUNG IN DOKU EINFÜGEN ###########################
Dim E(3) As Single                                          'Parameter der Ebene E1 zu jedem Arm. Die Ebene geht durch B, AA und [ B - (0,0,1) ]

'******************************************************************************
'***  Definition der Konstruktionsmerkmale                                  ***
'******************************************************************************
'Nachfolgend werden die Konstruktionsmerkmale des Bots festgelegt. Die Bedeutung
'der Variablen ist bei der Deklaration kommentiert.

B_x = 0
B_y = 0
B_z = 0

Oa = 100000000                                              '=10cm
Ua = 200000000                                              '=20cm

Abst_b_aa_xy = 10000000                                     '=1cm
Abst_b_aa_z = 20000000                                      '=2cm
Abst_t_ae_xy = 20000000                                     '=2cm
Abst_t_ae_z = 10000000                                      '=1cm

'******************************************************************************
'******************************************************************************
'******************************************************************************
'***  Hauptprogramm                                                         ***
'******************************************************************************
'******************************************************************************
'******************************************************************************

Debug On                                                    'Wenn ON, werden viele Zwischenergebnisse ausgegeben.
                                                             'Wenn OFF, werden nur Fehler ausgegeben

Call Init_calc
Print
Print "Gestartet"
Print "Tastenbelegung:"
Print "Taste1: Testweise den Punkt T ändern"
Print "Taste2: Armenden berechnen & Armabstaende berechnen"
Print "Taste3: Armabstaende pruefen"
Print "Taste4: Schnittebene berechnen"
Print "Taste5: Hilfsvariablen nullen"
Do
   Taste = Tastenabfrage()
   If Taste <> 0 Then
      Select Case Taste
         Case 1
            Call Change_t
         Case 2
            Call Update_ae
            Call Update_abst
         Case 3
            Abst_ok = Check_abst()
         Case 4
            Call Update_ebene1
         Case 5
            Call Clr_hilf
      End Select
   End If
   Waitms 100
Loop
End

'******************************************************************************
'***  Unterprogramme                                                        ***
'******************************************************************************

'Fragt die Tastatur am analogen Port A.7 ab. Rückgabewert=Tastennummer(1...5)
Function Tastenabfrage() As Byte
Local Ws As Word
   Tastenabfrage = 0
   Start Adc
   Ws = Getadc(7)
   If Ws < 500 Then
      Select Case Ws
         Case 400 To 450
            Tastenabfrage = 1
         Case 330 To 380
            Tastenabfrage = 2
         Case 260 To 305
            Tastenabfrage = 3
         Case 180 To 220
            Tastenabfrage = 4
         Case 90 To 130
            Tastenabfrage = 5
      End Select
   End If
End Function

'Generiert aus dem Toolpunkt T die Armendpunkte Ae
Sub Update_ae()
Debug "Update_ae() gestartet"
'Arm1
Ae_x(1) = T_x
Ae_y(1) = T_y + Abst_t_ae_xy
Ae_z(1) = T_z + Abst_t_ae_z
'Arm2
Single_hilf(1) = 0.866025403784435                          '=Cos(30°)
Long_hilf(1) = Single_hilf(1) * Abst_t_ae_xy
Ae_x(2) = T_x - Long_hilf(1)
Long_hilf(2) = Abst_t_ae_xy / 2
Ae_y(2) = T_y - Long_hilf(2)
Ae_z(2) = T_z + Abst_t_ae_z
'Arm3
Ae_x(3) = T_x + Long_hilf(1)
Ae_y(3) = T_y - Long_hilf(2)
Ae_z(3) = T_z + Abst_t_ae_z
Debug "Armenden berechnet:"
Debug "Ae1: ( " ; Ae_x(1) ; " , " ; Ae_y(1) ; " , " ; Ae_z(1) ; ")"
Debug "Ae2: ( " ; Ae_x(2) ; " , " ; Ae_y(2) ; " , " ; Ae_z(2) ; ")"
Debug "Ae3: ( " ; Ae_x(3) ; " , " ; Ae_y(3) ; " , " ; Ae_z(3) ; ")"
End Sub
'###############################################################################


'Berechnet den Abstand von Armanfang zu Armende für jeden Arm.
'Die Formel ist der "verallgemeinerte Pythagoras".
Sub Update_abst()
Debug "Update_abst() gestartet"
For I = 1 To 3
   Long_hilf(1) = Aa_x(i) - Ae_x(i)
   Single_hilf(1) = Long_hilf(1) * Long_hilf(1)
   Long_hilf(1) = Aa_y(i) - Ae_y(i)
   Single_hilf(2) = Long_hilf(1) * Long_hilf(1)
   Long_hilf(1) = Aa_z(i) - Ae_z(i)
   Single_hilf(3) = Long_hilf(1) * Long_hilf(1)
   Single_hilf(3) = Single_hilf(3) + Single_hilf(2)
   Single_hilf(3) = Single_hilf(3) + Single_hilf(1)
   Abst(i) = Sqr(single_hilf(3))
Next I
Debug "Abstände berechnet:"
Debug "Abst(1) = " ; Abst(1)
Debug "Abst(2) = " ; Abst(2)
Debug "Abst(2) = " ; Abst(3)
End Sub
'###############################################################################


'Prueft, ob der Abstand von Armanfang zu Armende zulaessig ist,
'also, ob der Bot diesen Ort erreichen kann. Wenn der Ort erreichbar ist,
'wird 3 zurückgegeben, sonst eine Zahl < 3.
Function Check_abst() As Byte
Debug "Check_abst() gestartet"
Debug "Abst_max = " ; Abst_max
Debug "Abst_min = " ; Abst_min

Check_abst = 0
I = 1
For I = 1 To 3
   If Abst(i) < Abst_max And Abst(i) > Abst_min Then
      Check_abst = Check_abst + 1
   End If
Next I
Debug "Rueckgabewert: " ; Check_abst ;
Debug " (3=OK, weniger=Fehler)"
Debug "Check_abst beendet"
End Function
'###############################################################################


'Berechnet eine Ebenengleichung. In dieser Ebene muss das Armgelenk liegen.
'Die Ebenengleichung hat die Form D(j)*x + D(j+1)*y + D(j+2)*z - D(j+3) = 0
'wobei für Arm1 J=1, für Arm2 J=5 und für Arm3 J=9 gilt.
'Alle drei Ebenen liegen nachher im Array D(12) vor.
Sub Update_ebene1()
Debug "Update_ebene1() gestartet"
If Abst_ok = 3 Then
   Debug "Ebenendaten werden berechnet."
   I = 1
   J = 1
   Gosub Berechne_ebene1
   I = 2
   J = 5
   Gosub Berechne_ebene1
   I = 3
   J = 9
   Gosub Berechne_ebene1
   Debug "Ebenendaten berechnet"
   Debug "Ebene1(1): " ; D(1) ; " , " ; D(2) ; " , " ; D(3) ; " , " ; D(4)
   Debug "Ebene1(2): " ; D(5) ; " , " ; D(6) ; " , " ; D(7) ; " , " ; D(8)
   Debug "Ebene1(3): " ; D(9) ; " , " ; D(10) ; " , " ; D(11) ; " , " ; D(12)
Else
   Print "Fehler: Punkt nicht erreichbar. Ebene nicht berechnet."
   End If
Debug "Update_ebene1() beendet"
End Sub
'###############################################################################


'Führt einmalige Berechnungen durch, zB für den Arm-Anfangs-Punkt jedes Armes
Sub Init_calc()
Debug "Init_calc() gestartet"
'Arm1 Anfangspunkt
Aa_x(1) = B_x
Aa_y(1) = B_y + Abst_b_aa_xy
Aa_z(1) = B_z - Abst_b_aa_z
'Arm2 Anfangspunkt
Single_hilf(1) = 0.866025403784435                          '=Cos(30°)
Long_hilf(1) = Single_hilf(1) * Abst_b_aa_xy
Aa_x(2) = B_x - Long_hilf(1)
Long_hilf(2) = Abst_b_aa_xy / 2                             '=Sin(30°)*Abst_b_aa_xy = Abst_b_aa_xy / 2
Aa_y(2) = B_y - Long_hilf(2)
Aa_z(2) = B_z - Abst_b_aa_z
'Arm3 Anfangspunkt
Aa_x(3) = B_x + Long_hilf(1)
Aa_y(3) = B_y - Long_hilf(3)
Aa_z(3) = B_z - Abst_b_aa_z

'definiert minimalen/maximalen Abstand, den Armanfang von Armende haben muss.
'Die Korrektur verhindert, dass die Arme ganz durchgestreckt
'bzw komplett in sich gefaltet werden.
Long_hilf(1) = Oa - Ua
Long_hilf(1) = Abs(long_hilf(1))
'Korrektur:
Abst_min = Long_hilf(1)
Long_hilf(1) = Long_hilf(1) / 7
Abst_min = Abst_min + Long_hilf(1)

Long_hilf(1) = Oa + Ua
Abst_max = Long_hilf(1)
'Korrektur:
Long_hilf(1) = Long_hilf(1) / 8
Abst_max = Abst_max - Long_hilf(1)
Debug "Abst_max=" ; Abst_max
Debug "Abst_min=" ; Abst_min
Debug "Init_calc beendet"
End Sub
'###############################################################################


'Überschreibt alle Hilfsvariablen mit Nullen
Sub Clr_hilf()
Debug "Clr_Hilf() gestartet"
For I = 1 To 5
   Single_hilf(i) = 0
   Long_hilf(i) = 0
Next I
Debug "Clr_Hilf beendet:" ; Single_hilf(1) ; Single_hilf(2) ; Single_hilf(3) ; Single_hilf(4) ; Single_hilf(5) ; Long_hilf(1) ; Long_hilf(2) ; Long_hilf(3) ; Long_hilf(4) ; Long_hilf(5)
End Sub
'###############################################################################


Sub Change_t()
Debug "Change_t() gestartet"
Select Case T_z
   Case 0
      T_x = 20000000
      T_y = 30000000
      T_z = -103000000
   Case -103000000
      T_x = 0
      T_y = -30000000
      T_z = -150000000
   Case -150000000
      T_x = -20000000
      T_y = 60000000
      T_z = -180000000
   Case -180000000
      T_x = -40000000
      T_y = -20000000
      T_z = -250000000
   Case -250000000
      T_x = 60000000
      T_y = 150000000
      T_z = -320000000
   Case -320000000
      T_x = 0
      T_y = 0
      T_z = 0
End Select
Debug "T geändert auf ( " ; T_x ; " , " ; T_y ; " , " ; T_z ; ")"
Debug "Change_t beendet"
End Sub
'###############################################################################


'Dieses Label wird vom Sub Update-ebene1() benutzt.
'I=Arm-Nummer, J=Erstes Element im Array D(12) für den jeweiligen Arm.
Berechne_ebene1:
D(j) = Aa_x(i) - Ae_x(i)

D(j + 1) = Aa_y(i) - Ae_y(i)

D(j + 2) = Aa_z(i) - Ae_z(i)

Single_hilf(1) = Aa_x(i) * Aa_x(i)
D(j + 3) = Single_hilf(1)
Single_hilf(1) = Aa_y(i) * Aa_y(i)
D(j + 3) = D(j + 3) + Single_hilf(1)
Single_hilf(1) = Aa_z(i) * Aa_z(i)
D(j + 3) = D(j + 3) + Single_hilf(1)
Single_hilf(1) = Oa * Oa
D(j + 3) = D(j + 3) - Single_hilf(1)
Single_hilf(1) = Ae_x(i) * Ae_x(i)
D(j + 3) = D(j + 3) - Single_hilf(1)
Single_hilf(1) = Ae_y(i) * Ae_y(i)
D(j + 3) = D(j + 3) - Single_hilf(1)
Single_hilf(1) = Ae_z(i) * Ae_z(i)
D(j + 3) = D(j + 3) - Single_hilf(1)
Single_hilf(1) = Ua * Ua
D(j + 3) = D(j + 3) + Single_hilf(1)
Return



'Macht nichts und belegt unnoetig Speicherplatz. Die Funktion ist noetig,
'damit sich Nutzer von Windows heimisch fühlen.
Sub Nix()
End Sub
Deine Algebraische Lösung interessiert mich natürlich sehr (und auch alles andere, was du zu diesem Thema so loswerden willst )

Grüße aus Düsseldorf,
Benjamin