姿勢角検出装置、姿勢角検出方法、及び床面検出装置
【課題】コストがかかることがなく、姿勢角または走行する平面を検出する。
【解決手段】距離演算部２８は、受光装置２０で受光された反射光に基づいて、床面上の複数の点までの距離Ｌｍ（ｉ）を演算する。姿勢検出部３０は、受光装置２０の各受光素子の出力から演算された各々の距離Ｌｍ（ｉ）と、複数の候補姿勢角θ´の各々の場合における距離計測装置１２から床面までの理論上の各々の距離Ｌｒ（θ´）とに基づいて、姿勢角θを検出する。
【解決手段】距離演算部２８は、受光装置２０で受光された反射光に基づいて、床面上の複数の点までの距離Ｌｍ（ｉ）を演算する。姿勢検出部３０は、受光装置２０の各受光素子の出力から演算された各々の距離Ｌｍ（ｉ）と、複数の候補姿勢角θ´の各々の場合における距離計測装置１２から床面までの理論上の各々の距離Ｌｒ（θ´）とに基づいて、姿勢角θを検出する。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、姿勢角検出装置、姿勢角検出方法、及び床面検出装置に関する。
【背景技術】
【０００２】
従来、所定の周波数で強度変調された光を対象物に対して出射してから、対象物からの反射光を受光部で受光するまでの時間に基づいて対象物までの距離を計測するタイムオブフライト方式の距離画像センサが知られている。また、この距離画像センサを用いて、移動体が走行する平面または観測対象物までの距離を計測する障害物検出装置が知られている（例えば、特許文献１参照）。距離画像センサは、所定の高さで斜め下向きに所定の姿勢角（所定の傾き）で、移動体に搭載されている。この障害物検出装置は、移動体の走行時の振動等により変動する姿勢角の各々に対応する平面の距離画像を候補基準画像として記憶手段に記憶している。これにより、姿勢角が変動した場合であっても、測定した距離画像と、複数の候補基準画像とを比較し、一致度が最も高い候補基準画像を用いることによって、走行する平面を検出することができる。
【特許文献１】特開２００６−２６２００９号公報
【発明の開示】
【発明が解決しようとする課題】
【０００３】
しかしながら、特許文献１に記載の障害物検出装置では、候補基準画像を、ロール方向、ピッチ方向について、変動が予測される姿勢角毎に予め用意しておく必要がある。例えば、予測される姿勢角の変動の範囲をロール方向±１０°、ピッチ方向±２０°とし、１°毎に記憶手段に記憶する場合には、２０×４０＝８００パターンもの候補基準画像を記憶手段に記憶しておく必要がある。しかしながら、８００パターンもの候補基準画像を記憶するためには、容量が大きい記憶手段が必要となる。このような容量が大きい記憶手段を用いるとコストがかかってしまう。そのため、特許文献１に記載の障害物検出装置は、姿勢角が変動した場合に走行する平面を検出するためには、コストがかかってしまう、という問題点がある。
【０００４】
本発明は上記問題点を解決するために成されたもので、コストがかかることがなく、姿勢角を検出する姿勢角検出装置、姿勢角検出方法、及び走行する平面を検出する床面検出装置を提供することを目的とする。
【課題を解決するための手段】
【０００５】
上記の目的を達成するために第１の発明に係る姿勢角検出装置は、床面上に変調光を照射する光源、及び前記光源から照射された光の反射光を受光する複数の受光素子が配列された受光手段を備え、前記床面から所定の高さの位置に前記床面に対して所定の姿勢角になるように設けられた計測手段と、前記受光手段で受光された反射光に基づいて、前記床面上の複数の点までの距離を演算する距離演算手段と、前記受光手段の各受光素子の出力から演算された各々の距離と、複数の候補姿勢角の各々の場合における前記計測手段から前記床面までの理論上の各々の距離とに基づいて、前記姿勢角を検出する姿勢角検出手段とを含んで構成されている。
【０００６】
第１の発明に係る姿勢角検出装置によれば、距離演算手段は、受光手段で受光された反射光に基づいて、床面上の複数の点までの距離を演算する。姿勢角検出手段は、受光手段の各受光素子の出力から演算された各々の距離と、複数の候補姿勢角の各々の場合における計測手段から床面までの理論上の各々の距離とに基づいて、姿勢角を検出する。従って、第１の発明に係る姿勢角検出装置によれば、容量の大きい記憶手段を必要とすることなく、姿勢角を検出することができる。よって、第１の発明に係る姿勢角検出装置は、コストがかかることなく、姿勢角を検出することができる。
【０００７】
上記の目的を達成するために第２の発明に係る姿勢角検出装置は、床面上に変調光を照射する光源、及び前記光源から照射された光の反射光を受光する複数の受光素子が配列された受光手段を備え、前記床面から所定の高さの位置に前記床面に対して所定の姿勢角になるように設けられた計測手段と、前記受光手段で受光された反射光に基づいて、前記床面上の複数の点までの距離を演算する距離演算手段と、前記受光手段の各受光素子の出力から演算された各々の距離に基づいて定まる近似平面と、複数の候補姿勢角の各々の場合における理論上の各々の平面とに基づいて、前記姿勢角を検出する姿勢角検出手段とを含んで構成されている。
【０００８】
第２の発明に係る姿勢角検出装置によれば、距離演算手段は、受光手段で受光された反射光に基づいて、床面上の複数の点までの距離を演算する。姿勢角検出手段は、受光手段の各受光素子の出力から演算された各々の距離に基づいて定まる近似平面と、複数の候補姿勢角の各々の場合における理論上の各々の平面とに基づいて、姿勢角を検出する。従って、第２の発明に係る姿勢角検出装置によれば、特に、床面に対する障害物の占める範囲が小さい場合や、床面上に障害物が存在しない場合などには、容量の大きい記憶手段を必要とすることなく、姿勢角を検出することができる。よって、第２の発明に係る姿勢角検出装置は、コストがかかることなく、姿勢角を検出することができる。
【０００９】
また、第１または第２の発明に係る姿勢角検出装置における前記距離演算手段は、前記受光手段によって受光された反射光の受光量が所定量以下の場合には、所定の距離を示す情報を演算することができる。これにより、所定の距離を示す情報を、例えば、距離の計測が不可能であることを示す情報とすることで、所定の周波数で光が１周期に進む距離より大きい距離に存在する床面までの距離を距離の計測が不可能であることを示す情報として演算することができるので、距離の計測の誤りを防止することができる。また、所定の距離を示す情報を、例えば、光源から光が照射されてから受光手段によって反射光が受光されるまでの時間に応じて定まる床面までの距離と所定の周波数で光が１周期に進む距離とを加算した距離を示す情報とすることで、所定の周波数で光が１周期に進む距離より大きい距離に存在する床面までの距離を正確に演算することができるので、距離の計測の誤りを防止することができる。
【００１０】
上記の目的を達成するために第３の発明に係る床面検出装置は、第１の発明に係る距離計測装置または第２の発明に係る距離計測装置と、前記姿勢角検出手段によって検出された姿勢角に基づいて、床面を検出する床面検出手段とを含んで構成されている。これにより、容量の大きい記憶手段を必要とすることなく、床面を検出することができる。よって、第３の発明に係る床面検出装置は、コストがかかることなく、床面を検出することができる。
【００１１】
上記の目的を達成するために第４の発明に係る姿勢角検出方法は、床面上に変調光を照射する光源、及び前記光源から照射された光の反射光を受光する複数の受光素子が配列された受光手段を備え、前記床面から所定の高さの位置に前記床面に対して所定の姿勢角になるように設けられた計測手段の受光手段が前記反射光を受光し、前記受光手段で受光された反射光に基づいて、前記床面上の複数の点までの距離を演算し、前記受光手段の各受光素子の出力から演算された各々の距離と、複数の候補姿勢角の各々の場合における前記計測手段から前記床面までの理論上の各々の距離とに基づいて、前記姿勢角を検出する。
【００１２】
第４の発明に係る姿勢角検出方法によれば、受光手段で受光された反射光に基づいて、床面上の複数の点までの距離を演算し、受光手段の各受光素子の出力から演算された各々の距離と、複数の候補姿勢角の各々の場合における計測手段から床面までの理論上の各々の距離とに基づいて、姿勢角を検出する。従って、第４の発明に係る姿勢角検出方法によれば、容量の大きい記憶手段を必要とすることなく、姿勢角を検出することができる。よって、第４の発明に係る姿勢角検出方法は、コストがかかることなく、姿勢角を検出することができる。
【００１３】
上記の目的を達成するために第５の発明に係る姿勢角検出方法は、床面上に変調光を照射する光源、及び前記光源から照射された光の反射光を受光する複数の受光素子が配列された受光手段を備え、前記床面から所定の高さの位置に前記床面に対して所定の姿勢角になるように設けられた計測手段の受光手段が前記反射光を受光し、前記受光手段で受光された反射光に基づいて、前記床面上の複数の点までの距離を演算し、前記受光手段の各受光素子の出力から演算された各々の距離に基づいて定まる近似平面と、複数の候補姿勢角の各々の場合における理論上の各々の平面とに基づいて、前記姿勢角を検出する。
【００１４】
第５の発明に係る姿勢角検出方法によれば、受光手段で受光された反射光に基づいて、床面上の複数の点までの距離を演算し、受光手段の各受光素子の出力から演算された各々の距離に基づいて定まる近似平面と、複数の候補姿勢角の各々の場合における理論上の各々の平面とに基づいて、姿勢角を検出する。従って、第５の発明に係る姿勢角検出方法によれば、容量の大きい記憶手段を必要とすることなく、姿勢角を検出することができる。よって、第５の発明に係る姿勢角検出方法は、コストがかかることなく、姿勢角を検出することができる。
【発明の効果】
【００１５】
以上説明したように、本発明の姿勢角検出装置、姿勢角検出方法、及び床面検出装置によれば、コストがかかることなく、姿勢角または床面を検出することができる、という効果が得られる。
【発明を実施するための最良の形態】
【００１６】
以下、図面を参照して本発明の実施の形態を詳細に説明する。
【００１７】
図１に示すように、本発明の実施の形態の床面検出装置１は、姿勢角検出装置１０と、走行体１６とを含んで構成されている。姿勢角検出装置１０は、距離計測装置１２と、制御装置１３とを含んで構成されている。姿勢角検出装置１０は、床面を走行する走行体１６に搭載されている。
【００１８】
距離計測装置１２は、図２に示すように、床面から所定の高さＨの位置に、光源１８から照射される光の光軸７０と水平方向との成す角度が所定の姿勢角θ、例えば姿勢角３０°で設置されている。
【００１９】
距離計測装置１２は、図３に示すように、距離演算装置１５と、光源１８と、受光装置２０とを備えている。
【００２０】
光源１８は、図４に示すように、マトリックス状に配列された光を照射するＬＥＤを複数個、例えば５０〜６０個含んで構成されている。各ＬＥＤは、光源制御部２２からの制御によって、所定周波数、例えば２０ＭＨｚの正弦波で強度変調された光を照射する。
【００２１】
受光装置２０は、複数の受光素子を含んで構成されており、例えば、複数の画素を備えたＣＭＯＳセンサを含んで構成されている。受光装置２０は、対象物２２または床面からの反射光を受光する。また、受光装置２０は、受光した反射光の光量に基づいて検出信号を出力する。
【００２２】
距離演算装置１５は、各種処理ルーチンを実行するプログラム等を記憶した記憶媒体としてのＲＯＭ（図示せず）、プログラムをＲＯＭから読み出して実行するＣＰＵ（図示せず）、データを一時的に記憶するＲＡＭ（図示せず）、及びＩ／Ｏ（入出力）ポート（図示せず）を含んだマイクロコンピュータで構成されている。距離演算装置１５は、光源制御部２２、飛行時間算出部２４、受光量検出部２６、及び距離演算部２８を備えている。
【００２３】
光源制御部２２は、所定の周波数、例えば２０ＭＨｚの正弦波で強度変調された変調光を照射するように光源１８を制御する。また、光源制御部２２は、光源１８の制御タイミングを示す信号を出力する。
【００２４】
飛行時間算出部２４は、受光装置２０からの検出信号等によって、光源１８から照射された光と受光装置２０によって受光した反射光の位相のずれを検出する。そして、飛行時間算出部２４は、検出した位相のずれに基づいて、光源１８から光が照射されてから受光装置２０によって反射光が受光されるまでの時間を算出する。
【００２５】
受光量検出部２６は、受光装置２０によって受光された反射光の受光量を検出する。
【００２６】
距離演算部２８は、基準受光量算出処理及び距離演算処理を行う。
【００２７】
基準受光量算出処理では、距離演算部２８は、飛行時間算出部２４で算出された時間に基づいて、床面または対象物２２までの仮の距離を受光装置２０の受光素子毎に計算する。そして、距離演算部２８は、図５に示すように、計算された仮の距離のうち、所定値以下、例えば、０．３ｍ以下の距離が計算された受光素子が受光した受光量の最大値Ｒｍａｘと、その際に演算された距離Ｘ１とを用いて、以下の式（１）によって、反射率ｒを算出する。
Ｒｍａｘ＝ｒ／（Ｘ１）２・・・・・式（１）
【００２８】
そして、距離演算部２８は、算出された反射率ｒと距離計測装置１２に固有である最大計測距離Ｘｍａｘ（本実施の形態では７．５ｍ）とを用いて、以下の式（２）によって、最大計測距離Ｘｍａｘにおける反射光強度Ｒを算出する。なお、この最大計測距離Ｘｍａｘは、距離計測装置１２に固有の値であり、光源１８から照射される光の周波数等に基づいて定まる。
Ｒ＝ｒ／（Ｘｍａｘ）２・・・・・式（２）
【００２９】
そして、距離演算部２８は、式（２）により算出されたＲを距離演算処理で用いる基準受光量Ｒｒｅｆとして算出する。
【００３０】
距離演算処理では、距離演算部２８は、受光量検出部２６によって検出された受光量が基準受光量Ｒｒｅｆより大きい受光素子については、飛行時間算出部２４で算出された時間に基づいて、床面または対象物２２までの距離を演算する。一方、距離演算部２８は、受光量が基準受光量Ｒｒｅｆ以下の受光素子については、距離の計測が不可能であることを示す情報を床面または対象物２２までの距離を示す情報として演算する。
【００３１】
従来、図６（Ａ）に示すように、最大計測距離Ｘｍａｘ（本実施の形態では７．５ｍ）を超えた距離に存在する対象物２２、例えば、８．５ｍの距離に存在する対象物２２を１．０ｍと誤計測していた。これは、１波長よりも大きく位相がずれている場合であっても、この位相のずれを１波長以内の位相のずれとみなして、光の飛行時間を算出しているためである。本実施形態のような、基準受光量算出処理及び距離演算処理を距離演算部２８が行うことにより、図６（Ｂ）に示すように、最大計測距離Ｘｍａｘを超えた位置に存在する対象物２２については、距離の計測が不可能であることを示す情報を、距離を示す情報として演算することで、誤計測が無くなる。
【００３２】
なお、距離演算処理において、受光量が基準受光量Ｒｒｅｆ以下の受光素子については、図６（Ｃ）に示すように、飛行時間算出部２４によって算出された時間に応じて定まる床面または対象物２２までの距離に、所定の周波数で光が１周期に進む距離を加えることによって床面または対象物２２までの距離を示す情報を演算するようにしてもよい。このようにすることで、最大計測距離Ｘｍａｘ（本実施の形態では７．５ｍ）を超えた距離に存在する対象物２２、例えば、８．５ｍの距離に存在する対象物２２を８．５ｍ（＝１．０ｍ＋７．５ｍ）と正確に計測することができる。
【００３３】
制御装置１４は、姿勢検出処理ルーチンを実行するプログラム、及び各種処理ルーチンを実行するプログラム等を記憶した記憶媒体としてのＲＯＭ（図示せず）、プログラムをＲＯＭから読み出して実行するＣＰＵ（図示せず）、データを一時的に記憶するＲＡＭ（図示せず）、及びＩ／Ｏ（入出力）ポート（図示せず）を含んだマイクロコンピュータで構成されている。制御装置１４は、姿勢検出部３０、設置条件記憶部３２、及び理論距離演算部３４を備えている。
【００３４】
設置条件記憶部３２には、候補姿勢角θ´が複数記憶されている。この候補姿勢角θ´は、走行体１６が走行した際に距離計測装置１２に振動が加わったことにより、移動体１６の進行方向（ピッチ方向）に、姿勢角θが所定の姿勢角θ（本実施の形態では、例えばθ＝３０°）からずれた場合に予想される距離計測装置１２の姿勢角θを表す。例えば、本実施の形態では、予め距離計測装置１２の姿勢角を３０°として設置した場合に、走行体１６が走行した際に姿勢角が最大で±５°ずれることが予想されるとする。このときには、設置条件記憶部３２に、２５°から３５°までの、例えば１°毎の候補姿勢角θ´を記憶しておく。また、設置条件記憶部３２には、床面から距離計測装置１２までの高さＨが記憶されている。
【００３５】
理論距離演算部３４は、設置条件記憶部３２に記憶されている高さＨ及び候補姿勢角θ´を用いて、距離計測装置１２の姿勢角θが各候補姿勢角θ´である場合の距離計測装置１２から床面までの光源１８の光軸７０上における理論上の距離（理論距離）を演算する。
【００３６】
姿勢検出部３０は、距離演算部２８で演算された、受光装置２０の複数の受光素子のそれぞれの距離のうち、Ｎ個の受光素子のそれぞれに対応する距離と、理論距離演算部３４によって演算された、候補姿勢角θ´毎の理論距離とに基づいて、距離計測装置１２の姿勢角θを検出する。
【００３７】
走行体１６は、床面を検出する床面検出部３６を備えている。
【００３８】
床面検出部３６は、距離演算部２８で演算された受光素子毎の距離、姿勢検出部３０によって検出された姿勢角θ等を用いて床面を検出する。そして、床面検出部３６は、検出した床面に対する障害物を検出する。そして、床面検出部３６は、検出した障害物に対応するように、走行体１６の走行（例えば、移動速度及び移動方向）を制御する。
【００３９】
次に、制御装置１４のＣＰＵが行う姿勢検出処理ルーチンについて図７を用いて説明する。なお、本実施の形態において、この姿勢検出処理ルーチンは、床面検出部３６からの信号に基づいて走行体１６が所定距離を走行する毎に実行される。
【００４０】
まずステップ１００で、姿勢検出部３０は、図８に示すように、距離演算部２８によって演算された、受光装置２０のＣＭＯＳセンサの複数の受光素子の床面までの距離のうち、Ｎ個の受光素子の各々から床面ｍ（ｉ）（ｉ＝１、２、・・・、Ｎ）までの距離Ｌｍ（ｉ）（ｉ＝１、２、・・・、Ｎ）を取得する。ここで、Ｎ個の受光素子についてはどのような方法で選んでもよく、受光装置２０の複数の受光素子のうち、等間隔となるようにＮ個の受光素子を選んでもよい。なお、同図に図示されるθｍ（ｉ）（ｉ＝１、２、・・・、Ｎ）は、床面ｍ（ｉ）の各々から反射されて受光装置２０（Ｐ）まで到達する光と、床面との成す角度を表している。
【００４１】
次のステップ１０２では、理論距離演算部３４は、図９に示すように、設置条件記憶部３２に記憶されている各候補姿勢角θ´と高さＨとから、距離計測装置１２の姿勢角θが各候補姿勢角θ´である場合の距離計測装置１２から床面までの光源１８の光軸７０上における理論距離Ｌｒ（θ´）（θ´＝２５、２６、・・・、３５）を下記の式（３）により演算する。
【００４２】
Ｌｒ（θ´）＝Ｈ／ｓｉｎθ´・・・・・式（３）
なお、図９には、例として、候補姿勢角θ´が２５°の場合（Ａ）、候補姿勢角θ´が２６°の場合（Ｂ）、候補姿勢角θ´が３５°の場合（Ｃ）における理論距離Ｌｒ（２５）、Ｌｒ（２６）、Ｌｒ（３５）が示されている。
【００４３】
この結果、複数の候補姿勢角θ´の各々について、理論距離Ｌｒ（θ´）が演算される。
【００４４】
次のステップ１０４では、姿勢角検出部３０は、ステップ１０２で演算した理論距離Ｌｒ（θ´）（θ´＝２５、２６、・・・、３５）とステップ１００で取得した距離Ｌｍ（ｉ）（ｉ＝１、２、・・・、Ｎ）の各々とを比較して、比較結果に基づいたヒストグラムを作成する。具体的には、姿勢角検出部３０は、各々の候補姿勢角θ´において、理論距離Ｌｒ（θ´）と複数の距離Ｌｍ（ｉ）の各々との絶対値の差を示すＮ個の差分を算出する。そして、姿勢角検出部３０は、算出したＮ個の差分のうち所定の閾値Ｑ以下となる数を候補姿勢角θ´の点数として、θ´＝２５〜３５の点数をそれぞれ算出する。（すなわち、｜Ｌｍ（ｉ）−Ｌｒ（θ´）｜≦Ｑとなる数を候補姿勢角θ´の点数として、θ´＝２５〜３５の点数をそれぞれ算出する。）
【００４５】
以上のステップ１００〜ステップ１０４の処理により、図１０に示すようなヒストグラムが作成される。このヒストグラムは、候補姿勢角θ´の点数が高くなるに従って、その候補姿勢角θ´が実際の姿勢角θである可能性が高いことを表す。
【００４６】
次のステップ１０６では、姿勢角検出部３０は、点数が最大の候補姿勢角θ´を現在の距離計測装置１２の姿勢角θとして検出する。
【００４７】
なお、図１０に示すようなヒストグラムが作成された場合には、点数が最大である候補姿勢角θ´は２９°であるので、姿勢角検出部３０は、２９°を現在の距離計測装置１２の姿勢角θとして検出する。
【００４８】
以上の姿勢角検出処理によって、距離計測装置１２の実際の姿勢角θが検出される。
【００４９】
以上説明したように、本実施の形態の姿勢角検出装置１０によれば、距離演算部２８は、受光装置２０で受光された反射光に基づいて、床面上の複数の点までの距離Ｌｍ（ｉ）を演算する。姿勢検出部３０は、受光装置２０の各受光素子の出力から演算された各々の距離Ｌｍ（ｉ）と、複数の候補姿勢角θ´の各々の場合における距離計測装置１２から床面までの理論上の各々の距離Ｌｒ（θ´）とに基づいて、姿勢角θを検出する。従って、本実施の形態の姿勢角検出装置１０によれば、容量の大きい記憶手段を必要とすることなく、姿勢角θを検出することができる。よって、本実施の形態の姿勢角検出装置１０は、コストがかかることなく、姿勢角を検出することができる。
【００５０】
また、本実施の形態の姿勢角検出装置１０は、受光量検出部２６によって検出された受光量が基準受光量Ｒｒｅｆ以下の場合には、姿勢角検出装置１０から、所定の周波数で光が１周期に進む距離以上の距離に存在する床面または対象物２２までの距離を距離の計測が不可能であることを示す情報として演算する。従って、本実施の形態の姿勢角検出装置１０によれば、距離の計測の誤りを防止することができる。
【００５１】
なお、上記で説明した姿勢角検出処理ルーチンは、上述したステップ１０４及びステップ１０６に限定されるものではない。例えば、ステップ１０４及びステップ１０６において、姿勢検出部３０は、候補姿勢角θ´毎に、理論距離Ｌｒ（θ´）と複数の距離Ｌｍ（ｉ）の差分の絶対値の総和（Σ｜Ｌｒ（θ´）−Ｌｍ（ｉ）｜）を示すヒストグラムを作成し、理論距離Ｌｒ（θ´）と複数の距離Ｌｍ（ｉ）の差分の絶対値の総和が最も小さくなる候補姿勢角θ´を現在の距離計測装置１２の姿勢角θとして検出してもよい。
【００５２】
また、上記で説明した姿勢角検出処理ルーチンは、上述したステップ１０６に限定されるものではない。上述したステップ１０６で検出される姿勢角θは整数値であるが、ステップ１０４で作成されたヒストグラムの重心計算を行うことにより、姿勢角θを検出してもよい。これにより、姿勢角θは、小数値となる場合がある。
【００５３】
また、上記で説明した姿勢角検出処理ルーチンは、上述したステップ１０２からステップ１０６に限定されるものではない。例えば、ステップ１０２からステップ１０６において、ステップ１００で取得したＮ個の受光素子の各々の距離に基づいて、最小二乗法等の多変量解析手法を用いて求めた近似平面の式と、複数の候補姿勢角θ´の各々から計算可能な床面を表す理論上の各々の平面の式とを比較し、最も一致度が高い理論上の平面の式における候補姿勢角θ´を姿勢角θとして検出するようにしてもよい。これにより、特に、床面に対する障害物の占める範囲が小さい場合や、床面上に障害物が存在しない場合などには、姿勢角を検出することができる。
【００５４】
また、上記で説明した姿勢角検出処理ルーチンは、ピッチ方向に姿勢角θがずれることが予想される場合に、その姿勢角θを検出する例について説明したが、これに限定されるものではない。例えば、ピッチ方向と水平面内において直角であるロール方向に姿勢角θがずれることが予想される場合に、その姿勢角θを検出することについても同様の原理で行うようにしてもよい。
【００５５】
また、上記で説明した距離演算部２８が行う基準受光量算出処理及び距離演算処理は上述した内容に限定されるものではなく、以下のように行ってもよい。例えば、基準受光量算出処理において、距離演算部２８は、飛行時間算出部２４で算出された時間に基づいて、床面または対象物２２までの仮の距離を受光装置２０の受光素子毎に計算する。そして、距離演算部２８は、計算された仮の距離のうち、所定値以下、例えば０．３ｍ以下の距離が計算された受光素子が受光した受光量の平均値を距離演算処理で用いる基準受光量として算出する。なお、基準受光量として、０．３ｍ以下の距離が計算された受光素子が受光した受光量の最大値と最小値とを足したものを２で割った値を基準受光量としてもよい。距離演算処理では、距離演算部２８は、基準受光量算出処理で計算された距離が０．３ｍより大きい受光素子については計算された距離を床面または対象物２２までの距離として演算する。基準受光量算出処理で計算された距離が０．３ｍ以下の受光素子については、受光量検出部２６によって検出された受光量が基準受光量より大きい受光素子の場合には、飛行時間算出部２４で算出された時間に基づいて、対象物２２までの距離を演算する。一方、受光量検出部２６によって検出された受光量が基準受光量以下の受光素子の場合には、距離演算部２８は、距離の計測が不可能であることを示す情報を対象物２２までの距離を示す情報として演算する。
【００５６】
また、本実施の形態において、距離計測装置１２の姿勢角θを、光源１８から照射される光の光軸７０と水平方向との成す角度とする例について説明したが、光源１８から照射される光の光軸７０と鉛直方向との成す角度としてもよい。このとき、上記の説明で用いたｓｉｎθはｓｉｎ（９０−θ）として用いることができる。
【図面の簡単な説明】
【００５７】
【図１】本実施の形態における床面検出装置の配置位置を示す図である。
【図２】本実施の形態における距離計測装置の配置位置を示す図である。
【図３】本実施の形態を示す概略図である。
【図４】本実施の形態における光源を説明するための図である。
【図５】距離と反射光強度の関係を示す図である。
【図６】本実施の形態の効果を示す図である。
【図７】本実施の形態における制御装置のＣＰＵが行う姿勢検出処理ルーチンのフローチャートを示す図である。
【図８】本実施の形態における光源からの光の照射範囲を示す図である。
【図９】本実施の形態における理論距離を説明するための図である。
【図１０】本実施の形態における候補姿勢角のヒストグラムを示す図である。
【符号の説明】
【００５８】
１ 床面検出装置
１０ 姿勢角検出装置
１２ 距離計測装置
１４ 制御装置
１５ 距離演算装置
１８ 光源
２０ 受光装置
２２ 光源制御部
２４ 飛行時間算出部
２６ 受光量検出部
２８ 距離演算部
３０ 姿勢検出部
３２ 設置条件記憶部
３４ 理論距離演算部
【技術分野】
【０００１】
本発明は、姿勢角検出装置、姿勢角検出方法、及び床面検出装置に関する。
【背景技術】
【０００２】
従来、所定の周波数で強度変調された光を対象物に対して出射してから、対象物からの反射光を受光部で受光するまでの時間に基づいて対象物までの距離を計測するタイムオブフライト方式の距離画像センサが知られている。また、この距離画像センサを用いて、移動体が走行する平面または観測対象物までの距離を計測する障害物検出装置が知られている（例えば、特許文献１参照）。距離画像センサは、所定の高さで斜め下向きに所定の姿勢角（所定の傾き）で、移動体に搭載されている。この障害物検出装置は、移動体の走行時の振動等により変動する姿勢角の各々に対応する平面の距離画像を候補基準画像として記憶手段に記憶している。これにより、姿勢角が変動した場合であっても、測定した距離画像と、複数の候補基準画像とを比較し、一致度が最も高い候補基準画像を用いることによって、走行する平面を検出することができる。
【特許文献１】特開２００６−２６２００９号公報
【発明の開示】
【発明が解決しようとする課題】
【０００３】
しかしながら、特許文献１に記載の障害物検出装置では、候補基準画像を、ロール方向、ピッチ方向について、変動が予測される姿勢角毎に予め用意しておく必要がある。例えば、予測される姿勢角の変動の範囲をロール方向±１０°、ピッチ方向±２０°とし、１°毎に記憶手段に記憶する場合には、２０×４０＝８００パターンもの候補基準画像を記憶手段に記憶しておく必要がある。しかしながら、８００パターンもの候補基準画像を記憶するためには、容量が大きい記憶手段が必要となる。このような容量が大きい記憶手段を用いるとコストがかかってしまう。そのため、特許文献１に記載の障害物検出装置は、姿勢角が変動した場合に走行する平面を検出するためには、コストがかかってしまう、という問題点がある。
【０００４】
本発明は上記問題点を解決するために成されたもので、コストがかかることがなく、姿勢角を検出する姿勢角検出装置、姿勢角検出方法、及び走行する平面を検出する床面検出装置を提供することを目的とする。
【課題を解決するための手段】
【０００５】
上記の目的を達成するために第１の発明に係る姿勢角検出装置は、床面上に変調光を照射する光源、及び前記光源から照射された光の反射光を受光する複数の受光素子が配列された受光手段を備え、前記床面から所定の高さの位置に前記床面に対して所定の姿勢角になるように設けられた計測手段と、前記受光手段で受光された反射光に基づいて、前記床面上の複数の点までの距離を演算する距離演算手段と、前記受光手段の各受光素子の出力から演算された各々の距離と、複数の候補姿勢角の各々の場合における前記計測手段から前記床面までの理論上の各々の距離とに基づいて、前記姿勢角を検出する姿勢角検出手段とを含んで構成されている。
【０００６】
第１の発明に係る姿勢角検出装置によれば、距離演算手段は、受光手段で受光された反射光に基づいて、床面上の複数の点までの距離を演算する。姿勢角検出手段は、受光手段の各受光素子の出力から演算された各々の距離と、複数の候補姿勢角の各々の場合における計測手段から床面までの理論上の各々の距離とに基づいて、姿勢角を検出する。従って、第１の発明に係る姿勢角検出装置によれば、容量の大きい記憶手段を必要とすることなく、姿勢角を検出することができる。よって、第１の発明に係る姿勢角検出装置は、コストがかかることなく、姿勢角を検出することができる。
【０００７】
上記の目的を達成するために第２の発明に係る姿勢角検出装置は、床面上に変調光を照射する光源、及び前記光源から照射された光の反射光を受光する複数の受光素子が配列された受光手段を備え、前記床面から所定の高さの位置に前記床面に対して所定の姿勢角になるように設けられた計測手段と、前記受光手段で受光された反射光に基づいて、前記床面上の複数の点までの距離を演算する距離演算手段と、前記受光手段の各受光素子の出力から演算された各々の距離に基づいて定まる近似平面と、複数の候補姿勢角の各々の場合における理論上の各々の平面とに基づいて、前記姿勢角を検出する姿勢角検出手段とを含んで構成されている。
【０００８】
第２の発明に係る姿勢角検出装置によれば、距離演算手段は、受光手段で受光された反射光に基づいて、床面上の複数の点までの距離を演算する。姿勢角検出手段は、受光手段の各受光素子の出力から演算された各々の距離に基づいて定まる近似平面と、複数の候補姿勢角の各々の場合における理論上の各々の平面とに基づいて、姿勢角を検出する。従って、第２の発明に係る姿勢角検出装置によれば、特に、床面に対する障害物の占める範囲が小さい場合や、床面上に障害物が存在しない場合などには、容量の大きい記憶手段を必要とすることなく、姿勢角を検出することができる。よって、第２の発明に係る姿勢角検出装置は、コストがかかることなく、姿勢角を検出することができる。
【０００９】
また、第１または第２の発明に係る姿勢角検出装置における前記距離演算手段は、前記受光手段によって受光された反射光の受光量が所定量以下の場合には、所定の距離を示す情報を演算することができる。これにより、所定の距離を示す情報を、例えば、距離の計測が不可能であることを示す情報とすることで、所定の周波数で光が１周期に進む距離より大きい距離に存在する床面までの距離を距離の計測が不可能であることを示す情報として演算することができるので、距離の計測の誤りを防止することができる。また、所定の距離を示す情報を、例えば、光源から光が照射されてから受光手段によって反射光が受光されるまでの時間に応じて定まる床面までの距離と所定の周波数で光が１周期に進む距離とを加算した距離を示す情報とすることで、所定の周波数で光が１周期に進む距離より大きい距離に存在する床面までの距離を正確に演算することができるので、距離の計測の誤りを防止することができる。
【００１０】
上記の目的を達成するために第３の発明に係る床面検出装置は、第１の発明に係る距離計測装置または第２の発明に係る距離計測装置と、前記姿勢角検出手段によって検出された姿勢角に基づいて、床面を検出する床面検出手段とを含んで構成されている。これにより、容量の大きい記憶手段を必要とすることなく、床面を検出することができる。よって、第３の発明に係る床面検出装置は、コストがかかることなく、床面を検出することができる。
【００１１】
上記の目的を達成するために第４の発明に係る姿勢角検出方法は、床面上に変調光を照射する光源、及び前記光源から照射された光の反射光を受光する複数の受光素子が配列された受光手段を備え、前記床面から所定の高さの位置に前記床面に対して所定の姿勢角になるように設けられた計測手段の受光手段が前記反射光を受光し、前記受光手段で受光された反射光に基づいて、前記床面上の複数の点までの距離を演算し、前記受光手段の各受光素子の出力から演算された各々の距離と、複数の候補姿勢角の各々の場合における前記計測手段から前記床面までの理論上の各々の距離とに基づいて、前記姿勢角を検出する。
【００１２】
第４の発明に係る姿勢角検出方法によれば、受光手段で受光された反射光に基づいて、床面上の複数の点までの距離を演算し、受光手段の各受光素子の出力から演算された各々の距離と、複数の候補姿勢角の各々の場合における計測手段から床面までの理論上の各々の距離とに基づいて、姿勢角を検出する。従って、第４の発明に係る姿勢角検出方法によれば、容量の大きい記憶手段を必要とすることなく、姿勢角を検出することができる。よって、第４の発明に係る姿勢角検出方法は、コストがかかることなく、姿勢角を検出することができる。
【００１３】
上記の目的を達成するために第５の発明に係る姿勢角検出方法は、床面上に変調光を照射する光源、及び前記光源から照射された光の反射光を受光する複数の受光素子が配列された受光手段を備え、前記床面から所定の高さの位置に前記床面に対して所定の姿勢角になるように設けられた計測手段の受光手段が前記反射光を受光し、前記受光手段で受光された反射光に基づいて、前記床面上の複数の点までの距離を演算し、前記受光手段の各受光素子の出力から演算された各々の距離に基づいて定まる近似平面と、複数の候補姿勢角の各々の場合における理論上の各々の平面とに基づいて、前記姿勢角を検出する。
【００１４】
第５の発明に係る姿勢角検出方法によれば、受光手段で受光された反射光に基づいて、床面上の複数の点までの距離を演算し、受光手段の各受光素子の出力から演算された各々の距離に基づいて定まる近似平面と、複数の候補姿勢角の各々の場合における理論上の各々の平面とに基づいて、姿勢角を検出する。従って、第５の発明に係る姿勢角検出方法によれば、容量の大きい記憶手段を必要とすることなく、姿勢角を検出することができる。よって、第５の発明に係る姿勢角検出方法は、コストがかかることなく、姿勢角を検出することができる。
【発明の効果】
【００１５】
以上説明したように、本発明の姿勢角検出装置、姿勢角検出方法、及び床面検出装置によれば、コストがかかることなく、姿勢角または床面を検出することができる、という効果が得られる。
【発明を実施するための最良の形態】
【００１６】
以下、図面を参照して本発明の実施の形態を詳細に説明する。
【００１７】
図１に示すように、本発明の実施の形態の床面検出装置１は、姿勢角検出装置１０と、走行体１６とを含んで構成されている。姿勢角検出装置１０は、距離計測装置１２と、制御装置１３とを含んで構成されている。姿勢角検出装置１０は、床面を走行する走行体１６に搭載されている。
【００１８】
距離計測装置１２は、図２に示すように、床面から所定の高さＨの位置に、光源１８から照射される光の光軸７０と水平方向との成す角度が所定の姿勢角θ、例えば姿勢角３０°で設置されている。
【００１９】
距離計測装置１２は、図３に示すように、距離演算装置１５と、光源１８と、受光装置２０とを備えている。
【００２０】
光源１８は、図４に示すように、マトリックス状に配列された光を照射するＬＥＤを複数個、例えば５０〜６０個含んで構成されている。各ＬＥＤは、光源制御部２２からの制御によって、所定周波数、例えば２０ＭＨｚの正弦波で強度変調された光を照射する。
【００２１】
受光装置２０は、複数の受光素子を含んで構成されており、例えば、複数の画素を備えたＣＭＯＳセンサを含んで構成されている。受光装置２０は、対象物２２または床面からの反射光を受光する。また、受光装置２０は、受光した反射光の光量に基づいて検出信号を出力する。
【００２２】
距離演算装置１５は、各種処理ルーチンを実行するプログラム等を記憶した記憶媒体としてのＲＯＭ（図示せず）、プログラムをＲＯＭから読み出して実行するＣＰＵ（図示せず）、データを一時的に記憶するＲＡＭ（図示せず）、及びＩ／Ｏ（入出力）ポート（図示せず）を含んだマイクロコンピュータで構成されている。距離演算装置１５は、光源制御部２２、飛行時間算出部２４、受光量検出部２６、及び距離演算部２８を備えている。
【００２３】
光源制御部２２は、所定の周波数、例えば２０ＭＨｚの正弦波で強度変調された変調光を照射するように光源１８を制御する。また、光源制御部２２は、光源１８の制御タイミングを示す信号を出力する。
【００２４】
飛行時間算出部２４は、受光装置２０からの検出信号等によって、光源１８から照射された光と受光装置２０によって受光した反射光の位相のずれを検出する。そして、飛行時間算出部２４は、検出した位相のずれに基づいて、光源１８から光が照射されてから受光装置２０によって反射光が受光されるまでの時間を算出する。
【００２５】
受光量検出部２６は、受光装置２０によって受光された反射光の受光量を検出する。
【００２６】
距離演算部２８は、基準受光量算出処理及び距離演算処理を行う。
【００２７】
基準受光量算出処理では、距離演算部２８は、飛行時間算出部２４で算出された時間に基づいて、床面または対象物２２までの仮の距離を受光装置２０の受光素子毎に計算する。そして、距離演算部２８は、図５に示すように、計算された仮の距離のうち、所定値以下、例えば、０．３ｍ以下の距離が計算された受光素子が受光した受光量の最大値Ｒｍａｘと、その際に演算された距離Ｘ１とを用いて、以下の式（１）によって、反射率ｒを算出する。
Ｒｍａｘ＝ｒ／（Ｘ１）２・・・・・式（１）
【００２８】
そして、距離演算部２８は、算出された反射率ｒと距離計測装置１２に固有である最大計測距離Ｘｍａｘ（本実施の形態では７．５ｍ）とを用いて、以下の式（２）によって、最大計測距離Ｘｍａｘにおける反射光強度Ｒを算出する。なお、この最大計測距離Ｘｍａｘは、距離計測装置１２に固有の値であり、光源１８から照射される光の周波数等に基づいて定まる。
Ｒ＝ｒ／（Ｘｍａｘ）２・・・・・式（２）
【００２９】
そして、距離演算部２８は、式（２）により算出されたＲを距離演算処理で用いる基準受光量Ｒｒｅｆとして算出する。
【００３０】
距離演算処理では、距離演算部２８は、受光量検出部２６によって検出された受光量が基準受光量Ｒｒｅｆより大きい受光素子については、飛行時間算出部２４で算出された時間に基づいて、床面または対象物２２までの距離を演算する。一方、距離演算部２８は、受光量が基準受光量Ｒｒｅｆ以下の受光素子については、距離の計測が不可能であることを示す情報を床面または対象物２２までの距離を示す情報として演算する。
【００３１】
従来、図６（Ａ）に示すように、最大計測距離Ｘｍａｘ（本実施の形態では７．５ｍ）を超えた距離に存在する対象物２２、例えば、８．５ｍの距離に存在する対象物２２を１．０ｍと誤計測していた。これは、１波長よりも大きく位相がずれている場合であっても、この位相のずれを１波長以内の位相のずれとみなして、光の飛行時間を算出しているためである。本実施形態のような、基準受光量算出処理及び距離演算処理を距離演算部２８が行うことにより、図６（Ｂ）に示すように、最大計測距離Ｘｍａｘを超えた位置に存在する対象物２２については、距離の計測が不可能であることを示す情報を、距離を示す情報として演算することで、誤計測が無くなる。
【００３２】
なお、距離演算処理において、受光量が基準受光量Ｒｒｅｆ以下の受光素子については、図６（Ｃ）に示すように、飛行時間算出部２４によって算出された時間に応じて定まる床面または対象物２２までの距離に、所定の周波数で光が１周期に進む距離を加えることによって床面または対象物２２までの距離を示す情報を演算するようにしてもよい。このようにすることで、最大計測距離Ｘｍａｘ（本実施の形態では７．５ｍ）を超えた距離に存在する対象物２２、例えば、８．５ｍの距離に存在する対象物２２を８．５ｍ（＝１．０ｍ＋７．５ｍ）と正確に計測することができる。
【００３３】
制御装置１４は、姿勢検出処理ルーチンを実行するプログラム、及び各種処理ルーチンを実行するプログラム等を記憶した記憶媒体としてのＲＯＭ（図示せず）、プログラムをＲＯＭから読み出して実行するＣＰＵ（図示せず）、データを一時的に記憶するＲＡＭ（図示せず）、及びＩ／Ｏ（入出力）ポート（図示せず）を含んだマイクロコンピュータで構成されている。制御装置１４は、姿勢検出部３０、設置条件記憶部３２、及び理論距離演算部３４を備えている。
【００３４】
設置条件記憶部３２には、候補姿勢角θ´が複数記憶されている。この候補姿勢角θ´は、走行体１６が走行した際に距離計測装置１２に振動が加わったことにより、移動体１６の進行方向（ピッチ方向）に、姿勢角θが所定の姿勢角θ（本実施の形態では、例えばθ＝３０°）からずれた場合に予想される距離計測装置１２の姿勢角θを表す。例えば、本実施の形態では、予め距離計測装置１２の姿勢角を３０°として設置した場合に、走行体１６が走行した際に姿勢角が最大で±５°ずれることが予想されるとする。このときには、設置条件記憶部３２に、２５°から３５°までの、例えば１°毎の候補姿勢角θ´を記憶しておく。また、設置条件記憶部３２には、床面から距離計測装置１２までの高さＨが記憶されている。
【００３５】
理論距離演算部３４は、設置条件記憶部３２に記憶されている高さＨ及び候補姿勢角θ´を用いて、距離計測装置１２の姿勢角θが各候補姿勢角θ´である場合の距離計測装置１２から床面までの光源１８の光軸７０上における理論上の距離（理論距離）を演算する。
【００３６】
姿勢検出部３０は、距離演算部２８で演算された、受光装置２０の複数の受光素子のそれぞれの距離のうち、Ｎ個の受光素子のそれぞれに対応する距離と、理論距離演算部３４によって演算された、候補姿勢角θ´毎の理論距離とに基づいて、距離計測装置１２の姿勢角θを検出する。
【００３７】
走行体１６は、床面を検出する床面検出部３６を備えている。
【００３８】
床面検出部３６は、距離演算部２８で演算された受光素子毎の距離、姿勢検出部３０によって検出された姿勢角θ等を用いて床面を検出する。そして、床面検出部３６は、検出した床面に対する障害物を検出する。そして、床面検出部３６は、検出した障害物に対応するように、走行体１６の走行（例えば、移動速度及び移動方向）を制御する。
【００３９】
次に、制御装置１４のＣＰＵが行う姿勢検出処理ルーチンについて図７を用いて説明する。なお、本実施の形態において、この姿勢検出処理ルーチンは、床面検出部３６からの信号に基づいて走行体１６が所定距離を走行する毎に実行される。
【００４０】
まずステップ１００で、姿勢検出部３０は、図８に示すように、距離演算部２８によって演算された、受光装置２０のＣＭＯＳセンサの複数の受光素子の床面までの距離のうち、Ｎ個の受光素子の各々から床面ｍ（ｉ）（ｉ＝１、２、・・・、Ｎ）までの距離Ｌｍ（ｉ）（ｉ＝１、２、・・・、Ｎ）を取得する。ここで、Ｎ個の受光素子についてはどのような方法で選んでもよく、受光装置２０の複数の受光素子のうち、等間隔となるようにＮ個の受光素子を選んでもよい。なお、同図に図示されるθｍ（ｉ）（ｉ＝１、２、・・・、Ｎ）は、床面ｍ（ｉ）の各々から反射されて受光装置２０（Ｐ）まで到達する光と、床面との成す角度を表している。
【００４１】
次のステップ１０２では、理論距離演算部３４は、図９に示すように、設置条件記憶部３２に記憶されている各候補姿勢角θ´と高さＨとから、距離計測装置１２の姿勢角θが各候補姿勢角θ´である場合の距離計測装置１２から床面までの光源１８の光軸７０上における理論距離Ｌｒ（θ´）（θ´＝２５、２６、・・・、３５）を下記の式（３）により演算する。
【００４２】
Ｌｒ（θ´）＝Ｈ／ｓｉｎθ´・・・・・式（３）
なお、図９には、例として、候補姿勢角θ´が２５°の場合（Ａ）、候補姿勢角θ´が２６°の場合（Ｂ）、候補姿勢角θ´が３５°の場合（Ｃ）における理論距離Ｌｒ（２５）、Ｌｒ（２６）、Ｌｒ（３５）が示されている。
【００４３】
この結果、複数の候補姿勢角θ´の各々について、理論距離Ｌｒ（θ´）が演算される。
【００４４】
次のステップ１０４では、姿勢角検出部３０は、ステップ１０２で演算した理論距離Ｌｒ（θ´）（θ´＝２５、２６、・・・、３５）とステップ１００で取得した距離Ｌｍ（ｉ）（ｉ＝１、２、・・・、Ｎ）の各々とを比較して、比較結果に基づいたヒストグラムを作成する。具体的には、姿勢角検出部３０は、各々の候補姿勢角θ´において、理論距離Ｌｒ（θ´）と複数の距離Ｌｍ（ｉ）の各々との絶対値の差を示すＮ個の差分を算出する。そして、姿勢角検出部３０は、算出したＮ個の差分のうち所定の閾値Ｑ以下となる数を候補姿勢角θ´の点数として、θ´＝２５〜３５の点数をそれぞれ算出する。（すなわち、｜Ｌｍ（ｉ）−Ｌｒ（θ´）｜≦Ｑとなる数を候補姿勢角θ´の点数として、θ´＝２５〜３５の点数をそれぞれ算出する。）
【００４５】
以上のステップ１００〜ステップ１０４の処理により、図１０に示すようなヒストグラムが作成される。このヒストグラムは、候補姿勢角θ´の点数が高くなるに従って、その候補姿勢角θ´が実際の姿勢角θである可能性が高いことを表す。
【００４６】
次のステップ１０６では、姿勢角検出部３０は、点数が最大の候補姿勢角θ´を現在の距離計測装置１２の姿勢角θとして検出する。
【００４７】
なお、図１０に示すようなヒストグラムが作成された場合には、点数が最大である候補姿勢角θ´は２９°であるので、姿勢角検出部３０は、２９°を現在の距離計測装置１２の姿勢角θとして検出する。
【００４８】
以上の姿勢角検出処理によって、距離計測装置１２の実際の姿勢角θが検出される。
【００４９】
以上説明したように、本実施の形態の姿勢角検出装置１０によれば、距離演算部２８は、受光装置２０で受光された反射光に基づいて、床面上の複数の点までの距離Ｌｍ（ｉ）を演算する。姿勢検出部３０は、受光装置２０の各受光素子の出力から演算された各々の距離Ｌｍ（ｉ）と、複数の候補姿勢角θ´の各々の場合における距離計測装置１２から床面までの理論上の各々の距離Ｌｒ（θ´）とに基づいて、姿勢角θを検出する。従って、本実施の形態の姿勢角検出装置１０によれば、容量の大きい記憶手段を必要とすることなく、姿勢角θを検出することができる。よって、本実施の形態の姿勢角検出装置１０は、コストがかかることなく、姿勢角を検出することができる。
【００５０】
また、本実施の形態の姿勢角検出装置１０は、受光量検出部２６によって検出された受光量が基準受光量Ｒｒｅｆ以下の場合には、姿勢角検出装置１０から、所定の周波数で光が１周期に進む距離以上の距離に存在する床面または対象物２２までの距離を距離の計測が不可能であることを示す情報として演算する。従って、本実施の形態の姿勢角検出装置１０によれば、距離の計測の誤りを防止することができる。
【００５１】
なお、上記で説明した姿勢角検出処理ルーチンは、上述したステップ１０４及びステップ１０６に限定されるものではない。例えば、ステップ１０４及びステップ１０６において、姿勢検出部３０は、候補姿勢角θ´毎に、理論距離Ｌｒ（θ´）と複数の距離Ｌｍ（ｉ）の差分の絶対値の総和（Σ｜Ｌｒ（θ´）−Ｌｍ（ｉ）｜）を示すヒストグラムを作成し、理論距離Ｌｒ（θ´）と複数の距離Ｌｍ（ｉ）の差分の絶対値の総和が最も小さくなる候補姿勢角θ´を現在の距離計測装置１２の姿勢角θとして検出してもよい。
【００５２】
また、上記で説明した姿勢角検出処理ルーチンは、上述したステップ１０６に限定されるものではない。上述したステップ１０６で検出される姿勢角θは整数値であるが、ステップ１０４で作成されたヒストグラムの重心計算を行うことにより、姿勢角θを検出してもよい。これにより、姿勢角θは、小数値となる場合がある。
【００５３】
また、上記で説明した姿勢角検出処理ルーチンは、上述したステップ１０２からステップ１０６に限定されるものではない。例えば、ステップ１０２からステップ１０６において、ステップ１００で取得したＮ個の受光素子の各々の距離に基づいて、最小二乗法等の多変量解析手法を用いて求めた近似平面の式と、複数の候補姿勢角θ´の各々から計算可能な床面を表す理論上の各々の平面の式とを比較し、最も一致度が高い理論上の平面の式における候補姿勢角θ´を姿勢角θとして検出するようにしてもよい。これにより、特に、床面に対する障害物の占める範囲が小さい場合や、床面上に障害物が存在しない場合などには、姿勢角を検出することができる。
【００５４】
また、上記で説明した姿勢角検出処理ルーチンは、ピッチ方向に姿勢角θがずれることが予想される場合に、その姿勢角θを検出する例について説明したが、これに限定されるものではない。例えば、ピッチ方向と水平面内において直角であるロール方向に姿勢角θがずれることが予想される場合に、その姿勢角θを検出することについても同様の原理で行うようにしてもよい。
【００５５】
また、上記で説明した距離演算部２８が行う基準受光量算出処理及び距離演算処理は上述した内容に限定されるものではなく、以下のように行ってもよい。例えば、基準受光量算出処理において、距離演算部２８は、飛行時間算出部２４で算出された時間に基づいて、床面または対象物２２までの仮の距離を受光装置２０の受光素子毎に計算する。そして、距離演算部２８は、計算された仮の距離のうち、所定値以下、例えば０．３ｍ以下の距離が計算された受光素子が受光した受光量の平均値を距離演算処理で用いる基準受光量として算出する。なお、基準受光量として、０．３ｍ以下の距離が計算された受光素子が受光した受光量の最大値と最小値とを足したものを２で割った値を基準受光量としてもよい。距離演算処理では、距離演算部２８は、基準受光量算出処理で計算された距離が０．３ｍより大きい受光素子については計算された距離を床面または対象物２２までの距離として演算する。基準受光量算出処理で計算された距離が０．３ｍ以下の受光素子については、受光量検出部２６によって検出された受光量が基準受光量より大きい受光素子の場合には、飛行時間算出部２４で算出された時間に基づいて、対象物２２までの距離を演算する。一方、受光量検出部２６によって検出された受光量が基準受光量以下の受光素子の場合には、距離演算部２８は、距離の計測が不可能であることを示す情報を対象物２２までの距離を示す情報として演算する。
【００５６】
また、本実施の形態において、距離計測装置１２の姿勢角θを、光源１８から照射される光の光軸７０と水平方向との成す角度とする例について説明したが、光源１８から照射される光の光軸７０と鉛直方向との成す角度としてもよい。このとき、上記の説明で用いたｓｉｎθはｓｉｎ（９０−θ）として用いることができる。
【図面の簡単な説明】
【００５７】
【図１】本実施の形態における床面検出装置の配置位置を示す図である。
【図２】本実施の形態における距離計測装置の配置位置を示す図である。
【図３】本実施の形態を示す概略図である。
【図４】本実施の形態における光源を説明するための図である。
【図５】距離と反射光強度の関係を示す図である。
【図６】本実施の形態の効果を示す図である。
【図７】本実施の形態における制御装置のＣＰＵが行う姿勢検出処理ルーチンのフローチャートを示す図である。
【図８】本実施の形態における光源からの光の照射範囲を示す図である。
【図９】本実施の形態における理論距離を説明するための図である。
【図１０】本実施の形態における候補姿勢角のヒストグラムを示す図である。
【符号の説明】
【００５８】
１ 床面検出装置
１０ 姿勢角検出装置
１２ 距離計測装置
１４ 制御装置
１５ 距離演算装置
１８ 光源
２０ 受光装置
２２ 光源制御部
２４ 飛行時間算出部
２６ 受光量検出部
２８ 距離演算部
３０ 姿勢検出部
３２ 設置条件記憶部
３４ 理論距離演算部
【特許請求の範囲】
【請求項１】
床面上に変調光を照射する光源、及び前記光源から照射された光の反射光を受光する複数の受光素子が配列された受光手段を備え、前記床面から所定の高さの位置に前記床面に対して所定の姿勢角になるように設けられた計測手段と、
前記受光手段で受光された反射光に基づいて、前記床面上の複数の点までの距離を演算する距離演算手段と、
前記受光手段の各受光素子の出力から演算された各々の距離と、複数の候補姿勢角の各々の場合における前記計測手段から前記床面までの理論上の各々の距離とに基づいて、前記姿勢角を検出する姿勢角検出手段と、
を含む姿勢角検出装置。
【請求項２】
床面上に変調光を照射する光源、及び前記光源から照射された光の反射光を受光する複数の受光素子が配列された受光手段を備え、前記床面から所定の高さの位置に前記床面に対して所定の姿勢角になるように設けられた計測手段と、
前記受光手段で受光された反射光に基づいて、前記床面上の複数の点までの距離を演算する距離演算手段と、
前記受光手段の各受光素子の出力から演算された各々の距離に基づいて定まる近似平面と、複数の候補姿勢角の各々の場合における理論上の各々の平面とに基づいて、前記姿勢角を検出する姿勢角検出手段と、
を含む姿勢角検出装置。
【請求項３】
前記距離演算手段は、前記受光手段によって受光された反射光の受光量が所定量以下の場合には、所定の距離を示す情報を演算する請求項１または請求項２に記載の姿勢角検出装置。
【請求項４】
前記所定の距離を示す情報は、距離の計測が不可能であることを示す情報である請求項３に記載の姿勢角検出装置。
【請求項５】
前記所定の距離を示す情報は、前記光源から光が照射されてから前記受光手段によって反射光が受光されるまでの時間に応じて定まる前記床面までの距離と、前記所定の周波数で光が１周期に進む距離とを加算した距離を示す情報である請求項３に記載の姿勢角検出装置。
【請求項６】
請求項１から請求項５までのいずれか１項に記載の姿勢角検出装置と、
前記姿勢角検出手段によって検出された前記姿勢角に基づいて、床面を検出する床面検出手段と、
を含む床面検出装置。
【請求項７】
床面上に変調光を照射する光源、及び前記光源から照射された光の反射光を受光する複数の受光素子が配列された受光手段を備え、前記床面から所定の高さの位置に前記床面に対して所定の姿勢角になるように設けられた計測手段の受光手段が前記反射光を受光し、
前記受光手段で受光された反射光に基づいて、前記床面上の複数の点までの距離を演算し、
前記受光手段の各受光素子の出力から演算された各々の距離と、複数の候補姿勢角の各々の場合における前記計測手段から前記床面までの理論上の各々の距離とに基づいて、前記姿勢角を検出する
姿勢角検出方法。
【請求項８】
床面上に変調光を照射する光源、及び前記光源から照射された光の反射光を受光する複数の受光素子が配列された受光手段を備え、前記床面から所定の高さの位置に前記床面に対して所定の姿勢角になるように設けられた計測手段の受光手段が前記反射光を受光し、
前記受光手段で受光された反射光に基づいて、前記床面上の複数の点までの距離を演算し、
前記受光手段の各受光素子の出力から演算された各々の距離に基づいて定まる近似平面と、複数の候補姿勢角の各々の場合における理論上の各々の平面とに基づいて、前記姿勢角を検出する
姿勢角検出方法。
【請求項１】
床面上に変調光を照射する光源、及び前記光源から照射された光の反射光を受光する複数の受光素子が配列された受光手段を備え、前記床面から所定の高さの位置に前記床面に対して所定の姿勢角になるように設けられた計測手段と、
前記受光手段で受光された反射光に基づいて、前記床面上の複数の点までの距離を演算する距離演算手段と、
前記受光手段の各受光素子の出力から演算された各々の距離と、複数の候補姿勢角の各々の場合における前記計測手段から前記床面までの理論上の各々の距離とに基づいて、前記姿勢角を検出する姿勢角検出手段と、
を含む姿勢角検出装置。
【請求項２】
床面上に変調光を照射する光源、及び前記光源から照射された光の反射光を受光する複数の受光素子が配列された受光手段を備え、前記床面から所定の高さの位置に前記床面に対して所定の姿勢角になるように設けられた計測手段と、
前記受光手段で受光された反射光に基づいて、前記床面上の複数の点までの距離を演算する距離演算手段と、
前記受光手段の各受光素子の出力から演算された各々の距離に基づいて定まる近似平面と、複数の候補姿勢角の各々の場合における理論上の各々の平面とに基づいて、前記姿勢角を検出する姿勢角検出手段と、
を含む姿勢角検出装置。
【請求項３】
前記距離演算手段は、前記受光手段によって受光された反射光の受光量が所定量以下の場合には、所定の距離を示す情報を演算する請求項１または請求項２に記載の姿勢角検出装置。
【請求項４】
前記所定の距離を示す情報は、距離の計測が不可能であることを示す情報である請求項３に記載の姿勢角検出装置。
【請求項５】
前記所定の距離を示す情報は、前記光源から光が照射されてから前記受光手段によって反射光が受光されるまでの時間に応じて定まる前記床面までの距離と、前記所定の周波数で光が１周期に進む距離とを加算した距離を示す情報である請求項３に記載の姿勢角検出装置。
【請求項６】
請求項１から請求項５までのいずれか１項に記載の姿勢角検出装置と、
前記姿勢角検出手段によって検出された前記姿勢角に基づいて、床面を検出する床面検出手段と、
を含む床面検出装置。
【請求項７】
床面上に変調光を照射する光源、及び前記光源から照射された光の反射光を受光する複数の受光素子が配列された受光手段を備え、前記床面から所定の高さの位置に前記床面に対して所定の姿勢角になるように設けられた計測手段の受光手段が前記反射光を受光し、
前記受光手段で受光された反射光に基づいて、前記床面上の複数の点までの距離を演算し、
前記受光手段の各受光素子の出力から演算された各々の距離と、複数の候補姿勢角の各々の場合における前記計測手段から前記床面までの理論上の各々の距離とに基づいて、前記姿勢角を検出する
姿勢角検出方法。
【請求項８】
床面上に変調光を照射する光源、及び前記光源から照射された光の反射光を受光する複数の受光素子が配列された受光手段を備え、前記床面から所定の高さの位置に前記床面に対して所定の姿勢角になるように設けられた計測手段の受光手段が前記反射光を受光し、
前記受光手段で受光された反射光に基づいて、前記床面上の複数の点までの距離を演算し、
前記受光手段の各受光素子の出力から演算された各々の距離に基づいて定まる近似平面と、複数の候補姿勢角の各々の場合における理論上の各々の平面とに基づいて、前記姿勢角を検出する
姿勢角検出方法。
【図１】
【図２】
【図３】
【図４】
【図５】
【図６】
【図７】
【図８】
【図９】
【図１０】
【図２】
【図３】
【図４】
【図５】
【図６】
【図７】
【図８】
【図９】
【図１０】
【公開番号】特開２００８−２９８７４２（Ｐ２００８−２９８７４２Ａ）
【公開日】平成２０年１２月１１日（２００８．１２．１１）
【国際特許分類】
【出願番号】特願２００７−１４８２７２（Ｐ２００７−１４８２７２）
【出願日】平成１９年６月４日（２００７．６．４）
【出願人】（０００００３６０９）株式会社豊田中央研究所 (4,200)
【出願人】（０００００３２０７）トヨタ自動車株式会社 (59,920)
【Ｆターム（参考）】
【公開日】平成２０年１２月１１日（２００８．１２．１１）
【国際特許分類】
【出願日】平成１９年６月４日（２００７．６．４）
【出願人】（０００００３６０９）株式会社豊田中央研究所 (4,200)
【出願人】（０００００３２０７）トヨタ自動車株式会社 (59,920)
【Ｆターム（参考）】
[ Back to top ]