知財求人 - 知財ポータルサイト「IP Force」
(19)【発行国】日本国特許庁(JP)
(12)【公報種別】特許公報(B2)
(11)【特許番号】
(24)【登録日】2024-01-04
(45)【発行日】2024-01-15
(54)【発明の名称】ロボットの末端経路輪郭誤差の制御方法
(51)【国際特許分類】
B25J 15/02 20060101AFI20240105BHJP
G05B 13/04 20060101ALI20240105BHJP
【ＦＩ】
B25J15/02
G05B13/04
【請求項の数】
5
(21)【出願番号】P 2022147924
(22)【出願日】2022-09-16
(65)【公開番号】P2023107726
(43)【公開日】2023-08-03
【審査請求日】2022-09-16
(31)【優先権主張番号】202210076416.6
(32)【優先日】2022-01-24
(33)【優先権主張国・地域又は機関】CN
(73)【特許権者】
【識別番号】514283423
【氏名又は名称】河北工業大学
(74)【代理人】
【識別番号】100085660
【弁理士】
【氏名又は名称】鈴木 均
(74)【代理人】
【識別番号】100149892
【弁理士】
【氏名又は名称】小川 弥生
(74)【代理人】
【識別番号】100185672
【弁理士】
【氏名又は名称】池田 雅人
(72)【発明者】
【氏名】韓旭
(72)【発明者】
【氏名】張建寧
(72)【発明者】
【氏名】李珊瑚
(72)【発明者】
【氏名】陶友瑞
(72)【発明者】
【氏名】段書用
【審査官】杉山 悟史
(56)【参考文献】
【文献】国際公開第２０１８／００９９８６（ＷＯ，Ａ１）
【文献】国際公開第２０２２／００１３２０（ＷＯ，Ａ１）
【文献】特開２００６－２８７５２０（ＪＰ，Ａ）
【文献】特表２０１５－５２８７１３（ＪＰ，Ａ）
(58)【調査した分野】(Int.Cl.，ＤＢ名)
Ｂ２５Ｊ １／００ ～ ２１／０２
Ｇ０５Ｂ １３／００ ～ １３／０４
(57)【特許請求の範囲】
【請求項1】
ロボットが末端アクチュエータ及び前記末端アクチュエータを制御するための制御モジュールを含み、前記制御モジュール内に前記末端アクチュエータを制御するためのサーボ制御ユニットが設けられ、前記サーボ制御ユニットの入力端が起動信号を受信し、制御信号を前記末端アクチュエータに出力して、前記末端アクチュエータを駆動して移動させ、前記制御モジュール内に前記サーボ制御ユニットに接続された外部演算モジュールをさらに含む、ロボットの末端経路輪郭誤差の制御方法であって、前記制御方法は、初期信号を取得するステップＳ１０と、
前記初期信号を前記外部演算モジュールに入力して、前記末端アクチュエータのプリセット軌跡を取得するステップＳ２０と、
等価予測モデルを構築するステップＳ３０と、
前記プリセット軌跡を前記等価予測モデルに入力して、等価予測軌跡を取得するステップＳ４０と、
前記プリセット軌跡と前記等価予測軌跡との差を求めて、プリセット追従誤差軌跡を取得するステップＳ５０と、
前記プリセット軌跡と前記プリセット追従誤差軌跡を重ね合わせて、第１軌跡を取得するステップＳ６０と、
前記第１軌跡を前記起動信号として、前記サーボ制御ユニットの入力端に入力するステップＳ７０とを含み、
周波数領域において、前記等価予測モデルは下式のとおりであり、
ここで、
【請求項2】
前記ロボットはＸＹ軸２軸ロボットであり、Ｘ軸に対応する第１末端アクチュエータ及びＹ軸に対応する第２末端アクチュエータを含み、前記第１末端アクチュエータと前記第２末端アクチュエータとの間に結合関係があり、前記制御方法は、前記第１末端アクチュエータの第１初期信号及び前記第２末端アクチュエータの第２初期信号をそれぞれ取得するステップと、
前記ステップＳ２０を実行して、前記第１末端アクチュエータの第１プリセット軌跡及び前記第２末端アクチュエータの第２プリセット軌跡を取得するステップと、
前記第１プリセット軌跡を前記起動信号として、前記第１末端アクチュエータの前記サーボ制御ユニットに入力して、第１出力軌跡を取得し、前記第２プリセット軌跡を前記起動信号として、前記第２末端アクチュエータの前記サーボ制御ユニットに入力して、第２出力軌跡を取得するステップと、
前記第１プリセット軌跡と前記第１出力軌跡との差を求めて、第１出力誤差軌跡を取得し、前記第２プリセット軌跡と前記第２出力軌跡との差を求めて、第２出力誤差軌跡を取得するステップと、
前記第１出力誤差軌跡及び前記第２出力誤差軌跡で前記末端経路輪郭誤差に関連する相関関数を構築して、前記第１出力誤差軌跡に対応する第１相関係数及び前記第２出力誤差軌跡に対応する第２相関係数を取得するステップと、
前記ステップＳ３０～前記ステップＳ５０を実行して、前記第１末端アクチュエータの第１プリセット追従誤差軌跡及び前記第２末端アクチュエータの第２プリセット追従誤差軌跡を取得するステップと、
前記第１相関係数と前記第２相関係数の大きさを判断し、前記第１相関係数が前記第２相関係数より大きい場合、前記第１末端アクチュエータに関連する、前記第１プリセット追従誤差軌跡とプリセットゲインとの和である第１ゲインを構築し、第２末端アクチュエータに関連する、前記第２プリセット追従誤差軌跡と前記プリセットゲインとの差である第２ゲインを構築するステップと、
前記第１相関係数が前記第２相関係数より小さい場合、第１末端アクチュエータに関連する、前記第１プリセット追従誤差軌跡と前記プリセットゲインとの差である前記第１ゲインを構築し、前記第２末端アクチュエータに関連する、前記第２プリセット追従誤差軌跡と前記プリセットゲインとの和である前記第２ゲインを構築するステップと、
前記第１ゲインと前記第１プリセット軌跡を重ね合わせて、第２軌跡を取得するステップと、
前記第２ゲインと前記第２プリセット軌跡を重ね合わせて、第３軌跡を取得するステップと、
前記第２軌跡を前記起動信号として、第１末端アクチュエータの前記サーボ制御ユニットの入力端に入力し、前記第３軌跡を前記起動信号として、第２末端アクチュエータの前記サーボ制御ユニットの入力端に入力するステップとを含むことを特徴とする請求項１に記載のロボットの末端経路輪郭誤差の制御方法。
【請求項3】
前記初期信号を取得する前記ステップＳ１０は、前記末端アクチュエータの振動周波数を取得するステップと、
前記振動周波数を除去するための畳み込み関数を構築して、畳み込みゲインを取得するステップと、
元の信号を取得するステップと、
前記元の信号と前記畳み込みゲインとの積を計算して、前記初期信号を取得するステップとを含むことを特徴とする請求項１に記載のロボットの末端経路輪郭誤差の制御方法。
【請求項4】
円弧輪郭誤差によって前記相関関数を構築し、前記相関関数は下式のとおりであり、
ここで、
は前記第２出力誤差軌跡を表し、α、Ｒはそれぞれ前記ロボットのＸ軸又はＹ軸の軌跡参照点での円弧接線と該軸の正方向との間の角度及び円弧半径を表し、
前記第１相関係数を
とすることを特徴とする請求項２に記載のロボットの末端経路輪郭誤差の制御方法。
【請求項5】
周波数領域において、前記畳み込み関数は下式のように設計され、
ここで、
【発明の詳細な説明】
【技術分野】
【0001】
本出願は、ロボットの技術分野に関し、具体的には、ロボットの末端経路輪郭誤差の制御方法に関する。
【背景技術】
【0002】
ロボットの末端経路輪郭誤差は、ロボットによる操作タスクの完了において重要な性能指標となる。ロボットの応用場面がますます複雑になり、ロボットの機能がますます完全になるにつれて、ロボットの末端経路輪郭誤差に対する要求もますます高くなっている。ロボットの応用場面が複雑になるため、ロボットの耐干渉性を向上させるために、通常、コントローラ、即ちサーボループの帯域幅を小さく設計する。したがって、サーボループの帯域幅によるタイムラグ及びロボットの可撓性末端又はサーボループの高ゲインによる振動は、単軸追従誤差に影響を与えると同時に、ロボットの末端経路輪郭誤差にも影響を与える。
【0003】
単軸追従精度を改善するための従来の方法は、通常、末端の残留振動の影響のみを考慮するが、実際には、サーボループの帯域幅の制限による影響は無視できない。サーボループの帯域幅に対する従来の改善方式は、通常、システムの帯域幅を増やすことによるものであり、機械的構造及びハードウェア構造が決定された後の効果改善には限界がある。
【発明の概要】
【発明が解決しようとする課題】
【0004】
従来技術における上記欠陥又は不足に鑑み、本出願は、ロボットの末端経路輪郭誤差の制御方法を提供することを目的とする。
【課題を解決するための手段】
【0005】
ロボットが末端アクチュエータ及び前記末端アクチュエータを制御するための制御モジュールを含み、前記制御モジュール内に前記末端アクチュエータを制御するためのサーボ制御ユニットが設けられ、前記サーボ制御ユニットの入力端が起動信号を受信し、制御信号を前記末端アクチュエータに出力して、前記末端アクチュエータを駆動して移動させ、前記制御モジュール内に前記サーボ制御ユニットに接続された外部演算モジュールをさらに含む、ロボットの末端経路輪郭誤差の制御方法であって、前記制御方法は、
初期信号を取得するステップＳ１０と、
前記初期信号を前記外部演算モジュールに入力して、前記末端アクチュエータのプリセット軌跡を取得するステップＳ２０と、
等価予測モデルを構築するステップＳ３０と、
前記プリセット軌跡を前記等価予測モデルに入力して、等価予測軌跡を取得するステップＳ４０と、
前記プリセット軌跡と前記等価予測軌跡との差を求めて、プリセット追従誤差軌跡を取得するステップＳ５０と、
前記プリセット軌跡と前記プリセット追従誤差軌跡を重ね合わせて、第１軌跡を取得するステップＳ６０と、
前記第１軌跡を前記起動信号として、前記サーボ制御ユニットの入力端に入力するステップＳ７０とを含むことを特徴とする。
初期信号を取得するステップＳ１０と、
前記初期信号を前記外部演算モジュールに入力して、前記末端アクチュエータのプリセット軌跡を取得するステップＳ２０と、
等価予測モデルを構築するステップＳ３０と、
前記プリセット軌跡を前記等価予測モデルに入力して、等価予測軌跡を取得するステップＳ４０と、
前記プリセット軌跡と前記等価予測軌跡との差を求めて、プリセット追従誤差軌跡を取得するステップＳ５０と、
前記プリセット軌跡と前記プリセット追従誤差軌跡を重ね合わせて、第１軌跡を取得するステップＳ６０と、
前記第１軌跡を前記起動信号として、前記サーボ制御ユニットの入力端に入力するステップＳ７０とを含むことを特徴とする。
【0006】
さらに、前記ロボットはＸＹ軸２軸ロボットであり、Ｘ軸に対応する第１末端アクチュエータ及びＹ軸に対応する第２末端アクチュエータを含み、前記第１末端アクチュエータと前記第２末端アクチュエータとの間に結合関係があり、前記制御方法は、
ステップＳ２０を実行して、前記第１末端アクチュエータの第１プリセット軌跡及び前記第２末端アクチュエータの第２プリセット軌跡を取得するステップと、
前記第１プリセット軌跡を前記起動信号として、前記第１末端アクチュエータの前記サーボ制御ユニットに入力して第１出力軌跡を取得し、前記第２プリセット軌跡を前記起動信号として、前記第２末端アクチュエータの前記サーボ制御ユニットに入力して第２出力軌跡を取得するステップと、
前記第１プリセット軌跡と前記第１出力軌跡との差を求めて、第１出力誤差軌跡を取得し、前記第２プリセット軌跡と前記第２出力軌跡との差を求めて、第２出力誤差軌跡を取得するステップと、
前記第１出力誤差軌跡及び前記第２出力誤差軌跡で前記末端経路輪郭誤差に関連する相関関数を構築して、前記第１出力誤差軌跡に対応する第１相関係数及び前記第２出力誤差軌跡に対応する第２相関係数を取得するステップと、
ステップＳ３０～Ｓ５０を実行して、前記第１末端アクチュエータの第１プリセット追従誤差軌跡及び前記第２末端アクチュエータの第２プリセット追従誤差軌跡を取得するステップと、
前記第１相関係数と前記第２相関係数の大きさを判断し、前記第１相関係数が前記第２相関係数より大きい場合、前記第１末端アクチュエータに関連する、前記第１プリセット追従誤差軌跡とプリセットゲインとの和である第１ゲインを構築し、第２末端アクチュエータに関連する、前記第２プリセット追従誤差軌跡と前記プリセットゲインとの差である第２ゲインを構築するステップと、
前記第１相関係数が前記第２相関係数より小さい場合、第１末端アクチュエータに関連する、前記第１プリセット追従誤差軌跡と前記プリセットゲインとの差である前記第１ゲインを構築し、前記第２末端アクチュエータに関連する、前記第２プリセット追従誤差軌跡と前記プリセットゲインとの和である前記第２ゲインを構築するステップと、
前記第１ゲインと前記第１プリセット軌跡を重ね合わせて、第２軌跡を取得するステップと、
前記第２ゲインと前記第２プリセット軌跡を重ね合わせて、第３軌跡を取得するステップと、
前記第２軌跡を前記起動信号として、第１末端アクチュエータの前記サーボ制御ユニットの入力端に入力し、前記第３軌跡を前記起動信号として、第２末端アクチュエータの前記サーボ制御ユニットの入力端に入力するステップとを含む。
ステップＳ２０を実行して、前記第１末端アクチュエータの第１プリセット軌跡及び前記第２末端アクチュエータの第２プリセット軌跡を取得するステップと、
前記第１プリセット軌跡を前記起動信号として、前記第１末端アクチュエータの前記サーボ制御ユニットに入力して第１出力軌跡を取得し、前記第２プリセット軌跡を前記起動信号として、前記第２末端アクチュエータの前記サーボ制御ユニットに入力して第２出力軌跡を取得するステップと、
前記第１プリセット軌跡と前記第１出力軌跡との差を求めて、第１出力誤差軌跡を取得し、前記第２プリセット軌跡と前記第２出力軌跡との差を求めて、第２出力誤差軌跡を取得するステップと、
前記第１出力誤差軌跡及び前記第２出力誤差軌跡で前記末端経路輪郭誤差に関連する相関関数を構築して、前記第１出力誤差軌跡に対応する第１相関係数及び前記第２出力誤差軌跡に対応する第２相関係数を取得するステップと、
ステップＳ３０～Ｓ５０を実行して、前記第１末端アクチュエータの第１プリセット追従誤差軌跡及び前記第２末端アクチュエータの第２プリセット追従誤差軌跡を取得するステップと、
前記第１相関係数と前記第２相関係数の大きさを判断し、前記第１相関係数が前記第２相関係数より大きい場合、前記第１末端アクチュエータに関連する、前記第１プリセット追従誤差軌跡とプリセットゲインとの和である第１ゲインを構築し、第２末端アクチュエータに関連する、前記第２プリセット追従誤差軌跡と前記プリセットゲインとの差である第２ゲインを構築するステップと、
前記第１相関係数が前記第２相関係数より小さい場合、第１末端アクチュエータに関連する、前記第１プリセット追従誤差軌跡と前記プリセットゲインとの差である前記第１ゲインを構築し、前記第２末端アクチュエータに関連する、前記第２プリセット追従誤差軌跡と前記プリセットゲインとの和である前記第２ゲインを構築するステップと、
前記第１ゲインと前記第１プリセット軌跡を重ね合わせて、第２軌跡を取得するステップと、
前記第２ゲインと前記第２プリセット軌跡を重ね合わせて、第３軌跡を取得するステップと、
前記第２軌跡を前記起動信号として、第１末端アクチュエータの前記サーボ制御ユニットの入力端に入力し、前記第３軌跡を前記起動信号として、第２末端アクチュエータの前記サーボ制御ユニットの入力端に入力するステップとを含む。
【0007】
さらに、初期信号を取得するステップは、
前記末端アクチュエータの振動周波数を取得するステップと、
前記振動周波数を除去するための畳み込み関数を構築して、畳み込みゲインを取得するステップと、
元の信号を取得するステップと、
前記元の信号と前記畳み込みゲインとの積を計算して、前記初期信号を取得するステップとを含む。
前記末端アクチュエータの振動周波数を取得するステップと、
前記振動周波数を除去するための畳み込み関数を構築して、畳み込みゲインを取得するステップと、
元の信号を取得するステップと、
前記元の信号と前記畳み込みゲインとの積を計算して、前記初期信号を取得するステップとを含む。
【0008】
さらに、周波数領域において、前記等価予測モデルは下式のとおりであり、
ここで、
は位置ループの比例係数であり、
は速度ループの比例係数であり、
は速度ループの積分係数であり、
は推力定数であり、ｍはリニアモータの質量であり、Ｂはモータの粘性摩擦係数であり、ｓは複素変数を表す。
ここで、
【0009】
さらに、円弧輪郭誤差によって前記相関関数を構築し、前記相関関数は下式のとおりであり、
ここで、
は前記末端経路輪郭誤差を表し、
は前記第１出力誤差軌跡を表し、
は前記第２出力誤差軌跡を表し、α、Ｒはそれぞれ前記ロボットのＸ軸又はＹ軸の軌跡参照点での円弧接線と該軸の正方向との間の角度及び円弧半径を表し、
前記第１相関係数を
に、前記第２相関係数を
に設定すると、
とする。
ここで、
前記第１相関係数を
とする。
【0010】
さらに、周波数領域において、前記畳み込み関数は下式のように設計され、
ここで、
はタイムラグであり、ｓは複素変数を表す。
ここで、
【発明の効果】
【0011】
要約すると、本出願は、ロボットの末端経路輪郭誤差の制御方法を提供し、初期信号を外部演算モジュールに入力することによって末端アクチュエータのプリセット軌跡を取得し、プリセット軌跡を等価予測モデルに入力して、等価予測軌跡を取得し、等価予測軌跡とプリセット軌跡との差を求めて、プリセット追従誤差軌跡を取得し、プリセット軌跡とプリセット追従誤差軌跡を重ね合わせて、第１軌跡を取得し、第１軌跡を起動信号として、ロボットの末端アクチュエータを制御するサーボ制御ユニットの入力端に入力し、ここでプリセット追従誤差軌跡は初期信号の補償量と見なすことができ、補償ポリシーを用いてサーボ帯域幅による誤差を補償することにより、末端輪郭の誤差が減少し、また、多軸ロボットの各軸の協調性が最適化され、ロボットの末端経路輪郭の精度が向上する。
【図面の簡単な説明】
【0012】
【図1】本出願の実施例が提供するロボットの末端経路輪郭誤差の制御方法のフローチャートである。
【図2】本出願の実施例が提供するＸＹ軸ロボットの輪郭誤差の制御方法のフローチャートである。
【図3】本出願の実施例が提供する振動抑制前後の元のプリセット軌跡及び初期プリセット軌跡の概略図である。
【図4】本出願の実施例が提供する振動抑制前後の元の信号及び初期信号の概略図である。
【図5】本出願の実施例が提供する補償前と補償後の末端アクチュエータの出力軌跡、補償軌跡及びプリセット軌跡の概略図である。
【図7】本出願の実施例が提供する末端アクチュエータのプリセット軌跡及び補償されていない出力軌跡の概略図である。
【発明を実施するための形態】
【0013】
以下では図面及び実施例を参照しながら本出願をさらに詳細に説明する。ただし、ここで説明した具体的な実施例は、かかる発明を説明するためのものに過ぎず、本発明を限定するものではないと理解されるべきである。また、説明の便宜上、図面には本発明に関連する部分のみを示す。
【0014】
なお、矛盾しない状況で、本出願の実施例及び実施例の特徴は互いに組み合わせることができる。以下では図面を参照しながら実施例に合わせて本出願を詳細に説明する。
【0015】
＜実施例１＞
背景技術で言及したように、従来技術の問題に対して、本出願は、ロボットの末端経路輪郭誤差の制御方法を提供し、前記ロボットは、末端アクチュエータ及び前記末端アクチュエータを制御するための制御モジュールを含み、前記制御モジュール内に前記末端アクチュエータを制御するためのサーボ制御ユニットが設けられ、前記サーボ制御ユニットの入力端が起動信号を受信し、制御信号を前記末端アクチュエータに出力して、前記末端アクチュエータを駆動して移動させ、前記制御モジュール内に前記サーボ制御ユニットに接続された外部演算モジュールをさらに含み、図１に示すように、前記制御方法は、
初期信号を取得するステップＳ１０と、
前記初期信号を前記外部演算モジュールに入力して、前記末端アクチュエータのプリセット軌跡を取得するステップＳ２０と、
等価予測モデルを構築するステップＳ３０と、
前記プリセット軌跡を前記等価予測モデルに入力して、等価予測軌跡を取得するステップＳ４０と、
前記プリセット軌跡と前記等価予測軌跡との差を求めて、プリセット追従誤差軌跡を取得するステップＳ５０と、
前記プリセット軌跡と前記プリセット追従誤差軌跡を重ね合わせて、第１軌跡を取得するステップＳ６０と、
前記第１軌跡を前記サーボ制御ユニットの入力端に入力して、補償軌跡を取得するステップＳ７０とを含むことを特徴とし、
前記ステップＳ１０において、前記初期信号は、前記末端アクチュエータの初期状態及び目標状態の分析により得られた形状であってもよく、前記初期信号はさらに、前記末端アクチュエータのプリセット経路形状であってもよく、具体的には、前記経路は円形、四角形等のプリセット形状であってもよい。
前記ステップＳ２０において、本実施例における前記外部演算モジュールはシングルチップマイクロコンピュータであってもよく、その型番は市販されている任意の型番であってもよく、前記初期信号を前記外部演算モジュールに入力して、前記末端アクチュエータが移動過程をシミュレートするように駆動できることにより得られた軌跡はプリセット軌跡として設定することができる。
前記ステップＳ３０において、本実施例における前記等価予測モデルは二次モデルである。
前記ステップＳ５０において、軌跡と経路との相違点は、時間の概念が追加され、複数の時刻の位置を前記末端アクチュエータの軌跡に組み合わせるため、前記プリセット軌跡と前記等価予測軌跡との差を求めると、各対応する時刻の前記プリセット軌跡の位置と前記等価予測軌跡の位置との差を求めて、複数の時刻の位置誤差点を取得し、複数の前記位置誤差点を接続すると、プリセット追従誤差軌跡が形成される。
前記ステップＳ６０において、同様に、対応する時刻の前記プリセット軌跡及び前記プリセット追従誤差軌跡の位置を加算して、複数の時刻の補償位置を取得し、複数の前記補償位置を接続すると、前記第１軌跡が形成される。
前記ステップＳ７０において、この時に、前記サーボ制御ユニットの入力は補償されたプリセット軌跡である。図５に示すように、図５の横座標は時間であり、単位は秒であり、縦座標は変位であり、単位はミリメートルである。図６は、図５の部分Ａの拡大図である。図６の横座標は時間であり、単位は秒であり、縦座標は変位であり、単位はミリメートルである。図６から分かるように、出力軌跡は、補償されていない前記初期信号をサーボ制御ユニットに入力した後に得られた軌跡であり、この出力軌跡とプリセット軌跡との間の誤差が大きい。プリセット軌跡が補償された第１軌跡をサーボ制御ユニットに入力した後に得られた補償軌跡とプリセット軌跡との間の誤差は極めて小さい。
背景技術で言及したように、従来技術の問題に対して、本出願は、ロボットの末端経路輪郭誤差の制御方法を提供し、前記ロボットは、末端アクチュエータ及び前記末端アクチュエータを制御するための制御モジュールを含み、前記制御モジュール内に前記末端アクチュエータを制御するためのサーボ制御ユニットが設けられ、前記サーボ制御ユニットの入力端が起動信号を受信し、制御信号を前記末端アクチュエータに出力して、前記末端アクチュエータを駆動して移動させ、前記制御モジュール内に前記サーボ制御ユニットに接続された外部演算モジュールをさらに含み、図１に示すように、前記制御方法は、
初期信号を取得するステップＳ１０と、
前記初期信号を前記外部演算モジュールに入力して、前記末端アクチュエータのプリセット軌跡を取得するステップＳ２０と、
等価予測モデルを構築するステップＳ３０と、
前記プリセット軌跡を前記等価予測モデルに入力して、等価予測軌跡を取得するステップＳ４０と、
前記プリセット軌跡と前記等価予測軌跡との差を求めて、プリセット追従誤差軌跡を取得するステップＳ５０と、
前記プリセット軌跡と前記プリセット追従誤差軌跡を重ね合わせて、第１軌跡を取得するステップＳ６０と、
前記第１軌跡を前記サーボ制御ユニットの入力端に入力して、補償軌跡を取得するステップＳ７０とを含むことを特徴とし、
前記ステップＳ１０において、前記初期信号は、前記末端アクチュエータの初期状態及び目標状態の分析により得られた形状であってもよく、前記初期信号はさらに、前記末端アクチュエータのプリセット経路形状であってもよく、具体的には、前記経路は円形、四角形等のプリセット形状であってもよい。
前記ステップＳ２０において、本実施例における前記外部演算モジュールはシングルチップマイクロコンピュータであってもよく、その型番は市販されている任意の型番であってもよく、前記初期信号を前記外部演算モジュールに入力して、前記末端アクチュエータが移動過程をシミュレートするように駆動できることにより得られた軌跡はプリセット軌跡として設定することができる。
前記ステップＳ３０において、本実施例における前記等価予測モデルは二次モデルである。
前記ステップＳ５０において、軌跡と経路との相違点は、時間の概念が追加され、複数の時刻の位置を前記末端アクチュエータの軌跡に組み合わせるため、前記プリセット軌跡と前記等価予測軌跡との差を求めると、各対応する時刻の前記プリセット軌跡の位置と前記等価予測軌跡の位置との差を求めて、複数の時刻の位置誤差点を取得し、複数の前記位置誤差点を接続すると、プリセット追従誤差軌跡が形成される。
前記ステップＳ６０において、同様に、対応する時刻の前記プリセット軌跡及び前記プリセット追従誤差軌跡の位置を加算して、複数の時刻の補償位置を取得し、複数の前記補償位置を接続すると、前記第１軌跡が形成される。
前記ステップＳ７０において、この時に、前記サーボ制御ユニットの入力は補償されたプリセット軌跡である。図５に示すように、図５の横座標は時間であり、単位は秒であり、縦座標は変位であり、単位はミリメートルである。図６は、図５の部分Ａの拡大図である。図６の横座標は時間であり、単位は秒であり、縦座標は変位であり、単位はミリメートルである。図６から分かるように、出力軌跡は、補償されていない前記初期信号をサーボ制御ユニットに入力した後に得られた軌跡であり、この出力軌跡とプリセット軌跡との間の誤差が大きい。プリセット軌跡が補償された第１軌跡をサーボ制御ユニットに入力した後に得られた補償軌跡とプリセット軌跡との間の誤差は極めて小さい。
【0016】
さらに、前記等価予測モデルは下式のとおりであり、
ここで、
は位置ループの比例係数であり、
は速度ループの比例係数であり、
は速度ループの積分係数であり、
は推力定数であり、ｍはリニアモータの質量であり、Ｂはモータの粘性摩擦係数である。
ここで、
【0017】
前記プリセット追従誤差軌跡を補償量として、前記サーボ制御ユニットの入力端に補償し、前記サーボ制御ユニットは電流ループ、速度ループ及び位置ループを含み、前記プリセット追従誤差軌跡をフィードフォワード補償により前記速度ループの所定位置に補償すると、補償後の前記起動信号は、前記プリセット追従誤差軌跡と前記プリセット軌跡との重ね合わせに等しい。
【0018】
＜実施例２＞
実施例１に基づいて、前記ロボットはＸＹ軸２軸ロボットであり、Ｘ軸に対応する第１末端アクチュエータ及びＹ軸に対応する第２末端アクチュエータを含み、前記第１末端アクチュエータと前記第２末端アクチュエータとの間に結合関係があり、図２に示すように、前記制御方法は、
前記第１末端アクチュエータの第１初期信号及び前記第２末端アクチュエータの第２初期信号をそれぞれ取得するステップＳ８１と、
ステップＳ２０を実行して、前記第１末端アクチュエータの第１プリセット軌跡及び前記第２末端アクチュエータの第２プリセット軌跡を取得するステップＳ８２と、
前記第１プリセット軌跡を前記起動信号として、前記第１末端アクチュエータの前記サーボ制御ユニットに入力して第１出力軌跡を取得し、前記第２プリセット軌跡を前記起動信号として、前記第２末端アクチュエータの前記サーボ制御ユニットに入力して第２出力軌跡を取得するステップＳ８３と、
前記第１プリセット軌跡と前記第１出力軌跡との差を求めて、第１出力誤差軌跡を取得し、前記第２プリセット軌跡と前記第２出力軌跡との差を求めて、第２出力誤差軌跡を取得するステップＳ８４と、
前記第１出力誤差軌跡及び前記第２出力誤差軌跡で前記末端経路輪郭誤差に関連する相関関数を構築して、前記第１出力誤差軌跡に対応する第１相関係数及び前記第２出力誤差軌跡に対応する第２相関係数を取得するステップＳ８５と、
ステップＳ３０～Ｓ５０を実行して、前記第１末端アクチュエータの第１プリセット追従誤差軌跡及び前記第２末端アクチュエータの第２プリセット追従誤差軌跡を取得するステップＳ８６と、
前記第１相関係数と前記第２相関係数の大きさを判断し、前記第１相関係数が前記第２相関係数より大きい場合、前記第１末端アクチュエータに関連する、前記第１プリセット追従誤差軌跡とプリセットゲインとの和である第１ゲインを構築し、第２末端アクチュエータに関連する、前記第２プリセット追従誤差軌跡と前記プリセットゲインとの差である第２ゲインを構築するステップＳ８７と、
前記第１相関係数が前記第２相関係数より小さい場合、第１末端アクチュエータに関連する、前記第１プリセット追従誤差軌跡と前記プリセットゲインとの差である前記第１ゲインを構築し、前記第２末端アクチュエータに関連する、前記第２プリセット追従誤差軌跡と前記プリセットゲインとの和である前記第２ゲインを構築するステップＳ８８と、
前記第１ゲインと前記第１プリセット軌跡を重ね合わせて、第２軌跡を取得するステップＳ８９と、
前記第２ゲイン及び前記第２プリセット軌跡を重ね合わせて、第３軌跡を取得するステップＳ９０と、
前記第２軌跡を前記起動信号として、第１末端アクチュエータの前記サーボ制御ユニットの入力端に入力し、前記第３軌跡を前記起動信号として、第２末端アクチュエータの前記サーボ制御ユニットの入力端に入力するステップＳ９１とを含む。
実施例１に基づいて、前記ロボットはＸＹ軸２軸ロボットであり、Ｘ軸に対応する第１末端アクチュエータ及びＹ軸に対応する第２末端アクチュエータを含み、前記第１末端アクチュエータと前記第２末端アクチュエータとの間に結合関係があり、図２に示すように、前記制御方法は、
前記第１末端アクチュエータの第１初期信号及び前記第２末端アクチュエータの第２初期信号をそれぞれ取得するステップＳ８１と、
ステップＳ２０を実行して、前記第１末端アクチュエータの第１プリセット軌跡及び前記第２末端アクチュエータの第２プリセット軌跡を取得するステップＳ８２と、
前記第１プリセット軌跡を前記起動信号として、前記第１末端アクチュエータの前記サーボ制御ユニットに入力して第１出力軌跡を取得し、前記第２プリセット軌跡を前記起動信号として、前記第２末端アクチュエータの前記サーボ制御ユニットに入力して第２出力軌跡を取得するステップＳ８３と、
前記第１プリセット軌跡と前記第１出力軌跡との差を求めて、第１出力誤差軌跡を取得し、前記第２プリセット軌跡と前記第２出力軌跡との差を求めて、第２出力誤差軌跡を取得するステップＳ８４と、
前記第１出力誤差軌跡及び前記第２出力誤差軌跡で前記末端経路輪郭誤差に関連する相関関数を構築して、前記第１出力誤差軌跡に対応する第１相関係数及び前記第２出力誤差軌跡に対応する第２相関係数を取得するステップＳ８５と、
ステップＳ３０～Ｓ５０を実行して、前記第１末端アクチュエータの第１プリセット追従誤差軌跡及び前記第２末端アクチュエータの第２プリセット追従誤差軌跡を取得するステップＳ８６と、
前記第１相関係数と前記第２相関係数の大きさを判断し、前記第１相関係数が前記第２相関係数より大きい場合、前記第１末端アクチュエータに関連する、前記第１プリセット追従誤差軌跡とプリセットゲインとの和である第１ゲインを構築し、第２末端アクチュエータに関連する、前記第２プリセット追従誤差軌跡と前記プリセットゲインとの差である第２ゲインを構築するステップＳ８７と、
前記第１相関係数が前記第２相関係数より小さい場合、第１末端アクチュエータに関連する、前記第１プリセット追従誤差軌跡と前記プリセットゲインとの差である前記第１ゲインを構築し、前記第２末端アクチュエータに関連する、前記第２プリセット追従誤差軌跡と前記プリセットゲインとの和である前記第２ゲインを構築するステップＳ８８と、
前記第１ゲインと前記第１プリセット軌跡を重ね合わせて、第２軌跡を取得するステップＳ８９と、
前記第２ゲイン及び前記第２プリセット軌跡を重ね合わせて、第３軌跡を取得するステップＳ９０と、
前記第２軌跡を前記起動信号として、第１末端アクチュエータの前記サーボ制御ユニットの入力端に入力し、前記第３軌跡を前記起動信号として、第２末端アクチュエータの前記サーボ制御ユニットの入力端に入力するステップＳ９１とを含む。
【0019】
ここで、前記ロボットは多軸末端アクチュエータを含み、多軸の間に結合関係があり、ロボットの多軸の間に差異があり、例えば前記末端アクチュエータの固有周波数が異なり、各軸モータのタイプ選択及びサーボループパラメータが異なる等であるため、各軸で発生するタイムラグも異なり、この時、協調制御を行わないと軌跡がずれ、したがって、本実施例における方法は、各軸のサーボ帯域幅のフィードフォワード補償において補償量を各軸が一致するように調整することを総合的に考慮して、多軸協調の効果を達成し、前記ロボットの末端軌跡がずれるという問題を解消するために用いられる。図７に示すように、ここで、図７の横座標は変位であり、単位はセンチメートルであり、縦軸は変位を表し、単位はセンチメートルである。本実施例の前記ロボットの末端アクチュエータのモータのプリセット軌跡は円形であるが、Ｘ軸とＹ軸にタイムラグがあるため、プリセット軌跡がずれて円形ではなく、即ち補償が行われていない前に、前記プリセット軌跡を信号として、前記サーボ制御ユニットの入力端に入力すると、前記サーボ制御ユニットの出力軌跡は非円形であり、図８は、前記プリセット軌跡と前記出力軌跡との間の誤差であり、ここで、横座標は時間を表し、単位は秒であり、縦座標は誤差値を表し、単位はセンチメートルであり、誤差が小さいため、図７の前記プリセット軌跡と前記出力軌跡はほぼ重なっている。
【0020】
本実施例はＸＹ２軸ロボットを例とし、Ｘ軸とＹ軸に結合関係がある場合、Ｘ軸に対応する前記第１出力誤差軌跡及びＹ軸に対応する前記第２出力誤差軌跡と前記末端経路輪郭誤差との間に相関性があり、前記第１出力誤差軌跡と前記第１相関係数との積と、前記第２出力誤差軌跡と前記第２相関係数との積との和は前記末端経路輪郭誤差であり、前記第１相関係数及び前記第２相関係数を決定する方法は多く、選択的に、本実施例は円弧輪郭誤差によって前記相関関数を構築し、前記相関関数は下式を参照し、
ここで、
は前記末端経路輪郭誤差を表し、
は前記第１出力誤差軌跡を表し、
は前記第２出力誤差軌跡を表し、α、Ｒはそれぞれ前記ロボットのＸ軸又はＹ軸の軌跡参照点での円弧接線と該軸の正方向との間の角度及び円弧半径を表し、
前記第１相関係数を
に、前記第２相関係数を
に設定すると、
とし、
式（２）から、前記第１出力誤差軌跡及び前記第２出力誤差軌跡と前記末端輪郭誤差との間の相関性を得ることができ、前記第１相関係数が前記第２相関係数より大きい場合、前記第１出力誤差軌跡は前記末端輪郭誤差に大きな影響を与え、逆に、前記第１相関係数が前記第２相関係数より小さい場合、前記第１出力誤差軌跡は前記末端輪郭誤差に小さな影響を与え、したがって、前記第１出力誤差軌跡及び前記第２出力誤差軌跡が前記末端輪郭誤差に与える影響をバランスさせるために、プリセットゲインを導入する。
ここで、
前記第１相関係数を
とし、
式（２）から、前記第１出力誤差軌跡及び前記第２出力誤差軌跡と前記末端輪郭誤差との間の相関性を得ることができ、前記第１相関係数が前記第２相関係数より大きい場合、前記第１出力誤差軌跡は前記末端輪郭誤差に大きな影響を与え、逆に、前記第１相関係数が前記第２相関係数より小さい場合、前記第１出力誤差軌跡は前記末端輪郭誤差に小さな影響を与え、したがって、前記第１出力誤差軌跡及び前記第２出力誤差軌跡が前記末端輪郭誤差に与える影響をバランスさせるために、プリセットゲインを導入する。
【0021】
実施例１から分かるように、単軸ロボットの末端アクチュエータについては、補償量をプリセット軌跡に導入した後、その末端経路輪郭誤差が減少し、したがって、ロボットの末端経路輪郭の精度を向上させるために、２軸ロボットの各末端アクチュエータも補償量を導入する必要があり、２軸の協調性を満たすために、２軸の補償量をバランスさせる必要がある。
【0022】
前記プリセットゲインを導入することにより、前記第１出力誤差軌跡及び前記第２出力誤差軌跡が前記末端輪郭誤差に与える影響をバランスさせるために、前記第１相関係数が前記第２相関係数より大きい場合、前記第１プリセット軌跡に対して大きな補償を行い、前記第２プリセット軌跡に対して小さな補償を行い、前記第１相関係数が前記第２相関係数より小さい場合、前記第１プリセット軌跡に対して小さな補償を行い、前記第２プリセット軌跡に対して大きな補償を行い、その式は以下のとおりであり、
ここで、
は前記第１ゲインであり、
は前記第２ゲインであり、
は前記第１プリセット追従誤差軌跡であり、
は前記第２プリセット追従誤差軌跡であり、Ｋは前記プリセットゲインである。
ここで、
【0023】
＜実施例３＞
実施例１及び実施例２に基づいて、さらに、初期信号を取得するステップは、
前記末端アクチュエータの振動周波数を取得するステップＳ０１と、
前記振動周波数を除去するための畳み込み関数を構築して、畳み込みゲインを取得するステップＳ０２と、
元の信号を取得するステップＳ０３と、
前記元の信号と畳み込みゲインとの積を計算して、初期信号を取得するステップＳ０４とを含み、
前記ステップＳ０１において、前記ロボットは可撓性末端であるため、末端に不可避的に振動が発生する可能性があり、各軸の軌跡計画に基づき、振動を発生させる前記振動周波数を時間領域解析と周波数領域解析の組み合わせにより判断し、ここで時間領域解析は、末端追従信号における２つの末端位置の極値点の時間差を選択することである。周波数領域解析は、末端追従信号に対してフーリエ解析を行うことである。
前記ステップＳ０２において、前記振動周波数を除去する方法は振動抑制と呼ばれる。好ましくは、本実施例における畳み込み関数は下式のように設計され、
ここで、
はタイムラグであり、
は前記畳み込みゲインである。
前記ステップＳ０３において、図４に示すように、
を前記元の信号に設定し、この元の信号を入力として、前記外部演算モジュールに入力して、前記末端アクチュエータの元のプリセット軌跡を取得し、図３に示すように、ここで、横座標は時間を表し、単位は秒であり、縦座標は変位を表し、単位はミリメートルである。
前記ステップＳ０４において、前記元の信号と前記畳み込みゲインとの積は畳み込みパルス関数であり、
ここで、
は畳み込みゲインであり、
は振動抑制後の信号、即ち初期信号であり、
は前記元の信号である。
実施例１及び実施例２に基づいて、さらに、初期信号を取得するステップは、
前記末端アクチュエータの振動周波数を取得するステップＳ０１と、
前記振動周波数を除去するための畳み込み関数を構築して、畳み込みゲインを取得するステップＳ０２と、
元の信号を取得するステップＳ０３と、
前記元の信号と畳み込みゲインとの積を計算して、初期信号を取得するステップＳ０４とを含み、
前記ステップＳ０１において、前記ロボットは可撓性末端であるため、末端に不可避的に振動が発生する可能性があり、各軸の軌跡計画に基づき、振動を発生させる前記振動周波数を時間領域解析と周波数領域解析の組み合わせにより判断し、ここで時間領域解析は、末端追従信号における２つの末端位置の極値点の時間差を選択することである。周波数領域解析は、末端追従信号に対してフーリエ解析を行うことである。
前記ステップＳ０２において、前記振動周波数を除去する方法は振動抑制と呼ばれる。好ましくは、本実施例における畳み込み関数は下式のように設計され、
ここで、
前記ステップＳ０３において、図４に示すように、
前記ステップＳ０４において、前記元の信号と前記畳み込みゲインとの積は畳み込みパルス関数であり、
【0024】
図４に示すように、ここで、図４の横座標は時間であり、単位は秒であり、縦座標は変位であり、単位はミリメートルであり、図４から分かるように、振動抑制後の前記初期信号と前記元の信号を比較して、前記振動周波数が除去されていない元の信号の応答曲線は大きな振動を示し、振動抑制後の初期信号の応答曲線は該現象がない。振動抑制後の前記初期信号を入力とし、前記外部演算モジュールに入力して、前記末端アクチュエータの初期プリセット軌跡を取得し、図３に示すように、前記初期プリセット軌跡は前記元のプリセット軌跡より滑らかである。
【0025】
さらに、前記ロボットが可撓性ロボットである場合、まず、前記畳み込み関数によって末端振動を引き起こす周波数を除去し、次に、振動周波数が除去された信号を前記外部演算モジュールに入力して経路計画を行ってプリセット軌跡を取得し、プリセット軌跡をプリセット等価モデルに入力して等価予測軌跡を取得し、プリセット軌跡と等価予測軌跡との差を求めて、プリセット追従誤差軌跡を取得し、プリセット追従誤差軌跡を補償量として、前記プリセット軌跡を補償して、前記起動信号を取得する。本実施例では、まず振動抑制を行い、次に補償を行い、前記振動抑制により低周波信号を除去するが、補償時に高周波成分であるため、高周波成分を補償する時に振動抑制に影響を与えない。
【0026】
本明細書では、具体的な例を用いて本出願の原理及び実施形態を説明したが、以上の実施例の説明は、本出願の方法及びその中核思想の理解を助けるためのものに過ぎない。以上は本出願の好ましい実施形態に過ぎず、指摘すべきこととして、文字表現の有限性を有する一方、客観的に無限の具体的な構造があるため、当業者であれば、本発明の原理から逸脱することなく、さらにいくつかの改良、潤色又は変化を行うことができ、上記技術的特徴を適切な方式で組み合わせることもできる。これらの改良、潤色、変化又は組み合わせ、又は改良せずに発明の構想及び技術的解決手段を他の場面に直接適用することは、いずれも本出願の保護範囲と見なされるべきである。
【図1】
【図2】
【図3】
【図4】
【図5】
【図6】
【図7】
【図8】