DE112021007716T5

DE112021007716T5 - Pfad-planungsvorrichtung

Info

Publication number: DE112021007716T5
Application number: DE112021007716.3T
Authority: DE
Inventors: Yuki Yoshida; Shota Kameoka; Rin Ito; Hiroaki Kitano; Kenta TOMINAGA
Original assignee: Mitsubishi Electric Corp
Current assignee: Mitsubishi Electric Corp
Priority date: 2021-05-24
Filing date: 2021-05-24
Publication date: 2024-05-08
Also published as: CN117413233A; WO2022249218A1; JPWO2022249218A1; US20240240951A1

Abstract

Die vorliegende Erfindung betrifft eine Pfad-Planungsvorrichtung, und sie weist Folgendes auf: Eine Berechnungseinheit für einen befahrbaren Bereich, die konfiguriert ist zum Berechnen eines befahrbaren Bereichs eines beweglichen Körpers auf der Basis einer Umgebungsinformation des beweglichen Körpers; eine Sollzustand-Berechnungseinheit, die konfiguriert ist zum Berechnen einer Sollzustandsgröße inklusive zumindest einer Sollposition des beweglichen Körpers; eine Zustands-Vorhersageeinheit, die konfiguriert ist zum Vorhersagen zumindest einer gegenwärtigen Zustandsgröße des beweglichen Körpers und einer Zustandsgröße des beweglichen Körpers an einer oder mehreren Positionen zwischen einer gegenwärtigen Position und der Sollposition des beweglichen Körpers, so dass sie dadurch einen oder mehrere Pfadkandidaten erzeugt; eine Bewertungseinheit für einen vorhergesagten Zustand, die konfiguriert ist zum Bewerten des einen oder der mehreren Pfadkandidaten auf der Basis der Sollzustandsgröße und des befahrbaren Bereichs, und zum Ausgeben von Bewertungsergebnissen; und eine Pfad-Erzeugungseinheit, die konfiguriert ist zum Erzeugen eines Pfads von dem einen oder den mehreren Pfadkandidaten auf der Basis der Bewertungsergebnisse, und zum Ausgeben des Pfads an eine Bewegungssteuerung, die konfiguriert ist zum Steuern des beweglichen Körpers auf der Basis des Pfades.

Description

Technisches Gebiet
Die vorliegende Erfindung betrifft eine Pfad-Planungsvorrichtung, und sie betrifft insbesondere eine Pfad-Planungsvorrichtung, die einen Betrieb zum Implementieren des automatisierten Fahrens eines Fahrzeugs oder dergleichen plant.
Stand der Technik
In jüngster Zeit wurde das automatisierte Fahren eines Automobils und eines autonom beweglichen Systems eines Förderwagens oder dergleichen entwickelt. In dem autonom beweglichen System werden ein Pfad inklusive einer Spur, auf der ein beweglicher Körper fahren soll, und eine Geschwindigkeit erzeugt, und es wird eine Steuerung durchgeführt, so dass der bewegliche Körper entlang des erzeugten Pfades fährt. Hinsichtlich der Pfadplanung wird in vielen Fällen eine Pfadplanung entlang der Mitte einer Straße und einer Induktivität wie z. B. einer magnetischen Markierung durchgeführt. In Abhängigkeit vom Fall kann jedoch in einer Situation, in der in der Nähe einer Mautstelle ohne weiße Linien auf der Straße oder auf einer unbefestigten Straße gefahren wird, und in einer Situation, in der sich ein autonomer Förderwagen ohne Verwendung einer Induktivität zu einem Ziel bewegt, eine solche Information nicht verwendet werden. In solchen Situationen ist ein Pfad notwendig, der das Erreichen des Ziels ermöglicht, während ein Hindernis in einem Raum vermieden wird, wo es keine Information einer Markierung gibt, auf der gefahren werden soll. Wie in Patentdokument 1 offenbart, wurde beispielsweise eine Technologie entwickelt, bei der die Pfadplanung implementiert wird, selbst wenn es keine Information über eine Markierung gibt, auf der gefahren werden soll.
Stand-der-Technik-Dokument
Patentdokument
Patentdokument 1: Japanische Patentanmeldungs-Offenlegungsschrift Nr. JP 2012- 145 998 A
Zusammenfassung
Mit der Erfindung zu lösendes Problem
Patentdokument 1 verwendet ein Verfahren zum Erzeugen einer Fahrtroute auf der Basis der Richtung entlang einer Straße, inklusive eines Überquerens einer Linie durch Mittelpunkte einer Mehrzahl von Kreisen, die in einem befahrbaren Bereich ohne Hindernisse einbeschrieben sind. In diesem Fall können beispielsweise einbeschriebene Kreise nicht in einem signifikant breiten befahrbaren Bereich wie z. B. einem Flughafen bestimmt werden, und folglich kann eine Fahrtroute nicht erzeugt werden, und ein Ziel kann nicht erreicht werden. Außerdem kann in einem befahrbaren Bereich, der eine komplizierte Form und große Änderungen der Breite hat, wie z. B. in der Nähe einer Mautstelle, die korrekte Richtung entlang einer Straße nicht berechnet werden, und ein Ziel kann nicht erreicht werden.
Die vorliegende Erfindung wurde konzipiert, um die oben beschriebenen Probleme zu lösen, und sie hat die Aufgabe, eine Pfad-Planungsvorrichtung bereitzustellen, die das Erreichen eines Ziels durch einen befahrbaren Bereich ermöglicht, selbst wenn der befahrbare Bereich kompliziert ist, wie z. B. auf einem Flughafen und in der Nähe einer Mautstelle.
Wege zum Lösen des Problems
Eine Pfad-Planungsvorrichtung gemäß der vorliegenden Erfindung ist eine Pfad-Planungsvorrichtung, die zum Planen eines Pfads eines beweglichen Körpers konfiguriert ist. Die Pfad-Planungsvorrichtung weist Folgendes auf: eine Berechnungseinheit für befahrbaren Bereich, die konfiguriert ist zum Berechnen eines befahrbaren Bereichs des beweglichen Körpers auf der Basis einer Umgebungsinformation des beweglichen Körpers; eine Sollzustand-Berechnungseinheit, die konfiguriert ist zum Berechnen einer Sollzustandsgröße inklusive zumindest einer Sollposition des beweglichen Körpers; eine Zustands-Vorhersageeinheit, die konfiguriert ist zum Vorhersagen zumindest einer gegenwärtigen Zustandsgröße des beweglichen Körpers und einer Zustandsgröße des beweglichen Körpers an einer oder mehreren Positionen zwischen einer gegenwärtigen Position und der Sollposition des beweglichen Körpers, so dass sie dadurch einen oder mehrere Pfadkandidaten erzeugt; eine Bewertungseinheit für einen vorhergesagten Zustand, die konfiguriert ist zum Bewerten des einen oder der mehreren Pfadkandidaten auf der Basis der Sollzustandsgröße und des befahrbaren Bereichs, und zum Ausgeben von Bewertungsergebnissen; und eine Pfad-Erzeugungseinheit, die konfiguriert ist zum Erzeugen des Pfads aus dem einen oder den mehreren Pfadkandidaten auf der Basis der Bewertungsergebnisse, und zum Ausgeben des Pfads an eine Bewegungssteuerung, die konfiguriert ist zum Steuern des beweglichen Körpers auf der Basis des Pfads.
Wirkungen der Erfindung
Gemäß der Pfad-Planungsvorrichtung der vorliegenden Erfindung wird ein Pfad bis zur Sollzustandsgröße auf der Basis der Sollzustandsgröße inklusive der Sollposition des beweglichen Körpers und des befahrbaren Bereichs bewertet, und der Pfad wird auf der Basis der Bewertungsergebnisse erzeugt. Daher kann selbst dann, wenn der befahrbare Bereich kompliziert ist, das Ziel durch den befahrbaren Bereich erreicht werden.
Kurze Beschreibung der Zeichnungen

1 ist ein Blockdiagramm, das ein Beispiel einer schematischen Konfiguration eines beweglichen Körpers veranschaulicht, der mit einer Pfad-Planungsvorrichtung gemäß einer ersten Ausführungsform ausgestattet ist.
2 ist ein Diagramm, das ein Beispiel eines befahrbaren Bereichs gemäß der ersten Ausführungsform veranschaulicht.
3 ist ein Diagramm, das ein Beispiel einer Sollzustandsgröße gemäß der ersten Ausführungsform veranschaulicht.
4 ist ein Diagramm, das ein Beispiel eines Pfadpunkts veranschaulicht, der von einer Pfadpunkt-Berechnungseinheit in der Pfad-Planungsvorrichtung gemäß der ersten Ausführungsform berechnet wird.
5 ist ein Diagramm, das ein Beispiel eines Pfads veranschaulicht, der von einer Pfad-Erzeugungseinheit in der Pfad-Planungsvorrichtung gemäß der ersten Ausführungsform erzeugt wird.
6 ist ein Ablaufdiagramm, das den Betrieb der Pfad-Planungsvorrichtung gemäß der ersten Ausführungsform veranschaulicht.
7 ist ein Diagramm, das ein Beispiel der Information veranschaulicht, die von einer Informations-Erfassungseinheit in der Pfad-Planungsvorrichtung gemäß der ersten Ausführungsform erfasst wird.
8 ist ein Diagramm, das ein Beispiel veranschaulicht, bei dem die Information, die von der Informations-Erfassungseinheit erfasst wird, in ein Koordinatensystem des beweglichen Körpers umgewandelt wird, in der Pfad-Planungsvorrichtung gemäß der ersten Ausführungsform.
9 ist ein Diagramm, das ein Beispiel der Vorhersage des befahrbaren Bereichs in der Pfad-Planungsvorrichtung gemäß der ersten Ausführungsform veranschaulicht.
10 ist ein Diagramm, das ein weiteres Beispiel der Vorhersage des befahrbaren Bereichs in der Pfad-Planungsvorrichtung gemäß der ersten Ausführungsform veranschaulicht.
11 ist ein Diagramm, das ein Beispiel des befahrbaren Bereichs in der Pfad-Planungsvorrichtung gemäß der ersten Ausführungsform veranschaulicht.
12 ist ein Diagramm, das ein Beispiel des befahrbaren Bereichs in der Pfad-Planungsvorrichtung gemäß der ersten Ausführungsform veranschaulicht.
13 ist ein Diagramm, das ein Beispiel des befahrbaren Bereichs in der Pfad-Planungsvorrichtung gemäß der ersten Ausführungsform veranschaulicht.
14 ist ein Diagramm, das ein Beispiel der Sollzustandsgröße in der Pfad-Planungsvorrichtung gemäß der ersten Ausführungsform veranschaulicht.
15 ist ein Diagramm, das ein Beispiel der Sollzustandsgrößen-Zurücksetzung in der Pfad-Planungsvorrichtung gemäß der ersten Ausführungsform veranschaulicht.
16 ist ein Diagramm, das ein Beispiel zum Vorgeben eines oberen Grenzwerts einer Geschwindigkeit unter den Sollzustandsgrößen in der Pfad-Planungsvorrichtung gemäß der ersten Ausführungsform veranschaulicht.
17 ist ein Diagramm, das ein Beispiel zum Vorgeben eines oberen Grenzwerts einer Geschwindigkeit unter den Sollzustandsgrößen in der Pfad-Planungsvorrichtung gemäß der ersten Ausführungsform veranschaulicht.
18 ist ein Diagramm, das ein Beispiel der Zustandsgröße zeigt, die unter Verwendung von Partikel-Filtern vorhergesagt wird, in der Pfad-Planungsvorrichtung gemäß der ersten Ausführungsform.
19 ist ein Diagramm, das ein Beispiel von Eingabewerten veranschaulicht, die in Abhängigkeit einer Form des befahrbaren Bereichs vorgegeben sind, in der Pfad-Planungsvorrichtung gemäß der ersten Ausführungsform.
20 ist ein Diagramm, das ein Beispiel von Eingabewerten veranschaulicht, die in Abhängigkeit einer Form des befahrbaren Bereichs vorgegeben sind, in der Pfad-Planungsvorrichtung gemäß der ersten Ausführungsform.
21 ist ein Diagramm, das ein Beispiel zum Vorgeben von Eingabewerten veranschaulicht, wenn eine Abweichung zwischen der gegenwärtigen Zustandsgröße und der Sollzustandsgröße des beweglichen Körpers groß ist, in der Pfad-Planungsvorrichtung gemäß der ersten Ausführungsform.
22 ist ein Diagramm, das ein Beispiel von Beobachtungsvariablen veranschaulicht, in der Pfad-Planungsvorrichtung gemäß der ersten Ausführungsform.
23 ist ein Diagramm, das ein Beispiel zum Gewichten von Partikeln außerhalb des befahrbaren Bereichs veranschaulicht, in der Pfad-Planungsvorrichtung gemäß der ersten Ausführungsform.
24 ist ein Diagramm, das ein Beispiel zum Gewichten von Partikeln innerhalb des befahrbaren Bereichs veranschaulicht, in der Pfad-Planungsvorrichtung gemäß der ersten Ausführungsform.
25 ist ein Diagramm, das ein Beispiel zum Gewichten von Partikeln innerhalb des befahrbaren Bereichs veranschaulicht, in der Pfad-Planungsvorrichtung gemäß der ersten Ausführungsform.
26 ist ein Diagramm, das ein Beispiel zum Gewichten von Partikeln innerhalb des befahrbaren Bereichs veranschaulicht, in der Pfad-Planungsvorrichtung gemäß der ersten Ausführungsform.
27 ist ein Diagramm, das ein Beispiel zum Gewichten von Partikeln innerhalb eines vorhergesagten befahrbaren Bereichs veranschaulicht, gemäß der ersten Ausführungsform.
28 ist ein Diagramm, das ein Beispiel zum Vorgeben einer Bewertungs-Gewichtung veranschaulicht, in der Pfad-Planungsvorrichtung gemäß der ersten Ausführungsform.
29 ist ein Diagramm, das ein Beispiel der Verarbeitung zum Erhalten einer Mehrzahl von Pfadpunkten zum Erreichen der Sollzustandsgröße veranschaulicht, in der Pfad-Planungsvorrichtung gemäß der ersten Ausführungsform.
30 ist ein Diagramm, das ein Beispiel der Pfaderzeugung bis zum Erreichen der Sollzustandsgröße veranschaulicht, in der Pfad-Planungsvorrichtung gemäß der ersten Ausführungsform.
31 ist ein Diagramm, das ein Beispiel der Pfaderzeugung bis zum Erreichen der Sollzustandsgröße veranschaulicht, in der Pfad-Planungsvorrichtung gemäß der ersten Ausführungsform.
32 ist ein Blockdiagramm, das ein Beispiel einer schematischen Konfiguration eines beweglichen Körpers veranschaulicht, der mit einer Pfad-Planungsvorrichtung gemäß einer zweiten Ausführungsform ausgestattet ist.
33 ist ein Diagramm, das ein Verfahren zum Herleiten eines Polynoms veranschaulicht, das den beweglichen Körper und die Sollposition verbindet, in der Pfad-Planungsvorrichtung gemäß der zweiten Ausführungsform.
34 ist ein Diagramm, das ein Verfahren zum Herleiten eines Polynoms veranschaulicht, das den beweglichen Körper und die Sollposition verbindet, in der Pfad-Planungsvorrichtung gemäß der zweiten Ausführungsform.
35 ist ein Diagramm, das ein Verfahren zum Herleiten eines Polynoms veranschaulicht, das den beweglichen Körper und die Sollposition verbindet, in der Pfad-Planungsvorrichtung gemäß der zweiten Ausführungsform.
36 ist ein Diagramm, das das Gewichten veranschaulicht, wenn die vorhergesagte Zustandsgröße stark von der gegenwärtigen Zustandsgröße des beweglichen Körpers abweicht.
37 ist ein Diagramm, das das Gewichten veranschaulicht, wenn die vorhergesagte Zustandsgröße stark von der Zustandsgröße des vorher berechneten Pfadpunkts abweicht.
38 ist ein Diagramm, das eine Hardwarekonfiguration zum Implementieren der Pfad-Planungsvorrichtungen gemäß der ersten und zweiten Ausführungsform veranschaulicht.
39 ist ein Diagramm, das eine Hardwarekonfiguration zum Implementieren der Pfad-Planungsvorrichtungen gemäß der ersten und zweiten Ausführungsform veranschaulicht.

Beschreibung von Ausführungsformen
Erste Ausführungsform
1 ist ein Blockdiagramm, das ein Beispiel einer schematischen Konfiguration eines beweglichen Körpers 1 veranschaulicht, der mit einer Pfad-Planungsvorrichtung gemäß einer ersten Ausführungsform ausgestattet ist. Der beweglicher Körper 1 weist Folgendes auf: Eine Pfad-Planungsvorrichtung 200, die einen Pfad erzeugt, auf dem der bewegliche Körper 1 fahren soll, und zwar auf der Basis einer Zielinformation eines Ziels, das vom beweglichen Körper 1 erreicht werden soll, einer Umgebungsinformation des beweglichen Körpers 1 und einer Information, die von einer Informations-Erfassungseinheit 100 erhalten wird, die einen eigenen Zustand des beweglichen Körpers 1 erfasst, sowie eine Bewegungssteuerung 300, die die Bewegung des beweglichen Körpers 1 auf der Basis des Pfads steuert, der von der Pfad-Planungsvorrichtung 200 erzeugt wird.
Die Informations-Erfassungseinheit 100 weist eine Sollwert-Erfassungseinheit 110, eine Erfassungseinheit 120 für eigenen Zustand und eine Umgebungs-Erfassungseinheit 130 auf.
Die Sollwert-Erfassungseinheit 110 erfasst eine Information wie z. B. die Sollposition, die vom beweglichen Körper erreicht werden soll, die Sollgeschwindigkeit und den Sollazimut. Die Sollwert-Erfassungseinheit 110 erfasst beispielsweise eine Information aus der Infrastruktur-Information, die vom Steuern empfangen wird, eine Information, die im Voraus von einem Benutzer spezifiziert wird, eine vorbestimmte Position in einer Karteninformation, die vom beweglichen Körper gespeichert wird, und dergleichen. Hier bezieht sich die Karteninformation, die vom beweglichen Körper gespeichert wird, beispielsweise auf eine Karte einer Fahrzeugnavigationseinrichtung oder eine Punktwolken-Karte, die unter Verwendung von gleichzeitiger Lokalisierung und Kartierung (SLAM) erzeugt wird, oder dergleichen, und nicht auf eine hochgenaue Karte.
Beispiele für die Sollposition schließen Folgendes ein: Den Eingang eines Tors oder die Position der Schranke an einer Mautstelle, die Position einer Verkehrsinsel auf einer Schnellstraße, ein Bugradteil eines Luftfahrzeugs für einen Abschlepper und die Position des beweglichen Körpers 1, die von einem Benutzer spezifiziert wird. Beispiele der Sollgeschwindigkeit schließen eine Geschwindigkeitsbegrenzung und eine spezifizierte Geschwindigkeit ein, die von einem Benutzer im Voraus vorgegeben wird. Der Sollazimut ist der Sollwinkel zu der Zeit, wenn die Sollposition passiert wird, wobei Beispiele die Richtung senkrecht zu einem Tor zur Zeit des Passierens des Tors einschließen.
Die Erfassungseinheit 120 für eigenen Zustand erfasst den gegenwärtigen Zustand des beweglichen Körpers selbst. Beispiele der Erfassungseinheit 120 für eigenen Zustand schließen Folgendes ein: Einen Geschwindigkeitssensor, einen Beschleunigungssensor, eine Inertialmesseinheit, einen Lenkwinkelsensor, einen Lenkmomentsensor, einen Gierratensensor und einen Sensor für globales Satellitenpositionierungssystem (GNSS). Die Inertialmesseinheit wird nachfolgend als Inertialmesseinheit-Sensor (IMU) bezeichnet.
Die Umgebungs-Erfassungseinheit 130 erfasst eine Wand in der Umgebung des beweglichen Körpers, eine Position, eine Geschwindigkeit, und einen Azimut eines beweglichen Hindernisses, sowie eine Rauminformation des befahrbaren Raums ohne Hindernisse. Beispiele der Umgebungs-Erfassungseinheit 130 schließen Folgendes ein: Millimeterwellenradar, eine Kamera, Light Detection and Ranging (LiDAR), Sonar, eine Fahrzeug-zu-Fahrzeug-Kommunikationsvorrichtung, und eine Straße-zu-Fahrzeug-Kommunikationsvorrichtung.
Die Pfad-Planungsvorrichtung 200 weist Folgendes auf: Eine Berechnungseinheit 210 für befahrbaren Bereich, eine Sollzustand-Berechnungseinheit 220, eine Zustands-Vorhersageeinheit 230, eine Bewertungseinheit 240 für einen vorhergesagten Zustand, eine Pfadpunkt-Berechnungseinheit 250 und eine Pfad-Erzeugungseinheit 260.
Die Berechnungseinheit 210 für einen befahrbaren Bereich berechnet den befahrbaren Bereich ohne Hindernisse, in dem der bewegliche Körper 1 fahren kann, auf der Basis der Umgebungsinformation des beweglichen Körpers 1, die von der Umgebungs-Erfassungseinheit 130 erfasst wird. 2 veranschaulicht ein Beispiel des befahrbaren Bereichs. In 2 gibt es ein ortsfestes Hindernis SOB auf der linken Seite des beweglichen Körpers 1 in dessen Fahrtrichtung, ein bewegliches Hindernis MOB wird demnächst eine Fahrspur befahren, die durch die linken und rechten Fahrspur-Begrenzungen LB definiert wird, und zwar von der rechten Seite in Fahrtrichtung, und ein befahrbarer Bereich TA ohne das ortsfeste Hindernis SOB und das bewegliche Hindernis MOB ist mit einer dicken Linie dargestellt. Wie in 2 veranschaulicht, ist der befahrbare Bereich TA nicht notwendigerweise auf die Fahrspur beschränkt, die durch die Fahrspur-Begrenzungen LB auf einer Straße definiert wird, wie z. B. weiße Linien.
Die Sollzustand-Berechnungseinheit 220 berechnet die Sollzustandsgröße in dem Ziel, das vom beweglichen Körper 1 erreicht werden soll, auf der Basis der Information von der Sollwert-Erfassungseinheit 110. Die Sollzustandsgröße schließt zumindest die Sollposition des beweglichen Körpers 1 ein. 3 veranschaulicht ein Beispiel der Sollzustandsgröße.
3 veranschaulicht eine Straße ohne weiße Linien, wie z. B. einen Platz vor einem elektronischen Mauterhebungstor (ETC), und hier ist die Fahrspur-Begrenzung LB eine Wand, eine Leitplanke oder dergleichen, anstatt einer weißen Linie. In 3 schließt die Sollzustandsgröße TG Koordinaten (x_t, y_t) der Sollposition, einen Sollazimut θ_t und eine Sollgeschwindigkeit v_t zur Zeit t ein. Es sei angemerkt, dass die gegenwärtige Zustandsgröße des beweglichen Körpers 1 ausgedrückt wird als (x_e, y_e, θ_e, v_e).
Die Zustands-Vorhersageeinheit 230 sagt zumindest die gegenwärtige Zustandsgröße des beweglichen Körpers 1 und die Zustandsgröße des beweglichen Körpers 1 an einer oder mehreren Positionen zwischen der gegenwärtigen Position und der Sollposition des beweglichen Körpers 1 vorher und erzeugt dadurch einen oder mehrere Pfadkandidaten. Beispielsweise durch Verwendung einer Zustandsschätzungs-Berechnung unter Verwendung eines Bewegungsmodells des beweglichen Körpers werden folglich eine vorbestimmte Mehrzahl von Eingaben in das Bewegungsmodell des beweglichen Körpers eingegeben, um die Zustandsgröße zumindest eines nächsten Schritts vor der Mehrzahl von Eingaben vorherzusagen, d. h. eine Abtastzeit im Steuerungszyklus voraus, und dadurch werden eine oder mehrere Pfadkandidaten erzeugt. In der vorliegenden Ausführungsform werden Partikel-Filter als ein Beispiel eines Verfahrens zur Zustandsschätzung verwendet.
Die Partikel-Filter sind ein Verfahren zum Vorhersagen von Zeitreihendaten unter Verwendung einer Wahrscheinlichkeitsdichteverteilung, und sie können daher als sequenzielles Monte-Carlo-Verfahren bezeichnet werden. Außerdem nähern Partikel-Filter als Zustandsschätzung-Berechnung eine Wahrscheinlichkeitsdichteverteilung eines Zustands unter Verwendung einer Mehrzahl von Partikeln an. Wenn es beispielsweise viele Partikel mit einer gewissen Zustandsgröße gibt, ermöglicht die Verwendung der Partikel-Filter als deren Zustandsschätzung-Berechnung die Schätzung der ganzen Wahrscheinlichkeitsdichteverteilung, und folglich kann die Frequenz zum Ausgeben einer lokal optimalen Lösung verringert werden.
Die Bewertungseinheit 240 für einen vorhergesagten Zustand weist jeder vorhergesagten Zustandsgröße eine Gewichtung zu, d. h. jedem Partikel, um gewichtete Pfadkandidaten zu erhalten, sie bewertet die gewichteten Pfadkandidaten auf der Basis von deren Gewichtungen, und sie gibt Bewertungsergebnisse aus. In diesem Fall werden Gewichtungen auf der Basis der Sollzustandsgröße, die in der Sollzustand-Berechnungseinheit 220 berechnet wird, und des befahrbaren Bereichs zugeordnet, der in der Berechnungseinheit 210 für einen befahrbaren Bereich berechnet wird. Wenn ein Pfadpunkt in der Pfadpunkt-Berechnungseinheit 250 berechnet wird, die später noch beschrieben wird, kann mit den Gewichtungen die Zustandsgröße mit hoher Wahrscheinlichkeit berechnet werden, indem ein gewichteter Durchschnitt einer Mehrzahl von Zustandsgrößen berechnet wird, die in der Zustands-Vorhersageeinheit 230 vorhergesagt werden, und zwar auf der Basis eines Werts der Gewichtung jeder der vorhergesagten Zustandsgrößen.
Wenn beispielsweise ein Gewichtungskoeffizient der Zustandsgröße außerhalb des befahrbaren Bereichs auf 0 vorgegeben wird, wird bei der Berechnung des gewichteten Durchschnitts die Zustandsgröße mit dem Gewichtungskoeffizienten 0 multipliziert, und folglich kann verhindert werden, dass die Zustandsgröße der Pfadpunkt außerhalb des befahrbaren Bereichs ist, und ein Pfad, von dem gewährleistet ist, dass er ein Pfad innerhalb des befahrbaren Bereichs TA ist, kann erzeugt werden.
Die Pfadpunkt-Berechnungseinheit 250 berechnet den Pfadpunkt aus den Pfadkandidaten. Genauer gesagt: Die Pfadpunkt-Berechnungseinheit 250 führt eine Berechnung zum Berechnen des gewichteten Durchschnitts der vorhergesagten Zustandsgrößen, vorhergesagt in der Zustands-Vorhersageeinheit 230, gemäß den Gewichtungen durch, die von der Bewertungseinheit 240 für einen vorhergesagten Zustand zugeordnet werden, und berechnet dann die Zustandsgröße, die dem gewichteten Durchschnitt unterzogen ist, als den Pfadpunkt. 4 veranschaulicht ein konzeptuelles Diagramm der Berechnung.
Wie in 4 veranschaulicht, gibt es innerhalb und in der Nähe des befahrbaren Bereichs TA eine Partikelgruppe G0 von Zustandsgrößen mit dem Gewichtungskoeffizienten 0, eine Partikelgruppe GL von Zustandsgrößen mit einem niedrigen Gewichtungskoeffizienten und eine Partikelgruppe GH von Zustandsgrößen mit einem hohen Gewichtungskoeffizienten, und die Zustandsgröße, die dem gewichteten Durchschnitt unterzogen ist, in der Partikelgruppe GH der Zustandsgrößen, die den hohen Gewichtungskoeffizienten haben, wird als ein Pfadpunkt TP verwendet. Ferner kann die Zustandsgröße, deren Gewichtung, die von der Bewertungseinheit 240 für einen vorhergesagten Zustand zugeordnet ist, am höchsten ist, als der Pfadpunkt verwendet werden.
Die Pfad-Erzeugungseinheit 260 erzeugt einen Pfad aus den Pfadkandidaten auf der Basis der Bewertungsergebnisse, und sie gibt den Pfad an die Bewegungssteuerung 300 aus, die den beweglichen Körper 1 auf der Basis des erzeugten Pfades steuert. Genauer gesagt: Die Pfad-Erzeugungseinheit 260 gibt eine Punktsequenz inklusive Pfadpunkten zu jeweiligen diskreten Zeitpunkten, die in der Pfadpunkt-Berechnungseinheit 250 berechnet werden, an die Bewegungssteuerung 300 als einen erzeugten Pfad aus. 5 veranschaulicht ein konzeptuelles Diagramm des erzeugten Pfades.
Wie in 5 veranschaulicht, sind Pfadpunkte TP1, TP2, TP3, TP4 und TP5 jeweils zu diskreten Zeitpunkten t₁, t₂, t₃, t₄ und t₅ im befahrbaren Bereich TA veranschaulicht, und ein erzeugter Pfad GT wird durch fünf Pfadpunkte erhalten.
Die Bewegungssteuerung 300 weist eine Steuerungswert-Berechnungseinheit 310 und eine Aktor-Steuerung 320 auf.
Die Steuerungswert-Berechnungseinheit 310 berechnet einen Soll-Steuerungswert für den beweglichen Körper 1 zum Fahren entlang des Sollpfads, indem sie den Pfad, der in der Pfad-Erzeugungseinheit 260 erzeugt wird, als den Sollpfad verwendet, und sie gibt den Soll-Steuerungswert an die Aktor-Steuerung 320 aus.
Die Aktor-Steuerung 320 ist eine Steuerung, mit der der bewegliche Körper 1 ausgestattet ist, und sie veranlasst, dass ein Aktor so arbeitet, dass der bewegliche Körper dem Soll-Steuerungswert folgt, der in der Steuerungswert-Berechnungseinheit 310 berechnet wird. Beispiele des Aktors schließen die Lenkung, einen Antriebsmotor, sowie Bremsen ein.
Als nächstes wird ein Beispiel des Betriebs der Pfad-Planungsvorrichtung 200 gemäß der ersten Ausführungsform unter Bezugnahme auf das Ablaufdiagramm beschrieben, das in 6 dargestellt ist. Nachfolgend wird der Fall beschrieben, in dem Partikel-Filter verwendet werden. Es sei angemerkt, dass nachfolgend „ein Schritt“ eine Abtastzeit bzw. einen Abtastzeitpunkt in einem Steuerungszyklus bezeichnet.
Zunächst werden als eingegebene Information der Pfad-Planungsvorrichtung 200 Sollwerte, wie z. B. die Sollposition, die Sollgeschwindigkeit und der Sollazimut, von der Sollwert-Erfassungseinheit 110 erfasst, eigene Zustände, wie z. B. die Position, die Geschwindigkeit und der Azimut des beweglichen Körpers, werden von der Erfassungseinheit 120 für eigenen Zustand erfasst, und eine Umgebungsinformation, wie z. B. die Koordinaten jedes Gipfels, die Position des beweglichen Hindernisses und die Geschwindigkeit im befahrbaren Bereich, werden von der Umgebungs-Erfassungseinheit 130 erfasst (Schritt S101). 7 veranschaulicht ein konzeptuelles Diagramm der eingegebenen Information in diesem Fall.
In 7 schließen die Koordinaten einer Mehrzahl von Gipfeln bzw. Scheitpunkten VTA, die den befahrbaren Bereich TA definieren, eine x-Koordinate, die durch x_f1, x_f2, x_f3, ..., x_fi dargestellt wird, und eine y-Koordinate ein, die durch y_f1, y_f2, y_f3, ..., y_fi dargestellt wird.
Die Sollzustandsgröße TG schließt Koordinaten (x_t, y_t) der Sollposition, einen Sollazimut θ_t und eine Sollgeschwindigkeit v_t ein, und die gegenwärtige eigene Zustandsgröße des beweglichen Körpers 1 schließt eine x-Koordinate, die durch x_e, ausgedrückt wird, eine y-Koordinate, die durch y_e ausgedrückt wird, eine Geschwindigkeit, die durch v_e ausgedrückt wird, und einen Azimut ein, der durch θ_e ausgedrückt wird.
Außerdem schließen die Koordinaten des beweglichen Hindernisses MOB eine x-Koordinate, die durch x_O1, x_O2, x_O3, ..., x_Oi ausgedrückt wird, eine y-Koordinate, die durch y_O1, y_O2, y_O3, ..., y_Oi ausgedrückt wird, eine Geschwindigkeit, die durch v_O1, v_O2, v_O3, ..., v_Oi ausgedrückt wird, und einen Azimut ein, der durch θ_O1, θ_O2, θ_O3, ..., θ_Oi ausgedrückt wird.
In 7 fehlt der befahrbare Bereich TA teilweise, da ein Hindernis vorhanden ist, und die Mehrzahl von Gipfeln VTA befinden sich entlang des Umrisses des Hindernisses.
Es sei angemerkt, dass in der vorliegenden Ausführungsform jeder Gipfel des befahrbaren Bereichs TA als Information des befahrbaren Bereichs TA extrahiert wird. Es kann jedoch auch eine Information einer Linie wie z. B. eines Kreises oder einer Ellipse verwendet werden.
Indem die eigenen Zustände verwendet werden, die von der Erfassungseinheit 120 für eigenen Zustand erhalten werden, können Werte verwendet werden, die in ein Koordinatensystem des beweglichen Körpers umgewandelt sind, wobei die Position des beweglichen Körpers 1 als der Ursprung dargestellt wird, die Richtung des beweglichen Körpers 1 als die x-Achse dargestellt wird und die Richtung senkrecht zu der Richtung des beweglichen Körpers als die y-Achse dargestellt wird, wie in 8 veranschaulicht. Solche Werte des beweglichen Körpers Koordinatensystem werden nachfolgend verwendet.
Wie in 8 veranschaulicht, schließen im Koordinatensystem des beweglichen Körpers die Koordinaten der Mehrzahl von Gipfeln VTA eine x-Koordinate, die durch X_f1, X_f2, X_f3, ..., X_fi ausgedrückt wird, und eine y-Koordinate ein, die durch Y_f1, Y_f2, Y_f3, ..., Y_fi ausgedrückt wird.
Die Sollzustandsgröße TG schließt Koordinaten (X_t, Y_t) der Sollposition, einen Sollazimut Θ_t und eine Sollgeschwindigkeit V_t ein, und die gegenwärtig eigene Zustandsgröße des beweglichen Körpers 1 schließt eine x-Koordinate, die durch X_e dargestellt wird, eine y-Koordinate, die durch Y_e dargestellt wird, eine Geschwindigkeit, die durch v_e dargestellt wird, und einen Azimut ein, der durch Θ_e dargestellt wird.
Außerdem schließen die Koordinaten des beweglichen Hindernisses MOB eine x-Koordinate, die durch x_O1, x_O2, x_O3, ..., x_Oi ausgedrückt wird, eine y-Koordinate, die durch y_O1, y_O2, y_O3, ..., y_Oi ausgedrückt wird, eine Geschwindigkeit, die durch v_O1, v_O2, v_O3, ..., v_Oi ausgedrückt wird, und einen Azimut ein, der durch θ_O1, θ_O2, θ_O3, ..., θ_Oi ausgedrückt wird.
In der Koordinatentransformation jedes Werts, der sich auf die Sollwert-Information bezieht, wird der folgende Ausdruck (1) verwendet.
[Ausdruck 1] $\begin{array}{l} (\begin{matrix} X_{t} \\ Y_{t} \end{matrix}) = (\begin{matrix} cos (- θ_{e}) & - sin (- θ_{e}) \\ sin (- θ_{e}) & cos (- θ_{e}) \end{matrix}) (\begin{matrix} x_{t} - x_{e} \\ y_{t} - y_{e} \end{matrix}) \\ Θ_{t} = θ_{t} - θ_{e} \end{array}$
In der Koordinatentransformation jedes Werts, der sich auf die bewegliche Hindernis-Information bezieht, wird der folgende Ausdruck (2) verwendet.
[Ausdruck 2] $\begin{array}{l} (\begin{matrix} X_{o i} \\ Y_{o i} \end{matrix}) = (\begin{matrix} cos (- θ_{e}) & - sin (- θ_{e}) \\ sin (- θ_{e}) & cos (- θ_{e}) \end{matrix}) (\begin{matrix} x_{o i} - x_{e} \\ y_{o i} - y_{e} \end{matrix}) \\ Θ_{o i} = θ_{o i} - θ_{e} \end{array}$
In der Koordinatentransformation jedes Werts, der sich auf jeden Gipfel des befahrbaren Bereichs TA bezieht, wird der folgende Ausdruck (3) verwendet.
[Ausdruck 3] $\begin{array}{l} (\begin{matrix} X_{f i} \\ Y_{f i} \end{matrix}) = (\begin{matrix} cos (- θ_{e}) & - sin (- θ_{e}) \\ sin (- θ_{e}) & cos (- θ_{e}) \end{matrix}) (\begin{matrix} x_{f i} - x_{e} \\ y_{f i} - y_{e} \end{matrix}) \\ Θ_{f i} = θ_{f i} - θ_{e} \end{array}$
Hier nimmt die Beschreibung erneut auf das Ablaufdiagramm in 6 Bezug. Nachdem die Umgebungsinformation in Schritt S101 erfasst wurde, wird der befahrbare Bereich TA ohne Hindernisse, in dem der bewegliche Körper 1 fahren kann, in der Berechnungseinheit 210 für einen befahrbaren Bereich berechnet, und zwar auf der Basis der Information, die von der Umgebungs-Erfassungseinheit 130 erfasst wird (Schritt S 102). In der vorliegenden Ausführungsform werden die X-Koordinaten X_f1 bis X_fi und die Y-Koordinaten Y_f1 bis Y_fi, die sich auf jeden Gipfel VTA des befahrbaren Bereichs TA im Koordinatensystem des beweglichen Körpers beziehen, das in in 8 veranschaulicht ist, zur Pfaderzeugung verwendet.
Es sei angemerkt, dass anstatt die X-Koordinaten X_f1 bis X_fi und die Y-Koordinaten Y_f1 bis Y_fi, die sich auf jeden Gipfel VTA des befahrbaren Bereichs TA beziehen, zur Pfaderzeugung zu verwenden, auf der Basis einer Zeitreihen-Variation der Form des befahrbaren Bereichs TA, berechnet in der Vergangenheit, auch ein befahrbarer Bereich zu einer Zeit später als der befahrbare Bereich, der zur gegenwärtigen Zeit berechnet wird, vorhergesagt werden und als ein vorhergesagter befahrbarer Bereich verwendet werden kann, und der befahrbare Bereich, der zur gegenwärtigen Zeit berechnet wird, und der vorhergesagte befahrbare Bereich können miteinander kombiniert werden und als der befahrbare Bereich verwendet werden, der zur Pfaderzeugung verwendet werden soll.
9 ist ein konzeptuelles Diagramm, das ein Verfahren zum Vorhersagen des befahrbaren Bereichs veranschaulicht, auf der Basis einer Zeitreihen-Variation der Form des befahrbaren Bereichs TA. In 9 ist die gegenwärtige Zeit durch t als Zeit T dargestellt, die Zeit früher als t um eine Abtastzeit im Steuerungszyklus ist durch t-1 dargestellt, die Zeit noch früher als t-1 um eine Abtastzeit ist durch t-2 dargestellt, und die Zeit später als die Zeit t um eine Abtastzeit ist durch t+1 dargestellt.
In 9 fährt der bewegliche Körper 1 in Richtung des Pfeils vorwärts, und der befahrbare Bereich TA fehlt teilweise, weil voraus ein Hindernis vorhanden ist. Aus der Variation des in der Vergangenheit befahrbaren Bereichs TA zur Zeit t-2, des in der Vergangenheit befahrbaren Bereichs TA zur Zeit t-1 und des gegenwärtig befahrbaren Bereichs TA zur Zeit t über der Zeit ist ersichtlich, dass es ein Teil NP1 gibt, das sich in der Rückwärtsrichtung so viel ändert, wie der bewegliche Körper 1 fährt, und ein Teil NP2, das sich nicht ändert oder nur geringfügig ändert, obwohl der bewegliche Körpers 1 fährt. Es ist wahrscheinlich, dass das Teil NP1 ein ortsfestes Hindernis ist, und es ist wahrscheinlich, dass das Teil NP2 ein bewegliches Hindernis ist. Aus der Information wie oben beschrieben gilt Folgendes: Wenn ein in der Zukunft befahrbarer Bereich zur Zeit t+1 vorhergesagt wird, ist der schraffierte Bereich in der Zeichnung am rechten Ende in 9 ein vorhergesagter befahrbarer Bereich ETA, und indem der gegenwärtig befahrbare Bereich TA zur Zeit t und der vorhergesagte befahrbare Bereich ETA kombiniert werden, kann der gegenwärtig befahrbare Bereich TA erweitert werden.
Indem der vorhergesagte befahrbare Bereich ETA verwendet wird, kann der Pfad bis zu einem Bereich erzeugt werden, der weiter als der befahrbare Bereich entfernt ist, der zur gegenwärtigen Zeit erkannt werden kann und durch externe Sensoren erhalten wird. Der Bereich kann der befahrbare Bereich in der Zukunft sein.
Auf der Basis des Typs eines Hindernisses außerhalb des befahrbaren Bereichs TA kann außerdem der vorhergesagte befahrbare Bereich berechnet und zur Pfaderzeugung verwendet werden.
10 ist ein konzeptuelles Diagramm, das ein Verfahren zum Vorhersagen des befahrbaren Bereichs auf der Basis des Typs eines Hindernisses veranschaulicht. Es sei angemerkt, dass Beispiele des Typs eines Hindernisses eine Wand, ein anderes Fahrzeug, das angehalten hat, und ein anderes Fahrzeug in Bewegung einschließen. Wenn das Hindernis ein anderes Fahrzeug in Bewegung ist, erfasst die Umgebungs-Erfassungseinheit 130 nicht bloß die Position, sondern auch die Geschwindigkeit und dergleichen des andren Fahrzeugs in Bewegung. Dann berechnet die Berechnungseinheit 210 für einen befahrbaren Bereich den vorhergesagten befahrbaren Bereich auf der Basis der Position, der Geschwindigkeit und dergleichen des anderen Fahrzeugs in Bewegung. Folglich schließt der vorhergesagte befahrbare Bereich auch einen Bereich ein, der nicht im befahrbaren Bereich enthalten ist, der zur gegenwärtigen Zeit berechnet wird. Dies bedeutet, dass selbst dann, wenn bestimmt wird, dass ein Bereich zur gegenwärtigen Zeit nicht befahrbar ist, bestimmt wird, dass der Bereich in der Zukunft befahrbar ist.
In 10 ist der gegenwärtig befahrbare Bereich TA zur gegenwärtigen Zeit t für den beweglichen Körper 1 bei der Geschwindigkeit v_e als linke Zeichnung dargestellt, und ein zukünftig vorhergesagter befahrbarer Bereich TAX, der um die Zeit t_x später als die gegenwärtige Zeit ist, ist als rechte Zeichnung dargestellt.
In der linken Zeichnung in 10 ist der maximale erkannte Abstand des befahrbaren Bereichs TA durch L_max dargestellt, und ein Bereich NR ohne Hindernisse ist durch eine unterbrochene Linie vor dem am weitesten entfernten Teil des befahrbaren Bereichs TA angezeigt. Außerdem fehlt der befahrbare Bereich TA teilweise wegen des Vorhandenseins des ortsfesten Hindernisses SOB und des beweglichen Hindernisses MOB bei der Geschwindigkeit vo.
In der rechten Zeichnung in 10 ist ein Punkt, wo sich das ortsfeste Hindernis SOB voraus befindet, ein Bereich R1, der selbst in der Zukunft nicht befahren werden kann, und ein Punkt, wo sich das bewegliche Hindernis MOB voraus befindet, ist ein erweiterter Bereich R2, der erhalten wird, indem der befahrbare Bereich TA erweitert wird, unter der Annahme, dass sich der befahrbare Bereich in der Zukunft in der Fahrtrichtung des beweglichen Hindernisses MOB erweitert. Die zu erweiternde Länge wird ausgedrückt durch (v_O - v_e) × t_x. Außerdem ist ein Punkt jenseits des maximal erkannten Abstands L_max des befahrbaren Bereichs TA ein erweiterter Bereich R3, der erhalten wird, indem der befahrbare Bereich TA erweitert wird, unter der Annahme, dass ein Bereich jenseits des Punkts ebenfalls befahrbar ist. Die zu erweiternde Länge wird ausgedrückt durch v_e × t_x.
Indem der vorhergesagte befahrbare Bereich TAX verwendet wird, kann der Pfad bis zu einer Position erzeugt werden, die weiter als der befahrbare Bereich entfernt ist, der zur gegenwärtigen Zeit erkannt werden kann und durch externe Sensoren erhalten wird. Dies verbessert die Zuverlässigkeit, da der befahrbare Bereich auf der Basis der tatsächlichen Bewegung des Hindernisses vorhergesagt wird. Außerdem kann der befahrbare Bereich TA nicht nur unter Berücksichtigung der tatsächlichen Bewegung des Hindernisses vorhergesagt werden, sondern auch der Zeitreihen-Variation der Form des befahrbaren Bereichs TA, wie in 9 veranschaulicht. Dies verbessert die Zuverlässigkeit weiter.
Bei der Berechnung des befahrbaren Bereichs TA in Schritt S102 gilt außerdem Folgendes: Wenn es einen Bereich gibt, der befahrbar ist, aber eine Sackgasse sein könnte, wird eine Verarbeitung zum Ausschließen des Bereichs aus dem befahrbaren Bereich TA durchgeführt, wie notwendig. 11 ist ein konzeptuelles Diagramm, das die Verarbeitung veranschaulicht.
11 veranschaulicht eine Situation, in der es eine schmale Durchfahrt, wie z. B. ein ETC-Tor, vor dem beweglichen Körper 1 gibt und eine Sollposition TGP innerhalb eines passierbaren Tors vorhanden ist. Andere Tore sind nicht befahrbar und sind Sackgassen DE, die in der Zukunft nicht befahrbar sind, und sie werden aus dem befahrbaren Bereich TA als ausgeschlossene Bereiche AR ausgeschlossen.
Hinsichtlich eines Bereichs, der eine Sackgasse sein kann, wird beispielsweise ein Ort mit dessen Wert in y-Achsen-Richtung, der eine Abweichung hat, obwohl ein Wert in der x-Achsen-Richtung ein Wert ist, der im Wesentlichen der gleiche wie die Sollposition TGP ist, als eine Sackgasse angesehen. Wenn alternativ eine Überkopfansicht, eine Luftaufnahme oder dergleichen wie diejenige in 11 erhalten wird, wird ein Bereich, wo es eine Wand vor der Sollposition TGP gibt, ein Bereich, in dem die Sollposition TGP von einer Wand umgeben ist, oder dergleichen unter Verwendung von Bildverarbeitungstechnologie detektiert. Wenn eine Positionsinformation wie z. B. eine Baustelle und ein nicht betriebsbereites ETC-Tor erhalten wird, indem die Infrastruktur-Information verwendet wird, wird ein solcher unpassierbarer Bereich mit dem befahrbaren Bereich TA verglichen, und ein Bereich in dem befahrbaren Bereich TA, in dem der beweglicher Körper 1 in der Zukunft nicht fahren kann, wird als ein Bereich angesehen, der eine Sackgasse sein kann.
Dies ermöglicht die Erzeugung eines Pfads, um zu verhindern, dass er von einer Sackgasse umgeben wird und unbeweglich ist, bevor die Sollposition TGP erreicht wird, und zwar sogar innerhalb des befahrbaren Bereichs TA.
Bei der Berechnung des befahrbaren Bereichs TA in Schritt S102 gilt außerdem Folgendes: Wenn es einen Bereich gibt, der befahrbar ist, aber die Einfahrt verboten ist, wird eine Verarbeitung zum Ausschließen des Bereichs aus dem befahrbaren Bereich TA durchgeführt, wie notwendig. 12 ist ein konzeptuelles Diagramm, das die Verarbeitung veranschaulicht.
12 veranschaulicht eine Situation, in der ein Bereich IPA, in den die Einfahrt verboten ist, vor dem beweglichen Körper 1 liegt. Beispiele für den Bereich IPA, in den die Einfahrt verboten ist, schließen einen Bereich ein, der eine Baustelle ist oder dergleichen, und er ist ein Bereich, der nicht von einem Hindernis wie z. B. einem Zaun umgeben ist. Ein Bereich, wo sich der befahrbare Bereich TA und der Bereich IPA, in den die Einfahrt verboten ist, überlappen, wird aus dem befahrbaren Bereich TA als der ausgeschlossene Bereich AR ausgeschlossen.
Hinsichtlich des Bereichs IPA, in den die Einfahrt verboten ist, gilt Folgendes: Wenn eine Überkopfansicht, eine Luftaufnahme oder dergleichen wie jene in 12 erhalten wird, wird beispielsweise ein Baustellenbereich oder dergleichen unter Verwendung von Bildverarbeitungstechnologie detektiert, und ein Bereich, in den die Einfahrt nicht erlaubt ist, wie z. B. eine Baustelle, wird unter Verwendung von Bildverarbeitungstechnologie detektiert, indem eine Frontkamera verwendet wird, die am beweglichen Körper 1 angebracht ist. Wenn die Positionsinformation wie z. B. die Baustelle erhalten wird, indem die Infrastruktur-Information verwendet wird, wird ein solcher Baustellenbereich oder dergleichen mit dem befahrbaren Bereich verglichen, und ein Baustellenbereich oder dergleichen im befahrbaren Bereich TA wird als der Bereich IPA angesehen, in den die Einfahrt verboten ist.
Ferner gilt bei der Berechnung des befahrbaren Bereichs TA in Schritt S102 Folgendes: Wenn es eine die Querung verbietende Linie gibt, die ein Fahren über die Querung verbietende Linie hinweg verbietet, wird eine Verarbeitung durchgeführt, bei der nur ein Bereich, in dem der befahrbare Bereich TA und die Querung verbietende Linie einander überlappen, befahrbar gemacht wird. 13 ist ein konzeptuelles Diagramm, das die Verarbeitung veranschaulicht.
13 veranschaulicht eine Situation, bei der die Querung verbietende Linien NSL auf der linken und rechten Seite des beweglichen Körpers 1 in Fahrtrichtung vorhanden sind. Beispiele der die Querung verbietenden Linie NSL schließen Folgendes ein: Eine weiße durchgezogene Linie, die das Fahren aus der Fahrspur verbietet, und eine gelbe durchgezogene Linie, die das Fahren aus der Fahrspur verbietet, und zwar zum Überholen, und die auf einer Straßenfläche angeordnet sind. Außerdem sind Regeln wie das Verbot zum Wechseln der Fahrspur innerhalb von 30 m einer Kreuzung und das Verbot zum Wechseln der Fahrspur unmittelbar vor einem ETC-Tor ebenfalls eingeschlossen. Außerdem ist ein Bereich, der vom befahrbaren Bereich TA und den die Querung verbietenden Linien NSL umgeben ist, aus dem befahrbaren Bereich TA als der ausgeschlossene Bereich AR ausgeschlossen, und nur ein Bereich, in dem der befahrbare Bereich TA und die die Querung verbietenden Linien überlappen, ist befahrbar.
Wie oben beschrieben, gilt unter Bezugnahme auf 11 auf 13 Folgendes: Die Berechnungseinheit 210 für einen befahrbaren Bereich sagt einen Bereich vorher, in dem der bewegliche Körper 1 in der Zukunft nicht fahren kann, und zwar auf der Basis der Umgebungsinformation des beweglichen Körpers 1, und sie berechnet einen Bereich, der erhalten wird, indem der vorhergesagte Bereich ausgeschlossen wird, als den befahrbaren Bereich TA. Es sei angemerkt, dass der Bereich, der in der Zukunft nicht befahrbar ist, nicht auf die Bereiche beschränkt ist, die in 11 bis 13 veranschaulicht sind.
Hier nimmt die Beschreibung erneut auf das Ablaufdiagramm in 6 Bezug. Nachdem der befahrbare Bereich TA in Schritt S102 berechnet wurde, wird die Sollzustandsgröße, die vom beweglichen Körper 1 erreicht werden soll, in der Sollzustand-Berechnungseinheit 220 berechnet, und zwar auf der Basis der Sollwerte, die in der Sollwert-Erfassungseinheit 110 erfasst werden (Schritt S103). 14 veranschaulicht ein schematisches Diagramm der Sollzustandsgröße gemäß der vorliegenden Ausführungsform.
Die Sollzustandsgröße P_t wird durch den folgenden Ausdruck (4) ausgedrückt, wobei die Sollzustandsgröße Folgendes aufweist: eine Soll-Lateralposition Y_L, eine Sollgeschwindigkeit V_t und einen Sollazimut Θ_t, der eine Position in der Richtung senkrecht zur Sollposition in Sollazimut-Richtung darstellt, berechnet unter Verwendung der Sollposition (X_t, Y_t), der Sollgeschwindigkeit V_t und des Sollazimuts Θ_t, und sie weist ferner einen Soll-Fahrzeug-zu-Fahrzeug-Abstand D_t auf, um den Abstand D_O vom beweglichen Hindernis MOB in einem Gefahrbereich D gleich groß wie oder größer als den Soll-Fahrzeug-zu-Fahrzeug-Abstand D_t zu machen, wie in 14 veranschaulicht.
[Ausdruck 4] $P_{t} = {[\begin{matrix} Y_{L} & V_{t} & θ_{t} & D_{t} \end{matrix}]}^{T}$
Indem die Sollgeschwindigkeit V_t in die Sollzustandsgröße eingeschlossen wird, kann beispielsweise ein Pfad erzeugt werden, der mit jeglicher Geschwindigkeit wie z. B. innerhalb der zulässigen Geschwindigkeitsbeschränkung befahrbar ist.
Indem der Sollazimut Θ_t in die Sollzustandsgröße eingeschlossen wird, gilt beispielsweise beim Einfahren in eine vorausliegende schmale Durchfahrt in einer Situation, in der ein Mautstelle-Tor oder dergleichen durchfahren wird, Folgendes: Indem der Azimut in der Richtung senkrecht zum Tor auf die Sollzustandsgröße vorgegeben wird, kann ein Pfad erzeugt werden, der das Einfahren geradeaus in das Tor erlaubt.
Indem die Soll-Lateralposition Y_L in die Sollzustandsgröße eingeschlossen wird und nur Komponenten der Sollposition in der Seitwärts-Richtung als die Sollzustandsgröße für den beweglichen Körper 1 verwendet werden, der sich nicht seitwärts bewegen kann, kann die Abweichung bezogen auf die Sollposition in der Seitwärts-Richtung frühzeitig verringert werden, und die Sollposition kann in einen Zustand gebracht werden, der ein frühzeitiges Passieren im Sollazimut erlaubt.
Der Gefahrbereich D, der in 14 veranschaulicht ist, ist als ein Sicherheits-Fahrzeug-zu-Fahrzeug-Abstand definiert, der von einem in Bewegung befindlichen vorausfahrenden Fahrzeug und einem ortsfesten Hindernis gewährleistet werden muss, und ein Abstand für eine Person und ein anderes Fahrzeug, von dem sich ferngehalten werden soll, da es dann, wenn sich der bewegliche Körper 1 bewegt, eine größere Möglichkeit der Kollision beim Vorhandensein einer Person und eines anderen Fahrzeugs in der Nähe des beweglichen Körpers 1 gibt, und wenn es eine Kollision gibt, verursacht dies einen großen Schaden, und es ist gefährlich. In 14 befindet sich das bewegliche Hindernis MOB innerhalb des Gefahrbereichs D, und der Abstand D_O vom beweglichen Hindernis MOB ist kleiner als der Soll-Fahrzeug-zu-Fahrzeug-Abstand D_t.
Außerdem kann die Sollzustandsgröße P_t im folgenden Ausdruck (5) unter Verwendung der Sollposition (X_t, Y_t) ausgedrückt werden.
[Ausdruck 5] $P_{t} = {[\begin{matrix} \begin{matrix} X_{t} \end{matrix} Y_{L} & V_{t} & θ_{t} & D_{t} \end{matrix}]}^{T}$
Wenn ferner zumindest die Sollposition (X_t, Y_t) in der Sollzustandsgröße P_t enthalten ist, kann die Sollzustandsgröße P_t auch nicht die Sollgeschwindigkeit V_t, den Sollazimut Θ_t und den Soll-Fahrzeug-zu-Fahrzeug-Abstand D_t verwenden. Dies rührt daher, dass dann, wenn die Sollposition (X_t, Y_t) zumindest als die Sollzustandsgröße enthalten ist, ein Pfad, der zur Sollposition erreichbar ist, in der Pfad-Erzeugungseinheit 260 erzeugt werden kann.
Wenn die Sollzustandsgröße außerhalb des befahrbaren Bereichs TA ist, kann die Sollzustand-Berechnungseinheit 220 auch eine Verarbeitung durchführen, so dass die Zustandsgröße innerhalb des befahrbaren Bereichs TA am nächsten zur Sollzustandsgröße als die Sollzustandsgröße vorgegeben wird. 15 ist ein konzeptuelles Diagramm, das die Verarbeitung veranschaulicht.
15 veranschaulicht einen Fall, in dem die Sollposition (X_t, Y_t) als die Sollzustandsgröße verwendet wird und eine ursprüngliche Sollposition OTGP außerhalb des befahrbaren Bereichs TA vorhanden ist. Die Sollposition innerhalb des befahrbaren Bereichs TA am nächsten zur ursprünglichen Sollposition OTGP in geradem Abstand ist in der Umgebung der rechten Ecke des befahrbaren Bereichs TA, und folglich wird die Sollposition TGP an der rechten Ecke vorgegeben.
Indem eine solche Verarbeitung durchgeführt wird, wird im Vergleich zu dem Fall, in dem die Sollposition TGP außerhalb des befahrbaren Bereichs TA ist, der erzeugte Pfad leichter innerhalb des befahrbaren Bereichs TA vorgegeben, was ein sicheres Fahren ermöglicht.
Außerdem kann die Sollzustand-Berechnungseinheit 220 auch einen oberen Grenzwert auf die Sollzustandsgröße vorgeben, die sich zumindest auf die Geschwindigkeit unter den Sollzustandsgrößen bezieht, und zwar in Abhängigkeit der Form des befahrbaren Bereichs TA, berechnet in der Berechnungseinheit 210 für befahrbaren Bereich. Beispielsweise wird ein oberer Grenzwert auf die Sollzustandsgröße vorgegeben, die sich auf die Geschwindigkeit bezieht, wenn der befahrbare Bereich TA schmal ist und wenn der erkannte Abstand des befahrbaren Bereichs TA so kurz ist, dass ein Fahren mit hoher Geschwindigkeit gehemmt wird.
16 ist ein konzeptuelles Diagramm, das eine Situation veranschaulicht, in der der befahrbare Bereich TA schmal ist. Wie in 16 veranschaulicht, befinden sich die ortsfesten Hindernisse SOB auf der rechten und linken Seite des beweglichen Körpers 1 in einem engen Abstand entlang der Fahrtrichtung des beweglichen Körpers 1. Der maximale erkannte Abstand des befahrbaren Bereichs TA ist L_max1, und er ist ungefähr so lang wie die Länge des ortsfesten Hindernisses SOB, aber er hat eine schmale Breite.
In diesem Fall gilt Folgendes: Indem ein oberer Grenzwert auf die Sollzustandsgröße vorgegeben wird, die sich auf die Sollposition oder den Sollazimut bezieht, wird der erzeugte Pfad leichter innerhalb des befahrbaren Bereichs TA vorgegeben, was ein Fahren ermöglicht, das den Insassen des beweglichen Körpers 1 keine geistigen Belastungen aufbürdet.
17 ist ein konzeptuelles Diagramm, das eine Situation veranschaulicht, in der der erkannte Abstand des befahrbaren Bereichs TA kurz ist. Wie in 17 veranschaulicht, ist der erkannte Abstand L_max2 des befahrbaren Bereichs TA vor dem beweglichen Körper 1 in Fahrtrichtung signifikant kleiner als der maximale erkannt Abstand L_max1 des befahrbaren Bereichs TA, der in 16 veranschaulicht ist, was eine Erkennung aus der Ferne hemmt.
Eine solche Situation wird angenommen, wenn der beweglicher Körper 1 ein Abschlepper ist und es ein Luftfahrzeug, das abgeschleppt werden soll, vor dem beweglichen Körper 1 gibt, wenn es eine Wand an einer nahen Position voraus gibt, und wenn der Frontdetektionsabstand eines Kamerasensors, der am beweglichen Körper 1 angebracht ist, kurz ist.
In diesem Fall gilt Folgendes: Indem ein oberer Grenzwert auf die Sollzustandsgröße vorgegeben wird, die sich auf die Geschwindigkeit bezieht, wird der erzeugte Pfad leichter innerhalb des befahrbaren Bereichs TA vorgegeben, was ein Fahren ermöglicht, das den Insassen des beweglichen Körpers 1 keine geistigen Belastungen aufbürdet.
Hier nimmt die Beschreibung erneut auf das Ablaufdiagramm in 6 Bezug. Nachdem die Sollzustandsgröße in Schritt S103 berechnet wurde, werden N_p Partikel in der Zustands-Vorhersageeinheit 230 definiert, und zwar auf der Basis des gegenwärtigen Zustands des beweglichen Körpers (Schritt S104). Die N_p Partikel schließen Zustandsgrößen ein, die voneinander verschieden sind. N_p ist eine Ganzzahl von 2 oder größer. In der vorliegenden Ausführungsform schließt eine Zustandsgröße P des Partikels zweidimensionale Positionen X_p und Y_p, einen Azimut Θ_p, eine Geschwindigkeit V_p, einen Lenkwinkel δ_p, eine Beschleunigung a_p und eine Lenkwinkelgeschwindigkeit u_p des beweglichen Körpers ein und wird durch den Ausdruck (6) ausgedrückt.
[Ausdruck 6] $P = {[\begin{matrix} X_{p} & Y_{p} & θ_{p} & V_{p} & δ_{p} & a_{p} & u_{p} \end{matrix}]}^{T}$
Es sei angemerkt, dass die zweidimensionalen Positionen X_p und Y_p und der Azimut Θ_p im Koordinatensystem aus 8 unter Bezugnahme auf die gegenwärtige Position und den gegenwärtigen Azimut des beweglichen Körpers 1 dargestellt werden. Außerdem wird die Zustandsgröße eines n-ten Partikels durch P_n dargestellt. Die Anfangswerte der Zustandsvariablen sind die gleichen Werte in sämtlichen Partikeln. Die Anfangswerte der zweidimensionalen Positionen X_p und Y_p und der Azimut Θ_p sind 0, der Anfangswert der Geschwindigkeit V_p is die gegenwärtige Geschwindigkeit des beweglichen Körpers 1, der Anfangswert des Lenkwinkels δ_p ist der gegenwärtige Lenkwinkel des beweglichen Körpers 1, und die Anfangswerte der Beschleunigung a_p und der Lenkwinkelgeschwindigkeit u_p sind 0. Ferner ist eine Gewichtung W für jeden der Partikel definiert, und der Anfangswert der Gewichtung W ist in sämtlichen Partikeln gleich, und er ist ein Wert, der durch den folgenden Ausdruck (7) ausgedrückt wird, und die Zeit T_p wird definiert und auf den Anfangswert 0 vorgegeben.
[Ausdruck 7] $W = 1 / N p$
Es sei angemerkt, dass die Anzahl von Partikeln in Abhängigkeit der Form und der Fläche des befahrbaren Bereichs TA variabel sein kann, oder dass sie in Abhängigkeit des Abweichungsgrads von der Sollzustandsgröße variabel sein kann.
Nachdem die Partikel in Schritt S104 definiert wurden, werden in der Zustands-Vorhersageeinheit 230 so viele Eingaben wie die Anzahl von zufälligen Partikeln unter Verwendung von gleichmäßigen Zufallszahlen den jeweiligen Partikeln zugewiesen, und die Zustandsgröße in einer diskreten Zeitlänge T_d wird vorhergesagt (Schritt S105). Ein Verfahren zum Vorhersagen des Zustands jedes Partikels wird unten beschrieben.
Die Vorhersage der Zustandsgröße jedes Partikels wird unter Verwendung eines Systemmodells durchgeführt, und ein Modell, das in der vorliegenden Ausführungsform verwendet wird, wird unten beschrieben. Zustandsvariablen des Systemmodell sind zweidimensionale Positionen X_p und Y_p, ein Azimut Θ_p, eine Geschwindigkeit V_p und ein Lenkwinkel δ_p des Partikels, und die Zustandsgröße wird durch den folgenden Ausdruck (8) ausgedrückt.
Ausdruck 8 $P_{x} = {[\begin{matrix} X_{p} & Y_{p} & θ_{p} & V_{p} & δ_{p} \end{matrix}]}^{T}$
Außerdem schließt ein Eingabewert P_u in das Systemmodell die Beschleunigung a und die Lenkwinkelgeschwindigkeit u des Fahrzeugs ein, und wird durch den folgenden Ausdruck (9) ausgedrückt.
Ausdruck 9] $P_{u} = {[\begin{matrix} a & u \end{matrix}]}^{T}$
Außerdem wird der Schräglaufwinkel β des beweglichen Körpers 1 durch den folgenden Ausdruck (10) ausgedrückt.
Ausdruck 10] $β = t a n^{- 1} (t a n (δ) / 2)$
Hier wird das Systemmodell durch den folgenden Ausdruck (11) als Differentialgleichung ausgedrückt, unter Verwendung des Achsstands L des beweglichen Körpers 1.
[Ausdruck 11] $\frac{d P_{x}}{d t} = [\begin{matrix} V_{p} \cdot c o s (θ + β) / c o s (β) \\ V_{p} \cdot s i n (θ + β) / c o s (β) \\ V_{p} \cdot t a n (δ_{p}) / L \\ a \\ u \end{matrix}]$
Es sei angemerkt, dass gesagt werden kann, dass das oben beschriebene Systemmodell ein kinematisches Modell ist, bei dem ein vierrädriges Fahrzeug an ein zweirädriges Fahrzeug angenähert wird und die Mechanik nicht berücksichtigt wird. Es kann jedoch auch ein anderes Fahrzeug-Bewegungsmodell verwendet werden, wie z. B. ein zweirädriges Fahrzeugmodell, das ein dynamisches Modell ist, bei dem ein vierrädriges Fahrzeug an ein zweirädriges Fahrzeug angenähert wird.
Unter den Eingabevariablen in das Systemmodell wird für die Beschleunigung a hinsichtlich irgendeines oberen Grenzwerts a_mx und irgendeines unteren Grenzwerts a_mn, die im Voraus vorgegeben sind, ein Wert bestimmt, der den folgenden Ausdruck (12) erfüllt, und zwar unter Verwendung einer gleichmäßigen Zufallszahl für jeden Partikel.
[Ausdruck 12] $a_{m x} \geq a \geq a_{m n}$
Außerdem ist unter den Eingabevariablen in das Systemmodell für die Lenkwinkelgeschwindigkeit u hinsichtlich eines oberen Grenzwerts u_mx (> 0), der im Voraus vorgegeben ist, die Erfüllung des folgenden Ausdrucks (13) eine erste Nebenbedingung für die Lenkwinkelgeschwindigkeit u.
Ausdruck 13 $u_{m x} \geq | u |$
Außerdem ist hinsichtlich eines oberen Grenzwerts δ_mx (> 0), der sich auf den Lenkwinkel bezieht, ein Lenkwinkel δ' in einer diskreten Zeitlänge Td, der den folgenden Ausdruck (14) erfüllt, eine zweite Nebenbedingung für die Lenkwinkelgeschwindigkeit u.
Ausdruck 14 $δ_{m x} \geq | δ' |$
Der Lenkwinkel δ' in der diskreten Zeitlänge T_d wird ausgedrückt durch den folgenden Ausdruck (15).
[Ausdruck 15] $δ' = δ + u \cdot T d$
Folglich wird die zweite Nebenbedingung durch den folgenden Ausdruck (16) ausgedrückt.
[Ausdruck 16] $(δ_{mx} - δ) / T d \geq u \geq - (δ_{mx} + δ) / T d$
Für die Lenkwinkelgeschwindigkeit u unter dem Eingabewert P_u in das Systemmodell wird ein Wert, der die erste Nebenbedingung und die zweite Nebenbedingung erfüllt, unter Verwendung einer gleichmäßigen Zufallszahl für jeden Partikel bestimmt.
Wie oben beschrieben, wird die Zustandsgröße P_x' in der diskreten Zeitlänge T_d gemäß dem oben beschriebenen Systemmodell vorhergesagt, unter Verwendung des Eingabewerts P_u, der auf der Basis des oberen Lenkwinkel-Grenzwerts δ_mx als Nebenbedingung bestimmt wird. Dies ermöglicht die Vorhersage des Zustands der Partikel unter Berücksichtigung der Nebenbedingungen.
Die Zustandsgröße jedes Partikels wird unter Verwendung der vorhergesagten Zustandsgröße P_x' und des Eingabewerts P_u aktualisiert, und sie wird durch den folgenden Ausdruck (17) ausgedrückt. Außerdem wird ein Wert, der erhalten wird, indem die diskrete Zeitlänge T_d zur Zeit T addiert wird, als die Zeit nach der Aktualisierung verwendet.
Ausdruck 17 $P = {[\begin{matrix} P_{x}^{''} & P_{u}^{'} \end{matrix}]}^{'}$
18 veranschaulicht ein konzeptuelles Diagramm der Vorhersage des Zustands in der diskreten Zeitlänge T_d, wo die vorhergesagte Zustandsgröße jedes Partikels in der Zeit T_d durch P_nx' dargestellt wird. In 18 sind die vorhergesagten Zustandsgrößen P_nx' von n Partikeln in der Zeit T_d vor dem beweglichen Körper 1 veranschaulicht. Indem die Zustandsgrößen unter Verwendung einer Mehrzahl von Partikeln vorhergesagt werden, kann auf diese Weise eine Bewertung hinsichtlich außerhalb oder innerhalb des befahrbaren Bereichs TA aus der Positionsrelation zwischen dem Punkt jedes Partikels und dem befahrbarer Bereich TA durchgeführt werden, und folglich kann ein Pfad erzeugt werden, von dem gewährleistet werden kann, dass er innerhalb des befahrbaren Bereichs TA liegt.
Bei der Berechnung der Zustands-Vorhersageeinheit 230 werden die Eingaben unter Verwendung gleichmäßiger Zufallszahlen gemäß einer gleichmäßigen Verteilung verwendet, um zufällige Eingaben zu geben. Es können jedoch auch normale Zufallszahlen gemäß einer Normalverteilung oder zufällige Eingaben gemäß einer anderen Verteilung verwendet werden.
Bei der Berechnung der Zustands-Vorhersageeinheit 230 werden ferner Eingaben verwendet, die irgendeinen oberen Grenzwert nicht überschreiten, der im Voraus vorgegeben wird. Die Eingaben können jedoch auch Werte sein, die variabel sein können, und zwar in Abhängigkeit der Form des befahrbaren Bereichs TA.
19 ist ein konzeptuelles Diagramm, das einen Fall veranschaulicht, in dem Eingaben gemäß der Form des befahrbaren Bereichs TA vorgegeben werden, der schmal und lang ist. Wie in 19 veranschaulicht, gilt Folgendes: Wenn der befahrbare Bereich TA eine langgezogene Form hat, wird ein solcher oberer Grenzwert einer Eingabe der Lenkwinkelgeschwindigkeit u vorgegeben, der nicht veranlasst, dass sich die vorhergesagten Zustandsgrößen P_x' außerhalb des befahrbaren Bereichs TA befinden, wie in der Veranschaulichung, oder es wird eine Verteilung der Eingabe der Lenkwinkelgeschwindigkeit u schmaler gemacht. Indem eine solche Verarbeitung durchgeführt wird, werden die vorhergesagten Zustandsgrößen so gemacht, dass sie leicht erzeugt werden können.
Wenn der befahrbare Bereich TA eine schmale und lange Form hat, kann durch Verringern der Anzahl von Eingaben der Wert der Berechnung verringert werden.
20 ist ein konzeptuelles Diagramm, das einen Fall veranschaulicht, in dem die Eingaben gemäß der Form des befahrbaren Bereichs TA vorgegeben werden, der breit ist. Wie in 20 veranschaulicht, gilt Folgendes: Wenn der befahrbare Bereich TA eine breite Form hat, wird ein solcher oberer Grenzwert einer Eingabe der Beschleunigung a vorgegeben, der nicht verursacht, dass sich die vorhergesagten Zustandsgrößen P_x' außerhalb des befahrbaren Bereichs TA befinden, wie in der Veranschaulichung, eine Verteilung der Eingabe der Beschleunigung a wird verbreitert, oder die Anzahl von Eingaben wird erhöht. Indem eine solche Verarbeitung durchgeführt wird, werden die vorhergesagten Zustandsgrößen so gemacht, dass sie leicht erzeugt werden können.
Wenn der befahrbare Bereich TA eine breite Form hat, wird außerdem durch Erhöhen der Anzahl von Eingaben ein passenderer Pfad leichter erhalten.
Wenn die Abweichung zwischen der gegenwärtigen Zustandsgröße und der Sollzustandsgröße des beweglichen Körpers 1 groß ist, sei angemerkt, dass die Zustands-Vorhersageeinheit 230 die Anzahl von Eingabewerten in das Bewegungsmodell des beweglichen Körpers 1 erhöhen kann, wohingegen dann, wenn der Abweichungsgrad klein ist, die Zustands-Vorhersageeinheit 230 die Anzahl von Eingabewerten verringern kann. Fig .21 ist ein konzeptuelles Diagramm, das das Vorgeben von Eingabewerten veranschaulicht, wenn die Abweichung zwischen der gegenwärtigen Zustandsgröße und der Sollzustandsgröße groß ist.
Wie in 21 veranschaulicht, gilt Folgendes: Wenn die Abweichung zwischen der Azimut-Sollzustandsgröße Ot und dem Azimut Θ_e an der gegenwärtigen Position des beweglichen Körpers 1 groß ist und die Sollposition nicht erreicht werden kann, ohne dass der Lenkwinkel vergrößert wird, wird die Lenkwinkelgeschwindigkeit als ein Eingabewert dem Bewegungsmodell hinzugefügt, und ein Pfad wird so vorhergesagt, dass die Lenkwinkelgeschwindigkeit erhöht wird.
In diesem Fall wird die Verteilung des Lenkwinkels breit vorgegeben. Mit anderen Worten: Die Bereiche der oberen und unteren Grenzwerte des Lenkwinkels werden breit vorgegeben. Indem die Anzahl von Eingabewerten erhöht wird, kann ferner ein passenderer Pfad leichter erhalten werden. Selbst wenn die Abweichung zwischen der gegenwärtigen Zustandsgröße und der Sollzustandsgröße des beweglichen Körpers 1 groß ist, kann damit die Sollposition leichter erreicht werden. Wenn im Gegensatz dazu die Abweichung klein ist, wird durch Verringern der Anzahl von Eingabewerten der Wert der Berechnung verringert, und die Rechenlast kann folglich verringert werden. Indem die Anzahl von Eingabewerten in Abhängigkeit des Abweichungsgrads zwischen der gegenwärtigen Zustandsgröße und der Sollzustandsgröße variabel gemacht wird, können auf diese Weise eine glatte Pfaderzeugung und eine Verringerung der Rechenlast im beweglichen Körper 1 implementiert werden.
Hier nimmt die Beschreibung erneut auf das Ablaufdiagramm in 6 Bezug. Nachdem die Zustandsgröße jedes Partikels in Schritt S105 vorhergesagt wurde, wird in der Bewertungseinheit 240 für einen vorhergesagten Zustand ein Beobachtungswert aus der aktualisierten Zustandsgröße jedes Partikels (Schritt S106) berechnet. Eine Beobachtungsvariable wird auf der Basis der Sollzustandsgröße definiert, die in der Sollzustand-Berechnungseinheit 220 berechnet wird. In der vorliegenden Ausführungsform sind das Erreichen der Soll-Lateralposition, die eine Position in der Richtung senkrecht zur Sollazimut-Richtung an der Sollposition ist, dass Beibehalten der Sollgeschwindigkeit, das Erreichen des Sollazimuts und das Beibehalten eines sicheren Abstands vom beweglichen Hindernis die Ziele. Auf der Basis dieser Ziele wird ein Beobachtungswert P_y durch den folgenden Ausdruck (18) ausgedrückt, wobei eine Abweichung L_e zwischen dem Partikel und der Soll-Lateralposition Y_L, eine Geschwindigkeit Zustandsgröße V_p des Partikels, eine Azimut-Zustandsgröße Θ_p des Partikels und ein Einfahrtabstand D_p zum Gefahrbereich D die Beobachtungsvariablen sind.
[Ausdruck 18] $P_{y} = {[\begin{matrix} L_{e} & V_{p} & θ_{p} & D_{p} \end{matrix}]}^{'}$
22 ist ein Diagramm, das jede der Beobachtungsvariablen veranschaulicht. 22 veranschaulicht schematisch einen Partikel P_d, der die vorhergesagte Zustandsgröße P_x' in der Zeit T_d bezogen auf den gegenwärtigen beweglichen Körper 1 einschließt. Auf der Basis der Position und des Azimuts des Partikels P_d wird der Gefahrbereich D vorgegeben, und das bewegliche Hindernis MOB fährt in den Gefahrbereich D ein. Der Einfahrtabstand D_p des beweglichen Hindernisses MOB zum Gefahrbereich D wird durch den Abstand in der Richtung parallel zum Azimut Θ_p des Partikels P_d definiert.
Hier ist der Gefahrbereich D ein rechteckiger Bereich, wobei die Richtung der Langseiten in Richtung des Azimuts Θ_p des Partikels P_d geneigt ist, und der Bereich ist ein Bereich mit einer Länge des Abstands D_x vor dem Partikel P_d und einer Länge des Abstands D_y nach rechts und links. Hier wird der Abstand D_x durch den folgenden Ausdruck (19) ausgedrückt, unter Verwendung der Geschwindigkeit V des Partikels P_d und der Sicherheits-Erwartungszeit T_s, die im Voraus vorgegeben ist.
[Ausdruck 19] $D_{x} = V \cdot T_{x}$
Ferner wird der Abstand D_y durch den folgenden Ausdruck (20) ausgedrückt, unter Verwendung eines Parameters T_sy, der im Voraus vorgegeben ist.
[Ausdruck 20] $D_{y} = V \cdot T_{s y}$
Nachdem der Beobachtungswert jedes Partikels in Schritt S 106 berechnet wurde, wird in der Bewertungseinheit 240 für einen vorhergesagten Zustand die Gewichtung W jedes Partikels aktualisiert, und zwar aus der Differenz zwischen dem Beobachtungswert P_y jedes Partikels und einem idealen Beobachtungswert P_yi (Schritt S107). Hier ist der ideale Beobachtungswert P_yi ein Beobachtungswert für den beweglichen Körper 1 in einer virtuell vorgegebenen Sollzustandsgröße, und wenn der bewegliche Körper 1 die Sollzustandsgröße erfüllt, ist der bewegliche Körper 1 ein idealer Zustand. In der vorliegenden Ausführungsform schließt der ideale Beobachtungswert P_yi eine Abweichung L_enom von einer Soll-Seitwärts-Abweichung, eine Soll-Fahrzeuggeschwindigkeit V_nom, einen Sollazimut Θ_tnom und einen Soll-Einfahrtabstand D_nom auf der Basis der Sollzustandsgröße ein und wird durch den folgenden Ausdruck (21) ausgedrückt.
[Ausdruck 21] $P_{y i} = {[\begin{matrix} L_{e n o m} & V_{n o m} & θ_{n o m} & D_{n o m} \end{matrix}]}^{'}$
Wenn die Position des Partikels außerhalb des befahrbaren Bereichs TA liegt, der in Schritt S102 berechnet wird, wird außerdem die Gewichtung jedes Partikels als 0 vorgegeben, oder sie wird auf einen Wert niedriger als der Partikel innerhalb des befahrbaren Bereichs TA vorgegeben. Beispielsweise wird eine Bestimmung hinsichtlich außerhalb oder innerhalb des befahrbaren Bereichs TA durchgeführt, indem bestimmt wird, ob oder ob nicht es zweidimensionale Positionen X_p und Y_p des Partikels in einem polygonalen Bereich gibt, der jeden Gipfel des befahrbaren Bereichs TA und den beweglichen Körper 1 verbindet.
In der obigen Beschreibung haben hier die Partikel außerhalb des befahrbaren Bereichs TA Gewichtungen von 0. Hinsichtlich der Partikel außerhalb des befahrbaren Bereichs TA, können die Gewichtungen, die den Partikeln zugeordnet werden sollen, jedoch auch variabel sein, in Abhängigkeit vom Abweichungsgrad von dem befahrbaren Bereich TA.
23 ist ein konzeptuelles Diagramm der Verarbeitung, um die Gewichtungen, die den Partikeln außerhalb des befahrbaren Bereichs TA zugeordnet werden sollen, variabel zu machen. In 23 wird den Partikeln PW₄ innerhalb des befahrbaren Bereichs TA vor dem beweglichen Körper 1 eine Gewichtung W₄ zugeordnet, und der befahrbare Bereich TA kann als ein Gewichtung W₄-zugeordneter Bereich RW₄ bezeichnet werden.
Außerdem wird außerhalb des Gewichtung W₄-zugeordneten Bereichs RW₄ ein Gewichtung W₃-zugeordneter Bereich RW₃ vorgegeben, und einem Partikel PW₃, der darin vorhanden ist, wird die Gewichtung W₃ zugeordnet. Weiter außerhalb des Gewichtung W₃-zugeordneten Bereichs RW₃ wird außerdem ein Gewichtung W₂zugeordneter Bereich RW₂ vorgegeben, und wenn Partikel darin vorhanden ist, wird dem Partikel die Gewichtung W₂ zugeordnet. Weiter außerhalb des Gewichtung W₂zugeordneten Bereichs RW₂ wird außerdem ein Gewichtung W₁-zugeordneter Bereich RW₁ vorgegeben, und einem darin befindlichen Partikel PW₁ wird die Gewichtung W₁ zugeordnet. Es sei angemerkt, dass hinsichtlich der Gewichtung W₄ die schwerste ist, und die Gewichtung wird in der Reihenfolge der Gewichtungen W₃, W₂ und W₁ leichter.
Indem eine Bewertung der vorhergesagten Zustandsgröße in Abhängigkeit vom Abstand von dem befahrbaren Bereich TA durchgeführt wird, ohne die Partikel (Vorhersagepunkte) außerhalb des befahrbaren Bereichs TA zu entfernen, können auf diese Weise Pfadkandidaten auch in einem Bereich verbleiben, der von den externen Sensoren als außerhalb des befahrbaren Bereichs TA erkannt wird, obwohl er tatsächlich der befahrbare Bereich TA infolge einer Begrenzung des erkennbaren Bereichs der externen Sensoren oder dergleichen ist, und für die verbleibenden Pfadkandidaten kann eine Bewertung der Vorhersagepunkte in Abhängigkeit des Abstands von dem befahrbaren Bereich TA durchgeführt werden, d. h. der Zuverlässigkeit, was es ermöglicht, dass die Pfadplanung leichter gelingt. Indem den Partikeln, die vom befahrbaren Bereich TA weiter entfernt sind, leichtere Gewichtungen zugeordnet werden, kann es außerdem weniger wahrscheinlich gemacht werden, dass ein Pfad auf der Basis der Partikel erzeugt wird, und die Sicherheit des erzeugten Pfads kann verbessert werden.
Außerdem sind in 23 die Gewichtungen, die den Partikeln zugeordnet werden, diskontinuierliche Werte. Die zuzuordnenden Gewichtungen können jedoch auch Werte sein, die sich kontinuierlich in Abhängigkeit des Abstands vom befahrbaren Bereich TA ändern.
Hinsichtlich der Partikel im befahrbaren Bereich TA können die Gewichtungen, die den Partikeln zugeordnet werden sollen, auch variabel sein, in Abhängigkeit vom Abstand von einer Grenze, die den befahrbaren Bereich TA definiert.
24 ist ein konzeptuelles Diagramm der Verarbeitung, um die Gewichtungen, die den Partikeln innerhalb des befahrbaren Bereichs TA zugeordnet werden sollen, variabel zu machen. In 24 werden im befahrbaren Bereich TA vor dem beweglichen Körper 1, der Gewichtung W₁-zugeordnete Bereich RW₁, der Gewichtung W₂-zugeordnete Bereich RW₂, der Gewichtung W₃-zugeordnete Bereich RW₃ und der Gewichtung W₄zugeordnete Bereich RW₄ in der Reihenfolge von der Seite der Grenze, die den befahrbaren Bereich TA definiert, in Richtung der Innenseite vorgegeben. Die Verarbeitung in jedem zugeordneten Bereich ist die gleiche wie die Verarbeitung, die unter Bezugnahme auf 23 beschrieben ist, und hinsichtlich der Gewichtung ist W₄ die schwerste, und die Gewichtung wird in der Reihenfolge der Gewichtungen W₃, W₂ und W₁ leichter.
Indem eine Zuordnung der Gewichtungen durchgeführt wird, die kleiner werden, wenn die Bereiche näher an der Grenze liegen, die den befahrbaren Bereich TA definieren, und zwar in der vorhergesagten Zustandsgröße innerhalb des befahrbaren Bereichs TA, sind die Gewichtungen der vorhergesagten Zustandsgröße, die sich in der Mitte des befahrbaren Bereichs TA befindet, größer als jene in der Nähe der Grenze des befahrbaren Bereichs TA, und die Pfadplanung, die die Grenze des befahrbaren Bereichs TA in dem größtmöglichen Ausmaß vermeidet, kann durchgeführt werden, und die Sicherheit des erzeugten Pfads kann verbessert werden.
Außerdem sind in 24 die Gewichtungen, die den Partikeln zugeordnet werden, diskontinuierliche Werte. Die zuzuordnenden Gewichtungen können jedoch auch Werte sein, die sich kontinuierlich in Abhängigkeit des Abstands von der Grenze ändern, die den befahrbaren Bereich TA definiert.
Hinsichtlich der Partikel innerhalb des befahrbaren Bereichs TA kann außerdem die Verarbeitung so durchgeführt werden, dass Gewichtungen von 0 oder kleine Gewichtungen den Partikeln zugeordnet werden, die an einem Punkt vorhergesagt werden, der nach dem Fahren eine Sackgasse sein könnte, obwohl er sich innerhalb des befahrbaren Bereichs TA befindet. 25 ist ein konzeptuelles Diagramm, das die Verarbeitung veranschaulicht.
25 veranschaulicht eine Situation, in der es eine schmale Durchfahrt, wie z. B. ein ETC-Tor, vor dem beweglichen Körper 1 gibt und die Sollposition TGP innerhalb eines passierbaren Tors vorhanden ist. Andere Tore sind nicht befahrbar und sind Sackgassen DE, und sie sind Bereiche, auf denen nach dem Fahren der bewegliche Körper 1 in der Zukunft nicht fahren kann. Partikel PW, die in den Bereichen vorhergesagt sind, haben eine Gewichtung 0 (W = 0).
Dies ermöglicht auf diese Weise die Erzeugung eines Pfads, um zu verhindern, dass er von einer Sackgasse umgeben wird und unbeweglich ist, bevor die Sollposition TGP erreicht wird, und zwar sogar innerhalb des befahrbaren Bereichs TA.
Hinsichtlich der Bestimmung, ob es eine Sackgasse sein könnte, werden beispielsweise Partikel, die eine kleine Abweichung in der x-Richtung und eine große Abweichung in der y-Richtung von der Sollposition TGP in 25 haben, d. h. Partikel, die von der Sollposition TGP in der Horizontalrichtung (y-Richtung) verschoben, von einem Hindernis vor der Sollposition blockiert. Abhängig davon, ob oder ob nicht die Abweichung in der x-Richtung klein und die Abweichung in der y-Richtung von der Sollposition TGP groß ist, kann folglich bestimmt werden, ob oder ob nicht die Partikel sind, die an einem Punkt vorhergesagt sind, die eine Sackgasse sein könnte.
Wie oben unter Bezugnahme auf 11 beschrieben, können ferner die Partikel, die an einem Punkt vorhergesagt sind, der eine Sackgasse sein kann, wobei die Position der Sackgasse aus dem befahrbaren Bereich TA ausgeschlossen ist, im Voraus als außerhalb des befahrbaren Bereichs TA befindlich angesehen werden, und ihnen kann die Gewichtung W zugeordnet sein.
Hinsichtlich der Partikel innerhalb des befahrbaren Bereichs gilt ferner Folgendes: Wenn es eine die Querung verbietende Linie innerhalb des befahrbaren Bereichs gibt, und wenn die Partikel, die in Schritt S105 berechnet sind, am Pfadpunkt vorhanden sind, der in der Pfadpunkt-Berechnungseinheit 250 einen Schritt vorher berechnet ist, oder der Position zum Überqueren der die Querung verbietenden Linie für den beweglicher Körper 1, kann den Partikeln die Gewichtung W von 0 zugeordnet werden, oder ihnen kann eine kleine Gewichtung zugeordnet werden. 26 ist ein konzeptuelles Diagramm, das die Verarbeitung veranschaulicht.
26 veranschaulicht eine Situation, in der die die Querung verbietende Linien NSL auf der rechten und linken Seite des beweglichen Körpers 1 in der Fahrtrichtung vorhanden sind, und der erzeugte Pfad GT inklusive einer Mehrzahl von Pfadpunkten TP in der Fahrspur, die durch die rechten und linken die Querung verbietenden Linien NSL definiert wird, vorgegeben ist. Zur gegenwärtigen Zeit sind manche der Partikel, die in der Bewertungseinheit 240 für einen vorhergesagten Zustand bewertet werden, die Partikel PW, die an der Position zum Überqueren der die Querung verbietenden Linien NSL vorhergesagt sind, und zwar bezogen auf den Pfadpunkt TP, der in der Pfadpunkt-Berechnungseinheit 250 einen Schritt vorher erzeugt wurden, und sie haben die Gewichtung 0 (W = 0).
Indem die Gewichtung von 0 den Partikeln vorgegeben wird, die an der Position zum Überqueren der die Querung verbietenden Linien NSL bezogen auf den Pfadpunkt TP vorhergesagt sind, kann verhindert werden, dass ein Pfad in einem Bereich vorgegeben wird, in dem der bewegliche Körper 1 in der Zukunft nicht fahren kann, und die Sicherheit des erzeugten Pfads kann verbessert werden.
Wie oben unter Bezugnahme auf 13 beschrieben, kann ferner der Bereich, der vom befahrbaren Bereich TA und den die Querung verbietenden Linien NSL umgeben ist, aus dem befahrbaren Bereich TA als der ausgeschlossene Bereich AR ausgeschlossen werden, und die Partikel, die an dem Punkt vorhergesagt sind, können als außerhalb des befahrbaren Bereichs TA befindlich angesehen werden, und ihnen kann die Gewichtung W zugeordnet werden.
Hinsichtlich der Partikel außerhalb des befahrbaren Bereichs, die zur gegenwärtigen Zeit erkannt werden können, gilt ferner Folgendes: Wenn es einen vorhergesagten befahrbaren Bereich gibt, und wenn sich die Partikel in dem vorhergesagten befahrbaren Bereich befinden, kann die Verarbeitung so durchgeführt werden, dass sie nicht die Gewichtung W der Partikel auf 0 vorgibt, und die Gewichtung kleiner als die Gewichtung der Partikel innerhalb des befahrbaren Bereichs, die zur gegenwärtigen Zeit erkannt werden können, kann zugeordnet werden. 27 ist ein konzeptuelles Diagramm, das die Gewichtungs-Verarbeitung für die Partikel innerhalb des vorhergesagten befahrbaren Bereichs veranschaulicht.
In 27 ist der vorhergesagte befahrbare Bereich ETA vor dem befahrbaren Bereichs TA vorhanden, der zur gegenwärtigen Zeit erkannt werden kann, und dem Partikel PW₁, der innerhalb des vorhergesagten befahrbaren Bereichs ETA vorhanden ist, wird die Gewichtung W₁ zugeordnet. Ferner wird dem Partikel PW₂, der innerhalb des befahrbaren Bereichs TA vorhanden ist, die Gewichtung W₂ zugeordnet. Hier ist die Gewichtung W₁ kleiner als die Gewichtung W₂ (W₁ < W₂).
Indem der vorhergesagte befahrbare Bereich ETA verwendet wird, kann ein Pfad erzeugt werden, und zwar bis zu einer Position, die weiter als der befahrbare Bereich TA ist, der von den externen Sensoren zur gegenwärtigen Zeit erkannt werden kann. Da ferner die Zuverlässigkeit des vorhergesagten befahrbaren Bereichs ETA nicht hoch ist, kann durch relatives Erhöhen der Gewichtung der vorhergesagten Zustandsgröße innerhalb des befahrbaren Bereichs TA, der von den externen Sensoren zur gegenwärtigen Zeit erkannt werden kann, ein Pfad mit hoher Zuverlässigkeit erzeugt werden.
Hinsichtlich der Partikel zum Vorhersagen der Zustandsgröße in der diskreten Zeitlänge T_d in Schritt S105 wird hier unter Verwendung des vorhergesagten Pfads des beweglichen Hindernisses, erhalten von der Umgebungs-Erfassungseinheit 130, eine Bestimmung dahingehend durchgeführt, ob oder ob nicht es ein vorhergesagtes Hindernis gibt, das unter Verwendung des vorhergesagten Pfad zur gleichzeitigen Zeit erhalten wird, d. h. der gleichen Zeit wie die Zeit zum Vorhersagen der Partikel, und zwar in einem Bereich in der Umgebung jedes Partikels, und wenn das vorhergesagte Hindernis vorhanden ist, kann die Verarbeitung zum Vorgeben der Gewichtung W der Partikel auf 0 durchgeführt werden.
Die Gewichtung W vor der Aktualisierung jedes Partikels wird wieder als W_p definiert. Die Gewichtung W ist proportional zu der Gewichtung vor der Aktualisierung und der Wahrscheinlichkeit LLV, und sie wird aktualisiert, so dass der integrierte Wert der Gewichtungen sämtlicher Partikel 1 ist, unter Verwendung des folgenden Ausdrucks (22).
[Ausdruck 22] $W_{n} = \frac{W_{p n} \cdot L L V_{n}}{\sum_{m=1}^{N p} W_{p m} \cdot L L V_{m}} (n = 1 \dots N_{p})$
Hier wird die Wahrscheinlichkeit LLV unter Verwendung des folgenden Ausdrucks (23) berechnet, unter Verwendung einer Kovarianzmatrix Q, die sich auf die Zustandsgröße P_x des Partikels bezieht, die im Voraus vorgegeben ist, und eine Kovarianzmatrix R, die sich auf den Beobachtungswert P_y bezieht.
[Ausdruck 23] $L L V = \frac{1}{{(\sqrt{2 π})}^{2} \sqrt{| \prod |}} \cdot e x p (- \frac{1}{2} {(P_{y i} - P_{y})}^{T} \prod^{- 1} (P_{y i} - P_{y}))$
Hier wird die Matrix Π durch den folgenden Ausdruck (24) ausgedrückt.
Ausdruck 24] $\prod = H Q H' + R$
Es sei angemerkt, dass, wenn P_x = P_xn gilt, ein Wert H_n einer Messmatrix H im n-ten Partikel ein Wert ist, der erhalten wird, indem eine Messfunktion h nach der Zustandsgröße P_x differenziert wird, und wird durch den folgenden Ausdruck (25) ausgedrückt.
[Ausdruck 25] $P_{x} = h (P_{y})$
Außerdem ist die Messfunktion h eine Funktion zum Berechnen des Beobachtungswerts P_y aus der Zustandsgröße P_x und wird durch den folgenden Ausdruck (26) ausgedrückt.
[Ausdruck 26] $H_{n} = \frac{\partial h}{\partial P_{x}} |_{P_{x} = P_{x n}}$
Hinsichtlich der Gewichtungs-Aktualisierung jedes Partikels auf der Basis der Differenz zwischen dem Beobachtungswert jedes Partikels und dem idealen Beobachtungswert kann eine Bewertungs-Gewichtung hinsichtlich dessen vorgegeben werden, welches Element zu welchem Grad bewertet werden soll, auf der Basis der Differenz zwischen Dem beweglichen Körper 1 und der Sollzustandsgröße oder einer Anwendungssituation. 28 ist ein konzeptuelles Diagramm, das die Verarbeitung zum Vorgeben der Bewertungs-Gewichtung veranschaulicht.
In 28 ist der bewegliche Körper 1 von der Soll-Lateralposition Y_L entfernt, und in einem Bereich IA1, wo es wichtig ist, die Abweichung von der Soll-Lateralposition Y_L zu verringern, ist die Bewertungs-Gewichtung so vorgegeben, dass die Wahrscheinlichkeit der Partikel mit einer kleinen Abweichung von der Soll-Lateralposition Y_L zur Zeit der Gewichtungs-Aktualisierung erhöht wird.
Indem eine solche Vorgabe durchgeführt wird, beispielsweise in einer Situation, in dem ein Mautstellen-Tor durchfahren wird, wenn die Sollposition ein Tor zum Durchfahren ist, in einer Situation, in der das Passieren in einer Richtung senkrecht zum Tor notwendig ist, kann - indem eine größere Wichtigkeit der Differenz von der Sollzustandsgröße, die sich auf die Position in der Seitwärts-Richtung bezieht, als die Differenz von der Sollzustandsgröße gelegt wird, die sich auf die Position in der Richtung senkrecht dazu bezieht - die Position in der Seitwärts-Richtung angepasst werden, bevor die Position in der Richtung senkrecht dazu angepasst wird, und ein Pfad, der ein frühes Erreichen der Position zum Erlauben des Einfahrens in der Richtung senkrecht zum Tor erlaubt, kann erzeugt werden.
In 28 gibt es außerdem eine große Abweichung zwischen dem Azimut Θ_e des beweglichen Körpers 1 und dem Sollazimut Θ_t in der Sollzustandsgröße TG, und in einem Bereich IA2, in dem es wichtig ist, die Abweichung vom Sollazimut Θ_t zu verringern, wird die Bewertungs-Gewichtung so vorgegeben, dass die Wahrscheinlichkeit der Partikel mit einer kleinen Abweichung vom Sollazimut Θ_t zur Zeit der Gewichtungs-Aktualisierung erhöht wird. Hinsichtlich des Vorgebens der Bewertungs-Gewichtung wird ein größerer Gewichtungskoeffizient vorgegeben, wenn der Partikel eine kleinere Abweichung hat.
Indem eine solche Vorgabe erfolgt, gibt es beispielsweise in einer Situation, in der ein Mautstellen-Tor durchfahren wird, wenn die Sollposition ein zu durchfahrendes Tor ist, indem eine niedrige Bewertungs-Gewichtung der Zustandsgröße vorgegeben wird, die sich auf den Azimut an einem Punkt mit einem weiten Abstand von der Sollposition bezieht, und indem eine große Bewertungs-Gewichtung der Zustandsgröße vorgegeben wird, die sich auf den Azimut an einem Punkt mit einem nahen Abstand davon bezieht, einen hohen Freiheitsgrad der Zustandsgröße, die sich auf den Azimut eines Pfads an dem Punkt bezieht, der weit von der Sollposition entfernt ist, und ein solcher Pfad zum Annehmen der Zustandsgröße, die sich auf den Azimut bezieht, wenn sich an die Sollposition angenähert wird, kann erzeugt werden.
Hier nimmt die Beschreibung erneut auf das Ablaufdiagramm in 6 Bezug. Nachdem die Gewichtung jedes Partikels in Schritt S 107 aktualisiert wurde, wird in der Bewertungseinheit 240 für einen vorhergesagten Zustand ein erneutes Abtasten bzw. Resampling des Partikels durchgeführt, und zwar auf der Basis der Gewichtung jedes Partikels (Schritt S 108). Um eine signifikante Verringerung der Anzahl von Partikeln zu verhindern, wird eine erneute Abtastung nur dann vorgenommen, wenn die effektive Anzahl N_eff von Partikeln gleich oder mehr als ein Schwellenwert Nthr ist, und anderenfalls wird keine Verarbeitung in diesem Schritt durchgeführt.
Hier wird die effektive Anzahl N_eff von Partikeln durch den folgenden Ausdruck (27) ausgedrückt.
[Ausdruck 27] $N_{e f f} = \frac{1}{\sum_{n=1}^{N p} {(W_{n})}^{2}}$
Beim Resampling wird - ähnlich wie bei regulären Partikel-Filtern - die Abtastung mit gleichförmigen Intervallen aus einer empirischen Verteilungsfunktion durchgeführt. Wenn ein Resampling durchgeführt wird, werden auf der Basis des folgenden Ausdrucks (28) die Gewichtungen zurückgesetzt, und zwar auf der Basis der Annahme, dass die Gewichtungen der Partikel gleich sind.
[Ausdruck 28] $W = 1 / N p$
Nachdem das Resampling der Partikel in Schritt S108 durchgeführt wurde, wird in der Pfadpunkt-Berechnungseinheit 250 hinsichtlich der Position und der Geschwindigkeit der Partikel ein gewichteter Durchschnittswert in der Pfad-Erzeugungseinheit 260 berechnet, und zwar auf der Basis der Gewichtungen, die in der Bewertungseinheit 240 für einen vorhergesagten Zustand berechnet werden, und die Punkte, die dem gewichteten Durchschnitt unterzogen sind, inklusive zumindest Positionsdaten und Geschwindigkeitsdaten, werden als Pfadpunkte beispielsweise in einem Speicher (nicht dargestellt) in der Pfad-Erzeugungseinheit 260 gespeichert (Schritt S109).
Hinsichtlich der Pfadpunkte können ferner auf der Basis der Gewichtungen, die in der Bewertungseinheit 240 für einen vorhergesagten Zustand berechnet werden, die Partikel mit der größten Gewichtung, d. h. dem größten Gewichtungskoeffizienten, als die Pfadpunkte verwendet werden.
Ferner können die Pfadpunkte nicht nur die Positionsdaten und die Geschwindigkeitsdaten enthalten, sondern auch Azimutdaten, Lenkwinkeldaten und dergleichen.
Ferner können die Pfadpunkte auch nur die Punkte der Positionsdaten als die Pfadpunkte verwenden.
Nachdem die Pfadpunkte in Schritt S109 gespeichert sind, wird bestimmt, ob die Zeit T einen Planungshorizont Tthr erreicht hat (Schritt S110). Wenn die Zeit kleiner als der Planungshorizont ist (im Fall „Nein“), wird die Verarbeitung von Schritt S104 und der nachfolgenden Schritte wiederholt. Wenn die Zeit T gleich oder mehr als der Planungshorizont Tthr ist (im Fall „Ja“), wird eine Punktsequenz der Pfadpunkte, die die Positionsdaten und die Geschwindigkeitsdaten an jeweiligen diskreten Zeitpunkten enthalten, die als die Pfadpunkte in der Pfad-Erzeugungseinheit 260 gespeichert sind, an die Bewegungssteuerung 300 als ein erzeugter Pfad ausgegeben.
29 veranschaulicht ein konzeptuelles Diagramm der Verarbeitung zum Erhalten einer Mehrzahl von Pfadpunkten durch Wiederholen der Berechnung von Schritt S104 bis Schritt S110, bis die Zeit gleich oder mehr als der Planungshorizont Tthr ist. In 29 beträgt die Anzahl von Partikeln vier, und die Partikel in der Zeichnung stellen einen Zustand nach dem Resampling dar.
Wie in 29 veranschaulicht, gibt es innerhalb des befahrbaren Bereichs TA eine Partikelgruppe G1 zur Zeit T = T₁, eine Partikelgruppe G2 zur Zeit T = T₂ und eine Partikelgruppe G3 zur Zeit T = T₃, und die Zeit T = T₃ überschreitet den Planungshorizont Tthr. Die Zustandsgrößen, die dem gewichteten Durchschnitt vierer Partikel jeweils in den Partikelgruppen G1 bis G3 unterzogen sind, sind die Pfadpunkte TP1, TP2 und TP3. Eine Punktsequenz dieser Pfadpunkte wird ein erzeugter Pfad, und die Pfadpunkte mit hoher Wahrscheinlichkeit auf der Basis der Bewertung der Bewertungseinheit 240 für einen vorhergesagten Zustand können erzeugt werden.
Indem die Partikel mit der größten Gewichtung verwendet werden, d. h. dem größten Gewichtungskoeffizient, können in jeder der Partikelgruppen G1 bis G3 als die Pfadpunkte Wirkungen ähnlich den obigen erzielt werden.
Ein Beispiel für die Pfaderzeugung bis zum Erreichen der Sollzustandsgröße unter Verwendung der Pfad-Planungsvorrichtung 200 der vorliegenden Ausführungsform, die oben beschrieben ist, wird unter Bezugnahme auf 30 und 31 beschrieben. 30 und 31 sind schematische Diagramme, die ein Beispiel der Pfaderzeugung veranschaulichen. Die linke Zeichnung in 30 veranschaulicht schematisch einen erzeugten Pfad GT1, wenn der bewegliche Körper 1 an einer Position weit von der Sollzustandsgröße TG entfernt vorhanden ist. Die rechte Zeichnung in 30 veranschaulicht einen erzeugten Pfad GT2 an einer Position, die von dem Fall der linken Zeichnung leicht vorwärts verschoben ist, auf ein Vorwärtsfahren entlang des erzeugten Pfads GT1 hin, unter Verwendung des erzeugten Pfads GT1 der linken Zeichnung. Die linke Zeichnung in 31 veranschaulicht einen erzeugten Pfad GT3 an einer Position, die von dem Fall der rechten Zeichnung in 30 leicht vorwärts verschoben ist, auf ein Vorwärtsfahren entlang des erzeugten Pfads GT2 hin, unter Verwendung des erzeugten Pfads GT2 der rechten Zeichnung in 30. Die rechte Zeichnung in 31 veranschaulicht einen erzeugten Pfad GT4 nach Erreichen der Sollzustandsgröße TG auf das Vorwärtsfahren entlang des erzeugten Pfads GT3 hin, unter Verwendung des erzeugten Pfads GT3 der linken Zeichnung.
Gemäß der Pfad-Planungsvorrichtung 200 der ersten Ausführungsform, wie oben beschrieben, wird ein Pfad bis zur Sollzustandsgröße auf der Basis der Sollzustandsgröße bewertet, die die Zustandsgröße der Position des beweglichen Körpers 1 und den befahrbaren Bereich enthält, und der Pfad wird auf der Basis der Bewertungsergebnisse erzeugt. Daher kann selbst dann, wenn der befahrbare Bereich kompliziert ist, das Ziel durch den befahrbaren Bereich erreicht werden.
Zweite Ausführungsform
32 ist ein Blockdiagramm, das ein Beispiel einer schematischen Konfiguration des beweglichen Körpers 1 veranschaulicht, der mit einer Pfad-Planungsvorrichtung gemäß einer zweiten Ausführungsform ausgestattet ist. Es sei angemerkt, dass in 32 die gleichen Konfigurationen wie jene des beweglichen Körpers 1, beschrieben unter Bezugnahme auf 1, mit den gleichen Bezugszeichen versehen sind, und die überlappende Beschreibung wird weggelassen.
In dem beweglichen Körper 1, der in 32 veranschaulicht ist, ist die Konfiguration einer Pfad-Planungsvorrichtung 200A, die einen Pfad erzeugt, auf dem vom beweglichen Körper 1 gefahren werden soll, von der Pfad-Planungsvorrichtung 200 der ersten Ausführungsform verschieden. Mit anderen Worten: Die Pfad-Planungsvorrichtung 200A der zweiten Ausführungsform erzeugt einen Pfad, indem sie Polynome erzeugt, die jeweils durch die gegenwärtige Position und die Sollposition des beweglichen Körpers 1 in der Zustands-Vorhersageeinheit 230 gehen, anstatt Partikel-Filter für die Zustandsschätzung zu verwenden, und sie unterscheidet sich folglich von der Pfad-Planungsvorrichtung 200 darin, dass die Pfad-Planungsvorrichtung 200A den Pfadpunkt-Berechnungseinheit 250 nicht aufweist.
Als ein Verfahren zum Berechnen von Polynompfaden, die den beweglichen Körper 1 und die Sollzustandsgröße TG verbinden, erfolgt die folgende Beschreibung auf der Basis der Annahme, dass die Sollzustandsgröße TG eine Sollposition (x_g, y_g) ist. 33 ist ein konzeptuelles Diagramm, das ein Verfahren zum Herleiten eines Polynoms veranschaulicht, das den beweglichen Körper 1 und die Sollposition TG verbindet.
Wie in 33 gilt Folgendes: Wenn die Position des beweglichen Körpers 1 der Ursprung ist, die Richtung des beweglichen Körpers 1 die x-Achse ist und die Richtung senkrecht zu der Richtung des beweglichen Körpers die y-Achse ist, wird ein Polynom y = f(x), das den beweglichen Körper 1 und die Sollposition TG verbindet, durch den folgenden Ausdruck (29) ausgedrückt.
[Ausdruck 29] $y = f (x) = \sum G_{i} x^{i}$
Ferner wird das Polynom der Nebenbedingung unterworfen, dass das Polynom durch die Position (Ursprung) und die Sollposition TG des beweglichen Körpers 1 gegen muss und die Richtung der x-Achse (der Winkel beträgt 0°) am Positionspunkt des beweglichen Körpers 1 zugewandt sein muss oder einer vorbestimmten Richtung (Winkel) am Sollpositionspunkt zugewandt sein muss.
Indem das Obige mit einem mathematischen Ausdruck ausgedrückt wird, kann durch Lösen der folgenden Randbedingung, d. h. simultane Gleichungen der Bedingungen eines beweglichen Körperpositionpunkts und eines Sollpositionpunkts, jeder Koeffizient c_i berechnet werden. Die simultanen Gleichungen werden durch die folgenden Ausdrücke (30) und (31) ausgedrückt. Hier ist x₀ der x-Koordinatenwert der Position des beweglichen Körpers, und x_g is der x-Koordinatenwert der Sollposition.
[Ausdruck 30] $f (x_{o}) = y_{0}, f (x_{g}) = y_{g}$

[Ausdruck 31] $f' (x_{o}) = y'_{o}, f' (x_{g}) = y'_{g}$
Als nächstes wird der Fall beschrieben, in dem das Polynom durch den folgenden Ausdruck (32) ausgedrückt wird.
[Ausdruck 32] $f (x) = C_{3} x^{3} + C_{2} x^{2} + C_{1} x + C_{0}$
Beispielsweise bei einer gegebenen Randbedingung, dass der Funktionswert (f(x₀)) am Positionspunkt des beweglichen Körpers ein y-Koordinatenwert des beweglichen Körpers 1 ist, der Funktionswert (f'(x₀)), der die Steigung am Positionspunkt des beweglichen Körpers ausdrückt, der Azimut des beweglichen Körpers 1 ist und der Funktionswert (f''(x₀)), der die Krümmung am Positionspunkt des beweglichen Körpers angibt, 0 ist, werden die Koeffizienten C₀, C₁ und C₂ eindeutig berechnet, und indem der verbleibende Term von x³ zufällig vorgegeben wird, können so viele Polynompfade wie die Anzahl solcher zufälligen Terme erzeugt werden, wie in 34 veranschaulicht.
34 veranschaulicht, wenn es eine Mehrzahl von Hindernissen OB gibt, drei Muster von Polynompfaden PT1, PT2 und PT3 zum Erhalten der Polynompfade, die den beweglichen Körper 1 und die Sollposition TG verbinden, unter Vermeidung der Mehrzahl von Hindernissen OB.
Die Polynome zum Vorgeben der Polynompfade PT1, PT2 und PT3 werden jeweils ausgedrückt durch die folgenden Ausdrücke (33), (34) und (35).
[Ausdruck 33] $f (x) = C'_{3} x^{3} + C_{2} x^{2} + C_{1} x + C_{0}$

[Ausdruck 34] $f (x) = C''_{3} x^{3} + C_{2} x^{2} + C_{1} x + C_{0}$

[Ausdruck 35] $f (x) = C'''_{3} x^{3} + C_{2} x^{2} + C_{1} x + C_{0}$
Aus der Mehrzahl von Polynompfaden wird eine Bewertung durchgeführt, wie z. B. ein Erreichungswert zur Sollposition TG und ob der Polynom Pfad weg von der Grenze des befahrbaren Bereichs TA und innerhalb des befahrbaren Bereichs TA liegt, und der Polynompfad mit der höchsten Bewertung wird als der erzeugte Pfad verwendet. In dem Beispiel in 34 ist der Polynompfad PT3 der erzeugte Pfad.
Ferner können in einem Fall, in dem das Polynom durch den obigen Ausdruck (32) ausgedrückt wird, die Polynompfade auch mit dem folgenden Verfahren erhalten werden.
Beispielsweise bei einer gegebenen Randbedingung, dass der Funktionswert am Positionspunkt des beweglichen Körpers ein y-Koordinatenwert des beweglichen Körpers 1 ist, der Funktionswert der die Steigung am Positionspunkt des beweglichen Körpers darstellt, der Azimut des beweglichen Körpers 1 ist, der Funktionswert (f(x_g)) am Positionspunkt des beweglichen Körpers ein y-Koordinatenwert der Sollposition TG ist und der Funktionswert (f'(x_g)) am Positionspunkt des beweglichen Körpers der Azimut der Sollposition TG ist, können die Koeffizienten C₃, C₂, C₁ und C₀ eindeutig berechnet werden. Indem die jeweiligen Koeffizienten an zufälligen Umgebungswerten der berechneten Koeffizienten C₃, C₂, C₁ und C₀ geändert werden, kann eine Mehrzahl von Polynompfaden erzeugt werden, wie in 35.
35 veranschaulicht, wenn es eine Mehrzahl von Hindernissen OB gibt, drei Muster von Polynompfaden PT1, PT2 und PT3 zum Erhalten der Polynompfade, die den beweglichen Körper 1 und die Sollposition TG verbinden, unter Vermeidung der Mehrzahl von Hindernissen OB.
Die Polynome zum Vorgeben der Polynompfade PT1, PT2 und PT3 werden jeweils ausgedrückt durch die folgenden Ausdrücke (36), (37) und (38).
[Ausdruck 36] $f (x) = C'_{3} x^{3} + C'_{2} x^{2} + C'_{1} x + C'_{0}$

[Ausdruck 37] $f (x) = C''_{3} x^{3} + C''_{2} x^{2} + C''_{1} x + C''_{0}$

[Ausdruck 38] $f (x) = C'''_{3} x^{3} + C'''_{2} x^{2} + C'''_{1} x + C'''_{0}$
Aus der Mehrzahl von Polynompfaden wird eine Bewertung durchgeführt, wie z. B. ein Erreichungswert zur Sollposition TG und ob der Polynom Pfad weg von der Grenze des befahrbaren Bereichs TA und innerhalb des befahrbaren Bereichs TA liegt, und der Polynompfad mit der höchsten Bewertung wird als der erzeugte Pfad verwendet. In dem Beispiel in 35 ist der Polynompfad PT1 der erzeugte Pfad.
Wie oben beschrieben, erhält die Pfad-Planungsvorrichtung 200A der zweiten Ausführungsform den erzeugten Pfad, indem sie Polynome berechnet, die jeweils durch die gegenwärtige Position und die Sollposition des beweglichen Körpers 1 gehen, anstatt Partikel-Filter zur Zustandsschätzung zu verwenden, und folglich berechnet sie nicht die Pfadpunkte und weist nicht die Pfadpunkt-Berechnungseinheit 250 auf.
Hier ist ein Vorteil eines Falls, in dem das Verfahren zum Erhalten des erzeugten Pfades durch Berechnen von Polynomen verwendet wird, dass es eine niedrige Rechenlast gibt. Mit anderen Worten: Indem bloß die oben beschriebenen simultanen Gleichungen gelöst werden, kann ein Kandidatenpfad berechnet werden. Wenn jedoch Partikel-Filter verwendet werden, muss die Berechnung für die Länge des Pfades wiederholt werden, in dem Zustandsübergänge einer großen Anzahl von Partikeln erzeugt werden sollen, und folglich gibt es eine große Rechenlast.
Im Gegensatz dazu ist ein Vorteil des Falls, in dem das Verfahren zum Erhalten des erzeugten Pfades unter Verwendung von Partikel-Filtern verwendet wird, dass ein Pfad erzeugt werden kann, von dem gewährleistet ist, dass er innerhalb des befahrbaren Bereichs TA liegt. Mit anderen Worten: Die Partikel-Filter sind ein Verfahren zum Berechnen eines Pfadpunkts, indem eine große Anzahl von Vorhersagepunkten (vorhergesagte Zustandsgrößen) verwendet werden, und aus der Positionsrelation zwischen dem Punkt jedes Partikels und des befahrbaren Bereichs TA kann eine Bewertung durchgeführt werden, ob der Pfadpunkt außerhalb oder innerhalb des befahrbaren Bereichs TA liegt. Daher kann ein Pfad erzeugt werden, von dem gewährleistet ist, dass er innerhalb des befahrbaren Bereichs TA liegt. Im Gegensatz dazu kann in dem Verfahren zum Erhalten des erzeugten Pfades durch Berechnen von Polynomen ein Pfad erzeugt werden, der den beweglichen Körper 1 und die Sollposition TG verbindet, aber „innerhalb des befahrbaren Bereichs TA befindlich“ und „Vermeiden eines Hindernisses“ kann nicht in einem mathematischen Ausdruck der Polynome aufgenommen sein oder kann nicht berücksichtigt werden, und folglich kann der erzeugte Pfad außerhalb des befahrbaren Bereichs TA liegen.
Außerdem ist ein Vorteil des Falls, in dem das Verfahren zum Erhalten des erzeugten Pfads unter Verwendung von Partikel-Filtern verwendet wird, dass der Suchbereich des Pfads breit ist. Mit anderen Worten: In dem Verfahren zum Erhalten des erzeugten Pfads durch Berechnen von Polynomen kann nur ein Pfad erhalten werden, der durch ein Polynom ausgedrückt werden kann, und folglich ist der Suchbereich klein. Wenn jedoch Partikel-Filter verwendet werden, werden Vorhersagepunkte des Zustands als eine Basis verwendet, d. h. Punkte werden für den Ausdruck verwendet, und folglich ist der Suchbereich breit. Außerdem kann auch ein nichtlinearer Pfad erhalten werden, und ein Pfad, der nicht durch ein Polynom ausgedrückt werden kann, kann ebenfalls erzeugt werden.
Modifikation der Gewichtung
Die Bewertungseinheit 240 für einen vorhergesagten Zustand der ersten Ausführungsform, die oben beschrieben ist, ordnet jeder vorhergesagten Zustandsgröße eine Gewichtung zu, d. h. jedem Partikel, und erhält gewichtete Pfadkandidaten. Wenn die vorhergesagte Zustandsgröße stark von der gegenwärtigen Zustandsgröße des beweglichen Körpers 1 abweicht oder stark von der Zustandsgröße des vorher berechneten Pfadpunkts abweicht und ein Pfad unter Verwendung des Pfadpunkts erzeugt wird, der auf der Basis einer solchen vorhergesagten Zustandsgröße berechnet wird, kann sich in diesem Fall der Pfad plötzlich ändern und veranlassen, dass sich das Fahren des beweglichen Körpers 1 unbehaglich anfühlt.
In Anbetracht dessen gilt in der Bewertungseinheit 240 für einen vorhergesagten Zustand Folgendes: Wenn die vorhergesagte Zustandsgröße stark von der gegenwärtigen Zustandsgröße des beweglichen Körpers 1 abweicht und stark von der Zustandsgröße des vorher berechneten Pfadpunkts abweicht, kann der Gewichtungskoeffizient verringert werden, so dass verhindert werden kann, dass solch ein sich plötzlich ändernder Pfad erzeugt wird, und das Fahren des beweglichen Körpers 1 kann so verbessert werden, dass es behaglich ist.
36 veranschaulicht ein konzeptuelles Diagramm zum Gewichten, wenn die vorhergesagte Zustandsgröße stark von der gegenwärtigen Zustandsgröße des beweglichen Körpers 1 abweicht. Wie in 36 veranschaulicht, gilt Folgendes: Wenn es eine Partikelgruppe GS gibt, die stark von der gegenwärtigen Zustandsgröße (x_e, y_e, θ_e, v_e) des beweglichen Körpers 1 abweicht, und eine Partikelgruppe GM, die nicht so stark von der gegenwärtigen Zustandsgröße des beweglichen Körpers 1 abweicht, dann verringert die Bewertungseinheit 240 für einen vorhergesagten Zustand den Gewichtungskoeffizienten der Partikel der Partikelgruppe GS. Im Gegensatz dazu verringert die Bewertungseinheit 240 für einen vorhergesagten Zustand den Gewichtungskoeffizienten der Partikel der Partikelgruppe GM nicht.
Hinsichtlich des Abweichungsgrads zwischen der gegenwärtigen Zustandsgröße und der vorhergesagten Zustandsgröße (Partikel) des beweglichen Körpers 1 kann hier die Differenz zwischen beiden Zustandsgrößen auf der Basis eines Schwellenwerts bestimmt werden, der im Voraus bestimmt wird. Wenn die Differenz größer als der Schwellenwert ist, kann bestimmt werden, dass die Abweichung groß ist, wohingegen dann, wenn die Differenz gleich groß wie oder kleiner als der Schwellenwert ist, bestimmt werden kann, dass die Abweichung klein ist.
Hinsichtlich des Gewichtens für den Partikel können ferner die Gewichtungskoeffizienten auf der Basis des Absolutwerts der Differenz zwischen der gegenwärtigen Zustandsgröße und der vorhergesagten Zustandsgröße (Partikel) des beweglichen Körpers 1 geändert werden. Beispielsweise können solche Gewichtungskoeffizienten, die sich allmählich ändern, im Voraus auf der Basis des Absolutwerts der Differenz zwischen beiden Zustandsgrößen vorgegeben werden. Wenn die Differenz zwischen beiden Zustandsgrößen größer wird, können die Gewichtungskoeffizienten allmählich verringert werden, wohingegen dann, wenn die Differenz zwischen beiden Zustandsgrößen kleiner wird, die Gewichtungskoeffizienten allmählich erhöht werden können.
37 veranschaulicht ein konzeptuelles Diagramm zum Gewichten, wenn die vorhergesagte Zustandsgröße stark von der Zustandsgröße des vorher berechneten Pfadpunkts abweicht. Wie in 37 veranschaulicht, gilt Folgendes: Wenn es eine Partikelgruppe GS gibt, die stark von der Zustandsgröße des Pfadpunkts TP2 abweicht, wie vorher berechnet, und eine Partikelgruppe GM, die nicht so stark von der Zustandsgröße des Pfadpunkts TP2 abweicht, wie vorher berechnet, verringert die Bewertungseinheit 240 für einen vorhergesagten Zustand den Gewichtungskoeffizienten der Partikel der Partikelgruppe GS. Im Gegensatz dazu verringert die Bewertungseinheit 240 für einen vorhergesagten Zustand den Gewichtungskoeffizienten der Partikel der Partikelgruppe GM nicht.
Hinsichtlich des Abweichungsgrads zwischen der Zustandsgröße des Pfadpunkts TP2, wie vorher berechnet, und der vorhergesagten Zustandsgröße (Partikel) kann die Differenz zwischen beiden Zustandsgrößen auf der Basis eines im Voraus bestimmten Schwellenwerts bestimmt werden. Wenn die Differenz größer als der Schwellenwert ist, kann bestimmt werden, dass die Abweichung groß ist, wohingegen dann, wenn die Differenz gleich groß wie oder kleiner als der Schwellenwert ist, bestimmt werden kann, dass die Abweichung klein ist.
Hinsichtlich des Gewichtens für die Partikel können die Gewichtungskoeffizienten auf der Basis des Absolutwerts der Differenz zwischen der Zustandsgröße des Pfadpunkts TP2, wie vorher berechnet, und der vorhergesagten Zustandsgröße (Partikel) geändert werden. Beispielsweise können solche Gewichtungskoeffizienten, die sich allmählich ändern, im Voraus auf der Basis des Absolutwerts der Differenz zwischen beiden Zustandsgrößen vorgegeben werden. Wenn die Differenz zwischen beiden Zustandsgrößen größer wird, können die Gewichtungskoeffizienten allmählich verringert werden, wohingegen dann, wenn die Differenz zwischen beiden Zustandsgrößen kleiner wird, die Gewichtungskoeffizienten allmählich erhöht werden können.
Es sei angemerkt, dass jedes Bestandteilselement der Pfad-Planungsvorrichtungen 200 und 200A gemäß den oben beschriebenen ersten und zweiten Ausführungsformen unter Verwendung eines Computers konfiguriert werden kann und implementiert wird, wenn der Computer ein Programm ausführt. Mit anderen Worten: Die Pfad-Planungsvorrichtungen 200 und 200A werden beispielsweise von einer Verarbeitungsschaltung 50 implementiert, die in 38 veranschaulicht ist. Ein Prozessor wie z. B. eine zentrale Verarbeitungseinheit (CPU) oder ein digitaler Signalprozessor (DSP) wird auf die Verarbeitungsschaltung 50, angewendet, und die Funktion jeder Einheit wird implementiert, wenn das in der Speichereinrichtung gespeicherte Programm ausgeführt wird.
Es sei angemerkt, dass dedizierte Hardware auf die Verarbeitungsschaltung 50 angewendet werden kann. Wenn die Verarbeitungsschaltung 50 die dedizierte Hardware ist, ist die Verarbeitungsschaltung 50 beispielsweise eine Einzelschaltung, eine zusammengesetzte Schaltung, ein programmierter Prozessor, eine parallel programmierter Prozessor, eine anwendungsspezifische integrierte Schaltung (ASIC), ein feldprogrammierbares Gate-Array (FPGA), einer Kombination daraus, oder dergleichen.
In den Pfad-Planungsvorrichtungen 200 und 200A können Funktionen der jeweiligen Bestandteilselemente durch einzelne Verarbeitungsschaltungen implementiert werden, oder diese Funktionen können kollektiv durch eine einzelne Verarbeitungsschaltung implementiert werden.
Außerdem veranschaulicht 39 eine Hardwarekonfiguration, wenn die Verarbeitungsschaltung 50 unter Verwendung eines Prozessors konfiguriert ist. In diesem Fall wird eine Funktion jeder Einheit der Pfad-Planungsvorrichtungen 200 und 200A durch eine Kombination mit Software oder dergleichen (Software, Firmware oder Software und Firmware) implementiert. Die Software und dergleichen ist als ein Programm beschrieben und in einem Speicher 52 gespeichert. Ein Prozessor 51, der als die Verarbeitungsschaltung 50 fungiert, liest das Programm, das im Speicher 52 (Speichervorrichtung) gespeichert ist, aus und führt es aus, und er implementiert dadurch die Funktion jeder Einheit. Mit anderen Worten: Es kann gesagt werden, dass das Programm den Computer veranlasst, Prozeduren und Verfahren der Bestandteilselemente der Pfad-Planungsvorrichtungen 200 und 200A auszuführen.
Hier kann der Speicher 52 beispielsweise ein nichtflüchtiger oder flüchtiger Halbleiterspeicher wie z. B. ein RAM, ein ROM, ein Flash-Speicher, ein löschbarer programmierbarer Nur-Lese-Speicher (EPROM), ein elektrisch löschbarer programmierbarer Nur-Lese-Speicher (EEPROM), ein Festplattenlaufwerk (HDD), eine magnetische Scheibe, eine flexible Scheibe, eine optische Scheibe, eine Compact Disc, eine MiniDisc, eine Digital Versatile Disc (DVD) und eine Laufwerkvorrichtung oder dergleichen dafür sein, oder irgendein Speichermedium, das in der Zukunft verwendet wird.
Eine Konfiguration, bei der die Funktionen der Bestandteilselemente der Pfad-Planungsvorrichtungen 200 und 200A durch eine von Hardware und Software erzielt werden, ist oben beschrieben. Dies ist jedoch nicht einschränkend, und ein Teil der Bestandteilselemente der Pfad-Planungsvorrichtungen 200 und 200A kann durch dedizierte Hardware implementiert werden, und ein anderer Teil der Bestandteilselemente kann durch Software oder dergleichen implementiert werden. Beispielsweise können Funktionen eines Teils der Bestandteilselemente durch die Verarbeitungsschaltung 50 als dedizierte Hardware implementiert werden, und Funktionen eines anderen Teils der Bestandteilselemente können durch die Verarbeitungsschaltung 50 als der Prozessor 51 implementiert werden, der das im Speicher 52 gespeicherte Programm ausliest und ausführt.
Wie oben beschrieben, können die Pfad-Planungsvorrichtungen 200 und 200A jede der oben beschriebenen Funktionen implementieren, und zwar unter Verwendung von Hardware, Software oder dergleichen, oder einer Kombination aus diesen.
Während die vorliegende Offenbarung detailliert beschrieben wurde, ist die obige Beschreibung in jeglicher Hinsicht anschaulich und nicht einschränkend. Es versteht sich daher, dass zahlreiche nicht dargestellte Modifikationen ersonnen werden können, ohne vom Umfang der vorliegenden Offenbarung abzuweichen.
Es sei angemerkt, dass in der vorliegenden Offenbarung jede der Ausführungsformen frei kombiniert werden kann und jede der Ausführungsformen passend modifiziert oder weggelassen werden kann, und zwar innerhalb des Umfangs der Offenbarung.
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

JP 2012145998 A [0003]

Claims

Pfad-Planungsvorrichtung, die zum Planen eines Pfads eines beweglichen Körpers konfiguriert ist, wobei die Pfad-Planungsvorrichtung Folgendes aufweist: eine Berechnungseinheit für einen befahrbaren Bereich, die konfiguriert ist zum Berechnen eines befahrbaren Bereichs des beweglichen Körpers auf der Basis einer Umgebungsinformation des beweglichen Körpers; eine Sollzustand-Berechnungseinheit, die konfiguriert ist zum Berechnen einer Sollzustandsgröße inklusive zumindest einer Sollposition des beweglichen Körpers; eine Zustands-Vorhersageeinheit, die konfiguriert ist zum Vorhersagen zumindest einer gegenwärtigen Zustandsgröße des beweglichen Körpers und einer Zustandsgröße des beweglichen Körpers an einer oder mehreren Positionen zwischen einer gegenwärtigen Position und der Sollposition des beweglichen Körpers, so dass sie dadurch einen oder mehrere Pfadkandidaten erzeugt; eine Bewertungseinheit für einen vorhergesagten Zustand, die konfiguriert ist zum Bewerten des einen oder der mehreren Pfadkandidaten auf der Basis der Sollzustandsgröße und des befahrbaren Bereichs, und zum Ausgeben von Bewertungsergebnissen; und eine Pfad-Erzeugungseinheit, die konfiguriert ist zum Erzeugen des Pfads aus dem einen oder den mehreren Pfadkandidaten auf der Basis der Bewertungsergebnisse, und zum Ausgeben des Pfads an eine Bewegungssteuerung, die konfiguriert ist zum Steuern des beweglichen Körpers auf der Basis des Pfads.
Pfad-Planungsvorrichtung nach Anspruch 1, die ferner Folgendes aufweist: eine Pfadpunkt-Berechnungseinheit, die zum Berechnen eines Pfadpunkts aus dem einen oder den mehreren Pfadkandidaten auf der Basis der Bewertungsergebnisse konfiguriert ist, wobei die Pfad-Erzeugungseinheit eine Punktsequenz, die den Pfadpunkt enthält, als den Pfad erzeugt.
Pfad-Planungsvorrichtung nach Anspruch 1, wobei die Berechnungseinheit für einen befahrbaren Bereich einen zukünftig befahrbaren Bereich eines gegenwärtig befahrbaren Bereichs auf der Basis einer Zeitreihen-Variation einer Form eines vergangenen befahrbaren Bereichs vorhersagt, der früher als die gegenwärtige Zeit berechnet wurde, den zukünftigen befahrbaren Bereich als einen vorhergesagten befahrbaren Bereich verwendet, den gegenwärtig befahrbaren Bereich und den vorhergesagten befahrbaren Bereich miteinander kombiniert und dadurch den befahrbaren Bereich erweitert.
Pfad-Planungsvorrichtung nach Anspruch 1, wobei die Berechnungseinheit für einen befahrbaren Bereich einen zukünftig befahrbaren Bereich eines gegenwärtig befahrbaren Bereichs auf der Basis eines Typs eines Hindernisses außerhalb eines vergangenen befahrbaren Bereichs vorhersagt, der früher als die gegenwärtige Zeit berechnet wurde, den zukünftigen befahrbaren Bereich als einen vorhergesagten befahrbaren Bereich verwendet, den gegenwärtig befahrbaren Bereich und den vorhergesagten befahrbaren Bereich miteinander kombiniert und dadurch den befahrbaren Bereich erweitert.
Pfad-Planungsvorrichtung nach Anspruch 4, wobei wenn das Hindernis außerhalb des vergangenen befahrbaren Bereichs ein ortsfestes Hindernis ist, die Berechnungseinheit für einen befahrbaren Bereich bestimmt, dass ein Bereich vor dem ortsfesten Hindernis selbst in der Zukunft ein nicht befahrbarer Bereich verbleibt, und wenn das Hindernis außerhalb des vergangenen befahrbaren Bereich ein bewegliches Hindernis ist, in der Zukunft die Berechnungseinheit für einen befahrbaren Bereich den befahrbaren Bereich auf der Basis der Bewegungsrichtung und der Bewegungsgeschwindigkeit des beweglichen Hindernisses erweitert und den befahrbaren Bereich auf der Basis der Geschwindigkeit des beweglichen Körpers in einem Bereich ohne das Hindernis außerhalb des vergangenen befahrbaren Bereichs erweitert.
Pfad-Planungsvorrichtung nach Anspruch 1, wobei die Berechnungseinheit für einen befahrbaren Bereich einen Bereich vorhersagt, in dem der bewegliche Körper in der Zukunft nicht fahren kann, und zwar auf der Basis der Umgebungsinformation, und den Bereich ausschließt, so dass sie den befahrbaren Bereich erhält.
Pfad-Planungsvorrichtung nach Anspruch 1, wobei die Sollzustand-Berechnungseinheit eine Sollgeschwindigkeit des beweglichen Körpers in der Sollzustandsgröße einschließt.
Pfad-Planungsvorrichtung nach Anspruch 7, wobei die Sollzustand-Berechnungseinheit einen Sollazimut des beweglichen Körpers in der Sollzustandsgröße einschließt.
Pfad-Planungsvorrichtung nach Anspruch 8, wobei die Sollzustand-Berechnungseinheit eine Soll-Lateralposition in der Sollzustandsgröße einschließt, wobei die Soll-Lateralposition eine Position in der Richtung senkrecht zu der Richtung des Sollazimuts ist.
Pfad-Planungsvorrichtung nach Anspruch 1, wobei wenn die Sollposition außerhalb des befahrbaren Bereichs vorhanden ist, die Sollzustand-Berechnungseinheit die Position, die der Sollposition im befahrbaren Bereich am nächsten ist, als die Sollposition zurücksetzt.
Pfad-Planungsvorrichtung nach Anspruch 7, wobei die Sollzustand-Berechnungseinheit einen oberen Grenzwert auf die Sollgeschwindigkeit in Abhängigkeit der Form des befahrbaren Bereichs vorgibt.
Pfad-Planungsvorrichtung nach Anspruch 1, wobei die Zustands-Vorhersageeinheit einen oberen Grenzwert in Abhängigkeit von der Form des befahrbaren Bereichs auf einen oder mehrere Eingabewerte vorgibt, den einen oder die mehreren Eingabewerte in ein Bewegungsmodell des beweglichen Körpers eingibt und dadurch den einen oder die mehreren Pfadkandidaten berechnet.
Pfad-Planungsvorrichtung nach Anspruch 12, wobei die Zustands-Vorhersageeinheit die Verteilung des einen oder der mehreren Eingabewerte in Abhängigkeit von der Form des befahrbaren Bereichs für den einen oder die mehreren Eingabewerte ändert, den einen oder die mehreren Eingabewerte in das Bewegungsmodell des beweglichen Körpers eingibt und dadurch den einen oder die mehreren Pfadkandidaten berechnet.
Pfad-Planungsvorrichtung nach Anspruch 12 oder 13, wobei die Zustands-Vorhersageeinheit die Anzahl der einen oder mehreren Eingabewerte in Abhängigkeit der Form des befahrbaren Bereichs ändert.
Pfad-Planungsvorrichtung nach Anspruch 1, wobei die Zustands-Vorhersageeinheit die Verteilung der einen oder mehreren Eingabewerte in Abhängigkeit eines Abweichungsgrads zwischen der Sollzustandsgröße und der gegenwärtigen Zustandsgröße des beweglichen Körpers für den einen oder die mehreren Eingabewerte ändert, den einen oder die mehreren Eingabewerte in ein Bewegungsmodell des beweglichen Körpers eingibt und dadurch den einen oder die mehreren Pfadkandidaten berechnet.
Pfad-Planungsvorrichtung nach Anspruch 15, wobei die Zustands-Vorhersageeinheit die Anzahl der einen oder mehreren Eingabewerte in Abhängigkeit des Freiheitsgrads ändert.
Pfad-Planungsvorrichtung nach Anspruch 1, wobei die Bewertungseinheit für einen vorhergesagten Zustand einen Gewichtungskoeffizienten auf eine Abweichung zwischen der Sollzustandsgröße und der gegenwärtigen Zustandsgröße des beweglichen Körpers vorgibt und dadurch den einen oder die mehreren Pfadkandidaten bewertet.
Pfad-Planungsvorrichtung nach Anspruch 8, wobei die Bewertungseinheit für einen vorhergesagten Zustand die Gewichtung auf eine Abweichung zwischen der Sollzustandsgröße und der gegenwärtigen Zustandsgröße des beweglichen Körpers vorgibt, einen Gewichtungskoeffizienten für einen Azimut des beweglichen Körpers in Abhängigkeit einer Abweichung zwischen dem Azimut und dem Sollazimut des beweglichen Körpers ändert und dadurch den einen oder die mehreren Pfadkandidaten bewertet.
Pfad-Planungsvorrichtung nach Anspruch 1, wobei die Bewertungseinheit für einen vorhergesagten Zustand die Gewichtung auf den einen oder die mehreren Pfadkandidaten vorgibt, so dass sie einen oder mehrere gewichtete Pfadkandidaten erhält, und dadurch den einen oder die mehreren Pfadkandidaten bewertet, und zwar auf der Basis einer Gewichtung des einen oder der mehreren gewichteten Pfadkandidaten.
Pfad-Planungsvorrichtung nach Anspruch 19, wobei die Bewertungseinheit für einen vorhergesagten Zustand einen Gewichtungskoeffizienten von 0 auf den einen oder die mehreren Pfadkandidaten vorgibt, der/die außerhalb des befahrbaren Bereichs vorhanden ist/sind und so den einen oder die mehreren gewichteten Pfadkandidaten erhält.
Pfad-Planungsvorrichtung nach Anspruch 19, wobei die Bewertungseinheit für einen vorhergesagten Zustand einen Gewichtungskoeffizienten auf den einen oder die mehreren Pfadkandidaten ändert, der/die außerhalb des befahrbaren Bereichs vorhanden ist/sind, und zwar in Abhängigkeit des Abstands von einer Grenze des befahrbaren Bereichs, so dass sie den einen oder die mehreren gewichteten Pfadkandidaten erhält.
Pfad-Planungsvorrichtung nach Anspruch 19, wobei die Bewertungseinheit für einen vorhergesagten Zustand einen Gewichtungskoeffizienten auf den einen oder die mehreren Pfadkandidaten ändert, der/die innerhalb des befahrbaren Bereichs vorhanden ist/sind, und zwar in Abhängigkeit des Abstands von einer Grenze des befahrbaren Bereichs, so dass sie den einen oder die mehreren gewichteten Pfadkandidaten erhält.
Pfad-Planungsvorrichtung nach Anspruch 19, wobei die Berechnungseinheit für einen befahrbaren Bereich einen Bereich vorhersagt, in dem der bewegliche Körper in der Zukunft nicht fahren kann, und zwar auf der Basis der Umgebungsinformation, und die Bewertungseinheit für einen vorhergesagten Zustand einen Gewichtungskoeffizient so vorgibt, dass er kleiner als ein Wert ist, der im Voraus dem einem oder den mehreren Pfadkandidaten vorgegeben ist, der/die im Bereich vorhanden ist/sind, so dass sie den einen oder die mehreren gewichteten Pfadkandidaten erhält.
Pfad-Planungsvorrichtung nach Anspruch 3 oder 4, wobei die Bewertungseinheit für einen vorhergesagten Zustand die Gewichtung auf den einen oder die mehreren Pfadkandidaten vorgibt, so dass sie den einen oder die mehreren gewichteten Pfadkandidaten erhält, und die Bewertungseinheit für einen vorhergesagten Zustand einen Gewichtungskoeffizienten des einen oder der mehreren Pfadkandidaten innerhalb des vorhergesagten befahrbaren Bereichs so vorgibt, dass er kleiner als der Gewichtungskoeffizient des einen oder der mehreren Pfadkandidaten innerhalb des gegenwärtig befahrbaren Bereichs ist und so den einen oder die mehreren gewichteten Pfadkandidaten erzeugt und dadurch den einen oder die mehreren Pfadkandidaten bewertet, und zwar auf der Basis einer Gewichtung des einen oder der mehreren gewichteten Pfadkandidaten.
Pfad-Planungsvorrichtung nach Anspruch 2, wobei die Bewertungseinheit für einen vorhergesagten Zustand die Gewichtung auf den einen oder die mehreren Pfadkandidaten vorgibt, so dass sie den einen oder die mehreren gewichteten Pfadkandidaten erhält, und wenn der eine oder die mehreren gewichteten Pfadkandidaten an einer Position zum Überqueren einer die Querung verbietenden Linie vorhanden sind, für den vorher berechneten Pfadpunkt die Bewertungseinheit für einen vorhergesagten Zustand einen Gewichtungskoeffizienten des einen oder der mehreren Pfadkandidaten so vorgibt, dass er kleiner als der Gewichtungskoeffizient anderer von dem einen oder der mehreren Pfadkandidaten ist, so dass der eine oder die mehreren gewichteten Pfadkandidaten erzeugt werden, und dadurch den einen oder die mehreren Pfadkandidaten bewertet, und zwar auf der Basis einer Gewichtung des einen oder der mehreren gewichteten Pfadkandidaten.
Pfad-Planungsvorrichtung nach Anspruch 2, wobei die Bewertungseinheit für einen vorhergesagten Zustand die Gewichtung auf den einen oder die mehreren Pfadkandidaten vorgibt, so dass sie den einen oder die mehreren gewichteten Pfadkandidaten erhält, und die Pfadpunkt-Berechnungseinheit einen gewichteten Durchschnittswert für den einen oder die mehreren gewichteten Pfadkandidaten zum Erhalten des Pfadpunkts berechnet.
Pfad-Planungsvorrichtung nach Anspruch 2, wobei die Bewertungseinheit für einen vorhergesagten Zustand die Gewichtung auf den einen oder die mehreren Pfadkandidaten vorgibt, so dass sie den einen oder die mehreren gewichteten Pfadkandidaten erhält, und die Pfadpunkt-Berechnungseinheit den einen oder die mehreren gewichteten Pfadkandidaten mit dem größten Gewichtungskoeffizienten aus dem einen oder den mehreren gewichteten Pfadkandidaten als den Pfadpunkt verwendet.
Pfad-Planungsvorrichtung nach Anspruch 2, wobei die Bewertungseinheit für einen vorhergesagten Zustand die Gewichtung auf den einen oder die mehreren Pfadkandidaten vorgibt, und zwar in Abhängigkeit der Differenz von der Zustandsgröße des Pfadpunkts, wie vorher berechnet, oder der gegenwärtigen Zustandsgröße des beweglichen Körpers, so dass sie den einen oder die mehreren gewichteten Pfadkandidaten erhält.