QUYVN/Denso
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-03 16:31:37 +07:00
commit 899c7c637d
1939 changed files with 641750 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

625
srcs/RobotNet10/RobotApp/RobotNet10.RobotApp/Services/Navigation/CSharp/DockToController.cs Normal file
View File

@@ -0,0 +1,625 @@
using RobotNet10.Common;
using RobotNet10.Common.Models;
using RobotNet10.RobotApp.Services.Simulation;
using RobotDirection = RobotNet10.RobotApp.Shared.Enums.RobotDirection;
namespace RobotNet10.RobotApp.Services.Navigation.CSharp;
public class DockToConfig
{
#region Core Stanley Parameters
/// <summary>
/// Cross-track error gain (K)
/// Default: 2.5
///
/// Meaning: How aggressively to correct lateral position error
/// Formula: δ = ψ + arctan(K × e / (v + Ks))
///
/// ↑ Increase (3.0-5.0):
/// ✓ Faster correction of cross-track error
/// ✓ Tighter path following
/// ✗ May cause oscillation
/// ✗ Less smooth on noisy paths
///
/// ↓ Decrease (1.5-2.0):
/// ✓ Smoother motion
/// ✓ Less oscillation
/// ✗ Slower error correction
/// ✗ Larger cross-track error
///
/// Tuning Tips:
/// - Start: 2.5 for general use
/// - High precision: 3.0-4.0
/// - Smooth priority: 1.5-2.0
/// - Check stability by observing steering oscillation
/// </summary>
public double K { get; set; } = 2.5;
/// <summary>
/// Softening constant (Ks) - meters/second
/// Default: 0.1 m/s
///
/// Meaning: Added to velocity denominator to prevent division by zero at low speeds
/// Formula: δ = ψ + arctan(K × e / (v + Ks))
///
/// ↑ Increase (0.15-0.2):
/// ✓ Less aggressive correction at low speed
/// ✓ Smoother motion when starting
/// ✗ Slower error correction at low speed
///
/// ↓ Decrease (0.05-0.08):
/// ✓ More responsive at low speed
/// ✗ May cause oscillation when slow
/// ✗ Risk of instability near zero velocity
///
/// Tuning Tips:
/// - Should be ~10% of typical operating velocity
/// - If robot oscillates when slow: increase to 0.15-0.2
/// - If too sluggish at startup: decrease to 0.05-0.08
/// </summary>
public double Ks { get; set; } = 0.1;
#endregion
#region Vehicle Parameters
/// <summary>
/// Wheelbase (L) - distance between front and rear axles (meters)
/// Default: 0.5m
///
/// Meaning: Distance from rear axle (robot center) to virtual front axle
/// Used to calculate front axle position and convert steering angle to angular velocity
///
/// IMPORTANT: Must match actual robot geometry
///
/// Formula: ω = (v × tan(δ)) / L
/// </summary>
public double WheelBase { get; set; } = 0.6;
/// <summary>
/// Maximum steering angle (radians)
/// Default: 0.5 rad (≈28.6°)
///
/// Meaning: Physical limit of equivalent steering angle
///
/// ↑ Increase (0.6-0.8 rad ≈ 34-46°):
/// ✓ Sharper turns possible
/// ✗ May exceed robot's turning capability
///
/// ↓ Decrease (0.3-0.4 rad ≈ 17-23°):
/// ✓ Safer, gentler turns
/// ✗ Cannot track sharp curves
///
/// Tuning Tips:
/// - Test robot's max practical turn rate
/// - Calculate: δ_max = arctan(L × ω_max / v_typical)
/// - Example: L=0.5m, ω_max=2rad/s, v=1m/s → δ_max = 0.785 rad (45°)
/// - Conservative: 0.4-0.5 rad
/// </summary>
public double MaxSteeringAngle { get; set; } = 0.5;
#endregion
#region Goal Approach Parameters
/// <summary>
/// Distance to start increasing K gain near goal (meters)
/// Default: 1.0m
///
/// Meaning: When within this distance, K gain increases linearly
/// to improve tracking accuracy during final approach
///
/// ↑ Increase (1.5-2.0m):
/// ✓ Earlier tightening, smoother transition
/// ✗ May be too aggressive on long approach
///
/// ↓ Decrease (0.5-0.8m):
/// ✓ Only tighten very close to goal
/// ✗ Less time to correct errors
///
/// Tuning Tips:
/// - Should be larger than GoalTolerance × 10
/// - Typical: 0.8-1.5m
/// </summary>
public double GoalApproachDistance { get; set; } = 1.0;
/// <summary>
/// K gain multiplier at goal position
/// Default: 2.0 (K doubles when at goal)
///
/// Meaning: At goal, effective K = K × GoalGainMultiplier
/// Linearly interpolated from 1.0 at GoalApproachDistance to this value at goal
///
/// ↑ Increase (2.5-3.0):
/// ✓ Much tighter tracking near goal
/// ✗ Risk of oscillation
///
/// ↓ Decrease (1.3-1.5):
/// ✓ Gentler increase
/// ✗ Less improvement near goal
///
/// Tuning Tips:
/// - Start at 2.0
/// - If oscillating near goal: decrease to 1.5
/// - If still drifting: increase to 2.5
/// </summary>
public double GoalGainMultiplier { get; set; } = 2.0;
public double ReachedRadius { get; set; } = 0.03;
#endregion
#region Angular Velocity Limit
/// <summary>
/// Maximum angular velocity during dock-to approach (rad/s)
/// Default: 0.8 rad/s
///
/// Meaning: Clamps the angular velocity output of Stanley controller
/// to prevent excessive rotation during docking.
///
/// ↑ Increase (1.0-1.5):
/// ✓ Faster heading correction
/// ✗ May overshoot or oscillate during docking
///
/// ↓ Decrease (0.3-0.5):
/// ✓ Smoother, more precise docking
/// ✗ Slower heading correction
///
/// Tuning Tips:
/// - Should be lower than PurePursuit/Stanley MaxAngularVelocity for precise docking
/// - Start at 0.8, decrease if robot oscillates during dock
/// </summary>
public double MaxAngularVelocity { get; set; } = 0.05;
#endregion
#region Path Resolution
/// <summary>
/// Waypoint spacing for path sampling (meters)
/// Default: 0.05m (5cm)
///
/// Meaning: Distance between interpolated path points
/// Same as PurePursuit.ResolutionSplit for consistency
/// </summary>
public double ResolutionSplit { get; set; } = 0.05;
#endregion
public RobotDirection DockToDirection { get; set; } = RobotDirection.FORWARD;
public double DockToLength { get; set; } = 3;
#region Fine Positioning
/// <summary>
/// Timeout per FinePositioning attempt (milliseconds).
/// Default: 6000ms (6s)
/// </summary>
public int FinePositioningTimeoutMs { get; set; } = 6000;
/// <summary>
/// Maximum retries for FinePositioning before transitioning to Error.
/// Default: 3
/// </summary>
public int FinePositioningMaxRetries { get; set; } = 3;
#endregion
#region Docking Overshoot & FinePositioning Tuning
/// <summary>
/// Distance (meters) from goal at which overshoot detection begins during Docking.
/// Default: 0.2m
///
/// Meaning: Similar to MovingOvershootDetectionRadius but for Docking phase.
/// Smaller default because docking requires higher precision.
///
/// ↑ Increase (0.3-0.5):
/// ✓ Earlier overshoot detection
/// ✗ May false-trigger during normal approach deceleration
///
/// ↓ Decrease (0.1-0.15):
/// ✓ Fewer false triggers
/// ✗ Robot may overshoot further before detection
///
/// Tuning Tips:
/// - Should be > ReachedRadius (default 0.03m)
/// - If robot frequently enters FinePositioning unnecessarily: increase
/// - If robot overshoots too far before FP kicks in: decrease
/// </summary>
public double DockingOvershootDetectionRadius { get; set; } = 0.2;
/// <summary>
/// Distance (meters) from goal at which PID deceleration begins during Docking.
/// Default: 3.0m
///
/// Meaning: When distance to goal > this value, robot runs at MaxLinearVelocity.
/// Below this distance, PID ramps velocity down proportionally.
///
/// ↑ Increase (4-5):
/// ✓ Earlier, smoother deceleration — better for heavy robots
/// ✗ Slower docking approach
///
/// ↓ Decrease (1-2):
/// ✓ Faster approach — stays at max speed longer
/// ✗ Risk of overshoot on high-inertia robots
///
/// Tuning Tips:
/// - Typically smaller than Moving's DecelerationDistance (docking is slower)
/// - Should account for DockToMaxSpeed and robot braking capability
/// </summary>
public double DecelerationDistance { get; set; } = 3.0;
/// <summary>
/// Heading alignment threshold (degrees) for FinePositioning Phase 1 (Align).
/// Default: 0.5°
///
/// Meaning: Robot must align heading within this tolerance before transitioning
/// from Align phase to Advance phase.
///
/// ↑ Increase (1-2°):
/// ✓ Faster alignment — less time spent rotating
/// ✗ Robot may drift more during advance due to initial heading error
///
/// ↓ Decrease (0.1-0.3°):
/// ✓ More precise heading before advancing
/// ✗ Longer alignment time, may oscillate on noisy heading sensor
///
/// Tuning Tips:
/// - Must be < ReAlignThresholdDegrees
/// - If robot drifts during advance: decrease
/// - If alignment takes too long or oscillates: increase
/// </summary>
public double FineAlignThresholdDegrees { get; set; } = 0.5;
/// <summary>
/// Heading drift threshold (degrees) to trigger re-alignment during FinePositioning Phase 2 (Advance).
/// Default: 3.0°
///
/// Meaning: If heading error exceeds this during advance, robot stops and re-enters Align phase.
///
/// ↑ Increase (5-10°):
/// ✓ Fewer re-alignment interruptions
/// ✗ Robot may approach goal at a large angle, reducing accuracy
///
/// ↓ Decrease (1-2°):
/// ✓ Tighter heading control during advance
/// ✗ Frequent re-alignment, slower convergence
///
/// Tuning Tips:
/// - Must be > FineAlignThresholdDegrees (to avoid immediate re-trigger)
/// - Typical ratio: ReAlign ≈ 5-10x FineAlign
/// - If robot keeps re-aligning: increase or tune AdvanceHeadingCorrectionGain
/// </summary>
public double ReAlignThresholdDegrees { get; set; } = 3.0;
/// <summary>
/// P-gain for heading correction during FinePositioning Phase 2 (Advance).
/// Default: 1.5
///
/// Meaning: Angular velocity correction = headingError × Gain × effectiveLinearVel
/// Higher gain → stronger angular correction while advancing.
///
/// ↑ Increase (2.0-3.0):
/// ✓ Faster heading correction during advance
/// ✗ May cause oscillation or jerky steering
///
/// ↓ Decrease (0.5-1.0):
/// ✓ Smoother advance motion
/// ✗ Heading drift may accumulate, triggering re-alignment
///
/// Tuning Tips:
/// - Works together with DockToAdvanceMaxAngularVelocity
/// - If robot oscillates during advance: decrease
/// - If robot drifts and re-aligns too often: increase
/// </summary>
public double AdvanceHeadingCorrectionGain { get; set; } = 1.5;
/// <summary>
/// Maximum angular velocity (rad/s) for heading correction during FinePositioning Phase 2 (Advance).
/// Default: 0.15 rad/s
///
/// Meaning: Angular correction during advance is clamped to ±this value.
/// Independent of NavigationConfig.MaxAngularVelocity (which is tuned for Moving phase).
///
/// ↑ Increase (0.2-0.3):
/// ✓ Stronger heading correction during advance
/// ✗ May cause path deviation or jerky steering
///
/// ↓ Decrease (0.05-0.1):
/// ✓ Very smooth, nearly straight-line advance
/// ✗ Cannot correct heading drift effectively, may trigger re-alignment
///
/// Tuning Tips:
/// - Should be small relative to DockToRetrySpeed to keep motion smooth
/// - If advance path is too curved: decrease
/// - If heading correction is too weak and triggers frequent re-align: increase
/// </summary>
public double DockToAdvanceMaxAngularVelocity { get; set; } = 0.15;
/// <summary>
/// Number of consecutive distance-increasing cycles to trigger overshoot during FinePositioning Advance.
/// Default: 3
///
/// Meaning: During advance, if distance to goal increases for this many consecutive cycles,
/// overshoot is declared and a retry is initiated.
///
/// ↑ Increase (5-7):
/// ✓ More tolerant of temporary distance fluctuations (sensor noise)
/// ✗ Slower overshoot detection — robot travels further past goal
///
/// ↓ Decrease (1-2):
/// ✓ Faster overshoot detection
/// ✗ May false-trigger on sensor noise or minor jitter
///
/// Tuning Tips:
/// - At 30ms cycle: 3 counts = 90ms detection delay
/// - Noisy localization: increase to 5-7
/// - Stable localization: 2-3 is sufficient
/// </summary>
public int FinePositioningOvershootCount { get; set; } = 3;
#endregion
#region Continuous Goal Update
/// <summary>
/// Maximum allowed position shift (meters) between consecutive goal updates.
/// If new goal is farther than this from old goal, it is rejected.
/// Default: 0.5m
/// </summary>
public double MaxGoalPositionShift { get; set; } = 0.2;
/// <summary>
/// Maximum allowed angle shift (degrees) between consecutive goal updates.
/// If new goal orientation differs by more than this from old goal, it is rejected.
/// Default: 15 degrees
/// </summary>
public double MaxGoalAngleShiftDegrees { get; set; } = 5.0;
#endregion
public DockToConfig Clone() => (DockToConfig)MemberwiseClone();
}
public class DockToController(DockToConfig dockConfig)
{
public DockToConfig DockConfig { get; private set; } = dockConfig;
public List<NavigationNode> Waypoints_Value = [];
public NavigationNode Goal = null!;
private int _currentWaypointAheadIndex = 0;
public NavigationNode StartNode = null!;
public DockToController WithPath(NavigationNode startNode, NavigationNode currentGoal)
{
_currentWaypointAheadIndex = 0;
StartNode = startNode;
Goal = currentGoal;
Waypoints_Value = [..PathSplit(StartNode, Goal)];
return this;
}
public void ResetTracking()
{
_currentWaypointAheadIndex = 0;
}
public void UpdateGoal(NavigationNode currentGoal)
{
Goal = currentGoal;
Waypoints_Value = [.. PathSplit(StartNode, Goal)];
}
private NavigationNode[] PathSplit(NavigationNode startNode, NavigationNode currentGoal)
{
List<NavigationNode> navigationNode = [startNode];
var spaceEdge = new SpaceEdge()
{
StartX = startNode.X,
StartY = startNode.Y,
EndX = currentGoal.X,
EndY = currentGoal.Y,
Degree = 1,
};
double length = SpaceCompute.GetEdgeLength(spaceEdge, DockConfig.ResolutionSplit);
if (length <= 0) return [];
double step = DockConfig.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 = DockConfig.DockToDirection,
Speed = startNode.Speed,
});
}
navigationNode.Add(currentGoal);
return [.. navigationNode];
}
/// <summary>
/// Calculate distance between two points
/// </summary>
private static double CalculateDistance(double x1, double y1, double x2, double y2)
{
double dx = x2 - x1;
double dy = y2 - y1;
return Math.Sqrt(dx * dx + dy * dy);
}
public (NavigationNode node, int index) GetClosestAheadWaypoint(double x, double y)
{
if (Waypoints_Value.Count == 0)
throw new InvalidOperationException("Path not set");
double minDistance = double.MaxValue;
int closestIndex = 0;
// Start search from current index for efficiency
for (int i = _currentWaypointAheadIndex; i < Waypoints_Value.Count; i++)
{
double distance = CalculateDistance(x, y, Waypoints_Value[i].X, Waypoints_Value[i].Y);
if (distance < minDistance)
{
minDistance = distance;
closestIndex = i;
}
}
// Also check previous waypoints in case robot moved backwards
for (int i = 0; i < _currentWaypointAheadIndex; i++)
{
double distance = CalculateDistance(x, y, Waypoints_Value[i].X, Waypoints_Value[i].Y);
if (distance < minDistance)
{
minDistance = distance;
closestIndex = i;
}
}
_currentWaypointAheadIndex = closestIndex;
return (Waypoints_Value[closestIndex], closestIndex);
}
/// <summary>
/// Calculate path heading at given waypoint index (for Stanley)
/// Uses current point and next point to determine direction
/// </summary>
private double CalculatePathHeading(int index)
{
if (index >= Waypoints_Value.Count - 1)
{
// Last point - use previous segment direction
if (index > 0)
{
double dx = Waypoints_Value[index].X - Waypoints_Value[index - 1].X;
double dy = Waypoints_Value[index].Y - Waypoints_Value[index - 1].Y;
return Math.Atan2(dy, dx);
}
return 0;
}
// Use current to next point
double dxNext = Waypoints_Value[index + 1].X - Waypoints_Value[index].X;
double dyNext = Waypoints_Value[index + 1].Y - Waypoints_Value[index].Y;
return Math.Atan2(dyNext, dxNext);
}
/// <summary>
/// Calculate signed cross-track error (for Stanley)
/// Positive: front axle is to the left of path
/// Negative: front axle is to the right of path
/// </summary>
private static double CalculateStanleyCrossTrackError(double frontX, double frontY, NavigationNode closestPoint, double pathHeading)
{
// Vector from closest point to front axle
double dx = frontX - closestPoint.X;
double dy = frontY - closestPoint.Y;
// Path direction vector
double pathDx = Math.Cos(pathHeading);
double pathDy = Math.Sin(pathHeading);
// Cross product to get signed perpendicular distance
// positive = left, negative = right
double crossTrackError = dx * pathDy - dy * pathDx;
return crossTrackError;
}
/// <summary>
/// Normalize angle to [-π, π]
/// </summary>
private static double NormalizeAngle(double angle)
{
while (angle > Math.PI) angle -= 2 * Math.PI;
while (angle < -Math.PI) angle += 2 * Math.PI;
return angle;
}
/// <summary>
/// Stanley-based final approach controller
/// When robot enters goal region (IsApproachGoal), uses Stanley algorithm for precise CTE-based tracking
/// </summary>
public (double linearVel, double angularVel) FinalApproachController(
double robotX,
double robotY,
double robotTheta,
double actualLinearVelocity,
double maxLinearVelocity)
{
if (Goal is null || Waypoints_Value.Count == 0) return (0, 0);
if (DockConfig.DockToDirection == RobotDirection.BACKWARD) robotTheta += Math.PI;
robotTheta = NormalizeAngle(robotTheta);
// Calculate front axle position
double frontX = robotX + DockConfig.WheelBase * Math.Cos(robotTheta);
double frontY = robotY + DockConfig.WheelBase * Math.Sin(robotTheta);
// Find closest point on path to front axle
var (closestPoint, closestIndex) = GetClosestAheadWaypoint(frontX, frontY);
// Calculate heading at closest point (path direction)
double pathHeading = CalculatePathHeading(closestIndex);
// Calculate cross-track error (signed distance from front axle to path)
double crossTrackError = CalculateStanleyCrossTrackError(frontX, frontY, closestPoint, pathHeading);
if (DockConfig.DockToDirection == RobotDirection.BACKWARD) crossTrackError = -crossTrackError;
// Calculate heading error (path heading - robot heading)
double headingError = NormalizeAngle(pathHeading - robotTheta);
// Adaptive K gain: increase when close to goal for tighter tracking
double distanceToGoal = CalculateDistance(robotX, robotY, Goal.X, Goal.Y);
double adaptiveK = DockConfig.K;
if (distanceToGoal < DockConfig.GoalApproachDistance)
{
// Linearly increase K from K to K*GoalGainMultiplier as distance decreases
double approachRatio = 1.0 - (distanceToGoal / DockConfig.GoalApproachDistance);
adaptiveK = DockConfig.K * (1.0 + approachRatio * (DockConfig.GoalGainMultiplier - 1.0));
}
// Stanley formula: δ = ψ + arctan(K × e / (v + Ks))
double crossTrackTerm = Math.Atan2(adaptiveK * crossTrackError, Math.Abs(actualLinearVelocity) + DockConfig.Ks);
// Total steering angle
double steeringAngle = headingError + crossTrackTerm;
// Clamp to maximum steering angle
steeringAngle = Math.Clamp(steeringAngle, -DockConfig.MaxSteeringAngle, DockConfig.MaxSteeringAngle);
// Convert steering angle to angular velocity using bicycle model
double angularVelocity = (actualLinearVelocity * Math.Tan(steeringAngle)) / DockConfig.WheelBase;
// Clamp angular velocity for dock-to approach
angularVelocity = Math.Clamp(angularVelocity, -DockConfig.MaxAngularVelocity, DockConfig.MaxAngularVelocity);
// Apply direction to velocities
double linearVel = maxLinearVelocity;
if (DockConfig.DockToDirection == RobotDirection.BACKWARD)
{
linearVel = -linearVel;
}
// Debug output
Console.WriteLine($"DT-Stanley: Front=({frontX:F3},{frontY:F3}), Closest=({closestPoint.X:F3},{closestPoint.Y:F3}), " +
$"Pose=({robotX:F3},{robotY:F3},{robotTheta * 180 / Math.PI:F1}°), " +
$"CTE={crossTrackError:F3}m, HeadErr={headingError * 180 / Math.PI:F1}°, " +
$"SteerAng={steeringAngle * 180 / Math.PI:F1}°, " +
$"K={adaptiveK:F2}, DTG={distanceToGoal:F3}m, " +
$"LVel={linearVel:F3}, AnVel={angularVelocity:F3}");
return (linearVel, angularVelocity);
}
}