前のページ [目次] (https://qiita.com/lier80/items/8d4ababd14e4db4222e3)
次のページ来週中には進みます

地上座標系と機上座標系の変換

なぜ座標系を変換するか

当然の疑問だと思います。我々の住む世界と飛んでいるものの世界が違うということはありませんし、実際のところ飛翔体の運動を地上から見た座標系で記述することも
十分に可能です。

ところが、実際にこれを行ってみると非常に面倒なことに気がつくと思います。

簡単のため - それでも頭にくるほど面倒ですが - 2Dの世界で考えてみます。
平面上にX,Y軸を貼り付けて、Y軸負方向に重力がかかるとします。

空力を考慮した放物運動を考慮すると、

(中略を読むには優しい言葉をかけてください)

このようなコードが出来上がります。ここで注目したいのが、座標系が一定の姿勢であるにもかかわらず、飛翔体の進行方向と飛翔体にかかる力の象限(方向同士の関係)
が変化しているため、これらの処理を正しく行うためには迎角(A.O.A)による場合分けが多数必要に鳴るということです。一応動くようにしましたが、めっちゃめんどくさいです。発展性もないですし。

これ端的に言ってクソなんで、別の方法を考えます。

そもそもの原因は、機体に対しての力の働き方がAOAに対して全部変わってくるところにあります。
だから、機体に座標軸を貼り付けてします。そうすれば、軸の変換(回転)をパラメータとしてすべての力を一定の関係性で表すことができます。

思いつく方法はいくつかあります。

1.回転行列によってxyz-にたいしてロール角、ピッチ角、ヨー角(r,p,y)を用いて、機体の座標系がどのような向きをしているのかを直接表してしまう
2.そもそも角度を用いず、四元数で座標を表してしまう。

いろいろな理由から、ここではrpyを用いた表し方を先ず選択します。
(あとで四元数もやります)

さて、よく見かける回転行列は、以下のように表されています。

\begin{matrix}
cos \theta & sin \theta & 0\\
-sin \theta & cos \theta & 0\\
0 & 0 & 1
\end{matrix}

これは x 方向の回転を表す行列であり、ここでの θ はx軸周りの回転を表すロール角となっています。面倒なのでこのページのみすべての回転角を全部 θ で表します。

全く同様に、 y, z 方向の回転も表していきます。

y方向(ピッチ)

\begin{matrix}
cos \theta & 0 & sin \theta \\
0 & 1 & 0 \\
-sin \theta & 0 & cos \theta
\end{matrix}

z方向(ヨー)

\begin{matrix}
1 & 0 & 0\\
0 & cos \theta & sin \theta\\
0 & -sin \theta & cos \theta
\end{matrix}

ここで注意して欲しいのが、r-p-yという書き順にはすでに意味があり、入れ替えは不可能となることです。
参考までに、

一般にベクトル積は

AB != BA

であります。具体的には、クリップのついたボールペンをロール30度、ピッチ45度、ヨー30度で動かしてみてください。順番によって向いている方向が違うはずです。
感覚的には、”回転の回転”が発生することが原因です。そのため、機上の座標系-機内の計器-が表しているr,p,yの数値で表される姿勢を、地上からの変換行列という形にすると、複数形式：複数数値で表すことができます。

極端な話、ひとつの姿勢系を表す際、r-p-r系なんていうもので三次元座標系を表すことも可能です。

そのため、なにか参考書を読む際はどの系で書いてあるか、rpy系なのかyrp系なのかを常に気をつけてください。
いま手元にある本はXYZ系に対してz-x-yの順番で回転しています。なので、yrp系ということになります。

さて、これによって系同士、機上座標系と地上座標系の間の角度の違い…具体的には飛翔体の姿勢…との関係性を表すことができましたから、
この関係性を使って、元の座標系もベクトルに直して行列として、2つの座標系の間の位置関係をベクトル同士の掛け算で表してみます。

以下、地上座標系を $ R_1, = (x_1,y_1,z_1) $ 機上座標系を $ R_2 = (x_2,y_2,z_2)$とします。ここでは簡単のため変換はx-y-zの順番で行うものとし、変換行列Aを使って

A = r \cdot p \cdot y \\

R_2 = A \cdot R_1

とします。
これらの計算を行うことによって、座標の変化を表すことができます。
これは、理系向けに言うと、端的に言って一次変換(ベクトルの写像)です。

ですから、飛翔体の座標系を絶対座標からの相対関係において記述することについて、飛翔体の姿勢の記述が記述できるということがお分かりいただけると思います。

※ 当方の都合上一旦ここで中断します。次回までに機上座標系を具体的に記述し、最低でも四元数記述まで、余裕があれば機体の位置推定の途中まででも行ければ良いなぁと思っていますが、何分時間が取れないので一旦失礼。@出社前

前のページ [目次] (https://qiita.com/lier80/items/8d4ababd14e4db4222e3)
次のページ来週中には進みます