Schnittpunkt dreier Kugeln berechnen

[dj_cyborg]

Hallo,

ich versuche mich seit geraumer Zeit an der Berechnung des Schnittpunktes dreier Kreise. Ich habe eine sehr gute Dokumentation (http://www.mevis-research.de/~richard/mnu05-slides.pdf) gefunden. Nur leider ist mir der Rechenweg zu komplex. Hier (http://www.gomatlab.de/bestimmung-des-schnittpunktes-von-3-kugeln-im-raum-t11960.html) werden die Ausgangsgleichungen mit der funktion SOLVE gelöst. Diese funktion gibt es meineswissens aber in BASCOM nicht.

Gegeben sind die Positionen der Punkte S1, S2, S3 und deren Abstände (Radien) zum Punkt P1. (alle Winkel innerhalb der Dreiecks-Pyramide kann ich auch berechnen)
Gesucht sind die Koordinaten zum Punkt P1.

29917

Ich versuche zu einer Formel zu kommen bei der ich meine Eingangsdaten verwenden kann.
Ich hatte gehofft über dreiecksberechnung innerhalb der unregelmäßigen Dreieckspyramide ans Ziel zu kommen.

Evtl. hat jemand für mich eine Formle/Lösungsweg/Tipp der sich auch in BASCOM umsetzen lässt.

vielen Dank

mfG
Mario

[damfino]

Die Funktion Solve ist ganz nett, aber von Mathlab. Es braucht schon einige Einarbeitungszeit um mit Mathlab umgehen zu können, abgesehen vom Preis.
Wenn du mehr mit Mathematik machen willst, kann man Scilab empfehlen. Leicht ist es aber auch nicht.

Zum Problem Schnittpunkt 2er Kreise: man erhält 0-2 Lösungen (berührend, oder 2 Schnittpunkte, oder gar kein Schnittpunkt), daher muss man vorher überprüfen welche Lösung die richtige ist. Dabei insbesondere auf Divison durch Null aufpassen.
Kosinussatz bietet sich dafür an.

Schnittpunkt 3er Kreise: wie oben, nur 3x so viele Lösungsmöglichkeiten.

Da du dich anscheinend auf GPS beziehst (Doku): mit den ganzen Abweichungen ergibt sich ein Bereich in dem die Position liegt.

Einfacherer Ansatz: richtigen Schnittpunkte von Kreis 1 und 2, 2 und 3, 3 und 1 berechnen, und daraus den Mittelwert. Damit umgeht man die Mehrdeutigkeiten.

Am Besten vorher alles in Ruhe durchrechnen, Formeln ausrechnen und vereinfachen wenn geht, und erst dann das Programm schreiben. Die Atmegas sind mathematisch nicht so gut drauf.

LG!

[HaWe]

je nachdem, wie genau die Messergebnisse sind und wie genau die Lösung sein muss, gibt es mehrere Ansätze:
1.) trigonometrisch (schwierig, weil die Messungenauigkeit "im echten Leben" keine eindeutigen Schnittpunkte zulässt wie in der Trigonometrie oder analytischen Geometrie, und (auch) zeitaufwändig wegen der - ohne fpu - langsamen atan-s etc)
2.) brute-force, iterativ: den gesamten Raum in n*m Quadrate zerlegen und iterativ die gesamte Fläche in einer doppelten Schleife abgrasen und dann den Wert nehmen, der am besten passt
3.) stochastische Monte-Carlo-Methode: ca. 100-1000 sehr gute Zufallszahlen erzeugen (z.B. mit Mersenne Twister, notfalls auch mit K+R Linear Congruence Generator) und dann den am besten passendsten Wert nehmen. Die Zufallszahlen in einer Schleife immer wieder zyklisch neu erzeugen und die ältesten Werte und ggf. die schlechtesten langsam "sterben" lassen. Etwa 3-4 Generationen würde ich sie dabei max. leben lassen. Man kann die Zufallszahlen mit ihrem Fehler auch z.B. in einem FIFO stack oder einem Ring speichern und so nach und nach wieder rausdrängen etc.

zum "am besten passend": da würde ich die Summe der Quadrate der Abweichungen vom Radius der Einzel-Kreise nehmen.

Vorteile bei 2+3:
Man kann leichter durch Einschränkung des Ausgangspunktebereichs Ergebnisse ausschließen, die mathematisch korrekt sind aber unplausibel wären (etwa wie "Schnittpunkte jenseits der Wand").

HTH!

[TiGePa]

Ich versuch hier mal nen Ansatz wiederzugeben:
x1,y1,z1 Koordinaten von S1, x2,y2,z2,x3,y3,z3 analog, x,y,z Koordinaten von P1 (die Unbekannten), r1,r2,r3 Abstände von S1,S2,S3 zu P1
Aus den drei Punkten S1-S3 und den Abständen zu P1 ergeben sich drei Kugelgleichungen:
(x-x1)^2 + (y-y1)^2 + (z-z1)^2 = r1^2 (I)
(x-x2)^2 + (y-y2)^2 + (z-z2)^2 = r2^2 (II)
(x-x3)^2 + (y-y3)^2 + (z-z3)^2 = r3^2 (III)
Diese nun ausmultiplizieren und Konstanten auf die rechte Seite bringen liefert:
x^2-2xx1 + y^2-2yy1 + z^2-2zz1 = r1^2 - x1^2 - y1^2 - z1^2 (I)
x^2-2xx2 + y^2-2yy2 + z^2-2zz2 = r2^2 - x2^2 - y2^2 - z2^2 (II)
x^2-2xx3 + y^2-2yy3 + z^2-2zz3 = r3^2 - x3^2 - y3^2 - z3^2 (III)
Subtrahieren der ersten Gleichung von den anderen beiden liefert:
x^2-2xx1 + y^2-2yy1 + z^2-2zz1 = r1^2 - x1^2 - y1^2 - z1^2 (I)
2x(x1-x2) + 2y(y1-y2) + 2z(z1-z2) = r2^2-r1^2 + x1^2 - x2^2 + y1^2 - y2^2 + z1^2 - z2^2 (IV)
2x(x1-x3) + 2y(y1-y3) + 2z(z1-z3) = r3^2-r1^2 + x1^2 - x3^2 + y1^2 - y3^2 + z1^2 - z3^2 (V)
Da mir das jetzt ein wenig zuviel Schreibarbeit wird und ich dir nicht alles abnehmen möchte ;-), so sollte es weiter gehen:
das (x1-x3)-fache von (IV) von dem (x1-x2)-fachem von (V) abziehen -> Ergebnis (VI) hat nun nur noch zwei unbekannte.
Auflösen von (VI) nach y liefert was in der Form y = a*z (VII)
Einsetzen in (IV) und auflösen nach x liefert was von der Form x = b*z (VIII)
(VII) und (VIII) in (I) einsetzen liefert eine quadratische Gleichung mit Unbekannter z. Diese liefert theoretisch zwei Lösungen von der vermutlich schon eine als unmöglich ausgeschlossen werden kann.
Habs nicht nachgerechnet. Könnte aber klappen :-)
Das ganze ist natürlich ne rein analytische Lösung. Wie stabil das ganze später numrisch läuft ist ne andere Geschichte. Die Ausdrücke werden extrem lang wenn man das von Hand alles ausrechnet. Wenn sich P1 nur in einer Ebene bewegt (z bleibt konstant), wird das ganze einfacher, da man dann mit ner Projektion der Punkte S1-S3 auf die Ebene arbeiten könnte und man nur noch zwei Unbekannte hat.

[dj_cyborg]

Hallo,

vielen Dank für eure Antworten.

Brute-force und Zufallszahl könnten im 3D Raum etwas zu lange dauern .

Ich werde mich zu erst an TiGePa´s Ansatz versuchen.

Mal sehen wie weit ich komme.

Vielen Dank erst mal.

mfG
Mario

Zusatzfrage:

Ist die Berechnung nicht Ähnlich der von Deltabots?

[WL]

Hallo,

ich habe so etwas (Schnittpunkt zweier Kreise) mal hier https://www.roboternetz.de/community/threads/54798-Laser-Positionsscanner gebraucht:


Sub Schnitt_kreis(xm1 As Single , Ym1 As Single , R1 As Single , Xm2 As Single , Ym2 As Single , R2 As Single , Xs1 As Single , Ys1 As Single , Xs2 As Single , Ys2 As Single , M As Integer)
Dx = Xm2 - Xm1
Dy = Ym2 - Ym1
A2 = Dx * Dx : Temp = Dy * Dy : A2 = A2 + Temp
R12 = R1 * R1
R22 = R2 * R2
H = -4 * A2
If H = 0 Then
M = 1
Exit Sub
End If
Hi = R12 - R22 : Hi = Hi - A2
B2 = Hi * Hi : B2 = B2 / H : B2 = B2 + R22
If B2 < 0 Then
M = 1
Exit Sub
End If
B = Sqr(b2)
Temp = R12 - B2
Y = Sqr(temp)
A = Sqr(a2)
Xk = Dx / A
Yk = Dy / A
Xh = Xk * Y : Xh = Xh + Xm1
Yh = Yk * Y : Yh = Yh + Ym1
If B2 = 0 Then
M = 0
Xs1 = Xh : Ys1 = Yh
Exit Sub
End If
Xl = Xk * B
Yl = Yk * B
Xs1 = Xh - Yl
Ys1 = Yh + Xl
Xs2 = Xh + Yl
Ys2 = Yh - Xl
M = -1
End Sub

Der Algorithmus ist von hier http://www.antonis.de/faq/progs/_inhalt.htm

[Podi]

Ich würde das ganze iterativ lösen, mit Vektorrechnung. die maximale Ausdehnung des Raumes ist a*b*c

Den maximal möglichen Raum in 27 Kugeln aufteilen, die Mittelpunkte jeweils an den Ecken (4), in der Mitte der Kanten (16), in der Mitte der Flächen (6) und einer in der Mitte des Körpers (1)
Die Koordinaten der Punkte können so sehr einfach errechnet werden, die benötigte Kugelgröße auch. Der Radius der Kugeln beträgt den R=GrößteKante*1,5/4. (Eigendlich R=3.Wurzel{Größtekante^2+Größtekante^2+Größtekante ^2}/4=Größstekante*1,44.../4

Nun wird nacheinander für jeden der Kugeln überprüft ob die drei "Messabstände" durch gehen:
A_x: Vektor von Nullpunkt zu Satelit x
P_y: Vektor des Kugelmittelpunktes
a_x: Messabstand zu Satelit x
R: (Abstand der Kugelmittelpunkte)*0,75=Größtekante*1,5/4

Den Betrag des Verbindungsvektor berechnen ap_x=|(A_x-P_y)| und prüfen ob ap_x>(a_x-R) und ap_x<(a_x+R) ist.
Falls diese Bedingung für einen der drei Sateliten nicht erfüllt ist kann man zum nächsten Punkt gehen.

Sobald ein Punkt alle Bedingungen der Sateliten erfüllt kann wird der Raum verkleinert, mit dem Kugelmittelpunkt in der Mitte.
Der neue Raum hat dann die Kantenlänge R*2 und bekommt wieder die 27 Punkte. Nun wird das ganze wiederholt.

Da der neue Raum größer ist als die bisherige Kugel ist es egal das sich die Kugeln überschneiden, es ist aber nur eine minimale Menge an Wurzelrechnungen nötig (beim Betrag des Verbindungsvektors).

Als Abbruchkriterium ist dann:
R<Gewünschte Genauigkeit: Letzter Kugelmittelpunkt ist die Position mit Genauigkeit R

1000%ig ist die Lösung auch nicht, es sollte aber funktionieren wenn die gewünschte Genauigkeit größer ist als die Messungenauigkeit^3 der Abstände. Denkfehler vorbehalten!
Im Prinzip kann man auch mehr Punkte nehmen, da steigt aber meiner meinung nach auch der Rechenaufwand stark an (viele nicht-Treffer)

Ansonsten wäre der noch der analytische Weg, die drei Messkugeln als Vektor definieren und durch ein Gleichungsystem die Schnittpunkte ermitteln (viel spaß ^^), was durchaus schief gehen kann.

Grüße
Podi

[dj_cyborg]

Hallo,

und vielen Dank für Eure Antworten.

Da ich nicht gerade das riesige Mathegenie bin habe ich es doch mit einer automtischen Suche gemacht.


Begrenzt durch maximale Suchdurchgänge und dem Abgleich der Abweichung der berechneten Radien von den Zielradien lassen sich auch ganz gut Falsche Eingangswerte erkennen. Die Zeit ist durch eine dynamische Schrittanpassung auch nicht so lang wie ich dachte.

Habe mein Excel Testprgramm und den Code mal angehangen.

Vielen Dank für Eure hilfe.



Option Explicit

Dim P(5) As Double
Dim S1(5) As Double
Dim S2(5) As Double
Dim S3(5) As Double

Sub GetGPSPos()

Dim iStepCount As Integer
Dim iStepLimit As Integer
Dim iStep As Integer
Dim i As Integer

Dim dError As Double
Dim dTolerance As Double

Dim Mybook As Workbook
Dim MySheet As Worksheet

Dim StartTime As Date
Dim EndTime As Date

'Zeitmessung
StartTime = Timer

'Excelwerte holen
Set Mybook = ActiveWorkbook
Set MySheet = Mybook.ActiveSheet

For i = 0 To 3
S1(i) = MySheet.Cells(2, i + 2)
S2(i) = MySheet.Cells(3, i + 2)
S3(i) = MySheet.Cells(4, i + 2)
P(i) = 0
Next


'Grenzen festlegen
dTolerance = 1 'benötigte Toleranz
iStepCount = 0 'Durchlaufzähler rücksetzen
iStepLimit = 1000 'Durchlaufgrenze festlegen

Do
'Durchlaeufe zählen
iStepCount = iStepCount + 1

'Gesamtabweichung berechnen
dError = GetError

'Schritt festlegen
iStep = Fix(dError / 3)
If iStep = 0 Then
iStep = 1
End If


'X-Check
P(0) = P(0) + iStep
If GetError > dError Then
P(0) = P(0) - (2 * iStep)
If GetError > dError Then
P(0) = P(0) + iStep
End If
End If


'Y-Check
P(1) = P(1) + iStep
If GetError > dError Then
P(1) = P(1) - (2 * iStep)
If GetError > dError Then
P(1) = P(1) + iStep
End If
End If


'Z-Check
P(2) = P(2) + iStep
If GetError > dError Then
P(2) = P(2) - (2 * iStep)
If GetError > dError Then
P(2) = P(2) + iStep
End If
End If


'Ergebnis prüfen
If dError < dTolerance Or iStepCount >= iStepLimit Then

'Zeitmessung ende
EndTime = Timer


'Werte Ausgeben
MySheet.Cells(5, 2) = P(0)
MySheet.Cells(5, 3) = P(1)
MySheet.Cells(5, 4) = P(2)
MySheet.Cells(5, 6) = dError
MySheet.Cells(5, 7) = iStep
MySheet.Cells(5, 8) = iStepCount
MySheet.Cells(5, 9) = Format(EndTime - StartTime, "0.0")

'Schleife beenden
Exit Do

End If

Loop

End Sub

'Distanz berechnen
Function GetR(P1() As Double, P2() As Double) As Double

GetR = Sqr(((P1(0) - P2(0))) ^ 2 + ((P1(1) - P2(1))) ^ 2 + ((P1(2) - P2(2))) ^ 2)

End Function

Function GetError() As Double

'Distanz berechnen
S1(4) = GetR(S1, P)
S2(4) = GetR(S2, P)
S3(4) = GetR(S3, P)

'Abweichung berechnen
S1(5) = S1(4) - S1(3)
S2(5) = S2(4) - S2(3)
S3(5) = S3(4) - S3(3)

'Gesamtabweichung berechnen
GetError = Abs(S1(5)) + Abs(S2(5)) + Abs(S3(5))

End Function



29932