QUYVN/BQP
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Initial commit

Browse Source
This commit is contained in:
QUYVN
2026-07-13 09:25:40 +07:00
parent c08ff54676
commit bccfb156d7
1938 changed files with 641646 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

333
srcs/RobotNet10/RobotApp/RobotNet10.RobotApp/Services/Navigation/CSharp/FuzzyLogic.cs Normal file
View File

@@ -0,0 +1,333 @@
namespace RobotNet10.RobotApp.Services.Navigation.CSharp;
public class FuzzyLogic
{
private double Gain_P = 0.5;
private double Gain_I = 0.01;
private double piIntegratorState; // Trạng thái tích phân của PI controller
// Các tham số cho membership functions hình thang của tín hiệu góc
// Negative Large: [-∞, -∞, -1.0, -0.5]
private static readonly double[] NegativeLargeAngularParams = [-1.0E+10, -1.0E+10, -1.0, -0.5];
// Positive Large: [0.5, 1.0, +∞, +∞]
private static readonly double[] PositiveLargeAngularParams = [0.5, 1.0, 1.0E+10, 1.0E+10];
// Các tham số cho membership functions hình thang của vận tốc
// High Velocity: [0.75, 1.0, +∞, +∞]
private static readonly double[] HighVelocityParams = [0.75, 1.0, 1.0E+9, 1.0E+9];
// Low Velocity: [-∞, -∞, 0.0, 0.25]
private static readonly double[] LowVelocityParams = [-1.0E+9, -1.0E+9, 0.0, 0.25];
// Mảng quy tắc cho bộ điều khiển bánh phải (wr)
// 25 phần tử đầu: chỉ số membership function cho tín hiệu góc (input 1)
// 25 phần tử sau: chỉ số membership function cho vận tốc (input 2)
private static readonly byte[] RightWheelRuleInput1Indices = [ 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 ];
// Mảng quy tắc output cho bánh phải (25 quy tắc)
private static readonly byte[] RightWheelRuleOutputIndices = [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];
// Mảng quy tắc cho bộ điều khiển bánh trái (wl)
private static readonly byte[] LeftWheelRuleInput1Indices = [ 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 ];
private static readonly byte[] LeftWheelRuleOutputIndices = [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];
public void SetGainP(double gainP)
{
Gain_P = gainP;
}
public void SetGainI(double gainI)
{
Gain_I = gainI;
}
/// <summary>
/// Tính toán giá trị membership cho hàm hình thang (trapezoidal membership function).
/// </summary>
/// <param name="inputValue">Giá trị đầu vào cần tính độ thuộc</param>
/// <param name="parameters">Mảng 4 phần tử: [a, b, c, d] trong đó:
/// - a: điểm bắt đầu của cạnh tăng (left foot)
/// - b: điểm bắt đầu của phần phẳng (left shoulder)
/// - c: điểm kết thúc của phần phẳng (right shoulder)
/// - d: điểm kết thúc của cạnh giảm (right foot)</param>
/// <returns>Giá trị membership trong khoảng [0, 1]</returns>
private static double Fuzzy_trapmf(double inputValue, double[] parameters)
{
// Extract các tham số để dễ đọc
double leftFoot = parameters[0]; // a: điểm bắt đầu tăng
double leftShoulder = parameters[1]; // b: điểm bắt đầu phẳng
double rightShoulder = parameters[2]; // c: điểm kết thúc phẳng
double rightFoot = parameters[3]; // d: điểm kết thúc giảm
// Tính giá trị membership từ cạnh trái (từ a đến b)
double leftMembership = 0.0;
if (inputValue < leftFoot)
{
// Ngoài vùng hình thang bên trái
leftMembership = 0.0;
}
else if (inputValue >= leftShoulder)
{
// Trong vùng phẳng bên trái
leftMembership = 1.0;
}
else if (leftFoot != leftShoulder)
{
// Trên cạnh tăng (tính toán tuyến tính từ a đến b)
leftMembership = (inputValue - leftFoot) / (leftShoulder - leftFoot);
}
// Tính giá trị membership từ cạnh phải (từ c đến d)
double rightMembership = 0.0;
if (inputValue <= rightShoulder)
{
// Trong vùng phẳng bên phải
rightMembership = 1.0;
}
else if (inputValue > rightFoot)
{
// Ngoài vùng hình thang bên phải
rightMembership = 0.0;
}
else if (rightShoulder != rightFoot)
{
// Trên cạnh giảm (tính toán tuyến tính từ c đến d)
rightMembership = (rightFoot - inputValue) / (rightFoot - rightShoulder);
}
// Kết quả là giá trị nhỏ hơn để đảm bảo không vượt quá 1.0
return leftMembership < rightMembership ? leftMembership : rightMembership;
}
/// <summary>
/// Tính toán giá trị membership cho hàm tam giác (triangular membership function).
/// </summary>
/// <param name="inputValue">Giá trị đầu vào cần tính độ thuộc</param>
/// <param name="parameters">Mảng 3 phần tử: [a, b, c] trong đó:
/// - a: điểm bắt đầu (left foot)
/// - b: điểm đỉnh (peak) - giá trị membership = 1.0
/// - c: điểm kết thúc (right foot)</param>
/// <returns>Giá trị membership trong khoảng [0, 1]</returns>
private static double Fuzzy_trimf(double inputValue, double[] parameters)
{
// Extract các tham số để dễ đọc
double leftFoot = parameters[0]; // a: điểm bắt đầu
double peak = parameters[1]; // b: điểm đỉnh
double rightFoot = parameters[2]; // c: điểm kết thúc
// Kiểm tra nếu giá trị nằm ngoài vùng tam giác
if (inputValue < leftFoot || inputValue > rightFoot)
{
return 0.0;
}
// Nếu giá trị tại đỉnh, membership = 1.0
if (inputValue == peak)
{
return 1.0;
}
// Tính toán membership trên cạnh tăng (từ a đến b)
if (leftFoot < inputValue && inputValue < peak && leftFoot != peak)
{
return (inputValue - leftFoot) / (peak - leftFoot);
}
// Tính toán membership trên cạnh giảm (từ b đến c)
if (peak < inputValue && inputValue < rightFoot && peak != rightFoot)
{
return (rightFoot - inputValue) / (rightFoot - peak);
}
// Trường hợp đặc biệt: nếu không khớp với điều kiện nào
return 0.0;
}
/// <summary>
/// Tính toán vận tốc bánh trái và bánh phải dựa trên fuzzy logic controller.
/// </summary>
/// <param name="v">Vận tốc tuyến tính (linear velocity) - giá trị chuẩn hóa [0, 1]</param>
/// <param name="w">Vận tốc góc (angular velocity)</param>
/// <param name="timeSample">Thời gian mẫu (sampling time) cho tích phân - phải > 0</param>
/// <returns>Tuple chứa (wl: vận tốc bánh trái, wr: vận tốc bánh phải) - giá trị chuẩn hóa [0, 1]</returns>
/// <exception cref="ArgumentException">Thrown khi timeSample <= 0 hoặc các tham số không hợp lệ</exception>
public (double wl, double wr) Fuzzy_step(double v, double w, double timeSample)
{
// Validation đầu vào
if (timeSample <= 0.0 || double.IsNaN(timeSample) || double.IsInfinity(timeSample))
{
throw new ArgumentException("timeSample must be a positive finite number", nameof(timeSample));
}
if (double.IsNaN(v) || double.IsNaN(w) || double.IsInfinity(v) || double.IsInfinity(w))
{
throw new ArgumentException("Input parameters v and w must be finite numbers", nameof(v));
}
(double wl, double wr) result = new();
// Cache cho các giá trị membership của đầu vào (10 membership functions)
// [0-4]: membership functions cho tín hiệu góc đã xử lý
// [5-9]: membership functions cho vận tốc tuyến tính
double[] inputMembershipValues = new double[10];
// Cache cho các giá trị membership của đầu ra (5 levels: 0.0, 0.25, 0.5, 0.75, 1.0)
double[] outputMembershipValuesRight = new double[5]; // Cho bánh phải (wr)
double[] outputMembershipValuesLeft = new double[5]; // Cho bánh trái (wl)
// Mảng tạm để chứa tham số cho hàm tam giác (3 phần tử: [a, b, c])
double[] triangularParams = new double[3];
// Các biến tạm để cache kết quả fuzzification (tránh tính toán lại)
double negativeLargeMembership;
double positiveLargeMembership;
double highVelocityMembership;
double lowVelocityMembership;
// ========== BƯỚC 1: Xử lý tín hiệu đầu vào bằng PI Controller ==========
// Tích phân vận tốc góc để loại bỏ sai số ổn định
piIntegratorState += Gain_I * w * timeSample;
// Kết hợp thành phần tỷ lệ và tích phân
double piControllerOutput = Gain_P * w + piIntegratorState;
// ========== BƯỚC 2: Fuzzification - Chuyển đổi đầu vào thành độ thuộc ==========
// Tính toán membership values cho tín hiệu góc đã xử lý (5 membership functions)
// MF1: Negative Large (hình thang)
negativeLargeMembership = Fuzzy_trapmf(piControllerOutput, NegativeLargeAngularParams);
inputMembershipValues[0] = negativeLargeMembership;
// MF2: Negative (tam giác: -0.5, 0.0, 0.5)
triangularParams[0] = -0.5;
triangularParams[1] = 0.0;
triangularParams[2] = 0.5;
inputMembershipValues[1] = Fuzzy_trimf(piControllerOutput, triangularParams);
// MF3: Positive Large (hình thang)
positiveLargeMembership = Fuzzy_trapmf(piControllerOutput, PositiveLargeAngularParams);
inputMembershipValues[2] = positiveLargeMembership;
// MF4: Very Negative (tam giác: -1.0, -0.5, 0.0)
triangularParams[0] = -1.0;
triangularParams[1] = -0.5;
triangularParams[2] = 0.0;
inputMembershipValues[3] = Fuzzy_trimf(piControllerOutput, triangularParams);
// MF5: Positive (tam giác: 0.0, 0.5, 1.0)
triangularParams[0] = 0.0;
triangularParams[1] = 0.5;
triangularParams[2] = 1.0;
inputMembershipValues[4] = Fuzzy_trimf(piControllerOutput, triangularParams);
// Tính toán membership values cho vận tốc tuyến tính (5 membership functions)
// MF6: Low (tam giác: 0.0, 0.25, 0.5)
triangularParams[0] = 0.0;
triangularParams[1] = 0.25;
triangularParams[2] = 0.5;
inputMembershipValues[5] = Fuzzy_trimf(v, triangularParams);
// MF7: Medium (tam giác: 0.25, 0.5, 0.75)
triangularParams[0] = 0.25;
triangularParams[1] = 0.5;
triangularParams[2] = 0.75;
inputMembershipValues[6] = Fuzzy_trimf(v, triangularParams);
// MF8: High (hình thang)
highVelocityMembership = Fuzzy_trapmf(v, HighVelocityParams);
inputMembershipValues[7] = highVelocityMembership;
// MF9: Very Low (hình thang)
lowVelocityMembership = Fuzzy_trapmf(v, LowVelocityParams);
inputMembershipValues[8] = lowVelocityMembership;
// MF10: High-Medium (tam giác: 0.5, 0.75, 1.0)
triangularParams[0] = 0.5;
triangularParams[1] = 0.75;
triangularParams[2] = 1.0;
inputMembershipValues[9] = Fuzzy_trimf(v, triangularParams);
// ========== BƯỚC 3: Tính toán vận tốc bánh phải (wr) ==========
// Khởi tạo giá trị membership cho đầu ra (5 mức: 0.0, 0.25, 0.5, 0.75, 1.0)
outputMembershipValuesRight[0] = 0.0;
outputMembershipValuesRight[1] = 0.25;
outputMembershipValuesRight[2] = 0.5;
outputMembershipValuesRight[3] = 0.75;
outputMembershipValuesRight[4] = 1.0;
// Đánh giá 25 quy tắc fuzzy và tính toán defuzzification
double totalRuleActivation = 0.0;
double weightedOutputSum = 0.0;
const int numberOfRules = 25;
for (int ruleIndex = 0; ruleIndex < numberOfRules; ruleIndex++)
{
// Tính độ kích hoạt của quy tắc: product(input1_membership, input2_membership)
// Sử dụng phép nhân (product) thay vì min() cho fuzzy AND operation
// input1: tín hiệu góc (index từ RightWheelRuleInput1Indices[ruleIndex] - 1, vì mảng bắt đầu từ 0)
// input2: vận tốc (index từ RightWheelRuleInput1Indices[ruleIndex + 25] + 4, offset 4 vì vận tốc bắt đầu từ index 5)
int angularSignalIndex = RightWheelRuleInput1Indices[ruleIndex] - 1;
int velocityIndex = RightWheelRuleInput1Indices[ruleIndex + numberOfRules] + 4;
double ruleActivation = inputMembershipValues[velocityIndex] * inputMembershipValues[angularSignalIndex];
totalRuleActivation += ruleActivation;
// Tính tổng có trọng số cho defuzzification (Center of Gravity)
int outputIndex = RightWheelRuleOutputIndices[ruleIndex] - 1;
weightedOutputSum += outputMembershipValuesRight[outputIndex] * ruleActivation;
}
// Defuzzification: Center of Gravity method
if (totalRuleActivation == 0.0)
{
// Nếu không có quy tắc nào được kích hoạt, trả về giá trị mặc định
result.wr = 0.5;
}
else
{
result.wr = weightedOutputSum / totalRuleActivation;
}
// ========== BƯỚC 4: Tính toán vận tốc bánh trái (wl) ==========
// Sử dụng lại các giá trị membership đã tính ở BƯỚC 2 (không cần tính lại vì không thay đổi)
// Các giá trị trong inputMembershipValues[0-9] đã được tính toán và lưu trữ ở BƯỚC 2
// Khởi tạo giá trị membership cho đầu ra bánh trái
outputMembershipValuesLeft[0] = 0.0;
outputMembershipValuesLeft[1] = 0.25;
outputMembershipValuesLeft[2] = 0.5;
outputMembershipValuesLeft[3] = 0.75;
outputMembershipValuesLeft[4] = 1.0;
// Đánh giá 25 quy tắc fuzzy cho bánh trái và tính toán defuzzification
totalRuleActivation = 0.0;
weightedOutputSum = 0.0;
for (int ruleIndex = 0; ruleIndex < numberOfRules; ruleIndex++)
{
// Tính độ kích hoạt của quy tắc cho bánh trái: product(input1_membership, input2_membership)
// Sử dụng phép nhân (product) thay vì min() cho fuzzy AND operation
// input1: tín hiệu góc (index từ LeftWheelRuleInput1Indices[ruleIndex] - 1)
// input2: vận tốc (index từ LeftWheelRuleInput1Indices[ruleIndex + 25] + 4)
int angularSignalIndex = LeftWheelRuleInput1Indices[ruleIndex] - 1;
int velocityIndex = LeftWheelRuleInput1Indices[ruleIndex + numberOfRules] + 4;
double ruleActivation = inputMembershipValues[velocityIndex] * inputMembershipValues[angularSignalIndex];
totalRuleActivation += ruleActivation;
// Tính tổng có trọng số cho defuzzification
int outputIndex = LeftWheelRuleOutputIndices[ruleIndex] - 1;
weightedOutputSum += outputMembershipValuesLeft[outputIndex] * ruleActivation;
}
// Defuzzification: Center of Gravity method cho bánh trái
if (totalRuleActivation == 0.0)
{
// Nếu không có quy tắc nào được kích hoạt, trả về giá trị mặc định
result.wl = 0.5;
}
else
{
result.wl = weightedOutputSum / totalRuleActivation;
}
return result;
}
}