JP7088318B2 - 制御装置 - Google Patents

制御装置 Download PDF

Info

Publication number
JP7088318B2
JP7088318B2 JP2020565234A JP2020565234A JP7088318B2 JP 7088318 B2 JP7088318 B2 JP 7088318B2 JP 2020565234 A JP2020565234 A JP 2020565234A JP 2020565234 A JP2020565234 A JP 2020565234A JP 7088318 B2 JP7088318 B2 JP 7088318B2
Authority
JP
Japan
Prior art keywords
control
predetermined
target
obstacle
potential field
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Active
Application number
JP2020565234A
Other languages
English (en)
Other versions
JPWO2020145403A1 (ja
Inventor
守 恵木
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Omron Corp
Original Assignee
Omron Corp
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by Omron Corp filed Critical Omron Corp
Publication of JPWO2020145403A1 publication Critical patent/JPWO2020145403A1/ja
Application granted granted Critical
Publication of JP7088318B2 publication Critical patent/JP7088318B2/ja
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05B—CONTROL OR REGULATING SYSTEMS IN GENERAL; FUNCTIONAL ELEMENTS OF SUCH SYSTEMS; MONITORING OR TESTING ARRANGEMENTS FOR SUCH SYSTEMS OR ELEMENTS
    • G05B13/00—Adaptive control systems, i.e. systems automatically adjusting themselves to have a performance which is optimum according to some preassigned criterion
    • G05B13/02—Adaptive control systems, i.e. systems automatically adjusting themselves to have a performance which is optimum according to some preassigned criterion electric
    • G05B13/04—Adaptive control systems, i.e. systems automatically adjusting themselves to have a performance which is optimum according to some preassigned criterion electric involving the use of models or simulators
    • G05B13/048—Adaptive control systems, i.e. systems automatically adjusting themselves to have a performance which is optimum according to some preassigned criterion electric involving the use of models or simulators using a predictor
    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
    • B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
    • B25J9/00—Programme-controlled manipulators
    • B25J9/16—Programme controls
    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05B—CONTROL OR REGULATING SYSTEMS IN GENERAL; FUNCTIONAL ELEMENTS OF SUCH SYSTEMS; MONITORING OR TESTING ARRANGEMENTS FOR SUCH SYSTEMS OR ELEMENTS
    • G05B19/00—Programme-control systems
    • G05B19/02—Programme-control systems electric
    • G05B19/04—Programme control other than numerical control, i.e. in sequence controllers or logic controllers
    • G05B19/05—Programmable logic controllers, e.g. simulating logic interconnections of signals according to ladder diagrams or function charts
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06F—ELECTRIC DIGITAL DATA PROCESSING
    • G06F17/00—Digital computing or data processing equipment or methods, specially adapted for specific functions
    • G06F17/10—Complex mathematical operations
    • G06F17/18—Complex mathematical operations for evaluating statistical data, e.g. average values, frequency distributions, probability functions, regression analysis
    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05B—CONTROL OR REGULATING SYSTEMS IN GENERAL; FUNCTIONAL ELEMENTS OF SUCH SYSTEMS; MONITORING OR TESTING ARRANGEMENTS FOR SUCH SYSTEMS OR ELEMENTS
    • G05B2219/00—Program-control systems
    • G05B2219/10—Plc systems
    • G05B2219/14—Plc safety
    • G05B2219/14073—Real time modeling of plc behaviour, display pictogram of system

Description

本発明は、対象装置を所定の指令に対して追従させるための目標指令を供給する制御装置に関する。
対象装置を指令軌道に追従させて動かすために、一般的にはフィードバック制御が利用されている。例えば多関節ロボットにおいては、ロボットの制御装置により、フィードバック制御を用いてロボットの手先部の位置を予め設定（教示）された指令軌道に追従させるように、各関節軸のサーボモータの制御が行われる。ところが、一般的なフィードバック制御では、どうしても各サーボモータに応答遅れが生ずるため、ロボットの実際の軌跡が指令軌道からずれる問題がある。このような指令軌道に対するずれを抑制するために、モデル予測制御に関する技術が利用されている（例えば、非特許文献１を参照）。
また、対象装置を指令軌道に追従させているときに該対象装置に干渉し得る障害物が存在する場合、その障害物との衝突を回避するように対象装置を制御する必要がある。例えば、特許文献１に示す技術では、対象装置の周囲に存在する障害物の存在確率に基づいて確率ポテンシャル場を形成するとともにその確率ポテンシャル場の勾配に基づいて対象装置が進行すべき経路が決定される。同じように特許文献２に示す車両の衝突回避制御においても、障害物の存在確率に基づく確率ポテンシャル場が利用されている。
特開２００３－２４１８３６号公報 特開２０１１－１８６８７８号公報
Yuta Sakurai and Toshiyuki Ohtsuka: Offset Compensation of Continuous Time Model Predictive Control By Disturbance Estimation;システム制御情報学会論文誌, Vol.25, No. 7, pp.10-18(2012)
従来では、サーボ制御の対象である対象装置の出力を所定の指令に対して追従させるためにモデル予測制御を行う場合、該モデル予測制御で利用される予測モデルが、対象装置が有する一又は複数の制御軸の駆動モータの関節座標等のような一般化座標に基づいて設定されている。このように対象装置の一般化座標に基づいた予測モデルの設定は容易であるが、この場合、モデル予測制御における最適化が一般化座標を基準として行われるため、対象装置の出力が辿る軌跡が必ずしも最適化されたものとなるかは定かではない。これは、対象装置の出力は、対象装置全体に設定される一般化座標系に基づいて定義されるのが一般的であって、それは各制御軸の駆動モータと制御軸が関連する装置構造の幾何学関係により複雑に変動し得るからである。
また、予測モデルに対象装置の作業座標系の状態量と一般化座標系の状態量の両者を含め、ヤコビ行列を一般化座標のパラメータを含めて表現する手法も考えられる。しかし、ヤコビ行列が位置と角度の相関を与えるものではないため、サーボ制御が進むにつれて生じる誤差が拡大し、対象装置の出力の追従性が低下する問題が生じ得る。
本発明は、このような問題に鑑みてなされたものであり、モデル予測制御により、対象装置の出力を所定の指令に対して好適に追従させる技術を提供することを目的とする。
本発明においては、上記課題を解決するために、所定の指令に対象装置の出力を追従させるための指令を対象装置側に供給する制御装置において、予測モデルを用いたモデル予測制御は、対象装置に関連付けられた所定の作業座標系に従って行われ、対象装置側に供給される目標指令は、モデル予測制御で得られた出力をもとに、制御対象の逆運動学を用いて一般化座標系における目標指令を算出する構成を採用した。これにより、所定の指令への追従性を好適に高めることができる。
具体的に、本発明は、所定の作業座標系において、サーボ制御の対象である対象装置の出力を所定の指令に対して追従させる制御装置であって、前記対象装置は、一又は複数の制御軸を有し、該一又は複数の制御軸のそれぞれには、該制御軸をサーボ制御するための独立した一般化座標系が設定される。そして、前記制御装置は、前記対象装置を前記所定の作業座標系に基づいてモデル化した対象モデルを有し、該対象モデルを用いて該所定の作業座標系に従った出力をシミュレーションして出力する対象モデル制御部と、前記対象モデル制御部が有する前記対象モデルに関する、前記所定の作業座標系に基づいた所定の状態変数の値を取得する状態取得部と、前記所定の状態変数と前記対象モデル制御部への制御入力の相関を、前記所定の作業座標系に基づいた所定の状態方程式の形式で画定した予測モデルを有し、前記所定の指令を入力として、所定時間幅の予測区間において所定の評価関数に従って該予測モデルに基づいた前記モデル予測制御を行い、少なくとも該予測区間の初期時刻での前記制御入力の値を、前記所定の指令に対応する該対象モデル制御部への制御入力として出力するモデル予測制御部と、前記対象装置における前記一又は複数の制御軸が関連する装置構造の幾何学関係に従って、前記モデル予測制御部の出力が前記対象モデルに入力されて得られる、前記所定の作業座標系に基づいた該対象モデル制御部の出力から、前記一又は複数の制御軸のそれぞれに設定されている前記一般化座標系に基づいた、各制御軸への目標指令を算出する算出部と、前記一又は複数の制御軸への前記目標指令を、前記対象装置側へ供給する供給部と、を備える。
本発明の制御装置は、対象装置の出力を所定の指令に追従させるための目標指令を供給する制御装置であり、対象装置をモデル化した対象モデルを有する対象モデル制御部が備えられる。なお、対象装置は、一又は複数の制御軸を有する装置であり、装置の機械構成を前提としてこれらの制御軸が関連し合うことで、対象装置自体の出力が得られることになる。そこで、対象装置自体の出力は、一又は複数の制御軸の関連性を考慮して対象装置の全体を前提とした、所定の作業座標系で画定される。一方で、対象装置を形成する一又は複数の制御軸のそれぞれには、該制御軸をサーボ制御するための独立した一般化座標系が設定される。一般化座標系の一例として、各制御軸が回転駆動されるアクチュエータで構成される場合には、各制御軸の回転角度に対応する回転座標系（対象装置がロボットであれば関節座標系の場合もある）が例示できる。
また、対象モデル制御部は、当該対象モデルを用いて対象装置のシミュレーション処理を実行する。そして、上記モデル予測制御部は、対象モデル制御部に関連する所定の状態変数と、対象モデル制御部への制御入力との相関を画定した予測モデルを用いてモデル予測制御を行うように構成される。ここで、当該モデル予測制御では、各制御時刻で、所定時間幅の予測区間が設定され、その予測区間において所定の評価関数に従った所定の演算処理が行われ、少なくともその予測区間での初期時刻の、算出された制御入力値が実時間で生成され出力される。この生成された出力が対象モデル制御部に供されて、そこで上記シミュレーション処理が実行されることになる。また、当該モデル予測制御では、予測区間が制御時間の経過とともに移動していくことになり、いわゆるReceding Horizon制御が実行されることになる。このような構成により、所定の指令への追従のための制御入力を実時間で生成し、対象装置に対して出力することができる。
ここで、上記制御装置では、モデル予測制御における予測モデルは、対象装置に関連付けられた所定の作業座標系に基づいて画定されている。したがって、モデル予測制御における、所定の評価関数に従った最適化は、所定の作業座標系に基づいた最適化が行われることになり、対象装置の出力を所定の指令に好適に追従させやすくなる。一方で、当該モデル予測制御は所定の作業座標系に従ったものであるから、モデル予測制御部の出力を対象モデル制御部に入力してそこのシミュレーション処理で得られる信号、すなわち対象モデル制御部の出力は、対象装置を形成する一又は複数の制御軸にそのまま指令値として適用することはできない。そこで、算出部により、対象モデル制御部の出力から、一般化座標系に基づいた各制御軸の目標指令への変換のための算出処理が行われる。なお、当該算出処理は、対象装置における各制御軸が関連する装置構造の幾何学関係に従って行われる。そのため、演算誤差の蓄積に起因する対象装置の出力誤差（位置誤差）の発生を好適に回避することができ、以て対象装置の所定の指令への好適な追従性を得ることができる。
なお、このように算出部によって算出された一又は複数の制御軸のそれぞれへの目標指令は、供給部によって対象装置側に供給される。供給された各制御軸への目標指令は、例えば、サーボドライバ等の装置に供されてそこに形成されるサーボ制御構造等に従って、各制御軸が適宜サーボ制御され得る。
ここで、上記の制御装置において、前記予測モデル及び前記対象モデルは、それぞれ前記所定の作業座標系の座標軸に対応する単慣性モデルとして設定されてもよい。すなわち、算出部により最終的に各制御軸への目標指令として変換される、対象モデルを用いたシミュレーションにおいては、現実の対象装置における制御軸同士の幾何学的（構造的）関連性を排除し、各制御軸が独立したものと仮定して予測モデルと対象モデルの設定が行われる。このように予測モデルと対象モデルを構成することで、モデル予測制御に関連する処理負荷を軽減することができ、以て対象装置の追従性は十分に確保され得る。なお、このような構成を採用することで、予測モデルが実際の対象装置の構成から乖離し、モデル予測制御による最適化の精度が低下し得るが、供給部により供給されるのは目標指令（目標軌道）であるため、実際の制御装置の駆動に及ぼされる影響は大きくはなく、上記の処理負荷の軽減から得られるメリットを大きく下回り得る。
また、上述までの制御装置において、前記算出部は、前記装置構造の幾何学関係に従って前記対象モデル制御部の出力から前記目標指令として虚数を含む所定の複素数解が得られたとき、該所定の複素数解の実部を該目標指令として算出してもよい。このように算出部による算出処理で、所定の複素数解が得られるのは、本願発明は、先ず対象装置の出力が所定の作業座標系において最適化された上で、対象装置の装置構造に従って上記算出処理が行われるからである。そして、所定の複素数解が得られるときは、対象装置の装置構造が律速となって、モデル予測制御部の出力が入力されて得られる、対象モデル制御部の出力を物理上実現できない状態にある。そこで、このような場合には、暫定的に、上記のように所定の複素数解の実部を目標指令として算出することで、対象装置側への目標指令の供給を継続することができる。
ここで、上述までの制御装置は、前記所定の指令と、前記対象モデル制御部の出力との偏差が入力される積分器を更に備えてもよい。そして、前記モデル予測制御部は、前記偏差が入力された前記積分器の出力を入力として前記モデル予測制御を行い、前記予測モデルは、前記対象モデル制御部の出力と前記所定の指令との偏差と、所定の積分ゲインとの積で表される所定の積分項を含んでもよい。このような構成を採用することで、偏差に基づいたモデル予測制御が行われることになる。これにより、所定の指令への追従過渡応答をいたずらに劣化させることなく、定常偏差を効果的に解消することができる。また、上記制御装置は、予測モデルに所定の積分項を含ませることで定常偏差の解消を図るものであるから、制御系の設計に要する負荷を大きく軽減でき、対象装置の好適な追従制御が可能となる。従来技術のように定常偏差の要因となる外乱を推定するオブザーバ等を利用する場合は、そのパラメータ設計が困難であり、計算負荷が比較的大きくなるため、このような観点からも上記本発明の構成は有用である。
また、上記の制御装置において、前記所定の積分ゲインは、前記偏差の値が零を含む所定の第１範囲内に属するときは該偏差が小さくなるに従い大きくなるように調整され、該偏差の値が該所定の第１範囲に属さないときは零とされてもよい。この結果、偏差が大きいとき、すなわち、対象装置の出力が所定の指令に対して大きくずれている場合には、所定の積分ゲインは小さく調整され、モデル予測制御における積分量が過度に溜まらないように調整される。また、偏差が小さくなると、所定の積分ゲインは大きく調整されモデル予測制御において積分量に基づいたサーボ制御が促進されることになる。そして、このような所定の積分ゲインの調整を伴う、モデル予測制御における積分量に基づいたサーボ制御が行われるのを偏差の値が所定の第１範囲内に属する場合に限定することで、過渡応答時のオーバーシュートが好適に抑制され得る。なお、所定の第１範囲は、サーボ制御による所定の指令への追従性や許容し得るオーバーシュート量等を考慮して適宜設定することができる。
ここで、上述までの制御装置は、前記対象装置に対する障害物の位置を取得する取得部を、更に備えてもよい。そして、前記所定の評価関数により算出されるステージコストに、前記所定の状態変数に関するステージコストである状態量コストと、前記制御入力に関連するステージコストである制御入力コストと、前記対象装置の周りに前記障害物が存在し得る確率を表し且つ所定の評価位置に基づいて算出される確率ポテンシャル場に関連する障害物ステージコストとが含まれてよく、更に、前記モデル予測制御部は、前記対象装置と前記障害物とが、いずれか一方を中心とした所定の近接領域内に他方が属している近接状態に置かれている場合には前記障害物ステージコストを発生させて前記モデル予測制御を行い、該近接状態に置かれていない場合には該障害物ステージコストを発生させずに該モデル予測制御を行ってもよい。
対象装置を所定の指令へ追従させているときに、その近くに障害物が存在する場合には、当該障害物との衝突を好適に回避しつつ、その指令への追従を実現する必要がある。そこで、上記制御装置は、取得部によって障害物の位置を取得する。そして、障害物の位置を把握した上で、モデル予測制御のための所定の評価関数により算出されるステージコストに、状態量コストと制御入力コストに加えて、障害物に対応する障害物ステージコストを含める。そのため、所定の指令への追従のためのモデル予測制御において、障害物との相関における確率ポテンシャル場を作用させることができる。なお、上記制御装置では、このような確率ポテンシャル場の作用による障害物ステージコストは、対象装置と障害物が近接状態に置かれている場合に限って発生される。この結果、障害物の確率ポテンシャル場が形成する斥力（対象装置を遠ざけようとする作用）が対象装置に作用することで、障害物を回避できるとともに、不用意に対象装置へ上記斥力が作用するのを避けて対象装置の追従性の低下を抑制できる。
ここで、上記の制御装置において、前記対象装置に、該対象装置の周りに前記障害物が存在し得る確率を表した第１ポテンシャル場が、該対象装置の出力部位を含む所定部位の形状に応じた形状となり且つ該所定部位を囲むように設定されてもよく、前記障害物に、該障害物の周りに前記対象装置が存在し得る確率を表した第２ポテンシャル場が、該障害物の形状に応じた形状となり且つ該障害物の少なくとも一部を囲むように設定されてもよい。そして、前記モデル予測制御部は、前記第１ポテンシャル場と前記第２ポテンシャル場が少なくとも一部分において重複したときには、前記対象装置と前記障害物とが前記近接状態に置かれているとして、該第１ポテンシャル場と該第２ポテンシャル場の重複領域に属する少なくとも１つの前記所定の評価位置に基づいて算出される該第１ポテンシャル場及び該第２ポテンシャル場を前記確率ポテンシャル場として、前記障害物ステージコストを発生させて前記モデル予測制御を行い、前記第１ポテンシャル場と前記第２ポテンシャル場が全く重複していないときは、該対象装置と該障害物とが該近接状態に置かれていないとして該障害物ステージコストを発生させずに前記モデル予測制御を行ってもよい。なお、本願の開示において、一例として、第１ポテンシャル場及び第２ポテンシャル場は、二次元で設定される場合は円や楕円の形状を有してもよく、三次元で設定される場合は球や楕円体の形状を有してもよい。このようにポテンシャル場を円形状や楕円形状に設定すると、計算上、２つのポテンシャル場の重複判断をしやすくなる。
このように対象装置と障害物のそれぞれに、対応する楕円形状のポテンシャル場を設定することで、対象装置や障害物の形状に即したポテンシャル場を設定できる。そのため、障害物回避のための対象装置の変位がいたずらに大きくなってしまうことを抑制できる。また、障害物ステージコストの発生は、そのような第１ポテンシャル場と第２ポテンシャル場の少なくとも一部分が重複する重複領域内の所定の評価位置に基づいて行われるため、障害物との衝突を回避するための斥力を好適に発生させることができる。
ここで、対象装置と障害物の相対的な位置関係によっては、対象装置が、所定の指令への追従に関するサーボ制御においてその変位が停滞するデッドロック状態に陥る状況が発生し得る。デッドロック状態が生じると、対象装置は所定の指令に対して実質的に追従できない状態となるため、可及的速やかにデッドロック状態から抜け出すのが好ましい。そこで、上記の制御装置は、対象装置がデッドロック状態に置かれていることを検知する検知部を、更に備えてもよい。
そして、前記検知部により前記デッドロック状態が検知されると、前記第1ポテンシャル場と前記第２ポテンシャル場の少なくとも何れか一方の形状が変化され、そして、前記モデル予測制御部は、変形後の前記第１ポテンシャル場と前記第２ポテンシャル場との重複関係に基づいて前記モデル予測制御を行ってもよい。また別法として、前記検知部により前記デッドロック状態が検知されると、前記対象装置と前記障害物の少なくとも何れか一方に、該第１ポテンシャル場及び該第２ポテンシャル場とは異なる新しい一又は複数のポテンシャル場が追加され、そして、前記モデル予測制御部は、前記新しい一又は複数のポテンシャル場の一部又は全部が追加された、前記対象装置側のポテンシャル場である新しい第１ポテンシャル場と、該新しい一又は複数のポテンシャル場の他部が追加された、前記障害物側の新しい第２ポテンシャル場との重複関係に基づいて前記モデル予測制御を行ってもよい。このように対象装置がデッドロック状態に置かれる前後で、対象装置と障害物のそれぞれに設定されているポテンシャル場を調整することで、デッドロック状態の解消が期待される。
また、上記の制御装置において、前記モデル予測制御部は、前記モデル予測制御において該モデル予測制御部の出力を得るために前記所定の評価関数に従って所定演算を繰り返し実行するように構成されてもよく、その場合、前記検知部は、前記モデル予測制御において前記モデル予測制御部の一の出力を得るために繰り返された前記所定演算の回数が基準回数を超えたときに、前記対象装置が前記デッドロック状態に置かれていることを検知することができる。所定演算の回数が基準回数を超えることは、モデル予測制御において所定の評価関数に従った最適化された制御入力が速やかに導出されていない状態を意味することから、対象装置がデッドロック状態に置かれていると判断することは妥当と考えられる。
また、別法として、上記の制御装置において、前記対象装置と前記障害物とが前記近接状態に置かれている場合において、前記算出部により前記装置構造の幾何学関係に従って前記対象モデル制御部の出力から前記目標指令として虚数を含む所定の複素数解が得られたとき、前記検知部は、前記対象装置が前記デッドロック状態に置かれていることを検知することができる。対象装置と障害物とが近接状態に置かれている場合に算出部により所定の複素数解が得られることは、対象装置がその装置構造の幾何学関係により所定の指令に十分に追従できていないことを意味することから、対象装置がデッドロック状態に置かれていると判断することは妥当と考えられる。
モデル予測制御により、対象装置の出力を所定の指令に対して好適に追従させるための目標指令を供給することを可能とする。
制御装置である標準ＰＬＣを含む制御システムの概略構成、及び制御システムによるサーボ制御の対象となるロボットアームの概略構成を示す図である。 主に実施形態の標準ＰＬＣの制御構造を示す第１の図である。 ロボットアームの装置構造の幾何学関係を説明する図である。 主に実施形態の標準ＰＬＣの制御構造を示す第２の図である。 標準ＰＬＣからの指令に従って駆動されたロボットアームの動きを示す第１の図である。 標準ＰＬＣからの指令に従って駆動されたロボットアームの動きを示す第２の図である。 標準ＰＬＣからの指令に従って駆動されたロボットアームの動きを示す第３の図である。 標準ＰＬＣからの指令に従って駆動されたロボットアームの動きを示す第４の図である。 標準ＰＬＣからの指令に従って駆動されたロボットアームの動きを示す第５の図である。 確率ポテンシャル場の設定について説明するための図である。
＜適用例＞
実施形態に係る制御装置の適用例について、図１～図３に基づいて説明する。図１の上段（ａ）は、制御システムの概略構成図である。当該制御システムは、ネットワーク１と、サーボドライバ４と、標準ＰＬＣ(Programmable Logic Controller)５とを備える。サーボドライバ４は、２つの制御軸（関節）に対応する２台のモータ２ａ、２ｂを有するロボットアーム６をサーボ制御するためのサーボ制御装置である。なお、図１に示す制御システムは、１台のサーボドライバ４で２台の制御軸（モータ）を駆動可能に構成されているが、そのような構成に代えて、１つの制御軸毎に１台のサーボドライバ４が配置され、各サーボドライバ４がネットワーク１で接続される構成を採用することもできる。そして、サーボドライバ４に対して、ロボットアーム６を駆動制御するための目標指令が標準ＰＬＣ５から供給される。ロボットアーム６が対象装置に相当し、標準ＰＬＣ５が制御装置に相当する。
上記制御システムでは、標準ＰＬＣ５から送られてくる目標指令を用いて、ロボットアーム６の出力が所定の指令に追従するように、サーボドライバ４がロボットアーム６をフィードバック制御する。目標指令が供給されたサーボドライバ４は、モータ２ａ、２ｂに接続されている各エンコーダから出力されたフィードバック信号を受けることで、ロボットアーム６の出力が所定の指令に追従するように、モータ２に駆動電流を供給する。この供給電流は、交流電源からサーボドライバ４に対して送られる交流電力が利用される。本実施例では、サーボドライバ４は三相交流を受けるタイプのものであるが、単相交流を受けるタイプのものでもよい。なお、本願ではサーボドライバ４によるフィードバック制御の形態は特定のものに限定されない。また、サーボドライバ４の構成は、本願発明の核をなすものではないため、その詳細な開示は割愛する。
ここで、図１の下段（ｂ）に、ロボットアーム６の概略構成が開示されている。ロボットアーム６では、モータ２ａを第１関節として第１アーム３ａが回転駆動されるようにモータ２ａの出力軸に第１アーム３ａの一端が取り付けられる。更に、第１アーム３ａの他端にモータ２ｂが配置され、そのモータ２ｂを第２関節として第２アーム３ｂが回転駆動されるようにモータ２ｂの出力軸に第２アーム３ｂの一端が取り付けられる。そして、第２アーム３ｂの先端部３ｃはロボットアーム６の出力部であり、例えば、所定物を把持するためのエンドエフェクタが取り付けられる。そして、ロボットアーム６において、第１アーム３ａの回転面と第２アーム３ｂの回転面は同一平面であり、当該回転面を定義するための直交座標としてｘ１軸とｘ２軸が設定されている。当該直交座標が、ロボットアーム６に関連する作業座標となり、ロボットアーム６の先端部３ｃの位置を、当該直交座標上で画定することができる。
また、ロボットアーム６に組み込まれるモータ２ａ、２ｂは、例えば、ＡＣサーボモータである。なお、モータ２ａ、２ｂには図示しないエンコーダが取り付けられており、当該エンコーダによりモータ２ａ、２ｂの動作に関するパラメータ信号（位置信号）がサーボドライバ４にフィードバック送信され、そこでのフィードバック制御に供される。
ここで、図２に基づいて、標準ＰＬＣ５の制御構造について説明する。標準ＰＬＣ５は、ロボットアーム６の動作（モーション）に関する目標指令を生成し、サーボドライバ４へ供給する。標準ＰＬＣ５は、指令生成部５０、状態取得部５２、モデル予測制御部５３、対象モデル制御部５６、算出部５７、供給部５８の機能部を有している。そして、これらの機能部による各処理は、標準ＰＬＣ５に搭載されている演算処理装置によって演算実行される。また、図２に示す制御構造において、指令生成部５０によって生成される所定の指令はｘｆで参照され、対象モデル制御部５６への制御入力はｕで参照される。
指令生成部５０は、ロボットアーム６の先端部３ｃが追従すべき所定の指令ｘｆを生成する。この指令ｘｆは、ロボットアーム６に関連する作業座標系（図１（ｂ）を参照）に基づくものであり、後述のモデル予測制御部５３によるモデル予測制御に供されることになる。また、対象モデル制御部５６は、ロボットアーム６をモデル化した予測モデルに対応した、単慣性の対象モデルを有し、その対象モデルを用いてロボットアーム６の出力（先端部３ｃの位置等）をシミュレーションする。そのシミュレーション結果は、対象モデル制御部５６の出力ｙとされる。なお、対象モデル制御部５６の出力ｙは、後述の算出部５７へと送られる。
ここで、状態取得部５２及びモデル予測制御部５３について説明する。状態取得部５２は、モデル予測制御部５３によって行われるモデル予測制御に供される、ロボットアーム６に対応した対象モデル（対象モデル制御部５６が有する対象モデル）に関する状態ｘに含まれる状態変数の値を取得する。なお、当該対象モデルは、ロボットアーム６の動きに対応した作業座標系（図１（ｂ）に示す例では、２次元の作業座標系である。ただし、ロボットアーム６の動きが３次元的である場合には、３次元の作業座標系を設定することができる）における単慣性モデルに基づくものであり、当該状態変数は、ロボットアーム６に関連する作業座標系（図１（ｂ）を参照）に基づくものである。そして、モデル予測制御部５３は、状態取得部５２が取得するロボットアーム６に関する状態ｘと、自身の出力する対象モデル制御部５６への制御入力ｕとを用いて、モデル予測制御(Receding Horizon制御)を実行する。
詳細には、モデル予測制御部５３は、対象モデルに関する状態ｘと、ロボットアーム６への制御入力ｕとの相関を、下記の状態方程式（式１）で画定した予測モデルを有している。なお、下記式１に示す予測モデルも上記の対象モデルと同じように、ロボットアーム６が有する所定の物理的特徴を忠実に反映するものではなく、ロボットアーム６を、上記作業座標系に基づいたｘ１軸とｘ２軸のそれぞれに対応する単慣性の構成とみなして設定された単慣性モデルである。すなわち、ロボットアーム６において第１関節と第２関節との幾何学的（構造的）な関連性を排除し、作業座標系を画定する軸毎に、駆動されるロボットアーム６を表す適切な単一の質量を設定することで、当該単慣性モデルが得られる。なお、ここで言う適切な質量としては、ロボットアーム６全体の質量そのものとしてもよく、また、ロボットアーム６のサーボ制御における操作量飽和を避けるために、ロボットアーム６全体の質量よりも若干大きい質量を設定してもよい。このように対象モデルと予測モデルにおいて、それぞれ作業座標軸に対応した単慣性モデルを設定することで、ロボットアーム６の目標軌道生成のためのモデル予測制御に関連する処理負荷を軽減することができる。
Figure 0007088318000001
 ・・・（式１）
式１中のＭは、上記「適切な質量」としての、ロボットアーム６全体の質量である。
ここで、モデル予測制御部５３は、対象モデルに関する状態ｘとロボットアーム６への制御入力ｕとを入力として、所定の時間幅Ｔを有する予測区間において下記の式２に示す評価関数に従って、式１で表す予測モデルに基づいたモデル予測制御を行う。
Figure 0007088318000002
 ・・・（式２）
上記式２の右辺の第１項が終端コストであり、右辺の第２項がステージコストである。そして、当該ステージコストは、下記の式３で表すことができる。
Figure 0007088318000003
 ・・・（式３）
ただし、ｘｒｅｆ（ｋ）は時刻ｋにおける目標状態量を、ｘ（ｋ）は時刻ｋにおける計算上の状態量を表し、ｕｒｅｆ（ｋ）は時刻ｋにおける、定常状態での目標制御入力を、ｕ（ｋ）は時刻ｋにおける計算上の制御入力を表している。また、Ｑ及びＲは、それぞれステージコストにおける状態量の重みを表す係数（重み係数）、制御入力の重みを表す係数（重み係数）である。したがって、式３の右辺の第１項が、状態量に関するステージコストを意味し「状態量コスト」と称し、右辺の第２項が、制御入力に関するステージコストを意味し「制御入力コスト」と称する。
以上を踏まえてモデル予測制御において算出された、予測区間の初期時刻ｔでの制御入力ｕの値が、その時刻ｔでの、指令ｘｆに対応する対象モデル制御部５６への制御入力ｕとして出力される。そして、モデル予測制御では、その制御時刻において、都度、所定の時間幅Ｔの予測区間が設定されるとともに、式２の評価関数に従って当該制御時刻での対象モデル制御部５６への制御入力ｕが算出され、対象モデル制御部５６へ送られることになる。式２のような形の評価関数Ｊの値を最良とする操作量を求める問題は、最適制御問題として広く知られている問題であり、その数値解を算出するアルゴリズムが公知技術として開示されている。そのような技術として連続変形法が例示でき、例えば、公知の文献である「連続変形法とGMRES法を組み合わせた非線形Receding horizon制御の高速アルゴリズム（A continuation /GMRES method for fast computation of nonlinear receding horizon control）」{大塚敏之（T. Ohtsuka）, オートマティカ（ Automatica）, 第４０巻, ｐ563～574, ２００４. }に詳細が開示されている。
連続変形法では、下記の式４に示す、入力Ｕ（ｔ）に関する連立１次方程式を解くことでモデル予測制御における入力Ｕ（ｔ）が算出される。具体的には、式４を解き、dU/dtを数値積分して、入力Ｕ（ｔ）を更新していく。このように連続変形法では、反復計算を行わないため、各時刻での入力Ｕ（ｔ）を算出するための演算負荷を可及的に抑制することができる。
Figure 0007088318000004
 ・・・（式４）
ただし、Ｆ、Ｕ（ｔ）は、以下の式５で表される。
Figure 0007088318000005
 ・・・（式５）
ただし、Ｈはハミルトニアン、λは共状態、μは拘束条件Ｃ＝０のラグランジュ乗数である。
次に算出部５７について説明する。制御入力ｕが入力されて得られる、対象モデル制御部５６の出力ｙは、図１（ｂ）に示す作業座標系に基づくものである。一方で、ロボットアーム６を構成するモータ２ａ、２ｂは、回転駆動されるアクチュエータであり、それぞれのモータには、サーボドライバ４によるサーボ制御のために、作業座標系とは異なる独立した座標系である回転座標系が設定されている。したがって、出力ｙは、そのままではモータ２ａ、２ｂを駆動するための目標指令とはなり得ない。そこで、算出部５７は、対象モデル制御部５６の出力ｙから、各モータの回転座標系に基づいた目標指令への変換のための算出処理を行う。
算出部５７による算出処理は、ロボットアーム６の装置構造の幾何学関係に従って行われる。図３に、ロボットアーム６の装置構造を示す。なお、説明を簡便にするために、図３では、モータ２ａが配置される第１関節を作業座標系の原点に位置させている。第１アーム３ａの長さをＬ１とし、第２アーム３ｂの長さをＬ２、先端部３ｃの位置を（ｘ１，ｘ２）としたときに、ロボットアーム６の装置構造の幾何学関係を考慮すると、モータ２ａ、２ｂの回転角θ１、θ２は、下記の式６で表すことができる。なお、モータ２ａの回転角θ１は、ｘ１軸と第１アーム３ａとの間の角と定義され、モータ２ｂの回転角θ２は、第１アーム３ａと第２アーム３ｂとの間の角と定義される。
Figure 0007088318000006
 ・・・（式６）
ただし、式６中の関数ａｔａｎ２は、ａｔａｎ２（ｙ，ｘ）で表すとき直交座標系における（ｘ，ｙ）の偏角を返す関数である。
このように算出部５７により、対象モデル制御部５６の出力ｙから算出されたθ１、θ２は、モータ２ａ、２ｂをサーボ制御するために好適な、各モータの位置（角度）に関する目標指令である。そこで、算出部５７による算出結果は、供給部５８によってサーボドライバ４へと供給され、そこでのサーボ制御（フィードバック制御）に供されることになる。このように本実施形態の制御システムでは、標準ＰＬＣ５において行われるモデル予測制御は、ロボットアーム６に関連する作業座標系に基づいて行われる。そのため、ロボットアーム６の出力を、同じように作業座標系に基づいた所定の指令ｘｆに好適に追従させやすくなる。更に、算出部５７による上記算出処理を経由することで、作業座標系に基づいたモデル予測制御の結果を、回転座標系に基づいたモータ２ａ、２ｂのサーボ制御に好適に適用できる目標指令に変換することができる。このとき、当該算出処理は、ロボットアーム６の装置構造の幾何学関係に基づくことから、演算誤差の蓄積に起因するロボットアーム６の出力誤差（位置誤差）の発生を好適に回避することができる。このことは、ロボットアーム６の追従性を考慮する上で、極めて有用な効果である。
＜第１の構成例＞
本構成例に係る標準ＰＬＣ５の制御構造について、図４に基づいて説明する。本構成例の標準ＰＬＣ５では、上記適用例と同様にモデル予測制御部５３によるモデル予測制御が行われるが、その際に積分器５１の出力ｚが状態取得部５２によって取得され、当該モデル予測制御に供される。具体的には、指令生成部５０からの所定の指令ｘｆと、フィードバック系によってフィードバックされた対象モデル制御部５６の出力ｙとの偏差ｅ（ｅ＝ｘｆ－ｙ）が、積分器５１に入力される。そして、その積分器５１の出力ｚが、状態取得部５２を経てモデル予測制御部５３に入力される。したがって、状態取得部５２により、ロボットアーム６に関する上記の状態変数に出力ｚが加えられ、モデル予測制御部５３によるモデル予測制御に供される。
このように積分器５１を含む制御構造を踏まえ、本構成におけるモデル予測制御部５３が有する予測モデルは、例えば下記の式７に表すことができる。
Figure 0007088318000007
 ・・・（式７）
ただし、式７中のｘｆ１、ｘｆ２は、作業座標系におけるｘ１軸、ｘ２軸のそれぞれの目標位置を表している。
式７における（ｘｆ－ｙ）は偏差ｅを表している。そして、上記予測モデルには、偏差ｅ（ｘｆ－ｙ）と、積分ゲインＫｉとの積で表される積分項が含まれていることが理解できる。積分ゲインＫｉが、所定の積分ゲインに相当する。これにより、モデル予測制御を用いた標準ＰＬＣ５での処理において適用例で示した所定の指令ｘｆへの好適な追従性に関連する積分量を調整しやすくなり、従来のように外乱モデルの拡張やオブザーバゲインの設計等、容易ではない調整が必要な外乱オブザーバを利用することなく、オーバーシュートを抑制した所定の指令ｘｆへの追従性を高めることが容易となる。
ここで、式７に示す予測モデルに含まれている積分項の積分ゲインＫｉは、偏差ｅに基づいて調整することができる。具体的には、偏差ｅの大きさが小さくなるに従い、積分ゲインＫｉの値が大きくなるように、該積分ゲインＫｉを調整する。例えば、偏差ｅの大きさが所定のｅ０以上となる場合には、積分ゲインＫｉは０となり、偏差ｅの大きさがｅ０未満の範囲で積分ゲインＫｉに０より大きく１以下の値が設定されることになる。また、偏差ｅの大きさが０に近づくほど、積分ゲインＫｉの値が急峻に１に近づき、偏差ｅの大きさが０である場合には積分ゲインＫｉは１となるように積分ゲインＫｉの推移を設定してもよい。このように、積分ゲインＫｉが偏差ｅの大きさに基づいて調整可能とされることで、対象モデル制御部５６の出力ｙが指令ｘｆと比較的乖離している場合には、積分ゲインＫｉの値は小さく調整され、以て、追従性向上のための積分量が不用意に溜まらないように調整されることになる。また、対象モデル制御部５６の出力ｙと指令ｘｆとの乖離量が少なくなると、すなわち偏差ｅの大きさが小さくなると、積分ゲインＫｉの値が大きく調整されるため、ロボットアーム６の追従性を効果的に高めることができる。
積分ゲインＫｉの調整の別の態様について説明する。本態様でも、偏差ｅの大きさが小さくなるに従い積分ゲインＫｉの値が大きくなるように、該積分ゲインＫｉが調整されるが、その積分ゲインＫｉを用いた積分処理は、所定の条件が満たされる場合に限って行われる。
本実施例において、上記の所定の条件は、偏差ｅの値が零を含む所定の第１範囲内に属するときは、積分ゲインＫｉの調整が行われ、その調整された積分ゲインＫｉに従って積分処理が行われる。一方で、偏差ｅの値が所定の第１範囲に属さないときは積分ゲインＫｉを零とすることで、積分処理が実質的に行われないことになる。一例として、対象モデル制御部５６の出力が２次元である場合、所定の第１範囲は下に凸な関数ｆ（ｘ）で定義できる。このような場合、上述した積分ゲインＫｉは、下記のように設定することで、積分ゲインＫｉの調整が実現される。
Ｋｉ＝α（｜ｆ（ｘ）｜－ｆ（ｘ））
なお、αは、所定の係数である。
このように積分ゲインＫｉを関数で表すことで、上述した連続変形法によるモデル予測制御でのプログラム処理において、条件判断処理を行わずに積分ゲインＫｉの数値を調整する（例えば、Ｋｉ＝０とする）ことが可能になり、上記式４及び式５に従ったモデル予測制御のためのプログラム生成が簡便となる。
また、所定の第１範囲は上に凸な関数ｆ（ｘ）で定義することもできる。このような場合には、積分ゲインＫｉは下記のように設定することができる。
Ｋｉ＝α（｜ｆ（ｘ）｜＋ｆ（ｘ））
なお、対象モデル制御部５６の出力が３次元以上の多次元である場合も、上記の技術思想を適用することができる。
ここで、対象モデル制御部５６の出力が２次元の場合、所定の第１範囲を、作業座標系におけるｘ１軸及びｘ２軸の目標位置を中心とした所定半径（ｒ）の円内とすると、積分ゲインＫｉは、一例として下記の式８に従って設定することができる。
Figure 0007088318000008
 ・・・（式８）
式８に従うことで、対象モデル制御部５６の出力が目標位置（ｘｆ１，ｘｆ２）を中心とした所定半径ｒの円内にあるときに、すなわち偏差ｅが所定の第１範囲内にあるときに、積分ゲインＫｉが設定される一方で、対象モデル制御部５６の出力が当該円の外にあるときに、すなわち偏差ｅが所定の第１範囲の外にあるときに、積分ゲインＫｉが零となる。この結果、モデル予測制御における積分処理が、限定された領域で行われることになり、いたずらに積分量が溜まることに起因するオーバーシュートの発生を好適に抑制できる。
更に、本構成例では、標準ＰＬＣ５の制御構造には、取得部５４が形成されている。指令ｘｆに追従するように制御されるロボットアーム６の周囲に障害物が存在していると、ロボットアーム６には当該障害物との衝突を回避することが求められる。なお、当該障害物が移動しているか否かは問わない。そこで、取得部５４は、ロボットアーム６と障害物との衝突を回避させるために必要な情報を取得する。具体的には、取得部５４は、カメラ８の撮像を介して認識される障害物に関する所定のパラメータを取得する。所定のパラメータとしては、障害物の位置や、ロボットアーム６に対する障害物の接近方向等が挙げられる。例えば、取得部５４は、カメラ８により撮像される領域が既知とされることで、その撮像画像を画像処理すること等の従来技術により障害物の位置を取得できる。また、上記の障害物の接近方向については、取得された障害物の過去の位置と現在の位置から障害物の進行方向を算出し、障害物の進行方向とロボットアーム６の進行方向の相関から接近方法を取得することができる。
そして、ロボットアーム６と障害物との衝突を回避させるために、取得部５４により取得された情報に基づいて、ロボットアーム６の周囲において障害物が存在し得る確率を表す確率ポテンシャル場が算出され、それをモデル予測制御に反映させる。当該確率ポテンシャル場の算出そのものは公知の技術であるから、例えば特開２００３－２４１８３６号に記載の技術を利用して算出が可能である。具体的には、下記の式９に従って、上記の確率ポテンシャル場を、モデル予測制御におけるステージコスト（上記式２の右辺の第２項がステージコストである）に反映させる。
Figure 0007088318000009
 ・・・（式９）
なお、上記ＯＤは、障害物の位置（ｘｄ１，ｘｄ２）とロボットアーム６の先端部３ｃの位置（ｘ１，ｘ２）との間の距離を表し、Ｊｐは障害物による確率ポテンシャルを表し、Ｌはステージコストを表している。上記のステージコストＬにおいて、右辺の第３項が障害物ステージコストに相当する。
このようにモデル予測制御部５３によるモデル予測制御における所定の評価関数に、障害物に起因する確率ポテンシャル場を反映させることで、ロボットアーム６の追従性向上と障害物の回避とを両立することができる。
ここで、障害物回避のための障害物ステージコストの発生の別態様について説明する。本態様でも、上記確率ポテンシャル場を利用して障害物ステージコストを発生させることで障害物の回避が図られるが、その障害物ステージコストの発生処理は、所定の条件が満たされる場合に限って行われる。
上記の所定の条件は、ロボットアーム６と障害物とが、何れか一方を中心とした近接領域に他方が属している近接状態に置かれていることである。したがって、ロボットアーム６と障害物が近接状態に置かれているときに限って障害物ステージコストが発生され、そうではない場合、すなわち両者が十分に離れている場合には、障害物回避のための障害物ステージコストの発生は行われない。一例として、対象モデル制御部５６の出力が２次元である場合、下に凸な関数をｇ（ｘ）と定義したときに、ロボットアーム６と障害物のうち一方に、｜ｇ（ｘ）｜－ｇ（ｘ）の関数で表される確率ポテンシャル場を設定し、その確率ポテンシャルの場を算出するための評価位置を、ロボットアーム６と障害物のうち他方に設定し、又は、ロボットアーム６と障害物の近傍に設定する。このようにすることで、評価位置が｜ｇ（ｘ）｜－ｇ（ｘ）で定義される領域内に位置するとき、すなわちロボットアーム６と障害物が近接状態に置かれるときに限って確率ポテンシャル場が発生し、以て障害物ステージコストが発生する。一方で、当該領域の外に位置するときには確率ポテンシャル場は零となり、以て障害物ステージコストは発生しない。このようにすることで、上述した連続変形法によるモデル予測制御でのプログラム処理において、条件判断処理を行わずに障害物ステージコストを発生させることが可能になり、上記式４及び式５に従ったモデル予測制御のためのプログラム生成が簡便となる。
また、別法として、上に凸な関数をｇ（ｘ）と定義したときに、ロボットアーム６と障害物のうち一方に、｜ｇ（ｘ）｜＋ｇ（ｘ）の関数で表される確率ポテンシャル場を設定し、その確率ポテンシャルの場を算出するための評価位置を、ロボットアーム６と障害物のうち他方に設定し、又は、ロボットアーム６と障害物の近傍に設定してもよい。
ここで、対象モデル制御部５６の出力が２次元の場合、障害物ステージコストを算出するための確率ポテンシャル場は、一例として下記の式１０に従って設定することができる。
Figure 0007088318000010
 ・・・（式１０）
式１０に従うことで、ｘ１軸の軸長をｒｐ１としｘ２軸の軸長をｒｐ２とする楕円形状のポテンシャル場が、ロボットアーム６又は障害物の上の（ｘｃ１、ｘｃ２）を中心として設定され、そのポテンシャル場の評価位置は、（ｘ１、ｘ２）とされ、ポテンシャル場が設定されていない障害物又はロボットアーム６、もしくは両者の近傍に設定される。このような設定により、ロボットアーム６と障害物が近接状態にあるときに限って、モデル予測制御において障害物ステージコストが発生することになり、制御上、障害物を回避するための斥力が不用意にロボットアーム６に作用するのを避けてロボットアーム６の追従性の低下を抑制できる。
なお、対象モデル制御部５６の出力の次元数を一般化した場合、障害物ステージコストを算出するための確率ポテンシャル場は、一例として下記の式１１に従って設定することができる。
Figure 0007088318000011
 ・・・（式１１）
＜第２の構成例＞
本構成例に係る標準ＰＬＣ５による、障害物を回避するための目標指令の供給形態について説明する。本構成例では、図５に示すように、ロボットアーム６と障害物のそれぞれに、式１０に示す楕円形状のポテンシャル場を設定する。図５の上段（ａ）は、制御対象となるロボットアーム６と、それに対して障害物となり得る別のロボットアーム６０とにそれぞれ設定されているポテンシャル場の状態を表し、下段（ｂ）は、当該ポテンシャル場を拡大して示している。なお、別のロボットアーム６０は、ロボットアーム６と実質的に同一に構成され、２つの関節部によって、第１アーム３０ａと第２アーム３０ｂがＸ１軸、Ｘ２軸で画定される作業座標系に基づいて平面上を回転駆動される。
本構成例では、制御対象となるロボットアーム６においては、その第２アーム３ｂの形状に応じて、楕円形状の第１ポテンシャル場Ｐ１が設定される。すなわち、第２アーム３ｂに沿って、楕円形状の第１ポテンシャル場Ｐ１の長軸が延在するように、第１ポテンシャル場Ｐ１が設定される。これは、ロボットアーム６において、その出力部は、第２アーム３ｂの先端部３ｃに配置されていることから、第２アーム３ｂを障害物との衝突から保護する必要があることを考慮したものである。したがって、第２アーム３ｂはロボットアーム６の所定部位に相当する。更に、ロボットアーム６にとって障害物となり得る別のロボットアーム６０においても、第２アーム３０ｂに沿って、楕円形状の第２ポテンシャル場Ｐ２の長軸が延在するように、第２ポテンシャル場Ｐ２が設定される。これも、第１ポテンシャル場Ｐ１の設定と同じように、第２アーム３０ｂをロボットアーム６との衝突から保護する必要があることを考慮したものである。なお、楕円形状のポテンシャル場については、上記の式１０を参照されたい。また、ロボットアーム６と別のロボットアーム６０に設定されるポテンシャル場は同じ形状、大きさである必要はなく、各アームの形状や大きさに適合させるのが好ましい。
更に、この場合、ロボットアーム６に関するモデル予測制御において、障害物ステージコストを発生させるための評価位置を、第１ポテンシャル場Ｐ１と第２ポテンシャル場Ｐ２の重複領域（図５（ｂ）に示す斜線領域）Ｒ１に属する少なくとも作業座標系の１点とする。評価位置の設定の一例を以下に示す。なお、以下は例示であり、これら以外の位置に評価位置を設定しても構わない。
（１）第１ポテンシャル場Ｐ１の外郭線と第２ポテンシャル場Ｐ２の外郭線の交点ｐ１０とｐ２０の近傍であって、重複領域内の点
（２）第１ポテンシャル場Ｐ１の外郭線と第２ポテンシャル場Ｐ２の外郭線の交点ｐ１０とｐ２０の中間点
（３）（２）に示す中間点と第２アーム３ｂの中間点を結ぶ仮想線上の点であって、重複領域内の点
なお、本構成例では第１ポテンシャル場Ｐ１と第２ポテンシャル場Ｐ２の形状を楕円形状としているため、幾何学的に両者の重複判断を容易に行うことができる。更に、重複判断（交点ｐ１０、ｐ２０の算出なども含む）の計算において、何れかのポテンシャル場の中心が計算上の原点に位置するように、両ポテンシャル場を、平行移動、回転、スケール変換（後述の第５の構成例に開示の技術も参照）などの工夫を行えば、より容易に重複判断を行い得る。ただし、このことには両ポテンシャル場の形状を楕円形状に限定する意図は無く、両ポテンシャル場の形状に任意の形状を採用してもよく、また、制御対象と障害物の動きを考慮して両ポテンシャル場の形状を３次元形状で設定してもよい。
このように評価位置を設定することで、第１ポテンシャル場Ｐ１と第２ポテンシャル場Ｐ２とがその一部において重複した場合に、ロボットアーム６に関連するモデル予測制御において障害物ステージコストが発生することになる。すなわち、両ポテンシャル場の重複は、ロボットアーム６と別のロボットアーム６０とが近接状態に置かれていることを意味し、そのときに両者の衝突を回避するために、モデル予測制御において第１ポテンシャル場Ｐ１と第２ポテンシャル場Ｐ２を確率ポテンシャル場として障害物ステージコストが発生することになる。一方で、第１ポテンシャル場Ｐ１と第２ポテンシャル場Ｐ２とが重複しない場合には、評価位置の設定は行われず、モデル予測制御において障害物ステージコストは生じない。このような構成により衝突回避のために不用意な斥力を発生させることを抑制できる。また、ロボットアーム６と別のロボットアーム６０のそれぞれに、対応する楕円形状のポテンシャル場を設定するため、ロボットアーム６と別のロボットアーム６０の形状に即したポテンシャル場を設定でき、衝突回避のためのロボットアーム６の変位がいたずらに大きくなってしまうことを抑制できる。
ここで、本構成例に係る標準ＰＬＣ５による、障害物を回避するための目標指令の供給が行われた場合の、ロボットアーム６と別のロボットアーム６０の動きについて、図６に基づいて説明する。図６においては、ロボットアーム６はその先端部３ｃがスタート地Ｓ１から目標地Ｇ１に到達するように所定の指令ｘｆが生成される。また、別のロボットアーム６０は、ロボットアーム６にとっては障害物となり得るが、別のロボットアーム６０も、その先端部がスタート地Ｓ２から目標地Ｇ２に到達するように所定の指令が生成される。
図６の上段（ａ）は各ロボットアームが駆動開始しスタート地と目標地の概ね中間地点に移動した状態を表し、下段（ｂ）は各ロボットアームがそれぞれの目標地に到達した状態を表している。また、ロボットアーム６の先端部３ｃの軌跡は線Ｌ１で示され、別のロボットアーム６０の先端部の軌跡は線Ｌ２で示されている。上述したように、ロボットアーム６に関するモデル予測制御は、作業座標系に基づいて行われるため、スタート地Ｓ１を出発した直後は、ロボットアーム６の先端部３ｃは、目標地Ｇ１に向かって最適化された概ね直線上を進んでいる。しかし、その後、ロボットアーム６と別のロボットアーム６０とが近接状態に置かれると、両者の衝突を回避するための駆動制御が行われて、先端部３ｃの軌跡は曲がることになる。しかし、当該衝突が回避された後には、すなわちロボットアーム６と別のロボットアーム６０とが近接状態から解放されると、そのときの位置から目標地Ｇ１に向かって最適化された概ね直線上を進んでいる。なお、別のロボットアーム６０の動きについても同様である。このように、本構成例では、ロボットアーム６の高い追従性と、好適な衝突回避が実現される。
＜第３の構成例＞
本構成例に係る標準ＰＬＣ５による、障害物を回避するための目標指令の供給形態について説明する。上述までの開示で示したように、図５に示すように、標準ＰＬＣ５では、対象モデル制御部５６の出力ｙは、算出部５７によって、ロボットアーム６の各制御軸に対応する回転座標系の目標指令に変換されて、供給部５８により当該目標指令がサーボドライバ４に供給される。ここで、算出部５７は、ロボットアーム６の装置構造の幾何学関係に基づいて上記の式６に従って目標指令の算出を行っているため、その算出の結果、目標指令としてのθ１、θ２として、虚数を含む所定の複素数解が得られる場合がある。このことは、ロボットアーム６の装置構造が律速となって、モデル予測制御部５３の出力が入力されて得られる、対象モデル制御部５６の出力を物理上実現できない状態にあることを意味する。そこで、このような場合には、暫定的に、下記の式１２に示すように、上記複素数解の実部を目標指令として算出する。これにより、ロボットアーム６のサーボ制御が中断されることを回避できる。
Figure 0007088318000012
 ・・・（式１２）
ただし、式１２中の関数ｒｅａｌは、ｒｅａｌ（ＸＹ）で表すとき複素数解ＸＹの実部を返す関数である。
そして、本構成例に係る標準ＰＬＣ５による、障害物を回避するための目標指令の供給が行われた場合の、ロボットアーム６の動きについて、図７に基づいて説明する。なお、本構成例では、ロボットアーム６の先端部３ｃがスタート地Ｓ１から目標地Ｇ１に向かうように標準ＰＬＣ５による目標指令の供給が行われるが、障害物については、図７においては、関連する第２ポテンシャル場Ｐ２のみを記載している。図７の上段（ａ）では、算出部５７は式６に従って目標指令が算出されている。しかし、更に時間が経過すると、中段（ｂ）に示す状態となる。このとき、先ず式６に従って算出された目標指令は、虚数を含む複素数解となる。そこで、この場合は、改めて式１２に従うことで、当該複素数解の実部が目標指令として算出される。このとき、第１アーム３ａと第２アーム３ｂとの間の為す回転角θ２は零となり、図７（ｂ）に示すようにロボットアーム６は概ね真っ直ぐに伸びた状態となる。その後、更に時間が経過すると、式６に従って算出された目標指令は、虚数を含まない解となり、ロボットアーム６は下段（ｃ）に示す状態となる。すなわち、モデル予測制御部５３の出力が入力されて得られる、対象モデル制御部５６の出力を、ロボットアーム６において物理上実現できる状態に戻る。このように、ロボットアーム６のサーボ制御は中断されずに継続され、最終的には所定の指令ｘｆへの追従性を取り戻すことになる。
＜第４の構成例＞
本構成例に係る標準ＰＬＣ５による、障害物を回避するための目標指令の供給形態について説明する。本構成例では、図４に示す検知部５５について言及する。検知部５５は、ロボットアーム６がデッドロック状態に置かれていることを検知する。デッドロック状態とは、ロボットアーム６が所定の指令ｘｆに追従するためにサーボ制御が行われているにもかかわらず、その変位が停滞している状態を意味する。本構成例においては、ロボットアーム６は、障害物となり得る別のロボットアーム６０に対してデッドロック状態となり得る。
ここで、検知部５５によるデッドロック状態の検知に関する形態として、以下の２つの形態を例示できる。
（第１の検知形態）
上述の通り、モデル予測制御部５３は、そのモデル予測制御において該モデル予測制御部５３の出力である制御入力ｕを得るために、設定されている所定時間幅の予測区間において、式２に示す所定の評価関数に従って所定演算を繰り返し実行するように構成されている。所定演算とは、例えば、式５で示されるハミルトニアンＨの偏微分が零に近づくような制御入力ｕを探索する演算である。この場合、仮にロボットアーム６がデッドロック状態に置かれると、この所定演算が繰り返されながらも、評価関数を満たした、好適に最適化された制御入力が導出されない状態となる。この点を考慮して、検知部５５は、当該所定演算の回数が基準回数を超えたときに、ロボットアーム６が前記デッドロック状態に置かれていることを検知することができる。
（第２の検知形態）
別の形態として、ロボットアーム６と別のロボットアーム６０とが上記の近接状態に置かれている場合、すなわち第１ポテンシャル場Ｐ１と第２ポテンシャル場Ｐ２が少なくともその一部で重複している場合において、算出部５７により対象モデル制御部５６の出力ｙから目標指令として虚数を含む所定の複素数解が得られたときには、検知部５５は、ロボットアーム６がデッドロック状態に置かれていることを検知することができる。このように近接状態が形成されているときに算出部５７により虚数を含む複素数解が得られることは、ロボットアーム６の装置構造の幾何学関係が律速となってロボットアーム６が所定の指令ｘｆに十分に追従できていないことを意味することから、上記のようにデッドロック状態の検知が可能となる。
なお、本構成例では、上記の形態以外の検知形態を利用することも妨げられない。例えば、ロボットアーム６を構成する２つの関節軸の回転速度に基づいてデッドロック状態を検知しても構わない。
そして、検知部５５によりデッドロック状態が検知された場合、モデル予測制御部５３は、ロボットアーム６と障害物である別のロボットアーム６０に設定されているポテンシャル場を調整することで、デッドロック状態の解消が図られる。このデッドロック状態の解消形態として、以下の２つの形態を例示できる。
（第１の解消形態）
第１の解消形態について、図８に基づいて説明する。図８の上段（ａ）にはデッドロック状態に置かれているロボットアーム６と別のロボットアーム６０が示されている。この状態において、ロボットアーム６においては、第２アーム３ｂに沿って楕円形状の第１ポテンシャル場Ｐ１が設定されており、その楕円の長軸の延在方向は第２アーム３ｂの延在方向に概ね合致する。同じように、別のロボットアーム６０においては、第２アーム３０ｂに沿って楕円形状の第２ポテンシャル場Ｐ２が設定されており、その楕円の長軸の延在方向は第２アーム３０ｂの延在方向に概ね合致する。
このような状態から、モデル予測制御部５３は、ロボットアーム６の第２アーム３ｂに設定されている第１ポテンシャル場Ｐ１を、図８の下段（ｂ）に示すように変更する。具体的には、第２アーム３ｂに沿って楕円形状の第１ポテンシャル場Ｐ１は設定されるが、その楕円の短軸の延在方向が第２アーム３ｂの延在方向に概ね合致するように、第１ポテンシャル場Ｐ１の形状、大きさが変更される。変更後においても、第２アーム３ｂは、変更後の第１ポテンシャル場Ｐ１に囲まれた状態となっている。同様に、モデル予測制御部５３は、別のロボットアーム６０の第２アーム３０ｂに設定されている第２ポテンシャル場Ｐ２の形状、大きさも変更する。この結果、各ポテンシャル場の長軸方向が、図８（ａ）に示す状態と比べて９０度回転した状態となり、ロボットアーム６への衝突回避のための斥力の掛かり方に変化が生じるため、デッドロック状態の解消が期待される。なお、デッドロック状態が解消された後は、各ポテンシャル場の形状を再び元の形状に戻してもよく、又は、変更後のポテンシャル場の形状を維持してもよい。
（第２の解消形態）
第２の解消形態について、図９に基づいて説明する。図９の上段（ａ）にはデッドロック状態に置かれているロボットアーム６と別のロボットアーム６０が示されており、図８（ａ）に示す状態と同じである。このような状態から、モデル予測制御部５３は、図９の下段（ｂ）に示すようにロボットアーム６の第２アーム３ｂに、第１ポテンシャル場Ｐ１に加えて新たなポテンシャル場Ｐ１’を追加設定する。具体的には、第１ポテンシャル場Ｐ１はそのままの状態を維持し、更にロボットアーム６のモータ２ａが配置される第２関節を中心とした、下記の式１３で示される円形状のポテンシャル場Ｐ１’が追加設定される。
Figure 0007088318000013
 ・・・（式１３）
この円形のポテンシャル場Ｐ１’によっても、第２アーム３ｂを含むロボットアーム６全体が囲まれた状態となっている。同様に、モデル予測制御部５３は、別のロボットアーム６０の第２アーム３０ｂについても、第２ポテンシャル場Ｐ２に加えて新たな円形のポテンシャル場Ｐ２’を、その第２関節を中心に追加設定する。
この結果、ロボットアーム６への衝突回避のための斥力の掛かり方に変化が生じるため、デッドロック状態の解消が期待される。なお、デッドロック状態が解消された後は、追加された各ポテンシャル場を消去してもよく、又は、追加されたポテンシャル場を維持してもよい。
＜第５の構成例＞
上記の第２の構成例では、ロボットアーム６と障害物（別のロボットアーム６等）のそれぞれにポテンシャル場（第１ポテンシャル場Ｐ１、第２ポテンシャル場Ｐ２）を設定しその両者を本願の確率ポテンシャル場としたが、本構成例では、そのような態様に代えて、ロボットアーム６と障害物のそれぞれに対応する暫定のポテンシャル場に基づいて、１つの確率ポテンシャル場を生成し、その上で当該確率ポテンシャル場に関連する障害物ステージコストを反映させてモデル予測制御が行われる。以下に、本構成例における確率ポテンシャル場Ｊｐの生成について、詳細に説明する。
例示として、図５に示す一対のロボットアーム６、６０の例を用いて説明を行う。制御対象となるロボットアーム６に対応する暫定の第１ポテンシャル場Ｊｐ１は、その第２アーム３ｂの形状に応じてそれを囲むように楕円形状を有する領域とされ、下記の式１４で定義することができる。
Figure 0007088318000014
 ・・・（式１４）
更に、ロボットアーム６にとって障害物となり得る別のロボットアーム６０に対応する暫定の第２ポテンシャル場Ｊｐ２は、その第２アーム３０ｂを囲むように円形状を有する領域とされ、下記の式１５で定義することができる。
Figure 0007088318000015
 ・・・（式１５）
そして、本構成例での確率ポテンシャル場Ｊｐは、楕円形状の第１ポテンシャル場Ｊｐ１の外周に対して、円形状の第２ポテンシャル場Ｊｐ２をスライドさせたときに該第２ポテンシャル場Ｊｐ２の中心点の軌跡によって画定される領域とされ、下記の式１６で定義することができる。なお、本願において第２ポテンシャル場Ｊｐ２の中心点は、第２ポテンシャル場Ｊｐ２の形状の幾何学的な重心として定義される。
Figure 0007088318000016
 ・・・（式１６）
図１０に、式１６に基づいて画定される確率ポテンシャル場Ｊｐのイメージを示す。図１０において一点鎖線で示されるＪｐ１、Ｊｐ２は、それぞれ上記式１４、式１５に基づいて画定される暫定のポテンシャル場である。これらの暫定のポテンシャル場Ｊｐ１、Ｊｐ２は、モデル予測制御において実際に設定されるものではなく、実際に設定される確率ポテンシャル場Ｊｐの生成について説明のために、図１０において記載するものである。そして、本構成例では、制御対象と障害物の相関に基づいてモデル予測制御を用いて両者の衝突回避が図られるに当たって、各々に障害物ステージコストを算出するための確立ポテンシャル場を設定するのではなく、制御対象側にのみ確率ポテンシャル場Ｊｐを設定して実質的に等価のモデル予測制御が行われる。このような形態では、制御対象側の確率ポテンシャル場と障害物側の確率ポテンシャル場との重複判断（上記第２の構成例等を参照）を行う代わりに、モデル予測制御において、制御対象側に設定された確率ポテンシャル場Ｊｐに障害物の中心点が属することをもって、障害物ステージコストを発生させればよい（式１６を参照）。そのため、モデル予測制御に関する演算負荷を効果的に軽減することができる。
また、上記のように暫定の第２ポテンシャル場Ｊｐ２が円形状の場合、その中心点と外周との距離は常に一定（ｒｓｐ）であるが、暫定の第２ポテンシャル場Ｊｐ２の形状が非円形状であれば、その中心点と外周との距離は常に一定とならない。このような場合でも、上述した技術思想に基づいて確率ポテンシャル場Ｊｐを設定することができる。すなわち、確率ポテンシャル場Ｊｐは、楕円形状の第１ポテンシャル場Ｊｐ１の外周に対して、第２ポテンシャル場Ｊｐ２の中心点とその外周との距離が最大となるように、第１ポテンシャル場Ｊｐ１に対する第２ポテンシャル場Ｊｐ２の姿勢を調整しながら第２ポテンシャル場Ｊｐ２をスライドさせたときにその中心点の軌跡によって画定される領域とされてもよい。また、別法として、確率ポテンシャル場Ｊｐは、楕円形状の第１ポテンシャル場Ｊｐ１の外周に対して、第２ポテンシャル場Ｊｐ２の中心点とその外周との距離が最小となるように、第１ポテンシャル場Ｊｐ１に対する第２ポテンシャル場Ｊｐ２の姿勢を調整しながら第２ポテンシャル場Ｊｐ２をスライドさせたときにその中心点の軌跡によって画定される領域とされてもよい。更に別法として、確率ポテンシャル場Ｊｐは、楕円形状の第１ポテンシャル場Ｊｐ１の外周に対して、第２ポテンシャル場Ｊｐ２の中心点とその外周との距離が、上記の最大値と最小値との間の値になるように、第１ポテンシャル場Ｊｐ１に対する第２ポテンシャル場Ｊｐ２の姿勢を維持した状態で第２ポテンシャル場Ｊｐ２をスライドさせたときにその中心点の軌跡によって画定される領域とされてもよい。いずれの形態においても、モデル予測制御において、制御対象側に設定された確率ポテンシャル場Ｊｐに障害物の中心点が属することをもって障害物ステージコストを発生させ、以て、モデル予測制御に関する演算負荷を効果的に軽減することができる。
なお、図１０に示した例では２次元のポテンシャル場（円形状、楕円形状）が示されているが、ポテンシャル等の次元数は、制御対象と障害物の動き等に応じて適宜設定することができる。例えば、制御対象であるロボットアーム６、６０が３次元空間を移動するように構成される場合には、図１０に示す楕円形状のポテンシャル場は楕円体の形状を有するポテンシャル場に、円形状のポテンシャル場は球体の形状を有するポテンシャル場としてもよい。また、各ポテンシャル場の形状は、円形状、楕円形状、楕円体、球体以外の形状であってもよい。
＜その他の構成例＞
上述までの構成例で採用されているステージコストＬ（式９を参照）について、上記の状態量コスト、制御入力コスト、障害物ステージコストに加えて、更に、制御入力の上限と下限のうち少なくとも一方の閾値に関連する飽和関連コストＬｕを含めてもよい。一例として、飽和関連コストＬｕを含むステージコストＬは、下記の式１７に示す関数として定義することができる。
Figure 0007088318000017
 ・・・（式１７）
上記式１７では、飽和関連コストＬｕは、制御入力がその閾値である上限値ｕｍａｘを超える値となったときにその超過分が大きくなるほど該飽和関連コストＬｕの値が大きくなり、且つ、該制御入力が上限値ｕｍａｘ以下である場合には飽和関連コストＬｕの値が零になる関数として定義されている。このような飽和関連コストＬｕが予測制御におけるステージコストに反映されることで、制御入力がいたずらに大きく算出されてしまうことを抑制することができる。この結果、例えば、ロボットアーム６の障害物回避において、ロボットアーム６の回避軌跡を好適にコンパクトにすることができ、回避時のロボットアーム６の振動抑制にも貢献し得る。
＜付記１＞
所定の作業座標系において、サーボ制御の対象である対象装置（６）の出力を所定の指令に対して追従させる制御装置（５）であって、
前記対象装置（６）は、一又は複数の制御軸を有し、該一又は複数の制御軸のそれぞれには、該制御軸をサーボ制御するための独立した一般化座標系が設定され、
前記制御装置（５）は、
前記対象装置（６）を前記所定の作業座標系に基づいてモデル化した対象モデルを有し、該対象モデルを用いて該所定の作業座標系に従った出力をシミュレーションして出力する対象モデル制御部（５６）と、
前記対象モデル制御部（５６）が有する前記対象モデルに関する、前記所定の作業座標系に基づいた所定の状態変数の値を取得する状態取得部（５２）と、
前記所定の状態変数と前記対象モデル制御部（５６）への制御入力（ｕ）の相関を、前記所定の作業座標系に基づいた所定の状態方程式の形式で画定した予測モデルを有し、前記所定の指令を入力として、所定時間幅の予測区間において所定の評価関数に従って該予測モデルに基づいた前記モデル予測制御を行い、少なくとも該予測区間の初期時刻での前記制御入力（ｕ）の値を、前記所定の指令に対応する該対象モデル制御部への制御入力（ｕ）として出力するモデル予測制御部と、
前記対象装置（６）における前記一又は複数の制御軸が関連する装置構造の幾何学関係に従って、前記モデル予測制御部の出力が前記対象モデルに入力されて得られる、前記所定の作業座標系に基づいた該対象モデル制御部（５６）の出力から、前記一又は複数の制御軸のそれぞれに設定されている前記一般化座標系に基づいた、各制御軸への目標指令を算出する算出部（５７）と、
前記一又は複数の制御軸への前記目標指令を、前記対象装置側へ供給する供給部（５８）と、
を備える、制御装置（５）。
１・・・・ネットワーク
２ａ、２ｂ・・・・モータ
３ａ・・・・第１アーム
３ｂ・・・・第２アーム
３ｃ・・・・先端部
５・・・・標準ＰＬＣ
６・・・・ロボットアーム
５０・・・・指令生成部
５１・・・・積分器
５２・・・・状態取得部
５３・・・・モデル予測制御部
５４・・・・取得部
５５・・・・検知部
５６・・・・対象モデル制御部
５７・・・・算出部
５８・・・・供給部
６０・・・・別のロボットアーム（障害物）

Claims (18)

  1. 所定の作業座標系において、サーボ制御の対象である対象装置の出力を所定の指令に対して追従させる制御装置であって、
    前記対象装置は、一又は複数の制御軸を有し、該一又は複数の制御軸のそれぞれには、該制御軸をサーボ制御するための独立した一般化座標系が設定され、
    前記制御装置は、
    前記対象装置を前記所定の作業座標系に基づいてモデル化した対象モデルを有し、該対象モデルを用いて該所定の作業座標系に従った出力をシミュレーションして出力する対象モデル制御部と、
    前記対象モデル制御部が有する前記対象モデルに関する、前記所定の作業座標系に基づいた所定の状態変数の値を取得する状態取得部と、
    前記所定の状態変数と前記対象モデル制御部への制御入力の相関を、前記所定の作業座標系に基づいた所定の状態方程式の形式で画定した予測モデルを有し、前記所定の指令を入力として、所定時間幅の予測区間において所定の評価関数に従って該予測モデルに基づいたモデル予測制御を行い、少なくとも該予測区間の初期時刻での前記制御入力の値を、前記所定の指令に対応する該対象モデル制御部への制御入力として出力するモデル予測制御部と、
    前記対象装置における前記一又は複数の制御軸が関連する装置構造の幾何学関係に従って、前記モデル予測制御部の出力が前記対象モデルに入力されて得られる、前記所定の作業座標系に基づいた該対象モデル制御部の出力から、前記一又は複数の制御軸のそれぞれに設定されている前記一般化座標系に基づいた、各制御軸への目標指令を算出する算出部と、
    前記一又は複数の制御軸への前記目標指令を、前記対象装置側へ供給する供給部と、
    を備える、制御装置。
  2. 前記予測モデル及び前記対象モデルは、それぞれ前記所定の作業座標系の座標軸に対応する単慣性モデルとして設定される、
    請求項１に記載の制御装置。
  3. 前記算出部は、前記装置構造の幾何学関係に従って前記対象モデル制御部の出力から前記目標指令として虚数を含む所定の複素数解が得られたとき、該所定の複素数解の実部を該目標指令として算出する、
    請求項１又は請求項２に記載の制御装置。
  4. 前記一又は複数の制御軸のそれぞれは、回転駆動又は直線駆動されるアクチュエータで構成され、
    前記一般化座標系は、前記一又は複数の制御軸のそれぞれの回転角度に対応する回転座標系又は直線移動量に対応する直線座標系である、
    請求項１から請求項３の何れか１項に記載の制御装置。
  5. 前記所定の指令と、前記対象モデル制御部の出力との偏差が入力される積分器を更に備え、
    前記モデル予測制御部は、前記偏差が入力された前記積分器の出力を入力として前記モデル予測制御を行い、
    前記予測モデルは、前記対象モデル制御部の出力と前記所定の指令との偏差と、所定の積分ゲインとの積で表される所定の積分項を含む、
    請求項１から請求項４の何れか１項に記載の制御装置。
  6. 前記所定の積分ゲインは、前記偏差の値が零を含む所定の第１範囲内に属するときは該偏差が小さくなるに従い大きくなるように調整され、該偏差の値が該所定の第１範囲に属さないときは零とされる、
    請求項５に記載の制御装置。
  7. 前記対象モデル制御部の出力は、多次元の出力であり、
    前記所定の第１範囲は、下に凸な関数ｆ（ｘ）で定義され、
    前記所定の積分ゲインは、｜ｆ（ｘ）｜－ｆ（ｘ）の関数で表される、
    請求項５に記載の制御装置。
  8. 前記対象モデル制御部の出力は、多次元の出力であり、
    前記所定の第１範囲は、上に凸な関数ｆ（ｘ）で定義され、
    前記所定の積分ゲインは、｜ｆ（ｘ）｜＋ｆ（ｘ）の関数で表される、
    請求項５に記載の制御装置。
  9. 前記対象装置に対する障害物の位置を取得する取得部を、更に備え、
    前記所定の評価関数により算出されるステージコストに、前記所定の状態変数に関するステージコストである状態量コストと、前記制御入力に関連するステージコストである制御入力コストと、前記対象装置の周りに前記障害物が存在し得る確率を表し且つ所定の評価位置に基づいて算出される確率ポテンシャル場に関連する障害物ステージコストとが含まれ、
    前記モデル予測制御部は、前記対象装置と前記障害物とが、いずれか一方を中心とした所定の近接領域内に他方が属している近接状態に置かれている場合には前記障害物ステージコストを発生させて前記モデル予測制御を行い、該近接状態に置かれていない場合には該障害物ステージコストを発生させずに該モデル予測制御を行う、
    請求項１から請求項８の何れか１項に記載の制御装置。
  10. 前記対象モデル制御部の出力は、多次元の出力であり、
    下に凸な関数をｇ（ｘ）と定義したときに、前記対象装置と前記障害物のうち一方に、｜ｇ（ｘ）｜－ｇ（ｘ）の関数で表される前記確率ポテンシャル場が設定され、
    前記対象装置と前記障害物のうち他方に、又は、該対象装置及び該障害物の近傍に、前
    記所定の評価位置が設定される、
    請求項９に記載の制御装置。
  11. 前記対象モデル制御部の出力は、多次元の出力であり、
    上に凸な関数をｇ（ｘ）と定義したときに、前記対象装置と前記障害物のうち一方に、｜ｇ（ｘ）｜＋ｇ（ｘ）の関数で表される前記確率ポテンシャル場が設定され、
    前記対象装置と前記障害物のうち他方に、又は、該対象装置及び該障害物の近傍に、前記所定の評価位置が設定される、
    請求項９に記載の制御装置。
  12. 前記対象装置に、該対象装置の周りに前記障害物が存在し得る確率を表した第１ポテンシャル場が、該対象装置の出力部位を含む所定部位の形状に応じた形状となり且つ該所定部位を囲むように設定され、
    前記障害物に、該障害物の周りに前記対象装置が存在し得る確率を表した第２ポテンシャル場が、該障害物の形状に応じた形状となり且つ該障害物の少なくとも一部を囲むように設定され、
    前記モデル予測制御部は、
    前記第１ポテンシャル場と前記第２ポテンシャル場が少なくとも一部分において重複したときには、前記対象装置と前記障害物とが前記近接状態に置かれているとして、該第１ポテンシャル場と該第２ポテンシャル場の重複領域に属する少なくとも１つの前記所定の評価位置に基づいて算出される該第１ポテンシャル場及び該第２ポテンシャル場を前記確率ポテンシャル場として、前記障害物ステージコストを発生させて前記モデル予測制御を行い、
    前記第１ポテンシャル場と前記第２ポテンシャル場が全く重複していないときは、該対象装置と該障害物とが該近接状態に置かれていないとして該障害物ステージコストを発生させずに前記モデル予測制御を行う、
    請求項９に記載の制御装置。
  13. 前記対象装置が、前記所定の指令への追従に関する前記サーボ制御において該対象装置の変位が停滞するデッドロック状態に置かれていることを検知する検知部を、更に備え、
    前記検知部により前記デッドロック状態が検知されると、前記第1ポテンシャル場と前
    記第２ポテンシャル場の少なくとも何れか一方の形状が変化され、
    前記モデル予測制御部は、変形後の前記第１ポテンシャル場と前記第２ポテンシャル場との重複関係に基づいて前記モデル予測制御を行う、
    請求項１２に記載の制御装置。
  14. 前記対象装置が、前記所定の指令への追従に関する前記サーボ制御において該対象装置の変位が停滞するデッドロック状態に置かれていることを検知する検知部を、更に備え、
    前記検知部により前記デッドロック状態が検知されると、前記対象装置と前記障害物の少なくとも何れか一方に、該第１ポテンシャル場及び該第２ポテンシャル場とは異なる新しい一又は複数のポテンシャル場が追加され、
    前記モデル予測制御部は、前記新しい一又は複数のポテンシャル場の一部又は全部が追加された、前記対象装置側のポテンシャル場である新しい第１ポテンシャル場と、該新しい一又は複数のポテンシャル場の他部が追加された、前記障害物側の新しい第２ポテンシャル場との重複関係に基づいて前記モデル予測制御を行う、
    請求項１２に記載の制御装置。
  15. 前記モデル予測制御部は、前記モデル予測制御において該モデル予測制御部の出力を得るために前記所定の評価関数に従って所定演算を繰り返し実行するように構成され、
    前記検知部は、前記モデル予測制御において前記モデル予測制御部の一の出力を得るた
    めに繰り返された前記所定演算の回数が基準回数を超えたときに、前記対象装置が前記デッドロック状態に置かれていることを検知する、
    請求項１３又は請求項１４に記載の制御装置。
  16. 前記対象装置と前記障害物とが前記近接状態に置かれている場合において、前記算出部により前記装置構造の幾何学関係に従って前記対象モデル制御部の出力から前記目標指令として虚数を含む所定の複素数解が得られたとき、前記検知部は、前記対象装置が前記デッドロック状態に置かれていることを検知する、
    請求項１３又は請求項１４に記載の制御装置。
  17. 前記確率ポテンシャル場は、前記対象装置の周りに前記障害物が存在し得る確率を表した第１ポテンシャル場であって、該対象装置の出力部位を含む所定部位の形状に応じた形状となり且つ該所定部位を囲むように形成される該第１ポテンシャル場の外周に対して、前記障害物の周りに該対象装置が存在し得る確率を表した第２ポテンシャル場であって、該障害物の形状に応じた形状となり且つ該障害物の少なくとも一部を囲むように形成される該第２ポテンシャル場を仮にスライドさせたときに該第２ポテンシャル場の中心点の軌跡によって画定され、
    前記第２ポテンシャル場の中心位置は、前記所定の評価位置として設定され、
    前記モデル予測制御部は、
    前記第２ポテンシャル場の中心位置が前記確率ポテンシャル場の領域内にあるときに、前記対象装置と前記障害物とが前記近接状態に置かれているとして、前記確率ポテンシャル場に関連する前記障害物ステージコストを発生させて前記モデル予測制御を行い、
    前記第２ポテンシャル場の中心位置が前記確率ポテンシャル場の領域外にあるときに、該対象装置と該障害物とが該近接状態に置かれていないとして該障害物ステージコストを発生させずに前記モデル予測制御を行う、
    請求項９に記載の制御装置。
  18. 前記所定の評価関数により算出される前記ステージコストに、更に、前記制御入力の上限と下限のうち少なくとも一方の閾値に関連する飽和関連コストが含まれ、
    前記飽和関連コストは、前記制御入力が前記閾値を超える値となったときにその超過分が大きくなるほど該飽和関連コストの値が大きくなり、且つ、該制御入力が該閾値以下である場合には該飽和関連コストの値が零になる関数として定義される、
    請求項９から請求項１７の何れか１項に記載の制御装置。
JP2020565234A 2019-01-11 2020-01-10 制御装置 Active JP7088318B2 (ja)

Applications Claiming Priority (3)

Application Number Priority Date Filing Date Title
JP2019003607 2019-01-11
JP2019003607 2019-01-11
PCT/JP2020/000749 WO2020145403A1 (ja) 2019-01-11 2020-01-10 制御装置

Publications (2)

Publication Number Publication Date
JPWO2020145403A1 JPWO2020145403A1 (ja) 2021-11-18
JP7088318B2 true JP7088318B2 (ja) 2022-06-21

Family

ID=71521353

Family Applications (1)

Application Number Title Priority Date Filing Date
JP2020565234A Active JP7088318B2 (ja) 2019-01-11 2020-01-10 制御装置

Country Status (5)

Country Link
US (1) US20220075333A1 (ja)
JP (1) JP7088318B2 (ja)
CN (1) CN113260928A (ja)
DE (1) DE112020000380T5 (ja)
WO (1) WO2020145403A1 (ja)

Citations (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JP2017102617A (ja) 2015-11-30 2017-06-08 オムロン株式会社 補正装置、補正装置の制御方法、情報処理プログラム、および記録媒体
JP2017151687A (ja) 2016-02-24 2017-08-31 本田技研工業株式会社 移動体の経路計画生成装置
JP2018151899A (ja) 2017-03-14 2018-09-27 オムロン株式会社 制御装置、制御プログラムおよび制御システム
JP2019109598A (ja) 2017-12-15 2019-07-04 オムロン株式会社 制御システム
JP2019136817A (ja) 2018-02-09 2019-08-22 国立大学法人京都大学 多脚ロボットの登攀動作制御方法

Family Cites Families (17)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JP2610996B2 (ja) * 1989-05-26 1997-05-14 株式会社神戸製鋼所 多関節ロボット制御装置
JP3666578B2 (ja) * 2000-08-18 2005-06-29 株式会社安川電機 予測制御装置
AU2002331786A1 (en) * 2001-08-31 2003-03-18 The Board Of Regents Of The University And Community College System, On Behalf Of The University Of Coordinated joint motion control system
JP4054984B2 (ja) * 2001-12-20 2008-03-05 株式会社安川電機 ロボットの制御装置および制御方法
JP2003241836A (ja) 2002-02-19 2003-08-29 Keio Gijuku 自走移動体の制御方法および装置
EP2106886B1 (en) * 2002-04-26 2011-03-23 Honda Giken Kogyo Kabushiki Kaisha Self-position estimating device for leg type movable robots
WO2006011519A1 (ja) * 2004-07-27 2006-02-02 Kabushiki Kaisha Yaskawa Denki サーボ制御装置
JP2007076807A (ja) * 2005-09-14 2007-03-29 Institute Of Physical & Chemical Research パワーアシスト装置とその適応モデル予測制御方法
KR100986215B1 (ko) * 2009-04-15 2010-10-07 한국해양연구원 원격제어 수중로봇팔의 작업공간 구동 및 자동 제어 마스터 시스템
CN102681489B (zh) * 2012-06-01 2014-01-15 南京航空航天大学 多轴联动数控系统运动平稳性和轮廓加工精度控制方法
KR102157281B1 (ko) * 2013-03-29 2020-09-17 파나소닉 아이피 매니지먼트 가부시키가이샤 모터 구동 장치의 서보 조정 방법
JP5910647B2 (ja) * 2014-02-19 2016-04-27 トヨタ自動車株式会社 移動ロボットの移動制御方法
JP6421683B2 (ja) * 2015-04-14 2018-11-14 トヨタ自動車株式会社 最適制御装置、最適制御方法及び最適制御プログラム
JP6443311B2 (ja) * 2015-11-30 2018-12-26 オムロン株式会社 制御装置、制御プログラムおよび記録媒体
AU2016404622B2 (en) * 2016-04-29 2020-05-14 Softbank Robotics Europe A mobile robot with enhanced balanced motion and behavior capabilities
JP2018041150A (ja) * 2016-09-05 2018-03-15 オムロン株式会社 モデル予測制御装置、モデル予測制御装置の制御方法、情報処理プログラム、および記録媒体
DE102017114731A1 (de) * 2017-06-30 2019-01-03 Technische Universität Dortmund Verfahren zum regeln eines mechatronischen systems, regelungseinheit für ein mechatronisches system und mechatronisches system

Patent Citations (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JP2017102617A (ja) 2015-11-30 2017-06-08 オムロン株式会社 補正装置、補正装置の制御方法、情報処理プログラム、および記録媒体
JP2017151687A (ja) 2016-02-24 2017-08-31 本田技研工業株式会社 移動体の経路計画生成装置
JP2018151899A (ja) 2017-03-14 2018-09-27 オムロン株式会社 制御装置、制御プログラムおよび制御システム
JP2019109598A (ja) 2017-12-15 2019-07-04 オムロン株式会社 制御システム
JP2019136817A (ja) 2018-02-09 2019-08-22 国立大学法人京都大学 多脚ロボットの登攀動作制御方法

Also Published As

Publication number Publication date
DE112020000380T5 (de) 2021-09-23
WO2020145403A1 (ja) 2020-07-16
CN113260928A (zh) 2021-08-13
JPWO2020145403A1 (ja) 2021-11-18
US20220075333A1 (en) 2022-03-10

Similar Documents

Publication Publication Date Title
Lu et al. Collision-free and smooth joint motion planning for six-axis industrial robots by redundancy optimization
Abe Trajectory planning for residual vibration suppression of a two-link rigid-flexible manipulator considering large deformation
WO2022088593A1 (zh) 机械臂的控制方法、装置及人机协同模型的训练方法
JP4243309B2 (ja) ロボットアームの制御装置
KR102003216B1 (ko) 로봇을 위한 모터 제어 및/또는 조정
CN101612734A (zh) 管道喷涂机器人及其作业轨迹规划方法
JP2003241836A (ja) 自走移動体の制御方法および装置
Ogbemhe et al. Towards achieving a fully intelligent robotic arc welding: a review
CN115605328A (zh) 一种用于工业机器人的碰撞避免运动规划方法
JP7324932B2 (ja) 動的計画コントローラ
JP2020032481A (ja) ロボット制御装置、ロボット装置、ロボット制御のパラメータ調整方法、およびプログラム
CN113552877A (zh) 用于机器人优化动作规划的初始参考生成
WO2019139133A1 (ja) モデル予測制御のための制御パラメータの設定方法
JP5173958B2 (ja) 移動体姿勢生成方法および移動体姿勢生成装置
JP2009053926A (ja) 経路計画装置及び経路計画方法
JP2020052810A (ja) 制御装置
JP2019063912A (ja) ロボット制御データ処理方法、ロボット制御データ処理装置、およびロボットシステム
JP2005309990A (ja) 経路設定方法
JP7088318B2 (ja) 制御装置
Ying-Shi et al. Online minimum-acceleration trajectory planning with the kinematic constraints
Westkämper et al. Task-oriented programming of large redundant robot motion
JP2023075884A (ja) 動的動作計画システム
Lange et al. Predictive visual tracking of lines by industrial robots
Jin et al. A synthetic algorithm for tracking a moving object in a multiple-dynamic obstacles environment based on kinematically planar redundant manipulators
JP2023084062A (ja) ロボットの動的動作計画と制御の方法

Legal Events

Date Code Title Description
A621 Written request for application examination

Free format text: JAPANESE INTERMEDIATE CODE: A621

Effective date: 20210705

A131 Notification of reasons for refusal

Free format text: JAPANESE INTERMEDIATE CODE: A131

Effective date: 20220222

A521 Request for written amendment filed

Free format text: JAPANESE INTERMEDIATE CODE: A523

Effective date: 20220418

TRDD Decision of grant or rejection written
A01 Written decision to grant a patent or to grant a registration (utility model)

Free format text: JAPANESE INTERMEDIATE CODE: A01

Effective date: 20220510

A61 First payment of annual fees (during grant procedure)

Free format text: JAPANESE INTERMEDIATE CODE: A61

Effective date: 20220523

R150 Certificate of patent or registration of utility model

Ref document number: 7088318

Country of ref document: JP

Free format text: JAPANESE INTERMEDIATE CODE: R150