(57)【特許請求の範囲】
【請求項１】
先導車両から与えられる情報に基づいて後続車両が追尾走行を行う追尾システムであって、
先導車両は、
周辺の環境情報を取得する環境情報取得手段と、
前記環境情報と自己位置及び姿勢とに基づいて、環境マップを作成する環境マップ作成手段と、
前記環境マップを送信する通信手段と
を備え、
前記後続車両は、
前記先導車両によって作成された前記環境マップを受信する通信手段と、
周辺の環境情報を取得する環境情報取得手段と、
前記環境情報取得手段により取得された環境情報と、前記先導車両によって作成された環境マップとを照合することにより、自己の位置及び姿勢を推定する地図照合手段と、
前記地図照合手段によって推定された自己の位置及び姿勢を参照して、最終的な自己の位置及び姿勢を決定する位置姿勢決定手段と
を備える追尾システム。
【請求項２】
前記先導車両は、前記環境情報取得手段によって取得された環境情報の中から、物体の属性に関する所定の条件を満たす物体をランドマークとして抽出するランドマーク抽出手段を更に備え、
前記環境マップ作成手段は、前記ランドマーク抽出手段によって抽出された前記ランドマークの位置と、前記環境情報が取得されたときの自己の位置及び姿勢とに基づいて前記環境マップを作成する請求項１に記載の追尾システム。
【請求項３】
前記環境情報取得手段は、レーザレーダである請求項１または請求項２に記載の追尾システム。
【請求項４】
前記環境情報取得手段はカメラである請求項１または請求項２に記載の追尾システム。
【請求項５】
前記先導車両は、
自己の位置と姿勢とに基づいて作成された走行軌跡データを送信し、
前記後続車両は、
前記位置姿勢決定手段によって決定された最終的な自己の位置及び姿勢と、前記先導車両の走行軌跡データとに基づいて経路計画を作成する経路計画手段と、
前記経路計画手段によって作成された経路計画に基づいて自律走行を制御する走行制御手段と
を更に備える請求項１から請求項４のいずれかに記載の追尾システム。
【請求項６】
前記後続車両は、自己が通過不可能な経路と、その経路の迂回経路とが対応付けられている地図情報を予め保有しており、前記先導車両が自己の通過不可能な経路を通過した場合には、その迂回経路として登録されている経路を前記地図情報から取得し、取得した前記迂回経路を走行する請求項１から請求項５のいずれかに記載の追尾システム。
【請求項７】
前記後続車両は、自己が駐車不可能な駐車場所と、その駐車場所に代わる他の駐車場所とが対応付けられている地図情報を予め保有しており、前記先導車両が自己の駐車不可能な駐車場所に停止した場合には、その駐車場所に代わる他の駐車場所を前記地図情報から取得し、取得した前記駐車場所に駐車する請求項１から請求項６のいずれかに記載の追尾システム。
【請求項８】
前記先導車両及び前記後続車両には、互いを連結するための連結機構が設けられている請求項１から請求項７のいずれかに記載の追尾システム。
【請求項９】
前記後続車両がｎ（ｎは２以上の整数）台存在し、隊列走行する場合において、
ｎ−１番目までの前記後続車両は、前記先導車両から受信した環境マップを、自己が備える前記環境情報取得手段により取得された環境情報に基づいて更新する環境マップ更新手段を備え、
更新後の前記環境マップを自己よりも後方に位置する前記後続車両に送信する請求項１から請求項８のいずれかに記載の追尾システム。
【請求項１０】
前記更新後の前記環境マップは、自己よりも前方に位置する前記後続車両及び前記先導車両にも送信される請求項９に記載の追尾システム。
【請求項１１】
先導車両から与えられる情報に基づいて後続車両が追尾走行を行う追尾システムに用いられる車両であって、
周辺の環境情報を取得する環境情報取得手段と、
他の車両が取得する周辺の環境情報と照合されることで、前記他の車両の位置及び姿勢を推定させ、最終的な前記他の車両の位置及び姿勢を決定するために用いられる環境マップを、前記環境情報と自己の位置及び姿勢に基づいて作成する環境マップ作成手段と、
前記環境マップを送信する通信手段と
を備える車両。
【請求項１２】
先導車両から与えられる情報に基づいて後続車両が追尾走行を行う追尾システムに用いられる車両であって、
他の車両において作成された環境マップを受信する通信手段と、
周辺の環境情報を取得する環境情報取得手段と、
前記環境情報取得手段により取得された環境情報と、前記他の車両において作成された環境マップとを照合することにより、自己位置及び姿勢を推定する地図照合手段と、
前記地図照合手段によって推定された自己の位置及び姿勢を参照して、最終的な自己の位置及び姿勢を決定する位置姿勢決定手段と
を備える車両。
【請求項１３】
先導車両から与えられる情報に基づいて後続車両が追尾走行を行う追尾方法であって、
先導車両は、
周辺の環境情報を取得し、
前記環境情報と自己位置及び姿勢とに基づいて、環境マップを作成し、
前記環境マップを送信し、
前記後続車両は、
前記先導車両によって作成された前記環境マップを受信し、
周辺の環境情報を取得し、取得した周辺の環境情報と前記先導車両によって作成された環境マップとを照合することにより、自己の位置及び姿勢を推定し、
推定した自己の位置及び姿勢を参照して、最終的な自己の位置及び姿勢を決定する追尾方法。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、追尾システム及びその方法および追尾システムに適用される車両に関するものである。
【背景技術】
【０００２】
従来、運転手により運転される先導車両と、該先導車両に自動追尾して縦列走行する複数の後続車両とからなる自動追尾走行システムが提案されている（例えば、特許文献１参照）。
特許文献１には、ＧＰＳにより得られる現在位置情報に基づいて生成される走行軌跡情報と、運転者による操舵量やスロットル開度等の運転操作量を先導車両において作成し、これらの軌跡情報と運転操作量とを後続車両に送信し、後続車両において、先導車両からの軌跡情報及び運転操作量を参照しながら後続車両の運転を行うことが開示されている。
【特許文献１】特許第３５９４４９８号公報
【発明の開示】
【発明が解決しようとする課題】
【０００３】
しかしながら、上記特許文献１の発明では、ＧＰＳを用いて自己位置を検出し、これに基づいて走行軌跡情報を生成している。したがって、例えば、トンネルの中や木が生い茂っている場所などのように、ＧＰＳが利用できない環境下においては、自己位置の特定ができず、走行軌跡情報を生成することができない。この結果、後続車両に追尾走行のための有効な情報を送信することができず、後続車両の追尾運転が不可能となるおそれがある。
【０００４】
本発明は、上記問題を解決するためになされたもので、ＧＰＳが利用できない環境下であっても安定した追尾走行を実現することの可能な追尾システムおよびその方法並びに車両を提供することを目的とする。
【課題を解決するための手段】
【０００５】
上記課題を解決するために、本発明は以下の手段を採用する。
本発明は、先導車両から与えられる情報に基づいて後続車両が追尾走行を行う追尾システムであって、先導車両は、周辺の環境情報を取得する環境情報取得手段と、前記環境情報と自己位置及び姿勢とに基づいて、環境マップを作成する環境マップ作成手段と、前記環境マップを送信する通信手段とを備え、前記後続車両は、前記先導車両によって作成された前記環境マップを受信する通信手段と、周辺の環境情報を取得する環境情報取得手段と、前記環境情報取得手段により取得された環境情報と、前記先導車両によって作成された環境マップとを照合することにより、自己の位置及び姿勢を推定する地図照合手段と、前記地図照合手段によって推定された自己の位置及び姿勢を参照して、最終的な自己の位置及び姿勢を決定する位置姿勢決定手段とを備える追尾システム。
【０００６】
このような構成によれば、後続車両においては、先導車両から受信した環境マップに基づいて推定された位置及び姿勢を考慮して、最終的な自己の位置及び姿勢を特定するので、ＧＰＳを用いることなく、容易に、かつ、十分な精度をもって自己位置及び姿勢を推定することが可能となる。これにより、ＧＰＳが利用できない環境下においても安定した追尾走行を実現させることが可能となる。
先導車両の自己位置及び姿勢については、ＧＰＳ、IMU,或いは先導車両の行動を監視する他の監視装置等から自己位置及び姿勢を取得してもよいし、また、オドメトリを用いて自己位置及び姿勢を推定してもよい。
先導車両の行動を監視する他の監視装置等とは、例えば、先導車両が通過しそうな道にカメラ等を配置しておき、カメラによって先導車両が通過したことが確認された場合に、その位置を記憶することで、先導車両の位置や走行経路を管理するものが挙げられる。
また、後続車両は、自己の位置及び姿勢を推定する位置姿勢推定手段を備えていてもよい。位置姿勢推定手段は、例えば、ＧＰＳや後続車両の行動を監視する他の監視装置等から自己位置及び姿勢を取得してもよいし、オドメトリを用いてある原点からの位置と姿勢とを推定することとしてもよい。
オドメトリを用いる場合には、例えば、位置姿勢推定手段は、車輪の回転数と操舵角とを用いてオドメトリにより自己位置及び姿勢の推定を行ってもよいし、車輪の回転速度やステアリングを用いてオドメトリにより自己位置及び姿勢の推定を行ってもよい。このように、オドメトリに用いられるパラメータとしては、特に限定されない。
また、後続車両が位置姿勢推定手段を備える場合において、上記位置姿勢決定手段は、位置姿勢推定手段によって推定された位置及び姿勢と前記地図照合手段によって推定された自己の位置及び姿勢とから最終的な自己の位置及び姿勢を決定することとしてもよい。
【０００７】
上記追尾システムにおいて、前記先導車両は、前記環境情報取得手段によって取得された環境情報の中から、物体の属性に関する所定の条件を満たす物体をランドマークとして抽出するランドマーク抽出手段を更に備え、前記環境マップ作成手段は、前記ランドマーク抽出手段によって抽出された前記ランドマークの位置と、前記環境情報が取得されたときの自己の位置及び姿勢とに基づいて前記環境マップを作成することとしてもよい。
【０００８】
このような構成によれば、環境情報取得手段によって取得された環境情報の中から位置を特定するのに適した物体のみをランドマークとして抽出し、このランドマークに基づいて環境マップを作成するので、位置姿勢の特定に効果的な環境マップを作成することが可能となる。
物体の属性としては、例えば、物体の形状、物体の性質、温度、色、テクスチャー、シール、バーコード、反射材等が一例として挙げられる。
【０００９】
上記追尾システムにおいて、前記環境情報取得手段は、例えば、レーザレーダである。
上記追尾システムにおいて、前記環境情報取得手段は例えば、カメラである。
【００１０】
環境情報取得手段としてカメラを採用することにより、広範囲にわたる周囲環境の情報取得が可能となるとともに、色の情報も取得することが可能となる。これにより、レーザレーダに比べてより多くの周囲の環境情報を取得することが可能となる。更に、カメラはパッシブ式であるので、周囲の相手に認識されることなく、情報を取得することができるという利点がある。
【００１１】
上記追尾システムにおいて、前記先導車両は、自己の位置と姿勢とに基づいて作成された走行軌跡データを送信し、前記後続車両は、前記位置姿勢決定手段によって決定された最終的な自己の位置及び姿勢と、前記先導車両の走行軌跡データとに基づいて経路計画を作成する経路計画手段と、前記経路計画手段によって作成された経路計画に基づいて自律走行を制御する走行制御手段とを更に備えることとしてもよい。
【００１２】
このような構成によれば、先導車両が通過した走行軌跡データについても後続車両に送信されるので、上述した環境マップと走行軌跡データとに基づいて先導車両の走行経路をより高い精度で求めることが可能となり、追尾走行の精度を高めることが可能となる。
上記走行軌跡データは、後続車両に対して直接的に送信されてもよいし、他の装置を経由して最終的に後続車両によって受信されることとしてもよい。
【００１３】
上記追尾システムにおいて、前記後続車両は、自己が通過不可能な経路と、その経路の迂回経路とが対応付けられている地図情報を予め保有しており、前記先導車両が自己の通過不可能な経路を通過した場合には、その迂回経路として登録されている経路を前記地図情報から取得し、取得した前記迂回経路を走行することとしてもよい。
【００１４】
例えば、先導車両と後続車両とが大きさや重量の面で異なる場合には、先導車両と後続車両とが同じ経路（例えば、高さ制限、重量制限のある経路）を走行できないという場合が生ずる。このような場合でも、後続車両は通過不可能な経路に代わる他の経路の情報を有しているので、速やかに対応することが可能となる。
【００１５】
上記追尾システムにおいて、前記後続車両は、自己が駐車不可能な駐車場所と、その駐車場所に代わる他の駐車場所とが対応付けられている地図情報を予め保有しており、前記先導車両が自己の駐車不可能な駐車場所に停止した場合には、その駐車場所に代わる他の駐車場所を前記地図情報から取得し、取得した前記駐車場所に駐車することとしてもよい。
【００１６】
例えば、先導車両と後続車両とが高さや重量の面で異なる場合には、先導車両と後続車両とが同じ駐車位置に駐車できないという場合が生ずる。このような場合でも、後続車両は駐車不可能な駐車場所に代わる他の駐車場所の情報を有しているので、速やかに対応することが可能となる。
【００１７】
上記追尾システムにおいて、前記先導車両及び前記後続車両には、互いを連結するための連結機構が設けられていることとしてもよい。
【００１８】
このような構成によれば、先導車両と後続車両とを連結することが可能となる。これにより、連結中においては先導車両及び後続車両において追尾走行のために必要となる処理を行うことなく、追尾走行が可能となる。
【００１９】
上記追尾システムにおいて、前記後続車両がｎ（ｎは２以上の整数）台存在し、隊列走行する場合において、ｎ−１番目までの前記後続車両は、前記先導車両から受信した環境マップを、自己が備える前記環境情報取得手段により取得された環境情報に基づいて更新する環境マップ更新手段を備え、更新後の前記環境マップを自己よりも後方に位置する前記後続車両に送信することとしてもよい。
【００２０】
このような構成とすることで、先導車両が通過するときと後続車両が通過するときとで周囲の環境状況が変化した場合でも、後続車両は、環境状況の変化が反映された環境マップを取得することが可能となるので、安定した追尾走行を実現することが可能となる。
【００２１】
更に、上記追尾システムにおいて、前記更新後の前記環境マップは、自己よりも前方に位置する前記後続車両及び前記先導車両にも送信されることとしてもよい。
このようにすることで、先導車両或いは環境マップを更新した後続車両よりも前方に位置する後続車両が行き止まりに到達し、これらの車両が今まで通った道を引き返すような場合に、更新環境マップを参照することで、より効果的に位置推定を行うことが可能となる。
【００２２】
本発明は、先導車両から与えられる情報に基づいて後続車両が追尾走行を行う追尾システムに用いられる車両であって、周辺の環境情報を取得する環境情報取得手段と、他の車両が取得する周辺の環境情報と照合されることで、前記他の車両の位置及び姿勢を推定させ、最終的な前記他の車両の位置及び姿勢を決定するために用いられる環境マップを、前記環境情報と自己の位置及び姿勢に基づいて作成する環境マップ作成手段と、前記環境マップを送信する通信手段とを備える車両を提供する。
【００２３】
本発明は、先導車両から与えられる情報に基づいて後続車両が追尾走行を行う追尾システムに用いられる車両であって、他の車両において作成された環境マップを受信する通信手段と、周辺の環境情報を取得する環境情報取得手段と、前記環境情報取得手段により取得された環境情報と、前記他の車両において作成された環境マップとを照合することにより、自己位置及び姿勢を推定する地図照合手段と、前記地図照合手段によって推定された自己の位置及び姿勢を参照して、最終的な自己の位置及び姿勢を決定する位置姿勢決定手段とを備える車両を提供する。
【００２４】
本発明は、先導車両から与えられる情報に基づいて後続車両が追尾走行を行う追尾方法であって、先導車両は、周辺の環境情報を取得し、前記環境情報と自己位置及び姿勢とに基づいて、環境マップを作成し、前記環境マップを送信し、前記後続車両は、前記先導車両によって作成された前記環境マップを受信し、周辺の環境情報を取得し、取得した周辺の環境情報と前記先導車両によって作成された環境マップとを照合することにより、自己の位置及び姿勢を推定し、推定した自己の位置及び姿勢を参照して、最終的な自己の位置及び姿勢を決定する追尾方法を提供する。
また、上記構成は、可能な範囲で組み合わせて利用することができるものである。
【発明の効果】
【００２５】
本発明によれば、ＧＰＳが利用できない環境下においても安定した追尾走行を実現することができるという効果を奏する。
【発明を実施するための最良の形態】
【００２６】
以下に、本発明に係る追尾システムおよびその方法ならびに車両の各実施形態について、図面を参照して説明する。
【００２７】
〔第１の実施形態〕
図１は、本発明の第１の実施形態に係る追尾システムの全体構成を示した図である。
図１に示した追尾システムは、先導車両１から与えられる情報に基づいて後続車両２が追尾走行を行う追尾システムである。本実施形態では、先導車両１は、人が運転する有人車両であり、後続車両２は無人走行を可能とする自律走行車両である。なお、これらは一例であり、例えば、先導車両１および後続車両２の両方を自律走行車両としてもよい。
【００２８】
図２は、先導車両１の全体構成の概略図である。
図２に示すように、先導車両１は、車速センサ１１、ＩＭＵ（Inertial Measurement
Unit：慣性測定装置）１２、レーザレーダ（環境情報取得手段）１３、制御装置１４、及び通信装置（通信手段）１５を主な構成として備えている。
【００２９】
上記車速センサ１１及びＩＭＵ１２は自己位置及び姿勢を推定するための情報を取得するための手段である。車速センサ１１は、車速を検出して制御装置１４に出力する。ＩＭＵ１２は、車両の姿勢を検出するものであり、直交する３軸（Ｘ，Ｙ，Ｚ）に各々一個設置された３つの加速度計と３つのジャイロスコープから構成されている。ＩＭＵ１２は、加速度計により検出された車両の３方向の加速度と、ジャイロスコープにより検出される当該ＩＭＵの中心を原点とする３軸回りの角速度とを車両の姿勢データとして制御装置１４に出力する。レーザレーダ１３は、周辺の環境情報を取得するものであり、取得した周囲環境の情報を制御装置１４に出力する。
【００３０】
制御装置１４は、車速センサ１１により検出される車速、ＩＭＵ１２により検出される姿勢データ、及びレーザレーダ１３により検出される環境情報に基づいて、走行軌跡データ及び環境マップを作成し、これを通信装置１５に出力する。通信装置１５は、後続車両２が備える通信装置２５（図３参照）との間で双方向通信を可能とする。
【００３１】
図３は後続車両２の全体構成の概略図である。
図３に示すように、後続車両２は、上述した先導車両１と同様に、車速センサ２１、ＩＭＵ２２、レーザレーダ２３、制御装置２４、及び通信装置２５を備えている。更に、後続車両２は、自律走行を実現するための走行駆動機構として、例えば、ステアリング駆動用のステアリングアクチュエータ２６、アクセル・ブレーキ操作用のアクセルブレーキアクチュエータ２７等を備えている。
【００３２】
後続車両２において、車速センサ２１、ＩＭＵ２２、及びレーザレーダ２３により検出された情報、並びに、通信装置２５により受信された先導車両１からの走行軌跡データ並びに環境マップは、制御装置２４に入力される。制御装置２４は、これらの情報に基づいてステアリングアクチュエータ２６およびアクセルブレーキアクチュエータ２７を駆動制御する制御指令を生成することにより、後続車両２の安定した走行を実現させる。
【００３３】
次に、先導車両１の制御装置１４によって実現される処理について図４及び図５を参照して説明する。図４は、制御装置の機能を展開して示した機能ブロック図である。制御装置１４は、例えば、ＣＰＵ（中央演算装置）、ＲＯＭ（Read Only Memory）、ＲＡＭ（Random
Access Memory）等から構成されている。図４に示した各種機能を実現するための一連の処理の過程は、プログラムの形式でＲＯＭ等に記録されており、このプログラムをＣＰＵがＲＡＭ等に読み出して、情報の加工・演算処理を実行することにより、各種機能が実現される。
【００３４】
図４に示されるように、制御装置１４は、位置姿勢推定部（位置姿勢推定手段）４１と、ランドマーク抽出部（ランドマーク抽出手段）４２と、環境マップ作成部（環境マップ作成手段）４３とを備えている。
位置姿勢推定部４１は、車速センサ１１から入力される車速とＩＭＵ１２から入力される姿勢データとをパラメータとし、ある原点からの位置と姿勢とを推定し、推定した位置及び姿勢（ｘ，ｙ，θ）を出力する。
位置姿勢推定部４１は、例えば、オドメトリを用いて自己の位置及び姿勢を推定する。オドメトリとは、例えば、車輪の回転数と操舵角とを累積することにより、ある原点に対する位置及び姿勢を追跡するという原理に基づくものであり、例えば、日本ロボット学会誌５巻５号、1987年10月、”自律移動ロボットの機構と移動制御技術”、５２頁から５８頁に開示されている。例えば、以下の（１）式を時間積分することで自己の位置及び姿勢を推定することが可能である。なお、オドメトリに用いられるパラメータは、上記車輪の回転数や操舵角に制約されるものではなく、これに関連する情報、例えば、車輪の回転速度やステアリング角度等を用いることとしてもよい。
【００３５】
【数１】
0005249577.tif 000002
【００３６】
この（１）式は、上記論文”自律移動ロボットの機構と移動制御技術”の５３頁の（４）式記載の式となる。
ランドマーク抽出部４２は、レーザレーダ１３によって取得された環境情報から物体形状に関する所定の条件を満たす物体をランドマークとして抽出する。ランドマークとして抽出される物体は、例えば、ビルや木などのように直線部分を多く含み、ある程度の大きさがあるような認識されやすい物体であることが好ましい。ランドマークの抽出については、ランドマークとして登録された物体の形状に関する特徴量を予め保有しており、これらの特徴量を所定値以上含む物体をランドマークとして抽出する。なお、レーザレーダ１３によって検出されたデータを数学的に解析することにより数値化し、この数値が予め設定されている閾値以上の物体をランドマークとして抽出することとしてもよい。
【００３７】
ランドマーク抽出部４２は、ランドマークを抽出すると、抽出したランドマークの位置（Ｘｓ１，Ｙｓ１），・・・，（Ｘｓｎ，Ｙｓｎ）を出力する。このときのランドマークの位置は、先導車両１において予め設定されている基準点、例えば、レーザレーダ１３の設置位置を基準とした位置座標とされる。
【００３８】
環境マップ作成部４３には、位置姿勢推定部４１からの自己の位置姿勢（ｘ，ｙ，θ）と、ｎ個のランドマーク位置（Ｘｓ１，Ｙｓ１），・・・，（Ｘｓｎ，Ｙｓｎ）とが入力される。環境マップ作成部４３は、ランドマーク抽出部４２によって抽出されたランドマークの位置（Ｘｓ１，Ｙｓ１），・・・，（Ｘｓｎ，Ｙｓｎ）と、そのランドマークが環境情報として取得されたときの自己の位置姿勢（ｘ，ｙ，θ）とに基づいて環境マップを作成する。具体的には、環境マップ作成部４３は、自己の位置姿勢（ｘ，ｙ，θ）に基づいて、ランドマークの位置座標（Ｘｓ１，Ｙｓ１），・・・，（Ｘｓｎ，Ｙｓｎ）を変換することにより、車両の位置座標軸とランドマークの位置座標軸とを一致させ、車両とランドマークとの位置関係を示す環境マップを作成する。
【００３９】
図５は、環境マップの一例を示した図である。このように、環境マップは、同じ位置座標軸（本実施形態ではＸ−Ｙ座標で表している）に対する先導車両１の自己の位置姿勢（ｘ，ｙ，θ）とランドマークの位置（Ｘ１，Ｙ１），・・・，（Ｘｎ，Ｙｎ）とが示された情報である。
【００４０】
制御装置１４において、位置姿勢推定部４１によって推定された自己の位置姿勢は累積されることにより、先導車両１の走行軌跡データが作成される。制御装置１４において作成された上記環境マップと走行軌跡データとは、通信装置１５を介して後続車両２に送信される。
【００４１】
次に、後続車両２が備える制御装置２４について図６を参照して説明する。図６は、制御装置２４の機能を展開して示した機能ブロック図である。制御装置２４は、上述した制御装置１４と同様に、ＣＰＵ（中央演算装置）、ＲＯＭ（Read Only Memory）、ＲＡＭ（Random
Access Memory）等から構成されている。図６に示した各種機能を実現するための一連の処理の過程は、プログラムの形式でＲＯＭ等に記録されており、このプログラムをＣＰＵがＲＡＭ等に読み出して情報の加工・演算処理を実行することにより、各種機能が実現される。
【００４２】
図６に示すように、制御装置２４は、位置姿勢推定部（位置姿勢推定手段）５１と、地図照合部（地図照合手段）５２と、位置姿勢決定部（位置姿勢決定部）５３と、経路計画部（経路計画手段）５４と、走行制御部（走行制御手段）５５とを備えている。
【００４３】
上記位置姿勢推定部５１については、先導車両１の制御装置１４が備える位置姿勢推定部４１と同様であるので、説明を省略する。
地図照合部５２は、レーザレーダ（環境情報取得手段）２３により取得された周囲の環境情報と、先導車両１から受信した環境マップとを照合することにより、自己位置及び姿勢を推定する。具体的には、地図照合部５２は、レーザレーダ２３により取得された環境情報の中から上述したランドマーク抽出部４２と同様にランドマークを抽出する。続いて、抽出したランドマークと先導車両１から受信した複数の環境マップとを照合することにより、自己のレーザレーダ２３によって取得されたランドマークに一致する環境マップのランドマークを特定する。そして、後続車両２からみたときのランドマークの位置、本実施形態では、ランドマークまでの距離ｒn及び車両進行方向からの角度Ψnと、環境マップにおける当該ランドマークの位置（ｘｎ、ｙｎ）とに基づいて、自己の位置姿勢（ｘ，ｙ，θ）を推定する。位置姿勢の推定には、例えば、最小二乗法が用いられる。具体的には、地図照合部５２は、以下に示す（１）式を用いて、自己の位置姿勢（ｘ，ｙ，θ）を推定する。
【００４４】
【数２】
0005249577.tif 000003
【００４５】
図７に示されるように、上記（２）式において、Ψｉ（ｉ＝１，２，・・・ｎ）は車両進行方向からの角度、（ｘｉ、ｙｉ）は先導車両１にて得られたランドマークｉの位置である。
位置姿勢推定部５１は、レーザレーダ２３により得られたランドマークまでの距離ｒi及び車両進行方向からの角度Ψiと、環境マップから得られたランドマークの位置（ｘi、ｙi）とを上記（２）式に代入することにより、左辺の自己位置姿勢（ｘ，ｙ，θ）を得る。
ここで、θ＝ａｔａｎ２（ｓｉｎθ，ｃｏｓθ）である。ａｔａｎ２（ｙ，ｘ）関数は、二つの変数ｘとｙに対する逆正接（アークタンジェント）を計算する。これは、ｙ／ｘの逆正接を計算し、二つの引数の符号は結果の象限を決定するために用いられる。
【００４６】
位置姿勢推定部５１によって推定された自己の位置姿勢（ｘｋ，ｙｋ，θｋ）及び地図照合部５２によって推定された自己の位置姿勢（ｘ，ｙ，θ）は、位置姿勢決定部５３に出力される。
位置姿勢決定部５３は、位置姿勢推定部５１によって推定された自己の位置姿勢（ｘｋ，ｙｋ，θｋ）と地図照合部５２によって求められた自己の位置姿勢（ｘ，ｙ，θ）とをカルマンフィルタを用いて融合し、車両モデルを考慮した自己の位置姿勢を求め、これを最終的な自己の位置姿勢（Ｘ、Ｙ、Ｚ）として、経路計画部５４に出力する。
経路計画部５４は、先導車両１から受信した走行軌跡データと位置姿勢決定部５３から入力された自己の位置姿勢（Ｘ，Ｙ，Ｚ）に基づいて先導車両１に追尾する走行経路を計画し、この走行経路を走行制御部５５に出力する。
【００４７】
走行制御部５５は、経路計画部５４によって作成された走行経路に基づいて、ステアリングアクチュエータ２６を制御するためのステアリング回転指令およびアクセルブレーキアクチュエータ２７を制御するためのアクセルブレーキ操作指令等を作成する。これにより、ステアリングアクチュエータ２６及びアクセルブレーキアクチュエータ２７が図示しない走行駆動用の制御部によって駆動制御されることにより、後続車両２の自動追尾走行が実現される。
【００４８】
次に、本実施形態に係る追尾システムの作用について説明する。
まず、先導車両１において、人の運転により走行が開始されると、制御装置１４には、車速センサ１１、ＩＭＵ１２から車速、姿勢データが入力されるとともに、レーザレーダ１３から周囲の環境情報が入力される。
【００４９】
制御装置１４において、位置姿勢推定部４１によりオドメトリを用いて自己の位置姿勢が推定される。また、ランドマーク抽出部４２によって環境情報からランドマークが抽出され、その位置情報が環境マップ作成部４３に出力される。環境マップ作成部ではランドマークの位置と位置姿勢推定部からの位置及び姿勢情報に基づいて環境マップが作成される。環境マップ作成部４３によって作成された環境マップ、及び位置姿勢推定部によって推定される位置姿勢を累積することで求められる走行軌跡情報は、通信装置１５を介して後続車両に送信される。
【００５０】
後続車両２では、位置姿勢推定部５１により後続車両２の位置姿勢が推定されるとともに、地図照合部５２により環境マップとレーザレーダ２３により取得された周囲の環境情報に基づいて自己の位置姿勢が推定される。位置姿勢推定部５１により推定された位置姿勢及び地図照合部５２により推定された位置姿勢とは、位置姿勢決定部５３に入力され、これらの位置姿勢を融合することで、より精度の高い位置姿勢が求められる。この位置姿勢は経路計画部５４に出力され、この位置姿勢に基づく走行経路が生成され、この走行経路に基づく駆動制御指令が走行制御部５５によって生成される。これにより、ステアリングアクチュエータ２６およびアクセルブレーキアクチュエータ２７が走行制御部５５により与えられた走行指令に基づいて駆動することにより、後続車両２の追尾走行を可能とする。
【００５１】
以上説明してきたように、本実施形態に係る追尾システムによれば、先導車両１及び後続車両２ともに、ＧＰＳを用いずに車速と姿勢データとに基づいて自己の位置姿勢を推定するので、ＧＰＳが利用できない環境下であっても安定した位置姿勢推定を行うことが可能となる。これにより、後続車両２の安定した追尾走行が可能となる。
【００５２】
〔第２の実施形態〕
次に、本発明の第２の実施形態に係る追尾システムについて説明する。
第２の実施形態に係る追尾システムは、上述したレーザレーダ１３、２３に代えて、カメラを備える点で上述した第１の実施形態に係る追尾システムと異なる。
【００５３】
本実施形態に係る追尾システムの先行車両１においては、カメラによって取得された画像情報がランドマーク抽出部４２に出力されることとなる。ランドマーク抽出部４２は、画像処理を行うことにより予め登録されている特徴量、例えば、カラー、エッジ等を抽出し、これらをランドマークとして抽出する。また、ランドマークの位置については、カメラの設置位置及び設置角度を予め登録しておくことにより推定が可能である。環境マップ作成部４３では、カメラによって取得された画像情報に基づく環境マップが作成され、この環境マップが後続車両３に送信される。
【００５４】
また、後続車両３においても、レーザレーダ２３に代えてカメラが設置されている。したがって、後続車両３の地図照合部５２は、カメラによって取得された周囲の画像情報と先導車両１から受信した環境マップとを照合することにより、自己の位置姿勢を推定し、この情報を位置姿勢決定部５３に出力する。環境マップに基づく自己の位置姿勢の推定等の手順については上述した第１の実施形態と同様である。
【００５５】
本実施形態によれば、周辺の環境情報を取得する手段としてカメラを採用するので、レーザレーダ１３，２３を採用する場合と比較して、色情報の取得が可能となるとともに、広範囲による撮影が可能となるという利点がある。また、レーザレーダ１３，２３は、自己から信号を発するために相手に認識されてしまうが、カメラはパッシブタイプであることから相手に認識されずに周囲の環境情報を取得することができるという利点がある。
【００５６】
〔第３の実施形態〕
次に、本発明の第３の実施形態に係る追尾システムについて説明する。
上述した第１及び第２の実施形態においては、図１に示したように、先導車両１が後続車両に対して環境マップと走行軌跡データとを送信し、各後続車両２はこの環境マップと走行軌跡データとに基づいて先導車両１の追尾走行をそれぞれ行っていた。
本実施形態に係る追尾システムでは、図８に示すように、後続車両２ａにおいて、当該後続車両２ａが備えるレーザレーダ２３或いはカメラによって取得されたランドマークの情報に基づいて先導車両１から受信した環境マップを更新し、更新後の環境マップを自己よりも後方に位置する後続車両２ｂに送信することとしている。
【００５７】
例えば、先導車両１が通過したときと後続車両が同じ場所を通過したときとで、周囲の環境が変化している場合がある。例えば、ビルが取り壊されたり、木が倒れたりすることが考えられる。このような場合、先導車両１により作成される環境マップに示されているランドマークと後続車両２のレーザレーダ１３またはカメラによって取得される環境情報のランドマークとは一致しないこととなる。
【００５８】
本実施形態に係る追尾システムでは、先導車両１からの環境マップにおけるランドマークと後続車両２ａにおいて撮影・抽出されたランドマークとが一致しなかった場合に、その不一致のランドマークを環境マップから削除することで、環境マップを更新する環境マップ更新機能（環境マップ更新手段）を地図照合部５２が備える。そして、更新後の環境マップを自己よりも後方に位置する後続車両２ｂに対して送信する。
これにより、後続車両２ｂはより新規な環境マップに基づく追尾走行が可能となり、追尾走行の精度を向上させることが可能となる。
【００５９】
〔第４の実施形態〕
次に、本発明の第４の実施形態に係る追尾システムについて説明する。
本実施形態に係る追尾システムは、上述した各実施形態に係る追尾システムと略同様の構成を備えるが、先導車両１と後続車両２とが大きさや重量の面で異なる場合には、先導車両１と後続車両２とが同じ経路（例えば、高さ制限、重量制限のある経路）を走行できないという場合が考えられる。
【００６０】
このような場合に備えて、本実施形態に係る追尾システムの後続車両２は、自己が通過不可能な経路と、その経路の迂回経路とが対応付けられている地図情報を予め保有している。そして、先導車両１が自己の通過不可能な経路を通過した場合には、その迂回経路として登録されている経路を保有している地図情報から取得し、取得した迂回経路を走行する。このように、自己が通過不可能な経路に代わる他の経路の情報を地図情報として有していることで、安定した追尾走行を続行することが可能となる。
【００６１】
また、上記通過不可能な経路だけでなく、例えば、駐車場等で先導車両と同じ場所に駐車できないような場合も考えられる。このような場合に備えて、後続車両２は、自己が駐車不可能な駐車場所と、その駐車場所に代わる他の駐車場所とが対応付けられている地図情報を予め保有していてもよい。先導車両１が自己の駐車不可能な駐車場所に停止した場合には、その駐車場所に代わる他の駐車場所をこの地図情報から取得することで、このような場合にも対応することが可能となる。
【００６２】
以上、本発明の実施形態について図面を参照して詳述してきたが、具体的な構成はこの実施形態に限られるものではなく、本発明の要旨を逸脱しない範囲の設計変更等も含まれる。
【００６３】
〔変形例１〕
例えば、上述した各実施形態においては、先導車両１において、レーザレーダ１３またはカメラにより取得された周囲の環境情報の中から物体形状に関する所定の条件を満たすものをランドマークとして抽出し、このランドマークを用いて環境マップを作成していたが、このようなランドマークの抽出を省略し、レーザレーダ１３またはカメラにより検出された全ての物体の情報とその位置情報とに基づいて環境マップを作成することとしてもよい。このように、検出された全ての物体の情報に基づいて環境マップを作成することで、周辺環境に関するより多くの情報を後続車両２に与えることが可能となる。
【００６４】
〔変形例２〕
上述した各実施形態における追尾システムにおいて、図９に示すように、先導車両１と後続車両２とを連結させる連結機構１００を先導車両１及び各後続車両２にそれぞれ設けることとしてもよい。そして、通常の状態においては連結機構により先導車両１に後続車両２を連結させた状態で走行を行い、連結させた状態での走行が不可能な場合に限って各車両を分離させ、上述したような追尾走行を行うこととしてもよい。
このように、必要な場合に限って追尾走行をさせることにより、先導車両１及び後続車両２における制御装置１４，２４の処理負担を軽減させることが可能となる。また、先導車両１と後続車両２とがそれほど距離を開けずに追尾走行するような場合には、後続車両２は長距離レーザレーダを搭載する必要がないために、精度の高くない廉価なセンサを利用することが可能となる。これにより、コスト面についても利点がある。
【００６５】
〔変形例３〕
また、上述した各実施形態においては、先導車両１から後続車両２に対して走行軌跡データを送信することとしたが、走行軌跡データを省略し、環境マップのみを送信することとしてもよい。この場合、走行軌跡データが送信されるのに比べて追尾走行の精度が低下することが予想されるが、追尾走行を実現するのに十分な精度は確保可能である。
【００６６】
〔変形例４〕
また、上記実施形態においては、ＧＰＳを用いずに位置姿勢を推定していたが、ＧＰＳを用いた位置姿勢の検出と組み合わせることとしてもよい。例えば、ＧＰＳによる位置情報の取得が可能な領域においては、ＧＰＳによる位置検出を優先させ、ＧＰＳによる位置情報の取得が不可能な領域においては、速度及び姿勢データに基づく位置姿勢推定を行う。このようにすることで、位置姿勢の推定精度を向上させることが可能となり、この結果、追尾走行の精度を高めることが可能となる。
【００６７】
〔変形例５〕
また、上述した各実施形態においては、先導車両１のみが環境マップを作成し、後続車は先導車両１によって作成された環境マップに基づいて追尾走行を行うこととして説明したが、これに代えて、後続車両２が先導車両１の制御装置１４が備える機能を更に有していてもよい。このように、後続車両２が先導車両１の備える機能を併せ持つことで、例えば、自己よりも前方を走行する車両と後方に位置する車両との両方が存在する場合に、先行する車両に対しては自己が後続車両２として機能することにより、先行する車両の追尾走行を行い、自己よりも後方に位置する車両に対しては先導車両１として振舞うことが可能となる。なお、制御装置１４と制御装置２４の機能を併せ持つ場合には、重複する機能について併用することとすればよい。
【００６８】
〔変形例６〕
また、上述の各実施形態においては、先導車両１と後続車両２とが異なる車両である場合について説明したが、先導車両１と後続車両２とは同一の車両であってもよい。例えば、車両が、一度走行した走行経路を、再度走行する場合がこれに該当する。この場合、初回の走行において作成された走行軌跡データと環境マップとを記憶しておき、２度目以降の走行時においては、記憶しておいた走行軌跡データと環境マップとを読み出し、これらの情報に基づいて走行すればよい。なお、この場合には、他の車両と通信を行う必要がないので、通信装置１５，２５を不要とすることができる。
【００６９】
〔変形例７〕
また、上記各実施形態においては、先導車両１と後続車両２とが直接的に通信を行うこととしたが、この態様に限らず、他の装置を経由して最終的に情報が後続車両２に届けられることとしてもよい。
【００７０】
〔変形例８〕
上記各実施形態においては、先導車両１、後続車両２が自己の位置及び姿勢を推定する位置姿勢推定部４１，５１を備えていたが、これに代えて、他の方法により自己位置及び姿勢を推定することとしてもよい。
例えば、自己位置及び姿勢の取得については、ＧＰＳ、IMU,或いは先導車両、後続車両の行動を監視する他の監視装置等から自己位置及び姿勢を取得してもよい。先導車両、後続車両の行動を監視する他の監視装置等とは、例えば、先導車両や後続車両が通過しそうな道に車両を特定するセンサを配置しておき、センサによって先導車両や後続車両が通過したことが確認された場合に、その位置及び姿勢を記憶することで、先導車両及び後続車両の位置及び姿勢や走行経路を管理するものが挙げられる。より具体的には、センサを用いて車両を特徴付ける属性（例えば、形状、性質、温度、色、テクスチャー、シール、バーコード、反射材等）を捕らえ、位置姿勢を推定する。センサとしては、カメラ、レーザレーダ、磁気センサ、超音波センサ等が挙げられる。具体的な態様としては、例えば、車両にマーカ等を貼り付け、カメラによってこのマーカを検出することで車両の位置検出を行う。
このように、他の装置から自己位置及び姿勢を取得することにより、先導車両及び後続車両において、位置姿勢推定部４１，５１並びに位置姿勢の推定に必要とされるセンサ類を省略することが可能となる。
【図面の簡単な説明】
【００７１】
【図１】本発明の第１の実施形態に係る追尾システムの全体構成を示した図である。
【図２】図１に示した先導車両の全体概略構成を示した図である。
【図３】図１に示した後続車両の全体概略構成を示した図である。
【図４】図２に示した先導車両の制御装置が備える機能を展開して示した機能ブロック図である。
【図５】環境マップの一例を示した図である。
【図６】図３に示した後続車両の制御装置が備える機能を展開して示した機能ブロック図である。
【図７】地図照合部による自己の位置姿勢推定について説明するための図である。
【図８】本発明の第３の実施形態に係る追尾システムについて示した図である。
【図９】本発明の変形例２に係る追尾システムの全体構成について示した図である。
【符号の説明】
【００７２】
１  先導車両
２  後続車両
１１、２１  車速センサ
１２、２２  ＩＭＵ
１３、２３  レーザレーダ
１４、２４  制御装置
１５、２５  通信装置
２６  ステアリングアクチュエータ
２７  アクセルブレーキアクチュエータ
４１  位置姿勢推定部
４２  ランドマーク抽出部
４３  環境マップ作成部
５１  位置姿勢推定部
５２  地図照合部
５３  位置姿勢決定部
５４  経路計画部
５５  走行制御部
１００  連結機構
【図１】
0005249577.tif 000004
【図２】
0005249577.tif 000005
【図３】
0005249577.tif 000006
【図４】
0005249577.tif 000007
【図５】
0005249577.tif 000008
【図６】
0005249577.tif 000009
【図７】
0005249577.tif 000010
【図８】
0005249577.tif 000011
【図９】
0005249577.tif 000012