RSJ2014AC3M1-03
クアッドコプタのパーティクルフィルタモデル予測制御
島田 健人 (九州工業大学) 津上 聖也 (九州工業大学) ○西田 健 (九州工業大学)
1.
はじめに
近年，マルチコプタと呼ばれる垂直上方を向いた複数
のプロペラを有する UAV（Unmanned Aerial Vehicle）
の設計 [1] やその制御 [2, 3] に関する研究が数多く行わ
れており，特に，四つのプロペラを有するクアッドコ
プタに関する研究が盛んに行われている．クアッドコ
プタには十字のボディーフレームの端にロータとプロ
ペラが組み合わされて搭載されている．クアッドコプ
タは，それぞれのプロペラの回転速度を変化させるこ
とで，前後上下左右の移動やピッチング動作が可能で
あり，垂直離着陸や一定位置でのホバリングが可能で
ある．
一方で，クアッドコプタは入力飽和が発生する非線
形システムであり，空中で風などの外乱の影響を強く
受ける．位置誤差のフィードバック制御系などの簡便な
制御系では，制御入力の変動や絶対値が過大になり飽
和する傾向があり，不安定化を招く場合がある [4]．そ
こで本研究では，このような問題の解決に有効なモデ
ル予測制御法 (model predictive control, MPC) の導入
を考える．しかし一般の予測制御法では，参照軌道に
対する最適な予測入力の算出は確定論的に行われ，予
測制御中の確率的に振舞う外乱の影響は考慮されない．
そこで，本研究ではパーティクルフィルタモデル予測
制御 [5, 6] を，上述の不確実さを考慮したクアッドコ
プタの予測制御法のために導入する．これにより，ク
アッドコプタの状態量を任意の範囲に留め，制御不能
や墜落を生じる可能性のある制御入力の発生を防ぐこ
とができるようになることが期待できる．
2.
クアッドコプタの運動モデル
2.1
連続時間モデル
座標系とクアッドコプタ座標系の関係を図 1 に示す．
慣性座標系を Σo ，ロボットに固定されたロボット座標
系を Σr とする．また Σr の z 軸の上向きを正とする．
Σo に対する Σr の回転姿勢は，3 つのオイラー角によっ
て定まる．回転変換は次のように表すことができる．
RO
r (θz , θy , θx ) = Rz (θz )Ry (θy )Rx (θx )
(1)
ここで，Rz (θz )，Ry (θy )，Rx (θx ) はそれぞれ z 軸，y
軸，x 軸周りの回転座標変換を表す．また Σo に対す
る Σr の位置を z(t) = [x(t) y(t) z(t)]T とする．ロボッ
トは四つのプロペラがあり，十字のボィーフレームの
四端にそれぞれ固定されており，プロペラはロータに
よって回転し推力を得る．ローターの角速度を si (t) と
表すと，鉛直上向きの推力は
fi (t) = bs2i (t)
(i = 1, 2, 3, 4)
(2)
と表せる．ここで，b は推力定数と呼ばれる正の定数
である．ΣO に対するクアッドコプタの運動モデルは
ニュートンの第二法則に従って次のように与えられる．
0
0
vx (t)
(3)
m¨
z (t) = RO
r 0 − 0 − K vy (t)
F (t)
mg
vz (t)
ただし，g は重力加速度，m はロボットの質量，F (t) =
∑
fi (t) は上方向の推力である．右辺第二項は重力であ
り，Σo の Z 軸方向，すなわち鉛直下向きに作用する．
右辺第三項は空気抵抗に関する項で K ∈ R3×1 は比例
定数による行列を表し，v(t) , [vx (t) vy (t) vz (t)]T は
Σo に対するロボットの速度である．
次に，オイラーの運動方程式より回転に関する運動
は次のように与えられる．
˙
J ω(t)
= −ω(t) × J ω(t) + T (t) − K e ω(t)
(4)
ここで，J ∈ R3×3 は慣性行列，ω(t) は角速度ベクト
T
ル，T (t) , [τxr (t) τyr (t) τzr (t)] はロボットに働くト
ルクである．右辺第三項は角速度による抵抗に関する
項で K e , [Kex Key Kez ]T は比例定数行列である．慣
性行列は正定値対称行列であり，適当な直交座標系を
選ぶことで対角化できる．また，そのときの座標軸を
慣性主軸と呼び，慣性行列 J は次のように表される．
Ixx 0
0
J = 0 Iyy 0
(5)
0
0
Izz
これを用いると，式 (4) は次のように表される．
Ixx ω˙1 (t) + (Izz − Iyy ) ω2 (t)ω3 (t) = τx (t) − Kex ω1
Iyy ω˙2 (t) + (Ixx − Izz ) ω3 (t)ω1 (t) = τy (t) − Key ω2
図 1 慣性座標系とクアッドコプタの座標系の関係．
第32回日本ロボット学会学術講演会（2014年9月4日～6日）
Izz ω˙3 (t) + (Iyy − Ixx ) ω1 (t)ω2 (t) = τz (t) − Kez ω3 ,
RSJ2014AC3M1-03
T
˙
こ こ で ，ω(t)
, [ω˙ 1 (t) ω˙ 2 (t) ω˙ 3 (t)] ，ω(t) ,
T
[ω1 (t) ω2 (t) ω3 (t)] である．
クアッドコプタの運動モデルは式 (3) と式 (4) を統合
することによって得ることができる．
]
[
F (t)
= As2 (t)
(6)
T (t)
ここで，
b
b
0
−db
A,
−db
0
−k
k
[
s2 (t) , s21 (t) s22 (t)
b
0
db
−k
b
db
0
k
(7)
s23 (t) s24 (t)
]T
xk+1 = xk
∆2 {
vk }
∆3 v k + { 2 3 RO
r (θ k )F k (sk ) − g − K
m
}
vk
∆3 RO
r (θ k )F k (sk ) − g − K m
+
}
∆23 { −1
∆3 ω k +{ 2 J (−ω k × J ω k + T k (sk ) − K e}ω k )
∆3 J −1 (−ω k × Jω k + T k (sk ) − K e ω k )
ここで，F k (sk ) = [0 0 F (t)/m]T ，g = [0 0 g]T であ
る. したがって, この状態方程式は非線形である．
離散時間状態方程式と観測方程式は次のように表現
できる．
(8)
である．これは，ロータ回転数の関数である．Aは，
b, k, d > 0 ならばフルランクであり，機体に特定の推
力とモーメントを与えるロータの回転周波数は次のよ
うに与えられる．
[
]T
s2 (t) = A−1 F (t) τxr (t) τyr (t) τzr (t)
(9)
2.2
行列, 03 ∈ R3×3 は零行列, ∆3 = diag[∆, ∆, ∆], そし
て ∆23 = diag[∆2 , ∆2 , ∆2 ] である. さらに，方程式は
次のように表現できる．
離散時間状態方程式
計算機やセンサによる計測，通信などの実際のクアッ
ドコプタの構成要素は離散時間で駆動されるため，そ
の制御系も実際には離散系として構成される．また，予
測制御系や時系列の確率推定手法は離散時間系を対象
として理論が構築されている．したがって，ここでは
前述した連続時間における運動方程式を離散時間の記
述に変換する．
まず，クアッドコプタの重力中心の速度，加速度，角
速度，角加速度を次のように表す．
¨ (t) , v(t)
˙
z
, a(t),
(10)
¨ , ω(t)
˙
θ(t)
, α(t)
(11)
(
)
xk = ϕ xk−1 , uk−1 , ξk−1
(15)
y k = ψ (xk , η k )
(16)
ここで，ϕ は状態遷移関数, ξ k はシステムノイズ, ψ は
計測関数，η k は計測ノイズである．
3.
パーティクル予測制御
従来の MPC は，有限の予測時間区間において入力
を最適化する．MPC の主な利点は現在の時間区間を最
適化することができ，一方で将来の時間区間を考慮す
ることができることである (図 2) が，計測ノイズや不
確定な外乱を考慮しない．そこで，そのような不確定
性を考慮できる PF-MPC [6] を導入する．
PF-MPC では，現時刻から NP までの複数の予測
¯ k:k+NP が計測ノイズやシステムノイズを考慮し生成
x
され，入力は目標軌道との偏差と xk:k+NP に基づいた
評価関数によって制御則が最適化される．以下に具体
的な PF-MPC のアルゴリズムを示す．
(Particle Filtering)
[m]
これらにテーラー展開を適用し，利用可能な信号が存
在する次元までの項を用いると次のように表現できる．
xk+1 = Φxk + Γ uk
ここで，
(12)
given 粒子の初期状態 x0 を想定される状態量周辺
[m]
に与える．粒子の重みは w0 = 1/M とする．
step 1-1 サンプリングする．
(m)
zk
I3
vk
03
xk ,
θ , Φ , 0
k
3
ωk
03
∆2
3
03
2
∆3
0
, uk
Γ ,
2
03 ∆2 3
03 ∆ 3
∆3
I3
03
03
03
03
I3
03
xk
03
03
∆3
I3
(m)
= ϕ(xk−1 , uk−1 , ξ k−1 )
(17)
(13)
Reference trajectory
Predicted output
Measured output
Predicted control input
[
ak
,
αk
Past control input
]
(14)
k は離散時間，∆ はサンプリング時間，ak ∈ ΣO は加
速度 ω k は角速度，αk は角加速度，I 3 ∈ R3×3 は単位
第32回日本ロボット学会学術講演会（2014年9月4日～6日）
Prediction horizon
Sampling time
k
k+1 k+2
...
図 2 モデル予測制御．
k+p
RSJ2014AC3M1-03
step 1-2 正規化重みを計算する．
(m)
w
¯k
(m)
(m)
= wk−1 h(y k |xk )
(18)
w
¯
= ∑M k
j=1
軌道追従シミュレーション
4.1
条件
4.1.1
(m)
(m)
wk
4.
(19)
(j)
w
¯k
step 1-3 Nef f < NT であればリサンプリングを行う．
[7]: リサンプリングは次式によって行う [7]．
1
Nef f = ∑
)2 .
(
(i)
M
w
i=1
k
(20)
各パラメータを次のように設定した．∆ = 0.01
[s], m = 420 [g], Ixx = Iyy = 5.136 [gm2 ],
Izz = 10.16 [gm2 ], d = 0.21 [m], NP = 40,
ξk ∼ N (0, Σv ), Σv = diag(0.03, 0, 0.03, 0, 0, 0), η k ∼
N (0, Σw ), Σw = diag(0.001, 0, 0.001, 0, 0, 0), Σu =
diag(10.5, 0, 10.5, 0, 6, 0), g = 9.807 [ms−2 ]. 粒子数
は M = 20 とした. 実験的に，比例定数ベクトルは
K = [1 1 1]T ，K e = [1 1 1]T とした.
4.1.2
全ての粒子の重みが等しい場合は Nef f = M ，重みの
種類が最大のときは Nef f = 1 となり，有効に利用され
ている粒子の数を示す指標として用いることができる．
これは，頻繁なリサンプリングを避けるために導入さ
れる．適切な閾値 NT を設定し，得られた Nef f が NT
未満であるときリサンプリングを行う．
(Particle Filtering-Model Predictive Control)
Given それぞれの粒子の入力は次のように生成される．
(m)
¯k
u
= uk−1 + v, v ∼ N (0, Σu )
(m)
¯k
さらに, x
(m)
(m)
= xk ，w
¯k
(m)
= wk
(21)
これらの関係を図 3 に示す.
4.1.3
(m)
Step 2-2 粒子のサンプリング
(
)
(m)
(m)
(m)
¯ k+j = ϕ x
¯ k+j−1 , u
¯ k+j−1 , ξ k+j−1
x
(
hp
(m)
w
¯k+j =
(m)
sk+j |¯
xk+j
(23)
}
2σd2
(28)
ここで，σd2 = 0.03 である.
4.1.4
入力の決定
パーティクル予測によって，時刻 k + NP ステップで
目標軌道と最も近い状態を持った粒子の時刻 k + 1 に
(25)
Step 2-5 j = 1 から j = NP まで上記の過程を繰り
返す.
第32回日本ロボット学会学術講演会（2014年9月4日～6日）
∼ exp −
(m)
dk+j
(m)
rz
次の評価関数に基づいている．
{
¯ k+j と目標軌道との
ここで，j ステップ後の予測粒子 x
距離は次のように計算できる．
(
)
(m)
x
¯
−yxk
(m)
(m)
− k+jTs
dk+j , z¯k+j − (rz − yzk ) 1 − e
+ yzk x[m]
k+3
¯ef f < N
¯T であればリサンプリングを行う．
Step 2-4 N
[m]
¯ k+j を決める.
入力系列 u
Step 3 次式から uk+1 を決定する．
}
{
(m)
(m)
(m)
¯ k+1:k+NP , u
¯ k+1:k+NP , wk+1:k+NP
x
)
(22)
Step 2-3 目標軌道 sk+j を用いた尤度計算を行い, 次
式より重みを更新する．
(
)
(m)
(m)
(m)
w
˜k+j = w
¯k+j−1 hp sk+j |¯
xk+j
(24)
(m)
w
˜k+j
∑M (m)
˜l
l=1 w
尤度評価関数
尤度評価のために次の評価関数を与える．
Step 2-1 入力のサンプリング
(m)
目標軌道
簡単のため前進および上下方向の移動のみを考える．
[m]
[m]
[m]
[m]
[m]
[m]
つまり，y0 = vy0 = θx0 = θz0 = ωx0 = ωz0 = 0
と設定した.
次に，離散時間 k におけるクアッドコプタの計測位
置 y k , [yxk 0 yzk ]T から目標位置 r , [rx 0 rz ]T =
[20 0 5]T まで徐々に近づくような参照軌道 s(x) を次の
ように構築した．
)
(
x−yxk
+ yzk
s(x) = (rz − yzk ) 1 − e− Ts
(27)
である.
¯ k+j = u
¯ k+j−1 + v, v ∼ N (0, Σu )
u
パラメータ
z
s(z) (=s(x))
d [m]
k+1
Particle cloud
Measurement
x[m]
k+2
(yxk yzk)
x[m]
k+1
(26)
x
図 3 参照軌道，目標軌道，粒子雲の関係．
RSJ2014AC3M1-03
おける入力 uk+1 が次の入力として採用される．すな
わち，次の処理を実行する．
{ N
}
P
∑
(m)
(m) ¯ k+Np uk+1 = argmin λ
dk+i + (1 − λ) r − x
¯ (m)
u
i=0
k+1
(29)
4.2
結果
クアッドコプタの位置の時間推移と粒子の軌道の一
部を図 4 に示す. この図で示される結果は，外乱や計
測ノイズの影響を受けるクアッドコプタの制御におい
て提案した PF-MPC の手法が有効であることを示して
いる．さらに，様々のパラメータを変化させたシミュ
レーションより入力系列の時間推移が安定であること
を確かめた．
5.
(a) State values (k=10)
まとめ
本論文では環境外乱や計測ノイズによって影響を受
けるクアッドコプタの制御システムにパーティクル予
測制御を用いる手法を提案した．提案手法は逆モデル
を要求しないので，クアッドコプタの非線形系モデル
を予測制御に用いることができる．さらに，パーティ
クルフィルタモデル予測制御が有効に作動したことを
シミュレーションにより示した．
今後の課題は，シミュレーション条件における y 軸方
向への拡張，および回転角度の制御などが挙げられる．
(b) Input (k=10)
参考文献
[1] V.Kumar, N.Michael: Opportunities and challenges with autonomous micro aerial vehicles,
The International Journal of Robotics Research,
Vol. 31, No. 11, pp. 1279–1291, 2012.
[2] B.Sumantri, N.Uchiyama, S.Sano, Y.Kawabata:
Robust tracking control of a quad-Rotor helicopter utilizing sliding mode control with a nonlinear sliding surface, Journal of System Design
and Dynamics, Vol.7, No.2, pp.226–241, 2013.
[3] D.Mellinger, N.Michael, V.Kumar: Trajectory
generation and control for precise aggressive maneuvers with quadrotors, The International Journal of Robotics Research, Vol.31, No.5, pp.664–
674, 2012.
[4] P.Corke: Robotics,Vision and Control Fundamental Algorithms in MATLAB, Springer,
pp.78–86, 2011.
[5] J.P.de Villiers, S.J.Godsill, S.S.Singh: Particle
predictive control, Journal of Statistical Planning
and Inference, Vol.141, No.5, pp.1753–1763, 2011.
[6] D.Stahl, J.Hauth: PF-MPC particle filter-model
predictive control, Systems & Control Letters,
Vol.60, No.8, pp.632–643, 2011.
[7] J.S.Liu, R. Chen: Sequential Monte Carlo methods for dynamic systems, Journal of the American
Statistical Association, Vol.93, No.443, pp.1032–
1044, 1998.
第32回日本ロボット学会学術講演会（2014年9月4日～6日）
(a) State values (k=100)
(b) Input (k=100)
(c) State values (k=200)
(d) Input (k=200)
図 4 クアッドコプタの位置の時間推移と粒子の軌道．
© Copyright 2024 ExpyDoc
