JP5636707B2 - 生産機器の制御装置及び生産機器用モータの停止制御方法 - Google Patents

Description

本発明は、例えばロボットのような生産機器に使用されるモータを制御する制御装置，及び生産機器用モータの停止制御方法に関する。

ロボットシステムについては、故障の発生を極力抑止するために十分な考慮を払った上で設計や生産が行われている。しかしながら、経年劣化や、設計時の想定を上回る負荷が加わるなどした結果、一部の部品に故障が発生することが避けられない場合がある。そのような故障の一つとして、ロボット本体各軸のサーボモータに配置されている、例えばロータリエンコーダやレゾルバなどの位置検出器が故障したり、これらの位置検出器とロボットの制御装置との間を接続する通信線が断線するケースが想定される。
このような故障が発生するとサーボモータの制御を正確に行うことが不能になるため、ロボットの動作を緊急停止させる必要がある。従来、ロボットの動作を緊急停止させるには、サーボモータの逆起電力を利用するダイナミックブレーキ（発電制動，短絡制動）や（例えば特許文献１参照）、本来はロボット本体のアーム姿勢を重量に抗して保持するために用いる機械式ブレーキを作動させることが一般的である。

しかしながら、ダイナミックブレーキでは必ずしも十分な制動力が得られず、ロボット本体の動作を完全に停止させるまでに長い時間を要する。また、機械式ブレーキでは、通常の使用状態に比較すると負荷が過度に掛るため劣化を速めてしまい、本来の使用時に制動力が不足する事態を招くおそれがある。緊急停止用として、別途機械式ブレーキを設けることも考えられるが、ロボットの配置スペースが余分に必要になると共にコストアップに直結するため、容易に採用はできない。
例えば特許文献２には、位置センサレス制御を行っているモータが脱調した場合でも、モータを目標位置で停止させることができるように、同期運転制御との連携を適切に行うようにした技術が開示されている。

特開２００１−２０４１８４号公報 国際公開ＷＯ００／００４６３２号公報

すなわち、位置検出器が故障等することでロータの電気角を得ることができなくなった場合は、特許文献２のように位置センサレス制御を行えば電気角を推定することはできる。しかしながら、本来は位置検出器を用いてモータを制御することを前提とした構成であるにもかかわらず、緊急時に対処するためだけに位置センサレス制御も可能となる構成も採用することは、やはり開発コスト等が余分に必要となってしまう。

本発明は上記事情に鑑みてなされたものであり、その目的は、位置検出器に故障が発生した場合でも、簡単な構成で迅速にモータを停止させることができる生産機器の制御装置，及び生産機器用モータの停止制御方法を提供することにある。

請求項１記載の生産機器の制御装置によれば、停止制御手段は、異常判定手段が位置検出器の異常を判定すると以下のようにモータの停止制御を行う。先ず、発電制動を開始させ、その制動期間中に検出されるモータの各相電流の比からモータの現在の電気角を推定する。そして、発電制動を解除すると推定した電気角に応じて逆トルク制動を行う。すなわち、位置検出器に故障等が発生することでモータの電気角を得ることができなくなった場合でも、発電制動中に検出されるモータの各相電流比を参照すれば、現在の電気角を推定することができる。そして、推定した電気角に基づいてモータに逆トルク制動を作用させることが可能となる。

この停止制御は、緊急停止用のブレーキ機構などを別途設けることなく、また、位置センサレス制御のように複雑な演算を行わずとも、簡単な制御で制動力が高い逆トルク制動を作用させ得る。したがって、生産機器の配置スペースを余分に要したり、制御装置の開発コスト等を上昇させることなく、位置検出器の異常発生時に各軸モータを迅速に停止させて生産機器の動作を停止させることができる。

そして、停止制御手段は、発電制動の作用期間中に検出されるモータの各相電流の２乗和とモータの定数及び回転速度に応じて決まる係数との関係から当該回転速度を推定すると、逆トルク制動を行っている期間に、推定した電気角及び回転速度から所定時間経過後の回転回数及び電気角を予測する。ここで、ある時点を基準として永久磁石モータが回転した量を「回転位置」と称し、上記の所定時間経過後について予測した回転回数及び電気角を「次回予測回転位置」と称す。

そして、前記所定時間の経過後に再度発電制動を行うと、当該発電制動期間に推定した電気角に応じて複数の予測回転位置候補を設定し、それら複数の予測回転位置候補のうち、次回予測回転位置に最も近い値のものを予測回転位置として確定する。すなわち、逆トルク制動を行っている期間は回転位置の変化を正確に把握できないので、逆トルク制動を開始した時点での推定電気角及び回転速度から、時間経過に応じた回転回数及び電気角；回転位置を予測する。また、所定時間の経過後に再度発電制動を行った期間に推定した電気角は、電気角１周期以上の誤差を含む可能性がある。

そこで、推定した電気角に応じて複数の予測位置候補を設定し、それら複数の予測位置候補のうち次回予測回転位置に最も近い値のものを予測回転位置として確定すれば回転位置を妥当に決定できる。このように回転位置を決定することで、モータにより駆動される生産機器の最終的な停止位置を予測できるので、各軸間の回転位置関係を補正しながら、例えばロボットのアーム先端が直線上を移動するようにして停止することが可能となる。

請求項２記載の生産機器の制御装置によれば、停止制御手段は、モータが停止するまで発電制動と逆トルク制動とを交互に繰り返し実行するので、逆トルク制動を繰り返し作用させてモータの停止をより早めることができる。

請求項３記載の生産機器の制御装置によれば、停止制御手段は、最初の発電制動を開始させる前に、異常判定手段が異常を判定する直前に得られていた位置検出信号に基づいて逆トルク制動を一定時間行う。そして、逆トルク制動を行っている期間に一定時間経過後の回転回数及び電気角を予測し（初回予測回転位置）、最初の発電制動を開始すると、当該発電制動期間に推定した電気角に応じて複数の予測回転位置候補を設定し、それら複数の予測回転位置候補のうち、前記初回予測回転位置に最も近い値のものを予測回転位置として確定する。

すなわち、異常が判定された時点の直前に得られていた位置検出信号は、そこまでに行われていた制御と連続性を有している妥当な値とみなせるので、発電制動を開始する前に逆トルク制動を行うことで、モータの停止を一層早めることができる。そして請求項２と同様に、逆トルク制動を行っている期間は一定時間経過後の回転位置を予測して、最初の発電制動を開始した場合の回転位置を妥当に決定できる。

請求項４記載の生産機器の制御装置によれば、停止制御手段は、逆トルク制動を行っている期間に予測した電気角と、発電制動期間に推定した電気角との誤差量Ｅｒ（ｎ）を求め、誤差量Ｅｒ（ｎ）が減少した場合は前記所定時間（逆トルク制動作用時間）を長くし、誤差量が増加した場合は前記所定時間を短くする。尚、ｎは周期的に求められる誤差量のある周期を示す（…ｎ−１,ｎ，ｎ＋１，…）。すなわち、逆トルク制動の制動力は、モータの電気角に対する通電電流の位相差に応じて決まるので、誤差量Ｅｒ（ｎ）が大きくなり通電電流の位相差を適切に付与できなくなると制動力が低下する。そこで、誤差量Ｅｒ（ｎ）が増加した場合は逆トルク制動の作用時間を短くして電気角の推定精度を向上させる。またこの場合、逆トルク制動を作用させる回数が増加する場合もあるが、それにより制動力を確実に与えてモータを短い時間で停止させることができる。

請求項５記載の生産機器の制御装置によれば、停止制御手段は、今回決定する所定時間をＴ_RB（ｎ），前回決定した所定時間をＴ_RB（ｎ−１），予め定めた電気角の推定精度をＰｃとすると、所定時間Ｔ_RB（ｎ）を
Ｔ_RB（ｎ）＝Ｔ_RB（ｎ−１）×Ｐｃ／Ｅｒ（ｎ）
で決定する。斯様に決定すれば、逆トルク制動の作用時間Ｔ_RB（ｎ）は、電気角の推定精度Ｐｃと誤差量Ｅｒ（ｎ）との比に応じて、Ｐｃ＜Ｅｒ（ｎ）であれば前回の時間Ｔ_RB（ｎ−１）よりも短く、Ｐｃ＞Ｅｒ（ｎ）であれば前回の時間Ｔ_RB（ｎ−１）よりも長くなるように決定される。したがって、システム上必要とされる推定精度に応じて、作用時間Ｔ_RB（ｎ）を適切に決定できる。

請求項６記載の生産機器の制御装置によれば、停止制御手段は、所定の監視時間内に、誤差量Ｅｒが推定精度Ｐｃを超える比率が所定値以上になると、以降の逆トルク制動を禁止して発電制動のみを行うようにする。すなわち、外乱等の影響により電気角の推定精度が向上しない場合は、誤差量Ｅｒが推定精度Ｐｃを下回らなくなる。そこで、監視時間内に誤差量Ｅｒが必要な推定精度Ｐｃを超える比率が所定値を超えると、以降は発電制動のみを行うようにする。斯様に構成すれば、逆トルク制動を適切に作用させることができない状況下でも、モータを確実に停止させることができる。

第１実施例であり、制動処理の内容を示すフローチャート 電気角に応じたモータの三相電流波形を示す図 各相電流の位相関係を単位円上で示す図 モータの回転数と制動トルクとの関係を示す図 電気角と駆動電流との位相差に応じてサーボモータに発生する回転力と、固定子−回転子間に作用する吸引・反発力とが変化する状態を示す図 図１の処理に対応するタイミングチャート 図６の期間Ｂで行う回転位置Ｒ_Pの決定を説明する図 サーボモータの駆動系及び制御系を示すブロック図 ロボットシステムの構成を示す図 第２実施例を示す図１相当図 第３実施例を示す図１０の一部相当図

（第１実施例）
以下、第１実施例について図１ないし図９を参照して説明する。図９は、ロボットシステムの構成を示す。このロボット本体（産業機器）１は、ベース（回転軸）２上に、この場合６軸のアームを有し、そのアームの先端には、図示しないハンド等のツールが取り付けられる。前記ベース２上には、第１関節Ｊ１を介して第１のアーム３が回転可能に連結されている。この第１のアーム３には、第２関節Ｊ２を介して上方に延びる第２のアーム４の下端部が回転可能に連結され、さらに、この第２のアーム４の先端部には、第３関節Ｊ３を介して第３のアーム５が回転可能に連結されている。

この第３のアーム５の先端には第４関節Ｊ４を介して第４のアーム６が回転可能に連結され、この第４のアーム６の先端には第５関節Ｊ５を介して第５のアーム７が回転可能に連結され、この第５のアーム７には第６関節Ｊ６を介して第６のアーム８が回転可能に連結されている。なお、各関節Ｊ１〜Ｊ６においては、サーボモータ９（１〜６）により（図７参照）各アーム３〜８を回転駆動するようになっている。

制御装置（停止制御手段）１１は、各軸のサーボモータ９（１〜６）に対応して独立したドライバユニット（図８に示すインバータ１６）を内蔵しており、これらのドライバユニットは制御回路１３（図８参照）によって制御される。そして、ロボット１本体と制御装置１１との間は、モータ線１４及びエンコーダ線１５によって接続されている。モータ線１４はインバータ（駆動回路，図８参照）１６の各相出力端子と、サーボモータ９の固定子巻線との間を接続する配線である。

また、エンコーダ線１５は、ロボット本体１の各軸について、サーボモータ９（１〜６）に配置されている位置検出器１７（１〜６）（図８参照）と、制御装置１1とを接続する配線である。位置検出器１７は、例えばロータリエンコーダ等で構成され、サーボモータ９の回転子の絶対位置を例えば光学式により検出し、位置データを通信によって制御装置１１に送信する。

図８は、サーボモータ９の駆動系及び制御系を示すブロック図である。インバータ１６は、例えばパワートランジスタ等の６個のスイッチング素子を三相ブリッジ接続して構成されている。電源回路１８は、６個のダイオードを三相ブリッジ接続して構成される整流回路１９と平滑コンデンサ２０とで構成され、三相交流電源２１を整流・平滑してインバータ１６に直流電源を供給する。

また、図８には、例えばマイクロコンピュータにより構成される制御回路１３がソフトウェアにより実現する各機能をブロック化して示している。減算器２２には、位置指令値と位置検出器１７により検出される回転位置（回転回数＋電気角＝回転量）Ｒ_Pが与えられ、両者の差分（位置偏差）が乗算器２３に出力される。乗算器２３では、上記差分に位置ループ比例ゲインＫｐｐが乗じられ、その乗算結果が速度指令値として減算器２４に出力される。減算器２４には、上記回転位置Ｒ_Pを微分器２５で微分して得られる速度が減算値として与えられる。

減算器２４における減算結果（速度偏差）は、乗算器２６に与えられて速度ループ比例ゲインＫｖｐが乗じられると加算器２７に出力され、また積分器２８に与えられて積分される。積分器２８による積分結果は、乗算器２９に与えられて速度ループ積分ゲインＫｖｉが乗じられると加算器２７に出力される。加算器２７における加算結果はトルク指令値となり、トルク制限器（リミッタ）３２の入力端子に与えられる。トルク制限器３２は、加算器２７より出力されるトルク指令値を上限，下限で制限して座標変換部３３Ｕ，３３Ｖに出力する。座標変換部３３Ｕ，３３Ｖには、サーボモータ９に正弦波状の電流を通電するための波形データが記憶されており、位置検出器１７が検出した機械角θ_M（位置検出信号）が与えられる。

座標変換部３３は、位置検出器１７が検出した機械角θ_M（位置検出信号）から得られる電気角θ_Eに応じて正弦波状の電流波形データを読み出して出力する。そして、トルク制限器３２を介して与えられるトルク指令値は、上記電流波形データに乗じられて振幅値を設定する。尚、座標変換部３３Ｕ，３３Ｖより出力される波形データには、１２０°の位相差が付与されている。

座標変換部３３Ｕ，３３Ｖより出力される波形データは、減算器３６Ｕ，３６Ｖに夫々与えられ、インバータ１６のＵ，Ｖ相出力端子に夫々配置されている電流検出器３７Ｕ，３７Ｖにより検出されるＵ相電流，Ｖ相電流との差がとられる。電流検出器３７Ｕ，３７Ｖは、例えばカレントトランス（ＣＴ）等で構成される。減算器３６Ｕ，３６Ｖより出力される減算結果は、乗算器３８Ｕ，３８Ｖにより電流ループ比例ゲインＫｃｐが乗じられて、ＰＷＭ発生器・プリドライバ３９に出力される。

すなわち、ＰＷＭ発生器・プリドライバ３９には、Ｕ，Ｖ相のＰＷＭ指令値のみが与えられ、ＰＷＭ発生器・プリドライバ３９は、上記ＰＷＭ指令値に基づき内部でＷ相のＰＷＭ指令値を生成する。そしてＰＷＭ発生器・プリドライバ３９は、インバータ１６を構成する各スイッチング素子（例えばパワートランジスタやパワーＭＯＳＦＥＴ，ＩＧＢＴなど）の制御端子（ベース又はゲート）に三相ＰＷＭ信号を出力する。

次に、本実施例の作用について図１乃至図７を参照して説明する。図９に示したように、ロボット本体１と制御装置１１との間はモータ線１４及びエンコーダ線１５により接続されているが、エンコーダ線１５が脱落して、位置検出信号が制御装置１１側に出力されなくなる場合等が想定される。また、位置検出器１７が異常検出機能付きである場合には、位置検出器１７側で異常を検出した結果を、エラー情報として位置検出データに付加して制御装置１１側に送信するようになっている。本実施例は、そのような状態が発生した場合に、サーボモータ９の回転をより短い時間で停止させるように制動を行う。

図１は、制御装置１１による制動処理の内容を示すフローチャートである。尚、この処理は６軸のうち何れか１つの軸について示したものであり、６軸の全てについて同様に処理を行う。制御装置１１は位置検出器１７の出力信号を参照して、位置検出器１７に異常が発生したか否かを判断する（ステップＳ１）。ここでは上述のように、エンコーダ線１５が脱落したことで上記出力信号が入力されなくなったり、或いは出力信号に含まれているエラー情報を取得した場合に「異常」と判断する。位置検出器１７の異常が検出されなければ（ＮＯ）、位置検出器１７の出力信号からサーボモータ９の回転位置及び回転速度Ｒ_Vを検出し（ステップＳ２）、それらに基づきサーボモータ９を制御する（ステップＳ３）。

また、位置検出器１７は、ある時点を基準とするサーボモータ９の回転回数Ｒ_Nについても出力するようになっている。したがって、ステップＳ２で取得する「回転位置」とは、その時点の機械角θ_Mと回転回数Ｒ_Nとから得られる、前回の取得時点からのロータの相対回転位置Ｒ_P（回転量）である。

以上のようにしてサーボモータ９の通常制御を行っている間に、位置検出器１７の異常を検出すると（ステップＳ１：ＹＥＳ）、位置検出器１７からは回転位置Ｒ_Pや回転速度Ｒ_Vに関する情報が得られなくなる。そこで、制御装置１１は、その異常検出の直前にステップＳ２において得られていた回転位置Ｒ_P及び回転速度Ｒ_Vに基づいて、サーボモータ９に予め設定した一定時間だけ逆トルク制動を行う（ステップＳ４）。この場合、サーボモータ９の現在の回転方向に対して作用する逆方向トルクが最大となるように、上記の回転位置（この場合、電気角θ_E）に対して、通電電流位相との位相差を付与する。図５に示すように、例えばサーボモータ９が右回転している場合に通電電流位相差を−９０°とすれば、左回転方向への逆トルク制動力が最大に作用する。

逆トルク制動の作用時間が経過すると、続くステップＳ５において、サーボモータ９にダイナミックブレーキ（発電制動）を作用させる。ダイナミックブレーキは、例えばインバータ１６の上アーム側スイッチング素子を全てＯＦＦ，下アーム側スイッチング素子を全てＯＮにして、サーボモータ９の各相巻線に短絡電流を流すループを形成して行う。尚、必要に応じて、インバータ１６の直流母線間にスイッチ及び回生抵抗素子の直列回路を設け、スイッチを閉じることで回生抵抗素子に電流を消費させても良い。

そして、ダイナミックブレーキを作用させている期間にサーボモータ９の各相電流Ｉｕ，Ｉｖ，Ｉｗを得ると（ステップＳ６）、それらの電流から回転速度Ｒ_Vを演算して推定する（ステップＳ７）。したがって、ステップＳ５でダイナミックブレーキを作用させる時間は、ブレーキを開始した時点から各相電流Ｉｕ，Ｉｖ，Ｉｗが安定するのに十分な時間とするのが望ましい。当該時間はモータの巻線インピーダンス等に応じて決まるが、例えば数ｍ秒程度である。

ここで、ダイナミックブレーキと各相電流Ｉｕ，Ｉｖ，Ｉｗとの関係について説明する。モータの巻線のインダクタンスをＬ，相電流をＩとすると、巻線に通電される電流の変化に応じて発生する逆起電力は（−Ｌ×ｄＩ／ｄｔ）となり、ダイナミックブレーキ中は、特にモータの回転速度が高い領域ではインダクタンスＬにより相電流Ｉが減少するので制動トルクは小さくなる。また、モータの回転速度が低い領域では、インダクタンスＬの影響は小さくなるが、起電力も小さくなるのでやはり制動トルクは小さくなる。

すなわち、ダイナミックブレーキは、限定された回転速度領域では制動力は高いが、低速，高速領域では逆トルク制動に比較して制動力が小さくなる。図４には、モータの回転数［ｒｐｍ］と制動トルク［Ｎ・ｍ］との関係の一例を示しており、９００ｒｐｍ付近で作用する制動トルクがピークとなっている。そして、ダイナミックブレーキを作用させている期間に得られる各相電流Ｉｕ，Ｉｖ，Ｉｗは、次式で表わされる。
Ｉｕ＝｛Ｖｕ−（Ｎ×ｄφｕ／ｄｔ＋Ｌ×ｄＩｕ／ｄｔ）｝／Ｒ …（1.1）
Ｉｖ＝｛Ｖｖ−（Ｎ×ｄφｖ／ｄｔ＋Ｌ×ｄＩｖ／ｄｔ）｝／Ｒ …（1.2）
Ｉｕ＝｛Ｖｗ−（Ｎ×ｄφｗ／ｄｔ＋Ｌ×ｄＩｗ／ｄｔ）｝／Ｒ …（1.3）
但し、Ｖｕ，Ｖｖ，Ｖｗは各相巻線への印加電圧（ダイナミックブレーキ中は０になる），φｕ，φｖ，φｗは各相巻線と鎖交する界磁磁束，Ｎは巻線の巻回数，Ｒはここでは巻線の抵抗及びその他の配線等の外部抵抗である。

また、ダイナミックブレーキ中の各相電流Ｉｕ，Ｉｖ，Ｉｗは、次式のように表わすこともできる。
Ｉｕ＝ｋ×ｓｉｎ（θ_E＋σθ_E） …（2.1）
Ｉｖ＝ｋ×ｓｉｎ（θ_E＋σθ_E＋２π／３） …（2.2）
Ｉｗ＝ｋ×ｓｉｎ（θ_E＋σθ_E＋４π／３） …（2.3）
尚、σθ_Eは、巻線インダクタンスＬにより発生する電流位相ずれであるが、回転速度の関数となるので予め求めることができる。図２は、電気角θ_Eと各相電流Ｉｕ，Ｉｖ，Ｉｗとの関係を示しており（但しσθ_E＝０の場合）、図３は、各相電流の位相関係を単位円上で示したものである。

（2.1）〜（2.3）式からは、（３）式の関係を導き出すことができ、
Ｉｕ²＋Ｉｖ²＋Ｉｗ²＝１．５×ｋ² …（３）
（３）式から、係数ｋを求めることができる。係数ｋは、回転数やモータ及びインバータの特性から決まる値であり、予め係数ｋと回転数［ｒｐｍ］との関係を求めておくことで（一般に線形になる）回転数；回転速度Ｒ_Vを正確に推定できる。そこで、ステップＳ７では（３）式に基づいて回転速度Ｒ_Vを算出する。例えばＲ_V＝α・ｋの関係にあるとすれば（αは比例係数）、回転速度Ｒ_Vは
Ｒ_V＝√｛（Ｉｕ²＋Ｉｖ²＋Ｉｗ²）／１．５｝／α …（４）
で求められる。但し√｛｝は括弧内の平方根である。
続くステップＳ８では（2.1）〜（2.3）式から逆三角関数を用いて（θ_E＋σθ_E）を求める。すなわち、図２に示すように、三相電流の各振幅の比を満たす位相（θ_E＋σθ_E）を一意に決定することができる。更に、続くステップＳ９では、電流位相ずれθ_EをステップＳ７で算出した回転速度Ｒ_Vの関数から求め、最終的に電気角θ_Eを求める。

以上のようにして、回転速度Ｒ_V，電気角θ_Eを推定すると、それらに基づいて、逆トルク制動を作用させた場合のサーボモータ９の回転位置Ｒ_Pを予測して確定し（ステップＳ１０）、その時点でサーボモータ９が停止していなければ（ステップＳ１１：ＮＯ）逆トルク制動を作用させる（ステップＳ１２）。それから、ステップＳ５に戻り、以降はサーボモータ９が停止する（ステップＳ１１：ＹＥＳ）まで、ダイナミックブレーキと逆トルク制動とを交互に作用させる。

ステップＳ１２で行う逆トルク制動は、予め決めることができる異常発生時の停止軌道をトレースしながら行う。すなわち、ステップＳ７〜Ｓ９で推定した回転速度Ｒ_V，電気角θ_Eに基づき、図８に示す制御系において通常の制御と同様に位置フィードバック制御，速度フィードバック制御を行いながら逆トルク制動を作用させる。推定した回転速度Ｒ_Vの変化履歴から制動が作用した結果による減速の度合いも判る。そして、回転速度Ｒ_Vと前記減速の度合いから短い時間が経過した後の電気角θ_Eも推定できる。例えば、時刻ｔから微小時間Δｔが経過した時点の電気角θ_E（ｔ＋Δｔ）は、時刻ｔに推定した電気角θ_E（ｔ）に対して、
θ_E（ｔ＋Δｔ）≒θ_E（ｔ）＋Ｒ_V×Δｔ …（５）
上記（５）式で近似できる。このようにして、時間経過に応じた推定電気角θ_Eも用いながら相電流制御を継続する。

図６は、図１の処理に対応するタイミングチャートを示しており、横軸は時刻Ｔ，縦軸はサーボモータ９の回転位置Ｒ_P（＝回転回数Ｒ_N＋機械角θ_M）である。尚、縦軸の変化は、回転位置Ｒ_Pの増加／減少に関わらず、回転位置Ｒ_Pが予測される最終的な停止位置に向けて変化して行く状態を示している。位置検出器１７の異常が検出されると（Ｓ１：ＹＥＳ）最初の逆トルク制動が行われる（Ｓ４，期間Ａ）。この期間では正確な位置情報が得られないため、常に適切な通電位相差を付与して逆トルク制動が行われるとは限らない。また、逆トルク制動を作用させた結果、予測される回転位置Ｒ_Pにもずれが生じる。

期間Ａが終了すると、ダイナミックブレーキを作用させる期間Ｂとなる（Ｓ５〜Ｓ１０）。ここで、取得された相電流Ｉｕ，Ｉｖ，Ｉｗから回転速度Ｒ_V，電気角θ_Eの推定が行われる。図７は、期間Ｂで行う回転位置Ｒ_Pの決定を説明したものである。図７中に示す「予測位置」は、回転速度Ｒ_Vの変化からこの決定時点に到達していると予測される回転位置Ｒ_P（予測回転位置）である。（５）式で推定された電気角θ_Eは、０ＤＥＧ〜３６０ＤＥＧの間で決まるが、実際のサーボモータ９の回転位置Ｒ_Pは、期間Ａの間にサーボモータ９が何回転しているかに応じて決まる。

したがって、図７中に示す複数の「位置候補（予測位置候補）」は、上記の回転回数を考慮した、電気角０〜３６０度に相当する機械角を単位とする回転位置候補である。そして、「予測位置」に最も近い「位置候補」を、この時点で予測した回転位置Ｒ_Pとして決定する（予測回転位置として確定する）ことで誤差を補正する。次のステップＳ１２での逆トルク制動は（期間Ａ）、期間Ｂで予測した回転位置Ｒ_Pに基づいて制御を開始する。以降、ダイナミックブレーキと逆トルク制動とを交互に繰り返しながらサーボモータ９の回転を停止させる。ここで、ステップＳ４の実行後における期間Ｂで予測した回転位置Ｒ_Pが「初回予測回転位置」に対応し、ステップＳ１２の実行後における期間Ｂで予測した回転位置Ｒ_Pが「次回予測回転位置」に対応する。

尚、以上のプロセスでは、位置検出器１７からの位置情報が得られなくても、サーボモータ９の回転位置Ｒ_Pを妥当に推定しつつ回転を停止させるので、最終的な停止位置がどの辺りになるかも事前に予測できる。そして、図９に示すような６軸のロボットであれば、各軸のサーボモータ６（１〜６）について同様の制御を行うことで各サーボモータ６（１〜６）の停止位置が夫々予測できる。したがって、それらを統括的に制御することで、各軸がばらばらに停止することなく、アームの手先が目標とする停止位置に到達するように連動させて制御することも可能となる。

ここで、電気角θ_Eの推定精度について検討する。
（１）逆トルク制動を作用させる条件
電気角θ_Eの誤差が±９０ＤＥＧを超えると逆トルク制動が作用しなくなり、逆に加速力を作用させる場合があるため、逆トルク制動を作用させるには、誤差を±９０ＤＥＧ未満とする必要がある。
（２）逆トルク制動で、ダイナミックブレーキよりも大きな制動力が得られる条件
図４に示すように、逆トルク制動では、一般的に定格トルクに対して３倍程度の制動力が得られる。一方、ダイナミックブレーキで得られる制動力は、最大でも定格トルクの２倍程度であり、高速領域ではより小さくなる。したがって、逆トルク制動における最大制動力の２／３程度であれば、ダイナミックブレーキよりも大きな制動力を得ることができる。この場合に必要な位相精度は±４２ＤＥＧ以下である。
（３）回転位置Ｒ_Pの推定に必要な精度
これについては、図７に示したように、予測位置が何れかの位置候補に近くなる程度の精度であれば良いから、±１８０ＤＥＧ以下である。以上の３つの条件を考慮すると、結果として必要な推定精度は±４２ＤＥＧ以下となる。

以上のように本実施例によれば、制御装置１１は、位置検出器１７の異常を判定すると、先ず、ダイナミックブレーキを作用させ、その制動期間中に検出されるサーボモータ９の各相電流の値から、具体的には各相電流Ｉｕ，Ｉｖ，Ｉｗの比からサーボモータ９の現在の電気角θ_Eを推定する。そして、ダイナミックブレーキを解除すると、推定した電気角θ_Eに応じて逆トルク制動を行う。すなわち、位置検出器１７に故障等が発生して電気角θ_Eを得られなくなった場合でも、ダイナミックブレーキの作用中に検出される各相の電流値から現在の電気角θ_Eを推定すれば、その電気角θ_Eに応じてサーボモータ９に逆トルク制動を作用させることができる。

上記の位置推定停止制御は、緊急停止用のブレーキ機構などを別途設けることなく、また、位置センサレス制御のように複雑な演算を行わずとも、簡単な制御で制動力が高い逆トルク制動を作用させ得る。したがって、ロボットシステムの配置スペースを余分に要したり、制御装置１１の開発コスト等を上昇させることなく、位置検出器１７の異常発生時に各軸のサーボモータ９を迅速に停止させて、ロボット本体１の動作を停止させることができる。

また、制御装置１１は、各相電流Ｉｕ，Ｉｖ，Ｉｗの２乗和とサーボモータ９の定数及び回転速度Ｒ_Vに応じて決まる係数ｋとの関係から回転速度Ｒ_Vを推定すると、逆トルク制動を行っている期間に、推定した電気角θ_E及び回転速度Ｒ_Vから所定時間経過後の回転位置を予測する。そして、所定時間の経過後に再度ダイナミックブレーキを行うと、そのダイナミックブレーキの作用期間に推定した電気角に応じて複数の予測位置候補を設定し、それら複数の予測位置候補のうち、予測した回転位置に最も近い値のものを予測回転位置Ｒ_Pとして確定する。したがって、回転位置Ｒ_Pを妥当に決定することができ、サーボモータ９により駆動されるロボット本体１の手先について最終的な停止位置を予測することが可能となる。例えば、各軸間の回転位置関係を補正しながら、手先が直線上を移動するようにして停止させることができる。

また、制御装置１１は、サーボモータ９が停止するまでダイナミックブレーキと逆トルク制動とを交互に繰り返し実行するので、逆トルク制動を繰り返し作用させてサーボモータ９の停止をより早めることができる。更に、制御装置１１は、最初のダイナミックブレーキを開始させる前に、位置検出器１７について異常を判定する直前に得られていた電気角θ_Eに基づいて逆トルク制動を一定時間行い、その逆トルク制動期間についても上記と同様にして予測回転位置Ｒ_Pを確定する。

すなわち、異常が判定された時点の直前に位置検出器１７より得られていた電気角θ_Eは、その時点までに行っていた制御と連続性を有している妥当な値とみなせるので、ダイナミックブレーキを開始する前に逆トルク制動を行うことで、サーボモータ９の停止を一層早めることができる。そして逆トルク制動を行っている期間は一定時間経過後の回転位置Ｒ_Pを予測・確定して、最初のダイナミックブレーキを開始した場合の回転位置Ｒ_Pを妥当に決定できる。また、逆トルク制動を行っている期間は、推定した電気角θ_E、回転速度Ｒ_Vから、経過時間におけるサーボモータ９の電気角θ_Eを常に求めながら制動をかける。

（第２実施例）
図１０は本発明の第２実施例であり、第１実施例と同一部分には同一符号を付して説明を省略し、以下異なる部分について説明する。第２実施例の構成は第１実施例と同様であり、制御装置１１により行われる制御内容が若干相違している。すなわち、ステップＳ１１で「ＮＯ」と判断すると、ステップＳ１３〜Ｓ１６を実行するようになっている。

ステップＳ１３では、ステップＳ８で算出した電気角θ_EBと、ステップＳ４，若しくは前回実行した（ステップＳ１２に替わる）ステップＳ１６で逆トルク制動を作用させた期間に推定した電気角θ_EAとの誤差Ｅｒ（ｎ）を算出する。すなわち、
Ｅｒ（ｎ）＝θ_EB（Ｂ）−θ_EA …（６）
とする。そして、その誤差に基づき、ステップＳ１５で逆トルク制動を作用させる時間（所定時間）を算出する（ステップＳ１４）。ここでの作用時間Ｔ_RBの算出は、例えば以下のようにして行う。今回求める作用時間をＴ_RB（ｎ），前回の作用時間をＴ_RB（ｎ−１），第１実施例で検討した電気角θ_Eの必要推定精度をＰｃとすると、
Ｔ_RB（ｎ）＝Ｔ_RB（ｎ−１）×Ｐｃ／｛Ｅｒ（ｎ）＋β｝ …（７）
で作用時間Ｔ_RB（ｎ）（期間Ａの長さに相当）を求める。尚、βは余裕度であり、実際の制御・駆動系に応じてあらかじめ決定しておく。前回の作用時間Ｔ_RB（ｎ−１）についても初期値を設定しておく。作用時間Ｔ_RB（ｎ）を求めると、次回の演算用に、当該時間Ｔ_RB（ｎ）を前回値時間Ｔ_RB（ｎ−１）として記憶する（ステップＳ１５）。

また、ここでは、ｎを周期的に求められる誤差量Ｅｒや作用時間Ｔ_RBのある周期を示す意味で用いている（…ｎ−１,ｎ，ｎ＋１，…）。すなわち、（ｎ）を今回の制御周期（或いは演算周期）で求めた値とすれば、（ｎ−１）は、その前回の制御周期で求められた値を示す。

（７）式によれば、逆トルク制動の作用時間Ｔ_RB（ｎ）は、推定精度Ｐｃと誤差量Ｅｒ（ｎ）に余裕度βを加えた値との比に応じて、Ｐｃ＜｛Ｅｒ（ｎ）＋β｝であれば前回の時間Ｔ_RB（ｎ−１）よりも短く、Ｐｃ＞｛Ｅｒ（ｎ）＋β｝であれば前回の時間Ｔ_RB（ｎ−１）よりも長くなるように決定される。これにより、電気角θ_Eの推定精度を向上させる。そしてこの場合、逆トルク制動を作用させる回数は増加することになるが、制動力を確実に与えてサーボモータ９を短時間で停止させることができる。すなわち、上記の作用は作用時間Ｔ_RB（ｎ）の最適値を求めるための学習機能を実現している。

以上のように第２実施例によれば、制御装置１１は、逆トルク制動期間Ａに予測した電気角θ_EAと、ダイナミックブレーキ期間Ｂに推定した電気角θ_EBとの誤差量Ｅｒ（ｎ）を求め、誤差量Ｅｒが減少した場合は作用時間Ｔ_RB（ｎ）を長くし、誤差量Ｅｒ（ｎ）が増加した場合は作用時間Ｔ_RB（ｎ）を短くする。具体的には、（７）式により作用時間Ｔ_RB（ｎ）を決定する。したがって、システム上必要とされる推定精度Ｐｃを基準とする誤差量Ｅｒ（ｎ）の増減に応じて作用時間Ｔ_RB（ｎ）を調整し、サーボモータ９に制動力を確実に与えることができる。

（第３実施例）
図１１は第３実施例であり、第２実施例と異なる部分について説明する。第３実施例では、第２実施例のように作用時間Ｔ_RB（ｎ）の学習機能を実現する場合に、電気角θ_Eの推定精度が向上しない状況にあると推定される場合は、逆トルク制動を中止するように制御する。図１１は、図１０の一部相当図であり、ステップＳ１１で「ＮＯ」と判断すると、後述するステップＳ２７でセットされるＲＢストップフラグがセットされているか否かを判断する（ステップＳ２１）。初期状態（例えば、ステップＳ１：ＹＥＳの時点）ではＲＢストップフラグがクリアされているので（ＮＯ）次のステップＳ２２に移行する。

ステップＳ２２では、誤差量Ｅｒが推定精度Ｐｃを超えているか否かを判断し、Ｅｒ＞Ｐｃでなければ（ＮＯ）ステップＳ２３に移行し、Ｅｒ＞Ｐｃであれば（ＹＥＳ）ステップＳ２４に移行する。ステップＳ２３ではＥｒカウンタをデクリメントし（但しカウンタ値がゼロの場合はゼロを維持する）、ステップＳ２４ではＥｒカウンタをインクリメントする。ステップＳ２３又はＳ２４の実行後はステップＳ２５に移行する。
ステップＳ２５では、例えばステップＳ４の実行後に計時動作を開始させた監視タイマが、予め定めた監視時間相当する時間をカウントアップしたか否かを判断し、カウントアップしていなければ（ＮＯ）ステップＳ１４に移行し、カウントアップした場合は（ＹＥＳ）ステップＳ２６に移行する。ステップＳ２６では、Ｅｒカウンタの値が所定値以上となったか否かを判断する。

すなわち、ステップＳ２６における判断は、ステップＳ５〜Ｓ１６のループを回る間の
監視時間内において、誤差量Ｅｒが推定精度Ｐｃを超えた比率が所定値以上か否かを判断することに対応する。ステップＳ１３で求められる誤差量Ｅｒが推定精度Ｐｃ以下となる比率が高い場合は、Ｅｒカウンタ値はゼロ若しくはゼロ付近の値になり、ステップＳ１６で作用させる逆トルク制動の制動力を十分に得ることができると判断される。したがって、「ＮＯ」と判断するとＥｒカウンタをクリアする（ステップＳ２８）。

一方、例えば外乱等の影響を受けることで誤差量Ｅｒが推定精度Ｐｃを超える比率が高い場合は、Ｅｒカウンタ値はより大きな値を示すようになり、逆トルク制動の制動力を十分に得ることができない状態にあると推定される。したがって、ステップＳ２６で「ＹＥＳ」と判断すると、ＲＢストップフラグをセットして（ステップＳ２７）ステップＳ５に移行する。ＲＢストップフラグがセットされると、ステップＳ２１では「ＹＥＳ」と判断されるのでステップＳ５に移行する。これにより、以降は逆トルク制動を作用させず、サーボモータ９の回転が停止するまで（ステップＳ１１：ＹＥＳ）ダイナミックブレーキのみを行うようにする。

以上のように第３実施例によれば、制御装置１１は、所定の監視時間内に、誤差量Ｅｒが推定精度Ｐｃを超える比率が所定値以上になると、以降の逆トルク制動を禁止してダイナミックブレーキのみを行うようにする。したがって、逆トルク制動を適切に作用させることができない状況下でも、モータを確実に停止させることができる。

本発明は上記し又は図面に記載した実施例にのみ限定されるものではなく、以下のような変形又は拡張が可能である。
ステップＳ４を削除して、最初にダイナミックブレーキを作用させても良い。
また、ダイナミックブレーキと逆トルク制動とをそれぞれ１回ずつ行っても良い。
推定したサーボモータ９の回転速度に応じて、逆トルク制動の作用力を調整しても良い。例えば、回転速度が閾値以上であれば制動力を最大にするが、回転速度が閾値未満であれば、その回転速度に応じて制動力を弱めるようにしても良い。
必要とする電気角θ_Eの推定精度については、±４２ＤＥＧより小さい値に設定しても良い。
第２，第３実施例において、（７）式の余裕度βについては必要に応じて設定すれば良い。

また、作用時間Ｔ_RB（ｎ）の調整は（７）式に基づくものに限らず、例えば前回値Ｔ_RB（ｎ）に一定値を加減算したり、一定比率の係数を乗じるなどして増減させても良い。
位置検出器としては、絶対的な回転回数の情報を出力するものや、回転回数の情報を出力せず、例えばＡ・Ｂ相の二相パルスだけを出力するものを用いても良い。後者の場合は、出力されるパルスを積算することで相対的な回転回数を求めても良い。
また、位置検出器には、その他例えばレゾルバ等を用いても良い。
第３実施例において監視タイマの計時を開始する時点は、ステップＳ４の実行後に限らない。例えば最初にステップＳ１１やＳ２１で「ＮＯ」と判断した時点から計時を開始しても良い。
産業機器はロボットに限ることなく、その他工作機械などにも適用することが可能である。

図面中、１はロボット本体（産業機器）、９はサーボモータ（永久磁石型同期モータ）、１１は制御装置（停止制御手段）、１７は位置検出器（位置検出手段）を示す。

Claims (12)

1. 生産機器に使用されるモータを制御する制御装置において、
   前記モータの巻線に通電される電流を検出する電流検出手段と、
   前記モータに配置されている位置検出器より出力される位置検出信号から、若しくは前記位置検出信号の出力状態から前記位置検出器の異常を判定する異常判定手段と、
   この異常判定手段が前記異常を判定した場合に、前記モータの停止制御を行う停止制御手段とを備え、
   前記停止制御手段は、
   発電制動を開始させると、その制動期間中に検出される前記モータの各相電流の比から前記モータの現在の電気角を推定し、
   前記発電制動を解除すると、前記推定した電気角に応じて、前記モータの回転方向とは逆方向にトルクを発生させる逆トルク制動を行い、
   前記発電制動を開始させると、前記各相電流の２乗和と前記モータの定数及び回転速度に応じて決まる係数との関係から前記モータの回転速度を推定し、
   前記逆トルク制動を行っている期間に、前記推定した電気角及び前記推定した回転速度から所定時間経過後の回転回数及び電気角を予測し（次回予測回転位置と称す）、
   前記所定時間の経過後に再度発電制動を行うと、当該発電制動期間に推定した電気角に応じて複数の予測位置候補を設定し、それら複数の予測位置候補のうち、前記次回予測回転位置に最も近い値のものを予測回転位置として確定し、
   前記発電制動を解除すると再度逆トルク制動を行うことを特徴とする生産機器の制御装置。
2. 前記停止制御手段は、前記モータが停止するまで、発電制動と逆トルク制動とを交互に繰り返し実行することを特徴とする請求項１記載の生産機器の制御装置。
3. 前記停止制御手段は、
   最初の発電制動を開始させる前に、前記異常判定手段が前記異常を判定する直前に、前記位置検出器より得られていた位置検出信号に基づいて逆トルク制動を一定時間行い、
   前記逆トルク制動を行っている期間に、前記一定時間経過後の回転回数及び電気角を予測し（初回予測回転位置と称す）、
   前記最初の発電制動を開始すると、当該発電制動期間に推定した電気角に応じて複数の予測位置候補を設定し、それら複数の予測位置候補のうち、前記初回予測回転位置に最も近い値のものを予測回転位置として確定することを特徴とする請求項１又は２記載の生産機器の制御装置。
4. 前記停止制御手段は、逆トルク制動期間に予測した電気角と、発電制動期間に推定した電気角との誤差量Ｅｒ（ｎ）を求め、前記誤差量Ｅｒ（ｎ）が減少した場合は前記所定時間を長くし、前記誤差量Ｅｒ（ｎ）が増加した場合は前記所定時間を短くすることを特徴とする請求項１から３の何れかに一項に記載の生産機器の制御装置。
5. 前記停止制御手段は、今回決定する所定時間をＴ _RB （ｎ），前回決定した所定時間をＴ _RB （ｎ−１），予め定めた電気角の推定精度をＰｃとすると、所定時間Ｔ _RB （ｎ）を
   Ｔ _RB （ｎ）＝Ｔ _RB （ｎ−１）×Ｐｃ／Ｅｒ（ｎ）
   で決定することを特徴とする請求項４記載の生産機器の制御装置。
6. 前記停止制御手段は、所定の監視時間内に、前記誤差量Ｅｒが前記推定精度Ｐｃを超える比率が所定値以上になると、以降の逆トルク制動を禁止して発電制動のみを行うことを特徴とする請求項５記載の生産機器の制御装置。
7. 生産機器に使用されるモータを停止制御する方法において、
   前記モータの巻線に通電される電流を検出し、
   前記モータに配置されている位置検出器より出力される位置検出信号から、若しくは前記位置検出信号の出力状態から前記位置検出器の異常を判定すると、
   前記モータの発電制動を開始させ、その制動期間中に検出される前記モータの各相電流の比から前記モータの現在の電気角を推定し、前記各相電流の２乗和と前記モータの定数及び回転速度に応じて決まる係数との関係から前記モータの回転速度を推定し、
   前記発電制動を解除すると、前記推定した電気角に応じて、前記モータの回転方向とは逆方向にトルクを発生させる逆トルク制動を行い、
   前記逆トルク制動を行っている期間に、前記推定した電気角及び前記推定した回転速度から所定時間経過後の回転回数及び電気角を予測し（次回予測回転位置と称す）、
   前記所定時間の経過後に再度発電制動を行うと、当該発電制動期間に推定した電気角に応じて複数の予測位置候補を設定し、それら複数の予測位置候補のうち、前記次回予測回転位置に最も近い値のものを予測回転位置として確定し、
   前記発電制動を解除すると再度逆トルク制動を行うことを特徴とする生産機器用モータの停止制御方法。
8. 前記モータが停止するまで、発電制動と逆トルク制動とを交互に繰り返し実行することを特徴とする請求項７記載の生産機器用モータの停止制御方法。
9. 最初の発電制動を開始させる前に、前記異常を判定する直前に、前記位置検出器より得られていた位置検出信号に基づいて逆トルク制動を一定時間行い、
   前記逆トルク制動を行っている期間に、前記一定時間経過後の回転回数及び電気角を予測し（初回予測回転位置と称す）、
   前記最初の発電制動を開始すると、当該発電制動期間に推定した電気角に応じて複数の予測位置候補を設定し、それら複数の予測位置候補のうち、前記初回予測回転位置に最も近い値のものを予測回転位置として確定することを特徴とする請求項７又は８記載の生産機器用モータの停止制御方法。
10. 逆トルク制動期間に予測した電気角と、発電制動期間に推定した電気角との誤差量Ｅｒ（ｎ）を求め、前記誤差量Ｅｒ（ｎ）が減少した場合は前記所定時間を長くし、前記誤差量Ｅｒ（ｎ）が増加した場合は前記所定時間を短くすることを特徴とする請求項７から９の何れか一項に記載の生産機器用モータの停止制御方法。
11. 今回決定する所定時間をＴ _RB （ｎ），前回決定した所定時間をＴ _RB （ｎ−１），予め定めた電気角の推定精度をＰｃとすると、所定時間Ｔ _RB （ｎ）を
    Ｔ _RB （ｎ）＝Ｔ _RB （ｎ−１）×Ｐｃ／Ｅｒ（ｎ）
    で決定することを特徴とする請求項１０記載の生産機器用モータの停止制御方法。
12. 所定の監視時間内に、前記誤差量Ｅｒが前記推定精度Ｐｃを超える比率が所定値以上になると、以降の逆トルク制動を禁止して発電制動のみを行うことを特徴とする請求項１１記載の生産機器用モータの停止制御方法。

Images