510 lines
22 KiB
C#
510 lines
22 KiB
C#
namespace RobotApp.Services.Robot.Simulation.Navigation.Algorithm;
|
||
|
||
public class FuzzyLogic
|
||
{
|
||
// ============================================================================
|
||
// 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)
|
||
// ============================================================================
|
||
|
||
// ============================================================================
|
||
// PHẦN 1: CÁC HÀM MEMBERSHIP (HÀM THUỘC)
|
||
// ============================================================================
|
||
|
||
/// <summary>
|
||
/// 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
|
||
///
|
||
/// </summary>
|
||
/// <param name="inputValue">Giá trị đầu vào cần tính độ thuộc</param>
|
||
/// <param name="left_base">Điểm bắt đầu hình thang (μ = 0)</param>
|
||
/// <param name="left_top">Điểm bắt đầu vùng phẳng trên (μ = 1)</param>
|
||
/// <param name="right_top">Điểm kết thúc vùng phẳng trên (μ = 1)</param>
|
||
/// <param name="right_base">Điểm kết thúc hình thang (μ = 0)</param>
|
||
/// <returns>Độ thuộc trong khoảng [0, 1]</returns>
|
||
private static double Fuzzy_trapmf(double inputValue, double left_base, double left_top, double right_top, double right_base)
|
||
{
|
||
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;
|
||
}
|
||
|
||
/// <summary>
|
||
/// 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
|
||
///
|
||
/// </summary>
|
||
/// <param name="inputValue">Giá trị đầu vào cần tính độ thuộc</param>
|
||
/// <param name="left">Điểm bắt đầu tam giác (μ = 0)</param>
|
||
/// <param name="peak">Điểm đỉnh tam giác (μ = 1)</param>
|
||
/// <param name="right">Điểm kết thúc tam giác (μ = 0)</param>
|
||
/// <returns>Độ thuộc trong khoảng [0, 1]</returns>
|
||
private static double Fuzzy_trimf(double inputValue, double left, double peak, double right)
|
||
{
|
||
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;
|
||
}
|
||
|
||
// ============================================================================
|
||
// PHẦN 2: FUZZIFICATION (MỜ HÓA ĐẦU VÀO)
|
||
// ============================================================================
|
||
|
||
/// <summary>
|
||
/// Mờ hóa tín hiệu PI thành 5 tập mờ
|
||
/// </summary>
|
||
/// <param name="piSignal">Tín hiệu đầu ra của bộ PI</param>
|
||
/// <param name="membershipValues">Mảng lưu giá trị độ thuộc (vị trí 0-4)</param>
|
||
private static void FuzzifyPISignal(double piSignal, double[] membershipValues)
|
||
{
|
||
// 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);
|
||
}
|
||
|
||
/// <summary>
|
||
/// Mờ hóa vận tốc V thành 9 tập mờ (dải 0.0 - 2.0)
|
||
/// </summary>
|
||
/// <param name="velocity">Vận tốc tuyến tính mong muốn (0.0 - 2.0 m/s)</param>
|
||
/// <param name="membershipValues">Mảng lưu giá trị độ thuộc (vị trí 5-13)</param>
|
||
private static void FuzzifyVelocity(double velocity, double[] membershipValues)
|
||
{
|
||
// 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);
|
||
}
|
||
|
||
// ============================================================================
|
||
// PHẦN 3: RULE EVALUATION (ĐÁNH GIÁ LUẬT MỜ)
|
||
// ============================================================================
|
||
|
||
/// <summary>
|
||
/// Đánh giá luật mờ cho một bộ điều khiển
|
||
/// </summary>
|
||
/// <param name="inputMembershipValues">Độ thuộc của các đầu vào (14 giá trị: 5 PI + 9 V)</param>
|
||
/// <param name="ruleAntecedentIndices">Ma trận chỉ số tiền đề (numRules * 2 phần tử)</param>
|
||
/// <param name="ruleConsequentIndices">Ma trận chỉ số hệ quả (numRules phần tử)</param>
|
||
/// <param name="outputSingletons">Các giá trị singleton đầu ra</param>
|
||
/// <param name="numRules">Số lượng luật</param>
|
||
/// <returns>(weightedSum: tổng có trọng số, totalWeight: tổng trọng số)</returns>
|
||
private static (double weightedSum, double totalWeight) EvaluateRules(
|
||
double[] inputMembershipValues,
|
||
byte[] ruleAntecedentIndices,
|
||
byte[] ruleConsequentIndices,
|
||
double[] outputSingletons,
|
||
int numRules)
|
||
{
|
||
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++)
|
||
{
|
||
// 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;
|
||
}
|
||
|
||
return (weightedSum, totalWeight);
|
||
}
|
||
|
||
// ============================================================================
|
||
// PHẦN 4: DEFUZZIFICATION (GIẢI MỜ ĐẦU RA)
|
||
// ============================================================================
|
||
|
||
/// <summary>
|
||
/// 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)
|
||
/// </summary>
|
||
/// <param name="weightedSum">Tổng đầu ra có trọng số</param>
|
||
/// <param name="totalWeight">Tổng trọng số của tất cả các luật</param>
|
||
/// <param name="defaultValue">Giá trị mặc định nếu totalWeight = 0</param>
|
||
/// <returns>Giá trị đầu ra rõ (crisp output)</returns>
|
||
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)
|
||
{
|
||
return defaultValue;
|
||
}
|
||
else
|
||
{
|
||
// Tính trọng tâm: output = weightedSum / totalWeight
|
||
return weightedSum / totalWeight;
|
||
}
|
||
}
|
||
|
||
// ============================================================================
|
||
// PHẦN 5: BẢNG LUẬT VÀ CẤU HÌNH
|
||
// ============================================================================
|
||
|
||
/// <summary>
|
||
/// 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
|
||
/// </summary>
|
||
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
|
||
];
|
||
|
||
/// <summary>
|
||
/// 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)
|
||
/// </summary>
|
||
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
|
||
];
|
||
|
||
/// <summary>
|
||
/// 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
|
||
/// </summary>
|
||
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
|
||
];
|
||
|
||
/// <summary>
|
||
/// 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)
|
||
/// </summary>
|
||
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
|
||
];
|
||
|
||
/// <summary>
|
||
/// 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ũ)
|
||
/// </summary>
|
||
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)
|
||
|
||
/// <summary>
|
||
/// 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ũ)
|
||
/// </summary>
|
||
/// <param name="desiredVelocity">Vận tốc tuyến tính mong muốn (0.0 - 2.0 m/s)</param>
|
||
/// <param name="desiredAngularVelocity">Vận tốc góc mong muốn (rad/s)</param>
|
||
/// <param name="samplingTime">Chu kỳ lấy mẫu (giây)</param>
|
||
/// <returns>(leftWheelSpeed, rightWheelSpeed): Tốc độ bánh trái và phải trong [0.0, 2.0]</returns>
|
||
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Ợ
|
||
// ============================================================================
|
||
|
||
/// <summary>
|
||
/// 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
|
||
/// </summary>
|
||
public void ResetIntegrator()
|
||
{
|
||
integratorState = 0.0;
|
||
}
|
||
|
||
/// <summary>
|
||
/// Thiết lập hệ số cho bộ điều khiển PI
|
||
/// </summary>
|
||
/// <param name="kp">Hệ số tỷ lệ (Proportional Gain) - Phản ứng với sai số hiện tại</param>
|
||
/// <param name="ki">Hệ số tích phân (Integral Gain) - Loại bỏ sai số tích lũy</param>
|
||
public FuzzyLogic WithPIGains(double kp, double ki)
|
||
{
|
||
proportionalGain = kp;
|
||
integralGain = ki;
|
||
return this;
|
||
}
|
||
|
||
/// <summary>
|
||
/// 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
|
||
/// </summary>
|
||
/// <returns>Giá trị tích phân hiện tại</returns>
|
||
public double GetIntegratorState()
|
||
{
|
||
return integratorState;
|
||
}
|
||
|
||
/// <summary>
|
||
/// Lấy hệ số PI hiện tại
|
||
/// </summary>
|
||
/// <returns>(Kp, Ki): Hệ số tỷ lệ và tích phân</returns>
|
||
public (double Kp, double Ki) GetPIGains()
|
||
{
|
||
return (proportionalGain, integralGain);
|
||
}
|
||
}
|