Files
BQP/srcs/RobotNet10/Shared/RobotNet10.NavigationTune.Shared/Models/NavigationParameterSet.cs
2026-07-13 09:25:40 +07:00

737 lines
23 KiB
C#
Raw Blame History

This file contains ambiguous Unicode characters
This file contains Unicode characters that might be confused with other characters. If you think that this is intentional, you can safely ignore this warning. Use the Escape button to reveal them.
namespace RobotNet10.NavigationTune.Shared.Models;
/// <summary>
/// Path following controller type
/// </summary>
public enum PathFollowingController
{
PurePursuit = 1,
Stanley = 2
}
/// <summary>
/// Complete parameter set for navigation tuning
/// </summary>
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();
}
/// <summary>
/// Pure Pursuit path tracking configuration
/// Controls how the robot follows planned paths
/// </summary>
public class PurePursuitConfig
{
#region Basic Lookahead Parameters
/// <summary>
/// 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)
/// </summary>
public double LookaheadMin { get; set; } = 0.3;
/// <summary>
/// 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
/// </summary>
public double Kdd { get; set; } = 1.0;
/// <summary>
/// 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
/// </summary>
public double LookaheadMax { get; set; } = 2.0;
/// <summary>
/// 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
/// </summary>
public double MaxAngularVelocity { get; set; } = 1.5;
/// <summary>
/// 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
/// </summary>
public double ResolutionSplit { get; set; } = 0.05f;
#endregion
#region Final Approach Parameters
/// <summary>
/// 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
/// </summary>
public double FinalApproachThreshold { get; set; } = 0.2;
/// <summary>
/// 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)
/// </summary>
public double HeadingTolerance { get; set; } = 2.0;
#endregion
#region Adaptive Lookahead Parameters
/// <summary>
/// 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)
/// </summary>
public double GoalRegionDistance { get; set; } = 1.5;
/// <summary>
/// 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)
/// </summary>
public double KCurvature { get; set; } = 2.0;
/// <summary>
/// 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+
/// </summary>
public double MinLookaheadTimeRatio { get; set; } = 0.3;
/// <summary>
/// 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)
/// </summary>
public double MaxLookaheadTimeRatio { get; set; } = 2.0;
#endregion
}
/// <summary>
/// Hybrid Velocity Estimator configuration
/// Blends motor model prediction with encoder feedback
/// </summary>
public class VelocityEstimatorConfig
{
/// <summary>
/// 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
/// </summary>
public double MinBlendRatio { get; set; } = 0.15f;
/// <summary>
/// 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
/// </summary>
public double MaxBlendRatio { get; set; } = 0.8f;
/// <summary>
/// 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
/// </summary>
public double DefaultBlendRatio { get; set; } = 0.6;
/// <summary>
/// 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
/// </summary>
public double GoodTrackingThreshold { get; set; } = 0.12f;
/// <summary>
/// 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
/// </summary>
public double ModerateTrackingThreshold { get; set; } = 0.3;
/// <summary>
/// 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
/// </summary>
public double GoodTrackingBlend { get; set; } = 0.7;
/// <summary>
/// Blend ratio for moderate tracking
/// Default: 0.5 (50% model, 50% encoder)
///
/// Meaning: Balanced when tracking is OK
/// </summary>
public double ModerateTrackingBlend { get; set; } = 0.5;
/// <summary>
/// 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
/// </summary>
public double PoorTrackingBlend { get; set; } = 0.25f;
/// <summary>
/// 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
/// </summary>
public double ConfidenceDecayRate { get; set; } = 0.95f;
/// <summary>
/// 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)
/// </summary>
public double MinConfidence { get; set; } = 0.3;
}
/// <summary>
/// Velocity signal processing configuration
/// Filters encoder velocity noise
/// </summary>
public class VelocitySignalProcessingConfig
{
/// <summary>
/// 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
/// </summary>
public double AlphaFilter { get; set; } = 0.3;
/// <summary>
/// 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
/// </summary>
public double NoiseThreshold { get; set; } = 0.5;
}
/// <summary>
/// Navigation system limits configuration
/// Physical and safety constraints
/// </summary>
public class NavigationConfig
{
/// <summary>
/// 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
/// </summary>
public double MaxLinearVelocity { get; set; } = 1.5;
/// <summary>
/// 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
/// </summary>
public double MaxAngularVelocity { get; set; } = 6.0;
/// <summary>
/// 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
/// </summary>
public double MinLinearVelocity { get; set; } = 0.1;
/// <summary>
/// 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
/// </summary>
public double RotateAngularVelocity { get; set; } = 1.0;
/// <summary>
/// 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)
/// </summary>
public double ReachedRadius { get; set; } = 0.015;
/// <summary>
/// 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°
/// </summary>
public double InitialRotationThreshold { get; set; } = 5.0;
/// <summary>
/// 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
/// </summary>
public double Acceleration { get; set; } = 0.5;
/// <summary>
/// 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
/// </summary>
public double Deceleration { get; set; } = 0.5;
}