特開 2024-42136 ロボット制御情報生成装置、ロボット制御装置及びロボット制御情報生成方法

(19)【発行国】日本国特許庁(JP)

(12)【公報種別】公開特許公報(A)

(11)【公開番号】P2024042136

(43)【公開日】2024-03-28

(54)【発明の名称】ロボット制御情報生成装置、ロボット制御装置及びロボット制御情報生成方法

(51)【国際特許分類】

   B25J 9/22 20060101AFI20240321BHJP

   G05B 19/42 20060101ALI20240321BHJP

【ＦＩ】

B25J9/22 A

G05B19/42 C

【審査請求】未請求

【請求項の数】10

【出願形態】ＯＬ

(21)【出願番号】P 2022146629

(22)【出願日】2022-09-15

(71)【出願人】

【識別番号】000006666

【氏名又は名称】アズビル株式会社

(74)【代理人】

【識別番号】110003166

【氏名又は名称】弁理士法人山王内外特許事務所

(72)【発明者】

【氏名】大川 陽一

(72)【発明者】

【氏名】田原 鉄也

【テーマコード（参考）】

3C269

3C707

【Ｆターム（参考）】

3C269AB33

3C269BB09

3C269CC09

3C269MN05

3C269MN14

3C269MN15

3C269MN16

3C269MN27

3C269SA02

3C269SA22

3C269SA30

3C707AS01

3C707BS13

3C707BT05

3C707CT05

3C707HS27

3C707KS34

3C707KV01

3C707KW03

3C707KX06

3C707LS15

3C707LT07

3C707LU05

3C707LV15

3C707LV19

3C707LW07

3C707MT05

(57)【要約】

【課題】ロボットの手先部の移動速度を安定させることができるロボット制御情報生成装置、ロボット制御装置及びロボット制御情報生成方法を提供する。
【解決手段】ロボット制御情報生成装置（２０，２０Ａ）は、複数の関節（Ｊ１，Ｊ２）を有するロボット（Ｒ１）の手先部（Ｅ１）の予め設定された目標軌跡に基づいて、手先部が目標軌跡に沿って移動する際の複数の関節の動作方向を示す第１動作情報と、手先部が目標軌跡に沿って移動する際の複数の関節の第１動作情報とは異なる動作方向を示す第２動作情報と、を取得する動作情報取得部（２１）と、手先部が目標軌跡に沿って移動する際にいずれかの関節の動作方向が変化する変化点の発生条件に基づいて、第１動作情報及び第２動作情報のうち一方の情報を選択する選択部（２２）と、を備えた。
【選択図】図２

【特許請求の範囲】

【請求項1】

複数の関節を有するロボットの手先部の予め設定された目標軌跡に基づいて、前記手先部が前記目標軌跡に沿って移動する際の前記複数の関節の動作方向を示す第１動作情報と、前記手先部が前記目標軌跡に沿って移動する際の前記複数の関節の前記第１動作情報とは異なる動作方向を示す第２動作情報と、を取得する動作情報取得部と、
前記手先部が前記目標軌跡に沿って移動する際にいずれかの関節の動作方向が変化する変化点の発生条件に基づいて、前記第１動作情報及び前記第２動作情報のうち一方の情報を選択する選択部と、を備えた
ことを特徴とするロボット制御情報生成装置。

【請求項2】

前記選択部は、前記手先部が前記目標軌跡に沿って移動する際にいずれかの関節の動作方向が反転する前記変化点の前記発生条件に基づいて、前記第１動作情報及び前記第２動作情報のうち一方の情報を選択する
ことを特徴とする請求項１記載のロボット制御情報生成装置。

【請求項3】

前記発生条件は、前記変化点の発生する頻度である
ことを特徴とする請求項１記載のロボット制御情報生成装置。

【請求項4】

前記発生条件は、前記変化点が同時期に２以上の関節で発生する頻度である
ことを特徴とする請求項１記載のロボット制御情報生成装置。

【請求項5】

前記発生条件は、前記変化点が同時期に発生する関節の数である
ことを特徴とする請求項１記載のロボット制御情報生成装置。

【請求項6】

前記発生条件は、前記変化点が同時期に発生する関節の位置の差である
ことを特徴とする請求項１記載のロボット制御情報生成装置。

【請求項7】

前記選択部は、前記手先部が前記目標軌跡に沿って移動する際に、前記変化点の発生条件、及び予め設定された関節毎に異なる重み付けに基づいて、前記第１動作情報及び前記第２動作情報のうち一方の情報を選択する
ことを特徴とする請求項１記載のロボット制御情報生成装置。

【請求項8】

前記選択部は、前記ロボットが対象物に作用させる力の方向に基づいて、前記第１動作情報及び前記第２動作情報のうち一方の情報を選択する
ことを特徴とする請求項１記載のロボット制御情報生成装置。

【請求項9】

請求項１乃至８のいずれか１項記載のロボット制御情報生成装置と、
前記選択部が選択した前記第１動作情報及び前記第２動作情報のうち一方の情報に基づいて、前記複数の関節の駆動を制御する駆動制御部を備えた
ことを特徴とするロボット制御装置。

【請求項10】

動作情報取得部と、選択部と、を備えた装置が行うロボット制御情報生成方法であって、
前記動作情報取得部が、複数の関節を有するロボットの手先部の予め設定された目標軌跡に基づいて、前記手先部が前記目標軌跡に沿って移動する際の前記複数の関節の動作方向を示す第１動作情報と、前記手先部が前記目標軌跡に沿って移動する際の前記複数の関節の前記第１動作情報とは異なる動作方向を示す第２動作情報と、を取得するステップと、
前記選択部が、前記手先部が前記目標軌跡に沿って移動する際にいずれかの関節の動作方向が変化する変化点の発生条件に基づいて、前記第１動作情報及び前記第２動作情報のうち一方の情報を選択するステップと、を備えた
ことを特徴とするロボット制御情報生成方法。

【発明の詳細な説明】

【技術分野】

【0001】

本開示は、ロボット制御情報生成装置、ロボット制御装置及びロボット制御情報生成方法に関する。

【背景技術】

【0002】

従来、ロボットの先端部を対象物の表面に沿って倣い動作をさせる倣い制御装置が開示されている（特許文献１参照）。この倣い制御装置は、ロボットと対象物に作用する力の検出結果と、ロボットの現在位置の検出結果と、に基づいてロボットに倣い動作をさせる際の軌道を生成する。

【先行技術文献】

【特許文献】

【0003】

【特許文献1】特開１９９１－１８４７８６号公報

【発明の概要】

【発明が解決しようとする課題】

【0004】

一般に、複数の関節を有するロボットの手先部（先端部）を目標軌跡に沿って移動させる際に、目標軌跡上でいずれかの関節の動作方向が変化する変化点が発生する場合、変化点で当該関節の動作が停止すると、当該関節において静止摩擦力が発生する。このような場合、手先部の移動中に関節において静止摩擦力が発生すると、静止摩擦力が外乱となり手先部の移動速度を安定させることが難しい。また、ロボットが対象物に作用する力を安定させることも難しくなる。

【0005】

本開示は、上記課題を解決するものであって、ロボットの手先部の移動速度を安定させることができるロボット制御情報生成装置、ロボット制御装置及びロボット制御情報生成方法を提供することを目的としている。

【課題を解決するための手段】

【0006】

本開示に係るロボット制御情報生成装置は、複数の関節を有するロボットの手先部の予め設定された目標軌跡に基づいて、手先部が目標軌跡に沿って移動する際の複数の関節の動作方向を示す第１動作情報と、手先部が目標軌跡に沿って移動する際の複数の関節の第１動作情報とは異なる動作方向を示す第２動作情報と、を取得する動作情報取得部と、手先部が目標軌跡に沿って移動する際にいずれかの関節の動作方向が変化する変化点の発生条件に基づいて、第１動作情報及び第２動作情報のうち一方の情報を選択する選択部と、を備えたことを特徴とする。

【発明の効果】

【0007】

本開示によれば、上記のように構成し、手先部の目標軌跡上でいずれかの関節の動作方向が変化する変化点の発生条件に基づいて、複数の関節の動作量を示す第１動作情報及び第２動作情報のうちいずれか一方の情報を選択するので、例えば、変化点による手先部の移動速度への影響が少ない関節の動作情報を選択することが可能になり、手先部の移動速度を安定させることができる。また、ロボットが対象物に作用する力を安定させることも可能になる。

【図面の簡単な説明】

【0008】

【図1】実施の形態１に係るロボットアームの概略構成を示す斜視図。

【図2】図２Ａは、実施の形態１に係るロボット制御システムの構成を示すブロック図、図２Ｂは、実施の形態１に係る関節制御部及び関節の構成を示すブロック図。

【図3】実施の形態１に係るロボット制御装置のハードウェア構成の例を示す図。

【図4】実施の形態１に係るロボット制御装置のハードウェア構成の例を示す図。

【図5】実施の形態１に係るロボット制御装置が行う処理を示すフローチャート。

【図6】実施の形態１に係るロボットアームの手先の目標軌跡を示す模式図。

【図7】腕姿勢を表現するパラメータを示す図。

【図8】アームアングルの概念を示す図。

【図9】実施の形態１に係る関節軌跡生成部が生成した関節軌跡を示す表。

【図10】実施の形態１に係る関節軌跡生成部が生成した関節軌跡を示す表。

【図11】腕姿勢を表現するパラメータを示す図。

【図12】実施の形態１に係るロボットアームの目標軌道を示すグラフ。

【図13】実施の形態１に係る関節軌跡生成部が生成した関節軌跡を示すグラフ。

【図14】実施の形態１に係る関節軌跡生成部が生成した関節軌跡における角速度の変化を示すグラフ。

【図15】実施の形態１に係る関節軌跡生成部が生成した関節軌跡を示すグラフ。

【図16】実施の形態２に係るロボット制御システムの構成を示すブロック図。

【図17】実施の形態２に係る力制御部の構成を示すブロック図。

【図18】実施の形態２係る関節軌跡生成部が生成した関節軌跡を示す表。

【図19】実施の形態２係る関節軌跡生成部が生成した関節軌跡を示す表。

【図20】実施の形態２の変形例に係る力制御部の構成を示すブロック図。

【発明を実施するための形態】

【0009】

以下、本開示に係る実施の形態について図面を参照しながら詳細に説明する。
実施の形態１．
まず、図１を参照して、実施の形態１に係るロボットアームＲ１の概略構成について説明する。図１は、実施の形態１に係るロボットアームＲ１の概略構成を示す斜視図である。図１に示すように、ロボットとしてのロボットアームＲ１は、互いに相対移動可能な複数のリンクを有しており、複数のリンク間には、それぞれ隣接するリンク間を接続する関節が形成されている。実施の形態１に係るロボットアームＲ１は、冗長な自由度を有する多関節ロボット（冗長アーム）である。

【0010】

冗長アームとは、目標とする作業に必要な自由度以上の関節自由度を有するロボットアームのことを指す。例えば、目標とする作業に必要な自由度を直交座標において並進３方向と回転３方向の６自由度とした場合、７自由度を有するロボットアームは、冗長アームといえる。このとき７自由度を有するロボットアームは、ロボットアームの手先位置姿勢に影響を及ぼさない機構内部における１自由度のセルフモーションが可能であり、冗長自由度が１と呼ぶ。アームの冗長性によって、手先に備えたツールの位置姿勢を実現するための各関節の組み合わせが１つに限定されないため、特異姿勢や障害物回避といった作業の融通性が高められる。
本開示も、アームの冗長性によって、手先に備えたツールの位置姿勢を実現するための各関節の組み合わせが１つに限定されないことを利用することで、手先部の移動速度やロボットが対象物に作用する力がより安定する関節の動作情報を選択することを可能にしている。

【0011】

例えば、ロボットアームＲ１は、リンクＬ１と、リンクＬ１に対して回動可能に支持されているリンクＬ２と、リンクＬ２に対して回動可能に支持されているリンクＬ３と、リンクＬ３に対して回動可能に支持されているリンクＬ４と、リンクＬ４に対して回動可能に支持されているリンクＬ５と、リンクＬ５に対して回動可能に支持されているリンクＬ６と、リンクＬ６に対して回動可能に支持されているリンクＬ７と、リンクＬ７に対して回動可能に支持されているリンクとしてのエンドエフェクタＥ１と、を備えている。

【0012】

また、例えば、ロボットアームＲ１は、リンクＬ１とリンクＬ２との間に関節Ｊ１が、リンクＬ２とリンクＬ３との間に関節Ｊ２が、リンクＬ３とリンクＬ４との間に関節Ｊ３が、リンクＬ４とリンクＬ５との間に関節Ｊ４が、リンクＬ５とリンクＬ６との間に関節Ｊ５が、リンクＬ６とリンクＬ７との間に関節Ｊ６が、リンクＬ７とエンドエフェクタＥ１との間に関節Ｊ７が、それぞれ形成されて、７軸のロボットとして構成されている。

【0013】

次に、図２Ａ及び図２Ｂを参照して、実施の形態１に係るロボット制御システム１について説明する。図２Ａは、実施の形態１に係るロボット制御システム１の構成を示すブロック図であり、図２Ｂは、実施の形態１に係る関節制御部３１及び関節Ｊ１の構成を示すブロック図である。実施の形態１に係るロボット制御システム１は、上述したロボットアームＲ１と、ロボットアームＲ１と電気的に接続されてロボットアームＲ１を制御するロボット制御装置１０と、を備えている。

【0014】

ロボット制御装置１０は、メイン制御部２０、関節制御部３１，３２，・・・，３ｎからなる複数の関節制御部を備えている。上述したようにロボットアームＲ１は、関節Ｊ１，Ｊ２，・・・，Ｊｎからなる複数の関節を有しており、関節一つに関節制御部一つが対応するように設けられている。すなわち、ロボット制御装置１０は、ロボットアームＲ１の関節の数と同数の関節制御部を有している。

【0015】

図２Ｂは、一関節分の関節Ｊ１及び関節制御部３１の詳細を図示したものである。ロボット制御情報生成装置としてのメイン制御部２０は、関節軌跡生成部２１、関節軌跡生成結果判定部２２、関節軌道生成部２３、送信部２４を有している。関節制御部３１は、受信部３１１と駆動制御部３１２とを有している。各関節は、関節を駆動するモータＭ１と、関節の位置（関節角）を計測するためのエンコーダＥＮ１とを備えている。このように構成されて、実施の形態１に係るロボット制御装置１０は、ロボットアームＲ１の手先（手先部）であるエンドエフェクタＥ１の位置姿勢（軌跡）は変えずに、関節角速度が同時期にゼロクロスとなる関節の数を出来るだけ減らす関節軌跡を探索する。そして、探索によって得られた関節軌跡に沿って各関節が動作するように、関節制御部が各関節の動作を制御する。

【0016】

なお、ロボットアームＲ１の手先の位置姿勢とは、ロボットアームＲ１の手先の位置及びアームの手先の姿勢のうちの少なくとも一方を意味する。また、ロボットアームＲ１の手先の位置とは、ロボットアームＲ１の先端に設けられたエンドエフェクタＥ１の先端または当該先端を基準とした任意の点、例えば、ツールセンターポイントの位置を意味し、ロボットアームＲ１の手先の姿勢とは、エンドエフェクタＥ１の向きを意味する。なお、実施の形態１において、ロボットアームＲ１の手先の位置姿勢をロボットアームＲ１の位置姿勢またはツールの位置姿勢と記述する場合があるが、特に断りのない場合、これらは同じ意味である。また、実施の形態１において、関節軌跡とは、ロボットアームＲ１の手先の軌跡を実現するための各関節角の系列データを意味する。上述したように、ロボットアームＲ１は、関節毎にモータ及びエンコーダを有している。これらのモータ及びエンコーダは、それぞれ、対応する関節制御部に対して電力線等により接続されている。エンコーダは、対応する関節の角度の現在値を検出する。

【0017】

動作情報取得部としての関節軌跡生成部２１は、ロボットアームＲ１の手先の目標軌跡を実現するように、各関節が取りうる関節軌跡を生成する。言い換えると、関節軌跡生成部２１は、ロボットアームＲ１の手先の目標軌跡に基づいて、手先が目標軌跡に沿って移動する際の複数の関節の関節軌跡を生成することで、当該関節軌跡を取得している。なお、実施の形態１におけるロボット制御装置１０は、上述したように冗長アームとして用いられるため、手先の目標軌跡を実現する関節軌跡は、複数の解軌跡が存在する。

【0018】

選択部としての関節軌跡生成結果判定部２２は、ロボットアームＲ１の手先が目標軌跡に沿って移動する際にいずれかの関節の動作方向が変化する変化点の発生条件に基づいて、関節軌跡生成部２１が生成した複数の関節軌跡のうちいずれかの関節軌跡を選択する。例えば、関節軌跡生成結果判定部２２は、関節軌跡生成部２１で生成した関節軌跡生成結果を解析し、角速度のゼロクロスが同時期に発生する軸数などから、位置や速度偏差が小さくなることを期待できる軌跡関節軌跡生成結果かどうかを判定する。例えば、関節軌跡生成結果判定部２２は、複数ある関節軌跡生成結果のうち、より速度偏差が小さくなることを期待できる関節軌跡生成結果をひとつ選択する。
ここで、「同時期の発生」とは、段落［０００４］で述べたように関節の動作が停止した際、静止摩擦力によって影響を受ける時間を考慮した上で、同時と考えられる程度の同時性を満たす、予め設定された所定時間内の発生を指す。例えば、ロボットアームＲ１が、関節の動作が停止して、静止摩擦力を振り切って再度関節が動き出した後、速度偏差が小さくなるまで約５０ミリ秒かかるロボットであるものとする。この場合、この約５０ミリ秒の間に複数の軸で角速度のゼロクロスが発生した場合は、ゼロクロスが複数の軸で同時期に発生したものと考える。つまり、ゼロクロスの発生が厳密に同時でなくても、複数のゼロクロスがロボットの動作へ同時に影響する程度小さい時間内にゼロクロスの発生があった場合は、同時期にゼロクロスが発生したものとする。

【0019】

関節軌跡生成結果判定部２２によって選択された関節軌跡生成結果は、関節座標系において軌跡が通過する点だけが決められており、通過する時刻の情報や速度に関する情報は含まれていない。実際にロボットを駆動するためには、通常、通過する時の速度情報も必要なので、関節軌道生成部２３が、前述した関節軌跡生成結果に角速度情報を付加して関節軌道を生成する。この角速度情報は、ユーザーが作成したロボットプログラム等から指定された、手先が軌跡上を移動する速度の目標値などに応じて、与えられる。関節軌道は、ロボットアームＲ１の動作開始時を始点とする時系列データであり、ある時刻において各関節が通過する点が時系列として記録されている。

【0020】

送信部２４は、この時系列データを始点から終点に向かって順に、各関節制御部が持つ受信部３１１へと所定の通信周期で、関節角指令値として送信する。なお、関節軌道のデータは、ある時刻における全関節の関節角がひとつのベクトルとなっているが、送信部２４はこのベクトルから各関節に対応する要素を取り出し、対応する関節制御部の受信部３１１へと送信する。

【0021】

受信部３１１は、メイン制御部２０の送信部２４から送信された関節角指令値を受信する。駆動制御部３１２は、エンコーダＥＮ１で取得されたロボットアームＲ１の関節角の計測値が、受信部３１１が受信した関節角指令値と一致するように制御する。各関節でこのような制御を行うことで、ロボットアームＲ１全体が関節軌道に従って駆動するように制御される。駆動制御部３１２の制御結果は、ロボットアームＲ１のモータＭ１を駆動する電流指令値として算出され、モータＭ１に出力される。

【0022】

次に、図３及び図４を参照して、ロボット制御装置１０のハードウェア構成について説明する。図３は、実施の形態１におけるロボット制御装置１０のハードウェア構成の一例を示す図であり、図４は、実施の形態１におけるロボット制御装置１０のハードウェア構成の他の例を示す図である。例えば、図３に示すように、ロボット制御装置１０は、プロセッサ１０ａ、メモリ１０ｂ及びＩ／Ｏポート１０ｃを有し、メモリ１０ｂに格納されているプログラムをプロセッサ１０ａが読み出して実行するように構成されている。

【0023】

また、例えば、図４に示すように、ロボット制御装置１０は、専用のハードウェアである処理回路１０ｄ及びＩ／Ｏポート１０ｃを有している。処理回路１０ｄは、例えば、単一回路、複合回路、プログラム化したプロセッサ、並列プログラム化したプロセッサ、システムＬＳＩ（Ｌａｒｇｅ－Ｓｃａｌｅ Ｉｎｔｅｇｒａｔｉｏｎ）、ＡＳＩＣ（Ａｐｐｌｉｃａｔｉｏｎ Ｓｐｅｃｉｆｉｃ Ｉｎｔｅｇｒａｔｅｄ Ｃｉｒｃｕｉｔ）、ＦＰＧＡ（Ｆｉｅｌｄ Ｐｒｏｇｒａｍｍａｂｌｅ Ｇａｔｅ Ａｒｒａｙ）、又はこれらの組み合わせによって構成される。ロボット制御装置１０の各機能は、これらプロセッサ１０ａ又は専用のハードウェアである処理回路１０ｄがソフトウェアであるプログラムを実行することによって実現される。

【0024】

次に、図５乃至図１５を参照して、所定の倣い軌道を目標軌道として与えられる場合を例に、ロボット制御装置１０による関節軌跡生成部２１及び関節軌跡生成結果判定部２２における処理について説明する。なお、目標軌道とは、ロボットアームＲ１の手先の動作開始時を始点とする時系列データであり、ある時刻においてロボットアームＲ１の手先が通過する位置の点と各点における姿勢を示す値とが時系列として記録されている。

【0025】

図５は、実施の形態１に係るロボット制御装置１０が行う処理を示すフローチャートである。処理を開始すると、ロボット制御装置１０は、まず、目標軌道を取得する（ステップＳＴ１）。この処理において、関節軌跡生成部２１は、ロボットアームＲ１の手先を作業の対象物の表面に沿って倣い動作をさせる倣い軌道のデータを取得している。なお、関節軌跡生成部２１は、目標軌道をロボット制御装置１０に接続されている外部の装置（不図示）から取得してもよいし、ロボット制御装置１０が備えている記憶装置（不図示）から取得してもよい。

【0026】

目標軌道を取得すると、ロボット制御装置１０は、関節軌跡生成部２１によって、アーム手先の移動速度の目標値に関する情報を持たない、目標軌跡を算出する（ステップＳＴ２）。図６は、実施の形態１に係るロボットアームＲ１の手先の目標軌跡を示す模式図であり、図７は、始点・選択した中間地点・終点におけるロボットアームＲ１の手先の位置姿勢ｐ、角度θ、後述するアームアングルΨをパラメータとして表記した図である。図６に示すように、例えば、ロボットアームＲ１が動作を開始してから終了するまでにおいて、Ｊ１からＪ７の各関節が連携して動作することで、手先Ｅ１が始点から終点まで移動する。ステップＳＴ２の処理を行うと、関節軌跡生成部２１は、ロボットアームＲ１の逆運動学を解いて、目標軌跡に対応する関節軌跡を生成する（ステップＳＴ３）。

【0027】

一般に、冗長アームでは、ロボットアームの手先の位置姿勢を実現するための各関節の組み合わせが一意に決定されないために逆運動学を解析的に解くことができないが、アーム構造によっては冗長自由度を表現するためのパラメータを導入することよって、逆運動学を解くことができるものがある（非特許文献１参照）。

【非特許文献1】清水昌幸，ｅｔ ａｌ．関節の可動範囲を考慮に入れた７自由度冗長マニピュレータの解析的逆運動学解法 日本ロボット学会誌，２００７，２５．４：６０６－６１７．

【0028】

冗長自由度を表現するパラメータは、いくつか提示されている。例えば、冗長自由度の数の分だけの関節を冗長関節として選択し、その関節変位を冗長パラメータとする手段がある。また他には、７自由度アームのセルフモーションを直接的に記述する方法として、「アームアングルΨ」というパラメータを導入する手段がある（特許文献２及び非特許文献２参照）。

【特許文献2】特開２０１２－０４０６３３

【非特許文献2】ＦＡＲＩＡ，Ｃａｒｌｏｓ，ｅｔ ａｌ．Ｐｏｓｉｔｉｏｎ－ｂａｓｅｄ ｋｉｎｅｍａｔｉｃｓ ｆｏｒ ７－ＤｏＦ ｓｅｒｉａｌ ｍａｎｉｐｕｌａｔｏｒｓ ｗｉｔｈ ｇｌｏｂａｌ ｃｏｎｆｉｇｕｒａｔｉｏｎ ｃｏｎｔｒｏｌ，ｊｏｉｎｔ ｌｉｍｉｔ ａｎｄ ｓｉｎｇｕｌａｒｉｔｙ ａｖｏｉｄａｎｃｅ．Ｍｅｃｈａｎｉｓｍ ａｎｄ Ｍａｃｈｉｎｅ Ｔｈｅｏｒｙ，２０１８，１２１：３１７－３３４．

【0029】

アームアングルは、回転軸が一点で交差する肩部の三関節と、回転軸が一点で交差する手首の三関節と、それらを結合する肘関節からなる７自由度のロボットアームに対して定義できる。図８に示すように、アームアングルは、ロボットアームの肩部三関節の交点、肘関節位置及び手首三関節の交点の三点を通る平面（以下、アーム平面と呼ぶ）と、その参照面との間の角度として定義される、腕姿勢を表現するパラメータである。図８は、腕姿勢を表現するパラメータを示す図である。参照面の決め方は、非特許文献２に記述されているように、それ自体が研究対象となっており、いくつか手法があるが、簡単な方法として、肩部三関節の交点と手首三関節の交点を結ぶ線と、所定のベクトルから決まる平面を参照面とする方法がある。

【0030】

図８は、参照面を決める所定のベクトルを鉛直上向き方向に取った場合の参照面と、アームアングルと、を示すアームアングルの概念図である。こうすることで、７自由度のロボットアームの冗長パラメータとしてのアームアングルを定義することができる。なお、参照面を決めるベクトルを、所定のベクトルに固定する方法は、肩部三関節の交点と手首三関節の交点を結ぶ線が所定のベクトルと平行になると、参照面を決めることができなくなり、アームアングルを定義できなくなるという欠点がある。しかし、例えば、ロボットアームで力作業を行う場合は、肩部三関節の交点と手首三関節の交点を結ぶ線が鉛直になることは少ないと考えられるので、実施の形態１においては考慮しないこととする。仮に、肩部三関節の交点と手首三関節の交点を結ぶ線が鉛直になる場合は、所定のベクトルを鉛直上向き以外に定めればよい。非特許文献２では、アームアングルが定義できないケースが起きないような参照面の決め方についても開示されているが、実施の形態１ではこれ以上議論しない。もちろん、非特許文献２や、その参考文献に記載された方法で参照面を決め、アームアングルを定義することも可能である。

【0031】

以下では、冗長自由度を表現するパラメータのひとつであるアームアングルΨを説明に用いて、関節軌跡を生成する処理の詳細について説明する（非特許文献２参照）。なお、下記に示す処理は、関節軌跡を生成する処理の一例であり、これに限定されない。アームアングルΨを変更することにより、各関節角の組み合わせを変えることができる。この時、関節軌跡が不連続にならないようにする必要があるが、これはアームアングルが連続であれば実現できる。すなわち、アームアングルが時間微分可能であれば、関節軌跡が不連続となることはない。

【0032】

まず、関節軌跡生成部２１は、手先の目標軌跡に対して、十分に細かい空間分解能を有するのに必要な系列データの数ｎになるよう、中間地点を（ｎ－２）点用意する。関節軌跡生成部２１では最初に、始点と終点の他に、中間地点の中から１点選択して用いる。ここで、始点・選択した中間地点・終点における位置姿勢ｐ、角度θ、アームアングルΨをパラメータとして表記すると、図７のようになる。各記号の添字は、始点が０、選択した中間地点がｉ（０＜ｉ＜ｎ）、終点がｎである。ｐ_０，ｐ_ｉ，ｐ_ｎは、目標軌跡内にあらかじめ定められている値である。

【0033】

次に、関節軌跡生成部２１は、アームアングルΨ_ｋ（ｋ＝０，ｉ，ｎ）の初期値を設定する。このとき、アームの姿勢が急激に変化しないよう、アームアングルの空間変化量ΔΨ_ｋ＝Ψ_ｋ－Ψ_ｋ－１（ｋ＝ｉ，ｎ）が、所定の範囲内を満たすよう制約を設ける。例えば、アームアングルΨ_ｋ（ｋ＝０，ｉ，ｎ）がすべて同じ値とすると、空間変化量ΔΨ_ｋ（ｋ＝ｉ，ｎ）が０となり、アームの姿勢が急激に変化しないため、これを初期値としてもよい。

【0034】

ｋ＝０，ｉ，ｎにおけるアームアングルΨが決定した後、関節軌跡生成部２１は、すべての中間地点を含む系列データの数ｎに対応したアームアングルΨを、例えば、直線補間やスプライン補間によって決定する。このときアームアングルΨは、アームの姿勢が急激に変化しないよう、アームアングルの空間変化量ΔΨ_ｋ＝Ψ_ｋ－Ψ_ｋ－１（１≦ｋ≦ｎ）が、所定の範囲内を満たすよう制約を設ける。系列の全データ数ｎに対応するアームアングルΨが決定すれば、それに対応した各関節角θｋ（０≦ｋ≦ｎ）を、逆運動学により求めることができる。

【0035】

関節軌跡生成部２１が関節軌跡を生成すると、関節軌跡生成結果判定部２２は、手先が目標軌跡に沿って移動する際にいずれかの関節の動作方向が変化する変化点の発生条件に基づいて、複数の関節軌跡のうちいずれか適切な関節軌跡を選択する（ステップＳＴ４）。例えば、関節軌跡生成結果判定部２２は、十分に細かい空間分解能を有するのに必要な系列データの数ｎの関節軌跡のデータに対し、関節の動作方向が変化する変化点である角速度のゼロクロスが発生する位置姿勢や回数といった情報に基づいて、あらかじめ定めた所定の評価関数を計算し、算出された評価関数の値に基づいて、適切な関節軌跡を選択する。

【0036】

以下に、関節軌跡生成結果判定部２２の評価関数の一例として、同時期にゼロクロスが発生する関節の数（同時発生軸数）を用いた場合を説明する。まず、関節軌跡生成結果判定部２２は、得られた関節軌跡、すなわちｎ個の関節角θ_ｋ（０≦ｋ≦ｎ）の系列データから、ゼロクロスが発生するかを判定する。すなわち、θ_０～θ_ｍ～θ_ｎと関節角が変化する過程で、角度が一定ないし単調増加もしくは単調減少でない場合には、θ_ｍで関節角の回転方向が反転する、すなわち角速度のゼロクロスが発生すると見做せるので、これの有無を判定する。
なお、より正確には、角度が一定の区間については注意を要する。例えば、θ_０～θ_ｍ～θ_ｎとずっと関節角が一定の場合は、この関節はθ_０～θ_ｎの間でずっと静止しているので、ゼロクロスは発生していないと考えられる。一方、θ_０～θ_ｍでは静止しているが、θ_ｍ～θ_ｎではある方向に動くようなケースでは、関節が動き出すθ_ｍにおいて、段落［０００４］で述べた静止摩擦力が外乱となる問題が発生する。よって、このようなケースは、ゼロクロスと同様に扱うことが好ましい。

【0037】

次に、ゼロクロスが発生する場合でも、同時期にゼロクロスが発生する関節の数（同時発生軸数）が最小となる場合の探索手順について説明する。なお、ある第Ｊｍ軸の関節角度が任意の角度θ_Ｊｍ°で回転方向が反転し、また、ある第Ｊｎ軸の関節角度が任意の角度θ_Ｊｎ°で回転方向が反転する場合、θ_Ｊｍ±α°以内（αは０以上の十分小さな数値、例えばα＝０．１°とする）とθ_Ｊｎ±α°以内となる領域とが重なる軸数を同時発生軸数と定義する。また、すべての軸で角速度のゼロクロスが発生しない場合は、ゼロクロスの同時発生軸数は０となる。

【0038】

関節軌跡生成結果判定部２２においてゼロクロスの同時発生軸数の判定を行った結果、同時発生軸数が０となるアームアングルΨ_ｋ（０≦ｋ≦ｎ）の組み合わせが得られることが望ましい。よって、関節軌跡生成結果判定部２２は、同時発生軸数が最小となるアームアングルΨ_ｋ（０≦ｋ≦ｎ）の組み合わせが得られるまで、関節軌跡生成部においてアームアングルΨ_ｋ（ｋ＝０，ｉ，ｎ）の組み合わせを変えて、関節軌跡θ_ｋ（０≦ｋ≦ｎ）の生成を繰り返す。一方、あらかじめ設定していた所定の繰り返し回数に到達しても、同時発生軸数が０となる関節軌跡θ_ｋ（０≦ｋ≦ｎ）が得られない場合、関節軌跡生成結果判定部２２は、そのような組み合わせは存在しないと判定する。そして、関節軌跡生成結果判定部２２は、それまでに得られている、同時発生軸数が最小となる関節軌跡を、より速度偏差が小さくなることが期待できる関節軌跡として選択し、計算を終了する。

【0039】

関節軌跡の例として、θ_０～θ_ｍ１～θ_ｍ２～θ_ｎと関節角が変化する過程が図９及び図１０に示すように得られたとする。図９及び図１０に示す関節軌跡は、各地点ｋ（ｋ＝０，ｍ１，ｍ２，ｎ，０＜ｍ１＜ｍ２＜ｎ）における各軸（Ｊ１～Ｊ７）の各関節角θ_ｋ（ｋ＝０，ｍ１，ｍ２，ｎ）と、その増減を矢印で示した情報である。言い換えると、図９及び図１０に示す関節軌跡は、複数の関節を有するロボットアームＲ１の手先の予め設定された目標軌跡に基づいて、手先が目標軌跡に沿って移動する際の複数の関節の動作方向を示す情報である。単調減少及び増加しているＪ１，Ｊ３，Ｊ５はゼロクロスが発生しないが、Ｊ２，Ｊ４，Ｊ６，Ｊ７はゼロクロスが１回発生する。また図９では、ゼロクロスが発生するＪ２，Ｊ４，Ｊ６，Ｊ７の増減が反転する地点がすべて同時期であるｋ＝ｍ１であることから、同時期に発生する関節の軸数（同時発生軸数）が最大の４となる場合である。一方、図１０では、Ｊ４を除くＪ２，Ｊ６，Ｊ７の増減が反転する地点がｋ＝ｍ２となり、図９の場合よりも同時発生軸数が少なくなっている。なお、実施の形態１において、図９に示す関節軌跡が第１動作情報を構成し、図１０に示す関節軌跡が第２動作情報を構成する。

【0040】

関節軌跡生成結果判定部２２は、あらかじめ設定していた所定の繰り返し回数に到達しても、同時発生軸数が３未満となるアームアングルΨ_ｋ（ｋ＝０，ｍ１，ｍ２，ｎ）の組み合わせが得られない場合、同時発生軸数の最小値は３と判定し、計算を終了する。また、ゼロクロスの同時発生軸数が３未満となるアームアングルΨ_ｋ（ｋ＝０，ｍ１，ｍ２，ｎ）の組み合わせが得られた場合は、最小値を更新する。

【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】

外力情報取得部としての力制御演算部２５１は、力覚センサＳ１からロボットアームＲ１の手先に発生する力を示す情報を取得し、ロボットアームＲ１の手先に発生する力の現在値が力目標値に従うロボットアームＲ１の動きを算出する。力制御演算部２５１では、まず、力目標値と、力覚センサＳ１で検知された力現在値の偏差を、減算により算出する。この偏差は直交座標系で表された値であり、そのままでは各関節の制御に利用できないので、次に、関節座標系で表された値に変換する。ここで、実施の形態２ではロボットアームＲ１が冗長アームなので、関節座標系への変換が一意にならない。そこで、ヤコビ行列の擬似逆行列を利用する。ヤコビ行列の擬似逆行列Ｊ^＋を乗算することにより、関節座標系における偏差を一意に算出できる。それから、この変換結果を各関節に対応したＰＩ制御部２５１ａに入力して、関節ごとにＰＩ制御演算（比例制御演算と積分制御演算）を行う。なお、ＰＩ制御部２５１ａは、２つの制御演算のうち一方のみを行うように構成されていてもよい。比例制御演算だけであれば、ゲインを掛けることと等価となる。ＰＩ制御演算の結果が、力制御演算部２５１の出力である力制御量となる。力制御量は、要素数が関節数になるベクトルとなる。

【0067】

なお、ヤコビ行列はロボットアームＲ１の関節角によって変化するので、実施の形態２ではメイン制御部２０ＡがロボットアームＲ１の各関節の角度を必要とする。これらは例えば、メイン制御部２０Ａと関節制御部との間の通信経路を利用して、関節制御部からメイン制御部２０Ａへと定周期で通信によって送信するようにすればよい。

【0068】

合成部２５３は、力制御演算部２５１が算出した力制御量を、関節軌道生成部２３が生成した関節軌道と合成する。力制御量は各関節の角度の補正量となっている。一方、関節軌道は各関節の角度の時系列データである。合成部２５３は、各関節の角度の時系列データから現在時刻に対応するもの（要素数が関節数Ｎのベクトルになる）を取り出し、角度の補正量である力制御量を加算する。この合成演算結果は、送信部２４を通じて駆動制御部３１２へと送信される。これによって、ロボットアームＲ１の先端の手先力を制御しながら、手先の位置を軌道に沿って動かす、倣い動作を実現することができる。

【0069】

以上、力制御部２５のより具体的な例を一つ示したので、この例に基づいて、ロボットアームＲ１の位置姿勢に応じて重み設定を変化させる方法について説明する。ここでは、前述したヤコビ行列の疑似逆行列が、重み設定に深く関わることになる。

【0070】

前述した力制御演算部２５１内の偏差（力目標値と力現在値の差）をΔＦとする。ΔＦは直交座標系で表された値であり、Ｘ，Ｙ，Ｚの並進力の偏差、Ｘ軸，Ｙ軸，Ｚ軸回りの回転力の偏差からなる、６次元のベクトルである。力制御部２５はこの偏差を０にするような力制御量を算出するが、偏差が各関節にどのような割合で配分されて力制御量が算出されるかは、ヤコビ行列の疑似逆行列Ｊ^＋で決まることになる。

【0071】

偏差ΔＦにヤコビ行列の疑似逆行列Ｊ^＋を乗算して得られる値をΔτで表すと、両者は次式のような関係となる。

Δτ＝Ｊ^＋ΔＦ ・・・（３）

【0072】

ここで、Δτ_ｊ（１≦ｊ≦Ｎ）は、Δτの第ｊ軸に対応する成分であり、ΔＦ_１～ΔＦ_３は力の偏差の並進方向成分（順にＸ軸方向、Ｙ軸方向、Ｚ軸方向）、ΔＦ_４～ΔＦ_６は回転方向成分（順にＸ軸回転方向、Ｙ軸回転方向、Ｚ軸回転方向）である。例えば、手先をＸ軸方向に１で押し付ける場合を考える。この場合、力目標値は第１成分が１で他の成分は０、押し付ける前の力現在値は全ての成分で０になるので、ΔＦ＝（１，０，０，０，０，０）^Ｔとなる（右肩のＴはベクトル、行列の転置を表す）。この時、Δτは次式のようになる。

Δτ＝（ａ_１１，ａ_２１，・・・，ａ_Ｎ１）^Ｔ ・・・（５）

【0073】

これは、Ｘ軸方向に押し付けると、各軸の力制御量は、ヤコビ行列の疑似逆行列Ｊ^＋の成分であるａ_１１，ａ_２１，・・・，ａ_Ｎ１に比例することを意味する。言い換えれば、力制御における各関節の寄与が、ａ_１１，ａ_２１，・・・，ａ_Ｎ１に比例するとも言える。よって、重みｗ_ｎを、ｗ_ｎ＝｜ａ_ｎ１｜のように各関節の寄与の絶対値に設定することで、Ｘ軸方向に押し付ける際に寄与が大きい関節の重みを大きくすることができ、さらには、Ｘ軸方向に押し付ける際に寄与が大きい関節のゼロクロス発生回数や、ゼロクロスの同時発生軸数を、可能な範囲で少なくするような軌跡を選択することができる。すなわち、押付け力への寄与の高い軸を優先して、ゼロクロスの発生が分散される、もしくは発生しない関節軌跡を選択するといった効果が期待できる。

【0074】

これを一般化すると、押付け方向が任意の方向でも、同様に重みを設定して、同様な効果を得ることができる。押付け方向を表す、大きさ１の単位ベクトルをｅで表す。ｅは６行１列のベクトルである。この時、Δτは次式で計算できる。

【0075】

ここで計算されたΔτ_１，Δτ_２，・・・，Δτ_ｎが、押付け方向がｅとなる場合の各関節の寄与となるので、Δτ_１，Δτ_２，・・・，Δτ_ｎの絶対値を重みｗ_ｎとして設定すれば、寄与が高い関節のゼロクロスを抑制する効果が得られる。なお、寄与の絶対値をそのまま重みｗ_ｎとして設定する方法以外に、各関節の寄与が大きい関節から重みをＮ，Ｎ－１，・・・，１と順番に設定する方法もある。この方法でも同様な効果が得られる。

【0076】

例えば、ある目標軌道と、押付け力の目標値Ｆ_ｉを与えたときを考える。θ_０～θ_ｉｍ１～θ_ｊｍ２～θ_ｎと関節角が変化する過程が図１８及び図１９に示すように得られたとする。各地点ｋ（ｋ＝０，ｉｍ１，ｊｍ２，ｎ，０＜ｍ１＜ｍ２＜ｎ）における各軸（Ｊ１～Ｊ７）の各関節角θｋ（ｋ＝０，ｍ１，ｍ２ｉ，ｊ，ｎ）と、その増減を矢印で示している。ここで、押付け力Ｆｉに対する寄与の大きい軸を調べた結果、図１８に示す結果及び図１９に示す結果ともに第４軸が最も大きいとする。

【0077】

図１８及び図１９における各関節の角度の増減を見ると、単調減少及び増加しているＪ１，Ｊ３，Ｊ５はゼロクロスが発生しないが、Ｊ２，Ｊ４，Ｊ６，Ｊ７はゼロクロスが１回発生する。また図１８では、ゼロクロスが発生するＪ２，Ｊ４，Ｊ７の増減が反転する地点がｋ＝ｍ１で、Ｊ６の増減が反転する地点はｋ＝ｍ２であることから、同時発生軸数が最大の３となる場合である。一方、図１９では、Ｊ４を除くＪ２，Ｊ６，Ｊ７の増減が反転する地点がｋ＝ｍ２となり、図１８の場合と同時発生軸数は同じである。

【0078】

押付け力Ｆ_ｉに対する寄与の大きな第４軸において、ゼロクロスが発生するときは、押付け力の現在値と目標値との間には、力の偏差は大きくなると考えられる。このとき、第４軸以外の関節も同時期にゼロクロスが発生すると、力の偏差はより大きくなる懸念がある。そのため、第４軸は、同時期にゼロクロスが発生する軸数ができるだけ少ない方が、押付け力の精度に対する影響は小さくなると期待される。したがって、このような場合は、ゼロクロスに同時発生する際に第４軸が含まれていない図１９の関節軌跡を選択する。なお、実施の形態２において、図１８に示す関節軌跡が第１動作情報を構成し、図１９に示す関節軌跡が第２動作情報を構成する。

【0079】

（実施の形態２における力制御部の具体的な例２）
力制御において各関節で必要なトルクの寄与がロボットアームＲ１の手先の位置姿勢に応じて変化することを考慮して重みを設定する方法として、図１７の構成に沿って説明したが、この方法は図１７の構成に限られるものではない。以下では、実施の形態２における力制御部の、もう１つの具体的な例を図２０に示し、この例でも同様な考え方で重みを設定できることを説明する。

【0080】

図２０に示すように、力制御部２６には、力制御演算部２６１及び位置制御演算部２６２が備わっている。力制御演算部２６１は、図１９に示すＰＩ制御部２５１ａに代わって力制御ゲインが入っている。すなわち、力制御演算部２６１では、比例制御のみが行われる。また、力制御量は図１９に示す構成では各関節の角度の補正量だったが、図２０に示す構成例では各関節の速度指令値である。

【0081】

位置制御演算部２６２では、まず、位置目標値取得部２６２ａが、関節軌道生成部２３で生成された関節軌道から、位置目標値（各関節の角度の目標値）を取得する。関節軌道は始点から終点までの、各関節の角度の目標値からなる時系列データなので、ここから現在時刻に対応する位置目標値を取得する。次に、この位置目標値から、各関節の位置現在値を減算して、位置偏差を算出する。ここで、各関節の位置現在値は、エンコーダＥＮ１により検知され、関節制御部に送信されているので、関節制御部とメイン制御部２０Ａとの通信経路により、通信で取得すればよい。そして、算出された偏差に、位置制御ゲインを乗算して、位置制御量を得る。この位置制御量は、各関節の速度指令値である。なお、位置制御演算部２６２の演算は、全て関節座標系で行われているため、力制御演算部２６１のようにヤコビ行列の擬似逆行列により変数変換する必要はない。

【0082】

力制御演算部２６１で算出された力制御量と、位置制御演算部２６２で算出された位置制御量を合成部２６３で合成し、合成制御量とする。合成制御量をロボットアームＲ１の関節毎の角速度の指令値として、送信部２４が駆動制御部３１２に送信する。駆動制御部３１２内ではロボットアームＲ１の関節毎の角速度の現在値が合成制御量となるように速度制御を行う。なお、ここでは合成制御量が角速度の指令値とする例を説明したが、これに限るものではない。合成制御量をロボットアームＲ１の関節毎を駆動するための電流値の指令値として、駆動制御部３１２内ではロボットアームＲ１の関節毎の電流値の現在値が合成制御量となるように電流制御を行うことも可能である。

【0083】

以上のように、実施の形態２では、関節軌跡生成結果判定部２２の評価関数において、力制御で影響や動作が大きい関節のゼロクロス発生回数に大きな重みをかけて評価したり、力制御で影響や動作が大きい関節にゼロクロスが発生した場合に大きな重みをかけて同時発生軸数を評価したりすることで、力制御で影響や動作が大きい関節でゼロクロスが発生することをできるだけ抑制し、力制御性能をより向上させるという効果を得ることができる。

【0084】

なお、本開示は、各実施の形態の自由な組合せ、あるいは各実施の形態の任意の構成要素の変形、若しくは各実施の形態において任意の構成要素の省略が可能である。

【符号の説明】

【0085】

１，１Ａ ：ロボット制御システム
１０，１０Ａ：ロボット制御装置
２０，２０Ａ：メイン制御部（ロボット制御情報生成装置）
２１ ：関節軌跡生成部（動作情報取得部）
２２ ：関節軌跡生成結果判定部（選択部）
２３ ：関節軌道生成部
２４ ：送信部
２５，２６：力制御部
３１，３２，３ｎ：関節制御部
２５１ ：力制御演算部
２５１ａ ：ＰＩ制御部
２５３，２６３：合成部
２６１ ：力制御演算部
２６２ ：位置制御演算部
２６２ａ ：位置目標値取得部
３１１ ：受信部
３１２ ：駆動制御部
Ｅ１ ：エンドエフェクタ（手先部）
ＥＮ１ ：エンコーダ
Ｊ１，Ｊ２，Ｊ３，Ｊ４，Ｊ５，Ｊ６，Ｊ７，Ｊｎ：関節
Ｌ１，Ｌ２，Ｌ３，Ｌ４，Ｌ５，Ｌ６，Ｌ７：リンク
Ｍ１ ：モータ
Ｒ１ ：ロボットアーム
Ｓ１ ：力覚センサ

【図1】

【図2】

【図3】

【図4】

【図5】

【図6】

【図7】

【図8】

【図9】

【図10】

【図11】

【図12】

【図13】

【図14】

【図15】

【図16】

【図17】

【図18】

【図19】

【図20】