JP6992719B2 - 制御装置 - Google Patents

制御装置 Download PDF

Info

Publication number
JP6992719B2
JP6992719B2 JP2018182565A JP2018182565A JP6992719B2 JP 6992719 B2 JP6992719 B2 JP 6992719B2 JP 2018182565 A JP2018182565 A JP 2018182565A JP 2018182565 A JP2018182565 A JP 2018182565A JP 6992719 B2 JP6992719 B2 JP 6992719B2
Authority
JP
Japan
Prior art keywords
obstacle
actual
predetermined
controlled object
control
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
JP2018182565A
Other languages
English (en)
Other versions
JP2020052810A (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
Priority to JP2018182565A priority Critical patent/JP6992719B2/ja
Priority to CN201980051701.9A priority patent/CN112513747B/zh
Priority to PCT/JP2019/038237 priority patent/WO2020067460A1/ja
Priority to EP19864043.5A priority patent/EP3859456A4/en
Priority to US17/267,033 priority patent/US11467547B2/en
Publication of JP2020052810A publication Critical patent/JP2020052810A/ja
Application granted granted Critical
Publication of JP6992719B2 publication Critical patent/JP6992719B2/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
    • 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
    • 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/13—Plc programming
    • G05B2219/13099—Function block, OOP, various functions grouped, called by name as servo
    • 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/13—Plc programming
    • G05B2219/13186—Simulation, also of test inputs

Landscapes

  • Engineering & Computer Science (AREA)
  • Health & Medical Sciences (AREA)
  • Artificial Intelligence (AREA)
  • Computer Vision & Pattern Recognition (AREA)
  • Evolutionary Computation (AREA)
  • Medical Informatics (AREA)
  • Software Systems (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Automation & Control Theory (AREA)
  • Feedback Control In General (AREA)

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制御が実行されることになる。そして、予測モデルには、制御対象に関連する所定の状態変数と制御入力との相関が反映される。このような構成により、所定の目標指令への追従のための制御入力を実時間で生成し、制御対象に対して出力することができる。
ここで、制御対象を目標指令へ追従させているときに、その近くを移動する障害物が存在する場合には、当該障害物との衝突を好適に回避しつつ、その目標指令への追従を実現する必要がある。そこで、上記制御装置は、第１取得部によって実際の障害物である実障害物の位置を取得するとともに、設定部によって、その実障害物に対応する仮想障害物の位置を設定する。当該仮想障害物は、実際に存在する障害物ではなく、実障害物に対応して目標指令への追従のためのモデル予測制御の演算のために設定される仮想的な障害物である。仮想障害物の位置は、制御対象の追従目標軌道を基準として、実障害物と略対称となる位置に設定される。これは、実障害物を起点とした確率ポテンシャル場と仮想障害物を起点とした確率ポテンシャル場とを、追従目標軌道を挟んで対称的に配置できるようにするためである。なお、このことは、必ずしも両ポテンシャル場を全く同一に形成することを意味するものではない。
そして、このように実障害物と仮想障害物の位置を把握した上で、モデル予測制御のための所定の評価関数により算出されるステージコストに、状態量コストと制御入力コストに加えて、実障害物に対応する第１ステージコストと、仮想障害物に対応する第２ステージコストとを含める。そのため、目標指令への追従のためのモデル予測制御において、実
障害物との相関における第１確率ポテンシャル場と、追従目標軌道を挟んで略対称に設定された仮想障害物との相関における第２確率ポテンシャル場とを作用させることができる。この結果、実障害物の第１ポテンシャル場が形成する斥力（制御対象を遠ざけようとする作用）に加えて、仮想障害物の第２ポテンシャル場が形成する斥力が制御対象に作用することで制御対象の目標指令への追従速度が好適に低減され、制御対象を実障害物の後方（進行方向の反対方向）に位置するようにその実障害物を回避させることができる。このことは、従来技術のように、制御対象が障害物と並走する形となってしまうことを抑制するものであり、好適に実障害物との衝突を回避することができるようになる。
特に、上記の制御装置では、第２ポテンシャル場は、第１ポテンシャル場以上の確率値を有するように両ポテンシャル場が設定される。このことは、仮想障害物の第２ポテンシャル場が形成する斥力は、実障害物の第１ポテンシャル場が形成する斥力以上であることを意味する。そのため、上述したようなステージコストが算出されるモデル予測制御によって、制御対象の目標指令への追従速度が好適に低減され実障害物の回避が好適に実現されることになる。
ここで、上記課題を解決する本発明の第２の側面から捉える。第２の側面から捉えられた本発明は、制御対象に関する所定の状態変数と該制御対象への制御入力との相関を所定の状態方程式の形式で画定した予測モデルを有し、該制御対象の出力が追従すべき所定の目標指令に対して、所定時間幅の予測区間において所定の評価関数に従って該予測モデルに基づいたモデル予測制御を行い、少なくとも該予測区間の初期時刻での前記制御入力の値を出力するモデル予測制御部を備え、該制御対象の出力を該所定の目標指令に対して追従させる制御装置である。そして、当該制御装置は、前記所定の目標指令と、前記制御対象の出力との偏差が入力されるサーボ用積分器と、前記制御対象に対する実際の障害物である実障害物の位置を取得する第１取得部と、前記所定の目標指令に基づく前記制御対象の追従目標軌道を基準として、前記実障害物に関連付けられた仮想障害物が該実障害物とは略対称に位置するように、前記第１取得部によって取得された前記実障害物の位置に基づいて該仮想障害物の位置を設定する設定部と、を備え、前記所定の評価関数により算出されるステージコストに、前記所定の状態変数に関するステージコストである状態量コストと、前記制御入力に関連するステージコストである制御入力コストと、前記実障害物の位置に基づいて該実障害物が存在し得る確率を表した第１確率ポテンシャル場に関連する第１ステージコストと、前記仮想障害物の位置に基づいて該仮想障害物が存在し得る確率を表した確率ポテンシャル場であって該第１確率ポテンシャル場以上の確率値を有する第２確率ポテンシャル場に関連する第２ステージコストとが含まれる。更に、前記制御対象に関連する状態変数に、前記偏差と所定の積分ゲインとの積で表される所定の積分項が含まれ、前記所定の積分ゲインは、前記制御対象と前記実障害物との間の距離が短くなるほど小さくなる。
第２の側面による制御装置は、サーボ用積分器を更に備える点で第１の側面による制御装置と異なる。このような構成を採用することで、偏差に基づいたモデル予測制御が行われることになる。これにより、所定の目標指令への追従過渡応答をいたずらに劣化させることなく、定常偏差を効果的に解消することができる。また、上記制御装置は、予測モデルに所定の積分項を含ませることで定常偏差の解消を図るものであるから、制御系の設計に要する負荷を大きく軽減でき、制御対象の好適な追従制御が可能となる。従来技術のように定常偏差の要因となる外乱を推定するオブザーバ等を利用する場合は、そのパラメータ設計が困難であり、計算負荷が比較的大きくなるため、このような観点からも上記本発明の構成は有用である。
ただし、上記状態変数にこのような所定の積分項が含まれることで、モデル予測制御において実障害物の第１ポテンシャル場が形成する斥力と仮想障害物の第２ポテンシャル場
が形成する斥力を制御対象に作用させていたとしても、時間が経過したり、目標指令の推移により制御対象の位置と目標位置との間のずれが広がったりすると相対的に所定の積分項による影響が強くなる。そのため、仮想障害物の第２ポテンシャル場が形成する斥力の効果が相対的に弱まり、結果として、制御対象を実障害物の後方を回避するように制御しにくくなる。そこで、第２の側面による制御装置では、所定の積分項に含まれる所定の積分ゲインが、制御対象と実障害物との間の距離が短くなるほど小さくされる。このようにすることで、制御対象が実障害物に近接し衝突の可能性が高まっている場合には、実障害物及び仮想障害物のそれぞれのポテンシャル場が形成する斥力の効果を相対的に高めることができ、以て、目標指令の追従を実現しながら、実障害物との衝突を好適に回避することができる。
ここで、上述までの制御装置において、実障害物の追従目標軌道に対する進入角が９０度に近くなるほど、モデル予測制御における、実障害物の第１ポテンシャル場が形成する斥力と仮想障害物の第２ポテンシャル場が形成する斥力との制御対象への作用が弱まる傾向がある。その結果、制御対象が実障害物に比較的近接した場合でもその追従速度を好適に低減できず、制御対象と実障害物との衝突が回避しにくくなる。そこで、前記制御対象の移動方向と前記実障害物の移動方向との間の角度として定義される、該実障害物の前記追従目標軌道に対する進入角を取得する第２取得部を更に備えてもよく、その場合、前記所定の評価関数により算出されるステージコストにおいて、前記進入角が９０度に近くなるほど前記第２ステージコストは大きくなるように算出されてもよい。このように第２ステージコストを進入角に依存して算出することで、実障害物の進入角に影響されずに実障害物と制御対象との衝突を回避することが可能となる。
なお、上記の制御装置において、前記進入角は、前記第１取得部によって取得されてきた前記実障害物の過去の位置に基づいて算出されてもよい。このようにすることで、本来的には、その進路を予め知ることのできない実障害物について追従目標軌道への進入角を予測することができる。なお、一例としては、直近の２点の実障害物の位置からその動きを直線近似し、又は直近の複数点の実障害物の位置からその動きを多項式近似することで、追従目標軌道への進入角を算出することが可能となる。
ここで、上述までの制御装置において、前記制御対象の位置が、前記追従目標軌道と前記実障害物が辿る障害物軌道とが交差する交差点の位置を過ぎた後は、前記所定の評価関数により算出されるステージコストにおいて、該制御対象の位置が該交差点の位置から遠くなるほど前記第２ステージコストは小さくなるように算出されてもよい。制御対象が追従目標軌道を進む過程において該制御対象の位置が上記交差点の位置を過ぎた場合、基本的には、制御対象と実障害物との衝突は回避されたことになる。そこで、上記のように制御対象の位置が交差点の位置を経過した後において当該交差点から遠くなるほど前記第２ステージコストが小さくなるように算出されることで、仮想障害物の第２ポテンシャル場が形成する斥力が不用意に制御対象に作用してしまうのを回避し、以て好適な制御対象の追従制御を実現することが可能となる。また、別法として、上述までの制御装置において、前記制御対象の位置が、前記追従目標軌道と前記実障害物が辿る障害物軌道とが交差する交差点の位置を過ぎた後は、前記所定の評価関数により算出されるステージコストにおいて、前記第２ステージコストは零とされてもよい。
ここで、上述までの制御装置において、目標指令への追従を行う制御対象に関し、複数の実障害物が存在する場合、設定部によりそれぞれの実障害物に対応する仮想障害物の位置を設定し、各実障害物及び各仮想障害物を、モデル抑制制御のステージコストの算出に反映させることで、実障害物の数が複数であってもそれらを好適に回避しつつ、制御対象の目標指令への追従を実現できる。ただし、多くの実障害物に対応して上記のモデル予測制御を行おうとすると、当該モデル予測制御に要する、制御装置の処理負荷が高くなるた
め、制御装置において対応可能な実障害物の数を所定数に限定するのが好ましい。
そして、そのように制御装置でのモデル予測制御において対応可能な実障害物の数が所定数に限定されることを踏まえて、前記所定の評価関数は、２以上の所定数の前記実障害物のそれぞれに対応する前記第１ステージコスト及び前記第２ステージコストが算出されるように形成されてもよい。そして、前記第１取得部により前記所定数を超える所定超過数の前記実障害物の位置が取得された場合、該所定超過数の実障害物の中から前記制御対象と該所定超過数の実障害物のそれぞれとの離間距離に基づいて該所定数の実障害物が抽出され、更に、前記所定の評価関数により算出されるステージコストにおいて、該抽出された該所定数の実障害物の位置に基づいて前記第１ステージコストが算出され、且つ、該抽出された該所定数の実障害物に対応する該所定数の仮想障害物の位置に基づいて前記第２ステージコストが算出されてもよい。すなわち、所定数を超えた実障害物が存在している場合には、制御対象との離間距離に基づいてモデル予測制御に優先的に反映させる実障害物が抽出される。これは、制御対象と実障害物との離間距離は、衝突回避のために重要な要素であるからである。このような構成を採用することで、制御装置の処理負荷を可及的に抑えながら、制御対象と実障害物との衝突を回避しつつ、目標指令への追従制御を実現することができる。
モデル予測制御により制御対象の出力を目標指令に対して追従させる場合において、移動する障害物との衝突を好適に回避することを可能とする。
制御装置であるサーボドライバを含む制御システムの概略構成を示す第１の図である。 実施形態のサーボドライバの制御構造を示す第１の図である。 実施形態のサーボドライバで実行されるモデル予測制御において実障害物との衝突回避を行うための概略を説明する図である。 実施形態のサーボドライバによる実プラントの追従制御の結果を示す第１の図である。 実施形態のサーボドライバの制御構造を示す第２の図である。 図５に示すサーボドライバでのモデル予測制御で設定される第１及び第２の積分ゲインの推移を表す図である。 実施形態のサーボドライバによる実プラントの追従制御の結果を示す第２の図である。 実施形態のサーボドライバによる実プラントの追従制御の結果を示す第３の図である。 実施形態のサーボドライバによる実プラントの追従制御の結果を示す第４の図である。 実施形態のサーボドライバによる実プラントの追従制御の結果を示す第５の図である。
＜適用例＞
実施形態に係る制御装置の適用例について、図１～図３に基づいて説明する。図１は、制御システムの概略構成図である。当該制御システムは、ネットワーク１と、サーボドライバ４と、標準ＰＬＣ(Programmable Logic Controller)５とを備える。サーボドライバ
４は、モータ２と負荷装置３とを含んでなる、実際のプラント（以下、単に「実プラント」と称する）６をサーボ制御するための制御装置である。実プラント６が制御対象となる。当該制御システムでは、標準ＰＬＣ５から送られてくる目標指令に、実プラント６の出
力を追従させるように、サーボドライバ４が実プラント６をフィードバック制御する。サーボドライバ４は、標準ＰＬＣ５から受けた目標指令に基づき、実プラント６の追従制御を行うための制御入力を生成する。サーボドライバ４による制御入力の生成については、後述する。ここで、実プラント６を構成する負荷装置３としては、各種の機械装置（例えば、産業用ロボットのアームや搬送装置）が例示でき、モータ２はその負荷装置３を駆動するアクチュエータとして負荷装置３内に組み込まれている。例えば、モータ２は、ＡＣサーボモータである。なお、モータ２には図示しないエンコーダが取り付けられており、当該エンコーダによりモータ２の動作に関するパラメータ信号（位置信号、速度信号等）がサーボドライバ４にフィードバック送信されている。
標準ＰＬＣ５は、実プラント６の動作（モーション）に関する目標指令を生成し、サーボドライバ４へ送信する。サーボドライバ４は、ネットワーク１を介して標準ＰＬＣ５から当該サーボ指令を受けるとともに、モータ２に接続されているエンコーダから出力されたフィードバック信号を受ける。そして、サーボドライバ４は、当該サーボ指令とエンコーダからのフィードバック信号に基づいて、実プラント６の出力が所定の指令に追従するように、モータ２に駆動電流を供給する。この供給電流は、交流電源からサーボドライバ４に対して送られる交流電力が利用される。本実施例では、サーボドライバ４は三相交流を受けるタイプのものであるが、単相交流を受けるタイプのものでもよい。なお、実プラント６のサーボ制御のために、サーボドライバ４において、図２に示すようにモデル予測制御部４３によるモデル予測制御が実行される。
ここで、図２に基づいて、サーボドライバ４の制御構造について説明する。なお、標準ＰＬＣ５からサーボドライバ４に供給される目標指令はｒで参照され、実プラント６への制御入力はｕで参照される。サーボドライバ４は、状態取得部４２、モデル予測制御部４３、取得部４５、設定部４６を有している。そして、これらの状態取得部４２、モデル予測制御部４３、取得部４５、設定部４６による各処理は、サーボドライバ４に搭載されている演算処理装置によって演算実行される。
そして、本実施例では、状態取得部４２及びモデル予測制御部４３が形成されている。状態取得部４２は、モデル予測制御部４３によって行われるモデル予測制御に供される、実プラント６に関する状態ｘに含まれる状態変数の値を取得する。そして、モデル予測制御部４３は、状態取得部４２が取得する実プラント６に関する状態ｘと、自身の出力する実プラント６への制御入力ｕとを用いて、モデル予測制御(Receding Horizon制御)を実行する。
詳細には、モデル予測制御部４３は、実プラント６に関する状態ｘと、実プラント６への制御入力ｕとの相関を、下記の状態方程式（式１）で画定した予測モデルを有している。なお、下記式１は、非線形の状態方程式である。当該予測モデルには、例えば、実プラント６が有する所定の物理的特徴が反映されてもよい。
Figure 0006992719000001
・・・（式１）
ここで、モデル予測制御部４３は、実プラント６に関する状態ｘと実プラント６への制御入力ｕとを入力として、所定の時間幅Ｔを有する予測区間において下記の式２に示す評価関数に従って、式１で表す予測モデルに基づいたモデル予測制御を行う。
Figure 0006992719000002
・・・（式２）
上記式２の右辺の第１項が終端コストであり、右辺の第２項がステージコストである。そして、当該ステージコストは、下記の式３で表すことができる。
Figure 0006992719000003
・・・（式３）
ただし、ｘｒｅｆ（ｋ）は時刻ｋにおける目標状態量を、ｘ（ｋ）は時刻ｋにおける計算上の状態量を表し、ｕｒｅｆ（ｋ）は時刻ｋにおける、定常状態での目標制御入力を、ｕ（ｋ）は時刻ｋにおける計算上の制御入力を表している。また、Ｑ及びＲは、それぞれステージコストにおける状態量の重みを表す係数（重み係数）、制御入力の重みを表す係数（重み係数）である。したがって、式３の右辺の第１項が、状態量に関するステージコストを意味し「状態量コスト」と称し、右辺の第２項が、制御入力に関するステージコストを意味し「制御入力コスト」と称する。また、右辺の第３項である第１ステージコスト、及び第４項である第２ステージコストについては後述する。
更に、本実施例では、取得部４５と設定部４６が形成されている。取得部４５は、カメラ８の撮像を介して認識される実際の障害物（実障害物）に関する所定のパラメータを取得する。所定のパラメータとしては、実障害物の位置や、実障害物が移動している場合の、その移動方向と、制御対象の追従制御時の移動方向との間の角度として定義される、実障害物の追従目標軌道に対する進入角θ（以下、単に「進入角」という）等が挙げられる。
例えば、取得部４５は、カメラ８により撮像される領域が既知とされることで、その撮像画像を画像処理すること等の従来技術により実障害物の位置を取得できる。また、上記の実障害物の進入角については、取得された実障害物の過去の位置に基づいて取得することができる。一例としては、取得部４５によって取得された直近の２点の実障害物の位置ＯＰ１、ＯＰ２からその動きを直線近似し、更に、対応するタイミングの実プラント６の２点の位置ＴＰ１、ＴＰ２からその動きを直線近似する。そして進入角θは余弦定理に従って算出できる。また、別法としては、直近の複数点の実障害物の位置からその動きを多項式近似することでも、追従目標軌道への進入角θを算出することが可能となる。
また、設定部４６は、実障害物の位置に基づいて仮想障害物の位置を設定する。仮想障害物は、制御対象である実プラント６と実障害物との衝突を回避するための、式３に従ったステージコストの算出、特に第２ステージコストの算出のために、仮想的に設定される障害物である。したがって、仮想障害物は、実プラント６及び実障害物とは異なり実際に
存在する物ではない。設定部４６は、実プラント６の追従目標軌道を基準として、実障害物とは略対称に位置するように仮想障害物の位置を設定するが、その詳細については後述する。
以上を踏まえてモデル予測制御において算出された、予測区間の初期時刻ｔでの制御入力ｕの値が、その時刻ｔでの、目標指令ｒに対応する実プラント６への制御入力ｕとして出力される。そして、モデル予測制御では、その制御時刻において、都度、所定の時間幅Ｔの予測区間が設定されるとともに、式２の評価関数に従って当該制御時刻での実プラント６への制御入力ｕが算出され、実プラント６へ送られることになる。式２のような形の評価関数Ｊの値を最良とする操作量を求める問題は、最適制御問題として広く知られている問題であり、その数値解を算出するアルゴリズムが公知技術として開示されている。そのような技術として連続変形法が例示でき、例えば、公知の文献である「連続変形法とGMRES法を組み合わせた非線形Receding horizon制御の高速アルゴリズム（A continuation /GMRES method for fast computation of nonlinear receding horizon control）」{大
塚敏之（T. Ohtsuka）, オートマティカ（ Automatica）, 第４０巻, ｐ563～574, ２０
０４. }に詳細が開示されている。
連続変形法では、下記の式４に示す、入力Ｕ（ｔ）に関する連立１次方程式を解くことでモデル予測制御における入力Ｕ（ｔ）が算出される。具体的には、式４を解き、dU/dt
を数値積分して、入力Ｕ（ｔ）を更新していく。このように連続変形法では、反復計算を行わないため、各時刻での入力Ｕ（ｔ）を算出するための演算負荷を可及的に抑制することができる。
Figure 0006992719000004
・・・（式４）
ただし、Ｆ、Ｕ（ｔ）は、以下の式５で表される。
Figure 0006992719000005
・・・（式５）
ただし、Ｈはハミルトニアン、λは共状態、μは拘束条件Ｃ＝０のラグランジュ乗数で
ある。
ここで、本実施形態における、上記式３に従ったステージコストの算出の詳細について、図３に基づいて説明する。図３は、目標指令ｒに追従するように制御される実プラント６の周囲に、移動する実障害物６１が存在している状態を表している。実障害物６１は、カメラ８の撮像によってその存在が認識されるとともに、実障害物６１の位置が取得部４５によって取得されることになる。なお、取得される実障害物６１の位置を「ｐ６１」と参照することとする。ここで、実プラント６に対して実障害物６１は相対的に移動することを踏まえ、実プラント６の周囲において実障害物６１が存在し得る確率を表す確率ポテンシャル場（実障害物６１に起因する確率ポテンシャル場を第１ポテンシャル場とする）を算出することができる。当該確率ポテンシャル場の算出そのものは公知の技術であるから、例えば特開２００３－２４１８３６号公報に記載の技術を利用して算出が可能である。
そして、本実施形態では、実障害物６１の第１ポテンシャル場を直接算出するか否かにかかわらず、第１ポテンシャル場に関連する第１ステージコストを、モデル予測制御部４３によるモデル予測制御において算出されるステージコストに含めることとする（上記の式３を参照）。なお、第１ステージコストは、下記の式６で表すことができる。
Figure 0006992719000006
・・・（式６）
ただし、ｐｔは、実プラント６の位置であり、ｋは、第１ポテンシャル場に関連する係数である。
このように第１ステージコストを生成することで、第１ポテンシャル場の確率値が高くなると第１ステージコストが上昇することになり、目標指令ｒへの追従制御を行いながら、結果として実障害物６１の第１ポテンシャル場が形成する「斥力」が、目標指令ｒへの追従制御がされている実プラント６を実障害物６１から遠ざけるように、モデル予測制御部４３により制御入力ｕが算出されることになる。例えば、実プラント６は、そのような制御入力ｕによって、図３で一点鎖線で示す回避軌道を辿ることになる。しかし、このような回避軌道では、実プラント６自体が、移動する実障害物６１の前方に回避することになるため、実プラント６が実障害物６１の第１ポテンシャル場に影響される時間が長くなり、場合によっては実プラント６が実障害物６１を回避することが難しくなり得る（図３では、回避軌道を辿る実プラント６が、「×」で示す位置で実障害物６１と衝突した状態を表している）。
そこで、本実施形態では、移動している実障害物６１と実プラント６との衝突を好適に回避するために、設定部４６によって仮想障害物６２の位置が設定される。具体的には、仮想障害物６２は、実プラント６の追従目標軌道を基準として実障害物６１とは略対称に位置するように、その位置が設定される。なお、仮想障害物６２の位置は、「ｐ６２」と参照することとする。また、実障害物６１の場合と同じように、実プラント６の周囲において仮想障害物６２が存在し得る確率を表す確率ポテンシャル場（仮想障害物６２に起因する確率ポテンシャル場を第２ポテンシャル場とする）を算出することができる。その上で、実障害物６１と同じように、仮想障害物６２の第２ポテンシャル場を直接算出するか否かにかかわらず、第２ポテンシャル場に関連する第２ステージコストを、モデル予測制御部４３によるモデル予測制御において算出されるステージコストに含めることとする（
上記の式３を参照）。なお、第２ステージコストは、下記の式７で表すことができる。
Figure 0006992719000007
・・・（式７）
ただし、ｐｔは、実プラント６の位置であり、ｋは、第２ポテンシャル場に関連する係数であり、本実施形態では式６に示すｋと同じである。
このようにモデル予測制御部４３によるモデル予測制御において算出されるステージコストに、第１ステージコストと第２ステージコストを含めることで、追従目標軌道の一方側から実障害物６１の第１ポテンシャル場の斥力を実プラント６に作用させつつ、追従目標軌道の他方側から仮想障害物６２の第２ポテンシャル場の斥力を実プラント６に作用させることができる。図４には、実プラント６が追従する追従目標軌道に対して３０度の進入角θで実障害物６１が進行してくるケースにおいて、本実施形態のモデル予測制御が行われた場合の、実プラント６、実障害物６１、仮想障害物６２の軌跡がプロットされている。なお、図４は、モデル予測制御に供される制御軸が横軸と縦軸の２つであることに留意されたい。これからも理解できるように、本実施形態のモデル予測制御により算出される制御指令ｕによって、実プラント６は実障害物６１と近接したときには好適に減速して、実障害物６１の後方を回避する形で追従目標軌道上の追従制御が好適に維持されている。
＜第１の構成例＞
本構成例に係るサーボドライバ４によるサーボ制御について、図５に基づいて説明する。本構成例のサーボドライバ４では、上記適用例と同様にモデル予測制御部４３によるモデル予測制御が行われるが、その際にサーボ積分器４１の出力ｚが状態取得部４２によって取得され、当該モデル予測制御に供される。具体的には、標準ＰＬＣ５から送信された目標指令ｒと、フィードバック系４４によってフィードバックされた実プラント６の出力ｙとの偏差ｅ（ｅ＝ｒ－ｙ）が、サーボ積分器４１に入力される。そして、そのサーボ積分器４１の出力ｚが、状態取得部４２を経てモデル予測制御部４３に入力される。したがって、状態取得部４２により、実プラント６に関する上記の状態変数に出力ｚが加えられ、モデル予測制御部４３によるモデル予測制御に供される。
このようにサーボ積分器４１を含む制御構造を踏まえ、本構成におけるモデル予測制御部４３が有する予測モデルは、例えば下記の式８に表すことができる。
Figure 0006992719000008
・・・（式８）
式８における（ｒ－ｙ）は偏差ｅを表している。そして、上記予測モデルには、偏差ｅ（ｒ－ｙ）と、第１積分ゲインＫｉ及び第２積分ゲインζとの積で表される積分項が含まれていることが理解できる。第１積分ゲインＫｉ及び第２積分ゲインζとの積が、所定の積分ゲインに相当する。これにより、モデル予測制御を用いたサーボドライバ４によるサーボ制御において適用例で示した実障害物との衝突回避及び目標指令への好適な追従性の効果に加えて、サーボ制御の駆動源となる積分量を調整しやすくなり、従来のように外乱モデルの拡張やオブザーバゲインの設計等、容易ではない調整が必要な外乱オブザーバを利用することなく、オーバーシュートを抑制したサーボ制御の実現が容易となる。
ここで、式８に示す予測モデルに含まれている積分項の所定の積分ゲインを構成する第１積分ゲインＫｉは、図６の上段（ａ）に示すように、偏差ｅに基づいて調整することができる。具体的には、偏差ｅの大きさが小さくなるに従い、第１積分ゲインＫｉの値が大きくなるように、該第１積分ゲインＫｉを調整する。例えば、偏差ｅの大きさが所定のｅ０以上となる場合には、第１積分ゲインＫｉは０となり、偏差ｅの大きさがｅ０未満の範囲で第１積分ゲインＫｉに０より大きく１以下の値が設定されることになる。また、偏差ｅの大きさが０に近づくほど、第１積分ゲインＫｉの値が急峻に１に近づき、偏差ｅの大きさが０である場合には第１積分ゲインＫｉは１となるように第１積分ゲインＫｉの推移が設定されている。このように、第１積分ゲインＫｉが偏差ｅの大きさに基づいて調整可能とされることで、実プラント６の出力ｙが目標指令ｒと比較的乖離している場合には、第１積分ゲインＫｉの値は小さく調整され、以て、サーボ制御のための積分量が不要に溜まらないように調整されることになる。また、実プラント６の出力ｙと目標指令ｒとの乖離量が少なくなると、すなわち偏差ｅの大きさが小さくなると、第１積分ゲインＫｉの値が大きく調整されるため、サーボ制御における追従性を効果的に高めることができる。このように第１積分ゲインＫｉの値を変動させることで、振動抑制とオーバーシュートの抑制を両立しながら、好適なサーボ制御の追従性を図ることができる。
一方で、実プラント６と実障害物６１との関係を考慮すると、両者の距離（ノルム）が小さくなると、基本的には両者の衝突回避が、目標指令への追従性よりも優先されるべき状況となる。このような状況において、上記の通り第１積分ゲインＫｉの作用により予測モデルの所定の積分項の影響が、実障害物６１及び仮想障害物６２の関連する第１ステージコスト及び第２ステージコストの影響より相対的に大きくなってしまうと、実障害物６１との衝突回避が困難となり得る。そこで、所定の積分ゲインには、第２積分ゲインζが含まれる。第２積分ゲインζは、図６の下段（ｂ）に示すように、実プラント６が実障害物６１と衝突する可能性に関連するパラメータ、例えば、実プラント６と実障害物６１との離間距離（ノルム）ｎｃｏｌに基づいて調整することができる。具体的には、当該ノルムｎｃｏｌの大きさが所定のｎｃｏｌ０以上となる場合には、第２積分ゲインζは１となり、ノルムｎｃｏｌの大きさがｎｃｏｌ０未満の範囲で第２積分ゲインζに０以上１以下の値が設定されることになる。また、ノルムｎｃｏｌの大きさが０に近づくほど、第２積分ゲインζの値が急峻に０に近づくように第２積分ゲインζの推移が設定されている。
このように、式８に示す予測モデルの所定の積分項においては、所定の積分ゲインが上記の第１積分ゲインＫｉと第２積分ゲインζとの積で構成されている。そのため、基本的には、偏差ｅに応じた第１積分ゲインＫｉによる積分効果により目標指令ｒへの追従性の向上が図られるが、実プラント６と実障害物６１とのノルムが小さくなり衝突の可能性が高まった場合には、第２積分ゲインζの効果によって当該積分効果の緩和が図られる。図７には、実プラント６が追従する追従目標軌道に対して３０度の進入角θで実障害物６１が進行してくるケースにおいて、本実施形態のモデル予測制御が行われた場合の、実プラント６、実障害物６１、仮想障害物６２の軌跡がプロットされている。なお、図７は、モ
デル予測制御に供される制御軸が横軸と縦軸の２つであることに留意されたい。また、図７の上段（ａ）は、式８に示す予測モデルの所定の積分項において第１積分ゲインＫｉのみが含まれているときの軌跡をプロットしたものであり、その下段（ｂ）は、式８に示すように当該所定の積分項において第１積分ゲインＫｉ及び第２積分ゲインζが含まれているときの軌跡をプロットしたものである。これからも理解できるように、上段（ａ）では、上記積分効果により目標指令への追従性は良好ではあるが、実プラント６と実障害物６１との衝突回避が十分ではない。一方で、下段（ｂ）では、上記積分効果が緩和された結果、実プラント６の減速が効果的に働き実障害物６１の後方を回避できている。
なお、第１積分ゲインＫｉ及び第２積分ゲインζの調整について、図６に示す偏差ｅやノルムｎｃｏｌと各積分ゲインとの相関に関するデータは、サーボドライバ４のメモリ内に格納されてもよく、その場合は、モデル予測制御部４３が当該データにアクセスすることで上述の各積分ゲインの調整を行う。
＜第２の構成例＞
本構成例に係るサーボドライバ４によるサーボ制御について説明する。実プラント６と実障害物６１との衝突が回避された後に、仮想障害物６２を考慮したモデル予測制御を継続すると、仮想障害物６２の第２ポテンシャル場で形成される斥力が実プラント６の目標指令ｒへの追従性に好ましくない影響を及ぼす可能性がある。そこで、本構成例のサーボドライバ４におけるモデル予測制御部４３が有するモデル予測制御では、例えば下記の式９に従って第２ステージコストが算出される。なお、本構成例が適用できる制御構造は、図２及び図５のいずれでもよい。
Figure 0006992719000009
・・・（式９）
ただし、上記Ｐｃｒｏｓｓは、実プラント６が実障害物６１と衝突する可能性がある位置であり、例えば、実プラント６による追従目標軌道と実障害物６１の進行が想定される障害物軌道とが交差する交差点の位置である。留意すべきは、Ｐｃｒｏｓｓは、実プラント６が実際に実障害物６１と衝突した位置ではなく、本実施形態によりその衝突は回避されるものの、衝突が起こり得ると想定される位置である。したがって、実プラント６がＰｃｒｏｓｓで表される位置を経過した後は、基本的には実プラント６と実障害物６１との衝突の可能性は小さくなることを意味する。
また、上記の交差点位置Ｐｃｒｏｓｓについては、取得された実障害物の過去の位置に基づいて取得することができる。一例としては、取得部４５によって取得された直近の２点の実障害物の位置ＯＰ１、ＯＰ２からその動きを直線近似し、更に、対応するタイミングの実プラント６の２点の位置ＴＰ１、ＴＰ２からその動きを直線近似する。そして、両直線の交点の位置が、交差点位置Ｐｃｒｏｓｓとされる。
第２ステージコストが、上記式９に従うことで、少なくとも実プラント６が交差点位置Ｐｃｒｏｓｓを過ぎた後は、モデル予測制御でのステージコストの算出において、実プラント６の位置が交差点位置Ｐｃｒｏｓｓから遠くなるほど、第２ステージコストは小さくなるように算出される。この結果、実プラント６が交差点位置Ｐｃｒｏｓｓを過ぎた後に
おいて、仮想障害物６２の第２ポテンシャル場に起因した実プラント６への不用意な作用を低減でき、以て追従性低下を回避することができる。
図８には、実プラント６が追従する追従目標軌道に対して３０度の進入角θで実障害物６１が進行してくるケースにおいて、本実施形態のモデル予測制御が行われた場合の、実プラント６、実障害物６１、仮想障害物６２の軌跡がプロットされている。なお、図８は、モデル予測制御に供される制御軸が横軸と縦軸の２つであることに留意されたい。図８からも理解できるように、実プラント６が交差点位置Ｐｃｒｏｓｓを経過した後は、仮想障害物６２を表すプロットの大きさが小さくなっている。これは、上記式９のｐｓｅｕｄｏＰｏｔにより、第２ステージコストの値が小さく計算されていることを意味する。この結果、実プラント６が交差点位置Ｐｃｒｏｓｓを経過した後において、仮想障害物６２の影響が緩和され、実プラント６の目標指令ｒへの追従性が低下することを回避できる。
なお、上記式９では、実プラント６が交差点位置Ｐｃｒｏｓｓを経過した後、実プラント６が交差点位置Ｐｃｒｏｓｓから遠ざかるほど第２ステージコストの値が小さく算出されるが、このように第２ステージコストの値を小さく算出するのを実プラント６が交差点位置Ｐｃｒｏｓｓに差し掛かる前から開始しても構わない。更に、別法として、実プラント６が交差点位置Ｐｃｒｏｓｓを経過した後は第２ステージコストの値を零としてもよい。
＜第３の構成例＞
本構成例に係るサーボドライバ４によるサーボ制御について説明する。実障害物６１の進入角が９０度に近くなるほど、実障害物６１の第１ポテンシャル場で形成される斥力、及び実障害物６１に対応する仮想障害物の第２ポテンシャル場で形成される斥力が、実プラント６に作用しにくくなり、実障害物６１との衝突回避を好適に図ることが困難となる。そこで、本構成例のサーボドライバ４におけるモデル予測制御部４３が有するモデル予測制御では、例えば下記の式１０に従って第２ステージコストが算出される。なお、本構成例が適用できる制御構造は、図２及び図５のいずれでもよい。
Figure 0006992719000010
・・・（式１０）
第２ステージコストが、上記式１０に従うことで、実障害物６１の進入角が９０度に近くなるほど、第２ステージコストは大きくなるように算出される。この結果、実障害物６１の進入角が９０度に近づいたとしても、実プラント６は目標指令ｒへの追従性を保ちながら、実障害物６１との衝突を好適に回避することができる。
図９には、実プラント６が追従する追従目標軌道に対して９０度の進入角θで実障害物６１が進行してくるケースにおいて、本実施形態のモデル予測制御が行われた場合の、実プラント６、実障害物６１、仮想障害物６２の軌跡がプロットされている。なお、図９は、モデル予測制御に供される制御軸が横軸と縦軸の２つであることに留意されたい。また、図９の上段（ａ）は、式１０に示す予測モデルのｐｓｅｕｄｏＰｏｔが一定値（＝１）に固定した場合の軌跡をプロットしたものであり、その下段（ｂ）は、当該ｐｓｅｕｄｏＰｏｔを式１０に従って進入角９０度に応じて調整したときの軌跡をプロットしたもので
ある。これからも理解できるように、上段（ａ）では、進入角が９０度であるから上記のように各ポテンシャル場による斥力が実プラント６に作用しにくく、結果として実プラント６は、移動する実障害物６１の前方に回避してしまっており、その回避量も比較的大きい。一方で、下段（ｂ）では、進入角θを考慮して第２ステージコストが算出されている。すなわち、進入角が９０度の場合、ｐｓｅｕｄｏＰｏｔの値は２となり、上段（ａ）の場合よりもｐｓｅｕｄｏＰｏｔの値は２倍となる。その結果、仮想障害物６２の第２ポテンシャル場による斥力の影響を相対的に大きく働かせることができ、下段（ｂ）に示すように、実プラント６を移動する実障害物６１の後方に回避させることができ、その回避量も比較的小さい。
なお、上記の例では、第２ステージコストの算出に進入角θを反映させているが、更に下記の式１１に示すように第１ステージコストの算出にも進入角θを反映させてもよい。
Figure 0006992719000011
・・・（式１１）
更に、上記の第２の構成例で開示した技術思想も併せて採用し、下記の式１２に示すように第２ステージコストの算出に進入角θを反映させてもよい。
Figure 0006992719000012
・・・（式１２）
＜第４の構成例＞
本構成例に係るサーボドライバ４によるサーボ制御について説明する。本構成例では、実プラント６が目標指令ｒへの追従制御を行っている際に、複数（例えば、２つ）の実障害物６１ａ、６１ｂ（後述の図１０を参照）の存在が認識され取得部４５によってそれぞれの位置が取得されたときの、モデル予測制御について言及する。本構成例のモデル予測制御部４３によるモデル予測制御では、下記の式１２に従って、認識された２つの実障害物６１ａ、６１ｂのそれぞれについて第１ステージコストが算出されるとともに、それぞれに対応して設定部４６により設定される仮想障害物６２ａ、６２ｂのそれぞれについて第２ステージコストが算出される。
Figure 0006992719000013
・・・（式１３）
なお、各第１ステージコスト及び各第２ステージコストの算出については、上述までの適用例及び構成例で示した通りである。
このように式１３に従って、モデル予測制御においてステージコストが算出されることで、複数の実障害物６１ａ、６１ｂが存在する場合でも、実プラント６の目標指令ｒへの追従性と、各実障害物との衝突回避とを好適に両立することが可能となる。図１０には、実プラント６が追従する追従目標軌道に対して２つの実障害物６１a、６１ｂが進行して
くるケースにおいて、本実施形態のモデル予測制御が行われた場合の、実プラント６、実障害物６１ａ、６１ｂ、及び仮想障害物６２ａ、６２ｂの軌跡がプロットされている。なお、図１０は、モデル予測制御に供される制御軸が横軸と縦軸の２つであることに留意されたい。仮想障害物６２ａ、６２ｂは、それぞれ設定部４６によって実障害物６１ａ、６１ｂに対応するように設定されたものである。また、図１０に示す形態では、構成例２で示したように、実プラント６が各実障害物との交差点位置を経過した後は、その実障害物に対応する仮想障害物に関連する第２ステージコストの値が零とされている。図１０からも理解できるように、式１３に従って本実施形態のモデル予測制御が行われることで、実プラント６は、２つの実障害物６１ａ、６１ｂのそれぞれの後方に回避しつつ、目標指令ｒへの追従性を好適に維持している。
また、実障害物の数が３以上の場合でも、その数に応じた仮想障害物を設定部４６により設定し、それぞれに関連する第１ステージコスト及び第２ステージコストを算出してモデル予測制御を行うことで、それぞれの実障害物との衝突を回避しつつ目標指令ｒへの好適な追従が実現される。
ただし、上記のモデル予測制御で考慮される実障害物の数が多くなると、その数に応じてサーボドライバ４の処理負荷が大きくなり、実プラント６の実時間での追従制御に支障を来たす可能性がある。そこで、サーボドライバ４の処理負荷を考慮して、上記のモデル予測制御で考慮される実障害物及びそれに応じて設定される仮想障害物の数は、所定数（例えば、それぞれ２つ）に限定されてもよい。このような場合に、所定数を超える所定超過数の実障害物（例えば、３つ）の存在が認識され、取得部４５によって所定超過数の実障害物の位置が取得された場合には、その所定超過数の実障害物の中から、所定の基準に従ってモデル予測制御において考慮可能な最大限の所定数の実障害物が抽出される。当該所定の基準としては、実プラント６と実障害物との衝突の可能性を考慮して、実プラント６と実障害物との離間距離が利用できる。例えば、当該離間距離が小さいほど衝突の可能性が高く、優先的にその衝突を回避すべき状態にあることを意味するから、当該離間距離が小さい方から順に所定数の実障害物として抽出されてもよい。その上で、モデル予測制御において、抽出された実障害物のそれぞれの位置に基づいて、それぞれに関連する第１ステージコストが算出され、且つ、該抽出された所定数の実障害物に対応する所定数の仮想障害物のそれぞれの位置に基づいて、それぞれに関連する第２ステージコストが算出される。このような構成を採用することで、サーボドライバ４の処理負荷を適切に抑制した上で、実プラント６の実時間での好適な追従制御が実現される。
＜付記１＞
制御対象（６）に関する所定の状態変数と該制御対象（６）への制御入力との相関を所定の状態方程式の形式で画定した予測モデルを有し、該制御対象（６）の出力が追従すべき所定の目標指令に対して、所定時間幅の予測区間において所定の評価関数に従って該予測モデルに基づいたモデル予測制御を行い、少なくとも該予測区間の初期時刻での前記制御入力の値を出力するモデル予測制御部（４３）を備え、該制御対象の出力を該所定の目標指令に対して追従させる制御装置（４）であって、
前記制御対象（６）に対する実際の障害物である実障害物（６１）の位置を取得する第１取得部（４５）と、
前記所定の目標指令に基づく前記制御対象（６）の追従目標軌道を基準として、前記実障害物（６１）に関連付けられた仮想障害物（６２）が該実障害物（６１）とは略対称に位置するように、前記第１取得部（４５）によって取得された前記実障害物（６１）の位置に基づいて該仮想障害物（６２）の位置を設定する設定部（４６）と、
を備え、
前記所定の評価関数により算出されるステージコストに、前記所定の状態変数に関するステージコストである状態量コストと、前記制御入力に関連するステージコストである制御入力コストと、前記実障害物（６１）の位置に基づいて該実障害物（６１）が存在し得る確率を表した第１確率ポテンシャル場に関連する第１ステージコストと、前記仮想障害物（６２）の位置に基づいて該仮想障害物（６２）が存在し得る確率を表した確率ポテンシャル場であって該第１確率ポテンシャル場以上の確率値を有する第２確率ポテンシャル場に関連する第２ステージコストとが含まれる、
制御装置。
＜付記２＞
制御対象（６）に関する所定の状態変数と該制御対象（６）への制御入力との相関を所定の状態方程式の形式で画定した予測モデルを有し、該制御対象（６）の出力が追従すべき所定の目標指令に対して、所定時間幅の予測区間において所定の評価関数に従って該予測モデルに基づいたモデル予測制御を行い、少なくとも該予測区間の初期時刻での前記制御入力の値を出力するモデル予測制御部（４３）を備え、該制御対象の出力を該所定の目標指令に対して追従させる制御装置（４）であって、
前記所定の目標指令と、前記制御対象の出力との偏差が入力されるサーボ用積分器（４１）と、
前記制御対象（６）に対する実際の障害物である実障害物（６１）の位置を取得する第１取得部（４５）と、
前記所定の目標指令に基づく前記制御対象（６）の追従目標軌道を基準として、前記実障害物（６１）に関連付けられた仮想障害物（６２）が該実障害物（６１）とは略対称に位置するように、前記第１取得部（４５）によって取得された前記実障害物（６１）の位置に基づいて該仮想障害物（６２）の位置を設定する設定部（４６）と、
を備え、
前記所定の評価関数により算出されるステージコストに、前記所定の状態変数に関するステージコストである状態量コストと、前記制御入力に関連するステージコストである制御入力コストと、前記実障害物（６１）の位置に基づいて該実障害物（６１）が存在し得る確率を表した第１確率ポテンシャル場に関連する第１ステージコストと、前記仮想障害物（６２）の位置に基づいて該仮想障害物（６２）が存在し得る確率を表した確率ポテンシャル場であって該第１確率ポテンシャル場以上の確率値を有する第２確率ポテンシャル場に関連する第２ステージコストとが含まれ、
前記制御対象（６）に関連する状態変数に、前記偏差と所定の積分ゲインとの積で表される所定の積分項が含まれ、
前記所定の積分ゲインは、前記制御対象と前記実障害物との間の距離が短くなるほど小さくなる、
制御装置。
１・・・・ネットワーク
２・・・・モータ
３・・・・負荷装置
４・・・・サーボドライバ
５・・・・標準ＰＬＣ
６・・・・実プラント
４１・・・・サーボ積分器
４２・・・・状態取得部
４３・・・・モデル予測制御部
４５・・・・取得部
４６・・・・設定部
６１・・・・実障害物
６２・・・・仮想障害物

Claims (7)

  1. 制御対象に関する所定の状態変数と該制御対象への制御入力との相関を所定の状態方程式の形式で画定した予測モデルを有し、該制御対象の出力が追従すべき所定の目標指令に対して、所定時間幅の予測区間において所定の評価関数に従って該予測モデルに基づいたモデル予測制御を行い、少なくとも該予測区間の初期時刻での前記制御入力の値を出力するモデル予測制御部を備え、該制御対象の出力を該所定の目標指令に対して追従させる制御装置であって、
    前記制御対象に対する実際の障害物である実障害物の位置を取得する第１取得部と、
    前記所定の目標指令に基づく前記制御対象の追従目標軌道を基準として、前記実障害物に関連付けられた仮想障害物が該実障害物とは略対称に位置するように、前記第１取得部によって取得された前記実障害物の位置に基づいて該仮想障害物の位置を設定する設定部と、
    を備え、
    前記所定の評価関数により算出されるステージコストに、前記所定の状態変数に関するステージコストである状態量コストと、前記制御入力に関連するステージコストである制御入力コストと、前記実障害物の位置に基づいて該実障害物が存在し得る確率を表した第１確率ポテンシャル場に関連する第１ステージコストと、前記仮想障害物の位置に基づいて該仮想障害物が存在し得る確率を表した確率ポテンシャル場であって該第１確率ポテンシャル場以上の確率値を有する第２確率ポテンシャル場に関連する第２ステージコストとが含まれる、
    制御装置。
  2. 制御対象に関する所定の状態変数と該制御対象への制御入力との相関を所定の状態方程式の形式で画定した予測モデルを有し、該制御対象の出力が追従すべき所定の目標指令に対して、所定時間幅の予測区間において所定の評価関数に従って該予測モデルに基づいたモデル予測制御を行い、少なくとも該予測区間の初期時刻での前記制御入力の値を出力するモデル予測制御部を備え、該制御対象の出力を該所定の目標指令に対して追従させる制御装置であって、
    前記所定の目標指令と、前記制御対象の出力との偏差が入力されるサーボ用積分器と、
    前記制御対象に対する実際の障害物である実障害物の位置を取得する第１取得部と、
    前記所定の目標指令に基づく前記制御対象の追従目標軌道を基準として、前記実障害物に関連付けられた仮想障害物が該実障害物とは略対称に位置するように、前記第１取得部によって取得された前記実障害物の位置に基づいて該仮想障害物の位置を設定する設定部と、
    を備え、
    前記所定の評価関数により算出されるステージコストに、前記所定の状態変数に関するステージコストである状態量コストと、前記制御入力に関連するステージコストである制御入力コストと、前記実障害物の位置に基づいて該実障害物が存在し得る確率を表した第１確率ポテンシャル場に関連する第１ステージコストと、前記仮想障害物の位置に基づいて該仮想障害物が存在し得る確率を表した確率ポテンシャル場であって該第１確率ポテンシャル場以上の確率値を有する第２確率ポテンシャル場に関連する第２ステージコストとが含まれ、
    前記制御対象に関連する状態変数に、前記偏差と所定の積分ゲインとの積で表される所定の積分項が含まれ、
    前記所定の積分ゲインは、前記制御対象と前記実障害物との間の距離が短くなるほど小さくなる、
    制御装置。
  3. 前記制御対象の移動方向と前記実障害物の移動方向との間の角度として定義される、該
    実障害物の前記追従目標軌道に対する進入角を取得する第２取得部を更に備え、
    前記所定の評価関数により算出されるステージコストにおいて、前記進入角が９０度に近くなるほど前記第２ステージコストは大きくなるように算出される、
    請求項１又は請求項２に記載の制御装置。
  4. 前記進入角は、前記第１取得部によって取得されてきた前記実障害物の過去の位置に基づいて算出される、
    請求項３に記載の制御装置。
  5. 前記制御対象の位置が、前記追従目標軌道と前記実障害物が辿る障害物軌道とが交差する交差点の位置を過ぎた後は、前記所定の評価関数により算出されるステージコストにおいて、該制御対象の位置が該交差点の位置から遠くなるほど前記第２ステージコストは小さくなるように算出される、
    請求項１から請求項４の何れか１項に記載の制御装置。
  6. 前記制御対象の位置が、前記追従目標軌道と前記実障害物が辿る障害物軌道とが交差する交差点の位置を過ぎた後は、前記所定の評価関数により算出されるステージコストにおいて、前記第２ステージコストは零とされる、
    請求項１から請求項４の何れか１項に記載の制御装置。
  7. 前記所定の評価関数は、２以上の所定数の前記実障害物のそれぞれに対応する前記第１ステージコスト及び前記第２ステージコストが算出されるように形成され、
    前記第１取得部により前記所定数を超える所定超過数の前記実障害物の位置が取得された場合、該所定超過数の実障害物の中から前記制御対象と該所定超過数の実障害物のそれぞれとの離間距離に基づいて該所定数の実障害物が抽出され、更に、前記所定の評価関数により算出されるステージコストにおいて、該抽出された該所定数の実障害物の位置に基づいて前記第１ステージコストが算出され、且つ、該抽出された該所定数の実障害物に対応する該所定数の仮想障害物の位置に基づいて前記第２ステージコストが算出される、
    請求項１から請求項６の何れか１項に記載の制御装置。
JP2018182565A 2018-09-27 2018-09-27 制御装置 Active JP6992719B2 (ja)

Priority Applications (5)

Application Number Priority Date Filing Date Title
JP2018182565A JP6992719B2 (ja) 2018-09-27 2018-09-27 制御装置
CN201980051701.9A CN112513747B (zh) 2018-09-27 2019-09-27 控制装置
PCT/JP2019/038237 WO2020067460A1 (ja) 2018-09-27 2019-09-27 制御装置
EP19864043.5A EP3859456A4 (en) 2018-09-27 2019-09-27 CONTROL DEVICE
US17/267,033 US11467547B2 (en) 2018-09-27 2019-09-27 Control device

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
JP2018182565A JP6992719B2 (ja) 2018-09-27 2018-09-27 制御装置

Publications (2)

Publication Number Publication Date
JP2020052810A JP2020052810A (ja) 2020-04-02
JP6992719B2 true JP6992719B2 (ja) 2022-01-13

Family

ID=69951902

Family Applications (1)

Application Number Title Priority Date Filing Date
JP2018182565A Active JP6992719B2 (ja) 2018-09-27 2018-09-27 制御装置

Country Status (5)

Country Link
US (1) US11467547B2 (ja)
EP (1) EP3859456A4 (ja)
JP (1) JP6992719B2 (ja)
CN (1) CN112513747B (ja)
WO (1) WO2020067460A1 (ja)

Families Citing this family (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN112000123B (zh) * 2020-07-20 2023-03-14 南京信息工程大学 一种旋翼无人机避障控制系统及其控制方法
CN112622932B (zh) * 2020-12-23 2022-02-01 同济大学 一种基于势能场启发式搜索的自动驾驶换道轨迹规划算法
DE112021007044T5 (de) 2021-02-09 2024-03-21 Mitsubishi Electric Corporation Steuerungsberechnungsvorrichtung und Steuerungsberechnungsverfahren
CN113805483B (zh) * 2021-09-17 2022-07-12 中国人民解放军国防科技大学 基于模型预测的机器人控制方法、装置和计算机设备
CN115229772B (zh) * 2022-08-23 2023-07-18 深圳市越疆科技股份有限公司 机器人及其控制方法、装置、设备、存储介质、机械臂

Citations (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JP2003280710A (ja) 2002-03-20 2003-10-02 Japan Atom Energy Res Inst ロボットハンドの作業軌道の生成と制御方法
JP2011186878A (ja) 2010-03-10 2011-09-22 Nissan Motor Co Ltd 移動体走行経路生成装置
JP2017102617A (ja) 2015-11-30 2017-06-08 オムロン株式会社 補正装置、補正装置の制御方法、情報処理プログラム、および記録媒体

Family Cites Families (12)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JP2003241836A (ja) 2002-02-19 2003-08-29 Keio Gijuku 自走移動体の制御方法および装置
US8260593B2 (en) * 2002-09-18 2012-09-04 Siemens Product Lifecycle Management Software Inc. System and method for simulating human movement
SE526913C2 (sv) * 2003-01-02 2005-11-15 Arnex Navigation Systems Ab Förfarande i form av intelligenta funktioner för fordon och automatiska lastmaskiner gällande kartläggning av terräng och materialvolymer, hinderdetektering och styrning av fordon och arbetsredskap
JP4576445B2 (ja) 2007-04-12 2010-11-10 パナソニック株式会社 自律移動型装置および自律移動型装置用プログラム
JP5278378B2 (ja) * 2009-07-30 2013-09-04 日産自動車株式会社 車両運転支援装置及び車両運転支援方法
US8706298B2 (en) * 2010-03-17 2014-04-22 Raytheon Company Temporal tracking robot control system
US9823634B2 (en) * 2012-04-24 2017-11-21 Cast Group Of Companies Inc. System and method for providing three-dimensional paths
US9199668B2 (en) * 2013-10-28 2015-12-01 GM Global Technology Operations LLC Path planning for evasive steering maneuver employing a virtual potential field technique
US9283678B2 (en) * 2014-07-16 2016-03-15 Google Inc. Virtual safety cages for robotic devices
JP6664807B2 (ja) * 2015-08-20 2020-03-13 国立大学法人東京農工大学 制御装置、制御方法、及び制御プログラム
WO2018135869A1 (ko) * 2017-01-19 2018-07-26 주식회사 만도 지능형 운전자 보조 시스템을 위한 카메라 시스템, 및 운전자 보조 시스템 및 방법
JP6880853B2 (ja) * 2017-03-14 2021-06-02 オムロン株式会社 処理装置、パラメータ調整方法、及びパラメータ調整プログラム

Patent Citations (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JP2003280710A (ja) 2002-03-20 2003-10-02 Japan Atom Energy Res Inst ロボットハンドの作業軌道の生成と制御方法
JP2011186878A (ja) 2010-03-10 2011-09-22 Nissan Motor Co Ltd 移動体走行経路生成装置
JP2017102617A (ja) 2015-11-30 2017-06-08 オムロン株式会社 補正装置、補正装置の制御方法、情報処理プログラム、および記録媒体

Also Published As

Publication number Publication date
US11467547B2 (en) 2022-10-11
EP3859456A1 (en) 2021-08-04
JP2020052810A (ja) 2020-04-02
US20210302924A1 (en) 2021-09-30
EP3859456A4 (en) 2022-06-15
CN112513747B (zh) 2022-05-24
WO2020067460A1 (ja) 2020-04-02
CN112513747A (zh) 2021-03-16

Similar Documents

Publication Publication Date Title
JP6992719B2 (ja) 制御装置
Miao et al. Compound line-of-sight nonlinear path following control of underactuated marine vehicles exposed to wind, waves, and ocean currents
JP2021175590A (ja) ロボット最適化動作計画用の初期参照生成
JP6958574B2 (ja) モデル予測制御のための制御パラメータの設定方法
JP6664807B2 (ja) 制御装置、制御方法、及び制御プログラム
JP2009053926A (ja) 経路計画装置及び経路計画方法
WO2022166330A1 (zh) 位置调整方法、装置、终端设备及可读存储介质
Kalaria et al. Delay-aware robust control for safe autonomous driving
US11513497B2 (en) Control device
JP7003623B2 (ja) 制御システム
EP1338937A1 (en) Servo control method
EP3726301B1 (en) Control device
US11872704B2 (en) Dynamic motion planning system
JP7433556B1 (ja) リニアトラック制御装置およびリニアトラックシステム
US20220075333A1 (en) Control device
JP3121920B2 (ja) 加減速制御装置
Zaini et al. Jerk-limited holonomic robot motion planning using barrier functions
JP2018202579A (ja) 移動体の制御装置及び制御システム
Dovgobrod Generation of a highly-smooth desired path for transverse feedback linearization
JPH0488510A (ja) 軌跡制御装置
CN118550295A (zh) 基于指数传递函数的移动机器人闭环定位方法以及系统
Lin et al. A Novel Robot Path Planning Based on 3d Artificial Potential Field by Force Sensor

Legal Events

Date Code Title Description
A621 Written request for application examination

Free format text: JAPANESE INTERMEDIATE CODE: A621

Effective date: 20201215

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: 20211109

A61 First payment of annual fees (during grant procedure)

Free format text: JAPANESE INTERMEDIATE CODE: A61

Effective date: 20211122

R150 Certificate of patent or registration of utility model

Ref document number: 6992719

Country of ref document: JP

Free format text: JAPANESE INTERMEDIATE CODE: R150