(57)【要約】【課題】構造物の昇降動作を迅速且つ安定的に行わせる移動ロボットを提供する。【解決手段】移動ロボット１０は、モータ２６の駆動により所定範囲内で動作可能な複数の肢体１１と、モータ２６の駆動制御を行う制御装置１３を備えている。この制御装置１３は、肢体１１の先端側に配置されたエンドエフェクタ２１が係合する構造物Ｈの係合部位を上下方向に移動させながら移動ロボット１０を昇降させる際の各肢体１１の目標動作を設定する動作設定手段３２を有している。動作設定手段３２は、移動ロボット１０が構造物Ｈから滑落しないように、構造物Ｈの係合時におけるエンドエフェクタ２１の反力を設定する反力設定部３９を有し、移動ロボット１０を多質点モデル化し、エンドエフェクタ２１が同時に係合する２箇所の係合部位を結んだ稜線Ｌ周りのモーメントの総和Ｍがゼロになるように反力が算出される。【選択図】          図２
2018034238.tif 000002
【特許請求の範囲】
【請求項１】
昇降対象となる構造物の一部に係合可能なエンドエフェクタが先端側に配置され、駆動装置の駆動によって動作可能な複数の肢体と、前記駆動装置の駆動を制御する制御装置とを備え、一部の前記肢体の前記エンドエフェクタを前記構造物に係合しながら、残りの前記肢体の前記エンドエフェクタを上下方向に移動することで、前記エンドエフェクタが前記構造物に係合する係合部位を上下方向に移動しながら昇降動作を行う移動ロボットにおいて、
前記制御装置は、前記昇降動作の際の前記各肢体の目標動作を設定する動作設定手段と、当該動作設定手段で設定された目標動作に応じて、前記駆動装置の駆動を制御する駆動制御手段とを備え、
前記動作設定手段は、前記移動ロボットが前記構造物から滑落しないように、前記構造物の係合時における前記エンドエフェクタの反力を設定する反力設定部を含み、
前記反力設定部では、前記移動ロボットを多質点モデル化し、前記エンドエフェクタが同時に係合する２箇所の前記係合部位を結んだ稜線周りのモーメントの総和がゼロになるように、前記反力を算出することを特徴とする移動ロボット。
【請求項２】
前記稜線周りのモーメントの総和は、前記反力によるモーメント、前記多質点モデルにおける各質点の重力によるモーメント、及び前記各質点の慣性力によるモーメントの総和であることを特徴とする請求項１記載の移動ロボット。
【請求項３】
前記制御装置は、１回の前記昇降動作毎の始点及び終点における前記エンドエフェクタの位置及び姿勢、及び前記始点から前記終点までに要する時間が入力される入力手段を更に備え、
前記動作設定手段は、前記始点から前記終点までの間を通過する中間点での前記エンドエフェクタの位置及び姿勢を決定する中間点決定部と、前記始点、前記終点及び前記中間点における前記エンドエフェクタの位置及び姿勢に基づき、前記エンドエフェクタの動作目標軌道を決定する動作目標軌道決定部と、決定した前記動作目標軌道で動作するために必要な前記肢体の目標動作値を求める目標動作値演算部と、当該目標動作値が適正か否かを判定し、適正でない場合に前記動作目標軌道を補正するための処理を行う補正処理部とを更に備え、
前記駆動制御手段では、前記補正処理部を経て最終決定された前記動作目標軌道で前記エンドエフェクタを動作させながら、前記反力設定部で設定された反力が得られるように、前記駆動装置の駆動が制御されることを特徴とする請求項１又は２記載の移動ロボット。
【請求項４】
前記中間点決定部では、前記始点から前記終点までの動作の遷移点が前記中間点の位置として定められるとともに、当該中間点での前記エンドエフェクタの位置及び姿勢が予め指定された条件で決定されることを特徴とする請求項３記載の移動ロボット。
【請求項５】
前記動作目標軌道決定部では、前記始点、前記終点、及び前記中間点における前記エンドエフェクタの位置ベクトルと姿勢ベクトルに基づき、前記始点、前記中間点及び前記終点を線形補完することで前記動作目標軌道が決定されることを特徴とする請求項３記載の移動ロボット。
【請求項６】
前記肢体は、複数の肢体構成部及び前記エンドエフェクタが関節部を介して相対回転可能に接続されてなる多関節型に設けられ、
前記目標動作値演算部では、前記目標動作値として、前記動作目標軌道から逆運動学を解くことにより、前記関節部の回転角度、回転角速度及び回転角加速度が算出されることを特徴とする請求項３記載の移動ロボット。
【請求項７】
前記補正処理部では、前記目標動作値が許容上限値を超える場合に、前記多質点モデルの各質点の位置を変更するように前提条件を変えた上で、再度、前記中間点の位置と前記動作目標軌道を求めるように、前記中間点決定部及び前記動作目標軌道決定部に指令することを特徴とする請求項３記載の移動ロボット。
【請求項８】
駆動装置の駆動により所定範囲内で動作可能な複数の肢体を備え、当該肢体の動作によって自律移動可能な移動ロボットの制御装置において、
前記肢体の先端側に配置されたエンドエフェクタが係合する構造物の係合部位を上下方向に移動させながら前記移動ロボットを昇降させる際の前記各肢体の目標動作を設定する動作設定手段と、当該動作設定手段で設定された目標動作に応じて、前記駆動装置の駆動を制御する駆動制御手段とを備え、
前記動作設定手段は、前記移動ロボットが前記構造物から滑落しないように、前記構造物の係合時における前記エンドエフェクタの反力を設定する反力設定部を含み、
前記反力設定部では、前記移動ロボットを多質点モデル化し、前記エンドエフェクタが同時に係合する２箇所の前記係合部位を結んだ稜線周りのモーメントの総和がゼロになるように、前記反力を算出することを特徴とする移動ロボットの制御装置。
【請求項９】
駆動装置の駆動により所定の範囲内で移動可能な複数の肢体を備えた移動ロボットが構造物を昇降する昇降動作の際に、当該移動ロボットの安定判別を行う方法であって、
前記移動ロボットを多質点モデル化し、前記昇降動作時に、前記肢体の先端側に配置されたエンドエフェクタが前記構造物に係合している２箇所の係合部位を結んだ稜線周りのモーメントの総和がゼロになるように、前記係合部位における前記エンドエフェクタが受ける反力の目標値を算出し、当該目標値と実際の前記反力とを対比することで、前記昇降動作の安定性を判別することを特徴とする移動ロボットの安定判別法。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、移動ロボット、並びにその制御装置及び安定判別法に係り、更に詳しくは、複数の肢体を動作させることで、はしご、脚立、壁面等の構造物の昇降を行う移動ロボット、並びにその制御装置及び安定判別法に関する。
【背景技術】
【０００２】
近時において、災害現場のような作業者が立ち入ることが困難な極限環境下で所定の作業を行わせるロボットの需要が増大している。このロボットとしては、様々な形状の地面を移動可能なクローラ等の車輪型ロボットや、複数の肢体の動作により移動可能な脚型ロボット等がある。そこで、例えば、災害現場ではしごを昇降させる必要がある場合、車輪型ロボットでは実現困難であり、脚型ロボットによる実現が考えられる。
【０００３】
ところで、特許文献１には、はしご等の構造物を昇降させるための脚型ロボットとして、はしごに肢体を掛けて３点で支持しながら１肢ずつ肢体を動作させる移動ロボットが提案されている。しかしながら、この移動ロボットにあっては、１肢ずつ肢体を動作させることから、はしごを昇降時の動作数が多く、はしごの昇降に多くの時間を要する。つまり、この３点支持によるロボットのはしご昇降動作の１サイクルにおいては、４つの肢体をそれぞれ別のタイミングで１肢ずつはしごの次の桟に移動させながら、各肢体に繋がる胴体を移動させる５つのフェーズが必要になり、これが昇降動作に時間がかかる要因となる。例えば、災害現場では、１００ｍ前後の高いはしごを昇る必要があり、更に、高いはしごを昇り切った後に、バルブやドアの開閉、ドリル操作等の様々な作業を行うことを最終目標としているため、はしごの昇り切り後の作業時間を確保するためにも可能な限り速くはしごを昇ることが望まれる。
【０００４】
そこで、非特許文献１には、ロボットを単質点でモデル化し、はしご登りを２点支持で行う手法が開示されている。これによれば、１つのフェーズで２つの肢体を同時に昇降させることができ、前述の３点支持方式に比べ、はしご昇降動作の１サイクル当たりの構成フェーズ数を少なくし、はしご昇降動作を高速に行うことができる。
【先行技術文献】
【特許文献】
【０００５】
【特許文献１】特開２０１６−７８１８３号公報
【非特許文献】
【０００６】
【非特許文献１】米田裕成他３名、「重力モーメント考慮の姿勢制御によるマルチロコモーションロボットを用いての垂直梯子登り運動」日本機械学会論文集７５巻７５１号、ｐｐ．１２−１９、２００９年３月
【発明の概要】
【発明が解決しようとする課題】
【０００７】
災害現場等で作業を行う脚式ロボットとしては、多様で力強い作業するために、各肢体の大型化や高重量化が想定されており、はしご等の昇降動作中における各肢体の慣性項の影響を無視することができない。ところが、前記非特許文献１の手法にあっては、ロボットの重心での単質点での近似であるため、各パーツが大型化、高重量化された脚型ロボットでは、昇降動作時に２箇所の支持点を結んだ稜線周りに回転してはしごから滑落する可能性が高くなる。
【０００８】
本発明は、このような課題に着目して案出されたものであり、その目的は、はしご、脚立、壁面等の構造物の昇降動作を迅速且つ安定的に行うことができる移動ロボット、並びにその制御装置及び安定判別法を提供することにある。
【課題を解決するための手段】
【０００９】
前記目的を達成するため、本発明は、主として、昇降対象となる構造物の一部に係合可能なエンドエフェクタが先端側に配置され、駆動装置の駆動によって動作可能な複数の肢体と、前記駆動装置の駆動を制御する制御装置とを備え、一部の前記肢体の前記エンドエフェクタを前記構造物に係合しながら、残りの前記肢体の前記エンドエフェクタを上下方向に移動することで、前記エンドエフェクタが前記構造物に係合する係合部位を上下方向に移動しながら昇降動作を行う移動ロボットにおいて、
前記制御装置は、前記昇降動作の際の前記各肢体の目標動作を設定する動作設定手段と、当該動作設定手段で設定された目標動作に応じて、前記駆動装置の駆動を制御する駆動制御手段とを備え、
前記動作設定手段は、前記移動ロボットが前記構造物から滑落しないように、前記構造物の係合時における前記エンドエフェクタの反力を設定する反力設定部を含み、
前記反力設定部では、前記移動ロボットを多質点モデル化し、前記エンドエフェクタが同時に係合する２箇所の前記係合部位を結んだ稜線周りのモーメントの総和がゼロになるように、前記反力を算出する、という構成を採っている。
【００１０】
また、本発明は、駆動装置の駆動により所定の範囲内で移動可能な複数の肢体を備えた移動ロボットが構造物を昇降する昇降動作の際に、当該移動ロボットの安定判別を行う方法であって、
前記移動ロボットを多質点モデル化し、前記昇降動作時に、前記肢体の先端側に配置されたエンドエフェクタが前記構造物に係合している２箇所の係合部位を結んだ稜線周りのモーメントの総和がゼロになるように、前記係合部位における前記エンドエフェクタが受ける反力の目標値を算出し、当該目標値と実際の前記反力とを対比することで、前記昇降動作の安定性を判別する、という手法を採っている。
【発明の効果】
【００１１】
本発明によれば、複数の肢体を有する移動ロボットを多質点モデル化し、エンドエフェクタが同時に係合する２箇所の係合部位を結んだ稜線周りのモーメントの総和がゼロになるように、エンドエフェクタの反力が算出される。このため、当該反力が得られるように移動ロボットを昇降動作させることで、移動ロボットの各パーツの高重量化や重量バランスの不均一化に関わらず、前記反力により、移動ロボットの昇降動作中に構造物から滑落することを防止できる。従って、移動ロボットに２点支持方式での構造物の昇降動作を採用したときに、３点支持方式よりも速いスピードで構造物を昇降させることに加え、当該昇降動作を安定的に行うことが可能になる。
【００１２】
また、本発明に係る安定判別法によれば、移動ロボットが構造物を昇降している際に、当該構造物が変形等するような場合において、その時点で各エンドエフェクタに必要となる反力と実際の反力とが対比され、現在の移動ロボットの状態が安定しているか否かが判別される。これにより、移動ロボットが構造物から滑落しないように、前記反力を逐次調整することができ、構造物が経時的に変形するようなタフな環境下においても、安定した移動ロボットの昇降動作に寄与することができる。
【図面の簡単な説明】
【００１３】
【図１】本実施形態に係る移動ロボットを概念的に表した構成図。
【図２】制御装置の構成を表すブロック図。
【図３】（Ａ）〜（Ｃ）は、トロット方式の２点支持のはしご昇降動作を順に説明するための概念図。
【図４】（Ａ）〜（Ｃ）は、ペース方式の２点支持のはしご昇降動作を順に説明するための概念図。
【図５】（Ａ）〜（Ｆ）は、３点支持のはしご昇降動作を順に説明するための概念図。
【図６】１回の昇降動作における肢体動作の遷移を説明するための概念図。
【図７】多質点モデル化した状態の移動ロボットを表す概念図。
【図８】昇降動作における各種処理や動作を順に説明するためのフローチャート。
【発明を実施するための形態】
【００１４】
以下、本発明の実施形態について図面を参照しながら説明する。
図１には、本実施形態における移動ロボットを概念的に表した構成図が示されている。この図において、前記移動ロボット１０は、所定の範囲内で動作可能な肢体１１と、当該肢体１１を支持する胴体１２と、肢体１１の動作制御を行う制御装置１３とを備えている。この移動ロボット１０は、肢体１１の動作によって自律移動可能となっており、特に、はしごや脚立等の構造物を昇降可能に設けられている点が特徴となっている。なお、以下においては、移動ロボット１０の昇降動作に関し、昇降対象の構造物として、上下方向に配置されたはしごを例に挙げて説明するが、他の構造物についても、後述する動作及び手順で移動ロボット１０を昇降させることが可能である。
【００１５】
前記移動ロボット１０は、特に限定されるものではないが、前記肢体１１を４つ有する４肢ロボットとして構成されており、前記肢体１１は、胴体１２の上側に相対移動可能に連なってロボットの腕部として機能する左右一対の上肢部１５，１６と、胴体１２の下側に相対移動可能に連なってロボットの脚部として機能する左右一対の下肢部１８，１９とからなる。
【００１６】
前記上肢部１５，１６は、左右両側で相互に同一の構成となっており、その先端側に設けられたエンドエフェクタ２１と、エンドエフェクタ２１から胴体１２までの間の２箇所に設けられた肢体構成部２３と、エンドエフェクタ２１と先端側の一方の肢体構成部２３との間、各肢体構成部２３の間、及び他方の肢体構成部２３と胴体１２との間を相対回転可能に接続する関節部２４と、各関節部２４に設けられ、前記相対回転の動力となる駆動装置としてのモータ２６とを備えている。この上肢部１５，１６は、複数の部材が関節部２４を介して相対回転可能に接続された多関節型に設けられ、特に限定されるものではないが、本実施形態では、７自由度の動作が可能となっている。すなわち、前記上肢部１５，１６は、制御装置１３で駆動制御された各モータ２６の作動により、先端側に設けられたエンドエフェクタ２１が、胴体１２に対して所定範囲の空間内を移動可能な公知の構造となっている。
【００１７】
本実施形態の前記エンドエフェクタ２１は、昇降動作時に肢体１１が係合するはしごの桟に引っ掛け可能なフック状に設けられているが、本発明においては、前記桟に係合できる限りにおいて、その他の様々な形状や構造を採用することができる。また、エンドエフェクタ２１には、桟に係合したときの反力を検出する６軸の力覚センサ２８（図２参照）が設けられている。
【００１８】
前記各関節部２４には、接続する部材の相対回転角度を検出するエンコーダ等の角度センサ２９（図２参照）が設けられている。
【００１９】
前記各下肢部１８，１９は、前記上肢部１５，１６と同一の構成となっているため、当該構成の詳細な説明を省略し、以下の説明においては、同一若しくは同等の構成部分に同一符号を用いることとする。
【００２０】
前記制御装置１３は、ＣＰＵ等の演算処理装置及びメモリやハードディスク等の記憶装置等からなるコンピュータによって構成され、当該コンピュータを以下の各手段として機能させるためのプログラムがインストールされている。
【００２１】
この制御装置１３は、図２に示されるように、所定の条件が入力される入力手段３１と、移動ロボット１０がはしごを昇降する際の肢体１１の目標動作を設定する動作設定手段３２と、動作設定手段３２で設定された目標動作を移動ロボット１０に実現させるように、各肢体１１にそれぞれ設けられた各モータ２６の駆動を制御する駆動制御手段３３とを備えている。
【００２２】
前記入力手段３１では、はしごの昇降パターンと移動ロボットの動作条件等が入力される。
【００２３】
前記昇降パターンとしては、肢体１１の４肢のうち常時２肢を桟に係合しながら移動ロボット１０を昇降させる図３及び図４の２種類の２点支持方式と、肢体１１の４肢のうち常時３肢を桟に係合しながら移動ロボット１０を昇降させる図５の３点支持方式とがある。
【００２４】
２点支持方式としては、図３の（Ａ）〜（Ｃ）に順に示されるように、左右の何れか一方の上肢部１５，１６及びその左右反対側の下肢部１８，１９をペアにして同時に動かすトロット方式と、図４の（Ａ）〜（Ｃ）に順に示されるように、左右の何れか一方の上肢部１５，１６、及びその左右同一側の下肢部１８，１９をペアにして同時に動かすペース方式とがある。従って、入力手段３１では、はしごの昇降パターンとして、トロット方式、ペース方式、若しくは３点支持方式の何れかが選択される。
【００２５】
ここで、２点支持方式のはしごの昇降パターンでは、図３及び図４に示されるように、先ず、桟Ｂに係合していない一方のペアの２つの肢体１１が、他方のペアの２つの肢体１１が係合している桟Ｂよりも１段上の桟Ｂに向かって動かされ（当該各図中（Ａ）参照）、当該桟Ｂに係合される。そして、一方のペアの２つの肢体１１が桟Ｂに係合した状態で、他方のペアの２つの肢体１１が、胴体１２とともに持ち上げられ（当該各図中（Ｂ）参照）、自身が係合していた桟Ｂよりも２段上の桟Ｂに係合させた上で、再び、一方のペアの２つの肢体１１を桟Ｂから離し（当該各図中（Ｃ）参照）、以上の動作が繰り返し行われる。
【００２６】
従って、２点支持方式のはしごの昇降パターンでは、ペアとなっている肢体１１の１回の動作により、はしごＨの２段分の昇降が可能になり、肢体１１を１肢ずつ動かして図５の（Ａ）〜（Ｆ）の順で昇降する３点支持方式よりも速い昇降動作が可能になる。なお、以下の説明においては、ペース方式での各種処理について説明するが、他の方式でも同様である。
【００２７】
また、入力手段３１では、移動ロボット１０の動作条件として、１回のはしご昇降動作毎に、始点及び終点におけるエンドエフェクタ２１の位置及び姿勢と、始点から終点までに要する時間とが入力される。ここで、１回のはしご昇降動作においては、図６に示されるように、はしごＨの桟Ｂにエンドエフェクタ２１が係合している地点を始点Ｓとし、その２段上の桟Ｂを終点Ｅとし、始点Ｓとなる桟Ｂからエンドエフェクタ２１を外す動作ａと、終点Ｅとなる桟Ｂの高さまでエンドエフェクタ２１を上げるために、エンドエフェクタ２１がはしごＨに干渉しない位置まで遠ざける動作ｂと、終点Ｅの桟Ｂの高さよりもやや高い位置までエンドエフェクタ２１を上昇させる動作ｃと、終点Ｅとなる桟Ｂにエンドエフェクタ２１を近づけながら、当該桟Ｂの上方にエンドエフェクタ２１を配置する動作ｄと、終点Ｅの桟Ｂに向かってエンドエフェクタ２１を下降させ、当該桟Ｂにエンドエフェクタ２１を引っ掛ける動作ｅとが順に行われるようになっている。
【００２８】
なお、本実施形態では、上下一対の肢体１１を同時に移動させる１回のはしご昇降動作において、終点Ｅを始点Ｓに対して２段上の桟Ｂに設定しているが、終点Ｅを１段上の桟Ｂの位置にしても良いし、移動ロボット１０のスペック上許容される限りにおいて、終点Ｅを３段以上の上の桟Ｂに設定しても良い。
【００２９】
前記動作設定手段３２は、図２に示されるように、エンドエフェクタ２１の始点Ｓから終点Ｅまでの間を通過する中間点Ｔでのエンドエフェクタ２１の位置及び姿勢を決定する中間点決定部３５と、エンドエフェクタ２１の始点Ｓ、終点Ｅ及び中間点Ｔの位置及び姿勢に基づき、エンドエフェクタ２１の動作目標軌道を決定する動作目標軌道決定部３６と、決定した動作目標軌道で動作するために必要な肢体１１の目標動作値を演算する目標動作値演算部３７と、算出された目標動作値が適正か否かを判定し、適正でない場合に動作目標軌道を補正するための処理を行う補正処理部３８と、補正処理部３８を経て決定された動作目標軌道に基づき、移動ロボット１０がはしご昇降動作中にはしごＨから滑落しないように、移動ロボット１０を多質点モデル化してエンドエフェクタ２１が桟Ｂに係合する際の反力を設定する反力設定部３９とを備えている。
【００３０】
前記中間点決定部３５では、はしごＨのサイズ等を考慮して、予め入力された前述の動作ａ〜ｅをはしごＨに干渉せずに行えるように、各動作ａ〜ｅの遷移地点となる４地点の中間点Ｔの位置を表す３次元の位置ベクトルが定められるとともに、当該中間点Ｔでのエンドエフェクタ２１の姿勢を表す３次元の姿勢ベクトルが予め指定された条件で決定される。
【００３１】
前記動作目標軌道決定部３６では、始点Ｓ及び終点Ｅにおける各エンドエフェクタ２１の３次元の位置ベクトル及び姿勢ベクトルと、その間の４つの中間点Ｔの各エンドエフェクタ２１の３次元の位置ベクトル及び姿勢ベクトルから、始点Ｓ、各中間点Ｔ及び終点Ｅを線形補完することにより、始点Ｓから終点Ｅまでのエンドエフェクタ２１の動作目標軌道が決定される。
【００３２】
前記目標動作値演算部３７では、動作に必要な肢体１１の目標動作値として、動作目標軌道から逆運動学を解くことにより、各関節部２４の回転角度と、当該回転角度に基づき特定される回転角速度及び回転角加速度とが算出される。
【００３３】
前記補正処理部３８では、目標動作値演算部３７で求められた各関節部２４の回転角度、回転角速度及び回転角加速度が、装置スペック等により特定された許容上限値を超えるか否かについて判定され、回転角度、回転角速度及び回転角加速度の何れか一つでも前記許容上限値より上回る場合に、次の補正処理が行われる。
【００３４】
すなわち、この場合、補正処理部３８では、上下２つの肢体１１が昇降動作する過程において、予め定めたルールにて、胴体１２が所定量水平方向に移動して前記多質点モデルの各質点の位置を変更するように前提条件を変えた上で、再度、中間点Ｔの位置及びエンドエフェクタ２１の動作目標軌道を求めるように、中間点決定部３５及び動作目標軌道決定部３６に指令する。
【００３５】
そして、補正された動作目標軌道による各関節部２４の回転角度、回転角速度及び回転角加速度が、再び許容上限値を超える否かについて判定され、当該許容上限値を超える場合には、以上の補正処理が再び行われ、各関節部２４の回転角度、回転角速度及び回転角加速度が許容上限値以下に収まるまで、前述の補正処理が繰り返し行われる。
【００３６】
前記反力設定部３９では、移動ロボット１０を多質点モデルとし、上下２箇所で桟Ｂに係合している係合部位のエンドエフェクタ２１を結んだ稜線周りのモーメントがゼロになるように、以下のように反力が算出される。
【００３７】
すなわち、ここでは、図７に示されるように、移動ロボット１０を予め設定された多質点モデルとして考え、次の演算が行われる。なお、図７は、同図中右側の上肢部１５及び下肢部１８が桟Ｂに引っ掛けられ、同図中各左側の上肢部１６及び下肢部１９が桟Ｂから外された状態を表している。この状態から、はしご昇降動作として、桟Ｂから外された左側の上肢部１６及び下肢部１９が移動する際に、移動ロボット１０にかかる力としては、桟Ｂに係合している上肢部１５及び下肢部１８の把持点（係合点）となる桟Ｂから受ける反力と、移動ロボット１０の自重により各質点Ｐｉに発生する重力と、移動ロボット１０がはしご昇降動作をする際に各質点Ｐｉに発生する慣性力とがある。これらの力による稜線Ｌ周りのモーメントＭは、前記反力によるモーメントＭ１、各質点Ｐｉの重力によるモーメントＭ２、及び各質点Ｐｉの慣性力によるモーメントＭ３の総和になる。
【００３８】
ここで、図７中右側の下肢部１８の把持点を原点Ｏとしたときに、反力ＦによるモーメントＭ１は、下肢部１８の同モーメントがゼロとなるため、把持部における上肢部１５のみを考慮すれば良く、次式（１）で求められる。式（１）中、ｒｈは、上肢部１５の位置ベクトルである。また、各質点Ｐｉ（ｉ＝１〜ｎ、図７の事例では、ｎ＝１３）の重力によるモーメントＭ２は、次式（２）で求められ、各質点Ｐｉの慣性力によるモーメントＭ３は、次式（３）で求められる。式（２）、（３）中、ｍｉは、各質点Ｐｉの質量であり、ｒｉは、各質点Ｐｉの位置ベクトルであり、ｇは重力加速度であり、ａｉは、入力手段３１で入力された時間に基づき導出された各質点Ｐｉの加速度である。そして、次式（４）で示されるモーメントの総和Ｍがゼロになるように、把持点での上肢部１５の反力Ｆが求められる。なお、ここで求めた反力Ｆは、下肢部１８にも適用される。
【数１】
2018034238.tif 000003
【００３９】
前記駆動制御手段３３では、補正処理部３８を経て最終決定された動作目標軌道でエンドエフェクタ２１が動作し、且つ、反力設定部３９で設定された反力が得られるように、エンドエフェクタ２１に設けられた力覚センサ２８や角度センサ２９の検出値をフィードバックしながら、各関節部２４にそれぞれ設けられた各モータ２６の駆動が制御され、移動ロボット１０によるはしご昇降動作が行われることになる。
【００４０】
次に、移動ロボット１０によるはしご昇降動作を行わせる各種処理や動作について、図８のフローチャートを用いながら説明する。
【００４１】
先ず、オペレータ等により、はしごＨの昇降パターンと、各桟Ｂそれぞれの昇降動作における各エンドエフェクタ２１の始点Ｓと終点Ｅの位置及び姿勢と所要時間とが入力手段３１を通じて入力される（ステップＳ１０１）。
【００４２】
次に、中間点決定部３５にて、各エンドエフェクタ２１の始点Ｓと終点Ｅの位置及び姿勢から、始点Ｓと終点Ｅの間となる４箇所の中間点Ｔでのエンドエフェクタ２１の位置及び姿勢が求められる（ステップＳ１０２）。
【００４３】
その後、動作目標軌道決定部３６にて、始点Ｓ、終点Ｅ、及び各中間点Ｔでの各エンドエフェクタ２１の位置ベクトル及び姿勢ベクトルとから、エンドエフェクタ２１の動作目標軌道が決定される（ステップＳ１０３）。
【００４４】
更に、目標動作値演算部３７にて、決定した動作目標軌道でのエンドエフェクタ２１の動作を可能にする目標動作値、すなわち、各関節部２４の回転角度、回転角速度及び回転角加速度が求められる（ステップＳ１０４）。
【００４５】
次に、補正処理部３８において、回転角度、回転角速度及び回転角加速度が、前記許容上限値を超える否かが判定され（ステップＳ１０５）、当該許容上限値以内の場合には、反力設定部３９にて、各エンドエフェクタ２１の反力が求められる（ステップＳ１０６）。一方、前記許容上限値を超えている場合には、補正処理部３８において、その際の動作目標軌道に対し、移動ロボット１０の動作過程におけるモデル上の質点Ｐｉの位置をスライドさせるように前提条件を変更し、中間点Ｔでのエンドエフェクタ２１の位置及び姿勢と動作目標軌道を再度求めるように、中間点決定部３５及び動作目標軌道決定部３６での再処理を行う指令がなされる（ステップＳ１０７）。
【００４６】
そして、以上の処理を経て最終決定された動作目標軌道に従いエンドエフェクタ２１が動作し、且つ、反力設定部３９で設定された反力が得られるように、各関節部２４にそれぞれ設けられた各モータ２６の駆動が制御され（ステップＳ１０８）、移動ロボット１０によるはしご昇降動作が行われる。
【００４７】
なお、前記実施形態では、反力設定部３９での反力の演算について、２点支持方式によるはしごの昇降パターンについて説明したが、３点支持方式のはしごの昇降パターンが選択された場合、上式（１）の反力ＦによるモーメントＭ１は、３点の把持点のうち各２点を結ぶ各稜線Ｌ周りのモーメントについて、原点Ｏとする１点で係合する肢体１１を除く２つの肢体１１のモーメントの総和とし、上式（４）から各反力Ｆが求められる。
【００４８】
また、反力設定部３９での処理は、はしごＨ上で移動ロボット１０が滑落しないための安定判別法として利用することもできる。すなわち、この安定判別法では、力覚センサ２８で検出された実際の反力と、反力設定部３９で求められた反力を目標反力とが対比され、実際の反力が目標反力よりも低いか否かによって、移動ロボット１０の安定性が判別される。そして、実際の反力が目標反力よりも低く、移動ロボット１０が不安定な状態と判定されれば、実際の反力を高めるように、対応するモータ２６の出力を上げたり、目標とする把持点の位置を更に下げたりする等、モータ２６の駆動制御がなされる。この安定判別法によれば、例えば、災害時にはしごＨが変形する等のタフな環境においても移動ロボット１０が滑落しないような運動生成が可能であり、また、はしごＨの移動ロボット１０の昇降動作中に外乱が加わっても、把持点での反力の調整により、移動ロボット１０の転倒を回避できる。従って、現場で作業しないと分からない実際の環境に応じて、移動ロボット１０が滑落しないための肢体１１の動作制御が可能となる。
【００４９】
また、移動ロボット１０としては、図示構成例に限定されるものではなく、後述するように、少なくとも上下２箇所のエンドエフェクタ２１を構造物の一部に係合しながら、係合していないエンドエフェクタ２１を上下方向に移動することで、構造物の昇降動作を可能にする構成及び数の肢体１１を有している限りにおいて、例えば、頭部を更に設ける等、種々の形状、構成の脚式ロボットを採用することができる。
【００５０】
その他、本発明における装置各部の構成は図示構成例に限定されるものではなく、実質的に同様の作用を奏する限りにおいて、種々の変更が可能である。
【符号の説明】
【００５１】
１０  移動ロボット
１１  肢体
１３  制御装置
２１  エンドエフェクタ
２３  胴体構成部
２４  関節部
２６  モータ（駆動装置）
３２  動作設定手段
３３  駆動制御手段
３５  中間点決定部
３６  動作目標軌道決定部
３７  目標動作値演算部
３８  補正処理部
３９  反力設定部
Ｅ  終点
Ｓ  始点
Ｈ  はしご（構造物）
Ｌ  稜線
Ｍ  モーメント
Ｔ  中間点
【図１】
2018034238.tif 000004
【図２】
2018034238.tif 000005
【図３】
2018034238.tif 000006
【図４】
2018034238.tif 000007
【図５】
2018034238.tif 000008
【図６】
2018034238.tif 000009
【図７】
2018034238.tif 000010
【図８】
2018034238.tif 000011