３次元距離測定装置及び脚車輪型ロボット
【課題】測距センサの駆動機構に阻害されずに走査範囲を走査することが可能な３次元距離測定装置及び当該装置を備えた脚車輪型ロボットを提供する。
【解決手段】３次元距離測定装置２００を、２次元距離測定装置１１２と、モータ１１６と、エンコーダ１１８と、プーリ１２０ａ及び１２０ｂと、ベルト１２１とを含んだ構成とし、モータ１１６の第１の回転軸を、プーリ１２０ａの回転軸となるように当該プーリ１２０ａと係合し、モータ１１６の回転駆動力で、プーリ１２０ａを回転駆動する。プーリ１２０ａ及びプーリ１２０ｂは、その軸心Ａ及びＢの位置が水平方向に所定の距離を開けるように配設し、プーリ１２０ａの回転駆動力は、ベルト１２１を介してプーリ１２０ｂに伝達されるようにした。また、プーリ１２０ｂと、２次元距離測定装置１１２を回転駆動する第２の回転軸とは、当該第２の回転軸がプーリ１２０ｂの回転に連動して回転するように係合した。
【解決手段】３次元距離測定装置２００を、２次元距離測定装置１１２と、モータ１１６と、エンコーダ１１８と、プーリ１２０ａ及び１２０ｂと、ベルト１２１とを含んだ構成とし、モータ１１６の第１の回転軸を、プーリ１２０ａの回転軸となるように当該プーリ１２０ａと係合し、モータ１１６の回転駆動力で、プーリ１２０ａを回転駆動する。プーリ１２０ａ及びプーリ１２０ｂは、その軸心Ａ及びＢの位置が水平方向に所定の距離を開けるように配設し、プーリ１２０ａの回転駆動力は、ベルト１２１を介してプーリ１２０ｂに伝達されるようにした。また、プーリ１２０ｂと、２次元距離測定装置１１２を回転駆動する第２の回転軸とは、当該第２の回転軸がプーリ１２０ｂの回転に連動して回転するように係合した。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、測距センサ及び回転駆動機構を含んで構成される３次元距離測定装置に係り、特に、所定回転角度範囲内において、前記回転駆動機構を含む駆動機構に阻害されずに、測距センサで走査範囲を走査することが可能な３次元距離測定装置及び当該装置を有する脚車輪型ロボットに関する。
【背景技術】
【０００２】
従来、２次元レンジセンサ、レーザレンジファインダ、測域センサ、平面センサなどの２次元距離測定装置を回転駆動して３次元の距離情報を測定する３次元距離測定装置がある。例えば、平面センサを用いた３次元距離測定装置として、非特許文献１に記載の技術がある。
非特許文献１では、環境をくまなくスキャンするために、平面センサを３６０[°]以上回転できるようにしている。
【非特許文献１】上田達朗、川田浩彦、冨沢哲雄、大矢晃久、油田信一「無限回転機構を用いた３次元測域センサ」、ロボディクスメカトロニクス講演会06、2P1-C16
【発明の開示】
【発明が解決しようとする課題】
【０００３】
しかしながら、上記非特許文献１の従来技術では、平面スキャナを３６０[°]以上回転させるために、平面スキャナを駆動する全ての機構をその下部の一箇所に集める構成としている。そのため、縦に長い機構となり、スペースファクタ的に不利になる場合がある。
また、上記非特許文献１の従来技術では、３６０[°]以上回転できる機構とするために、配線を行わずに通信及び電源の供給ができるスリップリングを用いているため、接点寿命の問題が発生する。なお、通信部を無線化しても電源の配線は必要となる。また、その電源をバッテリで補っても、重量増、バッテリ持続時間などの問題が発生する。
【０００４】
また、環境認識、形状認識、障害物回避等において、人間が活動できるように整備された空間であれば、必ずしも３６０[°]以上の回転機構がなくても十分な測定情報は得られる。
そこで、本発明は、このような従来の技術の有する未解決の課題に着目してなされたものであり、所定の回転角度範囲において、測距センサの駆動機構に阻害されずに走査範囲を走査することが可能な３次元距離測定装置及び当該装置を備えた脚車輪型ロボットを提供することを目的としている。
【課題を解決するための手段】
【０００５】
〔発明１〕 上記目的を達成するために、発明１の３次元距離測定装置は、測定距離範囲内に存在する物体上の測定点までの距離を測定する測距センサと、前記測距センサを、その測定方向に対して直交する２つの軸のうち一方の軸まわりに回転駆動させる第１の回転機構と、前記測距センサを、前記第１の回転機構と共に、前記測定方向に対して直交する２つの軸のうち前記第１の回転機構とは異なる方向の軸まわりに回転駆動させる第２の回転機構とを有し、前記第１の回転機構及び前記第２の回転機構を所定の回転角度ずつ回転駆動しながら前記測距センサによって距離を測定することで、前記物体の３次元の距離を測定する３次元距離測定装置であって、前記第１の回転機構及び前記第２の回転機構の少なくとも一方の回転駆動を所定の回転角度範囲内で行うと共に、当該回転角度範囲内の回転駆動によって前記測距センサが走査する走査範囲外に、前記第１の回転機構及び前記第２の回転機構含む自己の駆動機構を設けたことを特徴とする。
【０００６】
このような構成であれば、測距センサは、その測定方向に対して直交する２つの軸のうち一方の軸まわりに回転駆動し、且つ、測距センサ及び第１の回転機構は、前記測定方向に対して直交する２つの軸のうち他方の軸まわりに回転駆動しながら物体上の測定点までの距離を測定することが可能である。
また、第１の回転機構及び第２の回転機構の少なくとも一方の回転駆動による走査を、所定の回転角度範囲内において、測距センサの駆動機構に阻害されずに行うことが可能である。
ここで、所定の回転角度とは、環境認識、形状認識、障害物回避等を行うのに十分な距離データが得られる走査範囲の角度である。
【０００７】
〔発明２〕 更に、発明２の３次元距離測定装置は、発明１の３次元距離測定装置において、前記第２の回転機構は、回転駆動力を発生する回転駆動装置を含み、
前記回転駆動装置の第１の回転軸と、前記測距センサ及び前記第１の回転機構を共に回転させる第２の回転軸とを水平方向にずらして設けると共に、前記回転駆動装置の第１の回転軸の回転駆動力を、所定の動力伝達機構を介して前記第２の回転軸に伝達する構成としたことを特徴とする。
このような構成であれば、前記測距センサ及び前記第１の回転機構とから構成される２次元距離測定装置と、前記第２の回転機構とを水平方向に並べて配設することが可能である。
【０００８】
〔発明３〕 更に、発明３の３次元距離測定装置は、発明２の３次元距離測定装置において、前記動力伝達機構は、複数の滑車及び各滑車に動力を伝達するベルトを介して、前記回転駆動装置の第１の回転軸の回転駆動力を、前記第２の回転軸に伝達する構成となっていることを特徴とする。
このような構成であれば、回転駆動装置の第１の回転軸の回転駆動力を滑車に伝達することが可能であり、各滑車間の動力をベルトを介して伝達することが可能であり、伝達された滑車の回転力を第２の回転軸に伝達することが可能である。
【０００９】
〔発明４〕 更に、発明４の３次元距離測定装置は、発明２の３次元距離測定装置において、前記動力伝達機構は、複数の歯車を介して、前記回転駆動装置の第１の回転軸の回転駆動力を前記第２の回転軸に伝達する構成となっていることを特徴とする。
このような構成であれば、回転駆動装置の第１の回転軸の回転駆動力を複数の歯車を介して第２の回転軸へと伝達することが可能である。
【００１０】
〔発明５〕 更に、発明５の３次元距離測定装置は、発明１乃至４のいずれか１の３次元距離測定装置において、前記第１の回転機構を制御して、前記測距センサを、前記第１の回転機構の回転方向に、所定の回転角度ずつ回動させると共に、前記測距センサによって各回転角度に応じた距離情報を測定する処理である第１走査処理を行う第１走査手段と、前記第１走査手段によって測定された、各測定点の距離情報とその周辺の測定点の距離情報とに基づき、各測定点に対する勾配を算出する勾配算出手段と、前記勾配算出手段で算出した各測定点の勾配に基づき、各勾配の出現頻度を算出する出現頻度算出手段と、前記出現頻度算出手段で算出した各勾配の出現頻度と、各勾配に対応する測定点の座標情報とに基づき、前記測距センサの測定平面内に存在する物体の形状を認識する形状認識手段と、を備えることを特徴とする。
【００１１】
このような構成であれば、第１走査手段によって、第１の回転機構を制御して、前記測距センサを、前記第１の回転機構の回転方向に、所定の回転角度ずつ回動させると共に、前記測距センサによって各回転角度に応じた距離情報を測定する処理である第１走査処理を行うことが可能である。
また、勾配算出手段によって、第１走査手段によって測定された、各測定点の距離情報とその周辺の測定点の距離情報とに基づき、各測定点に対する勾配を算出することが可能であり、出現頻度算出手段によって、勾配算出手段で算出した各測定点の勾配に基づき、各勾配の出現頻度を算出することが可能であり、形状認識手段によって、出現頻度算出手段で算出した各勾配の出現頻度と、各勾配に対応する測定点の座標情報とに基づき、前記測距センサの測定平面内に存在する物体の詳細な形状を認識することが可能である。
【００１２】
ここで、勾配算出手段は、例えば、直交座標系において複数個の測定値に対する回帰直線を公知の最小二乗法で近似すると共に、この回帰直線から勾配を算出したり、直交座標系において一方の軸方向の複数の測定点を間に挟む両端２つの測定値の差分値と、他方の軸方向の複数の測定点を間に挟む両端２つの測定値の差分値とを用いて勾配を算出したりする。
また、形状認識手段は、各勾配の出現頻度と、各勾配に対応する測定点の座標情報とに基づき、測定平面内に存在する物体の凹凸を認識する。例えば、勾配が０又は略０の測定点が連続する場合は、そこに平面があると認識することが可能である。
【００１３】
〔発明６〕 更に、発明６の３次元距離測定装置は、発明５の３次元距離測定装置において、１つの前記測定平面に対する前記第１走査処理が終了する毎に、前記第２の回転機構を制御して、前記測距センサを前記第１の回転機構と共に、前記第２の回転機構の回転方向に所定の回転角度ずつ回動させる処理である第２走査処理を行う第２走査手段を備え、前記第１走査手段は、前記第２走査処理において、前記測距センサが前記所定の回転角度の回動をする毎に前記第１走査処理を実行し、前記勾配算出手段は、前記測定平面毎に、前記第１走査処理によって測定された、各測定点の距離情報と、その周辺の測定点の距離情報とに基づき、各測定点に対する勾配を算出し、前記出現頻度算出手段は、前記測定平面毎に、前記勾配算出手段で算出した各測定点の勾配に基づき、各勾配の出現頻度を算出し、前記形状認識手段は、前記測定平面毎に、前記出現頻度算出手段で算出した各勾配の出現頻度と、各勾配に対応する測定点の座標情報とに基づき、前記測定平面内に存在する物体の形状を認識することを特徴とする。
【００１４】
このような構成であれば、第２走査手段によって、１つの測定平面に対する第１走査処理が終了する毎に、第２の回転機構を制御して、測距センサを第１の回転機構と共に、第２の回転機構の回転方向に所定の回転角度ずつ回動させる処理である第２走査処理を行うことが可能である。
また、勾配算出手段は、測定平面毎に、第１走査処理によって測定された、各測定点の距離情報と、その周辺の測定点の距離情報とに基づき、各測定点に対する勾配を算出し、出現頻度算出手段は、測定平面毎に、勾配算出手段で算出した各測定点の勾配に基づき、各勾配の出現頻度を算出し、形状認識手段は、測定平面毎に、出現頻度算出手段で算出した各勾配の出現頻度と、各勾配に対応する測定点の座標情報とに基づき、測定平面内に存在する物体の形状を認識することが可能である。
ここで、形状認識手段は、測距センサを、例えば、ピッチ回転させたときの距離情報、及びヨー回転させたときの距離情報の双方から算出した各測定点の勾配情報と、各測定点の座標情報とから物体の形状を認識することができるので、物体の詳細な面形状を認識することが可能である。
【００１５】
〔発明７〕 一方、上記目的を達成するために、発明５の脚車輪型ロボットは、基体と、前記基体に対して自由度を有して連結された脚部と、前記脚部に回転可能に設けられた車輪と、前記脚部及び前記車輪を駆動するための動力を付与するアクチュエータと、前記アクチュエータを制御する制御手段とを備え、前記脚部の駆動および前記車輪の回転により移動する脚車輪型ロボットであって、発明１乃至発明６のいずれか１項に記載の３次元距離測定装置を備え、前記制御手段は、前記３次元距離測定装置の測定距離に基づき前記アクチュエータを制御することを特徴とする。
このような構成であれば、発明１乃至４のいずれか１の３次元距離測定装置によって測定した測定距離に基づき、車輪走行制御及び歩行制御を行うことが可能である。
【００１６】
〔発明８〕 また、上記目的を達成するために、発明８の脚車輪型ロボットは、基体と、前記基体に対してヨー軸回りの自由度およびピッチ軸またはロール軸回りの自由度を有して連結された脚部と、前記脚部に回転可能に設けられた車輪と、前記ヨー軸回りの自由度の範囲で前記脚部を駆動するための動力を付与する第１アクチュエータと、前記ピッチ軸またはロール軸回りの自由度の範囲で前記脚部を駆動するための動力を付与する第２アクチュエータと、前記第１アクチュエータおよび前記第２アクチュエータを制御する制御手段と、発明１乃至発明６のいずれか１項に記載の３次元距離測定装置とを備え、前記脚部の駆動および前記車輪の回転により移動する脚車輪型ロボットであって、前記制御手段は、前記基体の向きを一定方向に保ちながら、自脚車輪型ロボットの進行方向と、前記各車輪の進行方向とが一致するように、前記第２アクチュエータを制御する。
このような構成であれば、自脚車輪型ロボットの移動時において、脚車輪型ロボットの基体の向きを一定方向に保ちながら、自脚車輪型ロボットの進行方向と各車輪の進行方向とが一致するように第２アクチュエータが制御される。
【００１７】
〔発明９〕 また、上記目的を達成するために、発明９の脚車輪型ロボットは、基体と、前記基体に対してヨー軸回りの自由度およびピッチ軸またはロール軸回りの自由度を有して連結された複数の脚部と、前記各脚部に回転可能に設けられた車輪と、前記ヨー軸回りの自由度の範囲で前記各脚部を駆動するための動力を付与する第１アクチュエータと、前記ピッチ軸またはロール軸回りの自由度の範囲で前記各脚部を駆動するための動力を付与する第２アクチュエータと、前記第１アクチュエータおよび前記第２アクチュエータを制御する制御手段と、発明１乃至発明６のいずれか１項に記載の３次元距離測定装置とを備え、前記脚部の駆動および前記車輪の回転により移動する脚車輪型ロボットであって、前記制御手段は、旋回時に、前記基体を所定の回転中心位置でヨー軸周りに自転運動させたときの前記各車輪の操舵時の回転中心の描く円弧軌道と前記操舵時の回転中心との接点位置における該回転中心の運動方向と、前記各車輪の進行方向とが一致するように、前記第２アクチュエータを制御する。
【００１８】
このような構成であれば、旋回時は、制御手段により、基体をヨー軸（垂直軸）周りに自転運動させたときの各車輪の操舵時の回転中心の描く円弧軌道と操舵時の回転中心との接点位置における該回転中心の運動方向と、各車輪の進行方向とが一致するように、第２アクチュエータが制御される。
【００１９】
〔発明１０〕 更に、発明１０の脚車輪型ロボットは、発明９の脚車輪型ロボットにおいて、前記制御手段は、前記基体の向きを一定方向に保ちながら、自脚車輪型ロボットの進行方向と、前記各車輪の進行方向とが一致するように、前記第２アクチュエータを制御する。
【００２０】
このような構成であれば、自脚車輪型ロボットの移動時において、脚車輪型ロボットの基体の向きを一定方向に保ちながら、自脚車輪型ロボットの進行方向と各車輪の進行方向とが一致するように第２アクチュエータが制御される。
【発明の効果】
【００２１】
以上説明したように、発明１の３次元距離測定装置によれば、所定の回転角度範囲内において、測距センサの駆動機構に阻害されずに走査を行うことができるので、正確な距離測定を行うことができるという効果が得られる。
また、発明２〜４の３次元距離測定装置によれば、前記測距センサ及び前記第１の回転機構と、前記第２の回転機構とを水平方向に並べた構成とすることができるので、３次元距離測定装置の垂直方向の占有率を抑えることができるという効果が得られる。
【００２２】
また、発明５〜６の３次元距離測定装置によれば、物体の面の形状を認識（又は推定）することができるので、これにより、段差が一定でない階段、建物の非常階段のように蹴込が連続面でない階段、非常用の外階段のように階段踏板のみの階段など、様々な形状の物体を認識できるという効果が得られる。
一方、発明８、１０の脚車輪型ロボットによれば、脚車輪型ロボットを、その向きとは関係なく自由な方向へと移動させることができるので、各方向への素早い移動を実現できると共に、例えば、脚車輪型ロボットの各構成部が妨げとなって旋回できないような狭くて入り組んだエリアなど、脚車輪型ロボットの向きを変更することが困難なエリアにおいても活動が可能となるという効果が得られる。
【００２３】
また、発明９の脚車輪型ロボットによれば、旋回時に、脚車輪型ロボットの基体を所定の回転中心位置でヨー軸周りにに自転運動させたときの各車輪の操舵時の回転中心の描く円弧軌道と操舵時の回転中心との接点における該回転中心の運動方向と、各車輪の進行方向とを一致させることができるので、脚車輪型ロボットを、所定の旋回中心位置で前後移動させずに旋回（クローラなどによる超信地旋回と同等の旋回）をさせることができるという効果が得られる。また、基体の中心位置を、自転させる時の回転中心位置とすることで、最小の旋回半径で脚車輪型ロボットを旋回させることが可能である。
【発明を実施するための最良の形態】
【００２４】
〔第１の実施の形態〕
以下、本発明の第１の実施の形態を図面に基づき説明する。図１〜図１４は、本発明に係る３次元距離測定装置及び脚車輪型ロボットの第１の実施の形態を示す図である。
まず、図１に基づき、本発明に係る脚車輪型ロボットの概略構成を説明する。
図１は、本発明に係る脚車輪型ロボット１００の側面図である。
【００２５】
図１に示すように、脚車輪型ロボット１００は、基体と、基体に連結された４つの脚部とを有して構成されている。 各脚部は、３つの回転関節を介して連結されている。上段の回転関節は、脚車輪型ロボット１００の底面と直交する方向を軸方向として回転する。すなわち、ヨー軸回りに回転する。また、中段及び下段の回転関節は、上段の回転関節が図１の状態であるときは、ピッチ軸回りに回転し、上段の回転関節が図１の状態から９０度回転した状態であるときは、ロール軸回りに回転する。つまり、各脚部は、それぞれ３自由度を有する。
【００２６】
また、図１に示すように、脚車輪型ロボット１００の基体上部には、３次元距離測定装置２００が配設されている。図１に示すように、３次元距離測定装置２００及び後述する測距センサ１１２ａの原点位置（回転角度θ及びφが０[°]の位置）における、測距センサ１１２ａの測定方向の軸がｘ軸、測定方向に対して水平方向の軸（後述するｙ軸）と直交する軸がｚ軸となる。
【００２７】
次に、図２及び図３に基づき、３次元距離測定装置２００の機械的な構成を説明する。
ここで、図２（ａ）は、３次元距離測定装置２００の機械的な構成を示す図であり、（ｂ）は、（ａ）を真上から見た図である。また、図３は、測距センサ１１２ａの走査範囲の一例を示す図である。
図２（ａ）に示すように、３次元距離測定装置２００は、２次元距離測定装置１１２と、モータ１１６と、エンコーダ１１８と、プーリ１２０ａ及び１２０ｂと、ベルト１２１とを含んだ構成となっている。
【００２８】
モータ１１６の回転軸（以下、第１の回転軸と称す）は、プーリ１２０ａの回転軸となるように当該プーリ１２０ａと係合されており、モータ１１６の回転駆動力が、プーリ１２０ａを回転駆動する。また、プーリ１２０ａ及びプーリ１２０ｂは、その軸心Ａ及びＢの位置が水平方向に所定の距離を開けるように配設されている。このように、プーリ１２０ａ及び１２０ｂを介して、モータ１１６の軸心Ａと、２次元距離測定装置１１２の回転軸（以下、第２の回転軸と称す）の軸心Ｂとを水平方向にずらすことで、モータ１１６及びエンコーダ１１８と、２次元距離測定装置１１２とを水平方向に並べた構成とすることができる。また、図２（ａ）に示すように、プーリ１２０ａ及び１２０ｂ、ベルト１２１、その他の支持部材は、２次元距離測定装置１１２の上部に配設している。
【００２９】
そして、水平方向に所定の距離を開けて配設されたプーリ１２０ａ及びプーリ１２０ｂには、外周面に窪みが設けられており、両者の窪みには、プーリ１２０ａの回転をプーリ１２０ｂに伝えるために、ベルト１２１が架け渡されている。つまり、プーリ１２０ａが回転することでベルト１２１が回転し、プーリ１２０ｂに、この回転駆動力を伝える。
また、プーリ１２０ｂと、第２の回転軸とは、当該第２の回転軸がプーリ１２０ｂの回転に連動して回転するように係合されている。従って、プーリ１２０ｂに伝達された回転駆動力は、第２の回転軸へと伝達される。
【００３０】
つまり、モータ１１６がその回転軸を回転駆動することで、その回転駆動力は、プーリ１２０ａ、ベルト１２１、プーリ１２０ｂを介して、第２の回転軸へと伝達され、図２（ｂ）に示すように、２次元距離測定装置１１２を垂直方向の軸（ｚ軸）まわり回転駆動する。
一方、本実施の形態の２次元距離測定装置１１２における測距センサ１１２ａは、水平方向の軸（ｙ軸）まわりに回転駆動しながら距離を測定する。また、測距センサ１１２ａの走査範囲は、脚車輪型ロボット１００が階段の昇降や障害物の回避を行うことを目的としているため、図３に示すように、主に脚車輪型ロボット１００の下方を重点的に走査する範囲となっている。
【００３１】
３次元距離測定装置２００は、図１に示すように、２次元距離測定装置１１２を回転駆動する駆動機構（モータ１１６、エンコーダ１１８、プーリ１２０ａ及び１２０ｂ、ベルト１２１、その他の支持部材）が、図３に示す走査範囲外に設けられているため、図３に示す走査範囲であれば、３次元距離測定装置２００を構成する各機構部によって、測距センサ１１２ａのｙ軸まわりの走査が阻害されない。
【００３２】
また、脚車輪型ロボット１００の走行経路上の障害物を認識できれば良いので、ｚ軸まわりの回転駆動による走査範囲も、前方１８０[°]までをカバーする必要はなく、２次元距離測定装置１１２の水平方向に設けられたモータ１１６及びエンコーダ１１８を走査範囲外とする範囲でも十分である。従って、本実施の形態においては、２次元距離測定装置１１２の回転駆動範囲を、モータ１１６及びエンコーダ１１８を含まない範囲とする。
【００３３】
次に、図４に基づき、３次元距離測定装置２００の構成を説明する。
図４（ａ）は、３次元距離測定装置２００の構成を示すブロック図であり、（ｂ）は、本実施の形態に係る２次元距離測定装置１１２の詳細な構成を示すブロック図である。
３次元距離測定装置２００は、図４（ａ）に示すように、センシングプロセッサ１１０と、２次元距離測定装置１１２と、指令信号及び後述するエンコーダ１１８の出力信号に基づき後述するモータ１１６を制御するドライバ１１４と、２次元距離測定装置１１２を回転駆動するモータ１１６と、モータ１１６の回転角度位置を検出するエンコーダ１１８とを含んで構成されている。
【００３４】
センシングプロセッサ１１０は、専用のプログラムを実行し、後述するドライバ１１２ｂに指令信号を与えて、後述する測距センサ１１２ａを回転駆動し、その測定平面内に存在する物体上の距離を測定する第１走査処理を実行すると共に、１つの測定平面に対する第１走査処理が終了する毎に、ドライバ１１４に指令信号を与えて、２次元距離測定装置１１２を回転駆動する第２走査処理を実行する。
【００３５】
更に、センシングプロセッサ１１０は、上記第１走査処理及び上記第２走査処理を経て、２次元距離測定装置１１２で測定した距離の情報（以下、距離情報という）に基づき、各測定平面の各測定点の勾配の出現頻度を算出する処理を実行すると共に、当該算出した出現頻度及び各測定点の座標情報に基づき、各測定平面における物体の１次元の形状（凹凸）及び測定範囲における物体の面の形状を認識する処理を実行する。
【００３６】
２次元距離測定装置１１２は、図４（ｂ）に示すように、測定範囲内に存在する物体上の測定点までの距離を測定する測距センサ１１２ａと、指令信号及び後述するエンコーダ１１２ｄの出力信号に基づきモータ１１２ｃの駆動を制御するドライバ１１２ｂと、モータ１１２ｃと、モータ１１２ｃの回転角度位置を検出するエンコーダ１１２ｄとを含んで構成されている。
【００３７】
ドライバ１１２ｂは、センシングプロセッサ１１０からの指令信号において指定された走査角度範囲及び走査角度単位（例えば、０．３６[°]などの所定の回転角度）に基づき、モータ１１２ｃの回転軸を、指定された走査角度単位ずつ回転させる制御を行う。
ここで、測距センサ１１２ａの原点位置（走査角度が０[°]の位置）における、レーザの出力方向の軸をｘ軸、ｘ軸に対して直交する２つの軸のうち一方の軸をｙ軸（本実施の形態では、センサの横方向と水平方向の軸となる）、他方の軸をｚ軸（本実施の形態では、垂直方向の軸となる）とする。
【００３８】
モータ１１２ｃは、測距センサ１１２ａのレーザ出力部（不図示）及び受光部（不図示）を、水平方向の軸（ｙ軸）まわりに回転駆動するように設けられており、ドライバ１１２ｂからの制御信号に応じて、自己の回転軸を指定された走査角度単位（回転角度θ）ずつ回転駆動する。
ここで、図５は、２次元距離測定装置１１２の距離測定の原理を説明する図である。
【００３９】
２次元距離測定装置１１２は、測距センサ１１２ａが、モータ１１２ｃの回転軸の回転駆動に応じて、水平方向の軸（ｙ軸）まわりに指定された走査角度単位ずつ回動し、且つ、回動する毎に、図５に示すように、レーザ出力部からレーザ光を出力すると共に、出力光に対する物体（図５中の障害物）からの反射光を受光部で受光して、各回転角度（走査角度）に応じた距離（図２中の測定距離Ｌ（物体と受光部との間の距離））を測定する。
【００４０】
また、図６（ａ）は、水平方向の軸（ｙ軸）まわりに測距センサ１１２ａを回転駆動させたときの測定距離と回転角度θとの関係を示す図であり、（ｂ）は、垂直方向の軸（ｚ軸）まわりに２次元距離測定装置１１２を回転駆動したときの測定平面と回転角度φとの関係を示す図である。
つまり、上記第１走査処理は、例えば、図６（ａ）に示すように、測距センサ１１２ａを、水平方向の軸（ｙ軸）まわりに指定された走査角度単位ずつ回動させながら、原点位置に対する各回転角度（図６（ａ）中のθ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)は、ステップＳ１３２に移行する。
【００５６】
ステップＳ１２８に移行した場合は、センシングプロセッサ１１０において、ステップＳ１２４においてＲＡＭ等の記録媒体に記録した連続面と判断された測定距離の情報に基づき、面データを生成して、ステップＳ１３０に移行する。
ここで、面データの生成は、例えば、各測定平面間において、連続面と判断された勾配及びその測定点の座標情報とを比較し、座標値と勾配とが近いものを面と判断して、これらの座標情報を対応付けたり、公知の補完法を用いてつなぎ合わせたりすることで行う。
【００５７】
例えば、勾配が０に近い測定点の連続する座標情報から構成される面データの面は、水平面と判断できるので、そこが歩行可能な面だと判断できる。
ステップＳ１３０では、３次元距離測定装置２００において、後述するハブ７６及び通信Ｉ／Ｆ６４を介して、ステップＳ１２８で生成した面データを、ＣＰＵ６０に出力して処理を終了する。
【００５８】
一方、ステップＳ１２６において、全ての測定平面に対して連続面の判断処理が終了しておらず、ステップＳ１３２に移行した場合は、３次元距離測定装置２００において、センシングプロセッサ１１０からの指令信号に応じて、ドライバ１１４、モータ１１６及びエンコーダ１１８を駆動し、２次元距離測定装置１１２を、ステップＳ１００で設定されたｚ軸まわりの走査角度範囲内において、同じくステップＳ１００で設定された走査角度単位ずつｚ軸まわりに回動させる処理（第２走査処理）を実行して、ステップＳ１０２に移行する。
【００５９】
また、ステップＳ１１４において、出現頻度が閾値以上ではなく、ステップＳ１３４に移行した場合は、センシングプロセッサ１１０において、ステップＳ１１２で選択した勾配に対応する測定点の測定距離を測定ノイズとして除外し、ステップＳ１１８に移行する。
次に、図１０に基づき、脚車輪型ロボット１００の移動制御システムを説明する。
【００６０】
ここで、図１０は、脚車輪型ロボット１００の移動制御システムを示すブロック図である。
脚車輪型ロボット１００の４本の各脚部の回転関節には、図１０に示すように、各回転関節を回転駆動する関節モータ４０がそれぞれ設けられている。各関節モータ４０には、関節モータ４０の回転角度位置を検出するエンコーダ４２と、モータ指令信号およびエンコーダ４２の出力信号に基づいて関節モータ４０の駆動を制御するドライバ４４が設けられている。
【００６１】
各脚部の駆動輪には、駆動輪を回転駆動する車輪モータ５０がそれぞれ設けられている。各車輪モータ５０には、車輪モータ５０の回転角度位置を検出するエンコーダ５２と、モータ指令信号およびエンコーダ５２の出力信号に基づいて車輪モータ５０の駆動を制御するドライバ５４が設けられている。
脚車輪型ロボット１００は、さらに、ＣＰＵ６０と、脚車輪型ロボット１００の姿勢を検出する３軸姿勢センサ７０と、カメラ３２の画像信号を処理するビジョンプロセッサ７２と、３次元距離測定装置２００と、外部のＰＣ等と無線通信を行う無線通信部７４と、ビジョンプロセッサ７２、無線通信部７４およびセンシングプロセッサ１１０とＣＰＵ６０の入出力を中継するハブ７６と、警告音等を出力するスピーカ７８とを有して構成される。
【００６２】
３軸姿勢センサ７０は、ジャイロ若しくは加速度センサ、またはその両方を有し、地軸に対して脚車輪型ロボット１００の姿勢の傾きを検出する。
ＣＰＵ６０は、モータ指令出力Ｉ／Ｆ６１を介してドライバ４４、５４にモータ指令信号を出力し、角度取込Ｉ／Ｆ６２を介してエンコーダ４２、５２の出力信号を入力する。なお、図示しないが、モータ指令出力Ｉ／Ｆ６１を介してモータ８２のドライバにモータ指令信号を出力する。
【００６３】
ＣＰＵ６０は、センサ入力Ｉ／Ｆ６３を介して、脚先センサ３８、３次元距離測定装置２００および３軸姿勢センサ７０からそれぞれセンサ信号を入力する。また、通信Ｉ／Ｆ６４を介してハブ７６と信号の入出力を行い、サウンド出力Ｉ／Ｆ６５を介してスピーカ７８に音声信号を出力する。
次に、ＣＰＵ６０で実行される処理を説明する。
【００６４】
ＣＰＵ６０は、３次元距離測定装置２００から面データが入力されると、そのセンサ座標系の座標をグローバル座標系に変換する座標変換処理を行う。座標変換処理では、脚車輪型ロボット１００に対する中心のグローバル座標をＸc、Ｙc、Ｚc、脚車輪型ロボット１００の姿勢（ロール角、ピッチ角、ヨー角）をφ、θ、ψとして、センサ座標系における各測定点を、グローバル座標系の測定点（Ｘs、Ｙs、Ｚs）に変換する。
【００６５】
更に、３軸姿勢センサ７０のセンサ信号、並びにグローバル座標系に変換された各測定点に基づいて逆運動学計算および重心計算を行い、この計算結果に基づいて脚先（駆動輪）の着地位置を決定する。更に、物体平面までの距離を算出し、脚先と物体平面の位置関係を算出し、決定した着地位置および算出した両距離に基づいてドライバ４４、５４へのモータ指令信号を生成し、生成したモータ指令信号をドライバ４４、５４に出力する。
【００６６】
更に、脚先が物体平面に着地したか否かを判定し、脚先が着地したと判定したときは、一連の処理を終了して元の処理に復帰させる。 一方、脚先が着地しないと判定したときは、面データを入力する処理から上記一連の処理を行う。
次に、図１１〜図１４に基づき、本実施の形態の動作を説明する。
ここで、図１１は、測定範囲内に階段がある場合の、第１走査処理による測定結果を示す図である。また、図１２は、図１１の各測定点の回転座標系の測定距離を直交座標系の測定距離に変換した場合の測定結果を示す図である。また、図１３（ａ）及び（ｂ）は、最小二乗法を用いて算出したｘ方向及びｚ方向の勾配の一例を示す図である。また、図１４は、ある測定平面に対する勾配の出現頻度の一例を示す図である。
【００６７】
まず、３次元距離測定装置２００は、ＣＰＵ６０からの指令信号に応じて、センシングプロセッサ１１０において走査角度範囲及び走査角度単位を設定する（ステップＳ１００）。
ここで、２次元距離測定装置１１２は、測距範囲２０〜４０９５[ｍｍ]、最大走査角度範囲２４０[°]、角度分解能０．３６[°]の２次元レンジセンサであることとする。走査角度範囲が２４０[°]であれば、先述した図３に示す測定平面を形成する走査角度範囲と同じとなり、２次元距離測定装置１１２の上部にある支持部材、プーリ２０及びベルト１２１が走査範囲に含まれないことになる。
【００６８】
また、第１走査処理に対して、走査角度範囲２４０[°]及び走査角度単位０．３６[°]が設定され、第２走査処理に対して、走査角度範囲−４０[°]〜＋４０[°]及び走査角度単位１０[°]が設定されたとする（この場合は、測定平面が９つ形成される）。走査角度範囲−４０[°]〜＋４０[°]であれば、２次元距離測定装置１１２の水平方向に並べて設けられたモータ１１６及びエンコーダ１１８が走査範囲に含まれないことになる。
【００６９】
センシングプロセッサ１１０は、まず、第１走査処理に対して設定された走査角度範囲及び走査角度単位（θ）に基づき、ドライバ１１２ｂに指令信号を出力する。
２次元距離測定装置１１２は、ドライバ１１２ｂにおいて、センシングプロセッサ１１０からの指令信号及びエンコーダ１１２ｄからの出力信号に基づき、モータ１１２ｃの回転軸を回転駆動して、測距センサ１１２ａを、水平方向の軸（ｙ軸）まわりに、回転角度「θ＝０．３６[°]」ずつ回動すると共に、各回転角度に応じた距離を測定する処理（第１走査処理）を実行する（ステップＳ１０２）。最初は、垂直方向の軸（ｚ軸）まわりに対して回転角度φが０[°]の位置（初期位置）に対する第１走査処理を実行する。また、各測定距離の情報（回転座標系）は、データ列Ｌ（θi，φj）として、センシングプロセッサ１１０へと出力される。
【００７０】
なお、水平方向の軸（ｙ軸）まわりの走査範囲内には、ロボットの基体以外に、２次元距離測定装置１１２の駆動機構等の走査を阻害する物が一切存在しないため、走査範囲内に存在する物体の正確な距離情報を得ることができる。
そして、１つの測定平面に対する第１走査処理が終了すると、センシングプロセッサ１１０は、第１走査処理において測定された距離情報に対して、メディアンフィルタを用いたフィルタリング処理を行う（ステップＳ１０４）。これにより、測定情報におけるノイズ成分を除去する。
【００７１】
ここで、ノイズ成分除去後の各測定点の測定距離Ｌ[ｍｍ]は、例えば、図１１に示すようになる。図１１において、横軸は各走査角度に応じた測定点の番号（第１走査角度番号）であり、縦軸は各走査角度番号の測定点に対する測定距離Ｌ[ｍｍ]である。
図１１に示す例では、脚車輪型ロボット１００の歩行経路上に、段差が一定で且つ蹴込板が連続面となっている階段が存在している。
【００７２】
次に、センシングプロセッサ１１０は、フィルタリング処理後の回転座標系の距離情報を、直交座標系の距離情報に変換する（ステップＳ１０６）。
つまり、図１１に示す各測定点の回転座標系の測定距離を、直交座標系の測定距離へと変換することで、図１２に示すように、各回転角度に対応するｘ軸方向の距離[ｍｍ]とｚ軸方向の距離[ｍｍ]とで表現される２次元の座標情報となる。図１２において、横軸はｘ軸方向の距離Ｌx[ｍｍ]であり、縦軸はｚ軸方向の距離Ｌz[ｍｍ]である。
【００７３】
次に、直交座標系に変換された距離情報に基づき、各測定点の勾配を算出する（ステップＳ１０８）。
ここでは、最小二乗法を用いて、各測定点の勾配を算出する。ここで、最小二乗法を用いた場合に、図１３（ａ）及び（ｂ）に示すように、最小二乗法を適用する複数の測定点に対して、ｘ方向の勾配と、ｚ方向の勾配とが異なる場合がある。そこで、本実施の形態においては、下式（１）及び（２）に示す、ｘ方向及びｚ方向の勾配を求める２つの直線の方程式を用いて最小二乗法計算を行う。
ｚ＝ａx・ｘ＋ｂx ・・・（１）
ｘ＝ａz・ｚ＋ｂz ・・・（２）
そして、上式（１）におけるａx及びｂxを、下式（３）に従って算出し、上式（２）におけるａz及びｂzを、下式（４）に従って算出する。
【００７４】
【数１】
【００７５】
ａx及びｂx、並びにａz及びｂzを算出すると、これらの算出結果を代入した、上式（１）及び（２）の直線と、最小二乗計算に用いた各測定点との距離を残差ｈx及びｈzとして算出すると共に、前記各測定点に対する残差ｈxの二乗和と、残差ｈzの二乗和とを算出する（下式（５））。
【００７６】
【数２】
【００７７】
残差ｈxの二乗和及び残差ｈzの二乗和が算出されると、両者を比較して値の小さい方の直線を、正しい直線として採用し、この直線の勾配に基づき、算出対象の測定点の勾配を求める。具体的に、残差ｈxの二乗和の方が小さければ、「ａ＝ａx、ｂ＝ｂx」とし、残差ｈzの二乗和の方が小さければ、「ａ＝１／ａz、ｂ＝１／ｂz」とする。
【００７８】
上記のようにして、測定平面に対する全測定点の勾配が算出されると、次に、各勾配の出現頻度を算出する（ステップＳ１１０）。
ここでは、図１４に示すような、出現頻度が得られたとする。
図１４に示すように、勾配が０近傍の出現頻度が比較的高くなっており、これらの勾配に対応する測定点の座標情報が連続していれば、そこに水平面があると推定することができる。
【００７９】
出現頻度が算出されると、センシングプロセッサ１１０は、次に、未判定の勾配の出現頻度を選択し（ステップＳ１１２）、その出現頻度が、予め設定された閾値以上か否かを判定する（ステップＳ１１４）。そして、閾値以上であれば（ステップＳ１１４の「Ｙｅｓ」の分岐）、勾配と測定点の座標情報とを対応付けてＲＡＭ（不図示）に記録する（ステップＳ１１６）。一方、閾値未満であれば（ステップＳ１１４の「Ｎｏ」の分岐）、測定ノイズとして、測定点の距離情報を除外する（ステップＳ１３４）。
【００８０】
上記の判定処理が、全ての勾配に対して終了すると（ステップＳ１１８の「Ｙｅｓ」の分岐）、次に、ＲＡＭに記録された、出現頻度が閾値以上の勾配及び測定点の座標情報に基づき、連続面の有無を判断する（ステップＳ１２０）。
脚車輪型ロボット１００の移動経路上には、段差が一定で且つ蹴込板が連続面となっている階段が存在しているため、階段部分にある水平面などが、勾配０及び略０の測定点の連続する座標として現われる。このような場合に、センシングプロセッサ１１０は、連続面（実際は、面と測定平面との交差線）がある（ステップＳ１２２の「Ｙｅｓ」の分岐）と判断する。一方、座標の途中で勾配がいきなり外れた値となったときは、そこで面（実際は、面と走査平面との交差線）が途切れていると判断する。これらの判断結果から、物体の凹凸を認識することが可能である。
【００８１】
そして、連続面に対応する直交座標系の測定距離を、センサ座標系（３次元）の測定距離に変換し、ＲＡＭに記録する（ステップＳ１２４）。
連続面の判断が終了すると、３次元距離測定装置２００は、ドライバ１１４において、センシングプロセッサ１１０からの指令信号及びエンコーダ１１８からの出力信号に基づき、モータ１１６の回転軸を回転駆動して、２次元距離測定装置１１２を、垂直方向の軸（ｚ軸）まわりに、回転角度１０[°]ずつ回動する処理（第２走査処理）を実行する（ステップＳ１３２）。この第２走査処理によって、ｚ軸はそのままで、ｘ軸の位置が１つ前の位置に対して垂直方向の軸（ｚ軸）まわりに１０[°]だけ変化する。そして、２次元距離測定装置１１２は、この状態で、再び、上記第１走査処理を実行する（ステップＳ１０２）。これにより、ｚ軸まわりに１０[°]ずれた位置に新たな測定平面が形成される。
【００８２】
そして、設定された走査角度範囲（−４０[°]〜＋４０[°]）に対して、設定された走査角度単位１０[°]で、２次元距離測定装置１１２を回動させ、各回転角度φにおいて、上記第１走査処理を実行する。
ここで、２次元距離測定装置１１２が脚車輪型ロボット１００の基体上部に取り付けられているため、当該基体が走査範囲内に含まれてしまうが、脚車輪型ロボット１００の前方及び歩行経路上を含む範囲においては阻害物が一切ないため、脚車輪型ロボット１００の歩行制御を行うのに十分な走査範囲が確保できているといえる。
【００８３】
そして、９つの測定平面に対する距離の測定が終了すると（ステップＳ１２６の「Ｙｅｓ」の分岐）、次に、ＲＡＭに記録された連続面の情報に基づき、面データを生成する（ステップＳ１２８）。
面データは、先述したように、ｚ軸まわりに連続する各隣り合う測定平面間において、勾配及び座標の近い連続面をつなぎ合わせる（対応付ける）ことで生成される。生成した面データは、ハブ７６および通信Ｉ／Ｆ６４を介して、ＣＰＵ６０に出力される（ステップＳ１３０）。この面データによって、測定範囲内に存在する階段などの物体の構成面の詳細な形状を認識することが可能である。
【００８４】
一方、ＣＰＵ６０では、入力された面データを解析して、特に、勾配が０に近い交差線を有する面がある場合は、それを水平面とみなし、脚車輪型ロボット１００が歩行可能な面であると判断する。
面データの示す面が歩行可能な面であると判断された場合は、入力された面データのセンサ座標系をグローバル座標系に変換し、この変換結果と、３軸姿勢センサ７０のセンサ信号などとに基づき、脚先の着地位置を決定する。
【００８５】
更に、脚先センサ３８からのセンサ信号に基づき、例えば、対象が階段である場合は、蹴込板までの距離および脚先と踏板の位置関係とを算出する。そして、決定された着地位置および算出された両距離に基づいてモータ指令信号が生成され、生成されたモータ指令信号がドライバ４４に出力される。これにより、回転関節が駆動し、脚車輪型ロボット１００が姿勢を適切に保ちつつ階段を乗り越える。また、状況によっては階段を回避、停止する。
【００８６】
なお、段差が一定で且つ蹴込板が連続面となる階段を例に挙げたが、段差が一定でない階段、蹴込板が連続面とならない階段、蹴込板の無い階段などに対しても、正確にその面形状を認識することができるので、脚車輪型ロボット１００の階段への適応性を高めることが可能である。
また、脚車輪型ロボット１００は、平地では、車輪走行で移動することができる。したがって、車輪型と同様に平地での移動性が高い。
【００８７】
上記したように、本実施の形態の脚車輪型ロボット１００は、その３次元距離測定装置２００の機械的な構成を、モータ１１６及びエンコーダ１１８と、２次元距離測定装置１１２とを水平方向に並べて配設し、モータ１１６の第１の回転軸の回転駆動力を、プーリ１２０ａ、ベルト１２１、プーリ１２０ｂを介して、２次元距離測定装置１１２を回転する第２回転軸へと伝達し、２次元距離測定装置１１２を、垂直方向の軸（ｚ軸）まわりに回転駆動する構成とした。
【００８８】
これにより、２次元距離測定装置１１２の所定走査角度範囲内には、走査を除外するものが一切なくなるので、正確な距離情報を得ることができる。また、モータ１１６及びエンコーダ１１８と、２次元距離測定装置１１２とを水平方向に並べて配設したので、３次元距離測定装置２００の高さ方向の占有率を低減することができる。
また、３次元距離測定装置２００において、水平方向の軸まわりに測距センサ１１２ａを、設定された走査角度単位ずつ回動させると共に、各回転角度における距離を測定する第１走査処理を実行し、測定距離の情報に基づき、各測定点の勾配及び勾配の出現頻度を算出することで、測定平面内に存在する物体の凹凸を認識することができる。
【００８９】
これにより、連続面が存在することを推定することができる。
更に、１つの測定平面に対する第１走査処理が終了する毎に、２次元距離測定装置１１２を、垂直方向の軸まわりに、設定された走査角度単位だけ回動させる第２走査処理を実行することができる。これにより、複数の測定平面に対して第１走査処理を実行することができるので、測定範囲内に存在する物体の面形状を認識することができる。また、面形状が認識できるので、脚車輪型ロボット１００は、未知の障害物に対して高い適応性を実現することができる。また、人が活動する環境での動作を行えるので、人と一緒に行動する用途に用いられるホームロボット、パーソナルロボットなどのヒューマンアシストロボットに好適である。
【００９０】
上記第１の実施の形態において、モータ１１６及びエンコーダ１１８は、発明２、３、４及び５のいずれか１の回転駆動装置に対応し、プーリ１２０ａ、ベルト１２１及びプーリ１２０ｂによって、モータ１１６の第１の回転軸の回転駆動力を２次元距離測定装置１１２の第２の回転軸に伝達する機構は、発明２又は３の動力伝達機構に対応し、プーリ１２０ａ及び１２０ｂは、発明３の滑車に対応する。
【００９１】
また、上記第１の実施の形態において、ドライバ１１２ｂ、モータ１１２ｃ及びエンコーダ１１２ｄによる測距センサ１１２ａの回転機構は、発明１、２、５及び６のいずれか１の第１の回転機構に対応し、センシングプロセッサ１１０によって２次元距離測定装置１１２を制御することで第１走査処理を実行する機能は、発明５又は６の第１走査手段に対応し、センシングプロセッサ１１０における各測定点の勾配を算出する機能は、発明５又は６の勾配算出手段に対応し、センシングプロセッサ１１０における各勾配の出現頻度を算出する機能は、発明５又は６の出現頻度算出手段に対応し、センシングプロセッサ１１０における連続面の判断処理は、発明５の形状認識手段に対応する。
【００９２】
また、上記第１の実施の形態において、ドライバ１１４、モータ１１６、エンコーダ１１８、不図示の減速機及びプーリによる２次元距離測定装置１１２の回転機構は、発明１、２及び６のいずれか１の第２の回転機構に対応し、センシングプロセッサ１１０によってドライバ１１４を制御することで第２走査処理を実行する機能は、発明６の第２走査手段に対応し、３次元距離測定装置２００における面データの生成機能及びＣＰＵ６０による面形状の認識機能は、発明６の形状認識手段に対応する。
【００９３】
また、上記第１の実施の形態において、ステップＳ１０２は、発明５又は６の第１走査手段に対応し、ステップＳ１０４〜Ｓ１０８は、発明５又は６の勾配算出手段に対応し、ステップＳ１１０は、発明５又は６の出現頻度算出手段に対応し、ステップＳ１１２〜Ｓ１２４は、発明５又は６の形状認識手段に対応し、ステップＳ１２６〜Ｓ１３２は、発明６の第２走査手段に対応する。
〔第２の実施の形態〕
次に、本発明の第２の実施の形態を図面を参照しながら説明する。図１６ないし図２２は、本発明に係る３次元距離測定装置及び脚車輪型ロボットの第２の実施の形態を示す図である。
【００９４】
本実施の形態においては、上記第１の実施の形態と同様に、上記第１の実施の形態で説明した３次元距離測定装置２００を脚車輪型ロボットに適用する。
本実施の形態の脚車輪型ロボットは、上記第１の実施の形態で説明した３次元距離測定装置２００を搭載した脚車輪型ロボット１００の各機能に加え、脚車輪型ロボット１００を、基体１０の向きを一定方向に保持した状態で任意の進行方向に走行移動させる機能と、脚車輪型ロボット１００を、所定の旋回中心位置で前後移動させずに旋回させる（超信地旋回させる）機能とを有している。
【００９５】
つまり、本実施の形態における脚車輪型ロボット１００は、上記各機能を実現するためのアクチュエータの制御処理（ＣＰＵ６０の制御プログラムの実行による制御処理）が追加されたのみで、力センサ８２の設置構造など、その他の構成は、上記第１の実施の形態の脚車輪型ロボット１００と同様となる。従って、上記第１の実施の形態と同様の機能については説明を適宜省略し、追加された機能部分については詳細に説明する。
【００９６】
まず、本発明を適用する脚車輪型ロボット１００の詳細な構成を説明する。
図１６は、脚車輪型ロボット１００の正面図である。
図１７は、脚車輪型ロボット１００の側面図である。
脚車輪型ロボット１００は、図１６および図１７に示すように、基体１０と、基体１０に連結された４つの脚部１２とを有して構成されている。
【００９７】
基体１０の前方には、２本の脚部１２が回転関節１４を介して左右対称の位置に連結されている。また、基体１０の後方には、２本の脚部１２が回転関節１４を介して左右対称の位置に連結されている。
回転関節１４は、脚車輪型ロボット１００の底面と直交する方向を軸方向として回転する。すなわち、ヨー軸回りに回転する。
【００９８】
各脚部１２には、２つの回転関節１６、１８が設けられている。回転関節１６、１８は、回転関節１４が図１６のような状態であるときは、脚車輪型ロボット１００の側面と直交する方向を軸方向として回転する。したがって、脚部１２は、それぞれ３自由度を有する。
各脚部１２の先端には、回転関節１６、１８と軸方向を同一にして駆動輪２０が回転可能に設けられている。駆動輪２０は、回転関節１４の回転によりヨー軸周りに回動する。つまり、回転関節１４の回転を制御することで、走行移動時の操舵制御が行われる。
【００９９】
各脚部１２の先端には、脚車輪型ロボット１００の移動経路上に存在する物体までの脚先からの距離を測定する脚先センサ３８が設けられている。
一方、基体１０の正面の下部中央には、水平面レーザ光を照射する水平レーザ２６が設けられている。また、基体１０の正面の中央左右には、垂直面レーザ光を照射する垂直レーザ２８、３０がそれぞれ設けられている。
【０１００】
基体１０の正面の上部中央には、水平面レーザ光および垂直面レーザ光の反射光を含む画像を撮影するカメラ３２が設けられている。
また、上記第１の実施の形態で述べたように、カメラ３２と３次元距離測定装置２００とは、支持部材及びプーリを介して、カメラ３２が上側に、３次元距離測定装置２００が下側に配設されている。
【０１０１】
水平レーザ２６は、カメラ３２で水平面レーザ光の反射光を含む画像が撮影できるように下方に所定角度傾けて設けられている。同様に、垂直レーザ２８は、カメラ３２で垂直面レーザ光の反射光を含む画像が撮影できるように右方に所定角度傾けて設けられ、垂直レーザ３０は、左方に所定角度傾けて設けられている。
カメラ３２の左右には、障害物を検出する障害物センサ３４、３６がそれぞれ設けられている。
【０１０２】
なお、各脚部１２の回転関節１４〜１８には、上記第１の実施の形態における図１０に示すように、回転関節１４〜１８を回転駆動する関節モータ４０がそれぞれ設けられている。また、各脚部１２の駆動輪２０には、図１０に示すように、駆動輪２０を回転駆動する車輪モータ５０がそれぞれ設けられている。
次に、図１８ないし図２０に基づき、本実施の形態のＣＰＵ６０で実行される走行制御処理を説明する。
【０１０３】
ここで、図１８（ａ）及び（ｂ）は、脚車輪型ロボット１００の車輪走行移動時の姿勢を示す図である。
以下の、各走行制御処理においては、脚車輪型ロボット１００の姿勢が、図１８（ｂ）に示すように、膝屈曲姿勢となるように各関節モータ４０を制御する。但し、膝屈曲姿勢における走行制御時に、脚部１２が互いに干渉する（接触などする）場合は、図１８（ａ）に示すように、膝伸展姿勢となるように各関節モータ４０を制御する。
【０１０４】
まず、基体１０の向きを一定の方向に保持（固定）した状態で、脚車輪型ロボット１００を目的の進行方向に向けて走行させる走行制御処理（以下、無変向走行制御処理と称す）について説明する。
ＣＰＵ６０は、ＲＯＭ等の所定領域に格納されている制御プログラムを起動させ、その制御プログラムに従って、無変向走行制御処理を実行する。
【０１０５】
ここで、無変向走行制御処理は、脚車輪型ロボット１００を、その基体１０の向きを一定の方向に保持した状態で、目的の進行方向に移動させるものであるため、基体１０の前方側に設けられたカメラ３２、３次元距離測定装置２００、障害物センサ３４、３６などを、後方や側方などにも設けることが望ましい。これら後方及び側方をカバーできるカメラ、３次元距離測定装置及び障害物センサなどによって、脚車輪型ロボット１００を、基体１０の向きとは異なる方向に移動させるときに、その進行方向の環境（地形の状態等）を把握し、適切な制御を行うことができる。
【０１０６】
また、無変向走行制御処理は、具体的に、無変向走行制御指令があったときに実行され、時々刻々の、ロボットの進行方向（角度α）、ロボット進行方向速度Ｖcを入力として、基体１０の向きを一定の方向に保持した状態で、脚車輪型ロボット１００を前記入力された進行方向へと走行させるための、各脚部１２の回転関節１４（joint0）の角度（操舵角度）θi0（ｉ=0,1,2,3,・・・）、各脚部１２の各駆動輪２０の回転角速度ωi（ｉ=0,1,2,3,・・・）を算出し、各アクチュエータに指令を与えるものである。
【０１０７】
本実施の形態の脚車輪型ロボット１００は、先述したように、基体１０の前方に左右一対及び後方に左右一対の計４本の脚部１２を有している。
従って、ここでは、各脚部１２の駆動輪２０の操舵角度θi0を、基体１０の上面側から見て、左前輪操舵角度θ00、右前輪操舵角度θ10、左後輪操舵角度θ20、右後輪操舵角度θ30とする。なお、回転関節１４によって各脚部１２をヨー軸周りに回動させたときに、基体１０の上面側から見て、左前輪操舵角度θ00及び右後輪操舵角度θ30は、反時計回り方向を正方向とし、右前輪操舵角度θ10及び左後輪操舵角度θ20は、時計回り方向を正方向とする。
【０１０８】
また、各脚部１２の駆動輪２０の回転角速度ωiを、左前輪回転角速度ω0、右前輪回転角速度ω1、左後輪回転角速度ω2、右後輪回転角速度ω3とする。
また、各脚部１２の駆動輪２０の線速度Ｖi（ｉ=0,1,2,3,・・・）を、左前輪線速度Ｖ0、右前輪線速度Ｖ1、左後輪線速度Ｖ2、右後輪線速度Ｖ3とする。
ここで、図１９（ａ）〜（ｃ）は、無変向走行制御時の脚車輪型ロボット１００の走行状態例を示す図である。なお、図１９（ａ）〜（ｃ）は、脚車輪型ロボット１００を上面側から見た図であり、各駆動輪２０に付けられた黒塗りの半円の目印は、「θ00＝θ10＝θ20＝θ30＝０[°]」のときの基準となる向きを示す。
【０１０９】
また、左右前輪は上記目印のある方向に進行する回転方向が正回転方向となり、左右後輪は目印の無い方向に進行する回転方向が正回転方向となる。
まず、図１９（ａ）に基づき、基体１０の向きを一定方向に保持（固定）した状態で、脚車輪型ロボット１００を、基体１０の向いている方向（前方向）に直進走行させる場合の無変向走行制御処理を説明する。
【０１１０】
本実施の形態では、基体１０の向いている方向（前方向）をロボットの進行方向とした場合に、進行方向を表す角度αを「０[°]」とする。そして、前方向の０[°]を基準に、各進行方向に対応するαを決定する。
ここでは、脚車輪型ロボット１００を、基体１０の前方向に直進させるので、進行方向αとして「０[°]」が入力され、更に、進行方向速度Ｖｃが入力される。
【０１１１】
進行方向α（０[°]）及び進行方向速度Ｖｃが入力されると、各駆動輪２０の操舵角度θ00、θ10、θ20、θ30、及び各駆動輪２０の線速度Ｖ0、Ｖ1、Ｖ2、Ｖ3が算出される。
基体１０の向きを保持した状態のまま、脚車輪型ロボット１００を前方向に直進走行させるためには、図１９（ａ）の各駆動輪２０から伸びる矢印に示すように、各駆動輪２０の進行方向を基体１０の向いている方向（前方向）に全て揃える必要がある。従って、操舵角度θ00、θ10、θ20、θ30は、例えば、「θ00＝θ10＝θ20＝θ30＝０[°]」と算出される。
【０１１２】
また、この場合に、直進走行させるための各駆動輪２０の線速度Ｖ0、Ｖ1、Ｖ2、Ｖ3は、「Ｖ0＝Ｖ1＝Ｖｃ」、「Ｖ2＝Ｖ3＝−Ｖｃ」と算出される。
なお、各脚部１２が互いに干渉しなければ、例えば、「θ00＝θ10＝０[°]」、「θ20＝θ30＝π（１８０[°]）又は−π（−１８０[°]）」、「Ｖ0＝Ｖ1＝Ｖ2＝Ｖ3＝Ｖｃ」などの組み合わせとしても良い。
【０１１３】
更に、線速度Ｖ0、Ｖ1、Ｖ2、Ｖ3は、下式（６）に従って、回転角速度ω0、ω1、ω2、ω3に変換される。
ωi＝２Ｖi／Ｄ ・・・（６）
但し、上式（６）において、Ｄは車輪径である。
【０１１４】
各駆動輪２０の操舵角度θ00、θ10、θ20、θ30、及び各駆動輪２０の回転角速度ω0、ω1、ω2、ω3が算出されると、次に、各駆動輪２０の現在の操舵角度及び回転角速度を取得する。そして、現在の操舵角度と、上記算出した前方向に直進させるための操舵角度とから回転関節１４の関節モータ４０を駆動する指令値を算出する。更に、現在の回転角速度と、上記算出した前方向に直進させるときの回転角速度とから、駆動輪２０の車輪モータ５０を駆動する指令値を算出する。
【０１１５】
このようにして、各駆動輪２０の操舵制御の指令値及び速度制御の指令値を算出すると、これらの指令値を各モータのドライバに入力する。そして、入力された指令値に基づき各関節モータ４０及び車輪モータ５０が駆動されると、これにより、脚車輪型ロボット１００の各駆動輪２０の操舵角度及び回転角速度が変化し、脚車輪型ロボット１００は、基体１０の向きを保持した状態で、その向いている方向（前方向）に直進走行する。
【０１１６】
なお、基体１０の向きを一定方向に保持した状態で、脚車輪型ロボット１００を、その向いている方向に対して反対側（真後ろ）の方向に直進走行させる場合は、上記前方向のときと駆動輪２０の回転方向を正反対とすればよい。
例えば、「θ00＝θ10＝θ20＝θ30＝０[°]」及び「Ｖ0＝Ｖ1＝Ｖ2＝Ｖ3＝−Ｖｃ」、又は「θ00＝θ10＝０[°]」、「θ20＝θ30＝π若しくは−π[°]」、「「Ｖ0＝Ｖ1＝−Ｖｃ」及び「Ｖ2＝Ｖ3＝Ｖｃ」などとする。
【０１１７】
次に、図１９（ｂ）に基づき、基体１０の向きを保持した状態（前方向に向けたままの状態）で、脚車輪型ロボット１００を、その向いている方向に対して右斜め前方向に直進走行させる場合の無変向走行制御処理を説明する。
ここでは、脚車輪型ロボット１００を、基体１０の右斜め前方向に直進させるので、進行方向αとして「α（−９０＜α＜０）[°]（但し、反時計回りが正方向）」が入力され、更に、進行方向速度Ｖｃが入力される。
【０１１８】
そして、進行方向α[°]及び進行方向速度Ｖｃが入力されると、各駆動輪２０の操舵角度θ00、θ10、θ20、θ30、及び各駆動輪２０の線速度Ｖ0、Ｖ1、Ｖ2、Ｖ3が算出される。
基体１０の向きを保持した状態で、脚車輪型ロボット１００を右斜め前方向（α[°]の方向）に直進走行させるためには、図１９（ｂ）の各駆動輪２０から伸びる矢印に示すように、各駆動輪２０の進行方向を、基体１０の向いている方向に対して右斜め前方向に全て揃える必要がある。
【０１１９】
従って、操舵角度θ00、θ10、θ20、θ30は、例えば、「θ00＝θ30＝α[°]」、「θ10＝θ20＝−α[°]」と算出される。
また、この場合に、直進走行させるための各駆動輪２０の線速度Ｖ0、Ｖ1、Ｖ2、Ｖ3は、「Ｖ0＝Ｖ1＝Ｖｃ」、「Ｖ2＝Ｖ3＝−Ｖｃ」と算出される。
なお、各脚部１２が互いに干渉しなければ、例えば、「θ00＝θ30＝α[°]」、「θ10＝θ20＝−α−π[°]」、「Ｖ0＝Ｖ2＝Ｖｃ」、「Ｖ1＝Ｖ3＝−Ｖｃ」などの組み合わせとしても良い。
【０１２０】
更に、線速度Ｖ0、Ｖ1、Ｖ2、Ｖ3は、上式（６）に従って、回転角速度ω0、ω1、ω2、ω3に変換される。
操舵角度θ00、θ10、θ20、θ30、及び各駆動輪２０の回転角速度ω0、ω1、ω2、ω3が算出されると、次に、上記前方向への直進走行のときと同様に、回転関節１４の関節モータ４０を駆動する指令値と、駆動輪２０の車輪モータ５０を駆動する指令値とを算出する。
【０１２１】
このようにして、各駆動輪２０の操舵制御の指令値及び速度制御の指令値を算出すると、これらの指令値を各モータのドライバに入力する。そして、入力された指令値に基づき各関節モータ４０及び車輪モータ５０が駆動されると、これにより、脚車輪型ロボット１００の各駆動輪２０の操舵角度及び回転角速度が変化し、脚車輪型ロボット１００は、基体１０の向きを保持した状態で、その向いている方向に対して右斜め前方向に直進走行する。
【０１２２】
なお、基体１０の向きを保持した状態で、脚車輪型ロボット１００を、その向いている方向に対して左斜め後方に直進走行させる場合は、上記右斜め前方向のときの各駆動輪２０の回転方向を正反対とすればよい。また、左斜め前方向に直進移動させる場合は、進行方向αを「０＜α＜９０[°]」の範囲で設定し、更に、操舵角度の符号を正反対とすればよい。また、右斜め後方に直進走行させる場合は、左斜め前方向のときの各駆動輪２０の回転方向を正反対とすればよい。
【０１２３】
次に、図１９（ｃ）に基づき、基体１０の向きを保持した状態で、脚車輪型ロボット１００を、その向いている方向に対して左真横方向に直進走行させる場合の無変向走行制御処理を説明する。
ここでは、脚車輪型ロボット１００を、基体１０の左真横方向に直進させるので、進行方向αとして「α（９０（π／２））[°]」が入力され、更に、進行方向速度Ｖｃが入力される。
【０１２４】
そして、進行方向α[°]及び進行方向速度Ｖｃが入力されると、各駆動輪２０の操舵角度θ00、θ10、θ20、θ30、及び各駆動輪２０の線速度Ｖ0、Ｖ1、Ｖ2、Ｖ3が算出される。
基体１０の向きを保持した状態で、脚車輪型ロボット１００を、その向いている方向に対して左真横方向（π／２[°]の方向）に直進走行させるためには、図１９（ｃ）の各駆動輪２０から伸びる矢印に示すように、各駆動輪２０の進行方向を左真横方向に全て揃える必要がある。
【０１２５】
従って、操舵角度θ00、θ10、θ20、θ30は、例えば、「θ00＝θ10＝θ20＝θ30＝π／２[°]」と算出される。
また、この場合に、直進走行させるための各駆動輪２０の線速度Ｖ0、Ｖ1、Ｖ2、Ｖ3は、「Ｖ0＝Ｖ2＝Ｖｃ」、「Ｖ1＝Ｖ3＝−Ｖｃ」と算出される。
なお、各脚部１２が互いに干渉しなければ、例えば、「θ00＝θ20＝π／２[°]」、「θ10＝θ30＝−π／２[°]」、「Ｖ0＝Ｖ1＝Ｖ2＝Ｖ3＝Ｖｃ」などの組み合わせとしても良い。
【０１２６】
更に、線速度Ｖ0、Ｖ1、Ｖ2、Ｖ3は、上式（６）に従って、回転角速度ω0、ω1、ω2、ω3に変換される。
操舵角度θ00、θ10、θ20、θ30、及び各駆動輪２０の回転角速度ω0、ω1、ω2、ω3が算出されると、次に、上記前方向への直進走行のときと同様に、回転関節１４の関節モータ４０を駆動する指令値と、駆動輪２０の車輪モータ５０を駆動する指令値とを算出する。
【０１２７】
このようにして、各駆動輪２０の操舵制御の指令値及び速度制御の指令値を算出すると、これらの指令値を各モータのドライバに入力する。そして、入力された指令値に基づき各関節モータ４０及び車輪モータ５０が駆動されると、これにより、脚車輪型ロボット１００の各駆動輪２０の操舵角度及び回転角速度が変化し、脚車輪型ロボット１００は、基体１０の向きを保持した状態で、その向いている方向に対して左真横方向に直進走行する。
【０１２８】
なお、基体１０の向きを保持した状態で、脚車輪型ロボット１００を、その向いている方向に対して右真横方向に直進走行させる場合は、上記左真横方向のときの各駆動輪２０の回転方向を正反対とすればよい。
次に、脚車輪型ロボット１００を、所定の旋回中心位置で前後移動させずに旋回させる走行制御処理（以下、超信地旋回制御処理と称す）について説明する。
【０１２９】
ＣＰＵ６０は、ＲＯＭ等の所定領域に格納されている制御プログラムを起動させ、その制御プログラムに従って、超信地旋回制御処理を実行する。
ここで、図２０（ａ）及び（ｂ）は、基体１０の座標（０，０）及び座標（ｘｃ，ｙｃ）を回転中心とした場合の超信地旋回制御時の脚車輪型ロボット１００の走行状態を示す図である。
【０１３０】
なお、図２０（ａ）及び（ｂ）においては、基体１０を上面側から見た平面において、長手方向の軸をｘ軸、それと直交する方向の軸をｙ軸とし、基体１０の中心位置の座標を（ｘ，ｙ）＝（０，０）とする。
超信地旋回制御処理は、クローラ機構を有したパワーショベルや戦車などの車両が行う超信地旋回と同等の旋回動作を脚車輪型ロボット１００に行わせるものである。
【０１３１】
ここで、超信地旋回とは、クローラ機構を有した車両が、左右のクローラを同速度で互いに反対方向に回転させることで、前後に進まず、車体の向きを変える旋回方法であり、これは、クローラ機構に限らず、左右に最低２輪の独立した駆動輪を有する車両であれば実現可能な旋回方法である。
本実施の形態においては、具体的に、超信地旋回制御指令があったときに実行され、ロボットの旋回角速度Ω、旋回中心（ｘｃ，ｙｃ）を入力として、脚車輪型ロボット１００を旋回中心（ｘｃ，ｙｃ）で超信地旋回させるための、各脚部１２の回転関節１４（joint0）の角度（操舵角度）θ00、θ10、θ20、θ30、各脚部１２の各駆動輪２０の回転角速度ω0、ω1、ω2、ω3を算出し、各アクチュエータに指令を与えるものである。
【０１３２】
脚車輪型ロボット１００を超信地旋回させるためには、基体１０を、旋回中心（ｘｃ，ｙｃ）でヨー軸周りに自転させたときに、各駆動輪２０の操舵時のヨー軸周りの回転中心が描く円弧軌道と各駆動輪２０の前記回転中心との接点における運動方向と、各駆動輪２０の進行方向とが一致するように各回転関節１４の関節モータ４０を制御すると共に、各駆動輪２０が前記運動方向に応じた回転方向に一定速度で回転するように各車輪モータ５０を制御する必要がある。
【０１３３】
まず、図２０（ａ）に基づき、基体１０の中心位置の座標（０，０）を旋回中心とした場合の超信地旋回制御処理について説明する。
この場合は、旋回角速度Ωと、旋回中心座標（０，０）とが入力される。
旋回角速度Ω及び旋回中心座標（０，０）が入力されると、下式（７）に基づき、基体１０のｘ軸と各駆動輪２０の回転中心とのなす角度φ0、φ1、φ2、φ3が算出される。
ｔａｎφ＝Ｗt／Ｗb ・・・（７）
但し、上式（７）は、基体１０の中心座標（０，０）を旋回中心とした場合の式である。また、上式（７）において、Ｗtはトレッド（車輪間隔）であり、Ｗbはホイールベースである。
【０１３４】
なお、φ0〜φ3は、ｘ軸と、各駆動輪２０の回転中心とのなす角度のうち最小の角度とし、φの正方向は、駆動輪によってはΩの正方向とは異なる。
Ｗt及びＷbは既知であるため（予め情報を持っておく）、上式（７）から、上記角度φ0、φ1、φ2、φ3を算出することができる。
なお、旋回中心が基体１０の中心座標（０，０）であるので、角度φ0、φ1、φ2、φ3はいずれも等角度「φ0＝φ1＝φ2＝φ3＝φ」となる。
【０１３５】
また、基体１０が旋回座標（０，０）でヨー軸周りに自転時に、各駆動輪２０の操舵時のヨー軸周りの回転中心の描く円弧軌道と該回転中心との接点における運動方向は、図２０（ａ）の各駆動輪２０から伸びる矢印線に示すように、円弧軌道上の各回転中心を通る接線方向（図中の矢印線方向）となる。
各駆動輪２０の回転中心に対する角度「φ0＝φ1＝φ2＝φ3＝φ」が算出されると、次に、各回転中心の運動方向と各駆動輪２０の進行方向とを一致させるための操舵角度θ00、θ10、θ20、θ30を算出する。
【０１３６】
各回転中心の運動方向と各駆動輪２０の進行方向とを一致させるためには、図２０（ａ）に示すように、各駆動輪２０の進行方向と、旋回中心と回転中心とを結ぶ線分との成す角度が直角（π／２（９０[°]））となるように操舵すれば良く、従って、操舵角度θ00、θ10、θ20、θ30は、「θ00＝θ10＝θ20＝θ30＝−（π／２−φ）」と算出される。
一方、下式（８）に基づき、各駆動輪２０の操舵時のヨー軸周りの回転中心と旋回中心（０，０）との距離Ｌ0、Ｌ1、Ｌ2、Ｌ3が算出される。
【０１３７】
【数３】
【０１３８】
但し、上式（８）は、基体１０の中心座標（０，０）を旋回中心とした場合の式である。
なお、トレッドＷt及びホイールベースＷbから、左前輪の回転中心の座標は（Ｗb／２，Ｗt／２）、右前輪の回転中心の座標は（Ｗb／２，−Ｗt／２）、左後輪の回転中心の座標は（−Ｗb／２，Ｗt／２）、右後輪の回転中心の座標は（−Ｗb／２，−Ｗt／２）と表すことができる。
【０１３９】
なお、旋回中心が基体１０の中心座標（０，０）であるので、距離Ｌ0、Ｌ1、Ｌ2、Ｌ3は、等距離「Ｌ0＝Ｌ1＝Ｌ2＝Ｌ3＝Ｌ」となる。
距離Ｌ0、Ｌ1、Ｌ2、Ｌ3が算出されると、次に、これらの距離Ｌと旋回角速度Ωとから、下式（９）に基づき、各駆動輪２０の線速度Ｖ0、Ｖ1、Ｖ2、Ｖ3を算出する。
Ｖ0＝Ｖ1＝Ｖ2＝Ｖ3＝ＬΩ （９）
但し、上式（９）は、基体１０の中心座標（０，０）を旋回中心とした場合の式である。
【０１４０】
ここで、先述したように、左右前輪は黒半円の目印のある方向に進行する回転方向が正回転方向となり、左右後輪は目印の無い方向に進行する回転方向が正回転方向となる。
また、各駆動輪２０の向きは、図２０（ａ）に示すようになるので、線速度Ｖ0、Ｖ1、Ｖ2、Ｖ3は、「Ｖ0＝Ｖ3＝−ＬΩ」、「Ｖ1＝Ｖ2＝ＬΩ」と算出される。
更に、線速度Ｖ0、Ｖ1、Ｖ2、Ｖ3は、上式（６）に従って、回転角速度ω0、ω1、ω2、ω3に変換される。
【０１４１】
操舵角度θ00、θ10、θ20、θ30、及び各駆動輪２０の回転角速度ω0、ω1、ω2、ω3が算出されると、次に、各駆動輪２０の現在の操舵角度及び回転角速度を取得する。そして、現在の操舵角度と、上記算出した操舵角度とから回転関節１４の関節モータ４０を駆動する指令値を算出する。更に、現在の回転角速度と、上記算出した回転角速度とから、駆動輪２０の車輪モータ５０を駆動する指令値を算出する。
【０１４２】
このようにして、各駆動輪２０の操舵制御の指令値及び速度制御の指令値を算出すると、これらの指令値を各モータのドライバに入力する。そして、入力された指令値に基づき各関節モータ４０及び車輪モータ５０が駆動されると、これにより、脚車輪型ロボット１００の各駆動輪２０の操舵角度及び回転角速度が変化し、基体１０の中心座標（０，０）を旋回中心として、脚車輪型ロボット１００が前後移動せずにその場で旋回（超信地旋回）する。
【０１４３】
次に、基体１０の中心座標（０，０）以外の座標を旋回中心とした場合の超信地旋回制御処理について説明する。以下、この超信地旋回制御処理を、旋回中心オフセット型超信地旋回制御処理と称す。
この場合は、旋回角速度Ωと、基体１０の中心座標（０，０）以外の座標である旋回中心座標（ｘｃ，ｙｃ）≠（０，０）とが入力される。
【０１４４】
そして、旋回角速度Ω及び旋回中心座標（ｘｃ，ｙｃ）が入力されると、下式（１０）に基づき、基体１０のｘ軸と各駆動輪２０の回転中心とのなす角度φ0、φ1、φ2、φ3が算出される。
ｔａｎφ0＝（Ｗt／２−ｙｃ）／（Ｗb／２−ｘｃ）
ｔａｎφ1＝（Ｗt／２＋ｙｃ）／（Ｗb／２−ｘｃ）
ｔａｎφ2＝（Ｗt／２−ｙｃ）／（Ｗb／２＋ｘｃ）
ｔａｎφ3＝（Ｗt／２＋ｙｃ）／（Ｗb／２＋ｘｃ） ・・・（１０）
但し、上式（１０）は、脚車輪型ロボット１００の脚部１２が、基体１０の前方に左右一対及び後方に左右一対の計４本の場合の式である。
【０１４５】
具体的に、入力された旋回中心座標（ｘｃ，ｙｃ）と、既知のＷt及びＷbとを、上式（１０）に代入して、基体１０のｘ軸と各駆動輪２０の回転中心とのなす角度φ0、φ1、φ2、φ3を算出する。
なお、旋回中心が基体１０の中心座標以外の座標となるので、角度φ0、φ1、φ2、φ3はそれぞれ異なる角度となる。
【０１４６】
また、基体１０が、旋回中心座標（ｘｃ，ｙｃ）でヨー軸周りに自転時に、各駆動輪２０の操舵時のヨー軸周りの回転中心の描く円弧軌道と該回転中心との接点における運動方向は、図２０（ｂ）の各駆動輪２０から伸びる矢印線に示すように、各円弧軌道上の各回転中心を通る接線方向（図中の矢印線方向）となる。また、旋回中心と各駆動輪２０の回転中心との距離はそれぞれ異なるため、各回転中心の描く円弧軌道も異なる。
【０１４７】
各駆動輪２０の回転中心に対する角度φ0、φ1、φ2、φ3が算出されると、次に、各回転中心の運動方向と各駆動輪２０の進行方向とを一致させるための操舵角度θ00、θ10、θ20、θ30を算出する。
各回転中心の運動方向と各駆動輪２０の進行方向とを一致させるためには、図２０（ｂ）に示すように、各駆動輪２０の回転方向と、旋回中心と回転中心とを結ぶ線分との成す角度が直角（π／２（９０[°]））となるように操舵すれば良く、従って、操舵角度θ00、θ10、θ20、θ30は、「θi0＝−（π／２−φi）（i=0,1,2,3）」と算出される。
【０１４８】
一方、下式（１１）に基づき、各駆動輪２０の操舵時のヨー軸周りの回転中心と旋回中心（ｘｃ，ｙｃ）との距離Ｌ0、Ｌ1、Ｌ2、Ｌ3が算出される。
【０１４９】
【数４】
【０１５０】
先述したように、旋回中心が基体１０の中心座標（０，０）以外の座標であるので、距離Ｌ0、Ｌ1、Ｌ2、Ｌ3は、それぞれ異なる距離となる。
距離Ｌ0、Ｌ1、Ｌ2、Ｌ3が算出されると、次に、これらの距離と旋回角速度Ωとから、下式（１２）に基づき、各駆動輪２０の線速度Ｖ0、Ｖ1、Ｖ2、Ｖ3を算出する。
｜Ｖi｜＝｜ＬiΩ｜ （１２）
従って、線速度Ｖ0、Ｖ1、Ｖ2、Ｖ3は、「Ｖ0＝−Ｌ0Ω」、「Ｖ1＝Ｌ1Ω」、「Ｖ2＝Ｌ2Ω」、「Ｖ3＝−Ｌ3Ω」と算出される。
【０１５１】
更に、線速度Ｖ0、Ｖ1、Ｖ2、Ｖ3は、上式（６）に従って、回転角速度ω0、ω1、ω2、ω3に変換される。
操舵角度θ00、θ10、θ20、θ30、及び各駆動輪２０の回転角速度ω0、ω1、ω2、ω3が算出されると、次に、各駆動輪２０の現在の操舵角度及び回転角速度を取得する。そして、現在の操舵角度と、上記算出した操舵角度とから回転関節１４の関節モータ４０を駆動する指令値を算出する。更に、現在の回転角速度と、上記算出した回転角速度とから、駆動輪２０の車輪モータ５０を駆動する指令値を算出する。
【０１５２】
このようにして、各駆動輪２０の操舵制御の指令値及び速度制御の指令値を算出すると、これらの指令値を各モータのドライバに入力する。そして、入力された指令値に基づき各関節モータ４０及び車輪モータ５０が駆動されると、これにより、脚車輪型ロボット１００の各駆動輪２０の操舵角度及び回転角速度が変化し、基体１０における中心位置（０，０）以外の座標（ｘｃ，ｙｃ）を旋回中心として、脚車輪型ロボット１００が旋回（超信地旋回）する。
【０１５３】
次に、図２１及び図２２に基づき、本実施の形態の動作を説明する。
ここで、図２１（ａ）及び（ｂ）は、脚車輪型ロボット１００の走行経路の一例を示す図である。また、図２２は、ロボットの重心位置の一例を示す図である。
脚車輪型ロボット１００は、障害物センサ３４、３６によって、脚車輪型ロボット１００の移動経路上に障害物（ここでは階段とする）が存在することを確認すると、３次元距離測定装置２００によって、上記第１の実施の形態で説明した距離測定及び面データの生成処理を行い、生成した面データをハブ７６及び通信Ｉ／Ｆ６４を介してＣＰＵ６０に入力する。一方、ＣＰＵ６０は、入力された面データを解析して、特に、勾配が０に近い交差線を有する面がある場合は、それを水平面とみなし、脚車輪型ロボット１００が歩行可能な面であると判断する。面データの示す面が歩行可能な面であると判断された場合は、入力された面データのセンサ座標系をグローバル座標系に変換し、この変換結果と、３軸姿勢センサ７０のセンサ信号などとに基づき、脚先の着地位置を決定する。更に、脚先センサ３８からのセンサ信号に基づき、蹴込板までの距離および脚先と踏板の位置関係とを算出する。そして、決定された着地位置および算出された両距離に基づいてモータ指令信号が生成され、生成されたモータ指令信号がドライバ４４に出力される。これにより、各関節モータが駆動し、脚車輪型ロボット１００が姿勢を適切に保ちつつ階段を乗り越える。また、状況によっては階段を回避、停止する。したがって、脚型ロボットと同様に階段への適応性が高い。
【０１５４】
一方、脚車輪型ロボット１００は、障害物センサ３４、３６のセンサ信号などに基づき、自己の移動経路上に何も障害物が存在しない（平地である）と判断すると、移動モードを、脚部１２を用いる脚部移動モードから駆動輪２０を用いる車輪走行移動モードへと切り替える。
平地では、脚車輪型ロボット１００は、上記した無変向走行で移動することができる。さらに、上記した超信地旋回及び旋回中心オフセット型超信地旋回を行うことができる。
【０１５５】
車輪走行移動モードへと切り替えられると、脚車輪型ロボット１００は、脚部１２の各関節モータ４０を制御して膝屈曲姿勢へと移行する。そして、脚車輪型ロボット１００が膝屈曲姿勢へと移行すると、各種走行制御が開始される。
まず、無変向走行制御時の脚車輪型ロボット１００の動作について説明する。
ここでは、図２１（ａ）に示すような通路を、脚車輪型ロボット１００で走行移動させることとする。なお、図２１（ａ）及び（ｂ）は、走行経路の一部を真上から見た俯瞰図である。
【０１５６】
図２１（ａ）に示すように、通路は、最初、基体１０の向いている方向（前方向）に直進しないと通れないほど幅が狭くなっているので、まず、通路への進入前において、通路の伸びる方向と基体１０の向きとを合わせると共に、進入位置及び進入角度を調整する。
そして、無変向走行制御指令を入力し、脚車輪型ロボット１００を、無変向走行制御モードへと移行させる。これにより、脚車輪型ロボット１００は、ＣＰＵ６０において、基体１０の向きを保持した状態で、目的の進行方向へと走行移動する制御を行う。
【０１５７】
まず最初は、脚車輪型ロボット１００を、前方向に直進移動させたいので、ロボットの進行方向α＝０[°]、及びロボット進行方向速度Ｖcを入力する。これにより、各駆動輪２０の操舵角度が「θ00＝θ10＝θ20＝θ30＝０[°]」と算出され、各駆動輪２０の線速度が「Ｖ0＝Ｖ1＝Ｖ2＝Ｖ3＝Ｖｃ」と算出される。
更に、線速度Ｖ0、Ｖ1、Ｖ2、Ｖ3は、上式（６）に従って、回転角速度ω0、ω1、ω2、ω3に変換される。
【０１５８】
ＣＰＵ６０は、操舵角度θ00、θ10、θ20、θ30、及び各駆動輪２０の回転角速度ω0、ω1、ω2、ω3を算出すると、次に、角度取込Ｉ／Ｆ６２を介して、現在の各駆動輪２０の操舵角度及び回転角速度を取得する。ここでは、取得した操舵角度と上記算出した操舵角度との差分値を算出し、該差分値に基づき回転関節１４の関節モータ４０を駆動する指令値を算出する。更に、取得した現在の回転角速度と、上記算出した回転角速度との差分値を算出し、該差分値に基づき駆動輪２０の車輪モータ５０を駆動する指令値を算出する。
【０１５９】
ＣＰＵ６０は、上記算出した、各駆動輪２０の操舵制御の指令値及び速度制御の指令値を各モータのドライバに入力する。この指令値により、各関節モータ４０が駆動され、各脚部１２の回転関節１４がヨー軸周りに回動して目標の操舵角度へと変化する。その後、各車輪モータ５０が駆動され、各駆動輪２０が指令値に応じた回転角速度で回転駆動する。これにより、脚車輪型ロボット１００は、基体１０の向きを保持した状態で、基体１０の向いている方向（前方向）に通路へと進入すると共に通路内を直進走行する。
【０１６０】
脚車輪型ロボット１００が前方向にしばらく直進すると、通路は右に略直角に折れ曲がり、更に路幅も広くなるので、基体１０の向きを保持した状態で、その向いている方向に対して右真横方向に直進移動するように制御する。つまり、進行方向として、α＝−９０（−π／２）[°]を入力し、更に、進行方向速度Ｖｃを入力する。
これにより、例えば、操舵角度として、「θ00＝θ10＝θ20＝θ30＝α[°]」が算出され、線速度として、「Ｖ0＝Ｖ2＝−Ｖｃ」、「Ｖ1＝Ｖ3＝Ｖｃ」が算出される。
【０１６１】
更に、線速度Ｖ0、Ｖ1、Ｖ2、Ｖ3は、上式（６）に従って、回転角速度ω0、ω1、ω2、ω3に変換される。
ＣＰＵ６０は、各駆動輪２０の現在の操舵角度及び回転角速度を取得し、これらと上記算出した操舵角度及び回転角速度とから各指令値を算出する。この指令値により、各関節モータ４０及び各車輪モータ５０が駆動され、これにより、脚車輪型ロボット１００は、基体１０の向きを保持した状態で、その向いている方向に対して右真横方向に直進走行する。これにより、通路の曲がり角で脚車輪型ロボット１００を旋回せずに、その曲がった先へと走行させることができる。
【０１６２】
脚車輪型ロボット１００が右真横方向にしばらく直進すると、通路は右斜め下方向に折れ曲がるので、今度は、基体１０の向きを保持した状態で、その向いている方向に対して右斜め後ろ方向に直進移動するように制御する。つまり、進行方向として、α（通路の角度＜０）[°]を入力し、更に、進行方向速度Ｖｃを入力する。
これにより、操舵角度として、「θ00＝θ30＝π＋α[°]」及び「θ10＝θ20＝−（π＋α）[°]」が算出され、線速度として、「Ｖ0＝Ｖ1＝−Ｖｃ」及び「Ｖ2＝Ｖ3＝Ｖｃ」が算出される。
【０１６３】
更に、線速度Ｖ0、Ｖ1、Ｖ2、Ｖ3は、上式（６）に従って、回転角速度ω0、ω1、ω2、ω3に変換される。
そして、各駆動輪２０の現在の操舵角度及び回転角速度を取得し、これらと上記算出した操舵角度及び回転角速度とから各指令値を算出する。この指令値により、各関節モータ４０及び各車輪モータ５０が駆動され、これにより、脚車輪型ロボット１００は、基体１０の向きを保持した状態で、その向いている方向に対して右斜め後ろ方向に直進走行する。
【０１６４】
次に、超信地旋回制御時の脚車輪型ロボット１００の動作について説明する。
ここでは、図１９（ｂ）に示すような通路を、脚車輪型ロボット１００で走行移動させることとする。
図２１（ｂ）に示すように、通路は、最初真っ直ぐに伸びており、その後、略直角に右に折れ曲がり、その先で行き止まりとなっている。
【０１６５】
まず、通路の伸びる方向と基体１０の向きとを合わせ、進入位置を微調整した後に、脚車輪型ロボット１００を前方向に直進走行させる。これにより、脚車輪型ロボット１００は、通路へと進入すると共に通路内を直進走行する。
脚車輪型ロボット１００は、しばらく直進すると、やがて曲がり角へと到達するので、時計回りに旋回して、基体１０の向きを脚車輪型ロボット１００が進行できる向きへと変更する。
【０１６６】
図２１（ｂ）に示す曲がり角であれば、前移動を伴う旋回動作でも十分に右折できるが、ここでは、超信地旋回により右折することとする。そのため、超信地旋回制御指令を入力し、この指令により、脚車輪型ロボット１００を、超信地旋回制御モードへと移行させる。これにより、脚車輪型ロボット１００は、ＣＰＵ６０において、前後移動を行わずに所定の旋回中心位置でロボットを旋回する制御処理を行う。
【０１６７】
まず、旋回角速度Ω（基体１０を時計回りに回転させる角速度）と、旋回中心座標（０，０）とを入力する。更に、略直角に右折させるので、旋回角度−９０[°]を入力する。
旋回角速度Ω、旋回中心座標（０，０）及び旋回角度（−９０[°]）が入力されると、上式（７）に基づき、基体１０のｘ軸と各駆動輪２０の回転中心とのなす角度φ0、φ1、φ2、φ3が算出される。
【０１６８】
各駆動輪２０の回転中心に対する角度「φ0＝φ1＝φ2＝φ3＝φ」が算出されると、次に、各回転中心の運動方向と各駆動輪２０の進行方向とを一致させるための操舵角度θ00、θ10、θ20、θ30が算出される。
具体的に、操舵角度θ00、θ10、θ20、θ30は、「θ00＝θ10＝θ20＝θ30＝−（π／２−φ）」が算出される。
【０１６９】
また、上式（８）に基づき、各駆動輪２０の操舵時のヨー軸周りの回転中心と旋回中心（０，０）との距離Ｌ0、Ｌ1、Ｌ2、Ｌ3が算出される。
旋回中心が基体１０の中心座標（０，０）となっているので、距離Ｌ0、Ｌ1、Ｌ2、Ｌ3は、等距離「Ｌ0＝Ｌ1＝Ｌ2＝Ｌ3＝Ｌ」となる。
次に、距離Ｌと旋回角速度Ωとから、上式（９）に基づき、各駆動輪２０の線速度Ｖ0、Ｖ1、Ｖ2、Ｖ3を算出する。
【０１７０】
ここでは、脚車輪型ロボット１００を、時計回りに旋回（Ω＜０）させるので、線速度Ｖ0、Ｖ1、Ｖ2、Ｖ3は、「Ｖ1＝Ｖ2＝ＬΩ」、「Ｖ0＝Ｖ3＝−ＬΩ」と算出される。
更に、線速度Ｖ0、Ｖ1、Ｖ2、Ｖ3は、上式（６）に従って、回転角速度ω0、ω1、ω2、ω3に変換される。
操舵角度θ00、θ10、θ20、θ30、及び各駆動輪２０の回転角速度ω0、ω1、ω2、ω3が算出されると、次に、角度取込Ｉ／Ｆ６２を介して、現在の各駆動輪２０の操舵角度及び回転角速度を取得する。ここでは、取得した操舵角度と上記算出した操舵角度との差分値を算出し、該差分値に基づき回転関節１４の関節モータ４０を駆動する指令値を算出する。更に、取得した現在の回転角速度と、上記算出した回転角速度との差分値を算出し、該差分値に基づき駆動輪２０の車輪モータ５０を駆動する指令値を算出する。
【０１７１】
ＣＰＵ６０は、上記算出した、各駆動輪２０の操舵制御の指令値及び速度制御の指令値を各モータのドライバに入力する。この指令値により、各関節モータ４０が駆動され、各脚部１２の回転関節１４がヨー軸周りに回動して目標の操舵角度へと変化する。その後、各車輪モータ５０が駆動され、各駆動輪２０が指令値に応じた回転角速度で回転駆動する。これにより、脚車輪型ロボット１００は、基体１０の中心座標（０，０）を旋回中心として、前後移動せずにその場で時計回りに９０[°]旋回（超信地旋回）する。
【０１７２】
そして、脚車輪型ロボット１００を、時計回りに９０[°]旋回して右折させると、その先の通路を、前方向へと直進走行させる。
図２１（ｂ）に示すように、右折した先の通路は袋小路となっているため、脚車輪型ロボット１００は、やがて通路の行き止まりへと到達する。
脚車輪型ロボット１００は、これ以上先に進めないため（各種センサにより状況を把握）、１８０[°]旋回して通路を引き返すことになる。
【０１７３】
ここでは、通路の幅が前後移動を伴う旋回動作（例えば、Ｕターン）を行えるほど広くないため、上記右折のときと同様に、超信地旋回制御モードへと移行し、超信地旋回により１８０[°]旋回して、脚車輪型ロボット１００の向きを変更し、引き返すこととする。
また、図２１（ｂ）に示すように、通路幅が自転ぎりぎりの幅となっているので、脚車輪型ロボット１００を最小の旋回半径で旋回させる必要がある。従って、旋回中心座標（０，０）と、旋回角速度Ω（基体１０を時計回りに回転させる角速度）と、旋回角度１８０[°]とを入力する。なお、脚部１２が通路にぶつからないように、脚車輪型ロボット１００の各脚部１２の姿勢を膝伸展姿勢へと変更する。
【０１７４】
旋回中心座標（０，０）、旋回角速度Ω及び旋回角度１８０[°]が入力されると、上記右折のときと同様に、操舵角度θ00、θ10、θ20、θ30として、「θ00＝θ10＝θ20＝θ30＝−（π／２−φ）」が算出され、線速度Ｖ0、Ｖ1、Ｖ2、Ｖ3として、「Ｖ1＝Ｖ2＝ＬΩ」、「Ｖ0＝Ｖ3＝−ＬΩ」が算出される。
更に、線速度Ｖ0、Ｖ1、Ｖ2、Ｖ3は、上式（６）に従って、回転角速度ω0、ω1、ω2、ω3に変換される。
【０１７５】
操舵角度θ00、θ10、θ20、θ30、及び各駆動輪２０の回転角速度ω0、ω1、ω2、ω3が算出されると、各指令値が算出され、関節モータ４０及び車輪モータ５０が駆動される。
これにより、基体１０の中心座標（０，０）を旋回中心として、脚車輪型ロボット１００が前後移動せずにその場で時計回りに１８０[°]旋回（超信地旋回）し、真後ろ方向へと向きを変える。
【０１７６】
ここで、脚車輪型ロボット１００が、例えば、遠隔操作で人手により走行制御される場合や、自動制御で且つ基体１０の後ろ側にも各種センサやカメラを備えている場合などは、無変向走行制御により、前を向いたまま真後ろに走行させて通路を引き返させることも可能である。しかし、自動制御の場合で且つ各種センサが基体１０の前側にしか備わっていない場合は、基体１０の向きと進行方向とを合わせる必要がある。従って、後者の場合などに、超信地旋回は有用な旋回手段となる。
【０１７７】
次に、旋回中心オフセット型超信地旋回制御時の脚車輪型ロボット１００の動作について説明する。
いま、車輪走行移動モードへと移行し、脚車輪型ロボット１００の各関節モータ４０が制御され、ロボットの姿勢が膝屈曲姿勢に変更されたとする。このとき、ロボットの重心が図２２に示すように、基体１０の中心位置の座標から外れた位置となるとする。
【０１７８】
この場合は、重心位置が基体１０の中心位置の座標（０，０）から外れているため、中心位置を旋回中心として超信地旋回を行わせると旋回がアンバランスとなり、不具合が発生する恐れがある。
このようなときに、本実施の形態の超信地旋回制御指令モードにおいては、任意の旋回中心で超信地旋回させることができるので、旋回角速度Ωを入力すると共に、基体１０における、脚車輪型ロボット１００の重心位置に対応する座標（ｘｇ，ｙｇ）を旋回中心座標（ｘｃ，ｙｃ）として入力する。 そして、旋回角速度Ω及び旋回中心座標（ｘｃ，ｙｃ）＝（ｘｇ，ｙｇ）が入力されると、上式（１０）に基づき、基体１０のｘ軸と各駆動輪２０の回転中心とのなす角度φ0、φ1、φ2、φ3が算出される。
【０１７９】
各駆動輪２０の回転中心に対する角度φ0、φ1、φ2、φ3が算出されると、次に、各回転中心の運動方向と各駆動輪２０の進行方向とを一致させるための操舵角度θ00、θ10、θ20、θ30を算出する。
操舵角度θ00、θ10、θ20、θ30は、「θi0＝−（π／２−φi）（i=0,1,2,3）」と算出される。
【０１８０】
次に、上式（１１）に基づき、各駆動輪２０の操舵時のヨー軸周りの回転中心と旋回中心（ｘｃ，ｙｃ）との距離Ｌ0、Ｌ1、Ｌ2、Ｌ3を算出する。
距離Ｌ0、Ｌ1、Ｌ2、Ｌ3が算出されると、次に、これらの距離と旋回角速度Ωとから、上式（１２）に基づき、各駆動輪２０の線速度Ｖ0、Ｖ1、Ｖ2、Ｖ3を算出する。
ここでは、脚車輪型ロボット１００を反時計回りに旋回（Ω＞０）させるとして、線速度Ｖ0、Ｖ1、Ｖ2、Ｖ3は、「Ｖ0＝−Ｌ0Ω」、「Ｖ1＝Ｌ1Ω」、「Ｖ2＝Ｌ2Ω」、「Ｖ3＝−Ｌ3Ω」と算出される。
【０１８１】
更に、線速度Ｖ0、Ｖ1、Ｖ2、Ｖ3は、上式（６）に従って、回転角速度ω0、ω1、ω2、ω3に変換される。
操舵角度θ00、θ10、θ20、θ30、及び各駆動輪２０の回転角速度ω0、ω1、ω2、ω3が算出されると、次に、角度取込Ｉ／Ｆ６２を介して、現在の各駆動輪２０の操舵角度及び回転角速度を取得する。ここでは、取得した操舵角度と上記算出した操舵角度との差分値を算出し、該差分値に基づき回転関節１４の関節モータ４０を駆動する指令値を算出する。更に、取得した現在の回転角速度と、上記算出した回転角速度との差分値を算出し、該差分値に基づき駆動輪２０の車輪モータ５０を駆動する指令値を算出する。
【０１８２】
ＣＰＵ６０は、上記算出した、各駆動輪２０の操舵制御の指令値及び速度制御の指令値を各モータのドライバに入力する。この指令値により、各関節モータ４０及び各車輪モータ５０が駆動され、これにより、基体１０における重心座標と対応する座標（ｘｇ，ｙｇ）を旋回中心として、脚車輪型ロボット１００がバランスのとれた状態で反時計回りに超信地旋回する。
【０１８３】
このようにして、本実施の形態では、３次元距離測定装置２００の機械的な構成を、モータ１１６及びエンコーダ１１８と、２次元距離測定装置１１２とを水平方向に並べて配設し、モータ１１６の第１の回転軸の回転駆動力を、プーリ１２０ａ、ベルト１２１、プーリ１２０ｂを介して、２次元距離測定装置１１２を回転する第２回転軸へと伝達し、２次元距離測定装置１１２を、垂直方向の軸（ｙ軸）まわりに回転駆動する構成とした。
【０１８４】
これにより、２次元距離測定装置１１２の所定走査角度範囲内には、走査を除外するものが一切なくなるので、正確な距離情報を得ることができる。また、モータ１１６及びエンコーダ１１８と、２次元距離測定装置１１２とを水平方向に並べて配設したので、３次元距離測定装置２００の高さ方向の占有率を低減することができる。
さらに、本実施の形態では、３次元距離測定装置２００において、測定平面内に存在する物体の凹凸を認識することができる。これにより、連続面が存在することを推定することができる。更に、複数の測定平面に対して第１走査処理を実行することができるので、測定範囲内に存在する物体の面形状を認識することができる。
【０１８５】
これにより、障害物（通路の壁等を含む）までの距離及び障害物の形状を正確に把握することができるので、脚車輪型ロボット１００の障害物の回避、通路内の走行等の移動動作をより安全且つ確実に行うことができる。
さらに、本実施の形態では、基体１０の向きを一定の方向に保持した状態で、脚車輪型ロボット１００が目的の進行方向に走行するように関節モータ４０及び車輪モータ５０を制御する。
【０１８６】
これにより、旋回を行わず（向きを変えず）に自由な方向へ移動できるので、各方向への素早い移動を実現できると共に、脚車輪型ロボット１００の各構成部が妨げとなって旋回できないような狭くて入り組んだエリアなど、脚車輪型ロボットの向きを変更することが困難なエリアにおいても活動が可能となる。
さらに、本実施の形態では、脚車輪型ロボット１００を所定の旋回中心位置で前後移動させずに旋回（超信地旋回）するように関節モータ４０及び車輪モータ５０を制御する。このとき、基体１０における中心位置の座標を旋回中心座標とする制御（超信地旋回制御）と、基体１０における中心位置以外の座標を旋回中心座標とする制御（旋回中心オフセット型超信地旋回制御）とを行うことが可能である。
【０１８７】
これにより、基体１０の中心位置を旋回中心位置とする場合は、最小の旋回半径で脚車輪型ロボットを旋回をさせることが可能である。また、基体１０の中心位置以外を旋回中心位置とする場合は、基体１０の中心位置と重心位置とが異なる場合などに、重心位置を旋回中心として超信地旋回させることができるので、脚車輪型ロボット１００を、バランスよく超信地旋回させることが可能である。
【０１８８】
上記第２の実施の形態において、３次元距離測定装置２００は、発明１乃至６の３次元距離測定装置に対応し、脚車輪型ロボット１００は、発明８ないし１０の脚車輪型ロボットに対応し、駆動輪２０は、発明７乃至９の車輪に対応し、関節モータ４０は、発明８ないし１０の第１アクチュエータ及び第２アクチュエータに対応し、ＣＰＵ６０は、発明８ないし１０の制御手段に対応している。
【０１８９】
なお、上記第１及び第２の実施の形態においては、モータ１１６の第１の回転軸にエンコーダ１１８を設ける構成としたが、これに限らず、２次元距離測定装置１１２の第２の回転軸にエンコーダを設ける構成としても良い。
これにより、伝達誤差の影響を受けずに第２走査処理における走査角を高精度に検出することができる。
【０１９０】
また、上記第１及び第２の実施の形態においては、プーリ２０及びベルト１２１を介して、モータ１１６の第１の回転軸の回転駆動力を、２次元距離測定装置１１２を回転駆動する第２の回転軸へと伝達する構成としたが、これに限らず、複数の歯車を介して、第１の回転軸の回転駆動力を、第２の回転軸へと伝達する構成としても良い。
また、上記第１及び第２の実施の形態においては、第１走査処理における距離情報の取得を離散的（第２走査処理で回転駆動してから第１走査処理で距離情報を取得）に行う構成としたが、これに限らず、走査角と測定距離との対応付けを行うことで連続的（第２走査処理で回転駆動しつつ、第１走査処理で距離情報を取得）に行う構成としても良い。
【０１９１】
また、上記第１及び第２の実施の形態においては、３次元距離測定装置２００で生成した面データに基づき、ＣＰＵ６０において、物体の面形状を認識する構成としたが、これに限らず、３次元距離測定装置２００において、面形状の認識まで行い、ＣＰＵ６０には、面形状の認識結果を示すデータを出力する構成としても良い。
また、上記第１及び第２の実施の形態においては、ＣＰＵ６０からの指令信号に基づき、走査角度範囲及び走査角度単位を設定する構成としたが、これに限らず、３次元距離測定装置２００に予め設定しておくなど別の構成としても良い。
【０１９２】
また、上記第１及び第２の実施の形態においては、本発明に係る３次元距離測定装置２００を、脚車輪型ロボットに適用する例を説明したが、これに限らず、本発明に係る３次元距離測定装置２００を、他の構成のロボット、自動車の外界を認識する装置、視覚障害者が外界を認識するためのアシストを行う装置などに適用することも可能である。
また、上記第１及び第２の実施の形態においては、３次元距離測定装置２００を、測距センサ１１２ａを水平方向の軸（ｙ軸）まわりに回転駆動し、２次元距離測定装置１１２を垂直方向の軸（ｚ軸）まわりに回転駆動する構成としたが、この構成に限らず、測距センサ１１２ａを垂直方向の軸まわりに回転駆動し、２次元距離測定装置１１２を水平方向の軸まわりに回転駆動する構成としても良い。また、水平方向及び垂直方向の軸まわりに限らず、測距センサ１１２ａの測定方向に対して直交する２つの軸であれば、どの方向の軸まわりでも良い。
【０１９３】
また、上記第１及び第２の実施の形態においては、図１５（ａ）に示すように、２次元距離測定装置１１２自身を回転させているが、これに限らず、光学式の測距センサを有する２次元距離測定装置であれば、図１５（ｂ）に示すように、測定方向の光軸上に挿入したミラーを回転させてもよい。
また、上記第１及び第２の実施の形態においては、本発明に係る脚車輪型ロボット１００の構成を、基体１０の前方に左右一対及び後方に左右一対の４本の脚部１２を有する構成としたが、これに限らず、基体１０の中央に左右一対の脚部１２を設ける構成や、３本の脚部１２を対称に設ける構成、５本以上の脚部１２を設ける構成など、本発明の趣旨を逸脱しない範囲で他の構成としても良い。また、多脚とした場合には、不要な脚部１２を走行制御に用いないように制御するようにしても良い。
【図面の簡単な説明】
【０１９４】
【図１】本発明に係る脚車輪型ロボット１００の側面図である。
【図２】（ａ）は、３次元距離測定装置２００の機械的な構成を示す図であり、（ｂ）は、（ａ）を真上から見た図である。
【図３】測距センサ１１２ａの走査範囲の一例を示す図である。
【図４】（ａ）は、（ａ）は、３次元距離測定装置２００の構成を示すブロック図であり、（ｂ）は、本実施の形態に係る２次元距離測定装置１１２の詳細な構成を示すブロック図である。
【図５】測距センサ１１２ａの距離測定の原理を説明する図である。
【図６】（ａ）は、水平方向の軸（ｙ軸）まわりに測距センサ１１２ａを回転駆動させたときの測定距離と回転角度θとの関係を示す図であり、（ｂ）は、垂直方向の軸（ｚ軸）まわりに２次元距離測定装置１１２を回転駆動したときの測定平面と回転角度φとの関係を示す図である。
【図７】３次元距離測定装置２００の距離の計測例を示す図である。
【図８】３次元距離測定装置２００における物体認識処理を示すフローチャートである。
【図９】（ａ）及び（ｂ）は、勾配の算出例を示す図である。
【図１０】脚車輪型ロボット１００の移動制御システムを示すブロック図である。
【図１１】測定範囲内に階段がある場合の、第１走査処理による測定結果を示す図である。
【図１２】図１１の各測定点の回転座標系の測定距離を直交座標系の測定距離に変換した場合の測定結果を示す図である。
【図１３】（ａ）及び（ｂ）は、最小二乗法を用いて算出したｘ方向及びｚ方向の勾配の一例を示す図である。
【図１４】ある測定平面に対する勾配の出現頻度の一例を示す図である。
【図１５】測距センサの測定方向を変更する場合の構成を示す図である。
【図１６】脚車輪型ロボット１００の正面図である。
【図１７】脚車輪型ロボット１００の側面図である。
【図１８】（ａ）及び（ｂ）は、脚車輪型ロボット１００の車輪走行移動時の姿勢を示す図である。
【図１９】無変向走行制御時の脚車輪型ロボット１００の走行状態例を示す図である。
【図２０】ａ）及び（ｂ）は、基体１０の座標（０，０）及び座標（ｘｃ，ｙｃ）を回転中心とした場合の超信地旋回制御時の脚車輪型ロボット１００の走行状態を示す図である。
【図２１】（ａ）及び（ｂ）は、脚車輪型ロボット１００の走行経路の一例を示す図である。
【図２２】ロボットの重心位置の一例を示す図である。
【符号の説明】
【０１９５】
１００ 脚車輪型ロボット
２００ ３次元距離測定装置
１１０ センシングプロセッサ
１１２ ２次元距離測定装置
１１２ａ 測距センサ
１０ 基体
１２ 脚部
１４〜１８ 回転関節
２０ 駆動輪
２６ 水平レーザ
２８、３０ 垂直レーザ
３２ カメラ
３４、３６ 障害物センサ
３８ 脚先センサ
１１２ｃ、１１６、４０、５０ モータ
１１２ｄ、１１８、４２、５２ エンコーダ
１１２ｂ、１１４、４４、５４ ドライバ
１２０ａ、１２０ｂ プーリ
２１ ベルト
６０ ＣＰＵ
６４ 通信Ｉ／Ｆ
７６ ハブ
【技術分野】
【０００１】
本発明は、測距センサ及び回転駆動機構を含んで構成される３次元距離測定装置に係り、特に、所定回転角度範囲内において、前記回転駆動機構を含む駆動機構に阻害されずに、測距センサで走査範囲を走査することが可能な３次元距離測定装置及び当該装置を有する脚車輪型ロボットに関する。
【背景技術】
【０００２】
従来、２次元レンジセンサ、レーザレンジファインダ、測域センサ、平面センサなどの２次元距離測定装置を回転駆動して３次元の距離情報を測定する３次元距離測定装置がある。例えば、平面センサを用いた３次元距離測定装置として、非特許文献１に記載の技術がある。
非特許文献１では、環境をくまなくスキャンするために、平面センサを３６０[°]以上回転できるようにしている。
【非特許文献１】上田達朗、川田浩彦、冨沢哲雄、大矢晃久、油田信一「無限回転機構を用いた３次元測域センサ」、ロボディクスメカトロニクス講演会06、2P1-C16
【発明の開示】
【発明が解決しようとする課題】
【０００３】
しかしながら、上記非特許文献１の従来技術では、平面スキャナを３６０[°]以上回転させるために、平面スキャナを駆動する全ての機構をその下部の一箇所に集める構成としている。そのため、縦に長い機構となり、スペースファクタ的に不利になる場合がある。
また、上記非特許文献１の従来技術では、３６０[°]以上回転できる機構とするために、配線を行わずに通信及び電源の供給ができるスリップリングを用いているため、接点寿命の問題が発生する。なお、通信部を無線化しても電源の配線は必要となる。また、その電源をバッテリで補っても、重量増、バッテリ持続時間などの問題が発生する。
【０００４】
また、環境認識、形状認識、障害物回避等において、人間が活動できるように整備された空間であれば、必ずしも３６０[°]以上の回転機構がなくても十分な測定情報は得られる。
そこで、本発明は、このような従来の技術の有する未解決の課題に着目してなされたものであり、所定の回転角度範囲において、測距センサの駆動機構に阻害されずに走査範囲を走査することが可能な３次元距離測定装置及び当該装置を備えた脚車輪型ロボットを提供することを目的としている。
【課題を解決するための手段】
【０００５】
〔発明１〕 上記目的を達成するために、発明１の３次元距離測定装置は、測定距離範囲内に存在する物体上の測定点までの距離を測定する測距センサと、前記測距センサを、その測定方向に対して直交する２つの軸のうち一方の軸まわりに回転駆動させる第１の回転機構と、前記測距センサを、前記第１の回転機構と共に、前記測定方向に対して直交する２つの軸のうち前記第１の回転機構とは異なる方向の軸まわりに回転駆動させる第２の回転機構とを有し、前記第１の回転機構及び前記第２の回転機構を所定の回転角度ずつ回転駆動しながら前記測距センサによって距離を測定することで、前記物体の３次元の距離を測定する３次元距離測定装置であって、前記第１の回転機構及び前記第２の回転機構の少なくとも一方の回転駆動を所定の回転角度範囲内で行うと共に、当該回転角度範囲内の回転駆動によって前記測距センサが走査する走査範囲外に、前記第１の回転機構及び前記第２の回転機構含む自己の駆動機構を設けたことを特徴とする。
【０００６】
このような構成であれば、測距センサは、その測定方向に対して直交する２つの軸のうち一方の軸まわりに回転駆動し、且つ、測距センサ及び第１の回転機構は、前記測定方向に対して直交する２つの軸のうち他方の軸まわりに回転駆動しながら物体上の測定点までの距離を測定することが可能である。
また、第１の回転機構及び第２の回転機構の少なくとも一方の回転駆動による走査を、所定の回転角度範囲内において、測距センサの駆動機構に阻害されずに行うことが可能である。
ここで、所定の回転角度とは、環境認識、形状認識、障害物回避等を行うのに十分な距離データが得られる走査範囲の角度である。
【０００７】
〔発明２〕 更に、発明２の３次元距離測定装置は、発明１の３次元距離測定装置において、前記第２の回転機構は、回転駆動力を発生する回転駆動装置を含み、
前記回転駆動装置の第１の回転軸と、前記測距センサ及び前記第１の回転機構を共に回転させる第２の回転軸とを水平方向にずらして設けると共に、前記回転駆動装置の第１の回転軸の回転駆動力を、所定の動力伝達機構を介して前記第２の回転軸に伝達する構成としたことを特徴とする。
このような構成であれば、前記測距センサ及び前記第１の回転機構とから構成される２次元距離測定装置と、前記第２の回転機構とを水平方向に並べて配設することが可能である。
【０００８】
〔発明３〕 更に、発明３の３次元距離測定装置は、発明２の３次元距離測定装置において、前記動力伝達機構は、複数の滑車及び各滑車に動力を伝達するベルトを介して、前記回転駆動装置の第１の回転軸の回転駆動力を、前記第２の回転軸に伝達する構成となっていることを特徴とする。
このような構成であれば、回転駆動装置の第１の回転軸の回転駆動力を滑車に伝達することが可能であり、各滑車間の動力をベルトを介して伝達することが可能であり、伝達された滑車の回転力を第２の回転軸に伝達することが可能である。
【０００９】
〔発明４〕 更に、発明４の３次元距離測定装置は、発明２の３次元距離測定装置において、前記動力伝達機構は、複数の歯車を介して、前記回転駆動装置の第１の回転軸の回転駆動力を前記第２の回転軸に伝達する構成となっていることを特徴とする。
このような構成であれば、回転駆動装置の第１の回転軸の回転駆動力を複数の歯車を介して第２の回転軸へと伝達することが可能である。
【００１０】
〔発明５〕 更に、発明５の３次元距離測定装置は、発明１乃至４のいずれか１の３次元距離測定装置において、前記第１の回転機構を制御して、前記測距センサを、前記第１の回転機構の回転方向に、所定の回転角度ずつ回動させると共に、前記測距センサによって各回転角度に応じた距離情報を測定する処理である第１走査処理を行う第１走査手段と、前記第１走査手段によって測定された、各測定点の距離情報とその周辺の測定点の距離情報とに基づき、各測定点に対する勾配を算出する勾配算出手段と、前記勾配算出手段で算出した各測定点の勾配に基づき、各勾配の出現頻度を算出する出現頻度算出手段と、前記出現頻度算出手段で算出した各勾配の出現頻度と、各勾配に対応する測定点の座標情報とに基づき、前記測距センサの測定平面内に存在する物体の形状を認識する形状認識手段と、を備えることを特徴とする。
【００１１】
このような構成であれば、第１走査手段によって、第１の回転機構を制御して、前記測距センサを、前記第１の回転機構の回転方向に、所定の回転角度ずつ回動させると共に、前記測距センサによって各回転角度に応じた距離情報を測定する処理である第１走査処理を行うことが可能である。
また、勾配算出手段によって、第１走査手段によって測定された、各測定点の距離情報とその周辺の測定点の距離情報とに基づき、各測定点に対する勾配を算出することが可能であり、出現頻度算出手段によって、勾配算出手段で算出した各測定点の勾配に基づき、各勾配の出現頻度を算出することが可能であり、形状認識手段によって、出現頻度算出手段で算出した各勾配の出現頻度と、各勾配に対応する測定点の座標情報とに基づき、前記測距センサの測定平面内に存在する物体の詳細な形状を認識することが可能である。
【００１２】
ここで、勾配算出手段は、例えば、直交座標系において複数個の測定値に対する回帰直線を公知の最小二乗法で近似すると共に、この回帰直線から勾配を算出したり、直交座標系において一方の軸方向の複数の測定点を間に挟む両端２つの測定値の差分値と、他方の軸方向の複数の測定点を間に挟む両端２つの測定値の差分値とを用いて勾配を算出したりする。
また、形状認識手段は、各勾配の出現頻度と、各勾配に対応する測定点の座標情報とに基づき、測定平面内に存在する物体の凹凸を認識する。例えば、勾配が０又は略０の測定点が連続する場合は、そこに平面があると認識することが可能である。
【００１３】
〔発明６〕 更に、発明６の３次元距離測定装置は、発明５の３次元距離測定装置において、１つの前記測定平面に対する前記第１走査処理が終了する毎に、前記第２の回転機構を制御して、前記測距センサを前記第１の回転機構と共に、前記第２の回転機構の回転方向に所定の回転角度ずつ回動させる処理である第２走査処理を行う第２走査手段を備え、前記第１走査手段は、前記第２走査処理において、前記測距センサが前記所定の回転角度の回動をする毎に前記第１走査処理を実行し、前記勾配算出手段は、前記測定平面毎に、前記第１走査処理によって測定された、各測定点の距離情報と、その周辺の測定点の距離情報とに基づき、各測定点に対する勾配を算出し、前記出現頻度算出手段は、前記測定平面毎に、前記勾配算出手段で算出した各測定点の勾配に基づき、各勾配の出現頻度を算出し、前記形状認識手段は、前記測定平面毎に、前記出現頻度算出手段で算出した各勾配の出現頻度と、各勾配に対応する測定点の座標情報とに基づき、前記測定平面内に存在する物体の形状を認識することを特徴とする。
【００１４】
このような構成であれば、第２走査手段によって、１つの測定平面に対する第１走査処理が終了する毎に、第２の回転機構を制御して、測距センサを第１の回転機構と共に、第２の回転機構の回転方向に所定の回転角度ずつ回動させる処理である第２走査処理を行うことが可能である。
また、勾配算出手段は、測定平面毎に、第１走査処理によって測定された、各測定点の距離情報と、その周辺の測定点の距離情報とに基づき、各測定点に対する勾配を算出し、出現頻度算出手段は、測定平面毎に、勾配算出手段で算出した各測定点の勾配に基づき、各勾配の出現頻度を算出し、形状認識手段は、測定平面毎に、出現頻度算出手段で算出した各勾配の出現頻度と、各勾配に対応する測定点の座標情報とに基づき、測定平面内に存在する物体の形状を認識することが可能である。
ここで、形状認識手段は、測距センサを、例えば、ピッチ回転させたときの距離情報、及びヨー回転させたときの距離情報の双方から算出した各測定点の勾配情報と、各測定点の座標情報とから物体の形状を認識することができるので、物体の詳細な面形状を認識することが可能である。
【００１５】
〔発明７〕 一方、上記目的を達成するために、発明５の脚車輪型ロボットは、基体と、前記基体に対して自由度を有して連結された脚部と、前記脚部に回転可能に設けられた車輪と、前記脚部及び前記車輪を駆動するための動力を付与するアクチュエータと、前記アクチュエータを制御する制御手段とを備え、前記脚部の駆動および前記車輪の回転により移動する脚車輪型ロボットであって、発明１乃至発明６のいずれか１項に記載の３次元距離測定装置を備え、前記制御手段は、前記３次元距離測定装置の測定距離に基づき前記アクチュエータを制御することを特徴とする。
このような構成であれば、発明１乃至４のいずれか１の３次元距離測定装置によって測定した測定距離に基づき、車輪走行制御及び歩行制御を行うことが可能である。
【００１６】
〔発明８〕 また、上記目的を達成するために、発明８の脚車輪型ロボットは、基体と、前記基体に対してヨー軸回りの自由度およびピッチ軸またはロール軸回りの自由度を有して連結された脚部と、前記脚部に回転可能に設けられた車輪と、前記ヨー軸回りの自由度の範囲で前記脚部を駆動するための動力を付与する第１アクチュエータと、前記ピッチ軸またはロール軸回りの自由度の範囲で前記脚部を駆動するための動力を付与する第２アクチュエータと、前記第１アクチュエータおよび前記第２アクチュエータを制御する制御手段と、発明１乃至発明６のいずれか１項に記載の３次元距離測定装置とを備え、前記脚部の駆動および前記車輪の回転により移動する脚車輪型ロボットであって、前記制御手段は、前記基体の向きを一定方向に保ちながら、自脚車輪型ロボットの進行方向と、前記各車輪の進行方向とが一致するように、前記第２アクチュエータを制御する。
このような構成であれば、自脚車輪型ロボットの移動時において、脚車輪型ロボットの基体の向きを一定方向に保ちながら、自脚車輪型ロボットの進行方向と各車輪の進行方向とが一致するように第２アクチュエータが制御される。
【００１７】
〔発明９〕 また、上記目的を達成するために、発明９の脚車輪型ロボットは、基体と、前記基体に対してヨー軸回りの自由度およびピッチ軸またはロール軸回りの自由度を有して連結された複数の脚部と、前記各脚部に回転可能に設けられた車輪と、前記ヨー軸回りの自由度の範囲で前記各脚部を駆動するための動力を付与する第１アクチュエータと、前記ピッチ軸またはロール軸回りの自由度の範囲で前記各脚部を駆動するための動力を付与する第２アクチュエータと、前記第１アクチュエータおよび前記第２アクチュエータを制御する制御手段と、発明１乃至発明６のいずれか１項に記載の３次元距離測定装置とを備え、前記脚部の駆動および前記車輪の回転により移動する脚車輪型ロボットであって、前記制御手段は、旋回時に、前記基体を所定の回転中心位置でヨー軸周りに自転運動させたときの前記各車輪の操舵時の回転中心の描く円弧軌道と前記操舵時の回転中心との接点位置における該回転中心の運動方向と、前記各車輪の進行方向とが一致するように、前記第２アクチュエータを制御する。
【００１８】
このような構成であれば、旋回時は、制御手段により、基体をヨー軸（垂直軸）周りに自転運動させたときの各車輪の操舵時の回転中心の描く円弧軌道と操舵時の回転中心との接点位置における該回転中心の運動方向と、各車輪の進行方向とが一致するように、第２アクチュエータが制御される。
【００１９】
〔発明１０〕 更に、発明１０の脚車輪型ロボットは、発明９の脚車輪型ロボットにおいて、前記制御手段は、前記基体の向きを一定方向に保ちながら、自脚車輪型ロボットの進行方向と、前記各車輪の進行方向とが一致するように、前記第２アクチュエータを制御する。
【００２０】
このような構成であれば、自脚車輪型ロボットの移動時において、脚車輪型ロボットの基体の向きを一定方向に保ちながら、自脚車輪型ロボットの進行方向と各車輪の進行方向とが一致するように第２アクチュエータが制御される。
【発明の効果】
【００２１】
以上説明したように、発明１の３次元距離測定装置によれば、所定の回転角度範囲内において、測距センサの駆動機構に阻害されずに走査を行うことができるので、正確な距離測定を行うことができるという効果が得られる。
また、発明２〜４の３次元距離測定装置によれば、前記測距センサ及び前記第１の回転機構と、前記第２の回転機構とを水平方向に並べた構成とすることができるので、３次元距離測定装置の垂直方向の占有率を抑えることができるという効果が得られる。
【００２２】
また、発明５〜６の３次元距離測定装置によれば、物体の面の形状を認識（又は推定）することができるので、これにより、段差が一定でない階段、建物の非常階段のように蹴込が連続面でない階段、非常用の外階段のように階段踏板のみの階段など、様々な形状の物体を認識できるという効果が得られる。
一方、発明８、１０の脚車輪型ロボットによれば、脚車輪型ロボットを、その向きとは関係なく自由な方向へと移動させることができるので、各方向への素早い移動を実現できると共に、例えば、脚車輪型ロボットの各構成部が妨げとなって旋回できないような狭くて入り組んだエリアなど、脚車輪型ロボットの向きを変更することが困難なエリアにおいても活動が可能となるという効果が得られる。
【００２３】
また、発明９の脚車輪型ロボットによれば、旋回時に、脚車輪型ロボットの基体を所定の回転中心位置でヨー軸周りにに自転運動させたときの各車輪の操舵時の回転中心の描く円弧軌道と操舵時の回転中心との接点における該回転中心の運動方向と、各車輪の進行方向とを一致させることができるので、脚車輪型ロボットを、所定の旋回中心位置で前後移動させずに旋回（クローラなどによる超信地旋回と同等の旋回）をさせることができるという効果が得られる。また、基体の中心位置を、自転させる時の回転中心位置とすることで、最小の旋回半径で脚車輪型ロボットを旋回させることが可能である。
【発明を実施するための最良の形態】
【００２４】
〔第１の実施の形態〕
以下、本発明の第１の実施の形態を図面に基づき説明する。図１〜図１４は、本発明に係る３次元距離測定装置及び脚車輪型ロボットの第１の実施の形態を示す図である。
まず、図１に基づき、本発明に係る脚車輪型ロボットの概略構成を説明する。
図１は、本発明に係る脚車輪型ロボット１００の側面図である。
【００２５】
図１に示すように、脚車輪型ロボット１００は、基体と、基体に連結された４つの脚部とを有して構成されている。 各脚部は、３つの回転関節を介して連結されている。上段の回転関節は、脚車輪型ロボット１００の底面と直交する方向を軸方向として回転する。すなわち、ヨー軸回りに回転する。また、中段及び下段の回転関節は、上段の回転関節が図１の状態であるときは、ピッチ軸回りに回転し、上段の回転関節が図１の状態から９０度回転した状態であるときは、ロール軸回りに回転する。つまり、各脚部は、それぞれ３自由度を有する。
【００２６】
また、図１に示すように、脚車輪型ロボット１００の基体上部には、３次元距離測定装置２００が配設されている。図１に示すように、３次元距離測定装置２００及び後述する測距センサ１１２ａの原点位置（回転角度θ及びφが０[°]の位置）における、測距センサ１１２ａの測定方向の軸がｘ軸、測定方向に対して水平方向の軸（後述するｙ軸）と直交する軸がｚ軸となる。
【００２７】
次に、図２及び図３に基づき、３次元距離測定装置２００の機械的な構成を説明する。
ここで、図２（ａ）は、３次元距離測定装置２００の機械的な構成を示す図であり、（ｂ）は、（ａ）を真上から見た図である。また、図３は、測距センサ１１２ａの走査範囲の一例を示す図である。
図２（ａ）に示すように、３次元距離測定装置２００は、２次元距離測定装置１１２と、モータ１１６と、エンコーダ１１８と、プーリ１２０ａ及び１２０ｂと、ベルト１２１とを含んだ構成となっている。
【００２８】
モータ１１６の回転軸（以下、第１の回転軸と称す）は、プーリ１２０ａの回転軸となるように当該プーリ１２０ａと係合されており、モータ１１６の回転駆動力が、プーリ１２０ａを回転駆動する。また、プーリ１２０ａ及びプーリ１２０ｂは、その軸心Ａ及びＢの位置が水平方向に所定の距離を開けるように配設されている。このように、プーリ１２０ａ及び１２０ｂを介して、モータ１１６の軸心Ａと、２次元距離測定装置１１２の回転軸（以下、第２の回転軸と称す）の軸心Ｂとを水平方向にずらすことで、モータ１１６及びエンコーダ１１８と、２次元距離測定装置１１２とを水平方向に並べた構成とすることができる。また、図２（ａ）に示すように、プーリ１２０ａ及び１２０ｂ、ベルト１２１、その他の支持部材は、２次元距離測定装置１１２の上部に配設している。
【００２９】
そして、水平方向に所定の距離を開けて配設されたプーリ１２０ａ及びプーリ１２０ｂには、外周面に窪みが設けられており、両者の窪みには、プーリ１２０ａの回転をプーリ１２０ｂに伝えるために、ベルト１２１が架け渡されている。つまり、プーリ１２０ａが回転することでベルト１２１が回転し、プーリ１２０ｂに、この回転駆動力を伝える。
また、プーリ１２０ｂと、第２の回転軸とは、当該第２の回転軸がプーリ１２０ｂの回転に連動して回転するように係合されている。従って、プーリ１２０ｂに伝達された回転駆動力は、第２の回転軸へと伝達される。
【００３０】
つまり、モータ１１６がその回転軸を回転駆動することで、その回転駆動力は、プーリ１２０ａ、ベルト１２１、プーリ１２０ｂを介して、第２の回転軸へと伝達され、図２（ｂ）に示すように、２次元距離測定装置１１２を垂直方向の軸（ｚ軸）まわり回転駆動する。
一方、本実施の形態の２次元距離測定装置１１２における測距センサ１１２ａは、水平方向の軸（ｙ軸）まわりに回転駆動しながら距離を測定する。また、測距センサ１１２ａの走査範囲は、脚車輪型ロボット１００が階段の昇降や障害物の回避を行うことを目的としているため、図３に示すように、主に脚車輪型ロボット１００の下方を重点的に走査する範囲となっている。
【００３１】
３次元距離測定装置２００は、図１に示すように、２次元距離測定装置１１２を回転駆動する駆動機構（モータ１１６、エンコーダ１１８、プーリ１２０ａ及び１２０ｂ、ベルト１２１、その他の支持部材）が、図３に示す走査範囲外に設けられているため、図３に示す走査範囲であれば、３次元距離測定装置２００を構成する各機構部によって、測距センサ１１２ａのｙ軸まわりの走査が阻害されない。
【００３２】
また、脚車輪型ロボット１００の走行経路上の障害物を認識できれば良いので、ｚ軸まわりの回転駆動による走査範囲も、前方１８０[°]までをカバーする必要はなく、２次元距離測定装置１１２の水平方向に設けられたモータ１１６及びエンコーダ１１８を走査範囲外とする範囲でも十分である。従って、本実施の形態においては、２次元距離測定装置１１２の回転駆動範囲を、モータ１１６及びエンコーダ１１８を含まない範囲とする。
【００３３】
次に、図４に基づき、３次元距離測定装置２００の構成を説明する。
図４（ａ）は、３次元距離測定装置２００の構成を示すブロック図であり、（ｂ）は、本実施の形態に係る２次元距離測定装置１１２の詳細な構成を示すブロック図である。
３次元距離測定装置２００は、図４（ａ）に示すように、センシングプロセッサ１１０と、２次元距離測定装置１１２と、指令信号及び後述するエンコーダ１１８の出力信号に基づき後述するモータ１１６を制御するドライバ１１４と、２次元距離測定装置１１２を回転駆動するモータ１１６と、モータ１１６の回転角度位置を検出するエンコーダ１１８とを含んで構成されている。
【００３４】
センシングプロセッサ１１０は、専用のプログラムを実行し、後述するドライバ１１２ｂに指令信号を与えて、後述する測距センサ１１２ａを回転駆動し、その測定平面内に存在する物体上の距離を測定する第１走査処理を実行すると共に、１つの測定平面に対する第１走査処理が終了する毎に、ドライバ１１４に指令信号を与えて、２次元距離測定装置１１２を回転駆動する第２走査処理を実行する。
【００３５】
更に、センシングプロセッサ１１０は、上記第１走査処理及び上記第２走査処理を経て、２次元距離測定装置１１２で測定した距離の情報（以下、距離情報という）に基づき、各測定平面の各測定点の勾配の出現頻度を算出する処理を実行すると共に、当該算出した出現頻度及び各測定点の座標情報に基づき、各測定平面における物体の１次元の形状（凹凸）及び測定範囲における物体の面の形状を認識する処理を実行する。
【００３６】
２次元距離測定装置１１２は、図４（ｂ）に示すように、測定範囲内に存在する物体上の測定点までの距離を測定する測距センサ１１２ａと、指令信号及び後述するエンコーダ１１２ｄの出力信号に基づきモータ１１２ｃの駆動を制御するドライバ１１２ｂと、モータ１１２ｃと、モータ１１２ｃの回転角度位置を検出するエンコーダ１１２ｄとを含んで構成されている。
【００３７】
ドライバ１１２ｂは、センシングプロセッサ１１０からの指令信号において指定された走査角度範囲及び走査角度単位（例えば、０．３６[°]などの所定の回転角度）に基づき、モータ１１２ｃの回転軸を、指定された走査角度単位ずつ回転させる制御を行う。
ここで、測距センサ１１２ａの原点位置（走査角度が０[°]の位置）における、レーザの出力方向の軸をｘ軸、ｘ軸に対して直交する２つの軸のうち一方の軸をｙ軸（本実施の形態では、センサの横方向と水平方向の軸となる）、他方の軸をｚ軸（本実施の形態では、垂直方向の軸となる）とする。
【００３８】
モータ１１２ｃは、測距センサ１１２ａのレーザ出力部（不図示）及び受光部（不図示）を、水平方向の軸（ｙ軸）まわりに回転駆動するように設けられており、ドライバ１１２ｂからの制御信号に応じて、自己の回転軸を指定された走査角度単位（回転角度θ）ずつ回転駆動する。
ここで、図５は、２次元距離測定装置１１２の距離測定の原理を説明する図である。
【００３９】
２次元距離測定装置１１２は、測距センサ１１２ａが、モータ１１２ｃの回転軸の回転駆動に応じて、水平方向の軸（ｙ軸）まわりに指定された走査角度単位ずつ回動し、且つ、回動する毎に、図５に示すように、レーザ出力部からレーザ光を出力すると共に、出力光に対する物体（図５中の障害物）からの反射光を受光部で受光して、各回転角度（走査角度）に応じた距離（図２中の測定距離Ｌ（物体と受光部との間の距離））を測定する。
【００４０】
また、図６（ａ）は、水平方向の軸（ｙ軸）まわりに測距センサ１１２ａを回転駆動させたときの測定距離と回転角度θとの関係を示す図であり、（ｂ）は、垂直方向の軸（ｚ軸）まわりに２次元距離測定装置１１２を回転駆動したときの測定平面と回転角度φとの関係を示す図である。
つまり、上記第１走査処理は、例えば、図６（ａ）に示すように、測距センサ１１２ａを、水平方向の軸（ｙ軸）まわりに指定された走査角度単位ずつ回動させながら、原点位置に対する各回転角度（図６（ａ）中のθ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)は、ステップＳ１３２に移行する。
【００５６】
ステップＳ１２８に移行した場合は、センシングプロセッサ１１０において、ステップＳ１２４においてＲＡＭ等の記録媒体に記録した連続面と判断された測定距離の情報に基づき、面データを生成して、ステップＳ１３０に移行する。
ここで、面データの生成は、例えば、各測定平面間において、連続面と判断された勾配及びその測定点の座標情報とを比較し、座標値と勾配とが近いものを面と判断して、これらの座標情報を対応付けたり、公知の補完法を用いてつなぎ合わせたりすることで行う。
【００５７】
例えば、勾配が０に近い測定点の連続する座標情報から構成される面データの面は、水平面と判断できるので、そこが歩行可能な面だと判断できる。
ステップＳ１３０では、３次元距離測定装置２００において、後述するハブ７６及び通信Ｉ／Ｆ６４を介して、ステップＳ１２８で生成した面データを、ＣＰＵ６０に出力して処理を終了する。
【００５８】
一方、ステップＳ１２６において、全ての測定平面に対して連続面の判断処理が終了しておらず、ステップＳ１３２に移行した場合は、３次元距離測定装置２００において、センシングプロセッサ１１０からの指令信号に応じて、ドライバ１１４、モータ１１６及びエンコーダ１１８を駆動し、２次元距離測定装置１１２を、ステップＳ１００で設定されたｚ軸まわりの走査角度範囲内において、同じくステップＳ１００で設定された走査角度単位ずつｚ軸まわりに回動させる処理（第２走査処理）を実行して、ステップＳ１０２に移行する。
【００５９】
また、ステップＳ１１４において、出現頻度が閾値以上ではなく、ステップＳ１３４に移行した場合は、センシングプロセッサ１１０において、ステップＳ１１２で選択した勾配に対応する測定点の測定距離を測定ノイズとして除外し、ステップＳ１１８に移行する。
次に、図１０に基づき、脚車輪型ロボット１００の移動制御システムを説明する。
【００６０】
ここで、図１０は、脚車輪型ロボット１００の移動制御システムを示すブロック図である。
脚車輪型ロボット１００の４本の各脚部の回転関節には、図１０に示すように、各回転関節を回転駆動する関節モータ４０がそれぞれ設けられている。各関節モータ４０には、関節モータ４０の回転角度位置を検出するエンコーダ４２と、モータ指令信号およびエンコーダ４２の出力信号に基づいて関節モータ４０の駆動を制御するドライバ４４が設けられている。
【００６１】
各脚部の駆動輪には、駆動輪を回転駆動する車輪モータ５０がそれぞれ設けられている。各車輪モータ５０には、車輪モータ５０の回転角度位置を検出するエンコーダ５２と、モータ指令信号およびエンコーダ５２の出力信号に基づいて車輪モータ５０の駆動を制御するドライバ５４が設けられている。
脚車輪型ロボット１００は、さらに、ＣＰＵ６０と、脚車輪型ロボット１００の姿勢を検出する３軸姿勢センサ７０と、カメラ３２の画像信号を処理するビジョンプロセッサ７２と、３次元距離測定装置２００と、外部のＰＣ等と無線通信を行う無線通信部７４と、ビジョンプロセッサ７２、無線通信部７４およびセンシングプロセッサ１１０とＣＰＵ６０の入出力を中継するハブ７６と、警告音等を出力するスピーカ７８とを有して構成される。
【００６２】
３軸姿勢センサ７０は、ジャイロ若しくは加速度センサ、またはその両方を有し、地軸に対して脚車輪型ロボット１００の姿勢の傾きを検出する。
ＣＰＵ６０は、モータ指令出力Ｉ／Ｆ６１を介してドライバ４４、５４にモータ指令信号を出力し、角度取込Ｉ／Ｆ６２を介してエンコーダ４２、５２の出力信号を入力する。なお、図示しないが、モータ指令出力Ｉ／Ｆ６１を介してモータ８２のドライバにモータ指令信号を出力する。
【００６３】
ＣＰＵ６０は、センサ入力Ｉ／Ｆ６３を介して、脚先センサ３８、３次元距離測定装置２００および３軸姿勢センサ７０からそれぞれセンサ信号を入力する。また、通信Ｉ／Ｆ６４を介してハブ７６と信号の入出力を行い、サウンド出力Ｉ／Ｆ６５を介してスピーカ７８に音声信号を出力する。
次に、ＣＰＵ６０で実行される処理を説明する。
【００６４】
ＣＰＵ６０は、３次元距離測定装置２００から面データが入力されると、そのセンサ座標系の座標をグローバル座標系に変換する座標変換処理を行う。座標変換処理では、脚車輪型ロボット１００に対する中心のグローバル座標をＸc、Ｙc、Ｚc、脚車輪型ロボット１００の姿勢（ロール角、ピッチ角、ヨー角）をφ、θ、ψとして、センサ座標系における各測定点を、グローバル座標系の測定点（Ｘs、Ｙs、Ｚs）に変換する。
【００６５】
更に、３軸姿勢センサ７０のセンサ信号、並びにグローバル座標系に変換された各測定点に基づいて逆運動学計算および重心計算を行い、この計算結果に基づいて脚先（駆動輪）の着地位置を決定する。更に、物体平面までの距離を算出し、脚先と物体平面の位置関係を算出し、決定した着地位置および算出した両距離に基づいてドライバ４４、５４へのモータ指令信号を生成し、生成したモータ指令信号をドライバ４４、５４に出力する。
【００６６】
更に、脚先が物体平面に着地したか否かを判定し、脚先が着地したと判定したときは、一連の処理を終了して元の処理に復帰させる。 一方、脚先が着地しないと判定したときは、面データを入力する処理から上記一連の処理を行う。
次に、図１１〜図１４に基づき、本実施の形態の動作を説明する。
ここで、図１１は、測定範囲内に階段がある場合の、第１走査処理による測定結果を示す図である。また、図１２は、図１１の各測定点の回転座標系の測定距離を直交座標系の測定距離に変換した場合の測定結果を示す図である。また、図１３（ａ）及び（ｂ）は、最小二乗法を用いて算出したｘ方向及びｚ方向の勾配の一例を示す図である。また、図１４は、ある測定平面に対する勾配の出現頻度の一例を示す図である。
【００６７】
まず、３次元距離測定装置２００は、ＣＰＵ６０からの指令信号に応じて、センシングプロセッサ１１０において走査角度範囲及び走査角度単位を設定する（ステップＳ１００）。
ここで、２次元距離測定装置１１２は、測距範囲２０〜４０９５[ｍｍ]、最大走査角度範囲２４０[°]、角度分解能０．３６[°]の２次元レンジセンサであることとする。走査角度範囲が２４０[°]であれば、先述した図３に示す測定平面を形成する走査角度範囲と同じとなり、２次元距離測定装置１１２の上部にある支持部材、プーリ２０及びベルト１２１が走査範囲に含まれないことになる。
【００６８】
また、第１走査処理に対して、走査角度範囲２４０[°]及び走査角度単位０．３６[°]が設定され、第２走査処理に対して、走査角度範囲−４０[°]〜＋４０[°]及び走査角度単位１０[°]が設定されたとする（この場合は、測定平面が９つ形成される）。走査角度範囲−４０[°]〜＋４０[°]であれば、２次元距離測定装置１１２の水平方向に並べて設けられたモータ１１６及びエンコーダ１１８が走査範囲に含まれないことになる。
【００６９】
センシングプロセッサ１１０は、まず、第１走査処理に対して設定された走査角度範囲及び走査角度単位（θ）に基づき、ドライバ１１２ｂに指令信号を出力する。
２次元距離測定装置１１２は、ドライバ１１２ｂにおいて、センシングプロセッサ１１０からの指令信号及びエンコーダ１１２ｄからの出力信号に基づき、モータ１１２ｃの回転軸を回転駆動して、測距センサ１１２ａを、水平方向の軸（ｙ軸）まわりに、回転角度「θ＝０．３６[°]」ずつ回動すると共に、各回転角度に応じた距離を測定する処理（第１走査処理）を実行する（ステップＳ１０２）。最初は、垂直方向の軸（ｚ軸）まわりに対して回転角度φが０[°]の位置（初期位置）に対する第１走査処理を実行する。また、各測定距離の情報（回転座標系）は、データ列Ｌ（θi，φj）として、センシングプロセッサ１１０へと出力される。
【００７０】
なお、水平方向の軸（ｙ軸）まわりの走査範囲内には、ロボットの基体以外に、２次元距離測定装置１１２の駆動機構等の走査を阻害する物が一切存在しないため、走査範囲内に存在する物体の正確な距離情報を得ることができる。
そして、１つの測定平面に対する第１走査処理が終了すると、センシングプロセッサ１１０は、第１走査処理において測定された距離情報に対して、メディアンフィルタを用いたフィルタリング処理を行う（ステップＳ１０４）。これにより、測定情報におけるノイズ成分を除去する。
【００７１】
ここで、ノイズ成分除去後の各測定点の測定距離Ｌ[ｍｍ]は、例えば、図１１に示すようになる。図１１において、横軸は各走査角度に応じた測定点の番号（第１走査角度番号）であり、縦軸は各走査角度番号の測定点に対する測定距離Ｌ[ｍｍ]である。
図１１に示す例では、脚車輪型ロボット１００の歩行経路上に、段差が一定で且つ蹴込板が連続面となっている階段が存在している。
【００７２】
次に、センシングプロセッサ１１０は、フィルタリング処理後の回転座標系の距離情報を、直交座標系の距離情報に変換する（ステップＳ１０６）。
つまり、図１１に示す各測定点の回転座標系の測定距離を、直交座標系の測定距離へと変換することで、図１２に示すように、各回転角度に対応するｘ軸方向の距離[ｍｍ]とｚ軸方向の距離[ｍｍ]とで表現される２次元の座標情報となる。図１２において、横軸はｘ軸方向の距離Ｌx[ｍｍ]であり、縦軸はｚ軸方向の距離Ｌz[ｍｍ]である。
【００７３】
次に、直交座標系に変換された距離情報に基づき、各測定点の勾配を算出する（ステップＳ１０８）。
ここでは、最小二乗法を用いて、各測定点の勾配を算出する。ここで、最小二乗法を用いた場合に、図１３（ａ）及び（ｂ）に示すように、最小二乗法を適用する複数の測定点に対して、ｘ方向の勾配と、ｚ方向の勾配とが異なる場合がある。そこで、本実施の形態においては、下式（１）及び（２）に示す、ｘ方向及びｚ方向の勾配を求める２つの直線の方程式を用いて最小二乗法計算を行う。
ｚ＝ａx・ｘ＋ｂx ・・・（１）
ｘ＝ａz・ｚ＋ｂz ・・・（２）
そして、上式（１）におけるａx及びｂxを、下式（３）に従って算出し、上式（２）におけるａz及びｂzを、下式（４）に従って算出する。
【００７４】
【数１】
【００７５】
ａx及びｂx、並びにａz及びｂzを算出すると、これらの算出結果を代入した、上式（１）及び（２）の直線と、最小二乗計算に用いた各測定点との距離を残差ｈx及びｈzとして算出すると共に、前記各測定点に対する残差ｈxの二乗和と、残差ｈzの二乗和とを算出する（下式（５））。
【００７６】
【数２】
【００７７】
残差ｈxの二乗和及び残差ｈzの二乗和が算出されると、両者を比較して値の小さい方の直線を、正しい直線として採用し、この直線の勾配に基づき、算出対象の測定点の勾配を求める。具体的に、残差ｈxの二乗和の方が小さければ、「ａ＝ａx、ｂ＝ｂx」とし、残差ｈzの二乗和の方が小さければ、「ａ＝１／ａz、ｂ＝１／ｂz」とする。
【００７８】
上記のようにして、測定平面に対する全測定点の勾配が算出されると、次に、各勾配の出現頻度を算出する（ステップＳ１１０）。
ここでは、図１４に示すような、出現頻度が得られたとする。
図１４に示すように、勾配が０近傍の出現頻度が比較的高くなっており、これらの勾配に対応する測定点の座標情報が連続していれば、そこに水平面があると推定することができる。
【００７９】
出現頻度が算出されると、センシングプロセッサ１１０は、次に、未判定の勾配の出現頻度を選択し（ステップＳ１１２）、その出現頻度が、予め設定された閾値以上か否かを判定する（ステップＳ１１４）。そして、閾値以上であれば（ステップＳ１１４の「Ｙｅｓ」の分岐）、勾配と測定点の座標情報とを対応付けてＲＡＭ（不図示）に記録する（ステップＳ１１６）。一方、閾値未満であれば（ステップＳ１１４の「Ｎｏ」の分岐）、測定ノイズとして、測定点の距離情報を除外する（ステップＳ１３４）。
【００８０】
上記の判定処理が、全ての勾配に対して終了すると（ステップＳ１１８の「Ｙｅｓ」の分岐）、次に、ＲＡＭに記録された、出現頻度が閾値以上の勾配及び測定点の座標情報に基づき、連続面の有無を判断する（ステップＳ１２０）。
脚車輪型ロボット１００の移動経路上には、段差が一定で且つ蹴込板が連続面となっている階段が存在しているため、階段部分にある水平面などが、勾配０及び略０の測定点の連続する座標として現われる。このような場合に、センシングプロセッサ１１０は、連続面（実際は、面と測定平面との交差線）がある（ステップＳ１２２の「Ｙｅｓ」の分岐）と判断する。一方、座標の途中で勾配がいきなり外れた値となったときは、そこで面（実際は、面と走査平面との交差線）が途切れていると判断する。これらの判断結果から、物体の凹凸を認識することが可能である。
【００８１】
そして、連続面に対応する直交座標系の測定距離を、センサ座標系（３次元）の測定距離に変換し、ＲＡＭに記録する（ステップＳ１２４）。
連続面の判断が終了すると、３次元距離測定装置２００は、ドライバ１１４において、センシングプロセッサ１１０からの指令信号及びエンコーダ１１８からの出力信号に基づき、モータ１１６の回転軸を回転駆動して、２次元距離測定装置１１２を、垂直方向の軸（ｚ軸）まわりに、回転角度１０[°]ずつ回動する処理（第２走査処理）を実行する（ステップＳ１３２）。この第２走査処理によって、ｚ軸はそのままで、ｘ軸の位置が１つ前の位置に対して垂直方向の軸（ｚ軸）まわりに１０[°]だけ変化する。そして、２次元距離測定装置１１２は、この状態で、再び、上記第１走査処理を実行する（ステップＳ１０２）。これにより、ｚ軸まわりに１０[°]ずれた位置に新たな測定平面が形成される。
【００８２】
そして、設定された走査角度範囲（−４０[°]〜＋４０[°]）に対して、設定された走査角度単位１０[°]で、２次元距離測定装置１１２を回動させ、各回転角度φにおいて、上記第１走査処理を実行する。
ここで、２次元距離測定装置１１２が脚車輪型ロボット１００の基体上部に取り付けられているため、当該基体が走査範囲内に含まれてしまうが、脚車輪型ロボット１００の前方及び歩行経路上を含む範囲においては阻害物が一切ないため、脚車輪型ロボット１００の歩行制御を行うのに十分な走査範囲が確保できているといえる。
【００８３】
そして、９つの測定平面に対する距離の測定が終了すると（ステップＳ１２６の「Ｙｅｓ」の分岐）、次に、ＲＡＭに記録された連続面の情報に基づき、面データを生成する（ステップＳ１２８）。
面データは、先述したように、ｚ軸まわりに連続する各隣り合う測定平面間において、勾配及び座標の近い連続面をつなぎ合わせる（対応付ける）ことで生成される。生成した面データは、ハブ７６および通信Ｉ／Ｆ６４を介して、ＣＰＵ６０に出力される（ステップＳ１３０）。この面データによって、測定範囲内に存在する階段などの物体の構成面の詳細な形状を認識することが可能である。
【００８４】
一方、ＣＰＵ６０では、入力された面データを解析して、特に、勾配が０に近い交差線を有する面がある場合は、それを水平面とみなし、脚車輪型ロボット１００が歩行可能な面であると判断する。
面データの示す面が歩行可能な面であると判断された場合は、入力された面データのセンサ座標系をグローバル座標系に変換し、この変換結果と、３軸姿勢センサ７０のセンサ信号などとに基づき、脚先の着地位置を決定する。
【００８５】
更に、脚先センサ３８からのセンサ信号に基づき、例えば、対象が階段である場合は、蹴込板までの距離および脚先と踏板の位置関係とを算出する。そして、決定された着地位置および算出された両距離に基づいてモータ指令信号が生成され、生成されたモータ指令信号がドライバ４４に出力される。これにより、回転関節が駆動し、脚車輪型ロボット１００が姿勢を適切に保ちつつ階段を乗り越える。また、状況によっては階段を回避、停止する。
【００８６】
なお、段差が一定で且つ蹴込板が連続面となる階段を例に挙げたが、段差が一定でない階段、蹴込板が連続面とならない階段、蹴込板の無い階段などに対しても、正確にその面形状を認識することができるので、脚車輪型ロボット１００の階段への適応性を高めることが可能である。
また、脚車輪型ロボット１００は、平地では、車輪走行で移動することができる。したがって、車輪型と同様に平地での移動性が高い。
【００８７】
上記したように、本実施の形態の脚車輪型ロボット１００は、その３次元距離測定装置２００の機械的な構成を、モータ１１６及びエンコーダ１１８と、２次元距離測定装置１１２とを水平方向に並べて配設し、モータ１１６の第１の回転軸の回転駆動力を、プーリ１２０ａ、ベルト１２１、プーリ１２０ｂを介して、２次元距離測定装置１１２を回転する第２回転軸へと伝達し、２次元距離測定装置１１２を、垂直方向の軸（ｚ軸）まわりに回転駆動する構成とした。
【００８８】
これにより、２次元距離測定装置１１２の所定走査角度範囲内には、走査を除外するものが一切なくなるので、正確な距離情報を得ることができる。また、モータ１１６及びエンコーダ１１８と、２次元距離測定装置１１２とを水平方向に並べて配設したので、３次元距離測定装置２００の高さ方向の占有率を低減することができる。
また、３次元距離測定装置２００において、水平方向の軸まわりに測距センサ１１２ａを、設定された走査角度単位ずつ回動させると共に、各回転角度における距離を測定する第１走査処理を実行し、測定距離の情報に基づき、各測定点の勾配及び勾配の出現頻度を算出することで、測定平面内に存在する物体の凹凸を認識することができる。
【００８９】
これにより、連続面が存在することを推定することができる。
更に、１つの測定平面に対する第１走査処理が終了する毎に、２次元距離測定装置１１２を、垂直方向の軸まわりに、設定された走査角度単位だけ回動させる第２走査処理を実行することができる。これにより、複数の測定平面に対して第１走査処理を実行することができるので、測定範囲内に存在する物体の面形状を認識することができる。また、面形状が認識できるので、脚車輪型ロボット１００は、未知の障害物に対して高い適応性を実現することができる。また、人が活動する環境での動作を行えるので、人と一緒に行動する用途に用いられるホームロボット、パーソナルロボットなどのヒューマンアシストロボットに好適である。
【００９０】
上記第１の実施の形態において、モータ１１６及びエンコーダ１１８は、発明２、３、４及び５のいずれか１の回転駆動装置に対応し、プーリ１２０ａ、ベルト１２１及びプーリ１２０ｂによって、モータ１１６の第１の回転軸の回転駆動力を２次元距離測定装置１１２の第２の回転軸に伝達する機構は、発明２又は３の動力伝達機構に対応し、プーリ１２０ａ及び１２０ｂは、発明３の滑車に対応する。
【００９１】
また、上記第１の実施の形態において、ドライバ１１２ｂ、モータ１１２ｃ及びエンコーダ１１２ｄによる測距センサ１１２ａの回転機構は、発明１、２、５及び６のいずれか１の第１の回転機構に対応し、センシングプロセッサ１１０によって２次元距離測定装置１１２を制御することで第１走査処理を実行する機能は、発明５又は６の第１走査手段に対応し、センシングプロセッサ１１０における各測定点の勾配を算出する機能は、発明５又は６の勾配算出手段に対応し、センシングプロセッサ１１０における各勾配の出現頻度を算出する機能は、発明５又は６の出現頻度算出手段に対応し、センシングプロセッサ１１０における連続面の判断処理は、発明５の形状認識手段に対応する。
【００９２】
また、上記第１の実施の形態において、ドライバ１１４、モータ１１６、エンコーダ１１８、不図示の減速機及びプーリによる２次元距離測定装置１１２の回転機構は、発明１、２及び６のいずれか１の第２の回転機構に対応し、センシングプロセッサ１１０によってドライバ１１４を制御することで第２走査処理を実行する機能は、発明６の第２走査手段に対応し、３次元距離測定装置２００における面データの生成機能及びＣＰＵ６０による面形状の認識機能は、発明６の形状認識手段に対応する。
【００９３】
また、上記第１の実施の形態において、ステップＳ１０２は、発明５又は６の第１走査手段に対応し、ステップＳ１０４〜Ｓ１０８は、発明５又は６の勾配算出手段に対応し、ステップＳ１１０は、発明５又は６の出現頻度算出手段に対応し、ステップＳ１１２〜Ｓ１２４は、発明５又は６の形状認識手段に対応し、ステップＳ１２６〜Ｓ１３２は、発明６の第２走査手段に対応する。
〔第２の実施の形態〕
次に、本発明の第２の実施の形態を図面を参照しながら説明する。図１６ないし図２２は、本発明に係る３次元距離測定装置及び脚車輪型ロボットの第２の実施の形態を示す図である。
【００９４】
本実施の形態においては、上記第１の実施の形態と同様に、上記第１の実施の形態で説明した３次元距離測定装置２００を脚車輪型ロボットに適用する。
本実施の形態の脚車輪型ロボットは、上記第１の実施の形態で説明した３次元距離測定装置２００を搭載した脚車輪型ロボット１００の各機能に加え、脚車輪型ロボット１００を、基体１０の向きを一定方向に保持した状態で任意の進行方向に走行移動させる機能と、脚車輪型ロボット１００を、所定の旋回中心位置で前後移動させずに旋回させる（超信地旋回させる）機能とを有している。
【００９５】
つまり、本実施の形態における脚車輪型ロボット１００は、上記各機能を実現するためのアクチュエータの制御処理（ＣＰＵ６０の制御プログラムの実行による制御処理）が追加されたのみで、力センサ８２の設置構造など、その他の構成は、上記第１の実施の形態の脚車輪型ロボット１００と同様となる。従って、上記第１の実施の形態と同様の機能については説明を適宜省略し、追加された機能部分については詳細に説明する。
【００９６】
まず、本発明を適用する脚車輪型ロボット１００の詳細な構成を説明する。
図１６は、脚車輪型ロボット１００の正面図である。
図１７は、脚車輪型ロボット１００の側面図である。
脚車輪型ロボット１００は、図１６および図１７に示すように、基体１０と、基体１０に連結された４つの脚部１２とを有して構成されている。
【００９７】
基体１０の前方には、２本の脚部１２が回転関節１４を介して左右対称の位置に連結されている。また、基体１０の後方には、２本の脚部１２が回転関節１４を介して左右対称の位置に連結されている。
回転関節１４は、脚車輪型ロボット１００の底面と直交する方向を軸方向として回転する。すなわち、ヨー軸回りに回転する。
【００９８】
各脚部１２には、２つの回転関節１６、１８が設けられている。回転関節１６、１８は、回転関節１４が図１６のような状態であるときは、脚車輪型ロボット１００の側面と直交する方向を軸方向として回転する。したがって、脚部１２は、それぞれ３自由度を有する。
各脚部１２の先端には、回転関節１６、１８と軸方向を同一にして駆動輪２０が回転可能に設けられている。駆動輪２０は、回転関節１４の回転によりヨー軸周りに回動する。つまり、回転関節１４の回転を制御することで、走行移動時の操舵制御が行われる。
【００９９】
各脚部１２の先端には、脚車輪型ロボット１００の移動経路上に存在する物体までの脚先からの距離を測定する脚先センサ３８が設けられている。
一方、基体１０の正面の下部中央には、水平面レーザ光を照射する水平レーザ２６が設けられている。また、基体１０の正面の中央左右には、垂直面レーザ光を照射する垂直レーザ２８、３０がそれぞれ設けられている。
【０１００】
基体１０の正面の上部中央には、水平面レーザ光および垂直面レーザ光の反射光を含む画像を撮影するカメラ３２が設けられている。
また、上記第１の実施の形態で述べたように、カメラ３２と３次元距離測定装置２００とは、支持部材及びプーリを介して、カメラ３２が上側に、３次元距離測定装置２００が下側に配設されている。
【０１０１】
水平レーザ２６は、カメラ３２で水平面レーザ光の反射光を含む画像が撮影できるように下方に所定角度傾けて設けられている。同様に、垂直レーザ２８は、カメラ３２で垂直面レーザ光の反射光を含む画像が撮影できるように右方に所定角度傾けて設けられ、垂直レーザ３０は、左方に所定角度傾けて設けられている。
カメラ３２の左右には、障害物を検出する障害物センサ３４、３６がそれぞれ設けられている。
【０１０２】
なお、各脚部１２の回転関節１４〜１８には、上記第１の実施の形態における図１０に示すように、回転関節１４〜１８を回転駆動する関節モータ４０がそれぞれ設けられている。また、各脚部１２の駆動輪２０には、図１０に示すように、駆動輪２０を回転駆動する車輪モータ５０がそれぞれ設けられている。
次に、図１８ないし図２０に基づき、本実施の形態のＣＰＵ６０で実行される走行制御処理を説明する。
【０１０３】
ここで、図１８（ａ）及び（ｂ）は、脚車輪型ロボット１００の車輪走行移動時の姿勢を示す図である。
以下の、各走行制御処理においては、脚車輪型ロボット１００の姿勢が、図１８（ｂ）に示すように、膝屈曲姿勢となるように各関節モータ４０を制御する。但し、膝屈曲姿勢における走行制御時に、脚部１２が互いに干渉する（接触などする）場合は、図１８（ａ）に示すように、膝伸展姿勢となるように各関節モータ４０を制御する。
【０１０４】
まず、基体１０の向きを一定の方向に保持（固定）した状態で、脚車輪型ロボット１００を目的の進行方向に向けて走行させる走行制御処理（以下、無変向走行制御処理と称す）について説明する。
ＣＰＵ６０は、ＲＯＭ等の所定領域に格納されている制御プログラムを起動させ、その制御プログラムに従って、無変向走行制御処理を実行する。
【０１０５】
ここで、無変向走行制御処理は、脚車輪型ロボット１００を、その基体１０の向きを一定の方向に保持した状態で、目的の進行方向に移動させるものであるため、基体１０の前方側に設けられたカメラ３２、３次元距離測定装置２００、障害物センサ３４、３６などを、後方や側方などにも設けることが望ましい。これら後方及び側方をカバーできるカメラ、３次元距離測定装置及び障害物センサなどによって、脚車輪型ロボット１００を、基体１０の向きとは異なる方向に移動させるときに、その進行方向の環境（地形の状態等）を把握し、適切な制御を行うことができる。
【０１０６】
また、無変向走行制御処理は、具体的に、無変向走行制御指令があったときに実行され、時々刻々の、ロボットの進行方向（角度α）、ロボット進行方向速度Ｖcを入力として、基体１０の向きを一定の方向に保持した状態で、脚車輪型ロボット１００を前記入力された進行方向へと走行させるための、各脚部１２の回転関節１４（joint0）の角度（操舵角度）θi0（ｉ=0,1,2,3,・・・）、各脚部１２の各駆動輪２０の回転角速度ωi（ｉ=0,1,2,3,・・・）を算出し、各アクチュエータに指令を与えるものである。
【０１０７】
本実施の形態の脚車輪型ロボット１００は、先述したように、基体１０の前方に左右一対及び後方に左右一対の計４本の脚部１２を有している。
従って、ここでは、各脚部１２の駆動輪２０の操舵角度θi0を、基体１０の上面側から見て、左前輪操舵角度θ00、右前輪操舵角度θ10、左後輪操舵角度θ20、右後輪操舵角度θ30とする。なお、回転関節１４によって各脚部１２をヨー軸周りに回動させたときに、基体１０の上面側から見て、左前輪操舵角度θ00及び右後輪操舵角度θ30は、反時計回り方向を正方向とし、右前輪操舵角度θ10及び左後輪操舵角度θ20は、時計回り方向を正方向とする。
【０１０８】
また、各脚部１２の駆動輪２０の回転角速度ωiを、左前輪回転角速度ω0、右前輪回転角速度ω1、左後輪回転角速度ω2、右後輪回転角速度ω3とする。
また、各脚部１２の駆動輪２０の線速度Ｖi（ｉ=0,1,2,3,・・・）を、左前輪線速度Ｖ0、右前輪線速度Ｖ1、左後輪線速度Ｖ2、右後輪線速度Ｖ3とする。
ここで、図１９（ａ）〜（ｃ）は、無変向走行制御時の脚車輪型ロボット１００の走行状態例を示す図である。なお、図１９（ａ）〜（ｃ）は、脚車輪型ロボット１００を上面側から見た図であり、各駆動輪２０に付けられた黒塗りの半円の目印は、「θ00＝θ10＝θ20＝θ30＝０[°]」のときの基準となる向きを示す。
【０１０９】
また、左右前輪は上記目印のある方向に進行する回転方向が正回転方向となり、左右後輪は目印の無い方向に進行する回転方向が正回転方向となる。
まず、図１９（ａ）に基づき、基体１０の向きを一定方向に保持（固定）した状態で、脚車輪型ロボット１００を、基体１０の向いている方向（前方向）に直進走行させる場合の無変向走行制御処理を説明する。
【０１１０】
本実施の形態では、基体１０の向いている方向（前方向）をロボットの進行方向とした場合に、進行方向を表す角度αを「０[°]」とする。そして、前方向の０[°]を基準に、各進行方向に対応するαを決定する。
ここでは、脚車輪型ロボット１００を、基体１０の前方向に直進させるので、進行方向αとして「０[°]」が入力され、更に、進行方向速度Ｖｃが入力される。
【０１１１】
進行方向α（０[°]）及び進行方向速度Ｖｃが入力されると、各駆動輪２０の操舵角度θ00、θ10、θ20、θ30、及び各駆動輪２０の線速度Ｖ0、Ｖ1、Ｖ2、Ｖ3が算出される。
基体１０の向きを保持した状態のまま、脚車輪型ロボット１００を前方向に直進走行させるためには、図１９（ａ）の各駆動輪２０から伸びる矢印に示すように、各駆動輪２０の進行方向を基体１０の向いている方向（前方向）に全て揃える必要がある。従って、操舵角度θ00、θ10、θ20、θ30は、例えば、「θ00＝θ10＝θ20＝θ30＝０[°]」と算出される。
【０１１２】
また、この場合に、直進走行させるための各駆動輪２０の線速度Ｖ0、Ｖ1、Ｖ2、Ｖ3は、「Ｖ0＝Ｖ1＝Ｖｃ」、「Ｖ2＝Ｖ3＝−Ｖｃ」と算出される。
なお、各脚部１２が互いに干渉しなければ、例えば、「θ00＝θ10＝０[°]」、「θ20＝θ30＝π（１８０[°]）又は−π（−１８０[°]）」、「Ｖ0＝Ｖ1＝Ｖ2＝Ｖ3＝Ｖｃ」などの組み合わせとしても良い。
【０１１３】
更に、線速度Ｖ0、Ｖ1、Ｖ2、Ｖ3は、下式（６）に従って、回転角速度ω0、ω1、ω2、ω3に変換される。
ωi＝２Ｖi／Ｄ ・・・（６）
但し、上式（６）において、Ｄは車輪径である。
【０１１４】
各駆動輪２０の操舵角度θ00、θ10、θ20、θ30、及び各駆動輪２０の回転角速度ω0、ω1、ω2、ω3が算出されると、次に、各駆動輪２０の現在の操舵角度及び回転角速度を取得する。そして、現在の操舵角度と、上記算出した前方向に直進させるための操舵角度とから回転関節１４の関節モータ４０を駆動する指令値を算出する。更に、現在の回転角速度と、上記算出した前方向に直進させるときの回転角速度とから、駆動輪２０の車輪モータ５０を駆動する指令値を算出する。
【０１１５】
このようにして、各駆動輪２０の操舵制御の指令値及び速度制御の指令値を算出すると、これらの指令値を各モータのドライバに入力する。そして、入力された指令値に基づき各関節モータ４０及び車輪モータ５０が駆動されると、これにより、脚車輪型ロボット１００の各駆動輪２０の操舵角度及び回転角速度が変化し、脚車輪型ロボット１００は、基体１０の向きを保持した状態で、その向いている方向（前方向）に直進走行する。
【０１１６】
なお、基体１０の向きを一定方向に保持した状態で、脚車輪型ロボット１００を、その向いている方向に対して反対側（真後ろ）の方向に直進走行させる場合は、上記前方向のときと駆動輪２０の回転方向を正反対とすればよい。
例えば、「θ00＝θ10＝θ20＝θ30＝０[°]」及び「Ｖ0＝Ｖ1＝Ｖ2＝Ｖ3＝−Ｖｃ」、又は「θ00＝θ10＝０[°]」、「θ20＝θ30＝π若しくは−π[°]」、「「Ｖ0＝Ｖ1＝−Ｖｃ」及び「Ｖ2＝Ｖ3＝Ｖｃ」などとする。
【０１１７】
次に、図１９（ｂ）に基づき、基体１０の向きを保持した状態（前方向に向けたままの状態）で、脚車輪型ロボット１００を、その向いている方向に対して右斜め前方向に直進走行させる場合の無変向走行制御処理を説明する。
ここでは、脚車輪型ロボット１００を、基体１０の右斜め前方向に直進させるので、進行方向αとして「α（−９０＜α＜０）[°]（但し、反時計回りが正方向）」が入力され、更に、進行方向速度Ｖｃが入力される。
【０１１８】
そして、進行方向α[°]及び進行方向速度Ｖｃが入力されると、各駆動輪２０の操舵角度θ00、θ10、θ20、θ30、及び各駆動輪２０の線速度Ｖ0、Ｖ1、Ｖ2、Ｖ3が算出される。
基体１０の向きを保持した状態で、脚車輪型ロボット１００を右斜め前方向（α[°]の方向）に直進走行させるためには、図１９（ｂ）の各駆動輪２０から伸びる矢印に示すように、各駆動輪２０の進行方向を、基体１０の向いている方向に対して右斜め前方向に全て揃える必要がある。
【０１１９】
従って、操舵角度θ00、θ10、θ20、θ30は、例えば、「θ00＝θ30＝α[°]」、「θ10＝θ20＝−α[°]」と算出される。
また、この場合に、直進走行させるための各駆動輪２０の線速度Ｖ0、Ｖ1、Ｖ2、Ｖ3は、「Ｖ0＝Ｖ1＝Ｖｃ」、「Ｖ2＝Ｖ3＝−Ｖｃ」と算出される。
なお、各脚部１２が互いに干渉しなければ、例えば、「θ00＝θ30＝α[°]」、「θ10＝θ20＝−α−π[°]」、「Ｖ0＝Ｖ2＝Ｖｃ」、「Ｖ1＝Ｖ3＝−Ｖｃ」などの組み合わせとしても良い。
【０１２０】
更に、線速度Ｖ0、Ｖ1、Ｖ2、Ｖ3は、上式（６）に従って、回転角速度ω0、ω1、ω2、ω3に変換される。
操舵角度θ00、θ10、θ20、θ30、及び各駆動輪２０の回転角速度ω0、ω1、ω2、ω3が算出されると、次に、上記前方向への直進走行のときと同様に、回転関節１４の関節モータ４０を駆動する指令値と、駆動輪２０の車輪モータ５０を駆動する指令値とを算出する。
【０１２１】
このようにして、各駆動輪２０の操舵制御の指令値及び速度制御の指令値を算出すると、これらの指令値を各モータのドライバに入力する。そして、入力された指令値に基づき各関節モータ４０及び車輪モータ５０が駆動されると、これにより、脚車輪型ロボット１００の各駆動輪２０の操舵角度及び回転角速度が変化し、脚車輪型ロボット１００は、基体１０の向きを保持した状態で、その向いている方向に対して右斜め前方向に直進走行する。
【０１２２】
なお、基体１０の向きを保持した状態で、脚車輪型ロボット１００を、その向いている方向に対して左斜め後方に直進走行させる場合は、上記右斜め前方向のときの各駆動輪２０の回転方向を正反対とすればよい。また、左斜め前方向に直進移動させる場合は、進行方向αを「０＜α＜９０[°]」の範囲で設定し、更に、操舵角度の符号を正反対とすればよい。また、右斜め後方に直進走行させる場合は、左斜め前方向のときの各駆動輪２０の回転方向を正反対とすればよい。
【０１２３】
次に、図１９（ｃ）に基づき、基体１０の向きを保持した状態で、脚車輪型ロボット１００を、その向いている方向に対して左真横方向に直進走行させる場合の無変向走行制御処理を説明する。
ここでは、脚車輪型ロボット１００を、基体１０の左真横方向に直進させるので、進行方向αとして「α（９０（π／２））[°]」が入力され、更に、進行方向速度Ｖｃが入力される。
【０１２４】
そして、進行方向α[°]及び進行方向速度Ｖｃが入力されると、各駆動輪２０の操舵角度θ00、θ10、θ20、θ30、及び各駆動輪２０の線速度Ｖ0、Ｖ1、Ｖ2、Ｖ3が算出される。
基体１０の向きを保持した状態で、脚車輪型ロボット１００を、その向いている方向に対して左真横方向（π／２[°]の方向）に直進走行させるためには、図１９（ｃ）の各駆動輪２０から伸びる矢印に示すように、各駆動輪２０の進行方向を左真横方向に全て揃える必要がある。
【０１２５】
従って、操舵角度θ00、θ10、θ20、θ30は、例えば、「θ00＝θ10＝θ20＝θ30＝π／２[°]」と算出される。
また、この場合に、直進走行させるための各駆動輪２０の線速度Ｖ0、Ｖ1、Ｖ2、Ｖ3は、「Ｖ0＝Ｖ2＝Ｖｃ」、「Ｖ1＝Ｖ3＝−Ｖｃ」と算出される。
なお、各脚部１２が互いに干渉しなければ、例えば、「θ00＝θ20＝π／２[°]」、「θ10＝θ30＝−π／２[°]」、「Ｖ0＝Ｖ1＝Ｖ2＝Ｖ3＝Ｖｃ」などの組み合わせとしても良い。
【０１２６】
更に、線速度Ｖ0、Ｖ1、Ｖ2、Ｖ3は、上式（６）に従って、回転角速度ω0、ω1、ω2、ω3に変換される。
操舵角度θ00、θ10、θ20、θ30、及び各駆動輪２０の回転角速度ω0、ω1、ω2、ω3が算出されると、次に、上記前方向への直進走行のときと同様に、回転関節１４の関節モータ４０を駆動する指令値と、駆動輪２０の車輪モータ５０を駆動する指令値とを算出する。
【０１２７】
このようにして、各駆動輪２０の操舵制御の指令値及び速度制御の指令値を算出すると、これらの指令値を各モータのドライバに入力する。そして、入力された指令値に基づき各関節モータ４０及び車輪モータ５０が駆動されると、これにより、脚車輪型ロボット１００の各駆動輪２０の操舵角度及び回転角速度が変化し、脚車輪型ロボット１００は、基体１０の向きを保持した状態で、その向いている方向に対して左真横方向に直進走行する。
【０１２８】
なお、基体１０の向きを保持した状態で、脚車輪型ロボット１００を、その向いている方向に対して右真横方向に直進走行させる場合は、上記左真横方向のときの各駆動輪２０の回転方向を正反対とすればよい。
次に、脚車輪型ロボット１００を、所定の旋回中心位置で前後移動させずに旋回させる走行制御処理（以下、超信地旋回制御処理と称す）について説明する。
【０１２９】
ＣＰＵ６０は、ＲＯＭ等の所定領域に格納されている制御プログラムを起動させ、その制御プログラムに従って、超信地旋回制御処理を実行する。
ここで、図２０（ａ）及び（ｂ）は、基体１０の座標（０，０）及び座標（ｘｃ，ｙｃ）を回転中心とした場合の超信地旋回制御時の脚車輪型ロボット１００の走行状態を示す図である。
【０１３０】
なお、図２０（ａ）及び（ｂ）においては、基体１０を上面側から見た平面において、長手方向の軸をｘ軸、それと直交する方向の軸をｙ軸とし、基体１０の中心位置の座標を（ｘ，ｙ）＝（０，０）とする。
超信地旋回制御処理は、クローラ機構を有したパワーショベルや戦車などの車両が行う超信地旋回と同等の旋回動作を脚車輪型ロボット１００に行わせるものである。
【０１３１】
ここで、超信地旋回とは、クローラ機構を有した車両が、左右のクローラを同速度で互いに反対方向に回転させることで、前後に進まず、車体の向きを変える旋回方法であり、これは、クローラ機構に限らず、左右に最低２輪の独立した駆動輪を有する車両であれば実現可能な旋回方法である。
本実施の形態においては、具体的に、超信地旋回制御指令があったときに実行され、ロボットの旋回角速度Ω、旋回中心（ｘｃ，ｙｃ）を入力として、脚車輪型ロボット１００を旋回中心（ｘｃ，ｙｃ）で超信地旋回させるための、各脚部１２の回転関節１４（joint0）の角度（操舵角度）θ00、θ10、θ20、θ30、各脚部１２の各駆動輪２０の回転角速度ω0、ω1、ω2、ω3を算出し、各アクチュエータに指令を与えるものである。
【０１３２】
脚車輪型ロボット１００を超信地旋回させるためには、基体１０を、旋回中心（ｘｃ，ｙｃ）でヨー軸周りに自転させたときに、各駆動輪２０の操舵時のヨー軸周りの回転中心が描く円弧軌道と各駆動輪２０の前記回転中心との接点における運動方向と、各駆動輪２０の進行方向とが一致するように各回転関節１４の関節モータ４０を制御すると共に、各駆動輪２０が前記運動方向に応じた回転方向に一定速度で回転するように各車輪モータ５０を制御する必要がある。
【０１３３】
まず、図２０（ａ）に基づき、基体１０の中心位置の座標（０，０）を旋回中心とした場合の超信地旋回制御処理について説明する。
この場合は、旋回角速度Ωと、旋回中心座標（０，０）とが入力される。
旋回角速度Ω及び旋回中心座標（０，０）が入力されると、下式（７）に基づき、基体１０のｘ軸と各駆動輪２０の回転中心とのなす角度φ0、φ1、φ2、φ3が算出される。
ｔａｎφ＝Ｗt／Ｗb ・・・（７）
但し、上式（７）は、基体１０の中心座標（０，０）を旋回中心とした場合の式である。また、上式（７）において、Ｗtはトレッド（車輪間隔）であり、Ｗbはホイールベースである。
【０１３４】
なお、φ0〜φ3は、ｘ軸と、各駆動輪２０の回転中心とのなす角度のうち最小の角度とし、φの正方向は、駆動輪によってはΩの正方向とは異なる。
Ｗt及びＷbは既知であるため（予め情報を持っておく）、上式（７）から、上記角度φ0、φ1、φ2、φ3を算出することができる。
なお、旋回中心が基体１０の中心座標（０，０）であるので、角度φ0、φ1、φ2、φ3はいずれも等角度「φ0＝φ1＝φ2＝φ3＝φ」となる。
【０１３５】
また、基体１０が旋回座標（０，０）でヨー軸周りに自転時に、各駆動輪２０の操舵時のヨー軸周りの回転中心の描く円弧軌道と該回転中心との接点における運動方向は、図２０（ａ）の各駆動輪２０から伸びる矢印線に示すように、円弧軌道上の各回転中心を通る接線方向（図中の矢印線方向）となる。
各駆動輪２０の回転中心に対する角度「φ0＝φ1＝φ2＝φ3＝φ」が算出されると、次に、各回転中心の運動方向と各駆動輪２０の進行方向とを一致させるための操舵角度θ00、θ10、θ20、θ30を算出する。
【０１３６】
各回転中心の運動方向と各駆動輪２０の進行方向とを一致させるためには、図２０（ａ）に示すように、各駆動輪２０の進行方向と、旋回中心と回転中心とを結ぶ線分との成す角度が直角（π／２（９０[°]））となるように操舵すれば良く、従って、操舵角度θ00、θ10、θ20、θ30は、「θ00＝θ10＝θ20＝θ30＝−（π／２−φ）」と算出される。
一方、下式（８）に基づき、各駆動輪２０の操舵時のヨー軸周りの回転中心と旋回中心（０，０）との距離Ｌ0、Ｌ1、Ｌ2、Ｌ3が算出される。
【０１３７】
【数３】
【０１３８】
但し、上式（８）は、基体１０の中心座標（０，０）を旋回中心とした場合の式である。
なお、トレッドＷt及びホイールベースＷbから、左前輪の回転中心の座標は（Ｗb／２，Ｗt／２）、右前輪の回転中心の座標は（Ｗb／２，−Ｗt／２）、左後輪の回転中心の座標は（−Ｗb／２，Ｗt／２）、右後輪の回転中心の座標は（−Ｗb／２，−Ｗt／２）と表すことができる。
【０１３９】
なお、旋回中心が基体１０の中心座標（０，０）であるので、距離Ｌ0、Ｌ1、Ｌ2、Ｌ3は、等距離「Ｌ0＝Ｌ1＝Ｌ2＝Ｌ3＝Ｌ」となる。
距離Ｌ0、Ｌ1、Ｌ2、Ｌ3が算出されると、次に、これらの距離Ｌと旋回角速度Ωとから、下式（９）に基づき、各駆動輪２０の線速度Ｖ0、Ｖ1、Ｖ2、Ｖ3を算出する。
Ｖ0＝Ｖ1＝Ｖ2＝Ｖ3＝ＬΩ （９）
但し、上式（９）は、基体１０の中心座標（０，０）を旋回中心とした場合の式である。
【０１４０】
ここで、先述したように、左右前輪は黒半円の目印のある方向に進行する回転方向が正回転方向となり、左右後輪は目印の無い方向に進行する回転方向が正回転方向となる。
また、各駆動輪２０の向きは、図２０（ａ）に示すようになるので、線速度Ｖ0、Ｖ1、Ｖ2、Ｖ3は、「Ｖ0＝Ｖ3＝−ＬΩ」、「Ｖ1＝Ｖ2＝ＬΩ」と算出される。
更に、線速度Ｖ0、Ｖ1、Ｖ2、Ｖ3は、上式（６）に従って、回転角速度ω0、ω1、ω2、ω3に変換される。
【０１４１】
操舵角度θ00、θ10、θ20、θ30、及び各駆動輪２０の回転角速度ω0、ω1、ω2、ω3が算出されると、次に、各駆動輪２０の現在の操舵角度及び回転角速度を取得する。そして、現在の操舵角度と、上記算出した操舵角度とから回転関節１４の関節モータ４０を駆動する指令値を算出する。更に、現在の回転角速度と、上記算出した回転角速度とから、駆動輪２０の車輪モータ５０を駆動する指令値を算出する。
【０１４２】
このようにして、各駆動輪２０の操舵制御の指令値及び速度制御の指令値を算出すると、これらの指令値を各モータのドライバに入力する。そして、入力された指令値に基づき各関節モータ４０及び車輪モータ５０が駆動されると、これにより、脚車輪型ロボット１００の各駆動輪２０の操舵角度及び回転角速度が変化し、基体１０の中心座標（０，０）を旋回中心として、脚車輪型ロボット１００が前後移動せずにその場で旋回（超信地旋回）する。
【０１４３】
次に、基体１０の中心座標（０，０）以外の座標を旋回中心とした場合の超信地旋回制御処理について説明する。以下、この超信地旋回制御処理を、旋回中心オフセット型超信地旋回制御処理と称す。
この場合は、旋回角速度Ωと、基体１０の中心座標（０，０）以外の座標である旋回中心座標（ｘｃ，ｙｃ）≠（０，０）とが入力される。
【０１４４】
そして、旋回角速度Ω及び旋回中心座標（ｘｃ，ｙｃ）が入力されると、下式（１０）に基づき、基体１０のｘ軸と各駆動輪２０の回転中心とのなす角度φ0、φ1、φ2、φ3が算出される。
ｔａｎφ0＝（Ｗt／２−ｙｃ）／（Ｗb／２−ｘｃ）
ｔａｎφ1＝（Ｗt／２＋ｙｃ）／（Ｗb／２−ｘｃ）
ｔａｎφ2＝（Ｗt／２−ｙｃ）／（Ｗb／２＋ｘｃ）
ｔａｎφ3＝（Ｗt／２＋ｙｃ）／（Ｗb／２＋ｘｃ） ・・・（１０）
但し、上式（１０）は、脚車輪型ロボット１００の脚部１２が、基体１０の前方に左右一対及び後方に左右一対の計４本の場合の式である。
【０１４５】
具体的に、入力された旋回中心座標（ｘｃ，ｙｃ）と、既知のＷt及びＷbとを、上式（１０）に代入して、基体１０のｘ軸と各駆動輪２０の回転中心とのなす角度φ0、φ1、φ2、φ3を算出する。
なお、旋回中心が基体１０の中心座標以外の座標となるので、角度φ0、φ1、φ2、φ3はそれぞれ異なる角度となる。
【０１４６】
また、基体１０が、旋回中心座標（ｘｃ，ｙｃ）でヨー軸周りに自転時に、各駆動輪２０の操舵時のヨー軸周りの回転中心の描く円弧軌道と該回転中心との接点における運動方向は、図２０（ｂ）の各駆動輪２０から伸びる矢印線に示すように、各円弧軌道上の各回転中心を通る接線方向（図中の矢印線方向）となる。また、旋回中心と各駆動輪２０の回転中心との距離はそれぞれ異なるため、各回転中心の描く円弧軌道も異なる。
【０１４７】
各駆動輪２０の回転中心に対する角度φ0、φ1、φ2、φ3が算出されると、次に、各回転中心の運動方向と各駆動輪２０の進行方向とを一致させるための操舵角度θ00、θ10、θ20、θ30を算出する。
各回転中心の運動方向と各駆動輪２０の進行方向とを一致させるためには、図２０（ｂ）に示すように、各駆動輪２０の回転方向と、旋回中心と回転中心とを結ぶ線分との成す角度が直角（π／２（９０[°]））となるように操舵すれば良く、従って、操舵角度θ00、θ10、θ20、θ30は、「θi0＝−（π／２−φi）（i=0,1,2,3）」と算出される。
【０１４８】
一方、下式（１１）に基づき、各駆動輪２０の操舵時のヨー軸周りの回転中心と旋回中心（ｘｃ，ｙｃ）との距離Ｌ0、Ｌ1、Ｌ2、Ｌ3が算出される。
【０１４９】
【数４】
【０１５０】
先述したように、旋回中心が基体１０の中心座標（０，０）以外の座標であるので、距離Ｌ0、Ｌ1、Ｌ2、Ｌ3は、それぞれ異なる距離となる。
距離Ｌ0、Ｌ1、Ｌ2、Ｌ3が算出されると、次に、これらの距離と旋回角速度Ωとから、下式（１２）に基づき、各駆動輪２０の線速度Ｖ0、Ｖ1、Ｖ2、Ｖ3を算出する。
｜Ｖi｜＝｜ＬiΩ｜ （１２）
従って、線速度Ｖ0、Ｖ1、Ｖ2、Ｖ3は、「Ｖ0＝−Ｌ0Ω」、「Ｖ1＝Ｌ1Ω」、「Ｖ2＝Ｌ2Ω」、「Ｖ3＝−Ｌ3Ω」と算出される。
【０１５１】
更に、線速度Ｖ0、Ｖ1、Ｖ2、Ｖ3は、上式（６）に従って、回転角速度ω0、ω1、ω2、ω3に変換される。
操舵角度θ00、θ10、θ20、θ30、及び各駆動輪２０の回転角速度ω0、ω1、ω2、ω3が算出されると、次に、各駆動輪２０の現在の操舵角度及び回転角速度を取得する。そして、現在の操舵角度と、上記算出した操舵角度とから回転関節１４の関節モータ４０を駆動する指令値を算出する。更に、現在の回転角速度と、上記算出した回転角速度とから、駆動輪２０の車輪モータ５０を駆動する指令値を算出する。
【０１５２】
このようにして、各駆動輪２０の操舵制御の指令値及び速度制御の指令値を算出すると、これらの指令値を各モータのドライバに入力する。そして、入力された指令値に基づき各関節モータ４０及び車輪モータ５０が駆動されると、これにより、脚車輪型ロボット１００の各駆動輪２０の操舵角度及び回転角速度が変化し、基体１０における中心位置（０，０）以外の座標（ｘｃ，ｙｃ）を旋回中心として、脚車輪型ロボット１００が旋回（超信地旋回）する。
【０１５３】
次に、図２１及び図２２に基づき、本実施の形態の動作を説明する。
ここで、図２１（ａ）及び（ｂ）は、脚車輪型ロボット１００の走行経路の一例を示す図である。また、図２２は、ロボットの重心位置の一例を示す図である。
脚車輪型ロボット１００は、障害物センサ３４、３６によって、脚車輪型ロボット１００の移動経路上に障害物（ここでは階段とする）が存在することを確認すると、３次元距離測定装置２００によって、上記第１の実施の形態で説明した距離測定及び面データの生成処理を行い、生成した面データをハブ７６及び通信Ｉ／Ｆ６４を介してＣＰＵ６０に入力する。一方、ＣＰＵ６０は、入力された面データを解析して、特に、勾配が０に近い交差線を有する面がある場合は、それを水平面とみなし、脚車輪型ロボット１００が歩行可能な面であると判断する。面データの示す面が歩行可能な面であると判断された場合は、入力された面データのセンサ座標系をグローバル座標系に変換し、この変換結果と、３軸姿勢センサ７０のセンサ信号などとに基づき、脚先の着地位置を決定する。更に、脚先センサ３８からのセンサ信号に基づき、蹴込板までの距離および脚先と踏板の位置関係とを算出する。そして、決定された着地位置および算出された両距離に基づいてモータ指令信号が生成され、生成されたモータ指令信号がドライバ４４に出力される。これにより、各関節モータが駆動し、脚車輪型ロボット１００が姿勢を適切に保ちつつ階段を乗り越える。また、状況によっては階段を回避、停止する。したがって、脚型ロボットと同様に階段への適応性が高い。
【０１５４】
一方、脚車輪型ロボット１００は、障害物センサ３４、３６のセンサ信号などに基づき、自己の移動経路上に何も障害物が存在しない（平地である）と判断すると、移動モードを、脚部１２を用いる脚部移動モードから駆動輪２０を用いる車輪走行移動モードへと切り替える。
平地では、脚車輪型ロボット１００は、上記した無変向走行で移動することができる。さらに、上記した超信地旋回及び旋回中心オフセット型超信地旋回を行うことができる。
【０１５５】
車輪走行移動モードへと切り替えられると、脚車輪型ロボット１００は、脚部１２の各関節モータ４０を制御して膝屈曲姿勢へと移行する。そして、脚車輪型ロボット１００が膝屈曲姿勢へと移行すると、各種走行制御が開始される。
まず、無変向走行制御時の脚車輪型ロボット１００の動作について説明する。
ここでは、図２１（ａ）に示すような通路を、脚車輪型ロボット１００で走行移動させることとする。なお、図２１（ａ）及び（ｂ）は、走行経路の一部を真上から見た俯瞰図である。
【０１５６】
図２１（ａ）に示すように、通路は、最初、基体１０の向いている方向（前方向）に直進しないと通れないほど幅が狭くなっているので、まず、通路への進入前において、通路の伸びる方向と基体１０の向きとを合わせると共に、進入位置及び進入角度を調整する。
そして、無変向走行制御指令を入力し、脚車輪型ロボット１００を、無変向走行制御モードへと移行させる。これにより、脚車輪型ロボット１００は、ＣＰＵ６０において、基体１０の向きを保持した状態で、目的の進行方向へと走行移動する制御を行う。
【０１５７】
まず最初は、脚車輪型ロボット１００を、前方向に直進移動させたいので、ロボットの進行方向α＝０[°]、及びロボット進行方向速度Ｖcを入力する。これにより、各駆動輪２０の操舵角度が「θ00＝θ10＝θ20＝θ30＝０[°]」と算出され、各駆動輪２０の線速度が「Ｖ0＝Ｖ1＝Ｖ2＝Ｖ3＝Ｖｃ」と算出される。
更に、線速度Ｖ0、Ｖ1、Ｖ2、Ｖ3は、上式（６）に従って、回転角速度ω0、ω1、ω2、ω3に変換される。
【０１５８】
ＣＰＵ６０は、操舵角度θ00、θ10、θ20、θ30、及び各駆動輪２０の回転角速度ω0、ω1、ω2、ω3を算出すると、次に、角度取込Ｉ／Ｆ６２を介して、現在の各駆動輪２０の操舵角度及び回転角速度を取得する。ここでは、取得した操舵角度と上記算出した操舵角度との差分値を算出し、該差分値に基づき回転関節１４の関節モータ４０を駆動する指令値を算出する。更に、取得した現在の回転角速度と、上記算出した回転角速度との差分値を算出し、該差分値に基づき駆動輪２０の車輪モータ５０を駆動する指令値を算出する。
【０１５９】
ＣＰＵ６０は、上記算出した、各駆動輪２０の操舵制御の指令値及び速度制御の指令値を各モータのドライバに入力する。この指令値により、各関節モータ４０が駆動され、各脚部１２の回転関節１４がヨー軸周りに回動して目標の操舵角度へと変化する。その後、各車輪モータ５０が駆動され、各駆動輪２０が指令値に応じた回転角速度で回転駆動する。これにより、脚車輪型ロボット１００は、基体１０の向きを保持した状態で、基体１０の向いている方向（前方向）に通路へと進入すると共に通路内を直進走行する。
【０１６０】
脚車輪型ロボット１００が前方向にしばらく直進すると、通路は右に略直角に折れ曲がり、更に路幅も広くなるので、基体１０の向きを保持した状態で、その向いている方向に対して右真横方向に直進移動するように制御する。つまり、進行方向として、α＝−９０（−π／２）[°]を入力し、更に、進行方向速度Ｖｃを入力する。
これにより、例えば、操舵角度として、「θ00＝θ10＝θ20＝θ30＝α[°]」が算出され、線速度として、「Ｖ0＝Ｖ2＝−Ｖｃ」、「Ｖ1＝Ｖ3＝Ｖｃ」が算出される。
【０１６１】
更に、線速度Ｖ0、Ｖ1、Ｖ2、Ｖ3は、上式（６）に従って、回転角速度ω0、ω1、ω2、ω3に変換される。
ＣＰＵ６０は、各駆動輪２０の現在の操舵角度及び回転角速度を取得し、これらと上記算出した操舵角度及び回転角速度とから各指令値を算出する。この指令値により、各関節モータ４０及び各車輪モータ５０が駆動され、これにより、脚車輪型ロボット１００は、基体１０の向きを保持した状態で、その向いている方向に対して右真横方向に直進走行する。これにより、通路の曲がり角で脚車輪型ロボット１００を旋回せずに、その曲がった先へと走行させることができる。
【０１６２】
脚車輪型ロボット１００が右真横方向にしばらく直進すると、通路は右斜め下方向に折れ曲がるので、今度は、基体１０の向きを保持した状態で、その向いている方向に対して右斜め後ろ方向に直進移動するように制御する。つまり、進行方向として、α（通路の角度＜０）[°]を入力し、更に、進行方向速度Ｖｃを入力する。
これにより、操舵角度として、「θ00＝θ30＝π＋α[°]」及び「θ10＝θ20＝−（π＋α）[°]」が算出され、線速度として、「Ｖ0＝Ｖ1＝−Ｖｃ」及び「Ｖ2＝Ｖ3＝Ｖｃ」が算出される。
【０１６３】
更に、線速度Ｖ0、Ｖ1、Ｖ2、Ｖ3は、上式（６）に従って、回転角速度ω0、ω1、ω2、ω3に変換される。
そして、各駆動輪２０の現在の操舵角度及び回転角速度を取得し、これらと上記算出した操舵角度及び回転角速度とから各指令値を算出する。この指令値により、各関節モータ４０及び各車輪モータ５０が駆動され、これにより、脚車輪型ロボット１００は、基体１０の向きを保持した状態で、その向いている方向に対して右斜め後ろ方向に直進走行する。
【０１６４】
次に、超信地旋回制御時の脚車輪型ロボット１００の動作について説明する。
ここでは、図１９（ｂ）に示すような通路を、脚車輪型ロボット１００で走行移動させることとする。
図２１（ｂ）に示すように、通路は、最初真っ直ぐに伸びており、その後、略直角に右に折れ曲がり、その先で行き止まりとなっている。
【０１６５】
まず、通路の伸びる方向と基体１０の向きとを合わせ、進入位置を微調整した後に、脚車輪型ロボット１００を前方向に直進走行させる。これにより、脚車輪型ロボット１００は、通路へと進入すると共に通路内を直進走行する。
脚車輪型ロボット１００は、しばらく直進すると、やがて曲がり角へと到達するので、時計回りに旋回して、基体１０の向きを脚車輪型ロボット１００が進行できる向きへと変更する。
【０１６６】
図２１（ｂ）に示す曲がり角であれば、前移動を伴う旋回動作でも十分に右折できるが、ここでは、超信地旋回により右折することとする。そのため、超信地旋回制御指令を入力し、この指令により、脚車輪型ロボット１００を、超信地旋回制御モードへと移行させる。これにより、脚車輪型ロボット１００は、ＣＰＵ６０において、前後移動を行わずに所定の旋回中心位置でロボットを旋回する制御処理を行う。
【０１６７】
まず、旋回角速度Ω（基体１０を時計回りに回転させる角速度）と、旋回中心座標（０，０）とを入力する。更に、略直角に右折させるので、旋回角度−９０[°]を入力する。
旋回角速度Ω、旋回中心座標（０，０）及び旋回角度（−９０[°]）が入力されると、上式（７）に基づき、基体１０のｘ軸と各駆動輪２０の回転中心とのなす角度φ0、φ1、φ2、φ3が算出される。
【０１６８】
各駆動輪２０の回転中心に対する角度「φ0＝φ1＝φ2＝φ3＝φ」が算出されると、次に、各回転中心の運動方向と各駆動輪２０の進行方向とを一致させるための操舵角度θ00、θ10、θ20、θ30が算出される。
具体的に、操舵角度θ00、θ10、θ20、θ30は、「θ00＝θ10＝θ20＝θ30＝−（π／２−φ）」が算出される。
【０１６９】
また、上式（８）に基づき、各駆動輪２０の操舵時のヨー軸周りの回転中心と旋回中心（０，０）との距離Ｌ0、Ｌ1、Ｌ2、Ｌ3が算出される。
旋回中心が基体１０の中心座標（０，０）となっているので、距離Ｌ0、Ｌ1、Ｌ2、Ｌ3は、等距離「Ｌ0＝Ｌ1＝Ｌ2＝Ｌ3＝Ｌ」となる。
次に、距離Ｌと旋回角速度Ωとから、上式（９）に基づき、各駆動輪２０の線速度Ｖ0、Ｖ1、Ｖ2、Ｖ3を算出する。
【０１７０】
ここでは、脚車輪型ロボット１００を、時計回りに旋回（Ω＜０）させるので、線速度Ｖ0、Ｖ1、Ｖ2、Ｖ3は、「Ｖ1＝Ｖ2＝ＬΩ」、「Ｖ0＝Ｖ3＝−ＬΩ」と算出される。
更に、線速度Ｖ0、Ｖ1、Ｖ2、Ｖ3は、上式（６）に従って、回転角速度ω0、ω1、ω2、ω3に変換される。
操舵角度θ00、θ10、θ20、θ30、及び各駆動輪２０の回転角速度ω0、ω1、ω2、ω3が算出されると、次に、角度取込Ｉ／Ｆ６２を介して、現在の各駆動輪２０の操舵角度及び回転角速度を取得する。ここでは、取得した操舵角度と上記算出した操舵角度との差分値を算出し、該差分値に基づき回転関節１４の関節モータ４０を駆動する指令値を算出する。更に、取得した現在の回転角速度と、上記算出した回転角速度との差分値を算出し、該差分値に基づき駆動輪２０の車輪モータ５０を駆動する指令値を算出する。
【０１７１】
ＣＰＵ６０は、上記算出した、各駆動輪２０の操舵制御の指令値及び速度制御の指令値を各モータのドライバに入力する。この指令値により、各関節モータ４０が駆動され、各脚部１２の回転関節１４がヨー軸周りに回動して目標の操舵角度へと変化する。その後、各車輪モータ５０が駆動され、各駆動輪２０が指令値に応じた回転角速度で回転駆動する。これにより、脚車輪型ロボット１００は、基体１０の中心座標（０，０）を旋回中心として、前後移動せずにその場で時計回りに９０[°]旋回（超信地旋回）する。
【０１７２】
そして、脚車輪型ロボット１００を、時計回りに９０[°]旋回して右折させると、その先の通路を、前方向へと直進走行させる。
図２１（ｂ）に示すように、右折した先の通路は袋小路となっているため、脚車輪型ロボット１００は、やがて通路の行き止まりへと到達する。
脚車輪型ロボット１００は、これ以上先に進めないため（各種センサにより状況を把握）、１８０[°]旋回して通路を引き返すことになる。
【０１７３】
ここでは、通路の幅が前後移動を伴う旋回動作（例えば、Ｕターン）を行えるほど広くないため、上記右折のときと同様に、超信地旋回制御モードへと移行し、超信地旋回により１８０[°]旋回して、脚車輪型ロボット１００の向きを変更し、引き返すこととする。
また、図２１（ｂ）に示すように、通路幅が自転ぎりぎりの幅となっているので、脚車輪型ロボット１００を最小の旋回半径で旋回させる必要がある。従って、旋回中心座標（０，０）と、旋回角速度Ω（基体１０を時計回りに回転させる角速度）と、旋回角度１８０[°]とを入力する。なお、脚部１２が通路にぶつからないように、脚車輪型ロボット１００の各脚部１２の姿勢を膝伸展姿勢へと変更する。
【０１７４】
旋回中心座標（０，０）、旋回角速度Ω及び旋回角度１８０[°]が入力されると、上記右折のときと同様に、操舵角度θ00、θ10、θ20、θ30として、「θ00＝θ10＝θ20＝θ30＝−（π／２−φ）」が算出され、線速度Ｖ0、Ｖ1、Ｖ2、Ｖ3として、「Ｖ1＝Ｖ2＝ＬΩ」、「Ｖ0＝Ｖ3＝−ＬΩ」が算出される。
更に、線速度Ｖ0、Ｖ1、Ｖ2、Ｖ3は、上式（６）に従って、回転角速度ω0、ω1、ω2、ω3に変換される。
【０１７５】
操舵角度θ00、θ10、θ20、θ30、及び各駆動輪２０の回転角速度ω0、ω1、ω2、ω3が算出されると、各指令値が算出され、関節モータ４０及び車輪モータ５０が駆動される。
これにより、基体１０の中心座標（０，０）を旋回中心として、脚車輪型ロボット１００が前後移動せずにその場で時計回りに１８０[°]旋回（超信地旋回）し、真後ろ方向へと向きを変える。
【０１７６】
ここで、脚車輪型ロボット１００が、例えば、遠隔操作で人手により走行制御される場合や、自動制御で且つ基体１０の後ろ側にも各種センサやカメラを備えている場合などは、無変向走行制御により、前を向いたまま真後ろに走行させて通路を引き返させることも可能である。しかし、自動制御の場合で且つ各種センサが基体１０の前側にしか備わっていない場合は、基体１０の向きと進行方向とを合わせる必要がある。従って、後者の場合などに、超信地旋回は有用な旋回手段となる。
【０１７７】
次に、旋回中心オフセット型超信地旋回制御時の脚車輪型ロボット１００の動作について説明する。
いま、車輪走行移動モードへと移行し、脚車輪型ロボット１００の各関節モータ４０が制御され、ロボットの姿勢が膝屈曲姿勢に変更されたとする。このとき、ロボットの重心が図２２に示すように、基体１０の中心位置の座標から外れた位置となるとする。
【０１７８】
この場合は、重心位置が基体１０の中心位置の座標（０，０）から外れているため、中心位置を旋回中心として超信地旋回を行わせると旋回がアンバランスとなり、不具合が発生する恐れがある。
このようなときに、本実施の形態の超信地旋回制御指令モードにおいては、任意の旋回中心で超信地旋回させることができるので、旋回角速度Ωを入力すると共に、基体１０における、脚車輪型ロボット１００の重心位置に対応する座標（ｘｇ，ｙｇ）を旋回中心座標（ｘｃ，ｙｃ）として入力する。 そして、旋回角速度Ω及び旋回中心座標（ｘｃ，ｙｃ）＝（ｘｇ，ｙｇ）が入力されると、上式（１０）に基づき、基体１０のｘ軸と各駆動輪２０の回転中心とのなす角度φ0、φ1、φ2、φ3が算出される。
【０１７９】
各駆動輪２０の回転中心に対する角度φ0、φ1、φ2、φ3が算出されると、次に、各回転中心の運動方向と各駆動輪２０の進行方向とを一致させるための操舵角度θ00、θ10、θ20、θ30を算出する。
操舵角度θ00、θ10、θ20、θ30は、「θi0＝−（π／２−φi）（i=0,1,2,3）」と算出される。
【０１８０】
次に、上式（１１）に基づき、各駆動輪２０の操舵時のヨー軸周りの回転中心と旋回中心（ｘｃ，ｙｃ）との距離Ｌ0、Ｌ1、Ｌ2、Ｌ3を算出する。
距離Ｌ0、Ｌ1、Ｌ2、Ｌ3が算出されると、次に、これらの距離と旋回角速度Ωとから、上式（１２）に基づき、各駆動輪２０の線速度Ｖ0、Ｖ1、Ｖ2、Ｖ3を算出する。
ここでは、脚車輪型ロボット１００を反時計回りに旋回（Ω＞０）させるとして、線速度Ｖ0、Ｖ1、Ｖ2、Ｖ3は、「Ｖ0＝−Ｌ0Ω」、「Ｖ1＝Ｌ1Ω」、「Ｖ2＝Ｌ2Ω」、「Ｖ3＝−Ｌ3Ω」と算出される。
【０１８１】
更に、線速度Ｖ0、Ｖ1、Ｖ2、Ｖ3は、上式（６）に従って、回転角速度ω0、ω1、ω2、ω3に変換される。
操舵角度θ00、θ10、θ20、θ30、及び各駆動輪２０の回転角速度ω0、ω1、ω2、ω3が算出されると、次に、角度取込Ｉ／Ｆ６２を介して、現在の各駆動輪２０の操舵角度及び回転角速度を取得する。ここでは、取得した操舵角度と上記算出した操舵角度との差分値を算出し、該差分値に基づき回転関節１４の関節モータ４０を駆動する指令値を算出する。更に、取得した現在の回転角速度と、上記算出した回転角速度との差分値を算出し、該差分値に基づき駆動輪２０の車輪モータ５０を駆動する指令値を算出する。
【０１８２】
ＣＰＵ６０は、上記算出した、各駆動輪２０の操舵制御の指令値及び速度制御の指令値を各モータのドライバに入力する。この指令値により、各関節モータ４０及び各車輪モータ５０が駆動され、これにより、基体１０における重心座標と対応する座標（ｘｇ，ｙｇ）を旋回中心として、脚車輪型ロボット１００がバランスのとれた状態で反時計回りに超信地旋回する。
【０１８３】
このようにして、本実施の形態では、３次元距離測定装置２００の機械的な構成を、モータ１１６及びエンコーダ１１８と、２次元距離測定装置１１２とを水平方向に並べて配設し、モータ１１６の第１の回転軸の回転駆動力を、プーリ１２０ａ、ベルト１２１、プーリ１２０ｂを介して、２次元距離測定装置１１２を回転する第２回転軸へと伝達し、２次元距離測定装置１１２を、垂直方向の軸（ｙ軸）まわりに回転駆動する構成とした。
【０１８４】
これにより、２次元距離測定装置１１２の所定走査角度範囲内には、走査を除外するものが一切なくなるので、正確な距離情報を得ることができる。また、モータ１１６及びエンコーダ１１８と、２次元距離測定装置１１２とを水平方向に並べて配設したので、３次元距離測定装置２００の高さ方向の占有率を低減することができる。
さらに、本実施の形態では、３次元距離測定装置２００において、測定平面内に存在する物体の凹凸を認識することができる。これにより、連続面が存在することを推定することができる。更に、複数の測定平面に対して第１走査処理を実行することができるので、測定範囲内に存在する物体の面形状を認識することができる。
【０１８５】
これにより、障害物（通路の壁等を含む）までの距離及び障害物の形状を正確に把握することができるので、脚車輪型ロボット１００の障害物の回避、通路内の走行等の移動動作をより安全且つ確実に行うことができる。
さらに、本実施の形態では、基体１０の向きを一定の方向に保持した状態で、脚車輪型ロボット１００が目的の進行方向に走行するように関節モータ４０及び車輪モータ５０を制御する。
【０１８６】
これにより、旋回を行わず（向きを変えず）に自由な方向へ移動できるので、各方向への素早い移動を実現できると共に、脚車輪型ロボット１００の各構成部が妨げとなって旋回できないような狭くて入り組んだエリアなど、脚車輪型ロボットの向きを変更することが困難なエリアにおいても活動が可能となる。
さらに、本実施の形態では、脚車輪型ロボット１００を所定の旋回中心位置で前後移動させずに旋回（超信地旋回）するように関節モータ４０及び車輪モータ５０を制御する。このとき、基体１０における中心位置の座標を旋回中心座標とする制御（超信地旋回制御）と、基体１０における中心位置以外の座標を旋回中心座標とする制御（旋回中心オフセット型超信地旋回制御）とを行うことが可能である。
【０１８７】
これにより、基体１０の中心位置を旋回中心位置とする場合は、最小の旋回半径で脚車輪型ロボットを旋回をさせることが可能である。また、基体１０の中心位置以外を旋回中心位置とする場合は、基体１０の中心位置と重心位置とが異なる場合などに、重心位置を旋回中心として超信地旋回させることができるので、脚車輪型ロボット１００を、バランスよく超信地旋回させることが可能である。
【０１８８】
上記第２の実施の形態において、３次元距離測定装置２００は、発明１乃至６の３次元距離測定装置に対応し、脚車輪型ロボット１００は、発明８ないし１０の脚車輪型ロボットに対応し、駆動輪２０は、発明７乃至９の車輪に対応し、関節モータ４０は、発明８ないし１０の第１アクチュエータ及び第２アクチュエータに対応し、ＣＰＵ６０は、発明８ないし１０の制御手段に対応している。
【０１８９】
なお、上記第１及び第２の実施の形態においては、モータ１１６の第１の回転軸にエンコーダ１１８を設ける構成としたが、これに限らず、２次元距離測定装置１１２の第２の回転軸にエンコーダを設ける構成としても良い。
これにより、伝達誤差の影響を受けずに第２走査処理における走査角を高精度に検出することができる。
【０１９０】
また、上記第１及び第２の実施の形態においては、プーリ２０及びベルト１２１を介して、モータ１１６の第１の回転軸の回転駆動力を、２次元距離測定装置１１２を回転駆動する第２の回転軸へと伝達する構成としたが、これに限らず、複数の歯車を介して、第１の回転軸の回転駆動力を、第２の回転軸へと伝達する構成としても良い。
また、上記第１及び第２の実施の形態においては、第１走査処理における距離情報の取得を離散的（第２走査処理で回転駆動してから第１走査処理で距離情報を取得）に行う構成としたが、これに限らず、走査角と測定距離との対応付けを行うことで連続的（第２走査処理で回転駆動しつつ、第１走査処理で距離情報を取得）に行う構成としても良い。
【０１９１】
また、上記第１及び第２の実施の形態においては、３次元距離測定装置２００で生成した面データに基づき、ＣＰＵ６０において、物体の面形状を認識する構成としたが、これに限らず、３次元距離測定装置２００において、面形状の認識まで行い、ＣＰＵ６０には、面形状の認識結果を示すデータを出力する構成としても良い。
また、上記第１及び第２の実施の形態においては、ＣＰＵ６０からの指令信号に基づき、走査角度範囲及び走査角度単位を設定する構成としたが、これに限らず、３次元距離測定装置２００に予め設定しておくなど別の構成としても良い。
【０１９２】
また、上記第１及び第２の実施の形態においては、本発明に係る３次元距離測定装置２００を、脚車輪型ロボットに適用する例を説明したが、これに限らず、本発明に係る３次元距離測定装置２００を、他の構成のロボット、自動車の外界を認識する装置、視覚障害者が外界を認識するためのアシストを行う装置などに適用することも可能である。
また、上記第１及び第２の実施の形態においては、３次元距離測定装置２００を、測距センサ１１２ａを水平方向の軸（ｙ軸）まわりに回転駆動し、２次元距離測定装置１１２を垂直方向の軸（ｚ軸）まわりに回転駆動する構成としたが、この構成に限らず、測距センサ１１２ａを垂直方向の軸まわりに回転駆動し、２次元距離測定装置１１２を水平方向の軸まわりに回転駆動する構成としても良い。また、水平方向及び垂直方向の軸まわりに限らず、測距センサ１１２ａの測定方向に対して直交する２つの軸であれば、どの方向の軸まわりでも良い。
【０１９３】
また、上記第１及び第２の実施の形態においては、図１５（ａ）に示すように、２次元距離測定装置１１２自身を回転させているが、これに限らず、光学式の測距センサを有する２次元距離測定装置であれば、図１５（ｂ）に示すように、測定方向の光軸上に挿入したミラーを回転させてもよい。
また、上記第１及び第２の実施の形態においては、本発明に係る脚車輪型ロボット１００の構成を、基体１０の前方に左右一対及び後方に左右一対の４本の脚部１２を有する構成としたが、これに限らず、基体１０の中央に左右一対の脚部１２を設ける構成や、３本の脚部１２を対称に設ける構成、５本以上の脚部１２を設ける構成など、本発明の趣旨を逸脱しない範囲で他の構成としても良い。また、多脚とした場合には、不要な脚部１２を走行制御に用いないように制御するようにしても良い。
【図面の簡単な説明】
【０１９４】
【図１】本発明に係る脚車輪型ロボット１００の側面図である。
【図２】（ａ）は、３次元距離測定装置２００の機械的な構成を示す図であり、（ｂ）は、（ａ）を真上から見た図である。
【図３】測距センサ１１２ａの走査範囲の一例を示す図である。
【図４】（ａ）は、（ａ）は、３次元距離測定装置２００の構成を示すブロック図であり、（ｂ）は、本実施の形態に係る２次元距離測定装置１１２の詳細な構成を示すブロック図である。
【図５】測距センサ１１２ａの距離測定の原理を説明する図である。
【図６】（ａ）は、水平方向の軸（ｙ軸）まわりに測距センサ１１２ａを回転駆動させたときの測定距離と回転角度θとの関係を示す図であり、（ｂ）は、垂直方向の軸（ｚ軸）まわりに２次元距離測定装置１１２を回転駆動したときの測定平面と回転角度φとの関係を示す図である。
【図７】３次元距離測定装置２００の距離の計測例を示す図である。
【図８】３次元距離測定装置２００における物体認識処理を示すフローチャートである。
【図９】（ａ）及び（ｂ）は、勾配の算出例を示す図である。
【図１０】脚車輪型ロボット１００の移動制御システムを示すブロック図である。
【図１１】測定範囲内に階段がある場合の、第１走査処理による測定結果を示す図である。
【図１２】図１１の各測定点の回転座標系の測定距離を直交座標系の測定距離に変換した場合の測定結果を示す図である。
【図１３】（ａ）及び（ｂ）は、最小二乗法を用いて算出したｘ方向及びｚ方向の勾配の一例を示す図である。
【図１４】ある測定平面に対する勾配の出現頻度の一例を示す図である。
【図１５】測距センサの測定方向を変更する場合の構成を示す図である。
【図１６】脚車輪型ロボット１００の正面図である。
【図１７】脚車輪型ロボット１００の側面図である。
【図１８】（ａ）及び（ｂ）は、脚車輪型ロボット１００の車輪走行移動時の姿勢を示す図である。
【図１９】無変向走行制御時の脚車輪型ロボット１００の走行状態例を示す図である。
【図２０】ａ）及び（ｂ）は、基体１０の座標（０，０）及び座標（ｘｃ，ｙｃ）を回転中心とした場合の超信地旋回制御時の脚車輪型ロボット１００の走行状態を示す図である。
【図２１】（ａ）及び（ｂ）は、脚車輪型ロボット１００の走行経路の一例を示す図である。
【図２２】ロボットの重心位置の一例を示す図である。
【符号の説明】
【０１９５】
１００ 脚車輪型ロボット
２００ ３次元距離測定装置
１１０ センシングプロセッサ
１１２ ２次元距離測定装置
１１２ａ 測距センサ
１０ 基体
１２ 脚部
１４〜１８ 回転関節
２０ 駆動輪
２６ 水平レーザ
２８、３０ 垂直レーザ
３２ カメラ
３４、３６ 障害物センサ
３８ 脚先センサ
１１２ｃ、１１６、４０、５０ モータ
１１２ｄ、１１８、４２、５２ エンコーダ
１１２ｂ、１１４、４４、５４ ドライバ
１２０ａ、１２０ｂ プーリ
２１ ベルト
６０ ＣＰＵ
６４ 通信Ｉ／Ｆ
７６ ハブ
【特許請求の範囲】
【請求項１】
測定距離範囲内に存在する物体上の測定点までの距離を測定する測距センサと、前記測距センサを、その測定方向に対して直交する２つの軸のうち一方の軸まわりに回転駆動させる第１の回転機構と、前記測距センサを、前記第１の回転機構と共に、前記測定方向に対して直交する２つの軸のうち前記第１の回転機構とは異なる方向の軸まわりに回転駆動させる第２の回転機構とを有し、前記第１の回転機構及び前記第２の回転機構を所定の回転角度ずつ回転駆動しながら前記測距センサによって距離を測定することで、前記物体の３次元の距離を測定する３次元距離測定装置であって、
前記第１の回転機構及び前記第２の回転機構の少なくとも一方の回転駆動を所定の回転角度範囲内で行うと共に、当該回転角度範囲内の回転駆動によって前記測距センサが走査する走査範囲外に、前記第１の回転機構及び前記第２の回転機構含む自己の駆動機構を設けたことを特徴とする３次元距離測定装置。
【請求項２】
前記第２の回転機構は、回転駆動力を発生する回転駆動装置を含み、
前記回転駆動装置の第１の回転軸と、前記測距センサ及び前記第１の回転機構を共に回転させる第２の回転軸とを水平方向にずらして設けると共に、前記回転駆動装置の第１の回転軸の回転駆動力を、所定の動力伝達機構を介して前記第２の回転軸に伝達する構成としたことを特徴とする請求項１記載の３次元距離測定装置。
【請求項３】
前記動力伝達機構は、複数の滑車及び各滑車に動力を伝達するベルトを介して、前記回転駆動装置の第１の回転軸の回転駆動力を、前記第２の回転軸に伝達する構成となっていることを特徴とする請求項２記載の３次元距離測定装置。
【請求項４】
前記動力伝達機構は、複数の歯車を介して、前記回転駆動装置の第１の回転軸の回転駆動力を前記第２の回転軸に伝達する構成となっていることを特徴とする請求項２記載の３次元距離測定装置。
【請求項５】
前記第１の回転機構を制御して、前記測距センサを、前記第１の回転機構の回転方向に、所定の回転角度ずつ回動させると共に、前記測距センサによって各回転角度に応じた距離情報を測定する処理である第１走査処理を行う第１走査手段と、
前記第１走査手段によって測定された、各測定点の距離情報とその周辺の測定点の距離情報とに基づき、各測定点に対する勾配を算出する勾配算出手段と、
前記勾配算出手段で算出した各測定点の勾配に基づき、各勾配の出現頻度を算出する出現頻度算出手段と、
前記出現頻度算出手段で算出した各勾配の出現頻度と、各勾配に対応する測定点の座標情報とに基づき、前記測距センサの測定平面内に存在する物体の形状を認識する形状認識手段と、を備えることを特徴とする請求項１乃至請求項４のいずれか１項に記載の３次元距離測定装置。
【請求項６】
１つの前記測定平面に対する前記第１走査処理が終了する毎に、前記第２の回転機構を制御して、前記測距センサを前記第１の回転機構と共に、前記第２の回転機構の回転方向に所定の回転角度ずつ回動させる処理である第２走査処理を行う第２走査手段を備え、
前記第１走査手段は、前記第２走査処理において、前記測距センサが前記所定の回転角度の回動をする毎に前記第１走査処理を実行し、
前記勾配算出手段は、前記測定平面毎に、前記第１走査処理によって測定された、各測定点の距離情報と、その周辺の測定点の距離情報とに基づき、各測定点に対する勾配を算出し、
前記出現頻度算出手段は、前記測定平面毎に、前記勾配算出手段で算出した各測定点の勾配に基づき、各勾配の出現頻度を算出し、
前記形状認識手段は、前記測定平面毎に、前記出現頻度算出手段で算出した各勾配の出現頻度と、各勾配に対応する測定点の座標情報とに基づき、前記測定平面内に存在する物体の形状を認識することを特徴とする請求項５記載の３次元距離測定装置。
【請求項７】
基体と、前記基体に対して自由度を有して連結された脚部と、前記脚部に回転可能に設けられた車輪と、前記脚部及び前記車輪を駆動するための動力を付与するアクチュエータと、前記アクチュエータを制御する制御手段とを備え、前記脚部の駆動および前記車輪の回転により移動する脚車輪型ロボットであって、
請求項１乃至請求項６のいずれか１項に記載の３次元距離測定装置を備え、
前記制御手段は、前記３次元距離測定装置の測定距離に基づき前記アクチュエータを制御することを特徴とする脚車輪型ロボット。
【請求項１】
測定距離範囲内に存在する物体上の測定点までの距離を測定する測距センサと、前記測距センサを、その測定方向に対して直交する２つの軸のうち一方の軸まわりに回転駆動させる第１の回転機構と、前記測距センサを、前記第１の回転機構と共に、前記測定方向に対して直交する２つの軸のうち前記第１の回転機構とは異なる方向の軸まわりに回転駆動させる第２の回転機構とを有し、前記第１の回転機構及び前記第２の回転機構を所定の回転角度ずつ回転駆動しながら前記測距センサによって距離を測定することで、前記物体の３次元の距離を測定する３次元距離測定装置であって、
前記第１の回転機構及び前記第２の回転機構の少なくとも一方の回転駆動を所定の回転角度範囲内で行うと共に、当該回転角度範囲内の回転駆動によって前記測距センサが走査する走査範囲外に、前記第１の回転機構及び前記第２の回転機構含む自己の駆動機構を設けたことを特徴とする３次元距離測定装置。
【請求項２】
前記第２の回転機構は、回転駆動力を発生する回転駆動装置を含み、
前記回転駆動装置の第１の回転軸と、前記測距センサ及び前記第１の回転機構を共に回転させる第２の回転軸とを水平方向にずらして設けると共に、前記回転駆動装置の第１の回転軸の回転駆動力を、所定の動力伝達機構を介して前記第２の回転軸に伝達する構成としたことを特徴とする請求項１記載の３次元距離測定装置。
【請求項３】
前記動力伝達機構は、複数の滑車及び各滑車に動力を伝達するベルトを介して、前記回転駆動装置の第１の回転軸の回転駆動力を、前記第２の回転軸に伝達する構成となっていることを特徴とする請求項２記載の３次元距離測定装置。
【請求項４】
前記動力伝達機構は、複数の歯車を介して、前記回転駆動装置の第１の回転軸の回転駆動力を前記第２の回転軸に伝達する構成となっていることを特徴とする請求項２記載の３次元距離測定装置。
【請求項５】
前記第１の回転機構を制御して、前記測距センサを、前記第１の回転機構の回転方向に、所定の回転角度ずつ回動させると共に、前記測距センサによって各回転角度に応じた距離情報を測定する処理である第１走査処理を行う第１走査手段と、
前記第１走査手段によって測定された、各測定点の距離情報とその周辺の測定点の距離情報とに基づき、各測定点に対する勾配を算出する勾配算出手段と、
前記勾配算出手段で算出した各測定点の勾配に基づき、各勾配の出現頻度を算出する出現頻度算出手段と、
前記出現頻度算出手段で算出した各勾配の出現頻度と、各勾配に対応する測定点の座標情報とに基づき、前記測距センサの測定平面内に存在する物体の形状を認識する形状認識手段と、を備えることを特徴とする請求項１乃至請求項４のいずれか１項に記載の３次元距離測定装置。
【請求項６】
１つの前記測定平面に対する前記第１走査処理が終了する毎に、前記第２の回転機構を制御して、前記測距センサを前記第１の回転機構と共に、前記第２の回転機構の回転方向に所定の回転角度ずつ回動させる処理である第２走査処理を行う第２走査手段を備え、
前記第１走査手段は、前記第２走査処理において、前記測距センサが前記所定の回転角度の回動をする毎に前記第１走査処理を実行し、
前記勾配算出手段は、前記測定平面毎に、前記第１走査処理によって測定された、各測定点の距離情報と、その周辺の測定点の距離情報とに基づき、各測定点に対する勾配を算出し、
前記出現頻度算出手段は、前記測定平面毎に、前記勾配算出手段で算出した各測定点の勾配に基づき、各勾配の出現頻度を算出し、
前記形状認識手段は、前記測定平面毎に、前記出現頻度算出手段で算出した各勾配の出現頻度と、各勾配に対応する測定点の座標情報とに基づき、前記測定平面内に存在する物体の形状を認識することを特徴とする請求項５記載の３次元距離測定装置。
【請求項７】
基体と、前記基体に対して自由度を有して連結された脚部と、前記脚部に回転可能に設けられた車輪と、前記脚部及び前記車輪を駆動するための動力を付与するアクチュエータと、前記アクチュエータを制御する制御手段とを備え、前記脚部の駆動および前記車輪の回転により移動する脚車輪型ロボットであって、
請求項１乃至請求項６のいずれか１項に記載の３次元距離測定装置を備え、
前記制御手段は、前記３次元距離測定装置の測定距離に基づき前記アクチュエータを制御することを特徴とする脚車輪型ロボット。
【図１】
【図２】
【図３】
【図４】
【図５】
【図６】
【図７】
【図８】
【図９】
【図１０】
【図１１】
【図１２】
【図１３】
【図１４】
【図１５】
【図１６】
【図１７】
【図１８】
【図１９】
【図２０】
【図２１】
【図２２】
【図２】
【図３】
【図４】
【図５】
【図６】
【図７】
【図８】
【図９】
【図１０】
【図１１】
【図１２】
【図１３】
【図１４】
【図１５】
【図１６】
【図１７】
【図１８】
【図１９】
【図２０】
【図２１】
【図２２】
【公開番号】特開２００９−８６４８（Ｐ２００９−８６４８Ａ）
【公開日】平成２１年１月１５日（２００９．１．１５）
【国際特許分類】
【出願番号】特願２００７−２７０１３８（Ｐ２００７−２７０１３８）
【出願日】平成１９年１０月１７日（２００７．１０．１７）
【出願人】（０００００４２０４）日本精工株式会社 (8,378)
【Ｆターム（参考）】
【公開日】平成２１年１月１５日（２００９．１．１５）
【国際特許分類】
【出願日】平成１９年１０月１７日（２００７．１０．１７）
【出願人】（０００００４２０４）日本精工株式会社 (8,378)
【Ｆターム（参考）】
[ Back to top ]