## 一阶惯性系统 所谓一阶惯性系统，就是指程序控制的变量，实际并不能直接作用于结果（误差）上。 举个例子： 往水池注水，目标水位是10，当前水位是5，那么误差是10-5=5，此时通过程序控制一个水阀往池中注水，设定水阀的出水量 = 误差。当程序所控制的水阀关闭的瞬间，误差会立刻停止改变。也就是程序控制的变量会立刻马上直接的响应到误差上，此为非惯性系统。 飞船当前位置是10，需要飞往100位置，误差是90.此时通过程序控制飞船的推进器出力，设定推力 = 误差。当程序所控制的推进器关闭推进的瞬间，误差不会立刻停止改变，因为推力产生加速度，加速度产生速度，而速度才产生位移。程序控制的变量不是直接作用于误差，此为惯性系统。 <br> 为了测试之前FCS算法，特地设置了一个测试动态误差的装置。该装置匀速旋转，让FCS锁定之，如果有动态误差，锁定瞄准点会总是落后于目标。  转动后，可见动态误差如下：  <br> 首先分析震荡问题 陀螺仪出力范围是-60~60，误差范围-1~1，如果直接采用误差 x 60反馈给陀螺仪，90度误差内也没有震荡。仅在超过90度以后才可能震荡。 如果误差 x 240，相当于4倍时，才产生明显震荡，此时采用 P(err + D*derr) 当D较大（10以上）时，能减少震荡次数。 但当D过大（30以上）时，也能减少更多的震荡次数，但会产生在瞄准后发生微小抖动的情况。 <br> 结论： 使用：P(err + D*derr) 当P设为120时，D设为20时，效果最好。 此时若采用直接误差反馈，P为120，约30度内会产生1次震荡。但采用上述算法不会产生震荡，且抵达时间相同。 ## 尝试转换为0阶响应系统 之前在做推进器控制导航时采用过一个算法： 其实这个算法就是把误差从距离转换为速度，所以推进器推速度的控制降阶为0阶。这个算法首先要求取当前所需的速度。 开始测试： 陀螺仪指向问题可以转化为Yaw和Pitch的二维问题 首先设法找到目标相对自己的两个角度，再把问题归纳为一维问题。 ``` void AimTarget(Vector3D position){ MatrixD refLookAtMatrix = MatrixD.CreateLookAt(new Vector3D(), cp.WorldMatrix.Forward, cp.WorldMatrix.Up); Vector3D PositionToMe = Vector3D.TransformNormal(position - cp.Position, refLookAtMatrix); //需要求出目标基于自己坐标系的相对角度 double TargetYawAngle = Caculate.TargetAngleToMe(PositionToMe, "Yaw"); double TargetPitchAngle = Caculate.TargetAngleToMe(PositionToMe, "Pitch"); //求出自己当前角速度相对自己的值，这是一套弧度值，其中最大值是0.45，最小值是0.0005，x表示俯仰Pitch（上+下-），y表示偏航Yaw（左+右-），z表示滚转（顺时针-逆时针+） Vector3D MeAngleVelocityToMe = Vector3D.TransformNormal(cp.AngleVelocity, refLookAtMatrix); double MeYawAngleVelocity = MeAngleVelocityToMe.Y*180/Math.PI; double MePitchAngleVelocity = MeAngleVelocityToMe.X*180/Math.PI; Echo(TargetPitchAngle.ToString()); Echo(MePitchAngleVelocity.ToString()); // gys.SetOverride(false); } //计算目标坐标点相对自己的角度 //传入目标相当于自己坐标系的坐标，传入一个方向: Yaw、Pitch //当传入Yaw返回目标相对自己左右的角度（左-右+），传入Pitch返回目标相对自己上下的角度（上-下+） public static double TargetAngleToMe(Vector3D target_to_me, string direction){ target_to_me = Vector3D.Normalize(target_to_me); Vector3D meForward = new Vector3D(0,0,-1); if(direction == "Yaw"){ //把目标投影到xz平面 Vector3D targetYawVector = new Vector3D(target_to_me.X, 0, target_to_me.Z); double pn = target_to_me.X >= 0 ? 1 : -1; return Math.Acos(targetYawVector.Dot(meForward)/(targetYawVector.Length() * meForward.Length()))*180/Math.PI * pn; }else if(direction == "Pitch"){ //把目标投影到yz平面 Vector3D targetPitchVector = new Vector3D(0, target_to_me.Y, target_to_me.Z); double pn = target_to_me.Y <= 0 ? 1 : -1; return Math.Acos(targetPitchVector.Dot(meForward)/(targetPitchVector.Length() * meForward.Length()))*180/Math.PI * pn; } return 0; } ``` 这样可以获取到目标基于自己坐标系的Yaw Pitch方位角以及自己角速度的Yaw Pitch分量。 但要精确求解还需要知道自己飞船在Yaw和Pitch方向的最大角加速度。 可以通过如下方法获得： ``` Vector2D MaxAngleAcceleration; //其中 X表示Yaw，Y表示Pitch Vector2D InitAngleVelocity; //其中 X表示Yaw，Y表示Pitch bool initGyros(){ if(MaxAngleAcceleration.Length() != 0) return true; //记录当前帧的Yaw角速度 MatrixD refLookAtMatrix = MatrixD.CreateLookAt(new Vector3D(), cp.WorldMatrix.Forward, cp.WorldMatrix.Up); Vector3D MeAngleVelocityToMe = Vector3D.TransformNormal(cp.AngleVelocity, refLookAtMatrix); double MeYawAngleVelocity = MeAngleVelocityToMe.Y*180/Math.PI; //当前角速度 double MePitchAngleVelocity = MeAngleVelocityToMe.X*180/Math.PI; if(InitAngleVelocity.Length() == 0){ //记录当前角速度 InitAngleVelocity = new Vector2D(MeYawAngleVelocity, MePitchAngleVelocity); //开启陀螺仪全力 gys.SetOnOff(true); gys.SetOverride(true); gys.Yaw = gys.Pitch = 60; }else{ //下一帧，记录新的Yaw角速度，计算最大角加速度 MaxAngleAcceleration = new Vector2D((MeYawAngleVelocity - InitAngleVelocity.X)*60, (MePitchAngleVelocity - InitAngleVelocity.Y)*60) ; //每秒60帧，所以乘以60 gys.SetOverride(false); gys.Yaw = gys.Pitch = 0; } return false; } ``` 获取到最大角加速度后，验证其正确性完成。 **同时发现了一个特性，游戏中物体的角速度会自然衰减，衰减速度大约在2~3度/s** **同时发现了最大角加速度也有限制，无论陀螺仪数量多少，最大角加速度在任意方向都是170度/s** <br> ## 新的方法 经过一系列探索，得到了如下两个算法 ``` //第一版算法，不考虑目标速度和加速度 //当速度与目标反向，全力减速。当速度与目标同向，通过计算减速时间和当前速度到达时间判断全力加速或减速 // 这个方法能精确抵达目标，完全消除震荡，但不能有效消除动态误差 double AimTargetRatio = 0.2; //瞄准精度，角度 List<Vector2D> AimTarget_Data = new List<Vector2D>(); void AimTarget(Vector3D position){ double yaw = 0; double pitch = 0; MatrixD refLookAtMatrix = MatrixD.CreateLookAt(new Vector3D(), cp.WorldMatrix.Forward, cp.WorldMatrix.Up); Vector3D PositionToMe = Vector3D.TransformNormal(position - cp.Position, refLookAtMatrix); //需要求出目标基于自己坐标系的相对角度 double TargetYawAngle = Caculate.TargetAngleToMe(PositionToMe, "Yaw"); double TargetPitchAngle = Caculate.TargetAngleToMe(PositionToMe, "Pitch"); //储存误差数据 while(AimTarget_Data.Count > 30){ AimTarget_Data.RemoveAt(0); } while(AimTarget_Data.Count < 30){ AimTarget_Data.Add(new Vector2D(TargetYawAngle, TargetPitchAngle)); } AimTarget_Data.RemoveAt(0); AimTarget_Data.Add(new Vector2D(TargetYawAngle, TargetPitchAngle)); //求出自己当前角速度相对自己的值，这是一套弧度值，其中最大值是0.45，最小值是0.0005，x表示俯仰Pitch（上+下-），y表示偏航Yaw（左+右-），z表示滚转（顺时针-逆时针+） double MeYawAngleVelocity = cp.YawVelocity; double MePitchAngleVelocity = cp.PitchVelocity; //将自己速度与目标角速度算合速度，对误差积分 // double YawErrSum = 0; // double PitchErrSum = 0; // foreach(Vector2D err in AimTarget_Data){ // YawErrSum += err.X; // PitchErrSum += err.Y; // } // MeYawAngleVelocity = MeYawAngleVelocity + YawErrSum/10; // MePitchAngleVelocity = MePitchAngleVelocity + PitchErrSum/10; //由于物体自带角速度削减属性，需要一个参数修正加速度 double AccelerationFixedRatio = 5; if(MeYawAngleVelocity*TargetYawAngle < 0){ if(Math.Abs(TargetYawAngle) > AimTargetRatio){ //当前速度正朝着目标过去 //刹车所需时间 double StopTime = Math.Abs(MeYawAngleVelocity / (MaxAngleAcceleration.X+AccelerationFixedRatio)); //当前速度抵达目标所需时间 double ArriveTime = Math.Abs(TargetYawAngle / MeYawAngleVelocity); //比较两个时间大小决定减速或加速 if(StopTime <= ArriveTime){ //如果刹车时间足够，全力推进 yaw = 60 * Math.Abs(TargetYawAngle) / TargetYawAngle; }else{ yaw = -60 * Math.Abs(TargetYawAngle) / TargetYawAngle; } }else{ yaw = TargetYawAngle*60/180; } }else{ //当前速度正背离目标 yaw = 60 * Math.Abs(TargetYawAngle) / TargetYawAngle; } if(MePitchAngleVelocity*TargetPitchAngle < 0){ if(Math.Abs(TargetPitchAngle) > AimTargetRatio){ //当前速度正朝着目标过去 //刹车所需时间 double StopTime = Math.Abs(MePitchAngleVelocity / (MaxAngleAcceleration.Y+AccelerationFixedRatio)); //当前速度抵达目标所需时间 double ArriveTime = Math.Abs(TargetPitchAngle / MePitchAngleVelocity); //比较两个时间大小决定减速或加速 if(StopTime <= ArriveTime){ //如果刹车时间足够，全力推进 pitch = -60 * Math.Abs(TargetPitchAngle) / TargetPitchAngle; }else{ pitch = 60 * Math.Abs(TargetPitchAngle) / TargetPitchAngle; } }else{ pitch = -TargetPitchAngle*60/180; } }else{ //当前速度正背离目标 pitch = -60 * Math.Abs(TargetPitchAngle) / TargetPitchAngle; } gys.Yaw = yaw; gys.Pitch = pitch; gys.Roll = -cp.InputRoll*40; gys.SetOverride(true); } ``` 上述的算法完全无震荡，对于静止的目标，可以达到最高的响应效率。因为是用自己的速度和加速度做判断，当自己速度与目标角度误差反向时，直接反向最大推进。当自己速度与目标角度误差同向时，如果未抵达刹车距离就最大推进，如果已经抵达就最大反向推进。 特性如下： 1. 绝不会错过目标 2. 获得了抵达目标的时间最优解，即在不错过的情况下，用时最短 3. 精确无震荡 4. 存在动态误差，而且在圆周运动时候，动态误差会逐渐扩大到无法控制的程度 <br> 第二个算法： ``` double AimTargetRatio = 1; //瞄准精度，角度 List<Vector2D> AimTarget_Data = new List<Vector2D>(); void AimTarget(Vector3D position){ double yaw = 0; double pitch = 0; MatrixD refLookAtMatrix = MatrixD.CreateLookAt(new Vector3D(), cp.WorldMatrix.Forward, cp.WorldMatrix.Up); Vector3D PositionToMe = Vector3D.TransformNormal(position - cp.Position, refLookAtMatrix); //需要求出目标基于自己坐标系的相对角度 double TargetYawAngle = Caculate.TargetAngleToMe(PositionToMe, "Yaw"); double TargetPitchAngle = Caculate.TargetAngleToMe(PositionToMe, "Pitch"); //储存误差数据 while(AimTarget_Data.Count > 30){ AimTarget_Data.RemoveAt(0); } while(AimTarget_Data.Count < 30){ AimTarget_Data.Add(new Vector2D(TargetYawAngle, TargetPitchAngle)); } AimTarget_Data.RemoveAt(0); AimTarget_Data.Add(new Vector2D(TargetYawAngle, TargetPitchAngle)); //求出自己当前角速度相对自己的值，这是一套弧度值，其中最大值是0.45，最小值是0.0005，x表示俯仰Pitch（上+下-），y表示偏航Yaw（左+右-），z表示滚转（顺时针-逆时针+） double MeYawAngleVelocity = cp.YawVelocity; double MePitchAngleVelocity = cp.PitchVelocity; //求解合速度（相当于目标相对自己的角速度与自己角速度的合值），使用合速度判定没有动态误差，但有静态震荡 double EffectYawVelocity = (AimTarget_Data[AimTarget_Data.Count - 1].X - AimTarget_Data[AimTarget_Data.Count - 2].X)*60; double EffectPitchVelocity = (AimTarget_Data[AimTarget_Data.Count - 1].Y - AimTarget_Data[AimTarget_Data.Count - 2].Y)*60; if(EffectYawVelocity*TargetYawAngle < 0){ if(Math.Abs(TargetYawAngle) > AimTargetRatio){ //当前速度正朝着目标过去 //刹车所需时间 double StopTime = Math.Abs(EffectYawVelocity / MaxAngleAcceleration.X); //当前速度抵达目标所需时间 double ArriveTime = Math.Abs(TargetYawAngle / EffectYawVelocity); //比较两个时间大小决定减速或加速 if(StopTime <= ArriveTime){ //如果刹车时间足够，全力推进 yaw = 60 * Math.Abs(TargetYawAngle) / TargetYawAngle; }else{ yaw = -60 * Math.Abs(TargetYawAngle) / TargetYawAngle; } }else{ yaw = TargetYawAngle*60/180; } }else{ //当前速度正背离目标 yaw = 60 * Math.Abs(TargetYawAngle) / TargetYawAngle; } if(EffectPitchVelocity*TargetPitchAngle < 0){ if(Math.Abs(TargetPitchAngle) > AimTargetRatio){ //当前速度正朝着目标过去 //刹车所需时间 double StopTime = Math.Abs(EffectPitchVelocity / MaxAngleAcceleration.Y); //当前速度抵达目标所需时间 double ArriveTime = Math.Abs(TargetPitchAngle / EffectPitchVelocity); //比较两个时间大小决定减速或加速 if(StopTime <= ArriveTime){ //如果刹车时间足够，全力推进 pitch = -60 * Math.Abs(TargetPitchAngle) / TargetPitchAngle; }else{ pitch = 60 * Math.Abs(TargetPitchAngle) / TargetPitchAngle; } }else{ pitch = -TargetPitchAngle*60/180; } }else{ //当前速度正背离目标 pitch = -60 * Math.Abs(TargetPitchAngle) / TargetPitchAngle; } gys.Yaw = yaw; gys.Pitch = pitch; gys.Roll = -cp.InputRoll*40; gys.SetOverride(true); } ``` 上述算法区别在于不是采用自己的角速度来判断刹车距离，而是采用合速度。 合速度可以直接根据上一帧和这一帧的目标方位角的差求出 使用合速度来判断，不会产生动态误差，但会在小角度上震荡（震荡也不算太严重） <br> ## 总结 在精确求解的算法下，实际上做了如下几个重要判断 1. 判断 **速度.a** 是否与目标方位角同向，即这个速度是在扩大与目标的方位角还是减小。如果是扩大，就立刻全力反向出力，如果是减小，那就做第二步判断 2. 用 **速度.b** 和最大加速度判断刹车时间，再用 **速度.c** 与方位角的差判断抵达时间。 3. 比较刹车时间和抵达时间，如果刹车时间更大，说明刹车距离已经不够，此时全力反向出力，以避免超越。如果刹车时间小，说明还可以继续全力正向出力。 <br> 系统可以获取3种速度： 1. 自己的角速度 2. 合角速度 3. 目标的绝对角速度 使用这三种速度分别代入上述两个判断的不同步骤中测试结果 1. 全部采用自己的速度：无震荡，有无法控制的自增动态误差 2. 全部采用合速度：有细微震荡，完全无动态误差 3. a使用自己的速度，bc使用合速度：无细微震荡，有可控的动态误差，但动态误差情况下会出现大幅度震荡 4. a使用合速度，b使用自己的速度，c采用合速度：结果和1相似，几乎没区别 5. a使用合速度，b使用合速度，c采用自己的速度：结果和1相似