JP5399593B2 - ロボット、ロボットの制御装置、制御方法、及び制御プログラム - Google Patents

Description

本発明は、物体の搬送を行う動作を制御する、ロボット、ロボットの制御装置、制御方法、及び制御プログラムに関する。

近年、介護ロボット又は家事支援ロボットなどの家庭用ロボットが盛んに開発されるようになってきた。また、産業用ロボットでも、セル生産工場の広がりなどから、人と協働するロボットの開発が盛んに行われている。このような人と協働するロボットは、従来のように人間の居るエリアとロボット用の作業エリアとを区切って動作するロボットとは異なり、人間と共生する必要があるため、従来の産業用ロボットなどとは異なる安全性が求められる。

また、ロボットの一例としてのロボットアームは、搬送している物体及び自らの質量及び慣性モーメント等より、各関節の出力トルクを計算する。物体を把持しているときと、把持していないときとでは、各関節に求められるトルクが変わるため、物体を把持しているかどうかの状況に合わせ、目標トルクを適切に切り替えていく必要がある。

しかし、静止した状態で物体を把持した場合、把持しているか、把持していないかの２状態の質量及び慣性モーメント等の切り替えが起こる。このため、出力トルクが切り替わり、ロボットアームの目標位置は静止した状態にもかかわらず、ロボットアームが動いてしまうケースがあり、把持している物体にダメージを与える可能性があり、また、把持物等の落下、転倒の可能性もあり安全の面からも問題であった。

物体を把持するロボットにおける質量及び慣性モーメント等の切り替えに対し、特許文献１は、質量及び慣性モーメント等を切り替えて計算するのではなく、フィードフォワード項を追加することによりロボットを制御する制御装置を開示している（特許文献１参照）。

特開昭６３−８９１２号公報

しかしながら、特許文献１では、フィードフォワード項の計算を追加する、追加しないの２つの状態を切り替える動作が発生するが、切り替える動作に伴いロボットが動いてしまうケースに対する対策はされていないと言う課題があった。

本発明の目的は、上記従来の課題を解決し、物体を把持したり、把持から開放したりする際にダイナミクスパラメータ（例えば、把持物体の質量及び慣性モーメントなど）の切り替えが発生しても、物体又は構造物にダメージを与えたり、落下、転倒させてしまうことのない、把持物又は外環境等に対し優しく、安全なロボット、ロボットの制御装置、制御方法、及び制御プログラムを提供することにある。

上記目的を達成するために、本発明は以下のように構成する。

本発明の１つの態様によれば、関節を有するロボットであって、
上記ロボットの位置の時系列の情報又は速度の情報又は加速度の情報を取得する位置情報取得部と、
物体を把持していない状態の上記ロボットのダイナミクスパラメータと上記ロボットが上記物体を把持している状態の上記物体も含めたダイナミクスパラメータとを有する複数のダイナミクスパラメータの間で、運動方程式の重力項成分と慣性力項成分とを分離してそれぞれ切り換えるダイナミクスパラメータ切換部と、
上記ダイナミクスパラメータ切換部が上記ダイナミクスパラメータを切り換えた後の運動方程式と上記位置情報取得部で取得した上記ロボットの位置の時系列の情報又は速度の情報又は加速度の情報とに基づき、上記ロボットの関節トルクの目標値を目標関節トルクとして出力する目標トルク計算部とを備えて、
上記目標トルク計算部からの出力に基づき上記ロボットの動作を制御するロボットを提供する。

これらの概括的かつ特定の態様は、システム、方法、コンピュータプログラム並びにシステム、方法及びコンピュータプログラムの任意の組み合わせにより実現してもよい。

本発明の前記態様のロボット、ロボットの制御装置、制御方法、及び制御プログラムによれば、ダイナミクスパラメータのうち、慣性力項成分すなわち慣性行列と重力項成分とをそれぞれ分離してそれぞれ独立に切り換えることで、ダイナミクスパラメータの切換により発生してしまう動きの垂直方向及び水平方向という方向性をコントロール可能となる。

本発明のこれらと他の目的と特徴は、添付された図面についての実施形態に関連した次の記述から明らかになる。この図面においては、
図１は、本発明の第１実施形態におけるロボットの全体構成を示す図であり、 図２は、本発明の第１実施形態におけるロボットのインピーダンス制御手段の構成を示すブロック図であり、 図３は、本発明の第１実施形態におけるロボットによる人との協調搬送作業を説明する図であり、 図４は、本発明の第１実施形態における搬送作業の概略手順を説明する図であり、 図５は、本発明の第１実施形態における搬送作業の詳細手順を説明する図であり、 図６は、本発明の第１実施形態における上記ロボットにおけるインピーダンス制御手段の全体的な動作ステップを表すフローチャートであり、 図７は、本発明の第１実施形態における上記ロボットにおけるインピーダンス制御手段のダイナミクスパラメータ切換手段における動作ステップを表すフローチャートであり、 図８は、本発明の第１実施形態における上記ロボットにおけるインピーダンス制御手段のハンド制御手段における動作ステップを表すフローチャートであり、 図９は、本発明の第１実施形態における搬送作業時のダイナミクスパラメータ切換のタイミングを説明する図であり、 図１０は、本発明の第２実施形態における上記ロボットにおけるインピーダンス制御手段のダイナミクスパラメータ切換手段における動作ステップを表すフローチャートであり、 図１１は、本発明の第３実施形態におけるロボットのインピーダンス制御手段の構成を示すブロック図であり、 図１２は、本発明の第３実施形態における上記ロボットにおけるインピーダンス制御手段の弾性力制御手段における動作ステップを表すフローチャートであり、 図１３は、本発明の第３実施形態におけるロボットのインピーダンス制御手段の他の構成を示すブロック図であり、 図１４Ａは、本発明の第３実施形態における弾性力を説明する図であり、 図１４Ｂは、本発明の第３実施形態における弾性力を説明する図であり、 図１５は、本発明の第３実施形態における搬送作業時のダイナミクスパラメータ切換及び弾性力切換のタイミングを説明する図である。

以下に、本発明にかかる実施の形態を図面に基づいて詳細に説明する。

以下、図面を参照して本発明における実施形態を詳細に説明する前に、本発明の種々の態様について説明する。

本発明の第１態様によれば、関節を有するロボットであって、
上記ロボットの位置の時系列の情報又は速度の情報又は加速度の情報を取得する位置情報取得部と、
物体を把持していない状態の上記ロボットのダイナミクスパラメータと上記ロボットが上記物体を把持している状態の上記物体も含めたダイナミクスパラメータとを有する複数のダイナミクスパラメータの間で、運動方程式の重力項成分と慣性力項成分とを分離してそれぞれ切り換えるダイナミクスパラメータ切換部と、
上記ダイナミクスパラメータ切換部が上記ダイナミクスパラメータを切り換えた後の運動方程式と上記位置情報取得部で取得した上記ロボットの位置の時系列の情報又は速度の情報又は加速度の情報とに基づき、上記ロボットの関節トルクの目標値を目標関節トルクとして出力する目標トルク計算部とを備えて、
上記目標トルク計算部からの出力に基づき上記ロボットの動作を制御するロボットを提供する。

本発明の第２態様によれば、上記ダイナミクスパラメータ切換部は、上記物体を把持及び把持開放を含む作業の切り替え信号に基づき、上記ロボットの手先による物体把持時に上記ロボットの上記手先が移動困難に拘束された拘束状態とそれ以外の非拘束状態との間で切り換わるタイミングに応じて、上記複数のダイナミクスパラメータを、上記ロボットと上記ロボットが把持している物体との運動方程式の上記重力項成分と上記慣性力項成分とを分離してそれぞれ切り換える第１の態様に記載のロボットを提供する。

本発明の第３態様によれば、上記ダイナミクスパラメータ切換部は、上記位置情報取得部で取得した上記ロボットの位置の時系列の情報又は速度の情報又は加速度の情報に基づき、上記ロボットの上記手先が上記非拘束状態になったときに上記慣性力項成分のダイナミクスパラメータのみを切換える第２の態様に記載のロボットを提供する。

本発明の第４態様によれば、上記ダイナミクスパラメータ切換部は、上記ロボットが上記手先で上記物体を把持し、把持している物体を設置後、把持を開放し、上記ロボットの上記手先が上記非拘束状態になったときに上記慣性力項成分のダイナミクスパラメータのみを切換える第３の態様に記載のロボットを提供する。

本発明の第５態様によれば、上記ダイナミクスパラメータ切換部は、上記ロボットが上記物体を把持し、搬送し、上記把持している物体を設置するときに上記把持又は設置のために位置決めをするため搬送時よりも低速の動作になったときに上記慣性力項成分のダイナミクスパラメータのみを切換える第１の態様に記載のロボットを提供する。

本発明の第６態様によれば、上記ダイナミクスパラメータ切換部は、複数段に分離して重力項成分のダイナミクスパラメータを切換える第１〜５のいずれか１つの態様に記載のロボットを提供する。

本発明の第７態様によれば、上記ダイナミクスパラメータ切換部は、上記慣性力項成分のダイナミクスパラメータを切換えるとき、上記慣性力項成分と共に、上記運動方程式の遠心力及びコリオリ力項成分のダイナミクスパラメータをも切換える第１〜６のいずれか１つの態様に記載のロボットを提供する。

本発明の第８態様によれば、上記ロボットは、弾性体アクチュエータで駆動されるロボットアームを備え、
さらに上記ロボットの上記弾性体アクチュエータにより発生する上記ロボットアームの手先の弾性力を制御する弾性力制御部を備え、
上記弾性力制御部は、上記ダイナミクスパラメータ切換部によるダイナミクスパラメータ切換時に発生する上記ロボットアームの上記手先の動きによる影響を抑制する方向に弾性力をかける第１の態様に記載のロボットを提供する。

本発明の第９態様によれば、上記ロボットは弾性体アクチュエータで駆動されるロボットアームを備え、
さらに上記ロボットの上記弾性体アクチュエータにより発生する上記ロボットアームの手先の弾性力を制御する弾性力制御部を備え、
上記弾性力制御部は、上記手先で把持している物体を設置した後、慣性力項成分のダイナミクスパラメータを切換えるより前に、上記手先の拘束が解除される方向に弾性力をかけるよう制御する第８の態様に記載のロボットを提供する。

本発明の第１０態様によれば、関節を有するロボットの制御装置であって、
上記ロボットの位置の時系列の情報又は速度の情報又は加速度の情報を取得する位置情報取得部と、
物体を把持していない状態の上記ロボットのダイナミクスパラメータと上記ロボットが上記物体を把持している状態の上記物体も含めたダイナミクスパラメータとを有する複数のダイナミクスパラメータの間で、運動方程式の重力項成分と慣性力項成分とを分離してそれぞれ切り換えるダイナミクスパラメータ切換部と、
上記ダイナミクスパラメータ切換部が上記ダイナミクスパラメータを切り換えた後の運動方程式と上記位置情報取得部で取得した上記ロボットの位置の時系列の情報又は速度の情報又は加速度の情報とに基づき、上記ロボットの関節トルクの目標値を目標関節トルクとして出力する目標トルク計算部とを備えて、
上記目標トルク計算部からの出力に基づき上記ロボットの動作を制御するロボットの制御装置を提供する。

本発明の第１１態様によれば、関節を有するロボットの制御方法であって、
上記ロボットの位置の時系列の情報又は速度の情報又は加速度の情報を位置情報取得部で取得し、
物体を把持していない状態の上記ロボットのダイナミクスパラメータと上記ロボットが上記物体を把持している状態の上記物体も含めたダイナミクスパラメータとを有する複数のダイナミクスパラメータの間で、運動方程式の重力項成分と慣性力項成分とを分離してそれぞれダイナミクスパラメータ切換部で切り換え、
上記ダイナミクスパラメータ切換部が上記ダイナミクスパラメータを切り換えた後の運動方程式と上記位置情報取得部で取得した上記ロボットの位置の時系列の情報又は速度の情報又は加速度の情報とに基づき、上記ロボットの関節トルクの目標値を目標関節トルクとして目標トルク計算部で出力して、
上記目標トルク計算部からの出力に基づき上記ロボットの動作を制御するロボットの制御方法を提供する。

本発明の第１２態様によれば、関節を有するロボットの制御プログラムであって、
コンピュータを、
上記ロボットの位置の時系列の情報又は速度の情報又は加速度の情報を取得する位置情報取得部としての機能と、
物体を把持していない状態の上記ロボットのダイナミクスパラメータと上記ロボットが上記物体を把持している状態の上記物体も含めたダイナミクスパラメータとを有する複数のダイナミクスパラメータの間で、運動方程式の重力項成分と慣性力項成分とを分離してそれぞれ切り換えるダイナミクスパラメータ切換部としての機能と、
上記ダイナミクスパラメータ切換部が上記ダイナミクスパラメータを切り換えた後の運動方程式と上記位置情報取得部で取得した上記ロボットの位置の時系列の情報又は速度の情報又は加速度の情報とに基づき、上記ロボットの関節トルクの目標値を目標関節トルクとして出力する目標トルク計算部としての機能と、
上記目標トルク計算部からの出力に基づき上記ロボットの動作を制御する機能とを実現させるためのロボットの制御プログラムを提供する。

以下に、本発明にかかる実施の形態を図面に基づいて詳細に説明する。

（第１実施形態）
図１は本発明の第１実施形態におけるロボット１の構成を示す。ロボット１は、多関節ロボットアーム５と、多関節ロボットアーム５の動作を制御する制御装置２とを備えている。

制御装置２は、ハードウェア的には一般的なパーソナルコンピュータにより構成されている。そして、制御装置２のうちのインピーダンス制御手段（インピーダンス制御部又はロボットアーム制御部）４の入出力ＩＦ１９を除く部分は、パーソナルコンピュータで実行される制御プログラム１７としてソフトウェア的に実現される。よって、例えば、それぞれの動作を構成するステップを有するコンピュータプログラムとして、記憶装置（ハードディスク等）などの記録媒体に読み取り可能に記憶させ、そのコンピュータプログラムをコンピュータの一時記憶装置（半導体メモリ等）に読み込んでＣＰＵを用いて実行することにより、上記した各ステップを実行することができる。

入出力ＩＦ１９は、パーソナルコンピュータのＰＣＩバスなどの拡張スロットルに接続された、Ｄ／Ａボード２０と、Ａ／Ｄボード２１と、カウンタボード２２とにより構成される。

ロボット１の多関節ロボットアーム５の動作を制御するための制御プログラム１７が実行されることにより、制御装置２が機能する。

ロボットアーム５の各関節（第１〜第５関節）１１、１２（１２ａ，１２ｂ，１２ｃ，１２ｄ）、１３、１４、１５の関節角度センサー又は位置情報取得手段（位置情報取得部）の一例である各エンコーダ４２より出力される関節角度情報は、カウンタボード２２を通じて制御装置２にそれぞれ取り込まれる。制御装置２は、取り込んだ関節角度情報に基づいて、各関節１１、１２、１３、１４、１５における関節軸周りの回転動作の制御指令値をそれぞれ算出する。算出された各制御指令値は、Ｄ／Ａボード２０を通じてモータードライバ１８に与えられ、モータードライバ１８から送られた各制御指令値に従って、ロボットアーム５の各関節１１、１２、１３、１４、１５のそれぞれのモータ４１が駆動される。位置情報取得手段は、ロボットアーム５の位置の時系列の情報を取得する機能を有している。
ロボットアーム５は、５自由度の多リンクマニピュレータであり、物体を把持するためのハンド（多関節ロボットアーム５の手先の一例）６と、前腕リンク８と、肘ブロック１６と、一対の上腕リンク９ａ、９ｂと、第１関節支柱２４と、台部１０とを有して構成されている。

前腕リンク８は、先端に、ハンド６が取付けられる手首部７が第４関節１４の関節軸周りに回転可能に接続されている。

肘ブロック１６の一端には、前腕リンク８の基端が第３関節１３の関節軸周りに回転可能に連結されている。

一対の上腕リンク９ａ、９ｂは、それらの各一端が、肘ブロック１６の他端にそれぞれ第２関節１２のうちの２つの関節１２ｃ、１２ｄの関節軸周りに回転可能に接続された平行リンク構造で構成されている。

第１関節支柱２４は、台部１０と支持部材１２４とで、上下方向沿いに位置しかつ第１関節１１の関節軸周りに回転可能に支持されている。第１関節支柱２４の上端近傍部には、上腕リンク９ａ、９ｂの各他端がそれぞれ第２関節１２のうちの２つの関節１２ａ、１２ｂの関節軸周りに回転可能に接続されている。詳しくは、第１関節支柱２４の下端は、台部１０に第１関節１１の関節軸周りに回転可能に支持され、第１関節支柱２４の上端は、台部１０に立設された支持部材１２４の支持部１２４ｂの上端から張り出した支持部材１２４の上端支持部１２４ｃに第１関節１１の関節軸周りに回転可能に支持されている。

第２関節１２（１２ａ、１２ｂ、１２ｃ、１２ｄ）の関節軸周りでの回転により、第３関節１３の関節軸が第１関節１１の関節軸に対し常に平行を保つように、肘ブロック１６が第２関節１２ａ及び第２関節１２ｂ周りの回転運動を行うようにしている。

このように構成することにより、ロボットアーム５は、合計５個の軸周りに回転可能として上記５自由度の多リンクマニピュレータを構成している。

手首部７は、大括弧（］）形状の第１ブラケット部７ａと、逆Ｔ字形状の上部と一対のＬ字状の下部とで構成される第２ブラケット部７ｂとが互いに組み合わされてハンド６を構成している。すなわち、第１ブラケット部７ａの上端中央部が、前腕リンク８の先端に第４関節１４の関節軸周りに回転可能に連結されている。第１ブラケット部７ａの両端部の内側には、第２ブラケット部７ｂの逆Ｔ字形状の上部の両端部が第５関節１５の関節軸周りに回転可能に連結されている。第２ブラケット部７ｂの逆Ｔ字形状の上部の中央部には、後述するように、操作ハンドル４０が取り付けられている。第２ブラケット部７ｂには、薄型テレビなどのパネルなどの物体と係合可能なフィンガー３７ａ、３７ｂ、３７ｃ、３７ｄ、３７ｅを、逆Ｔ字形状の上部の上端と両端部近傍部と一対のＬ字状の下部の下端との合計５箇所にそれぞれ備えている。また、第２ブラケット部７ｂの操作ハンドル４０の近傍にはハンドボタン４３が配置されており、ハンドボタン４３を押すことにより、ハンドボタン４３が押されたかどうかの信号Ｓ_ｈｂがハンド制御手段（ハンド制御部）３４に出力され、かつ、図示しないモータの駆動で一つの方向に対して進退させて、フィンガー３７ａとフィンガー３７ｂの間隔と、フィンガー３７ｃとフィンガー３７ｄ（フィンガー３７ｅ）の間隔とをそれぞれ変更可能であり、ハンド６の開閉動作として機能する。この結果、例えば、第２ブラケット部７ｂの逆Ｔ字形状の上部の上端のフィンガー３７ｃと両端部近傍部のフィンガー３７ａ、３７ｂとで、薄型テレビなどのパネルなどの矩形板状の物体の上端縁と両側縁とを支持し、第２ブラケット部７ｂの一対のＬ字状の下部の下端のフィンガー３７ｄ、３７ｅで薄型テレビなどのパネルなどの矩形板状の物体の下端縁を支持可能として、ハンド６で薄型テレビなどのパネルなどの矩形板状の物体を安定して把持あるいは開放できるようにしている。よって、ハンド６は、互いに直交し、かつ、上下方向沿いに配置された第４関節１４と上下方向と直交する横方向沿いに配置された第５関節１５との２つの回転軸を有しており、台部１０に対して、ハンド６の相対的な姿勢（向き）を変化させて、ハンド６で把持した物体の相対的な姿勢（向き）を変化させることができる。

各軸の回転部分を構成する各関節１１、１２ａ、１３、１４、１５には、各関節１１、１２ａ、１３、１４、１５の一方の部材に設けられた回転駆動装置（本第１実施形態ではモータ４１）と、モータ４１の回転軸の回転位相角（すなわち関節角）を検出するエンコーダ４２とを備える。そして、モータ４１の回転軸が関節の他方の部材に連結されて上記回転軸を正逆回転させることにより、他方の部材を一方の部材に対して各関節軸周りに回転可能とする。回転駆動装置は、後述するモータードライバ１８により駆動制御される。本第１実施形態では、回転駆動装置の一例としてのモータ４１とエンコーダ４２とは、ロボットアーム５の各関節１１、１２ａ、１３、１４、１５の内部に配設されている。

図１の参照符号３５は台部１０に対して相対的な位置関係が固定された絶対座標系であり、参照符号３６はハンド６に対して相対的な位置関係が固定された手先座標系である。絶対座標系３５から見た手先座標系３６の原点位置Ｏ_ｅ（ｘ、ｙ、ｚ）をロボットアーム５の手先位置とし、絶対座標系３５から見た手先座標系３６の姿勢をロール角とピッチ角とで表現した（φ、θ）をロボットアーム５のハンド６の手先姿勢とし、ハンド６の手先位置及び姿勢ベクトルｒを、ｒ＝［ｘ，ｙ，ｚ，φ，θ］^Ｔと定義する。厳密には、位置及び姿勢ベクトルｒは時系列情報（時間関数）ｒ（ｔ）＝［ｘ（ｔ），ｙ（ｔ），ｚ（ｔ），φ（ｔ），θ（ｔ）］^Ｔであり、一階微分すれば速度、二階微分すれば加速度情報が取得できる。

なお、この実施形態では、ロボットアーム５の位置の時系列の情報を取得する位置情報取得手段（位置情報取得部）の一例として、エンコーダ４２を使用している。この場合は、エンコーダ４２のセンシングした信号が位置に関する一次情報であることから、エンコーダ４２を位置情報取得手段の一例として取り扱うものであって、位置情報取得手段で取得した位置の情報に対して、ロボットアーム制御手段（ロボットアーム制御部）４側、すなわち、インピーダンス制御部側で、一階微分して速度の情報を取得し、二階微分して加速度の情報を取得するようにしてもよい。また、位置情報取得手段をロボットアーム５の位置の時系列の情報のみならず、速度の情報又は加速度の情報を取得する手段（部）として取り扱う場合には、エンコーダ４２でセンシングし、カウンタ２２でインピーダンス制御手段４に取込み、制御プログラムで関節角度ｑに変換し、順運動学計算手段（順運動学計算部）２６で位置及び姿勢に変換するところまでを含めて、１つの手段として考えればよい。

ロボットアーム５のハンド６の手先位置及び姿勢を制御する場合には、手先位置及び姿勢ベクトルｒを目標手先位置及び姿勢ベクトルｒ_ｄに追従させることになる。

操作ハンドル４０は、ハンド６と、外力取得手段（外力取得部又は外力取得装置）の一例として機能する力センサー３を介して接続されており、力センサー３に中央部が固定されたＨ型の支持体４０ｂに固定された一対の上下方向沿いの棒状握り部４０ａで構成されて、人３９は操作ハンドル４０の一対の握り部４０ａを直接握り、力をかけることでロボット５を操作することができる。操作ハンドル４０とロボットアーム５の間には力センサー３が配設されており、人３９が操作するときの力Ｆ_ｓを力センサー３で検出して、後述する力変換手段（力変換部）３０へ出力することができる。

次に、図２を使い、インピーダンス制御手段４の詳細について説明する。図２において、参照符号５は図１に示したロボットアームである。ロボットアーム５からは各関節の関節軸のエンコーダ４２により計測された関節角の現在値（関節角度ベクトル）ｑ＝［ｑ_１，ｑ_２，ｑ_３，ｑ_４，ｑ_５］^Ｔが出力され、カウンタボード２２によりインピーダンス制御手段４に取り込まれる。ただし、ｑ_１，ｑ_２，ｑ_３，ｑ_４，ｑ_５は、それぞれ、第１関節１１、第２関節１２ａ、第３関節１３、第４関節１４、第５関節１５の関節角度である。

また、ロボットアーム５の力センサー３からは、力センサー３により計測された外力（計測値）Ｆ_Ｓ＝［ｆ_ｘ，ｆ_ｙ，ｆ_ｚ，ｎ_ｘ，ｎ_ｙ，ｎ_ｚ］^Ｔが出力され、Ａ／Ｄボード２１によりインピーダンス制御手段４に取り込まれる。ここで、ｆ_ｘ，ｆ_ｙ，ｆ_ｚは手先座標系３６の互いに直交する３方向（ｘ軸、ｙ軸、ｚ軸方向）の方向別の力の成分である。また、ｎ_ｘ、ｎ_ｙ，ｎ_ｚは手先座標系３６の互いに直交する３方向まわりの回転モーメントである。

目標軌道生成手段（目標軌道生成部）２３は、目標とするロボットアーム５の動作を実現するための手先位置及び姿勢目標ベクトルｒ_ｄを出力する。目標とするロボットアーム５の動作は、目的とする作業に応じて、事前に、それぞれの時間（ｔ＝０、ｔ＝ｔ_１、ｔ＝ｔ_２、・・・）でのポイント毎の位置（ｒ_ｄ０、ｒ_ｄ１、ｒ_ｄ２、・・・）が時系列データとして与えられている。目標軌道生成手段２３は、多項式補間を使用し、各ポイント間の軌道を補完し、手先位置及び姿勢目標ベクトルｒ_ｄを生成する。

インピーダンス計算手段（インピーダンス計算部）２５は、ロボットアーム５に機械インピーダンス設定値へのロボットアーム５の機械インピーダンスの値の制御を実現する機能を果たす部分である。インピーダンス計算手段２５は、ロボットアーム５が目標軌道生成手段２３の生成する目標軌道に沿うよう位置制御により単独で動作する場合は、０を出力する。一方、ロボットアーム５と人間が協働作業をする場合には、インピーダンス計算手段２５は、設定したインピーダンスパラメータＭ_Ｉ、Ｄ_Ｉ、及び、Ｋ_Ｉ（慣性Ｍ_Ｉ、粘性Ｄ_Ｉ、及び、剛性Ｋ_Ｉ）と、後述する力変換手段（力変換部）３０へ入力される外力Ｆ＝［ｆ_ｘ，ｆ_ｙ，ｆ_ｚ，ｎ_φ，ｎ_θ］^Ｔと、それぞれのエンコーダ４２からの関節角度データ（関節角度ベクトルｑ）とにより、ロボットアーム５に機械インピーダンスを実現するための手先位置及び姿勢目標補正出力Δｒ_ｄを以下の式（１）により計算して出力する。手先位置及び姿勢目標補正出力Δｒ_ｄは、目標軌道生成手段２３の出力する手先位置及び姿勢目標ｒ_ｄに第１演算部５１で加算され、手先位置及び姿勢補正目標ベクトルｒ_ｄｍが生成される。ただし、ｎ_φ、ｎ_θはロール軸、ヨー軸回りの回転モーメントである。

ただし、

であり、ｓはラプラス演算子である。

順運動学計算手段（順運動学計算部）２６には、ロボットアーム５のそれぞれの関節軸のエンコーダ４２から出力された各関節角の現在値ｑである関節角度ベクトルｑがカウンタボード２２を介して入力される。順運動学計算手段２６では、ロボットアーム５の関節角度ベクトルｑから手先位置及び姿勢ベクトルｒへの変換の幾何学的計算を行う。

第２演算部５２は、順運動学計算手段２６により計算される手先位置及び姿勢ベクトルｒと、第１演算部５１で生成された手先位置及び姿勢補正目標ベクトルｒ_ｄｍとの誤差ｒ_ｅを求め、求めた誤差ｒ_ｅを位置誤差補償手段（位置誤差補償部）２７に出力する。

位置誤差補償手段２７は、第２演算部５２で求められた誤差ｒ_ｅに基づいて位置誤差補償出力ｕ_ｒｅを求め、求めた位置誤差補償出力ｕ_ｒｅを目標トルク計算手段（目標トルク計算部）２８に出力する。

目標トルク計算手段２８は、エンコーダ４２からの関節角度ベクトルｑと、位置誤差補償出力ｕ_ｒｅからの位置誤差補償出力ｕ_ｒｅと、ロボットアームの運動方程式に基づく計算式（５）、すなわち、

とを用いて、ロボットアーム５の各関節で発生すべき目標トルク（目標関節トルク）τ_ｄを計算して出力する。ただし、Ｍ（ｑ）は慣性行列（慣性力項）、

は遠心力・コリオリ力項、ｇ（ｑ）は重力項であり、
ロボットアーム５を構成する各リンク構造、ハンド６で把持した物体の重量、及び、慣性モーメントといった値により構成されるダイナミクスパラメータである。なお、遠心力・コリオリ力項は任意であり、少なくとも慣性行列（慣性力項）と重力項でダイナミクスパラメータを構成する。また、Ｊ_ｒ（ｑ）は式（６）、すなわち、

の関係を満たすヤコビ行列である。

ダイナミクスパラメータ切換手段（ダイナミクスパラメータ切換部）５０は、目標トルク計算手段２８において計算される式（５）のダイナミクスパラメータ

を、ロボットアーム５が物体を把持していない状態のロボットアーム５とハンド６だけの場合のダイナミクスパラメータ

又は、ロボットアーム５が物体を把持している状態のロボットアーム５とハンド６に物体も加えた場合のダイナミクスパラメータ

に、慣性行列、遠心力・コリオリ力項、重力項ごとに分離して独立に切り換える機能を有する。また、ダイナミクスパラメータ切換手段５０からは、ダイナミクスパラメータ切換完了信号Ｓ_ｄｐがハンド制御手段３４に出力される。

ダイナミクスパラメータ切換手段５０によるダイナミクスパラメータの切換動作は、ステップ的に行うと制御が不連続となり不安定になるため、例えば１００ｍｓｅｃのような短い時間をかけて連続的に変化させることで安定性を確保する。

トルク制御手段４８は、目標トルク計算手段２８で求められた目標トルクτ_ｄと各関節を駆動するモータドライバ１８で検出されたモータ４１に流れる電流ｉに基づいて式（７）、すなわち、

を用いて、モータードライバ１８への電圧指令値ｖを計算する。ただし、Ｋ_τはトルク制御のゲイン、Ｋ_Ｍはトルク定数である。

電圧指令値ｖは、トルク制御手段４８からＤ／Ａボード２０を介してモータードライバ１８に電圧指令値として与えられ、モータードライバ１８でそれぞれのモータ４１が駆動されて、各関節軸が正逆回転駆動されてロボットアーム５が動作する。

ハンド制御手段３４は、ハンドボタン４３が押されたかどうかの信号Ｓ_ｈｂとダイナミクスパラメータ切換手段５０からのダイナミクスパラメータ切換完了信号Ｓ_ｄｐとに基づき、ハンド制御信号Ｓ_ｈｃを出力し、ハンド６の開閉動作を制御する。また、ハンド制御手段３４は、ダイナミクスパラメータ切換手段５０へ、ハンド６の把持又は開放の動作状態を通知するため、ハンド動作状態信号Ｓ_ｈｓを出力する。

以上のように構成されるインピーダンス制御手段４の基本構成に関して動作の基本原理を説明する。

動作の基本は、位置誤差補償手段２７による手先位置及び姿勢誤差ｒ_ｅのフィードバック制御（位置制御）であり、図２の点線で囲まれた部分が位置制御系２９になっている。位置誤差補償手段２７として、例えば、ＰＩＤ補償器を使用すれば、手先位置及び姿勢誤差ｒ_ｅが０に収束するように制御が働き、目標とするロボットアーム５のインピーダンス制御動作が実現することができる。

インピーダンス制御を行う場合、上記説明した位置制御系２９に対し、インピーダンス計算手段２５から出力された手先位置及び姿勢目標補正出力Δｒ_ｄが、目標軌道生成手段２３から出力された手先位置及び姿勢目標ｒ_ｄに第１演算部５１で加算されて、手先位置及び姿勢の目標値の補正が行われ、この補正された手先位置及び姿勢の目標値が手先位置及び姿勢補正目標ベクトルｒ_ｄｍとして入力される。このために、上記した位置制御系２９は、手先位置及び姿勢の目標値が本来の値より微妙にずれることになり、結果的に機械インピーダンスが実現される。以上の図２の一点鎖線で囲まれた部分は、位置制御ベースのインピーダンス制御系４９と呼ばれる構成であり、このインピーダンス制御系４９により、慣性Ｍ_Ｉ、粘性Ｄ_Ｉ、及び、剛性Ｋ_Ｉの機械インピーダンスが実現される。インピーダンス制御手段４は、動作制御手段、又は動作制御部の一例として機能する。

以上のインピーダンス制御を利用すれば、図３に示すような、人３９とロボット１のロボットアーム５との物体３８の協調搬送のような協働作業が可能となる。人３９が物体３８を移動しようとして、一対の操作ハンドル４０の棒状握り部４０ａに両手で力をかけると、操作ハンドル４０を通じて力がロボット１のロボットアーム５に伝わる。上記ロボットアーム５に伝わった力が、ロボットアーム５の力センサー３により、外力Ｆ_ｓとして検出される。インピーダンス制御手段４への入力となる外力Ｆ_ｓは、操作ハンドル４０の重力を考慮するとともに、手先座標系３６の原点Ｏ_ｅと力センサー３の測定面４５の位置とが異なることを考慮するため、力変換手段３０において、力センサー３により検出された外力Ｆ_ｓを基に式（８）で計算を行い、この計算により算出される外力Ｆを使用する。

．．．．．（８）

ただし、ｍは操作ハンドル４０の質量、ｇは重力加速度、^０Ｒ_ｅは手先座標系３６から絶対座標系３５へ姿勢を変換する回転行列、ｌ_ｓｅは手先座標系３６の原点Ｏ_ｅから力センサー３の測定面４５までの距離、ｌ_ｓｈは操作ハンドル４０の重心から力センサー３の測定面４５までの距離である。なお、先に述べたように、ピッチ角θは手先座標系のｙ軸回りの回転角度である。式（８）で計算される外力Ｆを入力にインピーダンス制御を行えば、ロボットアーム５は人３９がかけた力の方向に沿うように動作するため、協調搬送が実現する。なお、操作ハンドル４０の質量ｍ、及び、重力加速度ｇは、力変換手段３０に予め記憶されている。

本発明の第１実施形態のロボット１の制御装置２の特徴は、以上の基本的なインピーダンス制御手段４の構成に加え、ダイナミクスパラメータ切換手段５０を有するところにあり、以下、詳細について図４及び図５に示す物体搬送作業を行う場合を例に説明する。図４は物体搬送作業の全体の流れの概略手順を説明する図であり、図５は物体搬送作業をステップ毎に区切った詳細手順を説明する図で、図５ではロボット１の内、ハンド６のみ表示している。また、ここでは、物体３８の一例として薄型テレビ４６を使用している。

図４及び図５での物体搬送作業内容は、以下の通りである。

まず、ハンドボタン４３を押して、図４の左側の第１作業台３１に伏して置かれた薄型テレビ４６をハンド６で把持し（図５（ａ）→（ｂ）参照）、持ち上げる（図５（ｃ）参照）。

その後、ｙ軸周りのθ方向周りに９０°回転する（図４矢印Ａ参照）ことで、上下方向に立てた状態にし、第１作業台３１の上方の位置から第２作業台３２の上方へと薄型テレビ４６をハンド６で水平移動し（図４の矢印Ｂ参照）、ハンド６で把持した薄型テレビ４６を第２作業台３２に配設されたＹ字状のスタンド３３に位置合わせをする（図５（ｃ）→（ｄ）参照）。

その後、ハンド６で把持した薄型テレビ４６を下降させ、薄型テレビ４６の底面に配設された一対の挿入口４７に、第２作業台３２のスタンド３３の一対の突起３３ａを挿入する（図４の矢印Ｃ参照、図５（ｅ）参照）。

その後、ハンドボタン４３を押してハンド６による把持を開放し、薄型テレビ４６からハンド６を分離し、物体搬送作業が完了する（図５（ｆ）参照）。

以上が上記した物体搬送作業である。

以上の物体搬送作業の流れの中で図５（ｂ）の状態においては、その後に薄型テレビ４６の持ち上げ動作を行うために、ダイナミクスパラメータ切換手段５０により、ダイナミクスパラメータを、ロボットアーム５が物体４６を把持していない状態のロボットアーム５とハンド６だけの場合のダイナミクスパラメータ

から、ロボットアーム５が物体６を把持している状態のロボットアーム５とハンド６に物体４６も加えた場合のダイナミクスパラメータ

に切り換える必要がある。

また、図５（ｅ）の状態においては、その後に薄型テレビ４６からハンド６を分離するためにイナミクスパラメータを、ロボットアーム５が物体４６を把持している状態のロボットアーム５とハンド６に物体４６も加えた場合のダイナミクスパラメータ

から、ロボットアーム５のハンド６が物体４６を把持していない状態のロボットアーム５とハンド６だけの場合のダイナミクスパラメータ

にダイナミクスパラメータ切換手段５０により切り換える必要がある。

以上のダイナミクスパラメータ切換手段５０によるダイナミクスパラメータの切換動作により、薄型テレビ４６の重量などの作業対象である物体の情報が制御に反映され、一連の作業動作が可能となる。

しかしながら、ダイナミクスパラメータにはモデル化誤差が含まれていたり、インピーダンス制御手段４の制御結果には制御誤差が存在するため、ダイナミクスパラメータの切換動作を単純に一度で全パラメータを切り換えると、切換前後で制御結果にズレが生じロボットアーム５が動いてしまう可能性がある。

例えば、図５（ｂ）の状態ではハンド６が水平方向に動いてしまい、薄型テレビ４６と第１作業台３１が擦れ合い、薄型テレビ４６の表面を傷つけてしまう可能性がある。

また、図５（ｅ）の状態ではハンド６が水平方向に動こうとすると、スタンド３３に過度の力が掛かかることでスタンド３３が破損し、ハンド６の解放後に薄型テレビ４６がスタンド３３から落下する可能性がある。また、ハンド６が水平方向に動こうとすると、薄型テレビ４６と第２作業台３２とのバランスが崩れ、薄型テレビ４６が転倒してしまう可能性もある。

ここで図５（ｂ）の状態は、薄型テレビ４６が第１作業台３１に接触した状態であり、ハンド６は垂直下方に動くことができない状態にある。また、図５（ｅ）の状態は、薄型テレビ４６がスタンド３３に挿入された状態であり、ハンド６は水平方向及び垂直下方に動くことができない状態にある。こうした外環境等との接触があり、ある方向に動けない状態を、ここでは「拘束状態」と定義する。

こうした拘束状態において発生する課題を解決するため、ダイナミクスパラメータ切換手段５０は、図９に示すように、慣性行列と、遠心力・コリオリ力項と、重力項とごとに分離して、それぞれ独立に、切り換えるよう動作する。

図５（ｂ）の状態では、フィンガー３７ａとフィンガー３７ｂとの間及びフィンガー３７ｃとフィンガー３７ｄ（フィンガー３７ｅ）との間の閉動作が完了、すなわちハンド６による把持が完了した時点で、ダイナミクスパラメータ切換手段５０は、まず、重力項のみｇ_ｒ（ｑ）からｇ_ｒｏ（ｑ）に切り換える。

その後、薄型テレビ４６が所定距離（すなわち、少し、例えば５ｃｍ）だけ持ち上がったとき、すなわち、ハンド６がすべての方向に自由に動くことができる非拘束状態になったとき、ダイナミクスパラメータ切換手段５０は、慣性行列、遠心力・コリオリ力項を

から

に切り換える。

ダイナミクスパラメータの特性を考えれば、重力項による垂直方向の動きに対する影響は大きく、慣性行列と、遠心力・コリオリ力項との垂直方向の動きに対する影響は、相対的に小さい。

逆に、重力項による水平方向の動きに対する影響は小さく、慣性行列と、遠心力・コリオリ力項との垂直方向の動きに対する影響は、相対的に大きい。

したがって、上記のように分離して切り換えれば、ダイナミクスパラメータの切換により発生してしまう動きの垂直方向及び水平方向という方向性を、コントロール可能となる。

図５（ｂ）の状態において、まず、重力項のみ切り換えることにより、意図しない水平方向の動きを抑えることができ、薄型テレビ４６と第１作業台３１とが擦れ合い、薄型テレビ４６の表面を傷つけてしまうことを防ぐことができる。

その後、薄型テレビ４６と第１作業台３１とが分離したときに、慣性行列と、遠心力・コリオリ力項との切換を行えば、もし意図しない水平方向の動きが発生しても、薄型テレビ４６と第１作業台３１とが分離しているため、薄型テレビ４６の表面を傷つけることはない。

また、薄型テレビ４６を持ち上げる際には、慣性行列と、遠心力・コリオリ力項との切換を行うまでは、薄型テレビ４６を把持していない状態のダイナミクスパラメータで、薄型テレビ４６を把持した状態の動作を行うことになるため、水平方向の操作性の劣化が発生するが、持ち上げる方向は主に垂直方向であり、また、少し持ち上げるだけの短距離であるため、操作性が問題になることは少ない。

次に、図５（ｅ）の状態では、スタンド３３への挿入が完了した時点で、ダイナミクスパラメータ切換手段５０は、まず、重力項のみ、ｇ_ｒｏ（ｑ）からｇ_ｒ（ｑ）に切り換える。

その後、ハンドボタン４３を押してハンド６による把持を開放し、薄型テレビ４６より所定距離（すなわち、少し、例えば５ｃｍ）だけハンド６が離れ、非拘束状態になったところで、ダイナミクスパラメータ切換手段５０は、慣性行列と、遠心力・コリオリ力項とを

から

にそれぞれ切り換える。

図５（ｅ）の状態において、ダイナミクスパラメータ切換手段５０により、まず、重力項のみ切り換えることにより、意図しない水平方向の動きを抑えることができ、スタンド３３に過度の力が掛かり、破損してしまうことを防いだり、又は、第２作業台３２等の転倒を防ぐことができる。

その後、薄型テレビ４６とハンド６が分離したときに慣性行列と、遠心力・コリオリ力項とのそれぞれの切換をダイナミクスパラメータ切換手段５０で行えば、もし意図しない水平方向の動きが発生しても、薄型テレビ４６とハンド６とが分離しているため、スタンド３３に力が掛かることはない。

また、ハンド６を薄型テレビ４６から分離する際には、慣性行列と、遠心力・コリオリ力項との切換をダイナミクスパラメータ切換手段５０で行うまでは、薄型テレビ４６を把持した状態のダイナミクスパラメータで、薄型テレビ４６を把持していない状態の動作を行うことになるため、水平方向の操作性の劣化が発生するが、精密な位置決め動作ではなくハンド６を分離するだけの動作であり、また、ハンド６を分離するまでの短距離であるため、操作性が問題になることは少ない。

図６は、上述した原理に基づく制御プログラムによるロボットアーム制御時の動作ステップを説明するフローチャートである。

ステップＳ１では、ロボットアーム５の動作制御を行うときロボットアーム５の各関節の関節軸のそれぞれのエンコーダ４２により計測された関節角度データ（関節角度ベクトルｑ）が制御装置２に取り込まれる。

次いで、ステップＳ２では、ロボットアーム５からの関節角度データ（関節角度ベクトルｑ）から、ロボットアーム５の現在（ロボットアーム５の動作制御を行うとき）の手先位置及び姿勢ベクトルｒが順運動学計算手段２６により計算される（順運動学計算手段２６での処理）。

次いで、ステップＳ３では、力センサー３の計測値である外力Ｆ_ｓが力変換手段３０に取り込まれ、外力Ｆ_ｓと式（８）とに基づき、外力Ｆが力変換手段３０で計算される。この結果、力センサー３の計測値である外力Ｆ_ｓが外力Ｆに力変換手段３０で変換される。

次いで、ステップＳ４では、インピーダンスパラメータＭ_Ｉ、Ｄ_Ｉ、及び、Ｋ_Ｉと、それぞれのエンコーダ４２からの関節角度データ（関節角度ベクトルｑ）と、力変換手段３０において変換された外力Ｆとから、手先位置及び姿勢目標補正出力Δｒ_ｄが、インピーダンス計算手段２５により計算される（インピーダンス計算手段２５での処理）。

次いで、ステップＳ５では、目標軌道生成手段２３からの手先位置及び姿勢目標ベクトルｒ_ｄとインピーダンス計算手段２５からの手先位置及び姿勢目標補正出力Δｒ_ｄとの和が第１演算部５１で計算される。第１演算部５１で計算されて求められた手先位置及び姿勢補正目標ベクトルｒ_ｄｍと、順運動学計算手段２６からの現在の手先位置及び姿勢ベクトルｒとの差である手先位置及び姿勢の誤差ｒ_ｅが、第２演算部５２で計算される。

次いで、ステップＳ６では、第２演算部５２で計算されて求められた手先位置及び姿勢の誤差ｒ_ｅが位置誤差補償手段２７に入力されて、位置誤差補償手段２７において、位置誤差補償出力ｕ_ｒｅが求められる（位置誤差補償手段２７での処理）。位置誤差補償手段２７の具体例としてはＰＩＤ補償器が考えられる。位置誤差補償手段２７では、定数の対角行列である比例、微分、積分の３つのゲインを適切に調整することにより、位置誤差が０に収束するように制御が働く。

次いで、ステップＳ７では、目標トルク計算手段２８において、位置誤差補償手段２７から位置誤差補償出力ｕ_ｒｅが入力されて、関節角度ベクトルｑとダイナミクスパラメータ切換手段５０で決定されたダイナミクスパラメータとに基づき、式（５）を使って目標トルクτ_ｄが計算される（目標トルク計算手段２８での処理）。

次いで、ステップＳ８では、目標トルク計算手段２８で計算された目標トルクτ_ｄとモータ４１に流れる電流ｉがトルク制御手段４８に入力されて、式（７）を使って電圧指令値ｖが計算される（トルク制御手段４８での処理）。

次いで、ステップＳ９では、電圧指令値ｖが、トルク制御手段４８からＤ／Ａボード２０を通じてモータードライバ１８に与えられ、モータードライバ１８により、各モータ４１を流れる電流量を変化させることにより、ロボットアーム５の各関節の関節軸の回転運動が発生する。

以上のステップＳ１〜ステップＳ９が、制御の計算ループとして繰り返し実行されることにより、ロボットアーム５の動作の制御が実現する。

図７は、ダイナミクスパラメータ切換手段５０における動作ステップを表すフローチャートである。ダイナミクスパラメータ切換手段５０では図７に示す動作ステップが、図６に示す動作ステップとは独立に並行して処理が行われる。

ステップＳ７０は、ロボット起動時に一度だけ実行され、ダイナミクスパラメータ切換手段５０においてダイナミクスパラメータを

の初期値に設定する。

次いで、ステップＳ７１では、ダイナミクスパラメータ切換手段５０において、ハンド制御手段３４よりのハンド動作状態信号Ｓ_ｈｓを確認し、ハンド動作状態信号Ｓ_ｈｓ＝０の場合はハンド動作状態信号Ｓ_ｈｓの確認を継続し、ハンド動作状態信号Ｓ_ｈｓ＝１の場合には、ハンド把持が完了したとして、ステップＳ７２へと進む。

ステップＳ７２では、ダイナミクスパラメータ切換手段５０において、重力項をｇ_ｒ（ｑ）からｇ_ｒｏ（ｑ）に切り換え、目標トルク計算手段２８へと出力する。

次いで、ステップＳ７３では、ダイナミクスパラメータ切換手段５０において、第１作業台３１に設置した状態からの上昇量を手先位置ｒから計算し、上昇量がある一定値、例えば５ｃｍを越えたかどうかをダイナミクスパラメータ切換手段５０で確認する。越えていないとダイナミクスパラメータ切換手段５０で確認した場合は所定時間後に、再度、確認を繰り返し、越えたとダイナミクスパラメータ切換手段５０で確認した場合はステップＳ７４へと進む。

次いで、ステップＳ７４では、ダイナミクスパラメータ切換手段５０において、慣性行列をＭ_ｒ（ｑ）からＭ_ｒｏ（ｑ）に切り換えるとともに、遠心力・コリオリ力項を

から

に切り換え、目標トルク計算手段２８へと出力する。

ステップＳ７５では、ダイナミクスパラメータ切換手段５０において、ハンド開放予告の確認のため、ハンド制御手段３４よりのハンド動作状態信号Ｓ_ｈｓを確認し、ハンド動作状態信号Ｓ_ｈｓ＝０の場合は、ハンド動作状態信号Ｓ_ｈｓの確認を継続し、ハンド動作状態信号Ｓ_ｈｓ＝１の場合には、ハンド開放を行うことを予告されているとして、ステップＳ７６へと進む。

次いで、ステップＳ７６では、ダイナミクスパラメータ切換手段５０において、重力項をｇ_ｒｏ（ｑ）からｇ_ｒ（ｑ）に切り換え、目標トルク計算手段２８へと出力し、ハンド制御手段３４へダイナミクスパラメータ切換完了信号Ｓ_ｄｐを出力する。

次いで、ステップＳ７７では、ダイナミクスパラメータ切換手段５０において、第２作業台３２のスタンド３３に薄型テレビ４６を設置した状態からの薄型テレビ４６の画面の垂線方向のハンド６の移動量、すなわち、ハンド６と薄型テレビ４６との間の距離を、手先位置及び姿勢ベクトルｒから計算し、距離がある一定値（閾値）、例えば５ｃｍを越えたかどうか（ハンド６が薄型テレビ４６から離脱したか否か）を確認する。越えていないとダイナミクスパラメータ切換手段５０で確認した場合は所定時間後に、再度、確認を繰り返し、越えたとダイナミクスパラメータ切換手段５０で確認した場合はステップＳ７８へと進む。

次いで、ステップＳ７８では、ダイナミクスパラメータ切換手段５０において、慣性行列をＭ_ｒｏ（ｑ）からＭ_ｒ（ｑ）に切り換えるとともに、遠心力・コリオリ力項を

から

に切り換え、目標トルク計算手段２８へと出力する。

その後、ステップＳ７１へと戻り、ステップＳ７１からステップＳ７８が繰り返し実行される。

図８は、ハンド制御手段３４における動作ステップを表すフローチャートである。ハンド制御手段３４では図８に示す動作ステップが、図６に示す動作ステップとは独立に並行して処理が行われる。

ステップＳ８１では、ハンド制御手段３４において、ハンドボタン４３が押されたかどうかの確認を信号Ｓ_ｈｂにより行い、Ｓ_ｈｂ＝０の場合、すなわち、ハンドボタン４３が押されていないとハンド制御手段３４で確認した場合は、信号Ｓ_ｈｂの確認を所定時間後に、再度、繰り返す。一方、Ｓ_ｈｂ＝１の場合、すなわち、ハンドボタン４３が押されたとハンド制御手段３４で確認した場合はステップＳ８２へと進む。

ステップＳ８２では、ハンド制御手段３４よりロボットアーム５にハンド制御信号Ｓ_ｈｃ＝１が出力され、モータ駆動で、フィンガー３７ａとフィンガー３７ｂとの間及びフィンガー３７ｃとフィンガー３７ｄ（フィンガー３７ｅ）との間を閉じ、薄型テレビ４６を把持する。

ステップＳ８３では、ハンド制御手段３４において、ダイナミクスパラメータ切換手段５０へとハンド把持が完了したことを通知するハンド動作状態信号Ｓ_ｈｓ＝１のパルスを出力する。

ステップＳ８４では、ハンド制御手段３４において、ハンドボタン４３が押されたかどうかの確認を信号Ｓ_ｈｂにより行い、Ｓ_ｈｂ＝０の場合、すなわち、ハンドボタン４３が押されていないとハンド制御手段３４で確認した場合は、所定時間後に、再度、信号Ｓ_ｈｂの確認を繰り返す。一方、Ｓ_ｈｂ＝１の場合、すなわち、ハンドボタン４３が押されたとハンド制御手段３４で確認した場合はステップＳ８５へと進む。

ステップＳ８５では、ハンド制御手段３４において、ダイナミクスパラメータ切換手段５０へ、ハンド開放を行うことを予告するためハンド動作状態信号Ｓ_ｈｓ＝１のパルスを出力する。

ステップＳ８６では、ハンド制御手段３４において、ダイナミクスパラメータ切換手段５０からハンド制御手段３４にダイナミクスパラメータ切換完了信号Ｓ_ｄｐ＝１のパルスが出力されたかどうかの確認を行う。ダイナミクスパラメータ切換完了信号Ｓ_ｄｐ＝１のパルスが出力されていないとハンド制御手段３４で確認した場合は、所定時間後に、再度、ダイナミクスパラメータ切換完了信号Ｓ_ｄｐの確認を繰り返し、ダイナミクスパラメータ切換完了信号Ｓ_ｄｐ＝１のパルスが出力されたとハンド制御手段３４で確認した場合はステップＳ８７へと進む。

ステップＳ８７では、ハンド制御手段３４よりロボットアーム５にハンド制御信号Ｓ_ｈｃ＝０が出力され、モータ駆動で、フィンガー３７ａとフィンガー３７ｂとの間及びフィンガー３７ｃとフィンガー３７ｄ（フィンガー３７ｅ）との間を開き、ハンド６から薄型テレビ４６を開放する。

ハンド制御手段３４は、以上のステップＳ８１〜Ｓ８７を繰り返すことで、ダイナミクスパラメータ切換手段５０と連携して物品の一例である薄型テレビ４６の把持及び開放を実現する。

以上、本第１実施形態によれば、ダイナミクスパラメータ切換手段５０を有することにより、ロボットアーム５が物体４６を把持していない状態のロボットアーム５とハンド６だけの場合のダイナミクスパラメータ、又はロボットアーム５が物体４６を把持している状態のロボットアーム５とハンド６に物体４６も加えた場合のダイナミクスパラメータに、慣性行列と、遠心力・コリオリ力項と、重力項とを、それぞれ、分離してそれぞれ独立に切り換えることで、ダイナミクスパラメータの切換により発生してしまう動きの垂直方向及び水平方向という方向性をコントロール可能となる。

したがって、把持している物体４６を外環境に対して擦ってしまったり、物体４６又は外環境の構造物に過度の力を与え破壊してしまったり、落下又は転倒させてしまうことのない、把持物又は外環境等に対して優しく安全なロボット１とすることができる。

なお、本第１実施形態では、インピーダンス制御を使い、人が操作する形態としたが、これに限られるわけではなく、インピーダンス計算手段２５の出力を０とし、目標軌道生成手段２３で作業を行うための軌道を生成すれば、自動動作が可能であり、この場合も、ダイナミクスパラメータ切換手段５０は同様の効果を発揮する。

また、本第１実施形態では、重力項の切換を行った後、慣性行列と、遠心力・コリオリ力項とのそれぞれの切換を行うとしたが、この切換パターンに限られるわけではなく、図４（ａ）のハンド６を薄型テレビ４６に接近させているときに把持位置に十分近づいていれば、先に、慣性行列と、遠心力・コリオリ力項とのそれぞれの切換を行い、ハンド６で把持した後に重力項のみを切り換えることも可能である。

さらに、図４（ｄ）のスタンド３３の位置決めの際に、挿入口４７がスタンド３３の真上に近づいていれば、先に、慣性行列と、遠心力・コリオリ力項とのそれぞれの切換を行い、スタンド３３の挿入後に重力項のみを切り換えることも可能である。

（第２実施形態）
本発明の第２実施形態におけるロボット１の基本的な構成は、図１及び図２に示した第１実施形態の場合と同様であるので、共通部分は第１実施形態と同一番号を付してそれらの説明は省略し、異なる部分についてのみ以下、詳細に説明する。

第２実施形態では、図１０に示すようにダイナミクスパラメータ切換手段５０の動作ステップのフローが異なり、ダイナミクスパラメータの切換を複数段に分離して行う。

ステップＳ７０及びステップＳ７１の後、ステップＳ９２では、ダイナミクスパラメータ切換手段５０において、切換の第１段として、重力項をｇ_ｒ（ｑ）から、以下で表されるｇ_ｒｍ（ｑ）に切り換える。

そして、ステップＳ７３の後、ステップＳ９４では、ダイナミクスパラメータ切換手段５０において、切換の第２段として、重力項をｇ_ｒｍ（ｑ）からｇ_ｒｏ（ｑ）に切り換える。

ここで、αは定数であり、例えばα＝０．９とすれば、把持物体の重量が実際の重さの９割とした場合の制御が働くため、ダイナミクスパラメータに誤差があっても把持物体の一例としての薄型テレビ４６を第１作業台３１に確実に設置させることになるため、薄型テレビ４６の一部が第１作業台３１に設置したまま微妙に浮き上がり、水平の動きに対して第１作業台３１と薄型テレビ４６の一部との間で擦り合いが発生するのを防ぐことができる。
実際の重さの９割とした場合の制御では、垂直方向の操作性が劣化することになるが、少し持ち上げることでステップＳ９４に移行するので、操作性への影響は小さい。

その後、ステップＳ７７からステップＳ７８を第１実施形態と同様に行なう。

（第３実施形態）
本発明の第３実施形態におけるロボット１の基本的な構成は、図１及び図２に示した第１実施形態の場合と同様であるので、共通部分は第１実施形態と同一番号を付してそれらの説明は省略し、異なる部分についてのみ以下、詳細に説明する。

図１１は、第３実施形態におけるロボットのインピーダンス制御手段４Ａの構成を示すブロック図である。第３実施形態のインピーダンス制御手段４Ａは、第１実施形態のインピーダンス制御手段４に比べて、弾性力制御手段（弾性力制御部）４４が新たに加わっている点が異なる。

弾性力制御手段４４は、ハンド制御手段３４からのハンド動作状態信号Ｓ_ｈｓを受け、手先位置及び姿勢目標補正出力Δｒ_ｄｃを第２演算部５２に出力し、位置誤差補償手段２７へゲイン調整信号Ｓ_ｇを出力する。

図１２は、弾性力制御手段４４における動作ステップを表すフローチャート、図１５は動作のタイミングを説明する図である。弾性力制御手段４４では図１２に示す動作ステップが、図６に示す動作ステップとは独立に並行して処理が行われる。図１２に示す動作ステップは、図７の第１実施形態の場合の動作ステップに加え、ステップＳ１１００、Ｓ１１０２、Ｓ１１０４、Ｓ１１０６、Ｓ１１０８の５つの動作ステップが新たに加わる。

まず、ステップＳ７０の後、ステップＳ１１００では、弾性力制御手段４４において、初期設定として手先位置及び姿勢目標補正出力Δｒ_ｄｃが０ベクトルに設定される。

次いで、ステップＳ７１の後、ステップＳ１１０２では、弾性力制御手段４４において、手先位置及び姿勢目標補正出力がΔｒ_ｄｃ＝［０，０，−Δｚ_ｄｃ，０，０］^Ｔに設定され、弾性力制御手段４４から位置誤差補償手段２７へゲイン調整信号Ｓ_ｇが出力される。ただし、Δｚ_ｄｃは正の定数値である。

位置誤差補償手段２７は、弾性力制御手段４４からゲイン調整信号Ｓ_ｇを受けると、位置制御のゲイン値を低下させる。したがって、手先位置及び姿勢目標値が−Δｚ_ｄｃだけ垂直下向き（−ｚ方向）に補正されているため、図５（ｂ）の状態に置いて、図１４Ａの矢印Ｆ_ｚに示すように、ハンド６で薄型テレビ４６を垂直下向きに第１作業台３１に押しつけるような弾性力がハンド６から薄型テレビ４６に働くことになる。すなわち、弾性力制御手段４４は、ハンド６から物体の一例としての薄型テレビ４６に働く弾性力を制御するものであって、ダイナミクスパラメータ切換手段５０によるダイナミクスパラメータ切換時に発生するロボットアーム５のハンド６の動きによる影響を抑制する方向に弾性力をかけるように構成している。

次いで、ステップＳ７２及びステップＳ７３の後、ステップＳ１１０４では、弾性力制御手段４４において、手先位置及び姿勢目標補正出力Δｒ_ｄｃを０ベクトルに戻し、弾性力制御手段４４から位置誤差補償手段２７へのゲイン調整信号Ｓ_ｇの出力が停止されて、弾性力の発生が解除される。

次いで、ステップＳ７４及びステップＳ７５及びステップＳ７６の後、ステップＳ１１０６では、弾性力制御手段４４において、手先位置及び姿勢目標補正出力がΔｒ_ｄｃ＝［Δｘ_ｄｃ，Δｙ_ｄｃ，０，０，０］^Ｔに設定され、弾性力制御手段４４から位置誤差補償手段２７へゲイン調整信号Ｓ_ｇが出力される。ただし、［Δｘ_ｄｃ，Δｙ_ｄｃ，０］^Ｔは、図５（ｅ）の状態に置いて、薄型テレビ４６の画面の法線方向に平行で薄型テレビ４６からハンド６の方向を向く定ベクトルである。

位置誤差補償手段２７は、このようなゲイン調整信号Ｓ_ｇを弾性力制御手段４４から受けると、位置制御のゲイン値を低下させる。したがって、手先位置及び姿勢目標値が［Δｘ_ｄｃ，Δｙ_ｄｃ，０］^Ｔだけ薄型テレビ４６からハンド６の方向に補正されているため、図５（ｅ）の状態に置いて、図１４Ｂの矢印Ｆ_ｘ（図１４Ｂの紙面を置く側から手前側に向けて貫通する方向の矢印）に示す薄型テレビ４６からハンド６が離脱しようとする弾性力が働くことになる。すなわち、ここでは、弾性力制御手段４４は、ハンド６で把持している物体の一例としての薄型テレビ４６を設置した後、慣性力項成分のダイナミクスパラメータを切換えるより前に、ハンド６の拘束が解除される方向に弾性力をかけるよう制御する。

次いで、ステップＳ７７の後、ステップＳ１１０８では、弾性力制御手段４４において、手先位置及び姿勢目標補正出力Δｒ_ｄｃを０ベクトルに戻し、弾性力制御手段４４から位置誤差補償手段２７へのゲイン調整信号Ｓ_ｇの出力が停止され、弾性力の発生が解除される。

その後、ステップＳ７８の後、ステップＳ７１へと戻り、ステップＳ７１からステップＳ７８が繰り返し実行される。

以上の弾性力制御手段４４の動作によれば、図５（ｂ）の状態に置いて、垂直下向きに押しつけるような弾性力が働き、薄型テレビ４６が第１作業台３１に対して確実に固定されるため、ダイナミクスパラメータ切換によって、薄型テレビ４６が中途半端に浮いてしまうことで水平方向に容易に動くようになり、人の操作で薄型テレビ４６と第１作業台３１を擦ってしまう可能性を低くできる。また、図５（ｅ）の状態に置いて、薄型テレビ４６からハンド６が離脱しようとする弾性力が働くため、ハンド６の解放時に弾性力によってもハンド６が薄型テレビ４６から離れる動作が促進されるため、着実に離脱が可能となる。

なお、上記第１〜第３実施形態では、ロボットアーム５をモータで関節を駆動するとしたが、これに限られるわけではなく、空気圧シリンダー又は空気圧人工筋などの空圧アクチュエータ言い換えれば弾性体アクチュエータでも同様の効果を発揮する。特に、第３実施形態では、空圧アクチュエータにすると、機構的な弾性力を利用できるようになるため、より効果を高めることが可能となる。

また、上記第１実施形態では、慣性行列と、遠心力・コリオリ力項との切換のタイミングを図５（ｃ）及び図５（ｆ）の状態で切り換えるとしたが、これに限られるわけではない。例えば、図５（ａ）においてハンド６が把持のために薄型テレビ４６に接触して停止する直前の低速の状態、及び、図５（ｄ）においてスタンド３３への薄型テレビ４６の位置決めが完了し、挿入動作に入る直前の低速状態にあるときに、慣性行列と、遠心力・コリオリ力項との切換を行う場合でも、同様の効果を発揮する。この場合、ハンド６は拘束状態に入る直前の非拘束状態にあるため、薄型テレビ４６と第１作業台３１とが擦れ合い、薄型テレビ４６の表面を傷つけてしまう等の問題は発生しない。また、物体を把持していない状態で物体を把持した状態のダイナミクスパラメータで動作することになり、操作性が劣化するが、速度が十分低速であるので、その影響は小さい。

また、上記第３実施形態では、弾性力制御手段４４を、図１１に示すように、手先位置及び姿勢目標補正出力Δｒ_ｄｃを出力して、位置誤差補償手段２７のゲイン調整を行う構成としたが、これに限られるわけではない。例えば、弾性力制御手段４４を、図１３に示すようにΔＦ_ｄｃを出力し、インピーダンス計算手段２５への力入力を補正することで、弾性力を発揮する構成も可能である。この場合、図１２のステップＳ１１０２において、ΔＦ_ｄｃ＝［０，０，−ΔＦｚ_ｄｃ，０，０］^Ｔ（ただし、ΔＦｚ_ｄｃは正の定数値）を出力すれば良く、また、ステップＳ６においてΔＦ_ｄｃ＝［ΔＦｘ_ｄｃ，ΔＦｙ_ｄｃ，０，０，０］^Ｔ（ただし、［ΔＦｘ_ｄｃ，ΔＦｙ_ｄｃ，０］^Ｔは、図５（ｅ）の状態に置いて、薄型テレビ４６の画面の法線方向に平行で薄型テレビ４６からハンド６の方向を向く定ベクトル）とすれば、同様の効果を発揮することができる。

なお、本発明を第１〜第３実施形態及び変形例に基づいて説明してきたが、本発明は、前記の第１〜第３実施形態及び変形例に限定されないのはもちろんである。以下のような場合も本発明に含まれる。
前記各制御装置の一部又は全部は、具体的には、マイクロプロセッサ、ＲＯＭ、ＲＡＭ、ハードディスクユニット、ディスプレイユニット、キーボード、マウスなどから構成されるコンピュータシステムである。前記ＲＡＭ又はハードディスクユニットには、コンピュータプログラムが記憶されている。前記マイクロプロセッサが、前記コンピュータプログラムにしたがって動作することにより、各部は、その機能を達成する。ここでコンピュータプログラムは、所定の機能を達成するために、コンピュータに対する指令を示す命令コードが複数個組み合わされて構成されたものである。
例えば、ハードディスク又は半導体メモリ等の記録媒体に記録されたソフトウェア・プログラムをＣＰＵ等のプログラム実行部が読み出して実行することによって、各構成要素が実現され得る。なお、前記実施形態又は変形例における制御装置を構成する要素の一部又は全部を実現するソフトウェアは、以下のようなプログラムである。つまり、このプログラムは、関節を有するロボットの制御プログラムであって、
コンピュータを、
上記ロボットの位置の時系列の情報又は速度の情報又は加速度の情報を取得する位置情報取得手段としての機能と、
物体を把持していない状態の上記ロボットのダイナミクスパラメータと上記ロボットが上記物体を把持している状態の上記物体も含めたダイナミクスパラメータとを有する複数のダイナミクスパラメータの間で、運動方程式の重力項成分と慣性力項成分とを分離してそれぞれ切り換えるダイナミクスパラメータ切換手段としての機能と、
上記ダイナミクスパラメータ切換手段が上記ダイナミクスパラメータを切り換えた後の運動方程式と上記位置情報取得手段で取得した上記ロボットの位置の時系列の情報又は速度の情報又は加速度の情報とに基づき、上記ロボットの関節トルクの目標値を目標関節トルクとして出力する目標トルク計算手段としての機能と、
上記目標トルク計算手段からの出力に基づき上記ロボットの動作を制御する機能とを実現させるためのロボットの制御プログラムである。
また、このプログラムは、サーバなどからダウンロードされることによって実行されてもよく、所定の記録媒体（例えば、ＣＤ−ＲＯＭなどの光ディスク、磁気ディスク、又は、半導体メモリなど）に記録されたプログラムが読み出されることによって実行されてもよい。
また、このプログラムを実行するコンピュータは、単数であってもよく、複数であってもよい。すなわち、集中処理を行ってもよく、あるいは分散処理を行ってもよい。
また、上記様々な実施形態又は変形例のうちの任意の実施形態又は変形例を適宜組み合わせることにより、それぞれの有する効果を奏するようにすることができる。

本発明のロボット、ロボットの制御装置、制御方法、及び制御プログラムは、ダイナミクスパラメータの切換により発生してしまう動きの垂直方向及び水平方向という方向性をコントロール可能になり、物体の搬送を行う動作を制御する、ロボット、ロボット制御装置、制御方法、及び制御プログラムとして有用である。
本発明は、添付図面を参照しながら実施形態に関連して充分に記載されているが、この技術の熟練した人々にとっては種々の変形又は修正は明白である。そのような変形又は修正は、添付した請求の範囲による本発明の範囲から外れない限りにおいて、その中に含まれると理解されるべきである。

Claims (12)

1. 関節を有するロボットであって、
   上記ロボットの位置の時系列の情報又は速度の情報又は加速度の情報を取得する位置情報取得部と、
   物体を把持していない状態の上記ロボットのダイナミクスパラメータと上記ロボットが上記物体を把持している状態の上記物体も含めたダイナミクスパラメータとを有する複数のダイナミクスパラメータの間で、運動方程式の重力項成分と慣性力項成分とを分離してそれぞれ切り換えるダイナミクスパラメータ切換部と、
   上記ダイナミクスパラメータ切換部が上記ダイナミクスパラメータを切り換えた後の運動方程式と上記位置情報取得部で取得した上記ロボットの位置の時系列の情報又は速度の情報又は加速度の情報とに基づき、上記ロボットの関節トルクの目標値を目標関節トルクとして出力する目標トルク計算部とを備えて、
   上記目標トルク計算部からの出力に基づき上記ロボットの動作を制御するロボット。
2. 上記ダイナミクスパラメータ切換部は、上記物体を把持及び把持開放を含む作業の切り替え信号に基づき、上記ロボットの手先による物体把持時に上記ロボットの上記手先が移動困難に拘束された拘束状態とそれ以外の非拘束状態との間で切り換わるタイミングに応じて、上記複数のダイナミクスパラメータを、上記ロボットと上記ロボットが把持している物体との運動方程式の上記重力項成分と上記慣性力項成分とを分離してそれぞれ切り換える請求項１に記載のロボット。
3. 上記ダイナミクスパラメータ切換部は、上記位置情報取得部で取得した上記ロボットの位置の時系列の情報又は速度の情報又は加速度の情報に基づき、上記ロボットの上記手先が上記非拘束状態になったときに上記慣性力項成分のダイナミクスパラメータのみを切換える請求項２に記載のロボット。
4. 上記ダイナミクスパラメータ切換部は、上記ロボットが上記手先で上記物体を把持し、把持している物体を設置後、把持を開放し、上記ロボットの上記手先が上記非拘束状態になったときに上記慣性力項成分のダイナミクスパラメータのみを切換える請求項３に記載のロボット。
5. 上記ダイナミクスパラメータ切換部は、上記ロボットが上記物体を把持し、搬送し、上記把持している物体を設置するときに上記把持又は設置のために位置決めをするため搬送時よりも低速の動作になったときに上記慣性力項成分のダイナミクスパラメータのみを切換える請求項１に記載のロボット。
6. 上記ダイナミクスパラメータ切換部は、複数段に分離して重力項成分のダイナミクスパラメータを切換える請求項１〜５のいずれか１つに記載のロボット。
7. 上記ダイナミクスパラメータ切換部は、上記慣性力項成分のダイナミクスパラメータを切換えるとき、上記慣性力項成分と共に、上記運動方程式の遠心力及びコリオリ力項成分のダイナミクスパラメータをも切換える請求項１〜５のいずれか１つに記載のロボット。
8. 上記ロボットは、弾性体アクチュエータで駆動されるロボットアームを備え、
   さらに上記ロボットの上記弾性体アクチュエータにより発生する上記ロボットアームの手先の弾性力を制御する弾性力制御部を備え、
   上記弾性力制御部は、上記ダイナミクスパラメータ切換部によるダイナミクスパラメータ切換時に発生する上記ロボットアームの上記手先の動きによる影響を抑制する方向に弾性力をかける請求項１に記載のロボット。
9. 上記ロボットは弾性体アクチュエータで駆動されるロボットアームを備え、
   さらに上記ロボットの上記弾性体アクチュエータにより発生する上記ロボットアームの手先の弾性力を制御する弾性力制御部を備え、
   上記弾性力制御部は、上記手先で把持している物体を設置した後、慣性力項成分のダイナミクスパラメータを切換えるより前に、上記手先の拘束が解除される方向に弾性力をかけるよう制御する請求項８に記載のロボット。
10. 関節を有するロボットの制御装置であって、
    上記ロボットの位置の時系列の情報又は速度の情報又は加速度の情報を取得する位置情報取得部と、
    物体を把持していない状態の上記ロボットのダイナミクスパラメータと上記ロボットが上記物体を把持している状態の上記物体も含めたダイナミクスパラメータとを有する複数のダイナミクスパラメータの間で、運動方程式の重力項成分と慣性力項成分とを分離してそれぞれ切り換えるダイナミクスパラメータ切換部と、
    上記ダイナミクスパラメータ切換部が上記ダイナミクスパラメータを切り換えた後の運動方程式と上記位置情報取得部で取得した上記ロボットの位置の時系列の情報又は速度の情報又は加速度の情報とに基づき、上記ロボットの関節トルクの目標値を目標関節トルクとして出力する目標トルク計算部とを備えて、
    上記目標トルク計算部からの出力に基づき上記ロボットの動作を制御するロボットの制御装置。
11. 関節を有するロボットの制御方法であって、
    上記ロボットの位置の時系列の情報又は速度の情報又は加速度の情報を位置情報取得部で取得し、
    物体を把持していない状態の上記ロボットのダイナミクスパラメータと上記ロボットが上記物体を把持している状態の上記物体も含めたダイナミクスパラメータとを有する複数のダイナミクスパラメータの間で、運動方程式の重力項成分と慣性力項成分とを分離してそれぞれダイナミクスパラメータ切換部で切り換え、
    上記ダイナミクスパラメータ切換部が上記ダイナミクスパラメータを切り換えた後の運動方程式と上記位置情報取得部で取得した上記ロボットの位置の時系列の情報又は速度の情報又は加速度の情報とに基づき、上記ロボットの関節トルクの目標値を目標関節トルクとして目標トルク計算部で出力して、
    上記目標トルク計算部からの出力に基づき上記ロボットの動作を制御するロボットの制御方法。
12. 関節を有するロボットの制御プログラムであって、
    コンピュータを、
    上記ロボットの位置の時系列の情報又は速度の情報又は加速度の情報を取得する位置情報取得部としての機能と、
    物体を把持していない状態の上記ロボットのダイナミクスパラメータと上記ロボットが上記物体を把持している状態の上記物体も含めたダイナミクスパラメータとを有する複数のダイナミクスパラメータの間で、運動方程式の重力項成分と慣性力項成分とを分離してそれぞれ切り換えるダイナミクスパラメータ切換部としての機能と、
    上記ダイナミクスパラメータ切換部が上記ダイナミクスパラメータを切り換えた後の運動方程式と上記位置情報取得部で取得した上記ロボットの位置の時系列の情報又は速度の情報又は加速度の情報とに基づき、上記ロボットの関節トルクの目標値を目標関節トルクとして出力する目標トルク計算部としての機能と、
    上記目標トルク計算部からの出力に基づき上記ロボットの動作を制御する機能とを実現させるためのロボットの制御プログラム。

Images