update with speed 2.0 m/s
This commit is contained in:
Đăng Nguyễn
parent
3b088a6d5d
commit
c35da9a73f
|
|
@ -2,251 +2,508 @@
|
||||||
|
|
||||||
public class FuzzyLogic
|
public class FuzzyLogic
|
||||||
{
|
{
|
||||||
private double Gain_P = 0.5;
|
// ============================================================================
|
||||||
private double Gain_I = 0.01;
|
// FUZZY LOGIC CONTROLLER - PHIÊN BẢN GỐC ĐÃ TỔ CHỨC LẠI
|
||||||
private double DiscreteTimeIntegrator_DSTATE;
|
// 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)
|
||||||
|
// ============================================================================
|
||||||
|
|
||||||
|
/// <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)
|
||||||
{
|
{
|
||||||
Gain_P = gainP;
|
double membership = 0.0;
|
||||||
return this;
|
|
||||||
|
// 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;
|
||||||
}
|
}
|
||||||
|
|
||||||
public FuzzyLogic WithGainI(double gainI)
|
return membership;
|
||||||
{
|
|
||||||
Gain_I = gainI;
|
|
||||||
return this;
|
|
||||||
}
|
}
|
||||||
|
|
||||||
private static double Fuzzy_trapmf(double x, double[] parame)
|
/// <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 b_y1;
|
double membership = 0.0;
|
||||||
double y2;
|
|
||||||
b_y1 = 0.0;
|
// Trường hợp 1: Nằm ngoài phía trái tam giác
|
||||||
y2 = 0.0;
|
if (inputValue <= left)
|
||||||
if (x >= parame[1])
|
|
||||||
{
|
{
|
||||||
b_y1 = 1.0;
|
membership = 0.0;
|
||||||
}
|
}
|
||||||
if (x < parame[0])
|
// Trường hợp 2: Nằm trên cạnh tăng dần (trái)
|
||||||
|
else if (inputValue > left && inputValue < peak)
|
||||||
{
|
{
|
||||||
b_y1 = 0.0;
|
if (peak != left) // Tránh chia cho 0
|
||||||
}
|
|
||||||
if (parame[0] <= x && x < parame[1] && parame[0] != parame[1])
|
|
||||||
{
|
{
|
||||||
b_y1 = 1.0 / (parame[1] - parame[0]) * (x - parame[0]);
|
membership = (inputValue - left) / (peak - left);
|
||||||
}
|
}
|
||||||
if (x <= parame[2])
|
}
|
||||||
|
// Trường hợp 3: Đúng tại đỉnh tam giác
|
||||||
|
else if (inputValue == peak)
|
||||||
{
|
{
|
||||||
y2 = 1.0;
|
membership = 1.0;
|
||||||
}
|
}
|
||||||
if (x > parame[3])
|
// Trường hợp 4: Nằm trên cạnh giảm dần (phải)
|
||||||
|
else if (inputValue > peak && inputValue < right)
|
||||||
{
|
{
|
||||||
y2 = 0.0;
|
if (right != peak) // Tránh chia cho 0
|
||||||
}
|
|
||||||
if (parame[2] < x && x <= parame[3] && parame[2] != parame[3])
|
|
||||||
{
|
{
|
||||||
y2 = 1.0 / (parame[3] - parame[2]) * (parame[3] - x);
|
membership = (right - inputValue) / (right - peak);
|
||||||
}
|
}
|
||||||
return b_y1 < y2 ? b_y1 : y2;
|
}
|
||||||
|
// Trường hợp 5: Nằm ngoài phía phải tam giác
|
||||||
|
else if (inputValue >= right)
|
||||||
|
{
|
||||||
|
membership = 0.0;
|
||||||
}
|
}
|
||||||
|
|
||||||
private static double Fuzzy_trimf(double x, double[] parame)
|
return membership;
|
||||||
{
|
|
||||||
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;
|
|
||||||
}
|
}
|
||||||
|
|
||||||
public (double wl, double wr) Fuzzy_step(double v, double w, double TimeSample)
|
// ============================================================================
|
||||||
|
// 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)
|
||||||
{
|
{
|
||||||
(double wl, double wr) result = new();
|
// 1. NB (Negative Big): [-∞, -∞, -1.0, -0.5]
|
||||||
double[] inputMFCache = new double[10];
|
membershipValues[0] = Fuzzy_trapmf(piSignal, -1.0E+10, -1.0E+10, -1.0, -0.5);
|
||||||
double[] outputMFCache = new double[5];
|
|
||||||
double[] outputMFCache_0 = new double[5];
|
// 2. Z (Zero): [-0.5, 0.0, 0.5]
|
||||||
double[] tmp = new double[3];
|
membershipValues[1] = Fuzzy_trimf(piSignal, -0.5, 0.0, 0.5);
|
||||||
double aggregatedOutputs;
|
|
||||||
double rtb_TmpSignalConversionAtSFun_0;
|
// 3. PB (Positive Big): [0.5, 1.0, +∞, +∞]
|
||||||
double rtb_antecedentOutputs_e;
|
membershipValues[2] = Fuzzy_trapmf(piSignal, 0.5, 1.0, 1.0E+10, 1.0E+10);
|
||||||
double sumAntecedentOutputs;
|
|
||||||
int ruleID;
|
// 4. NM (Negative Medium): [-1.0, -0.5, 0.0]
|
||||||
double[] f = [-1.0E+10, -1.0E+10, -1.0, -0.5];
|
membershipValues[3] = Fuzzy_trimf(piSignal, -1.0, -0.5, 0.0);
|
||||||
double[] e = [0.5, 1.0, 1.0E+10, 1.0E+10];
|
|
||||||
double[] d = [0.75, 1.0, 1.0E+9, 1.0E+9];
|
// 5. PM (Positive Medium): [0.0, 0.5, 1.0]
|
||||||
double[] c = [-1.0E+9, -1.0E+9, 0.0, 0.25];
|
membershipValues[4] = Fuzzy_trimf(piSignal, 0.0, 0.5, 1.0);
|
||||||
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: '<Root>/Fuzzy Logic Controller1' */
|
|
||||||
/* Outputs for Atomic SubSystem: '<Root>/Fuzzy Logic Controller' */
|
|
||||||
/* SignalConversion generated from: '<S4>/ SFunction ' incorporates:
|
|
||||||
* Constant: '<Root>/w'
|
|
||||||
* DiscreteIntegrator: '<Root>/Discrete-Time Integrator'
|
|
||||||
* Gain: '<Root>/Gain1'
|
|
||||||
* MATLAB Function: '<S1>/Evaluate Rule Antecedents'
|
|
||||||
* MATLAB Function: '<S2>/Evaluate Rule Antecedents'
|
|
||||||
* SignalConversion generated from: '<S7>/ SFunction '
|
|
||||||
* Sum: '<Root>/Sum'
|
|
||||||
*/
|
|
||||||
DiscreteTimeIntegrator_DSTATE += Gain_I * w * TimeSample;
|
|
||||||
rtb_TmpSignalConversionAtSFun_0 = Gain_P * w + DiscreteTimeIntegrator_DSTATE;
|
|
||||||
/* End of Outputs for SubSystem: '<Root>/Fuzzy Logic Controller1' */
|
|
||||||
/* MATLAB Function: '<S1>/Evaluate Rule Antecedents' incorporates:
|
|
||||||
* Constant: '<Root>/v'
|
|
||||||
* MATLAB Function: '<S2>/Evaluate Rule Antecedents'
|
|
||||||
* SignalConversion generated from: '<S4>/ SFunction '
|
|
||||||
*/
|
|
||||||
sumAntecedentOutputs = 0.0;
|
|
||||||
/* Outputs for Atomic SubSystem: '<Root>/Fuzzy Logic Controller1' */
|
|
||||||
inputMFCache_tmp = Fuzzy_trapmf(rtb_TmpSignalConversionAtSFun_0, f);
|
|
||||||
/* End of Outputs for SubSystem: '<Root>/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: '<Root>/Fuzzy Logic Controller1' */
|
|
||||||
inputMFCache_tmp_0 = Fuzzy_trapmf(rtb_TmpSignalConversionAtSFun_0, e);
|
|
||||||
/* End of Outputs for SubSystem: '<Root>/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: '<Root>/Fuzzy Logic Controller1' */
|
|
||||||
inputMFCache_tmp_1 = Fuzzy_trapmf(v, d);
|
|
||||||
/* End of Outputs for SubSystem: '<Root>/Fuzzy Logic Controller1' */
|
|
||||||
inputMFCache[7] = inputMFCache_tmp_1;
|
|
||||||
/* Outputs for Atomic SubSystem: '<Root>/Fuzzy Logic Controller1' */
|
|
||||||
inputMFCache_tmp_2 = Fuzzy_trapmf(v, c);
|
|
||||||
/* End of Outputs for SubSystem: '<Root>/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: '<S1>/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++)
|
|
||||||
{
|
|
||||||
/* MATLAB Function: '<S1>/Evaluate Rule Antecedents' */
|
|
||||||
rtb_antecedentOutputs_e = inputMFCache[b[ruleID + 25] + 4] * inputMFCache[b[ruleID] - 1];
|
|
||||||
sumAntecedentOutputs += rtb_antecedentOutputs_e;
|
|
||||||
/* MATLAB Function: '<S1>/Evaluate Rule Consequents' */
|
|
||||||
aggregatedOutputs += outputMFCache[b_0[ruleID] - 1] * rtb_antecedentOutputs_e;
|
|
||||||
}
|
}
|
||||||
/* MATLAB Function: '<S1>/Defuzzify Outputs' incorporates:
|
|
||||||
* MATLAB Function: '<S1>/Evaluate Rule Antecedents'
|
/// <summary>
|
||||||
* MATLAB Function: '<S1>/Evaluate Rule Consequents'
|
/// Mờ hóa vận tốc V thành 9 tập mờ (dải 0.0 - 2.0)
|
||||||
*/
|
/// </summary>
|
||||||
if (sumAntecedentOutputs == 0.0)
|
/// <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)
|
||||||
{
|
{
|
||||||
result.wr = 0.5;
|
// 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
|
else
|
||||||
{
|
{
|
||||||
result.wr = 1.0 / sumAntecedentOutputs * aggregatedOutputs;
|
// Tính trọng tâm: output = weightedSum / totalWeight
|
||||||
|
return weightedSum / totalWeight;
|
||||||
}
|
}
|
||||||
/* Outputs for Atomic SubSystem: '<Root>/Fuzzy Logic Controller1' */
|
}
|
||||||
/* MATLAB Function: '<S2>/Evaluate Rule Antecedents' incorporates:
|
|
||||||
* Constant: '<Root>/v'
|
// ============================================================================
|
||||||
* SignalConversion generated from: '<S7>/ SFunction '
|
// PHẦN 5: BẢNG LUẬT VÀ CẤU HÌNH
|
||||||
*/
|
// ============================================================================
|
||||||
sumAntecedentOutputs = 0.0;
|
|
||||||
inputMFCache[0] = inputMFCache_tmp;
|
/// <summary>
|
||||||
tmp[0] = -0.5;
|
/// Bảng chỉ số tiền đề luật cho bánh phải (Rule Antecedent Indices - Right Wheel)
|
||||||
tmp[1] = 0.0;
|
/// 90 phần tử: 45 cho PI + 45 cho Velocity
|
||||||
tmp[2] = 0.5;
|
/// Hệ thống: 5 tập mờ PI × 9 tập mờ V = 45 luật
|
||||||
inputMFCache[1] = Fuzzy_trimf(rtb_TmpSignalConversionAtSFun_0, tmp);
|
/// Giá trị từ 1-5 cho PI, 1-9 cho V
|
||||||
inputMFCache[2] = inputMFCache_tmp_0;
|
/// </summary>
|
||||||
tmp[0] = -1.0;
|
private readonly byte[] RULE_ANTECEDENT_INDICES_RIGHT =
|
||||||
tmp[1] = -0.5;
|
[
|
||||||
tmp[2] = 0.0;
|
// 45 phần tử đầu: Chỉ số tập mờ của PI Signal (1-5)
|
||||||
inputMFCache[3] = Fuzzy_trimf(rtb_TmpSignalConversionAtSFun_0, tmp);
|
// Mỗi tập PI lặp lại 9 lần (cho 9 mức vận tốc)
|
||||||
tmp[0] = 0.0;
|
1, 1, 1, 1, 1, 1, 1, 1, 1, // Luật 1-9: PI = NB (tập 1)
|
||||||
tmp[1] = 0.5;
|
2, 2, 2, 2, 2, 2, 2, 2, 2, // Luật 10-18: PI = Z (tập 2)
|
||||||
tmp[2] = 1.0;
|
3, 3, 3, 3, 3, 3, 3, 3, 3, // Luật 19-27: PI = PB (tập 3)
|
||||||
inputMFCache[4] = Fuzzy_trimf(rtb_TmpSignalConversionAtSFun_0, tmp);
|
4, 4, 4, 4, 4, 4, 4, 4, 4, // Luật 28-36: PI = NM (tập 4)
|
||||||
tmp[0] = 0.0;
|
5, 5, 5, 5, 5, 5, 5, 5, 5, // Luật 37-45: PI = PM (tập 5)
|
||||||
tmp[1] = 0.25;
|
|
||||||
tmp[2] = 0.5;
|
// 45 phần tử sau: Chỉ số tập mờ của Velocity (1-9)
|
||||||
inputMFCache[5] = Fuzzy_trimf(v, tmp);
|
// Lặp lại theo pattern: VVS, VS, S, SM, M, MF, F, VF, VVF
|
||||||
tmp[0] = 0.25;
|
1, 2, 3, 4, 5, 6, 7, 8, 9, // Luật 1-9: V = VVS đến VVF
|
||||||
tmp[1] = 0.5;
|
1, 2, 3, 4, 5, 6, 7, 8, 9, // Luật 10-18: V = VVS đến VVF
|
||||||
tmp[2] = 0.75;
|
1, 2, 3, 4, 5, 6, 7, 8, 9, // Luật 19-27: V = VVS đến VVF
|
||||||
inputMFCache[6] = Fuzzy_trimf(v, tmp);
|
1, 2, 3, 4, 5, 6, 7, 8, 9, // Luật 28-36: V = VVS đến VVF
|
||||||
inputMFCache[7] = inputMFCache_tmp_1;
|
1, 2, 3, 4, 5, 6, 7, 8, 9 // Luật 37-45: V = VVS đến VVF
|
||||||
inputMFCache[8] = inputMFCache_tmp_2;
|
];
|
||||||
tmp[0] = 0.5;
|
|
||||||
tmp[1] = 0.75;
|
/// <summary>
|
||||||
tmp[2] = 1.0;
|
/// Bảng chỉ số hệ quả cho bánh phải (Rule Consequent Indices - Right Wheel)
|
||||||
inputMFCache[9] = Fuzzy_trimf(v, tmp);
|
/// 45 phần tử tương ứng với 45 luật
|
||||||
/* MATLAB Function: '<S2>/Evaluate Rule Consequents' */
|
/// Giá trị từ 1-9 tương ứng với 9 mức tốc độ đầu ra (0.0 - 2.0)
|
||||||
aggregatedOutputs = 0.0;
|
///
|
||||||
outputMFCache_0[0] = 0.0;
|
/// Logic:
|
||||||
outputMFCache_0[1] = 0.25;
|
/// - Khi PI âm (NB, NM): Bánh phải chậm hơn (robot rẽ trái)
|
||||||
outputMFCache_0[2] = 0.5;
|
/// - Khi PI = 0 (Z): Bánh phải tỷ lệ với vận tốc V
|
||||||
outputMFCache_0[3] = 0.75;
|
/// - Khi PI dương (PB, PM): Bánh phải nhanh hơn (robot rẽ phải)
|
||||||
outputMFCache_0[4] = 1.0;
|
/// </summary>
|
||||||
for (ruleID = 0; ruleID < 25; ruleID++)
|
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)
|
||||||
{
|
{
|
||||||
/* MATLAB Function: '<S2>/Evaluate Rule Antecedents' */
|
const int NUM_INPUT_MEMBERSHIPS = 14; // 5 cho PI + 9 cho Velocity
|
||||||
rtb_antecedentOutputs_e = inputMFCache[b_1[ruleID + 25] + 4] * inputMFCache[b_1[ruleID] - 1];
|
const int NUM_OUTPUT_LEVELS = 9; // 9 mức đầu ra (0.0 - 2.0)
|
||||||
sumAntecedentOutputs += rtb_antecedentOutputs_e;
|
const int NUM_RULES = 45; // 5 × 9 = 45 luật
|
||||||
/* MATLAB Function: '<S2>/Evaluate Rule Consequents' */
|
|
||||||
aggregatedOutputs += outputMFCache_0[b_2[ruleID] - 1] *
|
// Khởi tạo mảng lưu độ thuộc đầu vào
|
||||||
rtb_antecedentOutputs_e;
|
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);
|
||||||
}
|
}
|
||||||
/* MATLAB Function: '<S2>/Defuzzify Outputs' incorporates:
|
|
||||||
* MATLAB Function: '<S2>/Evaluate Rule Antecedents'
|
// ============================================================================
|
||||||
* MATLAB Function: '<S2>/Evaluate Rule Consequents'
|
// PHẦN 7: HÀM HỖ TRỢ
|
||||||
*/
|
// ============================================================================
|
||||||
if (sumAntecedentOutputs == 0.0)
|
|
||||||
|
/// <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()
|
||||||
{
|
{
|
||||||
result.wl = 0.5;
|
integratorState = 0.0;
|
||||||
}
|
}
|
||||||
else
|
|
||||||
|
/// <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)
|
||||||
{
|
{
|
||||||
result.wl = 1.0 / sumAntecedentOutputs * aggregatedOutputs;
|
proportionalGain = kp;
|
||||||
|
integralGain = ki;
|
||||||
|
return this;
|
||||||
}
|
}
|
||||||
return result;
|
|
||||||
|
/// <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);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
|
||||||
Loading…
Reference in New Issue
Block a user