DE102022208972A1 - Method for roll angle and pitch angle error compensation in a terrain height measurement with a LiDAR sensor system - Google Patents

Abstract

Translated fromGerman

Verfahren zur Roll- und Nickfehlerkompensation bei einer Geländehöhenmessung mit einem LiDAR-Sensorsystem (20) mit zumindest nachfolgenden Verfahrensschritten:
a) Mithilfe einer korrigierten Pose χ_k wird eine neue Poseninformation χ_k|k-1 prognostiziert,
b) aus der gemäß a) prognostizierten Pose χ_k|k-1 und einer Höhenkarte m_i der Umgebung werden LiDAR-Messwerte h_r,i geschätzt,
c) mithilfe der Differenz der gemäß b) geschätzten LiDAR-Messwerte h_r,i und gemessenen oder interpolierten LiDAR-Messwerte z_r,i wird die prognostizierte Pose χ_k|k-1 gemäß Verfahrensschritt a) korrigiert.

Method for rolling and pitching error compensation in a terrain height measurement with a LiDAR sensor system (20) with at least the following method steps:
a) Using a corrected pose χ_k, new pose information χ_k|k-1 is predicted,
b) LiDAR measurement values h_r ,i are estimated from the pose χ_k|k-1 predicted according to a) and a height map m_i of the environment,
c) using the difference between the LiDAR measurement values h_r,i estimated according to b) and the measured or interpolated LiDAR measurement values z_r,i, the predicted pose χ_k|k-1 is corrected according to method step a).

Description

Translated fromGerman

Technisches GebietTechnical area

Die Erfindung bezieht sich auf ein Verfahren zur Rollwinkel- und Nickwinkelfehlerkompensation bei einer Geländehöhenmessung mittels eines LiDAR-Sensorsystems.The invention relates to a method for roll angle and pitch angle error compensation in a terrain height measurement using a LiDAR sensor system.

Stand der TechnikState of the art

US 2013/0041549 A1 bezieht sich auf ein Fahrzeugsteuerungssystem mit einem Controller und einer Datenbank mit räumlichen Daten, durch welche räumliche Daten dem Controller zur Verfügung gestellt werden können. Die räumlichen Daten, die von der Datenbank an den Controller übermittelt werden, können Bilder enthalten, die von einem optischen Sensor-Subsystem aufgenommen wurden, zusätzlich zu anderen Daten, die durch eine Vielzahl von anderen Sensortypen, beispielsweise eines GNSS- oder eines Trägheitsmesssystems herrühren können. Die räumlichen Daten, die vom Controller erhalten werden, stellen zumindest einen Teil des Inputs dar, den der Controller zur Steuerung des Fahrzeugs erhält. Durch die Lösung gemäßUS 2013/0041549 A1 können dem Fahrzeugsteuerungssystem Formationen vorgeführt werden, die es ermöglichen, das Fahrzeug unabhängig von den Raumdaten zu führen. Das Fahrzeugsteuerungssystem umfasst einen Aufgaben-Pfad-Generator, eine Datenbank, in der Raumdaten abgelegt und zur Verfügung gestellt werden, und zumindest einen externen Raumdatenempfänger sowie ein Kompensationsmodul, einen Positionsfehlergenerator sowie diverse Aktoren zur Steuerung des Fahrzeugs.US 2013/0041549 A1 refers to a vehicle control system with a controller and a database with spatial data through which spatial data can be made available to the controller. The spatial data transmitted from the database to the controller may include images captured by an optical sensor subsystem, in addition to other data that may come from a variety of other sensor types, such as a GNSS or an inertial measurement system . The spatial data received from the controller represents at least part of the input that the controller receives to control the vehicle. Through the solution according to US 2013/0041549 A1 Formations can be presented to the vehicle control system, allowing the vehicle to be guided independently of the spatial data. The vehicle control system includes a task path generator, a database in which spatial data is stored and made available, and at least one external spatial data receiver as well as a compensation module, a position error generator and various actuators for controlling the vehicle.

US 2014/0270744 A bezieht sich auf ein aktives Stabilisationssystem und ein Verfahren zur Korrektur einer Blickrichtung einer Kamera zur Kompensation von translatorischen Bewegungen der Kamera. Das System stabilisiert die Blickrichtung einer Kamera in Übereinstimmung mit einem geforderten Blickwinkel. Ein Abstand von der Kamera zu einem Ziel wird bestimmt, ferner werden eine oder mehrere translatorische Messungen in Zusammenhang mit einer translatorischen Bewegung der Kamera abgeleitet. Es wird ein Korrekturupdate berechnet als Funktion zumindest des Abstands und der einen oder mehreren translatorischen Bewegungen. Der Blickwinkel der Kamera wird basierend auf einem Korrekturupdate adjustiert, um das zu filmende Objekt innerhalb des Blickfelds der Kamera zu halten.US 2014/0270744 A refers to an active stabilization system and a method for correcting a viewing direction of a camera to compensate for translational movements of the camera. The system stabilizes the viewing direction of a camera in accordance with a required viewing angle. A distance from the camera to a target is determined, and one or more translational measurements are derived in connection with a translational movement of the camera. A correction update is calculated as a function of at least the distance and the one or more translational movements. The camera's angle of view is adjusted based on a correction update to keep the object being filmed within the camera's field of view.

Zur Messung des Höhenprofils, insbesondere der Umgebung eines Fahrzeugs, ist eine genaue Erfassung desselben erforderlich. Mithilfe beispielsweise eines LiDAR-Messsystems werden zum Beispiel Neigungsmodellwerte eines Kalman-Filters korrigiert. Eine extrinsische Kalibrierung zwischen dem LiDAR-Messsystem und einer Inertialsensorik erfolgt im laufenden Betrieb im Wesentlichen auf kontinuierliche Art und Weise. Dadurch werden Zeit und Kosten für eine initiale und für regelmäßige Kalibrierungen eingespart.In order to measure the height profile, in particular the surroundings of a vehicle, an accurate recording of the same is required. With the help of a LiDAR measuring system, for example, inclination model values of a Kalman filter are corrected. An extrinsic calibration between the LiDAR measuring system and an inertial sensor system takes place essentially in a continuous manner during ongoing operation. This saves time and costs for initial and regular calibrations.

Darstellung der ErfindungPresentation of the invention

Die Erfindung offenbart ein Verfahren zur Rollwinkel - und Nickwinkelfehlerkompensation bei einer Geländehöhenmessung mit einem LiDAR-Sensorsystem mit zumindest nachfolgenden Verfahrensschritten:

1. a) Mithilfe einer korrigierten Pose χ_k wird eine neue Poseninformation χ_k|k-1 prognostiziert,
2. b) aus der gemäß a) prognostizierten Pose χ_k|k-1 und einer Höhenkarte m_i der Umgebung werden LiDAR-Messwerte h_r,i geschätzt,
3. c) mithilfe der Differenz der gemäß b) geschätzten LiDAR-Messwerte h_r,i und gemessenen oder interpolierten LiDAR-Messwerte z_r,i wird die prognostizierte Pose χ_k|k-1 gemäß Verfahrensschritt a) korrigiert.

The invention discloses a method for roll angle and pitch angle error compensation in a terrain height measurement with a LiDAR sensor system with at least the following method steps:

1. a) Using a corrected pose χ_k, new pose information χ_k|k-1 is predicted,
2. b) LiDAR measurement values h_r ,i are estimated from the pose χ_k|k-1 predicted according to a) and a height map m_i of the environment,
3. c) using the difference between the LiDAR measurement values h_r,i estimated according to b) and the measured or interpolated LiDAR measurement values z_r,i, the predicted pose χ_k|k-1 is corrected according to method step a).

Durch das erfindungsgemäße Verfahren kann in vorteilhafter Weise erreicht werden, dass eine preisgünstigere Sensorik eingesetzt werden kann, die größere Toleranzen aufweist, die durch das erfindungsgemäße Verfahren kompensiert werden können.The method according to the invention can advantageously achieve that a cheaper sensor system can be used which has larger tolerances, which can be compensated for by the method according to the invention.

In vorteilhafter Weiterbildung des erfindungsgemäß vorgeschlagenen Verfahrens wird gemäß Verfahrensschritt a) ein Prädiktionsschritt durchlaufen mit$$x_{k|k-1}=f\left(x_{k-1},u_{k-1}\right)$$

und mit$$P_{k|k-1}=F_{k-1}{P}_{k-1}{F}_{k-1}^T+B_{k-1}{Q}_{k-1}{B}_{k-1}^T$$

wobei mit einer korrespondierenden Kovarianz P_k eine korrespondierende neue Kovarianz P_k|k-1 prognostiziert wird.In an advantageous development of the method proposed according to the invention, a prediction step is carried out according to method step a).$$x_{k|k-1}=f\left(x_{k-1},u_{k-1}\right)$$

and with$$P_{k|k-1}=F_{k-1}{P}_{k-1}{F}_{k-1}^T+b_{k-1}{Q}_{k-1}{b}_{k-1}^T$$

whereby a corresponding new covariance P_k|k-1 is predicted with a corresponding covariance P_k .

In vorteilhafter Ausgestaltung des Verfahrens wird gemäß Verfahrensschritt b) für die Berechnung eines LiDAR-Sensormodells h_r,i und eine Auswahl von LiDAR-Strahlen, welche auf einen Prüfpunkt auf einer ausgewählten horizontalen, ebenen Fläche in möglichst großer Entfernung gerichtet sind, eine Drehmatrix gemäß$$T_{EV}=\left(\begin{array}{ccc}\text{cos}\left(\psi \right)\text{cos}\left(\theta \right)& \text{cos}\left(\theta \right)\text{sin}\left(\psi \right)& -\text{sin}\left(\theta \right)\\ \text{cos}\left(\psi \right)\text{sin}\left(\varphi \right)\text{sin}\left(\theta \right)-\text{cos}\left(\varphi \right)\text{sin}\left(\psi \right)& \text{cos}\left(\varphi \right)\text{cos}\left(\psi \right)+\text{sin}\left(\varphi \right)\text{sin}\left(\psi \right)\text{sin}\left(\theta \right)& \text{cos}\left(\theta \right)\text{sin}\left(\varphi \right)\\ \text{sin}\left(\varphi \right)\text{sin}\left(\psi \right)+\text{cos}\left(\varphi \right)\text{cos}\left(\psi \right)\text{sin}\left(\theta \right)& \text{cos}\left(\varphi \right)\text{sin}\left(\psi \right)\text{sin}\left(\theta \right)-\text{cos}\left(\psi \right)\text{sin}\left(\varphi \right)& \text{cos}\left(\varphi \right)\text{cos}\left(\theta \right)\end{array}\right)$$

mit Rollwinkel ϕ, Nickwinkel θ und Gierwinkel ip des LiDAR-Sensorsystems bezüglich eines Weltkoordinatensystems sowie Abständen r_x, r_y und r_z des Prüfpunkts in LiDAR-Koordinaten aus den Koordinaten des Prüfpunkts m_i = [m_x m_y m_z]^T in Weltkoordinaten und in den Koordinaten des LiDARs p_i = [p_x p_y p_z]^T in Weltkoordinaten berechnet. Die Abstände r_x, r_y und r_z ergeben sich aus (4) gemäß:$$\left(\begin{array}{c}{r}_x\\ r_y\\ r_z\end{array}\right)=r\left(x\right)=T_{EV}\left(\left(\begin{array}{c}{m}_x\\ m_y\\ m_z\end{array}\right)-\left(\begin{array}{c}{p}_x\\ p_y\\ p_z\end{array}\right)\right)$$

In an advantageous embodiment of the method, according to method step b), a rotation matrix is used for the calculation of a LiDAR sensor model h_r,i and a selection of LiDAR beams which are directed at a test point on a selected horizontal, flat surface at the greatest possible distance$$T_{Ev}=\left(\begin{array}{ccc}\text{cos}\left(\psi \right)\text{cos}\left(\theta \right)& \text{cos}\left(\theta \right)\text{sin}\left(\psi \right)& -\text{sin}\left(\theta \right)\\ \text{cos}\left(\psi \right)\text{sin}\left(\varphi \right)\text{sin}\left(\theta \right)-\text{cos}\left(\varphi \right)\text{sin}\left(\psi \right)& \text{cos}\left(\varphi \right)\text{cos}\left(\psi \right)+\text{sin}\left(\varphi \right)\text{sin}\left(\psi \right)\text{sin}\left(\theta \right)& \text{cos}\left(\theta \right)\text{sin}\left(\varphi \right)\\ \text{sin}\left(\varphi \right)\text{sin}\left(\psi \right)+\text{cos}\left(\varphi \right)\text{cos}\left(\psi \right)\text{sin}\left(\theta \right)& \text{cos}\left(\varphi \right)\text{sin}\left(\psi \right)\text{sin}\left(\theta \right)-\text{cos}\left(\psi \right)\text{sin}\left(\varphi \right)& \text{cos}\left(\varphi \right)\text{cos}\left(\theta \right)\end{array}\right)$$

with roll angle ϕ, pitch angle θ and yaw angle ip of the LiDAR sensor system with respect to a world coordinate system as well as distances r_x , r_y and r_z of the test point in LiDAR coordinates from the coordinates of the test point m_i = [m_x m_y m_z ]^T in World coordinates and in the coordinates of the LiDAR p_i = [p_x p_y p_z ]^T in world coordinates. The distances r_x ,_ry and r_z result from (4) according to:$$\left(\begin{array}{c}{r}_x\\ r_y\\ r_{\mathrm{e.g}}\end{array}\right)=r\left(x\right)=T_{Ev}\left(\left(\begin{array}{c}{m}_x\\ m_y\\ m_{\mathrm{e.g}}\end{array}\right)-\left(\begin{array}{c}{p}_x\\ p_y\\ p_{\mathrm{e.g}}\end{array}\right)\right)$$

In weiterer Ausgestaltung des erfindungsgemäßen Verfahrens wird gemäß Verfahrensschritt b) das LiDAR-Sensormodell hr,i(χ) eines Laserstrahls i zu dem Prüfpunkt aus Abständen r = [r_x r_y r_z]^T des Prüfpunkts in LiDAR-Koordinaten berechnet gemäß$$h_{r,i}\left(r\right)=\sqrt{r_x^2+r_y^2+r_z^2}$$

In a further embodiment of the method according to the invention, according to method step b), the LiDAR sensor model hr,i(χ) of a laser beam i to the test point is calculated from distances r = [r_x r_y r_z ]^T of the test point in LiDAR coordinates according to$$H_{r,i}\left(r\right)=\sqrt{r_x^2+r_y^2+r_{\mathrm{e.g}}^2}$$

In Weiterbildung des erfindungsgemäß vorgeschlagenen Verfahrens sind Drehwinkel und Koordinaten des LiDARs Teil eines Zustandsvektors χ_k|k-1 = [p_x p_y p_z ϕ, θ und ψ]^T und die Beobachtungsmatrix gemäß$$H_{r,i,k}=\frac{\partial h_{r,i}\left(r\right)}{\partial x}=\frac{\partial h\left(r\right)}{\partial r}\frac{\partial r\left(x\right)}{\partial x}=\frac{\partial h\left(r\right)}{\partial r}\left(\frac{\partial T_{EV}\left(x\right)}{\partial x}\left(m_i-p_i\right)+T_{EV}\frac{\partial \left(m_i-p_i\right)}{\partial x}\right)$$

In a further development of the method proposed according to the invention, the rotation angle and coordinates of the LiDAR are part of a state vector χ_k|k-1 = [p_x p_y p_z ϕ, θ and ψ]^T and the observation matrix according to$$H_{r,i,k}=\frac{\partial H_{r,i}\left(r\right)}{\partial x}=\frac{\partial H\left(r\right)}{\partial r}\frac{\partial r\left(x\right)}{\partial x}=\frac{\partial H\left(r\right)}{\partial r}\left(\frac{\partial T_{Ev}\left(x\right)}{\partial x}\left(m_i-p_i\right)+T_{Ev}\frac{\partial \left(m_i-p_i\right)}{\partial x}\right)$$

Beim erfindungsgemäßen Verfahren erfolgt die Korrektur nach Verfahrensschritt c) gemäß$$K_i=P_{i|i-1}{H}_{r,i}^T{\left(H_{r,i}{P}_{i|i-1}{H}_{r,i}^T+R_i\right)}^{-1}$$

$$x_i=x_{i|i-1}+K_i\left(z_{r,i}-h_{r,i}\right)$$

$$P_i=\left(I-K_i{H}_{r,i}\right)P_{i|i-1}$$

In the method according to the invention, the correction is carried out according to method step c).$$K_i=P_{i|i-1}{H}_{r,i}^T{\left(H_{r,i}{P}_{i|i-1}{H}_{r,i}^T+R_i\right)}^{-1}$$

$$x_i=x_{i|i-1}+K_i\left({\mathrm{e.g}}_{r,i}-H_{r,i}\right)$$

$$P_i=\left(I-K_i{H}_{r,i}\right)P_{i|i-1}$$

Beim vorgeschlagenen Verfahren werden in Verfahrensschritt c) ein oder mehrere Messpunkte i mit zugehörigen Größen H_r,i, z_r,i und h_r,i innerhalb eines Zeitschritts k berechnet.In the proposed method, in method step c), one or more measuring points i with associated variables H_r,i , z_r,i and h_r,i are calculated within a time step k.

In Weiterbildung des erfindungsgemäßen Verfahrens wird eine Korrektur gemäß Verfahrensschritt c) auf jeden Messpunkt i einzeln durchgeführt, gemäß$$x_i=x_{i|i-1}+K_i\left(z_{r,i}-h_{r,i}\right)$$

$$P_i=\left(I-K_i{H}_{r,i}\right)P_{i|i-1}$$

$$K_k=P_{k|k-1}{H}_{r,k}^T{\left(H_{r,k}{P}_{k|k-1}{H}_{r,k}^T+R_k\right)}^{-1}$$

und mehrfach für jeden einzelnen Messpunkt i einzeln durchlaufen.In a further development of the method according to the invention, a correction according to method step c) is carried out individually on each measuring point i, according to$$x_i=x_{i|i-1}+K_i\left({\mathrm{e.g}}_{r,i}-H_{r,i}\right)$$

$$P_i=\left(I-K_i{H}_{r,i}\right)P_{i|i-1}$$

$$K_k=P_{k|k-1}{H}_{r,k}^T{\left(H_{r,k}{P}_{k|k-1}{H}_{r,k}^T+R_k\right)}^{-1}$$

and run through it several times for each individual measuring point i.

Beim erfindungsgemäß vorgeschlagenen Verfahren werden zur Korrektur gemäß Verfahrensschritt c) n Messpunkte i eines Zeitschritts k in einen Vektor z_r,k = [z_r,1 z_r,2 ... z_r,n]^T, die LiDAR-Modellwerte zu einem Vektor h_r,k = [h_r,1 h_r,2 ... h_r,n]^T und der Jakobimatrix H_r,k = [H_r,1 H_r,2 ... H_r,n]^T und ein Messrauschen R_k = diag(R₁, R₂, ... R_n) gemäß$$K_k=P_{k|k-1}{H}_{r,k}^T{\left(H_{r,k}{P}_{k|k-1}{H}_{r,k}^T+R_k\right)}^{-1}$$

$$x_k=x_{k|k-1}+K_k\left(z_{r,k}-h_{r,k}\right)$$

$$P_k=\left(I-K_k{H}_{r,k}\right)P_{k|k-1}$$

berechnet.In the method proposed according to the invention, for correction according to method step c), n measuring points i of a time step k are converted into a vector z_r,k = [z_r,1 z_r,2 ... z_r,n ]^T , the LiDAR model values into one Vector h_r,k = [h_r,1 h_r,2 ... h_r,n ]^T and the Jacobian matrix H_r,k = [H_r,1 H_r,2 ... H_r,n ]^T and a measurement noise R_k = diag(R₁ , R₂ , ... R_n ) according to$$K_k=P_{k|k-1}{H}_{r,k}^T{\left(H_{r,k}{P}_{k|k-1}{H}_{r,k}^T+R_k\right)}^{-1}$$

$$x_k=x_{k|k-1}+K_k\left({\mathrm{e.g}}_{r,k}-H_{r,k}\right)$$

$$P_k=\left(I-K_k{H}_{r,k}\right)P_{k|k-1}$$

calculated.

In Weiterbildung des erfindungsgemäßen Verfahrens können anstelle von Eulerwinkeln auch Quaternionen für die Berechnung gewählt werden. Beim vorgeschlagenen Verfahren kann anstelle eines LiDAR-Sensorsystems eine Tiefenkamera, insbesondere eine Stereo-Kamera oder eine Time-of-Flight(ToF)-Kamera, eingesetzt werden.In a further development of the method according to the invention, quaternions can also be selected for the calculation instead of Euler angles. In the proposed method, a depth camera, in particular a stereo camera or a time-of-flight (ToF) camera, can be used instead of a LiDAR sensor system.

Vorteile der ErfindungAdvantages of the invention

Durch das erfindungsgemäß vorgeschlagene Verfahren kann eine häufige extrinsische Kalibrierung eines LiDAR-Messsystems zur Reduktion der Neigungsfehler vermieden werden. Damit sinkt die Anzahl der erforderlichen Kalibrierungen. Durch das erfindungsgemäß vorgeschlagene Verfahren lässt sich des Weiteren eine kostengünstigere Inertialsensorik einsetzen, die größere Fehlertoleranzen aufweist, die jedoch mittels des erfindungsgemäß vorgeschlagenen Verfahrens korrigiert werden können, wobei sich der zu leistende Rechenaufwand in erträglichen Grenzen hält.The method proposed according to the invention makes it possible to avoid frequent extrinsic calibration of a LiDAR measuring system to reduce tilt errors. This reduces the number of calibrations required. The method proposed according to the invention also makes it possible to use a more cost-effective inertial sensor system which has larger error tolerances, which, however, can be corrected using the method proposed according to the invention, whereby the computing effort required is kept within tolerable limits.

Durch das erfindungsgemäß vorgeschlagene Verfahren lässt sich ein sehr viele Messwerte lieferndes LiDAR-Sensorsystem dahingehend effektiver nutzen, dass aus der Vielzahl der Messwerte einige Messwerte für eine Korrektur besser geeignet sind, andere hingegen nicht. Zum Beispiel können Messwerte mittlerer Entfernung in flacher Umgebung mit niedriger Steigung besser verwendet werden. Hingegen sind Messwerte aus zu großer Entfernung weniger geeignet, da der Signalrauschabstand aufgrund der geringen Messintensität gegenüber dem Streulicht sehr groß ausfallen kann.The method proposed according to the invention allows a LiDAR sensor system that delivers a large number of measured values to be used more effectively in such a way that, out of the large number of measured values, some measured values are better suited for correction, while others are not. For example, medium distance readings may be better used in flat, low gradient environments. On the other hand, measurements from a distance that are too far away are less suitable because the signal-to-noise ratio can be very high due to the low measurement intensity compared to the scattered light.

Durch das erfindungsgemäß vorgeschlagene Verfahren kann im Gegensatz zum merkmalbasierten SLAM-Verfahren Rechenzeit gespart werden, da eine aufwändige Merkmalextrahierung nicht notwendig ist.In contrast to the feature-based SLAM method, the method proposed according to the invention can save computing time because complex feature extraction is not necessary.

Kurze Beschreibung der ZeichnungenBrief description of the drawings

Anhand der Zeichnungen wird die Erfindung nachstehend eingehender beschrieben.The invention is described in more detail below with reference to the drawings.

Es zeigt:

• 1 ein Koordinatensystem mit X-, Y- und Z-Richtung sowie den Winkeln ϕ, θ und ψ,
• 2 Höhenlinien und
• 3 ein schematisch dargestelltes LiDAR-Sensorsystem mit Nickwinkel θ_i.

It shows:

• 1 a coordinate system with X, Y and Z directions as well as the angles ϕ, θ and ψ,
• 2 contour lines and
• 3 a schematically shown LiDAR sensor system with pitch angle θ_i .

Ausführungsformen der ErfindungEmbodiments of the invention

In der nachfolgenden Beschreibung der Ausführungsformen der Erfindung werden gleiche oder ähnliche Elemente mit gleichen Bezugszeichen bezeichnet, wobei auf eine wiederholte Beschreibung dieser Elemente in Einzelfällen verzichtet wird. Die Figuren stellen den Gegenstand der Erfindung nur schematisch dar.In the following description of the embodiments of the invention, the same or similar elements are referred to with the same reference numerals, with a repeated description of these elements being omitted in individual cases. The figures represent the subject matter of the invention only schematically.

1 zeigt in schematischer Weise ein Koordinatensystem mit den Raumrichtungen X, Y und Z sowie den eingetragenen Eulerwinkeln Rollwinkel ϕ, Nickwinkel θ und Gierwinkel ψ und deren Bezug auf die Raumrichtungen X, Y und Z.1 shows in a schematic manner a coordinate system with the spatial directions

Der Darstellung gemäß2 sind von einem LiDAR-Sensorsystem 20 eines Fahrzeugs 10 detektierte Höhenlinien zu entnehmen.According to the representation 2 10 contour lines detected by aLiDAR sensor system 20 of a vehicle can be seen.

3 zeigt eine schematische Darstellung des LiDAR-Sensorsystems 20 mit eingetragenem Nickwinkel θ_i.3 shows a schematic representation of theLiDAR sensor system 20 with the pitch angle θ_i entered.

Mithilfe des erfindungsgemäß vorgeschlagenen Verfahrens wird aus einer korrigierten Pose χ_k wird eine neue Poseninformation χ_k|k-1 prognostiziert. Dies erfolgt nach einem Initialisierungsschritt mit$$x_0=0$$

$$P_0={\sigma }^{2}I$$

Using the method proposed according to the invention, new pose information χ_k|k-1 is predicted from a corrected pose χ_k . This is done after an initialization step$$x_0=0$$

$$P_0={\sigma }^{2}I$$

Des Weiteren wird ein Prädiktionsschritt durchlaufen, anhand der nachfolgenden Gleichungen:$$x_{k|k-1}=f\left(x_{k-1},u_{k-1}\right)$$

$$P_{k|k-1}=F_{k-1}{P}_{k-1}{F}_{k-1}^T+B_{k-1}{Q}_{k-1}{B}_{k-1}^T$$

Furthermore, a prediction step is carried out using the following equations:$$x_{k|k-1}=f\left(x_{k-1},u_{k-1}\right)$$

$$P_{k|k-1}=F_{k-1}{P}_{k-1}{F}_{k-1}^T+b_{k-1}{Q}_{k-1}{b}_{k-1}^T$$

Mithilfe einer korrigierten Pose χ_k wird eine neue Poseninformation χ_k|k-1 gemäß Gleichung (1) prognostiziert und mithilfe der korrespondierenden korrigierten Kovarianz P_k wird die korrespondierende neue Kovarianz P_k|k-1 gemäß Gleichung (2) prognostiziert.Using a corrected pose χ_k, new pose information χ_k|k-1 is predicted according to equation (1) and using the corresponding corrected covariance P_k , the corresponding new covariance P_k|k-1 is predicted according to equation (2).

Aus der prognostizierten Pose χ_k|k-1 und einer Höhenkarte m_i der Umgebung des LiDAR-Sensorsystems 20 lassen sich LiDAR-Messwerte h_r,i schätzen. Für die Berechnung des LiDAR-Sensormodells h_r,i und für die Auswahl der LiDAR-Strahlen, die auf einem Prüfpunkt auf einer ausgewählten horizontal ebenen Fläche in möglichst großer Entfernung gerichtet sind, wird zunächst die Drehmatrix anhand nachfolgender Gleichung$$T_{\mathcal{E}V}=\left(\begin{array}{ccc}\text{cos}\left(\psi \right)\text{cos}\left(\theta \right)& \text{cos}\left(\theta \right)\text{sin}\left(\psi \right)& -\text{sin}\left(\theta \right)\\ \text{cos}\left(\psi \right)\text{sin}\left(\varphi \right)\text{sin}\left(\theta \right)-\text{cos}\left(\varphi \right)\text{sin}\left(\psi \right)& \text{cos}\left(\varphi \right)\text{cos}\left(\psi \right)+\text{sin}\left(\varphi \right)\text{sin}\left(\psi \right)\text{sin}\left(\theta \right)& \text{cos}\left(\theta \right)\text{sin}\left(\varphi \right)\\ \text{sin}\left(\varphi \right)\text{sin}\left(\psi \right)+\text{cos}\left(\varphi \right)\text{cos}\left(\psi \right)\text{sin}\left(\theta \right)& \text{cos}\left(\varphi \right)\text{sin}\left(\psi \right)\text{sin}\left(\theta \right)-\text{cos}\left(\psi \right)\text{sin}\left(\varphi \right)& \text{cos}\left(\varphi \right)\text{cos}\left(\theta \right)\end{array}\right)$$

bestimmt. Mit Rollwinkel ϕ, Nickwinkel θ und Gierwinkel ip des LiDAR-Sensorsystems 20 bezüglich eines Weltkoordinatensystems und durch die Bestimmung der Abstände r_x, r_y und r_z eines Prüfpunkts in LiDAR-Koordinaten werden aus den Koordinaten des Prüfpunkts m_i = [m_x m_y m_z]^T Weltkoordinaten und Koordinaten des LiDARs p_i = [p_x p_y p_z]^T in Weltkoordinaten berechnet. Dabei können die Drehwinkel und die Koordinaten des LiDARs Teil eines Zustandsvektors χ_k|k-1 = [p_x p_y p_z ϕ, θ und ψ]^T sein.$$\left(\begin{array}{c}{r}_x\\ r_y\\ r_z\end{array}\right)=r\left(x\right)=T_{EV}\left(\left(\begin{array}{c}{m}_x\\ m_y\\ m_z\end{array}\right)-\left(\begin{array}{c}{p}_x\\ p_y\\ p_z\end{array}\right)\right)$$

LiDAR measurement values h_r ,i can be estimated from the predicted pose χ_k|k-1 and a height map m_i of the surroundings of theLiDAR sensor system 20. To calculate the LiDAR sensor model h_r,i and to select the LiDAR beams that are aimed at a test point on a selected horizontally flat surface at the greatest possible distance, the rotation matrix is first determined using the following equation$$T_{\mathcal{E}v}=\left(\begin{array}{ccc}\text{cos}\left(\psi \right)\text{cos}\left(\theta \right)& \text{cos}\left(\theta \right)\text{sin}\left(\psi \right)& -\text{sin}\left(\theta \right)\\ \text{cos}\left(\psi \right)\text{sin}\left(\varphi \right)\text{sin}\left(\theta \right)-\text{cos}\left(\varphi \right)\text{sin}\left(\psi \right)& \text{cos}\left(\varphi \right)\text{cos}\left(\psi \right)+\text{sin}\left(\varphi \right)\text{sin}\left(\psi \right)\text{sin}\left(\theta \right)& \text{cos}\left(\theta \right)\text{sin}\left(\varphi \right)\\ \text{sin}\left(\varphi \right)\text{sin}\left(\psi \right)+\text{cos}\left(\varphi \right)\text{cos}\left(\psi \right)\text{sin}\left(\theta \right)& \text{cos}\left(\varphi \right)\text{sin}\left(\psi \right)\text{sin}\left(\theta \right)-\text{cos}\left(\psi \right)\text{sin}\left(\varphi \right)& \text{cos}\left(\varphi \right)\text{cos}\left(\theta \right)\end{array}\right)$$

certainly. With roll angle ϕ, pitch angle θ and yaw angle ip of theLiDAR sensor system 20 with respect to a world coordinate system and by determining the distances r_x ,_ry and r_z of a test point in LiDAR coordinates, the coordinates of the test point become m_i = [m_x m_y m_z ]^T world coordinates and coordinates of the LiDAR p_i = [p_x p_y p_z ]^T calculated in world coordinates. The rotation angles and the coordinates of the LiDAR can be part of a state vector χ_k|k-1 = [p_x p_y p_z ϕ, θ and ψ]^T.$$\left(\begin{array}{c}{r}_x\\ r_y\\ r_{\mathrm{e.g}}\end{array}\right)=r\left(x\right)=T_{Ev}\left(\left(\begin{array}{c}{m}_x\\ m_y\\ m_{\mathrm{e.g}}\end{array}\right)-\left(\begin{array}{c}{p}_x\\ p_y\\ p_{\mathrm{e.g}}\end{array}\right)\right)$$

Mithilfe der Differenz zwischen den geschätzten LiDAR-Messwerten h_r,i und gemessenen oder interpolierten LiDAR-Messwerten z_r,i wird die prognostizierte Pose χ_k|k-1 korrigiert, was mittels der Gleichungen$$K_i=P_{i|i-1}{H}_{r,i}^T{\left(H_{r,i}{P}_{i|i-1}{H}_{r,i}^T+R_i\right)}^{-1}$$

$$x_i=x_{i|i-1}+K_i\left(z_{r,i}-h_{r,i}\right)$$

$$P_i=\left(I-K_i{H}_{r,i}\right)P_{i|i-1}$$

$$K_k=P_{k|k-1}{H}_{r,k}^T{\left(H_{r,k}{P}_{k|k-1}{H}_{r,k}^T+R_k\right)}^{-1}$$

$$x_k=x_{k|k-1}+K_k\left(z_{r,k}-h_{r,k}\right)$$

$$P_k=\left(I-K_k{H}_{r,k}\right)P_{k|k-1}$$

durchgeführt wird.Using the difference between the estimated LiDAR measurements h_r,i and measured or interpolated LiDAR measurements z_r,i , the predicted pose χ_k|k-1 is corrected using the equations$$K_i=P_{i|i-1}{H}_{r,i}^T{\left(H_{r,i}{P}_{i|i-1}{H}_{r,i}^T+R_i\right)}^{-1}$$

$$x_i=x_{i|i-1}+K_i\left({\mathrm{e.g}}_{r,i}-H_{r,i}\right)$$

$$P_i=\left(I-K_i{H}_{r,i}\right)P_{i|i-1}$$

$$K_k=P_{k|k-1}{H}_{r,k}^T{\left(H_{r,k}{P}_{k|k-1}{H}_{r,k}^T+R_k\right)}^{-1}$$

$$x_k=x_{k|k-1}+K_k\left({\mathrm{e.g}}_{r,k}-H_{r,k}\right)$$

$$P_k=\left(I-K_k{H}_{r,k}\right)P_{k|k-1}$$

is carried out.

Häufig kann die Position des LiDAR-Sensorsystems 20 mit GPS und anderen Einrichtungen sehr genau bestimmt werden, während eine Ausrichtung des LiDAR-Sensorsystems 20 häufig nicht derart genau bestimmt werden kann und demzufolge die ungewisse Ausrichtung den größten Fehler bei der Höhenkartenberechnung beisteuert. Aus diesem Grund werden mit der Korrektur gemäß dem erfindungsgemäß vorgeschlagenen Verfahren vor allem Ausrichtungsfehler des LiDAR-Sensorsystems 20 erheblich reduziert. In der Regel soll mittels des LiDAR-Sensorsystems 20 ein Gelände mit niedrigen Steigungswerten vermessen werden. Aus diesem Grund führen fehlerhafte Gierwinkel zu deutlich kleineren Höhenfehlern verglichen mit fehlerhaften Rollwinkeln und Nickwinkeln. Mittels des erfindungsgemäß vorgeschlagenen Verfahrens werden daher vor allem die Fehler aus Rollwinkeln und Nickwinkeln erheblich herabgesetzt.Often the position of theLiDAR sensor system 20 can be determined very precisely using GPS and other devices, while an orientation of theLiDAR sensor system 20 often cannot be determined with such precision and, as a result, the uncertain orientation contributes the largest error in the height map calculation. For this reason, the correction according to the method proposed according to the invention significantly reduces, in particular, alignment errors of theLiDAR sensor system 20. As a rule, terrain with low gradient values should be measured using theLiDAR sensor system 20. For this reason, incorrect yaw angles result in significantly smaller altitude errors compared to incorrect roll and pitch angles. By means of the method proposed according to the invention, the errors from roll angles and pitch angles in particular are significantly reduced.

Ein LiDAR-Sensorsystem 20 kann bei seinem Einsatz beispielsweise in Fahrzeugen 10 sehr viele Messwerte liefern, wobei einige Messwerte für die Korrektur besser, schlechter oder gar nicht geeignet sind. Zum Beispiel sind Messwerte in mittlerer Entfernung in flacher Umgebung mit niedriger Steigung besser geeignet. Messwerte, die aus großen Entfernungen herrühren, sind hingegen weniger geeignet, da der Signal-Rausch-Abstand aufgrund der geringen Messintensität gegenüber dem Streulicht sehr groß werden kann. Auch Sonnenlicht, Staub, Regen und Nebel haben auf größere Entfernungen gesehen ein erheblich größeres Störpotential. Messwerte aus zu nahen Entfernungen werden hingegen nur gering von fehlerhaften Neigungswerten beeinflusst, sofern das LiDAR-Sensorsystem 20 annähernd senkrecht auf den Boden trifft. Vom LiDAR-Sensorsystem 20 ausgesandte LiDAR-Strahlen, die auf Teile des eigenen Fahrzeugs 10 treffen, sind nicht geeignet.When used invehicles 10, for example, aLiDAR sensor system 20 can deliver a large number of measured values, with some measured values being better, worse or not suitable for correction. For example, medium-distance readings are more appropriate in flat, low-slope environments. However, measured values that come from large distances are less suitable because the signal-to-noise ratio can become very large due to the low measurement intensity compared to the scattered light. Sunlight, dust, rain and fog also have a significantly greater potential for interference when viewed over greater distances. However, measured values from distances that are too close are only slightly influenced by incorrect inclination values, provided that theLiDAR sensor system 20 hits the ground approximately vertically. LiDAR beams emitted by theLiDAR sensor system 20 that hit parts of yourown vehicle 10 are not suitable.

Des Weiteren hat sich herausgestellt, dass die Korrektur der sehr vielen Messwerte, die ein LiDAR-Sensorsystem 20 zu liefern imstande ist, sehr rechenintensiv sein kann. Darum kann es von Vorteil sein, zur Begrenzung der Rechenintensität und der Rechenzeit nur einen Teil der Messwerte zur Korrektur heranzuziehen. Es kann des Weiteren günstig sein, mindestens zwei Messwerte heranzuziehen, wobei sich deren Richtung und deren um plus/minus 180° gedrehte Richtungen möglichst stark unterscheiden, d. h. entgegengesetzt zueinander verlaufen. Für zwei Messwerte ist eine Unterscheidung von 90°, zum Beispiel Norden und Osten, und für drei Messwerte ist eine Unterscheidung von 60°, zum Beispiel West-Nord-West, Norden und Ost-Nord-Ost, hingegen optimal.Furthermore, it has been found that correcting the large number of measured values that aLiDAR sensor system 20 is capable of delivering can be very computationally intensive. It can therefore be advantageous to use only part of the measured values for correction in order to limit the computing intensity and computing time. It can also be advantageous to use at least two measured values, whereby their direction and their directions rotated by plus/minus 180° differ as much as possible, i.e. H. run opposite to each other. For two measured values, a distinction of 90°, for example north and east, is optimal, and for three measured values, a distinction of 60°, for example west-north-west, north and east-north-east, is optimal.

Die Rollwinkelfehler sowie die Nickwinkelfehler wirken sich vor allem auf Messungen mit großer Entfernung aus. Aus diesem Grunde wird es bevorzugt, vor allem Messpunkte zur Korrektur heranzuziehen, die sehr weit vom LiDAR-Sensorsystem 20 entfernt angeordnet sind. Die Reichweite des LiDAR-Sensorsystems 20 kann durch die Lichtleistung eines LiDAR-Sensors, durch die Empfindlichkeit, durch Umwelteinflüsse wie Sonnenlicht, Staub, Regen oder Nebel, begrenzt sein. In diesem Falle werden Messpunkte herangezogen, die noch auf zuverlässige Weise in großer Entfernung erfasst werden können.The roll angle errors and the pitch angle errors primarily affect measurements at large distances. For this reason, it is preferred to use measurement points for correction that are located very far away from theLiDAR sensor system 20. The range of theLiDAR sensor system 20 can be limited by the light output of a LiDAR sensor, by the sensitivity, by environmental influences such as sunlight, dust, rain or fog. In this case, measuring points are used that can still be reliably recorded at a large distance.

Weist das Gelände um den Messpunkt herum keine Steigung auf, dann hat auch ein Gierwinkelfehler wenig bis keinen Einfluss auf die Korrektur der Rollwinkelfehler sowie der Nickwinkelfehler; die Korrekturkann besonders gut durchgeführt werden. Aus diesem Grunde werden Messpunkte für die Korrektur bevorzugt, die von Messpunkten mit gleicher Höhe umgeben sind. Zu den umgebenden Messpunkten werden alle Punkte gezählt, die durch den berechneten Gierwinkelfehler im Intervall von +/- einer Standardabweichung fälschlicherweise gemessen werden.If the terrain around the measuring point has no slope, then a yaw angle error has little to no influence on the correction of the roll angle errors and the pitch angle errors; the correction can be carried out particularly well. For this reason, measurement points that are surrounded by measurement points of the same height are preferred for correction. The surrounding measurement points include all points that are incorrectly measured due to the calculated yaw angle error in the interval of +/- one standard deviation.

Das erfindungsgemäß vorgeschlagene Verfahren zur Kompensation von Gier- und Nickwinkelfehlern wird bevorzugt in einem Kalman-Filter durchgeführt. Die Durchführung des Verfahrens wird im Wesentlichen anhand der nachfolgenden Gleichungssysteme beschrieben:

• Für die Berechnung des LiDAR-Sensormodells h_r, i und für die Auswahl der LiDAR-Strahlen, die auf einen Prüfpunkt auf einer ausgewählten horizontal ebenen Fläche in möglichst großer Entfernung gerichtet sind, wird zunächst die Drehmatrix nach (3) berechnet:

$$T_{\mathcal{E}V}=\left(\begin{array}{ccc}\text{cos}\left(\psi \right)\text{cos}\left(\theta \right)& \text{cos}\left(\theta \right)\text{sin}\left(\psi \right)& -\text{sin}\left(\theta \right)\\ \text{cos}\left(\psi \right)\text{sin}\left(\varphi \right)\text{sin}\left(\theta \right)-\text{cos}\left(\varphi \right)\text{sin}\left(\psi \right)& \text{cos}\left(\varphi \right)\text{cos}\left(\psi \right)+\text{sin}\left(\varphi \right)\text{sin}\left(\psi \right)\text{sin}\left(\theta \right)& \text{cos}\left(\theta \right)\text{sin}\left(\varphi \right)\\ \text{sin}\left(\varphi \right)\text{sin}\left(\psi \right)+\text{cos}\left(\varphi \right)\text{cos}\left(\psi \right)\text{sin}\left(\theta \right)& \text{cos}\left(\varphi \right)\text{sin}\left(\psi \right)\text{sin}\left(\theta \right)-\text{cos}\left(\psi \right)\text{sin}\left(\varphi \right)& \text{cos}\left(\varphi \right)\text{cos}\left(\theta \right)\end{array}\right)$$

The method proposed according to the invention for compensating for yaw and pitch angle errors is preferably carried out in a Kalman filter. The implementation of the method is essentially described using the following systems of equations:

• For the calculation of the LiDAR sensor model h_r , i and for the selection of the LiDAR beams that are aimed at a test point on a selected horizontally flat surface at the greatest possible distance, the rotation matrix is first calculated according to (3):

$$T_{\mathcal{E}v}=\left(\begin{array}{ccc}\text{cos}\left(\psi \right)\text{cos}\left(\theta \right)& \text{cos}\left(\theta \right)\text{sin}\left(\psi \right)& -\text{sin}\left(\theta \right)\\ \text{cos}\left(\psi \right)\text{sin}\left(\varphi \right)\text{sin}\left(\theta \right)-\text{cos}\left(\varphi \right)\text{sin}\left(\psi \right)& \text{cos}\left(\varphi \right)\text{cos}\left(\psi \right)+\text{sin}\left(\varphi \right)\text{sin}\left(\psi \right)\text{sin}\left(\theta \right)& \text{cos}\left(\theta \right)\text{sin}\left(\varphi \right)\\ \text{sin}\left(\varphi \right)\text{sin}\left(\psi \right)+\text{cos}\left(\varphi \right)\text{cos}\left(\psi \right)\text{sin}\left(\theta \right)& \text{cos}\left(\varphi \right)\text{sin}\left(\psi \right)\text{sin}\left(\theta \right)-\text{cos}\left(\psi \right)\text{sin}\left(\varphi \right)& \text{cos}\left(\varphi \right)\text{cos}\left(\theta \right)\end{array}\right)$$

Die Berechnung erfolgt mittels Rollwinkel ϕ, Nickwinkel θ und Gierwinkel ip des LiDAR-Sensorsystems 20 bezüglich eines Weltkoordinatensystems. Dadurch lassen sich die Abstände r_x, r_y und r_z des jeweiligen Prüfpunkts in LiDAR-Koordinaten aus den Koordinaten des Prüfpunkts m_i = [m_x m_y m_z]^T in Weltkoordinaten und den Koordinaten des LiDARs p_i = [p_x p_y p_z]^T in Weltkoordinaten berechnen. Dabei können die Drehwinkel und Koordinaten des LiDARs Teil des Zustandsvektors χ_k|k-1 = [p_x p_y p_z ϕ, θ und ψ]^T sein:$$\left(\begin{array}{c}{r}_x\\ r_y\\ r_z\end{array}\right)=r\left(x\right)=T_{EV}\left(\left(\begin{array}{c}{m}_x\\ m_y\\ m_z\end{array}\right)-\left(\begin{array}{c}{p}_x\\ p_y\\ p_z\end{array}\right)\right)$$

The calculation is carried out using the roll angle ϕ, pitch angle θ and yaw angle ip of theLiDAR sensor system 20 with respect to a world coordinate system._{Thisallowsthedistancesr__}^__{__} p_y p_z ] Calculate^T in world coordinates. The rotation angles and coordinates of the LiDAR can be part of the state vector χ_k|k-1 = [p_x p_y p_z ϕ, θ and ψ]^T :$$\left(\begin{array}{c}{r}_x\\ r_y\\ r_{\mathrm{e.g}}\end{array}\right)=r\left(x\right)=T_{Ev}\left(\left(\begin{array}{c}{m}_x\\ m_y\\ m_{\mathrm{e.g}}\end{array}\right)-\left(\begin{array}{c}{p}_x\\ p_y\\ p_{\mathrm{e.g}}\end{array}\right)\right)$$

Das LiDAR-Sensormodell h_r,i (χ) des Laserstrahls i zu dem ausgebildeten Prüfpunkt berechnet sich aus den Abständen r = [r_x r_y r_z]^T des Prüfpunkts in LiDAR-Koordinaten gemäß der Gleichung$$h_{r,i}\left(r\right)=\sqrt{r_x^2+r_y^2+r_z^2}$$

The LiDAR sensor model h_r,i (χ) of the laser beam i to the formed test point is calculated from the distances r = [r_x r_y r_z ]^T of the test point in LiDAR coordinates according to the equation$$H_{r,i}\left(r\right)=\sqrt{r_x^2+r_y^2+r_{\mathrm{e.g}}^2}$$

Die Beobachtungsmatrix H_r,i (χ) berechnet sich aus dem LiDAR-Sensormodell h_r,i (χ) durch die partielle Ableitung mit dem Zustandsvektor x = [p_x p_y p_z ϕ, θ und ψ]^T zu$$H_{r,i,k}=\frac{\partial h_{r,i}\left(r\right)}{\partial x}=\frac{\partial h\left(r\right)}{\partial r}\frac{\partial r\left(x\right)}{\partial x}=\frac{\partial h\left(r\right)}{\partial r}\left(\frac{\partial T_{EV}\left(x\right)}{\partial x}\left(m_i-p_i\right)+T_{EV}\frac{\partial \left(m_i-p_i\right)}{\partial x}\right)$$

The observation matrix H_r,i (χ) is calculated from the LiDAR sensor model h_r,i (χ) by the partial derivative with the state vector x = [p_x p_y p_z ϕ, θ and ψ]^T$$H_{r,i,k}=\frac{\partial H_{r,i}\left(r\right)}{\partial x}=\frac{\partial H\left(r\right)}{\partial r}\frac{\partial r\left(x\right)}{\partial x}=\frac{\partial H\left(r\right)}{\partial r}\left(\frac{\partial T_{Ev}\left(x\right)}{\partial x}\left(m_i-p_i\right)+T_{Ev}\frac{\partial \left(m_i-p_i\right)}{\partial x}\right)$$

Für die Auswahl der LiDAR-Messpunkte, die zum Prüfpunkt i passen, werden zunächst die Azimutwinkel θ_i zu$${\theta }_i=\text{arccot}\left(\frac{r_z}{\sqrt{r_x^2+r_y^2}}\right)$$

und der Polarwinkel ϕ_i$${\phi }_i=\text{atan}2\left(r_y,r_x\right)$$

berechnet.To select the LiDAR measurement points that match test point i, the azimuth angles θ_i are first determined$${\theta }_i=\text{arccot}\left(\frac{r_{\mathrm{e.g}}}{\sqrt{r_x^2+r_y^2}}\right)$$

and the polar angle ϕ_i$${\phi }_i=\text{atan}2\left(r_y,r_x\right)$$

calculated.

Die gemessenen Abstände der umliegenden gemessenen LiDAR-Abstände r₁₁(θ₁, φ₁), r₂₁(θ₂, φ₁), r₁₂(θ₁, φ₂), r₂₂ (θ₂, φ₂) θ_i um θ_i und φ_i herum können herangezogen werden, um den Abstand r_i per zweidimensional erfolgender linearer Interpolation zu berechnen gemäß den nachfolgenden Beziehungen.$$r_1=\frac{{\theta }_2-{\theta }_i}{{\theta }_2-{\theta }_1}{r}_{11}+\frac{{\theta }_i-{\theta }_1}{{\theta }_2-{\theta }_1}{r}_{21}$$

$$r_2=\frac{{\theta }_2-{\theta }_i}{{\theta }_2-{\theta }_1}{r}_{12}+\frac{{\theta }_i-{\theta }_1}{{\theta }_2-{\theta }_1}{r}_{22}$$

$$z_{r,i}=\frac{{\phi }_2-{\phi }_i}{{\phi }_2-{\phi }_1}{r}_1+\frac{{\phi }_i-{\phi }_1}{{\phi }_2-{\phi }_1}{r}_2$$

The measured distances of the surrounding measured LiDAR distances r₁₁ (θ₁ , φ₁ ), r₂₁ (θ₂ , φ₁ ), r₁₂ (θ₁ , φ₂ ), r₂₂ (θ₂ , φ₂ ) θ_i around θ_i and φ_i can be used to calculate the distance r_i via two-dimensional linear interpolation according to the following relationships.$$r_1=\frac{{\theta }_2-{\theta }_i}{{\theta }_2-{\theta }_1}{r}_{11}+\frac{{\theta }_i-{\theta }_1}{{\theta }_2-{\theta }_1}{r}_{21}$$

$$r_2=\frac{{\theta }_2-{\theta }_i}{{\theta }_2-{\theta }_1}{r}_{12}+\frac{{\theta }_i-{\theta }_1}{{\theta }_2-{\theta }_1}{r}_{22}$$

$${\mathrm{e.g}}_{r,i}=\frac{{\phi }_2-{\phi }_i}{{\phi }_2-{\phi }_1}{r}_1+\frac{{\phi }_i-{\phi }_1}{{\phi }_2-{\phi }_1}{r}_2$$

Die jeweiligen Größen r₁ und r₂ gemäß den Gleichungen (15) und (16) stellen dabei Zwischengrößen dar, die zur besseren Veranschaulichung herangezogen werden können. Der Korrekturschritt der Kalman-Filterberechnungen kann analog zu den Rechenschritten gemäß den Gleichungen (6) bis (8) gemäß den nachfolgenden Beziehungen berechnet werden:$$K_i=P_{i|i-1}{H}_{r,i}^T{\left(H_{r,i}{P}_{i|i-1}{H}_{r,i}^T+R_i\right)}^{-1}$$

$$x_i=x_{i|i-1}+K_i\left(z_{r,i}-h_{r,i}\right)$$

$$P_i=\left(I-K_i{H}_{r,i}\right)P_{i|i-1}$$

The respective variables r₁ and r₂ according to equations (15) and (16) represent intermediate variables that can be used for better illustration. The correction step of the Kalman filter calculations can be calculated analogously to the calculation steps according to equations (6) to (8) according to the following relationships:$$K_i=P_{i|i-1}{H}_{r,i}^T{\left(H_{r,i}{P}_{i|i-1}{H}_{r,i}^T+R_i\right)}^{-1}$$

$$x_i=x_{i|i-1}+K_i\left({\mathrm{e.g}}_{r,i}-H_{r,i}\right)$$

$$P_i=\left(I-K_i{H}_{r,i}\right)P_{i|i-1}$$

Es werden ein oder mehrere Messpunkte i mit den zugehörigen Größen H_r,i, z_r,i und h_r,i wie zuvor beschrieben innerhalb eines Zeitschritts k berechnet. Die Korrektur kann für jeden der Messpunkte i einzeln durchgeführt werden, wobei die Gleichungen$$x_i=x_{i|i-1}+K_i\left(z_{r,i}-h_{r,i}\right)$$

$$P_i=\left(I-K_i{H}_{r,i}\right)P_{i|i-1}$$

$$K_i=P_{i|i-1}{H}_{r,i}^T{\left(H_{r,i}{P}_{i|i-1}{H}_{r,i}^T+R_i\right)}^{-1}$$

mehrfach für jeden Messpunkt durchlaufen werden und das Messrauschen R_i eine Skalarvarianz darstellt. Alternativ könnten aber auch mehrere oder alle n Messpunkte i eines Zeitschritts k in einem Vektor z_r,k = [z_r,1 z_r,2 ... z_r,n]^T zusammengefasst werden. In analoger Weise gilt dies auch für die Modellwerte h_r,k = [h_r,1 h_r,2 ... h_r,n]^T, deren Jakobimatrix H_r,k = [H_r,1 H_r,2 ... H_r,n]^T und das Messrauschen R_k = diag(R₁, R₂, ... R_n):$$K_k=P_{k|k-1}{H}_{r,k}^T{\left(H_{r,k}{P}_{k|k-1}{H}_{r,k}^T+R_k\right)}^{-1}$$

$$x_k=x_{k|k-1}+K_k\left(z_{r,k}-h_{r,k}\right)$$

$$P_k=\left(I-K_k{H}_{r,k}\right)P_{k|k-1}$$

One or more measuring points i with the associated variables H_r,i , z_r,i and h_r,i are calculated within a time step k as described above. The correction can be carried out individually for each of the measuring points i, where the equations$$x_i=x_{i|i-1}+K_i\left({\mathrm{e.g}}_{r,i}-H_{r,i}\right)$$

$$P_i=\left(I-K_i{H}_{r,i}\right)P_{i|i-1}$$

$$K_i=P_{i|i-1}{H}_{r,i}^T{\left(H_{r,i}{P}_{i|i-1}{H}_{r,i}^T+R_i\right)}^{-1}$$

are run through several times for each measuring point and the measuring noise R_i represents a scalar variance. Alternatively, several or all n measuring points i of a time step k could be combined in a vector z_r,k = [z_r,1 z_r,2 ... z_r,n ]^T. In an analogous manner, this also applies to the model values h_r,k = [h_r,1 h_r,2 ... h_r,n ]^T , whose Jacobi matrix H_r,k = [H_r,1 H_r,2 . .. H_r,n ]^T and the measurement noise R_k = diag(R₁ , R₂ , ... R_n ):$$K_k=P_{k|k-1}{H}_{r,k}^T{\left(H_{r,k}{P}_{k|k-1}{H}_{r,k}^T+R_k\right)}^{-1}$$

$$x_k=x_{k|k-1}+K_k\left({\mathrm{e.g}}_{r,k}-H_{r,k}\right)$$

$$P_k=\left(I-K_k{H}_{r,k}\right)P_{k|k-1}$$

Insbesondere bei einer sich ergebenden großen Anzahl von Systemzuständen lässt sich durch das Zusammenfassen mehrerer Messpunkte i und einer gemeinsamen Korrektur der Kovarianzmatrix P_k Rechenzeit einsparen. Werden sehr viele Messpunkte i zur Korrektur eingesetzt, dann kann es auch günstig sein, einen Informationsfilter zur Prädiktion und Korrektur der entsprechenden Messwerte einzusetzen. Der Informationsfilter weist genauso wie ein in Rede stehender Kalman-Filter einen Prädiktionsschritt und einen Korrekturschritt auf, nutzt allerdings für die Berechnung eine invertierte Kovarianzmatrix. Dadurch ist zwar die Prädiktion rechenintensiver, jedoch kann erheblich Rechenzeit gespart werden, wenn sehr viele Messwerte für die Korrektur herangezogen werden sollten.Particularly when there is a large number of system states, computing time can be saved by combining several measurement points i and a joint correction of the covariance matrix P_k . If a large number of measuring points i are used for correction, then it can also be beneficial to use an information filter to predict and correct the corresponding measured values. The information filter, like a Kalman filter in question, has a prediction step and a correction step, but uses an inverted covariance matrix for the calculation. This means that the prediction is more computationally intensive, but considerable computing time can be saved if a large number of measured values are to be used for the correction.

Vorstehend ist das erfindungsgemäß vorgeschlagene Verfahren anhand des Einsatzes eines LiDAR-Sensorsystems 20 dargestellt. Anstelle eines LiDAR-Sensors oder eines LiDAR-Sensorsystems 20 könnte auch eine Tiefenkamera, so zum Beispiel eine Stereo-Kamera oder eine Time-of-Flight(ToF)-Kamera, eingesetzt werden. Des Weiteren ist anzumerken, dass alternativ zur beschriebenen Berechnung unter Einsatz der Eulerwinkel ϕ, θ und ip auch Quaternionen für die Berechnung eingesetzt werden könnten.The method proposed according to the invention is shown above using aLiDAR sensor system 20. Instead of a LiDAR sensor or aLiDAR sensor system 20, a depth camera, such as a stereo camera or a time-of-flight (ToF) camera, could also be used. It should also be noted that, as an alternative to the calculation described using the Euler angles ϕ, θ and ip, quaternions could also be used for the calculation.

Die Erfindung ist nicht auf die hier beschriebenen Ausführungsbeispiele und die darin hervorgehobenen Aspekte beschränkt. Vielmehr ist innerhalb des durch die Ansprüche angegebenen Bereichs eine Vielzahl von Abwandlungen möglich, die im Rahmen fachmännischen Handelns liegen.The invention is not limited to the exemplary embodiments described here and the aspects highlighted therein. Rather, within the range specified by the claims, a large number of modifications are possible, which are within the scope of professional action.

ZITATE ENTHALTEN IN DER BESCHREIBUNGQUOTES INCLUDED IN THE DESCRIPTION

Diese Liste der vom Anmelder aufgeführten Dokumente wurde automatisiert erzeugt und ist ausschließlich zur besseren Information des Lesers aufgenommen. Die Liste ist nicht Bestandteil der deutschen Patent- bzw. Gebrauchsmusteranmeldung. Das DPMA übernimmt keinerlei Haftung für etwaige Fehler oder Auslassungen.This list of documents listed by the applicant was generated automatically and is included solely for the better information of the reader. The list is not part of the German patent or utility model application. The DPMA assumes no liability for any errors or omissions.

Zitierte PatentliteraturCited patent literature

• US 2013/0041549 A1 [0002]US 2013/0041549 A1 [0002]
• US 2014/0270744 A [0003]US 2014/0270744 A [0003]

Claims (11)

Translated fromGerman

Verfahren zur Rollwinkel- und Nickwinkelfehlerkompensation bei einer Geländehöhenmessung mit einem LiDAR-Sensorsystem (20) mit zumindest nachfolgenden Verfahrensschritten:a) Mithilfe einer korrigierten Pose χ_k wird eine neue Poseninformation χ_k|k-1 prognostiziert,b) aus der gemäß a) prognostizierten Pose χ_k|k-1 und einer Höhenkarte m_i der Umgebung werden LiDAR-Messwerte h_r,i geschätzt,c) mithilfe der Differenz der gemäß b) geschätzten LiDAR-Messwerte h_r,i und gemessenen oder interpolierten LiDAR-Messwerte z_r,i wird die prognostizierte Pose χ_k|k-1 gemäß Verfahrensschritt a) korrigiert.Method for roll angle and pitch angle error compensation in a terrain height measurement with a LiDAR sensor system (20) with at least the following method steps: a) With the help of a corrected pose χ_k , new pose information χ_k|k-1 is predicted, b) from that predicted according to a). Pose χ_k|k-1 and a height map m_i of the environment, LiDAR measurements h_r,i are estimated, c) using the difference between the LiDAR measurements h_r,i estimated according to b) and measured or interpolated LiDAR measurements z_{r ,i,} the predicted pose χ_k|k-1 is corrected according to method step a).Verfahren gemäßAnspruch 1,dadurch gekennzeichnet, dass gemäß Verfahrensschritt a) ein Prädiktionsschritt durchlaufen wird mit$$x_{k|k-1}=f\left(x_{k-1},u_{k-1}\right)$$

und mit$$P_{k|k-1}=F_{k-1}{P}_{k-1}{F}_{k-1}^T+B_{k-1}{Q}_{k-1}{B}_{k-1}^T$$

wobei mit einer korrespondierenden Kovarianz P_k eine korrespondierende neue Kovarianz P_k|k-1 prognostiziert wird.Procedure according to Claim 1 ,characterized in that according to method step a), a prediction step is carried out with$$x_{k|k-1}=f\left(x_{k-1},u_{k-1}\right)$$

and with$$P_{k|k-1}=F_{k-1}{P}_{k-1}{F}_{k-1}^T+b_{k-1}{Q}_{k-1}{b}_{k-1}^T$$

whereby a corresponding new covariance P_k|k-1 is predicted with a corresponding covariance P_k .Verfahren gemäß denAnsprüchen 1 bis2,dadurch gekennzeichnet, dass gemäß Verfahrensschritt b) für die Berechnung eines LiDAR-Sensormodells h_r,i und eine Auswahl von LiDAR-Strahlen, welche auf einen Prüfpunkt auf einer ausgewählten horizontalen, ebenen Fläche in möglichst großer Entfernung gerichtet sind, eine Drehmatrix gemäß$$T_{EV}=\left(\begin{array}{ccc}\text{cos}\left(\psi \right)\text{cos}\left(\theta \right)& \text{cos}\left(\theta \right)\text{sin}\left(\psi \right)& -\text{sin}\left(\theta \right)\\ \text{cos}\left(\psi \right)\text{sin}\left(\varphi \right)\text{sin}\left(\theta \right)-\text{cos}\left(\varphi \right)\text{sin}\left(\psi \right)& \text{cos}\left(\varphi \right)\text{cos}\left(\psi \right)+\text{sin}\left(\varphi \right)\text{sin}\left(\psi \right)\text{sin}\left(\theta \right)& \text{cos}\left(\theta \right)\text{sin}\left(\varphi \right)\\ \text{sin}\left(\varphi \right)\text{sin}\left(\psi \right)+\text{cos}\left(\varphi \right)\text{cos}\left(\psi \right)\text{sin}\left(\theta \right)& \text{cos}\left(\varphi \right)\text{sin}\left(\psi \right)\text{sin}\left(\theta \right)-\text{cos}\left(\psi \right)\text{sin}\left(\varphi \right)& \text{cos}\left(\varphi \right)\text{cos}\left(\theta \right)\end{array}\right)$$

(3)mit Rollwinkel ϕ, Nickwinkel θ und Gierwinkel ip des LiDAR-Sensorsystems (20) bezüglich eines Weltkoordinatensystems und dadurch Abstände r_x, r_y und r_z des Prüfpunkts in LiDAR-Koordinaten aus den Koordinaten des Prüfpunkts m_i = [m_x m_y m_z]^T in Weltkoordinaten und in den Koordinaten des LiDARs p_i = [p_x p_y p_z]^T in Weltkoordinaten berechnet werden.Procedure in accordance with Claims 1 until 2 ,characterized in that according to method step b) for the calculation of a LiDAR sensor model h_r,i and a selection of LiDAR beams which are directed at a test point on a selected horizontal, flat surface at the greatest possible distance, a rotation matrix according to$$T_{Ev}=\left(\begin{array}{ccc}\text{cos}\left(\psi \right)\text{cos}\left(\theta \right)& \text{cos}\left(\theta \right)\text{sin}\left(\psi \right)& -\text{sin}\left(\theta \right)\\ \text{cos}\left(\psi \right)\text{sin}\left(\varphi \right)\text{sin}\left(\theta \right)-\text{cos}\left(\varphi \right)\text{sin}\left(\psi \right)& \text{cos}\left(\varphi \right)\text{cos}\left(\psi \right)+\text{sin}\left(\varphi \right)\text{sin}\left(\psi \right)\text{sin}\left(\theta \right)& \text{cos}\left(\theta \right)\text{sin}\left(\varphi \right)\\ \text{sin}\left(\varphi \right)\text{sin}\left(\psi \right)+\text{cos}\left(\varphi \right)\text{cos}\left(\psi \right)\text{sin}\left(\theta \right)& \text{cos}\left(\varphi \right)\text{sin}\left(\psi \right)\text{sin}\left(\theta \right)-\text{cos}\left(\psi \right)\text{sin}\left(\varphi \right)& \text{cos}\left(\varphi \right)\text{cos}\left(\theta \right)\end{array}\right)$$

(3) with roll angle ϕ, pitch angle θ and yaw angle ip of the LiDAR sensor system (20) with respect to a world coordinate system and thereby distances r_x ,_ry and r_z of the test point in LiDAR coordinates from the coordinates of the test point m_i = [m_x m_y m_z ]^T in world coordinates and in the coordinates of the LiDAR p_i = [p_x p_y p_z ]^T in world coordinates.Verfahren gemäßAnspruch 3,dadurch gekennzeichnet, dass die Drehwinkel und die Koordinaten des LiDARs Teil eines Zustandsvektors χ_k|k-1 = [p_x p_y p_z ϕ, θ und ψ]^T sind, gemäß$$\left(\begin{array}{c}{r}_x\\ r_y\\ r_z\end{array}\right)=r\left(x\right)=T_{EV}\left(\left(\begin{array}{c}{m}_x\\ m_y\\ m_z\end{array}\right)-\left(\begin{array}{c}{p}_x\\ p_y\\ p_z\end{array}\right)\right)$$

Procedure according to Claim 3 ,characterized in that the rotation angles and the coordinates of the LiDAR are part of a state vector χ_k|k-1 = [p_x p_y p_z ϕ, θ and ψ]^T , according to$$\left(\begin{array}{c}{r}_x\\ r_y\\ r_{\mathrm{e.g}}\end{array}\right)=r\left(x\right)=T_{Ev}\left(\left(\begin{array}{c}{m}_x\\ m_y\\ m_{\mathrm{e.g}}\end{array}\right)-\left(\begin{array}{c}{p}_x\\ p_y\\ p_{\mathrm{e.g}}\end{array}\right)\right)$$

Verfahren gemäß denAnsprüchen 1 bis4,dadurch gekennzeichnet, dass gemäß Verfahrensschritt b) das LiDAR-Sensormodell h_r,i(x) eines Laserstrahls i zu dem Prüfpunkt aus Abständen r = [r_x r_y r_z]^T des Prüfpunkts in LiDAR-Koordinaten berechnet wird gemäß$$h_{r,i}\left(r\right)=\sqrt{r_x^2+r_y^2+r_z^2}$$

Procedure in accordance with Claims 1 until 4 ,characterized in that according to method step b), the LiDAR sensor model h_r,i (x) of a laser beam i to the test point is calculated from distances r = [r_x r_y r_z ]^T of the test point in LiDAR coordinates according to$$H_{r,i}\left(r\right)=\sqrt{r_x^2+r_y^2+r_{\mathrm{e.g}}^2}$$

Verfahren gemäß denAnsprüchen 1 bis5,dadurch gekennzeichnet, dass die Korrektur gemäß Verfahrensschritt c) gemäß$$K_i=P_{i|i-1}{H}_{r,i}^T{\left(H_{r,i}{P}_{i|i-1}{H}_{r,i}^T+R_i\right)}^{-1}$$

$$x_i=x_{i|i-1}+K_i\left(z_{r,i}-h_{r,i}\right)$$

$$P_i=\left(I-K_i{H}_{r,i}\right)P_{i|i-1}$$

durchgeführt wird.Procedure in accordance with Claims 1 until 5 ,characterized in that the correction according to method step c) according to$$K_i=P_{i|i-1}{H}_{r,i}^T{\left(H_{r,i}{P}_{i|i-1}{H}_{r,i}^T+R_i\right)}^{-1}$$

$$x_i=x_{i|i-1}+K_i\left({\mathrm{e.g}}_{r,i}-H_{r,i}\right)$$

$$P_i=\left(I-K_i{H}_{r,i}\right)P_{i|i-1}$$

is carried out.Verfahren gemäß denAnsprüchen 1 bis6,dadurch gekennzeichnet, dass gemäß Verfahrensschritt c) ein oder mehrere Messpunkte i mit zugehörigen Größen H_r,i, z_r,i und h_r,i innerhalb eines Zeitschritts k berechnet werden.Procedure in accordance with Claims 1 until 6 ,characterized in that , according to method step c), one or more measuring points i with associated variables H_r,i , z_r,i and h_r,i are calculated within a time step k.Verfahren gemäß denAnsprüchen 1 bis7,dadurch gekennzeichnet, dass die Korrektur gemäß Verfahrensschritt c) auf jeden Messpunkt i einzeln durchgeführt wird, gemäß$$x_i=x_{i|i-1}+K_i\left(z_{r,i}-h_{r,i}\right)$$

$$P_i=\left(I-K_i{H}_{r,i}\right)P_{i|i-1}$$

$$K_i=P_{i|i-1}{H}_{r,i}^T{\left(H_{r,i}{P}_{i|i-1}{H}_{r,i}^T+R_i\right)}^{-1}$$

und mehrfach für jeden einzelnen Messpunkt i einzeln durchlaufen wird.Procedure in accordance with Claims 1 until 7 ,characterized in that the correction according to method step c) is carried out individually on each measuring point i, according to$$x_i=x_{i|i-1}+K_i\left({\mathrm{e.g}}_{r,i}-H_{r,i}\right)$$

$$P_i=\left(I-K_i{H}_{r,i}\right)P_{i|i-1}$$

$$K_i=P_{i|i-1}{H}_{r,i}^T{\left(H_{r,i}{P}_{i|i-1}{H}_{r,i}^T+R_i\right)}^{-1}$$

and is run through several times for each individual measuring point i.Verfahren gemäß denAnsprüchen 1 bis8,dadurch gekennzeichnet, dass zur Korrektur gemäß Verfahrensschritt c) n Messpunkte i eines Zeitschritts k in einen Vektor z_r,k = [z_r,1 z_r,2 ... z_r,n]^T, die LiDAR-Modellwerte zu einem Vektor h_r,k = [h_r,1 h_r,2 ... h_r,n]^T und der Jakobimatrix H_r,k = [H_r,1 H_r,2 ... H_r,n]^T und ein Messrauschen R_k = diag(R₁, R₂, ... R_n) gemäß$$K_k=P_{k|k-1}{H}_{r,k}^T{\left(H_{r,k}{P}_{k|k-1}{H}_{r,k}^T+R_k\right)}^{-1}$$

$$x_k=x_{k|k-1}+K_k\left(z_{r,k}-h_{r,k}\right)$$

$$P_k=\left(I-K_k{H}_{r,k}\right)P_{k|k-1}$$

berechnet werden.Procedure in accordance with Claims 1 until 8th ,characterized in that for correction according to method step c) n measuring points i of a time step k into a vector z_r,k = [z_r,1 z_r,2 ... z_r,n ]^T , the LiDAR model values into one Vector h_r,k = [h_r,1 h_r,2 ... h_r,n ]^T and the Jacobian matrix H_r,k = [H_r,1 H_r,2 ... H_r,n ]^T and a measurement noise R_k = diag(R₁ , R₂ , ... R_n ) according to$$K_k=P_{k|k-1}{H}_{r,k}^T{\left(H_{r,k}{P}_{k|k-1}{H}_{r,k}^T+R_k\right)}^{-1}$$

$$x_k=x_{k|k-1}+K_k\left({\mathrm{e.g}}_{r,k}-H_{r,k}\right)$$

$$P_k=\left(I-K_k{H}_{r,k}\right)P_{k|k-1}$$

be calculated.Verfahren gemäß denAnsprüchen 1 bis9,dadurch gekennzeichnet, dass anstelle von Eulerwinkeln auch Quaternionen für die Berechnung gewählt werden.Procedure in accordance with Claims 1 until 9 ,characterized in that quaternions are also chosen for the calculation instead of Euler angles.Verfahren gemäßAnspruch 1,dadurch gekennzeichnet, dass anstelle eines LiDAR-Sensorsystems (20) eine Tiefenkamera, insbesondere eine Stereo-Kamera oder eine Time-of-Flight(ToF)-Kamera, eingesetzt wird.Procedure according to Claim 1 ,characterized in that instead of a LiDAR sensor system (20), a depth camera, in particular a stereo camera or a time-of-flight (ToF) camera, is used.

Images