特許 7120512 軌跡制御装置

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

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

(11)【特許番号】

(24)【登録日】2022-08-08

(45)【発行日】2022-08-17

(54)【発明の名称】軌跡制御装置

(51)【国際特許分類】

   G05B 19/404 20060101AFI20220809BHJP

   B23Q 15/00 20060101ALI20220809BHJP

   B25J 9/10 20060101ALI20220809BHJP

   B23Q 35/04 20060101ALI20220809BHJP

   B23Q 35/121 20060101ALI20220809BHJP

   G05B 19/4097 20060101ALI20220809BHJP

   G05B 19/19 20060101ALI20220809BHJP

   B23Q 17/00 20060101ALI20220809BHJP

   B23K 9/127 20060101ALN20220809BHJP

【ＦＩ】

G05B19/404 H

B23Q15/00 305C

B25J9/10 A

B23Q35/04 B

B23Q35/121

G05B19/4097 C

G05B19/19 F

B23Q17/00 A

B23K9/127 505E

【請求項の数】 3

(21)【出願番号】P 2019211058

(22)【出願日】2019-11-22

(65)【公開番号】P2021082170

(43)【公開日】2021-05-27

【審査請求日】2021-06-24

(73)【特許権者】

【識別番号】000102511

【氏名又は名称】ＳＭＣ株式会社

(74)【代理人】

【識別番号】100077665

【弁理士】

【氏名又は名称】千葉 剛宏

(74)【代理人】

【識別番号】100116676

【弁理士】

【氏名又は名称】宮寺 利幸

(74)【代理人】

【識別番号】100191134

【弁理士】

【氏名又は名称】千馬 隆之

(74)【代理人】

【識別番号】100136548

【弁理士】

【氏名又は名称】仲宗根 康晴

(74)【代理人】

【識別番号】100136641

【弁理士】

【氏名又は名称】坂井 志郎

(74)【代理人】

【識別番号】100180448

【弁理士】

【氏名又は名称】関口 亨祐

(72)【発明者】

【氏名】脇坂 新路

(72)【発明者】

【氏名】小熊 和行

(72)【発明者】

【氏名】安田 光

【審査官】亀田 貴志

(56)【参考文献】

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

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

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

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

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

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

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

【文献】国際公開第１９９２／００５３９９（ＷＯ，Ａ１）

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

Ｇ０５Ｂ １９／１８ － １９／４１６

Ｇ０５Ｂ １９／４２ － １９／４６

Ｂ２３Ｑ １５／００

Ｂ２３Ｑ １７／００

Ｂ２５Ｊ ９／１０

Ｂ２３Ｑ ３５／００

Ｂ２３Ｋ ９／１２７

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

【請求項1】

任意位置に置かれたワーク上の軌跡に沿って軌跡追従部材を移動させる軌跡制御装置であって、前記ワーク側面に接触可能な接触センサと、前記軌跡追従部材および前記接触センサを移動させるアクチュエータと、基準位置の前記ワーク側面の位置情報と前記任意位置に置かれた前記ワーク側面の位置情報に基づいて前記基準位置の前記ワーク上の軌跡のＸＹ座標を変換することにより前記任意位置に置かれた前記ワーク上の軌跡のＸＹ座標を計算する軌跡コントローラとを備え、前記任意位置に置かれた前記ワーク側面の位置情報は前記接触センサによって得られるものであり、
前記軌跡コントローラは、前記アクチュエータを駆動して前記接触センサをＸ軸に平行な２つの移動方向に進めることにより、前記ワークの１つの側面における２つの接触点のＸＹ座標を検出するとともに、前記アクチュエータを駆動して前記接触センサをＹ軸に平行な２つの移動方向に進めることにより前記ワークの他の１つの側面における２つの接触点のＸＹ座標を検出し、前記ワークの１つの側面における２つの接触点を通る第１の直線の方程式および前記ワークの他の１つの側面における２つの接触点を通る第２の直線の方程式を求め、さらに前記第１の直線と前記第２の直線との交点のＸＹ座標を求め、前記ワークの基準点のＸ座標移動量とＹ座標移動量を計算するとともに、前記ワークの他の１つの側面について前記ワークの基準位置からの傾き角度を計算し、前記ワークの基準点のＸ座標移動量、前記ワークの基準点のＹ座標移動量および前記傾き角度に基づいて、アフィン変換を使って、前記任意位置に置かれた前記ワーク上の軌跡のＸＹ座標を計算する軌跡制御装置。

【請求項2】

請求項１記載の軌跡制御装置において、
前記アフィン変換は下記の式によるものである軌跡制御装置（ただし、式中の文字ないし記号の意味は下記のとおりとする）。
ｘ´＝（ｘ）Ｃｏｓ（Δθ）－（ｙ）Ｓｉｎ（Δθ）＋ｔｘ
ｙ´＝（ｘ）Ｓｉｎ（Δθ）＋（ｙ）Ｃｏｓ（Δθ）＋ｔｙ
ｘ´：前記任意位置に置かれた前記ワーク上の軌跡のＸ座標
ｙ´：前記任意位置に置かれた前記ワーク上の軌跡のＹ座標
ｘ：前記基準位置の前記ワーク上の軌跡のＸ座標
ｙ：前記基準位置の前記ワーク上の軌跡のＹ座標
Δθ：前記傾き角度
ｔｘ：前記ワークの基準点のＸ座標移動量にアフィン変換による前記基準点のＸ座標移動量を加えたもの
ｔｙ：前記ワークの基準点のＹ座標移動量にアフィン変換による前記基準点のＹ座標移動量を加えたもの

【請求項3】

請求項２記載の軌跡制御装置において、
前記アフィン変換による前記基準点のＸ座標移動量は、前記基準点のＸ座標と、前記基準点をＸＹ座標系の原点を中心として前記Δθだけ回転したときのＸ座標との差を計算したものであり、前記アフィン変換による前記基準点のＹ座標移動量は、前記基準点のＹ座標と、前記基準点をＸＹ座標系の原点を中心として前記Δθだけ回転したときのＹ座標との差を計算したものである軌跡制御装置。

【発明の詳細な説明】

【技術分野】

【0001】

本発明は、ワーク上の軌跡に沿ってディスペンサ等の部材を移動せしめる軌跡制御装置に関する。

【背景技術】

【0002】

産業用ロボットによるディスペンサのライン塗布や溶接等の軌跡制御において、軌跡の位置を検出してワークの位置ずれによる軌跡の補正を行うために、高解像度カメラや画像センサ等のセンサ技術が使用されている。

【0003】

例えば、特許文献１には、レーザスリット光を照射可能なセンサを用いて溶接線の位置を検出しながら、溶接トーチの位置を補正するトラッキング制御を行うロボットシステムが記載されている。

【先行技術文献】

【特許文献】

【0004】

【文献】特開２００８－２７２８１４号公報

【発明の概要】

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

【0005】

しかしながら、高解像度カメラや画像センサを用いることは、高価で複雑なシステムを導入することになる。

【0006】

本発明は、このような事情を背景としてなされたものであり、高精度カメラや画像センサを使わなくても、ワークの位置ずれやその移動量を認識して軌跡補正を行うことができる軌跡制御装置を提供することを目的とする。

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

【0007】

本発明に係る軌跡制御装置は、任意位置に置かれたワーク上の軌跡に沿って軌跡追従部材を移動させるもので、ワーク側面に接触可能な接触センサと、軌跡追従部材および接触センサを移動させるアクチュエータと、基準位置のワーク側面の位置情報と任意位置に置かれたワーク側面の位置情報に基づいて基準位置のワーク上の軌跡のＸＹ座標を変換することにより任意位置に置かれたワーク上の軌跡のＸＹ座標を計算する軌跡コントローラとを備える。上記任意位置に置かれたワーク側面の位置情報は、接触センサによって得られる。

【0008】

上記軌跡制御装置によれば、高精度カメラや画像センサを使うことなく、任意位置に置かれたワーク上の軌跡のＸＹ座標を簡単な手法で求めることができる。

【発明の効果】

【0009】

本発明に係る軌跡制御装置は、接触センサによって得られるワーク側面の位置情報に基づいて、既知のデータである基準位置のワーク上の軌跡のＸＹ座標から任意位置に置かれたワーク上の軌跡のＸＹ座標を計算するものである。したがって、高精度カメラや画像センサを使うことなく、任意位置に置かれたワーク上の軌跡のＸＹ座標を簡単な手法で求めることができる。

【図面の簡単な説明】

【0010】

【図1】本発明の実施形態に係る軌跡制御装置の概念図である。

【図2】図１の軌跡制御装置のＸＹ座標系とそこに置かれたワークを示した図である。

【図3】ワークが回転を伴わないで移動した具体例における制御テーブルＴ１に保存されたデータを示した図表である。

【図4】図３の具体例における制御テーブルＴ２に保存されたデータを示した図表である。

【図5】図３の具体例における制御テーブルＴ３～Ｔ５に保存されたデータを示した図表である。

【図6】図３の具体例における制御テーブルＴ６に保存されたデータを示した図表である。

【図7】図３の具体例における制御テーブルＴ７に保存されたデータを示した図表である。

【図8】図３の具体例におけるワークアライメントの座標変換結果を示した図表である。

【図9】図３の具体例における軌跡データの座標変換結果を示した図表である。

【図10】ワークが回転を伴って移動した具体例における制御テーブルＴ１に保存されたデータを示した図表である。

【図11】図１０の具体例における制御テーブルＴ２に保存されたデータを示した図表である。

【図12】図１０の具体例における制御テーブルＴ３～Ｔ５に保存されたデータを示した図表である。

【図13】図１０の具体例における制御テーブルＴ６に保存されたデータを示した図表である。

【図14】図１０の具体例における制御テーブルＴ７に保存されたデータを示した図表である。

【図15】図１０の具体例におけるワークアライメントの座標変換結果を示した図表である。

【図16】図１０の具体例における軌跡データの座標変換結果を示した図表である。

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

【0011】

以下、本発明に係る軌跡制御装置について、好適な実施形態を挙げ、添付の図面を参照しながら説明する。

【0012】

図１に示されるように、軌跡制御装置１０は、軌跡コントローラ１２、接触センサ２６、軌跡追従部材２８および電動アクチュエータ群２４を含む。軌跡コントローラ１２は、軌跡制御部１４、モータ制御部１６、補正量演算部１８、座標変換部２０およびＩ／Ｏ部（入出力部）２２を有する。軌跡コントローラ１２のこれらの各部は、１つまたは複数のハードウエアやソフトウエアで実現されている。

【0013】

軌跡コントローラ１２は、軌跡制御部１４が主体となって、電動アクチュエータ群２４を駆動して接触センサ２６を移動させることによりワークＷの位置を検出する検出制御を行うほか、電動アクチュエータ群２４を駆動して軌跡追従部材２８をワーク上の軌跡に沿って移動させることにより軌跡制御を行う。

【0014】

接触センサ２６は、ワークＷの側面に機械的に接触することでワークＷの位置を検出するものである。軌跡追従部材２８は、溶接ガンの先端部、あるいは、接着剤やシール材を供給するディスペンサノズルの先端部に相当する部材であり、溶接ラインや塗布ラインにおいて設置される装置に取り付けられる。

【0015】

電動アクチュエータ群２４は、Ｘ軸アクチュエータ２４ａ、Ｙ軸アクチュエータ２４ｂおよびＺ軸アクチュエータ２４ｃから構成される。接触センサ２６と軌跡追従部材２８は、いずれもＺ軸アクチュエータ２４ｃに取り付けられている。Ｘ軸アクチュエータ２４ａとＹ軸アクチュエータ２４ｂは、接触センサ２６と軌跡追従部材２８をＸＹ座標系の平面上で任意の位置に移動させる役割を担う。

【0016】

Ｚ軸アクチュエータ２４ｃは、接触センサ２６と軌跡追従部材２８のＺ軸方向（上下方向）の位置を調整する役割を担う。軌跡コントローラ１２が検出制御を行う場合、接触センサ２６が下方の作動位置に移動し、軌跡追従部材２８が上方の退避位置に移動する。一方、軌跡コントローラ１２が軌跡制御を行う場合、軌跡追従部材２８が下方の作動位置に移動し、接触センサ２６が上方の退避位置に移動する。

【0017】

軌跡制御部１４は、ワーク上の軌跡データおよびモータ制御データからなる軌跡制御データを保持している。軌跡データは、ＣＡＤで作成されたＤＸＦファイルなどの電子データであり、PolylineやArcなどの属性コマンドとＸＹ座標データで構成されている。モータ制御データは、Ｘ軸アクチュエータ２４ａ、Ｙ軸アクチュエータ２４ｂおよびＺ軸アクチュエータ２４ｃの位置、速度、加速度等の電子データである。

【0018】

また、軌跡制御部１４は、ワークＷの側面および基準点に関する属性データとともに、ワークＷの平面形状を表すＸＹ座標データ（以下「ワークアライメント」という）を保持している。このワークアライメントには、少なくとも、基準点を間に挟んで隣接する２つの側面に関するデータが含まれている。

【0019】

モータ制御部１６は、Ｘ軸アクチュエータ２４ａとＹ軸アクチュエータ２４ｂのモータエンコーダから送られてくるエンコーダ値を、これらのアクチュエータの位置データとしてリアルタイムに更新保持し、軌跡制御部１４からの軌跡制御データに基づいて、Ｘ軸アクチュエータ２４ａとＹ軸アクチュエータ２４ｂを駆動制御する。

【0020】

補正量演算部１８は、接触センサ２６で検出されたワーク側面の位置情報に基づいて、ワーク上の軌跡の補正量（補正係数）を演算する。座標変換部２０は、補正量演算部１８で演算された補正量（補正係数）に基づいて、任意位置に置かれたワーク上の軌跡の座標を計算する。Ｉ／Ｏ部２２は、周辺機器と同期をとるためのもので、Ｉ／Ｏ部２２には、軌跡追従部材２８と接触センサ２６が接続されている。

【0021】

軌跡コントローラ１２は、検出制御において、Ｘ軸アクチュエータ２４ａとＹ軸アクチュエータ２４ｂを駆動して、接触センサ２６をワークＷの側面の方向へ移動させる。接触センサ２６は、ワークＷの側面に接触すると、接触したことをＩ／Ｏ部２２を介して軌跡コントローラ１２へ伝える。軌跡コントローラ１２は、接触センサ２６がワークＷの側面に接触したことを認識すると、Ｘ軸アクチュエータ２４ａとＹ軸アクチュエータ２４ｂを停止させる。

【0022】

次いで、軌跡コントローラ１２は、Ｘ軸アクチュエータ２４ａのモータエンコーダ値とＹ軸アクチュエータ２４ｂのモータエンコーダ値をモータ制御部１６から読み込み、接触センサ２６によるワークＷの側面上の接触点をＸＹ座標データとして保持する。

【0023】

ワークＷの１つの側面について、異なる２つの接触点のＸＹ座標データが得られるように、上記一連の処理を繰り返す。同様に、ワークＷのもう１つの側面についても、異なる２つの接触点のＸＹ座標データが得られるように、上記一連の処理を繰り返す。これらワークＷの２つの側面が交差する位置が基準点となっている。

【0024】

図２は、軌跡制御装置１０を真上から見たＸＹ座標系とそこに置かれたワークＷの位置を表した図であり、また、補正量演算部１８における処理を説明するための図である。

【0025】

このＸＹ座標系において、基準位置に置かれた直方体形状のワークＷの４つの側面は、点Ｐ１と点Ｐ２を結ぶ線、点Ｐ２と点Ｐ３を結ぶ線、点Ｐ３と点Ｐ４を結ぶ線および点Ｐ４と点Ｐ１を結ぶ線で示されている。４つの点Ｐ１～Ｐ４のうち、Ｐ１が基準点となっている。以下において、点Ｐ４と点Ｐ１を結ぶ線で示される側面をワーク側面Ａといい、点Ｐ１と点Ｐ２を結ぶ線で示される側面をワーク側面Ｂという。

【0026】

本実施形態では、ワーク側面ＡがＹ軸に平行、かつ、ワーク側面ＢがＸ軸に平行であるようにワークＷが置かれた位置を基準位置としているが、例えば、ワーク側面Ａおよびワーク側面ＢがＸ軸にもＹ軸にも平行でない状態で置かれた位置が基準位置であってもよい。

【0027】

また、対象とするワークＷの平面形状としては、必ずしも矩形である必要はなく、輪郭の一部に２つの直線部分を有していればよい。換言すれば、ワークＷの側面に少なくとも２つの平面部分が存在すればよい。なお、軌跡データは、ワークアライメントの矩形の範囲内に１つまたは複数存在するか、あるいは、ワークアライメントの矩形の範囲外に存在してもよい。また、複数のワークＷが１つのトレイに固定されていてもよい。

【0028】

以下、図２を参照しながら、検出制御について、さらに具体的に説明する。基準位置にあるワークアライメントＷＰ（Ｐ１－Ｐ２－Ｐ３－Ｐ４）は、あらかじめ軌跡コントローラ１２に保持されているので、ワーク側面の位置を検出する必要はないが、ここでは、まず、基準位置にワークＷが置かれているものとして説明する。したがって、実際には、基準位置に置かれたワークＷについて、ワーク側面の検出は省略される場合がある。

【0029】

接触センサ２６は、Ｘ軸に平行な第１の移動方向３０に進み、ワーク側面Ａ上の点Ａ１に到達して止まり、Ａ１のＸ座標とＹ座標を検出して保持する。同様に、接触センサ２６は、Ｘ軸に平行な第２の移動方向３２に進み、ワーク側面Ａ上の点Ａ２に到達して止まり、Ａ２のＸ座標とＹ座標を検出して保持する。本実施形態では、第１の移動方向３０は、ｙ＝４０で表される直線に沿う方向であり、第２の移動方向３２は、ｙ＝３０で表される直線に沿う方向である。

【0030】

また、接触センサ２６は、Ｙ軸に平行な第３の移動方向３４に進み、ワーク側面Ｂ上の点Ｂ１に到達して止まり、Ｂ１のＸ座標とＹ座標を検出して保持する。同様に、接触センサ２６は、Ｙ軸に平行な第４の移動方向３６に進み、ワーク側面Ｂ上の点Ｂ２に到達して止まり、Ｂ２のＸ座標とＹ座標を検出して保持する。本実施形態では、第３の移動方向３４は、ｘ＝５０で表される直線に沿う方向であり、第４の移動方向３６は、ｘ＝４０で表される直線に沿う方向である。

【0031】

本実施形態では、Ａ１とＡ２を通る直線の方程式はｘ＝１０で表され、Ｂ１とＢ２を通る直線の方程式はｙ＝１０で表される。これら２つの直線の交点が基準点Ｐ１であり、基準点Ｐ１のＸＹ座標は（１０、１０）である。なお、これら２つの直線については、２点を通る直線の一般式をｙ＝ａｘ＋ｂとした場合の係数ａおよび係数ｂはいずれもゼロ（ａ＝０、ｂ＝０）として扱う。

【0032】

次に、任意位置に置かれたワークアライメントＷＱについて説明する。任意位置に置かれたワークアライメントＷＱは、基準位置に置かれたワークアライメントＷＰの４つの点Ｐ１～Ｐ４にそれぞれ対応する点Ｑ１～Ｑ４を含む。以下において、点Ｑ４と点Ｑ１を結ぶ線で示される側面をワーク側面Ｃといい、点Ｑ１と点Ｑ２を結ぶ線で示される側面をワーク側面Ｄという。

【0033】

接触センサ２６は、Ｘ軸に平行な上記第１の移動方向３０（ｙ＝４０）に進み、基準位置におけるワーク側面Ａに対応するワーク側面Ｃ上の点Ｃ１に到達して止まり、Ｃ１のＸ座標とＹ座標を検出して保持する。同様に、接触センサ２６は、Ｘ軸に平行な上記第２の移動方向３２（ｙ＝３０）に進み、ワーク側面Ｃ上の点Ｃ２に到達して止まり、Ｃ２のＸ座標とＹ座標を検出して保持する。

【0034】

また、接触センサ２６は、Ｙ軸に平行な上記第３の移動方向３４（ｘ＝５０）に進み、基準位置におけるワーク側面Ｂに対応するワーク側面Ｄ上の点Ｄ１に到達して止まり、Ｄ１のＸ座標とＹ座標を検出して保持する。同様に、接触センサ２６は、Ｙ軸に平行な上記第４の移動方向３６（Ｘ＝４０）に進み、ワーク側面Ｄ上の点Ｄ２に到達して止まり、Ｄ２のＸ座標とＹ座標を検出して保持する。

【0035】

Ｃ１とＣ２を通る直線の方程式は、ｙ＝ａ₁ｘ＋ｂ₁で表され、Ｄ１とＤ２を通る直線の方程式は、ｙ＝ａ₂ｘ＋ｂ₂（ａ₁≠ａ₂）で表される。これら２つの直線の交点が、基準点Ｐ１が移動した後の点Ｑ１である。すなわち、基準位置に置かれたワークＷが任意位置に移動した場合、基準点Ｐ１は点Ｑ１に移動する。Ｑ１のＸＹ座標（ｘ１、ｙ１）は、次の式で計算できる。
ｘ１＝（ｂ₂－ｂ₁）／（ａ₁－ａ₂）
ｙ１＝（ａ₁ｂ₂－ａ₂ｂ₁）／（ａ₁－ａ₂）

【0036】

基準点Ｐ１のＸＹ座標を（ｘ０、ｙ０）とした場合、基準点Ｐ１から点Ｑ１（ｘ１、ｙ１）までの移動量をＸ座標移動量ΔｘとＹ座標移動量Δｙで表すと、Δｘ＝ｘ１－ｘ０、Δｙ＝ｙ１－ｙ０である。本実施形態では、ｘ０＝１０、ｙ０＝１０であるから、Δｘ＝ｘ１－１０、Δｙ＝ｙ１－１０となる。

【0037】

また、基準位置に置かれたワーク側面Ｂを表す直線をｙ＝ａ₀ｘ＋ｂ₀とした場合、この直線と任意位置に置かれたワーク側面Ｄを表す直線ｙ＝ａ₂ｘ＋ｂ₂とに基づいて、基準位置のワーク側面Ｂに対する移動後のワーク側面Ｄの傾き角度Δθは、次の式で計算できる。
Δθ＝ｔａｎ^-1（（ａ₂－ａ₀）／（１＋ａ₀ａ₂））
本実施形態では、ａ₀＝０であるから、Δθ＝ｔａｎ^-1ａ₂となる。

【0038】

基準点Ｐ１のＸ座標移動量Δｘ、基準点Ｐ１のＹ座標移動量Δｙ、および上記傾き角度Δθを検出することができれば、基準位置の軌跡データから移動後の軌跡データを計算で求めることができる。

【0039】

図３～図１６を参照しながら、ワークＷが回転を伴わないで基準位置から移動した場合（位置ずれした場合）とワークＷが回転を伴って基準位置から移動した場合（位置ずれした場合）とに分けて、軌跡制御の具体的手法を説明する。

【0040】

［回転を伴わない場合の具体例］
まず、図３～図９を参照しながら、ワークＷが回転を伴わないで基準位置から移動した場合の具体例について説明する。なお、ワークＷの基準位置は、図２に示される場合と同じとする。

【0041】

図３に示されるように、基準位置におけるワークアライメントＷＰ上の４つの点Ａ１、Ａ２、Ｂ１およびＢ２の各ＸＹ座標データと、移動後のワークアライメントＷＱ上の４つの点Ｃ１、Ｃ２、Ｄ１およびＤ２の各ＸＹ座標データが制御テーブルＴ１に保存されている。

【0042】

上記制御テーブルＴ１の各ＸＹ座標データに基づいて、Ａ１とＡ２を通る直線の方程式、Ｂ１とＢ２を通る直線の方程式、Ｃ１とＣ２を通る直線の方程式、および、Ｄ１とＤ２を通る直線の方程式が求められ、図４に示されるように、制御テーブルＴ２に保存される。これら４つの直線は、それぞれワーク側面Ａ、ワーク側面Ｂ、ワーク側面Ｃおよびワーク側面Ｄに対応する。

【0043】

本具体例では、Ａ１とＡ２を通る直線（Ａ直線）、および、Ｃ１とＣ２を通る直線（Ｃ直線）は、いずれもＹ軸に平行であり、Ｂ１とＢ２を通る直線（Ｂ直線）、および、Ｄ１とＤ２を通る直線（Ｄ直線）は、いずれもＸ軸に平行である。ワーク側面Ａ（Ａ面）は、ｘ＝１０．００として保存され、ワーク側面Ｂ（Ｂ面）は、ｙ＝１０．００として保存され、ワーク側面Ｃ（Ｃ面）は、ｘ＝１５．００として保存され、ワーク側面Ｄ（Ｄ面）は、ｙ＝１７．００として保存されている。また、これら４つの直線について、直線の一般式ｙ＝ａｘ+ｂにおける係数ａおよび係数ｂは、いずれもゼロ（０．００）として保存されている。

【0044】

上記制御テーブルＴ２の各直線の方程式に基づいて、ワーク側面Ａとワーク側面Ｂとの交点のＸＹ座標、および、ワーク側面Ｃとワーク側面Ｄとの交点のＸＹ座標が求められ、図５に示されるように、制御テーブルＴ３に保存される。この制御テーブルＴ３に保存されたデータは、ワーク側面Ａとワーク側面Ｂとの交点、すなわち基準点Ｐ１（ｘ０、ｙ０）のＸ座標とＹ座標がいずれも１０．００であることを示している。また、ワーク側面Ｃとワーク側面Ｄとの交点、すなわち基準点Ｐ１の移動後の点Ｑ１（ｘ１、ｙ１）のＸ座標が１５．００、Ｙ座標が１７．００であることを示している。

【0045】

基準点Ｐ１（ｘ０、ｙ０）から点Ｑ１（ｘ１、ｙ１）までのＸ座標移動量ΔｘとＹ座標移動量Δｙは、Δｘ＝ｘ１－ｘ０＝５．００、Δｙ＝ｙ１－ｙ０＝７．００となる。図５に示されるように、Ｘ座標移動量Δｘの値は、ワークＷのＸ座標移動量（ｗｏｒｋ＿ｔｘ）として制御テーブルＴ４に保存され、Ｙ座標移動量Δｙの値は、ワークＷのＹ座標移動量（ｗｏｒｋ＿ｔｙ）として制御テーブルＴ４に保存される。

【0046】

また、基準位置のワーク側面Ｂを表す直線をｙ＝ａ₀ｘ＋ｂ₀とし、移動後のワーク側面Ｄを表す直線をｙ＝ａ₂ｘ＋ｂ₂とした場合、ワーク側面Ｂに対するワーク側面Ｄの傾き角度Δθは、前述のとおり、次の式で計算できる。
Δθ＝ｔａｎ^-1（（ａ₂－ａ₀）／（１＋ａ₀ａ₂））
本具体例では、ａ₀＝０、ａ₂＝０であるから、Δθ＝０となり、図５に示されるように、０．００ラジアンおよび０．００度として、制御テーブルＴ５に保存される。

【0047】

基準点Ｐ１のＸ座標移動量Δｘ、基準点Ｐ１のＹ座標移動量Δｙ、および、基準位置のワーク側面に対する移動後のワーク側面の傾き角度Δθが分かれば、ワークＷが基準位置にあるときの軌跡のＸＹ座標データから、移動後の軌跡のＸＹ座標データを計算で求めることができる。なお、ワークＷが基準位置にあるときの軌跡のＸＹ座標データが軌跡制御部１４に保持されていることは、既に述べたとおりである。

【0048】

具体的には、移動後の軌跡のＸＹ座標（ｘ´、ｙ´）は、基準位置の軌跡のＸＹ座標（ｘ、ｙ）に対して、アフィン変換と呼ばれる座標変換を使って、以下の式で計算することができる。
ｘ´＝（ｐ）（ｘ）Ｃｏｓ（Δθ）－（ｑ）（ｙ）Ｓｉｎ（Δθ）＋ｔｘ
ｙ´＝（ｐ）（ｘ）Ｓｉｎ（Δθ）＋（ｑ）（ｙ）Ｃｏｓ（Δθ）＋ｔｙ

【0049】

ここで、ワークサイズの拡大縮小がないことを想定して、ｐ＝１、ｑ＝１とする。また、ＸＹ座標系の原点（０、０）を中心に回転して座標変換するため、はじめに、アフィン変換式におけるＸ方向移動量ｔｘとＹ方向移動量ｔｙをゼロにして（ｔｘ＝０、ｔｙ＝０）、アフィン変換による基準点のＸ座標移動量ａｆｆ＿ｔｘとＹ座標移動量ａｆｆ＿ｔｙを計算する。

【0050】

本具体例では、Δθ＝０であるから、基準点Ｐ１（１０、１０）のアフィン変換による移動後のＸ座標とＹ座標は、下記の式に、ｘ＝１０、ｙ＝１０、Δθ＝０を代入して、
移動後のＸ座標＝（ｘ）Ｃｏｓ（Δθ）－（ｙ）Ｓｉｎ（Δθ）
移動後のＹ座標＝（ｘ）Ｓｉｎ（Δθ）＋（ｙ）Ｃｏｓ（Δθ）
いずれも１０と計算される。

【0051】

アフィン変換による基準点のＸ座標移動量ａｆｆ＿ｔｘとＹ座標移動量ａｆｆ＿ｔｙは、
ａｆｆ＿ｔｘ＝移動前のＸ座標－移動後のＸ座標＝１０－１０＝０
ａｆｆ＿ｔｙ＝移動前のＹ座標－移動後のＹ座標＝１０－１０＝０
となる（移動前のＸＹ座標は基準点Ｐ１のＸＹ座標）。図６に示されるように、これらの値が０．００として制御テーブルＴ６に保存される。

【0052】

以上のことから、回転角度θ＝０．００（度）
Ｘ方向移動量ｔｘ＝ａｆｆ＿ｔｘ＋ｗｏｒｋ＿ｔｘ＝０．００＋５．００＝５．００
Ｙ方向移動量ｔｙ＝ａｆｆ＿ｔｙ＋ｗｏｒｋ＿ｔｙ＝０．００＋７．００＝７．００
となる。このようにして、移動後の軌跡データを計算するアフィン変換（座標変換）条件が求められ、図７に示されるように、制御テーブルＴ７に保存される。

【0053】

ワークアライメントの座標変換結果が図８に示されている。図８において、点線で示されるのが基準位置におけるワークアライメントＷＰであり、実線で示されるのが移動後のワークアライメントＷＱである。

【0054】

また、軌跡データの座標変換結果が図９に示されている。図９において、点線で示されるのが基準位置における軌跡であり、実線で示されるのが移動後の軌跡である。

【0055】

［回転を伴う場合の具体例］
次に、図１０～図１６を参照しながら、ワークＷが回転を伴って基準位置から移動した場合について説明する。なお、ワークＷの基準位置は、図２に示されるものと同じとする。

【0056】

図１０に示されるように、基準位置におけるワークアライメントＷＰ上の４つの点Ａ１、Ａ２、Ｂ１およびＢ２の各ＸＹ座標データと、移動後のワークアライメントＷＱ上の４つの点Ｃ１、Ｃ２、Ｄ１およびＤ２の各ＸＹ座標データが制御テーブルＴ１に保存されている。

【0057】

上記制御テーブルＴ１の各ＸＹ座標データに基づいて、Ａ１とＡ２を通る直線の方程式、Ｂ１とＢ２を通る直線の方程式、Ｃ１とＣ２を通る直線の方程式、および、Ｄ１とＤ２を通る直線の方程式が求められ、図１１に示されるように、制御テーブルＴ２に保存される。これら４つの直線は、それぞれワーク側面Ａ、ワーク側面Ｂ、ワーク側面Ｃおよびワーク側面Ｄに対応する。

【0058】

本具体例では、Ａ１とＡ２を通る直線（Ａ直線）は、Ｙ軸に平行であり、Ｂ１とＢ２を通る直線（Ｂ直線）は、Ｘ軸に平行である。ワーク側面Ａ（Ａ面）は、ｘ＝１０．００として保存され、ワーク側面Ｂ（Ｂ面）は、ｙ＝１０．００として保存されている。また、これら２つの直線について、直線の一般式ｙ＝ａｘ＋ｂにおける係数ａおよび係数ｂは、いずれもゼロ（０．００）として保存されている。

【0059】

一方、Ｃ１とＣ２を通る直線（Ｃ直線）、および、Ｄ１とＤ２を通る直線（Ｄ直線）は、Ｘ軸にもＹ軸にも平行ではない。ワーク側面Ｃ（Ｃ面）は、ｙ＝－３．３３ｘ＋１４０となり、直線の一般式ｙ＝ａｘ＋ｂにおける係数ａの値が－３．３３、係数ｂの値が１４０．００として保存されている。また、ワーク側面Ｄ（Ｄ面）は、ｙ＝０．３ｘ＋８となり、係数ａの値が０．３０、係数ｂの値が８．００として保存されている。

【0060】

上記制御テーブルＴ２の各直線の方程式に基づいて、ワーク側面Ａとワーク側面Ｂとの交点のＸＹ座標、および、ワーク側面Ｃとワーク側面Ｄとの交点のＸＹ座標が求められ、図１２に示されるように、制御テーブルＴ３に保存される。制御テーブルＴ３に保存されたデータは、ワーク側面Ａとワーク側面Ｂとの交点、すなわち基準点Ｐ１（ｘ０、ｙ０）のＸ座標とＹ座標がいずれも１０．００であることを示している。また、ワーク側面Ｃとワーク側面Ｄとの交点、すなわち基準点Ｐ１の移動後の点Ｑ１（ｘ１、ｙ１）のＸ座標が３６．３３、Ｙ座標が１８．９０であることを示している。

【0061】

基準点Ｐ１（ｘ０、ｙ０）から点Ｑ１（ｘ１、ｙ１）までのＸ座標移動量ΔｘとＹ座標移動量Δｙは、Δｘ＝ｘ１－ｘ０＝２６．３３、Δｙ＝ｙ１－ｙ０＝８．９０となる。図１２に示されるように、Ｘ座標移動量Δｘの値は、ｗｏｒｋ＿ｔｘとして制御テーブルＴ４に保存され、Ｙ座標移動量Δｙの値は、ｗｏｒｋ＿ｔｙとして制御テーブルＴ４に保存される。

【0062】

また、基準位置のワーク側面Ｂを表す直線をｙ＝ａ₀ｘ＋ｂ₀とし、移動後のワーク側面Ｄを表す直線をｙ＝ａ₂ｘ＋ｂ₂とした場合、ワーク側面Ｂに対するワーク側面Ｄの傾き角度Δθは、前述のとおり、次の式で計算できる。
Δθ＝ｔａｎ^-1（（ａ₂－ａ₀）／（１＋ａ₀ａ₂））
本具体例では、ａ０＝０、ａ２＝０．３０であるから、Δθ＝０．２９となり、図１２に示されるように、０．２９ラジアンおよび１６．７０度として制御テーブルＴ５に保存される。

【0063】

前述のとおり、移動後の軌跡のＸＹ座標（ｘ´、ｙ´）は、基準位置の軌跡のＸＹ座標（ｘ、ｙ）に対して、アフィン変換を使って、以下の式で計算することができる。
ｘ´＝（ｐ）（ｘ）Ｃｏｓ（Δθ）－（ｑ）（ｙ）Ｓｉｎ（Δθ）＋ｔｘ
ｙ´＝（ｐ）（ｘ）Ｓｉｎ（Δθ）＋（ｑ）（ｙ）Ｃｏｓ（Δθ）＋ｔｙ

【0064】

ここで、ワークサイズの拡大縮小がないことを想定して、ｐ＝１、ｑ＝１とする。また、ＸＹ座標系の原点（０、０）を中心に回転して座標変換するため、アフィン変換式におけるＸ方向移動量ｔｘとＹ方向移動量ｔｙをゼロにして（ｔｘ＝０、ｔｙ＝０）、アフィン変換による基準点のＸ座標移動量ａｆｆ＿ｔｘとＹ座標移動量ａｆｆ＿ｔｙを計算する。

【0065】

本具体例では、Δθ＝０．２９ラジアンであるから、基準点Ｐ１（１０、１０）のアフィン変換による移動後のＸ座標とＹ座標は、下記の式に、ｘ＝１０、ｙ＝１０、Δθ＝０．２９を代入して、
移動後のＸ座標＝（ｘ）Ｃｏｓ（Δθ）－（ｙ）Ｓｉｎ（Δθ）
移動後のＹ座標＝（ｘ）Ｓｉｎ（Δθ）＋（ｙ）Ｃｏｓ（Δθ）
前者が６．７０、後者が１２．４５と計算される。

【0066】

アフィン変換による基準点のＸ座標移動量ａｆｆ＿ｔｘとＹ座標移動量ａｆｆ＿ｔｙは、
ａｆｆ＿ｔｘ＝移動前のＸ座標－移動後のＸ座標＝１０－６．７０＝３．３０
ａｆｆ＿ｔｙ＝移動前のＹ座標－移動後のＹ座標＝１０－１２．４５＝－２．４５
となる（移動前のＸＹ座標は基準点Ｐ１のＸＹ座標）。図１３に示されるように、これらの値が制御テーブルＴ６に保存される。

【0067】

別の見方をすれば、アフィン変換による基準点の移動量は、ＸＹ座標系における座標点（ｘ、ｙ）をＸＹ座標系の原点を中心としてΔθだけ回転したときと基準点を中心としてΔθだけ回転したときとで生じる回転後のＸＹ座標の差分を調整するものであり、その値は、座標点（ｘ、ｙ）の位置に関わらず、Δθの値および基準点のＸＹ座標の値に応じた一定値をとる。そして、アフィン変換による基準点のＸ座標移動量ａｆｆ＿ｔｘについては、基準点Ｐ１のＸ座標と、基準点Ｐ１をＸＹ座標系の原点を中心としてΔθだけ回転したときのＸ座標との差を計算すればよい。同様に、アフィン変換による基準点のＹ座標移動量ａｆｆ＿ｔｙについては、基準点Ｐ１のＹ座標と、基準点Ｐ１をＸＹ座標系の原点を中心としてΔθだけ回転したときのＹ座標との差を計算すればよい。

【0068】

以上のことから、回転角度θ＝１６．７０（度）
Ｘ方向移動量ｔｘ＝ａｆｆ＿ｔｘ＋ｗｏｒｋ＿ｔｘ＝３．３０＋２６．３３＝２９．６３
Ｙ方向移動量ｔｙ＝ａｆｆ＿ｔｙ＋ｗｏｒｋ＿ｔｙ＝－２．４５＋８．９０＝６．４５
となる。このようにして移動後の軌跡データを計算するアフィン変換（座標変換）条件が求められ、図１４に示されるように、制御テーブルＴ７に保存される。

【0069】

ワークアライメントの座標変換結果が図１５に示されている。図１５において、点線で示されるのが基準位置におけるワークアライメントＷＰであり、実線で示されるのが移動後のワークアライメントＷＱである。

【0070】

また、軌跡データの座標変換結果が図１６に示されている。図１６において、点線で示されるのが基準位置における軌跡であり、実線で示されるのが移動後の軌跡である。

【0071】

本実施形態の軌跡制御装置１０によれば、高精度カメラや画像センサを使うことなく、接触センサ２６を用いた簡単な検出制御によって、基準点Ｐ１のＸ座標移動量Δｘ、基準点Ｐ１のＹ座標移動量Δｙ、および、基準位置のワーク側面に対する移動後の該ワーク側面の傾き角度Δθを求めることができ、これに基づいてリアルタイムに軌跡補正ができる。

【0072】

より具体的に言えば、接触センサ２６によるワーク側面Ｃ上の２つの接触点のＸＹ座標とワーク側面Ｄ上の２つの接触点のＸＹ座標が分かれば、上記Ｘ座標移動量Δｘと上記Ｙ座標移動量Δｙと上記傾き角度Δθとを求めることができ、アフィン変換を使って、任意位置に置かれたワーク上の軌跡のＸＹ座標を容易に計算することができる。

【0073】

また、接触センサ２６と軌跡追従部材２８がいずれもＺ軸アクチュエータ２４ｃに取り付けられているので、接触センサ２６を駆動する機構と軌跡追従部材２８を駆動する機構の共通化を図ることができる。

【0074】

さらに、接触センサ２６がワーク側面Ｃまたはワーク側面Ｄに接触すると、Ｘ軸アクチュエータ２４ａおよびＹ軸アクチュエータ２４ｂの駆動を停止するので、任意位置に置かれたワークＷが検出制御時に移動するのを阻止することができる。

【0075】

本発明に係る軌跡制御装置は、上述の実施形態に限らず、本発明の要旨を逸脱することのない範囲で、種々の構成を採り得ることはもちろんである。

【符号の説明】

【0076】

１０…軌跡制御装置 １２…軌跡コントローラ
２４ａ…Ｘ軸アクチュエータ ２４ｂ…Ｙ軸アクチュエータ
２４ｃ…Ｚ軸アクチュエータ ２６…接触センサ
２８…軌跡追従部材 Ｗ…ワーク

【図1】

【図2】

【図3】

【図4】

【図5】

【図6】

【図7】

【図8】

【図9】

【図10】

【図11】

【図12】

【図13】

【図14】

【図15】

【図16】