(57)【要約】【課題】  歩容データに従って動作しているロボットに対し、リアルタイムにロボット腕部等の目標位置が与えられても簡単な制御ロジックで安定した動作を可能とするロボットを提供する。【解決手段】  ロボット動作中にロボット腕部の目標位置がロボット操作装置１３０からリアルタイムに出力される。このとき体幹リンクと脚リンクを目標位置に、腕部を標準位置にしたときのロボットの重心位置を算出する。同時に体幹リンクと脚リンクを目標位置に、腕部をロボット操作装置１３０から出力される目標位置にしたときのロボットの重心位置を算出する。両者重心位置の水平面内偏差を算出する。ロボット体幹リンクの目標位置をこの偏差により補正する。これによりロボットの重心位置は、腕部が標準位置にあったときの重心位置に近いものとなり安定性が確保される。この制御はロボットのＺＭＰを考慮する必要がないので簡単な制御ロジックで実現できる。【選択図】  図１
2007007797.tif 000002
【特許請求の範囲】
【請求項１】
体幹リンクに脚リンクと上方リンクが揺動可能に連結されている歩行ロボットであり、
体幹リンクと脚リンクの目標位置の時系列データを記憶している記憶手段と、
ロボット動作中に上方リンクの位置を取得する上方リンク位置取得手段と、
体幹リンクと脚リンクを前記記憶手段に記憶されている目標位置におくとともに上方リンクを標準位置においたときのロボットの重心位置と、体幹リンクと脚リンクを前記記憶手段に記憶されている目標位置におくとともに上方リンクを前記上方リンク位置取得手段で取得した位置においたときのロボットの重心位置との水平面内偏差を計算する水平面内偏差計算手段と、
前記記憶手段に記憶されている体幹リンクの目標位置の水平面内位置を、前記水平面内偏差だけ補正する補正手段と、
体幹リンクの目標位置の補正されたデータと、前記記憶手段に記憶されている脚リンクの目標位置のデータによって、体幹リンクと脚リンクの相対姿勢を制御する制御手段とを備えており、
前記記憶手段に記憶されている体幹リンクと脚リンクの目標位置の時系列データは、上方リンクを標準位置においたときにロボットのバランスが確保される関係に設定されていることを特徴とする歩行ロボット。
【請求項２】
上方リンクの前記標準位置は、体幹リンクに対して固定されていることを特徴とする請求項１の歩行ロボット。
【請求項３】
上方リンクの動作を指定するために用いる入力手段を備えており、
前記上方リンク位置取得手段は、前記入力手段から入力した上方リンクの目標位置を取得することを特徴とする請求項１又は２の歩行ロボット。
【請求項４】
体幹リンクと上方リンク間の関節角を計測する関節角計測手段を備えており、
前記上方リンク位置取得手段は、前記関節角計測手段の計測値から上方リンクの位置を取得することを特徴とする請求項１又は２の歩行ロボット。
【請求項５】
上方リンクで物体を把持する動作を指定するために用いる入力手段を備えており、
前記水平面内偏差計算手段は、体幹リンクと脚リンクを前記記憶手段に記憶されている目標位置におくとともに上方リンクを標準位置においたときのロボットの重心位置と、体幹リンクと脚リンクを前記記憶手段に記憶されている目標位置におくとともに上方リンクを前記上方リンク位置取得手段で取得した位置において物体を把持したときのロボットの重心位置との水平面内偏差を計算することを特徴とする請求項１から４のいずれかの歩行ロボット。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、体幹リンクに脚リンクと上方リンクが揺動可能に連結されている歩行ロボットに関する。
【背景技術】
【０００２】
体幹リンクに脚リンクが揺動可能に連結されており、体幹リンクに対する脚リンクの位置（姿勢）を変化させることによって歩行するロボットが開発されている。
体幹リンクに上肢リンクや頭部リンクといった上方リンクが揺動可能に連結されており、体幹リンクに対する上方リンクの位置（姿勢）を変化させることによって、上肢を振ったり首リンクを傾けたりするロボットが開発されている。ここでいう上方リンクは、体幹リンクに揺動可能に連結されているリンクであり、歩行の際に接地しないリンクをいう。
体幹リンクに脚リンクと上方リンクが揺動可能に連結されているロボットを動作させるには、体幹リンクと脚リンクと上方リンクの目標位置を時系列的に記述するデータを必要とする。なお「上方リンクの目標位置」とは上方リンクを構成する各リンクの、体幹リンクに対する位置と姿勢を決定できる位置の目標値ことである。例えば手先の位置を決めると腕の各リンクの姿勢が決定できる。このとき手先の位置が上方リンクの目標位置となる。
歩行ロボットの場合、ＺＭＰ（ゼロモーメントポイント）が接地脚リンクの足平内（両脚で接地している場合には両足平の外周と、両足平の外周に接する２本の共通接線で形成される領域内）に存在しなければならないという条件が必要とされる。体幹リンクと脚リンクと上方リンクの目標位置を時系列的に記述するデータは、そのデータに従ってロボットが動作したときに得られるＺＭＰが、接地脚リンクの足平内に存在するという関係を満たすものになっていなければならない。そうでないとロボットのバランスが確保されず、ロボットが転倒する可能性がある。
【０００３】
特許文献１に、脚リンクと上方リンクの目標位置を時系列的に記述するデータが与えられたときに、ＺＭＰが接地脚リンクの足平内に存在するという関係を満たすことができる体幹リンクの目標位置の時系列データを計算する技術が開示されている。この技術では、体幹リンクの目標位置を仮定し、脚リンクと上方リンクを与えられた目標位置に一致させる動作を実現するときのＺＭＰを計算し、計算されたＺＭＰが接地脚リンクの足平内に存在するという関係を満たす体幹リンクの目標位置を探索する。探索過程では、体幹リンクの目標位置を修正してからＺＭＰを計算し、計算されるＺＭＰが接地脚リンクの足平内からはみ出す場合には、体幹リンクの目標位置をさらに修正するという計算を繰返す。
【０００４】
【特許文献１】特開２００３−１１７８５８号公報（段落００３２、図６、図７）
【発明の開示】
【発明が解決しようとする課題】
【０００５】
特許文献１の技術は、上方リンクの運動がＺＭＰに与える影響を加味することから、ロボットを実際に運動させたときのＺＭＰが接地脚リンクの足平内に存在することが高い信頼性で保証されているが、上方リンクの運動まで加味して計算することから必要な計算量が多く、体幹リンクの目標位置を計算するのに時間を要するという問題を備えている。
ロボットを人間にフレンドリーな機械とするために、ジョイスティックなどの入力装置を利用して、上方リンクの運動をリアルタイムで指示したいという要求がある。あるいはロボットに物体位置認識センサを搭載し、「物体を探し、掴み、持ち上げる」動作をロボットに自律的に実行させたいという要求も存在する。
これらの場合、上方リンクの運動を加味して体幹リンクの目標位置を計算するのに時間を要するというのは不利であり、ロボットの動作が緩慢なものにしかできない。
上方リンクの予期せぬ運動（ロボットの動作開始前にはプログラムされていない運動）に対応して、体幹リンクの目標位置を高速に計算する技術が待望されている。
【課題を解決するための手段】
【０００６】
本発明は、上方リンクの予期せぬ運動に対応する体幹リンクの目標位置を高速に計算するために、下記の構成を採用する。
（１）脚リンクの目標位置の時系列データを特定することによって、ロボットの脚の運びを事前に特定する。
（２）その脚の運びから、ＺＭＰが接地脚リンクの足平内に存在するという条件を満たす体幹リンクの目標位置の時系列データを計算する。このとき本来は、上方リンクの目標位置の時系列データを特定しなければ、ＺＭＰが接地脚リンクの足平内に存在するか否かが不明である、しかしながら本発明では、上方リンクは標準位置にあると仮定する。
ここで仮定する上方リンクの標準位置は、体幹リンクに対して固定されているとは限らない。標準位置が時系列的に変化するものであってよい。時系列的に変化するものであっても、事前にわかっているものであればよい。上方リンクの通常の運動を、時系列的に変化する標準位置とすることができる。
（３）上方リンクの実際の位置を取得する。この段階では、上方リンクの運動を指定する入力手段から入力される目標位置を採用してもよいし、実際の位置を計測してもよい。ロボットが自律的に上方リンクを揺動させる場合は、実際の位置を計測する方式が適している。ロボットは、上方リンクの位置を制御装置が出力する目標位置に一致させることから、制御装置が出力する目標位置を実際位置と扱ってもよい。
（４）上方リンクが標準位置（その標準位置が時系列的に変化してもよい）におかれていると仮定すれば、（１）で特定されている脚リンクの目標位置の時系列データを併用することによって、ロボットのバランスを確保するのに必要な体幹リンクの目標位置の時系列データを計算することができる。ここでは、特許文献１に記載の技術等を活用する。
（５）計算して得られる体幹リンクの目標位置の時系列データは、脚リンクを（１）で特定されている目標位置の時系列データに基づいて移動させ、上方リンクを標準位置（その標準位置が時系列的に変化してもよい）においたときに、ロボットのバランスを確保する関係を満たしている。
標準位置が時系列的に変化する場合には、ロボットがバランスを確保する関係を満たす幹リンクの目標位置の時系列データを計算するのに複雑な計算を必要とするが、必要ならオフラインで事前に計算することができるので、この段階で計算量が大きいことは深刻な問題とならない。標準位置が固定されていれば、上方リンクと体幹リンクを合わせてひとつの剛体とみなせるので計算量は少なくてすむ。
（６）本発明では、（５）で計算した体幹リンクと脚リンクの目標位置の時系列データを記憶手段に記憶しておく。
（７）本発明では、上方リンクが標準位置（その標準位置が時系列的に変化してもよい）から外れて揺動することを許容する。例えば、ジョイスティク等を利用して、人間が上方リンクの位置を指定することを許容する。あるいは、物体位置認識センサを搭載したロボットが、物体を掴むために、上方リンクを自律的に揺動させることもある。
（８）上方リンクが標準位置（その標準位置が時系列的に変化してもよい）におかれているときのロボットの重心位置と、上方リンクが標準位置から離脱したときのロボットの重心位置は異なる。そこで本発明では、両者の水平面内偏差を計算する。
（９）ロボットを実際に動作させる場合には、前記（５）で求めて前記（６）で記憶しておいた体幹リンクの目標位置を、前記（８）で求めた水平面内偏差で修正した体幹リンクの目標位置を採用する。
修正した体幹リンクの目標位置を採用することで、上方リンクが標準位置から離脱したときのロボットの重心位置を、バランスが確保されている上方リンクが標準位置におかれているときのロボットの重心位置に近づけることができる。これによって上方リンクが標準位置から離脱した場合であってもロボットのバランスを確保する。
上記の技術では、（７）から（９）の計算量が少なくてすみ、上方リンクの実際の運動に対応する体幹リンクの目標位置を高速に計算することができる。上方リンクの運動をリアルタイムで指示する場合にも無理なく対応することができ、ロボットを機敏に動作させることができる。
本発明は、ロボッットに実際に動作させながら前記（７）の処理を実施する場合に有効であるが、ロボットを実際に動作させるのに先立って前記（７）の処理を実施する場合にも有効である。特に上方リンクの標準位置が固定されている場合には、ロボットを実際に動作させるのに先立って前記（７）の処理を実施することが実際的な意味を持つ。上方リンクの標準位置が固定されていると、前記（５）の計算量が少なくてすむ。前記（７）の処理を実施すると、（８）と（９）が実施可能となる。この場合、前記（７）の上方リンクの動作に対してバランスする体幹リンクの目標位置を少ない計算量で計算することができる。体幹リンクの目標位置を事前に計算する場合でも、計算量を少なくできることは有意義である。
上記の技術では、上方リンクの運動に起因する慣性を計算しない。静的な重心位置を計算するに過ぎない。計算量は少なくてすむ。慣性を考慮しないので正確ではないが、実際にロボットを動作させるとロボットは十分に安定し、無理のない動作が得られる。
体幹リンクの目標位置の修正に対応してロボットが姿勢を変える態様には各種が存在する。脚リンクに存在する膝関節の関節角を修正する態様であってもよいし、体幹リンクの傾斜角を修正する態様であってもよい。
【０００７】
本発明の歩行ロボットは、体幹リンクに脚リンクと上方リンクが揺動可能に連結されており、体幹リンクに対する脚リンクの位置（姿勢）を変えることによって歩行し、体幹リンクに対する上方リンクの位置（姿勢）を変えることによって上肢を振ったり首リンクを傾げたりする動作を実行する。
本発明の歩行ロボットは、体幹リンクと脚リンクの目標位置の時系列データを記憶している記憶手段と、ロボット動作中に上方リンクの位置を取得する上方リンク位置取得手段を備えている。本発明の歩行ロボットは、さらに、体幹リンクと脚リンクを記憶手段に記憶されている目標位置におくとともに上方リンクを標準位置においたときのロボットの重心位置と、体幹リンクと脚リンクを記憶手段に記憶されている目標位置におくとともに上方リンクを上方リンク取得手段で取得した位置においたときのロボットの重心位置との水平面内偏差を計算する水平面内偏差計算手段と、記憶手段に記憶されている体幹リンクの目標位置の水平面内位置を水平面内偏差計算手段で計算された水平面内偏差だけ補正する補正手段と、体幹リンクの目標位置の補正されたデータと、記憶手段に記憶されている脚リンクの目標位置のデータによって、体幹リンクと脚リンクの相対姿勢を制御する制御手段を備えている。
記憶手段に記憶されている体幹リンクと脚リンクの目標位置の時系列データは、上方リンクを標準位置においたときにロボットのバランスが確保される関係に設定されている。
【０００８】
本発明の歩行ロボットは、体幹リンクと脚リンクの目標位置の時系列データを記憶している。体幹リンクと脚リンクを記憶されている目標位置に一致させれば、歩行ロボットは歩行する。このとき、上方リンクが標準位置におかれている限り、ＺＭＰが接地脚リンクの足平内に存在するという条件を満たす時系列データが記憶されている。
ロボットが実際に動作する場合には上方リンクが標準位置から離脱する。この結果、上方リンクが標準位置におかれているときのロボットの重心位置と、上方リンクが標準位置から離脱したときのロボットの重心位置は変化し、それに対策しなければ上方リンクが標準位置から離脱したことによってロボットのバランスが崩れることがありえる。そこで、本発明の歩行ロボットは、上方リンクが標準位置から離脱したことによって生じる重心位置の水平面内の移動量（水平面内偏差）を計算する。
本発明の歩行ロボットは、上方リンクを標準位置から離脱させたことによってロボットのバランスが崩れることに対策するために、水平面内偏差分だけ体幹リンクの目標位置を補正する。
本技術を実際に検証してみると、上方リンクを標準位置から離脱するように揺動させてもロボットのバランスが崩れることが防止される。しかも、上方リンクの位置（姿勢）が決まってから、体幹リンクの目標位置を補正するまでの計算量が少なくてすみ、極めて短時間のうちに体幹リンクの目標位置の補正値を計算できることが確認された。上方リンクの位置（姿勢）をリアルタイムで指示する場合でも、あるいはロボットが自律的に上方リンクを揺動させる場合でも、ロボットは機敏に動作することができる。
【０００９】
上方リンクの標準位置は、体幹リンクに対して時系列的に変化するものであってもよいが、体幹リンクに対して固定されていることが好ましい。
標準位置が固定されていれば、ロボットのバランスを確保するのに必要な体幹リンクの目標位置の時系列データを簡単に計算することができる。ロボットの動作時には、上方リンクが標準位置から離脱するが、標準位置から離脱すれば、それに応じて体幹リンクの目標位置が補正されるので、体幹リンクの目標位置を事前に計算しておく段階では、上方リンクを体幹リンクに対して固定しておいても問題はない。
【００１０】
上方リンクの動作を指定するために用いる入力手段を備えていることが好ましい。この場合、上方リンク位置取得手段は、この入力手段から入力した上方リンクの目標位置を取得するものであればよい。上方リンクの目標位置が指定されれば、ロボットの制御装置が指定された目標位置に上方リンクを位置することから、目標位置と実際位置が略等しいとしてよいからである。
本発明は、ロボットを動作させながら上方リンクの動作を指定する場合に極めて有効であり、リアルタイムで上方リンクの動作を指定しながらロボットを機敏に動作させることができる。
上方リンクの動作を指定するために用いる入力手段には特に制約がなく、ジョイスティック、カーソル、ボタン、マウスなどの入力装置を利用することができる。
【００１１】
上方リンクの位置を取得するために、体幹リンクと上方リンク間の関節角を計測する関節角計測手段を備え、この関節角計測手段の計測値から上方リンクの位置を取得するようにしてもよい。この場合、上方リンクの実際位置を比較的簡単に取得することができる。ここで「体幹リンクと上方リンク間の関節角」とは、上方リンクを構成する複数のリンクについて、隣接するリンクを連結する関節の角度、および上方リンクのうちのいずれかのリンクと体幹リンクを連結する関節の角度、のことをいう。
上方リンクは、上記した入力手段から入力される指令にもとづいて位置を変えるものであってもよい。あるいは、ロボット自身がカメラと画像処理装置を備え、物体の位置を認識し、その物体に向けて上方リンクを移動させるようにプログラムされていてもよい。ロボットが自律的に上方リンクを揺動させるようにプログラムされている場合には、上方リンクの位置を計測する方式が有利である。
【００１２】
ロボットが上方リンクで物体を把持する動作を実行することがある。この場合、上方リンクで物体を把持する動作を指定するために用いる入力手段を備えており、水平面内偏差計算手段は、体幹リンクと脚リンクを記憶手段に記憶されている目標位置におくとともに上方リンクを標準位置においたときのロボットの重心位置と、体幹リンクと脚リンクを記憶手段に記憶されている目標位置におくとともに上方リンクを上方リンク位置取得手段で取得した位置において物体を把持したときのロボットの重心位置との水平面内偏差を計算する。
【００１３】
ロボットが上方リンクを用いて物体を持ち上げると、上方リンクの姿勢の変化と物体の質量によってロボットの重心位置が変化する。この重心位置の変化を考慮して体幹リンクの目標位置を補正すると、ロボットが物体を持ちあげる際にもバランスを崩すことがないロボットを実現できる。
【発明の効果】
【００１４】
本願発明によれば、上方リンクを揺動させながら歩行するロボットが上方リンクを揺動させてもバランスを崩すことがない体幹目標位置を少ない計算量で高速に計算することが可能となる。上方リンクの位置（姿勢）をリアルタイムで指定する場合でも、あるいはロボットが自律的に上方リンクを揺動させる場合でも、ロボットがバランスを崩すことがない体幹目標位置を高速に計算することができるために、ロボットを機敏に動作させることができる。
【発明を実施するための最良の形態】
【００１５】
実施例の主要な特徴を列記する。
（第１形態）体幹リンクと腰リンクに分離されており、腰リンクに対する体幹リンクの揺動角を変化することによって、上肢リンクの揺動に起因する重心位置の変化を相殺する。
【実施例】
【００１６】
図面を参照して実施例を詳細に説明する。
＜実施例１＞
図１、図２および図５、図６を用いて本発明の実施例１を説明する。
まず図５（Ａ）に脚式ロボットのリンク構成の概要を説明する。図５（Ａ）はロボット１００本体の正面図である。ロボットは複数のリンクと、隣接するリンクを相互に揺動可能に連結する関節から構成される。このロボット１００は、人間の胴体に相当する体幹リンク４０、腰に相当する腰リンク４２、右腕部４４Ｒ、左腕部４４Ｌ、右脚部４６Ｒ、左脚部４６Ｌそして頭部リンク４８で構成される。右腕部４４Ｒ、左腕部４４Ｌ、右脚部４６Ｒ、左脚部４４Ｌは夫々複数のリンクから構成されている。右脚部４６Ｒの先端に取り付けられているリンクが右足平リンク５２Ｒである。左脚部４６Ｌの先端には左足平リンク５２Ｌが取り付けられている。ここで右腕部４４Ｒ、左腕部４４Ｌおよび頭部リンク４８を合わせたものが上方リンクである。即ち「上方リンク」には複数のリンクが含まれる。そこで「上方リンク」に含まれる隣接するリンクを連結する関節、および右腕部４４Ｒと左腕部４４Ｌとを体幹リンクに連結する関節、頭部リンク４８と体幹リンクを連結する関節を合わせて「上方リンクの各関節」と呼ぶことにする。また体幹リンク４０と右腕部４４Ｒは肩関節５０で揺動可能に連結されている。図４（Ａ）では肩関節５０で図示した円と同じ円は全て関節を表す。但し、ひとつの円で複数の関節を表している場合もある。
【００１７】
図１は本発明によるロボット制御装置１０ａとロボット本体１００のブロック図である。まずロボット制御装置１０ａとロボット本体１００が有するモジュール（決められた処理を行う処理部、または特定のデータを記憶する記憶部）の構成を説明し、その後各モジュールの詳細を説明する。
このロボット制御装置１０ａは体幹リンクと脚リンクの目標位置の時系列データを含む歩容データを記憶する歩容データ記憶部２０を備える。また歩容データ記憶部２０から体幹リンクと脚リンクの目標位置の時系列データを読み出し、この目標位置を脚部に備えられた各関節の角度に変換する逆変換部２２ａを備える。逆変換部２２ａから出力される脚部の各関節の角度を用い、上方リンクを標準位置（「標準位置」については後述する）においたときのロボットの重心位置を算出する標準重心位置算出部２４を備える。また、歩容データ記憶部２０から体幹リンクと脚リンクの目標位置の時系列データを読み出すと同時に、ロボット操作装置１３０から出力されるロボットの上方リンクの目標位置を取得してロボットに備えられた全ての関節の角度を出力する逆変換部２２ｂを備える。逆変換部２２ｂから出力されたロボットの全ての関節の角度からロボットの重心位置を算出する実重心位置算出部２６を備える。
ロボット制御装置１０ａはまた、標準重心位置算出部２４で算出された重心位置と実重心位置算出部２６で算出された重心位置との水平面内の偏差を求める偏差算出部３２を備える。この水平面内の偏差が体幹リンク目標位置に対する補正量のベースとなる。さらに歩容データ記憶部２０から読み出された体幹リンクと脚リンクの目標位置の時系列データと、偏差算出部３２で算出された水平面内偏差（すなわち体幹リンク目標位置の水平面内の補正量）と、ロボット操作装置１３０から出力されるロボット上方リンクの目標位置とを加算する加算部３０と、加算された目標位置をロボットの各関節の目標関節角に変換する逆変換部２２ｃ、および変換された目標関節角にロボットの各関節の角度が追従するように各関節に備えられたアクチュエータ１１０を制御するアクチュエータ駆動部２８を備える。
【００１８】
ロボット本体１００には、各関節を駆動するアクチュエータ１１０と、ロボットの手先に取り付けられた力センサ１２０を備える。なお、力センサ１２０は本実施例１では利用せず、後に実施例４で利用するのでここでは説明を省略する。
またロボット操作装置１３０は、ロボットの上方リンクの動作を指定する装置である。例えばロボットの手先を希望する位置に動かしたり、頭部リンクを左右に振らせたりすることができる。そのような動作をさせるために、ロボット操作装置１３０はロボットの手先や頭部リンクに対して目標位置を出力する。ロボット操作装置１３０の具体例としては、ジョイスティックやボタン、マウスなどで構成される。
【００１９】
次に各モジュールについて詳細を説明する。
歩容データ記憶部２０に記憶されている歩容データとは、体幹リンクの目標位置の時系列データ、両脚リンク（脚リンクは複数のリンクで構成されているが、以下単に脚リンクと称す）の目標位置の時系列データを合わせたデータをいう。両脚先端の足平リンクに代表点が設定されており、脚リンクの目標位置とはこの足平リンク代表点の目標位置である。なお、「体幹リンクの目標位置」には、体幹リンクの所定の位置に設定された代表点の３次元空間での位置と体幹リンクの姿勢角とが含まれる。「脚リンクの目標位置」には足平リンク代表点の３次元空間での位置と足平リンクの姿勢角とが含まれる。また歩容データには上方リンクの目標位置の時系列データが含まれていてもよい。上方リンクの目標位置とは両手先リンクに設定された代表点の目標位置および頭部リンクに設定された代表点の目標位置である。ここで「目標位置」には上記と同様に３次元空間の位置と、代表点が設定されているリンクの姿勢角とを含む。歩容データ記憶部２０に記憶された歩容データは、ロボットを制御する前に作成されている。
【００２０】
歩容データは、ロボットに歩行動作を行わせる際に、ＺＭＰが接地脚の足平内（両脚で接地している場合には両足平の外周と、両足平の外周に接する２本の共通接線で形成される領域内）に存在するという条件を満足するように（即ちロボットのバランスが確保されるように）、体幹リンク、脚リンク及び上方リンクの目標位置の時系列データが作成される。このとき、上方リンクが所定の動作を行うように上方リンクの目標位置が時々刻々と変化する時系列データを作成しても良いし、体幹リンクに対する上方リンクの相対位置を固定し、体幹リンクと脚リンクの目標位置の時系列データを作成しても良い。ロボットを実際に動作させる際に上方リンクの目標位置を与えることを前提とするからである。ここで、上方リンクの時系列データを作成する場合には歩容データには体幹リンク、脚リンク及び上方リンクの目標位置の時系列データが含まれる。体幹リンクに対する上方リンクの相対位置を固定して体幹リンクと脚リンクの目標位置の時系列データを作成した場合には歩容データには体幹リンクと脚リンクの目標位置の時系列データが含まれ、上方リンクの目標位置の時系列データは含まなくともよい。ここで、上方リンクの目標位置の時系列データを作成した場合の上方リンクの目標位置又は体幹リンクに対して上方リンクの相対位置を固定した場合の上方リンクの位置を「上方リンクの標準位置」と呼ぶことにする。
なお、歩容データには上方リンクを標準位置としたときのロボットの重心位置の時系列データを含むことも好適である。歩容データに上方リンクを標準位置としたときのロボットの重心位置の時系列データを含む場合、図１のブロック図において、逆変換部２２ａ、標準重心位置算出部２４は不要となりロボット制御装置１０ａが簡略化できるからである。この場合、歩容データ記憶部２０から上方リンクを標準位置としたときのロボットの重心位置の時系列データを直接偏差算出部３２へ送るように構成する。
【００２１】
逆変換部２２ａ、２２ｂ、２２ｃは、体幹リンク目標位置、脚リンク目標位置および上方リンク目標位置をロボットの各関節の角度へ変換するモジュールである。図１では３つの逆変換部が記載されているが、これはロボット制御装置１０ａの構造を理解しやすくするために便宜上分けて記載したものであって、実際には１つのモジュールでそれぞれの逆変換を行うように構成しても良い。逆変換部がサブルーチンプログラムとして構成されている場合は、必要に応じて、入力するロボット代表点の値を指定して逆変換部サブルーチンを呼び出すように構成することも好適である。なお、上方リンクの位置については体幹リンクに固定されているものとして歩容データが作成された場合、歩容データには上方リンクの目標位置は含まれない場合がある。その場合は逆変換部２２ａ内に、歩容データ作成時の体幹リンクに固定された上方リンクの位置が記憶されている。逆変換部２２ａはこの記憶された上方リンクの位置を用いて上方リンクの各関節の角度を算出する。さらに、上方リンクの位置は体幹リンクに固定されているものとして歩容データが作成された場合、上方リンクの各関節の角度は一定である。逆変換部２２ａはこの上方リンクの各関節の角度を記憶しておくことも好ましい。上方リンクの各関節の角度を記憶していれば、上方リンクについてはその標準位置から各関節への逆変換計算を不要とすることができるからである。
【００２２】
標準重心位置算出部２４には、歩容データから逆変換部２２ａによって変換された各関節の角度が入力される。各関節の角度がわかれば各関節で連結されている各リンク相互の相対位置関係も算出できる。そして標準重心算出部２４には各リンクの質量が記憶されており各リンク相互の位置関係および各リンクの質量からロボット全体の、歩容データに基づく重心位置を算出する。すなわち体幹リンクと脚リンクを目標位置におくとともに上方リンクを標準位置においたときのロボットの重心位置を算出する。ここで、上方のリンクが体幹リンクに固定されているものとして歩容データが作成されている場合、上方リンクと体幹リンクは一つの剛体とみなせる。標準重心算出部２４は、上方リンクと体幹リンクを合わせて一つの剛体とみなした場合のその剛体の重心位置を記憶しておくことも好ましい。これにより体幹リンクと脚リンクを目標位置におくとともに上方リンクを標準位置においたときのロボットの重心位置の算出を簡略化することができる。
【００２３】
逆変換部２２ｂは、ロボット操作装置１３０から取得したロボットの上方のリンクの目標位置と、歩容データ記憶部２０から読み出した体幹リンクと脚リンクの目標位置のデータ（歩容データ）からロボットの各関節の角度に逆変換する。
実重心位置算出部２６は、逆変換部２２ｂにより逆変換されたロボットの各関節の角度に基づいてロボットの重心位置を算出する。ロボット操作装置１３０から上方リンクの目標位置が取得されている場合、上方リンクの目標位置は標準位置とは異なるものとなる。従って標準重心位置算出部２４で算出されたロボットの重心位置と、実重心位置算出部２６で算出されたロボットの重心位置とは異なるものとなる。
【００２４】
偏差算出部３２は、標準重心位置算出部２４で算出された上方リンクが標準位置にあるときのロボットの重心位置と、実重心位置算出部２６で算出された上方リンクがその目標位置にあるときのロボットの重心位置との水平面内の差（水平面内偏差）を求める。具体的には両者のロボット重心位置の座標値を水平面に投射する。そして上方リンクが標準位置にあるときのロボットの重心位置の水平面内座標値から、上方リンクがロボット操作装置１３０から取得した目標位置にあるときのロボットの重心位置の水平面内座標値を減算する。これにより、上方リンクがその目標位置にあるときのロボットの重心位置から上方リンクが標準位置にあるときのロボットの重心位置へ向けた水平面内のベクトルが算出される。このベクトルが水平面内偏差となる。またこのベクトル量が体幹リンク目標位置の補正量ともなる
【００２５】
加算部３０では、歩容データ記憶部２０から読み出した歩容データのうち、体幹リンク目標位置の水平方向座標値に偏差算出部３２で算出したベクトル（水平面内偏差、即ち補正量）を加算する。即ち体幹リンク目標位置を水平方向に前記ベクトル分だけ補正する。同時に加算部３０では、ロボット操作装置１３０から取得した上方リンクの目標位置を歩容データ記憶部２０から読み出した歩容データに加える。
こうして歩容データ記憶装置２０から読み出した歩容データに、補正量と上方リンクの目標位置が加えられた後の歩容データがロボット本体１００に与える最終的な歩容データとなる。
【００２６】
逆変換部２２ｃでは、上述した最終的な歩容データ、すなわち各代表点の目標位置をロボット本体１００の各関節の目標関節角に変換する。各関節の目標関節角は、アクチュエータ駆動部２８に送られる。アクチュエータ駆動部２８はロボット本体１００の各関節の角度を目標関節角に追従させるよう各関節に備えられたアクチュエータ１１０を駆動する。以上の処理を歩容データ内の各代表点の目標位置の時系列データの各時点ごとに繰り返す。こうしてロボット本体１００は、歩容データ記憶装置２０から読み出した歩容データに補正量と上方リンクの目標位置が加えられた後の歩容データに追従するように動作する。
なお上述した各モジュールはハードウエアで構成してもよいし、ソフトウエアモジュールとして構成しコンピュータにより処理を実行するよう構成してもよい。また歩行制御装置１０ａの一部あるいは全部のモジュールはロボット本体１００に内蔵されていても良い。
【００２７】
次に図２のフローチャート図により処理の流れを説明する。
まずステップＳ１００で歩容データ記憶部２０から歩容データを読み込む。同時にステップＳ１０２でロボット操作装置１３０が出力する上方リンクの目標位置を取得する。読み込まれた歩容データはステップＳ１０４で各関節の角度に逆変換される。前述したように、歩容データ内に上方リンクの目標位置が含まれる場合はその目標位置を用いて上方リンクの各関節の角度を求める。歩容データ内に上方リンクの目標位置が含まれていない場合は、上方リンクの各関節の角度は夫々一定角度であるとして歩容データが作成されているのでその一定角度を記憶しておけば、上方リンクについては逆変換する必要はない。
ステップＳ１０８では、各関節の角度から上方リンクが標準位置にあるときのロボット重心位置が算出される。
一方ステップＳ１０６では、ステップＳ１０２で取得した上方リンクの目標位置と歩容データ内の各代表点の目標位置とから、ロボットの各関節の角度に逆変換される。
ステップＳ１１０では、ステップＳ１０６で逆変換された各関節の角度に基づいてロボットの上方リンクが目標位置にあるときのロボットの重心位置が算出される。
ステップＳ１１２では、ステップＳ１０８で算出された重心位置と、ステップＳ１１０で算出された重心位置との水平面内偏差が算出される。
ステップＳ１１４では、歩容データ記憶部２０に記憶された歩容データに、算出された水平面内偏差と上方リンク目標位置とを加え、最終的にロボットに与える歩容データが完成する。
このステップＳ１１４で完成した歩容データはステップＳ１１６でロボットの各関節の目標角に逆変換される。ステップＳ１１８ではこの目標角に従って各関節のアクチュエータが駆動される。その結果ロボットが動作する。以上の処理をサンプリング毎に繰り返す。
【００２８】
実施例１によれば、ロボット動作中に、リアルタイムで上方リンクの目標位置が与えられた際、上方リンクがその目標位置にあるときのロボットの重心位置と上方リンクが標準位置にあるときのロボットの重心位置が算出される。両重心位置の水平面内偏差を算出してその偏差分、ロボット体幹リンクの水平面内の目標位置を補正する。これにより上方リンクがその目標位置にあるときのロボットの重心位置は、上方リンクが標準位置にあるときのロボットの重心位置に水平面内で一致する。ここに「一致する」とは厳密な一致でなくとも誤差を含むものであってよい。上方リンクがその目標位置にあるときのロボットの重心位置の算出には実際のロボットの各リンクの位置ではなく目標値を用いているため、誤差を含むからである。
上方リンクが標準位置にあるときのロボットの重心位置は、ロボットのバランスが確保されるようにその歩容データが作成されたものである。従って上方リンクの目標位置がリアルタイムに与えられた場合でも、ロボット全体の重心位置は、上方リンクが標準位置にあるときにバランスが確保されたロボットの重心位置に略一致する。
ロボットの歩容データは、多少の外乱に対しても転倒しないよう余裕を持たせて作成してある。よってＺＭＰに大きく影響するロボット重心位置を上方リンクが標準位置にあるときのロボット重心位置に略一致するよう体幹リンクの目標位置を補正するという簡単な制御により、ロボット動作中に上方リンクの目標位置がリアルタイムに与えられてもロボットのバランスを確保することができる。
【００２９】
また実施例１では、ロボット操作装置１３０から取得したロボットの上方のリンクの目標位置と歩容データとからロボットの重心位置を算出する。このロボットの重心位置から体幹リンクの目標位置の補正量が設定される。設定された補正量は、重心位置を算出した際に用いたロボットの上方のリンクの目標位置とともに、歩容データに加えられる。補正量と上方リンクの目標位置が加えられた歩容データからロボットの各関節の目標関節角度が算出され、ロボットが動作する。
換言すれば所定時点の歩容データと上方リンク目標位置から、この歩容データの補正量が求められ、補正量はその所定時点の歩容データに加えられてロボットへの指令値となる。すなわち補正量がフィードフォワード的に付加される制御となっている。
実施例１では、ロボット上方リンクの目標位置がリアルタイムに与えられた際、ロボットがバランスを失わないような制御をフィードフォワード制御により実現できる。従ってより安定した制御とすることが可能となる。
【００３０】
なお実施例１において、歩容データ記憶部２０が「記憶手段」の一態様に相当する。また逆変換部２２ｂおよび図２のステップＳ１０２が「上方リンク位置取得手段」の一態様に相当する。標準重心位置算出部２４と実重心位置算出部２６および偏差算出部３２が「水平面内偏差計算手段」の一態様に相当する。図２のフローチャート図においてはステップＳ１０８とステップＳ１１０およびステップＳ１１２が「水平面内偏差計算手段」の一態様に相当する。また加算部３０および図２のステップＳ１１４が「補正手段」の一態様に相当する。アクチュエータ駆動部２８とアクチュエータ１１０および図２のステップＳ１１８が「制御手段」の一態様に相当する。さらにロボット操作装置１３０が「入力手段」の一態様に相当する。
【００３１】
＜実施例２＞
次に図３及び図４を用いて実施例２を説明する。図３は実施例２のロボット制御装置１０ｂとロボット本体１００のブロック図である。
実施例２のロボット制御装置１０ｂでは、ロボット本体１００の各関節に取り付けられたエンコーダ１４０の出力値を実重心位置算出部２６にフィードバックする。実重心位置算出部２６では、フィードバックされた値に基づいて、上方リンクが目標位置にあるときのロボットの重心位置を算出する。実施例１のロボット制御装置１０ａでは、上方リンクが目標位置にあるときのロボットの重心位置を算出する実重心算出部２６は、歩容データ内の体幹リンクと脚リンクの目標位置と、ロボット操作装置１３０から取得した上方リンクの目標位置とに基づいてロボットの重心位置を算出した。この実重心位置算出部２６の処理が実施例１と異なる点である。その他のモジュールは実施例１と同じであるので説明を省略する。
【００３２】
図４に実施例２のフローチャート図を示す。実施例１と異なる点は、ステップＳ２００でロボット本体１００の各関節に取り付けられたエンコーダ１４０の出力値を読み込み、その値に基づいてステップＳ１１０で上方リンクの目標値が標準位置と異なる場合のロボットの重心位置を算出する点である。その他は図２のフローチャートと同じである。
【００３３】
図１と図３を比較すると理解できるように実施例２によると逆変換部２２ｂが不要となり制御装置を簡単化することができる。また図２と図４のフローチャート図を比較すると理解できるように実施例２によると処理を簡単化することができる。
また実施例２ではロボットの重心位置を算出する際、ロボット本体１００の各関節に取り付けられたエンコーダ値を用いるため、各リンクの相対位置関係が正確に取得できる。またエンコーダの出力値には、ロボット操作装置１３０によってロボットの上方リンクの目標位置が与えられ、その目標位置に従ってロボットが動作した結果も反映されている。従ってロボットの実際の重心位置を正確に算出することが可能となる。これによりロボットの実際の重心位置を、ロボットの上方リンクが標準位置にあるときのロボット重心位置に、より正確に一致するよう体幹リンクの目標位置を補正することが可能となる。
【００３４】
なお実施例２では、加算部３０が「補正手段」に相当するとともに「上方リンク位置取得手段」の一態様に相当する。またエンコーダ１４０が関節角計測手段の一態様に相当する。その他の請求項に記載の各手段と実施例の構成との対応は実施例１と同様である。
【００３５】
＜実施例３＞
次に図５と図６を用いて実施例３を説明する。この実施例ではロボット操作装置１３０ｂからは、制御サンプリング毎の上方リンクの目標関節角が出力される。これは例えば、ロボット操作装置１３０ｂが上方リンクの各関節の目標関節角を個別に指令する形態である場合などである。またはロボット操作装置１３０ｂがロボットの制御装置の一部として機能する場合でもよい。ロボットの制御装置の一部としての機能とは、ロボット自体がカメラと画像処理装置を備え、物体の位置を認識し、その物体に向けて上方リンクを移動させる機能などがある。この場合、ロボット操作装置１３０ｂ内で、物体の位置から手先目標位置を設定し、この手先目標位置から上腕リンク（上方リンク）の制御サンプリング毎の目標関節角を算出する。ロボット操作装置１３０ｂからは、算出された制御サンプリング毎の上方リンクの各関節の目標関節角が出力される。
【００３６】
図５は実施例３のロボット制御装置１０ｃとロボット本体１００のブロック図である。ロボット操作装置１３０ｂから出力された上方リンクの各関節の目標関節角は、実重心位置算出部２６ｂに送られる。同時に実重心位置算出部２６ｂには、逆変換部２２ａから出力される脚部の各関節の目標関節角が送られる。実重心位置算出部２６ｂは、上方リンクの各関節の目標関節角と脚部の各関節の目標関節角からロボットの重心位置を算出する。標準重心位置算出部２４および偏差算出部３２の機能は実施例１と同様である。
【００３７】
偏差算出部３２で算出された重心位置の水平面内の偏差は、体幹リンクの水平方向の目標位置の補正値として、加算部３０ｂで歩容データ上の体幹リンクの目標位置に加算される。
ここで、実施例１ではロボット操作装置１３０から上方リンクの目標位置が出力されるので、図１の加算部３０ではロボット操作装置１３０から出力される上方リンクの目標位置が加算された。実施例３ではロボット操作装置１３０ｂからは制御サンプリング毎の上方リンクの各関節の目標関節角が出力されるので、加算部３０ｂではロボット操作装置１３０ｂからの出力を加算する必要はない。
【００３８】
加算部３０ｂで、歩容データの体幹リンクの目標位置に対して補正が加えられた後の歩容データは逆変換部２２ｃで脚部の各関節の目標関節角に変換される。アクチュエータ駆動部２８には、逆変換部２２ｃから出力される脚部の各関節の目標関節角と、ロボット操作装置１３０ｂから出力される上方リンクの各関節の目標関節角が入力される。アクチュエータ駆動部２８は、これらの目標関節角に追従するようにアクチュエータ１１０を駆動する。
【００３９】
図６に上記動作の処理のフローチャートを示す。ステップＳ１０２ｂではロボット操作装置１３０ｂから上方リンクの各関節の目標角を取得する。ステップＳ１１０ｂでは、ステップＳ１０４から出力される脚部各関節の目標関節角と、ステップＳ１０２ｂで取得した上方リンクの各関節の目標関節角に基づいて、ロボットの上方リンクが目標関節角通りに動いた場合のロボットの重心位置を算出する。ステップＳ１１２では、ステップＳ１１０ｂで求めたロボットの実重心位置と、ステップＳ１０８で求めた上方リンクが標準位置にあるときのロボットの重心位置との水平面内偏差を算出する。ステップＳ１１４ｂでは、歩容データの体幹リンクの水平方向の目標位置を上記の水平面内偏差で補正し、脚部の最終的な歩容データが算出される。実施例１のステップＳ１１４の処理（図２）では、ここで上方リンクの目標位置が加えられロボット全体の最終的な歩容データが算出されたが、実施例３のステップＳ１１４ｂでは脚部の歩容データのみが算出される。ステップＳ１１６ｂではステップＳ１１４ｂで算出された脚部の歩容データを逆変換し、脚部の角関節の目標関節角が算出される。そしてステップＳ１１８において、ステップＳ１１６ｂで算出された脚部の各関節の目標関節角と、ステップＳ１０２ｂで取得した上方リンクの各関節の目標関節角とに基づいてロボットの各各関節が駆動される。
【００４０】
図１に示した実施例１のブロック図と図５に示した実施例３のブロック図を比較すると、または図２に示した実施例１のフローチャートと図と図６に示した実施例３のフローチャート図を比較すると、実施例３の方が処理が簡単となっている。これは実施例３のロボット操作装置１３０ｂからは、上方リンクの各関節の目標関節角が出力されることによる。このように上方リンクの各関節の目標関節角を別の制御回路（ロボット操作装置１３０ｂ）で求めることによって、実際の重心位置と上方リンクを標準位置においたときの重心位置との水平方向の差（水平面内偏差）を求めて体幹リンクの水平方向の目標位置を補正する処理を単純化することができる。
【００４１】
なお実施例３においては、「入力手段」の一態様であるロボット操作装置１３０ｂからは、制御サンプリング毎の上方リンクの目標関節角が出力される。従ってロボット操作装置１３０ｂからから出力される上方リンクの目標関節角は直接アクチュエータ駆動部２８へ送られる。よって実施例３においてはアクチュエータ駆動部は「制御手段」の一態様に相当するとともに「上方リンク位置取得手段」および「補正手段」の一態様に相当する。また図６のフローチャートにおいてステップＳ１１４ｂが「補正手段」の一態様に相当する。その他の請求項に記載の各手段と実施例の構成との対応は実施例１と同様である。
【００４２】
次に図７、図８を用いてロボットの体幹リンク目標位置を補正する補正量の設定を例示する。この例示は先に説明した実施例１から実施例３のいずれの構成でも可能な例である。説明を簡単にするため図７の例ではロボット体側方向、即ち図７に図示したＹ軸方向のみについてロボットの動作を説明する。
図７（Ａ）はロボット本体１００が紙面垂直上方に歩行しているときのある時点でのロボットの姿勢の模式図である。図７（Ａ）のロボットの構成は前述した通りである。
図７（Ａ）の姿勢はまた、ロボットの上方リンク（右腕部４４Ｒ、左腕部４４Ｌ、頭部リンク４８）が標準位置にあるときの姿勢でもある。ここで右腕部４４Ｒの代表点は右腕手先リンク先端４４ＲＰに設定され、同様に左腕部４４Ｌの代表点は左腕手先リンク先端４４ＬＰに設定されている。また頭部リンク４８の代表点は頭頂部４８Ｐに設定されている。そしてそれらの標準位置は夫々図７（Ａ）に示した、右腕手先リンク先端４４ＲＰ、左腕手先リンク先端４４ＬＰ、頭頂部４８Ｐの位置である。これらの標準位置は体幹リンク代表点４０Ｐに原点をおいた座標系での座標値で表される。例えば右腕手先リンク先端４４ＲＰの標準位置は図７（Ａ）に図示するベクトル４４ＲＰＨで表される。他の標準位置についても同様である。
【００４３】
図７（Ａ）では、体幹リンク４０と脚リンク（右脚部４６Ｒ、左脚部４６Ｌ）は歩容データ内の体幹リンク４０と脚リンクの目標位置の時系列データに従って動作している。上方リンクは体幹リンク４０に対して固定されているとして歩容データが作成されており、図７（Ａ）の状態では上方リンクは動作していない。即ち上方リンクは標準位置にある。
図７（Ａ）の姿勢におけるロボット全体の実際の重心Ｇの位置はＹ１にある。また体幹リンク４０の代表点４０Ｐの位置もＹ１である。図７（Ａ）ではまだロボット操作装置１３０は操作されておらず、上方リンクの目標位置はロボット操作装置１３０からは与えられていない。従ってロボットは歩容データ記憶装置２０に記憶された歩容データに追従して動作している。従って歩容データ内の体幹リンク４０と脚リンクの目標位置、及び上方リンクの標準位置から算出されるロボットの重心ＧＨの位置もＹ１である。
ここでロボット操作装置１３０から、ロボットの左腕部４４Ｌの手先目標位置がＱの位置に与えられる。すると左腕部４４Ｌはこの目標位置に従って動作する。その結果左腕部４４Ｌは標準位置から離脱し、図７（Ｂ）の姿勢になる。このとき、歩容データ内の体幹リンク６０と脚リンクの目標位置、およびロボット操作装置から取得した上方リンクの目標位置から算出されるロボットの重心Ｇの位置はＹ２となる。従って、体幹リンクと脚リンクが目標位置にあり上方リンクが標準位置にあるときのロボットの重心ＧＨの位置Ｙ１と、体幹リンクと脚リンクが目標位置にあり上方リンクが取得した目標位置にあるときのロボットの重心Ｇの位置Ｙ２との水平面内偏差はベクトルＬ１となる。ここでベクトルＬ１は、上方リンクが取得した目標位置にあるときのロボットの重心Ｇの位置Ｙ２から、上方リンクが標準位置にあるときのロボットの重心ＧＨの位置Ｙ１へと向いたベクトルである。
【００４４】
ロボット制御装置は、体幹リンク代表点４０Ｐの目標位置をベクトルＬ１だけ補正する。補正した後のロボットの姿勢を図７（Ｃ）に示す。補正された結果、体幹リンク代表点４０Ｐの目標位置は補正前のＹ１から図７（Ｃ）上で左方向へＬ１だけ移動したＹ３の位置となる。この補正により、上方リンクがロボット操作装置１３０から与えられた目標位置となったときのロボットの重心Ｇの位置は、体幹リンクと脚リンクの目標位置および上方リンクの標準位置から算出されたロボットの重心ＧＨの位置Ｙ１と水平面内でほぼ同じ位置とすることができる。なお、厳密には、体幹リンク４０の水平方向目標位置を補正した後のロボットの姿勢（図７（Ｃ））は、上方リンクが標準位置にあるときのロボットの重心位置を計算した際のロボットの姿勢（図７（Ａ））または上方リンクが取得した目標位置にあるときのロボットの重心位置を算出した際のロボットの姿勢（図７（Ｂ））のいずれとも異なる。従って補正後のロボットの姿勢（図７（Ｃ））の姿勢で実際の重心位置はＹ１に正確には一致しない。しかしバランスを確保するには十分な程度には一致する。
このようにリアルタイムにロボット上方リンクの目標位置が与えられた場合であっても、ロボット本体１００を、バランスを失わずに動作し続けるよう制御することができる
なお、図７（Ａ）、（Ｂ）において、左手先位置４４ＬＰが１サンプリングの間で図７（Ａ）の状態から図７（Ｂ）の状態へ移ることはなく、実際には図７（Ａ）の状態から徐々に図７（Ｂ）の状態へと移っていく。その過程でサンプリング周期ごとに上記のような補正を繰り返すことになる。図７はあくまでも補正量の設定を例示するための模式図である。
【００４５】
次に図８を用いてロボット重心位置を補正する他の例を示す。この例では図８に示すＸ軸方向についてのみ説明する。
図８（Ａ）は歩容データ内の体幹リンクと脚リンクの目標位置および上方リンクの標準位置に従ってロボットが直立姿勢をとったときの模式図である。上方リンクは標準位置にある。例えば右手先位置４４ＲＰの標準位置は体幹リンク代表点４０Ｐを原点とする座標系でのベクトル４４ＲＰＨで表される。
ロボットは歩容データに追従するよう制御されているので歩容データと上方リンクの標準位置とから算出されるロボットの重心ＧＨの位置と、ロボットの実際の重心Ｇの位置は、Ｘ軸上の位置Ｘ１で同一である。ここでロボット操作装置１３０から、両腕手先位置（４４ＲＰ、４４ＬＰ）の位置を図８（Ｂ）に示すＲの位置とするよう目標位置が出力される。ロボット本体１００は、この目標位置に追従して、図８（Ｂ）に示すようにロボット両腕の手先代表点（４４ＲＰ、４４ＬＰ）はＲの位置となる。ロボット両手先位置が標準位置から離脱したことにより、ロボットの実際の重心位置ＧはＸ２の位置になる。このときの重心位置の水平方向偏差はベクトルＬ２である。
ロボット制御装置は体幹リンクの代表点４０Ｐの目標位置の水平方向位置をベクトルＬ２だけ補正する。その結果が図８（Ｃ）である。この例では体幹リンクの代表点４０Ｐの水平方向位置をベクトルＬ２だけ補正するために体幹リンクの目標姿勢角をＡ１とするよう補正量が設定される。その結果、上方リンクがロボット操作装置１３０から取得した目標位置となったときの重心Ｇの位置は、体幹リンクと脚リンクを歩容データ内の目標位置におき、上方リンクを標準位置においたときのロボットの重心ＧＨの位置Ｘ１にほぼ一致する。従ってロボットはバランスを失わずに動作を続けることができる。
なお、図８も図７と同様にあくまでも補正量設定の様子を例示した模式図である。
【００４６】
＜実施例４＞
次に実施例４として、ロボット動作中に他の物体（例えば荷物）が付加された場合にロボットのバランスを確保する制御装置について、図１に戻って説明する。
実施例４ではロボット操作装置１３０は、ロボットの頭部リンクに設置されたカメラで荷物の位置を検出し、その荷物を両腕で把持するようリアルタイムに上方リンクの目標値を生成する装置である。
またロボットの両腕手先には力センサ１２０が取り付けられている。ロボット本体１００が荷物を持った時点で力センサ１２０は荷物から受ける力を検出する。検出された力は実重心位置算出部２６に送られる。実重心位置算出部２６では力センサの出力値および力センサの設置位置からロボットに付加された質量とその位置とを算出する。実重心位置算出部２６では、ロボット各リンクの質量とその位置のデータと、ロボットに付加された荷物の質量とその位置のデータとを合わせてロボット全体の重心位置を算出する。その後の処理は実施例１または実施例２と同様である。
リアルタイムで与えられるロボットの上方リンクの目標位置と同時に、ロボットに付加される物体（例えば荷物）の質量と位置を考慮してロボット全体の重心位置を算出する。この重心位置と、歩容データ内の体幹リンクと脚リンクの目標位置および上方リンクの標準位置から算出される重心位置との水平面内偏差を算出する。そして体幹リンクの代表点の水平方向の目標位置を、上記偏差を補正量として補正する。これによりロボットがバランスを失うことを回避するよう制御できる。このとき、ロボットに付加された物体の影響によるＺＭＰの変化を制御ロジック内で明示的に考慮する必要はない。制御ロジックを簡略化できる。
なおこの実施例４において、ロボットの頭部リンクに設置されたカメラで荷物の位置を検出し、その荷物を両腕で把持するようリアルタイムに上方リンクの目標値を生成する装置であるロボット操作装置１３０が「上方リンクで物体を把持する動作を指定するために用いる入力手段」の一態様に相当する。
【００４７】
図９を用いて実施例４の具体例を示す。図９（Ａ）は、ロボット本体１００が体幹リンクと脚リンクの目標位置と上方リンクの標準位置に制御されているときの姿勢である。このときのロボットの実際の重心Ｇの位置は、体幹リンクと脚リンクの目標位置および上方リンクの標準位置から算出されるロボットの重心ＧＨの位置とＸ軸上の位置Ｘ１で一致している。また体幹リンク４０の代表点４０Ｐの位置もＸ１である。
次にロボット頭部に備えられたカメラが荷物２００を検出し、ロボットの両腕に対してこの荷物を把持するよう両腕手先位置を荷物の両側に設定した手先目標位置を出力し、ロボット本体１００はこの目標位置に従って動作する。
図９（Ｂ）は、ロボット本体１００が荷物２００を持った瞬間を示す。ロボット本体１００の手先に設置された力センサ１２０は荷物の質量による外力を検出する。実重心位置算出部２６（図１参照）にはこの外力の値が入力されロボット本体１００に付加された質量とその位置が算出される。実重心位置算出部２６は、標準位置から離脱した手先位置と、この付加された質量とその位置からロボット本体１００全体の重心Ｇの位置（体幹リンクと脚リンクの目標位置、上方リンクの目標値および荷物の質量と位置から算出されるロボットの重心Ｇの位置）を算出する。算出された重心Ｇの位置は図９（Ｂ）に示すＸ４である。荷物把持前の、上方リンクの標準位置から算出されるロボットの重心ＧＨの位置Ｘ１と、荷物把持後のロボットの実際の重心Ｇの位置Ｘ４との水平面内の偏差はベクトルＬ３である。
【００４８】
ロボット制御装置は、体幹リンク４０の代表点４０Ｐの目標位置Ｘ１をこのベクトルＬ３で補正する。補正後のロボット本体１００の姿勢を図９（Ｃ）に示す。体幹リンク４０の位置Ｘ５はもとの位置Ｘ１からＬ３だけ移動している。そしてロボットの実際の重心位置は、体幹リンクと脚リンクの目標位置および上方リンクの標準位置から算出されるロボットの重心ＧＨの位置Ｘ１にほぼ一致する。従ってロボットが荷物を把持して上方に持ち上げてもバランスを失わないよう制御することができる。
【００４９】
なおこのとき、ロボット操作装置１３０からは荷物の位置を上方に上げるよう両腕の手先目標位置が出力し続けられている。従って両腕の手先目標位置は図９（Ｂ）のときと同じ位置に保たれる。そのため両腕４４Ｒ、４４Ｌの姿勢も変化する。
また、荷物を持った瞬間にロボットの質量配分は急に変化するが、ロボットの動作はサンプリング毎にみると微小量ずつ変化する。荷物を持つロボット手先が微小量変化したとき荷物の質量によりロボット全体の重心Ｇの位置が図９（Ｂ）に示す位置Ｘ４となってロボットが前へ倒れる場合でも荷物は机２１０に支えられる。そこでロボット本体１００は転倒することはない。次のサンプリング時に図９（Ｃ）に示すようにロボットの実際の重心Ｇ位置が、体幹リンクと脚リンクの目標位置および上方リンクの標準位置から算出されるロボットの重心ＧＨの位置Ｘ１にほぼ一致するよう体幹リンク代表点４０Ｐの位置が補正されることによって、さらにその次のサンプリング時には安定して荷物を持ち上げることが可能となる。
【００５０】
以上、本発明の具体例を詳細に説明したが、これらは例示に過ぎず、特許請求の範囲を限定するものではない。特許請求の範囲に記載の技術には、以上に例示した具体例を様々に変形、変更したものが含まれる。
また、本明細書または図面に説明した技術要素は、単独であるいは各種の組合せによって技術的有用性を発揮するものであり、出願時請求項記載の組合せに限定されるものではない。また、本明細書または図面に例示した技術は複数目的を同時に達成し得るものであり、そのうちの一つの目的を達成すること自体で技術的有用性を持つものである。
【図面の簡単な説明】
【００５１】
【図１】実施例１のロボット制御装置のブロック図である。
【図２】実施例１のロボット制御装置の処理のフローチャート図である。
【図３】実施例２のロボット制御装置のブロック図である。
【図４】実施例２のロボット制御装置の処理のフローチャート図である。
【図５】実施例３のロボット制御装置のブロック図である。
【図６】実施例３のロボット制御装置の処理のフローチャート図である。
【図７】実施例１から３による補正量設定の例を示す図である。
【図８】実施例１から３による補正量設定の他の例を示す図である。
【図９】実施例４による補正量設定の例を示す図である。
【符号の説明】
【００５２】
１０ａ、１０ｂ、１０ｃ：ロボット制御装置
２０：歩容データ記憶部
２２ａ、２２ｂ、２２ｃ：逆変換部
２４：標準重心位置算出部
２６、２６ｂ：実重心位置算出部
２８：アクチュエータ駆動部
３０、３０ｂ：加算部
３２：偏差算出部
１００：ロボット本体
１１０：アクチュエータ
１２０：力センサ
１３０：ロボット操作装置
【図１】
2007007797.tif 000003
【図２】
2007007797.tif 000004
【図３】
2007007797.tif 000005
【図４】
2007007797.tif 000006
【図５】
2007007797.tif 000007
【図６】
2007007797.tif 000008
【図７】
2007007797.tif 000009
【図８】
2007007797.tif 000010
【図９】
2007007797.tif 000011