namespace RobotNet10.NavigationTune.Shared.Models;
///
/// Path following controller type
///
public enum PathFollowingController
{
PurePursuit = 1,
Stanley = 2
}
///
/// Complete parameter set for navigation tuning
///
public class NavigationParameterSet
{
public Guid Id { get; set; } = Guid.NewGuid();
public string Name { get; set; } = string.Empty;
public string Description { get; set; } = string.Empty;
public DateTime CreatedAt { get; set; } = DateTime.UtcNow;
public DateTime? UpdatedAt { get; set; }
public bool IsDefault { get; set; }
public int Version { get; set; } = 1;
// Controller Selection
public PathFollowingController ControllerType { get; set; } = PathFollowingController.PurePursuit;
// PID Configs
public PIDConfig MovePidConfig { get; set; } = new()
{
Kp = 1.0,
Ki = 0.0001,
Kd = 0.6
};
public PIDConfig RotatePidConfig { get; set; } = new()
{
Kp = 10.0,
Ki = 0.01,
Kd = 0.1
};
// Pure Pursuit Config
public PurePursuitConfig PurePursuitConfig { get; set; } = new();
// Stanley Controller Config
public StanleyConfig StanleyConfig { get; set; } = new();
// Velocity Estimator Config
public VelocityEstimatorConfig EstimatorConfig { get; set; } = new();
public VelocitySignalProcessingConfig SignalConfig { get; set; } = new();
public MotorDynamicsConfig MotorDynamicsConfig { get; set; } = new();
// Navigation Limits
public NavigationConfig NavigationConfig { get; set; } = new();
}
///
/// Pure Pursuit path tracking configuration
/// Controls how the robot follows planned paths
///
public class PurePursuitConfig
{
#region Basic Lookahead Parameters
///
/// Minimum lookahead distance (meters)
/// Default: 0.3m
///
/// Meaning: Closest point ahead on path that robot aims for
///
/// ↑ Increase (0.4-0.6m):
/// ✓ Smoother tracking on straight paths
/// ✓ More predictive, less reactive
/// ✗ May cut corners on sharp curves
/// ✗ Less precise at low speeds
///
/// ↓ Decrease (0.2-0.25m):
/// ✓ Tighter tracking on curves
/// ✓ Better precision at low speeds
/// ✗ More jittery/oscillation
/// ✗ Sensitive to noise
///
/// Tuning Tips:
/// - Start: 0.3m for general use
/// - Warehouse AGV: 0.4-0.5m (smooth, wide corridors)
/// - Tight spaces: 0.25-0.3m (precision needed)
///
public double LookaheadMin { get; set; } = 0.3;
///
/// Lookahead velocity gain (seconds)
/// Default: 1.0s
///
/// Meaning: How much lookahead increases per m/s of velocity
/// Formula: lookahead = LookaheadMin + Kdd × |velocity|
///
/// ↑ Increase (1.2-1.5s):
/// ✓ Look further ahead at high speed → smoother
/// ✓ Better for fast robots (>1.5 m/s)
/// ✗ May be too predictive (overshoot)
///
/// ↓ Decrease (0.7-0.9s):
/// ✓ More reactive control
/// ✓ Better for slow, precise robots
/// ✗ May be jittery at high speed
///
/// Tuning Tips:
/// - Formula check: At 1.0 m/s → lookahead = 0.3 + 1.0×1.0 = 1.3m
/// - Slow robot (<0.5 m/s): Kdd = 0.8-1.0
/// - Fast robot (>1.5 m/s): Kdd = 1.2-1.5
///
public double Kdd { get; set; } = 1.0;
///
/// Maximum lookahead distance (meters)
/// Default: 2.0m
///
/// Meaning: Upper limit for lookahead distance
///
/// ↑ Increase (2.5-3.0m):
/// ✓ Very smooth at high speed
/// ✓ Good for long straight paths
/// ✗ May cut corners aggressively
/// ✗ Slower reaction to path changes
///
/// ↓ Decrease (1.5-1.8m):
/// ✓ Tighter path following
/// ✓ Better for complex paths
/// ✗ Less smooth at high speed
///
/// Tuning Tips:
/// - Should be > LookaheadMin + Kdd × MaxVelocity
/// - Example: MaxVel=1.5m/s → need LookaheadMax ≥ 0.3+1.0×1.5 = 1.8m
///
public double LookaheadMax { get; set; } = 2.0;
///
/// Maximum angular velocity during tracking (rad/s)
/// Default: 1.5 rad/s (≈86°/s)
///
/// Meaning: Limit on how fast robot can turn while tracking
///
/// ↑ Increase (2.0-2.5 rad/s):
/// ✓ Faster turning on sharp curves
/// ✓ Better for agile robots
/// ✗ May cause wheel slip
/// ✗ Less stable, jerky motion
///
/// ↓ Decrease (1.0-1.2 rad/s):
/// ✓ Smoother, more stable
/// ✓ Better for heavy/slow robots
/// ✗ Slower on sharp turns
/// ✗ May not track sharp curves well
///
/// Tuning Tips:
/// - Check robot physical limits first
/// - Warehouse AGV: 1.0-1.5 rad/s
/// - Fast AMR: 2.0+ rad/s
/// - Safety-critical: 0.8-1.0 rad/s
///
public double MaxAngularVelocity { get; set; } = 1.5;
///
/// Path waypoint spacing resolution (meters)
/// Default: 0.05m (5cm)
///
/// Meaning: How densely path is sampled into waypoints
///
/// ↑ Increase (0.08-0.1m):
/// ✓ Less memory usage
/// ✓ Faster path processing
/// ✗ Coarser path, may lose detail on curves
///
/// ↓ Decrease (0.02-0.03m):
/// ✓ More accurate curve representation
/// ✓ Smoother tracking
/// ✗ More memory usage
/// ✗ Slower processing
///
/// Tuning Tips:
/// - Long paths (>50m): Use 0.08-0.1m
/// - Complex curves: Use 0.03-0.05m
/// - Memory constrained: Increase
///
public double ResolutionSplit { get; set; } = 0.05f;
#endregion
#region Final Approach Parameters
///
/// Distance to activate final approach mode (meters)
/// Default: 0.2m (20cm)
///
/// Meaning: When robot is this close to goal, switch to precision mode
/// Final approach uses Stanley controller for precise CTE-based tracking
///
/// ↑ Increase (0.3-0.5m):
/// ✓ Earlier slow down → smoother
/// ✓ More gentle approach
/// ✗ Takes longer to reach goal
///
/// ↓ Decrease (0.1-0.15m):
/// ✓ Faster approach
/// ✗ May be abrupt
/// ✗ Risk of overshoot
///
/// Tuning Tips:
/// - High precision: 0.3-0.5m
/// - Speed priority: 0.15-0.2m
///
public double FinalApproachThreshold { get; set; } = 0.2;
///
/// Final heading tolerance (degrees)
/// Default: 2.0° (0.035 rad)
///
/// Meaning: How aligned robot heading must be with goal
///
/// ↑ Increase (5-10°):
/// ✓ Faster completion
/// ✓ Less strict
/// ✗ Robot may face wrong direction
///
/// ↓ Decrease (1-2°):
/// ✓ Very precise alignment
/// ✗ Takes much longer
/// ✗ May oscillate
///
/// Tuning Tips:
/// - Docking/charging: 2-3° (precision critical)
/// - General navigation: 5-8° (acceptable)
/// - No heading requirement: 10-15° (fast)
///
public double HeadingTolerance { get; set; } = 2.0;
#endregion
#region Adaptive Lookahead Parameters
///
/// Goal region distance for lookahead reduction (meters)
/// Default: 1.5m
///
/// Meaning: Start reducing lookahead when within this distance of goal
/// Reduction: Linear from 100% at this distance → 50% at goal
///
/// ↑ Increase (2.0-3.0m):
/// ✓ Earlier precision mode
/// ✓ Smoother deceleration
/// ✗ Slower overall
///
/// ↓ Decrease (0.8-1.2m):
/// ✓ Faster approach
/// ✗ More abrupt near goal
///
/// Tuning Tips:
/// - Long paths: 2.0-2.5m
/// - Short paths: 1.0-1.5m
/// - Fast robot: Increase (more brake distance)
///
public double GoalRegionDistance { get; set; } = 1.5;
///
/// Curvature sensitivity factor
/// Default: 2.0
///
/// Meaning: How much to reduce lookahead on curves
/// Formula: curvatureFactor = 1 / (1 + KCurvature × curvature)
///
/// ↑ Increase (3.0-5.0):
/// ✓ Tighter tracking on curves
/// ✓ Less corner cutting
/// ✗ May be too reactive
/// ✗ More oscillation on curves
///
/// ↓ Decrease (1.0-1.5):
/// ✓ Smoother on curves
/// ✗ May cut corners more
/// ✗ Less precise tracking
///
/// Tuning Tips:
/// - Warehouse (gentle curves): 1.5-2.0
/// - Tight spaces (sharp curves): 3.0-4.0
/// - High speed: Increase (need tighter control)
///
public double KCurvature { get; set; } = 2.0;
///
/// Minimum lookahead time ratio (seconds)
/// Default: 0.3s
///
/// Meaning: Look ahead at least this many seconds
/// Formula: minLookahead = max(LookaheadMin, velocity × 0.3s)
///
/// ↑ Increase (0.4-0.5s):
/// ✓ More predictive at all speeds
/// ✓ Smoother
/// ✗ May be too far ahead at low speed
///
/// ↓ Decrease (0.2-0.25s):
/// ✓ More reactive
/// ✗ May be too short at high speed
///
/// Tuning Tips:
/// - Human reaction time: ~0.25s
/// - Safe: 0.3-0.4s (reasonable preview)
/// - Very predictive: 0.5s+
///
public double MinLookaheadTimeRatio { get; set; } = 0.3;
///
/// Maximum lookahead time ratio (seconds)
/// Default: 2.0s
///
/// Meaning: Look ahead at most this many seconds
/// Formula: maxLookahead = min(LookaheadMax, velocity × 2.0s)
///
/// ↑ Increase (2.5-3.0s):
/// ✓ Very smooth at high speed
/// ✗ May be excessively far ahead
/// ✗ Cuts corners
///
/// ↓ Decrease (1.5-1.8s):
/// ✓ Tighter control
/// ✗ Less smooth at high speed
///
/// Tuning Tips:
/// - Should give comfortable preview distance
/// - At 1.5m/s: 2.0s → 3.0m ahead (reasonable)
/// - At 1.5m/s: 3.0s → 4.5m ahead (too far)
///
public double MaxLookaheadTimeRatio { get; set; } = 2.0;
#endregion
}
///
/// Hybrid Velocity Estimator configuration
/// Blends motor model prediction with encoder feedback
///
public class VelocityEstimatorConfig
{
///
/// Minimum blend ratio (model weight)
/// Default: 0.15 (15% model, 85% encoder)
///
/// Meaning: Lower bound for how much to trust motor model
///
/// ↑ Increase (0.2-0.3):
/// ✓ More model influence even when tracking poor
/// ✗ May diverge from actual velocity
///
/// ↓ Decrease (0.05-0.1):
/// ✓ More encoder influence
/// ✗ More susceptible to encoder noise
///
/// Tuning Tips:
/// - Good encoders: 0.1-0.15
/// - Noisy encoders: 0.2-0.25
///
public double MinBlendRatio { get; set; } = 0.15f;
///
/// Maximum blend ratio (model weight)
/// Default: 0.8 (80% model, 20% encoder)
///
/// Meaning: Upper bound for model trust
///
/// ↑ Increase (0.85-0.9):
/// ✓ More predictive
/// ✗ May ignore actual wheel behavior
///
/// ↓ Decrease (0.7-0.75):
/// ✓ More grounded in reality
/// ✗ Less predictive
///
/// Tuning Tips:
/// - Accurate motor model: 0.8-0.85
/// - Uncertain dynamics: 0.7-0.75
///
public double MaxBlendRatio { get; set; } = 0.8f;
///
/// Default blend ratio (startup)
/// Default: 0.6 (60% model, 40% encoder)
///
/// Meaning: Initial blend before adaptation kicks in
///
/// Tuning Tips:
/// - Should be between Min and Max
/// - Balanced: 0.5-0.6
/// - Trust model more: 0.65-0.7
///
public double DefaultBlendRatio { get; set; } = 0.6;
///
/// Good tracking error threshold
/// Default: 0.12 (12% error)
///
/// Meaning: If |predicted - actual| / actual < 12% → tracking is "good"
///
/// ↑ Increase (0.15-0.2):
/// ✓ Easier to achieve "good" status
/// ✗ May accept mediocre tracking
///
/// ↓ Decrease (0.08-0.1):
/// ✓ Stricter quality requirement
/// ✗ May rarely achieve "good"
///
/// Tuning Tips:
/// - Well-tuned system: 0.1-0.12
/// - Noisy system: 0.15-0.2
///
public double GoodTrackingThreshold { get; set; } = 0.12f;
///
/// Moderate tracking error threshold
/// Default: 0.3 (30% error)
///
/// Meaning: If error 12-30% → "moderate", >30% → "poor"
///
/// Tuning Tips:
/// - Should be > GoodTrackingThreshold
/// - Typical: 2-3× good threshold
///
public double ModerateTrackingThreshold { get; set; } = 0.3;
///
/// Blend ratio for good tracking
/// Default: 0.7 (70% model)
///
/// Meaning: When tracking well, trust model more
///
/// Tuning Tips:
/// - Reward good tracking: 0.7-0.75
/// - Conservative: 0.6-0.65
///
public double GoodTrackingBlend { get; set; } = 0.7;
///
/// Blend ratio for moderate tracking
/// Default: 0.5 (50% model, 50% encoder)
///
/// Meaning: Balanced when tracking is OK
///
public double ModerateTrackingBlend { get; set; } = 0.5;
///
/// Blend ratio for poor tracking
/// Default: 0.25 (25% model, 75% encoder)
///
/// Meaning: Trust encoder more when model is wrong
///
/// Tuning Tips:
/// - Very noisy encoders: 0.3-0.35
/// - Good encoders: 0.2-0.25
///
public double PoorTrackingBlend { get; set; } = 0.25f;
///
/// Confidence exponential decay rate
/// Default: 0.95 (5% decay per sample)
///
/// Meaning: How fast confidence updates
/// Formula: confidence = 0.95 × old + 0.05 × new
///
/// ↑ Increase (0.97-0.99):
/// ✓ Slower, smoother updates
/// ✗ Slow to detect changes
///
/// ↓ Decrease (0.9-0.93):
/// ✓ Faster adaptation
/// ✗ May be jittery
///
/// Tuning Tips:
/// - Stable system: 0.95-0.97
/// - Dynamic system: 0.92-0.94
///
public double ConfidenceDecayRate { get; set; } = 0.95f;
///
/// Minimum confidence floor
/// Default: 0.3 (30%)
///
/// Meaning: Never go below this confidence level
///
/// Tuning Tips:
/// - Safety-critical: 0.4-0.5 (cautious)
/// - Performance-focused: 0.2-0.3 (aggressive)
///
public double MinConfidence { get; set; } = 0.3;
}
///
/// Velocity signal processing configuration
/// Filters encoder velocity noise
///
public class VelocitySignalProcessingConfig
{
///
/// EMA (Exponential Moving Average) filter alpha
/// Default: 0.3
///
/// Meaning: Weight for new sample in filter
/// Formula: filtered = alpha × new + (1-alpha) × old
///
/// ↑ Increase (0.4-0.6):
/// ✓ More responsive to changes
/// ✗ Less noise filtering
/// ✗ May be jittery
///
/// ↓ Decrease (0.1-0.2):
/// ✓ More noise filtering
/// ✓ Smoother signal
/// ✗ Slower response
/// ✗ May lag actual velocity
///
/// Tuning Tips:
/// - Noisy encoders: 0.2-0.3 (more filtering)
/// - Clean encoders: 0.4-0.5 (more responsive)
/// - High-frequency control: 0.3-0.4
///
public double AlphaFilter { get; set; } = 0.3;
///
/// Noise detection threshold (m/s)
/// Default: 0.5 m/s
///
/// Meaning: Velocity changes > this are considered noise spikes
///
/// ↑ Increase (0.8-1.0):
/// ✓ Allow larger velocity changes
/// ✗ May not filter big spikes
///
/// ↓ Decrease (0.3-0.4):
/// ✓ Filter smaller spikes
/// ✗ May filter legitimate changes
///
/// Tuning Tips:
/// - Check max acceleration: threshold > max_accel × sample_time
/// - Example: 2m/s² accel, 30Hz → 0.067 m/s change/sample
/// - Set threshold ~5-10× expected change: 0.3-0.5 m/s
///
public double NoiseThreshold { get; set; } = 0.5;
}
///
/// Navigation system limits configuration
/// Physical and safety constraints
///
public class NavigationConfig
{
///
/// Maximum linear velocity (m/s)
/// Default: 1.5 m/s
///
/// Meaning: Top speed for robot during navigation
///
/// ↑ Increase (2.0-3.0 m/s):
/// ✓ Faster navigation
/// ✗ Requires more braking distance
/// ✗ May lose traction/stability
/// ✗ Safety concerns
///
/// ↓ Decrease (0.8-1.2 m/s):
/// ✓ Safer operation
/// ✓ More precise control
/// ✗ Slower task completion
///
/// Tuning Tips:
/// - MUST match motor controller limits
/// - Warehouse AGV: 1.0-1.5 m/s
/// - Outdoor robot: 2.0-3.0 m/s
/// - Crowded areas: 0.5-0.8 m/s
/// - Check: Braking distance = v²/(2×decel) < safety margin
///
public double MaxLinearVelocity { get; set; } = 1.5;
///
/// Maximum angular velocity (rad/s)
/// Default: 6.0 rad/s (≈344°/s)
///
/// Meaning: Fastest rotation speed (for in-place rotation)
///
/// ↑ Increase (8.0-10.0 rad/s):
/// ✓ Faster orientation changes
/// ✗ May be unsafe
/// ✗ High stress on motors
///
/// ↓ Decrease (4.0-5.0 rad/s):
/// ✓ Safer, gentler
/// ✗ Slower rotations
///
/// Tuning Tips:
/// - MUST match motor limits
/// - This is for in-place rotation (not tracking)
/// - Typical: 4-8 rad/s
/// - Heavy robot: 3-5 rad/s
///
public double MaxAngularVelocity { get; set; } = 6.0;
///
/// Minimum linear velocity (m/s)
/// Default: 0.1 m/s
///
/// Meaning: Slowest speed before considering "stopped"
///
/// ↑ Increase (0.15-0.2 m/s):
/// ✓ Avoid very slow creeping
/// ✗ Less precision at low speed
///
/// ↓ Decrease (0.05-0.08 m/s):
/// ✓ More precise low-speed control
/// ✗ May be too slow/jerky
///
/// Tuning Tips:
/// - Should be > encoder resolution
/// - Typical: 0.08-0.15 m/s
///
public double MinLinearVelocity { get; set; } = 0.1;
///
/// Angular velocity for in-place rotation (rad/s)
/// Default: 1.0 rad/s (≈57°/s)
///
/// Meaning: Speed when robot rotates without moving forward
///
/// ↑ Increase (1.5-2.0 rad/s):
/// ✓ Faster reorientation
/// ✗ Less smooth
///
/// ↓ Decrease (0.5-0.8 rad/s):
/// ✓ Gentle rotation
/// ✗ Slower
///
/// Tuning Tips:
/// - Should be < MaxAngularVelocity
/// - Gentle: 0.5-1.0 rad/s
/// - Fast: 1.5-2.0 rad/s
///
public double RotateAngularVelocity { get; set; } = 1.0;
///
/// Goal reached radius (meters)
/// Default: 0.015m (1.5cm)
///
/// Meaning: Distance to consider navigation complete
///
/// ↑ Increase (0.03-0.05m):
/// ✓ Easier to "reach" goal
/// ✓ Faster completion
/// ✗ Lower precision
///
/// ↓ Decrease (0.01m):
/// ✓ Higher precision
/// ✗ May never reach (localization error)
///
/// Tuning Tips:
/// - Must be ≥ localization RMS error
/// - Conservative: 0.02-0.03m
/// - High precision: 0.01-0.015m (if localization allows)
///
public double ReachedRadius { get; set; } = 0.015;
///
/// Initial rotation threshold (degrees)
/// Default: 20.0°
///
/// Meaning: If heading error to first lookahead point exceeds this, rotate in place first
///
/// ↑ Increase (30-45°):
/// ✓ Start moving sooner (less initial rotation)
/// ✗ May approach path from poor angle
///
/// ↓ Decrease (10-15°):
/// ✓ Better initial alignment
/// ✗ More time spent rotating before moving
///
/// Tuning Tips:
/// - Tight spaces: 10-15° (precision critical)
/// - Open areas: 25-35° (faster start)
/// - Balance: 20-25°
///
public double InitialRotationThreshold { get; set; } = 5.0;
///
/// Linear acceleration (m/s²)
/// Default: 0.5 m/s²
///
/// Meaning: How quickly the robot is allowed to reach target linear speed
///
/// ↑ Increase (1.0-2.0 m/s²):
/// ✓ Faster response to speed commands
/// ✓ Shorter ramp-up time
/// ✗ May cause slip or load spike
/// ✗ Less smooth start
///
/// ↓ Decrease (0.2-0.4 m/s²):
/// ✓ Smoother, gentler start
/// ✓ Better traction
/// ✗ Slower to reach target speed
///
/// Tuning Tips:
/// - Must not exceed motor/drive limits
/// - Heavy load or slippery floor: use lower (0.3-0.5)
/// - Empty AGV on good floor: 0.8-1.5 typical
/// - Match to Deceleration for symmetric feel
///
public double Acceleration { get; set; } = 0.5;
///
/// Linear deceleration (m/s²)
/// Default: 0.5 m/s²
///
/// Meaning: How quickly the robot is allowed to slow down / stop
///
/// ↑ Increase (1.0-2.0 m/s²):
/// ✓ Faster stopping
/// ✓ Shorter braking distance
/// ✗ May cause slip or cargo shift
/// ✗ Less smooth stop
///
/// ↓ Decrease (0.2-0.4 m/s²):
/// ✓ Smoother stop
/// ✓ Safer for fragile load
/// ✗ Longer braking distance
///
/// Tuning Tips:
/// - Often set equal to or slightly higher than Acceleration for safe stop
/// - Safety: ensure Deceleration allows stop within ReachedRadius
/// - Slippery surface: use lower value
///
public double Deceleration { get; set; } = 0.5;
}