diff --git a/MedullaAdapter/DiverCartDefinition.cs b/MedullaAdapter/DiverCartDefinition.cs index 55b7556..02cc8f0 100644 --- a/MedullaAdapter/DiverCartDefinition.cs +++ b/MedullaAdapter/DiverCartDefinition.cs @@ -16,6 +16,14 @@ namespace MedullaAdapter [UseManualController(manualController = typeof(Remote))] public class DiverCartDefinition : MultiWheelCartDefinition { + public DiverCartDefinition() + { + // 实车配置可覆盖这些回退值；四个舵轮在MotorRoutine中统一读取它们。 + DiffSteerKp = 0.0042f; + DiffSteerKi = 0f; + DiffSteerKd = 0f; + } + #region 基本成员 public MCUSerialBridge Bridge; @@ -65,16 +73,37 @@ namespace MedullaAdapter #region 初始参数 - [AsInitParam(desc = "差速转舵目标角速度前馈增益")] - // public float DiffSteerRateFeedforwardGain = 0.9f; + // 旧参数仅用于兼容已有配置和历史日志，新模型逆前馈不读取它。 + [AsInitParam(desc = "旧差速转舵目标角速度前馈增益（已停用）")] public float DiffSteerRateFeedforwardGain = 0f; - [AsInitParam(desc = "差速舵轮左右轮间距，单位mm")] + [AsInitParam(desc = "旧前馈差速舵轮左右轮间距，单位mm（已停用）")] public float DiffSteerWheelDistanceMillimeters = 85f; [AsInitParam(desc = "差速转舵前馈最大速度，单位m/s")] public float DiffSteerRateFeedforwardMaximumSpeed = 0.03f; + [AsInitParam(desc = "启用差速舵轮到位迟滞")] + public bool EnableDiffSteerSettlingHysteresis = false; + + [AsInitParam(desc = "差速舵轮停止调整误差，单位deg")] + public float DiffSteerStopErrorDegrees = 0.5f; + + [AsInitParam(desc = "差速舵轮重新启动误差，单位deg")] + public float DiffSteerRestartErrorDegrees = 0.8f; + + [AsInitParam(desc = "差速舵轮到位确认周期数")] + public int DiffSteerSettlingCycles = 3; + + [AsInitParam(desc = "启用差速转舵模型逆前馈")] + public bool EnableDiffSteerInverseFeedforward = false; + + [AsInitParam(desc = "静止舵轮模型增益")] + public float DiffSteerPlantGain = 687.06f; + + [AsInitParam(desc = "模型逆前馈滤波时间常数，单位s")] + public float DiffSteerInverseFeedforwardTimeConstantSeconds = 0.15f; + [AsInitParam(desc = "MCU端口号")] public string MCUPort = "COM4"; [AsInitParam(desc = "遥控器速度上限")] public float TransmitterSpeedUpperLimit = 1.0f; [AsInitParam(desc = "遥控器速度下限")] public float TransmitterSpeedLowerLimit = 0.0f; @@ -119,6 +148,18 @@ namespace MedullaAdapter [IOObjectMonitor(desc = "左后舵轮转向合成差速输出")] public float DiffSteerTotalOutputLeftRear; [IOObjectMonitor(desc = "右前舵轮转向合成差速输出")] public float DiffSteerTotalOutputRightFront; [IOObjectMonitor(desc = "右后舵轮转向合成差速输出")] public float DiffSteerTotalOutputRightRear; + [IOObjectMonitor(desc = "左前舵轮进入停止调整区")] public bool DiffSteerInStopZoneLeftFront; + [IOObjectMonitor(desc = "左后舵轮进入停止调整区")] public bool DiffSteerInStopZoneLeftRear; + [IOObjectMonitor(desc = "右前舵轮进入停止调整区")] public bool DiffSteerInStopZoneRightFront; + [IOObjectMonitor(desc = "右后舵轮进入停止调整区")] public bool DiffSteerInStopZoneRightRear; + [IOObjectMonitor(desc = "左前舵轮连续到位周期数")] public int DiffSteerSettlingCountLeftFront; + [IOObjectMonitor(desc = "左后舵轮连续到位周期数")] public int DiffSteerSettlingCountLeftRear; + [IOObjectMonitor(desc = "右前舵轮连续到位周期数")] public int DiffSteerSettlingCountRightFront; + [IOObjectMonitor(desc = "右后舵轮连续到位周期数")] public int DiffSteerSettlingCountRightRear; + [IOObjectMonitor(desc = "左前舵轮已到位")] public bool DiffSteerSettledLeftFront; + [IOObjectMonitor(desc = "左后舵轮已到位")] public bool DiffSteerSettledLeftRear; + [IOObjectMonitor(desc = "右前舵轮已到位")] public bool DiffSteerSettledRightFront; + [IOObjectMonitor(desc = "右后舵轮已到位")] public bool DiffSteerSettledRightRear; [IOObjectMonitor(desc = "灯光模式")] public int LightMode = 0; [IOObjectMonitor(desc = "实体遥控器当前速度倍率")] public float TransmitterSpeed = 0.3f; [IOObjectMonitor(desc = "轮速诊断记录已启用")] @@ -154,6 +195,14 @@ namespace MedullaAdapter internal float DiffSteerTargetRateRightFrontDegreesPerSecond; internal float DiffSteerTargetRateRightRearDegreesPerSecond; internal float DiffSteerFeedforwardDeltaTimeMilliseconds; + internal float DiffSteerInverseFeedforwardRawLeftFront; + internal float DiffSteerInverseFeedforwardRawLeftRear; + internal float DiffSteerInverseFeedforwardRawRightFront; + internal float DiffSteerInverseFeedforwardRawRightRear; + internal float DiffSteerInverseFeedforwardFilteredLeftFront; + internal float DiffSteerInverseFeedforwardFilteredLeftRear; + internal float DiffSteerInverseFeedforwardFilteredRightFront; + internal float DiffSteerInverseFeedforwardFilteredRightRear; internal bool DiffSteerFeedforwardLimitedLeftFront; internal bool DiffSteerFeedforwardLimitedLeftRear; internal bool DiffSteerFeedforwardLimitedRightFront; diff --git a/MedullaAdapter/MotorRoutine.cs b/MedullaAdapter/MotorRoutine.cs index e210ffb..cd150f5 100644 --- a/MedullaAdapter/MotorRoutine.cs +++ b/MedullaAdapter/MotorRoutine.cs @@ -12,14 +12,28 @@ namespace MedullaAdapter public class MotorRoutine : LadderLogic { private const double MaximumFeedforwardIntervalSeconds = 0.2; + + private sealed class DiffSteerWheelControlState + { + public float PreviousTargetAngleDegrees; + public double FilteredInverseFeedforwardMetersPerSecond; + public bool IsInStopZone; + public int SettlingCycleCount; + public bool IsSettled; + } + private bool _wasTransmitterControlling; private DateTime _lastMoveTime = DateTime.Now; private bool _diffSteerFeedforwardInitialized; private long _lastDiffSteerFeedforwardTimestamp; - private float _previousThLeftFront; - private float _previousThLeftRear; - private float _previousThRightFront; - private float _previousThRightRear; + private readonly DiffSteerWheelControlState _leftFrontSteerState = + new DiffSteerWheelControlState(); + private readonly DiffSteerWheelControlState _leftRearSteerState = + new DiffSteerWheelControlState(); + private readonly DiffSteerWheelControlState _rightFrontSteerState = + new DiffSteerWheelControlState(); + private readonly DiffSteerWheelControlState _rightRearSteerState = + new DiffSteerWheelControlState(); private DiverCartDefinition.ManualControlMode? _pendingTransmitterControlMode; private DateTime _pendingTransmitterControlModeSince = @@ -306,7 +320,16 @@ namespace MedullaAdapter out var feedforwardLf, out var feedforwardLr, out var feedforwardRf, - out var feedforwardRr); + out var feedforwardRr, + out var suppressLf, + out var suppressLr, + out var suppressRf, + out var suppressRr); + + if (suppressLf) feedbackLf = 0f; + if (suppressLr) feedbackLr = 0f; + if (suppressRf) feedbackRf = 0f; + if (suppressRr) feedbackRr = 0f; var diffLf = feedbackLf + feedforwardLf; var diffLr = feedbackLr + feedforwardLr; @@ -345,150 +368,383 @@ namespace MedullaAdapter } /// - /// 根据四个机械目标舵角的实际变化率计算本周期差速轮线速度前馈。 + /// 更新四轮独立的到位迟滞状态，并计算模型逆前馈。 /// private void CalculateDiffSteerRateFeedforward( out float leftFront, out float leftRear, out float rightFront, - out float rightRear) + out float rightRear, + out bool suppressLeftFront, + out bool suppressLeftRear, + out bool suppressRightFront, + out bool suppressRightRear) + { + leftFront = leftRear = rightFront = rightRear = 0f; + suppressLeftFront = suppressLeftRear = false; + suppressRightFront = suppressRightRear = false; + ResetDiffSteerCycleDiagnostics(); + + var currentTimestamp = Stopwatch.GetTimestamp(); + var deltaTimeSeconds = 0.0; + var hasValidControlPeriod = false; + + if (_diffSteerFeedforwardInitialized) + { + deltaTimeSeconds = + (currentTimestamp - + _lastDiffSteerFeedforwardTimestamp) / + (double)Stopwatch.Frequency; + + if (double.IsFinite(deltaTimeSeconds) && + deltaTimeSeconds > 0.0) + { + cart.DiffSteerFeedforwardDeltaTimeMilliseconds = + (float)(deltaTimeSeconds * 1000.0); + hasValidControlPeriod = + deltaTimeSeconds <= + MaximumFeedforwardIntervalSeconds; + } + } + + suppressLeftFront = UpdateDiffSteerSettlingState( + _leftFrontSteerState, + cart.ThLeftFront, + cart.ActualThLeftFront, + hasValidControlPeriod); + suppressLeftRear = UpdateDiffSteerSettlingState( + _leftRearSteerState, + cart.ThLeftRear, + cart.ActualThLeftRear, + hasValidControlPeriod); + suppressRightFront = UpdateDiffSteerSettlingState( + _rightFrontSteerState, + cart.ThRightFront, + cart.ActualThRightFront, + hasValidControlPeriod); + suppressRightRear = UpdateDiffSteerSettlingState( + _rightRearSteerState, + cart.ThRightRear, + cart.ActualThRightRear, + hasValidControlPeriod); + + leftFront = CalculateDiffSteerRateFeedforward( + _leftFrontSteerState, + cart.ThLeftFront, + deltaTimeSeconds, + hasValidControlPeriod, + suppressLeftFront, + out var targetRateLf, + out var rawLf, + out var limitedLf); + leftRear = CalculateDiffSteerRateFeedforward( + _leftRearSteerState, + cart.ThLeftRear, + deltaTimeSeconds, + hasValidControlPeriod, + suppressLeftRear, + out var targetRateLr, + out var rawLr, + out var limitedLr); + rightFront = CalculateDiffSteerRateFeedforward( + _rightFrontSteerState, + cart.ThRightFront, + deltaTimeSeconds, + hasValidControlPeriod, + suppressRightFront, + out var targetRateRf, + out var rawRf, + out var limitedRf); + rightRear = CalculateDiffSteerRateFeedforward( + _rightRearSteerState, + cart.ThRightRear, + deltaTimeSeconds, + hasValidControlPeriod, + suppressRightRear, + out var targetRateRr, + out var rawRr, + out var limitedRr); + + cart.DiffSteerTargetRateLeftFrontDegreesPerSecond = targetRateLf; + cart.DiffSteerTargetRateLeftRearDegreesPerSecond = targetRateLr; + cart.DiffSteerTargetRateRightFrontDegreesPerSecond = targetRateRf; + cart.DiffSteerTargetRateRightRearDegreesPerSecond = targetRateRr; + cart.DiffSteerInverseFeedforwardRawLeftFront = rawLf; + cart.DiffSteerInverseFeedforwardRawLeftRear = rawLr; + cart.DiffSteerInverseFeedforwardRawRightFront = rawRf; + cart.DiffSteerInverseFeedforwardRawRightRear = rawRr; + cart.DiffSteerFeedforwardLimitedLeftFront = limitedLf; + cart.DiffSteerFeedforwardLimitedLeftRear = limitedLr; + cart.DiffSteerFeedforwardLimitedRightFront = limitedRf; + cart.DiffSteerFeedforwardLimitedRightRear = limitedRr; + + // 无论周期是否有效，都保存本周期目标，避免补算过期阶跃。 + _leftFrontSteerState.PreviousTargetAngleDegrees = + cart.ThLeftFront; + _leftRearSteerState.PreviousTargetAngleDegrees = + cart.ThLeftRear; + _rightFrontSteerState.PreviousTargetAngleDegrees = + cart.ThRightFront; + _rightRearSteerState.PreviousTargetAngleDegrees = + cart.ThRightRear; + _lastDiffSteerFeedforwardTimestamp = currentTimestamp; + _diffSteerFeedforwardInitialized = true; + UpdateDiffSteerStateDiagnostics(); + } + + /// + /// 更新单个舵轮的停止区、连续确认和重新启动状态。 + /// + private bool UpdateDiffSteerSettlingState( + DiffSteerWheelControlState state, + float targetAngleDegrees, + float actualAngleDegrees, + bool hasValidControlPeriod) + { + var stopErrorDegrees = cart.DiffSteerStopErrorDegrees; + var restartErrorDegrees = cart.DiffSteerRestartErrorDegrees; + var settlingCycles = cart.DiffSteerSettlingCycles; + var hasValidAngles = + IsFinite(targetAngleDegrees) && + IsFinite(actualAngleDegrees); + var hasValidParameters = + IsFinite(stopErrorDegrees) && + stopErrorDegrees >= 0f && + IsFinite(restartErrorDegrees) && + restartErrorDegrees > stopErrorDegrees && + settlingCycles > 0; + + if (!hasValidAngles) + { + state.IsInStopZone = false; + state.SettlingCycleCount = 0; + state.IsSettled = false; + ClearDiffSteerFeedforwardState(state); + return true; + } + + var absoluteErrorDegrees = Math.Abs( + (double)targetAngleDegrees - actualAngleDegrees); + state.IsInStopZone = + hasValidParameters && + absoluteErrorDegrees <= stopErrorDegrees; + + if (!cart.EnableDiffSteerSettlingHysteresis || + !hasValidParameters) + { + state.SettlingCycleCount = 0; + state.IsSettled = false; + return false; + } + + if (state.IsSettled) + { + if (absoluteErrorDegrees <= restartErrorDegrees) + { + ClearDiffSteerFeedforwardState(state); + return true; + } + + state.IsSettled = false; + state.SettlingCycleCount = 0; + } + + if (!state.IsInStopZone) + { + state.SettlingCycleCount = 0; + return false; + } + + ClearDiffSteerFeedforwardState(state); + if (hasValidControlPeriod) + { + state.SettlingCycleCount = Math.Min( + state.SettlingCycleCount + 1, + settlingCycles); + state.IsSettled = + state.SettlingCycleCount >= settlingCycles; + } + else + { + state.SettlingCycleCount = 0; + } + + return true; + } + + /// + /// 将目标机械舵角变化率转换为带一阶滤波的对象逆前馈。 + /// + private float CalculateDiffSteerRateFeedforward( + DiffSteerWheelControlState state, + float targetAngleDegrees, + double deltaTimeSeconds, + bool hasValidControlPeriod, + bool suppressOutput, + out float targetRateDegreesPerSecond, + out float rawInverseFeedforward, + out bool limited) + { + targetRateDegreesPerSecond = 0f; + rawInverseFeedforward = 0f; + limited = false; + + if (!hasValidControlPeriod || + !IsFinite(targetAngleDegrees) || + !IsFinite(state.PreviousTargetAngleDegrees) || + !double.IsFinite(deltaTimeSeconds) || + deltaTimeSeconds <= 0.0) + { + ClearDiffSteerFeedforwardState(state); + return 0f; + } + + // 机械舵角受限，必须使用直接差值而不是圆周最短角差。 + var targetRate = + ((double)targetAngleDegrees - + state.PreviousTargetAngleDegrees) / + deltaTimeSeconds; + if (!double.IsFinite(targetRate)) + { + ClearDiffSteerFeedforwardState(state); + return 0f; + } + + targetRateDegreesPerSecond = (float)targetRate; + if (!IsFinite(targetRateDegreesPerSecond)) + { + targetRateDegreesPerSecond = 0f; + ClearDiffSteerFeedforwardState(state); + return 0f; + } + + var plantGain = cart.DiffSteerPlantGain; + var timeConstantSeconds = + cart.DiffSteerInverseFeedforwardTimeConstantSeconds; + var maximumSpeed = + cart.DiffSteerRateFeedforwardMaximumSpeed; + + if (!IsFinite(plantGain) || plantGain <= 0f || + !IsFinite(timeConstantSeconds) || + timeConstantSeconds <= 0f || + !IsFinite(maximumSpeed) || maximumSpeed <= 0f) + { + ClearDiffSteerFeedforwardState(state); + return 0f; + } + + var inverseFeedforward = targetRate / plantGain; + if (!double.IsFinite(inverseFeedforward)) + { + ClearDiffSteerFeedforwardState(state); + return 0f; + } + + rawInverseFeedforward = (float)inverseFeedforward; + if (!IsFinite(rawInverseFeedforward)) + { + rawInverseFeedforward = 0f; + ClearDiffSteerFeedforwardState(state); + return 0f; + } + + if (!cart.EnableDiffSteerInverseFeedforward || + suppressOutput) + { + ClearDiffSteerFeedforwardState(state); + return 0f; + } + + var alpha = 1.0 - Math.Exp( + -deltaTimeSeconds / timeConstantSeconds); + var filteredFeedforward = + state.FilteredInverseFeedforwardMetersPerSecond + + alpha * + (inverseFeedforward - + state.FilteredInverseFeedforwardMetersPerSecond); + if (!double.IsFinite(alpha) || + !double.IsFinite(filteredFeedforward)) + { + ClearDiffSteerFeedforwardState(state); + return 0f; + } + + state.FilteredInverseFeedforwardMetersPerSecond = + filteredFeedforward; + + var limitedSpeed = (float)Math.Clamp( + filteredFeedforward, + -maximumSpeed, + maximumSpeed); + limited = Math.Abs( + filteredFeedforward - limitedSpeed) > 1e-9; + return limitedSpeed; + } + + private void ResetDiffSteerCycleDiagnostics() { - leftFront = 0f; - leftRear = 0f; - rightFront = 0f; - rightRear = 0f; cart.DiffSteerTargetRateLeftFrontDegreesPerSecond = 0f; cart.DiffSteerTargetRateLeftRearDegreesPerSecond = 0f; cart.DiffSteerTargetRateRightFrontDegreesPerSecond = 0f; cart.DiffSteerTargetRateRightRearDegreesPerSecond = 0f; cart.DiffSteerFeedforwardDeltaTimeMilliseconds = 0f; + cart.DiffSteerInverseFeedforwardRawLeftFront = 0f; + cart.DiffSteerInverseFeedforwardRawLeftRear = 0f; + cart.DiffSteerInverseFeedforwardRawRightFront = 0f; + cart.DiffSteerInverseFeedforwardRawRightRear = 0f; cart.DiffSteerFeedforwardLimitedLeftFront = false; cart.DiffSteerFeedforwardLimitedLeftRear = false; cart.DiffSteerFeedforwardLimitedRightFront = false; cart.DiffSteerFeedforwardLimitedRightRear = false; - - var currentTimestamp = Stopwatch.GetTimestamp(); - - if (_diffSteerFeedforwardInitialized) - { - var deltaTimeSeconds = - (currentTimestamp - - _lastDiffSteerFeedforwardTimestamp) / - (double)Stopwatch.Frequency; - - if (deltaTimeSeconds > 0.0 && - deltaTimeSeconds <= - MaximumFeedforwardIntervalSeconds) - { - cart.DiffSteerFeedforwardDeltaTimeMilliseconds = - (float)(deltaTimeSeconds * 1000.0); - leftFront = CalculateDiffSteerRateFeedforward( - cart.ThLeftFront, - _previousThLeftFront, - deltaTimeSeconds, - out var targetRateLf, - out var limitedLf); - leftRear = CalculateDiffSteerRateFeedforward( - cart.ThLeftRear, - _previousThLeftRear, - deltaTimeSeconds, - out var targetRateLr, - out var limitedLr); - rightFront = CalculateDiffSteerRateFeedforward( - cart.ThRightFront, - _previousThRightFront, - deltaTimeSeconds, - out var targetRateRf, - out var limitedRf); - rightRear = CalculateDiffSteerRateFeedforward( - cart.ThRightRear, - _previousThRightRear, - 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; - } - } - - _previousThLeftFront = cart.ThLeftFront; - _previousThLeftRear = cart.ThLeftRear; - _previousThRightFront = cart.ThRightFront; - _previousThRightRear = cart.ThRightRear; - _lastDiffSteerFeedforwardTimestamp = currentTimestamp; - _diffSteerFeedforwardInitialized = true; } /// - /// 将单个机械目标舵角变化率转换为带限幅的左右轮差速线速度前馈。 + /// 将四轮独立控制状态复制到M层监控和CSV数据源。 /// - private float CalculateDiffSteerRateFeedforward( - float targetAngleDegrees, - float previousTargetAngleDegrees, - double deltaTimeSeconds, - out float targetRateDegreesPerSecond, - out bool limited) + private void UpdateDiffSteerStateDiagnostics() { - targetRateDegreesPerSecond = 0f; - limited = false; + cart.DiffSteerInStopZoneLeftFront = + _leftFrontSteerState.IsInStopZone; + cart.DiffSteerInStopZoneLeftRear = + _leftRearSteerState.IsInStopZone; + cart.DiffSteerInStopZoneRightFront = + _rightFrontSteerState.IsInStopZone; + cart.DiffSteerInStopZoneRightRear = + _rightRearSteerState.IsInStopZone; + cart.DiffSteerSettlingCountLeftFront = + _leftFrontSteerState.SettlingCycleCount; + cart.DiffSteerSettlingCountLeftRear = + _leftRearSteerState.SettlingCycleCount; + cart.DiffSteerSettlingCountRightFront = + _rightFrontSteerState.SettlingCycleCount; + cart.DiffSteerSettlingCountRightRear = + _rightRearSteerState.SettlingCycleCount; + cart.DiffSteerSettledLeftFront = + _leftFrontSteerState.IsSettled; + cart.DiffSteerSettledLeftRear = + _leftRearSteerState.IsSettled; + cart.DiffSteerSettledRightFront = + _rightFrontSteerState.IsSettled; + cart.DiffSteerSettledRightRear = + _rightRearSteerState.IsSettled; + cart.DiffSteerInverseFeedforwardFilteredLeftFront = + (float)_leftFrontSteerState + .FilteredInverseFeedforwardMetersPerSecond; + cart.DiffSteerInverseFeedforwardFilteredLeftRear = + (float)_leftRearSteerState + .FilteredInverseFeedforwardMetersPerSecond; + cart.DiffSteerInverseFeedforwardFilteredRightFront = + (float)_rightFrontSteerState + .FilteredInverseFeedforwardMetersPerSecond; + cart.DiffSteerInverseFeedforwardFilteredRightRear = + (float)_rightRearSteerState + .FilteredInverseFeedforwardMetersPerSecond; + } - var gain = cart.DiffSteerRateFeedforwardGain; - var wheelDistanceMillimeters = - cart.DiffSteerWheelDistanceMillimeters; - var maximumSpeed = - cart.DiffSteerRateFeedforwardMaximumSpeed; - - 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 = - targetRateDegreesPerSecond * - Math.PI / 180.0; - var wheelDistanceMeters = - wheelDistanceMillimeters / 1000.0; - var feedforwardSpeed = - 0.5 * - wheelDistanceMeters * - targetRateRadiansPerSecond * - gain; - - var limitedSpeed = (float)Math.Clamp( - feedforwardSpeed, - -maximumSpeed, - maximumSpeed); - limited = Math.Abs( - feedforwardSpeed - limitedSpeed) > 1e-9; - return limitedSpeed; + private static void ClearDiffSteerFeedforwardState( + DiffSteerWheelControlState state) + { + state.FilteredInverseFeedforwardMetersPerSecond = 0.0; } /// @@ -498,6 +754,14 @@ namespace MedullaAdapter { _diffSteerFeedforwardInitialized = false; _lastDiffSteerFeedforwardTimestamp = 0; + ResetDiffSteerWheelState(_leftFrontSteerState); + ResetDiffSteerWheelState(_leftRearSteerState); + ResetDiffSteerWheelState(_rightFrontSteerState); + ResetDiffSteerWheelState(_rightRearSteerState); + cart.DiffSteerOutputLeftFront = 0f; + cart.DiffSteerOutputLeftRear = 0f; + cart.DiffSteerOutputRightFront = 0f; + cart.DiffSteerOutputRightRear = 0f; cart.DiffSteerRateFeedforwardLeftFront = 0f; cart.DiffSteerRateFeedforwardLeftRear = 0f; cart.DiffSteerRateFeedforwardRightFront = 0f; @@ -507,6 +771,10 @@ namespace MedullaAdapter cart.DiffSteerTargetRateRightFrontDegreesPerSecond = 0f; cart.DiffSteerTargetRateRightRearDegreesPerSecond = 0f; cart.DiffSteerFeedforwardDeltaTimeMilliseconds = 0f; + cart.DiffSteerInverseFeedforwardRawLeftFront = 0f; + cart.DiffSteerInverseFeedforwardRawLeftRear = 0f; + cart.DiffSteerInverseFeedforwardRawRightFront = 0f; + cart.DiffSteerInverseFeedforwardRawRightRear = 0f; cart.DiffSteerFeedforwardLimitedLeftFront = false; cart.DiffSteerFeedforwardLimitedLeftRear = false; cart.DiffSteerFeedforwardLimitedRightFront = false; @@ -515,6 +783,17 @@ namespace MedullaAdapter cart.DiffSteerTotalOutputLeftRear = 0f; cart.DiffSteerTotalOutputRightFront = 0f; cart.DiffSteerTotalOutputRightRear = 0f; + UpdateDiffSteerStateDiagnostics(); + } + + private static void ResetDiffSteerWheelState( + DiffSteerWheelControlState state) + { + state.PreviousTargetAngleDegrees = 0f; + state.FilteredInverseFeedforwardMetersPerSecond = 0.0; + state.IsInStopZone = false; + state.SettlingCycleCount = 0; + state.IsSettled = false; } /// diff --git a/MedullaAdapter/WheelSpeedDiagnosticLogger.cs b/MedullaAdapter/WheelSpeedDiagnosticLogger.cs index ef97adf..33387f8 100644 --- a/MedullaAdapter/WheelSpeedDiagnosticLogger.cs +++ b/MedullaAdapter/WheelSpeedDiagnosticLogger.cs @@ -92,10 +92,17 @@ namespace MedullaAdapter "VoltageV,AlarmLevel,ChassisMode,WheelAbleState," + "DiffSteerKp,DiffSteerKi,DiffSteerKd,DiffSteerMaxI,DiffSteerDeadZone,DiffSteerThresh,DiffSteerSpeedAcc," + "DiffSteerRateFeedforwardGain,DiffSteerWheelDistanceMillimeters,DiffSteerRateFeedforwardMaximumSpeed," + - "FeedforwardDeltaTimeMs," + + "EnableDiffSteerSettlingHysteresis,DiffSteerStopErrorDegrees,DiffSteerRestartErrorDegrees,DiffSteerSettlingCycles," + + "EnableDiffSteerInverseFeedforward,DiffSteerPlantGain,DiffSteerInverseFeedforwardTimeConstantSeconds," + + "FeedforwardDeltaTimeMs,DiffSteerDeltaTimeSeconds," + "TargetRateThLeftFrontDegreesPerSecond,TargetRateThLeftRearDegreesPerSecond," + "TargetRateThRightFrontDegreesPerSecond,TargetRateThRightRearDegreesPerSecond," + + "InverseFeedforwardRawLeftFront,InverseFeedforwardRawLeftRear,InverseFeedforwardRawRightFront,InverseFeedforwardRawRightRear," + + "InverseFeedforwardFilteredLeftFront,InverseFeedforwardFilteredLeftRear,InverseFeedforwardFilteredRightFront,InverseFeedforwardFilteredRightRear," + "FeedforwardLimitedLeftFront,FeedforwardLimitedLeftRear,FeedforwardLimitedRightFront,FeedforwardLimitedRightRear," + + "InStopZoneLeftFront,InStopZoneLeftRear,InStopZoneRightFront,InStopZoneRightRear," + + "SettlingCountLeftFront,SettlingCountLeftRear,SettlingCountRightFront,SettlingCountRightRear," + + "SettledLeftFront,SettledLeftRear,SettledRightFront,SettledRightRear," + "PidOutLeftFront,PidOutLeftRear,PidOutRightFront,PidOutRightRear," + "RateFeedforwardLeftFront,RateFeedforwardLeftRear,RateFeedforwardRightFront,RateFeedforwardRightRear," + "TotalDiffLeftFront,TotalDiffLeftRear,TotalDiffRightFront,TotalDiffRightRear," + @@ -352,15 +359,46 @@ namespace MedullaAdapter Format(cart.DiffSteerRateFeedforwardGain), Format(cart.DiffSteerWheelDistanceMillimeters), Format(cart.DiffSteerRateFeedforwardMaximumSpeed), + FormatBoolean(cart.EnableDiffSteerSettlingHysteresis), + Format(cart.DiffSteerStopErrorDegrees), + Format(cart.DiffSteerRestartErrorDegrees), + Format(cart.DiffSteerSettlingCycles), + FormatBoolean(cart.EnableDiffSteerInverseFeedforward), + Format(cart.DiffSteerPlantGain), + Format( + cart.DiffSteerInverseFeedforwardTimeConstantSeconds), Format(cart.DiffSteerFeedforwardDeltaTimeMilliseconds), + Format( + cart.DiffSteerFeedforwardDeltaTimeMilliseconds / + 1000f), Format(cart.DiffSteerTargetRateLeftFrontDegreesPerSecond), Format(cart.DiffSteerTargetRateLeftRearDegreesPerSecond), Format(cart.DiffSteerTargetRateRightFrontDegreesPerSecond), Format(cart.DiffSteerTargetRateRightRearDegreesPerSecond), + Format(cart.DiffSteerInverseFeedforwardRawLeftFront), + Format(cart.DiffSteerInverseFeedforwardRawLeftRear), + Format(cart.DiffSteerInverseFeedforwardRawRightFront), + Format(cart.DiffSteerInverseFeedforwardRawRightRear), + Format(cart.DiffSteerInverseFeedforwardFilteredLeftFront), + Format(cart.DiffSteerInverseFeedforwardFilteredLeftRear), + Format(cart.DiffSteerInverseFeedforwardFilteredRightFront), + Format(cart.DiffSteerInverseFeedforwardFilteredRightRear), FormatBoolean(cart.DiffSteerFeedforwardLimitedLeftFront), FormatBoolean(cart.DiffSteerFeedforwardLimitedLeftRear), FormatBoolean(cart.DiffSteerFeedforwardLimitedRightFront), FormatBoolean(cart.DiffSteerFeedforwardLimitedRightRear), + FormatBoolean(cart.DiffSteerInStopZoneLeftFront), + FormatBoolean(cart.DiffSteerInStopZoneLeftRear), + FormatBoolean(cart.DiffSteerInStopZoneRightFront), + FormatBoolean(cart.DiffSteerInStopZoneRightRear), + Format(cart.DiffSteerSettlingCountLeftFront), + Format(cart.DiffSteerSettlingCountLeftRear), + Format(cart.DiffSteerSettlingCountRightFront), + Format(cart.DiffSteerSettlingCountRightRear), + FormatBoolean(cart.DiffSteerSettledLeftFront), + FormatBoolean(cart.DiffSteerSettledLeftRear), + FormatBoolean(cart.DiffSteerSettledRightFront), + FormatBoolean(cart.DiffSteerSettledRightRear), Format(cart.DiffSteerOutputLeftFront), Format(cart.DiffSteerOutputLeftRear), Format(cart.DiffSteerOutputRightFront), diff --git a/MedullaAdapter/build/Medulla/plugins/CommonUsage.dll b/MedullaAdapter/build/Medulla/plugins/CommonUsage.dll index d284e2e..280d7db 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 6a3cd4b..3d46cf9 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 dc4c9c6..4b8b4cb 100644 Binary files a/MedullaAdapter/build/Medulla/plugins/MedullaAdapter.pdb and b/MedullaAdapter/build/Medulla/plugins/MedullaAdapter.pdb differ diff --git a/MultiWheelC/Experiments/LongitudinalIdentificationTests.cs b/MultiWheelC/Experiments/LongitudinalIdentificationTests.cs new file mode 100644 index 0000000..f5ee215 --- /dev/null +++ b/MultiWheelC/Experiments/LongitudinalIdentificationTests.cs @@ -0,0 +1,562 @@ +using System; +using System.Diagnostics; +using System.Globalization; +using System.Numerics; +using System.Threading; +using ClumsyCore; +using ClumsyCore.Pilot; +using CommonUsage.Chassis; +using FundamentalLib; +using MultiWheelC.StateEstimation; +using MyParking.Shared; + +namespace MultiWheelC +{ + /// + /// 提供纵向开环辨识测试共用的舵轮准备、速度下发、采样和安全停车流程。 + /// + public abstract class LongitudinalIdentificationTestBase + : MovementTest + { + private const double MaximumTargetSpeedMetersPerSecond = + 0.70; + private const double MinimumTargetSpeedMetersPerSecond = + 0.02; + private const float MillimetersPerMeter = 1000f; + + private DriveTask _preparationTask; + private MultiWheelChassis _chassis; + private MultiWheelChassisAdapter _adapter; + private WheelFeedbackVehicleStateProvider _stateProvider; + private TrackingExperimentRecorder _recorder; + private int _testRunning; + private int _stopRequested; + + /// + /// 获取或设置本次重复实验编号。 + /// + public int TrialNumber = 1; + + /// + /// 获取或设置速度阶跃前的静止记录时间，单位为s。 + /// + public double BaselineSeconds = 1.0; + + /// + /// 获取或设置目标速度保持时间，单位为s。 + /// + public double CommandHoldSeconds = 5.0; + + /// + /// 获取或设置停车后的继续记录时间，单位为s。 + /// + public double PostStopSeconds = 2.0; + + /// + /// 获取或设置速度命令循环周期，单位为ms。 + /// + public int ControlIntervalMilliseconds = 50; + + /// + /// 派生测试选择是否使用底盘DeAccPerSecond完成正常减速。 + /// + protected abstract bool UseConfiguredDeceleration { get; } + + /// + /// 获取用于CSV文件名区分停车方式的标识。 + /// + protected abstract string StopModeName { get; } + + /// + /// 读取目标速度，完成舵轮回正后执行纵向开环命令并保存CSV。 + /// + public override void Test() + { + if (Interlocked.CompareExchange( + ref _testRunning, + 1, + 0) != 0) + { + Console.WriteLine( + "纵向辨识测试已经在运行，请先停止当前测试。"); + return; + } + + Interlocked.Exchange(ref _stopRequested, 0); + + try + { + ValidateSettings(); + if (!TryReadTargetSpeed( + out var targetSpeedMetersPerSecond)) + { + return; + } + + _chassis = + PilotDefinition.Chassis as MultiWheelChassis; + if (_chassis == null) + { + throw new InvalidOperationException( + "当前底盘不是MultiWheelChassis，无法执行纵向辨识。"); + } + + ValidateChassisAndTarget( + targetSpeedMetersPerSecond); + + Console.WriteLine( + "纵向辨识开始前请确认M层轮速诊断记录已经开启。"); + Hedingben.ToastText( + "请确认M层轮速诊断记录已开启。", + "LongitudinalIdentification"); + + if (!MovementTestPreparation.AlignWheelsForward( + ref _preparationTask)) + { + throw new InvalidOperationException( + "四个舵轮未能稳定回正，纵向辨识已经取消。"); + } + + ThrowIfStopRequested(); + + _adapter = new MultiWheelChassisAdapter( + _chassis, + PilotDefinition.Self.CarNum); + _adapter.ResetToBodyFrame(); + _adapter.StopImmediately(); + + _stateProvider = + ParkingVehicleStateProviderFactory.Create( + _chassis); + if (!_stateProvider.TryGetState( + out var initialState)) + { + throw new InvalidOperationException( + "无法读取纵向辨识起点位姿：" + + _stateProvider.LastFailureReason); + } + + _recorder = CreateRecorder( + targetSpeedMetersPerSecond, + initialState.PoseInWorld); + _recorder.Start(); + + Console.WriteLine( + $"纵向辨识参数：目标速度={targetSpeedMetersPerSecond:F3}m/s，" + + $"AccPerSecond={_chassis.AccPerSecond:F3}m/s²，" + + $"DeAccPerSecond={_chassis.DeAccPerSecond:F3}m/s²，" + + $"保持={CommandHoldSeconds:F1}s，停车方式={StopModeName}。"); + + RunStationaryPhase( + BaselineSeconds, + "静止基线"); + RunTargetSpeedPhase( + targetSpeedMetersPerSecond); + + if (UseConfiguredDeceleration) + { + RunConfiguredDecelerationPhase( + targetSpeedMetersPerSecond); + } + else + { + _adapter.StopImmediately(); + RunStationaryPhase( + PostStopSeconds, + "立即停车后记录"); + } + + Console.WriteLine( + "纵向辨识测试完成。请同时保存对应的M层轮速诊断CSV。"); + Hedingben.ToastText( + "纵向辨识完成，请停止并保存M层轮速诊断记录。", + "LongitudinalIdentification"); + } + catch (OperationCanceledException) + { + Console.WriteLine("纵向辨识已由用户停止。"); + } + catch (Exception exception) + { + Console.WriteLine( + "纵向辨识失败：" + + exception.Message); + Hedingben.ToastText( + "纵向辨识失败：" + + exception.Message, + "LongitudinalIdentification"); + } + finally + { + _adapter?.StopImmediately(); + _chassis?.PredefinedDriveStop(); + _recorder?.UpdateBodyCommand(0f, 0f, 0f); + _recorder?.StopAndSave(); + + _preparationTask?.Stop(); + _preparationTask = null; + _recorder = null; + _stateProvider = null; + _adapter = null; + _chassis = null; + Interlocked.Exchange(ref _testRunning, 0); + } + } + + /// + /// 请求停止当前辨识并立即清零底盘驱动速度。 + /// + public override void TestStop() + { + Interlocked.Exchange(ref _stopRequested, 1); + _preparationTask?.Stop(); + _adapter?.StopImmediately(); + _chassis?.PredefinedDriveStop(); + } + + /// + /// 在固定周期内保持零命令并记录静止数据。 + /// + private void RunStationaryPhase( + double durationSeconds, + string phaseName) + { + _recorder.UpdateBodyCommand(0f, 0f, 0f); + RunPeriodicPhase( + durationSeconds, + phaseName, + _ => UpdateWheelDiagnostics()); + } + + /// + /// 通过标准车体速度适配器持续下发直线目标速度。 + /// + private void RunTargetSpeedPhase( + double targetSpeedMetersPerSecond) + { + _recorder.UpdateBodyCommand( + (float)targetSpeedMetersPerSecond, + 0f, + 0f); + + RunPeriodicPhase( + CommandHoldSeconds, + "目标速度保持", + interval => + { + var accepted = _adapter.SendBodyTwist( + new Twist2D( + targetSpeedMetersPerSecond, + 0.0, + 0.0), + interval); + if (!accepted) + { + throw new InvalidOperationException( + "底盘拒绝纵向速度命令：" + + _adapter.LastFailureReason); + } + + UpdateWheelDiagnostics(); + }); + } + + /// + /// 重复发送零速SendMotion，使底盘内部DeAccPerSecond斜坡真实参与减速。 + /// + private void RunConfiguredDecelerationPhase( + double targetSpeedMetersPerSecond) + { + _recorder.UpdateBodyCommand(0f, 0f, 0f); + + var rampDurationSeconds = + Math.Abs(targetSpeedMetersPerSecond) / + _chassis.DeAccPerSecond; + var totalDurationSeconds = + rampDurationSeconds + + PostStopSeconds; + + RunPeriodicPhase( + totalDurationSeconds, + "配置化正常减速", + interval => + { + // SendBodyTwist(Zero)会立即停车；辨识DeAccPerSecond时必须 + // 直接保持SendMotion零目标，让底盘内部发送速度逐周期下降。 + var accepted = _chassis.SendMotion( + 0f, + 0f, + 0f, + interval); + if (!accepted) + { + throw new InvalidOperationException( + "底盘拒绝正常减速命令：" + + _chassis.LastMotionDecomposeFailureReason); + } + + UpdateWheelDiagnostics(); + }); + } + + /// + /// 按实际循环间隔运行一个阶段，并响应测试界面的停止请求。 + /// + private void RunPeriodicPhase( + double durationSeconds, + string phaseName, + Action cycleAction) + { + Console.WriteLine( + $"纵向辨识阶段：{phaseName}，预计{durationSeconds:F2}s。"); + + var periodSeconds = + ControlIntervalMilliseconds / 1000.0; + var phaseClock = Stopwatch.StartNew(); + var previousCycleSeconds = -periodSeconds; + + while (phaseClock.Elapsed.TotalSeconds < + durationSeconds) + { + ThrowIfStopRequested(); + + var cycleStartSeconds = + phaseClock.Elapsed.TotalSeconds; + var deltaTimeSeconds = + cycleStartSeconds - + previousCycleSeconds; + previousCycleSeconds = cycleStartSeconds; + + cycleAction( + TimeSpan.FromSeconds( + deltaTimeSeconds)); + + var elapsedMilliseconds = + (phaseClock.Elapsed.TotalSeconds - + cycleStartSeconds) * 1000.0; + var remainingMilliseconds = + ControlIntervalMilliseconds - + elapsedMilliseconds; + if (remainingMilliseconds > 1.0) + { + Thread.Sleep( + (int)Math.Floor( + remainingMilliseconds)); + } + } + } + + /// + /// 刷新轮组原始与滤波速度，供后台CSV记录器读取最新诊断值。 + /// + private void UpdateWheelDiagnostics() + { + _stateProvider.TryGetWheelTwist( + out _, + out _); + } + + /// + /// 创建复用现有字段格式的纵向辨识CSV记录器。 + /// + private TrackingExperimentRecorder CreateRecorder( + double targetSpeedMetersPerSecond, + Pose2D initialPoseInWorld) + { + var expectedTravelMeters = + targetSpeedMetersPerSecond * + CommandHoldSeconds; + var referenceStart = new Vector2( + (float)( + initialPoseInWorld.XMeters * + MillimetersPerMeter), + (float)( + initialPoseInWorld.YMeters * + MillimetersPerMeter)); + var referenceEnd = new Vector2( + referenceStart.X + + (float)( + Math.Cos(initialPoseInWorld.YawRadians) * + expectedTravelMeters * + MillimetersPerMeter), + referenceStart.Y + + (float)( + Math.Sin(initialPoseInWorld.YawRadians) * + expectedTravelMeters * + MillimetersPerMeter)); + var speedMillimetersPerSecond = + targetSpeedMetersPerSecond * + MillimetersPerMeter; + var trajectoryName = + "LongitudinalStep_" + + speedMillimetersPerSecond.ToString( + "+0;-0;0", + CultureInfo.InvariantCulture) + + "mmps_" + + StopModeName; + + return new TrackingExperimentRecorder( + controllerName: + "OpenLoopLongitudinalIdentification", + trajectoryName: trajectoryName, + trialNumber: TrialNumber, + referenceStart: referenceStart, + referenceEnd: referenceEnd, + referenceSpeed: + (float)targetSpeedMetersPerSecond, + sampleIntervalMs: + ControlIntervalMilliseconds, + referenceMotionFrameYawDegrees: 0f, + referenceAccelerationMetersPerSecondSquared: + _chassis.AccPerSecond, + referenceDecelerationMetersPerSecondSquared: + _chassis.DeAccPerSecond, + diagnosticChassis: _chassis, + diagnosticStateProvider: _stateProvider); + } + + /// + /// 从测试界面读取带方向的纵向目标速度，正值前进、负值倒车。 + /// + private static bool TryReadTargetSpeed( + out double targetSpeedMetersPerSecond) + { + targetSpeedMetersPerSecond = 0.0; + var input = UI.GetInput( + "输入纵向目标速度（m/s，正数前进、负数倒车，" + + $"范围-{MaximumTargetSpeedMetersPerSecond:F1}～" + + $"{MaximumTargetSpeedMetersPerSecond:F1}且不能为0）："); + var parsed = double.TryParse( + input, + NumberStyles.Float, + CultureInfo.CurrentCulture, + out targetSpeedMetersPerSecond) || + double.TryParse( + input, + NumberStyles.Float, + CultureInfo.InvariantCulture, + out targetSpeedMetersPerSecond); + + if (!parsed || + double.IsNaN(targetSpeedMetersPerSecond) || + double.IsInfinity(targetSpeedMetersPerSecond) || + Math.Abs(targetSpeedMetersPerSecond) < + MinimumTargetSpeedMetersPerSecond || + Math.Abs(targetSpeedMetersPerSecond) > + MaximumTargetSpeedMetersPerSecond) + { + Console.WriteLine( + "目标速度必须是绝对值位于" + + $"{MinimumTargetSpeedMetersPerSecond:F2}～" + + $"{MaximumTargetSpeedMetersPerSecond:F2}m/s之间的有限数值。"); + return false; + } + + return true; + } + + /// + /// 验证实验时间、循环周期和编号配置。 + /// + private void ValidateSettings() + { + NumericGuard.EnsureFiniteNonNegative( + BaselineSeconds, + nameof(BaselineSeconds)); + NumericGuard.EnsureFinitePositive( + CommandHoldSeconds, + nameof(CommandHoldSeconds)); + NumericGuard.EnsureFiniteNonNegative( + PostStopSeconds, + nameof(PostStopSeconds)); + + if (CommandHoldSeconds > 30.0 || + BaselineSeconds > 10.0 || + PostStopSeconds > 10.0) + { + throw new ArgumentOutOfRangeException( + nameof(CommandHoldSeconds), + "辨识阶段时间超出测试允许范围。"); + } + + if (ControlIntervalMilliseconds < 20 || + ControlIntervalMilliseconds > 200) + { + throw new ArgumentOutOfRangeException( + nameof(ControlIntervalMilliseconds), + "纵向辨识命令周期必须在20～200ms之间。"); + } + + if (TrialNumber <= 0) + { + throw new ArgumentOutOfRangeException( + nameof(TrialNumber), + "测试编号必须大于零。"); + } + } + + /// + /// 验证底盘运行时加载的速度、加速度和减速度配置。 + /// + private void ValidateChassisAndTarget( + double targetSpeedMetersPerSecond) + { + NumericGuard.EnsureFinitePositive( + _chassis.MaxSpeed, + nameof(_chassis.MaxSpeed)); + NumericGuard.EnsureFinitePositive( + _chassis.AccPerSecond, + nameof(_chassis.AccPerSecond)); + NumericGuard.EnsureFinitePositive( + _chassis.DeAccPerSecond, + nameof(_chassis.DeAccPerSecond)); + + if (Math.Abs(targetSpeedMetersPerSecond) > + _chassis.MaxSpeed) + { + throw new ArgumentOutOfRangeException( + nameof(targetSpeedMetersPerSecond), + "目标速度超过底盘当前MaxSpeed配置。"); + } + } + + /// + /// 在收到停止请求时中断当前阶段并转入finally安全停车。 + /// + private void ThrowIfStopRequested() + { + if (Volatile.Read(ref _stopRequested) != 0) + { + throw new OperationCanceledException(); + } + } + } + + /// + /// 测量速度阶跃、加速与稳态，并在保持结束后立即清零驱动速度。 + /// + [MovementTest(name = "纵向辨识：速度阶跃与立即停车")] + public sealed class LongitudinalStepImmediateStopTest + : LongitudinalIdentificationTestBase + { + protected override bool UseConfiguredDeceleration => + false; + + protected override string StopModeName => + "ImmediateStop"; + } + + /// + /// 测量速度阶跃、稳态以及由底盘DeAccPerSecond形成的正常减速过程。 + /// + [MovementTest(name = "纵向辨识：速度阶跃与正常减速")] + public sealed class LongitudinalStepConfiguredDecelerationTest + : LongitudinalIdentificationTestBase + { + protected override bool UseConfiguredDeceleration => + true; + + protected override string StopModeName => + "ConfiguredDeceleration"; + } +} diff --git a/MultiWheelC/build/Clumsy/CommonUsage.dll b/MultiWheelC/build/Clumsy/CommonUsage.dll index d284e2e..280d7db 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 666bccd..1aadbae 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 d62166c..121733b 100644 Binary files a/MultiWheelC/build/Clumsy/MultiWheelC.pdb and b/MultiWheelC/build/Clumsy/MultiWheelC.pdb differ diff --git a/data_process/系统辨识实验-舵轮延迟/prepare_steering_iddata.m b/data_process/系统辨识实验-舵轮延迟/prepare_steering_iddata.m new file mode 100644 index 0000000..808666e --- /dev/null +++ b/data_process/系统辨识实验-舵轮延迟/prepare_steering_iddata.m @@ -0,0 +1,651 @@ +function result = prepare_steering_iddata(experimentRoot, outputDirectory) +%PREPARE_STEERING_IDDATA Convert steering snapshot CSV files to MATLAB iddata. +% RESULT = PREPARE_STEERING_IDDATA() reads the two experiment folders next +% to this file, detects every steering target step, aligns command and +% feedback timestamps, resamples each response to 0.04 s, and saves: +% +% dataEstLF/LR/RF/RR uDiff -> delta actual steering angle +% dataValLF/LR/RF/RR independent validation data +% closedLoopEst* delta target angle -> delta actual angle +% closedLoopVal* independent closed-loop validation data +% +% Each variable is a multi-experiment iddata object. Import one wheel at a +% time into the System Identification app. The first experiment folder +% (Kp=0.0035, dead zone=0.1 deg) is used for estimation and the third +% folder (Kp=0.003, dead zone=0.1 deg) for validation. The second folder +% with a 0.5 deg dead zone is retained as supplementary data and is not +% used by this primary identification workflow. Raw CSV files are never +% modified. +% +% Example: +% result = prepare_steering_iddata; +% load(result.MatFile); +% systemIdentification; +% +% Units: uDiff in m/s, steering angle in degrees, time in seconds. + + if nargin < 1 || isempty(experimentRoot) + experimentRoot = fileparts(mfilename('fullpath')); + end + if nargin < 2 || isempty(outputDirectory) + outputDirectory = fullfile(experimentRoot, 'matlab_ready'); + end + + if exist('iddata', 'file') ~= 2 + error('prepare_steering_iddata:MissingToolbox', ... + ['找不到 iddata。请安装或启用 MATLAB System Identification ' ... + 'Toolbox 后重新运行。']); + end + + settings = struct(); + settings.SampleTimeSeconds = 0.04; + settings.PreStepSeconds = 0.8; + settings.PostStepSeconds = 4.0; + settings.BaselineSeconds = 0.5; + settings.NextStepGuardSeconds = 0.25; + settings.MinimumTargetStepDegrees = 5.0; + settings.MinimumSegmentSamples = 30; + settings.EstimationFolder = '第一次0.0035--死区0.1度'; + settings.ValidationFolder = '第三次0.0030--死区0.1度'; + + wheels = steeringWheelDefinitions(); + estimationPath = fullfile(experimentRoot, settings.EstimationFolder); + validationPath = fullfile(experimentRoot, settings.ValidationFolder); + requireFolder(estimationPath); + requireFolder(validationPath); + + fprintf('读取估计数据：%s\n', estimationPath); + estimationSegments = processExperimentFolder( ... + estimationPath, 'Est', wheels, settings); + fprintf('读取验证数据：%s\n', validationPath); + validationSegments = processExperimentFolder( ... + validationPath, 'Val', wheels, settings); + + allSegments = [estimationSegments, validationSegments]; + if isempty(allSegments) + error('prepare_steering_iddata:NoSegments', ... + '没有找到满足条件的舵角切换片段。'); + end + + dataEstLF = buildIdData(estimationSegments, 'LF', 'plant', settings); + dataEstLR = buildIdData(estimationSegments, 'LR', 'plant', settings); + dataEstRF = buildIdData(estimationSegments, 'RF', 'plant', settings); + dataEstRR = buildIdData(estimationSegments, 'RR', 'plant', settings); + + dataValLF = buildIdData(validationSegments, 'LF', 'plant', settings); + dataValLR = buildIdData(validationSegments, 'LR', 'plant', settings); + dataValRF = buildIdData(validationSegments, 'RF', 'plant', settings); + dataValRR = buildIdData(validationSegments, 'RR', 'plant', settings); + + closedLoopEstLF = buildIdData( ... + estimationSegments, 'LF', 'closedLoop', settings); + closedLoopEstLR = buildIdData( ... + estimationSegments, 'LR', 'closedLoop', settings); + closedLoopEstRF = buildIdData( ... + estimationSegments, 'RF', 'closedLoop', settings); + closedLoopEstRR = buildIdData( ... + estimationSegments, 'RR', 'closedLoop', settings); + + closedLoopValLF = buildIdData( ... + validationSegments, 'LF', 'closedLoop', settings); + closedLoopValLR = buildIdData( ... + validationSegments, 'LR', 'closedLoop', settings); + closedLoopValRF = buildIdData( ... + validationSegments, 'RF', 'closedLoop', settings); + closedLoopValRR = buildIdData( ... + validationSegments, 'RR', 'closedLoop', settings); + + metadata = buildMetadataTable(allSegments); + samples = buildSampleTable(allSegments); + + if ~exist(outputDirectory, 'dir') + mkdir(outputDirectory); + end + + matFile = fullfile(outputDirectory, 'steering_identification_data.mat'); + metadataFile = fullfile(outputDirectory, 'steering_segment_metadata.csv'); + samplesFile = fullfile(outputDirectory, 'steering_identification_samples.csv'); + + save(matFile, ... + 'dataEstLF', 'dataEstLR', 'dataEstRF', 'dataEstRR', ... + 'dataValLF', 'dataValLR', 'dataValRF', 'dataValRR', ... + 'closedLoopEstLF', 'closedLoopEstLR', ... + 'closedLoopEstRF', 'closedLoopEstRR', ... + 'closedLoopValLF', 'closedLoopValLR', ... + 'closedLoopValRF', 'closedLoopValRR', ... + 'metadata', 'settings', '-v7.3'); + writetable(metadata, metadataFile); + writetable(samples, samplesFile); + + result = struct(); + result.MatFile = matFile; + result.MetadataFile = metadataFile; + result.SamplesFile = samplesFile; + result.EstimationSegmentCount = numel(estimationSegments); + result.ValidationSegmentCount = numel(validationSegments); + result.SampleTimeSeconds = settings.SampleTimeSeconds; + + fprintf('\n处理完成。每个舵轮单独建立多实验数据：\n'); + printSegmentCounts(estimationSegments, validationSegments, wheels); + fprintf('MATLAB辨识数据：%s\n', matFile); + fprintf('片段元数据：%s\n', metadataFile); + fprintf('长表CSV：%s\n', samplesFile); + fprintf(['\n在工作区加载 MAT 文件后，将 dataEstLF 等变量作为估计数据，' ... + '将对应的 dataValLF 等变量作为验证数据。\n']); +end + +function wheels = steeringWheelDefinitions() +%STEERINGWHEELDEFINITIONS Describe the four differential steering modules. + wheels = struct( ... + 'Code', {'LF', 'LR', 'RF', 'RR'}, ... + 'Name', {'LeftFront', 'LeftRear', 'RightFront', 'RightRear'}, ... + 'TargetColumn', { ... + 'TargetThLeftFront', 'TargetThLeftRear', ... + 'TargetThRightFront', 'TargetThRightRear'}, ... + 'ActualColumn', { ... + 'ActualThLeftFront', 'ActualThLeftRear', ... + 'ActualThRightFront', 'ActualThRightRear'}, ... + 'ActualTimeColumn', { ... + 'ActualThLeftFrontReceiveElapsedMs', ... + 'ActualThLeftRearReceiveElapsedMs', ... + 'ActualThRightFrontReceiveElapsedMs', ... + 'ActualThRightRearReceiveElapsedMs'}, ... + 'SentLeftColumn', { ... + 'SentLFLMps', 'SentLRLMps', 'SentRFLMps', 'SentRRLMps'}, ... + 'SentRightColumn', { ... + 'SentLFRMps', 'SentLRRMps', 'SentRFRMps', 'SentRRRMps'}, ... + 'LeftLimitedColumn', { ... + 'CommandLimitedLFL', 'CommandLimitedLRL', ... + 'CommandLimitedRFL', 'CommandLimitedRRL'}, ... + 'RightLimitedColumn', { ... + 'CommandLimitedLFR', 'CommandLimitedLRR', ... + 'CommandLimitedRFR', 'CommandLimitedRRR'}, ... + 'PairLimitedColumn', { ... + 'PairCommandLimitedLeftFront', ... + 'PairCommandLimitedLeftRear', ... + 'PairCommandLimitedRightFront', ... + 'PairCommandLimitedRightRear'}); +end + +function segments = processExperimentFolder(folder, datasetCode, wheels, settings) +%PROCESSEXPERIMENTFOLDER Convert all snapshot files in one experiment set. + files = dir(fullfile(folder, '*_snapshot.csv')); + if isempty(files) + error('prepare_steering_iddata:NoSnapshotFiles', ... + '目录中没有 *_snapshot.csv：%s', folder); + end + + [~, order] = sort({files.name}); + files = files(order); + segments = emptySegmentArray(); + + for fileIndex = 1:numel(files) + sourcePath = fullfile(files(fileIndex).folder, files(fileIndex).name); + fprintf(' %s\n', files(fileIndex).name); + snapshot = readNumericSnapshot(sourcePath); + validateSnapshotColumns(snapshot, wheels); + + transitionType = regexprep( ... + files(fileIndex).name, '_Car\d+_snapshot\.csv$', ''); + fileSegments = processSnapshot( ... + snapshot, files(fileIndex).name, transitionType, ... + datasetCode, wheels, settings); + segments = [segments, fileSegments]; %#ok + end +end + +function snapshot = readNumericSnapshot(sourcePath) +%READNUMERICSNAPSHOT Read the numeric diagnostic snapshot without mutation. + importOptions = detectImportOptions(sourcePath, 'Delimiter', ','); + importOptions = setvartype( ... + importOptions, importOptions.VariableNames, 'double'); + snapshot = readtable(sourcePath, importOptions); + if isempty(snapshot) + error('prepare_steering_iddata:EmptySnapshot', ... + 'CSV中没有数据：%s', sourcePath); + end +end + +function validateSnapshotColumns(snapshot, wheels) +%VALIDATESNAPSHOTCOLUMNS Ensure every signal needed for identification exists. + required = { ... + 'ElapsedMs', 'ControlElapsedMs', 'WheelCommandElapsedMs', ... + 'WheelCommandSuppressed', 'DiffSteerKp', 'DiffSteerKi', ... + 'DiffSteerKd', 'DiffSteerDeadZone', ... + 'DiffSteerRateFeedforwardGain'}; + + for wheelIndex = 1:numel(wheels) + wheel = wheels(wheelIndex); + required = [required, { ... %#ok + wheel.TargetColumn, wheel.ActualColumn, ... + wheel.ActualTimeColumn, wheel.SentLeftColumn, ... + wheel.SentRightColumn, wheel.LeftLimitedColumn, ... + wheel.RightLimitedColumn, wheel.PairLimitedColumn}]; + end + + missing = setdiff(required, snapshot.Properties.VariableNames); + if ~isempty(missing) + error('prepare_steering_iddata:MissingColumns', ... + 'snapshot CSV缺少字段：%s', strjoin(missing, ', ')); + end +end + +function segments = processSnapshot( ... + snapshot, sourceFile, transitionType, datasetCode, wheels, settings) +%PROCESSSNAPSHOT Extract one iddata experiment for every wheel target step. + elapsedMs = snapshot.ElapsedMs; + finiteElapsed = elapsedMs(isfinite(elapsedMs)); + if isempty(finiteElapsed) + error('prepare_steering_iddata:InvalidTime', ... + '%s没有有效ElapsedMs。', sourceFile); + end + timeOriginMs = finiteElapsed(1); + + kp = constantParameter(snapshot.DiffSteerKp, 'DiffSteerKp', sourceFile); + ki = constantParameter(snapshot.DiffSteerKi, 'DiffSteerKi', sourceFile); + kd = constantParameter(snapshot.DiffSteerKd, 'DiffSteerKd', sourceFile); + deadZone = constantParameter( ... + snapshot.DiffSteerDeadZone, 'DiffSteerDeadZone', sourceFile); + feedforwardGain = constantParameter( ... + snapshot.DiffSteerRateFeedforwardGain, ... + 'DiffSteerRateFeedforwardGain', sourceFile); + + segments = emptySegmentArray(); + for wheelIndex = 1:numel(wheels) + wheel = wheels(wheelIndex); + wheelSegments = extractWheelSegments( ... + snapshot, timeOriginMs, sourceFile, transitionType, ... + datasetCode, wheel, settings, kp, ki, kd, ... + deadZone, feedforwardGain); + segments = [segments, wheelSegments]; %#ok + + if numel(wheelSegments) ~= 20 + warning('prepare_steering_iddata:UnexpectedStepCount', ... + '%s %s检测到%d次切换，预期为20次。', ... + sourceFile, wheel.Code, numel(wheelSegments)); + end + end +end + +function segments = extractWheelSegments( ... + snapshot, timeOriginMs, sourceFile, transitionType, datasetCode, ... + wheel, settings, kp, ki, kd, deadZone, feedforwardGain) +%EXTRACTWHEELSEGMENTS Align and split one steering module's time series. + targetTime = (snapshot.ControlElapsedMs - timeOriginMs) / 1000; + targetAngle = snapshot.(wheel.TargetColumn); + [targetTime, targetAngle] = compactTimedSignal(targetTime, targetAngle); + + commandTime = (snapshot.WheelCommandElapsedMs - timeOriginMs) / 1000; + uDiff = (snapshot.(wheel.SentRightColumn) - ... + snapshot.(wheel.SentLeftColumn)) / 2; + [commandTime, uDiff] = compactTimedSignal(commandTime, uDiff); + + actualTime = (snapshot.(wheel.ActualTimeColumn) - timeOriginMs) / 1000; + actualAngle = snapshot.(wheel.ActualColumn); + [actualTime, actualAngle] = compactTimedSignal(actualTime, actualAngle); + + if numel(targetTime) < 2 || numel(commandTime) < 2 || ... + numel(actualTime) < 2 + warning('prepare_steering_iddata:InsufficientSignal', ... + '%s %s的时间序列不足，已跳过。', sourceFile, wheel.Code); + segments = emptySegmentArray(); + return; + end + + rawEventIndices = find( ... + abs(diff(targetAngle)) >= settings.MinimumTargetStepDegrees) + 1; + eventIndices = suppressNearbyEvents( ... + rawEventIndices, targetTime, settings.NextStepGuardSeconds); + segments = emptySegmentArray(); + + for eventNumber = 1:numel(eventIndices) + targetIndex = eventIndices(eventNumber); + eventTime = targetTime(targetIndex); + targetStep = targetAngle(targetIndex) - targetAngle(targetIndex - 1); + + segmentStart = eventTime - settings.PreStepSeconds; + segmentEnd = eventTime + settings.PostStepSeconds; + if eventNumber < numel(eventIndices) + nextEventTime = targetTime(eventIndices(eventNumber + 1)); + segmentEnd = min( ... + segmentEnd, nextEventTime - settings.NextStepGuardSeconds); + end + + commonStart = max([ ... + segmentStart, targetTime(1), commandTime(1), actualTime(1)]); + commonEnd = min([ ... + segmentEnd, targetTime(end), commandTime(end), actualTime(end)]); + firstGridIndex = ceil(commonStart / settings.SampleTimeSeconds); + lastGridIndex = floor(commonEnd / settings.SampleTimeSeconds); + commonTime = (firstGridIndex:lastGridIndex)' * ... + settings.SampleTimeSeconds; + + if numel(commonTime) < settings.MinimumSegmentSamples + warning('prepare_steering_iddata:ShortSegment', ... + '%s %s第%d次切换数据过短，已跳过。', ... + sourceFile, wheel.Code, eventNumber); + continue; + end + + targetResampled = interp1( ... + targetTime, targetAngle, commonTime, 'previous'); + inputResampled = interp1( ... + commandTime, uDiff, commonTime, 'previous'); + outputResampled = interp1( ... + actualTime, actualAngle, commonTime, 'linear'); + relativeTime = commonTime - eventTime; + + baselineMask = relativeTime >= -settings.BaselineSeconds & ... + relativeTime < -settings.SampleTimeSeconds / 2; + if ~any(baselineMask) + baselineMask = relativeTime < 0; + end + if ~any(baselineMask) + warning('prepare_steering_iddata:MissingBaseline', ... + '%s %s第%d次切换没有切换前基线，已跳过。', ... + sourceFile, wheel.Code, eventNumber); + continue; + end + + inputBaseline = finiteMedian(inputResampled(baselineMask)); + outputBaseline = finiteMedian(outputResampled(baselineMask)); + targetBaseline = finiteMedian(targetResampled(baselineMask)); + inputDelta = inputResampled - inputBaseline; + outputDelta = outputResampled - outputBaseline; + targetDelta = targetResampled - targetBaseline; + + valid = isfinite(relativeTime) & isfinite(inputDelta) & ... + isfinite(outputDelta) & isfinite(targetDelta); + if nnz(valid) < settings.MinimumSegmentSamples + warning('prepare_steering_iddata:InvalidSegment', ... + '%s %s第%d次切换有效样本不足，已跳过。', ... + sourceFile, wheel.Code, eventNumber); + continue; + end + + relativeTime = relativeTime(valid); + inputDelta = inputDelta(valid); + outputDelta = outputDelta(valid); + targetDelta = targetDelta(valid); + + if segmentContainsUnsafeCommand( ... + snapshot, timeOriginMs, commonStart, commonEnd, wheel) + warning('prepare_steering_iddata:UnsafeCommandSegment', ... + ['%s %s第%d次切换出现命令限幅或安全抑制，' ... + '为保持线性辨识条件已跳过。'], ... + sourceFile, wheel.Code, eventNumber); + continue; + end + + experimentName = sprintf('%s_%s_%s_E%02d', ... + datasetCode, transitionType, wheel.Code, eventNumber); + segment = struct(); + segment.Dataset = datasetCode; + segment.Wheel = wheel.Code; + segment.SourceFile = sourceFile; + segment.TransitionType = transitionType; + segment.EventIndex = eventNumber; + segment.ExperimentName = experimentName; + segment.EventTimeSeconds = eventTime; + segment.TargetStepDegrees = targetStep; + segment.Kp = kp; + segment.Ki = ki; + segment.Kd = kd; + segment.DeadZoneDegrees = deadZone; + segment.FeedforwardGain = feedforwardGain; + segment.InputBaselineMps = inputBaseline; + segment.OutputBaselineDegrees = outputBaseline; + segment.TimeSeconds = relativeTime; + segment.UDiffMps = inputDelta; + segment.ActualAngleDeltaDegrees = outputDelta; + segment.TargetAngleDeltaDegrees = targetDelta; + segments(end + 1) = segment; %#ok + end +end + +function [time, value] = compactTimedSignal(time, value) +%COMPACTTIMEDSIGNAL Remove missing data and keep the last value per timestamp. + valid = isfinite(time) & isfinite(value); + time = time(valid); + value = value(valid); + [time, order] = sort(time); + value = value(order); + [time, uniqueIndices] = unique(time, 'last'); + value = value(uniqueIndices); +end + +function eventIndices = suppressNearbyEvents(rawIndices, time, minimumGap) +%SUPPRESSNEARBYEVENTS Treat rapid intermediate target updates as one step. + eventIndices = zeros(0, 1); + for index = 1:numel(rawIndices) + candidate = rawIndices(index); + if isempty(eventIndices) || ... + time(candidate) - time(eventIndices(end)) >= minimumGap + eventIndices(end + 1, 1) = candidate; %#ok + end + end +end + +function unsafe = segmentContainsUnsafeCommand( ... + snapshot, timeOriginMs, startTime, endTime, wheel) +%SEGMENTCONTAINSUNSAFECOMMAND Reject saturated or suppressed responses. + snapshotTime = (snapshot.ElapsedMs - timeOriginMs) / 1000; + inSegment = snapshotTime >= startTime & snapshotTime <= endTime; + unsafe = any(snapshot.WheelCommandSuppressed(inSegment) ~= 0) || ... + any(snapshot.(wheel.LeftLimitedColumn)(inSegment) ~= 0) || ... + any(snapshot.(wheel.RightLimitedColumn)(inSegment) ~= 0) || ... + any(snapshot.(wheel.PairLimitedColumn)(inSegment) ~= 0); +end + +function value = constantParameter(values, parameterName, sourceFile) +%CONSTANTPARAMETER Verify that one experiment did not change configuration. + finiteValues = values(isfinite(values)); + if isempty(finiteValues) + error('prepare_steering_iddata:MissingParameter', ... + '%s没有有效的%s。', sourceFile, parameterName); + end + value = finiteMedian(finiteValues); + tolerance = max(1e-9, abs(value) * 1e-6); + if any(abs(finiteValues - value) > tolerance) + error('prepare_steering_iddata:ChangingParameter', ... + '%s中的%s在记录期间发生变化，不能作为单一实验。', ... + sourceFile, parameterName); + end +end + +function value = finiteMedian(values) +%FINITEMEDIAN Return the median after removing non-finite samples. + values = values(isfinite(values)); + if isempty(values) + value = NaN; + else + value = median(values); + end +end + +function data = buildIdData(segments, wheelCode, inputKind, settings) +%BUILDIDDATA Build one multi-experiment iddata object for one steering module. + selected = segments(strcmp({segments.Wheel}, wheelCode)); + if isempty(selected) + error('prepare_steering_iddata:MissingWheelData', ... + '没有找到%s舵轮的%s数据。', wheelCode, inputKind); + end + + outputs = cell(1, numel(selected)); + inputs = cell(1, numel(selected)); + experimentNames = cell(1, numel(selected)); + for index = 1:numel(selected) + outputs{index} = selected(index).ActualAngleDeltaDegrees(:); + if strcmp(inputKind, 'plant') + inputs{index} = selected(index).UDiffMps(:); + else + inputs{index} = selected(index).TargetAngleDeltaDegrees(:); + end + experimentNames{index} = selected(index).ExperimentName; + end + + data = iddata(outputs, inputs, settings.SampleTimeSeconds); + data.ExperimentName = experimentNames; + data.OutputName = {'actualSteeringAngleDelta'}; + data.OutputUnit = {'deg'}; + data.TimeUnit = 'seconds'; + if strcmp(inputKind, 'plant') + data.Name = sprintf('%s steering plant: uDiff to angle', wheelCode); + data.InputName = {'uDiff'}; + data.InputUnit = {'m/s'}; + else + data.Name = sprintf('%s existing closed loop: target to angle', wheelCode); + data.InputName = {'targetSteeringAngleDelta'}; + data.InputUnit = {'deg'}; + end +end + +function metadata = buildMetadataTable(segments) +%BUILDMETADATATABLE Create one row of traceability data per step response. + count = numel(segments); + dataset = cell(count, 1); + wheel = cell(count, 1); + sourceFile = cell(count, 1); + transitionType = cell(count, 1); + experimentName = cell(count, 1); + eventIndex = zeros(count, 1); + eventTimeSeconds = zeros(count, 1); + targetStepDegrees = zeros(count, 1); + kp = zeros(count, 1); + ki = zeros(count, 1); + kd = zeros(count, 1); + deadZoneDegrees = zeros(count, 1); + feedforwardGain = zeros(count, 1); + inputBaselineMps = zeros(count, 1); + outputBaselineDegrees = zeros(count, 1); + sampleCount = zeros(count, 1); + + for index = 1:count + segment = segments(index); + dataset{index} = segment.Dataset; + wheel{index} = segment.Wheel; + sourceFile{index} = segment.SourceFile; + transitionType{index} = segment.TransitionType; + experimentName{index} = segment.ExperimentName; + eventIndex(index) = segment.EventIndex; + eventTimeSeconds(index) = segment.EventTimeSeconds; + targetStepDegrees(index) = segment.TargetStepDegrees; + kp(index) = segment.Kp; + ki(index) = segment.Ki; + kd(index) = segment.Kd; + deadZoneDegrees(index) = segment.DeadZoneDegrees; + feedforwardGain(index) = segment.FeedforwardGain; + inputBaselineMps(index) = segment.InputBaselineMps; + outputBaselineDegrees(index) = segment.OutputBaselineDegrees; + sampleCount(index) = numel(segment.TimeSeconds); + end + + metadata = table( ... + dataset, wheel, sourceFile, transitionType, experimentName, ... + eventIndex, eventTimeSeconds, targetStepDegrees, ... + kp, ki, kd, deadZoneDegrees, feedforwardGain, ... + inputBaselineMps, outputBaselineDegrees, sampleCount, ... + 'VariableNames', { ... + 'Dataset', 'Wheel', 'SourceFile', 'TransitionType', ... + 'ExperimentName', 'EventIndex', 'EventTimeSeconds', ... + 'TargetStepDegrees', 'Kp', 'Ki', 'Kd', ... + 'DeadZoneDegrees', 'FeedforwardGain', ... + 'InputBaselineMps', 'OutputBaselineDegrees', 'SampleCount'}); +end + +function samples = buildSampleTable(segments) +%BUILDSAMPLETABLE Export a tidy long-form table for manual inspection. + totalSamples = sum(arrayfun( ... + @(segment) numel(segment.TimeSeconds), segments)); + dataset = cell(totalSamples, 1); + wheel = cell(totalSamples, 1); + experimentName = cell(totalSamples, 1); + sourceFile = cell(totalSamples, 1); + transitionType = cell(totalSamples, 1); + eventIndex = zeros(totalSamples, 1); + timeSeconds = zeros(totalSamples, 1); + uDiffMps = zeros(totalSamples, 1); + actualAngleDeltaDegrees = zeros(totalSamples, 1); + targetAngleDeltaDegrees = zeros(totalSamples, 1); + kp = zeros(totalSamples, 1); + deadZoneDegrees = zeros(totalSamples, 1); + + firstRow = 1; + for index = 1:numel(segments) + segment = segments(index); + count = numel(segment.TimeSeconds); + rows = (firstRow:(firstRow + count - 1))'; + dataset(rows) = repmat({segment.Dataset}, count, 1); + wheel(rows) = repmat({segment.Wheel}, count, 1); + experimentName(rows) = repmat( ... + {segment.ExperimentName}, count, 1); + sourceFile(rows) = repmat({segment.SourceFile}, count, 1); + transitionType(rows) = repmat( ... + {segment.TransitionType}, count, 1); + eventIndex(rows) = segment.EventIndex; + timeSeconds(rows) = segment.TimeSeconds; + uDiffMps(rows) = segment.UDiffMps; + actualAngleDeltaDegrees(rows) = segment.ActualAngleDeltaDegrees; + targetAngleDeltaDegrees(rows) = segment.TargetAngleDeltaDegrees; + kp(rows) = segment.Kp; + deadZoneDegrees(rows) = segment.DeadZoneDegrees; + firstRow = firstRow + count; + end + + samples = table( ... + dataset, wheel, experimentName, sourceFile, transitionType, ... + eventIndex, timeSeconds, uDiffMps, ... + actualAngleDeltaDegrees, targetAngleDeltaDegrees, ... + kp, deadZoneDegrees, ... + 'VariableNames', { ... + 'Dataset', 'Wheel', 'ExperimentName', 'SourceFile', ... + 'TransitionType', 'EventIndex', 'TimeSeconds', ... + 'UDiffMps', 'ActualAngleDeltaDegrees', ... + 'TargetAngleDeltaDegrees', 'Kp', 'DeadZoneDegrees'}); +end + +function segments = emptySegmentArray() +%EMPTYSEGMENTARRAY Return an empty struct with the stable segment schema. + template = struct( ... + 'Dataset', '', ... + 'Wheel', '', ... + 'SourceFile', '', ... + 'TransitionType', '', ... + 'EventIndex', 0, ... + 'ExperimentName', '', ... + 'EventTimeSeconds', 0, ... + 'TargetStepDegrees', 0, ... + 'Kp', 0, ... + 'Ki', 0, ... + 'Kd', 0, ... + 'DeadZoneDegrees', 0, ... + 'FeedforwardGain', 0, ... + 'InputBaselineMps', 0, ... + 'OutputBaselineDegrees', 0, ... + 'TimeSeconds', zeros(0, 1), ... + 'UDiffMps', zeros(0, 1), ... + 'ActualAngleDeltaDegrees', zeros(0, 1), ... + 'TargetAngleDeltaDegrees', zeros(0, 1)); + segments = template([]); +end + +function printSegmentCounts(estimationSegments, validationSegments, wheels) +%PRINTSEGMENTCOUNTS Report the experiment count for every output iddata pair. + for wheelIndex = 1:numel(wheels) + wheelCode = wheels(wheelIndex).Code; + estimationCount = nnz(strcmp( ... + {estimationSegments.Wheel}, wheelCode)); + validationCount = nnz(strcmp( ... + {validationSegments.Wheel}, wheelCode)); + fprintf(' %s：估计%d段，验证%d段\n', ... + wheelCode, estimationCount, validationCount); + end +end + +function requireFolder(folder) +%REQUIREFOLDER Fail early when the expected experiment folder is absent. + if ~exist(folder, 'dir') + error('prepare_steering_iddata:MissingFolder', ... + '找不到实验目录：%s', folder); + end +end diff --git a/docs/decisions.md b/docs/decisions.md index 4e011f8..48a1255 100644 --- a/docs/decisions.md +++ b/docs/decisions.md @@ -72,7 +72,8 @@ ### 10. 对已观察的舵轮响应加入有限前馈与预瞄 -- M层差速转舵角速度前馈已接入，当前默认增益0.9、速度上限0.03m/s；实现位于 `MotorRoutine.CalculateDiffSteerRateFeedforward()`。 +- M层差速转舵已实施四轮独立的到位迟滞与模型逆前馈：默认回退参数为公共纯P `Kp=0.0042`，到位逻辑为0.5°停止区、0.8°重启区、连续3周期确认；对象逆前馈按 `targetRate/687.06` 计算并经0.15s一阶滤波，独立限幅0.03m/s。两项功能各有默认关闭的独立开关，实车参考配置可显式启用；状态不在四轮之间共享。实现位于 `DiverCartDefinition`、`MotorRoutine.CalculateDiffSteerRateFeedforward()` 和 `UpdateDiffSteerSettlingState()`。 +- 旧 `DiffSteerRateFeedforwardGain` 不再作为模型逆前馈的物理参数；PID反馈与前馈分别限幅后相加，合成差速不再受PID的±0.3m/s阈值二次截断，最终八电机仍由M层既有发送阈值独立保护。 - Stanley曲率前馈预瞄已接入，当前车辆默认时间0.15s、最大距离0.12m。 - 两者均是可配置补偿，不替代底层PID、真实周期和机械响应验证。 @@ -125,10 +126,17 @@ - 主车 `FleetSafetySupervisor` 依据主车本地接收时间、成员状态和故障锁存整队停车决定；每辆车的 `FleetMemberAgent` 依据本机单调时间和命令有效期独立看门狗停车。`FleetRuntime` 已执行本车停止并向可达成员下发停止，两层保护仍不能互相替代。 - 同一 `FleetRuntime` 部署到所有车辆，角色由显式 `selfVehicleId` 与 `leaderVehicleId` 决定，不把固定车号硬编码为主车；主车也是成员，本车命令直接执行，不要求传输层回环。 +### 17. 纵向辨识区分目标命令、实际发送命令和停车语义 + +- C层 `LongitudinalIdentificationTests.cs` 提供“速度阶跃与立即停车”和“速度阶跃与正常减速”两个入口，共用舵轮回正、定时命令、CSV记录、停止响应和异常停车流程，不经过Stanley或纵向PID。 +- 初次辨识保持生产配置中的 `AccPerSecond/DeAccPerSecond` 固定；上层目标速度只作为参考，车辆对象辨识以M层最终 `SentSpeed*` 为输入、轮组反馈合成速度为输出，避免把已知速度斜坡误认为车辆本体迟滞。 +- `SendBodyTwist(Twist2D.Zero)` 表示立即停车并绕过 `DeAccPerSecond`；正常减速实验必须持续发送零目标 `SendMotion`，让底盘内部发送速度按 `DeAccPerSecond` 逐周期下降。两类数据不能混为同一种制动响应。 +- 第一轮先完成当前配置的正向重复实验，再依据数据决定是否补充倒车和加减速度参数A/B；辨识过程中不同时改变多个限幅或控制参数。 + ## 已经确认但尚未实施 - 路线顺序：先完成单车闭环和停车功能验证，再正式实施多车通信、编队和协同控制。来源：`README.md`。 -- 转舵系统辨识应对四个轮组分别使用相同激励，输入采用实际差速转舵命令（`TotalDiff`或左右轮实际发送命令之差），输出采用实际舵角；若只用目标角到实际角，会把当前PID包含在闭环模型中，更换PID后该模型不能继续代表转舵机构。控制阶段优先保留四个独立PID实例和内部状态、共用一套参数，并依据四个健康轮组中最不利的动态设计稳定裕量；只有硬件健康且公共参数仍无法兼顾时，才评估配置化的逐轮小幅校准，不在代码中硬编码某个轮位特例。 +- 转舵系统辨识应对四个轮组分别使用相同激励，输入采用实际差速转舵命令（`TotalDiff`或左右轮实际发送命令之差），输出采用实际舵角；若只用目标角到实际角，会把当前PID包含在闭环模型中，更换PID后该模型不能继续代表转舵机构。当前主辨识流程固定用第一次实验作估计集、第三次实验作验证集，两者角度死区均为0.1°；第二次0.5°死区实验不进入主辨识，只保留作死区敏感性参考。控制阶段优先保留四个独立PID实例和内部状态、共用一套参数，并依据四个健康轮组中最不利的动态设计稳定裕量；只有硬件健康且公共参数仍无法兼顾时，才评估配置化的逐轮小幅校准，不在代码中硬编码某个轮位特例。当前15°/s参数由 `ParkingMaximumGcpAngleRateDegreesPerSecond` 配置并在 `GcpCommandExecutor` 中限制前后虚拟GCP目标，位于上述辨识对象上游，不进入“实际差速命令→实际舵角”模型；实施顺序确定为四轮对象辨识、公共反馈/前馈设计、舵角阶跃与斜坡验证、按最慢健康轮组留裕量设置GCP变化率，最后再用轨迹实验整定曲率预瞄和横向控制参数，不删除GCP变化率保护。 - 当前多车的固定布局滚动任务已形成内存通信可运行链路。仍缺少C层正式动作入口、布局原子激活、成员状态实际采集、跨机时间换算、公共世界坐标验证和无线传输实现。 - 单车 `MultiWheelC/StateEstimation` 继续负责Detour重复帧、跳变候选、轮速短时预测和任务坐标连续化；车队层不复制这套原始定位处理，只消费经过本车校验的成员状态，并负责跨车时间对齐、固定布局反算、成员一致性检查和中心融合。成员状态进入融合前仍必须确认处于同一公共坐标系；各车独立的 `_controlFromDetour` 连续化变换是否会造成跨车基准差异，属于通信/状态接收契约必须验证的事项。 - 第一版保留当前保守的等权车队中心融合；任务生命周期和内存零速运行链已经贯通。公共坐标系与主车时间轴语义确认并取得静止/低速双车日志后，再按数据增加车队历史预测、逐成员创新门控、健康降级和Huber等鲁棒加权；不在缺少Detour协方差时提前实现协方差加权或Covariance Intersection。 diff --git a/docs/interfaces.md b/docs/interfaces.md index fc969f7..e21dbb1 100644 --- a/docs/interfaces.md +++ b/docs/interfaces.md @@ -218,12 +218,15 @@ Vrear = (Vx, Vy - ωR) 输入是车体中心有符号平移速度模长、当前运动系中的虚拟前后GCP方向。前后GCP法线交点确定ICR；每个真实轮子使用其运动系位置计算切线舵角和半径速度比例。机械限位通过等效舵角/反向轮速解析，无法满足时返回失败原因。 +`SendMotion` 对每个轮子的目标速度应用 `AccPerSecond/DeAccPerSecond` 斜坡；加速或减速的判据是目标速度绝对值相对当前发送速度绝对值增大或减小。`PredefinedDriveStop()` 直接清零发送速度，不经过该减速斜坡。 + ## 配置接口 - `PilotConfig` 是Clumsy运行配置模型；`[FieldMember]` 字段提供默认值和宿主显示/持久化元数据。 - `PilotDefinition.Conf` 是C层动作实际读取的运行配置对象。 - `MultiWheelC/Configuration/PilotConfig.ParkingControl.cs` 集中停车状态估计、Stanley、纵向PID、原地自转、舵轮准备、GCP与完成条件参数。 - 动作中的 nullable 覆盖字段用于特定动作段或测试；为空时使用车辆配置。车辆级限速和通用控制参数不应在普通实验中随意覆盖。 +- 多舵轮底盘几何和 `MaxSpeed/AccPerSecond/DeAccPerSecond` 由 `MultiWheelChassisInitializer` 从宿主工作目录相对路径 `../chassis.json` 加载，并覆盖 `AbstractChassis` 的字段默认值；启动输出会打印最终加载值。当前实车参考副本 `参考文档/chassis参考.json` 为 `AccPerSecond=0.3m/s²`、`DeAccPerSecond=1.0m/s²`，但只有部署端实际命名和路径正确的 `chassis.json` 会影响运行。 - `参考文档/*.json` 是样例/实车复制资料；当前源码未发现这些JSON被 `PilotDefinition.Conf` 自动读取的入口。 运行时配置文件格式、位置和覆盖优先级由外部Clumsy宿主决定，当前仓库内待确认。 @@ -232,6 +235,8 @@ Vrear = (Vx, Vy - ωR) - C层 `PilotDefinition` 和M层 `DiverCartDefinition` 使用 `[AsUpperIO]`/`[AsLowerIO]` 对齐夹臂命令、驱动使能、位置反馈和车号等字段。 - M层 `WheelSpeedDiagnosticLogger` 每次记录生成 `_can.csv` 和 `_snapshot.csv`：前者按CAN回调时刻保存八电机速度/位置与四舵角事件，后者按最多50Hz保存目标/实际舵角、目标角速度、PID/前馈/合成差速、限幅前后电机命令、速度/位置/电流反馈及各事件的本机单调时间、序号和数据年龄。MATLAB辨识时可使用 `TargetTh` 作为参考、`TotalDiff` 或 `Sent*` 作为执行输入、`ActualTh` 作为输出；左右轮公共/差速通道由方向统一后的左右命令和反馈在分析侧组合。 +- C层纵向辨识复用 `TrackingExperimentRecorder` 保存目标 `CommandVx`、Detour位姿和轮组原始/滤波速度；M层诊断中的 `SentSpeed*` 才是经过底盘速度斜坡与最终限幅后的执行输入。正式辨识应一组C层CSV对应一组M层CSV，并以首个非零发送命令边沿对齐两套本机时间。 +- `DiffSteerThresh` 通过 `PIDController.ChangeParameters()` 设置差速转舵PID的对称输出上限：例如配置0.3时，PID反馈项限制在[-0.3,+0.3]m/s。`MotorRoutine.UpdateDiffSteerWheelSpeeds()` 先将该反馈项与独立限幅的角速度前馈相加，再按左轮减、右轮加叠加到基础轮速；合成差速不会再次受 `DiffSteerThresh` 限制。最终八个单轮命令还会在 `MCURoutine` 中分别受 `SendThresSpeed` 限制。因此当前前馈增益为0、基础轮速为0且 `SendThresSpeed` 不低于0.3m/s的静止模式切换实验中，配置 `DiffSteerThresh=0.3` 可视为实际转舵差速的±0.3m/s上限；车辆行驶或重新启用前馈时，不能把它误当作每个电机最终总速度的唯一上限。 - `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/problems.md b/docs/problems.md index 48f0547..ccb1a10 100644 --- a/docs/problems.md +++ b/docs/problems.md @@ -107,9 +107,14 @@ ### 控制周期和舵轮响应 - 代码已经记录控制周期分段耗时、请求/限速后GCP命令和四舵角；M层已有轮速/舵角诊断CSV。 -- 差速转舵角速度前馈默认增益0.9，曲率预瞄默认0.15s/0.12m。 +- 差速转舵角速度前馈实现仍保留0.03m/s独立限幅，但当前源码默认增益为0，作为对象辨识基线；曲率预瞄默认0.15s/0.12m。 +- 历史轨迹实验所称“约0.46s延迟”是 `TargetTh` 与 `ActualTh` 连续曲线的最佳时间平移量，反映当时工况下的等效跟随滞后，并非命令下发后等待0.46s才开始转动的纯死区时间，也不是任意目标角的固定到位时间。它与旧纯P近似模型约0.49s的90%响应时间数值接近，但两者指标不能等同；后续应分别报告目标变化至首次运动的启动时间、10%～90%上升时间、进入规定角差的稳定时间和连续曲线相位滞后，并在控制器或工况改变后重新测量。 - 2026-08-19第六轮共9131个轨迹控制周期：周期中位数约31.24ms、P95约32.68ms、最大约59.44ms；控制计算总耗时中位数约0.12ms、P95约0.22ms。当前C层计算不是主要周期瓶颈，历史约110ms现象不应继续归因于控制算法计算量。 - 2026-08-25静止诊断确认：目标角和角速度前馈均为零时，右后轮仍可形成约1s量级的持续差速转舵往复；死区由0.1°增至0.5°后，另外三轮均停止输出，但一次自转模式切回正常模式后的右后轮仍在约-1.9°至+2.9°间振荡。约2°的瞬态偏差本身不能证明硬件故障，待定位对象是仅该轮不收敛的闭环动态差异；应先用降低公共比例增益的重复切换实验区分控制稳定裕量，再对四轮分别辨识延迟、增益、摩擦和方向不对称，明显离群轮组先排查编码器、机械间隙和低速驱动响应。 +- 舵轮辨识数据共有三次物理实验：第一次为Kp=0.0035、死区0.1°、前馈0、约49.6V；第二次为Kp=0.003、死区0.5°、前馈0、约49.5～49.6V，因Kp和死区同时变化而排除在主辨识之外；第三次为重新采集的Kp=0.003、死区0.1°、前馈0，电压中位数约52.0V、范围约51.3～54.7V。`data_process/系统辨识实验-舵轮延迟/prepare_steering_iddata.m` 当前明确以第一次为Est、第三次为Val，因此“第二个处理批次”是第三次物理实验，不能与第二次0.5°实验混称。第一次与第三次统一了死区但电压不同，不是严格的仅Kp变化A/B实验，验证结果需要同时考虑供电、负载和机械状态差异。 +- 第三次验证数据中RR、尤其RRL通道被初步观察到低速跟随偏弱、电流升高及随后突发运动；该现象尚需用原始snapshot与CAN事件时间线复核。在复核前不应用该轮的异常验证结果调整全车公共PID，也不应据此否定LF/LR/RF的正常辨识结果。 +- 2026-08-27静止模式切换A/B表明，在公共 `Kp=0.0042` 不变时，同时启用0.5°/0.8°/3周期到位迟滞和0.15s模型逆前馈后，约69°、90°和20.85°动作的 `t90` 分别约缩短17%、18%和44%，进入±2°时间约缩短22%、19%和45%，最大超调也有所下降；当前数据缺少“只启用迟滞、关闭前馈”组，因此不能把总改善严格拆分到单项功能。 +- 同批日志显示M层差速转舵周期中位数约49.6ms，3个稳定周期在实车上约150ms而不是按75ms估算的225ms；到位停止区使末端误差中位数上升到约0.4°，并仍存在少量轮组0.8～1.6°未完全收敛样本，后续参数调整应以重复实车数据为依据。 - 不同速度、载荷下的舵轮物理响应和前馈参数仍需按具体工况验证。 - CAN/MCU正常运行逻辑风险较高；除诊断外不应在没有明确方案和实车回退措施时修改。 diff --git a/docs/progress.md b/docs/progress.md index 6ffb9b8..c668da4 100644 --- a/docs/progress.md +++ b/docs/progress.md @@ -1,6 +1,6 @@ # 当前进展 -更新日期：2026-08-25。这里只保存当前状态，不作为完整开发历史。 +更新日期：2026-08-27。这里只保存当前状态，不作为完整开发历史。 ## 已完成/已接入 @@ -16,6 +16,8 @@ - Detour单帧航向异常已增加连续帧/短时预测确认；第六轮实车数据确认前两帧异常由轮组Vw预测承接，持续到第三个异常新帧时才安全终止。 - 原地自转已增加 `RelativeWheelOdometry` 模式和不依赖Detour的 `TryGetWheelTwist()` 接口；第七轮19份记录中17份表现为正常停车回正，轮组积分与Detour原始航向变化绝对差中位数约0.89°、最大约3.07°，但目标角精度尚缺专用字段正式验收。 - C层轨迹/周期CSV与M层轮速/舵角诊断记录。 +- M层差速转舵已加入默认关闭、可独立回退的四轮到位迟滞和模型逆前馈；当前实车参考配置采用公共纯P `Kp=0.0042`、0.5°/0.8°/3周期到位判断及 `687.06/0.15s/0.03m/s` 逆前馈参数，首轮模式切换A/B已确认响应时间和超调整体改善。 +- C层已新增两个不经过轨迹控制器的纵向辨识入口：速度阶跃后立即停车，以及速度阶跃后按 `DeAccPerSecond` 正常减速；复用现有C层CSV并要求同步开启M层 `SentSpeed*` 诊断。 - C层Detour静态诊断入口，记录源时间、`l_step`、位姿帧差和轮组静态反馈；已有约14分41秒实车静态基线。 - C层轨迹CSV已补充原始/滤波轮组 `Vx/Vy/Vw`、轮组与预测时刻、Detour帧间隔、实际/允许创新、候选原因、估计器状态和不可用原因。 - 停车控制参数集中到 `Configuration/PilotConfig.ParkingControl.cs`。 @@ -36,6 +38,7 @@ - 多车共同搬运的固定布局滚动任务已在内存传输下完成端到端贯通。当前重点转为真实无线链路和工程接入：确认无线参数与帧格式，实现串口 `IFleetTransport`，接入公共坐标系成员状态和主车时间轴，再通过静止状态机与低速双车逐级验证。QP/HQP不作为第一版前置条件。 - 车队状态估计第一版保留“成员状态先经单车状态估计处理、车队层做时间对齐与刚体一致性检查、候选中心等权/圆周平均”的保守方案；暂不重复实现单车Detour跳变逻辑，也不在运行链和实车数据建立前加入复杂鲁棒优化。后续内部融合升级应尽量保持 `FleetStateEstimateResult`、`FleetState` 和 `MemberErrors` 外部接口不变。 - β在多车中定位为单车执行坐标系而非车队核心优化量：常规、斜行和横移动作先确定车队主要滚动方向，各车按布局朝向换算本地β，停车预对齐并经车队同步屏障统一释放。轨迹级β搜索只作为机械余量或复杂方向变化下的后续增强。 +- 当前单车实验工作转入纵向系统辨识：先固定实车 `AccPerSecond=0.3m/s²`、`DeAccPerSecond=1.0m/s²`，以M层最终发送速度为输入、轮组反馈速度为输出，取得可重复模型后再调整纵向控制、启动/停车策略和加减速度参数；横向辨识随后进行。 ## 阻塞/待确认 @@ -60,3 +63,4 @@ 7. 接入C层正式车队动作入口、成员状态实际来源和主车接收时间轴，保证 `FleetMemberReport` 位姿处于同一公共世界坐标系，远端采样时间已换算为主车时间后再形成 `FleetMemberStateSample[]`。 8. 先验证静止布局建立、β准备、全员Ready、统一激活和停止，再进行空载低速双车直线与圆弧；共同搬运前补齐夹紧/报警输入和可立即停车措施。 9. 记录各成员候选中心、数据年龄、状态来源、布局误差和减速/停车原因。只有数据证明当前等权融合频繁被单成员异常拖累时，再增加车队预测、逐成员门控和鲁棒加权。 +10. 先对纵向辨识两个入口各做一次0.05m/s低速流程验证，再在0.1/0.2/0.3/0.4m/s下采集当前固定配置的重复实验；每次单独保存C/M两份CSV，确认正向模型后再决定倒车补测和参数A/B。 diff --git a/output/C/CommonUsage.dll b/output/C/CommonUsage.dll index d284e2e..280d7db 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 666bccd..1aadbae 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 d62166c..121733b 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 d284e2e..280d7db 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 6a3cd4b..3d46cf9 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 dc4c9c6..4b8b4cb 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 d284e2e..280d7db 100644 Binary files a/ref/CommonUsage.dll and b/ref/CommonUsage.dll differ diff --git a/参考文档/cartparams.json b/参考文档/cartparams.json index 3ef5a58..a8f33a2 100644 --- a/参考文档/cartparams.json +++ b/参考文档/cartparams.json @@ -21,13 +21,22 @@ "MaxManualTheta": 45.0, "IsDiffSteer": true, "ManualThetaPow": 2.0, - "DiffSteerKp": 0.003, + "DiffSteerKp": 0.0042, "DiffSteerKi": 0.0, "DiffSteerKd": 0.0, "DiffSteerMaxI": 0.0, "DiffSteerThresh": 0.3, "DiffSteerDeadZone": 0.1, "DiffSteerSpeedAcc": 1.0, + "DiffSteerRateFeedforwardGain": 0.0, + "DiffSteerRateFeedforwardMaximumSpeed": 0.03, + "EnableDiffSteerSettlingHysteresis": true, + "DiffSteerStopErrorDegrees": 0.5, + "DiffSteerRestartErrorDegrees": 0.8, + "DiffSteerSettlingCycles": 3, + "EnableDiffSteerInverseFeedforward": true, + "DiffSteerPlantGain": 687.06, + "DiffSteerInverseFeedforwardTimeConstantSeconds": 0.15, "Ghost_ActualSpeedLeftFront_GhostPIDUpdater_kp": 0.5, "Ghost_ActualSpeedLeftFront_GhostPIDUpdater_threshold": 200.0, "Ghost_ActualSpeedLeftRear_GhostPIDUpdater_kp": 0.5, diff --git a/参考文档/辨识.txt b/参考文档/辨识.txt index 48fa3de..142a6c7 100644 --- a/参考文档/辨识.txt +++ b/参考文档/辨识.txt @@ -1,129 +1 @@ -对，当前日志足够做分层建模，但物理因果顺序要明确： - -```text -目标舵角 TargetTh - ↓ -舵角PID/前馈控制器 - ↓ -左右电机实际下发命令 SentLeft / SentRight - ↓ -左右电机实际速度 ActualLeft / ActualRight - ↓ -左右轮差速产生舵轮转动 - ↓ -实际舵角 ActualTh -``` - -你可以建立以下几层模型。 - -1. 单个电机响应模型 - -分别辨识： - -```text -SentLeft → ActualLeft -SentRight → ActualRight -``` - -例如左前舵轮： - -```matlab -G_LFL = ActualLFL / SentLFLMps; -G_LFR = ActualLFR / SentLFRMps; -``` - -这可以得到左右电机各自的延迟、时间常数和增益，判断两个电机是否同步。 - -2. 差速轮对模型 - -计算： - -```matlab -uDiff = (SentRightMps - SentLeftMps) / 2; -vDiff = (ActualRight - ActualLeft) / 2; -``` - -辨识： - -```text -uDiff → vDiff -``` - -它表示控制器给出的差速命令，到两个电机真正形成差速运动的过程。 - -3. 差速运动到舵角模型 - -辨识： - -```text -vDiff → ActualTh -``` - -理论运动学关系是： - -\[ -\dot{\theta}=\frac{v_R-v_L}{d} -=\frac{2v_{\mathrm{diff}}}{d} -\] - -所以这一层理论上主要是积分环节： - -\[ -\theta(s)=\frac{2}{d\,s}v_{\mathrm{diff}}(s) -\] - -实际数据中还会包含摩擦、轮胎滑动、机械间隙和舵角反馈延迟。 - -4. 整体舵轮对象模型 - -直接辨识： - -```text -uDiff → ActualTh -``` - -这一模型包含： - -- 两个电机的响应延迟 -- 左右电机不同步 -- 差速转舵运动学 -- 摩擦和机械滞后 -- 舵角传感器延迟 - -这是后续设计舵角PID最有用的模型。 - -5. 完整闭环模型 - -还可以辨识： - -```text -TargetTh → ActualTh -``` - -它包含当前PID参数，适合评价现有系统的上升时间、超调和稳定时间，但不适合单独用来重新设计PID对象。 - -因此建议在Simulink中建立： - -```text -TargetTh - ↓ -Controller - ↓ -Motor Pair Dynamics - ↓ -Differential Steering Mechanics - ↓ -ActualTh - └────────反馈────────┘ -``` - -第一版控制率设计优先使用： - -```text -输入：uDiff -输出：ActualTh -``` - -单电机模型用于解释左右不同步和优化补偿。 - -需要注意：模式切换时主要激励的是差速通道，`uCommon≈0`。所以现有三组实验足够辨识转舵过程，但不能完整辨识车轮公共滚动速度对舵角的干扰。以后如果要研究车辆自转运行中的舵角漂移，还需补一次低速公共轮速实验。 \ No newline at end of file +横纵向动力学分开 \ No newline at end of file