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

Abstract

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) With the help of a corrected pose χk, new pose information χk|k-1 is predicted, b) from the pose χk|k predicted according to a). -1 and a height map of the surroundings, LiDAR measured values hr,i are estimated, c) using the difference between the LiDAR measured values hr,i estimated according to b) and measured or interpolated LiDAR measured values zr,i, the predicted pose χk|k-1 is determined according to method step a) corrected.

Description

Technical area

The invention relates to a method for roll angle and pitch angle error compensation in a terrain height measurement using a LiDAR sensor system.

State of the art

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 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.

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.

Presentation of the invention

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).

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.

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 .

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 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 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)$$

$$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}$$

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.

$$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.

$$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 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.

Advantages of the invention

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.

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.

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.

Brief description of the drawings

The invention is described in more detail below with reference to the drawings.

• 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 .

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.

1 shows in a schematic manner a coordinate system with the spatial directions

According to the representation 2 10 contour lines detected by a LiDAR sensor system 20 of a vehicle can be seen.

3 shows a schematic representation of the LiDAR sensor system 20 with the pitch angle θ _i entered.

$$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$$

$$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$$

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).

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 the LiDAR 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 the LiDAR 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)$$

$$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.

Often the position of the LiDAR sensor system 20 can be determined very precisely using GPS and other devices, while an orientation of the LiDAR 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 the LiDAR sensor system 20. As a rule, terrain with low gradient values should be measured using the LiDAR 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.

When used in vehicles 10, for example, a LiDAR 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 the LiDAR sensor system 20 hits the ground approximately vertically. LiDAR beams emitted by the LiDAR sensor system 20 that hit parts of your own vehicle 10 are not suitable.

Furthermore, it has been found that correcting the large number of measured values that a LiDAR 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.

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 the LiDAR sensor system 20. The range of the LiDAR 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.

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.

• 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)$$

The calculation is carried out using the roll angle ϕ, pitch angle θ and yaw angle ip of the LiDAR sensor system 20 with respect to a world coordinate system. _{This allows the distances r _ _} ^_ _{_ _} 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)$$

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}$$

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)$$

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.

$$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$$

$$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}$$

$$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}$$

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.

The method proposed according to the invention is shown above using a LiDAR sensor system 20. Instead of a LiDAR sensor or a LiDAR 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.

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 2013/0041549 A1 [0002]
• US 2014/0270744 A [0003]

Claims (11)

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). 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 . 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. 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)$$

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}$$

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. 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. 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. 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. Procedure in accordance with Claims 1 until 9 , characterized in that quaternions are also chosen for the calculation instead of Euler angles. 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