(57)【要約】【課題】角度伝達誤差を補償する際には、ロボット個体ごとの調整や、外界センサが必要であった。【解決手段】速度指令生成部１２は、モータの現在位置と位置指令を用いて速度指令を生成する。速度算出部１５は、モータの現在速度を算出する。電流指令生成部１３は、モータの現在速度と速度指令を用いて電流指令を生成する。適応推定部１６は、現在速度を用いて、減速機の角度伝達誤差を示す周期的な関数の周期・振幅・位相を適応的に推定することで角度伝達誤差を生成する。誤差補償部１８は、その角度伝達誤差を用い、電流指令から減速機の角度伝達誤差成分を補償した電流指令を生成する。トルク指令生成部１４は、モータの現在電流と補償後の電流指令を用いてトルク指令を生成し、モータに出力する。このような構成により、角度伝達誤差を補償する際に、ロボット個体ごとの調整や外界センサが必要なくなる。【選択図】図１
2014136260.tif 000002
【特許請求の範囲】
【請求項１】
減速機を介してモータによって駆動される関節によって連結された複数のアームを有するマニピュレータを制御する制御装置であって、
前記モータの現在位置と、当該モータの位置指令とを用いて速度指令を生成する速度指令生成部と、
前記モータの現在位置から、当該モータの現在速度を算出する速度算出部と、
前記速度算出部が算出した現在速度と、前記速度指令とを用いて電流指令を生成する電流指令生成部と、
前記速度算出部が算出した現在速度を用いて、前記減速機の角度伝達誤差を示す周期的な関数の周期・振幅・位相を適応的に推定することによって、当該角度伝達誤差を生成する適応推定部と、
前記適応推定部が生成した角度伝達誤差を用いて、前記電流指令から前記減速機の角度伝達誤差成分を補償した電流指令を生成する誤差補償部と、
前記モータの現在電流と、前記誤差補償部が生成した電流指令とを用いてトルク指令を生成し、前記モータに出力するトルク指令生成部と、を備えた制御装置。
【請求項２】
前記周期的な関数は、三角関数である、請求項１記載の制御装置。
【請求項３】
前記適応推定部は、
前記モータの速度と、位相の余弦値とを乗算する第１の乗算器と、
前記モータの速度と、位相の正弦値の負値とを乗算する第２の乗算器と、
前記第１及び第２の乗算器の乗算結果を各要素に有するベクトルに、前記モータの軸に関する伝達関数に対応する伝達関数行列に応じた演算を行うことによって、前記振幅及び前記周波数を各要素に有するベクトルを生成する行列乗算器と、
前記行列乗算器が生成した周波数を積分した位相を算出する積分器と、
前記積分器が算出した位相の余弦値を算出し、前記第１の乗算器に入力する余弦算出器と、
前記積分器が算出した位相の正弦値の負値を算出し、前記第２の乗算器に入力する正弦算出器と、
前記行列乗算器が生成した振幅と、前記余弦算出器が算出した余弦値とを乗算することによって角度伝達誤差を生成する第３の乗算器と、を備えた、請求項２記載の制御装置。
【請求項４】
前記適応推定部が生成した角度伝達誤差に０より大きく１以下のパラメータを乗算することによって、前記角度伝達誤差を変更する変更部をさらに備え、
前記誤差補償部は、前記電流指令から、前記変更部による変更後の角度伝達誤差成分を補償した電流指令を生成する、請求項１から請求項３のいずれか記載の制御装置。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、減速機の角度伝達誤差を補償する制御装置に関する。
【背景技術】
【０００２】
アーク溶接ロボット等のマニピュレータの先端を直線動作させた際に周期的な振動が発生し、制御の精度に悪影響を及ぼすことがある。その振動は、マニピュレータの各軸に設けられた減速機の角度伝達誤差に起因すると考えられている。そのような角度伝達誤差を補償する方法として、角度伝達誤差を数式でモデル化し、補償する方法が知られている。その方法として、例えば、特許文献１には、角度伝達誤差を正弦波でモデル化し、その振幅と位相をモータへのトルク指令値、モータ角度指令、ロボットの手先位置から同定する方法が記載されている。また、特許文献２には、モータの回転方向とロボットの負荷重量を考慮して角度伝達誤差補正量を決定する方法が記載されている。また、特許文献３には、角度伝達誤差を、減速機のギア比変動、ギアの摩擦、ギア出力角変動の３つの要素でモデル化し、それらを考慮した制御系設計法が記載されている。
【０００３】
また、角度伝達誤差を補償するための他の方法として、角度伝達誤差の実測値を用いて補償を行う方法も知られている。その方法として、特許文献４には、減速時における減速機の動力伝達特性を記憶し、その情報をモータへの入力トルクに反映することで誤差を補償する方法が記載されている。
【先行技術文献】
【特許文献】
【０００４】
【特許文献１】特開２０１１−２１２８２３号公報
【特許文献２】特開２０１０−１４９２０３号公報
【特許文献３】特開平１０−２４３６７６号公報
【特許文献４】特開平１−２１０６４３号公報
【発明の概要】
【発明が解決しようとする課題】
【０００５】
特許文献４に記載されている実測値を用いるアプローチでは、ロボットごと、または減速機単体ごとの動力伝達特性の実測値が必要であるため、データ測定のコストが大きく、現実的でないという問題がある。
【０００６】
一方、特許文献１〜３に記載されているモデル化のアプローチでは、実測値を用いるよりもデータ測定にかかるコストは少なくなる。しかしながら、数式モデルが複雑になりすぎるという問題や、個体ごとのパラメータ同定作業に時間がかかるという問題がある。具体的には、特許文献１では、角度伝達誤差を正弦波でモデル化しているが、その振幅と位相を同定するために、ロボットの手先位置を計測するための外界センサが必要である。また、ロボットの個体ごとに測定を行う必要があることから、パラメータ同定のコストが大きいという問題もある。また、特許文献２には、ロボットの負荷重量とモータ回転方向の変化による角度伝達誤差のうねり振幅の変化を補正する方法が記載されている。この方法では、負荷重量と振幅の関係のマップを作るため、複数の負荷重量パターンでロボットを動作させる必要があり、パラメータ同定のコストが大きいという問題がある。また、特許文献３では、角度伝達誤差を３つの要素に分け、それぞれを数式でモデル化しているが、モデルが複雑であるために同定が容易ではない。また、うねりの振幅が変化することを考慮していないモデルとなっており、十分な補正効果が得られない可能性があるという問題がある。
【０００７】
本発明は、上記課題を解決するためになされたものであり、ロボット個体ごとの調整を必要とせず、外界センサを用いずに角度伝達誤差を補償することができる制御装置を提供することを目的とする。
【課題を解決するための手段】
【０００８】
上記目的を達成するため、本発明による制御装置は、減速機を介してモータによって駆動される関節によって連結された複数のアームを有するマニピュレータを制御する制御装置であって、モータの現在位置と、モータの位置指令とを用いて速度指令を生成する速度指令生成部と、モータの現在位置から、モータの現在速度を算出する速度算出部と、速度算出部が算出した現在速度と、速度指令とを用いて電流指令を生成する電流指令生成部と、速度算出部が算出した現在速度を用いて、減速機の角度伝達誤差を示す周期的な関数の周期・振幅・位相を適応的に推定することによって、角度伝達誤差を生成する適応推定部と、適応推定部が生成した角度伝達誤差を用いて、電流指令から減速機の角度伝達誤差成分を補償した電流指令を生成する誤差補償部と、モータの現在電流と、誤差補償部が生成した電流指令とを用いてトルク指令を生成し、モータに出力するトルク指令生成部と、を備えたものである。
このような構成により、減速機の角度伝達誤差を振幅・周波数・位相が未知の周期的な関数でモデル化することによって、モータの速度を用いるだけで適応的に未知パラメータを推定でき、角度伝達誤差を補償することができる。その結果、ロボット個体ごとの調整を必要とせず、また外界センサを用いずに角度伝達誤差を補償することができる。また、従来の数式モデルを用いたアプローチで問題となっていたパラメータの同定コストを低減させることができる。
【０００９】
また、本発明による制御装置では、周期的な関数は、三角関数であってもよい。
このような構成により、周期的な関数の解析が容易となる。
【００１０】
また、本発明による制御装置では、適応推定部は、モータの速度と、位相の余弦値とを乗算する第１の乗算器と、モータの速度と、位相の正弦値の負値とを乗算する第２の乗算器と、第１及び第２の乗算器の乗算結果を各要素に有するベクトルに、モータの軸に関する伝達関数に対応する伝達関数行列に応じた演算を行うことによって、振幅及び周波数を各要素に有するベクトルを生成する行列乗算器と、行列乗算器が生成した周波数を積分した位相を算出する積分器と、積分器が算出した位相の余弦値を算出し、第１の乗算器に入力する余弦算出器と、積分器が算出した位相の正弦値の負値を算出し、第２の乗算器に入力する正弦算出器と、行列乗算器が生成した振幅と、余弦算出器が算出した余弦値とを乗算することによって角度伝達誤差を生成する第３の乗算器と、を備えてもよい。
このような構成により、行列乗算器等を用いることによって、適応的に未知パラメータを推定することができる。
【００１１】
また、本発明による制御装置では、適応推定部が生成した角度伝達誤差に０より大きく１以下のパラメータを乗算することによって、角度伝達誤差を変更する変更部をさらに備え、誤差補償部は、電流指令から、変更部による変更後の角度伝達誤差成分を補償した電流指令を生成してもよい。
このような構成により、変更部が用いるパラメータを適切に設定することによって、角度伝達誤差の効果的な補償を実現することができるようになる。
【発明の効果】
【００１２】
本発明による制御装置によれば、ロボット個体ごとの調整を必要とせず、外界センサを用いないで、角度伝達誤差の補償を行うことができる。
【図面の簡単な説明】
【００１３】
【図１】本発明の実施の形態１による産業用ロボットの構成を示すブロック図
【図２】同実施の形態による適応推定部の構成を示すブロック図
【図３】同実施の形態による制御装置の動作を示すフローチャート
【発明を実施するための形態】
【００１４】
以下、本発明による制御装置について、実施の形態を用いて説明する。なお、以下の実施の形態において、同じ符号を付した構成要素及びステップは同一または相当するものであり、再度の説明を省略することがある。
【００１５】
（実施の形態１）
本発明の実施の形態１による制御装置について、図面を参照しながら説明する。本実施の形態による制御装置は、角度伝達誤差を周期的な関数でモデル化し、その関数の振幅・周波数・位相を適応的に推定することによって、角度伝達誤差を補償するものである。
【００１６】
図１は、本実施の形態による産業用ロボット１００の構成を示すブロック図である。本実施の形態による産業用ロボット１００は、制御装置１と、マニピュレータ２とを備える。制御装置１は、モータによって駆動される関節によって連結された複数のアームを有するマニピュレータ２を制御する。そのマニピュレータ２において、各モータとアームとは減速機を介して接続されている。また、直列に接続されたアームの先端に、手先効果器（エンドエフェクタ）が設けられていてもよい。マニピュレータ２は、例えば、垂直多関節ロボットのマニピュレータであってもよく、水平多関節ロボットのマニピュレータであってもよい。また、そのマニピュレータ２の軸数は問わない。また、制御装置１は、ティーチングプレイバック方式によりマニピュレータ２を制御してもよく、または、自律方式によりマニピュレータ２を制御してもよい。本実施の形態による産業用ロボット１００は、例えば、搬送ロボットであってもよく、溶接ロボットであってもよく、組立ロボットであってもよく、塗装ロボットであってもよく、あるいは、その他の用途のロボットであってもよい。
【００１７】
図１で示されるように、制御装置１は、位置指令生成部１１と、速度指令生成部１２と、電流指令生成部１３と、トルク指令生成部１４と、速度算出部１５と、適応推定部１６と、変更部１７と、誤差補償部１８とを備える。
【００１８】
位置指令生成部１１は、モータの位置指令を生成し、速度指令生成部１２に渡す。位置指令生成部１１は、例えば、教示情報等を用いることによって、モータの位置を算出し、その位置を示す位置指令を速度指令生成部１２に渡してもよい。なお、モータの位置は、厳密にはモータの角度であってもよい。
【００１９】
速度指令生成部１２は、モータの現在位置と、そのモータの位置指令とを用いて速度指令を生成する。その位置指令は、前述のように、位置指令生成部１１によって生成されたものである。速度指令生成部１２は、例えば、マニピュレータ２のモータの位置検出器から現在位置を受け取ってもよい。その位置検出器は、例えば、エンコーダであってもよい。
【００２０】
電流指令生成部１３は、速度算出部１５が算出したモータの現在速度と、そのモータについて速度指令生成部１２が生成した速度指令とを用いて電流指令を生成する。なお、モータの速度は、厳密にはモータの回転速度、すなわち、角速度であってもよい。
【００２１】
トルク指令生成部１４は、モータの現在電流と、そのモータについて誤差補償部１８が生成した電流指令とを用いてトルク指令を生成し、そのモータに出力する。その結果、そのモータは、トルク指令に応じたトルクとなるように動作することになる。トルク指令生成部１４は、例えば、モータの電流を検出する電流検出器から現在電流を受け取ってもよい。
なお、速度指令生成部１２、電流指令生成部１３、トルク指令生成部１４によるフィードバック制御処理は、サーボコントローラの処理としてすでに公知のものであり、その詳細な説明を省略する。そのサーボコントローラでは、例えば、一般的なＰＩＤ制御等が行われる。
【００２２】
速度算出部１５は、モータの現在位置から、モータの現在速度を算出する。具体的には、速度算出部１５は、モータの現在位置を時間微分することによって、モータの現在速度を算出してもよい。そのモータの現在速度は、電流指令生成部１３において用いられるため、速度算出部１５は、電流指令生成部１３が電流指令を生成する周期と同じ周期か、または、それよりも短い周期で現在速度を算出することが好適である。
【００２３】
適応推定部１６は、速度算出部１５が算出した現在速度を用いて、減速機の角度伝達誤差を示す周期的な関数の周期・振幅・位相を適応的に推定することによって、角度伝達誤差を生成する。すなわち、角度伝達誤差を周期的な関数でモデル化しており、この適応推定部１６では、その関数の未知パラメータである周期・振幅・位相を適応的に推定することになる。その関数は、三角関数であってもよく、のこぎり波であってもよく、三角波であってもよく、矩形波であってもよく、その他の周期的な関数であってもよい。本実施の形態では、この関数が三角関数である場合、すなわち、角度伝達誤差が三角関数でモデル化される場合について主に説明する。なお、適応推定部１６が推定した角度伝達誤差は、電流指令生成部１３が生成した電流指令に対する補償を行うために用いられるため、適応推定部１６は、電流指令生成部１３が電流指令を生成する周期と同じ周期か、または、それよりも短い周期で角度伝達誤差を推定することが好適である。
【００２４】
図２は、適応推定部１６の詳細な構成の一例を示すブロック図である。適応推定部１６は、第１の乗算器２１と、第２の乗算器２２と、行列乗算器２３と、積分器２４と、余弦算出器２５と、正弦算出器２６と、第３の乗算器２７とを備える。
【００２５】
第１の乗算器２１は、モータの速度ωＭと、余弦算出器２５で算出される、位相αの余弦値「ｃｏｓ（α）」とを乗算する。その乗算結果であるｙ１＝ωＭ×ｃｏｓ（α）は、行列乗算器２３に入力される。
第２の乗算器２２は、モータの速度ωＭと、正弦算出器２６で算出される、位相αの正弦値の負値「−ｓｉｎ（α）」とを乗算する。その乗算結果であるｙ２＝−ωＭ×ｓｉｎ（α）は、行列乗算器２３に入力される。
【００２６】
行列乗算器２３は、第１及び第２の乗算器２２の乗算結果ｙ１，ｙ２を各要素に有するベクトル（ｙ１  ｙ２）ｔに、モータの軸に関する伝達関数Ｐ（ｓ）に対応する伝達関数行列Ｃ（ｓ）に応じた演算を行うことによって、振幅θ１及び周波数θ２を各要素に有するベクトル（θ１  θ２）ｔを生成する。その振幅θ１は、第３の乗算器２７に入力される。また、その周波数θ２は、積分器２４に入力される。なお、モータの軸に関する伝達関数Ｐ（ｓ）、及びそれに対応する伝達関数行列Ｃ（ｓ）については後述する。
【００２７】
積分器２４は、行列乗算器２３が生成した周波数θ２を積分する。その積分結果は、位相αである。すなわち、積分器２４は、周波数θ２を積分して位相αを算出することになる。
【００２８】
余弦算出器２５は、積分器２４が算出した位相αの余弦値ｃｏｓ（α）を算出し、第１の乗算器２１に入力する。また、その余弦値「ｃｏｓ（α）」は、第３の乗算器２７にも入力される。
正弦算出器２６は、積分器２４が算出した位相αの正弦値の負値「−ｓｉｎ（α）」を算出し、第２の乗算器２２に入力する。
【００２９】
第３の乗算器２７は、行列乗算器２３が生成した振幅θ１と、余弦算出器２５が算出した余弦値ｃｏｓ（α）とを乗算することによって角度伝達誤差ｄｅｓｔ＝θ１×ｃｏｓ（α）を生成する。この角度伝達誤差ｄｅｓｔが、最終的に適応推定された角度伝達誤差となる。
【００３０】
変更部１７は、適応推定部１６が生成した角度伝達誤差ｄｅｓｔ（ｔ）に０より大きく１以下のパラメータｃを乗算することによって、角度伝達誤差を変更する。この角度伝達誤差の変更は、角度伝達誤差の補償の程度を調整するために行われる。したがって、このパラメータの値ｃは、ユーザ等が任意に設定できるように変更可能になっていることが好適である。なお、そのパラメータｃは、０より大きく１未満のパラメータであってもよく、０以上、１以下のパラメータであってもよい。なお、パラメータｃが１を含む場合には、この変更部１７による変更は、恒等変換を含むことになる。変更部１７による変更後の角度伝達誤差が、フィードフォワード補償量として用いられることになる。
【００３１】
誤差補償部１８は、適応推定部１６が生成し、変更部１７によって変更された角度伝達誤差ｃ×ｄｅｓｔ（ｔ）を用いて、電流指令から減速機の角度伝達誤差成分を補償した電流指令を生成する。これにより、フィードフォワード的に角度伝達誤差を補償することができる。ここで、角度伝達誤差成分を補償するとは、その補償後の電流指令に応じたトルク指令によってモータが回転し、その回転が角度伝達誤差を有する減速機を介してアームに伝えられることによって、結果として、その減速機の角度伝達誤差が相殺されるようにすることである。したがって、この角度伝達誤差成分の補償された電流指令を用いることによって、角度伝達誤差に起因するマニピュレータ２の先端の振動がなくなるか、または低減されることになる。誤差補償部１８は、例えば、電流指令生成部１３が生成した電流指定に対して、変更後の角度伝達誤差を加算する加算器や、変更後の角度伝達誤差を減算する減算器であってもよい。本実施の形態では、誤差補償部１８が加算器である場合について主に説明する。
【００３２】
なお、図１では、マニピュレータ２に一のトルク指令を出力し、マニピュレータ２から一の現在位置や現在電流を受け取る場合について示しているが、これは説明の便宜のためであり、制御装置１は、マニピュレータ２が有する軸数、すなわちモータの数だけトルク指令を出力し、その数だけ現在位置や現在電流を受け取ってもよい。すなわち、制御装置１の処理は、モータごとに行われてもよい。
【００３３】
ここで、適応推定部１６による適応推定の処理について説明する。
マニピュレータの１軸モデルとして、次の伝達関数Ｐ（ｓ）を考える。
ωＭ（ｓ）＝Ｐ（ｓ）τ（ｓ）
ここで、ωＭ（ｓ）は、モータの回転角速度ωＭ（ｔ）のラプラス変換であり、τ（ｓ）は、入力トルクτ（ｔ）のラプラス変換であり、伝達関数Ｐ（ｓ）は、次式のようになる。なお、この伝達関数Ｐ（ｓ）は、マニピュレータの１軸モデルの伝達関数として、すでに知られているものである。
【数１】
2014136260.tif 000003
【００３４】
なお、標準的なロボット制御において、トルクτ及び角速度ωＭは取得可能な量であり、上記式中の各パラメータは次の通りであり、既知である。
ＪＬ：負荷慣性モーメント
ＪＭ：負荷側換算のモータの慣性モーメント
Ｎ：減速比
Ｄ０：ダンピング係数（減衰比）
Ｋ０：ばね定数
【００３５】
上記式は、減速機の剛性を考慮した２慣性モデルであるが、角度伝達誤差を考慮していない。角度伝達誤差を考慮すると、制御対象のモデルは次式のようになる。
ωＭ（ｓ）＝Ｐ（ｓ）（τ（ｓ）−ｄ（ｓ））
ここで、ｄ（ｓ）は、角度伝達誤差ｄ（ｔ）のラプラス変換である。本実施の形態では、その角度伝達誤差ｄ（ｔ）が、周期的な関数である三角関数で示されるものとする。すなわち、角度伝達誤差ｄ（ｔ）は、次式のように示されるものとする。
【数２】
2014136260.tif 000004
【００３６】
ただし、θｄは、三角関数の振幅であり、ωｄは、周波数であり、αｄ（０）は、位相であり、それらが未知のパラメータである。なお、その周波数は、厳密には角周波数である。また、αｄ（０）を位相と呼ぶこともあり、αｄ（ｔ）＝ωｄ×ｔ＋αｄ（０）を位相と呼ぶこともある。
【００３７】
以降の説明では、角度伝達誤差ｄ（ｔ）の影響で速度ωＭに現れる振動を打ち消すように入力トルクτ（ｔ）に関する補償を行う方法について、図２を用いて説明する。図２から分かるように、角度伝達誤差ｄ（ｔ）を適応的に推定するために必要な外部入力は、モータの角速度ωＭ（ｔ）のみである。したがって、適応推定部１６では、外界センサを用いないで、角度伝達誤差を推定することが可能となる。
【００３８】
図２において、角度伝達誤差の推定値ｄｅｓｔ（ｔ）は、次式で示されるようになる。
【数３】
2014136260.tif 000005
ここで、θ１（ｔ）、θ２（ｔ）、α（ｔ）は、それぞれ角度伝達誤差ｄ（ｔ）の振幅θｄ、周波数ωｄ、位相αｄ（ｔ）＝ωｄ×ｔ＋αｄ（０）の推定値である。これらの推定値が真値に近づいていくことにより、角度伝達誤差の推定値ｄｅｓｔ（ｔ）が、より正確な値となる。したがって、そのｄｅｓｔ（ｔ）を用いて、入力トルクτ（ｔ）に対する角度伝達誤差の影響を打ち消すことによって、角度伝達誤差の補償を実現することができる。そのため、ロボットの個体ごと、または減速機ごとのパラメータ調整を行うことなく、適応推定処理のみによって、角度伝達誤差を補償することができるようになる。
【００３９】
図２で示される適応推定部１６におけるパラメータの更新処理は、次のように行われる。まず、システムの出力である角速度ωＭ（ｔ）を用いて、第１及び第２の乗算器２１，２２において、次式のように内部信号ｙ１（ｔ）、ｙ２（ｔ）を生成する。
ｙ１（ｔ）＝ωＭ（ｔ）×ｃｏｓ（α（ｔ））
ｙ２（ｔ）＝−ωＭ（ｔ）×ｓｉｎ（α（ｔ））
【００４０】
その後、行列乗算器２３において、ｙ１（ｔ）、ｙ２（ｔ）を各要素とするベクトル（ｙ１（ｔ）  ｙ２（ｔ））ｔに、伝達関数行列Ｃ（ｓ）に応じた演算を行うことによって、振幅θ１及び周波数θ２を各要素とするベクトル（θ１（ｔ）  θ２（ｔ））ｔを生成する。すなわち、次式のようになる。
（θ１（ｔ）  θ２（ｔ））ｔ＝Ｃ（ｓ）×（ｙ１（ｔ）  ｙ２（ｔ））ｔ
【００４１】
ただし、Ｃ（ｓ）は、時間軸上のものではなく、ラプラス変換されたものであるため、（ｙ１（ｔ）  ｙ２（ｔ））ｔの各要素に対しては、伝達関数行列Ｃ（ｓ）に応じた演算、すなわち伝達関数行列Ｃ（ｓ）の各要素に応じた演算が行われることになる。その演算は、例えば、積分であってもよく、フィルタリングであってもよく、その他の演算であってもよい。例えば、Ｃ（ｓ）のある要素に「１／ｓ」が含まれる場合には、その要素に応じた演算として、積分を行ってもよい。その伝達関数行列Ｃ（ｓ）は、次式によって示される。
Ｃ（ｓ）＝Ｃ１（ｓ）Ｇ−１（θ２）
ここで、Ｃ１（ｓ）、Ｇ（θ２）は、次のように示される。
【数４】
2014136260.tif 000006
【００４２】
なお、ｇ１，ｇ２，ａ，ｂは、それぞれ０より大きい実数の設計パラメータであり、適応推定部１６を含む閉ループ系の極を望ましい位置に配置するように設計されることが好適である。すなわち、それらの設計パラメータは、適応推定の収束が早くなるように設計されることが好適である。また、Ｐ（ｓ）は、モータの軸に関する前述の伝達関数であり、ＰＲ（θ２）、ＰＩ（θ２）等については、次の通りである。次式において、Ｒｅ［］、及びＩｍ［］は、それぞれ複素関数の実部と虚部とを示すものである。また、ＰＲ（θ２）、ＰＩ（θ２）で用いるθ２については、一回前の演算で算出されたθ２を用いるものとする。
ＰＲ（θ２）＝Ｒｅ［Ｐ（ｊθ２）］
ＰＩ（θ２）＝Ｉｍ［Ｐ（ｊθ２）］
【００４３】
なお、ここでは、ベクトル（ｙ１（ｔ）  ｙ２（ｔ））ｔと伝達関数行列Ｃ（ｓ）とを掛け合わせたのに等しい演算を行う場合について説明したが、結果が同じになるのであれば、ｙ１（ｔ）やｙ２（ｔ）、θ１（ｔ）やθ２（ｔ）をベクトルの各要素として取り扱わなくてもよく、伝達関数行列Ｃ（ｓ）を、行列として取り扱わなくてもよいことは言うまでもない。
【００４４】
位相の推定値α（ｔ）は、θ２（ｔ）を積分器２４で積分することによって得られる。また、余弦算出器２５において、その位相の推定値α（ｔ）の余弦値ｃｏｓ（α（ｔ））が算出され、正弦算出器２６において、その位相の推定値α（ｔ）の正弦値の負値−ｓｉｎ（α（ｔ））が算出され、それらを用いて内部信号ｙ１（ｔ）、ｙ２（ｔ）が更新される。最後に、第３の乗算器２７において、余弦算出器２５で算出された余弦値ｃｏｓ（α（ｔ））と、行列乗算器２３で得られた振幅θ１とが乗算されることによって、角度伝達誤差の推定値
ｄｅｓｔ（ｔ）＝θ１×ｃｏｓ（α（ｔ））
が算出され、変更部１７に渡される。このようにして、適応推定部１６では、三角関数で示される角度伝達誤差ｄ（ｔ）の振幅θ１（ｔ）、周期θ２（ｔ）、位相α（ｔ）がそれぞれ適応的に推定されることになる。
【００４５】
ここで、適応推定の処理の詳細については、例えば、次の文献に記載されているため、そのような文献を参照されたい。
文献：Ｍ．Ｂｏｄｓｏｎ、Ｓ．Ｄｏｕｇｌａｓ、「Ａｄａｐｔｉｖｅ  Ａｌｇｏｒｉｔｈｍｓ  ｆｏｒ  ｔｈｅ  Ｒｅｊｅｃｔｉｏｎ  ｏｆ  Ｓｉｎｕｓｏｉｄａｌ  Ｄｉｓｔｕｒｂａｎｃｅｓ  ｗｉｔｈ  Ｕｎｋｎｏｗｎ  Ｆｒｅｑｕｅｎｃｙ」、Ａｕｔｏｍａｔｉｃａ、ｖｏｌ．３３、ｎｏ．１２、ｐ．２２１３−２２２１，１９９７年
【００４６】
なお、パラメータ推定値θ１（ｔ），θ２（ｔ），α（ｔ）の初期値については、インプットギア歯数などの機械的な定数の情報を用い、また同定実験を行うことによって決定してもよい。厳密には減速機やロボットごとの個体差が存在するが、適応推定によって補償することができるため、個体ごとに同定実験を行う必要はない。
【００４７】
次に、制御装置１の動作について図３のフローチャートを用いて説明する。なお、このフローチャートでは、電流指令生成部１３、速度算出部１５、適応推定部１６、変更部１７、誤差補償部１８の動作についてのみ説明する。
【００４８】
（ステップＳ１０１）電流指令生成部１３は、電流指令を生成するかどうか判断する。そして、電流指令を生成する場合には、ステップＳ１０２に進み、そうでない場合には、電流指令を生成すると判断するまでステップＳ１０１の処理を繰り返す。なお、電流指令生成部１３は、例えば、あらかじめ決められた時間間隔ごとに、電流指令を生成すると判断してもよい。
【００４９】
（ステップＳ１０２）速度算出部１５は、モータの現在位置を用いて、そのモータの現在速度を算出する。
【００５０】
（ステップＳ１０３）電流指令生成部１３は、速度算出部１５が算出したモータの現在速度と、速度指令生成部１２が生成した速度指令とを用いて、電流指令を生成する。
【００５１】
（ステップＳ１０４）適応推定部１６は、速度算出部１５が算出したモータの現在速度を用いて、角度伝達誤差ｄｅｓｔ（ｔ）を推定する。
【００５２】
（ステップＳ１０５）変更部１７は、あらかじめ設定されているパラメータ値ｃを、推定された角度伝達誤差ｄｅｓｔ（ｔ）に掛けることによって、角度伝達誤差を変更する。
【００５３】
（ステップＳ１０６）誤差補償部１８は、電流指令生成部１３が生成した電流指令と、変更部１７によって変更された角度伝達誤差とを用いて、角度伝達誤差成分を補償した電流指令を生成する。そして、ステップＳ１０１に戻る。
【００５４】
なお、図３のフローチャートにおいて、電源オフや処理終了の割り込みにより処理は終了する。また、図３のフローチャートでは、電流指令の生成と、その電流指令に対する角度伝達誤差の補償との処理のみを示しているが、速度指令の生成やトルク指令の生成等のその他の処理は、サーボコントローラ等の処理としてすでに公知であり、その詳細な説明を省略する。
【００５５】
以上のように、本実施の形態による制御装置１によれば、適応推定部１６によって角度伝達誤差を適応的に推定し、その推定結果を用いることによって、角度伝達誤差を補償することができる。その結果、マニピュレータ２の先端を直線動作させる際に起こりうる周期的な振動を抑えることができ、より精度の高い軌跡制御を行うことができる。また、その適応推定において、ロボットごとや減速機ごとなどの実測が必要ないため、データ測定コストを低減することができるメリットもある。また、適応推定部１６は、モータの角速度のみを用いて角度伝達誤差を推定するため、外界センサを用いる必要がないというメリットもある。また、複雑なモデルを用いていないため、角度伝達誤差の推定が容易となる。
【００５６】
なお、本実施の形態では、適応推定部１６の構成として、図２に示されるものを一例として示したが、適応推定部１６は、それに限定されるものではない。モータの角速度を用いて角度伝達誤差を適応的に推定することができるものであれば、他の構成であってもよい。
【００５７】
また、本実施の形態では、制御装置１が変更部１７を備える場合について説明したが、そうでなくてもよい。制御装置１は、変更部１７を備えていなくてもよい。その場合には、誤差補償部１８は、適応推定部１６が推定した角度伝達誤差そのものを用いて、電流指令生成部１３が生成した電流指令から減速機の角度伝達誤差成分を補償した電流指令を生成してもよい。
【００５８】
また、上記実施の形態において、各処理または各機能は、単一の装置または単一のシステムによって集中処理されることによって実現されてもよく、あるいは、複数の装置または複数のシステムによって分散処理されることによって実現されてもよい。
【００５９】
また、上記実施の形態において、各構成要素間で行われる情報の受け渡しは、例えば、その情報の受け渡しを行う２個の構成要素が物理的に異なるものである場合には、一方の構成要素による情報の出力と、他方の構成要素による情報の受け付けとによって行われてもよく、あるいは、その情報の受け渡しを行う２個の構成要素が物理的に同じものである場合には、一方の構成要素に対応する処理のフェーズから、他方の構成要素に対応する処理のフェーズに移ることによって行われてもよい。
【００６０】
また、上記実施の形態において、各構成要素が実行する処理に関係する情報、例えば、各構成要素が受け付けたり、取得したり、選択したり、生成したり、送信したり、受信したりした情報や、各構成要素が処理で用いるしきい値や数式、アドレス、設定値、パラメータ等の情報等は、上記説明で明記していなくても、図示しない記録媒体において、一時的に、あるいは長期にわたって保持されていてもよい。また、その図示しない記録媒体への情報の蓄積を、各構成要素、あるいは、図示しない蓄積部が行ってもよい。また、その図示しない記録媒体からの情報の読み出しを、各構成要素、あるいは、図示しない読み出し部が行ってもよい。
【００６１】
また、上記実施の形態において、各構成要素等で用いられる情報、例えば、各構成要素が処理で用いるしきい値やアドレス、各種の設定値、パラメータ等の情報がユーザによって変更されてもよい場合には、上記説明で明記していなくても、ユーザが適宜、それらの情報を変更できるようにしてもよく、あるいは、そうでなくてもよい。それらの情報をユーザが変更可能な場合には、その変更は、例えば、ユーザからの変更指示を受け付ける図示しない受付部と、その変更指示に応じて情報を変更する図示しない変更部とによって実現されてもよい。その図示しない受付部による変更指示の受け付けは、例えば、入力デバイスからの受け付けでもよく、通信回線を介して送信された情報の受信でもよく、所定の記録媒体から読み出された情報の受け付けでもよい。
【００６２】
また、上記実施の形態において、制御装置１に含まれる２以上の構成要素が通信デバイスや入力デバイス等を有する場合に、２以上の構成要素が物理的に単一のデバイスを有してもよく、あるいは、別々のデバイスを有してもよい。
【００６３】
また、上記実施の形態において、各構成要素は専用のハードウェアにより構成されてもよく、あるいは、ソフトウェアにより実現可能な構成要素については、プログラムを実行することによって実現されてもよい。例えば、ハードディスクや半導体メモリ等の記録媒体に記録されたソフトウェア・プログラムをＣＰＵ等のプログラム実行部が読み出して実行することによって、各構成要素が実現され得る。その実行時に、プログラム実行部は、記憶部や記録媒体にアクセスしながらプログラムを実行してもよい。なお、そのプログラムは、サーバなどからダウンロードされることによって実行されてもよく、所定の記録媒体に記録されたプログラムが読み出されることによって実行されてもよい。また、そのプログラムは、プログラムプロダクトを構成するプログラムとして用いられてもよい。また、そのプログラムを実行するコンピュータは、単数であってもよく、複数であってもよい。すなわち、集中処理を行ってもよく、あるいは分散処理を行ってもよい。
【００６４】
また、本発明は、以上の実施の形態に限定されることなく、種々の変更が可能であり、それらも本発明の範囲内に包含されるものであることは言うまでもない。
【産業上の利用可能性】
【００６５】
以上より、本発明による制御装置によれば、ロボット個体ごとの調整を必要とせず、外界センサを用いないで角度伝達誤差の補償を行うことができ、マニピュレータの制御装置として有用である。
【符号の説明】
【００６６】
１  制御装置
２  マニピュレータ
１１  位置指令生成部
１２  速度指令生成部
１３  電流指令生成部
１４  トルク指令生成部
１５  速度算出部
１６  適応推定部
１７  変更部
１８  誤差補償部
２１  第１の乗算器
２２  第２の乗算器
２３  行列乗算器
２４  積分器
２５  余弦算出器
２６  正弦算出器
２７  第３の乗算器
【図１】
2014136260.tif 000007
【図２】
2014136260.tif 000008
【図３】
2014136260.tif 000009