626 lines
23 KiB
C#
626 lines
23 KiB
C#
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);
|
||
}
|
||
}
|