特許 7238450 ロボットの制御装置およびロボットの制御方法

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

(12)【公報種別】特許公報(B2)

(11)【特許番号】

(24)【登録日】2023-03-06

(45)【発行日】2023-03-14

(54)【発明の名称】ロボットの制御装置およびロボットの制御方法

(51)【国際特許分類】

   B25J 9/22 20060101AFI20230307BHJP

【ＦＩ】

B25J9/22 A

【請求項の数】 8

(21)【出願番号】P 2019024493

(22)【出願日】2019-02-14

(65)【公開番号】P2020131314

(43)【公開日】2020-08-31

【審査請求日】2021-10-04

(73)【特許権者】

【識別番号】501428545

【氏名又は名称】株式会社デンソーウェーブ

(74)【代理人】

【識別番号】110000567

【氏名又は名称】弁理士法人サトー

(72)【発明者】

【氏名】佐藤 晶則

(72)【発明者】

【氏名】河地 勇登

【審査官】臼井 卓巳

(56)【参考文献】

【文献】特開２０１７－０７４６６９（ＪＰ，Ａ）

【文献】特開２０１４－０１４９０１（ＪＰ，Ａ）

【文献】特開昭６３－２６０７７９（ＪＰ，Ａ）

【文献】特開昭６３－２７８７７７（ＪＰ，Ａ）

【文献】特開昭６３－２７８７７８（ＪＰ，Ａ）

【文献】特開平０５－０５７６４３（ＪＰ，Ａ）

【文献】特開平０７－２５１３９０（ＪＰ，Ａ）

【文献】特開平０８－２１６０７４（ＪＰ，Ａ）

【文献】特開平１１－２３９９８８（ＪＰ，Ａ）

【文献】特開２０１８－１１１１５５（ＪＰ，Ａ）

【文献】米国特許出願公開第２０１５／０２１７４４５（ＵＳ，Ａ１）

(58)【調査した分野】(Int.Cl.，ＤＢ名)

Ｂ２５Ｊ ３／００－ ９／２２

(57)【特許請求の範囲】

【請求項1】

垂直多関節型のロボットを制御するものであり、ユーザが前記ロボットを直接動かして前記ロボットのアーム先端の位置姿勢を教示するダイレクトティーチングに対応したロボットの制御装置であって、
前記ロボットの各軸の角度制御を行う角度制御部と、
前記アーム先端の姿勢を目標姿勢に維持しつつ前記アーム先端の位置を変化させるように前記ダイレクトティーチングが行われる際、前記各軸のうち、前記アーム先端の位置を決定するうえで支配的な要因となり得る軸を外力に従って自由に動作する状態とした操作軸に設定するとともに、前記アーム先端の姿勢を決定するうえで支配的な要因となり得る軸を前記角度制御部により制御される状態とした制御軸に設定する軸設定部と、
を備え、
前記角度制御部は、前記ダイレクトティーチングが行われる際、前記操作軸の現在角度および前記目標姿勢を入力として逆運動学計算により前記制御軸の指令角度を算出するロボットの制御装置。

【請求項2】

垂直多関節型のロボットを制御するものであり、ユーザが前記ロボットを直接動かして前記ロボットのアーム先端の位置姿勢を教示するダイレクトティーチングに対応したロボットの制御装置であって、
前記ロボットの各軸の角度制御を行う角度制御部と、
前記アーム先端の位置を目標位置に維持しつつ前記アーム先端の姿勢を変化させるように前記ダイレクトティーチングが行われる際、前記各軸のうち、前記アーム先端の姿勢を決定するうえで支配的な要因となり得る軸を外力に従って自由に動作する状態とした操作軸に設定するとともに、前記アーム先端の位置を決定するうえで支配的な要因となり得る軸を前記角度制御部により制御される状態とした制御軸に設定する軸設定部と、
を備え、
前記角度制御部は、前記ダイレクトティーチングが行われる際、前記操作軸の現在角度および前記目標位置を入力として逆運動学計算により前記制御軸の指令角度を算出するロボットの制御装置。

【請求項3】

前記角度制御部は、
前記制御軸の指令角度が算出されると、その指令角度に基づいて算出される前記制御軸の指令角速度および指令角加速度がそれぞれ予め定められた最大角速度および最大角加速度を超えないように前記指令角度を補正する加速補正処理を実行する請求項１または２に記載のロボットの制御装置。

【請求項4】

前記角度制御部は、
前記ロボットが特異点近傍であるか否かを検出する特異点検出処理と、
前記特異点検出処理により前記特異点近傍であることが検出されると、その時点以降の前記指令角度を、前回の前記指令角度に置換する指令角度置換処理と、
を実行する請求項１から３のいずれか一項に記載のロボットの制御装置。

【請求項5】

垂直多関節型のロボットを制御するものであり、ユーザが前記ロボットを直接動かして前記ロボットのアーム先端の位置姿勢を教示するダイレクトティーチングを行うことができるロボットの制御方法であって、
前記ロボットの各軸の角度制御を行う角度制御処理と、
前記アーム先端の姿勢を目標姿勢に維持しつつ前記アーム先端の位置を変化させるように前記ダイレクトティーチングが行われる際、前記各軸のうち、前記アーム先端の位置を決定するうえで支配的な要因となり得る軸を外力に従って自由に動作する状態とした操作軸に設定するとともに、前記アーム先端の姿勢を決定するうえで支配的な要因となり得る軸を前記角度制御処理により制御される状態とした制御軸に設定する軸設定処理と、
を含み、
前記角度制御処理では、前記ダイレクトティーチングが行われる際、前記操作軸の現在角度および前記目標姿勢を入力として逆運動学計算により前記制御軸の指令角度を算出するロボットの制御方法。

【請求項6】

垂直多関節型のロボットを制御するものであり、ユーザが前記ロボットを直接動かして前記ロボットのアーム先端の位置姿勢を教示するダイレクトティーチングを行うことができるロボットの制御方法であって、
前記ロボットの各軸の角度制御を行う角度制御処理と、
前記アーム先端の位置を目標位置に維持しつつ前記アーム先端の姿勢を変化させるように前記ダイレクトティーチングが行われる際、前記各軸のうち、前記アーム先端の姿勢を決定するうえで支配的な要因となり得る軸を外力に従って自由に動作する状態とした操作軸に設定するとともに、前記アーム先端の位置を決定するうえで支配的な要因となり得る軸を前記角度制御処理により制御される状態とした制御軸に設定する軸設定処理と、
を含み、
前記角度制御処理では、前記ダイレクトティーチングが行われる際、前記操作軸の現在角度および前記目標位置を入力として逆運動学計算により前記制御軸の指令角度を算出するロボットの制御方法。

【請求項7】

前記角度制御処理には、
前記制御軸の指令角度が算出されると、その指令角度に基づいて算出される前記制御軸の指令角速度および指令角加速度がそれぞれ予め定められた最大角速度および最大角加速度を超えないように前記指令角度を補正する加速補正処理が含まれる請求項５または６に記載のロボットの制御方法。

【請求項8】

前記角度制御処理には、
前記ロボットが特異点近傍であるか否かを検出する特異点検出処理と、
前記特異点検出処理により前記特異点近傍であることが検出されると、その時点以降の前記指令角度を、前回の前記指令角度に置換する指令角度置換処理と、
が含まれる請求項５から７のいずれか一項に記載のロボットの制御方法。

【発明の詳細な説明】

【技術分野】

【0001】

本発明は、ユーザがロボットを直接動かしてロボットのアーム先端の位置姿勢を教示するダイレクトティーチングに対応したロボットの制御装置およびロボットの制御方法に関する。

【背景技術】

【0002】

従来、ロボットに所定の作業を行わせるために動作をティーチング（教示）する方法として、ユーザが専用の操作端末を用いて遠隔でティーチングを行う手法（以下、遠隔ティーチングと称す）と、ユーザ（人）がロボットに直接触れて動かすことによりティーチングを行う手法（以下、ダイレクトティーチングと称す）と、がある。ダイレクトティーチングには、ユーザが、専門知識を必要とすることなく直感的に操作を行うことができるというメリットがある。

【0003】

ただし、ダイレクトティーチングでは、ユーザが手の感覚を頼りにロボットのアーム先端の位置および姿勢を調節することになるため、遠隔ティーチングのようにキー操作や数値入力で位置および姿勢を調節する場合に比べ、ユーザの意図した通りに位置および姿勢を変化させることが難しかった。具体的には、ユーザが、先端に取り付けられたハンドなどのツールの姿勢を保ちつつ位置だけを変化させようとしても、理想的な形で力を加えることは非常に難しく、どうしても余分な力が加わってしまい、先端の姿勢が意図とは異なる形で変化してしまうことがある。また、ユーザが、ツールの位置を保ちつつ姿勢だけを変化させようとしても、上述したように理想的な形で力を加えることが困難であることから、先端の位置が意図せずに変化してしまうことがある。

【0004】

このようなダイレクトティーチングにおける課題を解決するため、様々な技術が考えられている。例えば、特許文献１には、ロボットのアーム先端にジョイスティックや力覚センサを取り付け、それらを入力装置として先端の位置および姿勢のうちいずれか一方だけを変更し制御する方法が開示されている。また、ロボットの各関節にトルクセンサを取り付けたり、電流値から外力推定を行ったり、することにより力覚センサを取り付けるものと同等の機能を実現することも考えられる。

【0005】

しかし、このような従来技術では、センサを追加することに伴い、製造コストの上昇、重量増加に起因する性能の劣化など、別の問題が生じることになる。さらに、力覚センサなどを用いて力を制御する方法では、ロボットのポーズ（姿勢）や力の加え方によっては振動的な動きになるおそれがある。そのため、上記方法では、パラメータ設定を保守的にするなどして振動を抑制する必要があり、その結果、操作感が悪化してしまい、本来の目的である教示作業を簡単化するという効果が得られない。

【0006】

一方、特許文献２には、ロボットの各関節を、ユーザの操作に対して受動的に振る舞う関節、つまり人為的に操作可能な関節と、各関節の角度の変化から逆算して位置または姿勢を維持するための角度制御を行う関節と、の２つのグループに分ける手法が開示されている。特許文献２の手法では、現在の先端の位置・姿勢に対して維持しようとする姿勢成分を置換（上書き）するようになっているため、操作可能な関節がユーザにより大きく動かされるなどして操作可能な関節を大きく動かさなければならない指令値が出力された場合には、角度制御可能な関節しか制御することができないことから、本来の目的である先端の姿勢を維持することができなくなるおそれがある。このように、特許文献２の手法では、ユーザの意図した通りに位置および姿勢を変化させることが難しかった。

【先行技術文献】

【特許文献】

【0007】

【文献】特開平５－２８５８７０号公報

【文献】特開昭６０－２３３７０７号公報

【発明の概要】

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

【0008】

上記したように、特許文献１、２に記載された技術を含めた従来の技術は、ダイレクトティーチングにおけるアーム先端の位置姿勢の調節操作が簡単化されているとは言い難いものであった。

【0009】

本発明は上記事情に鑑みてなされたものであり、その目的は、ダイレクトティーチングにおけるアーム先端の位置姿勢の調節操作を簡単化することができるロボットの制御装置およびロボットの制御方法を提供することにある。

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

【0010】

請求項１に記載のロボットの制御装置は、垂直多関節型のロボットを制御するものであり、ユーザがロボットを直接動かしてロボットのアーム先端の位置姿勢を教示するダイレクトティーチングに対応している。上記したロボットの制御装置は、ロボットの各軸の角度制御を行う角度制御部および軸設定部を備える。軸設定部は、アーム先端の姿勢を目標姿勢に維持しつつアーム先端の位置を変化させるようにダイレクトティーチングが行われる際、各軸のうち、アーム先端の位置を決定するうえで支配的な要因となり得る軸を外力に従って自由に動作する状態とした操作軸に設定するとともに、アーム先端の姿勢を決定するうえで支配的な要因となり得る軸を角度制御部により制御される状態とした制御軸に設定する。角度制御部は、ダイレクトティーチングが行われる際、操作軸の現在角度および目標姿勢を入力として逆運動学計算により制御軸の指令角度を算出する。

【0011】

このような構成によれば、ユーザの操作（ロボットに触れて動かすこと）に応じて操作軸が自由に動作するため、ユーザの意図通りにアーム先端の位置を変化させることができる。そして、この際、ユーザの操作に応じて動作する操作軸の現在角度およびアーム先端の目標姿勢に基づいて制御軸の指令角度が算出されるようになっている。つまり、この場合、維持したい姿勢以外の目標値を未知数としたまま厳密な制御軸の目標値を算出するようになっているため、アーム先端の姿勢を目標姿勢に維持させることができる。

【0012】

このようなことから、上記構成によれば、ユーザは、特に難しい操作を必要とすることなく、アーム先端の姿勢を目標姿勢に保ちつつアーム先端の位置を変化させる、つまりアーム先端の姿勢を拘束したダイレクトティーチングを行うことができる。したがって、上記構成によれば、ロボットのダイレクトティーチングにおけるアーム先端の位置姿勢の調節操作を簡単化することができるという優れた効果が得られる。

【0013】

また、上記構成によれば、自由に操作可能な操作軸の現在角度に合わせて角度制御が有効な制御軸だけでアーム先端の姿勢を目標姿勢に一致させるように角度制御が行われることになるため、振動など不安定な動作が生じ難いという効果も得られる。さらに、上記構成によれば、力覚センサやトルクセンサなどのセンサを用いることなく、ロボットの制御装置に元々設けられる角度制御のための制御系を用いることによりアーム先端の姿勢を拘束したダイレクトティーチングを実現するようになっているため、製造コストの低減にも寄与することができる。

【0014】

請求項２に記載のロボットの制御装置は、垂直多関節型のロボットを制御するものであり、ユーザがロボットを直接動かしてロボットのアーム先端の位置姿勢を教示するダイレクトティーチングに対応している。上記したロボットの制御装置は、ロボットの各軸の角度制御を行う角度制御部および軸設定部を備える。軸設定部は、アーム先端の位置を目標位置に維持しつつアーム先端の姿勢を変化させるようにダイレクトティーチングが行われる際、各軸のうち、アーム先端の姿勢を決定するうえで支配的な要因となり得る軸を外力に従って自由に動作する状態とした操作軸に設定するとともに、アーム先端の位置を決定するうえで支配的な要因となり得る軸を角度制御部により制御される状態とした制御軸に設定する。角度制御部は、ダイレクトティーチングが行われる際、操作軸の現在角度および目標位置を入力として逆運動学計算により制御軸の指令角度を算出する。

【0015】

このような構成によれば、ユーザの操作（ロボットに触れて動かすこと）に応じて操作軸が自由に動作するため、ユーザの意図通りにアーム先端の姿勢を変化させることができる。そして、この際、ユーザの操作に応じて動作する操作軸の現在角度およびアーム先端の目標位置に基づいて制御軸の指令角度が算出されるようになっている。つまりこの場合、維持したい位置以外の目標値を未知数としたまま厳密な制御軸の目標値を算出するようになっているため、アーム先端の位置を目標位置に維持させることができる。

【0016】

このようなことから、上記構成によれば、ユーザは、特に難しい操作を必要とすることなく、アーム先端の位置を目標位置に保ちつつアーム先端の姿勢を変化させる、つまりアーム先端の位置を拘束したダイレクトティーチングを行うことができる。したがって、上記構成によれば、ロボットのダイレクトティーチングにおけるアーム先端の位置姿勢の調節操作を簡単化することができるという優れた効果が得られる。

【0017】

また、上記構成によれば、自由に操作可能な操作軸の現在角度に合わせて角度制御が有効な制御軸だけでアーム先端の位置を目標位置に一致させるように角度制御が行われることになるため、振動など不安定な動作が生じ難いという効果も得られる。さらに、上記構成によれば、力覚センサやトルクセンサなどのセンサを用いることなく、ロボットの制御装置に元々設けられる角度制御のための制御系を用いることによりアーム先端の位置を拘束したダイレクトティーチングを実現するようになっているため、製造コストの低減にも寄与することができる。

【0018】

請求項１または２に記載のロボットの制御装置においては、ダイレクトティーチングの実行中、ロボットが特異点近傍を通過する場合、操作軸が急激に操作された場合、制御軸が可動範囲外に接近する場合などには、制御軸の指令角度の変化量が過大になることがあり、そうすると、制御軸の動作が急峻な動作となって安全性が低下するおそれがある。そこで、このような点を改善するため、請求項３に記載の角度制御部は、次のような処理を実行するようになっている。

【0019】

すなわち、角度制御部は、制御軸の指令角度が算出されると、その指令角度に基づいて算出される制御軸の指令角速度および指令角加速度がそれぞれ予め定められた最大角速度および最大角加速度を超えないように指令角度を補正する加速補正処理を実行する。これにより、制御軸の角速度および角加速度がそれぞれ最大角速度および最大角加速度以下となる安全な指令角度が生成される。

【0020】

このようにすれば、ダイレクトティーチングの実行中にロボットが特異点近傍を通過したり、操作軸が急激に操作されたりすることによって制御軸の指令角度の変化量が過大になった場合でも、制御軸の角速度および角加速度をそれぞれ最大角速度および最大角加速度以下に抑えつつ、つまり安全な加速をしつつ、制御軸の角度を当初算出された指令角度に精度良く到達させることができる。

【0021】

なお、このような加速補正処理が行われる場合、制御軸の動作は、ユーザの操作による操作軸の動作に対して遅れた動作となる。しかしながら、このような遅れは、ロボットのダイレクトティーチングにおいて問題とはならない。なぜなら、一般に、ロボットのダイレクトティーチングでは、ユーザがロボットに触れて動かし、その後、ロボットを停止させた状態で、アーム先端の位置や姿勢が所望する状態となっているかを確認する。このような動作が繰り返されることにより、所望する教示が行われる。つまり、ロボットのダイレクトティーチングにおいて、ユーザがロボットを動かし続けることはなく、必ず、ロボットを停止させる期間が存在する。

【0022】

上述した制御では、操作軸の動作に対して制御軸の動作が遅れるが、その遅れ分は、ロボットが停止されている期間に取り戻すことができる。したがって、ロボットのダイレクトティーチングでは、上述した制御による制御軸の動作の遅れは、問題とはならない。このようなことから、上記構成では、制御軸の動作が遅れるような制御を敢えて採用しており、それにより、ロボットの安全性の向上を図るようになっている。

【0023】

請求項３に記載のロボットの制御装置によれば、制御軸の動作が急峻な動作となることは回避できるものの、ロボットの特異点近傍において各軸の移動量が大きくなる現象、つまり遠く離れた関節角度に移動しようとする現象は避けることができない。ダイレクトティーチング中、ユーザは、ロボットに近付いた位置から操作軸の操作を行うことになるため、特異点近傍において各軸の移動量が大きくなる現象が発生すると、ロボットのアームなどがユーザに接触する可能性があり、その結果、安全性が低下するおそれがある。

【0024】

そこで、このような点を改善するため、請求項４に記載の角度制御部は、次のような処理を実行するようになっている。すなわち、角度制御部は、ロボットが特異点近傍であるか否かを検出する特異点検出処理を実行する。なお、特異点近傍であることは、例えば、ヤコビアンが０になる各軸角度、５軸０度付近などにより検出することが可能である。そして、角度制御部は、特異点検出処理により特異点近傍であることが検出されると、その時点以降の指令角度を、前回の指令角度に置換する指令角度置換処理を実行する。

【0025】

このように、上記構成では、ロボットが特異点近傍に突入すると指令角度を前回の指令角度で固定（ロック）するという処理が加えられている。このようにすれば、ロボットの特異点近傍において各軸の移動量が大きくなる現象の発生が抑制され、その結果、安全性を向上させることができる。この場合、制御軸の角度制御による位置・姿勢の維持が一時的に中断されることになるが、操作軸の操作に応じて特異点近傍を脱出することができれば、その後は再び制御軸の角度制御による位置・姿勢の維持が再開される。

【0026】

また、上述したようにロボットが特異点近傍に突入すると指令角度を前回の指令角度でロックする処理を実行して制御軸の角度制御による位置・姿勢の維持を一時的に中断させることにより、請求項３に記載の加速補正処理による制御軸の遅れ分を取り戻すことが可能となる。つまり、上記構成によれば、特異点近傍におけるロボットの安全性を向上させつつ、加速補正処理が行われた場合における制御軸の動作の遅れを取り戻すことができる。

【0027】

請求項５～８に記載のロボットの制御方法は、それぞれ請求項１～４に記載のロボットの制御装置と共通する技術思想に基づくものである。したがって、請求項５～８に記載のロボットの制御方法によれば、それぞれ請求項１～４に記載のロボットの制御装置と同様の効果が得られる。

【図面の簡単な説明】

【0028】

【図1】第１実施形態に係るロボットシステムの構成を模式的に示す図

【図2】第１実施形態に係るロボットの座標系を示す図

【図3】第１実施形態に係るロボットの構成をｘｚ平面およびｙｚ平面で示す図

【図4】第１実施形態に係るコントローラが実行するダイレクトティーチングに関する処理の全体の流れを示すフローチャート

【図5】第１実施形態に係る第１モード時における操作軸および制御軸の設定状態を表す図

【図6】第１実施形態に係る第２モード時における操作軸および制御軸の設定状態を表す図

【図7】第１実施形態に係る第１指令算出処理の内容を示すフローチャート

【図8】第１実施形態に係る第２指令算出処理の内容を示すフローチャート

【図9】第１実施形態に係るロボットのアーム形態をｘｙ座標およびαｒ座標で示す図

【図10】第１実施形態に係るロボットのアーム形態をαｚ座標で示す図

【図11】比較例に係る操作軸が大きく動かされた場合のロボットの挙動を模式的に示す図その１

【図12】比較例に係る操作軸が大きく動かされた場合のロボットの挙動を模式的に示す図その２

【図13】第２実施形態に係る第１指令算出処理の内容を示すフローチャート

【図14】第２実施形態に係る第２指令算出処理の内容を示すフローチャート

【図15】第２実施形態に係る指令速度制限処理の内容を示すフローチャート

【図16】第２実施形態に係るダイレクトティーチング中に各指令値が修正される様子を示す図その１

【図17】第２実施形態に係るダイレクトティーチング中に各指令値が修正される様子を示す図その２

【図18】第２実施形態に係る各処理が実行されることに伴う指令角度の変化の様子を示す図

【図19】第３実施形態に係る第１指令算出処理の内容を示すフローチャート

【図20】第３実施形態に係る第２指令算出処理の内容を示すフローチャート

【図21】第３実施形態に係るダイレクトティーチング中に各指令値が補間される様子を示す図

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

【0029】

以下、本発明をロボットの制御装置およびロボットの制御方法に適用した複数の実施形態について図面を参照して説明する。なお、各実施形態において実質的に同一の構成には同一の符号を付して説明を省略する。
（第１実施形態）
以下、第１実施形態について図１～図１３を参照して説明する。

【0030】

＜ロボットシステムの構成＞
図１に示すように、ロボットシステム１は、垂直多関節型のロボット２、ロボット２を制御するコントローラ３をベース４の内部に備えている。このロボットシステム１は、一般的な産業用に用いられている。ロボット２は、いわゆる６軸の垂直多関節型ロボットである。ベース４上に、ｚ方向の軸心を持つ第１軸；Ｊ１を介してショルダ５が水平方向に回転可能に連結されている。ショルダ５には、ｙ方向の軸心を持つ第２軸；Ｊ２を備え、ｙ方向に伸びる第２オフセットアーム６を介して、上方に延びる第１アーム７の下端部が垂直方向に回転可能に連結されている。第１アーム７の先端部には、Ｙ方向の軸心を持つ第３軸；Ｊ３を備え、－ｙ方向に伸びる第３オフセットアーム８を介して、第２アーム９が垂直方向に回転可能に連結されている。第２アーム９は、基部９ａおよび先端部９ｂからなる。

【0031】

第２アーム９は、ｘ方向の軸心を持つ第４軸；Ｊ４を備え、基部９ａに対して先端部９ｂが捻り回転可能に連結されている。第２アーム９の先端部には、ｙ方向の軸心を持つ第５軸；Ｊ５を備え、－ｙ方向に伸びる第５オフセットアーム１０を介して、手首１１が垂直方向に回転可能に連結されている。手首１１には、ｘ方向の軸心を持つ第６軸；Ｊ６を介して、図２に示すフランジおよびハンド１２が捻り回転可能に連結されている。ロボット２に設けられている各軸；Ｊ１～Ｊ６には、それぞれに対応して駆動源となる図示しないモータが設けられている。なお、以下では、第１軸、第２軸、第３軸、第４軸、第５軸、第６軸のことをそれぞれ１軸、２軸、３軸、４軸、５軸、６軸と省略することがある。

【0032】

コントローラ３は、ロボット２の制御装置であり、図示しないＣＰＵ、ＲＯＭおよびＲＡＭなどで構成されたコンピュータからなる制御手段においてコンピュータプログラムを実行することで、ロボット２を制御している。具体的には、コントローラ３は、インバータ回路などから構成された駆動部を備えており、各モータに対応して設けられているエンコーダで検知したモータの回転位置に基づいて例えばフィードバック制御によりそれぞれのモータを駆動する。

【0033】

コントローラ３は、ＣＰＵ、ＲＯＭ、ＲＡＭ、駆動回路および位置検出回路などを備えている。ＲＯＭは、ロボット２のシステムプログラムや動作プログラムなどを記憶している。ＲＡＭは、これらのプログラムを実行する際にパラメータの値などを記憶する。位置検出回路には、ロボット２の各関節に設けられた図示しない各エンコーダの検出信号がそれぞれ入力される。位置検出回路は、各エンコーダの検出信号に基づいて、各関節に設けられたモータの回転角度位置を検出する。

【0034】

コントローラ３は、予め設定された動作プログラムを実行することにより、位置検出回路から入力される位置情報に基づいて、アーム先端部の制御点の位置および姿勢を制御する。本実施形態において、コントローラ３は、ＣＰ（Continuous Path）制御を行う。ＣＰ制御では、アーム先端部の制御点を目標まで動作させる際に制御点の目標となる位置および姿勢、つまり動作軌道が時間関数として設定される。目標となる位置および姿勢には、教示された位置および姿勢に加えて、教示された位置および姿勢に基づいて補間された位置および姿勢も含まれる。コントローラ３は、ＣＰ制御により、制御点の位置および姿勢が動作軌道に沿うように、アームにおける各関節の角度を制御する。コントローラ３は、位置および姿勢の制御において、現在指示されている目標となる位置および姿勢を実現するための第１軸～第６軸の角度を算出する逆変換処理を行う。

【0035】

図２に示すように、ロボット２の各関節には、３次元の直交座標系である第１～第６座標系Σ_１～Σ_６が規定されている。各座標系Σ_１～Σ_６の原点は、第１～第６軸線Ｊ１～Ｊ６上の所定位置に定められている。各座標系Σ_１～Σ_６のｚ軸であるｚ１～ｚ６軸は、第１～第６軸線Ｊ１～Ｊ６と一致している。

【0036】

ベース４には、ロボット座標系である第０座標系Σ_０が規定されている。第０座標系Σ_０は、第１軸～第６軸が回転しても変化しない座標系である。本実施形態において、座標系Σ_０の原点は、第１軸線Ｊ１上に定められている。また、座標系Σ_０のｚ軸であるｚ０軸は、第１軸線Ｊ１に一致している。

【0037】

図２に示すｄ１～ｄ６、ａ２およびａ３は、以下のように定義される。
ｄ１：第０座標系Σ_０の原点から第１座標系Σ_１の原点までのリンク長
ｄ２：第１座標系Σ_１の原点から第１アーム７の基部までのリンク長
ａ２：第１アーム７の基部から同先端部，第２座標系Σ_２の原点までのリンク長
ｄ３：第２座標系Σ_２の原点から第３オフセットアーム８の先端部までのリンク長
ａ３：第３軸Ｊ３，第４軸Ｊ４の軸心間距離
ｄ４：第３座標系Σ_３の原点から第４座標系Σ_４の原点までのリンク長
ｄ５：第４座標系Σ_４の原点から第５座標系Σ_５の原点までのリンク長
ｄ６：第５座標系Σ_５の原点から第６座標系Σ_６の原点までのリンク長
ａ３については図３を参照。そして、上記ｄ５が第５オフセットアーム１０のリンク長に相当する。

【0038】

＜ダイレクトティーチングに関する機能＞
本実施形態のコントローラ３は、ユーザがロボット２を直接動かしてロボット２のアーム先端の位置姿勢を教示するダイレクトティーチングに対応している。コントローラ３は、ダイレクトティーチングに関する２つのモードを有している。１つ目のモードは、アーム先端の姿勢を拘束したダイレクトティーチングに対応する第１モードである。２つ目のモードは、アーム先端の位置を拘束したダイレクトティーチングに対応する第２モードである。

【0039】

これらモードの設定は、ユーザによる所定の操作に応じて行われる。ユーザは、第１モードに設定することにより、アーム先端の姿勢を目標姿勢に維持しつつアーム先端の位置を変化させるようにダイレクトティーチングを行うことが可能となる。また、ユーザは、第２モードに設定することにより、アーム先端の位置を目標位置に維持しつつアーム先端の姿勢を変化させるようにダイレクトティーチングを行うことが可能となる。

【0040】

コントローラ３は、ロボット２の各軸の角度制御を行う角度制御部としての機能に加え、軸設定部としての機能を有する。以下、このようなコントローラ３の機能について説明する。まず、ユーザによりダイレクトティーチングに関するモード設定と、維持すべき目標位置または目標姿勢を記録しておく。そして、図４のフローチャートに示すように、ステップＳ１１０では、コントローラ３は、ダイレクトティーチングに関するモード設定を確認する。ここで、ユーザにより第１モードに設定されている場合、ステップＳ１１０での判断の結果が「第１モード」となり、ステップＳ１２０に進む。

【0041】

ステップＳ１２０に進むと、コントローラ３は、図５に示すように、ロボット２の各軸（Ｊ１～Ｊ６）のうち、アーム先端から遠い側の、言い換えるとベース４に近い側の３つの軸である１，２，３軸（Ｊ１，Ｊ２，Ｊ３）を操作軸に設定する。ロボット２において、１，２，３軸は、アーム先端の位置を決定するうえで支配的な要因となり得る軸、つまりアーム先端の位置を支配（決定）する軸であり、以下の説明では、これらを根元側軸と総称することがある。

【0042】

このように、コントローラ３は、第１モードに設定されると、根元側軸を操作軸に設定する。操作軸に設定された軸は、角度制御部として機能するコントローラ３により制御することができない状態にされるとともに、外力に従って自由に動作する状態とされる。すなわち、コントローラ３は、第１モードに設定されると、根元側軸の位置制御（角度制御）をオフにすることになる。

【0043】

ステップＳ１２０の実行後は、ステップＳ１３０に進む。ステップＳ１３０に進むと、コントローラ３は、図５に示すように、アーム先端に近い側の３つの軸である４，５，６軸（Ｊ４，Ｊ５，Ｊ６）を制御軸に設定する。ロボット２において、４，５，６軸は、アーム先端の姿勢を決定するうえで支配的な要因となり得る軸、つまりアーム先端の姿勢を支配（決定）する軸であり、以下の説明では、これらを先端側軸と総称することがある。

【0044】

このように、コントローラ３は、第１モードに設定されると、先端側軸を制御軸に設定する。制御軸に設定された軸は、角度制御部として機能するコントローラ３により制御することができる状態となる。すなわち、コントローラ３は、第１モードに設定されると、先端側軸の位置制御をオンにすることになる。ステップＳ１３０の実行後は、ステップＳ１４０に進む。ステップＳ１４０に進むと、コントローラ３は、第１指令算出処理を実行する。

【0045】

第１指令算出処理は、操作軸（Ｊ１，Ｊ２，Ｊ３）の現在角度および目標姿勢を入力として逆運動学計算により制御軸（Ｊ４，Ｊ５，Ｊ６）の指令角度を算出するものであり、例えば図７に示すような内容の処理となる。まず、ステップＳ２１０では、コントローラ３は、逆変換入力パラメータを取得する。この場合、操作軸である１，２，３軸の現在角度「θ_１，θ_２，θ_３」と、アーム先端の目標姿勢、つまり維持する姿勢「Ｒ_ｘ，Ｒ_ｙ，Ｒ_ｚ」とが、逆変換入力パラメータとなる。

【0046】

ステップＳ２１０の実行後は、ステップＳ２２０に進む。ステップＳ２２０に進むと、コントローラ３は、逆変換計算（逆運動学計算）により、制御軸である４，５，６軸の指令角度「θ_４（ｋ＋１），θ_５（ｋ＋１），θ_６（ｋ＋１）」を算出する。ステップＳ２２０の実行後、第１指令算出処理が終了となる。第１指令算出処理が終了すると、コントローラ３は、算出された指令角度を用いて制御軸である４，５，６軸の位置制御（角度制御）を実行する。

【0047】

一方、ユーザにより第２モードに設定されている場合、ステップＳ１１０での判断の結果が「第２モード」となり、ステップＳ１５０に進む。ステップＳ１５０に進むと、コントローラ３は、図６に示すように、先端側軸（Ｊ４，Ｊ５，Ｊ６）を操作軸に設定する、言い換えると、先端側軸の位置制御をオフにする。すなわち、コントローラ３は、第２モードに設定されると、先端側軸を操作軸に設定する。

【0048】

ステップＳ１５０の実行後は、ステップＳ１６０に進む。ステップＳ１６０に進むと、コントローラ３は、図６に示すように、根元側軸（Ｊ１，Ｊ２，Ｊ３）を制御軸に設定する、言い換えると、根元側軸の位置制御をオンにする。すなわち、コントローラ３は、第２モードに設定されると、根元側軸を制御軸に設定する。ステップＳ１６０の実行後は、ステップＳ１７０に進む。ステップＳ１７０に進むと、コントローラ３は、第２指令算出処理を実行する。

【0049】

第２指令算出処理は、操作軸（Ｊ４，Ｊ５，Ｊ６）の現在角度および目標位置を入力として逆運動学計算により制御軸（Ｊ１，Ｊ２，Ｊ３）の指令角度を算出するものであり、例えば図８に示すような内容の処理となる。まず、ステップＳ３１０では、コントローラ３は、逆変換入力パラメータを取得する。この場合、操作軸である４，５，６軸の現在角度「θ_４，θ_５，θ_６」と、アーム先端の目標位置、つまり維持する位置「Ｘ，Ｙ，Ｚ」とが、逆変換入力パラメータとなる。

【0050】

ステップＳ３１０の実行後は、ステップＳ３２０に進む。ステップＳ３２０に進むと、コントローラ３は、逆変換計算（逆運動学計算）により、制御軸である１，２，３軸の指令角度「θ_１（ｋ＋１），θ_２（ｋ＋１），θ_３（ｋ＋１）」を算出する。ステップＳ３２０の実行後、第２指令算出処理が終了となる。第２指令算出処理が終了すると、コントローラ３は、算出された指令角度を用いて制御軸である１，２，３軸の位置制御（角度制御）を実行する。

【0051】

＜順変換処理＞
続いて、本実施形態における逆変換処理（逆変換計算）を説明するための前提として、順変換処理（順変換計算）について説明する。
まず、ｚ軸回転、ｚ軸移動、ｘ軸移動、ｘ軸回転の順の座標変換で、ＤＨパラメータを表１のように決定した。θ_ｉが、図２の状態からの各関節の回転角度になる。

【0052】

【表1】

【0053】

ベース座標であるΣ_０からメカニカルインターフェース座標Σ_６までの同次変換行列は次のようになる。ｎ，ｏ，ａは、それぞれノーマルベクトル，オリエントベクトル，アプローチベクトルを示す。なお、表記を簡略化するため、ｓｉｎθ_ｉ，ｃｏｓθ_ｉをそれぞれｓ_ｉ，ｃ_ｉと表記している。また、例えばＳ_２３はｓｉｎ（θ_２＋θ_３）を示す。

【0054】

【数1】

【0055】

【数2】

【0056】

（８）～（１０）式を展開すると、（１３）～（１５）式となる。

【0057】

【数3】

【0058】

また、第５軸，第６軸についてのノーマル，オリエント，アプローチの各ベクトルおよび位置座標については以下のようになる。

【0059】

【数4】

【0060】

同次変換行列であるから、ツール座標上での位置ベクトルに左からかけて積をとることで、ベース座標Σ_０での位置ベクトルに変換できる。つまり、関節角とＤＨパラメータとからベース座標での先端位置を求めることができる。姿勢角は、第６軸のノーマル，オリエント，アプローチの各ベクトルｎ_６，ｏ_６，ａ_６で表されている。

【0061】

＜第１指令算出処理における逆変換処理＞
次に、第１指令算出処理における逆変換計算（逆変換処理）の具体的な内容の一例について説明する。すなわち、ダイレクトティーチング中に姿勢のみを維持する機能を実現するための、１，２，３軸は外力に対して受動的に動く状態のときに４，５，６軸を角度制御する場合の角度指令値の計算方法の一例について説明する。
１，２，３軸角度および姿勢が同次変換行列（位置成分無視）で与えられているとする。つまり、（２０）式と位置成分を無視した（２１）式とは既知の数値となる。

【0062】

【数5】

【0063】

このとき、４，５，６軸を求めるには、Σ_３座標からΣ_６座標を求められればよいため、（２３）式と（２１）式とが等号になるように解けばよい。

【0064】

【数6】

【0065】

等号になるのは、（２４）式のときで、右辺は単純な行列演算になっている。（２４）式の右辺の計算結果を（２５）式とすれば、（２６）式を４，５，６軸角度について解けばよいことが分かる。

【0066】

【数7】

【0067】

まず、５軸角度を求めると２つの解が得られる。どちらが適切なのかは、４，５，６軸の可動範囲のチェックと全て解いた後で現在値（現在角度）との差分がトータルで小さくなる方を選ぶといった方法が考えられる。

【0068】

【数8】

【0069】

また、５軸角度は、次のように求めてもよい。

【0070】

【数9】

【0071】

下記の場合には、４，６軸は不定になるため、特異点となる。

【0072】

【数10】

【0073】

アプローチベクトルのＸＹ成分から４軸、ノーマルベクトルとオリエントベクトルから６軸を求めると、次のようになる。

【0074】

【数11】

【0075】

以上説明した逆変換計算は、手首オフセットｄ５が０ではない（ｄ５≠０）本実施形態のロボット２を対象としたものであるが、手首部分のオフセットが無い（ｄ５＝０）一般的なＰＵＭＡ型６軸ロボットにも適用することができる。なお、手首オフセットｄ５が０の場合には、（２６）式が（４２）式に変わるが、４，５，６軸の角度決定に無関係な部分しか変化しないため逆変換は変わらない。

【0076】

【数12】

【0077】

＜第２指令算出処理における逆変換処理＞
次に、第２指令算出処理における逆変換計算（逆変換処理）の具体的な内容の一例について説明する。すなわち、ダイレクトティーチング中にツール先端の位置のみを維持する機能を実現するための、４，５，６軸は外力に対して受動的に動く状態のときに１，２，３を角度制御する場合の角度指令値の計算方法の一例について説明する。
まず、フランジ中心からツール先端位置への同次変換行列を求める。（４３）式は、予めツール定義として与えられているため、既知の数値となる。

【0078】

【数13】

【0079】

さらに、ベース座標系からツール先端までの同次変換行列は、（４４）式のようになる。

【0080】

【数14】

【0081】

この後、^３Ｔ_６の要素が多数出てきて式が煩雑になるため、単独で求めて定数変数に置き換えておく。

【0082】

【数15】

【0083】

この場合、（４４）式の位置成分と４，５，６軸角度が与えられているため、下記の（４６）式～（５０）式から１，２，３軸角度が求められればよい。

【0084】

【数16】

【0085】

まず、１軸角度を求める。（４６）式を展開すると、それぞれ（５１）式、（５２）式、（５３）式のようになる。

【0086】

【数17】

【0087】

ここで、（５４）式のように置くと、ｘ_ｔ，ｙ_ｔは、１軸角度の回転行列を用いて、（５５）式のように簡単な形で表される。なお、この式に用いられているαｒ平面（αｒ座標）は、図９および図１０に示すように、ｘｙ軸を１軸角度だけ回転させた軸をとったものである。αｒ平面におけるｒ方向の成分は、１軸自体と４軸のひねり成分しか寄与せず、２軸と３軸は無関係である。さらに、この場合は４軸以降の角度は固定されているため、これまでの条件で１軸角度を求めることができる。

【0088】

【数18】

【0089】

ここで、（５４）式によってｒ_ｔが求められる。さらに式を変形して、（５６）式のようにα_ｔを求めることができる。

【0090】

【数19】

【0091】

続いて、（５５）式を工夫して変形していくと、（５７）式のように、１軸角度の三角関数であるｓ_１，ｃ_１が求められる。

【0092】

【数20】

【0093】

次に、２軸角度を求める。上述した通り、αｒ平面におけるｒ方向の成分は１軸と４軸しか寄与しないため、以降の計算ではｒ方向の成分を無視して、α方向の成分のみを考えることとする。

【0094】

【数21】

【0095】

図１０に示すようにロボット２を側面からみたαｚ平面で考えると、下記式が成立する。

【0096】

【数22】

【0097】

したがって、下記式からｚ_１２を導出することができる。

【0098】

【数23】

【0099】

式が煩雑になるため、下記のように置いておくこととする。

【0100】

【数24】

【0101】

ｚ_１２を消してα_１２について解く。（６２）式に（６８）式を代入し、２次方程式の形にして、解の公式でα_１２を求める。

【0102】

【数25】

【0103】

ｋ_２＜０の場合はリーチが届かないため解なしとなる。ｚ_１２も、式（６４）以降を、α_１２を消してｚ_１２について解く方針で進めれば同様に求められるし、あるいは、（６８）式に（７４）式を代入しても求められる。

【0104】

【数26】

【0105】

最後に３軸角度を求める。（６１）式、（６５）式、（６６）式を使って式を変形していくと、ｃ_２３，ｓ_２３が求められる。（８４）式を用いれば、１，２，３軸角度を求めることができる。ただし、ｎは各軸番号および２軸と３軸を足した角度を示す。

【0106】

【数27】

【0107】

以上説明した逆変換計算は、手首オフセットｄ５が０ではない（ｄ５≠０）本実施形態のロボット２を対象としたものであるが、手首部分のオフセットが無い（ｄ５＝０）一般的なＰＵＭＡ型６軸ロボットにも適用することができる。なお、手首オフセットｄ５が０の場合には、（４５）式において「ｄ５＝０」を代入して以降の計算を行うことで同様に実施することができる。

【0108】

以上説明したように、本実施形態によれば次のような効果が得られる。
コントローラ３は、アーム先端の姿勢を目標姿勢に維持しつつアーム先端の位置を変化させるようにダイレクトティーチングが行われる際、１，２，３軸を外力に従って自由に動作する状態とした操作軸に設定するとともに、４，５，６軸をコントローラ３により制御される状態とした制御軸に設定する。そして、コントローラ３は、ダイレクトティーチングが行われる際、操作軸の現在角度および目標姿勢を入力として逆運動学計算により制御軸の指令角度を算出する。

【0109】

このような構成によれば、ユーザの操作（ロボットに触れて動かすこと）に応じて操作軸が自由に動作するため、ユーザの意図通りにアーム先端の位置を変化させることができる。そして、この際、ユーザの操作に応じて動作する操作軸の現在角度およびアーム先端の目標姿勢に基づいて制御軸の指令角度が算出されるようになっているため、アーム先端の姿勢を目標姿勢に維持させることができる。

【0110】

このようなことから、上記構成によれば、ユーザは、特に難しい操作を必要とすることなく、アーム先端の姿勢を目標姿勢に保ちつつアーム先端の位置を変化させる、つまりアーム先端の姿勢を拘束したダイレクトティーチングを行うことができる。したがって、本実施形態によれば、ロボット２のダイレクトティーチングにおけるアーム先端の位置姿勢の調節操作を簡単化することができるという優れた効果が得られる。

【0111】

コントローラ３は、アーム先端の位置を目標位置に維持しつつアーム先端の姿勢を変化させるようにダイレクトティーチングが行われる際、４，５，６軸を外力に従って自由に動作する状態とした操作軸に設定するとともに、１，２，３軸をコントローラ３により制御される状態とした制御軸に設定する。そして、コントローラ３は、ダイレクトティーチングが行われる際、操作軸の現在角度および目標位置を入力として逆運動学計算により制御軸の指令角度を算出する。

【0112】

このような構成によれば、ユーザの操作（ロボットに触れて動かすこと）に応じて操作軸が自由に動作するため、ユーザの意図通りにアーム先端の姿勢を変化させることができる。そして、この際、ユーザの操作に応じて動作する操作軸の現在角度およびアーム先端の目標位置に基づいて制御軸の指令角度が算出されるようになっているため、アーム先端の位置を目標位置に維持させることができる。

【0113】

このようなことから、上記構成によれば、ユーザは、特に難しい操作を必要とすることなく、アーム先端の位置を目標位置に保ちつつアーム先端の姿勢を変化させる、つまりアーム先端の位置を拘束したダイレクトティーチングを行うことができる。したがって、本実施形態によれば、ロボット２のダイレクトティーチングにおけるアーム先端の位置姿勢の調節操作を簡単化することができるという優れた効果が得られる。

【0114】

また、本実施形態によれば、自由に操作可能な操作軸の現在角度に合わせて角度制御が有効な制御軸だけでアーム先端の姿勢または位置を目標姿勢または目標位置に一致させるように角度制御が行われることになるため、振動など不安定な動作が生じ難いという効果も得られる。さらに、上記構成によれば、力覚センサやトルクセンサなどのセンサを用いることなく、ロボット２のコントローラ３に元々設けられる角度制御のための制御系を用いることによりアーム先端の姿勢を拘束したダイレクトティーチングを実現するようになっているため、製造コストの低減にも寄与することができる。

【0115】

このような本実施形態により得られる効果は、従来技術と比較することで一層明確になる。そこで、本明細書において特許文献２として挙げた特開昭６０－２３３７０７号公報に開示された技術を比較例とし、その比較例に対する本実施形態の優位性を図１１および図１２を参照しながら説明する。なお、図１１および図１２では、アーム先端の姿勢を拘束したダイレクトティーチングを想定している。したがって、この場合、１，２，３軸（Ｊ１，Ｊ２，Ｊ３）が外力に従って自由に動作する状態の軸（操作軸に相当）に設定されるとともに、４，５，６軸（Ｊ４，Ｊ５，Ｊ６）が位置制御可能な軸（制御軸に相当）に設定される。

【0116】

まず、図１１（ａ）に示すような初期姿勢において、２，３軸（Ｊ２，Ｊ３）を畳むようにユーザによる操作が行われると、ロボットは、図１１（ｂ）に示すような状態となり、アーム先端の姿勢が目標姿勢である初期姿勢からずれる。ここで、比較例では、位置はそのままで姿勢だけ上書きして逆変換が行われるため、ロボットは、図１１（ｃ）に実線で示す状態となる。

【0117】

比較例では、上述した逆変換の結果に基づいて、図１１（ｃ）に破線で示すように、２軸（Ｊ２）と５軸（Ｊ５）を動作させてアーム先端の姿勢を初期姿勢に維持したいが、２軸（Ｊ２）は操作軸のため動作させることができず、５軸（Ｊ５）だけが動作してしまい、その結果、図１１（ｃ）に実線で示すように、アーム先端の姿勢が初期姿勢とは異なる姿勢になってしまう。

【0118】

その後も、比較例では、位置はそのままで姿勢だけ上書きして逆変換が行われるため、ロボットは、図１１（ｄ）に実線で示す状態となる。比較例では、上述した逆変換の結果に基づいて、図１１（ｄ）に破線で示すように、２軸（Ｊ２）と５軸（Ｊ５）を動作させてアーム先端の姿勢を初期姿勢に維持したいが、２軸（Ｊ２）は操作軸のため動作させることができず、５軸（Ｊ５）だけが動作してしまい、その結果、図１１（ｄ）に実線で示すように、アーム先端の姿勢が初期姿勢とは異なる姿勢になってしまう。

【0119】

比較例では、このような動作が繰り返され、アーム先端の姿勢を初期姿勢に維持することができない。これに対し、本実施形態では、操作軸の現在角度に合わせて角度制御が有効な制御軸である４，５，６軸だけでアーム先端の姿勢を目標姿勢である初期姿勢に一致させるように角度制御が行われることになるため、図１１（ｄ）に一点鎖線で示すように、５軸（Ｊ５）だけを動作させてアーム先端の姿勢を初期姿勢に維持することができる。

【0120】

また、図１２（ａ）に示すような初期姿勢において、２，３軸を伸ばすようにユーザによる操作が行われると、ロボットは、図１２（ｂ）に示すような状態となり、アーム先端の姿勢が目標姿勢である初期姿勢からずれる。この場合も、比較例では、位置はそのままで姿勢だけ上書きして逆変換を実行しようとするが、図１２（ｃ）に破線で示すようにアーム先端が届かない位置となるため逆変換が不可能となる。

【0121】

したがって、比較例では、図１２（ｂ）および図１２（ｃ）に示すように、アーム先端の姿勢を初期姿勢に維持することができない。これに対し、本実施形態では、操作軸の現在角度に合わせて角度制御が有効な制御軸である４，５，６軸だけでアーム先端の姿勢を目標姿勢である初期姿勢に一致させるように角度制御が行われることになるため、図１２（ｃ）に一点鎖線で示すように、５軸（Ｊ５）だけを動作させてアーム先端の姿勢を初期姿勢に維持することができる。

【0122】

以上説明した２つのケースのように、比較例では、操作軸が大きく動かされた結果、操作軸を大きく動かさなければならない指令値が出力されることがある。しかし、このとき、制御軸しか動かすことができないため、いつまで待っても誤差が収束せずに姿勢の維持が困難となる。比較例では、既存の逆変換をそのまま利用することができるものの、一発で厳密解が得られず、徐々に解が収束していくような動作になるが、上述した２つのケースのように、いつ収束するのか不明となる。この場合、解が収束するまでにおけるロボットの挙動について、どのような挙動となるのかを一切保証することができない。

【0123】

また、比較例では、ロボットの特異点近傍では解が不安定になり暴走する可能性があり、その安全性に問題がある。さらに、比較例では、目標の位置・姿勢に対して各軸角度の解が複数存在するため、それらをどのように選択するかが不明確である。つまり、比較例では、形態の選択をどのように行うか不明確であり、無理やり切り替えようとすると特異点を跨ぐために暴走する可能性がある。

【0124】

このように、比較例では、アーム先端の位置姿勢を維持できないケースが存在するばかりか、その安全性にも問題がある。これに対し、本実施形態では、維持したい位置または姿勢以外の目標値を未知数としたまま厳密な制御軸の目標値を算出するようになっているため、安全性を良好に維持しつつ、比較例ではアーム先端の位置姿勢を維持することができないケースであっても、アーム先端の位置姿勢を確実に維持することができる。

【0125】

上記構成では、ベース４から連続した３つの軸である１，２，３軸を根元側軸とし、アーム先端から連続した３つの軸である４，５，６軸を先端側軸とするように、各軸を切り分けている。このようにすれば、ユーザは、アーム先端の位置を拘束して姿勢を教示しようとした際、手先から連続する３つの軸を自身の力で操作できるため、自身が考える操作（アーム先端の姿勢を調節するべく手先に近い軸を動かす操作）と、実際の操作状態とが一致し易い。

【0126】

また、ユーザは、アーム先端の姿勢を拘束して位置を教示しようとした際、ベース４から連続する３つの軸を自身の力で操作できるため、自身が考える操作（アーム先端の位置を調節するべくベース４に近い軸を動かす操作）と、実際の操作状態とが一致し易い。したがって、このような各軸の切り分けによれば、ダイレクトティーチング作業において、ユーザに違和感を与えることが少なくなるという効果が得られる。

【0127】

（第２実施形態）
以下、第１実施形態に対してコントローラ３による制御の内容、つまりロボット２の制御方法が変更された第２実施形態について図１３～図１８を参照して説明する。
第１実施形態において、ダイレクトティーチングの実行中、ロボット２が特異点近傍を通過する場合、操作軸が急激に操作された場合などには、制御軸の指令角度の変化量が過大になることがあり、そうすると、制御軸の動作が急峻な動作となって安全性が低下するおそれがある。

【0128】

そこで、本実施形態では、このような点を改善するため、角度制御部として機能するコントローラ３により実行される処理である第１指令算出処理および第２指令算出処理の内容に変更が加えられている。本実施形態の第１指令算出処理は、例えば図１３に示すような内容の処理となる。この場合、ステップＳ２２０の実行後、ステップＳ２５０に進む。ステップＳ２５０に進むと、コントローラ３は、指令速度制限処理を実行する。また、本実施形態の第２指令算出処理は、例えば図１４に示すような内容の処理となる。この場合、ステップＳ３２０の実行後、ステップＳ３５０に進む。ステップＳ３５０に進むと、コントローラ３は、指令速度制限処理を実行する。

【0129】

指令速度制限処理の概要は、以下の通りである。まず、前提として、予めユーザ（使用者）が安全と判断する各関節（各軸）の最大角速度ω_ｍａｘと最大角加速度ａ_ｍａｘを定めておく。これらパラメータ（ω_ｍａｘ、ａ_ｍａｘ）の設定方法としては、例えば、実際にユーザがロボット２の挙動を確認しながらチューニングする方法などを挙げることができる。そして、コントローラ３は、制御軸の指令角度θ（ｋ＋１）が算出されると、各関節の指令角度θ（ｋ＋１）に向かって安全に加速する指令角度θ（ｋ＋１）′に補正する加速補正処理と、最終的に制御軸の角度を元の指令角度θ（ｋ＋１）に収束させるために減速する指令角度θ（ｋ＋１）″に補正する減速補正処理と、を実行する。

【0130】

すなわち、コントローラ３は、制御軸の指令角度θ（ｋ＋１）が算出されると、その指令角度θ（ｋ＋１）に基づいて算出される制御軸の指令角速度ω（ｋ＋１）および指令角加速度ａ（ｋ＋１）がそれぞれ予め定められた最大角速度ω_ｍａｘおよび最大角加速度ａ_ｍａｘを超えないように指令角度θ（ｋ＋１）を補正する加速補正処理を実行する。これにより、制御軸の角速度および角加速度がそれぞれ最大角速度ω_ｍａｘおよび最大角加速度ａ_ｍａｘ以下となる安全な指令角度θ（ｋ＋１）′が生成される。

【0131】

ただし、加速補正処理により補正された後の指令角度θ（ｋ＋１）′から最大減速で減速しても当初算出された指令角度θ（ｋ＋１）を超過する場合が考えられる。そこで、コントローラ３は、制御軸の角度が算出された指令角度θ（ｋ＋１）へと収束するように加速補正処理により補正された後の指令角度θ（ｋ＋１）′をさらに補正する減速補正処理を実行する。これにより、制御軸の角度が当初算出された指令角度θ（ｋ＋１）を超過する事態を防止することができる。

【0132】

このような指令速度制限処理の具体的な処理内容は、例えば図１５に示すようなものとなる。なお、以下の説明において用いる各記号の定義は、以下の通りとなっている。ただし、ｎは軸番号である。
θ_ｎ（ｋ）：前回指令角度
θ_ｎ（ｋ＋１）：指令角度
θ_ｎ（ｋ＋１）′：指令角度（最大加速）
θ_ｎ（ｋ＋１）″：指令角度（最大減速）
ω_ｎ（ｋ）：前回指令角速度
ω_ｎ（ｋ＋１）：指令角速度
ω_ｎ（ｋ＋１）′：指令角速度（最大加速）
ω_ｎ（ｋ＋１）″：指令角速度（最大減速）
ａ_ｎ（ｋ＋１）：指令角加速度
ａ_ｎ（ｋ＋１）′：指令角加速度（最大加速）

【0133】

まず、ステップＳ４０１では、指令角度θ_ｎ（ｋ＋１）が取得される。ステップＳ４０２では、下記（８５）式に基づいて指令角速度ω_ｎ（ｋ＋１）が計算される。
ω_ｎ（ｋ＋１）＝｛θ_ｎ（ｋ＋１）－θ_ｎ（ｋ）｝／ｄｔ …（８５）
ステップＳ４０３では、下記（８６）式に基づいて指令角加速度が計算される。
ａ_ｎ（ｋ＋１）＝｛ω_ｎ（ｋ＋１）－ω_ｎ（ｋ）｝／ｄｔ …（８６）

【0134】

ステップＳ４０４では、ステップＳ４０２で計算された指令角速度ω_ｎ（ｋ＋１）が最大角速度ω_ｎｍａｘ未満であるという条件およびステップＳ４０３で計算された指令角加速度ａ_ｎ（ｋ＋１）が最大角加速度ａ_ｎｍａｘ未満であるという条件の双方を満たすか否か、つまり下記（８７）式および（８８）式の双方を満たすか否かが判断される。
｜ω_ｎ（ｋ＋１）｜＜ω_ｎｍａｘ …（８７）
｜ａ_ｎ（ｋ＋１）｜＜ａ_ｎｍａｘ …（８８）

【0135】

ここで、上記（８７）式および（８８）式の双方を満たす場合、ステップＳ４０４で「ＹＥＳ」となり、指令速度制限処理が終了となる。つまり、この場合、速度制限が不要であるため、当初の指令角度θ_ｎ（ｋ＋１）に基づいて制御軸の角度制御が実行される。一方、上記（８７）式および（８８）式の一方または双方を満たさない場合、ステップＳ４０４で「ＮＯ」となり、ステップＳ４０５に進む。

【0136】

ステップＳ４０５では、下記（８９）式に基づいて指令角加速度が修正（補正）される。ただし、ｓｇｎは、その後の括弧内の数式の計算結果の符号を返す関数である。
ａ_ｎ（ｋ＋１）′＝ｓｇｎ｛θ_ｎ（ｋ＋１）－θ_ｎ（ｋ）｝・ａ_ｎｍａｘ …（８９）

【0137】

このように、ステップＳ４０５では、指令角加速度が最大角加速度ａ_ｎｍａｘに置き換えられる（角加速度が最大値に張り付けられる）。また、この場合、加速方向は、位置偏差から計算されることになる。

【0138】

ステップＳ４０６では、下記（９０）式に基づいて指令角速度が再計算される。つまり、ステップＳ４０６では、最大角加速度ａ_ｎｍａｘで加速した場合の指令角速度ω_ｎ（ｋ＋１）′が求められる。
ω_ｎ（ｋ＋１）′＝ａ_ｎ（ｋ＋１）′・ｄｔ＋ω_ｎ（ｋ） …（９０）

【0139】

ステップＳ４０７では、ステップＳ４０６で再計算された指令角速度ω_ｎ（ｋ＋１）′が最大角速度ω_ｎｍａｘ以下であるか否か、つまり下記（９１）を満たすか否かが判断される。
｜ω_ｎ（ｋ＋１）′｜≦ω_ｎｍａｘ …（９１）

【0140】

ここで、指令角速度ω_ｎ（ｋ＋１）′が最大角速度ω_ｎｍａｘ以下である場合、ステップＳ４０７で「ＹＥＳ」となり、ステップＳ４０８を実行することなく、ステップＳ４０９に進む。一方、指令角速度ω_ｎ（ｋ＋１）′が最大角速度ω_ｎｍａｘを超える場合、ステップＳ４０７で「ＮＯ」となり、ステップＳ４０８に進む。ステップＳ４０８では、下記（９２）式に基づいて指令角速度が修正（補正）される。
ω_ｎ（ｋ＋１）′＝ｓｇｎ｛ω_ｎ（ｋ＋１）′｝・ω_ｎｍａｘ …（９２）
このように、ステップＳ４０８では、指令角速度が最大角速度ω_ｎｍａｘに置き換えられる（角速度が最大値に張り付けられる）。

【0141】

ステップＳ４０９では、下記（９３）式に基づいて指令角度が再計算される。つまり、ステップＳ４０９では、最大角加速度ａ_ｎｍａｘで加速した場合の指令角度θ_ｎ（ｋ＋１）′が求められる。
θ_ｎ（ｋ＋１）′＝ω_ｎ（ｋ＋１）′・ｄｔ＋θ_ｎ（ｋ） …（９３）

【0142】

指令角度は、ユーザによる操作軸の操作に応じて動的に変化しているため、指令角度が現在角度に近づいてくることがある。このとき、減速が間に合わずに指令角速度の方向と現在角度から指令角度へと向かう方向とが逆になることがある。そこで、ステップＳ４１０では、下記（９４）式を満たすか否かが判断される。すなわち、ステップＳ４１０では、位置偏差の符号から指令角度へ向かう方向（下記式の右辺）を割り出し、その方向が指令角速度ω_ｎ（ｋ＋１）′の方向（下記式の左辺）と一致するかどうかが判断される。
ｓｇｎ｛ω_ｎ（ｋ＋１）′｝＝ｓｇｎ｛θ_ｎ（ｋ＋１）－θ_ｎ（ｋ）｝ …（９４）

【0143】

ここで、上記（９４）式を満たさない場合、ステップＳ４１０で「ＮＯ」となり、ステップＳ４１１に進む。この場合、指令角度から離れようとしていると考えられる。そのため、ステップＳ４１１において、指令角度に向かって最大加速するように、指令角度がθ_ｎ（ｋ＋１）′に更新されるとともに、指令角速度がω_ｎ（ｋ＋１）′に更新される。ステップＳ４１１の実行後は、指令速度制限処理が終了となる。

【0144】

一方、上記（９４）式を満たす場合、ステップＳ４１０で「ＹＥＳ」となり、ステップＳ４１２に進む。この場合、減速が間に合うかどうか確認する必要がある。そこで、ステップＳ４１２では、下記（９５）式に基づいて、指令角度から最大減速した場合の移動量Δθが計算される。
Δθ＝｛ω_ｎ（ｋ＋１）′｝^２／｛２ａ_ｎ（ｋ＋１）′｝ …（９５）

【0145】

ステップＳ４１３では、下記（９６）式に基づいて、指令角度までの残りの距離（下記式の右辺）が減速停止に必要な距離（下記式の左辺）以下であるか否かが判断される。
｜Δθ｜≧｜θ_ｎ（ｋ＋１）－θ_ｎ（ｋ＋１）′｜ …（９６）

【0146】

ここで、上記（９６）式の右辺が左辺よりも大きい場合、ステップＳ４１３で「ＮＯ」となり、ステップＳ４１１に進む。この場合、減速できる区間が十分にあるため、ステップＳ４１１において、指令角度に向かって最大加速するように、指令角度および指令角速度が更新される。一方、上記（９６）式の右辺が左辺以下である場合、ステップＳ４１３で「ＹＥＳ」となり、ステップＳ４１４に進む。

【0147】

この場合、移動した先から最大減速しても指令角度をオーバーしてしまうと考えられる。つまり、この場合、このまま加速すると指令角度を超過することになるため、直ちに減速を始める必要がある。そこで、ステップＳ４１４では、下記（９７）式に基づいて、最大減速速度が求められる。
ω_ｎ（ｋ＋１）″＝ω_ｎ（ｋ）－ａ_ｎｍａｘ・ｄｔ …（９７）

【0148】

ステップＳ４１５では、ステップＳ４１４で再計算された指令角速度ω_ｎ（ｋ＋１）″が最大角速度ω_ｎｍａｘ以下であるか否か、つまり下記（９８）を満たすか否かが判断される。
｜ω_ｎ（ｋ＋１）″｜≦ω_ｎｍａｘ …（９８）

【0149】

ここで、指令角速度ω_ｎ（ｋ＋１）″が最大角速度ω_ｎｍａｘ以下である場合、ステップＳ４１５で「ＹＥＳ」となり、ステップＳ４１６を実行することなく、ステップＳ４１７に進む。一方、指令角速度ω_ｎ（ｋ＋１）″が最大角速度ω_ｎｍａｘを超える場合、ステップＳ４１５で「ＮＯ」となり、ステップＳ４１６に進む。ステップＳ４１６では、下記（９９）式に基づいて指令角速度が修正（補正）される。
ω_ｎ（ｋ＋１）″＝ｓｇｎ｛ω_ｎ（ｋ＋１）″｝・ω_ｎｍａｘ …（９９）
このように、ステップＳ４１６では、指令角速度が最大角速度ω_ｎｍａｘに置き換えられる（角速度が最大値に張り付けられる）。

【0150】

ステップＳ４１７では、下記（１００）式に基づいて指令角度が再計算される。
θ_ｎ（ｋ＋１）″＝ω_ｎ（ｋ＋１）″・ｄｔ＋θ_ｎ（ｋ） …（１００）
ステップＳ４１８では、最大減速するように、指令角度がθ_ｎ（ｋ＋１）″に更新されるとともに、指令角速度がω_ｎ（ｋ＋１）″に更新される。ステップＳ４１８の実行後は、指令速度制限処理が終了となる。

【0151】

続いて、ダイレクトティーチング中に指令速度制限処理が実行されることにより各指令値（指令角度θ_ｎ（ｔ）、指令角速度ω_ｎ（ｔ）、指令角加速度ａ_ｎ（ｔ））が修正（補正）される様子の一例について図１６～図１８を参照しながら説明する。この場合、図１６（ａ）に示すように、今回（「ｋ＋１」の時点）与えられた指令角度θ_ｎ（ｋ＋１）は、前回（「ｋ」の時点）の指令角度θ_ｎ（ｋ）から大きく変化した値になっているものとする。

【0152】

このような指令角度θ_ｎ（ｋ＋１）に基づいて計算された指令角速度ω_ｎ（ｋ＋１）および指令角加速度ａ_ｎ（ｋ＋１）は、図１６（ｂ）、（ｃ）に示すように、それぞれ最大角速度ω_ｎｍａｘおよび最大角加速度ａ_ｎｍａｘを超過した値となる。そこで、図１６（ｄ）に示すように、指令角加速度ａ_ｎ（ｔ）が最大角加速度ａ_ｎｍａｘに制限される。そして、このように制限された指令角加速度ａ_ｎ（ｔ）に基づいて指令角速度ω_ｎ（ｔ）が再計算される。ここで、図１６（ｅ）に示すように、再計算された指令角速度ω_ｎ（ｋ＋１）′が最大角速度ω_ｎｍａｘを超過する場合、指令角速度ω_ｎ（ｔ）が最大角速度ω_ｎｍａｘに制限される。

【0153】

その後、このように制限が行われた指令角速度ω_ｎ（ｋ＋１）に基づいて、指令角度θ_ｎ（ｔ）が再計算される。これにより、図１６（ｆ）に示すような指令角度θ_ｎ（ｋ＋１）′が生成される。この指令角度θ_ｎ（ｋ＋１）′は、角速度と角加速度が最大値以下となる安全な指令値となる。ここまでの処理が、加速補正処理に相当する手順となる。上述した通り、加速補正処理では、与えられた指令角度から角速度・角加速度を計算してリミットチェックが行われ、角速度・角加速度を制限して安全な指令角度に補正される。

【0154】

次に、図１７（ａ）に示すように、初速（＝指令角速度ω_ｎ（ｋ＋１）′）、最大角加速度ａ_ｎｍａｘで、減速停止するまでの距離Δθを計算し、元の指令角度θ_ｎ（ｋ＋１）を超過しないかどうかがチェックされる。この場合、図１７（ａ）に破線で示すように、このまま加速すると、その後に減速しきれず、元の指令角度θ_ｎ（ｋ＋１）をオーバーしてしまう。

【0155】

そこで、図１７（ｂ）に示すように、直ちに減速を開始するように、指令角速度ω_ｎ（ｔ）を下げ始める。つまり、この場合、図１７（ｃ）に示すように、指令角度θ_ｎ（ｔ）が、指令角加速度ａ_ｎ（ｔ）を－ａ_ｎｍａｘとして最大減速するような指令角度θ_ｎ（ｋ＋１）″に補正される。ここまでの処理が、減速補正処理に相当する手順となる。上述した通り、減速補正処理では、加速補正後の指令値に基づいて、元の指令角度で止まりきれるかどうかチェックが行われ、元の指令角度で止まりきれない場合は直ちに減速が開始されるような指令角度に補正される。

【0156】

このような処理全体の様子、つまり上記各処理が実行されることに伴う指令角度θ_ｎ（ｔ）の変化の様子は、図１８に示すようになる。なお、図１８において、時刻ｔ１～ｔ２の期間Ｔａは速度制限がされていない期間であり、時刻ｔ２～ｔ３の期間Ｔｂは加速補正が行われている期間であり、時刻ｔ３～ｔ４の期間Ｔｃは減速補正が行われている期間である。図１８に示すように、時刻ｔ２において前回値から大きく変化するような指令値（指令角度）が発生すると、期間Ｔｂにおいて安全な角速度・角加速度となるように指令角度が補正される。その後、期間Ｔｃにおいて元の指令角度で止まりきれないことが分かると、減速を始めるように指令角度が補正される。その結果、指令角度の変化量が大きくなった場合でも、安全な角速度・角加速度でもって指令角度に到達することができる。

【0157】

以上説明したように、本実施形態のコントローラ３は、制御軸の指令角度θ_ｎ（ｋ＋１）が算出されると、その指令角度に基づいて算出される制御軸の指令角速度ω_ｎ（ｋ＋１）および指令角加速度ａ_ｎ（ｋ＋１）がそれぞれ最大角速度ω_ｎｍａｘおよび最大角加速度ａ_ｎｍａｘを超えないように指令角度を補正する加速補正処理を実行する。これにより、制御軸の角速度および角加速度がそれぞれ最大角速度ω_ｎｍａｘおよび最大角加速度ａ_ｎｍａｘ以下となる安全な指令角度θ_ｎ（ｋ＋１）′が生成される。

【0158】

ただし、加速補正処理により補正された後の指令角度θ_ｎ（ｋ＋１）′から最大減速で減速しても当初算出された指令角度θ_ｎ（ｋ＋１）を超過する場合が考えられる。そこで、コントローラ３は、制御軸の角度が指令角度θ_ｎ（ｋ＋１）へと収束するように指令角度θ_ｎ（ｋ＋１）′をさらに補正する減速補正処理を実行する。これにより、制御軸の角度が指令角度θ_ｎ（ｋ＋１）を超過する事態を防止することができる。

【0159】

このようにすれば、ダイレクトティーチングの実行中にロボット２が特異点近傍を通過したり、操作軸が急激に操作されたりすることによって制御軸の指令角度θ_ｎ（ｋ＋１）の変化量が過大になった場合でも、制御軸の角速度および角加速度をそれぞれ最大角速度ω_ｎｍａｘおよび最大角加速度ａ_ｎｍａｘ以下に抑えつつ、つまり安全な加速をしつつ、制御軸の角度を当初算出された指令角度θ_ｎ（ｋ＋１）に精度良く到達させることができる。

【0160】

なお、上述した加速補正処理などが行われる場合、制御軸の動作は、ユーザの操作による操作軸の動作に対して遅れた動作となる。しかしながら、このような遅れは、ロボット２のダイレクトティーチングにおいて問題とはならない。なぜなら、一般に、ロボットのダイレクトティーチングでは、ユーザがロボットに触れて動かし、その後、ロボットを停止させた状態で、アーム先端の位置や姿勢が所望する状態となっているかを確認する。このような動作が繰り返されることにより、所望する教示が行われる。つまり、ロボット２のダイレクトティーチングにおいて、ユーザがロボット２を動かし続けることはなく、必ず、ロボット２を停止させる期間が存在する。

【0161】

本実施形態の制御では、操作軸の動作に対して制御軸の動作が遅れるが、その遅れ分は、ロボット２が停止されている期間に取り戻すことができる。したがって、ロボット２のダイレクトティーチングでは、上述した制御による制御軸の動作の遅れは、問題とはならない。このようなことから、本実施形態では、制御軸の動作が遅れるような制御を敢えて採用しており、それにより、ロボット２の安全性の向上を図るようになっている。

【0162】

（第３実施形態）
以下、第２実施形態に対してコントローラ３による制御の内容、つまりロボット２の制御方法が変更された第３実施形態について図１９～図２１を参照して説明する。

【0163】

第２実施形態によれば、制御軸の動作が急峻な動作となることは回避できるものの、ロボット２の特異点近傍において各軸の移動量が大きくなる現象、つまり遠く離れた関節角度に移動しようとする現象は避けることができない。ダイレクトティーチング中、ユーザは、ロボット２に近付いた位置から操作軸の操作を行うことになるため、特異点近傍において各軸の移動量が大きくなる現象が発生すると、ロボット２のアームなどがユーザに接触する可能性があり、その結果、安全性が低下するおそれがある。

【0164】

そこで、本実施形態では、このような点を改善するため、角度制御部として機能するコントローラ３により実行される処理である第１指令算出処理および第２指令算出処理の内容に変更が加えられている。本実施形態の第１指令算出処理は、例えば図１９に示すような内容の処理であり、第２実施形態の第１指令算出処理に対し、ステップＳ２３０およびＳ２４０が追加されている。

【0165】

この場合、ステップＳ２２０の実行後、ステップＳ２３０に進む。ステップＳ２３０に進むと、コントローラ３は、ロボット２が特異点近傍であるか否かを検出する。なお、特異点近傍であることは、例えば、ヤコビアンが０になる各軸角度、５軸０度付近などにより検出することが可能である。ここで、特異点近傍であることが検出されない場合、ステップＳ２３０で「ＮＯ」となり、ステップＳ２４０を実行することなく、ステップＳ２５０に進む。

【0166】

一方、特異点近傍であることが検出されると、ステップＳ２３０で「ＹＥＳ」となり、ステップＳ２４０に進む。ステップＳ２４０に進むと、コントローラ３は、今回の指令角度を、前回の指令角度で固定（ロック）する。この場合、今回の指令角度θ_４（ｋ＋１）、θ_５（ｋ＋１）、θ_６（ｋ＋１）が、それぞれ前回の指令角度θ_４（ｋ）、θ_５（ｋ）、θ_６（ｋ）に置換される。ステップＳ２４０の実行後は、ステップＳ２５０に進む。

【0167】

本実施形態の第２指令算出処理は、例えば図２０に示すような内容の処理であり、第２実施形態の第２指令算出処理に対し、ステップＳ３３０およびＳ３４０が追加されている。この場合、ステップＳ３２０の実行後、ステップＳ３３０に進む。ステップＳ３３０に進むと、コントローラ３は、ロボット２が特異点近傍であるか否かを検出する。ここで、特異点近傍であることが検出されない場合、ステップＳ３３０で「ＮＯ」となり、ステップＳ３４０を実行することなく、ステップＳ３５０に進む。

【0168】

一方、特異点近傍であることが検出されると、ステップＳ３３０で「ＹＥＳ」となり、ステップＳ３４０に進む。ステップＳ３４０に進むと、コントローラ３は、今回の指令角度を、前回の指令角度で固定（ロック）する。この場合、今回の指令角度θ_１（ｋ＋１）、θ_２（ｋ＋１）、θ_３（ｋ＋１）が、それぞれ前回の指令角度θ_１（ｋ）、θ_２（ｋ）、θ_３（ｋ）に置換される。ステップＳ３４０の実行後は、ステップＳ３５０に進む。

【0169】

本実施形態において、ステップＳ２３０およびＳ３３０の処理は、ロボット２が特異点近傍であるか否かを検出する特異点検出処理に相当する。また、本実施形態において、ステップＳ２４０およびＳ３４０の処理は、特異点検出処理により特異点近傍であることが検出されると、その時点以降の指令角度を、前回の指令角度に置換する指令角度置換処理に相当する。

【0170】

続いて、ダイレクトティーチング中に特異点検出処理、指令角度置換処理および指令速度制限処理が実行されることにより、特異点侵入時に制御軸の指令値（指令角度θ_ｎ（ｔ））が修正（補間）される様子の一例について図２１を参照しながら説明する。図２１において、時刻ｔ１１～ｔ１２の期間Ｔｄは特異点ではない正常な期間であり、時刻ｔ１２～ｔ１３の期間Ｔｅは特異点である期間であり、時刻ｔ１３～ｔ１４の期間Ｔｆは加速補正が行われている期間であり、時刻ｔ１４～ｔ１５の期間Ｔｇは減速補正が行われている期間である。

【0171】

なお、図２１において、（ａ）は、特異点検出処理および指令角度置換処理が実行されない場合を表し、（ｂ）は、特異点検出処理および指令角度置換処理が実行される場合を表している。この場合、時刻ｔ１２の時点にて、ロボット２が特異点に侵入したことが検出されると、つまり特異点が検出されると、図２１（ａ）に示すように、その時点以降の指令角度θ_ｎ（ｔ）が特異点検出時の指令角度にロック（固定）される。

【0172】

ただし、このとき、指令角度θ_ｎ（ｔ）は、図２１（ｂ）に示すように、指令速度制限処理が実行されることにより、特異点検出時の指令角度に向けて緩やかに変化する。時刻ｔ１２～時刻ｔ１３の期間Ｔｅでは、指令角度がロックされるため、制御軸の角度制御による位置・姿勢の維持が一時的に中断される。このとき、例えばティーチングペンダントなどの表示デバイスにメッセージを表示させたり、ロボット２の筐体などに設けられたＬＥＤなどの点灯状態や色を変化させたりすることによって、ユーザに対して特異点近傍であることと、位置・姿勢の維持が中断されていることとを報知するとよい。このようにすれば、ユーザは、ロボット２の現在の状況を正確に知ることができる。

【0173】

その後、時刻ｔ１３の時点にて、ユーザの操作によってロボット２が特異点を脱出すると、時刻ｔ１３～時刻ｔ１５において加速補正処理および減速補正処理が行われることにより、制御軸の角速度が低減されるように指令角度の補正が行われる。図２１（ｂ）に示すように、特異点検出処理および指令角度置換処理が実行される場合、これらの処理が実行されない場合（図２１（ａ）参照）に対し、指令角度θ_ｎ（ｔ）が遅れて変化するような挙動となる。

【0174】

以上説明したように、本実施形態のコントローラ３は、ロボット２が特異点近傍に突入すると、その時点以降の指令角度を前回の指令角度（特異点検出時の指令角度）で固定するという処理を実行するようになっている。このようにすれば、ロボット２の特異点近傍において各軸の移動量が大きくなる現象の発生が抑制され、その結果、安全性を向上させることができる。

【0175】

この場合、制御軸の角度制御による位置・姿勢の維持が一時的に中断されることになるが、操作軸の操作に応じて特異点近傍を脱出することができれば、その後は再び制御軸の角度制御による位置・姿勢の維持が再開される。また、上述したようにロボット２が特異点近傍に突入すると指令角度を前回の指令角度でロックする処理を実行して制御軸の角度制御による位置・姿勢の維持を一時的に中断させることにより、加速補正処理などによる制御軸の遅れ分を取り戻すことが可能となる。つまり、本実施形態によれば、特異点近傍におけるロボット２の安全性を向上させつつ、加速補正処理などが行われた場合における制御軸の動作の遅れを取り戻すことができる。

【0176】

（その他の実施形態）
なお、本発明は上記し且つ図面に記載した実施形態に限定されるものではなく、その要旨を逸脱しない範囲で任意に変形、組み合わせ、あるいは拡張することができる。
上記実施形態で示した数値などは例示であり、それに限定されるものではない。

【0177】

本発明は、一般的な産業用ロボットシステムであるロボットシステム１に用いられるものに限らず、垂直多関節型のロボットを制御するものであり且つダイレクトティーチングに対応したロボットの制御全般に適用することができる。
コントローラ３を、ロボット２のベース４の外に配置してもよい。

【0178】

先端側軸としては、４，５，６軸に限らず、例えば３，４，６軸など、アーム先端の姿勢を決定するうえで支配的な要因となり得る軸であればよい。また、根元側軸としては、１，２，３軸に限らず、例えば１，２，５軸など、アーム先端の位置を決定するうえで支配的な要因となり得る軸であればよい。また、これらの先端側軸および根元側軸の切り分けは、制御対象となるロボットの仕様に応じて適宜変更すればよい。

【符号の説明】

【0179】

１…ロボットシステム、２…ロボット、３…コントローラ、Ｊ１，Ｊ２，Ｊ３…軸、Ｊ４，Ｊ５，Ｊ６…軸。

【図1】

【図2】

【図3】

【図4】

【図5】

【図6】

【図7】

【図8】

【図9】

【図10】

【図11】

【図12】

【図13】

【図14】

【図15】

【図16】

【図17】

【図18】

【図19】

【図20】

【図21】