DE102022208972A1 - Verfahren zur Rollwinkel- und Nickwinkelfehlerkompensation bei einer Geländehöhenmessung mit einem LiDAR-Sensorsystem - Google Patents

Abstract

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

Description

• Technisches Gebiet
• Die Erfindung bezieht sich auf ein Verfahren zur Rollwinkel- und Nickwinkelfehlerkompensation bei einer Geländehöhenmessung mittels eines LiDAR-Sensorsystems.
• Stand der Technik
• 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 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.
• 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.
• Darstellung der Erfindung
• 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.
• 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.
• 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 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 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 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)$$

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

  
• 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 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.
• 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 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.
• Vorteile der Erfindung
• 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.
• 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.
• Durch das erfindungsgemäß vorgeschlagene Verfahren kann im Gegensatz zum merkmalbasierten SLAM-Verfahren Rechenzeit gespart werden, da eine aufwändige Merkmalextrahierung nicht notwendig ist.
• Kurze Beschreibung der Zeichnungen
• Anhand der Zeichnungen wird die Erfindung nachstehend eingehender beschrieben.
• 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.
• Ausführungsformen der Erfindung
• 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.
• 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.
• Der Darstellung gemäß 2 sind von einem LiDAR-Sensorsystem 20 eines Fahrzeugs 10 detektierte Höhenlinien zu entnehmen.
• 3 zeigt eine schematische Darstellung des LiDAR-Sensorsystems 20 mit eingetragenem Nickwinkel θ_i.
• 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$$

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

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

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

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

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

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

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

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

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

  
• 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.
• 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.
• 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.
• ZITATE ENTHALTEN IN DER BESCHREIBUNG
• 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.
• Zitierte Patentliteratur
• US 2013/0041549 A1 [0002]
• US 2014/0270744 A [0003]

Claims (11)

1. 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.
2. 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.
3. Verfahren gemäß den Ansprüchen 1 bis 2, 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.
4. 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)$$

   
5. Verfahren gemäß den Ansprüchen 1 bis 4, 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}$$

   
6. Verfahren gemäß den Ansprüchen 1 bis 5, 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.
7. Verfahren gemäß den Ansprüchen 1 bis 6, 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.
8. Verfahren gemäß den Ansprüchen 1 bis 7, 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.
9. Verfahren gemäß den Ansprüchen 1 bis 8, 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.
10. Verfahren gemäß den Ansprüchen 1 bis 9, dadurch gekennzeichnet, dass anstelle von Eulerwinkeln auch Quaternionen für die Berechnung gewählt werden.
11. 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.

Images