marker by rocamisakitohko

《Parallel Kozak II》光学追尾装置

この記事はStormworks 第1 Advent Calendar 2024 25日目の記事です。

光学追尾への挑戦

一般に、Stormworksで物体を追尾し、その位置と運動を計算するためには、レーダーが使われます。

Stormworksにおけるレーダーは、ビークルのPhysics Bodyやチャフ、港に落ちているクレートといった物体を検知し、それら物体までの距離と、レーダーから見た方位を取得することができます。

現行のレーダーには、距離に±1%、方位に0.001[turn]のノイズが掛かっていますが、カルマンフィルタなどのフィルタ技術が広まってきたこともあり、物体追尾・運動計算の精度は近年格段に向上しました。

ですが、埼玉地上軍がここ一年顔を出してきた「センシャドー」のようなマルチ会ではレーダーの搭載・使用が禁止されていることがあります。

また、センサーが地上/空中にあり、目標が海中にある場合、またその逆など、Stormworksの仕様による制限から目標追尾にレーダーを使用できないケースが存在するのも事実です。

そこで、レーダーに依存せず目標の位置と運動を計算するために、レーザーによる目標追尾を実現しました。

理論と着想

《Parallel Kozak II》光学追尾装置は、2基～12基のLaser Distance Sensorを使い、非線形カルマンフィルタを用いて静止・移動目標を追尾する装置です。

カルマンフィルタはノイズのある入力値から確率的に真値を推定する数学的装置で、これをレーザーによる追尾にも応用しました。

レーザーの指向にノイズを加え、目標を包み込むようにレーザーを照射して、目標に命中したであろうレーザーからのみサンプルを得て、カルマンフィルタに入力します。

目標に命中したかどうかの区別は、フィルタの状態からマハラノビス距離を計算することと、命中した面の傾き(=法線)を計算し、水平に近い面は排除することで行っています。

傾きは、レーザーを2基1組とし、ほとんど水平にレーザーを照射、それぞれの命中点の水平距離と垂直距離を出すことで得ています。

命中点の傾きを得ることで、目標と地面とを区別し、地面に引っ張られることなく目標を追尾することが可能となりました。これが事実上の核となる着想だったと言えます。

実装

実装当時、ビークルLuaでは4096文字までのコードしか使えなかったため、カルマンフィルタ本体とレーザーの指向制御を分割せざるを得ませんでした。

以下はフィルタ本体のコードであり、Physics Sensor、コントローラからのBool信号、乱数のシード、そして最大12基のLaser Distance Sensorの出力を受取り、拡張カルマンフィルタによって目標の位置・速度・加速度を推定します。

ParallelKozakII_Core.lua
-- Saitama Army "Parallel Kozak II" Laser Target Tracking Complex

i,o,p,m,t=input,output,property,math,table
IN,OB,ON,PN,TI,TR=i.getNumber,o.setBool,o.setNumber,p.getNumber,t.insert,t.remove
S,C,T,AS,AT,AB,SQ,pi,pi2=m.sin,m.cos,m.tan,m.asin,m.atan,m.abs,m.sqrt,m.pi,2*m.pi

function multiply(A,B,o,l)
    if #A[1]~=#B then return nil end
    o={}
    for i=1,#A do
        o[i]={}
    end
    for i=1,#A do
        for j=1,#B[1] do
            l=0
            for k=1,#B do
                l=l+A[i][k]*B[k][j]
            end
            o[i][j]=l
        end
    end
    return o
end

function add(A,B,o)
    o={}
    for i=1,#A do
        o[i]={}
        for j=1,#A[1] do
            o[i][j]=A[i][j]+B[i][j]
        end
    end
    return o
end

function scl(a,B,o)
    o={}
    for i=1,#B do
        o[i]={}
        for j=1,#B[1] do
            o[i][j]=a*B[i][j]
        end
    end
    return o
end

function transpose(A,o)
    o={}
    for i=1,#A[1] do
        o[i]={}
        for j=1,#A do
            o[i][j]=A[j][i]
        end
    end
    return o
end

function inverse(A,o,e,m,n)
    if #A~=#A[1] then return nil end
    o,e=identity(#A),#A
    for i=1,e do
        m=A[i][i]
        for j=1,e do
            A[i][j]=A[i][j]/m
            o[i][j]=o[i][j]/m
        end
        for k=1,e do
            if i~=k then
                n=A[k][i]
                for j=1,e do
                    A[k][j]=A[k][j]-n*A[i][j]
                    o[k][j]=o[k][j]-n*o[i][j]
                end
            end
        end
    end
    return o
end

function identity(n,o)
    o={}
    for i=1,n do
        o[i]={}
        for j=1,n do
            o[i][j]=(i==j) and 1 or 0
        end
    end
    return o
end

function convolution(A,o)
    o={}
    for i=1,9 do
        o[i]={}
        for j=1,9 do
            o[i][j]=0
        end
    end
    for i=0,2 do
        for j=1,3 do
            for k=1,3 do
                o[j+i*3][k+i*3]=A[j][k]
            end
        end
    end
    return o
end

function rotationXYZ(e)
local cx,sx,cy,sy,cz,sz=C(-e[1]),S(-e[1]),C(-e[2]),S(-e[2]),C(-e[3]),S(-e[3])
return{{cy*cz,sx*sy*cz+cx*sz,-cx*sy*cz+sx*sz},
{-cy*sz,-sx*sy*sz+cx*cz,cx*sy*sz+sx*cz},
{sy,-sx*cy,cx*cy}}
end

function localfromRadar(o)
local d,a,e=o[1],o[2],o[3]
w=d*C(e)
return {w*S(a),d*S(e),w*C(a)}
end

function localtoWorld(o,r)
return transpose(multiply(rotationXYZ(r),transpose({o})))
end

function angleNormalize(a)
    return(a%pi2+pi2*2.5)%pi2-pi
end

function BoxMuller(x,y)
    return SQ(-2*m.log(x))*C(y*pi2)
end

function LCG(x,a,c,m)
    a,c,m=1664525,1013904223,0x7FFFFFFF
    x=(x*a+c)&m
    return x,x/m
end
-- EKF

function Predict(x,P,Q,F,xp,pp)
    xp=multiply(F,x)
    pp=add(multiply(multiply(F,P),transpose(F)),Q)
    return xp,pp
end

function Update(xp,pp,R,z,pos,jH,y,K,x,P,IKH)
    jH=jhx(xp,pos)
    y=add({z},scl(-1,{hx(xp,pos)}))
    y[1][2]=angleNormalize(y[1][2])
    y[1][3]=angleNormalize(y[1][3])

    K=multiply(multiply(pp,transpose(jH)),inverse(add(multiply(multiply(jH,pp),transpose(jH)),R)))

    x=add(xp,multiply(K,transpose(y)))
    IKH=add(identity(n),scl(-1,multiply(K,jH)))
    P=add(multiply(multiply(IKH,pp),transpose(IKH)),multiply(multiply(K,R),transpose(K)))
    return x,P
end

function jhx(xp,P)
    local x,y,z,p,q,r=xp[1][1]-P[1],xp[4][1]-P[2],xp[7][1]-P[3]
    p=x^2+y^2+z^2
    q=SQ(p)
    return {
        {x/q,0,0,y/q,0,0,z/q,0,0}, 
        {1/(z*(x^2/z^2 + 1)),0,0,0,0,0,-x/(z^2*(x^2/z^2 + 1)),0,0},
        {-x*y/(SQ(-y^2/(p)+1)*(p)^(3/2)),0,0,(-y^2/(p)^(3/2)+1/q)/SQ(-y^2/(p)+1),0,0,-y*z/(SQ(-y^2/(p)+1)*(p)^(3/2)),0,0}
    }
end

function hx(xp,P)
    local l,x,y,z,d,a,e={xp[1][1]-P[1],xp[4][1]-P[2],xp[7][1]-P[3]}
    x,y,z=l[1],l[2],l[3]
    d=SQ(x^2+y^2+z^2)
    a=AT(x,z)
    e=AS(y/d)
    return {d,a,e}
end

function mahalanobissq(z,x,P,y,m)
    y=add(z,scl(-1,transpose({{x[1][1],0,0,x[4][1],0,0,x[7][1],0,0}})))
    m=multiply(multiply(transpose(y),inverse(scl(1,P))),y)
    return m[1][1]
end

function NaNCheck(x) return x~=x and 0 or x end

dt=1/60

n=9
transition3x3={
    {1,dt,dt^2/2},
    {0,1,dt},
    {0,0,0.95}
}
transition9x9=convolution(transition3x3)
sigma2Var=PN('Sigma2')
discreteWhiteNoise={
    {dt^5/20,dt^4/8,dt^3/6},
    {dt^4/8,dt^3/3,dt^2/2},
    {dt^3/6,dt^2/2,dt}
}
vecQ=scl(sigma2Var,convolution(discreteWhiteNoise))
lDetected=false

mahaGate=PN('Maha Gate')
delay=5
B={}
noiseArray={}

function onTick()
    pos={IN(1),IN(2),IN(3)}
    rot={IN(4),NaNCheck(IN(5)),IN(6)}

	int=string.unpack("I4",string.pack("f",IN(13)))
	for i=0,31 do B[1+i]=(int&(1<<i))>0 end
    ts=(int>>24)&255
    targetSize=ts/2.0
    ON(11,targetSize)
    

    if B[5] and not B[13] then
        -- laser noise
        seedf=IN(14)
        seed=string.unpack("I4",string.pack("f",seedf))
        noise={}
        for i=1,24 do
            seed,out=LCG(seed)
            TI(noise,out)
        end
        LOSDist=vecOut and hx(vecOut,pos)[1] or 100
        noise.s=AT(targetSize/m.max((LOSDist),100))
        TI(noiseArray,1,noise)
        
        ON(14,IN(14))
        ON(18,noise.s)


        if #noiseArray>delay then
            if not vecx then
                -- init
                -- need to delay
                raw={IN(15),IN(16)/8*pi2,IN(17)/8*pi2}
                l=localfromRadar(raw)
                l=localtoWorld(l,rot)
                lw=add(l,{pos})
                lw={lw[1][1],0,0,lw[1][2],0,0,lw[1][3],0,0}
                lw=transpose({lw})
                vecOut=lw
                
                if B[12] then
                    vecx=lw
                    vecP=scl(5,identity(9))
                end
            else
                -- predict
                lvecx,lvecP=vecx,vecP
                vecx,vecP=Predict(vecx,vecP,vecQ,transition9x9)

                -- calculate R
                standardx,standardy=IN(19),IN(20)
                deltay=AS((0.5-0.01)/LOSDist)/pi2

                hit=0
                for i=1,6 do
                    noise=noiseArray[delay+1]
                    worldPos,raw,l={},{},{}
                    for j=1,2 do
                        raw={IN(i*2+j+18),IN(19)+BoxMuller(noise[i*4-3],noise[i*4-2])*noise.s,IN(20)+BoxMuller(noise[i*4-1],noise[i*4])*noise.s/3-(j-1)*deltay}
                        l=localfromRadar(raw)
                        l=localtoWorld(l,rot)            
                        l=add(add(l,{{i<4 and -0.25*(4-i) or 0.25*(i-3),0.5*j-0.75,0.75}}),{pos})
                        worldPos[j]=l
                    end
                    lnew=transpose({{l[1][1],0,0,l[1][2],0,0,l[1][3],0,0}})
                    rotraw=hx(lnew,pos)
                    
                    -- gate function
                    -- maha distance
                    mahaDist=SQ(mahalanobissq(lnew,lvecx,lvecP))

                    -- pseudonormal
                    worldPosSub=add(worldPos[1],scl(-1,worldPos[2]))
                    pseudonormal=AT(worldPosSub[1][2],SQ(worldPosSub[1][1]*worldPosSub[1][1]+worldPosSub[1][3]*worldPosSub[1][3]))/pi2
                
                    vecR=scl(((noise.s)^2),identity(3))
                    vecR[1][1]=(LOSDist*0.02)^2/12
                    vecR[3][3]=vecR[2][2]/3

                    if raw[1]<4000 and mahaDist<mahaGate and AB(pseudonormal)>0.15 then
                        vecx,vecP=Update(vecx,vecP,vecR,rotraw,pos)
                        hit=hit+1
                    end
                end
                ON(13,hit)
                vecOut=vecx
            end
        end
    else
        noiseArray={}
        vecx,vecP,lvecx,lvecP=nil,nil,nil,nil
    end

    for i=1,12 do
        ON(i+20,IN(i))
    end

    if vecOut then
        for i=1,3 do
            for j=1,3 do
                ON((i-1)*3+j,vecOut[i+(j-1)*3][1])
            end
        end
    end
end

また、下記はレーザー指向部のLuaです。

ParallelKozakII_pivot.lua
-- Saitama Army "Parallel Kozak" Laser Target Tracking Complex

i,o,p,m,t=input,output,property,math,table
IB,IN,OB,ON,PN,PB,TI,TR,TP,TU=i.getBool,i.getNumber,o.setBool,o.setNumber,p.getNumber,p.getBool,t.insert,t.remove,t.pack,t.unpack
S,C,T,AS,AT,AB,SQ,pi,pi2=m.sin,m.cos,m.tan,m.asin,m.atan,m.abs,m.sqrt,m.pi,2*m.pi

function generateMatrix(n,m,o)
    o={}
    for i=1,n do
        o[i]={}
        for j=1,m do
            o[i][j]=0
        end
    end
    return o
end

function multiply(A,B,o,l)
    if #A[1]~=#B then return nil end
    o={}
    for i=1,#A do
        o[i]={}
    end
    for i=1,#A do
        for j=1,#B[1] do
            l=0
            for k=1,#B do
                l=l+A[i][k]*B[k][j]
            end
            o[i][j]=l
        end
    end
    return o
end

function add(A,B,o)
    o={}
    for i=1,#A do
        o[i]={}
        for j=1,#A[1] do
            o[i][j]=A[i][j]+B[i][j]
        end
    end
    return o
end

function subtract(A,B,o)
    o={}
    for i=1,#A do
        o[i]={}
        for j=1,#A[1] do
            o[i][j]=A[i][j]-B[i][j]
        end
    end
    return o
end

function scl(a,B,o)
    o={}
    for i=1,#B do
        o[i]={}
        for j=1,#B[1] do
            o[i][j]=a*B[i][j]
        end
    end
    return o
end

function transpose(A,o)
    o={}
    for i=1,#A[1] do
        o[i]={}
        for j=1,#A do
            o[i][j]=A[j][i]
        end
    end
    return o
end

function inverse(A,o,e,m,n)
    if #A~=#A[1] then return nil end
    o,e=identity(#A),#A
    for i=1,e do
        m=A[i][i]
        for j=1,e do
            A[i][j]=A[i][j]/m
            o[i][j]=o[i][j]/m
        end
        for k=1,e do
            if i~=k then
                n=A[k][i]
                for j=1,e do
                    A[k][j]=A[k][j]-n*A[i][j]
                    o[k][j]=o[k][j]-n*o[i][j]
                end
            end
        end
    end
    return o
end

function identity(n,o)
    o={}
    for i=1,n do
        o[i]={}
        for j=1,n do
            o[i][j]=(i==j) and 1 or 0
        end
    end
    return o
end

function cross(A,B)
    return {A[2]*B[3]-A[3]*B[2],A[3]*B[1]-A[1]*B[3],A[1]*B[2]-A[2]*B[1]}
end

function rotationX(r)
    return {{1,0,0},{0,C(r),-S(r)},{0,S(r),C(r)}}
end
function rotationY(r)
    return {{C(r),0,S(r)},{0,1,0},{-S(r),0,C(r)}}
end
function rotationZ(r)
    return {{C(r),-S(r),0},{S(r),C(r),0},{0,0,1}}
end

function rotationXYZ(e)
    return multiply(multiply(rotationX(-e[1]),rotationY(-e[2])),rotationZ(-e[3]))
end

function localfromRadar(o)
local d,a,e=o[1],o[2],o[3]
local w=d*C(e)
return {w*S(a),d*S(e),w*C(a)}
end

function angleNormalize(a)
    return(a%pi2+pi2*2.5)%pi2-pi
end

function BoxMuller(x,y)
    return SQ(-2*m.log(x))*C(y*pi2)
end

function magnitude(x,y,z)
    return SQ(x^2+y^2+z^2)
end

function LCG(x,a,c,m)
    a,c,m=1664525,1013904223,2147483647
    x=(x*a+c)&m
    return x,x/m
end

function hx(xp,P)
    local l,x,y,z,d,a,e={xp[1][1]-P[1],xp[4][1]-P[2],xp[7][1]-P[3]}
    x,y,z=l[1],l[2],l[3]
    d=magnitude(x,y,z)
    a=AT(x,z)
    e=AS(y/d)
    return {d,a,e}
end

function NaNCheck(x) return x~=x and 0 or x end

delay=5
langvel={0,0,0}


function onTick()
    pos={IN(21),IN(22),IN(23)}
    rot={IN(24),NaNCheck(IN(25)),IN(26)}
    vel={IN(27),IN(28),IN(29)}
    vel=transpose(multiply(transpose(rotationXYZ(rot)),transpose({vel})))
    angvel={NaNCheck(IN(30)*pi2),NaNCheck(IN(31)*pi2),NaNCheck(IN(32)*pi2)}

    targetPos={{IN(1),IN(2),IN(3)}}
    targetVel={{IN(4),IN(5),IN(6)}}

    targetPos=add(targetPos,scl(delay/60,targetVel))
    targetPos=add(targetPos,scl(-delay/60,vel))

    localPos=subtract(targetPos,{pos})

    
    angvelvel=cross(angvel,TU(localPos))
    angveldot=multiply({angvel},transpose({langvel}))
    angvelgain=m.min(m.max(
        angveldot[1][1]
        /magnitude(angvel[1],angvel[2],angvel[3])
        /magnitude(langvel[1],langvel[2],langvel[3])
        ,0.1),1)
    localPos=add(localPos,scl(1*-delay/60,{angvelvel}))
    

    localPos=multiply(rotationXYZ(rot),transpose(localPos))

    seed=string.unpack("I4",string.pack("f",IN(14)))
    noise={}
    for i=1,24 do
        seed,out=LCG(seed)
        TI(noise,out)
    end
    noise.s=IN(18)
    for i=0,6 do
        if i>0 then
            standardPos=transpose({{localPos[1][1],0,0,localPos[2][1],0,0,localPos[3][1],0,0}})
            localDAE=hx(standardPos,{0,0,0})
            ON(19+i*2,localDAE[2]+BoxMuller(noise[i*4-3],noise[i*4-2])*noise.s)
            ON(20+i*2,localDAE[3]+BoxMuller(noise[i*4-1],noise[i*4])*noise.s/3)
        else
            standardPos=transpose({{localPos[1][1],0,0,localPos[2][1],0,0,localPos[3][1],0,0}})
            localDAE=hx(standardPos,{0,0,0})
            ON(19+i*2,localDAE[2])
            ON(20+i*2,localDAE[3])
        end
        
    end

    ON(18,localDAE[1])

    langvel=angvel
end

性能

開発当初の要求性能は、

  • 目標距離100m～4000mの範囲において、
  • 20m/sの速度を発揮する、
  • 全長5m～10m程度の車両を、
  • 砲熕兵器の照準・ミサイルの誘導が可能な程度の正確さで、

追尾することでした。

では、チューン中の映像をご覧ください。目標はShairoさんが制作したチャレンジャー2、車体長8m程度で時速70kmを発揮します。

ご覧の通り、旋回・加減速をものともせず追尾し、少し不安定になっても再捕捉できていることがわかります。

また、車体が少々隠れてたとしても追尾が継続し、完全に隠れてしまったとしても位置を推定し続けます。

課題

そんな、レーダーの使えない状況でレーダーと同程度・それ以上に正確な目標追尾を提供してくれる《Parallel Kozak II》にも弱点があります。

まず、レーザーを用いているため遮蔽に弱く、目標が体を完全に隠してしまえば当然失探します。

失探してもこれまでの位置・速度などの情報からしばらくの間は現在位置を推定し続け、目標が体を出せば再捕捉することもありますが、これはあくまで副次的な機能です。

また、目標が急激な加減速・方向転換を行えば、フィルタの推定する現在位置と実際の位置が大きくズレます。

レーザーは推定位置に向けて照射されますから、結果としてレーザーが目標に命中せず、そうしてサンプルが入ってこなくなればそのまま失探するでしょう。

そして、目標と地面との区別を命中した面の傾斜を用いて行っている都合上、戦車の天板のように平たく地面に対して寝そべった面からはサンプルを得ることができません。

これによる悪影響は目標が坂を登っている・下っているときに顕著に現れます。車両の前面・側面からしかサンプルを取得できないので有効サンプル数が低下し、また推定位置もドリフトするでしょう。

より急な坂道では前面・側面すらも傾き、地面に対して垂直でなくなることにより、目標から有効なサンプルを取得できなくなります。

ここまでは追尾方式そのものに潜む問題ですが、兵装システムとして見たときの運用上の課題もあるでしょう。

目標位置の初期化を視線照準システムとレーザーによって行っている都合上、マルチ会のTPSと接続状況によっては初期化することすら困難なのです。

これはレーザーベースの目標捜索・捕捉システムを構築することによってのみ解決するでしょう。

stormworkslualaser