diff --git a/MultiWheelC/Control/Execution/ParkingGeometricController.cs b/MultiWheelC/Control/Execution/ParkingGeometricController.cs index 7c4f944..997c971 100644 --- a/MultiWheelC/Control/Execution/ParkingGeometricController.cs +++ b/MultiWheelC/Control/Execution/ParkingGeometricController.cs @@ -28,7 +28,7 @@ namespace MultiWheelC.Control.Execution 1e-6; private const double StartupRegionMeters = 0.02; private const double StartupPreviewDistanceMeters = 0.05; - private const double MaximumStartupSpeedMetersPerSecond = 0.05; + private const double MaximumStartupSpeedMetersPerSecond = 0.08; private readonly IVehicleStateProvider _stateProvider; private readonly ILateralController _lateralController; diff --git a/MultiWheelC/Control/Longitudinal/PidLongitudinalController.cs b/MultiWheelC/Control/Longitudinal/PidLongitudinalController.cs index ab5f957..ecf61f0 100644 --- a/MultiWheelC/Control/Longitudinal/PidLongitudinalController.cs +++ b/MultiWheelC/Control/Longitudinal/PidLongitudinalController.cs @@ -23,11 +23,15 @@ namespace MultiWheelC.Control.Longitudinal double integralGainPerSecond, double derivativeGainSeconds, double maximumIntegralCorrectionMetersPerSecond, - double maximumCommandSpeedMetersPerSecond) + double maximumCommandSpeedMetersPerSecond, + double speedErrorDeadbandMetersPerSecond = 0.025) { EnsureFinitePositive( maximumCommandSpeedMetersPerSecond, nameof(maximumCommandSpeedMetersPerSecond)); + EnsureFiniteNonNegative( + speedErrorDeadbandMetersPerSecond, + nameof(speedErrorDeadbandMetersPerSecond)); _feedbackPid = new PidController( proportionalGain, @@ -37,6 +41,8 @@ namespace MultiWheelC.Control.Longitudinal derivativeOnMeasurement: true); MaximumCommandSpeedMetersPerSecond = maximumCommandSpeedMetersPerSecond; + SpeedErrorDeadbandMetersPerSecond = + speedErrorDeadbandMetersPerSecond; } /// @@ -49,6 +55,11 @@ namespace MultiWheelC.Control.Longitudinal /// public double MaximumCommandSpeedMetersPerSecond { get; } + /// + /// 获取不触发纵向PID修正的速度误差死区，单位为m/s。 + /// + public double SpeedErrorDeadbandMetersPerSecond { get; } + /// /// 获取最近一次有效控制周期的参考速度减实际速度，单位为m/s。 /// @@ -98,6 +109,20 @@ namespace MultiWheelC.Control.Longitudinal referenceSpeedMetersPerSecond); } + var speedErrorMetersPerSecond = + referenceSpeedMetersPerSecond - + context.ActualLongitudinalSpeedMetersPerSecond; + + // Detour差分速度在参考速度附近会有小幅波动；死区内只使用速度前馈， + // 同时清除PID历史，避免噪声持续积累后产生突发修正。 + if (Math.Abs(speedErrorMetersPerSecond) <= + SpeedErrorDeadbandMetersPerSecond) + { + Reset(); + return LimitReferenceSpeed( + referenceSpeedMetersPerSecond); + } + GetCorrectionOutputRange( referenceSpeedMetersPerSecond, out var minimumCorrectionMetersPerSecond, @@ -178,5 +203,22 @@ namespace MultiWheelC.Control.Longitudinal "纵向控制器最大命令速度必须是正有限值。"); } } + + /// + /// 检查速度误差死区是否为非负有限值。 + /// + private static void EnsureFiniteNonNegative( + double value, + string parameterName) + { + if (double.IsNaN(value) || + double.IsInfinity(value) || + value < 0.0) + { + throw new ArgumentOutOfRangeException( + parameterName, + "纵向控制器速度误差死区必须是非负有限值。"); + } + } } } diff --git a/MultiWheelC/Experiments/NewControllerTrackingTests.cs b/MultiWheelC/Experiments/NewControllerTrackingTests.cs index 559f71d..8c47315 100644 --- a/MultiWheelC/Experiments/NewControllerTrackingTests.cs +++ b/MultiWheelC/Experiments/NewControllerTrackingTests.cs @@ -330,17 +330,17 @@ namespace MultiWheelC /// /// 获取或设置直线与等曲率转弯之间的曲率过渡长度，单位为m。 /// - public double CurvatureTransitionLengthMeters = 0.60; + public double CurvatureTransitionLengthMeters = 0.70; /// /// 获取或设置两段直线的最大参考速度，单位为m/s。 /// - public double StraightMaximumSpeedMetersPerSecond = 0.30; + public double StraightMaximumSpeedMetersPerSecond = 0.40; /// /// 获取或设置半圆段的最大参考速度，单位为m/s。 /// - public double SemicircleMaximumSpeedMetersPerSecond = 0.25; + public double SemicircleMaximumSpeedMetersPerSecond = 0.30; /// /// 获取或设置参考速度加速度，单位为m/s²。 diff --git a/MultiWheelC/Experiments/RotationTests.cs b/MultiWheelC/Experiments/RotationTests.cs index 56f6b4e..08b3a45 100644 --- a/MultiWheelC/Experiments/RotationTests.cs +++ b/MultiWheelC/Experiments/RotationTests.cs @@ -1,6 +1,7 @@ using System; +using System.Globalization; using System.Numerics; using System.Threading; using ClumsyCore; @@ -17,7 +18,6 @@ namespace MultiWheelC public abstract class InPlaceRotateTestBase : MovementTest { public float RelativeAngleDegrees; // 相对当前航向的旋转角度，逆时针为正。 - public float MaxAngularSpeedDegreesPerSecond = 20f; // PID输出的最大角速度。 public int TrialNumber = 1; // 重复实验编号。 private DriveTask _task; @@ -37,11 +37,18 @@ namespace MultiWheelC // 从当前Detour航向开始，原地相对旋转指定角度并记录实验数据。 public override void Test() { + var config = PilotDefinition.Conf; + if (float.IsNaN(RelativeAngleDegrees) || float.IsInfinity(RelativeAngleDegrees) || - float.IsNaN(MaxAngularSpeedDegreesPerSecond) || - float.IsInfinity(MaxAngularSpeedDegreesPerSecond) || - MaxAngularSpeedDegreesPerSecond <= 0f) + float.IsNaN(config.InPlaceRotateMaxSpeed) || + float.IsInfinity(config.InPlaceRotateMaxSpeed) || + config.InPlaceRotateMaxSpeed <= 0f || + float.IsNaN(config.InPlaceRotateMinimumSpeed) || + float.IsInfinity(config.InPlaceRotateMinimumSpeed) || + config.InPlaceRotateMinimumSpeed <= 0f || + config.InPlaceRotateMinimumSpeed > + config.InPlaceRotateMaxSpeed) { Console.WriteLine("原地旋转测试参数无效。"); return; @@ -66,8 +73,25 @@ namespace MultiWheelC (float)AngleMath.NormalizeDegrees( location.th + RelativeAngleDegrees); + Console.WriteLine( + "原地自转实际参数：" + + $"Kp={config.InPlaceRotateKp:F3}，" + + $"Ki={config.InPlaceRotateKi:F3}，" + + $"Kd={config.InPlaceRotateKd:F3}，" + + $"到位误差={config.InPlaceRotateArriveDeg:F2}°，" + + $"最小角速度={config.InPlaceRotateMinimumSpeed:F2}°/s，" + + $"最大角速度={config.InPlaceRotateMaxSpeed:F2}°/s，" + + $"角加速度={config.InPlaceRotateAcc:F2}°/s²，" + + $"舵轮到位误差={config.InPlaceRotateWheelAlignDeg:F2}°，" + + $"旋转超时={config.InPlaceRotateTimeoutSec:F1}s；" + + $"起点航向={location.th:F2}°，" + + $"目标航向={targetWorldAngle:F2}°。"); + Console.WriteLine( + "原地自转CSV保存目录：" + + TrackingExperimentRecorder.DefaultOutputDirectory); + _recorder = new TrackingExperimentRecorder( - controllerName: "InPlaceRotatePID", + controllerName: "InPlaceRotateFilteredPID", trajectoryName: _trajectoryName, trialNumber: TrialNumber, referenceStart: rotationCenter, @@ -75,7 +99,7 @@ namespace MultiWheelC referenceSpeed: 0f, referenceAngularSpeed: (float)AngleMath.DegreesToRadians( - MaxAngularSpeedDegreesPerSecond)); + config.InPlaceRotateMaxSpeed)); _recorder.Start(); try @@ -88,21 +112,26 @@ namespace MultiWheelC PidparamsRead = () => new PIDParams { Kp = - PilotDefinition.Conf.InPlaceRotateKp, + config.InPlaceRotateKp, Ki = - PilotDefinition.Conf.InPlaceRotateKi, + config.InPlaceRotateKi, Kd = - PilotDefinition.Conf.InPlaceRotateKd, + config.InPlaceRotateKd, DeadZone = - PilotDefinition.Conf - .InPlaceRotateArriveDeg, + config.InPlaceRotateArriveDeg, SpeedAccPerSec = - PilotDefinition.Conf.InPlaceRotateAcc, + config.InPlaceRotateAcc, OutputUpperThreshold = - MaxAngularSpeedDegreesPerSecond, + config.InPlaceRotateMaxSpeed, MaxI = - PilotDefinition.Conf.InPlaceRotateMaxI + config.InPlaceRotateMaxI }, + MinimumAngularSpeedDegreesPerSecond = + config.InPlaceRotateMinimumSpeed, + WheelAlignmentToleranceDegrees = + config.InPlaceRotateWheelAlignDeg, + RotationTimeoutSeconds = + config.InPlaceRotateTimeoutSec, CommandAngularSpeedObserver = commandAngularSpeed => _recorder?.UpdateCommand( @@ -136,23 +165,58 @@ namespace MultiWheelC } - [MovementTest(name = "SendXYThSpeed：原地自转90°")] - public sealed class TestRotate90 : + [MovementTest(name = "SendXYThSpeed：输入角度原地自转")] + public sealed class TestRotateAngle : InPlaceRotateTestBase { - public TestRotate90() - : base(90f, "Rotate90") + public TestRotateAngle() + : base(0f, "RotateCustomAngle") { } + + /// + /// 读取相对旋转角度并按正值逆时针、负值顺时针执行原地自转。 + /// + public override void Test() + { + var input = UI.GetInput( + "输入相对旋转角度（deg，正数逆时针，负数顺时针，范围-180到180之间）："); + + if ((!float.TryParse( + input, + NumberStyles.Float, + CultureInfo.CurrentCulture, + out var relativeAngleDegrees) && + !float.TryParse( + input, + NumberStyles.Float, + CultureInfo.InvariantCulture, + out relativeAngleDegrees)) || + float.IsNaN(relativeAngleDegrees) || + float.IsInfinity(relativeAngleDegrees)) + { + Console.WriteLine("旋转角度输入无效，测试已经取消。"); + return; + } + + if (Math.Abs(relativeAngleDegrees) < 1e-3f) + { + Console.WriteLine("旋转角度不能为0，测试已经取消。"); + return; + } + + // 当前控制器按照圆周最短角旋转；精确±180°的方向存在二义性。 + if (Math.Abs(relativeAngleDegrees) >= 180f) + { + Console.WriteLine( + "输入角度必须满足-180° < angle < 180°；" + + "当前最短角控制不支持指定精确±180°的旋转方向。"); + return; + } + + RelativeAngleDegrees = relativeAngleDegrees; + base.Test(); + } } - [MovementTest(name = "SendXYThSpeed：原地自转180°")] - public sealed class TestRotate180 : - InPlaceRotateTestBase - { - public TestRotate180() - : base(180f, "Rotate180") - { - } - } } diff --git a/MultiWheelC/Experiments/TrackingExperimentRecorder.cs b/MultiWheelC/Experiments/TrackingExperimentRecorder.cs index 1fd9ee3..a09ef46 100644 --- a/MultiWheelC/Experiments/TrackingExperimentRecorder.cs +++ b/MultiWheelC/Experiments/TrackingExperimentRecorder.cs @@ -145,6 +145,14 @@ namespace MultiWheelC // 保存成功后的CSV绝对路径；尚未保存时为空。 public string SavedFilePath { get; private set; } + /// + /// 获取Clumsy当前运行目录下统一保存轨迹实验CSV的文件夹。 + /// + public static string DefaultOutputDirectory => + Path.Combine( + AppContext.BaseDirectory, + "TrackingExperiments"); + // 启动后台采样线程。 public void Start() { @@ -242,6 +250,17 @@ namespace MultiWheelC } } + /// + /// 清除上一轨迹段参考量，避免停车或原地自转期间沿用已经结束的投影结果。 + /// + public void ClearControlReference() + { + lock (_stateSyncRoot) + { + _hasControlReference = false; + } + } + // 停止采样并将本次实验保存为CSV；重复调用只保存一次。 public void StopAndSave() { @@ -444,9 +463,8 @@ namespace MultiWheelC new List(_samples); } - var outputDirectory = Path.Combine( - AppContext.BaseDirectory, - "TrackingExperiments"); + var outputDirectory = + DefaultOutputDirectory; Directory.CreateDirectory(outputDirectory); diff --git a/MultiWheelC/Movements/RotateInPlace.cs b/MultiWheelC/Movements/RotateInPlace.cs index 883c87f..c35a517 100644 --- a/MultiWheelC/Movements/RotateInPlace.cs +++ b/MultiWheelC/Movements/RotateInPlace.cs @@ -7,6 +7,7 @@ using ClumsyCore.Pilot; using CommonUsage.Chassis; using MDCSToolBox.Commons.Controllers; using MyParking.Shared; +using MultiWheelC.StateEstimation; namespace MultiWheelC { @@ -17,7 +18,11 @@ namespace MultiWheelC /// public float AngleTarget; - public Func ThetaReader = () => (float)DetourInterface.getCartLocation().th; + // 留作标定或单元测试时显式替换；为空时使用经过校验的Detour状态源。 + public Func ThetaReader; + + public IVehicleStateProvider StateProvider = + new DetourVehicleStateProvider(); public MultiWheelChassis Chassis = (MultiWheelChassis)PilotDefinition.Chassis; @@ -37,6 +42,12 @@ namespace MultiWheelC // 自转舵轮准备超时时间，单位s。 public float WheelAlignmentTimeoutSeconds = 10f; + // 航向尚未到位时允许下发的最小有效角速度，单位deg/s。 + public float MinimumAngularSpeedDegreesPerSecond = 1f; + + // 舵轮到位后执行航向闭环允许的最长时间，单位s。 + public float RotationTimeoutSeconds = 15f; + // 先准备自转舵角，再通过安全版SendXYThSpeed闭环旋转到目标角度。 public override IEnumerable Get() { @@ -44,6 +55,8 @@ namespace MultiWheelC throw new InvalidOperationException( "当前底盘不是MultiWheelChassis，无法执行原地自转。"); + ValidateParameters(); + var adapter = new MultiWheelChassisAdapter( Chassis, PilotDefinition.Self.CarNum); @@ -55,7 +68,9 @@ namespace MultiWheelC DateTime? alignedSince = null; while (true) { - if (!adapter.PrepareSpin()) + if (!adapter.PrepareSpin( + alignmentToleranceDegrees: + WheelAlignmentToleranceDegrees)) throw new InvalidOperationException( "无法生成原地自转舵轮目标：" + adapter.LastFailureReason); @@ -84,18 +99,86 @@ namespace MultiWheelC yield return true; } + var alignmentToleranceRadians = + AngleMath.DegreesToRadians( + WheelAlignmentToleranceDegrees); + if (!adapter.AdoptPreparedSpinForXYTh( + alignmentToleranceRadians)) + { + throw new InvalidOperationException( + "无法将已到位的自转舵角交接给XYTh：" + + adapter.LastFailureReason); + } + var targetAngle = (float)AngleMath.NormalizeDegrees(AngleTarget); var p = PidparamsRead(); - thPid = new PIDController(ThetaReader, p.Kp); + var currentAngle = ReadCurrentAngleDegrees(); + var cachedCurrentAngle = currentAngle; + thPid = new PIDController( + () => cachedCurrentAngle, + p.Kp); thPid.ChangeParameters(p.Kp, p.Ki, p.Kd, p.MaxI, p.DeadZone, p.OutputUpperThreshold, p.SpeedAccPerSec); var lastCommandTime = DateTime.Now; + var rotationStarted = DateTime.Now; while (true) { + if ((DateTime.Now - rotationStarted) + .TotalSeconds > + RotationTimeoutSeconds) + { + throw new TimeoutException( + $"原地自转超过{RotationTimeoutSeconds:F1}s仍未到位。"); + } + + currentAngle = ReadCurrentAngleDegrees(); + cachedCurrentAngle = currentAngle; var s = thPid.GetResponse(targetAngle, true); - Console.WriteLine($"s:{s} AngleTarget:{AngleTarget}"); + var angleErrorDegrees = + (float)AngleMath + .ShortestDifferenceDegrees( + targetAngle, + currentAngle); + + // PID进入到位死区后等待其0.3s稳定确认；等待期间 + // 只清零驱动速度，不清除已经准备好的自转舵角状态。 + if (Math.Abs(angleErrorDegrees) <= + p.DeadZone) + { + CommandAngularSpeedObserver?.Invoke(0f); + adapter + .StopXYThDrivePreserveSteeringState(); + + if (thPid.IsArrived()) + break; + + yield return true; + continue; + } + + // PID输出低于底盘有效轮速范围时提高到最小可执行值， + // 避免接近目标时反复出现微小命令但车辆实际不动。 + if (Math.Abs(s) > 1e-6f && + Math.Abs(s) < + MinimumAngularSpeedDegreesPerSecond) + { + s = Math.Sign(angleErrorDegrees) * + MinimumAngularSpeedDegreesPerSecond; + } + + // PID加速限制在首周期可能暂时输出零；此时保留 + // 已交接的自转状态，等待下一周期产生有效角速度。 + if (Math.Abs(s) <= 1e-6f) + { + CommandAngularSpeedObserver?.Invoke(0f); + adapter + .StopXYThDrivePreserveSteeringState(); + yield return true; + continue; + } + CommandAngularSpeedObserver?.Invoke(s); var now = DateTime.Now; var interval = now - lastCommandTime; @@ -118,7 +201,6 @@ namespace MultiWheelC "安全XYTh原地旋转底盘解算失败：" + adapter.LastFailureReason); } - if (thPid.IsArrived()) break; yield return true; } @@ -130,5 +212,109 @@ namespace MultiWheelC adapter.StopImmediately(); } } + + /// + /// 检查原地自转的舵轮准备、最小速度和超时参数是否可执行。 + /// + private void ValidateParameters() + { + EnsureFinitePositive( + WheelAlignmentToleranceDegrees, + nameof(WheelAlignmentToleranceDegrees), + allowZero: true); + EnsureFinitePositive( + WheelAlignmentStableSeconds, + nameof(WheelAlignmentStableSeconds), + allowZero: true); + EnsureFinitePositive( + WheelAlignmentTimeoutSeconds, + nameof(WheelAlignmentTimeoutSeconds)); + EnsureFinitePositive( + MinimumAngularSpeedDegreesPerSecond, + nameof(MinimumAngularSpeedDegreesPerSecond)); + EnsureFinitePositive( + RotationTimeoutSeconds, + nameof(RotationTimeoutSeconds)); + + var pidParameters = PidparamsRead(); + if (pidParameters == null) + { + throw new InvalidOperationException( + "原地自转PID参数读取结果为空。"); + } + + EnsureFinitePositive( + pidParameters.DeadZone, + "PidparamsRead.DeadZone"); + EnsureFinitePositive( + pidParameters.OutputUpperThreshold, + "PidparamsRead.OutputUpperThreshold"); + EnsureFinitePositive( + pidParameters.SpeedAccPerSec, + "PidparamsRead.SpeedAccPerSec"); + EnsureFinitePositive( + pidParameters.Kp, + "PidparamsRead.Kp"); + + if (MinimumAngularSpeedDegreesPerSecond > + pidParameters.OutputUpperThreshold) + { + throw new InvalidOperationException( + "原地自转最小有效角速度不能大于最大角速度。"); + } + } + + /// + /// 读取经过状态源校验的世界航向，显式设置ThetaReader时优先使用替代读数。 + /// + private float ReadCurrentAngleDegrees() + { + if (ThetaReader != null) + { + var angleDegrees = ThetaReader(); + if (float.IsNaN(angleDegrees) || + float.IsInfinity(angleDegrees)) + { + throw new InvalidOperationException( + "自定义航向读取结果不是有效角度。"); + } + + return (float)AngleMath.NormalizeDegrees( + angleDegrees); + } + + if (StateProvider == null || + !StateProvider.TryGetState(out var state)) + { + throw new InvalidOperationException( + "无法从Detour状态源读取有效车辆航向。" + + (StateProvider is DetourVehicleStateProvider provider + ? provider.LastFailureReason + : "")); + } + + return (float)AngleMath.RadiansToDegrees( + state.PoseInWorld.YawRadians); + } + + /// + /// 检查原地自转参数是否为正有限值，部分时间和容差参数允许为零。 + /// + private static void EnsureFinitePositive( + float value, + string parameterName, + bool allowZero = false) + { + if (float.IsNaN(value) || + float.IsInfinity(value) || + (allowZero + ? value < 0f + : value <= 0f)) + { + throw new ArgumentOutOfRangeException( + parameterName, + "原地自转参数必须是有效的正数。"); + } + } } } diff --git a/MultiWheelC/Movements/TrajectoryTrackingMovement.cs b/MultiWheelC/Movements/TrajectoryTrackingMovement.cs index dded045..553f6c7 100644 --- a/MultiWheelC/Movements/TrajectoryTrackingMovement.cs +++ b/MultiWheelC/Movements/TrajectoryTrackingMovement.cs @@ -75,6 +75,12 @@ namespace MultiWheelC /// public double MaximumIntegralCorrectionMetersPerSecond = 0.05; + /// + /// 纵向PID不进行反馈修正的速度误差死区，单位为m/s。 + /// + public double LongitudinalSpeedErrorDeadbandMetersPerSecond = + 0.025; + /// /// 底盘纵向命令速度绝对值上限，单位为m/s。 /// @@ -90,7 +96,7 @@ namespace MultiWheelC /// 前后GCP目标转角最大变化率，单位为rad/s。 /// public double MaximumGcpAngleRateRadiansPerSecond = - AngleMath.DegreesToRadians(10.0); + AngleMath.DegreesToRadians(15.0); /// /// 终点位置和剩余弧长的完成容差，单位为m。 @@ -164,7 +170,8 @@ namespace MultiWheelC LongitudinalKiPerSecond, LongitudinalKdSeconds, MaximumIntegralCorrectionMetersPerSecond, - MaximumCommandSpeedMetersPerSecond); + MaximumCommandSpeedMetersPerSecond, + LongitudinalSpeedErrorDeadbandMetersPerSecond); var gcpAllocator = new AckermannGcpAllocator( controlPointRadiusMeters, diff --git a/MultiWheelC/PilotConfig.cs b/MultiWheelC/PilotConfig.cs index 810f2f2..cff2ed1 100644 --- a/MultiWheelC/PilotConfig.cs +++ b/MultiWheelC/PilotConfig.cs @@ -26,7 +26,7 @@ public class PilotConfig : MultiWheelPilotConfig public float InPlaceRotateSpeed = 30f; [FieldMember(desc = "原地旋转：到位角度精度(deg)")] - public float InPlaceRotateArriveDeg = 1f; + public float InPlaceRotateArriveDeg = 1.5f; [FieldMember(desc = "原地旋转：起转前舵轮对齐精度(deg)")] public float InPlaceRotateWheelAlignDeg = 2f; @@ -38,24 +38,25 @@ public class PilotConfig : MultiWheelPilotConfig #region 单车-临时 [FieldMember(desc = "原地旋转Kp")] - public float InPlaceRotateKp = 0.2f; - // public float InPlaceRotateKp = 0.2f; + public float InPlaceRotateKp = 1.1f; [FieldMember(desc = "原地旋转Ki")] - public float InPlaceRotateKi = 0.01f; - // public float InPlaceRotateKi = 0.01f; + public float InPlaceRotateKi = 0f; [FieldMember(desc = "原地旋转Kd")] public float InPlaceRotateKd = 0f; [FieldMember(desc = "原地旋转积分限幅")] - public float InPlaceRotateMaxI = 0.01f; + public float InPlaceRotateMaxI = 0f; + + [FieldMember(desc = "原地旋转最小有效角速度(deg/s)")] + public float InPlaceRotateMinimumSpeed = 1f; [FieldMember(desc = "原地旋转最大角速度(deg/s)")] - public float InPlaceRotateMaxSpeed = 30f; + public float InPlaceRotateMaxSpeed = 47.5f; [FieldMember(desc = "原地旋转角加速度(deg/s²)")] - public float InPlaceRotateAcc = 30f; + public float InPlaceRotateAcc = 60f; [FieldMember(desc = "原地旋转超时(s)")] public float InPlaceRotateTimeoutSec = 15f; diff --git a/MultiWheelC/StateEstimation/DetourVehicleStateProvider.cs b/MultiWheelC/StateEstimation/DetourVehicleStateProvider.cs index c94683f..4a07a9b 100644 --- a/MultiWheelC/StateEstimation/DetourVehicleStateProvider.cs +++ b/MultiWheelC/StateEstimation/DetourVehicleStateProvider.cs @@ -188,11 +188,11 @@ namespace MultiWheelC.StateEstimation poseInWorld, elapsedSeconds)) { - state = AcceptPoseAfterVelocityRebase( + state = AcceptPoseAfterReset( poseInWorld, timestampSeconds); LastFailureReason = - "Detour位姿偏离上一速度预测，本次只更新位姿基准并保留滤波速度。"; + "Detour位姿偏离速度预测，已重新建立速度估计基准。"; return true; } diff --git a/Shared/Chassis/MultiWheelChassisAdapter.cs b/Shared/Chassis/MultiWheelChassisAdapter.cs index a23224f..5d2fbe2 100644 --- a/Shared/Chassis/MultiWheelChassisAdapter.cs +++ b/Shared/Chassis/MultiWheelChassisAdapter.cs @@ -504,13 +504,26 @@ namespace MyParking.Shared /// 返回是否成功生成舵轮目标。 /// public bool PrepareSpin( - TimeSpan? interval = null) + TimeSpan? interval = null, + double alignmentToleranceDegrees = 2.0) { + ValidateFinite( + alignmentToleranceDegrees, + nameof(alignmentToleranceDegrees)); + + if (alignmentToleranceDegrees < 0.0) + { + throw new ArgumentOutOfRangeException( + nameof(alignmentToleranceDegrees), + "自转舵轮到位容差必须是非负有限值。"); + } + EnsureBodyFrameIsActive(); var success = _chassis.PrepareRotateWheels( - alignmentToleranceDegrees: 2.0f); + alignmentToleranceDegrees: + (float)alignmentToleranceDegrees); if (!success) { diff --git a/data_process/新版控制器轨迹测试处理/plot_new_controller_experiment.py b/data_process/新版控制器轨迹测试处理/plot_new_controller_experiment.py index a483532..7f37601 100644 --- a/data_process/新版控制器轨迹测试处理/plot_new_controller_experiment.py +++ b/data_process/新版控制器轨迹测试处理/plot_new_controller_experiment.py @@ -208,10 +208,13 @@ def load_experiment(csv_path: Path) -> dict[str, object]: has_control_reference = ( numeric_column(frame, "HasControlReference", 0.0) > 0.5 ) - lateral_error = np.where( - has_control_reference & np.isfinite(recorded_lateral_error), - recorded_lateral_error, - derived_lateral_error, + recorded_lateral_valid = ( + has_control_reference & np.isfinite(recorded_lateral_error) + ) + lateral_error = ( + np.where(recorded_lateral_valid, recorded_lateral_error, np.nan) + if np.any(recorded_lateral_valid) + else derived_lateral_error ) state_yaw = numeric_column(frame, "StateYawRadians") @@ -230,10 +233,28 @@ def load_experiment(csv_path: Path) -> dict[str, object]: frame, "ControlHeadingErrorRadians", ) - heading_error = np.where( - has_control_reference & np.isfinite(recorded_heading_error), - recorded_heading_error, - derived_heading_error, + recorded_heading_valid = ( + has_control_reference & np.isfinite(recorded_heading_error) + ) + heading_error = ( + np.where(recorded_heading_valid, recorded_heading_error, np.nan) + if np.any(recorded_heading_valid) + else derived_heading_error + ) + + # 投影定义满足：参考点 = 车体位置 + 横向误差 × 参考航向左法向。 + # 因此无需假设轨迹类型，即可从有效控制周期还原车辆实际使用的参考轨迹。 + projected_reference_yaw = actual_yaw + heading_error + reference_x = ( + actual_x - lateral_error * np.sin(projected_reference_yaw) + ) + reference_y = ( + actual_y + lateral_error * np.cos(projected_reference_yaw) + ) + valid_reference_position = ( + has_control_reference + & np.isfinite(reference_x) + & np.isfinite(reference_y) ) cruise_speed = first_finite( @@ -270,6 +291,8 @@ def load_experiment(csv_path: Path) -> dict[str, object]: numeric_column(frame, "ControlReferenceSpeedMetersPerSecond"), ideal_speed, ) + if np.any(has_control_reference): + reference_speed[~has_control_reference] = np.nan actual_speed = numeric_column(frame, "StateBodyVxMetersPerSecond") velocity_valid = ( numeric_column(frame, "StateVelocityEstimateValid", 0.0) > 0.5 @@ -283,6 +306,9 @@ def load_experiment(csv_path: Path) -> dict[str, object]: "actual_x": actual_x, "actual_y": actual_y, "valid_position": valid_position, + "reference_x": reference_x, + "reference_y": reference_y, + "valid_reference_position": valid_reference_position, "start": start, "end": end, "length": length_meters, @@ -336,13 +362,23 @@ def plot_experiment( valid_position = data["valid_position"] fig, axis = plt.subplots(figsize=(9.0, 6.5)) - axis.plot( - [data["start"][0], data["end"][0]], - [data["start"][1], data["end"][1]], - "--", - linewidth=2.0, - label="期望4m直线轨迹", - ) + valid_reference_position = data["valid_reference_position"] + if np.count_nonzero(valid_reference_position) >= 2: + axis.plot( + data["reference_x"][valid_reference_position], + data["reference_y"][valid_reference_position], + "--", + linewidth=2.0, + label="控制器实际使用的参考轨迹", + ) + else: + axis.plot( + [data["start"][0], data["end"][0]], + [data["start"][1], data["end"][1]], + "--", + linewidth=2.0, + label="参考起终点连线", + ) axis.plot( data["actual_x"][valid_position], data["actual_y"][valid_position], @@ -451,7 +487,7 @@ def discover_csv_files(arguments: list[str]) -> list[Path]: def main() -> None: """解析命令行并批量处理新版控制器实验CSV。""" parser = argparse.ArgumentParser( - description="绘制新版控制器4m直线实验的四类对比图。" + description="绘制新版控制器轨迹实验的四类对比图。" ) parser.add_argument("csv", nargs="*", help="需要处理的CSV文件路径。") parser.add_argument( diff --git a/docs/clumsy参考.json b/docs/clumsy参考.json new file mode 100644 index 0000000..f774adc --- /dev/null +++ b/docs/clumsy参考.json @@ -0,0 +1,534 @@ +{ + "layout": { + "chassis": { + "width": 1100.0, + "length": 1550.0, + "contour": [ + -775.0, + 550.0, + 775.0, + 550.0, + 775.0, + -550.0, + -775.0, + -550.0 + ] + }, + "components": [ + { + "type": "wheel", + "options": { + "platform": 0, + "scale": 1.0, + "radius": 200.0, + "group": null, + "id": 1, + "name": "w1", + "x": 0.0, + "y": 300.0, + "yaw": 0.0, + "z": 0.0, + "pitch": 0.0, + "roll": 0.0 + } + }, + { + "type": "wheel", + "options": { + "platform": 6, + "scale": 1.0, + "radius": 200.0, + "group": null, + "id": 2, + "name": "w2", + "x": 0.0, + "y": -300.0, + "yaw": 0.0, + "z": 0.0, + "pitch": 0.0, + "roll": 0.0 + } + }, + { + "type": "lidarssc", + "options": { + "usingLidars": "frontlidar", + "stopDist": 100.0, + "directionX": 1.0, + "directionY": 0.0, + "thresDot": 9999, + "contour": [ + 0.0, + 0.0 + ], + "group": [ + "0", + "stop" + ], + "id": 411683697, + "name": "autoStop", + "x": 0.0, + "y": 0.0, + "yaw": 0.0, + "z": 0.0, + "pitch": 0.0, + "roll": 0.0 + } + }, + { + "type": "lidarssc", + "options": { + "usingLidars": "frontlidar", + "stopDist": 100.0, + "directionX": 1.0, + "directionY": 0.0, + "thresDot": 999, + "contour": [ + 0.0, + 0.0 + ], + "group": [ + "0", + "slow" + ], + "id": 726862610, + "name": "autoSlow", + "x": 0.0, + "y": 0.0, + "yaw": 0.0, + "z": 0.0, + "pitch": 0.0, + "roll": 0.0 + } + }, + { + "type": "lidar2d", + "options": { + "isCircle": true, + "ignoreDist": 10.0, + "maxDist": 200000.0, + "useFilter": "", + "filterChassis": true, + "afterImageFilterOutN": 7, + "afterImageFilterOutDeg": 2.0, + "reflexThres": 0.4, + "reflexFilterWndSz": 30, + "reflexDistWnd": 50.0, + "reflexChunkThres": 2.5, + "BindLidar2dName": "", + "BindRelativeX": -4.9166203, + "BindRelativeY": 938.9871, + "BindRelativeTh": 1.300003, + "group": null, + "id": 1444795304, + "name": "rightlidar", + "x": -749.0, + "y": -475.0, + "yaw": 180.8, + "z": 0.0, + "pitch": 0.0, + "roll": 0.0 + } + }, + { + "type": "lidar2d", + "options": { + "isCircle": true, + "ignoreDist": 10.0, + "maxDist": 200000.0, + "useFilter": "", + "filterChassis": true, + "afterImageFilterOutN": 7, + "afterImageFilterOutDeg": 2.0, + "reflexThres": 0.4, + "reflexFilterWndSz": 30, + "reflexDistWnd": 50.0, + "reflexChunkThres": 2.5, + "BindLidar2dName": "", + "BindRelativeX": 0.0, + "BindRelativeY": 0.0, + "BindRelativeTh": 0.0, + "group": null, + "id": 1983955111, + "name": "leftlidar", + "x": -734.0, + "y": 475.0, + "yaw": 179.8, + "z": 0.0, + "pitch": 0.0, + "roll": 0.0 + } + }, + { + "type": "lidar3d", + "options": { + "ignoreDist": 5.0, + "maxDist": 200000.0, + "reduce": false, + "voxelSize": 70.0, + "pcklen": 82560, + "angleSgn": -1, + "endAngle": 0.0, + "RotationMatrix": [ + 1.0, + 0.0, + 0.0, + 0.0, + 1.0, + 0.0, + -0.0, + 0.0, + 1.0 + ], + "group": null, + "id": 1000069841, + "name": "frontlidar3d", + "x": 752.5, + "y": 0.0, + "yaw": 0.0, + "z": 0.0, + "pitch": 0.0, + "roll": 0.0 + } + }, + { + "type": "plannar3dlidarzrange", + "options": { + "zmin": -65.0, + "zmax": 7.0, + "useAbsolute": true, + "samples": 1024, + "lidar3dName": "frontlidar3d", + "isCircle": true, + "ignoreDist": 10.0, + "maxDist": 200000.0, + "useFilter": "", + "filterChassis": true, + "afterImageFilterOutN": 7, + "afterImageFilterOutDeg": 2.0, + "reflexThres": 0.4, + "reflexFilterWndSz": 30, + "reflexDistWnd": 50.0, + "reflexChunkThres": 2.5, + "BindLidar2dName": "", + "BindRelativeX": 0.0, + "BindRelativeY": 0.0, + "BindRelativeTh": 0.0, + "group": null, + "id": 1954892242, + "name": "frontlidar", + "x": 752.5, + "y": 0.0, + "yaw": 0.0, + "z": 0.0, + "pitch": 0.0, + "roll": 0.0 + } + }, + { + "type": "lidarssc", + "options": { + "usingLidars": "frontlidar", + "stopDist": 100.0, + "directionX": 1.0, + "directionY": 0.0, + "thresDot": 10, + "contour": [ + 600.0, + -650.0, + 600.0, + 650.0, + 1200.0, + 650.0, + 1200.0, + -650.0 + ], + "group": [ + "1", + "stop" + ], + "id": 1519627219, + "name": "fstop1", + "x": 230.0, + "y": 0.0, + "yaw": 0.0, + "z": 0.0, + "pitch": 0.0, + "roll": 0.0 + } + }, + { + "type": "lidarssc", + "options": { + "usingLidars": "frontlidar", + "stopDist": 100.0, + "directionX": 1.0, + "directionY": 0.0, + "thresDot": 10, + "contour": [ + 1200.0, + -650.0, + 1200.0, + 650.0, + 2600.0, + 650.0, + 2600.0, + -650.0 + ], + "group": [ + "1", + "slow" + ], + "id": 771141297, + "name": "fslow1", + "x": 230.0, + "y": 0.0, + "yaw": 0.0, + "z": 0.0, + "pitch": 0.0, + "roll": 0.0 + } + } + ] + }, + "DriveTaskInterval": 30, + "DriveTaskTimeout": 9999.0, + "basicSpeed": 0.7, + "msConf": { + "SyncThAccPerSec": 5.0, + "TestCarSyncDistance": 2893.0, + "TestCarSyncTh": 0.0, + "ManualCarSyncVxFac": 0.3, + "ManualCarSyncVyFac": 1.0, + "ManualCarSyncVthFac": 30.0, + "MultiVehicleCrabSteerLimitDeg": 120.0, + "DeltaDetectCenter": 742.5, + "MultiVehicleSyncUseDetour": true, + "MultiVehicleManualUseDetourCorrection": true, + "MultiVehicleFleetNum": 2, + "MultiVehicleSyncInterval": 25, + "MultiVehicleMasterEndpoint": "/", + "SimpleIp": "192.168.1.101", + "MultiVehicleSelfEndpoint": "", + "MultiVehicleUseDetect": false, + "MultiVehicleControlRadius": 0.0, + "MultiVehicleAutoCmdTimeoutMs": 9999, + "MultiVehicleMemberTtlMs": 0, + "MultiVehicleAutoUseIdealCenter": true, + "MultiVehicleAutoRequireFleetCenter": true, + "MultiVehiclePosBiasXFac": 0.005, + "MultiVehiclePosBiasYFac": 0.15, + "MultiVehiclePosBiasThFac": 0.1, + "MultiVehiclePosBiasXThreshold": 0.05, + "MultiVehiclePosBiasYThreshold": 10.0, + "MultiVehiclePosBiasThThreshold": 5.0, + "MultiVehicleDetectBiasXFac": 0.0, + "MultiVehicleDetectBiasYFac": 0.0, + "MultiVehicleDetectBiasThFac": 0.0, + "MultiVehicleDetectBiasXThreshold": 0.0, + "MultiVehicleDetectBiasYThreshold": 0.0, + "MultiVehicleDetectBiasThThreshold": 0.0, + "MultiVehicleRotateCompXyFac": 0.003, + "MultiVehicleRotateCompXyIFac": 0.01, + "MultiVehicleRotateCompXyMax": 3.0, + "MultiVehicleRotateCompThFac": 0.1, + "MultiVehicleRotateCompThIFac": 0.01, + "MultiVehicleRotateCompThMax": 3.0, + "MultiVehicleRotateActiveOmega": 0.5, + "MultiVehicleRotateCompTangentFrac": 0.1, + "SingleCarSyncPrecisionXy": 10.0, + "SingleCarSyncPrecisionTh": 0.1, + "PlaygroundWebApiUrl": "http://localhost:18090", + "MultiVehicleRotatePoseWebApiDiagEnabled": false, + "PlaygroundRobotName": "agv_multi_1", + "PlaygroundNeighborRobotName": "agv_multi_2", + "WebApiTranslateMm": 100.0, + "WebApiRotateDeg": 5.0, + "InPlaceRotateTargetWorldDeg": 90.0, + "InPlaceRotateSpeed": 30.0, + "InPlaceRotateArriveDeg": 1.0, + "InPlaceRotateWheelAlignDeg": 2.0, + "InPlaceRotateActiveWheelAlignDeg": 10.0, + "FleetRotateOmega": 6.0, + "FleetRotateTargetDeltaDeg": 90.0, + "FleetRotateArriveDeg": 1.5, + "FleetRotateSlowDeg": 10.0, + "FleetRotateMinOmega": 0.5, + "FleetRotateAccel": 1.0, + "FleetRotateSettleSec": 0.5, + "FleetRotateUseDetourHeading": true, + "FleetCrabAngleDeg": 90.0, + "FleetCrabBodyWorldHeadingDeg": 0.0, + "FleetCrabLengthMm": 2000.0, + "FleetCrabSpeed": 0.35, + "FleetCrabAccel": 0.1, + "FleetCrabStartAccel": 0.02, + "FleetCrabSlowDistance": 800.0, + "FleetCrabFinishDistance": 10.0, + "FleetCrabFinishSpeed": 0.0, + "FleetCrabSlowingPow": 0.7, + "FleetCrabGcpThetaThreshold": 120.0, + "FleetCrabDthLinearFac": 3.3, + "FleetCrabDthLinearThreshold": 10.0, + "FleetCrabStartSyncTimeoutSec": 9999.0, + "FleetCrabStartWheelAlignDeg": 2.0, + "TwoLegLidarName": "leftlidar,rearlidar", + "TwoLegGuessX": -2000.0, + "TwoLegWidth": 450.0, + "TwoLegWidthErr": 50.0, + "TwoLegBlobDist": 100.0, + "TwoLegBlobSize": 200.0, + "TwoLegBlobPtCount": 10, + "TwoLegPadding": 5, + "TwoLegPillarFindingScope": 20, + "TwoLegSgnDir": 1, + "TwoLegCenterChangeX": 0.0, + "TwoLegOutputBiasX": -15.0, + "TwoLegOutputBiasY": 0.0, + "TwoLegFilterLength": 500.0, + "TwoLegFilterWidth": 800.0, + "TireFilterLength": 1000.0, + "TireFilterWidth": 1900.0, + "TireTwoLegWidth": 1470.0, + "TireTwoLegWidthErr": 200.0, + "TireTwoLegBlobPtCount": 10, + "TireFrontTwoLegBlobDist": 100.0, + "TireFrontTwoLegBlobSize": 200.0, + "TireFrontPadding": 10, + "TireFrontTwoLegPillarFindingScope": 10, + "TireFrontTwoLegSgnDir": 1, + "TireFrontTwoLegCenterChangeX": 0.0, + "TireBackTwoLegBlobDist": 100.0, + "TireBackTwoLegBlobSize": 200.0, + "TireBackPadding": 5, + "TireBackTwoLegPillarFindingScope": 20, + "TireBackTwoLegSgnDir": 1, + "TireBackTwoLegCenterChangeX": 0.0, + "ClampControlKp": 0.0015, + "ClampControlKi": 0.0, + "ClampControlKd": 0.0, + "ClampControlMaxI": 0.02, + "ClampControlSpeedAcc": 2.0, + "ClampControlThresh": 0.2, + "ClampControlDeadZone": 30.0, + "MaxClampSpeed": 12.0, + "LineTrackDistance": 1000.0, + "LineTrackMaxSpeed": 0.3, + "LineTrackKp": 0.001, + "LineTrackKi": 0.0, + "LineTrackKd": 0.0, + "LineTrackDeadZone": 10.0, + "TireFollowingWalkBlindSwitchingDistance": 1300.0, + "TireFollowingStage1GuessX": 2200.0, + "TireFollowingStage2GuessX": 2600.0, + "TireFollowingWalkBlindFinishDistance": 10.0, + "TireFollowingSlowDistance": 750.0, + "TireFollowingMaxSpeed": 0.3, + "TireFollowingFrontLidarPathTransformationX": 160.0, + "TireFollowingFrontLidarPathTransformationY": 3.0, + "TireFollowingFrontLidarWalkBlindTh": 0.0, + "TireFollowingBackLidarPathTransformationX": 155.0, + "TireFollowingBackLidarPathTransformationY": 0.0, + "TireFollowingBackLidarWalkBlindTh": 0.0, + "TireFollowingLeaveCarBackLidarPathTransformationX": 1700.0, + "TireFollowingLeaveCarWalkBlindSwitchingDistance": 2400.0, + "TireFollowingTireNum": 2, + "TireFollowingCloseDistance": 1400.0, + "TireFollowingAngleIgnoreThr": 1.0, + "TireFollowingYAverageFrameCount": 6, + "DstTrackerMaxSpeed": 0.3, + "GcpThetaThreshold": 70.0, + "DthLinearFac": 0.75, + "DthLinearThreshold": 25.0, + "BiasFac": 0.5, + "BiasThreshold": 15.0, + "BiasControlGainFac": 1.0, + "BiasSlowSigma": 55.0, + "LineMagKp": 0.1, + "LineMagKi": 0.0, + "LineMagKd": 0.1, + "MagMaxI": 10.0, + "MagDeadZone": 1.0, + "LineMagThresh": 35.0, + "CurveMagKp": 0.4, + "CurveMagKi": 0.0, + "CurveMagKd": 0.1, + "CurveMagThresh": 65.0, + "MotionDebugPrint": true, + "DebugCurvature": false, + "SlowDistance": 1000.0, + "SlowingPow": 0.8, + "FinishDistance": 5.0, + "FinishSpeed": 0.02, + "FirstThAccuracy": 2.0, + "ThContinuousThreshold": 10.0, + "FirstRotateSpeedFac": 1.0, + "FirstRotateMaxSpeed": 30.0, + "FirstRotateAcc": 20.0, + "FirstRotateDeAcc": 30.0, + "SpeedAccPerSecond": 0.1, + "SpeedDeAccPerSecond": 1.0, + "NotContinuousAngle": 3.0, + "PowerSteeringLookAhead": 100.0, + "SpeedLookAhead": 1500.0, + "SpeedLookAheadCurveDiff": 1000.0, + "SpeedLookBackCurveDiff": 200.0, + "SpeedLimitCurveDiffMin": 0.2, + "SpeedLimitCurveMin": 0.2, + "MaxRotateSpeedCurveLimit": 30.0, + "MaxRotateAccCurveLimit": 30.0, + "BaisAlarmValue": 1500.0, + "DthAlarmValue": 150.0, + "UseAutoAvoidance": false, + "ObstacleStopDistance": 1000.0, + "ObstacleSlowDistance": 2500.0, + "CoefficientOfExpansion": 1.0, + "TargetSpeed": 0.5, + "EmptyCartLength": 1550.0, + "EmptyCartWidth": 1100.0, + "RotateStopFac": 1.3, + "RotateSlowFac": 1.8, + "SlowPow": 1.2, + "ShieldAutoObstacle": false, + "LidarName": "frontlidar", + "ObstacleRecoveryTime": 500, + "UseCameraAvoidance": false, + "UseManualContorolAvoidance": true, + "ManualAutoAvoidcaneSlowDistance": 500.0, + "ManualAutoAvoidcaneStopDistance": 300.0, + "ShieldAutoAvoidance": false, + "UpCamPoseX": 0.0, + "UpCamPoseY": 0.0, + "UpCamPoseTh": 0.0, + "DownCamPoseX": 0.0, + "DownCamPoseY": 0.0, + "DownCamPoseTh": 0.0, + "OutMapEnable": true, + "GroundLossThreshold": 5.0, + "LaserLossThreshold": 15.0, + "RiskSlowdownThreshold": 15.0, + "UseSimpleDetector": false, + "LoseConnectionTime": 5000, + "GroundCameraDisconnectAlarmTime": 200, + "FrontLidarName": "null", + "RearLidarName": "null", + "UseSkidDetector": true, + "SkidTimeThreshold": 3000, + "SkidFacThreshold": 3.0, + "MissionWarningTime": 3000, + "UseGyrosDetector": false, + "GyrosErrorTime": 3000, + "GyrosErrorFac": 10, + "ObstacleStopDec": 0.1 + }, + "script": "MultiWheelC.dll", + "guru": { + "MaxLogFiles": 20, + "interpreter": "javascript", + "throwSAIError": true + }, + "locationTimeout": 100, + "IOCheckIntegrity": true, + "detourHost": "127.0.0.1", + "detourPort": 4321 +} \ No newline at end of file