
上図のようなドローンについて、次の文献をフォローしていきます。
Randal Beard: Quadrotor Dynamics and Control, Rev0.1, Brigham Young University, 2008
以下では、、
をそれぞれ
、
と略記します。
準備

●上図のように、ベクトルを、ベクトル
のまわりに
だけ回転して得られるベクトル
の表現式を求めます。
ただし
から、次式を得ます。
ここで
とおくと
●の場合(left-handed rotation about z-axis)

●の場合(left-handed rotation about y-axis)

●の場合(left-handed rotation about x-axis)

以上のようなベクトルの回転は、相対的に座標軸を回転させても、把握できます。
●right-handed rotation about z-axis

●right-handed rotation about y-axis

●right-handed rotation about x-axis

座標系
●ドローンの運動の記述にあたっては、次のような地上座標系と機体固定座標系
を用います。

における機体の位置は次の変数で表します。
: the inertial (north) position of the aircraft along
in
: the inertial (east) position of the aircraft along
in
: the altitude of the aircraft measured along
in
また、は、
から、次のオイラー角
: the roll angle defined with respect to
: the pitch angle defined with respect to
: the yaw angle defined with respect to
を用いて、次のように逐次回転して得られます。
ここで、は原点を機体重心にもつ回転前の座標系(
に平行)を表しています。
●における任意ベクトル
(後出の位置ベクトル、速度ベクトルなど)は、
から、次のような変換を行って得られます。
●

●

●

●における任意ベクトル
(後出の位置ベクトル、速度ベクトルなど)の時間変化がどのように表されるかを調べます。

これは、次式で表されます。
実際
運動方程式

における運動は次の変数を用いて表されます。
: the body frame velocity measured along
in
: the body frame velocity measured along
in
: the body frame velocity measured along
in
: the roll rate measured along
in
: the pitch rate measured along
in
: the yaw rate measured along
in
これらがからどう表されるか、すなわち次の変数の微分とどう関係するか(キネマティックス)を調べます。
: the inertial (north) position of the quadrotor along
in
: the inertial (east) position of the quadrotor along
in
: the altitude of the aircraft measured along
in
: the roll angle defined with respect to
: the pitch angle defined with respect to
: the yaw angle defined with respect to
●と
における速度ベクトルについては
の関係が成り立ちます。したがって、
●と
における角速度ベクトルについては
の関係が成り立ちます。したがって、
すなわち、次の関係式を得ます。
●次に並進運動(ダイナミックス)は次式で表されます。
●また回転運動(ダイナミックス)は次式で表されます。
外力


4つのモータへの回転指令をとします。このとき、各モータによる推力とトルクは次式で表されます。
また、4つのモータの合推力と各軸回りのトルクは次式で表されます。
すなわち
また、並進運動方程式の外力項は次式となります。
このときドローンの運動方程式に基づく状態方程式は次式となります。
これを見ると、行列に状態変数が絡んでおり、複雑なダイナミクスをもっていることが分かります。
運動方程式の簡単化
次の仮定のもとで、運動方程式の簡単化を行ないます。
仮定1: 、
、
は小さく無視できる
仮定2: 、
は小さく無視できる
まず、仮定1のもとで、次の近似が可能です。
このとき、仮定2をおけば次の近似が可能です。
これは、機体の傾きを独立して制御できることを示しています。
さらに次のような近似が可能です。
結局、ドローンの運動方程式は、次式のように簡単化されます。
いま,重力補償のためにから決まる
を各操作入力に前もって加えておくことにすると、次式が成り立ちます。
このときドローンの線形状態方程式は次式となります。