23102計画型捜索レーダー《Zvezdy》

2023/12/01

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

捜索レーダーを作りました！

埼玉地上軍では、以前制作した9K330 トールの全面的改良、またZSU-23-4 シルカや2K22 ツングースカといった防空車両の新造を進めています。

しかしその過程で、以前のレーダーシステムやレーダーの配置・駆動方式を見直す必要が出てきました。

宇宙DLCのリリースに前後して、ミサイルやロケットといった、ビークル本体から分離可能なPhysics Bodyが別個のビークルとして扱われるようになりました。これにより、防空車両に搭載されたミサイルが車両自身のレーダーで検知されたり、車両自身のレーダーを遮るようになったのです。

特に、Physics Bodyやプレイヤー、NPCなどがそれより奥の物体の検出を遮ってしまう"遮蔽効果"は深刻で、レーダーの配置を根本的に変更し、また駆動方式も変更する必要が生じたので、捜索レーダー・レーダーマップのコードを最新の知見に基づいてリファクタリングするとともに、別途捜索レーダーの駆動を制御することとしました。

23102計画型捜索レーダー《Zvezdy》

《Zvezdy》捜索レーダーは、23102型レーダー装置の中核をなす捜索レーダーです。多様な地上車両に搭載すべく、1基のレーダーで全周を監視することを目指しました。

Stormworksの新型レーダーは、プロパティから適切なモードを選択することで、レーダー自身の姿勢を変えずとも指向方位を変化させることができます。 Clockwise・Anticlockwiseモードではそれぞれ時計回り・反時計回りに指向し、Sweepモードでは指定した角度で首振り様に指向することができるのですが、回転速度には限界があり(XML編集を許せば際限なく引き上げることができますが)、また回転速度をスポーン後に変更することができません。

そこで、さらなる走査速度を求め、レーダーのモードはStaticのまま、レーダーそのものをPivotで高速回転させることで全周捜索を実現することがあります。これを個人的にDynamicモードと呼んでおり、回転速度を柔軟に変更したり、ZSU-23-4のように追尾レーダーに捜索レーダーを兼任させることが可能です。

《Zvezdy》捜索レーダーでは、レーダーの駆動モードを問わずレーダーを運用できるよう工夫しているほか、Dynamic捜索レーダーを駆動させるための制御マイコンも並行して開発しました。

レーダーマイコンの概略

レーダー本体のマイコンはこのようになっています。

まず、レーダーから入力されたコンポジット信号の8,12,16,20,24,28Chを、Physics Sensorで取得したレーダーの位置と姿勢で上書きし、LuaブロックAに入力します。

LuaブロックAは、レーダーが得た目標距離・方位、Physics Sensorで得たレーダーの位置・姿勢を入力すると、目標のワールド座標を出力します。

その後、余ったChをレーダー自身の位置や方位、タッチパネルの情報、追尾レーダーからの入力などで改めて上書きし、LuaブロックBに入力します。

LuaブロックBは、マップ・目標位置を描画するほか、追尾レーダーへのハンドオーバーも担当しています。

Luaブロック A

LuaブロックAでは、オイラー角とクォータニオンの相互変換ライブラリを利用して、レーダーが得た目標距離・方位からワールド座標を計算しています。

Stormworksの新型レーダーは目標との位置関係を必ずしも毎tick計算しているわけではなく、その代わりにある間隔で距離・方位の真値を計算し、次の検出間隔までは同じ真値・異なるノイズで距離・方位を出力しています。検出間隔(tick)は有効レンジ(m)を2000で割り、切り上げた値になります。

そのため、レーダーの出力が同じ真値を共有している間はレーダーの姿勢を更新せず、真値が更新されたときにだけPhysics Sensorから姿勢を計算するようにしています。

また、お祈り程度のノイズ対策として、同じ真値を共有している出力をワールド座標系で平均しています。

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

Zvezdy_A.lua

-- Saitama Army project 23102 "Zvezdy" Radar Complex

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

interval=PN('Interval')
interval=interval<1 and 1 or interval

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
function LocalfromRadar(d,a,e)
local y,w=d*S(e),d*C(e)
local x,z=w*S(a),w*C(a)
return Vnew(x,y,z)
end

RadarPos={}
RadarQ={}
Targets={}
Detected={}

Offset=Vnew(0,0,0)

function onTick()
    timeSinceDetection=IN(4)
    if timeSinceDetection==0 then
        Targets={}
        Detected={}
        for i=1,8 do
            Targets[i]={}
            OB(i,false)
            ON(3*i-2,0)
            ON(3*i-1,0)
            ON(3*i,0)
        end
        RadarPos=Vnew(IN(8),IN(12),IN(16))
        RadarQ=QfromEulerZYX(Vnew(IN(20),IN(24),IN(28)))            
    end

    for i=1,8 do
        detected=IB(i)
        Detected[i]=detected
        if detected then
            localPos=LocalfromRadar(IN(4*i-3),IN(4*i-2),IN(4*i-1))
            localPos=VAdd(localPos,Offset)
            AAPos=QMulVec(RadarQ,localPos)
            worldPos=VAdd(AAPos,RadarPos)
            obj={}
            obj.x=worldPos.x
            obj.y=worldPos.y
            obj.z=worldPos.z
            Targets[i][timeSinceDetection+1]=obj
        end
    end

    if timeSinceDetection==interval-1 then
        for i=1,8 do
            x,y,z=0,0,0
            if Detected[i] then
                for j=1,interval do
                    obj=Targets[i][j]
                    x,y,z=x+obj.x,y+obj.y,z+obj.z
                end
                x,y,z=x/interval,y/interval,z/interval
            end
            OB(i,Detected[i])
            ON(3*i-2,x)
            ON(3*i-1,y)
            ON(3*i,z)
        end
    end
end

Luaブロック B

LuaブロックBは、ワールド座標系で入力された目標位置を保持し、マップと重ね合わせてディスプレイに表示しています。それに留まらず、ディスプレイ上に表示された輝点をタッチすることで、追尾レーダーへ目標位置を送信することができます。

地図上に表示された目標位置をタップすることで追尾レーダーにシームレスに目標情報をハンドオーバーできるのが、以前制作した9K330 トールや現在制作中の23102計画型レーダー装置の強みです。

もちろん、通信上の都合でプレイヤーの操作が同期されない場合に備え、23102計画型追尾レーダー《Sudno》には、プレイヤーがレーダーの指向方位・捕捉状況を直接操作できるモードも用意されています。

Zvezdy_B.lua

-- Saitama Navy project 23102 "Zvezdy" Radar Complex

i,o,p,m,t,s=input,output,property,math,table,screen
IB,IN,OB,ON,PB,PN=i.getBool,i.getNumber,o.setBool,o.setNumber,p.getBool,p.getNumber
S,C,T,AS,AT,AB=m.sin,m.cos,m.tan,m.asin,m.atan,m.abs
pi,pi2=m.pi,2*m.pi
TI,TR=t.insert,t.remove
Color,Text,Line,Rect,RectF,Circle,CircleF,Tri,TriF=s.setColor,s.drawText,s.drawLine,s.drawRect,s.drawRectF,s.drawCircle,s.drawCircleF,s.drawTriangle,s.drawTriangleF

w,h=0,0
centerX,centerY=0,0
touchX,touchY=0,0
GPSX,GPSY,bodyC,radarC=0,0,0
zoom=5
fix,activated,lPressed=true,false,false


Targets={}

zone=32
tgmax=100
tgMinDist=25

searchRange=PN('Search Radar Range')
missileRange=PN('Missile Range')
gunRange=PN('Gun Range')

function Dist3f(x,y,z) return x*x+y*y+z*z end
function Cursor(lx,ly,ux,uy) return touchX>lx and touchX<ux and touchY>ly and touchY<uy end

function onTick()
	activated=IB(9)
	pressed=IB(10)
    GPSX,GPSY=IN(25),IN(26)
	bodyC,radarC=IN(27)*pi2,IN(28)*pi2
	touchX,touchY=IN(31),IN(32)

	OB(1,false)

	if pressed then
		if not lPressed then
			if Cursor(1,h-9,8,h-1) then
				fix=true
			elseif Cursor(1,1,8,8) then
				zoom=m.max(zoom/1.5,0.1)
			elseif Cursor(1,9,8,16) then
				zoom=m.min(zoom*1.5,50)
			else
				for k=1,#Displayed do
					displayed=Displayed[k]
					if Cursor(displayed.x-3,displayed.y-3,displayed.x+3,displayed.y+3) then
						ON(1,Targets[displayed.i].x)
						ON(2,Targets[displayed.i].y)
						ON(3,Targets[displayed.i].z)
						OB(1,true)
					end
				end
			end
		elseif Cursor(w/2-zone,h/2-zone,w/2+zone,h/2+zone) then
			fix=false
			centerX,centerY=centerX+(touchX-w/2)*zoom*zone/w,centerY+(h/2-touchY)*zoom*zone/h
		end
	end

	if activated then
		for i=1,8 do
			if IB(i) and #Targets<tgmax then
				x,y,z=IN(3*i-2),IN(3*i-1),IN(3*i)
				index=0
				minDist=tgMinDist
				isNew=true
				for j=1,#Targets do
					target=Targets[j]
					dist=Dist3f(target.x-x,target.y-y,target.z-z)
					if dist<minDist then
						index=j
						minDist=dist
						isNew=false
					end
				end
				obj={}
				obj.x,obj.y,obj.z,obj.t=x,y,z,70
				if isNew then
					TI(Targets,obj)
				else
					Targets[index]=obj
				end
			else
				break
			end
		end
		for j=#Targets,1,-1 do
			t=Targets[j].t
			Targets[j].t=t-1
			if t<1 then TR(Targets,j) end
		end
	end

	if fix then centerX,centerY=GPSX,GPSY end

	lPressed=pressed
end

function onDraw()
    w = s.getWidth()
	h = s.getHeight()

	-- draw map
	s.setMapColorOcean(5,5,5)
	s.setMapColorShallows(10,10,10)
	s.setMapColorLand(30,30,30)
	s.setMapColorGrass(20,20,20)
	s.setMapColorSand(40,40,40)
	s.setMapColorSnow(50,50,50)
    s.drawMap(centerX,centerY,zoom)

    vX,vY=map.mapToScreen(centerX,centerY,zoom,w,h,GPSX,GPSY)
    Displayed={}

	if activated then
		-- render radar range
		Color(0,0,255,15)
		searchCircle=searchRange*w/(zoom*1000)
		CircleF(vX,vY,searchCircle)
		Color(0,0,255)
		Circle(vX,vY,searchCircle)
		Line(vX,vY,searchCircle*S(radarC+pi)+vX,searchCircle*C(radarC+pi)+vY)

		for i=1,#Targets do
			target=Targets[i]
			if target.y >10 then
				Color(0,255,0,m.min(target.t*3,255))
			else
				Color(0,255,255,m.min(target.t*3,255))
			end
			targetX,targetY=map.mapToScreen(centerX,centerY,zoom,w,h,target.x,target.z)
			if targetX>0 and targetX<w and targetY>0 and targetY<h then
				obj={}
				obj.x,obj.y,obj.i=targetX,targetY,i

				TI(Displayed,obj)
				Text(targetX-2,targetY-2,"+")
			end
		end
	end

	Color(255,0,0)
	-- render missile range
	missileCircle=missileRange*w/(zoom*1000)
	Circle(vX,vY,missileCircle)
	gunCircle=gunRange*w/(zoom*1000)
	Circle(vX,vY,gunCircle)

	-- render vehicle position
	CircleF(m.max(m.min(vX,w-2),2),m.max(m.min(vY,h-2),2),2)
	TriF(vX,vY,6*S(bodyC+pi2/6)+vX,6*C(bodyC+pi2/6)+vY,6*S(bodyC+pi)+vX,6*C(bodyC+pi)+vY)
	TriF(vX,vY,6*S(bodyC-pi2/6)+vX,6*C(bodyC-pi2/6)+vY,6*S(bodyC-pi)+vX,6*C(bodyC-pi)+vY)

	-- render button
	Color(0,0,0,127)
	RectF(1,1,7,7)
	RectF(1,9,7,7)
	RectF(1,h-8,7,7)
	Color(255,255,255)
	Text(3,2,"+")
	Text(3,10,"-")
	Text(3,h-6,"|")
end

駆動制御マイコンの概略

現在制作中の9K330 トール、2K22 ツングースカでは、捜索レーダーの指向に先述のDynamic式を採用しています。

この方式でレーダーの回転速度を制御するために、駆動制御マイコンを新たに開発しました。

LuaブロックでVelocity Pivotの回転角度と、端部のローカル角速度を計算してから、角速度をPIDに突っ込み、一定の角速度に管理しています。

設定項目は以下の通りです。

走査すべき区間の数

通常、これはレーダーのFOV Xの逆数か、それより少し大きい値にします。

レーダーの検出間隔

先述の通り、レーダーレンジ(m)を2000で割った値です。

Oddscan

このうちOddscanは、レーダーを倍速・3倍速で回転させ、走査速度を向上させるための仕組みです。区間数は奇数にするべきです。

Oddscanはレーダーの回転速度が向上するほか、同じ目標を連続して捉えてしまうことが減りますが、高速な目標がレーダーに映らなくなりやすく、一長一短といえます。

Oddscanのイメージ図

Luaブロック C

LuaブロックCの役目は、Velocity Pivotの基部と端部の姿勢を計算し、Ch1に角度の差を、Ch2にPivot端部のローカル角速度を出力することです。 Ch1に出力される数値は、Velocity PivotのCurrent Rotationノードを使って計算した場合と同じになるようになっています。

使用しているコードを掲示します。ライブラリはブロックAと共通です。

Zvezdy_C.lua

AngleNormalize=function(a,b) return (a%1-b%1+2.5)%1-0.5 end

function onTick()
    QRadar=QfromEulerZYX(Vnew(IN(1),IN(2),IN(3)))
    QBase=QfromEulerZYX(Vnew(IN(7),IN(8),IN(9)))
    ERadar=QtoEulerZXY(QRadar)
    EBase=QtoEulerZXY(QBase)
   

    AngVelRadar=Vnew(IN(4),IN(5),IN(6))
    AngVelRadar=QMulVec(QConjugate(QRadar),AngVelRadar)

    difference=AngleNormalize(ERadar.y/pi2,(EBase.y/pi2+0.5))*-5

    ON(1,difference)
    ON(2,AngVelRadar.y)
end

stormworksluaradar