Titel

Verfahren zur Bestimmung eines Bewegungszustands eines starren Körpers

Die vorliegende Erfindung betrifft ein Verfahren zur Bestimmung eines Bewe gungszustands eines starren Körpers relativ zu einer Umgebung mittels einer Vielzahl von Messdatensätzen und ein Verfahren zu Bestimmung einer relativen Position und/oder einer relativen Orientierung sowie eine Recheneinheit und ein Computerprogramm zu deren Durchführung.

Hintergrund der Erfindung

Odometrie beschäftigt sich mit dem Problem, die Position und die Orientierung eines mobilen Körpers, etwa eines Fahrzeugs und/oder Roboters, zu bestimmen. Dazu kann der Bewegungszustand des Körpers bestimmt werden, z.B. eine Ge schwindigkeit und/oder eine Winkelgeschwindigkeit, denen Unterschiede zeitlich aufeinanderfolgender Positionen bzw. Orientierungen des Körpers entsprechen. Zur Bestimmung des Bewegungszustands können inertiale Messeinrichtungen, d.h. Beschleunigungssensoren und Gyroskope, eingesetzt werden, die den Be wegungszustand oder Komponenten davon direkt durch Messung bestimmen. Auch können am Körper angebrachte Kameras verwendet werden, deren Bild- bzw. Videoaufnahmen mittels geeigneter Bildverarbeitungssoftware eine Ein schätzung des Bewegungszustands ermöglichen.

Offenbarung der Erfindung

Erfindungsgemäß werden ein Verfahren zur Bestimmung eines Bewegungszu stands eines starren Körpers relativ zu einer Umgebung mittels einer Vielzahl von Messdatensätzen und ein Verfahren zur Bestimmung einer relativen Position und/oder einer relativen Orientierung sowie eine Recheneinheit und ein Compu terprogramm zu deren Durchführung mit den Merkmalen der unabhängigen Pa tentansprüche vorgeschlagen. Vorteilhafte Ausgestaltungen sind Gegenstand der Unteransprüche sowie der nachfolgenden Beschreibung.

Die Erfindung bedient sich der Maßnahme, Bedingungsmengen aus Messdaten sätzen und basierend darauf mittels einer Regressionsanalyse den Bewegungs zustand zu bestimmen. Durch die Bildung von Bedingungsmengen können ge zielt Messdatensätze ausgesucht werden, so dass eine hohe Genauigkeit der Bewegungszustand-Bestimmung erreicht werden kann.

Im Einzelnen verwendet das Verfahren eine Vielzahl von Messdatensätzen zu Objekten in der Umgebung des Körpers, wobei jeder Messdatensatz jeweils ei nen Messzeitpunkt und eine Dopplergeschwindigkeit und einen Azimuthwinkel bezüglich eines (dreidimensionalen) Sensorbezugssystems eines Sensors um fasst. Die Messdatensätze können mit einem oder mehreren Sensoren erfasst sein, wobei jeder Sensor ein Sensorbezugssystem besitzt, auf das sich die in den Messdatensätzen umfassten Messergebnisse beziehen. Die Messdatensät ze betreffen jeweils ein von den Sensoren erfasstes Objekt, d.h. jeder Messda tensatz zu einem Objekt umfasst einen Messzeitpunkt und eine Dopplerge schwindigkeit und einen Azimuthwinkel bezüglich eines Sensorbezugssystems des Sensors, der das Objekt erfasst. Die Messdatensätze können auf geeignete Weise bereitgestellt werden. Bevorzugt umfasst das Verfahren einen entspre chenden Bereitstellungsschritt.

Allgemein können für die Erfindung Messdatensätze von Sensoren verwendet werden, die eine Dopplergeschwindigkeit von Objekten bestimmen, d.h. die eine radiale Geschwindigkeit auf den Sensor zu bzw. von diesem weg bestimmen.

Man kann sie daher auch als Doppler-Sensoren bezeichnen. Bevorzugt handelt es sich bei den Sensoren um Radarsensoren und/oder Lidarsensoren. Insbeson dere ist die Verwendung von FMCW-Radarsensoren und/oder FMCW- Lidarsensoren vorgesehen (Radar: 'Radio Detection and Ranging'; Lidar: 'Light Detection and Ranging' bzw. 'Light Imaging, Detection and Ranging'; FMCW: 'Frequency Modulated Continuous Wave', auf Deutsch etwa 'frequenzmodulierte Dauerstrich-Detektion'). Die Messdatensätzen sind entsprechend vorzugsweise Radar-Messdatensätze und/oder Lidar-Messdatensätze.

Bevorzugt umfassen ein oder mehrere (insbesondere alle) der Messdatensätze einen Höhenwinkel (bezüglich des Sensorbezugssystems des jeweiligen Sen sors). Durch Berücksichtigung des Höhenwinkels kann die Genauigkeit der Be stimmung des Bewegungszustands erheblich erhöht werden. Weiter bevorzugt ist vorgesehen, zumindest einige dieser ein oder mehreren (insbesondere alle) Messdatensätze aus Messergebnissen zu bestimmen, die lediglich den Mess zeitpunkt, die Dopplergeschwindigkeit und den Azimuthwinkel umfassen, wobei der Höhenwinkel gleich einem vorbestimmten Wert, insbesondere gleich Null, gesetzt wird. Ältere Radarsensoren bzw. Lidarsensoren messen bzw. bestimmen oftmals keinen Höhenwinkel. Die entsprechenden Messergebnisse umfassen al so zunächst nur die Dopplergeschwindigkeit und den Azimuthwinkel. Um sowohl Messergebnisse mit als auch ohne Höhenwinkel verwenden zu können, wird da von ausgegangen, dass der Höhenwinkel einen vorbestimmten Wert, vorzugs weise Null, hat (der vorbestimmte Wert ist sensorbezogen, d.h. für verschiedene Messergebnisse desselben Sensors gleich). Indem der Höhenwinkel auf Null, bzw. allgemeiner auf einen vorbestimmten Wert, gesetzt wird, ist eine einheitliche Behandlung möglich. In der weiteren Beschreibung wird der Einfachheit halber lediglich auf den Fall eingegangen, dass die Sensoren den Höhenwinkel erfas sen. Falls die Sensoren dies nicht tun, soll, wie vorstehend ausgeführt, der Hö henwinkel gleich dem vorbestimmten Wert bzw. gleich Null sein, ohne dass dies gesondert erwähnt wird. Selbstverständlich können Messdatensätze, die aus Messergebnissen gewonnen wurden, in denen der Höhenwinkel auf den vorbe stimmten Wert gesetzt wurde, gemeinsam mit Messdatensätzen, in denen der Höhenwinkel bereits von vornherein enthalten ist, verwendet werden. Es ist auch möglich, Höhenwinkel, selbst wenn sie gemessen werden, gleich dem vorbe stimmten Wert, insbesondere gleich Null, zu setzen, z.B. um eine Vereinfachung der in der Regressionsanalyse verwendeten Gleichungen und damit eine kürzere Berechnungszeit zu erreichen.

Das Verfahren umfasst ferner ein Bestimmen des Bewegungszustands des Kör pers relativ zu der Umgebung als ein Geschwindigkeitsvektor und ein Winkelge- schwindigkeitsvektor in einem Körperbezugssystem, wobei jedes Sensorbezugs system durch eine nichtsinguläre Transformation in das Körperbezugssystem umrechenbar (d.h. abbildbar) ist.

Der Begriff 'Geschwindigkeitsvektor' bezieht sich auf die lineare Geschwindigkeit (im Unterschied zur Winkelgeschwindigkeit) und wird im Rahmen dieser Be schreibung auch als Linear-Geschwindigkeitsvektor bezeichnet. Der Linear- Geschwindigkeitsvektor und der Winkelgeschwindigkeitsvektor sind im dreidi mensionalen Körperbezugssystem angegeben. Da sie basierend auf (Doppler- Messungen zu Objekten in der Umgebung berechnet werden, geben sie eine Geschwindigkeit relativ zu den Objekten an. Die (stationären) Objekte definieren im Prinzip wiederum ein Umgebungsbezugssystem, so dass der Linear- Geschwindigkeitsvektor und der Winkelgeschwindigkeitsvektor den Bewegungs zustand (dargestellt im Körperbezugssystem) relativ zur Umgebung bzw. zum Umgebungsbezugssystem angeben.

Dadurch, dass die (dreidimensionalen) Sensorbezugssysteme mit dem (dreidi mensionalen) Körperbezugssystem durch jeweilige (eines für jedes Sensorbe zugssystem bzw. jeden Sensor) nicht-singuläre Transformationen (also insbe sondere auch umkehrbar) verbunden sind bzw. ineinander abbildbar sind, wird sichergestellt, dass die relative Orientierung und Lage der Sensoren (engl.: 'Sen sorpose') bei der Bestimmung des Bewegungszustand berücksichtigt wird. Etwa würden bei einer Projektion der per se dreidimensionalen Messdaten in eine Ebene Informationen verloren gehen und die Genauigkeit der Bestimmung des Bewegungszustands verringert. Die Sensorbezugssysteme und das Körperbe zugssystem weisen jeweils drei Dimensionen auf. Entsprechend weisen auch der Geschwindigkeitsvektor und der Winkelgeschwindigkeitsvektor je drei Dimensio nen bzw. Komponenten auf. Es handelt sich also um einen Bewegungszustand im dreidimensionalen Raum, der durch 3 + 3 = 6 Komponenten bestimmt ist.

Beim Bestimmen des Bewegungszustands wird wenigstens eine Bedingungs menge gebildet, die mehrere Messdatensätze umfasst, wobei in einer Regressi onsanalyse für die wenigstens eine Bedingungsmenge ein Funktional, das von Dopplergeschwindigkeits-Abweichungen zwischen geschätzten Dopplerge- schwindigkeiten und den Dopplergeschwindigkeiten der in der wenigstens einen Bedingungsmenge umfassten Messdatensätze abhängig ist, minimiert wird, wo bei die geschätzten Dopplergeschwindigkeiten in die Regressionsanalyse als ab hängige Variablen eingehen bzw. dargestellt werden (die von dem Azimuthwinkel und gegebenenfalls dem Höhenwinkel des jeweiligen Messdatensatzes abhängig sind), wobei wenigstens eine Komponente des Geschwindigkeitsvektors und/oder Winkelgeschwindigkeitsvektors durch die Regressionsanalyse bestimmt wird. Die abhängige Variablen darstellenden Dopplergeschwindigkeiten werden jeweils durch eine Abbildung, die von dem Azimuthwinkel und gegebenenfalls dem Höhenwinkel des jeweiligen Messdatensatzes und der jeweiligen Transfor mation zwischen Sensor- und Körperbezugssystem abhängig ist, mit dem Ge schwindigkeitsvektor und dem Wnkelgeschwindigkeitsvektor in Beziehung ge setzt.

Der Bewegungszustand des Körpers, d.h. der Geschwindigkeitsvektor und der Winkelgeschwindigkeitsvektor bzw. zumindest eine Komponente davon, werden in Rahmen der Regressionsanalyse berechnet. Die geschätzten Dopplerge schwindigkeiten sind vom Azimuthwinkel und gegebenenfalls Höhenwinkel des jeweiligen Messdatensatzes abhängig, wobei dies nicht so zu verstehen ist, das Azimuth- und Höhenwinkel in der Regressionsanalyse unabhängige Variablen sein müssen. Insbesondere können die geschätzten Dopplergeschwindigkeiten in einem in der Regressionsanalyse eingeschlossenen Regressionsverfahren bzw. einer eingeschlossenen Regression durch Gleichungen als Funktion des Azimuth- und Höhenwinkels des jeweiligen Messdatensatzes und als Funktion der in dem Regressionsverfahren bzw. der Regression zu bestimmenden Ge schwindigkeiten, d.h. dem Geschwindigkeitsvektor und dem Wnkelgeschwindig keitsvektor bzw. den Geschwindigkeitsvektoren der Sensoren, siehe die Glei chungssysteme (2), (5) weiter unten, dargestellt werden. Die zu bestimmenden Geschwindigkeiten stellen, wenn das Regressionsverfahren bzw. die Regression als Optimierungsproblem oder Variationsproblem aufgefasst wird, die zu variie renden Größen dar.

Bevorzugt werden alle (insgesamt sechs) Komponenten des Geschwindigkeits vektors und/oder Winkelgeschwindigkeitsvektors durch die Regressionsanalyse bestimmt (stellen also zu variierende Größen dar). Es ist allerdings auch möglich, dass eine oder mehrere Komponenten im Sinne einer Einschränkung festgelegt sind (etwa kann bei einem Schienenfahrzeug festgelegt sein, dass keine Quer bewegung stattfinden kann) oder durch ein von den Sensoren unabhängiges Messergebnis vorgegeben sind (etwa von einem Drehratensensor). In diesem Fall können die entsprechenden Komponenten durch die Einschränkung bzw. das unabhängige Messergebnis bestimmt sein und in der Regressionsanalyse nicht als zu variierende Größen verwendet werden. Vorzugsweise werden aller dings auch Komponenten, für die eine Einschränkung bzw. ein unabhängiges Messergebnis vorliegt, in der Regressionsanalyse berücksichtigt, also variiert, wobei zusätzliche Bedingungen, die der Einschränkung bzw. dem unabhängigen Messergebnis entsprechen, eingeschlossen werden. Diese zusätzlichen Bedin gungen werden mit einem Gewicht relativ zu den Messdatensätzen versehen, das ausdrückt, wie stark die Einschränkung bzw. das unabhängige Messergebnis berücksichtigt werden soll.

Der Begriff 'Bedingungen' soll sich darauf beziehen, dass diese im Sinne der Re gressionsanalyse, d.h. im Sinne des Funktionais, möglichst gut erfüllt werden sol len. 'Bedingungen' stellen also keine absoluten Bedingungen dar, die unbedingt genau erfüllt werden müssen.

Vorzugsweise sind der eine oder die mehreren Sensoren am Körper angebracht, so dass sie die Umgebung des Körpers erfassen. Bevorzugt werden die Messda tensätze mittels des einen oder der mehreren an dem Körper angebrachten Sen soren, die die Umgebung des Körpers erfassen, erfasst. Das Verfahren umfasst bevorzugt ein Durchführen von Messungen mittels des einen oder der mehreren Sensoren, um die Messdatensätze zu erfassen. Ein Erfassen der Messdatensät ze entsprechend dieser Ausführungen kann zumindest als Teil eines Bereitstel lungsschritts angesehen werden, der bevorzugt im Verfahren eingeschlossen ist.

Bevorzugt umfassen die Messdatensätze wenigstens zwei Messdatensätze des selben Sensors mit verschiedenen Messzeitpunkten. Die Erfassung mehrerer Messdatensätze mit verschiedenen Messzeitpunkten ermöglicht eine genauere Bestimmung des Bewegungszustands und im Besonderen auch die Bestimmun gen des Bewegungszustands für verschiedene (Bestimmungs-)Zeitpunkte.

Bevorzugt sind die Messzeitpunkte von Messdatensätzen mindestens zweier un terschiedlicher Sensoren voneinander verschieden. Auf diese Weise kann ver mieden werden, dass es zu Interferenzen zwischen verschiedenen Sensoren kommt, was zu unzuverlässigen Messdatensätzen führen würde.

Bevorzugt wird an einem Berechnungszeitpunkt der Bewegungszustand für ei nen Bestimmungszeitpunkt bestimmt; wobei die wenigstens eine Bedingungs menge aus den Messdatensätzen gebildet wird, deren Messzeitpunkte innerhalb eines Bestimmungszeitraums, in welchem zweckmäßigerweise auch der Be stimmungszeitpunkt liegt, liegen. Dadurch wird ermöglicht, für die Bestimmung des Bewegungszustands nur bestimmte Messdatensätze zu berücksichtigen, de ren Messzeitpunkt etwa in zeitlicher Nähe zum Bestimmungszeitpunkt liegt. Messdatensätze mit Messzeitpunkten außerhalb des Bestimmungszeitraums werden nicht in die Bedingungsmenge aufgenommen. Z.B. kann so verhindert werden, dass zeitlich weit entfernte Messdatensätze, während derer noch ein anderer Bewegungszustand vorlag, berücksichtigt werden.

Unter dem Begriff 'Berechnungszeitpunkt' ist der Zeitpunkt zu verstehen, an dem die jeweiligen Daten (Messdatensätze) bestimmt bzw. ausgewählt werden, basie rend auf denen die Bestimmung des Bewegungszustands durchgeführt wird. Es können bei der jeweiligen Bestimmung des Bewegungszustands nur Messdaten sätze berücksichtigt werden (d.h. in die jeweilige Bedingungsmenge aufgenom men werden), die einen Messzeitpunkt aufweisen, der vor dem Berechnungs zeitpunkt liegt oder gleich dem Berechnungszeitpunkt ist. Die Berechnung selbst dauert klarerweise einen Berechnungszeitraum (dessen Länge von der zur Ver fügung stehenden Rechenleistung abhängig ist). Der Berechnungszeitpunkt kann als Anfangszeitpunkt der Berechnung bzw. des Berechnungszeitraums angese hen werden.

Vorzugsweise werden in dem Funktional die Dopplergeschwindigkeits- Abweichungen mit einem positiven Zeitgewicht multipliziert, das eine Funktion der Zeitdifferenz zwischen dem Messzeitpunkt des jeweiligen Messdatensatzes und dem Bestimmungszeitpunkt, für den der Bewegungszustand bestimmt wird, ist. Weiter bevorzugt nimmt das Zeitgewicht mit zunehmendem Betrag der Zeit differenz ab. Auf diese Weise können, bei entsprechender Wahl der Gewichte, Messdatensätze, die zeitlich nahe beim Bestimmungszeitpunkt liegen und somit den zu bestimmenden Bewegungszustand besser charakterisieren, stärker in der Regressionsanalyse berücksichtigt werden als zeitlich weiter entfernt liegende Messdatensätze.

Bevorzugt wird der Bewegungszustand für mehrere aufeinanderfolgende Be stimmungszeitpunkte bestimmt, wobei jeder Bestimmungszeitraum der mehreren aufeinanderfolgenden Bestimmungszeitpunkte eine untere zeitliche Grenze und eine obere zeitliche Grenze aufweist, wobei die untere zeitliche Grenze gleich ei nem vorherigen Bestimmungszeitpunkt der mehreren aufeinanderfolgenden Be stimmungszeitpunkte ist und die obere zeitliche Grenze gleich einem nachfol genden Bestimmungszeitpunkt der mehreren aufeinanderfolgenden Bestim mungszeitpunkte oder dem jeweiligen Berechnungszeitpunkt ist. Bei mehreren aufeinanderfolgenden Bestimmungszeitpunkten werden somit, um eine hohe Genauigkeit zu erreichen, nur die Messdatensätze berücksichtigt, die klar dem Bestimmungszeitpunkt zugeordnet sind bzw. sich nicht jenseits eines vorherigen oder nachfolgenden Bestimmungszeitpunktes befinden.

Weiterhin erfolgen bevorzugt eine erste Bestimmung des Bewegungszustands für den Bestimmungszeitpunkt basierend auf einem ersten Bestimmungszeitraum und eine zweite Bestimmung des Bewegungszustands für den Bestimmungszeit punkt basierend auf einem zweiten Bestimmungszeitraum; wobei der zweite Be stimmungszeitraum verschieden vom ersten Bestimmungszeitraum ist und we nigstens einen Messzeitpunkt umfasst, der im ersten Bestimmungszeitraum nicht umfasst ist. Weiter bevorzugt erfolgt die erste Bestimmung an einem ersten Be rechnungszeitpunkt und die zweite Bestimmung an einem späteren zweiten Be rechnungszeitpunkt, wobei eine obere zeitliche Grenze des zweiten Bestim mungszeitraums zeitlich nach dem ersten Berechnungszeitpunkt liegt. So wird eine mehrmalige Bestimmung des Bewegungszustands für denselben Bestim mungszeitpunkt, wenn neue Messdatensätze vorliegen, ermöglicht. Die erste Bestimmung kann als erste Abschätzung angesehen werden, die zu einem frü hen Zeitpunkt erfolgt und die, wenn neue Messdatensätze vorliegen, zu einem späteren zweiten Berechnungszeitpunkt verbessert wird, um eine verbesserte Abschätzung, nämlich die zweite Bestimmung, zu erhalten.

Vorzugsweise werden in der Regressionsanalyse die geschätzten Dopplerge schwindigkeiten weiterhin unter Verwendung der Transformation zwischen dem jeweiligen Sensorbezugssystem und dem Körperbezugssystem als von dem Ge schwindigkeitsvektor und dem Winkelgeschwindigkeitsvektor abhängig darge stellt, und der Geschwindigkeitsvektor und der Winkelgeschwindigkeitsvektor werden so bestimmt, dass das Funktional minimiert wird. Durch diese Vorge hensweise werden alle Messdatensätze in der Regressionsanalyse gleichzeitig berücksichtigt, was zu einer hohen Genauigkeit führt.

Bevorzugt werden mehrere Bedingungsmengen gebildet, die jeweils Messdaten sätze nur eines einzelnen Sensors umfassen. Mehrere Sensor- Geschwindigkeitsvektoren werden in dem jeweiligen Sensorbezugssystem be stimmt, indem für jede der Bedingungsmengen in der Regressionsanalyse die geschätzten Dopplergeschwindigkeiten weiterhin als von dem Sensor- Geschwindigkeitsvektor abhängig dargestellt werden, und der Körpersensor- Geschwindigkeitsvektor wird so bestimmt, dass das jeweilige Funktional mini miert wird. In einer Regression werden der Geschwindigkeitsvektor und der Win- kelgeschwindigkeitsvektor bzw. eine oder mehrere Komponenten davon so be stimmt, dass ein Regressions-Funktional, das von Sensor-Abweichungen zwi schen den Sensor-Geschwindigkeitsvektoren und geschätzten Sensor- Geschwindigkeitsvektoren abhängig ist, minimiert wird, wobei die Sensor- Geschwindigkeitsvektoren unter Verwendung der Transformation zwischen dem Körperbezugssystem und dem jeweiligen Sensorbezugssystem als von dem Ge schwindigkeitsvektor und dem Winkelgeschwindigkeitsvektor abhängig darge stellt werden. Durch diese Vorgehensweise werden zunächst die Geschwindig keitsvektoren der Sensoren bestimmt und basierend darauf dann der Bewe gungszustand des Körpers bestimmt. Dieses zweistufige Vorgehen ist mit relativ wenig Rechenaufwand verbunden, da die zu erfüllenden Gleichungssysteme bzw. Bedingungsmengen relativ klein sind. Vorzugsweise umfasst jede der Bedingungsmengen nur Messdatensätze, die den gleichen Messzeitpunkt aufweisen; wobei gegebenenfalls in dem Regressi- ons-Funktional die Sensor-Abweichungen jeweils mit einem Gewicht multipliziert werden, das eine Funktion der Zeitdifferenz zwischen dem Messzeitpunkt der je weiligen Bedingungsmenge und dem Bestimmungszeitpunkt, für den der Bewe gungszustand bestimmt wird, ist. Dies ermöglicht die Berücksichtigung unter schiedlicher Messzeitpunkte.

Bevorzugt umfassen die Bedingungsmengen wenigstens eine Komponentenein schränkung an wenigstens eine Komponente des Geschwindigkeitsvektors und/oder des Winkelgeschwindigkeitsvektors in Form wenigstens eines Kompo nentenvorgabewerts. Das Funktional bzw. gegebenenfalls das Regressions- Funktional ist weiterhin von einer Komponenten-Abweichung zwischen dem we nigstens eines Komponentenvorgabewert und wenigstens einem geschätzten Komponentenvorgabewert abhängig. Die Komponenteneinschränkungen stellen von den Messdatensätzen unabhängige Bedingungen dar, so dass sie von Feh lern bzw. Ungenauigkeiten, die eine Vielzahl von Messdatensätze betreffen, nicht beeinträchtigt werden und so zu einer Verbesserung der Genauigkeit der Be stimmung des Bewegungszustands beitragen.

Vorzugsweise wird wenigstens eine Winkelgeschwindigkeitskomponente im Kör perbezugssystem als Komponentenvorgabewert verwendet, die insbesondere durch einen Drehratensensor bestimmt wird; wobei weiter bevorzugt gegebenen falls in dem Funktional bzw. dem Regressions-Funktional die wenigstens eine Komponenten-Abweichung mit einem Gewicht multipliziert wird, das eine Funkti on der Zeitdifferenz zwischen dem Messzeitpunkt des Drehratensensors und dem Bestimmungszeitpunkt, für den der Bewegungszustand bestimmt wird, ist. Drehratensensoren weisen typischerweise eine relativ hohe Messgenauigkeit auf, so dass durch Einbeziehen von entsprechend bestimmten bzw. gemessenen Winkelgeschwindigkeitskomponenten eine genaue Bestimmung des Bewe gungszustands verbessert werden kann. Bevorzugt umfassen die Messdatensätze wenigstens einen Zusatzparameter ausgewählt aus einer Entfernung des erfassten Objekts, einer Varianz der Ent fernung, einer Varianz des Azimuthwinkels, einer Varianz des Höhenwinkels, ei ner Varianz der Dopplergeschwindigkeit, einer Signalstärke des empfangenen Signals, einem Querschnitt (d.h. einen effektiven Querschnitt eines Objekts), ins besondere einen Radarquerschnitt oder Lidarquerschnitt, jeweils bezogen auf das erfasste Objekt des jeweiligen Messdatensatzes, und einem Typ des Sen sors, und gegebenenfalls einer Anordnung des Sensors am Körper. Die Doppler- geschwindigkeits-Abweichungen werden mit einem Zusatzgewicht multipliziert, das eine Funktion des jeweiligen Zusatzparameters ist. Alternativ oder zusätzlich wird ein Messdatensatz verworfen, wenn der wenigstens eine Zusatzparameter außerhalb wenigstens eines vorbestimmten Bereichs liegt. Auf diese Weise kön nen Messdatensätze, bei denen anhand der Zusatzparameter von einer hohen Verlässlichkeit ausgegangen werden kann, stärker berücksichtigt werden. Eben so können unzuverlässige Messdatensätze weniger stark oder gar nicht berück sichtigt werden. So kann eine höhere Genauigkeit der Bestimmung des Bewe gungszustands des Körpers erreicht werden.

Bevorzugt erfolgt die Regressionsanalyse unter Verwendung eines sog. Fehler- in-den-Variablen-Regressionsverfahrens, wobei die Azimuthwinkel und/oder ge gebenenfalls die Höhenwinkel optimiert werden. Dies ermöglicht eine weitere Er höhung der Genauigkeit der Bestimmung des Bewegungszustands des Körpers.

Bevorzugt erfolgt die Regressionsanalyse unter Verwendung eines iterativ- neugewichteten-kleinsten-Quadrat-Verfahrens. Eine ungenaue Bestimmung des Bewegungszustands bedingt durch Ausreißer in den Messdatensätzen, insbe sondere solche von dynamischen, nicht statischen Objekten in der Umgebung, kann so vermieden werden.

Ein erfindungsgemäßes Verfahren zur Bestimmung einer relativen Position und/oder einer relativen Orientierung eines starren Körpers umfasst ein Bestim men mehrerer Bewegungszustände des Körpers für mehrere aufeinander fol gende Bestimmungszeitpunkte gemäß der Erfindung und ein Integrieren der Be wegungszustände über die Zeit zwischen einem Anfangs-Bestimmungszeitpunkt der mehreren Bestimmungszeitpunkte und einem End-Bestimmungszeitpunkt der mehreren Bestimmungszeitpunkte, um die relative Position und/oder die relative Orientierung als Ergebnisse dieser Integration zu erhalten.

Eine erfindungsgemäße Recheneinheit, z.B. ein Steuergerät eines Kraftfahr zeugs oder Roboters, ist, insbesondere programmtechnisch, dazu eingerichtet, eine Vielzahl von Messdatensätzen zu erhalten und ein erfindungsgemäßes Ver fahren mit den vorstehenden Aspekten durchzuführen.

Ein erfindungsgemäßes Fahrzeug und/oder ein erfindungsgemäßer Roboter, insbesondere ein Land-, Luft- oder Wasserfahrzeug und/oder ein Land-, Luft- o- der Wasserroboter, weist eine erfindungsgemäße Recheneinheit und einen oder mehrere an einem Körper des Fahrzeugs und/oder Roboters angebrachte Sen soren, die eine Umgebung des Körpers erfassen und die dazu eingerichtet sind, Messungen zu Objekten in der Umgebung durchzuführen und dabei erfasste Messdatensätze an die Recheneinheit zu senden, auf. Bevorzugt sind die Senso ren Radarsensoren und/oder Lidarsensoren.

Auch die Implementierung eines erfindungsgemäßen Verfahrens in Form eines Computerprogramms oder Computerprogrammprodukts mit Programmcode zur Durchführung aller Verfahrensschritte, insbesondere derjenigen, die die Regres sionsanalyse und das Bilden der Bedingungsmenge betreffen, ist vorteilhaft, da dies besonders geringe Kosten verursacht, insbesondere wenn ein ausführendes Steuergerät noch für weitere Aufgaben genutzt wird und daher ohnehin vorhan den ist. Geeignete Datenträger zur Bereitstellung des Computerprogramms sind insbesondere magnetische, optische und elektrische Speicher, wie z.B. Festplat ten, Flash-Speicher, EEPROMs, DVDs u.a.m. Auch ein Download eines Pro gramms über Computernetze (Internet, Intranet usw.) ist möglich.

Weitere Vorteile und Ausgestaltungen der Erfindung ergeben sich aus der Be schreibung und der beiliegenden Zeichnung. Die Erfindung ist anhand von Ausführungsbeispielen in der Zeichnung schema tisch dargestellt und wird im Folgenden unter Bezugnahme auf die Zeichnung beschrieben.

Kurze Beschreibung der Zeichnungen

Figur 1 zeigt eine Draufsicht eines Fahrzeugs dar, an dem Sensoren angebracht sind;

Figur 2 zeigt ein Bezugssystem eines Sensors, der ein Objekt erfasst;

Figur 3 zeigt ein Diagramm mit mehreren Bestimmungs- und Messzeitpunkten; und

Figur 4 zeigt ein Ablaufdiagramm gemäß einer bevorzugten Ausführungsform der Erfindung.

Ausführungsform(en) der Erfindung

Figur 1 stellt als Beispiel für einen starren Körper in einer Draufsicht ein Kraft fahrzeug 2 dar, an dem Sensoren S1, S2, S3, S4 angebracht sind (z.B. Radar sensoren und/oder Lidarsensoren). Auch andere Fahrzeuge, z.B. Luftfahrzeuge und Wasserfahrzeuge, sind als starre Körper denkbar. Ebenso sind Roboter möglich. Insbesondere kann das Fahrzeug automatisiert sein (Roboterfahrzeug). Allgemein kann die Erfindung auch bei nicht selbsttätig angetriebenen starren Körpern angewendet werden. Das Kraftfahrzeug (allgemeiner der Körper) defi niert ein Körperbezugssystem B mit drei zueinander orthogonalen Achsen, d.h. mit einer x-Achse, einer y-Achse und einer z-Achse, die sich in der Draufsicht senkrecht zur Zeichenebene aus dieser erstreckt. Der Ursprung UB des Kör perbezugssystems B wird für ein Kraftfahrzeug bzw. für ein Fahrzeug vorzugs weise in dem Drehzentrum des Fahrzeugs platziert. Bei einem Kraftfahrzeug mit Ackermann-Lenkung ist dies die Mitte der Hinterachse, wie gezeigt. Der starre Körper, d.h. das Kraftfahrzeug 2, bewegt sich in einer Umgebung, in der sich Objekte 4, z.B. Bäume, Gebäude, andere Kraftfahrzeuge, befinden. Da bei können statische Objekte, d.h. sich in der Umgebung nicht bewegende Ob jekte, und sich bewegende Objekte vorhanden sein. Die statischen Objekte be finden sich an festen Positionen in der Umgebung bzw. in einem Umgebungsbe zugssystem.

Der Körper (Kraftfahrzeug 2) bewegt sich in der Umgebung. Diese Bewegung, d.h. der Bewegungszustand des Körpers in dem Umgebungsbezugssystem ist im Körperbezugssystem B durch einen linearen Geschwindigkeitsvektor (Vx, Vy, Vz) und einen Winkelgeschwindigkeitsvektor (wc, ooy, wz) darstellbar, wobei die Komponenten des Geschwindigkeitsvektors die Geschwindigkeiten des Körpers in dem Umgebungsbezugssystem, aber entlang der jeweiligen Achsen x, y, z des Körperbezugssystems darstellen und die Komponenten des Wnkelgeschwindig- keitsvektors die Wnkelgeschwindigkeiten des Körpers in dem Umgebungsbe zugssystem, aber um die jeweiligen Achsen x, y, z des Körperbezugssystems darstellen. In der Ansicht der Figur 1 sind nur Vx, Vy und wz eingezeichnet.

An bestimmten Positionen am Körper, hier an den vier Ecken des Kraftfahrzeugs 2, sind die Sensoren S1, S2, S3, S4 angebracht. Auch eine andere Anzahl von Sensoren ist möglich, bevorzugt liegt die Anzahl der Sensoren im Bereich von 1 bis 10, weiter bevorzugt im Bereich von 3 bis 6. Anders als dargestellt können Sensoren nicht nur an Ecken angebracht sein, sondern auch an beliebigen ande ren Stellen des starren Körpers.

Jeder der Sensoren definiert ein Sensorbezugssystem. Stellvertretend ist in Figur 1 für einen der Sensoren S1 ein Sensorbezugssystem S eingezeichnet, das zu einander orthogonale Achsen, d.h. eine x-Achse xs, eine y-Achse ys und eine z- Achse zs, aufweist. Die z-Achse ist in der Draufsicht nicht eingezeichnet und kann sich, muss sich aber nicht, senkrecht zur Zeichenebene bzw. parallel zur z- Achse des Körperbezugssystems erstrecken.

Das Sensorbezugssystem jedes der Sensoren ist mittels einer linearen Trans formation mit dem Körperbezugssystem verbunden, d.h. durch die Transformati- on in dieses abbildbar. Vektoren/Koordinaten in jedem der Sensorbezugssyste me können also durch die jeweilige Transformation in entsprechende Vekto ren/Koordinaten im Körperbezugssystem abgebildet werden. Die Transformation für jedes der Sensorbezugssysteme ist jeweils durch Kombination einer Transla tion t (charakterisiert durch einen dreidimensionalen Translationsvektor zwischen dem Ursprung UB des Körperbezugssystems und einem Ursprung Us des Sen sorbezugssystems) und einer Rotation R (etwa charakterisiert durch Eulerwinkel) gegeben (wieder stellvertretend für einen der Sensoren eingezeichnet). Es han delt sich also um eine Transformation zwischen zwei dreidimensionalen Räumen. Diese sollte nicht-singulär sein.

Weiterhin umfasst das Fahrzeug der Figur 1 bevorzugt eine Recheneinheit 12 (z.B. ein Steuergerät), die dazu eingerichtet ist, von den Sensoren Messdaten sätze zu erhalten und basierend darauf einen Bewegungszustand zu bestimmen, wobei die Recheneinheit dazu eingerichtet ist, eine Regressionsanalyse, insbe sondere wie nachfolgend genauer beschrieben, durchzuführen.

Figur 2 stellt ein Bezugssystem S eines Sensors (eines Radarsensors und/oder eines Lidarsensores), d.h. ein Sensorbezugssystem, dar, der ein Objekt 4 er fasst. Das Sensorbezugssystem S weist eine x-Achse xs, eine y-Achse ys und eine z-Achse zs auf, die zueinander orthogonal sind. Eine schematisch gezeich nete Sensorfläche 6 des Sensor ist hier beispielhaft in der y-z-Ebene angeordnet und erfasst als Sichtfeld den Halbraum mit Koordinaten xs > 0. Selbstverständlich können die Sensoren auch ein anderes Sichtfeld aufweisen (als einen Halb raum). Der Sensor ist bzw. für die Erfindung verwendeten Sensoren sind dazu eingerichtet, unter Ausnutzung des Dopplereffekts eine als Dopplergeschwindig keit bezeichnete Geschwindigkeit eines erfassten Objekts auf den Sensor zu bzw. vom Sensor weg zu bestimmen, d.h. es werden Doppler-Sensoren verwen det. Bevorzugt handelt es sich bei den Sensoren um Radarsensoren bzw. Dopp- lerradar-Sensoren und/oder Lidar-Sensoren.

Die Position des Objekts 4 ist durch einen Objekt-Positionsvektor P, der sich vom Ursprung Us des Sensorbezugssystems zum Objekt 4 erstreckt gegeben. Dieser kann im Sensorbezugssystem durch Kugelkoordinaten (q, f, r) angeben werden. Der Radius r = \P \z ist die Länge des Objekt-Positionsvektors P, d.h. der Abstand zwischen dem Ursprungs Us und dem Objekt 4. Der Azimuthwinkel Q ist der Win kel zwischen der x-Achse xs und der Projektion 8 des Objekt-Positionsvektor P in die x-y-Ebene. Der Höhenwinkel f ist der Wnkel zwischen der x-y-Ebene und dem Objekt-Positionsvektor P. Entsprechend gilt:

P = (x _s ,y _s ,z _s ) = (rcos#cos^,rsin#cos^,rsin^)

Das Objekt 4 weist eine Geschwindigkeit, charakterisiert durch einen Objekt- Geschwindigkeitsvektor Vo, relativ zum Sensor bzw. im Sensorbezugssystem auf. Die Geschwindigkeit des Sensors relativ zum Objekt ist entsprechend durch V _s = - Vo gegeben. Die Dopplergeschwindigkeit d ist durch die Länge der Projek tion des Objekt-Geschwindigkeitsvektors Vo auf den Objekt-Positionsvektor P. Diese Länge ist gleich dem Skalarprodukt d = p ⁰ · Vo zwischen dem Objekt- Geschwindigkeitsvektors Vo und einem Einheitsvektor p° in Richtung Objekt- Positionsvektor P, wobei p° = /r ist. Insgesamt kann somit die Dopplergeschwin digkeit d durch den Geschwindigkeitsvektor V _s = (Vs _x , Vs _y , Vs _z ) des Sensors rela tiv zum Objekt ausgedrückt werden:

Der Sensor erfasst an einem einzelnen Messzeitpunkt eine Vielzahl von Messda tensätzen (z.B. 50 bis 150 Messdatensätze), so dass mit jeder Messung des Sensors eine Vielzahl von Dopplergeschwindigkeiten und entsprechenden Azi- muth- und Höhenwinkeln ermittelt wird. Die statischen Objekte definieren die Umgebung, relativ zu der die Geschwindigkeit des Sensors und weitergehend des starren Körpers mittels der gemessenen Dopplergeschwindigkeiten bestimmt werden kann. Um die dabei relevanten Gleichungen trotz der hohen Anzahl von Messdatensät zen in übersichtlicher Form darstellen zu können, wird im Folgenden eine geeig nete Notation eingeführt.

Vektoren, Matrizen und Skalare: Vektoren und Matrizen werden in Fettdruck dar gestellt, zur einfacheren Unterscheidung von Skalaren.

Punkte im dreidimensionalen (3D) Raum: Ein Punkt p, der in kartesischen Koor dinaten bezüglich eines Koordinatensystems bzw. Bezugssystems A ausge drückt werden soll, wird als p geschrieben.

Vektoren zwischen zwei Punkten im 3D-Raum: Ein Translationsvektor zwischen zwei Punkten B und C bezüglich eines Bezugssystems A wird als A*B,C geschrie ben.

Geschwindigkeiten von Bezugssystems im 3D-Raum: Wenn sich ein Bezugssys tem C relativ zu einem Bezugssystem B mit einer bestimmten linearen oder Win kelgeschwindigkeit bewegt, wird die jeweilige Geschwindigkeit bezüglich des Be zugssystems A für lineare Geschwindigkeiten geschrieben als:

A ^V B,C und für Winkelgeschwindigkeiten geschrieben als:

A ^m B,C

Drehungen und Transformationen des starren Körpers: Eine Transformation zwi schen zwei Bezugssystem A und B wird geschrieben als: A,B

Eine solche Transformation transformiert im Bezugssystem B ausgedrückte Punkte in das Bezugssystem A:

T'A,B BV ⁼ AP

Da Transformationen eine Drehung und eine Translation umfassen, kann dies auch geschrieben werden als: ^A,B BP + A ΪA,B ⁼ AP wobei KA,B die entsprechende Dreh- bzw. Rotationsmatrix ist. 7A, B kann als Lage und Orientierung eines Bezugssystems relativ zum anderen angesehen werden; im Englischen auch als 'Pose' bezeichnet.

Das Bezugssystem des starren Körpers, d.h. das Körperbezugssystem, wird mit dem Buchstaben 'B' bezeichnet. Das Bezugssystem eines Sensors, d.h. ein Sensorbezugssystem, wird mit 'S', 'S1 ', 'S2', ..., 'Si', ..., 'Sl' bezeichnet, wobei der Einfachheit halber auch die Sensoren selbst teilweise mit S, S1, S2, ..., Si, ..., Sl bezeichnet werden. Das Bezugssystem der Umgebung wird mit dem Buchstaben W bezeichnet.

In kartesischen Koordinaten bzw. Polarkoordinaten ausgedrückt ist unter Ver wendung dieser Notation die Position eines Objekts im Bezugsystem Si eines Sensors: siPj ⁼ ( ^x y ^Z ) ^{T = r} ( ^cos @ ^cos F ^s i ⁿ Q cos f , sin f ) ^T wobei 'j' einen (der Vielzahl) Messdatensatz (z.B. Ergebnisse einer Radarmes sung oder Lidarmessung) eines Objekts bezeichnet. Wenn sich ein Sensor Si mit einer Geschwindigkeit siVw.si relativ zur Umgebung W bewegt, in der das Objekt ein statisches Objekt ist, gilt wie oben beschrieben die Gleichung (1): wobei (cos Q cos f , sin Q cos f , sin f ) ^T die normalisierten Punkt-Koordinaten des Objekts sind, d.h. der Einheitsvektor in Richtung des Objekts (| | ₂ bezeichnet die übliche 2-Norm, d.h. die Länge des Vektors).

Aus mehreren Messdatensätzen mehrerer Dopplergeschwindigkeiten d _j , mit j aus 1 , 2, ..., J (J ist die Anzahl der verwendeten Messdatensätze, hier können an ei nem Messzeitpunkt mehrere Messdatensätze eines einzelnen Objekts und/oder Messdatensätze verschiedener Objekte umfasst sein), ergibt sich das folgende Gleichungssystem (2):

Falls Messdatensätze keinen Höhenwinkel f umfassen, werden in diesem Glei chungssystem die entsprechenden Höhenwinkel f, gleich einem vorbestimmten Winkel angenommen bzw. gesetzt. Vorzugsweise werden diese Höhenwinkel gleich Null gesetzt (f, = 0 für Messdatensätze j, für die kein Höhenwinkel vor liegt). Dies führt zu einer Vereinfachung des Gleichungssystems, da Kosinus und Sinus dieser Höhenwinkel entsprechend 1 bzw. 0 sind. In vereinfachter Schreib weise kann das Gleichungssystem (2) geschrieben werden als: wobei A und D als Bezeichnung für die jeweiligen Vektoren im Gleichungssystem verwendet wird; diese sind durch Messwerte der Sensoren gegeben.

A ergibt sich aus dem Azimuthwinkel Q und dem Höhenwinkel f eines Messda tensatzes und D ergibt sich aus den entsprechenden gemessenen Dopplerge schwindigkeiten di.

Wenn also ein Vielzahl von Messdatensätzen eines Sensors Si vorhanden ist, kann durch Lösen dieser Gleichungen die Geschwindigkeit siVw.si des Sensors geschätzt bzw. bestimmt werden, wobei die Gleichungen im Allgemeinen ein überbestimmtes Gleichungssystem bilden. Die Messdatensätze können als im Sinne des obigen Gleichungssystems zu erfüllende Bedingungen an die Sensor geschwindigkeit angesehen werden. Die Menge von Messdatensätzen bzw. zu mindest ein Teil davon stellt also eine Bedingungsmenge dar.

Das obige Gleichungssystem (1) kann mittels einer Regression gelöst werden, wobei die Sensor-Geschwindigkeit siWv.si so bestimmt bzw. variiert wird, dass ein Fehlermaß, d.h. ein Regressionsfunktional F, minimiert wird, das von einer Diffe- renz (Dopplergeschwindigkeits-Abweichung (d- d _j )) zwischen den gemessenen Dopplergeschwindigkeiten d _j und geschätzten Dopplergeschwindigkeiten d _j , die durch die linke Seite des Gleichungssystems für eine jeweilige Sensor- Geschwindigkeit gegeben sind, abhängig ist. Beispielsweise könnte ein mittlerer quadratischer Fehler minimiert werden:

F = F[(d ₁ -d ₁ )„. ,(d,-d,),. ,(d,-d,)] ~£(d,-d,)·

Selbstverständlich sind auch andere funktionale Abhängigkeiten als die quadrati sche möglich.

Zusätzlich können hier Gewichte g _} vorgesehen sein, die von Zusatzparametern, die von dem Sensor bei einer Messung (z.B. Radarmessung oder Lidarmessung) zusätzlich erfasst werden, abhängig sind. Die Gewichte g _} können auch als Pa rametergewichte bezeichnet werden. Im Funktional wird dann die Differenz zwi schen den gemessenen Dopplergeschwindigkeiten d _j und geschätzten Doppler geschwindigkeiten d _j mit dem entsprechenden Gewicht g _} multipliziert. Es ergibt sich:

F = ^[gi (d _j - d, ),..., g ^ (d ^ - d ^ ),..., g _j (d ₇ d,)]~£g^-d ) ²

Beispielsweise könnte ein Messdatensatz, in dem der Azimuth- und/oder Hö henwinkel eine hohe Unsicherheit bzw. Varianz aufweist, relativ zu Messdaten sätzen, in denen ein geringe Varianz vorhanden ist, mit einem geringeren Ge wicht versehen sein.

In Abhängigkeit von Zusatzparametern können auch einzelne Messdatensätze unberücksichtigt bleiben, d.h. nicht in das obige Gleichungssystem (2) einge schlossen werden, beispielsweise wenn ein Abstand zu einem Objekt über einer vorgegebenen Schwelle liegt. Einen ähnlichen Effekt hat es, das entsprechend (Parameter-)Gewicht g _} gleich Null zu setzen.

Im Allgemeinen werden die Messdatensätze fehlerbehaftet sein, insbesondere werden im Allgemeinen nicht alle erfassten Objekte statische Objekte sein, so dass in den Messdatensätzen Ausreißer vorhanden sein werden, die die Be- Stimmung der Sensorgeschwindigkeit verfälschen. Um diesen Effekt zu minimie ren, wird vorzugsweise ein robustes Verfahren zur Regression verwendet. Insbe sondere kann eine Methode der iterativ neugewichteten kleinsten Quadrate (IRLS, engl.: 'iteratively reweighted least squares') verwendet werden. Solche IRLS-Methoden sind dem Fachmann an sich bekannt.

Zur Bestimmung des linearen Geschwindigkeitsvektors _B W _V.B und des Winkelge schwindigkeitsvektors _B W _W.B des starren Körpers wird die Geschwindigkeit des Sensors mit den (Linear- und Winkel-) Geschwindigkeitsvektoren des Körpers in Beziehung gesetzt, wobei die relative Drehung RB,S und Translation _B * _B, si der fest am Körper angebrachten Sensoren verwendet wird, Gleichung (3):

Hier bezeichnet 'c' das übliche Kreuzprodukt zweier dreidimensionaler Vektoren.

Unter Verwendung der Schreibweise kann diese Gleichung wie folgt geschrieben werden:

Die Geschwindigkeit eines Sensors kann also durch einen Vektor, der die Linear und Winkel-Geschwindigkeitsvektoren des starren Körpers umfasst, ausgedrückt werden.

Für mehrere Sensoren 1, 2, ..., I können die entsprechenden Gleichungen zu sammengefasst geschrieben werden als Gleichungssystem (4): wobei in der Matrix M die einzelnen Transformationen (relative Drehungen und Translationen) der verschiedenen Sensoren zusammengefasst sind. Da die Matrix M bekannt ist, ergibt sich ein Gleichungssystem, aus dem die Geschwindigkeitsvektoren BV _W .B und BW _W .B des Körpers bestimmt werden können, wobei das Gleichungssystem bei einer ausreichend hohen Anzahl von Sensoren im Allgemeinen überbestimmt ist.

Eine Möglichkeit, den Bewegungszustand des Körpers zu bestimmen, d.h. den Linear-GeschwindigkeitsvektorBVw.B und den Winkelgeschwindigkeitsvektor BOÜW.B ZU bestimmen, besteht darin, zunächst die Geschwindigkeiten siVw.si der Sensoren wie weiter oben erläutert zu bestimmen, d.h. für jeden der Sensoren das obige Gleichungssystem (2) mittels eines Regressionsverfahrens zu lösen, und anschließend das vorstehende Gleichungssystem (4) zu lösen, wobei wieder eine Regression (z.B. die Methode der kleinsten Quadrate) verwendet wird. Insgesamt stellt dieser zweistufige Prozess eine Regressionsanalyse dar. Diese zweistufige Vorgehensweise kann als schwach gekoppelter Ansatz bezeichnet werden und hat den Vorteil, dass sie relativ schnell, d.h. im Vergleich zu den weiteren Ansätzen mit relativ wenig Rechenleistung, durchgeführt werden kann.

Eine weitere Vorgehensweise, der sogenannte stark gekoppelte Ansatz, besteht darin, die Linear-GeschwindigkeitsvektorBVw.B und den Winkelgeschwindigkeitsvektor BW _W .B mittels eines Regressionsverfahrens, das eine Regressionsanalyse oder zumindest einen Teil davon darstellt, direkt aus den Dopplergeschwindigkeiten zu bestimmen, d.h. ohne vorherige Bestimmung der Sensor- Geschwindigkeiten aus den Dopplergeschwindigkeiten. Der stark gekoppelte Ansatz führt im Allgemeinen zu einer genaueren Bestimmung des Bewegungszustands.

Dazu werden die obige Gleichung (1) für die Dopplergeschwindigkeit und die obige Gleichung (3), die die Sensor-Geschwindigkeit mit den Geschwindigkeitsvektoren des starren Körpers kombiniert: Bei einer Vielzahl von Messdatensätzen 1,..., J können diese Gleichungen zu folgendem Gleichungssystem (5) zusammengefasst werden:

Hier kann die Vielzahl von Messdatensätzen von jedem von mehreren Sensoren jeweils mehrere Messdatensätze umfassen. Beispielsweise könnten, wenn etwa 4 Sensoren vorgesehen sind, von jedem Sensor je 50 bis 150 Messdatensätze in das Gleichungssystem eingeschlossen sein, insgesamt also 200 bis 600 Mess datensätze. Der Index 'Si' wird hier in einem generischen Sinn verwendet, d.h. es ist in jeder Zeile des Gleichungssystems der jeweilige Sensor bzw. dessen Sen sorbezugssystem zu verstehen, dem das Messdatensatz zugeordnet ist. Es kön nen also Messdatensätze verschiedener Sensoren im Gleichungssystem umfasst sein.

Dieses Gleichungssystem kann mittels eines Regressionsverfahrens gelöst wer den. Es ist also wie oben ein von den Differenzen bzw. Dopplergeschwindigkeits- Abweichungen (d- d _j ) zwischen den gemessenen Dopplergeschwindigkeiten d _j und geschätzten Dopplergeschwindigkeiten d _j abhängiges Funktional F vorge geben, das durch die Lösung minimiert wird (hier wieder beispielhaft mit quadra tischen Abständen):

Auch hier können verschiedene Messdatensätze basierend auf Zusatzparame tern der Messdatensätze mit Gewichten bzw. Parametergewichten g unter schiedlich gewichtet werden:

Auch können einzelne Messdatensätze basierend auf Zusatzparametern im Glei chungssystem (5) unberücksichtigt bleiben.

Der Ausdruck 'geschätzte Dopplergeschwindigkeiten' bezieht sich hier, wie auch oben, darauf, das diese für (im Rahmen des Regressionsverfahrens bzw. der Optimierung geschätzten) eine gegebenen Linear-GeschwindigkeitsvektorBVw.B und einen gegebenen Winkelgeschwindigkeitsvektor _B W _W.B durch die Zeilen der linken Seite des vorstehenden Gleichungssystem (5) berechnet werden können. Im Regressionsverfahren werden dann ein Linear-GeschwindigkeitsvektorBVw.B und ein Winkelgeschwindigkeitsvektor _B W _W.B bestimmt, so dass das jeweilige Funktional optimiert bzw. minimiert wird. Dies stellt eine Bestimmung bzw. Schätzung des Bewegungszustands des Körpers dar.

Figur 3 stellt in einem Diagramm die zeitliche Abfolge mehrerer Bestimmungs und Messzeitpunkte dar. In dem Diagramm sind entlang einer Zeitachse t mehre re Bestimmungszeitpunkte To, Ti, T3 aufgetragen, die Zeitpunkte darstellen, für die der Bewegungszustand des starren Körpers bestimmt bzw. geschätzt werden soll. Der Bestimmungszeitpunkt ist im Allgemeinen nicht gleich einem Berech nungszeitpunkt, an dem die Bestimmung bzw. Berechnung des Bewegungszu stands erfolgt.

Die entsprechenden Bewegungszustände sind mit Zo = Z(To), Zi= Z(Ti), Z2 =

Z(T 2) bezeichnet. Ein Bewegungszustand Z(7 ) zu einem Zeitpunkt T _k ist durch den linearen Geschwindigkeitsvektor (T _k ) und den Winkelgeschwindigkeits vektor (T _k ) des starren Körpers zum jeweiligen Zeitpunkt T _k charakterisiert.

Die Bestimmungszeitpunkte können, wie gezeigt, regelmäßig voneinander beab- standet sein, wobei ein Zeitabstand DT zwischen zwei aufeinanderfolgenden Be stimmungszeitpunkten gegeben ist als DT = 7* _+i - T _k . Das Inverse 1/DT des Zeit abstands DT kann als Bestimmungsfrequenz angesehen werden. Allgemeiner sind jedoch auch unregelmäßig beabstandete Bestimmungszeitpunkte denkbar. Weiterhin sind in der Figur mehrere Messzeitpunkte eingezeichnet, d.h. Zeitpunk te an denen wenigstens ein Sensor Objekte erfasst. An jedem Messzeitpunkt wird der Sensor eine Vielzahl einzelner Messdatensätze bestimmen, die jeweils zumindest eine Dopplergeschwindigkeit, einen Azimuthwinkel und einen Höhen winkel eines erfassten Objekts umfassen. Zusätzlich kann jedes der Messdaten sätze Zusatzparameter umfassen.

Jedes der Messdatensätze umfasst neben der Dopplergeschwindigkeit, dem Azimuthwinkel und dem Höhenwinkel einen Messzeitpunkt, an dem diese ge messen wurden. Gezeigt sind beispielsweise Messzeitpunkte ti, to, ti, , fe, U, ts für zwei Sensoren S1 (Messzeitpunkte ti, ti, fc, t ₄ ), S2 (Messzeitpunkte to, k, ts), wobei hier t-i zufällig mit To zusammenfällt. Die Messzeitpunkte der Sensoren können unabhängig voneinander und/oder unabhängig von den Bestimmungs zeitpunkten sein. Auch können gegebenenfalls Messfrequenzen verschiedener Sensoren unterschiedlich sein und/oder verschieden von der Bestimmungsfre quenz sein. In der Figur ist z.B. die Messfrequenz des Sensors S2 gleich der Be stimmungsfrequenz, während der Sensor S1 eine höhere Messfrequenz auf weist. Bevorzugt sind die Messzeitpunkte verschiedener Sensoren voneinander verschieden, um Interferenzen zu vermeiden.

Eine Bestimmungsfrequenz bzw. eine durchschnittliche Anzahl von Bestim mungszeitpunkten je Sekunde kann beispielsweise im Bereich von 5 - 10 Hz lie gen. Die Messfrequenz eines Sensors bzw. eine durchschnittliche Anzahl von Messzeitpunkten des Sensors je Sekunde kann beispielsweise im Bereich von 10 - 20 Hz liegen. Die Messfrequenzen (bzw. durchschnittliche Anzahlen von Messzeitpunkten je Sekunde) der Sensoren sind bevorzugt größer als die Be stimmungsfrequenz (bzw. eine durchschnittliche Anzahl von Bestimmungszeit punkten je Sekunde).

Die Bestimmung bzw. Schätzung des Bewegungszustands erfolgt im Prinzip durch ein Regressionsverfahren bzw. eine Regression wie vorstehend beschrie ben, d.h. entsprechend dem schwach gekoppelten Ansatz oder dem stark ge koppelten Ansatz. Dabei werden Messdatensätze mit Messzeitpunkten einge schlossen, die in zeitlicher Nähe zum Bestimmungszeitpunkt, insbesondere in- nerhalb bestimmter zeitlicher Grenzen um den Bestimmungszeitpunkt, erfolgt sind. Z.B. können alle Messdatensätze eingeschlossen werden, die innerhalb ei nes vorgegebenen zeitlichen Abstands dT erfolgt sind, also: \t _} - T _k \ < dT; hier sind auch unsymmetrische Grenzen mit einem unteren zeitlichen Abstand d _u T und einem oberen zeitlichen Abstand d ₀ T möglich: -d _u T < t _} - T _k < d ₀ T. Eine weite re Möglichkeit besteht darin, alle Messdatensätze mit Messzeitpunkten t _} einzu schließen, die zwischen dem vorherigen Bestimmungszeitpunkt T _k. 1 und dem nachfolgenden Bestimmungszeitpunkt 7* _+i liegen: T _k. 1 < t _} < 7* _+i bzw., ausge drückt als oberer/unterer zeitlicher Abstand, d ₀ T = 7* _+i - T _k und d _u T = T _k - T _k. 1. Bei regelmäßig beabstandeten Bestimmungszeitpunkten gilt: d ₀ T= d _u T= DT.

Allgemein formuliert werden also Messdatensätze mit Messzeitpunkten innerhalb eines Bestimmungszeitraums in die Bestimmung des Bewegungszustands für den Bestimmungszeitpunkt T _k eingeschlossen. Der Bestimmungszeitraum ist durch eine untere und einer obere zeitliche Grenze bestimmt, z.B. kann, entspre chend der vorstehenden Ausführung, das Intervall [ T _k - d _u T ; T _k + d ₀ T\ ein Bestim mungszeitraum sein. Vorzugsweise liegt der Bestimmungszeitpunkt T _k im Be stimmungszeitraum. Allgemein ist auch denkbar, dass der Bestimmungszeitpunkt T _k außerhalb des Bestimmungszeitraums liegt, d.h. d _u T < 0 oder d ₀ T < 0 im vor stehenden Beispiel.

Die Berechnung des Bewegungszustands für einen Bestimmungszeitpunkt er folgt an mindestens einem Berechnungszeitpunkt, der im Allgemeinen verschie den vom Bestimmungszeitpunkt ist, für den die Bestimmung des Bewegungszu stands erfolgt, um auch Messdatensätze mit Messzeitpunkten nach dem Be stimmungszeitpunkt einschließen zu können. In der Figur sind beispielhaft zwei Berechnungszeitpunkte li, Kz, an denen der Bewegungszustand Zi für den Be stimmungszeitpunkt Ti bestimmt werden kann, eingezeichnet, wobei der erste Berechnungszeitpunkt li zwischen zwei Messzeitpunkten tz, t3 liegt und der zwei te Berechnungszeitpunkt Kz nach dem Messzeitpunkt tz liegt, hier beispielhaft zwischen dem Messzeitpunkt tz und dem Bestimmungszeitpunkt Tz. Er könnte aber ebenso mit diesem zusammenfallen. Für die vorstehend beschriebenen zeitlichen Grenzen um den Bestimmungszeitpunkt stellt der Berechnungszeit punkt eine obere Grenze dar, d.h. die Menge der zu berücksichtigenden Messda- tensätze wird dahingehend eingeschränkt, dass nur diejenigen berücksichtigt werden, die zusätzlich die Bedingung t _} < erfüllen (A _b steht für einen Berech nungszeitpunkt), d.h. deren Messzeitpunkt am oder vor dem Berechnungszeit punkt liegt. Zur Bestimmung des Bewegungszustands Zi in der Figur 3 werden z.B. beim Bestimmungszeitpunkt Ai die Messdatensätze mit Messzeitpunkt tz nicht berücksichtigt, während beim Bestimmungszeitpunkt l2 die Messdatensätze mit Messzeitpunkt tz berücksichtigt werden.

Eine Bestimmung des Bewegungszustands für einen Bestimmungszeitpunkt kann mehrmals, an mehreren verschiedenen Berechnungszeitpunkten erfolgen. Etwa könnte immer dann, wenn neue Messdatensätze mit Messzeitpunkten in nerhalb der zeitlichen Grenzen für den Bestimmungszeitpunkt vorliegen, eine Bestimmung ausgelöst werden. Es ist hier auch möglich dies nur dann durchzu führen, wenn der Messzeitpunkt nach dem Bestimmungszeitpunkt liegt. Im Prin zip ist es allerdings auch möglich, dass ein Berechnungszeitpunkt vor dem Be stimmungszeitpunkt, für den der Bewegungszustand bestimmt wird, liegt. Bei spielsweise könnten die Messdatensätze mit den Messzeitpunkten tz und tz am Bestimmungszeitpunkt l2 verwendet werden, um eine Bestimmung bzw. Schät zung für den Bewegungszustand Zz zum Bestimmungszeitpunkt T3 (in der Figur nicht mehr dargestellt) durchzuführen. Denkbar ist auch, die Berechnungszeit punkte zeitlich regelmäßig zu beabstanden. Auch könnten die Berechnungszeit punkte mit den Bestimmungszeitpunkten zusammenfallen, wobei jeweils der Be wegungszustand für den vorherigen Bestimmungszeitpunkt bestimmt wird. Wie die Berechnungszeitpunkte bestimmt werden, ist insbesondere auch abhängig von der zur Verfügung stehenden Rechenleistung.

Um zu berücksichtigen, dass die Messzeitpunkte nicht mit den Bestimmungszeit punkten zusammenfallen, werden die Messdatensätze im zu minimierenden Funktional F basierend auf dem Messzeitpunkt und dem Bestimmungszeitpunkt gewichtet. Es sind also Gewichte w _Jt die als Zeitgewichte bezeichnet werden können, vorgesehen, die von der Differenz zwischen Messzeitpunkt t _} und Be stimmungszeitpunkt T _k abhängig sind: w _} = W _j (t _j - T _k ), vorzugsweise sind die Ge wichte vom Betrag der Differenz abhängig: w _} = W _j (\t _j - T _k \). Diese Gewichte soll ten immer positiv oder gleich Null sein. Die funktionale Abhängigkeit der Gewichte sollte so gewählt sein, dass wenn die Differenz zwischen Messzeitpunkt t _} und Bestimmungszeitpunkt T _k gleich Null ist, das Gewicht seinen maximalen Wert annimmt: w,(0) = w _max , und mit zunehmen- den Betrag monoton, bevorzugt streng monoton, abnimmt, bis der Wert Null er reicht wird, wobei der Wert Null vorzugsweise dann erreicht wird, wenn die obere Grenze T _k + d ₀ T bzw. untere Grenze T _k - d _u T der Messzeitpunkte, die berücksich tigt werden, erreicht wird:

W _j {T _k + d ₀ T- T _k ) = W _j (d ₀ T) = 0 = W _j {T _k - d _u T - T _k ) = W _j (-d _u T). Der maximale Wert kann als beliebige positive Zahl gewählt werden, z.B. gleich eins: w _max = 1. Der maximale Wert w _max ist für alle Messdatensätze (und gegebe nenfalls für andere, zusätzliche Messung, siehe unten) gleich.

Insbesondere kann gelten:

Allgemeiner kann die vorstehende lineare Abnahme mit zunehmendem Abstand auch durch eine Funktion f, die monoton, bevorzugt streng monoton, mit zuneh menden Argument steigt, modifiziert werden:

Beispielsweise könnte eine quadratische Funktion f(x) = x ² verwendet werden. Im Funktional F, das im Regressionsverfahren zu minimieren ist, werden die Messdatensätze dann entsprechend gewichtet, d.h. die Differenzen (d- d _j ) von gemessenen und geschätzten Dopplergeschwindigkeiten werden im Funktional mit dem jeweiligen Zeitgewicht w _} des Messdatensatzes multipliziert, es wird also w _j ( d _j - d _j ) im Funktional verwendet:

Erfolgt zusätzlich eine Gewichtung basierend auf Zusatzparametern der Messda tensätze, werden die Differenzen zusätzlich mit dem jeweiligen Parameterge wicht g _j multipliziert, es wird also g _jWj (d _j - d _j ) im Funktional verwendet:

Hier wurde in beiden Fällen wieder das bevorzugte in den Argumenten (d.h. in den gewichteten Differenzen) quadratische Funktional als Beispiel angegeben. Dieses Funktional kann sowohl für die Lösung (im Sinne des Regressionsverfah rens) des Gleichungssystems (5) als auch für die Lösung des Gleichungssys tems (2) verwendet werden.

Jede Zeile des Gleichungssystems (5) bzw. des Gleichungssystems (2) stellt ei ne Bedingung an den Linear-Geschwindigkeitsvektorund Winkelgeschwindig keitsvektor bzw. an die jeweilige Geschwindigkeit des Sensors dar. Da die Zeilen des jeweiligen Gleichungssystems Messdatensätzen entsprechen, kann jeder Messdatensatz als Bedingung angesehen werden.

Zur Bestimmung des Bewegungszustands für einen Bestimmungszeitpunkt wird an einem Berechnungszeitpunkt aus der Menge aller Messdatensätze eine Men ge von Bedingungen, d.h. eine Bedingungsmenge, ausgewählt, die zur Bestim mung des Bewegungszustands verwendet wird. Dies sind insbesondere diejeni gen Messdatensätze mit einem Messzeitpunkt, der innerhalb eines Intervalls [T _k - d _u T ; T _k + d ₀ T\ um den Bestimmungszeitpunkt T _k liegt (und deren Messzeitpunkt vor oder am Berechnungszeitpunkt liegt). Es ist hier möglich, weitere, nicht von Messdatensätzen stammende Bedingun gen zu der Menge der Bedingungen hinzuzufügen. Dies können Messungen von anderen Messgeräten sein und/oder vorgegebene Einschränkungen an die Be wegung des Körpers.

Vorzugsweise ist ein Drehratensensor bzw. Gyroskop vorgesehen, der bzw. das insbesondere im Ursprung des Körperbezugssystems angeordnet ist. Als zusätz liche Bedingung bzw. Gleichung zum Gleichungssystem (5) ergibt sich dann: wobei _B ä _{W B} die vom Drehratensensor gemessene Winkelgeschwindigkeit ist und I die dreidimensional Einheitsmatrix.

Eine Einschränkung an die Bewegung des Körpers (etwa ein Kraftfahrzeug, das sich in einer Ebene bewegt) könnte beispielsweise wie folgt aussehen, hier als Bedingungen an einzelne Komponenten des Geschwindigkeitsvektors und des Winkelgeschwindigkeitsvektors formuliert:

Jede dieser Bedingungen wird im Funktional des Regressionsverfahrens mit ei nem jeweiligen (Zusatzbedingungs-)Gewicht berücksichtigt. Die Größe des Ge wichts bestimmt, wie stark die zusätzliche Bedingung bzw. Einschränkung be rücksichtigt wird. Weiterhin kann im Falle von Messungen, etwa der Winkelge schwindigkeitsmessung durch den Drehratensensor, ein (Zeit-)Gewicht vorgese hen sein, das vom Messzeitpunkt in Relation zum Bestimmungszeitpunkt abhän gig ist. Insbesondere können solche Zeitgewichte wie im Zusammenhang mit den Messzeitpunkten der Sensoren beschrieben bestimmt werden.

Beispielsweise kann im Fall von Winkelgeschwindigkeitsmessungen durch einen Drehratensensor zum obigen Funktional F ein Zusatzfunktional F _G addiert wer den, um ein Gesamtfunktional zu erhalten: F _gesamt = F + FG, wobei

_B ä _{)W B} steht für den geschätzten Winkelgeschwindigkeitsvektor und | | für die Vektornorm.

G ist das Gewicht, entsprechend dem die Messungen des Drehratensensors be rücksichtigt werden; w _G ist das Zeitgewicht, das basierend auf dem Messzeitpunkt des Drehraten sensors bestimmt wird.

Da der Drehratensensor typischerweise ein höhere Genauigkeit als Sensoren aufweist und da im alleigemeinen eine hohe Anzahl von Messdatensätzen und relativ dazu wenige Drehratensensor-Messungen vorliegen, wird das Gewicht G vorzugsweise sehr viel höher als die entsprechende Gewichtung der Sen sormessungen gewählt. Diese entsprechende Gewichtung der Sensormessun gen ist in den obigen Formeln für das Funktional F jeweils 1 bzw., anders formu liert, da ein konstanter (positiver) Faktor im Funktional für die Optimierung nicht relevant ist, sondern nur eine Normierung darstellt, kann das Funktional für die Messdatensätze so normiert werden, dass diese entsprechende Gewichtung der Sensormessungen gleich 1 ist. Das Gewicht G wird also vorzugsweise sehr viel größer als 1 gewählt: G » 1; z.B. G > 10 oder mehr bevorzugt G > 100. Nach oben kann das Gewicht G ebenso eingeschränkt werden, etwa G < 1000.

Wie bereits weiter oben erwähnt, wird vorzugsweise ein robustes Regressions verfahren verwendet, um die Auswirkung von Ausreißern, die insbesondere durch nicht statische Objekte gegeben sind, zu minimieren. Bevorzugt kann eine Methode der iterativ neugewichteten kleinsten Quadrate (IRLS, engl.: iteratively reweighted least squares) verwendet werden.

Hierbei wird vom obigen quadratischen Funktional ausgegangen und die quadra tische Abweichung geschätzter von gemessenen Dopplergeschwindigkeiten wird im Funktional für jeden Messdatensatz bzw. für jede Bedingung mit einem Itera tionsgewicht h _j,k gewichtet, wobei k ein Iterationsparameter und j wie gehabt den Messdatensatz nummeriert. Beispielsweise ergibt sich dann, wobei hier die opti onalen Zeitgewichte w _} und Parametergewichte g _} berücksichtigt werden:

Für gegebene Iterationsgewichte h _]k wird dieses Funktional mittels der Methode der kleinsten Quadrate minimiert. Basierend auf der gefundenen Lösung, d.h. auf den gefundenen d _j , werden neue Iterationsgewichte A _7; * ₊ i bestimmt. Dabei wer den Iterationsgewichte, für die die gefundenen geschätzten Dopplergeschwindig keiten eine hohe Abweichung von den gemessen Dopplergeschwindigkeiten aufweisen, in Relation zu solchen mit geringer Abweichung verringert. Als An fangswert ofür die Iterationsgewichte kann ein bestimmter Wert vorgeben wer den, etwa 1 : h _] o = 1 , für alle j.

Bisher wurden die in den Messdatensätzen enthaltenen Azimuthwinkel Q und Höhenwinkel f, die in den zu lösenden Gleichungssystemen jeweils auf der lin ken Seite enthalten sind, als exakt angenommen. Im Allgemeinen stellen diese allerdings als Messwerte fehlerbehaftete Werte dar. Um dies zu berücksichtigen, kann vorzugsweise weiterhin eine Fehler-in-den-Variablen-Regression (Error-in- Variables-Regression) verwendet werden.

Dabei kann wie folgt vorgegangen werden: Zunächst wird der Bewegungszu stand mittels des vorstehend beschriebenen stark gekoppelten Ansatzes be stimmt bzw. geschätzt. Anschließend wird, unter Verwendung des bestimmten Bewegungszustands, eine Fehler-in-den-Variablen-Regression durchgeführt, um Azimuthwinkel Q und/oder Höhenwinkel f zu optimieren. Mit dem bestimmten op timierten Azimuthwinkel Q und/oder Höhenwinkel f wird der Bewegungszustand mittels des stark gekoppelten Ansatzes erneut bestimmt bzw. geschätzt und ba sierend darauf wieder Azimuthwinkel Q und/oder Höhenwinkel f erneut optimiert. Dies wird wiederholt, bis die Ergebnisse konvergieren, z.B. bis sich aufeinander folgende bestimmte Bewegungszustände innerhalb einer vorgegebenen Toleranz nicht mehr ändern, und/oder bis eine vorgegebene maximale Anzahl von Wie derholungen erreicht ist. Insgesamt stellt dieses Vorgehen wieder eine Regressi onsanalyse dar. Werden die Bewegungszustände des starren Körpers für mehrere aufeinander folgende Bestimmungszeitpunkte bestimmt, kann durch zeitliche Integration der Linear-Geschwindigkeitsvektoren _B V _W.B und/oder der Winkelgeschwindigkeitsvek toren _B JÜ _W.B eine relative Position und/oder Orientierung des Körpers an einem End-Bestimmungszeitpunkt T _End relativ zu einer Anfangsposition und/oder An fangsorientierung, die zu einem Anfangs-Bestimmungszeitpunkt T _Anfang vorlag, berechnet werden. Da der Bewegungszustand nur für diskrete Bestimmungszeit punkte bestimmt wird, ist das zeitliche Integral typischerweise als Summe gege ben.

Figur 4 zeigt ein Ablaufdiagramm einer bevorzugten Ausführungsform der Erfin dung.

In Schritt 102 werden Messdatensätze durch einen oder mehrere Sensoren er halten. Ebenso können weitere Messergebnisse erfasst werden, insbesondere Drehratenmessergebnisse durch einen Drehratensensor bzw. ein Gyroskop. Die Erfassung kann zu mehreren Messzeitpunkten erfolgen, insbesondere kann jeder Sensor fortlaufend, etwa mit einer bestimmten Messfrequenz, Messungen durch führen und die Messergebnisse erfassen.

In Schritt 104 werden die Messdatensätze und gegebenenfalls weiteren Messer gebnisse an eine Recheneinheit übermittelt. Dabei werden die Messdatensätze von den Sensoren an die Recheneinheit gesendet und von dieser empfangen. Ebenso werden gegebenenfalls die weiteren Messergebnisse von den jeweiligen Messsensoren an die Recheneinheit gesendet und von dieser empfangen. In beiden Fällen können die Sensoren direkt oder indirekt mit der Recheneinheit verbunden sein, auch eine Zwischenspeicherung kann vorgesehen sein, um eine spätere Auswertung (d.h. Bestimmung des Bewegungszustands) durchzuführen. Die Recheneinheit kann sich am oder im starren Körper, dessen Bewegungszu stand bestimmt werden soll, befinden. Ebenso kann es sich um eine vom Körper entfernte Recheneinheit, z.B. ein entfernter Computer oder Server, handeln. Die Messergebnisse können an die Recheneinheit über ein Kommunikationsnetz- werk (beispielsweise auch drahtlos) oder auch auf geeigneten Datenträgern (z.B. im Fall einer späteren Auswertung) übermittelt werden.

Die Schritte 102 und 104 können zusammen als Bereitstellungsschritt angesehen werden. Eine Bereitstellung kann auch auf andere Weise erfolgen, z.B. wenn die Messdatensätze und weiteren Messergebnisse bereits als gespeicherte Daten vorhanden sind, oder wenn simulierte Messdatensätze verwendet werden. Inso fern handelt es sich bei den Schritten 102 und 104 um bevorzugte Schritte.

In Schritt 106 wird wenigstens eine Bedingungsmenge aus den Messdatensätzen und gegebenenfalls weiteren Messergebnissen gebildet. Dabei wird wie vorste hend in Zusammenhang mit Figur 3 beschrieben vorgegangen. Es werden also für einen Bestimmungszeitpunkt, für den der Bewegungszustand bestimmt bzw. berechnet werden soll, Messdatensätze und gegebenenfalls weitere Messergeb nisse mit Messzeitpunkten innerhalb eines Bestimmungszeitraums in eine Be dingungsmenge aufgenommen. Dies kann ein oder mehrmals an entsprechen den Berechnungszeitpunkten erfolgen.

Im bevorzugten Schritt 108 werden Gewichte bestimmt, mit denen die in der Be dingungsmenge eingeschlossenen Messdatensätze und gegebenenfalls weiteren Messergebnisse im Funktional, das in der Regressionsanalyse verwendet wird, gewichtet werden. Das sind, wie vorstehend beschrieben, Zeitgewichte und/oder Parametergewichte und/oder, gegebenenfalls, Gewichte, mit denen die weiteren Messergebnisse relativ zu den Messdatensätzen gewichtet werden. Diese Ge wichte werden vor der Durchführung der eigentlichen Regressionsmethode be stimmt und bleiben während der Durchführung der Regressionsmethode unver ändert (für einen Berechnungszeitpunkt). Zusätzlich können während der Re gressionsanalyse weitere, von den vorstehenden verschiedene Gewichte ver wendet werden, die gegebenenfalls über mehrere Iterationen der Regressions methode verändert werden, z.B. die oben genannten Iterationsgewichte im Fall von IRLS. Auch eine Filterung kann hier erfolgen, in der bestimmte Messdaten sätze basierend auf Zusatzparametern der Messdatensätze aus einer Bedin gungsmenge ausgeschlossen werden. Anschließend erfolgt basierend auf der wenigstens einen Bedingungsmenge eine Regressionsanalyse, in der der Bewegungszustand (Linear- Geschwindigkeitsvektor und Winkelgeschwindigkeitsvektor) des Körpers be stimmt wird.

Dabei kann einerseits, Schritt 110, entsprechend dem stark gekoppelten Ansatz vorgegangen werden, d.h. in der Regressionsanalyse werden die Dopplerge schwindigkeiten direkt mit dem Bewegungszustand in Beziehung gesetzt (Glei chungssystem (5)).

Andererseits kann entsprechend dem schwach gekoppelten Ansatz vorgegangen werden. Dabei werden in Schritt 112 zunächst die Sensor- Geschwindigkeitsvektoren bestimmt, indem diese in einem Regressionsverfahren mit den Dopplergeschwindigkeiten in Beziehung gesetzt werden. Anschließend, Schritt 114, wird aus den Sensor-Geschwindigkeitsvektoren in einerweiteren Regression der Bewegungszustand berechnet.

Bei den Schritten 110, 112 und 114 können, wie vorstehend bei Schritt 108 erläu tert, gegebenenfalls im Rahmen der jeweilig verwendeten Regressionsmethode (etwa im Rahmen von IRLS) weitere, veränderliche Gewichte verwendet werden, die von den Zeitgewichten, Parametergewichten und Gewichten, mit denen wei teren Messergebnisse gewichtet werden, verschieden sind.

In einem bevorzugten Schritt 116 kann bestimmt werden, ob der Bewegungszu stand des Fahrzeugs für einen weiteren Bestimmungszeitpunkt und/oder basie rend auf einem anderen Bestimmungszeitraum, etwa wenn inzwischen neue Messdatensätze vorliegen, bestimmt werden soll. Wenn dies der Fall ist, kann zu Schritt 106 zurückgesprungen werden (Pfeil 118), in dem wenigstens eine neue Bedingungsmenge für den weiteren Bestimmungszeitpunkt und/oder basierend auf einem anderen Bestimmungszeitraum bestimmt wird. Unabhängig davon kann für bereits berechnete Bewegungszustände mit dem optionalen Schritt 120 fortgefahren werden. Im bevorzugten Schritt 120 kann der berechnete Bewegungszustand bzw. kön nen berechnete Bewegungszustände ausgegeben und/oder für eine weitere Ver arbeitung weitergegeben werden. Insbesondere kann, wenn Bewegungszustän de für mehrere aufeinanderfolgende Bestimmungszeitpunkte bestimmt wurden, eine Integration der Bewegungszustände über die Zeit erfolgen, um eine relative

Position und Orientierung des Körpers zwischen einem Anfangszustand zu ei nem früheren Bestimmungszeitpunkt und einem Endzustand zu einem späteren Bestimmungszeitpunkt zu bestimmen.