stormworks/自動操縦（航空機）
自動操縦
自動操縦したいことがある
航空機のオートパイロットを実現する
巡航/弾道ミサイルの中間誘導を実現する
イニシャルフェイズは決め打ちで安定させる
機体の重量バランス
後ろを重くすると上にピッチングするのでどんどん上に行く
逆もまた然り
出力調整
大出力にすると揚力が増加するのでどんどん上に行く
逆もまた然り
補助ロケット
ジェットエンジンは出力が安定するまでしばらくかかるのでその間の速度を一時的に稼ぐ
重心に近い位置で噴射しないとひっくりかえる
揚力位置
揚力位置が前であるほど上にピッチングするモーメントが働くのでどんどん上に行く
逆もまた然り
ターミナルフェイズはアクティブレーダーで誘導などする
Missile finなどを使い分けてしまうのがシンプルで良い
別々の動翼で制御する
自動操縦の前に
まずは安定した機体を作る
勝手に安定してそれなりに水平に飛ぶようにする
入力がなくても自動的に水平に戻るように設計する
安定性を高める、という
重心より少し前に揚力を置け
機体を浮かせ続けるために必要
重心より後ろに垂直尾翼を置け
ヨー安定性が上がる
重心より後ろに水平尾翼を置け
ピッチ安定性が上がる
揚力よりも重心を下に置け
ロール安定性が上がる
この状態にすると、あとはエンジン出力だけで（ある程度）高度の制御ができるようになる
吹かすと上がっていく
抑えると下がっていく
バンクさせると、バンクさせた方向にゆっくり旋回するような機体にしておくと都合が良い
バンクすると主翼を貫く方向に横ずれが生じていく
重力によって下に滑っていくイメージ
機体がバンク方向にヨーする力が発生する
機体は横ずれによって横方向から風の力を受ける
垂直尾翼は機体の後部にしかないので、機体の後方だけが風に押される
機体がバンク方向に回転するようなモーメントが発生する
機体の先頭が重いとこのモーメントを発生させやすい？
自動操縦3種
自動操縦にもいろいろある
高度を維持
進路を維持
上昇率を維持
バンク角を維持
しかしこれらはつまるところ以下の3つの制御の組み合わせにすぎない
バンク角を一定のセットポイントに収め続ける
ピッチ角を一定のセットポイントに収め続ける
ヨーを一定のセットポイントに収め続ける
（これにスロットルによる制御が入るかも）
バンク中は同一スロットルでは高度を保てない。高度を維持するために加速する必要がある
揚力のうち、機体を重力に逆らって持ち上げている割合が減少するため
個人的には、ピッチングで高度を維持しようとするのは悪手
さらに減速するだけ
セットポイントを一定に保てば、上昇率維持やバンク角維持、旋回率維持ができる
セットポイントを動的に制御すれば、高度維持や方向維持ができる
バンク角を維持する
ティルトセンサを機体の真横に付ける
主翼の向きに1つつける
-0.25～0.25 turnの値を出してくれる
この値が一定になるようにエルロンを制御すればよい
PID用のマイコンを作成する
エルロンは左右で逆に動く必要があり、NOT gateをわざわざ設置するよりはマイコンに収めて出力を2つにしたほうが良い
入力
ティルトセンサ
セットポイント
0に固定すると水平飛行をするだけだが、ここを動かせるようにすることで任意の角度に向けられる
（オプショナル）ロール入力
自律飛行するミサイルでは不要だが、航空機ではユーザの入力 + 自動操縦用入力で操縦したいため
出力
ロール
ロール（反転）
プロパティ
P/I/Dパラメータ
（オプショナル）ロール出力を反転させるトグル
うっかり設置方向が逆になることはよくあるのであると便利
PIDのパラメータは調整が必要なためAdvanced PIDを選択しておく
パラメータ化して後からいじれるようにしよう
自分は初期値はP/I/D: 1/0.0005/0.2くらいにしている
https://scrapbox.io/files/690265a5f671e187d1dae453.png
マイコンにエルロンを接続する
セットポイントを0にしておくと勝手に水平飛行してくれる
方位を維持する(HDG)
『バンク角を維持する』で作ったマイコンのセットポイントを「良い感じ」に制御すればよい
バンクさせ続ければそっちに転針してくれるはずだから
バンクをやめればその方位を維持してくれるはずだから
目的の方位と自機の向きとの差分をとる
目的の方位を決め、それをラジアンに変換する
自機の向きはコンパスセンサで得られるので、それをラジアンに変換する
差分を取るのはちょっと複雑
雑に差分を取ると0度から359.999...度の間でガクンとしてしまう
ラジアンは不連続で困る
そこで、$ atan2を利用して差分を取る
すなわち、$ atan2(sin(\phi - \theta), cos(\phi - \theta))
ここで、$ \phiは目的の方位(ラジアン)であり、$ \thetaは自機の向き（ラジアン）
すると、どちら向きにどの程度転針するべきかわかる
あとはこれに一定の定数（0.05くらい）を掛けてセットポイントにつなぐ
壊れるときはセットポイントを反転させてみる
任意の位置に向けて飛行させる(LNAV?)
『方位を維持する(HDG)』では、目的の方位さえ分かればそこに向けて飛行できることがわかった
目的の方位は、自機のGPS座標と目的の位置から算出できる
まず位置の差分ベクトルを算出する
$ (x_\triangle, y_\triangle) = (x_t - x_c, y_t - y_c)
目的の方位を求めるには$ atan2を利用する
すなわち、$ \phi = atan2(x_\triangle, y_\triangle)
ここで、$ (x_t, y_t)は目的地の座標、$ (x_c, y_c)は現在地の座標（GPSセンサで得られる）
$ atan2はX軸の方向を向いた状態が0なので、磁北が0になるようにxとyとを入れ替えて-90度補正している
目的の位置はWaypointを使って決められる
WaypointはKeypadに転送できるのでこれを利用する
Keypadは2つのNumericalな入力を返してくれる
https://scrapbox.io/files/69050774490cfc8c20f4f947.png
方位維持機能と合体させたマイコンはこういう感じになる
任意のV/Sを維持させる
V/S: Vertical Speed（昇降速度）
『バンク角を維持する』では、ティルトセンサを利用してバンク角を維持させた
V/Sを維持させるには、ティルトセンサのかわりに速度センサを利用して同様のPID制御を行う
速度センサの設定をVerticalにすることで垂直速度を得られる
速度センサの値をProcess Variableに設定する
目標のV/Sをセットポイントにする
0だとその高度を維持する
厳密には「昇降がないように頑張る」なので、維持できない
垂直速度が一定になるようにPID制御を行う
PID制御の出力をエレベータに接続する
すると任意のV/Sになるようにエレベータが動作する
任意の高度を維持させる
高度の制御は案外難しい
重力の影響を受けるため力のかかり方が非対称であること
単純にPIDでエレベータを制御すると失敗する
ピッチ角を上げても即座には高度が上昇しないため、PIDがどんどん出力を上げる
そのまますごい角度で設定した高度を突き抜け、逆のことが起こる
『任意のV/Sを維持させる』では、PID制御とエレベータを利用してV/Sを制御しているが、これは安定して動作する
下降中に引き起こすとかならずV/Sは減少していき、上昇していくため、PID制御しやすい
つまり、任意の高度に到達できるようなV/Sを算出して、V/S制御のセットポイントにすればよい
目的の高度に到達するためにV/Sを制御したいとき、どのようにコントロールするべきか？
単純に考えると、
目的の高度にいるとき: V/S: 0
目的の高度よりも上にいるとき: V/S: -10
目的の高度よりも下にいるとき: V/S: 10
のように制御したい
が、これだと段差が大きすぎてうまくいかない
こうしたい
目的の高度から離れているほどたくさんV/Sを変化させたい
目的の高度に接近するにつれてV/Sの変化は小さくしたい
すると目的の高度で安定するはず
ここで、便利な関数としてシグモイド関数を利用する
xが0から離れるほどシグモイド関数の出力も大きくなっていくが、一定の限度で安定する
高度がたくさん離れているからといって急降下してほしくはないので都合がよい
シグモイド関数から0.5を引けば、x=0のときy=0になってくれる
あとは良い感じにゲインを調整し、最後に適当な係数をかけることでV/Sを算出できる