干渉判定装置および脚車輪型ロボット
【課題】 実際の干渉を伴うことなく干渉判定を行うことができ、部品点数および重量の増加を抑制し、干渉判定の制約を低減するのに好適な干渉判定装置を提供する。
【解決手段】 脚車輪型ロボット１００は、基体１０と、基体１０に対して自由度を有して連結された脚部１２と、脚部１２に回転可能に設けられた駆動輪２０と、レーザ２８〜３０およびカメラ３２からなる物体センサとを備え、物体センサの測定結果に基づいて障害物の特徴点を検出し、グローバル座標系Ｐgにおいて、脚車輪型ロボット１００の姿勢並びに基体１０および脚すね部の形状により特定される脚すね部平面と、検出した特徴点との位置関係に基づいて、脚すね部に対して障害物が干渉するか否かを判定する。
【解決手段】 脚車輪型ロボット１００は、基体１０と、基体１０に対して自由度を有して連結された脚部１２と、脚部１２に回転可能に設けられた駆動輪２０と、レーザ２８〜３０およびカメラ３２からなる物体センサとを備え、物体センサの測定結果に基づいて障害物の特徴点を検出し、グローバル座標系Ｐgにおいて、脚車輪型ロボット１００の姿勢並びに基体１０および脚すね部の形状により特定される脚すね部平面と、検出した特徴点との位置関係に基づいて、脚すね部に対して障害物が干渉するか否かを判定する。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、障害物との干渉判定を行う装置および段差を乗り越えて移動できる脚車輪型ロボットに係り、特に、実際の干渉を伴うことなく干渉判定を行うことができ、部品点数および重量の増加を抑制し、干渉判定の制約を低減するのに好適な干渉判定装置および脚車輪型ロボットに関する。
【背景技術】
【０００２】
ロボットの制御においては、ロボットに対して障害物が干渉（衝突）したか否かを判定し、その判定結果に基づいて障害物を回避する制御等の各種の制御を行っている。干渉を判定する技術としては、例えば、特許文献１、２記載の技術が知られている。
特許文献１記載の発明は、減速機を介してモータにより駆動されるロボットにおいて、モータが減速機に出力するトルクからロボットの逆動力学演算で求めた動力学トルクを差し引くことにより、衝突による外力をセンサレスで検出し、外力の検出値があらかじめ設定した所定の閾値より大きければアームが外力を受けたと判断する。
【０００３】
特許文献２記載の発明は、脚部の足平上にマークを付けておき、基体に取り付けられた視覚センサから取得された画像データに基づいて、歩行に対する障害物と脚部の位置関係を把握しながら歩行動作を行う。
【特許文献１】特開２００６−１１６６５０号公報
【特許文献２】特開２０００−１５３４７６号公報
【発明の開示】
【発明が解決しようとする課題】
【０００４】
しかしながら、特許文献１記載の発明にあっては、ロボットに対して障害物が実際に干渉しないと干渉したことが判定できないため、障害物を事前に回避する制御を行う上では実用的ではないという問題があった。
また、特許文献２記載の発明にあっては、干渉判定専用のカメラを設ける必要があるため、部品点数および重量が増加するという問題があった。
【０００５】
また、干渉を判定するには、カメラで撮影した画像に脚部と障害物の両方が含まれている必要があるため、干渉の判定に制約があるという問題もあった。例えば、脚部の可動範囲が大きいため下方を広範に撮影しようとすると、ロボットの前方にある障害物を撮影できず、前方の領域について干渉の判定が行えない。逆に、前方を広範に撮影しようとすると、脚部の可動範囲をすべてカバーすることができず、下方および後方の領域について干渉の判定が行えない。
【０００６】
ところで、ロボットの移動機構は、車輪型、クローラ型、脚型またはこれらを組み合わせた機構に分類される。一般に、車輪型ロボットは、平地での移動性は高いが、段差への適応性が低いという問題がある。また、クローラ型ロボットは、不整地に適し、多少の段差であれば乗り越えられるが、積極的な重心移動ができないために急な階段への適応性が低く、平地における移動性が車輪型ロボットより低いという問題があった。また、脚型ロボットは、階段への適応性が最も優れているが、平地での移動性が極端に低いという問題がある。
【０００７】
車輪型、クローラ型および脚型の問題を解決するため、脚型と車輪型を組み合わせた脚車輪型ロボットが提案されている。
脚車輪型ロボットが段差を乗り越える場合、重心を低くするために大きく開脚し、脚部の可動範囲について脚先と段差の干渉を判定しながら、段差を乗り越えるべく脚部を適切に駆動制御することが考えられる。このように、脚部の可動範囲および障害物の監視範囲が広範囲である場合に、特許文献２記載の発明を適用しようとすると、上記制約の問題がより顕著となる。
【０００８】
そこで、本発明は、このような従来の技術の有する未解決の課題に着目してなされたものであって、実際の干渉を伴うことなく干渉判定を行うことができ、部品点数および重量の増加を抑制し、干渉判定の制約を低減するのに好適な干渉判定装置および脚車輪型ロボットを提供することを目的としている。
【課題を解決するための手段】
【０００９】
〔発明１〕 上記目的を達成するために、発明１の干渉判定装置は、対象部位について障害物との干渉判定を行う干渉判定装置であって、障害物の特徴点を検出する特徴点検出手段と、前記特徴点および前記対象部位の座標を定める座標系において、前記対象部位またはこれと連動する部位の姿勢および前記対象部位の形状により特定される線、平面または空間であって、前記対象部位の形状に対応する線、前記対象部位の表面に対応する平面または前記対象部位に対応する空間と、前記特徴点検出手段で検出した特徴点との位置関係に基づいて、前記対象部位について障害物との干渉判定を行う干渉判定手段とを備える。
【００１０】
このような構成であれば、特徴点検出手段により、障害物の特徴点が検出される。そして、干渉判定手段により、特徴点および対象部位の座標を定める座標系において、対象部位の形状に対応する線、対象部位の表面に対応する平面または対象部位に対応する空間と、検出された特徴点との位置関係に基づいて、対象部位について障害物との干渉判定が行われる。この平面等は、対象部位またはこれと連動する部位の姿勢および対象部位の形状により特定される。
【００１１】
したがって、所定の座標系における平面等と特徴点の位置関係に基づいて干渉判定を行うので、実際の干渉を伴う必要はない。また、特許文献２における干渉判定専用のカメラのような専用のセンサを設けなくても、他の用途に用いられるセンサ等を利用することができる。さらに、所定の座標系において対象部位に対応する平面等は姿勢や形状により特定されるため、障害物の特徴点さえ検出できればよく、障害物の監視範囲が対象部位の可動範囲に制約されない。
【００１２】
ここで、特徴点および対象部位の座標を定める座標系としては、特徴点の絶対座標（例えば、接地面を基準とする座標）または相対座標（例えば、干渉判定装置を基準とする座標）を定める座標系、対象部位の絶対座標（例えば、接地面を基準とする座標）または相対座標（例えば、対象部位と連動する部位を基準とする座標）を定める座標系、その他任意の座標系が含まれる。なお、特徴点の座標を定める座標系と、対象部位の座標を定める座標系が異なる場合は、一方の座標系の座標を他方の座標系の座標に変換するか、両方の座標系の座標を、特徴点および対象部位の座標を定める他の座標系の座標に変換すればよい。
【００１３】
また、対象部位と連動する部位とは、対象部位と連なって動作する部位であって、自身の姿勢から対象部位の姿勢を間接的に特定できるものをいう。
また、干渉判定とは、対象部位に対する障害物の干渉に関する判定をいい、干渉しないことを判定すること、干渉することを判定すること、干渉するか否かを判定すること、干渉の度合いを判定すること、干渉までの度合い（例えば、特徴点と対象部位の間の距離）を判定することが含まれる。以下、発明２の脚車輪型ロボットにおいて同じである。
【００１４】
また、線には、直線、線分、多次曲線、円、楕円その他の曲線が含まれる。以下、発明２の脚車輪型ロボットにおいて同じである。
【００１５】
〔発明２〕 一方、上記目的を達成するために、発明２の脚車輪型ロボットは、基体と、前記基体に対して自由度を有して連結された脚部と、前記脚部に回転可能に設けられた車輪と、物体センサとを備え、前記脚部の駆動および前記車輪の回転により移動する脚車輪型ロボットであって、前記物体センサの測定結果に基づいて障害物の特徴点を検出する特徴点検出手段と、前記特徴点および前記対象部位の座標を定める座標系において、当該脚車輪型ロボットの姿勢および前記対象部位の形状により特定される線、平面または空間であって、前記対象部位の形状に対応する線、前記対象部位の表面に対応する平面または前記対象部位に対応する空間と、前記特徴点検出手段で検出した特徴点との位置関係に基づいて、前記対象部位について障害物との干渉判定を行う干渉判定手段とを備える。
【００１６】
このような構成であれば、段差等の障害物のあるところでは、物体センサにより、その測定方向について測定が行われ、特徴点検出手段により、物体センサの測定結果に基づいて障害物の特徴点が検出される。そして、干渉判定手段により、特徴点および対象部位の座標を定める座標系において、対象部位の形状に対応する線、対象部位の表面に対応する平面または対象部位に対応する空間と、検出された特徴点との位置関係に基づいて、対象部位について障害物との干渉判定が行われる。この平面等は、脚車輪型ロボットの姿勢および対象部位の形状により特定される。
【００１７】
したがって、所定の座標系における平面等と特徴点の位置関係に基づいて干渉判定を行うので、実際の干渉を伴う必要はない。また、特許文献２における干渉判定専用のカメラのような専用のセンサを設けなくても、他の用途に用いられる物体センサを利用することができる。さらに、所定の座標系において対象部位に対応する平面等は姿勢や形状により特定されるため、障害物の特徴点さえ検出できればよく、障害物の監視範囲が対象部位の可動範囲に制約されない。
【００１８】
また、脚車輪型ロボットにおいては、例えば、物体センサを基体に設け、対象部位に対する障害物の干渉判定を行うことが考えられる。この場合、基体に対して自由度の範囲で脚部が駆動することから、脚車輪型ロボットの姿勢により、対象部位に対する物体センサの位置が変化することとなる。また、物体センサを走査する場合は、対象部位に対する物体センサの向きが変化することとなる。
【００１９】
このように、対象部位に対する物体センサの位置や向きが変化する場合は、物体センサの測定結果に基づいて検出される特徴点の座標と、対象部位の座標は、物体センサおよび対象部位の動作の系が異なることからそれぞれ異なる座標系により定められる。しかしながら、座標系が異なる場合は、対象部位に対応する平面等と特徴点の位置関係を直ちには特定することができない。そこで、座標系の変換により平面等と特徴点の座標系を一致させて（すなわち、特徴点および対象部位の座標を定める座標系において）それらの位置関係を特定する必要があるところ、本発明は、脚車輪型ロボットの姿勢および対象部位の形状に基づいてこれを実現することができる。
【００２０】
したがって、対象部位との位置関係によらず物体センサを設置することができる。
さらに、段差のあるところでは、自由度の範囲で脚部が可動し、段差を乗り越えることができる。したがって、脚型ロボットと同様に段差への適応性が高い。
一方、平地では、車輪走行で移動することができる。したがって、車輪型ロボットと同様に平地での移動性が高い。
【００２１】
ここで、特徴点および対象部位の座標を定める座標系としては、特徴点の絶対座標（例えば、接地面を基準とする座標）または相対座標（例えば、脚車輪型ロボットを基準とする座標）を定める座標系、対象部位の絶対座標（例えば、接地面を基準とする座標）または相対座標（例えば、脚部を基準とする座標）を定める座標系、その他任意の座標系が含まれる。なお、特徴点の座標を定める座標系と、対象部位の座標を定める座標系が異なる場合は、一方の座標系の座標を他方の座標系の座標に変換するか、両方の座標系の座標を、特徴点および対象部位の座標を定める他の座標系の座標に変換すればよい。
また、脚車輪型ロボットの姿勢には、脚車輪型ロボットの全体の姿勢のほか、脚車輪型ロボットの一部分（例えば、脚部、物体センサ）の姿勢も含まれる。
【００２２】
〔発明３〕 さらに、発明３の脚車輪型ロボットは、発明２の脚車輪型ロボットにおいて、前記干渉判定手段は、前記座標系において、前記対象部位の表面に対応する複数の平面で囲まれる空間に前記特徴点が属するか否かを判定することにより、前記対象部位に対して障害物が干渉するか否かを判定する。
【００２３】
このような構成であれば、干渉判定手段により、座標系において、対象部位の表面に対応する複数の平面で囲まれる空間に特徴点が属するか否かが判定される。対象部位に対して障害物が干渉した場合は、同空間に特徴点が属し、障害物が干渉していない場合は、同空間に特徴点は属さない。したがって、この判定により、対象部位に対して障害物が干渉するか否かが判定される。
【００２４】
〔発明４〕 さらに、発明４の脚車輪型ロボットは、発明３の脚車輪型ロボットにおいて、前記干渉判定手段は、当該脚車輪型ロボットの姿勢および前記対象部位の形状に基づいて、前記対象部位の座標を定める第１座標系の座標を、前記特徴点の座標を定める第２座標系の座標に変換して前記第２座標系における前記複数の平面を特定し、前記第２座標系において、前記複数の平面で囲まれる空間に前記特徴点が属するか否かを判定する。
このような構成であれば、干渉判定手段により、脚車輪型ロボットの姿勢および対象部位の形状に基づいて、第１座標系から第２座標系に座標が変換されて第２座標系における複数の平面が特定され、第２座標系において、複数の平面で囲まれる空間に特徴点が属するか否かが判定される。
【００２５】
〔発明５〕 さらに、発明５の脚車輪型ロボットは、発明３および４のいずれか１の脚車輪型ロボットにおいて、前記干渉判定手段は、前記特徴点と前記平面上の任意の点を結ぶ直線と、前記平面の法線ベクトルとの内積に基づいて、前記複数の平面で囲まれる空間に前記特徴点が属するか否かを判定する。
このような構成であれば、干渉判定手段により、特徴点と平面上の任意の点を結ぶ直線と、平面の法線ベクトルとの内積に基づいて、複数の平面で囲まれる空間に特徴点が属するか否かが判定される。
【００２６】
〔発明６〕 さらに、発明６の脚車輪型ロボットは、発明２ないし５のいずれか１の脚車輪型ロボットにおいて、前記物体センサは、物体上の測定点までの距離を測定する測距センサであり、前記測距センサの測定方向とは異なる第１走査方向に前記測距センサを走査する第１走査手段と、前記測定方向および前記第１走査方向とは異なる第２走査方向に前記測距センサを走査する第２走査手段と、前記第１走査手段および前記第２走査手段の走査範囲で測定可能な前記測定点について前記測距センサの測定結果を取得する測定結果取得手段とを備え、前記特徴点検出手段は、前記測定結果取得手段で取得した測定結果に基づいて前記特徴点を検出する。
このような構成であれば、第１走査手段により、第１走査方向に測距センサが、第２走査手段により、第２走査方向に測距センサがそれぞれ走査可能となる。したがって、障害物の立体的な形状を把握することができる。そして、測定結果取得手段により、第１走査手段および第２走査手段の走査範囲で測定可能な測定点について測距センサの測定結果が取得され、特徴点検出手段により、取得された測定結果に基づいて特徴点が検出される。
【００２７】
ここで、第１走査手段および第２走査手段としては、例えば、次の構成を採用することができる。
（１）回転機構
前記測距センサの測定方向に対して所定角度をなす第１走査軸の回りに前記測距センサを回転させる第１回転手段と、前記測定方向および前記第１走査軸に対して所定角度をなす第２走査軸の回りに前記測距センサを回転させる第２回転手段とからなる構成である。
【００２８】
（２）移動機構
前記測距センサの測定方向とは異なる第１走査方向に前記測距センサを移動させる第１移動手段と、前記測定方向および前記第１走査方向とは異なる第２走査方向に前記測距センサを移動させる第２移動手段とからなる構成である。なお、第１移動手段は、前記第１走査方向に延長する経路を含む第１経路に沿って前記測距センサを移動させてもよいし、第２移動手段は、前記第２走査方向に延長する経路を含む第２経路に沿って前記測距センサを移動させてもよい。以下、（３）、（４）においても同様である。
【００２９】
（３）回転機構と移動機構の組み合わせ
前記測距センサの測定方向に対して所定角度をなす走査軸の回りに前記測距センサを回転させる回転手段と、前記走査軸の軸方向とは異なる走査方向に前記測距センサを移動させる移動手段とからなる構成である。
【００３０】
（４）移動機構と回転機構の組み合わせ
前記測距センサの測定方向とは異なる走査方向に前記測距センサを移動させる移動手段と、前記走査方向に対して所定角度をなす走査軸の回りに前記測距センサを回転させる回転手段とからなる構成である。
【００３１】
〔発明７〕 さらに、発明７の脚車輪型ロボットは、発明６の脚車輪型ロボットにおいて、前記第１走査手段は、前記測定方向に対して所定角度をなす第１走査軸の回りに前記測距センサを回転させる第１回転手段であり、前記第２走査手段は、前記測定方向および前記第１走査軸に対して所定角度をなす第２走査軸の回りに前記測距センサを回転させる第２回転手段であり、前記測定結果取得手段は、前記第１回転手段により前記測距センサを回転させながら前記第１回転手段の所定単位角度ごとに前記測距センサの測定結果を取得する第１走査を、前記第２回転手段により前記測距センサを回転させながら前記第２回転手段の所定単位角度ごとに行う第２走査を行うことにより、前記第１回転手段の所定単位角度ごとおよび前記第２回転手段の所定単位角度ごとの前記測定結果を取得する。
【００３２】
このような構成であれば、第１回転手段により、第１走査軸の回りに測距センサが、第２回転手段により、第２走査軸の回りに測距センサがそれぞれ回転可能となる。したがって、障害物の立体的な形状を把握することができる。そして、測定結果取得手段により、第２走査が行われることにより第１回転手段の所定単位角度ごとおよび第２回転手段の所定単位角度ごとの測定結果が取得される。第２走査では、第２回転手段により測距センサを回転させながら第２回転手段の所定単位角度ごとに第１走査が行われる。第１走査では、第１回転手段により測距センサを回転させながら第１回転手段の所定単位角度ごとに測定結果が取得される。
【発明の効果】
【００３３】
以上説明したように、発明１の干渉判定装置によれば、所定の座標系における平面等と特徴点の位置関係に基づいて干渉判定を行うので、実際の干渉を伴うことなく干渉判定を行うことができるという効果が得られる。また、干渉判定専用のカメラのような専用のセンサを設けなくても、他の用途に用いられるセンサ等を利用することができるので、部品点数および重量の増加を抑制することができるという効果も得られる。さらに、所定の座標系において対象部位に対応する平面等は姿勢や形状により特定されるため、障害物の特徴点さえ検出できればよく、障害物の監視範囲が対象部位の可動範囲に制約されず、干渉判定の制約を低減することができるという効果も得られる。
【００３４】
一方、発明２の脚車輪型ロボットによれば、所定の座標系における平面等と特徴点の位置関係に基づいて干渉判定を行うので、実際の干渉を伴うことなく干渉判定を行うことができるという効果が得られる。また、干渉判定専用のカメラのような専用のセンサを設けなくても、他の用途に用いられる物体センサを利用することができるので、部品点数および重量の増加を抑制することができるという効果も得られる。さらに、所定の座標系において対象部位に対応する平面等は姿勢や形状により特定されるため、障害物の特徴点さえ検出できればよく、障害物の監視範囲が対象部位の可動範囲に制約されず、干渉判定の制約を低減することができるという効果も得られる。さらに、対象部位との位置関係によらず物体センサを設置することができるので、干渉判定の制約をさらに低減することができるという効果も得られる。
【００３５】
さらに、発明３の脚車輪型ロボットによれば、対象部位に対して障害物が干渉するか否かを比較的正確に判定することができるという効果が得られる。
さらに、発明５の脚車輪型ロボットによれば、複数の平面で囲まれる空間に特徴点が属するか否かを比較的容易に判定することができるという効果が得られる。
さらに、発明６の脚車輪型ロボットによれば、障害物の立体的な形状を把握することができ、これに基づいて検出された特徴点により干渉判定を行うことができるので、干渉判定の精度を向上することができるという効果が得られる。
【００３６】
さらに、発明７の脚車輪型ロボットによれば、測距センサを回転させる回転機構を採用したので、移動機構に比して、走査に必要なスペースが小さくてすみ、走査のための機構が簡素となり、しかも高速な走査を実現することができるという効果が得られる。
【発明を実施するための最良の形態】
【００３７】
以下、本発明の第１の実施の形態を図面を参照しながら説明する。図１ないし図１４は、本発明に係る干渉判定装置および脚車輪型ロボットの第１の実施の形態を示す図である。
【００３８】
まず、本発明を適用する脚車輪型ロボット１００の構成を説明する。
図１は、脚車輪型ロボット１００の正面図である。
図２は、脚車輪型ロボット１００の側面図である。
脚車輪型ロボット１００は、図１および図２に示すように、基体１０と、基体１０に連結された４つの脚部１２とを有して構成されている。
基体１０の前部には、２本の脚部１２が回転関節１４を介して左右対称の位置に連結されている。また、基体１０の後部には、２本の脚部１２が回転関節１４を介して左右対称の位置に連結されている。回転関節１４は、脚車輪型ロボット１００の底面と直交する方向を軸方向として回転する。すなわち、ヨー軸回りに回転する。
【００３９】
各脚部１２には、２つの回転関節１６、１８が設けられている。回転関節１４は、下方を軸方向として回転し、回転関節１６、１８は、回転関節１４が図１の状態であるときは、脚車輪型ロボット１００の側面と直交する方向を軸方向として回転する。すなわち、回転関節１４が図１の状態であるときは、ピッチ軸回りに回転し、回転関節１４が図１の状態から９０度回転した状態であるときは、ロール軸回りに回転する。したがって、脚部１２は、それぞれ３自由度を有する。
【００４０】
各脚部１２の先端には、回転関節１６、１８と軸方向を同一にして駆動輪２０が回転可能に設けられている。
各脚部１２の先端には、脚車輪型ロボット１００の移動経路上に存在する物体までの距離を測定する前方脚先センサ２２と、接地面までの距離を測定する下方脚先センサ２４とが設けられている。
【００４１】
一方、基体１０の正面の上部中央には、水平面レーザ光を照射する水平レーザ２６が設けられている。また、基体１０の正面の中央左右には、垂直面レーザ光を照射する垂直レーザ２８、３０がそれぞれ設けられている。
基体１０の正面の下部中央には、水平面レーザ光および垂直面レーザ光の反射光を含む画像を撮影するカメラ３２が設けられている。
【００４２】
水平レーザ２６は、カメラ３２で水平面レーザ光の反射光を含む画像が撮影できるように下方に所定角度傾けて設けられている。同様に、垂直レーザ２８は、カメラ３２で垂直面レーザ光の反射光を含む画像が撮影できるように右方に所定角度傾けて設けられ、垂直レーザ３０は、左方に所定角度傾けて設けられている。
カメラ３２の左右には、障害物を検出する障害物センサ３４、３６がそれぞれ設けられている。
【００４３】
図３は、障害物センサ３４、３６の構成を示す図である。
障害物センサ３４、３６は、図３（ａ）に示すように、指向性の低い超音波測距センサを複数アレイ状に配列して構成することができる。また、図３（ｂ）に示すように、指向性の高い赤外線測距センサを複数アレイ状に配列して構成することもできる。アレイ状に配列する構成に限らず、単体で構成してもよい。また、超音波測距センサまたは赤外線測距センサを複数平面上に配列したエリアセンサで構成してもよい。これにより、脚車輪型ロボット１００の移動経路上に存在する物体を大まかに検出することができる。
【００４４】
次に、脚車輪型ロボット１００の移動制御システムを説明する。
図４は、脚車輪型ロボット１００の移動制御システムを示すブロック図である。
各脚部１２の回転関節１４〜１８には、図４に示すように、回転関節１４〜１８を回転駆動する関節モータ４０がそれぞれ設けられている。各関節モータ４０には、関節モータ４０の回転角度位置を検出するエンコーダ４２と、モータ指令信号およびエンコーダ４２の出力信号に基づいて関節モータ４０の駆動を制御するドライバ４４が設けられている。
【００４５】
各脚部１２の駆動輪２０には、駆動輪２０を回転駆動する車輪モータ５０がそれぞれ設けられている。各車輪モータ５０には、車輪モータ５０の回転角度位置を検出するエンコーダ５２と、モータ指令信号およびエンコーダ５２の出力信号に基づいて車輪モータ５０の駆動を制御するドライバ５４が設けられている。
脚車輪型ロボット１００は、さらに、ＣＰＵ６０と、脚車輪型ロボット１００の姿勢を検出する３軸姿勢センサ７０と、カメラ３２の画像信号を処理するビジョンプロセッサ７２と、外部のＰＣ等と無線通信を行う無線通信部７４と、ビジョンプロセッサ７２および無線通信部７４とＣＰＵ６０の入出力を中継するハブ７６と、警告音等を出力するスピーカ７８とを有して構成されている。
【００４６】
３軸姿勢センサ７０は、ジャイロ若しくは加速度センサ、またはその両方を有し、地軸に対して脚車輪型ロボット１００の姿勢の傾きを検出する。
ＣＰＵ６０は、モータ指令出力Ｉ／Ｆ６１を介してドライバ４４、５４にモータ指令信号を出力し、角度取込Ｉ／Ｆ６２を介してエンコーダ４２、５２の出力信号を入力する。また、センサ入力Ｉ／Ｆ６３を介して、前方脚先センサ２２、下方脚先センサ２４、障害物センサ３４、３６および３軸姿勢センサ７０からそれぞれセンサ信号を入力する。また、通信Ｉ／Ｆ６４を介してハブ７６と信号の入出力を行い、サウンド出力Ｉ／Ｆ６５を介してスピーカ７８に音声信号を出力する。
【００４７】
次に、ＣＰＵ６０で実行される処理を説明する。
ＣＰＵ６０は、ＲＯＭ等の所定領域に格納されている制御プログラムを起動させ、その制御プログラムに従って、図５のフローチャートに示す昇降制御処理を実行する。
次に、昇降制御処理を説明する。
図５は、昇降制御処理を示すフローチャートである。
【００４８】
昇降制御処理は、脚部１２の昇降制御を行う処理であって、ＣＰＵ６０において実行されると、まず、図５に示すように、ステップＳ１００に移行する。
ステップＳ１００では、ビジョンプロセッサ７２から画像を取り込み、ステップＳ１０２に移行する。
ステップＳ１０２では、取り込んだ画像に基づいて光切断法により階段の特徴点を検出する。
【００４９】
図６は、光切断法の原理を説明するための図である。
光切断法は、三角測量の原理により計測対象上の座標を求める計測法である。図６に計測座標系を示す。
計測対象上の座標Ｐ（x0、y0、z0）は、カメラ３２の撮像素子上の任意の座標をＰs（xi、yi、zi）とすると、下式（１）により求めることができる。
【００５０】
【数１】
【００５１】
次に、得られた３次元座標から、レーザ光の反射光の不連続点または屈曲点を階段の特徴点として検出する。
図７は、階段にレーザ光を照射した状態およびカメラ３２の撮像素子の画像を示す図である。
【００５２】
脚車輪型ロボット１００の移動経路上に階段が存在すると、図７（ａ）左側に示すように、水平レーザ２６から照射された水平面レーザ光が階段の蹴込板および床面で反射し、カメラ３２により、その反射光を含む階段の画像が撮影される。その画像に対して画像処理を行うと、図７（ａ）右側に示すように、蹴込板での反射光エッジおよび床面での反射光エッジを検出することができる。そして、そのエッジ画像および上式（１）により得られた３次元座標に基づいて、反射光エッジの不連続点に対応する実座標を算出することができる。
【００５３】
また、図７（ｂ）左側に示すように、垂直レーザ２８から照射された垂直面レーザ光が階段の蹴込板および踏板で反射し、カメラ３２により、その反射光を含む階段の画像が撮影される。その画像に対して画像処理を行うと、図７（ｂ）右側に示すように、蹴込板での反射光エッジおよび踏板での反射光エッジを検出することができる。また、垂直レーザ３０についても同様であり、図７（ｃ）右側に示すように、蹴込板での反射光エッジおよび踏板での反射光エッジを検出することができる。そして、それらエッジ画像および上式（１）により得られた３次元座標に基づいて、反射光エッジの屈曲点に対する実座標を算出することができる。
【００５４】
図８は、階段の昇降時に段鼻部の特徴点を検出する場合を示す図である。
脚車輪型ロボット１００が階段を上る場合は、図８（ａ）に示すように、階段を上る前に、反射光エッジの屈曲点として段鼻部の特徴点を検出することができる。また、階段を下る場合は、図８（ｂ）に示すように、階段を下る前に、反射光エッジの屈曲点として段鼻部の特徴点を検出することができる。
【００５５】
図５に戻り、次いで、ステップＳ１０３に移行して、障害物との干渉の有無を判定する干渉判定処理を実行し、ステップＳ１０４に移行して、干渉判定処理の判定結果に基づいて障害物と干渉するか否かを判定し、干渉しないと判定したとき(No)は、ステップＳ１０５に移行する。
ステップＳ１０５では、検出した特徴点に基づいて階段の幅を算出し、ステップＳ１０６に移行して、検出した特徴点に基づいて階段の段鼻部の実座標を算出し、ステップＳ１０８に移行する。
【００５６】
ステップＳ１０８では、算出した階段の幅および段鼻部の実座標、並びに３軸姿勢センサ７０のセンサ信号に基づいて逆運動学計算および重心計算を行い、ステップＳ１１０に移行して、ステップＳ１０８の計算結果に基づいて脚先（駆動輪２０）の着地位置を決定し、ステップＳ１１２に移行する。
ステップＳ１１２では、前方脚先センサ２２および下方脚先センサ２４からそれぞれセンサ信号を入力し、ステップＳ１１４に移行して、入力した前方脚先センサ２２のセンサ信号に基づいて蹴込板までの距離を算出し、ステップＳ１１６に移行して、入力した下方脚先センサ２４のセンサ信号に基づいて脚先と踏板の位置関係を算出し、ステップＳ１１８に移行する。
【００５７】
ステップＳ１１８では、決定した着地位置および算出した両距離に基づいてドライバ４４、５４へのモータ指令信号を生成し、ステップＳ１２０に移行して、生成したモータ指令信号をドライバ４４、５４に出力し、ステップＳ１２２に移行する。
ステップＳ１２２では、脚先が踏板に着地したか否かを判定し、脚先が着地したと判定したとき(Yes)は、一連の処理を終了して元の処理に復帰させる。
【００５８】
一方、ステップＳ１２２で、脚先が着地しないと判定したとき(No)は、ステップＳ１１２に移行する。
一方、ステップＳ１０４で、障害物と干渉すると判定したとき(Yes)は、ステップＳ１２４に移行して、干渉を回避する干渉回避処理を実行し、一連の処理を終了して元の処理に復帰させる。
【００５９】
次に、ステップＳ１０３の干渉判定処理を説明する。
脚車輪型ロボット１００が階段を昇降する場合は、脚部１２のうち回転関節１８および駆動輪２０が連結するリンク（以下、脚すね部という。）と、階段の段鼻部とが干渉し得る。したがって、干渉判定処理では、脚すね部に対して段鼻部が干渉するか否かを判定する。この判定は、特定の脚部１２（例えば、前脚）について行ってもよいし、すべての脚部１２について行ってもよい。
【００６０】
図９は、グローバル座標系において脚すね部の表面に対応する４つの平面を示す図である。同図（ａ）は、脚すね部の斜視図であり、同図（ｂ）は、脚すね部の厚さ方向の断面図であり、同図（ｃ）は、脚すね部の伸長方向の断面図である。
段鼻部の特徴点の座標は、特徴点を検出するための物体センサ（本実施の形態では、レーザ２６〜３０およびカメラ３２）からみた脚車輪型ロボット１００の外部を基準とするグローバル座標系で定められる。そこで、干渉の判定は、図９（ａ）に示すように、グローバル座標系において、矩形からなる脚すね部の正面、背面、右側面および左側面の各表面ごとにその表面に対応する平面（以下、脚すね部平面という。）を特定し、図９（ｂ）に示すように、特定した４つの脚すね部平面で囲まれる空間（すなわち、脚すね部の内部領域に対応する空間）に特徴点が属するか否かを判定することにより行う。同空間に特徴点が属すると判定したときは、脚すね部に対して段鼻部が干渉すると判定し、同空間に特徴点が属さないと判定したときは、脚すね部に対して段鼻部が干渉しないと判定する。
【００６１】
図１０は、段鼻部の特徴点Ａ、脚すね部平面、脚すね部平面上の任意の点Ｐおよび脚すね部平面の法線ベクトルＮを示す図である。
段鼻部の特徴点をＡ、脚すね平面上の任意の点をＰ、脚すね部平面の法線ベクトルをＮとすると、図１０に示すように、特徴点Ａと点Ｐを結ぶ直線ＰＡと、法線ベクトルＮとの内積Ｄは、下式（２）により求めることができる。
【００６２】
【数２】
【００６３】
Ｄ＞０のときは、脚すね平面の表側（脚すね部の外側）に特徴点が位置し、Ｄ＝０のときは、脚すね平面上に特徴点が位置し、Ｄ＜０のときは、脚すね平面の裏側（脚すね部の内側）に特徴点が位置する。
【００６４】
脚すね部の正面、背面、右側面および左側面に対応する脚すね平面についての内積ＤをそれぞれＤ1、Ｄ2、Ｄ3、Ｄ4とすると、Ｄ1≧０またはＤ2≧０またはＤ3≧０またはＤ4≧０が成立する場合は、４つの脚すね部平面で囲まれる空間に特徴点Ａが属さないので、脚すね部に対して段鼻部が干渉しないと判定する。
なお、４つの脚すね部平面で判定する場合は、脚すね部の底面よりも下方の領域も干渉判定の対象領域としてしまうため、脚すね部が特徴点Ａを跨ぐような場合も干渉すると判定してしまう。そこで、より好ましくは、脚すね部の底面に対応する脚すね平面も考慮することである。脚すね部の底面に対応する脚すね平面についての内積ＤをＤ5とすると、Ｄ1≧０またはＤ2≧０またはＤ3≧０またはＤ4≧０またはＤ5≧０が成立する場合は、５つの脚すね部平面で囲まれる空間に特徴点Ａが属さないので、脚すね部に対して段鼻部が干渉しないと判定することができる。
【００６５】
同様に、４つの脚すね部平面で判定する場合は、脚すね部の上面よりも上方の領域も干渉判定の対象領域としてしまうため、脚すね部が特徴点Ａを潜るような場合も干渉すると判定してしまう。そこで、より好ましくは、脚すね部の上面に対応する脚すね平面も考慮することである。脚すね部の上面に対応する脚すね平面についての内積ＤをＤ6とすると、Ｄ1≧０またはＤ2≧０またはＤ3≧０またはＤ4≧０またはＤ6≧０が成立する場合は、５つの脚すね部平面で囲まれる空間に特徴点Ａが属さないので、脚すね部に対して段鼻部が干渉しないと判定することができる。
【００６６】
さらに好ましくは、脚すね部の底面および上面に対応する脚すね平面も考慮することである。Ｄ1≧０またはＤ2≧０またはＤ3≧０またはＤ4≧０またはＤ5≧０またはＤ6≧０が成立する場合は、６つの脚すね部平面で囲まれる空間に特徴点Ａが属さないので、脚すね部に対して段鼻部が干渉しないと判定することができる。
次に、グローバル座標系において点Ｐの座標を求める場合を説明する。
【００６７】
特徴点Ａの座標は、グローバル座標系で定められるのに対し、脚すね部の座標は、脚すね部を基準とする相対座標系で定められる。したがって、グローバル座標系における点Ｐの座標は、脚すね部の座標を定める座標系の座標をグローバル座標系の座標に変換して求める必要がある。
図１１は、グローバル座標系および脚車輪型ロボット１００の各座標系を示す図である。
【００６８】
図１１において、Ｐgは、グローバル座標系、Ｐ0は、基体１０の底面中心を原点とする座標系、Ｐ1は、基体１０と右後ろの脚部１２との付け根を原点とする座標系、Ｐ2は、回転関節１４を原点とする座標系、Ｐ3は、回転関節１６を原点とする座標系、Ｐ4は、回転関節１８を原点とする座標系である。ここで、座標系Ｐ4が脚すね部の座標を定める座標系となる。
【００６９】
グローバル座標系Ｐgおよび座標系Ｐ0〜Ｐ4は、３軸が互いに直交する直交座標系として定義される。
グローバル座標系Ｐgは、特徴点Ａを検出するための物体センサからみた座標系であって、その原点は、座標系Ｐ0の原点から所定距離（Ａ、Ｂ、Ｃ）離れたところに設定されている。
【００７０】
座標系Ｐ0は、基体１０の奥行き（前後の長さ）方向をｘ0軸、基体１０の幅（左右の長さ）方向をｙ0軸、基体１０の高さ方向をｚ0軸とし、ｘ0軸は基体１０の後方を、ｙ0軸は基体１０の右方を、ｚ0軸は基体１０の上方をそれぞれ正の方向とする。原点は、基体１０の底面中心となるので、基体１０の奥行きをＬ、基体１０の幅をＷとすると、（Ｌ／２、Ｗ／２、０）となる。
【００７１】
グローバル座標系Ｐgと座標系Ｐ0の関係は、下式（３）の行列で表すことができる。したがって、下式（３）により、グローバル座標系Ｐgから座標系Ｐ0への座標変換を行うことができる。
【００７２】
【数３】
【００７３】
座標系Ｐ1は、ｘ0軸、ｙ0軸、ｚ0軸と同一の方向をｘ1軸、ｙ2軸、ｚ3軸とし、ｘ1軸は基体１０の後方を、ｙ1軸は基体１０の右方を、ｚ1軸は基体１０の下方をそれぞれ正の方向とする。原点は、基体１０と右後ろの脚部１２との付け根となるので、（０、０、０）となる。
【００７４】
座標系Ｐ0、Ｐ1の関係は、下式（４）の行列で表すことができる。したがって、下式（４）により、座標系Ｐ0から座標系Ｐ1への座標変換を行うことができる。
【００７５】
【数４】
【００７６】
座標系Ｐ2は、座標系Ｐ1に対して回転関節１４がθ1回転した座標系であって、ｚ1軸と同一の方向をｚ2軸、ｘ1軸に対してｚ2軸回りにθ1回転した方向をｘ2軸、ｙ1軸に対してｚ2軸回りにθ1回転した方向をｙ2軸とし、ｚ2軸は基体１０の下方を正の方向とする。
【００７７】
座標系Ｐ1、Ｐ2の関係は、下式（５）の行列で表すことができる。したがって、下式（５）により、座標系Ｐ1から座標系Ｐ2への座標変換を行うことができる。
【００７８】
【数５】
【００７９】
座標系Ｐ3は、座標系Ｐ2に対して回転関節１６がθ2回転した座標系であって、ｙ2軸と同一の方向をｙ3軸、ｘ2軸に対してｙ3軸回りにθ2回転した方向をｘ3軸、ｚ2軸に対してｙ3軸回りにθ2回転した方向をｚ3軸とし、ｚ3軸は、脚部１２のうち回転関節１４、１６が連結するリンクの下方を正の方向とする。
【００８０】
座標系Ｐ2、Ｐ3の関係は、下式（６）の行列で表すことができる。したがって、下式（６）により、座標系Ｐ2から座標系Ｐ3への座標変換を行うことができる。
【００８１】
【数６】
【００８２】
座標系Ｐ4は、座標系Ｐ3に対して回転関節１８がθ3回転した座標系であって、ｙ3軸と同一の方向をｙ4軸、ｘ3軸に対してｙ4軸回りにθ3回転した方向をｘ4軸、ｚ3軸に対してｙ4軸回りにθ3回転した方向をｚ4軸とし、ｚ4軸は、脚すね部の下方を正の方向とする。
【００８３】
座標系Ｐ3、Ｐ4の関係は、下式（７）の行列で表すことができる。したがって、下式（７）により、座標系Ｐ3から座標系Ｐ4への座標変換を行うことができる。
【００８４】
【数７】
【００８５】
図１２は、座標系Ｐ4を示す図である。
座標系Ｐ4における点Ｐの座標は、図１２に示すように、脚すね部の奥行きをｌ、幅をｍ、伸長方向の長さをｎとすると、（ｌ／２、０、０）となる。
【００８６】
したがって、グローバル座標系Ｐgにおける点Ｐの座標は、下式（８）により求めることができる。下式（８）のなかで、θ1〜θ3は、脚車輪型ロボット１００の姿勢により、ｌは、脚すね部の形状により、Ｌ、Ｗは、基体１０の形状によりそれぞれ特定されるので、グローバル座標系Ｐgにおける点Ｐの座標は、脚車輪型ロボット１００の姿勢並びに基体１０および脚すね部の形状により特定することができる。
【００８７】
【数８】
【００８８】
上式（８）において、4Ｘpは、下式（９）により表すことができ、4Ｘpのうち（ｌ／２、０、０）は、座標系Ｐ4における点Ｐの座標である。また、Ｘpのうち（ｘ、ｙ、ｚ）は、グローバル座標系Ｐgにおける点Ｐの座標である。
【００８９】
【数９】
【００９０】
次に、脚すね部平面の法線ベクトルＮを求める場合を説明する。
図１３は、脚すね部平面および法線ベクトルＮを示す図である。
法線ベクトルＮは、図１３に示すように、グローバル座標系Ｐgにおいて、脚すね部平面の４つの頂点（脚すね部の表面のうち右上、右下、左上および左下の各角部に対応する点）のうち３つをそれぞれＶ1、Ｖ2、Ｖ3とすると、点Ｖ3から点Ｖ1へのベクトルと、点Ｖ2から点Ｖ3のベクトルの外積で表すことができるので、下式（10）により求めることができる。
【００９１】
【数１０】
【００９２】
なお、グローバル座標系Ｐgにおける点Ｖ1の座標は、点Ｐの場合と同様に、下式（11）により求めることができる。グローバル座標系Ｐgにおける点Ｖ2、Ｖ3の座標も同様に求めることができる。
【００９３】
【数１１】
【００９４】
図１４は、干渉判定処理を示すフローチャートである。
干渉判定処理は、ステップＳ１０３において実行されると、まず、図１４に示すように、ステップＳ２００に移行する。
【００９５】
ステップＳ２００では、回転関節１４〜１８の角度θ1〜θ3、基体１０の寸法Ｌ、Ｗおよび脚すね部の寸法ｌに基づいて、上式（８）により、グローバル座標系Ｐgにおける点Ｐの座標を算出し、ステップＳ２０２に移行する。
ステップＳ２０２では、回転関節１４〜１８の角度θ1〜θ3、基体１０の寸法Ｌ、Ｗおよび脚すね部の寸法ｌに基づいて、上式（11）により、各脚すね部平面ごとにグローバル座標系Ｐgにおける点Ｖ1〜Ｖ3の座標を算出し、ステップＳ２０４に移行する。
【００９６】
ステップＳ２０４では、上式（10）により、各脚すね部平面ごとに法線ベクトルＮを算出し、ステップＳ２０６に移行して、特徴点Ａの座標を取得し、ステップＳ２０８に移行して、上式（２）により、特徴点Ａと点Ｐを結ぶ直線ＰＡと、法線ベクトルＮとの内積Ｄ1〜Ｄ4（またはＤ5、Ｄ6）を算出し、ステップＳ２１０に移行する。
ステップＳ２１０では、Ｄ1≧０またはＤ2≧０またはＤ3≧０またはＤ4≧０（またはＤ5≧０、Ｄ6≧０）が成立するか否かを判定することにより４つの脚すね部平面で囲まれる空間に特徴点Ａが属するか否かを判定し、同空間に特徴点Ａが属さないと判定したとき(No)は、ステップＳ２１２に移行する。
【００９７】
ステップＳ２１２では、未判定の特徴点Ａが存在するか否かを判定し、未判定の特徴点Ａが存在しないと判定したとき(No)は、ステップＳ２１４に移行して、障害物と干渉しないことを示す値（例えば、「０」）を戻り値として設定し、一連の処理を終了して元の処理に復帰させる。
一方、ステップＳ２１２で、未判定の特徴点Ａが存在すると判定したとき(Yes)は、ステップＳ２１６に移行して、次の特徴点Ａの座標を取得し、ステップＳ２０８に移行する。
一方、ステップＳ２１０で、４つの脚すね部平面で囲まれる空間に特徴点Ａが属すると判定したとき(Yes)は、ステップＳ２１８に移行して、障害物と干渉することを示す値（例えば、「１」）を戻り値として設定し、一連の処理を終了して元の処理に復帰させる。
【００９８】
次に、本実施の形態の動作を説明する。
脚車輪型ロボット１００の移動経路上に階段が存在すると、水平レーザ２６から照射された水平面レーザ光、および垂直レーザ２８、３０から照射された垂直面レーザ光がそれぞれ階段で反射し、カメラ３２により、それら反射光を含む画像が撮影される。次いで、ステップＳ１００、Ｓ１０２を経て、カメラ３２で撮影された画像が取り込まれ、取り込まれた画像から階段の特徴点が検出される。
【００９９】
次いで、ステップＳ１０３を経て、干渉判定処理が実行される。干渉判定処理では、ステップＳ２００〜Ｓ２１０を経て、各脚すね部平面ごとに法線ベクトルＮが算出され、特徴点Ａと点Ｐを結ぶ直線ＰＡと、法線ベクトルＮとの内積Ｄ1〜Ｄ4（またはＤ5、Ｄ6）が算出され、内積Ｄ1〜Ｄ4（またはＤ5、Ｄ6）に基づいて４つの脚すね部平面で囲まれる空間に特徴点Ａが属するか否かが判定される。いずれの特徴点Ａも同空間に属しないと判定されると、ステップＳ２１４、Ｓ１０４を経て、障害物と干渉しないと判定される。非干渉が判定されると、ステップＳ１０５〜Ｓ１１０を経て、検出された特徴点に基づいて階段の幅および段鼻部の実座標が算出され、算出された階段の幅および段鼻部の実座標に基づいて脚先の着地位置が決定される。
【０１００】
さらに、ステップＳ１１２〜Ｓ１１６を経て、脚先センサ２２、２４からそれぞれセンサ信号が入力され、蹴込板までの距離および脚先と踏板の位置関係が算出される。そして、ステップＳ１１８、Ｓ１２０を経て、決定された着地位置および算出された両距離に基づいてモータ指令信号が生成され、生成されたモータ指令信号がドライバ４４、５４に出力される。これにより、駆動輪２０が回転するとともに回転関節１４〜１８が駆動し、脚車輪型ロボット１００が姿勢を適切に保ちつつ階段を乗り越える。また、状況によっては階段を回避、停止する。したがって、脚型ロボットと同様に階段への適応性が高い。
【０１０１】
一方、いずれかの特徴点Ａが同空間に属すると判定されると、ステップＳ２１８、Ｓ１０４を経て、障害物と干渉すると判定される。干渉が判定されると、ステップＳ１２４を経て、干渉回避処理が実行され、干渉が回避される。
一方、平地では、脚車輪型ロボット１００は、車輪走行で移動することができる。したがって、車輪型ロボットと同様に平地での移動性が高い。
【０１０２】
このようにして、本実施の形態では、レーザ２８〜３０およびカメラ３２からなる物体センサを備え、物体センサの測定結果に基づいて障害物の特徴点を検出し、グローバル座標系Ｐgにおいて、脚車輪型ロボット１００の姿勢並びに基体１０および脚すね部の形状により特定される脚すね部平面と、検出した特徴点との位置関係に基づいて、脚すね部に対して障害物が干渉するか否かを判定する。
【０１０３】
これにより、グローバル座標系Ｐgにおける脚すね部平面と特徴点の位置関係に基づいて干渉の有無を判定するので、実際の干渉を伴うことなく干渉の有無を判定することができる。また、特許文献２における干渉判定専用のカメラのような専用のセンサを設けなくても、本実施の形態のように、他の用途（脚部１２の駆動制御等）に用いられる物体センサを利用することができるので、部品点数および重量の増加を抑制することができる。さらに、グローバル座標系Ｐgにおいて脚すね部平面は姿勢や形状により特定されるため、障害物の特徴点さえ検出できればよく、障害物の監視範囲が脚すね部の可動範囲に制約されず、干渉判定の制約を低減することができる。さらに、脚すね部との位置関係によらず物体センサを設置することができるので、干渉判定の制約をさらに低減することができる。
【０１０４】
さらに、本実施の形態では、グローバル座標系Ｐgにおいて、４つの脚すね部平面で囲まれる空間に特徴点が属するか否かを判定することにより、脚すね部に対して障害物が干渉するか否かを判定する。
これにより、脚すね部に対して障害物が干渉するか否かを比較的正確に判定することができる。
【０１０５】
さらに、本実施の形態では、特徴点と脚すね部平面上の任意の点を結ぶ直線と、脚すね部平面の法線ベクトルとの内積に基づいて、４つの脚すね部平面で囲まれる空間に特徴点が属するか否かを判定する。
これにより、４つの脚すね部平面で囲まれる空間に特徴点が属するか否かを比較的容易に判定することができる。
【０１０６】
さらに、本実施の形態では、レーザ２６〜３０およびカメラ３２からなる画像センサと、脚先センサ２２、２４とを備え、カメラ３２で撮影した画像および脚先センサ２２、２４で測定した距離に基づいて階段を認識し、その認識結果に基づいてモータ４０、５０を制御する。
これにより、画像センサおよび脚先センサ２２、２４を用いて未知の階段を認識しながら脚部１２の昇降制御を行うので、従来に比して、未知の階段に対して高い適応性を実現することができる。また、人が活動する環境での動作を行えるので、人と一緒に行動する用途に用いられるホームロボット、パーソナルロボット等に好適である。
【０１０７】
さらに、本実施の形態では、画像センサを基体１０の正面に設け、脚先センサ２２、２４を脚部１２の先端に設けた。
これにより、脚車輪型ロボット１００の移動経路上に存在する物体を広い視野で検出することができるとともに、階段昇降時に駆動輪２０と階段の距離を精度よく測定することができる。
【０１０８】
さらに、本実施の形態では、進行方向に対して所定角度にレーザ光を照射するレーザ２６〜３０と、レーザ２６〜３０から所定距離隔てて設けられかつレーザ光の反射光を含む画像を撮像するカメラ３２とを備え、カメラ３２で撮影した画像に基づいて光切断法により階段を認識する。
これにより、階段の特徴のうち脚部１２の昇降制御に有効な特徴を検出することができるので、未知の階段に対してさらに高い適応性を実現することができる。
【０１０９】
さらに、本実施の形態では、水平レーザ２６から照射された水平面レーザ光の反射光の撮影状態に基づいて階段の幅を算出し、垂直レーザ２８、３０から照射された２つの垂直面レーザ光の反射光の撮影状態に基づいて階段の段鼻部の実座標を算出する。
これにより、階段の特徴のうち脚部１２の昇降制御にさらに有効な特徴を検出することができるので、未知の階段に対してさらに高い適応性を実現することができる。
【０１１０】
さらに、本実施の形態では、前方脚先センサ２２の測定結果に基づいて階段の蹴込板までの距離を算出し、下方脚先センサ２４の測定結果に基づいて駆動輪２０と階段の踏板の位置関係を算出する。
これにより、階段の特徴のうち脚部１２の昇降制御にさらに有効な特徴を検出することができるので、未知の階段に対してさらに高い適応性を実現することができる。
【０１１１】
上記第１の実施の形態において、脚すね部は、発明１ないし４の対象部位に対応し、ステップＳ１０２は、発明１または２の特徴点検出手段に対応し、ステップＳ１０３は、発明１ないし５の干渉判定手段に対応し、座標系Ｐ4は、発明４の第１座標系に対応し、グローバル座標系Ｐgは、発明４の第２座標系に対応している。
次に、本発明の第２の実施の形態を図面を参照しながら説明する。図１５ないし図２８は、本発明に係る干渉判定装置および脚車輪型ロボットの第２の実施の形態を示す図である。
【０１１２】
本実施の形態は、上記第１の実施の形態に対して、レーザ２８〜３０およびカメラ３２からなる物体センサに代えて、測距センサを２方向にそれぞれ走査して測定を行う３次元距離測定装置を用いて特徴点を検出する点が異なる。
まず、脚車輪型ロボット１００の構成を説明する。
図１５は、脚車輪型ロボット１００を側面図である。
【０１１３】
基体１０の正面には、図１５に示すように、３次元距離測定装置２００が取り付けられている。３次元距離測定装置２００の座標系は、基体１０の奥行き方向をｘ軸、基体１０の幅方向をｙ軸、基体１０の高さ方向をｚ軸とし、ｘ軸は基体１０の前方を、ｙ軸は基体１０の右方を、ｚ軸は基体１０の上方をそれぞれ正の方向とする。
次に、３次元距離測定装置２００の外観構造を説明する。
【０１１４】
図１６は、３次元距離測定装置２００の外観構造を示す図である。同図（ａ）は、３次元距離測定装置２００の正面図（ｚ−ｙ平面）を示す図であり、同図（ｂ）は、３次元距離測定装置２００の上面図（ｘ−ｙ平面）である。
３次元距離測定装置２００は、図１６（ａ）に示すように、板状の支持部材２１９と、２次元距離測定装置２１２と、２次元距離測定装置２１２を回転駆動するモータ２１６と、モータ２１６の回転角度位置を検出するエンコーダ２１８と、モータ２１６の駆動力を２次元距離測定装置２１２に伝達する駆動プーリ２２０ａ、従動プーリ２２０ｂおよびベルト２２１とを有して構成されている。
【０１１５】
支持部材２１９の下面には、モータ２１６が取り付けられている。モータ２１６の回転軸（以下、駆動回転軸という。）は、支持部材２１９を下方から貫通し、支持部材２１９を挟んで反対側に配置された駆動プーリ２２０ａの軸穴に連結している。
一方、支持部材２１９の下面であってモータ２１６から水平方向に所定距離隔てた位置には、２次元距離測定装置２１２が取り付けられている。２次元距離測定装置２１２は、回転軸（以下、従動回転軸という。）を有し、従動回転軸が支持部材２１９を下方から貫通し、支持部材２１９を挟んで反対側に配置された従動プーリ２２０ｂの軸穴に連結している。
【０１１６】
駆動プーリ２２０ａおよび従動プーリ２２０ｂには、ベルト２２１が巻き掛けられている。したがって、モータ２１６により駆動プーリ２２０ａが回転し、駆動プーリ２２０ａに巻き掛けたベルト２２１により従動プーリ２２０ｂが回転することにより、２次元距離測定装置２１２は、図１６（ｂ）に示すように、ｚ軸回りに回転する。
次に、２次元距離測定装置２１２の制御構造を説明する。
【０１１７】
図１７は、２次元距離測定装置２１２の制御構造を示すブロック図である。
２次元距離測定装置２１２は、図１７に示すように、測定範囲内に存在する物体上の測定点までの距離を測定する測距センサ２１２ａと、測距センサ２１２ａを回転駆動するモータ２１２ｃと、モータ２１２ｃの回転角度位置を検出するエンコーダ２１２ｄと、指令信号およびエンコーダ２１２ｄの出力信号に基づいてモータ２１２ｃの駆動を制御するドライバ２１２ｂとを有して構成されている。
【０１１８】
ドライバ２１２ｂは、センシングプロセッサ２１０からの指令信号において設定された走査角度範囲（例えば、−４０°〜＋４０°等の所定の角度範囲）および走査単位角度（例えば、０．３６°等の所定の単位角度）に基づいて、モータ２１２ｃの回転軸を走査単位角度ずつ回転させる制御を行う。
モータ２１２ｃは、測距センサ２１２ａのレーザ出力部（不図示）および受光部（不図示）をｙ軸回りに回転駆動するように設けられており、ドライバ２１２ｂからの制御信号に応じて、自己の回転軸を走査単位角度（Δθ）ずつ回転駆動する。
【０１１９】
図１８は、測距センサ２１２ａの走査範囲を示す図である。
測距センサ２１２ａの走査範囲は、脚車輪型ロボット１００が階段の昇降や障害物の回避を行うことを目的としているため、図１８に示すように、脚車輪型ロボット１００の下方を重点的に走査するように設定されている。なお、３次元距離測定装置２００および測距センサ２１２ａの原点位置（回転角度θおよびφが０°の位置）においては、測距センサ２１２ａの測定方向がｘ軸と一致し、測距センサ２１２ａの回転軸がｙ軸と一致する。
【０１２０】
３次元距離測定装置２００は、２次元距離測定装置２１２を回転駆動する回転駆動機構（モータ２１６、エンコーダ２１８、駆動プーリ２２０ａ、従動プーリ２２０ｂ、ベルト２２１および支持部材２１９）が、図１８に示す走査範囲外に設けられているため、図１８に示す走査範囲であれば、３次元距離測定装置２００を構成する各機構部によって、測距センサ２１２ａのｙ軸回りの走査が阻害されない。
【０１２１】
また、脚車輪型ロボット１００の走行経路上の障害物を認識できればよいので、ｚ軸回りの回転駆動による走査範囲も、前方１８０°までをカバーする必要はなく、２次元距離測定装置２１２から水平方向に所定距離隔てて配置されたモータ２１６およびエンコーダ２１８を走査範囲外とする範囲でも十分である。したがって、２次元距離測定装置２１２の回転駆動範囲を、モータ２１６およびエンコーダ２１８を含まない範囲とする。
【０１２２】
次に、３次元距離測定装置２００の制御構造を説明する。
図１９は、３次元距離測定装置２００の制御構造を示すブロック図である。
３次元距離測定装置２００は、図１９に示すように、センシングプロセッサ２１０と、２次元距離測定装置２１２と、モータ２１６と、エンコーダ２１８と、指令信号およびエンコーダ２１８の出力信号に基づいてモータ２１６の駆動を制御するドライバ２１４とを有して構成されている。
【０１２３】
センシングプロセッサ２１０は、専用のプログラムを実行し、ドライバ２１２ｂに指令信号を与えて測距センサ２１２ａを回転させ、測距センサ２１２ａの走査範囲で測定可能な領域（以下、走査平面という。）内に存在する物体上の測定点までの距離を測定する第１走査処理を実行するとともに、１つの走査平面に対する第１走査処理が終了するごとに、ドライバ２１４に指令信号を与えて２次元距離測定装置２１２を回転させる第２走査処理を実行する。
【０１２４】
さらに、センシングプロセッサ２１０は、第１走査処理および第２走査処理を経て２次元距離測定装置２１２で測定した距離の情報（以下、距離情報という。）に基づいて、各走査平面の各測定点の勾配の出現頻度を算出する処理を実行するとともに、算出した出現頻度および各測定点の座標情報に基づいて、各走査平面における物体の１次元の形状（凹凸）および測定範囲内に存在する物体上の面の形状を認識する処理を実行する。
【０１２５】
次に、３次元距離測定装置２００の距離測定の原理を説明する。
図２０は、２次元距離測定装置２１２の距離測定の原理を説明する図である。
２次元距離測定装置２１２は、測距センサ２１２ａが、モータ２１２ｃの回転軸の回転駆動に応じて、ｙ軸回りに走査単位角度ずつ回転し、かつ、回転するごとに、図２０に示すように、レーザ出力部からレーザ光を出力するとともに、出力光に対する物体（図２０中の障害物）からの反射光を受光部で受光し、各回転角度（走査角度）に応じた距離（図２０中の測定距離Ｌ（物体と受光部との間の距離））を測定する。
【０１２６】
図２１は、第１走査処理および第２走査処理により走査を行った場合を示す図である。同図（ａ）は、測距センサ２１２ａをｙ軸回りに回転させたときの測定距離と回転角度θとの関係を示す図であり、同図（ｂ）は、２次元距離測定装置２１２をｚ軸回りに回転させたときの走査平面と回転角度φとの関係を示す図である。
第１走査処理は、例えば、図２１（ａ）に示すように、測距センサ２１２ａをｙ軸回りに走査単位角度ずつ回転させながら、原点位置に対する各回転角度（図２１（ａ）中のθ1、θ2、θ3）に応じた距離情報（図２１（ａ）中のＬ（θ1）、Ｌ（θ2）、Ｌ（θ3））を測定する処理となる。
【０１２７】
また、第１走査処理における、モータ２１２ｃの回転軸の回転中心と、レーザの走査軌道線の両端とを結んで形成される平面が、走査平面（物体が存在しない場合は扇形の平面）となる。
ドライバ２１４は、センシングプロセッサ２１０からの指令信号において設定された走査角度範囲および走査単位角度（Δφ）に基づいて、モータ２１６の回転軸を走査単位角度ずつ回転させる制御を行う。
【０１２８】
モータ２１６は、減速機（不図示）、駆動プーリ２２０ａ、従動プーリ２２０ｂおよびベルト２２１を介して、２次元距離測定装置２１２をｚ軸回りに回転駆動するように設けられており、ドライバ２１４からの制御信号に応じて、自己の回転軸を走査単位角度ずつ回転駆動する。これにより、モータ２１６の回転軸の回転駆動に応じて、駆動プーリ２２０ａ、従動プーリ２２０ｂおよびベルト２２１を介して従動回転軸に回転駆動力が伝達され、２次元距離測定装置２１２がｚ軸回りに走査単位角度ずつ回転する。
【０１２９】
すなわち、第２走査処理は、図２１（ｂ）に示すように、２次元距離測定装置２１２をｚ軸回りに走査単位角度ずつ回転させる処理となる。そして、第１走査処理と第２走査処理とを交互に連続して行うことにより、第１走査処理によって形成される走査平面をｚ軸回りに連続して形成する。
図２２は、３次元距離測定装置２００の距離の計測例を示す図である。
【０１３０】
これにより、例えば、図２２に示すように、壁、ついたて、スタンド、棚等の物体の立体的な形状を測定することが可能である。
また、図２１（ｂ）に示すように、第２走査処理後の各測定点の距離情報をＬ（θi，φj）と表記する。ここで、ｉは、ｙ軸回りの走査角度に応じて各測定点に付与される通し番号であり、ｊは、ｚ軸回りの走査角度に応じて各測定点に付与される通し番号である。
【０１３１】
次に、３次元距離測定装置２００における物体認識処理の流れを説明する。
図２３は、３次元距離測定装置２００における物体認識処理を示すフローチャートである。
物体認識処理は、ＣＰＵ６０からの指令信号に基づいて、センシングプロセッサ２１０が、ＲＯＭ（不図示）に記憶された専用のプログラムを読み出し、読み出したプログラムを実行することで実現される処理であり、処理が開始されると、図２３に示すように、まず、ステップＳ３００に移行する。
【０１３２】
ステップＳ３００では、３次元距離測定装置２００において、ＣＰＵ６０からの指令信号に基づいて、測距センサ２１２ａおよび２次元距離測定装置２１２の走査角度範囲および走査単位角度を設定し、ステップＳ３０２に移行する。ここで、ＣＰＵ６０からの指令信号には、走査角度範囲および走査単位角度の情報が含まれている。
ステップＳ３０２では、２次元距離測定装置２１２において、センシングプロセッサ２１０からの指令信号に応じて、ドライバ２１２ｂ、モータ２１２ｃおよびエンコーダ２１２ｄを駆動し、測距センサ２１２ａを、ステップＳ３００で設定されたｙ軸回りの走査角度範囲内において、同じくステップＳ３００で設定された走査単位角度ずつｙ軸回りに回転させるとともに、各回転角度に応じた距離情報を測定する処理（第１走査処理）を実行し、ステップＳ３０４に移行する。
【０１３３】
ステップＳ３０４では、センシングプロセッサ２１０において、ステップＳ３０２で測定した距離情報に対して、メディアンフィルタを用いたフィルタリング処理を実行してノイズ成分を除去し、ステップＳ３０６に移行する。
ステップＳ３０６では、センシングプロセッサ２１０において、ステップＳ３０４でノイズ除去後の回転座標系の距離情報を直交座標系の距離情報に変換し、ステップＳ３０８に移行する。
【０１３４】
回転座標系から直交座標系への変換は、第１走査処理による各測定点の座標は、ｚ−ｘ平面上の座標となり、ｘ軸は、第２走査処理によって、ｚ軸回りに走査単位角度ずつ座標位置が変化する。したがって、直交座標系への変換は、ｚ軸回りの各回転角度ごとに行われる。
ステップＳ３０８では、センシングプロセッサ２１０において、ステップＳ３０６で直交座標系に変換された距離情報に基づいて、各測定点の勾配を算出してステップＳ３１０に移行する。
【０１３５】
図２４は、勾配の算出例を示す図である。
各測定点の勾配は、各測定点の測定距離と、その前後の所定数の測定点の距離情報とを用いて、図２４（ａ）に示すように、最小二乗法を用いて算出する。
図２４（ａ）に示すように、最小二乗法を用いて、勾配の算出対象の測定点（図２４（ａ）中の白丸）を含む前後所定数の測定点に対する回帰直線を近似し、回帰直線の勾配を、対象の測定点の勾配とする。最小二乗法は、データのばらつきによって受ける影響は少ないが、計算量が比較的多くなる。
【０１３６】
また、別の方法として、図２４（ｂ）に示すように、各測定点に対して、所定数の他の測定点を挟む前後２つの測定点の距離情報の差分値を用いて勾配を算出する。
具体的に、図２４（ｂ）に示すように、前後２つの測定点のｚ軸方向の測定距離の差分値である第１差分値（例えば、注目する測定点のｚ軸方向の距離をｚiとすると、ｚi+2−ｚi-2）を算出するとともに、前後２つの測定点のｘ軸方向の測定距離の差分値である第２差分値（例えば、注目する測定点のｘ軸方向の距離をｘiとすると、ｘi+2−ｘi-2）を算出し、これら算出結果から、注目する測定点の勾配（第１差分値／第２差分値）を求める。この方法では、データのばらつきによる影響が最小二乗法よりも大きくなるが、計算が、単純な引き算と割り算のみで済むため最小二乗法よりも高速に勾配を算出することができる。
【０１３７】
ステップＳ３１０では、センシングプロセッサ２１０において、ステップＳ３０８で算出した勾配に基づいて各勾配の出現頻度を算出し、ステップＳ３１２に移行する。
本実施の形態では、勾配ごとの測定点の個数を出現頻度として算出する。
ステップＳ３１２では、センシングプロセッサ２１０において、ステップＳ３１０で算出した各勾配の出現頻度に基づいて、出現頻度の判定処理が未処理の勾配の出現頻度を選択し、ステップＳ３１４に移行する。
【０１３８】
ステップＳ３１４では、センシングプロセッサ２１０において、ステップＳ３１２で選択した勾配の出現頻度とあらかじめ設定された閾値とを比較するとともに選択された出現頻度は、閾値以上か否かを判定し、閾値以上と判定された場合(Yes)は、ステップＳ３１６に移行し、そうでない場合(No)は、ステップＳ３３４に移行する。
ステップＳ３１６に移行した場合は、センシングプロセッサ２１０において、選択した勾配とこの勾配を有する各測定点の座標情報とを対応付けて不図示のＲＡＭ等の記録媒体に記録し、ステップＳ３１８に移行する。
【０１３９】
ステップＳ３１８では、センシングプロセッサ２１０において、すべての勾配についてステップＳ３１４の判定処理が終了したか否かを判定し、終了したと判定された場合(Yes)は、ステップＳ３２０に移行し、そうでない場合(No)は、ステップＳ３１２に移行する。
ステップＳ３２０に移行した場合は、センシングプロセッサ２１０において、ステップＳ３１６で記録した勾配と各測定点の座標情報とに基づいて、連続面（実際は、連続面と走査平面とが交差する線）の有無を判断し、ステップＳ３２２に移行する。
【０１４０】
ここで、連続面の判断は、例えば、所定の勾配範囲に含まれる勾配に対応する各測定点の座標情報に基づいて、測定点の座標が連続しているときに、これらの座標位置と交差する連続面があると判断する。所定の勾配範囲に含まれる勾配に対応する測定点の座標が連続していないときは、連続面がないと判断する。
ステップＳ３２２では、センシングプロセッサ２１０において、ステップＳ３２０の判断結果に基づいて連続面があるか否かを判定し、連続面があると判定された場合(Yes)は、ステップＳ３２４に移行し、そうでない場合(No)は、ステップＳ３２６に移行する。
【０１４１】
ステップＳ３２４に移行した場合は、センシングプロセッサ２１０において、連続面に対応する各測定点の測定距離の座標系をセンサ座標系（３次元座標）に変換し、変換後のセンサ座標系の距離情報を勾配の情報と対応付けて、不図示のＲＡＭ等の記録媒体に記録し、ステップＳ３２６に移行する。
ステップＳ３２６では、センシングプロセッサ２１０において、第２走査処理の走査角度範囲および走査単位角度に対応する、すべての走査平面に対して、連続面の判断処理が終了したか否かを判定し、終了したと判定された場合(Yes)は、ステップＳ３２８に移行し、そうでない場合(No)は、ステップＳ３３２に移行する。
【０１４２】
ステップＳ３２８に移行した場合は、センシングプロセッサ２１０において、ステップＳ３２４においてＲＡＭ等の記録媒体に記録した連続面と判断された距離情報に基づいて面データを生成し、ステップＳ３３０に移行する。
ここで、面データの生成は、例えば、各走査平面間において、連続面と判断された勾配およびその測定点の座標情報とを比較し、座標値と勾配とが近いものを面と判断し、これらの座標情報を対応付けたり、公知の補完法を用いてつなぎ合わせたりすることで行う。
【０１４３】
例えば、勾配が０に近い測定点の連続する座標情報から構成される面データの面は、水平面と判断できるので、そこが歩行可能な面だと判断できる。
ステップＳ３３０では、３次元距離測定装置２００において、ハブ７６および通信Ｉ／Ｆ６４を介して、ステップＳ３２８で生成した面データをＣＰＵ６０に出力して処理を終了する。
【０１４４】
一方、ステップＳ３２６において、すべての走査平面に対して連続面の判断処理が終了しておらず、ステップＳ３３２に移行した場合は、３次元距離測定装置２００において、センシングプロセッサ２１０からの指令信号に応じて、ドライバ２１４、モータ２１６およびエンコーダ２１８を駆動し、２次元距離測定装置２１２を、ステップＳ３００で設定されたｚ軸回りの走査角度範囲内において、同じくステップＳ３００で設定された走査単位角度ずつｚ軸回りに回転させる処理（第２走査処理）を実行し、ステップＳ３０２に移行する。
【０１４５】
また、ステップＳ３１４において、出現頻度が閾値以上ではなく、ステップＳ３３４に移行した場合は、センシングプロセッサ２１０において、ステップＳ３１２で選択した勾配に対応する測定点の測定距離を測定ノイズとして除外し、ステップＳ３１８に移行する。
【０１４６】
次に、脚車輪型ロボット１００の移動制御システムを説明する。
ＣＰＵ６０は、センサ入力Ｉ／Ｆ６３を介して、３次元距離測定装置２００、前方脚先センサ２２、下方脚先センサ２４、障害物センサ３４、３６および３軸姿勢センサ７０からそれぞれセンサ信号を入力する。
次に、ＣＰＵ６０で実行される処理を説明する。
ＣＰＵ６０は、ステップＳ１００、Ｓ１０２に代えて、３次元距離測定装置２００を用いた特徴点検出処理を実行する。特徴点検出処理では、３次元距離測定装置２００から面データを入力し、そのセンサ座標系における各測定点の座標をグローバル座標系の座標に変換し、変換した測定点のうち面の周縁上の点（段部の境界点）を特徴点として検出する。
【０１４７】
次に、本実施の形態の動作を説明する。
図２５は、測定範囲内に階段がある場合の、第１走査処理による測定結果を示す図である。
まず、３次元距離測定装置２００は、ＣＰＵ６０からの指令信号に基づいて、センシングプロセッサ２１０において走査角度範囲および走査単位角度を設定する（ステップＳ３００）。
【０１４８】
ここで、２次元距離測定装置２１２は、測距範囲２０〜４０９５[mm]、最大走査角度範囲２４０°、角度分解能０．３６°の２次元レンジセンサであることとする。走査角度範囲が２４０°であれば、上述した図１８に示す走査平面を形成する走査角度範囲と同じとなり、２次元距離測定装置２１２の上方にある支持部材２１９、駆動プーリ２２０ａ、従動プーリ２２０ｂおよびベルト２２１が走査範囲に含まれないことになる。
【０１４９】
また、第１走査処理に対して、走査角度範囲２４０°および走査単位角度０．３６°が設定され、第２走査処理に対して、走査角度範囲−４０°〜＋４０°および走査単位角度１０°が設定されたとする（この場合は、走査平面が９つ形成される）。走査角度範囲−４０°〜＋４０°であれば、２次元距離測定装置２１２から水平方向に所定距離隔てて配置されたモータ２１６およびエンコーダ２１８が走査範囲に含まれないことになる。
【０１５０】
センシングプロセッサ２１０は、まず、第１走査処理に対して設定された走査角度範囲および走査単位角度に基づいて、ドライバ２１２ｂに指令信号を出力する。
２次元距離測定装置２１２は、ドライバ２１２ｂにおいて、センシングプロセッサ２１０からの指令信号およびエンコーダ２１２ｄからの出力信号に基づいて、モータ２１２ｃの回転軸を回転駆動し、測距センサ２１２ａをｙ軸回りに走査単位角度Δθ＝０．３６°ずつ回転するとともに、各回転角度に応じた距離を測定する第１走査処理を実行する（ステップＳ３０２）。最初は、ｚ軸回りに対して回転角度φが０°の位置（初期位置）に対する第１走査処理を実行する。また、各距離情報（回転座標系）は、データ列Ｌ（θi，φj）として、センシングプロセッサ２１０へと出力される。
【０１５１】
なお、ｙ軸回りの走査範囲内には、基体１０以外に、２次元距離測定装置２１２の駆動機構等の走査を阻害する物が一切存在しないため、走査範囲内に存在する物体の正確な距離情報を得ることができる。
そして、１つの走査平面に対する第１走査処理が終了すると、センシングプロセッサ２１０は、第１走査処理において測定された距離情報に対して、メディアンフィルタを用いたフィルタリング処理を行う（ステップＳ３０４）。これにより、測定情報におけるノイズ成分を除去する。
【０１５２】
ここで、ノイズ成分除去後の各測定点の測定距離Ｌ[mm]は、例えば、図２５に示すようになる。図２５において、横軸は各走査角度に応じた測定点の番号（第１走査角度番号）であり、縦軸は各走査角度番号の測定点に対する測定距離Ｌ[mm]である。
図２５に示す例では、脚車輪型ロボット１００の歩行経路上に、段差が一定でかつ蹴込板が連続面となっている階段が存在している。
【０１５３】
次に、センシングプロセッサ２１０は、フィルタリング処理後の回転座標系の距離情報を直交座標系の距離情報に変換する（ステップＳ３０６）。
図２６は、図２５の各測定点の回転座標系の測定距離を直交座標系の測定距離に変換した場合の測定結果を示す図である。
すなわち、図２５に示す各測定点の回転座標系の測定距離を直交座標系の測定距離へと変換することで、図２６に示すように、各回転角度に対応するｘ軸方向の距離[mm]とｚ軸方向の距離[mm]とで表現される２次元の座標情報となる。図２６において、横軸はｘ軸方向の距離Ｌx[mm]であり、縦軸はｚ軸方向の距離Ｌz[mm]である。
【０１５４】
次に、直交座標系に変換された距離情報に基づいて、各測定点の勾配を算出する（ステップＳ３０８）。
図２７は、最小二乗法を用いて算出したｘ方向およびｚ方向の勾配の一例を示す図である。
【０１５５】
ここでは、最小二乗法を用いて、各測定点の勾配を算出する。ここで、最小二乗法を用いた場合に、図２７（ａ）および（ｂ）に示すように、最小二乗法を適用する複数の測定点に対して、ｘ方向の勾配と、ｚ方向の勾配とが異なる場合がある。そこで、下式（12）および（13）に示すように、ｘ方向およびｚ方向の勾配を求める２つの直線の方程式を用いて最小二乗法計算を行う。
ｚ＝ａx・ｘ＋ｂx …（12）
ｘ＝ａz・ｚ＋ｂz …（13）
【０１５６】
そして、上式（12）におけるａxおよびｂxを下式（14）により算出し、上式（13）におけるａzおよびｂzを下式（15）により算出する。
【０１５７】
【数１２】
【０１５８】
ａxおよびｂx、並びにａzおよびｂzを算出すると、これらの算出結果を代入した、上式（12）および（13）の直線と、最小二乗計算に用いた各測定点との距離を残差ｈxおよびｈzとして算出するとともに、前記各測定点に対する残差ｈxの二乗和と、残差ｈzの二乗和とを算出する（下式（16））。
【０１５９】
【数１３】
【０１６０】
残差ｈxの二乗和および残差ｈzの二乗和が算出されると、両者を比較して値の小さい方の直線を、正しい直線として採用し、この直線の勾配に基づいて、算出対象の測定点の勾配を求める。具体的に、残差ｈxの二乗和の方が小さければ、「ａ＝ａx、ｂ＝ｂx」とし、残差ｈzの二乗和の方が小さければ、「ａ＝１／ａz、ｂ＝１／ｂz」とする。
【０１６１】
上記のようにして、走査平面に対する全測定点の勾配が算出されると、次に、各勾配の出現頻度を算出する（ステップＳ３１０）。
図２８は、ある走査平面に対する勾配の出現頻度の一例を示す図である。
ここでは、図２８に示すような、出現頻度が得られたとする。
図２８に示すように、勾配が０近傍の出現頻度が比較的高くなっており、これらの勾配に対応する測定点の座標情報が連続していれば、そこに水平面があると推定することができる。
【０１６２】
出現頻度が算出されると、センシングプロセッサ２１０は、次に、未判定の勾配の出現頻度を選択し（ステップＳ３１２）、その出現頻度が、あらかじめ設定された閾値以上か否かを判定する（ステップＳ３１４）。そして、閾値以上であれば（ステップＳ３１４の「Ｙｅｓ」の分岐）、勾配と測定点の座標情報とを対応付けてＲＡＭ（不図示）に記録する（ステップＳ３１６）。一方、閾値未満であれば（ステップＳ３１４の「Ｎｏ」の分岐）、測定ノイズとして、測定点の距離情報を除外する（ステップＳ３３４）。
【０１６３】
上記の判定処理が、すべての勾配に対して終了すると（ステップＳ３１８の「Ｙｅｓ」の分岐）、次に、ＲＡＭに記録された、出現頻度が閾値以上の勾配および測定点の座標情報に基づいて、連続面の有無を判断する（ステップＳ３２０）。
脚車輪型ロボット１００の移動経路上には、段差が一定でかつ蹴込板が連続面となっている階段が存在しているため、階段部分にある水平面等が、勾配０および略０の測定点の連続する座標として現われる。このような場合に、センシングプロセッサ２１０は、連続面（実際は、面と走査平面との交差線）がある（ステップＳ３２２の「Ｙｅｓ」の分岐）と判断する。一方、座標の途中で勾配がいきなり外れた値となったときは、そこで面（実際は、面と走査平面との交差線）が途切れていると判断する。これらの判断結果から、物体の凹凸を認識することが可能である。
【０１６４】
そして、連続面に対応する直交座標系の測定距離を、センサ座標系（３次元）の測定距離に変換し、ＲＡＭに記録する（ステップＳ３２４）。
連続面の判断が終了すると、３次元距離測定装置２００は、ドライバ２１４において、センシングプロセッサ２１０からの指令信号およびエンコーダ２１８からの出力信号に基づいて、モータ２１６の回転軸を回転駆動し、２次元距離測定装置２１２をｚ軸回りに回転角度１０°ずつ回転する第２走査処理を実行する（ステップＳ３３２）。第２走査処理によって、２次元距離測定装置２１２の向きが１つ前の状態に対してｚ軸回りに１０°だけ変化する。そして、２次元距離測定装置２１２は、この状態で、再び、第１走査処理を実行する（ステップＳ３０２）。これにより、ｚ軸回りに１０°ずれた位置に新たな走査平面が形成される。
【０１６５】
そして、設定された走査角度範囲（−４０°〜＋４０°）に対して、設定された走査単位角度１０°で、２次元距離測定装置２１２を回転させ、各回転角度φにおいて、第１走査処理を実行する。
ここで、２次元距離測定装置２１２が基体１０の正面に取り付けられているため、基体１０が走査範囲内に含まれてしまうが、脚車輪型ロボット１００の前方および歩行経路上を含む範囲においては阻害物が一切ないため、脚車輪型ロボット１００の歩行制御を行うのに十分な走査範囲が確保できているといえる。
【０１６６】
そして、９つの走査平面に対する距離の測定が終了すると（ステップＳ３２６の「Ｙｅｓ」の分岐）、次に、ＲＡＭに記録された連続面の情報に基づいて面データを生成する（ステップＳ３２８）。
面データは、上述したように、ｚ軸回りに連続する各隣り合う走査平面間において、勾配および座標の近い連続面をつなぎ合わせる（対応付ける）ことで生成される。生成した面データは、ハブ７６および通信Ｉ／Ｆ６４を介して、ＣＰＵ６０に出力される（ステップＳ３３０）。
【０１６７】
そして、ＣＰＵ６０により、入力された面データについてセンサ座標系の座標がグローバル座標系の座標に変換され、特徴点が検出される。
また、ＣＰＵ６０により、入力された面データが解析され、特に、勾配が０に近い交差線を有する面がある場合は、それを水平面とみなし、脚車輪型ロボット１００が歩行可能な面であると判断される。
【０１６８】
歩行可能な面であると判断された場合は、面データのグローバル座標系への変換結果と、３軸姿勢センサ７０のセンサ信号等とに基づいて脚先の着地位置が決定される。
なお、段差が一定でかつ蹴込板が連続面となる階段を例に挙げたが、段差が一定でない階段、蹴込板が連続面とならない階段、蹴込板の無い階段等に対しても、正確にその面の形状を認識することができるので、脚車輪型ロボット１００の階段への適応性を高めることが可能である。
【０１６９】
このようにして、本実施の形態では、測距センサ２１２ａをｙ軸回りに回転させるモータ２１２ｃ等からなるｙ軸回転機構と、測距センサ２１２ａをｚ軸回りに回転させるモータ２１６等からなるｚ軸回転機構とを備え、ｙ軸回転機構により測距センサ２１２ａを回転させながらｙ軸回転機構の走査単位角度ごとに測距センサ２１２ａの測定結果を取得する第１走査を、ｚ軸回転機構により測距センサ２１２ａを回転させながらｚ軸回転機構の走査単位角度ごとに行う第２走査を行うことにより、ｙ軸回転機構の走査単位角度ごとおよびｚ軸回転機構の走査単位角度ごとの測定結果を取得する。
【０１７０】
これにより、障害物の立体的な形状を把握することができ、これに基づいて検出された特徴点により干渉判定を行うことができるので、干渉判定の精度を向上することができる。また、測距センサ２１２ａを回転させる回転機構を採用したので、移動機構に比して、走査に必要なスペースが小さくてすみ、走査のための機構が簡素となり、しかも高速な走査を実現することができる。
【０１７１】
さらに、本実施の形態では、モータ２１６およびエンコーダ２１８と、２次元距離測定装置２１２とを水平方向に所定距離隔てて配置し、駆動回転軸の回転駆動力を、駆動プーリ２２０ａ、従動プーリ２２０ｂおよびベルト２２１を介して従動回転軸へと伝達し、２次元距離測定装置２１２をｚ軸回りに回転駆動する構成とした。
これにより、２次元距離測定装置２１２の走査角度範囲内には、走査を阻害するものが一切なくなるので、正確な距離情報を得ることができる。また、モータ２１６およびエンコーダ２１８と、２次元距離測定装置２１２とを水平方向に配置したので、３次元距離測定装置２００の高さ方向の占有率を低減することができる。
【０１７２】
さらに、本実施の形態では、測距センサ２１２ａをｙ軸回りに走査単位角度ずつ回転させるとともに、各回転角度における距離を測定する第１走査処理を実行し、距離情報に基づいて、各測定点の勾配および勾配の出現頻度を算出することで、走査平面内に存在する物体の凹凸を認識する。
これにより、連続面が存在することを推定することができる。
【０１７３】
さらに、本実施の形態では、１つの走査平面に対する第１走査処理が終了するごとに、２次元距離測定装置２１２をｚ軸回りに走査単位角度だけ回転させる第２走査処理を実行する。
これにより、複数の走査平面に対して第１走査処理を実行することができるので、測定範囲内に存在する物体上の面の形状を認識することができる。また、面の形状が認識できるので、脚車輪型ロボット１００は、未知の障害物に対して高い適応性を実現することができる。また、人が活動する環境での動作を行えるので、人と一緒に行動する用途に用いられるホームロボット、パーソナルロボット等のヒューマンアシストロボットに好適である。
【０１７４】
上記第２の実施の形態において、ｙ軸は、発明７の第１走査軸に対応し、ｚ軸は、発明７の第２走査軸に対応し、ｙ軸回転機構は、発明６若しくは７の第１走査手段、または発明７の第１回転手段に対応し、ｚ軸回転機構は、発明６若しくは７の第２走査手段、または発明７の第２回転手段に対応している。また、ステップＳ３０２は、発明６または７の測定結果取得手段に対応している。
【０１７５】
なお、上記第１および第２の実施の形態においては、脚すね部平面と特徴点の位置関係に基づいて干渉の有無を判定するように構成したが、これに限らず、脚すね部の形状に対応する線（直線、線分、多次曲線、円、楕円その他の曲線）と、特徴点との位置関係に基づいて干渉の有無を判定するように構成することもできる。また、脚すね部に対応する空間と、特徴点との位置関係に基づいて干渉の有無を判定するように構成することもできる。
【０１７６】
また、上記第１および第２の実施の形態においては、レーザ２８〜３０およびカメラ３２からなる物体センサまたは３次元距離測定装置２００を用いて特徴点を検出するように構成したが、これに限らず、他の方式の物体センサを用いて特徴点を検出するように構成することもできる。
また、上記第１および第２の実施の形態においては、物体センサを用いて特徴点を検出するように構成したが、これに限らず、物体センサを用いることなく、あらかじめ測定、計算若しくは定義した特徴点に関する特徴点情報、またはシミュレーション等の計算結果に基づいて特徴点を検出するように構成することもできる。
【０１７７】
また、上記第１および第２の実施の形態においては、干渉の有無を判定するように構成したが、これに限らず、干渉しないことのみを判定すること、干渉することのみを判定すること、干渉の度合いを判定すること、干渉までの度合い（例えば、特徴点と脚すね部の間の距離）を判定するように構成することもできる。
また、上記第２の実施の形態においては、エンコーダ２１８を駆動回転軸に設けて構成としたが、これに限らず、従動回転軸に設けて構成とすることもできる。
【０１７８】
これにより、伝達誤差の影響を受けずに第２走査処理における走査角度を高精度に検出することができる。
また、上記第２の実施の形態においては、駆動プーリ２２０ａ、従動プーリ２２０ｂおよびベルト２２１を介して、駆動回転軸の回転駆動力を従動回転軸へと伝達するように構成としたが、これに限らず、複数の歯車を介して、駆動回転軸の回転駆動力を従動回転軸へと伝達するように構成とすることもできる。
【０１７９】
また、上記第２の実施の形態においては、第１走査処理における距離情報の取得を離散的（第２走査処理で回転させてから第１走査処理で距離情報を取得）に行う構成としたが、これに限らず、走査角度と測定距離との対応付けを行うことで連続的（第２走査処理で回転させつつ、第１走査処理で距離情報を取得）に行う構成とすることもできる。
また、上記第２の実施の形態においては、ＣＰＵ６０からの指令信号に基づいて、走査角度範囲および走査単位角度を設定するように構成としたが、これに限らず、３次元距離測定装置２００にあらかじめ設定しておく構成とすることもできる。
【０１８０】
また、上記第２の実施の形態において、３次元距離測定装置２００は、測距センサ２１２ａをｙ軸回りに回転させ、２次元距離測定装置２１２をｚ軸回りに回転させる構成としたが、これに限らず、測距センサ２１２ａをｚ軸回りに回転させ、２次元距離測定装置２１２をｙ軸回りに回転させる構成とすることもできる。また、ｙ軸およびｚ軸回りに限らず、測距センサ２１２ａの測定方向に対して互いに直交する２つの軸であれば、どの方向の軸回りでもよい。さらに、このような回転機構に限らず、測距センサ２１２ａの測定方向とは異なる第１走査方向に測距センサ２１２ａを移動させ、測定方向および第１走査方向とは異なる第２走査方向に測距センサ２１２ａを移動させるように移動機構として構成することもできる。この場合、移動経路の形状としては、直線のほか円弧その他の曲線を採用することができる。回転機構と移動機構の組み合わせることもできる。
【０１８１】
また、上記第２の実施の形態においては、図２９（ａ）に示すように、２次元距離測定装置２１２自身を回転させているが、これに限らず、光学式の測距センサを有する２次元距離測定装置であれば、図２９（ｂ）に示すように、測定方向の光軸上に挿入したミラーを回転させてもよい。
【０１８２】
図２９は、測距センサの測定方向を変更する場合の構成を示す図である。
また、上記第１および第２の実施の形態においては、脚すね部を判定の対象部位として設定したが、これに限らず、脚車輪型ロボット１００の任意の部位を判定の対象部位として設定することができる。
また、上記第１および第２の実施の形態においては、本発明に係る干渉判定装置および脚車輪型ロボットを、階段を乗り越える場合について適用したが、これに限らず、階段以外の段差を乗り越える場合についても同様に適用することができる。
【０１８３】
また、上記第１および第２の実施の形態においては、本発明に係る干渉判定装置および脚車輪型ロボットを脚車輪型ロボット１００に適用したが、これに限らず、本発明の主旨を逸脱しない範囲で他の場合にも適用可能である。例えば、直動関節で脚構造を実現する脚車輪型ロボットや他の構成のロボット、車両その他の装置に適用することができる。
【図面の簡単な説明】
【０１８４】
【図１】脚車輪型ロボット１００の正面図である。
【図２】脚車輪型ロボット１００の側面図である。
【図３】障害物センサ３４、３６の構成を示す図である。
【図４】脚車輪型ロボット１００の移動制御システムを示すブロック図である。
【図５】昇降制御処理を示すフローチャートである。
【図６】光切断法の原理を説明するための図である。
【図７】階段にレーザ光を照射した状態およびカメラ３２の撮像素子の画像を示す図である。
【図８】階段の昇降時に段鼻部の特徴点を検出する場合を示す図である。
【図９】グローバル座標系において脚すね部の表面に対応する４つの平面を示す図である。
【図１０】段鼻部の特徴点Ａ、脚すね部平面、脚すね部平面上の任意の点Ｐおよび脚すね部平面の法線ベクトルＮを示す図である。
【図１１】グローバル座標系および脚車輪型ロボット１００の各座標系を示す図である。
【図１２】座標系Ｐ4を示す図である。
【図１３】脚すね部平面および法線ベクトルＮを示す図である。
【図１４】干渉判定処理を示すフローチャートである。
【図１５】脚車輪型ロボット１００を側面図である。
【図１６】３次元距離測定装置２００の外観構造を示す図である。
【図１７】２次元距離測定装置２１２の制御構造を示すブロック図である。
【図１８】測距センサ２１２ａの走査範囲を示す図である。
【図１９】３次元距離測定装置２００の制御構造を示すブロック図である。
【図２０】２次元距離測定装置２１２の距離測定の原理を説明する図である。
【図２１】第１走査処理および第２走査処理により走査を行った場合を示す図である。
【図２２】３次元距離測定装置２００の距離の計測例を示す図である。
【図２３】３次元距離測定装置２００における物体認識処理を示すフローチャートである。
【図２４】勾配の算出例を示す図である。
【図２５】測定範囲内に階段がある場合の、第１走査処理による測定結果を示す図である。
【図２６】図２５の各測定点の回転座標系の測定距離を直交座標系の測定距離に変換した場合の測定結果を示す図である。
【図２７】最小二乗法を用いて算出したｘ方向およびｚ方向の勾配の一例を示す図である。
【図２８】ある走査平面に対する勾配の出現頻度の一例を示す図である。
【図２９】測距センサの測定方向を変更する場合の構成を示す図である。
【符号の説明】
【０１８５】
１００ 脚車輪型ロボット
１０ 基体
１２ 脚部
１４〜１８ 回転関節
２０ 駆動輪
２２、２４ 脚先センサ
２６ 水平レーザ
２８、３０ 垂直レーザ
３２ カメラ
３４、３６ 障害物センサ
４０、５０ モータ
４２、５２ エンコーダ
４４、５４ ドライバ
７０ ３軸姿勢センサ
２００ ３次元距離測定装置
２１０ センシングプロセッサ
２１２ ２次元距離測定装置
２１２ａ 測距センサ
２１２ｃ、２１６ モータ
２１２ｄ、２１８ エンコーダ
２１２ｂ、２１４ ドライバ
２２０ａ 駆動プーリ
２２０ｂ 従動プーリ
２２１ ベルト
【技術分野】
【０００１】
本発明は、障害物との干渉判定を行う装置および段差を乗り越えて移動できる脚車輪型ロボットに係り、特に、実際の干渉を伴うことなく干渉判定を行うことができ、部品点数および重量の増加を抑制し、干渉判定の制約を低減するのに好適な干渉判定装置および脚車輪型ロボットに関する。
【背景技術】
【０００２】
ロボットの制御においては、ロボットに対して障害物が干渉（衝突）したか否かを判定し、その判定結果に基づいて障害物を回避する制御等の各種の制御を行っている。干渉を判定する技術としては、例えば、特許文献１、２記載の技術が知られている。
特許文献１記載の発明は、減速機を介してモータにより駆動されるロボットにおいて、モータが減速機に出力するトルクからロボットの逆動力学演算で求めた動力学トルクを差し引くことにより、衝突による外力をセンサレスで検出し、外力の検出値があらかじめ設定した所定の閾値より大きければアームが外力を受けたと判断する。
【０００３】
特許文献２記載の発明は、脚部の足平上にマークを付けておき、基体に取り付けられた視覚センサから取得された画像データに基づいて、歩行に対する障害物と脚部の位置関係を把握しながら歩行動作を行う。
【特許文献１】特開２００６−１１６６５０号公報
【特許文献２】特開２０００−１５３４７６号公報
【発明の開示】
【発明が解決しようとする課題】
【０００４】
しかしながら、特許文献１記載の発明にあっては、ロボットに対して障害物が実際に干渉しないと干渉したことが判定できないため、障害物を事前に回避する制御を行う上では実用的ではないという問題があった。
また、特許文献２記載の発明にあっては、干渉判定専用のカメラを設ける必要があるため、部品点数および重量が増加するという問題があった。
【０００５】
また、干渉を判定するには、カメラで撮影した画像に脚部と障害物の両方が含まれている必要があるため、干渉の判定に制約があるという問題もあった。例えば、脚部の可動範囲が大きいため下方を広範に撮影しようとすると、ロボットの前方にある障害物を撮影できず、前方の領域について干渉の判定が行えない。逆に、前方を広範に撮影しようとすると、脚部の可動範囲をすべてカバーすることができず、下方および後方の領域について干渉の判定が行えない。
【０００６】
ところで、ロボットの移動機構は、車輪型、クローラ型、脚型またはこれらを組み合わせた機構に分類される。一般に、車輪型ロボットは、平地での移動性は高いが、段差への適応性が低いという問題がある。また、クローラ型ロボットは、不整地に適し、多少の段差であれば乗り越えられるが、積極的な重心移動ができないために急な階段への適応性が低く、平地における移動性が車輪型ロボットより低いという問題があった。また、脚型ロボットは、階段への適応性が最も優れているが、平地での移動性が極端に低いという問題がある。
【０００７】
車輪型、クローラ型および脚型の問題を解決するため、脚型と車輪型を組み合わせた脚車輪型ロボットが提案されている。
脚車輪型ロボットが段差を乗り越える場合、重心を低くするために大きく開脚し、脚部の可動範囲について脚先と段差の干渉を判定しながら、段差を乗り越えるべく脚部を適切に駆動制御することが考えられる。このように、脚部の可動範囲および障害物の監視範囲が広範囲である場合に、特許文献２記載の発明を適用しようとすると、上記制約の問題がより顕著となる。
【０００８】
そこで、本発明は、このような従来の技術の有する未解決の課題に着目してなされたものであって、実際の干渉を伴うことなく干渉判定を行うことができ、部品点数および重量の増加を抑制し、干渉判定の制約を低減するのに好適な干渉判定装置および脚車輪型ロボットを提供することを目的としている。
【課題を解決するための手段】
【０００９】
〔発明１〕 上記目的を達成するために、発明１の干渉判定装置は、対象部位について障害物との干渉判定を行う干渉判定装置であって、障害物の特徴点を検出する特徴点検出手段と、前記特徴点および前記対象部位の座標を定める座標系において、前記対象部位またはこれと連動する部位の姿勢および前記対象部位の形状により特定される線、平面または空間であって、前記対象部位の形状に対応する線、前記対象部位の表面に対応する平面または前記対象部位に対応する空間と、前記特徴点検出手段で検出した特徴点との位置関係に基づいて、前記対象部位について障害物との干渉判定を行う干渉判定手段とを備える。
【００１０】
このような構成であれば、特徴点検出手段により、障害物の特徴点が検出される。そして、干渉判定手段により、特徴点および対象部位の座標を定める座標系において、対象部位の形状に対応する線、対象部位の表面に対応する平面または対象部位に対応する空間と、検出された特徴点との位置関係に基づいて、対象部位について障害物との干渉判定が行われる。この平面等は、対象部位またはこれと連動する部位の姿勢および対象部位の形状により特定される。
【００１１】
したがって、所定の座標系における平面等と特徴点の位置関係に基づいて干渉判定を行うので、実際の干渉を伴う必要はない。また、特許文献２における干渉判定専用のカメラのような専用のセンサを設けなくても、他の用途に用いられるセンサ等を利用することができる。さらに、所定の座標系において対象部位に対応する平面等は姿勢や形状により特定されるため、障害物の特徴点さえ検出できればよく、障害物の監視範囲が対象部位の可動範囲に制約されない。
【００１２】
ここで、特徴点および対象部位の座標を定める座標系としては、特徴点の絶対座標（例えば、接地面を基準とする座標）または相対座標（例えば、干渉判定装置を基準とする座標）を定める座標系、対象部位の絶対座標（例えば、接地面を基準とする座標）または相対座標（例えば、対象部位と連動する部位を基準とする座標）を定める座標系、その他任意の座標系が含まれる。なお、特徴点の座標を定める座標系と、対象部位の座標を定める座標系が異なる場合は、一方の座標系の座標を他方の座標系の座標に変換するか、両方の座標系の座標を、特徴点および対象部位の座標を定める他の座標系の座標に変換すればよい。
【００１３】
また、対象部位と連動する部位とは、対象部位と連なって動作する部位であって、自身の姿勢から対象部位の姿勢を間接的に特定できるものをいう。
また、干渉判定とは、対象部位に対する障害物の干渉に関する判定をいい、干渉しないことを判定すること、干渉することを判定すること、干渉するか否かを判定すること、干渉の度合いを判定すること、干渉までの度合い（例えば、特徴点と対象部位の間の距離）を判定することが含まれる。以下、発明２の脚車輪型ロボットにおいて同じである。
【００１４】
また、線には、直線、線分、多次曲線、円、楕円その他の曲線が含まれる。以下、発明２の脚車輪型ロボットにおいて同じである。
【００１５】
〔発明２〕 一方、上記目的を達成するために、発明２の脚車輪型ロボットは、基体と、前記基体に対して自由度を有して連結された脚部と、前記脚部に回転可能に設けられた車輪と、物体センサとを備え、前記脚部の駆動および前記車輪の回転により移動する脚車輪型ロボットであって、前記物体センサの測定結果に基づいて障害物の特徴点を検出する特徴点検出手段と、前記特徴点および前記対象部位の座標を定める座標系において、当該脚車輪型ロボットの姿勢および前記対象部位の形状により特定される線、平面または空間であって、前記対象部位の形状に対応する線、前記対象部位の表面に対応する平面または前記対象部位に対応する空間と、前記特徴点検出手段で検出した特徴点との位置関係に基づいて、前記対象部位について障害物との干渉判定を行う干渉判定手段とを備える。
【００１６】
このような構成であれば、段差等の障害物のあるところでは、物体センサにより、その測定方向について測定が行われ、特徴点検出手段により、物体センサの測定結果に基づいて障害物の特徴点が検出される。そして、干渉判定手段により、特徴点および対象部位の座標を定める座標系において、対象部位の形状に対応する線、対象部位の表面に対応する平面または対象部位に対応する空間と、検出された特徴点との位置関係に基づいて、対象部位について障害物との干渉判定が行われる。この平面等は、脚車輪型ロボットの姿勢および対象部位の形状により特定される。
【００１７】
したがって、所定の座標系における平面等と特徴点の位置関係に基づいて干渉判定を行うので、実際の干渉を伴う必要はない。また、特許文献２における干渉判定専用のカメラのような専用のセンサを設けなくても、他の用途に用いられる物体センサを利用することができる。さらに、所定の座標系において対象部位に対応する平面等は姿勢や形状により特定されるため、障害物の特徴点さえ検出できればよく、障害物の監視範囲が対象部位の可動範囲に制約されない。
【００１８】
また、脚車輪型ロボットにおいては、例えば、物体センサを基体に設け、対象部位に対する障害物の干渉判定を行うことが考えられる。この場合、基体に対して自由度の範囲で脚部が駆動することから、脚車輪型ロボットの姿勢により、対象部位に対する物体センサの位置が変化することとなる。また、物体センサを走査する場合は、対象部位に対する物体センサの向きが変化することとなる。
【００１９】
このように、対象部位に対する物体センサの位置や向きが変化する場合は、物体センサの測定結果に基づいて検出される特徴点の座標と、対象部位の座標は、物体センサおよび対象部位の動作の系が異なることからそれぞれ異なる座標系により定められる。しかしながら、座標系が異なる場合は、対象部位に対応する平面等と特徴点の位置関係を直ちには特定することができない。そこで、座標系の変換により平面等と特徴点の座標系を一致させて（すなわち、特徴点および対象部位の座標を定める座標系において）それらの位置関係を特定する必要があるところ、本発明は、脚車輪型ロボットの姿勢および対象部位の形状に基づいてこれを実現することができる。
【００２０】
したがって、対象部位との位置関係によらず物体センサを設置することができる。
さらに、段差のあるところでは、自由度の範囲で脚部が可動し、段差を乗り越えることができる。したがって、脚型ロボットと同様に段差への適応性が高い。
一方、平地では、車輪走行で移動することができる。したがって、車輪型ロボットと同様に平地での移動性が高い。
【００２１】
ここで、特徴点および対象部位の座標を定める座標系としては、特徴点の絶対座標（例えば、接地面を基準とする座標）または相対座標（例えば、脚車輪型ロボットを基準とする座標）を定める座標系、対象部位の絶対座標（例えば、接地面を基準とする座標）または相対座標（例えば、脚部を基準とする座標）を定める座標系、その他任意の座標系が含まれる。なお、特徴点の座標を定める座標系と、対象部位の座標を定める座標系が異なる場合は、一方の座標系の座標を他方の座標系の座標に変換するか、両方の座標系の座標を、特徴点および対象部位の座標を定める他の座標系の座標に変換すればよい。
また、脚車輪型ロボットの姿勢には、脚車輪型ロボットの全体の姿勢のほか、脚車輪型ロボットの一部分（例えば、脚部、物体センサ）の姿勢も含まれる。
【００２２】
〔発明３〕 さらに、発明３の脚車輪型ロボットは、発明２の脚車輪型ロボットにおいて、前記干渉判定手段は、前記座標系において、前記対象部位の表面に対応する複数の平面で囲まれる空間に前記特徴点が属するか否かを判定することにより、前記対象部位に対して障害物が干渉するか否かを判定する。
【００２３】
このような構成であれば、干渉判定手段により、座標系において、対象部位の表面に対応する複数の平面で囲まれる空間に特徴点が属するか否かが判定される。対象部位に対して障害物が干渉した場合は、同空間に特徴点が属し、障害物が干渉していない場合は、同空間に特徴点は属さない。したがって、この判定により、対象部位に対して障害物が干渉するか否かが判定される。
【００２４】
〔発明４〕 さらに、発明４の脚車輪型ロボットは、発明３の脚車輪型ロボットにおいて、前記干渉判定手段は、当該脚車輪型ロボットの姿勢および前記対象部位の形状に基づいて、前記対象部位の座標を定める第１座標系の座標を、前記特徴点の座標を定める第２座標系の座標に変換して前記第２座標系における前記複数の平面を特定し、前記第２座標系において、前記複数の平面で囲まれる空間に前記特徴点が属するか否かを判定する。
このような構成であれば、干渉判定手段により、脚車輪型ロボットの姿勢および対象部位の形状に基づいて、第１座標系から第２座標系に座標が変換されて第２座標系における複数の平面が特定され、第２座標系において、複数の平面で囲まれる空間に特徴点が属するか否かが判定される。
【００２５】
〔発明５〕 さらに、発明５の脚車輪型ロボットは、発明３および４のいずれか１の脚車輪型ロボットにおいて、前記干渉判定手段は、前記特徴点と前記平面上の任意の点を結ぶ直線と、前記平面の法線ベクトルとの内積に基づいて、前記複数の平面で囲まれる空間に前記特徴点が属するか否かを判定する。
このような構成であれば、干渉判定手段により、特徴点と平面上の任意の点を結ぶ直線と、平面の法線ベクトルとの内積に基づいて、複数の平面で囲まれる空間に特徴点が属するか否かが判定される。
【００２６】
〔発明６〕 さらに、発明６の脚車輪型ロボットは、発明２ないし５のいずれか１の脚車輪型ロボットにおいて、前記物体センサは、物体上の測定点までの距離を測定する測距センサであり、前記測距センサの測定方向とは異なる第１走査方向に前記測距センサを走査する第１走査手段と、前記測定方向および前記第１走査方向とは異なる第２走査方向に前記測距センサを走査する第２走査手段と、前記第１走査手段および前記第２走査手段の走査範囲で測定可能な前記測定点について前記測距センサの測定結果を取得する測定結果取得手段とを備え、前記特徴点検出手段は、前記測定結果取得手段で取得した測定結果に基づいて前記特徴点を検出する。
このような構成であれば、第１走査手段により、第１走査方向に測距センサが、第２走査手段により、第２走査方向に測距センサがそれぞれ走査可能となる。したがって、障害物の立体的な形状を把握することができる。そして、測定結果取得手段により、第１走査手段および第２走査手段の走査範囲で測定可能な測定点について測距センサの測定結果が取得され、特徴点検出手段により、取得された測定結果に基づいて特徴点が検出される。
【００２７】
ここで、第１走査手段および第２走査手段としては、例えば、次の構成を採用することができる。
（１）回転機構
前記測距センサの測定方向に対して所定角度をなす第１走査軸の回りに前記測距センサを回転させる第１回転手段と、前記測定方向および前記第１走査軸に対して所定角度をなす第２走査軸の回りに前記測距センサを回転させる第２回転手段とからなる構成である。
【００２８】
（２）移動機構
前記測距センサの測定方向とは異なる第１走査方向に前記測距センサを移動させる第１移動手段と、前記測定方向および前記第１走査方向とは異なる第２走査方向に前記測距センサを移動させる第２移動手段とからなる構成である。なお、第１移動手段は、前記第１走査方向に延長する経路を含む第１経路に沿って前記測距センサを移動させてもよいし、第２移動手段は、前記第２走査方向に延長する経路を含む第２経路に沿って前記測距センサを移動させてもよい。以下、（３）、（４）においても同様である。
【００２９】
（３）回転機構と移動機構の組み合わせ
前記測距センサの測定方向に対して所定角度をなす走査軸の回りに前記測距センサを回転させる回転手段と、前記走査軸の軸方向とは異なる走査方向に前記測距センサを移動させる移動手段とからなる構成である。
【００３０】
（４）移動機構と回転機構の組み合わせ
前記測距センサの測定方向とは異なる走査方向に前記測距センサを移動させる移動手段と、前記走査方向に対して所定角度をなす走査軸の回りに前記測距センサを回転させる回転手段とからなる構成である。
【００３１】
〔発明７〕 さらに、発明７の脚車輪型ロボットは、発明６の脚車輪型ロボットにおいて、前記第１走査手段は、前記測定方向に対して所定角度をなす第１走査軸の回りに前記測距センサを回転させる第１回転手段であり、前記第２走査手段は、前記測定方向および前記第１走査軸に対して所定角度をなす第２走査軸の回りに前記測距センサを回転させる第２回転手段であり、前記測定結果取得手段は、前記第１回転手段により前記測距センサを回転させながら前記第１回転手段の所定単位角度ごとに前記測距センサの測定結果を取得する第１走査を、前記第２回転手段により前記測距センサを回転させながら前記第２回転手段の所定単位角度ごとに行う第２走査を行うことにより、前記第１回転手段の所定単位角度ごとおよび前記第２回転手段の所定単位角度ごとの前記測定結果を取得する。
【００３２】
このような構成であれば、第１回転手段により、第１走査軸の回りに測距センサが、第２回転手段により、第２走査軸の回りに測距センサがそれぞれ回転可能となる。したがって、障害物の立体的な形状を把握することができる。そして、測定結果取得手段により、第２走査が行われることにより第１回転手段の所定単位角度ごとおよび第２回転手段の所定単位角度ごとの測定結果が取得される。第２走査では、第２回転手段により測距センサを回転させながら第２回転手段の所定単位角度ごとに第１走査が行われる。第１走査では、第１回転手段により測距センサを回転させながら第１回転手段の所定単位角度ごとに測定結果が取得される。
【発明の効果】
【００３３】
以上説明したように、発明１の干渉判定装置によれば、所定の座標系における平面等と特徴点の位置関係に基づいて干渉判定を行うので、実際の干渉を伴うことなく干渉判定を行うことができるという効果が得られる。また、干渉判定専用のカメラのような専用のセンサを設けなくても、他の用途に用いられるセンサ等を利用することができるので、部品点数および重量の増加を抑制することができるという効果も得られる。さらに、所定の座標系において対象部位に対応する平面等は姿勢や形状により特定されるため、障害物の特徴点さえ検出できればよく、障害物の監視範囲が対象部位の可動範囲に制約されず、干渉判定の制約を低減することができるという効果も得られる。
【００３４】
一方、発明２の脚車輪型ロボットによれば、所定の座標系における平面等と特徴点の位置関係に基づいて干渉判定を行うので、実際の干渉を伴うことなく干渉判定を行うことができるという効果が得られる。また、干渉判定専用のカメラのような専用のセンサを設けなくても、他の用途に用いられる物体センサを利用することができるので、部品点数および重量の増加を抑制することができるという効果も得られる。さらに、所定の座標系において対象部位に対応する平面等は姿勢や形状により特定されるため、障害物の特徴点さえ検出できればよく、障害物の監視範囲が対象部位の可動範囲に制約されず、干渉判定の制約を低減することができるという効果も得られる。さらに、対象部位との位置関係によらず物体センサを設置することができるので、干渉判定の制約をさらに低減することができるという効果も得られる。
【００３５】
さらに、発明３の脚車輪型ロボットによれば、対象部位に対して障害物が干渉するか否かを比較的正確に判定することができるという効果が得られる。
さらに、発明５の脚車輪型ロボットによれば、複数の平面で囲まれる空間に特徴点が属するか否かを比較的容易に判定することができるという効果が得られる。
さらに、発明６の脚車輪型ロボットによれば、障害物の立体的な形状を把握することができ、これに基づいて検出された特徴点により干渉判定を行うことができるので、干渉判定の精度を向上することができるという効果が得られる。
【００３６】
さらに、発明７の脚車輪型ロボットによれば、測距センサを回転させる回転機構を採用したので、移動機構に比して、走査に必要なスペースが小さくてすみ、走査のための機構が簡素となり、しかも高速な走査を実現することができるという効果が得られる。
【発明を実施するための最良の形態】
【００３７】
以下、本発明の第１の実施の形態を図面を参照しながら説明する。図１ないし図１４は、本発明に係る干渉判定装置および脚車輪型ロボットの第１の実施の形態を示す図である。
【００３８】
まず、本発明を適用する脚車輪型ロボット１００の構成を説明する。
図１は、脚車輪型ロボット１００の正面図である。
図２は、脚車輪型ロボット１００の側面図である。
脚車輪型ロボット１００は、図１および図２に示すように、基体１０と、基体１０に連結された４つの脚部１２とを有して構成されている。
基体１０の前部には、２本の脚部１２が回転関節１４を介して左右対称の位置に連結されている。また、基体１０の後部には、２本の脚部１２が回転関節１４を介して左右対称の位置に連結されている。回転関節１４は、脚車輪型ロボット１００の底面と直交する方向を軸方向として回転する。すなわち、ヨー軸回りに回転する。
【００３９】
各脚部１２には、２つの回転関節１６、１８が設けられている。回転関節１４は、下方を軸方向として回転し、回転関節１６、１８は、回転関節１４が図１の状態であるときは、脚車輪型ロボット１００の側面と直交する方向を軸方向として回転する。すなわち、回転関節１４が図１の状態であるときは、ピッチ軸回りに回転し、回転関節１４が図１の状態から９０度回転した状態であるときは、ロール軸回りに回転する。したがって、脚部１２は、それぞれ３自由度を有する。
【００４０】
各脚部１２の先端には、回転関節１６、１８と軸方向を同一にして駆動輪２０が回転可能に設けられている。
各脚部１２の先端には、脚車輪型ロボット１００の移動経路上に存在する物体までの距離を測定する前方脚先センサ２２と、接地面までの距離を測定する下方脚先センサ２４とが設けられている。
【００４１】
一方、基体１０の正面の上部中央には、水平面レーザ光を照射する水平レーザ２６が設けられている。また、基体１０の正面の中央左右には、垂直面レーザ光を照射する垂直レーザ２８、３０がそれぞれ設けられている。
基体１０の正面の下部中央には、水平面レーザ光および垂直面レーザ光の反射光を含む画像を撮影するカメラ３２が設けられている。
【００４２】
水平レーザ２６は、カメラ３２で水平面レーザ光の反射光を含む画像が撮影できるように下方に所定角度傾けて設けられている。同様に、垂直レーザ２８は、カメラ３２で垂直面レーザ光の反射光を含む画像が撮影できるように右方に所定角度傾けて設けられ、垂直レーザ３０は、左方に所定角度傾けて設けられている。
カメラ３２の左右には、障害物を検出する障害物センサ３４、３６がそれぞれ設けられている。
【００４３】
図３は、障害物センサ３４、３６の構成を示す図である。
障害物センサ３４、３６は、図３（ａ）に示すように、指向性の低い超音波測距センサを複数アレイ状に配列して構成することができる。また、図３（ｂ）に示すように、指向性の高い赤外線測距センサを複数アレイ状に配列して構成することもできる。アレイ状に配列する構成に限らず、単体で構成してもよい。また、超音波測距センサまたは赤外線測距センサを複数平面上に配列したエリアセンサで構成してもよい。これにより、脚車輪型ロボット１００の移動経路上に存在する物体を大まかに検出することができる。
【００４４】
次に、脚車輪型ロボット１００の移動制御システムを説明する。
図４は、脚車輪型ロボット１００の移動制御システムを示すブロック図である。
各脚部１２の回転関節１４〜１８には、図４に示すように、回転関節１４〜１８を回転駆動する関節モータ４０がそれぞれ設けられている。各関節モータ４０には、関節モータ４０の回転角度位置を検出するエンコーダ４２と、モータ指令信号およびエンコーダ４２の出力信号に基づいて関節モータ４０の駆動を制御するドライバ４４が設けられている。
【００４５】
各脚部１２の駆動輪２０には、駆動輪２０を回転駆動する車輪モータ５０がそれぞれ設けられている。各車輪モータ５０には、車輪モータ５０の回転角度位置を検出するエンコーダ５２と、モータ指令信号およびエンコーダ５２の出力信号に基づいて車輪モータ５０の駆動を制御するドライバ５４が設けられている。
脚車輪型ロボット１００は、さらに、ＣＰＵ６０と、脚車輪型ロボット１００の姿勢を検出する３軸姿勢センサ７０と、カメラ３２の画像信号を処理するビジョンプロセッサ７２と、外部のＰＣ等と無線通信を行う無線通信部７４と、ビジョンプロセッサ７２および無線通信部７４とＣＰＵ６０の入出力を中継するハブ７６と、警告音等を出力するスピーカ７８とを有して構成されている。
【００４６】
３軸姿勢センサ７０は、ジャイロ若しくは加速度センサ、またはその両方を有し、地軸に対して脚車輪型ロボット１００の姿勢の傾きを検出する。
ＣＰＵ６０は、モータ指令出力Ｉ／Ｆ６１を介してドライバ４４、５４にモータ指令信号を出力し、角度取込Ｉ／Ｆ６２を介してエンコーダ４２、５２の出力信号を入力する。また、センサ入力Ｉ／Ｆ６３を介して、前方脚先センサ２２、下方脚先センサ２４、障害物センサ３４、３６および３軸姿勢センサ７０からそれぞれセンサ信号を入力する。また、通信Ｉ／Ｆ６４を介してハブ７６と信号の入出力を行い、サウンド出力Ｉ／Ｆ６５を介してスピーカ７８に音声信号を出力する。
【００４７】
次に、ＣＰＵ６０で実行される処理を説明する。
ＣＰＵ６０は、ＲＯＭ等の所定領域に格納されている制御プログラムを起動させ、その制御プログラムに従って、図５のフローチャートに示す昇降制御処理を実行する。
次に、昇降制御処理を説明する。
図５は、昇降制御処理を示すフローチャートである。
【００４８】
昇降制御処理は、脚部１２の昇降制御を行う処理であって、ＣＰＵ６０において実行されると、まず、図５に示すように、ステップＳ１００に移行する。
ステップＳ１００では、ビジョンプロセッサ７２から画像を取り込み、ステップＳ１０２に移行する。
ステップＳ１０２では、取り込んだ画像に基づいて光切断法により階段の特徴点を検出する。
【００４９】
図６は、光切断法の原理を説明するための図である。
光切断法は、三角測量の原理により計測対象上の座標を求める計測法である。図６に計測座標系を示す。
計測対象上の座標Ｐ（x0、y0、z0）は、カメラ３２の撮像素子上の任意の座標をＰs（xi、yi、zi）とすると、下式（１）により求めることができる。
【００５０】
【数１】
【００５１】
次に、得られた３次元座標から、レーザ光の反射光の不連続点または屈曲点を階段の特徴点として検出する。
図７は、階段にレーザ光を照射した状態およびカメラ３２の撮像素子の画像を示す図である。
【００５２】
脚車輪型ロボット１００の移動経路上に階段が存在すると、図７（ａ）左側に示すように、水平レーザ２６から照射された水平面レーザ光が階段の蹴込板および床面で反射し、カメラ３２により、その反射光を含む階段の画像が撮影される。その画像に対して画像処理を行うと、図７（ａ）右側に示すように、蹴込板での反射光エッジおよび床面での反射光エッジを検出することができる。そして、そのエッジ画像および上式（１）により得られた３次元座標に基づいて、反射光エッジの不連続点に対応する実座標を算出することができる。
【００５３】
また、図７（ｂ）左側に示すように、垂直レーザ２８から照射された垂直面レーザ光が階段の蹴込板および踏板で反射し、カメラ３２により、その反射光を含む階段の画像が撮影される。その画像に対して画像処理を行うと、図７（ｂ）右側に示すように、蹴込板での反射光エッジおよび踏板での反射光エッジを検出することができる。また、垂直レーザ３０についても同様であり、図７（ｃ）右側に示すように、蹴込板での反射光エッジおよび踏板での反射光エッジを検出することができる。そして、それらエッジ画像および上式（１）により得られた３次元座標に基づいて、反射光エッジの屈曲点に対する実座標を算出することができる。
【００５４】
図８は、階段の昇降時に段鼻部の特徴点を検出する場合を示す図である。
脚車輪型ロボット１００が階段を上る場合は、図８（ａ）に示すように、階段を上る前に、反射光エッジの屈曲点として段鼻部の特徴点を検出することができる。また、階段を下る場合は、図８（ｂ）に示すように、階段を下る前に、反射光エッジの屈曲点として段鼻部の特徴点を検出することができる。
【００５５】
図５に戻り、次いで、ステップＳ１０３に移行して、障害物との干渉の有無を判定する干渉判定処理を実行し、ステップＳ１０４に移行して、干渉判定処理の判定結果に基づいて障害物と干渉するか否かを判定し、干渉しないと判定したとき(No)は、ステップＳ１０５に移行する。
ステップＳ１０５では、検出した特徴点に基づいて階段の幅を算出し、ステップＳ１０６に移行して、検出した特徴点に基づいて階段の段鼻部の実座標を算出し、ステップＳ１０８に移行する。
【００５６】
ステップＳ１０８では、算出した階段の幅および段鼻部の実座標、並びに３軸姿勢センサ７０のセンサ信号に基づいて逆運動学計算および重心計算を行い、ステップＳ１１０に移行して、ステップＳ１０８の計算結果に基づいて脚先（駆動輪２０）の着地位置を決定し、ステップＳ１１２に移行する。
ステップＳ１１２では、前方脚先センサ２２および下方脚先センサ２４からそれぞれセンサ信号を入力し、ステップＳ１１４に移行して、入力した前方脚先センサ２２のセンサ信号に基づいて蹴込板までの距離を算出し、ステップＳ１１６に移行して、入力した下方脚先センサ２４のセンサ信号に基づいて脚先と踏板の位置関係を算出し、ステップＳ１１８に移行する。
【００５７】
ステップＳ１１８では、決定した着地位置および算出した両距離に基づいてドライバ４４、５４へのモータ指令信号を生成し、ステップＳ１２０に移行して、生成したモータ指令信号をドライバ４４、５４に出力し、ステップＳ１２２に移行する。
ステップＳ１２２では、脚先が踏板に着地したか否かを判定し、脚先が着地したと判定したとき(Yes)は、一連の処理を終了して元の処理に復帰させる。
【００５８】
一方、ステップＳ１２２で、脚先が着地しないと判定したとき(No)は、ステップＳ１１２に移行する。
一方、ステップＳ１０４で、障害物と干渉すると判定したとき(Yes)は、ステップＳ１２４に移行して、干渉を回避する干渉回避処理を実行し、一連の処理を終了して元の処理に復帰させる。
【００５９】
次に、ステップＳ１０３の干渉判定処理を説明する。
脚車輪型ロボット１００が階段を昇降する場合は、脚部１２のうち回転関節１８および駆動輪２０が連結するリンク（以下、脚すね部という。）と、階段の段鼻部とが干渉し得る。したがって、干渉判定処理では、脚すね部に対して段鼻部が干渉するか否かを判定する。この判定は、特定の脚部１２（例えば、前脚）について行ってもよいし、すべての脚部１２について行ってもよい。
【００６０】
図９は、グローバル座標系において脚すね部の表面に対応する４つの平面を示す図である。同図（ａ）は、脚すね部の斜視図であり、同図（ｂ）は、脚すね部の厚さ方向の断面図であり、同図（ｃ）は、脚すね部の伸長方向の断面図である。
段鼻部の特徴点の座標は、特徴点を検出するための物体センサ（本実施の形態では、レーザ２６〜３０およびカメラ３２）からみた脚車輪型ロボット１００の外部を基準とするグローバル座標系で定められる。そこで、干渉の判定は、図９（ａ）に示すように、グローバル座標系において、矩形からなる脚すね部の正面、背面、右側面および左側面の各表面ごとにその表面に対応する平面（以下、脚すね部平面という。）を特定し、図９（ｂ）に示すように、特定した４つの脚すね部平面で囲まれる空間（すなわち、脚すね部の内部領域に対応する空間）に特徴点が属するか否かを判定することにより行う。同空間に特徴点が属すると判定したときは、脚すね部に対して段鼻部が干渉すると判定し、同空間に特徴点が属さないと判定したときは、脚すね部に対して段鼻部が干渉しないと判定する。
【００６１】
図１０は、段鼻部の特徴点Ａ、脚すね部平面、脚すね部平面上の任意の点Ｐおよび脚すね部平面の法線ベクトルＮを示す図である。
段鼻部の特徴点をＡ、脚すね平面上の任意の点をＰ、脚すね部平面の法線ベクトルをＮとすると、図１０に示すように、特徴点Ａと点Ｐを結ぶ直線ＰＡと、法線ベクトルＮとの内積Ｄは、下式（２）により求めることができる。
【００６２】
【数２】
【００６３】
Ｄ＞０のときは、脚すね平面の表側（脚すね部の外側）に特徴点が位置し、Ｄ＝０のときは、脚すね平面上に特徴点が位置し、Ｄ＜０のときは、脚すね平面の裏側（脚すね部の内側）に特徴点が位置する。
【００６４】
脚すね部の正面、背面、右側面および左側面に対応する脚すね平面についての内積ＤをそれぞれＤ1、Ｄ2、Ｄ3、Ｄ4とすると、Ｄ1≧０またはＤ2≧０またはＤ3≧０またはＤ4≧０が成立する場合は、４つの脚すね部平面で囲まれる空間に特徴点Ａが属さないので、脚すね部に対して段鼻部が干渉しないと判定する。
なお、４つの脚すね部平面で判定する場合は、脚すね部の底面よりも下方の領域も干渉判定の対象領域としてしまうため、脚すね部が特徴点Ａを跨ぐような場合も干渉すると判定してしまう。そこで、より好ましくは、脚すね部の底面に対応する脚すね平面も考慮することである。脚すね部の底面に対応する脚すね平面についての内積ＤをＤ5とすると、Ｄ1≧０またはＤ2≧０またはＤ3≧０またはＤ4≧０またはＤ5≧０が成立する場合は、５つの脚すね部平面で囲まれる空間に特徴点Ａが属さないので、脚すね部に対して段鼻部が干渉しないと判定することができる。
【００６５】
同様に、４つの脚すね部平面で判定する場合は、脚すね部の上面よりも上方の領域も干渉判定の対象領域としてしまうため、脚すね部が特徴点Ａを潜るような場合も干渉すると判定してしまう。そこで、より好ましくは、脚すね部の上面に対応する脚すね平面も考慮することである。脚すね部の上面に対応する脚すね平面についての内積ＤをＤ6とすると、Ｄ1≧０またはＤ2≧０またはＤ3≧０またはＤ4≧０またはＤ6≧０が成立する場合は、５つの脚すね部平面で囲まれる空間に特徴点Ａが属さないので、脚すね部に対して段鼻部が干渉しないと判定することができる。
【００６６】
さらに好ましくは、脚すね部の底面および上面に対応する脚すね平面も考慮することである。Ｄ1≧０またはＤ2≧０またはＤ3≧０またはＤ4≧０またはＤ5≧０またはＤ6≧０が成立する場合は、６つの脚すね部平面で囲まれる空間に特徴点Ａが属さないので、脚すね部に対して段鼻部が干渉しないと判定することができる。
次に、グローバル座標系において点Ｐの座標を求める場合を説明する。
【００６７】
特徴点Ａの座標は、グローバル座標系で定められるのに対し、脚すね部の座標は、脚すね部を基準とする相対座標系で定められる。したがって、グローバル座標系における点Ｐの座標は、脚すね部の座標を定める座標系の座標をグローバル座標系の座標に変換して求める必要がある。
図１１は、グローバル座標系および脚車輪型ロボット１００の各座標系を示す図である。
【００６８】
図１１において、Ｐgは、グローバル座標系、Ｐ0は、基体１０の底面中心を原点とする座標系、Ｐ1は、基体１０と右後ろの脚部１２との付け根を原点とする座標系、Ｐ2は、回転関節１４を原点とする座標系、Ｐ3は、回転関節１６を原点とする座標系、Ｐ4は、回転関節１８を原点とする座標系である。ここで、座標系Ｐ4が脚すね部の座標を定める座標系となる。
【００６９】
グローバル座標系Ｐgおよび座標系Ｐ0〜Ｐ4は、３軸が互いに直交する直交座標系として定義される。
グローバル座標系Ｐgは、特徴点Ａを検出するための物体センサからみた座標系であって、その原点は、座標系Ｐ0の原点から所定距離（Ａ、Ｂ、Ｃ）離れたところに設定されている。
【００７０】
座標系Ｐ0は、基体１０の奥行き（前後の長さ）方向をｘ0軸、基体１０の幅（左右の長さ）方向をｙ0軸、基体１０の高さ方向をｚ0軸とし、ｘ0軸は基体１０の後方を、ｙ0軸は基体１０の右方を、ｚ0軸は基体１０の上方をそれぞれ正の方向とする。原点は、基体１０の底面中心となるので、基体１０の奥行きをＬ、基体１０の幅をＷとすると、（Ｌ／２、Ｗ／２、０）となる。
【００７１】
グローバル座標系Ｐgと座標系Ｐ0の関係は、下式（３）の行列で表すことができる。したがって、下式（３）により、グローバル座標系Ｐgから座標系Ｐ0への座標変換を行うことができる。
【００７２】
【数３】
【００７３】
座標系Ｐ1は、ｘ0軸、ｙ0軸、ｚ0軸と同一の方向をｘ1軸、ｙ2軸、ｚ3軸とし、ｘ1軸は基体１０の後方を、ｙ1軸は基体１０の右方を、ｚ1軸は基体１０の下方をそれぞれ正の方向とする。原点は、基体１０と右後ろの脚部１２との付け根となるので、（０、０、０）となる。
【００７４】
座標系Ｐ0、Ｐ1の関係は、下式（４）の行列で表すことができる。したがって、下式（４）により、座標系Ｐ0から座標系Ｐ1への座標変換を行うことができる。
【００７５】
【数４】
【００７６】
座標系Ｐ2は、座標系Ｐ1に対して回転関節１４がθ1回転した座標系であって、ｚ1軸と同一の方向をｚ2軸、ｘ1軸に対してｚ2軸回りにθ1回転した方向をｘ2軸、ｙ1軸に対してｚ2軸回りにθ1回転した方向をｙ2軸とし、ｚ2軸は基体１０の下方を正の方向とする。
【００７７】
座標系Ｐ1、Ｐ2の関係は、下式（５）の行列で表すことができる。したがって、下式（５）により、座標系Ｐ1から座標系Ｐ2への座標変換を行うことができる。
【００７８】
【数５】
【００７９】
座標系Ｐ3は、座標系Ｐ2に対して回転関節１６がθ2回転した座標系であって、ｙ2軸と同一の方向をｙ3軸、ｘ2軸に対してｙ3軸回りにθ2回転した方向をｘ3軸、ｚ2軸に対してｙ3軸回りにθ2回転した方向をｚ3軸とし、ｚ3軸は、脚部１２のうち回転関節１４、１６が連結するリンクの下方を正の方向とする。
【００８０】
座標系Ｐ2、Ｐ3の関係は、下式（６）の行列で表すことができる。したがって、下式（６）により、座標系Ｐ2から座標系Ｐ3への座標変換を行うことができる。
【００８１】
【数６】
【００８２】
座標系Ｐ4は、座標系Ｐ3に対して回転関節１８がθ3回転した座標系であって、ｙ3軸と同一の方向をｙ4軸、ｘ3軸に対してｙ4軸回りにθ3回転した方向をｘ4軸、ｚ3軸に対してｙ4軸回りにθ3回転した方向をｚ4軸とし、ｚ4軸は、脚すね部の下方を正の方向とする。
【００８３】
座標系Ｐ3、Ｐ4の関係は、下式（７）の行列で表すことができる。したがって、下式（７）により、座標系Ｐ3から座標系Ｐ4への座標変換を行うことができる。
【００８４】
【数７】
【００８５】
図１２は、座標系Ｐ4を示す図である。
座標系Ｐ4における点Ｐの座標は、図１２に示すように、脚すね部の奥行きをｌ、幅をｍ、伸長方向の長さをｎとすると、（ｌ／２、０、０）となる。
【００８６】
したがって、グローバル座標系Ｐgにおける点Ｐの座標は、下式（８）により求めることができる。下式（８）のなかで、θ1〜θ3は、脚車輪型ロボット１００の姿勢により、ｌは、脚すね部の形状により、Ｌ、Ｗは、基体１０の形状によりそれぞれ特定されるので、グローバル座標系Ｐgにおける点Ｐの座標は、脚車輪型ロボット１００の姿勢並びに基体１０および脚すね部の形状により特定することができる。
【００８７】
【数８】
【００８８】
上式（８）において、4Ｘpは、下式（９）により表すことができ、4Ｘpのうち（ｌ／２、０、０）は、座標系Ｐ4における点Ｐの座標である。また、Ｘpのうち（ｘ、ｙ、ｚ）は、グローバル座標系Ｐgにおける点Ｐの座標である。
【００８９】
【数９】
【００９０】
次に、脚すね部平面の法線ベクトルＮを求める場合を説明する。
図１３は、脚すね部平面および法線ベクトルＮを示す図である。
法線ベクトルＮは、図１３に示すように、グローバル座標系Ｐgにおいて、脚すね部平面の４つの頂点（脚すね部の表面のうち右上、右下、左上および左下の各角部に対応する点）のうち３つをそれぞれＶ1、Ｖ2、Ｖ3とすると、点Ｖ3から点Ｖ1へのベクトルと、点Ｖ2から点Ｖ3のベクトルの外積で表すことができるので、下式（10）により求めることができる。
【００９１】
【数１０】
【００９２】
なお、グローバル座標系Ｐgにおける点Ｖ1の座標は、点Ｐの場合と同様に、下式（11）により求めることができる。グローバル座標系Ｐgにおける点Ｖ2、Ｖ3の座標も同様に求めることができる。
【００９３】
【数１１】
【００９４】
図１４は、干渉判定処理を示すフローチャートである。
干渉判定処理は、ステップＳ１０３において実行されると、まず、図１４に示すように、ステップＳ２００に移行する。
【００９５】
ステップＳ２００では、回転関節１４〜１８の角度θ1〜θ3、基体１０の寸法Ｌ、Ｗおよび脚すね部の寸法ｌに基づいて、上式（８）により、グローバル座標系Ｐgにおける点Ｐの座標を算出し、ステップＳ２０２に移行する。
ステップＳ２０２では、回転関節１４〜１８の角度θ1〜θ3、基体１０の寸法Ｌ、Ｗおよび脚すね部の寸法ｌに基づいて、上式（11）により、各脚すね部平面ごとにグローバル座標系Ｐgにおける点Ｖ1〜Ｖ3の座標を算出し、ステップＳ２０４に移行する。
【００９６】
ステップＳ２０４では、上式（10）により、各脚すね部平面ごとに法線ベクトルＮを算出し、ステップＳ２０６に移行して、特徴点Ａの座標を取得し、ステップＳ２０８に移行して、上式（２）により、特徴点Ａと点Ｐを結ぶ直線ＰＡと、法線ベクトルＮとの内積Ｄ1〜Ｄ4（またはＤ5、Ｄ6）を算出し、ステップＳ２１０に移行する。
ステップＳ２１０では、Ｄ1≧０またはＤ2≧０またはＤ3≧０またはＤ4≧０（またはＤ5≧０、Ｄ6≧０）が成立するか否かを判定することにより４つの脚すね部平面で囲まれる空間に特徴点Ａが属するか否かを判定し、同空間に特徴点Ａが属さないと判定したとき(No)は、ステップＳ２１２に移行する。
【００９７】
ステップＳ２１２では、未判定の特徴点Ａが存在するか否かを判定し、未判定の特徴点Ａが存在しないと判定したとき(No)は、ステップＳ２１４に移行して、障害物と干渉しないことを示す値（例えば、「０」）を戻り値として設定し、一連の処理を終了して元の処理に復帰させる。
一方、ステップＳ２１２で、未判定の特徴点Ａが存在すると判定したとき(Yes)は、ステップＳ２１６に移行して、次の特徴点Ａの座標を取得し、ステップＳ２０８に移行する。
一方、ステップＳ２１０で、４つの脚すね部平面で囲まれる空間に特徴点Ａが属すると判定したとき(Yes)は、ステップＳ２１８に移行して、障害物と干渉することを示す値（例えば、「１」）を戻り値として設定し、一連の処理を終了して元の処理に復帰させる。
【００９８】
次に、本実施の形態の動作を説明する。
脚車輪型ロボット１００の移動経路上に階段が存在すると、水平レーザ２６から照射された水平面レーザ光、および垂直レーザ２８、３０から照射された垂直面レーザ光がそれぞれ階段で反射し、カメラ３２により、それら反射光を含む画像が撮影される。次いで、ステップＳ１００、Ｓ１０２を経て、カメラ３２で撮影された画像が取り込まれ、取り込まれた画像から階段の特徴点が検出される。
【００９９】
次いで、ステップＳ１０３を経て、干渉判定処理が実行される。干渉判定処理では、ステップＳ２００〜Ｓ２１０を経て、各脚すね部平面ごとに法線ベクトルＮが算出され、特徴点Ａと点Ｐを結ぶ直線ＰＡと、法線ベクトルＮとの内積Ｄ1〜Ｄ4（またはＤ5、Ｄ6）が算出され、内積Ｄ1〜Ｄ4（またはＤ5、Ｄ6）に基づいて４つの脚すね部平面で囲まれる空間に特徴点Ａが属するか否かが判定される。いずれの特徴点Ａも同空間に属しないと判定されると、ステップＳ２１４、Ｓ１０４を経て、障害物と干渉しないと判定される。非干渉が判定されると、ステップＳ１０５〜Ｓ１１０を経て、検出された特徴点に基づいて階段の幅および段鼻部の実座標が算出され、算出された階段の幅および段鼻部の実座標に基づいて脚先の着地位置が決定される。
【０１００】
さらに、ステップＳ１１２〜Ｓ１１６を経て、脚先センサ２２、２４からそれぞれセンサ信号が入力され、蹴込板までの距離および脚先と踏板の位置関係が算出される。そして、ステップＳ１１８、Ｓ１２０を経て、決定された着地位置および算出された両距離に基づいてモータ指令信号が生成され、生成されたモータ指令信号がドライバ４４、５４に出力される。これにより、駆動輪２０が回転するとともに回転関節１４〜１８が駆動し、脚車輪型ロボット１００が姿勢を適切に保ちつつ階段を乗り越える。また、状況によっては階段を回避、停止する。したがって、脚型ロボットと同様に階段への適応性が高い。
【０１０１】
一方、いずれかの特徴点Ａが同空間に属すると判定されると、ステップＳ２１８、Ｓ１０４を経て、障害物と干渉すると判定される。干渉が判定されると、ステップＳ１２４を経て、干渉回避処理が実行され、干渉が回避される。
一方、平地では、脚車輪型ロボット１００は、車輪走行で移動することができる。したがって、車輪型ロボットと同様に平地での移動性が高い。
【０１０２】
このようにして、本実施の形態では、レーザ２８〜３０およびカメラ３２からなる物体センサを備え、物体センサの測定結果に基づいて障害物の特徴点を検出し、グローバル座標系Ｐgにおいて、脚車輪型ロボット１００の姿勢並びに基体１０および脚すね部の形状により特定される脚すね部平面と、検出した特徴点との位置関係に基づいて、脚すね部に対して障害物が干渉するか否かを判定する。
【０１０３】
これにより、グローバル座標系Ｐgにおける脚すね部平面と特徴点の位置関係に基づいて干渉の有無を判定するので、実際の干渉を伴うことなく干渉の有無を判定することができる。また、特許文献２における干渉判定専用のカメラのような専用のセンサを設けなくても、本実施の形態のように、他の用途（脚部１２の駆動制御等）に用いられる物体センサを利用することができるので、部品点数および重量の増加を抑制することができる。さらに、グローバル座標系Ｐgにおいて脚すね部平面は姿勢や形状により特定されるため、障害物の特徴点さえ検出できればよく、障害物の監視範囲が脚すね部の可動範囲に制約されず、干渉判定の制約を低減することができる。さらに、脚すね部との位置関係によらず物体センサを設置することができるので、干渉判定の制約をさらに低減することができる。
【０１０４】
さらに、本実施の形態では、グローバル座標系Ｐgにおいて、４つの脚すね部平面で囲まれる空間に特徴点が属するか否かを判定することにより、脚すね部に対して障害物が干渉するか否かを判定する。
これにより、脚すね部に対して障害物が干渉するか否かを比較的正確に判定することができる。
【０１０５】
さらに、本実施の形態では、特徴点と脚すね部平面上の任意の点を結ぶ直線と、脚すね部平面の法線ベクトルとの内積に基づいて、４つの脚すね部平面で囲まれる空間に特徴点が属するか否かを判定する。
これにより、４つの脚すね部平面で囲まれる空間に特徴点が属するか否かを比較的容易に判定することができる。
【０１０６】
さらに、本実施の形態では、レーザ２６〜３０およびカメラ３２からなる画像センサと、脚先センサ２２、２４とを備え、カメラ３２で撮影した画像および脚先センサ２２、２４で測定した距離に基づいて階段を認識し、その認識結果に基づいてモータ４０、５０を制御する。
これにより、画像センサおよび脚先センサ２２、２４を用いて未知の階段を認識しながら脚部１２の昇降制御を行うので、従来に比して、未知の階段に対して高い適応性を実現することができる。また、人が活動する環境での動作を行えるので、人と一緒に行動する用途に用いられるホームロボット、パーソナルロボット等に好適である。
【０１０７】
さらに、本実施の形態では、画像センサを基体１０の正面に設け、脚先センサ２２、２４を脚部１２の先端に設けた。
これにより、脚車輪型ロボット１００の移動経路上に存在する物体を広い視野で検出することができるとともに、階段昇降時に駆動輪２０と階段の距離を精度よく測定することができる。
【０１０８】
さらに、本実施の形態では、進行方向に対して所定角度にレーザ光を照射するレーザ２６〜３０と、レーザ２６〜３０から所定距離隔てて設けられかつレーザ光の反射光を含む画像を撮像するカメラ３２とを備え、カメラ３２で撮影した画像に基づいて光切断法により階段を認識する。
これにより、階段の特徴のうち脚部１２の昇降制御に有効な特徴を検出することができるので、未知の階段に対してさらに高い適応性を実現することができる。
【０１０９】
さらに、本実施の形態では、水平レーザ２６から照射された水平面レーザ光の反射光の撮影状態に基づいて階段の幅を算出し、垂直レーザ２８、３０から照射された２つの垂直面レーザ光の反射光の撮影状態に基づいて階段の段鼻部の実座標を算出する。
これにより、階段の特徴のうち脚部１２の昇降制御にさらに有効な特徴を検出することができるので、未知の階段に対してさらに高い適応性を実現することができる。
【０１１０】
さらに、本実施の形態では、前方脚先センサ２２の測定結果に基づいて階段の蹴込板までの距離を算出し、下方脚先センサ２４の測定結果に基づいて駆動輪２０と階段の踏板の位置関係を算出する。
これにより、階段の特徴のうち脚部１２の昇降制御にさらに有効な特徴を検出することができるので、未知の階段に対してさらに高い適応性を実現することができる。
【０１１１】
上記第１の実施の形態において、脚すね部は、発明１ないし４の対象部位に対応し、ステップＳ１０２は、発明１または２の特徴点検出手段に対応し、ステップＳ１０３は、発明１ないし５の干渉判定手段に対応し、座標系Ｐ4は、発明４の第１座標系に対応し、グローバル座標系Ｐgは、発明４の第２座標系に対応している。
次に、本発明の第２の実施の形態を図面を参照しながら説明する。図１５ないし図２８は、本発明に係る干渉判定装置および脚車輪型ロボットの第２の実施の形態を示す図である。
【０１１２】
本実施の形態は、上記第１の実施の形態に対して、レーザ２８〜３０およびカメラ３２からなる物体センサに代えて、測距センサを２方向にそれぞれ走査して測定を行う３次元距離測定装置を用いて特徴点を検出する点が異なる。
まず、脚車輪型ロボット１００の構成を説明する。
図１５は、脚車輪型ロボット１００を側面図である。
【０１１３】
基体１０の正面には、図１５に示すように、３次元距離測定装置２００が取り付けられている。３次元距離測定装置２００の座標系は、基体１０の奥行き方向をｘ軸、基体１０の幅方向をｙ軸、基体１０の高さ方向をｚ軸とし、ｘ軸は基体１０の前方を、ｙ軸は基体１０の右方を、ｚ軸は基体１０の上方をそれぞれ正の方向とする。
次に、３次元距離測定装置２００の外観構造を説明する。
【０１１４】
図１６は、３次元距離測定装置２００の外観構造を示す図である。同図（ａ）は、３次元距離測定装置２００の正面図（ｚ−ｙ平面）を示す図であり、同図（ｂ）は、３次元距離測定装置２００の上面図（ｘ−ｙ平面）である。
３次元距離測定装置２００は、図１６（ａ）に示すように、板状の支持部材２１９と、２次元距離測定装置２１２と、２次元距離測定装置２１２を回転駆動するモータ２１６と、モータ２１６の回転角度位置を検出するエンコーダ２１８と、モータ２１６の駆動力を２次元距離測定装置２１２に伝達する駆動プーリ２２０ａ、従動プーリ２２０ｂおよびベルト２２１とを有して構成されている。
【０１１５】
支持部材２１９の下面には、モータ２１６が取り付けられている。モータ２１６の回転軸（以下、駆動回転軸という。）は、支持部材２１９を下方から貫通し、支持部材２１９を挟んで反対側に配置された駆動プーリ２２０ａの軸穴に連結している。
一方、支持部材２１９の下面であってモータ２１６から水平方向に所定距離隔てた位置には、２次元距離測定装置２１２が取り付けられている。２次元距離測定装置２１２は、回転軸（以下、従動回転軸という。）を有し、従動回転軸が支持部材２１９を下方から貫通し、支持部材２１９を挟んで反対側に配置された従動プーリ２２０ｂの軸穴に連結している。
【０１１６】
駆動プーリ２２０ａおよび従動プーリ２２０ｂには、ベルト２２１が巻き掛けられている。したがって、モータ２１６により駆動プーリ２２０ａが回転し、駆動プーリ２２０ａに巻き掛けたベルト２２１により従動プーリ２２０ｂが回転することにより、２次元距離測定装置２１２は、図１６（ｂ）に示すように、ｚ軸回りに回転する。
次に、２次元距離測定装置２１２の制御構造を説明する。
【０１１７】
図１７は、２次元距離測定装置２１２の制御構造を示すブロック図である。
２次元距離測定装置２１２は、図１７に示すように、測定範囲内に存在する物体上の測定点までの距離を測定する測距センサ２１２ａと、測距センサ２１２ａを回転駆動するモータ２１２ｃと、モータ２１２ｃの回転角度位置を検出するエンコーダ２１２ｄと、指令信号およびエンコーダ２１２ｄの出力信号に基づいてモータ２１２ｃの駆動を制御するドライバ２１２ｂとを有して構成されている。
【０１１８】
ドライバ２１２ｂは、センシングプロセッサ２１０からの指令信号において設定された走査角度範囲（例えば、−４０°〜＋４０°等の所定の角度範囲）および走査単位角度（例えば、０．３６°等の所定の単位角度）に基づいて、モータ２１２ｃの回転軸を走査単位角度ずつ回転させる制御を行う。
モータ２１２ｃは、測距センサ２１２ａのレーザ出力部（不図示）および受光部（不図示）をｙ軸回りに回転駆動するように設けられており、ドライバ２１２ｂからの制御信号に応じて、自己の回転軸を走査単位角度（Δθ）ずつ回転駆動する。
【０１１９】
図１８は、測距センサ２１２ａの走査範囲を示す図である。
測距センサ２１２ａの走査範囲は、脚車輪型ロボット１００が階段の昇降や障害物の回避を行うことを目的としているため、図１８に示すように、脚車輪型ロボット１００の下方を重点的に走査するように設定されている。なお、３次元距離測定装置２００および測距センサ２１２ａの原点位置（回転角度θおよびφが０°の位置）においては、測距センサ２１２ａの測定方向がｘ軸と一致し、測距センサ２１２ａの回転軸がｙ軸と一致する。
【０１２０】
３次元距離測定装置２００は、２次元距離測定装置２１２を回転駆動する回転駆動機構（モータ２１６、エンコーダ２１８、駆動プーリ２２０ａ、従動プーリ２２０ｂ、ベルト２２１および支持部材２１９）が、図１８に示す走査範囲外に設けられているため、図１８に示す走査範囲であれば、３次元距離測定装置２００を構成する各機構部によって、測距センサ２１２ａのｙ軸回りの走査が阻害されない。
【０１２１】
また、脚車輪型ロボット１００の走行経路上の障害物を認識できればよいので、ｚ軸回りの回転駆動による走査範囲も、前方１８０°までをカバーする必要はなく、２次元距離測定装置２１２から水平方向に所定距離隔てて配置されたモータ２１６およびエンコーダ２１８を走査範囲外とする範囲でも十分である。したがって、２次元距離測定装置２１２の回転駆動範囲を、モータ２１６およびエンコーダ２１８を含まない範囲とする。
【０１２２】
次に、３次元距離測定装置２００の制御構造を説明する。
図１９は、３次元距離測定装置２００の制御構造を示すブロック図である。
３次元距離測定装置２００は、図１９に示すように、センシングプロセッサ２１０と、２次元距離測定装置２１２と、モータ２１６と、エンコーダ２１８と、指令信号およびエンコーダ２１８の出力信号に基づいてモータ２１６の駆動を制御するドライバ２１４とを有して構成されている。
【０１２３】
センシングプロセッサ２１０は、専用のプログラムを実行し、ドライバ２１２ｂに指令信号を与えて測距センサ２１２ａを回転させ、測距センサ２１２ａの走査範囲で測定可能な領域（以下、走査平面という。）内に存在する物体上の測定点までの距離を測定する第１走査処理を実行するとともに、１つの走査平面に対する第１走査処理が終了するごとに、ドライバ２１４に指令信号を与えて２次元距離測定装置２１２を回転させる第２走査処理を実行する。
【０１２４】
さらに、センシングプロセッサ２１０は、第１走査処理および第２走査処理を経て２次元距離測定装置２１２で測定した距離の情報（以下、距離情報という。）に基づいて、各走査平面の各測定点の勾配の出現頻度を算出する処理を実行するとともに、算出した出現頻度および各測定点の座標情報に基づいて、各走査平面における物体の１次元の形状（凹凸）および測定範囲内に存在する物体上の面の形状を認識する処理を実行する。
【０１２５】
次に、３次元距離測定装置２００の距離測定の原理を説明する。
図２０は、２次元距離測定装置２１２の距離測定の原理を説明する図である。
２次元距離測定装置２１２は、測距センサ２１２ａが、モータ２１２ｃの回転軸の回転駆動に応じて、ｙ軸回りに走査単位角度ずつ回転し、かつ、回転するごとに、図２０に示すように、レーザ出力部からレーザ光を出力するとともに、出力光に対する物体（図２０中の障害物）からの反射光を受光部で受光し、各回転角度（走査角度）に応じた距離（図２０中の測定距離Ｌ（物体と受光部との間の距離））を測定する。
【０１２６】
図２１は、第１走査処理および第２走査処理により走査を行った場合を示す図である。同図（ａ）は、測距センサ２１２ａをｙ軸回りに回転させたときの測定距離と回転角度θとの関係を示す図であり、同図（ｂ）は、２次元距離測定装置２１２をｚ軸回りに回転させたときの走査平面と回転角度φとの関係を示す図である。
第１走査処理は、例えば、図２１（ａ）に示すように、測距センサ２１２ａをｙ軸回りに走査単位角度ずつ回転させながら、原点位置に対する各回転角度（図２１（ａ）中のθ1、θ2、θ3）に応じた距離情報（図２１（ａ）中のＬ（θ1）、Ｌ（θ2）、Ｌ（θ3））を測定する処理となる。
【０１２７】
また、第１走査処理における、モータ２１２ｃの回転軸の回転中心と、レーザの走査軌道線の両端とを結んで形成される平面が、走査平面（物体が存在しない場合は扇形の平面）となる。
ドライバ２１４は、センシングプロセッサ２１０からの指令信号において設定された走査角度範囲および走査単位角度（Δφ）に基づいて、モータ２１６の回転軸を走査単位角度ずつ回転させる制御を行う。
【０１２８】
モータ２１６は、減速機（不図示）、駆動プーリ２２０ａ、従動プーリ２２０ｂおよびベルト２２１を介して、２次元距離測定装置２１２をｚ軸回りに回転駆動するように設けられており、ドライバ２１４からの制御信号に応じて、自己の回転軸を走査単位角度ずつ回転駆動する。これにより、モータ２１６の回転軸の回転駆動に応じて、駆動プーリ２２０ａ、従動プーリ２２０ｂおよびベルト２２１を介して従動回転軸に回転駆動力が伝達され、２次元距離測定装置２１２がｚ軸回りに走査単位角度ずつ回転する。
【０１２９】
すなわち、第２走査処理は、図２１（ｂ）に示すように、２次元距離測定装置２１２をｚ軸回りに走査単位角度ずつ回転させる処理となる。そして、第１走査処理と第２走査処理とを交互に連続して行うことにより、第１走査処理によって形成される走査平面をｚ軸回りに連続して形成する。
図２２は、３次元距離測定装置２００の距離の計測例を示す図である。
【０１３０】
これにより、例えば、図２２に示すように、壁、ついたて、スタンド、棚等の物体の立体的な形状を測定することが可能である。
また、図２１（ｂ）に示すように、第２走査処理後の各測定点の距離情報をＬ（θi，φj）と表記する。ここで、ｉは、ｙ軸回りの走査角度に応じて各測定点に付与される通し番号であり、ｊは、ｚ軸回りの走査角度に応じて各測定点に付与される通し番号である。
【０１３１】
次に、３次元距離測定装置２００における物体認識処理の流れを説明する。
図２３は、３次元距離測定装置２００における物体認識処理を示すフローチャートである。
物体認識処理は、ＣＰＵ６０からの指令信号に基づいて、センシングプロセッサ２１０が、ＲＯＭ（不図示）に記憶された専用のプログラムを読み出し、読み出したプログラムを実行することで実現される処理であり、処理が開始されると、図２３に示すように、まず、ステップＳ３００に移行する。
【０１３２】
ステップＳ３００では、３次元距離測定装置２００において、ＣＰＵ６０からの指令信号に基づいて、測距センサ２１２ａおよび２次元距離測定装置２１２の走査角度範囲および走査単位角度を設定し、ステップＳ３０２に移行する。ここで、ＣＰＵ６０からの指令信号には、走査角度範囲および走査単位角度の情報が含まれている。
ステップＳ３０２では、２次元距離測定装置２１２において、センシングプロセッサ２１０からの指令信号に応じて、ドライバ２１２ｂ、モータ２１２ｃおよびエンコーダ２１２ｄを駆動し、測距センサ２１２ａを、ステップＳ３００で設定されたｙ軸回りの走査角度範囲内において、同じくステップＳ３００で設定された走査単位角度ずつｙ軸回りに回転させるとともに、各回転角度に応じた距離情報を測定する処理（第１走査処理）を実行し、ステップＳ３０４に移行する。
【０１３３】
ステップＳ３０４では、センシングプロセッサ２１０において、ステップＳ３０２で測定した距離情報に対して、メディアンフィルタを用いたフィルタリング処理を実行してノイズ成分を除去し、ステップＳ３０６に移行する。
ステップＳ３０６では、センシングプロセッサ２１０において、ステップＳ３０４でノイズ除去後の回転座標系の距離情報を直交座標系の距離情報に変換し、ステップＳ３０８に移行する。
【０１３４】
回転座標系から直交座標系への変換は、第１走査処理による各測定点の座標は、ｚ−ｘ平面上の座標となり、ｘ軸は、第２走査処理によって、ｚ軸回りに走査単位角度ずつ座標位置が変化する。したがって、直交座標系への変換は、ｚ軸回りの各回転角度ごとに行われる。
ステップＳ３０８では、センシングプロセッサ２１０において、ステップＳ３０６で直交座標系に変換された距離情報に基づいて、各測定点の勾配を算出してステップＳ３１０に移行する。
【０１３５】
図２４は、勾配の算出例を示す図である。
各測定点の勾配は、各測定点の測定距離と、その前後の所定数の測定点の距離情報とを用いて、図２４（ａ）に示すように、最小二乗法を用いて算出する。
図２４（ａ）に示すように、最小二乗法を用いて、勾配の算出対象の測定点（図２４（ａ）中の白丸）を含む前後所定数の測定点に対する回帰直線を近似し、回帰直線の勾配を、対象の測定点の勾配とする。最小二乗法は、データのばらつきによって受ける影響は少ないが、計算量が比較的多くなる。
【０１３６】
また、別の方法として、図２４（ｂ）に示すように、各測定点に対して、所定数の他の測定点を挟む前後２つの測定点の距離情報の差分値を用いて勾配を算出する。
具体的に、図２４（ｂ）に示すように、前後２つの測定点のｚ軸方向の測定距離の差分値である第１差分値（例えば、注目する測定点のｚ軸方向の距離をｚiとすると、ｚi+2−ｚi-2）を算出するとともに、前後２つの測定点のｘ軸方向の測定距離の差分値である第２差分値（例えば、注目する測定点のｘ軸方向の距離をｘiとすると、ｘi+2−ｘi-2）を算出し、これら算出結果から、注目する測定点の勾配（第１差分値／第２差分値）を求める。この方法では、データのばらつきによる影響が最小二乗法よりも大きくなるが、計算が、単純な引き算と割り算のみで済むため最小二乗法よりも高速に勾配を算出することができる。
【０１３７】
ステップＳ３１０では、センシングプロセッサ２１０において、ステップＳ３０８で算出した勾配に基づいて各勾配の出現頻度を算出し、ステップＳ３１２に移行する。
本実施の形態では、勾配ごとの測定点の個数を出現頻度として算出する。
ステップＳ３１２では、センシングプロセッサ２１０において、ステップＳ３１０で算出した各勾配の出現頻度に基づいて、出現頻度の判定処理が未処理の勾配の出現頻度を選択し、ステップＳ３１４に移行する。
【０１３８】
ステップＳ３１４では、センシングプロセッサ２１０において、ステップＳ３１２で選択した勾配の出現頻度とあらかじめ設定された閾値とを比較するとともに選択された出現頻度は、閾値以上か否かを判定し、閾値以上と判定された場合(Yes)は、ステップＳ３１６に移行し、そうでない場合(No)は、ステップＳ３３４に移行する。
ステップＳ３１６に移行した場合は、センシングプロセッサ２１０において、選択した勾配とこの勾配を有する各測定点の座標情報とを対応付けて不図示のＲＡＭ等の記録媒体に記録し、ステップＳ３１８に移行する。
【０１３９】
ステップＳ３１８では、センシングプロセッサ２１０において、すべての勾配についてステップＳ３１４の判定処理が終了したか否かを判定し、終了したと判定された場合(Yes)は、ステップＳ３２０に移行し、そうでない場合(No)は、ステップＳ３１２に移行する。
ステップＳ３２０に移行した場合は、センシングプロセッサ２１０において、ステップＳ３１６で記録した勾配と各測定点の座標情報とに基づいて、連続面（実際は、連続面と走査平面とが交差する線）の有無を判断し、ステップＳ３２２に移行する。
【０１４０】
ここで、連続面の判断は、例えば、所定の勾配範囲に含まれる勾配に対応する各測定点の座標情報に基づいて、測定点の座標が連続しているときに、これらの座標位置と交差する連続面があると判断する。所定の勾配範囲に含まれる勾配に対応する測定点の座標が連続していないときは、連続面がないと判断する。
ステップＳ３２２では、センシングプロセッサ２１０において、ステップＳ３２０の判断結果に基づいて連続面があるか否かを判定し、連続面があると判定された場合(Yes)は、ステップＳ３２４に移行し、そうでない場合(No)は、ステップＳ３２６に移行する。
【０１４１】
ステップＳ３２４に移行した場合は、センシングプロセッサ２１０において、連続面に対応する各測定点の測定距離の座標系をセンサ座標系（３次元座標）に変換し、変換後のセンサ座標系の距離情報を勾配の情報と対応付けて、不図示のＲＡＭ等の記録媒体に記録し、ステップＳ３２６に移行する。
ステップＳ３２６では、センシングプロセッサ２１０において、第２走査処理の走査角度範囲および走査単位角度に対応する、すべての走査平面に対して、連続面の判断処理が終了したか否かを判定し、終了したと判定された場合(Yes)は、ステップＳ３２８に移行し、そうでない場合(No)は、ステップＳ３３２に移行する。
【０１４２】
ステップＳ３２８に移行した場合は、センシングプロセッサ２１０において、ステップＳ３２４においてＲＡＭ等の記録媒体に記録した連続面と判断された距離情報に基づいて面データを生成し、ステップＳ３３０に移行する。
ここで、面データの生成は、例えば、各走査平面間において、連続面と判断された勾配およびその測定点の座標情報とを比較し、座標値と勾配とが近いものを面と判断し、これらの座標情報を対応付けたり、公知の補完法を用いてつなぎ合わせたりすることで行う。
【０１４３】
例えば、勾配が０に近い測定点の連続する座標情報から構成される面データの面は、水平面と判断できるので、そこが歩行可能な面だと判断できる。
ステップＳ３３０では、３次元距離測定装置２００において、ハブ７６および通信Ｉ／Ｆ６４を介して、ステップＳ３２８で生成した面データをＣＰＵ６０に出力して処理を終了する。
【０１４４】
一方、ステップＳ３２６において、すべての走査平面に対して連続面の判断処理が終了しておらず、ステップＳ３３２に移行した場合は、３次元距離測定装置２００において、センシングプロセッサ２１０からの指令信号に応じて、ドライバ２１４、モータ２１６およびエンコーダ２１８を駆動し、２次元距離測定装置２１２を、ステップＳ３００で設定されたｚ軸回りの走査角度範囲内において、同じくステップＳ３００で設定された走査単位角度ずつｚ軸回りに回転させる処理（第２走査処理）を実行し、ステップＳ３０２に移行する。
【０１４５】
また、ステップＳ３１４において、出現頻度が閾値以上ではなく、ステップＳ３３４に移行した場合は、センシングプロセッサ２１０において、ステップＳ３１２で選択した勾配に対応する測定点の測定距離を測定ノイズとして除外し、ステップＳ３１８に移行する。
【０１４６】
次に、脚車輪型ロボット１００の移動制御システムを説明する。
ＣＰＵ６０は、センサ入力Ｉ／Ｆ６３を介して、３次元距離測定装置２００、前方脚先センサ２２、下方脚先センサ２４、障害物センサ３４、３６および３軸姿勢センサ７０からそれぞれセンサ信号を入力する。
次に、ＣＰＵ６０で実行される処理を説明する。
ＣＰＵ６０は、ステップＳ１００、Ｓ１０２に代えて、３次元距離測定装置２００を用いた特徴点検出処理を実行する。特徴点検出処理では、３次元距離測定装置２００から面データを入力し、そのセンサ座標系における各測定点の座標をグローバル座標系の座標に変換し、変換した測定点のうち面の周縁上の点（段部の境界点）を特徴点として検出する。
【０１４７】
次に、本実施の形態の動作を説明する。
図２５は、測定範囲内に階段がある場合の、第１走査処理による測定結果を示す図である。
まず、３次元距離測定装置２００は、ＣＰＵ６０からの指令信号に基づいて、センシングプロセッサ２１０において走査角度範囲および走査単位角度を設定する（ステップＳ３００）。
【０１４８】
ここで、２次元距離測定装置２１２は、測距範囲２０〜４０９５[mm]、最大走査角度範囲２４０°、角度分解能０．３６°の２次元レンジセンサであることとする。走査角度範囲が２４０°であれば、上述した図１８に示す走査平面を形成する走査角度範囲と同じとなり、２次元距離測定装置２１２の上方にある支持部材２１９、駆動プーリ２２０ａ、従動プーリ２２０ｂおよびベルト２２１が走査範囲に含まれないことになる。
【０１４９】
また、第１走査処理に対して、走査角度範囲２４０°および走査単位角度０．３６°が設定され、第２走査処理に対して、走査角度範囲−４０°〜＋４０°および走査単位角度１０°が設定されたとする（この場合は、走査平面が９つ形成される）。走査角度範囲−４０°〜＋４０°であれば、２次元距離測定装置２１２から水平方向に所定距離隔てて配置されたモータ２１６およびエンコーダ２１８が走査範囲に含まれないことになる。
【０１５０】
センシングプロセッサ２１０は、まず、第１走査処理に対して設定された走査角度範囲および走査単位角度に基づいて、ドライバ２１２ｂに指令信号を出力する。
２次元距離測定装置２１２は、ドライバ２１２ｂにおいて、センシングプロセッサ２１０からの指令信号およびエンコーダ２１２ｄからの出力信号に基づいて、モータ２１２ｃの回転軸を回転駆動し、測距センサ２１２ａをｙ軸回りに走査単位角度Δθ＝０．３６°ずつ回転するとともに、各回転角度に応じた距離を測定する第１走査処理を実行する（ステップＳ３０２）。最初は、ｚ軸回りに対して回転角度φが０°の位置（初期位置）に対する第１走査処理を実行する。また、各距離情報（回転座標系）は、データ列Ｌ（θi，φj）として、センシングプロセッサ２１０へと出力される。
【０１５１】
なお、ｙ軸回りの走査範囲内には、基体１０以外に、２次元距離測定装置２１２の駆動機構等の走査を阻害する物が一切存在しないため、走査範囲内に存在する物体の正確な距離情報を得ることができる。
そして、１つの走査平面に対する第１走査処理が終了すると、センシングプロセッサ２１０は、第１走査処理において測定された距離情報に対して、メディアンフィルタを用いたフィルタリング処理を行う（ステップＳ３０４）。これにより、測定情報におけるノイズ成分を除去する。
【０１５２】
ここで、ノイズ成分除去後の各測定点の測定距離Ｌ[mm]は、例えば、図２５に示すようになる。図２５において、横軸は各走査角度に応じた測定点の番号（第１走査角度番号）であり、縦軸は各走査角度番号の測定点に対する測定距離Ｌ[mm]である。
図２５に示す例では、脚車輪型ロボット１００の歩行経路上に、段差が一定でかつ蹴込板が連続面となっている階段が存在している。
【０１５３】
次に、センシングプロセッサ２１０は、フィルタリング処理後の回転座標系の距離情報を直交座標系の距離情報に変換する（ステップＳ３０６）。
図２６は、図２５の各測定点の回転座標系の測定距離を直交座標系の測定距離に変換した場合の測定結果を示す図である。
すなわち、図２５に示す各測定点の回転座標系の測定距離を直交座標系の測定距離へと変換することで、図２６に示すように、各回転角度に対応するｘ軸方向の距離[mm]とｚ軸方向の距離[mm]とで表現される２次元の座標情報となる。図２６において、横軸はｘ軸方向の距離Ｌx[mm]であり、縦軸はｚ軸方向の距離Ｌz[mm]である。
【０１５４】
次に、直交座標系に変換された距離情報に基づいて、各測定点の勾配を算出する（ステップＳ３０８）。
図２７は、最小二乗法を用いて算出したｘ方向およびｚ方向の勾配の一例を示す図である。
【０１５５】
ここでは、最小二乗法を用いて、各測定点の勾配を算出する。ここで、最小二乗法を用いた場合に、図２７（ａ）および（ｂ）に示すように、最小二乗法を適用する複数の測定点に対して、ｘ方向の勾配と、ｚ方向の勾配とが異なる場合がある。そこで、下式（12）および（13）に示すように、ｘ方向およびｚ方向の勾配を求める２つの直線の方程式を用いて最小二乗法計算を行う。
ｚ＝ａx・ｘ＋ｂx …（12）
ｘ＝ａz・ｚ＋ｂz …（13）
【０１５６】
そして、上式（12）におけるａxおよびｂxを下式（14）により算出し、上式（13）におけるａzおよびｂzを下式（15）により算出する。
【０１５７】
【数１２】
【０１５８】
ａxおよびｂx、並びにａzおよびｂzを算出すると、これらの算出結果を代入した、上式（12）および（13）の直線と、最小二乗計算に用いた各測定点との距離を残差ｈxおよびｈzとして算出するとともに、前記各測定点に対する残差ｈxの二乗和と、残差ｈzの二乗和とを算出する（下式（16））。
【０１５９】
【数１３】
【０１６０】
残差ｈxの二乗和および残差ｈzの二乗和が算出されると、両者を比較して値の小さい方の直線を、正しい直線として採用し、この直線の勾配に基づいて、算出対象の測定点の勾配を求める。具体的に、残差ｈxの二乗和の方が小さければ、「ａ＝ａx、ｂ＝ｂx」とし、残差ｈzの二乗和の方が小さければ、「ａ＝１／ａz、ｂ＝１／ｂz」とする。
【０１６１】
上記のようにして、走査平面に対する全測定点の勾配が算出されると、次に、各勾配の出現頻度を算出する（ステップＳ３１０）。
図２８は、ある走査平面に対する勾配の出現頻度の一例を示す図である。
ここでは、図２８に示すような、出現頻度が得られたとする。
図２８に示すように、勾配が０近傍の出現頻度が比較的高くなっており、これらの勾配に対応する測定点の座標情報が連続していれば、そこに水平面があると推定することができる。
【０１６２】
出現頻度が算出されると、センシングプロセッサ２１０は、次に、未判定の勾配の出現頻度を選択し（ステップＳ３１２）、その出現頻度が、あらかじめ設定された閾値以上か否かを判定する（ステップＳ３１４）。そして、閾値以上であれば（ステップＳ３１４の「Ｙｅｓ」の分岐）、勾配と測定点の座標情報とを対応付けてＲＡＭ（不図示）に記録する（ステップＳ３１６）。一方、閾値未満であれば（ステップＳ３１４の「Ｎｏ」の分岐）、測定ノイズとして、測定点の距離情報を除外する（ステップＳ３３４）。
【０１６３】
上記の判定処理が、すべての勾配に対して終了すると（ステップＳ３１８の「Ｙｅｓ」の分岐）、次に、ＲＡＭに記録された、出現頻度が閾値以上の勾配および測定点の座標情報に基づいて、連続面の有無を判断する（ステップＳ３２０）。
脚車輪型ロボット１００の移動経路上には、段差が一定でかつ蹴込板が連続面となっている階段が存在しているため、階段部分にある水平面等が、勾配０および略０の測定点の連続する座標として現われる。このような場合に、センシングプロセッサ２１０は、連続面（実際は、面と走査平面との交差線）がある（ステップＳ３２２の「Ｙｅｓ」の分岐）と判断する。一方、座標の途中で勾配がいきなり外れた値となったときは、そこで面（実際は、面と走査平面との交差線）が途切れていると判断する。これらの判断結果から、物体の凹凸を認識することが可能である。
【０１６４】
そして、連続面に対応する直交座標系の測定距離を、センサ座標系（３次元）の測定距離に変換し、ＲＡＭに記録する（ステップＳ３２４）。
連続面の判断が終了すると、３次元距離測定装置２００は、ドライバ２１４において、センシングプロセッサ２１０からの指令信号およびエンコーダ２１８からの出力信号に基づいて、モータ２１６の回転軸を回転駆動し、２次元距離測定装置２１２をｚ軸回りに回転角度１０°ずつ回転する第２走査処理を実行する（ステップＳ３３２）。第２走査処理によって、２次元距離測定装置２１２の向きが１つ前の状態に対してｚ軸回りに１０°だけ変化する。そして、２次元距離測定装置２１２は、この状態で、再び、第１走査処理を実行する（ステップＳ３０２）。これにより、ｚ軸回りに１０°ずれた位置に新たな走査平面が形成される。
【０１６５】
そして、設定された走査角度範囲（−４０°〜＋４０°）に対して、設定された走査単位角度１０°で、２次元距離測定装置２１２を回転させ、各回転角度φにおいて、第１走査処理を実行する。
ここで、２次元距離測定装置２１２が基体１０の正面に取り付けられているため、基体１０が走査範囲内に含まれてしまうが、脚車輪型ロボット１００の前方および歩行経路上を含む範囲においては阻害物が一切ないため、脚車輪型ロボット１００の歩行制御を行うのに十分な走査範囲が確保できているといえる。
【０１６６】
そして、９つの走査平面に対する距離の測定が終了すると（ステップＳ３２６の「Ｙｅｓ」の分岐）、次に、ＲＡＭに記録された連続面の情報に基づいて面データを生成する（ステップＳ３２８）。
面データは、上述したように、ｚ軸回りに連続する各隣り合う走査平面間において、勾配および座標の近い連続面をつなぎ合わせる（対応付ける）ことで生成される。生成した面データは、ハブ７６および通信Ｉ／Ｆ６４を介して、ＣＰＵ６０に出力される（ステップＳ３３０）。
【０１６７】
そして、ＣＰＵ６０により、入力された面データについてセンサ座標系の座標がグローバル座標系の座標に変換され、特徴点が検出される。
また、ＣＰＵ６０により、入力された面データが解析され、特に、勾配が０に近い交差線を有する面がある場合は、それを水平面とみなし、脚車輪型ロボット１００が歩行可能な面であると判断される。
【０１６８】
歩行可能な面であると判断された場合は、面データのグローバル座標系への変換結果と、３軸姿勢センサ７０のセンサ信号等とに基づいて脚先の着地位置が決定される。
なお、段差が一定でかつ蹴込板が連続面となる階段を例に挙げたが、段差が一定でない階段、蹴込板が連続面とならない階段、蹴込板の無い階段等に対しても、正確にその面の形状を認識することができるので、脚車輪型ロボット１００の階段への適応性を高めることが可能である。
【０１６９】
このようにして、本実施の形態では、測距センサ２１２ａをｙ軸回りに回転させるモータ２１２ｃ等からなるｙ軸回転機構と、測距センサ２１２ａをｚ軸回りに回転させるモータ２１６等からなるｚ軸回転機構とを備え、ｙ軸回転機構により測距センサ２１２ａを回転させながらｙ軸回転機構の走査単位角度ごとに測距センサ２１２ａの測定結果を取得する第１走査を、ｚ軸回転機構により測距センサ２１２ａを回転させながらｚ軸回転機構の走査単位角度ごとに行う第２走査を行うことにより、ｙ軸回転機構の走査単位角度ごとおよびｚ軸回転機構の走査単位角度ごとの測定結果を取得する。
【０１７０】
これにより、障害物の立体的な形状を把握することができ、これに基づいて検出された特徴点により干渉判定を行うことができるので、干渉判定の精度を向上することができる。また、測距センサ２１２ａを回転させる回転機構を採用したので、移動機構に比して、走査に必要なスペースが小さくてすみ、走査のための機構が簡素となり、しかも高速な走査を実現することができる。
【０１７１】
さらに、本実施の形態では、モータ２１６およびエンコーダ２１８と、２次元距離測定装置２１２とを水平方向に所定距離隔てて配置し、駆動回転軸の回転駆動力を、駆動プーリ２２０ａ、従動プーリ２２０ｂおよびベルト２２１を介して従動回転軸へと伝達し、２次元距離測定装置２１２をｚ軸回りに回転駆動する構成とした。
これにより、２次元距離測定装置２１２の走査角度範囲内には、走査を阻害するものが一切なくなるので、正確な距離情報を得ることができる。また、モータ２１６およびエンコーダ２１８と、２次元距離測定装置２１２とを水平方向に配置したので、３次元距離測定装置２００の高さ方向の占有率を低減することができる。
【０１７２】
さらに、本実施の形態では、測距センサ２１２ａをｙ軸回りに走査単位角度ずつ回転させるとともに、各回転角度における距離を測定する第１走査処理を実行し、距離情報に基づいて、各測定点の勾配および勾配の出現頻度を算出することで、走査平面内に存在する物体の凹凸を認識する。
これにより、連続面が存在することを推定することができる。
【０１７３】
さらに、本実施の形態では、１つの走査平面に対する第１走査処理が終了するごとに、２次元距離測定装置２１２をｚ軸回りに走査単位角度だけ回転させる第２走査処理を実行する。
これにより、複数の走査平面に対して第１走査処理を実行することができるので、測定範囲内に存在する物体上の面の形状を認識することができる。また、面の形状が認識できるので、脚車輪型ロボット１００は、未知の障害物に対して高い適応性を実現することができる。また、人が活動する環境での動作を行えるので、人と一緒に行動する用途に用いられるホームロボット、パーソナルロボット等のヒューマンアシストロボットに好適である。
【０１７４】
上記第２の実施の形態において、ｙ軸は、発明７の第１走査軸に対応し、ｚ軸は、発明７の第２走査軸に対応し、ｙ軸回転機構は、発明６若しくは７の第１走査手段、または発明７の第１回転手段に対応し、ｚ軸回転機構は、発明６若しくは７の第２走査手段、または発明７の第２回転手段に対応している。また、ステップＳ３０２は、発明６または７の測定結果取得手段に対応している。
【０１７５】
なお、上記第１および第２の実施の形態においては、脚すね部平面と特徴点の位置関係に基づいて干渉の有無を判定するように構成したが、これに限らず、脚すね部の形状に対応する線（直線、線分、多次曲線、円、楕円その他の曲線）と、特徴点との位置関係に基づいて干渉の有無を判定するように構成することもできる。また、脚すね部に対応する空間と、特徴点との位置関係に基づいて干渉の有無を判定するように構成することもできる。
【０１７６】
また、上記第１および第２の実施の形態においては、レーザ２８〜３０およびカメラ３２からなる物体センサまたは３次元距離測定装置２００を用いて特徴点を検出するように構成したが、これに限らず、他の方式の物体センサを用いて特徴点を検出するように構成することもできる。
また、上記第１および第２の実施の形態においては、物体センサを用いて特徴点を検出するように構成したが、これに限らず、物体センサを用いることなく、あらかじめ測定、計算若しくは定義した特徴点に関する特徴点情報、またはシミュレーション等の計算結果に基づいて特徴点を検出するように構成することもできる。
【０１７７】
また、上記第１および第２の実施の形態においては、干渉の有無を判定するように構成したが、これに限らず、干渉しないことのみを判定すること、干渉することのみを判定すること、干渉の度合いを判定すること、干渉までの度合い（例えば、特徴点と脚すね部の間の距離）を判定するように構成することもできる。
また、上記第２の実施の形態においては、エンコーダ２１８を駆動回転軸に設けて構成としたが、これに限らず、従動回転軸に設けて構成とすることもできる。
【０１７８】
これにより、伝達誤差の影響を受けずに第２走査処理における走査角度を高精度に検出することができる。
また、上記第２の実施の形態においては、駆動プーリ２２０ａ、従動プーリ２２０ｂおよびベルト２２１を介して、駆動回転軸の回転駆動力を従動回転軸へと伝達するように構成としたが、これに限らず、複数の歯車を介して、駆動回転軸の回転駆動力を従動回転軸へと伝達するように構成とすることもできる。
【０１７９】
また、上記第２の実施の形態においては、第１走査処理における距離情報の取得を離散的（第２走査処理で回転させてから第１走査処理で距離情報を取得）に行う構成としたが、これに限らず、走査角度と測定距離との対応付けを行うことで連続的（第２走査処理で回転させつつ、第１走査処理で距離情報を取得）に行う構成とすることもできる。
また、上記第２の実施の形態においては、ＣＰＵ６０からの指令信号に基づいて、走査角度範囲および走査単位角度を設定するように構成としたが、これに限らず、３次元距離測定装置２００にあらかじめ設定しておく構成とすることもできる。
【０１８０】
また、上記第２の実施の形態において、３次元距離測定装置２００は、測距センサ２１２ａをｙ軸回りに回転させ、２次元距離測定装置２１２をｚ軸回りに回転させる構成としたが、これに限らず、測距センサ２１２ａをｚ軸回りに回転させ、２次元距離測定装置２１２をｙ軸回りに回転させる構成とすることもできる。また、ｙ軸およびｚ軸回りに限らず、測距センサ２１２ａの測定方向に対して互いに直交する２つの軸であれば、どの方向の軸回りでもよい。さらに、このような回転機構に限らず、測距センサ２１２ａの測定方向とは異なる第１走査方向に測距センサ２１２ａを移動させ、測定方向および第１走査方向とは異なる第２走査方向に測距センサ２１２ａを移動させるように移動機構として構成することもできる。この場合、移動経路の形状としては、直線のほか円弧その他の曲線を採用することができる。回転機構と移動機構の組み合わせることもできる。
【０１８１】
また、上記第２の実施の形態においては、図２９（ａ）に示すように、２次元距離測定装置２１２自身を回転させているが、これに限らず、光学式の測距センサを有する２次元距離測定装置であれば、図２９（ｂ）に示すように、測定方向の光軸上に挿入したミラーを回転させてもよい。
【０１８２】
図２９は、測距センサの測定方向を変更する場合の構成を示す図である。
また、上記第１および第２の実施の形態においては、脚すね部を判定の対象部位として設定したが、これに限らず、脚車輪型ロボット１００の任意の部位を判定の対象部位として設定することができる。
また、上記第１および第２の実施の形態においては、本発明に係る干渉判定装置および脚車輪型ロボットを、階段を乗り越える場合について適用したが、これに限らず、階段以外の段差を乗り越える場合についても同様に適用することができる。
【０１８３】
また、上記第１および第２の実施の形態においては、本発明に係る干渉判定装置および脚車輪型ロボットを脚車輪型ロボット１００に適用したが、これに限らず、本発明の主旨を逸脱しない範囲で他の場合にも適用可能である。例えば、直動関節で脚構造を実現する脚車輪型ロボットや他の構成のロボット、車両その他の装置に適用することができる。
【図面の簡単な説明】
【０１８４】
【図１】脚車輪型ロボット１００の正面図である。
【図２】脚車輪型ロボット１００の側面図である。
【図３】障害物センサ３４、３６の構成を示す図である。
【図４】脚車輪型ロボット１００の移動制御システムを示すブロック図である。
【図５】昇降制御処理を示すフローチャートである。
【図６】光切断法の原理を説明するための図である。
【図７】階段にレーザ光を照射した状態およびカメラ３２の撮像素子の画像を示す図である。
【図８】階段の昇降時に段鼻部の特徴点を検出する場合を示す図である。
【図９】グローバル座標系において脚すね部の表面に対応する４つの平面を示す図である。
【図１０】段鼻部の特徴点Ａ、脚すね部平面、脚すね部平面上の任意の点Ｐおよび脚すね部平面の法線ベクトルＮを示す図である。
【図１１】グローバル座標系および脚車輪型ロボット１００の各座標系を示す図である。
【図１２】座標系Ｐ4を示す図である。
【図１３】脚すね部平面および法線ベクトルＮを示す図である。
【図１４】干渉判定処理を示すフローチャートである。
【図１５】脚車輪型ロボット１００を側面図である。
【図１６】３次元距離測定装置２００の外観構造を示す図である。
【図１７】２次元距離測定装置２１２の制御構造を示すブロック図である。
【図１８】測距センサ２１２ａの走査範囲を示す図である。
【図１９】３次元距離測定装置２００の制御構造を示すブロック図である。
【図２０】２次元距離測定装置２１２の距離測定の原理を説明する図である。
【図２１】第１走査処理および第２走査処理により走査を行った場合を示す図である。
【図２２】３次元距離測定装置２００の距離の計測例を示す図である。
【図２３】３次元距離測定装置２００における物体認識処理を示すフローチャートである。
【図２４】勾配の算出例を示す図である。
【図２５】測定範囲内に階段がある場合の、第１走査処理による測定結果を示す図である。
【図２６】図２５の各測定点の回転座標系の測定距離を直交座標系の測定距離に変換した場合の測定結果を示す図である。
【図２７】最小二乗法を用いて算出したｘ方向およびｚ方向の勾配の一例を示す図である。
【図２８】ある走査平面に対する勾配の出現頻度の一例を示す図である。
【図２９】測距センサの測定方向を変更する場合の構成を示す図である。
【符号の説明】
【０１８５】
１００ 脚車輪型ロボット
１０ 基体
１２ 脚部
１４〜１８ 回転関節
２０ 駆動輪
２２、２４ 脚先センサ
２６ 水平レーザ
２８、３０ 垂直レーザ
３２ カメラ
３４、３６ 障害物センサ
４０、５０ モータ
４２、５２ エンコーダ
４４、５４ ドライバ
７０ ３軸姿勢センサ
２００ ３次元距離測定装置
２１０ センシングプロセッサ
２１２ ２次元距離測定装置
２１２ａ 測距センサ
２１２ｃ、２１６ モータ
２１２ｄ、２１８ エンコーダ
２１２ｂ、２１４ ドライバ
２２０ａ 駆動プーリ
２２０ｂ 従動プーリ
２２１ ベルト
【特許請求の範囲】
【請求項１】
対象部位について障害物との干渉判定を行う干渉判定装置であって、
障害物の特徴点を検出する特徴点検出手段と、
前記特徴点および前記対象部位の座標を定める座標系において、前記対象部位またはこれと連動する部位の姿勢および前記対象部位の形状により特定される線、平面または空間であって、前記対象部位の形状に対応する線、前記対象部位の表面に対応する平面または前記対象部位に対応する空間と、前記特徴点検出手段で検出した特徴点との位置関係に基づいて、前記対象部位について障害物との干渉判定を行う干渉判定手段とを備えることを特徴とする干渉判定装置。
【請求項２】
基体と、前記基体に対して自由度を有して連結された脚部と、前記脚部に回転可能に設けられた車輪と、物体センサとを備え、前記脚部の駆動および前記車輪の回転により移動する脚車輪型ロボットであって、
前記物体センサの測定結果に基づいて障害物の特徴点を検出する特徴点検出手段と、
前記特徴点および前記対象部位の座標を定める座標系において、当該脚車輪型ロボットの姿勢および前記対象部位の形状により特定される線、平面または空間であって、前記対象部位の形状に対応する線、前記対象部位の表面に対応する平面または前記対象部位に対応する空間と、前記特徴点検出手段で検出した特徴点との位置関係に基づいて、前記対象部位について障害物との干渉判定を行う干渉判定手段とを備えることを特徴とする脚車輪型ロボット。
【請求項３】
請求項２において、
前記干渉判定手段は、前記座標系において、前記対象部位の表面に対応する複数の平面で囲まれる空間に前記特徴点が属するか否かを判定することにより、前記対象部位に対して障害物が干渉するか否かを判定することを特徴とする脚車輪型ロボット。
【請求項４】
請求項３において、
前記干渉判定手段は、当該脚車輪型ロボットの姿勢および前記対象部位の形状に基づいて、前記対象部位の座標を定める第１座標系の座標を、前記特徴点の座標を定める第２座標系の座標に変換して前記第２座標系における前記複数の平面を特定し、前記第２座標系において、前記複数の平面で囲まれる空間に前記特徴点が属するか否かを判定することを特徴とする脚車輪型ロボット。
【請求項５】
請求項３および４のいずれか１項において、
前記干渉判定手段は、前記特徴点と前記平面上の任意の点を結ぶ直線と、前記平面の法線ベクトルとの内積に基づいて、前記複数の平面で囲まれる空間に前記特徴点が属するか否かを判定することを特徴とする脚車輪型ロボット。
【請求項６】
請求項２ないし５のいずれか１項において、
前記物体センサは、物体上の測定点までの距離を測定する測距センサであり、
前記測距センサの測定方向とは異なる第１走査方向に前記測距センサを走査する第１走査手段と、前記測定方向および前記第１走査方向とは異なる第２走査方向に前記測距センサを走査する第２走査手段と、前記第１走査手段および前記第２走査手段の走査範囲で測定可能な前記測定点について前記測距センサの測定結果を取得する測定結果取得手段とを備え、
前記特徴点検出手段は、前記測定結果取得手段で取得した測定結果に基づいて前記特徴点を検出することを特徴とする脚車輪型ロボット。
【請求項７】
請求項６において、
前記第１走査手段は、前記測定方向に対して所定角度をなす第１走査軸の回りに前記測距センサを回転させる第１回転手段であり、
前記第２走査手段は、前記測定方向および前記第１走査軸に対して所定角度をなす第２走査軸の回りに前記測距センサを回転させる第２回転手段であり、
前記測定結果取得手段は、前記第１回転手段により前記測距センサを回転させながら前記第１回転手段の所定単位角度ごとに前記測距センサの測定結果を取得する第１走査を、前記第２回転手段により前記測距センサを回転させながら前記第２回転手段の所定単位角度ごとに行う第２走査を行うことにより、前記第１回転手段の所定単位角度ごとおよび前記第２回転手段の所定単位角度ごとの前記測定結果を取得することを特徴とする脚車輪型ロボット。
【請求項１】
対象部位について障害物との干渉判定を行う干渉判定装置であって、
障害物の特徴点を検出する特徴点検出手段と、
前記特徴点および前記対象部位の座標を定める座標系において、前記対象部位またはこれと連動する部位の姿勢および前記対象部位の形状により特定される線、平面または空間であって、前記対象部位の形状に対応する線、前記対象部位の表面に対応する平面または前記対象部位に対応する空間と、前記特徴点検出手段で検出した特徴点との位置関係に基づいて、前記対象部位について障害物との干渉判定を行う干渉判定手段とを備えることを特徴とする干渉判定装置。
【請求項２】
基体と、前記基体に対して自由度を有して連結された脚部と、前記脚部に回転可能に設けられた車輪と、物体センサとを備え、前記脚部の駆動および前記車輪の回転により移動する脚車輪型ロボットであって、
前記物体センサの測定結果に基づいて障害物の特徴点を検出する特徴点検出手段と、
前記特徴点および前記対象部位の座標を定める座標系において、当該脚車輪型ロボットの姿勢および前記対象部位の形状により特定される線、平面または空間であって、前記対象部位の形状に対応する線、前記対象部位の表面に対応する平面または前記対象部位に対応する空間と、前記特徴点検出手段で検出した特徴点との位置関係に基づいて、前記対象部位について障害物との干渉判定を行う干渉判定手段とを備えることを特徴とする脚車輪型ロボット。
【請求項３】
請求項２において、
前記干渉判定手段は、前記座標系において、前記対象部位の表面に対応する複数の平面で囲まれる空間に前記特徴点が属するか否かを判定することにより、前記対象部位に対して障害物が干渉するか否かを判定することを特徴とする脚車輪型ロボット。
【請求項４】
請求項３において、
前記干渉判定手段は、当該脚車輪型ロボットの姿勢および前記対象部位の形状に基づいて、前記対象部位の座標を定める第１座標系の座標を、前記特徴点の座標を定める第２座標系の座標に変換して前記第２座標系における前記複数の平面を特定し、前記第２座標系において、前記複数の平面で囲まれる空間に前記特徴点が属するか否かを判定することを特徴とする脚車輪型ロボット。
【請求項５】
請求項３および４のいずれか１項において、
前記干渉判定手段は、前記特徴点と前記平面上の任意の点を結ぶ直線と、前記平面の法線ベクトルとの内積に基づいて、前記複数の平面で囲まれる空間に前記特徴点が属するか否かを判定することを特徴とする脚車輪型ロボット。
【請求項６】
請求項２ないし５のいずれか１項において、
前記物体センサは、物体上の測定点までの距離を測定する測距センサであり、
前記測距センサの測定方向とは異なる第１走査方向に前記測距センサを走査する第１走査手段と、前記測定方向および前記第１走査方向とは異なる第２走査方向に前記測距センサを走査する第２走査手段と、前記第１走査手段および前記第２走査手段の走査範囲で測定可能な前記測定点について前記測距センサの測定結果を取得する測定結果取得手段とを備え、
前記特徴点検出手段は、前記測定結果取得手段で取得した測定結果に基づいて前記特徴点を検出することを特徴とする脚車輪型ロボット。
【請求項７】
請求項６において、
前記第１走査手段は、前記測定方向に対して所定角度をなす第１走査軸の回りに前記測距センサを回転させる第１回転手段であり、
前記第２走査手段は、前記測定方向および前記第１走査軸に対して所定角度をなす第２走査軸の回りに前記測距センサを回転させる第２回転手段であり、
前記測定結果取得手段は、前記第１回転手段により前記測距センサを回転させながら前記第１回転手段の所定単位角度ごとに前記測距センサの測定結果を取得する第１走査を、前記第２回転手段により前記測距センサを回転させながら前記第２回転手段の所定単位角度ごとに行う第２走査を行うことにより、前記第１回転手段の所定単位角度ごとおよび前記第２回転手段の所定単位角度ごとの前記測定結果を取得することを特徴とする脚車輪型ロボット。
【図１】
【図２】
【図３】
【図４】
【図５】
【図６】
【図７】
【図８】
【図９】
【図１０】
【図１１】
【図１２】
【図１３】
【図１４】
【図１５】
【図１６】
【図１７】
【図１８】
【図１９】
【図２０】
【図２１】
【図２２】
【図２３】
【図２４】
【図２５】
【図２６】
【図２７】
【図２８】
【図２９】
【図２】
【図３】
【図４】
【図５】
【図６】
【図７】
【図８】
【図９】
【図１０】
【図１１】
【図１２】
【図１３】
【図１４】
【図１５】
【図１６】
【図１７】
【図１８】
【図１９】
【図２０】
【図２１】
【図２２】
【図２３】
【図２４】
【図２５】
【図２６】
【図２７】
【図２８】
【図２９】
【公開番号】特開２００９−５０９３６（Ｐ２００９−５０９３６Ａ）
【公開日】平成２１年３月１２日（２００９．３．１２）
【国際特許分類】
【出願番号】特願２００７−２１８６４６（Ｐ２００７−２１８６４６）
【出願日】平成１９年８月２４日（２００７．８．２４）
【出願人】（０００００４２０４）日本精工株式会社 (8,378)
【Ｆターム（参考）】
【公開日】平成２１年３月１２日（２００９．３．１２）
【国際特許分類】
【出願日】平成１９年８月２４日（２００７．８．２４）
【出願人】（０００００４２０４）日本精工株式会社 (8,378)
【Ｆターム（参考）】
[ Back to top ]