diff --git a/MedullaAdapter/build/Medulla/plugins/CommonUsage.dll b/MedullaAdapter/build/Medulla/plugins/CommonUsage.dll index 9fcee49..403ea8b 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 d63173f..3eb4a89 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 de52922..2f446aa 100644 Binary files a/MedullaAdapter/build/Medulla/plugins/MedullaAdapter.pdb and b/MedullaAdapter/build/Medulla/plugins/MedullaAdapter.pdb differ diff --git a/MultiWheelC.Tests/FleetControllerTests.cs b/MultiWheelC.Tests/FleetControllerTests.cs new file mode 100644 index 0000000..3667bd8 --- /dev/null +++ b/MultiWheelC.Tests/FleetControllerTests.cs @@ -0,0 +1,295 @@ +using System; +using MultiWheelC.Control.Abstractions; +using MultiWheelC.Control.Allocation; +using MultiWheelC.Fleet; +using MultiWheelC.Trajectory; +using MyParking.Shared; + +namespace MultiWheelC.Tests +{ + internal static class FleetControllerTests + { + private const double Tolerance = 1e-9; + + public static void Run() + { + VerifyInactiveControllerStops(); + VerifyStraightCommand(); + VerifyInvalidVelocityUsesReferenceSpeed(); + VerifyCompletionStops(); + VerifyExcessiveTrackingErrorFaults(); + VerifyCancelStops(); + + Console.WriteLine( + "FleetController车队中心控制测试通过。共6个场景。"); + } + + private static void VerifyInactiveControllerStops() + { + var controller = CreateController(); + var result = controller.ComputeCommand( + CreateState(0.5, 0.0, 0.0, true), + 0.02, + out var command); + + AssertResult( + result, + FleetControlCycleResult.Inactive, + "未启动控制器"); + AssertStop(command, "未启动控制器"); + } + + private static void VerifyStraightCommand() + { + var controller = CreateController(); + controller.Start(CreateStraightTrajectory()); + + var result = controller.ComputeCommand( + CreateState(0.5, 0.0, 0.4, true), + 0.02, + out var command); + + AssertResult( + result, + FleetControlCycleResult.CommandGenerated, + "直线控制"); + AssertNear( + command.ReferencePointInFleet.XMeters, + 0.0, + "直线控制参考点X"); + AssertNear( + command.ReferencePointInFleet.YMeters, + 0.0, + "直线控制参考点Y"); + AssertTwist( + command.TwistAtReferencePoint, + 0.4, + 0.0, + 0.0, + "直线控制"); + } + + private static void VerifyInvalidVelocityUsesReferenceSpeed() + { + var controller = CreateController(); + controller.Start(CreateStraightTrajectory()); + + var result = controller.ComputeCommand( + CreateState(0.5, 0.0, 0.0, false), + 0.02, + out var command); + + AssertResult( + result, + FleetControlCycleResult.CommandGenerated, + "速度尚未初始化"); + AssertTwist( + command.TwistAtReferencePoint, + 0.4, + 0.0, + 0.0, + "速度尚未初始化"); + } + + private static void VerifyCompletionStops() + { + var controller = CreateController(); + controller.Start(CreateStraightTrajectory()); + + var result = controller.ComputeCommand( + CreateState(1.0, 0.0, 0.0, true), + 0.02, + out var command); + + AssertResult( + result, + FleetControlCycleResult.Completed, + "终点完成"); + AssertStop(command, "终点完成"); + + if (controller.IsActive || !controller.IsCompleted) + { + throw new InvalidOperationException( + "终点完成后控制器状态错误。"); + } + } + + private static void VerifyExcessiveTrackingErrorFaults() + { + var controller = CreateController(); + controller.Start(CreateStraightTrajectory()); + + var result = controller.ComputeCommand( + CreateState(0.5, 0.5, 0.0, true), + 0.02, + out var command); + + AssertResult( + result, + FleetControlCycleResult.Faulted, + "轨迹偏离保护"); + AssertStop(command, "轨迹偏离保护"); + + if (string.IsNullOrWhiteSpace( + controller.LastFailureReason)) + { + throw new InvalidOperationException( + "轨迹偏离故障没有保存原因。"); + } + } + + private static void VerifyCancelStops() + { + var controller = CreateController(); + controller.Start(CreateStraightTrajectory()); + controller.Cancel(); + + var result = controller.ComputeCommand( + CreateState(0.5, 0.0, 0.0, true), + 0.02, + out var command); + + AssertResult( + result, + FleetControlCycleResult.Inactive, + "取消控制"); + AssertStop(command, "取消控制"); + } + + private static FleetController CreateController() + { + return new FleetController( + new StraightLateralController(), + new ReferenceLongitudinalController(), + new GcpCommandAllocator( + Math.PI / 4.0), + virtualControlPointRadiusMeters: 0.5); + } + + private static Trajectory2D CreateStraightTrajectory() + { + return new Trajectory2D( + new[] + { + new TrajectoryPoint( + 0.0, + Pose2D.Identity, + 0.0, + 0.4), + new TrajectoryPoint( + 1.0, + new Pose2D(1.0, 0.0, 0.0), + 0.0, + 0.4) + }); + } + + private static FleetState CreateState( + double xMeters, + double yMeters, + double vxMetersPerSecond, + bool hasValidVelocityEstimate) + { + return new FleetState( + sampleTimestampSeconds: 1.0, + fleetPoseInWorld: new Pose2D( + xMeters, + yMeters, + 0.0), + twistAtFleetOriginInWorld: new Twist2D( + vxMetersPerSecond, + 0.0, + 0.0), + hasValidVelocityEstimate: + hasValidVelocityEstimate); + } + + private static void AssertResult( + FleetControlCycleResult actual, + FleetControlCycleResult expected, + string scenario) + { + if (actual != expected) + { + throw new InvalidOperationException( + $"{scenario}结果错误：" + + $"actual={actual}, expected={expected}。"); + } + } + + private static void AssertStop( + FleetMotionCommand command, + string scenario) + { + AssertTwist( + command.TwistAtReferencePoint, + 0.0, + 0.0, + 0.0, + scenario); + } + + private static void AssertTwist( + Twist2D actual, + double expectedVx, + double expectedVy, + double expectedOmega, + string scenario) + { + AssertNear( + actual.VxMetersPerSecond, + expectedVx, + $"{scenario} Vx"); + AssertNear( + actual.VyMetersPerSecond, + expectedVy, + $"{scenario} Vy"); + AssertNear( + actual.OmegaRadiansPerSecond, + expectedOmega, + $"{scenario} Omega"); + } + + private static void AssertNear( + double actual, + double expected, + string valueName) + { + if (Math.Abs(actual - expected) > Tolerance) + { + throw new InvalidOperationException( + $"{valueName}错误：" + + $"actual={actual:F9}, expected={expected:F9}。"); + } + } + + private sealed class StraightLateralController : + ILateralController + { + public LateralControlCommand Compute( + PathTrackingContext context) + { + return LateralControlCommand.Straight; + } + + public void Reset() + { + } + } + + private sealed class ReferenceLongitudinalController : + ILongitudinalController + { + public double ComputeSpeedMetersPerSecond( + PathTrackingContext context) + { + return context + .ControlReferenceSpeedMetersPerSecond; + } + + public void Reset() + { + } + } + } +} diff --git a/MultiWheelC.Tests/Program.cs b/MultiWheelC.Tests/Program.cs index 2551060..9360103 100644 --- a/MultiWheelC.Tests/Program.cs +++ b/MultiWheelC.Tests/Program.cs @@ -1,7 +1,6 @@ using System; using MultiWheelC.Control.Abstractions; using MultiWheelC.Control.Lateral; -using MultiWheelC.StateEstimation; using MultiWheelC.Trajectory; using MyParking.Shared; @@ -37,6 +36,7 @@ namespace MultiWheelC.Tests Console.WriteLine( "Stanley前进/倒车横向符号测试通过。共8个场景。"); FleetKinematicsTests.Run(); + FleetControllerTests.Run(); } /// @@ -140,14 +140,10 @@ namespace MultiWheelC.Tests double headingErrorRadians, double feedforwardCurvaturePerMeter) { - var vehicleState = new VehicleState( - sampleTimestampSeconds: 0.0, - poseInWorld: Pose2D.Identity, - twistInWorld: new Twist2D( - signedSpeedMetersPerSecond, - 0.0, - 0.0), - hasValidVelocityEstimate: true); + var actualTwistInBody = new Twist2D( + signedSpeedMetersPerSecond, + 0.0, + 0.0); var referencePoint = new TrajectoryPoint( arcLengthMeters: 0.0, poseInWorld: Pose2D.Identity, @@ -164,7 +160,8 @@ namespace MultiWheelC.Tests remainingDistanceMeters: 1.0); return new PathTrackingContext( - vehicleState, + actualTwistInBody, + true, projection, signedSpeedMetersPerSecond, feedforwardCurvaturePerMeter, diff --git a/MultiWheelC/Control/Abstractions/PathTrackingContext.cs b/MultiWheelC/Control/Abstractions/PathTrackingContext.cs index e685493..4483669 100644 --- a/MultiWheelC/Control/Abstractions/PathTrackingContext.cs +++ b/MultiWheelC/Control/Abstractions/PathTrackingContext.cs @@ -1,12 +1,11 @@ using System; -using MultiWheelC.StateEstimation; using MultiWheelC.Trajectory; using MyParking.Shared; namespace MultiWheelC.Control.Abstractions { /// - /// 保存一次轨迹跟踪控制周期使用的车辆状态、轨迹投影和真实时间间隔。 + /// 保存一次轨迹跟踪控制周期使用的刚体速度、轨迹投影和真实时间间隔。 /// public readonly struct PathTrackingContext { @@ -14,7 +13,8 @@ namespace MultiWheelC.Control.Abstractions /// 创建横向和纵向控制器共享的只读控制输入快照。 /// public PathTrackingContext( - VehicleState vehicleState, + Twist2D actualTwistInBody, + bool hasValidVelocityEstimate, TrajectoryProjection projection, double controlReferenceSpeedMetersPerSecond, double feedforwardCurvaturePerMeter, @@ -33,8 +33,13 @@ namespace MultiWheelC.Control.Abstractions EnsureFinite( motionDirectionInBodyRadians, nameof(motionDirectionInBodyRadians)); + NumericGuard.EnsureFinite( + actualTwistInBody, + nameof(actualTwistInBody)); - VehicleState = vehicleState; + ActualTwistInBody = actualTwistInBody; + HasValidVelocityEstimate = + hasValidVelocityEstimate; Projection = projection; ControlReferenceSpeedMetersPerSecond = controlReferenceSpeedMetersPerSecond; @@ -47,9 +52,9 @@ namespace MultiWheelC.Control.Abstractions } /// - /// 获取本周期经过校验的实际车辆位姿和速度状态。 + /// 获取受控刚体坐标系下的实际速度。 /// - public VehicleState VehicleState { get; } + public Twist2D ActualTwistInBody { get; } /// /// 获取实际车体中心投影到参考轨迹后得到的参考状态和跟踪误差。 @@ -71,9 +76,9 @@ namespace MultiWheelC.Control.Abstractions /// public double ActualLongitudinalSpeedMetersPerSecond => Math.Cos(MotionDirectionInBodyRadians) * - VehicleState.TwistInBody.VxMetersPerSecond + + ActualTwistInBody.VxMetersPerSecond + Math.Sin(MotionDirectionInBodyRadians) * - VehicleState.TwistInBody.VyMetersPerSecond; + ActualTwistInBody.VyMetersPerSecond; /// /// 获取当前运动坐标系X轴在车体系中的方向，单位为rad。 @@ -111,10 +116,9 @@ namespace MultiWheelC.Control.Abstractions Projection.RemainingDistanceMeters; /// - /// 获取实际速度是否已经由至少两个连续有效定位样本估算得到。 + /// 获取本周期实际速度估计是否可供闭环控制使用。 /// - public bool HasValidVelocityEstimate => - VehicleState.HasValidVelocityEstimate; + public bool HasValidVelocityEstimate { get; } /// /// 检查控制周期是否为正有限值。 diff --git a/MultiWheelC/Control/Allocation/GcpCommandAllocator.cs b/MultiWheelC/Control/Allocation/GcpCommandAllocator.cs index 77ccdca..008c2b7 100644 --- a/MultiWheelC/Control/Allocation/GcpCommandAllocator.cs +++ b/MultiWheelC/Control/Allocation/GcpCommandAllocator.cs @@ -1,6 +1,7 @@ using System; using MultiWheelC.Control.Abstractions; - +using MyParking.Shared; +// 限制目标角度的最大绝对值，例如不能超过60°。 namespace MultiWheelC.Control.Allocation { /// @@ -13,7 +14,7 @@ namespace MultiWheelC.Control.Allocation /// public GcpCommandAllocator(double maximumGcpAngleRadians) { - EnsureFinitePositive( + NumericGuard.EnsureFinitePositive( maximumGcpAngleRadians, nameof(maximumGcpAngleRadians)); @@ -39,7 +40,7 @@ namespace MultiWheelC.Control.Allocation double speedMetersPerSecond, LateralControlCommand lateralCommand) { - EnsureFinite( + NumericGuard.EnsureFinite( speedMetersPerSecond, nameof(speedMetersPerSecond)); @@ -68,37 +69,5 @@ namespace MultiWheelC.Control.Allocation Math.Min(maximumAbsoluteValue, value)); } - /// - /// 检查参数是否为正有限值。 - /// - private static void EnsureFinitePositive( - double value, - string parameterName) - { - EnsureFinite(value, parameterName); - - if (value <= 0.0) - { - throw new ArgumentOutOfRangeException( - parameterName, - "GCP分配参数必须是正有限值。"); - } - } - - /// - /// 检查参数或命令是否为有限值。 - /// - private static void EnsureFinite( - double value, - string parameterName) - { - if (double.IsNaN(value) || - double.IsInfinity(value)) - { - throw new ArgumentOutOfRangeException( - parameterName, - "GCP分配参数和命令必须是有限值。"); - } - } } } diff --git a/MultiWheelC/Control/Allocation/GcpMotionCommand.cs b/MultiWheelC/Control/Allocation/GcpMotionCommand.cs index f36ce0e..f2ed2a7 100644 --- a/MultiWheelC/Control/Allocation/GcpMotionCommand.cs +++ b/MultiWheelC/Control/Allocation/GcpMotionCommand.cs @@ -1,4 +1,4 @@ -using System; +using MyParking.Shared; namespace MultiWheelC.Control.Allocation { @@ -15,13 +15,13 @@ namespace MultiWheelC.Control.Allocation double frontAngleRadians, double rearAngleRadians) { - EnsureFinite( + NumericGuard.EnsureFinite( speedMetersPerSecond, nameof(speedMetersPerSecond)); - EnsureFinite( + NumericGuard.EnsureFinite( frontAngleRadians, nameof(frontAngleRadians)); - EnsureFinite( + NumericGuard.EnsureFinite( rearAngleRadians, nameof(rearAngleRadians)); @@ -48,20 +48,5 @@ namespace MultiWheelC.Control.Allocation /// public double RearAngleRadians { get; } - /// - /// 检查底盘中间命令是否为有限值。 - /// - private static void EnsureFinite( - double value, - string parameterName) - { - if (double.IsNaN(value) || - double.IsInfinity(value)) - { - throw new ArgumentOutOfRangeException( - parameterName, - "GCP运动命令必须由有限值组成。"); - } - } } } diff --git a/MultiWheelC/Control/Execution/ParkingGeometricController.cs b/MultiWheelC/Control/Execution/ParkingGeometricController.cs index e5a2a3e..ad7880d 100644 --- a/MultiWheelC/Control/Execution/ParkingGeometricController.cs +++ b/MultiWheelC/Control/Execution/ParkingGeometricController.cs @@ -8,9 +8,7 @@ using MyParking.Shared; namespace MultiWheelC.Control.Execution { - /// - /// 表示新版停车机器人单周期轨迹控制的执行结果。 - /// + // 单周期轨迹控制结果。 public enum ParkingControlCycleResult { Inactive = 0, @@ -20,9 +18,7 @@ namespace MultiWheelC.Control.Execution Faulted = 4 } - /// - /// 保存一个轨迹控制周期各阶段的高精度耗时和定位帧更新状态。 - /// + // 单周期各阶段耗时及定位帧更新状态，用于区分控制计算与底层通信延迟。 public readonly struct ParkingControlCycleTiming { public ParkingControlCycleTiming( @@ -66,37 +62,18 @@ namespace MultiWheelC.Control.Execution public ParkingControlCycleResult Result { get; } } - /// - /// 组织状态读取、轨迹投影、横纵向控制、GCP分配和底盘命令执行。 - /// + // 负责单车状态读取、公共轨迹计算和真实底盘命令发送。 public sealed class ParkingGeometricController { - private const double ZeroReferenceSpeedToleranceMetersPerSecond = - 1e-6; - private const double StartupRegionMeters = 0.02; - private const double StartupPreviewDistanceMeters = 0.05; - private const double MaximumStartupSpeedMetersPerSecond = 0.08; - private const double ProjectionBackwardSearchDistanceMeters = - 0.10; - private const double ProjectionForwardSearchDistanceMeters = - 1.00; - private readonly IVehicleStateProvider _stateProvider; - private readonly ILateralController _lateralController; - private readonly ILongitudinalController _longitudinalController; - private readonly GcpCommandAllocator _gcpAllocator; private readonly GcpCommandExecutor _commandExecutor; - private readonly double _motionDirectionInBodyRadians; + private readonly PathTrackingCore _trackingCore; - private Trajectory2D _trajectory; - private double _terminalTravelDirection = 1.0; private long _cycleIndex; private bool _hasPreviousStateTimestamp; private double _previousStateTimestampSeconds; - /// - /// 创建具有终点判定和轨迹偏离保护的单车轨迹控制器。 - /// + // 创建具有终点判定、轨迹偏离保护和曲率前馈预瞄的单车轨迹控制器。 public ParkingGeometricController( IVehicleStateProvider stateProvider, ILateralController lateralController, @@ -119,226 +96,107 @@ namespace MultiWheelC.Control.Execution _stateProvider = stateProvider ?? throw new ArgumentNullException( nameof(stateProvider)); - _lateralController = lateralController ?? - throw new ArgumentNullException( - nameof(lateralController)); - _longitudinalController = longitudinalController ?? - throw new ArgumentNullException( - nameof(longitudinalController)); - _gcpAllocator = gcpAllocator ?? - throw new ArgumentNullException( - nameof(gcpAllocator)); _commandExecutor = commandExecutor ?? throw new ArgumentNullException( nameof(commandExecutor)); - - EnsureFinitePositive( + _trackingCore = new PathTrackingCore( + lateralController, + longitudinalController, + gcpAllocator, finishDistanceMeters, - nameof(finishDistanceMeters)); - EnsureFiniteNonNegative( finishSpeedMetersPerSecond, - nameof(finishSpeedMetersPerSecond)); - EnsureFinitePositive( finishHeadingToleranceRadians, - nameof(finishHeadingToleranceRadians)); - EnsureFinitePositive( maximumDistanceToTrajectoryMeters, - nameof(maximumDistanceToTrajectoryMeters)); - EnsureFiniteNonNegative( terminalBrakingPreviewMeters, - nameof(terminalBrakingPreviewMeters)); - EnsureFinitePositive( terminalApproachDistanceMeters, - nameof(terminalApproachDistanceMeters)); - EnsureFinitePositive( terminalApproachGainPerSecond, - nameof(terminalApproachGainPerSecond)); - EnsureFinitePositive( maximumTerminalApproachSpeedMetersPerSecond, - nameof(maximumTerminalApproachSpeedMetersPerSecond)); - EnsureFiniteNonNegative( curvaturePreviewSeconds, - nameof(curvaturePreviewSeconds)); - EnsureFiniteNonNegative( maximumCurvaturePreviewMeters, - nameof(maximumCurvaturePreviewMeters)); - NumericGuard.EnsureFinite( - motionDirectionInBodyRadians, - nameof(motionDirectionInBodyRadians)); - - if (terminalApproachDistanceMeters <= - finishDistanceMeters) - { - throw new ArgumentOutOfRangeException( - nameof(terminalApproachDistanceMeters), - "终点单向逼近范围必须大于终点位置容差。"); - } - - FinishDistanceMeters = finishDistanceMeters; - FinishSpeedMetersPerSecond = - finishSpeedMetersPerSecond; - FinishHeadingToleranceRadians = - finishHeadingToleranceRadians; - MaximumDistanceToTrajectoryMeters = - maximumDistanceToTrajectoryMeters; - TerminalBrakingPreviewMeters = - terminalBrakingPreviewMeters; - TerminalApproachDistanceMeters = - terminalApproachDistanceMeters; - TerminalApproachGainPerSecond = - terminalApproachGainPerSecond; - MaximumTerminalApproachSpeedMetersPerSecond = - maximumTerminalApproachSpeedMetersPerSecond; - CurvaturePreviewSeconds = curvaturePreviewSeconds; - MaximumCurvaturePreviewMeters = - maximumCurvaturePreviewMeters; - _motionDirectionInBodyRadians = - AngleMath.NormalizeRadians( - motionDirectionInBodyRadians); + motionDirectionInBodyRadians); } - /// - /// 获取终点位置和剩余弧长允许的误差，单位为m。 - /// - public double FinishDistanceMeters { get; } + // 以下控制参数由公共轨迹核心统一持有。 + public double FinishDistanceMeters => + _trackingCore.FinishDistanceMeters; - /// - /// 获取判定轨迹执行完成时允许的最大实际纵向速度，单位为m/s。 - /// - public double FinishSpeedMetersPerSecond { get; } + public double FinishSpeedMetersPerSecond => + _trackingCore.FinishSpeedMetersPerSecond; - /// - /// 获取判定轨迹完成时允许的最大终点航向误差，单位为rad。 - /// - public double FinishHeadingToleranceRadians { get; } + public double FinishHeadingToleranceRadians => + _trackingCore.FinishHeadingToleranceRadians; - /// - /// 获取允许车辆偏离参考轨迹的最大距离，单位为m。 - /// - public double MaximumDistanceToTrajectoryMeters { get; } + public double MaximumDistanceToTrajectoryMeters => + _trackingCore.MaximumDistanceToTrajectoryMeters; - /// - /// 获取沿轨迹提前读取更低制动参考速度的距离，单位为m。 - /// - public double TerminalBrakingPreviewMeters { get; } + public double TerminalBrakingPreviewMeters => + _trackingCore.TerminalBrakingPreviewMeters; - /// - /// 获取切换到终点单向低速逼近的最大剩余弧长，单位为m。 - /// - public double TerminalApproachDistanceMeters { get; } + public double TerminalApproachDistanceMeters => + _trackingCore.TerminalApproachDistanceMeters; - /// - /// 获取由终点纵向剩余距离生成低速参考的比例增益，单位为1/s。 - /// - public double TerminalApproachGainPerSecond { get; } + public double TerminalApproachGainPerSecond => + _trackingCore.TerminalApproachGainPerSecond; - /// - /// 获取终点单向逼近参考速度允许的最大绝对值，单位为m/s。 - /// - public double MaximumTerminalApproachSpeedMetersPerSecond { get; } + public double MaximumTerminalApproachSpeedMetersPerSecond => + _trackingCore.MaximumTerminalApproachSpeedMetersPerSecond; - /// - /// 获取按照车辆纵向速度换算曲率前馈预瞄距离的预测时间，单位为s，0表示关闭。 - /// - public double CurvaturePreviewSeconds { get; } + public double CurvaturePreviewSeconds => + _trackingCore.CurvaturePreviewSeconds; - /// - /// 获取曲率前馈沿轨迹点序允许预瞄的最大距离，单位为m。 - /// - public double MaximumCurvaturePreviewMeters { get; } + public double MaximumCurvaturePreviewMeters => + _trackingCore.MaximumCurvaturePreviewMeters; - /// - /// 获取控制器当前是否持有并正在执行一条轨迹。 - /// - public bool IsActive { get; private set; } + public bool IsActive => _trackingCore.IsActive; - /// - /// 获取最近一次轨迹是否已经满足终点完成条件。 - /// - public bool IsCompleted { get; private set; } + public bool IsCompleted => _trackingCore.IsCompleted; - /// - /// 获取最近一次控制失败原因，正常时为空字符串。 - /// - public string LastFailureReason { get; private set; } = - string.Empty; + public string LastFailureReason => + _trackingCore.LastFailureReason; - /// - /// 获取最近一次控制异常，正常时为空。 - /// - public Exception LastException { get; private set; } + public Exception LastException => + _trackingCore.LastException; - /// - /// 获取最近一次有效车辆状态。 - /// + // 最近一次有效车辆状态仍由单车外层保存。 public VehicleState? LastVehicleState { get; private set; } - /// - /// 获取最近一次车体中心到参考轨迹的投影结果。 - /// - public TrajectoryProjection? LastProjection { get; private set; } + public TrajectoryProjection? LastProjection => + _trackingCore.LastProjection; - /// - /// 获取最近一次由控制器和分配器生成、尚未经过角速度限制的GCP运动命令。 - /// - public GcpMotionCommand? LastRequestedCommand { get; private set; } + public GcpMotionCommand? LastRequestedCommand => + _trackingCore.LastRequestedCommand; - /// - /// 获取最近一次经过GCP角速度限制后实际发送给底盘的运动命令。 - /// + // 经过GCP角速度限制后实际发送给底盘的最近一次命令。 public GcpMotionCommand? LastCommand { get; private set; } - /// - /// 获取最近控制周期实际交给纵向控制器的参考速度，单位为m/s。 - /// - public double? LastControlReferenceSpeedMetersPerSecond { get; private set; } + public double? LastControlReferenceSpeedMetersPerSecond => + _trackingCore.LastControlReferenceSpeedMetersPerSecond; - /// - /// 获取最近控制周期实际采用的曲率前馈预瞄距离，单位为m。 - /// - public double? LastCurvaturePreviewDistanceMeters { get; private set; } + public double? LastCurvaturePreviewDistanceMeters => + _trackingCore.LastCurvaturePreviewDistanceMeters; - /// - /// 获取最近控制周期沿轨迹预瞄后交给横向控制器的曲率，单位为1/m。 - /// - public double? LastFeedforwardCurvaturePerMeter { get; private set; } + public double? LastFeedforwardCurvaturePerMeter => + _trackingCore.LastFeedforwardCurvaturePerMeter; - /// - /// 获取最近一次控制周期的分阶段耗时诊断；尚未执行时为空。 - /// public ParkingControlCycleTiming? LastCycleTiming { get; private set; } - /// - /// 停止当前底盘并从起点开始执行指定二维轨迹。 - /// + // 先停车，再从轨迹起点重置公共控制核心和单车执行诊断。 public void Start(Trajectory2D trajectory) { - if (trajectory == null) - { - throw new ArgumentNullException( - nameof(trajectory)); - } - - StopAndResetControllers(); - _trajectory = trajectory; - _terminalTravelDirection = - ResolveTerminalTravelDirection(trajectory); - IsActive = true; - IsCompleted = false; - ClearDiagnostics(); + _commandExecutor.Stop(); + _trackingCore.Start(trajectory); + ClearExecutionDiagnostics(); } - /// - /// 读取本周期车辆状态并执行一次完整的轨迹跟踪控制计算。 - /// + // 读取车辆状态、调用公共核心并发送一次真实底盘命令。 public ParkingControlCycleResult ExecuteCycle( double deltaTimeSeconds) { - EnsureFinitePositive( + NumericGuard.EnsureFinitePositive( deltaTimeSeconds, nameof(deltaTimeSeconds)); - if (!IsActive || _trajectory == null) + if (!_trackingCore.IsActive) { return ParkingControlCycleResult.Inactive; } @@ -350,8 +208,6 @@ namespace MultiWheelC.Control.Execution var projectionMilliseconds = 0.0; var controllerComputeMilliseconds = 0.0; var commandSendMilliseconds = 0.0; - var controllerComputeStartTimestamp = 0L; - var controllerComputeCompleted = false; var hasStateTimestamp = false; var stateTimestampSeconds = 0.0; var stateTimestampChanged = false; @@ -379,7 +235,7 @@ namespace MultiWheelC.Control.Execution if (!stateAvailable) { - var stopCommandStartTimestamp = + var stopStartTimestamp = Stopwatch.GetTimestamp(); try { @@ -389,7 +245,7 @@ namespace MultiWheelC.Control.Execution { commandSendMilliseconds = GetElapsedMilliseconds( - stopCommandStartTimestamp); + stopStartTimestamp); } cycleResult = ParkingControlCycleResult @@ -409,105 +265,44 @@ namespace MultiWheelC.Control.Execution stateTimestampSeconds; _hasPreviousStateTimestamp = true; - // 首周期允许全局定位轨迹进度；后续周期仅在上次进度 - // 前后有限物理距离内搜索，避免交叉或平行轨迹间跳段。 - var projectionStartTimestamp = - Stopwatch.GetTimestamp(); - TrajectoryProjection projection; - try - { - projection = LastProjection.HasValue - ? TrajectoryProjector.Project( - _trajectory, - vehicleState.PoseInWorld, - LastProjection.Value.ArcLengthMeters, - ProjectionBackwardSearchDistanceMeters, - ProjectionForwardSearchDistanceMeters) - : TrajectoryProjector.Project( - _trajectory, - vehicleState.PoseInWorld); - } - finally - { - projectionMilliseconds = - GetElapsedMilliseconds( - projectionStartTimestamp); - } + var output = _trackingCore.Compute( + vehicleState.PoseInWorld, + vehicleState.TwistInBody, + vehicleState.HasValidVelocityEstimate, + deltaTimeSeconds); + projectionMilliseconds = + output.ProjectionMilliseconds; + controllerComputeMilliseconds = + output.ControllerComputeMilliseconds; - LastProjection = projection; - controllerComputeStartTimestamp = - Stopwatch.GetTimestamp(); - - if (projection.DistanceToTrajectoryMeters > - MaximumDistanceToTrajectoryMeters) + if (output.Result == + PathTrackingCycleResult.Completed) { - cycleResult = EnterFault( - "车辆距离参考轨迹" + - $"{projection.DistanceToTrajectoryMeters:F3}m，" + - "超过允许值" + - $"{MaximumDistanceToTrajectoryMeters:F3}m。"); - return cycleResult; - } - - if (HasReachedEnd( - vehicleState, - projection)) - { - CompleteTrajectory(); + commandSendMilliseconds = + StopForCompletedTrajectory(); cycleResult = ParkingControlCycleResult.Completed; return cycleResult; } - if (HasStoppedAtUnsatisfiedTerminal( - vehicleState, - projection, - out var terminalFailureReason)) + if (output.Result == + PathTrackingCycleResult.Faulted) { - cycleResult = EnterFault( - terminalFailureReason); + commandSendMilliseconds = + StopForTrackingFault(); + cycleResult = + ParkingControlCycleResult.Faulted; return cycleResult; } - var controlReferenceSpeedMetersPerSecond = - ResolveControlReferenceSpeed( - vehicleState, - projection); - LastControlReferenceSpeedMetersPerSecond = - controlReferenceSpeedMetersPerSecond; - var curvaturePreviewDistanceMeters = - ResolveCurvaturePreviewDistanceMeters( - vehicleState, - controlReferenceSpeedMetersPerSecond); - var feedforwardCurvaturePerMeter = - ResolveFeedforwardCurvaturePerMeter( - projection, - curvaturePreviewDistanceMeters); - LastCurvaturePreviewDistanceMeters = - curvaturePreviewDistanceMeters; - LastFeedforwardCurvaturePerMeter = - feedforwardCurvaturePerMeter; - var context = new PathTrackingContext( - vehicleState, - projection, - controlReferenceSpeedMetersPerSecond, - feedforwardCurvaturePerMeter, - deltaTimeSeconds, - _motionDirectionInBodyRadians); - var lateralCommand = - _lateralController.Compute(context); - var commandSpeedMetersPerSecond = - _longitudinalController - .ComputeSpeedMetersPerSecond(context); - var gcpCommand = _gcpAllocator.Allocate( - commandSpeedMetersPerSecond, - lateralCommand); - LastRequestedCommand = gcpCommand; - - controllerComputeMilliseconds = - GetElapsedMilliseconds( - controllerComputeStartTimestamp); - controllerComputeCompleted = true; + if (output.Result != + PathTrackingCycleResult.CommandGenerated || + !output.Command.HasValue) + { + cycleResult = + ParkingControlCycleResult.Inactive; + return cycleResult; + } var commandSendStartTimestamp = Stopwatch.GetTimestamp(); @@ -516,7 +311,7 @@ namespace MultiWheelC.Control.Execution { commandSucceeded = _commandExecutor.Execute( - gcpCommand, + output.Command.Value, deltaTimeSeconds); } finally @@ -538,9 +333,6 @@ namespace MultiWheelC.Control.Execution LastCommand = _commandExecutor.LastSentCommand; - - LastFailureReason = string.Empty; - LastException = null; cycleResult = ParkingControlCycleResult.CommandSent; return cycleResult; @@ -555,14 +347,6 @@ namespace MultiWheelC.Control.Execution } finally { - if (controllerComputeStartTimestamp != 0L && - !controllerComputeCompleted) - { - controllerComputeMilliseconds = - GetElapsedMilliseconds( - controllerComputeStartTimestamp); - } - LastCycleTiming = new ParkingControlCycleTiming( cycleIndex, @@ -580,462 +364,66 @@ namespace MultiWheelC.Control.Execution } } - /// - /// 主动取消当前轨迹、立即停车并清除全部控制器状态。 - /// + // 主动取消当前轨迹、立即停车并清除全部控制状态。 public void Cancel() { - StopAndResetControllers(); - _trajectory = null; - _terminalTravelDirection = 1.0; - IsActive = false; - IsCompleted = false; - ClearDiagnostics(); + _commandExecutor.Stop(); + _trackingCore.Cancel(); + ClearExecutionDiagnostics(); } - /// - /// 在普通空间速度规划和终点单向低速逼近之间选择本周期参考速度。 - /// - private double ResolveControlReferenceSpeed( - VehicleState vehicleState, - TrajectoryProjection projection) - { - if (projection.RemainingDistanceMeters > - TerminalApproachDistanceMeters) - { - return ResolveReferenceSpeedForControl( - projection); - } - - return ResolveTerminalApproachSpeed( - vehicleState); - } - - /// - /// 根据有效实际纵向速度或控制参考速度计算带上限的曲率前馈预瞄距离。 - /// - private double ResolveCurvaturePreviewDistanceMeters( - VehicleState vehicleState, - double controlReferenceSpeedMetersPerSecond) - { - if (CurvaturePreviewSeconds <= 0.0 || - MaximumCurvaturePreviewMeters <= 0.0) - { - return 0.0; - } - - var previewSpeedMetersPerSecond = - vehicleState.HasValidVelocityEstimate - ? CalculateActualLongitudinalSpeedMetersPerSecond( - vehicleState) - : Math.Abs( - controlReferenceSpeedMetersPerSecond); - - return Math.Min( - MaximumCurvaturePreviewMeters, - previewSpeedMetersPerSecond * - CurvaturePreviewSeconds); - } - - /// - /// 沿轨迹实际执行点序向前采样专供横向几何前馈使用的曲率。 - /// - private double ResolveFeedforwardCurvaturePerMeter( - TrajectoryProjection projection, - double previewDistanceMeters) - { - var previewArcLengthMeters = Math.Min( - _trajectory.TotalLengthMeters, - projection.ArcLengthMeters + - previewDistanceMeters); - - return _trajectory - .SampleAtArcLength(previewArcLengthMeters) - .CurvaturePerMeter; - } - - /// - /// 根据终点车头方向上的有符号剩余距离生成只保持原轨迹行驶方向的低速参考。 - /// - private double ResolveTerminalApproachSpeed( - VehicleState vehicleState) - { - var distanceToEndMeters = - CalculateDistanceToEndMeters( - vehicleState); - var headingErrorToEndRadians = - CalculateHeadingErrorToEndRadians( - vehicleState); - - // 一旦位置和航向已经进入完成容差，先要求纵向停车； - // 后续周期在实际速度也满足条件后完成轨迹。 - if (distanceToEndMeters <= - FinishDistanceMeters && - headingErrorToEndRadians <= - FinishHeadingToleranceRadians) - { - return 0.0; - } - - var endPose = - _trajectory.EndPoint.PoseInWorld; - var deltaX = - endPose.XMeters - - vehicleState.PoseInWorld.XMeters; - var deltaY = - endPose.YMeters - - vehicleState.PoseInWorld.YMeters; - var longitudinalErrorMeters = - deltaX * Math.Cos(endPose.YawRadians) + - deltaY * Math.Sin(endPose.YawRadians); - var remainingAlongTravelMeters = - _terminalTravelDirection * - longitudinalErrorMeters; - - // 到达或越过终点后只停车，禁止生成与原轨迹方向相反的修正速度。 - if (remainingAlongTravelMeters <= 0.0) - { - return 0.0; - } - - var speedMagnitudeMetersPerSecond = - Math.Min( - MaximumTerminalApproachSpeedMetersPerSecond, - TerminalApproachGainPerSecond * - remainingAlongTravelMeters); - - return _terminalTravelDirection * - speedMagnitudeMetersPerSecond; - } - - /// - /// 在轨迹起步区域内保持最低释放速度，避免空间速度曲线零速固定点。 - /// - private double ResolveReferenceSpeedForControl( - TrajectoryProjection projection) - { - var currentReferenceSpeed = - projection.ReferencePoint - .ReferenceSpeedMetersPerSecond; - - currentReferenceSpeed = - ApplyTerminalBrakingPreview( - projection, - currentReferenceSpeed); - - var isInStartupRegion = - projection.ArcLengthMeters <= - StartupRegionMeters && - projection.RemainingDistanceMeters > - FinishDistanceMeters; - - if (!isInStartupRegion) - { - return currentReferenceSpeed; - } - - var previewArcLengthMeters = Math.Min( - _trajectory.TotalLengthMeters, - projection.ArcLengthMeters + - StartupPreviewDistanceMeters); - - var previewReferenceSpeed = - _trajectory - .SampleAtArcLength( - previewArcLengthMeters) - .ReferenceSpeedMetersPerSecond; - - if (Math.Abs(previewReferenceSpeed) <= - ZeroReferenceSpeedToleranceMetersPerSecond) - { - return currentReferenceSpeed; - } - - var startupReleaseSpeed = - Math.Sign(previewReferenceSpeed) * - Math.Min( - Math.Abs(previewReferenceSpeed), - MaximumStartupSpeedMetersPerSecond); - - // 当前空间速度已经高于起步释放速度时， - // 正常交还给原始速度曲线。 - if (Math.Sign(currentReferenceSpeed) == - Math.Sign(startupReleaseSpeed) && - Math.Abs(currentReferenceSpeed) >= - Math.Abs(startupReleaseSpeed)) - { - return currentReferenceSpeed; - } - - return startupReleaseSpeed; - } - - /// - /// 采用前方更低的同方向参考速度，使车辆在终点减速段提前制动。 - /// - private double ApplyTerminalBrakingPreview( - TrajectoryProjection projection, - double currentReferenceSpeed) - { - if (TerminalBrakingPreviewMeters <= 0.0) - { - return currentReferenceSpeed; - } - - var previewArcLengthMeters = Math.Min( - _trajectory.TotalLengthMeters, - projection.ArcLengthMeters + - TerminalBrakingPreviewMeters); - var previewReferenceSpeed = - _trajectory - .SampleAtArcLength( - previewArcLengthMeters) - .ReferenceSpeedMetersPerSecond; - - var previewIsStop = - Math.Abs(previewReferenceSpeed) <= - ZeroReferenceSpeedToleranceMetersPerSecond; - var hasSameDirection = - Math.Sign(previewReferenceSpeed) == - Math.Sign(currentReferenceSpeed); - var previewIsSlower = - Math.Abs(previewReferenceSpeed) < - Math.Abs(currentReferenceSpeed); - - if (previewIsSlower && - (previewIsStop || hasSameDirection)) - { - return previewReferenceSpeed; - } - - return currentReferenceSpeed; - } - - /// - /// 从轨迹终点前最后一个非零参考速度解析单向逼近允许的行驶方向。 - /// - private static double ResolveTerminalTravelDirection( - Trajectory2D trajectory) - { - for (var index = trajectory.Count - 1; - index >= 0; - index--) - { - var referenceSpeedMetersPerSecond = - trajectory[index] - .ReferenceSpeedMetersPerSecond; - - if (Math.Abs(referenceSpeedMetersPerSecond) > - ZeroReferenceSpeedToleranceMetersPerSecond) - { - return Math.Sign( - referenceSpeedMetersPerSecond); - } - } - - throw new ArgumentException( - "轨迹必须在终点前包含至少一个非零参考速度，才能确定终点单向逼近方向。", - nameof(trajectory)); - } - - /// - /// 根据终点距离、剩余弧长和实际线速度判断轨迹是否完成。 - /// - private bool HasReachedEnd( - VehicleState vehicleState, - TrajectoryProjection projection) - { - if (!vehicleState.HasValidVelocityEstimate) - { - return false; - } - - return projection.RemainingDistanceMeters <= - FinishDistanceMeters && - CalculateDistanceToEndMeters( - vehicleState) <= - FinishDistanceMeters && - CalculateHeadingErrorToEndRadians( - vehicleState) <= - FinishHeadingToleranceRadians && - CalculateActualLongitudinalSpeedMetersPerSecond( - vehicleState) <= - FinishSpeedMetersPerSecond; - } - - /// - /// 检查车辆是否已在终点零速参考处停稳但最终位置或航向仍不合格。 - /// - private bool HasStoppedAtUnsatisfiedTerminal( - VehicleState vehicleState, - TrajectoryProjection projection, - out string failureReason) - { - failureReason = string.Empty; - - var isTerminalZeroSpeedReference = - projection.RemainingDistanceMeters <= - FinishDistanceMeters && - Math.Abs( - projection.ReferencePoint - .ReferenceSpeedMetersPerSecond) <= - ZeroReferenceSpeedToleranceMetersPerSecond; - - if (!isTerminalZeroSpeedReference || - !vehicleState.HasValidVelocityEstimate || - CalculateActualLongitudinalSpeedMetersPerSecond( - vehicleState) > - FinishSpeedMetersPerSecond) - { - return false; - } - - var positionErrorMeters = - CalculateDistanceToEndMeters( - vehicleState); - var headingErrorRadians = - CalculateHeadingErrorToEndRadians( - vehicleState); - - failureReason = - "车辆已在终点零速参考处停稳，但终点精度不满足要求：" + - $"位置误差={positionErrorMeters:F3}m，" + - "航向误差=" + - $"{AngleMath.RadiansToDegrees(headingErrorRadians):F2}°。"; - return true; - } - - /// - /// 计算实际车体中心到轨迹终点的欧氏距离，单位为m。 - /// - private double CalculateDistanceToEndMeters( - VehicleState vehicleState) - { - var endPoint = _trajectory.EndPoint.PoseInWorld; - var deltaX = - vehicleState.PoseInWorld.XMeters - - endPoint.XMeters; - var deltaY = - vehicleState.PoseInWorld.YMeters - - endPoint.YMeters; - - return Math.Sqrt( - deltaX * deltaX + - deltaY * deltaY); - } - - /// - /// 计算实际车体航向到轨迹终点航向的最短角度误差绝对值，单位为rad。 - /// - private double CalculateHeadingErrorToEndRadians( - VehicleState vehicleState) - { - return Math.Abs( - AngleMath.ShortestDifferenceRadians( - _trajectory.EndPoint - .PoseInWorld.YawRadians, - vehicleState - .PoseInWorld.YawRadians)); - } - - /// - /// 计算车辆沿当前运动坐标系X轴实际速度的绝对值，单位为m/s。 - /// - private double CalculateActualLongitudinalSpeedMetersPerSecond( - VehicleState vehicleState) - { - return Math.Abs( - Math.Cos(_motionDirectionInBodyRadians) * - vehicleState.TwistInBody.VxMetersPerSecond + - Math.Sin(_motionDirectionInBodyRadians) * - vehicleState.TwistInBody.VyMetersPerSecond); - } - - /// - /// 在状态暂不可用时停车并重置反馈控制器，同时保留轨迹等待下一周期恢复。 - /// + // 状态不可用时停车并重置反馈历史，同时保留轨迹等待恢复。 private void StopForUnavailableState() { _commandExecutor.Stop(); - _lateralController.Reset(); - _longitudinalController.Reset(); - LastRequestedCommand = null; + _trackingCore.PauseForUnavailableState( + "当前无法获得有效车辆状态，底盘已停车并等待定位恢复。"); LastCommand = null; - LastFailureReason = - "当前无法获得有效车辆状态，底盘已停车并等待定位恢复。"; - LastException = null; } - /// - /// 完成当前轨迹并停车，但保留最后状态和投影供实验记录读取。 - /// - private void CompleteTrajectory() + // 公共核心完成轨迹后发送停车，并保留零命令供实验记录。 + private double StopForCompletedTrajectory() { - StopAndResetControllers(); - IsActive = false; - IsCompleted = true; - LastRequestedCommand = new GcpMotionCommand( - 0.0, - 0.0, - 0.0); + var startTimestamp = Stopwatch.GetTimestamp(); + _commandExecutor.Stop(); LastCommand = new GcpMotionCommand( 0.0, 0.0, 0.0); - LastFailureReason = string.Empty; - LastException = null; + return GetElapsedMilliseconds(startTimestamp); } - /// - /// 发生不可继续的控制故障时停车、退出活动状态并保存诊断信息。 - /// + // 公共核心故障后只负责真实底盘停车，不覆盖核心保存的失败原因。 + private double StopForTrackingFault() + { + var startTimestamp = Stopwatch.GetTimestamp(); + _commandExecutor.Stop(); + LastCommand = null; + return GetElapsedMilliseconds(startTimestamp); + } + + // 将底盘执行或外层异常同步到公共核心，并立即停车。 private ParkingControlCycleResult EnterFault( string reason, Exception exception = null) { - StopAndResetControllers(); - IsActive = false; - IsCompleted = false; - LastRequestedCommand = null; + _commandExecutor.Stop(); + _trackingCore.Fail(reason, exception); LastCommand = null; - LastFailureReason = reason; - LastException = exception; return ParkingControlCycleResult.Faulted; } - /// - /// 立即停止底盘并清除横向和纵向控制器的跨周期状态。 - /// - private void StopAndResetControllers() - { - _commandExecutor.Stop(); - _lateralController.Reset(); - _longitudinalController.Reset(); - } - - /// - /// 清除上一条轨迹留下的状态、命令和故障诊断信息。 - /// - private void ClearDiagnostics() + // 清除只属于单车状态读取、发送和周期计时的诊断。 + private void ClearExecutionDiagnostics() { _cycleIndex = 0; _hasPreviousStateTimestamp = false; _previousStateTimestampSeconds = 0.0; LastVehicleState = null; - LastProjection = null; - LastRequestedCommand = null; LastCommand = null; - LastControlReferenceSpeedMetersPerSecond = null; - LastCurvaturePreviewDistanceMeters = null; - LastFeedforwardCurvaturePerMeter = null; LastCycleTiming = null; - LastFailureReason = string.Empty; - LastException = null; } - /// - /// 将Stopwatch高精度时间戳差转换为毫秒。 - /// private static double GetElapsedMilliseconds( long startTimestamp) { @@ -1043,39 +431,5 @@ namespace MultiWheelC.Control.Execution 1000.0 / Stopwatch.Frequency; } - - /// - /// 检查控制参数是否为正有限值。 - /// - private static void EnsureFinitePositive( - double value, - string parameterName) - { - if (double.IsNaN(value) || - double.IsInfinity(value) || - value <= 0.0) - { - throw new ArgumentOutOfRangeException( - parameterName, - "轨迹控制器距离和周期参数必须是正有限值。"); - } - } - - /// - /// 检查控制参数是否为非负有限值。 - /// - 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/Control/Execution/PathTrackingCore.cs b/MultiWheelC/Control/Execution/PathTrackingCore.cs new file mode 100644 index 0000000..d3f0ef2 --- /dev/null +++ b/MultiWheelC/Control/Execution/PathTrackingCore.cs @@ -0,0 +1,793 @@ +using System; +using System.Diagnostics; +using MultiWheelC.Control.Abstractions; +using MultiWheelC.Control.Allocation; +using MultiWheelC.Trajectory; +using MyParking.Shared; + +namespace MultiWheelC.Control.Execution +{ + // 表示公共轨迹跟踪核心单周期的计算结果，不包含底盘发送结果。 + public enum PathTrackingCycleResult + { + Inactive = 0, + CommandGenerated = 1, + Completed = 2, + Faulted = 3 + } + + // 保存公共核心生成的GCP命令、轨迹投影和分阶段计算耗时。 + public readonly struct PathTrackingCycleOutput + { + public PathTrackingCycleOutput( + PathTrackingCycleResult result, + GcpMotionCommand? command, + TrajectoryProjection? projection, + double projectionMilliseconds, + double controllerComputeMilliseconds) + { + Result = result; + Command = command; + Projection = projection; + ProjectionMilliseconds = projectionMilliseconds; + ControllerComputeMilliseconds = + controllerComputeMilliseconds; + } + + public PathTrackingCycleResult Result { get; } + + public GcpMotionCommand? Command { get; } + + public TrajectoryProjection? Projection { get; } + + public double ProjectionMilliseconds { get; } + + public double ControllerComputeMilliseconds { get; } + } + + // 统一处理单车和虚拟车队共有的轨迹投影、速度整形及GCP命令生成。 + public sealed class PathTrackingCore + { + private const double ZeroReferenceSpeedToleranceMetersPerSecond = + 1e-6; + private const double StartupRegionMeters = 0.02; + private const double StartupPreviewDistanceMeters = 0.05; + private const double MaximumStartupSpeedMetersPerSecond = 0.08; + private const double ProjectionBackwardSearchDistanceMeters = + 0.10; + private const double ProjectionForwardSearchDistanceMeters = + 1.00; + + private readonly ILateralController _lateralController; + private readonly ILongitudinalController _longitudinalController; + private readonly GcpCommandAllocator _gcpAllocator; + private readonly double _motionDirectionInBodyRadians; + + private Trajectory2D _trajectory; + private double _terminalTravelDirection = 1.0; + + public PathTrackingCore( + ILateralController lateralController, + ILongitudinalController longitudinalController, + GcpCommandAllocator gcpAllocator, + double finishDistanceMeters = 0.04, + double finishSpeedMetersPerSecond = 0.02, + double finishHeadingToleranceRadians = + 3.0 * Math.PI / 180.0, + double maximumDistanceToTrajectoryMeters = 0.30, + double terminalBrakingPreviewMeters = 0.02, + double terminalApproachDistanceMeters = 0.10, + double terminalApproachGainPerSecond = 0.8, + double maximumTerminalApproachSpeedMetersPerSecond = 0.05, + double curvaturePreviewSeconds = 0.20, + double maximumCurvaturePreviewMeters = 0.12, + double motionDirectionInBodyRadians = 0.0) + { + _lateralController = lateralController ?? + throw new ArgumentNullException( + nameof(lateralController)); + _longitudinalController = longitudinalController ?? + throw new ArgumentNullException( + nameof(longitudinalController)); + _gcpAllocator = gcpAllocator ?? + throw new ArgumentNullException( + nameof(gcpAllocator)); + + NumericGuard.EnsureFinitePositive( + finishDistanceMeters, + nameof(finishDistanceMeters)); + NumericGuard.EnsureFiniteNonNegative( + finishSpeedMetersPerSecond, + nameof(finishSpeedMetersPerSecond)); + NumericGuard.EnsureFinitePositive( + finishHeadingToleranceRadians, + nameof(finishHeadingToleranceRadians)); + NumericGuard.EnsureFinitePositive( + maximumDistanceToTrajectoryMeters, + nameof(maximumDistanceToTrajectoryMeters)); + NumericGuard.EnsureFiniteNonNegative( + terminalBrakingPreviewMeters, + nameof(terminalBrakingPreviewMeters)); + NumericGuard.EnsureFinitePositive( + terminalApproachDistanceMeters, + nameof(terminalApproachDistanceMeters)); + NumericGuard.EnsureFinitePositive( + terminalApproachGainPerSecond, + nameof(terminalApproachGainPerSecond)); + NumericGuard.EnsureFinitePositive( + maximumTerminalApproachSpeedMetersPerSecond, + nameof(maximumTerminalApproachSpeedMetersPerSecond)); + NumericGuard.EnsureFiniteNonNegative( + curvaturePreviewSeconds, + nameof(curvaturePreviewSeconds)); + NumericGuard.EnsureFiniteNonNegative( + maximumCurvaturePreviewMeters, + nameof(maximumCurvaturePreviewMeters)); + NumericGuard.EnsureFinite( + motionDirectionInBodyRadians, + nameof(motionDirectionInBodyRadians)); + + if (terminalApproachDistanceMeters <= + finishDistanceMeters) + { + throw new ArgumentOutOfRangeException( + nameof(terminalApproachDistanceMeters), + "终点单向逼近范围必须大于终点位置容差。"); + } + + FinishDistanceMeters = finishDistanceMeters; + FinishSpeedMetersPerSecond = + finishSpeedMetersPerSecond; + FinishHeadingToleranceRadians = + finishHeadingToleranceRadians; + MaximumDistanceToTrajectoryMeters = + maximumDistanceToTrajectoryMeters; + TerminalBrakingPreviewMeters = + terminalBrakingPreviewMeters; + TerminalApproachDistanceMeters = + terminalApproachDistanceMeters; + TerminalApproachGainPerSecond = + terminalApproachGainPerSecond; + MaximumTerminalApproachSpeedMetersPerSecond = + maximumTerminalApproachSpeedMetersPerSecond; + CurvaturePreviewSeconds = curvaturePreviewSeconds; + MaximumCurvaturePreviewMeters = + maximumCurvaturePreviewMeters; + _motionDirectionInBodyRadians = + AngleMath.NormalizeRadians( + motionDirectionInBodyRadians); + } + + public double FinishDistanceMeters { get; } + + public double FinishSpeedMetersPerSecond { get; } + + public double FinishHeadingToleranceRadians { get; } + + public double MaximumDistanceToTrajectoryMeters { get; } + + public double TerminalBrakingPreviewMeters { get; } + + public double TerminalApproachDistanceMeters { get; } + + public double TerminalApproachGainPerSecond { get; } + + public double MaximumTerminalApproachSpeedMetersPerSecond { get; } + + public double CurvaturePreviewSeconds { get; } + + public double MaximumCurvaturePreviewMeters { get; } + + public bool IsActive { get; private set; } + + public bool IsCompleted { get; private set; } + + public string LastFailureReason { get; private set; } = + string.Empty; + + public Exception LastException { get; private set; } + + public TrajectoryProjection? LastProjection { get; private set; } + + public GcpMotionCommand? LastRequestedCommand { get; private set; } + + public double? LastControlReferenceSpeedMetersPerSecond { get; private set; } + + public double? LastCurvaturePreviewDistanceMeters { get; private set; } + + public double? LastFeedforwardCurvaturePerMeter { get; private set; } + + // 重置跨周期状态，并从轨迹起点开始新的跟踪过程。 + public void Start(Trajectory2D trajectory) + { + if (trajectory == null) + { + throw new ArgumentNullException( + nameof(trajectory)); + } + + var terminalTravelDirection = + ResolveTerminalTravelDirection(trajectory); + + ResetFeedbackControllers(); + _trajectory = trajectory; + _terminalTravelDirection = + terminalTravelDirection; + IsActive = true; + IsCompleted = false; + ClearDiagnostics(); + } + + // 将受控刚体的位姿和速度转换为本周期GCP命令。 + public PathTrackingCycleOutput Compute( + Pose2D poseInWorld, + Twist2D actualTwistInBody, + bool hasValidVelocityEstimate, + double deltaTimeSeconds) + { + NumericGuard.EnsureFinite( + poseInWorld, + nameof(poseInWorld)); + NumericGuard.EnsureFinite( + actualTwistInBody, + nameof(actualTwistInBody)); + NumericGuard.EnsureFinitePositive( + deltaTimeSeconds, + nameof(deltaTimeSeconds)); + + if (!IsActive || _trajectory == null) + { + return new PathTrackingCycleOutput( + PathTrackingCycleResult.Inactive, + null, + LastProjection, + 0.0, + 0.0); + } + + var projectionStartTimestamp = + Stopwatch.GetTimestamp(); + var projectionCompleted = false; + var projectionMilliseconds = 0.0; + var controllerComputeStartTimestamp = 0L; + + try + { + var projection = LastProjection.HasValue + ? TrajectoryProjector.Project( + _trajectory, + poseInWorld, + LastProjection.Value.ArcLengthMeters, + ProjectionBackwardSearchDistanceMeters, + ProjectionForwardSearchDistanceMeters) + : TrajectoryProjector.Project( + _trajectory, + poseInWorld); + + projectionMilliseconds = + GetElapsedMilliseconds( + projectionStartTimestamp); + projectionCompleted = true; + LastProjection = projection; + controllerComputeStartTimestamp = + Stopwatch.GetTimestamp(); + + if (projection.DistanceToTrajectoryMeters > + MaximumDistanceToTrajectoryMeters) + { + Fail( + "受控刚体距离参考轨迹" + + $"{projection.DistanceToTrajectoryMeters:F3}m，" + + "超过允许值" + + $"{MaximumDistanceToTrajectoryMeters:F3}m。"); + return CreateOutput( + PathTrackingCycleResult.Faulted, + null, + projection, + projectionMilliseconds, + controllerComputeStartTimestamp); + } + + if (HasReachedEnd( + poseInWorld, + actualTwistInBody, + hasValidVelocityEstimate, + projection)) + { + CompleteTrajectory(); + return CreateOutput( + PathTrackingCycleResult.Completed, + null, + projection, + projectionMilliseconds, + controllerComputeStartTimestamp); + } + + if (HasStoppedAtUnsatisfiedTerminal( + poseInWorld, + actualTwistInBody, + hasValidVelocityEstimate, + projection, + out var terminalFailureReason)) + { + Fail(terminalFailureReason); + return CreateOutput( + PathTrackingCycleResult.Faulted, + null, + projection, + projectionMilliseconds, + controllerComputeStartTimestamp); + } + + var controlReferenceSpeedMetersPerSecond = + ResolveControlReferenceSpeed( + poseInWorld, + projection); + LastControlReferenceSpeedMetersPerSecond = + controlReferenceSpeedMetersPerSecond; + var curvaturePreviewDistanceMeters = + ResolveCurvaturePreviewDistanceMeters( + actualTwistInBody, + hasValidVelocityEstimate, + controlReferenceSpeedMetersPerSecond); + LastCurvaturePreviewDistanceMeters = + curvaturePreviewDistanceMeters; + var feedforwardCurvaturePerMeter = + ResolveFeedforwardCurvaturePerMeter( + projection, + curvaturePreviewDistanceMeters); + LastFeedforwardCurvaturePerMeter = + feedforwardCurvaturePerMeter; + var context = new PathTrackingContext( + actualTwistInBody, + hasValidVelocityEstimate, + projection, + controlReferenceSpeedMetersPerSecond, + feedforwardCurvaturePerMeter, + deltaTimeSeconds, + _motionDirectionInBodyRadians); + var lateralCommand = + _lateralController.Compute(context); + var commandSpeedMetersPerSecond = + _longitudinalController + .ComputeSpeedMetersPerSecond(context); + var command = _gcpAllocator.Allocate( + commandSpeedMetersPerSecond, + lateralCommand); + + LastRequestedCommand = command; + LastFailureReason = string.Empty; + LastException = null; + + return CreateOutput( + PathTrackingCycleResult.CommandGenerated, + command, + projection, + projectionMilliseconds, + controllerComputeStartTimestamp); + } + catch (Exception exception) + { + if (!projectionCompleted) + { + projectionMilliseconds = + GetElapsedMilliseconds( + projectionStartTimestamp); + } + + Fail( + "轨迹跟踪核心计算异常：" + + exception.Message, + exception); + + return CreateOutput( + PathTrackingCycleResult.Faulted, + null, + LastProjection, + projectionMilliseconds, + controllerComputeStartTimestamp); + } + } + + // 状态暂不可用时重置反馈历史，但保留当前轨迹和投影进度等待恢复。 + public void PauseForUnavailableState(string reason) + { + ResetFeedbackControllers(); + LastRequestedCommand = null; + LastFailureReason = reason ?? string.Empty; + LastException = null; + } + + // 将外层执行故障同步到公共核心，并终止当前轨迹。 + public void Fail( + string reason, + Exception exception = null) + { + ResetFeedbackControllers(); + _trajectory = null; + IsActive = false; + IsCompleted = false; + LastRequestedCommand = null; + LastFailureReason = reason ?? string.Empty; + LastException = exception; + } + + // 取消当前轨迹并清除全部跟踪状态。 + public void Cancel() + { + ResetFeedbackControllers(); + _trajectory = null; + _terminalTravelDirection = 1.0; + IsActive = false; + IsCompleted = false; + ClearDiagnostics(); + } + + private double ResolveControlReferenceSpeed( + Pose2D poseInWorld, + TrajectoryProjection projection) + { + if (projection.RemainingDistanceMeters > + TerminalApproachDistanceMeters) + { + return ResolveReferenceSpeedForControl( + projection); + } + + return ResolveTerminalApproachSpeed( + poseInWorld); + } + + private double ResolveCurvaturePreviewDistanceMeters( + Twist2D actualTwistInBody, + bool hasValidVelocityEstimate, + double controlReferenceSpeedMetersPerSecond) + { + if (CurvaturePreviewSeconds <= 0.0 || + MaximumCurvaturePreviewMeters <= 0.0) + { + return 0.0; + } + + var previewSpeedMetersPerSecond = + hasValidVelocityEstimate + ? CalculateActualLongitudinalSpeedMetersPerSecond( + actualTwistInBody) + : Math.Abs( + controlReferenceSpeedMetersPerSecond); + + return Math.Min( + MaximumCurvaturePreviewMeters, + previewSpeedMetersPerSecond * + CurvaturePreviewSeconds); + } + + private double ResolveFeedforwardCurvaturePerMeter( + TrajectoryProjection projection, + double previewDistanceMeters) + { + var previewArcLengthMeters = Math.Min( + _trajectory.TotalLengthMeters, + projection.ArcLengthMeters + + previewDistanceMeters); + + return _trajectory + .SampleAtArcLength(previewArcLengthMeters) + .CurvaturePerMeter; + } + + private double ResolveTerminalApproachSpeed( + Pose2D poseInWorld) + { + var distanceToEndMeters = + CalculateDistanceToEndMeters( + poseInWorld); + var headingErrorToEndRadians = + CalculateHeadingErrorToEndRadians( + poseInWorld); + + if (distanceToEndMeters <= + FinishDistanceMeters && + headingErrorToEndRadians <= + FinishHeadingToleranceRadians) + { + return 0.0; + } + + var endPose = _trajectory.EndPoint.PoseInWorld; + var deltaX = endPose.XMeters - + poseInWorld.XMeters; + var deltaY = endPose.YMeters - + poseInWorld.YMeters; + var longitudinalErrorMeters = + deltaX * Math.Cos(endPose.YawRadians) + + deltaY * Math.Sin(endPose.YawRadians); + var remainingAlongTravelMeters = + _terminalTravelDirection * + longitudinalErrorMeters; + + // 越过终点后不生成与原轨迹方向相反的修正速度。 + if (remainingAlongTravelMeters <= 0.0) + { + return 0.0; + } + + var speedMagnitudeMetersPerSecond = + Math.Min( + MaximumTerminalApproachSpeedMetersPerSecond, + TerminalApproachGainPerSecond * + remainingAlongTravelMeters); + + return _terminalTravelDirection * + speedMagnitudeMetersPerSecond; + } + + private double ResolveReferenceSpeedForControl( + TrajectoryProjection projection) + { + var currentReferenceSpeed = + ApplyTerminalBrakingPreview( + projection, + projection.ReferencePoint + .ReferenceSpeedMetersPerSecond); + var isInStartupRegion = + projection.ArcLengthMeters <= + StartupRegionMeters && + projection.RemainingDistanceMeters > + FinishDistanceMeters; + + if (!isInStartupRegion) + { + return currentReferenceSpeed; + } + + var previewArcLengthMeters = Math.Min( + _trajectory.TotalLengthMeters, + projection.ArcLengthMeters + + StartupPreviewDistanceMeters); + var previewReferenceSpeed = + _trajectory + .SampleAtArcLength(previewArcLengthMeters) + .ReferenceSpeedMetersPerSecond; + + if (Math.Abs(previewReferenceSpeed) <= + ZeroReferenceSpeedToleranceMetersPerSecond) + { + return currentReferenceSpeed; + } + + var startupReleaseSpeed = + Math.Sign(previewReferenceSpeed) * + Math.Min( + Math.Abs(previewReferenceSpeed), + MaximumStartupSpeedMetersPerSecond); + + if (Math.Sign(currentReferenceSpeed) == + Math.Sign(startupReleaseSpeed) && + Math.Abs(currentReferenceSpeed) >= + Math.Abs(startupReleaseSpeed)) + { + return currentReferenceSpeed; + } + + return startupReleaseSpeed; + } + + private double ApplyTerminalBrakingPreview( + TrajectoryProjection projection, + double currentReferenceSpeed) + { + if (TerminalBrakingPreviewMeters <= 0.0) + { + return currentReferenceSpeed; + } + + var previewArcLengthMeters = Math.Min( + _trajectory.TotalLengthMeters, + projection.ArcLengthMeters + + TerminalBrakingPreviewMeters); + var previewReferenceSpeed = + _trajectory + .SampleAtArcLength(previewArcLengthMeters) + .ReferenceSpeedMetersPerSecond; + var previewIsStop = + Math.Abs(previewReferenceSpeed) <= + ZeroReferenceSpeedToleranceMetersPerSecond; + var hasSameDirection = + Math.Sign(previewReferenceSpeed) == + Math.Sign(currentReferenceSpeed); + var previewIsSlower = + Math.Abs(previewReferenceSpeed) < + Math.Abs(currentReferenceSpeed); + + if (previewIsSlower && + (previewIsStop || hasSameDirection)) + { + return previewReferenceSpeed; + } + + return currentReferenceSpeed; + } + + private static double ResolveTerminalTravelDirection( + Trajectory2D trajectory) + { + for (var index = trajectory.Count - 1; + index >= 0; + index--) + { + var referenceSpeedMetersPerSecond = + trajectory[index] + .ReferenceSpeedMetersPerSecond; + + if (Math.Abs(referenceSpeedMetersPerSecond) > + ZeroReferenceSpeedToleranceMetersPerSecond) + { + return Math.Sign( + referenceSpeedMetersPerSecond); + } + } + + throw new ArgumentException( + "轨迹必须在终点前包含至少一个非零参考速度。", + nameof(trajectory)); + } + + private bool HasReachedEnd( + Pose2D poseInWorld, + Twist2D actualTwistInBody, + bool hasValidVelocityEstimate, + TrajectoryProjection projection) + { + if (!hasValidVelocityEstimate) + { + return false; + } + + return projection.RemainingDistanceMeters <= + FinishDistanceMeters && + CalculateDistanceToEndMeters(poseInWorld) <= + FinishDistanceMeters && + CalculateHeadingErrorToEndRadians(poseInWorld) <= + FinishHeadingToleranceRadians && + CalculateActualLongitudinalSpeedMetersPerSecond( + actualTwistInBody) <= + FinishSpeedMetersPerSecond; + } + + private bool HasStoppedAtUnsatisfiedTerminal( + Pose2D poseInWorld, + Twist2D actualTwistInBody, + bool hasValidVelocityEstimate, + TrajectoryProjection projection, + out string failureReason) + { + failureReason = string.Empty; + + var isTerminalZeroSpeedReference = + projection.RemainingDistanceMeters <= + FinishDistanceMeters && + Math.Abs( + projection.ReferencePoint + .ReferenceSpeedMetersPerSecond) <= + ZeroReferenceSpeedToleranceMetersPerSecond; + + if (!isTerminalZeroSpeedReference || + !hasValidVelocityEstimate || + CalculateActualLongitudinalSpeedMetersPerSecond( + actualTwistInBody) > + FinishSpeedMetersPerSecond) + { + return false; + } + + var positionErrorMeters = + CalculateDistanceToEndMeters( + poseInWorld); + var headingErrorRadians = + CalculateHeadingErrorToEndRadians( + poseInWorld); + + failureReason = + "受控刚体已在终点零速参考处停稳，但终点精度不满足要求：" + + $"位置误差={positionErrorMeters:F3}m，" + + "航向误差=" + + $"{AngleMath.RadiansToDegrees(headingErrorRadians):F2}°。"; + return true; + } + + private double CalculateDistanceToEndMeters( + Pose2D poseInWorld) + { + var endPose = _trajectory.EndPoint.PoseInWorld; + var deltaX = poseInWorld.XMeters - + endPose.XMeters; + var deltaY = poseInWorld.YMeters - + endPose.YMeters; + + return Math.Sqrt( + deltaX * deltaX + + deltaY * deltaY); + } + + private double CalculateHeadingErrorToEndRadians( + Pose2D poseInWorld) + { + return Math.Abs( + AngleMath.ShortestDifferenceRadians( + _trajectory.EndPoint + .PoseInWorld.YawRadians, + poseInWorld.YawRadians)); + } + + private double CalculateActualLongitudinalSpeedMetersPerSecond( + Twist2D actualTwistInBody) + { + return Math.Abs( + Math.Cos(_motionDirectionInBodyRadians) * + actualTwistInBody.VxMetersPerSecond + + Math.Sin(_motionDirectionInBodyRadians) * + actualTwistInBody.VyMetersPerSecond); + } + + private void CompleteTrajectory() + { + ResetFeedbackControllers(); + _trajectory = null; + IsActive = false; + IsCompleted = true; + LastRequestedCommand = new GcpMotionCommand( + 0.0, + 0.0, + 0.0); + LastFailureReason = string.Empty; + LastException = null; + } + + private void ResetFeedbackControllers() + { + _lateralController.Reset(); + _longitudinalController.Reset(); + } + + private void ClearDiagnostics() + { + LastProjection = null; + LastRequestedCommand = null; + LastControlReferenceSpeedMetersPerSecond = null; + LastCurvaturePreviewDistanceMeters = null; + LastFeedforwardCurvaturePerMeter = null; + LastFailureReason = string.Empty; + LastException = null; + } + + private static PathTrackingCycleOutput CreateOutput( + PathTrackingCycleResult result, + GcpMotionCommand? command, + TrajectoryProjection? projection, + double projectionMilliseconds, + long controllerComputeStartTimestamp) + { + var controllerComputeMilliseconds = + controllerComputeStartTimestamp == 0L + ? 0.0 + : GetElapsedMilliseconds( + controllerComputeStartTimestamp); + + return new PathTrackingCycleOutput( + result, + command, + projection, + projectionMilliseconds, + controllerComputeMilliseconds); + } + + private static double GetElapsedMilliseconds( + long startTimestamp) + { + return (Stopwatch.GetTimestamp() - startTimestamp) * + 1000.0 / + Stopwatch.Frequency; + } + } +} diff --git a/MultiWheelC/Experiments/RotationTests.cs b/MultiWheelC/Experiments/RotationTests.cs index 9197b5e..cfda378 100644 --- a/MultiWheelC/Experiments/RotationTests.cs +++ b/MultiWheelC/Experiments/RotationTests.cs @@ -20,6 +20,8 @@ namespace MultiWheelC { public float RelativeAngleDegrees; // 相对当前航向的旋转角度，逆时针为正。 public int TrialNumber = 1; // 重复实验编号。 + public InPlaceRotationFeedbackMode FeedbackMode = + InPlaceRotationFeedbackMode.DetourAbsoluteHeading; private DriveTask _task; private TrackingExperimentRecorder _recorder; @@ -93,6 +95,12 @@ namespace MultiWheelC var targetWorldAngle = (float)AngleMath.NormalizeDegrees( location.th + RelativeAngleDegrees); + var movementAngleTarget = + FeedbackMode == + InPlaceRotationFeedbackMode + .RelativeWheelOdometry + ? RelativeAngleDegrees + : targetWorldAngle; Console.WriteLine( "原地自转实际参数：" + @@ -106,13 +114,19 @@ namespace MultiWheelC $"舵轮到位误差={config.InPlaceRotateWheelAlignDeg:F2}°，" + $"旋转超时={config.InPlaceRotateTimeoutSec:F1}s；" + $"起点航向={location.th:F2}°，" + - $"目标航向={targetWorldAngle:F2}°。"); + $"目标航向={targetWorldAngle:F2}°，" + + $"反馈模式={FeedbackMode}。"); Console.WriteLine( "原地自转CSV保存目录：" + TrackingExperimentRecorder.DefaultOutputDirectory); _recorder = new TrackingExperimentRecorder( - controllerName: "InPlaceRotateFilteredPID", + controllerName: + FeedbackMode == + InPlaceRotationFeedbackMode + .RelativeWheelOdometry + ? "InPlaceRotateWheelOdometry" + : "InPlaceRotateFilteredPID", trajectoryName: _trajectoryName, trialNumber: TrialNumber, referenceStart: rotationCenter, @@ -130,8 +144,8 @@ namespace MultiWheelC _task = new DriveTask( new MultiWheelRotateInPlace { - // MultiWheelRotateInPlace接收世界坐标系绝对航向。 - AngleTarget = targetWorldAngle, + AngleTarget = movementAngleTarget, + FeedbackMode = FeedbackMode, Chassis = chassis, StateProvider = stateProvider, CommandAngularSpeedObserver = @@ -165,6 +179,46 @@ namespace MultiWheelC _recorder?.StopAndSave(); } + // 读取并校验测试使用的有符号相对旋转角度。 + protected static bool TryReadRelativeAngleDegrees( + out float relativeAngleDegrees) + { + var input = UI.GetInput( + "输入相对旋转角度（deg，正数逆时针，负数顺时针，范围-180到180之间）："); + + if ((!float.TryParse( + input, + NumberStyles.Float, + CultureInfo.CurrentCulture, + out relativeAngleDegrees) && + !float.TryParse( + input, + NumberStyles.Float, + CultureInfo.InvariantCulture, + out relativeAngleDegrees)) || + float.IsNaN(relativeAngleDegrees) || + float.IsInfinity(relativeAngleDegrees)) + { + Console.WriteLine("旋转角度输入无效，测试已经取消。"); + return false; + } + + if (Math.Abs(relativeAngleDegrees) < 1e-3f) + { + Console.WriteLine("旋转角度不能为0，测试已经取消。"); + return false; + } + + if (Math.Abs(relativeAngleDegrees) >= 180f) + { + Console.WriteLine( + "输入角度必须满足-180° < angle < 180°。"); + return false; + } + + return true; + } + } [MovementTest(name = "SendXYThSpeed：输入角度原地自转")] @@ -181,58 +235,37 @@ namespace MultiWheelC /// 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)) + if (!TryReadRelativeAngleDegrees( + out var relativeAngleDegrees)) { - Console.WriteLine("旋转角度输入无效，测试已经取消。"); return; } - var chassis = - PilotDefinition.Chassis as MultiWheelChassis; - if (chassis == null) - { - Console.WriteLine( - "当前底盘不是MultiWheelChassis，无法执行原地旋转测试。"); - return; - } + RelativeAngleDegrees = relativeAngleDegrees; + base.Test(); + } + } - var stateProvider = - ParkingVehicleStateProviderFactory.Create( - chassis); - if (!stateProvider.TryGetState(out _)) - { - Console.WriteLine( - "无法读取原地旋转起点状态：" + - stateProvider.LastFailureReason); - return; - } + [MovementTest(name = "轮组里程计：输入角度原地相对自转")] + public sealed class TestWheelOdometryRotateAngle : + InPlaceRotateTestBase + { + public TestWheelOdometryRotateAngle() + : base(0f, "RotateWheelOdometryCustomAngle") + { + FeedbackMode = + InPlaceRotationFeedbackMode + .RelativeWheelOdometry; + } - if (Math.Abs(relativeAngleDegrees) < 1e-3f) + /// + /// 读取相对角度并仅用滤波后的轮组角速度积分完成自转。 + /// + public override void Test() + { + if (!TryReadRelativeAngleDegrees( + out var relativeAngleDegrees)) { - Console.WriteLine("旋转角度不能为0，测试已经取消。"); - return; - } - - // 当前控制器按照圆周最短角旋转；精确±180°的方向存在二义性。 - if (Math.Abs(relativeAngleDegrees) >= 180f) - { - Console.WriteLine( - "输入角度必须满足-180° < angle < 180°；" + - "当前最短角控制不支持指定精确±180°的旋转方向。"); return; } diff --git a/MultiWheelC/Fleet/FleetController.cs b/MultiWheelC/Fleet/FleetController.cs index 2fa2ac9..e0ca3e7 100644 --- a/MultiWheelC/Fleet/FleetController.cs +++ b/MultiWheelC/Fleet/FleetController.cs @@ -1 +1,201 @@ -// 输出FleetMotionCommand \ No newline at end of file +using System; +using MultiWheelC.Control.Abstractions; +using MultiWheelC.Control.Allocation; +using MultiWheelC.Control.Execution; +using MultiWheelC.Trajectory; +using MyParking.Shared; + +namespace MultiWheelC.Fleet +{ + // 表示车队中心单周期轨迹控制的计算结果，不包含通信发送结果。 + public enum FleetControlCycleResult + { + Inactive = 0, + CommandGenerated = 1, + Completed = 2, + Faulted = 3 + } + + // 将公共轨迹核心生成的GCP命令转换为车队坐标系下的刚体速度命令。 + public sealed class FleetController + { + private readonly PathTrackingCore _trackingCore; + + // virtualControlPointRadiusMeters必须与横向控制器采用的虚拟车队GCP半径一致。 + public FleetController( + ILateralController lateralController, + ILongitudinalController longitudinalController, + GcpCommandAllocator gcpAllocator, + double virtualControlPointRadiusMeters, + double finishDistanceMeters = 0.04, + double finishSpeedMetersPerSecond = 0.02, + double finishHeadingToleranceRadians = + 3.0 * Math.PI / 180.0, + double maximumDistanceToTrajectoryMeters = 0.30, + double terminalBrakingPreviewMeters = 0.02, + double terminalApproachDistanceMeters = 0.10, + double terminalApproachGainPerSecond = 0.8, + double maximumTerminalApproachSpeedMetersPerSecond = 0.05, + double curvaturePreviewSeconds = 0.20, + double maximumCurvaturePreviewMeters = 0.12) + { + NumericGuard.EnsureFinitePositive( + virtualControlPointRadiusMeters, + nameof(virtualControlPointRadiusMeters)); + + VirtualControlPointRadiusMeters = + virtualControlPointRadiusMeters; + _trackingCore = new PathTrackingCore( + lateralController, + longitudinalController, + gcpAllocator, + finishDistanceMeters, + finishSpeedMetersPerSecond, + finishHeadingToleranceRadians, + maximumDistanceToTrajectoryMeters, + terminalBrakingPreviewMeters, + terminalApproachDistanceMeters, + terminalApproachGainPerSecond, + maximumTerminalApproachSpeedMetersPerSecond, + curvaturePreviewSeconds, + maximumCurvaturePreviewMeters); + } + + // 虚拟车队中心到前、后GCP的距离，单位为m。 + public double VirtualControlPointRadiusMeters { get; } + + public double FinishDistanceMeters => + _trackingCore.FinishDistanceMeters; + + public double FinishSpeedMetersPerSecond => + _trackingCore.FinishSpeedMetersPerSecond; + + public double FinishHeadingToleranceRadians => + _trackingCore.FinishHeadingToleranceRadians; + + public double MaximumDistanceToTrajectoryMeters => + _trackingCore.MaximumDistanceToTrajectoryMeters; + + public double TerminalBrakingPreviewMeters => + _trackingCore.TerminalBrakingPreviewMeters; + + public double TerminalApproachDistanceMeters => + _trackingCore.TerminalApproachDistanceMeters; + + public double TerminalApproachGainPerSecond => + _trackingCore.TerminalApproachGainPerSecond; + + public double MaximumTerminalApproachSpeedMetersPerSecond => + _trackingCore.MaximumTerminalApproachSpeedMetersPerSecond; + + public double CurvaturePreviewSeconds => + _trackingCore.CurvaturePreviewSeconds; + + public double MaximumCurvaturePreviewMeters => + _trackingCore.MaximumCurvaturePreviewMeters; + + public bool IsActive => _trackingCore.IsActive; + + public bool IsCompleted => _trackingCore.IsCompleted; + + public string LastFailureReason => + _trackingCore.LastFailureReason; + + public Exception LastException => + _trackingCore.LastException; + + public FleetState? LastFleetState { get; private set; } + + public TrajectoryProjection? LastProjection => + _trackingCore.LastProjection; + + public GcpMotionCommand? LastGcpCommand => + _trackingCore.LastRequestedCommand; + + public FleetMotionCommand? LastCommand { get; private set; } + + // 重置公共核心，并从轨迹起点开始跟踪虚拟车队中心。 + public void Start(Trajectory2D trajectory) + { + _trackingCore.Start(trajectory); + LastFleetState = null; + LastCommand = null; + } + + // 计算一周期车队中心命令；非运行状态、完成或故障时返回停车命令。 + public FleetControlCycleResult ComputeCommand( + FleetState fleetState, + double deltaTimeSeconds, + out FleetMotionCommand command) + { + NumericGuard.EnsureFinitePositive( + deltaTimeSeconds, + nameof(deltaTimeSeconds)); + + command = FleetMotionCommand.Stop(); + + if (!_trackingCore.IsActive) + { + return FleetControlCycleResult.Inactive; + } + + LastFleetState = fleetState; + var output = _trackingCore.Compute( + fleetState.FleetPoseInWorld, + fleetState.TwistAtFleetOriginInFleet, + fleetState.HasValidVelocityEstimate, + deltaTimeSeconds); + + if (output.Result == + PathTrackingCycleResult.Completed) + { + LastCommand = command; + return FleetControlCycleResult.Completed; + } + + if (output.Result == + PathTrackingCycleResult.Faulted) + { + LastCommand = command; + return FleetControlCycleResult.Faulted; + } + + if (output.Result != + PathTrackingCycleResult.CommandGenerated || + !output.Command.HasValue) + { + return FleetControlCycleResult.Inactive; + } + + try + { + var twistAtFleetOrigin = + GcpKinematics.ToBodyTwist( + output.Command.Value, + VirtualControlPointRadiusMeters); + command = new FleetMotionCommand( + Point2D.Zero, + twistAtFleetOrigin); + LastCommand = command; + return FleetControlCycleResult.CommandGenerated; + } + catch (Exception exception) + { + _trackingCore.Fail( + "车队中心GCP命令转换异常：" + + exception.Message, + exception); + LastCommand = command; + return FleetControlCycleResult.Faulted; + } + } + + // 取消当前轨迹并使后续周期只生成停车命令。 + public void Cancel() + { + _trackingCore.Cancel(); + LastFleetState = null; + LastCommand = null; + } + } +} diff --git a/MultiWheelC/Fleet/FleetState.cs b/MultiWheelC/Fleet/FleetState.cs new file mode 100644 index 0000000..269051c --- /dev/null +++ b/MultiWheelC/Fleet/FleetState.cs @@ -0,0 +1,64 @@ +using MyParking.Shared; + +namespace MultiWheelC.Fleet +{ + // 一次经过校验的虚拟车队原点状态快照。 + public readonly struct FleetState + { + public FleetState( + double sampleTimestampSeconds, + Pose2D fleetPoseInWorld, + Twist2D twistAtFleetOriginInWorld, + bool hasValidVelocityEstimate) + { + NumericGuard.EnsureFiniteNonNegative( + sampleTimestampSeconds, + nameof(sampleTimestampSeconds)); + NumericGuard.EnsureFinite( + fleetPoseInWorld, + nameof(fleetPoseInWorld)); + NumericGuard.EnsureFinite( + twistAtFleetOriginInWorld, + nameof(twistAtFleetOriginInWorld)); + + SampleTimestampSeconds = + sampleTimestampSeconds; + FleetPoseInWorld = new Pose2D( + fleetPoseInWorld.XMeters, + fleetPoseInWorld.YMeters, + AngleMath.NormalizeRadians( + fleetPoseInWorld.YawRadians)); + HasValidVelocityEstimate = + hasValidVelocityEstimate; + + // 位姿有效但速度尚未初始化时显式置零，避免控制器误用输入值。 + TwistAtFleetOriginInWorld = + hasValidVelocityEstimate + ? twistAtFleetOriginInWorld + : Twist2D.Zero; + + var worldPoseInFleet = + FrameTransform2D.Inverse( + FleetPoseInWorld); + TwistAtFleetOriginInFleet = + FrameTransform2D.TransformTwistAtSamePoint( + worldPoseInFleet, + TwistAtFleetOriginInWorld); + } + + // 状态源单调时钟中的采样时刻，单位为s。 + public double SampleTimestampSeconds { get; } + + // 车队坐标系原点在世界坐标系中的实际位姿。 + public Pose2D FleetPoseInWorld { get; } + + // 车队原点处的实际刚体速度，在世界坐标系中表达。 + public Twist2D TwistAtFleetOriginInWorld { get; } + + // 同一刚体速度在车队坐标系中表达，供车队控制器使用。 + public Twist2D TwistAtFleetOriginInFleet { get; } + + // 速度是否已经初始化并可用于闭环控制。 + public bool HasValidVelocityEstimate { get; } + } +} diff --git a/MultiWheelC/Movements/RotateInPlace.cs b/MultiWheelC/Movements/RotateInPlace.cs index 4386b77..e9ae1d6 100644 --- a/MultiWheelC/Movements/RotateInPlace.cs +++ b/MultiWheelC/Movements/RotateInPlace.cs @@ -12,15 +12,30 @@ using MultiWheelC.StateEstimation; namespace MultiWheelC { /// - /// 将四个舵轮准备到自转姿态并按世界航向闭环旋转，正常完成后等待舵轮回正。 + /// 指定原地自转使用Detour绝对航向或轮组相对角度反馈。 + /// + public enum InPlaceRotationFeedbackMode + { + DetourAbsoluteHeading, + RelativeWheelOdometry + } + + /// + /// 将四个舵轮准备到自转姿态，按所选反馈旋转并在完成后等待舵轮回正。 /// public class MultiWheelRotateInPlace : MovementDefinition { /// - /// 旋转目标角度 + /// Detour模式表示世界目标航向，轮组模式表示有符号相对旋转角度，单位deg。 /// public float AngleTarget; + /// + /// 获取或设置原地自转反馈模式；默认保持现有Detour绝对航向闭环。 + /// + public InPlaceRotationFeedbackMode FeedbackMode = + InPlaceRotationFeedbackMode.DetourAbsoluteHeading; + // 留作标定或单元测试时显式替换；为空时使用配置化Detour与电机反馈组合状态源。 public Func ThetaReader; @@ -152,10 +167,32 @@ namespace MultiWheelC adapter.LastFailureReason); } + var useRelativeWheelOdometry = + FeedbackMode == + InPlaceRotationFeedbackMode + .RelativeWheelOdometry; + var wheelStateProvider = + useRelativeWheelOdometry + ? stateProvider as + WheelFeedbackVehicleStateProvider + : null; + if (useRelativeWheelOdometry && + wheelStateProvider == null) + { + throw new InvalidOperationException( + "轮组相对角度自转需要" + + "WheelFeedbackVehicleStateProvider。"); + } + var targetAngle = - (float)AngleMath.NormalizeDegrees(AngleTarget); + useRelativeWheelOdometry + ? AngleTarget + : (float)AngleMath.NormalizeDegrees( + AngleTarget); var currentAngle = - ReadCurrentAngleDegrees(stateProvider); + useRelativeWheelOdometry + ? 0f + : ReadCurrentAngleDegrees(stateProvider); var cachedCurrentAngle = currentAngle; thPid = new PIDController( () => cachedCurrentAngle, @@ -170,6 +207,10 @@ namespace MultiWheelC pidParameters.SpeedAccPerSec); var lastCommandTime = DateTime.Now; var rotationStarted = DateTime.Now; + var accumulatedWheelAngleRadians = 0.0; + var previousWheelOmegaRadiansPerSecond = 0.0; + var previousWheelTimestampSeconds = 0.0; + var hasPreviousWheelSample = false; while (true) { @@ -181,15 +222,74 @@ namespace MultiWheelC $"原地自转超过{rotationTimeoutSeconds:F1}s仍未到位。"); } - currentAngle = - ReadCurrentAngleDegrees(stateProvider); + if (useRelativeWheelOdometry) + { + if (!wheelStateProvider.TryGetWheelTwist( + out var wheelTwist, + out var wheelTimestampSeconds)) + { + CommandAngularSpeedObserver?.Invoke(0f); + adapter + .StopXYThDrivePreserveSteeringState(); + + if (hasPreviousWheelSample) + { + throw new InvalidOperationException( + "原地自转期间轮组角速度不可用：" + + wheelStateProvider.LastFailureReason); + } + + yield return true; + continue; + } + + if (hasPreviousWheelSample) + { + var wheelDeltaTimeSeconds = + wheelTimestampSeconds - + previousWheelTimestampSeconds; + if (!NumericGuard.IsFinite( + wheelDeltaTimeSeconds) || + wheelDeltaTimeSeconds <= 0.0) + { + throw new InvalidOperationException( + "轮组角速度采样时间没有单调递增。"); + } + + accumulatedWheelAngleRadians += + 0.5 * + (previousWheelOmegaRadiansPerSecond + + wheelTwist + .OmegaRadiansPerSecond) * + wheelDeltaTimeSeconds; + } + + previousWheelOmegaRadiansPerSecond = + wheelTwist.OmegaRadiansPerSecond; + previousWheelTimestampSeconds = + wheelTimestampSeconds; + hasPreviousWheelSample = true; + currentAngle = + (float)AngleMath.RadiansToDegrees( + accumulatedWheelAngleRadians); + } + else + { + currentAngle = + ReadCurrentAngleDegrees(stateProvider); + } + cachedCurrentAngle = currentAngle; - var s = thPid.GetResponse(targetAngle, true); + var s = thPid.GetResponse( + targetAngle, + !useRelativeWheelOdometry); var angleErrorDegrees = - (float)AngleMath - .ShortestDifferenceDegrees( - targetAngle, - currentAngle); + useRelativeWheelOdometry + ? targetAngle - currentAngle + : (float)AngleMath + .ShortestDifferenceDegrees( + targetAngle, + currentAngle); // PID进入到位死区后等待其0.3s稳定确认；等待期间 // 只清零驱动速度，不清除已经准备好的自转舵角状态。 @@ -255,7 +355,8 @@ namespace MultiWheelC CommandAngularSpeedObserver?.Invoke(0f); adapter.StopXYThDrivePreserveSteeringState(); - if (IsLocalizationRecoveryPending( + if (!useRelativeWheelOdometry && + IsLocalizationRecoveryPending( stateProvider)) { BeginPostRotationPositionRecovery( @@ -309,7 +410,10 @@ namespace MultiWheelC } Console.WriteLine( - $"final rotate to {targetAngle}, wheels forward"); + useRelativeWheelOdometry + ? "final relative wheel rotate to " + + $"{currentAngle:F2}deg, wheels forward" + : $"final rotate to {targetAngle}, wheels forward"); } finally { @@ -345,6 +449,33 @@ namespace MultiWheelC rotationTimeoutSeconds, nameof(RotationTimeoutSeconds)); + if (float.IsNaN(AngleTarget) || + float.IsInfinity(AngleTarget)) + { + throw new ArgumentOutOfRangeException( + nameof(AngleTarget), + "原地自转目标角度必须是有限值。"); + } + + if (!Enum.IsDefined( + typeof(InPlaceRotationFeedbackMode), + FeedbackMode)) + { + throw new ArgumentOutOfRangeException( + nameof(FeedbackMode), + "原地自转反馈模式无效。"); + } + + if (FeedbackMode == + InPlaceRotationFeedbackMode + .RelativeWheelOdometry && + Math.Abs(AngleTarget) >= 180f) + { + throw new ArgumentOutOfRangeException( + nameof(AngleTarget), + "轮组相对自转角度必须满足-180° < angle < 180°。"); + } + if (pidParameters == null) { throw new InvalidOperationException( diff --git a/MultiWheelC/StateEstimation/WheelFeedbackVehicleStateProvider.cs b/MultiWheelC/StateEstimation/WheelFeedbackVehicleStateProvider.cs index 6a74ea2..36adc74 100644 --- a/MultiWheelC/StateEstimation/WheelFeedbackVehicleStateProvider.cs +++ b/MultiWheelC/StateEstimation/WheelFeedbackVehicleStateProvider.cs @@ -96,67 +96,19 @@ namespace MultiWheelC.StateEstimation { try { - var actualCarSpeed = - _chassis.GetCarSpeed(true); - var rawBodyVxMetersPerSecond = - (double)actualCarSpeed.Vx; - var rawBodyVyMetersPerSecond = - (double)actualCarSpeed.Vy; - // CommonUsage.CarSpeed.Vw在旧底盘边界使用deg/s； - // 状态估计内部统一转换为rad/s。 - var rawBodyOmegaRadiansPerSecond = - AngleMath.DegreesToRadians( - actualCarSpeed.Vw); - - NumericGuard.EnsureFinite( - rawBodyVxMetersPerSecond, - "电机反馈车体纵向速度"); - NumericGuard.EnsureFinite( - rawBodyVyMetersPerSecond, - "电机反馈车体横向速度"); - NumericGuard.EnsureFinite( - rawBodyOmegaRadiansPerSecond, - "电机反馈车体角速度"); - - var wheelSpeedTimestampSeconds = - _wheelSpeedClock.Elapsed.TotalSeconds; - - UpdateBodyVelocityFilters( - rawBodyVxMetersPerSecond, - rawBodyVyMetersPerSecond, - rawBodyOmegaRadiansPerSecond, - wheelSpeedTimestampSeconds, - out var filteredBodyVxMetersPerSecond, - out var filteredBodyVyMetersPerSecond, - out var filteredBodyOmegaRadiansPerSecond, + ReadFilteredWheelTwist( + out var filteredWheelTwist, + out _, out var hasValidWheelSpeedEstimate); - _latestRawWheelBodyVxMetersPerSecond = - rawBodyVxMetersPerSecond; - _latestFilteredWheelBodyVxMetersPerSecond = - filteredBodyVxMetersPerSecond; - _latestRawWheelBodyVyMetersPerSecond = - rawBodyVyMetersPerSecond; - _latestFilteredWheelBodyVyMetersPerSecond = - filteredBodyVyMetersPerSecond; - _latestRawWheelBodyOmegaRadiansPerSecond = - rawBodyOmegaRadiansPerSecond; - _latestFilteredWheelBodyOmegaRadiansPerSecond = - filteredBodyOmegaRadiansPerSecond; - _latestWheelSampleTimestampSeconds = - wheelSpeedTimestampSeconds; - _latestWheelVelocityValid = - hasValidWheelSpeedEstimate; - _latestWheelFeedbackReadSucceeded = true; - // Detour位姿跳变确认期间需要用轮速维持短时运动预测。 if (_poseProvider is DetourVehicleStateProvider detourStateProvider) { detourStateProvider.UpdateWheelVelocityEstimate( - filteredBodyVxMetersPerSecond, - filteredBodyVyMetersPerSecond, - filteredBodyOmegaRadiansPerSecond, + filteredWheelTwist.VxMetersPerSecond, + filteredWheelTwist.VyMetersPerSecond, + filteredWheelTwist.OmegaRadiansPerSecond, hasValidWheelSpeedEstimate); } @@ -175,13 +127,18 @@ namespace MultiWheelC.StateEstimation poseState.HasValidVelocityEstimate; _hasVelocityDiagnostics = true; - // 车体平面线速度来自四轮电机和舵角反馈；角速度继续使用Detour， - // 避免轮速差和舵角误差放大Omega噪声。 + // 轮组反馈有效后统一使用滤波后的平面速度；初始化期间 + // 暂时保留Detour角速度作为回退值。 + var omegaRadiansPerSecond = + hasValidWheelSpeedEstimate + ? filteredWheelTwist + .OmegaRadiansPerSecond + : poseState.TwistInBody + .OmegaRadiansPerSecond; var twistInBody = new Twist2D( - filteredBodyVxMetersPerSecond, - filteredBodyVyMetersPerSecond, - poseState.TwistInBody - .OmegaRadiansPerSecond); + filteredWheelTwist.VxMetersPerSecond, + filteredWheelTwist.VyMetersPerSecond, + omegaRadiansPerSecond); var twistInWorld = FrameTransform2D @@ -210,6 +167,47 @@ namespace MultiWheelC.StateEstimation } } + /// + /// 读取并滤波轮组反馈速度，不访问Detour；首帧仅建立滤波时间基准并返回false。 + /// + public bool TryGetWheelTwist( + out Twist2D twistInBody, + out double sampleTimestampSeconds) + { + lock (_syncRoot) + { + try + { + ReadFilteredWheelTwist( + out var filteredWheelTwist, + out sampleTimestampSeconds, + out var hasValidWheelSpeedEstimate); + + if (!hasValidWheelSpeedEstimate) + { + twistInBody = Twist2D.Zero; + LastFailureReason = + "轮组速度估计正在建立采样时间基准。"; + return false; + } + + twistInBody = filteredWheelTwist; + LastFailureReason = string.Empty; + return true; + } + catch (Exception exception) + { + _latestWheelFeedbackReadSucceeded = false; + twistInBody = Twist2D.Zero; + sampleTimestampSeconds = 0.0; + LastFailureReason = + "舵轮电机反馈车体速度解算失败：" + + exception.Message; + return false; + } + } + } + /// /// 读取Detour独立校验后的航向，同时保持轮组速度预测输入更新。 /// @@ -524,6 +522,73 @@ namespace MultiWheelC.StateEstimation } } + /// + /// 从底盘读取一次轮组速度，统一转换为SI单位并更新共用低通滤波状态。 + /// + private void ReadFilteredWheelTwist( + out Twist2D filteredTwistInBody, + out double sampleTimestampSeconds, + out bool hasValidWheelSpeedEstimate) + { + var actualCarSpeed = + _chassis.GetCarSpeed(true); + var rawBodyVxMetersPerSecond = + (double)actualCarSpeed.Vx; + var rawBodyVyMetersPerSecond = + (double)actualCarSpeed.Vy; + // CommonUsage.CarSpeed.Vw在旧底盘边界使用deg/s； + // 状态估计内部统一转换为rad/s。 + var rawBodyOmegaRadiansPerSecond = + AngleMath.DegreesToRadians( + actualCarSpeed.Vw); + + NumericGuard.EnsureFinite( + rawBodyVxMetersPerSecond, + "电机反馈车体纵向速度"); + NumericGuard.EnsureFinite( + rawBodyVyMetersPerSecond, + "电机反馈车体横向速度"); + NumericGuard.EnsureFinite( + rawBodyOmegaRadiansPerSecond, + "电机反馈车体角速度"); + + sampleTimestampSeconds = + _wheelSpeedClock.Elapsed.TotalSeconds; + + UpdateBodyVelocityFilters( + rawBodyVxMetersPerSecond, + rawBodyVyMetersPerSecond, + rawBodyOmegaRadiansPerSecond, + sampleTimestampSeconds, + out var filteredBodyVxMetersPerSecond, + out var filteredBodyVyMetersPerSecond, + out var filteredBodyOmegaRadiansPerSecond, + out hasValidWheelSpeedEstimate); + + filteredTwistInBody = new Twist2D( + filteredBodyVxMetersPerSecond, + filteredBodyVyMetersPerSecond, + filteredBodyOmegaRadiansPerSecond); + + _latestRawWheelBodyVxMetersPerSecond = + rawBodyVxMetersPerSecond; + _latestFilteredWheelBodyVxMetersPerSecond = + filteredBodyVxMetersPerSecond; + _latestRawWheelBodyVyMetersPerSecond = + rawBodyVyMetersPerSecond; + _latestFilteredWheelBodyVyMetersPerSecond = + filteredBodyVyMetersPerSecond; + _latestRawWheelBodyOmegaRadiansPerSecond = + rawBodyOmegaRadiansPerSecond; + _latestFilteredWheelBodyOmegaRadiansPerSecond = + filteredBodyOmegaRadiansPerSecond; + _latestWheelSampleTimestampSeconds = + sampleTimestampSeconds; + _latestWheelVelocityValid = + hasValidWheelSpeedEstimate; + _latestWheelFeedbackReadSucceeded = true; + } + /// /// 使用同一个真实采样间隔更新车体Vx、Vy和Omega低通滤波，并在首帧建立共同时间基准。 /// diff --git a/MultiWheelC/build/Clumsy/CommonUsage.dll b/MultiWheelC/build/Clumsy/CommonUsage.dll index 9fcee49..403ea8b 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 46fcd3e..864ccd2 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 0199b2b..b41c738 100644 Binary files a/MultiWheelC/build/Clumsy/MultiWheelC.pdb and b/MultiWheelC/build/Clumsy/MultiWheelC.pdb differ diff --git a/Shared/Fleet/FleetKinematics.cs b/Shared/Fleet/FleetKinematics.cs index 55d2d8a..60a55c8 100644 --- a/Shared/Fleet/FleetKinematics.cs +++ b/Shared/Fleet/FleetKinematics.cs @@ -22,9 +22,7 @@ namespace MyParking.Shared var referencePoint = command.ReferencePointInFleet; var referenceTwist = command.TwistAtReferencePoint; - for (var index = 0; - index < layout.VehicleCount; - index++) + for (var index = 0; index < layout.VehicleCount; index++) { var vehicle = layout.Vehicles[index]; var offsetXMeters = diff --git a/docs/architecture.md b/docs/architecture.md index f227fe2..3e20c9e 100644 --- a/docs/architecture.md +++ b/docs/architecture.md @@ -37,7 +37,7 @@ Medulla宿主 | `MultiWheelC/Control/Lateral/` | 当前默认 `StanleyLateralController` | | `MultiWheelC/Control/Longitudinal/` | 当前默认 `PidLongitudinalController` | | `MultiWheelC/Control/Allocation/` | 横纵结果组合、GCP限幅及GCP与刚体速度的转换 | -| `MultiWheelC/Control/Execution/` | 单周期编排、终点策略、命令执行和耗时诊断 | +| `MultiWheelC/Control/Execution/` | `PathTrackingCore` 共享纯控制周期、单车命令执行、终点策略和耗时诊断 | | `MultiWheelC/Movements/` | 舵轮准备、轨迹跟踪、原地自转和组合动作计划 | | `MultiWheelC/Experiments/` | Clumsy宿主人工测试、测试轨迹工厂和CSV记录 | | `MultiWheelC/Old/` | 保留的旧实现；不能仅因仍参与编译就视为新版流程依赖 | @@ -73,10 +73,11 @@ MovementTest / MotionPlanExecutor → ParkingVehicleStateProviderFactory.Create() → ParkingGeometricController.Start()/ExecuteCycle() → IVehicleStateProvider.TryGetState() - → TrajectoryProjector.Project() - → ILateralController.Compute() - → ILongitudinalController.ComputeSpeedMetersPerSecond() - → GcpCommandAllocator.Allocate() + → PathTrackingCore.Compute() + → TrajectoryProjector.Project() + → ILateralController.Compute() + → ILongitudinalController.ComputeSpeedMetersPerSecond() + → GcpCommandAllocator.Allocate() → GcpCommandExecutor.Execute() → GcpKinematics.ToBodyTwist() → MultiWheelChassisAdapter.SendBodyTwist() @@ -84,8 +85,24 @@ MovementTest / MotionPlanExecutor → 四个真实舵轮角度和速度 ``` +`PathTrackingCore` 只依赖受控刚体的 `Pose2D`、车体系 `Twist2D`、速度有效标志和控制周期，不依赖 `VehicleState`、`FleetState`、底盘或通信。`ParkingGeometricController` 负责把单车状态源和实体底盘接到该核心；横向、纵向控制算法仍通过接口组合注入，没有采用控制器继承层次。 + `TrajectoryTrackingMovement` 默认从 `PilotDefinition.Conf` 读取车辆级参数，同时保留少量动作级覆盖字段；横向控制器可通过 `LateralControllerFactory` 替换，纵向控制器当前固定创建为 `PidLongitudinalController`。 +## 车队纯计算调用链 + +```text +FleetState + → FleetController + → PathTrackingCore.Compute() + → GcpKinematics.ToBodyTwist() + → FleetMotionCommand（参考点为车队原点） + → FleetKinematics.Decompose(FleetLayout, command) + → FleetMemberCommand[]（各成员真实车体系Twist） +``` + +这条链已经覆盖虚拟中心轨迹闭环和确定性刚体速度分解，但尚未接入车队状态估计、通信/时间对齐、相对布局纠偏、共同能力限幅、安全降级和成员底盘发送。 + ## 状态数据流 ```text @@ -97,8 +114,8 @@ DetourInterface.getCartLocation() MultiWheelChassis.GetCarSpeed(true) → WheelFeedbackVehicleStateProvider ├─ Vx、Vy分别使用同一时间常数低通滤波 - ├─ 覆盖Detour线速度 - └─ 保留Detour位姿和Omega + ├─ 轮组估计有效后覆盖Detour的Vx、Vy和Omega + └─ 保留Detour位姿 → VehicleState（世界位姿、世界Twist、车体Twist） → ParkingGeometricController diff --git a/docs/decisions.md b/docs/decisions.md index 7c937a6..01b3d00 100644 --- a/docs/decisions.md +++ b/docs/decisions.md @@ -50,10 +50,10 @@ ### 7. 默认状态采用Detour位姿与轮组平面速度组合 -- 位姿和Omega来自经过校验/滤波的Detour。 -- 车体系Vx、Vy来自 `GetCarSpeed(true)`，分别使用同一时间常数低通滤波。 -- 轮组Vw同样滤波，但仅用于Detour跳变期间的短时位姿预测、运动合理性和动态航向创新阈值，不替换控制输出中的Detour Omega。 -- 原因：位置依赖SLAM，控制平面速度优先使用响应更直接的电机/舵角反馈；轮组Vw对短时运动趋势有用，但动态精度尚不足以直接作为闭环角速度。 +- 位姿来自经过校验和任务坐标连续化处理的Detour。 +- 车体系 `Vx/Vy/Vw` 来自 `GetCarSpeed(true)`，使用同一时间常数低通滤波；轮组估计有效后，控制状态的 `Omega` 也使用轮组 `Vw`。首帧尚未建立采样时间基准时速度状态无效并对外置零。 +- 轮组 `Vw` 同时用于Detour跳变期间的短时位姿预测、运动合理性和动态航向创新阈值。 +- 原因：位置仍依赖SLAM，控制速度优先使用响应更直接且不受Detour自转退化影响的电机/舵角反馈。 - 依据：`ParkingVehicleStateProviderFactory`、`WheelFeedbackVehicleStateProvider`。 ### 8. 横向控制可替换，纵向控制暂保持PID @@ -90,22 +90,39 @@ - `l_step` 当前只作为诊断和后续健康分级依据，不单独决定状态有效性。 - 依据：`DetourVehicleStateProvider`、`WheelFeedbackVehicleStateProvider`、`MultiWheelRotateInPlace`。 -### 13. 车队布局采用不可变快照，刚体速度采用确定性分解 +### 13. 车队布局与状态采用不可变快照，刚体速度采用确定性分解 - `Shared/Fleet/FleetLayout.cs` 保存经车号唯一性和有限值校验的 `VehicleLayout` 快照，不提供搬运过程中的逐车修改入口。 - `FleetKinematics.Decompose()` 已按平面刚体关系把 `FleetMotionCommand` 分解为各成员中心速度，并转换到各车真实车体系；不依赖通信、Detour、β、底盘限幅或QP。 - `MultiWheelC.Tests/FleetKinematicsTests.cs` 已覆盖整体平移、绕车队中心旋转、绕成员车旋转和停止四个数学场景。 +- `MultiWheelC/Fleet/FleetState.cs` 保存车队虚拟中心的世界位姿、同一点的世界系/车队系速度、状态采样时间和速度有效性；`FleetPoseInWorld.Yaw` 定义车队 `+X` 方向，两份速度只是同一物理速度的不同坐标表达。 +- `FleetState.SampleTimestampSeconds` 采用主车/协调器生成聚合快照时的本机单调时间。成员本机时钟和Detour `tick` 的同步属于未来接收/状态估计层职责，不阻塞使用人工构造 `FleetState` 开发纯车队控制器。 + +### 14. 原地自转保留绝对航向与轮组相对角两种反馈模式 + +- `DetourAbsoluteHeading` 保留世界航向闭环和停车后的Detour位姿恢复，适用于必须对准绝对方向的任务。 +- `RelativeWheelOdometry` 通过 `TryGetWheelTwist()` 获取与Detour无关的滤波轮组 `Vw`，按轮组采样时间积分相对角度；活动自转不因Detour位置或航向退化中断。 +- 轮组相对模式是工程降级/测试模式，不等同于绝对定位：允许约±5°误差，不能消除轮胎打滑、轮径误差和积分漂移，也不能用于长期世界航向基准。 +- 依据：`WheelFeedbackVehicleStateProvider`、`MultiWheelRotateInPlace`、`RotationTests`。 + +### 15. 单车与车队轨迹跟踪通过组合共享纯控制核心 + +- `PathTrackingContext` 只携带受控刚体的车体系速度、速度有效性、轨迹投影和周期控制量，不依赖 `VehicleState` 或 `FleetState`。 +- `PathTrackingCore` 集中实现投影连续性、保护/终点策略、曲率预瞄、横纵向控制和GCP分配；`ParkingGeometricController` 与 `FleetController` 分别组合该核心，负责各自的状态适配和输出边界，不通过继承复制控制流程。 +- `FleetController` 第一版只闭环车队虚拟中心，并将GCP结果转换成车队原点处的 `FleetMotionCommand`；成员速度继续由 `FleetKinematics.Decompose()` 确定性分解。 +- 控制算法的可替换性继续由 `ILateralController`、`ILongitudinalController` 组合注入；状态源、通信、底盘发送和成员协调不进入纯核心。 +- `MultiWheelC.Tests` 已覆盖8个Stanley前进/倒车符号场景、6个车队控制周期场景和4个刚体分解场景；统一构建与打包通过。 ## 已经确认但尚未实施 - 路线顺序：先完成单车闭环和停车功能验证，再正式实施多车通信、编队和协同控制。来源：`README.md`。 -- 当前只完成车队布局模型和纯运动学分解，尚未形成可运行的多车链路：布局采集/激活、车队状态估计、轨迹控制、通信和安全协调仍未接入业务流程。 +- 当前已完成车队布局模型、虚拟中心状态模型、虚拟中心轨迹控制和纯运动学分解，尚未形成可运行的多车链路：布局采集/激活、车队状态估计、通信、成员纠偏和安全协调仍未接入业务流程。 - 布局生命周期区分夹紧前后的语义：夹紧前的预设布局只用于引导车辆就位；车辆夹紧且静止后，应同步读取成员位姿，选择车队参考系并创建新的不可变 `FleetLayout`，再由上层协调器原子激活。共同搬运期间的相对位姿变化属于状态误差，不能通过修改 `FleetLayout` 吸收；松开车辆后清除激活布局。具体采集和激活接口尚未实施。 - 多车共同搬运不能只闭环车队中心：整体位姿误差与成员相对布局误差必须分开估计和约束，否则成员误差可能相互抵消而使平均中心看似正确。 - 计划采用分层职责：车队控制器产生参考点 `FleetTwist`，分配层依据成员 `VehicleLayout` 计算每车真实车体系 `BodyTwist`，单车层继续负责β变换、GCP和本车四轮解算。 - 第一版采用确定性的虚拟刚体速度分配，不先引入QP/HQP：若成员在车队系中的固定布局为位置 `(x_i,y_i)`、朝向 `theta_i`，则成员中心在车队系中的速度为 `(Vx-omega*y_i, Vy+omega*x_i, omega)`，再通过 `R(-theta_i)` 转到本车体系后交给 `SendBodyTwist()`。QP/HQP只在需要同时调整车队参考速度、处理成员能力差异、松弛约束或严格任务优先级时再引入。 - “按状态最差车辆协调速度”采用车队共同可行性和统一缩放表达：普通能力受限时由所有成员约束确定共同速度比例；任一成员报警、通信超时、状态不可用或刚体误差越界时整队停车。时间戳、心跳、命令有效期和本地超时停车属于第一版安全契约，延迟预测补偿可以后续增加。 -- 虚拟车队可使用固定在车队坐标系中的前后GCP作为控制几何，例如位于 `±L_F`；这些点只用于把横向控制结果转换成车队参考点 `FleetTwist`，不是物理轮轴，也不直接参与单车四轮解算。前后GCP方向仍随控制输出动态变化，且横向控制器与GCP到Twist转换必须使用同一 `L_F`。该方案尚未实施。 +- 虚拟车队使用固定在车队坐标系中的对称前后GCP，把横向控制结果转换成车队原点 `FleetTwist`；这些点不是物理轮轴，也不直接参与单车四轮解算。横向控制器与GCP到Twist转换必须使用同一控制点半径；具体车辆级配置值和实车验证仍待完成。 - 每辆成员车都应作为反馈来源，但反馈职责必须分层：成员Detour位姿用于融合车队整体位姿和检查相对布局，单车轮速/舵角用于确认命令执行偏差，电机电流、扭矩或力传感信息用于负载与内力监控。相对位姿接近目标并不能证明没有内力，因此不能只依靠刚性连接或位姿误差判断负载均衡。 - 第一版不把每车β作为复杂优化变量：车队动作先明确主要滚动方向 `beta_fleet`（如正常0°、斜行45°、横移90°），成员按固定布局朝向换算 `beta_i = beta_fleet - theta_i`，并利用180°等效和有符号速度选择方便的本地表示。β是单车执行坐标系，不改变刚体分配得到的真实车体系 `BodyTwist`，也不会让各车命令数值相同；仅允许在全车停车时准备和激活，全部成员舵轮到位后通过同步屏障释放非零命令。只有出现复杂布局、整段方向变化、限位余量或频繁反号问题时，才增加轨迹级β候选搜索。 - 旧版参考项目采用固定双车布局：各车由 `carWorld ∘ layout⁻¹` 反推车队中心，再对位置和圆周航向求平均；路径控制器以该虚拟中心跟踪轨迹。同时它可按 `fleetTarget ∘ layout_i` 生成每车理想位姿，并叠加Detour布局纠偏和邻车两腿检测纠偏，因此并非只控制平均中心。来源：`原版停车机器人/parkingrobot/ClumsyPilot/PilotDefinition.cs`、`ChassisController.cs`。 diff --git a/docs/interfaces.md b/docs/interfaces.md index 094d971..44e9bf8 100644 --- a/docs/interfaces.md +++ b/docs/interfaces.md @@ -26,11 +26,16 @@ | `FleetLayout` | 不可变的成员布局快照，构造时校验成员数量、车号唯一性和位姿有限值 | | `FleetMotionCommand` | 车队参考点及该点在车队系中表达的刚体速度 | | `FleetMemberCommand` | 指定车辆及其真实车体系中表达的成员中心速度 | +| `FleetState` | 同一采样时刻的车队虚拟中心位姿和速度快照 | 坐标变换集中在 `FrameTransform2D`；有限值检查集中在 `NumericGuard`；角度处理集中在 `AngleMath`。 `FleetKinematics.Decompose()` 依据 `v_i = v_ref + omega × (r_i-r_ref)` 生成每车命令，再按 `VehicleLayout.PoseInFleet` 的朝向把线速度从车队系转换到成员车体系；所有刚性连接成员的 `Omega` 保持相同。 +`MultiWheelC/Fleet/FleetState.cs` 中，`FleetPoseInWorld` 表示车队虚拟中心位姿，其 `Yaw` 同时定义车队坐标系 `+X` 在世界系中的方向；车队系采用 `+X` 前、`+Y` 左、逆时针为正。`TwistAtFleetOriginInWorld` 与 `TwistAtFleetOriginInFleet` 是同一参考点、同一物理速度在两个坐标系中的表达，后者由前者和 `FleetPoseInWorld` 推导，不是第二份独立测量。`HasValidVelocityEstimate=false` 时速度按零保存，用于区分尚未形成可靠速度估计与真实零速。 + +`FleetState.SampleTimestampSeconds` 表示主车/协调器生成该车队状态快照时的本机单调时间，不是Detour全局时间。各成员电脑的本机时钟和Detour `tick` 当前不能直接互相比较；未来接收层应另行保存来源时间并完成新鲜度和时间对齐。 + ## 轨迹契约 文件：`MultiWheelC/Trajectory/`。 @@ -93,13 +98,18 @@ bool TryGetState(out VehicleState state) `ParkingVehicleStateProviderFactory.Create()` 创建： - `DetourVehicleStateProvider`：读取Detour位姿，处理源时间、重复帧、运动合理性、预测创新、静止确认和跳变候选；候选确认期间使用轮组速度短时预测控制位姿。确认后的有限小坐标偏移可更新 `controlFromDetour` 以保持当前任务坐标连续，超限或未恢复时将状态置为不可用。 -- `WheelFeedbackVehicleStateProvider`：调用 `MultiWheelChassis.GetCarSpeed(true)`，低通滤波车体 `Vx`、`Vy` 和由deg/s转换为rad/s的 `Vw`。最终 `VehicleState` 的平面速度使用轮组 `Vx/Vy`，Omega仍使用Detour估计；轮组 `Vw` 只提供给Detour短时运动预测和动态航向合理性判断。 +- `WheelFeedbackVehicleStateProvider`：调用 `MultiWheelChassis.GetCarSpeed(true)`，低通滤波车体 `Vx`、`Vy` 和由deg/s转换为rad/s的 `Vw`。轮组估计有效后，最终 `VehicleState` 的 `Vx/Vy/Omega` 均使用滤波后的轮组反馈；首帧速度标记为无效，并由 `VehicleState` 对外置零。轮组 `Vw` 同时提供给Detour短时运动预测和动态航向合理性判断。 默认滤波参数来自 `PilotConfig.ParkingControl.cs`：Detour线速度0.15s、Detour角速度0.20s、轮组反馈 `Vx/Vy/Vw` 统一为0.10s。 跳变处理的重要边界：航向创新允许量随 `|Vw| × Detour源帧间隔` 增加；候选若在近似原地自转期间开始，则整个候选确认过程禁止自动改写任务坐标系。默认候选确认窗口为0.60s，窗口内输出轮组预测状态，超时后完整位姿安全返回不可用。 -完整轨迹控制仍通过 `TryGetState()` 要求位置和航向均有效。原地自转则由 `WheelFeedbackVehicleStateProvider.TryGetHeadingRadians()` 获取独立可靠航向：仅位置异常时可进入 `PositionUnavailableHeadingAvailable`，不会中断正在进行的航向闭环；航向异常仍会停止动作。到达目标并停止驱动后，`MultiWheelRotateInPlace` 调用 `BeginPostRotationPositionRecovery()`，在原有3帧、0.60s及最大自动平移/航向偏移限制内恢复完整位姿，恢复失败时不释放下一运动段。 +完整轨迹控制仍通过 `TryGetState()` 要求位置和航向均有效。原地自转提供两种反馈模式： + +- `DetourAbsoluteHeading` 使用 `TryGetHeadingRadians()` 做世界航向闭环；仅位置异常时可继续，航向异常仍会停止。到达目标并停车后调用 `BeginPostRotationPositionRecovery()` 恢复完整位姿，恢复失败时不释放下一运动段。 +- `RelativeWheelOdometry` 使用 `TryGetWheelTwist(out Twist2D, out timestamp)` 直接读取并滤波轮组 `Vx/Vy/Vw`，按轮组采样时间梯形积分相对转角；活动自转期间不调用Detour，也不执行Detour停车后恢复。该模式用于允许约±5°误差的相对角动作，不能提供绝对世界航向校正，轮胎打滑和轮径误差会累积到角度结果中。 + +`TryGetWheelTwist()` 是不依赖Detour的窄接口；首次尚未形成滤波样本时返回 `false`，其时间戳来自该状态源使用的本机单调时钟。 `TrackingExperimentRecorder` 已记录原始/滤波轮组 `Vx/Vy/Vw`、轮组采样时刻、Detour `tick/l_step`、数据年龄和源帧间隔、轮速预测时刻、实际/允许的位置与航向创新、候选触发原因、估计器状态及不可用原因。创新和源帧间隔只在收到Detour新帧时更新；若CSV记录频率高于Detour帧率，后续记录行会重复最近一个新帧的诊断值，分析时应按 `DetourTickRaw` 去重或分组。 @@ -107,7 +117,7 @@ bool TryGetState(out VehicleState state) ### `PathTrackingContext` -一次控制周期的只读快照，包含：`VehicleState`、`TrajectoryProjection`、处理后的控制参考速度、预瞄曲率、真实 `deltaTime` 和运动方向β。 +一次控制周期的只读快照，包含：受控刚体的车体系 `Twist2D`、速度有效标志、`TrajectoryProjection`、处理后的控制参考速度、预瞄曲率、真实 `deltaTime` 和运动方向β。该类型不依赖单车 `VehicleState` 或车队 `FleetState`。 `ActualLongitudinalSpeedMetersPerSecond` 是完整车体平面速度沿β方向的投影： @@ -117,6 +127,14 @@ Vβ = cos(β)·Vx_body + sin(β)·Vy_body 因此45°/90°蟹行不能只使用车体 `Vx` 判断纵向速度。 +### `PathTrackingCore` + +`PathTrackingCore.Compute(Pose2D, Twist2D, bool, double)` 是单车与车队共用的纯轨迹跟踪周期：负责连续投影、距离保护、起步释放、终点制动/完成判断、曲率预瞄、横纵向控制和GCP分配，返回 `PathTrackingCycleOutput`。输出只包含周期结果、可选 `GcpMotionCommand`、投影和计算耗时；核心不读取状态源、不发送底盘命令，也不处理通信。 + +状态暂时不可用时，外层调用 `PauseForUnavailableState()` 保留当前轨迹与投影连续性；明确失败或取消分别使用 `Fail()`、`Cancel()`。 + +`ParkingGeometricController` 是单车适配层，负责读取 `IVehicleStateProvider` 并通过 `GcpCommandExecutor` 发送实体底盘命令。`FleetController` 是车队虚拟中心适配层，使用 `FleetState.FleetPoseInWorld` 和 `TwistAtFleetOriginInFleet` 调用同一核心，再将GCP结果转换为车队原点处的 `FleetMotionCommand`。 + 当前速度闭环和执行边界的语义并不完全相同：纵向PID与Stanley实际速度分母使用 `Vβ`（Stanley也可按配置改用参考速度），而 `GcpMotionCommand.SpeedMetersPerSecond` 和旧版 `SendMotion.speed` 表示带行驶方向符号的车体中心平移速度模长。正常圆弧理想跟踪时 `Vy=0`，两者相等；只有横向误差共同转角产生非零 `Vy` 时，模长与 `Vβ` 才相差余弦因子。当前最大命令速度仍限制最终发送的模长。 ### `ILateralController` diff --git a/docs/overview.md b/docs/overview.md index 3c85092..e7d05e4 100644 --- a/docs/overview.md +++ b/docs/overview.md @@ -2,7 +2,7 @@ ## 项目定位 -`MyParking` 是停车机器人控制软件的当前正式开发版本，主体为 C# 插件工程。项目目标是让多舵轮停车机器人完成底盘运动、车辆状态获取、轨迹跟踪、原地自转和后续停车作业；当前研发重点仍是单车闭环和实车联调，多车协同尚未形成可运行实现。来源：`README.md`、`MultiWheelC/PilotConfig.cs`、`Shared/Fleet/FleetKinematics.cs`。 +`MyParking` 是停车机器人控制软件的当前正式开发版本，主体为 C# 插件工程。项目目标是让多舵轮停车机器人完成底盘运动、车辆状态获取、轨迹跟踪、原地自转和后续停车作业；当前研发重点仍是单车闭环和实车联调，多车协同已有纯计算骨架但尚未形成可运行实现。来源：`README.md`、`MultiWheelC/PilotConfig.cs`、`MultiWheelC/Fleet/`、`Shared/Fleet/`。 项目不是可直接 `dotnet run` 的独立应用： @@ -17,7 +17,7 @@ - 代码接口面向四个可转向轮组，并暴露8个驱动电机的位置/速度反馈和4个舵角反馈。M/C IO定义见 `PilotDefinition`、`DiverCartDefinition`。 - 车体支持正常前后行驶、倒车轨迹、任意固定运动方向β下的滚动运动、蟹行和停车后原地自转。主要入口见 `MultiWheelC/Movements/` 和 `MultiWheelC/Experiments/`。 - 夹臂速度、位置、限位和报警IO已经接入，但轮胎识别、自动钻车、释放车辆和完整停车作业状态机尚未在当前新版流程中完成。来源：`PilotConfig.cs`、`PilotDefinition.cs`、`README.md`。 -- 多车模型数据类型已经预留，但多车配置位于 `PilotConfig.cs` 的 `#if false`，`FleetKinematics.cs` 只有占位注释，不能把当前代码视为已支持多车编队。 +- 多车已具备 `FleetLayout`、`FleetState`、`FleetController` 和 `FleetKinematics` 组成的纯计算链；多车配置仍位于 `PilotConfig.cs` 的 `#if false`，且状态聚合、通信、成员纠偏、安全协调和实体发送尚未接入，因此不能把当前代码视为已支持可运行的多车编队。 ## 整体运行流程 diff --git a/docs/problems.md b/docs/problems.md index a24b2d0..0b336d7 100644 --- a/docs/problems.md +++ b/docs/problems.md @@ -45,6 +45,12 @@ - 位置：`DetourVehicleStateProvider`、`WheelFeedbackVehicleStateProvider`、`MultiWheelRotateInPlace`、`PilotConfig.ParkingControl.cs`。 - 验证：第三轮实车组合动作中发生一次约47mm/3.44°的原始坐标变化，自动连续化计数增加后控制轨迹保持连续，动作最终以约24.7mm剩余距离、2.8mm横向误差完成。 +### Detour自转退化会中断允许小角度误差的相对自转 + +- 处理：`MultiWheelRotateInPlace` 增加 `RelativeWheelOdometry` 反馈模式，直接读取并积分滤波轮组 `Vw`；活动自转期间不调用Detour。原有 `DetourAbsoluteHeading` 模式继续用于需要世界绝对航向的任务。 +- 验证：2026-08-21第七轮包含19份轮组里程计自转记录，其中17份表现为正常停车并完成舵轮回正；这17份中轮组积分转角与Detour原始航向变化的绝对差中位数约0.89°、最大约3.07°。同批Detour原始位置端点变化中位数约56mm、最大约298mm，说明轮组模式确实绕开了自转期间的Detour退化依赖。 +- 限制：CSV尚未记录请求角度、内部积分角和明确完成原因，因此上述数据不能作为目标角精度的正式验收；另有2份记录未完成回正，属于手动停止、小角度超时或其他原因仍待确认。轮组积分也不能替代绝对世界航向。 + ## 待解决 ### 完整停车作业流程尚未实现 @@ -52,11 +58,16 @@ - 缺少或未接入新版流程：轮胎识别、自动钻车、夹抱/释放动作编排、完整安全状态机。 - 现有 `PilotConfig` 和IO字段不能视为业务流程已经完成。 -### 多车能力仍是占位 +### 多车执行链仍未完成 -- `Shared/Fleet/FleetKinematics.cs` 没有实现。 +- 纯计算链 `FleetState → FleetController → FleetMotionCommand → FleetKinematics.Decompose()` 已实现，并有6个车队控制周期场景和4个刚体分解场景通过。 - `PilotConfig.cs` 的多车区域在 `#if false` 中，不参与当前编译。 -- 当前没有车队状态、通信、命令分配、安全降级和多车测试闭环。 +- 当前仍没有布局采集/激活、车队状态估计、通信接收与时间对齐、相对布局闭环、能力限幅、安全降级、成员底盘发送和多车实车测试闭环，因此不能把纯计算层视为可运行的多车功能。 + +### 轮组速度初始化回退语义不一致 + +- `WheelFeedbackVehicleStateProvider` 在轮组首帧无效时局部选择Detour `Omega`，但随后以 `hasValidVelocityEstimate=false` 构造 `VehicleState`；`VehicleState` 会把整份速度对外置零，因此该Detour角速度回退实际不可见。 +- 影响仅限轮组滤波尚未形成有效采样的初始化阶段；应确认期望是保持整份速度无效并置零，还是允许单独提供有效的Detour角速度，再决定是否修改代码和注释。 ### Detour跳变判据和动作段衔接仍需收敛 @@ -68,7 +79,7 @@ - 健康 `l_step` 下仅凭预测创新不应轻易自动改写坐标系；在完成同一时刻比较前，不继续通过反复放宽阈值堆叠补丁。 - 不建议通过全局放宽0.60s确认窗口、40mm残差或允许自转期间完整坐标修正来掩盖问题。 - 2026-08-19第六轮记录确认普通正向/倒车直线、曲线和小范围运动中坐标连续化总体可用，主要剩余失败集中在原地自转：有样本在 `l_step=94` 时连续3个Detour新帧航向创新超限，状态层先短时使用轮组Vw预测，第三帧才安全终止，说明单帧航向容错已按设计生效；另有约163mm、207mm的位置不连续或候选不稳定导致停车后恢复失败。 -- 当前决定：保持普通运动150mm自动连续化上限、自转最大角速度30°/s和角加速度40°/s²，不以全局放宽阈值或提高自转速度掩盖Detour退化。原地自转后的条件化“仅位置连续化”仍是可评估方案，但尚未实施；持续航向异常需要Detour质量/重定位语义或额外可信航向来源才能进一步收敛。 +- 当前决定：保持普通运动150mm自动连续化上限、自转最大角速度30°/s和角加速度40°/s²，不以全局放宽阈值或提高自转速度掩盖Detour退化。允许相对角误差的动作可选已实现的轮组里程计模式；必须闭环到绝对世界航向时，持续航向异常仍需要Detour质量/重定位语义或额外可信绝对航向来源才能进一步收敛。 ### 自动化测试和CI覆盖有限 diff --git a/docs/progress.md b/docs/progress.md index a13c120..73fd53f 100644 --- a/docs/progress.md +++ b/docs/progress.md @@ -1,6 +1,6 @@ # 当前进展 -更新日期：2026-08-20。这里只保存当前状态，不作为完整开发历史。 +更新日期：2026-08-21。这里只保存当前状态，不作为完整开发历史。 ## 已完成/已接入 @@ -9,28 +9,30 @@ - 弧长参数化 `Trajectory2D`、统一插值、轨迹投影窗口和进度连续性。 - 默认 Stanley 横向 + PID 纵向的新版轨迹控制链，横向控制器可注入替换。 - 前进4m、倒车4m、45°蟹行4m、直线-左半圆-直线和组合运动宿主测试入口。 -- Detour位姿与轮组反馈组合状态源：Vx/Vy用于控制速度，Vw用于短时位姿预测和动态航向合理性判断，最终控制Omega仍来自Detour。 +- Detour位姿与轮组反馈组合状态源：位姿来自Detour；轮组估计有效后，控制状态的Vx/Vy/Omega均来自滤波后的轮组反馈，Vw同时用于短时位姿预测和动态航向合理性判断。 - Detour源 `tick/l_step` 诊断、跳变候选确认、有限小偏移任务坐标连续化、自转期间禁止自动吸收偏移，以及超时状态不可用保护。 - 起步释放、曲率前馈预瞄、GCP角速度限制、终点制动预瞄和单向低速收敛。 - 原地自转舵轮准备、航向PID、超时保护和完成后回正；自转期间位置/航向有效性已分离，到位停车后才恢复完整位姿并决定是否释放下一段。 - Detour单帧航向异常已增加连续帧/短时预测确认；第六轮实车数据确认前两帧异常由轮组Vw预测承接，持续到第三个异常新帧时才安全终止。 +- 原地自转已增加 `RelativeWheelOdometry` 模式和不依赖Detour的 `TryGetWheelTwist()` 接口；第七轮19份记录中17份表现为正常停车回正，轮组积分与Detour原始航向变化绝对差中位数约0.89°、最大约3.07°，但目标角精度尚缺专用字段正式验收。 - C层轨迹/周期CSV与M层轮速/舵角诊断记录。 - C层Detour静态诊断入口，记录源时间、`l_step`、位姿帧差和轮组静态反馈；已有约14分41秒实车静态基线。 - C层轨迹CSV已补充原始/滤波轮组 `Vx/Vy/Vw`、轮组与预测时刻、Detour帧间隔、实际/允许创新、候选原因、估计器状态和不可用原因。 - 停车控制参数集中到 `Configuration/PilotConfig.ParkingControl.cs`。 +- `PathTrackingContext` 已解除对单车 `VehicleState` 的依赖；`PathTrackingCore` 统一单车与车队的投影、保护/终点策略、横纵向控制和GCP分配，`ParkingGeometricController` 保留单车状态读取与实体命令执行职责。 - `MultiWheelC.Tests` 已提供不依赖实车的Stanley前进/倒车横向符号回归，8个场景通过；该项目不进入正式解决方案和打包脚本。 -- `Shared/Fleet` 已加入不可变 `FleetLayout`、成员/车队命令模型和确定性 `FleetKinematics`；不依赖实车的4个刚体分解场景已通过。 +- `Shared/Fleet` 已加入不可变 `FleetLayout`、成员/车队命令模型和确定性 `FleetKinematics`；`MultiWheelC/Fleet` 已加入虚拟中心 `FleetState` 和复用 `PathTrackingCore` 的纯计算 `FleetController`。6个车队控制周期场景和4个刚体分解场景通过。 - 本次建立工作区/项目AGENTS导航和 `docs/` 按需知识库。 以上表示代码入口存在，不表示全部实车工况已经验收。 ## 当前进行方向 -- 普通正向/倒车直线、曲线和小范围运动中坐标连续化已基本可用；当前单车状态估计的主要未决问题收敛到原地自转时Detour偶发持续退化。 +- 普通正向/倒车直线、曲线和小范围运动中坐标连续化已基本可用；允许小角度误差的相对自转已有轮组里程计模式，必须对准绝对世界航向时仍受Detour偶发持续退化限制。 - 暂时冻结普通运动跳变阈值和30°/s、40°/s²自转参数，等待Detour接口语义后再决定是否实施自转结束后的条件化仅位置连续化或调整航向恢复策略。 - Detour对接最小问题已整理到 `docs/detour-information-checklist.md`，不要求取得源码。 - 验证非零β运动系，当前已有45°蟹行直线入口；曲线蟹行仍需设计实验。 -- 多车共同搬运已完成布局模型和纯刚体速度分解；下一阶段是夹紧后布局采集/原子激活、车队状态与控制、通信和安全协调，再把成员命令接入各车 `SendBodyTwist()`。QP/HQP不作为第一版前置条件。 +- 多车共同搬运已完成布局模型、虚拟中心状态、虚拟中心轨迹闭环和纯刚体速度分解；下一步处理夹紧后布局采集/原子激活、车队状态估计、通信和安全协调，并把成员命令接入各车 `SendBodyTwist()`。QP/HQP不作为第一版前置条件。 - β在多车中定位为单车执行坐标系而非车队核心优化量：常规、斜行和横移动作先确定车队主要滚动方向，各车按布局朝向换算本地β，停车预对齐并经车队同步屏障统一释放。轨迹级β搜索只作为机械余量或复杂方向变化下的后续增强。 ## 阻塞/待确认 @@ -48,8 +50,8 @@ 1. 按 `docs/detour-information-checklist.md` 确认 `getCartLocation()` 字段、`tick`、`l_step`、定位延迟、重定位行为和质量状态接口。 2. 在Detour信息返回前不继续全局放宽状态估计边界，也不提高自转角速度/角加速度；现有偶发定位不可用保持安全停车。 -3. 若工程上必须先降低位置型自转失败率，再单独设计仅在纯自转结束、轮组无平移且航向有效时启用的位置连续化；不得扩展到普通轨迹运动，也不得吸收航向偏移。 +3. 为轮组里程计自转CSV补充请求角度、内部积分角、完成/超时/手动停止原因，按固定目标角重复验证误差和重复性；绝对航向任务继续保留Detour模式，不用轮组积分冒充世界航向。 4. 为轨迹插值/投影、坐标变换、状态跳变候选和终点策略继续补充不依赖宿主的数学回归测试。 5. 设计车队建立生命周期：预设布局引导就位，夹紧静止后同步采集成员位姿，创建并原子激活新的 `FleetLayout`，松开后清除；车队参考系和采样同步方法需先明确。 -6. 继续补充 `FleetKinematics` 的圆弧、任意布局和倒车数学场景，再接入车队状态与控制流程，不直接启用 `#if false` 旧多车代码。 +6. 建立车队状态与执行边界：将时间对齐后的成员状态聚合为 `FleetState`，把 `FleetKinematics` 的成员命令接入各车 `SendBodyTwist()`；通信、成员纠偏和底盘发送不塞回 `PathTrackingCore`，也不直接启用 `#if false` 旧多车代码。 7. 在接入通信前定义车队状态时间对齐、心跳/命令有效期、共同速度缩放、成员故障整队停车和舵轮准备同步屏障。 diff --git a/output/C/CommonUsage.dll b/output/C/CommonUsage.dll index 9fcee49..403ea8b 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 46fcd3e..864ccd2 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 0199b2b..b41c738 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 9fcee49..403ea8b 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 d63173f..3eb4a89 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 de52922..2f446aa 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 9fcee49..403ea8b 100644 Binary files a/ref/CommonUsage.dll and b/ref/CommonUsage.dll differ diff --git a/参考文档/记录.txt b/参考文档/记录.txt index 2048417..10525ee 100644 --- a/参考文档/记录.txt +++ b/参考文档/记录.txt @@ -45,3 +45,10 @@ ParkingGeometricController.cs +定义 FleetState +至少包含： +车队参考中心世界位姿 +车队整体世界/车队系速度 +采样时间 +状态是否可用 +成员相对布局误差 \ No newline at end of file