JP2009214268A - 全身協調装置、ロボット及びロボットの制御方法 - Google Patents

Abstract

【課題】全身協調動作においても正確な位置制御を行うことができるロボット及びロボットの制御方法を提供すること。
【解決手段】ロボットは、モータと、モータ駆動するモータドライバと、モータドライバに指令値として関節角を入力する協調制御部１３３とを有する。協調制御部１３３は、
重心速度から関節角速度を求める角速度算出手段１４と、角速度算出手段１４が算出した角速度から第１関節角を算出する積分手段１３と、予め定められた第２関節角を入力する関節角入力手段１１と、積分手段１３が算出した第１関節角及び関節角入力手段１１から入力された第２関節角から重心位置を算出する重心位置算出手段１４ａと、重心位置算出手段１４ａが算出した重心位置及び外部から入力される目標重心位置から重心速度を算出するフィードバック手段１５ａとを有する。
【選択図】図１

Description

本発明は、２足以上の脚部を有し、全身の関節角を強調制御する協調制御装置、及びこれを搭載したロボット並びにロボットの制御方法に関する。

従来の全身協調制御について説明する。従来の協調制御においては、バランスを取ることができる重心軌道と手先の作業のための軌道を与えて両軌道を満足する関節角度を関節速度と重心軌道又は手先軌道の速度の関係を表す線形な式（一般的には微分関係と言われている）を解き、関節速度を数値積分して関節角度を求める（特許文献１参照）。

図５に示すように、重心位置と左手の軌道を拘束する場合を考える。重心速度ベクトルと、関節角速度ベクトルの微分関係はヤコビアンを用いて下記のように表わされる。

ここでは、ロボットの関節が全部で３０個あるとすれば、関節角速度ベクトルは３０×１のベクトル、重心速度ベクトルは３×１のベクトルなので、ヤコビアンは、３×３０の行列となる。一方、左手の速度ベクトルと関節角速度ベクトルの微分係数は、ヤコビアンを用いて、下記のように表わされる。

次に、（１）と（２）を同時に満たす関節角速度ベクトルを求める。これには、一般手には以下のように求める。先ず、式（１）の一般解を求める。一般解は、下記のようになる。

式（３）を式（２）に代入すると下記となる。

式（６）を式（３）に代入して

ｋ_２は任意のベクトルであるからこれを０ベクトルとすれば式（７）は、式（８）となり、これより関節角速度を求めることができる。

関節下記度を求めるときは、式（８）で計算された関節角速度を積分するが、重心位置、手先位置の積分誤差を補償するために、以下のようなフィードバックを行う。重心位置で説明する。積分により、算出された関節角度と、この角度を用いて計算された重心位置の関係は、幾何学的に求まり、関数ｆ_ｇを用いて以下のように表すことができる。

一方、以下のフィードバックを施した重心速度を式（８）に与える。

左手の位置のフィードバックも重心と同じように考えて下記のように求まる。

図６は、上記式（８）乃至（１２）を演算する回路を示す。当該回路は、モータドライバに対して指令値を生成する指令値生成回路である。図６に示すように、指令値生成回路２００は、関節角速度ベクトル算出手段２１２と、関節角速度を積分する積分手段２１３と、重心位置算出手段２１４ａ、２１４ｂと、フィードバック手段２１５ａ、２１５ｂとを有する。フィードバック手段２１５ａ、２１５ｂは、それぞれフィードバックゲインを乗ずる乗算部２２１ａ、２２１ｂと、重心位置を微分する微分手段２２２ａ、２２２ｂとを有する。指令値生成回路２００には、足位置や重心位置が入力され、関節角速度ベクトルが積分されて得られる関節角が出力される。この関節角の値を使用してモータ制御部が各モータを制御する。

なお、その他、従来のロボットの制御方法としては、特許文献２、３等に記載の技術がある。
特開２００３−５８９０７号公報 特開２００７−７７９７号公報 特開２００６−３２１０１２号公報

このように、従来の技術においては、ロボットの全関節を使用して全身協調を行っているが、関節角度が関節速度を積分するため、積分誤差が生じる。この積分誤差のため、手先位置にも誤差が生じ、正確な位置制御を行うことができないという問題点がある。すなわち、積分誤差により、より精密なタスクに対して正確なリンクの位置を算出することができない。より正確にしようと思うと収束判定の閾値を小さくしなければならず、収束回数が増え、計算時間がかかる。これは、全ての関節が積分されているためである。

本発明は、このような問題点を解決するためになされたものであり、全身協調動作においても正確な位置制御を行うことができる協調制御装置、ロボット及びロボットの制御方法を提供することを目的とする。

上述した課題を解決するために、本発明に係る協調制御装置は、重心速度から関節角速度を求める角速度算出手段と、前記角速度算出手段が算出した角速度から１以上の関節角を第１関節角群として算出する積分手段と、予め定められた１以上の関節角を第２関節角群として入力する関節角入力手段と、前記第１関節角群及び前記第２関節角群から重心位置を算出する重心位置算出手段と、重心位置算出手段が算出した重心位置及び外部から入力される目標重心位置から前記重心速度を算出するフィードバック手段とを有するものである。

本発明においては、全関節角のうち、一部の関節角を第２関節角群として外部から入力することで、全関節を関節角速度から積分して求める場合に比して積分誤差をなくすことができる。

また、前記フィードバック手段は、前記目標重心位置を微分して重心速度を算出する微分手段と、前記重心位置算出手段が算出した重心位置と外部から入力される目標重心位置の差分をゲイン調整するゲイン調整手段と、前記微分手段により算出された重心速度とゲイン調整手段の調整結果とを加算して前記角速度算出手段に入力する加算手段とを有するものとすることができる。フィードバック制御により、積分誤差を補償する。

さらに、前記重心位置算出手段及び前記フィードバック手段は、重心位置及び少なくとも１以上の基準座標系からの軌道に対応して設けられることができる。たとえば、ヒューマノイドロボットの場合、右手の軌道を第２関節群として入力し、重心位置、左手、両足の関節角は第１関節角として算出するなどすることができる。

本発明に係るロボットは、モータと、前記モータ駆動するモータドライバと、前記モータドライバに指令値として関節角を入力する制御手段とを有し、前記制御手段は、重心速度から関節角速度を求める角速度算出手段と、前記角速度算出手段が算出した角速度から１以上の関節角を第１関節角群として算出する積分手段と、予め定められた１以上の関節角を第２関節角群として入力する関節角入力手段と、前記第１関節角群及び前記第２関節角群から重心位置を算出する重心位置算出手段と、重心位置算出手段が算出した重心位置及び外部から入力される目標重心位置から前記重心速度を算出するフィードバック手段とを有するものである。

本発明にかかるロボットの制御方法は、２足以上の脚部を有するロボットの制御方法であって、前記脚部を含む全身の関節角から重心位置を算出する重心位置算出工程と、目標重心位置及び前記重心位置算出工程で算出された重心位置から重心速度を算出するフィードバック工程と、前記重心速度に基づき関節角速度を算出する関節角速度算出工程と、前記関節角速度算出工程にて算出された関節角速度を積分して関節角を算出する積分工程と、前記積分工程で算出された関節角を予め指定される関節角と共に出力する出力工程とを有し、前記重心位置算出工程は、前記出力工程にて出力される関節角から前記重心位置を算出するものである。

本発明によれば、全身協調動作においても正確な位置制御を行うことができる協調制御装置、ロボット及びロボットの制御方法を提供することができる。

以下、本発明を適用した具体的な実施の形態について、図面を参照しながら詳細に説明する。本実施の形態にかかるロボットは、精度を要求する関節には明示的に指令を与え、全身協調アルゴリズムから除外するものである。本実施の形態においては、少なくとも２脚、２腕を備えたヒューマノイドロボットの制御方法について説明する。図１は、本実施の形態にかかるヒューマノイドロボットの一例を示す斜視図である。図１に示すように、ロボット１は、体幹部ユニット１ｃの所定の位置に頭部ユニット１ａ、左右２つの腕部ユニット１ｂ、及び左右２つの脚部ユニット１ｄが連結されている。

頭部ユニット１ａには、撮像部が備えられ、周囲所定範囲を撮影することが可能である。また、頭部ユニット１ａには、図示せぬマイク、スピーカ等が備えられ、ユーザからの呼びかけを認識したり、それに答えたりすることも可能である。頭部ユニット１ａは、体幹部ユニット１ｃに対して床面に対して水平な面内で左右方向に回動可能に接続されており、頭部ユニット１ａを回動することで撮像する範囲を状況に応じて変更し、周囲の環境を撮像することができる。

腕部ユニット１ｂ及び脚部ユニット１ｄは、体幹部ユニット１ｃに内蔵された制御部に含まれる後述する協調制御部によって、所定の制御プログラムに従って各腕部ユニット１ｂに含まれる関節部、脚部ユニット１ｄに含まれる関節部を駆動する量が制御され、各関節の関節駆動角度が決定されることで、所望の位置および姿勢をとるものである。

このようなヒューマノイドロボットにおいては、安定領域が狭く、重心が高いため、物理的に不安定である。図２は、ロボットの安定領域を示す図である。ロボットの安定領域は、図２に示すように、左右の足を外包する領域となる。

ヒューマノイドロボットは、バランスを取ること（倒れないこと）及び腕を使って作業することが要求される場合があるが、バランスを取るためには、上述の安定領域と重心位置の関係が重要となる。バランスを取るためには一般的には、ＺＭＰ（Zero Moment Point：ゼロモーメントポイント）安定規範という安定の程度を示す指標が使用される。この指標は重力と慣性力の合力ベクトルが地面と交差する点が安定領域内にあれば安定であるというものである。一方、腕を使ってする作業が様々考えられるが、バイオリンを演奏する等、高い位置精度が必要な場合がある。以上のように、腕に何らかの高い位置精度を要求する作業をさせつつ、ロボットが転倒しないようにバランスを保つことが必要である。

上述したように、関節角度は、関節角速度ベクトルを積分することで得られるが、当該積分による積分誤差により、精密な動作を行うことができないという問題点があった。そこで、本実施の形態においては、関節角度の目標値が分かっている場合は、当該関節を全身協調アルゴリズムから除外することで、正確な動作を実現する。先ず、このような全身協調運動をするロボットの一構成例について説明する。図３は、本実施の形態にかかるロボットを示すブロック図である。ロボット１は、制御部１０１、入出力部１０２、駆動部１０３、電源部１０４、及び外部記憶部１０５などを有している。

入出力部１０２は、周囲の映像を取得するためのＣＣＤ（Charge Coupled Device）などからなるカメラ１２１、周囲の音を集音するための１又は複数の内蔵マイク１２２、音声を出力してユーザと対話等を行なうためのスピーカ１２３、ユーザへの応答や感情等を表現するためのＬＥＤ１２４、タッチセンサなどからなるセンサ部１２５などを備える。

また、駆動部１０３は、モータ１３１、モータを駆動するドライバ１３２、モータドライバ１３２に指令値を与える協調制御部１３３等を有し、ユーザの指示などに従って脚部ユニット４ａ，４ｂや腕部ユニット３ａ，３ｂを動作させる。電源部１０４は、バッテリ１４１及びその放充電を制御するバッテリ制御部１４２を有し、各部に電源を供給する。

外部記憶部１０５は、着脱可能なＨＤＤ、光ディスク、光磁気ディスク等からなり、各種プログラムや制御パラメータなどを記憶し、そのプログラムやデータを必要に応じて制御部１０１内のメモリ（不図示）等に供給する。

制御部は、ＣＰＵ（Central Processing Unit）、ＲＯＭ（Read Only Memory）、ＲＡＭ（Random Access Memory）、無線通信用のインターフェースなどを有し、ロボット１の各種動作を制御する。そして、この制御部１０１は、例えばＲＯＭに格納された制御プログラムに従って、カメラ１２１により取得した映像を解析する画像認識モジュール１１１、画像認識結果に基づき、経路探索を行なう経路探索モジュール１１２、各種認識結果に基づきとるべき行動を選択する行動決定モジュール１１３、音声認識を行なう音声認識モジュール１１４などを有する。

特に、本実施の形態においては、協調制御部１３３にて、全身協調アルゴリズムにより関節角を算出するが、この際、全関節のうち一部の関節角は演算ではなく、予め決められた値を使用するようにする。これにより、積分誤差を低減し、より高速かつ正確にロボット１の全身協調制御を可能とする。

図４は、協調制御部の詳細を示す図である。当該指定値生成部１３３には、手先位置、足位置、重心位置など、基準座標系からの軌道（以下、タスクという。）を与える手段（不図示）によりタスクｒ_ｇ ^ｄ、ｒ_ｔａｓｋ ^ｄを入力する手段を有する。この協調制御部１３３は、関節角入力手段１１以外は従来と同様の構成を有する。すなわち、重心速度から関節角速度を求める関節角速度算出手段１２と、関節角速度算出手段１２が算出した角速度から１以上の関節角を第１関節角群として算出する積分手段１３と、予め定められた１以上の関節角を第２関節角群として入力する関節角入力手段１１と、第１関節角群及び第２関節角群から重心位置を算出する重心位置算出手段１４ａと、重心位置算出手段１４ａが算出した重心位置及び外部から入力される目標重心位置から重心速度を算出するフィードバック手段１５ａとを有する。

また、タスクに対応して、タスク位置算出手段１４ｂ及びフィードバック手段１５ｂを有する。本実施の形態においては、タスク位置算出手段１４ｂ及びフィードバック手段１５ｂを１つのみ示しているが、これらはタスクに応じて複数設けられるものである。

フィードバック手段１５ａは、目標重心位置を微分して重心速度を算出する微分手段２２ａと、重心位置算出手段１４ａが算出した重心位置と外部から入力される目標重心位置の差分を求める加算器２３ａと、加算器２３ａの結果をゲイン調整するゲイン調整手段２１ａと、微分手段２２ａにより算出された重心速度とゲイン調整手段２１ａの調整結果とを加算して関節角速度算出手段１２に入力する加算手段２４ａとを有する。なお、フィードバック手段１５ｂも同様に構成される。

関節角速度算出手段１２は、タスクを満たす、選択された任意の関節を除外した関節の角速度を計算する手段である。積分手段１３は、計算された関節角速度を積分する手段である。重心位置算出手段１４ａ、１４ｂは、積分された全関節角度からタスクを計算する手段である。フィードバック手段１５ａ、１５ｂは、計算されたタスクとタスクの目標値の偏差に基づいてフィードバックゲインを行う手段である。

以下、本実施の形態にかかる協調制御装置について更に詳細に説明する。本実施の形態においては、積分手段１３における積分誤差を補償するためフィードバック手段１５ａ、１５ｂを有している。ただし、フィードバックが収束したかの判定は偏差がある閾値以下になったかどうかで判定する。しかし、閾値を小さくすると収束するまでに時間がかかってしまう。上述したように、重心速度ベクトルは関節角速度ベクトル及びヤコビアンを用いて下記のように表わされる。

ここで、本実施の形態においては、位置の拘束を与える式（１）などにおいて、高精度を要求する位置に関わる関節に、明示的に値を与え、ヤコビアン演算から除外する。たとえば、右手と左手が協調して作業する場合、右手から左手にいたる関節の値を明示的に与える。θのうち、明示的に与える関数をθ_Ａ、それ以外の関数をθ_Ｂとすると、上記式（１）は、式（１３）のように表すことができる。

式（１３）を変形すると下記（１４）のようになる。

この式（１５）を、式（１）の代わりに用いて従来と同様に関節角を計算する。すなわち、従来の方法では、ロボットの全関節θがすべて全身協調のために使用されていたのに対し、本実施の形態においては、θより少ない数の関節θ_Ｂを使用して全身協調を行う。

本発明においては、全関節角を使用して全身協調制御を行う場合、通常は、関節角速度を積分して求める関節角を、精度が要求されるような一部の間接角については外部から明示的に与えることで、積分誤差を低減させる。すなわち、一部の間接角を強調制御から除外する。このことにより、精密な軌道位置を迅速に演算することができる。

なお、本発明は上述した実施の形態のみに限定されるものではなく、本発明の要旨を逸脱しない範囲において種々の変更が可能であることは勿論である。

例えば、上述の実施の形態では、ハードウェアの構成として説明したが、これに限定されるものではなく、任意の処理を、ＣＰＵ（Central Processing Unit）にコンピュータプログラムを実行させることにより実現することも可能である。この場合、コンピュータプログラムは、記録媒体に記録して提供することも可能であり、また、インターネットその他の伝送媒体を介して伝送することにより提供することも可能である。

本発明の実施の形態にかかるヒューマノイドロボットの一例を示す斜視図である。 ロボットの安定領域を示す図である。 本発明の実施の形態にかかるロボットを示すブロック図である。 本発明の実施の形態における協調制御部の詳細を示す図である。 重心位置と左手の軌道を示す図である。 モータを駆動するモータドライバに対して指令値を生成する指令値生成回路を示す図である。

符号の説明

１ ロボット
２ 頭部ユニット
３ａ，３ｂ 腕部ユニット
４ａ，４ｂ 脚部ユニット
５ 体幹部ユニット
１１ 関節入力部
１２ 関節角速度算出手段
１３ 積分手段
１４ａ 重心位置算出手段
１４ｂ タスク位置算出手段
１５ａ、１５ｂ フィードバック手段
２１ａ、２１ｂ 乗算部
２２ａ、２２ｂ 微分手段
２３ａ、２３ｂ、２４ａ、２４ｂ 加算器
１０１ 制御部
１０２ 入出力部
１０３ 駆動部
１０４ 電源部
１０５ 外部記憶部
１１１ 画像認識モジュール
１１２ 経路探索モジュール
１１３ 行動決定モジュール
１１４ 音声認識モジュール
１２１ カメラ
１２２ 内蔵マイク
１２３ スピーカ
１２５ センサ部
１３１ モータ
１３２ ドライバ
１３２ モータドライバ
１３３ 協調制御部
１３３ 指定値生成部
１４１ バッテリ
１４２ バッテリ制御部

Claims (7)

1. 重心速度から関節角速度を求める角速度算出手段と、
   前記角速度算出手段が算出した角速度から１以上の関節角を第１関節角群として算出する積分手段と、
   予め定められた１以上の関節角を第２関節角群として入力する関節角入力手段と、
   前記第１関節角群及び前記第２関節角群から重心位置を算出する重心位置算出手段と、
   重心位置算出手段が算出した重心位置及び外部から入力される目標重心位置から前記重心速度を算出するフィードバック手段とを有する協調制御装置。
2. 前記フィードバック手段は、前記目標重心位置を微分して重心速度を算出する微分手段と、前記重心位置算出手段が算出した重心位置と外部から入力される目標重心位置の差分をゲイン調整するゲイン調整手段と、前記微分手段により算出された重心速度とゲイン調整手段の調整結果とを加算して前記角速度算出手段に入力する加算手段とを有する
   ことを特徴とする請求項１記載の協調制御装置。
3. 前記重心位置算出手段及び前記フィードバック手段は、重心位置及び少なくとも１以上の基準座標系からの軌道に対応して設けられる
   ことを特徴とする請求項１記載の協調制御装置。
4. モータと、
   前記モータ駆動するモータドライバと、
   前記モータドライバに指令値として関節角を入力する制御手段とを有し、
   前記制御手段は、
   重心速度から関節角速度を求める角速度算出手段と、
   前記角速度算出手段が算出した角速度から１以上の関節角を第１関節角群として算出する積分手段と、
   予め定められた１以上の関節角を第２関節角群として入力する関節角入力手段と、
   前記第１関節角群及び前記第２関節角群から重心位置を算出する重心位置算出手段と、
   重心位置算出手段が算出した重心位置及び外部から入力される目標重心位置から前記重心速度を算出するフィードバック手段とを有するロボット。
5. 前記フィードバック手段は、前記目標重心位置を微分して重心速度を算出する微分手段と、前記重心位置算出手段が算出した重心位置と外部から入力される目標重心位置の差分をゲイン調整するゲイン調整手段と、前記微分手段により算出された重心速度とゲイン調整手段の調整結果とを加算して前記角速度算出手段に入力する加算手段とを有する
   ことを特徴とする請求項４載のロボット。
6. 前記重心位置算出手段及び前記フィードバック手段は、重心位置及び少なくとも１以上の基準座標系からの軌道に対応して設けられる
   ことを特徴とする請求項４記載のロボット。
7. ２足以上の脚部を有するロボットの制御方法であって、
   前記脚部を含む全身の関節角から重心位置を算出する重心位置算出工程と、
   目標重心位置及び前記重心位置算出工程で算出された重心位置から重心速度を算出するフィードバック工程と、
   前記重心速度に基づき関節角速度を算出する関節角速度算出工程と、
   前記関節角速度算出工程にて算出された関節角速度を積分して関節角を算出する積分工程と、
   前記積分工程で算出された関節角を予め指定される関節角と共に出力する出力工程とを有し、
   前記重心位置算出工程は、前記出力工程にて出力される関節角から前記重心位置を算出する、ロボットの制御方法。

Images