JP2020052816A

JP2020052816A - 制御装置

Info

Publication number: JP2020052816A
Application number: JP2018182621A
Authority: JP
Inventors: 研太郎浦邊; Kentaro Urabe; 守恵木; Mamoru Egi
Original assignee: Omron Corp; Omron Tateisi Electronics Co
Current assignee: Omron Corp
Priority date: 2018-09-27
Filing date: 2018-09-27
Publication date: 2020-04-02
Anticipated expiration: 2038-09-27
Also published as: CN112534376A; US11586215B2; EP3859457A1; US20210311487A1; EP3859457A4; WO2020067445A1; CN112534376B; JP7070295B2

Abstract

【課題】モデル予測制御により移動体を目標軌道に対して追従させる場合において、障害物との衝突を好適に回避することを可能とする制御装置を提供する。【解決手段】制御装置のモデル予測制御部は、回避すべき複数の実障害物が存在する場合に動作する障害物回避用制御手段であって、取得部によって取得された当該複数の実障害物の位置から当該複数の実障害物の間に位置するように仮想障害物の位置を決定し、ステージコストとして、標準コストと、少なくとも、前記仮想障害物の位置と前記移動体の位置とをパラメータとした所定の関数に仮想障害物用重みを乗じた仮想障害物評価項との加算結果を使用して、モデル予測制御を行う障害物回避用制御手段を含む。【選択図】図２

Description

本発明は、移動体を移動させる駆動装置を制御する制御装置に関する。

障害物と衝突することなく移動体を移動させるために、当該移動体の経路を決定するための評価関数に、障害物毎に、その障害物に移動体を接近させないための評価項（ペナルティ項；以下、障害物評価項と表記する）を含めることが行われている（例えば、特許文献１、２参照）。

特開２０１１−１８６８７８号公報特開２０１４−１３０４９５号公報

障害物毎に障害物評価項を含めた評価関数を用いれば、障害物と衝突することなく移動体を移動させることが出来る。ただし、単に、障害物評価項を含めた評価関数を用いたのでは、移動体が停留状態になる、移動体の障害物回避経路が大回りになるといったように、障害物との衝突を好適に回避できない場合が生じ得る。

本発明は、このような問題に鑑みてなされたものであり、モデル予測制御により移動体を目標軌道に対して追従させる場合において、障害物との衝突を好適に回避することを可能とする制御装置を提供することを目的とする。

上記目的を達成するために、本発明の１観点に係る、移動体を移動させる駆動装置を制御する制御装置は、前記移動体の、目標軌道に沿った移動の障害となり得る各実障害物の位置を取得する取得部と、前記駆動装置への制御入力から前記移動体の位置を予測するための予測モデルに基づき、所定時間幅の予測区間におけるステージコストの総和を含む評価関数の値が最小となる前記制御入力を算出して前記駆動装置に供給することにより前記移動体を前記目標軌道に沿った形で移動させるモデル予測制御部であって、前記取得部からの情報により回避すべき実障害物の有無を判定し、回避すべき実障害物が存在しない場合には、前記ステージコストとして標準コストを使用する前記モデル予測制御部と、を備える。そして、制御装置のモデル予測制御部は、回避すべき複数の実障害物が存在する場合に動作する障害物回避用制御手段であって、前記取得部によって取得された当該複数の実障害物の位置から当該複数の実障害物の間に位置するように仮想障害物の位置を決定し、前記ステージコストとして、前記標準コストと、少なくとも、前記仮想障害物の位置と前記移動体の位置とをパラメータとした所定の関数に仮想障害物用重みを乗じた仮想障害物評価項との加算結果を使用して、前記評価関数を最小化する前記制御入力を算出する障害物回避用制御手段を含む。

すなわち、制御装置は、単に障害物評価項を含めた評価関数を用いたのでは、移動体と障害物との衝突を好適に回避できない場合（目標軌道を挟むように２つの障害物が存在している場合等）に、仮想障害物が存在するとして仮定して制御入力が算出される構成を
有する。従って、本制御装置によれば、単に障害物評価項を含めた評価関数を用いたのでは、移動体と障害物との衝突を好適に回避できない場合にも、移動体と障害物との衝突を
好適に回避することが出来る。

障害物回避用制御手段は、前記ステージコストとして、前記標準コストと、前記仮想障害物評価項と、回避すべき各実障害物についての、前記所定の関数に重みを乗じた実障害物評価項との加算結果を使用する手段であっても、前記ステージコストとして、前記標準コストと前記仮想障害物評価項との加算結果を使用する手段であっても良い。

障害物回避用制御手段として後者の手段を採用する場合には、障害物回避用制御手段に、『動作開始時に、その時点における前記仮想障害物評価項の算出結果が、回避すべき各実障害物についての、前記所定の関数に重みを乗じた実障害物評価項の、その時点における値の総和と一致することになるように、前記仮想障害物用重みの値を決定する』機能を付与しておくことが好ましい。

また、障害物回避用制御手段は、回避すべき２つ又は３つの実障害物が存在する場合には、前記仮想障害物の位置を、当該２つ又は３つの実障害物から等距離の、各実障害物との間の距離が最も短い位置に決定しても良い。

また、制御装置に、『前記モデル予測制御部は、前記移動体が減速状態となったか否かを判定する判定手段を、さらに含み、前記モデル予測制御部の前記障害物回避用制御手段は、回避すべき複数の実障害物が存在する状況下において、前記判定手段により前記移動体が減速状態となったと判定された場合に動作を開始する』構成を採用しておいても良い。なお、判定手段としては、例えば、前記移動体の位置又は前記評価関数の最小値の今回値と前回値の差の絶対値が所定の閾値未満となったときに、前記移動体が減速状態となったと判定する手段や、前記移動体の位置又は前記評価関数の最小値の今回値と前回値の差の絶対値が所定回数連続して所定の閾値未満となったときに、前記移動体が減速状態となったと判定する手段を採用することが出来る。

また、障害物回避用制御手段として、回避すべき２つの実障害物が存在する場合には、前記所定の関数として、前記仮想障害物評価項の前記２つの実障害物の並び方向における変化率が他の方向よりも小さくなる関数を使用する手段を採用しておいても良い。

モデル予測制御部は、取得部により位置が取得された実障害物の全てが、回避すべき障害物であるとするものであっても良い。また、モデル予測制御手段は、前記取得部により複数の実障害物の位置が取得された場合、当該複数の実障害物のそれぞれについて、前記移動体との衝突が生じ得る確率を表す評価値を算出し、各実障害物について算出した評価値に基づき、前記複数の実障害物のいずれが回避すべき実障害物であるかを決定しても良い。

本発明によれば、モデル予測制御により移動体を目標軌道に対して追従させる場合において、移動体と障害物との衝突を好適に回避することが可能となる。

本発明の一実施形態に係る制御装置の使用形態の説明図である。実施形態に係る制御装置の概略構成の説明図である。実施形態に係る制御装置による制御結果例の説明図である。実施形態に係る制御装置の機能の説明図である。実施形態に係る制御装置による制御結果例の説明図である。

以下、図面を参照して本発明の実施の形態について説明する。

図１に、本発明の一実施形態に係る制御装置１０の使用形態を示し、図２に、本実施形態に係る制御装置１０の概略構成を示す。

本実施形態に係る制御装置１０（図１）は、それぞれ、特定の負荷装置２２を駆動する複数（図では、１つ）のモータ２１をモデル予測制御により制御するための、一般にサーボアンプ、サーボドライバ等と称されている装置である。なお、負荷装置２２とは、産業用ロボットのアームや搬送装置等のことである。以下、制御装置１０により制御される複数のモータ２１と各モータ２１により駆動される負荷装置２２とからなる部分のことを制御対象２０と表記する。

図２に示してあるように、制御装置１０は、取得部１１及びモデル予測制御部１２として機能するように構成されている。

取得部１１は、カメラ３０による撮像結果を解析することで、制御対象２０内の移動体（ロボットの手先部等；以下、単に移動体と表記する）の周囲に存在する各実障害物の位置（中心位置）及びサイズを取得するユニットである。なお、カメラ３０は、移動体の進行方向の所定範囲内のみを撮像するものであっても良い。また、取得部１１は、移動体の進行方向の所定範囲内に存在する各実障害物（換言すれば、回避すべき実障害物）のみの位置及びサイズを取得するものであっても良い。

モデル予測制御部１２は、取得部１１により取得された各障害物の位置及びサイズと、制御対象２０からの情報（移動体の位置等）とに基づき、移動体が、各障害物を回避しながら、ＰＬＣ(Programmable Logic Controller)３１から指示される目標軌道に沿って移
動するように、モデル予測制御により制御対象２０を制御するユニットである。

このモデル予測制御部１２が行う制御は、『制御対象２０の予測モデルを用いて、制御対象の状態ｘに基づき、以下の評価関数Ｊの値が最小となる入力ｕを算出して制御対象２０に供給する』というものである。

なお、式１の右辺の第１項は、終端コストであり、式１におけるＬは、ステージコストである。また、Ｔは、モデル予測制御における予測区間の長さとして予め設定されている時間幅である。

すなわち、モデル予測制御部１２が行う制御は、基本的には、一般に行われているモデル予測制御と同様のものとなっている。

ただし、モデル予測制御部１２は、状況に応じて、Ｊ値の算出に用いるステージコストＬを切り替えるように構成されている。

以上のことを前提に、以下、モデル予測制御部１２の機能を詳細に説明する。なお、制御装置１０は、移動体を、３次元空間内を移動させることも可能な装置である。ただし、以下では、説明の便宜上、移動体が、平面上を移動するものであるとする。

モデル予測制御部１２は、取得部１１により取得された各実障害物の位置及びサイズに基づき、回避すべき実障害物の有無を周期的に判定する。より詳細には、モデル予測制御部１２は、取得部１１により実障害物の位置及びサイズが取得されなかった場合（実障害物が存在していない場合）には、回避すべき実障害物が存在していないと判定する。また、モデル予測制御部１２は、取得部１１により１つ以上の実障害物の位置及びサイズが取得された場合には、取得部１１により位置及びサイズが取得された実障害物の中から、各実障害物の位置及びサイズに基づき、以下の式２で定義される実障害物評価項Ｌ_ｋの値が規定値以上であるという条件を満たす実障害物を特定する。そして、モデル予測制御部１２は、特定した各実障害物が回避すべき実障害物であると判定する。

上記式２において、添え字ｋは、上記条件を満たす各実障害物に割り当てられる、１から始まる番号である。ｘ_１，ｙ_２は、移動体の位置（ｘ、ｙ座標）であり、ｘ_ｏｋ，ｙ_ｏｋは、実障害物ｋの位置（ｘ、ｙ座標）である。また、ｇ_ｋは、実障害物ｋのサイズに応じて定められる値である。

なお、モデル予測制御部１２は、上記条件を満たす実障害物が４つ以上存在していた場合には、実障害物評価項の値の降順に３つの実障害物を選択し、選択した各実障害物が、回避すべき実障害物であると判定するように構成されている。従って、ｋの最大値は、３である。

また、モデル予測制御部１２は、移動体が減速状態となったか否かを周期的に判定するようにも構成されている。この判定処理は、移動体の位置の今回値と前回値の差の絶対値が、所定の位置閾値未満となったときに、移動体が減速状態となったと判定するものであっても、算出されたＪ値の最小値の今回値と前回値の差の絶対値が、所定のＪ値閾値未満となったときに、移動体が減速状態となったと判定するものであっても良い。また、誤検出を防ぐために、上記判定処理を、絶対値が移動体の位置又はＪ値の最小値の今回値と前回値の差の絶対値が所定回数連続して閾値未満となったときに、移動体が減速状態となったと判定する処理としておいても良い。

上記のように、モデル予測制御部１２は、移動体が減速状態となったか否かと回避すべき実障害物の有無とを周期的に判定するが、モデル予測制御部１２は、双方の判定結果に基づき、以下の第１〜第３条件のいずれが満たされているかを判定する処理も行う。
・第１条件：回避すべき実障害物が存在していない。
・第２条件：回避すべき１つ以上の実障害物が存在しており、且つ、第３条件が満たされていない。
・第３条件：回避すべき複数の実障害物が存在している状況下、移動体が減速状態となった。

そして、モデル予測制御部１２は、第１条件が満たされている場合（回避すべき実障害物が存在していない場合）には、ステージコストＬとして、以下の基準コストＬ_０を使用するモデル予測制御である第１モデル予測制御を行う。

なお、ｘｒｅｆは、予測区間内の或る時刻における目標状態量であり、ｘは、同時刻における状態量、ｕは、同時刻における制御入力である。また、Ｑ及びＲは、それぞれステージコストにおける状態量の重みを表す係数（重み係数）、制御入力の重みを表す係数（重み係数）である。

また、モデル予測制御部１２は、第２条件が満たされている場合には、ステージコストＬとして、以下の値、すなわち、基準コストＬ_０と各実障害物の実障害物評価項Ｌ_ｋとの和を使用するモデル予測制御である第２モデル予測制御を行う。

なお、上記式におけるｋｍａｘは、回避すべき実障害物の総数（本実施形態では、≦３）である。

また、モデル予測制御部１２は、第３条件が満たされた場合には、まず、回避すべき２つ又は３つの実障害物から等距離の、各実障害物との間の距離が最も短い位置に決定する。次いで、モデル予測制御部１２は、以下の仮想障害物評価項の値が、各実障害物の仮想障害物評価項の値の総和となる重みｇ_ｐを算出する。

なお、ｘ_pｋ，ｙ_pkは、決定した仮想障害物の位置（ｘ、ｙ座標）である。

具体的には、例えば、回避すべき実障害物が２つ存在する場合、モデル予測制御部１２は、その時点における（移動体が現在位置にある場合における）実障害物評価項Ｌ_１、Ｌ_２の値Ｖ_１、Ｖ_２を用いて、以下の式によりｇ_ｐを算出する。

そして、モデル予測制御部１２は、ステージコストＬとして、基準コストＬ_０と仮想障害物評価項Ｌ_ｐとの和を使用するモデル予測制御である第３モデル予測制御を開始する。

以上、説明したように、制御装置１０には、回避すべき複数の実障害物が存在している状況下、移動体が減速状態となった場合に、各実障害物の障害物評価項を仮想障害物評価項に置換した評価関数を用いるモデル予測制御（第３モデル予測制御）を行う機能が付与されている。当該場合は、単に、障害物評価項を含めた評価関数を用いたのでは、移動体が停留状態になる、移動体の障害物回避経路が大回りになるといったような問題が生じ易い場合である。そして、第３モデル予測制御によれば、図３に実線で示してあるように、移動体を、上記のような不具合が生じない形で移動させることが出来る。なお、図３において、ｏｂｓ１、ｏｂｓ２は、回避すべき実障害物であり、ｐｏｂｓは、仮想障害物である。また、×は、目標軌道を規定する目標位置群（ＰＣＳ３２からの指令）である。

また、制御装置１０は、障害物評価項の値の大きな実障害物のみを、回避すべき実障害物として取り扱うように構成されている。従って、例えば、図４に示したように、４つの実障害物ｏｂｓ１〜ｏｂｓ４が存在していても、障害物評価項の値の小さな実障害物ｏｂｓ３及びｏｂｓ４は、回避すべき実障害物として取り扱われない。そのため、図５にしたように、多数の実障害物（図では、ｏｂｓ１〜ｏｂｓ４）が存在していても、実障害物の回避経路が過度に大回りになることを防止できることにもなる。

《変形形態》
上記した実施形態に係る制御装置１０は、各種の変形が可能なものである。例えば、第３モデル予測制御時に、以下のステージコストＬが使用されるようにしておいても良い。

なお、上記ステージコストＬが使用されるようにする場合、ｇ_ｐ値は、上記値よりも小さな値（例えば、予め設定されている値）とすることが出来る。

また、第３モデル予測制御時に、上記式３における各ｇ_ｋ値を“０”とした演算が行われるようにしておいても良い。

また、実障害物評価項や仮想障害物評価項は、移動体との距離が短くなる（換言すれば、移動体との衝突確率が高くなる）につれ、値が大きくなる関数であれば良い。従って、各評価項として、上記したものとは異なる関数を使用しても良い。また、回避軌道が過度に大回りになるのを防ぐため（目標軌道から離れる期間を短くするため）に、回避すべき２つの実障害物が存在する場合には、仮想障害物評価項として、それら２つの実障害物の並び方向における変化率が他の方向よりも小さくなる関数が使用されるようにしておいても良い。

制御装置１０を、回避対象とする実障害物の選択時と実際のモデル予測制御時とに、異なる実障害物評価項を使用する装置に変形することも出来る。また、カメラ３０がＬＩＤＡＲ（Light Detection and Ranging）等であっても良いことや、制御装置１０を、車輌
の移動方向を制御する装置に変形しても良いことなどは、当然のことである。

《付記》
移動体を移動させる駆動装置（２１）を制御する制御装置（１０）であって、
前記移動体の、目標軌道に沿った移動の障害となり得る各実障害物の位置を取得する取
得部（１１）と、
前記駆動装置への制御入力から前記移動体の位置を予測するための予測モデルに基づき、所定時間幅の予測区間におけるステージコストの総和を含む評価関数の値が最小となる前記制御入力を算出して前記駆動装置に供給することにより前記移動体を前記目標軌道に沿った形で移動させるモデル予測制御部（１２）であって、前記取得部からの情報により回避すべき実障害物の有無を判定し、回避すべき実障害物が存在しない場合には、前記ステージコストとして標準コストを使用する前記モデル予測制御部（１２）と、
を備え、
前記モデル予測制御部（１２）は、
回避すべき複数の実障害物が存在する場合に動作する障害物回避用制御手段であって、前記取得部によって取得された当該複数の実障害物の位置から当該複数の実障害物の間に位置するように仮想障害物の位置を決定し、前記ステージコストとして、前記標準コストと、少なくとも、前記仮想障害物の位置と前記移動体の位置とをパラメータとした所定の関数に仮想障害物用重みを乗じた仮想障害物評価項との加算結果を使用して、前記評価関数を最小化する前記制御入力を算出する障害物回避用制御手段を含む、
ことを特徴とする制御装置（１０）。

１０・・・制御装置
１１・・・取得部
１２・・・モデル予測制御部
２０・・・制御対象
２１・・・モータ
２２・・・負荷装置
３０・・・カメラ
３１・・・ＰＬＣ

Claims

移動体を移動させる駆動装置を制御する制御装置であって、
前記移動体の、目標軌道に沿った移動の障害となり得る各実障害物の位置を取得する取得部と、
前記駆動装置への制御入力から前記移動体の位置を予測するための予測モデルに基づき、所定時間幅の予測区間におけるステージコストの総和を含む評価関数の値が最小となる前記制御入力を算出して前記駆動装置に供給することにより前記移動体を前記目標軌道に沿った形で移動させるモデル予測制御部であって、前記取得部からの情報により回避すべき実障害物の有無を判定し、回避すべき実障害物が存在しない場合には、前記ステージコストとして標準コストを使用する前記モデル予測制御部と、
を備え、
前記モデル予測制御部は、
回避すべき複数の実障害物が存在する場合に動作する障害物回避用制御手段であって、前記取得部によって取得された当該複数の実障害物の位置から当該複数の実障害物の間に位置するように仮想障害物の位置を決定し、前記ステージコストとして、前記標準コストと、少なくとも、前記仮想障害物の位置と前記移動体の位置とをパラメータとした所定の関数に仮想障害物用重みを乗じた仮想障害物評価項との加算結果を使用して、前記評価関数を最小化する前記制御入力を算出する障害物回避用制御手段を含む、
ことを特徴とする制御装置。
前記障害物回避用制御手段は、前記ステージコストとして、前記標準コストと、前記仮想障害物評価項と、回避すべき各実障害物についての、前記所定の関数に重みを乗じた実障害物評価項との加算結果を使用する、
ことを特徴とする請求項１に記載の制御装置。
前記障害物回避用制御手段は、前記ステージコストとして、前記標準コストと前記仮想障害物評価項との加算結果を使用する、
ことを特徴とする請求項１に記載の制御装置。
前記障害物回避用制御手段は、動作開始時に、その時点における前記仮想障害物評価項の算出結果が、回避すべき各実障害物についての、前記所定の関数に重みを乗じた実障害物評価項の、その時点における値の総和と一致することになるように、前記仮想障害物用重みの値を決定する、
ことを特徴とする請求項３に記載の制御装置。
前記障害物回避用制御手段は、回避すべき２つ又は３つの実障害物が存在する場合には、前記仮想障害物の位置を、当該２つ又は３つの実障害物から等距離の、各実障害物との間の距離が最も短い位置に決定する、
ことを特徴とする請求項１から４のいずれか一項に記載の制御装置。
前記モデル予測制御部は、前記移動体が減速状態となったか否かを判定する判定手段を、さらに含み、
前記モデル予測制御部の前記障害物回避用制御手段は、回避すべき複数の実障害物が存在する状況下において、前記判定手段により前記移動体が減速状態となったと判定された場合に動作を開始する、
ことを特徴とする請求項１から５のいずれか一項に記載の制御装置。
前記判定手段は、前記移動体の位置又は前記評価関数の最小値の今回値と前回値の差の絶対値が所定の閾値未満となったときに、前記移動体が減速状態となったと判定する、
ことを特徴とする請求項６に記載の制御装置。
前記判定手段は、前記移動体の位置又は前記評価関数の最小値の今回値と前回値の差の絶対値が所定回数連続して所定の閾値未満となったときに、前記移動体が減速状態となったと判定する、
ことを特徴とする請求項７に記載の制御装置。
前記障害物回避用制御手段は、回避すべき２つの実障害物が存在する場合には、前記所定の関数として、前記仮想障害物評価項の前記２つの実障害物の並び方向における変化率が他の方向よりも小さくなる関数を使用する、
ことを特徴とする請求項４に記載の制御装置。
前記モデル予測制御部は、前記取得部により複数の実障害物の位置が取得された場合、当該複数の実障害物のそれぞれについて、前記移動体との衝突が生じ得る確率を表す評価値を算出し、各実障害物について算出した評価値に基づき、前記複数の実障害物のいずれが回避すべき実障害物であるかを決定する、
ことを特徴とする請求項１に記載の制御装置。