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);
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,55 @@
|
||||
namespace RobotNet10.RobotApp.Services.Simulation.Algorithm;
|
||||
|
||||
public class PID
|
||||
{
|
||||
private double Kp = 0.3;
|
||||
private double Ki = 0.0001;
|
||||
private double Kd = 0.01;
|
||||
|
||||
private double _prevError;
|
||||
private double _integral;
|
||||
|
||||
public PID WithKp(double kp)
|
||||
{
|
||||
Kp = kp;
|
||||
return this;
|
||||
}
|
||||
|
||||
public PID WithKi(double ki)
|
||||
{
|
||||
Ki = ki;
|
||||
return this;
|
||||
}
|
||||
|
||||
public PID WithKd(double kd)
|
||||
{
|
||||
Kd = kd;
|
||||
return this;
|
||||
}
|
||||
|
||||
public double PID_step(double error, double max, double min, double timeSample)
|
||||
{
|
||||
double integralStep = 0.5 * (error + _prevError) * timeSample;
|
||||
_integral += integralStep;
|
||||
|
||||
double derivative = (error - _prevError) / timeSample;
|
||||
_prevError = error;
|
||||
|
||||
double Out = Kp * error
|
||||
+ Ki * _integral
|
||||
+ Kd * derivative;
|
||||
|
||||
// Anti-windup
|
||||
double clamped = Math.Clamp(Out, min, max);
|
||||
if (clamped != Out)
|
||||
_integral -= integralStep;
|
||||
|
||||
return clamped;
|
||||
}
|
||||
|
||||
public void Reset()
|
||||
{
|
||||
_prevError = 0;
|
||||
_integral = 0;
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,380 @@
|
||||
using RobotNet.VDA5050.Order;
|
||||
using RobotNet10.Common;
|
||||
using RobotNet10.Common.Models;
|
||||
using RobotNet10.RobotApp.Services.Exceptions;
|
||||
using RobotNet10.RobotApp.Services.Robot;
|
||||
using RobotNet10.RobotApp.Services.Robot.Helper;
|
||||
using RobotNet10.RobotApp.Services.Robot.Models;
|
||||
using RobotNet10.RobotApp.Shared.Enums;
|
||||
|
||||
namespace RobotNet10.RobotApp.Services.Simulation.Algorithm;
|
||||
|
||||
public class PurePursuit
|
||||
{
|
||||
private double MaxAngularVelocity = 1.5;
|
||||
private double LookaheadDistance = 0.5;
|
||||
|
||||
private readonly double ResolutionSplit = 0.1;
|
||||
|
||||
private KDTree? KDTreeOrder;
|
||||
private KDTree? KDTreeWay;
|
||||
private OrderNode? LastNode;
|
||||
private Dictionary<string, (int start, int end)>? _segmentCache;
|
||||
|
||||
public OrderNode? LastOrderNode = null;
|
||||
private int LastNavNodeIndex = 0;
|
||||
public int OnNodeIndex = 0;
|
||||
public NavigationNode? Goal;
|
||||
public List<NavigationNode> Waypoints_Value = [];
|
||||
|
||||
public OrderNode[] OrderNodes = [];
|
||||
public OrderEdge[] OrderEdges = [];
|
||||
|
||||
public PurePursuit WithLookheadDistance(double distance)
|
||||
{
|
||||
LookaheadDistance = distance;
|
||||
return this;
|
||||
}
|
||||
|
||||
public PurePursuit WithMaxAngularVelocity(double vel)
|
||||
{
|
||||
MaxAngularVelocity = vel;
|
||||
return this;
|
||||
}
|
||||
|
||||
public PurePursuit WithPath(Node[] nodes, Edge[] edges, double currentTheta)
|
||||
{
|
||||
if (nodes.Length < 2) throw new SimulationException(RobotErrors.Error1002(nodes.Length));
|
||||
if (edges.Length < 1) throw new SimulationException();
|
||||
if (edges.Length != nodes.Length - 1) throw new SimulationException(RobotErrors.Error1004(nodes.Length, edges.Length));
|
||||
(OrderNodes, OrderEdges) = OrderConverter.Validate(nodes, edges, currentTheta);
|
||||
Waypoints_Value = [.. PathSplit(OrderNodes, OrderEdges)];
|
||||
BuildSegmentCache();
|
||||
KDTreeOrder = new KDTree(OrderNodes.Select(n => new KDTreeData(n.NodeId, n.X, n.Y)));
|
||||
KDTreeWay = new KDTree(Waypoints_Value.Select(n => new KDTreeData(n.Id.ToString(), n.X, n.Y)));
|
||||
return this;
|
||||
}
|
||||
|
||||
public void UpdateGoal(string goalId)
|
||||
{
|
||||
var goal = Waypoints_Value.FirstOrDefault(n => n.NodeId == goalId);
|
||||
if (goal is not null) Goal = goal;
|
||||
}
|
||||
|
||||
private NavigationNode[] PathSplit(OrderNode[] nodes, OrderEdge[] edges)
|
||||
{
|
||||
List<NavigationNode> navigationNode = [new()
|
||||
{
|
||||
Id = Guid.NewGuid(),
|
||||
NodeId = nodes[0].NodeId,
|
||||
X = nodes[0].X,
|
||||
Y = nodes[0].Y,
|
||||
Theta = nodes[0].Theta,
|
||||
Direction = edges[0].Direction,
|
||||
}];
|
||||
foreach (var edge in edges)
|
||||
{
|
||||
var startNode = nodes.FirstOrDefault(n => n.NodeId == edge.StartNodeId);
|
||||
var endNode = nodes.FirstOrDefault(n => n.NodeId == edge.EndNodeId);
|
||||
if (startNode is null) throw new PathPlannerException(RobotErrors.Error1008(edge.EdgeId, edge.StartNodeId));
|
||||
if (endNode is null) throw new PathPlannerException(RobotErrors.Error1009(edge.EdgeId, edge.EndNodeId));
|
||||
|
||||
var spaceEdge = new SpaceEdge()
|
||||
{
|
||||
StartX = startNode.X,
|
||||
StartY = startNode.Y,
|
||||
EndX = endNode.X,
|
||||
EndY = endNode.Y,
|
||||
ControlPoint1X = edge.ControlPoint1X ?? 0,
|
||||
ControlPoint1Y = edge.ControlPoint1Y ?? 0,
|
||||
ControlPoint2X = edge.ControlPoint2X ?? 0,
|
||||
ControlPoint2Y = edge.ControlPoint2Y ?? 0,
|
||||
Degree = edge.Degree,
|
||||
};
|
||||
|
||||
double length = SpaceCompute.GetEdgeLength(spaceEdge, ResolutionSplit);
|
||||
if (length <= 0) continue;
|
||||
double step = ResolutionSplit / length;
|
||||
|
||||
for (double t = step; t <= 1 - step; t += step)
|
||||
{
|
||||
(double x, double y) = SpaceCompute.BezierPoint(t, spaceEdge);
|
||||
navigationNode.Add(new()
|
||||
{
|
||||
Id = Guid.NewGuid(),
|
||||
NodeId = string.Empty,
|
||||
X = x,
|
||||
Y = y,
|
||||
Theta = null,
|
||||
Direction = edge.Direction,
|
||||
});
|
||||
}
|
||||
navigationNode.Add(new()
|
||||
{
|
||||
Id = Guid.NewGuid(),
|
||||
NodeId = endNode.NodeId,
|
||||
X = endNode.X,
|
||||
Y = endNode.Y,
|
||||
Theta = endNode.Theta,
|
||||
Direction = edge.Direction,
|
||||
});
|
||||
}
|
||||
return [.. navigationNode];
|
||||
}
|
||||
|
||||
private void BuildSegmentCache()
|
||||
{
|
||||
_segmentCache = [];
|
||||
|
||||
for (int i = 0; i < OrderNodes.Length - 1; i++)
|
||||
{
|
||||
var currentNodeId = OrderNodes[i].NodeId;
|
||||
|
||||
// Xác định phạm vi: từ (i-1) đến (i+1)
|
||||
int rangeStart = Math.Max(0, i - 1);
|
||||
int rangeEnd = Math.Min(OrderNodes.Length - 1, i + 1);
|
||||
|
||||
var startNodeId = OrderNodes[rangeStart].NodeId;
|
||||
var endNodeId = OrderNodes[rangeEnd].NodeId;
|
||||
|
||||
// Tìm indices trong Waypoints_Value
|
||||
int waypointStartIdx = Waypoints_Value.FindIndex(n => n.NodeId == startNodeId);
|
||||
int waypointEndIdx = Waypoints_Value.FindIndex(n => n.NodeId == endNodeId);
|
||||
|
||||
// Xử lý trường hợp không tìm thấy
|
||||
if (waypointStartIdx == -1) waypointStartIdx = 0;
|
||||
if (waypointEndIdx == -1) waypointEndIdx = Waypoints_Value.Count - 1;
|
||||
|
||||
// Cache theo NodeId của OrderNode
|
||||
_segmentCache[currentNodeId] = (waypointStartIdx, waypointEndIdx);
|
||||
}
|
||||
}
|
||||
|
||||
private (OrderNode node, int index)? GetOnNodeWithKDTree(double x, double y)
|
||||
{
|
||||
KDTreeOrder ??= new KDTree(OrderNodes.Select(n => new KDTreeData(n.NodeId, n.X, n.Y)));
|
||||
LastNode ??= OrderNodes[0];
|
||||
var dx = LastNode.X - x;
|
||||
var dy = LastNode.Y - y;
|
||||
var minDistance = Math.Sqrt(dx * dx + dy * dy);
|
||||
var closesFindedNode = KDTreeOrder.FindNearest(x, y, minDistance);
|
||||
if (closesFindedNode == null) return null;
|
||||
|
||||
var closesNodeIndex = Array.FindIndex(OrderNodes, n => n.NodeId == closesFindedNode?.Id);
|
||||
if (closesNodeIndex == -1) return null;
|
||||
|
||||
if (OrderNodes[closesNodeIndex].NodeId != LastNode.NodeId)
|
||||
{
|
||||
var skipIndex = closesNodeIndex == 0 ? 0 : closesNodeIndex - 1;
|
||||
var newNodes = OrderNodes.Skip(skipIndex);
|
||||
KDTreeOrder = new KDTree(newNodes.Select(n => new KDTreeData(n.NodeId, n.X, n.Y)));
|
||||
|
||||
LastNode = OrderNodes[closesNodeIndex];
|
||||
}
|
||||
return (OrderNodes[closesNodeIndex], closesNodeIndex);
|
||||
}
|
||||
|
||||
private (OrderNode node, int index) GetOnNode(double x, double y)
|
||||
{
|
||||
LastNode ??= OrderNodes[0];
|
||||
var lastIndex = Array.FindIndex(OrderNodes, n => n.NodeId == LastNode?.NodeId);
|
||||
lastIndex = Math.Max(0, lastIndex);
|
||||
|
||||
var dx = LastNode.X - x;
|
||||
var dy = LastNode.Y - y;
|
||||
var minDistance = Math.Sqrt(dx * dx + dy * dy);
|
||||
|
||||
OrderNode onNode = LastNode;
|
||||
int index = 0;
|
||||
for (int i = lastIndex; i < OrderNodes.Length; i++)
|
||||
{
|
||||
var node = OrderNodes[i]; // FIX: Dùng đúng array
|
||||
dx = x - node.X;
|
||||
dy = y - node.Y;
|
||||
var distance = dx * dx + dy * dy;
|
||||
if (distance < minDistance)
|
||||
{
|
||||
onNode = OrderNodes[i];
|
||||
minDistance = distance;
|
||||
index = i;
|
||||
}
|
||||
}
|
||||
return (onNode, index);
|
||||
}
|
||||
|
||||
public (NavigationNode node, int index) GetOnNavNode(double x, double y)
|
||||
{
|
||||
double minDistance = double.MaxValue;
|
||||
NavigationNode onNode = Waypoints_Value[0];
|
||||
int index = 0;
|
||||
for (int i = 1; i < Waypoints_Value.Count; i++)
|
||||
{
|
||||
var distance = Math.Sqrt(Math.Pow(x - Waypoints_Value[i].X, 2) + Math.Pow(y - Waypoints_Value[i].Y, 2));
|
||||
if (distance < minDistance)
|
||||
{
|
||||
onNode = Waypoints_Value[i];
|
||||
minDistance = distance;
|
||||
index = i;
|
||||
}
|
||||
}
|
||||
return (onNode, index);
|
||||
}
|
||||
|
||||
private (NavigationNode? node, int index) OnNodeLinear(double x, double y)
|
||||
{
|
||||
var (node, index) = GetOnNodeWithKDTree(x, y) ?? GetOnNode(x, y);
|
||||
|
||||
int waypointStartIdx = 0;
|
||||
int waypointEndIdx = Waypoints_Value.Count - 1;
|
||||
if (_segmentCache is null)
|
||||
{
|
||||
var startIndex = Math.Max(0, index - 1);
|
||||
var endIndex = Math.Min(OrderNodes.Length - 1, index + 1);
|
||||
var navStartNodeIndex = Waypoints_Value.FindIndex(n => n.NodeId == OrderNodes[startIndex].NodeId);
|
||||
waypointStartIdx = navStartNodeIndex == -1 ? 0 : navStartNodeIndex;
|
||||
var navEndNodeIndex = Waypoints_Value.FindIndex(n => n.NodeId == OrderNodes[endIndex].NodeId);
|
||||
waypointEndIdx = navEndNodeIndex == -1 ? Waypoints_Value.Count - 1 : navEndNodeIndex;
|
||||
}
|
||||
else if (_segmentCache.TryGetValue(node.NodeId, out var range)) (waypointStartIdx, waypointEndIdx) = range;
|
||||
|
||||
waypointStartIdx = Math.Min(waypointStartIdx, OnNodeIndex);
|
||||
|
||||
var dx = OrderNodes[index].X - x;
|
||||
var dy = OrderNodes[index].Y - y;
|
||||
var minDistance = Math.Sqrt(dx * dx + dy * dy);
|
||||
|
||||
NavigationNode? bestNode = null;
|
||||
int bestIndex = -1;
|
||||
|
||||
for (int i = waypointStartIdx; i <= waypointEndIdx; i++)
|
||||
{
|
||||
var orderNodeId = Waypoints_Value[i].NodeId;
|
||||
|
||||
// Tìm NavigationNode tương ứng
|
||||
var navIndex = Waypoints_Value.FindIndex(n => n.NodeId == orderNodeId);
|
||||
if (navIndex == -1) continue;
|
||||
|
||||
var navNode = Waypoints_Value[navIndex];
|
||||
dx = x - navNode.X;
|
||||
dy = y - navNode.Y;
|
||||
var distanceSq = dx * dx + dy * dy;
|
||||
|
||||
if (distanceSq < minDistance)
|
||||
{
|
||||
minDistance = distanceSq;
|
||||
bestNode = navNode;
|
||||
bestIndex = navIndex;
|
||||
}
|
||||
}
|
||||
|
||||
return (bestNode, bestIndex);
|
||||
}
|
||||
|
||||
private (NavigationNode? node, int index) OnNodeWithKDTree(double x, double y)
|
||||
{
|
||||
KDTreeWay ??= new KDTree(Waypoints_Value.Select(n => new KDTreeData(n.Id.ToString(), n.X, n.Y)));
|
||||
var kdtreeNode = KDTreeWay.FindNearest(x, y, double.MaxValue);
|
||||
if(kdtreeNode is not null && Guid.TryParse(kdtreeNode.Id, out Guid kdtreeNodeId))
|
||||
{
|
||||
var nodeIndex = Waypoints_Value.FindIndex(n => n.Id == kdtreeNodeId);
|
||||
if(nodeIndex != -1) return (Waypoints_Value[nodeIndex], nodeIndex);
|
||||
}
|
||||
return (null, -1);
|
||||
}
|
||||
|
||||
private (NavigationNode? node, int index) OnNode(double x, double y)
|
||||
{
|
||||
var (node, index) = OnNodeWithKDTree(x, y);
|
||||
if (node is null || index == -1) return OnNodeLinear(x, y);
|
||||
return (node, index);
|
||||
}
|
||||
|
||||
private void UpdateLastOrderNode()
|
||||
{
|
||||
var oldNavNodes = Waypoints_Value.Skip(LastNavNodeIndex).Take(OnNodeIndex);
|
||||
var lastNavNode = oldNavNodes.LastOrDefault(n => string.IsNullOrEmpty(n.NodeId));
|
||||
if (lastNavNode is null) return;
|
||||
LastOrderNode = OrderNodes.FirstOrDefault(n => n.NodeId == lastNavNode.NodeId);
|
||||
if (LastOrderNode is not null) LastNavNodeIndex = Waypoints_Value.IndexOf(lastNavNode);
|
||||
}
|
||||
|
||||
public double PurePursuit_step(double X_Ref, double Y_Ref, double Angle_Ref)
|
||||
{
|
||||
if (Waypoints_Value is null || Waypoints_Value.Count < 2) return 0;
|
||||
NavigationNode? lookaheadStartPt = null;
|
||||
var (onNode, index) = OnNode(X_Ref, Y_Ref);
|
||||
if (onNode is null || Goal is null) return 0;
|
||||
OnNodeIndex = index;
|
||||
UpdateLastOrderNode();
|
||||
double lookDistance = 0;
|
||||
for (int i = OnNodeIndex + 1; i < Waypoints_Value.IndexOf(Goal); i++)
|
||||
{
|
||||
lookDistance += Math.Sqrt(Math.Pow(Waypoints_Value[i - 1].X - Waypoints_Value[i].X, 2) + Math.Pow(Waypoints_Value[i - 1].Y - Waypoints_Value[i].Y, 2));
|
||||
if (lookDistance >= LookaheadDistance || Waypoints_Value[i].Direction != onNode.Direction)
|
||||
{
|
||||
lookaheadStartPt = Waypoints_Value[i];
|
||||
break;
|
||||
}
|
||||
}
|
||||
lookaheadStartPt ??= Goal;
|
||||
if (onNode.Direction == RobotDirection.BACKWARD)
|
||||
{
|
||||
if (Angle_Ref > Math.PI) Angle_Ref -= Math.PI * 2;
|
||||
else if (Angle_Ref < -Math.PI) Angle_Ref += Math.PI * 2;
|
||||
Angle_Ref += Math.PI;
|
||||
if (Angle_Ref > Math.PI) Angle_Ref -= Math.PI * 2;
|
||||
}
|
||||
var distance = Math.Atan2(lookaheadStartPt.Y - Y_Ref, lookaheadStartPt.X - X_Ref) - Angle_Ref;
|
||||
|
||||
if (Math.Abs(distance) > Math.PI)
|
||||
{
|
||||
double minDistance;
|
||||
if (distance + Math.PI == 0.0) minDistance = 0.0;
|
||||
else
|
||||
{
|
||||
double data = (distance + Math.PI) / (2 * Math.PI);
|
||||
if (data < 0) data = Math.Round(data + 0.5);
|
||||
else data = Math.Round(data - 0.5);
|
||||
minDistance = distance + Math.PI - data * (2 * Math.PI);
|
||||
double checker = 0;
|
||||
if (minDistance != 0.0)
|
||||
{
|
||||
checker = Math.Abs((distance + Math.PI) / (2 * Math.PI));
|
||||
}
|
||||
if (!(Math.Abs(checker - Math.Floor(checker + 0.5)) > 2.2204460492503131E-16 * checker))
|
||||
{
|
||||
minDistance = 0.0;
|
||||
}
|
||||
else if (distance + Math.PI < 0.0)
|
||||
{
|
||||
minDistance += Math.PI * 2;
|
||||
}
|
||||
}
|
||||
if (minDistance == 0.0 && distance + Math.PI > 0.0)
|
||||
{
|
||||
minDistance = Math.PI * 2;
|
||||
}
|
||||
distance = minDistance - Math.PI;
|
||||
}
|
||||
|
||||
var AngularVelocity = 2.0 * 0.5 * Math.Sin(distance) / LookaheadDistance;
|
||||
if (Math.Abs(AngularVelocity) > MaxAngularVelocity)
|
||||
{
|
||||
if (AngularVelocity < 0.0)
|
||||
{
|
||||
AngularVelocity = -1.0;
|
||||
}
|
||||
else if (AngularVelocity > 0.0)
|
||||
{
|
||||
AngularVelocity = 1.0;
|
||||
}
|
||||
else if (AngularVelocity == 0.0)
|
||||
{
|
||||
AngularVelocity = 0.0;
|
||||
}
|
||||
AngularVelocity *= MaxAngularVelocity;
|
||||
}
|
||||
return AngularVelocity;
|
||||
}
|
||||
|
||||
}
|
||||
Reference in New Issue
Block a user