334 lines
16 KiB
C#
334 lines
16 KiB
C#
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;
|
|
}
|
|
}
|