JP2012130999A - 制御装置およびロボットアームの制御方法 - Google Patents

Abstract

【課題】本発明は、多関節型ロボットアームの滑らかな動作を実現することができる制御装置を提案する。
【解決手段】
本発明の制御装置２は、第一駆動モータ、第二駆動モータ、および第三駆動モータにかかる負荷イナーシャ、重力トルク、モータトルク、ならびにモータイナーシャに加えて、ばね装置のトルク、ならびに第一駆動モータ、第二駆動モータ、および第三駆動モータそれぞれの動力を伝達するための駆動系部材列のイナーシャである駆動系イナーシャのうちの少なくともいずれか一つを用いて、第一駆動モータ、前記第二駆動モータ、および第三駆動モータの角加速度を算出する加速度算出手段を備える。
【選択図】 図３

Description

本発明は、多関節型ロボットアームを制御する制御装置に関する。

従来、多関節型ロボットアームの滑らかな動作を実現するために、ロボットアームを動作させる駆動モータの加速度を制御している。駆動モータの加速および減速それぞれにおいて必要な時間は、駆動モータにかかる負荷イナーシャ、モータイナーシャ、モータトルク、モータ速度により決定される。そのため、加速および減速それぞれにおいて必要な時間としては、最小または最大の負荷イナーシャから算出された固定値を設定していた。

ところで、多関節型ロボットアームは、動作することにより自らの姿勢が変化する。このため、多関節型ロボットアームを滑らかに動作させるためには、この姿勢変化に伴う駆動モータにかかる力の変動、より具体的には駆動モータにかかる負荷イナーシャと重力トルクとの変動も考慮する必要がある。そこで、多関節型ロボットアームの姿勢変更に伴うこれらの変動を考慮して、滑らかな動作を行うように制御する制御装置が、例えば、特許文献１に開示されている。

より具体的には、特許文献１では、多関節型ロボットアームの姿勢変化による負荷イナーシャの変動と重力トルクの変動とを考慮して、駆動モータの加速時間および減速時間を可変とすることができる制御装置が開示されている。

特開平１１−１０２２１５号公報

ところで、上述した特許文献１の制御装置は、多関節型ロボットアームの姿勢変化に起因する負荷イナーシャの変動と重力トルクの変動とを考慮したものであるが、多関節型ロボットアームの実際の詳細な構造を考慮して、駆動モータの適切な加速度を求めるものではない。

すなわち、一般的には多関節型ロボットアームは、姿勢変化に伴い生じる関節部に加わる重力負荷を低減するためのバランサ（ばね装置）、およびギアなど駆動モータからの動力を伝達するための駆動系部材列をさらに備えてなる構成である。このため、多関節型ロボットアームの滑らかな動作を実現するためには、上述したバランサ、駆動系部材列に関わって作用する力も考慮する必要がある。

ところが、特許文献１では、バランサや駆動系部材列に関わる力を考慮した構成となっていないため、多関節型ロボットアームの滑らかな動作を実現することができない。

本発明は、上述した問題点に鑑みてなされたものであり、その目的は、多関節型ロボットアームの滑らかな動作を実現することができる制御装置およびロボットアームの制御方法を実現することにある。

本発明に係る制御装置は、上記した課題を解決するために、基部と、第一リンク部材と、 第二リンク部材と、第三リンク部材と、前記基部に対して、回動自在に前記第一リンク部材の一方の端部を接合する第一接合部と、該基部に対して前記第一リンク部材の一方の端部を回動させるための第一駆動モータとを有する第一関節部と、前記第一リンク部材の他方の端部と前記第二リンク部材の一方の端部とを互いに回動自在に接合する第一接合部と、該第一リンク部材の他方の端部と前記第二リンク部材の一方の端部とを互いに回動させるための第二駆動モータとを有する第二関節部と、前記第二リンク部材の他方の端部と前記第三リンク部材の一方の端部とを互いに回動自在に接合する第三接合部と、該第二リンク部材の他方の端部と前記第三リンク部材の一方の端部とを互いに回動させるための第三駆動モータとを有する第三関節部と、前記第二リンク部材および第三リンク部材の姿勢変更に伴い生じる前記第二関節部に加わる重力負荷を低減するためのばね装置と、を備えたロボットアームの動作を、前記第一駆動モータ、第二駆動モータ、および第三駆動モータを介して制御する制御装置であって、前記第一駆動モータ、第二駆動モータ、および第三駆動モータにかかる負荷イナーシャ、重力トルク、モータトルク、ならびにモータイナーシャに加えて、前記ばね装置のトルク、ならびに該第一駆動モータ、第二駆動モータ、および第三駆動モータそれぞれの動力を伝達するための駆動系部材列のイナーシャである駆動系イナーシャのうちの少なくともいずれか一つを用いて、前記第一駆動モータ、前記第二駆動モータ、および第三駆動モータの角加速度を算出する加速度算出手段を備えている。

上記した構成によると、制御装置は、加速度算出手段を備えているため、ばね装置のトルクおよび前記駆動系イナーシャのうちの少なくともいずれか一つを用いて第一駆動モータ、第二駆動モータ、および第三駆動モータの角加速度を算出することができる。

このため、制御装置は、ばね装置、駆動系部材列に関わって作用する力も考慮した上で、多関節型ロボットアームにおける第一駆動モータ、第二駆動モータ、および第三駆動モータそれぞれの角加速度を算出することができる。

したがって、本発明に係る制御装置は、実際のロボットアームの各関節部に働く力を考慮して第一駆動モータ、第二駆動モータ、および第三駆動モータの適切な角加速度を算出することできるため、このロボットアームの滑らかな動作を実現することができるという効果を奏する。

また、本発明に係る制御装置は、前記第二駆動モータおよび前記第三駆動モータそれぞれは、同一方向に延在した回転軸を中心に回動しており、前記加速度算出手段が、前記第二関節部が備える第二駆動モータおよび前記第三関節部が備える第三駆動モータの両者が共に動作している場合における、該第二関節部と該第三関節部との相互間で作用する慣性力に起因して生じるトルクを、前記第二駆動モータおよび第三駆動モータそれぞれのモータトルクから減算して、前記第二駆動モータおよび前記第三駆動モータの角加速度を算出するように構成されていてもよい。

上記した構成によると、加速度算出手段は、第二関節部が備える第二駆動モータおよび第三関節部が備える第三駆動モータの両者が共に動作している場合における、該第二関節部と該第三関節部との相互間で作用する慣性力に起因して生じるトルクを、第二駆動モータおよび第三駆動モータそれぞれのモータトルクから減算して角加速度を算出する。つまり、該第二関節部と該第三関節部との相互間で作用する慣性力による影響をモータトルクから取り除いて角加速度を算出することができる。

したがって、本発明に係る制御装置は、実際のロボットアームの各関節部に働く力を考慮して第一駆動モータ、第二駆動モータ、および第三駆動モータの適切な角加速度を算出することできる。

また、本発明に係る制御装置は、前記第一駆動モータ、前記第二駆動モータ、および前記第三駆動モータそれぞれで固有に定められた、駆動モータの回転数に応じた駆動モータのトルクを示すトルク情報を記憶する記憶装置をさらに備え、前記加速度算出手段は、前記第一駆動モータ、第二駆動モータおよび前記第三駆動モータにおける動作開始から動作終了までの間の加速時および減速時における角加速度から、前記第一駆動モータ、前記第二駆動モータ、および前記第三駆動モータそれぞれの回転数を算出し、前記記憶装置に記憶した前記トルク情報に基づき、算出した回転数に対応する該第一駆動モータ、該第二駆動モータ、および第三駆動モータのモータトルクをそれぞれ求め、これら求めたモータトルクを用いて、前記第一駆動モータ、前記第二駆動モータ、および前記第三駆動モータの角加速度を算出するように構成されていてもよい。

また、本発明に係る制御装置は、前記加速度算出手段によって算出した角加速度が、時間軸に対して台形パターンとなるように前記第一駆動モータ、前記第二駆動モータ、および前記第三駆動モータに対する指令を補正するための補正指令値を生成する補正指令値生成手段を備える。

上記した構成によると、補正指令値生成手段をさらに備えるため、前記加速度算出手段によって算出した角加速度が、時間軸に対して台形パターンとなるように前記第一駆動モータ、前記第二駆動モータ、および前記第三駆動モータに対する指令を補正することができる。これにより、第一駆動モータ、第二駆動モータ、および第三駆動モータの加加速度を略一定とすることができる。

すなわち、第一駆動モータ、第二駆動モータ、および第三駆動モータにおける動作開始時点、加速期間から一定速度期間に切り替わる点、一定速度期間から減速期間に切り替わる点、減速期間から動作終了時点に切り替わる点等においてパルス的に生じる加加速度を低減することができる。

したがって、ロボットアームの動作時に生じる振動を抑制することができる。

また、本発明に係るロボットアームの制御方法は、上記した課題を解決するために、基部と、第一リンク部材と、第二リンク部材と、第三リンク部材と、前記基部に対して、回動自在に前記第一リンク部材の一方の端部を接合する第一接合部と、該基部に対して前記第一リンク部材の一方の端部を回動させるための第一駆動モータとを有する第一関節部と、 前記第一リンク部材の他方の端部と前記第二リンク部材の一方の端部とを互いに回動自在に接合する第一接合部と、該第一リンク部材の他方の端部と前記第二リンク部材の一方の端部とを互いに回動させるための第二駆動モータとを有する第二関節部と、前記第二リンク部材の他方の端部と前記第三リンク部材の一方の端部とを互いに回動自在に接合する第三接合部と、該第二リンク部材の他方の端部と前記第三リンク部材の一方の端部とを互いに回動させるための第三駆動モータとを有する第三関節部と、前記第二リンク部材および第三リンク部材の姿勢変更に伴い生じる前記第二関節部に加わる重力負荷を低減するためのばね装置と、を備えたロボットアームの動作を、前記第一駆動モータ、第二駆動モータ、および第三駆動モータを介して制御するロボットアームの制御方法であって、前記第一駆動モータ、第二駆動モータ、および第三駆動モータにかかる負荷イナーシャ、重力トルク、モータトルク、ならびにモータイナーシャに加えて、前記ばね装置のトルク、該第一駆動モータ、第二駆動モータ、および第三駆動モータそれぞれの動力を伝達するための駆動系部材列のイナーシャである駆動系イナーシャのうちの少なくともいずれか一つを用いて、前記第一駆動モータ、前記第二駆動モータ、および第三駆動モータの角加速度を算出する。

上記した方法によると、ばね装置のトルクおよび前記駆動系イナーシャのうちの少なくともいずれか一つを用いて第一駆動モータ、第二駆動モータ、および第三駆動モータの角加速度を算出することができる。

このため、ばね装置、駆動系部材列に関わって作用する力も考慮した上で、多関節型ロボットアームにおける第一駆動モータ、第二駆動モータ、および第三駆動モータそれぞれの角加速度を算出することができる。

したがって、本発明に係るロボットアームの制御方法は、実際のロボットアームの各関節部に働く力を考慮して第一駆動モータ、第二駆動モータ、および第三駆動モータの適切な角加速度を算出することできるため、このロボットアームの滑らかな動作を実現することができるという効果を奏する。

本発明は以上に説明したように構成され、実際のロボットアームの各関節部に働く力を考慮して第一駆動モータ、第二駆動モータ、および第三駆動モータの適切な角加速度を算出することできるという効果を奏する。

本発明の実施形態に係るロボットアーム制御システムを示す概略図である。 図１に示すロボットアーム制御システムが備えるロボットアームのリンク部材と関節部との位置関係を模式的に示す図である。 図１に示すロボットアーム制御システムが備える制御装置に係る機能的構造の一例を示すブロック図である。 本実施の形態に係るロボットアームにおける第二関節部および第三関節部それぞれの角加速度の算出フローを説明するフローチャートである。 所定の定格出力容量を有する駆動モータのモータトルクと（最高）回転数との対応関係の概略を示す表である。 図６（ａ）は、駆動モータの動作速度の時間軸に対する変化について、本実施の形態に係る加加速度制御の場合と従来の非加加速度制御の場合とを比較したグラフである。図６（ｂ）は、駆動モータを回す際の角加速度の時間軸に対する変化について、加加速度制御の場合と非加加速度制御の場合とを比較したグラフである。図６（ｃ）は、発生するジャークについて加加速度制御の場合と非加加速度制御の場合とを比較したグラフである。 駆動モータの角加速度の時間軸に対する変化パターンを示すグラフと駆動モータの速度の時間軸に対する変化パターンを示すグラフとを対比して示す図である。

以下、本発明の好ましい実施の形態を、図面を参照して説明する。なお、以下では全ての図を通じて同一又は対応する構成部材には同一の参照符号を付して、その説明については省略する。

図１は、本発明の実施形態に係るロボットアーム制御システム１００を示す概略図である。

本実施の形態に係るロボットアーム制御システム１００は、図１に示すように、多関節型のロボットアーム１と、該ロボットアーム１の動作を制御する制御装置２とを備えてなる構成である。

（ロボットアームの構成）
まず、ロボットアーム１の構成について図１および図２を参照して説明する。図２は、図１に示すロボットアーム制御システム１００が備えるロボットアーム１のリンク部材１３と関節部１２との位置関係を模式的に示す図である。

本実施の形態に係るロボットアーム１は、複数の関節部（第一関節部１２ａ〜第六関節部１２ｆ）と、関節部間をつなぐ、複数の棒形状のリンク部材（第一リンク部材１３ａ〜第五リンク部材１３ｅ）とを備えた、例えば、製造ラインにおける溶接、塗装、組み立て等を行う産業用ロボットである。なお、第一関節部１２ａ〜第三関節部１２ｃ、もしくは第一関節部１２ａ〜第六関節部１２ｆを特に区別して示す必要が無い場合は単に関節部１２と称する。また、第一リンク部材１３ａ〜第五リンク部材１３ｅを特に区別して示す必要がない場合は単にリンク部材１３と称する。

ロボットアーム１は、各関節部１２において、異なるリンク部材１３を相互に回動可能に接合し、関節部１２が備える駆動モータ１５により回動することができるようになっている。また、各関節部１２の駆動モータ１５の回転角を制御することにより、ロボットアーム１の先端部αの位置を決定することができる。また、各関節部１２の駆動モータ１５の角加速度を制御することにより、ロボットアーム１のある姿勢から別の姿勢へと変更する速度を決定することができる。

より具体的には、図１および図２に示すように、ロボットアーム１は、基部１１、基軸部３０、および手首軸部３１を備えてなる構成である。なお、図１および図２では特に図示していないが、手首軸部３１の先端部αには、対象物を把持するためのハンド、または対象物に対して溶接を行うための溶接装置等のエンドエフェクタが取り付け可能となっている。

基部１１は、ロボットアーム１を地面または載置台等に設置するための土台である。ロボットアーム１を安定して設置できるように底面において一定の面積を有する直方体形状をしているがこれに限定されるものではない。例えば、円柱形状であってもよい。

基部１１の上面には、基軸部３０が設けられている。基軸部３０は、ロボットアーム１の主たる動作姿勢を規定する部分であり、第一関節部１２ａ、第一リンク部材１３ａ、第二関節部１２ｂ、第二リンク部材１３ｂ、第三関節部１２ｃ、および第三リンク部材１３ｃから構成される。

第一関節部１２ａは、基部１１に対して、垂直方向に延在した回転軸を中心に回動自在となるように第一リンク部材１３ａの一端を接合する接合部１４ａと、基部１１に対して第一リンク部材１３ａを回動させる第一駆動モータ１５ａとを備えてなる構成である。すなわち、この第一関節部１２ａによって第一リンク部材１３ａを水平面上で旋回させることができる。

第一リンク部材１３ａは、その一方の端部で上述したように第一関節部１２ａを介して基部１１に接合されており、その他方の端部で、第二関節部１２ｂを介して第二リンク部材１３ｂと接合される。

第二関節部１２ｂは、第一リンク部材１３ａと第二リンク部材１３ｂとを相互に回動自在に接合する接合部１４ｂと、この第一リンク部材１３ａと第二リンク部材１３ｂとを回動させる第二駆動モータ１５ｂとを備えてなる構成である。第二関節部１２ｂは、紙面奥行き方向に延在する回転軸を中心に回動自在となるように第一リンク部材１３ａと第二リンク部材１３ｂとを接合する。

第二リンク部材１３ｂは、棒形状の部材であり、その一方の端部で上述したように第二関節部１２ｂを介して第一リンク部材１３ａと接合されており、その他方の端部で、第三関節部１２ｃを介して第三リンク部材１３ｃと接合される。

第三関節部１２ｃは、第二リンク部材１３ｂと第三リンク部材１３ｃとを相互に回動自在に接合する接合部１４ｃと、この第二リンク部材１３ｂと第三リンク部材１３ｃとを回動させる第三駆動モータ１５ｃとを備えてなる構成である。第三関節部１２ｃは、紙面奥行き方向に延在する回転軸を中心に回動自在となるように、すなわち第二関節部１２ｂと同じ方向に回動自在となるように、第二リンク部材１３ｂと第三リンク部材１３ｃとを接合する。

また、基軸部３０では、接合部１４ｂと接合部１４ｃとの間の第二リンク部材１３ｂにバランサ３２が設けられている。バランサ３２は、第二リンク部材１３ｂおよび第三リンク部材１３ｃの姿勢変更に伴い、第二駆動モータ１５ｂに発生する重力トルクを相殺して軽減するものである。このバランサ３２には、スプリングが内蔵されており、棒形状を成す第二リンク部材１３ｂと平行に配される。バランサ３２の一端は、第一リンク部材１３ａのある点に回動自在に取り付けられ、他端は、第二リンク部材１３ｂのある点、例えば、第三関節部１２ｃの回転軸に回動自在に取り付けられている。このため、第二リンク部材１３ｂが図１に示すような直立姿勢から手首軸部３１の先端方向側に傾斜するように回転し、これに伴い第三リンク部材１３ｃが移動したとき、このバランサ３２により第二リンク部材１３ｂに、その重力方向の反対向きの力が付与され、これにより第二関節部１２ｂの第二駆動モータ１５ｂへの重力によるトルクを軽減させることができる。

基軸部３０の先端部αは、手首軸部３１と接合されている。手首軸部３１は、作業を行うエンドエフェクタ（不図示）の細かい位置決めを制御する部分である。すなわち、本実施の形態に係るロボットアーム１では、基軸部３０によって自身の主たる姿勢を規定し、手首軸部３１によりエンドエフェクタの作業位置に応じるように細かい位置決めがなさるようになっている。

手首軸部３１は、第四関節部１２ｄ、第四リンク部材１３ｄ、第五関節部１２ｅ、第五リンク部材１３ｅ、および第六関節部１２ｆを備えてなる構成である。手首軸部３１も基軸部３０と同様に、第四関節部１２ｄを介して第三リンク部材１３ｃと第四リンク部材１３ｄとが、第五関節部１２ｅを介して、第四リンク部材１３ｄと第五リンク部材１３ｅとが相互に回動自在に取り付けられ、第四・第五駆動モータ１５ｄ、１５ｅによってそれぞれ回動させられるように構成されている。

より具体的には、第四関節部１２ｄおよび第六関節部１２ｆは、第三リンク部材１３ｃの長手方向と一致する方向に配された回転軸を中心に回動するようになっている。一方、第五関節部１２ｅは、第二、第三関節部１２ｂ、１２ｃと同じ方向に回動するようになっている。

このように、手首軸部３１は、基軸部３０と同様に複数の関節部１２を介して複数のリンク部材１３が接合されている。

（制御装置の構成）
次に、上述した構成を有するロボットアーム１の動作を制御する制御装置２について図３を参照して説明する。図３は、図１に示すロボットアーム制御システム１００が備える制御装置２に係る機能的構造の一例を示すブロック図である。

制御装置２は、ロボットアーム１の動作を制御するものであり、通信制御部５１、主制御部５２、および記憶装置５３を備えてなる構成である。

通信制御部５１は、主制御部５２からの制御指示に基づき、ロボットアーム１との通信を確立するものである。本実施の形態に係るロボットアーム１と制御装置２とは有線により接続されるように構成されているが、これに限定されるものではない。例えば、無線により接続を確立し、相互にデータを送受信できる構成であってもよい。

主制御部５２は、制御装置２における各種制御を行うものであり、機能ブロックとして、動作計画部６１および補正指令値生成部６２を備えてなる構成である。

動作計画部６１は、ロボットアーム１の動作パターンを規定する動作計画時において、このロボットアーム１の動作パターンを規定するためのロジックを設定するものである。また、動作計画部６１は、このロジックを設定するにあたり、ロボットアーム１の各関節部１２における駆動モータ１５の適切な角加速度（駆動モータ１５の動作中において最高となる角加速度）を求める。なお、動作計画部６１による各関節部１２における駆動モータ１５の角加速度の算出処理については後述する。

補正指令値生成部６２は、動作計画部６１で設定された動作計画のロジックに即して動作するように、駆動モータ１５に設定したパラメータに対する補正値の算出を行うものである。なお、補正指令値生成部６２によるこの補正値の算出処理についての詳細は後述する。

記憶装置５３は、読み書き可能な記録媒体であり、動作計画部６１にて駆動モータ１５の最高となる角加速度を算出するにあたり利用する情報である算出情報６３を記憶している。この算出情報６３には、駆動モータ１５において固有に定められた、駆動モータ１５の回転数に応じたモータトルクＴ_ｍｉを示すトルク情報も含まれる。なお、トルク情報については後述する。

ところで上述した構成を有する本実施の形態に係るロボットアーム制御システム１００では、ロボットアーム１の姿勢変更を考慮し、動作計画部６１が各関節部１２に作用する力の関係から適切な駆動モータ１５の最高となる角加速度を求める。そして、求めた角加速度となるように駆動モータ１５を回転させ、駆動モータ１５の加速時および減速時の速度が時間軸に対して台形（または、三角形）となるように駆動モータ１５の指令値を生成するようになっている。

つまり、駆動モータ１５は、加速、定速、減速という一連の速度パターンで動作を行うが、この速度パターンの加速時および減速時において角加速度が、時間軸に対して台形となるように駆動モータ１５への指令値を補正している。また、駆動モータ１５の動作において定速時がない場合、つまり加速してすぐに減速に動作が移るような場合は、この台形形状が三角形となる。このような場合も加速時および減速時において角加速度が時間軸に対して台形となるように駆動モータ１５に対する指令値を補正する。なお、本明細書では、この定速時が存在する動作を長ピッチの動作、定速時が存在しない動作を短ピッチの動作と称する。

上述したように、動作計画部６１は、駆動モータ１５の速度パターンが時間軸に対して台形（短ピッチの場合は三角形）となるようにロボットアーム１の動作計画を生成し、この動作計画に合わせて動作させる駆動モータ１５の最高角加速度を決定している。以下において、動作計画部６１による各関節部１２における駆動モータ１５の適切な角加速度の算出方法について説明する。

（ロボットアームの姿勢変更を考慮した角加速度の算出）
まず、動作計画部６１は、この角加速度の算出を、ロボットアーム１の動作パターンを規定する動作計画時に事前に求める。より具体的には、動作計画部６１は、この角加速度を、回転運動の運動方程式に重力トルクを加えた以下の方程式を用いて求める。

トルク＝イナーシャ×角加速度＋重力トルク
なお、角加速度以外の値（モータトルク、イナーシャ等に関連するパラメータの値）は、一番負荷の大きいロボットアーム１の操作開始時、動作終了時、加速開始時、加速終了時それぞれのタイミングで事前に求めている。つまり、実際にはロボットアーム１の動作姿勢をモニタリングしてイナーシャ等の値を求めることが理想的である。しかしながら、コストとパラメータの値の決定にかかる時間とを低減させるため、本実施の形態に係るロボットアーム１では、上述した一番負荷が大きくなるタイミングでモータトルクやイナーシャ等の値を求める。そして、求められたこれらの値は、算出情報６３として記憶装置５３に格納される。

本実施形態では、ロボットアーム１の主要な動作を規定する基軸部３０を例に挙げ、適切な角加速度の算出方法について説明するが、手首軸部３１についても同様にして角加速度を求めることができる。

基軸部３０は上述したように、第一関節部１２ａ、第二関節部１２ｂ、および第三関節部１２ｃの３つの関節部１２を有している。そこで、運動方程式をたてると次の数（１）ようになる。

なお、ここで駆動系伝達効率ＥＴＡ_ｉとは、駆動モータ１５の回転軸に対して垂直にとりつけられたギアなどの駆動系部材列を介し、リンク部材１３に伝えられる力の伝達効率である。また、減速比Ｒ_ｉとは、駆動モータ１５の回転速度とリンク部材１３の回転速度との比である。駆動系イナーシャＩ_ｉ２とは、駆動モータ１５の回転軸に対して算出される駆動系部材列の持つ慣性モーメントである。

なお、基軸部３０において、第一関節部１２ａと、第二関節部１２ｂおよび第三関節部１２ｃとは回動する方向が異なる。そこで、数式（１）では、第一関節部１２ａに対する第二関節部１２ｂ、第三関節部１２ｃから作用する力（慣性）は無視できるものとして計算することができる。また、逆に、第二関節部１２ｂおよび第三関節部１２ｃそれぞれに対する第一関節部１２ａから作用する力（慣性）も無視できるものとして計算することができる。

上記した数式（１）から分かるように、各関節部の回転軸まわりにおけるイナーシャＨ_ｉ(ｉ＝１，２，３)は、各関節部により回動されるリンク部材１３のもつ慣性モーメントを各関節部の回転軸に対するものとして算出したイナーシャＨ_ｉ(ｉ＝１，２，３)に、駆動モータ１５およびギアなどの慣性モーメントを、減速比Ｒ_ｉ(ｉ＝１，２，３)の二乗と駆動系伝達効率ＥＴＡ_ｉ(ｉ＝１，２，３)とをかけ合わせたものとを加えることで求めている。

つまり、モータイナーシャＩ_i１と駆動系イナーシャＩ_ｉ２とから、駆動モータ１５から関節部１２までの駆動系部材列の全慣性モーメントを駆動モータ１５の回転軸に対する慣性モーメントとして算出することができる。これに減速比Ｒ_ｉ(ｉ＝１，２，３)の二乗をかけることで、駆動系部材列の慣性モーメントを各関節部の回転軸に対する慣性モーメントとして算出することができる。さらに、これに駆動系伝達効率ＥＴＡ_ｉをかけ合わせることによって、駆動モータ１５の動力や駆動系部材列の慣性モーメントの影響がギア等の機械的伝達によりどれだけ伝達するか考慮することができる。

さらには、第二関節部１２ｂと第三関節部１２ｃとでは、両者の間で作用する慣性力による影響（干渉）Ｈ_２３、Ｈ_３２を考慮するようになっている。また、第二関節部１２ｂについては、第二リンク部材１３ｂからの重力トルクＵ_２と、バランサ３２によるバランサトルクＴ_Ｂとが考慮され、第三関節部１２ｃについては、第三リンク部材１３ｃからの重力トルクＵ_３を考慮するようになっている。また、トルクＴ_ｉ(ｉ＝１，２，３)は、駆動系伝達効率ＥＴＡ_ｉと減速比Ｒ_ｉとをモータトルクＴ_ｍｉ(ｉ＝１，２，３)に乗算して求められる。

次に数式（１）から、第一関節部１２ａにおける角加速度は、以下数式（２）のように求めることができる。

ただし、Ｈ_１１について、ロボットアーム１の動作開始姿勢時をＨ_１１Ｓ、動作終了姿勢時をＨ_１１Ｅとすると、これらのうち値の大きい方をＨ_１１として採用する（|Ｈ_１１|＝max(|Ｈ_１１Ｓ|,|Ｈ_１１Ｅ|)）。

つまり、第一リンク部材１３ａ、第二リンク部材１３ｂ、第三リンク部材１３ｃの姿勢変更に伴い、これらリンク部材１３の重心位置および第一関節部１２ａからの距離が異なることにより負荷イナーシャＨ_１１の値が変化する。そこで、動作開始時の負荷イナーシャＨ_１１Ｓと動作終了時の負荷イナーシャＨ_１１Ｅとを比較して一番値が大きい方、つまり、第一関節部１２ａに負荷がかかっている方の値を採用する。

次に、第二関節部１２ｂ、第三関節部１２ｃにおける角加速度を求める。第二関節部１２ｂ、第三関節部１２ｃでは、複合動作時の他の関節部１２からの慣性力による影響（干渉）を考慮し、下記に示すように求める。

まず、数式（１）から、第二関節部１２ｂにおける角加速度、第三関節部１２ｃにおける角加速度は、それぞれ以下の数式（３）、（４）のように求めることができる。

ただし、第二関節部１２ｂおよび第三関節部１２ｃの角加速度はこの時点では定まっていないので、最初の角加速度の算出は、以下の数式（５）、（６）を使用する。つまり、最初は第二関節部１２ｂおよび第三関節部１２ｃで相互に働く干渉Ｈ_２３はないものとして計算を行う。

これら数式（５）、（６）で第二関節部１２ｂおよび第三関節部１２ｃそれぞれの角加速度を求めると、この求めた値を利用して数式（３）、（４）を再計算する。より具体的には、数式（５）で求めた第二関節部１２ｂの角加速度、数式（６）で求めた第三関節部１２ｃの角加速度の値を利用して干渉項Ｈ_２３に第二関節部１２ｂ、第三関節部１２ｃそれぞれの角加速度を乗算した値（干渉に関わる値）を算出する。

この値の算出には、まず、第二関節部１２ｂ、第三関節部１２ｃそれぞれの加速時および減速時の運動を考慮する。

つまり、ロボットアーム１では、作業の内容、作業間隔等に応じて、駆動モータ１５が加速し、一定の速度となる期間が所定期間存在した後、減速を行う速度パターンとなる場合（長ピッチの場合）と、加速し、瞬時に減速に切り替わる速度パターンとなる場合（短ピッチの場合）の２つのパターンがありうる。

また、ロボットアーム１では、各関節部１２における運動速度がそれぞれ異なる場合、一番速度が遅い関節部１２に合わせて他の関節部１２も運動を行うように規定されている。すなわち、ロボットアーム１における加速および減速時では、全関節部１２の加速・減速時間を統一する。これは、定常速度到達までの時間を等しくして、補間精度を保つためである。そこで、以下の数式（７）によって一番、運動速度が遅い関節部１２を選択する。

そして、この数式（７）にて求めたＴＴを利用して、ロボットアーム１を動作させる駆動モータ１５の速度パターンが短ピッチであるのか長ピッチであるのか判定を行う。具体的には以下の数式（８）の不等式によって判定を行うことができる。

ここで、上記した長ピッチの場合の加速時間Ｔ_ａ、減速時間Ｔ_ｄ、ならびに短ピッチの場合の加速時間Ｔ_ａ、減速時間Ｔ_ｄをそれぞれ算出する。なお、求まる加速時間Ｔ_ａ、減速時間Ｔ_ｄの中で一番長い時間を求めるようにする。

したがって、加速時間Ｔ_ａ、減速時間Ｔ_ｄは、長ピッチの場合は、以下の数式（９）に短ピッチの場合は、以下の数式（１０）によって求められる。

また、長ピッチの場合の、干渉項Ｈ_２３に乗算する加速時の角加速度および減速時の角加速度を、数式（９）で求めた加速時間Ｔ_ａ、減速時間Ｔ_ｄを利用して求めると以下の数式（１１）のようになる。

一方、短ピッチの場合の、干渉項Ｈ_２３に乗算する加速時の角加速度および減速時の角加速度を、数（１０）で求めた加速時間Ｔ_ａ、減速時間Ｔ_ｄを利用して求めると以下の数式（１２）のようになる。

以上のようにして、第二関節部１２ｂ、第三関節部１２ｃについて加速時の角加速度と減速時の角加速度とを長ピッチの場合と短ピッチの場合とに分けて求める。このようにして求めた加速時と減速時との第二関節部１２ｂおよび第三関節部１２ｃの角加速度を干渉項Ｈ_２３に乗算して、数式（３）、（４）を求める。数式（３）、（４）を求めて得た結果（第二関節部１２ｂ、第三関節部１２ｃそれぞれの角加速度の値）を利用して再度、上述したように第二関節部１２ｂ、第三関節部１２ｃについて加速時および減速時の角加速度を長ピッチの場合と短ピッチの場合とに分けて求め、数式（３）、（４）を演算する。

また、第二関節部１２ｂおよび第三関節部１２ｃそれぞれにおける動作開始角度と終点角度とから、動作時における第二駆動モータ１５ｂおよび第三駆動モータ１５ｃそれぞれの加速時および減速時での角加速度を求める。そして、求めた加速時および減速時における第二関節部１２ｂおよび第三関節部１２ｃの角加速度を利用して第二駆動モータ１５ｂおよび第三駆動モータ１５ｃの（最高となる）回転数を求めるようにも構成されている。

次に図４を参照して、上記した第二関節部１２ｂ、第三関節部１２ｃにおける角加速度の算出方法の処理の流れを具体的に説明する。図４は、本実施の形態に係るロボットアーム１における第二関節部１２ｂおよび第三関節部１２ｃそれぞれの角加速度の算出フローを説明するフローチャートである。

ところで、上記した算出方法では、第二関節部１２ｂと第三関節部１２ｃとの間で作用する干渉（干渉項Ｈ_２３）を考慮した角加速度の演算について説明したが、本実施形態では、この干渉（干渉項Ｈ_２３）を考慮する際に、さらに駆動モータ１５の回転数に応じたモータトルクＴ_ｍｉを用いる。

より具体的には、例えば図５に示すように駆動モータ１５は、固有に定められた、駆動モータ１５の回転数に応じたモータトルクＴ_ｍｉが決まっている。図５は所定の定格出力容量を有する駆動モータ１５のモータトルクＴ_ｍｉと（最高）回転数との対応関係の概略を示す表である。この図５では、縦軸がモータトルクＴ_ｍｉの値を示し、横軸が駆動モータ１５の（最高）回転数を示している。

図５から分かるように、駆動モータ１５の（最高）回転数とモータトルクＴ_ｍｉとの関係は常に一定とならず、図５の点（ａ、ｂ）にて両者の対応関係を示すグラフの傾きが変化している。そこで、本実施形態では、動作計画部６１が、さらに駆動モータ１５において固有に定められる回転数に応じたモータトルクＴ_ｍｉも考慮して角加速度の算出を行う。

なお、ここでは、第二駆動モータ１５ｂ、および第三駆動モータ１５ｃの角加速度算出にあたり、この回転数とモータトルクＴ_ｍｉの特性について考慮するが、先に説明した第一駆動モータ１５ａの角加速度算出時にも同様に考慮してもよい。

図４に示すように、まず、動作計画部６１は、上記した数式（５）、（６）により、第二関節部１２ｂおよび第三関節部１２ｃの角加速度を算出する（ステップＳ１１、これ以降、Ｓ１１のように称する）。つまり、最初の段階では、第二・第三関節部１２ｂ、１２ｃについて角加速度が求められていないため、相互に作用する干渉を考慮することができない。また、同様に角加速度が求められていないため、駆動モータ１５固有に定められている回転数に応じたモータトルクＴ_ｍｉも考慮できない。したがって、ステップＳ１１で求めた角加速度は、第二・第三関節部１２ｂ、１２ｃが単独で動作するとした場合の角加速度である。

また、本実施形態では、制御装置２では、事前に各関節部１２の動作角度（動作距離）と各関節部１２の最高速度から動作速度を求め、最も遅い関節部１２の動作時をＴＴとして求めている。そこで、上記したステップＳ１１を行う際、この求めたＴＴを使って、単体で各関節部１２が動作する場合の動作時の動作速度を求め、加速時間Ｔ_ａと減速時間Ｔ_ｄとをそれぞれ算出する。

次に、上記して求めたＴＴと、各関節部１２での加速時間Ｔ_ａ、減速時間Ｔ_ｄとから、第二関節部１２ｂおよび第三関節部１２ｃそれぞれにおける第二・第三駆動モータ１５ｂ、１５ｃの速度パターンが長ピッチであるのか短ピッチであるのか判定する。具体的には、数式（８）に示す不等号を使って判定する（Ｓ１２）。

数式（８）に示す不等号の関係が成り立つ場合（Ｓ１２において「ＹＥＳ」）、第二・第三駆動モータ１５ｂ、１５ｃの速度パターンが長ピッチであると判定する。そして、動作計画部６１が、長ピッチの場合について、各関節部（第二関節部１２ｂ、第三関節部１２ｃ）に関して加速時間および減速時間を合わせる（Ｓ１３）。すなわち、ロボットアーム１全体の動きの中で、各関節部（第二関節部１２ｂ、第三関節部１２ｃ）での最大動作時間を求める。つまり、どれか一つの関節部１２の動作時間が最大となる場合、他の関節部１２はその動作時間に合わせて動作することとなる。各関節部１２の加速時間Ｔ_ａ、減速時間Ｔ_ｄは、長ピッチの場合、数式（９）に示す式により求められる。

このようにして、長ピッチの場合について各関節部１２の加速時間Ｔ_ａ、減速時間Ｔ_ｄを求めると、数式（３）、（４）の干渉項Ｈ_２３に乗算する第二関節部１２ｂ、第三関節部１２ｃの角加速度それぞれを数式（９）から求める。

そして、この求めた値を数式（３）、（４）に代入して第二関節部１２ｂ、第三関節部１２ｃそれぞれの角加速度を算出する（Ｓ１４）。この演算は、第二駆動モータ１５ｂおよび第二駆動モータ１５ｂそれぞれにおいて固有に定められている回転数に応じたモータトルクＴ_ｍｉも考慮して行う。

一方、ステップＳ１２において「ＮＯ」の場合、駆動モータ１５の速度パターンが短ピッチであると判定する。そして、動作計画部６１は、短ピッチの場合について、各関節部１２（第二関節部１２ｂ、第三関節部１２ｃ）に関して加速時間および減速時間を合わせる（Ｓ１８）。すなわち、ロボットアーム１全体の動きの中で、各関節部１２での加速時および減速時における最大動作時間を求める。各関節部１２の加速時間Ｔ_ａ、減速時間Ｔ_ｄは、短ピッチの場合、数式（２０）に示す式により求められる。

このようにして、短ピッチの場合について各関節部１２の加速時間Ｔ_ａ、減速時間Ｔ_ｄを求めると、数式（３）、（４）の干渉項Ｈ_２３に乗算する第二関節部１２ｂ、第三関節部１２ｃの角加速度を数式（１０）から求める。

そして、この求めた角加速度を数式（３）、（４）に代入して第二関節部１２ｂ、第三関節部１２ｃそれぞれの角加速度を算出する（Ｓ１９）。この演算は、駆動モータ１５において固有に定められている回転数に応じたモータトルクＴ_ｍｉも考慮して行う。

上述したステップＳ１３、Ｓ１４により、長ピッチの場合における各関節部（第二関節部１２ｂ、第三関節部１２ｃ）の角加速度を求めると、ステップＳ１３で求めた加速時間Ｔ_ａ、減速時間Ｔ_ｄを使って、駆動モータ１５の速度パターンが長ピッチであるのか短ピッチであるのか判定を再度行う（Ｓ１５）。同様に、上述したステップＳ１８、Ｓ１９により、短ピッチの場合における各関節部１２（第二関節部１２ｂ、第三関節部１２ｃ）の角加速度を求めると、ステップＳ１８で求めた加速時間Ｔ_ａ、減速時間Ｔ_ｄを使って、駆動モータ１５の速度パターンが長ピッチであるのか短ピッチであるのか判定を再度行う（Ｓ２０）。

つまり、各関節部１２（第二関節部１２ｂ、第三関節部１２ｃ）単体の動きだけを考慮してステップＳ１２では長ピッチであるか短ピッチであるかを判断していた。しかしながら、ステップＳ１３またはステップＳ１８では、ロボットアーム１の動きの中での各関節部１２（第二関節部１２ｂ、第三関節部１２ｃ）の動作速度に応じた加速時間Ｔ_ａ、減速時間Ｔ_ｄを求めた。

このため、加速時間Ｔ_ａ、減速時間Ｔ_ｄには変更が生じており、この変更により駆動モータ１５それぞれの速度パターンに対する判定が変化する場合がある。そこで、再度、ステップＳ１５、またはステップＳ２０で、駆動モータ１５の動作パターンが、長ピッチであるのか、短ピッチであるのかについての判定を行う。

ステップＳ１５の判定において「ＹＥＳ」の場合、これ以降の処理（ステップＳ１６、Ｓ１７）はステップＳ１３、ステップＳ１４と同様な演算を、ステップＳ１４で求めた第二関節部１２ｂ、第三関節部１２ｃそれぞれの角加速度の値を使って行うため説明は省略する。

また、ステップＳ２０の判定において「ＹＥＳ」の場合、ステップＳ１９で求めた第二関節部１２ｂ、第三関節部１２ｃそれぞれの角加速度の値を使って、テップＳ１３、ステップＳ１４と同様な演算を行うため説明は省略する。

一方、ステップＳ１５において「ＮＯ」の場合、ステップＳ２１に進み、ステップＳ１４で求めた第二関節部１２ｂ、第三関節部１２ｃそれぞれの角加速度の値を使って、ステップ１８、ステップＳ１９と同様な演算を行う（Ｓ２１、Ｓ２２）。このため、これ以降の処理ステップについての説明は省略する。

またステップＳ２０において「ＮＯ」の場合も、これ以降の処理（ステップＳ２１、Ｓ２２）はステップＳ１９で求めた第二関節部１２ｂ、第三関節部１２ｃそれぞれの角加速度の値を使って、テップＳ１８、Ｓ１９と同様な演算を行う（Ｓ２１、Ｓ２２）。このため、これ以降の処理ステップについての説明は省略する。

以上のように、３回の繰り返し演算により第二・第三関節部１２ｂ、１２ｃの角加速度を求める。なお、第二・第三関節部１２ｂ、１２ｃの角加速度を上述した３回の繰り返し演算により求める構成であるが、演算を行う制御装置２の主制御部５２の処理能力等により、この演算回数を３回よりも多く繰り返してもよい。演算を繰り返す回数が増えれば増えるほど、求められる第二・第三関節部１２ｂ、１２ｃにおける第二・第三駆動モータ１５ｂ、１５ｃの角加速度の値の精度は高くなっていく。

このようにして、主制御部５２における動作計画部６１により、ロボットアーム１の動作計画時に、第二関節部１２ｂおよび第三関節部１２ｃの適切な角加速度を求める。

以上、基軸部３０を例に挙げて各関節部１２における駆動モータ１５の適切な角加速度の算出処理について説明した。このように、動作計画時において設定したロジックでの駆動モータ１５の適切な角加速度を求めると、制御装置２は、次に、ロボットアーム１がより滑らかな動きとなるように、補正指令値（指令値）を生成する。より具体的には、駆動モータ１５（第一駆動モータ１５ａ、第二駆動モータ１５ｂ、第三駆動モータ１５ｃ）の角加速度の時間軸に対する変化が台形形状となるような補正指令値を生成する。

例えば、図６（ａ）に示すように、駆動モータ１５の動作速度（角速度）を時間軸に対して台形形状となるように制御するものとする。ここで、図６（ｂ）に示すように角加速度をいきなり最高角加速度となるように駆動モータ１５を動作させる場合と、角加速度の時間軸に対する変化が台形形状となるように駆動モータ１５を動作させる場合とを比較する。この比較において、図６（ｃ）に示すように前者は加加速度（ジャーク）がパルス状に発生する。一方、後者は、前者よりも加加速度の発生が大幅に低減される。なお、前者を非加加速度制御、後者を加加速度制御と称するものとする。

なお、図６（ａ）は、駆動モータ１５の動作速度の時間軸に対する変化について、本実施の形態に係る加加速度制御の場合と従来の非加加速度制御の場合とを比較したグラフである。図６（ｂ）は、駆動モータ１５を回す際の角加速度の時間軸に対する変化について、加加速度制御の場合と非加加速度制御の場合とを比較したグラフである。図６（ｃ）は、発生するジャークについて加加速度制御の場合と非加加速度制御の場合とを比較したグラフである。

ロボットアーム１のように運動系にばね性がある場合、上述した加加速度が大きいほど振動が出やすくなる。そこで、本実施形態では、補正指令値生成部６２が、角加速度の時間軸に対する変化が台形形状となるように駆動モータ１５を動作させるために、駆動モータ１５に対す指令値を補正した補正指令値を生成する。

（補正指令値の算出方法）
以下において、図７を参照して、補正指令値の算出方法について説明する。なお、駆動モータ１５の速度パターンが長ピッチの場合を例に挙げて説明する。図７は、駆動モータ１５の角加速度の時間軸に対する変化パターンを示すグラフと駆動モータ１５の速度の時間軸に対する変化パターンを示すグラフとを対比して示す図である。図７において、Ｖｍは駆動モータ１５の最高速度、Ａｍは駆動モータ１５の最高角加速度を示すものとする。

まず、図７の下側に示す、駆動モータ１５の速度の時間軸に対する変化パターンを表したグラフに示すように、駆動モータ１５の時間軸に対する速度のパターンをＴ１からＴ７までの７つの区間に分離する。そして、Ｔ１からＴ３までを加速時サイクル、Ｔ４を定速時サイクル、Ｔ５からＴ７を減速時サイクルと規定する。これら各区間の時間間隔は、所望される駆動モータ１５の時間軸に対する速度のパターンに応じて決定される。

また、算出する補正指令値は、駆動モータ１５の速度の時間軸に対する変化パターンを示す台形の面積を１として正規化し、目標地までの移動量を１から０までの面積変化で示すＳパラメータＳＰを用いる。

Ｔ１は、最高角加速度に到達するまでの加加速度区間である。つまり、図７の上側のグラフに示すように、Ｔ１区間において一定の変化率で最高角加速度に達するように角加速度を変化させる区間である。角加速度が一定の変化率で最高角加速度に達するように変化させることで、駆動モータ１５の速度は図７の下側グラフに示すようにその立ち上がりの傾斜が緩やかな曲線を描くこととなる。このため、加加速度（ジャーク）をパルス的に発生させないため、ロボットアーム１の動作時において生じる振動を低減することができる。

Ｔ１区間での角加速度、速度、および移動量は以下の式で求められる。まず、駆動モータ１５の速度をｖ、角加速度をａ、時間をｔ、任意の時点での位置をｓとおき、Ｉｎｔｅｇｒａｔｅ［ａ，ｔ］はａのｔについての積分、Ｉｎｔｅｇｒａｔｅ［ｓ，ｔ］はｓのｔについての積分をそれぞれ意味するものとする。

ここで、ロボットアーム１の移動量が１から０に向かって変化するものとした場合、１からｓを減算して得られる移動量の値ＳＰをＳパラメータとするとき、このＳパラメータは以下のようにして求められる。

ａ＝（Ａｍ／Ｔ１）×ｔ
ｖ＝Ｉｎｔｅｇｒａｔｅ［ａ，ｔ］
ｓ＝Ｉｎｔｅｇｒａｔｅ［ｖ，ｔ］
よって、
ａ＝Ａｍ×ｔ／Ｔ１
ｖ＝Ａｍ×ｔ^２／（２×Ｔ１）
ｓ＝Ａｍ×ｔ^３／（６×Ｔ１）
ＳＰ＝１−ｓ
＝１−Ａｍ×ｔ^３／（６×Ｔ１）
となる。

次に、Ｔ２区間に移るがＴ２区間での角加速度、速度、および移動量は以下の式で求めることができる。つまり、このＴ２区間は、最高角加速度に到達し、角加速度を一定に維持する区間である。図７の下側グラフで示されるように、Ｔ２の区間では駆動モータ１５は時間に比例して一定の角加速度で速度が上昇する。

まず、Ｔ２区間は、Ｔ１区間の後の区間、つまりＴ１時間経過後の区間であるため、Ｔ２区間の始点では、角加速度、速度、および移動量はＴ１区間の終点での角加速度ａ１、速度ｖ１、移動量ｓ１とすることができる。また、ｔ＝Ｔ１とおくことができ、Ｔ１区間にて求めた速度ｖ、角加速度ａ、移動量ｓの式に当てはめると以下のようになる。

ａ１＝Ａｍ
ｖ１＝Ａｍ×（Ｔ１）／２
ｓ１＝Ａｍ×（Ｔ１）^２／６
したがって、ｔ＝Ｔ１とするとＴ２区間での角加速度、速度、移動量は、
ａ＝Ａｍ
ｖ＝Ｉｎｔｅｇｒａｔｅ［ａ，ｔ］＋ｖ１
ｓ＝Ｉｎｔｅｇｒａｔｅ［ｖ，ｔ］
となる。

よって、
ｖ＝Ａｍ×ｔ＋Ａｍ×Ｔ１／２
ｓ＝Ａｍ×ｔ^２＋Ａｍ×ｔ×（Ｔ１）／２
となる。したがって、Ｓパラメータは以下の式で表すことができる。

ＳＰ＝１−ｓ−ｓ１
次に、Ｔ３区間に移るがＴ３区間での角加速度、速度、および移動量は以下の式で求めることができる。つまり、このＴ３区間は、最高角加速度からの減加速度区間である。図７の上側グラフに示すように、Ｔ３区間において一定の変化率で最高角加速度から０に向かって減じるように角加速度を変化させる区間である。角加速度を一定の変化率で最高角加速度から０（角加速度一定）に向かって変化させることで、駆動モータ１５の速度は図７の下側のグラフに示すようにその立ち上がりの傾斜が緩やかな曲線を描くこととなる。このように加加速度（ジャーク）をパルス的に発生させないため、ロボットアーム１の動作時において生じる振動を低減することができる。

まず、Ｔ３区間は、Ｔ１、Ｔ２区間の後の区間、つまりＴ１、Ｔ２時間経過後の区間であるため、Ｔ３区間の始点では、角加速度、速度、および移動量はＴ２区間の終点での角加速度ａ２、速度ｖ２とすることができる。また、Ｔ２区間での移動量ｓ２とし、ｔ＝Ｔ１＋Ｔ２とおきＴ２区間にて求めた速度ｖ、角加速度ａ、移動量ｓの式に当てはめると以下のようになる。

ａ２＝Ａｍ
ｖ２＝Ａｍ×（Ｔ１）／２＋Ａｍ×Ｔ２
ｓ２＝Ａｍ×（Ｔ２）^２＋Ａｍ×Ｔ２×（Ｔ１）／２
したがって、ｔ＝ｔ−Ｔ１−Ｔ２とするとこのＴ３区間での角加速度、速度、移動量は、
ａ＝Ａｍ−Ａｍ×ｔ／（Ｔ３）
ｖ＝Ｉｎｔｅｇｒａｔｅ［ａ，ｔ］＋ｖ２
ｓ＝Ｉｎｔｅｇｒａｔｅ［ｖ，ｔ］
よって、
ｖ＝Ａｍ×ｔ＋Ａｍ×（Ｔ１）／２＋Ａｍ×Ｔ２−Ａｍ×ｔ^２／（２×Ｔ３）
ｓ＝Ａｍ×ｔ×（３×（ｔ＋Ｔ１＋２×Ｔ２）−ｔ^２／Ｔ３）／６
ＳＰ＝１−ｓ−ｓ１−ｓ２
となる。

次のＴ４区間は最高速度Ｖｍでの等速区間となり、
ｖ＝Ｖｍ
移動量は、Ｔ１区間の移動量をｓ１、Ｔ２区間の移動量をｓ２、Ｔ３区間の移動量をｓ３とおくと、
ｓ＝ｓ１＋ｓ２＋ｓ３＋ｖ×（ｔ−Ｔ１−Ｔ２−Ｔ３）
ＳＰ＝１−ｓ
となる。

Ｔ４の定速時サイクルの次は、Ｔ５からＴ７を減速時サイクルへと移行する。この減速時サイクルは角加速度の変化の向きが負となる点を除いて基本的には、加速時サイクルと同様である。

Ｔ５区間は、負の最高角加速度（最高減速度）に到達するまでの加減速度区間である。つまり、図７の上側のグラフに示すように、Ｔ５区間において一定の変化率で負の最高角加速度（最高減速度）に達するように角加速度を変化させる区間である。角加速度を一定の変化率で最高減速度に達するように変化させることで、駆動モータ１５の速度は図７の下側のグラフに示すようにその立ち下がりの傾斜が緩やかな曲線を描くこととなる。このように加加速度（ジャーク）をパルス的に発生させないため、ロボットアーム１の動作時において生じる振動を低減することができる。

Ｔ５区間での角加速度、速度、および移動量は以下の式で求められる。まず、Ｔ５の次にくる区間Ｔ６、Ｔ７の移動量をそれぞれｓ６、ｓ７とすると、Ｔ１、Ｔ２区間での計算式より、
ｓ７＝Ａｍ×（Ｔ７）^２／６
ｓ６＝Ａｍ×（Ｔ６）^２＋Ａｍ×Ｔ６×（Ｔ７）／２
ここで、ｔ＝ｔ−Ｔ１−Ｔ２−Ｔ３−Ｔ４とすると、このＴ５区間の移動量は、区間Ｔ３での移動量の式
ｓ＝Ａｍ×ｔ×（３×（ｔ＋Ｔ１＋２×Ｔ２）−ｔ^２／Ｔ３）／６
において、Ｔ１、Ｔ２、Ｔ３をそれぞれＴ７、Ｔ６、Ｔ５で置き換え、ｔ＝Ｔ５−ｔで置き換えたものとなる。

よって、Ｔ５区間の残り移動量ｓは、ｔｍｐ＝Ｔ５−ｔとすると
ｓ＝Ａｍ×ｔｍｐ×（３×（ｔｍｐ＋Ｔ７＋２×Ｔ６）−ｔｍｐ^２／Ｔ５）／６
ＳＰ＝ｓ７＋ｓ６＋ｓ
となる。

続くＴ６区間は、負の最高角加速度（最高減速度）に到達し、角加速度を一定に維持する、最高減速度による減速区間である。図７の下側のグラフで示されるように、Ｔ６の区間では駆動モータ１５は時間に比例して一定の角加速度で速度が減少する。

ここでＴ７区間の移動量をｓ７とすると、
ｓ７＝Ａｍ×（Ｔ７）^２／６
となる。また、
ｔ＝ｔ−Ｔ１−Ｔ２−Ｔ３−Ｔ４−Ｔ５
とすると、このＴ６区間での残り移動量ｓは、Ｔ２区間で求めた移動量の式、
ｓ＝Ａｍ×ｔ^２＋Ａｍ×ｔ×（Ｔ１）／２
において、Ｔ１をＴ７に、ｔをＴ６−ｔにそれぞれ置き換えて求めることができる。

したがって、この区間での残り移動量ｓをｔｍｐ＝Ｔ６−ｔすると、
ｓ＝Ａｍ×ｔ^２＋Ａｍ×ｔｍｐ×（Ｔ７）／２
ＳＰ＝ｓ７＋ｓ
となる。

続くＴ７区間は、負の最高角加速度（最高減速度）からの減加速度区間である。図７の上側のグラフに示すように、Ｔ７区間において一定の変化率で負の最高角加速度（最高減速度）の値が正の方向に向かうように角加速度を変化させ、減速速度を低減させる区間（減減速度区間）である。角加速度が一定の変化率で最高減速度を低減させる方向（正の方向）に変化させることで、駆動モータ１５の速度は図７の下側のグラフに示すようにその立ち下がりの傾斜が緩やかな曲線を描くこととなる。このように加加速度（ジャーク）をパルス的に発生させないため、ロボットアーム１の動作時において生じる振動を低減することができる。

ここで、ｔ＝ｔ−Ｔ１−Ｔ２−Ｔ３−Ｔ４−Ｔ５−Ｔ６とする。そして、このＴ７区間での残りの移動量をＴ１区間で求めた以下の移動量の式を用いて求める。

ｓ＝Ａｍ×ｔ^３／（６×Ｔ１）
ただし、Ｔ７区間は、このＴ１区間の移動量の式において、Ｔ１をＴ７に置き換え、ｔをＴ７−ｔにそれぞれ置き換えたものとなる。

したがって、このＴ７区間での残り移動量ｓは、ｔｍｐ＝Ｔ７−ｔとすると、
ｓ＝Ａｍ×ｔｍｐ^３／（６×Ｔ７）
となり、Ｓパラメータは、
ＳＰ＝ｓ
となる。

以上のように、駆動モータの加速期間において、その加速開始から所定期間（Ｔ１区間）の角加速度と、加速から定速期間へと移行するまでの所定期間（Ｔ３区間）の角加速度とを一定の変化率で変化させる。すなわち、加速期間において駆動モータ１５の角加速度を時間軸に対して台形形状となるように制御させると、加速期間の最初と最後に生じるジャークを低減させることができる。

また、同様に、駆動モータの減速期間において、その減速開始から所定期間（Ｔ５区間）の角加速度と、減速終了するまでの所定期間（Ｔ７区間）の角加速度とを一定の変化率で変化させる。すなわち、減速期間において駆動モータ１５の角加速度を時間軸に対して負の方向に台形形状となるように制御させると、減速期間の最初と最後に生じるジャークを低減させることができる。

したがって、ロボットアーム１の動作時に生じる振動を低減させることができる。

また、本実施の形態に係るロボットアーム制御システム１００は動作計画時に設定したロジックに即して駆動モータ１５を動作させるにあたり、上述した加加速度制御によって、該駆動モータ１５へ与える指令値を補正する構成である。このため、フィルタを用いた平均化処理により指令値を補正する構成と比較してサイクルタイムを低減することができる。つまり、フィルタを用いた平均化処理により指令値を補正する構成の場合、所定のサンプリング時間当たりの指令値を算出し補正を行う必要があるため、時間遅れが生じ、本実施の形態に係る加加速度制御により加速度を制御する構成と比較してサイクルタイムが長くなってしまうからである。

なお、上記では、動作計画部６１は、駆動系イナーシャＩ_ｉ２およびバランサ３２によるバランサトルクＴ_Ｂも考慮して、ロボットアーム１の各関節部１２における駆動モータ１５の適切な角加速度を求める構成であった。しかしながら、駆動系イナーシャＩ_ｉ２またはバランサトルクＴ_Ｂからの影響が非常に小さい等、無視しても適切な角加速度を求めることができる場合、駆動系イナーシャＩ_ｉ２およびバランサトルクＴ_Ｂを必ずしも両方とも考慮する必要はない。すなわち、駆動系イナーシャＩ_i2およびバランサトルクＴ_Ｂのうちの少なくともいずれか一つだけを考慮する構成としてもよい。しかしながら、より正確に適切な角加速度を算出するためには、駆動系イナーシャＩ_ｉ２およびバランサトルクＴ_Ｂをともに考慮する構成の方が好適である。

上記説明から、当業者にとっては、本発明の多くの改良や他の実施形態が明らかである。従って、上記説明は、例示としてのみ解釈されるべきであり、本発明を実行する最良の態様を当業者に教示する目的で提供されたものである。本発明の精神を逸脱することなく、その構造及び／又は機能の詳細を実質的に変更できる。

本発明は、多関節型のロボットアーム１を滑らかに動作させるように制御する制御装置として有用である。

１ ロボットアーム
２ 制御装置
１１ 基部
１２ 関節部
１２ａ 第一関節部
１２ｂ 第二関節部
１２ｃ 第三関節部
１３ リンク部材
１３ａ 第一リンク部材
１３ｂ 第二リンク部材
１３ｃ 第三リンク部材
１４ 接合部
１４ａ 接合部
１４ｂ 接合部
１４ｃ 接合部
１５ 駆動モータ
１５ａ 第一駆動モータ
１５ｂ 第二駆動モータ
１５ｃ 第三駆動モータ
３０ 基軸部
３２ バランサ
５２ 主制御部
５３ 記憶装置
６１ 動作計画部
６２ 補正指令値生成部
６３ 算出情報
１００ ロボットアーム制御システム

Claims (5)

1. 基部と、
   第一リンク部材と、
   第二リンク部材と、
   第三リンク部材と、
   前記基部に対して、回動自在に前記第一リンク部材の一方の端部を接合する第一接合部と、該基部に対して前記第一リンク部材の一方の端部を回動させるための第一駆動モータとを有する第一関節部と、
   前記第一リンク部材の他方の端部と前記第二リンク部材の一方の端部とを互いに回動自在に接合する第一接合部と、該第一リンク部材の他方の端部と前記第二リンク部材の一方の端部とを互いに回動させるための第二駆動モータとを有する第二関節部と、
   前記第二リンク部材の他方の端部と前記第三リンク部材の一方の端部とを互いに回動自在に接合する第三接合部と、該第二リンク部材の他方の端部と前記第三リンク部材の一方の端部とを互いに回動させるための第三駆動モータとを有する第三関節部と、
   前記第二リンク部材および第三リンク部材の姿勢変更に伴い生じる前記第二関節部に加わる重力負荷を低減するためのばね装置と、を備えたロボットアームの動作を、前記第一駆動モータ、第二駆動モータ、および第三駆動モータを介して制御する制御装置であって、
   前記第一駆動モータ、第二駆動モータ、および第三駆動モータにかかる負荷イナーシャ、重力トルク、モータトルク、ならびにモータイナーシャに加えて、前記ばね装置のトルク、ならびに該第一駆動モータ、第二駆動モータ、および第三駆動モータそれぞれの動力を伝達するための駆動系部材列のイナーシャである駆動系イナーシャのうちの少なくともいずれか一つを用いて、前記第一駆動モータ、前記第二駆動モータ、および第三駆動モータの角加速度を算出する加速度算出手段を備えた制御装置。
2. 前記第二駆動モータおよび前記第三駆動モータそれぞれは、同一方向に延在した回転軸を中心に回動しており、
   前記加速度算出手段が、前記第二関節部が備える第二駆動モータおよび前記第三関節部が備える第三駆動モータの両者が共に動作している場合における、該第二関節部と該第三関節部との相互間で作用する慣性力に起因して生じるトルクを、前記第二駆動モータおよび第三駆動モータそれぞれのモータトルクから減算して、前記第二駆動モータおよび前記第三駆動モータの角加速度を算出する請求項１に記載の制御装置。
3. 前記第一駆動モータ、前記第二駆動モータ、および前記第三駆動モータそれぞれで固有に定められた、駆動モータの回転数に応じた駆動モータのトルクを示すトルク情報を記憶する記憶装置をさらに備え、
   前記加速度算出手段は、前記第一駆動モータ、第二駆動モータおよび前記第三駆動モータにおける動作開始から動作終了までの間の加速時および減速時における角加速度から、前記第一駆動モータ、前記第二駆動モータ、および前記第三駆動モータそれぞれの回転数を算出し、前記記憶装置に記憶した前記トルク情報に基づき、算出した回転数に対応する該第一駆動モータ、該第二駆動モータ、および第三駆動モータのモータトルクをそれぞれ求め、これら求めたモータトルクを用いて、前記第一駆動モータ、前記第二駆動モータ、および前記第三駆動モータの角加速度を算出する請求項２に記載の制御装置。
4. 前記加速度算出手段によって算出した角加速度が、時間軸に対して台形パターンとなるように前記第一駆動モータ、前記第二駆動モータ、および前記第三駆動モータに対する指令を補正するための補正指令値を生成する補正指令値生成手段を備える、請求項３に記載の制御装置。
5. 基部と、
   第一リンク部材と、
   第二リンク部材と、
   第三リンク部材と、
   前記基部に対して、回動自在に前記第一リンク部材の一方の端部を接合する第一接合部と、該基部に対して前記第一リンク部材の一方の端部を回動させるための第一駆動モータとを有する第一関節部と、
   前記第一リンク部材の他方の端部と前記第二リンク部材の一方の端部とを互いに回動自在に接合する第一接合部と、該第一リンク部材の他方の端部と前記第二リンク部材の一方の端部とを互いに回動させるための第二駆動モータとを有する第二関節部と、
   前記第二リンク部材の他方の端部と前記第三リンク部材の一方の端部とを互いに回動自在に接合する第三接合部と、該第二リンク部材の他方の端部と前記第三リンク部材の一方の端部とを互いに回動させるための第三駆動モータとを有する第三関節部と、
   前記第二リンク部材および第三リンク部材の姿勢変更に伴い生じる前記第二関節部に加わる重力負荷を低減するためのばね装置と、を備えたロボットアームの動作を、前記第一駆動モータ、第二駆動モータ、および第三駆動モータを介して制御するロボットアームの制御方法であって、
   前記第一駆動モータ、第二駆動モータ、および第三駆動モータにかかる負荷イナーシャ、重力トルク、モータトルク、ならびにモータイナーシャに加えて、前記ばね装置のトルク、該第一駆動モータ、第二駆動モータ、および第三駆動モータそれぞれの動力を伝達するための駆動系部材列のイナーシャである駆動系イナーシャのうちの少なくともいずれか一つを用いて、前記第一駆動モータ、前記第二駆動モータ、および第三駆動モータの角加速度を算出するロボットアームの制御方法。

Images