JP5895682B2 - 障害物検出装置及びそれを備えた移動体 - Google Patents

Description

本明細書に開示の技術は、移動体の周囲の障害物を検出する技術に関する。

移動ロボット等の移動体を安全に移動させるために、移動体の周囲の障害物を検出する技術が開発されている。例えば、特許文献１には、レーザレンジファインダによって移動体の周囲の障害物を検出する技術が開示されている。特許文献１に開示の技術では、移動体が移動する移動領域の地図（環境地図）が設けられる。レーザレンジファインダによって障害物が検出されると、その障害物の位置を地図に記憶する。そして、障害物を回避するように移動経路を決定する。

特開２０１１−１１８５９１号公報

移動体と障害物との衝突を回避するためには、移動体から障害物までの距離を正確に計測する必要がある。特に、移動体が狭路を移動する場合等には、障害物との安全距離を長く設定できないため、移動体から障害物までの距離を正確に計測する必要がある。従来の技術では、移動体が平地を移動する際は、移動体から障害物までの距離が正確に計測できるものの、移動体が複数の傾斜面を移動する場合は、移動体から障害物までの距離を正確に計測できないときがある。例えば、図１０（ａ）に示すように、移動体１０が走路面（平面）１００から走路面（傾斜面）１０２に向かって移動する場合において、走路面１０２に障害物１０４があるときを考える。従来の技術では、障害物までの距離が２．９８ｍとされるが、移動体が障害物と接触するまでの実際の距離は２．８４ｍである。このため、従来の技術では、障害物までの距離を２．９８ｍとして移動経路を作成すると、移動体と障害物とが衝突する可能性等が生じる。

本明細書は、複数の傾斜面が連続する移動領域内を移動体が移動する場合であっても、移動体から障害物までの移動距離を正確に算出することができる技術を開示する。

本明細書に開示する障害物検出装置は、移動体が移動する移動領域内の障害物を検出する。この障害物検出装置は、移動領域内の路面の情報を記憶する路面情報記憶手段と、移動体の周囲の路面及び障害物までの距離を計測する距離計測手段と、距離計測手段で得られた計測結果から抽出される路面に関する情報を用いて、路面情報記憶手段に記憶されている路面の情報を更新する更新手段と、路面情報記憶手段に記憶されている複数の路面を連結する連結手段と、距離計測手段で得られた計測結果から抽出される障害物に関する情報を用いて、当該障害物が対応する路面を特定すると共に、その路面における当該障害物の位置を特定する障害物特定手段と、抽出された障害物が対応付けられると共に連結された複数の路面を２次元平面に展開して障害物の位置を記憶する２次元地図を作成する地図作成手段を有する。

上記の障害物検出装置では、移動体の周囲の複数の路面が連結（リンク）され、検出された障害物の路面上の位置が特定される。そして、連結（リンク）された複数の路面を２次元平面に展開し、障害物の位置を記憶する２次元地図を作成する。すなわち、移動体が存在する平面（基準平面）と連結している平面（第１連結平面）は、その基準平面と同一平面となるように基準平面に対して変換され、その第１連結平面と連結している平面（第２連結平面）は、第１連結平面と同一平面となるように第１連結平面に対して変換され、以下、同様に連結された全ての平面が変換される。これによって、障害物の位置を記憶した２次元地図が作成される。連結した複数の平面を２次元平面に展開して２次元地図を作成するため、移動体から障害物までの移動距離（移動体を障害物に衝突するまで移動させたときの移動体の移動距離）を正確に算出することができる。

実施例に係る車両の構成を模式的に示す平面図。 実施例に係る車両の制御系の構成を示すブロック図。 制御装置で行われる２次元地図作成処理の手順を示すフローチャート（その１）。 制御装置で行われる２次元地図作成処理の手順を示すフローチャート（その２）。 走路面（平面）ｉの座標系を模式的に示す図。 走路面毎に障害物を検出する理由を説明するための図。 走路面毎に空間を分割する処理を説明するための図。 走路面（平面）へ障害物を射影する処理を説明するための図。 連結された複数の走路面（平面）を２次元化する処理を説明するための図。 本実施例の作用効果を説明するための図。

以下に説明する実施例の主要な特徴を列記しておく。なお、以下に記載する技術要素は、それぞれ独立した技術要素であって、単独であるいは各種の組合せによって技術的有用性を発揮するものであり、出願時請求項記載の組合せに限定されるものではない。

（特徴１） 本明細書に開示する実施例では、距離計測手段は、所定の周期で移動体の周囲の路面及び障害物までの距離を計測してもよい。この場合に、更新手段は、距離計測手段で得られた計測結果から１又は複数の路面を抽出してもよい。そして、更新手段は、抽出した路面が路面情報記憶手段に記憶されている路面のいずれかと対応する場合は、抽出した路面の情報を用いて路面情報記憶手段に記憶されている対応する路面の路面情報を更新し、抽出した路面が路面情報記憶手段に記憶されている路面のいずれとも対応しない場合は、その抽出した路面を新たな路面として路面情報記憶手段に記憶してもよい。このような構成によると、距離計測手段で得られる路面の情報に基づいて、移動体の周囲の路面の情報を最新の情報に更新することができる。

（特徴２） 本明細書に開示する実施例では、連結手段は、路面情報記憶手段に記憶されている複数の路面の位置と傾きを特定すると共に、移動体の進行方向に位置する複数の路面を移動体が通過する順番で連結してもよい。このような構成によると、移動体の進行方向に連続する路面が連結され、移動体から障害物までの移動距離を算出することができる。

（特徴３） 本明細書に開示する実施例では、障害物特定手段は、抽出される障害物を対応する路面に射影することで、対応する路面上の当該障害物の位置を特定してもよい。このような構成によると、障害物の路面上の位置を簡易に算出することができる。

なお、上述した障害物検出装置は移動体に搭載することができる。これによって、移動体は、移動しながら障害物を検出することができる。そして、障害物が顕出されたときは、その障害物を回避することができる。

本実施例について、図面を参照しながら説明する。図１に示すように、移動体１０は、左右の前輪１２ａ，１２ｂと、１つの後輪１４（従動輪）と、前輪１２ａ，１２ｂ及び後輪１４が取付けられる車体１８を備えている。

前輪１２ａ，１２ｂは、車体１８の前端側であって、車体１８の側面に回転可能に取付けられている。各前輪１２ａ，１２ｂは、インホイールモータ２０ａ，２０ｂ（図２に図示）により独立して駆動される。前輪１２ａ，１２ｂを駆動することで、移動体１０は走路面を走行する。また、左右の前輪１２ａ，１２ｂの回転駆動量（回転角速度）を制御することで、移動体１０は、その進行方向を任意の方向に変えることができる。

後輪１４は、車体１８の後端側であって、車体１８の車幅方向の略中央に取付けられている。本実施例では、後輪１４に、全方位車輪又はキャスタ車輪が用いられている。なお、後輪１４を操舵輪とし、後輪１４を操舵することによって移動体１０の進行方向を制御するようにしてもよい。

車体１８は、車体１８の姿勢角（ロール角，ピッチ角，ヨー角）を検出する３軸のジャイロセンサ２４（図２に図示）と、車体１８の進行方向に位置する路面及び障害物までの距離を計測する距離センサ１６と、エンコーダ２２ａ，２２ｂ、ジャイロセンサ２４及び距離センサ１６の出力に基づいて２次元マップ４６（すなわち、２次元地図）を作成する処理等を行う制御装置３０を備えている。

ジャイロセンサ２４は、車体１８の内部に設置されている。ジャイロセンサ２４は、車体１８の姿勢角（ロール角，ピッチ角，ヨー角）を検出する。図２に示すように、ジャイロセンサ２４は、制御装置３０と電気的に接続されている。ジャイロセンサ２４で検出した車体１８の姿勢角（ロール角，ピッチ角，ヨー角）は、制御装置３０に入力される。

距離センサ１６は、車体１８の前端側の上面であって、車体１８の車幅方向の略中央に取付けられている（図１，９参照）。距離センサ１６は、３次元スキャン型の距離センサである。距離センサ１６は、レーザ光を照射すると共にその照射したレーザ光の反射光を検出し、レーザ光を照射してから反射光を検出するまでの時間で、距離センサ１６から走路面及び障害物までの距離を計測する。レーザ光を照射する方向は、３次元的に走査される。すなわち、図１に示すように、距離センサ１６は、車体１８の前方の所定の角度範囲の領域５０にレーザ光を照射すると共に、レーザ光を照射する方向を高さ方向に変化させる。これにより、車体１８の前方に位置する走路面及び障害物の３次元構造が検出される。図２に示すように、距離センサ１６は、制御装置３０と電気的に接続されている。距離センサ１６で検出した走路面及び障害物の３次元構造は、制御装置３０に入力される。

なお、距離センサ１６としては、例えば、日本信号株式会社製のＭＥＭＳを利用したレーザスキャナーを用いることができる。なお、本実施例では、レーザ光によって距離を計測する距離センサ１６を用いたが、このような例に限られず、公知の種々の方式の距離センサを用いることができる。なお、距離センサ１６がレーザ光を照射する角度範囲が狭い場合（図１に示す領域５０の角度範囲が狭い場合）は、車体１８に複数の距離センサ１６を搭載し、車体１８の前方の領域を漏れなく走査することが好ましい。複数の距離センサ１６を搭載することで、車体１８の前方の走路面及び障害物を漏れなく検出することができる。

制御装置３０は、ＣＰＵ，ＲＯＭ，ＲＡＭを備えたマイクロプロセッサによって構成されている。制御装置３０は、車体１８内に配されている。図２に示すように、制御装置３０は、モータ２０ａ、２０ｂと、エンコーダ２２ａ、２２ｂと、ジャイロセンサ２４と、距離センサ２６に電気的に接続されている。

制御装置３０は、モータ２０ａ，２０ｂを駆動制御することで、移動体１０の進行方向及び走行速度を制御する。すなわち、制御装置３０は、モータ２０ａ，２０ｂに制御指令値を出力することで、前輪１２ａ、１２ｂを駆動する。これによって、移動体１０の進行方向及び走行速度等を制御する。なお、モータ２０ａ，２０ｂの回転角速度（すなわち、前輪１２ａ，１２ｂの回転角速度）は、エンコーダ２２ａ，２２ｂで検出される。エンコーダ２２ａ，２２ｂは、検出したモータ２０ａ，２０ｂの回転角速度を制御装置３０に入力する。なお、制御装置３０による移動体１０の進行方向及び走行速度の制御については、従来公知の方法で行うことができるため、ここではその詳細な説明は省略する。

また、制御装置３０は、エンコーダ２２ａ，２２ｂ、ジャイロセンサ２４及び距離センサ１６の出力に基づいて、２次元マップ４６（すなわち、２次元地図）を作成する処理を行う。すなわち、制御装置３０は、メモリに記憶されているプログラムを実行することで、座標変換部３２、平面抽出部３４、平面追跡部３６、平面連結部３８、障害物検出部４０及びマップ更新部４２として機能する。制御装置３０が上述した各部３２〜４２として機能することで、走路平面データベース４４及び２次元マップ４６が更新される。各部３２〜４２、走路平面データベース４４及び２次元マップ４６については、制御装置３０で行われる２次元マップ作成処理を説明する際に説明する。なお、制御装置３０は、作成された２次元マップ４６に基づいて移動体１０が走行する経路を計画する経路計画部と、経路計画部で計画された経路を走行する際の移動体１０の速度を計画する速度計画部と、速度計画部で計画された速度計画に基づいてモータ２０ａ，２０ｂを制御するモータコントローラとしても機能する。経路計画部、速度計画部及びモータコントローラとしての機能は、従来公知の技術と同様であるため、ここではその詳細な説明は省略する。

次に、制御装置３０によって行われる２次元マップ作成処理について説明する。図３，４は、制御装置３０で実行される処理手順を示すフローチャートである。図３に示すように、制御装置３０は、まず、距離センサ１６によって、移動体１０の前方の走路面及び障害物までの距離を計測する（Ｓ１０）。距離センサ１６によって計測された距離データは、制御装置３０に入力される。

次に、制御装置３０は、移動体１０の自己位置情報を用いて、距離センサ１６によって計測された走路面又は障害物までの距離データ（計測データ）を、絶対座標系のデータに変換する（Ｓ１２）。すなわち、制御装置３０は、ジャイロセンサ２４の出力から、車体１０の姿勢角（ロール角，ピッチ角，ヨー角）を特定することができる。また、制御装置３０は、エンコーダ２２ａ，２２ｂの出力から、車体１０（基準位置）の絶対座標系の位置を算出することができる。さらに、車体１０（基準位置）に対する距離センサ１６の取付位置は既知である。このため、車体１０の姿勢角と、車体１０（基準位置）の絶対座標系の位置と、車体１０（基準位置）に対する距離センサ１６の取付位置とから、距離センサ１６の絶対座標系の位置を算出することができる。したがって、ステップＳ１０において、距離センサ１６がレーザ光を照射した方向と、距離センサ１６で計測された距離が特定されれば、制御装置３０は、ステップＳ１０で計測された走路面又は障害物の絶対座標系の位置を算出することができる。なお、上記のステップＳ１２の処理によって実現される制御装置３０の機能が、図２に示す座標変換部３２の一例に相当する。

次に、制御装置３０は、ステップＳ１２によって得られた絶対座標系に変換した計測データ群から、１又は複数の走路面（平面）を抽出する（Ｓ１４）。すなわち、制御装置３０は、走路面に近い計測データ群（例えば、移動体１０の中心部（車体１８の中心）より低い位置にある計測データ群）から、１又は複数の走路面を抽出する。ここで、平面は方程式ａｘ＋ｂｙ＋ｃｚ＋ｄ＝０で表される。したがって、制御装置３０は、走路面に近い計測データ群から１又は複数の平面を抽出し、その平面のパラメータａ，ｂ，ｃ，ｄを特定する。なお、計測データ群から走路面（平面）を抽出する方法としては、例えば、ＲＡＮＳＡＣやＬＭＥＤＳなどのロバスト推定と呼ばれるアルゴリズムを利用することができる。これらのアルゴリズムは、従来公知の文献（例えば、A. M. Fischler and C. R. Bolles, “Random sample consensus: a paradigm for model fitting with applications to image analysis and automated catography”, Communications of the ACM, vol. 24, no. 6, pp. 381-395, June 1981、または、D. Massart, L. Kaufman, P. Rousseeuw and A. Leroy: “Least median of squares: a robust method for outlier and model error detection in regression and calibration”, Analytica Chimica Acta, Vol. 187, pp. 171-179, 1986）に開示されている。なお、上記のステップＳ１４の処理によって実現される制御装置３０の機能が、図２に示す平面抽出部３６の一例に相当する。

次に、制御装置３０は、ステップＳ１４で抽出した１又は複数の走路面（平面）から１つの走路面（平面）を選択し（Ｓ１６）、また、走路平面データベース４４に記憶されている走路面（平面）から１つの走路面（平面）を選択する（Ｓ１８）。そして、制御装置３０は、ステップＳ１６で選択した走路面（平面）と、ステップＳ１８で選択した走路面（平面）（すなわち、走路平面データベース４４に記憶されている走路面）とが同一平面となるか否かを判定する（Ｓ２０）。すなわち、本実施例では、制御装置３０は、図３，４に示す２次元マップ作成処理を所定の周期で繰り返し実行している。そして、過去に実行された処理周期のステップＳ１０〜Ｓ１４の処理によって抽出された走路面（平面）は、走路平面データベース４４に記憶されている。このため、現在の処理周期のステップＳ１０〜Ｓ１４で抽出される走路面（平面）は、既に走路平面データベース４４に記憶されている走路面である可能性もあるし、新しく検出された走路面（平面）である可能性もある。そこで、制御装置３０は、ステップＳ１４で抽出した走路面ｊ（平面）が走路平面データベース４４に記憶されている走路面ｉ（平面）と同一平面となるか否かを判定する。

具体的には、ステップＳ１６で選択した走路面ｊ（平面）と、ステップＳ１８で選択した走路面ｉ（平面）との平面適合率ｐ_ｉｊを算出する。平面適合率ｐ_ｉｊは、次の手順で算出する。まず、ステップＳ１６で選択した走路面ｊ（平面）に含まれる各点ｋと、ステップＳ１８で選択した走路面ｉ（平面）との距離Ｄ_ｋｉを、下記の式（１）によって算出する。

次に、下記の式（２）によって、平面適合率ｐ_ｉｊを算出する。ここで、Ｄ_{ｔｈｒｅｓｈ}は、ステップＳ１６で選択した走路面ｊ（平面）と、ステップＳ１８で選択した走路面ｉ（平面）とが同一か否かを判断するための閾値であり、移動体１０が移動する移動領域の特性等に応じて適宜決定することができる。

式（２）より明らかなように、走路面ｊに含まれる点のうち、走路面ｉまでの距離が閾値Ｄ_{ｔｈｒｅｓｈ}より短くなる点の数が多くなるほど、平面適合率ｐ_ｉｊが大きくなる。平面適合率ｐ_ｉｊが所定値より小さい場合、制御装置３０は、ステップＳ１６で選択した走路面ｊ（平面）と、ステップＳ１８で選択した走路面ｉ（平面）とが同一平面ではないと判断する。一方、平面適合率ｐ_ｉｊが所定値以上となる場合、制御装置３０は、ステップＳ１６で選択した走路面ｊ（平面）と、ステップＳ１８で選択した走路面ｉ（平面）とが同一平面であると判断する。

ステップＳ１６で選択した走路面ｊ（平面）と、ステップＳ１８で選択した走路面ｉ（平面）とが同一平面であると判断される場合（ステップＳ２０でＹＥＳ）は、制御装置３０は、ステップＳ１６で選択した走路面ｊ（平面）と、ステップＳ１８で選択した走路面ｉとの対応付けを行う（Ｓ２２）。具体的には、ステップＳ１６で選択した走路面ｊのパラメータ（ａ，ｂ，ｃ，ｄ）を、対応付けする走路面ｉ（ステップＳ１８で選択した走路面）に適用する。この際、観測誤差による不安定化を防止するため、時間方向の平滑化処理（例えば、ローパスフィルタ）を行い、走路平面データベース４４に記憶されている走路面ｉ（平面）のパラメータ（ａ，ｂ，ｃ，ｄ）を更新する。

ステップＳ１６で選択した走路面ｊ（平面）と、ステップＳ１８で選択した走路面ｉ（平面）とが同一平面でないと判断される場合（ステップＳ２０でＮＯ）は、制御装置３０は、走路平面データベース４４に記憶されている全ての走路面ｉについてステップＳ２０、Ｓ２２の処理を実行したか否かを判断する（Ｓ２４）。走路平面データベース４４に記憶されている全ての走路面ｉについて処理を実行していない場合（ステップＳ２４でＮＯ）は、制御装置３０は、ステップＳ１８に戻って、ステップＳ１８からの処理を実行する。これによって、走路平面データベース４４に記憶されている全ての走路面ｉと、ステップＳ１６で選択された走路面ｊの組み合わせについて、同一平面か否かの判断が行われる。

走路平面データベース４４に記憶されている全ての走路面ｉについて処理を実行している場合（ステップＳ２４でＹＥＳ）は、ステップＳ１４で抽出された全ての走路面ｊ（平面）についてステップＳ２０〜Ｓ２４の処理を実行したか否かを判断する（Ｓ２６）。ステップＳ１４で抽出された全ての走路面ｊ（平面）について処理を実行していない場合（ステップＳ２６でＮＯ）は、制御装置３０は、ステップＳ１６に戻って、ステップＳ１６からの処理を実行する。これによって、ステップＳ１４で抽出された全ての走路面ｊ（平面）と、走路平面データベース４４に記憶されている全ての走路面ｉの各組み合わせについて、上述した処理が実行される。

ステップＳ１４で抽出された全ての走路面ｊ（平面）について処理を実行している場合（ステップＳ２６でＹＥＳ）は、制御装置３０は、走路平面データベース４４を更新する（Ｓ２８）。具体的には、ステップＳ１４で抽出された走路面ｊ（平面）と、走路平面データベース４４に記憶されている走路面ｉが同一平面であると判断される場合は、ステップＳ１４で抽出された走路面ｊのパラメータ（ａ，ｂ，ｃ，ｄ）で、走路平面データベース４４に記憶されている走路面ｉのパラメータ（ａ，ｂ，ｃ，ｄ）を更新する。一方、ステップＳ１４で抽出された走路面ｊ（平面）と、走路平面データベース４４に記憶されている走路面ｉのいずれもが同一平面ではないと判断される場合は、ステップＳ１４で抽出された走路面ｊを新たな走路面として走路平面データベース４４に記憶する。上記のステップＳ１６〜Ｓ２８の処理によって実現される制御装置３０の機能が、図２に示す平面追跡部３６の一例に相当する。

次に、図４に進んで、制御装置３０は、走路平面データベース４４に記憶されている複数の走路面を連結する（Ｓ３０）。すなわち、制御装置３０は、走路平面データベース４４に記憶されている複数の走路面ｉの基準座標系（代表位置，傾き）を決定し、これら走路面ｉを連結する順序を決定し、これら走路面ｉのリンク構造を特定する。具体的には、制御装置３０は、まず、移動体１０が通過する順に、走路平面データベース４４に記憶されている複数の走路面ｉの順序付けを行う。すなわち、移動体１０が進行する順に、複数の走路面ｉの順序付けを行う。次に、制御装置３０は、順序付けされた走路面ｉ毎に、絶対座標系での位置及び姿勢を表す座標変換行列^ｗＡ_ｉ（４×４の同次行列）を求める（式３）。

上記の式において、ベクトルｘ_ｉ（ｘ_ｉｘ，ｘ_ｉｙ，ｘ_ｉｚ）と、ベクトルｙ_ｉ（ｙ_ｉｘ，ｙ_ｉｙ，ｙ_ｉｚ）と、ベクトルｚ_ｉ（ｚ_ｉｘ，ｚ_ｉｙ，ｚ_ｉｚ）と、ベクトルｔ_ｉ（ｔ_ｉｘ，ｔ_ｉｙ，ｔ_ｉｚ）は、下記の意味を有する。すなわち、図５に示すように、ベクトルｘ_ｉ（ｘ_ｉｘ，ｘ_ｉｙ，ｘ_ｉｚ）は、ベクトルｙ_ｉ（ｙ_ｉｘ，ｙ_ｉｙ，ｙ_ｉｚ）とベクトルｚ_ｉ（ｚ_ｉｘ，ｚ_ｉｙ，ｚ_ｉｚ）に直交するベクトルであり、ベクトルｙ_ｉ（ｙ_ｉｘ，ｙ_ｉｙ，ｙ_ｉｚ）は、走路面ｉ−１と走路面ｉの両法線ベクトルに直行するベクトルであり、ベクトルｚ_ｉ（ｚ_ｉｘ，ｚ_ｉｙ，ｚ_ｉｚ）は、走路面ｉ（平面ｉ）の法線ベクトルであり、ベクトルｔ_ｉ（ｔ_ｉｘ，ｔ_ｉｙ，ｔ_ｉｚ）は、走路面ｉ（平面ｉ）の代表位置（基準位置）の平行移動成分を表すベクトルである。なお、ベクトルｘ_ｉ，ｙ_ｉ，ｚ_ｉ，ｔ_ｉは、次の順番で算出する。まず、制御装置３０は、ベクトルｚ_ｉ（走路面ｉの法線ベクトル）を算出する。次に、ベクトルｚ_ｉ（走路面ｉの法線ベクトル）とベクベクトルｚ_ｉ−１（走路面ｉ−１の法線ベクトル）の外積をとることで、ベクトルｙ_ｉを算出する。次に、ベクトルｙ_ｉとベクベクトルｚ_ｉの外積をとることで、ベクトルｘ_ｉを算出する。最後に、走路面ｉと走路面ｉ−１の交線を求め、この交線上に代表位置（基準位置）を設定し、ベクトルｔ_ｉを計算する。なお、上記のステップＳ３０の処理によって実現される制御装置３０の機能が、図２に示す平面連結部３８の一例に相当する。

次に、制御装置３０は、ステップＳ３０で連結した走路面ｉ毎に、その走路面ｉの上方又は下方に位置する障害物（すなわち、ステップＳ１２で得られた計測データ群のうち、走路面ｉの近傍の計測データ群を除外したもの）を検出し、検出した障害物をその走路面ｉに射影する（Ｓ３２〜Ｓ３８）。具体的には、制御装置３０は、まず、ステップＳ３０で連結した複数の走路面から１つの走路面を選択する（Ｓ３２）。

次に、制御装置３０は、ステップＳ３２で選択した走路面ｉの上方又は下方に位置する障害物を検出する（Ｓ３４）。ここで、ステップＳ３４の処理を実行する際には、まず、ステップＳ１２で得られた計測データ群を、連結された各走路面が占有する領域毎に分割する。すなわち、本実施例では、ステップＳ１２で得られた計測データ群のうち走路面の近傍にない計測データ群を障害物とする。このため、ステップＳ１２で得られた計測データ群を走路面毎に分割しないと、ある走路面においては障害物となる計測データが、他の走路面においては走路面上の点となる場合が生じる。例えば、図６（ａ）に示すように、走路面を分割しない状態で、走路面２（無限に大きい平面）を基準として走路面１又は３を見ると、走路面３又は走路面１は、走路面１の上方又は下方に位置する障害物（段差）として検出されることになる。一方、図６（ｂ）に示すように、走路面毎に分割すると、走路面１又は３が障害物（段差）として検出されることはない。したがって、まず、ステップＳ１２で得られた計測データ群を、各走路面ｉが占有する領域毎に分割する。具体的には、図７に示すように、走路面ｉをその法線方向に水平移動させたときに占める空間を、その走路面ｉの占有領域とする。次いで、その占有領域内の計測データを、その走路面ｉの計測データ群として認定する。走路面ｉ毎に計測データ群を認定すると、その認定された計測データ群のうち、走路面ｉの近傍に位置しない計測データ群（走路面ｉからの距離が所定値以上となる計測データ群）を障害物として認定する。

次に、制御装置３０は、ステップＳ３４で検出した障害物を、ステップＳ３２で選択した走路面ｉに射影する（Ｓ３８）。これによって、図８に示すように、ステップＳ３４で検出された障害物Ｐが、走路面ｉの座標系の点Ｐ_ｐに変換される。ここで、障害物Ｐ（絶対座標系）から射影点Ｐ_ｐ（絶対座標系）への変換は、下記の式（４）で算出される。

上記の式（４）における定数Ｋは、下記の式（５）によって算出される。また、ａ_ｉ，ｂ_ｉ，ｃ_ｉは、走路面ｉのパラメータである。

したがって、上記の式（４）によって算出される射影点Ｐ_ｐ（絶対座標系）を、走路面ｉの座標系の点Ｐ_ｐｉ（平面座標系）に変換する。すなわち、Ｐ_ｐｉ＝^０Ａ_ｉ ^−１Ｐｐによって算出される。^０Ａ_ｉは、絶対座標系から走路面ｉの座標系へ変換するための座標変換行列である。

次に、制御装置３０は、連結された複数の走路面の全てについて、ステップＳ３４，３６の処理を実行したか否かを判定する（Ｓ３８）。連結された複数の走路面の全てについてステップＳ３４，３６の処理を実行していない場合（ステップＳ３８でＮＯ）は、制御装置３０は、ステップＳ３２に戻って、ステップＳ３２からの処理を実行する。これによって、ステップＳ３０で連結された複数の走路面の全てについて、ステップＳ３４，３６の処理が行われる。すなわち、各走路面の上方又は下方の障害物が検出され、その障害物が走路面上に射影される。なお、上記のステップＳ３２〜Ｓ３８の処理によって実現される制御装置３０の機能が、図２に示す障害物検出部４０の一例に相当する。

次に、制御装置３０は、連結されると共に障害物が射影された複数の走路面を、２次元平面に展開して２次元マップを作成する（Ｓ４０）。すなわち、制御装置３０は、移動体１０の車輪１２ａ，１２ｂ、１４の回転量に影響を与える成分のみを残して２次元化する。具体的には、図９に示すように、（１）走路面ｉ−１と走路面ｉとの間の平行移動成分^ｉ−１Ｔ_ｉを変化させない、（２）走路面ｉ−１と走路面ｉとの間のヨー角周りの回転成分^ｉ−１Ｙ_ｉを変化させない、（３）走路面ｉ−１と走路面ｉとの間のピッチ角、ロール角周りの回転成分^ｉ−１Ｐ_ｉ，^ｉ−１Ｒ_ｉを単位行列にする、という条件に従って、各走路面ｉを２次元平面に展開する。

ここで、走路面ｉ−１と走路面ｉとの３次元空間での相対座標変換行列^ｉ−１Ａ_ｉは、^ｉ−１Ｔ_ｉ ^ｉ−１Ｙ_ｉ ^ｉ−１Ｐ_ｉ ^ｉ−１Ｒ_ｉで表される。ここで、^ｉ−１Ｔ_ｉと、^ｉ−１Ｙ_ｉと、^ｉ−１Ｐ_ｉと、^ｉ−１Ｒ_ｉは、下記の式（６）で表される。

ここで、走路面ｉ−１と走路面ｉとの間のピッチ角、ロール角周りの回転成分^ｉ−１Ｐ_ｉと^ｉ−１Ｒ_ｉが単位行列であるため、走路面ｉ−１と走路面ｉとの２次元平面での相対座標変換行列^ｉ−１Ａ_２Ｄｉは、^ｉ−１Ｔ_ｉ ^ｉ−１Ｙ_ｉとなる。したがって、走路面ｉと走路面ｉ−１のヨー角が分かれば、走路面ｉ−１と走路面ｉとの２次元平面での相対座標変換行列^ｉ−１Ａ_２Ｄｉが算出される。ここで、走路面ｉと走路面ｉ−１のヨー角は、走路面ｉのベクトルｙ_ｉ（ｙ_ｉｘ，ｙ_ｉｙ，ｙ_ｉｚ）と、走路面ｉ−１のベクトルｙ_ｉ−１（ｙ_{（ｉ−１）ｘ}，ｙ_{（ｉ−１）ｙ}，ｙ_{（ｉ−１）ｚ}）より算出される。したがって、制御装置３０は、走路面ｉ−１と走路面ｉとの２次元平面での相対座標変換行列^ｉ−１Ａ_２Ｄｉを算出することができる。その結果、走路面ｉの２次元絶対座標系での位置・姿勢は、下記の式（７）で示す座標変換行列を用いて算出することができる。

したがって、ステップＳ３６で走路面ｉに射影した障害物Ｐの座標Ｐ_ｐｉを２次元絶対座標系に変換すると、２次元マップ上の障害物Ｐの位置Ｐ_ｐ２Ｄｗ＝^ｗＡ_２ＤｉＰ_ｐｉにより算出することができる。

上記の手順で２次元マップが作成されると、制御装置３０は、作成された２次元マップをメモリに記憶（更新）し（Ｓ４２）、２次元マップ作成処理を終了する。なお、上記のステップＳ４０，４２の処理によって実現される制御装置３０の機能が、図２に示すマップ更新部４２の一例に相当する。

上述した説明から明らかなように、制御装置３０は、２次元マップ作成処理を所定の周期で繰り返し実行する。このため、移動体１０は、所定の周期毎に、距離センサ１６で計測された新たなデータを用いて、２次元マップ４６を更新する。したがって、移動体１０が移動している間は、移動体１０の移動に応じて新たな２次元マップ４６が作成されて更新されてゆくこととなる。

本実施例の移動体１０では、移動体１０が走行する走路面を検出し、その検出した走路面毎に障害物を検出する。走路面と障害物を分離して検出するため、走路面と障害物とを誤って検出することを防止することができる。例えば、移動体１０の前方に上りスロープが存在する場合、走路面と障害物とを分離して検出しないと、走路面（上りスロープ）を障害物として検出してしまう。一方、本実施例の移動体１０では、走路面と障害物とを分離して検出するため、このような誤検出を防止することができる。

また、移動体１０の車輪１２ａ，１２ｂ，１４の回転量に影響を与える成分のみを残した２次元マップが作成される。このため、移動体１０が複数の傾斜した走路面が連続する移動領域内を移動する場合であっても、移動体１０から障害物までの移動距離（移動体１０を障害物に衝突するまで移動させたときの移動体の移動距離）を正確に算出することができる。例えば、図１０（ａ）に示す例において、従来の技術では、障害物１０４までの移動距離が、ある時点では２．９８ｍと実際よりも長く算出され、ある時点（すなわち、障害物１０４と同一の平面に到達した時）に急に正確な距離（すなわち、算出されていた距離よりも短い距離）が算出される。この様な場合、急停止や障害物との衝突などが起こりうる。また、車輪型の移動体では、急に進路を変えることができず、後退せざるを得なくなることも起こり得る。一方、本実施例の技術では、図１０（ａ）に示す例においても、移動体１０から障害物１０４までの移動距離が２．８４ｍと正確に算出される。障害物までの移動距離を正確に算出できるため、制御装置３０は、移動体１０が障害物に衝突しないように適切な移動経路等を作成することができる。また、制御装置３０は、障害物との安全間隔（安全マージン）を小さくすることができるため、狭路での走行を適切に行うことができる。

また、移動体１０の周辺の走路面の傾斜角を事前に推定できるため、走行不可能な急斜面への進入を防止することができ、また、車体１８の横転を防止するために適切な角度で傾斜面に進入することができる。

なお、上述したことから明らかなように、本実施例の技術は、障害物との間隔を狭くしながら障害物を安全に回避して移動することができる。このため、道幅が狭く、かつ、スロープとなっている通路を移動する移動体（例えば、電動車椅子等）に好適に応用することができる。

最後に、上述した実施例と請求項との対応関係を説明しておく。走路平面データベース４４が請求項でいう「路面情報記憶手段」の一例であり、距離センサ１６が請求項でいう「距離計測手段」の一例であり、平面追跡部３６が請求項でいう「更新手段」の一例であり、平面連結部３８が請求項でいう「連結手段」の一例であり、障害物検出部４０が請求項でいう「障害物特定手段」の一例であり、マップ更新部４２が請求項でいう「地図作成手段」のの一例である。

以上、本発明の具体例を詳細に説明したが、これらは例示にすぎず、特許請求の範囲を限定するものではない。特許請求の範囲に記載の技術には、以上に例示した具体例をさまざまに変形、変更したものが含まれる。

例えば、上述した実施例では、本明細書に開示する技術を車輪型の移動体に適用した例であったが、本明細書に開示する技術は、歩行型の移動体に適用することもできる。

本明細書または図面に説明した技術要素は、単独であるいは各種の組み合わせによって技術的有用性を発揮するものであり、出願時請求項記載の組み合わせに限定されるものではない。また、本明細書または図面に例示した技術は複数目的を同時に達成するものであり、そのうちの一つの目的を達成すること自体で技術的有用性を持つものである。

１０ 移動体
１２ａ，１２ｂ 前輪
１４ 後輪
１６ 距離センサ
１８ 車体
２０ａ，２０ｂ モータ
２４ ジャイロセンサ
３０ 制御装置

Claims (5)

1. 移動体が移動する移動領域内の障害物を検出する障害物検出装置であって、
   移動領域内の路面の情報を記憶する路面情報記憶手段と、
   移動体の周囲の路面及び障害物までの距離を計測する距離計測手段と、
   距離計測手段で得られた計測結果から抽出される路面に関する情報を用いて、路面情報記憶手段に記憶されている路面の情報を更新する更新手段と、
   路面情報記憶手段に記憶されている複数の路面を連結する連結手段と、
   距離計測手段で得られた計測結果から抽出される障害物に関する情報を用いて、当該障害物が対応する路面を特定すると共に、その路面における当該障害物の位置を特定する障害物特定手段と、
   抽出された障害物が対応付けられると共に連結された複数の路面を２次元平面に展開して障害物の位置を記憶する２次元地図を作成する地図作成手段と、
   を有する障害物検出装置。
2. 距離計測手段は、所定の周期で移動体の周囲の路面及び障害物までの距離を計測し、
   更新手段は、
   距離計測手段で得られた計測結果から１又は複数の路面を抽出し、
   抽出した路面が路面情報記憶手段に記憶されている路面のいずれかと対応する場合は、抽出した路面の情報を用いて路面情報記憶手段に記憶されている対応する路面の路面情報を更新し、
   抽出した路面が路面情報記憶手段に記憶されている路面のいずれとも対応しない場合は、その抽出した路面を新たな路面として路面情報記憶手段に記憶する、
   請求項１に記載の障害物検出装置。
3. 連結手段は、路面情報記憶手段に記憶されている複数の路面の位置と傾きを特定すると共に、移動体の進行方向に位置する複数の路面を移動体が通過する順番で連結する、請求項１又は２に記載の障害物検出装置。
4. 障害物特定手段は、抽出される障害物を対応する路面に射影することで、対応する路面上の当該障害物の位置を特定する、請求項１〜３のいずれか一項に記載の障害物検出装置。
5. 請求項１〜４のいずれか一項に記載の障害物検出装置を備える移動体。

Images