特開 2024-17379 制御装置、アクチュエータ、及び衝突検出方法

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

(12)【公報種別】公開特許公報(A)

(11)【公開番号】P2024017379

(43)【公開日】2024-02-08

(54)【発明の名称】制御装置、アクチュエータ、及び衝突検出方法

(51)【国際特許分類】

   H02P 29/024 20160101AFI20240201BHJP

【ＦＩ】

H02P29/024

【審査請求】未請求

【請求項の数】9

【出願形態】ＯＬ

(21)【出願番号】P 2022119972

(22)【出願日】2022-07-27

(71)【出願人】

【識別番号】391008515

【氏名又は名称】株式会社アイエイアイ

(74)【代理人】

【識別番号】110001519

【氏名又は名称】弁理士法人太陽国際特許事務所

(72)【発明者】

【氏名】榊原 則夫

(72)【発明者】

【氏名】鈴木 新悟

(72)【発明者】

【氏名】飯田 峻也

(72)【発明者】

【氏名】服部 智壮

【テーマコード（参考）】

5H501

【Ｆターム（参考）】

5H501BB08

5H501DD04

5H501DD07

5H501GG01

5H501GG03

5H501GG05

5H501HB08

5H501JJ03

5H501JJ12

5H501JJ18

5H501JJ22

5H501JJ24

5H501JJ26

5H501LL07

5H501LL22

5H501LL35

5H501LL51

5H501MM04

5H501MM09

(57)【要約】

【課題】移動体の物体への衝突を精度良く検出する。
【解決手段】制御装置１０は、移動体２７に関する位置指令値に基づいて、移動体２７の位置を推定し、モータ２２の電流値が第１閾値を超え、かつ、移動体２７の推定位置と移動体２７の現在位置との差が第２閾値を超える場合に、移動体２７が物体に衝突したと判定する。
【選択図】図１

【特許請求の範囲】

【請求項1】

移動体を移動させるモータを制御する制御装置であって、
前記移動体に関する位置指令値に基づいて、前記移動体の位置を推定する推定部と、
前記モータの電流値が第１閾値を超え、かつ、前記推定部により推定された推定位置と前記移動体の現在位置との差が第２閾値を超える場合に、前記移動体が物体に衝突したと判定する衝突判定部と、
を備えた制御装置。

【請求項2】

前記推定部は、位置ゲインの逆数を時定数とするローパスフィルタ処理の実行により前記推定位置を算出する、
請求項１に記載の制御装置。

【請求項3】

前記電流値は、前記モータの電流検出値である、又は、前記位置指令値に基づいて算出される電流指令値である、
請求項１に記載の制御装置。

【請求項4】

前記衝突判定部は、前記電流値が前記第１閾値を超え、かつ、前記推定位置と前記現在位置との差が前記第２閾値を超える状態が所定時間継続した場合に、前記移動体が前記物体に衝突したと判定する、
請求項１に記載の制御装置。

【請求項5】

前記衝突判定部は、前記電流値が前記第１閾値を超え、かつ、前記推定位置と前記現在位置との差が前記第２閾値以下で、かつ、前記推定位置が前記移動体の到達目標とする位置である目標位置に到達したときに前記現在位置が前記目標位置に対して所定の範囲外である場合に、前記移動体が前記物体に衝突したと判定する、
請求項１に記載の制御装置。

【請求項6】

前記衝突判定部は、前記電流値が前記第１閾値を超え、かつ、前記推定位置と前記現在位置との差が前記第２閾値以下で、かつ、前記推定位置が前記目標位置に到達したときに前記現在位置が前記目標位置に対して所定の範囲外である状態が所定時間継続した場合に、前記移動体が前記物体に衝突したと判定する、
請求項５に記載の制御装置。

【請求項7】

移動体を移動させるモータを制御する制御装置であって、
前記移動体に関する位置指令値に基づいて、前記移動体の位置を推定する推定部と、
前記モータの電流値が第１閾値を超え、かつ、前記推定部により推定された推定位置が前記移動体の到達目標とする位置である目標位置に到達したときに前記移動体の現在位置が前記目標位置に対して所定の範囲外である場合に、前記移動体が物体に衝突したと判定する衝突判定部と、
を備えた制御装置。

【請求項8】

移動体と、
前記移動体を移動させるモータと、
前記モータを制御する、請求項１～請求項７の何れか１項に記載の制御装置と、
を含むアクチュエータ。

【請求項9】

移動体を移動させるモータを制御する制御装置が実行する衝突検出方法であって、
前記移動体に関する位置指令値に基づいて、前記移動体の位置を推定し、
前記モータの電流値が第１閾値を超え、かつ、前記推定された推定位置と前記移動体の現在位置との差が第２閾値を超える場合に、前記移動体が物体に衝突したと判定する、
衝突検出方法。

【発明の詳細な説明】

【技術分野】

【0001】

本発明は、制御装置、アクチュエータ、及び衝突検出方法に関する。

【背景技術】

【0002】

例えば、特許文献１には、モータの回転運動をスライダ等の移動体の直線運動等に変換させるアクチュエータの制御装置が記載されている。このアクチュエータの制御装置は、移動体の移動速度が一定である期間においてモータに流れる電流値が所定の閾値を超える場合に、移動体が障害物等の物体に衝突したことを検出する。

【先行技術文献】

【特許文献】

【0003】

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

【発明の概要】

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

【0004】

しかしながら、上記のようなモータの電流値と閾値との比較による衝突検出技術では、移動体の加減速中は衝突を検出することができない。より詳細には、移動体の加減速中は電流値が比較的大きくなるため同じ閾値を使うと誤検出してしまう。

【0005】

本発明は、以上の事実を考慮して成されたもので、移動体の物体への衝突を精度良く検出することができる制御装置、アクチュエータ、及び衝突検出方法を提供することを目的とする。

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

【0006】

上記目的を達成するために、第１態様に係る制御装置は、移動体を移動させるモータを制御する制御装置であって、前記移動体に関する位置指令値に基づいて、前記移動体の位置を推定する推定部と、前記モータの電流値が第１閾値を超え、かつ、前記推定部により推定された推定位置と前記移動体の現在位置との差が第２閾値を超える場合に、前記移動体が物体に衝突したと判定する衝突判定部と、を備える。

【0007】

第１態様によれば、移動体の物体への衝突を精度良く検出することができる。

【0008】

また、第２態様に係る制御装置は、第１態様に係る制御装置において、位置ゲインの逆数を時定数とするローパスフィルタ処理の実行により前記推定位置を算出する。

【0009】

第２態様によれば、ローパスフィルタ処理の実行により精度の高い推定位置を得ることができる。

【0010】

また、第３態様に係る制御装置は、第１態様又は第２態様に係る制御装置において、前記電流値が、前記モータの電流検出値である、又は、前記位置指令値に基づいて算出される電流指令値である。

【0011】

第３態様によれば、電流検出値又は電流指令値として電流値を得ることができる。

【0012】

また、第４態様に係る制御装置は、第１態様～第３態様の何れか１態様に係る制御装置において、前記衝突判定部が、前記電流値が前記第１閾値を超え、かつ、前記推定位置と前記現在位置との差が前記第２閾値を超える状態が所定時間継続した場合に、前記移動体が前記物体に衝突したと判定する。

【0013】

第４態様によれば、衝突判定条件を満たす状態が所定時間継続するという条件を追加することにより誤判定を抑制することができる。

【0014】

また、第５態様に係る制御装置は、第１態様～第４態様の何れか１態様に係る制御装置において、前記衝突判定部が、前記電流値が前記第１閾値を超え、かつ、前記推定位置と前記現在位置との差が前記第２閾値以下で、かつ、前記推定位置が前記移動体の到達目標とする位置である目標位置に到達したときに前記現在位置が前記目標位置に対して所定の範囲外である場合に、前記移動体が前記物体に衝突したと判定する。

【0015】

第５態様によれば、目標位置付近の衝突を精度良く検出することができる。

【0016】

また、第６態様に係る制御装置は、第５態様に係る制御装置において、前記衝突判定部が、前記電流値が前記第１閾値を超え、かつ、前記推定位置と前記現在位置との差が前記第２閾値以下で、かつ、前記推定位置が前記目標位置に到達したときに前記現在位置が前記目標位置に対して所定の範囲外である状態が所定時間継続した場合に、前記移動体が前記物体に衝突したと判定する。

【0017】

第６態様によれば、衝突判定条件を満たす状態が所定時間継続するという条件を追加することにより誤判定を抑制することができる。

【0018】

また、第７態様に係る制御装置は、移動体を移動させるモータを制御する制御装置であって、前記移動体に関する位置指令値に基づいて、前記移動体の位置を推定する推定部と、前記モータの電流値が第１閾値を超え、かつ、前記推定部により推定された推定位置が前記移動体の到達目標とする位置である目標位置に到達したときに前記移動体の現在位置が前記目標位置に対して所定の範囲外である場合に、前記移動体が物体に衝突したと判定する衝突判定部と、を備える。

【0019】

第７態様によれば、目標位置付近の衝突を精度良く検出することができる。

【0020】

更に、上記目的を達成するために、第８態様に係るアクチュエータは、移動体と、前記移動体を移動させるモータと、前記モータを制御する、第１態様～第７態様の何れか１態様に係る制御装置と、を含む。

【0021】

第８態様によれば、移動体の物体への衝突を精度良く検出可能なアクチュエータを提供することができる。

【0022】

更に、上記目的を達成するために、第９態様に係る衝突検出方法は、移動体を移動させるモータを制御する制御装置が実行する衝突検出方法であって、前記移動体に関する位置指令値に基づいて、前記移動体の位置を推定し、前記モータの電流値が第１閾値を超え、かつ、前記推定された推定位置と前記移動体の現在位置との差が第２閾値を超える場合に、前記移動体が物体に衝突したと判定する。

【0023】

第９態様によれば、上記第１態様と同様に、移動体の物体への衝突を精度良く検出することができる。

【発明の効果】

【0024】

以上説明したように、本発明によれば、移動体の物体への衝突を精度良く検出することができる、という効果が得られる。

【図面の簡単な説明】

【0025】

【図1】第１の実施形態に係る制御装置及びアクチュエータの構成の一例を概略的に示す図である。

【図2】第１の実施形態に係る制御装置が備える制御部の電気的な構成の一例を示すブロック図である。

【図3】各種移動指令における移動体の移動速度の遷移例を示す波形図である。

【図4】比較例に係る衝突検出機能の説明に供する図である。

【図5】第１の実施形態に係る制御装置の機能的な構成の一例を示すブロック図である。

【図6】第１の実施形態に係るアクチュエータの衝突判定時の状態を模式的に示す図である。

【図7】第１の実施形態に係るアクチュエータによる衝突判定処理の説明に供する図である。

【図8】衝突検出ステータスがオンされるまでの衝突判定処理の説明に供する図である。

【図9】第１の実施形態に係る制御プログラムによる衝突判定処理の流れの一例を示すフローチャートである。

【図10】第２の実施形態に係るアクチュエータの衝突判定時の状態を模式的に示す図である。

【図11】第２の実施形態に係るアクチュエータによる衝突判定処理の説明に供する図である。

【図12】移動体の現在位置が位置決め範囲外である例と位置決め範囲内である例を模式的に示す図である。

【図13】第２の実施形態に係る制御プログラムによる衝突判定処理の流れの一例を示すフローチャートである。

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

【0026】

以下、図面を参照して、本開示の技術を実施するための形態の一例について詳細に説明する。なお、動作、作用、機能が同じ働きを担う構成要素及び処理には、全図面を通して同じ符号を付与し、重複する説明を適宜省略する場合がある。各図面は、本開示の技術を十分に理解できる程度に、概略的に示してあるに過ぎない。よって、本開示の技術は、図示例のみに限定されるものではない。また、本実施形態では、本発明と直接的に関連しない構成や周知な構成については、説明を省略する場合がある。

【0027】

[第１の実施形態]
図１は、第１の実施形態に係る制御装置１０及びアクチュエータ２０の構成の一例を概略的に示す図である。

【0028】

図１に示すように、アクチュエータ２０は、エンコーダ２１、モータ２２、出力シャフト２３、カップリング２４、ボールねじ軸２５、ボールねじナット２６、移動体２７、及びハウジング２８を備えている。なお、ボールねじ軸２５及びボールねじナット２６は、すべりねじ軸及びすべりねじナットとしてもよい。また、図１の例では、移動体２７は、テーブルタイプとして示しているが、ロッドタイプであってもよい。

【0029】

エンコーダ２１は、モータ２２に取り付けられている。エンコーダ２１は、モータ２２の回転位置を検出し、検出した位置をフィードバック位置信号として制御装置１０に出力する。

【0030】

モータ２２は、制御装置１０によって制御され、移動体２７を移動させるための駆動源である。モータ２２は、例えば、三相のＡＣサーボモータである。但し、これに限るものではなく、モータ２２は、ＤＣブラシレスモータ又はステッピングモータなどのモータであってもよい。

【0031】

モータ２２の出力シャフト２３は、カップリング２４を介して、ボールねじ軸２５が結合されている。ボールねじ軸２５は、ボールねじナット２６と共に、モータ２２の回転運動を並進運動に変換するための機械部品として構成される。移動体２７は、ボールねじナット２６を介して、ボールねじ軸２５に接合されている。

【0032】

ハウジング２８は、出力シャフト２３、カップリング２４、ボールねじ軸２５、ボールねじナット２６を収容している。ボールねじ軸２５は、ハウジング２８内にてベアリング（図示省略）によって回転可能に支持されている。また、ハウジング２８の上面は、移動体２７がハウジング２８の上部で移動可能となるように開口している。但し、当該開口は、移動体２７のハウジング２８の上部での移動を妨げないような構造でステンレスシート（図示省略）などのシート部材によって塞がれている。

【0033】

制御装置１０は、制御部１００、インバータ部１０１、及び電流センサ２９を備え、ｄ軸電流指令値及びｑ軸電流指令値を用いたベクトル制御によってモータ２２を制御するコントローラである。制御部１００、インバータ部１０１、及び電流センサ２９は、例えば、筐体に収容されている。ｄ軸電流指令値及びｑ軸電流指令値は、モータ２２の通電電流を制御するための指令値である。ｄ軸とは、磁束の方向であり、ｑ軸とは、ｄ軸に直交する方向であり、トルクの大きさに寄与する。なお、図１では、制御装置１０は、アクチュエータ２０と別体に設けられているが、アクチュエータ２０に内蔵されていてもよい。すなわち、アクチュエータ２０は、制御装置１０を備えていてもよい。また、インバータ部１０１及び電流センサ２９は、アクチュエータ２０に内蔵されていてもよい。すなわち、アクチュエータ２０は、インバータ部１０１及び電流センサ２９を備えていてもよく、この場合、制御装置１０は制御部１００のみを備えていてもよい。

【0034】

インバータ部１０１は、例えば、図示省略の交流電源及びコンバータを介して供給された直流電力を交流電力に変換してモータ２２に供給する駆動回路（ドライバ）である。インバータ部１０１は、モータ２２の複数の巻線（三相の場合、Ｕ、Ｖ、Ｗ相巻線）に対応して設けられ、制御部１００によってオン、オフ駆動が制御される複数のスイッチング素子等で構成されている。電流センサ２９は、例えば、ホール素子などの磁気検出素子等で構成されている。より詳細には、電流センサ２９は、例えば、インバータ部１０１とモータ２２の複数の巻線との間の電流路のうちＵ相巻線、Ｗ相巻線のそれぞれに向かう電流路に設けられており、モータ２２に流れる電流に応じた信号を制御部１００に出力する。

【0035】

図２は、第１の実施形態に係る制御装置１０が備える制御部１００の電気的な構成の一例を示すブロック図である。

【0036】

図２に示すように、本実施形態に係る制御装置１０の制御部１００は、ＣＰＵ（Ｃｅｎｔｒａｌ Ｐｒｏｃｅｓｓｉｎｇ Ｕｎｉｔ）１１と、ＲＯＭ（Ｒｅａｄ Ｏｎｌｙ Ｍｅｍｏｒｙ）１２と、ＲＡＭ（Ｒａｎｄｏｍ Ａｃｃｅｓｓ Ｍｅｍｏｒｙ）１３と、入出力インタフェース（Ｉ／Ｏ）１４と、記憶部１５と、接続部１６と、を備えている。

【0037】

ＣＰＵ１１、ＲＯＭ１２、ＲＡＭ１３、及びＩ／Ｏ１４は、バスを介して各々接続されている。Ｉ／Ｏ１４には、記憶部１５と、接続部１６と、を含む各機能部が接続されている。これらの各機能部は、Ｉ／Ｏ１４を介して、ＣＰＵ１１と相互に通信可能とされる。

【0038】

ＣＰＵ１１、ＲＯＭ１２、ＲＡＭ１３、及びＩ／Ｏ１４は、例えば、マイクロコンピュータ（マイコン）を構成する。マイコンは、制御部１００の一部の動作を制御するサブ制御部として構成されてもよいし、制御部１００の全体の動作を制御するメイン制御部の一部として構成されてもよい。マイコンの各ブロックの一部又は全部には、例えば、ＬＳＩ（Ｌａｒｇｅ Ｓｃａｌｅ Ｉｎｔｅｇｒａｔｉｏｎ）等の集積回路又はＩＣ（Ｉｎｔｅｇｒａｔｅｄ Ｃｉｒｃｕｉｔ）チップセットが用いられる。上記各ブロックに個別の回路を用いてもよいし、一部又は全部を集積した回路を用いてもよい。上記各ブロック同士が一体として設けられてもよいし、一部のブロックが別に設けられてもよい。また、上記各ブロックのそれぞれにおいて、その一部が別に設けられてもよい。マイコンの集積化には、ＬＳＩに限らず、専用回路又は汎用プロセッサを用いてもよい。

【0039】

記憶部１５としては、例えば、ＨＤＤ（Ｈａｒｄ Ｄｉｓｋ Ｄｒｉｖｅ）、ＳＳＤ（Ｓｏｌｉｄ Ｓｔａｔｅ Ｄｒｉｖｅ）、フラッシュメモリ等が用いられる。ＲＯＭ１２又は記憶部１５には、モータ２２を制御するための制御プログラム、モータ２２の制御に必要な各種の設定値、データテーブル等が記憶される。

【0040】

制御プログラムは、例えば、制御部１００に予めインストールされていてもよい。制御プログラムは、不揮発性の記憶媒体に記憶して、又はネットワークを介して配布して、制御部１００に適宜インストールすることで実現してもよい。なお、不揮発性の記憶媒体の例としては、ＣＤ-ＲＯＭ（Ｃｏｍｐａｃｔ Ｄｉｓｃ Ｒｅａｄ Ｏｎｌｙ Ｍｅｍｏｒｙ）、光磁気ディスク、ＨＤＤ、ＤＶＤ-ＲＯＭ（Ｄｉｇｉｔａｌ Ｖｅｒｓａｔｉｌｅ Ｄｉｓｃ Ｒｅａｄ Ｏｎｌｙ Ｍｅｍｏｒｙ）、フラッシュメモリ、メモリカード等が想定される。

【0041】

接続部１６は、エンコーダ２１、モータ２２、及びＰＬＣ（Ｐｒｏｇｒａｍｍａｂｌｅ Ｌｏｇｉｃ Ｃｏｎｔｒｏｌｌｅｒ）等の上位装置の各々と接続するためのインタフェースである。

【0042】

図３は、各種移動指令における移動体２７の移動速度の遷移例を示す波形図である。縦軸に移動体２７の移動速度を示し、横軸に時間を示す。

【0043】

各種移動指令としては、例えば、位置決め動作指令、及び原点復帰動作指令が挙げられる。位置決め動作とは、モータ２２の回転に応じて、移動体２７を、位置決め動作開始点から位置決め動作の目標位置まで移動させる動作である。なお、目標位置とは、移動体２７の到達目標とする位置である。例えば、図１に示す矢印のように、移動体２７は、前進端への位置決め動作では原点（後退端）から前進端まで移動し、原点（後退端）への位置決め動作では前進端から原点（後退端）まで移動する。原点復帰動作とは、モータ２２の回転に応じて、移動体２７を原点復帰動作開始点から原点(原点復帰動作の目標位置)まで移動させる動作である。例えば、移動体２７は、前進端から原点（後退端）まで移動する。

【0044】

図３は、各種動作における移動体２７の移動速度の時間変化の一例を示す。より詳細には、図３は、例えば、原点復帰動作、前進端への位置決め動作、及び原点（後退端）への位置決め動作の順に行った場合の移動体２７の移動速度の時間変化を示す。図３に示すように、位置決め動作では移動体２７を比較的高速で移動させて、原点復帰動作では移動体２７を比較的低速で移動させる。なお、図３に示す例において、各動作における移動体２７の移動速度は、前進端への位置決め動作における速度＞原点（後退端）への位置決め動作における速度＞原点復帰動作における速度という関係となっている。

【0045】

ところで、上述したように、モータの電流値としてのフィードバック電流値（以下、単に「ＦＢ電流値」という。）と閾値との比較による衝突検出機能では、移動体の加減速中は電流値が比較的大きくなるため同じ閾値を使うと誤検出してしまう。

【0046】

図４は、比較例に係る衝突検出機能の説明に供する図であり、移動体の速度とＦＢ電流との関係を示す。なお、図４において、横軸は移動体の位置を示し、縦軸左は電流値を示し、縦軸右は移動体の速度を示す。

【0047】

図４に示すように、比較例に係る衝突検出機能は、移動体が移動開始位置から目標位置に至るまでの速度一定期間中にのみ設定される。つまり、速度一定期間中にＦＢ電流値（例えば、トルク電流検出値（ｑ軸電流検出値））が閾値を超えた場合に、衝突と判定する。この場合、速度一定期間外、すなわち、移動開始位置付近及び目標位置付近における加減速中では、衝突を検出することができない。また、この衝突検出機能を加減速中にも適用した場合、加減速中ではＦＢ電流値が速度一定期間中よりも大きくなるため、同じ閾値を使うと誤検出してしまう。

【0048】

これに対して、本実施形態に係る制御装置１０は、移動体２７に関する位置指令値に基づいて、移動体２７の位置を推定し、モータ２２の電流値が第１閾値を超え、かつ、推定された推定位置と移動体２７の現在位置との差が第２閾値を超える場合に、移動体２７が物体に衝突したと判定する。なお、ここでいう物体とは、移動体２７の移動経路に存在する障害物等である。また、衝突とは、移動体２７の移動中に物体にぶつかる状態をいう。また、推定位置とは、障害物等の物体がなければ移動したであろう仮想の位置である。

【0049】

具体的に、本実施形態に係る制御装置１０のＣＰＵ１１は、ＲＯＭ１２又は記憶部１５に記憶されている制御プログラムをＲＡＭ１３に書き込んで実行することにより、図５に示す各部として機能する。

【0050】

図５は、第１の実施形態に係る制御装置１０の機能的な構成の一例を示すブロック図である。

【0051】

図５に示すように、本実施形態に係る制御装置１０のＣＰＵ１１は、移動指令取得部１１１、運転計画生成部１１２、第１減算器１１３、第１位置制御器１１４、第２減算器１１５、速度制御器１１６、第３減算器１１７、電流制御部１１８、推定部１１９、衝突判定部１２３、位置情報取得部１２４、現在速度換算部１２５、電気角換算部１２６、及び電流検出部１２７として機能する。また、推定部１１９は、第４減算器１２０、第２位置制御器１２１、及び積分器１２２を含む。

【0052】

移動指令取得部１１１は、例えば、ＰＬＣ等の上位装置から移動指令を取得する。なお、制御装置１０がプログラム作成機能を有している場合には、制御装置１０自体がＰＬＣとして機能してもよい。この移動指令は、例えば、位置決め動作指令及び原点復帰動作指令のいずれかである。位置決め動作指令は、例えば、前進端などの位置決め動作終了位置を示す目標位置等で構成される位置情報と、位置決め動作の上限速度を示す目標速度等で構成される速度情報と、加速度及び減速度で構成される加減速度情報と、を含むパラメータで構成され、位置決め動作を実現するための指令である。なお、位置決め動作速度は、位置決め動作の時間が短くなるように比較的高い目標速度に設定されている。原点復帰動作指令は、例えば、原点復帰動作終了位置を示す原点（後退端）等で構成される位置情報と、原点復帰動作の上限速度を示す目標速度等で構成される速度情報と、加減速度情報と、を含むパラメータで構成され、原点復帰動作を実現するための指令である。原点復帰動作速度は、例えば、位置決め動作速度よりも比較的低い目標速度（例えば、２０ｍｍ／ｓ以下）に設定されている。移動指令取得部１１１は、移動指令から、当該移動指令の種別を判定し、判定結果を運転計画生成部１１２に出力する。

【0053】

運転計画生成部１１２は、移動体２７の目標位置に向けて位置指令値を生成し、生成した位置指令値を第１減算器１１３及び第４減算器１２０の各々に出力する。具体的に、運転計画生成部１１２は、移動体２７に関する加速度、減速度、目標速度及び目標位置を含むパラメータと現在位置とに基づいて位置指令値を生成する。移動体２７の目標位置、目標速度、及び加減速度を含むパラメータは、各種移動指令から取得される。運転計画生成部１１２は、移動体２７の移動速度が、一例として、上述の図３に示すように、目標速度を上限とした台形形状に時間的に変化するように位置指令値を生成する。

【0054】

より詳細には、運転計画生成部１１２は、各種移動指令を取得すると、現在の位置指令値と目標位置との差から移動体２７を移動させたい距離を定め、移動体２７の速度が目標速度を上底とする台形形状（移動距離によっては目標速度に達しない三角形状）となるように位置指令値を逐次（例えば、１ｍｓの制御周期で）生成する。言い換えると、運転計画生成部１１２は、移動体２７の移動速度を加速中は移動速度が目標速度となるまでパラメータの加速度で加速させて、一定速中は目標速度を維持し、減速中は移動速度がゼロとなるまでパラメータの減速度で減速させて、移動開始時点から移動終了時点までの時間における速度変化を示す台形（移動距離によっては目標速度に達しない三角形）の面積、つまり、速度の時間積分が、上記定めた距離となるように位置指令値を逐次生成する。また、逐次生成される位置指令値の変化量は、加速中においては増加し、減速中においては減少し、速度一定のときはパラメータの目標速度を超えないように調整される。つまり、目標速度は位置指令値の変化量のリミッタを意味する。これにより、図３に示す挙動、つまり、各種移動指令における速度の時間変化を実現するような位置指令値が生成される。

【0055】

第１減算器１１３は、運転計画生成部１１２からの位置指令値から、位置情報取得部１２４からの現在位置（位置検出値）を減じて得られた位置偏差を第１位置制御器１１４に出力する。

【0056】

第１位置制御器１１４は、第１減算器１１３から得られた位置偏差に位置制御用ゲイン（設定値）を乗じて速度指令値を生成し、生成した速度指令値を第２減算器１１５に出力する。第１位置制御器１１４は、比例制御を行う。

【0057】

第２減算器１１５は、第１位置制御器１１４からの速度指令値から、現在速度換算部１２５からの現在速度（速度検出値）を減じて得られた速度偏差を速度制御器１１６に出力する。

【0058】

速度制御器１１６は、第２減算器１１５から得られた速度偏差に速度制御用比例ゲイン／速度制御用積分ゲイン（設定値）を乗じて積分することによりトルク電流指令値を生成し、生成したトルク電流指令値を第３減算器１１７に出力する。速度制御器１１６は、比例／積分制御を行う。ここで、本実施形態では、ｄ軸電流指令値Ｉ^＊ｄ＝０としてモータ２２の制御を行うため、図５の例において、ｄ軸電流指令値Ｉ^＊ｄの制御ブロックに関する図示を省略している。そのため、トルク電流指令値は、モータ２２のトルクの発生に寄与するｑ軸電流指令値Ｉ^＊ｑであるものとして説明する。

【0059】

第３減算器１１７は、速度制御器１１６からのトルク電流指令値（＝ｑ軸電流指令値Ｉ^＊ｑ）から、後述の電流検出部１２７からのＦＢ電流値としてのｑ軸電流検出値Ｉｑを減じて得られた電流偏差を電流制御部１１８に出力する。

【0060】

位置情報取得部１２４は、モータ２２に取り付けられたエンコーダ２１から、モータ２２のフィードバック位置信号を取得する。位置情報取得部１２４は、フィードバック位置信号に基づいて得られる移動体２７の位置検出値を現在位置として第１減算器１１３、現在速度換算部１２５、衝突判定部１２３、及び電気角換算部１２６の各々に出力する。

【0061】

現在速度換算部１２５は、位置情報取得部１２４からの現在位置を現在速度（速度検出値）に換算し、換算した現在速度（速度検出値）を第２減算器１１５に出力する。

【0062】

電気角換算部１２６は、位置情報取得部１２４からの現在位置をモータ２２の電気角に換算し、電流制御部１１８に出力する。

【0063】

電流検出部１２７は、電流センサ２９からの出力に基づいてＦＢ電流値としてのｑ軸電流検出値を算出することにより検出する。より詳細には、電流検出部１２７は、まず、電流センサ２９からのＵ相電流検出値Ｉｕ、Ｗ相電流検出値Ｉｗを入力し、Ｉｕ＋Ｉｖ＋Ｉｗ＝０の関係からＶ相電流検出値を算出することで検出する。次に、電流検出部１２７は、Ｕ，Ｖ，Ｗ相電流Ｉｕ，Ｉｖ，Ｉｗを静止座標系（α、β座標系）でのα、β軸電流検出値に変換する。その後、電流検出部１２７は、電気角換算部１２６からの電気角を用いて、α、β軸電流検出値を回転座標系であるｄ、ｑ軸座標系のｄ軸電流検出値Ｉｄ及びｑ軸電流検出値Ｉｑを算出する。電流検出部１２７は、ｑ軸電流検出値ＩｑをＦＢ電流値として第３減算器１１７及び衝突判定部１２３に出力する。一方、電流検出部１２７は、図示については省略するが、ｄ軸電流検出値Ｉｄを電流制御部１１８へ出力する。

【0064】

電流制御部１１８は、第３減算器１１７からの電流偏差及び電気角換算部１２６からの電気角を用いて、モータ２２を制御する。より詳細には、モータ２２が三相のＡＣサーボモータである場合、電流制御部１１８は、まず、ｄ軸電流指令値I^＊ｄを０とし（Ｉ^＊ｄ＝０）、ｄ軸電流検出値Ｉｄとのｄ軸に関する電流偏差を算出する。次に、電流制御部１１８は、ｄ、ｑ軸に関する電流偏差に基づいて、ｄ、ｑ軸電圧指令値Ｖｄ、Ｖｑを生成する。次に、電流制御部１１８は、電気角を用いてｄ、ｑ軸電圧指令値Ｖｄ、Ｖｑをα、β軸での電圧指令値に変換する。その後、電流制御部１１８は、α、β軸での電圧指令値に基づいて、インバータ部１０１のＵ，Ｖ，Ｗ相巻線に対応して設けられた複数のスイッチング素子をオン、オフ駆動させるための指令値を算出し、これらに基づいてインバータ部１０１を制御することで、モータ２２の駆動を制御する。

【0065】

一方、推定部１１９は、移動体２７に関する位置指令値に基づいて、移動体２７の位置を推定する。推定部１１９は、例えば、位置制御ゲインの逆数を時定数とするローパスフィルタの実行により推定位置を算出する。具体的に、第４減算器１２０は、運転計画生成部１１２からの位置指令値から、積分器１２２から得られる推定位置を減じて得られた推定位置偏差を第２位置制御器１２１に出力する。

【0066】

第２位置制御器１２１は、第４減算器１２０から得られた推定位置偏差に位置制御用ゲイン（設定値）を乗じて速度推定値を生成し、生成した速度推定値を積分器１２２に出力する。

【0067】

積分器１２２は、第２位置制御器１２１からの速度推定値を積分することにより得られる推定位置を第４減算器１２０及び衝突判定部１２３の各々に出力する。推定位置は、第１位置制御器１１４が位置偏差に乗算した位置制御ゲインと同じ位置制御用ゲインを考慮して、位置指令値に対する理想的な位置として算出されるものである。

【0068】

衝突判定部１２３は、モータ２２の電流値（ＦＢ電流値）が第１閾値を超え、かつ、推定部１１９で推定された推定位置と、位置情報取得部１２４から得られた移動体２７の現在位置との差が第２閾値を超える場合に、移動体２７が物体に衝突したと判定する。第１閾値及び第２閾値は、例えば、ＲＯＭ１２又は記憶部１５に予め記憶されており、過去の知見、実験結果等に基づいて適切な値が設定される。つまり、本実施形態では、ＦＢ電流値が第１閾値を超えることを条件１とし、推定位置と現在位置との差が第２閾値を超えることを条件２とし、条件１と条件２の両方を満たす場合に、衝突と判定する。なお、第２閾値を用いた位置の衝突判定は、例えば、エンコーダ２１のカウント値に基づいて実行される。この場合、第２閾値をエンコーダ２１のカウント値に換算して用いればよい。一例として、第２閾値を２[ｍｍ]、ボールねじ軸２５のリード幅を２０[ｍｍ]、エンコーダ分解能を１６３８４[ｃｏｕｎｔ]とした場合、エンコーダ２１のカウント値に換算した第２閾値は、２／２０×１６３８４≒１６３８、となる。

【0069】

また、衝突判定部１２３は、電流値（ＦＢ電流値）が第１閾値を超え、かつ、推定位置と現在位置との差が第２閾値を超える状態が所定時間継続した場合に、移動体２７が物体に衝突したと判定してもよい。ここで、所定時間は、例えば、５[ｍｓ]以上１５[ｍｓ]以下の範囲で適切な値が設定され、望ましくは、１０[ｍｓ]が設定される。以下、この所定時間を「衝突判定時間」という。

【0070】

次に、図６～図８を参照して、第１の実施形態に係る衝突判定処理について具体的に説明する。

【0071】

図６は、第１の実施形態に係るアクチュエータ２０の衝突判定時の状態を模式的に示す図である。

【0072】

図６に示すように、移動体２７が物体３０に衝突した場合、ＦＢ電流値が第１閾値を超えるため条件１を満たすが、条件１だけでは、移動体２７が加減速中である可能性を排除できない。また、速度一定時においてもベアリング等の抵抗によりＦＢ電流値が上がる場合もある。このため、移動体２７の現在位置と推定位置との差Ｄ１が第２閾値を超えるという条件２についても判定し、条件２も満たす場合に、加減速中であること、あるいは、ベアリング等の抵抗が原因でＦＢ電流値が増大したわけではない、つまり、加減速中、あるいは、ベアリング等の抵抗が原因である可能性が排除される。これにより、加減速中であるか否かによらず、あるいは、ベアリング等の抵抗によらず、精度良く衝突が検出される。したがって、上述の図４を参照して説明した比較例と比較して、衝突を検出可能な範囲を拡大することができる。また、条件２と共に条件１を満たしたときに、衝突と判定することで、条件２を満たしたことのみで衝突と判定する場合と比較して、衝突検出の精度を高めることができる。

【0073】

図７は、第１の実施形態に係るアクチュエータ２０による衝突判定処理の説明に供する図である。なお、図７の例では、衝突判定時間を例えば１０[ｍｓ]とした場合について示す。

【0074】

図７において、（Ａ）に示すグラフは、条件２に係る移動体２７の位置の時間変化を示し、（Ｂ）に示すグラフは、条件１に係るＦＢ電流値の時間変化を示す。（Ｃ）に示すタイミングチャートは、電流（ＦＢ電流値）による衝突検出をオンするタイミング、及び、位置差分（推定位置と現在位置との差）による衝突検出をオンするタイミングを示す。（Ｄ）に示すグラフは、衝突判定時間を計測する衝突検出カウンタ（以下、単に「カウンタ」ともいう。）のカウント値の時間変化を示す。（Ｅ）に示すタイミングチャートは、衝突検出ステータスをオンするタイミングを示す。なお、カウンタは、図示を省略するが、例えば、制御装置１０が備えるタイマ機能により実現される。

【0075】

但し、（Ａ）において、Ｐ_ｔｈは第２閾値を示し、（Ｂ）において、Ｉ_ｔｈは第１閾値を示し、（Ｄ）において、Ｔ_ｔｈは衝突判定時間（例えば１０ｍｓ）を示す。

【0076】

（Ａ）において、時刻Ｔ１は移動体２７が物体に衝突した時刻を示す。時刻Ｔ１での移動体２７の位置が現在位置に対応する。また、時刻Ｔ３は移動体２７の推定位置と現在位置との差が第２閾値Ｐ_ｔｈを超えたときの時刻を示す。時刻Ｔ３では（Ｃ）に示す位置差分による衝突検出がオンされる。つまり、条件２を満たす。

【0077】

（Ｂ）において、時刻Ｔ２はＦＢ電流値が第１閾値Ｉ_ｔｈを超えたときの時刻を示す。時刻Ｔ２では（Ｃ）に示す電流による衝突検出がオンされる。つまり、条件１を満たす。

【0078】

（Ｄ）において、条件１、２を満たすと判定した時点、つまり、時刻Ｔ３でカウンタが衝突判定時間Ｔ_ｔｈのカウントを開始する。時刻Ｔ４は時刻Ｔ３からの経過時間が衝突判定時間Ｔ_ｔｈに到達したときの時刻を示す。時刻Ｔ４では（Ｅ）に示す衝突検出ステータスがオンされる。つまり、カウンタのカウント値が衝突判定時間Ｔ_ｔｈに到達すると、衝突と判定され、（Ｅ）に示す衝突検出ステータスがオンされる。

【0079】

上記より、条件１、２を満たす状態が衝突判定時間Ｔ_ｔｈ以上継続した場合に、移動体２７が物体に衝突したと判定される。条件１、２を満たした場合に即時衝突検出とすると、誤検出の可能性が残るため、誤検出の可能性をより低減するために、衝突判定時間Ｔ_ｔｈを設けることが望ましい。ＦＢ電流値は、アクチュエータ２０の運転状態、設置状態等によっても変化する。また、移動体２７の推定位置についても、摩擦等の外乱は考慮されない。このため、移動体２７の現在位置と推定位置との差には動作開始、停止間際で若干の誤差が発生する可能性がある。このような誤差による衝突の誤検出を防止するためにも、衝突判定時間Ｔ_ｔｈを設けることが望ましい。

【0080】

図８は、衝突検出ステータスがオンされるまでの衝突判定処理の説明に供する図である。

【0081】

図８において、（Ａ）に示すグラフは、条件２に係る移動体２７の位置の時間変化を示し、（Ｂ）に示すグラフは、条件１に係るＦＢ電流値の時間変化を示す。（Ｃ）に示すグラフは、衝突判定時間を計測するカウンタのカウント値の時間変化を示す。

【0082】

（Ａ）において、時刻Ｔ１１で移動体２７が物体に衝突する。時刻Ｔ１１での移動体２７の位置が現在位置に対応する。また、時刻Ｔ１２で移動体２７の推定位置と現在位置との差が第２閾値Ｐ_ｔｈを超え、条件２を満たす。

【0083】

（Ｂ）において、時刻Ｔ１１と時刻Ｔ１２の間でＦＢ電流値が第１閾値Ｉ_ｔｈを超え、条件１を満たす。

【0084】

（Ｃ）において、条件１、２を満たすと判定した時点、つまり、時刻Ｔ１２でカウンタが衝突判定時間Ｔ_ｔｈのカウントを開始する。時刻Ｔ１３は時刻Ｔ１２からの経過時間が衝突判定時間Ｔ_ｔｈに到達したときの時刻を示す。カウンタのカウント値が衝突判定時間Ｔ_ｔｈに到達すると、衝突と判定され、衝突検出ステータスがオンされる。

【0085】

つまり、条件１、２を満たすと判定した時刻Ｔ１２から衝突判定時間Ｔ_ｔｈのカウントを開始し、衝突判定時間Ｔ_ｔｈに到達すると、衝突検出ステータスがオンされる。このとき、衝突判定時間Ｔ_ｔｈへの到達の前に、条件１及び条件２の少なくとも一方を満たさない状態になると、カウント値は減少する。図８の例では、時刻Ｔ１２と時刻Ｔ１３との間で条件１を満たさない状態になるため、カウント値が減少するが、その後、条件１を満たすと再びカウント値が増加するようになっている。

【0086】

次に、図９を参照して、第１の実施形態に係る制御装置１０の作用を説明する。

【0087】

図９は、第１の実施形態に係る制御プログラムによる衝突判定処理の流れの一例を示すフローチャートである。

【0088】

まず、制御装置１０に対してモータ制御の指示がなされると、ＣＰＵ１１によって制御プログラムが起動され、以下の各処理を実行する。なお、本衝突判定処理は、所定の間隔（例えば、１ｍｓ周期）で繰り返し実行される。また、推定位置は、本衝突判定処理の実行周期よりも短い時間間隔（例えば、１００μｓ周期）で算出されている。

【0089】

図９のステップＳ１１１では、ＣＰＵ１１が、移動体２７の推定位置と現在位置との差[Ｄｎ]を算出する。

【0090】

ステップＳ１１２では、ＣＰＵ１１が、ステップＳ１１１で算出した差[Ｄｎ]が第２閾値Ｐ_ｔｈを超えたか否かを判定する。差[Ｄｎ]が第２閾値Ｐ_ｔｈを超えたと判定した場合（ＹＥＳ：肯定判定の場合）、ステップＳ１１３に移行し、差[Ｄｎ]が第２閾値Ｐ_ｔｈ以下と判定した場合（ＮＯ：否定判定の場合）、ステップＳ１１５に移行する。

【0091】

ステップＳ１１３では、ＣＰＵ１１が、ＦＢ電流値が第１閾値Ｉ_ｔｈを超えたか否かを判定する。ＦＢ電流値が第１閾値Ｉ_ｔｈを超えたと判定した場合（ＹＥＳ：肯定判定の場合）、ステップＳ１１４に移行し、ＦＢ電流値が第１閾値Ｉ_ｔｈ以下と判定した場合（ＮＯ：否定判定の場合）、ステップＳ１１５に移行する。

【0092】

ステップＳ１１４では、ＣＰＵ１１が、衝突と判定し、ステップＳ１１６に移行する。

【0093】

一方、ステップＳ１１５では、ＣＰＵ１１が、非衝突と判定し、ステップＳ１１６に移行する。

【0094】

ステップＳ１１６では、ＣＰＵ１１が、衝突と判定したか否かを判定する。衝突と判定した場合（ＹＥＳ：肯定判定の場合）、ステップＳ１１７に移行し、非衝突と判定した場合（ＮＯ：否定判定の場合）、ステップＳ１１８に移行する。

【0095】

ステップＳ１１７では、ＣＰＵ１１が、衝突検出カウンタのカウント値をインクリメントし、ステップＳ１２１に移行する。

【0096】

一方、ステップＳ１１８では、ＣＰＵ１１が、衝突検出カウンタのカウント値をデクリメントし、ステップＳ１１９に移行する。

【0097】

ステップＳ１１９では、ＣＰＵ１１が、衝突検出カウンタのカウント値がゼロ（＝０）より小さいか否かを判定する。カウント値がゼロより小さいと判定した場合（ＹＥＳ：肯定判定の場合）、ステップＳ１２０に移行し、カウント値がゼロ以上と判定した場合（ＮＯ：否定判定の場合）、ステップＳ１２１に移行する。

【0098】

ステップＳ１２０では、ＣＰＵ１１が、衝突検出カウンタのカウント値に「ゼロ（＝０）」を設定し、ステップＳ１２１に移行する。

【0099】

ステップＳ１２１では、ＣＰＵ１１が、衝突検出カウンタのカウント値が衝突判定時間Ｔ_ｔｈ以上であるか否かを判定する。カウント値が衝突判定時間Ｔ_ｔｈ以上であると判定した場合（ＹＥＳ：肯定判定の場合）、ステップＳ１２２に移行し、カウント値が衝突判定時間Ｔ_ｔｈ未満であると判定した場合（ＮＯ：否定判定の場合）、ステップＳ１２４に移行する。

【0100】

ステップＳ１２２では、ＣＰＵ１１が、衝突検出カウンタに「衝突判定時間Ｔ_ｔｈ」を設定する。

【0101】

ステップＳ１２３では、ＣＰＵ１１が、衝突検出として、衝突検出ステータスをオンし、本制御プログラムによる衝突判定処理を終了する。

【0102】

一方、ステップＳ１２４では、ＣＰＵ１１が、衝突非検出として、衝突検出ステータスをオフし、本制御プログラムによる衝突判定処理を終了する。

【0103】

ここで、衝突検出後、制御装置１０は、自ら警告音等によって衝突検出を報知してもよいし、ＰＬＣ等の上位装置に衝突検出を通知するようにしてもよい。また、衝突検出後、制御装置１０がモータ２２を減速、停止させるようにしてもよい。

【0104】

また、第２閾値（例えば、２ｍｍ）は、ユーザ設定可能とされる。第２閾値は、例えば、ユーザの使用状態、アクチュエータのサイズ（ストローク、移動体の駆動機構としてのボールねじのリード幅）、アクチュエータの設置状態等を考慮して設定するのが望ましい。

【0105】

また、速度一定区間では、条件１を満たすか否かによって衝突を検出し、加減速区間では、条件１、２を満たすか否かによって衝突を検出してもよい。このとき、速度一定区間、加減速区間であることは、上述の特許文献１（特開２０１４－０８７２３５号公報）のように検出ゾーンを設定して現在位置が検出ゾーン内にあるか否かを判定する方法、あるいは、移動指令の目標速度と検出速度との比較によって認識する方法等が考えられる。

【0106】

また、移動指令に含まれる情報から運転計画が「原点復帰動作」又は「押付け動作」である場合は、衝突検出機能をオフするようにしてもよい。「原点復帰動作」又は「押付け動作」も衝突に該当するが、意図的な衝突であるため、衝突検出の対象外とする。なお、「押付け動作」とは、障害物ではなく、押付け対象として予め用意されたワークに移動体を押付ける動作を意味する。

【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】

１０ 制御装置
１１ ＣＰＵ
１２ ＲＯＭ
１３ ＲＡＭ
１４ Ｉ／Ｏ
１５ 記憶部
１６ 接続部
２０ アクチュエータ
２１ エンコーダ
２２ モータ
２３ 出力シャフト
２４ カップリング
２５ ボールねじ軸
２６ ボールねじナット
２７ 移動体
２８ ハウジング
２９ 電流センサ
１００ 制御部
１０１ インバータ部

【図1】

【図2】

【図3】

【図4】

【図5】

【図6】

【図7】

【図8】

【図9】

【図10】

【図11】

【図12】

【図13】