JP2021036390A - 非線形モデル予測制御装置 - Google Patents
非線形モデル予測制御装置 Download PDFInfo
- Publication number
- JP2021036390A JP2021036390A JP2019158051A JP2019158051A JP2021036390A JP 2021036390 A JP2021036390 A JP 2021036390A JP 2019158051 A JP2019158051 A JP 2019158051A JP 2019158051 A JP2019158051 A JP 2019158051A JP 2021036390 A JP2021036390 A JP 2021036390A
- Authority
- JP
- Japan
- Prior art keywords
- control
- collision
- time
- robot
- leg
- 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.)
- Granted
Links
- 238000004364 calculation method Methods 0.000 claims abstract description 18
- 238000005457 optimization Methods 0.000 claims abstract description 16
- 230000004044 response Effects 0.000 claims abstract description 4
- 230000033001 locomotion Effects 0.000 abstract description 25
- 230000002093 peripheral effect Effects 0.000 abstract 3
- 210000002414 leg Anatomy 0.000 description 101
- 238000000034 method Methods 0.000 description 50
- 230000006870 function Effects 0.000 description 25
- 238000004422 calculation algorithm Methods 0.000 description 16
- 230000008859 change Effects 0.000 description 16
- 238000011156 evaluation Methods 0.000 description 15
- 210000004394 hip joint Anatomy 0.000 description 13
- 210000000629 knee joint Anatomy 0.000 description 12
- 238000005070 sampling Methods 0.000 description 12
- 210000002683 foot Anatomy 0.000 description 11
- 238000004088 simulation Methods 0.000 description 11
- 210000000544 articulatio talocruralis Anatomy 0.000 description 10
- 238000010586 diagram Methods 0.000 description 9
- 230000005484 gravity Effects 0.000 description 7
- 238000012545 processing Methods 0.000 description 5
- 210000001503 joint Anatomy 0.000 description 4
- 239000011159 matrix material Substances 0.000 description 4
- 241001465754 Metazoa Species 0.000 description 3
- 230000006399 behavior Effects 0.000 description 3
- 230000005540 biological transmission Effects 0.000 description 3
- 238000009795 derivation Methods 0.000 description 3
- 238000004891 communication Methods 0.000 description 2
- 238000001514 detection method Methods 0.000 description 2
- 238000009472 formulation Methods 0.000 description 2
- 230000005021 gait Effects 0.000 description 2
- 238000003754 machining Methods 0.000 description 2
- 239000000203 mixture Substances 0.000 description 2
- 230000036544 posture Effects 0.000 description 2
- 241000282326 Felis catus Species 0.000 description 1
- 230000003044 adaptive effect Effects 0.000 description 1
- 230000009194 climbing Effects 0.000 description 1
- 238000012937 correction Methods 0.000 description 1
- 238000005265 energy consumption Methods 0.000 description 1
- 230000003993 interaction Effects 0.000 description 1
- 230000014759 maintenance of location Effects 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
- 230000003287 optical effect Effects 0.000 description 1
- 239000013307 optical fiber Substances 0.000 description 1
- 230000000737 periodic effect Effects 0.000 description 1
- 230000000704 physical effect Effects 0.000 description 1
- 230000008569 process Effects 0.000 description 1
- 239000004065 semiconductor Substances 0.000 description 1
- 230000006641 stabilisation Effects 0.000 description 1
- 238000011105 stabilization Methods 0.000 description 1
- 238000011426 transformation method Methods 0.000 description 1
- 230000001052 transient effect Effects 0.000 description 1
Images
Landscapes
- Feedback Control In General (AREA)
Abstract
Description
図1は、実施の形態1にかかるロボットシステム1を示す概略図である。また、図2は、実施の形態1にかかるロボットシステム1の構成を示す機能ブロック図である。ロボットシステム1は、ロボット100と、ロボットの動作を制御する制御装置2とを有する。
非線形モデル予測制御とは、非線形システムに対し、各サンプリング時刻で有限時刻未来までの最適入力(制御入力値の最適解)を求め、得られた入力のうち初期値を実際の入力とする制御である。換言すれば、非線形モデル予測制御部14は、制御対象の非線形制御モデルの最適化問題を演算しながらフィードバック制御を行うことによって、各時刻において将来の制御対象の応答を予測しながら制御対象の制御を行うことが可能に構成されている。非線形モデル予測制御には、非線形最適制御である、フィードバック制御である、及び、拘束条件を組み込み易いという、3つの利点がある。
次に、上述した非線形モデル予測制御を、本実施の形態にかかるロボット100の動作の制御に適用した例について説明する。なお、実施の形態1においては、制御対象としてのロボット100がコンパス型モデルである例について説明するが、非線形モデル予測制御は、ロボット100がコンパス型モデルでなくても適用可能である。
・遊脚と地面90との衝突は完全非弾性衝突とする。すなわち、地面90と衝突した脚が跳ね返ることはない。
・衝突の直後に、それまで支持脚だった脚は相互作用無しで地面90から離れる。
・遊脚と地面90との衝突は一瞬とする。すなわち、両脚支持期は一瞬とする。
・衝突の撃力によりロボットの速度は瞬間的に変わるが、座標は瞬間的には変わらないとものする。
ψ(x)=l(cosθ1−cosθ2)=0 ・・・(A14)
である。
次に、このように一般化したモデルに対して非線形モデル予測制御を適用する際の最適制御問題の定式化を予測ホライゾン上の衝突の回数ごとに導出するとともに、この非線形モデル予測制御を実時間で実行する実時間制御アルゴリズムについて説明する。
ここで、システムが衝突のような不連続現象を含む際は、それに応じた最適性条件を導出する必要がある。そこで、ここでは、ホライゾン上での衝突回数に応じた最適性条件を導出する。
ψk(x(tk−))=0 ・・・(A16)
を満たすことで、状態ジャンプ(状態の不連続変化)
x(tk+)=γk(x(tk−)) ・・・(A17)
が起きる。
ホライゾン上で衝突が起きない時、NMPCではシステムのモデル(A15)とシステムの現時刻の状態x(t)に基づき、評価関数
u(t)=u*(0;t) ・・・(A23)
として与えられる。
x0 *(t)=x(t) ・・・(A24)
xi+1 *(t)=xi *(t)+fk(xi *(t),ui *(t))Δτ,
i=0,・・・,N−1
・・・(A25)
のもとで、評価関数
Hk(x,u,λ)=Lk(x,u)+λTfk(x,u) ・・・(A30)
である。
次に、ホライゾン上で衝突が1回起きる場合について最適性条件を導出する。ホライゾン上の時刻τkで衝突が起きるとすると、状態について、
ikΔτ≦τk≦(ik+1)Δτ ・・・(A38)
として定義すると、状態の予測は
ホライゾンで複数回衝突が起きる場合の最適性条件は、衝突の回数が1回の場合の最適性条件を拡張することで求めることができる。k,・・・,k+l回目の衝突がホライゾン上の時刻τk,・・・,τk+lで起きるとする。このとき、(A38)で求めた衝突が起きるステップik,・・・,ik+lについて、(A40)−(A43)からx*(τk−;t),x*(τk+;t),xik+1 *(t)を、k=k,・・・,k+lについて求めることができる。λについても同様に、(A49)−(A51)からλ*(τk−;t),λ*(τk+;t),λik *(t)を各ik,・・・,ik+lについて求めることができる。ik,・・・,ik+l以外の全てのiについては、xiとλiはそれぞれ(A39)、(A52)から計算することができる。
非線形モデル予測制御を行うためには、上述のように求めた最適性条件を数値的に解く必要がある。本実施の形態では短いサンプリング周期内でもロボットのような複雑なシステムの制御を目標とするため、NMPCの高速数値計算アルゴリズムであるC/GMRES法を用いる。
C/GMRES法は連続変形法とGMRES法を組み合わせた手法である。C/GMRES法では、非線形方程式F(U(t),x(t),t)=0を直接解いてU(t)を直接求めるのではない。C/GMRES法では、U(t)の時間についての連続性を前提として、各サンプリング時刻で最適なU(t)を求め、
U(t+Δt)=U(t)+U(t)Δt ・・・(A64)
として更新する。但し、Δtはサンプリング周期である。
C/GMRES法はU(t)の連続性が前提にされている。その一方で、C/GMRES法は、衝突現象を含むシステムに対する問題は不連続現象が伴い、この(A64)による更新では解の連続性の前提が成り立たなくなる場合がある。この連続性を成り立たせるためには、ホライゾン上でのあるサブシステムkに割り当てられた制御入力が、同様にあるサブシステムに割り当てられた制御入力によって更新される必要がある。
ikΔτ≦τk(t)<ikΔτ+h ・・・(A69)
のとき、この連続性が成り立たなくなる。
次に、衝突によって追加される変数の初期化アルゴリズムについて説明する。
もし時刻t−Δtのホライゾン上にスイッチkが存在しないが、時刻tには存在する場合、すなわちスイッチkが時刻tにホライゾン上に現れる場合、次のようになる。すなわち、このような場合、時刻t−Δtのときの最適制御問題には含まれていなかった新たな変数u*(τk+;t)、νk *、τkが時刻tに追加されることになる。このとき新たな変数u*(τk+;t)、νk *、τkは前の時刻で求めてはいないため、C/GMRES法によりこれらを求めることができない。また、スイッチkがN−1ステップ以前のikで起きたとき、それまでサブシステムkについて最適に求められたuik *(t),・・・,uN−1 *(t)をサブシステムk+1について最適に求め直す必要がある。但し、サンプリング周期を十分小さくしている限り、ほとんどのケースでは、ik=N−1であり、このとき初期化する変数はu*(τk+;t)、νk *、τkだけでよい。このとき、本来であればホライゾン全体についてニュートン法などを用いてもう1度最適制御問題を解く必要があるが、計算時間が膨大になってしまうという問題点がある。そこで、少ない計算時間で部分的に最適な値を求めることでこれらの値を初期化する手法を採用するとよい。以下に、そのような手法について説明する。
1:τkを(A98)により初期化する。
2:u*(τk+;t)とνk *(t)に適当な初期推定解を代入する。
3:x*(τk−;t),x*(τk+;t),xN *(t),λN *(t),λ*(τk+;t)を求め、Fk,init(Uk,init(t),t)を計算する。
4:while |Fk,init(Uk,init(t),t)|<τinit or i<imax do
5:前進差分Newton−GMRES法をFk,init(Uk,init(t),t)に用いることでΔUk,initを求める。
6:Uk,init(t)をUk,init(t)←Uk,init(t)+ΔUk,initと更新する。
7:x*(τk−;t),x*(τk+;t),xN *(t),λN *(t),λ*(τk+;t)を求め、Fk,init(Uk,init(t),t)を計算する。
8:end while
9:Uk,init(t)の初期化終わり。
1:τkを(A98)により初期化する。
2:u*(τk+;t)とνk *(t)に適当な初期推定解を代入する。
3:x*(τk−;t),x*(τk+;t),xik+1 *(t),・・・,xN *(t),λN *(t),・・・,λik+2 *(t),λ*(τk+;t)を求め、Fk,initを計算する。
4:while |Fk,init(U(t),x(t),t)|<τinit or i<imax do
5:前進差分Newton−GMRES法をFk,init(U(t),x(t),t)に用いることでΔUk,initを求める。
6:Uk,init(t)をUk,init(t)←Uk,init(t)+ΔUk,initと更新する。
7:x*(τk−;t),x*(τk+;t),xik+1 *(t),・・・,xN *(t),λN *(t),・・・,λik+2 *(t),λ*(τk+;t)を求め、Fk,initを計算する。
8:end while
9:Uk,init(t)の初期化終わり。
次に、上述した手法を用いて具体的にコンパス型モデルの歩行制御の数値シミュレーションを実行した結果を示す。以下に説明するシミュレーションは、本実施の形態にかかる非線形モデル予測制御のアルゴリズムを、図4で例示したコンパス型モデルにかかるロボット100に適用したものである。
まず、コンパス型歩行制御に用いる際の評価関数を設定する。継続的な歩行制御を実現するために、振り足を前に出す動作を評価関数として加えることを考える。すなわち、遊脚を前に出す速度
シミュレーションに用いたコンパス型モデルの物理パラメータは、m0=m=1.0[kg]、l=1.0[m]、d=0.5[m]、I=0.08333[kg・m2]として与える。
図7〜図12は、本実施の形態にかかる非線形モデル予測制御の例として、コンパス型モデルの歩行制御をシミュレーションした結果を示す図である。図13はその歩行制御におけるホライゾン上での予測衝突時刻のグラフを示す図で、図14は図13の一部を拡大したグラフを示す図である。
上述したように、本実施の形態では、その主たる特徴の一つとして、上記予測手段が制御対象に対して、各時刻において当該時刻から所定期間後までにおける当該衝突の回数を予測する。
なお、本発明は上記実施の形態に限られたものではなく、趣旨を逸脱しない範囲で適宜変更することが可能である。例えば、ロボット100の片方の脚は、股関節部120、膝関節部122及び足首関節部124を有するとしたが、このような構成に限られない。ロボット100の脚は、3個よりも少ない数の関節部を有してもよいし、3個よりも多い数の関節部を有してもよい。この場合、状態ベクトル及び関節トルクベクトル(制御入力値)は、関節部の数に応じて、適宜、変更され得る。そして、状態方程式等の関数も、関節部の数に応じて、適宜、変更され得る。
Claims (1)
- 制御対象の非線形制御モデルの最適化問題を演算しながらフィードバック制御を行うことによって、各時刻において将来の制御対象の応答を予測しながら制御対象の制御を行うことが可能に構成された非線形モデル予測制御装置であって、
繰り返し周囲物との衝突が発生することを前提とする動作を実行制御されるように構成された制御対象に対して、各時刻において当該時刻から所定期間後までにおける当該衝突の回数を予測する予測手段と、
周囲物との衝突が発生することを前提とした前記非線形制御モデルの最適化問題の演算を、予測した衝突回数に応じて実行する演算手段と、
を備える、非線形モデル予測制御装置。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
JP2019158051A JP7221833B2 (ja) | 2019-08-30 | 2019-08-30 | 非線形モデル予測制御装置 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
JP2019158051A JP7221833B2 (ja) | 2019-08-30 | 2019-08-30 | 非線形モデル予測制御装置 |
Publications (2)
Publication Number | Publication Date |
---|---|
JP2021036390A true JP2021036390A (ja) | 2021-03-04 |
JP7221833B2 JP7221833B2 (ja) | 2023-02-14 |
Family
ID=74716671
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
JP2019158051A Active JP7221833B2 (ja) | 2019-08-30 | 2019-08-30 | 非線形モデル予測制御装置 |
Country Status (1)
Country | Link |
---|---|
JP (1) | JP7221833B2 (ja) |
Cited By (1)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN115389077A (zh) * | 2022-08-26 | 2022-11-25 | 法奥意威(苏州)机器人系统有限公司 | 碰撞检测方法、装置、控制设备及可读存储介质 |
Citations (3)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
JPH09251320A (ja) * | 1996-03-14 | 1997-09-22 | Nissan Motor Co Ltd | ロボット衝突防止システム |
JP2016198873A (ja) * | 2015-04-14 | 2016-12-01 | トヨタ自動車株式会社 | 最適制御装置、最適制御方法及び最適制御プログラム |
JP2018185747A (ja) * | 2017-04-27 | 2018-11-22 | 国立大学法人京都大学 | 非線形システムの制御方法、二足歩行ロボットの制御装置、二足歩行ロボットの制御方法及びそのプログラム |
-
2019
- 2019-08-30 JP JP2019158051A patent/JP7221833B2/ja active Active
Patent Citations (3)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
JPH09251320A (ja) * | 1996-03-14 | 1997-09-22 | Nissan Motor Co Ltd | ロボット衝突防止システム |
JP2016198873A (ja) * | 2015-04-14 | 2016-12-01 | トヨタ自動車株式会社 | 最適制御装置、最適制御方法及び最適制御プログラム |
JP2018185747A (ja) * | 2017-04-27 | 2018-11-22 | 国立大学法人京都大学 | 非線形システムの制御方法、二足歩行ロボットの制御装置、二足歩行ロボットの制御方法及びそのプログラム |
Cited By (2)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN115389077A (zh) * | 2022-08-26 | 2022-11-25 | 法奥意威(苏州)机器人系统有限公司 | 碰撞检测方法、装置、控制设备及可读存储介质 |
CN115389077B (zh) * | 2022-08-26 | 2024-04-12 | 法奥意威(苏州)机器人系统有限公司 | 碰撞检测方法、装置、控制设备及可读存储介质 |
Also Published As
Publication number | Publication date |
---|---|
JP7221833B2 (ja) | 2023-02-14 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN111558941B (zh) | 浮动基动力学前馈控制方法、装置和多足机器人 | |
JP5034235B2 (ja) | 制御システム及び制御方法、並びにコンピュータ・プログラム | |
JP6781101B2 (ja) | 非線形システムの制御方法、二足歩行ロボットの制御装置、二足歩行ロボットの制御方法及びそのプログラム | |
US8504208B2 (en) | Mobile object controller and floor surface estimator | |
US20040044440A1 (en) | Gait pattern generating device for legged mobile robot | |
JP6228079B2 (ja) | 移動ロボットの動作目標生成装置 | |
Xu et al. | Adaptive impedance control with variable target stiffness for wheel-legged robot on complex unknown terrain | |
JP7324932B2 (ja) | 動的計画コントローラ | |
Kajita et al. | Biped walking pattern generation based on spatially quantized dynamics | |
WO2006013779A1 (ja) | 脚式移動ロボットの歩容生成装置および制御装置 | |
WO2006067904A1 (ja) | 脚式移動ロボットの歩容生成装置 | |
JP2010221395A (ja) | モーションキャプチャデータを模倣するロボットの追跡及びバランス維持システム及び方法 | |
Dantec et al. | Whole-body model predictive control for biped locomotion on a torque-controlled humanoid robot | |
JP6421683B2 (ja) | 最適制御装置、最適制御方法及び最適制御プログラム | |
Buschmann et al. | Experiments in fast biped walking | |
JP2013184232A (ja) | 脚式歩行ロボットおよびその重心軌道生成方法 | |
Samy et al. | QP-based adaptive-gains compliance control in humanoid falls | |
Suleiman et al. | On humanoid motion optimization | |
JP2021036390A (ja) | 非線形モデル予測制御装置 | |
WO2015087504A1 (ja) | 移動ロボットの移動制御方法、および、移動ロボット | |
Xie et al. | Gait optimization and energy-based stability for biped locomotion using large-scale programming | |
Li et al. | Dynamic Locomotion of a Quadruped Robot with Active Spine via Model Predictive Control | |
Wang et al. | Fast Terrain-Adaptive Motion of Humanoid Robots Based on Model Reference One-Step-Ahead Predictive Control | |
JP5440152B2 (ja) | 脚式ロボットとそのための歩容データ生成方法 | |
Suleiman et al. | Optimizing humanoid motions using recursive dynamics and lie groups |
Legal Events
Date | Code | Title | Description |
---|---|---|---|
A80 | Written request to apply exceptions to lack of novelty of invention |
Free format text: JAPANESE INTERMEDIATE CODE: A80 Effective date: 20190920 |
|
A621 | Written request for application examination |
Free format text: JAPANESE INTERMEDIATE CODE: A621 Effective date: 20220117 |
|
TRDD | Decision of grant or rejection written | ||
A977 | Report on retrieval |
Free format text: JAPANESE INTERMEDIATE CODE: A971007 Effective date: 20221228 |
|
A01 | Written decision to grant a patent or to grant a registration (utility model) |
Free format text: JAPANESE INTERMEDIATE CODE: A01 Effective date: 20230110 |
|
A61 | First payment of annual fees (during grant procedure) |
Free format text: JAPANESE INTERMEDIATE CODE: A61 Effective date: 20230202 |
|
R150 | Certificate of patent or registration of utility model |
Ref document number: 7221833 Country of ref document: JP Free format text: JAPANESE INTERMEDIATE CODE: R150 |