ドローンの制御

2 次元上を動くドローンをある位置に移動させたい。ドローンの制御を、機体系（推力による速度の制御）とモーター-プロペラ系（推力の制御）に分けて考える。

機体系

二つのプロペラの推力でドローンを動かす。状態の観測のために加速度センサ、角速度センサを取り付ける。位置の測定のために、カメラでドローンを撮る。

記号の定義
定数
	機体重量
	機体の重心まわりの慣性モーメント
	プロペラの軸間長さ
変数
	重心位置
	傾き
	プロペラの推力
	加速度センサの値
	角速度センサの値
	カメラから推定した位置

運動方程式

モーターの反トルク、空気抵抗は無視している。

Mdot.double(x) = -(f_1+f_2)sintheta \

Mdot.double(y) =  (f_1+f_2)costheta - Mg \

Idot.double(theta) = (L)/(2)(f_1-f_2)

観測方程式

加速度センサの値はセンサに固定された座標系における値であることに注意して、観測方程式を求める。

x_c = x \

y_c = y \

a_x =  dot.double(x)costheta + (dot.double(y)-g)sintheta \

a_y = -dot.double(x)sintheta + (dot.double(y)-g)costheta \

omega = dot(theta)

加速度センサはセンサにかかる加速度を計測するが、機体の加速度に比べて重力加速度が十分大きいので、もっぱら鉛直方向を指し示すセンサとして働く。

状態方程式

平衡点（ホバリング状態） bold(x)_0=bold(0),bold(u)_0=lr(((Mg)/(2),(Mg)/(2))) まわりで一次近似して線形な状態方程式を求めると、

制御方式・状態の計測

Matlab を用いて可制御・可観測を確認した。

SS = ss(A,B,C,D)
rank(ctrb(SS)) == length(A)
rank(obsv(SS)) == length(A)

制御方式は最適制御とし、フィードバックゲインを以下の Matlab コードで求めた。

Q = diag([80,10,80,10,10,30]);
R = diag([50,50]);
[Kreg,~,Ef] = lqr(A,B,Q,R);

双対システムのフィードバックゲインを求め、オブザーバゲインとした。

S = diag([64,100,63,100,100,100]);
T = diag([10,10,10,10,10]);
[Lregt,~,Eo] = lqr(A',C',S,T);
Lreg = Lregt';

ブロック線図

シュミレーション

パラメタを M=0.2 upright(kg), L=0.4 upright(m), I=0.008upright(kgm^2) とする。ホバリング推力は u_0=(Mg)/(2)=0.98upright(N) である。

初期位置をとしたときのシュミレーション結果を示す。

また、初期位置をとしたときのシュミレーション結果は

考察

最適制御の評価関数を決めるのに苦労した。各重みの変化に対して根軌跡をプロットすれば直観的に決められると考えられる。
x 方向に大きく移動すると大きく傾いている。線形近似した運動方程式では、x 方向の加速度を増すために傾きを大きくするしかない。さらに、元の非線形な運動方程式では、傾きによる x 方向の加速度の増加が、線形の場合よりも抑えられるため、現実のドローンはシュミレーションよりも大きな傾きとなることが考えられる。傾きが大きくなると、今回の制御の線形近似から外れ、さらに傾きが増大する危険性がある。x 方向に大きく動かさないようにする必要がある。

モーター-プロペラ系

モーター

記号
定数
	トルク定数
	逆起電力定数
	巻線抵抗
	インダクタンス
変数
	逆起電力
	入力電圧
	電流
	角速度

キルヒホッフの法則より

E-e=RI+Ldot(I)

逆起電力は

e=K_Eomega

トルクは

T=K_TI

プロペラ

記号
定数
	空気密度
	直径
	慣性モーメント
	羽根角
変数
	対気速度
	回転数
	角速度
	進行率
	プロペラ推力係数
	プロペラ反トルク係数
	入力トルク
	推力
	反トルク

回転の運動方程式は

I_Rdot(omega)=T-Q

推力と反トルクはの関数

  F = C_Frho D^4 n^2 \

  Q = C_Qrho D^5 n^2

は一般に羽根角と進行率 J=(V)/(nD) の非線形な関数であり、風洞実験から計測する。今回は対気速度がプロペラの回転数に対して十分小さい領域で動作するので、は定数とする。

定数を

  K_F := (C_Frho D^4)/(4pi^2) \

  K_Q := (C_Qrho D^5)/(4pi^2)

とすると、

  F = K_Fomega^2\

  Q = K_Qomega^2

参考：航空工学講座６　プロペラ（日本航空技術協会）

ブロック線図

プロペラとモーターを結合した系のブロック線図は以下のようになる。

制御

センサとして回転軸にエンコーダを取り付け回転数を計測する。推力指令に対して追従する制御を行う。ホバリング推力前後で特性が良くなるようにゲインを調整する。