三次元計測によるクアッドコプタの遠隔姿勢制御

RSJ2014AC3M2-04
三次元計測によるクアッドコプタの遠隔姿勢制御
○津上聖也 (九州工業大学) 島田健人 (九州工業大学) 西田健 (九州工業大学)
1.
はじめに
近年，機体上に対角に配置された四つの回転翼
の回転数を制御することで飛行する無人航空機
（UAV:Unmanned Aerial Vehicle）に関する研究が注
目を集めており [1]，これらは総じてクアッドコプタ
と呼ばれる．クアッドコプタには，ロータの回転によ
る反作用を打ち消すために，右回りと左回りのロータ
が交互に配置され，四つのロータの回転速度の制御に
よって上昇，下降，前進，後進および旋回を行うこと
ができる．一般に，ヘリコプタよりも各種制御が容易
であるという特長を有しており，無人航空運送や点検
業務への導入が検討されている [2]．一方，クアッドコ
プタは機体の構造上，風の影響を受けやすく，風が強
い状況では飛行が困難である．したがって，気候変動
などが発生する屋外環境でクアッドコプタを利用する
ためには，以下の性能を維持することが要求される．
1. 風に対して機体の姿勢を維持する．
2. 風に対して流されること無く，目標の航空軌道を
飛行する．
3. 搬送物がある場合には，クアッドコプタのシステ
ムパラメータが変動するが，その場合であっても
上述の性能を維持する．
本研究は，クアッドコプタの三次元位置と機体の姿勢
をリアルタイムで計測し制御するための遠隔自動操縦
システムを開発することで，上述の制御性能の実現を
目的とする．
2.
クアッドコプタの遠隔自動制御システム
2.1 遠隔自動制御システムの概要
遠隔自動制御システムの概要を図 1 に示す．地上の
基地局からクアッドコプタの位置を三次元計測装置で
計測し，計算機で速度指令を算出する．また，基地局
からの速度指令とクアッドコプタ内部のセンサの計測
値は無線 LAN で通信される．
3.
クアッドコプタの運動モデル
の鉛直下向きに作用する．また，クアッドコプタの上
昇力として四つのロータの揚力
fi = bωi2 (i = 1, 2, 3, 4)
が Σq の上向きに作用する．ここで ωi はロータの回転
速度，b > 0 は揚力定数である．次に，ニュートンの第
二法則の並進運動方程式より，
   
0
0
   
(2)
maq = Roq  0  −  0 
F
F =
4
∑
mg
fi
第32回日本ロボット学会学術講演会（2014年9月4日～6日）
(3)
i=1
が与えられる．aq = [axq ayq azq ]T はクアットコプタ
の各軸方向の加速度，F は各揚力の和，g は鉛直下向き
の重力加速度，m はクアットコプタの質量を表す．こ
こで，回転行列 Roq は
Roq = R(θzq )R(θyq )R(θxq )
(4)
であり，R(θzq ) は Zq 軸まわりの回転，R(θyq ) は Yq
軸まわりの回転，R(θzq ) は Xq 軸まわりの回転を表す．
さらに，向かい合うロータの揚力の違いから発生する
Xq 軸まわりのトルクは
d
τxq = √ {(f3 + f4 ) − (f1 + f2 )}
2
(5)
となる．ここで，d は質量中点からロータの距離を表
す．この式に式 (1) を代入すると，
bd
τxr = √ {(ω32 + ω42 ) − (ω12 + ω22 )}
2
クアッドコプタの姿勢は，三次元ユークリッド空間
内の剛体運動群（すなわち回転と並進運動）で表され
る．まず，慣性座標系を Σ，クアットコプタの座標系
を Σq と表し，それらの関係を図 2 に示す．重力は Σ
図 1 遠隔自動制御システムの概要
(1)
図 2 慣性座標系とクアッドコプタ座標系
(6)
RSJ2014AC3M2-04
となる．また，同様に Yr 軸まわりのトルクは
bd
τyr = √ {(ω22 + ω32 ) − (ω12 + ω42 )}
2
(7)
となる．機体のロータの回転方向と逆に機体のプロペ
ラシャフトを回転させるトルクは
qi = kωi2
(8)
で表される．k は b と同じ要因に依存する定数である．
また，Zr 軸方向の全反発トルクは式 (8) を用いると，
τzr = q1 − q2 + q3 − q4
= k(ω12 + ω32 − ω22 − ω42 )
(9)
図 3 三次元計測装置とクアッドコプタの関係
と表せる．すなわち，τzr は四つすべてのロータの回転
速度を適切に調整することで生み出される．また，機
体の回転角速度はオイラーの運動方程式から得られる．
Iαr = −ω r × Iω r + τ r
(10)
ここで，I ∈ R3×3 は慣性行列，αr = [αxr αyr αzr ]T
は角加速度，ω r = [ωxr ωyr ωzr ]T は回転速度，τ r =
[τxr τyr τzr ]T は式 (6)，(7)，(9) から得られる機体に
発生するトルクである．ここで，慣性行列は


Ixx Ixy Ixz


I = Iyx Iyy Iyz 
Izx
Izy
Izz
となる．クアッドコプタの動作を表す式は，式 (2) と
式 (10) の並進運動方程式と回転運動方程式より

 
 
2
b
b
b
b
ω
ω12
1
[ ] 
  2
 2
bd
bd
bd
bd
√  ω  4
− √
ω 
− √2 √2
F
2
2   2 = A  2
=




ω 2 
bd
bd
bd
bd
2
√
√
τr
− √2  ω3 
− √2
 3
2
2
2
ω4
ω42
k
−k
k
−k
(11)
となる．行列 A は b, k, d > 0 でフルランクより，左か
ら逆行列をかけると
 
ω12
[ ]
 2
ω2 
F
−1
 =A
(12)
ω 2 
τ
r
 3
ω42
となる．ロータの回転速度は機体の合成揚力 F と機体
にかかる各軸まわりのトルク τ r を与えることで求める
ことができる．
4.
三次元座標計測装置
三次元計測装置として Kinect を用いた．これは RGB
カメラ，奥行き測定用赤外線カメラ，赤外線照射発光部
の三つの光学系センサ，左右に首を振る電動チルト機構
が搭載されている．三次元計測装置の中心を原点とする
座標系を慣性座標系 Σ と一致させて定義する．Σ におけ
るクアッドコプタの重心の目標位置を r , [rx ry rz ]T
と表す．また，離散時刻 k のクアッドコプタの重心位置
第32回日本ロボット学会学術講演会（2014年9月4日～6日）
を pk , [pxk pyk pzk ]T ，その計測を mk , [mxk myk mzk ]T
とする．さらに，目標と計測との誤差を，
ek , r − mk
(13)
ek , [exk eyk ezk ]T と表す．クアッドコプタへの入力は，
速度指令 uk , [uxk urk urk ]T である．
5.
ホバリングのための遠隔制御系の構成
5.1 クアッドコプタのホバリング
ホバリングは，すべてのロータが同じ回転速度を保
ち，かつ，四つのロータの回転速度の和と機体にかか
る重力が釣り合うよう制御することで実現する．ホバ
リング時のロータの回転速度は以下のようになる．
√
mg
ω0 =
(14)
4b
5.2 制御系の構成
クアッドコプタの姿勢を安定させると同時に目標軌
道に追従させるための制御系として，内部モデルに依存
しない離散型 PID 制御系を構成した．ここで，クアッ
ドコプタへの入力は Σ の各軸方向の速度であり，Σ に
おけるクアッドコプタの位置を出力とする．構成した
離散型 PID 制御系のブロック線図を図 4 に示す．
5.3 離散 PID 制御の設計
対象の離散状態方程式は以下の式で表される．
uk = uk−1 + αek + βek−1 + γek−2
(15)
ここで，
∆k
TD
+
)
TI
∆k
TD
TD
β = −KP (1 +
+
)
∆k
∆k−1
KP TD
γ=
∆k−1
α = KP (1 +
(16)
(17)
(18)
であり，KP は比例ゲイン，TI は積分時間，TD は微
分時間を表す．さらに，∆k は離散時刻 k におけるサン
プリング周期を表し，これは時変の変数とした．これ
RSJ2014AC3M2-04
図 4 離散型 PID 制御系のブロック線図．複数のデバイスと複数の通信経路が存在し実行周期が揺らぐため，∆k は一定
ではない．したがってこの離散系では，各時刻において ∆k を実測し利用する構成となっている．
図 5 クアッドコプタの位置の時間推移
図 6 クアッドコプタへの入力の時間推移
は，本研究で構築した遠隔操作システムには，大きく
分けて以下のような 5 種類の実行周期が混在するため
である．すなわち，計算機に実装した制御プログラム
の実行周期，Kinect による計測周期，クアッドコプタ
内部の制御周期，および，それぞれの通信経路の通信
周期であり，これらの実行周期は必ずしも一定ではな
い．したがって制御系に混在するこれらの実行周期の
揺らぎに対処するため，計算機の制御プログラム中で
各時刻の ∆k を実測して利用することとした．
与えたため，x 軸方向の動きは他の軸方向の動きに比
べて大きい．また，x 軸方向の速度指令入力も，他の
軸方向の速度指令よりも大きくなっている．これらの
結果より，本研究で構成した遠隔姿勢制御系が有効に
実行されたことが分かる．
6.
実験
6.1 実験条件と構成機器
本実験ではクアッドコプタとして ARDrone2.0 を使
用した．また，目標軌道を r = [1000 320 240]T とし，
比例ゲインを KP = 0.30，積分時間を TI = 0.75，微
分時間を TD = 0.1875 と設定した．外乱の発生源とし
て室内で機体の前方に扇風機を設置し，クアッドコプ
タが一定の高さでホバリング中に風を当て，構成した
制御系のロバスト性を検証する実験を行った．
6.2 実験結果
図 5 に各軸方向のクアッドコプタの位置の時間推移
を示し，図 6 に各軸方向のクアッドコプタへの入力の
時間推移を示す．クアッドコプタの前方から風外乱を
第32回日本ロボット学会学術講演会（2014年9月4日～6日）
7.
まとめ
三次元計測装置を用いるクアッドコプタの遠隔姿勢
制御系を構築した．また，複数のデバイスから構成さ
れ複数の信号通信経路を持つ本システムに対して離散
型 PID 制御法を実装するために，サンプリング周期を
時変とする工夫を施した．構成した遠隔姿勢制御系に
対して風外乱を与える実験を行った結果，有効に動作
することが確認された．
今後は，屋外などの実環境における本制御系の性能
検証を行うことが必要であると考えられる．
参考文献
[1] 三輪昌史: ”UAV を応用した物資運搬用空中台
車 ”，SI2013，2D3-5，pp.1261-1263，2013.
[2] 三輪昌史: ”隔絶地域・不整地での運搬用空中台
車 ”，SI2013，1C1-3，pp.0161-0163，2013.

Download Report