diff --git a/MedullaAdapter/DiverCartDefinition.cs b/MedullaAdapter/DiverCartDefinition.cs index d8e3847..55b7556 100644 --- a/MedullaAdapter/DiverCartDefinition.cs +++ b/MedullaAdapter/DiverCartDefinition.cs @@ -66,7 +66,8 @@ namespace MedullaAdapter #region 初始参数 [AsInitParam(desc = "差速转舵目标角速度前馈增益")] - public float DiffSteerRateFeedforwardGain = 0.9f; + // public float DiffSteerRateFeedforwardGain = 0.9f; + public float DiffSteerRateFeedforwardGain = 0f; [AsInitParam(desc = "差速舵轮左右轮间距，单位mm")] public float DiffSteerWheelDistanceMillimeters = 85f; @@ -124,6 +125,47 @@ namespace MedullaAdapter public bool WheelSpeedDiagnosticEnabled; [IOObjectMonitor(desc = "轮速诊断记录状态")] public string WheelSpeedDiagnosticStatus = "未启动"; + + // M层辨识日志：以下字段只保存控制和反馈事件的单调时钟快照， + // 不参与底盘控制、限幅或模式切换。 + internal long DiffSteerControlTimestamp; + internal long DiffSteerControlSequence; + internal long WheelCommandTimestamp; + internal long WheelCommandSequence; + internal float SentSpeedLFL; + internal float SentSpeedLFR; + internal float SentSpeedLRL; + internal float SentSpeedLRR; + internal float SentSpeedRFL; + internal float SentSpeedRFR; + internal float SentSpeedRRL; + internal float SentSpeedRRR; + internal bool WheelCommandLimitedLFL; + internal bool WheelCommandLimitedLFR; + internal bool WheelCommandLimitedLRL; + internal bool WheelCommandLimitedLRR; + internal bool WheelCommandLimitedRFL; + internal bool WheelCommandLimitedRFR; + internal bool WheelCommandLimitedRRL; + internal bool WheelCommandLimitedRRR; + internal bool WheelCommandSuppressed; + internal float DiffSteerTargetRateLeftFrontDegreesPerSecond; + internal float DiffSteerTargetRateLeftRearDegreesPerSecond; + internal float DiffSteerTargetRateRightFrontDegreesPerSecond; + internal float DiffSteerTargetRateRightRearDegreesPerSecond; + internal float DiffSteerFeedforwardDeltaTimeMilliseconds; + internal bool DiffSteerFeedforwardLimitedLeftFront; + internal bool DiffSteerFeedforwardLimitedLeftRear; + internal bool DiffSteerFeedforwardLimitedRightFront; + internal bool DiffSteerFeedforwardLimitedRightRear; + internal long ActualThLeftFrontTimestamp; + internal long ActualThLeftFrontSequence; + internal long ActualThLeftRearTimestamp; + internal long ActualThLeftRearSequence; + internal long ActualThRightFrontTimestamp; + internal long ActualThRightFrontSequence; + internal long ActualThRightRearTimestamp; + internal long ActualThRightRearSequence; [IOObjectMonitor(desc = "左前左驱动器远程帧701")] public byte LFLRemoteCode = 0; [IOObjectMonitor(desc = "左前右驱动器远程帧702")] public byte LFRRemoteCode = 0; [IOObjectMonitor(desc = "右前左驱动器远程帧703")] public byte RFLRemoteCode = 0; diff --git a/MedullaAdapter/MCURoutine.cs b/MedullaAdapter/MCURoutine.cs index e75ffb6..64dca68 100644 --- a/MedullaAdapter/MCURoutine.cs +++ b/MedullaAdapter/MCURoutine.cs @@ -4,7 +4,9 @@ using FundamentalLib; using MCUSerialBridgeCLR; using System; using System.Collections.Generic; +using System.Diagnostics; using System.IO; +using System.Threading; namespace MedullaAdapter { @@ -51,6 +53,19 @@ namespace MedullaAdapter { return BitConverter.ToInt32(payload, offset) * 1875f / 512f / 10000f; } + + /// + /// 保存异步CAN反馈的本机单调接收时刻和递增序号，仅供辨识日志使用。 + /// + private static void MarkFeedbackReceived( + ref long timestamp, + ref long sequence) + { + Interlocked.Exchange( + ref timestamp, + Stopwatch.GetTimestamp()); + Interlocked.Increment(ref sequence); + } // M层硬件主循环：交换IO、发送轮组指令并更新车辆反馈状态。 public override void Operation(int iteration) { @@ -394,6 +409,44 @@ namespace MedullaAdapter var sendLArm = BitConverter.GetBytes((int)Math.Round(v9 * 512f * 10000f / 1875f)); var sendRArm = BitConverter.GetBytes((int)Math.Round(v10 * 512f * 10000f / 1875f)); + var wheelCommandSuppressed = + cart.AlarmLevel == 2 || + cart.WaitStart || + cart.PreparaStart || + _driversDisabled; + var speedLimit = Math.Max(0f, cart.SendThresSpeed); + + cart.SentSpeedLFL = wheelCommandSuppressed ? 0f : lfl; + cart.SentSpeedLFR = wheelCommandSuppressed ? 0f : lfr; + cart.SentSpeedLRL = wheelCommandSuppressed ? 0f : lrl; + cart.SentSpeedLRR = wheelCommandSuppressed ? 0f : lrr; + cart.SentSpeedRFL = wheelCommandSuppressed ? 0f : rfl; + cart.SentSpeedRFR = wheelCommandSuppressed ? 0f : rfr; + cart.SentSpeedRRL = wheelCommandSuppressed ? 0f : rrl; + cart.SentSpeedRRR = wheelCommandSuppressed ? 0f : rrr; + cart.WheelCommandLimitedLFL = + Math.Abs(cart.SpeedLFL) > speedLimit; + cart.WheelCommandLimitedLFR = + Math.Abs(cart.SpeedLFR) > speedLimit; + cart.WheelCommandLimitedLRL = + Math.Abs(cart.SpeedLRL) > speedLimit; + cart.WheelCommandLimitedLRR = + Math.Abs(cart.SpeedLRR) > speedLimit; + cart.WheelCommandLimitedRFL = + Math.Abs(cart.SpeedRFL) > speedLimit; + cart.WheelCommandLimitedRFR = + Math.Abs(cart.SpeedRFR) > speedLimit; + cart.WheelCommandLimitedRRL = + Math.Abs(cart.SpeedRRL) > speedLimit; + cart.WheelCommandLimitedRRR = + Math.Abs(cart.SpeedRRR) > speedLimit; + cart.WheelCommandSuppressed = wheelCommandSuppressed; + Interlocked.Exchange( + ref cart.WheelCommandTimestamp, + Stopwatch.GetTimestamp()); + Interlocked.Increment( + ref cart.WheelCommandSequence); + if (iteration % 2 == 0) { //SendNodeGuardRequests(SendCan); @@ -685,13 +738,18 @@ namespace MedullaAdapter if (payload == null || payload.Length < 8) return; var rpm = DecodeRpmFromPayload(payload); var speed = ConvertRpm2Mps(rpm); + var positionMillimeters = + ConvertR2MM( + BitConverter.ToInt32(payload, 0) / + 10000f); cart.ActualSpeedLeftFrontLeft = speed; + cart.LFLActualPos = positionMillimeters; _wheelSpeedLogger.RecordCanFeedback( 0x281, "LFL", rpm, - speed); - cart.LFLActualPos = ConvertR2MM(BitConverter.ToInt32(payload, 0) / 10000f); + speed, + positionMillimeters); }, [0x282] = (msg) => { @@ -700,13 +758,18 @@ namespace MedullaAdapter if (payload == null || payload.Length < 8) return; var rpm = DecodeRpmFromPayload(payload); var speed = -ConvertRpm2Mps(rpm); + var positionMillimeters = + ConvertR2MM( + -BitConverter.ToInt32(payload, 0) / + 10000f); cart.ActualSpeedLeftFrontRight = speed; + cart.LFRActualPos = positionMillimeters; _wheelSpeedLogger.RecordCanFeedback( 0x282, "LFR", rpm, - speed); - cart.LFRActualPos = ConvertR2MM(-BitConverter.ToInt32(payload, 0) / 10000f); + speed, + positionMillimeters); }, [0x283] = (msg) => { @@ -715,13 +778,18 @@ namespace MedullaAdapter if (payload == null || payload.Length < 8) return; var rpm = DecodeRpmFromPayload(payload); var speed = ConvertRpm2Mps(rpm); + var positionMillimeters = + ConvertR2MM( + BitConverter.ToInt32(payload, 0) / + 10000f); cart.ActualSpeedRightFrontLeft = speed; + cart.RFLActualPos = positionMillimeters; _wheelSpeedLogger.RecordCanFeedback( 0x283, "RFL", rpm, - speed); - cart.RFLActualPos = ConvertR2MM(BitConverter.ToInt32(payload, 0) / 10000f); + speed, + positionMillimeters); }, [0x284] = (msg) => { @@ -730,13 +798,18 @@ namespace MedullaAdapter if (payload == null || payload.Length < 8) return; var rpm = DecodeRpmFromPayload(payload); var speed = -ConvertRpm2Mps(rpm); + var positionMillimeters = + ConvertR2MM( + -BitConverter.ToInt32(payload, 0) / + 10000f); cart.ActualSpeedRightFrontRight = speed; + cart.RFRActualPos = positionMillimeters; _wheelSpeedLogger.RecordCanFeedback( 0x284, "RFR", rpm, - speed); - cart.RFRActualPos = ConvertR2MM(-BitConverter.ToInt32(payload, 0) / 10000f); + speed, + positionMillimeters); }, [0x285] = (msg) => { @@ -745,13 +818,18 @@ namespace MedullaAdapter if (payload == null || payload.Length < 8) return; var rpm = DecodeRpmFromPayload(payload); var speed = ConvertRpm2Mps(rpm); + var positionMillimeters = + ConvertR2MM( + BitConverter.ToInt32(payload, 0) / + 10000f); cart.ActualSpeedLeftRearLeft = speed; + cart.LRLActualPos = positionMillimeters; _wheelSpeedLogger.RecordCanFeedback( 0x285, "LRL", rpm, - speed); - cart.LRLActualPos = ConvertR2MM(BitConverter.ToInt32(payload, 0) / 10000f); + speed, + positionMillimeters); }, [0x286] = (msg) => { @@ -760,13 +838,18 @@ namespace MedullaAdapter if (payload == null || payload.Length < 8) return; var rpm = DecodeRpmFromPayload(payload); var speed = -ConvertRpm2Mps(rpm); + var positionMillimeters = + ConvertR2MM( + -BitConverter.ToInt32(payload, 0) / + 10000f); cart.ActualSpeedLeftRearRight = speed; + cart.LRRActualPos = positionMillimeters; _wheelSpeedLogger.RecordCanFeedback( 0x286, "LRR", rpm, - speed); - cart.LRRActualPos = ConvertR2MM(-BitConverter.ToInt32(payload, 0) / 10000f); + speed, + positionMillimeters); }, [0x287] = (msg) => { @@ -775,13 +858,18 @@ namespace MedullaAdapter if (payload == null || payload.Length < 8) return; var rpm = DecodeRpmFromPayload(payload); var speed = ConvertRpm2Mps(rpm); + var positionMillimeters = + ConvertR2MM( + BitConverter.ToInt32(payload, 0) / + 10000f); cart.ActualSpeedRightRearLeft = speed; + cart.RRLActualPos = positionMillimeters; _wheelSpeedLogger.RecordCanFeedback( 0x287, "RRL", rpm, - speed); - cart.RRLActualPos = ConvertR2MM(BitConverter.ToInt32(payload, 0) / 10000f); + speed, + positionMillimeters); }, [0x288] = (msg) => { @@ -790,13 +878,18 @@ namespace MedullaAdapter if (payload == null || payload.Length < 8) return; var rpm = DecodeRpmFromPayload(payload); var speed = -ConvertRpm2Mps(rpm); + var positionMillimeters = + ConvertR2MM( + -BitConverter.ToInt32(payload, 0) / + 10000f); cart.ActualSpeedRightRearRight = speed; + cart.RRRActualPos = positionMillimeters; _wheelSpeedLogger.RecordCanFeedback( 0x288, "RRR", rpm, - speed); - cart.RRRActualPos = ConvertR2MM(-BitConverter.ToInt32(payload, 0) / 10000f); + speed, + positionMillimeters); }, [0x289] = (msg) => { @@ -914,10 +1007,18 @@ namespace MedullaAdapter //Console.WriteLine("Received 0x18B CAN Message"); var payload = msg.Payload; if (payload == null || payload.Length < 4) return; - cart.ActualThLeftFront = BitConverter.ToInt32(payload, 0); - cart.ActualThLeftFront = cart.ActualThLeftFront >= 16384 - ? (cart.ActualThLeftFront - 98303) / 4096f / 5f * 360 - cart.ThBiasLeftFront - : cart.ActualThLeftFront / 4096f / 5f * 360 - cart.ThBiasLeftFront; + var raw = BitConverter.ToInt32(payload, 0); + cart.ActualThLeftFront = raw >= 16384 + ? (raw - 98303) / 4096f / 5f * 360 - cart.ThBiasLeftFront + : raw / 4096f / 5f * 360 - cart.ThBiasLeftFront; + MarkFeedbackReceived( + ref cart.ActualThLeftFrontTimestamp, + ref cart.ActualThLeftFrontSequence); + _wheelSpeedLogger.RecordSteeringAngleFeedback( + 0x18B, + "LF", + raw, + cart.ActualThLeftFront); }, [0x18C] = (msg) => { @@ -927,6 +1028,14 @@ namespace MedullaAdapter cart.ActualThRightFront = raw >= 16384 ? (raw - 98303) / 4096f / 5f * 360 - cart.ThBiasRightFront : raw / 4096f / 5f * 360 - cart.ThBiasRightFront; + MarkFeedbackReceived( + ref cart.ActualThRightFrontTimestamp, + ref cart.ActualThRightFrontSequence); + _wheelSpeedLogger.RecordSteeringAngleFeedback( + 0x18C, + "RF", + raw, + cart.ActualThRightFront); //DLog.Log($"RF raw=0x{raw:X8}({raw}) angle={cart.ActualThRightFront:F2}", "0x18C"); }, [0x18D] = (msg) => @@ -934,20 +1043,36 @@ namespace MedullaAdapter //Console.WriteLine($"Received 0x18D CAN Message {DateTime.Now:yyyy-MM-dd HH:mm:ss.ffffff}"); var payload = msg.Payload; if (payload == null || payload.Length < 4) return; - cart.ActualThLeftRear = BitConverter.ToInt32(payload, 0); - cart.ActualThLeftRear = cart.ActualThLeftRear >= 16384 - ? (cart.ActualThLeftRear - 98303) / 4096f / 5f * 360 - cart.ThBiasLeftRear - : cart.ActualThLeftRear / 4096f / 5f * 360 - cart.ThBiasLeftRear; + var raw = BitConverter.ToInt32(payload, 0); + cart.ActualThLeftRear = raw >= 16384 + ? (raw - 98303) / 4096f / 5f * 360 - cart.ThBiasLeftRear + : raw / 4096f / 5f * 360 - cart.ThBiasLeftRear; + MarkFeedbackReceived( + ref cart.ActualThLeftRearTimestamp, + ref cart.ActualThLeftRearSequence); + _wheelSpeedLogger.RecordSteeringAngleFeedback( + 0x18D, + "LR", + raw, + cart.ActualThLeftRear); }, [0x18E] = (msg) => { //Console.WriteLine("Received 0x18E CAN Message"); var payload = msg.Payload; if (payload == null || payload.Length < 4) return; - cart.ActualThRightRear = BitConverter.ToInt32(payload, 0); - cart.ActualThRightRear = cart.ActualThRightRear >= 16384 - ? (cart.ActualThRightRear - 98303) / 4096f / 5f * 360 - cart.ThBiasRightRear - : cart.ActualThRightRear / 4096f / 5f * 360 - cart.ThBiasRightRear; + var raw = BitConverter.ToInt32(payload, 0); + cart.ActualThRightRear = raw >= 16384 + ? (raw - 98303) / 4096f / 5f * 360 - cart.ThBiasRightRear + : raw / 4096f / 5f * 360 - cart.ThBiasRightRear; + MarkFeedbackReceived( + ref cart.ActualThRightRearTimestamp, + ref cart.ActualThRightRearSequence); + _wheelSpeedLogger.RecordSteeringAngleFeedback( + 0x18E, + "RR", + raw, + cart.ActualThRightRear); }, // 远程帧 diff --git a/MedullaAdapter/MotorRoutine.cs b/MedullaAdapter/MotorRoutine.cs index bc0165f..e210ffb 100644 --- a/MedullaAdapter/MotorRoutine.cs +++ b/MedullaAdapter/MotorRoutine.cs @@ -4,6 +4,7 @@ using FundamentalLib; using MDCSToolBox.Commons; using System; using System.Diagnostics; +using System.Threading; using static MDCSToolBox.Medulla.Chassis.BasicCartDefinition; namespace MedullaAdapter @@ -75,6 +76,11 @@ namespace MedullaAdapter cart.ClumsyControl = CartDefinition.currentPriority == 0; // 计算四个舵轮PID和8个驱动电机最终速度。 UpdateDiffSteerWheelSpeeds(); + Interlocked.Exchange( + ref cart.DiffSteerControlTimestamp, + Stopwatch.GetTimestamp()); + Interlocked.Increment( + ref cart.DiffSteerControlSequence); // 平滑更新硬件速度限制。 UpdateSendSpeedLimit(); // 更新红黄绿灯状态。 @@ -351,6 +357,15 @@ namespace MedullaAdapter leftRear = 0f; rightFront = 0f; rightRear = 0f; + cart.DiffSteerTargetRateLeftFrontDegreesPerSecond = 0f; + cart.DiffSteerTargetRateLeftRearDegreesPerSecond = 0f; + cart.DiffSteerTargetRateRightFrontDegreesPerSecond = 0f; + cart.DiffSteerTargetRateRightRearDegreesPerSecond = 0f; + cart.DiffSteerFeedforwardDeltaTimeMilliseconds = 0f; + cart.DiffSteerFeedforwardLimitedLeftFront = false; + cart.DiffSteerFeedforwardLimitedLeftRear = false; + cart.DiffSteerFeedforwardLimitedRightFront = false; + cart.DiffSteerFeedforwardLimitedRightRear = false; var currentTimestamp = Stopwatch.GetTimestamp(); @@ -365,22 +380,45 @@ namespace MedullaAdapter deltaTimeSeconds <= MaximumFeedforwardIntervalSeconds) { + cart.DiffSteerFeedforwardDeltaTimeMilliseconds = + (float)(deltaTimeSeconds * 1000.0); leftFront = CalculateDiffSteerRateFeedforward( cart.ThLeftFront, _previousThLeftFront, - deltaTimeSeconds); + deltaTimeSeconds, + out var targetRateLf, + out var limitedLf); leftRear = CalculateDiffSteerRateFeedforward( cart.ThLeftRear, _previousThLeftRear, - deltaTimeSeconds); + deltaTimeSeconds, + out var targetRateLr, + out var limitedLr); rightFront = CalculateDiffSteerRateFeedforward( cart.ThRightFront, _previousThRightFront, - deltaTimeSeconds); + deltaTimeSeconds, + out var targetRateRf, + out var limitedRf); rightRear = CalculateDiffSteerRateFeedforward( cart.ThRightRear, _previousThRightRear, - deltaTimeSeconds); + deltaTimeSeconds, + out var targetRateRr, + out var limitedRr); + + cart.DiffSteerTargetRateLeftFrontDegreesPerSecond = + targetRateLf; + cart.DiffSteerTargetRateLeftRearDegreesPerSecond = + targetRateLr; + cart.DiffSteerTargetRateRightFrontDegreesPerSecond = + targetRateRf; + cart.DiffSteerTargetRateRightRearDegreesPerSecond = + targetRateRr; + cart.DiffSteerFeedforwardLimitedLeftFront = limitedLf; + cart.DiffSteerFeedforwardLimitedLeftRear = limitedLr; + cart.DiffSteerFeedforwardLimitedRightFront = limitedRf; + cart.DiffSteerFeedforwardLimitedRightRear = limitedRr; } } @@ -398,29 +436,44 @@ namespace MedullaAdapter private float CalculateDiffSteerRateFeedforward( float targetAngleDegrees, float previousTargetAngleDegrees, - double deltaTimeSeconds) + double deltaTimeSeconds, + out float targetRateDegreesPerSecond, + out bool limited) { + targetRateDegreesPerSecond = 0f; + limited = false; + var gain = cart.DiffSteerRateFeedforwardGain; var wheelDistanceMillimeters = cart.DiffSteerWheelDistanceMillimeters; var maximumSpeed = cart.DiffSteerRateFeedforwardMaximumSpeed; - if (!IsFinite(gain) || gain <= 0f || - !IsFinite(wheelDistanceMillimeters) || - wheelDistanceMillimeters <= 0f || - !IsFinite(maximumSpeed) || maximumSpeed <= 0f || - !IsFinite(targetAngleDegrees) || - !IsFinite(previousTargetAngleDegrees)) + if (!IsFinite(targetAngleDegrees) || + !IsFinite(previousTargetAngleDegrees) || + !double.IsFinite(deltaTimeSeconds) || + deltaTimeSeconds <= 0.0) { return 0f; } // 机械舵角受限，必须使用直接差值而不是圆周最短角差。 + targetRateDegreesPerSecond = + (float)((targetAngleDegrees - + previousTargetAngleDegrees) / + deltaTimeSeconds); + + if (!IsFinite(gain) || gain <= 0f || + !IsFinite(wheelDistanceMillimeters) || + wheelDistanceMillimeters <= 0f || + !IsFinite(maximumSpeed) || maximumSpeed <= 0f) + { + return 0f; + } + var targetRateRadiansPerSecond = - (targetAngleDegrees - previousTargetAngleDegrees) * - Math.PI / 180.0 / - deltaTimeSeconds; + targetRateDegreesPerSecond * + Math.PI / 180.0; var wheelDistanceMeters = wheelDistanceMillimeters / 1000.0; var feedforwardSpeed = @@ -429,10 +482,13 @@ namespace MedullaAdapter targetRateRadiansPerSecond * gain; - return (float)Math.Clamp( + var limitedSpeed = (float)Math.Clamp( feedforwardSpeed, -maximumSpeed, maximumSpeed); + limited = Math.Abs( + feedforwardSpeed - limitedSpeed) > 1e-9; + return limitedSpeed; } /// @@ -446,6 +502,15 @@ namespace MedullaAdapter cart.DiffSteerRateFeedforwardLeftRear = 0f; cart.DiffSteerRateFeedforwardRightFront = 0f; cart.DiffSteerRateFeedforwardRightRear = 0f; + cart.DiffSteerTargetRateLeftFrontDegreesPerSecond = 0f; + cart.DiffSteerTargetRateLeftRearDegreesPerSecond = 0f; + cart.DiffSteerTargetRateRightFrontDegreesPerSecond = 0f; + cart.DiffSteerTargetRateRightRearDegreesPerSecond = 0f; + cart.DiffSteerFeedforwardDeltaTimeMilliseconds = 0f; + cart.DiffSteerFeedforwardLimitedLeftFront = false; + cart.DiffSteerFeedforwardLimitedLeftRear = false; + cart.DiffSteerFeedforwardLimitedRightFront = false; + cart.DiffSteerFeedforwardLimitedRightRear = false; cart.DiffSteerTotalOutputLeftFront = 0f; cart.DiffSteerTotalOutputLeftRear = 0f; cart.DiffSteerTotalOutputRightFront = 0f; diff --git a/MedullaAdapter/WheelSpeedDiagnosticLogger.cs b/MedullaAdapter/WheelSpeedDiagnosticLogger.cs index d55975f..ef97adf 100644 --- a/MedullaAdapter/WheelSpeedDiagnosticLogger.cs +++ b/MedullaAdapter/WheelSpeedDiagnosticLogger.cs @@ -38,8 +38,10 @@ namespace MedullaAdapter private volatile bool _isRunning; private int _queuedRecordCount; private long _receiveSequence; + private long _snapshotSequence; private long _droppedRecordCount; private double _lastSnapshotMilliseconds = double.NegativeInfinity; + private long _startTimestamp; public bool IsRunning => _isRunning; @@ -79,22 +81,43 @@ namespace MedullaAdapter _snapshotWriter = CreateWriter(SnapshotLogPath); _canWriter.WriteLine( - "ElapsedMs,ReceiveSequence,CanId,MotorName,RawRpm,SpeedMps"); + "ElapsedMs,ReceiveSequence,CanId,EventType,ChannelName," + + "RawRpm,SpeedMps,PositionMm,RawAngle,AngleDegrees"); _snapshotWriter.WriteLine( - "ElapsedMs,CarNum,ManualControlMode,ManualMode,SendThresSpeed," + + "ElapsedMs,SnapshotSequence," + + "ControlElapsedMs,ControlSequence,ControlAgeMs," + + "WheelCommandElapsedMs,WheelCommandSequence,WheelCommandAgeMs," + + "CarNum,ManualControlMode,ManualMode,SendThresSpeed," + + "VoltageV,AlarmLevel,ChassisMode,WheelAbleState," + "DiffSteerKp,DiffSteerKi,DiffSteerKd,DiffSteerMaxI,DiffSteerDeadZone,DiffSteerThresh,DiffSteerSpeedAcc," + "DiffSteerRateFeedforwardGain,DiffSteerWheelDistanceMillimeters,DiffSteerRateFeedforwardMaximumSpeed," + + "FeedforwardDeltaTimeMs," + + "TargetRateThLeftFrontDegreesPerSecond,TargetRateThLeftRearDegreesPerSecond," + + "TargetRateThRightFrontDegreesPerSecond,TargetRateThRightRearDegreesPerSecond," + + "FeedforwardLimitedLeftFront,FeedforwardLimitedLeftRear,FeedforwardLimitedRightFront,FeedforwardLimitedRightRear," + "PidOutLeftFront,PidOutLeftRear,PidOutRightFront,PidOutRightRear," + "RateFeedforwardLeftFront,RateFeedforwardLeftRear,RateFeedforwardRightFront,RateFeedforwardRightRear," + "TotalDiffLeftFront,TotalDiffLeftRear,TotalDiffRightFront,TotalDiffRightRear," + "CmdLFL,CmdLFR,CmdLRL,CmdLRR,CmdRFL,CmdRFR,CmdRRL,CmdRRR," + "PidLFL,PidLFR,PidLRL,PidLRR,PidRFL,PidRFR,PidRRL,PidRRR," + + "SentLFLMps,SentLFRMps,SentLRLMps,SentLRRMps,SentRFLMps,SentRFRMps,SentRRLMps,SentRRRMps," + + "CommandLimitedLFL,CommandLimitedLFR,CommandLimitedLRL,CommandLimitedLRR," + + "CommandLimitedRFL,CommandLimitedRFR,CommandLimitedRRL,CommandLimitedRRR," + + "PairCommandLimitedLeftFront,PairCommandLimitedLeftRear," + + "PairCommandLimitedRightFront,PairCommandLimitedRightRear,WheelCommandSuppressed," + "ActualLFL,ActualLFR,ActualLRL,ActualLRR,ActualRFL,ActualRFR,ActualRRL,ActualRRR," + "ActualLeftFront,ActualLeftRear,ActualRightFront,ActualRightRear," + + "PositionLFL,PositionLFR,PositionLRL,PositionLRR,PositionRFL,PositionRFR,PositionRRL,PositionRRR," + + "CurrentLFLAmps,CurrentLFRAmps,CurrentLRLAmps,CurrentLRRAmps," + + "CurrentRFLAmps,CurrentRFRAmps,CurrentRRLAmps,CurrentRRRAmps," + "TargetThLeftFront,TargetThLeftRear,TargetThRightFront,TargetThRightRear," + "ActualThLeftFront,ActualThLeftRear,ActualThRightFront,ActualThRightRear," + - "ErrorThLeftFront,ErrorThLeftRear,ErrorThRightFront,ErrorThRightRear"); + "ErrorThLeftFront,ErrorThLeftRear,ErrorThRightFront,ErrorThRightRear," + + "ActualThLeftFrontReceiveElapsedMs,ActualThLeftFrontReceiveSequence,ActualThLeftFrontAgeMs," + + "ActualThLeftRearReceiveElapsedMs,ActualThLeftRearReceiveSequence,ActualThLeftRearAgeMs," + + "ActualThRightFrontReceiveElapsedMs,ActualThRightFrontReceiveSequence,ActualThRightFrontAgeMs," + + "ActualThRightRearReceiveElapsedMs,ActualThRightRearReceiveSequence,ActualThRightRearAgeMs"); while (_records.TryDequeue(out _)) { @@ -102,9 +125,11 @@ namespace MedullaAdapter _queuedRecordCount = 0; _receiveSequence = 0; + _snapshotSequence = 0; _droppedRecordCount = 0; _lastSnapshotMilliseconds = double.NegativeInfinity; + _startTimestamp = Stopwatch.GetTimestamp(); _stopwatch = Stopwatch.StartNew(); _isRunning = true; @@ -157,13 +182,15 @@ namespace MedullaAdapter ushort canId, string motorName, float rawRpm, - float speedMetersPerSecond) + float speedMetersPerSecond, + float positionMillimeters) { if (!_isRunning) return; var elapsedMilliseconds = - _stopwatch.Elapsed.TotalMilliseconds; + GetElapsedMilliseconds( + Stopwatch.GetTimestamp()); var receiveSequence = Interlocked.Increment( ref _receiveSequence); @@ -174,9 +201,52 @@ namespace MedullaAdapter receiveSequence.ToString( CultureInfo.InvariantCulture), $"0x{canId:X3}", + "MotorSpeedPosition", motorName, Format(rawRpm), - Format(speedMetersPerSecond)); + Format(speedMetersPerSecond), + Format(positionMillimeters), + "", + ""); + + Enqueue(new LogRecord( + isCanEvent: true, + line)); + } + + /// + /// 按CAN回调到达时刻记录一帧舵角原始值和换算后的机械角度。 + /// + public void RecordSteeringAngleFeedback( + ushort canId, + string wheelName, + int rawAngle, + float angleDegrees) + { + if (!_isRunning) + return; + + var elapsedMilliseconds = + GetElapsedMilliseconds( + Stopwatch.GetTimestamp()); + var receiveSequence = + Interlocked.Increment( + ref _receiveSequence); + + var line = string.Join( + ",", + Format(elapsedMilliseconds), + receiveSequence.ToString( + CultureInfo.InvariantCulture), + $"0x{canId:X3}", + "SteeringAngle", + wheelName, + "", + "", + "", + rawAngle.ToString( + CultureInfo.InvariantCulture), + Format(angleDegrees)); Enqueue(new LogRecord( isCanEvent: true, @@ -192,8 +262,9 @@ namespace MedullaAdapter if (!_isRunning || cart == null) return; + var snapshotTimestamp = Stopwatch.GetTimestamp(); var elapsedMilliseconds = - _stopwatch.Elapsed.TotalMilliseconds; + GetElapsedMilliseconds(snapshotTimestamp); if (elapsedMilliseconds - _lastSnapshotMilliseconds < @@ -205,14 +276,72 @@ namespace MedullaAdapter _lastSnapshotMilliseconds = elapsedMilliseconds; + var snapshotSequence = + Interlocked.Increment( + ref _snapshotSequence); + var controlTimestamp = + Interlocked.Read( + ref cart.DiffSteerControlTimestamp); + var controlSequence = + Interlocked.Read( + ref cart.DiffSteerControlSequence); + var commandTimestamp = + Interlocked.Read( + ref cart.WheelCommandTimestamp); + var commandSequence = + Interlocked.Read( + ref cart.WheelCommandSequence); + var actualThLeftFrontTimestamp = + Interlocked.Read( + ref cart.ActualThLeftFrontTimestamp); + var actualThLeftFrontSequence = + Interlocked.Read( + ref cart.ActualThLeftFrontSequence); + var actualThLeftRearTimestamp = + Interlocked.Read( + ref cart.ActualThLeftRearTimestamp); + var actualThLeftRearSequence = + Interlocked.Read( + ref cart.ActualThLeftRearSequence); + var actualThRightFrontTimestamp = + Interlocked.Read( + ref cart.ActualThRightFrontTimestamp); + var actualThRightFrontSequence = + Interlocked.Read( + ref cart.ActualThRightFrontSequence); + var actualThRightRearTimestamp = + Interlocked.Read( + ref cart.ActualThRightRearTimestamp); + var actualThRightRearSequence = + Interlocked.Read( + ref cart.ActualThRightRearSequence); + var line = string.Join( ",", Format(elapsedMilliseconds), + snapshotSequence.ToString( + CultureInfo.InvariantCulture), + FormatEventElapsedMilliseconds(controlTimestamp), + controlSequence.ToString( + CultureInfo.InvariantCulture), + FormatEventAgeMilliseconds( + snapshotTimestamp, + controlTimestamp), + FormatEventElapsedMilliseconds(commandTimestamp), + commandSequence.ToString( + CultureInfo.InvariantCulture), + FormatEventAgeMilliseconds( + snapshotTimestamp, + commandTimestamp), cart.CarNum.ToString( CultureInfo.InvariantCulture), Format((int)cart.TransmitterControlMode), Format(cart.ManualMode), Format(cart.SendThresSpeed), + Format(cart.Voltage), + Format(cart.AlarmLevel), + Format(cart.ChassisMode), + FormatBoolean(cart.WheelAbleState), Format(cart.DiffSteerKp), Format(cart.DiffSteerKi), Format(cart.DiffSteerKd), @@ -223,6 +352,15 @@ namespace MedullaAdapter Format(cart.DiffSteerRateFeedforwardGain), Format(cart.DiffSteerWheelDistanceMillimeters), Format(cart.DiffSteerRateFeedforwardMaximumSpeed), + Format(cart.DiffSteerFeedforwardDeltaTimeMilliseconds), + Format(cart.DiffSteerTargetRateLeftFrontDegreesPerSecond), + Format(cart.DiffSteerTargetRateLeftRearDegreesPerSecond), + Format(cart.DiffSteerTargetRateRightFrontDegreesPerSecond), + Format(cart.DiffSteerTargetRateRightRearDegreesPerSecond), + FormatBoolean(cart.DiffSteerFeedforwardLimitedLeftFront), + FormatBoolean(cart.DiffSteerFeedforwardLimitedLeftRear), + FormatBoolean(cart.DiffSteerFeedforwardLimitedRightFront), + FormatBoolean(cart.DiffSteerFeedforwardLimitedRightRear), Format(cart.DiffSteerOutputLeftFront), Format(cart.DiffSteerOutputLeftRear), Format(cart.DiffSteerOutputRightFront), @@ -251,6 +389,35 @@ namespace MedullaAdapter Format(cart.SpeedRFR), Format(cart.SpeedRRL), Format(cart.SpeedRRR), + Format(cart.SentSpeedLFL), + Format(cart.SentSpeedLFR), + Format(cart.SentSpeedLRL), + Format(cart.SentSpeedLRR), + Format(cart.SentSpeedRFL), + Format(cart.SentSpeedRFR), + Format(cart.SentSpeedRRL), + Format(cart.SentSpeedRRR), + FormatBoolean(cart.WheelCommandLimitedLFL), + FormatBoolean(cart.WheelCommandLimitedLFR), + FormatBoolean(cart.WheelCommandLimitedLRL), + FormatBoolean(cart.WheelCommandLimitedLRR), + FormatBoolean(cart.WheelCommandLimitedRFL), + FormatBoolean(cart.WheelCommandLimitedRFR), + FormatBoolean(cart.WheelCommandLimitedRRL), + FormatBoolean(cart.WheelCommandLimitedRRR), + FormatBoolean( + cart.WheelCommandLimitedLFL || + cart.WheelCommandLimitedLFR), + FormatBoolean( + cart.WheelCommandLimitedLRL || + cart.WheelCommandLimitedLRR), + FormatBoolean( + cart.WheelCommandLimitedRFL || + cart.WheelCommandLimitedRFR), + FormatBoolean( + cart.WheelCommandLimitedRRL || + cart.WheelCommandLimitedRRR), + FormatBoolean(cart.WheelCommandSuppressed), Format(cart.ActualSpeedLeftFrontLeft), Format(cart.ActualSpeedLeftFrontRight), Format(cart.ActualSpeedLeftRearLeft), @@ -263,6 +430,22 @@ namespace MedullaAdapter Format(cart.ActualSpeedLeftRear), Format(cart.ActualSpeedRightFront), Format(cart.ActualSpeedRightRear), + Format(cart.LFLActualPos), + Format(cart.LFRActualPos), + Format(cart.LRLActualPos), + Format(cart.LRRActualPos), + Format(cart.RFLActualPos), + Format(cart.RFRActualPos), + Format(cart.RRLActualPos), + Format(cart.RRRActualPos), + Format(cart.LeftFrontLeftElectric), + Format(cart.LeftFrontRightElectric), + Format(cart.LeftRearLeftElectric), + Format(cart.LeftRearRightElectric), + Format(cart.RightFrontLeftElectric), + Format(cart.RightFrontRightElectric), + Format(cart.RightRearLeftElectric), + Format(cart.RightRearRightElectric), Format(cart.ThLeftFront), Format(cart.ThLeftRear), Format(cart.ThRightFront), @@ -274,7 +457,35 @@ namespace MedullaAdapter Format(cart.ThLeftFront - cart.ActualThLeftFront), Format(cart.ThLeftRear - cart.ActualThLeftRear), Format(cart.ThRightFront - cart.ActualThRightFront), - Format(cart.ThRightRear - cart.ActualThRightRear)); + Format(cart.ThRightRear - cart.ActualThRightRear), + FormatEventElapsedMilliseconds( + actualThLeftFrontTimestamp), + actualThLeftFrontSequence.ToString( + CultureInfo.InvariantCulture), + FormatEventAgeMilliseconds( + snapshotTimestamp, + actualThLeftFrontTimestamp), + FormatEventElapsedMilliseconds( + actualThLeftRearTimestamp), + actualThLeftRearSequence.ToString( + CultureInfo.InvariantCulture), + FormatEventAgeMilliseconds( + snapshotTimestamp, + actualThLeftRearTimestamp), + FormatEventElapsedMilliseconds( + actualThRightFrontTimestamp), + actualThRightFrontSequence.ToString( + CultureInfo.InvariantCulture), + FormatEventAgeMilliseconds( + snapshotTimestamp, + actualThRightFrontTimestamp), + FormatEventElapsedMilliseconds( + actualThRightRearTimestamp), + actualThRightRearSequence.ToString( + CultureInfo.InvariantCulture), + FormatEventAgeMilliseconds( + snapshotTimestamp, + actualThRightRearTimestamp)); Enqueue(new LogRecord( isCanEvent: false, @@ -386,6 +597,54 @@ namespace MedullaAdapter CultureInfo.InvariantCulture); } + /// + /// 将诊断布尔值写成便于MATLAB直接读取的0或1。 + /// + private static string FormatBoolean(bool value) + { + return value ? "1" : "0"; + } + + /// + /// 将本机单调时钟值换算为相对本次日志开始的毫秒数。 + /// + private double GetElapsedMilliseconds(long timestamp) + { + return (timestamp - _startTimestamp) * + 1000.0 / + Stopwatch.Frequency; + } + + /// + /// 格式化发生在本次记录期间的事件时刻，记录前事件返回空字段。 + /// + private string FormatEventElapsedMilliseconds(long timestamp) + { + if (timestamp < _startTimestamp) + return ""; + + return Format(GetElapsedMilliseconds(timestamp)); + } + + /// + /// 计算快照时刻相对最近一次控制或反馈事件的数据年龄。 + /// + private static string FormatEventAgeMilliseconds( + long currentTimestamp, + long eventTimestamp) + { + if (eventTimestamp <= 0 || + eventTimestamp > currentTimestamp) + { + return ""; + } + + return Format( + (currentTimestamp - eventTimestamp) * + 1000.0 / + Stopwatch.Frequency); + } + public void Dispose() { Stop(); diff --git a/MedullaAdapter/build/Medulla/plugins/CommonUsage.dll b/MedullaAdapter/build/Medulla/plugins/CommonUsage.dll index 403ea8b..2c0496a 100644 Binary files a/MedullaAdapter/build/Medulla/plugins/CommonUsage.dll and b/MedullaAdapter/build/Medulla/plugins/CommonUsage.dll differ diff --git a/MedullaAdapter/build/Medulla/plugins/MedullaAdapter.dll b/MedullaAdapter/build/Medulla/plugins/MedullaAdapter.dll index 3eb4a89..13f09f7 100644 Binary files a/MedullaAdapter/build/Medulla/plugins/MedullaAdapter.dll and b/MedullaAdapter/build/Medulla/plugins/MedullaAdapter.dll differ diff --git a/MedullaAdapter/build/Medulla/plugins/MedullaAdapter.pdb b/MedullaAdapter/build/Medulla/plugins/MedullaAdapter.pdb index 2f446aa..1f8a518 100644 Binary files a/MedullaAdapter/build/Medulla/plugins/MedullaAdapter.pdb and b/MedullaAdapter/build/Medulla/plugins/MedullaAdapter.pdb differ diff --git a/MultiWheelC.Tests/FleetControllerTests.cs b/MultiWheelC.Tests/FleetControllerTests.cs index 3667bd8..6a6dbe3 100644 --- a/MultiWheelC.Tests/FleetControllerTests.cs +++ b/MultiWheelC.Tests/FleetControllerTests.cs @@ -15,13 +15,15 @@ namespace MultiWheelC.Tests { VerifyInactiveControllerStops(); VerifyStraightCommand(); + VerifyFortyFiveDegreeMotionDirection(); + VerifyNinetyDegreeMotionDirection(); VerifyInvalidVelocityUsesReferenceSpeed(); VerifyCompletionStops(); VerifyExcessiveTrackingErrorFaults(); VerifyCancelStops(); Console.WriteLine( - "FleetController车队中心控制测试通过。共6个场景。"); + "FleetController车队中心控制测试通过。共8个场景。"); } private static void VerifyInactiveControllerStops() @@ -91,6 +93,56 @@ namespace MultiWheelC.Tests "速度尚未初始化"); } + private static void VerifyFortyFiveDegreeMotionDirection() + { + VerifyMotionDirection( + Math.PI / 4.0, + "45度运动方向"); + } + + private static void VerifyNinetyDegreeMotionDirection() + { + VerifyMotionDirection( + Math.PI / 2.0, + "90度运动方向"); + } + + private static void VerifyMotionDirection( + double motionDirectionInFleetRadians, + string scenario) + { + var controller = CreateController( + motionDirectionInFleetRadians); + controller.Start( + CreateStraightTrajectory( + motionDirectionInFleetRadians)); + + var directionX = + Math.Cos(motionDirectionInFleetRadians); + var directionY = + Math.Sin(motionDirectionInFleetRadians); + var result = controller.ComputeCommand( + CreateState( + 0.5 * directionX, + 0.5 * directionY, + 0.4 * directionX, + 0.4 * directionY, + true), + 0.02, + out var command); + + AssertResult( + result, + FleetControlCycleResult.CommandGenerated, + scenario); + AssertTwist( + command.TwistAtReferencePoint, + 0.4 * directionX, + 0.4 * directionY, + 0.0, + scenario); + } + private static void VerifyCompletionStops() { var controller = CreateController(); @@ -156,18 +208,27 @@ namespace MultiWheelC.Tests AssertStop(command, "取消控制"); } - private static FleetController CreateController() + private static FleetController CreateController( + double motionDirectionInFleetRadians = 0.0) { return new FleetController( new StraightLateralController(), new ReferenceLongitudinalController(), new GcpCommandAllocator( Math.PI / 4.0), - virtualControlPointRadiusMeters: 0.5); + virtualControlPointRadiusMeters: 0.5, + motionDirectionInFleetRadians: + motionDirectionInFleetRadians); } - private static Trajectory2D CreateStraightTrajectory() + private static Trajectory2D CreateStraightTrajectory( + double motionDirectionInFleetRadians = 0.0) { + var directionX = + Math.Cos(motionDirectionInFleetRadians); + var directionY = + Math.Sin(motionDirectionInFleetRadians); + return new Trajectory2D( new[] { @@ -178,7 +239,10 @@ namespace MultiWheelC.Tests 0.4), new TrajectoryPoint( 1.0, - new Pose2D(1.0, 0.0, 0.0), + new Pose2D( + directionX, + directionY, + 0.0), 0.0, 0.4) }); @@ -189,6 +253,21 @@ namespace MultiWheelC.Tests double yMeters, double vxMetersPerSecond, bool hasValidVelocityEstimate) + { + return CreateState( + xMeters, + yMeters, + vxMetersPerSecond, + 0.0, + hasValidVelocityEstimate); + } + + private static FleetState CreateState( + double xMeters, + double yMeters, + double vxMetersPerSecond, + double vyMetersPerSecond, + bool hasValidVelocityEstimate) { return new FleetState( sampleTimestampSeconds: 1.0, @@ -198,7 +277,7 @@ namespace MultiWheelC.Tests 0.0), twistAtFleetOriginInWorld: new Twist2D( vxMetersPerSecond, - 0.0, + vyMetersPerSecond, 0.0), hasValidVelocityEstimate: hasValidVelocityEstimate); diff --git a/MultiWheelC.Tests/FleetCoordinatorTests.cs b/MultiWheelC.Tests/FleetCoordinatorTests.cs new file mode 100644 index 0000000..d19f5ec --- /dev/null +++ b/MultiWheelC.Tests/FleetCoordinatorTests.cs @@ -0,0 +1,654 @@ +using System; +using System.Collections.Generic; +using MultiWheelC.Control.Abstractions; +using MultiWheelC.Control.Allocation; +using MultiWheelC.Fleet; +using MultiWheelC.Trajectory; +using MyParking.Shared; + +namespace MultiWheelC.Tests +{ + internal static class FleetCoordinatorTests + { + private const double Tolerance = 1e-9; + + public static void Run() + { + VerifyCommandCycle(); + VerifySmallLayoutErrorIsCorrected(); + VerifyWarningRangeScalesAllCommands(); + VerifyUnavailableStateStopsAndRecovers(); + VerifyUnsafeLayoutFaultsAndLatches(); + VerifyCompletionStops(); + VerifyTrackingFaultStops(); + VerifyCancelReturnsInactive(); + + Console.WriteLine( + "FleetCoordinator车队协调测试通过。共8个场景。"); + } + + private static void VerifyCommandCycle() + { + var layout = CreateLayout(); + var coordinator = CreateCoordinator(); + coordinator.Start(layout, CreateTrajectory()); + + var result = coordinator.ExecuteCycle( + CreateRigidMemberStates( + layout, + new Pose2D(0.5, 0.0, 0.0), + new Twist2D(0.4, 0.0, 0.0), + 1.0), + targetTimestampSeconds: 1.0, + deltaTimeSeconds: 0.02, + out var output); + + AssertResult( + result, + FleetCoordinationCycleResult.CommandGenerated, + "正常协调周期"); + if (!output.State.HasValue) + { + throw new InvalidOperationException( + "正常协调周期没有返回车队状态。"); + } + + AssertNear( + output.State.Value.FleetPoseInWorld.XMeters, + 0.5, + "正常协调周期中心X"); + AssertNear( + output.SpeedScale, + 1.0, + "正常协调周期速度比例"); + AssertTwist( + output.FleetCommand.TwistAtReferencePoint, + 0.4, + 0.0, + 0.0, + "正常协调周期车队命令"); + AssertTwist( + FindCommand(output.MemberCommands, 1) + .TwistInVehicleBody, + 0.4, + 0.0, + 0.0, + "正常协调周期车辆1"); + AssertTwist( + FindCommand(output.MemberCommands, 2) + .TwistInVehicleBody, + -0.4, + 0.0, + 0.0, + "正常协调周期车辆2"); + } + + private static void VerifySmallLayoutErrorIsCorrected() + { + var layout = CreateLayout(); + var coordinator = CreateCoordinator(); + coordinator.Start(layout, CreateTrajectory()); + var result = coordinator.ExecuteCycle( + new[] + { + CreateMemberState( + 1, + new Pose2D(-0.99, 0.0, 0.0), + 1.0), + CreateMemberState( + 2, + new Pose2D(0.99, 0.0, Math.PI), + 1.0) + }, + targetTimestampSeconds: 1.0, + deltaTimeSeconds: 0.02, + out var output); + + AssertResult( + result, + FleetCoordinationCycleResult.CommandGenerated, + "小范围布局误差纠偏"); + AssertNear( + output.SpeedScale, + 1.0, + "小范围布局误差速度比例"); + AssertTwist( + FindCommand(output.BaseMemberCommands, 1) + .TwistInVehicleBody, + 0.4, + 0.0, + 0.0, + "车辆1基础命令"); + AssertTwist( + FindCommand(output.BaseMemberCommands, 2) + .TwistInVehicleBody, + -0.4, + 0.0, + 0.0, + "车辆2基础命令"); + AssertTwist( + FindCommand(output.MemberCommands, 1) + .TwistInVehicleBody, + 0.395, + 0.0, + 0.0, + "车辆1纠偏后命令"); + AssertTwist( + FindCommand(output.MemberCommands, 2) + .TwistInVehicleBody, + -0.405, + 0.0, + 0.0, + "车辆2纠偏后命令"); + } + + private static void VerifyWarningRangeScalesAllCommands() + { + var layout = CreateLayout(); + var coordinator = CreateCoordinator(); + coordinator.Start(layout, CreateTrajectory()); + var result = coordinator.ExecuteCycle( + new[] + { + CreateMemberState( + 1, + new Pose2D(-0.965, 0.0, 0.0), + 1.0), + CreateMemberState( + 2, + new Pose2D(0.965, 0.0, Math.PI), + 1.0) + }, + targetTimestampSeconds: 1.0, + deltaTimeSeconds: 0.02, + out var output); + + AssertResult( + result, + FleetCoordinationCycleResult.CommandGenerated, + "警告区间统一缩放"); + AssertNear( + output.SpeedScale, + 0.5, + "警告区间速度比例"); + AssertTwist( + output.FleetCommand.TwistAtReferencePoint, + 0.2, + 0.0, + 0.0, + "警告区间车队命令"); + AssertTwist( + FindCommand(output.MemberCommands, 1) + .TwistInVehicleBody, + 0.2, + 0.0, + 0.0, + "警告区间车辆1"); + AssertTwist( + FindCommand(output.MemberCommands, 2) + .TwistInVehicleBody, + -0.2, + 0.0, + 0.0, + "警告区间车辆2"); + + if (string.IsNullOrWhiteSpace(output.Reason)) + { + throw new InvalidOperationException( + "警告区间缩放没有返回限制原因。"); + } + } + + private static void VerifyUnavailableStateStopsAndRecovers() + { + var layout = CreateLayout(); + var coordinator = CreateCoordinator(); + coordinator.Start(layout, CreateTrajectory()); + var unavailableStates = CreateRigidMemberStates( + layout, + new Pose2D(0.5, 0.0, 0.0), + new Twist2D(0.4, 0.0, 0.0), + 1.0); + unavailableStates[1] = new FleetMemberStateSample( + unavailableStates[1].VehicleId, + unavailableStates[1].SampleTimestampSeconds, + unavailableStates[1].PoseInWorld, + unavailableStates[1].TwistAtVehicleOriginInWorld, + isStateAvailable: false, + hasValidVelocityEstimate: true); + + var waitingResult = coordinator.ExecuteCycle( + unavailableStates, + targetTimestampSeconds: 1.0, + deltaTimeSeconds: 0.02, + out var waitingOutput); + + AssertResult( + waitingResult, + FleetCoordinationCycleResult.WaitingForState, + "状态暂时不可用"); + AssertStop(waitingOutput, layout.VehicleCount); + + if (!coordinator.IsActive) + { + throw new InvalidOperationException( + "状态暂时不可用不应取消车队控制器。"); + } + + var recoveredResult = coordinator.ExecuteCycle( + CreateRigidMemberStates( + layout, + new Pose2D(0.5, 0.0, 0.0), + new Twist2D(0.4, 0.0, 0.0), + 1.02), + targetTimestampSeconds: 1.02, + deltaTimeSeconds: 0.02, + out _); + + AssertResult( + recoveredResult, + FleetCoordinationCycleResult.CommandGenerated, + "状态恢复"); + } + + private static void VerifyUnsafeLayoutFaultsAndLatches() + { + var layout = CreateLayout(); + var coordinator = CreateCoordinator(); + coordinator.Start(layout, CreateTrajectory()); + var deformedStates = new[] + { + CreateMemberState( + 1, + new Pose2D(-0.9, 0.0, 0.0), + 1.0), + CreateMemberState( + 2, + new Pose2D(0.9, 0.0, Math.PI), + 1.0) + }; + + var result = coordinator.ExecuteCycle( + deformedStates, + targetTimestampSeconds: 1.0, + deltaTimeSeconds: 0.02, + out var output); + + AssertResult( + result, + FleetCoordinationCycleResult.Faulted, + "布局误差超限"); + AssertStop(output, layout.VehicleCount); + + if (!coordinator.IsFaulted || + string.IsNullOrWhiteSpace( + coordinator.LastFailureReason)) + { + throw new InvalidOperationException( + "布局误差故障没有被锁存。"); + } + + var latchedResult = coordinator.ExecuteCycle( + CreateRigidMemberStates( + layout, + new Pose2D(0.5, 0.0, 0.0), + new Twist2D(0.4, 0.0, 0.0), + 1.02), + targetTimestampSeconds: 1.02, + deltaTimeSeconds: 0.02, + out var latchedOutput); + + AssertResult( + latchedResult, + FleetCoordinationCycleResult.Faulted, + "布局误差故障锁存"); + AssertStop(latchedOutput, layout.VehicleCount); + } + + private static void VerifyCompletionStops() + { + var layout = CreateLayout(); + var coordinator = CreateCoordinator(); + coordinator.Start(layout, CreateTrajectory()); + + var result = coordinator.ExecuteCycle( + CreateRigidMemberStates( + layout, + new Pose2D(1.0, 0.0, 0.0), + Twist2D.Zero, + 1.0), + targetTimestampSeconds: 1.0, + deltaTimeSeconds: 0.02, + out var output); + + AssertResult( + result, + FleetCoordinationCycleResult.Completed, + "车队轨迹完成"); + AssertStop(output, layout.VehicleCount); + + if (!coordinator.IsCompleted) + { + throw new InvalidOperationException( + "车队轨迹完成状态没有被保存。"); + } + } + + private static void VerifyTrackingFaultStops() + { + var layout = CreateLayout(); + var coordinator = CreateCoordinator(); + coordinator.Start(layout, CreateTrajectory()); + + var result = coordinator.ExecuteCycle( + CreateRigidMemberStates( + layout, + new Pose2D(0.5, 0.5, 0.0), + Twist2D.Zero, + 1.0), + targetTimestampSeconds: 1.0, + deltaTimeSeconds: 0.02, + out var output); + + AssertResult( + result, + FleetCoordinationCycleResult.Faulted, + "车队中心跟踪故障"); + AssertStop(output, layout.VehicleCount); + + if (!coordinator.IsFaulted) + { + throw new InvalidOperationException( + "车队中心跟踪故障没有传递到协调器。"); + } + } + + private static void VerifyCancelReturnsInactive() + { + var layout = CreateLayout(); + var coordinator = CreateCoordinator(); + coordinator.Start(layout, CreateTrajectory()); + coordinator.Cancel(); + + var result = coordinator.ExecuteCycle( + CreateRigidMemberStates( + layout, + new Pose2D(0.5, 0.0, 0.0), + Twist2D.Zero, + 1.0), + targetTimestampSeconds: 1.0, + deltaTimeSeconds: 0.02, + out var output); + + AssertResult( + result, + FleetCoordinationCycleResult.Inactive, + "取消车队协调"); + AssertStop(output, layout.VehicleCount); + } + + private static FleetCoordinator CreateCoordinator() + { + var estimator = new FleetStateEstimator( + maximumMemberStateAgeSeconds: 0.25, + maximumPositionDisagreementMeters: 0.5, + maximumYawDisagreementRadians: + AngleMath.DegreesToRadians(10.0)); + var controller = new FleetController( + new StraightLateralController(), + new ReferenceLongitudinalController(), + new GcpCommandAllocator(Math.PI / 4.0), + virtualControlPointRadiusMeters: 0.5); + var commandCorrector = + new FleetMemberCommandCorrector( + longitudinalPositionGainPerSecond: 1.0, + lateralPositionGainPerSecond: 1.0, + yawGainPerSecond: 1.0, + positionErrorDeadbandMeters: 0.005, + yawErrorDeadbandRadians: + AngleMath.DegreesToRadians(0.5), + maximumLinearCorrectionMetersPerSecond: + 0.03, + maximumAngularCorrectionRadiansPerSecond: + AngleMath.DegreesToRadians(2.0)); + + return new FleetCoordinator( + estimator, + controller, + commandCorrector, + memberPositionErrorWarningMeters: 0.02, + maximumMemberPositionErrorMeters: 0.05, + memberYawErrorWarningRadians: + AngleMath.DegreesToRadians(1.0), + maximumMemberYawErrorRadians: + AngleMath.DegreesToRadians(3.0)); + } + + private static FleetLayout CreateLayout() + { + return new FleetLayout( + new[] + { + new VehicleLayout( + 1, + new Pose2D(-1.0, 0.0, 0.0)), + new VehicleLayout( + 2, + new Pose2D(1.0, 0.0, Math.PI)) + }); + } + + private static Trajectory2D CreateTrajectory() + { + return new Trajectory2D( + new[] + { + new TrajectoryPoint( + 0.0, + Pose2D.Identity, + 0.0, + 0.4), + new TrajectoryPoint( + 1.0, + new Pose2D(1.0, 0.0, 0.0), + 0.0, + 0.4) + }); + } + + private static FleetMemberStateSample[] + CreateRigidMemberStates( + FleetLayout layout, + Pose2D fleetPoseInWorld, + Twist2D twistAtFleetOriginInWorld, + double timestampSeconds) + { + var states = + new FleetMemberStateSample[layout.VehicleCount]; + + for (var index = 0; + index < layout.Vehicles.Count; + index++) + { + var vehicle = layout.Vehicles[index]; + var poseInWorld = FrameTransform2D.Compose( + fleetPoseInWorld, + vehicle.PoseInFleet); + var offsetX = + poseInWorld.XMeters - + fleetPoseInWorld.XMeters; + var offsetY = + poseInWorld.YMeters - + fleetPoseInWorld.YMeters; + var twistInWorld = new Twist2D( + twistAtFleetOriginInWorld.VxMetersPerSecond - + twistAtFleetOriginInWorld.OmegaRadiansPerSecond * + offsetY, + twistAtFleetOriginInWorld.VyMetersPerSecond + + twistAtFleetOriginInWorld.OmegaRadiansPerSecond * + offsetX, + twistAtFleetOriginInWorld.OmegaRadiansPerSecond); + + states[index] = new FleetMemberStateSample( + vehicle.VehicleId, + timestampSeconds, + poseInWorld, + twistInWorld, + isStateAvailable: true, + hasValidVelocityEstimate: true); + } + + return states; + } + + private static FleetMemberStateSample CreateMemberState( + int vehicleId, + Pose2D poseInWorld, + double timestampSeconds) + { + return new FleetMemberStateSample( + vehicleId, + timestampSeconds, + poseInWorld, + Twist2D.Zero, + isStateAvailable: true, + hasValidVelocityEstimate: true); + } + + private static FleetMemberCommand FindCommand( + IReadOnlyList commands, + int vehicleId) + { + for (var index = 0; + index < commands.Count; + index++) + { + if (commands[index].VehicleId == vehicleId) + { + return commands[index]; + } + } + + throw new InvalidOperationException( + $"没有找到车辆{vehicleId}的成员命令。"); + } + + private static void AssertStop( + FleetCoordinationCycleOutput output, + int expectedMemberCount) + { + AssertTwist( + output.FleetCommand.TwistAtReferencePoint, + 0.0, + 0.0, + 0.0, + "车队停止命令"); + + if (output.MemberCommands.Count != expectedMemberCount) + { + throw new InvalidOperationException( + "停止输出的成员命令数量错误。"); + } + + if (output.BaseMemberCommands.Count != expectedMemberCount) + { + throw new InvalidOperationException( + "停止输出的成员基础命令数量错误。"); + } + + for (var index = 0; + index < output.MemberCommands.Count; + index++) + { + AssertTwist( + output.BaseMemberCommands[index] + .TwistInVehicleBody, + 0.0, + 0.0, + 0.0, + "成员基础停止命令"); + AssertTwist( + output.MemberCommands[index].TwistInVehicleBody, + 0.0, + 0.0, + 0.0, + "成员停止命令"); + } + } + + private static void AssertResult( + FleetCoordinationCycleResult actual, + FleetCoordinationCycleResult expected, + string scenario) + { + if (actual != expected) + { + throw new InvalidOperationException( + $"{scenario}结果错误：" + + $"actual={actual}, expected={expected}。"); + } + } + + private static void AssertTwist( + Twist2D actual, + double expectedVx, + double expectedVy, + double expectedOmega, + string scenario) + { + AssertNear( + actual.VxMetersPerSecond, + expectedVx, + scenario + " Vx"); + AssertNear( + actual.VyMetersPerSecond, + expectedVy, + scenario + " Vy"); + AssertNear( + actual.OmegaRadiansPerSecond, + expectedOmega, + scenario + " Omega"); + } + + private static void AssertNear( + double actual, + double expected, + string valueName) + { + if (Math.Abs(actual - expected) > Tolerance) + { + throw new InvalidOperationException( + $"{valueName}错误：" + + $"actual={actual:F9}, expected={expected:F9}。"); + } + } + + private sealed class StraightLateralController : + ILateralController + { + public LateralControlCommand Compute( + PathTrackingContext context) + { + return LateralControlCommand.Straight; + } + + public void Reset() + { + } + } + + private sealed class ReferenceLongitudinalController : + ILongitudinalController + { + public double ComputeSpeedMetersPerSecond( + PathTrackingContext context) + { + return context.ControlReferenceSpeedMetersPerSecond; + } + + public void Reset() + { + } + } + } +} diff --git a/MultiWheelC.Tests/FleetLayoutCaptureTests.cs b/MultiWheelC.Tests/FleetLayoutCaptureTests.cs new file mode 100644 index 0000000..416ecc9 --- /dev/null +++ b/MultiWheelC.Tests/FleetLayoutCaptureTests.cs @@ -0,0 +1,268 @@ +using System; +using MultiWheelC.Fleet; +using MyParking.Shared; + +namespace MultiWheelC.Tests +{ + internal static class FleetLayoutCaptureTests + { + private const double Tolerance = 1e-9; + + public static void Run() + { + VerifySymmetricTailToTailLayout(); + VerifyAsymmetricLayoutAndPoseReconstruction(); + VerifyInputOrderDoesNotChangeResult(); + VerifyEmptyInputIsRejected(); + VerifyDuplicateVehicleIdIsRejected(); + VerifyMissingLeaderIsRejected(); + + Console.WriteLine( + "FleetLayoutCapture布局建立测试通过。共6个场景。"); + } + + private static void VerifySymmetricTailToTailLayout() + { + var result = FleetLayoutCapture.Capture( + new[] + { + new FleetMemberPose( + 1, + new Pose2D(-1.2, 0.0, 0.0)), + new FleetMemberPose( + 2, + new Pose2D(1.2, 0.0, Math.PI)) + }, + leaderVehicleId: 1); + + AssertPose( + result.FleetPoseInWorld, + Pose2D.Identity, + "对称双车中心"); + AssertVehicleLayout( + result.Layout, + 1, + new Pose2D(-1.2, 0.0, 0.0), + "对称双车主车布局"); + AssertVehicleLayout( + result.Layout, + 2, + new Pose2D(1.2, 0.0, Math.PI), + "对称双车从车布局"); + } + + private static void VerifyAsymmetricLayoutAndPoseReconstruction() + { + var members = new[] + { + new FleetMemberPose( + 3, + new Pose2D(1.0, 1.0, 0.4)), + new FleetMemberPose( + 1, + new Pose2D(4.0, 1.0, 0.4)), + new FleetMemberPose( + 2, + new Pose2D(1.0, 4.0, -0.8)) + }; + var result = FleetLayoutCapture.Capture( + members, + leaderVehicleId: 1); + + AssertPose( + result.FleetPoseInWorld, + new Pose2D(2.0, 2.0, 0.4), + "非对称三车中心"); + + for (var index = 0; + index < members.Length; + index++) + { + if (!result.Layout.TryGetVehicle( + members[index].VehicleId, + out var vehicleLayout)) + { + throw new InvalidOperationException( + "非对称布局缺少成员车。" + + members[index].VehicleId); + } + + var reconstructedPoseInWorld = + FrameTransform2D.Compose( + result.FleetPoseInWorld, + vehicleLayout.PoseInFleet); + AssertPose( + reconstructedPoseInWorld, + members[index].PoseInWorld, + "非对称布局世界位姿还原"); + } + } + + private static void VerifyInputOrderDoesNotChangeResult() + { + var first = FleetLayoutCapture.Capture( + new[] + { + new FleetMemberPose( + 1, + new Pose2D(2.0, 3.0, 0.6)), + new FleetMemberPose( + 2, + new Pose2D(4.0, 5.0, -1.0)) + }, + leaderVehicleId: 1); + var second = FleetLayoutCapture.Capture( + new[] + { + new FleetMemberPose( + 2, + new Pose2D(4.0, 5.0, -1.0)), + new FleetMemberPose( + 1, + new Pose2D(2.0, 3.0, 0.6)) + }, + leaderVehicleId: 1); + + AssertPose( + first.FleetPoseInWorld, + second.FleetPoseInWorld, + "输入顺序不变中心"); + AssertSameLayout(first.Layout, second.Layout); + } + + private static void VerifyEmptyInputIsRejected() + { + ExpectException( + () => FleetLayoutCapture.Capture( + Array.Empty(), + leaderVehicleId: 1), + "空成员集合"); + } + + private static void VerifyDuplicateVehicleIdIsRejected() + { + ExpectException( + () => FleetLayoutCapture.Capture( + new[] + { + new FleetMemberPose( + 1, + Pose2D.Identity), + new FleetMemberPose( + 1, + new Pose2D(1.0, 0.0, 0.0)) + }, + leaderVehicleId: 1), + "重复车号"); + } + + private static void VerifyMissingLeaderIsRejected() + { + ExpectException( + () => FleetLayoutCapture.Capture( + new[] + { + new FleetMemberPose( + 2, + Pose2D.Identity) + }, + leaderVehicleId: 1), + "缺少主车"); + } + + private static void AssertSameLayout( + FleetLayout first, + FleetLayout second) + { + if (first.VehicleCount != second.VehicleCount) + { + throw new InvalidOperationException( + "输入顺序变化后成员数量发生变化。"); + } + + for (var index = 0; + index < first.Vehicles.Count; + index++) + { + var vehicle = first.Vehicles[index]; + AssertVehicleLayout( + second, + vehicle.VehicleId, + vehicle.PoseInFleet, + "输入顺序不变布局"); + } + } + + private static void AssertVehicleLayout( + FleetLayout layout, + int vehicleId, + Pose2D expectedPoseInFleet, + string scenario) + { + if (!layout.TryGetVehicle( + vehicleId, + out var vehicle)) + { + throw new InvalidOperationException( + $"{scenario}缺少车辆{vehicleId}。"); + } + + AssertPose( + vehicle.PoseInFleet, + expectedPoseInFleet, + scenario); + } + + private static void AssertPose( + Pose2D actual, + Pose2D expected, + string scenario) + { + AssertNear( + actual.XMeters, + expected.XMeters, + scenario + " X"); + AssertNear( + actual.YMeters, + expected.YMeters, + scenario + " Y"); + AssertNear( + AngleMath.NormalizeRadians( + actual.YawRadians - + expected.YawRadians), + 0.0, + scenario + " Yaw"); + } + + private static void AssertNear( + double actual, + double expected, + string valueName) + { + if (Math.Abs(actual - expected) > Tolerance) + { + throw new InvalidOperationException( + $"{valueName}错误：" + + $"actual={actual:F9}, expected={expected:F9}。"); + } + } + + private static void ExpectException( + Action action, + string scenario) + where TException : Exception + { + try + { + action(); + } + catch (TException) + { + return; + } + + throw new InvalidOperationException( + $"{scenario}没有抛出{typeof(TException).Name}。"); + } + } +} diff --git a/MultiWheelC.Tests/FleetMemberCommandCorrectorTests.cs b/MultiWheelC.Tests/FleetMemberCommandCorrectorTests.cs new file mode 100644 index 0000000..7c6fe7d --- /dev/null +++ b/MultiWheelC.Tests/FleetMemberCommandCorrectorTests.cs @@ -0,0 +1,302 @@ +using System; +using System.Collections.Generic; +using MultiWheelC.Fleet; +using MyParking.Shared; + +namespace MultiWheelC.Tests +{ + internal static class FleetMemberCommandCorrectorTests + { + private const double Tolerance = 1e-9; + + public static void Run() + { + VerifyZeroErrorsPreserveBaseCommands(); + VerifyRelativePositionErrorProducesOpposingCorrection(); + VerifyCommonTranslationIsRemoved(); + VerifyCommonRotationIsRemoved(); + VerifyDeadbandSuppressesSmallErrors(); + VerifyCorrectionLimits(); + + Console.WriteLine( + "FleetMemberCommandCorrector测试通过，共6个场景。"); + } + + private static void VerifyZeroErrorsPreserveBaseCommands() + { + var layout = CreateLayout(); + var baseCommands = FleetKinematics.Decompose( + layout, + new FleetMotionCommand( + Point2D.Zero, + new Twist2D(0.4, 0.1, 0.05))); + var corrected = CreateCorrector().Correct( + layout, + baseCommands, + CreateErrors(Pose2D.Identity, Pose2D.Identity)); + + AssertCommandsEqual( + corrected, + baseCommands, + "零布局误差"); + } + + private static void + VerifyRelativePositionErrorProducesOpposingCorrection() + { + var layout = CreateLayout(); + var corrected = CreateCorrector().Correct( + layout, + CreateStopCommands(layout), + CreateErrors( + new Pose2D(0.02, 0.0, 0.0), + new Pose2D(0.02, 0.0, 0.0))); + + AssertTwist( + FindCommand(corrected, 1).TwistInVehicleBody, + -0.02, + 0.0, + 0.0, + "车辆1相对位置纠偏"); + AssertTwist( + FindCommand(corrected, 2).TwistInVehicleBody, + -0.02, + 0.0, + 0.0, + "车辆2相对位置纠偏"); + } + + private static void VerifyCommonTranslationIsRemoved() + { + var layout = CreateLayout(); + var corrected = CreateCorrector().Correct( + layout, + CreateStopCommands(layout), + CreateErrors( + new Pose2D(0.02, 0.0, 0.0), + new Pose2D(-0.02, 0.0, 0.0))); + + AssertAllStopped( + corrected, + "共同平移不应成为成员相对纠偏"); + } + + private static void VerifyCommonRotationIsRemoved() + { + const double fleetYawErrorRadians = 0.02; + var layout = CreateLayout(); + var commonRotationError = new Pose2D( + 0.0, + -fleetYawErrorRadians, + fleetYawErrorRadians); + var corrected = CreateCorrector().Correct( + layout, + CreateStopCommands(layout), + CreateErrors( + commonRotationError, + commonRotationError)); + + AssertAllStopped( + corrected, + "共同旋转不应成为成员相对纠偏"); + } + + private static void VerifyDeadbandSuppressesSmallErrors() + { + var layout = CreateLayout(); + var corrector = new FleetMemberCommandCorrector( + longitudinalPositionGainPerSecond: 1.0, + lateralPositionGainPerSecond: 1.0, + yawGainPerSecond: 1.0, + positionErrorDeadbandMeters: 0.005, + yawErrorDeadbandRadians: 0.02, + maximumLinearCorrectionMetersPerSecond: 1.0, + maximumAngularCorrectionRadiansPerSecond: 1.0); + var corrected = corrector.Correct( + layout, + CreateStopCommands(layout), + CreateErrors( + new Pose2D(0.004, 0.003, 0.01), + new Pose2D(0.004, -0.003, -0.01))); + + AssertAllStopped(corrected, "布局误差死区"); + } + + private static void VerifyCorrectionLimits() + { + var layout = CreateLayout(); + var corrector = new FleetMemberCommandCorrector( + longitudinalPositionGainPerSecond: 1.0, + lateralPositionGainPerSecond: 1.0, + yawGainPerSecond: 1.0, + positionErrorDeadbandMeters: 0.0, + yawErrorDeadbandRadians: 0.0, + maximumLinearCorrectionMetersPerSecond: 0.03, + maximumAngularCorrectionRadiansPerSecond: 0.05); + var corrected = corrector.Correct( + layout, + CreateStopCommands(layout), + CreateErrors( + new Pose2D(0.2, 0.0, 0.2), + new Pose2D(0.2, 0.0, -0.2))); + + for (var index = 0; index < corrected.Count; index++) + { + var twist = corrected[index].TwistInVehicleBody; + var linearMagnitude = Math.Sqrt( + twist.VxMetersPerSecond * + twist.VxMetersPerSecond + + twist.VyMetersPerSecond * + twist.VyMetersPerSecond); + + AssertNear( + linearMagnitude, + 0.03, + "线速度纠偏限幅"); + AssertNear( + Math.Abs(twist.OmegaRadiansPerSecond), + 0.05, + "角速度纠偏限幅"); + } + } + + private static FleetMemberCommandCorrector CreateCorrector() + { + return new FleetMemberCommandCorrector( + longitudinalPositionGainPerSecond: 1.0, + lateralPositionGainPerSecond: 1.0, + yawGainPerSecond: 1.0, + positionErrorDeadbandMeters: 0.0, + yawErrorDeadbandRadians: 0.0, + maximumLinearCorrectionMetersPerSecond: 1.0, + maximumAngularCorrectionRadiansPerSecond: 1.0); + } + + private static FleetLayout CreateLayout() + { + return new FleetLayout( + new[] + { + new VehicleLayout( + 1, + new Pose2D(-1.0, 0.0, 0.0)), + new VehicleLayout( + 2, + new Pose2D(1.0, 0.0, Math.PI)) + }); + } + + private static IReadOnlyList + CreateStopCommands(FleetLayout layout) + { + return FleetKinematics.Decompose( + layout, + FleetMotionCommand.Stop()); + } + + private static FleetMemberLayoutError[] CreateErrors( + Pose2D vehicle1Error, + Pose2D vehicle2Error) + { + return new[] + { + new FleetMemberLayoutError(1, vehicle1Error), + new FleetMemberLayoutError(2, vehicle2Error) + }; + } + + private static FleetMemberCommand FindCommand( + IReadOnlyList commands, + int vehicleId) + { + for (var index = 0; index < commands.Count; index++) + { + if (commands[index].VehicleId == vehicleId) + { + return commands[index]; + } + } + + throw new InvalidOperationException( + $"没有找到车辆{vehicleId}的成员命令。"); + } + + private static void AssertCommandsEqual( + IReadOnlyList actual, + IReadOnlyList expected, + string scenario) + { + if (actual.Count != expected.Count) + { + throw new InvalidOperationException( + $"{scenario}的命令数量不一致。"); + } + + for (var index = 0; index < expected.Count; index++) + { + var expectedCommand = expected[index]; + var actualCommand = FindCommand( + actual, + expectedCommand.VehicleId); + AssertTwist( + actualCommand.TwistInVehicleBody, + expectedCommand.TwistInVehicleBody + .VxMetersPerSecond, + expectedCommand.TwistInVehicleBody + .VyMetersPerSecond, + expectedCommand.TwistInVehicleBody + .OmegaRadiansPerSecond, + scenario); + } + } + + private static void AssertAllStopped( + IReadOnlyList commands, + string scenario) + { + for (var index = 0; index < commands.Count; index++) + { + AssertTwist( + commands[index].TwistInVehicleBody, + 0.0, + 0.0, + 0.0, + scenario); + } + } + + private static void AssertTwist( + Twist2D actual, + double expectedVx, + double expectedVy, + double expectedOmega, + string scenario) + { + AssertNear( + actual.VxMetersPerSecond, + expectedVx, + scenario + " Vx"); + AssertNear( + actual.VyMetersPerSecond, + expectedVy, + scenario + " Vy"); + AssertNear( + actual.OmegaRadiansPerSecond, + expectedOmega, + scenario + " Omega"); + } + + private static void AssertNear( + double actual, + double expected, + string valueName) + { + if (Math.Abs(actual - expected) > Tolerance) + { + throw new InvalidOperationException( + $"{valueName}错误：" + + $"actual={actual:F9}, expected={expected:F9}。"); + } + } + } +} diff --git a/MultiWheelC.Tests/FleetPreparationCoordinatorTests.cs b/MultiWheelC.Tests/FleetPreparationCoordinatorTests.cs new file mode 100644 index 0000000..3e41d08 --- /dev/null +++ b/MultiWheelC.Tests/FleetPreparationCoordinatorTests.cs @@ -0,0 +1,310 @@ +using System; +using MultiWheelC.Fleet; +using MyParking.Shared; + +namespace MultiWheelC.Tests +{ + internal static class FleetPreparationCoordinatorTests + { + private const double Tolerance = 1e-9; + + public static void Run() + { + VerifyLeaderYawExample(); + VerifyTailToTailUsesEquivalentAxis(); + VerifyAllMembersMustBeReady(); + VerifyStaleStatusIsIgnored(); + VerifyMemberFaultIsLatched(); + VerifyFaultAfterAuthorizationIsLatched(); + VerifyPrematureActiveIsRejected(); + VerifyCancelClearsPlan(); + + Console.WriteLine( + "FleetPreparationCoordinator测试通过。共8个场景。"); + } + + private static void VerifyLeaderYawExample() + { + var capture = FleetLayoutCapture.Capture( + new[] + { + new FleetMemberPose( + 1, + new Pose2D( + -1.0, + 0.0, + AngleMath.DegreesToRadians(20.0))), + new FleetMemberPose( + 2, + new Pose2D(1.0, 0.0, 0.0)) + }, + leaderVehicleId: 1); + var coordinator = + new FleetPreparationCoordinator(); + + coordinator.StartRollingPreparation( + planId: 1, + capture.Layout, + motionDirectionInFleetRadians: 0.0); + + AssertTargetDegrees(coordinator, 1, 0.0); + AssertTargetDegrees(coordinator, 2, 20.0); + } + + private static void VerifyTailToTailUsesEquivalentAxis() + { + var coordinator = + new FleetPreparationCoordinator(); + coordinator.StartRollingPreparation( + planId: 2, + CreateTailToTailLayout(), + motionDirectionInFleetRadians: 0.0); + + AssertTargetDegrees(coordinator, 1, 0.0); + AssertTargetDegrees(coordinator, 2, 0.0); + } + + private static void VerifyAllMembersMustBeReady() + { + var coordinator = CreateStartedCoordinator(3); + + AssertState( + coordinator.ReportMemberStatus( + CreateStatus( + 3, + 1, + FleetMemberAgentState.Ready)), + FleetPreparationCoordinatorState + .WaitingForMembers, + "仅一辆车Ready"); + AssertState( + coordinator.ReportMemberStatus( + CreateStatus( + 3, + 2, + FleetMemberAgentState.Ready)), + FleetPreparationCoordinatorState + .ReadyToActivate, + "全部成员Ready"); + AssertState( + coordinator.ReportMemberStatus( + CreateStatus( + 3, + 1, + FleetMemberAgentState.Preparing)), + FleetPreparationCoordinatorState + .WaitingForMembers, + "成员失去Ready"); + AssertState( + coordinator.ReportMemberStatus( + CreateStatus( + 3, + 1, + FleetMemberAgentState.Ready)), + FleetPreparationCoordinatorState + .ReadyToActivate, + "成员重新Ready"); + + if (coordinator.TryAuthorizeActivation(4)) + { + throw new InvalidOperationException( + "错误任务编号不应获得激活授权。"); + } + + if (!coordinator.TryAuthorizeActivation(3)) + { + throw new InvalidOperationException( + "全部成员Ready后没有获得激活授权。"); + } + + AssertState( + coordinator.State, + FleetPreparationCoordinatorState + .ActivationAuthorized, + "统一激活授权"); + } + + private static void VerifyStaleStatusIsIgnored() + { + var coordinator = CreateStartedCoordinator(5); + var state = coordinator.ReportMemberStatus( + CreateStatus( + 4, + 1, + FleetMemberAgentState.Ready)); + + AssertState( + state, + FleetPreparationCoordinatorState + .WaitingForMembers, + "旧任务状态报告"); + } + + private static void VerifyMemberFaultIsLatched() + { + var coordinator = CreateStartedCoordinator(6); + var state = coordinator.ReportMemberStatus( + CreateStatus( + 6, + 2, + FleetMemberAgentState.Faulted, + "舵轮未到位")); + + AssertState( + state, + FleetPreparationCoordinatorState.Faulted, + "成员准备故障"); + if (string.IsNullOrWhiteSpace( + coordinator.LastFailureReason)) + { + throw new InvalidOperationException( + "成员准备故障没有保存原因。"); + } + } + + private static void VerifyFaultAfterAuthorizationIsLatched() + { + var coordinator = CreateStartedCoordinator(9); + coordinator.ReportMemberStatus( + CreateStatus( + 9, + 1, + FleetMemberAgentState.Ready)); + coordinator.ReportMemberStatus( + CreateStatus( + 9, + 2, + FleetMemberAgentState.Ready)); + coordinator.TryAuthorizeActivation(9); + + var state = coordinator.ReportMemberStatus( + CreateStatus( + 9, + 2, + FleetMemberAgentState.Faulted, + "激活失败")); + + AssertState( + state, + FleetPreparationCoordinatorState.Faulted, + "授权后的成员故障"); + } + + private static void VerifyPrematureActiveIsRejected() + { + var coordinator = CreateStartedCoordinator(7); + var state = coordinator.ReportMemberStatus( + CreateStatus( + 7, + 1, + FleetMemberAgentState.Active)); + + AssertState( + state, + FleetPreparationCoordinatorState.Faulted, + "成员提前运动"); + } + + private static void VerifyCancelClearsPlan() + { + var coordinator = CreateStartedCoordinator(8); + coordinator.Cancel(); + + AssertState( + coordinator.State, + FleetPreparationCoordinatorState.Idle, + "取消准备任务"); + if (coordinator.CurrentPlanId != 0 || + coordinator.Targets.Count != 0) + { + throw new InvalidOperationException( + "取消后没有清除准备任务数据。"); + } + } + + private static FleetPreparationCoordinator + CreateStartedCoordinator(long planId) + { + var coordinator = + new FleetPreparationCoordinator(); + coordinator.StartRollingPreparation( + planId, + CreateTailToTailLayout(), + motionDirectionInFleetRadians: 0.0); + return coordinator; + } + + private static FleetLayout CreateTailToTailLayout() + { + return new FleetLayout( + new[] + { + new VehicleLayout( + 1, + new Pose2D(-1.0, 0.0, 0.0)), + new VehicleLayout( + 2, + new Pose2D(1.0, 0.0, Math.PI)) + }); + } + + private static FleetMemberPreparationStatus CreateStatus( + long planId, + int vehicleId, + FleetMemberAgentState state, + string failureReason = "") + { + return new FleetMemberPreparationStatus( + planId, + vehicleId, + state, + failureReason); + } + + private static void AssertTargetDegrees( + FleetPreparationCoordinator coordinator, + int vehicleId, + double expectedDegrees) + { + if (!coordinator.TryGetTarget( + vehicleId, + out var target)) + { + throw new InvalidOperationException( + $"没有找到车辆{vehicleId}的准备目标。"); + } + + AssertNear( + AngleMath.RadiansToDegrees( + target.MotionDirectionInBodyRadians), + expectedDegrees, + $"车辆{vehicleId}本地β"); + } + + private static void AssertState( + FleetPreparationCoordinatorState actual, + FleetPreparationCoordinatorState expected, + string scenario) + { + if (actual != expected) + { + throw new InvalidOperationException( + $"{scenario}状态错误：" + + $"actual={actual}, expected={expected}。"); + } + } + + private static void AssertNear( + double actual, + double expected, + string valueName) + { + if (Math.Abs(actual - expected) > Tolerance) + { + throw new InvalidOperationException( + $"{valueName}错误：" + + $"actual={actual:F9}, expected={expected:F9}。"); + } + } + } +} diff --git a/MultiWheelC.Tests/FleetStateEstimatorTests.cs b/MultiWheelC.Tests/FleetStateEstimatorTests.cs new file mode 100644 index 0000000..8d5f1b7 --- /dev/null +++ b/MultiWheelC.Tests/FleetStateEstimatorTests.cs @@ -0,0 +1,430 @@ +using System; +using System.Collections.Generic; +using MultiWheelC.Fleet; +using MyParking.Shared; + +namespace MultiWheelC.Tests +{ + internal static class FleetStateEstimatorTests + { + private const double Tolerance = 1e-9; + + public static void Run() + { + VerifyRigidStateIsRecovered(); + VerifyOlderSamplesAreAligned(); + VerifyYawWrapAroundIsAveraged(); + VerifySmallLayoutErrorIsReported(); + VerifyInconsistentCentersAreRejected(); + VerifyMissingMemberIsRejected(); + VerifyInvalidVelocityRemainsExplicit(); + + Console.WriteLine( + "FleetStateEstimator车队状态估计测试通过。共7个场景。"); + } + + private static void VerifyRigidStateIsRecovered() + { + var layout = CreateLayout(); + var fleetPose = new Pose2D(4.0, -2.0, 0.4); + var fleetTwist = new Twist2D(0.3, -0.1, 0.2); + var result = CreateEstimator().Estimate( + layout, + CreateRigidMemberStates( + layout, + fleetPose, + fleetTwist, + sampleTimestampSeconds: 5.0, + hasValidVelocityEstimate: true), + targetTimestampSeconds: 5.0); + + var state = RequireState(result, "刚体状态还原"); + AssertPose( + state.FleetPoseInWorld, + fleetPose, + "刚体状态还原"); + AssertTwist( + state.TwistAtFleetOriginInWorld, + fleetTwist, + "刚体速度还原"); + + for (var index = 0; + index < result.MemberErrors.Count; + index++) + { + AssertPose( + result.MemberErrors[index] + .ActualPoseInExpectedVehicleFrame, + Pose2D.Identity, + "刚体布局误差"); + } + } + + private static void VerifyOlderSamplesAreAligned() + { + var layout = CreateLayout(); + var sampleFleetPose = + new Pose2D(1.0, 2.0, 0.3); + var fleetTwist = + new Twist2D(0.4, -0.2, 0.0); + var result = CreateEstimator().Estimate( + layout, + CreateRigidMemberStates( + layout, + sampleFleetPose, + fleetTwist, + sampleTimestampSeconds: 0.9, + hasValidVelocityEstimate: true), + targetTimestampSeconds: 1.0); + + var state = RequireState(result, "成员时间对齐"); + AssertPose( + state.FleetPoseInWorld, + new Pose2D(1.04, 1.98, 0.3), + "成员时间对齐"); + AssertTwist( + state.TwistAtFleetOriginInWorld, + fleetTwist, + "时间对齐后速度"); + } + + private static void VerifySmallLayoutErrorIsReported() + { + var layout = CreateLayout(); + var result = CreateEstimator().Estimate( + layout, + new[] + { + CreateMemberState( + 1, + new Pose2D(-0.98, 0.0, 0.0), + Twist2D.Zero, + 1.0, + true), + CreateMemberState( + 2, + new Pose2D(0.99, 0.0, Math.PI), + Twist2D.Zero, + 1.0, + true) + }, + targetTimestampSeconds: 1.0); + + var state = RequireState(result, "小范围布局误差"); + AssertNear( + state.FleetPoseInWorld.XMeters, + 0.005, + "小范围布局误差中心X"); + AssertNear( + FindError(result.MemberErrors, 1) + .ActualPoseInExpectedVehicleFrame.XMeters, + 0.015, + "车辆1布局误差X"); + AssertNear( + FindError(result.MemberErrors, 2) + .ActualPoseInExpectedVehicleFrame.XMeters, + 0.015, + "车辆2布局误差X"); + } + + private static void VerifyYawWrapAroundIsAveraged() + { + var layout = CreateLayout(); + var firstCandidate = new Pose2D( + 0.0, + 0.0, + AngleMath.DegreesToRadians(179.0)); + var secondCandidate = new Pose2D( + 0.0, + 0.0, + AngleMath.DegreesToRadians(-179.0)); + var result = CreateEstimator().Estimate( + layout, + new[] + { + CreateMemberState( + 1, + FrameTransform2D.Compose( + firstCandidate, + layout.Vehicles[0].PoseInFleet), + Twist2D.Zero, + 1.0, + true), + CreateMemberState( + 2, + FrameTransform2D.Compose( + secondCandidate, + layout.Vehicles[1].PoseInFleet), + Twist2D.Zero, + 1.0, + true) + }, + targetTimestampSeconds: 1.0); + + var state = RequireState(result, "跨正负π航向平均"); + AssertNear( + Math.Abs(state.FleetPoseInWorld.YawRadians), + Math.PI, + "跨正负π航向平均"); + } + + private static void VerifyInconsistentCentersAreRejected() + { + var result = CreateEstimator().Estimate( + CreateLayout(), + new[] + { + CreateMemberState( + 1, + new Pose2D(-1.0, 0.0, 0.0), + Twist2D.Zero, + 1.0, + true), + CreateMemberState( + 2, + new Pose2D(1.3, 0.0, Math.PI), + Twist2D.Zero, + 1.0, + true) + }, + targetTimestampSeconds: 1.0); + + AssertUnavailable(result, "候选中心冲突"); + } + + private static void VerifyMissingMemberIsRejected() + { + var result = CreateEstimator().Estimate( + CreateLayout(), + new[] + { + CreateMemberState( + 1, + new Pose2D(-1.0, 0.0, 0.0), + Twist2D.Zero, + 1.0, + true) + }, + targetTimestampSeconds: 1.0); + + AssertUnavailable(result, "成员缺失"); + } + + private static void VerifyInvalidVelocityRemainsExplicit() + { + var layout = CreateLayout(); + var result = CreateEstimator().Estimate( + layout, + CreateRigidMemberStates( + layout, + Pose2D.Identity, + new Twist2D(0.4, 0.0, 0.0), + sampleTimestampSeconds: 1.0, + hasValidVelocityEstimate: false), + targetTimestampSeconds: 1.0); + + var state = RequireState(result, "速度未初始化"); + if (state.HasValidVelocityEstimate) + { + throw new InvalidOperationException( + "成员速度无效时车队速度不应标记为有效。"); + } + + AssertTwist( + state.TwistAtFleetOriginInWorld, + Twist2D.Zero, + "速度未初始化"); + } + + private static FleetStateEstimator CreateEstimator() + { + return new FleetStateEstimator( + maximumMemberStateAgeSeconds: 0.25, + maximumPositionDisagreementMeters: 0.1, + maximumYawDisagreementRadians: + AngleMath.DegreesToRadians(5.0)); + } + + private static FleetLayout CreateLayout() + { + return new FleetLayout( + new[] + { + new VehicleLayout( + 1, + new Pose2D(-1.0, 0.0, 0.0)), + new VehicleLayout( + 2, + new Pose2D(1.0, 0.0, Math.PI)) + }); + } + + private static FleetMemberStateSample[] + CreateRigidMemberStates( + FleetLayout layout, + Pose2D fleetPoseInWorld, + Twist2D twistAtFleetOriginInWorld, + double sampleTimestampSeconds, + bool hasValidVelocityEstimate) + { + var states = + new FleetMemberStateSample[layout.VehicleCount]; + + for (var index = 0; + index < layout.Vehicles.Count; + index++) + { + var vehicle = layout.Vehicles[index]; + var memberPoseInWorld = + FrameTransform2D.Compose( + fleetPoseInWorld, + vehicle.PoseInFleet); + var xFromFleetOrigin = + memberPoseInWorld.XMeters - + fleetPoseInWorld.XMeters; + var yFromFleetOrigin = + memberPoseInWorld.YMeters - + fleetPoseInWorld.YMeters; + var memberTwistInWorld = new Twist2D( + twistAtFleetOriginInWorld + .VxMetersPerSecond - + twistAtFleetOriginInWorld + .OmegaRadiansPerSecond * + yFromFleetOrigin, + twistAtFleetOriginInWorld + .VyMetersPerSecond + + twistAtFleetOriginInWorld + .OmegaRadiansPerSecond * + xFromFleetOrigin, + twistAtFleetOriginInWorld + .OmegaRadiansPerSecond); + + states[index] = new FleetMemberStateSample( + vehicle.VehicleId, + sampleTimestampSeconds, + memberPoseInWorld, + memberTwistInWorld, + isStateAvailable: true, + hasValidVelocityEstimate: + hasValidVelocityEstimate); + } + + return states; + } + + private static FleetMemberStateSample CreateMemberState( + int vehicleId, + Pose2D poseInWorld, + Twist2D twistInWorld, + double timestampSeconds, + bool hasValidVelocityEstimate) + { + return new FleetMemberStateSample( + vehicleId, + timestampSeconds, + poseInWorld, + twistInWorld, + isStateAvailable: true, + hasValidVelocityEstimate: + hasValidVelocityEstimate); + } + + private static FleetState RequireState( + FleetStateEstimateResult result, + string scenario) + { + if (!result.IsAvailable || !result.State.HasValue) + { + throw new InvalidOperationException( + $"{scenario}应产生可用状态：" + + result.UnavailableReason); + } + + return result.State.Value; + } + + private static void AssertUnavailable( + FleetStateEstimateResult result, + string scenario) + { + if (result.IsAvailable || + string.IsNullOrWhiteSpace( + result.UnavailableReason)) + { + throw new InvalidOperationException( + $"{scenario}应返回带原因的不可用结果。"); + } + } + + private static FleetMemberLayoutError FindError( + IReadOnlyList errors, + int vehicleId) + { + for (var index = 0; + index < errors.Count; + index++) + { + if (errors[index].VehicleId == vehicleId) + { + return errors[index]; + } + } + + throw new InvalidOperationException( + $"没有找到车辆{vehicleId}的布局误差。"); + } + + private static void AssertPose( + Pose2D actual, + Pose2D expected, + string scenario) + { + AssertNear( + actual.XMeters, + expected.XMeters, + scenario + " X"); + AssertNear( + actual.YMeters, + expected.YMeters, + scenario + " Y"); + AssertNear( + AngleMath.ShortestDifferenceRadians( + actual.YawRadians, + expected.YawRadians), + 0.0, + scenario + " Yaw"); + } + + private static void AssertTwist( + Twist2D actual, + Twist2D expected, + string scenario) + { + AssertNear( + actual.VxMetersPerSecond, + expected.VxMetersPerSecond, + scenario + " Vx"); + AssertNear( + actual.VyMetersPerSecond, + expected.VyMetersPerSecond, + scenario + " Vy"); + AssertNear( + actual.OmegaRadiansPerSecond, + expected.OmegaRadiansPerSecond, + scenario + " Omega"); + } + + private static void AssertNear( + double actual, + double expected, + string valueName) + { + if (Math.Abs(actual - expected) > Tolerance) + { + throw new InvalidOperationException( + $"{valueName}错误：" + + $"actual={actual:F9}, expected={expected:F9}。"); + } + } + } +} diff --git a/MultiWheelC.Tests/Program.cs b/MultiWheelC.Tests/Program.cs index 9360103..3487e4b 100644 --- a/MultiWheelC.Tests/Program.cs +++ b/MultiWheelC.Tests/Program.cs @@ -35,8 +35,13 @@ namespace MultiWheelC.Tests Console.WriteLine( "Stanley前进/倒车横向符号测试通过。共8个场景。"); + FleetLayoutCaptureTests.Run(); + FleetStateEstimatorTests.Run(); FleetKinematicsTests.Run(); FleetControllerTests.Run(); + FleetMemberCommandCorrectorTests.Run(); + FleetCoordinatorTests.Run(); + FleetPreparationCoordinatorTests.Run(); } /// diff --git a/MultiWheelC/Configuration/PilotConfig.ParkingControl.cs b/MultiWheelC/Configuration/PilotConfig.ParkingControl.cs index 367e321..281bafa 100644 --- a/MultiWheelC/Configuration/PilotConfig.ParkingControl.cs +++ b/MultiWheelC/Configuration/PilotConfig.ParkingControl.cs @@ -49,6 +49,12 @@ public partial class PilotConfig [FieldMember(desc = "停车控制：Detour自动坐标连续化最大航向变化(deg)")] public float ParkingDetourMaximumAutomaticHeadingShiftDegrees = 5f; + [FieldMember(desc = "停车控制：Detour缓存帧最大允许时间(s)")] + public float ParkingDetourMaximumCachedFrameAgeSeconds = 0.50f; + + [FieldMember(desc = "停车控制：Detour定位质量失效/恢复确认新帧数")] + public int ParkingDetourLocalizationQualityConfirmationFrames = 3; + [FieldMember(desc = "停车控制：Detour线速度滤波时间常数(s)")] public float ParkingDetourLinearVelocityFilterSeconds = 0.15f; diff --git a/MultiWheelC/Fleet/FleetController.cs b/MultiWheelC/Fleet/FleetController.cs index e0ca3e7..2cf04b8 100644 --- a/MultiWheelC/Fleet/FleetController.cs +++ b/MultiWheelC/Fleet/FleetController.cs @@ -37,14 +37,21 @@ namespace MultiWheelC.Fleet double terminalApproachGainPerSecond = 0.8, double maximumTerminalApproachSpeedMetersPerSecond = 0.05, double curvaturePreviewSeconds = 0.20, - double maximumCurvaturePreviewMeters = 0.12) + double maximumCurvaturePreviewMeters = 0.12, + double motionDirectionInFleetRadians = 0.0) { NumericGuard.EnsureFinitePositive( virtualControlPointRadiusMeters, nameof(virtualControlPointRadiusMeters)); + NumericGuard.EnsureFinite( + motionDirectionInFleetRadians, + nameof(motionDirectionInFleetRadians)); VirtualControlPointRadiusMeters = virtualControlPointRadiusMeters; + MotionDirectionInFleetRadians = + AngleMath.NormalizeRadians( + motionDirectionInFleetRadians); _trackingCore = new PathTrackingCore( lateralController, longitudinalController, @@ -58,12 +65,16 @@ namespace MultiWheelC.Fleet terminalApproachGainPerSecond, maximumTerminalApproachSpeedMetersPerSecond, curvaturePreviewSeconds, - maximumCurvaturePreviewMeters); + maximumCurvaturePreviewMeters, + MotionDirectionInFleetRadians); } // 虚拟车队中心到前、后GCP的距离，单位为m。 public double VirtualControlPointRadiusMeters { get; } + // 当前运动坐标系+X轴相对车队坐标系+X轴的方向，单位为rad。 + public double MotionDirectionInFleetRadians { get; } + public double FinishDistanceMeters => _trackingCore.FinishDistanceMeters; @@ -169,13 +180,20 @@ namespace MultiWheelC.Fleet try { - var twistAtFleetOrigin = + var twistAtFleetOriginInMotionFrame = GcpKinematics.ToBodyTwist( output.Command.Value, VirtualControlPointRadiusMeters); + var twistAtFleetOriginInFleet = + FrameTransform2D.TransformTwistAtSamePoint( + new Pose2D( + 0.0, + 0.0, + MotionDirectionInFleetRadians), + twistAtFleetOriginInMotionFrame); command = new FleetMotionCommand( Point2D.Zero, - twistAtFleetOrigin); + twistAtFleetOriginInFleet); LastCommand = command; return FleetControlCycleResult.CommandGenerated; } diff --git a/MultiWheelC/Fleet/FleetCoordinator.cs b/MultiWheelC/Fleet/FleetCoordinator.cs index 9a0d323..fd690f9 100644 --- a/MultiWheelC/Fleet/FleetCoordinator.cs +++ b/MultiWheelC/Fleet/FleetCoordinator.cs @@ -1 +1,509 @@ -// 只在主车激活，完成车队轨迹控制和命令分配 \ No newline at end of file +using System; +using System.Collections.Generic; +using MultiWheelC.Trajectory; +using MyParking.Shared; + +namespace MultiWheelC.Fleet +{ + // 主车单周期车队协调结果，不表示通信或成员底盘执行结果。 + public enum FleetCoordinationCycleResult + { + Inactive = 0, + WaitingForState = 1, + CommandGenerated = 2, + Completed = 3, + Faulted = 4 + } + + // 保存本周期的状态估计、车队命令和成员基础命令。 + public sealed class FleetCoordinationCycleOutput + { + internal FleetCoordinationCycleOutput( + FleetState? state, + IReadOnlyList memberErrors, + FleetMotionCommand fleetCommand, + IReadOnlyList baseMemberCommands, + IReadOnlyList memberCommands, + double speedScale, + string reason) + { + NumericGuard.EnsureFiniteNonNegative( + speedScale, + nameof(speedScale)); + if (speedScale > 1.0) + { + throw new ArgumentOutOfRangeException( + nameof(speedScale), + "车队统一速度比例不能大于1。"); + } + + State = state; + MemberErrors = memberErrors ?? + throw new ArgumentNullException( + nameof(memberErrors)); + FleetCommand = fleetCommand; + BaseMemberCommands = baseMemberCommands ?? + throw new ArgumentNullException( + nameof(baseMemberCommands)); + MemberCommands = memberCommands ?? + throw new ArgumentNullException( + nameof(memberCommands)); + SpeedScale = speedScale; + Reason = reason ?? string.Empty; + } + + public FleetState? State { get; } + + public IReadOnlyList MemberErrors { get; } + + public FleetMotionCommand FleetCommand { get; } + + // 仅由车队刚体命令分解得到，尚未叠加成员相对布局纠偏。 + public IReadOnlyList BaseMemberCommands { get; } + + // 已转换到成员当前车体系并叠加小范围相对布局纠偏的最终命令。 + public IReadOnlyList MemberCommands { get; } + + // 因成员布局误差施加到Vx、Vy和Omega的统一比例，范围为[0,1]。 + public double SpeedScale { get; } + + public string Reason { get; } + } + + // 在主车上串联车队状态估计、中心轨迹控制和成员命令分解。 + public sealed class FleetCoordinator + { + private static readonly IReadOnlyList + EmptyMemberErrors = Array.AsReadOnly( + Array.Empty()); + private static readonly IReadOnlyList + EmptyMemberCommands = Array.AsReadOnly( + Array.Empty()); + + private readonly FleetStateEstimator _stateEstimator; + private readonly FleetController _fleetController; + private readonly FleetMemberCommandCorrector + _memberCommandCorrector; + private readonly double _memberPositionErrorWarningMeters; + private readonly double _maximumMemberPositionErrorMeters; + private readonly double _memberYawErrorWarningRadians; + private readonly double _maximumMemberYawErrorRadians; + + private FleetLayout _activeLayout; + private bool _isCompleted; + private bool _isFaulted; + + public FleetCoordinator( + FleetStateEstimator stateEstimator, + FleetController fleetController, + FleetMemberCommandCorrector memberCommandCorrector, + double memberPositionErrorWarningMeters, + double maximumMemberPositionErrorMeters, + double memberYawErrorWarningRadians, + double maximumMemberYawErrorRadians) + { + _stateEstimator = stateEstimator ?? + throw new ArgumentNullException( + nameof(stateEstimator)); + _fleetController = fleetController ?? + throw new ArgumentNullException( + nameof(fleetController)); + _memberCommandCorrector = + memberCommandCorrector ?? + throw new ArgumentNullException( + nameof(memberCommandCorrector)); + NumericGuard.EnsureFiniteNonNegative( + memberPositionErrorWarningMeters, + nameof(memberPositionErrorWarningMeters)); + NumericGuard.EnsureFinitePositive( + maximumMemberPositionErrorMeters, + nameof(maximumMemberPositionErrorMeters)); + NumericGuard.EnsureFiniteNonNegative( + memberYawErrorWarningRadians, + nameof(memberYawErrorWarningRadians)); + NumericGuard.EnsureFinitePositive( + maximumMemberYawErrorRadians, + nameof(maximumMemberYawErrorRadians)); + + if (memberPositionErrorWarningMeters >= + maximumMemberPositionErrorMeters) + { + throw new ArgumentOutOfRangeException( + nameof(memberPositionErrorWarningMeters), + "成员位置误差警告阈值必须小于停止阈值。"); + } + + if (memberYawErrorWarningRadians >= + maximumMemberYawErrorRadians) + { + throw new ArgumentOutOfRangeException( + nameof(memberYawErrorWarningRadians), + "成员航向误差警告阈值必须小于停止阈值。"); + } + + if (maximumMemberYawErrorRadians > Math.PI) + { + throw new ArgumentOutOfRangeException( + nameof(maximumMemberYawErrorRadians), + "成员航向误差上限不能大于π。"); + } + + _memberPositionErrorWarningMeters = + memberPositionErrorWarningMeters; + _maximumMemberPositionErrorMeters = + maximumMemberPositionErrorMeters; + _memberYawErrorWarningRadians = + memberYawErrorWarningRadians; + _maximumMemberYawErrorRadians = + maximumMemberYawErrorRadians; + LastFailureReason = string.Empty; + } + + public FleetLayout ActiveLayout => _activeLayout; + + public bool IsActive => + _activeLayout != null && + !_isCompleted && + !_isFaulted && + _fleetController.IsActive; + + public bool IsCompleted => _isCompleted; + + public bool IsFaulted => _isFaulted; + + public string LastFailureReason { get; private set; } + + public void Start( + FleetLayout layout, + Trajectory2D trajectory) + { + if (layout == null) + { + throw new ArgumentNullException(nameof(layout)); + } + + if (trajectory == null) + { + throw new ArgumentNullException(nameof(trajectory)); + } + + _fleetController.Start(trajectory); + _activeLayout = layout; + _isCompleted = false; + _isFaulted = false; + LastFailureReason = string.Empty; + } + + public FleetCoordinationCycleResult ExecuteCycle( + IReadOnlyList memberStates, + double targetTimestampSeconds, + double deltaTimeSeconds, + out FleetCoordinationCycleOutput output) + { + if (memberStates == null) + { + throw new ArgumentNullException(nameof(memberStates)); + } + + NumericGuard.EnsureFiniteNonNegative( + targetTimestampSeconds, + nameof(targetTimestampSeconds)); + NumericGuard.EnsureFinitePositive( + deltaTimeSeconds, + nameof(deltaTimeSeconds)); + + if (_activeLayout == null) + { + output = CreateOutput( + null, + EmptyMemberErrors, + EmptyMemberCommands, + "车队布局尚未激活。"); + return FleetCoordinationCycleResult.Inactive; + } + + if (_isFaulted) + { + output = CreateStopOutput( + null, + EmptyMemberErrors, + LastFailureReason); + return FleetCoordinationCycleResult.Faulted; + } + + if (_isCompleted) + { + output = CreateStopOutput( + null, + EmptyMemberErrors, + string.Empty); + return FleetCoordinationCycleResult.Completed; + } + + if (!_fleetController.IsActive) + { + output = CreateStopOutput( + null, + EmptyMemberErrors, + "车队中心控制器尚未启动或已经取消。"); + return FleetCoordinationCycleResult.Inactive; + } + + var estimate = _stateEstimator.Estimate( + _activeLayout, + memberStates, + targetTimestampSeconds); + if (!estimate.IsAvailable || + !estimate.State.HasValue) + { + output = CreateStopOutput( + null, + estimate.MemberErrors, + estimate.UnavailableReason); + return FleetCoordinationCycleResult.WaitingForState; + } + + var state = estimate.State.Value; + var speedScale = CalculateLayoutSpeedScale( + estimate.MemberErrors, + out var layoutFailureReason, + out var limitingReason); + if (layoutFailureReason != null) + { + return Fail( + state, + estimate.MemberErrors, + layoutFailureReason, + out output); + } + + var controlResult = + _fleetController.ComputeCommand( + state, + deltaTimeSeconds, + out var fleetCommand); + + if (controlResult == + FleetControlCycleResult.CommandGenerated) + { + var scaledFleetCommand = ScaleFleetCommand( + fleetCommand, + speedScale); + var baseMemberCommands = + FleetKinematics.Decompose( + _activeLayout, + scaledFleetCommand); + var memberCommands = + _memberCommandCorrector.Correct( + _activeLayout, + baseMemberCommands, + estimate.MemberErrors, + applyRelativeCorrection: + speedScale >= 1.0); + output = new FleetCoordinationCycleOutput( + state, + estimate.MemberErrors, + scaledFleetCommand, + baseMemberCommands, + memberCommands, + speedScale, + limitingReason); + return FleetCoordinationCycleResult.CommandGenerated; + } + + if (controlResult == + FleetControlCycleResult.Completed) + { + _isCompleted = true; + output = CreateStopOutput( + state, + estimate.MemberErrors, + string.Empty); + return FleetCoordinationCycleResult.Completed; + } + + if (controlResult == + FleetControlCycleResult.Faulted) + { + var reason = string.IsNullOrWhiteSpace( + _fleetController.LastFailureReason) + ? "车队中心轨迹控制失败。" + : _fleetController.LastFailureReason; + return Fail( + state, + estimate.MemberErrors, + reason, + out output, + cancelController: false); + } + + output = CreateStopOutput( + state, + estimate.MemberErrors, + "车队中心控制器当前未生成命令。"); + return FleetCoordinationCycleResult.Inactive; + } + + public void Cancel() + { + _fleetController.Cancel(); + _isCompleted = false; + _isFaulted = false; + LastFailureReason = string.Empty; + } + + private double CalculateLayoutSpeedScale( + IReadOnlyList memberErrors, + out string failureReason, + out string limitingReason) + { + var speedScale = 1.0; + failureReason = null; + limitingReason = string.Empty; + + for (var index = 0; + index < memberErrors.Count; + index++) + { + var memberError = memberErrors[index]; + var poseError = + memberError.ActualPoseInExpectedVehicleFrame; + var positionErrorMeters = Math.Sqrt( + poseError.XMeters * poseError.XMeters + + poseError.YMeters * poseError.YMeters); + var yawErrorRadians = + Math.Abs(poseError.YawRadians); + + if (positionErrorMeters >= + _maximumMemberPositionErrorMeters) + { + failureReason = + $"车辆{memberError.VehicleId}相对布局位置误差" + + $"{positionErrorMeters:F3}m达到停止阈值。"; + return 0.0; + } + + if (yawErrorRadians >= + _maximumMemberYawErrorRadians) + { + failureReason = + $"车辆{memberError.VehicleId}相对布局航向误差" + + $"{AngleMath.RadiansToDegrees(yawErrorRadians):F2}°" + + "达到停止阈值。"; + return 0.0; + } + + var positionScale = CalculateScale( + positionErrorMeters, + _memberPositionErrorWarningMeters, + _maximumMemberPositionErrorMeters); + if (positionScale < speedScale) + { + speedScale = positionScale; + limitingReason = + $"车辆{memberError.VehicleId}相对布局位置误差" + + $"{positionErrorMeters:F3}m，车队统一速度比例" + + $"降至{speedScale:F3}。"; + } + + var yawScale = CalculateScale( + yawErrorRadians, + _memberYawErrorWarningRadians, + _maximumMemberYawErrorRadians); + if (yawScale < speedScale) + { + speedScale = yawScale; + limitingReason = + $"车辆{memberError.VehicleId}相对布局航向误差" + + $"{AngleMath.RadiansToDegrees(yawErrorRadians):F2}°，" + + $"车队统一速度比例降至{speedScale:F3}。"; + } + } + + return speedScale; + } + + private static double CalculateScale( + double errorMagnitude, + double warningThreshold, + double stopThreshold) + { + if (errorMagnitude <= warningThreshold) + { + return 1.0; + } + + return (stopThreshold - errorMagnitude) / + (stopThreshold - warningThreshold); + } + + private static FleetMotionCommand ScaleFleetCommand( + FleetMotionCommand command, + double speedScale) + { + var twist = command.TwistAtReferencePoint; + return new FleetMotionCommand( + command.ReferencePointInFleet, + new Twist2D( + twist.VxMetersPerSecond * speedScale, + twist.VyMetersPerSecond * speedScale, + twist.OmegaRadiansPerSecond * speedScale)); + } + + private FleetCoordinationCycleResult Fail( + FleetState state, + IReadOnlyList memberErrors, + string reason, + out FleetCoordinationCycleOutput output, + bool cancelController = true) + { + if (cancelController) + { + _fleetController.Cancel(); + } + + _isFaulted = true; + _isCompleted = false; + LastFailureReason = reason ?? string.Empty; + output = CreateStopOutput( + state, + memberErrors, + LastFailureReason); + return FleetCoordinationCycleResult.Faulted; + } + + private FleetCoordinationCycleOutput CreateStopOutput( + FleetState? state, + IReadOnlyList memberErrors, + string reason) + { + var stopCommands = _activeLayout == null + ? EmptyMemberCommands + : FleetKinematics.Decompose( + _activeLayout, + FleetMotionCommand.Stop()); + + return CreateOutput( + state, + memberErrors, + stopCommands, + reason); + } + + private static FleetCoordinationCycleOutput CreateOutput( + FleetState? state, + IReadOnlyList memberErrors, + IReadOnlyList memberCommands, + string reason) + { + return new FleetCoordinationCycleOutput( + state, + memberErrors, + FleetMotionCommand.Stop(), + memberCommands, + memberCommands, + 0.0, + reason); + } + } +} diff --git a/MultiWheelC/Fleet/FleetLayoutCapture.cs b/MultiWheelC/Fleet/FleetLayoutCapture.cs new file mode 100644 index 0000000..e998bd8 --- /dev/null +++ b/MultiWheelC/Fleet/FleetLayoutCapture.cs @@ -0,0 +1,181 @@ +using System; +using System.Collections.Generic; +using MyParking.Shared; +// 夹紧后只执行一次 → 建立固定布局 +namespace MultiWheelC.Fleet +{ + // 建立编队时使用的一辆成员车世界位姿快照。 + public readonly struct FleetMemberPose + { + public FleetMemberPose( + int vehicleId, + Pose2D poseInWorld) + { + if (vehicleId <= 0) + { + throw new ArgumentOutOfRangeException( + nameof(vehicleId), + "编队成员车号必须大于零。"); + } + + NumericGuard.EnsureFinite( + poseInWorld, + nameof(poseInWorld)); + + VehicleId = vehicleId; + PoseInWorld = new Pose2D( + poseInWorld.XMeters, + poseInWorld.YMeters, + AngleMath.NormalizeRadians( + poseInWorld.YawRadians)); + } + + public int VehicleId { get; } + + public Pose2D PoseInWorld { get; } + } + + // 保存布局建立时的车队世界位姿和固定成员布局。 + public readonly struct FleetLayoutCaptureResult + { + public FleetLayoutCaptureResult( + Pose2D fleetPoseInWorld, + FleetLayout layout) + { + NumericGuard.EnsureFinite( + fleetPoseInWorld, + nameof(fleetPoseInWorld)); + + FleetPoseInWorld = new Pose2D( + fleetPoseInWorld.XMeters, + fleetPoseInWorld.YMeters, + AngleMath.NormalizeRadians( + fleetPoseInWorld.YawRadians)); + Layout = layout ?? + throw new ArgumentNullException( + nameof(layout)); + } + + public Pose2D FleetPoseInWorld { get; } + + public FleetLayout Layout { get; } + } + + // 根据同一世界坐标系中的成员位姿建立车队几何中心和固定布局。 + public static class FleetLayoutCapture + { + public static FleetLayoutCaptureResult Capture( + IReadOnlyList memberPoses, + int leaderVehicleId) + { + if (memberPoses == null) + { + throw new ArgumentNullException( + nameof(memberPoses)); + } + + if (memberPoses.Count == 0) + { + throw new ArgumentException( + "建立编队布局至少需要一辆成员车。", + nameof(memberPoses)); + } + + if (leaderVehicleId <= 0) + { + throw new ArgumentOutOfRangeException( + nameof(leaderVehicleId), + "主车车号必须大于零。"); + } + + var vehicleIds = new HashSet(); + var centerXMeters = 0.0; + var centerYMeters = 0.0; + var leaderFound = false; + var leaderYawRadians = 0.0; + + for (var index = 0; + index < memberPoses.Count; + index++) + { + var memberPose = memberPoses[index]; + if (memberPose.VehicleId <= 0) + { + throw new ArgumentOutOfRangeException( + nameof(memberPoses), + $"第{index}辆成员车的车号必须大于零。"); + } + + NumericGuard.EnsureFinite( + memberPose.PoseInWorld, + $"{nameof(memberPoses)}[{index}]." + + nameof(FleetMemberPose.PoseInWorld)); + + if (!vehicleIds.Add(memberPose.VehicleId)) + { + throw new ArgumentException( + $"成员位姿包含重复车号{memberPose.VehicleId}。", + nameof(memberPoses)); + } + + centerXMeters += + memberPose.PoseInWorld.XMeters; + centerYMeters += + memberPose.PoseInWorld.YMeters; + + if (memberPose.VehicleId == leaderVehicleId) + { + leaderFound = true; + leaderYawRadians = + memberPose.PoseInWorld.YawRadians; + } + } + + if (!leaderFound) + { + throw new ArgumentException( + $"成员位姿中不存在主车{leaderVehicleId}。", + nameof(leaderVehicleId)); + } + + centerXMeters /= memberPoses.Count; + centerYMeters /= memberPoses.Count; + NumericGuard.EnsureFinite( + centerXMeters, + nameof(centerXMeters)); + NumericGuard.EnsureFinite( + centerYMeters, + nameof(centerYMeters)); + + var fleetPoseInWorld = new Pose2D( + centerXMeters, + centerYMeters, + AngleMath.NormalizeRadians( + leaderYawRadians)); + var worldPoseInFleet = + FrameTransform2D.Inverse( + fleetPoseInWorld); + var vehicleLayouts = + new VehicleLayout[memberPoses.Count]; + + for (var index = 0; + index < memberPoses.Count; + index++) + { + var memberPose = memberPoses[index]; + var poseInFleet = + FrameTransform2D.Compose( + worldPoseInFleet, + memberPose.PoseInWorld); + vehicleLayouts[index] = + new VehicleLayout( + memberPose.VehicleId, + poseInFleet); + } + + return new FleetLayoutCaptureResult( + fleetPoseInWorld, + new FleetLayout(vehicleLayouts)); + } + } +} diff --git a/MultiWheelC/Fleet/FleetMemberAgent.cs b/MultiWheelC/Fleet/FleetMemberAgent.cs index 3b7ba8d..d66294a 100644 --- a/MultiWheelC/Fleet/FleetMemberAgent.cs +++ b/MultiWheelC/Fleet/FleetMemberAgent.cs @@ -1 +1,456 @@ -// 主车、从车都有，负责执行分配给本车的命令 \ No newline at end of file +using System; +using MyParking.Shared; + +namespace MultiWheelC.Fleet +{ + /// 表示成员车在一次车队动作中的本地执行阶段。 + public enum FleetMemberAgentState + { + Idle = 0, + Preparing = 1, + Ready = 2, + Active = 3, + Faulted = 4 + } + + /// 区分固定β滚动运动和车辆中心纯自转的准备方式。 + public enum FleetMemberPreparationMode + { + Rolling = 0, + Spin = 1 + } + + /// 负责一辆成员车的舵轮准备、激活和本车速度命令执行。 + public sealed class FleetMemberAgent + { + private const double MotionDeadband = 1e-6; + + private readonly MultiWheelChassisAdapter _adapter; + private readonly double _alignmentToleranceRadians; + private readonly double _alignmentStableSeconds; + + private double _alignedDurationSeconds; + + /// 创建绑定到一辆多舵轮底盘的成员车执行器。 + public FleetMemberAgent( + MultiWheelChassisAdapter adapter, + double alignmentToleranceRadians, + double alignmentStableSeconds) + { + _adapter = adapter ?? + throw new ArgumentNullException(nameof(adapter)); + NumericGuard.EnsureFinitePositive( + alignmentToleranceRadians, + nameof(alignmentToleranceRadians)); + NumericGuard.EnsureFiniteNonNegative( + alignmentStableSeconds, + nameof(alignmentStableSeconds)); + + if (alignmentToleranceRadians > Math.PI) + { + throw new ArgumentOutOfRangeException( + nameof(alignmentToleranceRadians), + "舵轮到位容差不能大于π。"); + } + + _alignmentToleranceRadians = + alignmentToleranceRadians; + _alignmentStableSeconds = + alignmentStableSeconds; + State = FleetMemberAgentState.Idle; + LastFailureReason = string.Empty; + } + + public int VehicleId => _adapter.VehicleId; + + public FleetMemberAgentState State { get; private set; } + + public FleetMemberPreparationMode? PreparationMode + { + get; + private set; + } + + public long CurrentPlanId { get; private set; } + + public double MotionDirectionInBodyRadians + { + get; + private set; + } + + public string LastFailureReason { get; private set; } + + /// 停车并开始准备本车固定β滚动运动系。 + public bool BeginRollingPreparation( + long planId, + double motionDirectionInBodyRadians) + { + ValidatePlanId(planId); + NumericGuard.EnsureFinite( + motionDirectionInBodyRadians, + nameof(motionDirectionInBodyRadians)); + + return BeginPreparation( + planId, + FleetMemberPreparationMode.Rolling, + AngleMath.NormalizeRadians( + motionDirectionInBodyRadians)); + } + + /// 停车并开始准备车辆中心纯自转所需的舵轮方向。 + public bool BeginSpinPreparation(long planId) + { + ValidatePlanId(planId); + return BeginPreparation( + planId, + FleetMemberPreparationMode.Spin, + motionDirectionInBodyRadians: 0.0); + } + + /// 检查舵轮是否已连续稳定到位；宿主应在准备阶段周期调用。 + public FleetMemberAgentState UpdatePreparation( + double deltaTimeSeconds) + { + NumericGuard.EnsureFinitePositive( + deltaTimeSeconds, + nameof(deltaTimeSeconds)); + + if (State != FleetMemberAgentState.Preparing) + { + return State; + } + + bool aligned; + try + { + aligned = UpdateAndCheckAlignment(); + } + catch (InvalidOperationException exception) + { + Fail(exception.Message); + return State; + } + catch (ArgumentException exception) + { + Fail(exception.Message); + return State; + } + + if (State == FleetMemberAgentState.Faulted) + { + return State; + } + + _alignedDurationSeconds = aligned + ? _alignedDurationSeconds + deltaTimeSeconds + : 0.0; + + if (aligned && + _alignedDurationSeconds >= + _alignmentStableSeconds) + { + State = FleetMemberAgentState.Ready; + LastFailureReason = string.Empty; + } + + return State; + } + + /// 在主车确认全队Ready后激活本车已经准备好的运动方式。 + public bool Activate(long planId) + { + if (planId != CurrentPlanId) + { + LastFailureReason = + "激活任务编号与当前准备任务不一致。"; + return false; + } + + if (State == FleetMemberAgentState.Active) + { + return true; + } + + if (State != FleetMemberAgentState.Ready || + !PreparationMode.HasValue) + { + return RejectWhileStopped( + "成员车尚未完成舵轮准备。"); + } + + bool stillAligned; + try + { + stillAligned = ArePreparedWheelsStillAligned(); + } + catch (InvalidOperationException exception) + { + return Fail(exception.Message); + } + catch (ArgumentException exception) + { + return Fail(exception.Message); + } + + if (!stillAligned) + { + State = FleetMemberAgentState.Preparing; + _alignedDurationSeconds = 0.0; + return RejectWhileStopped( + "成员车在激活前失去舵轮到位状态。"); + } + + try + { + if (PreparationMode.Value == + FleetMemberPreparationMode.Rolling) + { + _adapter.ActivateMotionFrame( + MotionDirectionInBodyRadians); + } + else if (!_adapter.AdoptPreparedSpinForXYTh( + _alignmentToleranceRadians)) + { + return Fail( + BuildAdapterFailureReason( + "无法激活已经准备好的原地自转舵轮。")); + } + } + catch (InvalidOperationException exception) + { + return Fail(exception.Message); + } + catch (ArgumentException exception) + { + return Fail(exception.Message); + } + + State = FleetMemberAgentState.Active; + LastFailureReason = string.Empty; + return true; + } + + /// 校验任务和车号后执行分配给本车的车体系速度命令。 + public bool Execute( + long planId, + FleetMemberCommand command, + TimeSpan? interval = null) + { + ValidatePlanId(planId); + NumericGuard.EnsureFinite( + command.TwistInVehicleBody, + nameof(command)); + + if (planId != CurrentPlanId) + { + return Fail( + "速度命令任务编号与当前激活任务不一致。"); + } + + if (command.VehicleId != VehicleId) + { + return Fail( + $"速度命令属于车辆{command.VehicleId}，" + + $"当前成员车号为{VehicleId}。"); + } + + if (State != FleetMemberAgentState.Active || + !PreparationMode.HasValue) + { + return RejectWhileStopped( + "成员车尚未激活，不能执行速度命令。"); + } + + if (!IsCommandCompatibleWithPreparation( + command.TwistInVehicleBody)) + { + return Fail( + "速度命令与本次舵轮准备方式不一致。"); + } + + try + { + if (!_adapter.SendBodyTwist( + command.TwistInVehicleBody, + interval)) + { + return Fail( + BuildAdapterFailureReason( + "成员车底盘拒绝执行速度命令。")); + } + } + catch (InvalidOperationException exception) + { + return Fail(exception.Message); + } + catch (ArgumentException exception) + { + return Fail(exception.Message); + } + + LastFailureReason = string.Empty; + return true; + } + + /// 正常取消当前任务并立即停止驱动轮。 + public void Stop() + { + _adapter.StopImmediately(); + State = FleetMemberAgentState.Idle; + PreparationMode = null; + CurrentPlanId = 0; + MotionDirectionInBodyRadians = 0.0; + _alignedDurationSeconds = 0.0; + LastFailureReason = string.Empty; + } + + /// 重置上一动作并下发本次滚动或自转舵轮准备目标。 + private bool BeginPreparation( + long planId, + FleetMemberPreparationMode mode, + double motionDirectionInBodyRadians) + { + try + { + _adapter.StopImmediately(); + _adapter.ResetToBodyFrame(); + + CurrentPlanId = planId; + PreparationMode = mode; + MotionDirectionInBodyRadians = + motionDirectionInBodyRadians; + State = FleetMemberAgentState.Preparing; + LastFailureReason = string.Empty; + _alignedDurationSeconds = 0.0; + + var accepted = mode == + FleetMemberPreparationMode.Rolling + ? _adapter.PrepareParallelDirection( + motionDirectionInBodyRadians) + : _adapter.PrepareSpin( + alignmentToleranceDegrees: + AngleMath.RadiansToDegrees( + _alignmentToleranceRadians)); + + if (!accepted) + { + return Fail( + BuildAdapterFailureReason( + "成员车底盘拒绝舵轮准备目标。")); + } + + return true; + } + catch (InvalidOperationException exception) + { + return Fail(exception.Message); + } + catch (ArgumentException exception) + { + return Fail(exception.Message); + } + } + + /// 更新当前准备目标并读取舵轮到位状态。 + private bool UpdateAndCheckAlignment() + { + if (PreparationMode == + FleetMemberPreparationMode.Rolling) + { + return _adapter.AreParallelWheelsAligned( + MotionDirectionInBodyRadians, + _alignmentToleranceRadians); + } + + if (!_adapter.PrepareSpin( + alignmentToleranceDegrees: + AngleMath.RadiansToDegrees( + _alignmentToleranceRadians))) + { + Fail( + BuildAdapterFailureReason( + "成员车底盘无法继续更新原地自转准备。")); + return false; + } + + return _adapter.AreSpinWheelsAligned; + } + + /// 确认舵轮在全队释放前仍保持到位。 + private bool ArePreparedWheelsStillAligned() + { + return PreparationMode == + FleetMemberPreparationMode.Rolling + ? _adapter.AreParallelWheelsAligned( + MotionDirectionInBodyRadians, + _alignmentToleranceRadians) + : _adapter.AreSpinWheelsAligned; + } + + /// 禁止滚动准备执行纯自转，也禁止自转准备执行平移。 + private bool IsCommandCompatibleWithPreparation( + Twist2D bodyTwist) + { + var linearSpeed = Math.Sqrt( + bodyTwist.VxMetersPerSecond * + bodyTwist.VxMetersPerSecond + + bodyTwist.VyMetersPerSecond * + bodyTwist.VyMetersPerSecond); + var hasLinearMotion = + linearSpeed > MotionDeadband; + var hasAngularMotion = + Math.Abs( + bodyTwist.OmegaRadiansPerSecond) > + MotionDeadband; + + if (!hasLinearMotion && !hasAngularMotion) + { + return true; + } + + return PreparationMode == + FleetMemberPreparationMode.Rolling + ? hasLinearMotion + : !hasLinearMotion && hasAngularMotion; + } + + /// 拒绝未满足执行条件的命令并保持车辆零速。 + private bool RejectWhileStopped(string reason) + { + _adapter.StopImmediately(); + LastFailureReason = reason ?? string.Empty; + return false; + } + + /// 锁存成员车故障并立即清零驱动轮速度。 + private bool Fail(string reason) + { + _adapter.StopImmediately(); + State = FleetMemberAgentState.Faulted; + LastFailureReason = reason ?? string.Empty; + return false; + } + + /// 优先返回底盘提供的具体失败原因。 + private string BuildAdapterFailureReason( + string fallbackReason) + { + return string.IsNullOrWhiteSpace( + _adapter.LastFailureReason) + ? fallbackReason + : _adapter.LastFailureReason; + } + + /// 拒绝零值和负值任务编号。 + private static void ValidatePlanId(long planId) + { + if (planId <= 0) + { + throw new ArgumentOutOfRangeException( + nameof(planId), + "车队动作任务编号必须大于零。"); + } + } + } +} diff --git a/MultiWheelC/Fleet/FleetMemberCommandCorrector.cs b/MultiWheelC/Fleet/FleetMemberCommandCorrector.cs new file mode 100644 index 0000000..b5ceba2 --- /dev/null +++ b/MultiWheelC/Fleet/FleetMemberCommandCorrector.cs @@ -0,0 +1,433 @@ +using System; +using System.Collections.Generic; +using MyParking.Shared; + +namespace MultiWheelC.Fleet +{ + // 将小范围成员布局误差转换为不改变车队整体刚体运动的相对速度修正。 + public sealed class FleetMemberCommandCorrector + { + private const double GeometryTolerance = 1e-12; + + private readonly double _longitudinalPositionGainPerSecond; + private readonly double _lateralPositionGainPerSecond; + private readonly double _yawGainPerSecond; + private readonly double _positionErrorDeadbandMeters; + private readonly double _yawErrorDeadbandRadians; + private readonly double _maximumLinearCorrectionMetersPerSecond; + private readonly double _maximumAngularCorrectionRadiansPerSecond; + + public FleetMemberCommandCorrector( + double longitudinalPositionGainPerSecond, + double lateralPositionGainPerSecond, + double yawGainPerSecond, + double positionErrorDeadbandMeters, + double yawErrorDeadbandRadians, + double maximumLinearCorrectionMetersPerSecond, + double maximumAngularCorrectionRadiansPerSecond) + { + NumericGuard.EnsureFiniteNonNegative( + longitudinalPositionGainPerSecond, + nameof(longitudinalPositionGainPerSecond)); + NumericGuard.EnsureFiniteNonNegative( + lateralPositionGainPerSecond, + nameof(lateralPositionGainPerSecond)); + NumericGuard.EnsureFiniteNonNegative( + yawGainPerSecond, + nameof(yawGainPerSecond)); + NumericGuard.EnsureFiniteNonNegative( + positionErrorDeadbandMeters, + nameof(positionErrorDeadbandMeters)); + NumericGuard.EnsureFiniteNonNegative( + yawErrorDeadbandRadians, + nameof(yawErrorDeadbandRadians)); + NumericGuard.EnsureFinitePositive( + maximumLinearCorrectionMetersPerSecond, + nameof(maximumLinearCorrectionMetersPerSecond)); + NumericGuard.EnsureFinitePositive( + maximumAngularCorrectionRadiansPerSecond, + nameof(maximumAngularCorrectionRadiansPerSecond)); + + if (yawErrorDeadbandRadians > Math.PI) + { + throw new ArgumentOutOfRangeException( + nameof(yawErrorDeadbandRadians), + "成员航向误差死区不能大于π。"); + } + + _longitudinalPositionGainPerSecond = + longitudinalPositionGainPerSecond; + _lateralPositionGainPerSecond = + lateralPositionGainPerSecond; + _yawGainPerSecond = yawGainPerSecond; + _positionErrorDeadbandMeters = + positionErrorDeadbandMeters; + _yawErrorDeadbandRadians = + yawErrorDeadbandRadians; + _maximumLinearCorrectionMetersPerSecond = + maximumLinearCorrectionMetersPerSecond; + _maximumAngularCorrectionRadiansPerSecond = + maximumAngularCorrectionRadiansPerSecond; + } + + public IReadOnlyList Correct( + FleetLayout layout, + IReadOnlyList baseCommands, + IReadOnlyList memberErrors, + bool applyRelativeCorrection = true) + { + if (layout == null) + { + throw new ArgumentNullException(nameof(layout)); + } + + if (baseCommands == null) + { + throw new ArgumentNullException(nameof(baseCommands)); + } + + if (memberErrors == null) + { + throw new ArgumentNullException(nameof(memberErrors)); + } + + if (baseCommands.Count != layout.VehicleCount) + { + throw new ArgumentException( + "成员基础命令数量必须与车队布局一致。", + nameof(baseCommands)); + } + + if (memberErrors.Count != layout.VehicleCount) + { + throw new ArgumentException( + "成员布局误差数量必须与车队布局一致。", + nameof(memberErrors)); + } + + var commandsByVehicleId = + IndexCommands(baseCommands); + var errorsByVehicleId = + IndexErrors(memberErrors); + var orderedCommands = + new FleetMemberCommand[layout.VehicleCount]; + var orderedErrors = + new FleetMemberLayoutError[layout.VehicleCount]; + var rawCorrectionsInFleet = + new Twist2D[layout.VehicleCount]; + + for (var index = 0; + index < layout.Vehicles.Count; + index++) + { + var vehicleLayout = layout.Vehicles[index]; + if (!commandsByVehicleId.TryGetValue( + vehicleLayout.VehicleId, + out var baseCommand)) + { + throw new ArgumentException( + $"缺少车辆{vehicleLayout.VehicleId}的基础命令。", + nameof(baseCommands)); + } + + if (!errorsByVehicleId.TryGetValue( + vehicleLayout.VehicleId, + out var memberError)) + { + throw new ArgumentException( + $"缺少车辆{vehicleLayout.VehicleId}的布局误差。", + nameof(memberErrors)); + } + + orderedCommands[index] = baseCommand; + orderedErrors[index] = memberError; + rawCorrectionsInFleet[index] = + applyRelativeCorrection + ? CalculateRawCorrectionInFleet( + vehicleLayout, + memberError) + : Twist2D.Zero; + } + + var relativeCorrectionsInFleet = + RemoveCommonRigidMotion( + layout, + rawCorrectionsInFleet); + var correctedCommands = + new FleetMemberCommand[layout.VehicleCount]; + + for (var index = 0; + index < layout.Vehicles.Count; + index++) + { + var vehicleLayout = layout.Vehicles[index]; + var baseTwistInFleet = + FrameTransform2D.TransformTwistAtSamePoint( + vehicleLayout.PoseInFleet, + orderedCommands[index].TwistInVehicleBody); + var correctionInFleet = LimitCorrection( + relativeCorrectionsInFleet[index]); + var correctedTwistInFleet = Add( + baseTwistInFleet, + correctionInFleet); + + // 使用成员当前相对姿态表达最终命令，避免小航向误差造成坐标表达偏差。 + var actualPoseInFleet = + FrameTransform2D.Compose( + vehicleLayout.PoseInFleet, + orderedErrors[index] + .ActualPoseInExpectedVehicleFrame); + var fleetPoseInActualVehicle = + FrameTransform2D.Inverse( + actualPoseInFleet); + var correctedTwistInVehicleBody = + FrameTransform2D.TransformTwistAtSamePoint( + fleetPoseInActualVehicle, + correctedTwistInFleet); + + correctedCommands[index] = + new FleetMemberCommand( + vehicleLayout.VehicleId, + correctedTwistInVehicleBody); + } + + return Array.AsReadOnly(correctedCommands); + } + + private Twist2D CalculateRawCorrectionInFleet( + VehicleLayout vehicleLayout, + FleetMemberLayoutError memberError) + { + var error = + memberError.ActualPoseInExpectedVehicleFrame; + var correctionInExpectedVehicle = new Twist2D( + -_longitudinalPositionGainPerSecond * + ApplyDeadband( + error.XMeters, + _positionErrorDeadbandMeters), + -_lateralPositionGainPerSecond * + ApplyDeadband( + error.YMeters, + _positionErrorDeadbandMeters), + -_yawGainPerSecond * + ApplyDeadband( + error.YawRadians, + _yawErrorDeadbandRadians)); + + return FrameTransform2D.TransformTwistAtSamePoint( + vehicleLayout.PoseInFleet, + correctionInExpectedVehicle); + } + + private static Twist2D[] RemoveCommonRigidMotion( + FleetLayout layout, + IReadOnlyList rawCorrectionsInFleet) + { + var count = layout.VehicleCount; + var meanX = 0.0; + var meanY = 0.0; + var meanVx = 0.0; + var meanVy = 0.0; + var meanOmega = 0.0; + + for (var index = 0; index < count; index++) + { + var position = layout.Vehicles[index].PoseInFleet; + var correction = rawCorrectionsInFleet[index]; + meanX += position.XMeters; + meanY += position.YMeters; + meanVx += correction.VxMetersPerSecond; + meanVy += correction.VyMetersPerSecond; + meanOmega += correction.OmegaRadiansPerSecond; + } + + meanX /= count; + meanY /= count; + meanVx /= count; + meanVy /= count; + meanOmega /= count; + + var rotationalNumerator = 0.0; + var rotationalDenominator = 0.0; + for (var index = 0; index < count; index++) + { + var position = layout.Vehicles[index].PoseInFleet; + var correction = rawCorrectionsInFleet[index]; + var centeredX = position.XMeters - meanX; + var centeredY = position.YMeters - meanY; + var centeredVx = + correction.VxMetersPerSecond - meanVx; + var centeredVy = + correction.VyMetersPerSecond - meanVy; + + rotationalNumerator += + -centeredY * centeredVx + + centeredX * centeredVy; + rotationalDenominator += + centeredX * centeredX + + centeredY * centeredY; + } + + var commonOmegaFromTranslation = + rotationalDenominator <= GeometryTolerance + ? 0.0 + : rotationalNumerator / + rotationalDenominator; + var commonVxAtFleetOrigin = + meanVx + + commonOmegaFromTranslation * meanY; + var commonVyAtFleetOrigin = + meanVy - + commonOmegaFromTranslation * meanX; + var relativeCorrections = new Twist2D[count]; + + for (var index = 0; index < count; index++) + { + var position = layout.Vehicles[index].PoseInFleet; + var correction = rawCorrectionsInFleet[index]; + var commonVxAtMember = + commonVxAtFleetOrigin - + commonOmegaFromTranslation * + position.YMeters; + var commonVyAtMember = + commonVyAtFleetOrigin + + commonOmegaFromTranslation * + position.XMeters; + + relativeCorrections[index] = new Twist2D( + correction.VxMetersPerSecond - + commonVxAtMember, + correction.VyMetersPerSecond - + commonVyAtMember, + correction.OmegaRadiansPerSecond - + meanOmega); + } + + return relativeCorrections; + } + + private Twist2D LimitCorrection(Twist2D correction) + { + var linearMagnitude = Math.Sqrt( + correction.VxMetersPerSecond * + correction.VxMetersPerSecond + + correction.VyMetersPerSecond * + correction.VyMetersPerSecond); + var linearScale = + linearMagnitude <= + _maximumLinearCorrectionMetersPerSecond + ? 1.0 + : _maximumLinearCorrectionMetersPerSecond / + linearMagnitude; + var limitedOmega = Math.Max( + -_maximumAngularCorrectionRadiansPerSecond, + Math.Min( + _maximumAngularCorrectionRadiansPerSecond, + correction.OmegaRadiansPerSecond)); + + return new Twist2D( + correction.VxMetersPerSecond * linearScale, + correction.VyMetersPerSecond * linearScale, + limitedOmega); + } + + private static Dictionary + IndexCommands( + IReadOnlyList commands) + { + var indexed = + new Dictionary( + commands.Count); + + for (var index = 0; index < commands.Count; index++) + { + var command = commands[index]; + if (command.VehicleId <= 0) + { + throw new ArgumentOutOfRangeException( + nameof(commands), + $"第{index}个成员命令的车号无效。"); + } + + NumericGuard.EnsureFinite( + command.TwistInVehicleBody, + $"{nameof(commands)}[{index}]." + + nameof(FleetMemberCommand.TwistInVehicleBody)); + + if (indexed.ContainsKey(command.VehicleId)) + { + throw new ArgumentException( + $"成员命令包含重复车号{command.VehicleId}。", + nameof(commands)); + } + + indexed.Add(command.VehicleId, command); + } + + return indexed; + } + + private static Dictionary + IndexErrors( + IReadOnlyList errors) + { + var indexed = + new Dictionary( + errors.Count); + + for (var index = 0; index < errors.Count; index++) + { + var error = errors[index]; + if (error.VehicleId <= 0) + { + throw new ArgumentOutOfRangeException( + nameof(errors), + $"第{index}个布局误差的车号无效。"); + } + + NumericGuard.EnsureFinite( + error.ActualPoseInExpectedVehicleFrame, + $"{nameof(errors)}[{index}]." + + nameof(FleetMemberLayoutError + .ActualPoseInExpectedVehicleFrame)); + + if (indexed.ContainsKey(error.VehicleId)) + { + throw new ArgumentException( + $"成员布局误差包含重复车号{error.VehicleId}。", + nameof(errors)); + } + + indexed.Add(error.VehicleId, error); + } + + return indexed; + } + + private static double ApplyDeadband( + double value, + double deadband) + { + var magnitude = Math.Abs(value); + if (magnitude <= deadband) + { + return 0.0; + } + + return Math.Sign(value) * (magnitude - deadband); + } + + private static Twist2D Add( + Twist2D first, + Twist2D second) + { + return new Twist2D( + first.VxMetersPerSecond + + second.VxMetersPerSecond, + first.VyMetersPerSecond + + second.VyMetersPerSecond, + first.OmegaRadiansPerSecond + + second.OmegaRadiansPerSecond); + } + } +} diff --git a/MultiWheelC/Fleet/FleetPreparationCoordinator.cs b/MultiWheelC/Fleet/FleetPreparationCoordinator.cs new file mode 100644 index 0000000..0eecb5b --- /dev/null +++ b/MultiWheelC/Fleet/FleetPreparationCoordinator.cs @@ -0,0 +1,372 @@ +using System; +using System.Collections.Generic; +using System.Collections.ObjectModel; +using MyParking.Shared; + +namespace MultiWheelC.Fleet +{ + /// 表示主车侧车队运动准备的当前阶段。 + public enum FleetPreparationCoordinatorState + { + Idle = 0, + WaitingForMembers = 1, + ReadyToActivate = 2, + ActivationAuthorized = 3, + Faulted = 4 + } + + /// 保存一次滚动准备中分配给指定成员车的本地β目标。 + public readonly struct FleetMemberPreparationTarget + { + /// 创建一条属于指定任务和成员车的准备目标。 + public FleetMemberPreparationTarget( + long planId, + int vehicleId, + double motionDirectionInBodyRadians) + { + if (planId <= 0) + { + throw new ArgumentOutOfRangeException( + nameof(planId), + "车队动作任务编号必须大于零。"); + } + + if (vehicleId <= 0) + { + throw new ArgumentOutOfRangeException( + nameof(vehicleId), + "成员车号必须大于零。"); + } + + NumericGuard.EnsureFinite( + motionDirectionInBodyRadians, + nameof(motionDirectionInBodyRadians)); + + PlanId = planId; + VehicleId = vehicleId; + MotionDirectionInBodyRadians = + AngleMath.NormalizeRadians( + motionDirectionInBodyRadians); + } + + public long PlanId { get; } + + public int VehicleId { get; } + + public double MotionDirectionInBodyRadians { get; } + } + + /// 保存成员车对某次准备任务上报的本地状态。 + public readonly struct FleetMemberPreparationStatus + { + /// 创建一条成员车准备状态报告。 + public FleetMemberPreparationStatus( + long planId, + int vehicleId, + FleetMemberAgentState state, + string failureReason = "") + { + if (planId <= 0) + { + throw new ArgumentOutOfRangeException( + nameof(planId), + "车队动作任务编号必须大于零。"); + } + + if (vehicleId <= 0) + { + throw new ArgumentOutOfRangeException( + nameof(vehicleId), + "成员车号必须大于零。"); + } + + if (!Enum.IsDefined( + typeof(FleetMemberAgentState), + state)) + { + throw new ArgumentOutOfRangeException( + nameof(state), + "成员车准备状态无效。"); + } + + PlanId = planId; + VehicleId = vehicleId; + State = state; + FailureReason = failureReason ?? string.Empty; + } + + public long PlanId { get; } + + public int VehicleId { get; } + + public FleetMemberAgentState State { get; } + + public string FailureReason { get; } + } + + /// 在主车侧分配成员β并管理全队Ready统一激活屏障。 + public sealed class FleetPreparationCoordinator + { + private static readonly IReadOnlyList< + FleetMemberPreparationTarget> + EmptyTargets = Array.AsReadOnly( + Array.Empty()); + + private readonly Dictionary + _memberStates = + new Dictionary(); + + private IReadOnlyList + _targets = EmptyTargets; + + /// 创建尚未激活准备任务的主车侧协调器。 + public FleetPreparationCoordinator() + { + State = FleetPreparationCoordinatorState.Idle; + LastFailureReason = string.Empty; + } + + public FleetPreparationCoordinatorState State + { + get; + private set; + } + + public long CurrentPlanId { get; private set; } + + public double MotionDirectionInFleetRadians + { + get; + private set; + } + + public IReadOnlyList + Targets => _targets; + + public string LastFailureReason { get; private set; } + + /// 根据车队固定布局为全部成员建立本次滚动β准备目标。 + public void StartRollingPreparation( + long planId, + FleetLayout layout, + double motionDirectionInFleetRadians) + { + if (planId <= 0) + { + throw new ArgumentOutOfRangeException( + nameof(planId), + "车队动作任务编号必须大于零。"); + } + + if (layout == null) + { + throw new ArgumentNullException(nameof(layout)); + } + + NumericGuard.EnsureFinite( + motionDirectionInFleetRadians, + nameof(motionDirectionInFleetRadians)); + + var normalizedFleetDirection = + AngleMath.NormalizeRadians( + motionDirectionInFleetRadians); + var targets = + new FleetMemberPreparationTarget[ + layout.VehicleCount]; + + _memberStates.Clear(); + for (var index = 0; + index < layout.Vehicles.Count; + index++) + { + var vehicle = layout.Vehicles[index]; + var rawDirectionInBody = + AngleMath.NormalizeRadians( + normalizedFleetDirection - + vehicle.PoseInFleet.YawRadians); + var equivalentDirectionInBody = + SelectSteeringAxisEquivalent( + rawDirectionInBody); + + targets[index] = + new FleetMemberPreparationTarget( + planId, + vehicle.VehicleId, + equivalentDirectionInBody); + _memberStates.Add( + vehicle.VehicleId, + FleetMemberAgentState.Idle); + } + + CurrentPlanId = planId; + MotionDirectionInFleetRadians = + normalizedFleetDirection; + _targets = Array.AsReadOnly(targets); + State = + FleetPreparationCoordinatorState + .WaitingForMembers; + LastFailureReason = string.Empty; + } + + /// 接收一辆成员车的状态并重新计算全队Ready状态。 + public FleetPreparationCoordinatorState + ReportMemberStatus( + FleetMemberPreparationStatus status) + { + if (State == + FleetPreparationCoordinatorState.Idle || + State == + FleetPreparationCoordinatorState.Faulted) + { + return State; + } + + if (status.PlanId != CurrentPlanId) + { + return State; + } + + if (!_memberStates.ContainsKey(status.VehicleId)) + { + throw new ArgumentException( + $"车辆{status.VehicleId}不属于当前车队布局。", + nameof(status)); + } + + if (status.State == + FleetMemberAgentState.Faulted) + { + return Fail( + string.IsNullOrWhiteSpace( + status.FailureReason) + ? $"车辆{status.VehicleId}准备失败。" + : $"车辆{status.VehicleId}准备失败：" + + status.FailureReason); + } + + if (State == + FleetPreparationCoordinatorState + .ActivationAuthorized) + { + return State; + } + + if (status.State == + FleetMemberAgentState.Active) + { + return Fail( + $"车辆{status.VehicleId}在全队统一激活前已经进入Active。"); + } + + _memberStates[status.VehicleId] = status.State; + State = AreAllMembersReady() + ? FleetPreparationCoordinatorState + .ReadyToActivate + : FleetPreparationCoordinatorState + .WaitingForMembers; + LastFailureReason = string.Empty; + return State; + } + + /// 在全部成员Ready后授权外层向全队广播同一任务的激活命令。 + public bool TryAuthorizeActivation(long planId) + { + if (planId != CurrentPlanId) + { + return false; + } + + if (State == + FleetPreparationCoordinatorState + .ActivationAuthorized) + { + return true; + } + + if (State != + FleetPreparationCoordinatorState + .ReadyToActivate) + { + return false; + } + + State = FleetPreparationCoordinatorState + .ActivationAuthorized; + LastFailureReason = string.Empty; + return true; + } + + /// 查找指定成员车在当前任务中的本地β准备目标。 + public bool TryGetTarget( + int vehicleId, + out FleetMemberPreparationTarget target) + { + for (var index = 0; + index < _targets.Count; + index++) + { + if (_targets[index].VehicleId == vehicleId) + { + target = _targets[index]; + return true; + } + } + + target = default; + return false; + } + + /// 取消当前准备任务并清除成员状态和β目标。 + public void Cancel() + { + _memberStates.Clear(); + _targets = EmptyTargets; + CurrentPlanId = 0; + MotionDirectionInFleetRadians = 0.0; + State = FleetPreparationCoordinatorState.Idle; + LastFailureReason = string.Empty; + } + + /// 判断当前任务中的每辆成员车是否都已报告Ready。 + private bool AreAllMembersReady() + { + foreach (var state in _memberStates.Values) + { + if (state != FleetMemberAgentState.Ready) + { + return false; + } + } + + return _memberStates.Count > 0; + } + + /// 将有向β转换为±90°内的等效滚动轴，反向运动由轮速符号表达。 + private static double SelectSteeringAxisEquivalent( + double directionRadians) + { + var equivalent = AngleMath.NormalizeRadians( + directionRadians); + + if (equivalent > Math.PI / 2.0) + { + equivalent -= Math.PI; + } + else if (equivalent < -Math.PI / 2.0) + { + equivalent += Math.PI; + } + + return AngleMath.NormalizeRadians(equivalent); + } + + /// 锁存准备故障，等待外层停止所有成员并取消任务。 + private FleetPreparationCoordinatorState Fail( + string reason) + { + State = FleetPreparationCoordinatorState.Faulted; + LastFailureReason = reason ?? string.Empty; + return State; + } + } +} diff --git a/MultiWheelC/Fleet/FleetSafetySupervisor.cs b/MultiWheelC/Fleet/FleetSafetySupervisor.cs deleted file mode 100644 index 4935d90..0000000 --- a/MultiWheelC/Fleet/FleetSafetySupervisor.cs +++ /dev/null @@ -1 +0,0 @@ -// 检查通信超时、成员故障、定位状态、夹臂状态和相对误差 \ No newline at end of file diff --git a/MultiWheelC/Fleet/FleetStateEstimator.cs b/MultiWheelC/Fleet/FleetStateEstimator.cs index 97a5704..21ba16d 100644 --- a/MultiWheelC/Fleet/FleetStateEstimator.cs +++ b/MultiWheelC/Fleet/FleetStateEstimator.cs @@ -1 +1,579 @@ -// 主车汇总各车状态、时间对齐、计算车队中心和相对布局误差；输出FleetState \ No newline at end of file +using System; +using System.Collections.Generic; +using System.Collections.ObjectModel; +using MyParking.Shared; + +namespace MultiWheelC.Fleet +{ + // 一辆成员车在主车统一时间轴上的状态样本。 + public readonly struct FleetMemberStateSample + { + public FleetMemberStateSample( + int vehicleId, + double sampleTimestampSeconds, + Pose2D poseInWorld, + Twist2D twistAtVehicleOriginInWorld, + bool isStateAvailable, + bool hasValidVelocityEstimate) + { + if (vehicleId <= 0) + { + throw new ArgumentOutOfRangeException( + nameof(vehicleId), + "编队成员车号必须大于零。"); + } + + NumericGuard.EnsureFiniteNonNegative( + sampleTimestampSeconds, + nameof(sampleTimestampSeconds)); + NumericGuard.EnsureFinite( + poseInWorld, + nameof(poseInWorld)); + NumericGuard.EnsureFinite( + twistAtVehicleOriginInWorld, + nameof(twistAtVehicleOriginInWorld)); + + VehicleId = vehicleId; + SampleTimestampSeconds = sampleTimestampSeconds; + PoseInWorld = new Pose2D( + poseInWorld.XMeters, + poseInWorld.YMeters, + AngleMath.NormalizeRadians( + poseInWorld.YawRadians)); + TwistAtVehicleOriginInWorld = + twistAtVehicleOriginInWorld; + IsStateAvailable = isStateAvailable; + HasValidVelocityEstimate = + hasValidVelocityEstimate; + } + + public int VehicleId { get; } + + // 该时间戳必须已经换算到主车/协调器的单调时间轴。 + public double SampleTimestampSeconds { get; } + + public Pose2D PoseInWorld { get; } + + // 成员车体中心处的实际速度，在世界坐标系中表达。 + public Twist2D TwistAtVehicleOriginInWorld { get; } + + public bool IsStateAvailable { get; } + + public bool HasValidVelocityEstimate { get; } + } + + // 成员实际位姿相对固定布局目标位姿的误差。 + public readonly struct FleetMemberLayoutError + { + public FleetMemberLayoutError( + int vehicleId, + Pose2D actualPoseInExpectedVehicleFrame) + { + if (vehicleId <= 0) + { + throw new ArgumentOutOfRangeException( + nameof(vehicleId), + "编队成员车号必须大于零。"); + } + + NumericGuard.EnsureFinite( + actualPoseInExpectedVehicleFrame, + nameof(actualPoseInExpectedVehicleFrame)); + + VehicleId = vehicleId; + ActualPoseInExpectedVehicleFrame = + new Pose2D( + actualPoseInExpectedVehicleFrame.XMeters, + actualPoseInExpectedVehicleFrame.YMeters, + AngleMath.NormalizeRadians( + actualPoseInExpectedVehicleFrame + .YawRadians)); + } + + public int VehicleId { get; } + + // 期望成员车体系中表达的实际成员位姿；理想刚体布局时为Identity。 + public Pose2D ActualPoseInExpectedVehicleFrame { get; } + } + + // 一次车队状态估计的结果；不可用时不提供FleetState。 + public sealed class FleetStateEstimateResult + { + private static readonly IReadOnlyList + EmptyMemberErrors = Array.AsReadOnly( + Array.Empty()); + + private FleetStateEstimateResult( + bool isAvailable, + FleetState? state, + IReadOnlyList memberErrors, + string unavailableReason) + { + IsAvailable = isAvailable; + State = state; + MemberErrors = memberErrors; + UnavailableReason = unavailableReason; + } + + public bool IsAvailable { get; } + + public FleetState? State { get; } + + public IReadOnlyList MemberErrors { get; } + + public string UnavailableReason { get; } + + internal static FleetStateEstimateResult Available( + FleetState state, + FleetMemberLayoutError[] memberErrors) + { + return new FleetStateEstimateResult( + true, + state, + Array.AsReadOnly(memberErrors), + string.Empty); + } + + internal static FleetStateEstimateResult Unavailable( + string reason) + { + return new FleetStateEstimateResult( + false, + null, + EmptyMemberErrors, + reason ?? string.Empty); + } + } + + // 从各成员状态反算并融合车队虚拟中心状态。 + public sealed class FleetStateEstimator + { + private const double TimestampToleranceSeconds = 1e-9; + private const double MinimumCircularMeanMagnitude = 1e-12; + + private readonly double _maximumMemberStateAgeSeconds; + private readonly double _maximumPositionDisagreementMeters; + private readonly double _maximumYawDisagreementRadians; + + public FleetStateEstimator( + double maximumMemberStateAgeSeconds, + double maximumPositionDisagreementMeters, + double maximumYawDisagreementRadians) + { + NumericGuard.EnsureFinitePositive( + maximumMemberStateAgeSeconds, + nameof(maximumMemberStateAgeSeconds)); + NumericGuard.EnsureFinitePositive( + maximumPositionDisagreementMeters, + nameof(maximumPositionDisagreementMeters)); + NumericGuard.EnsureFinitePositive( + maximumYawDisagreementRadians, + nameof(maximumYawDisagreementRadians)); + + if (maximumYawDisagreementRadians > Math.PI) + { + throw new ArgumentOutOfRangeException( + nameof(maximumYawDisagreementRadians), + "车队候选航向差阈值不能大于π。"); + } + + _maximumMemberStateAgeSeconds = + maximumMemberStateAgeSeconds; + _maximumPositionDisagreementMeters = + maximumPositionDisagreementMeters; + _maximumYawDisagreementRadians = + maximumYawDisagreementRadians; + } + + public FleetStateEstimateResult Estimate( + FleetLayout layout, + IReadOnlyList memberStates, + double targetTimestampSeconds) + { + if (layout == null) + { + throw new ArgumentNullException(nameof(layout)); + } + + if (memberStates == null) + { + throw new ArgumentNullException(nameof(memberStates)); + } + + NumericGuard.EnsureFiniteNonNegative( + targetTimestampSeconds, + nameof(targetTimestampSeconds)); + + if (memberStates.Count != layout.VehicleCount) + { + return FleetStateEstimateResult.Unavailable( + $"成员状态数量{memberStates.Count}与布局数量" + + $"{layout.VehicleCount}不一致。"); + } + + var statesByVehicleId = + new Dictionary( + memberStates.Count); + + for (var index = 0; + index < memberStates.Count; + index++) + { + var memberState = memberStates[index]; + if (memberState.VehicleId <= 0) + { + return FleetStateEstimateResult.Unavailable( + $"第{index}个成员状态的车号无效。"); + } + + if (!layout.TryGetVehicle( + memberState.VehicleId, + out _)) + { + return FleetStateEstimateResult.Unavailable( + $"成员状态包含布局外车辆" + + $"{memberState.VehicleId}。"); + } + + if (statesByVehicleId.ContainsKey( + memberState.VehicleId)) + { + return FleetStateEstimateResult.Unavailable( + $"成员状态包含重复车号" + + $"{memberState.VehicleId}。"); + } + + statesByVehicleId.Add( + memberState.VehicleId, + memberState); + } + + var alignedMembers = + new AlignedMemberState[layout.VehicleCount]; + + for (var index = 0; + index < layout.Vehicles.Count; + index++) + { + var vehicleLayout = layout.Vehicles[index]; + if (!statesByVehicleId.TryGetValue( + vehicleLayout.VehicleId, + out var memberState)) + { + return FleetStateEstimateResult.Unavailable( + $"缺少车辆{vehicleLayout.VehicleId}的状态。"); + } + + var alignmentResult = AlignMemberState( + memberState, + targetTimestampSeconds, + out var alignedPoseInWorld); + if (alignmentResult != null) + { + return FleetStateEstimateResult.Unavailable( + alignmentResult); + } + + var candidateFleetPoseInWorld = + FrameTransform2D.Compose( + alignedPoseInWorld, + FrameTransform2D.Inverse( + vehicleLayout.PoseInFleet)); + + alignedMembers[index] = + new AlignedMemberState( + vehicleLayout, + memberState, + alignedPoseInWorld, + candidateFleetPoseInWorld); + } + + var disagreementReason = + FindCandidateDisagreement(alignedMembers); + if (disagreementReason != null) + { + return FleetStateEstimateResult.Unavailable( + disagreementReason); + } + + if (!TryAverageCandidateFleetPose( + alignedMembers, + out var fleetPoseInWorld)) + { + return FleetStateEstimateResult.Unavailable( + "成员候选航向无法形成唯一的车队平均航向。"); + } + + var hasValidVelocityEstimate = + TryAverageFleetOriginTwist( + alignedMembers, + fleetPoseInWorld, + out var twistAtFleetOriginInWorld); + var fleetState = new FleetState( + targetTimestampSeconds, + fleetPoseInWorld, + twistAtFleetOriginInWorld, + hasValidVelocityEstimate); + var memberErrors = CalculateMemberErrors( + alignedMembers, + fleetPoseInWorld); + + return FleetStateEstimateResult.Available( + fleetState, + memberErrors); + } + + private string AlignMemberState( + FleetMemberStateSample memberState, + double targetTimestampSeconds, + out Pose2D alignedPoseInWorld) + { + alignedPoseInWorld = memberState.PoseInWorld; + + if (!memberState.IsStateAvailable) + { + return $"车辆{memberState.VehicleId}状态不可用。"; + } + + var ageSeconds = + targetTimestampSeconds - + memberState.SampleTimestampSeconds; + + if (ageSeconds < -TimestampToleranceSeconds) + { + return $"车辆{memberState.VehicleId}的状态时间晚于" + + "本次估计目标时间。"; + } + + if (ageSeconds > _maximumMemberStateAgeSeconds) + { + return $"车辆{memberState.VehicleId}的状态已过期：" + + $"{ageSeconds:F3}s。"; + } + + if (ageSeconds <= TimestampToleranceSeconds) + { + return null; + } + + if (!memberState.HasValidVelocityEstimate) + { + return $"车辆{memberState.VehicleId}缺少时间对齐所需的" + + "有效速度。"; + } + + var twist = memberState.TwistAtVehicleOriginInWorld; + alignedPoseInWorld = new Pose2D( + memberState.PoseInWorld.XMeters + + twist.VxMetersPerSecond * ageSeconds, + memberState.PoseInWorld.YMeters + + twist.VyMetersPerSecond * ageSeconds, + AngleMath.NormalizeRadians( + memberState.PoseInWorld.YawRadians + + twist.OmegaRadiansPerSecond * ageSeconds)); + return null; + } + + private string FindCandidateDisagreement( + IReadOnlyList alignedMembers) + { + for (var firstIndex = 0; + firstIndex < alignedMembers.Count; + firstIndex++) + { + var first = alignedMembers[firstIndex]; + for (var secondIndex = firstIndex + 1; + secondIndex < alignedMembers.Count; + secondIndex++) + { + var second = alignedMembers[secondIndex]; + var dx = + first.CandidateFleetPoseInWorld.XMeters - + second.CandidateFleetPoseInWorld.XMeters; + var dy = + first.CandidateFleetPoseInWorld.YMeters - + second.CandidateFleetPoseInWorld.YMeters; + var positionDifferenceMeters = + Math.Sqrt(dx * dx + dy * dy); + var yawDifferenceRadians = Math.Abs( + AngleMath.ShortestDifferenceRadians( + first.CandidateFleetPoseInWorld + .YawRadians, + second.CandidateFleetPoseInWorld + .YawRadians)); + + if (positionDifferenceMeters > + _maximumPositionDisagreementMeters) + { + return $"车辆{first.VehicleLayout.VehicleId}与" + + $"车辆{second.VehicleLayout.VehicleId}反算的" + + $"车队中心相差{positionDifferenceMeters:F3}m，" + + "超过允许值。"; + } + + if (yawDifferenceRadians > + _maximumYawDisagreementRadians) + { + return $"车辆{first.VehicleLayout.VehicleId}与" + + $"车辆{second.VehicleLayout.VehicleId}反算的" + + $"车队航向相差" + + $"{AngleMath.RadiansToDegrees(yawDifferenceRadians):F2}°，" + + "超过允许值。"; + } + } + } + + return null; + } + + private static bool TryAverageCandidateFleetPose( + IReadOnlyList alignedMembers, + out Pose2D fleetPoseInWorld) + { + var xMeters = 0.0; + var yMeters = 0.0; + var yawCosineSum = 0.0; + var yawSineSum = 0.0; + + for (var index = 0; + index < alignedMembers.Count; + index++) + { + var candidate = + alignedMembers[index] + .CandidateFleetPoseInWorld; + xMeters += candidate.XMeters; + yMeters += candidate.YMeters; + yawCosineSum += Math.Cos(candidate.YawRadians); + yawSineSum += Math.Sin(candidate.YawRadians); + } + + var count = alignedMembers.Count; + var circularMeanMagnitude = Math.Sqrt( + yawCosineSum * yawCosineSum + + yawSineSum * yawSineSum); + if (circularMeanMagnitude < + MinimumCircularMeanMagnitude) + { + fleetPoseInWorld = Pose2D.Identity; + return false; + } + + fleetPoseInWorld = new Pose2D( + xMeters / count, + yMeters / count, + Math.Atan2(yawSineSum, yawCosineSum)); + return true; + } + + private static bool TryAverageFleetOriginTwist( + IReadOnlyList alignedMembers, + Pose2D fleetPoseInWorld, + out Twist2D twistAtFleetOriginInWorld) + { + for (var index = 0; + index < alignedMembers.Count; + index++) + { + if (!alignedMembers[index] + .MemberState + .HasValidVelocityEstimate) + { + twistAtFleetOriginInWorld = Twist2D.Zero; + return false; + } + } + + var vxMetersPerSecond = 0.0; + var vyMetersPerSecond = 0.0; + var omegaRadiansPerSecond = 0.0; + + for (var index = 0; + index < alignedMembers.Count; + index++) + { + var member = alignedMembers[index]; + var twist = member.MemberState + .TwistAtVehicleOriginInWorld; + var memberXFromFleetOrigin = + member.AlignedPoseInWorld.XMeters - + fleetPoseInWorld.XMeters; + var memberYFromFleetOrigin = + member.AlignedPoseInWorld.YMeters - + fleetPoseInWorld.YMeters; + + vxMetersPerSecond += + twist.VxMetersPerSecond + + twist.OmegaRadiansPerSecond * + memberYFromFleetOrigin; + vyMetersPerSecond += + twist.VyMetersPerSecond - + twist.OmegaRadiansPerSecond * + memberXFromFleetOrigin; + omegaRadiansPerSecond += + twist.OmegaRadiansPerSecond; + } + + var count = alignedMembers.Count; + twistAtFleetOriginInWorld = new Twist2D( + vxMetersPerSecond / count, + vyMetersPerSecond / count, + omegaRadiansPerSecond / count); + return true; + } + + private static FleetMemberLayoutError[] CalculateMemberErrors( + IReadOnlyList alignedMembers, + Pose2D fleetPoseInWorld) + { + var errors = + new FleetMemberLayoutError[alignedMembers.Count]; + + for (var index = 0; + index < alignedMembers.Count; + index++) + { + var member = alignedMembers[index]; + var expectedPoseInWorld = + FrameTransform2D.Compose( + fleetPoseInWorld, + member.VehicleLayout.PoseInFleet); + var actualPoseInExpectedVehicleFrame = + FrameTransform2D.Compose( + FrameTransform2D.Inverse( + expectedPoseInWorld), + member.AlignedPoseInWorld); + + errors[index] = new FleetMemberLayoutError( + member.VehicleLayout.VehicleId, + actualPoseInExpectedVehicleFrame); + } + + return errors; + } + + private readonly struct AlignedMemberState + { + public AlignedMemberState( + VehicleLayout vehicleLayout, + FleetMemberStateSample memberState, + Pose2D alignedPoseInWorld, + Pose2D candidateFleetPoseInWorld) + { + VehicleLayout = vehicleLayout; + MemberState = memberState; + AlignedPoseInWorld = alignedPoseInWorld; + CandidateFleetPoseInWorld = + candidateFleetPoseInWorld; + } + + public VehicleLayout VehicleLayout { get; } + + public FleetMemberStateSample MemberState { get; } + + public Pose2D AlignedPoseInWorld { get; } + + public Pose2D CandidateFleetPoseInWorld { get; } + } + } +} diff --git a/MultiWheelC/StateEstimation/DetourVehicleStateProvider.cs b/MultiWheelC/StateEstimation/DetourVehicleStateProvider.cs index c1fbcb0..c2b2dbd 100644 --- a/MultiWheelC/StateEstimation/DetourVehicleStateProvider.cs +++ b/MultiWheelC/StateEstimation/DetourVehicleStateProvider.cs @@ -6,6 +6,16 @@ using MyParking.Shared; namespace MultiWheelC.StateEstimation { + /// 表示Detour位姿相对已标注地图关键帧的当前可信级别。 + public enum DetourLocalizationQuality + { + Unknown = 0, + Normal = 1, + Degraded = 2, + Poor = 3, + Lost = 4 + } + /// /// 读取Detour位姿，并将可能发生重定位的原始世界坐标转换为当前任务使用的连续坐标。 /// @@ -55,6 +65,10 @@ namespace MultiWheelC.StateEstimation public const double DefaultMaximumAutomaticHeadingShiftRadians = 5.0 * Math.PI / 180.0; + public const double DefaultMaximumCachedFrameAgeSeconds = + 0.50; + public const int + DefaultLocalizationQualityConfirmationFrameCount = 3; private const double MillimetersPerMeter = 1000.0; private const double PositionEqualityToleranceMeters = 1e-9; @@ -64,6 +78,9 @@ namespace MultiWheelC.StateEstimation private const double InPlaceRotationAngularSpeedThresholdRadiansPerSecond = Math.PI / 180.0; + private const double NormalLocalizationStepUpperBound = 5.0; + private const double DegradedLocalizationStepUpperBound = 10.0; + private const double LostLocalizationStepLowerBound = 99.0; private readonly object _syncRoot = new object(); private readonly Stopwatch _clock = Stopwatch.StartNew(); @@ -83,6 +100,9 @@ namespace MultiWheelC.StateEstimation private readonly double _maximumAutomaticFrameShiftMeters; private readonly double _maximumAutomaticHeadingShiftRadians; + private readonly double _maximumCachedFrameAgeSeconds; + private readonly int + _localizationQualityConfirmationFrameCount; private Pose2D _acceptedPoseInControl; private double _acceptedTimestampSeconds; @@ -94,8 +114,15 @@ namespace MultiWheelC.StateEstimation private long _lastObservedTickRaw; private bool _hasDetourSourceFrameInterval; private double _lastDetourSourceFrameIntervalSeconds; + private double _lastNewDetourFrameTimestampSeconds; private Pose2D _controlFromDetour = Pose2D.Identity; + private int _lostLocalizationStepConsecutiveFrameCount; + private int _normalLocalizationStepRecoveryFrameCount; + private bool _localizationStepUnavailable; + private bool _localizationStepPredictionActive; + private bool _cachedFramePredictionActive; + private bool _hasMotionPrediction; private Pose2D _predictedPoseInControl; private double _predictionTimestampSeconds; @@ -144,7 +171,9 @@ namespace MultiWheelC.StateEstimation DefaultJumpConfirmationFrameCount, DefaultJumpConfirmationTimeoutSeconds, DefaultMaximumAutomaticFrameShiftMeters, - DefaultMaximumAutomaticHeadingShiftRadians) + DefaultMaximumAutomaticHeadingShiftRadians, + DefaultMaximumCachedFrameAgeSeconds, + DefaultLocalizationQualityConfirmationFrameCount) { } @@ -175,7 +204,9 @@ namespace MultiWheelC.StateEstimation DefaultJumpConfirmationFrameCount, DefaultJumpConfirmationTimeoutSeconds, DefaultMaximumAutomaticFrameShiftMeters, - DefaultMaximumAutomaticHeadingShiftRadians) + DefaultMaximumAutomaticHeadingShiftRadians, + DefaultMaximumCachedFrameAgeSeconds, + DefaultLocalizationQualityConfirmationFrameCount) { } @@ -197,6 +228,46 @@ namespace MultiWheelC.StateEstimation double jumpConfirmationTimeoutSeconds, double maximumAutomaticFrameShiftMeters, double maximumAutomaticHeadingShiftRadians) + : this( + velocityEstimator, + maximumLinearSpeedMetersPerSecond, + maximumAngularSpeedRadiansPerSecond, + positionJumpMarginMeters, + headingJumpMarginRadians, + velocityPositionResidualMeters, + velocityHeadingResidualRadians, + stationaryConfirmationSeconds, + headingOutlierConfirmationFrameCount, + headingOutlierPredictionTimeoutSeconds, + jumpConfirmationFrameCount, + jumpConfirmationTimeoutSeconds, + maximumAutomaticFrameShiftMeters, + maximumAutomaticHeadingShiftRadians, + DefaultMaximumCachedFrameAgeSeconds, + DefaultLocalizationQualityConfirmationFrameCount) + { + } + + /// + /// 创建同时配置缓存停更和定位质量确认参数的Detour状态源。 + /// + public DetourVehicleStateProvider( + VelocityEstimator2D velocityEstimator, + double maximumLinearSpeedMetersPerSecond, + double maximumAngularSpeedRadiansPerSecond, + double positionJumpMarginMeters, + double headingJumpMarginRadians, + double velocityPositionResidualMeters, + double velocityHeadingResidualRadians, + double stationaryConfirmationSeconds, + int headingOutlierConfirmationFrameCount, + double headingOutlierPredictionTimeoutSeconds, + int jumpConfirmationFrameCount, + double jumpConfirmationTimeoutSeconds, + double maximumAutomaticFrameShiftMeters, + double maximumAutomaticHeadingShiftRadians, + double maximumCachedFrameAgeSeconds, + int localizationQualityConfirmationFrameCount) { _velocityEstimator = velocityEstimator ?? throw new ArgumentNullException( @@ -235,6 +306,9 @@ namespace MultiWheelC.StateEstimation NumericGuard.EnsureFinitePositive( maximumAutomaticHeadingShiftRadians, nameof(maximumAutomaticHeadingShiftRadians)); + NumericGuard.EnsureFinitePositive( + maximumCachedFrameAgeSeconds, + nameof(maximumCachedFrameAgeSeconds)); if (jumpConfirmationFrameCount < 2) { @@ -250,6 +324,13 @@ namespace MultiWheelC.StateEstimation "Detour航向异常至少需要两个新帧确认。"); } + if (localizationQualityConfirmationFrameCount < 2) + { + throw new ArgumentOutOfRangeException( + nameof(localizationQualityConfirmationFrameCount), + "Detour定位质量失效和恢复至少需要两个新帧确认。"); + } + _maximumLinearSpeedMetersPerSecond = maximumLinearSpeedMetersPerSecond; _maximumAngularSpeedRadiansPerSecond = @@ -274,6 +355,10 @@ namespace MultiWheelC.StateEstimation maximumAutomaticFrameShiftMeters; _maximumAutomaticHeadingShiftRadians = maximumAutomaticHeadingShiftRadians; + _maximumCachedFrameAgeSeconds = + maximumCachedFrameAgeSeconds; + _localizationQualityConfirmationFrameCount = + localizationQualityConfirmationFrameCount; } /// @@ -289,15 +374,22 @@ namespace MultiWheelC.StateEstimation string.Empty; /// - /// 获取最近一次读取到的Detour源时间戳原值。 + /// 获取最近一次共享对象提交时的DateTime.Ticks原值。 /// public long? LastDetourTickRaw { get; private set; } /// - /// 获取最近一次Detour l_step原值；其精确定义仍由Detour接口文档确认。 + /// 获取最近一次Detour l_step原值，即到已标注地图关键帧的图上步数。 /// public double? LastDetourLocalizationStep { get; private set; } + /// 获取最近一帧Detour定位质量的瞬时分级。 + public DetourLocalizationQuality LocalizationQuality + { + get; + private set; + } = DetourLocalizationQuality.Unknown; + /// /// 获取当前是否正在确认疑似Detour坐标跳变。 /// @@ -435,10 +527,35 @@ namespace MultiWheelC.StateEstimation LastDetourTickRaw = observation.TickRaw; LastDetourLocalizationStep = observation.LocalizationStep; + LocalizationQuality = + ClassifyLocalizationStep( + observation.LocalizationStep); if (!_hasObservedDetourFrame) { SetLastObservedDetourFrame(observation); + _lastNewDetourFrameTimestampSeconds = + timestampSeconds; + + if (LocalizationQuality == + DetourLocalizationQuality.Lost) + { + _localizationStepUnavailable = true; + _lostLocalizationStepConsecutiveFrameCount = + _localizationQualityConfirmationFrameCount; + InvalidateHeadingObservation( + "Detour初始l_step达到丢失定位范围，航向暂不可用。"); + state = default; + LastFailureReason = + "Detour初始l_step=" + + observation.LocalizationStep + .ToString( + "F0", + CultureInfo.InvariantCulture) + + "，定位状态不可用。"; + return false; + } + AcceptReliableHeadingObservation(observation); state = AcceptPoseAfterReset( observation.PoseInDetour, @@ -471,6 +588,9 @@ namespace MultiWheelC.StateEstimation var previousPoseInDetour = _lastObservedPoseInDetour; SetLastObservedDetourFrame(observation); + _lastNewDetourFrameTimestampSeconds = + timestampSeconds; + _cachedFramePredictionActive = false; if (!NumericGuard.IsFinite( sourceDeltaTimeSeconds) || @@ -488,6 +608,14 @@ namespace MultiWheelC.StateEstimation sourceDeltaTimeSeconds; _hasDetourSourceFrameInterval = true; + if (HandleLocalizationStepQuality( + observation, + timestampSeconds, + out state)) + { + return !_localizationStepUnavailable; + } + var poseInControl = FrameTransform2D.TransformPose( _controlFromDetour, @@ -605,6 +733,18 @@ namespace MultiWheelC.StateEstimation { lock (_syncRoot) { + if ((_cachedFramePredictionActive || + _localizationStepPredictionActive) && + _hasReliableHeadingObservation && + _latestWheelVelocityValid && + _hasMotionPrediction) + { + headingRadians = + GetPredictedPoseInControl().YawRadians; + LastHeadingFailureReason = string.Empty; + return true; + } + if (_headingOutlierCandidateActive) { var candidateAgeSeconds = @@ -742,8 +882,15 @@ namespace MultiWheelC.StateEstimation _lastObservedTickRaw = 0L; _hasDetourSourceFrameInterval = false; _lastDetourSourceFrameIntervalSeconds = 0.0; + _lastNewDetourFrameTimestampSeconds = 0.0; _controlFromDetour = Pose2D.Identity; + _lostLocalizationStepConsecutiveFrameCount = 0; + _normalLocalizationStepRecoveryFrameCount = 0; + _localizationStepUnavailable = false; + _localizationStepPredictionActive = false; + _cachedFramePredictionActive = false; + _hasMotionPrediction = false; _predictedPoseInControl = Pose2D.Identity; _predictionTimestampSeconds = 0.0; @@ -765,15 +912,158 @@ namespace MultiWheelC.StateEstimation AutomaticFrameShiftCount = 0; LastDetourTickRaw = null; LastDetourLocalizationStep = null; + LocalizationQuality = + DetourLocalizationQuality.Unknown; LastFailureReason = string.Empty; LastHeadingFailureReason = string.Empty; } } + /// 按l_step确认定位丢失，并要求连续正常新帧后才恢复。 + private bool HandleLocalizationStepQuality( + DetourObservation observation, + double timestampSeconds, + out VehicleState state) + { + if (LocalizationQuality == + DetourLocalizationQuality.Lost) + { + _normalLocalizationStepRecoveryFrameCount = 0; + _lostLocalizationStepConsecutiveFrameCount++; + + if (_lostLocalizationStepConsecutiveFrameCount >= + _localizationQualityConfirmationFrameCount) + { + _localizationStepUnavailable = true; + _localizationStepPredictionActive = false; + InvalidateHeadingObservation( + "Detour l_step连续达到丢失定位范围，航向暂不可用。"); + state = default; + LastFailureReason = + "Detour l_step=" + + observation.LocalizationStep.ToString( + "F0", + CultureInfo.InvariantCulture) + + "已连续" + + _lostLocalizationStepConsecutiveFrameCount + + "个新帧达到丢失定位范围，车辆状态暂不可用。"; + return true; + } + + if (!_hasMotionPrediction || + !_latestWheelVelocityValid || + !_hasReliableHeadingObservation) + { + _localizationStepUnavailable = true; + _localizationStepPredictionActive = false; + InvalidateHeadingObservation( + "Detour定位质量异常且缺少可靠轮组预测，航向暂不可用。"); + state = default; + LastFailureReason = + "Detour定位质量异常且缺少可靠轮组预测，车辆状态暂不可用。"; + return true; + } + + _localizationStepPredictionActive = true; + state = CreatePredictedState(timestampSeconds); + LastFailureReason = + "Detour l_step达到丢失定位范围，正在等待连续新帧确认，" + + "当前使用轮组速度短时预测位姿。"; + return true; + } + + _lostLocalizationStepConsecutiveFrameCount = 0; + _localizationStepPredictionActive = false; + + if (!_localizationStepUnavailable) + { + _normalLocalizationStepRecoveryFrameCount = 0; + state = default; + return false; + } + + if (LocalizationQuality == + DetourLocalizationQuality.Normal) + { + _normalLocalizationStepRecoveryFrameCount++; + } + else + { + _normalLocalizationStepRecoveryFrameCount = 0; + } + + if (_normalLocalizationStepRecoveryFrameCount < + _localizationQualityConfirmationFrameCount) + { + state = default; + LastFailureReason = + "Detour定位质量正在恢复确认(" + + _normalLocalizationStepRecoveryFrameCount + + "/" + + _localizationQualityConfirmationFrameCount + + ")，车辆状态暂不可用。"; + return true; + } + + _localizationStepUnavailable = false; + _normalLocalizationStepRecoveryFrameCount = 0; + AcceptReliableHeadingObservation(observation); + state = AcceptPoseAfterReset( + FrameTransform2D.TransformPose( + _controlFromDetour, + observation.PoseInDetour), + timestampSeconds); + LastFailureReason = string.Empty; + return true; + } + private bool HandleRepeatedDetourFrame( double timestampSeconds, out VehicleState state) { + var cachedFrameAgeSeconds = + timestampSeconds - + _lastNewDetourFrameTimestampSeconds; + + if (!NumericGuard.IsFinite(cachedFrameAgeSeconds) || + cachedFrameAgeSeconds < 0.0) + { + InvalidateHeadingObservation( + "Detour缓存帧本机计时无效，航向暂不可用。"); + state = default; + LastFailureReason = + "Detour缓存帧本机计时无效，车辆状态暂不可用。"; + return false; + } + + if (_localizationStepUnavailable) + { + state = default; + LastFailureReason = + "Detour定位质量尚未通过连续健康新帧恢复确认。"; + return false; + } + + if (cachedFrameAgeSeconds > + _maximumCachedFrameAgeSeconds) + { + _cachedFramePredictionActive = false; + InvalidateHeadingObservation( + "Detour源tick超过缓存停更上限，航向暂不可用。"); + state = default; + LastFailureReason = + "Detour源tick已连续" + + cachedFrameAgeSeconds.ToString( + "F3", + CultureInfo.InvariantCulture) + + "s未更新，超过" + + _maximumCachedFrameAgeSeconds.ToString( + "F3", + CultureInfo.InvariantCulture) + + "s上限，车辆状态暂不可用。"; + return false; + } + if (_jumpCandidateActive) { return ReturnCandidateState( @@ -781,6 +1071,16 @@ namespace MultiWheelC.StateEstimation out state); } + if (IsVehicleMovingFromWheelFeedback()) + { + _cachedFramePredictionActive = true; + state = CreatePredictedState(timestampSeconds); + LastFailureReason = + "Detour源tick暂未更新，当前使用轮组速度短时预测位姿。"; + return true; + } + + _cachedFramePredictionActive = false; state = HandleRepeatedPose(timestampSeconds); LastFailureReason = string.Empty; return true; @@ -974,6 +1274,54 @@ namespace MultiWheelC.StateEstimation InPlaceRotationAngularSpeedThresholdRadiansPerSecond; } + /// 判断轮组反馈是否表明车辆仍在平移或转动。 + private bool IsVehicleMovingFromWheelFeedback() + { + if (!_latestWheelVelocityValid) + { + return false; + } + + var linearSpeedMetersPerSecond = Math.Sqrt( + _latestWheelTwistInBody.VxMetersPerSecond * + _latestWheelTwistInBody.VxMetersPerSecond + + _latestWheelTwistInBody.VyMetersPerSecond * + _latestWheelTwistInBody.VyMetersPerSecond); + + return linearSpeedMetersPerSecond > + InPlaceRotationLinearSpeedThresholdMetersPerSecond || + Math.Abs( + _latestWheelTwistInBody + .OmegaRadiansPerSecond) > + InPlaceRotationAngularSpeedThresholdRadiansPerSecond; + } + + /// 按照Detour源码中l_step到可信锚点的图距离进行质量分级。 + public static DetourLocalizationQuality + ClassifyLocalizationStep(double localizationStep) + { + NumericGuard.EnsureFiniteNonNegative( + localizationStep, + nameof(localizationStep)); + + if (localizationStep >= + LostLocalizationStepLowerBound) + { + return DetourLocalizationQuality.Lost; + } + + if (localizationStep > + DegradedLocalizationStepUpperBound) + { + return DetourLocalizationQuality.Poor; + } + + return localizationStep > + NormalLocalizationStepUpperBound + ? DetourLocalizationQuality.Degraded + : DetourLocalizationQuality.Normal; + } + private void ClearJumpCandidate() { _jumpCandidateActive = false; @@ -1015,7 +1363,7 @@ namespace MultiWheelC.StateEstimation NumericGuard.EnsureFinite( yawDegrees, "DetourTheta"); - NumericGuard.EnsureFinite( + NumericGuard.EnsureFiniteNonNegative( localizationStep, "DetourLStep"); @@ -1459,6 +1807,21 @@ namespace MultiWheelC.StateEstimation return "Uninitialized"; } + if (_localizationStepUnavailable) + { + return "LocalizationUnavailable"; + } + + if (_localizationStepPredictionActive) + { + return "LocalizationStepPrediction"; + } + + if (_cachedFramePredictionActive) + { + return "CachedFramePrediction"; + } + if (_jumpCandidateActive) { var candidateAgeSeconds = @@ -1482,6 +1845,18 @@ namespace MultiWheelC.StateEstimation return "HeadingUnavailable"; } + if (LocalizationQuality == + DetourLocalizationQuality.Poor) + { + return "LocalizationPoor"; + } + + if (LocalizationQuality == + DetourLocalizationQuality.Degraded) + { + return "LocalizationDegraded"; + } + return string.IsNullOrWhiteSpace(LastFailureReason) ? "Healthy" : "Unavailable"; diff --git a/MultiWheelC/StateEstimation/ParkingVehicleStateProviderFactory.cs b/MultiWheelC/StateEstimation/ParkingVehicleStateProviderFactory.cs index 2cb4c2b..d3f1be8 100644 --- a/MultiWheelC/StateEstimation/ParkingVehicleStateProviderFactory.cs +++ b/MultiWheelC/StateEstimation/ParkingVehicleStateProviderFactory.cs @@ -64,7 +64,10 @@ namespace MultiWheelC.StateEstimation config.ParkingDetourMaximumAutomaticFrameShift, AngleMath.DegreesToRadians( config - .ParkingDetourMaximumAutomaticHeadingShiftDegrees)); + .ParkingDetourMaximumAutomaticHeadingShiftDegrees), + config.ParkingDetourMaximumCachedFrameAgeSeconds, + config + .ParkingDetourLocalizationQualityConfirmationFrames); return new WheelFeedbackVehicleStateProvider( detourStateProvider, diff --git a/MultiWheelC/build/Clumsy/CommonUsage.dll b/MultiWheelC/build/Clumsy/CommonUsage.dll index 403ea8b..2c0496a 100644 Binary files a/MultiWheelC/build/Clumsy/CommonUsage.dll and b/MultiWheelC/build/Clumsy/CommonUsage.dll differ diff --git a/MultiWheelC/build/Clumsy/MultiWheelC.dll b/MultiWheelC/build/Clumsy/MultiWheelC.dll index 864ccd2..59c03a9 100644 Binary files a/MultiWheelC/build/Clumsy/MultiWheelC.dll and b/MultiWheelC/build/Clumsy/MultiWheelC.dll differ diff --git a/MultiWheelC/build/Clumsy/MultiWheelC.pdb b/MultiWheelC/build/Clumsy/MultiWheelC.pdb index b41c738..0106fc9 100644 Binary files a/MultiWheelC/build/Clumsy/MultiWheelC.pdb and b/MultiWheelC/build/Clumsy/MultiWheelC.pdb differ diff --git a/detour-information-checklist.md b/detour-information-checklist.md new file mode 100644 index 0000000..d66d36b --- /dev/null +++ b/detour-information-checklist.md @@ -0,0 +1,388 @@ +# Detour 定位接口信息确认（源码答复） + +用途：答复控制程序（`MultiWheelC` / `StateEstimation`）使用定位结果所必需的接口语义。 +依据：本仓库 `DetourCore` 源码（`Location.cs`、`TightCoupler.cs`、`Frame.cs`、`LidarOdometry.cs`、`LidarMap.cs`、`WebAPI.cs`、`G.cs`）。 +仓库内**没有**名为 `getCartLocation()` 的函数；控制侧该调用对应 Detour 的两类对外位姿出口。下文按字段对齐后统一说明。 + +--- + +## 对外位姿出口（先对齐接口） + +控制程序看到的 `x / y / theta / tick / l_step`，来自下面之一（或对它们的封装）。字段名在源码里是 `th`，不是 `theta`。 + +| 出口 | 入口 | 字段 | `tick` 的真实来源 | +|---|---|---|---| +| HTTP `GET /getPos` | `CartLocation.FormatPosition()` | JSON：`x, y, th, l_step, tick`，异常时多 `error` | `tick = CartLocation.st_time`（观测时刻） | +| 共享对象 `DetourPos{shareObjectTag}` | `TightCoupler.CommitLocation` 每次提交后 `Post` | 二进制：`x, y, th, tick, l_step, error` | `tick = DateTime.Now.Ticks`（提交/推送时刻） | + +默认 HTTP 端口：`Configuration.conf.guru.DetourPort`，默认 `4321`。 +`shareObjectTag` 必须与控制侧 Clumsy 的 soTag 一致。 + +两种出口的 `x/y` 都已经过： + +``` +x_out = 内部x / guru.inputScale + guru.biasX +y_out = 内部y / guru.inputScale + guru.biasY +``` + +默认 `inputScale = 1`、`biasX = biasY = 0`，因此默认单位就是内部单位（毫米）。 +`th` / `l_step` 不做缩放。 + +--- + +## 1. `getCartLocation()` 字段定义 + +### 1.1 各字段含义 + +| 字段 | 源码名 | 含义 | +|---|---|---| +| `x` | `CartLocation.x` | 车体原点在**当前地图坐标系**中的 X。已从传感器坐标系经安装外参反变换到车体中心（`TightCoupler.CommitLocation` 里 `ReverseTransform(传感器位姿, 组件安装位姿)`）。 | +| `y` | `CartLocation.y` | 同上，Y。 | +| `theta` | `CartLocation.th` | 车体航向。`0°` 朝向地图 `+X`，逆时针为正（内部用 `cos(th)` / `sin(th)` 画朝向）。 | +| `tick` | 见上表 | **不是**统一的一种时间。HTTP 是观测时间 `st_time`；DObject 是提交瞬间的 `DateTime.Now.Ticks`。控制侧必须先确认自己走的是哪条通道。 | +| `l_step` | `Frame.l_step` | **到已标注（labeled）关键帧的图上步数**，用来表达“离可信锚点有多远”，**不是**优化迭代次数，也不是匹配分数。注释原文：`steps to labeled keyframe`。 | + +### 1.2 单位、正方向、坐标系 + +- **内部单位**：`x`、`y` 为毫米。UI 直接按 `mm` 显示。 +- **对外单位**：默认仍是毫米；只有改了 `guru.inputScale`（例如设为 `1000` 输出米）才会变。 +- **角度单位**：度。 +- **坐标系**：当前加载的 SLAM 地图坐标系。原点由建图/标注关键帧决定，不是 GNSS 或车体启动点。 +- **车体姿态**：输出的是**车体原点**，不是雷达原点。雷达/相机安装位姿在 `layout.components` 的 `x,y,th` 里。 +- **右手平面**：`+X` 为航向 0，`+Y` 为航向 +90°。 + +### 1.3 `theta` 取值范围 + +**不是严格的 `[-180°, 180°]`。** + +`LessMath.normalizeTh` 只在绝对值 ≥ 360 时按 360° 回绕，`(-360, 360)` 内原样返回。因此对外可能看到 `200`、`-270` 这类值。控制程序若假设 `[-180, 180]` 或 `[0, 360)`，应自己归一化。内部比较角度用 `LessMath.thDiff`，会按最短弧处理。 + +### 1.4 一次调用的字段是否同一帧 + +**是。** 一次读取对应同一个 `CartLocation` 对象上的 `x/y/th/l_step/st_time`。 +`getPos` 先用当前 `latest` 判断 Timeout/Unstable，再序列化；若这两步之间恰好发生新提交，状态字和位姿可能差一帧，但单次 JSON 里的数值字段仍来自同一个对象。 + +--- + +## 2. `tick` 的准确含义（优先） + +### 2.1 它是什么时刻 + +分通道： + +**HTTP `/getPos` 的 `tick` = `st_time`** + +- `st_time` 在 `Frame` 上的注释是：传感器数据**到达 Detour 时**由 `G.watch` 打的时间，不是激光头硬件曝光时刻，也不是 HTTP 返回时刻。 +- 激光采集线程会按扫描周期做轻微平滑，并加上 `time_bias_ms`。 +- 里程计把 `frame.st_time` 原样交给 `TightCoupler.CommitLocation`，再写入 `CartLocation.st_time`。 +- 因此：`tick` ≈ **该位姿所对应的那帧观测到达时刻**。SLAM 计算发生在这之后，接口返回更晚。 + +**DObject `DetourPos` 的 `tick` = `DateTime.Now.Ticks`** + +- 这是 **TightCoupler 提交并推送的本地时刻**，100 纳秒为单位，从公元 1 年 1 月 1 日起算（.NET `DateTime.Ticks`）。 +- 它**不是**传感器采集时刻，也**不是** Unix epoch。 +- 受本机本地时钟、时区、校时影响，必要时可能回跳。 + +### 2.2 单位、频率、单调、回绕 + +| 项目 | HTTP `tick`（`st_time`） | DObject `tick`（`DateTime.Now.Ticks`） | +|---|---|---| +| 单位 | 毫秒 | 100 ns（1 ms = 10000） | +| 时钟 | 进程启动时锚定一次 UTC，之后用 `Stopwatch` 单调累加：`ElapsedTicks*1000/Frequency + stMillis` | 本机本地 `DateTime.Now` | +| 更新频率 | 随传感器/融合提交，通常接近雷达帧率（常见 10–20 Hz，取决于设备 `timeBudget`） | 每次 `CommitLocation` 成功推一次 | +| 单调 | 进程内单调递增（不受事后改系统时间影响） | 一般递增；改系统时间或 DST 可能回跳 | +| 回绕 | `long` 毫秒，实际不会回绕 | `long` ticks，实际不会回绕 | +| 进程重启 | 重新锚定当前 UTC，**不会从 0 开始**，但与重启前不保证连续 | 继续跟本机时钟，与进程无关 | + +`G.watch.TimeStampMillis` **不是** Unix 毫秒（1970），而是从公元 1 年起算的 UTC 毫秒，再加上启动后的单调流逝。 + +### 2.3 多车能否用 `tick` 对齐 + +**不能当作已同步的多车时钟。** + +- 各车各自启动 `G.watch`，没有 PTP/NTP 协议，也没有跨车时间服务。 +- HTTP `tick`：若各车系统 UTC 在启动时大致同步，数值会接近“同一套绝对时间”，但仍有启动锚定误差和各车处理延迟，**不能直接当多车位姿对齐的主时钟**。 +- DObject `tick`：本地时间 ticks，跨时区会直接错开，更不适合多车对齐。 +- 进程重启、暂停（`G.paused`）、提交失败都会造成时间空洞。 + +控制侧建议：单车内部用 `tick` 做延迟估计和短时预测；多车对齐应使用外部统一时钟，或只把 Detour `tick` 当相对时间。 + +--- + +## 3. 定位延迟与更新方式 + +### 3.1 输出位姿比真实运动滞后多少 + +源码**没有**标定“官方延迟 xx ms”。能确定的是延迟结构： + +``` +真实运动 → 雷达扫描/到达(st_time) → 里程计配准 → TightCoupler 融合 → 写入 CartLocation.latest → 接口读到缓存 +``` + +经验上由源码阈值约束： + +- 激光一圈通常几十毫秒（`Lidar2DStat.timeBudget`）。 +- TightCoupler 常用窗口 `TCtimeWndSz = 150 ms`，上限 `TCtimeWndLimit = 700 ms`。 +- `LidarOdometry` 把 `reg_ms > 200` 记为 `bad perf`。 +- `/getPos` 若 `now - st_time > 500 ms` 打 `Timeout`。 + +因此正常运行时，**从观测到可读位姿大约是 1 帧雷达周期 + 配准/融合，常见几十到两百毫秒；超过 500 ms 会被 HTTP 接口判超时。** 这是实现上的门槛，不是出厂标定值。 + +### 3.2 `getCartLocation()` 是否返回最近一次缓存 + +**是。** + +- `CartLocation.latest` 是全局缓存。 +- 只有 `TightCoupler.CommitLocation` 成功后才会替换（取 `history` 中 `st_time` 最新的一帧）。 +- `/getPos` 只读缓存，不触发新的 SLAM 计算。 +- 配准失败、`CommitLocation` 返回 `null`（`bad variance` / `bad trace`）、或 `G.paused` 时，缓存停在上一帧。 + +### 3.3 原地自转时频率/延迟是否明显变化 + +**轮询频率不变，有效更新和延迟会变差。** + +- 接口仍按调用方频率读同一缓存。 +- 原地旋转时二维扫描重叠变差，帧间分、相位锁分容易掉，`allowCommit` 更常失败，缓存更容易停住。 +- 配准变慢时单帧 `reg_ms` 上升；失败后 `l_step` 被加大（见第 4 节），看起来像“转的时候定位变钝”。 +- 源码没有“自转专用降频”。 + +控制侧：自转短时预测应假设**有效位姿更新可能变稀、变老**，不要假设 `tick` 仍按雷达周期前进。 + +--- + +## 4. `l_step` 的含义（优先） + +### 4.1 它是什么 + +**定位质量的图距离，不是优化迭代次数，也不是匹配状态枚举。** + +定义：当前位姿/关键帧沿约束图走到**已标注关键帧**还要几步。 + +- `0`：本身就是标注帧（`labeledXY` / `labeledTh`），或刚被标成锚点。 +- `1`：刚和地图/标注帧配准成功（`LidarMap` 成功后会把 `compared.l_step = 1`）。 +- 正常递推：新关键帧 `l_step = 旧关键帧.l_step + lstepInc`。 +- TightCoupler 提交时：有参考关键帧则 `reference.l_step + 1`；没有则 `latest.l_step + 3`。 +- `9999`：未定位、手动设位、或与标注图断开。这是 `Frame` / 初始 `CartLocation` 的默认值。 + +**越小越好、越可信。** +`AIImplementationNote.md` 里“越小越不确定”与源码相反，不要采用。 + +内部用途包括： + +- TightCoupler 边权：`l_step > 10 → 1`，`> 5 → 2`，`> 2 → 3`，否则 `4`；标注帧权 1000。 +- 图优化用 `1/(l_step+0.01)` 当权。 +- `l_step` 大时 `LidarMap` 更容易走全局配准（`forceGRegStep`、`GregThresK * l_step`）。 +- `l_step > 1000` 且孤立的关键帧可被删掉。 + +源码 TODO 也写过：希望将来“去掉 `l_step`，改用误差圆”。当前对外接口仍是这个整数。 + +### 4.2 为什么原地自转会从 2～4 升到几十、上百 + +自转本身不会改公式，但会让 **`lstepInc` 连续被惩罚**，再在切关键帧时一次性加到 `l_step` 上。 + +`LidarOdometry` 里常见加项： + +| 条件 | `lstepInc` 增量 | +|---|---| +| 帧间隔过大 | `+3`～`+20` | +| 时间落后 > 1 s | 可到 `+1000` 量级并重启局部图 | +| 有效点太少 | `+3` | +| 掩膜后点过少 | `+10` | +| 帧间序贯配准失败 | `+5` | +| 局部里程计分过低 | `+15` | +| 历史分偏低 | `+1`～`+15` | +| 局部图重启 / 相位锁差 | `+2` | +| 上一帧能提交、这一帧不能 | `+100` | +| 离开 hard 区域 | 默认再 `+20` | + +原地自转时典型连锁是：旋转导致重叠变差 → 分数掉 → `lstepInc` 累加 → 因“新点变多 / 超时 / restart”切关键帧 → `新 l_step = 旧值 + lstepInc`。 +若此时地图匹配也失败，TightCoupler 按 `latest.l_step + 3` 继续推高。 +所以 2～4 很快可以变成几十，失败恢复前看到上百是符合实现的。 + +匹配一旦重新成功，关键帧会被改回 `l_step = 1`，输出也会掉下来。 + +### 4.3 有没有官方阈值 + +**没有写给控制程序的官方阈值表。** 下面是源码内部实际在用的分档，可作控制侧初值，现场仍要按地图和雷达标定。 + +| `l_step` | 源码含义 | 给控制程序的建议 | +|---|---|---| +| `0` | 标注锚点 | 最可信 | +| `1`～`2` | 刚贴上地图 / 离锚点很近 | **正常，接受** | +| `3`～`5` | TightCoupler 已降权 | 可用，开始警惕跳变 | +| `6`～`10` | 权更低；地图更倾向全局配准 | **退化，降低信任 / 收紧预测** | +| `11`～`98` | 最低权；曾从不稳定区回到 `2` 时会打工作区快照 | **明显退化，不宜做精细控制** | +| `≥ 99` | TightCoupler 允许关键帧被推得更狠 | 按不可靠处理 | +| `≥ 1000` | 孤立帧可删 | 基本断开 | +| `9999` | 未定位 / 手动设位 / 断开 | **不可用，应安全停止或等待重定位** | + +Detour 自己判断“车是否静止可做全局重定位”时用：`l_step > 2` 且 TightCoupler 速度估计很小。这是内部策略，不是对外状态机。 + +--- + +## 5. 坐标跳变与重定位行为（优先） + +### 5.1 重定位、回环、失败恢复后，`x/y/theta` 会不会永久跳变 + +**会。跳变是设计行为，不是毛刺。** + +会改当前输出的情况： + +1. **地图配准成功**：`LidarMap` 把当前关键帧改到匹配位姿，并 `l_step = 1`，再交给 TightCoupler。下一帧 `CartLocation` 跟着新参考走，表现为一次台阶。 +2. **全局重定位**（`/relocalize` 或 UI Relocalize）：定位器进入 `relocalizing`，按全图关键帧搜索（`source = 9`）。第一次更好的匹配会 `TightCoupler.Reset`，位姿被拉到地图上，通常是大幅度永久跳变。 +3. **回环 + `GraphOptimizer`**：关键帧 `x/y/th` 被就地改写。当前参考帧若被挪动，后续融合位姿跟着变。优化有动量平滑，但仍可能出现肉眼可见的台阶。 +4. **手动 `/setLocation`**：直接改 `CartLocation.latest` 并重置融合窗口。 +5. **匹配长期失败后突然恢复**：从里程计漂过的位置一下子贴回地图，跳变幅度等于累计漂移。 + +没有“只在内部跳、对外插值抹平”的保证。控制程序必须自己做跳变连续化或拒绝。 + +### 5.2 跳变后还在原来的地图坐标系吗 + +**同一张已加载地图内：是。** +跳的是车在这张图里的估计，不是换了一套轴。 + +会换坐标系的只有：换图（`/loadMap`）、`Remapper`（GNSS/外参映射）、或手动把车标到另一个锚点。这些不是普通重定位。 + +### 5.3 有没有“正在重定位 / 定位丢失 / 地图坐标调整”标志 + +**定位结果包里没有这些标志。** + +内部有、但**不随 `getPos` / `DetourPos` 下发**： + +| 内部量 | 作用 | 是否对外 | +|---|---|---| +| `Locator.relocalizing` / `relocalized` | 地图层正在/已经全局搜 | 否 | +| `G.IsSettingPosition` | 正在手动设位 | 否（`/getStat` 的 `globalStat` 里能看到） | +| `G.paused` | 定位暂停 | 否（同上，或调 `/pause` `/resume`） | +| `CartLocation.unstable` | 本帧掩膜过狠、场景不稳定 | **仅 HTTP**：`error = "Unstable"`。DObject 当前推送的 `error` 被写成空串 | +| `l_step` 升到很大 / `9999` | 实际的“丢了” | 是，但这是间接指标 | +| 图优化改关键帧 | “地图坐标调整” | 无单独标志 | + +HTTP `/getPos` 仅有的显式错误： + +- `"Timeout"`：`st_time` 已超过 500 ms +- `"Unstable"`：`latest.unstable == true` + +没有 `"Relocalizing"`、`"Lost"`、`"MapAdjusted"`。 +`/getStat` 可拉到各模块 `StatusMember`（雷达间隔、里程计状态、TC 状态等），但不是每帧位姿附属字段，也不适合当硬实时互锁。 + +控制侧应自己构造状态： + +- **疑似丢失**：`error` 为 Timeout/Unstable，或 `l_step ≥ 99`，或 `tick` 长期不涨。 +- **疑似重定位/回环跳变**：相邻两帧 `x/y/th` 突变，同时 `l_step` 突然掉回 1～2。 +- **地图在拧**：跳变较缓、持续多帧，且 `l_step` 并不爆掉。 + +--- + +## 6. 可用的定位质量接口（优先） + +### 6.1 除 `l_step` 外还能拿到什么 + +**位姿包几乎只有 `l_step` + HTTP 的 `error`。** + +| 信息 | 有没有 | 说明 | +|---|---|---| +| 匹配得分 | 对控制程序：无 | 在 `LidarOdometry` / `LidarMap` 内部（`score`、`phaselocker_score`），不下发 | +| 协方差 | 对控制程序：无 | `Frame.errXX/errXY/errYY` 已预留，**未填进 `/getPos` 或 `DetourLocation`** | +| 置信度 | 间接 | 就是 `l_step`；源码 TODO 想换成误差圆，尚未做 | +| 定位状态 | 很弱 | HTTP：`Timeout` / `Unstable`；DObject：`error` 目前恒为空 | +| 错误码 | 无枚举 | 只有上述字符串 | +| 系统状态 | 有，但是慢接口 | `GET /getStat` → 布局/里程计/定位器/TC/GO/`G.paused`/`G.IsSettingPosition` | + +因此控制程序**不能**指望每帧拿到匹配分或协方差。 + +### 6.2 Detour 实际用什么条件接受一帧（可当作官方内部规则） + +源码里“接受并提交”的条件是分层的，没有单独的对外规范文档： + +1. **里程计层**（`LidarOdometry.updateLocation`） + - 分数过低会把 `strength` 压下去，甚至不把 `reference` 挂上。 + - 掩膜过狠 → `unstable = true`。 + - 点太少、序贯/局部配准失败 → 不切健康关键帧，并加大 `lstepInc`。 +2. **融合层**（`TightCoupler.CommitLocation`） + - 传感器处于 `bad variance` / `bad trace` → 直接丢弃，返回 `null`。 + - 丢弃后该源会被禁一段时间（约 1～2 s）。 +3. **地图层**（`LidarMap`） + - `result.score < ScoreThres` 丢弃。 + - 相对已有位姿的 `xy/th` 偏差超过按 `l_step` 放大的门限则丢弃（防止乱跳）。 + - 落到无效区域丢弃。 +4. **HTTP 出口** + - 缓存超过 500 ms → `Timeout`。 + - `unstable` → `Unstable`。 + +### 6.3 建议控制程序如何接受/拒绝一帧 + +源码没有写给 `MultiWheelC` 的官方判据。按上面的内部规则，建议： + +**接受（正常闭环）** + +- HTTP：无 `error`（DObject：至少 `tick` 在前进）。 +- `l_step ≤ 5`。 +- `tick` 新鲜（HTTP：`now - tick < 300 ms` 较稳妥；DObject：换算成 ms 后同样看提交间隔）。 +- 相对上一接受帧：位移/转角不超过本底盘短周期能达到的上限。 + +**降级（短时预测、降低增益、禁止精细对位）** + +- `6 ≤ l_step ≤ 10`，或偶发 Timeout 后立刻恢复。 +- 原地自转期间 `l_step` 爬升但 `tick` 仍在更新。 + +**拒绝并安全停止 / 等待** + +- `error == "Timeout"` 持续，或 `error == "Unstable"`。 +- `l_step ≥ 99`（含 `9999`）。 +- 单帧出现与运动学不符的永久台阶（尤其伴随 `l_step` 从很大突然回到 1）：先当重定位跳变，做连续化或刹停，不要直接当编码器。 + +**不要做的事** + +- 不要用 `l_step` 当 ICP 迭代次数或“正在计算中”。 +- 不要假设 `theta ∈ [-180, 180]`。 +- 不要用 `tick` 做多车时间同步主时钟。 +- 不要假设 DObject 的 `error` 会带 Timeout/Unstable（当前实现是空的）。 + +--- + +## 对 `StateEstimation` 的直接含义 + +清单里最优先的 2 / 4 / 5 / 6，对应控制侧应这样定： + +1. **时间同步** + - 先确认 `getCartLocation` 走 HTTP 还是 `DetourPos`。两条通道的 `tick` **单位和语义都不同**。 + - 单车：用 `tick` 估延迟、做自转短时预测。 + - 多车：另选同步时钟。 + +2. **自转短时预测** + - 自转时有效更新可能变稀，`l_step` 会从个位数爬到几十上百。 + - 这是质量变差，不是接口卡死。预测窗口应随 `tick` 变老、`l_step` 变大而缩短。 + +3. **跳变连续化** + - 重定位、回环、失败恢复都会造成**同地图下的永久台阶**。 + - 没有“正在改图”标志；用位姿差分 + `l_step` 回落来识别。 + +4. **安全停止** + - 硬条件：`Timeout` / `Unstable` / `l_step ≥ 99` / `tick` 停更。 + - `l_step` 建议按 `≤5` 正常、`6–10` 退化、`>10` 不可用于精细控制。 + +--- + +## 源码锚点 + +| 主题 | 位置 | +|---|---| +| 对外 JSON / Timeout / Unstable | `DetourCore/Location.cs`：`ConstructRet`、`FormatPosition` | +| HTTP 路由 | `DetourCore/WebAPI.cs`：`/getPos`、`/setLocation`、`/relocalize`、`/getStat` | +| DObject 推送与 `tick=DateTime.Now.Ticks` | `DetourCore/Algorithms/TightCoupler.cs`：`DetourLocation`、`CommitLocation` | +| `l_step` 定义 | `DetourCore/Types/Frame.cs` | +| 车体中心变换、提交缓存 | `TightCoupler.CommitLocation` | +| 时钟 | `DetourCore/G.cs`：`DetourWatch.TimeStampMillis` | +| 角度回绕 | `DetourCore/LessMath.cs`：`normalizeTh` | +| 自转时 `l_step` 被加大 | `DetourCore/Algorithms/LidarOdometry.cs`（`lstepInc`） | +| 匹配成功后的位姿跳变 | `DetourCore/LocatorTypes/LidarMap.cs`（loop / relocalize） | +| 回环拧图 | `DetourCore/Algorithms/GraphOptimizer.cs` | +| 全局重定位入口 | `DetourCore/DetourLib.cs`：`Relocalize()` | +| 单位缩放 | `DetourCore/Configuration.cs`：`GuruOptions.inputScale / biasX / biasY` | + +--- + +## 修订记录 + +- 2026-08-24：按当前 Detour 源码逐条答复原《Detour 信息确认清单》。 diff --git a/docs/architecture.md b/docs/architecture.md index 3e20c9e..b3ad42a 100644 --- a/docs/architecture.md +++ b/docs/architecture.md @@ -89,19 +89,33 @@ MovementTest / MotionPlanExecutor `TrajectoryTrackingMovement` 默认从 `PilotDefinition.Conf` 读取车辆级参数，同时保留少量动作级覆盖字段；横向控制器可通过 `LateralControllerFactory` 替换，纵向控制器当前固定创建为 `PidLongitudinalController`。 -## 车队纯计算调用链 +## 车队组件与尚未贯通的执行链 ```text -FleetState - → FleetController - → PathTrackingCore.Compute() - → GcpKinematics.ToBodyTwist() - → FleetMotionCommand（参考点为车队原点） - → FleetKinematics.Decompose(FleetLayout, command) - → FleetMemberCommand[]（各成员真实车体系Twist） +夹紧且静止时的成员世界位姿快照 + → FleetLayoutCapture.Capture() + → 初始FleetPoseInWorld + 不可变FleetLayout + +成员状态样本 FleetMemberStateSample[] + → FleetStateEstimator.Estimate() + → FleetState + FleetMemberLayoutError[] + → FleetCoordinator.ExecuteCycle() + → FleetController（虚拟中心轨迹闭环和固定β_fleet） + → 相对布局误差统一速度缩放 + → FleetKinematics.Decompose() + → FleetMemberCommandCorrector + → FleetMemberCommand[] + +车队动作主要滚动方向β_fleet + → FleetPreparationCoordinator（换算每车β_i并等待全员Ready） + → FleetMemberAgent（本车停车准备、舵轮到位、激活、Execute/Stop） ``` -这条链已经覆盖虚拟中心轨迹闭环和确定性刚体速度分解，但尚未接入车队状态估计、通信/时间对齐、相对布局纠偏、共同能力限幅、安全降级和成员底盘发送。 +`FleetLayoutCapture` 只负责固定布局的几何计算：车队原点X/Y取成员车体中心的算术平均，车队Yaw取主车Yaw，再把各成员世界位姿反变换为 `VehicleLayout.PoseInFleet`。它不读取通信或Detour，也不负责静止/夹紧确认、时间对齐和布局激活。 + +上述类目前是可以独立构造和测试的组件，并没有正式的车队任务运行入口把两条链串起来。缺少的外层需要负责布局原子激活、成员状态实际采集与时间对齐、准备/激活状态机、每周期协调、安全门控、成员命令分发、任务完成与取消。`FleetCoordinator` 生成零速或故障结果不等于实车已经停车；只有运行层把结果送到各车 `FleetMemberAgent.Execute()` 或 `Stop()` 后才会影响底盘。 + +实际部署为每车独立电脑，因此主车还需要状态/命令通信，从车需要本地命令超时看门狗。无线串口初始化可以后接，但消息契约、任务号、心跳/有效期和本地失联停车语义必须在运行层接入前明确。 ## 状态数据流 diff --git a/docs/decisions.md b/docs/decisions.md index 01b3d00..315db7c 100644 --- a/docs/decisions.md +++ b/docs/decisions.md @@ -97,6 +97,8 @@ - `MultiWheelC.Tests/FleetKinematicsTests.cs` 已覆盖整体平移、绕车队中心旋转、绕成员车旋转和停止四个数学场景。 - `MultiWheelC/Fleet/FleetState.cs` 保存车队虚拟中心的世界位姿、同一点的世界系/车队系速度、状态采样时间和速度有效性；`FleetPoseInWorld.Yaw` 定义车队 `+X` 方向，两份速度只是同一物理速度的不同坐标表达。 - `FleetState.SampleTimestampSeconds` 采用主车/协调器生成聚合快照时的本机单调时间。成员本机时钟和Detour `tick` 的同步属于未来接收/状态估计层职责，不阻塞使用人工构造 `FleetState` 开发纯车队控制器。 +- 当前无法直接获得被搬运车辆中心，因此 `FleetLayoutCapture.Capture()` 已确定用成员车体中心X/Y的算术平均定义车队原点，用指定主车Yaw定义车队朝向；主车缺失时拒绝采集，不做隐式降级。 +- 布局采集只返回采集时刻的 `FleetPoseInWorld` 与不可变 `FleetLayout`，不读取Detour/通信，也不负责夹紧、静止、时间对齐或原子激活。6个布局采集数学场景已通过。 ### 14. 原地自转保留绝对航向与轮组相对角两种反馈模式 @@ -109,22 +111,25 @@ - `PathTrackingContext` 只携带受控刚体的车体系速度、速度有效性、轨迹投影和周期控制量，不依赖 `VehicleState` 或 `FleetState`。 - `PathTrackingCore` 集中实现投影连续性、保护/终点策略、曲率预瞄、横纵向控制和GCP分配；`ParkingGeometricController` 与 `FleetController` 分别组合该核心，负责各自的状态适配和输出边界，不通过继承复制控制流程。 -- `FleetController` 第一版只闭环车队虚拟中心，并将GCP结果转换成车队原点处的 `FleetMotionCommand`；成员速度继续由 `FleetKinematics.Decompose()` 确定性分解。 +- `FleetController` 第一版只闭环车队虚拟中心，并支持固定 `β_fleet`：实际纵向速度沿β投影，GCP结果从运动坐标系旋转到车队坐标系后形成车队原点处的 `FleetMotionCommand`；成员速度继续由 `FleetKinematics.Decompose()` 确定性分解。 +- `FleetStateEstimator` 已根据固定 `FleetLayout` 和时间对齐目标时刻反算、融合车队中心，并输出每车 `FleetMemberLayoutError`；成员样本的实际采集和跨电脑时间处理仍属于外层接收链。 +- `FleetCoordinator` 已串联状态估计、虚拟中心控制、布局误差警告区间内的统一速度缩放、刚体分解和 `FleetMemberCommandCorrector` 小范围纠偏；越过停止阈值时返回故障和零速成员命令，但尚无正式运行层把该结果送到实车。 +- `FleetPreparationCoordinator` 已按 `beta_i = beta_fleet - theta_i` 生成成员准备目标，并通过任务号和全员Ready形成统一激活屏障；`FleetMemberAgent` 已封装本车停车准备、舵轮到位确认、激活、命令校验和底盘执行。两者尚未接入同一个车队任务生命周期，也未接入跨电脑通信。 - 控制算法的可替换性继续由 `ILateralController`、`ILongitudinalController` 组合注入；状态源、通信、底盘发送和成员协调不进入纯核心。 -- `MultiWheelC.Tests` 已覆盖8个Stanley前进/倒车符号场景、6个车队控制周期场景和4个刚体分解场景；统一构建与打包通过。 +- `MultiWheelC.Tests` 已覆盖8个Stanley前进/倒车符号场景、8个车队控制周期场景、6个布局采集场景和4个刚体分解场景；统一构建与打包通过。 ## 已经确认但尚未实施 - 路线顺序：先完成单车闭环和停车功能验证，再正式实施多车通信、编队和协同控制。来源：`README.md`。 -- 当前已完成车队布局模型、虚拟中心状态模型、虚拟中心轨迹控制和纯运动学分解，尚未形成可运行的多车链路：布局采集/激活、车队状态估计、通信、成员纠偏和安全协调仍未接入业务流程。 -- 布局生命周期区分夹紧前后的语义：夹紧前的预设布局只用于引导车辆就位；车辆夹紧且静止后，应同步读取成员位姿，选择车队参考系并创建新的不可变 `FleetLayout`，再由上层协调器原子激活。共同搬运期间的相对位姿变化属于状态误差，不能通过修改 `FleetLayout` 吸收；松开车辆后清除激活布局。具体采集和激活接口尚未实施。 +- 当前多车的布局、状态估计、中心控制、刚体分解、成员纠偏、β准备屏障和本车执行代理均已有代码组件，但尚未形成可运行的多车链路。仍缺少车队任务运行入口、布局原子激活、成员状态实际采集/时间对齐、跨电脑通信、命令分发和可实际触发停车的安全执行链。 +- 布局生命周期区分夹紧前后的语义：夹紧前的预设布局只用于引导车辆就位；车辆夹紧且静止后，应同步取得同一世界坐标系下的成员位姿，调用 `FleetLayoutCapture` 创建新的不可变布局，再由上层协调器原子激活。共同搬运期间的相对位姿变化属于状态误差，不能通过修改 `FleetLayout` 吸收；松开车辆后清除激活布局。实际数据采集和激活接口尚未实施。 - 多车共同搬运不能只闭环车队中心：整体位姿误差与成员相对布局误差必须分开估计和约束，否则成员误差可能相互抵消而使平均中心看似正确。 - 计划采用分层职责：车队控制器产生参考点 `FleetTwist`，分配层依据成员 `VehicleLayout` 计算每车真实车体系 `BodyTwist`，单车层继续负责β变换、GCP和本车四轮解算。 - 第一版采用确定性的虚拟刚体速度分配，不先引入QP/HQP：若成员在车队系中的固定布局为位置 `(x_i,y_i)`、朝向 `theta_i`，则成员中心在车队系中的速度为 `(Vx-omega*y_i, Vy+omega*x_i, omega)`，再通过 `R(-theta_i)` 转到本车体系后交给 `SendBodyTwist()`。QP/HQP只在需要同时调整车队参考速度、处理成员能力差异、松弛约束或严格任务优先级时再引入。 -- “按状态最差车辆协调速度”采用车队共同可行性和统一缩放表达：普通能力受限时由所有成员约束确定共同速度比例；任一成员报警、通信超时、状态不可用或刚体误差越界时整队停车。时间戳、心跳、命令有效期和本地超时停车属于第一版安全契约，延迟预测补偿可以后续增加。 +- “按状态最差车辆协调速度”第一步已经对成员相对布局误差实现警告阈值至停止阈值之间的统一速度缩放。成员报警、通信超时、夹紧异常、状态持续不可用等整队停车条件仍需由真实运行层统一执行；时间戳、心跳、命令有效期和从车本地超时停车属于第一版安全契约，延迟预测补偿可以后续增加。 - 虚拟车队使用固定在车队坐标系中的对称前后GCP，把横向控制结果转换成车队原点 `FleetTwist`；这些点不是物理轮轴，也不直接参与单车四轮解算。横向控制器与GCP到Twist转换必须使用同一控制点半径；具体车辆级配置值和实车验证仍待完成。 - 每辆成员车都应作为反馈来源，但反馈职责必须分层：成员Detour位姿用于融合车队整体位姿和检查相对布局，单车轮速/舵角用于确认命令执行偏差，电机电流、扭矩或力传感信息用于负载与内力监控。相对位姿接近目标并不能证明没有内力，因此不能只依靠刚性连接或位姿误差判断负载均衡。 -- 第一版不把每车β作为复杂优化变量：车队动作先明确主要滚动方向 `beta_fleet`（如正常0°、斜行45°、横移90°），成员按固定布局朝向换算 `beta_i = beta_fleet - theta_i`，并利用180°等效和有符号速度选择方便的本地表示。β是单车执行坐标系，不改变刚体分配得到的真实车体系 `BodyTwist`，也不会让各车命令数值相同；仅允许在全车停车时准备和激活，全部成员舵轮到位后通过同步屏障释放非零命令。只有出现复杂布局、整段方向变化、限位余量或频繁反号问题时，才增加轨迹级β候选搜索。 +- 第一版不把每车β作为复杂优化变量：`FleetController` 已支持固定的车队主要滚动方向 `beta_fleet`（如正常0°、斜行45°、横移90°）及其控制坐标转换；`FleetPreparationCoordinator` 已按布局换算成员 `beta_i`，并采用180°等效轴保持在方便的本地表示范围。β是单车执行坐标系，不改变刚体分配得到的真实车体系 `BodyTwist`，也不会让各车命令数值相同。当前缺的是把停车预对齐、全员Ready和统一激活接入真实运行链；只有出现复杂布局、整段方向变化、限位余量或频繁反号问题时，才增加轨迹级β候选搜索。 - 旧版参考项目采用固定双车布局：各车由 `carWorld ∘ layout⁻¹` 反推车队中心，再对位置和圆周航向求平均；路径控制器以该虚拟中心跟踪轨迹。同时它可按 `fleetTarget ∘ layout_i` 生成每车理想位姿，并叠加Detour布局纠偏和邻车两腿检测纠偏，因此并非只控制平均中心。来源：`原版停车机器人/parkingrobot/ClumsyPilot/PilotDefinition.cs`、`ChassisController.cs`。 - 旧版 `SetOriginBias(layoutX, layoutY, layoutTh)` 是把各车真实轮子统一表达在车队虚拟坐标系中，属于固定编队布局变换。旧版联动显式区分常规、蟹行和绕车队中心旋转三类模式；蟹行角可由动作或遥控给出任意值（`FleetCrabWalk` 默认45°），并在运动前以零速度对齐舵轮、运行时使用180°等效和轮速反号，但没有根据整段轨迹和每车约束自主求解β的统一规划过程。给定简单蟹行动作时，它与新版固定β可能产生相同的实际轮子姿态和车辆运动。 - 旧版自动 `FleetCurveWalk`、`FleetCrabWalk` 会先以零速度下发初始GCP角，等待成员新鲜、布局正确、命令可行、舵轮到位和从车应用新序列后才开始运动；原地旋转通过 `RotateWheelsAligned` 和 `FleetMotionReleased` 做整队释放。普通手动入口仍有 `SendMotion` 本车舵轮未对齐时速度置零的门控，但不保证与自动动作相同的车队级同步屏障。 diff --git a/docs/interfaces.md b/docs/interfaces.md index 44e9bf8..8157797 100644 --- a/docs/interfaces.md +++ b/docs/interfaces.md @@ -36,6 +36,17 @@ `FleetState.SampleTimestampSeconds` 表示主车/协调器生成该车队状态快照时的本机单调时间，不是Detour全局时间。各成员电脑的本机时钟和Detour `tick` 当前不能直接互相比较；未来接收层应另行保存来源时间并完成新鲜度和时间对齐。 +### `FleetLayoutCapture` + +文件：`MultiWheelC/Fleet/FleetLayoutCapture.cs`。 + +- `FleetMemberPose`：用于布局采集的车号和成员世界位姿输入。 +- `FleetLayoutCaptureResult`：同时返回采集时刻的 `FleetPoseInWorld` 和固定的 `FleetLayout`；车队当前世界位姿不存入布局。 +- `Capture(members, leaderVehicleId)`：车队原点X/Y取成员车体中心的算术平均，Yaw取主车Yaw，并通过 `inverse(FleetPoseInWorld) ∘ VehiclePoseInWorld` 得到每车 `PoseInFleet`。 +- 空成员、非法或重复车号、非有限位姿、主车不存在均拒绝采集；主车缺失时不使用其他车辆降级代替。 + +该接口是纯几何计算，调用者必须在外部保证成员位姿处于同一世界坐标系，并完成夹紧、静止、数据新鲜度和时间对齐检查；布局的存储与原子激活也不属于该类。 + ## 轨迹契约 文件：`MultiWheelC/Trajectory/`。 @@ -133,7 +144,7 @@ Vβ = cos(β)·Vx_body + sin(β)·Vy_body 状态暂时不可用时，外层调用 `PauseForUnavailableState()` 保留当前轨迹与投影连续性；明确失败或取消分别使用 `Fail()`、`Cancel()`。 -`ParkingGeometricController` 是单车适配层，负责读取 `IVehicleStateProvider` 并通过 `GcpCommandExecutor` 发送实体底盘命令。`FleetController` 是车队虚拟中心适配层，使用 `FleetState.FleetPoseInWorld` 和 `TwistAtFleetOriginInFleet` 调用同一核心，再将GCP结果转换为车队原点处的 `FleetMotionCommand`。 +`ParkingGeometricController` 是单车适配层，负责读取 `IVehicleStateProvider` 并通过 `GcpCommandExecutor` 发送实体底盘命令。`FleetController` 是车队虚拟中心适配层，使用 `FleetState.FleetPoseInWorld` 和 `TwistAtFleetOriginInFleet` 调用同一核心。构造参数 `motionDirectionInFleetRadians` 定义固定的 `β_fleet`：核心按该方向投影实际纵向速度，GCP结果先解释为运动坐标系Twist，再通过 `R(β_fleet)` 转换成车队坐标系下、车队原点处的 `FleetMotionCommand`。`β_fleet=0` 保持常规前向语义；当前控制器不负责运行中切换β。 当前速度闭环和执行边界的语义并不完全相同：纵向PID与Stanley实际速度分母使用 `Vβ`（Stanley也可按配置改用参考速度），而 `GcpMotionCommand.SpeedMetersPerSecond` 和旧版 `SendMotion.speed` 表示带行驶方向符号的车体中心平移速度模长。正常圆弧理想跟踪时 `Vy=0`，两者相等；只有横向误差共同转角产生非零 `Vy` 时，模长与 `Vβ` 才相差余弦因子。当前最大命令速度仍限制最终发送的模长。 @@ -198,6 +209,7 @@ Vrear = (Vx, Vy - ωR) ## M/C IO与MCU边界 - C层 `PilotDefinition` 和M层 `DiverCartDefinition` 使用 `[AsUpperIO]`/`[AsLowerIO]` 对齐夹臂命令、驱动使能、位置反馈和车号等字段。 +- M层 `WheelSpeedDiagnosticLogger` 每次记录生成 `_can.csv` 和 `_snapshot.csv`：前者按CAN回调时刻保存八电机速度/位置与四舵角事件，后者按最多50Hz保存目标/实际舵角、目标角速度、PID/前馈/合成差速、限幅前后电机命令、速度/位置/电流反馈及各事件的本机单调时间、序号和数据年龄。MATLAB辨识时可使用 `TargetTh` 作为参考、`TotalDiff` 或 `Sent*` 作为执行输入、`ActualTh` 作为输出；左右轮公共/差速通道由方向统一后的左右命令和反馈在分析侧组合。 - `DiverCartDefinition.CommunicationInit()` 默认通过Windows `COM4`、`1,000,000 baud` 打开MCU桥；`MCUPort` 是可配置初始化参数。 - MCU内部配置为逻辑端口0：CAN `500,000 bit/s`；逻辑端口1～3：串口 `9,600 bit/s`。`MCURoutine.BatteryPortIndex = 3` 指MCU桥逻辑端口，不等同于Windows `COM3`。 - CAN命令/反馈范围集中在 `MCURoutine.cs`：驱动命令 `0x201～0x20A`，速度/位置反馈 `0x281～0x28A`，状态 `0x181～0x18A`，舵角 `0x18B～0x18E`，远程帧 `0x701～0x70A`。 diff --git a/docs/progress.md b/docs/progress.md index 73fd53f..b697734 100644 --- a/docs/progress.md +++ b/docs/progress.md @@ -1,6 +1,6 @@ # 当前进展 -更新日期：2026-08-21。这里只保存当前状态，不作为完整开发历史。 +更新日期：2026-08-24。这里只保存当前状态，不作为完整开发历史。 ## 已完成/已接入 @@ -21,7 +21,7 @@ - 停车控制参数集中到 `Configuration/PilotConfig.ParkingControl.cs`。 - `PathTrackingContext` 已解除对单车 `VehicleState` 的依赖；`PathTrackingCore` 统一单车与车队的投影、保护/终点策略、横纵向控制和GCP分配，`ParkingGeometricController` 保留单车状态读取与实体命令执行职责。 - `MultiWheelC.Tests` 已提供不依赖实车的Stanley前进/倒车横向符号回归，8个场景通过；该项目不进入正式解决方案和打包脚本。 -- `Shared/Fleet` 已加入不可变 `FleetLayout`、成员/车队命令模型和确定性 `FleetKinematics`；`MultiWheelC/Fleet` 已加入虚拟中心 `FleetState` 和复用 `PathTrackingCore` 的纯计算 `FleetController`。6个车队控制周期场景和4个刚体分解场景通过。 +- `Shared/Fleet` 已加入不可变 `FleetLayout`、成员/车队命令模型和确定性 `FleetKinematics`；`MultiWheelC/Fleet` 已具备 `FleetLayoutCapture`、`FleetStateEstimator`、固定 `β_fleet` 的 `FleetController`、`FleetMemberCommandCorrector`、`FleetCoordinator`、`FleetPreparationCoordinator` 和 `FleetMemberAgent`。这些组件覆盖布局采集、车队中心/成员误差估计、中心轨迹闭环、统一速度缩放、刚体分解、小范围成员纠偏、成员β准备屏障和本车命令执行边界，但尚未由正式车队任务运行入口贯通。 - 本次建立工作区/项目AGENTS导航和 `docs/` 按需知识库。 以上表示代码入口存在，不表示全部实车工况已经验收。 @@ -32,14 +32,15 @@ - 暂时冻结普通运动跳变阈值和30°/s、40°/s²自转参数，等待Detour接口语义后再决定是否实施自转结束后的条件化仅位置连续化或调整航向恢复策略。 - Detour对接最小问题已整理到 `docs/detour-information-checklist.md`，不要求取得源码。 - 验证非零β运动系，当前已有45°蟹行直线入口；曲线蟹行仍需设计实验。 -- 多车共同搬运已完成布局模型、虚拟中心状态、虚拟中心轨迹闭环和纯刚体速度分解；下一步处理夹紧后布局采集/原子激活、车队状态估计、通信和安全协调，并把成员命令接入各车 `SendBodyTwist()`。QP/HQP不作为第一版前置条件。 +- 多车共同搬运的核心算法和本车执行组件已基本具备，当前重点不再是增加孤立算法文件，而是建立完整车队任务运行链：布局激活→β准备→全员Ready→统一激活→周期状态估计与协调→安全门控→成员命令分发→完成/故障/取消。没有这层调用者时，`FleetCoordinator` 的零速输出或任何独立安全判定都不会自动让实车停车。QP/HQP不作为第一版前置条件。 - β在多车中定位为单车执行坐标系而非车队核心优化量：常规、斜行和横移动作先确定车队主要滚动方向，各车按布局朝向换算本地β，停车预对齐并经车队同步屏障统一释放。轨迹级β搜索只作为机械余量或复杂方向变化下的后续增强。 ## 阻塞/待确认 - Clumsy/Medulla正式宿主、插件部署和配置持久化说明未纳入仓库。 - 完整停车业务流程和验收指标未确认。 -- 多车通信协议、统一时间轴、车队参考点选取、夹紧后布局采集方法、刚体误差阈值和故障降级策略未确认；已确认运行布局应在夹紧后生成不可变快照并由上层整体激活。 +- 多车通信协议、统一时间轴、成员状态实际采集方式、布局原子激活接口、刚体误差实车阈值、夹紧/报警状态来源和故障降级策略未确认；车队参考点和纯几何采集方法已经确定，运行布局应在夹紧后生成不可变快照并由上层整体激活。 +- 实际部署为每车独立电脑。主车需要成员状态接收和命令分发，从车必须具备本地命令超时停车；仅依赖主车广播停止不能覆盖通信中断场景。 - 最新实车实验数据对控制周期与舵轮滞后的结论尚未沉淀为可复核结果。 - Detour `l_step` 精确定义、显式重定位/坐标重置状态和部署端MDCS恢复策略待确认；当前不能把 `l_step<4` 当作唯一有效性条件。 - Detour `getCartLocation()` 的位姿坐标系、`tick` 采样/解算/发布语义、是否已有独立连续里程计或定位状态接口，以及部署版本和实际里程计/SLAM配置待确认。 @@ -52,6 +53,7 @@ 2. 在Detour信息返回前不继续全局放宽状态估计边界，也不提高自转角速度/角加速度；现有偶发定位不可用保持安全停车。 3. 为轮组里程计自转CSV补充请求角度、内部积分角、完成/超时/手动停止原因，按固定目标角重复验证误差和重复性；绝对航向任务继续保留Detour模式，不用轮组积分冒充世界航向。 4. 为轨迹插值/投影、坐标变换、状态跳变候选和终点策略继续补充不依赖宿主的数学回归测试。 -5. 设计车队建立生命周期：预设布局引导就位，夹紧静止后同步采集成员位姿，创建并原子激活新的 `FleetLayout`，松开后清除；车队参考系和采样同步方法需先明确。 -6. 建立车队状态与执行边界：将时间对齐后的成员状态聚合为 `FleetState`，把 `FleetKinematics` 的成员命令接入各车 `SendBodyTwist()`；通信、成员纠偏和底盘发送不塞回 `PathTrackingCore`，也不直接启用 `#if false` 旧多车代码。 -7. 在接入通信前定义车队状态时间对齐、心跳/命令有效期、共同速度缩放、成员故障整队停车和舵轮准备同步屏障。 +5. 先建立车队任务运行入口，把布局原子激活、`FleetPreparationCoordinator`、`FleetCoordinator` 和成员执行结果串成明确的生命周期；不要再增加没有运行消费者的独立安全判定器。 +6. 定义最小通信数据契约：成员状态、成员准备/执行状态、任务号、主车命令、心跳和命令有效期。无线串口初始化可后接，但运行层不能假设主车能直接调用另一台电脑上的 `FleetMemberAgent`。 +7. 接入成员状态实际来源和主车接收时间轴，形成 `FleetMemberStateSample[]`；再把 `FleetCoordinator` 输出按车号分发到各车 `FleetMemberAgent.Execute()`。 +8. 在真实执行链内实现安全门控：可恢复状态下持续下发零速并保留任务；硬故障时锁存任务失败、整队停止；每辆从车独立实现命令超时停车。随后补充无通信模拟的端到端测试，再进行低速双车实车验证。 diff --git a/output/C/CommonUsage.dll b/output/C/CommonUsage.dll index 403ea8b..2c0496a 100644 Binary files a/output/C/CommonUsage.dll and b/output/C/CommonUsage.dll differ diff --git a/output/C/MultiWheelC.dll b/output/C/MultiWheelC.dll index 864ccd2..59c03a9 100644 Binary files a/output/C/MultiWheelC.dll and b/output/C/MultiWheelC.dll differ diff --git a/output/C/MultiWheelC.pdb b/output/C/MultiWheelC.pdb index b41c738..0106fc9 100644 Binary files a/output/C/MultiWheelC.pdb and b/output/C/MultiWheelC.pdb differ diff --git a/output/M/CommonUsage.dll b/output/M/CommonUsage.dll index 403ea8b..2c0496a 100644 Binary files a/output/M/CommonUsage.dll and b/output/M/CommonUsage.dll differ diff --git a/output/M/MedullaAdapter.dll b/output/M/MedullaAdapter.dll index 3eb4a89..13f09f7 100644 Binary files a/output/M/MedullaAdapter.dll and b/output/M/MedullaAdapter.dll differ diff --git a/output/M/MedullaAdapter.pdb b/output/M/MedullaAdapter.pdb index 2f446aa..1f8a518 100644 Binary files a/output/M/MedullaAdapter.pdb and b/output/M/MedullaAdapter.pdb differ diff --git a/ref/CommonUsage.dll b/ref/CommonUsage.dll index 403ea8b..2c0496a 100644 Binary files a/ref/CommonUsage.dll and b/ref/CommonUsage.dll differ