增加实验测试

MedullaAdapter/DiverCartDefinition.cs

@@ -64,6 +64,16 @@ namespace MedullaAdapter
         #endregion
 
         #region  初始参数
+
+        [AsInitParam(desc = "差速转舵目标角速度前馈增益")]
+        public float DiffSteerRateFeedforwardGain = 0.9f;
+
+        [AsInitParam(desc = "差速舵轮左右轮间距，单位mm")]
+        public float DiffSteerWheelDistanceMillimeters = 85f;
+
+        [AsInitParam(desc = "差速转舵前馈最大速度，单位m/s")]
+        public float DiffSteerRateFeedforwardMaximumSpeed = 0.03f;
+
         [AsInitParam(desc = "MCU端口号")] public string MCUPort = "COM4";
         [AsInitParam(desc = "遥控器速度上限")] public float TransmitterSpeedUpperLimit = 1.0f;
         [AsInitParam(desc = "遥控器速度下限")] public float TransmitterSpeedLowerLimit = 0.0f;
@@ -100,6 +110,14 @@ namespace MedullaAdapter
         [IOObjectMonitor(desc = "左后舵轮转向PID输出")] public float DiffSteerOutputLeftRear;
         [IOObjectMonitor(desc = "右前舵轮转向PID输出")] public float DiffSteerOutputRightFront;
         [IOObjectMonitor(desc = "右后舵轮转向PID输出")] public float DiffSteerOutputRightRear;
+        [IOObjectMonitor(desc = "左前舵轮目标角速度前馈输出")] public float DiffSteerRateFeedforwardLeftFront;
+        [IOObjectMonitor(desc = "左后舵轮目标角速度前馈输出")] public float DiffSteerRateFeedforwardLeftRear;
+        [IOObjectMonitor(desc = "右前舵轮目标角速度前馈输出")] public float DiffSteerRateFeedforwardRightFront;
+        [IOObjectMonitor(desc = "右后舵轮目标角速度前馈输出")] public float DiffSteerRateFeedforwardRightRear;
+        [IOObjectMonitor(desc = "左前舵轮转向合成差速输出")] public float DiffSteerTotalOutputLeftFront;
+        [IOObjectMonitor(desc = "左后舵轮转向合成差速输出")] public float DiffSteerTotalOutputLeftRear;
+        [IOObjectMonitor(desc = "右前舵轮转向合成差速输出")] public float DiffSteerTotalOutputRightFront;
+        [IOObjectMonitor(desc = "右后舵轮转向合成差速输出")] public float DiffSteerTotalOutputRightRear;
         [IOObjectMonitor(desc = "灯光模式")] public int LightMode = 0;
         [IOObjectMonitor(desc = "实体遥控器当前速度倍率")] public float TransmitterSpeed = 0.3f;
         [IOObjectMonitor(desc = "轮速诊断记录已启用")]

MedullaAdapter/MotorRoutine.cs

@@ -3,14 +3,22 @@ using CartActivator;
 using FundamentalLib;
 using MDCSToolBox.Commons;
 using System;
+using System.Diagnostics;
 using static MDCSToolBox.Medulla.Chassis.BasicCartDefinition;
 
 namespace MedullaAdapter
 {
     public class MotorRoutine : LadderLogic<DiverCartDefinition>
     {
+        private const double MaximumFeedforwardIntervalSeconds = 0.2;
         private bool _wasTransmitterControlling;
         private DateTime _lastMoveTime = DateTime.Now;
+        private bool _diffSteerFeedforwardInitialized;
+        private long _lastDiffSteerFeedforwardTimestamp;
+        private float _previousThLeftFront;
+        private float _previousThLeftRear;
+        private float _previousThRightFront;
+        private float _previousThRightRear;
         private DiverCartDefinition.ManualControlMode?
             _pendingTransmitterControlMode;
         private DateTime _pendingTransmitterControlModeSince =
@@ -213,7 +221,9 @@ namespace MedullaAdapter
             cart.SpeedRightArm = 0;
         }
 
-        // M层单车底盘：根据四个舵轮的目标角度和实际角度修正8个驱动电机速度。
+        /// <summary>
+        /// 根据四个舵轮的目标角速度前馈和实际角度反馈修正八个驱动电机速度。
+        /// </summary>
         private void UpdateDiffSteerWheelSpeeds()
         {
             if (cart.LeftFrontPid == null ||
@@ -229,6 +239,7 @@ namespace MedullaAdapter
                 cart.SpeedLRR = 0;
                 cart.SpeedRRL = 0;
                 cart.SpeedRRR = 0;
+                ResetDiffSteerRateFeedforward();
                 return;
             }
 
@@ -272,26 +283,45 @@ namespace MedullaAdapter
                 cart.DiffSteerThresh,
                 cart.DiffSteerSpeedAcc);
 
-            // 根据实际舵角计算四条腿的差速修正量。
-            var diffLf = cart.LeftFrontPid.GetResponse(
+            // 根据实际舵角计算四条腿的PID反馈修正量。
+            var feedbackLf = cart.LeftFrontPid.GetResponse(
                 cart.ThLeftFront, false, false, "LF");
 
-            var diffLr = cart.LeftRearPid.GetResponse(
+            var feedbackLr = cart.LeftRearPid.GetResponse(
                 cart.ThLeftRear, false, false, "LR");
 
-            var diffRf = cart.RightFrontPid.GetResponse(
+            var feedbackRf = cart.RightFrontPid.GetResponse(
                 cart.ThRightFront, false, false, "RF");
 
-            var diffRr = cart.RightRearPid.GetResponse(
+            var feedbackRr = cart.RightRearPid.GetResponse(
                 cart.ThRightRear, false, false, "RR");
 
-            // 保存四个转向PID的本周期修正量，供M层监控和舵轮响应CSV记录使用。
-            cart.DiffSteerOutputLeftFront = diffLf;
-            cart.DiffSteerOutputLeftRear = diffLr;
-            cart.DiffSteerOutputRightFront = diffRf;
-            cart.DiffSteerOutputRightRear = diffRr;
+            CalculateDiffSteerRateFeedforward(
+                out var feedforwardLf,
+                out var feedforwardLr,
+                out var feedforwardRf,
+                out var feedforwardRr);
 
-            // 左前腿：左右电机施加方向相反的PID修正量。
+            var diffLf = feedbackLf + feedforwardLf;
+            var diffLr = feedbackLr + feedforwardLr;
+            var diffRf = feedbackRf + feedforwardRf;
+            var diffRr = feedbackRr + feedforwardRr;
+
+            // 分别保留PID、前馈和合成差速，便于独立标定与诊断。
+            cart.DiffSteerOutputLeftFront = feedbackLf;
+            cart.DiffSteerOutputLeftRear = feedbackLr;
+            cart.DiffSteerOutputRightFront = feedbackRf;
+            cart.DiffSteerOutputRightRear = feedbackRr;
+            cart.DiffSteerRateFeedforwardLeftFront = feedforwardLf;
+            cart.DiffSteerRateFeedforwardLeftRear = feedforwardLr;
+            cart.DiffSteerRateFeedforwardRightFront = feedforwardRf;
+            cart.DiffSteerRateFeedforwardRightRear = feedforwardRr;
+            cart.DiffSteerTotalOutputLeftFront = diffLf;
+            cart.DiffSteerTotalOutputLeftRear = diffLr;
+            cart.DiffSteerTotalOutputRightFront = diffRf;
+            cart.DiffSteerTotalOutputRightRear = diffRr;
+
+            // 左前腿：左右电机施加方向相反的合成差速修正量。
             cart.SpeedLFL = cart.SpeedLeftFrontLeft - diffLf;
             cart.SpeedLFR = cart.SpeedLeftFrontRight + diffLf;
 
@@ -308,6 +338,129 @@ namespace MedullaAdapter
             cart.SpeedRRR = cart.SpeedRightRearRight + diffRr;
         }
 
+        /// <summary>
+        /// 根据四个机械目标舵角的实际变化率计算本周期差速轮线速度前馈。
+        /// </summary>
+        private void CalculateDiffSteerRateFeedforward(
+            out float leftFront,
+            out float leftRear,
+            out float rightFront,
+            out float rightRear)
+        {
+            leftFront = 0f;
+            leftRear = 0f;
+            rightFront = 0f;
+            rightRear = 0f;
+
+            var currentTimestamp = Stopwatch.GetTimestamp();
+
+            if (_diffSteerFeedforwardInitialized)
+            {
+                var deltaTimeSeconds =
+                    (currentTimestamp -
+                     _lastDiffSteerFeedforwardTimestamp) /
+                    (double)Stopwatch.Frequency;
+
+                if (deltaTimeSeconds > 0.0 &&
+                    deltaTimeSeconds <=
+                    MaximumFeedforwardIntervalSeconds)
+                {
+                    leftFront = CalculateDiffSteerRateFeedforward(
+                        cart.ThLeftFront,
+                        _previousThLeftFront,
+                        deltaTimeSeconds);
+                    leftRear = CalculateDiffSteerRateFeedforward(
+                        cart.ThLeftRear,
+                        _previousThLeftRear,
+                        deltaTimeSeconds);
+                    rightFront = CalculateDiffSteerRateFeedforward(
+                        cart.ThRightFront,
+                        _previousThRightFront,
+                        deltaTimeSeconds);
+                    rightRear = CalculateDiffSteerRateFeedforward(
+                        cart.ThRightRear,
+                        _previousThRightRear,
+                        deltaTimeSeconds);
+                }
+            }
+
+            _previousThLeftFront = cart.ThLeftFront;
+            _previousThLeftRear = cart.ThLeftRear;
+            _previousThRightFront = cart.ThRightFront;
+            _previousThRightRear = cart.ThRightRear;
+            _lastDiffSteerFeedforwardTimestamp = currentTimestamp;
+            _diffSteerFeedforwardInitialized = true;
+        }
+
+        /// <summary>
+        /// 将单个机械目标舵角变化率转换为带限幅的左右轮差速线速度前馈。
+        /// </summary>
+        private float CalculateDiffSteerRateFeedforward(
+            float targetAngleDegrees,
+            float previousTargetAngleDegrees,
+            double deltaTimeSeconds)
+        {
+            var gain = cart.DiffSteerRateFeedforwardGain;
+            var wheelDistanceMillimeters =
+                cart.DiffSteerWheelDistanceMillimeters;
+            var maximumSpeed =
+                cart.DiffSteerRateFeedforwardMaximumSpeed;
+
+            if (!IsFinite(gain) || gain <= 0f ||
+                !IsFinite(wheelDistanceMillimeters) ||
+                wheelDistanceMillimeters <= 0f ||
+                !IsFinite(maximumSpeed) || maximumSpeed <= 0f ||
+                !IsFinite(targetAngleDegrees) ||
+                !IsFinite(previousTargetAngleDegrees))
+            {
+                return 0f;
+            }
+
+            // 机械舵角受限，必须使用直接差值而不是圆周最短角差。
+            var targetRateRadiansPerSecond =
+                (targetAngleDegrees - previousTargetAngleDegrees) *
+                Math.PI / 180.0 /
+                deltaTimeSeconds;
+            var wheelDistanceMeters =
+                wheelDistanceMillimeters / 1000.0;
+            var feedforwardSpeed =
+                0.5 *
+                wheelDistanceMeters *
+                targetRateRadiansPerSecond *
+                gain;
+
+            return (float)Math.Clamp(
+                feedforwardSpeed,
+                -maximumSpeed,
+                maximumSpeed);
+        }
+
+        /// <summary>
+        /// 清除差速转舵前馈历史和监控输出，避免恢复控制时使用过期目标角。
+        /// </summary>
+        private void ResetDiffSteerRateFeedforward()
+        {
+            _diffSteerFeedforwardInitialized = false;
+            _lastDiffSteerFeedforwardTimestamp = 0;
+            cart.DiffSteerRateFeedforwardLeftFront = 0f;
+            cart.DiffSteerRateFeedforwardLeftRear = 0f;
+            cart.DiffSteerRateFeedforwardRightFront = 0f;
+            cart.DiffSteerRateFeedforwardRightRear = 0f;
+            cart.DiffSteerTotalOutputLeftFront = 0f;
+            cart.DiffSteerTotalOutputLeftRear = 0f;
+            cart.DiffSteerTotalOutputRightFront = 0f;
+            cart.DiffSteerTotalOutputRightRear = 0f;
+        }
+
+        /// <summary>
+        /// 判断单精度参数是否可安全参与底盘控制计算。
+        /// </summary>
+        private static bool IsFinite(float value)
+        {
+            return !float.IsNaN(value) &&
+                   !float.IsInfinity(value);
+        }
+
         // M层单车限速：按照加速度和减速度平滑更新实际下发速度上限。
         private void UpdateSendSpeedLimit()
         {

MedullaAdapter/WheelSpeedDiagnosticLogger.cs

@@ -84,7 +84,10 @@ namespace MedullaAdapter
                 _snapshotWriter.WriteLine(
                     "ElapsedMs,CarNum,ManualControlMode,ManualMode,SendThresSpeed," +
                     "DiffSteerKp,DiffSteerKi,DiffSteerKd,DiffSteerMaxI,DiffSteerDeadZone,DiffSteerThresh,DiffSteerSpeedAcc," +
+                    "DiffSteerRateFeedforwardGain,DiffSteerWheelDistanceMillimeters,DiffSteerRateFeedforwardMaximumSpeed," +
                     "PidOutLeftFront,PidOutLeftRear,PidOutRightFront,PidOutRightRear," +
+                    "RateFeedforwardLeftFront,RateFeedforwardLeftRear,RateFeedforwardRightFront,RateFeedforwardRightRear," +
+                    "TotalDiffLeftFront,TotalDiffLeftRear,TotalDiffRightFront,TotalDiffRightRear," +
                     "CmdLFL,CmdLFR,CmdLRL,CmdLRR,CmdRFL,CmdRFR,CmdRRL,CmdRRR," +
                     "PidLFL,PidLFR,PidLRL,PidLRR,PidRFL,PidRFR,PidRRL,PidRRR," +
                     "ActualLFL,ActualLFR,ActualLRL,ActualLRR,ActualRFL,ActualRFR,ActualRRL,ActualRRR," +
@@ -217,10 +220,21 @@ namespace MedullaAdapter
                 Format(cart.DiffSteerDeadZone),
                 Format(cart.DiffSteerThresh),
                 Format(cart.DiffSteerSpeedAcc),
+                Format(cart.DiffSteerRateFeedforwardGain),
+                Format(cart.DiffSteerWheelDistanceMillimeters),
+                Format(cart.DiffSteerRateFeedforwardMaximumSpeed),
                 Format(cart.DiffSteerOutputLeftFront),
                 Format(cart.DiffSteerOutputLeftRear),
                 Format(cart.DiffSteerOutputRightFront),
                 Format(cart.DiffSteerOutputRightRear),
+                Format(cart.DiffSteerRateFeedforwardLeftFront),
+                Format(cart.DiffSteerRateFeedforwardLeftRear),
+                Format(cart.DiffSteerRateFeedforwardRightFront),
+                Format(cart.DiffSteerRateFeedforwardRightRear),
+                Format(cart.DiffSteerTotalOutputLeftFront),
+                Format(cart.DiffSteerTotalOutputLeftRear),
+                Format(cart.DiffSteerTotalOutputRightFront),
+                Format(cart.DiffSteerTotalOutputRightRear),
                 Format(cart.SpeedLeftFrontLeft),
                 Format(cart.SpeedLeftFrontRight),
                 Format(cart.SpeedLeftRearLeft),

MultiWheelC/Configuration/PilotConfig.ParkingControl.cs

@@ -55,6 +55,12 @@ public partial class PilotConfig
     [FieldMember(desc = "停车控制：Stanley使用电机实际速度")]
     public bool ParkingStanleyUseActualSpeed = true;
 
+    [FieldMember(desc = "停车控制：Stanley曲率前馈预瞄时间(s)，0为关闭")]
+    public float ParkingStanleyCurvaturePreviewSeconds = 0.15f;
+
+    [FieldMember(desc = "停车控制：Stanley曲率前馈最大预瞄距离(m)")]
+    public float ParkingStanleyMaximumCurvaturePreviewMeters = 0.12f;
+
     [FieldMember(desc = "停车控制：Stanley横向修正上限(deg)")]
     public float ParkingMaximumCrossTrackCorrectionDegrees = 10f;
 

MultiWheelC/Control/Abstractions/PathTrackingContext.cs

@@ -16,11 +16,15 @@ namespace MultiWheelC.Control.Abstractions
             VehicleState vehicleState,
             TrajectoryProjection projection,
             double controlReferenceSpeedMetersPerSecond,
+            double feedforwardCurvaturePerMeter,
             double deltaTimeSeconds)
         {
             EnsureFinite(
                 controlReferenceSpeedMetersPerSecond,
                 nameof(controlReferenceSpeedMetersPerSecond));
+            EnsureFinite(
+                feedforwardCurvaturePerMeter,
+                nameof(feedforwardCurvaturePerMeter));
             EnsureFinitePositive(
                 deltaTimeSeconds,
                 nameof(deltaTimeSeconds));
@@ -29,6 +33,8 @@ namespace MultiWheelC.Control.Abstractions
             Projection = projection;
             ControlReferenceSpeedMetersPerSecond =
                 controlReferenceSpeedMetersPerSecond;
+            FeedforwardCurvaturePerMeter =
+                feedforwardCurvaturePerMeter;
             DeltaTimeSeconds = deltaTimeSeconds;
         }
 
@@ -66,6 +72,11 @@ namespace MultiWheelC.Control.Abstractions
             Projection.ReferencePoint
                 .CurvaturePerMeter;
 
+        /// <summary>
+        /// 获取沿轨迹点序适量预瞄后专供几何前馈使用的参考曲率，单位为1/m，左弯为正。
+        /// </summary>
+        public double FeedforwardCurvaturePerMeter { get; }
+
         /// <summary>
         /// 获取参考轨迹相对车辆的有符号横向误差，单位为m，轨迹在车辆左侧时为正。
         /// </summary>

MultiWheelC/Control/Execution/ParkingGeometricController.cs

@@ -110,7 +110,9 @@ namespace MultiWheelC.Control.Execution
             double terminalBrakingPreviewMeters = 0.02,
             double terminalApproachDistanceMeters = 0.10,
             double terminalApproachGainPerSecond = 0.8,
-            double maximumTerminalApproachSpeedMetersPerSecond = 0.05)
+            double maximumTerminalApproachSpeedMetersPerSecond = 0.05,
+            double curvaturePreviewSeconds = 0.20,
+            double maximumCurvaturePreviewMeters = 0.12)
         {
             _stateProvider = stateProvider ??
                 throw new ArgumentNullException(
@@ -152,6 +154,12 @@ namespace MultiWheelC.Control.Execution
             EnsureFinitePositive(
                 maximumTerminalApproachSpeedMetersPerSecond,
                 nameof(maximumTerminalApproachSpeedMetersPerSecond));
+            EnsureFiniteNonNegative(
+                curvaturePreviewSeconds,
+                nameof(curvaturePreviewSeconds));
+            EnsureFiniteNonNegative(
+                maximumCurvaturePreviewMeters,
+                nameof(maximumCurvaturePreviewMeters));
 
             if (terminalApproachDistanceMeters <=
                 finishDistanceMeters)
@@ -176,6 +184,9 @@ namespace MultiWheelC.Control.Execution
                 terminalApproachGainPerSecond;
             MaximumTerminalApproachSpeedMetersPerSecond =
                 maximumTerminalApproachSpeedMetersPerSecond;
+            CurvaturePreviewSeconds = curvaturePreviewSeconds;
+            MaximumCurvaturePreviewMeters =
+                maximumCurvaturePreviewMeters;
         }
 
         /// <summary>
@@ -218,6 +229,16 @@ namespace MultiWheelC.Control.Execution
         /// </summary>
         public double MaximumTerminalApproachSpeedMetersPerSecond { get; }
 
+        /// <summary>
+        /// 获取按照车辆纵向速度换算曲率前馈预瞄距离的预测时间，单位为s，0表示关闭。
+        /// </summary>
+        public double CurvaturePreviewSeconds { get; }
+
+        /// <summary>
+        /// 获取曲率前馈沿轨迹点序允许预瞄的最大距离，单位为m。
+        /// </summary>
+        public double MaximumCurvaturePreviewMeters { get; }
+
         /// <summary>
         /// 获取控制器当前是否持有并正在执行一条轨迹。
         /// </summary>
@@ -259,6 +280,16 @@ namespace MultiWheelC.Control.Execution
         /// </summary>
         public double? LastControlReferenceSpeedMetersPerSecond { get; private set; }
 
+        /// <summary>
+        /// 获取最近控制周期实际采用的曲率前馈预瞄距离，单位为m。
+        /// </summary>
+        public double? LastCurvaturePreviewDistanceMeters { get; private set; }
+
+        /// <summary>
+        /// 获取最近控制周期沿轨迹预瞄后交给横向控制器的曲率，单位为1/m。
+        /// </summary>
+        public double? LastFeedforwardCurvaturePerMeter { get; private set; }
+
         /// <summary>
         /// 获取最近一次控制周期的分阶段耗时诊断；尚未执行时为空。
         /// </summary>
@@ -431,10 +462,23 @@ namespace MultiWheelC.Control.Execution
                         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);
                 var lateralCommand =
                     _lateralController.Compute(context);
@@ -552,6 +596,50 @@ namespace MultiWheelC.Control.Execution
                 vehicleState);
         }
 
+        /// <summary>
+        /// 根据有效实际纵向速度或控制参考速度计算带上限的曲率前馈预瞄距离。
+        /// </summary>
+        private double ResolveCurvaturePreviewDistanceMeters(
+            VehicleState vehicleState,
+            double controlReferenceSpeedMetersPerSecond)
+        {
+            if (CurvaturePreviewSeconds <= 0.0 ||
+                MaximumCurvaturePreviewMeters <= 0.0)
+            {
+                return 0.0;
+            }
+
+            var previewSpeedMetersPerSecond =
+                vehicleState.HasValidVelocityEstimate
+                    ? Math.Abs(
+                        vehicleState.TwistInBody
+                            .VxMetersPerSecond)
+                    : Math.Abs(
+                        controlReferenceSpeedMetersPerSecond);
+
+            return Math.Min(
+                MaximumCurvaturePreviewMeters,
+                previewSpeedMetersPerSecond *
+                CurvaturePreviewSeconds);
+        }
+
+        /// <summary>
+        /// 沿轨迹实际执行点序向前采样专供横向几何前馈使用的曲率。
+        /// </summary>
+        private double ResolveFeedforwardCurvaturePerMeter(
+            TrajectoryProjection projection,
+            double previewDistanceMeters)
+        {
+            var previewArcLengthMeters = Math.Min(
+                _trajectory.TotalLengthMeters,
+                projection.ArcLengthMeters +
+                previewDistanceMeters);
+
+            return _trajectory
+                .SampleAtArcLength(previewArcLengthMeters)
+                .CurvaturePerMeter;
+        }
+
         /// <summary>
         /// 根据终点车头方向上的有符号剩余距离生成只保持原轨迹行驶方向的低速参考。
         /// </summary>
@@ -914,6 +1002,8 @@ namespace MultiWheelC.Control.Execution
             LastProjection = null;
             LastCommand = null;
             LastControlReferenceSpeedMetersPerSecond = null;
+            LastCurvaturePreviewDistanceMeters = null;
+            LastFeedforwardCurvaturePerMeter = null;
             LastCycleTiming = null;
             LastFailureReason = string.Empty;
             LastException = null;

MultiWheelC/Control/Lateral/StanleyLateralController.cs

@@ -104,7 +104,7 @@ namespace MultiWheelC.Control.Lateral
             var feedforwardAngleRadians =
                 travelDirection *
                 Math.Atan(
-                    context.ReferenceCurvaturePerMeter *
+                    context.FeedforwardCurvaturePerMeter *
                     ControlPointRadiusMeters);
 
             // 横向误差生成前后同向的共同转角，使四舵轮车辆平稳靠近轨迹。

MultiWheelC/Experiments/CompositeMotionPlanTests.cs

@@ -332,7 +332,10 @@ namespace MultiWheelC
                     projection.LateralErrorMeters,
                     projection.HeadingErrorRadians,
                     projection.DistanceToTrajectoryMeters,
-                    projection.RemainingDistanceMeters);
+                    projection.RemainingDistanceMeters,
+                    controller.LastCurvaturePreviewDistanceMeters ?? 0.0,
+                    controller.LastFeedforwardCurvaturePerMeter ??
+                    projection.ReferencePoint.CurvaturePerMeter);
             }
 
             var command = controller.LastCommand.Value;

MultiWheelC/Experiments/NewControllerTrackingTests.cs

@@ -365,7 +365,10 @@ namespace MultiWheelC
                     projection.LateralErrorMeters,
                     projection.HeadingErrorRadians,
                     projection.DistanceToTrajectoryMeters,
-                    projection.RemainingDistanceMeters);
+                    projection.RemainingDistanceMeters,
+                    controller.LastCurvaturePreviewDistanceMeters ?? 0.0,
+                    controller.LastFeedforwardCurvaturePerMeter ??
+                    projection.ReferencePoint.CurvaturePerMeter);
             }
 
             var command = controller.LastCommand.Value;
@@ -728,7 +731,10 @@ namespace MultiWheelC
                     projection.LateralErrorMeters,
                     projection.HeadingErrorRadians,
                     projection.DistanceToTrajectoryMeters,
-                    projection.RemainingDistanceMeters);
+                    projection.RemainingDistanceMeters,
+                    controller.LastCurvaturePreviewDistanceMeters ?? 0.0,
+                    controller.LastFeedforwardCurvaturePerMeter ??
+                    projection.ReferencePoint.CurvaturePerMeter);
             }
 
             var command = controller.LastCommand.Value;

MultiWheelC/Experiments/TrackingExperimentRecorder.cs

@@ -49,6 +49,8 @@ namespace MultiWheelC
         public double ControlHeadingErrorRadians;
         public double ControlDistanceToTrajectoryMeters;
         public double ControlRemainingDistanceMeters;
+        public double CurvaturePreviewDistanceMeters;
+        public double FeedforwardCurvaturePerMeter;
 
         // 并列保存Detour速度与轮速解算速度，避免StateBodyVx的数据来源产生歧义。
         public bool HasVelocityDiagnostics;
@@ -150,6 +152,8 @@ namespace MultiWheelC
         private double _controlHeadingErrorRadians;
         private double _controlDistanceToTrajectoryMeters;
         private double _controlRemainingDistanceMeters;
+        private double _curvaturePreviewDistanceMeters;
+        private double _feedforwardCurvaturePerMeter;
         private bool _hasVelocityDiagnostics;
         private double _detourEstimatedBodyVxMetersPerSecond;
         private bool _detourVelocityEstimateValid;
@@ -371,7 +375,9 @@ namespace MultiWheelC
             double lateralErrorMeters,
             double headingErrorRadians,
             double distanceToTrajectoryMeters,
-            double remainingDistanceMeters)
+            double remainingDistanceMeters,
+            double curvaturePreviewDistanceMeters,
+            double feedforwardCurvaturePerMeter)
         {
             lock (_stateSyncRoot)
             {
@@ -387,6 +393,10 @@ namespace MultiWheelC
                     distanceToTrajectoryMeters;
                 _controlRemainingDistanceMeters =
                     remainingDistanceMeters;
+                _curvaturePreviewDistanceMeters =
+                    curvaturePreviewDistanceMeters;
+                _feedforwardCurvaturePerMeter =
+                    feedforwardCurvaturePerMeter;
                 _hasControlReference = true;
             }
         }
@@ -480,6 +490,8 @@ namespace MultiWheelC
                 double controlHeadingErrorRadians;
                 double controlDistanceToTrajectoryMeters;
                 double controlRemainingDistanceMeters;
+                double curvaturePreviewDistanceMeters;
+                double feedforwardCurvaturePerMeter;
                 bool hasVelocityDiagnostics;
                 double detourEstimatedBodyVxMetersPerSecond;
                 bool detourVelocityEstimateValid;
@@ -540,6 +552,10 @@ namespace MultiWheelC
                         _controlDistanceToTrajectoryMeters;
                     controlRemainingDistanceMeters =
                         _controlRemainingDistanceMeters;
+                    curvaturePreviewDistanceMeters =
+                        _curvaturePreviewDistanceMeters;
+                    feedforwardCurvaturePerMeter =
+                        _feedforwardCurvaturePerMeter;
                     hasVelocityDiagnostics =
                         _hasVelocityDiagnostics;
                     detourEstimatedBodyVxMetersPerSecond =
@@ -587,6 +603,10 @@ namespace MultiWheelC
                         controlDistanceToTrajectoryMeters,
                     ControlRemainingDistanceMeters =
                         controlRemainingDistanceMeters,
+                    CurvaturePreviewDistanceMeters =
+                        curvaturePreviewDistanceMeters,
+                    FeedforwardCurvaturePerMeter =
+                        feedforwardCurvaturePerMeter,
                     HasVelocityDiagnostics =
                         hasVelocityDiagnostics,
                     DetourEstimatedBodyVxMetersPerSecond =
@@ -802,6 +822,8 @@ namespace MultiWheelC
                     "ControlHeadingErrorRadians," +
                     "ControlDistanceToTrajectoryMeters," +
                     "ControlRemainingDistanceMeters," +
+                    "CurvaturePreviewDistanceMeters," +
+                    "FeedforwardCurvaturePerMeter," +
                     "HasVelocityDiagnostics," +
                     "DetourEstimatedBodyVxMetersPerSecond," +
                     "DetourVelocityEstimateValid," +
@@ -907,6 +929,12 @@ namespace MultiWheelC
                         FormatOptional(
                             sample.HasControlReference,
                             sample.ControlRemainingDistanceMeters),
+                        FormatOptional(
+                            sample.HasControlReference,
+                            sample.CurvaturePreviewDistanceMeters),
+                        FormatOptional(
+                            sample.HasControlReference,
+                            sample.FeedforwardCurvaturePerMeter),
                         sample.HasVelocityDiagnostics
                             ? "1"
                             : "0",

MultiWheelC/Movements/TrajectoryTrackingMovement.cs

@@ -62,6 +62,16 @@ namespace MultiWheelC
         /// </summary>
         public bool? StanleyUsesActualSpeed;
 
+        /// <summary>
+        /// 获取或设置本次动作的Stanley曲率前馈预瞄时间覆盖值，单位为s，0为关闭；为空时读取车辆配置。
+        /// </summary>
+        public double? StanleyCurvaturePreviewSeconds;
+
+        /// <summary>
+        /// 获取或设置本次动作的Stanley曲率前馈最大预瞄距离覆盖值，单位为m；为空时读取车辆配置。
+        /// </summary>
+        public double? StanleyMaximumCurvaturePreviewMeters;
+
         /// <summary>
         /// 获取或设置本次动作的Stanley横向修正上限覆盖值，单位为rad；为空时读取车辆配置。
         /// </summary>
@@ -180,6 +190,12 @@ namespace MultiWheelC
             var stanleyUsesActualSpeed =
                 StanleyUsesActualSpeed ??
                 config.ParkingStanleyUseActualSpeed;
+            var stanleyCurvaturePreviewSeconds =
+                StanleyCurvaturePreviewSeconds ??
+                config.ParkingStanleyCurvaturePreviewSeconds;
+            var stanleyMaximumCurvaturePreviewMeters =
+                StanleyMaximumCurvaturePreviewMeters ??
+                config.ParkingStanleyMaximumCurvaturePreviewMeters;
             var maximumCrossTrackCorrectionRadians =
                 MaximumCrossTrackCorrectionRadians ??
                 AngleMath.DegreesToRadians(
@@ -332,7 +348,9 @@ namespace MultiWheelC
                 terminalBrakingPreviewMeters,
                 terminalApproachDistanceMeters,
                 terminalApproachGainPerSecond,
-                maximumTerminalApproachSpeedMetersPerSecond);
+                maximumTerminalApproachSpeedMetersPerSecond,
+                stanleyCurvaturePreviewSeconds,
+                stanleyMaximumCurvaturePreviewMeters);
 
             var clock = Stopwatch.StartNew();
             var previousCycleSeconds =

docs/记录.txt

@@ -37,23 +37,6 @@ GcpCommandExecutor.cs
 ParkingGeometricController.cs
 
 
-轨迹投影没有进度连续性
-[TrajectoryProjector.cs (line 33)](/D:/Users/Desktop/入职培训/停车机器人/MyParking/MultiWheelC/Trajectory/TrajectoryProjector.cs:33) 每个控制周期都会遍历整条轨迹并选择全局最近线段。
-当前4 m直线、圆弧、普通 S 曲线一般没有问题；但以后遇到自交、回环、相邻平行路径或 Detour 跳变时，投影可能突然跳到另一段轨迹。
-建议在开始复杂曲线测试前增加：
-上一次投影线段索引
-有限前向搜索窗口
-少量允许回退范围
-现阶段测试直线不需要马上改。
-
-通用轨迹动作没有起点航向保护
-当前直线测试以实时 Detour 位姿作为起点，因此起点位置和航向天然匹配，没有问题。
-但 [TrajectoryTrackingMovement.cs (line 119)](/D:/Users/Desktop/入职培训/停车机器人/MyParking/MultiWheelC/Movements/TrajectoryTrackingMovement.cs:119) 本身允许传入任意世界坐标轨迹。如果将来传入的轨迹起点航向和车辆实际航向差别很大，车辆会直接边走边纠正。
-后续至少应选择一种：
-规划层保证起点位姿和实际车辆一致；
-控制器检查初始航向误差，超限则拒绝启动；
-增加起步航向对齐状态。
-第一阶段建议采用第二种，简单、安全。
 
 把前后GCP转角分解成两个模态：
 共同转角 = (前GCP转角 + 后GCP转角) / 2