JP4525325B2 - 超冗長自由度ロボットの動作制御方法 - Google Patents

Description

本発明は、多数の関節を備える起立した胴体部と、胴体部に取り付けられ、多数の関節を備えるアーム部とを備え、超冗長自由度を有する超冗長自由度ロボットの動作を制御する超冗長自由度ロボットの動作制御方法に関する。

自由度７以上を有する冗長自由度を有するロボットにおいて、その手先の位置を位置決めするための関節角度の個数は無限になる。このような冗長自由度を有するロボットの姿勢制御を行うものとして、特開平５−２３３０４２号公報（特許文献１）に開示された冗長自由度ロボットの姿勢制御方式がある。この姿勢制御方式は、作業対象領域および障害物の各データを取得してニューラルネットワークに入力し、このニューラルネットワークによって冗長自由度のロボット肘角を求めている。そして、作業対象物のデータおよび肘角度のデータに基づいて、冗長自由度ロボットの姿勢を一意的に決定して制御を行うというものである。
特開平５−２３３０４２号公報

上記特許文献１に記載された姿勢制御方式においては、ロボットがある作業を行うために、ロボット逆運動学方程式によって、アーム先端の位置と姿勢との情報により、アームと胴体の関節角を求めることが必要となる。冗長自由度を有するロボットに対して、ロボット逆運動学方程式を解くためには、事前にその拘束条件を確定して冗長自由度を制約しなければならない。

ここで、冗長自由度ロボットにおいては、ニューラルネットワークによって逆運動学方程式の拘束条件のひとつであるロボットの肘角度のみを指定している。このため、たとえば胴体にも多数の関節を有する超冗長自由度のロボット、たとえば人間型のロボットについては、姿勢制御を行うことができず、たとえば肘角度を指定したとしても、胴体部の関節角度によっては、ロボットが転倒してしまう恐れがあるという問題があった。

そこで、本発明の課題は、アーム部および胴体部に複数の関節が設けられた超冗長自由度ロボットが作業を行う際の転倒を防止することができる超冗長自由度ロボットの動作制御方法を提供することにある。

上記課題を解決した本発明に係る超冗長自由度ロボットの動作制御方は、多数の関節を備える起立した胴体部と、胴体部に取り付けられ、多数の関節を備え、冗長自由度を有するアーム部と、胴体部が搭載された移動装置と、を備え、アーム部の冗長自由度に胴体部の自由度を加えた超冗長自由度を有する超冗長自由度ロボットの作業対象物体に対する動作を制御する超冗長自由度ロボットの制御方法であって、胴体部における複数の胴体動作パターンを設定し、各胴体動作パターンに対して、胴体部が起立状態を維持しうる胴体部の関節の最大可動角を求め、胴体動作パターンに対する胴体部の関節の最大可動角に対するアーム部の作業可能領域を求め、胴体動作パターンとアーム部の作業可能領域との関係、および作業対象物体の位置情報に基づいて、超冗長自由度ロボットの動作を決定することを特徴とするものである。

本発明に係る超冗長自由度ロボットの動作制御方法では、アーム部および胴体部に多数の関節が設けられた超冗長ロボットを動作制御するにあたり、まず胴体部の最大可動角を求め、この最大可動角に対して、アーム部の作業可能領域を求めている。このため、胴体部での最大可動角を求めることにより、ロボットが転倒しない範囲でアーム部の関節角度を求めることができるようになる。この条件下でアーム部の関節角度を求めるようにしているので、ロボットを転倒させることなく、アーム部における複数の関節の関節角度を求めることができる。

ここで、超冗長自由度ロボットは、周囲の作業対象物体を視認可能な視覚センサを有し、作業対象物体の位置情報を、視覚センサによって取得する態様とすることができる。

視覚センサによって作業対象物体の位置情報を取得することにより、作業対象物体をより確実に把握することができる。

本発明に係る超冗長自由度ロボットの動作制御方法によれば、アーム部および胴体部に複数の関節が設けられた超冗長自由度ロボットが作業を行う際の転倒を効果的に防止することができる。

以下、図面を参照して本発明の実施形態について説明する。図１は、本実施形態に係る動作制御装置のブロック構成図である。

図１に示すように、本実施形態に係る動作制御装置１は、胴体動作パターン検討部１１、胴体関節角度算出部１２、作業領域算出部１３、ロボット動作戦略決定部１４、およびロボット動作パターン出力部１５を備えている。また、動作制御装置１には、ロボット２および視覚センサ３が接続されている。視覚センサ３は、ロボット２の周囲を撮像し、作業対象物体の位置情報を検出する。視覚センサ３は、検出した作業対象物体の位置情報を動作制御装置１におけるロボット動作戦略決定部１４に出力している。

図２に示すように、ロボット２は、胴体部２１、右アーム部２２、左アーム部２３、頭部２４、および台車２５を備えている。胴体部２１には、第一胴体関節２１Ａ〜第四胴体関節２１Ｄまでの４つの関節が設けられており、胴体部２１は起立した状態とされている。胴体部２１には、これらの４つの関節が設けられており、自由度４とされている。

図３に示すように、第一胴体関節２１Ａ、第二胴体関節２１Ｂ、および第四胴体関節２１Ｄは、Ｐｉｔｃｈ方向（水平軸Ｙ軸周り方向）に回動可能とされ、第三胴体関節２１Ｃは、Ｙａｗ方向（鉛直軸Ｚ軸周り方向）に回動可能とされている。また、各胴体関節２１Ａ〜２１Ｄの回動可能方向（可動方向）および回動可能範囲（稼動範囲）を表１に示す。なお、表１においては、第一胴体関節２１ＡをＴ♯１、第二胴体関節２１ＢをＴ♯２、第三胴体関節２１ＣをＴ♯３、第四胴体関節２１ＤをＴ♯４とそれぞれ示している。

また、右アーム部２２および左アーム部２３は、それぞれ第一アーム関節２２Ａ，２３Ａ〜第七アーム関節２２Ｇ〜２３Ｇまでの７つの関節が設けられている。右アーム部２２および左アーム部２３は、それぞれその基端部が胴体部２１に取り付けられており、右アーム部２２および左アーム部２３の先端部には、それぞれハンド２２Ｈ，２３Ｈが取り付けられている。右アーム部２２および左アーム部２３は、それぞれ７つの関節を有するので、それぞれ自由度７の冗長自由度とされている。

かくして、ロボット２は、アーム部２２，２３の冗長自由度に加えて、胴体部２１の複数自由度を有することから、超冗長自由度ロボットとされている。

さらに、頭部２４は、胴体部２１の上端に固定された状態で取り付けられており、その自由度は０である。また、胴体部２１は、台車２５に搭載されている。台車２５は、本体部２５Ａを有しており、本体部２５Ａに４つの車輪２５Ｂが取り付けられている。

動作制御装置１における胴体動作パターン検討部１１には、図示しないロボット諸元記憶手段からロボット２の諸元が出力される。ここでの諸元には、ロボット２の自由度や表１に示す胴体部２１における各胴体関節２１Ａ〜２１Ｄの可動方向および可動範囲などが含まれる。胴体動作パターン検討部１１では、これらの諸元に基づいて、胴体動作パターンを検討する。また、胴体動作パターン検討部１１では、胴体動作パターンの検討結果を胴体関節角度算出部１２に出力する。

胴体関節角度算出部１２では、胴体動作パターン検討部１１から出力された胴体部２１の動作パターンのそれぞれについて、それぞれの胴体関節２１Ａ〜２１Ｄの最大関節角度を算出する。胴体関節角度算出部１２では、算出した最大関節角度を作業領域算出部１３に出力する。

作業領域算出部１３では、胴体関節角度算出部１２から出力された最大関節角度に基づいて、アーム部２２，２３における作業が可能となる作業領域を動作パターンごとに算出する。作業領域算出部１３は、算出した各動作パターンの作業領域をロボット動作戦略決定部１４に出力する。

ロボット動作戦略決定部１４では、作業領域算出部１３から出力された作業領域および視覚センサ３から出力される作業対象物体の位置情報に基づいて、ロボットの動作パターン戦略を決定する。ロボット動作戦略決定部１４は、決定した動作パターン戦略をロボット動作パターン出力部１５に出力する。

ロボット動作パターン出力部１５は、ロボット動作戦略決定部１４から出力された動作戦略に従って、ロボット２を駆動させる。そのため、ロボット動作パターン出力部１５はロボット２に接続され、ロボット２における胴体部２１およびアーム部２２，２３を駆動制御する。

以上の構成を有する本実施形態に係る動作制御装置１による制御手順について、図１を参照しながら説明する。

動作パターン制御を開始する際には、まず、胴体動作パターン検討部１１において、胴体動作パターンを検討する。本実施形態に係るロボット２では、胴体部２１には、４つの胴体関節２１Ａ〜２１Ｄが設けられており、このうち、ピッチ方向に回動可能となっている関節は、第一胴体関節２１Ａ，第二胴体関節２１Ｂ、および第四胴体関節２１Ｄの３つの関節である。したがって、これらの３つの関節をどのように屈折させるかについて検討する。

これらの関節をどのように屈折されるかを考える基準として、ロボット２が転倒しないことが条件となる。ロボット２が転倒しないようにするためには、ＺＭＰ（Ｚｅｒｏ Ｍｏｍｅｎｔ Ｐｏｉｎｔ）が安定領域にあることを条件とすることができる。第一胴体関節２１Ａと第二胴体関節２１Ｂの稼動範囲を考慮し、胴体動作パターンとしては、図４（ａ）〜（ｆ）に示す６つのパターンが考えられる。

図４（ａ）に示す第一動作パターンでは、第一胴体関節２１Ａ、第二胴体関節２１Ｂ、および第四胴体関節２１Ｄのいずれもが屈曲していない。また、図４（ｂ），（ｃ）に示す第二，第三動作パターンでは、１つの関節のみが屈曲しており、図４（ｂ）に示す第二動作パターンでは、第二胴体関節２１Ｂのみが、図４（ｃ）に示す第三動作パターンでは、第四胴体関節２１Ｄのみが屈曲している。さらに、図４（ｄ），（ｅ）に示す第四，第五動作パターンでは２つの関節が屈曲しており、図４（ｄ）に示す第四動作パターンでは第一胴体関節２１Ａおよび第二胴体関節２１Ｂが屈曲し、図４（ｅ）に示す第五動作パターンでは、第二胴体関節２１Ｂおよび第四胴体関節２１Ｄが屈曲している。そして、図４（ｆ）に示す第六動作パターンでは、第一胴体関節２１Ａ、第二胴体関節２１Ｂ、および第四胴体関節２１Ｄのすべての関節が屈曲している。このように、胴体動作パターンとしては６つの動作パターンが考えられる。これらの動作パターンでは、ロボット２の肩の部分が水平になるように、アーム部２２，２３の第一アーム関節２２Ａ，２３Ａの関節角度を調整している。

胴体動作パターン検討部１１は、このようにして検討した結果得られた６つのパターンを胴体関節角度算出部１２に出力する。胴体関節角度算出部１２では、これら６つのパターンのそれぞれについて、胴体関節角度を算出する。ここでの胴体関節角度としては、ロボット２が起立状態を維持しつつ、各関節角度が最も大きくなる関節角度（最大可動角、以下、「最大関節角度」という）を求め、この最大関節角度以内の角度を胴体関節角度として求める。

いま、図５に示すように、ロボット２の安定領域ＳＡは、台車２５における４つの車輪２５Ｂの接地点が形成する支持多角形（四角形）である。ロボット２のＺＭＰの位置がこの安定領域内に存在していれば、ロボット２は、転倒することなく、運動、作業を行うことができる。逆に、ロボット２のＺＭＰの位置が安定領域から外れている場合には、ロボット２は不安定となり、理論上は転倒してしまう。

ここで、ピッチ方向に回動可能な第一胴体関節２１Ａ、第二胴体関節２１Ｂ、および第四胴体関節２１Ｄの最大関節角度を次の要領で求める。ここでは、図４（ｂ）に示す第二アーム部２１Ｂのみが屈曲する第二動作パターンについての最大関節角度の求め方について説明する。ロボット２における両アーム部２２，２３の第一アーム関節２２Ａ，２３Ａについては、両アーム部２２，２３が常に水平となるように回転させ、その他の両アーム部２２，２３におけるその他の関節角は０とする。このときの両アーム部２２，２３の姿勢は、ロボット２が転倒に対して不安定となる最悪姿勢となる。また、ロボット２の両手は、最大設計重量の物体を所持しているとする。この状態で、ロボット２を第二胴体関節２１Ｂ周りに回転させ、ＺＭＰの位置を計算する。この場合、慣性座標系におけるｘ軸方向、ｙ軸方向のＺＭＰの位置ｘ_ｚｍｐ、ｙ_ｚｍｐは、下記（１）式および（２）式によって表される。

ここで、［ｇｘ、ｇｙ、ｇｚ］：重力加速度
ｍｉ：ｉ番目のリンクの質量
［ｘｉ、ｙｉ、ｚｉ］：ｉ番目のリンクの質心位置
上記（１）式および（２）式に第二胴体関節２１Ｂの関節角度を変えて代入した計算結果を図６に示す。ここで、ロボット２を第二胴体関節２１Ｂ周りに回転させるとき、ｘ軸方向においてＺＭＰは変化するがｙ軸方向には変化しない。このため、図６にはｘ軸方向においてもＺＭＰの位置ｘ_ｚｍｐのみを示す。図６に示される第二胴体関節２１Ｂの関節角度θ２とＺＭＰの位置ｘ_ｚｍｐとの関係、およびロボット２の安定領域の定義により、図３（ｂ）に示す第二動作パターンにおけるロボット２が転倒に対して不安定となる第二胴体関節２１Ｂの最大可動角を決定することができる。具体的には、たとえばｘ軸方向における安定領域を−１２０．０ｍｍ≦ｘ_ｚｍｐ≦１２０．０ｍｍに設定する場合、第二胴体関節２１Ｂの最大可動角度（最大関節角度）は１４度となる。第二胴体関節２１Ｂの関節角度が１４度以内となっているときには、両アーム部２２，２３が任意の姿勢を保持しても、ロボット２は転倒することなく、安定している。

このようにして、第二パターンにおける第二胴体関節２１Ｂの最大関節角度を算出することができるが、他のパターンについても、同様の手法によって胴体関節角度（最大関節角度）を算出することができる。胴体関節角度算出部１２は、こうして求めた胴体関節角度を作業領域算出部１３に出力する。

作業領域算出部１３では、胴体関節角度算出部１２から出力される胴体関節角度を用いて、両アーム部２２，２３における作業可能領域（最大作業領域）を求める。胴体動作パターン検討部１１で検討された各動作パターンにおいて、胴体関節角度が最大関節角度を取るときのロボット２の肩の位置を決定することができる。ロボット２の前方を作業領域とし、しかもアーム部２２，２３の各関節の可動範囲を考慮しない場合、各動作パターンにおいての両手先の作業可能領域は、右アーム部２２については下記（３）式、左アーム部２３については（４）式によって推定することができる。ただし、ロボット２が両手で作業する場合、その作業領域は、左右手先の作業可能範囲の集合である。

（ｘ−ｘ_０ｉ）^２＋（ｙ＋ｙ_０ｉ）^２＋（ｚ−ｚ_０ｉ）^２≦ｒ^２ ・・・（３）
（ｘ−ｘ_０ｉ）^２＋（ｙ−ｙ_０ｉ）^２＋（ｚ−ｚ_０ｉ）^２≦ｒ^２ ・・・（４）
上記（３）式（４）式において、
ｘ≧ｘ_０ｉ
（ｘ_０ｉ，±ｙ_０ｉ，ｚ_０ｉ）：各動作パターンにおける胴体関節が最大関節角度をとるときにおける左右の肩の位置
ｒ：アームの長さ
ロボット２における左右手先の作業可能領域の一例（ｙ＝±ｙ_０ｉのｘｚ平面）を図７に示す。図中のｘ_ｅｎｄは、ｘ軸方向における手先位置、ｚ_ｅｎｄはｚ軸方向における手先位置である。図７における各曲線は、各動作パターンにおいての手先作業領域の境界線である。このようにして、作業領域算出部１３において、両手先の作業領域を算出することができる。作業領域算出部１３は、算出した作業領域をロボット動作戦略決定部１４に出力する。

ロボット動作戦略決定部１４では、各動作パターンにおける作業領域の関係、および視覚センサ３によって取得される作業対象物体の位置情報に基づいて、ロボット２の動作戦略を決定する。ただし、動作戦略を決定する際には、ロボット２の動作をできるだけ容易かつ単純に実現することが望まれる。このため、動作する胴体関節の数をできるだけ少なくすることと、アルゴリズムをできるだけ単純化することを考慮する。

図７に示す例では、第二動作パターンにおける手先作業領域と第五動作パターンにおける手先作業領域とがほぼ重なっている。また、第六動作パターンの手先作業領域は、第四動作パターンの手先作業領域に含まれている。さらに、第二動作パターンでは１つの関節が動作するのに対して、第五動作パターンでは２つの関節が動作し、第四動作パターンでは２つの関節が動作するのに対して、第六動作パターンでは３つの関節が動作する。したがって、動作する関節の数およびアルゴリズムの単純化の観点から、第五動作パターンおよび第六動作パターンについては、考慮しないこととする。

以上の考慮点のもとで、ロボット動作戦略決定部１４におけるロボット動作戦略の決定手順について説明する。図８は、ロボット動作戦略の決定手順を示すフローチャートである。

図８に示すように、まず、視覚センサ３から出力される作業対象物体の位置情報（ｘ座標、ｙ座標、およびｚ座標）を取得する（Ｓ１）。次に、作業対象物体の位置情報を第一動作パターンの手先作業領域を表す不等式に代入する（Ｓ２）。ここでの不等式は、図７における第一動作パターンに対応する曲線（ａ）の下側の領域を表す式である。そして、不等式が成立するか否かを判断する（Ｓ３）。

その結果、不等式が成立すると判断した場合には、第一動作パターンによって作業を行うと決定し（Ｓ４）、処理を終了する。一方、不等式が成立しないと判断した場合には、作業対象物体の位置情報を第二動作パターンの手先作業領域を表す不等式に代入する（Ｓ５）。ここでの不等式は、図７における第二動作パターンに対応する曲線（ｂ）の下側の領域を表す式である。そして、不等式が成立するか否かを判断する（Ｓ６）。

その結果、不等式が成立すると判断した場合には、第二動作パターンによって作業を行うと決定し（Ｓ７）、処理を終了する。一方、不等式が成立しないと判断した場合には、作業対象物体の位置情報を第三動作パターンの手先作業領域を表す不等式に代入する（Ｓ８）。ここでの不等式は、図７における第三動作パターンに対応する曲線（ｃ）の下側の領域を表す式である。そして、不等式が成立するか否かを判断する（Ｓ９）。

その結果、不等式が成立すると判断した場合には、第三動作パターンによって作業を行うと決定し（Ｓ１０）、処理を終了する。一方、不等式が成立しないと判断した場合には、作業対象物体の位置情報を第四動作パターンの手先作業領域を表す不等式に代入する（Ｓ１１）。ここでの不等式は、図７における第四動作パターンに対応する曲線（ｄ）の下側の領域を表す式である。そして、不等式が成立するか否かを判断する（Ｓ１２）。

その結果、不等式が成立すると判断した場合には、第四動作パターンによって作業を行うと決定し（Ｓ１３）、処理を終了する。一方、不等式が成立しないと判断した場合には、現在のロボット２の位置での作業は不可能であると判断し、台車を移動する（Ｓ１４）。その後、ステップＳ１に戻って、再びロボットの動作戦略を検討して決定する。

こうして、ロボット動作戦略決定部１４においてロボット動作戦略を決定したら、決定したロボット戦略をロボット動作パターン出力部１５に出力する。ロボット動作パターン出力部１５では、決定されたロボット動作戦略に基づいて、ロボット２の動作制御を行う。

このように、本実施形態に係るロボットの動作制御では、複数の自由度を有する胴体部２１が起立しうる条件での胴体動作パターンおよび胴体部２１における各胴体関節の最大可動角を求め、冗長自由度を有するアーム部２２，２３の作業可能領域を求めている。この作業可能領域内でアーム部２２，２３の動作戦略を決定するので、超冗長自由度を有するロボット２が転倒することなく、作業を行うことができるようになる。

以上、本発明の好適な実施形態について説明したが、本発明は上記実施形態に限定されるものではない。たとえば、アーム部２２，２３としては、冗長自由度を有するものとして７のつのアーム関節を設けているが、８以上の関節を設けることができる。また、胴体部２１については、４つの胴体関節を設けているが、５以上の関節を設けることができ、もちろん、胴体部が冗長自由度となる７以上の胴体関節を設けることもできる。さらに、上記実施形態では、アルゴリズムを単純化させる目的で、第五動作パターンおよび第六動作パターンを考慮せずに動作戦略を決定しているが、すべての動作パターンを考慮して動作戦略を決定することもできる。また、上記実施形態では、胴体部２１の動作パターンとして第六動作パターンまで設定しているが、すべてのパターンである８つの動作パターンを設定することもできる。

本発明に係る動作制御装置のブロック構成図である。 動作制御装置で制御されるロボットの模式図である。 胴体関節の回動方向を説明するための模式的斜視図である。 胴体部の動作パターンを説明するための模式図であり、（ａ）は第一動作パターン、（ｂ）は第二動作パターン、（ｃ）は第三動作パターン、（ｄ）は第四動作パターン、（ｅ）は第五動作パターン、（ｆ）は第六動作パターンをそれぞれ示す。 安定領域を説明するための台車の模式的平面図である。 第二胴体関節の関節角度とｘ方向のＺＭＰの位置ｘ_ｚｍｐの関係を示すグラフである。 ロボットにおける手先の作業可能領域の一例を示すグラフである。 ロボット動作戦略の決定手順を示すフローチャートである。

符号の説明

１…動作制御装置、２…ロボット、３…視覚センサ、１１…胴体動作パターン検討部、１２…胴体関節角度算出部、１３…作業領域算出部、１４…ロボット動作戦略決定部、１５…ロボット動作パターン出力部、２１…胴体部、２１Ａ…第一胴体関節、２１Ｂ…第二胴体関節、２１Ｃ…第三胴体関節、２１Ｄ…第四胴体関節、２２…右アーム部、２３…左アーム部、２４…頭部、２５…台車、ＳＡ…安定領域。

Claims (2)

1. 多数の関節を備える起立した胴体部と、前記胴体部に取り付けられ、多数の関節を備え、冗長自由度を有するアーム部と、前記胴体部が搭載された移動装置と、を備え、前記アーム部の冗長自由度に前記胴体部の自由度を加えた超冗長自由度を有する超冗長自由度ロボットの作業対象物体に対する動作を制御する超冗長自由度ロボットの制御方法であって、
   前記胴体部における複数の胴体動作パターンを設定し、前記各胴体動作パターンに対して、前記胴体部が起立状態を維持しうる前記胴体部の関節の最大可動角を求め、
   前記胴体動作パターンに対する前記胴体部の関節の最大可動角に対する前記アーム部の作業可能領域を求め、
   前記胴体動作パターンと前記アーム部の作業可能領域との関係、および前記作業対象物体の位置情報に基づいて、前記超冗長自由度ロボットの動作を決定することを特徴とする超冗長自由度ロボットの動作制御方法。
2. 前記超冗長自由度ロボットは、周囲の作業対象物体を視認可能な視覚センサを有し、
   前記作業対象物体の位置情報を、前記視覚センサによって取得する請求項１に記載の超冗長自由度ロボットの動作制御方法。

Images