DE4138270C2

DE4138270C2 - Verfahren zur Navigation eines selbstfahrenden Landfahrzeugs

Info

Publication number: DE4138270C2
Application number: DE4138270A
Authority: DE
Inventors: Norbert Dr Paryjas; Matthias Dr Reuschenbach
Original assignee: Rheinmetall Industrie AG
Current assignee: Rheinmetall Industrie AG
Priority date: 1991-11-21
Filing date: 1991-11-21
Publication date: 1996-10-02
Anticipated expiration: 2011-11-22
Also published as: DE4138270A1

Description

Die Erfindung betrifft ein Verfahren zur Navigation eines selbstfahrenden Landfahrzeugs nach dem Oberbegriff des Anspruchs 1.

Ein derartiges Verfahren ist aus der EP 02 21 423 A1 bekannt, gemäß dem eine selbstfahrende Bodenreinigungsmaschine längs einer vorgegebenen Bewegungsbahn auf einem begrenzten, abzuarbeitenden Flächenareal geführt wird, wobei eine Folge von in einem Datenspeicher gespeicherten Wegteilstrecken abgerufen und von der Maschine abgefahren wird. Hierbei werden Markierungen wie Regalkanten oder -ränder, Begrenzungswände u. dgl. über Ultraschall- oder Laserentfernungsmesser abgetastet und jeweils im Endbereich einer erkannten Struktur eine gesonderte Überprüfung der Ausrichtung der Maschine relativ zu der erkannten Struktur vorgenommen, wozu ein Vergleich mehrerer, aufeinander folgender Abstandsmessungen bei sich fortbewegender Maschine erfolgen kann. Bei Abweichung von der Parallelausrichtung erfolgt eine Ausrich tungskorrektur durch entsprechende Befehle an den Antrieb bzw. die Lenkung. Die im Datenspeicher für die abzufahrenden Wegteilstrecken gespeicherten Daten werden während einer manuell gesteuerten Lernfahrt erzeugt. Ein derartiges Navigationsverfahren eignet sich jedoch nicht für unbeschränkte Reichweite und bedarf klarer Kanten und Raumbegrenzungen.

Aus der EP 02 73 976 A1 ist es bekannt, optische Marken paarweise längs einer zu durchfahrenden Strecke anzubringen, die von einer Videokamera eines Bodenreinigungsfahrzeugs erfaßt werden. Für Fahrten im Gelände und unbeschränkte Reichweiten ist dies jedoch nicht geeignet, abgesehen davon, daß das Anbringen von optischen Marken aufwen dig und manchmal auch nicht möglich oder unerwünscht ist.

Aus der EP-OS 03 41 985 A2 ist es ferner bekannt, Fahrbahn begrenzungslinien, die von einer Videokamera erfaßt werden, über ein Bildverarbeitungssystem zur Straßenerkennung auszunutzen. Jedoch ist dies ebenfalls nur auf diese spezielle Markierung beschränkt.

Aus den Schriften JP-Abstr. 3-242 711 A und JP-Abstr. 3-191 404 A sind Verfahren zur Navigation selbstfahrender Landfahrzeuge bekannt, bei denen die zu ermittelnde Position mit Hilfe von Videokameras bestimmt wird. Hierzu werden die Kamerawinkel zu dem Zeitpunkt ermittelt, an dem sich entsprechende Marken an einer festen Position im Bild befinden. Dieses bedeutet, daß diesen Verfahren ein "tracking" oder "tracking"-ähnlicher Algorithmus zugrunde gelegt werden muß. Dazu ist es notwendig, schnell und mit wenig Aufwand erkennbare Marken (d. h. stark reflektierende Marken) zu benutzen. Außerdem findet mit diesen Verfahren keine Bestimmung der aktuellen Fahrtrichtung (Lage) statt, sondern der berechnete Abstand und die ermittelte Richtung definieren lediglich die aktuelle Position des Fahrzeuges.

Aus der EP 0 273 976 A1 ist ebenfalls ein Verfahren zur Navigation eines selbstfahrenden Landfahrzeuges mit einer Kamera bekannt, bei dem die Marken periodisch entlang des Fahrweges angeordnet werden und eine unterbrochene Referenzlinie bilden. Durch mindestens zwei Marken im Bild wird ein Kartenausschnitt gebildet. Zur Verfolgung dieser Kante sind Bildeinzüge während der Fahrt in schneller Abfolge notwendig, so daß auch in diesem Fall die Marken stark reflektierend sein müssen.

Aufgabe der Erfindung ist es, ein Verfahren nach dem Oberbe griff des Anspruchs 1 zu schaffen, das unabhängig von vorher angebrachten Markierungen sowie im Gelände und nicht reichweitenbeschränkt arbeiten kann.

Diese Aufgabe wird entsprechend dem kennzeichnenden Teil des Anspruchs 1 gelöst.

Weitere Ausgestaltungen der Erfindung sind der nachfolgenden Beschreibung und den Unteransprüchen zu entnehmen.

Die Erfindung wird nachstehend anhand eines in den beigefügten Abbildungen dargestellten Ausführungsbeispiels näher erläutert.

Fig. 1 zeigt schematisch den Aufbau einer Einrichtung zur Vor nahme der Navigation.

Fig. 2 veranschaulicht die Feststellung einer Sollwertabwei chung des Fahrzeugs.

Fig. 3 veranschaulicht den Datenfluß bei der Feststellung der Sollwertabweichung.

Fig. 4 zeigt ein Diagramm zur Erläuterung der Koordinaten bestimmung.

Die in Fig. 1 dargestellte Einrichtung umfaßt eine auf dem zu navigierenden Landfahrzeug angebrachte CCD-Kamera 1, deren Frequenz bereich nicht an den sichtbaren Bereich gebunden ist. Wenn es zweckmäßig ist, kann auch mit dem nahen Infrarotbereich gearbeitet werden. Die CCD-Kamera 1 kann gegebenenfalls mit einem Zoomobjektiv 2 versehen sein, das von einer Steuerung 3 ansteuerbar ist. Die Kamera 1 ist an dem Landfahrzeug über eine zweiachsige Kameraaufhängung 4 befestigt, so daß die Kamera 1 um eine Achse senkrecht und eine Achse waagerecht zur Fahrtebene des Landfahrzeugs über die Steuerung 3 gesteuert drehbar ist.

Die Kamera 1 ist an einen Digitalisierer 5 angeschlossen, der das Gesamtbild oder auch Bildausschnitte an einen Korrelatormodul 6 weitergibt. Das Korrelatormodul 6 bestimmt die Verschiebung einer Land marke im Bild der Kamera 1. Das Ergebnis wird einem Koordinatentrans formator 7 zugeführt. Aus den Verschiebungen eines Landmarkenpaares berechnet dieser die Istposition des Landfahrzeugs relativ zu einem Sollpunkt und die aktuelle Fahrzeugrichtung in bezug auf eine Sollrichtung. In der Steuerung 3 werden die Sollwertabweichungen zur Erzeugung von entsprechenden Navigationssteuersignalen zum Navigieren des Landfahrzeugs verwendet. Außerdem liefert die Steuerung 3 zu bestimmten Zeitpunkten Auftragssignale zur Positionsbestimmung an den Digitalisierer 5.

Zu Beginn wird eine Lernfahrt vorgenommen. Hierbei werden der Einrichtung an Sollpunkten 8, an denen eine Positionsbestimmung vorgenommen werden soll, jeweils zwei Landmarken A, B zur Kenntnis gebracht und automatisch vermessen. Hierbei handelt es sich um beliebige, im Raum befindliche Objekte, die bei einer Erfassung durch die Video kamera 1 im Bild genügend klar erkennbar sind. Die Definition der zweiten Landmarke B erfolgt nach der Definition der ersten Landmarke A, wobei das Landfahrzeug um eine wohlbekannte Strecke in Fahrtrichtung weiterbewegt wurde. Hierbei muß die zur Strecke gehörige Fahrtzeit ausreichen, um die Kamera bei der autonomen Fahrt von der ersten Landmarke A zur zweiten Landmarke B zu schwenken.

Die Definition einer Landmarke A, B erfolgt durch Ausrichten der Kamera 1 auf einen ausgewählten Punkt im Raum. Die Eignung wird von der Einrichtung geprüft. Bei ausreichender Detektionswahrscheinlichkeit wird der Bildausschnitt mit den zugehörigen Blickwinkeln abgespeichert.

Die Vermessung einer Landmarke A, B erfolgt durch Weiterbe wegen des Landfahrzeugs um eine wohldefinierte Strecke in Fahrtrichtung. Die Länge der Strecke wird von der Einrichtung vorgeschlagen und an die Fahrzeugkontrolle übermittelt. Die Länge der zurückgelegten Strecke sowie eventuelle Richtungs- und Lageänderungen werden von der Einrichtung genauestens bestimmt. Aus der Verschiebung im Bild werden die Koordinaten der Landmarke A, B in bezug auf den Sollpunkt 8 abgeleitet.

Bei autonomer Fahrt wird die Kamera vor Erreichen eines Soll punktes 8 entsprechend dem abgespeicherten Blickwinkel an diesem Soll punkt 8 auf die erste Landmarke A gerichtet. Den Zeitpunkt, bei dem der Sollpunkt 8 erreicht sein sollte, bestimmt die Einrichtung auf Basis der kontinuierlichen Navigationsmittel. Sie erteilt an den Digitalisierer 5 zu diesem Zeitpunkt den Auftrag zur Ermittlung der aktuellen Abweichung vom Sollpunkt 8.

Hierdurch wird das zu diesem Zeitpunkt gehörige Kamerabild gespeichert und die Landmarke A im Bild detektiert. Die Abweichung von der ursprünglichen Position im Bild wird festgehalten. Danach wird die Kamera 1 auf die zweite Landmarke B geschwenkt. Das aktuelle Bild wird gespeichert und die Bewegung des Fahrzeugs zwischen der Aufnahme der ersten und zweiten Landmarke A, B auf Basis der kontinuierlichen Navigationsmittel genauestens bestimmt. Dies sind der in Fahrtrichtung zurückgelegte Weg und eventuelle Änderungen in der Fahrtrichtung, dargestellt durch den Verschiebungsvektor s in bezug auf aktuellen Standort und aktuelle Richtung zur Erfassung der zweiten Landmarke B. (Sonstige, nicht detektierbare Lageänderungen dürfen in diesem Zeitraum nicht stattfinden.)

Die Verschiebung der zweiten Landmarke B im Bild wird detektiert. Aus den Verschiebungen der ersten und zweiten Landmarke im Bild wird die lineare Abweichung des Landfahrzeugs vom Sollpunkt 8, d. h. es werden die Verschiebung in Fahrtrichtung und senkrecht dazu sowie eventuelle Richtungsänderungen, dx, dy, berechnet.

Dies erlaubt somit die Bestimmung der Istposition eines Landfahrzeugs an ausgewählten Sollpunkten 8 mit einer Genauigkeit, die den Bedürfnissen angepaßt werden kann, wobei die Istpositionsbestimmung zur Navigation genutzt wird, so daß sich eine kostengünstige Möglichkeit der autonomen Steuerung von Landfahrzeugen mit festgelegten Routen über beliebig lange Strecken ergibt. Ausreichende Lichtverhältnisse können gegebenenenfalls durch künstliche Beleuchtung am Landfahrzeug erreicht werden.

Lageänderungen des Landfahrzeugs, die beispielsweise durch Aufnahme oder Abgabe von Lasten bedingt sind, können an den Ladestellen detektiert werden. Hierzu wird vor dem Be- oder Entladen eine Positions bestimmung durchgeführt. Nach dem Lade- oder Entladevorgang werden die Kippwinkel des Landfahrzeugs aus den Verschiebungen dreier Landmarken A, B und C im Bild ermittelt, um bei nachfolgenden Positionsbestimmungen entsprechend berücksichtigt zu werden.

Es sei davon ausgegangen, daß an einem Sollpunkt 8 zwei unterschiedliche Landmarken A, B aufgenommen werden.

Weiter sei angenommen, daß sich der Drehpunkt des Koordinaten systems C der Kamera 1 (Kamerasystem) im Ursprung des Landfahrzeugs befindet, das seinerseits ein Weltkoordinatensystem W festlegt. Die z-Achse hiervon weist nach "oben", die y-Achse nach "vorn" (in Fahrtrichtung). Die z-Achse des Koordinatensystems C der Kamera 1 weist in "Blickrichtung" der Kamera 1. Ferner gelten folgende Definitionen:

λ: Brennweite der Kamera 1
α₁, α₂: Winkel, um den die z-y-Ebene um die x-Achse rotiert (Kippen), wobei sich die Indizes auf die erste und zweite Landmarke A, B beziehen
Θ₁, Θ₂: Winkel (gegen den Uhrzeigersinn gemessen), um den die x-y-Ebene um die z-Achse rotiert (Schwenken), wobei sich die Indizes auf die erste und zweite Landmarke A, B beziehen

(x₀, y₀): Koordinaten des Bildpunktes (Projektion auf x-y-Ebene in C
(x, y, z) = (x₀/λ(λ-z), y₀/λ(λ-z), z): Urbildgerade der Projektion
(X, Y, Z): Weltkoordinaten der Landmarke
r=(r₁, r₂, r₃): Verschiebung des Ursprungs des Kamerakoordinatensystems C aus dem Drehpunkt

Bestimmung der Weltkoordinaten aus der Lernfahrt

Die Weltkoordinaten der Landmarke werden aus den Bildkoordinaten mittels der Matrix , i = 1, 2 und der Verschiebung r bestimmt; wenn zwischen den Winkeln nicht unterschieden werden muß, wird kurz α, Θ geschrieben. Da α, Θ gegen den Uhrzeigersinn gemessen werden, ist

Bekannt sind λ, α, Θ, x₀, y₀, Z, r.
Aus

folgt, da R_Θ ^-1 die dritte Komponente fest läßt:
Z = (y + r₂) sin α +(z + r₃) cos α
= (y₀/λ(λ-z) + r₂) sin α + (z + r₃) cos α
= (cos α - y₀/λ sin α) z + (y₀ + r₂) sin α + r₃cos α
also

z = (Z - (y₀ + r₂) sin α - r₃ cos α) / (cosα - y₀/λ sin α)

Daher können aus den Koordinaten des Bildpunktes eines Bildes - mit Kenntnis der Z-Komponente der Landmarke - die Koordinaten des Urbildpunktes und damit die Koordinaten der Landmarke eindeutig bestimmt werden, falls cos α und y₀ nicht beide Null sind. Das bedeutet, daß die Marke nicht auf der y-Achse des Bildschirms erscheinen darf, wenn die Blickrichtung parallel zur x,y-Ebene des Fahrzeugsystems verläuft.

Bestimmung der Weltkoordinaten einer Marke bei der Lernfahrt

Für jede der beiden Marken wird folgendermaßen verfahren:
In einem Updatepunkt wird von zwei verschiedenen Punkten der Fahrstrecke jeweils eine Aufnahme der Landmarke gemacht. Die beiden Punkte haben die Bildschirmkoordinaten (x₀, y₀) und (a₀, b₀). Die Koordinaten im Kamerasystem seien x_C und x_C,2; der Abstand entlang der Fahrstrecke wird durch einen Vektor d beschrieben. Man erhält für die Weltkoordinaten x_W:

Es sei nun g_i eine Gerade im Kamerasystem zu Punkt i, die durch den Bildpunkt und den Brennpunkt verläuft, also

Mit (*) folgt dann

g₁ - g₂ = R_Cd

also

Lösen des linearen Gleichungssystems liefert s und t. Setzt man bei spielsweise t in die Gleichung für g₁ ein, so erhält man x_C. Dies eingesetzt in (*) ergibt die gesuchten Koordinaten der Landmarke im Weltsystem.

Bestimmung der Weltkoordinaten aus der autonomen Fahrt

Für einen Kontrollpunkt werden zwei Landmarken zugrundegelegt. Da nur eine Kamera 1 benutzt wird, legt das Landfahrzeug, während die Kamera 1 von der einen Landmarke A auf die andere Landmarke B ausgerich tet wird, den Weg ΔS zurück. Daher erhält man für die Lernfahrt zwei Weltsysteme W₁ und W₂ mit den Kamerasystemen C₁ und C₂ und den entsprechenden Koordinatenvektoren x_C₁und x_C₂′ für einen Punkt x. Für die autonome Fahrt werden die Weltsysteme mit W₃ und W₄, ferner die Kamerakoordinatensysteme mit C₃ und C₄ und die Koordinatenvektoren eines Punktes x mit x_C₃ und x_C₄′ bezeichnet. Es gilt

x_W₂ = x_W₁ - ΔS_L, x_W₄ = x_W₃ - ΔS_A,

wobei ΔS_L und ΔS_A den Weg bezeichnet, den das Landfahrzeug während der Ausrichtung der Kamera 1 von der einen auf die andere Landmarke A, B bei der Lern- bzw. Automatikfahrt zurücklegt, vgl. Fig.. 4. Wenn a und b die Koordinatenvektoren der ersten und zweiten Landmarke A bzw. B sind, so gilt:

Dabei ist R_C₁ ^-1 im allgemeinen nicht gleich R_C₁ ^-1, da die Stellung des Kamerasystems gegenüber dem Weltkoordinatensystem für die Landmarke A nicht dieselbe sein muß wie die für die Landmarke B. Da die Kamera 1 bei der Automatikfahrt jeweils die gleiche Position gegenüber dem Weltkoordinatensystem wie bei der Lernfahrt einnimmt, ist C₁ = C₃ und C₂ = C₄. Aus der jeweils dritten Komponente der oben aufgeführten Gleichungen lassen sich a_C₁, a_C₃, b_C₂, b_C₄ bestimmen.

W₁ und W₂ bzw. W₃ und W₄ hängen folgendermaßen zusammen:

Wegen x_W₂ = x_W₁ - ΔS_L, x_W₃ = x_W₄ - ΔS_A gilt:

Fall 1: R = R_ω (= R_Θ mit Θ: = ω; es wurde nur um die z-Achse gedreht).

Damit gilt:

Setzt man dann über d gleich, so folgt:

R_ω läßt die dritte Komponente fest. Streicht man diese Komponente, so nimmt die letzte Gleichung folgende Form an:

wobei P, Q, M, N bekannt sind. Daraus erhält man

falls P oder Q ungleich null sind (P=Q=0) würde Übereinstimmung der Koordinatenvektoren der beiden Landmarken A, B bedeuten).

Insgesamt ist dann auch ω bekannt, und beispielsweise aus

kann d bestimmt werden.

Fall 2: R = R_βR_ω (= R_αR_Θ mit α: = β, Θ: = ω).

Analog zu oben ergibt sich:

Setzt man

und

so ergibt sich aus der jeweils dritten Komponente, die von R_ω nicht verändert wird

U₃ - dZ = V₂ sin β + V₃ cos β

C₃ - dZ = D₂ sin β + D₃ cos β

also

Falls V₃D₂-V₂D₃ ungleich null ist, ergibt sich hieraus der Winkel β.

Analog zu Fall 1 erhält man durch Gleichsetzen über d die Beziehung

mit der dann wie im Fall 1 verfahren werden kann, so daß zunächst der Winkel ω und dann die Abweichung d berechnet werden kann. Eine eindeuti ge Lösung ergibt sich, wenn V₃D₃-V₂D₃ ungleich Null ist, d. h. wenn a und b nicht auf einer Geraden liegen, die parallel zur z-Achse des Koor dinatensystems W₃ verläuft, und a bzw. b nicht in der von b bzw. a und der x-Achse des Koordinatensystems W₃ aufgespannten Ebene enthalten sind.

Claims

1. Verfahren zur Navigation eines selbstfahrenden Landfahr zeugs, wobei Marken während der Fahrt - mittels einer um eine Achse senkrecht und eine Achse waagerecht zur Fahrtebene schwenkbare Videokamera bildmäßig unter einem gespeicherten Blickwinkel zu einem Zeitpunkt, an dem ein zu der jeweiligen Marke in Bezug gesetzter Sollpunkt erreicht sein soll - erfaßt, digitalisiert und mit abgespeicherten Daten verglichen werden, um Abweichungen von Sollwerten festzustellen, die zu entsprechenden Navigationssteuersignalen verarbeitet werden, wobei die abgespeicherten Daten während einer Lernfahrt erzeugt werden, dadurch gekennzeichnet, daß als Marken natürliche im Raum befindliche Landmarken verwendet werden, daß in jedem Sollpunkt zwei voneinander verschiedene Landmarken aufgenommen werden, und daß nach Digitalisierung der erfaßten Landmarkenbilder die koordinatenmäßige Verschiebung der Landmarken im Bild bestimmt und aus den Verschiebungen des Landmarkenpaares die Istposition des Fahrzeugs relativ zu einem Sollpunkt und die aktuelle Fahrzeugrichtung in bezug auf eine Sollrichtung berechnet wird.

2. Verfahren nach Anspruch 1, dadurch gekennzeichnet, daß als Videokamera eine CCD-Kamera verwendet wird.

3. Verfahren nach Anspruch 1 oder 2, dadurch gekennzeichnet, daß an Ladepunkten vor und nach einem Be- oder Entladen eine Positionsbestimmung bezüglich zweier Landmarken durchgeführt und hieraus der Kippwinkel des Landfahrzeugs bestimmt wird, der bei einer nachfolgenden Positionsbestimmung zu berücksichtigen ist.