特開 2023-15848 ロボットコントローラ及びロボットの緊急停止方法

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

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

(11)【公開番号】P2023015848

(43)【公開日】2023-02-01

(54)【発明の名称】ロボットコントローラ及びロボットの緊急停止方法

(51)【国際特許分類】

   B25J 19/06 20060101AFI20230125BHJP

   B25J 9/10 20060101ALI20230125BHJP

【ＦＩ】

B25J19/06

B25J9/10 A

【審査請求】未請求

【請求項の数】10

【出願形態】ＯＬ

(21)【出願番号】P 2021119888

(22)【出願日】2021-07-20

(71)【出願人】

【識別番号】000002233

【氏名又は名称】日本電産サンキョー株式会社

(74)【代理人】

【識別番号】100123788

【弁理士】

【氏名又は名称】宮崎 昭夫

(74)【代理人】

【識別番号】100127454

【弁理士】

【氏名又は名称】緒方 雅昭

(72)【発明者】

【氏名】花岡 正志

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

3C707

【Ｆターム（参考）】

3C707BS15

3C707HS27

3C707KS17

3C707KS20

3C707KS21

3C707KX10

3C707LS15

3C707LU03

3C707MS14

(57)【要約】

【課題】複数の軸を有するロボットを緊急停止させるときに、手先の座標を所定の軌道に維持しつつロボットを速やかに停止させる。
【解決手段】ロボットは、所定の軌道に基づいて複数の軸に対する位置指令値を一括して算出する全軸制御部５０と、軸ごとに設けられて軸ごとの位置指令値に基づいてモータをサーボ制御するモータ駆動制御部４０と、を有するロボットコントローラによって制御される。緊急停止信号が入力されたときに、サーボ制御に用いる位置指令値を全軸制御部５０からの位置指令値からモータ駆動制御部４０内で算出される各軸停止用位置指令値に切り替え、その後、サーボ制御に用いる位置指令値を全軸制御部５０から位置指令値に戻す。全軸制御部５０は、緊急停止信号が入力されたときに、所定の軌道上でロボットを停止させるための緊急停止用位置指令値を出力するための演算を開始する。
【選択図】図９

【特許請求の範囲】

【請求項1】

複数の軸を備えて軸ごとに設けられたモータによって駆動されるロボットを制御するロボットコントローラであって、
前記ロボットの所定の軌道に基づいて前記複数の軸に対する位置指令値を一括して算出する全軸制御部と、
前記軸ごとに設けられて前記全軸制御部からの当該軸についての前記位置指令値に基づいて当該軸のモータをサーボ制御するモータ駆動制御部と、
を有し、
前記モータ駆動制御部は、対応するモータを停止させるための各軸停止用位置指令値をモータ座標系基準で算出する停止用位置指令演算部を備え、
緊急停止信号が入力されたときに、前記モータ駆動制御部は、サーボ制御に用いる位置指令値を前記全軸制御部からの前記位置指令値から前記各軸停止用位置指令値に切り替えて前記対応するモータをサーボ制御し、その後、前記サーボ制御に用いる位置指令値を前記各軸停止用位置指令値から前記全軸制御部からの前記位置指令値に戻して前記対応するモータのサーボ制御を続行し、
前記全軸制御部は、前記緊急停止信号が入力されたときに、前記所定の軌道上で前記ロボットを停止させるための緊急停止用位置指令値を出力するための演算を開始するロボットコントローラ。

【請求項2】

前記モータ駆動制御部は、前記サーボ制御に用いる位置指令値を前記各軸停止用位置指令値から前記全軸制御部からの前記位置指令値に戻す移行期において、按分比を変えながら前記各軸停止用位置指令値と前記全軸制御部からの前記位置指令値とを按分した位置指令値を算出して前記サーボ制御に用いる、請求項１に記載のロボットコントローラ。

【請求項3】

前記全軸制御部は、
前記各軸停止用位置指令値によって各軸の前記モータがサーボ制御されたことによる前記ロボットの動きを推定する推定値演算部と、
前記推定値演算部での推定の結果に基づいて前記ロボットを前記所定の軌道に戻す修正動作用の位置指令値を算出する位置指令値演算部と、
を有し、
前記位置指令値演算部において、前記修正動作用の位置指令値に引き続いて前記緊急停止用位置指令値が算出される、請求項１に記載のロボットコントローラ。

【請求項4】

前記全軸制御部が算出する位置指令切替指令に基づいて、前記モータ駆動制御部は、前記サーボ制御に用いる位置指令値を前記各軸停止用位置指令値から前記全軸制御部からの前記位置指令値に戻す、請求項１乃至３のいずれか１項に記載のロボットコントローラ。

【請求項5】

前記各軸停止用位置指令値は、前記モータごとに設定された加速度または減速時間によって当該モータを減速停止させる位置指令値である、請求項１乃至４のいずれか１項に記載のロボットコントローラ。

【請求項6】

複数の軸を備え所定の軌道に沿って動くように全軸制御部によって前記複数の軸に対して一括して算出された位置指令値に基づいて軸ごとのモータがサーボ制御されるロボットの緊急停止方法であって、
緊急停止信号が入力されたときに、前記軸ごとに、サーボ制御に用いる位置指令値を前記全軸制御部からの前記位置指令値から、対応するモータを停止させるためにモータ座標系基準で算出される各軸停止用位置指令値に切り替え、その後、前記サーボ制御に用いる位置指令値を前記各軸停止用位置指令値から前記全軸制御部からの前記位置指令値に戻し、
前記緊急停止信号が入力されたときに、前記全軸制御部において、前記所定の軌道上で前記ロボットを停止させるための緊急停止用位置指令値を出力するための演算を開始する、緊急停止方法。

【請求項7】

前記サーボ制御に用いる位置指令値を前記各軸停止用位置指令値から前記全軸制御部からの前記位置指令値に戻す移行期において、按分比を変えながら前記各軸停止用位置指令値と前記全軸制御部からの前記位置指令値とを按分した位置指令値を算出して前記サーボ制御に用いる、請求項６に記載の緊急停止方法。

【請求項8】

前記緊急停止信号が入力されたときに、前記全軸制御部において、前記各軸停止用位置指令値によって各軸の前記モータがサーボ制御されたことによる前記ロボットの動きを推定し、推定された前記ロボットの動きに基づいて前記ロボットを前記所定の軌道に戻す修正動作用の位置指令値を算出する、請求項６に記載の緊急停止方法。

【請求項9】

前記全軸制御部が算出する位置指令切替指令に基づいて、前記サーボ制御に用いる位置指令値を前記各軸停止用位置指令値から前記全軸制御部からの前記位置指令値に戻す、請求項６乃至８のいずれか１項に記載の緊急停止方法。

【請求項10】

前記各軸停止用位置指令値は、前記モータごとに設定された加速度または減速時間によって当該モータを減速停止させる位置指令値である、請求項６乃至９のいずれか１項に記載の緊急停止方法。

【発明の詳細な説明】

【技術分野】

【0001】

本発明は、複数の軸を有するロボットの制御に関し、特に、手先の座標を所定の軌道に維持したままロボットを緊急停止させることができるロボットコントローラと、ロボットの緊急停止方法とに関する。

【背景技術】

【0002】

複数の軸を有し、軸ごとにモータが設けられているロボットを制御するロボットコントローラは、モータごとに設けられてそのモータに対する位置指令値に基づいてサーボ制御を行うモータ駆動制御部を備えている。指定された軌道に沿ってロボットを動作させるためにはロボットが備える複数の軸を同時に動かす必要があるから、ロボットコントローラは、ロボットの各軸に対する位置指令値を一括して計算して出力する全軸制御部を備え、全軸制御部で算出した各軸の位置指令値は、対応するモータ駆動制御部に伝達される。また、ロボットではその動作中に異常を検出したり外部から緊急停止の指令が入力されたりしたときに、ロボットを緊急停止させる必要がある。緊急停止を行う場合、所定の時間内にすべての軸のモータの速度を０にする必要がある。そのため、ロボットコントローラは、緊急停止を指示する緊急停止信号が入力される緊急停止信号入力部を備える。ロボットを緊急停止させる場合、緊急停止後のロボットの位置及び姿勢は、ロボットの通常時の軌道上にあることが好ましい。通常時の軌道上以外の場所で停止することすなわちロボットの軌道が目標値に維持されないことは、ロボットが想定外の位置に移動することであるから安全上の問題を発生させるおそれがあり、また、緊急停止からの復帰するときのロボットの制御を難しくする。

【0003】

緊急停止信号が入力されてロボットを緊急停止させるときのロボットコントローラにおける制御方法としては、緊急停止信号を全軸制御部に供給し、全軸制御部が、緊急停止信号を受け付けたときの手先の参照位置に基づいて手先の動作計画及び逆運動学演算を行って、なるべく通常の軌道上でロボットが停止するように各軸の位置指令値を出力する方法がある。例えば特許文献１には、緊急停止を行うときに、全軸制御部が、ハンドの向きを保ちながら、緊急停止信号が入力されたときのハンドの移動方向へハンドが直線的に移動するような動作を計画して各軸のモータを制御することが開示されている。ロボットを緊急停止させるときのロボットコントローラにおける制御方法として、緊急停止信号を各モータ駆動制御部に供給し、各モータ駆動制御部が、モータごとに定められている緊急停止用の加速度で各モータを減速停止させる方法もある。

【先行技術文献】

【特許文献】

【0004】

【特許文献1】特開２０１４－３４１０８号公報

【発明の概要】

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

【0005】

ロボットコントローラに緊急停止信号が入力されときに、手先の座標、向きを目標軌道上に維持しながらロボットを減速停止さえる場合には、逆運動学演算や動作計画のための処理時間が必要となり、全軸制御部とモータ駆動制御部との間での位置指令値の送信間隔に起因する遅延も発生するので、異常が検出されて緊急停止信号が発報されてからの速やかなロボットの停止を達成することが難しい。一方、緊急停止信号をモータ駆動制御部に入力して各モータを減速停止させる場合には、各軸のモータ座標系基準でモータ駆動制御部内部で算出した速度指令値によってモータが制御されるので、軸ごとに独立した停止挙動となり、ロボットが通常の軌道上で停止することが保証されない。

【0006】

本発明の目的は、ロボットを緊急停止させるときに、手先の座標、向きを目標軌道上に維持しつつロボットを速やかに停止することができるロボットコントローラと、そのような緊急停止方法とを提供することにある。

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

【0007】

本発明のロボットコントローラは、複数の軸を備えて軸ごとに設けられたモータによって駆動されるロボットを制御するロボットコントローラであって、ロボットの所定の軌道に基づいて複数の軸に対する位置指令値を一括して算出する全軸制御部と、軸ごとに設けられて全軸制御部からのその軸についての位置指令値に基づいてその軸のモータをサーボ制御するモータ駆動制御部と、を有し、モータ駆動制御部は、対応するモータを停止させるための各軸停止用位置指令値をモータ座標系基準で算出する停止用位置指令演算部を備え、緊急停止信号が入力されたときに、モータ駆動制御部は、サーボ制御に用いる位置指令値を全軸制御部からの位置指令値から各軸停止用位置指令値に切り替えて対応するモータをサーボ制御し、その後、サーボ制御に用いる位置指令値を各軸停止用位置指令値から全軸制御部からの位置指令値に戻して対応するモータのサーボ制御を続行し、全軸制御部は、緊急停止信号が入力されたときに、所定の軌道上でロボットを停止させるための緊急停止用位置指令値を出力するための演算を開始する。

【0008】

ロボットの緊急停止の際に、所定の軌道上で、すなわち手先の座標、向きを目標軌道上に維持したままロボットを停止させるための緊急停止用位置指令値は、全軸制御部において緊急停止指令が入力されてから算出され、モータ駆動制御部に伝達される。そのため、緊急停止用位置指令値の出力が開始されるまでの演算処理の時間や全軸制御部とモータ駆動制御部との間での位置指令値の伝送に要する時間は、ロボットを停止させることに対する遅れ時間として作用する。本発明のロボットコントローラでは、緊急停止信号が入力されたときに、モータ駆動制御部において、サーボ制御に用いる位置指令値を全軸制御部からの位置指令値から各軸停止用位置指令値に切り替えて、対応するモータをサーボ制御し、その後、サーボ制御に用いる位置指令値を各軸停止用位置指令値から全軸制御部からの位置指令値に戻して、対応するモータのサーボ制御を続行するので、ロボットの停止までの遅れ時間を短縮することができ、手先の座標、向きを目標軌道上に維持しつつロボットを速やかに停止することができる。

【0009】

本発明のロボットコントローラでは、モータ駆動制御部において、サーボ制御に用いる位置指令値を各軸停止用位置指令値から全軸制御部からの位置指令値に戻す移行期において、按分比を変えながら各軸停止用位置指令値と全軸制御部からの位置指令値とを按分した位置指令値を算出してサーボ制御に用いてもよい。このような制御を行なうことで、ロボットにおける振動や衝撃の発生を抑えつつ、少ない演算量で、サーボ制御に用いる位置指令値を全軸制御部からの位置指令値に戻すことができる。

【0010】

本発明のロボットコントローラでは、各軸停止用位置指令値によって各軸のモータがサーボ制御されたことによるロボットの動きを推定する推定値演算部と、推定値演算部での推定の結果に基づいてロボットを所定の軌道すなわち目標軌道に戻す修正動作用の位置指令値を算出する位置指令値演算部と、を全軸制御部に設け、位置指令値演算部において、修正動作用の位置指令値に引き続いて緊急停止用位置指令値が算出されるようにしてもよい。このように構成すれば、より滑らかにサーボ制御に用いる位置指令値を全軸制御部からの位置指令値に戻すことができる。

【0011】

本発明のロボットコントローラでは、全軸制御部が算出する位置指令切替指令に基づいて、サーボ制御に用いる位置指令値を各軸停止用位置指令値から全軸制御部からの位置指令値に戻すことができる。このように構成すれば、全軸制御部が修正動作用の位置指令値あるいは緊急停止用位置指令値が出力できるようになったタイミングに合わせてサーボ制御に用いる位置指令値を全軸制御部からの位置指令値に戻すことができるようになるので、ロボットをより迅速に目標軌道上に戻すことが可能になる。

【0012】

本発明のロボットコントローラでは、各軸停止用位置指令値は、例えば、モータごとに設定された加速度または減速時間によってそのモータを減速停止させる位置指令値である。このような各軸停止用位置指令値を用いることにより、例えば各軸停止用位置指令値によって各軸のモータがサーボ制御されたことによるロボットの動きを推定するときに、推定を容易に行えるようになる。

【0013】

本発明の緊急停止方法は、複数の軸を備え所定の軌道に沿って動くように全軸制御部によって複数の軸に対して一括して算出された位置指令値に基づいて軸ごとのモータがサーボ制御されるロボットの緊急停止方法であって、緊急停止信号が入力されたときに、軸ごとに、サーボ制御に用いる位置指令値を全軸制御部からの位置指令値から、対応するモータを停止させるためにモータ座標系基準で算出される各軸停止用位置指令値に切り替え、その後、サーボ制御に用いる位置指令値を各軸停止用位置指令値から全軸制御部からの位置指令値に戻し、緊急停止信号が入力されたときに、全軸制御部において、所定の軌道すなわち目標軌道上で前記ロボットを停止させるための緊急停止用位置指令値を出力するための演算を開始する。

【0014】

本発明の緊急停止方法では、緊急停止信号が入力されたときに、軸ごとに、サーボ制御に用いる位置指令値を全軸制御部からの位置指令値から各軸停止用位置指令値に切り替え、その後、サーボ制御に用いる位置指令値を各軸停止用位置指令値から全軸制御部からの位置指令値に戻すので、緊急停止信号の入力から緊急停止用位置指令値の出力開始までの遅れ時間や、全軸制御部からの位置指令値の送信間隔による遅れ時間の影響を低減して、より速やかに目標軌道上でロボットを停止させることができるようになる。

【0015】

本発明の緊急停止方法では、サーボ制御に用いる位置指令値を各軸停止用位置指令値から全軸制御部からの位置指令値に戻す移行期において、按分比を変えながら各軸停止用位置指令値と全軸制御部からの位置指令値とを按分した位置指令値を算出してサーボ制御に用いるようにしてもよい。このような制御を行なうことで、ロボットにおける振動や衝撃の発生を抑えつつ、少ない演算量で、サーボ制御に用いる位置指令値を全軸制御部からの位置指令値に戻すことができる。

【0016】

本発明の緊急停止方法では、緊急停止信号が入力されたときに、全軸制御部において、各軸停止用位置指令値によって各軸のモータがサーボ制御されたことによるロボットの動きを推定し、推定されたロボットの動きに基づいてロボットを所定の軌道すなわち目標軌道に戻す修正動作用の位置指令値を算出するようにしてもよい。このように算出された修正動作用の位置指令値を用いることで、より滑らかにサーボ制御に用いる位置指令値を全軸制御部からの位置指令値に戻すことができる。

【0017】

本発明の緊急停止方法では、全軸制御部が算出する位置指令切替指令に基づいて、サーボ制御に用いる位置指令値を各軸停止用位置指令値から全軸制御部からの位置指令値に戻してもよい。このような位置指令切替指令を用いることより、全軸制御部が修正動作用の位置指令値あるいは緊急停止用位置指令値が出力できるようになったタイミングに合わせてサーボ制御に用いる位置指令値を全軸制御部からの位置指令値に戻すことができるようになるので、ロボットをより迅速に目標軌道上に戻すことが可能になる。

【0018】

本発明の緊急停止方法において、各軸停止用位置指令値は、例えば、モータごとに設定された加速度または減速時間によってそのモータを減速停止させる位置指令値である。このような各軸停止用位置指令値を用いることにより、例えば各軸停止用位置指令値によって各軸のモータがサーボ制御されたことによるロボットの動きを推定するときに、推定を容易に行えるようになる。

【発明の効果】

【0019】

本発明によれば、ロボットの緊急停止の際に、手先の座標、向きを目標軌道上に維持しつつロボットを速やかに停止することができる。

【図面の簡単な説明】

【0020】

【図1】多関節ロボットの構成の一例を示す図である。

【図2】図１に示すロボットの動きを説明する平面図である。

【図3】ロボット各部の位置、角度、速度及び角速度を説明する図である。

【図4】ロボットコントローラの構成の一例を示すブロック図である。

【図5】全軸制御部の構成を示すブロック図である。

【図6】モータ駆動制御部の構成を示すブロック図である。

【図7】緊急停止時のロボット各部の角度及び角速度を説明する図である。

【図8】緊急停止時のロボットの軌道を説明する図である。

【図9】本発明の第１の実施形態の緊急停止方法を説明するタイミング図である。

【図10】モータ駆動制御部の構成を示すブロック図である。

【図11】第２の実施形態の緊急停止方法を説明するタイミング図である。

【図12】緊急停止に伴う手先の位置と姿勢の変化を説明する図である。

【図13】緊急停止時のロボット各部の位置、角度、速度及び角速度を説明する図である。

【図14】全軸制御部の構成を示すブロック図である。

【図15】モータ駆動制御部の構成を示すブロック図である。

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

【0021】

次に、本発明を実施するための形態について説明する。本発明に基づくロボットコントローラは、複数の軸を有するロボットの制御に用いられるものであるが、以下では、図１に示すように３つの関節を有する水平多関節ロボットがロボットコントローラの制御対象であるものとする。もちろん、図１に示すロボット以外のロボットを制御するロボットコントローラにも本発明は適用可能である。

【0022】

図１において、（ａ），（ｂ）はそれぞれ水平多関節ロボットの上面図と正面図である。図１に示す水平多関節ロボットは、基台１０に対して関節１を介してリンク１１の一端が接続し、リンク１１の他端に対して関節２を介してリンク１２の一端が接続し、リンク１２の他端に対して関節３を介して手先１３の一端が接続している。手先１３の他端がこのロボットの手先位置である。関節１～３の軸はいずれも垂直方向を向いている。関節１にはモータ２１とモータ２１に接続する減速機３１とが設けられ、モータ２１を駆動することでリンク１１が水平面内を移動する。同様に関節２にはモータ２２と減速機３２とが設けられ、モータ２２を駆動することでリンク１２が水平面内を移動し、関節３にはモータ２３と減速機３３とが設けられ、モータ２３を駆動することで手先１３が水平面内を移動する。図１（ａ）に示すように、関節１の軸位置を中心として水平面内でＸＹの直交座標を定義する。このロボットは、手先１３の長手方向がＹ軸と平行な状態を保ったまま、図示直線Ｌ上を手先１３がＹ方向（進行方向ともいう）に沿って往復移動するように駆動制御される。直線Ｌは、Ｙ方向に延びる直線であって、ロボットの手先位置の目標軌道を示す直線である。直線Ｌは、関節１の軸からＸ方向（横方向ともいう）に所定の距離だけオフセットした位置にある。

【0023】

図２は、図１に示すロボットを通常時の軌道で動作させたときに、リンク１１，１２及び手先１３の位置及び姿勢がどのように変化するかを示している。ロボットは、図２に示すように、リンク１１，１２及び手先１３がＺ字状に折り畳まれた状態と伸びた状態との間で動作する。図において（ａ）で示す折り畳まれた状態では、関節３は関節１からＸ方向に延びる直線上にある。この状態から延びた状態に移行するときは、リンク１１が関節１の軸周りを反時計回りに回転し、手先１３の長手方向とリンク１２の長手方向とが直交する状態（図示（ｂ））に至る。その後、今度はリンク１１が時計回りに回転し、図において（ｃ）に示す延びた状態となる。手先１３は。その長手方向の中心線が常に直線Ｌ上にあるようにＹ方向を運動する。

【0024】

図３は、図２（ａ）に示す状態から図２（ｃ）に示す状態にロボットが動くときの、ロボット各部の位置、角度、速度及び角速度の変化を説明するグラフであり、図２（ａ）に示す状態にあるロボットが時刻Ａから動き出し、時刻Ｃにおいて図２（ｃ）に示す状態に達し、その後、静止する場合を示している。図３において（ａ）は手先１３の先端の位置のＹ座標値を示し、（ｂ）は関節１の関節角度を示し、（ｃ）は関節２の関節角度を示し、（ｄ）は関節３の関節角度を示している。さらに図３において、（ｅ）は手先１３のＹ方向に沿った速度を示し、（ｆ）は関節１の関節角速度を示し、（ｇ）は関節２の関節角速度を示し、（ｈ）は関節３の関節角速度を示している。

【0025】

図４は、図１に示す水平多関節ロボットを制御するロボットコントローラ１５の構成を示すブロック図である。ロボットにおいてリンク１１，１２及び手先１３からなる部分をマニピュレータ１４と総称し、このマニピュレータ１４が、モータ２１～２３からそれぞれ減速機３１～３３を介して駆動される関節１～３によって動かされることが示されている。ロボットコントローラ１５は、モータ２１～２３ごとに設けられてモータごとの位置指令値により対応するモータをサーボ制御するモータ駆動制御部４０と、モータ２１～２３に対する位置指令値を一括して算出する全軸制御部５０と、ロボットを緊急停止するための緊急停止信号が外部から入力される緊急停止信号入力部９０と、を備えている。モータ２１～２３はいずれもエンコーダ付きのものであり、エンコーダで検出されたモータ位置は、サーボ制御のためにモータ駆動制御部４０にフィードバックされている。緊急停止信号入力部９０で受け付けた緊急停止信号は各モータ駆動制御部４０と全軸制御部５０に送られる。また、全軸制御部５０側でモータ駆動制御部４０の状態を知るために、各モータ駆動制御部４０からそのモータ駆動制御部４０での緊急停止状態を示す信号が全軸制御部５０に送られる。

【0026】

図５は、全軸制御部５０の構成を示すブロック図である。全軸制御部５０は、手先位置指令を算出する手先位置指令算出部５１と、手先位置指令に基づいて逆運動学演算を行い、各軸の関節角度によって表される位置指令（関節角度位置指令）を算出する逆運動学演算部５２と、各軸の関節角度で表される位置指令に基づいて逆速比演算を行い、モータ２１～２３ごとの位置指令値を算出する逆速比演算部４３と、を備えている。

【0027】

次に、図１に示すロボットを緊急停止する方法について説明する。本発明に基づく緊急停止方法を説明する前に、従来の緊急停止方法について説明する。上述したように従来の緊急停止方法には、逆運動学演算により手先の座標、向きを目標軌道上に維持しながら減速停止させる方法と、各軸のモータ座標系基準でモータ駆動制御部内部で指令値を発生してモータを減速停止させる方法とがある。手先を目標軌道に維持しながら減速停止させる方法では、ロボットが通常動作しており手先１３が直線Ｌ（図２参照）に沿って移動しているときに緊急停止信号が入力されると、全軸制御部５０は、通常動作モードから緊急減速停止モードに切り替わり、目標軌道に沿ってロボットを移動させながらロボットを停止させるためのモータ２１～２３に対する位置指令値を逆運動学演算によって算出する。算出された位置指令値は各モータ駆動制御部４０に送られる。この場合、逆運動学演算などに要する処理時間や全軸制御部５０と各モータ駆動制御部４０との間での通信の送信間隔などのために、全軸制御部５０から各モータ駆動制御部４０に送信される位置指令値が通常動作用のものから緊急停止用のものに切り替わるのに、多少の時間を要する。例えば、全軸制御部５０に緊急停止信号が受け付けてから各モータ２１～２３での減速動作が開始するまでに数ミリ秒程度の遅延が発生する。

【0028】

一方、各軸のモータ座標系基準でモータ駆動制御部内部で指令値を算出してモータを減速停止させる場合には、各モータ駆動制御部４０は緊急停止信号を受信できるように構成される。図６はこの場合のモータ駆動制御部４０の構成を示しており、ここではモータ駆動制御部４０の内部において独自に速度指令値を発生するものとしている。上述のようにモータ駆動制御部４０には、モータに付属するエンコーダからモータ位置がフィードバックされている。モータ駆動制御部４０は、位置指令値とモータ位置とに基づいて速度指令値を算出する位置制御部４１と、モータ位置からモータ速度を算出する速度算出部４２と、速度指令値と速度算出部４２から入力するモータ速度とに基づいてモータに対するトルク指令値を算出する速度制御部４３と、緊急停止信号が入力されたときに位置制御部４１から入力する速度指令値に基づいてモータを減速停止させるためのブレーキ用の速度指令値を算出する停止用速度指令演算部４４と、緊急停止信号の有無に応じ、位置制御部４１が出力する速度指令値と停止用速度指令演算部４４が算出するブレーキ用の速度指令値とを切り替えて速度制御部４３に入力させるセレクタ４５と、を備えている。速度制御部４３が出力するトルク指令値は電流に換算され、換算により得られた電流でモータが駆動される。

【0029】

図７は、図６に示すモータ駆動制御部４０を使用して緊急停止動作を行ったときのロボット各部の角度及び角速度の変化を説明するグラフである。ここでは、図３に示す通常動作でのロボットの動きの途中で緊急停止信号が入力されて緊急停止動作が開始したものとしており、破線は、図３に示す通常動作での関節角度及び関節角速度を示している。図７において（ａ）は関節１の関節角度を示し、（ｂ）は関節２の関節角度を示し、（ｃ）は関節３の関節角度を示し、（ｄ）は関節１の関節角速度を示し、（ｅ）は関節２の関節角速度を示し、（ｆ）は関節３の関節角速度を示している。（ｄ）～（ｆ）に示す各関節角速度のグラフから明らかなように、緊急停止動作が開始すると、各関節すなわち各モータは、モータごとに設定された加速度で減速し、停止する。それに伴い（ａ）～（ｃ）のグラフに示すように、各関節の角度が変化する。このとき各関節の角度は、ロボットがその通常の軌道（図２に示す直線Ｌ）を維持するための角度からずれる。図８は通常の軌道からのこのずれを説明する図である。図８（ａ）は、図２（ａ）に示す状態と同じ状態を示している。この状態から図２（ｃ）に示す状態にロボットを動作させているときにロボットを緊急停止させると、例えば図８（ｂ）に示す状態でロボットは停止する。図８（ｂ）に示す停止状態では、ロボットの手先１３の位置は本来の軌道を示す直線Ｌから大きくずれ、手先１３の長手方向の向きも直線Ｌの延びる方向から大きくずれている。

【0030】

［第１の実施形態］
上述したように従来の緊急停止方法では、モータが減速を開始するまでに遅延が生じる、あるいは手先の座標、向きを目標軌道上に維持できない、という課題がある。そこで本発明の第１の実施形態の緊急停止方法では、上述した２つの緊急停止方法、すなわち逆運動学演算により手先を減速停止させる方法と各軸のモータ座標系基準でモータ駆動制御部内部で指令値を発生する方法とを組み合わせ、緊急停止信号を受信したからモータの減速が始まるまでの時間を短縮しつつ、手先の座標、向きを目標軌道上に維持したままロボットを停止させる。具体的には、各モータ駆動制御部４０において緊急停止のための位置指令値（各軸停止用位置指令値）を算出できるようにして、緊急停止信号が入力されたときに、各モータ駆動制御部４０においてモータのサーボ制御のために用いられる位置指令値を全軸制御部５０からの位置指令値から各軸停止用位置指令値に切り替え、これによりモータを減速させる。これと並行して緊急停止信号の入力とともに全軸制御部５０において逆運動学演算により手先を減速停止させるための演算を開始し、所定の時間が経過したら各モータ制御駆動部４０において各軸停止用位置指令値によるサーボ制御から逆運動学演算により手先を減速停止させるサーボ制御に徐々に切り替える。そして最終的には、逆運動学演算での手先の減速停止のための位置指令値のみによる制御によってロボットを停止させる。各軸停止用位置指令値によるサーボ制御から逆運動学演算で算出した手先を減速させる位置指令値によるサーボ制御への切替の開始は、位置指令切替指令によって与えられる。図９は、このような第１の実施形態における緊急停止動作を説明する。

【0031】

図９に示すタイミング図において、初期状態ではロボットは通常動作を行っていて手先１３も所定の速度で移動しており、また緊急停止信号と位置指令切替指令はいずれもオフ（“０”）の状態にある。全軸制御部５０から各モータ駆動制御部４０に送られる位置指令は通常動作用のものであり、モータ駆動制御部４０の内部では全軸制御部５０からの位置指令値に基づいてモータのサーボ制御を行なっている。ここで時刻Ｐにおいて緊急停止信号がオフからオン（“１”）に切り替わったとする。この時点では位置指令切替指令は依然としてオフのままであり、全軸制御部５０から各モータ駆動制御部４０に送られる位置指令は通常動作用のものである。全軸制御部５０は、緊急停止信号が入力されても通常動作用の位置指令値を出力し続けるが、手先を減速させるための動作計画を開始する。

【0032】

一方、モータ駆動制御部４０は、時刻Ｐにおいて緊急停止信号が入力されると、モータのサーボ制御に用いる位置指令値を全軸制御部５０からのものから各軸停止用位置指令値に切り替える。これによりモータは減速を開始し、手先位置の速度の低下も始まる。各軸停止用位置指令値は、例えばモータごとに設定されている加速度あるいは減速時間によってそのモータを減速停止させるために、モータごとにそのモータについてモータ座標系基準により算出される位置指令値である。その後、時刻Ｑにおいて、位置指令切替指令がオフからオンに切り替わる。すると全軸制御部５０は、通常動作用の位置指令値の出力を停止して緊急停止用のすなわち目標軌道を維持したまま手先を減速停止させるための位置指令値の出力を開始する。この時点でモータ駆動制御部４０がサーボ制御に用いる位置指令値を全軸制御部５０からの緊急停止用の位置指令値に切り替えてもよいが、そうすると、ロボットが滑らかに動作できずに、過大なトルク、振動、衝撃などが発生する恐れがある。そこで本実施形態では、時刻Ｑから時刻Ｒまでの期間を移行期とし、移行期内で徐々に位置指令値を切り替えるようにする。例えば、モータ駆動制御部４０の内部においてサーボ制御に用いる位置指令値をＰｃとし、全軸制御部５０から出力される緊急停止用の位置指令値をＰｃｉｋとし、モータ駆動制御部４０の内部で発生する各軸停止用位置指令値をＰｃｍとし、時刻Ｑからの経過時間をｔ（ただしｔ≧０）として、
Ｐｃ＝ｒ（ｔ）×Ｐｃｉｋ＋｛１－ｒ（ｔ）｝×Ｐｃｍ （１）
とすることができる。ここでｒ（ｔ）は按分率であって、移行期の長さをＴとすれば、
ｔ≦Ｔであれば
ｒ（ｔ）＝ａ×ｔ
であり、ｔ＞Ｔであれば
ｒ（ｔ）＝１
である。ａは按分率の時間変化率を示すパラメータであり、ａ＝１／Ｔで表される。移行期においても手先位置の速度は低下を続けている。移行期の長さＴは、ロボットの構成などに応じて定められる。

【0033】

時刻Ｒが到来し移行期が終了すれば、モータ制御駆動部４０の内部においてサーボ制御に用いられる位置指令値は、全軸制御部５０からの緊急停止用位置指令値に完全に切り替わり、モータは減速を続け、手先位置の速度も０に向けて低下する。図９には手先位置の速度も示されている。ここで破線で示す手先位置の速度は、逆運動学演算による手先の減速停止のみを実行したときの手先位置の速度を示している。逆運動学演算による手先の減速停止のみの場合、モータが減速を開始するのは、全軸制御部５０での逆運動学の演算が終了してからであり、それは時刻Ｑの直前であると考えられるから、ここで説明した本実施形態の緊急停止方法によれば、手先の軌道を目標値に維持した減速停止のみによって緊急停止を行う場合に比べ、モータの減速の開始が時刻Ｑから時刻Ｐへと早まり、その分、ロボットの手先が完全に停止するまでの時間も短縮される。

【0034】

本実施形態の緊急停止方法を実施するために用いられるロボットコントローラは、基本構成としては図４を用いて説明したロボットコントローラ１５と同じであるが、モータ駆動制御部４０の内部構成が異なっている。全軸制御部５０としては図５に示したものを使用することができる。ただし全軸制御部５０には、各モータ駆動制御部４０から、緊急停止状態を示す信号のほかに、モータ駆動制御部４０の内部でサーボ制御に用いる位置指令値が全軸制御部５０のからのものであるか各軸停止用位置指令値であるかを示す信号も入力する。

【0035】

図１０は、第１の本実施形態で用いるモータ駆動制御部４０の構成を示している。モータ駆動制御部４０は、位置指令値とモータ位置とに基づいて速度指令値を算出する位置制御部４１と、モータ位置からモータ速度を算出する速度算出部４２と、速度指令値と速度算出部４２から入力するモータ速度とに基づいてモータに対するトルク指令値を算出するする速度制御部４３と、緊急停止信号が入力されるとともに全軸制御部５０からの位置指令値に基づいて各軸停止用位置指令値を算出する停止用位置指令演算部４６と、指令切替移行演算部４７と、緊急停止信号に基づき、全軸制御部５０からの位置指令値と指令切替移行演算部４７での演算で得た位置指令値とを切り替えて位置制御部４１に入力させるセレクタ４８と、を備えている。指令切替移行演算部４７は、位置指令切替指令が入力される前は、停止用位置指令演算部４６が出力する各軸停止用位置指令値をセレクタ４８に出力し、位置指令切替指令が入力されたら上記（１）式にしたがって演算を行った結果を出力する。セレクタ４８は、緊急停止信号が入力されていないときは全軸制御部５０からの位置指令値を選択し、緊急停止信号が入力されているときは指令位置移行演算部４７が出力する位置指令値を選択する。その結果、モータ駆動制御部４０は、緊急停止信号が入力されるまでは全軸制御部５０からの位置指令値を用いてモータのサーボ制御を行ない、緊急停止信号が入力されて位置指令切替指令が入力されるまでは各軸停止用位置指令値を用いてモータのサーボ制御を行なう。さらに、位置指令切替指令が入力された後は、モータ駆動制御部４０は、（１）式に基づく移行期のサーボ制御を行なう。特に、移行期が終了した後は、指令切替移行演算部４７が全軸制御部５０からの位置指令値をそのまま出力するので、モータ駆動制御部４０は、全軸制御部５０からの緊急停止用の位置指令値を用いてモータのサーボ制御を行なう。

【0036】

本実施形態において位置指令切替指令は、全軸制御部５０において緊急停止用の位置指令値の出力が可能になった時点で全軸制御部５０が出力してもよいし、あるいは、緊急停止信号の入力から所定の時間が経過した時点で緊急停止信号入力部９０が出力してもよい。さらには、モータ駆動制御部４０の外部で位置指令切替指令を算出するのではなく、緊急停止信号が入力されてから所定の時間の経過後にモータ駆動制御部４０の内部で位置指令切替指令を算出してもよい。

【0037】

以上説明した本実施形態の緊急停止方法によれば、簡単な演算で、緊急停止信号を受信したからモータの減速が始まるまでの時間を短縮しつつ、手先の座標、向きを目標軌道上に維持したままロボットを停止させることが可能になる。

【0038】

［第２の実施形態］
次に、本発明の第２の実施形態の緊急停止方法について説明する。上述した第１の実施形態では移行期を設け、移行期において全軸制御部５０からの位置指令値と停止用内部位置指令とを時間経過に応じて按分することよって、各軸停止用位置指令値から全軸制御部５０からの緊急停止用の位置指令値への切り替えを滑らかに行うようにしている。しかしながら、モータ駆動制御部４０において按分演算を行うよりも全軸制御部５０側で修正演算を行うことによって、各軸停止用位置指令値から全軸制御部５０からの緊急停止用位置指令値へのより滑らかな切り替えを実現することができる。第２の実施形態では、全軸演算部５０側で修正演算を行う場合を説明する。図１１は第２の実施形態の緊急停止方法を説明するタイミング図である。図１１に示すタイミング図において、時刻Ｐ以前の動作は図９を用いて説明したものと同様である。

【0039】

図１１に示すタイミング図において、時刻Ｐにおいて緊急停止信号がオフ（“０”）からオン（“１”）に切り替わったとする。この時点では位置指令切替指令は依然としてオフのままであり、全軸制御部５０から各モータ駆動制御部４０に送られる位置指令は通常動作用のものである。全軸制御部５０は、緊急停止信号が入力されても通常動作用の位置指令値を出力し続けるが、手先の座標、向きを目標軌道上に維持する減速停止のための動作計画を開始する。特に全軸制御部５０は、モータ駆動制御部４０がその各軸停止用位置指令値に基づいてサーボ制御を行なったときのロボット位置を推定し、推定結果に基づいてロボットの位置と姿勢を修正する修正動作のための演算（修正演算）を開始する。モータ駆動制御部４０は、時刻Ｐにおいて緊急停止信号が入力されると、モータのサーボ制御に用いる位置指令値を全軸制御部５０からのものから各軸停止用位置指令値に切り替える。これによりモータは減速を開始し、手先位置の速度の低下も始まる。

【0040】

その後、時刻Ｑにおいて、位置指令切替指令がオフからオンに切り替わる。すると全軸制御部５０は、通常動作用の位置指令値の出力を停止し、各軸停止用位置指令値に基づいてロボットを制御したときに発生する本来の軌道からのずれを修正するために修正動作の位置指令値を出力する。そして時刻Ｒにおいてロボットが本来の軌道すなわち目標軌道に戻ったら、引き続いて緊急停止用の位置指令値を出力する。モータ駆動制御部４０は、位置指令切替指令がオンに切り替わると、その内部でサーボ制御のために使用する位置指令値を各軸停止用位置指令値から全軸制御部５０が出力する位置指令値に切り替える。以上の処理により、手先位置の速度は図９に示す場合と同様に減速して速やかに０に到達する。

【0041】

図１２は、第２の実施形態において緊急停止を行ったときのロボットの手先１３の位置と姿勢の変化を示す図であって、ＸＹ平面内で手先１３がどのように移動するかを示している。緊急停止信号が入力されて各モータ駆動制御部４０での停止用内部位置指令に基づくサーボ制御が開始すると、手先１３の位置は本来の軌道からずれ、手先１３の長手方向の向きもＹ方向に対して斜めになる。そして、全軸制御部５０からの修正動作の位置指令値によるサーボ制御が開始されると手先１３の位置と姿勢は本来の軌道に沿うものとなり、引き続く全軸制御部５０からの緊急停止用の位置指令値によって手先１３は本来の軌道上で位置と姿勢とを保って停止する。ここで、手先１３における関節３の位置と本来の軌道とのずれの量を横座標誤差Δｘとし、手先１３の長手方向がＹ方向となす角度を方位角誤差Δθとする。

【0042】

図１３は、第２の実施形態においてロボットの緊急停止動作を行ったときのロボットの各部の位置、角度、速度及び角速度の変化を示すグラフである。図１３において（ａ）は手先位置のＹ座標値を示し、（ｂ）は関節３のＸ方向位置すなわち横座標誤差Δｘを示し、（ｃ）は方位角誤差Δθを示し、（ｄ）は手先位置のＹ方向速度を示し、（ｅ）は手先位置のＸ方向速度を示し、（ｆ）は方位角誤差Δθの時間変化すなわち方位角についての角速度を示している。緊急停止信号が入力されたタイミング（時刻Ｐ）から位置指令切替指令が入力されるタイミング（時刻Ｑ）までの期間は、モータ駆動制御部４０内で算出される各軸停止用位置指令値に基づいて各モータがサーボ制御される期間であり、図１３ではこの期間での位置、角度、速度及び角速度の変化は示されていない。時刻Ｐにおいて緊急停止信号が入力されるまでは、ロボットは所定の軌道に沿って動いている。緊急停止信号が入力されると、モータ駆動制御部４０内で算出される各軸停止用位置指令値に基づいて各モータがサーボ制御されるので、ロボットは軌道からずれる。どれだけずれたかは、時刻Ｑにおける横座標誤差Δｘと方位角誤差Δθによって分かる。時刻Ｑからは時刻Ｒまでの期間では、全軸制御部５０が出力する修正動作の位置指令値に基づいて各モータがサーボ制御され、ロボットが本来の軌道に戻るように手先姿勢などが修正される。各軸停止用位置指令値による制御によって時刻Ｑの時点においてモータは既に減速を開始しており、修正動作の位置指令値は、時刻Ｑでの手先の座標や速度を初期値として滑らかな補正動作を達成するように計画される。その結果、時刻Ｒではロボットは本来の軌道に戻っている。その後、手先の座標、向きを目標軌道上に維持する減速停止のための緊急停止用の位置指令値に基づいて各モータが制御され、ロボットがその目標軌道上で最終的に停止する。

【0043】

第２の実施形態の緊急停止動作を実施するロボットコントローラは、第１の実施形態において用いられたものと同様に全軸制御部５０とモータごとのモータ駆動制御部４０とを備える。第２の実施形態の場合、図１２及び図１３を用いて説明したように、全軸制御部５０は、時刻Ｑにおいて修正動作用の位置指令値を出力する必要がある。その一方で、全軸制御部５０が手先の減速停止用の位置指令（すなわち緊急停止用位置指令）の出力を開始するまでには時間を要する。そのため、全軸制御部５０は、時刻Ｑにおける各モータの位置や速度を推定し、それに基づいて時刻Ｑでの手先の座標や速度を推定する。図１４は、本実施形態において用いられる全軸制御部５０の構成を示している。

【0044】

図１４に示す全軸制御部５０は、図５に示した全軸制御部５０に対し、時刻Ｑでのロボットの位置や速度を推定するための推定値演算部５６と、推定値演算部５６で得られた推定値に基づいて修正動作の位置指令値と手先の座標、向きを目標軌道に維持する減速停止を行うためでの緊急停止用位置指令値とを算出する位置指令値演算部５７とを加えたものである。通常動作用の位置指令値を出力する逆速比演算部５３は、位置指令値のほかに位置指令微分値も出力する。推定値演算部５６には、緊急停止信号も入力される。

【0045】

推定値演算部５６は、緊急停止信号が入力された時点で逆速比演算部５３が出力する位置指令値を取り込み、各軸停止用位置指令値による減速後のモータ位置を推定する減速後角度演算部６１と、減速後角度演算部６１で推定したモータ位置に基づいて速比演算を行う速比演算部６２と、速比演算部６２での演算結果に基づいて運動学演算を行って手先位置推定値を算出する運動学演算部６３とを備えている。さらに推定値演算部５６は、緊急停止信号が入力された時点で逆速比演算部５３が出力する位置指令微分値を取り込み、各軸停止用位置指令値による減速後のモータ速度を推定する減速後角速度演算部６４と、減速後角速度演算部６１で推定したモータ速度に基づいて速比演算を行う速比演算部６５と、速比演算部６２での演算結果に基づいてヤコビアン演算を行って手先速度推定値を算出するヤコビアン演算部６６とを備えている。各モータ駆動制御部４０において各軸停止用位置指令値に基づいてモータを減速させるときは、モータごとに設定された加速度（あるいは減速時間）でモータを減速させる制御を実行し、このときの減速用の加速度は既知であるから、減速後角度演算部６１及び減速後角速度演算部６４は、緊急停止信号が入力されたた時点で逆速比演算部５３が出力する位置指令値及び位置指令微分値に基づいて、時刻Ｑにおける各モータの位置及び速度をそれぞれ推定することができる。そしてモータ位置及びモータ速度の推定値に速比演算を行い、さらに運動学演算やヤコビアン演算を行うことによって、手先位置及び手先速度を推定することができる。手先位置推定値は、手先位置のＹ方向座標、手先位置のＸ方向座標（あるいは横座標誤差Δｘ）、及び方位角誤差Δθを含んでいる。手先速度推定値は、手先位置のＹ方向速度、手先位置のＸ方向速度、及び方位角誤差Δθの時間変化（すなわち方位角角速度）が含まれている。

【0046】

位置指令値演算部５７は、推定値演算部５６で求めた手先位置のＹ方向座標及びＹ方向速度に基づいて減速用の手先位置指令を算出する手先位置指令算出部７１と、手先位置指令算出部７１で算出した手先位置指令に対して逆運動学演算を行う逆運動学演算部７４と、逆運動学演算部７４での演算結果に対して逆速比演算を行って位置指令値を出力する逆速比演算部７５とを備えている。Ｙ方向の座標及び速度だけに基づいて手先位置指令を算出した場合にはロボットの軌道からのずれを修正できないから、位置指令値演算部５７は、さらに、手先位置のＸ方向座標、方位角誤差Δｘ、手先位置のＸ方向速度及び方位角速度に基づいて誤差補正量を演算する誤差補正量演算部７２と、手先位置指令算出部７１が算出した手先位置指令に対して誤差補正量を加算する加算部７３とを備えている。加算部７３において誤差補正量が加算された手先位置指令が、逆運動学演算部７４での逆運動学演算の対象となっている。このような位置指令値演算部５７を用いることにより、手先の軌道を目標値に維持した減速停止のための緊急停止用位置指令値だけでなく修正動作の位置指令値を算出することが可能となっている。

【0047】

図１５は第２の実施形態で用いることができるモータ駆動制御部４０の構成を示している。モータ駆動制御部４０は、位置指令値とモータ位置とに基づいて速度指令値を算出する位置制御部４１と、モータ位置からモータ速度を算出する速度算出部４２と、速度指令値と速度算出部４２から入力するモータ速度とに基づいてモータに対するトルク指令値を算出する速度制御部４３と、緊急停止信号が入力されるとともに全軸制御部５０からの位置指令値に基づいて各軸停止用位置指令値を算出する停止用位置指令演算部４６とを備えている。さらに、位置制御部４１に入力される位置指令値を切り替えるためにセレクタ４８，４９が設けられている。セレクタ４８は、緊急停止信号に応じて、全軸制御部５０からの位置指令値とセレクタ４９が出力する位置指令値とを切り替えて位置制御部４１に入力する。セレクタ４９は、位置指令切替指令に応じて、全軸制御部５０からの位置指令値と停止用位置指令演算部４６が算出する各軸停止用位置指令値とを切り替えて出力する。セレクタ４８，４９により、緊急停止信号が入力されてから位置指令切替指令が入力されるまでの期間において各軸停止用位置指令値が位置制御部４１に入力し、その他の期間では全軸制御部５０からの位置指令値が位置制御部４１に入力することになる。

【0048】

以上説明した第２の実施形態の緊急停止方法によれば、第１の実施形態と比べ、全軸制御部５０での演算処理が複雑になるものの、目標軌道上でより滑らかにロボットを緊急停止させることができる。

【0049】

以上、本発明に基づく緊急停止方法について、３軸の水平多関節ロボットに適用した例を説明した。本発明が適用できるロボットは、ここで説明したものに限定されるものではなく、水平多関節ロボット以外のロボットにも本発明を適用することができる。一般に軸数が多いロボットでは手先の座標、向きを目標軌道上に保ったままロボットを速やかに停止させることが難しいことが多いが、本発明を適用することにより、そのようなロボットにおいても手先の座標、向きを目標軌道上に保ったままそのロボットを速やかに停止させることが可能になる。

【符号の説明】

【0050】

１～３…関節；１０…基台；１１，１２…リンク；１３…手先；１４…マニピュレータ；２１～２３…モータ；３１～３３…減速機；４０…モータ駆動制御部；４１…位置制御部；４２…速度算出部；４３…速度制御部；４４…停止用速度指令演算部；４５，４８，４９…セレクタ；４６…停止用位置指令演算部；４７…指令切替移行演算部；５０…全軸制御部；５１，７１…手先位置指令算出部；５２，７４…逆運動学演算部；５３，７５…逆速比演算部；５６…推定値演算部；５７…位置指令値演算部；６１…減速後角度演算部；６２，６５…速比演算部；６３…運動学演算部；６４…減速後角速度演算部；６６…ヤコビアン演算部；７２…誤差補正量演算部；７３…加算部。

【図1】

【図2】

【図3】

【図4】

【図5】

【図6】

【図7】

【図8】

【図9】

【図10】

【図11】

【図12】

【図13】

【図14】

【図15】