(57)【要約】【課題】  サーボモータを用いたフィードバック制御において、サーボモータの回転量をエンコーダなどにより検出し速度信号等を得る場合、検出量は分解能の限界とソフトウェアを用いた周期的な演算によりリップルが発生し、フィルタなどリップルの平滑化要素を用いると時間遅れが生じ、制御的に安定性を損なうなどの問題があった。これを解決するフィードバック制御装置を提供する。【解決手段】  運動指令１１０を入力とし、制御器１０１をシミュレートする制御器モデル演算部１０５および機械装置をシミュレートする機械モデル演算部１０６からなるシミュレーション演算部１０４と、この演算部１０４の演算結果を用いてセンサによる検出信号を修正し、制御器へフィードバックする修正演算部１０８とを備え、修正演算部１０８ではシミュレーション演算とセンサ信号の両者を用いてリップルが少なく時間遅れがない修正センサ信号を得る。【選択図】  図１
2005316937.tif 000002
【特許請求の範囲】
【請求項１】
運動指令に従って制御器が機械装置を駆動し、前記機械装置の物理的状態量をセンサによって検出し、前記制御器へフィードバックする制御装置において、
前記運動指令を入力とし、前記制御器をシミュレートする制御器モデル演算部および前記機械装置をシミュレートする機械モデル演算部からなるシミュレーション演算部と、
前記シミュレーション演算部の演算結果を用いて前記センサによる検出信号を修正し、前記制御器へフィードバックする修正演算部とを備えることを特徴とする制御装置。
【請求項２】
前記修正演算部は、前記センサによる検出信号または前記シミュレーション演算部の演算結果に対し座標変換を行なう座標変換部を備えることを特徴とする請求項１記載の制御装置。
【請求項３】
前記センサは、加速度センサまたはトルクセンサまたは位置センサであることを特徴とする請求項１または２記載の制御装置。
【請求項４】
前記修正演算部は、前記センサによる検出信号と前記シミュレーション演算部の演算結果に各々重み付け係数を乗じることを特徴とする請求項１記載の制御装置。
【請求項５】
前記修正演算部は、前記シミュレーション演算部の演算結果の変化率に基づき前記センサによる検出信号を修正することを特徴とする請求項１記載の制御装置。
【請求項６】
前記修正演算部は、前記センサによる検出信号と前記シミュレーション演算部の演算結果を比較し、その乖離度により前記重み付け係数を変化させる乖離度評価部を備えることを特徴とする請求項４または５記載の制御装置
【請求項７】
前記シミュレーション演算部は、演算結果の振幅を調整する振幅調整手段と時間遅れ・進みを調整する時間遅れ・進み調整手段とを備えることを特徴とする請求項４または５記載の制御装置。
【請求項８】
前記センサによる検出信号および前記シミュレーション演算部の演算結果を提示する提示手段と、前記機械モデル演算部の演算モデルの調整を行なう調整手段とを備えることを特徴とする請求項１〜７のいずれか１項記載の制御装置。
【請求項９】
前記提示手段は、前記センサによる検出信号と前記シミュレーション演算部の演算結果との差分または前記差分の時間積分値を提示することを特徴とする請求項８記載の制御装置。
【請求項１０】
前記シミュレーション演算部の演算結果を量子化する手段と、前記量子化した結果と前記センサによる検出信号を比較表示する比較表示手段とを備えることを特徴とする請求項９記載の制御装置。
【請求項１１】
前記運動指令と前記センサによる検出信号を時系列で保存し、オフラインにて前記シミュレーション演算部のパラメータを調整するパラメータ調整手段を備えることを特徴とする請求項９または１０記載の制御装置。
【請求項１２】
運動指令に従って制御器が機械装置を駆動し、前記機械装置の物理的状態量をセンサによって検出し、前記制御器へフィードバックする制御方法において、
前記運動指令に対する前記制御器および前記機械装置の動作を演算によりシミュレートし、前記シミュレートした演算結果を用いて前記センサの検出信号を修正し、前記制御器へフィードバックすることを特徴とする制御方法。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、サーボモータや、サーボモータにより駆動される機械に、その物理的状態量を計測するセンサを付加し、センサからの情報を用いフィードバック制御を行う制御装置および制御方法に関する。
【背景技術】
【０００２】
＜背景技術の１＞
従来、サーボモータで駆動される機械の振動を加速度センサにより検出し制御装置にフィードバックすることで振動を抑制する制御方法があった。
例えば図２２のように、リニアスライダ２２１５の加速度を加速度センサ２２１３で検出し制御アンプ部２２１０にフィードバックすることで振動を抑制する制御方法があった（例えば、特許文献１参照）。
また、複数のサーボモータが協調してアーム手先を制御するロボットのような機械はオープンリンクの構成を取ることが多く、そのため振動が発生しやすい。その振動をロボットアーム上に配置された加速度センサにより検出しフィードバックすることで振動を抑制する制御方法があった（例えば、特許文献２参照）。
また、センサにより検出された加速度信号を制御系にフィードバックすることで振動を抑制する場合に、加速度信号をフィルタリングして振動抑制に必要な周波数帯の信号を抽出し、その信号をフィードバック演算に利用することで振動を抑制する方法もあった（例えば、特許文献３参照）。
一方、加速度フィードバックは振動抑制だけでなく別の目的にも用いられる。例えば、ロボットのインピーダンス制御と呼ばれる制御方式がある（例えば、非特許文献１参照）。この制御方式はロボットに外部から作用する力に対し、予め設定したメカニカルな目標インピーダンスの特性で応答させるものである。インピーダンス制御を実現するための検出手段としては、力センサを用いる方法と加速度センサを用いる方法がある。加速度信号の検出値をフィードバックすることは目標インピーダンス（慣性、粘性、剛性）のうち外力に対する見かけ上の慣性（質量またはイナーシャ）を調整することを意味している。
また、加速度センサは、ロボットなどの機械と外界との衝突のような異常検出を目的として用いられることもある。ロボットアームなどの機械が周辺にあるワークや物体に接触または衝突した場合、装置に運動の変化が発生する。その時の加速度を検出し非常停止などを行うものである（例えば、特許文献４参照）。
【０００３】
以上、加速度センサの使用例を説明したが、トルクセンサも同様な目的で用いられている。トルクセンサを用いた公知例としてはフィードバック制御で振動抑制のために用いた例（例えば、特許文献５参照）や、図２３に示すようにロボットアームの衝突検出に利用した例がある（例えば、特許文献６参照）。
特許文献５ではトルクセンシングループを従来の位置制御、速度制御の内側の内部ループとして構成している。
特許文献５ではトルクセンサの検出値と外乱オブザーバの検出値とを用いることでモータ側と負荷側のトルク差を導出し、主に減速機に由来する振動成分の抑制を目的としている。
また、特許文献６ではトルクセンサ２３０３の信号を用いてロボットアーム２３０１の衝突を検出している。またロボットアーム２３０１に作用する外力やロボットアーム２３０１が自ら外界に接触した衝撃をトルクセンサ２３０３により検出する。
【０００４】
＜背景技術の２＞
サーボモータのセンシングに用いられるエンコーダなどの回転量センサは位置や速度のフィードバック信号源として用いられている。代表的な検出方式として光学式、磁気式があり、その検出や処理の限界があるため両者とも分解能に限界がある。前者の光学式ではガラス基板のスリットの細かさや検出部感度に、磁気式では検出波形のリニアリティや再現性に限界がある。また、処理系でもデジタル処理のビット数の限界がある。制御装置の観点からは現在主流のＣＰＵを用いたデジタル処理方式では、応答性を上げるためサイクルタイムは短時間になり、その影響で１つのサンプリングタイム中のパルス数は減少してしまう。パルス数が減少してしまうと速度の演算などでは差分演算を主体としているためリップルの多い波形になってしまう。さらに制御系ではサーボモータの応答性を上げるためフィードバック制御系では可能な限りゲインは高く保たれ、その結果トルク指令など最終的にアンプに指令するための制御量は非常にリップルの多いものになってしまう。このことは発生トルクの脈動や、振動、発熱などにつながる。これらリップルの悪影響を低減する目的でローパスフィルタが良く用いられるが、制御全体として即応性や制御的安定性を犠牲にすることになる。特に速度の低い領域では特にサンプリングパルス数が少なくなりリップルの影響は相対的に大きくなるため、パルス分解能の影響は顕著になる。
【０００５】
これらエンコーダの分解能の問題を回避するために各種の方策が提案されている。
一般的に良く用いられる方法として、先に述べたローパスフィルタなどの低域通過フィルタである。速度信号の部分や最終的なトルク指令の部分に挿入されることが多い。
その他の方法としては、サーボモータや負荷のイナーシャなど既知の情報を用いて速度を演算により推定する、いわゆるオブザーバを構成し利用する方式がある（特許文献７）。
また、複数のオブザーバを組み合わせて利用する方法がある（特許文献８）。
また、フィルタやオブザーバの遅れの問題点を改善し、特に低速回転域での特性向上を図る方法として
１つは現在および過去の複数のエンコーダデータから未来予測を含めた推定値を求める位置や速度の信号の内挿方法がある（特許文献９）。
さらに、他の方法として演算時間中にパルスが少なくなる低速付近の特性向上をはかった補償方法がある（特許文献１０）。
また、制御対象のモデルを用いて必要な制御量を予め求め、フィードフォワード的制御量としてフィードバック制御系の前向き部分に加算する制御手法がある（特許文献１１）。
減速機などのギアの入出力部に検出器を取り付け、２種の信号をフィードバックし適当な重み付けを行うことで制御系を安定化するという方法がある（特許文献１２）。
【０００６】
【特許文献１】特開平６−９１４８２号公報
【特許文献２】特開平２−３３１６９号公報
【特許文献３】特開平７−４０２６８号公報
【特許文献４】特開平６−３４９９３０号公報
【特許文献５】特開平５−２５２７７９号公報
【特許文献６】特開平５−２０８３９４号公報
【特許文献７】特開平６−２６２５５９号公報（第３−５頁  １図）
【特許文献８】特許２６５８９７６号公報（第３−５頁  １図）
【特許文献９】特許第３３６７２６０号公報（第６頁  １図）
【特許文献１０】特許第２７７８３４１号公報（第５−６頁  １図）
【特許文献１１】特許第３０８４９２８号（第４−５  １図）
【特許文献１２】特開平１１−８９２６６号公報（第３頁  １図）
【非特許文献１】舘、榊、荒井他："力センサを用いないダイレクト・ドライブ・マニピュレータのインピーダンス制御"，日本ロボット学会誌，Vol．7、No．3，pp.60-72，1989
【発明の開示】
【発明が解決しようとする課題】
【０００７】
ところが、背景技術の１において、特許文献１や特許文献２のようにセンサで加速度を検出し、その信号を直接フィードバックして制御する方法では、検出信号に機械の振動成分と機械の運動指令成分による運動を合わせた情報として検出しフィードバックしているため、振動抑制効果が十分ではなかった。
また、振動成分を打ち消すようにフィードバックするためには、制御装置で位相を調整する必要があるが、機械の状態やロボットの姿勢によって振動抑制の効果が変化し、場合によっては逆に振幅が増大することがあった。
また特許文献３の方法では振動抑制に必要な周波数成分のみをフィルタリングで抽出しフィードバックしているが、機械の運動指令に対する応答も減衰対象の周波数成分に含まれているため、振動の抑制制御を行うことで機械の運動指令に対する応答性が損なわれることがあった。
非特許文献１の、加速度信号を用いてインピーダンス制御されるロボットでは外からの力を受けた時の受動的なインピーダンス特性を制御すると同時に、ロボットが自ら発生する運動指令によりロボットを動作させる必要がある。外力に対しては柔らかく、ロボットの運動指令に対しては追従性を高くするなどそれぞれの特性を分けて制御する場合が多い。その場合ロボットの制御系には外力およびロボットの運動指令の併せて２つの入力系統を有することになるが、加速度検出器の出力だけでは、検出される加速度成分がその２種類の制御系のどの入力によるものか区別することができない。この時、例えば既知の情報である運動指令から加速度を求め運動指令を補償するとしても、ロボットの実際の応答には時間遅れがあるため、正確なロボットアームの加速度信号を得ることはできなかった。
特許文献４のロボットと外界との衝突による加速度検出に関しても同様のことが言える。ロボットの衝突検出では自らの運動指令による加速度と外力により生じる加速度を区別する必要があるが、周波数成分に基づくフィルタリングだけで原因を特定することは困難で、誤検出の可能性が高かった。
また他の問題として、原因を特定するためには一定時間データをサンプリングする必要があり、そのための時間は衝突に対処するまでのデッドタイムとなる。衝突への対処遅れは損害を致命的な状態に至らしめる。そのため、衝突を一刻も早く検出し対処する必要がある。この検出遅れの問題も大きな課題となっていた。
【０００８】
次にトルクセンサの場合に関して説明する。特許文献５ではトルクセンシングループを従来の位置制御、速度制御の内側の内部ループとして構成している。
また、特許文献６ではロボットアームの衝突をトルクセンサの信号を用いて検出している。
特許文献５ではトルクセンサの検出値と外乱オブザーバの検出値とを用いることでモータ側と負荷側のトルク差を導出し、主に減速機に由来する振動成分抑制を目的としている。
また、特許文献６ではロボットアームに作用する外力またはロボットが自ら外界に接触した衝撃をトルクセンサにより検出する。
どちらの場合もトルクセンサでの検出値はモータが発生したトルクか、外力により検出されたトルクかを区別する手段を持たないため、外乱要素を含む検出トルクをフィードバック制御系に用いた場合には逆に安定性を損ねてしまったり、衝突の検出にトルクセンサの検出値をそのまま用いると、モータの発生するアームの加速トルクを誤検出してしまったりするといった問題を有していた。
【０００９】
本発明はこのような問題点に鑑みてなされたものであり、シミュレーション演算部にて行なった演算結果を利用して、センサによる検出信号から、振動抑制などの目的に関する要素のみを抽出し遅れなくかつ安定してフィードバックすることができる制御装置および制御方法を提供することを第１の目的としている。
【００１０】
また、背景技術の２においては、従来から良く利用されているローパスフィルタを用いる方法ではフィルタにより遅れが発生し、その遅れにより制御応答特性が上げられず、制御的な安定性が損なわれるという問題があった。
特許文献７の速度オブザーバを用いた方式では、リップルを低減する効果はあるが、状態推定器である以上、一定の時定数をもって推定するため、遅れを持ちながら推定値を算出する。そのためフィルタと同様に遅れが生じることには変わりはなく、制御系全体の安定性を保つという点では課題があった。
また、特許文献８の多重にオブザーバを構成した例では、長周期と短周期、複数の時定数のオブザーバを組み合わせることで、速度オブザーバのリップル低減効果と制御特性の安定性の劣化を適度なバランスで実現することを目的としているが、オブザーバを用いることで遅れ要素を含むという点での本質的な課題が変わるものではない。
【００１１】
一方、オブザーバとは別の観点からの信号処理により、速度検出特性を改善する方策がある。特許文献９では過去の位置検出の情報変化から予測的に検出値を求めようとするもので、リップルを低減する効果がある。ところが低速時にはパルス間隔のばらつきによる誤差の影響を受けやすい、また情報処理量の増加と高速処理、高速通信など処理系、伝送系の負荷が増加しシステムが複雑になってしまうという問題があった。
特許文献１０の方法では極低速時にパルス間の時間（サンプリング時間のカウント数）を計測することによりリップルの低減を図っている。ところが、定常状態が継続している状態では有効に作用するが、未来の状態を何らかの情報を手がかりに求めているわけではないので、外乱や急な速度変動に対して十分な対応できないという問題があった。また、信号の連続性が保てなくなる部分が生じ、その部分での制御特性の連続性を保つことができずショックが発生してしまうという課題があった。
特許文献１１では、速度などの状態量を出さずに制御特性の改善を実施している。モーションの指令値とモデルを用いて制御量を調整するため、制御遅れや分解能の影響なく全体の制御特性を改善することができるが、フィードバック制御と一体となって機能しているためそれぞれを分離して観測することや、適切に調整することが難しいという問題があった。
特許文献１２ではモータ側と負荷側の情報を基に制御を行うのできめ細かな制御ができる点でメリットであるが、エンコーダが本来持っている分解能の限界を改良するものではなく、また高価なエンコーダを限られたスペースで実現するという、技術的または設計上の課題があった。
【００１２】
本発明はこのような問題点に鑑みてなされたものであり、シミュレーション演算部での演算結果を利用して、センサによる検出信号から、エンコーダの分解能により生じるリップルや制御の時間遅れがない良質な速度フィードバック信号を得ることにより良好な制御特性を持った制御装置を提供することを第２の目的とする。
【課題を解決するための手段】
【００１３】
上記第１の目的および第２の目的を達成する共通の発明として、請求項１記載の制御装置の発明は、運動指令に従って制御器が機械装置を駆動し、前記機械装置の物理的状態量をセンサによって検出し、前記制御器へフィードバックする制御装置において、前記運動指令を入力とし、前記制御器をシミュレートする制御器モデル演算部および前記機械装置をシミュレートする機械モデル演算部からなるシミュレーション演算部と、前記シミュレーション演算部の演算結果を用いて前記センサによる検出信号を修正し、前記制御器へフィードバックする修正演算部とを備えることを特徴としている。
また、上記第１の目的を達成するため、請求項２記載の発明は、請求項１記載の制御装置において、前記修正演算部が、前記センサによる検出信号または前記シミュレーション演算部の演算結果に対し座標変換を行なう座標変換部を備えることを特徴としている。
同じく請求項３記載の発明は、請求項１または２記載の制御装置において、前記センサが、加速度センサまたはトルクセンサまたは位置センサであることを特徴としている。
【００１４】
また、上記第２の目的を達成するため、請求項４記載の発明は、請求項１記載の制御装置において、前記修正演算部が、前記センサによる検出信号と前記シミュレーション演算部の演算結果に各々重み付け係数を乗じることを特徴としている。
同じく請求項５記載の発明は、請求項１記載の制御装置において、前記修正演算部が、前記シミュレーション演算部の演算結果の変化率に基づき前記センサによる検出信号を修正することを特徴としている。
同じく請求項６記載の発明は、請求項４または５記載の制御装置において、前記修正演算部が、前記センサによる検出信号と前記シミュレーション演算部の演算結果を比較し、その乖離度により前記重み付け係数を変化させる乖離度評価部を備えることを特徴としている。
同じく請求項７記載の発明は、請求項４または５記載の制御装置において、前記シミュレーション演算部が、演算結果の振幅を調整する振幅調整手段と時間遅れ・進みを調整する時間遅れ・進み調整手段とを備えることを特徴としている。
同じく請求項８記載の発明は、請求項１〜７のいずれか１項記載の制御装置において、前記センサによる検出信号および前記シミュレーション演算部の演算結果を提示する提示手段と、前記機械モデル演算部の演算モデルの調整を行なう調整手段とを備えることを特徴としている。
同じく請求項９記載の発明は、請求項８記載の制御装置において、前記提示手段が、前記センサによる検出信号と前記シミュレーション演算部の演算結果との差分または前記差分の時間積分値を提示することを特徴としている。
同じく請求項１０記載の発明は、請求項９記載の制御装置において、前記シミュレーション演算部の演算結果を量子化する手段と、前記量子化した結果と前記センサによる検出信号を比較表示する比較表示手段とを備えることを特徴としている。
同じく請求項１１記載の発明は、請求項９または１０記載の制御装置において、  前記運動指令と前記センサによる検出信号を時系列で保存し、オフラインにて前記シミュレーション演算部のパラメータを調整するパラメータ調整手段を備えることを特徴としている。
また、請求項１２記載の発明は、制御方法に係り、運動指令に従って制御器が機械装置を駆動し、前記機械装置の物理的状態量をセンサによって検出し、前記制御器へフィードバックする制御方法において、前記運動指令に対する前記制御器および前記機械装置の動作を演算によりシミュレートし、前記シミュレートした演算結果を用いて前記センサの検出信号を修正し、前記制御器へフィードバックすることを特徴としている。
【発明の効果】
【００１５】
請求項１記載の発明によると、運動指令に対する機械装置の応答をシミュレーション演算により求め、機械装置に取り付けられたセンサの検出信号から、制御目的のために必要な成分のみを抽出することができる。これにより、例えば振動抑制など制御対象に対して高い効果が得られるフィードバック制御が可能になる。
請求項２記載の発明によると、機械装置への運動指令が急峻に変化した場合や、制御対象が多関節ロボットのような複雑な動きをする場合でも、制御系パラメータと簡単なモデルに基づくシミュレーション演算から、制御遅れなどを修正したフィードバック信号が得られ、振動抑制やインピーダンス制御、また衝突検出などに高い効果が得られる。
請求項３記載の発明によると、運動指令により発生する機械装置の加速度と機械系が内在する振動要素により発生する振動要素または機械装置に作用する外力により発生する加速度とを区別してフィードバックすることが可能になる。
または、運動指令により発生する機械装置のトルクと機械装置に作用する外力により生じるトルクを区別して検出することが可能となる。
または、エンコーダの分解能により生じるリップルや時間遅れのないフィードバック信号を得ることが可能となる。
また、請求項４記載の発明によると、運動指令に対する機械装置の応答をシミュレーション演算により求め、機械装置に取り付けられたセンサの検出信号から、制御目的のために必要な成分のみを抽出することができる。
請求項５記載の発明によると、シミュレーション波形の変化率を基に実機センサの変化率を修正することにより、実機センサ信号の変化率を一定以下に押さえリップルと遅れを低減することができる。
【００１６】
請求項６記載の発明によると、実機のセンサとシミュレーション出力をリアルタイムに比較して外乱の影響を評価して、請求項４記載の信号合成の重み付けを変更する。これにより外乱が作用する場合も適切にその影響を考慮してサーボ剛性を保つことができるサーボモータのフィードバック制御系を実現することができる。
請求項７記載の発明によると、シミュレーション演算部に振幅および時間遅れ進み要素とその調整手段を挿入することにより、簡便に実機センサ出力とシミュレーション出力を合致させることができる。
請求項８および９記載の発明によると、実機センサ波形とシミュレーション波形を調整者に提示する手段と振幅および時間遅れを調整する手段を設けることで、調整者は視覚的に波形の変化を確認しながら、センサによる出力信号とシミュレーション演算とのずれを認識することができ、さらに提示される情報に基づきシミュレーションモデルのパラメータを調整することで両者の合致度を上げ、短時間で精度の高いフィードバック制御系を構築することが可能となる。
請求項１０記載の発明によると、シミュレーションの出力を量子化することで、実機センサとシミュレーション出力との比較および調整を容易にすることができる。
請求項１１記載の発明によると、運動指令と実機のセンサ信号を時系列でメモリに保存し、オフライン的にシミュレーション系のパラメータを調整する手段を備えることで、実機を何度も動作させることなく、シミュレーション部の調整を可能とする。
さらに、請求項１２記載の発明によると、リップルを含んだ実機のセンサ信号および実機と非常に近似してリップルや遅れが全くないシミュレーションの出力を適切に重み付けをすることでリップルが低減した新たな修正センサ信号を得ることができる。この修正センサ信号をフィードバック演算に用いることにより、リップルと遅れが減少するため、即応性、安定性の高いサーボモータのフィードバックを実現することができる。
【発明を実施するための最良の形態】
【００１７】
以下、本発明の実施の形態について図を参照して説明する。
図１は本発明の原理を説明する制御ブロック図を示している。
図１において、実機制御部１００では、運動指令θrefを基に制御器１０１が機械装置１０２を駆動し、センサ１０３にて機械装置１０２の状態量を検出している。この状態量には、機械装置１０２に内在する振動などの状態量や外界から作用する力により発生する状態量のような、運動指令θref以外による機械装置１０２の状態量が含まれている。
一方、シミュレーション演算部１０４では、制御器モデル演算部１０５および機械モデル演算部１０６にて運動指令θrefに対する機械装置１０２の状態量をシミュレーションにより求める。
そして修正演算部１０８にて、実測した状態量とシミュレーションによる状態量とを用いて、運動指令θrefによる状態量とそれ以外の要因による状態量とを分離する。
その結果をセンサ修正値１０９として制御器１０１へとフィードバックする。
以下、本願第１発明について実施例１〜実施例６で、本願第２発明について実施例７〜実施例１１で詳細に説明する。
【実施例１】
【００１８】
本願第１発明の第１実施例として、各軸が独立に駆動され、機構的な干渉がほとんどない制御系の振動補償に本発明を適用した場合について述べる。
図２に示すＸＹテーブル装置２０４にＸ方向の加速度を検知するように単軸加速度センサ２０５を取り付けた場合を例にとる。
２個のモータ２０６、２０７のそれぞれの回転動作がテーブル２０８のＸまたはＹ方向の動作に対応する。従って単軸加速度センサ２０５では、運動指令θrefに応答するＸ軸モータ２０６による加速度と、振動成分の加速度とが重畳された形で検出される。
ここで、単軸加速度２０５の検出値から振動成分の加速度のみを抽出し、その振動を低減するようＸ軸モータ２０６を制御する。
図３は図２の装置の制御ブロック図である。
本実施例では、図３のブロック図のうちアンプ２０２、機械装置１０２、単軸加速度センサ２０５を除いた部分は制御演算装置２０１内に内蔵されている。
制御演算装置２０１の内のシミュレーション演算部１０４は、マクロ的には実機制御部１００と全く同等な振る舞いをする。また、機械モデル演算部１０６で用いる機械モデルは運動指令θrefに対する機械装置１０２のマクロな特性のみを模擬すればよいため、剛体モデルなど単純化されたものでよい。
厳密にはテーブル２０８はモータ２０６、２０７のリップルやカップリング、ボールねじなどの動力伝達系を有するため完全な剛体ではなく、剛体モデルとは異なった応答を示す。しかし、時間軸に関してマクロに見ると剛体系の応答となるため、剛体モデルによるシミュレーション演算出力と機械装置１０２の応答との差分をとることで、振動成分など抑制すべき成分が抽出される。
【００１９】
振動を低減する方式は多く提案されているのでそのいずれかを用いることができる。例えば、特許文献１などの方式がある。本実施例では、シミュレーション演算に基づき修正された加速度信号をセンサ修正値１０９としてトルク指令の部分にフィードバックする制御系を示している。
【００２０】
図３を用いて概略を説明する。
制御器１０１と制御器モデル演算部１０５は共に運動指令θrefが入力され、位置速度系として一般的な比例・積分制御の構成とする。
運動指令θrefを受けた実機制御器１０１は比例積分制御などの位置速度制御演算を実行する。その結果トルク指令が生成され、その指令をＸ軸アンプ２０２に払い出す。
Ｘ軸アンプ２０２ではトルク指令に基づきＸ軸モータ２０６の電機子に流れる電流を制御し、指令トルクに従ったトルクを発生することができる。駆動されるＸ軸モータ２０６の出力はボールねじなどの伝達機構を介してテーブル２０８を駆動する。
また、Ｘ軸モータ２０６には回転角度を検出する光学式エンコーダなどのセンサが接続され、その回転角度が制御器１０１に送られる。回転角度はその変化量から速度が演算され回転角度とともにフィードバック制御に利用される。
【００２１】
一方、シミュレーション演算部１０４では機械装置１０２の制御演算と同期して同様の制御演算が実行される。すなわち、実機制御部１００にはＸ軸アンプ２０２によって駆動されるＸ軸モータ２０６や、ボールねじなどの伝達系が存在するのに対し、シミュレーション演算部１０４ではそれら機械装置を模擬するための演算が行なわれる。例えば、慣性負荷モデルを数値モデルとして演算に用いる。
ここで用いられる機械装置１０２のモデルは、前述のように剛体モデルで構わないが、位置速度制御の応答特性に合わせて制御遅れを無視できない場合はアンプなどの特性を含める必要がある。また機械や伝達系で無視できないバネ要素やダンピングなどを含む場合はそれらの要素をモデル化して演算に加えることもできる。
本実施例では、シミュレーション演算部１０４内の機械モデル演算部１０６によって運動指令θrefに対するマクロな応答を演算する。
そして、修正演算部１０８にて、単軸加速度センサ２０５の検出信号からＸ軸モータ２０６およびテーブル２０８などの機械系のマクロな応答が差し引かれ、機械装置１０２で発生する振動など制御系以外の要因による加速度が抽出されることになる。
【００２２】
図４に制御演算のフロー図を示す。
フィードバック制御のリアルタイム性を損なわないよう、シミュレーション演算は制御演算装置２０１にてごく短時間で行なわれる。
前述のように運動指令θrefは実機制御部１００とシミュレーション演算部１０４それぞれに入力される。
まず制御器１０１にて図４に示す実機制御演算ステップの制御演算が実行される。その演算結果はＸ軸アンプ２０２へ払い出され、機械装置１０２が駆動される。
【００２３】
続いてシミュレーション制御演算ステップが実行される。
このステップでは制御器１０１と全く同じ構造を持つ制御器モデル演算部１０５にて演算が実施され、その結果を機械モデル演算部１０６に入力する。機械モデル演算部１０６は機械装置１０２のモデルに基づき、そのマクロな応答を出力する。機械モデル演算部１０６の出力から機械モデルの加速度を求める。
続くセンサ検出値修正演算ステップでは、修正演算部１０８にて加速度センサ２０５により得られた機械装置１０２の加速度と、機械モデルの加速度との差を求めることで制御系の遅れを含んだ加速度のセンサ修正値１０９を得る。
【００２４】
図４のフロー図は制御演算装置２０１内で実機制御と同時にシミュレーション演算を実行する場合を示しているが、シミュレーション演算は制御演算装置２０１とネットワークなど通信路で接続された別の制御演算装置で行なってもよい。
さらに運動指令θrefが予め決められている場合には、事前にシミュレーション演算部１０４にてシミュレーション演算を行ってシミュレーション加速度演算値３０８の時系列データを制御演算装置２０１に保存しておくことで、シミュレーション演算を並列して行なわずに同様の結果を得ることができる。
前述のように、実際の制御対象には伝達系が存在するため厳密には剛体ではないが、単軸加速度センサ２０５の出力信号に含まれる高周波成分の大部分が抑制するべき振動成分であるとすると、機械系のモデルは単純な剛体モデルとしてよい。
また、アンプ２０２やＸ軸モータ２０６へ指令を伝える電気的な伝送系の特性が無視できない場合には、それらをモデル化し、機械モデル演算部１０６での演算に加えてもよい。
【００２５】
次に、シミュレーション演算の方法を具体的に説明する。
Ｘ軸モータ２０６の角度をθとする。その時シミュレーション演算部１０４ではブロック図の各状態量を逐次演算する。
微分要素を含むブロックに関してはオイラー近似した下記の式で機械装置１０２の動作を順次演算する。ここでの演算方法は実機制御部と同じであれば問題ないが、実機とシミュレーションの状態量の対応づけが容易な下記の方法が望ましい。
【００２６】
θe  = θref − θk-1              …  式（１）
ωref  = Kp・θe                   …  式（２）
ωe  = ωref − ωk-1              …  式（３）
τk  = τk-1 ＋ ki・t0・ωe        …  式（４）
τref  = （Ve ＋ τｋ）・Kv        …  式（５）
τg  =  Kt・τref                  …  式（６）
α  = τg／Jm                      …  式（７）
ωk  = ωk-1 ＋ α・t0             …  式（８）
θk+1  = θk ＋ ωk・t0             …  式（９）
ただし、k=１、２、３・・・であり、
θref：モータ角度指令（運動指令）、
θｋ：モータ角度、
θe：角度偏差、
Kp：位置制御ゲイン、
ωref：モータ速度指令、
ωｋ：モータ速度、
ωe：速度偏差、
τｋ：積分器の出力、
t0：演算周期、
ki：積分ゲイン、
τref：トルク指令、
Kv：速度制御ゲイン、
Kt：トルク定数（演算値から発生トルクまでの全ての要素を含む比例定数）、
τg：発生トルク、
α：加速度、
Jm：モータロータ、駆動力伝達系、機械系など含むモータ負荷全体のモータ軸換算の慣性である。
【００２７】
図３におけるシミュレーション加速度演算値３０８は式（７）のαとして求められる。  前述のように単軸加速度センサ２０５から検出された値は、動作指令θrefに対する加速度と振動成分の加速度とが重畳されている。
このうち抑制対象である振動成分は、単軸加速度センサ２０５の検出信号からシミュレーション加速度演算値３０８を差し引いた値にほぼ等しい。従って修正演算部１０８にて差分をとり、その値をセンサ修正値１０９として制御器１０１にフィードバックすることで質のよい制振制御を実現できる。
【実施例２】
【００２８】
第２実施例として、本発明を多軸機械の振動補償制御に適用した例について述べる。
具体的には図５のように多関節ロボット５０１の作業ツール５０８に多軸加速度センサ５０６を取り付けた場合について説明する。
図６に本発明の第２実施例のブロック図を示す。
図において、制御器１０１、シミュレーション演算部１０４および修正演算部１０８は図５に示すロボット用多軸制御演算装置５０４に内蔵されている。
なお、図６では説明のため多関節ロボット５０１の１軸分のみを抜き出している。
複数のモータによって駆動され、複雑なモーションの組み合わせによって動作する多関節ロボット５０１において、作業ツール５０８に取り付けられた多軸加速度センサ５０６の検出信号を振動抑制制御に利用する場合には、その時点での多関節ロボット５０１の姿勢に基づく運動学に従って、多関節ロボット５０１を駆動するモータの座標系へ変換する必要がある。
本実施例は、多軸加速度センサ５０６の出力に対する座標変換が介在している点で第１実施例と異なる。詳細には、多軸加速度センサ５０６の検出値は直交座標系に基づいておりシミュレーション演算の出力はそれぞれのモータの関節座標系に基づいている。従って、２つの座標系を関係づける演算が必要である。
例としてヤコビアンと呼ばれる関係式がある。ヤコビアンは多関節ロボット５０１の関節角速度と手先速度との座標系関係を与える。ヤコビアンを用いた関係式を下記で示す。
【００２９】
【数１】
2005316937.tif 000003
【００３０】
ここで、
Ｊ：ヤコビアン（関節座標系と作業座標系間の微小変位関係式）、
0ｓi：第ｉ関節座標の回転方向ベクトル（多関節ロボット５０１のベース座標系を基準とする）、
0Ｐi：第ｉ関節位置ベクトル（多関節ロボット５０１のベース座標系を基準とする）、
ｒ：多関節ロボットの手先位置ベクトル
である。また、×はベクトルの外積を表す。
一方、
υ＝Ｊω    …  式（１１）
が成り立つ。
【００３１】
【数２】
2005316937.tif 000004
【００３２】
式（１３）より多関節ロボット５０１の手先加速度と関節角速度の関係が求まる。修正演算部１０８では、加速度分解演算部６０３にて多軸加速度センサ５０６の検出値に座標変換を施し各モータの関節座標系に変換された加速度値を求め、シミュレーション演算により求めた加速度３０８との差をとることによりセンサ修正値１０９を算出する。
そして、センサ修正値１０９がフィードバックされることで振動抑制制御が実施される。
図６では単軸の場合と同様、センサ修正値１０９が制御器１０１のトルク指令部分にフィードバックされる例を示している。ここでは、実機、シミュレーションともに関節角度（モータ角度）の制御系を用いた場合を示したが、実機制御部１００とシミュレーション演算部１０４とが同様の制御構造を有しており、それぞれに対する運動指令θrefが同じ値であれば、速度やトルクに対し座標変換を実施した場合でも図６のように実機とシミュレーションとを関節座標系で統一する必要はない。例えば多関節ロボット５０１の手先の直交座標系、すなわち作業座標系を用いて制御系を構成してもよい。
【実施例３】
【００３３】
第３の実施例として、本発明を加速度センサを用いたインピーダンス制御に適用した例について述べる。
インピーダンス制御とは外力に関して設定された機械的目標インピーダンスを実現するような制御方法である。
図７に第３の実施例に係るブロック図を示す。
ここでは説明を単純化するため、１軸の制御系に適用した場合に関して述べる。
機械装置１０２に取り付けた単軸加速度センサ２０５では運動指令θrefによるモータの運動と外力７０１により発生する運動とが加算された加速度が検出されるので、正確にインピーダンス制御を行なうには外力７０１によって発生する加速度のみを分離する必要がある。
そこで運動指令θrefによりモータが発生する加速度はシミュレーション演算部１０４にて求め、修正演算部１０８にて単軸加速度センサ２０５の検出値からシミュレーション加速度演算値３０８を差し引くことで外力７０１に関する機械装置１０２の加速度を抽出することができる。
この修正演算部１０８が出力するセンサ修正値１０９と、目標インピーダンス７０７の慣性ゲインJとの積を取り、その値を制御器１０１のトルク指令部分に加算することにより外力７０１から生成する慣性を調整することができる。
また、速度、位置に関しても同様に補償することで外力７０１に対する運動特性を関係づけるインピーダンス全体を目標インピーダンス７０７に調整することが可能になる。
【００３４】
以上は１軸の制御系の場合であるが、複数の自由度で構成される図５に示す多関節ロボットのような場合でも第１実施例と第２実施例との関係と同様に座標変換を介在させることで、運動指令θrefによらず、多自由度に干渉する機械系の目標インピーダンスを実現することができる。
【実施例４】
【００３５】
第４の実施例として、本発明を衝突検出に適用した例について述べる。
図８および図９は第４の実施例に係る制御ブロック図を示している。
図８は単軸の場合の衝突検出の例であり、図９は図５に示す多関節ロボットのような多軸機械の場合の衝突検出の例である。
単軸の機械系に取り付けられた単軸加速度センサ２０５の検出値により、外界との衝突を検知する例を図８に示す。制御対象である機械が外界と接触した場合、外力７０１が加わり単軸加速度センサ２０５の検出信号に変化が表れる。しかし、その信号変化だけでは実施例１〜３と同様、運動指令θrefに起因するものか外界との衝突に起因するものかを区別することができない。
そこで、シミュレーション演算部１０４にて運動指令θrefと機械系の慣性モデルとから演算により加速度を求め、その値と単軸加速度センサ２０５で検出した値とを用いて修正演算部１０８にて修正演算を行い、外力７０１に起因する加速度を抽出する。抽出された加速度によって、衝突判定部８０５にて衝突か否かの判定をする。衝突判定部８０５では、例えば予め定められた閾値との比較で判定を行なう。
制御対象が図５に示す多関節ロボットのような多軸機械の場合においても、多軸加速度センサ５０６で検出した値と、運動指令θrefにより発生する加速度をシミュレーション演算により求めた値とを比較し、修正演算部１０８にて修正演算を行い、外力７０１に起因する加速度を抽出する。
ここでシミュレーション演算で求められる加速度は関節座標系に基づく値であるため、多軸加速度センサ５０６と座標系を一致させる必要がある。
【００３６】
図９ではシミュレーション演算で求められる加速度を多軸加速度センサ５０６の座標系に一致させる場合を示している。
シミュレーション演算部１０４の出力は加速度座標変換演算部９０４にて多軸加速度センサ５０６の座標系での値に変換される。その値と多軸加速度センサ５０６で検出した値とを用いて修正演算部１０８にて修正演算を行い、外力７０１に起因する加速度を抽出する。
単軸の場合と同様に、抽出された加速度から、衝突判定部８０５にて複数の座標系に基づく衝突判定が実施される。
なお、加速度座標変換演算部９０４および衝突判定部８０５は図５に示すロボット用多軸制御演算装置５０４に内蔵されている。
【００３７】
図９ではシミュレーション演算値を多軸加速度センサ座標系に変換したが、実施例２のように多軸加速度センサ５０６の検出値を関節座標系に変換することも可能である。
シミュレーション演算値を用いて修正された多軸加速度センサ５０６の出力は運動指令θrefによる多軸機械の加速度成分を取り除いた値であるため、外力７０１に起因する加速度に近い値が得られる。しかも、時々刻々得られる加速度の変化率を観測することにより衝突判定部８０５はリアルタイムに衝突の判定を行なえ、従来のような判定のための遅れが解消される。
【実施例５】
【００３８】
図１０は第５の実施例を示している。
本実施例では、上述の加速度センサに代わりトルクセンサを利用する。本実施例においても、シミュレーションによる演算データを用いてトルクセンサの検出値を修正する。
図１０はトルクセンサ１００１を内蔵した多関節ロボット５０１の構成を示している。  図１０では多関節ロボット５０１を駆動する複数のモータの１つにトルクセンサ１００１を取り付けた例を示しているが、複数軸に利用した場合もこの例と同様に考えることができる。
トルクセンサ１００１で検出されるトルクは大きく２種類に分けられる。１つは外界から多関節ロボット５０１に作用する外力７０１や多関節ロボット５０１が自ら外界に接触した際の力であり、もう１つはモータ１００２が発生するロボットアームの慣性体を加速するトルクである。
トルクセンサ１００１の検出信号に対して処理を施さない場合は、両者を区別することができない。よって２種類の信号が混在したトルク値を、外力７０１によるトルクとモータ１００２によるトルクとに分離する必要がある。
ここで、ロボットアームのイナーシャが既知であれば運動指令θrefに対する加速トルクをシミュレーション演算部１０４でのシミュレーション演算により求めることができるため、これをトルクセンサ１００１の検出信号と比較することで慣性トルクを分離でき、加速度センサの場合と同様にモータ１００２により発生するトルクを求めることができる。
【００３９】
図１１は図１０の制御ブロック図を表している。
修正演算部１０８にて、トルクセンサ１００１の出力値はシミュレーション演算部１０４の出力であるシミュレーショントルク演算値１１０２によって修正され、モータ１００２の慣性力と、外部からロボットアームに作用する外力７０１とを分離することが可能になる。修正演算部１０８の出力は実機の制御に利用される。すなわち制御アルゴリズム処理部１１０３にて、例えば振動抑制のフィードバック信号やロボットアームの衝突検出後処理などが実行され、その結果は制御器１０１にフィードバックされたり、衝突検出の場合には非常停止が実行されたりする。
なお、制御アルゴリズム処理部１１０３は図１０に示すロボット用多軸制御演算装置５０４に内蔵されている。
【００４０】
以上はロボットアームを駆動する直前のトルクが検出される場合について示したが、トルクセンサ１００１は取り付けられる部位により、その検出信号に含まれる成分が異なる。
例えば減速機の前段にトルクセンサ１００１を付加すると、その検出信号には減速機の摩擦が含まれることになる。その場合には摩擦をモデル化してトルク検出値から差し引く必要がある。一般的にはクーロン摩擦と速度に比例する粘性摩擦の和のモデルで近似することができる。
この場合は検出トルクに対して摩擦モデル分を補正することでトルクセンサ１００１の検出値から修正トルクを導出することができる。
【実施例６】
【００４１】
図１２は第６の実施例を示している。
前述のように、実機制御部１００内の制御器１０１とシミュレーション演算部１０４内の制御器モデル演算部１０５は制御系演算としては同一の構成であり、シミュレーション演算部１０４ではモータまで含めた機械系のマクロな応答を模擬するモデルを用いる。
従って実機、シミュレーションともに同様の傾向を示す値となる。しかし、機械モデルに用いられる値は必ずしも実機系の物理量として精度良く求められているわけではなく、実機には存在するものの正確にモデル化しにくい摩擦など、状態により変動する非線形な値は初めから精度良く組み込まれているわけではない。
従って、センサの検出値を修正する目的に対して妥当なシミュレーションとなっているかどうかを作業者が判断できる情報を提示し、さらには機械モデルのパラメータを調整して最適なシミュレーションモデルにできるような調整手段を設ける。
【００４２】
図１２は実施例２の基本構成図を用いた具体的実例を示している。
図１２で実機制御部１００の多軸加速度センサ５０６の検出加速度とシミュレーション演算部１０４でのシミュレーションにより得られた加速度情報がティーチペンダント１２０２、ＰＣ１２０３などに入力され、それらの画面上に作業者に知覚可能なように提示される。
作業者は提示された情報を基に、実機およびシミュレーションの応答波形成分のうち、直流成分や低周波成分など波形の大まかな形状が一致するよう、入力キーなどの調整手段を用いてシミュレーションに用いる機械モデルのパラメータなどを変更する。提示内容はアナログ的なグラフ表示でもデジタル的な数値情報でもよいが、認識、調整、その結果の判断というサイクルの繰り返しになるためできるだけ短時間に直感的に捉えられる物理量の提示が望ましい。例えば視覚的に提示する方法として、実機およびシミュレーションの出力の時間推移が分かるグラフ表示や、その時々で刻々と変化するバー表示などがある。
また、実機およびシミュレーションの出力の振幅、周波数をそれぞれ音の大きさ、音の高低（周波数）に変換し、２音の違いを作業者が聞き取り調整するといった方法でもよい。
また、実機およびシミュレーションの出力を所定時間蓄積してから時系列的に作業者に提示し、その表示を基に調整を行なうようにすればリアルタイムに調整する必要がなく、さらに調整が容易になる。
【００４３】
本願第１発明は、ロボットなどの機械装置において、加速度やトルクなどの物理的状態量をセンサにより検出し制御器にフィードバックして制御する装置および方法全般に広く適用できる。
【００４４】
次に、本願第２発明について実施例７〜実施例１１で説明する。
【実施例７】
【００４５】
再び図１に戻って、シミュレーションでは実機と全く同じ動作指令に対し、全く同じ制御装置とパラメータにより、モータ特性、メカ特性を考慮した角度や速度などの状態量を演算により得ることができる。シミュレーションは全てＣＰＵでの演算処理なので実機のセンサの分解能の影響はない。そのためリップルがなく制御系など考慮された要素以外の時間遅れもない波形が得られる。
実機系にはトルクリップルや摩擦、機械系の特性などシミュレーションには含まれていない要素があるため、実機のセンサ出力とシミュレーションの出力は多少異なる。ところが通常フィードバック系のゲインは十分大きく取られており、種々の制御的補償手段が施されているため、実機系の出力とシミュレーションの出力は非常に近いものになる。
【００４６】
実機系の速度、加速度などの状態量信号を得る方法は過去のサンプリング時に得た値との差分処理を基本としており、微分的処理によりリップルを含んだ値となる。一方シミュレーション出力は実機出力と非常に近似した波形であるが全くリップルを含んでいない。  従って、シミュレーション出力を規範として、実機速度出力を修正することでリップルの成分を低減してしかも遅れのない実機速度情報を得ることができる。修正のやり方は種々考えられるが、実施例で具体的な方法を述べる。これらにより、見かけ上分解能を上げた速度などの状態量を得ることができる。
【００４７】
ただし、サーボモータの制御系では指令に対する追従性と同時に、負荷に作用する外力に対しても影響を受けないよう、サーボモータの制御的剛性も作り出している。シミュレーションでは既知の外乱力以外は考慮されない。そのため実機速度を修正する場合に、外乱力が作用した場合へも対処できるアルゴリズムになっていることが重要である。この点を考慮して評価するため乖離度評価のブロックを設ける。急加減速時や外力が作用する場合には、エンコーダ出力とシミュレーション出力に乖離が生じる。その場合にはエンコーダ信号の方をより反映したデータとなるよう調整する。
以上は角速度をセンサから求める場合に関して述べた。角加速度を求める場合はエンコーダの信号から求めた角速度に対しさらに差分処理を行う。そのためセンサ出力の修正は角速度の場合と同様の構成で行うことが可能である。
【００４８】
第７の実施例として、実機制御部とシミュレーション演算部の出力を重み付けして加算する場合に関して述べる。
図１３に制御ブロック図を示す。それぞれのブロックの機能は図１の原理図で示した通りであるが図１３では主に修正演算部１０８の機能を説明する。図１４、図１５は本発明が適用される対象の例を示している。図１４はＸＹテーブル、図１５は産業用ロボットに適用した場合の例を示す。
【００４９】
実機速度は制御器や機械装置の特性以外にリップルや外乱トルクを原因とする偏差を含んでいる。一方シミュレーション速度は制御器と機械モデルの特性を含みながらリップルを含まずしかも時間的に遅れのない滑らかな波形を描く。両者を組み合わせることでそれぞれの特徴の良い部分を併せ持った速度信号を得ることができる。その信号合成を図１３の修正演算部１０８にて重み付け係数１，２を用いて実施する。ここでは重み付け係数ａ、１−ａによりそれぞれの波形成分を合成する割合を調整している（ただし、０≦ａ≦１である。）。
【００５０】
図１３では実機速度とシミュレーション速度の２種類の波形を比較して乖離度を評価し、場合に応じて調整する例を示している。調整の中で割合を変化させる目的は外乱力に対する適応性を持たせることである。外乱が作用した場合はそうでない場合に比較して偏差が生じる。この偏差がフィードバックされて高いサーボ剛性が得られることになるが、これは理想的なシミュレーション波形に対しても偏差を生ずることにもなる。乖離度評価ブロック１３０３はその結果を信号合成の度合いに反映させる役割を持つ。
【００５１】
図１６に制御演算のフロー図を示す。
前述のように運動指令θrefは実機制御部１００とシミュレーション演算部１０４のそれぞれに入力され、通常の制御演算とほぼ同時にシミュレーション演算が実行される。例えば図１４、図１５のような制御対象の場合、これらの演算は、ＸＹテーブルの場合は制御演算装置２０１またはモジュール型のサーボアンプの部分で実行される。産業用ロボットの場合はモータの制御系は一般的に多軸のアンプで実行されるため、下記の演算は図の多軸アンプ部の部分で実行される。
上記のような多軸機構のサーボアンプの内、１つの軸に着目して説明する。まず制御器１０１にて図１６に示す実機制御演算ステップの制御演算が実行される。その演算結果はアンプへ払い出され、機械装置１０２が駆動される。
続いてシミュレーション制御演算ステップが実行される。このステップでは制御器１０１と全く同じ構造を持つ制御器モデル演算部１０５にて演算が実施され、その結果を機械モデル演算部１０６に入力する。機械モデル演算部１０６は機械装置１０２のモデルに基づき、その応答を出力する。
続くエンコーダ検出値修正演算ステップでは、修正演算部１０８にてエンコーダから得られた機械装置１０２の速度と、機械モデルの速度との差を求めることで制御系の遅れを含んだ速度のセンサ修正値１０９を得る。
【００５２】
図１６のフロー図は制御演算装置２０１内で実機制御と同時にシミュレーション演算を実行する場合を示しているが、シミュレーション演算は制御演算装置２０１とネットワークなど通信路で接続された別の制御演算装置で行なってもよい。
さらに運動指令θrefが予め決められている場合には、事前にシミュレーション演算部１０４にてシミュレーション演算を行ってシミュレーション加速度演算値３０８の時系列データを制御演算装置２０１に保存しておくことで、シミュレーション演算を並列して行なわずに同様の結果を得ることができる。
以下には、機械系は最も単純な剛体モデルを仮定した場合のシミュレーションの例を示している。通常は２慣性系や摩擦等を含むモデルが用いられることが多い。
またアンプ２０２やＸ軸モータ２０６へ指令を伝える電気的な伝送系の特性が無視できない場合には、それらをモデル化し、機械モデル演算部１０６での演算に加えてもよい。
【００５３】
次にシミュレーション演算の方法を具体的に説明する。
Ｘ軸モータ２０６の角度をθとする。その時シミュレーション演算部１０４ではブロック図の各状態量を逐次演算する。
微分要素を含むブロックに関しては、段落番号［００２６］で示したオイラー近似した関係式（１）〜（９）で機械装置１０２の動作を順次演算する。
シミュレーションは通常浮動小数点演算で実行されるため、分解能はほぼ無限に小さくできると考えてよい。後述する実施例８で加える振幅・時間遅れ進み調整により、シミュレーション出力と実機のセンサ出力との乖離は小さくなる。乖離度が評価された後、修正演算が施される。
乖離度の評価量Ａの定義は種々考えられるが、次式のような単純な差の絶対値や
Ａ＝ａｂｓ（ωm−ωs）
次式のような誤差の２乗値などが一般的である。
Ａ＝（ωm−ωs）2
ここで、ωm  は実機速度、ωs  はシミュレーション速度とする。
評価量Ａと予め定められた基準値Ｂを基に、例えば次式のように重み付け係数を定める。
ａ＝Ａ／Ｂ
この値を用いて次式のωc  のようにセンサ修正値１０９が定まる。
ωc＝ａ・ωm＋（１−ａ）・ωs
シミュレーション演算に基づき修正された値を制御器１０１にフィードバックすることでリップルが低減され遅れのない制御系を実現することができる。
【実施例８】
【００５４】
実施例７では実機とシミュレーションの信号を加え合わせることで、両者の特徴を生かした例を示したが、実施例８ではシミュレーションのスムーズで連続的な波形の特徴をエンコーダの波形修正に用いる実施例を示す。
図１７ではシミュレーション速度の変化率（すなわち加速度）を用いている。  加速度演算１７０１で求めたシミュレーション速度の変化率を用いてエンコーダの出力から得られた速度の変化を一定値以内に規制する。その場合も波形のマクロな変化に対しては乖離度を評価し、一定以上の乖離を生じた場合には規制の度合いを調整する。一定以上の乖離を生ずる原因としては外乱が作用する場合や運動指令の変化率が大きい場合が考えられる。
【００５５】
図１９はシミュレーション演算部１０４に振幅・時間遅れ進み調整部１９０１を設けた場合を示している。簡便な調整で機械モデルの不確定な部分の吸収を図る。具体的調整例は実施例９で示す。
また、振幅・時間遅れ進み調整部は実施例７の場合も同様に利用することが可能である。
【実施例９】
【００５６】
前述のように、実機制御部１００内の制御器１０１とシミュレーション演算部１０４内の制御器モデル演算部１０５は制御系演算としては同一の構成であり、シミュレーション演算部１０４ではモータまで含めた機械系のマクロな応答を模擬するモデルを用いる。
従って実機、シミュレーションともに同様の傾向を示す値となる。しかし、機械系の計算モデルに用いられる値は必ずしも実機系の物理量として精度良く求められているわけではなく、実機には存在するものの正確にはモデル化しにくい摩擦など、状態により変動する非線形な要素は初めから精度良く組み込まれているわけではない。
従って、センサの検出値を修正する目的に対して妥当なシミュレーションとなっているかどうかを作業者が判断できる情報を提示し、さらには機械モデルのパラメータを調整して最適なシミュレーションモデルにできるよう、図１８に示す提示・調整手段を設けている。
【００５７】
構成の実例を図１９に示す。
ここでは図１３の基本構成図に適用した場合を示している。図で実機制御部１００のエンコーダ差分値とシミュレーション演算部１０４でのシミュレーションにより得られた速度情報がティーチペンダント１２０２、ＰＣ１２０３などに入力され、図１９における表示部にあたるそれらの画面上に作業者に知覚可能なように提示される。
作業者は提示された情報を基に、実機およびシミュレーションの応答波形成分のうち、直流成分や低周波成分など波形ができるだけ一致するよう、入力キーなどの調整手段を用いて振幅・時間遅れ進み調整部１９０１、またはシミュレーションに用いる機械モデルのパラメータなどを変更する。
提示内容はアナログ的なグラフ表示でもデジタル的な数値情報でもよいが、認識、調整、その結果の判断というサイクルの繰り返しになるためできるだけ短時間に直感的に捉えられる物理量の提示が望ましい。例えば視覚的に提示する方法として、実機およびシミュレーションの出力の時間推移が分かるグラフ表示や、その時々で刻々と変化するバー表示などがある。
また、実機およびシミュレーションの出力を所定時間蓄積してから時系列的に作業者に提示し、その表示を基に調整を行なうようにすればリアルタイムに調整する必要がなく、さらに調整が容易になる。
【実施例１０】
【００５８】
実施例１０は提示・調整手段の変形例に係るもので、それを図２０に示す。
ここではシミュレーション出力１１２を量子化し、実機信号との比較対象を容易にしている。実施例９では実機の速度はデジタル量による表現であり、一方、シミュレーションの速度は非常に細かい有効数字まで表現できるアナログ的表現になっているため、マクロな調整では問題ないが、詳細な部分に関しては調整がむずかしくなる。特に低速域においては波形が大きく異なるため微妙な調整が困難になる（図２０（ａ））。
そこでシミュレーションの出力を実機の出力に近い、段階的な量へ変換し提示手段で表現する。これにより振幅、および時間的なずれの調整が簡単になる（図２０（ｂ））。
【実施例１１】
【００５９】
また、リアルタイムでの調整が困難な場合には実機の速度などの出力を運動指令の時系列と対応させて一旦メモリに保存し、シミュレーションではタイミングを合わせて状態を再現しなおすことで、実機を何度も繰り返し動かす必要がなく、オフライン的に調整ができる手段を提供することができる。図２１はデータ取得時、調整時、運転時に利用する制御系のブロック図の組み合わせを時系列に表現している。
(a)はデータ取得時の制御ブロック図を示している。この場合、一連の運動指令と実機の応答を入出力記憶部２１０１に保存する。通常、機械装置を動かすモーションのパターンは予め計算され、上位コントローラから、運動指令として実機を制御する制御器１０１に渡される。従って、あるパターンに従った一組単位の指令とそのパターンに従ったセンサ出力１１１が時系列に対応を保って、入出力記憶部２１０１に保存される。
(b)は調整時の制御ブロック図を示している。入出力記憶部２１０１に記憶された運動指令がシミュレーション演算部１０４に入力され、その運動指令に対応して記憶されたセンサ出力が併せて出力される。
そして運動指令に対するシミュレーション出力１１２は、センサ出力と共に表示部によって作業者に提示される。
作業者はあたかも実機を動かしながら対比しているのと同様な入出力信号の対応関係を得られ、その情報を基にシミュレーション演算部１０４のパラメータ調整を実施する。ここでシミュレーション演算部１０４に入力される運動指令は、入出力の対応さえ取ることができれば、記憶した値ではなく、上位コントローラからの指令でもよい。
(c)は運転時の制御ブロック図を示している。(a)(b)の調整の段階を経ているため、シミュレーション演算部１０４のパラメータは適切に調整されている。従って、運転時は実施例７や実施例８と変わらない制御ブロック図となる。
本願第２発明は、ロボットなどのサーボモータを用いた機械装置において、エンコーダの出力から高精度で遅れがない速度、加速度信号を得ることを可能とするため、サーボモータを用いた位置決め、速度制御を必要とする装置および方法全般に広く適用できる。
【図面の簡単な説明】
【００６０】
【図１】本願発明の基本構成図である。
【図２】第１発明の第１の具体的実施例の物理的構成イメージを表す図である。
【図３】第１発明の第１の具体的実施例を表す制御ブロック図である。
【図４】第１発明の基本構成のソフトウェアフロー図である。
【図５】第１発明の第２、３、４の具体的実施例の構成を表す図である。
【図６】第１発明の第２の具体的実施例を表す制御ブロック図である。
【図７】第１発明の第３の具体的実施例を表す制御ブロック図である。
【図８】第１発明の第４の具体的実施例の変形例（単軸の場合）を表す制御ブロック図である。
【図９】第１発明の第４の具体的実施例（多軸の場合）を表す制御ブロック図である。
【図１０】トルクセンサを利用した多軸機械構成（ロボットの例）を示す図である。
【図１１】トルクセンシング時の制御ブロック図である。
【図１２】第１発明の第６実施例の物理的構成を表す図である。
【図１３】第２発明の第１実施例（本願第７実施例）を表す制御ブロック図である。
【図１４】第７実施例の第１適用例を表すイメージ図（ＸＹステージの例）である。
【図１５】第７実施例の第２適用例を表すイメージ図（ロボットの例）である。
【図１６】第７実施例の基本構成のソフトウェアフロー図である。
【図１７】第２発明の第２実施例（本願第８実施例）を表す制御ブロック図（加速度規制）である。
【図１８】第２発明の第３実施例（本願第９実施例）を表す構成図（提示・調整方法）である。
【図１９】第９実施例の具体例を表す制御ブロック図（振幅調整、遅れ調整）である。
【図２０】第２発明の第４実施例（本願第１０実施例）を表す構成図（量子化、調整補助）である。
【図２１】第２発明の第５実施例（本願第１１実施例）を表す構成図（入出力記憶とシミュレーション再生による調整）である。
【図２２】従来の方法を示すブロック図である。
【図２３】トルクセンサによるロボットアームの衝突検出の従来例を示すブロック図である。
【符号の説明】
【００６１】
１００  実機制御部
１０１  制御器
１０２  機械装置
１０３  センサ
１０４  シミュレーション演算部
１０５  制御器モデル演算部
１０６  機械モデル演算部
１０８  修正演算部
１０９  センサ修正値
１１０  運動指令
１１１  センサ出力
１１２  シミュレーション出力
１３０１  重み付け係数１
１３０２  重み付け係数２
１３０３  乖離度評価
２０１  制御演算装置
２０２  Ｘ軸アンプ
２０３  Ｙ軸アンプ
２０４  ＸＹテーブル装置
２０５  単軸加速度センサ
２０６  Ｘ軸モータ
２０７  Ｙ軸モータ
２０８  テーブル
２０９  センサ用ケーブル
３０６  加速度フィードバックゲイン
３０７  シミュレーション速度演算値
３０８  シミュレーション加速度演算値
５０１  多関節ロボット
５０３  ロボット用多軸アンプ
５０４  ロボット用多軸制御演算装置
５０５  ロボット制御盤
５０６  多軸加速度センサ
６０３  加速度分解演算部
６０４  他軸制御状態量
７０１  外力
７０４  機械装置の加速度
７０５  機械装置の速度
７０６  機械装置の位置
７０７  目標インピーダンス
７０９  シミュレーション位置
８０５  衝突判定部
９０４  加速度座標変換演算部
１００１  トルクセンサ
１００２  モータ
１１０２  シミュレーショントルク演算値
１１０３  制御アルゴリズム処理部
１２０１  通信線
１２０２  ティーチペンダント
１２０３  ＰＣ
１７０１  加速度演算
１７０２  加減速制限
１９０１  振幅・時間遅れ進み調整部
２１０１  入出力記憶部
２２１０  制御アンプ部
２２１１  サーボモータ
２２１２  ロータリエンコーダ
２２１３  加速度センサ
２２１４  ボールねじ
２２１５  リニアスライダ
２３０１  ロボットアーム
２３０２  モータ
２３０３  トルクセンサ
【図１】
2005316937.tif 000005
【図２】
2005316937.tif 000006
【図３】
2005316937.tif 000007
【図４】
2005316937.tif 000008
【図５】
2005316937.tif 000009
【図６】
2005316937.tif 000010
【図７】
2005316937.tif 000011
【図８】
2005316937.tif 000012
【図９】
2005316937.tif 000013
【図１０】
2005316937.tif 000014
【図１１】
2005316937.tif 000015
【図１２】
2005316937.tif 000016
【図１３】
2005316937.tif 000017
【図１４】
2005316937.tif 000018
【図１５】
2005316937.tif 000019
【図１６】
2005316937.tif 000020
【図１７】
2005316937.tif 000021
【図１８】
2005316937.tif 000022
【図１９】
2005316937.tif 000023
【図２０】
2005316937.tif 000024
【図２１】
2005316937.tif 000025
【図２２】
2005316937.tif 000026
【図２３】
2005316937.tif 000027