AT18366U1

AT18366U1 - System and method for radar inertial odometry

Info

Publication number: AT18366U1
Application number: ATGM8038/2023U
Authority: AT
Original assignee: Alpen Adria Univ Klagenfurt
Priority date: 2023-09-06
Filing date: 2023-09-06
Publication date: 2024-12-15

Abstract

Ein Gerät (10) zur Anordnung auf oder in einem sich bewegenden Fahrzeug (20) wird bereitgestellt, wobei das Gerät (10) zum Bestimmen einer Pose des sich bewegenden Fahrzeugs (20), wobei die Pose eine Position und eine Ausrichtung des sich bewegenden Fahrzeugs (20) einschließt, und/oder einer Geschwindigkeit des sich bewegenden Fahrzeugs konfiguriert ist, wobei das Gerät (10) Folgendes einschließt: eine Inertialmesseinheit (IMU) (30), die zum Bestimmen einer Beschleunigung und/oder einer Winkelgeschwindigkeit des sich bewegenden Fahrzeugs konfiguriert ist; ein Radargerät (40), das zum Bestimmen eines Radarscans konfiguriert ist, wobei der Radarscan Positionen einer Mehrzahl von reflektierenden Objekten (50) einschließt; und Bearbeitungsmittel (60), die zum Bestimmen einer Schätzung für die Pose und/oder einer Schätzung für die Geschwindigkeit des sich bewegenden Fahrzeugs (20) auf der Basis der Beschleunigung und/oder der Winkelgeschwindigkeit des sich bewegenden Fahrzeugs (20), die durch die IMU (30) bestimmt worden sind, konfiguriert sind; wobei die Bearbeitungsmittel (60) ferner konfiguriert sind, mehrere vergangene Messungen der Positionen der Mehrzahl von reflektierenden Objekten (50) vom Radargerät (40) zu benutzen, um die Schätzung für die Pose und/oder die Schätzung für die Geschwindigkeit des sich bewegenden Fahrzeugs (20) zu adjustieren.A device (10) for placement on or in a moving vehicle (20) is provided, the device (10) being configured to determine a pose of the moving vehicle (20), the pose including a position and an orientation of the moving vehicle (20), and/or a speed of the moving vehicle, the device (10) including: an inertial measurement unit (IMU) (30) configured to determine an acceleration and/or an angular velocity of the moving vehicle; a radar device (40) configured to determine a radar scan, the radar scan including positions of a plurality of reflective objects (50); and processing means (60) configured to determine an estimate for the pose and/or an estimate for the speed of the moving vehicle (20) based on the acceleration and/or the angular velocity of the moving vehicle (20) determined by the IMU (30); wherein the processing means (60) is further configured to use a plurality of past measurements of the positions of the plurality of reflective objects (50) from the radar device (40) to adjust the estimate for the pose and/or the estimate for the speed of the moving vehicle (20).

Description

SYSTEM UND VERFAHREN FÜR RADAR-INERTIALE ODOMETRIE SYSTEM AND METHOD FOR RADAR-INERTIAL ODOMETERY

TECHNISCHES FELD [0001] Die vorliegende Offenbarung betrifft Systeme und Verfahren für radar-inertiale Odometrie. TECHNICAL FIELD [0001] The present disclosure relates to systems and methods for radar inertial odometry.

HINTERGRUND BACKGROUND

[0002] Genaue Schätzung von Position, Pose und/oder Geschwindigkeit ist eine Voraussetzung für den autonomen Betrieb eines sich bewegenden Fahrzeugs, z.B. eines Roboters oder unbemannten Luftfahrzeugs (UAV), insbesondere in GNSS-blockierten und/oder -unvorbereiteten Umgebungen. Unbemannte Luftfahrzeuge können autonome Missionen in solchen Umgebungen ausführen, während sie sich möglicherweise mit den externen Faktoren auseinandersetzen müssen, die ihrer Wahrnehmung dieser Umgebungen noch mehr Herausforderungen stellen. Solche Faktoren können extreme Beleuchtungsbedingungen oder die Anwesenheit von Aerosolen in der Luft wie etwa Nebel oder Rauch einschließen. In solchen Szenarios ist die Robustheit der verwendeten Sensoren von überragender Bedeutung. [0002] Accurate estimation of position, pose and/or velocity is a prerequisite for the autonomous operation of a moving vehicle, e.g. a robot or unmanned aerial vehicle (UAV), especially in GNSS-blocked and/or unprepared environments. Unmanned aerial vehicles can perform autonomous missions in such environments while potentially having to deal with external factors that further challenge their perception of these environments. Such factors may include extreme lighting conditions or the presence of airborne aerosols such as fog or smoke. In such scenarios, the robustness of the sensors used is of paramount importance.

[0003] Bildverarbeitungsbasierte Verfahren sind für UAV-Ortungssysteme vorgeschlagen worden. Kamerasensoren haben jedoch mehrere Nachteile, die ihre Robustheit gegenüber den zuvor erwähnten Umgebungsfaktoren einschränken, und abrupte Roboterbewegungen können Bewegungsverwischungen verursachen, die ihre Leistung negativ beeinflussen. Alternativen zu traditionellen Kameras schließen neuromorphe Kameras ein, diese sind jedoch erheblich teurer und nicht einfach einzustellen. Andere Optionen umfassen LIDAR-Sensoren, die jedoch empfindlich gegen Aerosole in der Luft sind, nachdem sie nahe bei sichtbaren Wellenlängen arbeiten. Es ist auch der Gebrauch von Wärmebildkameras für Navigation vorgeschlagen worden. Diese bieten jedoch schwache Leistungen in Umgebungen ohne thermische Gradienten. [0003] Image processing-based methods have been proposed for UAV tracking systems. However, camera sensors have several drawbacks that limit their robustness to the aforementioned environmental factors, and abrupt robot movements can cause motion blur that negatively affects their performance. Alternatives to traditional cameras include neuromorphic cameras, but these are considerably more expensive and not easy to adjust. Other options include LIDAR sensors, but these are sensitive to airborne aerosols since they operate near visible wavelengths. The use of thermal imaging cameras for navigation has also been proposed. However, these offer poor performance in environments without thermal gradients.

[0004] Im Licht von Umweltrobustheitsanforderungen ist eine Sensorkombination zusammengestellt aus einem Radar, z.B. einem FMCW-Radar, und einer Inertialmesseinheit (IMU) vorgeschlagen worden. Ein FMCW-Radar leidet nicht unter den oben erwähnten umgebungsverursachten Problemen, und hat dennoch eine kleine Größe und kann Messungen bei höheren oder ähnlichen Frequenzen wie andere üblicherweise für Navigation benutzte Sensoren liefern. Radare melden Distanz, Dopplergeschwindigkeit und in begrenztem Maße Winkelmessungen von reflektierenden Punkten in der Umgebung. Sie sind in wachsendem Maße in der Autoindustrie benutzt worden. Millimeterwellentechnologie löste ihre Miniaturisierung aus und erhöhte ihre Genauigkeit, was ihre Anwendung in UAVs als Sensoren angestoßen hat, die zum Korrigieren einer während der Integration angesammelten IMU-Drift benutzt werden. [0004] In light of environmental robustness requirements, a sensor combination composed of a radar, e.g., an FMCW radar, and an inertial measurement unit (IMU) has been proposed. An FMCW radar does not suffer from the above-mentioned environmentally induced problems, yet is small in size and can provide measurements at higher or similar frequencies as other sensors commonly used for navigation. Radars report distance, Doppler velocity, and to a limited extent angle measurements from reflecting points in the environment. They have been increasingly used in the automotive industry. Millimeter wave technology triggered their miniaturization and increased their accuracy, which has spurred their application in UAVs as sensors used to correct IMU drift accumulated during integration.

[0005] Es gibt Formulierungen von Zustandsschätzern [1], [2], in denen ein kleines Ein-ChipFMCW-Radar zum Korrigieren von IMU-Messungen im Drohnenkontext benutzt wird. Jedoch beruhen diese Verfahren auf Annahmen, die die Umgebung vereinfachen, was die Betriebsreichweite (Manhattan-Weltmodell) ernstlich begrenzt [2] oder sie benutzen komplementäre Sensoren (z.B. Drucksensoren). Andere vorgeschlagene Verfahren in diesem Kontext versuchen nicht, die volle 6D-Pose abzuschätzen [3]. In Verbindung mit Bodenfahrzeugen im 2D-Bereich gibt es Ansätze, die ähnliche Sensoren [4] benutzen, aber diese Ansätze können nicht direkt auf den 3DRaum ausgedehnt werden. [0005] There are formulations of state estimators [1], [2] in which a small single-chip FMCW radar is used to correct IMU measurements in the drone context. However, these methods rely on assumptions that simplify the environment, which severely limits the operational range (Manhattan World Model) [2] or they use complementary sensors (e.g. pressure sensors). Other proposed methods in this context do not attempt to estimate the full 6D pose [3]. In connection with ground vehicles in the 2D domain, there are approaches that use similar sensors [4], but these approaches cannot be directly extended to 3D space.

[0006] Deshalb besteht ein Bedarf an Systemen und Verfahren, die eine schnelle, robuste und genaue Schätzung einer Position, einer Pose und/oder einer Geschwindigkeit eines sich bewegenden Fahrzeugs ermöglichen. [0006] Therefore, there is a need for systems and methods that enable fast, robust and accurate estimation of a position, pose and/or velocity of a moving vehicle.

KURZDARSTELLUNG SUMMARY

[0007] Gemäß Ausführungsformen wird ein Gerät zur Anordnung auf oder in einem sich bewegenden Fahrzeug bereitgestellt. Das Gerät ist konfiguriert, eine Pose zu bestimmen, wobei die [0007] According to embodiments, a device for placement on or in a moving vehicle is provided. The device is configured to determine a pose, wherein the

Pose eine Position und eine Ausrichtung des sich bewegenden Fahrzeugs, vorzugsweise eine 6D-Pose des sich bewegenden Fahrzeugs, und/oder eine Geschwindigkeit des sich bewegenden Fahrzeugs einschließt. Das Gerät schließt eine Inertialmesseinheit, IMU, ein, die zum Bestimmen einer Beschleunigung und/oder einer Winkelgeschwindigkeit des sich bewegenden Fahrzeugs konfiguriert ist. Das Gerät schließt ferner ein Radargerät ein, das zum Bestimmen eines Radarscans konfiguriert ist, wobei der Radarscan Positionen, vorzugsweise 3D-Positionen einer Mehrzahl von reflektierenden Objekten, einschließt. Pose includes a position and an orientation of the moving vehicle, preferably a 6D pose of the moving vehicle, and/or a speed of the moving vehicle. The device includes an inertial measurement unit, IMU, configured to determine an acceleration and/or an angular velocity of the moving vehicle. The device further includes a radar device configured to determine a radar scan, the radar scan including positions, preferably 3D positions of a plurality of reflective objects.

[0008] Weiterhin schließt das Gerät Bearbeitungsmittel ein, die zum Bestimmen einer Schätzung für die Pose und/oder einer Schätzung für die Geschwindigkeit des sich bewegenden Fahrzeugs auf der Basis der Beschleunigung und/oder der Winkelgeschwindigkeit des sich bewegenden Fahrzeugs, die durch die IMU bestimmt worden sind, konfiguriert sind. Die Bearbeitungsmittel sind konfiguriert, mehrere vergangene Messungen der Positionen der Mehrzahl von reflektierenden Objekten vom Radargerät zu benutzen, um die Schätzung für die Pose und/oder die Schätzung für die Geschwindigkeit des sich bewegenden Fahrzeugs zu adjustieren. [0008] The device further includes processing means configured to determine an estimate for the pose and/or an estimate for the speed of the moving vehicle based on the acceleration and/or the angular velocity of the moving vehicle determined by the IMU. The processing means is configured to use a plurality of past measurements of the positions of the plurality of reflective objects from the radar device to adjust the estimate for the pose and/or the estimate for the speed of the moving vehicle.

[0009] Die Bereitstellung des Gerätes wie oben beschrieben ermöglicht, die Genauigkeit der Bestimmung der 6D-Pose und/oder der Geschwindigkeit des sich bewegenden Fahrzeugs zu erhö6öhen. Ferner wird eine effiziente Implementation möglich, die Ausführung selbst auf ressourcenbegrenzten tragbaren Computern (z.B. auf System-on-Chip mit sehr geringer Größe, Gewicht und Leistungsaufnahme) ermöglicht. Weiterhin ist das Gerät gegen schwierige Umweltbedingungen unempfindlich, insbesondere wo bildverarbeitungsbasierte Ansätze fehlschlagen. Noch weitergehend wird Online-Intra-Sensor-Kalibration für einfachen Betriebseinsatz selbst durch unerfahrene Benutzer möglich. Darüber hinaus ist inhärent garantierter Datenschutz dadurch, dass keine visuellen Daten benutzt werden, möglich. [0009] Providing the device as described above enables to increase the accuracy of determining the 6D pose and/or the speed of the moving vehicle. Furthermore, an efficient implementation becomes possible, allowing execution even on resource-limited portable computers (e.g. on system-on-chip with very small size, weight and power consumption). Furthermore, the device is robust to difficult environmental conditions, especially where image processing-based approaches fail. Even further, online intra-sensor calibration becomes possible for easy operational use even by inexperienced users. Moreover, inherently guaranteed data protection is possible by not using visual data.

[0010] Das Radargerät kann ferner dazu konfiguriert werden, relative radiale Geschwindigkeiten von zumindest einigen der Mehrzahl von reflektierenden Objekten zu bestimmen. Die Bearbeitungsmittel können ferner konfiguriert werden, die relativen radialen Geschwindigkeiten zum Adjustieren der Schätzung für die Pose und die Geschwindigkeit des sich bewegenden Fahrzeugs zu benutzen. Dies ermöglicht, die Genauigkeit der Bestimmung der Pose und der Geschwindigkeit des sich bewegenden Fahrzeugs weiter zu verbessern. [0010] The radar device may be further configured to determine relative radial velocities of at least some of the plurality of reflecting objects. The processing means may be further configured to use the relative radial velocities to adjust the estimate for the pose and the speed of the moving vehicle. This allows the accuracy of the determination of the pose and the speed of the moving vehicle to be further improved.

[0011] Die Bearbeitungsmittel können dazu konfiguriert werden, die Schätzung für die Pose und die Geschwindigkeit des sich bewegenden Fahrzeugs mithilfe eines erweiterten Kalman-Filter(EKF)-Verfahrens zu adjustieren. Dies ermöglicht eine effiziente und genaue Adjustierung der IMU-Schätzung für die Pose und/oder für die Geschwindigkeit des sich bewegenden Fahrzeugs durch Verwendung der Messungen mit dem Radargerät. [0011] The processing means may be configured to adjust the estimate for the pose and the speed of the moving vehicle using an extended Kalman filter (EKF) method. This allows efficient and accurate adjustment of the IMU estimate for the pose and/or for the speed of the moving vehicle using the measurements with the radar device.

[0012] Ein Zustandsvektor des EKF kann die Pose und/oder die Geschwindigkeit des sich bewegenden Fahrzeugs zu einer aktuellen Zeit umfassen. Der Zustandsvektor des EKF kann ferner Schätzungen für die Posen des sich bewegenden Fahrzeugs entsprechend einer vorherbestimmten Anzahl N von Zeitpunkten mehrerer aufeinander folgender vergangener Radarscans umfassen. Dies ermöglicht, die Genauigkeit der Bestimmung der Pose und der Geschwindigkeit des sich bewegenden Fahrzeugs weiter zu verbessern. [0012] A state vector of the EKF may comprise the pose and/or the speed of the moving vehicle at a current time. The state vector of the EKF may further comprise estimates for the poses of the moving vehicle corresponding to a predetermined number N of time points of several consecutive past radar scans. This makes it possible to further improve the accuracy of the determination of the pose and the speed of the moving vehicle.

[0013] Die Bearbeitungsmittel können ferner dazu konfiguriert werden, eine Zuordnung der Mehrzahl von reflektierenden Objekten in aufeinander folgenden Radarscans durchzuführen und ein reflektierendes Objekt als einen persistenten Referenzpunkt in dem Fall zu initialisieren, dass das reflektierende Objekt kontinuierlich zwischen einer vorherbestimmten Anzahl von aufeinander folgenden Radarscans zugeordnet wird, wobei die vorherbestimmte Anzahl von aufeinander folgenden Radarscan vorzugsweise gleich N ist. Dies ermöglicht, persistente Referenzpunkte unter der Mehrzahl von reflektierenden Objekten zu identifizieren. [0013] The processing means may be further configured to perform an assignment of the plurality of reflective objects in consecutive radar scans and to initialize a reflective object as a persistent reference point in case the reflective object is continuously assigned between a predetermined number of consecutive radar scans, wherein the predetermined number of consecutive radar scans is preferably equal to N. This allows to identify persistent reference points among the plurality of reflective objects.

[0014] Die Bearbeitungsmittel können konfiguriert werden, die Zuordnung durch Bestimmen von Entsprechungen zwischen den aufeinander folgenden Radarscans und Ausrichten der aufeinander folgenden Radarscans unter Verwendung der Schätzung für die Pose des sich bewegenden Fahrzeugs durchzuführen, um die Positionen der Mehrzahl von reflektierenden Objekten in den aufeinander folgenden Radarscans zuzuordnen. Dies ermöglicht eine effiziente und präzise Zu-[0014] The processing means may be configured to perform the mapping by determining correspondences between the successive radar scans and aligning the successive radar scans using the estimate for the pose of the moving vehicle to map the positions of the plurality of reflective objects in the successive radar scans. This enables efficient and precise mapping.

ordnung zwischen den aufeinander folgenden Radarscans. order between successive radar scans.

[0015] Der Zustandsvektor des EKF kann ferner eine Position zumindest eines persistenten Referenzpunktes umfassen. Ein Aktualisierungsschritt des EKF kann einen Vergleich eines Abstands zum persistenten Referenzpunkt, wobei der Abstand aus einem aktuellen Radarscan bestimmt wird, mit einem geschätzten Abstand zu dem persistenten Referenzpunkt einschließen, wobei der geschätzte Abstand aus einem Radarscan zu einem Zeitpunkt einer Initialisierung des persistenten Referenzpunktes und zwei Posen des sich bewegenden Fahrzeugs bestimmt wird, wobei die zwei Posen des sich bewegenden Fahrzeugs jeweils dem Zeitpunkt der Initialisierung des Referenzpunktes und dem Zeitpunkt des aktuellen Radarscans entsprechen. Dies ermöglicht eine präzise Bestimmung der Pose und/oder der Geschwindigkeit des sich bewegenden FahrZeugs. [0015] The state vector of the EKF may further comprise a position of at least one persistent reference point. An updating step of the EKF may include a comparison of a distance to the persistent reference point, wherein the distance is determined from a current radar scan, with an estimated distance to the persistent reference point, wherein the estimated distance is determined from a radar scan at a time of initialization of the persistent reference point and two poses of the moving vehicle, wherein the two poses of the moving vehicle correspond respectively to the time of initialization of the reference point and the time of the current radar scan. This enables a precise determination of the pose and/or the speed of the moving vehicle.

[0016] Die Bearbeitungsmittel können ferner dazu konfiguriert werden, einen Satz von Spuren zu bestimmen, wobei jede der Spuren einem reflektierenden Objekt entspricht, dessen Position zwischen aufeinander folgenden Radarscans zugeordnet worden ist, und jede der Spuren die Positionen der entsprechenden reflektierenden Objekte für eine vorherbestimmte Anzahl von aufeinander folgenden Radarscans enthält, wobei eine maximale vorherbestimmte Anzahl von aufeinander folgenden Radarscans vorzugsweise gleich N ist. Dies ermöglicht eine effiziente Verfolgung der reflektierenden Objekte. [0016] The processing means may be further configured to determine a set of tracks, each of the tracks corresponding to a reflecting object whose position has been assigned between successive radar scans, and each of the tracks containing the positions of the corresponding reflecting objects for a predetermined number of successive radar scans, a maximum predetermined number of successive radar scans preferably being equal to N. This enables efficient tracking of the reflecting objects.

[0017] Der Aktualisierungsschritt des EKF kann ferner einen Vergleich eines aktuellen Abstands zu einem reflektierenden Objekt in einer Spur, wobei der aktuelle Abstand aus einer Position des reflektierenden Objekts in einem aktuellen Radarscan bestimmt wird, mit einem geschätzten Abstand zum reflektierenden Objekt in der Spur einschließen, wobei der geschätzte Abstand aus einer Position des reflektierenden Objekts in einem vergangenen Radarscan und zwei Posen des sich bewegenden Fahrzeugs bestimmt wird, wobei die zwei Posen des sich bewegenden Fahrzeugs den Zeitpunkten jeweils des vergangenen Radarscans und des aktuellen Radarscans entsprechen. Dies ermöglicht, die Präzision der Bestimmung der Pose und/oder der Geschwindigkeit des sich bewegenden Fahrzeugs weiter zu verbessern. [0017] The updating step of the EKF may further include a comparison of a current distance to a reflecting object in a lane, the current distance being determined from a position of the reflecting object in a current radar scan, with an estimated distance to the reflecting object in the lane, the estimated distance being determined from a position of the reflecting object in a past radar scan and two poses of the moving vehicle, the two poses of the moving vehicle corresponding to the times of the past radar scan and the current radar scan, respectively. This makes it possible to further improve the precision of the determination of the pose and/or the speed of the moving vehicle.

[0018] Der oder ein Aktualisierungsschritt des EKF kann ferner einen Vergleich einer relativen radialen Geschwindigkeit eines reflektierenden Objekts aus einem aktuellen Radarscan, wobei die relative radiale Geschwindigkeit durch das Radargerät bestimmt wird, mit einer geschätzten relativen radialen Geschwindigkeit des reflektierenden Objekts einschließen, wobei die geschätzte relative radiale Geschwindigkeit auf der Basis der vom Radargerät bestimmten Position des reflektierenden Objekts und der aktuellen Schätzungen der von der IMU bestimmten Pose und der Geschwindigkeit des sich bewegenden Fahrzeugs bestimmt wird. Dies ermöglicht, die Präzision der Bestimmung der Pose und/oder der Geschwindigkeit des sich bewegenden Fahrzeugs noch weiter zu verbessern. [0018] The or an updating step of the EKF may further include a comparison of a relative radial velocity of a reflecting object from a current radar scan, wherein the relative radial velocity is determined by the radar device, with an estimated relative radial velocity of the reflecting object, wherein the estimated relative radial velocity is determined on the basis of the position of the reflecting object determined by the radar device and the current estimates of the pose and the speed of the moving vehicle determined by the IMU. This allows the precision of the determination of the pose and/or the speed of the moving vehicle to be improved even further.

[0019] Gemäß Ausführungsformen wird ein Verfahren zum Bestimmen einer Pose, wobei die Pose eine Position und eine Ausrichtung des sich bewegenden Fahrzeugs einschließt, und einer Geschwindigkeit eines sich bewegenden Fahrzeugs bereitgestellt. Das Verfahren umfasst das Bestimmen, vorzugsweise durch eine Inertialmesseinheit (IMU), die auf dem sich bewegenden Fahrzeug angeordnet ist, einer Beschleunigung und/oder einer Winkelgeschwindigkeit des sich bewegenden Fahrzeugs. Das Verfahren umfasst ferner das Bestimmen, vorzugsweise durch ein auf dem sich bewegenden Fahrzeug angeordnetes Radargerät, eines Radarscans, wobei der Radarscan Positionen einer Mehrzahl von reflektierenden Objekten einschließt. Weiterhin umfasst das Verfahren das Bestimmen einer Schätzung für die Pose und/oder einer Schätzung der Geschwindigkeit des sich bewegenden Fahrzeugs auf der Basis der Beschleunigung und/oder der Winkelgeschwindigkeit des sich bewegenden Fahrzeugs. Noch weiterhin umfasst das Verfahren das Adjustieren der Schätzung für die Pose und/oder der Schätzung für die Geschwindigkeit des sich bewegenden Fahrzeugs durch Verwendung von mehreren vergangenen Messungen der Positionen der Mehrzahl von reflektierenden Objekten. [0019] According to embodiments, a method for determining a pose, the pose including a position and an orientation of the moving vehicle, and a speed of a moving vehicle is provided. The method comprises determining, preferably by an inertial measurement unit (IMU) arranged on the moving vehicle, an acceleration and/or an angular velocity of the moving vehicle. The method further comprises determining, preferably by a radar device arranged on the moving vehicle, a radar scan, the radar scan including positions of a plurality of reflective objects. Furthermore, the method comprises determining an estimate for the pose and/or an estimate of the speed of the moving vehicle based on the acceleration and/or the angular velocity of the moving vehicle. Still further, the method comprises adjusting the estimate for the pose and/or the estimate for the speed of the moving vehicle by using multiple past measurements of the positions of the plurality of reflective objects.

[0020] Die Bereitstellung des Verfahrens wie oben beschrieben ermöglicht, die Genauigkeit der Bestimmung der 6D-Pose und/oder der Geschwindigkeit des sich bewegenden Fahrzeugs zu [0020] The provision of the method as described above makes it possible to increase the accuracy of the determination of the 6D pose and/or the speed of the moving vehicle.

erhöhen. Ferner wird eine effiziente Implementation möglich, die Ausführung selbst auf ressourcenbegrenzten tragbaren Computern (z.B. auf System-on-Chip mit sehr geringer Größe, Gewicht und Leistungsaufnahme) ermöglicht. Weiterhin ist das Verfahren gegen schwierige Umweltbedingungen unempfindlich, insbesondere wo bildverarbeitungsbasierte Ansätze fehlschlagen. Noch weitergehend wird Online-Intra-Sensor-Kalibration für einfachen Betriebseinsatz selbst durch unerfahrene Benutzer möglich. Darüber hinaus ist inhärent garantierter Datenschutz dadurch, dass keine visuellen Daten benutzt werden, möglich. Furthermore, an efficient implementation is possible, allowing execution even on resource-limited portable computers (e.g. on system-on-chip with very small size, weight and power consumption). Furthermore, the method is robust against difficult environmental conditions, especially where image processing-based approaches fail. Even further, online intra-sensor calibration is possible for easy operational use even by inexperienced users. Furthermore, inherently guaranteed data protection is possible by not using visual data.

[0021] Das Verfahren kann ferner das Bestimmen, vorzugsweise durch das Radargerät, von relativen radialen Geschwindigkeiten zumindest einiger der Mehrzahl von reflektierenden Objekten umfassen, wobei das Adjustieren der Schätzung für die Pose und/oder die Geschwindigkeit des sich bewegenden Fahrzeugs durch weiteres Benutzen der relativen radialen Geschwindigkeiten durchgeführt werden kann. Dies ermöglicht, die Genauigkeit der Bestimmung der Pose und der Geschwindigkeit des sich bewegenden Fahrzeugs weiter zu verbessern. [0021] The method may further comprise determining, preferably by the radar device, relative radial velocities of at least some of the plurality of reflecting objects, wherein adjusting the estimate for the pose and/or the speed of the moving vehicle may be performed by further using the relative radial velocities. This allows the accuracy of the determination of the pose and the speed of the moving vehicle to be further improved.

[0022] Das Adjustieren der Schätzung für die Pose und/oder die Geschwindigkeit des sich bewegenden Fahrzeugs wird mithilfe eines erweiterten Kalman-Filter-(EKF)-Verfahrens durchgeführt. Dies ermöglicht eine effiziente und genaue Adjustierung der IMU-Schätzung für die Pose und/oder für die Geschwindigkeit des sich bewegenden Fahrzeugs durch Verwendung der Messungen mit dem Radargerät. [0022] Adjusting the estimate for the pose and/or the speed of the moving vehicle is performed using an extended Kalman filter (EKF) method. This allows efficient and accurate adjustment of the IMU estimate for the pose and/or the speed of the moving vehicle by using the measurements with the radar device.

KURZE BESCHREIBUNG DER ZEICHNUNGEN SHORT DESCRIPTION OF THE DRAWINGS

[0023] Abb. 1 zeigt eine schematische Ansicht eines Gerätes, das auf oder in einem sich bewegenden Fahrzeug angeordnet werden soll. [0023] Figure 1 shows a schematic view of a device intended to be placed on or in a moving vehicle.

[0024] Abb. 2 zeigt mehrere aufeinander folgende Posen eines sich bewegenden Fahrzeugs und eine Mehrzahl von reflektierenden Objekten einschließlich eines persistenten Referenzpunktes. [0024] Figure 2 shows several consecutive poses of a moving vehicle and a plurality of reflective objects including a persistent reference point.

[0025] Abb. 3 zeigt eine schematische Ansicht von Messspuren, die zum Aufzeichnen einer Geschichte der Erkennung eines reflektierenden Objekts benutzt werden. [0025] Figure 3 shows a schematic view of measurement traces used to record a history of the detection of a reflecting object.

[0026] Abb. 4 zeigt eine schematische Ansicht eines augmentierten nominalen Zustands und einer Fehlerzustand-Kovarianzmatrix eines erweiterten Kalman-Filters (EKF) nach dem Hinzufügen eines persistenten Referenzpunktes. [0026] Figure 4 shows a schematic view of an augmented nominal state and an error state covariance matrix of an extended Kalman filter (EKF) after adding a persistent reference point.

[0027] Abb. 5 zeigt eine vom sich bewegenden Fahrzeug geflogr > Troinktarin [0027] Fig. 5 shows a picture taken from the moving vehicle > Troinktarin

3 = 3 3 = 3

Ay Ra A Das 8 ODER EN Ay Ra A Das 8 OR EN

[0028] Abb. 6 zeigt den mittleren absoluten Fehler (MAE) über "1' = | at A 5) & SECH EL © 80/4 )eflogene Trajektorien. . [0028] Fig. 6 shows the mean absolute error (MAE) over "1' = | at A 5) & SECH EL © 80/4 )flown trajectories. .

S S

[0029] Abb. 7 zeigt ein Flussdiagramm, das ein Verfahren gemäß Ausführungsformen illustriert. ex wi w A — Ar N KEN i [0029] Fig. 7 shows a flow chart illustrating a method according to embodiments. ex wi w A — Ar N KEN i

Hd Hd Hd Hd Hd Hd Hd Hd Hd Hd

DETAILLIERTE BESCHREIBUNG 1 DETAILED DESCRIPTION 1

Hd A Hd A

A. BEZEICHNUNGEN A. DESIGNATIONS

[0030] Im Folgenden wird ein Bezeichnungssystem eingeführt, das in der vorliegenden Offenbarung benutzt wird. Eine normalverteilte multivariate Variable ist definiert als X;-N (x;, Zu) mit einem Mittelwert x; und Kovarianz (Unsicherheit) Z;; welche der Belief von £ genannt wird. Wegen der Symmetrieeigenschaften von Kovarianzen sind Elemente des unteren Dreiecks durch ein {se} abgekürzt. [0030] In the following, a notation system is introduced that is used in the present disclosure. A normally distributed multivariate variable is defined as X;-N(x;, Zu) with a mean x; and covariance (uncertainty) Z;; which is called the belief of £. Because of the symmetry properties of covariances, elements of the lower triangle are abbreviated by a {se}.

[0031] Namen von Bezugssystemen sind mit Großbuchstaben geschrieben und kalligrafisch, z.B. {FT} für Inertialmesseinheit (IMU). [0031] Names of reference systems are written in capital letters and calligraphically, e.g. {FT} for inertial measurement unit (IMU).

[0032] Eine Pose zwischen den Bezugssystemen ‚A und % ist als definiert mit und p € R®. Die Transformation eines Koordinatenvektors [0032] A pose between the reference systems 'A and % is defined as with and p € R®. The transformation of a coordinate vector

Cpp.; der vom Ursprung des Bezugssystems C zu einem Punkt P; zeigt, ausgedrückt in C, kann Cpp.; which points from the origin of the reference system C to a point P;, expressed in C, can

in das Bezugssystem A. transformiert werden durch (zu lesen als Y°" jp X zu). be transformed into the reference system A by (to be read as Y°" jp X to).

[0033] Rotationen werden als Einheitsquaternionen q € S0O(3) gespeichert mit ||q|| = 1, was eine direkte Abbildung zwischen Rotationsmatrizen und Hamiltonschen EinheitsQuaternionen ermöglicht durch [0033] Rotations are stored as unit quaternions q € S0O(3) with ||q|| = 1, which allows a direct mapping between rotation matrices and Hamiltonian unit quaternions by

| ist die Identitätsmatrix. | is the identity matrix.

[0034] Die a priori- und a posteriori-Formen eines Belief werden jeweils angegeben durch ein {e}) und {ee}. {e}* spezifiziert gemessene (gestörte) Größen. [0034] The a priori and a posteriori forms of a belief are specified by {e}) and {ee}, respectively. {e}* specifies measured (perturbed) quantities.

[0035] Für Vektoren und Blockmatrizen dienen Semikolons und Doppelpunkte zum Verbessern [0035] For vectors and block matrices, semicolons and colons are used to improve

der Lesbarkeit derart, dass [A : B] = [S] und [AB] = [A Bl. the readability such that [A : B] = [S] and [AB] = [A Bl.

B. BESCHREIBUNG DER AUSFÜHRUNGSFORMEN B. DESCRIPTION OF THE EMBODIMENTS

[0036] Abb. 1 zeigt ein Gerät 10, das auf oder in einem sich bewegenden Fahrzeug 20 angeordnet werden soll. Das sich bewegende Fahrzeug 20 kann z.B. ein unbemanntes Luftfahrzeug, UAV, sein, z.B. eine Drohne. Das Gerät 10 ist konfiguriert, eine Pose [p,, ’g,] des sich bewegenden Fahrzeugs 20 zu bestimmen, wobei die Pose eine Position Sp, , vorzugsweise eine 3DPosition %p,, und eine Orientierung “g,, vorzugsweise eine 3D-Orientierung “g,, des sich bewegenden Fahrzeugs 20 einschließt. Vorzugsweise ist das Gerät 10 konfiguriert, eine 6D-Pose [ Spp Sqj] des sich bewegenden Fahrzeugs 20 zu bestimmen, wobei die 6D-Pose die 3D-Position Sp; und die 3D-Orientierung %g, des sich bewegenden Fahrzeugs 20 einschließt. Weiterhin kann das Gerät 10 konfiguriert sein, eine Geschwindigkeit $v, des sich bewegenden Fahrzeugs 20 zu bestimmen. In Abb. 1 sind ein IMU-Bezugssystem 7, das mit dem IMU 30 assoziiert ist, ein Navigationsbezugssystem G, das ein globales Bezugssystem der Navigation ist, und ein Radarbezugssystem %, das mit dem Radargerät 40 assoziiert ist, gezeigt. %p,, 9a, und Sv, sind die Position, Orientierung und Geschwindigkeit jeweils des IMU-Bezugssystems 7 mit Bezug auf das Navigationsbezugssystem G. [0036] Fig. 1 shows a device 10 to be arranged on or in a moving vehicle 20. The moving vehicle 20 can be, for example, an unmanned aerial vehicle, UAV, e.g. a drone. The device 10 is configured to determine a pose [p,, ’g,] of the moving vehicle 20, wherein the pose includes a position Sp, , preferably a 3D position %p,, and an orientation “g,, preferably a 3D orientation “g,, of the moving vehicle 20. Preferably, the device 10 is configured to determine a 6D pose [ Spp Sqj] of the moving vehicle 20, wherein the 6D pose includes the 3D position Sp; and the 3D orientation %g, of the moving vehicle 20. Furthermore, the device 10 may be configured to determine a speed $v, of the moving vehicle 20. In Fig. 1, an IMU reference frame 7 associated with the IMU 30, a navigation reference frame G, which is a global navigation reference frame, and a radar reference frame % associated with the radar device 40 are shown. %p,, 9a, and Sv, are the position, orientation and speed of the IMU reference frame 7 with respect to the navigation reference frame G, respectively.

[0037] Wie in Abb. 1 gezeigt schließt das Gerät 10 eine Inertialmesseinheit (IMU) 30 ein. Die IMU 30 ist zum Bestimmen einer Beschleunigung ‚a* und/oder einer Winkelgeschwindigkeit ‚w* des sich bewegenden Fahrzeugs 20 konfiguriert. Die IMU 30 kann einen oder mehrere Sensoren (nicht gezeigt) einschließen, z.B. einen Beschleunigungsmesser und ein Gyroskop. Weiterhin schließt das Gerät 10 ein Radargerät 40 ein, das konfiguriert ist, Positionen *p„;, vorzugsweise 3D-Positionen *“pp;, einer Mehrzahl von reflektierenden Objekten 50 zum Einschließen zu bestimmen, wobei die Positionen *p„; der Mehrzahl von reflektierenden Objekten 50 einen Radarscan darstellen. Im Beispiel, das in Abb. 1 gezeigt ist, werden reflektierende Objekte P,, ...,Pı (mit einer ganzen Zahl n > 1) gezeigt, die die Mehrzahl von reflektierenden Objekten 50 darstellen. Das Radargerät kann z.B. ein frequenzmoduliertes Dauerstrich-(FMCW)-Radar sein. [0037] As shown in Fig. 1, the device 10 includes an inertial measurement unit (IMU) 30. The IMU 30 is configured to determine an acceleration 'a* and/or an angular velocity 'w* of the moving vehicle 20. The IMU 30 may include one or more sensors (not shown), e.g., an accelerometer and a gyroscope. Furthermore, the device 10 includes a radar device 40 configured to determine positions *p„;, preferably 3D positions *“pp;, of a plurality of reflective objects 50 for inclusion, wherein the positions *p„; of the plurality of reflective objects 50 represent a radar scan. In the example shown in Fig. 1, reflective objects P , ..., P ı (with an integer n > 1) are shown representing the plurality of reflective objects 50. The radar device can, for example, be a frequency modulated continuous wave (FMCW) radar.

[0038] Fer. Sa Ta Ag a Ar 60 ein, die zum Bestimmen einer Schätzung für dic Ma A Blau € 8077 de 5 41 RS für die Geschwindigkeit Sv, des sich bewegenden Fahrzeugs 20 auf der Basis der Beschleunigung ‚a* und/oder der Winkelgeschwindigkeit ‚w* des sich bewegenden Fahrzeugs 20, die von der IMU bestimmt wurden, konfiguriert sind. [0038] Fer. Sa Ta Ag a Ar 60 configured to determine an estimate for the speed Sv, of the moving vehicle 20 based on the acceleration 'a* and/or the angular velocity 'w* of the moving vehicle 20 determined by the IMU.

[0039] Die Bearbeitungsmittel 60 sind ferner konfiguriert, mehrere vergangene Messungen pP, der Positionen “pp; der Mehrzahl von reflektierenden Objekten 50 vom Radargerät 40 zum Adjustieren der Schätzung für die Pose [9pr Saj] und/oder die Schätzung für die Geschwindigkeit %v, des sich bewegenden Fahrzeugs 20 zu benutzen. Im vorliegenden Beispiel entspre-[0039] The processing means 60 are further configured to use a plurality of past measurements pP, of the positions “pp; of the plurality of reflecting objects 50 from the radar device 40 to adjust the estimate for the pose [9pr Saj] and/or the estimate for the speed %v, of the moving vehicle 20. In the present example,

chen die mehreren vergangenen Messungen Xp Pa, der Positionen *p„,; der Mehrzahl von re-the several past measurements Xp Pa, of the positions *p„,; of the majority of re-

flektierenden Objekten 50 den Zeitpunkten t, mit p = 1, ..., V vor einem aktuellen Radarscan zum aktuellen Zeitpunkt t.. reflecting objects 50 at times t, with p = 1, ..., V before a current radar scan at the current time t..

[0040] Vorzugsweise sind die Bearbeitungsmittel 60 dazu konfiguriert, die Schätzung für die Pose [“p,, %a,] und/oder die Schätzung für die Geschwindigkeit %v, des sich bewegenden Fahrzeugs 20 mithilfe eines erweiterten Kalman-Filter-(EKF)-Verfahrens zu adjustieren. [0040] Preferably, the processing means 60 are configured to adjust the estimate for the pose [“p,, %a,] and/or the estimate for the speed %v, of the moving vehicle 20 using an extended Kalman filter (EKF) method.

[0041] Abb. 2 zeigt das sich bewegende Fahrzeug 20 zu einem aktuellen Zeitpunkt te und mehrere aufeinander folgende vergangene Posen des sich bewegenden Fahrzeugs 20. Der Stern in Abb. 2 gab einen persistenten Referenzpunkt an, wie nachfolgend erklärt wird. [0041] Fig. 2 shows the moving vehicle 20 at a current time te and several consecutive past poses of the moving vehicle 20. The star in Fig. 2 indicates a persistent reference point, as explained below.

ÜBERBLICK ÜBER DAS VERFAHREN OVERVIEW OF THE PROCEDURE

[0042] Das nachfolgend beschriebene Verfahren basiert auf einer Error-State-EKF-Formulierung [5], die die IMU 30 als den primären Sensor für die Zustandspropagation benutzt. Aktualisierungen werden mit den FMCW-Radarmessungen durchgeführt, welche dünnbesetzte und Rauschen enthaltende 3D-Punktwolken und relative radiale Geschwindigkeiten von erkannten Punkten umfassen. Das hier benutzte Prinzip der Wahrnehmung des Radars ist kurz in [6] erklärt. Jedes Mal, wenn eine Radarmessung erhalten wird, wird der Zustand des EKF mit der Pose des Roboters augmentiert, bei welcher die Messung stattfand unter Verwendung von stochastischem Klonen wie im Folgenden beschrieben. Neue Posen werden dem Puffer von vergangenen Posen in einer FIFO-Weise angefügt. Die Maximalzahl von geklonten Posen ist durch den Parameter N definiert. [0042] The method described below is based on an error-state EKF formulation [5] that uses the IMU 30 as the primary sensor for state propagation. Updates are performed using the FMCW radar measurements, which include sparse and noisy 3D point clouds and relative radial velocities of detected points. The principle of radar perception used here is briefly explained in [6]. Each time a radar measurement is obtained, the state of the EKF is augmented with the pose of the robot at which the measurement took place using stochastic cloning as described below. New poses are appended to the buffer of past poses in a FIFO manner. The maximum number of cloned poses is defined by the parameter N.

[0043] Die Bearbeitungsmittel 60 sind ferner konfiguriert, einen Satz von Spuren zu bestimmen. Abb. 3 zeigt Messspuren, die zum Aufzeichnen einer Geschichte der Erkennungen benutzt werden. Der Stern in Abb. 3 markiert eine Spur, die als persistenter Referenzpunkt klassifiziert wurde, wie im Folgenden beschrieben. X in Abb. 3 bedeutet, dass die Spur W keinem Punkt im aktuellen Scan zur Zeit te zugeordnet ist und deshalb gelöscht werden wird. Ein Punkt in jeder Spur ist eine einzelne vergangene Erkennung. Unter den Zeitpunkten t in Abb.3, die unter der Zeitachse gezeigt sind, sind zugeordnete geklonte Roboterposen gezeigt, bei denen die Messungen aufgenommen wurden. Jede der Spuren entspricht einem reflektierenden Objekt, dessen Position zwischen aufeinander folgenden Radarscans zugeordnet worden ist. Jede der Spuren enthält die Positionen des entsprechenden reflektierenden Objekts für eine vorherbestimmte Anzahl von aufeinander folgenden Radarscans, wobei eine maximale vorherbestimmte Anzahl von aufeinander folgenden Radarscans vorzugsweise gleich N ist. [0043] The processing means 60 are further configured to determine a set of tracks. Fig. 3 shows measurement tracks used to record a history of detections. The star in Fig. 3 marks a track that has been classified as a persistent reference point, as described below. X in Fig. 3 means that the track W is not associated with any point in the current scan at time te and will therefore be deleted. A point in each track is a single past detection. Below the time points t in Fig. 3 shown below the time axis, associated cloned robot poses are shown at which the measurements were taken. Each of the tracks corresponds to a reflective object whose position has been associated between consecutive radar scans. Each of the tracks contains the positions of the corresponding reflective object for a predetermined number of consecutive radar scans, wherein a maximum predetermined number of consecutive radar scans is preferably equal to N.

[0044] D.h. aus aufgenommenen Messungen wird der Satz von Spuren konstruiert und beibehalten, der die kontinuierliche Erkennbarkeit von 3D-Punkten durch das Radar aufzeichnet. Jede der Spuren hat eine maximale Spurlänge N. Dies bedeutet, dass jeder zugeordnete Punkt eine Geschichte seiner erkannten Positionen behält und jedes Element dieser Geschichte sich auf eine geklonte Roboterpose bezieht, bei der die Erkennung aufgenommen wurde. Ein solcher Punkt mit einer Geschichte von Erkennungen wird als eine Spur bezeichnet und wird im Speicher gehalten, solange sie aktiv einem Punkt im aktuellen Scan im Zug des in [6] beschriebenen Zuordnungsverfahrens in dünnbesetzten Radarpunktwolken zugeordnet ist. Wenn sie nicht zugeordnet ist, ist sie inaktiv und wird deshalb entfernt. Für die Zuordnung von 3D-Punkten wird die letzte Erkennung jeder Spur benutzt zusammen mit der Roboterpose, bei der sie aufgenommen wurde, zusammen mit der Pose, bei der der aktuelle Scan aufgenommen wurde. [0044] That is, from recorded measurements the set of traces is constructed and maintained, which records the continuous detectability of 3D points by the radar. Each of the traces has a maximum trace length N. This means that each associated point keeps a history of its detected positions and each element of this history refers to a cloned robot pose at which the detection was recorded. Such a point with a history of detections is called a trace and is kept in memory as long as it is actively associated with a point in the current scan in the course of the sparse radar point cloud mapping procedure described in [6]. If it is not associated, it is inactive and is therefore removed. For the mapping of 3D points, the last detection of each trace is used together with the robot pose at which it was recorded, together with the pose at which the current scan was recorded.

[0045] Insbesondere werden diese zwei Posen benutzt, um den aktuellen Radarscan und die Spuren räumlich zueinander auszurichten und die Zuordnungsprozedur an solchen ausgerichteten Zuordnungen anzustoßen. Mit der Zuordnung gegeben wird die gesamte Spurgeschichte benutzt, um den Residuenvektor in der EKF-Aktualisierung zu bilden. Als nächstes werden Projektionen der aktuellen Robotergeschwindigkeit auf Normalenvektoren zu allen im aktuellen Radarscan erkannten Punkten zusammen mit ihren gemessenen Geschwindigkeiten benutzt, um den Residuenvektor weiter zu augmentieren. [0045] In particular, these two poses are used to spatially align the current radar scan and the tracks and to trigger the mapping procedure at such aligned mappings. With the mapping given, the entire track history is used to form the residual vector in the EKF update. Next, projections of the current robot velocity onto normal vectors to all points detected in the current radar scan, along with their measured velocities, are used to further augment the residual vector.

[0046] Die letzte Komponente des Residuenvektors ergibt sich aus der Benutzung von persis-[0046] The last component of the residual vector results from the use of persistent

tenten Referenzpunkten. Der Zustandsvektor des EKF kann ferner eine Position zumindest eines persistenten Referenzpunktes umfassen. persistent reference points. The state vector of the EKF can further comprise a position of at least one persistent reference point.

Die Bearbeitungsmittel 60 können ferner dazu konfiguriert werden, eine Zuordnung der Mehrzahl von reflektierenden Objekten 50 in aufeinander folgenden Radarscans durchzuführen und ein reflektierendes Objekt P; als einen persistenter Referenzpunkt in dem Fall zu identifizieren, dass das reflektierende Objekt P; kontinuierlich zwischen einer vorherbestimmten Anzahl von aufeinander folgenden Radarscans zugeordnet ist, wobei die vorherbestimmte Anzahl von aufeinander folgenden Radarscans vorzugsweise gleich N ist. The processing means 60 may be further configured to perform an assignment of the plurality of reflective objects 50 in consecutive radar scans and to identify a reflective object P; as a persistent reference point in case the reflective object P; is continuously assigned between a predetermined number of consecutive radar scans, wherein the predetermined number of consecutive radar scans is preferably equal to N.

[0047] Die Bearbeitungsmittel 60 sind ferner konfiguriert, die Zuordnung durch Bestimmen von Entsprechungen zwischen den aufeinander folgenden Radarscans und Ausrichten der aufeinander folgenden Radarscans unter Verwendung der Schätzung für die Pose des sich bewegenden Fahrzeugs 20 durchzuführen, um die Positionen der Mehrzahl von reflektierenden Objekten in den aufeinander folgenden Radarscans zuzuordnen. [0047] The processing means 60 are further configured to perform the mapping by determining correspondences between the successive radar scans and aligning the successive radar scans using the estimate for the pose of the moving vehicle 20 to map the positions of the plurality of reflective objects in the successive radar scans.

[0048] D.h. im Fall, dass eine Spur kontinuierlich für N Male gesehen wurde, diese aus dem Satz von Spuren entfernt wird, zum Zustandsvektor als persistenter Referenzpunkt hinzugefügt wird und den Erkennungen zugeordnet wird. Residuenvektoren werden dann im Aktualisierungsschritt benutzt, um den Mittelwert des Fehlerzustands zu bestimmen, der in den regulären Zustand eingeführt wird. Die Anordnung der Koordinatensysteme für Messungen, die in der vorliegenden Offenbarung beschrieben werden, ist in Abb. 1 gezeigt. [0048] That is, in case a trace has been seen continuously for N times, it is removed from the set of traces, added to the state vector as a persistent reference point and associated with the detections. Residual vectors are then used in the update step to determine the mean of the error state that is introduced into the regular state. The arrangement of the coordinate systems for measurements described in the present disclosure is shown in Fig. 1.

SYSTEMZUSTAND SYSTEM STATE

[0049] Ein Zustandsvektor des EKF umfasst die 6D-Pose und/oder die Geschwindigkeit des sich bewegenden Fahrzeugs 20 zu einer aktuellen Zeit. Der Zustandsvektor des EKF umfasst ferner Schätzungen für die Posen des sich bewegenden Fahrzeugs 20 entsprechend einer vorherbestimmten Anzahl N von Zeitpunkten mehrerer aufeinander folgender vergangener Radarscans, wie nachfolgend erklärt. [0049] A state vector of the EKF includes the 6D pose and/or the velocity of the moving vehicle 20 at a current time. The state vector of the EKF further includes estimates for the poses of the moving vehicle 20 corresponding to a predetermined number N of time points of several consecutive past radar scans, as explained below.

[0050] Der Zustandsvektor des EKF ist definiert wie folgt: [0050] The state vector of the EKF is defined as follows:

mit einem IMU-Zustand x, = [ Sp; 991; vi; ba; ba], Stochastisch geklonten Zuständen xc der IMU-Posen entsprechend den vorangehenden Radarmessungen (wie nachfolgend beschrieben) und einem Satz von persistenten Referenzpunkten x, (wie nachfolgend beschrieben). bu und ba sind jeweils Messungsverschiebungen des Gyroskops und des Beschleunigungsmessers. with an IMU state x, = [ Sp; 991; vi; ba; ba], stochastically cloned states xc of the IMU poses corresponding to the previous radar measurements (as described below), and a set of persistent reference points x, (as described below). bu and ba are measurement displacements of the gyroscope and accelerometer, respectively.

[0051] [*Pın Sam] mit n = 1, ..., N definieren einen Satz von vergangenen IMU-Posen mit Bezug auf das Navigationsbezugssystem G zu den Zeitpunkten von vergangenen Radarmessungen. Spem mitm = 1, ..., M definieren Positionen von persistenten Referenzpunkten {£} mit Bezug auf das Navigationsbezugssystem 6. Wie nachfolgend erklärt, werden [ “p,., %4,] (entsprechend den neuesten Koordinaten der Spuren) für die Erzeugung von Ad- hoc-Punktentsprechung benutzt derart, dass es nicht notwendig ist, 3D-Punkte im Zustandsvektor zu halten, um abstandsbasierte Messungen zu benutzen. [0051] [*Pın Sam] with n = 1, ..., N define a set of past IMU poses with respect to the navigation reference system G at the times of past radar measurements. Spem with m = 1, ..., M define positions of persistent reference points {£} with respect to the navigation reference system 6. As explained below, [“p,., %4,] (corresponding to the latest coordinates of the tracks) are used for generating ad hoc point correspondence such that it is not necessary to keep 3D points in the state vector to use distance-based measurements.

[0052] Die Entwicklung des Zustands wird durch die folgenden Differentialgleichungen ausgedrückt [0052] The evolution of the state is expressed by the following differential equations

Da = Pi, Ds SR & Dr a 3, 8 Da = Pi, Ds SR & Dr a 3, 8

wo n=1, .., N sich auf die Jüngsten vergangenen IMU-Posen beziehen, die sich nicht mit der Zeit ändern, m=1, .., M beziehen sich auf M jüngste geschätzte Positionen von Referenzpunkten, ‚a* und ‚w* sind die Beschleunigungsmesser- und Gyroskopmessungen der IMU mit einem weißen Messrauschen na und na. Nia UNd Nıw Sind als weißes Gauss‘sches Rauschen angenommen, um die Verschiebungsänderung über die Zeit hin als einen Zufallsprozess zu modellieren. Für den Schwerkraftvektor wird eine Ausrichtung auf die z-Achse des Navigationsbezugssystems 6g = where n=1, .., N refer to the most recent past IMU poses that do not change with time, m=1, .., M refer to M most recent estimated positions of reference points, ‚a* and ‚w* are the accelerometer and gyroscope measurements of the IMU with a white measurement noise na and na. Nia and Nıw are assumed to be white Gaussian noise to model the displacement change over time as a random process. For the gravity vector, an alignment to the z-axis of the navigation reference system 6g =

[0, 0, 9.8117 angenommen. [0, 0, 9.8117 assumed.

[0053] Weil eine Error-State-EKF-Formulierung benutzt wird, wird der Fehlerzustandsvektor aus den in Gleichung (1) definierten Zuständen eingeführt wie folgt: + PS . [0053] Because an error-state EKF formulation is used, the error state vector is introduced from the states defined in equation (1) as follows: + PS .

I am I I Se ® z a PEN rl hr By & Ah £ Pa De ; . I am I I Se ® z a PEN rl hr By & Ah £ Pa De ; .

U U U U

An 13} On 13}

[0054] Für Translationskomponenten, z.B. die Position, ist der Fehler als 95, = 9%; — Ypır definiert, während er für Rotationen/Quaternionen definiert ist als [0054] For translation components, e.g. position, the error is defined as 95, = 9%; — Ypır, while for rotations/quaternions it is defined as

3 aa €f AR Si €& — BR X 8 wobei & und 6 jeweils ein Quaternionenprodukt und eine Kleinwinkelnäherung sind. 3 aa €f AR Si €& — BR X 8 where & and 6 are a quaternion product and a small angle approximation, respectively.

ZUSTANDSAUGMENTIERUNG CONDITION AUGMENTATION

[0055] Zum Bearbeiten relativer Messungen in Bezug auf Schätzungen zu verschiedenen Zeitinstanzen führen Roumeliotis und Burdick das Konzept des stochastischen Klonens (SC) in [7] ein. Um die Korrelationen/wechselseitigen Abhängigkeiten zwischen den Schätzungen von verschiedenen Zeitinstanzen zweckgemäß zu betrachten, wird eine identische Kopie der erforderlichen Zustände und ihrer Unsicherheiten benutzt, um den Zustandsvektor und die entsprechende Fehlerzustandskovarianzmatrix zu augmentieren. [0055] To handle relative measurements with respect to estimates at different time instances, Roumeliotis and Burdick introduce the concept of stochastic cloning (SC) in [7]. To properly consider the correlations/interdependencies between the estimates from different time instances, an identical copy of the required states and their uncertainties is used to augment the state vector and the corresponding error state covariance matrix.

[0056] Gegeben die Definition des Fehlerzustands in Gleichung (3), ist X. definiert als der Fehlerzustand des stochastischen Klons der IMU-Pose [9pr Sa] Weil der neueste geklonte Zu-[0056] Given the definition of the error state in equation (3), X. is defined as the error state of the stochastic clone of the IMU pose [9pr Sa] Because the latest cloned

stand voll mit der IMU-Pose korreliert ist und verbleibende geklonte Zustände miteinander korreliert sind, führt es zu der folgenden augmentierten Kovarianzmatrix des entsprechenden Fehlerzustands: state is fully correlated with the IMU pose and remaining cloned states are correlated with each other, it leads to the following augmented covariance matrix of the corresponding error state:

x er ER © f EN ?} 8 .zx X ? Ki x Ox + Den x SE SS a2 * s {Si} x er ER © f EN ?} 8 .zx X ? Ki x Ox + Den x SE SS a2 * s {Si}

* * Sen Bez $* & * ®* Dr * * Sen Bez $* & * ®* Dr

wobei X, die 15 x 15-Unsicherheit des IMU-Fehlerzustands %, ist. Z, ist die 3M x 3M-Unsicherheit eines Satzes von M Referenzpunkt-Fehlerzuständen X%,.= [%,1; ...; Xim].Zc, = Zi 9 ist die 6 x 6-where X, is the 15 x 15 uncertainty of the IMU error state %, Z, is the 3M x 3M uncertainty of a set of M reference point error states X%,.= [%,1; ...; Xim].Zc, = Zi 9 is the 6 x 6-

Unsicherheit des neu-geklonten IMU-Pose-Fehlerzustands (welche voll korreliert ist, somit Zjc, = Zuge): Alle Kreuzkovarianzen der aktuellen IMU-Pose sind den Kreuzkovarianzen des neu geklonten Zustands zugewiesen: Zee; = Zac 3. Cm ist die 6 x 6Unsicherheit der ältesten geklonten IMU-Pose. ZeiC ist die Kreuzkorrelation zwischen der i-ten Uncertainty of the newly cloned IMU pose error state (which is fully correlated, thus Zjc, = Zuge): All cross-covariances of the current IMU pose are assigned to the cross-covariances of the newly cloned state: Zee; = Zac 3. Cm is the 6 x 6 uncertainty of the oldest cloned IMU pose. ZeiC is the cross-correlation between the i-th

und j-ten geklonten IMU-Pose, X,c, ist die Kreuzkorrelation zwischen dem aktuellen IMU-Zustand und der i-ten geklonten IMU-Pose, und Xc, sind die Kreuzkorrelationen zwischen den IMU-Posen und den Referenzpunkten. and j-th cloned IMU pose, X,c, is the cross-correlation between the current IMU state and the i-th cloned IMU pose, and Xc, are the cross-correlations between the IMU poses and the reference points.

mit £ = 2,...,n und ZC1 = Zr with £ = 2,...,n and ZC1 = Zr

[0057] Die geklonten Posen und Referenzpunkte entwickeln sich nicht mit der Zeit weiter, was bedeutet, dass es keine Zustandsübergänge gibt (d.h. £*"“ = / mitn=1,..., N und ef" = 7 [0057] The cloned poses and reference points do not evolve over time, which means that there are no state transitions (i.e. £*"“ = / with n=1,..., N and ef" = 7

mit m = 1, ..., M und kein Prozessrauschen (d.h. G4** = 0 und 61" = 0) wird angewandt, während sich die Navigationszustände mit den IMU-Messungen weiterentwickeln. Die linearisierte Fehlerzustandsfortpflanzung kann abgeleitet werden als: with m = 1, ..., M and no process noise (i.e. G4** = 0 and 61" = 0) is applied while the navigation states evolve with the IMU measurements. The linearized error state propagation can be derived as:

X X

ww ww

X : gar ik en X : I don't have one

Ü Ü

Se SEE DE X Se SEE DE X

{E23 {E23

mit der linearisierten Zustandsübergangsmatrix ® und der linearisierten Störungsmatrix G, die wie z.B. in [8] erklärt berechnet wird. with the linearized state transition matrix ® and the linearized perturbation matrix G, which is calculated as explained e.g. in [8].

[0058] Die volle Fehlerzustandsunsicherheit von Gleichung (5) kann dann fortgepflanzt werden als [0058] The full fault state uncertainty of equation (5) can then be propagated as

ae KOREA 8 ae KOREA 8

CA wobei Q die diskretisierte Matrix des Prozessrauschens und Ep die Fehlerzustandsübergangsmatrix des IMU-Fehlerzustands %; sind. CA where Q is the discretized matrix of the process noise and Ep is the error state transition matrix of the IMU error state %;.

[0059] Diese Fortpflanzung ermöglicht, die Kreuzkorrelationen zwischen dem Referenzpunkt, den geklonten Zuständen und den entwickelten IMU-Zuständen in unserer Error-State-Formulierung rigoros widerzuspiegeln. Der vorstehend beschriebene Formalismus ermöglicht, die Zustandsvariablen korrekt zu benutzen, um die Spuren zum aktuellen Scan vor der Zuordnung der Punkte auszurichten und ebenso Residuen während der Aktualisierung zu berechnen. [0059] This propagation allows the cross-correlations between the reference point, the cloned states and the evolved IMU states to be rigorously reflected in our error state formulation. The formalism described above allows the state variables to be correctly used to align the traces to the current scan before assigning the points and also to compute residuals during the update.

MULTIZUSTANDSAKTUALISIERUNG MIT MESSSPUREN MULTI-STATE UPDATE WITH MEASUREMENT TRACKS

[0060] Ein Aktualisierungsschritt des EKF schließt einen Vergleich eines aktuellen 1D-Abstands zu einem reflektierenden Objekt in einer Spur, wobei der aktuelle Abstand aus einer Position des reflektierenden Objekts in einem aktuellen Radarscan bestimmt wird, mit einem geschätzten Abstand zum reflektierenden Objekt in der Spur ein, wobei der geschätzte Abstand aus einer Posi-[0060] An updating step of the EKF includes a comparison of a current 1D distance to a reflecting object in a lane, the current distance being determined from a position of the reflecting object in a current radar scan, with an estimated distance to the reflecting object in the lane, the estimated distance being determined from a position

tion des reflektierenden Objekts in einem vergangenen Radarscan und zwei Posen des sich bewegenden Fahrzeugs bestimmt wird, wobei die zwei Posen des sich bewegenden Fahrzeugs den Zeitpunkten jeweils des vergangenen Radarscans und des aktuellen Radarscans entsprechen. tion of the reflecting object in a past radar scan and two poses of the moving vehicle, where the two poses of the moving vehicle correspond to the times of the past radar scan and the current radar scan respectively.

[0061] Gegeben ein Satz von zugeordneten 3D-Punkt-Spuren wie in Abb. 3 gezeigt, können die Abstände zu den zugeordneten Punkten im aktuellen Radar über alle Punkte, die in der Spurengeschichte enthalten sind, wie folgt geschätzt werden. Für eine einzelne zugeordnete Spur, unter Verwendung von geklonten Posen im Puffer, werden alle Punkte RppiP aus der Spurgeschichte zum Zeitpunkt t,, wo p = 1, ... V und V die Länge der zugeordneten Spur ist, in das aktuelle Radarbezugssystem transformiert, unter Berücksichtigung der räumlichen Entwicklung des Roboters: [0061] Given a set of associated 3D point tracks as shown in Fig. 3, the distances to the associated points in the current radar over all points contained in the track history can be estimated as follows. For a single associated track, using cloned poses in the buffer, all points RppiP from the track history at time t,, where p = 1, ... V and V is the length of the associated track, are transformed into the current radar reference frame, taking into account the spatial evolution of the robot:

3 R Pa T > WE tn E ER, N £ 3 R Pa T > WE tn E ER, N £

wo ’Rz und ’pr die konstante Pose (Ausrichtung und Position) des Radarbezugssystems in Bezug auf das IMU-Bezugssystem ist. %R,(*“-*P} und Sp,(*etP} sind jeweils die IMU-Ausrichtung und -Position entsprechend dem Spurgeschichtenelement zur Zeit t, und dem aktuellen Radarscan zur Zeit te, in Bezug auf das Navigationsbezugssystem 6. Der 3D-Punkt wird vom kartesischen Raum in Kugelkoordinaten transformiert und nur die informativste Dimension, nämlich der Abstand, wird für die Konstruktion von Residuen benutzt. where ’Rz and ’pr is the constant pose (orientation and position) of the radar reference frame with respect to the IMU reference frame. %R,(*“-*P} and Sp,(*etP} are the IMU orientation and position corresponding to the track history element at time t, and the current radar scan at time te, with respect to the navigation reference frame 6, respectively. The 3D point is transformed from Cartesian space to spherical coordinates and only the most informative dimension, namely the distance, is used for the construction of residuals.

[0062] Der geschätzte Abstand, der mit der aktuellen Abstandsmessung verglichen wird, wird für jeden Punkt in der Spurgeschichte als die Norm des transformierten Punktes von tp berechnet: [0062] The estimated distance, which is compared with the current distance measurement, is calculated for each point in the track history as the norm of the transformed point of tp:

ir ep F 3. ir ep F 3.

3 RE AO de | PL 0) 3 RE AO de | PL 0)

wobei dp; der Abstand zu einem einzelnen Punkt in der zugeordneten Spurgeschichte Rp’piP zur where dp; is the distance to a single point in the associated track history Rp’piP to

Zeit ty ausgerichtet zur aktuellen Radarpose zur Zeit t.. Weil diese Messung Zustände von vergangenen Zeitinstanzen betrifft, ist stochastisches Klonen erforderlich wie oben eingeführt. Time ty aligned to the current radar pose at time t. Because this measurement concerns states from past time instances, stochastic cloning is required as introduced above.

AKTUALISIERUNG MIT PERSISTENTEN REFERENZPUNKTEN UPDATE WITH PERSISTENT REFERENCE POINTS

[0063] Der Aktualisierungsschritt des EKF kann ferner einen Vergleich eines Abstands zum persistenten Referenzpunkt, der aus einem aktuellen Radarscan bestimmt wird, mit einem geschätzten Abstand zu dem persistenten Referenzpunkt einschließen, wobei der geschätzte Abstand aus einem Radarscan zu einem Zeitpunkt einer Initialisierung des Referenzpunktes und zwei Posen des sich bewegenden Fahrzeugs bestimmt wird, wobei die zwei Posen des sich bewegenden Fahrzeugs jeweils dem Zeitpunkt der Initialisierung des Referenzpunktes und dem Zeitpunkt des aktuellen Radarscans entsprechen. [0063] The updating step of the EKF may further include a comparison of a distance to the persistent reference point determined from a current radar scan with an estimated distance to the persistent reference point, wherein the estimated distance is determined from a radar scan at a time of initialization of the reference point and two poses of the moving vehicle, wherein the two poses of the moving vehicle correspond to the time of initialization of the reference point and the time of the current radar scan, respectively.

[0064] Wenn eine Spur für eine vorherdefinierte Zeitdauer in der Vergangenheit kontinuierlich zugeordnet worden ist, wird sie zu einem persistenten Referenzpunkt angehoben und als solcher dem Zustandsvektor hinzugefügt. Insbesondere wird der Satz von Spuren nach jeder Aktualisierung auf Elemente gescannt, die N Male aufeinander folgend zugeordnet worden sind. Wenn eine Spur dieses Kriterium erfüllt, wird sie zum Initialisieren eines persistenten Referenzpunktes im Zustandsvektor benutzt und die Kovarianzmatrix wird gemäß [9] augmentiert. Für praktische Zwecke wird xp = [xı; xc] eingeführt. [0064] If a trace has been continuously allocated for a predefined period of time in the past, it is raised to a persistent reference point and as such is added to the state vector. In particular, after each update, the set of traces is scanned for elements that have been allocated N times consecutively. If a trace satisfies this criterion, it is used to initialize a persistent reference point in the state vector and the covariance matrix is augmented according to [9]. For practical purposes, xp = [xı; xc] is introduced.

[0065] Blöcke, die zur Augmentierung des Zustandsvektors und der Fehlerzustandskovarianz benötigt werden, sind in Abb. 4 gezeigt, die einen augmentierten Nominalzustand und eine Fehlerzustandskovarianz des EKF nach dem Hinzufügen eines persistenten Referenzpunktes zeigt. Die vorstehenden Blöcke werden wie folgt berechnet: [0065] Blocks needed to augment the state vector and the error state covariance are shown in Fig. 4, which shows an augmented nominal state and error state covariance of the EKF after adding a persistent reference point. The above blocks are calculated as follows:

X = HnXaHp + HERE, CL X = HnXaHp + HERE, CL

X Hrn {EEE X Mr. {EEE

wobei Zpx = [ZpXpıl, und wobei Xp. die Kreuzkovarianz zwischen der IMU und dem Fehlerzustandsvektorsegment von IMU-Klonen und den persistenten Referenzpunkten ist. R ist die Kova-where Zpx = [ZpXpıl, and where Xp. is the cross-covariance between the IMU and the error state vector segment of IMU clones and the persistent reference points. R is the covariance

rianzmatrix des Messrauschens, Hp = Zr und Hı = SE sind die Jacobi-Matrizen jeweils des inD rance matrix of the measurement noise, Hp = Zr and Hı = SE are the Jacobi matrices of the inD

versen Beobachtungsmodells einer 3D-Punkt-Radarmessung, p (siehe Gleichung 12), bezogen verse observation model of a 3D point radar measurement, p (see equation 12), related

auf die IMU und Fehlerzustandsvariablen zx5 von IMU-Klonen, und des neu hinzugefügten Refe-to the IMU and error state variables zx5 of IMU clones, and the newly added reference

renzpunktes 1 = x... Das inverse Beobachtungsmodell des 3D-Radarpunktes im Navigations-reference point 1 = x... The inverse observation model of the 3D radar point in the navigation

bezugssystem G wird wie folgt ausgedrückt: reference system G is expressed as follows:

& ee AR FR a GC N N „OS OS EB Sp {x Es RU Ra RD, Dal SEE & ee AR FR a GC N N “OS OS EB Sp {x Es RU Ra RD, Dal SEE

CZ CZ

wobei 'R„ und /,pr die Pose zwischen der IMU und dem Radarsensor ist (die als starr und apriori bekannt angenommen wird), *„p- die Radarbeobachtung des Spurpunktes im aktuellen Radarbezugssystem R ist, mit dem ein m-ter Referenzpunkt initialisiert wird, und %R, und Scp, die aktuelle Pose der IMU im Navigationsbezugssystem ist. Für Lesbarkeit wird die Schätzung des m-ten Referenzpunktes als 1 = %cpcm abgekürzt. where 'R„ and /,pr is the pose between the IMU and the radar sensor (which is assumed to be rigid and known a priori), *„p- is the radar observation of the track point in the current radar reference frame R, with which an m-th reference point is initialized, and %R, and Scp, is the current pose of the IMU in the navigation reference frame. For readability, the estimate of the m-th reference point is abbreviated as 1 = %cpcm.

[0066] Wenn ein persistenter Referenzpunkt keine Zuordnung im aktuellen Radarscan hat, wird er vom Zustandsvektor ausgeschlossen und die Kovarianzmatrix wird dementsprechend verkleinert. [0066] If a persistent reference point has no assignment in the current radar scan, it is excluded from the state vector and the covariance matrix is reduced accordingly.

[0067] Schließlich wird der für die Aktualisierung benutzte geschätzte Abstand folgendermaßen bestimmt: [0067] Finally, the estimated distance used for the update is determined as follows:

{13 {13

(14 (14

AKTUALISIERUNG MIT DOPPLERGESCHWINDIGKEITEN UPDATING AT DOPPLER SPEEDS

[0068] Das Radargerät 40 kann ferner dazu konfiguriert werden, relative radiale Geschwindigkeiten von zumindest einigen der Mehrzahl 50 von reflektierenden Objekten zu bestimmen. Die Bearbeitungsmittel 60 können ferner konfiguriert werden, die relativen radialen Geschwindigkeiten der reflektierenden Objekte zum Adjustieren der Schätzung für die Pose und die Geschwindigkeit des sich bewegenden Fahrzeugs 20 zu benutzen. [0068] The radar device 40 may be further configured to determine relative radial velocities of at least some of the plurality 50 of reflective objects. The processing means 60 may be further configured to use the relative radial velocities of the reflective objects to adjust the estimate for the pose and speed of the moving vehicle 20.

[0069] Zu diesem Zweck kann der Aktualisierungsschritt des EKF einen Vergleich der relativen radialen Geschwindigkeit des reflektierenden Objekts aus einem aktuellen Radarscan, wobei die relative radiale Geschwindigkeit durch das Radargerät 40 bestimmt wird, mit einer geschätzten relativen radialen Geschwindigkeit des reflektierenden Objekts einschließen, wobei die geschätzte relative radiale Geschwindigkeit auf der Basis der vom Radargerät 40 bestimmten Position des reflektierenden Objekts und der aktuellen Schätzungen der durch die IMU 30 bestimmten Pose und Geschwindigkeit des sich bewegenden Fahrzeugs bestimmt wird. [0069] For this purpose, the updating step of the EKF may include a comparison of the relative radial velocity of the reflecting object from a current radar scan, where the relative radial velocity is determined by the radar device 40, with an estimated relative radial velocity of the reflecting object, where the estimated relative radial velocity is determined based on the position of the reflecting object determined by the radar device 40 and the current estimates of the pose and velocity of the moving vehicle determined by the IMU 30.

[0070] Die Bearbeitungsmittel 60 können dazu konfiguriert werden, die geschätzte relative radiale Geschwindigkeit des reflektierenden Objekts durch Projizieren der aktuellen Schätzung der Geschwindigkeit des sich bewegenden Fahrzeugs 20 auf einen Richtungsvektor, der auf das reflektierende Objekt zeigt, unter Verwendung der Position des reflektierenden Objekts und der aktuellen Schätzungen der Pose und der Winkelgeschwindigkeit des sich bewegenden Fahrzeugs 20 zu bestimmen. Dies wird durch das folgende Messungsmodell ausgedrückt: [0070] The processing means 60 may be configured to determine the estimated relative radial velocity of the reflecting object by projecting the current estimate of the velocity of the moving vehicle 20 onto a direction vector pointing towards the reflecting object using the position of the reflecting object and the current estimates of the pose and angular velocity of the moving vehicle 20. This is expressed by the following measurement model:

En ÖRTS : Wa ST f & Rx N BT MOM eb xx x . a A En ÖRTS: Wa ST f & Rx N BT MOM eb xx x. a A

48 : x 5.23 48 : x 5.23

Rad bike

(zie X Pa} (zie X Pa}

[0071] Wie aus Gleichung (15) ersichtlich wird zum Schätzen der Geschwindigkeiten der erkannten Radar-3D-Punkte *v„; im aktuellen Radarscan zur Zeit t., die aktuelle Roboter-Ego-Ge-[0071] As can be seen from equation (15), to estimate the speeds of the detected radar 3D points *v„; in the current radar scan at time t., the current robot ego velocity

schwindigkeit aus dem IMU-Bezugssystem in das aktuelle Radarbezugssystem transformiert und anschließend auf den Richtungsvektor projiziert, der auf den entsprechenden 3D-Punkt zeigt. In der obigen Gleichung (15) ist r = ”r»; der im aktuellen Radarscan erkannte 3D-Punkt, ‚w die aktuelle Winkelgeschwindigkeit der IMU 30 im IMU- Bezugssystem, und Sv, ist die aktuelle lineare Geschwindigkeit der IMU im Navigationsbezugssystem. Zum Zurückweisen von Ausreißern kann ein Chi-Quadrat- Test auf das Residuum jeder Messung angewandt werden, wobei geprüft wird, ob der dem Residuum entsprechende Mahalanobis-Abstand in einem Intervall enthalten ist, das durch Schwellen definiert ist, die mit einem gewählten Prozentbereich der y?-Verteilung assozliiert sind. velocity from the IMU reference frame is transformed to the current radar reference frame and then projected onto the direction vector pointing to the corresponding 3D point. In equation (15) above, r = ”r”; the 3D point detected in the current radar scan, ‚w is the current angular velocity of the IMU 30 in the IMU reference frame, and Sv is the current linear velocity of the IMU in the navigation reference frame. To reject outliers, a chi-square test can be applied to the residual of each measurement, checking whether the Mahalanobis distance corresponding to the residual is contained in an interval defined by thresholds associated with a chosen percentage range of the y? distribution.

[0072] Abb. 7 zeigt ein Flussdiagramm, das ein Verfahren gemäß Ausführungsformen illustriert. Verfahren zum Bestimmen einer Pose, wobei die Pose eine Position und eine Ausrichtung des sich bewegenden Fahrzeugs, vorzugsweise eine 6D-Pose, und eine Geschwindigkeit des sich bewegenden Fahrzeugs einschließt. Das Verfahren umfasst das Bestimmen 100, vorzugsweise durch eine Inertialmesseinheit, IMU 30, die auf oder in dem sich bewegenden Fahrzeug 20 angeordnet ist, einer Beschleunigung und/oder einer Winkelgeschwindigkeit des sich bewegenden Fahrzeugs 20. Das Verfahren umfasst ferner das Bestimmen 110, vorzugsweise durch das auf oder in dem sich bewegenden Fahrzeug 20 angeordnete Radargerät 40, eines Radarscans, wobei der Radarscan Positionen einer Mehrzahl 50 von reflektierenden Objekten einschließt. Weiterhin umfasst das Verfahren das Bestimmen 120 einer Schätzung für die Pose und/oder einer Schätzung für die Geschwindigkeit des sich bewegenden Fahrzeugs 20 auf der Basis der Beschleunigung und/oder der Winkelgeschwindigkeit des sich bewegenden Fahrzeugs 20. Noch weiterhin umfasst das Verfahren das Adjustieren 130 der Schätzung für die Pose und/oder der Schätzung für die Geschwindigkeit des sich bewegenden Fahrzeugs 20 durch Verwendung von mehreren vergangenen Messungen der Positionen der Mehrzahl 50 von reflektierenden Objekten. [0072] Fig. 7 shows a flow chart illustrating a method according to embodiments. Method for determining a pose, wherein the pose includes a position and an orientation of the moving vehicle, preferably a 6D pose, and a speed of the moving vehicle. The method comprises determining 100, preferably by an inertial measurement unit, IMU 30, arranged on or in the moving vehicle 20, an acceleration and/or an angular velocity of the moving vehicle 20. The method further comprises determining 110, preferably by the radar device 40 arranged on or in the moving vehicle 20, a radar scan, wherein the radar scan includes positions of a plurality 50 of reflective objects. The method further includes determining 120 an estimate for the pose and/or an estimate for the speed of the moving vehicle 20 based on the acceleration and/or the angular velocity of the moving vehicle 20. Still further, the method includes adjusting 130 the estimate for the pose and/or the estimate for the speed of the moving vehicle 20 by using a plurality of past measurements of the positions of the plurality 50 of reflective objects.

[0073] Das Verfahren kann ferner das Bestimmen 140, vorzugsweise durch das Radargerät 40, der relativen radialen Geschwindigkeiten zumindest einiger der Mehrzahl 50 von reflektierenden Objekten umfassen. Das Adjustieren 130 der Schätzung für die Pose und/oder die Geschwindigkeit des sich bewegenden Fahrzeugs kann durch weitere Benutzung der relativen radialen Geschwindigkeiten zumindest einiger der Mehrzahl 50 von reflektierenden Objekten durchgeführt werden. [0073] The method may further comprise determining 140, preferably by the radar device 40, the relative radial velocities of at least some of the plurality 50 of reflective objects. Adjusting 130 the estimate for the pose and/or the speed of the moving vehicle may be performed by further using the relative radial velocities of at least some of the plurality 50 of reflective objects.

[0074] Im oben beschriebenen Verfahren kann das Adjustieren der Schätzung für die Pose und/ oder die Geschwindigkeit des sich bewegenden Fahrzeugs 20 mithilfe eines erweiterten KalmanFilter-(EKF)-Verfahrens durchgeführt werden. [0074] In the method described above, adjusting the estimate for the pose and/or the speed of the moving vehicle 20 can be performed using an extended Kalman filter (EKF) method.

[0075] Zusammengefasst werden in dem oben beschriebenen System und Verfahren der EKFZustand und seine Kovarianz jeweils gemäß Gleichung (2) und Gleichung (7) fortgepflanzt. Der Aktualisierungsschritt des eng-gekoppelten EKF kann drei Komponenten umfassen. Eine erste benutzt Abstände zu Punkten in den Geschichtselementen der Spuren verglichen mit aktuellen Radarmessungen, siehe Gleichungen (8) und (9). In einer zweiten werden Abstände zu persistenten Referenzpunkten mit aktuellen Radarmessungen verglichen, siehe Gleichungen (13) und (14). In einer dritten werden Dopplergeschwindigkeiten eingesetzt, reduziert auf den Satz von akzeptablen Punkten unter Verwendung einer 3-Punkt-RANSAC- Prozedur, wie in [1] ausgeführt. Für alle Komponenten kann die Zurückweisung von Ausreißern durch Benutzung des Chi-Quadrat-Tests angewandt werden. [0075] In summary, in the system and method described above, the EKF state and its covariance are propagated according to equation (2) and equation (7), respectively. The update step of the tightly coupled EKF may comprise three components. A first uses distances to points in the history elements of the tracks compared to current radar measurements, see equations (8) and (9). In a second, distances to persistent reference points are compared to current radar measurements, see equations (13) and (14). In a third, Doppler velocities are used, reduced to the set of acceptable points using a 3-point RANSAC procedure as outlined in [1]. For all components, rejection of outliers can be applied using the chi-square test.

EXPERIMENTE EXPERIMENTS

[0076] Im Folgenden werden der Aufbau und die Experimente, die zum Validieren des oben beschriebenen RIO-Verfahrens auf einer reellen Plattform mit den Daten von reellen Flügen benutzt wurden, und ebenso die Ergebnisse der Auswertung beschrieben. [0076] In the following, the setup and experiments used to validate the RIO method described above on a real platform with data from real flights are described, as well as the results of the evaluation.

[0077] Der für die Experimente benutzte Sensor ist ein leichtgewichtiges und preiswertes FMCWRadar, hergestellt von Texas Instruments, integriert auf einer Auswertungsplatine AWR1843BOOST, angebaut an ein UAV, ausgestattet mit einer USB-Schnittstelle und versorgt [0077] The sensor used for the experiments is a lightweight and inexpensive FMCW radar manufactured by Texas Instruments, integrated on an evaluation board AWR1843BOOST, mounted on a UAV, equipped with a USB interface and powered

mit 2,5 V. Das Frequenzspektrum von vom Radar erzeugten Chirps liegt zwischen fi = 77 GHz und f, = 81 GHz. Das Sichtfeld, FoV, ist 120° im Azimut und 30° in der Höhe. with 2.5 V. The frequency spectrum of chirps generated by the radar is between fi = 77 GHz and f, = 81 GHz. The field of view, FoV, is 120° in azimuth and 30° in elevation.

[0078] Messungen werden bei einer Rate von fın = 15 Hz aufgenommen. Das Radar ist an einer Extremität der Experimentierplattform befestigt, vorwärts ausgerichtet mit einer Neigung von ungefähr 45° bezogen auf die horizontale Ebene. Dies verbessert die Geschwindigkeitsablesungen im Vergleich zur Nadiransicht, während Punktmessungen am Boden und damit bei einem sinnvollen Abstand gehalten werden. Für Inertialmessungen wird eine IMU der Pixhawk-4-Flugsteuerungseinheit (FCU) mit einer Abtastrate von f;; = 200 Hz benutzt. [0078] Measurements are taken at a rate of fın = 15 Hz. The radar is mounted on an extremity of the experimental platform, facing forward with a tilt of approximately 45° with respect to the horizontal plane. This improves the velocity readings compared to the nadir view, while keeping point measurements on the ground and thus at a reasonable distance. For inertial measurements, an IMU of the Pixhawk-4 Flight Control Unit (FCU) is used with a sampling rate of f;; = 200 Hz.

[0079] Die Transformation zwischen den Radar- und IMU-Sensoren ist manuell kalibriert und wird als eine konstante räumliche Verschiebung im EKF benutzt. Die Anfangsnavigationszustände des Filters sind auf die Ground-Truth-Werte festgelegt. N wurde auf 7 festgelegt. Etwas willkürliches Durcheinander von Reflexionen wurde in die Szene eingebracht, weil die Testumgebung anderweitig ein ungestört sauberer Laborraum war. Keine Positionsinformation jeglicher Art von den hinzugefügten Objekten wurde gemessen oder in unserem Ansatz benutzt, außer was der Radarsensor an Bord von sich aus wahrgenommen hat. [0079] The transformation between the radar and IMU sensors is manually calibrated and used as a constant spatial displacement in the EKF. The initial navigation states of the filter are set to the ground truth values. N was set to 7. Some random clutter of reflections was introduced into the scene because the test environment was otherwise an undisturbed clean laboratory space. No position information of any kind from the added objects was measured or used in our approach, except what the onboard radar sensor sensed on its own.

[0080] Ein Bewegungserfassungssystem wird zum Aufzeichnen der Ground-Truth-Trajektorien benutzt. Während der Datenaufnahme wurden die Sensorablesungen von der IMU und dem Radar zusammen mit den Posen des UAV aufgezeichnet, die vom Bewegungserfassungssystem als Ground-Truth gestreamt werden. Das EKF-basierte RIO-Verfahren wird offline auf den aufgezeichneten Sensordaten auf einem Intel Core i7-10850H vPRO Laptop mit 16 GB RAM, in einem zweckspezifischen C++-Rahmensystem kompiliert mit gcc 9.4.0 bei einer Optimierungsstufe -O3, ausgeführt. Ausführungszeiten für die zuvor erwähnte Maschine sind in Tabelle II gezeigt und bestätigen die Echtzeitfähigkeit der Implementation. [0080] A motion capture system is used to record the ground truth trajectories. During data acquisition, the sensor readings from the IMU and radar were recorded along with the poses of the UAV, which are streamed from the motion capture system as ground truth. The EKF-based RIO method is executed offline on the recorded sensor data on an Intel Core i7-10850H vPRO laptop with 16 GB RAM, in a purpose-built C++ framework compiled with gcc 9.4.0 at an optimization level of -O3. Execution times for the aforementioned machine are shown in Table II and confirm the real-time capability of the implementation.

BEWERTUNG EVALUATION

[0081] Zur Bewertung des vorgestellten RIO-Ansatzes werden die Daten verwendet, die in einem Innenraum während sieben manuell kontrollierten UAV-Flügen aufgezeichnet wurden. Die geflogenen Trajektorien waren nicht im Voraus geplant und schlossen ausgeprägte Bewegungen in allen drei Dimensionen ein. Eine der ausgeführten Trajektorien ist in Abb. 5 zu sehen. Wir wählen, die Qualität unseres Schätzers unter Verwendung der Norm des mittleren absoluten Fehlers, MAE, der Position und der endgültigen Drift der Pose in Prozent (ohne Gierausrichtung) zu messen, um einen einfachen Vergleich mit dem Stand der Technik zu haben. In Abb. 6 ist der Mittelwert des ||MAE!|| über die geflogenen Trajektorien gezeigt. Der ||MAE|| nimmt stetig mit der geflogenen Distanz zu, während sich die Drift der Pose wie erwartet aufbaut. Zum Vergleich zeigen wir, dass der ||MAE|| um einen Faktor 2 gegenüber den in [21] präsentierten Ergebnissen reduziert ist, und um einen Faktor 4 gegenüber dem in [1] gezeigten Stand der Technik, wo nur ein FMCW-Radar und eine IMU benutzt werden und keine Annahmen über die Umgebung gemacht werden. [0081] To evaluate the presented RIO approach, the data recorded in an indoor environment during seven manually controlled UAV flights are used. The flown trajectories were not planned in advance and included pronounced movements in all three dimensions. One of the executed trajectories is shown in Fig. 5. We choose to measure the quality of our estimator using the norm of the mean absolute error, MAE, of the position and the final drift of the pose in percent (excluding yaw alignment) to have an easy comparison with the state of the art. In Fig. 6, the mean of the ||MAE!|| over the flown trajectories is shown. The ||MAE|| increases steadily with the flown distance, while the drift of the pose builds up as expected. For comparison, we show that the ||MAE|| is reduced by a factor of 2 compared to the results presented in [21], and by a factor of 4 compared to the state of the art shown in [1], where only an FMCW radar and an IMU are used and no assumptions about the environment are made.

[0082] Die auf dem Datensatz basierenden 1-c-Grenzen wachsen bis zu einem Wert von ungefähr o = 0,25 m an. Bezüglich der endgültigen Drift, wie in Tabelle | gezeigt, wird im Mittel 0,81 % auf Trajektorien, die von 127,5 m bis 175,0 m reichen, erreicht, im Gegensatz zu 5,0 %, die für eine 60 m lange geflogene Trajektorie in [1] für einen ähnlichen Aufbau wie hier beschrieben, der nur aus einem FMCW-Radar und einer IMU besteht, berichtet wird. Autoren in [1], [11] berichten auch die endgültigen Driftwerte für dieselben Aufbauten, jedoch zusätzlich mit einem Barometersensor augmentiert, um die vertikale Drift zu reduzieren - selbst in diesem Fall bleiben die hier beschriebenen Ergebnisse, ohne einen zusätzlichen Sensor, vergleichbar - 0,81 % (vorliegende Offenbarung) gegenüber 0,60 % in [1] und 0,36 % in [11]. [0082] The 1-c limits based on the dataset grow up to a value of approximately o = 0.25 m. Regarding the final drift, as shown in Table |, on average 0.81% is achieved on trajectories ranging from 127.5 m to 175.0 m, in contrast to 5.0% reported for a 60 m long flown trajectory in [1] for a similar setup as described here, consisting only of an FMCW radar and an IMU. Authors in [1], [11] also report the final drift values for the same setups, but additionally augmented with a barometer sensor to reduce the vertical drift - even in this case, the results described here, without an additional sensor, remain comparable - 0.81% (present disclosure) versus 0.60% in [1] and 0.36% in [11].

[0083] Interessanterweise ist der Minimumwert der endgültigen Drift in den in der vorliegenden Offenbarung beschriebenen Experimenten - 0,17 % - noch niedriger als der niedrigste in [1], [11] berichtete Wert, der beim Fliegen derselben Distanz aber mit zusätzlichen Sensoren und viel geringerer Anregung in ihrem Fall erhalten wurde. [0083] Interestingly, the minimum value of the final drift in the experiments described in the present disclosure - 0.17% - is even lower than the lowest value reported in [1], [11], obtained when flying the same distance but with additional sensors and much lower excitation in their case.

[0084] Verglichen mit einer endgültigen Drift von 3,32 % für eine in der Hand gehaltene Trajek-[0084] Compared to a final drift of 3.32% for a hand-held trajectory

torie von 116,4 m mit geringer Anregung in [11], übertrifft das Verfahren der vorliegenden Offenbarung jenes um einen Faktor 4 in einem reellen UAV-Flug mit hoher Anregung. torie of 116.4 m with low excitation in [11], the method of the present disclosure outperforms that by a factor of 4 in a real UAV flight with high excitation.

TABELLE | Über alle geflogenen Trajektorien gesammelte Maße TABLE | Measures collected over all flown trajectories

1 127,5 0,90 0,17 2 150,5 1,10 1,62 3 158,4 0,68 0,80 4 160,4 0,71 0,61 5 166,7 0,59 0,78 6 168,1 0,84 0,91 7 175,0 0,45 0,81 1 127.5 0.90 0.17 2 150.5 1.10 1.62 3 158.4 0.68 0.80 4 160.4 0.71 0.61 5 166.7 0.59 0.78 6,168.1 0.84 0.91 7,175.0 0.45 0.81

Mittelwert 0,75 0,81 mean 0.75 0.81

TABELLE II TABLE II

Ausführungszeiten für eine einzelne Trajektorie execution times for a single trajectory

Gemittelte Zeit [ms] Average time [ms]

Fortpflanzung | Aktualisierung Dauer [s] Echtzeitfaktor Trajektorie 7 0,08 2,07 436 25,36 Propagation | Update Duration [s] Real-time factor Trajectory 7 0.08 2.07 436 25.36

ZITATENLISTE QUOTE LIST

[0085] [1] C. Doer und G. F. Trommer, “An EKF Based Approach to Radar Inertial Odometry”, in 2020 IEEE International Conference on Multisensor Fusion and Integration for Intelligent Systems (MFI)}, 2020, pp. 152-159. [0085] [1] C. Doer and G. F. Trommer, “An EKF Based Approach to Radar Inertial Odometry,” in 2020 IEEE International Conference on Multisensor Fusion and Integration for Intelligent Systems (MFI)}, 2020, pp. 152-159.

[0086] [2] C. Doer und G. F. Trommer, “Yaw aided radar inertial odometry using manhattan world assumptions,” in 2021 28" Saint Petersburg International Conference on Integrated Navigation Systems (ICINS), 2021, pp. 1-9. [0086] [2] C. Doer and G. F. Trommer, “Yaw aided radar inertial odometry using manhattan world assumptions,” in 2021 28" Saint Petersburg International Conference on Integrated Navigation Systems (ICINS), 2021, pp. 1-9.

[0087] [3] A. Kramer, C. Stahoviak, A. Santamaria-Navarro, A.-A. Agha-Mohammadi und C. Heckman, “Radar-inertial egovelocity estimation for visually degraded environments,” in 2020 IEEE International Conference on Robotics and Automation (ICRA), IEEE, 2020, pp. 5739-5746. [0087] [3] A. Kramer, C. Stahoviak, A. Santamaria-Navarro, A.-A. Agha-Mohammadi and C. Heckman, “Radar-inertial egovelocity estimation for visually degraded environments,” in 2020 IEEE International Conference on Robotics and Automation (ICRA), IEEE, 2020, pp. 5739-5746.

[0088] [4] Y. Almalioglu, M. Turan, C. X. Lu, N. Trigoni und A. Markham, “Milli-rio: Egomotion estimation with low-cost millimetre-wave radar,” IEEE Sensors Journal, vol. 21, no. 3, pp. 33143323, 2020. [4] Y. Almalioglu, M. Turan, C. X. Lu, N. Trigoni and A. Markham, “Milli-rio: Egomotion estimation with low-cost millimeter-wave radar,” IEEE Sensors Journal, vol. 21, no. 3, pp. 33143323, 2020.

[0089] [5] P. Maybeck, Stochastic Models, Estimation and Control, ser. Mathematics in science and engineering. Academic Press, 1979, no. pt. 1. [0089] [5] P. Maybeck, Stochastic Models, Estimation and Control, ser. Mathematics in science and engineering. Academic Press, 1979, no. pt. 1.

[0090] [6] J. Michalczyk, R. Jung und S. Weiss, “Radar-inertial odometry,” in 2022 International Conference on Intelligent Robots and Systems (IROS). [0090] [6] J. Michalczyk, R. Jung and S. Weiss, “Radar-inertial odometry,” in 2022 International Conference on Intelligent Robots and Systems (IROS).

[0091] [7] S. I. Roumeliotis und J.W. Burdick, “Stochastic cloning: A generalized framework for processing relative state measurements,” in Proceedings 2002 IEEE International Conference on Robotics and Automation (Cat. No. 02CH37292), vol. 2. IEEE, 2002, pp. 1788-1795. [0091] [7] S.I. Roumeliotis and J.W. Burdick, “Stochastic cloning: A generalized framework for processing relative state measurements,” in Proceedings 2002 IEEE International Conference on Robotics and Automation (Cat. No. 02CH37292), vol. 2. IEEE, 2002, pp. 1788-1795.

[0092] [8] S. Weiss und R. Siegwart, “Real-time metric state estimation for modular visioninertial systems,” in 2011 IEEE International Conference on Robotics and Automation, 2011, pp. 4531-4537. [0092] [8] S. Weiss and R. Siegwart, “Real-time metric state estimation for modular visioninertial systems,” in 2011 IEEE International Conference on Robotics and Automation, 2011, pp. 4531-4537.

[0093] [9] J. Sola, A. Monin, M. Devy und T. Lemaire, “Undelayed initialization in bearing only slam,” in 2005 IEEE/RSJ International Conference on Intelligent Robots and Systems, 2005, pp. 2499-2504. [0093] [9] J. Sola, A. Monin, M. Devy and T. Lemaire, “Undelayed initialization in bearing only slam,” in 2005 IEEE/RSJ International Conference on Intelligent Robots and Systems, 2005, pp. 2499- 2504.

[0094] [10] J. Michalczyk, R. Jung und S. Weiss, “Tightly-coupled ekf-based radar-inertial odometry,” in 2022 International Conference on Intelligent Robots and Systems (IROS). IEEE, Accepted June 2022. [0094] [10] J. Michalczyk, R. Jung and S. Weiss, “Tightly-coupled ekf-based radar-inertial odometry,” in 2022 International Conference on Intelligent Robots and Systems (IROS). IEEE, Accepted June 2022.

[0095] [11] ©C. Doer und G. F. Trommer, “Radar inertial odometry with online calibration,” in 2020 European Navigation Conference (ENC). IEEE, 2020, pp. 1-10. [0095] [11] ©C. Doer and G. F. Trommer, “Radar inertial odometry with online calibration,” in 2020 European Navigation Conference (ENC). IEEE, 2020, pp. 1-10.

BEZUGSZEICHEN REFERENCE SIGN

10 Gerät 10 devices

20 sich bewegendes Fahrzeug 20 moving vehicle

30 Inertialmesseinheit, IMU 30 Inertial Measurement Unit, IMU

40 Radargerät 40 radar devices

50 Mehrzahl von reflektierenden Objekten 60 Bearbeitungsmittel 50 Plurality of reflective objects 60 Processing equipment

P. reflektierendes Objekt P. reflecting object

Claims

claims

1. A device (10) for placement on or in a moving vehicle (20), the device (10) being configured to determine a pose of the moving vehicle (20), the pose including a position and an orientation of the moving vehicle (20), and/or a speed of the moving vehicle (20), the device (10) including:

an inertial measurement unit (IMU) (30) configured to determine an acceleration and/or an angular velocity of the moving vehicle (20);

a radar device (40) configured to determine a radar scan, the radar scan including positions of a plurality of reflective objects (50); and

Processing means (60) configured to determine an estimate for the pose and/or an estimate for the speed of the moving vehicle (20) based on the acceleration and/or the angular velocity of the moving vehicle (20) determined by the IMU (30);

wherein the processing means (60) is further configured to use a plurality of past measurements of the positions of the plurality of reflective objects (50) from the radar device (40) to adjust the estimate for the pose and/or the estimate for the speed of the moving vehicle (20).

2. Device (10) according to claim 1, wherein

the radar device (40) is further configured to determine relative radial velocities of at least some of the plurality of reflective objects (50); and

the processing means (60) is further configured to use the relative radial velocities to adjust the estimate for the pose and the velocity of the moving vehicle (20).

3. Device (10) according to one of claims 1 or 2, wherein the processing means (60) are configured to adjust the estimate for the pose and the speed of the moving vehicle (20) using an extended Kalman filter (EKF) method.

4. The device (10) of claim 3, wherein a state vector of the EKF comprises the pose and/or the velocity of the moving vehicle (20) at a current time.

5. The apparatus (10) of claim 4, wherein the state vector of the EKF further comprises estimates for the poses of the moving vehicle (20) corresponding to a predetermined number N of times of several consecutive past radar scans.

6. Device (10) according to one of claims 1 to 5, wherein the processing means (60) is further configured to perform an assignment of the plurality of reflective objects (50) in consecutive radar scans and to initialize a reflective object (P:) as a persistent reference point in case the reflective object (Pi) is continuously assigned between a predetermined number of consecutive radar scans, wherein the predetermined number of consecutive radar scans is preferably equal to N.

7. The device (10) of claim 6, wherein the processing means (60) is configured to perform the mapping by determining correspondences between the successive radar scans and aligning the successive radar scans using the estimate for the pose of the moving vehicle (20) to map the positions of the plurality of reflective objects (50) in the successive radar scans.

8. Device (10) according to one of claims 6 or 7, wherein the or a state vector of the EKF comprises a position of at least one persistent reference point.

9. Device (10) according to claim 8, wherein an updating step of the EKF comprises a comparison of a distance to the persistent reference point, the distance being determined from a current radar

10th

11th

12th

13th

14th

AT 18 366 U1 2024-12-15

scan is determined, with an estimated distance to the persistent reference point, wherein the estimated distance is determined from a radar scan at a time of initialization of the persistent reference point and two poses of the moving vehicle (20), wherein the two poses of the moving vehicle (20) correspond respectively to the time of initialization of the reference point and the time of the current radar scan.

Device (10) according to one of claims 6 to 9, wherein the processing means (60) are further configured to:

Determining a set of tracks, each of the tracks corresponding to a reflecting object (P:) whose position has been assigned between successive radar scans, and each of the tracks containing the positions of the corresponding reflecting objects (P;) for a predetermined number of successive radar scans, wherein a maximum predetermined number of successive radar scans is preferably equal to N.

Device (10) according to claim 10, wherein the or an updating step of the EKF includes a comparison of a current distance to a reflecting object (P) in a lane, the current distance being determined from a position of the reflecting object (Pi) in a current radar scan, with an estimated distance to the reflecting object (P:) in the lane, the estimated distance being determined from a position of the reflecting object (P:) in a past radar scan and two poses of the moving vehicle (20), the two poses of the moving vehicle (20) corresponding to the times of the past radar scan and the current radar scan, respectively.

Device (10) according to one of claims 3 to 10, wherein the or an updating step of the EKF includes a comparison of a relative radial velocity of a reflecting object (Pi) from a current radar scan, the relative radial velocity being determined by the radar device, with an estimated relative radial velocity of the reflecting object (P;), the estimated relative radial velocity being determined based on the position of the reflecting object (P:) determined by the radar device (40) and the current estimates of the pose and the speed of the moving vehicle (20) determined by the IMU (30).

A method for determining a pose, the pose including a position and an orientation of a moving vehicle (20) and/or a speed of the moving vehicle (20), the method comprising:

Determining (100), preferably by an inertial measurement unit (IMU) (30) arranged on the moving vehicle, an acceleration and/or an angular velocity of the moving vehicle (20);

Determining (110), preferably by a radar device (40) arranged on or in the moving vehicle (20), a radar scan, the radar scan including positions of a plurality of reflective objects (50);

Determining (120) an estimate for the pose and/or an estimate for the speed of the moving vehicle (20) based on the acceleration and/or the angular velocity of the moving vehicle (20); and

Adjusting (130) the estimate for the pose and/or the estimate for the speed of the moving vehicle (20) by using a plurality of past measurements of the positions of the plurality of reflective objects (50).

The method of claim 13, further comprising:

Determining (140), preferably by the radar device (40), the relative radial velocities of at least some of the plurality (50) of reflecting objects, wherein

Adjusting (130) the estimate for the pose and/or the speed of the moving vehicle (20) by further using the relative radial speed

abilities are carried out.

15. The method according to any one of claims 13 or 14, wherein

Adjusting the estimate (130) for the pose and/or the speed of the moving vehicle (20) is performed using an extended Kalman filter (EKF) method.

4 sheets of drawings