特許 7469523 変位検出センサ、制御装置、及び制御システム

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

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

(11)【特許番号】

(24)【登録日】2024-04-08

(45)【発行日】2024-04-16

(54)【発明の名称】変位検出センサ、制御装置、及び制御システム

(51)【国際特許分類】

   B25J 19/02 20060101AFI20240409BHJP

【ＦＩ】

B25J19/02

【請求項の数】 10

(21)【出願番号】P 2022578334

(86)(22)【出願日】2022-01-21

(86)【国際出願番号】 JP2022002249

(87)【国際公開番号】W WO2022163536

(87)【国際公開日】2022-08-04

【審査請求日】2023-08-09

(31)【優先権主張番号】P 2021011910

(32)【優先日】2021-01-28

(33)【優先権主張国・地域又は機関】JP

(73)【特許権者】

【識別番号】390008235

【氏名又は名称】ファナック株式会社

(74)【代理人】

【識別番号】100099759

【弁理士】

【氏名又は名称】青木 篤

(74)【代理人】

【識別番号】100123582

【弁理士】

【氏名又は名称】三橋 真二

(74)【代理人】

【識別番号】100112357

【弁理士】

【氏名又は名称】廣瀬 繁樹

(72)【発明者】

【氏名】村上 邦彦

【審査官】仁木 学

(56)【参考文献】

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

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

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

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

Ｂ２５Ｊ １／００ ー ２１／０２

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

【請求項1】

所定の軸線に関して相対運動可能に連結された第一リンクと第二リンクに取付けた二つのエンコーダを備え、
前記二つのエンコーダは、前記軸線から検出位置までの距離が異なる二つのスケール部材と、二つのスケール部材の目盛りをそれぞれ検出する二つの検出器と、を備え、
前記第二リンクの先端で受けた外力に応じて前記スケール部材と前記検出器の位置関係が変化して前記二つのエンコーダで検出される二つの目盛りに差が生じ、前記二つの目盛りに基づいて前記第二リンクの先端の変位を検出可能にした、変位検出センサ。

【請求項2】

前記検出器と前記スケール部材の一方を支持するベース部材をさらに備え、前記ベース部材は、前記第二リンクの先端に固定された固定端と、前記第一リンクと第二リンクの連結部に配置されていて拘束されない自由端と、を備え、前記自由端に前記検出器と前記スケール部材の前記一方が取付けられ、前記第一リンクに前記検出器と前記スケール部材の他方が固定された、請求項１に記載の変位検出センサ。

【請求項3】

前記二つの目盛りに加え、前記軸線から前記二つのスケール部材の検出位置までの異なる二つの距離と、前記第二リンクの先端から前記スケール部材の前記検出位置までの距離と、に基づき、前記第二リンクの先端の前記変位を算出する変位算出部をさらに備える、請求項２に記載の変位検出センサ。

【請求項4】

前記変位算出部は前記第二リンクの先端の前記変位に応じた補正量をさらに算出する、請求項３に記載の変位検出センサ。

【請求項5】

前記二つのエンコーダは前記軸線回りに回転可能に連結された前記第一リンクと前記第二リンクの相対角度を検出するロータリエンコーダである、請求項１から４のいずれか一項に記載の変位検出センサ。

【請求項6】

前記二つのエンコーダは前記軸線に沿って直進可能に連結された前記第一リンクと前記第二リンクの相対位置を検出するリニアエンコーダである、請求項１から４のいずれか一項に記載の変位検出センサ。

【請求項7】

請求項１から６のいずれか一項に記載の変位検出センサを取付けた複数のリンクを備える機械を制御する制御装置であって、前記複数のリンクのそれぞれの先端の前記変位に応じた補正量に基づいて前記機械の動作指令を生成する動作指令生成部を備える、制御装置。

【請求項8】

前記制御装置は、前記二つの目盛りに加え、前記軸線から前記二つのスケール部材の検出位置までの異なる二つの距離と、前記第二リンクの先端から前記スケール部材の前記検出位置までの距離と、に基づき、前記複数のリンクのそれぞれの先端の前記変位を算出する変位算出部をさらに備える、請求項７に記載の制御装置。

【請求項9】

前記変位算出部は前記複数のリンクのそれぞれの先端の前記変位に応じた前記補正量を前記リンク毎にさらに算出する、請求項８に記載の制御装置。

【請求項10】

請求項１から６のいずれか一項に記載の変位検出センサと、
前記変位検出センサを取付けた複数のリンクを備える機械と、
前記機械を制御する制御装置と、
を備え、
前記制御装置は前記複数のリンクのそれぞれの先端の前記変位に応じた補正量に基づいて前記機械の動作指令を生成する動作指令生成部を備える、制御システム。

【発明の詳細な説明】

【技術分野】

【0001】

本発明は、機械制御に関し、特に機械先端の変位を検出可能な変位検出センサ、制御装置、及び制御システムに関する。

【背景技術】

【0002】

産業用ロボット、工作機械等の機械は相対運動可能に連結された複数のリンクを備えている。機械先端に加工ツールやワークを持たせて加工を行うアプリケーションでは、機械先端で外力を受けるため、外力によって生じるリンクの撓み、リンク連結部の捻じれ等の影響で機械先端の位置が変化する。機械先端の変位が加工精度に直接影響するため、外力による変位の小さい機械が求められる。これに対し、連結部の出力軸に装着したエンコーダにより出力軸の位置を制御することで連結部の捻じれ、バックラッシ等の影響を無くす技術が知られている。しかしながら、出力軸に装着したエンコーダで出力軸の位置を制御する場合、連結部より先のリンクの撓みによる機械先端の変位には対応できない。本願に関連する技術としては例えば後述の文献が公知である。

【0003】

特許文献１には、ロボットアームにおいて、主たる負荷を受け持つ主アームと、主アームの根元側に固定された固定端と主アームの先端側に延在する自由端とを備えたサブアームと、自由端側に取付けられていて負荷に起因する主アームの撓みを検出する変位検出器と、を備え、変位検出器を用いて撓みを補正してアーム先端の位置決めを行うロボットアームが開示されている。

【0004】

特許文献２には、多関節形ロボットにおいて、回動腕を駆動する電動機の回転角度検出器とは別に、各関節に回動腕同士のなす実際の角度を検出する角度検出器を、直結ないし増速歯車を介して付設した多角形ロボットが記載されている。

【0005】

特許文献３には、二つのエンコーダを備えたエンコーダ装置において、第一軸に取付けた第一スケールと第二軸に取付けた第二スケールとを隣接配置し、第二スケールに第一スケールを検出するための光透過部を備えたエンコーダ装置が記載されている。

【先行技術文献】

【特許文献】

【0006】

【文献】特開昭６１－１２５７８６号公報

【文献】特開昭６４－４５５９２号公報

【文献】特開２０１６－３９４７号公報

【発明の概要】

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

【0007】

本発明は、従来の問題点に鑑み、機械先端の変位を精度良く検出する技術を提供することを目的とする。

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

【0008】

本開示の一態様は、所定の軸線に関して相対運動可能に連結された第一リンクと第二リンクに取付けた二つのエンコーダを備え、二つのエンコーダは軸線からの距離が異なる二つのスケール部材と二つのスケール部材の目盛りをそれぞれ検出する二つの検出器とを備え、第二リンクの先端で受けた外力に応じてスケール部材と検出器の位置関係が変化して二つのエンコーダでそれぞれ検出される二つの目盛りに差が生じ、二つの目盛りに基づいて第二リンクの先端の変位を検出可能にした、変位検出センサを提供する。
本開示の他の態様は、前述の変位検出センサを取付けた複数のリンクを備える機械を制御する制御装置であって、複数のリンクのそれぞれの先端の変位に応じた補正量に基づいて機械の動作指令を生成する動作指令生成部を備える、制御装置を提供する。
本開示の別の態様は、前述の変位検出センサと、変位検出センサを取付けた複数のリンクを備える機械と、機械を制御する制御装置と、を備え、制御装置は複数のリンクのそれぞれの先端の変位に応じた補正量に基づいて機械の動作指令を生成する動作指令生成部を備える、制御システムを提供する。

【発明の効果】

【0009】

本開示の態様によれば、機械先端の変位を精度良く検出できる。

【図面の簡単な説明】

【0010】

【図1】第一実施形態の変位検出センサを示す機械の上面図である。

【図2】第一実施形態の変位検出センサを示す機械の斜視図である。

【図3】第一実施形態の変位検出センサを示す機械の側面図である。

【図4】図３に示す変位検出センサの一部拡大図である。

【図5】リンク先端の変位の算出例を示す幾何学図である。

【図6】第一実施形態の制御システムのブロック図である。

【図7】第二実施形態の変位検出センサを示す機械の側面図である。

【図8】リンク先端の変位の算出例を示す幾何学図である。

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

【0011】

以下、添付図面を参照して本開示の実施形態を詳細に説明する。各図面において、同一又は類似の構成要素には同一又は類似の符号が付与されている。また、以下に記載する実施形態は請求の範囲に記載される発明の技術的範囲及び用語の意義を限定するものではない。

【0012】

図１及び図２は第一実施形態の変位検出センサ２０を示す機械１０の上面図及び斜視図である。機械１０は例えば産業用ロボット、工作機械等の機械である。機械１０は軸線Ｏに関して相対運動可能に連結された第一リンク１１と第二リンク１２を備えている。変位検出センサ２０は第一リンク１１と第二リンク１２に取付けられる。第一リンク１１と第二リンク１２は例えば軸線Ｏ回りに回転可能に連結されている。第二リンク１２はアクチュエータによって駆動される。アクチュエータは例えばモータ１３と減速機１４である。アクチュエータは減速機を備えないダイレクトモータで構成されてもよい。

【0013】

変位検出センサ２０は第一リンク１１と第二リンク１２に取付けた二つのエンコーダ２１、２２を備えている。二つのエンコーダ２１、２２は例えば第一リンク１１と第二リンク１２の相対角度を検出するロータリエンコーダである。二つのエンコーダ２１、２２は例えば光学式エンコーダであるが、磁気式エンコーダ等の他の方式のエンコーダでもよい。二つのエンコーダ２１、２２は軸線Ｏから検出位置までの距離が異なる二つのスケール部材２１ａ、２２ａと二つのスケール部材２１ａ、２２ａの目盛りを検出する検出器２１ｂ、２２ｂとを備えている。二つのスケール部材２１ａ、２２ａは、例えば径の異なる二つのリング状部材であり、軸線Ｏを中心とした同心円上に配置される。一方、二つの検出器２１ｂ、２２ｂは二つのスケール部材２１ａ、２２ａにそれぞれ対向する位置に配置される。

【0014】

変位検出センサ２０は二つの検出器２１ｂ、２２ｂを支持するベース部材２３をさらに備えているとよい。ベース部材２３は、例えば棒状部材であり、第二リンク１２の先端に固定された固定端２３ａと、第一リンク１１と第二リンク１２の連結部に配置されていて拘束されない自由端２３ｂと、を備えている。ベース部材２３の自由端２３ｂに検出器２１ｂ、２２ｂが取付けられ、第一リンク１１にスケール部材２１ａ、２２ａが固定される。これにより、第二リンク１２の先端で受けた外力に応じてスケール部材２１ａ、２２ａと検出器２１ｂ、２２ｂの位置関係が変化して二つのエンコーダ２１、２２で検出される二つの目盛りに差が生じる。

【0015】

図３は第一実施形態の変位検出センサ２０を示す機械１０の側面図である。この図では、外力を受ける前の第二リンク１２が二点鎖線で描かれ、外力を受けて撓んだ第二リンク１２が実線で描かれている。矢印で示すように第二リンク１２の先端に外力を受けると、第二リンク１２の撓み、リンク連結部の捻じれ等の影響で第二リンク１２の先端（ベース部材２３の固定端２３ａ）が位置Ａから位置Ｂへ移動する。つまり第二リンク１２の先端がδだけ変位する。一方、ベース部材２３は固定端２３ａが第二リンク１２の先端に固定されているものの自由端２３ｂが拘束されてないため、ベース部材２３自体は変形しない（つまり撓まない）。従って、第二リンク１２の先端の変位δ分だけ検出器２１ｂ、２２ｂとスケール部材２１ａ、２２ａの位置関係が変化して二つのエンコーダ２１、２２で検出される二つの目盛りに差が生じる。

【0016】

図４は図３に示す変位検出センサ２０の一部拡大図である。第二リンク１２の先端の変位δに応じて検出器２１ｂ、２２ｂとスケール部材２１ａ、２２ａの位置関係が変化して二つのエンコーダ２１、２２で検出される二つの目盛り、即ち二つの角度θ１、θ２に差が生じている。二つの角度θ１、θ２は外力作用前後の角度変化である。変位検出センサ２０は二つの角度θ１、θ２に基づいて第二リンク１２の先端の変位δを検出可能にしている。

【0017】

変位検出センサ２０は、二つの角度θ１、θ２に加え、軸線Ｏから二つのスケール部材２１ａ、２２ａの検出位置Ｃ、Ｄ（又はＣ’、Ｄ’）までの異なる二つの距離ｒ１、ｒ２と（図４を参照）、第二リンク１２の先端（ベース部材２３の固定端２３ａ）からスケール部材２２ａの検出位置Ｃまでの距離Ｌと（図３を参照）、に基づき、第二リンク１２の先端の変位δを算出する変位算出部（図示せず）をさらに備えるとよい。ｒ１、ｒ２はそれぞれスケール部材２１ａ、２２ａの半径であり、Ｌは外力作用前のベース部材２３の固定端２３ａからスケール部材２２ａの検出位置Ｃまでの距離であるため、ｒ１、ｒ２、Ｌは既知の値である。図示しないが、変位算出部は例えばプログラムを実行するプロセッサ、メモリ、入出力部等を備えたコンピュータ装置であるが、プログラムを実行しないＦＰＧＡ（field-programmable gate array）、ＡＳＩＣ（application specific integrated circuit）等の半導体集積回路で構成されてもよい。

【0018】

図５はリンク先端の変位δの算出例を示す幾何学図である。先ず、変位検出センサ２０は既知の値であるｒ１、ｒ２、Ｌと外力作用前の二つのエンコーダ２１、２２の目盛りとをメモリに記憶しておく。そして、変位検出センサ２０は外力作用後の二つのエンコーダ２１、２２の目盛りを取得して二つの角度θ１、θ２を取得する。

【0019】

図５において、外力作用前の第二リンクの先端の位置Ａ（ベース部材の固定端の位置）とスケール部材の検出位置Ｃを結ぶ線分ＡＣに平行であり且つ外力作用後のスケール部材の検出位置Ｃ’を通る線分ＥＦを引き、さらに外力作用後のベース部材の固定端の位置Ｂから線分ＥＦへ垂線ＢＥを引き、線分ＡＣと垂線ＢＥの交点をＧとし、線分ＥＧの長さをＬ２とし、垂線ＢＥの長さをＬ３とすると、第二リンクの先端の変位δは以下の式で表すことができる。

【0020】

【数1】

【0021】

また、外力作用後のベース部材の角度∠ＢＣ’Ｅをβとし、ベース部材の固定端の位置Ｂからスケール部材の検出位置Ｃ’までの辺ＢＣ’の長さをＬ’とすると、Ｌ３は以下の式で表すことができる。

【0022】

【数2】

【0023】

Ｌ’に関し、外力作用後はスケール部材と検出器との間の距離が微小変化するため、この変化量をΔＬとすると、辺ＢＣ’の長さＬ’はＬ’＝Ｌ＋ΔＬと表される。ここでΔＬはＬに対して十分に小さいため、Ｌ’＝Ｌと近似できる。よって、Ｌ３はさらに以下の式で表すことができる。

【0024】

【数3】

【0025】

一方、Ｌ２は、スケール部材の検出位置Ｃ’から線分ＡＯに下した垂線である辺Ｃ’Ｈの長さに等しいため、角度θ２とスケール部材の半径ｒ２から以下の式で表すことができる。

【0026】

【数4】

【0027】

式３、式４から既知の値でないβを求めれば、第二リンクの先端の変位δを求められることになる。ここで∠Ｄ’Ｃ’Ｏをαとすると、βは以下の式で表すことができる。

【0028】

【数5】

【0029】

式５から既知の値でないαを求めれば、第二リンクの先端の変位δを求められることになる。ここで△ＯＣ’Ｄ’に注目し、辺Ｃ’Ｄ’の長さをＬ１とすると、余弦定理からαは以下の式で表すことができる。

【0030】

【数6】

【0031】

式６から既知の値でないＬ１を求めれば、第二リンクの先端の変位δを求められることになる。ここで△ＯＣ’Ｄ’にさらに注目し、辺ＯＤ’の長さはｒ１であり、辺ＯＣ’の長さはｒ２であり、これら二辺の挟む角∠Ｃ’ＯＤ’は二つの角度θ１、θ２の差であるため、余弦定理からＬ１は以下の式で表すことができる。

【0032】

【数7】

【0033】

ここでｒ１、ｒ２、θ１、θ２は既知であるため、第二リンクの先端の変位δを求められることになる。

【0034】

また第二リンクの先端の変位δを補正する場合、位置Ｂから位置Ａまで軸線Ｏ回りに第二リンクを回転させる必要がある。この回転角度を補正量εとすると、変位算出部は第二リンクの先端の変位δを補正する補正量εをさらに算出してもよい。補正量εは以下の式で表すことができる。

【0035】

【数8】

【0036】

ここで辺ＧＯは辺ＥＣ’と辺Ｃ’Ｆの和である。△ＢＣ’Ｅに注目すると、辺ＥＣ’は以下の式で表すことができる。

【0037】

【数9】

【0038】

また△Ｃ’ＯＦに注目すると、辺Ｃ‘Ｆは以下の式で表すことができる。

【0039】

【数10】

【0040】

式９、式１０から補正量εを算出できることになる。

【0041】

図６は第一実施形態の制御システム１のブロック図である。制御システム１は、変位検出センサ２０と、変位検出センサ２０を取付けた複数のリンクを備える機械１０と、機械１０を制御する制御装置３０と、を備えている。機械１０は例えば垂直多関節ロボットである。機械１０は複数のリンクのそれぞれの連結部にモータ１３と変位検出センサ２０を備えている。変位検出センサ２０は例えばロータリエンコーダである。変位検出センサ２０は複数のリンクのそれぞれの先端の変位δを検出可能にする。複数のリンクのそれぞれの先端の変位δを補正すれば機械１０の先端の変位を補正できることになる。

【0042】

制御装置３０は教示プログラムに従って機械１０の動作指令を生成する動作指令生成部３３を備えている。機械１０の動作指令は例えばモータ１３の位置指令、速度指令、トルク指令等を含む。動作指令生成部３３は複数のリンクのそれぞれの先端の変位δに応じた補正量εに基づいて機械１０の動作指令を生成する。動作指令生成部３３は例えばモータ１３を制御するコントローラ、アンプ等で構成される。

【0043】

制御装置３０は、θ１、θ２、ｒ１、ｒ２、及びＬを記憶するメモリ３１と、θ１、θ２、ｒ１、ｒ２、及びＬに基づいて複数のリンクのそれぞれの先端の変位δを算出する変位算出部３２と、をさらに備えていてもよい。変位算出部３２は例えばプログラムを実行するプロセッサ、メモリ、入出力部等を備えたコンピュータ装置であるが、プログラムを実行しないＦＰＧＡ（field-programmable gate array）、ＡＳＩＣ（application specific integrated circuit）等の半導体集積回路で構成されてもよい。制御装置３０に変位算出部３２を設ける場合、変位検出センサ２０に変位算出部を設ける必要はない。この場合、変位検出センサ２０は機械１０の先端で受けた外力に応じて二つのエンコーダで検出される二つの角度θ１、θ２を制御装置３０に出力すればよい。

【0044】

変位算出部３２は複数のリンクのそれぞれの先端の変位δを補正する補正量εをリンク毎にさらに算出するとよい。動作指令生成部３３はこれら補正量εに基づいて機械１０の動作指令を生成する。

【0045】

第一実施形態の変位検出センサ２０、制御装置３０、及び制御システム１の構成及び動作は一例であり、適宜変更できることに留意されたい。例えばベース部材２３は、二つの検出器２１ｂ、２２ｂを支持するのではなく、二つのスケール部材２１ａ、２２ａを支持してもよい。つまりベース部材２３の自由端２３ｂにスケール部材２１ａ、２２ａを取付け、第一リンク１１に検出器２１ｂ、２２ｂを固定してもよい。また、例えば変位検出センサ２０がベース部材２３を備えるのではなく、機械１０がベース部材２３を備えていてもよい。

【0046】

また第一リンク１１と第二リンク１２は、軸線Ｏ回りに回転可能に連結されるのではなく、軸線に沿って直進可能に連結されてもよい。つまり二つのエンコーダ２１、２２は、ロータリエンコーダではなく、リニアエンコーダでもよい。

【0047】

図７は第二実施形態の変位検出センサ２０を示す機械１０の側面図である。以下では、第一実施形態の変位検出センサ２０とは異なる構成及び動作についてのみ説明する。第一リンク１１と第二リンク１２は例えば軸線Ｏに沿って直進可能に連結されている。エンコーダ２１、２２は例えば第一リンク１１と第二リンク１２の相対位置を検出するリニアエンコーダである。二つのスケール部材２１ａ、２２ａは、例えば軸線Ｏから検出位置までの距離が異なる二つの線状部材であり、軸線Ｏの平行線上に配置される。一方、二つの検出器２１ｂ、２２ｂは二つのスケール部材２１ａ、２２ａにそれぞれ対向する位置に配置される。ベース部材２３の自由端２３ｂに検出器２１ｂ、２２ｂが取付けられ、第一リンク１１にスケール部材２１ａ、２２ａが固定される。これにより、第二リンク１２の先端で受けた外力に応じてスケール部材２１ａ、２２ａと検出器２１ｂ、２２ｂの位置関係が変化して二つのエンコーダ２１、２２で検出される二つの目盛りに差が生じる。

【0048】

第二リンク１２の先端の変位δに応じて検出器２１ｂ、２２ｂとスケール部材２１ａ、２２ａの位置関係が変化して二つのエンコーダ２１、２２で検出される二つの目盛り、即ち二つの位置Ｐ１、Ｐ２に差が生じている。二つの位置Ｐ１、Ｐ２は外力作用前後の位置変化である。変位検出センサ２０は二つの位置Ｐ１、Ｐ２に基づいて第二リンク１２の先端の変位δを検出可能にしている。

【0049】

変位検出センサ２０は、二つの位置Ｐ１、Ｐ２に加え、軸線Ｏから二つのスケール部材２１ａ、２２ａの検出位置Ｃ、Ｄまでの異なる二つの距離ｒ１、ｒ２と、第二リンク１２の先端（ベース部材２３の固定端２３ａ）からスケール部材２２ａの検出位置Ｃまでの距離Ｌと、に基づき、第二リンク１２の先端の変位δを算出する変位算出部（図示せず）をさらに備えるとよい。ｒ１、ｒ２はそれぞれ軸線Ｏからスケール部材２１ａ、２２ａの検出位置Ｃ、Ｄまでの距離であり、Ｌは外力作用前のベース部材２３の固定端２３ａからスケール部材２２ａの検出位置Ｃまでの距離であるため、ｒ１、ｒ２、Ｌは既知の値である。

【0050】

図８はリンク先端の変位δの算出例を示す幾何学図である。先ず、変位検出センサ２０は既知の値であるｒ１、ｒ２、Ｌと外力作用前の二つのエンコーダ２１、２２の目盛りとをメモリに記憶しておく。そして、変位検出センサ２０は外力作用後の二つのエンコーダ２１、２２の目盛りを取得して二つの位置Ｐ１、Ｐ２を取得する。

【0051】

図８において、外力作用前の第二リンクの先端の位置Ａ（ベース部材の固定端の位置）とスケール部材の検出位置Ｃを結ぶ線分ＡＣに平行であり且つ外力作用後のスケール部材の検出位置Ｃ’を通る線分ＥＦを引き、さらに外力作用後のベース部材の固定端の位置Ｂから線分ＥＦへ垂線ＢＥを引き、線分ＡＣと垂線ＢＥの交点をＧとし、線分ＥＧの長さをＬ２とし、垂線ＢＥの長さをＬ３とすると、第二リンクの先端の変位δは以下の式で表すことができる。

【0052】

【数11】

【0053】

また、外力作用後のベース部材の角度∠ＢＣ’Ｅをβとし、ベース部材の固定端の位置Ｂからスケール部材の検出位置Ｃ’までの辺ＢＣ’の長さをＬ’とすると、Ｌ３は以下の式で表すことができる。

【0054】

【数12】

【0055】

Ｌ’に関し、外力作用後はスケール部材と検出器との間の距離が微小変化するため、この変化量をΔＬとすると、辺ＢＣ’の長さＬ’はＬ’＝Ｌ＋ΔＬと表される。ここでΔＬはＬに対して十分に小さいため、Ｌ’＝Ｌと近似できる。よって、Ｌ３はさらに以下の式で表すことができる。

【0056】

【数13】

【0057】

一方、Ｌ２は、スケール部材の検出位置Ｃから検出位置Ｃ’までの長さに等しいため、以下の式で表すことができる。

【0058】

【数14】

【0059】

式１３、式１４から既知の値でないβを求めれば、第二リンクの先端の変位δを求められることになる。ここで△Ｃ’ＦＤ’に注目すると、βは以下の式で表すことができる。

【数15】

【0060】

ここでｒ１、ｒ２、Ｐ１、Ｐ２は既知であるため、第二リンクの先端の変位δを求められることになる。

【0061】

また第二リンクの先端の変位δを補正する場合、位置Ｂから位置Ａまで軸線Ｏに沿って第二リンクを直進させる必要がある。この直進位置を補正量εとすると、変位算出部は第二リンクの先端の変位δを補正する補正量εをさらに算出してもよい。補正量εは、変位δに等しいため、以下の式で表すことができる。

【0062】

【数16】

【0063】

以上の実施形態によれば、機械１０の先端の変位を精度良く検出できる。また種々のリンク連結構造に応じて機械１０の先端の変位を精度良く補正できる。

【0064】

なお、前述のプロセッサで実行されるプログラムは、コンピュータ読取り可能な非一時的記録媒体、例えばＣＤ－ＲＯＭ等に記録して提供してもよいし、或いは有線又は無線を介してＷＡＮ（wide area network）又はＬＡＮ（local area network）上のサーバ装置から配信して提供してもよいことに留意されたい。

【0065】

本明細書において種々の実施形態について説明したが、本発明は、前述の実施形態に限定されるものではなく、請求の範囲に記載された範囲内において種々の変更を行えることを認識されたい。

【符号の説明】

【0066】

１ 制御システム
１０ 機械
１１ 第一リンク
１２ 第二リンク
１３ モータ
１４ 減速機
２０ 変位検出センサ
２１、２２ エンコーダ
２１ａ、２２ａ スケール部材
２１ｂ、２２ｂ 検出器
２３ ベース部材
２３ａ 固定端
２３ｂ 自由端
３０ 制御装置
３１ メモリ
３２ 変位算出部
３３ 動作指令生成部
Ｏ 軸線
δ 変位
ε 補正量
Ｌ ベース部材の固定端からスケール部材の検出位置までの距離
ｒ１、ｒ２ 軸線からスケール部材の検出位置までの距離
θ１、θ２ 二つのエンコーダによって検出された二つの角度
Ｐ１、Ｐ２ 二つのエンコーダによって検出された二つの位置

【図1】

【図2】

【図3】

【図4】

【図5】

【図6】

【図7】

【図8】