比例航法ミサイルを作ろう[Stormworks: Build and Rescue]

0. はじめに

この記事は、Stormworks advent calendar 2023の17日目の記事です。

ミサイルを作りたいストームワーカーはたくさんいることでしょう。今回は、やや本格的な誘導ロジックを雰囲気で実装してみた話です。私もよくわかってないので、適当にやります。

こんな感じのができた↓

1. さまざまな誘導法

ミサイル誘導には、さまざまな方法があります。

• LOS誘導¹
• 定方位航法(Constant Bearing Navigation : CBN)²
• 単純追尾航法(Pure Pursuit Navigation : PPN)
• 比例航法(Proportional Navigation : PN)
  • Pure PN
  • True PN
  • 拡張比例航法(Augmented PN)
  • 修正比例航法(Modified PN)

これ以外にも方法はあるのでしょうが、stormworksでミサイルを作っている方はだいたいPPNとPNを利用している気がします。この記事ではその2つの誘導法に注目していきます。馴染みのない方向けに簡単に説明しますが、わかっている方は飛ばしてください。

1-1. 単純追尾航法(PPN)とは

単純追尾航法は端的に言えば、ミサイルがターゲットを常に正面に捉え続ける誘導法です。LOS(Line-Of-Sight,ミサイルとターゲットを結ぶ直線)と、ミサイルの進行方向が常に同じになるようにフィンを操作すればよいわけですね。

PPNはとても簡単な誘導方法である反面、デメリットもあります。

1つは、移動目標をミサイルが追いかける際、必ず目標の後ろに回り込む軌道になってしまうことです。これでは射程距離が短くなってしまいます(効率が悪い)。

2つ目は、ミサイルがターゲットよりもある程度高速な場合、命中直前の加速度が無限大に発散してしまうことです。無限大の加速度での機動など不可能なので、必ずミスディスタンスが生じます。

このように、PPNは固定目標を狙う場合には適していますが、高速で動く目標を狙うのは苦手です。

LOSベクトルと速度ベクトルの向きを一致させる誘導法
仕組みは簡単だが移動目標には不向き

1-2. 比例航法(PN)とは

名前の通り「比例」が登場します。LOSはターゲットを追いかけるにつれて角度が変わっていきますね。その角度の変化に比例させてミサイル自身の向きを変えることで誘導します。LOSが一定になればターゲットに命中するコースに乗ります。

ミサイルとターゲットを結ぶ直線の角度が一定になっているのがわかると思います。このように移動目標に対しても効果的な誘導ができるのです。

式で表現するとミサイルの角速度$ \dot{\gamma_m} $ 、LOSの角速度$ \dot{\sigma} $、航法定数$ N $が以下の関係になるようにします。

\dot{\gamma_m} = N \dot{\sigma}

LOSの角速度のN倍の角速度でミサイルを回す

2. 実装の前に

本題に入る前に、前提条件の確認です。

2-1. ミサイル本体の動き

ミサイルには座標や姿勢を取得するphysics sensorを設置します。大きい矢印がZ軸、小さい矢印がX軸、センサー上面方向がY軸です。わかりづらい。

ミサイルに設置するときは、私は進行方向をZ軸、右方向をX軸としています。

そしてミサイルフィンの入力と動作ですが、1ch(Yaw)に正入力でY軸周りに順回転(右へ曲がる)、2ch(Pitch)に正入力でX軸周りに順回転する(下へ曲がる)ようにしています。

1chに(+)入力 → 右に曲がる
2chに(+)入力 → 下に曲がる

今回はロールの制御は特にしていませんが、ローカルZ軸角速度を0になるようにするのも良いと思います。

2-2. ワールド系→ローカル系変換

ミサイルに設置したphysics sensorから得られる角速度はワールド系基準の角速度です(単位はrot/sec)。また、ミサイルの位置やターゲットの位置などもワールド系で考えていくので指令角速度もまずはワールド系で得られます。

これらの数値をフィンに入力する前にローカル系に変換する必要があります。手段としては回転行列でもクォータニオンでも何でもいいです。自分に合う方でやったらいいんじゃないですかね。今回はクォータニオンを使ってみました。

一応、ちょっとした解説と今回使用するプログラムを載せておきます。

クォータニオンについて

クォータニオン(四元数)は、3つの虚部と1つの実部からなり……といった解説はよくありますが、実際使う上では4要素からなるベクトルとして考えるだけです。

回転軸ベクトル$(\lambda_x, \lambda_y, \lambda_z)$の周りに角度$\theta$だけ回転することを表すクォータニオンは

q = (\lambda_xsin\frac{\theta}{2}, \lambda_ysin\frac{\theta}{2}
, \lambda_zsin\frac{\theta}{2}, cos\frac{\theta}{2})

となります。これだけ見ると訳わからん……となりそうですが、軸別に整理すると分かりやすいです。

つまり、$x$軸周りの回転なら回転軸ベクトルは$(1, 0, 0)$となりますから、$x$軸周りに$\theta$だけ回す場合

q_x = (sin\frac{\theta}{2}, 0, 0, cos\frac{\theta}{2})

となります。同様に$y$軸周りに$\phi$、$z$軸周りに$\psi$の回転はそれぞれ、

\begin{align}
q_y &= (0, sin\frac{\phi}{2}, 0, cos\frac{\phi}{2}) \\

q_z &= (0, 0, sin\frac{\psi}{2}, cos\frac{\psi}{2})
\end{align}

となります。physics sensorの出力は各軸周りの回転角なので、これらの式がそのまま使えるわけです。

さて、ではこのクォータニオンを使ってベクトルを回転させるにはどうすればよいのでしょうか。ベクトル$\vec{v}$をクォータニオン$q$で回転させるには、

\vec{v'} = q⊗\vec{v}⊗\bar{q}

のようにします。ただし、$\bar{q}$は$q$の共役クォータニオンで、⊗はクォータニオンの積を表します。

physics sensorはch4~6に$x, y, z$軸周りの回転角$\theta,\phi,\psi$を出力します。この出力の意味するところは、

ワールド座標系を

1. 最初に $x$軸周りに $\theta$回転させる
2. つぎに $y$軸周りに $\phi$回転させる
3. 最後に $z$軸周りに $\psi$回転させるとローカル座標系になる

ということです。したがって

\begin{align}
\vec{v_{local}} &= q_z⊗(
q_y⊗(
q_x⊗\vec{v_{world}}⊗\bar{q_x}
)⊗\bar{q_y}
)⊗\bar{q_z} \\

&= q_z q_y q_x ⊗ \vec{v_{world}} ⊗ \bar{(q_zq_yq_x)}
\end{align}

のように掛け算をすることでphysics sensor基準のローカル座標系に変換することができます。

クォータニオンの積は非可換です。$q_xq_yq_z$などのように順番を間違えると正しく動作しないので気を付けましょう。

ちなみに共役クォータニオンは逆回転を表すので、ローカル→ワールド変換をしたい場合は$q_zq_yq_x$の共役クォータニオンを使うだけです。

Quaternion

Quat = {
    new = function(x, y, z, w)
        local q = {}
        q.x = x
        q.y = y
        q.z = z
        q.w = w
        return q
    end,

    conjugate = function(q)
        local x, y, z, w
        x = -q.x
        y = -q.y
        z = -q.z
        w =  q.w
        return Quat.new(x, y, z, w)
    end,

    fromEulerXYZ = function(x, y, z) --radian
        local S,C = math.sin, math.cos
        local qx, qy, qz
        qx = Quat.new(S(x/2), 0, 0, C(x/2))
        qy = Quat.new(0, S(y/2), 0, C(y/2))
        qz = Quat.new(0, 0, S(z/2), C(z/2))
        --掛ける順番は Z -> Y -> X
        return Quat.mul(Quat.mul(qz, qy), qx)
    end,

    mul = function(a, b)
		local x, y, z, w
		x = a.w*b.x + a.x*b.w + a.y*b.z - a.z*b.y
		y = a.w*b.y + a.y*b.w + a.z*b.x - a.x*b.z
		z = a.w*b.z + a.z*b.w + a.x*b.y - a.y*b.x
		w = a.w*b.w - a.z*b.z - a.y*b.y - a.x*b.x
		return Quat.new(x, y, z, w)
    end,

    rotVec = function(v, q)
        local _q = Quat.conjugate(q)
        --ベクトルは4要素目(実部)が0のクォータニオンとして扱う
        v = Quat.new(v.x, v.y, v.z, 0)
        return Quat.mul(Quat.mul(q, v), _q)
    end
}

3. 実装してみよう

ベクトルの計算をどんどんやるので、ベクトルライブラリも用意します。

ベクトルライブラリ

Vector3

Vec3 = {
    new = function(x, y, z)
        local v = {}
        v.x = x
        v.y = y
        v.z = z
        return v
    end,

    add = function(a, b)
        return Vec3.new(a.x+b.x, a.y+b.y, a.z+b.z)
    end,

    sub = function(a, b)
        return Vec3.add(a, Vec3.scale(b, -1))
    end,

    scale = function(v, k)
        return Vec3.new(v.x*k, v.y*k, v.z*k)
    end,

    dot = function(a, b)
        return a.x*b.x + a.y*b.y + a.z*b.z
    end,

    cross = function(a, b)
        local x, y, z
        x = a.y*b.z - a.z*b.y
        y = a.z*b.x - a.x*b.z
        z = a.x*b.y - a.y*b.x
        return Vec3.new(x, y, z)
    end,

    length = function(v)
        return math.sqrt(Vec3.dot(v, v))
    end
}

3-1. 単純追尾航法の実装

単純追尾航法では、LOSベクトル(以下、$\vec{R}$)と、ミサイルの速度ベクトル(以下、$\vec{V_m}$)のなす角が0になるように横加速度$\vec{a_c}$を発生させます。したがって、指令加速度$\vec{a_c}$は定数K'を用いて

\begin{align}
\vec{a_c} &= K'\frac{\vec{V_m}\times\vec{R}}
{\vec{V_m}・\vec{R}}\times\vec{V_m} \\

\vec{a_c} &= K\frac{\vec{V_m}\times{\vec{R}}}{R^2}\times\vec{V_m}

\end{align}

と表せます。(ただし、$K = K'\frac{R}{V_m}$とする)

多くの資料ではこのようにミサイルの目標横加速度について書かれているのですが、ゲーム内では横加速度を制御するより角速度を制御したほうが楽です。

ですので、指令加速度を指令角速度の形式に変換しましょう。
円運動を考えると、$a=\omega^2 r = \omega v$となるので

\begin{align}
\vec{a_c}(=\vec{\omega_c}\times\vec{V_m}) &= K\frac{\vec{V_m}\times\vec{R}}{R^2}\times\vec{V_m} \\\

\vec{\omega_c} &= K\frac{\vec{V_m}\times\vec{R}}{R^2}
\end{align}

この指令角速度$\vec{\omega_c}$を目指してミサイルの角速度をPID制御につっこめばPPNは完成します。

以上の内容を簡潔にコードにするとこのようになります。

PPN Guidance

R = Vec3.sub(Rt, Rm) --LOSベクトル Rtターゲット位置 Rmミサイル位置
r = Vec3.length(R) --LOSの長さ=ターゲットとの距離
Vm = Vec3.sub(Rm, Rm_old) --ミサイル速度
q = Quat.fromEulerXYZ(x, y, z) --physics sensor ch4-6を入力
K = 1*r /Vec3.length(Vm) --係数

--指令角速度を計算し、ローカル系に変換
Omega = Vec3.scale(Vec3.cross(Vm, R), K/r^2)
set_angular = Quat.rotVec(Omega, q)

--この値をPIDの目標値にする
output.setNumber(1, set_angular.y)
output.setNumber(2, set_angular.x)

3-2. 比例航法の実装

比例航法はLOSの角速度に比例して横加速度を与える誘導法ですが、厳密にはミサイル自身の速度情報のみを用いるPure PNと、ターゲットとの相対速度(以下$\vec{V_r}$)も加味するTrue PNに分類できます。

限定的な条件下ですが、有効航法定数を3に設定したTrue PNが最高効率の誘導則と一致すると知られています。ということで、True PNを実装しましょう。

True PNの指令加速度$\vec{a_c}$は、有効航法定数$N'$を用いて

\begin{align}
\vec{a_c} &= N\frac{\vec{R}\times\vec{V_r}}{R^2}\times\vec{V_m} \\
ただし、N &= \frac{N'V_r}{V_mcos\phi}
\end{align}

のようになります($\phi$は$\vec{V_m}$と$\vec{R}$のなす角)。

PPNと同様にして指令角速度$\vec{\omega_c}$を求めます。

\vec{\omega_c} = N\frac{\vec{R}\times\vec{V_r}}{R^2}

では、これをコードにしましょう。

True PN Guidance

R = Vec3.sub(Rt, Rm) --LOSベクトル
r = Vec3.length(R) --ターゲットとの距離

Vm = Vec3.sub(Rm, Rm_old) --ミサイル速度
Vt = Vec3.sub(Rt, Rt_old) --ターゲット速度
Vr = Vec3.sub(Vt, Vm) --相対速度

q = Quat.fromEulerXYZ(x, y, z) --physics sensor ch4-6を入力
Ne = 3 --有効航法定数
N = Ne*Vec3.length(Vr)*r /Vec3.dot(Vm, R) --航法定数

--指令角速度を計算し、ローカル系に変換
Omega = Vec3.scale(Vec3.cross(R, Vr), N/r^2)
set_angular = Quat.rotVec(Omega, q)

--この値をPIDの目標値にする
output.setNumber(1, set_angular.y)
output.setNumber(2, set_angular.x)

3-3. ミサイルフィンに入力

誘導に必要な角速度を算出することができました。あとはミサイル自身のローカル角速度を求め、PID制御することでミサイルが完成します。

ミサイルローカル角速度

IN = input.getNumber
ax, ay, az = IN(10), IN(11), IN(12) --センサー入力

msl_angular = Quat.rotVec(Vec3.new(ax, ay, az), q)

では今までのコードをまとめてミサイル制御プログラムを完成させます。外部から標的の座標を受け取り、命中まで常に比例航法を行います。これは、あまり現実的でないので、終末誘導のミサイルレーダーとのスイッチなどは適宜組み込んでみよう(丸投げ)！

Missile Guidance

IN = input.getNumber
ON = output.setNumber
PN = property.getNumber
m = math
pi = m.pi
pi2 = pi*2
S = m.sin
C = m.cos

--Vector3
Vec3 = {} --省略
--Quaternion
Quat = {} --省略

--1tick前のミサイル位置・ターゲット位置を初期化
Rm_old, Rt_old = Vec3.new(0,0,0), Vec3.new(0,0,0)

launched = false

function onTick()
Rm = Vec3.new(IN(1), IN(2), IN(3)) --ミサイル座標
Rt = Vec3.new(IN(7), IN(8), IN(9)) --ターゲット座標
q = Quat.fromEulerXYZ(IN(4), IN(5), IN(6)) --姿勢クォータニオン
angular = Vec3.scale(Vec3.new(IN(10), IN(11), IN(12)), pi2)
msl_angular = Quat.rotVec(angular, q) --ミサイル角速度 rad/s

R = Vec3.sub(Rt, Rm) --LOSベクトル
r = Vec3.length(R) --ターゲットとの距離
Vm = Vec3.scale(Vec3.sub(Rm, Rm_old), 60) --ミサイル速度 m/s
Vt = Vec3.scale(Vec3.sub(Rt, Rt_old), 60) --ターゲット速度 m/s
Vr = Vec3.sub(Vt, Vm) --相対速度 m/s

--True PN
Ne = 3 --有効航法定数
N = Ne*Vec3.length(Vr)*r / Vec3.dot(Vm, R) --航法定数
PN_Omega = Vec3.scale(Vec3.cross(R, Vr), N/r^2)
set_angular = Quat.rotVec(PN_Omega, q)

--出力
launched = Vec3.length(Vm) > 100
if launched then
    --PID 目標値
    ON(1, set_angular.y)
    ON(2, set_angular.x)

    --PID 入力値
    ON(3, msl_angular.y)
    ON(4, msl_angular.x)
end

Rm_old = Vec3.new(IN(1), IN(2), IN(3))
Rt_old = Vec3.new(IN(7), IN(8), IN(9))
end

3-4. おまけ

上記のプログラムは、PIDの調整やミサイル本体の調整やらがかなり苦しいです。なんというか、ロバストネスに欠ける感じです。技量がある人ならいいのですが、お手軽に比例航法っぽく誘導したい！という方には厳しいです(私にも厳しかった)。

というわけで調整の手間が格段に減る、パチモン比例航法も紹介します。邪道ですが、気にしないようにしましょう。

どうするかというと、PID制御なんかしないで、直接目標角速度をミサイルフィンにぶち込みます。

function onTick()

--~~~

--True PN
Ne = property.getNumber("navigation constant")
N = Ne*Vec3.length(Vr)*r / Vec3.dot(Vm, R)
PN_Omega = Vec3.scale(Vec3.cross(R, Vr), N/r)
set_angular = Quat.rotVec(PN_Omega, q)

--出力
if launched then
    --フィンに直接突っ込む
    ON(1, set_angular.y)
    ON(2, set_angular.x)
end

--~~~

end

①フィンに目標値を直接突っ込む。
②目標角速度に距離を掛けると良かった(N/r^2 → N/r に変更)。

これだけでOKです。あとは安定するように定数をいじってやればなんかうまくいきます。そう、うまくいくのが重要なんです。

1. 地上のレーダー等とターゲットを結ぶ直線上にミサイルを乗っけるという誘導法らしいです。よくわかりません。 ↩

2. ターゲットが向かう場所に先回りして直線で向かえばいいじゃんという発想です。つまりLOS角が一定である必要がありますが、そのような状況はなかなか起こりませんね。 ↩