Initial commit
This commit is contained in:
@@ -0,0 +1,538 @@
|
||||
namespace RobotNet10.RobotApp.Services.Simulation.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 11 tập mờ (dải 0.0 - 3.0)
|
||||
/// Độ phân giải: 0.3 m/s - Cân bằng tốt giữa độ chính xác và hiệu suất
|
||||
/// </summary>
|
||||
/// <param name="velocity">Vận tốc tuyến tính mong muốn (0.0 - 3.0 m/s)</param>
|
||||
/// <param name="membershipValues">Mảng lưu giá trị độ thuộc (vị trí 5-15)</param>
|
||||
private static void FuzzifyVelocity(double velocity, double[] membershipValues)
|
||||
{
|
||||
// 11 tập mờ phân bố đều từ 0.0 đến 3.0
|
||||
// Khoảng cách giữa các đỉnh: 3.0 / 10 = 0.3 m/s
|
||||
|
||||
// 1. VVS (Very Very Slow): [-∞, -∞, 0.0, 0.3]
|
||||
membershipValues[5] = Fuzzy_trapmf(velocity, -1.0E+9, -1.0E+9, 0.0, 0.3);
|
||||
|
||||
// 2. VS (Very Slow): [0.0, 0.3, 0.6]
|
||||
membershipValues[6] = Fuzzy_trimf(velocity, 0.0, 0.3, 0.6);
|
||||
|
||||
// 3. S- (Slow Low): [0.3, 0.6, 0.9]
|
||||
membershipValues[7] = Fuzzy_trimf(velocity, 0.3, 0.6, 0.9);
|
||||
|
||||
// 4. S (Slow): [0.6, 0.9, 1.2]
|
||||
membershipValues[8] = Fuzzy_trimf(velocity, 0.6, 0.9, 1.2);
|
||||
|
||||
// 5. S+ (Slow High): [0.9, 1.2, 1.5]
|
||||
membershipValues[9] = Fuzzy_trimf(velocity, 0.9, 1.2, 1.5);
|
||||
|
||||
// 6. M (Medium): [1.2, 1.5, 1.8]
|
||||
membershipValues[10] = Fuzzy_trimf(velocity, 1.2, 1.5, 1.8);
|
||||
|
||||
// 7. F- (Fast Low): [1.5, 1.8, 2.1]
|
||||
membershipValues[11] = Fuzzy_trimf(velocity, 1.5, 1.8, 2.1);
|
||||
|
||||
// 8. F (Fast): [1.8, 2.1, 2.4]
|
||||
membershipValues[12] = Fuzzy_trimf(velocity, 1.8, 2.1, 2.4);
|
||||
|
||||
// 9. F+ (Fast High): [2.1, 2.4, 2.7]
|
||||
membershipValues[13] = Fuzzy_trimf(velocity, 2.1, 2.4, 2.7);
|
||||
|
||||
// 10. VF (Very Fast): [2.4, 2.7, 3.0]
|
||||
membershipValues[14] = Fuzzy_trimf(velocity, 2.4, 2.7, 3.0);
|
||||
|
||||
// 11. VVF (Very Very Fast): [2.7, 3.0, +∞, +∞]
|
||||
membershipValues[15] = Fuzzy_trapmf(velocity, 2.7, 3.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 (16 giá trị: 5 PI + 11 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-11, 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-11, 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)
|
||||
/// 110 phần tử: 55 cho PI + 55 cho Velocity
|
||||
/// Hệ thống: 5 tập mờ PI × 11 tập mờ V = 55 luật
|
||||
/// Giá trị từ 1-5 cho PI, 1-11 cho V
|
||||
/// </summary>
|
||||
private static readonly byte[] RULE_ANTECEDENT_INDICES_RIGHT =
|
||||
[
|
||||
// 55 phần tử đầu: Chỉ số tập mờ của PI Signal (1-5)
|
||||
// Mỗi tập PI lặp lại 11 lần (cho 11 mức vận tốc)
|
||||
1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, // Luật 1-11: PI = NB (tập 1)
|
||||
2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, // Luật 12-22: PI = Z (tập 2)
|
||||
3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, // Luật 23-33: PI = PB (tập 3)
|
||||
4, 4, 4, 4, 4, 4, 4, 4, 4, 4, 4, // Luật 34-44: PI = NM (tập 4)
|
||||
5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, // Luật 45-55: PI = PM (tập 5)
|
||||
|
||||
// 55 phần tử sau: Chỉ số tập mờ của Velocity (1-11)
|
||||
// Lặp lại theo pattern: VVS, VS, S-, S, S+, M, F-, F, F+, VF, VVF
|
||||
1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, // Luật 1-11: V = VVS đến VVF
|
||||
1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, // Luật 12-22: V = VVS đến VVF
|
||||
1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, // Luật 23-33: V = VVS đến VVF
|
||||
1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, // Luật 34-44: V = VVS đến VVF
|
||||
1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11 // Luật 45-55: V = VVS đến VVF
|
||||
];
|
||||
|
||||
/// <summary>
|
||||
/// Bảng chỉ số hệ quả cho bánh phải (Rule Consequent Indices - Right Wheel)
|
||||
/// 55 phần tử tương ứng với 55 luật
|
||||
/// Giá trị từ 1-11 tương ứng với 11 mức tốc độ đầu ra (0.0 - 3.0)
|
||||
///
|
||||
/// Logic bánh phải:
|
||||
/// - PI âm (NB, NM): Bánh phải chậm hơn → Robot rẽ trái
|
||||
/// - PI = 0 (Z): Bánh phải theo vận tốc V → Robot đi thẳng
|
||||
/// - PI dương (PB, PM): Bánh phải nhanh hơn → Robot rẽ phải
|
||||
/// </summary>
|
||||
private static readonly byte[] RULE_CONSEQUENT_INDICES_RIGHT =
|
||||
[
|
||||
// PI = NB (Negative Big) - Bánh phải RẤT CHẬM (rẽ trái mạnh)
|
||||
// V: VVS VS S- S S+ M F- F F+ VF VVF
|
||||
1, 1, 1, 2, 2, 3, 3, 4, 5, 6, 7, // Luật 1-11
|
||||
|
||||
// PI = Z (Zero) - Bánh phải THEO VẬN TỐC (đi thẳng)
|
||||
// V: VVS VS S- S S+ M F- F F+ VF VVF
|
||||
2, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, // Luật 12-22
|
||||
|
||||
// PI = PB (Positive Big) - Bánh phải RẤT NHANH (rẽ phải mạnh)
|
||||
// V: VVS VS S- S S+ M F- F F+ VF VVF
|
||||
4, 5, 6, 7, 8, 9, 10, 10, 11, 11, 11, // Luật 23-33
|
||||
|
||||
// PI = NM (Negative Medium) - Bánh phải HƠI CHẬM (rẽ trái nhẹ)
|
||||
// V: VVS VS S- S S+ M F- F F+ VF VVF
|
||||
1, 2, 2, 3, 4, 5, 6, 7, 8, 9, 10, // Luật 34-44
|
||||
|
||||
// PI = PM (Positive Medium) - Bánh phải HƠI NHANH (rẽ phải nhẹ)
|
||||
// V: VVS VS S- S S+ M F- F F+ VF VVF
|
||||
2, 3, 4, 5, 6, 7, 8, 8, 9, 10, 11 // Luật 45-55
|
||||
];
|
||||
|
||||
/// <summary>
|
||||
/// Bảng chỉ số tiền đề luật cho bánh trái (Rule Antecedent Indices - Left Wheel)
|
||||
/// 110 phần tử: 55 cho PI + 55 cho Velocity
|
||||
/// </summary>
|
||||
private static readonly byte[] RULE_ANTECEDENT_INDICES_LEFT =
|
||||
[
|
||||
// 55 phần tử đầu: Chỉ số tập mờ của PI Signal (1-5)
|
||||
1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, // Luật 1-11: PI = NB (tập 1)
|
||||
2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, // Luật 12-22: PI = Z (tập 2)
|
||||
3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, // Luật 23-33: PI = PB (tập 3)
|
||||
4, 4, 4, 4, 4, 4, 4, 4, 4, 4, 4, // Luật 34-44: PI = NM (tập 4)
|
||||
5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, // Luật 45-55: PI = PM (tập 5)
|
||||
|
||||
// 55 phần tử sau: Chỉ số tập mờ của Velocity (1-11)
|
||||
1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, // Luật 1-11
|
||||
1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, // Luật 12-22
|
||||
1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, // Luật 23-33
|
||||
1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, // Luật 34-44
|
||||
1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11 // Luật 45-55
|
||||
];
|
||||
|
||||
/// <summary>
|
||||
/// Bảng chỉ số hệ quả cho bánh trái (Rule Consequent Indices - Left Wheel)
|
||||
/// 55 phần tử tương ứng với 55 luật
|
||||
///
|
||||
/// Logic bánh trái: NGƯỢC LẠI với bánh phải
|
||||
/// - PI âm (NB, NM): Bánh trái nhanh hơn → Robot rẽ trái
|
||||
/// - PI = 0 (Z): Bánh trái theo vận tốc V → Robot đi thẳng
|
||||
/// - PI dương (PB, PM): Bánh trái chậm hơn → Robot rẽ phải
|
||||
/// </summary>
|
||||
private static readonly byte[] RULE_CONSEQUENT_INDICES_LEFT =
|
||||
[
|
||||
// PI = NB (Negative Big) - Bánh trái RẤT NHANH (rẽ trái mạnh)
|
||||
// V: VVS VS S- S S+ M F- F F+ VF VVF
|
||||
4, 5, 6, 7, 8, 9, 10, 10, 11, 11, 11, // Luật 1-11
|
||||
|
||||
// PI = Z (Zero) - Bánh trái THEO VẬN TỐC (đi thẳng)
|
||||
// V: VVS VS S- S S+ M F- F F+ VF VVF
|
||||
2, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, // Luật 12-22
|
||||
|
||||
// PI = PB (Positive Big) - Bánh trái RẤT CHẬM (rẽ phải mạnh)
|
||||
// V: VVS VS S- S S+ M F- F F+ VF VVF
|
||||
1, 1, 1, 2, 2, 3, 3, 4, 5, 6, 7, // Luật 23-33
|
||||
|
||||
// PI = NM (Negative Medium) - Bánh trái HƠI NHANH (rẽ trái nhẹ)
|
||||
// V: VVS VS S- S S+ M F- F F+ VF VVF
|
||||
3, 4, 5, 6, 7, 8, 8, 9, 10, 11, 11, // Luật 34-44
|
||||
|
||||
// PI = PM (Positive Medium) - Bánh trái HƠI CHẬM (rẽ phải nhẹ)
|
||||
// V: VVS VS S- S S+ M F- F F+ VF VVF
|
||||
1, 2, 2, 3, 4, 4, 5, 6, 7, 8, 9, // Luật 45-55
|
||||
];
|
||||
|
||||
/// <summary>
|
||||
/// Các mức đầu ra singleton (Output Singletons)
|
||||
/// 11 mức tốc độ từ 0.0 đến 3.0 m/s
|
||||
/// Độ phân giải: 3.0 / 10 = 0.3 m/s
|
||||
/// </summary>
|
||||
private static readonly double[] OUTPUT_SINGLETON_LEVELS =
|
||||
[
|
||||
0.0, // Mức 1: Dừng hoàn toàn (0%)
|
||||
0.3, // Mức 2: Rất chậm (10%)
|
||||
0.6, // Mức 3: Chậm (20%)
|
||||
0.9, // Mức 4: Hơi chậm (30%)
|
||||
1.2, // Mức 5: Chậm vừa (40%)
|
||||
1.5, // Mức 6: Trung bình (50%) - Điểm chuẩn
|
||||
1.8, // Mức 7: Hơi nhanh (60%)
|
||||
2.1, // Mức 8: Nhanh (70%)
|
||||
2.4, // Mức 9: Nhanh vừa (80%)
|
||||
2.7, // Mức 10: Rất nhanh (90%)
|
||||
3.0 // Mức 11: 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 static double integratorState = 0.0;
|
||||
|
||||
// Hệ số bộ điều khiển PI
|
||||
private static double proportionalGain = 1.0; // Hệ số tỷ lệ (Kp)
|
||||
private static double integralGain = 0.1; // 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 PHIÊN BẢN 3.0 - 11 TẬP MỜ:
|
||||
/// - PI Signal: 5 tập mờ (NB, Z, PB, NM, PM)
|
||||
/// - Velocity: 11 tập mờ (VVS, VS, S-, S, S+, M, F-, F, F+, VF, VVF)
|
||||
/// - Tổng số luật: 5 × 11 = 55 luật cho mỗi bánh
|
||||
/// - Dải đầu ra: 0.0 - 3.0 m/s
|
||||
/// - Độ phân giải: 0.3 m/s (tối ưu - cân bằng giữa độ mịn và hiệu suất)
|
||||
/// </summary>
|
||||
/// <param name="desiredVelocity">Vận tốc tuyến tính mong muốn (0.0 - 3.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, khuyến nghị 0.01s)</param>
|
||||
/// <returns>(leftWheelSpeed, rightWheelSpeed): Tốc độ bánh trái và phải trong [0.0, 3.0]</returns>
|
||||
public (double leftWheelSpeed, double rightWheelSpeed) Fuzzy_step(
|
||||
double desiredVelocity,
|
||||
double desiredAngularVelocity,
|
||||
double samplingTime)
|
||||
{
|
||||
const int NUM_INPUT_MEMBERSHIPS = 16; // 5 cho PI + 11 cho Velocity
|
||||
const int NUM_OUTPUT_LEVELS = 11; // 11 mức đầu ra (0.0 - 3.0)
|
||||
const int NUM_RULES = 55; // 5 × 11 = 55 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;
|
||||
|
||||
// Chống bão hòa tích phân (Anti-windup)
|
||||
integratorState = Math.Clamp(integratorState, -1.5, 1.5);
|
||||
|
||||
// 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.5);
|
||||
|
||||
// ========== 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.5);
|
||||
|
||||
// ========== BƯỚC 5: GIỚI HẠN AN TOÀN ==========
|
||||
// Đảm bảo tốc độ không vượt quá giới hạn
|
||||
leftWheelSpeed = Math.Clamp(leftWheelSpeed, 0.0, 3.0);
|
||||
rightWheelSpeed = Math.Clamp(rightWheelSpeed, 0.0, 3.0);
|
||||
|
||||
// ========== BƯỚC 6: 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);
|
||||
}
|
||||
}
|
||||
Reference in New Issue
Block a user