Beispiel #1
0
        public void CalculateCompensation(double dt, Plane plane, PositionLoop positionLoop, int step_count)
        {
            if (DeckEnable && (plane.l_path > plane.l_path_0 - 1620))
            {
                if (DeckCompensationStartCount < DeckCompensationStartThreshold)
                {
                    DeckCompensationStartCount++;
                }
                else
                {
                    DeckMotionCount++;
                    positionLoop.X1Desired[1] = positionLoop.X1Desired[1] - CurrentDeckControl[step_count];
                }

                // lateral deck motion, new added in mk4.1
                if (DeckCompensationLateralStartCount < DeckCompensationLateralStartThreshold)
                {
                    DeckCompensationLateralStartCount++;
                }
                else
                {
                    DeckMotionLateralCount++;
                    positionLoop.X1Desired[0] = positionLoop.X1Desired[0] + CurrentDeckLateralControl[step_count];
                }
            }

            switch (Configuration.TrajactoryConfig)
            {
            case TrajactoryType.TypeI:
                vector_trac_err = HelperFunction.vector_filed_trac(plane.Position, Position);
                break;

            case TrajactoryType.TypeII:
                vector_trac_err = HelperFunction.vector_filed_trac(plane.Position, Position);
                if (DeckCompensationStartCount > (DeckCompensationStartThreshold - 1))
                {
                    vector_trac_err[1] = vector_trac_err[1] - (-CurrentDeckControl[step_count]);
                    DeriveDeckControl  = ((-CurrentDeckControl[step_count]) - (-CurrentDeckControl[step_count - 1])) / dt;    // current_deck_control向上为正 derive_deck_control向下为正
                }
                else
                {
                    DeriveDeckControl = 0;
                }
                // 横向运动与补偿 new added in mk4.1
                if (DeckCompensationLateralStartCount > (DeckCompensationLateralStartThreshold - 1))
                {
                    vector_trac_err[0]       = vector_trac_err[0] - (CurrentDeckLateralControl[step_count]);
                    DeriveDeckLateralControl = ((CurrentDeckLateralControl[step_count]) - (CurrentDeckLateralControl[step_count - 1])) / dt;     // 向右为正
                }
                else
                {
                    DeriveDeckLateralControl = 0;
                }
                break;

            default:
                break;
            }
        }
Beispiel #2
0
        public void CalculatePrescribedParameter()
        {
            previousX1Desired = X1Desired;

            var current_desired_position = vb.Dense(3, _plane.Position[0]);

            current_desired_position.SetSubVector(
                1, 2, HelperFunction.ideal_path(_plane.Position, _ship.Position, _ship.Theta, _ship.Psi));
            X1Desired = current_desired_position.SubVector(1, 2);
            epp       = X1Desired - _plane.Position.SubVector(1, 2); // 不使用滤波器得到的横向、纵向追踪误差,结果更准确 特别注意,此值用于反步法可能导致未考虑甲板运动补偿

            epp /= Cos(-_ship.Theta + _ship.Psi);
        }
Beispiel #3
0
        public void Reset(Ship ship)
        {
            //var current_position_ship = ship.Position;
            //double ship.Theta = ship.Theta;
            //double ship.Psi = ship.Psi;

            Alpha    = 6.0 * Pi / 180; // 5.0
            Miu      = 0 * Pi / 180;
            Beta     = 0 * Pi / 180;
            Phi      = 0; // 欧拉角
            Psi      = 0;
            P        = 0 * Pi / 180;
            Q        = 0 * Pi / 180;
            R        = 0 * Pi / 180;
            Vk       = 72; // 70
            DeltaA   = 0 * Pi / 180;
            DeltaE   = 0 * Pi / 180;
            DeltaR   = 0 * Pi / 180;
            DeltaP   = 0.100;             // 0.10
            DeltaLEF = 33 * Pi / 180 * 1; // leading - edge flap
            DeltaTEF = 45 * Pi / 180 * 1; // tailing - edge flap

            GammaDerive = 0;
            ChiDerive   = 0;
            Flow        = 0.5 * PlaneInertia.Rou * Math.Pow(Vk, 2);
            T           = PlaneInertia.TMax * DeltaP;

            CalculatePneumaticParameters();
            CalculateForceAndMoment();
            Initialize(ship);
            Theta           = Gamma + Alpha;
            DesiredPosition = vb.Dense(3, Position[0]);
            DesiredPosition.SetSubVector(
                1, 2, HelperFunction.ideal_path(Position, ship.Position, ship.Theta, ship.Psi));
            DeltaTEFDesired = DeltaTEF;

            l_path   = 0;
            l_path_0 = 3500;
        }
Beispiel #4
0
        public Plane(Ship ship)
        {
            PlaneInertia = new()
            {
                Ixx   = 74608.24,// 转动惯量 kg/m2
                Iyy   = 227616.4,
                Izz   = 231114.32,
                Ixz   = 0,
                WingS = 62.006,       // 翼面积
                WingC = 4.218,        // 平均气动弦长
                WingL = 14.70,        // 翼展
                Mass  = 16382.85,     // 空重 kg
                TMax  = 122.6 * 1000, // 军用推力(两台)
                Rou   = 1.225,        // 空气密度
                G     = 9.8
            };
            DesiredParameter = new()
            {
                Alpha       = 10 * Pi / 180,   // 期望迎角 9.1
                Chi         = 0 * Pi / 180,    // 期望航向角
                Gamma       = -3.5 * Pi / 180, // 期望爬升角
                Vk          = 62,              // 期望速度 71
                EngineDelta = 0 * Pi / 180     // 发动机安装角
            };

            Flow            = 0.5 * PlaneInertia.Rou * Math.Pow(Vk, 2);
            T               = PlaneInertia.TMax * DeltaP;
            ThrustRange     = PlaneInertia.TMax * DeltaPRange;
            ThrustRateRange = PlaneInertia.TMax * DeltaPRateRange;
            CalculatePneumaticParameters();
            CalculateForceAndMoment();
            Initialize(ship);
            Theta           = Gamma + Alpha;
            DeltaTEFDesired = DeltaTEF;
            DesiredPosition = vb.Dense(3, Position[0]);
            DesiredPosition.SetSubVector(
                1, 2, HelperFunction.ideal_path(Position, ship.Position, ship.Theta, ship.Psi));
        }

        void AddListeners()
        {
        }

        void Initialize(Ship ship)
        {
            Vector <double> p_d_2p;

            if (l_path_0 > 1620)
            {
                double x_d_2p = -1620 * Cos(ship.Theta) * Cos(ship.Gamma) - (l_path_0 - 1620); // 期望点坐标 P系下表示
                double y_d_2p = 1620 * Sin(ship.Theta) * Cos(ship.Gamma);
                double z_d_2p = 1620 * Sin(ship.Gamma);

                p_d_2p = vb.Dense(new double[] { x_d_2p, y_d_2p, z_d_2p }); // 期望点坐标 P系下表示
            }
            else
            {
                double x_d_2p = -(l_path_0 - l_path) * Cos(ship.Theta) * Cos(ship.Gamma); // 期望点坐标 P系下表示
                double y_d_2p = (l_path_0 - l_path) * Sin(ship.Theta) * Cos(ship.Gamma);
                double z_d_2p = (l_path_0 - l_path) * Sin(ship.Gamma);

                p_d_2p = vb.Dense(new double[] { x_d_2p, y_d_2p, z_d_2p }); // 期望点坐标 P系下表示
            }
            Matrix <double> R_i2p = mb.DenseOfArray(new double[, ] {
                { Cos(ship.Psi), Sin(ship.Psi), 0 }, { -Sin(ship.Psi), Cos(ship.Psi), 0 }, { 0, 0, 1 }
            });
            Matrix <double> R_p2i  = R_i2p.Transpose();
            Vector <double> p_d_2i = R_p2i * p_d_2p + ship.Position; // 期望点坐标 I系下表示

            double kai_f   = 0;
            double gamma_f = 0;

            Matrix <double> R_i2f = mb.DenseOfArray(new double[, ] {
                { Cos(gamma_f) * Cos(kai_f), Cos(gamma_f) * Sin(kai_f), -Sin(gamma_f) },
                { -Sin(kai_f), Cos(kai_f), 0 },
                { Sin(gamma_f) * Cos(kai_f), Sin(gamma_f) * Sin(kai_f), Cos(gamma_f) }
            });
            Matrix <double> R_f2i  = R_i2f.Transpose();
            Vector <double> p_b_2f = vb.Dense(new double[] { 0, 5, 10 }); // 飞机在F系中初始位置

            x_b_2f = p_b_2f[0];
            y_b_2f = p_b_2f[1];
            z_b_2f = p_b_2f[2];

            Position = R_f2i * p_b_2f + p_d_2i; // 飞机在I系中初始位置
            Gamma    = 0;                       // 初始爬升角
            Chi      = -ship.Theta;             // 初始航向角
            Vector <double> omega_d_2f  = R_i2f * ship.omega_d_2i;
            double          omega_dx_2f = omega_d_2f[0];
            double          omega_dy_2f = omega_d_2f[1];
            double          omega_dz_2f = omega_d_2f[2];

            Vector <double> omega_f_2f = vb.Dense(new double[] { omega_dx_2f, omega_dy_2f, omega_dz_2f });

            omega_fx_2f = omega_f_2f[0];
            omega_fy_2f = omega_f_2f[1];
            omega_fz_2f = omega_f_2f[2];

            kai_b2f   = Chi - kai_f;
            gamma_b2f = Gamma - gamma_f;
        }

        public void Record()
        {
            RecordPlaneStateEvent?.Invoke(this, null);
        }

        void CalculatePneumaticParameters()
        {
            double WingC = PlaneInertia.WingC;
            double WingS = PlaneInertia.WingS;
            double WingL = PlaneInertia.WingL;

            double CM_delta_tef      = 0.001 * 180 / Pi;
            double CM_delta_lef      = 0;
            double Cnormal_delta_tef = 0.009 * 180 / Pi;
            double Cnormal_delta_lef = Cnormal_delta_tef * 0.5; // 推测数据,缺少实测数据
            double Caxis_delta_tef   = 0;
            double Caxis_delta_lef   = 0;

            // 升力系数相关参数 弧度制
            double CY_alpha3 = -20.779;
            double CY_alpha2 = 1.1682;
            double CY_alpha1 = 3.8091;
            double CY_0      = 0.12;

            CY_alpha = CY_alpha3 * Math.Pow(Alpha, 2) + CY_alpha2 * Alpha + CY_alpha1;
            double CY_delta_e   = 0.6;
            double CY_delta_lef = 0;

            CY_delta_tef = 0.02 * 57.3;
            CY           = CY_0
                           + CY_alpha * Alpha
                           + CY_delta_e * DeltaE
                           + CY_delta_lef * DeltaLEF  // leading - egde flap && tailing - edge flap
                           + CY_delta_tef * DeltaTEF; // lift coefficient
                                                      // 阻力系数相关参数 弧度制
            double CD_alpha2 = 0.3009;
            double CD_alpha1 = -0.0622;
            double CD_alpha0 = -1.4994;
            double CD_0      = -0.0019 + 0.0109;

            CD_alpha = CD_alpha2 * Alpha + CD_alpha1;
            double CD_delta_e   = 0.04;
            double CD_delta_lef = 0;
            double CD_delta_tef = 0.002 * 57.3;
            double CD_Ma;

            if (Vk / 340 <= 0.81)
            {
                CD_Ma = 0;
            }
            else
            {
                CD_Ma = 0.1007 * Math.Pow(Vk / 340, 3) - 0.4653 * Math.Pow(Vk / 340, 2) + 0.6903 * (Vk / 340) - 0.3074;
            }
            double CD_beta1 = 0.4946;
            double CD_beta  = CD_beta1 * Beta;

            CD = CD_0
                 + CD_beta * Beta                // 侧滑阻力
                 + CD_alpha * Alpha              // 迎角引起的阻力
                 + CD_delta_e * Math.Abs(DeltaE) // 升降舵偏转引起的阻力。
                 + CD_delta_lef * DeltaLEF       // leading - egde flap引起的阻力认为是0 && tailing - edge flap
                 + CD_delta_tef * DeltaTEF       // 襟翼引起的阻力
                 + 0.1 * Math.Pow(CY, 2)         // 升致阻力
                 + CD_Ma;                        // 马赫导致的阻力 // drag coefficient

            // 侧力系数 弧度制
            CC_beta = -1;
            CC      = CC_beta * Beta;

            // 滚转力矩相关参数 弧度制
            CL_beta    = -0.1;
            CL_delta_a = 0.12;
            CL_delta_r = 0.01;
            double CL_p = -0.4;
            double CL_r = 0.15;

            CL = CL_beta * Beta          // beta引起的滚转力矩
                 + CL_delta_a * DeltaA
                 + CL_delta_r * DeltaR
                 + WingL / 2 / Vk * CL_p * P
                 + WingL / 2 / Vk * CL_r * R; // rolling moment coefficient

            // 俯仰力矩相关参数 弧度制
            CM_delta_e = 0.35 * (Vk / 340) - 1.1;
            double CM_q = -23;

            CM_alpha = -0.1;
            double CM_alpha_dot = -9;

            CM = CM_alpha * Alpha                  // 由alpha引起的俯仰力矩
                 + CM_delta_e * DeltaE             // 升降舵产生的俯仰力矩
                 + WingC / 2 / Vk * CM_q * Q;      // 俯仰角速度产生的俯仰力矩


            // 偏航力矩相关参数 弧度制
            CN_beta    = 0.12;
            CN_delta_r = -0.15;
            double CN_r = -0.15;

            CN_delta_a = 0;
            CN         = CN_beta * Beta
                         + CN_delta_r * DeltaR
                         + WingL / 2 / Vk * CN_r * R; // yawing moment coefficient
        }

        void CalculateForceAndMoment()
        {
            Y = Flow * PlaneInertia.WingS * CY;
            D = Flow * PlaneInertia.WingS * CD;
            C = Flow * PlaneInertia.WingS * CC;
            L = Flow * PlaneInertia.WingS * PlaneInertia.WingL * CL;
            M = Flow * PlaneInertia.WingS * PlaneInertia.WingC * CM;
            N = Flow * PlaneInertia.WingS * PlaneInertia.WingL * CN;
        }