top of page

ミサイル

20190526170214_1.jpg

    1|   string status = "Ready";
    2|   double remaining_time;
    3|   double remaining_time2;
    4|   int count;
    5|   int BootOffset;
    6|   int BootWait;
    7|   double kp = 0;
    8|   double kd = 0;
    9|   double prevY = 0;
  10|   double prevP = 0;
  11|   double EpL = 0;
  12|   double EpD = 0;
  13|   double EdL = 0;
  14|   double EdD = 0;
  15|   float Dist_Det = 0;
  16|   Vector3D targetGPS;
  17|   Vector3D targetVelocity;
  18|   Vector3D DestGPS;
  19|   IMyShipController rc;
  20|   List<IMyGyro> gyros = new List<IMyGyro>();
  21|   List<IMyWarhead> warheads = new List<IMyWarhead>();
  22|   List<IMyThrust> thrusters = new List<IMyThrust>();            
  23|   
  24|   public Program()
  25|   {
  26|       rc = GridTerminalSystem.GetBlockWithName("Remote Control") as IMyShipController;
  27|   }
  28|   
  29|   public void Main(string argument, UpdateType updateSource)
  30|   {
  31|       var com = argument.Split(',');
  32|       
  33|       switch (com[0]){
  34|           case "MissileLaunch":
  35|               IMyShipMergeBlock mb= GridTerminalSystem.GetBlockWithName("MBStation") as IMyShipMergeBlock;
  36|               var para = mb.CustomData.Split(',');
  37|               BootOffset = int.Parse(para[0]);
  38|               BootWait = int.Parse(para[1]);
  39|               kp = double.Parse(para[2]);
  40|               kd = double.Parse(para[3]);
  41|               Dist_Det = float.Parse(para[4]);
  42|               status = "Launch";
  43|               count = 0;
  44|               Runtime.UpdateFrequency = UpdateFrequency.Update1;
  45|               break;
  46|           case "Target":
  47|               if (status != "Ready"){
  48|                   targetGPS = new Vector3D(float.Parse(com[1]), float.Parse(com[2]), float.Parse(com[3]));
  49|                   targetVelocity = new Vector3D(float.Parse(com[4]), float.Parse(com[5]), float.Parse(com[6]));
  50|                   SetTarget();
  51|               }
  52|               break;
  53|           case "Detonate":
  54|               if (status == "Tracking" || status == "Tracking2"){
  55|                   foreach (var warhead in warheads)
  56|                       warhead.ApplyAction("Detonate");
  57|               }
  58|               break;
  59|       }
  60|               
  61|       switch (status){
  62|           case "Launch":
  63|               IMyShipMergeBlock mb = GridTerminalSystem.GetBlockWithName("MBMissile") as IMyShipMergeBlock;
  64|               IMyPistonBase pis = GridTerminalSystem.GetBlockWithName("PJ Piston") as IMyPistonBase;
  65|               GridTerminalSystem.GetBlocksOfType(gyros);
  66|               GridTerminalSystem.GetBlocksOfType(thrusters);
  67|               foreach(var thruster in thrusters)
  68|                   thruster.SetValue("Override", 345600f);            
  69|               pis.Retract();
  70|               mb.ApplyAction("OnOff_Off");
  71|               status = "Launching";
  72|               break;
  73|           case "Launching":
  74|               count++;
  75|               if (count > BootOffset){
  76|                   status = "Stop";
  77|                   count = 0;
  78|                   GridTerminalSystem.GetBlocksOfType(warheads);
  79|                   foreach (var warhead in warheads)
  80|                       warhead.ApplyAction("Safety");
  81|               }
  82|               break;
  83|           case "Stop":
  84|               Operation(-rc.GetShipVelocities().LinearVelocity);
  85|               foreach(var thruster in thrusters)
  86|                   thruster.SetValue("Override", (Single)(25 * 345600 * rc.GetShipSpeed()/100));                   
  87|               if (rc.GetShipSpeed() < 0.01f){
  88|                   status = "Rotation";
  89|               }
  90|               break;
  91|           case "Rotation":
  92|               if (Operation(DestGPS - rc.GetPosition()) < 0.01)
  93|                   status = "Wait";
  94|               break;
  95|           case "Wait":
  96|               count++;
  97|               if (count > BootWait){
  98|                   foreach(var thruster in thrusters)
  99|                       thruster.SetValue("Override", 345600f);                   
100|                   status = "Tracking";
101|               }
102|               break;
103|           case "Tracking":
104|                   Operation2();
105|                   if ((DestGPS-rc.GetPosition()).Length() < 1000)
106|                       status = "Tracking2";
107|               break;
108|           case "Tracking2":
109|               Operation(DestGPS - rc.GetPosition());
110|               if ((DestGPS-rc.GetPosition()).Length() < Dist_Det)
111|                   status = "Detonate";
112|               break;
113|           case "Detonate":
114|               foreach (var warhead in warheads)
115|                   warhead.ApplyAction("Detonate");
116|               break;
117|       }
118|   }
119|   
120|   private void SetTarget()
121|   {
122|       Vector3D Vt = targetVelocity;
123|       Vector3D Pt = targetGPS;
124|       Vector3D P = Pt-rc.GetPosition();
125|       double PP = Math.Pow(P.Length(), 2);
126|       double PVt = Vector3D.Dot(P, Vt);
127|       double VtVt = Math.Pow(Vt.Length(), 2);
128|       double S = rc.GetShipSpeed();
129|       remaining_time = (2*PP)/(-2*PVt+Math.Sqrt(4*PVt*PVt-4*PP*(VtVt-S*S)));
130|       remaining_time2 = (2*PP)/(-2*PVt-Math.Sqrt(4*PVt*PVt-4*PP*(VtVt-S*S)));
131|   
132|       if (!double.IsNaN(remaining_time) && !double.IsNaN(remaining_time2)){
133|           if (remaining_time >= 0 && remaining_time2 >= 0){
134|               if (remaining_time < remaining_time2)
135|                   DestGPS = Pt + Vt * remaining_time;
136|               else if (remaining_time > remaining_time2)
137|                   DestGPS = Pt + Vt * remaining_time2;
138|           }
139|           else if (remaining_time >=0)
140|               DestGPS = Pt + Vt * remaining_time;
141|           else if (remaining_time2 >=0)
142|               DestGPS = Pt + Vt * remaining_time2;          
143|       }
144|       else if (!double.IsNaN(remaining_time) && double.IsNaN(remaining_time2))
145|           DestGPS = Pt + Vt * remaining_time;
146|       else if (double.IsNaN(remaining_time) && !double.IsNaN(remaining_time2))
147|           DestGPS = Pt + Vt * remaining_time2;
148|   }
149|   
150|   private double Operation(Vector3D destV)
151|   {
152|       var rc = GridTerminalSystem.GetBlockWithName("Remote Control") as IMyShipController;
153|       destV.Normalize();
154|       Vector3D destLV = Vector3D.Cross(destV, rc.WorldMatrix.Down);
155|       Vector3D destDV = Vector3D.Cross(destV, rc.WorldMatrix.Left);
156|       destLV.Normalize();
157|       destDV.Normalize();
158|       
159|       double angleL = Math.Acos(Vector3D.Dot(rc.WorldMatrix.Forward, destLV)) - Math.PI/2;
160|       double angleD = Math.Acos(Vector3D.Dot(rc.WorldMatrix.Forward, destDV)) - Math.PI/2;
161|       double scaling_factor = 20;
162|       
163|       foreach(var gyro in gyros){
164|           gyro.SetValue("Override", true);
165|           gyro.SetValue("Yaw", (Single)(-scaling_factor * angleL));
166|           gyro.SetValue("Pitch", (Single)(scaling_factor * angleD));
167|       }
168|   
169|       double tilt = Vector3D.Dot(destV, rc.WorldMatrix.Forward);
170|       return Math.Acos(tilt > 1 ? 1 : tilt);
171|   }
172|   
173|   private void Operation2()
174|   {    
175|       Vector3D destV = DestGPS - rc.GetPosition();
176|       Vector3D Vel = rc.GetShipVelocities().LinearVelocity;
177|       Vector3D destLV = Vector3D.Cross(destV, rc.WorldMatrix.Down);
178|       Vector3D destDV = Vector3D.Cross(destV, rc.WorldMatrix.Left);
179|       
180|       Vector3D destYV = Vector3D.Cross(destLV, rc.WorldMatrix.Down);
181|       Vector3D destPV = Vector3D.Cross(destDV, rc.WorldMatrix.Left);
182|       destYV.Normalize();
183|       destPV.Normalize();
184|       
185|       Vector3D VelLV  = Vector3D.Cross(Vel, rc.WorldMatrix.Down);
186|       Vector3D VelDV  = Vector3D.Cross(Vel, rc.WorldMatrix.Left);
187|       VelLV.Normalize();
188|       VelDV.Normalize();
189|   
190|       double dif_angleY = Math.Acos(Vector3D.Dot(destYV, VelLV))-Math.PI/2;
191|       double dif_angleP = Math.Acos(Vector3D.Dot(destPV, VelDV))-Math.PI/2;
192|   
193|       EpL = dif_angleY;
194|       EpD = dif_angleP;
195|       EdL = dif_angleY - prevY;
196|       EdD = dif_angleP - prevP;
197|       prevY = dif_angleY;
198|       prevP = dif_angleP;
199|      
200|       float OPYaw = -(float)(kp * EpL + kd * EdL);
201|       float OPPitch = (float)(kp * EpD + kd * EdD);
202|           
203|       foreach(var gyro in gyros){
204|           gyro.SetValueBool("Override", true);
205|           if (!Single.IsNaN(OPYaw))
206|               gyro.SetValueFloat("Yaw", OPYaw);
207|           if (!Single.IsNaN(OPPitch))
208|               gyro.SetValueFloat("Pitch", OPPitch);
209|       }
210|   }

　10 基あるミサイルステーションのミサイルそれぞれ個別のパラメータを設定するため、各ステーションのマージブロックの CustomData プロパティーにデータを書き込んであります。このデータをミサイル発射時に読み込み（35-36 行）、ミサイルのパラメータとしてセットします（37-41 行）。status を "Launch" に設定し、1 tick 毎に Main メソッドが呼び出されるように UpdateFrequency を設定します。

  34|           case "MissileLaunch":
  35|               IMyShipMergeBlock mb= GridTerminalSystem.GetBlockWithName("MBStation") as IMyShipMergeBlock;
  36|               var para = mb.CustomData.Split(',');
  37|               BootOffset = int.Parse(para[0]);
  38|               BootWait = int.Parse(para[1]);
  39|               kp = double.Parse(para[2]);
  40|               kd = double.Parse(para[3]);
  41|               Dist_Det = float.Parse(para[4]);
  42|               status = "Launch";
  43|               count = 0;
  44|               Runtime.UpdateFrequency = UpdateFrequency.Update1;
  45|               break;

　トラッカーがターゲット追跡中は、常に "Target" コマンドを送信してきます。このコマンドにはターゲットの座標と速度が一緒に送られてくるので、これらのデータを変数にセットします（48-49 行）。これらのデータを基に撃墜予想座標を計算する SetTarget() メソッドを呼びます。

  46|           case "Target":
  47|               if (status != "Ready"){
  48|                   targetGPS = new Vector3D(float.Parse(com[1]), float.Parse(com[2]), float.Parse(com[3]));
  49|                   targetVelocity = new Vector3D(float.Parse(com[4]), float.Parse(com[5]), float.Parse(com[6]));
  50|                   SetTarget();
  51|               }
  52|               break;

　ミサイルが処理すべきもう一つのコマンドが "Detonate" です。複数のミサイルをターゲットに向けて発射した場合、ターゲット撃墜後に生き残るミサイルがでてきます。これらの目標を失ったミサイルに自爆を支持するコマンドが "Detonate" です。ミサイルが "Tacking" または "Tracking 2" の場合、このコマンドを受信するとミサイルの弾頭を起爆します。

  53|           case "Detonate":
  54|               if (status == "Tracking" || status == "Tracking2"){
  55|                   foreach (var warhead in warheads)
  56|                       warhead.ApplyAction("Detonate");
  57|               }
  58|               break;

　Main メソッド後半では、ミサイルの状態を表す status の処理です。発射コマンドを受信した後、status はまず "Launch" になります。ミサイルを発射するため、すべてのスラスターを全開にし（67-68 行）、マージブロックを下げながらミサイルをステーションから切り離します（69-70 行）。

  62|           case "Launch":
  63|               IMyShipMergeBlock mb = GridTerminalSystem.GetBlockWithName("MBMissile") as IMyShipMergeBlock;
  64|               IMyPistonBase pis = GridTerminalSystem.GetBlockWithName("PJ Piston") as IMyPistonBase;
  65|               GridTerminalSystem.GetBlocksOfType(gyros);
  66|               GridTerminalSystem.GetBlocksOfType(thrusters);
  67|               foreach(var thruster in thrusters)
  68|                   thruster.SetValue("Override", 345600f);            
  69|               pis.Retract();
  70|               mb.ApplyAction("OnOff_Off");
  71|               status = "Launching";
  72|               break;

　status が "Launching" では、変数 count が BootOffset 値を超えるまでミサイルは直進します。その後、ミサイルに搭載しているすべての弾頭の安全装置を解除します（78-80 行）。

  73|           case "Launching":
  74|               count++;
  75|               if (count > BootOffset){
  76|                   status = "Stop";
  77|                   count = 0;
  78|                   GridTerminalSystem.GetBlocksOfType(warheads);
  79|                   foreach (var warhead in warheads)
  80|                       warhead.ApplyAction("Safety");
  81|               }
  82|               break;

　続いて status は "Stop" となり、ミサイルを反転してこれまでの進行方向にスラスターを向けて停止するため、姿勢制御メソッド Operation を呼びます（84 行）。スラスターの出力をミサイルのスピードで比例制御し（85-86 行）、十分に速度が低下したら status を "Rotation" に更新します（87‐88 行）。

  83|           case "Stop":
  84|               Operation(-rc.GetShipVelocities().LinearVelocity);
  85|               foreach(var thruster in thrusters)
  86|                   thruster.SetValue("Override", (Single)(25 * 345600 * rc.GetShipSpeed()/100));                   
  87|               if (rc.GetShipSpeed() < 0.01f){
  88|                   status = "Rotation";
  89|               }
  90|               break;

　"Rotation" ではミサイル停止後、再び姿勢制御メソッド Operation を呼び、ミサイルを目的の方向に向けます（92 行）。続いて "Wait" では、それぞれのミサイルに設定された BootWait 値に従って一定期間待機します（97‐101 行）。これによりターゲットへの着弾タイミングを調整しています。

  91|           case "Rotation":
  92|               if (Operation(DestGPS - rc.GetPosition()) < 0.01)
  93|                   status = "Wait";
  94|               break;
  95|           case "Wait":
  96|               count++;
  97|               if (count > BootWait){
  98|                   foreach(var thruster in thrusters)
  99|                       thruster.SetValue("Override", 345600f);                   
100|                   status = "Tracking";
101|               }
102|               break;

　この後 "Tracking" で、ミサイルを目標地点に向けて誘導するため Operation2() メソッドを呼びます（104 行）。そして、目的地点までの距離が 1,000 m 未満になると（105 行）、status は "Tracking2” となり、目標地点に向けて姿勢制御しながら進みます（109 行）。また、ターゲットとの衝突により弾頭が起爆しなくても、一定の距離 Dist_Det 未満になるとこのプログラムから弾頭を起爆します（110-116 行）。

103|           case "Tracking":
104|                   Operation2();
105|                   if ((DestGPS-rc.GetPosition()).Length() < 1000)
106|                       status = "Tracking2";
107|               break;
108|           case "Tracking2":
109|               Operation(DestGPS - rc.GetPosition());
110|               if ((DestGPS-rc.GetPosition()).Length() < Dist_Det)
111|                   status = "Detonate";
112|               break;

113|           case "Detonate":
114|               foreach (var warhead in warheads)
115|                   warhead.ApplyAction("Detonate");
116|               break;

　SetTarget メソッドは、トラッカーから受信したターゲット座標 targetGPS とターゲット速度 targetVelocity、ミサイルの座標と速度から撃墜予想座標 DestGPS を計算します。撃墜までの残り時間を表す remaining_time と remaining_time2 は、式中（129-130 行）の平方根の中が負の場合 NaN となるため、この後の計算ではこれらの値が NaN でないことをチェックして（132-146 行）計算をしています。

129|       remaining_time = (2*PP)/(-2*PVt+Math.Sqrt(4*PVt*PVt-4*PP*(VtVt-S*S)));
130|       remaining_time2 = (2*PP)/(-2*PVt-Math.Sqrt(4*PVt*PVt-4*PP*(VtVt-S*S)));

  ⁞

132|       if (!double.IsNaN(remaining_time) && !double.IsNaN(remaining_time2)){
  ⁞
144|       else if (!double.IsNaN(remaining_time) && double.IsNaN(remaining_time2))
  ⁞
146|       else if (double.IsNaN(remaining_time) && !double.IsNaN(remaining_time2))

　ミサイルが目標ベクトルの方向を向いていれば、求めたこれら ２ つのベクトルとミサイル正面ベクトルのなす角はともに π/2 となります。これらのなす角から π/2 を引いた angleL および angleD は目標ベクトルからの角度のずれを表し（159-160 行）、このずれ角を基にジャイロの Yaw と Pitch を設定する（163-167 行）ことで、ミサイル正面を destV に近づけます。

159|       double angleL = Math.Acos(Vector3D.Dot(rc.WorldMatrix.Forward, destLV)) - Math.PI/2;
160|       double angleD = Math.Acos(Vector3D.Dot(rc.WorldMatrix.Forward, destDV)) - Math.PI/2;
  ⁞
163|       foreach(var gyro in gyros){
164|           gyro.SetValue("Override", true);
165|           gyro.SetValue("Yaw", (Single)(-scaling_factor * angleL));
166|           gyro.SetValue("Pitch", (Single)(scaling_factor * angleD));
167|       }

　Operation メソッドは、引数として渡される ベクトル destV とミサイルの前方ベクトルが一致するように姿勢制御します。まず、目標ベクトル destV と ミサイルの下方ベクトル rc.WorldMatrix.Down を含む平面の法線ベクトル destLV を求めます（154 行）。同様に destV とミサイルの左向きベクトルを含む平面の法線ベクトル destDV を求めます（155 行）。

154|       Vector3D destLV = Vector3D.Cross(destV, rc.WorldMatrix.Down);
155|       Vector3D destDV = Vector3D.Cross(destV, rc.WorldMatrix.Left);

　最後にメソッドは、ミサイル正面のベクトルと destV のなす角を返します（169-170 行）。

169|       double tilt = Vector3D.Dot(destV, rc.WorldMatrix.Forward);
170|       return Math.Acos(tilt > 1 ? 1 : tilt);

　Operation2 メソッドは、さきほどの Operation メソッドと異なり、ミサイルの進行方向ベクトルをターゲットに向けるようにミサイルの姿勢を制御します。ミサイルの進行方向ベクトルと目標ベクトルのずれ dif_angleY と dif_angleP を計算し（190-191 行）、これらの値をもとにジャイロの Yaw と Pitch プロパティー値を PD 制御しています（193-201 行）。このミサイルは他の宇宙船と違い、ミサイル後部に一方向にのみスラスターがあり、ジャイロによってミサイルの姿勢を制御することで進行方向を変えています。このような機体を宇宙空間で精度良くコントロールするために PD 制御を行っています。

190|       double dif_angleY = Math.Acos(Vector3D.Dot(destYV, VelLV))-Math.PI/2;
191|       double dif_angleP = Math.Acos(Vector3D.Dot(destPV, VelDV))-Math.PI/2;
192|   
193|       EpL = dif_angleY;
194|       EpD = dif_angleP;
195|       EdL = dif_angleY - prevY;
196|       EdD = dif_angleP - prevP;
197|       prevY = dif_angleY;
198|       prevP = dif_angleP;
199|      
200|       float OPYaw = -(float)(kp * EpL + kd * EdL);
201|       float OPPitch = (float)(kp * EpD + kd * EdD);

　ジャイロの Yaw と Pitch に計算された操作量を設定します。このように、ジャイロをオーバーライドして直接値を設定する際、設定値が NaN の場合にジャイロがロックして操作不能になります。これを回避するため、プロパティーに値を書き込む前に値が NaN でないことをチェックしています（205, 207 行）。

203|       foreach(var gyro in gyros){
204|           gyro.SetValueBool("Override", true);
205|           if (!Single.IsNaN(OPYaw))
206|               gyro.SetValueFloat("Yaw", OPYaw);
207|           if (!Single.IsNaN(OPPitch))
208|               gyro.SetValueFloat("Pitch", OPPitch);
209|       }

bottom of page