SE导弹实现思路
Posted dudujerry
tags:
篇首语：本文由小常识网(cha138.com)小编为大家整理，主要介绍了SE导弹实现思路相关的知识，希望对你有一定的参考价值。
其实是挺久以前弄的......
——序
要实现导弹，首先就是要制导。
所以，不能傻乎乎的直接撞过去，所以我们要设计一些算法，来让他直直的撞过去！
那么如何瞄准呢？SE提供了转向的设备（陀螺仪），相信聪明的读者已然想到！！！我们要通过陀螺仪转向，对准目标！！然后给他一炮！！！
......先等会，我们连如何控制转向都不知道呢(┬＿┬)......
接下来查阅资料，大佬的实验！！！https://tieba.baidu.com/p/6226176333?pn=2
知道了陀螺仪的三个参数分别是反向的yaw,pitch,roll轴的目标角速度(科学是挺科学的...可是为啥要反过来啊啊啊啊啊)
懂得了原理...接下来设计算法......
显然，只要得到我们与目标的相对角度，然后算一下我们的角速度，让陀螺仪只要不能刹车了就刹车，其余时间全部加速！！！
但是怎么获得相对角度呢......?
原来的想法是用逆三角函数根据XZ,YZ坐标求出相应的yaw,pitch轴，但是感觉挺麻烦而且不切实际（老觉得自己的想法不行）
于是查阅资料( 获得这篇神奇博客！！！https://www.kancloud.cn/kylilimin/mea-ship/1003204
里面的代码，告诉了我怎么获得相对角度，它先转换世界坐标系为局部坐标系(用法线变换，说实话这玩意我也不知道是啥，矩阵变换这玩意我也不是很懂）
然后将目标点在局部坐标系中分别投影到XZ,YZ！！！！
这样一来就很清晰明了，实际上与我的思路差不多，但是看起来更可靠一些(
然后用点积求平面上两向量(Z轴与投影到XZ,YZ上的标准向量)的夹角的cos(cos = |A · B| / |A|*|B| )，然后再用逆三角函数求得角......
是不是很科学......但是经过群里的大佬点拨，其实特么的直接用逆tan就可以了！！！傻了傻了(其实用点积比较有逼格)
这份代码中也有许多问题......不细说了，看下文。
解决了获取相对坐标的方式，就可以求解角速度了。
我们知道，v = Δv * t
则 t = v / Δv (1)
我们知道, t = s/v (2)
我们通过垃圾K社提供的垃圾API( ，得到飞船的角速度v，和我们求出的相对角度s，通过这个公式t1 = s/v(2)，可以得到我们要走的时间！！！
我们又可以通过将所有陀螺仪开到最大，两帧相减算出其可提供的最大角加速度 Δv，注意，这个 Δv不是目标角加速度，只是陀螺仪一帧可以达到的最大角加速度，
所以实际上我们是不关心所谓的目标角加速度的，我们只关心一帧能有多少角加速度。
所以，刹车时间就是 t2 = v / Δv(1)，即可求得刹车时间。
所以，我们根据得到的t1和t2,显然可以得到如下关系
若t1 > t2，则时间足够刹车，可以尽情开动陀螺仪。
若t1 < t2，则时间不够刹车，需要尽力反向开动陀螺仪。
通过这两个关系的控制，我们可以控制时间是否足够刹车，也就完成了这个系统。（总感觉意犹未尽）
上面说的博客，提供了这种算法的特点：
- 绝不会错过目标
- 获得了抵达目标的时间最优解，即在不错过的情况下，用时最短
- 精确无震荡
- 存在动态误差，而且在圆周运动时候，动态误差会逐渐扩大到无法控制的程度
实际上，只要掌握了获取相对坐标的方法，同时记得请陀螺仪的加速是反的，其实也可以上个PID。
另外，需要注意的是，最好使用atan2这个函数，因为atan函数的值域只有-90~90,意味着只能处理两个象限的情况。
但是我们的目标可不会乖乖的呆在这两个象限，所以使用范围更大的atan2(-180~180)可以避免出现有两个位置拥有相同的相对角度导致不停往复。
推进器：直接无脑上个缓冲器，把Z轴(Forward)方向的缓冲控制去掉！！！Forward方向直接越级控制拉满！！！目标如果转移就是另外上下左右四个方向的推进器的事儿了！！！！我特么直接冲！！！
上个PID就完事儿了。
附代码(不完善，有些许bug，之后有时间再修改)
1 //by dudujerry 2 //5.28.导弹 3 4 5 //算法变量 6 double _AimTargetRatio = 1;//精度 7 List<Vector2D> _AimTarget_Data = new List<Vector2D>(); 8 Vector2D _MaxAngleAcceleration;//最大角加速度,X=Yaw;y=Pitch 9 Vector2D _InitAngleVelocity; 10 int _BeforeAim = 60;//1秒出膛 11 int _TimeGoesBy = 0; 12 13 //设备变量 14 List<IMyGyro> _gyros = new List<IMyGyro>(); 15 List<IMyRemoteControl> _remoteControls = new List<IMyRemoteControl>(); 16 List<IMyLargeGatlingTurret> _gatlings = new List<IMyLargeGatlingTurret>(); 17 IMyRemoteControl cp; 18 List<IMyThrust> _thrusts = new List<IMyThrust>(); 19 20 //实际变量 21 MyDetectedEntityInfo _enemy = new MyDetectedEntityInfo(); 22 23 void SetGyros(double yaw,double pitch){ 24 25 foreach(var gyro in _gyros){ 26 gyro.Enabled = true; 27 gyro.GyroOverride = true; 28 gyro.Yaw = (float)yaw; 29 gyro.Pitch = (float)pitch; 30 } 31 32 } 33 34 Vector3D SubVector3D(Vector3D a,Vector3D b){ 35 return new Vector3D(a.X - b.X,a.Y - b.Y, a.Z - b.Z); 36 37 } 38 39 bool InitGyros(){//测陀螺仪最大角加速度 40 if(_MaxAngleAcceleration.Length() != 0){ 41 //若已经测过了，返回 42 return true; 43 } 44 45 Vector3D angularVelocity = cp.GetShipVelocities().AngularVelocity; 46 47 MatrixD refLookAtMatrix = MatrixD.CreateLookAt(new Vector3D(), cp.WorldMatrix.Forward, cp.WorldMatrix.Up); 48 Vector3D meAngleVelocityToMe = Vector3D.TransformNormal(angularVelocity, refLookAtMatrix); 49 //(*)- 50 double meYawAngleVelocity = meAngleVelocityToMe.Y*180/Math.PI; //当前角速度 51 double mePitchAngleVelocity = meAngleVelocityToMe.X*180/Math.PI; 52 //得到当前帧的角速度(*)- 53 54 if(_InitAngleVelocity.Length() == 0){ 55 _InitAngleVelocity = new Vector2D(meYawAngleVelocity,mePitchAngleVelocity); 56 57 //陀螺仪全开 58 //gys.SetOnOff(true); 59 //gys.SetOverride(true); 60 //gys.Yaw = gys.Pitch = 60; 61 //(*)- 62 SetGyros(60,60); 63 } 64 else{ 65 _MaxAngleAcceleration = new Vector2D((meYawAngleVelocity - _InitAngleVelocity.X)*60, (mePitchAngleVelocity - _InitAngleVelocity.Y)*60); 66 //计算最大角加速度，由于一秒60帧所以乘60 67 68 //gys.SetOverride(false); 69 //gys.Yaw = gys.Pitch = 0; 70 //返回原状态(*)- 71 SetGyros(0,0); 72 } 73 return false; 74 } 75 76 void AimTarget(Vector3D targetPosition){ 77 double yaw = 0;//陀螺仪输出 78 double pitch = 0; 79 80 81 MatrixD refLookAtMatrix = MatrixD.CreateLookAt(new Vector3D() , cp.WorldMatrix.Forward, cp.WorldMatrix.Up); 82 Vector3D positionToMe = Vector3D.TransformNormal(SubVector3D(targetPosition,cp.CenterOfMass),refLookAtMatrix);//[1]-cp.Position 83 //转换坐标系 (*)- 84 85 //Echo(positionToMe.ToString()); 86 double targetYawAngle = TargetAngleToMe(positionToMe,"Yaw"); 87 double targetPitchAngle = TargetAngleToMe(positionToMe,"Pitch"); 88 89 Echo("A:" + ((int)targetYawAngle).ToString() + " " + ((int)targetPitchAngle).ToString()); 90 91 //算出基于自己坐标系的相对角度 92 93 while(_AimTarget_Data.Count > 30)//周期：30 94 { 95 _AimTarget_Data.RemoveAt(0); 96 } 97 while(_AimTarget_Data.Count < 30){ 98 _AimTarget_Data.Add(new Vector2D(targetYawAngle,targetPitchAngle)); 99 } 100 _AimTarget_Data.RemoveAt(0); 101 _AimTarget_Data.Add(new Vector2D(targetYawAngle,targetPitchAngle)); 102 //统计误差数据 103 104 double meYawAngleVelocity = cp.GetShipVelocities().AngularVelocity.Y; 105 double mePitchAngleVelocity = cp.GetShipVelocities().AngularVelocity.X; 106 107 108 double effectYawVelocity = (_AimTarget_Data[_AimTarget_Data.Count - 1] .X - _AimTarget_Data[_AimTarget_Data.Count - 2] .X) * 60; 109 double effectPitchVelocity = (_AimTarget_Data[_AimTarget_Data.Count - 1] .Y - _AimTarget_Data[_AimTarget_Data.Count - 2] .Y) * 60; 110 Echo("合速度V:" + ((int)effectYawVelocity).ToString() + " " + ((int)effectPitchVelocity).ToString()); 111 //算合速度 (*)- 112 113 if(effectYawVelocity*targetYawAngle > 0){ 114 //目标角度和合速度符号不同，即正朝着目标靠近 115 if(Math.Abs(targetYawAngle) > _AimTargetRatio){ 116 //距离目标还有距离 117 118 double stopTime = Math.Abs(effectYawVelocity / _MaxAngleAcceleration.X); 119 //刹车时间 120 double arriveTime = Math.Abs(targetYawAngle / effectYawVelocity); 121 //抵达目标所用时间 122 123 //yaw = -60 * (targetYawAngle >= 0 ? 1 : (-1)); 124 125 126 if(stopTime <= arriveTime){ 127 //刹车时间足够 128 yaw = 60 * (targetYawAngle >= 0 ? 1 : (-1)); 129 //控制方向 130 Echo("刹车时间足够:是"); 131 } 132 else{ 133 yaw = -60 * (targetYawAngle >= 0 ? 1 : (-1)); 134 //控制方向 135 Echo("刹车时间足够:否"); 136 } 137 138 } 139 else{ 140 //Echo("?:" + Math.Abs((int)targetYawAngle).ToString() +"-" + ((int)_AimTargetRatio).ToString()); 141 yaw = targetYawAngle*60/180; 142 Echo("刹车时间足够:已到达"); 143 } 144 145 } 146 else { 147 //正在远离目标 148 yaw = 60 * (targetYawAngle >= 0 ? 1 : (-1)); 149 Echo("刹车时间足够:反方向"); 150 } 151 152 if(effectPitchVelocity*targetPitchAngle > 0){ 153 //目标pitch速度和合速度符号不同，即正朝着目标靠近 154 if(Math.Abs(targetPitchAngle) > _AimTargetRatio){ 155 //距离目标还有距离 156 157 double stopTime = Math.Abs(effectPitchVelocity / _MaxAngleAcceleration.Y); 158 //刹车时间 159 double arriveTime = Math.Abs(targetPitchAngle / effectPitchVelocity); 160 //抵达目标所用时间 161 162 //pitch = -60 * (targetPitchAngle >= 0 ? 1 : (-1)); 163 164 if(stopTime <= arriveTime){ 165 //刹车时间足够 166 Echo("刹车时间足够:是"); 167 pitch = -60 * (targetPitchAngle >= 0 ? 1 : (-1)); 168 //控制方向 169 } 170 else{ 171 pitch = 60 * (targetPitchAngle >= 0 ? 1 : (-1)); 172 Echo("刹车时间足够:否"); 173 //控制方向 174 } 175 176 } 177 else{ 178 pitch = -targetPitchAngle*60/180; 179 Echo("刹车时间足够:已到达"); 180 181 } 182 183 } 184 else { 185 //正在远离目标 186 pitch = -60 * (targetPitchAngle >= 0 ? 1 : (-1)); 187 Echo("刹车时间足够:反方向"); 188 189 } 190 Echo("Yaw:" + yaw.ToString() + " Pitch:" + pitch.ToString()); 191 192 //gys.SetOverride(true); 193 //gys.Yaw = yaw; 194 //gys.Pitch = pitch; 195 //控制陀螺仪(*)- 196 SetGyros(yaw,pitch); 197 198 Echo("自己的V:" + ((int)meYawAngleVelocity).ToString()+((int)mePitchAngleVelocity).ToString()); 199 Echo("陀螺仪的V:" + ((int)_gyros[0].Yaw).ToString()+((int)_gyros[0].Pitch).ToString()); 200 } 201 202 public static double TargetAngleToMe(Vector3D targetToMe,string direction){ 203 targetToMe = Vector3D.Normalize(targetToMe); 204 Vector3D zForward = new Vector3D(0,0,-1); 205 if(direction == "Yaw"){ 206 207 Vector3D targetYawVector = new Vector3D(targetToMe.X,0,targetToMe.Z); 208 209 return Math.Atan2(targetToMe.X,targetToMe.Z) * 180/Math.PI; 210 211 } 212 else if (direction == "Pitch"){ 213 Vector3D targetYawVector = new Vector3D(0,targetToMe.Y,targetToMe.Z); 214 return Math.Atan2(targetToMe.Y,targetToMe.Z) * 180/Math.PI; 215 216 217 } 218 return 0; 219 } 220 221 222 223 224 public Program(){ 225 Runtime.UpdateFrequency = UpdateFrequency.Update1; 226 227 GridTerminalSystem.GetBlocksOfType<IMyGyro>(_gyros); 228 GridTerminalSystem.GetBlocksOfType<IMyRemoteControl>(_remoteControls); 229 GridTerminalSystem.GetBlocksOfType<IMyLargeGatlingTurret>(_gatlings); 230 GridTerminalSystem.GetBlocksOfType<IMyThrust>(_thrusts); 231 232 cp = _remoteControls[0]; 233 Echo("Programed"); 234 235 } 236 237 public void ControlThrusts(){ 238 239 MatrixD refLookAtMatrix = MatrixD.CreateLookAt(new Vector3D() , cp.WorldMatrix.Forward, cp.WorldMatrix.Up); 240 241 foreach(var thrust in _thrusts){ 242 var maxThrust = thrust.MaxEffectiveThrust; 243 244 Echo("in"); 245 246 switch (cp.WorldMatrix.GetClosestDirection(thrust.WorldMatrix.Backward)){ 247 case Base6Directions.Direction.Forward: 248 thrust.ThrustOverride = maxThrust; 249 thrust.ThrustOverridePercentage = 1; 250 251 break; 252 253 case Base6Directions.Direction.Right: 254 255 float velocity1 = (float)-(Vector3D.TransformNormal(cp.GetShipVelocities().LinearVelocity,refLookAtMatrix)).X; 256 thrust.ThrustOverridePercentage = (velocity1 > 0) ? velocity1 : 0; 257 break; 258 259 case Base6Directions.Direction.Left: 260 float velocity2 = (float)-(Vector3D.TransformNormal(cp.GetShipVelocities().LinearVelocity,refLookAtMatrix)).X; 261 thrust.ThrustOverridePercentage = (velocity2 < 0) ? -velocity2 : 0; 262 break; 263 264 case Base6Directions.Direction.Up: 265 float velocity3 = (float)-(Vector3D.TransformNormal(cp.GetShipVelocities().LinearVelocity,refLookAtMatrix)).Y; 266 thrust.ThrustOverridePercentage = (velocity3 > 0) ? velocity3 : 0; 267 break; 268 269 case Base6Directions.Direction.Down: 270 float velocity4 = (float)-(Vector3D.TransformNormal(cp.GetShipVelocities().LinearVelocity,refLookAtMatrix)).Y; 271 thrust.ThrustOverridePercentage = (velocity4 < 0) ? -velocity4 : 0; 272 break; 273 } 274 } 275 276 } 277 278 279 public void Main(string argument, UpdateType updateSource) 280 { 281 if(_TimeGoesBy <= _BeforeAim) 282 _TimeGoesBy ++; 283 if(InitGyros() == false) 284 return; 285 286 MyDetectedEntityInfo enemy = _gatlings[0].GetTargetedEntity(); 287 //Echo(enemy.Position.ToString()); 288 if(enemy.IsEmpty() == false){ 289 _enemy = enemy; 290 } 291 292 //Echo( "rel:" + _enemy.Relationship.Enemies + _enemy.Relationship.Neutral); 293 //&& (_enemy.Type == MyDetectedEntityType.SmallGrid || _enemy.Type == MyDetectedEntityType.LargeGrid) 294 295 296 if(_enemy.IsEmpty() == false && _TimeGoesBy > _BeforeAim && (_enemy.Relationship == MyRelationsBetweenPlayerAndBlock.Enemies || _enemy.Relationship == MyRelationsBetweenPlayerAndBlock.Neutral) ){ 297 AimTarget(_enemy.Position); 298 Echo("Aimed"); 299 } 300 else if (_enemy.IsEmpty() == true) 301 { 302 Echo("Can‘t Find Entity"); 303 } 304 //Echo(TargetAngleToMe(new Vector3D(4,2,2),"Yaw").ToString()); 305 //ControlThrusts(); 306 307 308 309 return; 310 }
二营长，你他娘的意大利炮呢？给我拉上来！
以上是关于SE导弹实现思路的主要内容，如果未能解决你的问题，请参考以下文章