JP6559535B2

JP6559535B2 - 障害物マップ生成装置、その方法、及び、そのプログラム

Info

Publication number: JP6559535B2
Application number: JP2015208195A
Authority: JP
Inventors: 雅起山崎; 豪田崎; 理絵香月; 友樹渡辺; 関根　真弘; 真弘関根; 学西山
Original assignee: Toshiba Corp
Current assignee: Toshiba Corp
Priority date: 2015-10-22
Filing date: 2015-10-22
Publication date: 2019-08-14
Anticipated expiration: 2035-10-22
Also published as: JP2017083919A; US10354150B2; US20170116487A1

Description

本発明の実施形態は、障害物マップ生成装置、その方法、及び、そのプログラムに関する。

自動車の自動運転支援システムを実現するためには、自車の周辺の環境を解析し、自車の経路を生成することが必要となる。この経路を生成するためには、ステレオカメラ、レーザセンサなどのデータから、歩行者、他の車両などの障害物を検出し、自車の周辺をグリッド状に分割し、各グリッドにおける障害物を時系列的に表現した障害物マップ（Occupancy Grid Map）を利用する。

特許第４６５０７５０号公報

しかし、上記障害物マップに関して、障害物マップ全体の単位当たりのグリッド数を増やして、解像度を上げることは、処理時間が長くなり、メモリ量も増加するという問題点があった。

そこで本発明の実施形態は、上記問題点を解決するために、障害物マップの中で重要な範囲だけ解像度を上げることができる障害物マップ生成装置、その方法、及び、そのプログラムを提供することを目的とする。

本発明の実施形態は、２次元のグリッドで構成されたピクセル直交座標系上の第１移動体を中心とした障害物マップを所定時刻毎に生成する障害物マップ生成装置であって、前記ピクセル直交座標系は、２次元平面の前記障害物マップに時刻Ｔ毎に設定される２次元直交座標系であってｐ軸とｑ軸からなり、前記ピクセル直交座標系の原点は、第１移動体に設定し、ｐ軸方向は、ある時刻における第１移動体の走行方向に設定され、
カメラ座標系は、前記ピクセル直交座標系の解像度より解像度が高い３次元直交座標系であって、横方向のＸ軸、縦方向のＹ軸、奥行き方向のＺ軸からなり、前記カメラ座標系の原点の位置が、前記第１移動体に設定し、また、前記ピクセル直交座標系の前記ｐ軸と前記Ｚ軸が一致するように設定され、前記カメラ座標系上において、前記第１移動体から障害物までの現時刻の距離データを取得する距離データ取得部と、前記距離データから前記障害物の前記現時刻の次の時刻における位置を予測する予測部と、前記現時刻における前記第１移動体の移動状態を示す第１情報を取得し、前記第１情報から、前記現時刻における前記第１移動体の動作モードを決定する決定部と、前記動作モードから、前記第１移動体の前記次の時刻における移動範囲を求め、前記次の時刻における前記移動範囲と、前記次の時刻における前記障害物の予測した位置を含む注目範囲を、前記カメラ座標系の前記解像度と同じ解像度で設定する範囲設定部と、前記障害物マップと前記注目範囲とを位置合わせし、前記次の時刻の前記障害物マップにおける前記距離データに対応した前記グリッドと、前記次の時刻の前記注目範囲における前記距離データに対応した前記グリッドとに、前記障害物が存在するように生成する生成部と、を有する障害物マップ生成装置である。

また、本発明の実施形態は、３次元のグリッドで構成されたボクセル直交座標系上の第１移動体を中心とした障害物マップを所定時刻毎に生成する障害物マップ生成装置であって、前記ボクセル直交座標系は、３次元平面の前記障害物マップに時刻Ｔ毎に設定される３次元直交座標系であってｐ軸とｑ軸とｒ軸からなり、前記ボクセル直交座標系の原点は、第１移動体に設定し、ｐ軸方向は、ある時刻における第１移動体の移動方向に設定され、カメラ座標系は、前記ボクセル直交座標系の解像度より解像度が高い３次元直交座標系であって、横方向のＸ軸、縦方向のＹ軸、奥行き方向のＺ軸からなり、前記カメラ座標系の原点の位置が、前記第１移動体に設定し、また、前記ボクセル直交座標系の前記ｐ軸と前記Ｚ軸が一致するように設定され、前記カメラ座標系上において、前記第１移動体から障害物までの現時刻の距離データを取得する距離データ取得部と、前記距離データから前記障害物の前記現時刻の次の時刻における位置を予測する予測部と、前記現時刻における前記第１移動体の移動状態を示す第１情報を取得し、前記第１情報から、前記現時刻における前記第１移動体の動作モードを決定する決定部と、前記動作モードから、前記第１移動体の前記次の時刻における移動範囲を求め、前記次の時刻における前記移動範囲と、前記次の時刻における前記障害物の予測した位置を含む注目範囲を、前記カメラ座標系の前記解像度と同じ解像度で設定する範囲設定部と、前記障害物マップと前記注目範囲とを位置合わせし、前記次の時刻の前記障害物マップにおける前記距離データに対応した前記グリッドと、前記次の時刻の前記注目範囲における前記距離データに対応した前記グリッドとに、前記障害物が存在するように生成する生成部と、を有する障害物マップ生成装置である。

実施形態１に係る障害物マップ生成装置を表すブロック図。障害物マップ生成装置１の処理を表すフローチャート。カメラ座標系の説明図。画像座標系の説明図。自車の位置姿勢を算出する説明図。経路候補Ｃ１の説明図。経路候補Ｃ２の説明図。経路候補Ｃ３の説明図。経路候補Ｃ４の説明図。経路候補Ｃ５の説明図。経路候補Ｃ６の説明図。経路候補Ｃ７の説明図。移動先Ｄの説明図。環境情報の説明図。障害物マップＭの説明図。実施形態２に係る障害物マップ生成装置を表すブロック図。障害物マップの説明図。障害物マップ生成装置のハードウェア構成を示すブロック図。

以下、本発明の一実施形態の障害物マップ生成装置１について図面に基づいて説明する。

実施形態１の障害物マップ生成装置１は、２次元平面を移動する自動車のような移動体の経路（走路）に関する障害物マップを生成するものである。障害物マップは、２次元平面を格子状のグリッド（ピクセル）で分割され、各ピクセル単位で障害物の有無が表現されている。

実施形態２の障害物マップ生成装置１は、３次元空間を移動するＵＡＶ（無人航空機：Unmanned aerial vehicle）のような移動体の経路（航路）に関する障害物マップを生成するものである。障害物マップは、３次元空間を立方体、直方体形状の格子状のグリッド（ボクセル）で分割され、各ボクセル単位で障害物の有無が表現されている。

実施形態１

実施形態１の障害物マップ生成装置１について図１〜図１６に基づいて説明する。本実施形態では、第１移動体としては、自動車（以下、「自車」という）２であり、障害物マップ生成装置１は、自車２の周辺の２次元平面の障害物マップＭを生成する。すなわち、図６〜図１２に示すように、障害物マップ生成装置１は、ピクセル状に分割された障害物マップＭの中心に自車２が常に存在するようにして、各ピクセル単位で障害物の有無が表現された障害物マップＭを時刻Ｔ毎に生成する。

障害物マップ生成装置１の構成について図１のブロック図に基づいて説明する。障害物マップ生成装置１は、距離データ取得部１１、動き算出部１２、決定部１３、環境取得部１４、予測部１５、範囲設定部１６、生成部１７とを備えている。

自車２には、撮像機器としてステレオカメラ３、レーザセンサ４が設置されている。

距離データ取得部１１は、ステレオカメラ３から得られる左右画像７Ｌ、７Ｒ、レーザセンサ４より得られる自車２の周辺にある物体までの３次元的な距離データを取得する。

動き算出部１２は、距離データ取得部１１で取得された左右画像７Ｌ、７Ｒや距離データより、自車２の動きを算出する。

決定部１３は、自車２の第１情報である「自情報」を取得し、自車２の動作モードを決定する。

環境取得部１４は、自車２が走行している経路に関する環境情報を取得する。

予測部１５は、自車２の周辺にある第２移動体である「他移動体」（他車や歩行者など）の数秒後の移動先Ｄを予測する。

範囲設定部１６は、決定部１３が決定した動作モード、予測部１５が予測した他移動体の移動先Ｄ、環境取得部１４が取得した環境情報より、経路候補Ｃ、高い解像度の注目範囲Ａを設定する。

生成部１７は、距離データ取得部１１が取得した３次元的な距離データ、動き算出部１２から取得した自車２の姿勢と実スケールの移動状態より、自車２を中心とした障害物マップＭを生成し、注目範囲Ａを設定する。

障害物マップ生成装置１の処理について図２のフローチャートに基づいて説明する。

ステップＳ１において、距離データ取得部１１は、自車２に設置されたステレオカメラ３から左右画像７Ｌ、７Ｒ、レーザセンサ４より自車２の周辺にある物体（障害物）までの３次元的な距離データを取得する。

ステップＳ２において、動き算出部１２は、自車２の動き（姿勢と実スケールの移動状態）を算出する。

ステップＳ３において、決定部１３は、自情報を取得し、自車２の動作モードを決定する。

ステップＳ４において、環境取得部１４は、自車２の経路に関する環境情報を取得する。

ステップＳ５において、予測部１５は、自車２の周辺にある他移動体の数秒（例えば、１〜３秒）後の移動先Ｄを予測する。

ステップＳ６において、範囲設定部１６は、決定部１３が決定した動作モード、予測部１５が予測した他移動体の移動先Ｄ、環境取得部１４が取得した環境情報より、自車２の経路候補Ｃ、高い解像度の注目範囲Ａを算出する。

ステップＳ７において、生成部１７は、距離データ取得部１１が取得した距離データ、動き算出部１２が取得した自車２の姿勢、高い解像度の注目範囲Ａより、自車２を中心とした障害物マップＭを生成する。

ステップＳ８において、生成部１７は、障害物マップＭを次の時刻に更新する。

ステップＳ９において、自車２が目的地に到着した場合には、障害物マップＭの更新を終了し、到着していない場合にはステップＳ１に戻る。

距離データ取得部１１について説明する。距離データ取得部１１は、自車２に設置されたステレオカメラ３、レーザセンサ４より、自車２の周辺にある物体（障害物）までの距離データを取得する（図２のステップＳ１参照）。ステレオカメラ３は、左カメラ３Ｌと右カメラ３Ｒからなる。

ステレオカメラ３の焦点距離データなどを算出する校正、ステレオカメラ３の左右カメラ３Ｌ、３Ｒ間の相対的な位置と姿勢を算出する校正、ステレオカメラ３とレーザセンサ４との間の位置関係（回転と並進）を算出する校正などは、予め行われているものとする。

ここで、本実施形態で用いられる座標系は、ピクセル直交座標系、カメラ座標系、画像座標系の３種類である。

ピクセル直交座標系は、２次元平面の障害物マップＭに時刻Ｔ毎に設定される２次元直交座標系であってｐ軸とｑ軸からなる。ピクセル直交座標系の原点は、自車２の中心に設定し、ｐ軸方向は、時刻Ｔにおける自車２の走行方向に設定する。

図３に示すように、カメラ座標系は、３次元直交座標系であって、横方向のＸ軸、縦方向のＹ軸、奥行き方向のＺ軸からなる。カメラ座標系の原点の位置が、自車２の中心位置とし、また、ピクセル直交座標系のｐ軸（進行方向）とＺ軸が一致するように校正しておく。

図４に示すように、画像座標系は、左右カメラ３Ｌ、３Ｒのそれぞれで撮像した左右画像７Ｌ、７Ｒ上の横方向のｘ軸、縦方向のｙ軸からなる２次元直交座標系である。

ステレオカメラ３について説明する。距離データ取得部１１は、ステレオカメラ３から得られる左右画像７Ｌ、７Ｒから自車２の周辺にある物体までの３次元的な距離データを取得する。ステレオカメラ３は、自車２の進行方向を撮像するのが望ましいが、校正を行うため、ステレオカメラ３の設置する位置や角度に対する制約はない。ステレオカメラ３は、可視光を受光できるデジタルカメラ、又は、赤外線を受光できるデジタルカメラなどである。

距離データ取得部１１は、ステレオカメラ３から３次元的な距離データを取得するには、次のように行う。まず、ステレオカメラ３から得られる左右画像７Ｌ、７Ｒ間で特徴点の対応位置の探索を行う。対応位置の探索は、特徴点の回りに小領域を設定して、左右画像７Ｌ、７Ｒの輝度パタンで類似度をＳＳＤ（Sum of Squared Difference）などによって評価することで求める。次に、ステレオカメラ３の左右カメラ３Ｌ、３Ｒ間の相対的な位置と姿勢が予め校正されているため、左右画像７Ｌ、７Ｒ上の特徴点の位置関係と左右カメラ３Ｌ、３Ｒの空間的な位置関係を用いて特徴点の３次元位置を求める。

レーザセンサ４について説明する。距離データ取得部１１は、レーザセンサ４からレーザ光を照射し、障害物で反射したレーザ光の反射光を測定することにより得られた３次元的な距離データを取得する。レーザセンサ４は、自車２の進行方向を照射するのが望ましい。但し、自車２にレーザセンサ４の設置する位置や角度に対する制約はない。

レーザセンサ４としては、例えば、レーザ光を照射してから障害物に反射し帰ってくるまでの時間を計測して距離データを算出する時間差方式、複数に変調させたレーザ光を照射し対象物に当たって戻ってきた拡散反射成分の位相差により対象物との距離データを求める位相差方式などがある。そのほかの距離センサとして、ミリ波を用いたミリ波レーダなどがある。

動き算出部１２について説明する。動き算出部１２は、ステレオカメラ３が取得した左右画像７Ｌ、７Ｒより自車２の動き（姿勢と実スケールの移動状態）を算出する（図２のステップＳ２参照）。

「姿勢」とは、自車２の回転行列Ｒを意味する。「回転行列Ｒ」とは、例えば、自車２が時刻Ｔ０から時刻Ｔ１において、右折、又は、左折した場合に発生する方位を意味する。

「実スケールの移動状態」とは、実スケール化した並進ベクトルｔを意味する。時刻Ｔ１の自車２の位置ベクトルｖ１と時刻Ｔ２の位置ベクトルｖ２の差を示すベクトルが並進ベクトルｔとなり、その移動距離（並進ベクトルのスカラー量）を実スケール（現実の距離）で表現したものである。

なお、最初の回転行列は単位行列、並進ベクトルは零ベクトルとする。

動き算出部１２が、自車２の姿勢と実スケールの移動状態を算出する方法について説明する。その方法は、大きく２つの処理からなる。

動き算出部１２が行う第１処理では、カメラ座標系における特徴点８の３次元位置を求める。なお、初期時刻Ｔ０において、動き算出部１２は、自車２の姿勢と実スケールの移動状態を何も取得していないとする。

第１処理の第１段階において、図５（ａ）に示すように、ステレオカメラ３の左右カメラ３Ｌ、３Ｒが、例えば、３次元形状が立方体の物体５を撮像する。

第１処理の第２段階において、図５（ｂ）に示すように、動き算出部１２は、左右画像７Ｌａ、７Ｒａから特徴点８Ｌａ、８Ｒａを抽出する。ここで特徴点８Ｌａ、８Ｒａとしては、物体５の角部とする。なお、特徴点８Ｌａ、８Ｒａは、画像の一部に集中することがないように、すなわち、特徴点の一定範囲内で他の特徴点を検出しないようにすることが望ましい。

第１処理の第３段階において、動き算出部１２は、左右画像７Ｌａ、７Ｒａ間で特徴点８Ｌａ、８Ｒａの対応位置の探索を行う。対応位置の探索は、特徴点８の回りに小領域を設定し、左右画像７Ｌａ、７Ｒａにおける小領域の輝度パタンをＳＳＤによって評価し、左右の小領域に関して一定値以上の類似度であれば、左右の小領域内に特徴点８Ｌａ、８Ｒａがあるとする。

第１処理の第４段階において、動き算出部１２は、特徴点８Ｌａ、８Ｒａの対応関係と、左右カメラ３Ｌ、３Ｒ間の位置関係から三角測量の原理によって、カメラ座標系における特徴点８の３次元位置を算出する。ステレオカメラ３は、左右カメラ３Ｌ、３Ｒ間の相対的な位置と姿勢が予め校正されているため、左右画像７Ｌ、７Ｒのそれぞれの画像座標系における特徴点８Ｌａ、８Ｒａの位置と、左右カメラ３Ｌ、３Ｒの空間的な位置関係を用いて、カメラ座標系における特徴点８の３次元位置を求める。

動き算出部１２が行う第２処理は、左画像７Ｌｂの画像座標系における特徴点８の画像位置、右画像７Ｒｂの画像座標系における特徴点８の画像位置と、第１処理で求めたカメラ座標系における特徴点８の３次元位置に基づいて、時刻Ｔ１における自車２の姿勢と実スケールの移動状態を推定する。また、動き算出部１２は、この第１の処理と第２の処理を繰り返し行うことで、時刻Ｔ毎の自車２の姿勢と実スケールの移動状態を推定する。但し、時刻Ｔに関して、Ｔ０＜Ｔ１である。

第２処理の第１段階として、図５（ｃ）に示すように、動き算出部１２には、初期時刻Ｔ０に続けて時刻Ｔ１で左右画像７Ｌｂ、７Ｒｂが入力する。

第２処理の第２段階として、動き算出部１２は、時刻Ｔ１で入力した左右画像７Ｌｂ、７Ｒｂに対して、第１処理で検出した初期時刻Ｔ０における特徴点８Ｌａ、８Ｒａと一致する特徴点８Ｌｂ、８Ｒｂを検出して、左右画像７Ｌｂ、７Ｒｂの画像座標系における左右画像座標値をそれぞれ求める。

第２処理の第３段階として、検出した特徴点８Ｌｂ、８Ｒｂの左右画像７Ｌｂ、７Ｒｂの画像座標系における左右画像座標値に基づいて、時刻Ｔ１における左右画像７Ｌｂ、７Ｒｂの位置の対応付けを行う（特徴点追跡処理）。なお、動き算出部１２は、時刻Ｔ１において、時刻Ｔ０からステレオカメラ３が大きく動いていない場合には、時刻Ｔ０の左右画像７Ｌａ、７Ｒａで検出した特徴点８Ｌａ、８Ｒａの周囲を探索することで、特徴点８Ｌｂ、８Ｒｂを追跡できる。

第２処理の第４段階として、時刻Ｔ１における左右画像７Ｌｂ、７Ｒｂの位置の対応付けができたため、動き算出部１２は、時刻Ｔ１の左右画像７Ｌｂ、７Ｒｂで検出した特徴点８Ｌｂ、８Ｒｂのカメラ座標系における特徴点８の３次元位置を検出する。

第２処理の第５段階として、動き算出部１２は、下記の式（１）のコスト関数Ｃを用いて、自車２の回転行列Ｒと並進ベクトルｔを推定する。このコスト関数Ｃは、検出した特徴点の３次元位置を、仮定の回転行列Ｒと仮定の並進ベクトルｔを用いて左右画像７Ｌ、７Ｒの中の一方の画像（以下、「基準画像」という）に投影し、その投影した位置と、基準画像上での特徴点の画像座標値との差分が小さくなるように、仮定の回転行列Ｒと仮定の並進ベクトルｔを変化させて最適な回転行列Ｒと並進ベクトルｔを求めるように設計されている。

ここで、ｘｉは検出したｉ番目の特徴点の基準画像上での画像座標値、Ｐ（Ｒ，ｔ）は透視投影行列を表しており、自車２の回転行列Ｒと並進ベクトルｔが含まれる。Ｘｉは、特徴点に関して３次元のカメラ座標系の３次元位置（座標値）で表現したものである。このコスト関数Ｃを最小化するために非線形最適化を行い、回転行列Ｒと並進ベクトルｔを求める。

決定部１３について説明する。決定部１３は、時刻Ｔ１における自車２の自情報を取得し、自車２の動作モードを決定する（図２のステップＳ３参照）。

第１に、決定部１３は、時刻Ｔ１における自車２の自情報を取得する。「自情報」とは、自車２の移動状態を示す情報であり、自車２の車速、ブレーキの動作状態、左右のウィンカー情報（左右のウィンカーの点滅、又は、消灯状態）、バック（後方直進）をしているか否かの情報、ハザードの点消灯状態などの情報がある。これらの自情報は、ＣＡＮ（車載ネットワーク）などを使って取得する。

第２に、決定部１３は、自情報より自車２が例えば以下の７つのどの動作モードに該当するかを決定する。

動作モード１は、自車２が停止の状態である、
動作モード２は、自車２が停止、かつ、右折ウィンカーの点滅の状態である、
動作モード３は、自車２が停止、かつ、左折ウィンカーの点滅の状態である、
動作モード４は、自車２が前進の状態である、
動作モード５は、自車２が前進、かつ、右折ウィンカーの点滅の状態である、
動作モード６は、自車２が前進、かつ、左折ウィンカーの点滅の状態である、
動作モード７は、自車２が後退の状態である。

決定部１３は、時刻Ｔ１における自車２の動作状態が停止、前方直進、後方直進のどの状態に属するかは、次のように決定する。

まず、決定部１３は、時刻Ｔ１における自車２の車速が０に限りなく近い場合、自車２が停止していると決定でき、自情報のウィンカー情報（左右のウィンカーの点滅状態、又は、消灯状態）、又は、バック（後方直進）の情報があるか否かで動作モード１〜３のどれかであるかを決定できる。

次に、決定部１３は、時刻Ｔ１における自車２の車速が０でない場合、自車２が直進していると決定でき、自情報のウィンカー情報、又は、バックの情報があるか否かで動作モード４〜７のどれかであるかを決定できる。

環境取得部１４について説明する。環境取得部１４は、時刻Ｔ１における自車２が走行している経路（道路）の環境情報を取得して、環境情報で示される環境物体６の３次元位置をカメラ座標系で表現して記憶する（図２のステップＳ４参照）。

「環境情報」としては、自車２が走行している道路の道路標識６Ａの有無、信号機６Ｂの点灯状態、白線６Ｃの有無、ガードレール６Ｄの有無、ガイドポスト６Ｅの有無、横断歩道６Ｆの有無などがある。道路標識６Ａは、道路の傍らに設置され、交通規制などを示す掲示板の内容である。ガードレール６Ｄは、道路の路肩や歩道との境界、中央分離帯などに設けられる。ガイドポスト６Ｅは車道と歩道の区切り（車止め）や、車線と車線の区切り部分に設置されている。

「環境物体６」とは、環境情報に示された道路標識６Ａ、信号機６Ｂ、白線６Ｃ、ガードレール６Ｄ、ガイドポスト６Ｅ、横断歩道６Ｆなどをいう。

環境取得部１４は、これらの環境情報を、カーナビなどの通信機器を使って取得するか、又は、道路上に設置された無線機器、車間通信機器、自車２に設置されたステレオカメラ３が取得した画像から画像認識技術を使って取得する。

環境取得部１４は、時刻Ｔ１における動き算出部１２が取得した自車２の姿勢と実スケールの移動状態を基準として、環境情報で示される環境物体６までの３次元的な距離データをカメラ座標系において求め、その距離データに対応した３次元位置に環境物体６を設定し、その３次元位置をカメラ座標系で表現して記憶する。環境物体６までの３次元的な距離データの求め方は、距離データ取得部１１で取得した自車２の周辺の物体までの３次元的な距離データと、環境情報で示される環境物体６とが一致した場合に、その一致した物体までの３次元的な距離データを使用してもよい。

予測部１５について説明する。予測部１５は、時刻Ｔ１における自車２の周辺を移動している他移動体（他車や歩行者など）の（Ｔ１−Ｔ０）秒後の移動先Ｄを予測して、その移動先Ｄをカメラ座標系で表現して記憶する（図２のステップＳ５参照）。

予測部１５が、時刻Ｔ１における他移動体（他車や歩行者など）の（Ｔ２−Ｔ１）秒後の移動先Ｄを予測するためには、自車２に設置されたステレオカメラ３、レーザセンサ４などの各時刻における他移動体の位置を追跡する必要がある。

予測部１５が、他移動体の位置を追跡するには、道路上に設置された無線機器、車間通信機器や、自車２に設置されたステレオカメラ３、レーザセンサ４が取得したデータから画像認識技術などを用いて追跡する。すなわち、予測部１５は、動き算出部１２が取得した自車２の姿勢と実スケールの移動状態を基準として、時刻Ｔ１における他移動体の位置、姿勢、速度ベクトルを取得し、それに基づいて（Ｔ１−Ｔ０）秒後の他移動体の位置、姿勢、速度ベクトルを求めて他移動体の移動先Ｄを求め、その移動先Ｄをカメラ座標系で表現して記憶する。

範囲設定部１６について説明する。範囲設定部１６は、決定部１３が決定した動作モード１〜７、予測部１７が予測した他移動体の移動先Ｄ、環境取得部１４が取得した環境情報に基づいて、時刻Ｔ１における自車２の経路候補Ｃと注目範囲Ａを、カメラ座標系における３次元範囲で表現して設定する（図２のステップＳ６参照）。

まず、経路候補Ｃについて説明する。範囲設定部１６は、決定部１３が決定した動作モードを用いて、時刻Ｔ１における自車２の経路候補Ｃをカメラ座標系における３次元範囲で表現して設定する。この経路候補Ｃは、障害物マップＭと同様にピクセル状に分割され、１ピクセルの大きさは、障害物マップＭのピクセルと同じ大きさに設定されている。

図６に示すように、時刻Ｔ１において動作モード１の場合、範囲設定部１６は、自車２を中心とした一定の前後範囲ｐＭ１、左右範囲ｑＭ１内に経路候補Ｃ１を設定する。自車２を中心とした前後範囲ｐＭ１、左右範囲ｑＭ１の大きさは、障害物マップＭで表されるピクセル直交座標系におけるピクセルの大きさ（前後左右のピクセルサイズ）で表現する。

図７に示すように、時刻Ｔ１において動作モード２の場合、範囲設定部１６は、自車２を中心とした前方右側の一定の前方範囲ｐＭ２、右範囲ｑＭ２内に経路候補Ｃ２を設定する。

図８に示すように、時刻Ｔ１において動作モード３の場合、自車２を中心とした前方左側の一定の前方範囲ｐＭ３、左範囲ｑＭ３内に経路候補Ｃ３を設定する。

図９に示すように、時刻Ｔ１において動作モード４の場合、範囲設定部１６は、自車２を中心としたの前方範囲ｐＭ４、左右範囲ｑＭ４内に経路候補Ｃ４を設定する。

図１０に示すように、時刻Ｔ１において動作モード５の場合、範囲設定部１６は、自車２を中心とした前方右側の一定の前方範囲ｐＭ５、右範囲ｑＭ５内に経路候補Ｃ５を設定する。自車２が前方直進しているため、経路候補Ｃ５は、経路候補Ｃ２と比べてより広い範囲となる。

図１１に示すように、時刻Ｔ１において動作モード６の場合、範囲設定部１６は、自車２を中心とした前方左側の一定の前方範囲ｐＭ６、左範囲ｑＭ６内に経路候補Ｃ６を設定する。自車２が前方直進しているため、経路候補Ｃ６は、経路候補Ｃ６と比べてより広い範囲となる。

図１２に示すように、時刻Ｔ１において動作モード７の場合、範囲設定部１６は、自車２を中心とした後方左右の一定の後方範囲ｐＭ７、左右範囲ｑＭ７内に経路候補Ｃ７を設定する。

ここでは動作モードによる経路候補設定法について記載したが、自動運転車のようにシステムが経路をすでに算出している場合は、動作モードを使用せず、システムがたどろうとしている経路が存在する範囲を経路候補として設定することもできる。

次に、注目範囲Ａについて説明する。範囲設定部１６は、時刻Ｔ１における経路候補Ｃと予測部１５が予測した他移動体の移動先Ｄ、環境取得部１４が取得した環境情報より、時刻Ｔ１における自車２の注目範囲Ａをカメラ座標系における３次元範囲で表現して設定する。

図１３に示すように、範囲設定部１６は、時刻Ｔ１における経路候補Ｃの端から一定の距離データ以内（第１閾値）に他移動体の移動先Ｄがある場合は、他移動体の移動先Ｄまで含めた範囲を注目範囲Ａとしてカメラ座標系に表現して設定する。

図１４に示すように、範囲設定部１６は、時刻Ｔ１における経路候補Ｃの端から一定の距離データ以内（第２閾値）に環境情報の環境物体６がある場合は、環境情報の環境物体６まで含めた範囲を注目範囲Ａとしてカメラ座標系における３次元範囲で表現して設定する。

図１６に示すように、範囲設定部１６は、この注目範囲Ａを障害物マップＭのピクセルの解像度より高解像度で設定する。例えば、障害物マップＭのピクセルサイズ（１．０ｍ）とすると、注目範囲Ａは、より小さいピクセルサイズ（０．５ｍ）で設定する。この小さいピクセルサイズで注目範囲Ａを設定できる理由は、予測部１５が予測した他移動体の移動先Ｄ、環境取得部１４が取得した環境情報がカメラ座標系における距離データで高い解像度を有するからである。

生成部１７について説明する。生成部１７は、時刻Ｔ１における距離データ取得部１１が取得した距離データ、動き算出部１２が算出した自車２の姿勢と実スケールの移動状態、範囲設定部１６が設定した注目範囲Ａより、時刻Ｔ１における自車２の周辺の障害物マップＭを生成する（図２のステップＳ７参照）。

まず、生成部１７は、時刻Ｔ１におけるカメラ座標系の３次元位置や３次元範囲で表現された他移動体の移動先Ｄ、環境情報で示された環境物体６、経路候補Ｃ、注目範囲Ａを障害物マップＭの各ピクセルに設定する。この場合に、障害物マップＭにおけるピクセル直交座標系の原点をカメラ座標系の原点に合わせ、かつ、ピクセル直交座標系のｐ軸とカメラ座標系のＺ軸を合わせる。そして、例えば、環境物体６の場合には、環境物体６のカメラ座標系における３次元位置（Ｘｋ，Ｙｋ，Ｚｋ）に対応するピクセル直交座標系におけるピクセル（ｐｋ、ｑｚ）に、環境物体６を設定する。なお、カメラ座標系の縦方向の成分Ｙｋについては、例えば、Ｙｋ＝０に設定する。これにより、時刻Ｔ１における自車２を中心として他移動体の移動先Ｄ、環境物体６、経路候補Ｃ、注目範囲Ａが、障害物マップＭの各ピクセルに設定できる。

次に、生成部１７は、距離データ取得部１１が取得した距離データから、障害物マップＭの各ピクセル、注目範囲Ａの高解像度の各ピクセルにおける障害物の存在確率（０〜１の間の確率値、以下、「障害物存在確率」という）を時系列的（時刻Ｔ毎）に計算する。

図１５に示すように、生成部１７は、障害物マップＭの各ピクセル、注目範囲Ａの高解像度の各ピクセルを移動可領域（障害物が無の区画）、移動不可領域（障害物が有の区画）、未知領域（障害物の有無の不明の区画）の３つの状態へ障害物マップＭの各ピクセル、注目範囲Ａの高解像度の各ピクセルを分類する。なお、移動可領域の障害物存在確率は１．０付近、未知領域は０．５付近、移動不可領域は０．０付近とする。以下、その分類方法について説明する。

第１に、生成部１７は、自車２に搭載されたレーザセンサ４を用いて時刻Ｔ毎で得られる周辺の物体までの距離データ（観測値）を、障害物マップＭのピクセル直交座標系に変換する。なお、この変換のときは、注目範囲Ａのみを高い解像度で変換する。注目範囲Ａのみを高い解像度で変換できる理由は、予測部１５が予測した他移動体の移動先Ｄ、環境取得部１４が取得した環境情報がカメラ座標系における距離データで高い解像度を有するからである。

第２に、生成部１７は、障害物マップＭの各ピクセル、注目範囲Ａの高解像度の各ピクセルに移動可領域（障害物が無の区画）、移動不可領域（障害物が有の区画）、未知領域（障害物の有無の不明区画）を生成する（レイキャスティング処理）。移動可領域は、障害物マップＭの中心にある自車２（レーザセンサ４）から距離データが取得できた物体の直前までのピクセルの領域である。移動不可領域は、レーザセンサ４から取得した距離データに対応するピクセルの領域である。未知領域は、移動不可領域より先のピクセルの領域である。生成部１７は、レーザセンサ４の角度解像度毎の対応するピクセルのインデックスは事前（プレキャスト処理）に求めておき、それを使ってピクセルを更新する。プレキャスティング処理としては、予め全てのピクセルを探索して、レーザセンサ４の角度解像度毎のビンの対応するピクセルのインデックスを、レーザセンサ４の中心からの距離データでソートしておいた状態でデータベースに追加する。ピクセルのインデックスを登録したデータベースを使うことで、ピクセルを毎回探索する必要がなくなる。予め求めない場合は、レーザセンサ４毎の極座標系でレイキャスティング処理を行い、その結果をピクセル直交座標系に変換してもよい。

第３に、生成部１７は、レーザセンサ４が観測した距離データより、障害物マップＭのピクセル単位、注目範囲Ａの高い解像度のピクセル単位で障害物存在確率を計算する。その計算方法は、距離データによる誤差分散に基づいて、計算した障害物存在確率を近傍ピクセルに伝搬させる。実際の処理は、光線方向上の距離データをガウス分布に当てはめて尤度を計算する。また、複数のレーザセンサ４が同時に距離データを観測したピクセルに関しては、お互いに計算した障害物存在確率を統合する。統合方法として、誤差が小さいレーザセンサ４の確率を採用する。他移動体は、予測部１５が予測した他移動体の移動先Ｄへ障害物存在確率を遷移させる方法などがある。

第４に、生成部１７は、動き算出部１２が算出した自車２の姿勢と実スケールの移動状態に基づき、時刻Ｔ１における自車２を中心とした障害物マップＭ、注目範囲Ａを、次の時刻Ｔ２における自車２を中心とした障害物マップＭ、注目範囲Ａに座標変換する（図２のステップＳ８参照）。この座標変換は、ピクセル直交座標系で行う。

生成部１７は、この座標変換した時刻Ｔ２における障害物マップＭの各ピクセルの障害物存在確率、注目範囲Ａの解像度の高い各ピクセルの障害物存在確率を、ベイズの定理で計算し更新する。この更新をするため、時刻Ｔ１で観測した障害物存在確率を時刻Ｔ２の障害物マップＭ、注目範囲Ａへ伝播する。障害物が存在するとして観測された事象をＯで表現する。「移動不可領域」とラベル付けされたピクセルでは事象Ｏが起こる。「移動可領域」とラベル付けされたピクセルでは事象Ｏの補集合が起こる。「未知領域」とラベル付けされたピクセルでは、時刻Ｔ１の観測から何の情報も得られないので、事象Ｏも事象Ｏの補集合も起こらない。具体的には、以下の方法で障害物マップＭ、注目範囲Ａを更新する。

まず、障害物が存在するという事象をＥで表現し、障害物マップＭのピクセル、注目範囲Ａの高い解像度のピクセルにおける障害物存在確率をＰ（Ｅ）で表現する。障害物マップＭ、注目範囲Ａは以下の条件付き確率を計算することで更新する。

これらの確率は、ベイズの定理を用いて式（２）のように計算する。

ここで、Ｐ（Ｅ）は事前確率で、初期値は０．５とする。

ベイズの定理での更新法は、時系列として一時刻前の確率値だけを用いて更新する手法でもよいし、複数時刻にわたった確率変動をもとにベイズ推定する手法でもよい。時系列の確率値をベイズの定理で更新する一般的な手法を用いることができる。

生成部１７は、時刻Ｔ１の障害物マップＭのピクセル毎、注目範囲Ａの高い解像度のピクセル毎に障害物存在確率を独立して更新し（独立仮定）、時刻Ｔ２の障害物マップＭ、注目範囲Ａを求める。

第５において、生成部１７は、自車２が目的地に到着した場合には、障害物マップＭの更新を終了する（図２のステップＳ９参照）。

本実施形態の障害物マップ生成装置１は、ピクセル状の障害物マップＭを生成する場合に、自車２の自情報、他移動体の予測した移動先Ｄ、環境情報で示された環境物体６に応じて自車２の経路に重要な注目範囲Ａを、障害物マップＭのピクセルより高い解像度で生成できる。そのため、自車２の走行において、より確実に障害物を避けることができる。

実施形態２

次に、実施形態２の障害物マップ生成装置１００について図１６と図１７に基づいて説明する。本実施形態では、第１移動体は、３次元空間を移動するＵＡＶ（無人航空機：Unmanned aerial vehicle）のような飛行体（以下、「自飛行体」という）１０２であり、図１７に示すように、障害物マップＭは、３次元空間を立方体、直方体形状の格子状のボクセルで区切った際に、ボクセル単位で障害物の有無を表現する。

本実施形態の障害物マップ生成装置１００は、図１６のブロック図に示すように、距離データ取得部１１１、動き算出部１１２、決定部１１３、環境取得部１１４、予測部１１５、範囲設定部１１６、生成部１１７とを備えている。以下の説明では、実施形態１と異なる構成、作用だけ説明する。

自飛行体１０２は、ステレオカメラ１０３、投影式の距離データセンサ１０４、自飛行体１０２の３次元的な位置姿勢を取得する位置姿勢センサ１０５を有する。位置姿勢センサ１０５の一例としては、次の通りである。ステレオカメラ１０３を使用している場合は、ステレオカメラ１０３の時系列処理によって３次元的な位置姿勢を計算する。その他には、ＧＰＳ(Global Positioning System)、ＧＮＳＳ(Global Navigation satellite System) ,高度計などの位置センサ、ＩＭＵ（慣性計測装置：Inertial Measurement Unit）、地磁気などの姿勢センサを組み合わせることで３次元位置姿勢を取得する。

ボクセル直交座標系は、図１７に示すように、３次元平面の障害物マップＭに時刻Ｔ毎に設定される３次元直交座標系であってｐ軸とｑ軸とｒ軸からなる。ボクセル直交座標系の原点は、自飛行体１０２の中心に設定し、ｐ軸方向は自飛行体１０４の移動方向、ｒ軸方向は、時刻Ｔにおける自飛行体１０２の高さ方向に設定する。

カメラ座標系は、ステレオカメラ１０３の間の位置を原点に設定した３次元直交座標系であって、横方向のＸ軸、縦方向のＹ軸、奥行き方向のＺ軸からなる。この原点の位置が、自飛行体１０２の中心位置となる。Ｚ軸が自飛行体１０２の移動方向となる。

距離データ取得部１１１は、ステレオカメラ３、投影式の距離データセンサ１０４から自飛行体１０２の３６０°周囲にある物体（障害物）の画像群、その物体までのカメラ座標系における３次元的な距離データを取得する。

動き算出部１１２は、位置姿勢センサ１０５から自飛行体１０２の３次元的な位置と姿勢と移動状態を算出する。

決定部１１３は、自飛行体１０２がマニュアル飛行している時は、外部にある操縦者が有するコントローラからの指令である自情報に基づいて、カメラ座標系における自飛行体１０２の移動方向を示す「３次元移動ベクトル」を計算し、範囲設定部１１６へ出力する。自飛行体１０２が自動飛行をしている時は、予め入力された経路（航路）情報である自情報に基づいて、決定部１１３は、カメラ座標系における自飛行体１０２の３次元移動ベクトルを計算する。

環境取得部１１４は、カメラ座標系における環境情報を取得して予測部１１５に出力する。本実施形態の「環境情報」は、航空法などに示される自飛行体１０２周囲で飛行可能な高度情報や飛行可能空域情報、避けなければならない電線、広告塔などの航路を制限する障害物、規則の情報（公共物の上空の飛行禁止）などである。

予測部１１５は、距離データ取得部１１１が取得したカメラ座標系における物体の画像群、その物体までの３次元的な距離データに基づき、カメラ座標系における立体物（障害物）を算出する。この立体物を時系列で追跡したときに得られる３次元的な軌跡から、追跡された立体物の次の時刻の３次元位置をカルマンフィルタで予測する。予測部１１５は、予測された立体物の３次元位置を範囲設定部１１６に出力する。時系列の情報からの予測方法としては、これ以外に、パーティクルフィルタを用いたり、単純に等速直線運動、等加速度運動などを仮定した線形予測を用いたりしてもよい。立体物の予測された３次元位置が、環境情報に基づく航路を制限する領域に入っている場合は、制限されない領域の中で、予測された立体物の３次元位置から最近傍の３次元位置を、立体物の新たな移動先Ｄとして用いる。ここでは、一例を示したが、予測された３次元位置から制限されていない領域へと回避する航路を予測する方法であれば、その他の方法で代用することもできる。

範囲設定部１１６は、決定部１１３が決定した自飛行体１０２の３次元的移動ベクトルに基づくカメラ座標系における３次元的な移動位置、環境取得部１１４が取得したカメラ座標系における環境情報、予測部１１５が予測した障害物のカメラ座標系における３次元的な移動先Ｄより、カメラ座標系における３次元的な注目範囲を設定する。このとき、範囲設定部１１６は、注目範囲Ａのボクセルサイズを障害物マップＭのボクセルより小さくし、ボクセルの数を増加させた高解像度で設定する。

生成部１１７は、カメラ座標系における環境情報、障害物の３次元的な移動先Ｄ、３次元的な注目範囲Ａを、障害物マップＭに設定する。このとき、カメラ座標系における原点と、ボクセル直交座標系の原点を一致させ、Ｚ軸とｐ軸を一致させる。

本実施形態の障害物マップ生成装置１００によれば、自飛行体１０２の航路（経路）の重要範囲に関して、注目範囲Ａで障害物マップＭを生成できる。

変更例

図１８は、障害物マップ生成装置１、１００のハードウェア構成の一例を表すブロック図である。図１８に示すように、障害物マップ生成装置１、１００は、ＣＰＵ２０１と、障害物マップ生成プログラムなどを記憶するＲＯＭ２０２と、ＲＡＭ２０３と、ＨＤＤ(Hard Disk Drive)２０４と、ＨＤＤ２０４とのインターフェイスであるＩ／Ｆ２０５と、ステレオカメラ３からの画像、レーザセンサ４から距離データの入力用のインターフェイスであるＩ／Ｆ２０６と、マウスやキーボードなどの入力装置２０７と、入力装置２０７とのインターフェイスであるＩ／Ｆ２０６と、ディスプレイなどの表示装置２０８と、表示装置２０８とのインターフェイスであるＩ／Ｆ２１０と、バス２１１とを備えており、通常のコンピュータを利用したハードウェア構成となっている。

障害物マップ生成装置１では、ＣＰＵ２０１がＲＯＭ２０２から障害物マップ生成プログラムをＲＡＭ２０３上に読み出して実行することにより、上記各部（距離データ取得部１１、１１１、動き算出部１２、１１２、決定部１３、１１３、環境取得部１４、１１４、予測部１５、１１５、範囲設定部１６、１１６、生成部１７、１１７）がコンピュータ上で実現され、ＨＤＤ２０４に記憶されているデータ等を用いて、Ｉ／Ｆ２０６からの上記処理を行う。

なお、障害物マップ生成プログラムはＨＤＤ２０４に記憶されていてもよい。また、障害物マップ生成プログラムは、インストール可能な形式又は実行可能な形式のファイルでＣＤ(Compact Disk)−ＲＯＭ、ＣＤ−Ｒ、メモリカード、ＤＶＤ(Digital Versatile Disk)、フレキシブルディスク（ＦＤ）、ＵＳＢメモリ等のコンピュータで読み取り可能な記憶媒体に記憶されて提供されるようにしてもよい。また、プログラムを、インターネット等のネットワークに接続されたコンピュータ上に格納し、ネットワーク経由でダウンロードさせることにより提供するようにしてもよい。また、計測プログラムを、インターネット等のネットワーク経由で提供または配布するようにしてもよい。

また、上記コンピュータに限らず、障害物マップ生成装置１は、システムＬＳＩ(LargeScale Integration)で実現してもよく、ＣＰＵ（Central Processing Unit）と、障害物マップ生成プログラムを記憶するＲＯＭ（Read Only Memory)）と、ＲＡＭ(Random Access Memory)と、ステレオカメラ３からの画像、レーザセンサ４から距離データの入力用のインターフェイスであるＩ／Ｆと、生成した障害物マップＭの情報を外部へ出力する出力用Ｉ／Ｆと、これら部品を接続するバスとを備えた構成でもよい。

上記実施形態では、撮像機器としてステレオカメラ３を自車２に搭載したが、これに限らず３６０°全方位カメラ、１台のカメラでもよい。

また、上記実施形態では、レーザセンサ４は１台であったが、１ラインの水平ラインレーザセンサ４複数台を用いてもよいし、３６０°複数ラインを同時に取得できる３６０°ライダレーダセンサを用いてもよい。カメラ、レーザは、他の距離計測センサで代用することもできる。

また、上記実施形態では、ステレオカメラ３が撮像した左右画像７Ｌ、７Ｒより自車２の姿勢と実スケールの移動状態を算出したが、１台のカメラの場合は、並進ベクトルｔの実スケールが未知のため、レーザセンサ４やＩＭＵ（加速度、ジャイロなど）などの別のセンサから実スケールを取得する。また、実スケールは、ＩＭＵやＧＰＳで算出してもよい。

これまで、本発明のいくつかの実施形態を説明したが、これらの実施形態は、例として提示したものであり、発明の範囲を限定することは意図していない。これら新規な実施形態は、その他の様々な形態で実施されることが可能であり、発明の要旨を逸脱しない範囲で、種々の省略、置き換え、変更を行うことができる。これら実施形態やその変形は、発明の範囲や要旨に含まれると共に、特許請求の範囲に記載された発明とその均等の範囲に含まれる。

１・・・障害物マップ生成装置、２・・・自車、３・・・ステレオカメラ、４・・・レーザセンサ、５・・・物体、６・・・環境物体６、７・・・画像、８・・・特徴点、１１・・・距離データ取得部、１２・・・動き算出部、１３・・・決定部、１４・・・環境取得部、１５・・・予測部、１６・・・範囲設定部、１７・・・生成部

Claims

２次元のグリッドで構成されたピクセル直交座標系上の第１移動体を中心とした障害物マップを所定時刻毎に生成する障害物マップ生成装置であって、
前記ピクセル直交座標系は、２次元平面の前記障害物マップに時刻Ｔ毎に設定される２次元直交座標系であってｐ軸とｑ軸からなり、前記ピクセル直交座標系の原点は、第１移動体に設定し、ｐ軸方向は、ある時刻における第１移動体の走行方向に設定され、
カメラ座標系は、前記ピクセル直交座標系の解像度より解像度が高い３次元直交座標系であって、横方向のＸ軸、縦方向のＹ軸、奥行き方向のＺ軸からなり、前記カメラ座標系の原点の位置が、前記第１移動体に設定し、また、前記ピクセル直交座標系の前記ｐ軸と前記Ｚ軸が一致するように設定され、
前記カメラ座標系上において、前記第１移動体から障害物までの現時刻の距離データを取得する距離データ取得部と、
前記距離データから前記障害物の前記現時刻の次の時刻における位置を予測する予測部と、
前記現時刻における前記第１移動体の移動状態を示す第１情報を取得し、前記第１情報から、前記現時刻における前記第１移動体の動作モードを決定する決定部と、
前記動作モードから、前記第１移動体の前記次の時刻における移動範囲を求め、前記次の時刻における前記移動範囲と、前記次の時刻における前記障害物の予測した位置を含む注目範囲を、前記カメラ座標系の前記解像度と同じ解像度で設定する範囲設定部と、
前記障害物マップと前記注目範囲とを位置合わせし、前記次の時刻の前記障害物マップにおける前記距離データに対応した前記グリッドと、前記次の時刻の前記注目範囲における前記距離データに対応した前記グリッドとに、前記障害物が存在するように生成する生成部と、
を有する障害物マップ生成装置。
前記第１移動体の前記現時刻における動きを算出する動き算出部をさらに有し、
前記生成部は、前記第１移動体の動きに基づいて、前記現在時刻における前記障害物マップと前記注目範囲を前記次の時刻における前記障害物マップと前記注目範囲に更新する、
請求項１に記載の障害物マップ生成装置。
前記第１移動体の経路の環境情報を取得する環境取得部をさらに有し、
前記生成部は、前記距離データにより表現された前記障害物に加えて、前記障害物マップの前記グリッドと前記注目範囲の前記グリッドに、前記環境情報で示された環境物体を前記障害物マップ上に生成する、
請求項１又は２に記載の障害物マップ生成装置。
前記予測部は、第１移動体の周辺を移動している第２移動体である前記障害物の次の時刻の移動先を予測する、
請求項１乃至３のいずれか一項に記載の障害物マップ生成装置。
前記決定部が決定する動作モードが、前記第１移動体の停止状態の動作モード、右折のための停止状態の動作モード、左折のための停止状態の動作モード、前方直進の動作モード、右折のための前方直進の動作モード、左折のための前方直進の動作モード、又は、後方直進の動作モードである、
請求項１乃至４のいずれか一項に記載の障害物マップ生成装置。
前記決定部が決定する動作モードが、前記第１移動体の停止状態の動作モード、右折のための停止状態の動作モード、左折のための停止状態の動作モード、前方直進の動作モード、右折のための前方直進の動作モード、左折のための前方直進の動作モード、又は、後方直進の動作モードであり、
前記範囲設定部は、前記動作モードを用いて、前記第１移動体が移動する範囲の候補である経路候補を生成し、前記経路候補に前記環境情報で示された前記環境物体を含めて前記注目範囲を設定する、
ことを特徴とする請求項３に記載の障害物マップ生成装置。
前記決定部が決定する動作モードが、前記第１移動体の停止状態の動作モード、右折のための停止状態の動作モード、左折のための停止状態の動作モード、前方直進の動作モード、右折のための前方直進の動作モード、左折のための前方直進の動作モード、又は、後方直進の動作モードであり、
前記範囲設定部は、前記動作モードを用いて、前記第１移動体が移動する範囲の候補である経路候補を生成し、前記経路候補に前記移動先を含めて前記注目範囲を設定する、
ことを特徴とする請求項４に記載の障害物マップ生成装置。
２次元のグリッドで構成されたピクセル直交座標系上の第１移動体を中心とした障害物マップを所定時刻毎に生成する障害物マップ生成方法であって、
前記ピクセル直交座標系は、２次元平面の前記障害物マップに時刻Ｔ毎に設定される２次元直交座標系であってｐ軸とｑ軸からなり、前記ピクセル直交座標系の原点は、第１移動体に設定し、ｐ軸方向は、ある時刻における第１移動体の走行方向に設定され、
カメラ座標系は、前記ピクセル直交座標系の解像度より解像度が高い３次元直交座標系であって、横方向のＸ軸、縦方向のＹ軸、奥行き方向のＺ軸からなり、前記カメラ座標系の原点の位置が、前記第１移動体に設定し、また、前記ピクセル直交座標系の前記ｐ軸と前記Ｚ軸が一致するように設定され、
前記カメラ座標系上において、前記第１移動体から障害物までの現時刻の距離データを取得する距離データ取得ステップと、
前記距離データから前記障害物の前記現時刻の次の時刻における位置を予測する予測ステップと、
前記現時刻における前記第１移動体の移動状態を示す第１情報を取得し、前記第１情報から、前記現時刻における前記第１移動体の動作モードを決定する決定ステップと、
前記動作モードから、前記第１移動体の前記次の時刻における移動範囲を求め、前記次の時刻における前記移動範囲と、前記次の時刻における前記障害物の予測した位置を含む注目範囲を、前記カメラ座標系の前記解像度と同じ解像度で設定する範囲設定ステップと、
前記障害物マップと前記注目範囲とを位置合わせし、前記次の時刻の前記障害物マップにおける前記距離データに対応した前記グリッドと、前記次の時刻の前記注目範囲における前記距離データに対応した前記グリッドとに、前記障害物が存在するように生成する生成ステップと、
を有する障害物マップ生成方法。
２次元のグリッドで構成されたピクセル直交座標系上の第１移動体を中心とした障害物マップを所定時刻毎に生成する障害物マップ生成プログラムであって、
前記ピクセル直交座標系は、２次元平面の前記障害物マップに時刻Ｔ毎に設定される２次元直交座標系であってｐ軸とｑ軸からなり、前記ピクセル直交座標系の原点は、第１移動体に設定し、ｐ軸方向は、ある時刻における第１移動体の走行方向に設定され、
カメラ座標系は、前記ピクセル直交座標系の解像度より解像度が高い３次元直交座標系であって、横方向のＸ軸、縦方向のＹ軸、奥行き方向のＺ軸からなり、前記カメラ座標系の原点の位置が、前記第１移動体に設定し、また、前記ピクセル直交座標系の前記ｐ軸と前記Ｚ軸が一致するように設定され、
前記カメラ座標系上において、前記第１移動体から障害物までの現時刻の距離データを取得する距離データ取得機能と、
前記距離データから前記障害物の前記現時刻の次の時刻における位置を予測する予測機能と、
前記現時刻における前記第１移動体の移動状態を示す第１情報を取得し、前記第１情報から、前記現時刻における前記第１移動体の動作モードを決定する決定機能と、
前記動作モードから、前記第１移動体の前記次の時刻における移動範囲を求め、前記次の時刻における前記移動範囲と、前記次の時刻における前記障害物の予測した位置を含む注目範囲を、前記カメラ座標系の前記解像度と同じ解像度で設定する範囲設定機能と、
前記障害物マップと前記注目範囲とを位置合わせし、前記次の時刻の前記障害物マップにおける前記距離データに対応した前記グリッドと、前記次の時刻の前記注目範囲における前記距離データに対応した前記グリッドとに、前記障害物が存在するように生成する生成機能と、
をコンピュータによって実現させるための障害物マップ生成プログラム。
３次元のグリッドで構成されたボクセル直交座標系上の第１移動体を中心とした障害物マップを所定時刻毎に生成する障害物マップ生成装置であって、
前記ボクセル直交座標系は、３次元平面の前記障害物マップに時刻Ｔ毎に設定される３次元直交座標系であってｐ軸とｑ軸とｒ軸からなり、前記ボクセル直交座標系の原点は、第１移動体に設定し、ｐ軸方向は、ある時刻における第１移動体の移動方向に設定され、
カメラ座標系は、前記ボクセル直交座標系の解像度より解像度が高い３次元直交座標系であって、横方向のＸ軸、縦方向のＹ軸、奥行き方向のＺ軸からなり、前記カメラ座標系の原点の位置が、前記第１移動体に設定し、また、前記ボクセル直交座標系の前記ｐ軸と前記Ｚ軸が一致するように設定され、
前記カメラ座標系上において、前記第１移動体から障害物までの現時刻の距離データを取得する距離データ取得部と、
前記距離データから前記障害物の前記現時刻の次の時刻における位置を予測する予測部と、
前記現時刻における前記第１移動体の移動状態を示す第１情報を取得し、前記第１情報から、前記現時刻における前記第１移動体の動作モードを決定する決定部と、
前記動作モードから、前記第１移動体の前記次の時刻における移動範囲を求め、前記次の時刻における前記移動範囲と、前記次の時刻における前記障害物の予測した位置を含む注目範囲を、前記カメラ座標系の前記解像度と同じ解像度で設定する範囲設定部と、
前記障害物マップと前記注目範囲とを位置合わせし、前記次の時刻の前記障害物マップにおける前記距離データに対応した前記グリッドと、前記次の時刻の前記注目範囲における前記距離データに対応した前記グリッドとに、前記障害物が存在するように生成する生成部と、
を有する障害物マップ生成装置。
３次元のグリッドで構成されたボクセル直交座標系上の第１移動体を中心とした障害物マップを所定時刻毎に生成する障害物マップ生成方法であって、
前記ボクセル直交座標系は、３次元平面の前記障害物マップに時刻Ｔ毎に設定される３次元直交座標系であってｐ軸とｑ軸とｒ軸からなり、前記ボクセル直交座標系の原点は、第１移動体に設定し、ｐ軸方向は、ある時刻における第１移動体の移動方向に設定され、
カメラ座標系は、前記ボクセル直交座標系の解像度より解像度が高い３次元直交座標系であって、横方向のＸ軸、縦方向のＹ軸、奥行き方向のＺ軸からなり、前記カメラ座標系の原点の位置が、前記第１移動体に設定し、また、前記ボクセル直交座標系の前記ｐ軸と前記Ｚ軸が一致するように設定され、
前記カメラ座標系上において、前記第１移動体から障害物までの現時刻の距離データを取得する距離データ取得ステップと、
前記距離データから前記障害物の前記現時刻の次の時刻における位置を予測する予測ステップと、
前記現時刻における前記第１移動体の移動状態を示す第１情報を取得し、前記第１情報から、前記現時刻における前記第１移動体の動作モードを決定する決定ステップと、
前記動作モードから、前記第１移動体の前記次の時刻における移動範囲を求め、前記次の時刻における前記移動範囲と、前記次の時刻における前記障害物の予測した位置を含む注目範囲を、前記カメラ座標系の前記解像度と同じ解像度で設定する範囲設定ステップと、
前記障害物マップと前記注目範囲とを位置合わせし、前記次の時刻の前記障害物マップにおける前記距離データに対応した前記グリッドと、前記次の時刻の前記注目範囲における前記距離データに対応した前記グリッドとに、前記障害物が存在するように生成する生成ステップと、
を有する障害物マップ生成方法。
３次元のグリッドで構成されたボクセル直交座標系上の第１移動体を中心とした障害物マップを所定時刻毎に生成する障害物マップ生成プログラムであって、
前記ボクセル直交座標系は、３次元平面の前記障害物マップに時刻Ｔ毎に設定される３次元直交座標系であってｐ軸とｑ軸とｒ軸からなり、前記ボクセル直交座標系の原点は、第１移動体に設定し、ｐ軸方向は、ある時刻における第１移動体の移動方向に設定され、
カメラ座標系は、前記ボクセル直交座標系の解像度より解像度が高い３次元直交座標系であって、横方向のＸ軸、縦方向のＹ軸、奥行き方向のＺ軸からなり、前記カメラ座標系の原点の位置が、前記第１移動体に設定し、また、前記ボクセル直交座標系の前記ｐ軸と前記Ｚ軸が一致するように設定され、
前記カメラ座標系上において、前記第１移動体から障害物までの現時刻の距離データを取得する距離データ取得機能と、
前記距離データから前記障害物の前記現時刻の次の時刻における位置を予測する予測機能と、
前記現時刻における前記第１移動体の移動状態を示す第１情報を取得し、前記第１情報から、前記現時刻における前記第１移動体の動作モードを決定する決定機能と、
前記動作モードから、前記第１移動体の前記次の時刻における移動範囲を求め、前記次の時刻における前記移動範囲と、前記次の時刻における前記障害物の予測した位置を含む注目範囲を、前記カメラ座標系の前記解像度と同じ解像度で設定する範囲設定機能と、
前記障害物マップと前記注目範囲とを位置合わせし、前記次の時刻の前記障害物マップにおける前記距離データに対応した前記グリッドと、前記次の時刻の前記注目範囲における前記距離データに対応した前記グリッドとに、前記障害物が存在するように生成する生成機能と、
をコンピュータによって実現させるための障害物マップ生成プログラム。