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

Abstract

The invention relates to a method for pitch angle error and yaw angle error compensation in a terrain height measurement with a LiDAR sensor system with the following method steps: a) Using a corrected pose χk-1, a new pose χk|k-1 is predicted, from the predicted pose χk|k -1 and a height map of the surroundings, a forecast range information h comprising at least three (n) estimated LiDAR measurement values is calculated, predicted range information h and measured range information z have the same constant angular distance from Δφ; c) a shift yk between them is determined from the predicted range information h and the measured range information z; yk, a corrected pose χk is calculated.

Description

Technical area

The present invention relates to a method for pitch angle error and yaw angle error compensation in a terrain height measurement with a LiDAR sensor system. Furthermore, the invention relates to the use of the method in a LiDAR sensor system of a vehicle.

State of the art

US 2015/0045059 refers to orientation information, where, among other angles, the azimuth angle is also determined. The current position and orientation information are associated with three-dimensional localization by accessing a mobile data collection platform when the image is captured. The image, the fixed position and the orientation information are stored in a hardware memory of the mobile data on the platform. Using this approach, corrections can be made to improve position detection in GNSS and GPS systems.

US 2013/0041549 A1 refers to spatial data transmitted from a spatial database to a controller, the spatial data comprising images derived from an optical sensor subsystem and merged with other data obtained from a number of different types of sensors, including a GNSS or an inertial measurement system , come. A Kalman filter is used, which works as part of a prediction correction algorithm. The algorithm first works according to a mathematical model to predict certain values of different stages at a time step K+1, which are available on the known inputs at time step K+1. The known values of the stage at a previous time K are then used. The predicted value is then corrected, for which current measurements of the vehicle at a time step K+1 and the optimized statistical properties of the model used are evaluated.

Presentation of the invention

1. a) Mittels einer korrigierten Pose χ_k-1 wird eine neue Pose χ_k|k-1 prognostiziert, wobei aus der prognostizierten Pose χ_k|k-1 und einer Höhenkarte m_i der Umgebung eine mindestens drei (n) geschätzte LiDAR-Messwerte umfassende prognostizierte Reichweiteninformation h berechnet,
2. b) es wird eine mithilfe des LiDAR-Sensors oder des LiDAR-Sensorsystems gemessenen Werten und mit n korrespondierenden gemessenen LiDAR-Messwerten verknüpfte Reichweiteninformation z gebildet, wobei sowohl die prognostizierte Reichweiteninformation h als auch eine gemessene Reichweiteninformation z einen gleichen konstanten Winkelabstand aus Δφ aufweisen;
3. c) aus der prognostizierten Reichweiteninformation h und der gemessenen Reichweiteninformation z wird eine Verschiebung y_k zwischen diesen ermittelt;
4. d) aus der neu prognostizierten Pose χ_k|k-1 und der Verschiebung y_k wird eine korrigierte Pose χ_k berechnet.

According to the invention, a method is proposed for pitch angle error and yaw angle error compensation in a terrain height measurement with a LiDAR sensor or a LiDAR sensor system with the following method steps:

1. a) Using a corrected pose χ _k-1 , a new pose χ _k|k-1 is predicted, whereby at least three (n) estimated LiDAR measurements are obtained from the predicted pose χ _k|k-1 and a height map m _i of the environment comprehensive forecast range information h is calculated,
2. b) range information z is formed using measured values using the LiDAR sensor or the LiDAR sensor system and linked to n corresponding measured LiDAR measurement values, whereby both the predicted range information h and a measured range information z have the same constant angular distance from Δφ;
3. c) a shift y _k between them is determined from the predicted range information h and the measured range information z;
4. d) a corrected pose χ _k is calculated from the newly predicted pose χ _k|k-1 and the displacement y _k .

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

In a further development of the method according to the invention, according to method step a), a new pose χ k| _{k- 1 is predicted in a prediction step with a corrected pose χ k-1} $$x_{x|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$$

berechnet.In the method proposed according to the invention, according to method step b), with each test point m _i a predicted range r _i = [r _x r _y r _z ] ^T in LiDAR coordinates from the coordinates of the test point m _i in world coordinates and the coordinates of the LiDAR p = [ p _x p _y p _z ] ^T in world coordinates according to $$r_i\left(x\right)=\left(m_i-p\left(x\right)\right)$$

calculated.

In an advantageous development of the solution proposed according to the invention, after method step b), predicted range information h _i (r) of the laser beam with index i to the test point m _i is calculated from the ranges r _i (χ) = [r _x r _y r _z ] ^T of the test point in LiDAR coordinates $$H_i\left(r\right)=\left|r_i\left(x\right)\right|=\sqrt{r_x^2+r_y^2+r_{\mathrm{e.g}}^2}$$

In a further embodiment of the solution according to the invention, a number n of different test points are used for a time step k and the predicted range information h _i (r) becomes a vector h = [h ₁ ... h _n ] ^T and the corresponding measured range information z _i a vector z = [z ₁ ... z _n ] ^T.

mit j ≙ Anzahl der Stellen, um die der Messvektor der LiDAR-Sensormodelle h_r,i und der Messvektor der realen Messwerte z_r,i gegeneinander verschoben sind.In _the method _proposed according to the invention, _a ^cross -correlation function diff(z) calculated according to $$X_{H\mathrm{e.g}}\left(j\right)=\{\begin{array}{cc}{\displaystyle \sum _{i=0}^{n-j-1}\Delta H_{i+j}\Delta {\mathrm{e.g}}_i}& j\ge 0\\ X_{H\mathrm{e.g}}\left(-j\right)& j<0\end{array}$$

with j ≙ number of places by which the measurement vector of the LiDAR sensor models h _r,i and the measurement vector of the real measured values z _r,i are shifted relative to one another.

In the proposed method, the difference of y _k between the measurement and the predicted measurement h is calculated from a constant angle difference Δφ for the index _j , where

mithilfe eines Residuums y_k ersatzweise berechnet zu $$x_k=x_{k|k-1}+K_k{y}_k$$

In the solution proposed according to the invention, a correction step is carried out according to $$x_k=x_{k|k-1}+K_k\left({\mathrm{e.g}}_k-H_k\left(x_{k|k-1}\right)\right)$$

calculated using a residual y _k as an alternative $$x_k=x_{k|k-1}+K_k{y}_k$$

aus einer propagierten Kovarianzmatrix P_yy,k und einer kreuzkorrelierten Matrix P_xy,k gebildet.In the method according to the invention, a Kalman gain is used $$K_k=P_{xy,k}{P}_{yy,k}^{-1}$$

formed from a propagated covariance matrix P _yy,k and a cross-correlated matrix P _xy,k .

The proposed method provides that error compensation of yaw angle errors and/or pitch angle errors is carried out independently of one another or one after the other.

berechnet werden, mit Δh = sobel(h), Δz = sobel(z) oder mittels Sobel-, Prewitt- oder Scharroperatorkernel gefaltet werden.In the method according to the invention, simultaneous yaw angle and pitch angle error compensation is carried out in such a way that n horizontal xs vertical measured values z and the same number of propagated values h according to $$x_{H\mathrm{e.g}}\left(j,v\right)=\{\begin{array}{cc}{\displaystyle \sum _{u=0}^{s-v-1}{\displaystyle \sum _{i=0}^{n-j-1}\Delta H_{i+j,u+v}\Delta {\mathrm{e.g}}_{i,u}}}& j\ge \mathrm{0,}v\ge 0\\ H_{H\mathrm{e.g}}\left(-j,-v\right)& j<\mathrm{0,}v<0\\ H_{H\mathrm{e.g}}\left(-j,v\right)& j<\mathrm{0,}v\ge 0\\ H_{H\mathrm{e.g}}\left(j,-v\right)& j\ge \mathrm{0,}v<0\end{array}$$

can be calculated with Δh = sobel(h), Δz = sobel(z) or folded using Sobel, Prewitt or Scharroperator kernels.

berechnet, wobei j mit einem zugehörigen horizontalen Winkel Δφ und v mit einem vertikalen Winkel Δθ berechnet werden.Following the solution according to the invention, a displacement y _k in the horizontal or vertical direction is achieved $$y_k=\left(\begin{array}{c}J\Delta \phi \\ v\Delta \theta \end{array}\right)\text{with}{X}_{H\mathrm{e.g}}\left(J,v\right)\ge X_{H\mathrm{e.g}}\left(j,v\right)\forall j,v$$

calculated, where j is calculated with an associated horizontal angle Δφ and v with a vertical angle Δθ.

Advantages of the invention

The solution proposed according to the invention makes it possible to avoid frequent extrinsic calibration of the LiDAR sensor system to reduce tilt errors. Furthermore, the compensation method proposed according to the invention makes it possible to use a more cost-effective LiDAR sensor system and a more cost-effective inertial sensor system, which can certainly have larger errors and tolerances, but which can be compensated for again by using the method proposed according to the invention.

Brief description of the drawings

Embodiments of the invention are explained in more detail with reference to the drawings and the following description.

• 1 eine Korrelationsfunktion X_hz (j) mit einem bei -3 liegenden Maximum und
• 2 ein Koordinatensystem mit X, Y und Z-Richtung und den Winkeln ϕ, θ und ψ.

It shows:

• 1 a correlation function X _hz (j) with a maximum at -3 and
• 2 a coordinate system with X, Y and Z directions and the angles ϕ, θ and ψ.

Embodiments of the invention

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.

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

The method proposed according to the invention allows new pose information χ _{k|k- 1 to be predicted using a corrected pose χ k-1} according to a prediction step $$x_{k|k-1}=f\left(x_{k-1},u_{k-1}\right)$$

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

sowie $$h_i\left(r\right)=\left|r_i\left(x\right)\right|=\sqrt{r_x^2+r_y^2+r_z^2}$$

Within this procedure, in a first step, a predicted range information h consisting of at least three (n) estimated LiDAR measurements is calculated from the predicted pose χ _k|k-1 and a height map m _i of the environment. This is done using $$r_i\left(x\right)=\left(m_i-p\left(x\right)\right)$$

as well as $$H_i\left(r\right)=\left|r_i\left(x\right)\right|=\sqrt{r_x^2+r_y^2+r_{\mathrm{e.g}}^2}$$

$$y_k=J\Delta \phi \text{ mit }{X}_{hz}\left(J\right)\ge X_{hz}\left(j\right)\forall j$$

$$x_k=x_{k|k-1}+K_k{y}_k$$

$$X_{hz}\left(j,v\right)=\{\begin{array}{cc}{\displaystyle \sum _{u=0}^{s-v-1}{\displaystyle \sum _{i=0}^{n-j-1}\Delta h_{i+j,u+v}\Delta z_{i,u}}}& j\ge \mathrm{0,}v\ge 0\\ X_{hz}\left(-j,-v\right)& j<\mathrm{0,}v<0\\ X_{hz}\left(-j,v\right)& j<\mathrm{0,}v\ge 0\\ X_{hz}\left(j,-v\right)& j\ge \mathrm{0,}v<0\end{array}$$

The range information h also includes range information z measured using the LiDAR sensor system and equipped with as many (n) corresponding measured LiDAR measurement values, whereby both the predicted range information h and the measured range information z have the same constant angular distance Δφ. As part of the further procedure, in a second step, a shift y _k between the predicted and measured range information h, z is calculated using the predicted range information h and the measured range information z. This is done using the following equations: $$X_{H\mathrm{e.g}}\left(j\right)=\{\begin{array}{cc}{\displaystyle \sum _{i=0}^{n-j-1}\Delta H_{i+j}\Delta {\mathrm{e.g}}_i}& j\ge 0\\ H_{\underset{\_}{H\mathrm{e.g}}}\left(-j\right)& j<0\end{array}$$

$$y_k=J\Delta \phi \text{with}{X}_{H\mathrm{e.g}}\left(J\right)\ge X_{H\mathrm{e.g}}\left(j\right)\forall j$$

$$x_k=x_{k|k-1}+K_k{y}_k$$

$$X_{H\mathrm{e.g}}\left(j,v\right)=\{\begin{array}{cc}{\displaystyle \sum _{u=0}^{s-v-1}{\displaystyle \sum _{i=0}^{n-j-1}\Delta H_{i+j,u+v}\Delta {\mathrm{e.g}}_{i,u}}}& j\ge \mathrm{0,}v\ge 0\\ X_{H\mathrm{e.g}}\left(-j,-v\right)& j<\mathrm{0,}v<0\\ X_{H\mathrm{e.g}}\left(-j,v\right)& j<\mathrm{0,}v\ge 0\\ X_{H\mathrm{e.g}}\left(j,-v\right)& j\ge \mathrm{0,}v<0\end{array}$$

und $$x_k=x_{k|k-1}+K_k{y}_k$$

durchgeführt wird.In a final step, corrected pose information χ _k is calculated using the new predicted pose information χ _k|k-1 and the shift y _k , which is based on the equations $$K_k=P_{xy,k}{P}_{yy,k}^{-1}$$

and $$x_k=x_{k|k-1}+K_k{y}_k$$

is carried out.

The position of the LiDAR sensor system can often be determined very precisely using the Global Positioning System (GPS) or a tachymeter. On the other hand, the orientation of the LiDAR sensor system cannot be determined so precisely, so that the uncertain orientation within the vehicle, for example, contributes the largest error when calculating the height map. For this reason, the pitch angle error and yaw angle error compensation method proposed according to the invention is intended to avoid corresponding alignment errors that occur, for example, when installing the sensor or in the event of damage to the body or the like, and to not insignificantly falsify the spectrum of information recorded via the LiDAR sensor system.

The LiDAR sensor system can provide a wide range of measurements, with some measurements being better suited for correction, some being worse, and other measurements not being able to be processed at all. For example, recorded measurements at medium distances are more suitable in rugged environments with large changes in gradient. However, measured values from too great a distance appear less suitable because the signal-to-noise ratio can be very large due to the low measurement intensity compared to the scattered light. Environmental influences such as sunlight, dust, rain, fog and the like also have a greater potential for interference when viewed from a greater distance. However, LiDAR beams that hit parts of your own vehicle cannot be exploited at all.

Normally, Kalman filter states would be corrected with the differences between all measured values and predicted measured values, which would be very computationally time-intensive in the case of LiDAR data. Therefore The measured values and the predicted measured values are first compared with each other using the cross-correlation function, then the distance of the maximum of the cross-correlation function from the center can be determined and finally only this distance can be used for correction instead of the large number of individual differences. This procedure allows the required computing effort to be reduced significantly.

Using a Kalman filter, the method can be implemented as described below. The individual calculation steps to be carried out for the method proposed according to the invention, which are run through in a computing unit, will now be described in order to accordingly compensate for the yaw angle errors or pitch angle errors of the LiDAR sensor system in its installation position in the vehicle.

To calculate the LiDAR sensor model h _r,i, n test points m _i = [m _x m _y m _z ] ^T are determined by selecting from the map material. These correspond to as many n measured ranges z _i in that each of the test points m _i is presumably detected by a range measurement z _i because the respective laser beam i with the polar angles Φ _i = [ψ _i θ _i ] ^T with respect to the LiDAR Coordinate system with the Euler angles ϕ, θ and ip of the LiDAR sensor system with respect to the world coordinate system is directed to this test point. The test point can be selected by first calculating the position of the laser beam as a function of the laser beam propagation l _i according to the following relationships: $$\begin{array}{l}{w}_{x,i}=p_x-l_i(\text{cos}{\psi }_i\text{cos}\psi \text{cos}\theta \text{cos}{\theta }_i\\ -\text{cos}{\theta }_i\text{sin}{\psi }_i(\text{cos}\varphi \text{sin}\psi \\ -\text{cos}\psi \text{sin}\varphi \text{sin}\theta )-\text{sin}{\theta }_l(\text{sin}\theta \text{sin}\psi +\text{cos}\varphi \text{cos}\psi \text{sin}\theta ))\\ w_{y,i}=p_y+l_i(\text{sin}{\theta }_l(\text{cos}\psi \text{sin}\varphi -\text{cos}\varphi \text{sin}\psi \text{sin}\theta )\\ +\text{cos}{\theta }_i\text{sin}{\psi }_i(\text{cos}\varphi \text{cos}\psi +\text{sin}\varphi \text{sin}\psi \text{sin}\theta )\\ +\text{cos}{\psi }_i\text{cos}\theta \text{cos}{\theta }_i\text{sin}\psi )\\ w_{\mathrm{e.g},i}=p_{\mathrm{e.g}}+l_i\left(\text{cos}\theta \text{cos}{\theta }_i\text{sin}\varphi \text{sin}{\psi }_i-\text{cos}{\psi }_i\text{cos}{\theta }_i\text{sin}\theta -\text{cos}\varphi \text{cos}\theta \text{sin}{\theta }_i\right)\end{array}$$

Mit einem jeden Prüfpunkt m_i wird die eine prognostizierte Reichweite r_i = [r_x r_y r_z]^T in LiDAR-Koordinaten ausgedrückt und aus den Koordinaten des Prüfpunkts m_i in Weltkoordinaten und den Koordinaten des LiDAR-Sensors beziehungsweise LiDAR-Sensorsystems p = [p_x p_y p_z]^T in Weltkoordinaten gemäß $$r_i\left(x\right)=\left(m_i-p\left(x\right)\right)$$

berechnet.The desired test point m _i = [w _x,i w _y,i w _z,i ] ^T is the point on the surface map as a function m _i,z of w _x,i and w _y,i with the smallest distance to the LiDAR Sensor system for which the following applies: $$w_{\mathrm{e.g},i}\cong m_{i,\mathrm{e.g}}\left(w_{x,i},w_{y,i}\right)$$

With each test point m _i, the predicted range r _i = [r _x r _y r _z ] ^T is expressed in LiDAR coordinates and from the coordinates of the test point m _i in world coordinates and the coordinates of the LiDAR sensor or LiDAR sensor system p = [p _x p _y p _z ] ^T in world coordinates according to $$r_i\left(x\right)=\left(m_i-p\left(x\right)\right)$$

calculated.

The coordinates p _x , p _y and p _z of the LiDAR sensor system can be part of the state vector χ _k|k-1 = [p _x p _y p _z ϕ θ ψ] ^T of the Kalman filter.

The predicted range information h _i (r) of the laser beam with the index i to the test point m _i is calculated from the ranges r _i (x) = [r _x r _y r _z ] ^T of the test point in LiDAR coordinates according to the Euclidean formula $$H_i\left(r\right)=\left|r_i\left(x\right)\right|=\sqrt{r_x^2+r_y^2+r_{\mathrm{e.g}}^2}$$

For a single time step k, the number n of different test points is used to combine the predicted range information h _i (r) into a vector h = [h ₁ ... h _n ] ^T and to combine the corresponding measured range information z _i into a vector z = [z ₁ ... z _n ] ^T to summarize. _{The discrete} ^cross -correlation _function ) lists: $$X_{H\mathrm{e.g}}\left(j\right)=\{\begin{array}{cc}{\displaystyle \sum _{i=0}^{n-j-1}\Delta H_{i+j}\Delta {\mathrm{e.g}}_i}& j\ge 0\\ X_{\underset{\_}{H\mathrm{e.g}}}\left(-j\right)& j<0\end{array}$$

The index j denotes by how many places the measurement vector of the LiDAR sensor models h _r,i and the measurement vector of the real measured values z _r,i are shifted relative to one another.

In relation to 1 It should be noted that the cross-correlation function X _hz (j) is shown in this figure. Out of 1 It turns out that the maximum j is at -3. 2 shows a coordinate system with the Euler angles ϕ, θ and ψ and the spatial directions x, y and z.

With the index j, at which the cross-correlation function X _hz (j) is maximum, the difference y _k between the measurement z and the predicted measurement h is calculated from the constant angular difference Δϕ as follows: $$y_k=J\Delta \phi \text{with}{X}_{H\mathrm{e.g}}\left(J\right)\ge X_{H\mathrm{e.g}}\left(j\right)\forall j$$

kann mithilfe des Residuums y_k ersatzweise zu $$x_k=x_{k|k-1}+K_k{y}_k$$

bestimmt werden, wobei der Kalman-Gain K_k zuvor gemäß dem Sigma-Point-Kalman-Filter-Verfahren gemäß der Gleichung $$K_k=P_{xy,k}{P}_{yy,k}^{-1}$$

aus einer propagierten Kovarianzmatrix P_yy,k und einer Kreuzkorrelationsmatrix P_xy,k abgeleitet werden kann.The correction step according to the calculation $$x_k=x_{k|k-1}+K_k\left({\mathrm{e.g}}_k-H_k\left(x_{k|k-1}\right)\right)$$

can be substituted using the residual y _k $$x_k=x_{k|k-1}+K_k{y}_k$$

are determined, where the Kalman gain K _k is previously determined according to the sigma point Kalman filter method according to the equation $$K_k=P_{xy,k}{P}_{yy,k}^{-1}$$

can be derived from a propagated covariance matrix P _yy,k and a cross-correlation matrix P _xy,k .

berechnet wird, wobei Δh = sobel(h) und Δz = sobel(z) zuvor beispielsweise mithilfe des Sobel-, Prewitt- oder Scharroperatorkernels gefaltet werden und mithilfe der Faltungen in ein mittelwertfreies zweidimensionales Bild umgewandelt werden.According to this method, both yaw angle errors and pitch angle errors can be compensated between an inertial sensor system and the LiDAR sensor system. The proposed error compensation of yaw and pitch angle errors can be carried out independently or one after the other. However, it is particularly advantageous to simultaneously compensate for both errors, ie the yaw angle error and also the pitch angle error. This is done using n horizontal xs vertical measured values z and as many predicted values h according to which the two-dimensional cross-correlation is based on $$X_{H\mathrm{e.g}}\left(j,v\right)=\{\begin{array}{cc}{\displaystyle \sum _{u=0}^{s-v-1}{\displaystyle \sum _{i=0}^{n-j-1}\Delta H_{i+j,u+v}\Delta {\mathrm{e.g}}_{i,u}}}& j\ge \mathrm{0,}v\ge 0\\ X_{H\mathrm{e.g}}\left(-j,-v\right)& j<\mathrm{0,}v<0\\ X_{H\mathrm{e.g}}\left(-j,v\right)& j<\mathrm{0,}v\ge 0\\ X_{H\mathrm{e.g}}\left(j,-v\right)& j\ge \mathrm{0,}v<0\end{array}$$

is calculated, where Δh = sobel(h) and Δz = sobel(z) are previously convolved using, for example, the Sobel, Prewitt or Scharroperator kernel and are converted into a mean-free two-dimensional image using the convolutions.

ausgedrückt werden, wobei aus dem Index j und dem zugehörigen horizontalen Winkel Δφ und dem Index v vertikale Winkel Δθ berechnet werden.The displacement y _k in the horizontal and vertical directions can be expressed as a vector $$y_k\left(\begin{array}{c}J\Delta \phi \\ v\Delta \theta \end{array}\right)\text{with}{X}_{H\mathrm{e.g}}\left(J,v\right)\ge X_{H\mathrm{e.g}}\left(j,v\right)\forall j,v$$

can be expressed, whereby vertical angles Δθ are calculated from the index j and the associated horizontal angle Δφ and the index v.

As an alternative to a LiDAR sensor or a LiDAR sensor system, a depth camera, for example a stereo camera or a time-of-flight (ToF) camera, can also be used. As an alternative to the calculation described, which is carried out using the Euler angles, quaternions could also be used for the calculation.

The method proposed according to the invention for compensating for yaw angle errors and pitch angle errors can be used in autonomous and semi-autonomous vehicles and, for example, in aviation. A variety of applications can be imagined that carry out LiDAR-based environmental detection on a mobile platform.

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.

QUOTES INCLUDED IN THE DESCRIPTION

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.

Cited patent literature

• US 20150045059 [0002]
• US 20130041549 A1 [0003]

Claims (14)

Method for pitch angle error and yaw angle error compensation in a terrain height measurement with a LiDAR sensor system with the following process steps: a) Using a corrected pose χ _k-1, a new pose χ _k|k-1 is predicted, with the predicted pose χ _{k|k- 1} and a height map m _{i of} the environment, a forecast range information h comprising at least three (n) estimated LiDAR measurement values is calculated, b) a range information z is formed using measured values using the LiDAR sensor system and linked to n corresponding measured LiDAR measurement values, where both the predicted range information h and a measured range information z have the same constant angular distance from Δφ; c) a shift y _k between them is determined from the predicted range information h and the measured range information z; d) a corrected pose χ _k is calculated from the newly predicted pose χ _k|k-1 and the displacement y _k . Procedure according to Claim 1 , characterized in that according to method step a) in a prediction step with a corrected pose χ _k-1, a new pose χ _{k | k-1} is predicted according to $$x_{k|k-1}=f\left(x_{k-1},u_{k-1}\right)$$

and $$P_{k|k-1}=F_{k-1}{P}_{k-1}{F}_{k-1}^T\_b_{k-1}{Q}_{k-1}{b}_{k-1}^T$$

Procedure in accordance with Claims 1 until 2 , characterized in that according to method step b) with each test point m _i a predicted range r _i = [r _x r _y r _z ] ^T in LiDAR coordinates from the coordinates of the test point m _i in world coordinates and the coordinates of the LiDAR p = [ p _x p _y p _z ] ^T in world coordinates according to $$r_i\left(x\right)=\left(m_i-p\left(x\right)\right)$$

is calculated. Procedure in accordance with Claims 1 until 2 , characterized in that , according to method step b), a predicted range information h _i (r) of the laser beam with index i to the test point m _i is calculated from the ranges r _i (χ) = [r _x r _{y r z} ] ^T of the test point in LiDAR coordinates according to $$H_i\left(r\right)=\left|r_i\left(x\right)\right|=\sqrt{r_x^2+r_y^2+r_{\mathrm{e.g}}^2}$$

Procedure in accordance with Claims 1 until 4 , characterized in that a number n of different test points is used for a time step k and the predicted range information h _i (r) becomes a vector h = [h ₁ ... h _n ] ^T and the corresponding measured range information z _i forms a vector z = [z ₁ ... z _n ] ^T can be summarized. Procedure in accordance with Claims 1 until 5 _, characterized in that _a ^cross _- correlation function diff(z) is calculated according to $$X_{H\mathrm{e.g}}=\{\begin{array}{cc}{\displaystyle \sum _{i=0}^{n-j-1}\Delta H_{i+j}\Delta {\mathrm{e.g}}_i}& j\ge 0\\ X_{\underset{\_}{H\mathrm{e.g}}}\left(-j\right)& j<0\end{array}$$

with j ≙ number of places by which the measurement vector of the LiDAR sensor models h _r,i and the measurement vector of the real measured values z _r,i are shifted relative to one another. Procedure in accordance with Claims 1 until 6 , characterized in that for the index j, at which X _hz (j) = max, the difference of y _k between the measurement and the predicted measurement h is calculated from a constant angle difference Δφ, according to

Procedure in accordance with Claims 1 until 7 , characterized in that a correction step according to $$x_k=x_{k|k-1}+K_k\left({\mathrm{e.g}}_k-H_k\left(x_{k|k-1}\right)\right)$$

$$x_k=x_{k|k-1}+K_k{y}_k$$

(8th). Procedure according to Claim 8 , characterized in that a Kalman gain according to $$K_k=P_{xy,k}{P}_{yy,k}^{-1}$$

is formed from a propagated covariance matrix P _yy,k and a cross-correlated matrix P _xy,k . Procedure in accordance with Claims 1 until 9 , characterized in that error compensation of yaw angle errors and/or pitch angle errors are carried out independently of one another or one after the other. Procedure in accordance with Claims 1 until 9 , characterized in that a simultaneous yaw angle and pitch angle error compensation is carried out in such a way that n horizontal xs vertical measured values z and as many propagated values h according to $$X_{H\mathrm{e.g}}\left(j,v\right)=\{\begin{array}{cc}{\displaystyle \sum _{u=0}^{s-v-1}{\displaystyle \sum _{i=0}^{n-j-1}\Delta H_{i+j,u+v}\Delta {\mathrm{e.g}}_{i,u}}}& j\ge \mathrm{0,}v\ge 0\\ X_{H\mathrm{e.g}}\left(-j,-v\right)& j<\mathrm{0,}v<0\\ X_{H\mathrm{e.g}}\left(-j,v\right)& j<\mathrm{0,}v\ge 0\\ X_{H\mathrm{e.g}}\left(j,-v\right)& j\ge \mathrm{0,}v<0\end{array}$$

can be calculated with Δh = sobel(h), Δz = sobel(z) or folded using Sobel, Prewitt or Scharroperator kernels. Procedure in accordance with Claims 1 until 11 , characterized in that a displacement y _k in the horizontal or vertical direction according to $$y_k=\left(\begin{array}{c}J\Delta \phi \\ v\Delta \theta \end{array}\right)\text{with}{H}_{H\mathrm{e.g}}\left(J,v\right)\ge X_{H\mathrm{e.g}}\left(j,v\right)\forall j,v$$

is calculated, where j is calculated with the associated horizontal angle Δφ and v with the vertical angle Δθ. Procedure in accordance with Claims 1 until 12 , characterized in that instead of a LiDAR sensor system, a depth camera, in particular a stereo camera or a time-of-flight (ToF) camera, is used. Procedure in accordance with Claims 1 until 12 , characterized in that instead of a calculation with Euler angles, this is carried out with quaternions.

Images