知財求人 - 知財ポータルサイト「IP Force」
(19)【発行国】日本国特許庁(JP)
(12)【公報種別】特許公報(B2)
(11)【特許番号】
(24)【登録日】2024-12-13
(45)【発行日】2024-12-23
(54)【発明の名称】制御装置、ロボット及び制御方法
(51)【国際特許分類】
B25J 13/08 20060101AFI20241216BHJP
【ＦＩ】
B25J13/08 Z
【請求項の数】
7
(21)【出願番号】P 2021024980
(22)【出願日】2021-02-19
(65)【公開番号】P2022127061
(43)【公開日】2022-08-31
【審査請求日】2024-02-15
(73)【特許権者】
【識別番号】000000974
【氏名又は名称】川崎重工業株式会社
(74)【代理人】
【識別番号】110000556
【氏名又は名称】弁理士法人有古特許事務所
(72)【発明者】
【氏名】梶原 慎司
(72)【発明者】
【氏名】田邊 雅隆
(72)【発明者】
【氏名】渡邉 圭
【審査官】杉山 悟史
(56)【参考文献】
【文献】特開２０１７－０８７３７５（ＪＰ，Ａ）
【文献】特開２００５－２４２７９４（ＪＰ，Ａ）
【文献】特開２０１７－０５６５４４（ＪＰ，Ａ）
【文献】米国特許出願公開第２０１９／０００９４１０（ＵＳ，Ａ１）
【文献】特開平１－２８１８９１（ＪＰ，Ａ）
(58)【調査した分野】(Int.Cl.，ＤＢ名)
Ｂ２５Ｊ １／００ ～ ２１／０２
(57)【特許請求の範囲】
【請求項1】
第２ロボットアームを支持する少なくとも２つのリンクと、前記少なくとも２つのリンクを相互に接続する少なくとも１つの関節とを有する第１ロボットアームの制御装置であって、前記第１ロボットアームに目標動作である第１目標動作をさせるための第１動作指令を取得し、前記第１ロボットアームが前記第１動作指令に従って動作する場合の前記少なくとも１つの関節の動作を推定する第１推定部と、
前記第２ロボットアームに目標動作である第２目標動作をさせるための第２動作指令を取得し、前記第２ロボットアームが前記第２動作指令に従って動作する場合に、前記第２ロボットアームと前記リンクとの接続部分が前記第２ロボットアームから受ける力を推定する第２推定部と、
前記第１推定部によって推定される前記少なくとも１つの関節の動作に基づき、前記少なくとも１つの関節が動作することによって前記リンクに発生する慣性力を推定する慣性力推定部と、
前記慣性力推定部によって推定される慣性力と、前記第２推定部によって推定される力とに基づき、前記リンクが受ける力を推定する第３推定部と、
前記第３推定部によって推定される力に基づき、前記リンクの変形量と、前記少なくとも１つの関節の変形量とのうちの少なくとも一方を推定する第４推定部と、
前記第４推定部によって推定される変形量に基づき、前記第１目標動作に従って前記接続部分を移動させるように前記変形量を補償するための前記第１動作指令の補正指令を生成し、前記補正指令を前記第１ロボットアームに指令する補正指令部とを含む
制御装置。
【請求項2】
前記少なくとも２つのリンクは、基端リンクと、前記基端リンク及び前記第２ロボットアームと接続される第１リンクとを含み、前記少なくとも１つの関節は、前記第１リンクを前記基端リンクに接続する第１関節を含み、
前記第１推定部は、前記第１関節の動作を推定し、
前記慣性力推定部は、前記第１推定部によって推定される前記第１関節の動作に基づき、前記第１関節が動作することによって前記第１リンクに発生する慣性力を推定し、
前記第３推定部は、前記慣性力推定部によって推定される前記第１リンクに発生する慣性力と、前記第２推定部によって推定される力とに基づき、前記第１リンクが受ける力を推定し、
前記第４推定部は、前記第３推定部によって推定される前記第１リンクが受ける力に基づき、前記第１リンクの変形量と前記第１関節の変形量とのうちの少なくとも一方を推定する
請求項１に記載の制御装置。
【請求項3】
前記少なくとも２つのリンクは、基端リンクと、前記基端リンクと接続される第１リンクと、前記第２ロボットアームと接続される第２リンクとを含み、前記少なくとも１つの関節は、前記第１リンクを前記基端リンクに接続する第１関節と、前記第２リンクを前記第１リンクに接続する第２関節とを含み、
前記第１推定部は、前記第１関節及び前記第２関節の動作を推定し、
前記慣性力推定部は、前記第１推定部によって推定される前記第１関節及び前記第２関節の動作に基づき、前記第１関節及び前記第２関節が動作することによって前記第１リンク及び前記第２リンクに発生する慣性力を推定し、
前記第３推定部は、前記慣性力推定部によって推定される前記第１リンクに発生する慣性力と、前記第２推定部によって推定される力とに基づき、前記第１リンクが受ける力を推定し、
前記第３推定部は、前記慣性力推定部によって推定される前記第２リンクに発生する慣性力と、前記第２推定部によって推定される力とに基づき、前記第２リンクが受ける力を推定し、
前記第４推定部は、前記第３推定部によって推定される前記第１リンクが受ける力に基づき、前記第１リンクの変形量と前記第１関節の変形量とのうちの少なくとも一方を推定し、
前記第４推定部は、前記第３推定部によって推定される前記第２リンクが受ける力に基づき、前記第２リンクの変形量と前記第２関節の変形量とのうちの少なくとも一方を推定する
請求項１に記載の制御装置。
【請求項4】
前記リンクの位置を推定する第１位置推定部と、前記接続部分の位置を推定する第２位置推定部と、
前記リンクの慣性的特徴と前記第２ロボットアームの慣性的特徴とを推定する慣性要素推定部とをさらに含み、
前記リンクの慣性的特徴は、前記リンクの慣性モーメント及び質量を要素として含み、
前記第２ロボットアームの慣性的特徴は、前記第２ロボットアームの慣性モーメント及び質量を要素として含み、
前記慣性力推定部は、前記リンクの慣性的特徴と、前記第２ロボットアームの慣性的特徴と、前記少なくとも１つの関節の加速度と、前記少なくとも１つの関節のジョイントスクリュとを用いて、前記リンクに発生する前記慣性力を推定し、
前記ジョイントスクリュは、前記少なくとも１つの関節の向きを示す要素を含む、前記少なくとも１つの関節の状態を示す特徴である
請求項１～３のいずれか一項に記載の制御装置。
【請求項5】
請求項１～４のいずれか一項に記載の制御装置と、前記第１ロボットアームと、
前記第２ロボットアームとを備える
ロボット。
【請求項6】
前記制御装置は、前記第１動作指令及び前記第２動作指令を生成し、前記第１ロボットアーム及び前記第２ロボットアームの動作を制御するように構成される請求項５に記載のロボット。
【請求項7】
第２ロボットアームを支持する少なくとも２つのリンクと、前記少なくとも２つのリンクを相互に接続する少なくとも１つの関節とを有する第１ロボットアームの制御方法であって、
前記第１ロボットアームに目標動作である第１目標動作をさせるための第１動作指令を取得することと、
前記第１ロボットアームが前記第１動作指令に従って動作する場合の前記少なくとも１つの関節の動作である推定関節動作を推定することと、
前記第２ロボットアームに目標動作である第２目標動作をさせるための第２動作指令を取得することと、
前記第２ロボットアームが前記第２動作指令に従って動作する場合に、前記第２ロボットアームと前記リンクとの接続部分が前記第２ロボットアームから受ける力である推定接続部分力を推定することと、
前記推定関節動作に基づき、前記少なくとも１つの関節が動作することによって前記リンクに発生する慣性力である推定慣性力を推定することと、
前記推定慣性力と、前記推定接続部分力とに基づき、前記リンクが受ける力である推定リンク力を推定することと、
前記推定リンク力に基づき、前記リンクの変形量と、前記少なくとも１つの関節の変形量とのうちの少なくとも一方を含む推定変形量を推定することと、
前記推定変形量に基づき、前記第１目標動作に従って前記接続部分を移動させるように前記推定変形量を補償するための前記第１動作指令の補正指令を生成することと、
前記補正指令を前記第１ロボットアームに指令することと、を含む
制御方法。
【発明の詳細な説明】
【技術分野】
【0001】
本開示は、制御装置及びロボットに関する。
【背景技術】
【0002】
従来、移動装置に搭載されるロボットアームがある。例えば、特許文献１は、多関節型の工業用ロボットと、工業用ロボットを移動させる移動装置とを備える工業用ロボット装置を開示する。移動装置は、２つのリンクにより構成される水平２関節のリンク機構を有し、据付面に固定される。工業用ロボットは先端側のリンクの先端部に取り付けられる。
【先行技術文献】
【特許文献】
【0003】
【文献】特開平１－２８１８９１号公報
【発明の概要】
【発明が解決しようとする課題】
【0004】
特許文献１の工業用ロボット装置では、工業用ロボットが動作すると、工業用ロボットの重心が変動し、工業用ロボットが移動装置のリンクに与えるモーメントが変動し得る。例えば、工業用ロボットがワークを把持すると、工業用ロボットが移動装置のリンクに与える荷重及びモーメントが変動し得る。このような荷重及びモーメントの変動は、移動装置のリンク及び関節にたわみ等の変形を生じ得る。リンク及び関節の変形は、工業用ロボットの位置を変動し、当該位置の変動は、工業用ロボットを振動させる。これにより、工業用ロボットの作業の精度が低下する可能性がある。
【0005】
本開示は、第１ロボットアームによって支持される第２ロボットアームの基部の位置の意図しない変動を抑制する制御装置及びロボットを提供することを目的とする。
【課題を解決するための手段】
【0006】
本開示の一態様に係る制御装置は、第２ロボットアームを支持する少なくとも２つのリンクと、前記少なくとも２つのリンクを相互に接続する少なくとも１つの関節とを有する第１ロボットアームの制御装置であって、前記第１ロボットアームに目標動作である第１目標動作をさせるための第１動作指令を取得し、前記第１ロボットアームが前記第１動作指令に従って動作する場合の前記少なくとも１つの関節の動作を推定する第１推定部と、前記第２ロボットアームに目標動作である第２目標動作をさせるための第２動作指令を取得し、前記第２ロボットアームが前記第２動作指令に従って動作する場合に、前記第２ロボットアームと前記リンクとの接続部分が前記第２ロボットアームから受ける力を推定する第２推定部と、前記第１推定部によって推定される前記少なくとも１つの関節の動作と、前記第２推定部によって推定される力とに基づき、前記リンクが受ける力を推定する第３推定部と、前記第３推定部によって推定される力に基づき、前記リンクの変形量と、前記少なくとも１つの関節の変形量とのうちの少なくとも一方を推定する第４推定部と、前記第４推定部によって推定される変形量に基づき、前記第１目標動作に従って前記接続部分を移動させるように前記変形量を補償するための前記第１動作指令の補正指令を生成し、前記補正指令を前記第１ロボットアームに指令する補正指令部とを含む。
【発明の効果】
【0007】
本開示の技術によれば、第１ロボットアームによって支持される第２ロボットアームの基部の位置の意図しない変動を抑制することが可能になる。
【図面の簡単な説明】
【0008】
【図1】実施の形態に係るロボットの構成の一例を示す側面図
【図2】実施の形態に係るロボットの構成の一例をモデル化した側面図
【図3】実施の形態に係る制御装置のハードウェア構成の一例を示すブロック図
【図4】実施の形態に係る制御装置の機能的構成の一例を示すブロック図
【図5】実施の形態に係る制御装置の動作の一例を示すフローチャート
【図6】変形例１に係るロボットの構成をモデル化した側面図
【図7】変形例２に係るロボットの構成をモデル化した側面図
【発明を実施するための形態】
【0009】
（実施の形態）
以下において、本開示の実施の形態を、図面を参照しつつ説明する。なお、以下で説明する実施の形態は、いずれも包括的又は具体的な例を示すものである。また、以下の実施の形態における構成要素のうち、最上位概念を示す独立請求項に記載されていない構成要素については、任意の構成要素として説明される。また、添付の図面における各図は、模式的な図であり、必ずしも厳密に図示されたものでない。さらに、各図において、実質的に同一の構成要素に対しては同一の符号を付しており、重複する説明は省略又は簡略化される場合がある。また、本明細書及び特許請求の範囲では、「装置」とは、１つの装置を意味し得るだけでなく、複数の装置からなるシステムも意味し得る。
以下において、本開示の実施の形態を、図面を参照しつつ説明する。なお、以下で説明する実施の形態は、いずれも包括的又は具体的な例を示すものである。また、以下の実施の形態における構成要素のうち、最上位概念を示す独立請求項に記載されていない構成要素については、任意の構成要素として説明される。また、添付の図面における各図は、模式的な図であり、必ずしも厳密に図示されたものでない。さらに、各図において、実質的に同一の構成要素に対しては同一の符号を付しており、重複する説明は省略又は簡略化される場合がある。また、本明細書及び特許請求の範囲では、「装置」とは、１つの装置を意味し得るだけでなく、複数の装置からなるシステムも意味し得る。
【0010】
［ロボットの構成］
図１及び図２を参照しつつ、実施の形態に係るロボット１の構成を説明する。図１は、実施の形態に係るロボット１の構成の一例を示す側面図である。図２は、実施の形態に係るロボット１の構成の一例をモデル化した側面図である。
図１及び図２を参照しつつ、実施の形態に係るロボット１の構成を説明する。図１は、実施の形態に係るロボット１の構成の一例を示す側面図である。図２は、実施の形態に係るロボット１の構成の一例をモデル化した側面図である。
【0011】
実施の形態に係るロボット１は、第１ロボットアーム１００と、第２ロボットアーム２００と、制御装置３００とを備える。第１ロボットアーム１００は、支持面Ｓに固定され、第２ロボットアーム２００を支持する。第１ロボットアーム１００は、少なくとも１つの関節と少なくとも２つのリンクとを有し、第２ロボットアーム２００は、少なくとも１つの関節と少なくとも１つのリンクとを有する。
【0012】
これに限定されないが、本実施の形態では、第１ロボットアーム１００は、２つの関節ＪＴＡ１及びＪＴＡ２と、３つのリンク１０１～１０３と、関節ＪＴＡ１及びＪＴＡ２の駆動装置ＭＡ１及びＭＡ２とを備える。第２ロボットアーム２００は、６つの関節ＪＴＢ１～ＪＴＢ６と、７つのリンク２０１～２０７と、関節ＪＴＢ１～ＪＴＢ６それぞれの駆動装置ＭＢ１～ＭＢ６とを備える。第２ロボットアーム２００は、垂直多関節型のロボットアームであるが、いかなる形式のロボットアームであってもよい。例えば、第２ロボットアーム２００は、水平多関節型、極座標型、円筒座標型、直角座標型、又はその他の型式のロボットアームであってもよい。また、第２ロボットアーム２００のリンクの数量は７つに限定されず、６つ以下又は８つ以上であってもよく、第２ロボットアーム２００の関節の数量は６つに限定されず、５つ以下又は７つ以上であってもよい。
【0013】
第１ロボットアーム１００について、リンク１０３は、第１ロボットアーム１００の基部に位置し、支持面Ｓに固定される。これに限定されないが、本実施の形態では、リンク１０３は、台状の形状を有し、リンク１０１及び１０２の基台として機能する。リンク１０２は、第１ロボットアーム１００の先端部に位置し、リンク１０１は、リンク１０２及び１０３の間に位置する。リンク１０３は、基端リンクの一例である。
【0014】
関節ＪＴＡ１は、１自由度の回転関節であり、柱状のリンク１０１の一端をリンク１０３に第１軸Ｓ１を中心に回動可能に接続する。関節ＪＴＡ２は、１自由度の回転関節であり、柱状のリンク１０２の一端をリンク１０１の他端に第２軸Ｓ２を中心に回動可能に接続する。リンク１０２の他端には、第２ロボットアーム２００が取り付けられる。よって、リンク１０１及び１０２は、関節ＪＴＡ１及びＪＴＡ２によって動作可能である。
【0015】
これに限定されないが、本実施の形態では、第１軸Ｓ１は、支持面Ｓに沿う方向、例えば、水平方向に延び、第２軸Ｓ２は、支持面Ｓと垂直な方向、例えば、鉛直方向に延びる。リンク１０１は、支持面Ｓと垂直な面に沿う方向に回動でき、リンク１０２は、支持面Ｓに沿う方向に回動できる。なお、第１軸Ｓ１及び第２軸Ｓ２の方向は、いかなる方向であってもよく、互いに異なっていても同じであってもよい。第１軸Ｓ１及び第２軸Ｓ２はそれぞれ、関節ＪＴＡ１及びＪＴＡ２の関節軸でもある。
【0016】
駆動装置ＭＡ１及びＭＡ２はそれぞれ、関節ＪＴＡ１及びＪＴＡ２を回転駆動するように構成される。駆動装置ＭＡ１及びＭＡ２はそれぞれ、駆動源としての電気モータ（図示略）と、減速機（図示略）とを含む。駆動装置ＭＡ１及びＭＡ２の減速機はそれぞれ、それぞれの電気モータの回転駆動力の回転速度を減速し且つ回転駆動力を増大させて関節ＪＴＡ１及びＪＴＡ２に伝達する。本実施の形態では、電気モータはサーボモータである。
【0017】
なお、本明細書及び特許請求の範囲において、「水平方向」及び「鉛直方向」はそれぞれ、水平な支持面Ｓ上にロボット１が配置される場合での「水平方向」及び「鉛直方向」を意味する。さらに、「上方向」は、同様の場合での支持面Ｓに垂直な上向きの方向、つまり、鉛直上方向を意味し、「下方向」は、同様の場合での支持面Ｓに垂直な下向きの方向、つまり、鉛直下方向を意味する。さらに、本明細書及び特許請求の範囲において、「垂直」、「鉛直」、「水平」及び「平行」はそれぞれ、完全に垂直、鉛直、水平又は平行である場合と、完全な垂直、鉛直、水平又は平行の近傍を含む実質的に垂直、鉛直、水平又は平行とみなすことができる場合とを含み得る。
【0018】
第２ロボットアーム２００について、リンク２０７は、第２ロボットアーム２００の基部に位置し、リンク１０２の上面に固定される。リンク２０７は、第１ロボットアーム１００よりも上方に位置する。リンク２０７とリンク１０２との界面１１０は、ロボットアーム１００及び２００の接続部分の一例である。これに限定されないが、本実施の形態では、界面１１０の中心点１１０Ａは、ロボットアーム１００及び２００の基準点である。以下において、「界面１１０」を「接続部分１１０」と表現する場合があり、「中心点１１０Ａ」を「基準点１１０Ａ」と表現する場合がある。
【0019】
これに限定されないが、本実施の形態では、リンク２０７は、台状の形状を有し、リンク２０１～２０６の基台として機能する。リンク２０６は、第２ロボットアーム２００の先端部に位置し、リンク２０１～２０５は、リンク２０６及び２０７の間で、リンク２０７からリンク２０６に向かってこの順で配列される。リンク２０６の先端に、作業の対象物Ｗに作用を加えることができるエンドエフェクタＥが取り付けられる。これに限定されないが、本実施の形態では、エンドエフェクタＥは対象物Ｗを把持できるように構成される。
【0020】
関節ＪＴＢ１～ＪＴＢ６はいずれも、１つの軸を中心に回転可能である１自由度の回転関節である。関節ＪＴＢ１は、リンク２０１をリンク２０７に回動可能に接続する。関節ＪＴＢ２は、リンク２０２をリンク２０１に回動可能に接続する。関節ＪＴＢ３は、リンク２０３をリンク２０２に回動可能に接続する。関節ＪＴＢ４は、リンク２０４をリンク２０３に回動可能に接続する。関節ＪＴＢ５は、リンク２０５をリンク２０４に回動可能に接続する。関節ＪＴＢ６は、リンク２０６をリンク２０５に回動可能に接続する。
【0021】
駆動装置ＭＢ１～ＭＢ６はそれぞれ、関節ＪＴＢ１～ＪＴＢ６を回転駆動するように構成される。駆動装置ＭＢ１～ＭＢ６はそれぞれ、駆動源としての電気モータ（図示略）と、減速機（図示略）とを含む。本実施の形態では、電気モータはサーボモータである。
【0022】
これに限定されないが、本実施の形態では、制御装置３００は、ロボット１全体の動作を制御するように構成される。制御装置３００は、情報処理装置３００Ａとロボットコントローラ３００Ｂとを含む。ロボットコントローラ３００Ｂは、ロボットアーム１００及び２００の動作を制御するように構成される。情報処理装置３００Ａは、ロボットアーム１００及び２００の動作制御に関連する演算処理を行うように構成される。
【0023】
情報処理装置３００Ａ及びロボットコントローラ３００Ｂはコンピュータ装置を含む。情報処理装置３００Ａの構成は特に限定されないが、例えば、情報処理装置３００Ａは、電子回路基板、電子制御ユニット、マイクロコンピュータ、パーソナルコンピュータ、ワークステーション、スマートフォン及びタブレットなどのスマートデバイス、並びにその他の電子機器等であってもよい。ロボットコントローラ３００Ｂは、ロボットアーム１００及び２００等に供給する電力を制御するための電気回路を含んでもよい。
【0024】
［制御装置のハードウェア構成］
図３を参照しつつ、実施の形態に係る制御装置３００のハードウェア構成を説明する。図３は、実施の形態に係る制御装置３００のハードウェア構成の一例を示すブロック図である。情報処理装置３００Ａは、プロセッサ３０１Ａと、メモリ３０２Ａと、ストレージ３０３Ａと、入出力Ｉ／Ｆ（インタフェース：Interface）３０４Ａとを構成要素として含む。情報処理装置３００Ａの各構成要素は、バス３１０Ａによって相互に接続されるが、他のいかなる有線通信又は無線通信で接続されてもよい。ロボットコントローラ３００Ｂは、プロセッサ３０１Ｂと、メモリ３０２Ｂと、入出力Ｉ／Ｆ３０４Ｂと、駆動Ｉ／Ｆ３０５Ｂ及び３０６Ｂとを構成要素として含む。ロボットコントローラ３００Ｂは、ストレージを含んでもよい。ロボットコントローラ３００Ｂの各構成要素は、バス３１０Ｂによって相互に接続されるが、他のいかなる有線通信又は無線通信で接続されてもよい。情報処理装置３００Ａ及びロボットコントローラ３００Ｂそれぞれに含まれる構成要素の全てが必須ではない。
図３を参照しつつ、実施の形態に係る制御装置３００のハードウェア構成を説明する。図３は、実施の形態に係る制御装置３００のハードウェア構成の一例を示すブロック図である。情報処理装置３００Ａは、プロセッサ３０１Ａと、メモリ３０２Ａと、ストレージ３０３Ａと、入出力Ｉ／Ｆ（インタフェース：Interface）３０４Ａとを構成要素として含む。情報処理装置３００Ａの各構成要素は、バス３１０Ａによって相互に接続されるが、他のいかなる有線通信又は無線通信で接続されてもよい。ロボットコントローラ３００Ｂは、プロセッサ３０１Ｂと、メモリ３０２Ｂと、入出力Ｉ／Ｆ３０４Ｂと、駆動Ｉ／Ｆ３０５Ｂ及び３０６Ｂとを構成要素として含む。ロボットコントローラ３００Ｂは、ストレージを含んでもよい。ロボットコントローラ３００Ｂの各構成要素は、バス３１０Ｂによって相互に接続されるが、他のいかなる有線通信又は無線通信で接続されてもよい。情報処理装置３００Ａ及びロボットコントローラ３００Ｂそれぞれに含まれる構成要素の全てが必須ではない。
【0025】
プロセッサ３０１Ａ及びメモリ３０２Ａの組、及び、プロセッサ３０１Ｂ及びメモリ３０２Ｂの組はそれぞれ、演算器を構成する。演算器は、他の装置との指令、情報及びデータ等の送受信を行う。演算器は、各種機器からの信号の入力及び各制御対象への制御信号の出力を行う。
【0026】
メモリ３０２Ａ及び３０２Ｂはそれぞれ、プロセッサ３０１Ａ及び３０１Ｂが実行するプログラム、及び各種固定データ等を記憶する。メモリ３０２Ａ及び３０２Ｂは、揮発性メモリ及び不揮発性メモリなどの半導体メモリ等の記憶装置で構成されてもよい。これに限定されないが、本実施の形態では、メモリ３０２Ａ及び３０２Ｂは、揮発性メモリであるＲＡＭ（Random Access Memory）と不揮発性メモリであるＲＯＭ（Read-Only Memory）とを含む。
【0027】
ストレージ３０３Ａは、各種データを記憶する。ストレージ３０３Ａは、半導体メモリ、ハードディスクドライブ（ＨＤＤ：Hard Disk Drive）及び固体ドライブ（ＳＳＤ：Solid State Drive）等の記憶装置で構成されてもよい。
【0028】
プロセッサ３０１Ａ及び３０１Ｂはいずれも、ＲＡＭ及びＲＯＭと一緒にコンピュータシステムを形成する。情報処理装置３００Ａのコンピュータシステムは、プロセッサ３０１ＡがＲＡＭをワークエリアとして用いてＲＯＭに記録されたプログラムを実行することによって、情報処理装置３００Ａの機能を実現してもよい。ロボットコントローラ３００Ｂのコンピュータシステムは、プロセッサ３０１ＢがＲＡＭをワークエリアとして用いてＲＯＭに記録されたプログラムを実行することによって、ロボットコントローラ３００Ｂの機能を実現してもよい。
【0029】
情報処理装置３００Ａ及びロボットコントローラ３００Ｂの機能の一部又は全部は、上記コンピュータシステムにより実現されてもよく、電子回路又は集積回路等の専用のハードウェア回路により実現されてもよく、上記コンピュータシステム及びハードウェア回路の組み合わせにより実現されてもよい。情報処理装置３００Ａ及びロボットコントローラ３００Ｂはそれぞれ、単一の装置による集中制御により各処理を実行するように構成されてもよく、複数の装置の協働による分散制御により各処理を実行するように構成されてもよい。なお、情報処理装置３００Ａ及びロボットコントローラ３００Ｂは、互いの機能の少なくとも一部を含むように構成されてもよく、一体化されてもよい。
【0030】
これに限定されないが、例えば、プロセッサ３０１Ａ及び３０１Ｂは、ＣＰＵ（中央処理装置：Central Processing Unit）、ＭＰＵ（Micro Processing Unit）、ＧＰＵ（Graphics Processing Unit）、マイクロプロセッサ（microprocessor）、プロセッサコア（processor core）、マルチプロセッサ（multiprocessor）、ＡＳＩＣ（Application-Specific Integrated Circuit）、ＦＰＧＡ（Field Programmable Gate Array）等を含み、ＩＣ（集積回路）チップ、ＬＳＩ（Large Scale Integration）等に形成された論理回路又は専用回路によって各処理を実現してもよい。複数の処理は、１つ又は複数の集積回路により実現されてもよく、１つの集積回路により実現されてもよい。
【0031】
情報処理装置３００Ａの入出力Ｉ／Ｆ３０４Ａは、情報処理装置３００Ａとロボットコントローラ３００Ｂとを接続し、これらの間での情報、指令及びデータ等の入出力を可能にする。ロボットコントローラ３００Ｂの入出力Ｉ／Ｆ３０４Ｂは、ロボットコントローラ３００Ｂと情報処理装置３００Ａの入出力Ｉ／Ｆ３０４Ａとを接続し、これらの間での情報、指令及びデータ等の入出力を可能にする。
【0032】
第１駆動Ｉ／Ｆ３０５Ｂは、ロボットコントローラ３００Ｂと第１ロボットアーム１００の第１駆動回路１２０とを接続し、これらの間での信号等の送受信を可能にする。第１駆動回路１２０は、ロボットコントローラ３００Ｂから受信する信号に含まれる指令値に従って、第１ロボットアーム１００の駆動装置ＭＡ１及びＭＡ２に供給する電力を制御するように構成される。
【0033】
第２駆動Ｉ／Ｆ３０６Ｂは、ロボットコントローラ３００Ｂと第２ロボットアーム２００の第２駆動回路２２０とを接続し、これらの間での信号等の送受信を可能にする。第２駆動回路２２０は、ロボットコントローラ３００Ｂから受信する信号に含まれる指令値に従って、第２ロボットアーム２００の駆動装置ＭＢ１～ＭＢ６に供給する電力を制御するように構成される。
【0034】
なお、ロボットコントローラ３００Ｂは、駆動装置ＭＡ１、ＭＡ２及びＭＢ１～ＭＢ６のサーボモータをサーボ制御するように構成されてもよい。ロボットコントローラ３００Ｂは、各サーボモータが備える回転センサの検出値と、駆動回路１２０及び２２０から各サーボモータへの電流の指令値とを、フィードバック情報として駆動回路１２０及び２２０から受信する。ロボットコントローラ３００Ｂは、フィードバック情報を用いて各サーボモータの駆動の指令値を決定し、駆動回路１２０及び２２０に送信する。
【0035】
ロボットコントローラ３００Ｂは、複数のサーボモータの軸制御を互いに連携できるように構成されてもよい。ロボットコントローラ３００Ｂは、複数の軸制御のうちの一部であるロボット軸制御として駆動装置ＭＢ１～ＭＢ６のサーボモータを制御し、複数の軸制御のうちの一部である外部軸制御として駆動装置ＭＡ１及びＭＡ２のサーボモータを制御するように構成されてもよい。
【0036】
［制御装置の機能的構成］
図４を参照しつつ、実施の形態に係る制御装置３００の機能的構成を説明する。図４は、実施の形態に係る制御装置３００の機能的構成の一例を示すブロック図である。情報処理装置３００Ａは、第１動作指令部４０１と、第２動作指令部４０２と、第１推定部４０３と、第２推定部４０４と、第３推定部４０５と、第４推定部４０６と、第１位置推定部４０７と、第２位置推定部４０８と、慣性要素推定部４０９と、慣性力推定部４１０と、補正指令部４１１と、記憶部４２１～４２３とを機能的構成要素として含む。記憶部４２１～４２３を除く機能的構成要素の機能は、プロセッサ３０１Ａ等によって実現され、記憶部４２１～４２３の機能は、メモリ３０２Ａ及び／又はストレージ３０３Ａ等の記憶装置によって実現される。記憶部４２１～４２３それぞれの機能は、別々の記憶装置によって実現されてもよく、記憶部４２１～４２３の機能の少なくとも２つが１つの記憶装置によって実現されてもよい。上記の機能的構成要素の全てが必須ではない。
図４を参照しつつ、実施の形態に係る制御装置３００の機能的構成を説明する。図４は、実施の形態に係る制御装置３００の機能的構成の一例を示すブロック図である。情報処理装置３００Ａは、第１動作指令部４０１と、第２動作指令部４０２と、第１推定部４０３と、第２推定部４０４と、第３推定部４０５と、第４推定部４０６と、第１位置推定部４０７と、第２位置推定部４０８と、慣性要素推定部４０９と、慣性力推定部４１０と、補正指令部４１１と、記憶部４２１～４２３とを機能的構成要素として含む。記憶部４２１～４２３を除く機能的構成要素の機能は、プロセッサ３０１Ａ等によって実現され、記憶部４２１～４２３の機能は、メモリ３０２Ａ及び／又はストレージ３０３Ａ等の記憶装置によって実現される。記憶部４２１～４２３それぞれの機能は、別々の記憶装置によって実現されてもよく、記憶部４２１～４２３の機能の少なくとも２つが１つの記憶装置によって実現されてもよい。上記の機能的構成要素の全てが必須ではない。
【0037】
ロボットコントローラ３００Ｂは、第１駆動指令部５０１と、第２駆動指令部５０２とを機能的構成要素として含む。駆動指令部５０１及び５０２の機能は、プロセッサ３０１Ｂ等によって実現される。上記の機能的構成要素の全てが必須ではない。
【0038】
記憶部４２１～４２３は、様々な情報及びデータ等を記憶し、記憶している情報及びデータ等の読み出しを可能にする。例えば、第１記憶部４２１は、ロボットアーム１００及び２００の制御プログラムを記憶する。これに限定されないが、本実施の形態では、制御装置３００は、制御プログラムに従ってロボットアーム１００及び２００に自律的に動作させる、つまり、ロボットアーム１００及び２００に自動運転させるように構成される。第１記憶部４２１は、ロボットアーム１００及び２００に自律的に動作させるための教示データ等の目標動作データを記憶してもよい。なお、制御装置３００は、図示しない操作装置から受け取る操作指令に従ってロボットアーム１００及び２００に動作させる、つまり、ロボットアーム１００及び２００に手動運転させるように構成されてもよい。
【0039】
例えば、第２記憶部４２２は、第１ロボットアーム１００の情報を記憶する。第１ロボットアーム１００の情報は、リンク１０１～１０３並びに関節ＪＴＡ１及びＪＴＡ２の情報を含む。リンク１０１～１０３の情報は、リンク１０１～１０３それぞれの質量、寸法、形状、重心位置、慣性モーメント、剛性、及び関節との接続位置等の情報を含んでもよい。リンクの剛性は、リンクの曲げ剛性及び捻り剛性等を含んでもよい。関節ＪＴＡ１及びＪＴＡ２の情報は、関節ＪＴＡ１及びＪＴＡ２それぞれの質量、寸法、形状、重心位置、慣性モーメント、剛性、回転中心軸の位置及び方向、並びにリンクとの接続位置等の情報を含んでもよい。関節の剛性は、関節を構成する部材の剛性、関節の回転部分の剛性、及び関節の減速機の剛性等を含んでもよい。
【0040】
リンク１０１～１０３の情報は、リンク１０１～１０３の上記情報が反映された仮想的なモデルであってもよく、関節ＪＴＡ１及びＪＴＡ２の情報は、関節ＪＴＡ１及びＪＴＡ２の上記情報が反映された仮想的なモデルであってもよい。例えば、仮想的なモデルは、３次元ＣＡＤ（Computer-Aided Design）モデル等の３次元モデルであってもよい。
【0041】
例えば、第３記憶部４２３は、第２ロボットアーム２００の情報を記憶する。第２ロボットアーム２００の情報は、リンク２０１～２０７及び関節ＪＴＢ１～ＪＴＢ６の情報を含む。さらに、第２ロボットアーム２００の情報は、第２ロボットアーム２００に装着されるエンドエフェクタＥの情報、及び、エンドエフェクタＥによって保持されるワーク等の対象物Ｗの情報等を含み得る。リンク２０１～２０７の情報は、リンク２０１～２０７それぞれの質量、寸法、形状、重心位置、慣性モーメント、及び関節との接続位置等の情報を含んでもよい。関節ＪＴＢ１～ＪＴＢ６の情報は、関節ＪＴＢ１～ＪＴＢ６それぞれの質量、寸法、形状、重心位置、慣性モーメント、回転中心軸の位置及び方向、並びにリンクとの接続位置等の情報を含んでもよい。エンドエフェクタＥの情報は、その質量、寸法、形状、重心位置、慣性モーメント、及びリンク２０６との接続位置等の情報を含んでもよい。対象物Ｗの情報は、その質量、寸法、形状、重心位置及び保持されるときの姿勢等の情報を含んでもよい。リンク２０１～２０７、関節ＪＴＢ１～ＪＴＢ６、エンドエフェクタＥ及び対象物Ｗの情報は、それぞれの上記情報が反映された仮想的なモデルであってもよい。例えば、仮想的なモデルは、３次元ＣＡＤモデル等の３次元モデルであってもよい。
【0042】
第１動作指令部４０１は、第１記憶部４２１に記憶される第１ロボットアーム１００の目標動作データである第１目標動作データに従った目標動作を第１ロボットアーム１００に実行させるための動作指令（以下、「第１動作指令」とも称する）を生成する。第１目標動作データは、制御プログラムに従って第１ロボットアーム１００を動作させるためのデータである。例えば、第１動作指令部４０１は、第１目標動作データに従って接続部分１１０を移動させるための接続部分１１０の目標位置及び目標姿勢を、第２記憶部４２２に記憶される第１ロボットアーム１００の情報を用いて演算する。目標位置及び目標姿勢は、基準点１１０Ａの３次元の位置及び接続部分１１０の界面の３次元の姿勢であってもよい。
【0043】
さらに、第１動作指令部４０１は、接続部分１１０を目標位置及び目標姿勢に移動させるための関節ＪＴＡ１及びＪＴＡ２の目標回転方向及び目標回転量を、第２記憶部４２２に記憶される第１ロボットアーム１００の情報を用いて演算する。つまり、第１動作指令部４０１は、関節ＪＴＡ１及びＪＴＡ２の目標回転位置を演算する。第１動作指令部４０１は、第１目標動作データと第１ロボットアーム１００の情報とを用いて、関節ＪＴＡ１及びＪＴＡ２の目標回転位置を演算するように構成されてもよい。第１動作指令部４０１は、関節ＪＴＡ１及びＪＴＡ２の目標回転位置を含む第１動作指令を生成する。なお、手動運転の場合、第１動作指令部４０１は、操作装置（図示略）から受けとる操作指令に従った目標動作を第１ロボットアーム１００に実行させるための第１動作指令を上記と同様に生成する。
【0044】
第２動作指令部４０２は、第１記憶部４２１に記憶される第２ロボットアーム２００の目標動作データである第２目標動作データに従った目標動作を第２ロボットアーム２００に実行させるための動作指令（以下、「第２動作指令」とも称する）を生成する。第２目標動作データは、制御プログラムに従って第２ロボットアーム２００を動作させるためのデータである。例えば、第２動作指令部４０２は、第２目標動作データに従ってリンク２０６を移動させるためのリンク２０６の目標位置及び目標姿勢を、第３記憶部４２３に記憶される第２ロボットアーム２００の情報を用いて演算する。目標位置及び目標姿勢は、リンク２０６の先端部の３次元の位置及び３次元の姿勢であってもよい。
【0045】
さらに、第２動作指令部４０２は、リンク２０６を目標位置及び目標姿勢に移動させるための関節ＪＴＢ１～ＪＴＢ６の目標回転方向及び目標回転量を、第３記憶部４２３に記憶される第２ロボットアーム２００の情報を用いて演算する。つまり、第２動作指令部４０２は、関節ＪＴＢ１～ＪＴＢ６の目標回転位置を演算する。第２動作指令部４０２は、第２目標動作データと第２ロボットアーム２００の情報とを用いて、関節ＪＴＢ１～ＪＴＢ６の目標回転位置を演算するように構成されてもよい。第２動作指令部４０２は、関節ＪＴＢ１～ＪＴＢ６の目標回転位置を含む第２動作指令を生成する。なお、手動運転の場合、第２動作指令部４０２は、操作装置（図示略）から受けとる操作指令に従った目標動作を第２ロボットアーム２００に実行させるための第２動作指令を上記と同様に生成する。
【0046】
第１推定部４０３は、第１動作指令部４０１から第１動作指令を取得し、第１ロボットアーム１００が第１動作指令に従って動作する場合の関節ＪＴＡ１及びＪＴＡ２の動作を推定する。例えば、第１推定部４０３は、関節ＪＴＡ１及びＪＴＡ２の動作として、関節ＪＴＡ１及びＪＴＡ２の回転位置の推定値である推定回転位置を算出する。関節ＪＴＡ１及びＪＴＡ２の推定回転位置は、関節ＪＴＡ１及びＪＴＡ２の回転方向及び回転量の推定値を含む。関節ＪＴＡ１及びＪＴＡ２の推定回転位置は、第１動作指令に含まれる関節ＪＴＡ１及びＪＴＡ２の目標回転位置に対応する。さらに、第１推定部４０３は、関節ＪＴＡ１及びＪＴＡ２の推定回転位置に基づき、関節ＪＴＡ１及びＪＴＡ２の回転速度の推定値である推定回転速度を算出する。
【0047】
第２推定部４０４は、第２動作指令部４０２から第２動作指令を取得し、第２ロボットアーム２００が第２動作指令に従って動作する場合に、接続部分１１０が基準点１１０Ａにおいて第２ロボットアーム２００から受ける力を推定する。これに限定されないが、本実施の形態では、第２推定部４０４は、基準点１１０Ａを原点Оとする基準座標系を用いて、上記力を推定する。
【0048】
基準座標系は、基準点１１０Ａを原点Оとする座標系であればよい。これに限定されないが、本実施の形態では、基準座標系は、図２に示すように、基準点１１０Ａから接続部分１１０の界面に沿って延び且つ互いに直交するＸ軸及びＹ軸と、基準点１１０Ａから接続部分１１０の界面と垂直な方向に延び且つＸ軸及びＹ軸と直交するＺ軸とにより定義される。Ｚ軸正方向は、リンク２０７からリンク１０２に向かう方向である。又は、例えば、基準座標系は、基準点１１０Ａから水平方向に延び且つ互いに直交するＸ軸及びＹ軸と、基準点１１０Ａから鉛直下方向に延び且つＸ軸及びＹ軸と直交するＺ軸とにより定義されてもよい。鉛直下方向は重力方向であってもよく、水平方向は重力方向に垂直な方向であってもよい。
【0049】
例えば、上記力は、第２ロボットアーム２００の荷重と、第２ロボットアーム２００が接続部分１１０に発生するモーメントとを含み得る。例えば、第２推定部４０４は、第２ロボットアーム２００が第２動作指令に従って動作する場合のリンク２０１～２０７及び関節ＪＴＢ１～ＪＴＢ６の位置及び姿勢を推定し、推定結果と第３記憶部４２３に記憶される第２ロボットアーム２００の情報とを用いて、上記力を推定してもよい。
【0050】
第２ロボットアーム２００の荷重は、第２ロボットアーム２００自体の質量と、エンドエフェクタＥの質量と、エンドエフェクタＥによって保持されている対象物Ｗの質量とを含み得る。第２ロボットアーム２００のモーメントは、第２ロボットアーム２００自体とエンドエフェクタＥと対象物Ｗとが接続部分１１０に発生するモーメントを含み得る。第２推定部４０４は、第３記憶部４２３に記憶される第２ロボットアーム２００の情報と第２動作指令に含まれる関節ＪＴＢ１～ＪＴＢ６の目標回転位置とを用いて、第２ロボットアーム２００の荷重及びモーメントを演算する。第２推定部４０４は、基準座標系を用いた第２ロボットアーム２００の荷重及びモーメントを表す。さらに、第２推定部４０４は、第３記憶部４２３に記憶される第２ロボットアーム２００の情報と第２動作指令に含まれる関節ＪＴＢ１～ＪＴＢ６の目標回転位置とを用いて、第２ロボットアーム２００全体の慣性モーメントを、基準座標系を基準に演算する。当該慣性モーメントは、基準座標系の原点Оを基準とする慣性モーメントである。
【0051】
第３推定部４０５は、第１推定部４０３によって推定される関節ＪＴＡ１及びＪＴＡ２の動作と、第２推定部４０４によって推定される接続部分１１０が第２ロボットアーム２００から受ける力とに基づき、リンク１０１～１０３が受ける力を推定する。これに限定されないが、本実施の形態では、台状形状のリンク１０３は、リンク１０１及び関節ＪＴＡ１を介して力を受けても、たわみ等の変形をほとんど生じず不動であるため、第３推定部４０５は、リンク１０１及び１０２が受ける力を推定する。
【0052】
例えば、第３推定部４０５は、第１推定部４０３によって推定される関節ＪＴＡ１及びＪＴＡ２の動作に基づき慣性力推定部４１０によって推定される、リンク１０１及び１０２に発生する慣性力と、接続部分１１０が第２ロボットアーム２００から受ける力とに基づき、リンク１０１及び１０２が受ける力を推定する。
【0053】
第４推定部４０６は、第３推定部４０５によって推定されるリンク１０１及び１０２が受ける力に基づき、リンク１０１及び１０２の変形量と関節ＪＴＡ１及びＪＴＡ２の変形量とのうちの少なくとも一方を推定する。本実施の形態では、第４推定部４０６は、リンク１０１及び１０２の変形量と、関節ＪＴＡ１及びＪＴＡ２の変形量との両方を推定する。
【0054】
第１位置推定部４０７は、リンク１０１及び１０２並びに関節ＪＴＡ１及びＪＴＡ２それぞれの位置及び姿勢を推定する。第１位置推定部４０７は、リンク１０１及び１０２並びに関節ＪＴＡ１及びＪＴＡ２それぞれの３次元の位置及び３次元の姿勢を、基準座標系を用いて推定する。例えば、第１位置推定部４０７は、第１推定部４０３によって推定される関節ＪＴＡ１及びＪＴＡ２の推定回転位置と、第２記憶部４２２に記憶される第１ロボットアーム１００の情報とを用いて、当該推定回転位置でのリンク１０１及び１０２並びに関節ＪＴＡ１及びＪＴＡ２それぞれの推定位置及び推定姿勢を演算する。第１位置推定部４０７は、リンク１０１及び１０２並びに関節ＪＴＡ１及びＪＴＡ２それぞれの推定位置及び推定姿勢に基づき、リンク１０１及び１０２の慣性モーメントを、基準座標系を基準に演算する。当該慣性モーメントは、基準座標系の原点Оを基準とする慣性モーメントである。
【0055】
第２位置推定部４０８は、接続部分１１０の位置を推定する。具体的には、第２位置推定部４０８は、接続部分１１０の基準点１１０Ａの３次元の位置及び接続部分１１０の界面の３次元の姿勢を、基準座標系を用いて推定する。例えば、第１位置推定部４０７は、第１推定部４０３によって推定される関節ＪＴＡ１及びＪＴＡ２の推定回転位置と、第２記憶部４２２に記憶される第１ロボットアーム１００の情報とを用いて、当該推定回転位置での接続部分１１０の推定位置及び推定姿勢を演算してもよい。第２位置推定部４０８は、第１位置推定部４０７によって推定されるリンク１０１及び１０２並びに関節ＪＴＡ１及びＪＴＡ２それぞれの推定位置及び推定姿勢を用いて、接続部分１１０の推定位置及び推定姿勢を演算してもよい。第２位置推定部４０８は、第１動作指令部４０１から、関節ＪＴＡ１及びＪＴＡ２の目標回転位置に対応する接続部分１１０の目標位置及び目標姿勢を取得し、当該目標位置及び目標姿勢を基準座標系に変換し推定位置及び推定姿勢としてもよい。
【0056】
慣性要素推定部４０９は、リンク１０１～１０３の慣性的特徴と第２ロボットアーム２００の慣性的特徴とを推定する。これに限定されないが、本実施の形態では、リンク１０３は、リンク１０１及び関節ＪＴＡ１を介して力を受けても、たわみ等の変形をほとんど生じず不動であるため、慣性要素推定部４０９は、リンク１０１及び１０２の慣性的特徴を推定する。リンク１０１及び１０２の慣性的特徴は、リンク１０１及び１０２の慣性モーメント及び質量を要素として含む。第２ロボットアーム２００の慣性的特徴は、第２ロボットアーム２００の慣性モーメント及び質量を要素として含む。
【0057】
慣性力推定部４１０は、第１推定部４０３によって推定される関節ＪＴＡ１及びＪＴＡ２の動作に基づき、関節ＪＴＡ１及びＪＴＡ２それぞれが動作することによってリンク１０１～１０３に発生する慣性力（以下、「関節起因慣性力」とも称する）を推定する。これに限定されないが、本実施の形態では、リンク１０３は、リンク１０１及び関節ＪＴＡ１を介して力を受けても、たわみ等の変形をほとんど生じず不動であるため、慣性力推定部４１０は、リンク１０１及び１０２に発生する関節起因慣性力を推定する。
【0058】
具体的には、慣性力推定部４１０は、リンク１０１及び１０２の慣性的特徴と、第２ロボットアーム２００の慣性的特徴と、関節ＪＴＡ１及びＪＴＡ２の加速度と、関節ＪＴＡ１及びＪＴＡ２のジョイントスクリュとを用いて、リンク１０１及び１０２に発生する関節起因慣性力を推定する。関節ＪＴＡ１及びＪＴＡ２のジョイントスクリュはそれぞれ、当該関節の向きを示す要素を含む当該関節の状態を示す特徴である。
【0059】
補正指令部４１１は、第４推定部４０６によって推定されるリンク１０１及び１０２の変形量並びに／又は関節ＪＴＡ１及びＪＴＡ２の変形量に基づき、第１目標動作に従って接続部分１１０を移動させるように上記変形量を補償するための第１動作指令の補正指令を生成する。補正指令部４１１は、当該補正指令を第１ロボットアーム１００、具体的には、第１駆動指令部５０１に出力する。これに限定されないが、本実施の形態では、補正指令部４１１は、上記変形量を補償するために第１動作指令を補正し、補正後の第１動作指令を補正指令として生成する。補正指令部４１１は、第１動作指令を補正しない場合、未補正の第１動作指令を補正指令として、第１駆動指令部５０１に出力する。また、補正指令部４１１は、第２動作指令を第２ロボットアーム２００、具体的には、第２駆動指令部５０２に出力する。
【0060】
第１駆動指令部５０１は、補正指令に含まれる関節ＪＴＡ１及びＪＴＡ２の目標回転位置にまで駆動装置ＭＡ１及びＭＡ２に関節ＪＴＡ１及びＪＴＡ２を駆動させるための駆動指令を生成し、駆動装置ＭＡ１及びＭＡ２に出力する。駆動指令は、駆動装置ＭＡ１及びＭＡ２のサーボモータの電流の指令値を含む。第１駆動指令部５０１は、サーボモータのフィードバック情報を用いて駆動指令を生成する。
【0061】
第２駆動指令部５０２は、第２動作指令に含まれる関節ＪＴＢ１～ＪＴＢ６の目標回転位置にまで駆動装置ＭＢ１～ＭＢ６に関節ＪＴＢ１～ＪＴＢ６を駆動させるための駆動指令を生成し、駆動装置ＭＢ１～ＭＢ６に出力する。駆動指令は、駆動装置ＭＢ１～ＭＢ６のサーボモータの電流の指令値を含む。第２駆動指令部５０２は、サーボモータのフィードバック情報を用いて駆動指令を生成する。
【0062】
［制御装置の動作］
図５を参照しつつ、制御装置３００の動作を説明する。図５は、実施の形態に係る制御装置３００の動作の一例を示すフローチャートである。まず、情報処理装置３００Ａにおいて、第１動作指令部４０１は、第１記憶部４２１に記憶される第１目標動作データに従って第１ロボットアーム１００を動作させるための第１動作指令を生成する（ステップＳ１０１）。
図５を参照しつつ、制御装置３００の動作を説明する。図５は、実施の形態に係る制御装置３００の動作の一例を示すフローチャートである。まず、情報処理装置３００Ａにおいて、第１動作指令部４０１は、第１記憶部４２１に記憶される第１目標動作データに従って第１ロボットアーム１００を動作させるための第１動作指令を生成する（ステップＳ１０１）。
【0063】
次いで、第２動作指令部４０２は、第１記憶部４２１に記憶される第２目標動作データに従って第２ロボットアーム２００を動作させるための第２動作指令を生成する（ステップＳ１０２）。
【0064】
次いで、第１推定部４０３は、第１ロボットアーム１００が第１動作指令に従って動作する場合の関節ＪＴＡ１及びＪＴＡ２の動作を推定する（ステップＳ１０３）。第１推定部４０３は、推定対象動作として、関節ＪＴＡ１及びＪＴＡ２の推定回転位置及び推定回転速度を算出する。
【0065】
次いで、第１位置推定部４０７は、第１ロボットアーム１００が第１動作指令に従って動作する場合のリンク１０１及び１０２並びに関節ＪＴＡ１及びＪＴＡ２それぞれの位置及び姿勢を推定する（ステップＳ１０４）。例えば、第１位置推定部４０７は、第１推定部４０３によって推定される関節ＪＴＡ１及びＪＴＡ２の推定回転位置と、第２記憶部４２２に記憶される第１ロボットアーム１００の情報とを用いて、リンク１０１及び１０２並びに関節ＪＴＡ１及びＪＴＡ２それぞれの推定位置及び推定姿勢を演算してもよい。
【0066】
例えば、第１位置推定部４０７は、リンク１０１及び１０２の推定位置として、リンク１０１及び１０２の重心の推定位置を、基準座標系を基準に演算する。例えば、第１位置推定部４０７は、基準座標系での第１リンク１０１の重心の推定位置ベクトルγｇ１оと、基準座標系での第２リンク１０２の重心の推定位置ベクトルγｇ2оとを算出する。以下において、基準座標系の原点Оである基準点１１０Ａを基準とする第ｉリンクの位置ベクトルを「γｇｉо」と表す。リンクの番号「ｉ」は、第１ロボットアーム１００の基端から先端に向かって増加する。なお、第１位置推定部４０７は、リンク１０１及び１０２に設定される他の基準点の推定位置を、基準座標系を基準に演算するように構成されてもよい。
【0067】
例えば、第１位置推定部４０７は、関節ＪＴＡ１及びＪＴＡ２の推定位置及び推定姿勢として、関節ＪＴＡ１及びＪＴＡ２の重心の推定位置及び基準軸の推定姿勢を、基準座標系を基準に演算する。例えば、第１位置推定部４０７は、基準座標系での第１関節ＪＴＡ１の重心の推定位置ベクトルγ１оと、基準座標系での第２関節ＪＴＡ２の重心の推定位置ベクトルγ2оとを算出する。以下において、基準座標系の原点Оを基準とする第ｊ関節の位置ベクトルを「γｊо」と表す。関節の番号「ｊ」は、第１ロボットアーム１００の基端から先端に向かって増加する。なお、第１位置推定部４０７は、関節ＪＴＡ１及びＪＴＡ２に設定される他の基準点の推定位置を、基準座標系を基準に演算するように構成されてもよい。
【0068】
例えば、第１位置推定部４０７は、関節ＪＴＡ１及びＪＴＡ２の基準軸の推定姿勢として、基準座標系での第１関節ＪＴＡ１の推定方向ベクトルｄ１оと、基準座標系での第２関節ＪＴＡ２の推定方向ベクトルｄ２оとを算出する。以下において、基準座標系の原点Оを基準とする第ｊ関節の方向ベクトルを「ｄｊо」と表す。方向ベクトルｄｊоは、第ｊ関節の動作の基準軸の方向を示すベクトルである。例えば、第ｊ関節が回転関節である場合、方向ベクトルｄｊоは、関節軸である回転中心軸の方向ベクトルである。第ｊ関節が直動関節である場合、方向ベクトルｄｊоは、関節軸である直動方向の軸の方向ベクトルである。
【0069】
さらに、第１位置推定部４０７は、リンク１０１及び１０２の推定位置並びに関節ＪＴＡ１及びＪＴＡ２の推定位置及び推定姿勢と、第２記憶部４２２に記憶される第１ロボットアーム１００の情報とを用いて、基準座標系の原点Оを基準とするリンク１０１及び１０２の慣性モーメントｌ１О及びｌ２Оを演算する。以下において、基準座標系の原点Оを基準とする第ｉリンクの慣性モーメントを「ｌｉО」と表す。
【0070】
次いで、第２位置推定部４０８は、第１ロボットアーム１００が第１動作指令に従って動作する場合の接続部分１１０の位置及び姿勢を推定する（ステップＳ１０５）。例えば、第１位置推定部４０７は、第１位置推定部４０７によって推定されるリンク１０１及び１０２並びに関節ＪＴＡ１及びＪＴＡ２の推定位置及び推定姿勢と、第２記憶部４２２に記憶される第１ロボットアーム１００の情報とを用いて、接続部分１１０の推定位置及び推定姿勢を演算してもよい。
【0071】
第２位置推定部４０８は、基準点１１０Ａの推定位置と、接続部分１１０の界面の推定姿勢とを、ロボット座標系を基準に算出する。つまり、第２位置推定部４０８は、基準座標系の推定位置及び推定姿勢を、ロボット座標系を基準に算出する。ロボット座標系は、ロボット１を基準に定義される座標系である。これに限定されないが、本実施の形態では、図２に示すように、ロボット座標系の原点Оｒは、支持面Ｓ上のリンク１０３の位置に設定される。Ｘｒ軸及びＹｒ軸は、原点Оｒから支持面Ｓに沿って延び、互いに直交する。Ｚｒ軸は、原点Оｒから支持面Ｓに垂直に上方へ延び、Ｘｒ軸及びＹｒ軸と直交する。
【0072】
次いで、第２推定部４０４は、第２ロボットアーム２００が第２動作指令に従って動作する場合に接続部分１１０が第２ロボットアーム２００から受ける力を推定する（ステップＳ１０６）。上記力は、第２ロボットアーム２００の荷重と、第２ロボットアーム２００が接続部分１１０に発生するモーメントとを含む。例えば、第２推定部４０４は、第２動作指令に含まれる関節ＪＴＢ１～ＪＴＢ６の目標回転位置に基づき、基準座標系を基準とするリンク２０１～２０７及び関節ＪＴＢ１～ＪＴＢ６の推定位置及び推定姿勢を演算する。さらに、第２推定部４０４は、リンク２０１～２０７及び関節ＪＴＢ１～ＪＴＢ６の推定位置及び推定姿勢と、第２位置推定部４０８によって推定された接続部分１１０の推定位置及び推定姿勢と、第３記憶部４２３に記憶される第２ロボットアーム２００の情報とを用いて、第２ロボットアーム２００が接続部分１１０に発生するモーメント及び第２ロボットアーム２００の荷重を、基準座標系を基準として算出する。
【0073】
さらに、第２推定部４０４は、リンク２０１～２０７及び関節ＪＴＢ１～ＪＴＢ６の推定位置及び推定姿勢と、第２ロボットアーム２００の情報とを用いて、第２ロボットアーム２００全体の重心位置である推定重心位置と、第２ロボットアーム２００全体の慣性モーメントである推定慣性モーメントとを算出する。このとき、第２推定部４０４は、第２ロボットアーム２００の情報として、リンク２０１～２０７の重心位置及び質量、関節ＪＴＢ１～ＪＴＢ６の重心位置及び質量、エンドエフェクタＥの重心位置及び質量、並びに、対象物Ｗの重心位置及び質量等を用い得る。
【0074】
次いで、慣性要素推定部４０９は、第１ロボットアーム１００のリンク１０１及び１０２それぞれの慣性的特徴と第２ロボットアーム２００全体の慣性的特徴とを推定する（ステップＳ１０７）。慣性要素推定部４０９は、第２記憶部４２２に記憶される第１ロボットアーム１００の情報及び第３記憶部４２３に記憶される第２ロボットアーム２００の情報を用いて、基準座標系を基準とする各慣性的特徴を算出する。
【0075】
以下において、第ｉリンクの慣性的特徴を「Ｌｉо」と表し、第２ロボットアーム２００全体の慣性的特徴を「Ｌａо」と表す。慣性的特徴「Ｌｉо」及び「Ｌａо」は、基準座標系の原点Оを基準とする。慣性的特徴Ｌｉоは、第ｉリンクの質量及び慣性モーメントを表す６×６行列である。慣性的特徴Ｌａоは、第２ロボットアーム２００全体の質量及び慣性モーメントを表す６×６行列である。慣性要素推定部４０９は、慣性的特徴Ｌｉо及びＬａоをそれぞれ、下記の式１及び式２を用いて算出する。式１及び式２に示される慣性的特徴Ｌｉо及びＬａоは、慣性バイナとも呼ばれる。
【0076】
【数1】
【0077】
式１において、要素「ｍｉ」は第ｉリンクの質量であり、スカラーである。要素「ｌｉО」は第ｉリンクの慣性モーメントを表す３×３行列である。要素「Ｅ」は、単位行列である。要素「γｇｉО」は第ｉリンクの重心の３次元の位置ベクトルである。[γｇｉО×]は、位置ベクトルγｇｉОの外積を歪対称行列で表現したものであり、位置ベクトルγｇｉОの外積行列とも呼ばれる。
【0078】
式２において、要素「ｍａ」は第２ロボットアーム２００全体の質量であり、スカラーである。要素「ｌａО」は第２ロボットアーム２００全体の慣性モーメントを表す３×３行列である。要素「γｇａО」は第２ロボットアーム２００全体の重心の３次元の位置ベクトルである。[γｇａО×]は、位置ベクトルγｇａОの外積を歪対称行列で表現したものである。
【0079】
次いで、慣性力推定部４１０は、第１推定部４０３によって推定される関節ＪＴＡ１及びＪＴＡ２の動作に基づき、関節ＪＴＡ１及びＪＴＡ２それぞれが動作することによってリンク１０１及び１０２に発生する慣性力（関節起因慣性力）を推定する（ステップＳ１０８）。慣性力推定部４１０は、第１推定部４０３によって推定される関節ＪＴＡ１及びＪＴＡ２の推定回転位置と、慣性要素推定部４０９によって推定される慣性的特徴とを用いて、リンク１０１及び１０２に発生する関節起因慣性力を演算する。
【0080】
具体的には、慣性力推定部４１０は、関節ＪＴＡ１及びＪＴＡ２それぞれのジョイントスクリュを演算する。第ｊ関節のジョイントスクリュを「Ｍｊо」と表す。慣性力推定部４１０は、第１関節ＪＴＡ１の推定ジョイントスクリュＭ１оと、第２関節ＪＴＡ２の推定ジョイントスクリュＭ２оとを、基準座標系を基準として算出する。ジョイントスクリュＭｊоは、６次元のベクトルで表される。
【0081】
本実施の形態では、関節ＪＴＡ１及びＪＴＡ２はいずれも、１つの回転中心軸を中心に回転可能である回転関節である。例えば、慣性力推定部４１０は、下記の式３に示すように、第ｊ関節の推定ジョイントスクリュＭｊоを算出する。式３において、γｊо×ｄｊоは、γｊо及びｄｊоの外積を示す。要素「γｊо」は第ｊ関節の３次元の推定位置ベクトルであり、要素「ｄｊо」は第ｊ関節の３次元の推定方向ベクトルである。第ｊ関節の推定方向ベクトルｄｊоは、第ｊ関節の関節軸である回転中心軸の方向ベクトルを示す。
【0082】
【数2】
【0083】
上記の式３は、第ｊ関節が回転関節である場合の第ｊ関節のジョイントスクリュを示すが、第ｊ関節が他の形式の関節であっても、第ｊ関節のジョイントスクリュを算出することができる。例えば、第ｊ関節が直動関節である場合、第ｊ関節のジョイントスクリュは、下記の式４のように表され得る。第ｊ関節がネジ関節である場合、第ｊ関節のジョイントスクリュは、下記の式５のように表され得る。式４において、要素「ｄｊо」は、第ｊ関節の関節軸である直動方向の軸の方向ベクトルを示す。式５において、要素「ｄｊо」は、第ｊ関節の関節軸であるネジ回転の回転中心軸の方向ベクトルを示し、当該方向ベクトルは、ネジ回転による進退方向の軸の方向ベクトルでもある。ネジ関節は、ネジ関節に接続されているリンクが回転中心軸周りに回動すると、回転中心軸方向に当該リンクを進退させるように構成される。要素「ｈ」は、ネジ回転のピッチを示し、例えば、１ラジアンの回転当たりのリンクの回転中心軸方向の進退量を示す。
【0084】
【数3】
【0085】
さらに、慣性力推定部４１０は、関節ＪＴＡ１及びＪＴＡ２の加速度を推定する。当該加速度は、関節ＪＴＡ１及びＪＴＡ２が推定回転位置へ動作するときに関節ＪＴＡ１及びＪＴＡ２が発生する加速度である。例えば、慣性力推定部４１０は、第１推定部４０３によって推定される関節ＪＴＡ１及びＪＴＡ２それぞれの推定回転速度を時間微分することによって、関節ＪＴＡ１及びＪＴＡ２それぞれの推定加速度θ１
’’及びθ２
’’を算出する。
【0086】
慣性力推定部４１０は、慣性的特徴Ｌ１о、Ｌ２о及びＬａоと、ジョイントスクリュＭ１о及びＭ２оと、推定加速度θ１
’’及びθ２
’’とを用いて、推定加速度θ１
’’及びθ２
’’によって第ｉリンクに加わる関節起因慣性力Ｆｉｊоを、基準座標系を基準として算出する。関節起因慣性力Ｆｉｊоは、基準座標系の原点Оを基準として、第ｊ関節の推定加速度θｊ
’’によって第ｉリンクに発生する慣性力を表す。関節起因慣性力Ｆｉｊоは、３つの並進方向の成分と３つの回転方向の成分とを含み、６次元のベクトルで表される。慣性力推定部４１０は、推定加速度θ１
’’及びθ２
’’によってリンク１０１及び１０２に発生する関節起因慣性力Ｆ１１о、Ｆ１２о、Ｆ２１о及びＦ２２оを下記の式６～式９に示すように算出する。関節起因慣性力Ｆｉｊоは、第ｊ関節が動作することで移動するリンクの慣性的特徴と、第２ロボットアーム２００の慣性的特徴とを要素として含む。
【0087】
【数4】
【0088】
次いで、第３推定部４０５は、慣性力推定部４１０によって推定される関節起因慣性力と、第２推定部４０４によって推定される力とに基づき、リンク１０１及び１０２が受ける力を推定する（ステップＳ１０９）。第３推定部４０５は、第２推定部４０４によって推定される力として、第２ロボットアーム２００の推定慣性力Ｆａоを用いる。推定慣性力Ｆａоは、基準座標系の原点Оを基準として第２ロボットアーム２００が原点Оに加える慣性力であってもよい。これに限定されないが、本実施の形態では、推定慣性力Ｆａоとして、第２推定部４０４によって推定される第２ロボットアーム２００の荷重及びモーメントが用いられる。推定慣性力Ｆａоは、荷重の３方向の成分とモーメントの３方向の成分とを含み、６次元のベクトルで表される。
【0089】
第３推定部４０５は、関節起因慣性力Ｆｉｊоと推定慣性力Ｆａоとを用いて、基準座標系の原点Оを基準として第ｉリンクに発生する推定慣性力Ｆｉоを、第ｉリンクが受ける力として算出する。推定慣性力Ｆｉоは、３つの並進方向の成分と３つの回転方向の成分とを含み、６次元のベクトルで表される。第３推定部４０５は、リンク１０１及び１０２に発生する推定慣性力Ｆ１о及びＦ２оを下記の式１０及び式１１に示すように算出する。
【0090】
【数5】
【0091】
次いで、第４推定部４０６は、第３推定部４０５によって推定されるリンク１０１及び１０２が受ける力に基づき、リンク１０１及び１０２の変形量と関節ＪＴＡ１及びＪＴＡ２の変形量とを推定する（ステップＳ１１０）。
【0092】
第４推定部４０６は、第２記憶部４２２に記憶される第１ロボットアーム１００の情報に含まれる関節ＪＴＡ１及びＪＴＡ２の剛性ｋｇ１及びｋｇ２と、推定慣性力Ｆ１о及びＦ２оとを用いて、関節ＪＴＡ１及びＪＴＡ２の推定変形量を演算する。この場合、第４推定部４０６は、推定慣性力Ｆ１о及びＦ２оと、ジョイントスクリュＭ１о及びＭ２оとを用いて、関節ＪＴＡ１及びＪＴＡ２それぞれに作用する推定トルクτ１及びτ２を、下記の式１２及び式１３に示すように算出する。以下において、第ｊ関節の剛性を「ｋｇｊ」と表し、第ｊ関節の推定トルクを「τｊ」と表す。剛性ｋｇｊは、３つの並進方向の成分と３つの回転方向の成分とを含み、６次元のベクトルで表される。剛性ｋｇｊは、第ｊ関節の減速機の剛性である減速機のばね定数等を含む。推定トルクτｊはスカラーである。
【0093】
【数6】
【0094】
式１２及び式１３における演算子「〇」は、ベクトルの分配演算を示す。例えば、６次元のベクトルＡが、３次元の並進方向の成分を含むベクトルａ１と、３次元の回転方向の成分を含むベクトルａ２とを含み、６次元のベクトルＢが、３次元の並進方向の成分を含むベクトルｂ１と、３次元の回転方向の成分を含むベクトルｂ２とを含む場合、演算子「〇」を用いたベクトルＡ及びＢの演算は、下記の式１４のように示される。
【0095】
【数7】
【0096】
さらに、第４推定部４０６は、推定トルクτｊを剛性ｋｇｊで除算することによって、第ｊ関節の推定変形量Δθｊを算出する。第４推定部４０６は、関節ＪＴＡ１及びＪＴＡ２それぞれの推定変形量Δθ１及びΔθ２を下記の式１５及び式１６に示すように算出する。推定変形量Δθｊは、第ｊ関節での変形に対する補償量とすることができる。
【0097】
【数8】
【0098】
また、第４推定部４０６は、第２記憶部４２２に記憶される第１ロボットアーム１００の情報に含まれるリンク１０１及び１０２のコンプライアンスＣＩ１о及びＣＩ２оと、推定慣性力Ｆ１о及びＦ２оとを用いて、リンク１０１及び１０２の推定変形量を演算する。以下において、第ｉリンクのコンプライアンスを「ＣＩｉо」と表す。コンプライアンスＣＩｉоは、基準座標系の原点Оを基準とする第ｉリンクの剛性を示し、６×６行列で表される。
【0099】
第４推定部４０６は、推定慣性力Ｆ１о及びＦ２оと、コンプライアンスＣＩ１о及びＣＩ２оとを用いて、リンク１０１及び１０２それぞれの推定変形量Δ１о及びΔ２оを、下記の式１７及び式１８に示すように算出する。以下において、第ｉリンクの推定変形量を「Δｉо」と表す。推定変形量Δｉоは、３つの並進方向の成分と３つの回転方向の成分とを含み、基準座標系を基準とする６次元のベクトルで表される。
【0100】
【数9】
【0101】
さらに、第４推定部４０６は、第ｉリンクの推定変形量Δｉоと第ｊ関節のジョイントスクリュＭｊоとを用いて、第ｉリンクの推定変形量Δｉоを第ｊ関節の推定変形量に変換してもよい。推定変形量Δｉоに含まれる３次元の並進方向の成分を含むベクトルを、ベクトルδｔｉоとし、ジョイントスクリュＭｊоに含まれる３次元の並進方向の成分を含むベクトルを、ベクトルυｊоとする。第ｉリンクの推定変形量Δｉоは、下記の式１９に示すように、第ｊ関節の推定変形量ΔθＬｊに変換することができる。推定変形量ΔθＬｊは、リンクの変形に対する補償量とすることができる。
【0102】
【数10】
【0103】
次いで、補正指令部４１１は、第４推定部４０６によって推定される変形量に基づき、第１目標動作に従って接続部分１１０を移動させるように上記変形量を補償するための第１動作指令の補正指令を生成する（ステップＳ１１１）。補正指令部４１１は、補正指令及び第２動作指令を関連付けて、ロボットコントローラ３００Ｂに送信する。補正指令部４１１は、上記変形量を補償することで接続部分１１０の位置及び姿勢を目標位置及び目標姿勢にするように第１動作指令を補正し、補正後の第１動作指令を補正指令として送信する。
【0104】
例えば、補正指令部４１１は、第１動作指令に含まれる第ｊ関節の目標回転位置に、第ｊ関節の推定変形量Δθｊを加えることによって、第１動作指令を補正してもよい。補正指令部４１１は、第１動作指令に含まれる第ｊ関節の目標回転位置に、第ｊ関節の推定変形量ΔθＬｊを加えることによって、第１動作指令を補正してもよい。これにより、リンク及び関節の両方の変形量が補償される。なお、本実施の形態では、第ｊ関節は、１つの自由度を有する回転関節であるため、補正指令部４１１は、推定変形量Δθｊと推定変形量ΔθＬｊとのうちの一方を、第ｊ関節の目標回転位置に加えるが、両方を第ｊ関節の目標回転位置に加えることもある。第ｊ関節が２以上の自由度を有する場合、補正指令部４１１は、推定変形量Δθｊと推定変形量ΔθＬｊとの両方を、第ｊ関節の目標回転位置に加えることもある。
【0105】
次いで、ロボットコントローラ３００Ｂにおいて、駆動指令部５０１及び５０２は駆動指令を生成しロボットアーム１００及び２００に出力する（ステップＳ１１２）。第１駆動指令部５０１は、補正指令に従った駆動指令を生成し、第１ロボットアーム１００の駆動装置ＭＡ１及びＭＡ２に出力する。第２駆動指令部５０２は、第２動作指令に従った駆動指令を生成し、第２ロボットアーム２００の駆動装置ＭＢ１～ＭＢ６に出力する。駆動指令部５０１及び５０２は、それぞれの駆動指令の出力タイミングを同期させるように、互いに協働してそれぞれの駆動指令を出力する。これにより、ロボットアーム１００及び２００は、同時にそれぞれの目標動作を実行するが、２つのロボットアーム１００及び２００の動作に起因して、接続部分１１０の位置及び姿勢が目標位置及び目標姿勢から変動することが抑えられる。よって、第２ロボットアーム２００は、作用を加える対象物Ｗに対して目標の動作を正確に行うことができる。
【0106】
（変形例１）
本変形例は、第１ロボットアーム１００Ａが１つの関節のみを備える点で、実施の形態と異なる。以下、本変形例について、実施の形態と異なる点を中心に説明し、実施の形態と同様の点の説明を適宜省略する。
本変形例は、第１ロボットアーム１００Ａが１つの関節のみを備える点で、実施の形態と異なる。以下、本変形例について、実施の形態と異なる点を中心に説明し、実施の形態と同様の点の説明を適宜省略する。
【0107】
図６は、変形例１に係るロボット１Ａの構成をモデル化した側面図である。図６に示すように、ロボット１Ａは、実施の形態と同様の第２ロボットアーム２００と、第１ロボットアーム１００Ａとを備える。第１ロボットアーム１００Ａは、リンク１０１Ａ及び１０２Ａと、リンク１０１Ａ及び１０２Ａを回転可能に接続する関節ＪＴＡＡ１とを備える。これに限定されないが、本変形例の第１リンク１０１Ａは、実施の形態の第２リンク１０２と同様の構成を有し、本変形例の第２リンク１０２Ａは、実施の形態の第３リンク１０３と同様の構成を有する。本変形例の第１関節ＪＴＡＡ１は、実施の形態の第２関節ＪＴＡ２と同様の構成を有する。ロボット１Ａは、実施の形態と同様である情報処理装置３００Ａ及びロボットコントローラ３００Ｂを含む制御装置３００を備える。情報処理装置３００Ａは、実施の形態と同様の処理を行うが、下記の点で実施の形態と異なる演算式を用いて演算する。
【0108】
慣性力推定部４１０は、実施の形態の式６～式９の代わりに下記の式２０を用いて、第１関節ＪＴＡＡ１の推定加速度θ１
’’によって第１リンク１０１Ａに発生する関節起因慣性力Ｆ１１оを演算する。
【0109】
【数11】
【0110】
第３推定部４０５は、実施の形態の式１０及び式１１の代わりに下記の式２１を用いて、第１リンク１０１Ａに発生する推定慣性力Ｆ１оを演算する。
【0111】
【数12】
【0112】
第４推定部４０６は、式１２を用いて第１関節ＪＴＡＡ１の推定トルクτ１を演算する。さらに、第４推定部４０６は、式１５を用いて第１関節ＪＴＡＡ１の推定変形量Δθ１を演算する。第４推定部４０６は、式１７を用いて第１リンク１０１Ａの推定変形量Δ１оを演算する。第４推定部４０６は、式１９を用いて第１リンク１０１Ａの推定変形量Δ１оを第１関節ＪＴＡＡ１の推定変形量ΔθＬ１に変換する。
【0113】
（変形例２）
本変形例は、第１ロボットアーム１００Ｂが３つの関節を備える点で、実施の形態と異なる。以下、本変形例について、実施の形態及び変形例１と異なる点を中心に説明し、実施の形態及び変形例１と同様の点の説明を適宜省略する。
本変形例は、第１ロボットアーム１００Ｂが３つの関節を備える点で、実施の形態と異なる。以下、本変形例について、実施の形態及び変形例１と異なる点を中心に説明し、実施の形態及び変形例１と同様の点の説明を適宜省略する。
【0114】
図７は、変形例２に係るロボット１Ｂの構成をモデル化した側面図である。図７に示すように、ロボット１Ｂは、実施の形態と同様の第２ロボットアーム２００と、第１ロボットアーム１００Ｂとを備える。第１ロボットアーム１００Ｂは、リンク１０１Ｂ、１０２Ｂ、１０３Ｂ及び１０４Ｂと、関節ＪＴＡＢ１、ＪＴＡＢ２及びＪＴＡＢ３とを備える。これに限定されないが、本変形例のリンク１０１Ｂ及び１０２Ｂはそれぞれ、実施の形態のリンク１０１及び１０２と同様の構成を有し、本変形例の第４リンク１０４Ｂは、実施の形態の第３リンク１０３と同様の構成を有する。本変形例の関節ＪＴＡＢ１及びＪＴＡＢ２はそれぞれ、実施の形態の関節ＪＴＡ１及びＪＴＡ２と同様の構成を有する。
【0115】
本変形例の第３リンク１０３Ｂの一端は、第３関節ＪＴＡＢ３によって第２リンク１０２Ｂと接続される。本変形例の第３リンク１０３Ｂの他端には、実施の形態の第２リンク１０２と同様に、第２ロボットアーム２００が取り付けられる。第３関節ＪＴＡＢ３は、支持面Ｓと垂直な方向に延びる第３軸Ｓ３を中心に回動可能である。なお、第１軸Ｓ１、第２軸Ｓ２及び第３軸Ｓ３の方向は、いかなる方向であってもよく、互いに異なっていても同じであってもよい。ロボット１Ｂは、実施の形態と同様である情報処理装置３００Ａ及びロボットコントローラ３００Ｂを含む制御装置３００を備える。情報処理装置３００Ａは、実施の形態と同様の処理を行うが、下記の点で実施の形態と異なる演算式を用いて演算する。
【0116】
慣性要素推定部４０９は、式１を用いて、リンク１０１Ｂ、１０２Ｂ及び１０３Ｂそれぞれの慣性的特徴Ｌ１о、Ｌ２о及びＬ３оを演算する。
【0117】
慣性力推定部４１０は、式３を用いて、関節ＪＴＡＢ１、ＪＴＡＢ２及びＪＴＡＢ３それぞれのジョイントスクリュＭ１о、Ｍ２о及びＭ３оを演算する。
【0118】
慣性力推定部４１０は、実施の形態の式６～式９の代わりに下記の式２２～式３０を用いて、関節ＪＴＡＢ１、ＪＴＡＢ２及びＪＴＡＢ３それぞれの推定加速度θ１
’’、θ１２
’’及びθ３
’’によってリンク１０１Ｂ、１０２Ｂ及び１０３Ｂに発生する関節起因慣性力Ｆ１１о、Ｆ１２о、Ｆ１３о、Ｆ２１о、Ｆ２２о、Ｆ２３о、Ｆ３１о、Ｆ３２о及びＦ３３оを演算する。この演算において、慣性力推定部４１０は、関節ＪＴＡＢ１、ＪＴＡＢ２及びＪＴＡＢ３それぞれの推定加速度θ１
’’、θ１２
’’及びθ３
’’と、リンク１０１Ｂ、１０２Ｂ及び１０３Ｂそれぞれの慣性的特徴Ｌ１о、Ｌ２о及びＬ３оと、第２ロボットアーム２００の慣性的特徴Ｌａоと、関節ＪＴＡＢ１、ＪＴＡＢ２及びＪＴＡＢ３それぞれのジョイントスクリュＭ１о、Ｍ２о及びＭ３оとを用いる。
【0119】
【数13】
【0120】
第３推定部４０５は、実施の形態の式１０及び式１１の代わりに下記の式３１～式３３を用いて、リンク１０１Ｂ、１０２Ｂ及び１０３Ｂそれぞれに発生する推定慣性力Ｆ１о、Ｆ２о及びＦ３оを演算する。
【0121】
【数14】
【0122】
第４推定部４０６は、実施の形態の式１２及び式１３の代わりに下記の式３４～式３６を用いて、関節ＪＴＡＢ１、ＪＴＡＢ２及びＪＴＡＢ３それぞれに作用する推定トルクτ１、τ２及びτ３を演算する。
【0123】
【数15】
【0124】
第４推定部４０６は、実施の形態の式１５及び式１６の代わりに下記の式３７～式３９を用いて、関節ＪＴＡＢ１、ＪＴＡＢ２及びＪＴＡＢ３それぞれの推定変形量Δθ１、Δθ２及びΔθ３を演算する。この演算において、第４推定部４０６は、関節ＪＴＡＢ１、ＪＴＡＢ２及びＪＴＡＢ３それぞれの剛性ｋｇ１、ｋｇ２及びｋｇ３を用いる。
【0125】
【数16】
【0126】
第４推定部４０６は、実施の形態の式１７及び式１８の代わりに下記の式４０～式４２を用いて、リンク１０１Ｂ、１０２Ｂ及び１０３Ｂそれぞれの推定変形量Δ１о、Δ２о及びΔ３оを演算する。この演算において、第４推定部４０６は、リンク１０１Ｂ、１０２Ｂ及び１０３ＢそれぞれのコンプライアンスＣＩ１о、ＣＩ２о及びＣＩ３оを用いる。
【0127】
【数17】
【0128】
第４推定部４０６は、式１９を用いてリンク１０１Ｂ、１０２Ｂ及び１０３Ｂの推定変形量Δ１о、Δ２о及びΔ３оを関節ＪＴＡＢ１、ＪＴＡＢ２及びＪＴＡＢ３の推定変形量ΔθＬ１、ΔθＬ２及びΔθＬ３に変換する。
【0129】
（その他の実施の形態）
以上、本開示の実施の形態及び変形例について説明したが、本開示は、上記実施の形態及び変形例に限定されない。すなわち、本開示の範囲内で種々の変形及び改良が可能である。例えば、各種変形を実施の形態及び変形例に施したもの、及び、異なる実施の形態及び変形例における構成要素を組み合わせて構築される形態も、本開示の範囲内に含まれる。
以上、本開示の実施の形態及び変形例について説明したが、本開示は、上記実施の形態及び変形例に限定されない。すなわち、本開示の範囲内で種々の変形及び改良が可能である。例えば、各種変形を実施の形態及び変形例に施したもの、及び、異なる実施の形態及び変形例における構成要素を組み合わせて構築される形態も、本開示の範囲内に含まれる。
【0130】
例えば、実施の形態及び変形例において、第１ロボットアームの関節の数量が１、２又は３であるケースが例示されるが、第１ロボットアームの関節の数量は、４以上であってもよい。この場合も、制御装置３００は、実施の形態及び変形例と同様の制御を行うことができる。
【0131】
また、実施の形態及び変形例において、第１ロボットアーム及び第２ロボットアームの関節の自由度は１であるが、これに限定されない。第１ロボットアーム及び第２ロボットアームの関節の自由度が２以上であっても、制御装置３００は、実施の形態及び変形例と同様の制御を行うことができる。
【0132】
また、実施の形態及び変形例において、ロボット１が対象物Ｗを保持する作業を行うケースが例示されるが、ロボット１がいかなる作業を行う場合であっても、制御装置３００は、実施の形態及び変形例と同様の制御を行うことができる。
【0133】
また、本開示の技術の各態様例は、以下のように挙げられる。本開示の一態様に係る制御装置は、第２ロボットアームを支持する少なくとも２つのリンクと、前記少なくとも２つのリンクを相互に接続する少なくとも１つの関節とを有する第１ロボットアームの制御装置であって、前記第１ロボットアームに目標動作である第１目標動作をさせるための第１動作指令を取得し、前記第１ロボットアームが前記第１動作指令に従って動作する場合の前記少なくとも１つの関節の動作を推定する第１推定部と、前記第２ロボットアームに目標動作である第２目標動作をさせるための第２動作指令を取得し、前記第２ロボットアームが前記第２動作指令に従って動作する場合に、前記第２ロボットアームと前記リンクとの接続部分が前記第２ロボットアームから受ける力を推定する第２推定部と、前記第１推定部によって推定される前記少なくとも１つの関節の動作と、前記第２推定部によって推定される力とに基づき、前記リンクが受ける力を推定する第３推定部と、前記第３推定部によって推定される力に基づき、前記リンクの変形量と、前記少なくとも１つの関節の変形量とのうちの少なくとも一方を推定する第４推定部と、前記第４推定部によって推定される変形量に基づき、前記第１目標動作に従って前記接続部分を移動させるように前記変形量を補償するための前記第１動作指令の補正指令を生成し、前記補正指令を前記第１ロボットアームに指令する補正指令部とを含む。
【0134】
上記態様によると、制御装置は、第１ロボットアームが第１目標動作を実行し且つ第２ロボットアームが第２目標動作を実行する場合に第１ロボットアームのリンク及び／又は関節に生じる変形量を推定する。上記変形量は、第１動作指令に従った接続部分の実際の動作と第１目標動作との間に差異を生じさせ得る。制御装置は、補正指令を第１ロボットアームに指令することによって、第１ロボットアームに、上記差異を低減した動作、つまり、変形量を補償した動作をさせることができる。よって、制御装置は、リンクの変形及び／又は関節の変形に起因して、接続部分が、意図しない目標位置からの位置ずれを生じること及び意図しない揺動を生じることを抑えることができる。従って、第２ロボットアームの基部である接続部分の位置の意図しない変動が抑えられる。
【0135】
本開示の一態様に係る制御装置において、前記少なくとも２つのリンクは、基端リンクと、前記基端リンク及び前記第２ロボットアームと接続される第１リンクとを含み、前記少なくとも１つの関節は、前記第１リンクを前記基端リンクに接続する第１関節を含み、前記第１推定部は、前記第１関節の動作を推定し、前記第３推定部は、前記第１推定部によって推定される前記第１関節の動作と、前記第２推定部によって推定される力とに基づき、前記第１リンクが受ける力を推定し、前記第４推定部は、前記第３推定部によって推定される前記第１リンクが受ける力に基づき、前記第１リンクの変形量と前記第１関節の変形量とのうちの少なくとも一方を推定してもよい。
【0136】
上記態様によると、制御装置は、１つの関節を有する第１ロボットアームを制御し、第２ロボットアームの基部の位置の意図しない変動を抑えることができる。
【0137】
本開示の一態様に係る制御装置において、前記少なくとも２つのリンクは、基端リンクと、前記基端リンクと接続される第１リンクと、前記第２ロボットアームと接続される第２リンクとを含み、前記少なくとも１つの関節は、前記第１リンクを前記基端リンクに接続する第１関節と、前記第２リンクを前記第１リンクに接続する第２関節とを含み、前記第１推定部は、前記第１関節及び前記第２関節の動作を推定し、前記第３推定部は、前記第１推定部によって推定される前記第１関節及び前記第２関節の動作と、前記第２推定部によって推定される力とに基づき、前記第１リンクが受ける力を推定し、前記第３推定部は、前記第１推定部によって推定される前記第１関節及び前記第２関節の動作と、前記第２推定部によって推定される力とに基づき、前記第２リンクが受ける力を推定し、前記第４推定部は、前記第３推定部によって推定される前記第１リンクが受ける力に基づき、前記第１リンクの変形量と前記第１関節の変形量とのうちの少なくとも一方を推定し、前記第４推定部は、前記第３推定部によって推定される前記第２リンクが受ける力に基づき、前記第２リンクの変形量と前記第２関節の変形量とのうちの少なくとも一方を推定してもよい。
【0138】
上記態様によると、制御装置は、２つの関節を有する第１ロボットアームを制御し、第２ロボットアームの基部の位置の意図しない変動を抑えることができる。制御装置は、第１リンクが受ける力及び第２リンクが受ける力それぞれの推定に、第１関節の推定動作及び第２関節の推定動作の全てを用いる。よって、各リンクが受ける力の推定精度が向上し、それにより、各リンク及び各関節の変形量の推定精度が向上する。
【0139】
本開示の一態様に係る制御装置は、前記第１推定部によって推定される前記少なくとも１つの関節の動作に基づき、前記少なくとも１つの関節が動作することによって前記リンクに発生する慣性力を推定する慣性力推定部をさらに含み、前記第３推定部は、前記慣性力推定部によって推定される慣性力と、前記第２推定部によって推定される力とに基づき、前記リンクが受ける力を推定してもよい。
【0140】
上記態様によると、制御装置は、関節の動作に起因して発生するリンクの慣性力を推定し、推定されたリンクの慣性力を用いて当該リンクが受ける力を推定する。よって、リンクが受ける力の推定精度が向上する。
【0141】
本開示の一態様に係る制御装置は、前記リンクの位置を推定する第１位置推定部と、前記接続部分の位置を推定する第２位置推定部と、前記リンクの慣性的特徴と前記第２ロボットアームの慣性的特徴とを推定する慣性要素推定部とをさらに含み、前記リンクの慣性的特徴は、前記リンクの慣性モーメント及び質量を要素として含み、前記第２ロボットアームの慣性的特徴は、前記第２ロボットアームの慣性モーメント及び質量を要素として含み、前記慣性力推定部は、前記リンクの慣性的特徴と、前記第２ロボットアームの慣性的特徴と、前記少なくとも１つの関節の加速度と、前記少なくとも１つの関節のジョイントスクリュとを用いて、前記リンクに発生する前記慣性力を推定し、前記ジョイントスクリュは、前記少なくとも１つの関節の向きを示す要素を含む、前記少なくとも１つの関節の状態を示す特徴であってもよい。
【0142】
上記態様によると、慣性的特徴は、慣性モーメントの要素と質量の要素とを含む。制御装置は、リンク及び第２ロボットアームの慣性的特徴を用いて、関節の動作に起因して発生するリンクに発生する慣性力を推定する。よって、当該慣性力の推定精度が向上する。
【0143】
本開示の一態様に係るロボットは、本開示の一態様に係る制御装置と、前記第１ロボットアームと、前記第２ロボットアームとを備える。上記態様によると、本開示の一態様に係る制御装置と同様の効果が得られる。ロボットは、第２ロボットアームの基部の位置の意図しない変動を抑えつつ、第１ロボットアーム及び第２ロボットアームに目標動作をさせることができる。
【0144】
本開示の一態様に係るロボットにおいて、前記制御装置は、前記第１動作指令及び前記第２動作指令を生成し、前記第１ロボットアーム及び前記第２ロボットアームの動作を制御するように構成されてもよい。上記態様によると、制御装置は、ロボットコントローラとして機能することができる。つまり、ロボットコントローラは、第２ロボットアームの基部の位置の意図しない変動を抑えつつ、第１ロボットアーム及び第２ロボットアームに目標動作をさせることができる。
【0145】
また、上記で用いた序数、数量等の数字は、全て本開示の技術を具体的に説明するために例示するものであり、本開示は例示された数字に制限されない。また、構成要素間の接続関係は、本開示の技術を具体的に説明するために例示するものであり、本開示の機能を実現する接続関係はこれに限定されない。
【0146】
また、機能ブロック図におけるブロックの分割は一例であり、複数のブロックを一つのブロックとして実現する、一つのブロックを複数に分割する、及び／又は、一部の機能を他のブロックに移してもよい。また、類似する機能を有する複数のブロックの機能を単一のハードウェア又はソフトウェアが並列又は時分割に処理してもよい。
【符号の説明】
【0147】
１，１Ａ，１Ｂ ロボット
１００，１００Ａ，１００Ｂ 第１ロボットアーム
１０１，１０１Ａ，１０１Ｂ 第１リンク
１０２，１０２Ａ，１０２Ｂ 第２リンク
１０３ リンク（基端リンク）
１０３Ｂ 第３リンク
１０４Ｂ 第４リンク（基端リンク）
１１０ 接続部分（界面）
２００ 第２ロボットアーム
３００ 制御装置
３００Ａ 情報処理装置
３００Ｂ ロボットコントローラ
４０３ 第１推定部
４０４ 第２推定部
４０５ 第３推定部
４０６ 第４推定部
４０７ 第１位置推定部
４０８ 第２位置推定部
４０９ 慣性要素推定部
４１０ 慣性力推定部
４１１ 補正指令部
ＪＴＡ１，ＪＴＡＡ１，ＪＴＡＢ１ 第１関節
ＪＴＡ２，ＪＴＡＢ２ 第２関節
ＪＴＡＢ３ 第３関節
１００，１００Ａ，１００Ｂ 第１ロボットアーム
１０１，１０１Ａ，１０１Ｂ 第１リンク
１０２，１０２Ａ，１０２Ｂ 第２リンク
１０３ リンク（基端リンク）
１０３Ｂ 第３リンク
１０４Ｂ 第４リンク（基端リンク）
１１０ 接続部分（界面）
２００ 第２ロボットアーム
３００ 制御装置
３００Ａ 情報処理装置
３００Ｂ ロボットコントローラ
４０３ 第１推定部
４０４ 第２推定部
４０５ 第３推定部
４０６ 第４推定部
４０７ 第１位置推定部
４０８ 第２位置推定部
４０９ 慣性要素推定部
４１０ 慣性力推定部
４１１ 補正指令部
ＪＴＡ１，ＪＴＡＡ１，ＪＴＡＢ１ 第１関節
ＪＴＡ２，ＪＴＡＢ２ 第２関節
ＪＴＡＢ３ 第３関節
【図1】
【図2】
【図3】
【図4】
【図5】
【図6】
【図7】