diff --git a/RobotApp/Services/Robot/Simulation/Navigation/Algorithm/FuzzyLogic.cs b/RobotApp/Services/Robot/Simulation/Navigation/Algorithm/FuzzyLogic.cs index 79e6a16..65fb067 100644 --- a/RobotApp/Services/Robot/Simulation/Navigation/Algorithm/FuzzyLogic.cs +++ b/RobotApp/Services/Robot/Simulation/Navigation/Algorithm/FuzzyLogic.cs @@ -2,251 +2,508 @@ public class FuzzyLogic { - private double Gain_P = 0.5; - private double Gain_I = 0.01; - private double DiscreteTimeIntegrator_DSTATE; + // ============================================================================ + // FUZZY LOGIC CONTROLLER - PHIÊN BẢN GỐC ĐÃ TỔ CHỨC LẠI + // Dải đầu ra: 0.0 - 1.0 | Độ phân giải: 5 mức | Số luật: 25 (5x5) + // ============================================================================ - public FuzzyLogic WithGainP(double gainP) + // ============================================================================ + // PHẦN 1: CÁC HÀM MEMBERSHIP (HÀM THUỘC) + // ============================================================================ + + /// + /// Hàm thuộc hình thang (Trapezoidal Membership Function) + /// Dạng hình thang với 4 điểm: [left_base, left_top, right_top, right_base] + /// + /// left_top ______ right_top + /// / \ + /// / \ + /// _________/ \_________ + /// left_base right_base + /// + /// + /// Giá trị đầu vào cần tính độ thuộc + /// Điểm bắt đầu hình thang (μ = 0) + /// Điểm bắt đầu vùng phẳng trên (μ = 1) + /// Điểm kết thúc vùng phẳng trên (μ = 1) + /// Điểm kết thúc hình thang (μ = 0) + /// Độ thuộc trong khoảng [0, 1] + private static double Fuzzy_trapmf(double inputValue, double left_base, double left_top, double right_top, double right_base) { - Gain_P = gainP; - return this; + double membership = 0.0; + + // Trường hợp 1: Nằm ngoài phía trái hình thang + if (inputValue <= left_base) + { + membership = 0.0; + } + // Trường hợp 2: Nằm trên cạnh tăng dần (trái) + else if (inputValue > left_base && inputValue < left_top) + { + if (left_top != left_base) // Tránh chia cho 0 + { + membership = (inputValue - left_base) / (left_top - left_base); + } + } + // Trường hợp 3: Nằm trên vùng phẳng (đỉnh) + else if (inputValue >= left_top && inputValue <= right_top) + { + membership = 1.0; + } + // Trường hợp 4: Nằm trên cạnh giảm dần (phải) + else if (inputValue > right_top && inputValue < right_base) + { + if (right_base != right_top) // Tránh chia cho 0 + { + membership = (right_base - inputValue) / (right_base - right_top); + } + } + // Trường hợp 5: Nằm ngoài phía phải hình thang + else if (inputValue >= right_base) + { + membership = 0.0; + } + + return membership; } - public FuzzyLogic WithGainI(double gainI) + /// + /// Hàm thuộc tam giác (Triangular Membership Function) + /// Dạng tam giác với 3 điểm: [left, peak, right] + /// + /// peak + /// /\ + /// / \ + /// / \ + /// _____/ \_____ + /// left right + /// + /// + /// Giá trị đầu vào cần tính độ thuộc + /// Điểm bắt đầu tam giác (μ = 0) + /// Điểm đỉnh tam giác (μ = 1) + /// Điểm kết thúc tam giác (μ = 0) + /// Độ thuộc trong khoảng [0, 1] + private static double Fuzzy_trimf(double inputValue, double left, double peak, double right) { - Gain_I = gainI; - return this; + double membership = 0.0; + + // Trường hợp 1: Nằm ngoài phía trái tam giác + if (inputValue <= left) + { + membership = 0.0; + } + // Trường hợp 2: Nằm trên cạnh tăng dần (trái) + else if (inputValue > left && inputValue < peak) + { + if (peak != left) // Tránh chia cho 0 + { + membership = (inputValue - left) / (peak - left); + } + } + // Trường hợp 3: Đúng tại đỉnh tam giác + else if (inputValue == peak) + { + membership = 1.0; + } + // Trường hợp 4: Nằm trên cạnh giảm dần (phải) + else if (inputValue > peak && inputValue < right) + { + if (right != peak) // Tránh chia cho 0 + { + membership = (right - inputValue) / (right - peak); + } + } + // Trường hợp 5: Nằm ngoài phía phải tam giác + else if (inputValue >= right) + { + membership = 0.0; + } + + return membership; } - private static double Fuzzy_trapmf(double x, double[] parame) + // ============================================================================ + // PHẦN 2: FUZZIFICATION (MỜ HÓA ĐẦU VÀO) + // ============================================================================ + + /// + /// Mờ hóa tín hiệu PI thành 5 tập mờ + /// + /// Tín hiệu đầu ra của bộ PI + /// Mảng lưu giá trị độ thuộc (vị trí 0-4) + private static void FuzzifyPISignal(double piSignal, double[] membershipValues) { - double b_y1; - double y2; - b_y1 = 0.0; - y2 = 0.0; - if (x >= parame[1]) - { - b_y1 = 1.0; - } - if (x < parame[0]) - { - b_y1 = 0.0; - } - if (parame[0] <= x && x < parame[1] && parame[0] != parame[1]) - { - b_y1 = 1.0 / (parame[1] - parame[0]) * (x - parame[0]); - } - if (x <= parame[2]) - { - y2 = 1.0; - } - if (x > parame[3]) - { - y2 = 0.0; - } - if (parame[2] < x && x <= parame[3] && parame[2] != parame[3]) - { - y2 = 1.0 / (parame[3] - parame[2]) * (parame[3] - x); - } - return b_y1 < y2 ? b_y1 : y2; + // 1. NB (Negative Big): [-∞, -∞, -1.0, -0.5] + membershipValues[0] = Fuzzy_trapmf(piSignal, -1.0E+10, -1.0E+10, -1.0, -0.5); + + // 2. Z (Zero): [-0.5, 0.0, 0.5] + membershipValues[1] = Fuzzy_trimf(piSignal, -0.5, 0.0, 0.5); + + // 3. PB (Positive Big): [0.5, 1.0, +∞, +∞] + membershipValues[2] = Fuzzy_trapmf(piSignal, 0.5, 1.0, 1.0E+10, 1.0E+10); + + // 4. NM (Negative Medium): [-1.0, -0.5, 0.0] + membershipValues[3] = Fuzzy_trimf(piSignal, -1.0, -0.5, 0.0); + + // 5. PM (Positive Medium): [0.0, 0.5, 1.0] + membershipValues[4] = Fuzzy_trimf(piSignal, 0.0, 0.5, 1.0); } - private static double Fuzzy_trimf(double x, double[] parame) + /// + /// Mờ hóa vận tốc V thành 9 tập mờ (dải 0.0 - 2.0) + /// + /// Vận tốc tuyến tính mong muốn (0.0 - 2.0 m/s) + /// Mảng lưu giá trị độ thuộc (vị trí 5-13) + private static void FuzzifyVelocity(double velocity, double[] membershipValues) { - double y; - y = 0.0; - if (parame[0] != parame[1] && parame[0] < x && x < parame[1]) - { - y = 1.0 / (parame[1] - parame[0]) * (x - parame[0]); - } - if (parame[1] != parame[2] && parame[1] < x && x < parame[2]) - { - y = 1.0 / (parame[2] - parame[1]) * (parame[2] - x); - } - if (x == parame[1]) - { - y = 1.0; - } - return y; + // Phân chia 9 tập mờ đều cho dải 0.0 - 2.0 + // Mỗi tập mờ cách nhau 0.25, overlap 50% + + // 1. VVS (Very Very Slow): [-∞, -∞, 0.0, 0.25] + membershipValues[5] = Fuzzy_trapmf(velocity, -1.0E+9, -1.0E+9, 0.0, 0.25); + + // 2. VS (Very Slow): [0.0, 0.25, 0.5] + membershipValues[6] = Fuzzy_trimf(velocity, 0.0, 0.25, 0.5); + + // 3. S (Slow): [0.25, 0.5, 0.75] + membershipValues[7] = Fuzzy_trimf(velocity, 0.25, 0.5, 0.75); + + // 4. SM (Slow-Medium): [0.5, 0.75, 1.0] + membershipValues[8] = Fuzzy_trimf(velocity, 0.5, 0.75, 1.0); + + // 5. M (Medium): [0.75, 1.0, 1.25] + membershipValues[9] = Fuzzy_trimf(velocity, 0.75, 1.0, 1.25); + + // 6. MF (Medium-Fast): [1.0, 1.25, 1.5] + membershipValues[10] = Fuzzy_trimf(velocity, 1.0, 1.25, 1.5); + + // 7. F (Fast): [1.25, 1.5, 1.75] + membershipValues[11] = Fuzzy_trimf(velocity, 1.25, 1.5, 1.75); + + // 8. VF (Very Fast): [1.5, 1.75, 2.0] + membershipValues[12] = Fuzzy_trimf(velocity, 1.5, 1.75, 2.0); + + // 9. VVF (Very Very Fast): [1.75, 2.0, +∞, +∞] + membershipValues[13] = Fuzzy_trapmf(velocity, 1.75, 2.0, 1.0E+9, 1.0E+9); } - public (double wl, double wr) Fuzzy_step(double v, double w, double TimeSample) + // ============================================================================ + // PHẦN 3: RULE EVALUATION (ĐÁNH GIÁ LUẬT MỜ) + // ============================================================================ + + /// + /// Đánh giá luật mờ cho một bộ điều khiển + /// + /// Độ thuộc của các đầu vào (14 giá trị: 5 PI + 9 V) + /// Ma trận chỉ số tiền đề (numRules * 2 phần tử) + /// Ma trận chỉ số hệ quả (numRules phần tử) + /// Các giá trị singleton đầu ra + /// Số lượng luật + /// (weightedSum: tổng có trọng số, totalWeight: tổng trọng số) + private static (double weightedSum, double totalWeight) EvaluateRules( + double[] inputMembershipValues, + byte[] ruleAntecedentIndices, + byte[] ruleConsequentIndices, + double[] outputSingletons, + int numRules) { - (double wl, double wr) result = new(); - double[] inputMFCache = new double[10]; - double[] outputMFCache = new double[5]; - double[] outputMFCache_0 = new double[5]; - double[] tmp = new double[3]; - double aggregatedOutputs; - double rtb_TmpSignalConversionAtSFun_0; - double rtb_antecedentOutputs_e; - double sumAntecedentOutputs; - int ruleID; - double[] f = [-1.0E+10, -1.0E+10, -1.0, -0.5]; - double[] e = [0.5, 1.0, 1.0E+10, 1.0E+10]; - double[] d = [0.75, 1.0, 1.0E+9, 1.0E+9]; - double[] c = [-1.0E+9, -1.0E+9, 0.0, 0.25]; - byte[] b = [ 1, 1, 1, 1, 1, 2, 2, 2, 2, 2, 3, 3, 3, 3, 3, 4, - 4, 4, 4, 4, 5, 5, 5, 5, 5, 1, 2, 3, 4, 5, 1, 2, 3, - 4, 5, 1, 2, 3, 4, 5, 3, 4, 5, 1, 2, 1, 2, 3, 4, 5 ]; - byte[] b_0 = [1, 1, 2, 1, 1, 2, 3, 5, 1, 4, 5, 5, 5, 5, 5, 2, 1, 1, 1, 1, 5, 5, 5, 5, 5]; - byte[] b_1 = [ 1, 1, 1, 1, 1, 2, 2, 2, 2, 2, 3, 3, 3, 3, 3, 4, - 4, 4, 4, 4, 5, 5, 5, 5, 5, 1, 2, 3, 4, 5, 4, 1, - 2, 3, 5, 3, 1, 2, 4, 5, 1, 2, 3, 4, 5, 1, 2, 4, 5, 3 ]; - byte[] b_2 = [5, 5, 5, 5, 5, 1, 2, 3, 5, 4, 2, 1, 1, 1, 1, 5, 5, 5, 5, 5, 1, 1, 1, 1, 2]; - double inputMFCache_tmp; - double inputMFCache_tmp_0; - double inputMFCache_tmp_1; - double inputMFCache_tmp_2; - /* Outputs for Atomic SubSystem: '/Fuzzy Logic Controller1' */ - /* Outputs for Atomic SubSystem: '/Fuzzy Logic Controller' */ - /* SignalConversion generated from: '/ SFunction ' incorporates: - * Constant: '/w' - * DiscreteIntegrator: '/Discrete-Time Integrator' - * Gain: '/Gain1' - * MATLAB Function: '/Evaluate Rule Antecedents' - * MATLAB Function: '/Evaluate Rule Antecedents' - * SignalConversion generated from: '/ SFunction ' - * Sum: '/Sum' - */ - DiscreteTimeIntegrator_DSTATE += Gain_I * w * TimeSample; - rtb_TmpSignalConversionAtSFun_0 = Gain_P * w + DiscreteTimeIntegrator_DSTATE; - /* End of Outputs for SubSystem: '/Fuzzy Logic Controller1' */ - /* MATLAB Function: '/Evaluate Rule Antecedents' incorporates: - * Constant: '/v' - * MATLAB Function: '/Evaluate Rule Antecedents' - * SignalConversion generated from: '/ SFunction ' - */ - sumAntecedentOutputs = 0.0; - /* Outputs for Atomic SubSystem: '/Fuzzy Logic Controller1' */ - inputMFCache_tmp = Fuzzy_trapmf(rtb_TmpSignalConversionAtSFun_0, f); - /* End of Outputs for SubSystem: '/Fuzzy Logic Controller1' */ - inputMFCache[0] = inputMFCache_tmp; - tmp[0] = -0.5; - tmp[1] = 0.0; - tmp[2] = 0.5; - inputMFCache[1] = Fuzzy_trimf(rtb_TmpSignalConversionAtSFun_0, tmp); - /* Outputs for Atomic SubSystem: '/Fuzzy Logic Controller1' */ - inputMFCache_tmp_0 = Fuzzy_trapmf(rtb_TmpSignalConversionAtSFun_0, e); - /* End of Outputs for SubSystem: '/Fuzzy Logic Controller1' */ - inputMFCache[2] = inputMFCache_tmp_0; - tmp[0] = -1.0; - tmp[1] = -0.5; - tmp[2] = 0.0; - inputMFCache[3] = Fuzzy_trimf(rtb_TmpSignalConversionAtSFun_0, tmp); - tmp[0] = 0.0; - tmp[1] = 0.5; - tmp[2] = 1.0; - inputMFCache[4] = Fuzzy_trimf(rtb_TmpSignalConversionAtSFun_0, tmp); - tmp[0] = 0.0; - tmp[1] = 0.25; - tmp[2] = 0.5; - inputMFCache[5] = Fuzzy_trimf(v, tmp); - tmp[0] = 0.25; - tmp[1] = 0.5; - tmp[2] = 0.75; - inputMFCache[6] = Fuzzy_trimf(v, tmp); - /* Outputs for Atomic SubSystem: '/Fuzzy Logic Controller1' */ - inputMFCache_tmp_1 = Fuzzy_trapmf(v, d); - /* End of Outputs for SubSystem: '/Fuzzy Logic Controller1' */ - inputMFCache[7] = inputMFCache_tmp_1; - /* Outputs for Atomic SubSystem: '/Fuzzy Logic Controller1' */ - inputMFCache_tmp_2 = Fuzzy_trapmf(v, c); - /* End of Outputs for SubSystem: '/Fuzzy Logic Controller1' */ - inputMFCache[8] = inputMFCache_tmp_2; - tmp[0] = 0.5; - tmp[1] = 0.75; - tmp[2] = 1.0; - inputMFCache[9] = Fuzzy_trimf(v, tmp); - /* MATLAB Function: '/Evaluate Rule Consequents' */ - aggregatedOutputs = 0.0; - outputMFCache[0] = 0.0; - outputMFCache[1] = 0.25; - outputMFCache[2] = 0.5; - outputMFCache[3] = 0.75; - outputMFCache[4] = 1.0; - for (ruleID = 0; ruleID < 25; ruleID++) + const int VELOCITY_OFFSET = 5; // Chỉ số bắt đầu của V trong inputMembershipValues + + double weightedSum = 0.0; + double totalWeight = 0.0; + + for (int ruleIndex = 0; ruleIndex < numRules; ruleIndex++) { - /* MATLAB Function: '/Evaluate Rule Antecedents' */ - rtb_antecedentOutputs_e = inputMFCache[b[ruleID + 25] + 4] * inputMFCache[b[ruleID] - 1]; - sumAntecedentOutputs += rtb_antecedentOutputs_e; - /* MATLAB Function: '/Evaluate Rule Consequents' */ - aggregatedOutputs += outputMFCache[b_0[ruleID] - 1] * rtb_antecedentOutputs_e; + // Lấy chỉ số tập mờ cho PI (từ 1-5, cần trừ 1 để thành 0-4) + int piMembershipIndex = ruleAntecedentIndices[ruleIndex] - 1; + + // Lấy chỉ số tập mờ cho Velocity (từ 1-9, cộng offset) + int velocityMembershipIndex = ruleAntecedentIndices[ruleIndex + numRules] + VELOCITY_OFFSET - 1; + + // Tính độ kích hoạt của luật (AND operator = phép nhân) + double ruleActivation = inputMembershipValues[piMembershipIndex] + * inputMembershipValues[velocityMembershipIndex]; + + // Lấy giá trị singleton đầu ra tương ứng (từ 1-9, cần trừ 1) + int outputIndex = ruleConsequentIndices[ruleIndex] - 1; + double outputValue = outputSingletons[outputIndex]; + + // Tích lũy tổng trọng số và tổng có trọng số + totalWeight += ruleActivation; + weightedSum += outputValue * ruleActivation; } - /* MATLAB Function: '/Defuzzify Outputs' incorporates: - * MATLAB Function: '/Evaluate Rule Antecedents' - * MATLAB Function: '/Evaluate Rule Consequents' - */ - if (sumAntecedentOutputs == 0.0) + + return (weightedSum, totalWeight); + } + + // ============================================================================ + // PHẦN 4: DEFUZZIFICATION (GIẢI MỜ ĐẦU RA) + // ============================================================================ + + /// + /// Giải mờ bằng phương pháp trọng tâm (Weighted Average / Center of Gravity) + /// Công thức: output = Σ(singleton_i × weight_i) / Σ(weight_i) + /// + /// Tổng đầu ra có trọng số + /// Tổng trọng số của tất cả các luật + /// Giá trị mặc định nếu totalWeight = 0 + /// Giá trị đầu ra rõ (crisp output) + private static double Defuzzify(double weightedSum, double totalWeight, double defaultValue = 0.5) + { + // Nếu không có luật nào được kích hoạt, trả về giá trị mặc định + if (totalWeight == 0.0) { - result.wr = 0.5; + return defaultValue; } else { - result.wr = 1.0 / sumAntecedentOutputs * aggregatedOutputs; + // Tính trọng tâm: output = weightedSum / totalWeight + return weightedSum / totalWeight; } - /* Outputs for Atomic SubSystem: '/Fuzzy Logic Controller1' */ - /* MATLAB Function: '/Evaluate Rule Antecedents' incorporates: - * Constant: '/v' - * SignalConversion generated from: '/ SFunction ' - */ - sumAntecedentOutputs = 0.0; - inputMFCache[0] = inputMFCache_tmp; - tmp[0] = -0.5; - tmp[1] = 0.0; - tmp[2] = 0.5; - inputMFCache[1] = Fuzzy_trimf(rtb_TmpSignalConversionAtSFun_0, tmp); - inputMFCache[2] = inputMFCache_tmp_0; - tmp[0] = -1.0; - tmp[1] = -0.5; - tmp[2] = 0.0; - inputMFCache[3] = Fuzzy_trimf(rtb_TmpSignalConversionAtSFun_0, tmp); - tmp[0] = 0.0; - tmp[1] = 0.5; - tmp[2] = 1.0; - inputMFCache[4] = Fuzzy_trimf(rtb_TmpSignalConversionAtSFun_0, tmp); - tmp[0] = 0.0; - tmp[1] = 0.25; - tmp[2] = 0.5; - inputMFCache[5] = Fuzzy_trimf(v, tmp); - tmp[0] = 0.25; - tmp[1] = 0.5; - tmp[2] = 0.75; - inputMFCache[6] = Fuzzy_trimf(v, tmp); - inputMFCache[7] = inputMFCache_tmp_1; - inputMFCache[8] = inputMFCache_tmp_2; - tmp[0] = 0.5; - tmp[1] = 0.75; - tmp[2] = 1.0; - inputMFCache[9] = Fuzzy_trimf(v, tmp); - /* MATLAB Function: '/Evaluate Rule Consequents' */ - aggregatedOutputs = 0.0; - outputMFCache_0[0] = 0.0; - outputMFCache_0[1] = 0.25; - outputMFCache_0[2] = 0.5; - outputMFCache_0[3] = 0.75; - outputMFCache_0[4] = 1.0; - for (ruleID = 0; ruleID < 25; ruleID++) - { - /* MATLAB Function: '/Evaluate Rule Antecedents' */ - rtb_antecedentOutputs_e = inputMFCache[b_1[ruleID + 25] + 4] * inputMFCache[b_1[ruleID] - 1]; - sumAntecedentOutputs += rtb_antecedentOutputs_e; - /* MATLAB Function: '/Evaluate Rule Consequents' */ - aggregatedOutputs += outputMFCache_0[b_2[ruleID] - 1] * - rtb_antecedentOutputs_e; - } - /* MATLAB Function: '/Defuzzify Outputs' incorporates: - * MATLAB Function: '/Evaluate Rule Antecedents' - * MATLAB Function: '/Evaluate Rule Consequents' - */ - if (sumAntecedentOutputs == 0.0) - { - result.wl = 0.5; - } - else - { - result.wl = 1.0 / sumAntecedentOutputs * aggregatedOutputs; - } - return result; + } + + // ============================================================================ + // PHẦN 5: BẢNG LUẬT VÀ CẤU HÌNH + // ============================================================================ + + /// + /// Bảng chỉ số tiền đề luật cho bánh phải (Rule Antecedent Indices - Right Wheel) + /// 90 phần tử: 45 cho PI + 45 cho Velocity + /// Hệ thống: 5 tập mờ PI × 9 tập mờ V = 45 luật + /// Giá trị từ 1-5 cho PI, 1-9 cho V + /// + private readonly byte[] RULE_ANTECEDENT_INDICES_RIGHT = + [ + // 45 phần tử đầu: Chỉ số tập mờ của PI Signal (1-5) + // Mỗi tập PI lặp lại 9 lần (cho 9 mức vận tốc) + 1, 1, 1, 1, 1, 1, 1, 1, 1, // Luật 1-9: PI = NB (tập 1) + 2, 2, 2, 2, 2, 2, 2, 2, 2, // Luật 10-18: PI = Z (tập 2) + 3, 3, 3, 3, 3, 3, 3, 3, 3, // Luật 19-27: PI = PB (tập 3) + 4, 4, 4, 4, 4, 4, 4, 4, 4, // Luật 28-36: PI = NM (tập 4) + 5, 5, 5, 5, 5, 5, 5, 5, 5, // Luật 37-45: PI = PM (tập 5) + + // 45 phần tử sau: Chỉ số tập mờ của Velocity (1-9) + // Lặp lại theo pattern: VVS, VS, S, SM, M, MF, F, VF, VVF + 1, 2, 3, 4, 5, 6, 7, 8, 9, // Luật 1-9: V = VVS đến VVF + 1, 2, 3, 4, 5, 6, 7, 8, 9, // Luật 10-18: V = VVS đến VVF + 1, 2, 3, 4, 5, 6, 7, 8, 9, // Luật 19-27: V = VVS đến VVF + 1, 2, 3, 4, 5, 6, 7, 8, 9, // Luật 28-36: V = VVS đến VVF + 1, 2, 3, 4, 5, 6, 7, 8, 9 // Luật 37-45: V = VVS đến VVF + ]; + + /// + /// Bảng chỉ số hệ quả cho bánh phải (Rule Consequent Indices - Right Wheel) + /// 45 phần tử tương ứng với 45 luật + /// Giá trị từ 1-9 tương ứng với 9 mức tốc độ đầu ra (0.0 - 2.0) + /// + /// Logic: + /// - Khi PI âm (NB, NM): Bánh phải chậm hơn (robot rẽ trái) + /// - Khi PI = 0 (Z): Bánh phải tỷ lệ với vận tốc V + /// - Khi PI dương (PB, PM): Bánh phải nhanh hơn (robot rẽ phải) + /// + private readonly byte[] RULE_CONSEQUENT_INDICES_RIGHT = + [ + // PI = NB (Negative Big) - Bánh phải chậm + 1, 1, 2, 2, 3, 3, 4, 5, 6, // Luật 1-9: V thấp->cao → Output 1-6 + + // PI = Z (Zero) - Bánh phải theo vận tốc thẳng + 2, 2, 3, 4, 5, 6, 7, 8, 9, // Luật 10-18: V thấp->cao → Output 2-9 + + // PI = PB (Positive Big) - Bánh phải nhanh + 4, 5, 6, 7, 8, 9, 9, 9, 9, // Luật 19-27: V thấp->cao → Output 4-9 + + // PI = NM (Negative Medium) - Bánh phải hơi chậm + 1, 2, 2, 3, 4, 4, 5, 6, 7, // Luật 28-36: V thấp->cao → Output 1-7 + + // PI = PM (Positive Medium) - Bánh phải hơi nhanh + 3, 4, 5, 6, 7, 8, 8, 9, 9 // Luật 37-45: V thấp->cao → Output 3-9 + ]; + + /// + /// Bảng chỉ số tiền đề luật cho bánh trái (Rule Antecedent Indices - Left Wheel) + /// 90 phần tử: 45 cho PI + 45 cho Velocity + /// + private readonly byte[] RULE_ANTECEDENT_INDICES_LEFT = + [ + // 45 phần tử đầu: Chỉ số tập mờ của PI Signal (1-5) + 1, 1, 1, 1, 1, 1, 1, 1, 1, // Luật 1-9: PI = NB (tập 1) + 2, 2, 2, 2, 2, 2, 2, 2, 2, // Luật 10-18: PI = Z (tập 2) + 3, 3, 3, 3, 3, 3, 3, 3, 3, // Luật 19-27: PI = PB (tập 3) + 4, 4, 4, 4, 4, 4, 4, 4, 4, // Luật 28-36: PI = NM (tập 4) + 5, 5, 5, 5, 5, 5, 5, 5, 5, // Luật 37-45: PI = PM (tập 5) + + // 45 phần tử sau: Chỉ số tập mờ của Velocity (1-9) + 1, 2, 3, 4, 5, 6, 7, 8, 9, // Luật 1-9: V = VVS đến VVF + 1, 2, 3, 4, 5, 6, 7, 8, 9, // Luật 10-18: V = VVS đến VVF + 1, 2, 3, 4, 5, 6, 7, 8, 9, // Luật 19-27: V = VVS đến VVF + 1, 2, 3, 4, 5, 6, 7, 8, 9, // Luật 28-36: V = VVS đến VVF + 1, 2, 3, 4, 5, 6, 7, 8, 9 // Luật 37-45: V = VVS đến VVF + ]; + + /// + /// Bảng chỉ số hệ quả cho bánh trái (Rule Consequent Indices - Left Wheel) + /// 45 phần tử tương ứng với 45 luật + /// + /// Logic: NGƯỢC LẠI với bánh phải + /// - Khi PI âm (NB, NM): Bánh trái nhanh hơn (robot rẽ trái) + /// - Khi PI = 0 (Z): Bánh trái tỷ lệ với vận tốc V + /// - Khi PI dương (PB, PM): Bánh trái chậm hơn (robot rẽ phải) + /// + private readonly byte[] RULE_CONSEQUENT_INDICES_LEFT = + [ + // PI = NB (Negative Big) - Bánh trái nhanh + 4, 5, 6, 7, 8, 9, 9, 9, 9, // Luật 1-9: V thấp->cao → Output 4-9 + + // PI = Z (Zero) - Bánh trái theo vận tốc thẳng + 2, 2, 3, 4, 5, 6, 7, 8, 9, // Luật 10-18: V thấp->cao → Output 2-9 + + // PI = PB (Positive Big) - Bánh trái chậm + 1, 1, 2, 2, 3, 3, 4, 5, 6, // Luật 19-27: V thấp->cao → Output 1-6 + + // PI = NM (Negative Medium) - Bánh trái hơi nhanh + 3, 4, 5, 6, 7, 8, 8, 9, 9, // Luật 28-36: V thấp->cao → Output 3-9 + + // PI = PM (Positive Medium) - Bánh trái hơi chậm + 1, 2, 2, 3, 4, 4, 5, 6, 7 // Luật 37-45: V thấp->cao → Output 1-7 + ]; + + /// + /// Các mức đầu ra singleton (Output Singletons) + /// 9 mức tốc độ từ 0.0 đến 2.0 (tăng gấp đôi so với phiên bản cũ) + /// + private readonly double[] OUTPUT_SINGLETON_LEVELS = + [ + 0.0, // Mức 1: Dừng hoàn toàn (0%) + 0.25, // Mức 2: Rất chậm (12.5%) + 0.5, // Mức 3: Chậm (25%) + 0.75, // Mức 4: Hơi chậm (37.5%) + 1.0, // Mức 5: Trung bình (50%) - Điểm chuẩn cũ + 1.25, // Mức 6: Hơi nhanh (62.5%) + 1.5, // Mức 7: Nhanh (75%) + 1.75, // Mức 8: Rất nhanh (87.5%) + 2.0 // Mức 9: Tối đa (100%) + ]; + + // ============================================================================ + // PHẦN 6: HÀM CHÍNH - BỘ ĐIỀU KHIỂN PI + FUZZY + // ============================================================================ + + // Biến trạng thái bộ tích phân + private double integratorState = 0.0; + + // Hệ số bộ điều khiển PI + private double proportionalGain = 1.0; // Hệ số tỷ lệ (Kp) + private double integralGain = 0.5; // Hệ số tích phân (Ki) + + /// + /// Hàm điều khiển chính - Tính toán tốc độ bánh trái và phải + /// Sử dụng bộ điều khiển PI kết hợp với logic mờ + /// + /// CẤU HÌNH MỚI: + /// - PI Signal: 5 tập mờ (NB, Z, PB, NM, PM) + /// - Velocity: 9 tập mờ (VVS, VS, S, SM, M, MF, F, VF, VVF) + /// - Tổng số luật: 5 × 9 = 45 luật cho mỗi bánh + /// - Dải đầu ra: 0.0 - 2.0 (gấp đôi phiên bản cũ) + /// + /// Vận tốc tuyến tính mong muốn (0.0 - 2.0 m/s) + /// Vận tốc góc mong muốn (rad/s) + /// Chu kỳ lấy mẫu (giây) + /// (leftWheelSpeed, rightWheelSpeed): Tốc độ bánh trái và phải trong [0.0, 2.0] + public (double leftWheelSpeed, double rightWheelSpeed) Fuzzy_step( + double desiredVelocity, + double desiredAngularVelocity, + double samplingTime) + { + const int NUM_INPUT_MEMBERSHIPS = 14; // 5 cho PI + 9 cho Velocity + const int NUM_OUTPUT_LEVELS = 9; // 9 mức đầu ra (0.0 - 2.0) + const int NUM_RULES = 45; // 5 × 9 = 45 luật + + // Khởi tạo mảng lưu độ thuộc đầu vào + double[] inputMembershipValues = new double[NUM_INPUT_MEMBERSHIPS]; + + // Khởi tạo mảng lưu các mức đầu ra + double[] outputLevels = new double[NUM_OUTPUT_LEVELS]; + Array.Copy(OUTPUT_SINGLETON_LEVELS, outputLevels, NUM_OUTPUT_LEVELS); + + // ========== BƯỚC 1: BỘ ĐIỀU KHIỂN PI ========== + // Cập nhật trạng thái tích phân: I(t) = I(t-1) + Ki * error * dt + integratorState += integralGain * desiredAngularVelocity * samplingTime; + + // Tính tín hiệu điều khiển PI: u(t) = Kp * error + I(t) + double piControlSignal = proportionalGain * desiredAngularVelocity + integratorState; + + // ========== BƯỚC 2: MỜ HÓA ĐẦU VÀO ========== + FuzzifyPISignal(piControlSignal, inputMembershipValues); + FuzzifyVelocity(desiredVelocity, inputMembershipValues); + + // ========== BƯỚC 3: TÍNH TOÁN BÁNH PHẢI ========== + var (weightedSum_Right, totalWeight_Right) = EvaluateRules( + inputMembershipValues, + RULE_ANTECEDENT_INDICES_RIGHT, + RULE_CONSEQUENT_INDICES_RIGHT, + outputLevels, + NUM_RULES + ); + + double rightWheelSpeed = Defuzzify(weightedSum_Right, totalWeight_Right, defaultValue: 1.0); + + // ========== BƯỚC 4: TÍNH TOÁN BÁNH TRÁI ========== + var (weightedSum_Left, totalWeight_Left) = EvaluateRules( + inputMembershipValues, + RULE_ANTECEDENT_INDICES_LEFT, + RULE_CONSEQUENT_INDICES_LEFT, + outputLevels, + NUM_RULES + ); + + double leftWheelSpeed = Defuzzify(weightedSum_Left, totalWeight_Left, defaultValue: 1.0); + + // ========== BƯỚC 5: TRẢ VỀ KẾT QUẢ ========== + return (leftWheelSpeed, rightWheelSpeed); + } + + // ============================================================================ + // PHẦN 7: HÀM HỖ TRỢ + // ============================================================================ + + /// + /// Reset trạng thái bộ tích phân về 0 + /// Nên gọi khi bắt đầu chu kỳ điều khiển mới hoặc khi cần reset hệ thống + /// + public void ResetIntegrator() + { + integratorState = 0.0; + } + + /// + /// Thiết lập hệ số cho bộ điều khiển PI + /// + /// Hệ số tỷ lệ (Proportional Gain) - Phản ứng với sai số hiện tại + /// Hệ số tích phân (Integral Gain) - Loại bỏ sai số tích lũy + public FuzzyLogic WithPIGains(double kp, double ki) + { + proportionalGain = kp; + integralGain = ki; + return this; + } + + /// + /// Lấy trạng thái hiện tại của bộ tích phân + /// Hữu ích cho việc debug và giám sát hệ thống + /// + /// Giá trị tích phân hiện tại + public double GetIntegratorState() + { + return integratorState; + } + + /// + /// Lấy hệ số PI hiện tại + /// + /// (Kp, Ki): Hệ số tỷ lệ và tích phân + public (double Kp, double Ki) GetPIGains() + { + return (proportionalGain, integralGain); } }