以下、添付図面に即してこの発明に係る2足移動体の補助装置を実施するための形態について説明する。
図1は、この発明の第1実施例に係る2足移動体の補助装置を全体的に示す概略図である。
図1において、符号10はこの発明の第1実施例に係る2足移動体の補助装置を示す。補助装置10は、2足移動体12を上方から支持するリフタ14からなる。リフタ14と2足移動体12は、ハンガ16とロープ状の接続部材(ハンガベルト)18とを介して接続される。ハンガベルト18は、具体的には、2足移動体12の両肩付近に設置されるブラケット(図示なし)に接続される。
尚、図において2足移動体12の前後方向をX方向、左右方向をY方向、重力方向をZ方向と定める。X、Y、Z方向は互いに直交する。
また、ここでは2足移動体12として2足移動ロボットを例にとって説明する。尚、2足移動体12は人であっても良い。
2足移動ロボット12は基体12aと、基体12aの上部から左右に延設された一対の腕部12bと、基体12aの下部から延設された左右一対の脚部12cと、基体12aの上面側に設けられた頭部12dと、それらの動作を制御するロボット制御部12eを備えると共に、左右の脚部12cのそれぞれの離床および着床を伴う動きにより、2足移動可能な機能を有する。ロボット制御部12eは、CPU、ROM、メモリおよび入出力回路などを備えるマイクロ・コンピュータからなる。詳しくは特開2006−015433号公報に記載されるので、ここでの説明は省略する。
リフタ14は2足移動ロボット12の上方において可動自在に設置されると共に、2足移動ロボット12が姿勢を崩して転倒しかけた場合、2足移動ロボット12を上方から支持する。
ハンガベルト18は2足移動ロボット12が姿勢を崩さずに自立して歩行する場合、歩行動作に影響を与えないように緩んだ状態(張力0の状態)にある。
リフタ14はその上方側でリフタZ軸ガイドフレーム20に接続される。具体的には、リフタZ軸ガイドフレーム20にはボールネジ20aが備えられ、リフタ14はボールネジ20aに適宜な接続部材を介して接続される。リフタZ軸ガイドフレーム20の上端にはZ軸電動モータ22が設置される。Z軸電動モータ22でボールネジ20aを回転させることにより、リフタ14はリフタZ軸ガイドフレーム20に対してZ方向に移動させられる。
リフタZ軸ガイドフレーム20はその上方側でリフタY軸ガイドフレーム24に接続される。具体的には、リフタY軸ガイドフレーム24にはボールネジ(図示せず)が備えられ、リフタZ軸ガイドフレーム20はボールネジに適宜な接続部材を介して接続される。リフタY軸ガイドフレーム24の端部にはY軸電動モータ26が設置される。Y軸電動モータ26でボールネジを回転させることにより、リフタZ軸ガイドフレーム20(およびリフタ14)はリフタY軸ガイドフレーム24に対してY方向に移動させられる。
リフタY軸ガイドフレーム24はその上方側でリフタX軸ガイドフレーム28に摺動自在に接続される。リフタX軸ガイドフレーム28の下面部にはラック&ピニオン駆動方式のラック28aが備えられる。リフタY軸ガイドフレーム24の上面部にはX軸電動モータ30が設置される。X軸電動モータ30でラック28aに係合するピニオン(図示なし)を回転させることにより、リフタY軸ガイドフレーム24(およびリフタZ軸ガイドフレーム20およびリフタ14)はリフタX軸ガイドフレーム28に対してX方向に移動させられる。尚、リフタX軸ガイドフレーム28自体は適宜な支柱に固定される。以上より、リフタ14は3次元空間において3次元方向に移動自在とされる。
補助装置10は、2足移動ロボット12のX方向とY方向における位置を検出するXY平面センサ32を備える。XY平面センサ32は2足移動ロボット12が歩行する床面などの適宜な位置に設置されると共に、レーザ光を2足移動ロボット12に照射し、2足移動ロボット12から反射される反射光に基づいて2足移動ロボット12のX方向とY方向における位置(X座標、Y座標)に応じた信号を出力する。
また、補助装置10は2足移動ロボット12のZ方向おける位置を検出するZ方向センサ34を備える。Z方向センサ34は2足移動ロボット12の腰位置に設けられると共に、レーザ光を床面に照射し、床面から反射される反射光に基づいて2足移動ロボット12のZ方向における位置(Z座標)に応じた信号を出力する。
さらに、補助装置10は2足移動ロボット12の荷重を検出する荷重センサ36を備える。荷重センサ36はリフタ14とハンガ16の間に設置されると共に、ハンガ16より下部にかかる荷重、特に2足移動ロボット12から受ける荷重に応じた信号を出力する。
XY平面センサ32とZ方向センサ34と荷重センサ36の出力信号は信号線を介してリフタ制御部38に入力される。また、前記した3個の電動モータ22、26、30も信号線を介してリフタ制御部38に接続される。
リフタ制御部38はCPU、ROM、メモリおよび入出力回路などを備えるマイクロ・コンピュータからなると共に、入力される各センサの出力信号に基づいて3個の電動モータ22、26、30の駆動を制御する。
図2は、その制御を説明するフロー・チャートである。図示のプログラムは補助装置10の電源(図示なし)がオンされたとき、リフタ制御部38において実行される。
S10において、このプログラムの制御ルーチンに用いられるカウンタnを0にリセットする。
次いでS12に進み、リフタ14と2足移動ロボット12の初期位置(リフタ14のX、Y、Z座標Xlift_0、Ylift_0、Zlift_0と2足移動ロボット12のX、Y、Z座標Xrobot_0、Yrobot_0、Zrobot_0)を検出する。リフタ14のX、Y、Z座標Xlift_0、Ylift_0、Zlift_0は、3個の電動モータ22、26、30それぞれに付随するロータリエンコーダの出力信号に基づいて検出される。2足移動ロボット12の初期位置は、前記したXY平面センサ32とZ方向センサ34の出力信号に基づいて検出される。
リフタ14と2足移動ロボット12の初期位置は、前記したハンガベルト18の緩み量が最小となるように設定される。また、図示のように、リフタ14と2足移動ロボット12の初期位置の偏差Δx、Δy、Δzが算出される。
次いでS14に進み、2足移動ロボット12のX座標Xrobot_nとリフタ14のX座標Xlift_nの差の絶対値が所定値σ未満であるか否か、あるいは2足移動ロボット12のX方向の歩行速度Vrobotx_nの絶対値が所定速度τ未満であるか否か判断する。2足移動ロボット12のX方向の歩行速度Vrobotx_nはプログラムループにおける前回のX座標Xrobot_n-1と今回のX座標Xrobot_nの差から算出する。
S14において肯定される場合、S16に進んでリフタ14のX方向の速度ゲインKβを小さい値(Low)に設定する。一方、S14において否定される場合、S18に進んでリフタのX方向の速度ゲインKβを大きい値(High)に設定する。
次いでS20に進み、2足移動ロボット12のY座標Yrobot_nとリフタ14のY座標Ylift_nの差の絶対値が所定値θ未満であるか否か、あるいは2足移動ロボット12のY方向の歩行速度Vroboty_nの絶対値が所定速度ρ未満であるか否か判断する。2足移動ロボット12のY方向の歩行速度Vroboty_nはプログラムループにおける前回のY座標Yrobot_n-1と今回のY座標Yrobot_nの差から算出する。
S20において肯定される場合、S22に進んでリフタ14のY方向の速度ゲインKγをLowに設定する。一方、S20において否定される場合、S24に進んでリフタ14のY方向の速度ゲインKγをHighに設定する。
S14やS20において否定されるとき、リフタ14の速度ゲインKβ、KγをHighに設定するのは、2足移動ロボット12とリフタ14の位置が離れている場合、あるいは2足移動ロボット12の移動速度が大きい場合には、リフタ14を2足移動ロボット12に応答性良く追従させるべく、リフタ14の移動速度も大きく変更する必要があるためである。
次いでS26に進み、リフタ14のX、Y、Z方向の移動速度Vliftx_n、Vlifty_n、Vliftz_nを算出する。リフタ14のX方向の移動速度Vliftx_nは、具体的には、プログラムループにおける前回のX方向の移動速度Vliftx_n-1に、X方向の2足移動ロボット12の位置Xrobot_nからリフタ14の位置Xlift_nと初期位置の偏差Δxを減算した値に速度ゲインKβを乗じたものを加算することによって算出される。リフタ14のY方向の移動速度Vlifty_nおよびZ方向の移動速度Vliftz_nについても同様に算出される。
リフタ制御部38は、リフタ14が移動速度Vliftx_n、Vlifty_n、Vliftz_nで移動するように3個の電動モータ22、26、30それぞれに駆動指令値を送出する。
次いでS28に進み、2足移動ロボット12が所定エリア内に位置するか否か判断する。これは、図1の安全柵40に近接スイッチなどを設け、そのスイッチのオン・オフから判断する。S28で肯定される場合、S30に進んで荷重センサ36によって検出された荷重が所定荷重α未満であるか否か判断する。
S30で肯定される場合にはS32に進んで、補助装置10の電源がオンされているか否か判断する。肯定される場合、S34に進んでカウンタnを1つインクリメントする。
次いでS36に進み、リフタ14と2足移動ロボット12の位置(リフタ14のX、Y、Z座標Xlift_n、Ylift_n、Zlift_nと2足移動ロボット12のX、Y、Z座標Xrobot_n、Yrobot_n、Zrobot_n)を再度検出し、S14に進む。
S28、S30およびS32において否定されない限り、S14からS36の処理が繰り返される。即ち、リフタ14のX、Y、Z方向の移動速度Vliftx_n、Vlifty_n、Vliftz_nがリフタ14と2足移動ロボット12の位置の偏差に基づいて算出され続ける結果、リフタ14は2足移動ロボット12に追従するように移動することになる。
S30において否定される場合、S38に進んでX方向とY方向の移動速度Vliftx_n、Vlifty_nを0に設定すると共に、Z方向の移動速度Vliftz_nを最大速度Vz_Maxに設定し、プログラムを終了する。即ち、S30において否定される場合、2足移動ロボット12が姿勢を崩して転倒しかける状況にあると判断し、リフタ14のX方向とY方向の移動を停止させると共に、リフタ14を上方に移動させることで2足移動ロボット12を上方から支持するようにした。また、S28で否定される場合もS38に進んで同様の処理を実行するようにした。これは、移動ロボット12が所定エリア内に位置しない場合、即ち移動ロボット12が安全柵40に衝突する場合にはその後に転倒する可能性が高く、転倒を事前に回避するためである。尚、Z方向の移動速度Vliftz_nは所定時間経過後に0とする。
S32において否定される場合、S40に進んで速度ゲインKβ、Kγを0にリセットし、プログラムを終了する。
以上のように、この発明の第1実施例にあっては、2足移動体12にハンガベルト18を介して接続されると共に、2足移動体12が姿勢を崩したときに2足移動体12を上方から支持するリフタ14を3次元空間において移動自在なリフタ移動手段(リフタZ軸ガイドフレーム20、Z軸電動モータ22、リフタY軸ガイドフレーム24、Y軸電動モータ26、リフタX軸ガイドフレーム28、X軸電動モータ30)と、リフタ移動手段の動作を制御するリフタ制御部38と、3次元空間における2足移動体12の位置を検出する検出手段(XY平面センサ32、Z方向センサ34)とを備えると共に、検出された2足移動体12の位置に基づいてリフタ14が2足移動体12に追従するようにリフタ移動手段の動作を制御する(S26)如く構成したので、ハンガベルト18の緩み量を少なくでき、よって2足移動体12が姿勢を崩して転倒しかけた場合、その荷重を支持するときの衝撃を緩和させることができる。
また、リフタ14に作用する2足移動体12の荷重を検出する荷重センサ36を備えると共に、検出された荷重が所定荷重α以上であるとき、リフタ14を上方に移動させるようにリフタ移動手段の動作を制御する(S30、S38)如く構成したので、2足移動体12が姿勢を崩して転倒しかけた場合、その荷重を支持するときの衝撃を一層緩和させることができる。
図3は、この発明の第2実施例に係る2足移動体の補助装置を全体的に示す概略図である。第1実施例と同一の構成については同一の符号を付すと共に、説明を省略する。
第2実施例に係る2足移動体の補助装置にあっては、2足移動体12をトレッドミル42の上で歩行させると共に、トレッドミル42を介してリフタ14の動作を制御するようにした。第2実施例においても、2足移動体12として2足移動ロボットを例にとって説明する。尚、2足移動体12は人であっても良い。
トレッドミル42は、2足移動ロボット12が歩行自在なエンドレスベルト42aと、エンドレスベルト42aを駆動する電動モータ42bと、電動モータ42bの動作を制御するベルト制御部42cとからなる。
エンドレスベルト42aは、トレッドミル42の両端部に設けられる2個のローラ42d、42eに掛け渡され、電動モータ42bはその内のローラ42dを回転させることによってエンドレスベルト42aを移動させる。
トレッドミル42の前部と後部には2個のX軸リミットセンサ42f、42gが配置される。X軸リミットセンサ42f、42gはそれぞれエンドレスベルト42aの対岸に設置される反射体42h、42iに向けてレーザ光を照射し、反射体42h、42iからの反射光を検知する。
また、トレッドミルの前部左右には2個のY軸リミットセンサ42j、42kが配置される。Y軸リミットセンサ42j、42kはそれぞれトレッドミル42の後部に設置される反射体42l、42mに向けてレーザ光を照射し、反射体42l、42mからの反射光を検知する。
X軸リミットセンサ42f、42gおよびY軸リミットセンサ42j、42kの出力信号はベルト制御部42cに入力される。また、XY平面センサ32とZ方向センサ34と荷重センサ36の出力信号もベルト制御部42cに入力される。
ベルト制御部42cは、CPU、ROM、メモリおよび入出力回路などを備えるマイクロ・コンピュータからなると共に、入力される各センサの出力信号に基づいて電動モータ42bの駆動を制御する。また、図示のように、ベルト制御部42cとリフタ制御部38は互いに通信自在に接続されると共に、リフタ制御部38はベルト制御部42cを介して送信される各種情報に基づいて前記した3個の電動モータ22、26、30の駆動を制御する。
図4は、それら制御を説明するフロー・チャートである。左側のフロー・チャートがベルト制御部42cにおいて実行されるプログラムであり、右側のフロー・チャートがリフタ制御部38において実行されるプログラムである。これらのプログラムは、補助装置10の電源(図示なし)がオンされたとき、ベルト制御部42cとリフタ制御部38においてそれぞれ並行して実行される。
まず左側のベルト制御部42cにおいて実行されるプログラムについて説明すると、S100において、2足移動ロボット12のX方向の目標位置Xaを設定する。この目標位置Xaは、2足移動ロボット12をエンドレスベルト42a上でX方向において定位置で歩行させるに際して定められる目標位置である。
次いでS102に進み、このプログラムの制御ルーチンに用いられるカウンタnを0にリセットする。
次いでS104に進み、2足移動ロボット12の初期位置(X、Y、Z座標Xrobot_0、Yrobot_0、Zrobot_0)を検出する。検出された2足移動ロボット12の初期位置に関する情報はリフタ制御部38に送信される。
次いでS106に進み、2足移動ロボット12が歩行開始(歩き出し)したか否か判断する。具体的には、2足移動ロボット12のX方向の位置についてプログラムループにおける前回値と今回値を比較し、2値が異なる場合、2足移動ロボット12が歩行開始したと判断する。
次いでS108に進み、エンドレスベルト42aのX方向の速度ゲインKαをHighに設定する。ここで速度ゲインKαをHighに設定するのは、2足移動ロボット12の歩き出しに合わせてエンドレスベルト42aを速く移動させるためである。
次いでS110に進み、検出された2足移動ロボット12のX方向の位置Xrobot_nが目標位置Xaに対して±100mmの範囲内にあるか否か判断する。否定される場合、S112に進んでエンドレスベルト42aの速度ゲインKαをHighに設定する。
一方、S110において肯定される場合、S114に進んで検出された2足移動ロボット12のX方向の位置Xrobot_nが目標位置Xaに対して±100mmの範囲内にある状態が1sec以上維持されたか否か判断する。否定される場合、S112に進んでエンドレスベルト42aの速度ゲインKαをHighに設定する。
一方、S114において肯定される場合、S116に進んでエンドレスベルト42aの速度ゲインKαをLowに設定する。エンドレスベルト42aの速度ゲインKαに関する情報はリフタ制御部38に送信される。
次いでS118に進み、エンドレスベルト42aのX方向の移動速度Vbelt_nを算出する。エンドレスベルト42aのX方向の移動速度Vbelt_nは、具体的には、プログラムループにおける前回の移動速度Vbelt_n-1に、目標位置XaからX方向の2足移動ロボットの位置Xrobot_nを減算した値に速度ゲインKαを乗じたものを加算することによって算出される。
ベルト制御部42cは、エンドレスベルト42aが移動速度Vbelt_nで移動するように電動モータ42bに駆動指令値を送出する。
次いでS120に進み、2足移動ロボット12が所定エリア内に位置するか否か判断する。これは、X軸リミットセンサ42f、42gとY軸リミットセンサ42j、42kの出力信号に基づいて判断する。S120で肯定される場合、S122に進んで荷重センサ36によって検出された荷重が所定荷重α未満であるか否か判断する。S120やS122の判断結果に関する情報もリフタ制御部38に送信される。
S122において肯定される場合、S124に進んで補助装置10の電源がオンされているか否か判断する。この判断結果もリフタ制御部38に送信される。S124において肯定される場合、S126に進んでカウンタnを1つインクリメントする。
次いでS128に進み、2足移動ロボット12の位置(X、Y、Z座標Xrobot_n、Yrobot_n、Zrobot_n)を再度検出し、S110に進む。S128で検出された2足移動ロボット12の位置に関する情報もリフタ制御部38に送信される。
S120、S122およびS124において否定されない限り、S110からS128の処理が繰り返される。即ち、エンドレスベルト42aのX方向の移動速度Vbelt_nが目標位置Xaと2足移動ロボット12の位置Xrobot_nの偏差に基づいて算出され続ける結果、2足移動ロボット12が3次元空間において目標位置Xaを含む所定範囲に位置するようにエンドレスベルト42aの動作が制御されることになる。
S122において否定される場合、S130に進んでエンドレスベルト42aのX方向の移動速度Vbelt_nを0に設定し、プログラムを終了する。即ち、S122において否定される場合、2足移動ロボット12が姿勢を崩して転倒しかける状況にあると判断し、エンドレスベルト42aの移動を停止させるようにした。また、S120で否定される場合もS130に進んで同様の処理を実行するようにした。これは、移動ロボット12が所定エリア内に位置しない場合、即ち移動ロボット12がエンドレスベルト42aから逸脱する場合にはその後に転倒する可能性が高いためである。
S124において否定される場合、S132に進んで速度ゲインKαを0にリセットし、プログラムを終了する。
次いで、右側のリフタ制御部38において実行されるプログラムについて説明すると、S200において、このプログラムの制御ルーチンに用いられるカウンタnを0にリセットする。
次いでS202に進み、リフタ14の初期位置(X、Y、Z座標Xlift_0、Ylift_0、Zlift_0)を検出する。また、図示のように、ベルト制御部42cから送信される2足移動ロボット12の初期位置(X、Y、Z座標Xrobot_0、Yrobot_0、Zrobot_0)との偏差Δx、Δy、Δzを算出する。
次いでS204に進み、リフタ14のX方向の速度ゲインKβをHighに設定する。ここで速度ゲインKβをHighに設定するのは、2足移動ロボット12の歩き出しに備えてリフタ14を速く移動させるためである。
次いでS206に進み、ベルト制御部42cから送信されるエンドレスベルト42aの速度ゲインKαに関する情報に基づいてその速度ゲインKαがHighであるかLowであるか判断する。エンドレスベルト42aの速度ゲインKαがLowである場合、S208に進んでリフタ14のX方向の速度ゲインKβもLowに設定する。一方、エンドレスベルト42aの速度ゲインKαがHighである場合、S210に進んでリフタ14のX方向の速度ゲインKβもHighに設定する。エンドレスベルト42aの速度ゲインKαに合わせてリフタ14の速度ゲインKβを変更するのは、エンドレスベルト42aの移動速度を大きく変更する必要がある場合は、2足移動ロボット12が大きく移動する場合であるから、リフタ14の移動速度も大きく変更する必要があるためである。
次いでS212に進み、ベルト制御部42cから送信された2足移動ロボット12のY座標Yrobot_nとリフタ14のY座標Ylift_nの差の絶対値が所定値θ未満であるか否か、あるいは2足移動ロボット12のY方向の歩行速度Vroboty_nの絶対値が所定速度ρ未満であるか否か判断する。2足移動ロボット12のY方向の歩行速度Vroboty_nは、プログラムループにおける前回のY座標Yrobot_n-1と今回のY座標Yrobot_nの差から算出する。
S212において肯定される場合、S214に進んでリフタ14のY方向の速度ゲインKγをLowに設定する。一方、S212において否定される場合、S216に進んでリフタ14のY方向の速度ゲインKγをHighに設定する。ここで速度ゲインKγをHighに設定するのは、2足移動ロボット12とリフタ14の位置が離れている場合、あるいは2足移動ロボット12の移動速度が大きい場合には、リフタ14を2足移動ロボット12に応答性良く追従させるべく、リフタ14の移動速度も大きく変更する必要があるためである。
次いでS218に進み、リフタ14のX、Y、Z方向の移動速度Vliftx_n、Vlifty_n、Vliftz_nを算出する。リフタ14のX方向の移動速度Vliftx_nは、具体的には、プログラムループにおける前回の移動速度Vliftx_n-1に、ベルト制御部42cから送信されるX方向の2足移動ロボット12の位置Xrobot_nからリフタ14の位置Xlift_nと初期位置の偏差Δxを減算した値に速度ゲインKβを乗じたものを加算することによって算出される。リフタ14のY方向の移動速度Vlifty_nやZ方向の移動速度Vliftz_nについても同様に算出される。
リフタ制御部38は、リフタ14が移動速度Vliftx_n、Vlifty_n、Vliftz_nで移動するように3個の電動モータ22、26、30それぞれに駆動指令値を送出する。
次いでS220に進み、2足移動ロボット12が所定エリア内に位置するか否か判断する。ここでは、ベルト制御部42cから送信されるS120の判断結果に基づいて判断する。肯定される場合、S222に進んで荷重センサ36によって検出された荷重が所定荷重α未満であるか否か判断する。ここでの判断も、ベルト制御部42cから送信されるS122の判断結果に基づいて行う。
S222で肯定される場合、S224に進んで補助装置10の電源がオンされているか否か判断する。ここでの判断も、ベルト制御部42cから送信されるS124の判断結果に基づいて行う。S224で肯定される場合、S226に進んでカウンタnを1つインクリメントする。
次いでS228に進み、リフタ14の位置(X、Y、Z座標Xlift_n、Ylift_n、Zlift_n)を再度検出し、S206に進む。
S220、S222およびS224において否定されない限り、S206からS228の処理が繰り返される。即ち、リフタ14のX、Y、Z方向の移動速度Vliftx_n、Vlifty_n、Vliftz_nがリフタ14と2足移動ロボット12の位置の偏差に基づいて算出され続ける結果、リフタ14は2足移動ロボット12に追従するように移動することになる。
S222において否定される場合、S230に進んでX方向とY方向の移動速度Vliftx_n、Vlifty_nを0に設定すると共に、Z方向の移動速度Vliftz_nを最大速度Vz_Maxに設定し、プログラムを終了する。即ち、S222において否定される場合、2足移動ロボット12が姿勢を崩して転倒しかける状況にあると判断し、リフタ14のX方向とY方向の移動を停止させると共に、リフタ14を上方に移動させることで2足移動ロボット12を上方から支持するようにした。また、S220で否定される場合もS230に進んで同様の処理を実行するようにした。これは、移動ロボット12が所定エリア内に位置しない場合、即ち移動ロボット12がエンドレスベルト42aから逸脱する場合にはその後に転倒する可能性が高く、転倒を事前に回避するためである。尚、Z方向の移動速度Vliftz_nは所定時間経過後に0とする。
S224において否定される場合、S232に進んで速度ゲインKβ、Kγを0にリセットし、プログラムを終了する。
以上のように、この発明の第2実施例にあっては、2足移動体12にハンガベルト18を介して接続されると共に、2足移動体12が姿勢を崩したときに2足移動体12を上方から支持するリフタ14を3次元空間において移動自在なリフタ移動手段(リフタZ軸ガイドフレーム20、Z軸電動モータ22、リフタY軸ガイドフレーム24、Y軸電動モータ26、リフタX軸ガイドフレーム28、X軸電動モータ30)と、リフタ移動手段の動作を制御するリフタ制御部38と、3次元空間における2足移動体12の位置を検出する検出手段(XY平面センサ32、Z方向センサ34)とを備えると共に、検出された2足移動体12の位置に基づいてリフタ14が2足移動体12に追従するようにリフタ移動手段の動作を制御する(S218)如く構成したので、ハンガベルト18の緩み量を少なくでき、よって2足移動体12が姿勢を崩して転倒しかけた場合、その荷重を支持するときの衝撃を緩和させることができる。
また、2足移動体12が歩行自在なエンドレスベルト42aと、エンドレスベルト42aを駆動させる電動モータ42bと、電動モータ42bの動作を制御するベルト制御部42cとを備えると共に、検出された2足移動体12の位置に基づいて2足移動体12が3次元空間の所定範囲に位置するように電動モータ42bの動作を制御する(S118)如く構成したので、2足移動体12を追従するためのリフタ移動量を減少させることができ、よってリフタ移動手段が大型化するのを防止することができる。また、2足移動体12を補助するに要するスペースを抑制することができる。
また、リフタ14に作用する2足移動体12の荷重を検出する荷重センサ36を備えると共に、検出された荷重が所定荷重α以上であるとき、リフタ14を上方に移動させるようにリフタ移動手段の動作を制御する(S222、S230)如く構成したので、2足移動体12が姿勢を崩して転倒しかけた場合、その荷重を支持するときの衝撃を一層緩和させることができる。
図5は、この発明の第3実施例に係る2足移動体の補助装置の動作を説明するフロー・チャートである。
第3実施例に係る2足移動体の補助装置にあっては、第2実施例に対してXY平面センサ32、Z方向センサ34および荷重センサ36を排除すると共に、所定の歩行計画に従って歩行する2足移動ロボット12から送信される各種情報に基づいてエンドレスベルト42aおよびリフタ14の動作を制御するようにした。
図5は、それら制御を説明するフロー・チャートである。左側のフロー・チャートがベルト制御部42cにおいて実行されるプログラムであり、右側のフロー・チャートがリフタ制御部38において実行されるプログラムである。これらのプログラムは、補助装置10の電源(図示なし)がオンされたとき、ベルト制御部42cとリフタ制御部38においてそれぞれ並行して実行される。
図の中央に示すロボット制御部12eには、2足移動ロボット12の歩行計画が予め登録されると共に、2足移動ロボット12が有するジャイロセンサの出力信号に基づいて算出された2足移動ロボット12の現在座標(3次元座標)が入力される。また、2足移動ロボット12が有する傾斜センサや力センサなどの出力信号に基づいて算出された2足移動ロボット12の状態パラメータStatus_robotも入力される。
ロボット制御部12eにおける2足移動ロボット12の歩行計画、現在座標および状態パラメータStatus_robotは公知の送受信手段を用いてベルト制御部42cおよびリフタ制御部38に送信される。
まず左側のベルト制御部42cにおいて実行されるプログラムについて説明すると、S300において、2足移動ロボット12のX方向の目標位置Xaを設定する。この目標位置Xaは、2足移動ロボット12をエンドレスベルト42a上でX方向において定位置で歩行させるに際して定められる目標位置である。
次いでS302に進み、このプログラムの制御ルーチンに用いられるカウンタnを0にリセットする。
次いでS304に進み、ロボット制御部12eから送信される2足移動ロボット12の歩行計画に基づいてエンドレスベルト42aの動作計画を作成する。エンドレスベルト42aの動作計画とは、エンドレスベルト42aの移動速度Vbelt_nの時間的推移のことである。即ち、歩行計画に基づいて2足移動ロボット12が目標位置Xaを含む所定範囲に位置するようにエンドレスベルト42aの移動速度推移が作成される。作成されたエンドレスベルト42aの動作計画はリフタ制御部38に送信される。
次いでS306に進み、ロボット制御部12eから送信される2足移動ロボット12の現在位置に基づいてエンドレスベルト42aの動作計画を補正する。具体的には、2足移動ロボット12の現在位置に基づいて2足移動ロボット12が目標位置Xaを含む所定範囲に位置するようにエンドレスベルト42aの動作計画を補正する。
次いでS308に進み、補正されたエンドレスベルト42aの動作計画に従ってエンドレスベルト42aの動作を制御する。具体的には、補正されたエンドレスベルト42aの動作計画に従って電動モータ42bに駆動指令値を送出する。エンドレスベルト42aの動作状況に関する情報もリフタ制御部38に送信される。
次いでS310に進み、ロボット制御部12eから送信される状態パラメータStatus_robotが異常(Err)であるか否か判断する。状態パラメータStatus_robotが異常(Err)であることは、2足移動ロボット12に何らかの異常が生じていることを示す。S310において否定される場合、S312に進む。
S312においては、ロボット制御部12eから送信される状態パラメータStatus_robotが停止(Stop)であるか否か判断する。状態パラメータStatus_robotが停止(Stop)であることは、2足移動ロボット12が歩行を停止していることを示す。S312において否定される場合、S314に進んでカウンタnを1つインクリメントし、S306に進む。
S310およびS312において肯定されない限り、S306からS314の処理が繰り返される。即ち、2足移動ロボット12が3次元空間において目標位置Xaを含む所定範囲に位置するようにエンドレスベルト42aの動作が制御されることになる。
S310において肯定される場合、S316に進んでエンドレスベルト42aのX方向の移動速度Vbelt_nを0に設定し、プログラムを終了する。即ち、S310において肯定される場合、2足移動ロボット12が姿勢を崩して転倒しかける状況にあると判断し、エンドレスベルト42aの移動を停止させるようにした。
S312において肯定される場合、S318に進んでエンドレスベルト42aの動作を停止し、プログラムを終了する。
次いで、右側のリフタ制御部38において実行されるプログラムについて説明すると、S400において、このプログラムの制御ルーチンに用いられるカウンタnを0にリセットする。
次いでS402に進み、ロボット制御部12eから送信される2足移動ロボット12の歩行計画とベルト制御部42cから送信されるエンドレスベルト42aの動作計画に基づいてリフタ14の動作計画を作成する。リフタ14の動作計画とは、リフタ14の移動速度Vliftx_n、Vlifty_n、Vliftz_nの時間的推移のことである。即ち、2足移動ロボット12の歩行計画とエンドレスベルト42aの動作計画に基づいてリフタ14が2足移動ロボット12に追従するようにリフタ14の移動速度推移が作成される。
次いでS404に進み、ロボット制御部12eから送信される2足移動ロボット12の現在位置とベルト制御部42cから送信されるエンドレスベルト42aの動作状況に基づいてリフタ14の動作計画を補正する。具体的には、2足移動ロボット12の現在位置とエンドレスベルト42aの動作状況に基づいてリフタ14が2足移動ロボット12に追従するようにリフタ14の動作計画を補正する。
次いでS406に進み、補正されたリフタ14の動作計画に従ってリフタ14の動作を制御する。具体的には、補正されたリフタ14の動作計画に従って3個の電動モータ22、26、30に駆動指令値を送出する。
次いでS408に進み、ロボット制御部12eから送信される状態パラメータStatus_robotが異常(Err)であるか否か判断する。否定される場合、S410に進む。
S410においては、ロボット制御部12eから送信される状態パラメータStatus_robotが停止(Stop)であるか否か判断する。否定される場合、S412に進んでカウンタnを1つインクリメントし、S404に進む。
S408およびS410において肯定されない限り、S404からS412の処理が繰り返される。即ち、リフタ14は2足移動ロボット12に追従するように移動制御されることになる。
S408において肯定される場合、S414に進んでX方向とY方向の移動速度Vliftx_n、Vlifty_nを0に設定すると共に、Z方向の移動速度Vliftz_nを最大速度Vz_Maxに設定し、プログラムを終了する。即ち、S408において肯定される場合、2足移動ロボット12が姿勢を崩して転倒しかける状況にあると判断し、リフタ14のX方向とY方向の移動を停止させると共に、リフタ14を上方に移動させるようにした。尚、Z方向の移動速度Vliftz_nは所定時間経過後に0とする。
S410において肯定される場合、S416に進んでリフタの動作を停止し、プログラムを終了する。
以上のように、この発明の第3実施例にあっては、所定の歩行計画に従って歩行する2足移動ロボット12にハンガベルト18を介して接続されると共に、2足移動ロボット12が姿勢を崩したときに2足移動ロボット12を上方から支持するリフタ14を3次元空間において移動自在なリフタ移動手段(リフタZ軸ガイドフレーム20、Z軸電動モータ22、リフタY軸ガイドフレーム24、Y軸電動モータ26、リフタX軸ガイドフレーム28、X軸電動モータ30)と、リフタ移動手段の動作を制御するリフタ制御部38とを備えると共に、所定の歩行計画に基づいてリフタ14が2足移動ロボット12に追従するようにリフタ移動手段の動作を制御する如く構成したので、ハンガベルト18の緩み量を少なくでき、よって2足移動ロボット12が姿勢を崩して転倒しかけた場合、その荷重を支持するときの衝撃を緩和させることができる。また、2足移動ロボット12の歩行計画に基づいてリフタ14の動作を制御するので、2足移動ロボット12の位置を検出することが不要となり、簡略な構成ながら、上記の作用効果を得ることができる。
また、2足移動ロボット12が歩行自在なエンドレスベルト42aと、エンドレスベルト42aを駆動させる電動モータ42bと、電動モータ42bの動作を制御するベルト制御部42cとを備えると共に、所定の歩行計画に基づいて2足移動ロボット12が3次元空間の所定範囲に位置するように電動モータ42bの動作を制御する如く構成したので、2足移動ロボット12を追従するためのリフタ移動量を減少させることができ、よってリフタ移動手段が大型化するのを防止することができる。また、2足移動ロボット12を補助するに要するスペースを抑制することができる。
上記した如く、第1実施例および第2実施例にあっては、2足移動体(2足移動ロボット12)にロープ状の接続部材(ハンガベルト18)を介して接続されると共に、前記2足移動体が姿勢を崩したときに前記2足移動体を上方から支持するリフタ(14)からなる2足移動体の補助装置において、前記リフタを3次元空間において移動自在なリフタ移動手段(リフタZ軸ガイドフレーム20、Z軸電動モータ22、リフタY軸ガイドフレーム24、Y軸電動モータ26、リフタX軸ガイドフレーム28、X軸電動モータ30)と、前記リフタ移動手段の動作を制御するリフタ移動手段制御手段(リフタ制御部38)と、前記3次元空間における前記2足移動体の位置を検出する検出手段(XY平面センサ32、Z方向センサ34)とを備えると共に、前記リフタ移動手段制御手段は、前記検出された位置に基づいて前記リフタが前記2足移動体に追従するように前記リフタ移動手段の動作を制御する(S26、S218)如く構成した。
それにより、ロープ状の接続部材(ハンガベルト18)の緩み量を少なくでき、よって2足移動体が姿勢を崩して転倒しかけた場合、その荷重を支持するときの衝撃を緩和させることができる。
また、前記2足移動体が歩行自在なベルト(エンドレスベルト42a)と、前記ベルトを駆動させるベルト駆動手段(電動モータ42b)と、前記ベルト駆動手段の動作を制御するベルト駆動手段制御手段(ベルト制御部42c)とを備えると共に、前記ベルト駆動手段制御手段は、前記検出された位置に基づいて前記2足移動体が前記3次元空間の所定範囲に位置するように前記ベルト駆動手段の動作を制御する(S118)如く構成した。
それにより、2足移動体を追従するためのリフタ移動量を減少させることができ、よってリフタ移動手段が大型化するのを防止することができる。また、2足移動体を補助するに要するスペースを抑制することができる。
また、前記リフタに作用する前記2足移動体の荷重を検出する荷重センサ(36)を備えると共に、前記リフタ移動手段制御手段は、前記検出された荷重が所定荷重(α)以上であるとき、前記リフタを上方に移動させるように前記リフタ移動手段の動作を制御する(S30、S38、S222、S230)如く構成した。
それにより、2足移動体が姿勢を崩して転倒しかけた場合、その荷重を支持するときの衝撃を一層緩和させることができる。
また、第3実施例にあっては、所定の歩行計画に従って歩行する2足移動ロボット(12)にロープ状の接続部材(ハンガベルト18)を介して接続されると共に、前記2足移動ロボットが姿勢を崩したときに前記2足移動ロボットを上方から支持するリフタ(14)からなる2足移動ロボットの補助装置において、前記リフタを3次元空間において移動自在なリフタ移動手段(リフタZ軸ガイドフレーム20、Z軸電動モータ22、リフタY軸ガイドフレーム24、Y軸電動モータ26、リフタX軸ガイドフレーム28、X軸電動モータ30)と、前記リフタ移動手段の動作を制御するリフタ移動手段制御手段(リフタ制御部38)とを備えると共に、前記リフタ移動手段制御手段は、前記所定の歩行計画に基づいて前記リフタが前記2足移動ロボットに追従するように前記リフタ移動手段の動作を制御する(S402、S406)如く構成した。
それにより、ロープ状の接続部材(ハンガベルト18)の緩み量を少なくでき、よって2足移動ロボット12が姿勢を崩して転倒しかけた場合、その荷重を支持するときの衝撃を緩和させることができる。また、2足移動ロボット12の歩行計画に基づいてリフタ14の動作を制御するので、2足移動ロボット12の位置を検出することが不要となり、簡略な構成ながら、上記の作用効果を得ることができる。
また、前記2足移動ロボットが歩行自在なベルト(エンドレスベルト42a)と、前記ベルトを駆動させるベルト駆動手段(電動モータ42b)と、前記ベルト駆動手段の動作を制御するベルト駆動手段制御手段(ベルト制御部42c)とを備えると共に、前記ベルト駆動手段制御手段は、前記所定の歩行計画に基づいて前記2足移動ロボットが前記3次元空間の所定範囲に位置するように前記ベルト駆動手段の動作を制御する(S304、S308)如く構成した。
それにより、2足移動ロボット12を追従するためのリフタ移動量を減少させることができ、よってリフタ移動手段が大型化するのを防止することができる。また、2足移動ロボット12を補助するに要するスペースを抑制することができる。
尚、上記の第1実施例および第2実施例にあっては、2足移動体として2足移動ロボットを例に挙げて説明したが、人であっても良い。即ち、この発明の第1実施例および第2実施例に係る2足移動体の補助装置は、人の歩行リハビリテーションに活用することができる。