JP5105596B2 - 自律走行移動体の走行経路決定用地図作成装置及び走行経路決定用地図作成方法 - Google Patents

Description

本発明は、自律して走行する移動体の走行経路を決めるのに用いる自律走行移動体の走行経路決定用地図作成装置及び走行経路決定用地図作成方法に関するものである。

従来、上記した自律走行移動体としては、例えば、車両の前部に配置された少なくとも三個の距離センサと、車両の後部に配置された少なくとも一個の距離センサと、走査角度を変更可能な距離センサと、車両の両サイドに配置された超音波センサと、車両の前後部に配置された少なくとも一個のカメラを備えた自律走行車両がある。
この自律走行車両では、車両の前後左右に配置された多数の距離センサや超音波センサやカメラで得た外界情報に基づいて、走行経路を決める地図を作成するようになっている（例えば、特許文献１参照）。
特開平11-212640号

ところが、従来にあっては、自律走行車両の走行経路決定用地図を作成するために、自律走行車両に搭載した距離センサや超音波センサやカメラなどの多数のセンサを用いなければならず、その分だけシステムが複雑になるのに加えて、コストも高くついてしまうという問題があり、この問題を解決することが従来の課題となっていた。
本発明は、上述した従来の課題に着目してなされたもので、シンプルで且つ低コストであるにもかかわらず、高速の自律走行に適した走行経路決定用地図を作成することが可能である自律走行移動体の走行経路決定用地図作成装置及び走行経路決定用地図作成方法を提供することを目的としている。

本発明の請求項１に係る発明は、自律して高速で走行する移動体の走行経路を決めるのに用いる地図（移動体近傍約１００ｍ四方の地図）の作成装置であって、最高速度（概ね時速５０ｋｍ／ｈ）で走行する前記移動体の少なくとも急制動停止可能位置よりも前方の路面上で且つ当該移動体の進行方向に対して直交する方向に光軸を走査する水平スキャンレーザレンジファインダと、前記移動体の前方に光軸を走査しつつその走査方向と直交する方向に揺動させる立体スキャンレーザレンジファインダと、前記移動体近傍から遠方にかけての平面の領域を抽出するステレオカメラと、前記水平及び立体の両レーザレンジファインダ並びにステレオカメラの各光軸のロール角度及びピッチ角度を計測する補正手段を具備した外界計測部と、デッドレコニングなどの手段により前記移動体の自己位置と方向を求める自己位置計測部と、前記外界計測部の水平及び立体の両レーザレンジファインダで得たセンサ座標系データを前記補正手段で計測した光軸のロール角度及びピッチ角度に基づいて鉛直方向上向きの軸を一軸とする機体座標系に補正変換して、前記移動体の前方における段差領域の有無を判定処理すると共に、前記ステレオカメラで得た画像内の平面領域抽出処理及び前記補正手段で計測した光軸のロール角度及びピッチ角度に基づいて鉛直方向上向きの軸を一軸とする機体座標系に補正変換する平面法線補正処理を行うデータ解析部と、このデータ解析部から得た段差領域抽出データ及び平面領域抽出データの地上座標系(移動体がスタートした位置及び方向を原点とする座標系)への座標変換を行う座標変換モジュールと、この座標変換により得た段差領域抽出データ及び平面領域抽出データに前記自己位置計測部からの地上座標系自己位置データを加えてベース地図を作成する地図作成モジュールと、前記ベース地図に基づいて前記移動体の走行経路を決める走行経路決定モジュールを具備した地図作成部を備え、前記地図作成部の地図作成モジュールは、前記水平及び立体の両レーザレンジファインダによる段差領域抽出データと水平及び立体の両レーザレンジファインダの未計測点に対するステレオ画像による平面領域抽出データとを反映させたベース地図を作成し、前記水平及び立体の両レーザレンジファインダの段差領域抽出データは、過去のものは保持すると共にベース地図上の同一地点に新規の計測があった場合には最新のものに逐次更新し、前記ステレオカメラの画像処理データは常に最新のものに逐次更新するものとしてある構成としたことを特徴としており、この自律走行移動体の走行経路決定用地図作成装置の構成を前述の従来の課題を解決するための手段としている。

本発明の請求項２に係る発明は、請求項１に係る発明と同じく、自律して高速で走行する移動体の走行経路を決めるのに用いる地図の作成装置であって、最高速度で走行する前記移動体の少なくとも急制動停止可能位置よりも前方の路面上で且つ当該移動体の進行方向に対して直交する方向に光軸を走査する水平スキャンレーザレンジファインダと、前記移動体の前方に光軸を走査しつつその走査方向と直交する方向に揺動させる立体スキャンレーザレンジファインダと、前記移動体近傍から遠方にかけての平面の領域を抽出するステレオカメラと、鉛直方向上向きの軸を一軸とする機体座標系で前記水平及び立体の両レーザレンジファインダ並びにステレオカメラを水平に保つ水平保持手段を具備した外界計測部と、デッドレコニングなどの手段により前記移動体の自己位置を求める自己位置計測部と、前記外界計測部の水平保持手段により水平に保たれた水平及び立体の両レーザレンジファインダで得たセンサ座標系データを機体座標系に補正変換して、前記移動体の前方における段差領域の有無を判定処理すると共に、前記ステレオカメラで得た画像内の平面領域抽出処理を行うデータ解析部と、このデータ解析部から得た段差領域抽出データ及び平面領域抽出データの地上座標系への座標変換を行う座標変換モジュールと、この座標変換により得た段差領域抽出データ及び平面領域抽出データに前記自己位置計測部からの地上座標系自己位置データを加えてベース地図を作成する地図作成モジュールと、前記ベース地図に基づいて前記移動体の走行経路を決める走行経路決定モジュールを具備した地図作成部を備え、前記地図作成部の地図作成モジュールは、前記水平及び立体の両レーザレンジファインダによる段差領域抽出データと水平及び立体の両レーザレンジファインダの未計測点に対するステレオ画像による平面領域抽出データとを反映させたベース地図を作成し、前記水平及び立体の両レーザレンジファインダの段差領域抽出データは、過去のものは保持すると共にベース地図上の同一地点に新規の計測があった場合には最新のものに逐次更新し、前記ステレオカメラの画像処理データは常に最新のものに逐次更新するものとしてある構成としたことを特徴としており、この自律走行移動体の走行経路決定用地図作成装置の構成を前述の従来の課題を解決するための手段としている。

前記水平及び立体の両レーザレンジファインダは、移動体の近傍から遠方までの距離を正確に計測することが可能であるが、現在実用的なものは、一本〜数本のレーザ光を機械的及び光学的のうちの少なくともいずれか一方でスキャンさせることにより、計測視野を確保するようにしている。つまり、広い視野の計測を行う場合には、遠方における計測点が疎になる。
また、ステレオカメラは、アレイ状に配置された受光素子から一度に得られた二枚以上のデジタル画像に基づいて、三角測量の原理により距離を算出する。つまり、一度に広い視野の計測が可能であるものの、測距については、精度が距離の二乗に比例して悪化し、加えて、計測対象のテクスチャに大きく依存するため、水平及び立体の両レーザレンジファインダと比較して精度及び信頼性が低い。

本発明に係る自律走行移動体の走行経路決定用地図作成装置では、これらのセンサ類の配置及びこれらのセンサ類から得られるデータの適切な処理を行うことにより、高速自律走行に適した走行経路決定用地図をシンプル且つ低コストで作成可能としている。
本発明の請求項１に係る自律走行移動体の走行経路決定用地図作成装置において、水平及び立体の両レーザレンジファインダは、移動体の車体に固定して設置する。ただし、そのままでは、両レーザレンジファインダの光軸方向が不明となり、移動体が揺れた場合には、どの方向を計測しているのかが判別できなくなるため、両レーザレンジファインダ近傍にリジッドに設置したバーチカルジャイロを用いて、両レーザレンジファインダの光軸のロール角度及びピッチ角度を計測し、座標変換によって鉛直方向上向きの軸を一軸とする機体座標系にセンサ座標系のデータを補正変換する。

一方、本発明の請求項２に係る自律走行移動体の走行経路決定用地図作成装置では、ジンバル機構等の水平保持手段により、鉛直方向上向きの軸を一軸とする機体座標系で水平及び立体の両レーザレンジファインダ並びにステレオカメラを水平に保つようにしている。
本発明に係る自律走行移動体の走行経路決定用地図作成装置において、水平スキャンレーザレンジファインダは、最高速度で走行する前記移動体の少なくとも急制動停止可能位置よりも前方の路面上で且つ当該移動体の進行方向に対して直交する方向に光軸を走査するようにしているが、光軸を遠方に走査するように成せばなるほど、車体の僅かな揺れにより大きく計測位置が変化してしまう。また、バーチカルジャイロを用いていると、このジャイロの精度誤差の影響も受けやすくなるので、水平スキャンレーザレンジファインダの光軸は、車体前方の中距離（２０ｍ程度）とする。

この水平スキャンレーザレンジファインダによれば、図５に示すように、車体Ｃの矢印方向への進行に伴って走査線Ｌも矢印方向へ前進することとなるので、光軸と前方の路面の交点付近を密に計測可能である。具体的には、スキャンレートが１００Ｈｚ、車速度が５０ｋｍ／ｈ(１３．８ｍ／ｓ)であれば、進行方向の計測密度は１３８ｍｍになるが、車体が一方向に直進する場合には、同一地点を一度しか計測することができない。

また、本発明に係る自律走行移動体の走行経路決定用地図作成装置において、立体スキャンレーザレンジファインダは、車体前方を広範囲で計測するもので、光軸スキャンの解像度は一定である。つまり、空間的に、数ｍの近傍では密なデータ取得が可能であるが、遠方では非常に疎なデータとなり、この立体スキャンレーザレンジファインダによれば、同一地点を複数回計測することが可能である。

これらのレーザレンジファインダの各データから走行の可否を判定する場合、すなわち、走行中に両レーザレンジファインダでそれぞれ取得された距離データから、正しい前方領域の三次元データを再構成するためには、レーザレンジファインダの位置の回転及び並進の６自由度を正しく計測する必要がある。
しかし、回転方向についてはジャイロの誤差により、また、並進方向についてはオドメトリやＧＰＳ（グローバル・ポジショニング・システム）の誤差により、センサ座標系から地上座標系への正しい座標変換が困難となり、その誤差が障害物検出精度に影響を与える。

したがって、本発明の請求項３に係る自律走行移動体の走行経路決定用地図作成装置では、上記問題を解決するために、両レーザレンジファインダにおいて、一回のスキャン分のデータに対してのみ解析を行うこととした。
両レーザレンジファインダのスキャンは、通常１００Ｈｚ（１０ｍｓｅｃ）程度で終了し、車体の揺動は数十°／秒であることから、車体の揺動がある場合であったとしても、一回のスキャン分のデータに含まれる相対的な距離値には車体の揺動の影響が少ないものとなる。

すなわち、本発明の請求項３に係る自律走行移動体の走行経路決定用地図作成装置において、前記水平及び立体の両レーザレンジファインダで得たセンサ座標系データを鉛直方向上向きの軸を一軸とする機体座標系に補正変換して、前記移動体の前方における段差領域の有無を判定処理するデータ解析部は、前記水平及び立体の両レーザレンジファインダの一回のスキャン分のセンサ座標系データを機体座標系に変換し、そのデータ中に前記移動体の走破特性を考慮した相対的な高さの連続性の変化及び勾配に閾値を設定し、前記水平及び立体の両レーザレンジファインダによる計測値が前記閾値を越えた場合を障害物領域として判定処理すると共に、前記水平及び立体の両レーザレンジファインダによる計測値が前記閾値を越えない場合を走行可能領域として判定処理し、前記水平及び立体の両レーザレンジファインダによる未計測部分を不明領域として判定処理するものとしてある構成としている。

ここで、ステレオカメラから得られる画像に対しては、車体前方の路面画像中から平面領域を抽出するアルゴリズムを適用する。これにより、車体前方５０ｍ程度までの平面領域の抽出が可能であり、これも上記立体スキャンレーザレンジファインダと同様に同一地点を複数回計測可能である。
なお、ステレオカメラ画像から平面を検出するアルゴリズムについては、ブロックマッチングで距離画像に変換した後、二次元ハフ変換によって平面抽出を行ってもよいほか、ステレオカメラ画像を同次変換してそれらのマッチングを取ることにより平面領域を抽出してもよい。また、色やテクスチャなどにより、車体近傍から連続的に広がる領域を平面領域としてもよい。

ステレオカメラの画像処理で抽出された平面領域は、走行可能とし、それ以外は不明とする。この際、ステレオカメラ画像を用いる手法は、一般的にレーザレンジファインダと比較して測距の精度及び信頼性が低い。
移動体の走行において、遠方の情報については、位置精度はそれほど重要ではなく、ステレカメラによるデータに基づいて、大まかに障害物の有無が認識できれば事足りるので、大まかな回避動作の準備や減速動作を行うことができる。

近傍の情報については、高い位置精度が必要とされるため、計測精度の良いレーザレンジファインダのデータが優先され、ステレオカメラの計測結果の誤差は除去されるので、高い精度での障害物回避が可能となる。
これらの手法は、走行する環境が静止している場合において最適化されているが、水平スキャンレーザレンジファインダは、同一地点を複数回計測することができないので、移動障害物が存在する場合において、水平スキャンレーザレンジファインダが移動障害物を検出した後に障害物が移動して、その移動後の領域が再び走行可能となったとしても、それが認識されない。これは一般的にゴーストと呼ばれる。

この際、ステレオカメラから得られるデータよりも、常にレーザレンジファインダのデータが優先されるため、ステレオカメラによるゴースト除去はできない。また、立体スキャンレーザレンジファインダは、同一地点を複数回計測することができるものの、遠方では計測密度が疎であることから、迅速にゴーストを除去することができない。
ただし、ゴーストが発生した領域に接近すると、計測点は密となるため、立体スキャンレーザレンジファインダであってもゴーストを除去することができるが、ゴースト領域には徐行で接近しなくてはならないため、運用効率が低下する。

そこで、水平スキャンレーザレンジファインダ及びステレオカメラからそれぞれ得られるデータに対して、一定の時間閾値より過去に計測された水平スキャンレーザレンジファインダのデータよりも、新しいステレオカメラによるデータを優先することとした。
このとき、水平スキャンレーザレンジファインダは計測精度が高く、１００ｍｍ以下の路面の縁石程度の障害物も検出可能であるが、ステレオカメラでは誤差に埋もれる恐れがある。したがって、移動障害物として想定し得る人や車輌の一般的な高さ１ｍを移動障害物の高さとして設定し、水平スキャンレーザレンジファインダにより計測された１ｍ以上の高さの障害物に対してのみ上記処理を行うこととする。

すなわち、本発明の請求項４に係る自律走行移動体の走行経路決定用地図作成装置において、前記地図作成部の地図作成モジュールは、前記外界測定部の水平スキャンレーザレンジファインダが高さ約１ｍの障害物を捕らえた場合において、前記水平スキャンレーザレンジファインダの過去の段差領域抽出データのうちの一定の時間閾値よりも過去に計測した段差領域抽出データよりも新しい前記ステレオカメラによる画像処理データを優先してベース地図を作成する構成としている。

これにより、最新データに対して水平スキャンレーザレンジファインダの信頼性の高さが反映され、また一定の時間が経過した後には、ステレオカメラによるデータにより更新可能となるため、移動障害物のゴースト除去がより効率よく行われることとなり、移動障害物が存在する環境下においても、シンプル且つ低コストな機器構成で、性能の高い自律走行に適した走行経路決定用地図を作成し得ることとなる。

なお、上記時間閾値は、それぞれのセンサの走行可否領域検出センサとしてのＳ／Ｎデータと、実際に移動障害物の計測を行った際の障害物位置と検出結果とを取得し、全てのセンサを統合した結果の障害物可否領域検出センサとしてのＳ／Ｎが最大となるよう決定する。
一方、本発明の請求項５に係る発明は、自律して走行する移動体の走行経路を決めるのに用いる地図の作成方法であって、水平スキャンレーザレンジファインダによって、最高速度で走行する前記移動体の少なくとも急制動停止可能位置よりも前方の路面上で且つ当該移動体の進行方向に対して直交する方向に光軸を走査すると共に、立体スキャンレーザレンジファインダにより、前記移動体の前方に光軸を走査しつつその走査方向と直交する方向に揺動させ、且つ、ステレオカメラで前記移動体近傍から遠方にかけての平面の領域を抽出し、前記水平及び立体の両レーザレンジファインダで得たセンサ座標系データを鉛直方向上向きの軸を一軸とする機体座標系に補正変換して、前記移動体の前方における段差領域の有無を判定処理すると共に、前記ステレオカメラで得た画像内の平面領域抽出処理及びセンサ座標系データを鉛直方向上向きの軸を一軸とする機体座標系に補正変換する平面法線補正処理を行い、これらの処理による段差領域抽出データ及び平面領域抽出データの地上座標系への座標変換により得た各種データに地上座標系自己位置データを加えた後、前記水平及び立体の両レーザレンジファインダによる段差領域抽出データと水平及び立体の両レーザレンジファインダの未計測点に対するステレオ画像による平面領域抽出データとを反映させつつ、前記水平及び立体の両レーザレンジファインダの段差領域抽出データは、過去のものは保持すると共にベース地図上の同一地点に新規の計測があった場合には最新のものに逐次更新し、前記ステレオカメラの画像処理データは常に最新のものに逐次更新してベース地図を作成し、このベース地図に基づいて前記移動体の走行経路を決める構成としている。

本発明の請求項６に係る自律走行移動体の走行経路決定用地図作成方法は、前記水平及び立体の両レーザレンジファインダで得たセンサ座標系データを鉛直方向上向きの軸を一軸とする機体座標系に補正変換して、前記移動体の前方における段差領域の有無を判定処理するに際して、前記水平及び立体の両レーザレンジファインダの一回のスキャン分のセンサ座標系データを機体座標系に変換し、そのデータ中に前記移動体の走破特性を考慮した相対的な高さの連続性の変化及び勾配に閾値を設定し、前記水平及び立体の両レーザレンジファインダによる計測値が前記閾値を越えた場合を障害物領域として判定処理すると共に、前記水平及び立体の両レーザレンジファインダによる計測値が前記閾値を越えない場合を走行可能領域として判定処理し、前記水平及び立体の両レーザレンジファインダによる未計測部分を不明領域として判定処理する構成としている。

本発明の請求項７に係る自律走行移動体の走行経路決定用地図作成方法は、前記水平スキャンレーザレンジファインダが高さ約１ｍの障害物を捕らえた場合において、前記水平スキャンレーザレンジファインダの過去の段差領域抽出データのうちの一定の時間閾値よりも過去に計測した段差領域抽出データよりも新しい前記ステレオカメラによる画像処理データを優先してベース地図を作成する構成としている。

本発明の請求項１，２に係る自律走行移動体の走行経路決定用地図作成装置及び請求項５に係る自律走行移動体の走行経路決定用地図作成方法では、それぞれ上記した構成としたから、シンプルで且つ低コストを実現したうえで、高速の自律走行に適した走行経路決定用地図を作成することが可能であるという非常に優れた効果がもたらされる。
また、本発明の請求項３に係る自律走行移動体の走行経路決定用地図作成装置及び請求項６に係る自律走行移動体の走行経路決定用地図作成方法では、それぞれ上記した構成としているので、ジャイロの誤差やＧＰＳの誤差にかかわらず高い精度で障害物を検出することが可能であるという非常に優れた効果がもたらされる。

さらに、本発明の請求項４に係る自律走行移動体の走行経路決定用地図作成装置及び請求項７に係る自律走行移動体の走行経路決定用地図作成方法では、それぞれ上記した構成としているので、移動障害物が存在する環境下においても、シンプル且つ低コストな機器構成で、性能の高い自律走行に適した走行経路決定用地図を作成することが可能であるという非常に優れた効果がもたらされる。

以下、本発明に係る自律走行移動体の走行経路決定用地図作成装置及び走行経路決定用地図作成方法を図面に基づいて説明する。
図１〜図８は、本発明に係る自律走行移動体の走行経路決定用地図作成装置の一実施形態を示しており、この実施形態では、自律走行移動体が自律走行車である場合を例に挙げて説明する。

図１及び図２に示すように、この走行経路決定用地図作成装置１は、最高速度（概ね時速５０ｋｍ／ｈ）で走行する自律走行車Ｃの少なくとも急制動停止可能位置よりも前方（自律走行車Ｃの前方２０ｍ付近）で且つこの自律走行車Ｃの進行方向に対して直交する方向に光軸を走査する水平スキャンレーザレンジファインダ１１，自律走行車Ｃの前方に光軸を走査しつつその走査方向と直交する方向に揺動させる立体スキャンレーザレンジファインダ１２及び自律走行車Ｃ近傍から遠方にかけての平面の領域を抽出するステレオカメラ１３を具備した外界計測部１０と、デッドレコニング用のホイルオドメータ２１，ヨーレートセンサ２２及びＧＰＳ２３を具備して自律走行車Ｃの自己位置と方向を求める自己位置計測部２０と、外界計測部１０で得たセンサ座標系外界データ及び自己位置計測部２０で得た地上座標系(自律走行車Ｃがスタートした位置及び方向を原点とする座標系) 自己位置データが入出力回路２を介して入力されるデータ解析部３０と、このデータ解析部３０とＬＡＮ３を介して接続する地図作成部４０を備えている。

この場合、外界計測部１０は自律走行車Ｃの前端部（図２左端部）に搭載され、一方、自己位置計測部２０はデータ解析部３０，地図作成部４０及び後述する車体制御部６０とともに自律走行車Ｃの後端部（図２右端部）に搭載されており、この自律走行車Ｃの後端部にはＧＰＳ２３用のアンテナ２４が配置してある。
そして、自律走行車Ｃにおける車体駆動部５０の操舵手段５１は、ドライバ５２及び入出力回路４を介して車体制御部６０と接続していると共に、車体駆動部５０の車速制御手段５３は、コンバータ５４及び入出力回路４を介して車体制御部６０と接続しており、この車体制御部６０には、地図作成部４０からの制御信号がＬＡＮ３を介して入力されるようになっている。

また、外界計測部１０のレーザレンジファインダ１１，１２及びステレオカメラ１３は、図４に示すように、レーザレンジファインダ１１，１２の各光軸のロール角度及びピッチ角度を計測するバーチカルジャイロ（補正手段）１４とともにベース１５上に固定してある。
上記データ解析部３０は、図３に示すように、外界計測部１０のレーザレンジファインダ１１，１２で得たセンサ座標系データをバーチカルジャイロ１４で計測した光軸のロール角度及びピッチ角度に基づいて鉛直方向上向きの軸を一軸とする機体座標系に補正変換する座標変換モジュール３１と、この座標変換モジュール３１で得た機体座標系データから自律走行車Ｃの前方における段差領域の有無を判定処理する段差抽出モジュール３２と、ステレオカメラ１３で得た画像内の平面領域抽出処理モジュール３３及びセンサ座標系データをバーチカルジャイロ１４で計測した光軸のロール角度及びピッチ角度に基づいて鉛直方向上向きの軸を一軸とする機体座標系に補正変換する平面法線補正処理モジュール３４を有している。

また、この走行経路決定用地図作成装置１において、図６及び図７に示すように、レーザレンジファインダ１１，１２で得たセンサ座標系データを鉛直方向上向きの軸を一軸とする機体座標系に補正変換して、自律走行車Ｃの前方における段差領域の有無を判定処理するデータ解析部３０は、レーザレンジファインダ１１，１２の一回のスキャン分のセンサ座標系データを機体座標系に変換し、そのデータ中に自律走行車Ｃの走破特性を考慮した相対的な高さの連続性の変化及び勾配に閾値を設定し、レーザレンジファインダ１１，１２による計測値が閾値を越えた場合を障害物領域（図７黒部分）として判定処理すると共に、レーザレンジファインダ１１，１２による計測値が閾値を越えない場合を走行可能領域（図７白部分）として判定処理し、レーザレンジファインダ１１，１２による未計測部分を不明領域（図７グレー部分）として判定処理するものとしてある。

さらに、上記地図作成部４０は、データ解析部３０から得た段差領域抽出データ及び平面領域抽出データの地上座標系への座標変換を行う座標変換モジュール４１と、この座標変換により得た段差領域抽出データ及び平面領域抽出データに自己位置計測部２０からの地上座標系自己位置データを加えてベース地図を作成する地図作成モジュール４２と、このベース地図に基づいて自律走行車Ｃの走行経路を決める走行経路決定モジュール４３を具備している。

この実施形態において、地図作成部４０の地図作成モジュール４２は、レーザレンジファインダ１１，１２による段差領域抽出データ及びレーザレンジファインダ１１，１２の未計測点に対するステレオカメラ１３の画像による平面領域抽出データを反映させたベース地図を作成し、レーザレンジファインダ１１の過去の段差領域抽出データは保持すると共に、レーザレンジファインダ１２の段差領域抽出データはベース地図上の同一地点に新規の計測があった場合に最新のものに逐次更し、ステレオカメラ１３の画像処理データは常に最新のものに逐次更新するものとしてある。

さらにまた、この走行経路決定用地図作成装置１において、地図作成部４０の地図作成モジュール４２は、図８に示すように、ステップＳ１で外界測定部１０の水平スキャンレーザレンジファインダ１１が高さ約１ｍの障害物を捕らえた場合に、ステップＳ２において一定の時間閾値（ｔ秒）よりも過去に計測した水平スキャンレーザレンジファインダ１１の段差領域抽出データよりも新しいステレオカメラ１３から出力された障害物なしの画像処理データを優先し、ステップＳ３においてｔ秒間のうちに水平スキャンレーザレンジファインダ１１が障害物を認識しないときは、ステップＳ４に進んでその領域は移動障害物の通過後の領域と判断して走行可能領域とし、ステップＳ３においてｔ秒間のうちに水平スキャンレーザレンジファインダ１１が障害物を認識したときは、ステップＳ５においてこの水平スキャンレーザレンジファインダ１１の処理データを優先してベース地図を作成するようにしている。

次に、上記した自律走行車Ｃの走行経路決定用地図の作成要領を説明する。
まず、自律走行車Ｃの停止時に、立体スキャンレーザレンジファインダ１２とステレオカメラ１３で前方の計測を行い、走行可能領域を検出する。
次いで、自律走行車Ｃの走行を開始すると、外界計測部１０の水平スキャンレーザレンジファインダ１１によって、最高速度で走行する自律走行車Ｃの少なくとも急制動停止可能位置よりも前方の路面上で且つ自律走行車Ｃの進行方向に対して直交する方向に光軸を走査すると共に、立体スキャンレーザレンジファインダ１２により、自律走行車Ｃの前方に光軸を走査しつつその走査方向と直交する方向に揺動させ、且つ、ステレオカメラ１３で自律走行車Ｃ近傍から遠方にかけての平面の領域を抽出する。

このとき、遠方については、ステレオカメラ１３により大まかな計測が可能であるため、走行可能領域を優先的に選択し、また不明領域が広がる場合には、あらかじめ車速を落として水平スキャンレーザレンジファインダ１１による計測に委ねる。
走行中、データ解析部３０では、レーザレンジファインダ１１，１２で得たセンサ座標系データを鉛直方向上向きの軸を一軸とする機体座標系に補正変換して、自律走行車Ｃの前方における段差領域の有無を判定処理すると共に、ステレオカメラ１３で得た画像内の平面領域抽出処理及びセンサ座標系データを鉛直方向上向きの軸を一軸とする機体座標系に補正変換する平面法線補正処理を行う。

このとき、データ解析部３０は、レーザレンジファインダ１１，１２の一回のスキャン分のセンサ座標系データを機体座標系に変換し、そのデータ中に自律走行車Ｃの走破特性を考慮した相対的な高さの連続性の変化及び勾配に閾値を設定し、レーザレンジファインダ１１，１２による計測値が閾値を越えた場合を障害物領域（図７黒部分）として判定処理すると共に、レーザレンジファインダ１１，１２による計測値が閾値を越えない場合を走行可能領域（図７白部分）として判定処理し、レーザレンジファインダ１１，１２による未計測部分を不明領域（図７グレー部分）として判定処理する。

地図作成部４０では、これらの処理による段差領域抽出データ及び平面領域抽出データの地上座標系への座標変換により得た各種データに地上座標系自己位置データを加えた後、レーザレンジファインダ１１，１２による段差領域抽出データ及びレーザレンジファインダ１１，１２の未計測点に対するステレオカメラ１３の画像による平面領域抽出データを反映させつつ、水平スキャンレーザレンジファインダ１１の過去の段差領域抽出データを保持すると共に、立体スキャンレーザレンジファインダ１２の段差領域抽出データ及びステレオカメラ１３の画像処理データをいずれも最新のものに逐次更新してベース地図を作成し、このベース地図に基づいて前記移動体の走行経路を決めると、自律走行車Ｃの走行経路Ｒに沿った自律走行が成されることとなる。

ここで、地図作成部４０において、図８に示すように、ステップＳ１で外界測定部１０の水平スキャンレーザレンジファインダ１１が高さ約１ｍの障害物を捕らえた場合には、ステップＳ２において一定の時間閾値（ｔ秒）よりも過去に計測した水平スキャンレーザレンジファインダ１１の段差領域抽出データよりも新しいステレオカメラ１３から出力された障害物なしの画像処理データを優先し、ステップＳ３においてｔ秒間のうちに水平スキャンレーザレンジファインダ１１が障害物を認識しないときは、ステップＳ４に進んでその領域は移動障害物の通過後の領域と判断して走行可能領域とし、ステップＳ３においてｔ秒間のうちに水平スキャンレーザレンジファインダ１１が障害物を認識したときは、ステップＳ５においてこの水平スキャンレーザレンジファインダ１１の処理データを優先してベース地図を作成する。

上記した一実施形態では、自律走行移動体が自律走行車である場合を示したが、これに限定されるものではない。

本発明に係る自律走行移動体の走行経路決定用地図作成装置の一実施形態を示すブロック図である。 図１における走行経路決定用地図作成装置を搭載した自律走行車の側面説明図である。 図１における走行経路決定用地図作成装置の地図作成フローを示すブロック図である。 図１における走行経路決定用地図作成装置の外界計測部のレーザレンジファインダ及びステレオカメラのレイアウトを示す斜視説明図である。 図１における走行経路決定用地図作成装置の外界計測部におけるレーザレンジファインダのスキャン動作を示す斜視説明図である。 図１における走行経路決定用地図作成装置の外界計測部におけるレーザレンジファインダスキャン動作及びステレオカメラの計測動作を示す平面説明図（ａ）及び側面説明図（ｂ）である。 図１における走行経路決定用地図作成装置のデータ解析部による判定処理の一例を示す説明図である。 図１における走行経路決定用地図作成装置の地図作成部による移動障害物を捕らえた場合の地図作成要領を示すフローチャートである。

符号の説明

１ 自律走行移動体の走行経路決定用地図作成装置
１０ 外界計測部
１１ 水平スキャンレーザレンジファインダ
１２ 立体スキャンレーザレンジファインダ
１３ ステレオカメラ
１４ バーチカルジャイロ（補正手段）
２０ 自己位置計測部
３０ データ解析部
４０ 地図作成部
４１ 座標変換モジュール
４２ 地図作成モジュール
４３ 走行経路決定モジュール
Ｃ 自律走行車（自律走行移動体）

Claims (7)

1. 自律して走行する移動体の走行経路を決めるのに用いる地図の作成装置であって、
   最高速度で走行する前記移動体の少なくとも急制動停止可能位置よりも前方の路面上で且つ当該移動体の進行方向に対して直交する方向に光軸を走査する水平スキャンレーザレンジファインダと、前記移動体の前方に光軸を走査しつつその走査方向と直交する方向に揺動させる立体スキャンレーザレンジファインダと、前記移動体近傍から遠方にかけての平面の領域を抽出するステレオカメラと、前記水平及び立体の両レーザレンジファインダ並びにステレオカメラの各光軸のロール角度及びピッチ角度を計測する補正手段を具備した外界計測部と、
   デッドレコニングなどの手段により前記移動体の自己位置と方向を求める自己位置計測部と、
   前記外界計測部の水平及び立体の両レーザレンジファインダで得たセンサ座標系データを前記補正手段で計測した光軸のロール角度及びピッチ角度に基づいて鉛直方向上向きの軸を一軸とする機体座標系に補正変換して、前記移動体の前方における段差領域の有無を判定処理すると共に、前記ステレオカメラで得た画像内の平面領域抽出処理及び前記補正手段で計測した光軸のロール角度及びピッチ角度に基づいて鉛直方向上向きの軸を一軸とする機体座標系に補正変換する平面法線補正処理を行うデータ解析部と、
   このデータ解析部から得た段差領域抽出データ及び平面領域抽出データの地上座標系への座標変換を行う座標変換モジュールと、この座標変換により得た段差領域抽出データ及び平面領域抽出データに前記自己位置計測部からの地上座標系自己位置データを加えてベース地図を作成する地図作成モジュールと、前記ベース地図に基づいて前記移動体の走行経路を決める走行経路決定モジュールを具備した地図作成部を備え、
   前記地図作成部の地図作成モジュールは、前記水平及び立体の両レーザレンジファインダによる段差領域抽出データと水平及び立体の両レーザレンジファインダの未計測点に対するステレオ画像による平面領域抽出データとを反映させたベース地図を作成し、前記水平及び立体の両レーザレンジファインダの段差領域抽出データは、過去のものは保持すると共にベース地図上の同一地点に新規の計測があった場合には最新のものに逐次更新し、前記ステレオカメラの画像処理データは常に最新のものに逐次更新するものとしてあることを特徴とする自律走行移動体の走行経路決定用地図作成装置。
2. 自律して走行する移動体の走行経路を決めるのに用いる地図の作成装置であって、
   最高速度で走行する前記移動体の少なくとも急制動停止可能位置よりも前方の路面上で且つ当該移動体の進行方向に対して直交する方向に光軸を走査する水平スキャンレーザレンジファインダと、前記移動体の前方に光軸を走査しつつその走査方向と直交する方向に揺動させる立体スキャンレーザレンジファインダと、前記移動体近傍から遠方にかけての平面の領域を抽出するステレオカメラと、鉛直方向上向きの軸を一軸とする機体座標系で前記水平及び立体の両レーザレンジファインダ並びにステレオカメラを水平に保つ水平保持手段を具備した外界計測部と、
   デッドレコニングなどの手段により前記移動体の自己位置を求める自己位置計測部と、
   前記外界計測部の水平保持手段により水平に保たれた水平及び立体の両レーザレンジファインダで得たセンサ座標系データを機体座標系に補正変換して、前記移動体の前方における段差領域の有無を判定処理すると共に、前記ステレオカメラで得た画像内の平面領域抽出処理を行うデータ解析部と、
   このデータ解析部から得た段差領域抽出データ及び平面領域抽出データの地上座標系への座標変換を行う座標変換モジュールと、この座標変換により得た段差領域抽出データ及び平面領域抽出データに前記自己位置計測部からの地上座標系自己位置データを加えてベース地図を作成する地図作成モジュールと、前記ベース地図に基づいて前記移動体の走行経路を決める走行経路決定モジュールを具備した地図作成部を備え、
   前記地図作成部の地図作成モジュールは、前記水平及び立体の両レーザレンジファインダによる段差領域抽出データと水平及び立体の両レーザレンジファインダの未計測点に対するステレオ画像による平面領域抽出データとを反映させたベース地図を作成し、前記水平及び立体の両レーザレンジファインダの段差領域抽出データは、過去のものは保持すると共にベース地図上の同一地点に新規の計測があった場合には最新のものに逐次更新し、前記ステレオカメラの画像処理データは常に最新のものに逐次更新するものとしてあることを特徴とする自律走行移動体の走行経路決定用地図作成装置。
3. 前記水平及び立体の両レーザレンジファインダで得たセンサ座標系データを鉛直方向上向きの軸を一軸とする機体座標系に補正変換して、前記移動体の前方における段差領域の有無を判定処理するデータ解析部は、
   前記水平及び立体の両レーザレンジファインダの一回のスキャン分のセンサ座標系データを機体座標系に変換し、そのデータ中に前記移動体の走破特性を考慮した相対的な高さの連続性の変化及び勾配に閾値を設定し、前記水平及び立体の両レーザレンジファインダによる計測値が前記閾値を越えた場合を障害物領域として判定処理すると共に、前記水平及び立体の両レーザレンジファインダによる計測値が前記閾値を越えない場合を走行可能領域として判定処理し、前記水平及び立体の両レーザレンジファインダによる未計測部分を不明領域として判定処理するものとしてある請求項１又は２に記載の自律走行移動体の走行経路決定用地図作成装置。
4. 前記地図作成部の地図作成モジュールは、前記外界測定部の水平スキャンレーザレンジファインダが高さ約１ｍの障害物を捕らえた場合において、前記水平スキャンレーザレンジファインダの過去の段差領域抽出データのうちの一定の時間閾値よりも過去に計測した段差領域抽出データよりも新しい前記ステレオカメラによる画像処理データを優先してベース地図を作成する請求項１〜３のいずれか一つの項に記載の自律走行移動体の走行経路決定用地図作成装置。
5. 自律して走行する移動体の走行経路を決めるのに用いる地図の作成方法であって、
   水平スキャンレーザレンジファインダによって、最高速度で走行する前記移動体の少なくとも急制動停止可能位置よりも前方の路面上で且つ当該移動体の進行方向に対して直交する方向に光軸を走査すると共に、立体スキャンレーザレンジファインダにより、前記移動体の前方に光軸を走査しつつその走査方向と直交する方向に揺動させ、且つ、ステレオカメラで前記移動体近傍から遠方にかけての平面の領域を抽出し、
   前記水平及び立体の両レーザレンジファインダで得たセンサ座標系データを鉛直方向上向きの軸を一軸とする機体座標系に補正変換して、前記移動体の前方における段差領域の有無を判定処理すると共に、前記ステレオカメラで得た画像内の平面領域抽出処理及びセンサ座標系データを鉛直方向上向きの軸を一軸とする機体座標系に補正変換する平面法線補正処理を行い、
   これらの処理による段差領域抽出データ及び平面領域抽出データの地上座標系への座標変換により得た各種データに地上座標系自己位置データを加えた後、
   前記水平及び立体の両レーザレンジファインダによる段差領域抽出データと水平及び立体の両レーザレンジファインダの未計測点に対するステレオ画像による平面領域抽出データとを反映させつつ、前記水平及び立体の両レーザレンジファインダ段差領域抽出データは、過去のものは保持すると共にベース地図上の同一地点に新規の計測があった場合には最新のものに逐次更新し、前記ステレオカメラの画像処理データを常に最新のものに逐次更新してベース地図を作成し、
   このベース地図に基づいて前記移動体の走行経路を決めることを特徴とする自律走行移動体の走行経路決定用地図作成方法。
6. 前記水平及び立体の両レーザレンジファインダで得たセンサ座標系データを鉛直方向上向きの軸を一軸とする機体座標系に補正変換して、前記移動体の前方における段差領域の有無を判定処理するに際して、
   前記水平及び立体の両レーザレンジファインダの一回のスキャン分のセンサ座標系データを機体座標系に変換し、そのデータ中に前記移動体の走破特性を考慮した相対的な高さの連続性の変化及び勾配に閾値を設定し、前記水平及び立体の両レーザレンジファインダによる計測値が前記閾値を越えた場合を障害物領域として判定処理すると共に、前記水平及び立体の両レーザレンジファインダによる計測値が前記閾値を越えない場合を走行可能領域として判定処理し、前記水平及び立体の両レーザレンジファインダによる未計測部分を不明領域として判定処理する請求項５に記載の自律走行移動体の走行経路決定用地図作成方法。
7. 前記水平スキャンレーザレンジファインダが高さ約１ｍの障害物を捕らえた場合において、前記水平スキャンレーザレンジファインダの過去の段差領域抽出データのうちの一定の時間閾値よりも過去に計測した段差領域抽出データよりも新しい前記ステレオカメラによる画像処理データを優先してベース地図を作成する請求項５又は６に記載の自律走行移動体の走行経路決定用地図作成方法。