marker by rocamisakitohko

23101計画型弾道計算機《Lacrimosa》

この記事はStormworks 第1 Advent Calendar 2023 3日目の記事です。

弾道計算機を作りました！

以前紹介した弾道計算機には、いくつか問題がありました。

  • システム遅延

姿勢を計算するLuaブロックと弾道計算を担当するLuaブロックと、複数のLuaブロックで情報をやり取りする方式ゆえ、システム遅延が増大している上、姿勢計算と弾道計算とでロジック遅延が異なる値になっており、遅延を御し難いものとなっていました。

  • 風速補正

砲弾が風を受けるときの加減速のメカニズムとパラメータを解明できておらず、風速に関する補正関数は近似式で構成されていました。

  • イテレーションの不確実性

弾道計算そのものと偏差射撃を同じループで行っていましたが、このループの終了条件が不確実で、偏差射撃に失敗したり、最悪の場合計算結果が発散することがありました。

  • Physics Sensor実装以前の設計

ほか全体的にも大仰なシステムであったため、艦砲などにはよいのでしょうが、地上車両やCIWSなど、コンパクトな設計が求められる場面には不適当でした。

これらの欠点を改善するためには、弾道計算機をいちから再開発する必要がありました。

弾道計算機とは？

読んで字の如く、銃砲から発射された弾丸の軌道、つまり砲外弾道を計算し、軌道や着弾地点を計算したり、また着弾させたい地点と砲の位置関係などから発射するべき方位を計算したりするための機械です。

現実世界でも、Stormworksにおいても、銃砲から発射された弾丸は直線的には飛びません。 弾丸は砲によって初速を与えられてからも、重力・空気抵抗など、様々な力を受けながら飛ぶことになります。

そのため、砲ごとに設定されているパラメータ、風速、砲と目標との位置関係、砲の移動速度などから飛翔軌道を計算する弾道計算機が必要になります。

弾道計算機は、発射方位から軌道や着弾地点を予測するものと、望ましい着弾地点から発射するべき方位を逆算するものがあります。 ここでは前者を『順行型』、後者を『逆行型』と呼ぶことにします。

これら2つのタイプの弾道計算機は、共通化できる部分も多いですが、共通化できない部分もまた多くあり、弾道計算のアプローチにもよりますが、順行型で実現できたことを逆行型では実現できないことがあります。

Stormworksにおいては、Rocket Launcherパーツから発射されるロケットや、ビークルのbody自体が飛翔するタイプのロケット・無誘導爆弾などはしばしば、順行型の弾道計算機でしか扱えないものとされることが多いようです。 弾体が飛翔中に加速したり、弾体に『向き』『空力中心』の概念があったりするので、飛翔時間や飛翔距離が解析的に解けなくなってしまいがちです。

今回は、Rocket Launcherやロケット、無誘導爆弾などは脇に置き、Machine GunからBertha Canonまでの実装しやすい砲熕兵器を対象にした逆行弾道計算機を開発しました。

23101計画型弾道計算機《Lacrimosa》

今回2023年10月初旬から中旬にかけて開発した弾道計算機、23101計画型弾道計算機《Lacrimosa》は、埼玉海軍・地上軍での使用を前提とした逆行弾道計算機です。

前回開発した弾道計算機についての思いつく限りの欠点を改善し、単一のLuaブロックですべての必要諸元を計算する方式としました。

マイコンの全体像

弾道計算マイコン

弾道計算機本体のマイコンはこのようになっています。

画面左側に大量のProperty Numberが鎮座しているのを除けば非常に簡素な形の、Luaブロックに必要な情報を入力し、出力されてきた値を外に出しているだけのマイコンです。

Ch1～6に目標のワールド座標とワールド速度を、Ch7～15にPhysics Sensorから得た自ビークル座標、姿勢、ローカル速度を、Ch16～18にVelocity Pivot終端の姿勢を、Ch19,20に風速と風向を入力し、LuaブロックAに渡します。

LuaブロックAは、Velocity PivotとRobotic Pivotで構成された2軸タレットの制御値と、着弾までの時間t、そして発射可否を出力します。

現状はAzimuth出力にさらにPIDブロックが直結されていますが、遅延低減のためにLuaブロックに統一することも考えています。

また、砲側の安全装置として距離計を搭載したり、姿勢誤差から発射可否を決定したりする改修が予定されています。

Luaブロック A

Luaブロックでは、コンポジット信号から入力された値のほか、砲ごとの諸元(初速、抵抗係数、タイマー、風速補正パラメータ)、Property Numberから入力された様々な制約条件をもとに、自機姿勢の決定から弾道計算、指向方位の計算までを一貫して実施します。

参考までに、使用しているコードの全文を掲示します。

Lacrimosa.lua
-- Saitama Navy project 23101 "Lacrimosa" by @rocamisakitohko

i,o,p,m=input,output,property,math
IB,IN,OB,ON,PN,PB=i.getBool,i.getNumber,o.setBool,o.setNumber,p.getNumber,p.getBool
S,C,T,AS,AT,AB,pi,pi2=m.sin,m.cos,m.tan,m.asin,m.atan,m.abs,m.pi,2*m.pi

wType=PN('Weapon Type')//1
params={
{800,0.025,300,0.15},       -- MG
{1000,0.02,300,0.135},      -- LAC
{1000,0.01,300,0.13},       -- RAC
{900,0.005,600,0.125},      -- HAC
{800,0.002,3600,0.12},      -- BC
{700,0.001,3600,0.11},      -- AC
{600,0.0005,3600,0.105},    -- Bertha
}
wType=wType<1 and 1 or wType>#params and #params or wType

gravity=-30
tick=60
deg=1/360

muzzleVel=params[wType][1]
drag=params[wType][2]*tick
lifeSpan=params[wType][3]/tick
windDrag=params[wType][4]

iteration=10
minerr=PN('Minimum Error')
latency=PN('System Latency')

azimuthLimitation=PB('Azi Limitation')
limitation={
PN('Azi Max')*deg,
PN('Azi Min')*deg,
PN('Elv Max')*deg,
PN('Elv Min')*deg
}
rotationCenter=(limitation[1]+limitation[2])/2
ceasefire={
PN('Azi CF Max')*deg,
PN('Azi CF Min')*deg,
PN('Elv CF Max')*deg,
PN('Elv CF Min')*deg
}
ceasefireCenter=(ceasefire[1]+ceasefire[2])/2
lock={
PN('Lock Azi')*deg,
PN('Lock Elv')*deg
}

Vnew=function(x,y,z)
local o={}
o.x=x
o.y=y
o.z=z
return o
end
VAdd=function(a,b)
local o={}
o.x=a.x+b.x
o.y=a.y+b.y
o.z=a.z+b.z
return o
end
VSub=function(a,b)
local o={}
o.x=a.x-b.x
o.y=a.y-b.y
o.z=a.z-b.z
return o
end
VScl=function(a,b)
local o={}
o.x=a*b.x
o.y=a*b.y
o.z=a*b.z
return o
end
QConjugate=function(q)
local o={}
o.x=-q.x
o.y=-q.y
o.z=-q.z
o.w=q.w
return o
end
QMul=function(a,b)
local o,j,k,l,m,n,p,q,r={},a.x,a.y,a.z,a.w,b.x,b.y,b.z,b.w
o.x=m*n-l*p+k*q+j*r
o.y=l*n+m*p-j*q+k*r
o.z=-k*n+j*p+m*q+l*r
o.w=-j*n-k*p-l*q+m*r
return o
end
QMulVec=function(q, v)
local o,N1,N2,N3={},q.x*2,q.y*2,q.z*2
o.x=(((1-(q.y*N2+q.z*N3))*v.x)+((q.x*N2-q.w*N3)*v.y))+((q.x*N3+q.w*N2)*v.z)
o.y=(((q.x*N2+q.w*N3)*v.x)+((1-(q.x*N1+q.z*N3))*v.y))+((q.y*N3-q.w*N1)*v.z)
o.z=(((q.x*N3-q.w*N2)*v.x)+((q.y*N3+q.w*N1)*v.y))+((1-(q.x*N1+q.y*N2))*v.z)
return o
end
QfromEulerZYX=function(e)
local o,cx,sx,cy,sy,cz,sz={},C(0.5*e.x),S(0.5*e.x),C(0.5*e.y),S(0.5*e.y),C(0.5*e.z),S(0.5*e.z)
o.x=sx*cy*cz-cx*sy*sz
o.y=cx*sy*cz+sx*cy*sz
o.z=cx*cy*sz-sx*sy*cz
o.w=sx*sy*sz+cx*cy*cz
return o
end
QtoEulerZXY=function(q)
local sx=2*q.y*q.z+2*q.x*q.w;
local lock=AB(sx)>0.999;
local o={}
if lock then 
o.x=AS(sx)
o.y=0
o.z=AT(2*q.x*q.y+2*q.z*q.w, 2*q.w*q.w+2*q.x*q.x-1)
else
o.x=AS(sx)
o.y=AT(-(2*q.x*q.z-2*q.y*q.w), 2*q.w*q.w+2*q.z*q.z-1)
o.z=AT(-(2*q.x*q.y-2*q.z*q.w), 2*q.w*q.w+2*q.y*q.y-1)
end
return o
end

AngleNormalize=function(a,b,c) return (a%1+b%1-c+2.5)%1+c-0.5 end
Clamp=function(e,max,min) return m.min(m.max(e,min),max) end
nanc=function(a) return a~=a and 0 or a end
dist3=function(o) return m.sqrt(o.x*o.x+o.y*o.y+o.z*o.z) end
dist2=function(x,y) return m.sqrt(x*x+y*y)end

getDisp=function(v,t,a) return t<=0 and 0 or a*t/drag+(-a/drag+v)*(1-m.exp(-drag*t))/drag end
getTime=function(v,x) return -m.log(1-drag*x/v)/drag end

pivotVec={
    Vnew(PN('Pivot 1 X'),PN('Pivot 1 Y'),PN('Pivot 1 Z')),
    Vnew(PN('Pivot 2 X'),PN('Pivot 2 Y'),PN('Pivot 2 Z')),
    Vnew(PN('Muzzle X'),PN('Muzzle Y'),PN('Muzzle Z'))
}
pivotAxis={PN('Pivot 1 Axis'),PN('Pivot 2 Axis')}

lLOS=Vnew(0,0,0)
theta=0

function onTick()
    QBody=QfromEulerZYX(Vnew(nanc(IN(10)),nanc(IN(11)),nanc(IN(12))))
    QTurret=QfromEulerZYX(Vnew(nanc(IN(16)),nanc(IN(17)),nanc(IN(18))))

    EBody=QtoEulerZXY(QBody)
    ETurret=QtoEulerZXY(QTurret)
    turretRotation=(EBody.y-ETurret.y)/pi2
	
    targetPos=Vnew(IN(1),IN(2),IN(3))
    targetVel=Vnew(IN(4),IN(5),IN(6))

    muzzlePos=Vnew(IN(7),IN(8),IN(9))

    bodyVel=QMulVec(QBody,Vnew(IN(13),IN(14),IN(15)))

    -- muzzle correction
    QMuzzle=QfromEulerZYX(Vnew(0,0,0))

    pivot={}
    pivot[1]=turretRotation
    pivot[2]=theta

    for j=1,3 do
        pivotPos=QMulVec(QMul(QBody,QMuzzle),pivotVec[j])
        muzzlePos=VAdd(muzzlePos,pivotPos)

        if #pivot>=j then
            if pivotAxis[j]==1 then
                QArm=QfromEulerZYX(Vnew(pivot[j]*pi2,0,0))
            elseif pivotAxis[j]==2 then
                QArm=QfromEulerZYX(Vnew(0,pivot[j]*pi2,0))
            else
                QArm=QfromEulerZYX(Vnew(0,0,pivot[j]*pi2))
            end
            QMuzzle=QMul(QMuzzle,QArm)
        end
    end

    -- there is a "anisotropic twitst" bug about wind sensor
    windDir=IN(19)*pi2
    windSpeed=IN(20)
    EWind=QtoEulerZXY(QMul(QBody,QfromEulerZYX(Vnew(0,windDir,0))))
    windVel=Vnew(-windSpeed*S(EWind.y)/C(EWind.z)+bodyVel.x,0,-windSpeed*C(EWind.y)/C(EWind.x)+bodyVel.z)

    LOS=VSub(targetPos,muzzlePos)
    nLOS=LOS
    relativeSpeed=(dist3(lLOS)-dist3(LOS))*tick
    lLOS=LOS
    devVec=Vnew(0,0,0)
    ldevVec=Vnew(0,0,0)

    dist=dist3(LOS)
    elv=AT(LOS.y,dist2(LOS.x,LOS.z))
    w,h=dist*C(elv),dist*S(elv)
    theta,t=elv,dist/(muzzleVel+relativeSpeed)

    inRange,inAngle=true,false

    for i=1,iteration do
        if outOfRange then
            t=0
        end
        tl=t+latency/60
        targetDev=VScl(tl,targetVel)
        dragDev=Vnew(getDisp(bodyVel.x,t,0),getDisp(bodyVel.y,t,0),getDisp(bodyVel.z,t,0))
        windDev=Vnew(getDisp(0,t,windDrag*windVel.x),0,getDisp(0,t,windDrag*windVel.z))
        devVec=VSub(targetDev,(VAdd(dragDev,windDev)))
        ldevVec=devVec

        nLOS=VAdd(LOS,devVec)
        dist=dist3(nLOS)
        elv=AT(nLOS.y,dist2(nLOS.x,nLOS.z))
        w,h=dist*C(elv),dist*S(elv)
        vx,vy=muzzleVel*C(theta),muzzleVel*S(theta)
        t=getTime(vx,w)
        if not inRange then
            theta=elv
            break
        end
        if t~=t or t>lifeSpan then
            t=0
            inRange=false
        end

        y=getDisp(vy,t,gravity)
        
        if AB(y-h)<minerr and dist3(VSub(devVec,ldevVec))<minerr then
            break
        end
        theta=theta+elv-AT(y,w)
    end
    
    trigger=IB(1)

    QTarget=QMul(QfromEulerZYX(Vnew(theta,0,0)),QfromEulerZYX(Vnew(0,AT(-nLOS.x,nLOS.z),0)))
    QTarget=QMul(QTarget,QBody)
    ETarget=QtoEulerZXY(QTarget)
    azimuth=-ETarget.y/pi2
    elevation=ETarget.x/pi2

    unlock=IB(2)
    if not unlock then
        azimuth=lock[1]
        elevation=lock[2]
    end
    
    ceasefireAzimuth=AngleNormalize(azimuth,0,ceasefireCenter)
    if ceasefireAzimuth==Clamp(ceasefireAzimuth,ceasefire[1],ceasefire[2]) and elevation==Clamp(elevation,ceasefire[3],ceasefire[4]) then
        inAngle=true
    end
    
    if azimuthLimitation then
        azimuth=AngleNormalize(azimuth,0,rotationCenter)
        azimuth=Clamp(azimuth,limitation[1],limitation[2])
        traverse=azimuth+AngleNormalize(turretRotation,0,-rotationCenter)

        ON(1,-nanc(traverse))
    else
        ON(1,-nanc(AngleNormalize(azimuth,turretRotation,0)))
    end

    elevation=Clamp(elevation,limitation[3],limitation[4])
    ON(2,nanc(elevation*4))
    ON(3,t)
    OB(1,trigger and inRange and inAngle and unlock)
end

計算の過程

まずはクォータニオンとオイラー角の相互変換ライブラリにより、Physics Sensorから入力されたXYZ系のオイラー角から、基部のワールド姿勢の逆行列を取得します。これをローカル系からワールド系への各種補正に用います。

また、Pivot Rotationはマルチプレイでの非同期が指摘されているので、これを全廃するためにPivot基部と端部の姿勢から同じ役目をする数値を取得します。

目標位置と速度、タレットの位置を取得し、タレットのワールド速度ベクトルを計算します。

  QBody=QfromEulerZYX(Vnew(nanc(IN(10)),nanc(IN(11)),nanc(IN(12))))
  QTurret=QfromEulerZYX(Vnew(nanc(IN(16)),nanc(IN(17)),nanc(IN(18))))

  EBody=QtoEulerZXY(QBody)
  ETurret=QtoEulerZXY(QTurret)
  turretRotation=(EBody.y-ETurret.y)/pi2

  targetPos=Vnew(IN(1),IN(2),IN(3))
  targetVel=Vnew(IN(4),IN(5),IN(6))
  muzzlePos=Vnew(IN(7),IN(8),IN(9))

  bodyVel=QMulVec(QBody,Vnew(IN(13),IN(14),IN(15)))

その後、砲口の正確な位置を計算し、弾道計算の起点とします。

  -- muzzle correction
  QMuzzle=QfromEulerZYX(Vnew(0,0,0))

  pivot={}
  pivot[1]=turretRotation
  pivot[2]=theta

  for j=1,3 do
      pivotPos=QMulVec(QMul(QBody,QMuzzle),pivotVec[j])
      muzzlePos=VAdd(muzzlePos,pivotPos)

      if #pivot>=j then
          if pivotAxis[j]==1 then
              QArm=QfromEulerZYX(Vnew(pivot[j]*pi2,0,0))
          elseif pivotAxis[j]==2 then
              QArm=QfromEulerZYX(Vnew(0,pivot[j]*pi2,0))
          else
              QArm=QfromEulerZYX(Vnew(0,0,pivot[j]*pi2))
          end
          QMuzzle=QMul(QMuzzle,QArm)
      end
  end

Wind Sensorからの入力を、ワールド系での風速に変換します。 このとき、Wind Sensorには『機体が傾斜している場合の風向の挙動が南北からの風と東西からの風で異なる』という未知の潜在的なバグがありますが、影響は軽微なので無視しています。

  windDir=IN(19)*pi2
  windSpeed=IN(20)
  EWind=QtoEulerZXY(QMul(QBody,QfromEulerZYX(Vnew(0,windDir,0))))
  windVel=Vnew(-windSpeed*S(EWind.y)/C(EWind.z)+bodyVel.x,0,-windSpeed*C(EWind.y)/C(EWind.x)+bodyVel.z)

砲口位置から目標位置への見越し線(Line of Sight)を計算し、また接近速度も計算しておきます。

その後、ワールド系での見越し線の距離、角度を計算し、弾道計算イテレーションの初期値とします。 仰角の初期値は見越し線と水平面との角度を、到達時間の初期値は距離を初速+接近速度で割った値を利用します。

  LOS=VSub(targetPos,muzzlePos)
  nLOS=LOS
  relativeSpeed=(dist3(lLOS)-dist3(LOS))*tick
  lLOS=LOS
  devVec=Vnew(0,0,0)
  ldevVec=Vnew(0,0,0)

  dist=dist3(LOS)
  elv=AT(LOS.y,dist2(LOS.x,LOS.z))
  w,h=dist*C(elv),dist*S(elv)
  theta,t=elv,dist/(muzzleVel+relativeSpeed)

  inRange,inAngle=true,false

イテレーションを開始します。

水平距離から水平到達時間を計算し、垂直偏差から誤差を計算、その誤差の分だけ次回の仰角を上げることで投射方向を決定しています。

また、同時に自ビークルの速度ベクトルから生じる偏差、風による偏差、目標の移動による偏差を計算し、次回のイテレーションではその分を差し引いた座標を狙います。

本来であれば、垂直偏差もこれらの偏差と同じように計算できるはずなのですが、発散のリスクを低減するために仰角は別個に扱っています。

イテレーションの終了条件は、

  • いつまでも目標の水平位置まで到達しない場合(射程外)
  • 弾体消滅までに目標の水平位置まで到達しない場合(射程外)
  • 弾道計算の垂直偏差と自機速度・風速・目標の移動による偏差がともに0.25m以下になった場合

です。

    for i=1,iteration do
        if outOfRange then
            t=0
        end
        tl=t+latency/60
        targetDev=VScl(tl,targetVel)
        dragDev=Vnew(getDisp(bodyVel.x,t,0),getDisp(bodyVel.y,t,0),getDisp(bodyVel.z,t,0))
        windDev=Vnew(getDisp(0,t,windDrag*windVel.x),0,getDisp(0,t,windDrag*windVel.z))
        devVec=VSub(targetDev,(VAdd(dragDev,windDev)))
        ldevVec=devVec

        nLOS=VAdd(LOS,devVec)
        dist=dist3(nLOS)
        elv=AT(nLOS.y,dist2(nLOS.x,nLOS.z))
        w,h=dist*C(elv),dist*S(elv)
        vx,vy=muzzleVel*C(theta),muzzleVel*S(theta)
        t=getTime(vx,w)
        if not inRange then
            theta=elv
            break
        end
        if t~=t or t>lifeSpan then
            t=0
            inRange=false
        end

        y=getDisp(vy,t,gravity)
        
        if AB(y-h)<minerr and dist3(VSub(devVec,ldevVec))<minerr then
            break
        end
        theta=theta+elv-AT(y,w)
    end

イテレーションが完了したら、指向方位を計算し、ローカル系に直します。 目標が射程外にいる場合は現在の目標位置を追尾するようになっています。

その後、砲塔のアンロックや回転範囲の制限などを計算し、指向すべき方位を出力します。

  trigger=IB(1)

  QTarget=QMul(QfromEulerZYX(Vnew(theta,0,0)),QfromEulerZYX(Vnew(0,AT(-nLOS.x,nLOS.z),0)))
  QTarget=QMul(QTarget,QBody)
  ETarget=QtoEulerZXY(QTarget)
  azimuth=-ETarget.y/pi2
  elevation=ETarget.x/pi2

  unlock=IB(2)
  if not unlock then
      azimuth=lock[1]
      elevation=lock[2]
  end
  
  ceasefireAzimuth=AngleNormalize(azimuth,0,ceasefireCenter)
  if ceasefireAzimuth==Clamp(ceasefireAzimuth,ceasefire[1],ceasefire[2]) and elevation==Clamp(elevation,ceasefire[3],ceasefire[4]) then
      inAngle=true
  end
  
  if azimuthLimitation then
      azimuth=AngleNormalize(azimuth,0,rotationCenter)
      azimuth=Clamp(azimuth,limitation[1],limitation[2])
      traverse=azimuth+AngleNormalize(turretRotation,0,-rotationCenter)

      ON(1,-nanc(traverse))
  else
      ON(1,-nanc(AngleNormalize(azimuth,turretRotation,0)))
  end

  elevation=Clamp(elevation,limitation[3],limitation[4])
  ON(2,nanc(elevation*4))
  ON(3,t)
  OB(1,trigger and inRange and inAngle and unlock)

さいごに

実際のコードは、機能を限定している上、Stormworks側の既知のバグを半ば無視したり、4kBに収めるために可読性を犠牲にしたりしています。

特にコード長の4kB制限は深刻で、現状、極端なminifyを実施しても4049文字/4096文字を使用しており、これ以上の機能追加・遅延低減が困難となっています。

しかし、ShairoさんのFCSではminifyもなしに同じ課題を4096文字以内でクリアしているので、可能だと示されているからにはこれを見習うほかありません。

ライブラリの圧縮やProperty Numberの廃止などを視野に、より御し易いバージョンを制作する予定です。

stormworkslua