TOP > 国内特許検索 > 障害物検出装置 > 明細書

明細書 :障害物検出装置

発行国 日本国特許庁(JP)
公報種別 特許公報(B2)
特許番号 特許第6218209号 (P6218209)
公開番号 特開2013-254474 (P2013-254474A)
登録日 平成29年10月6日(2017.10.6)
発行日 平成29年10月25日(2017.10.25)
公開日 平成25年12月19日(2013.12.19)
発明の名称または考案の名称 障害物検出装置
国際特許分類 G08G   1/16        (2006.01)
G08G   1/005       (2006.01)
A61G   1/00        (2006.01)
B60R  21/00        (2006.01)
G01C   3/06        (2006.01)
G06T   1/00        (2006.01)
G01S  17/93        (2006.01)
A61G   5/00        (2006.01)
FI G08G 1/16 C
G08G 1/005
A61G 1/00
B60R 21/00 624D
B60R 21/00 624C
B60R 21/00 626C
B60R 21/00 626B
B60R 21/00 626G
G01C 3/06 140
G01C 3/06 110A
G06T 1/00 330A
G01S 17/93
A61G 5/00
請求項の数または発明の数 3
全頁数 31
出願番号 特願2012-265840 (P2012-265840)
出願日 平成24年12月4日(2012.12.4)
新規性喪失の例外の表示 特許法第30条第2項適用 ・研究集会名 第44回 ストカスティック システム シンポジウム 主催者名 システム制御情報学会 開催日 平成24年11月2日 ・展示会名 イノベーション・ジャパン2012 展示日 平成24年9月27日~28日
優先権出願番号 2012027784
優先日 平成24年2月10日(2012.2.10)
優先権主張国 日本国(JP)
審査請求日 平成27年10月27日(2015.10.27)
特許権者または実用新案権者 【識別番号】397022911
【氏名又は名称】学校法人甲南学園
発明者または考案者 【氏名】田中 雅博
個別代理人の代理人 【識別番号】110000338、【氏名又は名称】特許業務法人HARAKENZO WORLD PATENT & TRADEMARK
審査官 【審査官】岩田 玲彦
参考文献・文献 特開2008-298742(JP,A)
特開2011-191221(JP,A)
特開2003-271975(JP,A)
特開2002-065721(JP,A)
特開2001-091217(JP,A)
調査した分野 G08G 1/00-1/16
A61G 1/00
B60R 21/00
G01C 3/06
G01S 17/93
G06T 1/00
A61G 5/00
特許請求の範囲 【請求項1】
車両に取り付けられて路面の距離画像を取得するデプスセンサと、
前記デプスセンサにより取得された距離画像に基づいて、前記車両に取り付けられたデプスセンサの俯角を推定する角度推定手段と、
前記角度推定手段により推定されたデプスセンサの俯角と、前記デプスセンサの前記路面からの高さと、前記距離画像の距離データとに基づいて、前記路面に存在する障害物を検出する検出手段とを備え、
前記車両はハンドルを有しており、
前記角度推定手段は、前記ハンドルの回転に応じて、前記俯角、及び前記ハンドルの回転に応じて生じる前記デプスセンサのロール角を推定することを特徴とする障害物検出装置。
【請求項2】
前記角度推定手段は、非路面上の点の影響を除去する最小二乗法に基づいて誤差を最小化して前記俯角を推定する請求項1に記載の障害物検出装置。
【請求項3】
前記デプスセンサは、非走査方式赤外線レーザセンサである請求項1に記載の障害物検出装置。
発明の詳細な説明 【技術分野】
【0001】
本発明は、車両、歩行者を含む移動体に取り付けられて路面の距離画像を取得するデプスセンサを備えた障害物検出装置に関する。
【背景技術】
【0002】
本明細書において、「移動体」とは、「車両」と「歩行者」とを含むものとする。
【0003】
従来の障害物検出装置として以下の構成が知られている。即ち、レーザを利用した障害物検知センサにより障害物を検知して走行停止させる電動車椅子の走行制御方法が知られている(特許文献1)。そして、車両近傍の路面あるいは床面を観測するセンサ装置の出力する画像を解析してその特徴を出力し、当該特徴を、過去に車両が走行した路面の履歴と比較することで、車両周辺の路面の安全性を推定する構成が開示されている(特許文献2)。
【0004】
また、車両に取り付けられたレーザを利用した距離センサから得られた障害物までの距離の継時的変化と速度検出手段から得られた速度とに基づいて障害物が存在する方向を算出してこの方向に応じた回避動作を行う構成が教示されている(特許文献3)。さらに、車両の走行中に、2次元レーザレーダによる車前方スキャンが行われ、段差や溝等の危険個所の認識と記憶をなし、駆動輪の回転速度検出、車両の移動量の算出、記憶をなし、記憶危険個所と時々刻々の車両移動量情報、及び車両移動方向に基づき、衝突予測手段による衝突予測が行われると、衝突回避用補正命令が出され、操作者のハンドル角の設定と、アクセル操作に対して車両が衝突しないように補正が行われる構成が知られている(特許文献4)。
【0005】
そして、距離センサを用いて自己の位置を推定し地図を作製する自己位置同定方法及び装置(SLAM(Simultaneous Localization And Mapping))の構成も開示されている(特許文献5)。
【0006】
暗い時間帯にシニアカー(老人用電動車)を安全に走らせるには、自転車、ライトを用いて人が前方を注視する方法しか実現していないが、そもそもシニアカーを使う人は視力や集中力が落ちている場合が多く、非常に危険な状態で走行するしかなかった。本発明者らは、機械を用いて自動的に路面上の障害物を検知する方法として、画像カメラを1つ用いて路面上の凸凹を検知するシステムを開示した(非特許文献1)。
【先行技術文献】
【0007】

【特許文献1】特開2011-177205号公報(2011年09月15日公開)
【特許文献2】特開2009-277063号公報(2009年11月26日公開)
【特許文献3】特開2005-335533号公報(2005年12月08日公開)
【特許文献4】特開2005-128722号公報(2005年05月19日公開)
【特許文献5】WO2007/069726号公報(2007年06月21日公開)
【0008】

【非特許文献1】Detection of Flat Ground Area by Single Camera on Mobile Vehicles, 2011 International Conference on Advanced Mechatronic Systems, Zhongyuan University of Technology, Zhengzhou, China, August 11-13, 2011 pp. 403-408, M. Tanaka and S. Onishi
【発明の概要】
【発明が解決しようとする課題】
【0009】
しかしながら、特許文献1~4に開示された構成は、レーザビームを走査することによって一画面分の距離画像データを取得する方式であるから、1回の走査で一画面分の距離画像データを取得するために例えば約数百ミリ秒程度の長い時間が必要である。従って、屋外で動いている移動体に取り付けて路面の距離画像を取得して処理することは困難であるという問題がある。
【0010】
特許文献5に開示されたSLAMの構成は、計算負荷が大きく、構成も複雑であるという問題がある。
【0011】
非特許文献1に開示された構成では、カメラを搭載した移動体が路面を走行していることが必須であり、また、障害物の検知能力も低いという問題がある。この構成では、特徴点をコンピュータビジョンの手法(SURF、 SIFTなど)の力を借りて抽出し、フレーム間で対応付けを行うことにより、路面より高い点、低い点の判別を行ったが、なかなか特徴点が取れないこと、対応付けが必ずしも成功しないことなどの問題が多く、実用化は困難であった。
【0012】
赤外線等を用いたセンサにより同じ瞬間の1画面分の距離画像データを取得する構成も考えられるが、走行中の移動体はガタガタと振動するため、センサの俯角が特定できず、路面における障害物を検出することが困難であるという問題がある。
【0013】
また、障害物検出装置を、視覚障害を有する歩行者の体に装着して使用したいという要望も存在する。
【0014】
本発明の目的は、移動体走行時にデプスセンサが揺れて俯角が変動しても、移動体が走行する路面における障害物を検出することができる障害物検出装置を提供することにある。
【0015】
また、本発明では、歩行者を含む移動体走行時にデプスセンサが揺れて俯角、ロール角、高さが変動しても、移動体が走行する路面における障害物を検出することができる障害物検出装置を提供することにある。
【課題を解決するための手段】
【0016】
上記の課題を解決するために、本発明に係る障害物検出装置は、車両に取り付けられて路面の距離画像を取得するデプスセンサと、前記デプスセンサにより取得された距離画像に基づいて、前記車両に取り付けられたデプスセンサの俯角を推定する角度推定手段と、前記角度推定手段により推定されたデプスセンサの俯角と、前記デプスセンサの前記路面からの高さと、前記距離画像の距離データとに基づいて、前記路面に存在する障害物を検出する検出手段とを備えたことを特徴とする。
【0017】
本明細書において、「デプスセンサ」とは、面情報を取得できる距離センサを意味するものとする。このようなデプスセンサのうち、市販されている例として、Kinect for Windows(登録商標)やXtion Pro(登録商標)などを挙げることができる。しかしながら、これらに限定されるものではない。
【0018】
この特徴により、デプスセンサにより取得された距離画像に基づいて、移動体に取り付けられたデプスセンサの俯角を推定し、この推定した俯角と、デプスセンサの路面からの高さと、距離画像の距離データとに基づいて、路面に存在する障害物を検出する。このため、デプスセンサが距離画像を取得した瞬間におけるデプスセンサの俯角を推定することができるので、移動体走行時にデプスセンサが揺れて俯角が変動しても、移動体が走行する路面における障害物を検出することができる。
【0019】
本発明に係る障害物検出装置では、前記角度推定手段は、非路面上の点の影響を除去する最小二乗法に基づいて誤差を最小化して前記俯角を推定することが好ましい。
【0020】
上記構成によれば、簡単な構成により、移動体に取り付けられたデプスセンサの俯角を推定することができる。
【0021】
本発明に係る障害物検出装置では、前記車両はハンドルを有しており、前記角度推定手段は、前記ハンドルの回転に応じて、前記俯角、及び前記ハンドルの回転に応じて生じる前記デプスセンサのロール角を推定することが好ましい。
【0022】
上記構成によれば、ハンドルが回転されて移動体が曲進するときも路面における障害物を検出することができる。
【0023】
本発明に係る障害物検出装置では、前記デプスセンサは、非走査方式赤外線レーザセンサであることが好ましい。
【0024】
上記構成によれば、デプスセンサを安価に構成することができる。
【0025】
本発明に係る障害物検出装置は、歩行者に取り付けられて路面の距離画像を取得するデプスセンサと、前記デプスセンサにより取得された距離画像に基づいて、前記歩行者に取り付けられたデプスセンサの俯角、ロール角、及び高さを推定する状態推定手段と、前記状態推定手段により推定されたデプスセンサの俯角、ロール角、及び高さと、前記距離画像の距離データとに基づいて、前記路面に存在する障害物を検出する検出手段とを備えたことを特徴とする。
【0026】
この特徴により、デプスセンサにより取得された距離画像に基づいて、移動体に取り付けられたデプスセンサの俯角、ロール角、及び高さを推定し、この推定した俯角、ロール角、及び高さと、距離画像の距離データとに基づいて、路面に存在する障害物を検出する。このため、デプスセンサが距離画像を取得した瞬間におけるデプスセンサの俯角、ロール角、及び高さを推定することができるので、歩行者を含む移動体走行時にデプスセンサが揺れて俯角、ロール角、及び高さが変動しても、移動体が走行する路面における障害物を検出することができる。
【0027】
本発明に係る障害物検出装置では、前記状態推定手段は、非路面上の点の影響を除去する最小二乗法に基づいて誤差を最小化して前記俯角、前記ロール角、及び前記高さを推定することが好ましい。
【0028】
上記構成によれば、簡単な構成により、移動体に取り付けられた時間変化するデプスセンサの俯角、ロール角、及び高さを推定することができる。
【0029】
本発明に係る障害物検出装置では、前記状態推定手段は、前記距離データ、前記俯角、前記ロール角、及び前記高さの関係に基づく状態空間モデルに対して、非路面上の点の影響を除去する非線形カルマンフィルタに基づいて変動を許容して前記俯角、前記ロール角、及び前記高さを推定することもできる。
【0030】
上記構成によれば、移動体に取り付けられたデプスセンサの俯角、ロール角、及び高さが変動する環境においても変動幅をモデルに組み込んで俯角、ロール角、及び高さを推定することができる。
【0031】
なお、俯角、ロール角、及び高さの推定において利用されるアルゴリズムは、非線形カルマンフィルタに限定されるわけではなく、他のフィルタリングアルゴリズムに基づいて、俯角、ロール角、及び高さが推定されてもよい。
【0032】
加えて、デプスセンサの姿勢を決定するパラメータの組は、センサーの状態を決定づけるものであれば、俯角、ロール角、及び高さの組に限定されない。
【発明の効果】
【0033】
本発明に係る、障害物検出装置は、前記デプスセンサにより取得された距離画像に基づいて、前記車両に取り付けられたデプスセンサの俯角を推定する角度推定手段と、前記角度推定手段により推定されたデプスセンサの俯角と、前記デプスセンサの前記路面からの高さと、前記距離画像の距離データとに基づいて、前記路面に存在する障害物を検出する検出手段とを備えたので、デプスセンサが距離画像を取得した瞬間におけるデプスセンサの俯角を推定することができ、車両走行時にデプスセンサが揺れて俯角が変動しても、移動体が走行する路面における障害物を検出することができる。
【0034】
また、本発明に係る、障害物検出装置は、歩行者に取り付けられて路面の距離画像を取得するデプスセンサと、前記デプスセンサにより取得された距離画像に基づいて、前記歩行者に取り付けられたデプスセンサの俯角、ロール角、及び高さを推定する状態推定手段と、前記状態推定手段により推定されたデプスセンサの俯角、ロール角、及び高さと、前記距離画像の距離データとに基づいて、前記路面に存在する障害物を検出する検出手段とを備えるので、デプスセンサが距離画像を取得した瞬間におけるデプスセンサの俯角、ロール角、及び高さを推定することができるので、歩行者の走行時にデプスセンサが揺れて俯角、ロール角、及び高さが変動しても、歩行者が歩行する路面における障害物を検出することができる。
【図面の簡単な説明】
【0035】
【図1】実施の形態1に係る障害物検出装置の構成を模式的に示す図である。
【図2】上記障害物検出装置の外観を示す図である。
【図3】上記障害物検出装置の動作のアルゴリズムを説明するための座標系を示す図である。
【図4】上記障害物検出装置に設けられたデプスセンサがデータを取得する対象画面を説明するための模式図である。
【図5】(a)は路面上に障害物及び溝がない場合のデプスセンサの測定データを説明するための図であり、(b)は路面上に障害物がある場合のデプスセンサの測定データを説明するための図であり、(c)は路面上に溝がある場合のデプスセンサの測定データを説明するための図である。
【図6】上記障害物検出装置の運転席から前方の路面に存在する障害物を見た図である。
【図7】図6に示す線L1に沿って路面の高さを推定した結果を示すグラフである。
【図8】図6に示す線L2に沿って路面の高さを推定した結果を示すグラフである。
【図9】実施の形態2に係る障害物検出装置の構成を模式的に示す図である。
【図10】実施の形態2に係る障害物検出装置の動作を説明するための図である。
【図11】実施の形態3に係る障害物検出装置の構成を模式的に示す図である。
【図12】図11に示す障害物検出装置の使用形態を説明するための図である。
【図13】図11に示す障害物検出装置の他の使用形態を説明するための図である。
【図14】直交座標及び極座標におけるデプスセンサが所定のロール角で回転するときの座標系を示す図である。
【図15】実施の形態4に係る障害物検出装置における障害物検出中の映像を示す図である。
【図16】(a)は上記障害物検出装置に設けられたデプスセンサの俯角の推定値の推移を示すグラフであり、(b)は上記デプスセンサのロール角の推定値の遷移を示すグラフであり、(c)は上記デプスセンサの高さの推定値の遷移を示すグラフである。
【図17】上記障害物検出装置を適用したスクーターを示す図である。
【発明を実施するための形態】
【0036】
以下、本発明の実施の形態に係る障害物検出装置について、詳細に説明する。

【0037】
(概要)
シニアカー(老人用電動車)を黄昏時あるいは夜間に走行しようとするとき、前方ライトだけでは特に近いところの視認が困難で、極めて危険な状態になる。そこで、以下の実施の形態では、黄昏時あるいは夜間時の薄暗い状況あるいは真っ暗な状況において、進行方向の路面の危険を検知し、運転者に教えることのできる障害物検出装置を、キネクト(Kinect(登録商標)) センサを使って実現する。

【0038】
(実施の形態1)
(障害物検出装置1の構成)
図1は、実施の形態1に係る障害物検出装置1の構成を模式的に示す図である。図2は、障害物検出装置1を搭載した移動体(車両)5の外観を示す図である。

【0039】
移動体5は、例えばシニアカーであり、障害物検出装置1を搭載して路面6を走行する。障害物検出装置1は、デプスセンサ2を備えている。デプスセンサ2は、移動体5のハンドル7(図2)の前に設けられた駕籠10(図2)に取り付けられており、路面6の距離画像を取得する。

【0040】
障害物検出装置1には、俯角推定部(角度推定手段)3が設けられている。俯角推定部3は、デプスセンサ2により取得された路面6の距離画像に基づいて、移動体5に取り付けられたデプスセンサ2の水平線9を基準とした俯角αを推定する。俯角推定部3は、非路面上の点の影響を除去する最小二乗法に基づいて誤差を最小化して俯角αを推定する。

【0041】
障害物検出装置1は、検出部4を備えている。検出部4は、俯角推定部3により推定されたデプスセンサ2の俯角αと、デプスセンサ2の路面6からの高さhと、デプスセンサ2により取得された距離画像の距離rとに基づいて、路面6に存在する障害物8を検出する。

【0042】
障害物検出装置1には、ディスプレイ11及びスピーカ12が設けられている。ディスプレイ11は、検出部4により検出された障害物8を表す情報を表示する。スピーカ12は、検出部4により検出された障害物8を報知する音または音声を出力する。

【0043】
移動体5は、バッテリ13を備えている。バッテリ13は、移動体5が走行するための電力を供給するとともに、デプスセンサ2、俯角推定部3、及び検出部4が動作するための電力を供給する。

【0044】
(デプスセンサ2の特性)
デプスセンサ2は、安価であり、画像だけでなく、距離画像も取得する能力を持っている。しかも、画像と同じ解像度である、640×480 という高解像度を有している。

【0045】
デプスセンサ2によるデータ取得の間隔は30fps 程度であり、一般的な画像カメラと同等である。また、取得できる距離は5-6m程度である。ただし、デプスセンサ2は昼間の日光には弱く、直射日光が当たっているときは、屋外ではデータ取得はほとんどできない。

【0046】
以上のようなデプスセンサ2の特性は、シニアカーによる路面障害物の検出という本実施の形態の目的に適合している。すなわち、5-6mという距離画像を取得可能な距離は、歩行者や時速6km以下でしか走行できないシニアカー等には十分であり、暗いときの方が好都合なデプスセンサ2のセンサ特性は、暗くなったときに使えればよいという路面障害物検出の目的に合っている。俯角推定部3及び検出部4はノートPCにより構成することができる。このノートPCは、移動体5から取り外し可能に構成しておけばよいから、既に所有しているPCを必要なときだけ移動体5に取り付けて使用すればよく、ノートPC、PDA(Personal Digital Assistant)、スマートホンなどの携帯型データ処理装置を新たに購入する必要もない。従って、デプスセンサ2を備えた障害物検出装置1の費用は、移動体(シニアカー)のオプションとして、十分安いものといえる。デプスセンサ2は屋内使用という認識が広く行き渡っているけれども、本発明者はデプスセンサ2の上記した特性に着目し、屋外を走行するシニアカーに搭載する障害物検出装置1にデプスセンサ2を適用した。このようなデプスセンサ2の使い方をする発明は例を見ない。

【0047】
(デプスセンサ2の利用方法)
デプスセンサ2は、図2に示すように、移動体5(シニアカー)のハンドル7の前側に設けられた駕籠10の上側に、俯角20-40度程度で固定して取り付ける。これにより、数十cmから数mまでの移動体5の前方の様子を距離画像として取得できる。デプスセンサ2は12Vの直流電源が必要である。これは、移動体5の電源を共用することにより、容易に準備することができる。また、デプスセンサ2により取得した距離画像データを処理するためにはノートパソコンが必要であるが、これも、畳んで駕籠10に入れることで、収納可能である。検知した障害物(物体)の情報を人に伝えるために、7-9インチ程度のモニタディスプレイ11(図1)をハンドル7に取り付ける。障害物8の検知に応じてスピーカ12から音を出すことも可能である。

【0048】
デプスセンサ2は駕籠10に取り付けた場合、移動体5の走行時に相当揺れるが、1枚の距離画像から障害物8を検知することができるため、揺れても基本的に問題ない。

【0049】
今、デプスセンサと、特定しているが、本発明はこれに限定されない。一般的に、距離データを面(1次元の線上を走査する1次元レーザスキャナにより取得された線の上の情報ではなく)で捉えることのできるセンサであれば他のセンサでもかまわない。

【0050】
また、1回の走査で一画面分の距離画像データを短時間で取得することができるのであれば、屋外で動いている車両や人といった移動体に取り付けて路面の距離画像を取得して処理することが可能になるから、レーザ走査方式の距離センサを使用してもよい。

【0051】
さらに、障害物検出装置1を移動体5に搭載する例を挙げているが、本発明はこれに限定されない。夜出歩く歩行者の腰に装着するように小型軽量化して構成してもよい。これにより、夜の路面の障害物の有無を歩行者が検知することができるという効果を奏する。

【0052】
どの程度の大きさの物体(障害物)が捉えられるかということについては、センサの誤差による。センサにより捉える物体は、移動体5の走行の大きな障害になる物体や段差などを想定している。

【0053】
本実施の形態に係るアルゴリズムの特徴は、俯角α を、デプスセンサ2により取得される毎回の画面ごとに推定していることである。そのため、このアルゴリズムは、移動体5の走行に応じたデプスセンサ2の揺れに強い。さらに、ハンドル7を切ったときにはデプスセンサ2が傾くが、実施の形態2で後述するように、そのロール角を推定することも可能であり、ハンドル7を切ったときに、進行方向の障害物8を見つけることができる。

【0054】
デプスセンサ2は、図2に示すように、駕籠10に取り付けて使う。あるいは、ハンドル7に取り付けることもできる。デプスセンサ2の性質上、あまり低い所に取りつけてデプスセンサ2の視野方向と路面6とのなす角度を鋭角にすると、路面6がうまく検知出来ない。そのため、図2に示す構成の他、移動体5の前部にセンサ専用の取り付け具を装着して、理想の角度にデプスセンサ2を取り付けるという構成も可能である。安全性が立証されれば、シニアカーの構造の一部として車体の中に埋め込めると、さらにスマートな外観になる。俯角推定部3及び検出部4を構成する処理用のPCは、実施の形態1の例では、駕籠10の中に畳んで入れる。なお、歩行者に装着する際には、腰、顔面、頭部その他に、適切な取り付け具を用いて可能な方法で人体に取り付けてもよい。

【0055】
(デプスセンサ2の距離画像データの処理方法)
移動体5(シニアカー)の駕籠10などに、路面6までの距離データを面状に測定することができるセンサ(ここでは、デプスセンサ2を想定している)を取り付けることにより、路面6上の障害物8や穴、溝など、移動体5の走行に障害となる凸凹をセンサ1つと処理PC(俯角推定部3及び検出部4)とで検知する。

【0056】
デプスセンサ2は屋内用の仕様であり、屋外では太陽光が直接当たる場所はセンサの特性上データが十分に取得できない可能性があるが、陰があればデータは取得できることから、よほどの悪条件でなければ日向でも使用可能である。また、夕方から夜にかけては屋外であっても屋内と同程度にセンシング能力が高い。特に、センシング能力を高めるために、比較的高い部分(駕籠10の上の方)に、ある程度の俯角(30度程度を想定)を持たせて取り付けることにより、問題なく路面までの距離データを測定することができる。

【0057】
デプスセンサ2から得られる各格子点(画素)上の位置での路面までの距離は、デプスセンサ2の俯角αが既知であれば、極座標と直交座標との関係から理論的な値は容易に得られる。従って、その理論値と実測値との比較により、その点が路面6上にあるのか、路面6よりも高い点なのか、低い点なのかを判定することができる。

【0058】
但し、移動体5(シニアカー)は走行時にかなり揺れるので、駕籠10に取り付けられているデプスセンサ2自身も揺れることは避けられず、俯角αを既知とすることは困難である。

【0059】
そのため、本実施の形態では、多くの点が路面6上にあることを利用して、ロバストな最小二乗法あるいはRANSACなど、非路面上の点の影響を外す方法を採用することにより、誤差を最小化して俯角αを推定する。俯角αが正確に推定できれば、以下に述べる方法で、路面6の凸凹の状況が検出できる。また、ハンドルを切ってロール角がある場合も、モデルを作成できる。

【0060】
(正面方向に進む場合のアルゴリズム)
図3は、障害物検出装置1の動作のアルゴリズムを説明するための座標系を示す図である。図4は、障害物検出装置1に設けられたデプスセンサ2がデータを取得する対象画面Sを説明するための模式図である。デプスセンサ2は、縦がnV=480ドット(視野角V=43度)、横がnH=640ドット(視野角H=57度)の路面6上の対象画面S上の格子点において距離を得ることのできるセンサである。画面左下を原点ORGとしてみると、縦方向がi=0,… ,nV-1、横方向がj=0, … ,nH-1となる。3次元空間を (x,y,z)とする。また、その空間を極座標(r,θ,φ)で表現する(図3参照)と、下記の(式1)

【0061】
【数1】
JP0006218209B2_000002t.gif

【0062】
となる。

【0063】
今、デプスセンサ2の縦方向の画素が、縦の視野角VをnV-1個に均等に分割しており、同様に、横方向の画素が、横の視野角HをnH-1個に均等に分割していると仮定する。また、デプスセンサ2の高さhはほぼ一定であり、直交座標は路面をz=0とする。

【0064】
また、進行方向正面をφ=0とする。真上の角度をθ=0、前方をθ=90度とする。このとき、下記の(式2)

【0065】
【数2】
JP0006218209B2_000003t.gif

【0066】
となる(図3参照)。俯角はα-90度である。

【0067】
デプスセンサ2の発光点Lから見た対象画面S上の各画素(i、j)(i=0,… ,nV-1、j=0, … ,nH-1)に対応する点の極座標の角度θi、φiは、下記の(式3)及び(式4)により表される。

【0068】
【数3】
JP0006218209B2_000004t.gif

【0069】
これらの角度の単位は、度(°) である。

【0070】
デプスセンサ2は、各画素(i,j)に対応する点と発光点Lとの間の距離rijを観測する機能を持つ。そこで、デプスセンサ2の位置の真下路面を原点とし、センサ位置から路面6までの距離は常に一定値hであることを利用して、(式2)の3列目のz及びrcosθ+hに対応して下記の(式5)

【0071】
【数4】
JP0006218209B2_000005t.gif

【0072】
が成り立つはずである。

【0073】
この関係式(式5)より、俯角αを次の方法で推定する。

【0074】
図5(a)は路面6上に障害物及び溝がない場合のデプスセンサ2の測定データを説明するための図であり、(b)は路面6上に障害物8がある場合のデプスセンサ2の測定データを説明するための図であり、(c)は路面6上に溝14がある場合のデプスセンサ2の測定データを説明するための図である。

【0075】
ある1枚の距離画像において、俯角αは一定であると考えられる(走査方式のレーザセンサの場合、首を振って1枚の距離データを取得するためデータ取得に時間がかかるから、俯角αが一定であるとは言えない)。また、図5(a)に示す路面6の上から反射した点P1においては、zij=0 である。しかし、図5(b)に示すように物体(障害物8)が路面6上にある場合、または、図5(c)に示すように溝14内の点のように路面6よりも低い点から反射した場合は、zijは0に等しくない。俯角αが未知な状態では、どの位置(i,j)においてzijが0に等しくないのかわからない。しかし、路面6は移動体5が走行する道を想定しているので、多くの位置は図5(a)に示すように路面6の上から反射しているはずだから、次のような方法で俯角αは決定できる。

【0076】
1.全部の点が路面6の上にあると仮定して、そのまま最も二乗誤差を最小にするような俯角αを数値的に決定する。すなわち、

【0077】
【数5】
JP0006218209B2_000006t.gif

【0078】
を求める。
2.得られた俯角αを用いて、

【0079】
【数6】
JP0006218209B2_000007t.gif

【0080】
を計算し、

【0081】
【数7】
JP0006218209B2_000008t.gif

【0082】
になるような位置(i,j)を除外して、除外された位置を含まない位置(i, j) の集合Sにおいて、

【0083】
【数8】
JP0006218209B2_000009t.gif

【0084】
を求める。これを、収束するまで繰り返す。

【0085】
上記の方法でうまくいかないほど平面(路面6)上の点が少ない場合は、RANSACなど、ロバストなアルゴリズムを適用すればよい。

【0086】
なお、上記1.の俯角αによる最適化は制約条件を伴わないので、黄金分割法など、簡単な1次元探索法の適用が可能である。俯角αは[110度、130度]などの区間において求めればよい。

【0087】
(高さの推定)
得られた俯角αの推定値を用いて(式2)より高さ方向のzijの推定値が得られる。また、xij,yij(障害物8、溝14の路面6上の平面位置)も同時に推定することができる。そのため、高さがあるもの(障害物8)または、溝14の正確な平面位置が分かる。

【0088】
(路面高さ推定実験結果)
図6は、障害物検出装置1の運転席から前方の路面6に存在する障害物8を見た図である。図7は、図6に示す線L1に沿って平行に路面6の高さを複数本推定した結果を示すグラフである。横軸はデプスセンサ2により取得された距離画像の縦方向に沿った画素番号を示しており、横軸の右側が図4における手前側、即ち、距離画像の下側を表しており、左側は図4における遠方側、即ち、距離画像の上側を表している。縦軸は路面6からの高さを示している。

【0089】
図7における線C1において、高さ680mmの左端の部分はデータが欠損している部分であり、除外できる。これらの点を除去すると、画素番号1~約90、及び画素番号約160~480に対応する路面6からの高さはほぼゼロである。そして、画素番号約160から画素番号約100に向かって路面6からの高さが増大し、画素番号約100~約90に対応する高さは約200mmとなっている。

【0090】
画素番号約160~約100の高さの増大は、デプスセンサ2からの赤外線が障害物8のデプスセンサ2側の側面15(図5(b))により反射されたためと考えられる。そして、画素番号約100~約90の高さ200は、デプスセンサ2からの赤外線が高さ200mmの障害物8の上面16(図5(b))により反射されたためと考えられる。このように、画素番号に対応する路面6からの高さを検出することにより、障害物8を検知することができる。

【0091】
図8は、図6に示す線L2に沿って障害物8の近辺で複数本のラインに沿って路面6の高さを推定した結果を示すグラフである。横軸はデプスセンサ2により取得された距離画像の横方向に沿った画素番号を示しており、横軸の右側が図4における右側、即ち、距離画像の右側を表しており、左側は図4における左側、即ち、距離画像の左側を表している。縦軸は路面6からの高さを示している。

【0092】
図8における線C2に示すように、高さ680と表示された部分は欠損データなので、これらを除去すると、画素番号1~約350、及び画素番号約420~640に対応する路面6からの高さは、俯角が最適化されておらず、少しドリフトが残っているが、これが水平になるような俯角を前述したように推定することができる。そして、画素番号約350から画素番号約420に対応する路面6からの高さは約200mmとなっており、路面6上の点とは明確に区別可能である。

【0093】
画素番号約350~約420の高さ200は、デプスセンサ2からの赤外線が高さ200mmの障害物8の上面16(図5(b))により反射されたためと考えられる。このように、画素番号に対応する路面6からの高さを検出することにより、障害物8を検知することができる。

【0094】
(実施の形態の効果)
本実施の形態の手法によれば、距離データを取得できるセンサを使うので、より安定的に距離を測定することができ、しかも、移動体5が静止している状態で路面6の凸凹の部分を検出することができる。

【0095】
また、移動体5(シニアカー)に装着できるように、デプスセンサ2の電源をシニアカー本体から取得することもできるし、電源ケーブルが不要なセンサもある。また、ディスプレイ11をハンドル7に装着することで、処理用パソコン(俯角推定部3及び検出部4)を畳んだ状態で障害物8の検知情報を移動体5の利用者に伝達することができる。あるいは、スピーカ12からの音などによる伝達方法も可能である。

【0096】
デプスセンサ2に必要な電源(直流12V)を、移動体5(シニアカー)のバッテリ13から直接供給することができると、デプスセンサ2専用の新たなバッテリを置く必要がなくなり、デプスセンサ2の移動体5への実装がより現実的になる。

【0097】
さらに、上述の俯角推定部3及び検出部4などをノートPCにより構成する例のように、障害物検出装置1が、バッテリーを内蔵し電力供給インターフェース(例えば、ユニバーサル・シリアル・バス(USB;Universal Serial Bus)など)を有しているノートPCなどを備えている場合には、デプスセンサ2は、当該ノートPCなどから電力の供給を受けてもよい。なお、このような構成に限定されるわけではなく、上記インターフェースを介して、デプスセンサ2から当該ノートPCへ、測定した画像や距離などのデータを受け渡してもよい。

【0098】
(変形例)
なお、移動体5がシニアカーである例を示したが、本発明はこれに限定されない。移動体5は、例えば、倉庫等の環境で作業する無人の移動ロボットであってもよい。全面的に直射日光があたるところばかりの環境でない限り無人の移動ロボットに本発明を適用することができ、移動ロボットが床面の障害物や溝を検出することができるという効果を奏する。ここで、「全面的に直射日光があたるところばかりの環境」とは、検出対象の障害物などの全面が日光に照らされ、当該障害物の陰が全くできない環境を意味する。例えば、全面的に直射日光があたるばかりであっても、当該日光に照らされる障害物などの陰が存在する環境は、「全面的に直射日光があたるところばかりの環境」ではない。上述のように、当該陰が存在する環境において、障害物検出装置1は距離データを測定可能である。

【0099】
本手法はシニアカーに限らず、歩行者手押しのシルバーカー、電動車いす、自転車等にも応用することが可能である。また、視覚等に障害を持つ人に対しても、聴覚など、何らかの伝達手段があれば歩行時の危険検知サポートに利用することが可能である。デプスセンサ2やデータ処理装置がさらに小型化すると、応用範囲は一層広がると考えられる。

【0100】
(実施の形態2)
(ハンドルを傾けることがある場合のアルゴリズム)
図9は、実施の形態2に係る障害物検出装置1aの構成を模式的に示す図である。図1で説明した構成要素と同一の構成要素には同一の参照符号を付している。これらの構成要素の詳細な説明は繰り返さない。

【0101】
実施の形態2に係る障害物検出装置1aは、実施の形態1の障害物検出装置1の俯角推定部3の代わりに俯角・ロール角推定部(角度推定手段)17を備えている。障害物検出装置1aのその他の構成は、障害物検出装置1と同様である。俯角・ロール角推定部17は、ハンドル7(図2)の回転に応じて、デプスセンサ2の俯角、及びハンドル7の回転に応じて生じるデプスセンサ2のロール角を推定する。

【0102】
図10は、実施の形態2に係る障害物検出装置1aの動作を説明するための図である。次に、ハンドル7を回転した場合への対応を考える。

【0103】
シニアカーをはじめとして、通常の乗り物はハンドル7の回転軸は鉛直ではなく上に上がるにつれて運転者とから見て遠くから手前の方向に傾いている。このため、図10に示すように、ハンドル7を回転したときに、駕籠に取り付けられたデプスセンサ2は斜めに傾く。即ち、ハンドル7を回転した後のデプスセンサ2は、ハンドル7の回転とともに回転して回転後の正面方向を向くx軸の周りに回転後のy-z平面内においてロール角ψほど回転した状態になっており、俯角も同時に変化していると見ることができる。
そこで、上記回転により生じるロール角をψとおいて、座標(x,y,z)は座標軸を回転したときの値に変換される。まず、回転がない場合は、

【0104】
【数9】
JP0006218209B2_000010t.gif

【0105】
さらに、センサー位置で回転する場合、

【0106】
【数10】
JP0006218209B2_000011t.gif

【0107】
これより、zがhに近い値になるような最適化問題を設定すると、

【0108】
【数11】
JP0006218209B2_000012t.gif

【0109】
ここに、

【0110】
【数12】
JP0006218209B2_000013t.gif

【0111】
である。また、ハンドルを切ったときに傾く角度は物理的に制約されており、

【0112】
【数13】
JP0006218209B2_000014t.gif

【0113】
程度である。
また、俯角αについても、取り付けた角度を基準としてせいぜい±15度程度の範囲で推移することから、制約条件付きの最小化問題として解くことができる。

【0114】
注意すべきことは、この座標系はデプスセンサ2の位置の真下の路面を原点としているため、センサが傾いて高さが変化する(低くなる)ことを考慮に入れる必要がある点である。最大に回転させると、約3cmほどセンサの位置が低くなる。誤差の範囲と考えることもできるが、厳密な値を計算しようとすれば、hをψの関数h(ψ)とおき、あらかじめψに対してhがどう変化するか実験的に求めておき、表などにして値を持たせておけばよい。

【0115】
(式12)で示される最適化問題を数値的に解くアルゴリズムは、一般に非線形最小化問題として多くの種類があり、特定のアルゴリズムに限定して使う必要はない。

【0116】
(高さの推定)
得られたロール角ψの推定値を用いて(式11)よりハンドル7を回転した場合の高さ方向の推定値zijが得られる。また、xij,yij(障害物8、溝14の路面6上の平面位置)も同時に推定することができる。そのため、ハンドル7を回転した場合に、高さがあるもの(障害物8)または、溝14の正確な平面位置が分かる。

【0117】
(実施の形態3)
上述の実施の形態、図で説明した構成要素と同一の構成要素には同一の参照符号を付している。これらの構成要素の詳細な説明は繰り返さない。

【0118】
上述の実施の形態1、2とは異なり、本実施の形態に係るアルゴリズムでは、俯角α、ロール角ψ、及び高さhを、デプスセンサ2により取得される毎回の画面の距離データごとに推定していることである。そのため、このアルゴリズムは、移動体5の走行または歩行に応じたデプスセンサ2の揺れ(路面が悪い場合などに生じる)、ロール角の変化(ハンドルを切ったり体が傾いたりするときに生じる)、高さの変化(歩行などの際に生じる)に強い。

【0119】
(障害物検出装置1bの構成)
図11は、実施の形態3に係る障害物検出装置1bの構成を模式的に示す図である。図11に示すように、障害物検出装置1bは、俯角・ロール角・高さ推定部(状態推定手段)17aを備える点が、他の実施の形態とは異なっている。

【0120】
図12は、図11に示す障害物検出装置1bの使用形態を説明するための図である。図12に示すように、障害物検出装置1bでは、歩行者5aの腰部50にデプスセンサ2が取り付けられている。ここで、歩行者5aは、上述の実施の形態における移動体5に対応している。移動体5を例えば車両とした場合と比較すると、移動体5を歩行者5aとして、歩行者5aにデプスセンサ2を取り付けた場合には、図11に示す高さhなどが比較的大きく変化する。しかしながら、後述する本実施の形態のアルゴリズムは高さhなどの変化に強いため、障害物検出装置1bは正確に障害物を検出できる。

【0121】
また、デプスセンサ2は、接続ケーブル31を介して、図12中に20で示すノートPCに接続されている。当該ノートPCは図11中に20で示す部位に対応しており、バッテリ13、ディスプレイ11、スピーカ12、検出部4、及び俯角・ロール角・高さ推定部17aに対応する構成を備えている。このような構成により、ノートPC20は、デプスセンサ2がセンシングした情報を受けて、俯角、ロール角、及び高さの推定を行い、障害物を検出し、ディスプレイ11やスピーカ12に対応する構成から検出結果を歩行者5aに提示することができる。

【0122】
なお、図12では、歩行者5aの腰部50に1つのデプスセンサ2を取り付けた例を示しているが、この構成に限定されるわけではなく、例えば歩行者5aの足部、胸部、腕部、肩部、頭部、背中などにデプスセンサ2を取り付けてもよいし、複数の障害物検出装置1bを複数の異なる箇所に取り付けてもよい。また、障害物検出装置1bは、複数のデプスセンサ2を備えていてもよい。

【0123】
また、デプスセンサ2は、図11における俯角αが20-40度程度になるように取り付けられることが好ましい。障害物検出に好適な俯角αを保ちつつ、人を含めた移動体の所望の部位にデプスセンサ2を取り付けるためには、デプスセンサ2は小型であることが好ましい。しかしながら、このような構成に限定されるわけではなく、障害物検出に好適な俯角αを確保できるのであれば、障害物検出装置1bは、デプスセンサ2の位置を安定させるための取り付け用品やジャイロなどを利用した動的な姿勢安定制御機構など備えていてもよい。

【0124】
また、図12では、デプスセンサ2及びノート側パーソナルコンピュータを利用する構成を示したが、この構成に限定されるわけではない。例えば、デプスセンサ2及び図11に示すノートPC20を1つの装置に実装してもよい。デプスセンサ2などは小型化することが可能であり、例えば1台のスマートホンに、障害物検出装置1bが必要とする構成をすべて実装してもよい。

【0125】
図13は、図11に示す障害物検出装置1bの他の使用形態を説明するための図である。図13に示すように、障害物検出装置1bでは、デプスセンサ2が図12に示す例と同様に歩行者5aの腰部50に取り付けられている。そして、図12に示すノートPC20を鞄10aの中に入れて、接続ケーブル32を介して、音声により障害物検出結果を歩行者5aが受け取っている。この構成により、便利に携帯可能な障害物検出装置1bを提供することができる。また、検出結果を音声などにより利用者が受け取る構成とすることにより、例えば目が不自由な利用者に対しても、障害物検出結果を提供することができる。障害物検出装置1bの検出結果の出力方法は音声に限定されるわけではなく、障害物検出装置1bは振動などにより検出結果を出力してもよい。

【0126】
(俯角α、ロール角ψ、高さhの推定)
デプスセンサ2から得られる各格子点(画素)上の位置での路面までの距離は、デプスセンサ2の俯角α、ロール角ψ、高さhが既知であれば、極座標と直交座標との関係から理論的な値は容易に得られる。従って、その理論値と実測値との比較により、その点が路面6上にあるのか、路面6よりも高い点なのか、低い点なのかを判定することができる。

【0127】
但し、移動体5(シニアカ—)は移動時にかなり揺れるので、駕籠10に取り付けられているデプスセンサ2自身も揺れることは避けられず、俯角αを既知とすることは困難である。歩行者の場合は、あらゆるα、ψ、hがいずれも不安定であり、一定とは言えない。

【0128】
そのため、本実施の形態では、多くの点が路面6上にあることを利用して、ロバストな最小二乗法あるいはRANSACなど、非路面上の点と思われる部分の影響を外す方法を採用することにより、実測値から得られる高さ推定値と路面高さ(0に設定することができる)との誤差を未知変数α、ψ、hにより最小化することでこれらの値を推定(更新)する。当該推定方法を、本明細書では「最適化を用いた推定方法」と称する。推定された値を適用することで、上述の方法で、路面6の凸凹の状況が検出できる。

【0129】
(極座標系と直交座標系の関係)
図14は、直交座標及び極座標におけるデプスセンサがロール角ψで回転するときの状況を示す図である。格子点において観測される距離データの中で、rijを縦i番目、横j番目の位置の距離の値とする。このとき、直交座標と極座標の間の関係は、次式により表現できる。ここで、kは離散時刻を表す整数である。

【0130】
【数14】
JP0006218209B2_000015t.gif

【0131】
直交座標系はセンサの真下の地面の位置を原点と置いている。極座標は、センサの位置を原点としている。そのため、その間には高さ分のバイアスがある。また、R(ψ(k))はx軸周りの回転である。つまり、R(ψ(k))は次式により表現できる。

【0132】
【数15】
JP0006218209B2_000016t.gif

【0133】
(最適化を用いた推定方法)
当該推定方法のアルゴリズムは、実施の形態2にて上述の「ハンドルを傾けることがある場合のアルゴリズム」を拡張したアルゴリズムである。

【0134】
高さzijの値を上の(式13)より抜き出し、次式のように表現できる。

【0135】
【数16】
JP0006218209B2_000017t.gif

【0136】
もし点i、jが路面上にあれば、次式のようにzijは、概ね0になる。

【0137】
【数17】
JP0006218209B2_000018t.gif

【0138】
そして、路面上の点の集合をGとすると、これらの点ではほとんど0のはずなので、0との誤差の二乗和を最小にするように、(式12)においてhも推定対象の未知変数として扱い、次式に従ってα、ψ、hを推定する。

【0139】
【数18】
JP0006218209B2_000019t.gif

【0140】
路面上の集合Gとすると、これらの点ではほとんど0のはずなので、0との誤差の二乗和を最小にするように未知パラメータα、ψ、hを選ぶことにより、正しい姿勢を表す未知パラメータα、ψ、hが求まる。しかし、ここで集合Gにどの点が含まれるべきなのかはわかっていない。ここで、サンプリングの周期に比べて姿勢の変化は緩やかであるという仮定をすると、姿勢の推定値は、直前の推定値の近傍にあると考えることができ、次のステップ1~4を含む、繰り返しのアルゴリズムが有効である。

【0141】
ステップ1.未知パラメータα、ψ、hを初期値にセットする。

【0142】
ステップ2.仮定されたパラメータの値、あるいは前回のパラメータ推定値に基づき、(式15)を計算し、zij(k)がほぼ0であるような点(i,j)の集合をGとする。

【0143】
ステップ3.上記のように求められたGによって、(式16)で表す最小化問題を解き、未知パラメータの推定値を更新する。この最小化問題は、さまざまな数理的アルゴリズムが使えるが、計算時間を小さくしなければならないことから、2階微分情報であるヘッセ行列を用い、収束の速いニュートン法を1回ずつ適用する方法を採るなどの工夫をすることが望ましい。

【0144】
ステップ4.ステップ2に戻る。

【0145】
(実施の形態4)
上述の実施の形態、図で説明した構成要素と同一の構成要素には同一の参照符号を付している。これらの構成要素の詳細な説明は繰り返さない。

【0146】
実施の形態3では、最適化を用いた推定方法により、3つの未知パラメータである俯角α、ロール角ψ、高さhを推定した。しかしながら、実施の形態3の方法では、後述するように俯角α、ロール角ψ、高さhの遷移速度などを考慮していない。しかしながら、未知パラメータの推定方法については、これらの値を確率変数としてとらえ、動的モデルを設定することにより、拡張カルマンフィルタや粒子フィルタなどの非線形フィルタを用いて推定するなど、様々な推定理論によるアプローチが可能である。そこで、当該推定方法を、本明細書では状態空間モデルを用いた推定方法と称し、以下で説明する。

【0147】
(状態空間モデルを用いた推定方法)
(状態ベクトルと動的モデル)
状態変数ベクトルをqを次式のように設定する。

【0148】
【数19】
JP0006218209B2_000020t.gif

【0149】
状態変数は、以下のランダムウォークモデルにより変化するものとする。このモデルは、センサを取り付けた人などの動体の姿勢が時間変化するモデルである。

【0150】
【数20】
JP0006218209B2_000021t.gif

【0151】
ここで、δklはKroneckerのデルタである。つまり、これら3つの未知数は、互いに無相関で、時間的にも無相関であるとする。qα、qψ、qはそれらの分散である。サンプリング時間の長さによって、変化していく単位時間あたりの大きさが決まる。たとえば人に装着した場合、歩いて行くときの、数十ミリ秒の間に、どれだけ前傾の角度が変化するか、どれだけ体が傾くか、どれだけ高さが変化するかというものを確率変数として表現しており、物理的に考察することによっておおよそ値が設定できる。

【0152】
(観測モデル)
観測モデルは、路面上の点を観測している場合の式を導出し、その式から推定される値と、実際に観測した値が大きく外れる場合に、路面上になく、差の正負により穴か障害物かを区別する。もし(i、j)番目の点が路面から来ているのであれば、zij(k)はほぼ0である。そこで、(式15)においてzij(k)=0とおくと、次式の非線形観測方程式を得る。

【0153】
【数21】
JP0006218209B2_000022t.gif

【0154】
関数θ(q)は(式13)である。これを、すべての観測値(i、j)について縦ベクトルとして並べることにより、観測モデルが構成できる。

【0155】
(式17)と(式18)式により、非線形状態空間モデルが構成できる。(式18)に、誤差を加えたものを実際に観測すると仮定すると、次式のように観測方程式を表現できる。

【0156】
【数22】
JP0006218209B2_000023t.gif

【0157】
【数23】
JP0006218209B2_000024t.gif

【0158】
また、上式のモデルに対しては、非線形カルマンフィルタを用いることができる。その際に、拡張カルマンフィルタを用いる場合、ヤコビアン(Jacobian)を求める必要がある。

【0159】
【数24】
JP0006218209B2_000025t.gif

【0160】
(状態推定アルゴリズム)
観測値の中には、路面以外のところから来たデータも含まれており、それは、閾値を用いて判定することができる。そうやって、路面以外の点として扱われる点は、上記観測ベクトルから除外した上で、拡張カルマンフィルタを適用する必要がある。

【0161】
(状態推定の詳細)
(路面を観測する場合)
観測値rijと、真値fijの値は、次式のように表現できる。

【0162】
【数25】
JP0006218209B2_000026t.gif

【0163】
ここでeij(g)(k)は、測定誤差を表している。当該測定誤差には、仕様の精度限界に基づく現実のセンサノイズが含まれているとともに、観測モデルと現実のセンサのプロパティとの幾何学的な測定差異に基づく誤差が含まれている。

【0164】
ij(g)(k)の確率密度関数(PDF;Probability Density Function)p(eij(g)(k))は、平均が0、分散がσのガウス分布として次式のように表現できる。

【0165】
【数26】
JP0006218209B2_000027t.gif

【0166】
(障害物を観測する場合)
デプスセンサから障害物までの距離は特定され得ないため、観測値rijは次式のように表現できる。

【0167】
【数27】
JP0006218209B2_000028t.gif

【0168】
ここで、eij(s)(k)の確率密度関数p(eij(s))は例えば次式のように表現できる。

【0169】
【数28】
JP0006218209B2_000029t.gif

【0170】
(溝または凹部を観測する場合)
この場合、障害物を観測する場合とは逆になる。観測値rijは次式のように表現できる。

【0171】
【数29】
JP0006218209B2_000030t.gif

【0172】
ここで、eij(l)(k)の確率密度関数p(eij(l))は例えば次式のように表現できる。

【0173】
【数30】
JP0006218209B2_000031t.gif

【0174】
ここで、rは、測定限界距離を表しており、概ね10m程度である。

【0175】
(確率密度関数モデル)
上述のように定義したPDFに基づいて、混合PDF(mixture PDF)を次式のように表現できる。

【0176】
【数31】
JP0006218209B2_000032t.gif

【0177】
ここで、右辺の第2項は観測した距離が「短い」場合を示し、第3項は観測した距離が「長い」場合を示している。事前確率Pは、観測視野内における障害物の量に概ね基づいて設定されるべきであるとともに、Pは、溝または凹部がない場合には0となるべきである。

【0178】
観測モデルは非線形であるため、状態推定スキームとしてはいくつかの候補が挙げられる。ここでは、拡張カルマンフィルタを適用している。これによれば、粒子フィルタや他の状態推定スキームと比較して、より計算負荷を小さくすることができる。

【0179】
(混合PDFの取り扱い)
次に、混合PDFの取り扱いについて説明する。障害物または穴もしくは凹部が観測源である場合には、観測値が異常値となるため、障害物または穴もしくは凹部から生成された(と思われる)観測データを削除する方法を、簡単なアプローチの一つとして挙げることができる。以下では、この方法に従ったアプローチについて説明する。

【0180】
(観測源の推定)
観測源が路面である場合の確率をPとし、障害物である場合の確率をPとし、溝または凹部である場合の確率をPとしている。rij(k)の観測後には、これらの確率は、次式の事後確率として記載することができる。

【0181】
【数32】
JP0006218209B2_000033t.gif

【0182】
ここで、R1:k={r(1),...,r(k)}(これらのrはベクトルである。)、m∈{g,s,l}であって、rij(k)は、添え字が(i,j)以外である任意の他の値と独立しているものと仮定している。P(cij(k))を、事前確率とすると、観測源が路面である場合の確率密度関数pは次式のように表現できる。

【0183】
【数33】
JP0006218209B2_000034t.gif

【0184】
また、観測源が障害物である場合の確率密度関数pは次式のように表現できる。

【0185】
【数34】
JP0006218209B2_000035t.gif

【0186】
ここで、Hij(k|k-1)の推定値(“^”を付している)は、次式のように表現できる。

【0187】
【数35】
JP0006218209B2_000036t.gif

【0188】
また、fij(k|k-1)の推定値(“^”を付している)は、次式のように表現できる。

【0189】
【数36】
JP0006218209B2_000037t.gif

【0190】
ここで、g(k)は、次式のように表現できる。

【0191】
【数37】
JP0006218209B2_000038t.gif

【0192】
そして、観測源が溝または凹部である場合の確率密度関数pは次式のように表現できる。

【0193】
【数38】
JP0006218209B2_000039t.gif

【0194】
これらのPDFに関して、以下の関係について説明する。まず、観測源が路面である場合の確率と、障害物である場合の確率との間には、次式の関係が成り立つ。ここで、rは、上述の最大測定限界距離である。

【0195】
【数39】
JP0006218209B2_000040t.gif

【0196】
また、観測源が路面である場合の確率と、溝または凹部である場合の確率との間には、次式の関係が成り立つ。

【0197】
【数40】
JP0006218209B2_000041t.gif

【0198】
そして、観測値rij(k)は、次式のように、所定の定値c(k)及びc(k)からなる区間の値となる。

【0199】
【数41】
JP0006218209B2_000042t.gif

【0200】
(検知した物体の取り扱い)
以下では、観測値rij(k)の観測源が、路面より上のレベルの物体である、または路面より下のレベルの場所であると判定された場合の取り扱い方法について説明する。

【0201】
P(cij(k)=g|R1:k)<P(cij(k)=s|R1:k)またはP(cij(k)=g|R1:k)<P(cij(k)=l|R1:k)であるとき、観測値rij(k)の観測源が路面ではないと考えることができる。

【0202】
(式18)において、観測値rij(k)の観測源が路面であることを仮定しているので、このようなときのrij(k)は、真値fijよりも非常に小さくまたは大きくなり得る。つまり、観測データから未知の状態変数を推定するために、このような異常値となる観測値の影響を抑制しなければならない。これは、ロバスト推定問題であると言える。そして、発明者らは、線形システムにおいて、状態及び未知のシステムパラメータを同時に推定する手法を開発している(文献「M. Tanaka and T. Katayama: Identification of a Linear System with Switching Parameters by the EM Algorithm, Systems, Control and Information, Vol.1, No.2, pp. 68-76, 1988.」(以下、文献Tanaka1)、「M. Tanaka and T. Katayama: A Robust Identification of a Linear System with Outliers by the EM Algorithm, Systems, Control and Information, Vol.1, No.4, pp. 117-126, 1988.」(以下、文献Tanaka2))。上述の問題でも、未知のパラメータ(共分散など)を含んでいるが、当該問題はそれ自体が複雑であるため、状態推定スキームに着目して当該問題の単純化を進める。

【0203】
文献Tanaka1において発明者らが実証しているように、非ノミナル問題に基づいて状態推定することができる。しかしながら、文献Tanaka1における手法と同様に状態推定を行うにはデータが充分ではないと考えられるため、文献Tanaka2の記載のように、非ノミナルな観測源から観測されたと判断されたデータを排除するアプローチを採用する。

【0204】
以下では、当該アプローチをさらに詳細に説明する。まず、観測値ri*j*(k)は、いくつかの(i,j)の組において異常値であると判定されたと仮定する。そして、非線形関数fi*j*(k)だけでなく当該観測値も、(式19)で表す観測モデルから排除する。(i,j)の要素を観測ベクトルから排除し、行列Hから(i,j)の行を排除する。以下では、当該排除後の行列を、行列を表す記号の右上に“*”を付して表現する。

【0205】
(アルゴリズムの詳細)
以下では、本実施形態のアルゴリズムにおける、反復計算の詳細について説明する。

【0206】
1.初期値を次式ように設定する。

【0207】
【数42】
JP0006218209B2_000043t.gif

【0208】
2.時間の更新を次式のように行う。

【0209】
【数43】
JP0006218209B2_000044t.gif

【0210】
3.観測値の更新を次式のように行う。

【0211】
【数44】
JP0006218209B2_000045t.gif

【0212】
ここで、観測源を表すcij(k)を、次式のように推定する。

【0213】
【数45】
JP0006218209B2_000046t.gif

【0214】
状態qの推定値(“^”を付している)と、誤差行列Pを以下のように更新する。

【0215】
【数46】
JP0006218209B2_000047t.gif

【0216】
上述の数式において“*”を付した行列またはベクトルは、元のサイズの行列またはベクトルにおいて、特定の行または列を0とした次式で示すIij(k)に対応している。ここで、I(k)は、次式で示す1または0の要素からなる、対角行列である。

【0217】
【数47】
JP0006218209B2_000048t.gif

【0218】
(パラメータの値)
パラメータは、現実のシステムに基づいて定義される必要がある。本発明のデプスセンサを歩行者に装着する場合には、以下のようにパラメータを設定する。

【0219】
(式20)において状態qの初期値(“-”を付している)は、[αψとおくと、本発明のデプスセンサを人体のベルトに装着する場合には、αは20[°]×π/180[rad]である。ここで、[]に囲まれた表記は、数値の単位を表している。また、ψは、0[rad]である。また、hは、人が立ち上がったときの当該デプスセンサの高さとなる(例えば、800[mm])。

【0220】
共分散の特定は、より難度が高い。1測定周期における移動量を考慮することにより、系のダイナミクスの不確かさを特定することができる。本発明においては、フレームレートは、10[fps]である。つまり、1測定周期は、100[ms]である。ここで、αの変動は、例えば5[°]である。この値を標準偏差として取り扱えば、その分散についてはqα=(5π/180)となる。同様に、qψ=(5π/180)となる。また、同様に、q=20[mm]となる。

【0221】
観測雑音は、レンズの歪みによるものである。経験則から、本発明においては、観測雑音を次式のように定めている。

【0222】
【数48】
JP0006218209B2_000049t.gif

【0223】
ここで、σ=10である。

【0224】
このように、俯角・ロール角・高さ推定部17aは、前記距離データ、前記俯角、前記ロール角、及び前記高さの関係に基づく、(式17)と(式18)式により構成した非線形状態空間モデルに対して、上述のように最適カルマンゲインKを求める拡張カルマンフィルタに基づいて変動を許容して前記俯角、前記ロール角、及び前記高さを推定する。

【0225】
(実験結果)
以下に示す実験結果においては、P=0.59、P=0.3、P=0.1、qα=2、qψ=0.5、q=1000としている。

【0226】
(静的な実験)
図15は、実施の形態4に係るシステムにおける障害物検出中の映像を示す図である。図15に示すように、本実施形態に係るシステムにおいては、2つの映像ウインドウが表示されている。右側の大きなウインドウは、検出結果を重ねたカメラ映像を表示している。路面より高い位置にあると判定された観測点は、所定の色により示されている。また、路面より低い位置にあると判定された観測点は、他の色により示されている。そして、距離の値を利用できない観測点は、さらに他の色により示されている。

【0227】
図15に示す映像は、天気が良い日に窓の近くの廊下にて本発明を実施したものであるため、相当な量の直射日光に照らされている映像であり、当該日光に照らされている範囲の観測点では、距離の値を利用できなくなっている。しかしながら、このような場合であっても、障害物は充分に検出可能である。

【0228】
次に、当該映像における数値の意味について説明する。左の0.68という数値は、サンプルデータ(垂直及び水平にそれぞれ8点毎)における路面の割合を示している。また、右の0.56という数値は、全観測点(640×480)における、路面エリアの割合を示している。2行目には、4つの数値が表示されている。左の数値(11 fps)は、現在のフレームレートである。左から2つ目の数値(37.5 deg)は、俯角αの推定値である。本実験においては、(人体に巻かれた)ベルトに装着されており、この結果は妥当である。3つ目の数値(-1.0 deg)は、ロール角である。最も右に表示されている数値(853.9 mm)は、デプスセンサの位置における高さである。

【0229】
左側の縦長のウインドウでは、平面マップ上に路面状態を表示している。当該状態は、8×3ブロックからなっている。中央のエリアは、左右15cmの範囲の状態を示している。左側のエリアは15cmから50cmまでの範囲の状態を、右側のエリアも同様の範囲の状態を示している。

【0230】
(動的な実験)
図16(a)~(c)は、スクーター(図17参照)の籠へ本実施形態に係るデプスセンサを取り付けてスクーターが廊下を走行したときの俯角α、ロール角ψ、高さhの推定値の推移を示す図である。

【0231】
図16(a)~(c)に示すように、時間軸の中央部分を除いて推定値は安定している。これは、当該時間軸の中央部分に対応する時間において、スクーターの運転者がハンドルを回転させたためである。図16(b)に示すように、当該スクーターの構造により、ロール角ψが大きく変化している。同時に、図16(c)に示すように、当該ハンドルの中央(h)は、回転中低くなっている。さらに、俯角αは、図16(a)に示すように、大きくなっている。これらは、実際の状況に適合している。

【0232】
高さhの推定値は実際の値よりも約10cm小さくなるが、その原因は、概ねレンズ系の構造に基づいている。

【0233】
本実施形態に係る状態空間モデルを用いた推定方法におけるアルゴリズムを適用した場合には、遷移速度(パラメータ(α、ψ、h)が、データ取り込み時間ごと(例えば、数十ミリ秒)の間に変化する量)は、上述の状態などを更新するモデルにより制御可能であるため、壁や階段などの高さ方向に大きい障害物などが観測視野に含まれる場合においても、障害物などを正しく検出することができる。加えて、当該状態を更新するモデルにおいて、分散を大きく取ると、当該状態が更新毎に大きく変化することを許容するモデルとすることができる。よって、本実施形態に係る構成により、デプスセンサが距離画像を取得した瞬間におけるデプスセンサの俯角、ロール角、高さを推定することができ、移動体走行時にデプスセンサが揺れるなどにより俯角、ロール角、高さが変動しても、移動体が走行する路面における障害物を検出することができる。

【0234】
(まとめ)
本実施形態において、デプスセンサにより測定したデータの状態空間モデルは、状態ベクトルに移動体の姿勢を表す3つの未知パラメータを含むように、構成されている。拡張カルマンフィルタを適用することにより、リアルタイムの推定スキームを実現している。これは、状態推定スキームにおいて、観測点から路面以外の観測点を除去することにより実現されている。本来、直射日光下においてデプスセンサはデータを取得することができないが、このような環境下であっても、障害物または溝といった危険な状況を比較的正しく検出できている。これは、物体の陰の部分でデータが得られているためである。

【0235】
本発明は上述した各実施形態に限定されるものではなく、請求項に示した範囲で種々の変更が可能であり、異なる実施形態にそれぞれ開示された技術的手段を適宜組み合わせて得られる実施形態についても本発明の技術的範囲に含まれる。
【産業上の利用可能性】
【0236】
本発明は、車両や人などの移動体に取り付けられて路面の距離画像を取得するデプスセンサを備えた障害物検出装置に利用することができる。また、視覚障害者や老人の夜間外出の道具として、応用可能性は広い。さらに、本発明は、視覚障害者の歩行補助に適用することができる。
【符号の説明】
【0237】
1、1a、1b 障害物検出装置
2 デプスセンサ
3 俯角推定部(角度推定手段)
4 検出部(検出手段)
5 移動体(車両、歩行者)
6 路面
7 ハンドル
8 障害物
9 水平線
10 駕籠
11 ディスプレイ
12 スピーカ
13 バッテリ
14 溝
15 側面
16 上面
17 俯角・ロール角推定部(角度推定手段)
17a 俯角・ロール角・高さ推定部(状態推定手段)
L1、L2 線
C1、C2 線
α 俯角
h 高さ
r 距離
ψ ロール角
図面
【図1】
0
【図2】
1
【図3】
2
【図4】
3
【図5】
4
【図6】
5
【図7】
6
【図8】
7
【図9】
8
【図10】
9
【図11】
10
【図12】
11
【図13】
12
【図14】
13
【図15】
14
【図16】
15
【図17】
16