ドローン | 制御系CAD

上図のようなドローンについて、次の文献をフォローしていきます。

Randal Beard: Quadrotor Dynamics and Control, Rev0.1, Brigham Young University, 2008

以下では、 $\sin(\cdot)$ 、 $\cos(\cdot)$ をそれぞれ $S_{(\cdot)}$ 、 $C_{(\cdot)}$ と略記します。

準備

●上図のように、ベクトル ${\bf p}$ を、ベクトル $\hat{n}$ のまわりに $\mu$ だけ回転して得られるベクトル ${\bf q}$ の表現式を求めます。

$\displaystyle{{\bf q}=\vec{ON}+\vec{NW}+\vec{WQ}}$
ただし
$\displaystyle{\vec{ON}=({\bf p}\cdot \hat{n}) \hat{n} }$
$\displaystyle{\vec{NW}=\frac{ {\bf p}-({\bf p}\cdot \hat{n}) \hat{n} }{ || {\bf p}-({\bf p}\cdot \hat{n}) \hat{n} ||}\overline{NQ}\,C_\mu =({\bf p}-({\bf p}\cdot \hat{n}) \hat{n})\, C_\mu \quad(\overline{NQ}=|| {\bf p}-({\bf p}\cdot \hat{n}) \hat{n} ||)}$
$\displaystyle{\vec{WQ}=\frac{{\bf p}\times\hat{n}}{||{\bf p}||S_\phi}\overline{NQ}\,S_\mu=-(\hat{n}\times{\bf p})\,S_\mu \quad(\overline{NQ}=||{\bf p}||S_\phi)}$

から、次式を得ます。

$\displaystyle{{\bf q}=(1-C_\mu)({\bf p}\cdot \hat{n}) \hat{n}+C_\mu{\bf p} -S_\mu(\hat{n}\times{\bf p})}$

ここで

$\displaystyle{{\bf p}=p_x^0\hat{i}^0+p_y^0\hat{j}^0+p_z^0\hat{k}^0}$
$\displaystyle{{\bf q}=q_x^0\hat{i}^0+q_y^0\hat{j}^0+q_z^0\hat{k}^0}$
$\displaystyle{\hat{n}=\hat{n}_x \hat{i}^0+\hat{n}_y \hat{j}^0+\hat{n}_z \hat{k}^0}$

とおくと

$\displaystyle{{\bf p}\cdot \hat{n}=p_x^0\hat{n}_x+p_y^0\hat{n}_y+p_z^0\hat{n}_z }$
$\displaystyle{\hat{n}\times{\bf p}= {\rm det} \left[\begin{array}{ccc} \hat{i}^0 & \hat{j}^0 & \hat{k}^0\\ \hat{n}_x & \hat{n}_y & \hat{n}_z\\ p_x^0 & p_y^0 & p_z^0 \end{array}\right]}$
$\displaystyle{=(\hat{n}_yp_z^0-\hat{n}_zp_y^0)\hat{i}^0+(\hat{n}_zp_x^0-\hat{n}_xp_z^0)\hat{j}^0+(\hat{n}_xp_y^0-\hat{n}_yp_x^0)\hat{k}^0 }$

● $\hat{n}=\hat{i}$ の場合（left-handed rotation about z-axis）

$\displaystyle{{\bf q}=(1-C_\psi)({\bf p}\cdot \hat{n}) \hat{n}+C_\psi{\bf p} -S_\psi(\hat{n}\times{\bf p}) }$
$\displaystyle{ \underbrace{ \left[\begin{array}{c} q_x^0 \\ q_y^0 \\ q_z^0 \end{array}\right] }_{{\bf q}} =(1-C_\psi) p_z^0 \underbrace{ \left[\begin{array}{c} 0 \\ 0 \\ 1 \end{array}\right] }_{\hat{i}} \hat{n}+C_\psi \left[\begin{array}{c} p_x^0 \\ p_y^0 \\ p_z^0 \end{array}\right] -S_\psi \left[\begin{array}{c} -p_y^0 \\ p_x^0 \\ 0 \end{array}\right] }$
$\displaystyle{ = \underbrace{ \left[\begin{array}{ccc} C_\psi & S_\psi & 0 \\ -S_\psi & C_\psi & 0 \\ 0 & 0 & 1 \end{array}\right] }_{R(\phi)} \underbrace{ \left[\begin{array}{c} p_x^0 \\ p_y^0 \\ p_z^0 \end{array}\right] }_{{\bf p}} }$

● $\hat{n}=\hat{j}$ の場合（left-handed rotation about y-axis)

$\displaystyle{{\bf q}=(1-C_\theta)({\bf p}\cdot \hat{n}) \hat{n}+C_\theta{\bf p} -S_\theta(\hat{n}\times{\bf p}) }$
$\displaystyle{ \underbrace{ \left[\begin{array}{c} q_x^0 \\ q_y^0 \\ q_z^0 \end{array}\right] }_{{\bf q}} =(1-C_\theta) p_y^0 \underbrace{ \left[\begin{array}{c} 0 \\ 1 \\ 0 \end{array}\right] }_{\hat{j}} \hat{n}+C_\theta \left[\begin{array}{c} p_x^0 \\ p_y^0 \\ p_z^0 \end{array}\right] -S_\theta \left[\begin{array}{c} p_z^0 \\ 0\\ -p_x^0 \\ \end{array}\right] }$
$\displaystyle{ =\underbrace{ \left[\begin{array}{ccc} C_\theta & 0 & -S_\theta \\ 0 & 1 & 0 \\ S_\theta & 0 & C_\theta \end{array}\right] }_{R(\theta)} \underbrace{ \left[\begin{array}{c} p_x^0 \\ p_y^0 \\ p_z^0 \end{array}\right] }_{{\bf p}} }$

● $\hat{n}=\hat{k}$ の場合（left-handed rotation about x-axis）

$\displaystyle{{\bf q}=(1-C_\phi)({\bf p}\cdot \hat{n}) \hat{n}+C_\phi{\bf p} -S_\phi(\hat{n}\times{\bf p}) }$
$\displaystyle{ \underbrace{ \left[\begin{array}{c} q_x^0 \\ q_y^0 \\ q_z^0 \end{array}\right] }_{{\bf q}} =(1-C_\phi) p_x^0 \underbrace{ \left[\begin{array}{c} 1 \\ 0 \\ 0 \end{array}\right] }_{\hat{k}} \hat{n}+C_\phi \left[\begin{array}{c} p_x^0 \\ p_y^0 \\ p_z^0 \end{array}\right] -S_\phi \left[\begin{array}{c} 0\\ -p_z^0 \\ p_y^0 \\ \end{array}\right] }$
$\displaystyle{ =\underbrace{ \left[\begin{array}{ccc} 1 & 0 & 0 \\ 0 & C_\phi & S_\phi \\ 0 & -S_\phi & C_\phi \end{array}\right] }_{R(\psi)} \underbrace{ \left[\begin{array}{c} p_x^0 \\ p_y^0 \\ p_z^0 \end{array}\right] }_{{\bf p}} }$

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

●right-handed rotation about z-axis

$\displaystyle{{\bf p}^0=p_x^0\hat{i}^0+p_y^0\hat{j}^0+p_z^0\hat{k}^0}$
$\displaystyle{{\bf p}^1=p_x^1\hat{i}^1+p_y^1\hat{j}^1+p_z^1\hat{k}^1}$
$\qquad\Downarrow$
$\displaystyle{p_x^1\hat{i}^1+p_y^1\hat{j}^1+p_z^1\hat{k}^1=p_x^0\hat{i}^0+p_y^0\hat{j}^0+p_z^0\hat{k}^0}$
$\qquad\Downarrow$
$\displaystyle{\hat{i}^1\cdot (p_x^1\hat{i}^1+p_y^1\hat{j}^1+p_z^1\hat{k}^1)=p_x^1=p_x^0\hat{i}^1\cdot\hat{i}^0+p_y^0\hat{i}^1\cdot\hat{j}^0+p_z^0\hat{i}^1\cdot\hat{k}^0}$
$\displaystyle{\hat{j}^1\cdot (p_x^1\hat{i}^1+p_y^1\hat{j}^1+p_z^1\hat{k}^1)=p_y^1=p_x^0\hat{j}^1\cdot\hat{i}^0+p_y^0\hat{j}^1\cdot\hat{j}^0+p_z^0\hat{j}^1\cdot\hat{k}^0}$
$\displaystyle{\hat{k}^1\cdot (p_x^1\hat{i}^1+p_y^1\hat{j}^1+p_z^1\hat{k}^1)=p_z^1=p_x^0\hat{k}^1\cdot\hat{i}^0+p_y^0\hat{k}^1\cdot\hat{j}^0+p_z^0\hat{k}^1\cdot\hat{k}^0}$
$\qquad\Downarrow$
$\displaystyle{ \underbrace{ \left[\begin{array}{c} p_x^1 \\ p_y^1 \\ p_z^1 \end{array}\right] }_{{\bf p}^1} = \underbrace{ \left[\begin{array}{ccc} \hat{i}^1\cdot\hat{i}^0&\hat{i}^1\cdot\hat{j}^0&\hat{i}^1\cdot\hat{k}^0\\ \hat{j}^1\cdot\hat{i}^0&\hat{j}^1\cdot\hat{j}^0&\hat{j}^1\cdot\hat{k}^0\\ \hat{k}^1\cdot\hat{i}^0&\hat{k}^1\cdot\hat{j}^0&\hat{k}^1\cdot\hat{k}^0 \end{array}\right] }_{R_0^1(\psi)} \underbrace{ \left[\begin{array}{c} p_x^0 \\ p_y^0 \\ p_z^0 \end{array}\right] }_{{\bf p}^0} }$
$\qquad\Downarrow$
$\displaystyle{ R_0^1(\psi)= \left[\begin{array}{ccc} C_\psi & S_\psi & 0 \\ -S_\psi & C_\psi & 0 \\ 0 & 0 & 1 \end{array}\right] }$

●right-handed rotation about y-axis

$\displaystyle{{\bf p}^0=p_x^0\hat{i}^0+p_y^0\hat{j}^0+p_z^0\hat{k}^0}$
$\displaystyle{{\bf p}^1=p_x^1\hat{i}^1+p_y^1\hat{j}^1+p_z^1\hat{k}^1}$
$\qquad\Downarrow$
$\displaystyle{p_x^1\hat{i}^1+p_y^1\hat{j}^1+p_z^1\hat{k}^1=p_x^0\hat{i}^0+p_y^0\hat{j}^0+p_z^0\hat{k}^0}$
$\qquad\Downarrow$
$\displaystyle{\hat{i}^1\cdot (p_x^1\hat{i}^1+p_y^1\hat{j}^1+p_z^1\hat{k}^1)=p_x^1=p_x^0\hat{i}^1\cdot\hat{i}^0+p_y^0\hat{i}^1\cdot\hat{j}^0+p_z^0\hat{i}^1\cdot\hat{k}^0}$
$\displaystyle{\hat{j}^1\cdot (p_x^1\hat{i}^1+p_y^1\hat{j}^1+p_z^1\hat{k}^1)=p_y^1=p_x^0\hat{j}^1\cdot\hat{i}^0+p_y^0\hat{j}^1\cdot\hat{j}^0+p_z^0\hat{j}^1\cdot\hat{k}^0}$
$\displaystyle{\hat{k}^1\cdot (p_x^1\hat{i}^1+p_y^1\hat{j}^1+p_z^1\hat{k}^1)=p_z^1=p_x^0\hat{k}^1\cdot\hat{i}^0+p_y^0\hat{k}^1\cdot\hat{j}^0+p_z^0\hat{k}^1\cdot\hat{k}^0}$
$\qquad\Downarrow$
$\displaystyle{ \underbrace{ \left[\begin{array}{c} p_x^1 \\ p_y^1 \\ p_z^1 \end{array}\right] }_{{\bf p}^1} = \underbrace{ \left[\begin{array}{ccc} \hat{i}^1\cdot\hat{i}^0&\hat{i}^1\cdot\hat{j}^0&\hat{i}^1\cdot\hat{k}^0\\ \hat{j}^1\cdot\hat{i}^0&\hat{j}^1\cdot\hat{j}^0&\hat{j}^1\cdot\hat{k}^0\\ \hat{k}^1\cdot\hat{i}^0&\hat{k}^1\cdot\hat{j}^0&\hat{k}^1\cdot\hat{k}^0 \end{array}\right] }_{R_0^1(\theta)} \underbrace{ \left[\begin{array}{c} p_x^0 \\ p_y^0 \\ p_z^0 \end{array}\right] }_{{\bf p}^0} }$
$\qquad\Downarrow$
$\displaystyle{ R_0^1(\theta)= \left[\begin{array}{ccc} C_\theta & 0 & -S_\theta \\ 0 & 1 & 0 \\ S_\theta & 0 & C_\theta \end{array}\right] }$

●right-handed rotation about x-axis

$\displaystyle{{\bf p}^0=p_x^0\hat{i}^0+p_y^0\hat{j}^0+p_z^0\hat{k}^0}$
$\displaystyle{{\bf p}^1=p_x^1\hat{i}^1+p_y^1\hat{j}^1+p_z^1\hat{k}^1}$
$\qquad\Downarrow$
$\displaystyle{p_x^1\hat{i}^1+p_y^1\hat{j}^1+p_z^1\hat{k}^1=p_x^0\hat{i}^0+p_y^0\hat{j}^0+p_z^0\hat{k}^0}$
$\qquad\Downarrow$
$\displaystyle{\hat{i}^1\cdot (p_x^1\hat{i}^1+p_y^1\hat{j}^1+p_z^1\hat{k}^1)=p_x^1=p_x^0\hat{i}^1\cdot\hat{i}^0+p_y^0\hat{i}^1\cdot\hat{j}^0+p_z^0\hat{i}^1\cdot\hat{k}^0}$
$\displaystyle{\hat{j}^1\cdot (p_x^1\hat{i}^1+p_y^1\hat{j}^1+p_z^1\hat{k}^1)=p_y^1=p_x^0\hat{j}^1\cdot\hat{i}^0+p_y^0\hat{j}^1\cdot\hat{j}^0+p_z^0\hat{j}^1\cdot\hat{k}^0}$
$\displaystyle{\hat{k}^1\cdot (p_x^1\hat{i}^1+p_y^1\hat{j}^1+p_z^1\hat{k}^1)=p_z^1=p_x^0\hat{k}^1\cdot\hat{i}^0+p_y^0\hat{k}^1\cdot\hat{j}^0+p_z^0\hat{k}^1\cdot\hat{k}^0}$
$\qquad\Downarrow$
$\displaystyle{ \underbrace{ \left[\begin{array}{c} p_x^1 \\ p_y^1 \\ p_z^1 \end{array}\right] }_{{\bf p}^1} = \underbrace{ \left[\begin{array}{ccc} \hat{i}^1\cdot\hat{i}^0&\hat{i}^1\cdot\hat{j}^0&\hat{i}^1\cdot\hat{k}^0\\ \hat{j}^1\cdot\hat{i}^0&\hat{j}^1\cdot\hat{j}^0&\hat{j}^1\cdot\hat{k}^0\\ \hat{k}^1\cdot\hat{i}^0&\hat{k}^1\cdot\hat{j}^0&\hat{k}^1\cdot\hat{k}^0 \end{array}\right] }_{R_0^1(\phi)} \underbrace{ \left[\begin{array}{c} p_x^0 \\ p_y^0 \\ p_z^0 \end{array}\right] }_{{\bf p}^0} }$
$\qquad\Downarrow$
$\displaystyle{ R_0^1(\phi)= \left[\begin{array}{ccc} 1 & 0 & 0 \\ 0 & C_\phi & S_\phi \\ 0 & -S_\phi & C_\phi \end{array}\right] }$

座標系

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

${\cal F}^i$ における機体の位置は次の変数で表します。

$p_n$ : the inertial (north) position of the aircraft along $\hat{i}^i$ in ${\cal F}^i$
$p_e$ : the inertial (east) position of the aircraft along $\hat{j}^i$ in ${\cal F}^i$
$h$ : the altitude of the aircraft measured along $-\hat{k}^i$ in ${\cal F}^i$

また、 ${\cal F}^b$ は、 ${\cal F}^i$ から、次のオイラー角

$\phi$ : the roll angle defined with respect to ${\cal F}^{v2}$
$\theta$ : the pitch angle defined with respect to ${\cal F}^{v1}$
$\psi$ : the yaw angle defined with respect to ${\cal F}^{v}$

を用いて、次のように逐次回転して得られます。

$\displaystyle{{\cal F}^i\rightarrow{\cal F}^v\rightarrow{\cal F}^{v1}\rightarrow{\cal F}^{v2}\rightarrow{\cal F}^b$

ここで、 ${\cal F}^{v}$ は原点を機体重心にもつ回転前の座標系（ ${\cal F}^{i}$ に平行）を表しています。

● ${\cal F}^b$ における任意ベクトル ${\bf p}$ （後出の位置ベクトル、速度ベクトルなど）は、 ${\cal F}^{i}$ から、次のような変換を行って得られます。

$\displaystyle{ {\bf p}^b=R_v^b(\phi,\theta,\psi){\bf p}^v }$
$\displaystyle{ R_v^b(\phi,\theta,\psi)=R_{v2}^b(\phi) R_{v1}^{v2}(\theta) R_v^{v1}(\psi) }$
$\displaystyle{= \left[\begin{array}{ccc} 1 & 0 & 0 \\ 0 & C_\phi & S_\phi \\ 0 & -S_\phi & C_\phi \end{array}\right] \left[\begin{array}{ccc} C_\theta & 0 & -S_\theta \\ 0 & 1 & 0 \\ S_\theta & 0 & C_\theta \end{array}\right] \left[\begin{array}{ccc} C_\psi & S_\psi & 0 \\ -S_\psi & C_\psi & 0 \\ 0 & 0 & 1 \end{array}\right] }$
$\displaystyle{ = \left[\begin{array}{ccc} C_\theta C_\psi & C_\theta S_\psi & -S_\theta \\ S_\phi S_\theta C_\psi - C_\phi S_\psi & S_\phi S_\theta S_\psi + C_\phi C_\psi & S_\phi C_\theta \\ C_\phi S_\theta C_\psi + C_\phi S_\psi & C_\phi S_\theta S_\psi - S_\phi C_\psi & C_\phi C_\theta \end{array}\right] }$

● $\displaystyle{{\cal F}^v\rightarrow{\cal F}^{v1}$
$\displaystyle{ {\bf p}^{v1}=R_v^{v1}(\psi){\bf p}^v }$
$\displaystyle{ R_v^{v1}(\psi)= \left[\begin{array}{ccc} C_\psi & S_\psi & 0 \\ -S_\psi & C_\psi & 0 \\ 0 & 0 & 1 \end{array}\right] }$

● $\displaystyle{{\cal F}^{v1}\rightarrow{\cal F}^{v2}$
$\displaystyle{ {\bf p}^{v2}=R_{v1}^{v2}(\theta){\bf p}^{v1} }$
$\displaystyle{ R_{v1}^{v2}(\theta)= \left[\begin{array}{ccc} C_\theta & 0 & -S_\theta \\ 0 & 1 & 0 \\ S_\theta & 0 & C_\theta \end{array}\right] }$

● $\displaystyle{{\cal F}^{v2}\rightarrow{\cal F}^b$
$\displaystyle{ {\bf p}^{b}=R_{v2}^b(\phi){\bf p}^{v2} }$
$\displaystyle{ R_{v2}^b(\phi)= \left[\begin{array}{ccc} 1 & 0 & 0 \\ 0 & C_\phi & S_\phi \\ 0 & -S_\phi & C_\phi \end{array}\right] }$

● ${\cal F}^b$ における任意ベクトル ${\bf p}$ （後出の位置ベクトル、速度ベクトルなど）の時間変化がどのように表されるかを調べます。

これは、次式で表されます。

$\displaystyle{\frac{d}{dt}_i{\bf p}=\frac{d}{dt}_b{\bf p}+\omega_{b/i}\times{\bf p} }$

実際

$\displaystyle{{\bf p}+\delta{\bf p}=(1-C_{(-\delta \phi)})({\bf p}\cdot \hat{s}) \hat{s}+C_{(-\delta \phi)}{\bf p} -S_{(-\delta \phi)}(\hat{s}\times{\bf p}) }$
$\displaystyle{\delta{\bf p}\simeq \delta \phi (\hat{s}\times{\bf p}) \Rightarrow \frac{\delta{\bf p}}{\delta t}\simeq\frac{\delta \phi}{\delta t} (\hat{s}\times{\bf p}) }$
$\qquad\Downarrow$
$\displaystyle{\frac{d}{dt}_i{\bf p}\simeq \underbrace{\dot{\phi} \hat{s}}_{\omega_{b/i}}\times{\bf p} }$

運動方程式

${\cal F}^b$ における運動は次の変数を用いて表されます。

$u$ : the body frame velocity measured along $\hat{i}^b$ in ${\cal F}^b$
$v$ : the body frame velocity measured along $\hat{j}^b$ in ${\cal F}^b$
$w$ : the body frame velocity measured along $\hat{k}^b$ in ${\cal F}^b$
$p$ : the roll rate measured along $\hat{i}^b$ in ${\cal F}^b$
$q$ : the pitch rate measured along $\hat{j}^b$ in ${\cal F}^b$
$r$ : the yaw rate measured along $\hat{k}^b$ in ${\cal F}^b$

これらが ${\cal F}^i$ からどう表されるか、すなわち次の変数の微分とどう関係するか（キネマティックス）を調べます。

$p_n$ : the inertial (north) position of the quadrotor along $\hat{i}^i$ in ${\cal F}^i$
$p_e$ : the inertial (east) position of the quadrotor along $\hat{j}^i$ in ${\cal F}^i$
$h$ : the altitude of the aircraft measured along $-\hat{k}^i$ in ${\cal F}^i$
$\phi$ : the roll angle defined with respect to ${\cal F}^{v2}$
$\theta$ : the pitch angle defined with respect to ${\cal F}^{v1}$
$\psi$ : the yaw angle defined with respect to ${\cal F}^{v}$

● ${\cal F}^i$ と ${\cal F}^b$ における速度ベクトルについては

$\displaystyle{ \left[\begin{array}{c} u \\ v \\ w \end{array}\right] = \underbrace{ \left[\begin{array}{ccc} C_\theta C_\psi & C_\theta S_\psi & -S_\theta \\ S_\phi S_\theta C_\psi - C_\phi S_\psi & S_\phi S_\theta S_\psi + C_\phi C_\psi & S_\phi C_\theta \\ C_\phi S_\theta C_\psi + C_\phi S_\psi & C_\phi S_\theta S_\psi - S_\phi C_\psi & C_\phi C_\theta \end{array}\right] }_{R_v^b} \left[\begin{array}{c} \dot{p}_n \\ \dot{p}_e \\ -\dot{h} \end{array}\right] }$

の関係が成り立ちます。したがって、

$\displaystyle{ \left[\begin{array}{c} \dot{p}_n \\ \dot{p}_e \\ -\dot{h} \end{array}\right] = \underbrace{ \left[\begin{array}{ccc} C_\theta C_\psi &S_\phi S_\theta C_\psi - C_\phi S_\psi &C_\phi S_\theta C_\psi + C_\phi S_\psi \\ C_\theta S_\psi &S_\phi S_\theta S_\psi + C_\phi C_\psi &C_\phi S_\theta S_\psi - S_\phi C_\psi \\ -S_\theta &S_\phi C_\theta &C_\phi C_\theta \end{array}\right] }_{R_b^v=(R_v^b)^T} \left[\begin{array}{c} u \\ v \\ w \end{array}\right] }$

● ${\cal F}^i$ と ${\cal F}^b$ における角速度ベクトルについては

$\displaystyle{ \left[\begin{array}{c} p \\ q \\ r \end{array}\right] = \underbrace{R_{v2}^b(\dot{\phi})}_{I} \left[\begin{array}{c} \dot{\phi} \\ 0 \\ 0 \end{array}\right] + R_{v2}^b(\phi)\underbrace{R_{v1}^{v2}(\dot{\theta})}_{I} \left[\begin{array}{c} 0 \\ \dot{\theta} \\ 0 \end{array}\right] + R_{v2}^b(\phi)R_{v1}^{v2}(\theta)\underbrace{R_{v}^{v1}(\dot{\psi})}_{I} \left[\begin{array}{c} 0 \\ 0 \\ \dot{\psi} \end{array}\right] }$
$\displaystyle{ = \left[\begin{array}{c} \dot{\phi} \\ 0 \\ 0 \end{array}\right] + \left[\begin{array}{ccc} 1 & 0 & 0 \\ 0 & C_\phi & S_\phi \\ 0 & -S_\phi & C_\phi \end{array}\right] \left[\begin{array}{c} 0 \\ \dot{\theta} \\ 0 \end{array}\right] + \left[\begin{array}{ccc} 1 & 0 & 0 \\ 0 & C_\phi & S_\phi \\ 0 & -S_\phi & C_\phi \end{array}\right] \left[\begin{array}{ccc} C_\theta & 0 & -S_\theta \\ 0 & 1 & 0 \\ S_\theta & 0 & C_\theta \end{array}\right] \left[\begin{array}{c} 0 \\ 0 \\ \dot{\psi} \end{array}\right] }$

の関係が成り立ちます。したがって、

$\displaystyle{ \left[\begin{array}{c} p \\ q \\ r \end{array}\right]= \left[\begin{array}{ccc} 1 & 0 & -S_\theta \\ 0 & C_\phi & S_\phi C_\theta \\ 0 & -S_\phi & C_\phi C_\theta \end{array}\right] \left[\begin{array}{c} \dot{\phi} \\ \dot{\theta} \\ \dot{\psi} \end{array}\right] }$

すなわち、次の関係式を得ます。

●次に並進運動（ダイナミックス）は次式で表されます。

$\displaystyle{m\frac{d}{dt}_i{\bf v}=\frac{d}{dt}_b{\bf v}+\omega_{b/i}\times{\bf v}=\frac{1}{m}{\bf f} }$
$\displaystyle{ \frac{d}{dt}_b \left[\begin{array}{c} u\\ v\\ w \end{array}\right] + \left[\begin{array}{c} p\\ q\\ r \end{array}\right] \times \left[\begin{array}{c} u\\ v\\ w \end{array}\right] =\frac{1}{m} \left[\begin{array}{c} f_x\\ f_y\\ f_z \end{array}\right] }$

$\displaystyle{ \left[\begin{array}{c} \dot{u}\\ \dot{v}\\ \dot{w} \end{array}\right] =- \underbrace{ \left[\begin{array}{ccc} 0 & -r & q\\ r & 0 &-p\\ -q & p & 0 \end{array}\right] \left[\begin{array}{c} u\\ v\\ w \end{array}\right] }_{\left[\begin{array}{c} qw-rv \\ ru-pw\\ pv-qu \end{array}\right]} +\frac{1}{m} \left[\begin{array}{c} f_x\\ f_y\\ f_z \end{array}\right] }$

●また回転運動（ダイナミックス）は次式で表されます。

$\displaystyle{\frac{d}{dt}_i{\bf h}=\frac{d}{dt}_b{\bf h}+\omega_{b/i}\times{\bf h}={\bf m} }$
$\displaystyle{{\bf h}=J\omega_{b/i} }$
$\displaystyle{J= \left[\begin{array}{ccc} \int(y^2+z^2)dm & -\int xydm & -\int xzdm \\ -\int yxdm & \int(x^2+z^2)dm & -\int yzdm \\ -\int zxdm & -\int zydm & \int(x^2+y^2)dm \end{array}\right] =\left[\begin{array}{ccc} J_x & 0 & 0 \\ 0 & J_y & 0 \\ 0 & 0 & J_z \end{array}\right] }$

$\displaystyle{ \frac{d}{dt}_bJ \left[\begin{array}{c} p\\ q\\ r \end{array}\right] + \left[\begin{array}{c} p\\ q\\ r \end{array}\right] \times J \left[\begin{array}{c} p\\ q\\ r \end{array}\right] = \left[\begin{array}{c} \tau_\phi\\ \tau_\theta\\ \tau_\psi \end{array}\right] }$
$\displaystyle{ \left[\begin{array}{c} J_x\dot{p}\\ J_y\dot{q}\\ J_z\dot{r} \end{array}\right] + \underbrace{ \left[\begin{array}{ccc} 0 & -r & q\\ r & 0 &-p\\ -q & p & 0 \end{array}\right] \left[\begin{array}{c} J_xp\\ J_yq\\ J_zr \end{array}\right] }_{\left[\begin{array}{c} (J_z-J_y)qr \\ (J_x-J_z)pr\\ (J_y-J_x)pq \end{array}\right]} = \left[\begin{array}{c} \tau_\phi\\ \tau_\theta\\ \tau_\psi \end{array}\right] }$

$\displaystyle{ \left[\begin{array}{c} \dot{p}\\ \dot{q}\\ \dot{r} \end{array}\right] =- \underbrace{ \left[\begin{array}{ccc} 0 & 0 & \frac{J_z-J_y}{J_x}q\\ \frac{J_x-J_z}{J_y} r & 0 & 0\\ 0 & \frac{J_y-J_x}{J_z}p & 0 \end{array}\right] \left[\begin{array}{c} p\\ q\\ r \end{array}\right] }_{ \left[\begin{array}{c} \frac{J_z-J_y}{J_x}qr \\ \frac{J_x-J_z}{J_y}pr \\ \frac{J_y-J_x}{J_z}pq \end{array}\right] } + \left[\begin{array}{c} \frac{1}{J_x}\tau_\phi\\ \frac{1}{J_y}\tau_\theta\\ \frac{1}{J_z}\tau_\psi \end{array}\right] }$

外力

４つのモータへの回転指令を $\delta_f,\delta_r,\delta_b,\delta_\ell$ とします。このとき、各モータによる推力とトルクは次式で表されます。

$\displaystyle{F_f=k_1\delta_f}$
$\displaystyle{F_r=k_1\delta_r}$
$\displaystyle{F_b=k_1\delta_b}$
$\displaystyle{F_\ell=k_1\delta_\ell}$

$\displaystyle{\tau_f=k_2\delta_f}$
$\displaystyle{\tau_r=k_2\delta_r}$
$\displaystyle{\tau_b=k_2\delta_b}$
$\displaystyle{\tau_\ell=k_2\delta_\ell}$

また、４つのモータの合推力と各軸回りのトルクは次式で表されます。

$\displaystyle{F=F_f+F_r+F_b+F_\ell}$
$\displaystyle{\tau_\phi=\ell(F_\ell-F_r)}$
$\displaystyle{\tau_\theta=\ell(F_f-F_b)}$
$\displaystyle{\tau_\psi=\tau_r+\tau_\ell-\tau_f-\tau_b}$

すなわち

$\displaystyle{ \left[\begin{array}{c} F\\ \tau_\phi\\ \tau_\theta\\ \tau_\psi \end{array}\right] = \underbrace{ \left[\begin{array}{cccc} k_1 & k_1 & k_1 & k_1 \\ 0 & -\ell k_1 & 0 & \ell k_1 \\ \ell k_1 & 0 & -\ell k_1 & 0 \\ -k_2 & k_2 & -k_2 & k_2 \end{array}\right] }_{{\cal M}} \left[\begin{array}{c} \delta_f\\ \delta_r\\ \delta_b\\ \delta_\ell \end{array}\right] }$

また、並進運動方程式の外力項は次式となります。

$\displaystyle{ \left[\begin{array}{c} f_x\\ f_y\\ f_z \end{array}\right] = R_v^b \left[\begin{array}{c} 0\\ 0\\ mg \end{array}\right] + \left[\begin{array}{c} 0\\ 0\\ -F \end{array}\right] =mg \left[\begin{array}{ccc} -S_\theta \\ S_\phi C_\theta \\ C_\phi C_\theta \end{array}\right] + \left[\begin{array}{c} 0\\ 0\\ -F \end{array}\right] }$

このときドローンの運動方程式に基づく状態方程式は次式となります。

$\displaystyle{ \left[\begin{array}{c} \dot{p}_n \\ \dot{p}_e \\ -\dot{h} \\ \hline \dot{\phi} \\ \dot{\theta} \\ \dot{\psi} \\ \hline \dot{u}\\ \dot{v}\\ \dot{w} \\ \hline \dot{p}\\ \dot{q}\\ \dot{r} \end{array}\right] = \left[\begin{array}{ccc|ccc} 0 & 0 & 0 & 0 & 0 & 0 \\ 0 & 0 & 0 & 0 & 0 & 0 \\ 0 & 0 & 0 & 0 & 0 & 0 \\ \hline 0 & 0 & 0 & 0 & 0 & 0 \\ 0 & 0 & 0 & 0 & 0 & 0 \\ 0 & 0 & 0 & 0 & 0 & 0 \\ \hline 0 & 0 & 0 & 0 & 0 & 0 \\ 0 & 0 & 0 & 0 & 0 & 0 \\ 0 & 0 & 0 & 0 & 0 & 0 \\ \hline 0 & 0 & 0 & 0 & 0 & 0 \\ 0 & 0 & 0 & 0 & 0 & 0 \\ 0 & 0 & 0 & 0 & 0 & 0 \\ \end{array}\right.}$
$\displaystyle{ \left.\begin{array}{ccc|ccc} C_\theta C_\psi &S_\phi S_\theta C_\psi - C_\phi S_\psi &C_\phi S_\theta C_\psi + C_\phi S_\psi & 0 & 0 & 0\\ C_\theta S_\psi &S_\phi S_\theta S_\psi + C_\phi C_\psi &C_\phi S_\theta S_\psi - S_\phi C_\psi & 0 & 0 & 0\\ -S_\theta &S_\phi C_\theta &C_\phi C_\theta & 0 & 0 & 0\\ \hline 0 & 0 & 0 & 1 & S_\phi T_\theta & C_\phi T_\theta \\ 0 & 0 & 0 & 0 & C_\phi & -S_\phi \\ 0 & 0 & 0 & 0 & \frac{S_\phi}{C_\theta} & \frac{C_\phi}{C_\theta}\\ \hline 0 & r &- q & 0 & 0 & 0\\ r- & 0 &p & 0 & 0 & 0\\ q & -p & 0 & 0 & 0 & 0\\ \hline 0 & 0 & 0 & 0 & 0 & -\frac{J_z-J_y}{J_x}q\\ 0 & 0 & 0 & -\frac{J_x-J_z}{J_y} r & 0 & 0\\ 0 & 0 & 0 & 0 & -\frac{J_y-J_x}{J_z}p & 0 \end{array}\right] }$
$\displaystyle{ \times \left[\begin{array}{c} p_n \\ p_e \\ -h \\ \hline \phi \\ \theta \\ \psi \\ \hline u\\ v\\ w \\ \hline p\\ q\\ r \end{array}\right] + \left[\begin{array}{cccc} 0 & 0 & 0 & 0 \\ 0 & 0 & 0 & 0 \\ 0 & 0 & 0 & 0 \\ \hline 0 & 0 & 0 & 0 \\ 0 & 0 & 0 & 0 \\ 0 & 0 & 0 & 0 \\ \hline 0 & 0 & 0 & 0 \\ 0 & 0 & 0 & 0 \\ \frac{1}{m} & 0 & 0 & 0 \\ \hline 0 & \frac{1}{J_x} & 0 & 0 \\ 0 & 0 & \frac{1}{J_y} & 0 \\ 0 & 0 & 0 & \frac{1}{J_z} \\ \end{array}\right] \left[\begin{array}{cccc} k_1 & k_1 & k_1 & k_1 \\ 0 & -\ell k_1 & 0 & \ell k_1 \\ \ell k_1 & 0 & -\ell k_1 & 0 \\ -k_2 & k_2 & -k_2 & k_2 \end{array}\right]^{-1} \left[\begin{array}{c} \delta_f\\ \delta_r\\ \delta_b\\ \delta_\ell \end{array}\right] + \left[\begin{array}{c} 0 \\ 0 \\ 0 \\ \hline 0 \\ 0 \\ 0 \\ \hline -gS_\theta \\ gS_\phi C_\theta \\ gC_\phi C_\theta \\ \hline 0 \\ 0 \\ 0 \end{array}\right] }$

これを見ると、 $A$ 行列に状態変数が絡んでおり、複雑なダイナミクスをもっていることが分かります。

運動方程式の簡単化

次の仮定のもとで、運動方程式の簡単化を行ないます。

仮定１：　 $qr$ 、 $pr$ 、 $pq$ は小さく無視できる
仮定２：　 $\phi$ 、 $\theta$ は小さく無視できる

まず、仮定１のもとで、次の近似が可能です。

$\displaystyle{ \left[\begin{array}{c} \dot{p}\\ \dot{q}\\ \dot{r} \end{array}\right] =- \left[\begin{array}{c} \frac{J_z-J_y}{J_x}qr \\ \frac{J_x-J_z}{J_y}pr \\ \frac{J_y-J_x}{J_z}pq \end{array}\right] + \left[\begin{array}{c} \frac{1}{J_x}\tau_\phi\\ \frac{1}{J_y}\tau_\theta\\ \frac{1}{J_z}\tau_\psi \end{array}\right] \simeq \left[\begin{array}{c} \frac{1}{J_x}\tau_\phi\\ \frac{1}{J_y}\tau_\theta\\ \frac{1}{J_z}\tau_\psi \end{array}\right] }$

このとき、仮定２をおけば次の近似が可能です。

$\displaystyle{ \left[\begin{array}{c} \dot{\phi} \\ \dot{\theta} \\ \dot{\psi} \end{array}\right] = \left[\begin{array}{ccc} 1 & S_\phi T_\theta & C_\phi T_\theta \\ 0 & C_\phi & -S_\phi \\ 0 & \frac{S_\phi}{C_\theta} & \frac{C_\phi}{C_\theta} \end{array}\right] \left[\begin{array}{c} p \\ q \\ r \end{array}\right] \simeq \left[\begin{array}{c} p \\ q \\ r \end{array}\right] }$
$\displaystyle{ \left[\begin{array}{c} \ddot{\phi} \\ \ddot{\theta} \\ \ddot{\psi} \end{array}\right] \simeq \left[\begin{array}{c} \dot{p}\\ \dot{q}\\ \dot{r} \end{array}\right] \simeq \left[\begin{array}{c} \frac{1}{J_x}\tau_\phi\\ \frac{1}{J_y}\tau_\theta\\ \frac{1}{J_z}\tau_\psi \end{array}\right] }$

これは、機体の傾きを独立して制御できることを示しています。

さらに次のような近似が可能です。

$\displaystyle{ \left[\begin{array}{c} \dot{p}_x \\ \dot{p}_y \\ \dot{p}_z \end{array}\right] = {R}_{v1}^b \left[\begin{array}{c} u \\ v \\ w \end{array}\right] }$
$\displaystyle{ \left[\begin{array}{c} \ddot{p}_x \\ \ddot{p}_y \\ \ddot{p}_z \end{array}\right] = \underbrace{\dot{R}_{v1}^b}_{\simeq0} \left[\begin{array}{c} {u}\\ {v}\\ {w} \end{array}\right] + {R}_{v1}^b \left[\begin{array}{c} \dot{u}\\ \dot{v}\\ \dot{w} \end{array}\right] \simeq {R}_{v1}^b \left[\begin{array}{c} \dot{u}\\ \dot{v}\\ \dot{w} \end{array}\right] }$
$\displaystyle{ \left[\begin{array}{c} \ddot{p}_x \\ \ddot{p}_y \\ \ddot{p}_z \end{array}\right] \simeq \left[\begin{array}{ccc} C_\theta & S_\phi S_\theta & C_\phi S_\theta \\ 0 & C_\phi & -S_\phi \\ -S_\theta &S_\phi C_\theta &C_\phi C_\theta \end{array}\right] (g \left[\begin{array}{c} -S_\theta \\ S_\phi C_\theta \\ C_\phi C_\theta \end{array}\right] +\frac{1}{m} \left[\begin{array}{c} 0\\ 0\\ -F \end{array}\right]) }$
$\displaystyle{ = \left[\begin{array}{c} 0 \\ 0 \\ g \end{array}\right] -\frac{1}{m}F \left[\begin{array}{ccc} C_\phi S_\theta \\ -S_\phi \\ C_\phi C_\theta \end{array}\right] \simeq \left[\begin{array}{c} 0 \\ 0 \\ g-\frac{1}{m}F \end{array}\right] }$

結局、ドローンの運動方程式は、次式のように簡単化されます。

$\displaystyle{m\ddot{p}_z=mg-(F_f+F_r+F_b+F_\ell)}$
$\displaystyle{J_x\ddot{\phi}=\ell(F_\ell-F_r)}$
$\displaystyle{J_y\ddot{\theta}=\ell(F_f-F_b)}$
$\displaystyle{J_z\ddot{\psi}=\tau_r+\tau_\ell-\tau_f-\tau_b}$

いま，重力補償のために $mg=4k_f\delta^*$ から決まる $\delta^*$ を各操作入力に前もって加えておくことにすると、次式が成り立ちます。

$\displaystyle{m\ddot{p}_z=4k_f\delta^*-(k_f\delta_f+k_f\delta_r+k_f\delta_b+k_f\delta_\ell)}$
$\displaystyle{=-k_f(\delta_f-\delta^*)-k_f(\delta_r-\delta^*)-k_f(\delta_b-\delta^*)-k_f(\delta_\ell-\delta^*))}$

$\displaystyle{J_x\ddot{\phi}=\ell(k_f\delta_\ell-k_f\delta_r) =\ell(k_f(\delta_\ell-\delta^*)-k_f(\delta_r-\delta^*))}$

$\displaystyle{J_y\ddot{\theta}=\ell(k_f\delta_f-k_f\delta_b)=\ell(k_f(\delta_f-\delta^*)-k_f(\delta_b-\delta^*))}$

$\displaystyle{J_z\ddot{\psi}=(k_\tau\delta_r+k_\tau\delta_\ell)-(k_\tau\delta_f+k_\tau\delta_b)}$
$\displaystyle{=(k_\tau(\delta_r-\delta^*)+k_\tau(\delta_\ell-\delta^*))-(k_\tau(\delta_f-\delta^*)+k_\tau(\delta_b-\delta^*))}$

このときドローンの線形状態方程式は次式となります。

$\displaystyle{ \underbrace{ \left[\begin{array}{c} \dot{p}_z \\ \dot{\phi} \\ \dot{\theta} \\ \dot{\psi} \\\hline \ddot{z} \\ \ddot{\phi} \\ \ddot{\theta} \\ \ddot{\psi} \\ \end{array}\right] }_{\dot{x}} = \underbrace{ \left[\begin{array}{cccc|cccc} 0 & 0 & 0 & 0 & 1 & 0 & 0 & 0 \\ 0 & 0 & 0 & 0 & 0 & 1 & 0 & 0 \\ 0 & 0 & 0 & 0 & 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 0 & 0 & 0 & 0 & 1 \\\hline 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 \\ 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 \\ 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 \\ 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 \\ \end{array}\right] }_{A} % \left[\begin{array}{c|c} % 0_{4\times4} & I_4 \\\hline % 0_{4\times4} & 0_{4\times4} % \end{array}\right] \underbrace{ \left[\begin{array}{c} {p_z} \\ {\phi} \\ {\theta} \\ {\psi} \\\hline \dot{z} \\ \dot{\phi} \\ \dot{\theta} \\ \dot{\psi} \\ \end{array}\right] }_{x} }$
$\displaystyle{+ \underbrace{ \left[\begin{array}{cccc} 0 & 0 & 0 & 0 \\ 0 & 0 & 0 & 0 \\ 0 & 0 & 0 & 0 \\ 0 & 0 & 0 & 0 \\\hline -\frac{k_f}{m} & -\frac{k_f}{m} & -\frac{k_f}{m} & -\frac{k_f}{m} \\ 0 & -\frac{\ell k_f}{J_x} & 0 & \frac{\ell k_f}{J_x} \\ \frac{\ell k_f}{J_y} & 0 & -\frac{\ell k_f}{J_y} & 0 \\ -\frac{k_\tau}{J_z} & \frac{k_\tau}{J_z} & -\frac{k_\tau}{J_z} & \frac{k_\tau}{J_z} \\ \end{array}\right] }_{B} % \left[\begin{array}{c} % 0_{4\times4} \\ % B_2 \\ % \end{array}\right] \underbrace{ \left[\begin{array}{c} \delta_f-\delta^* \\ \delta_r-\delta^* \\ \delta_b-\delta^* \\ \delta_\ell-\delta^* \\ \end{array}\right] }_{u} }$