diff --git a/MedullaAdapter/DiverCartDefinition.cs b/MedullaAdapter/DiverCartDefinition.cs index 0e09751..2f88f05 100644 --- a/MedullaAdapter/DiverCartDefinition.cs +++ b/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 = "轮速诊断记录已启用")] diff --git a/MedullaAdapter/MotorRoutine.cs b/MedullaAdapter/MotorRoutine.cs index 9c57c77..bc0165f 100644 --- a/MedullaAdapter/MotorRoutine.cs +++ b/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 { + 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个驱动电机速度。 + /// + /// 根据四个舵轮的目标角速度前馈和实际角度反馈修正八个驱动电机速度。 + /// 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; } + /// + /// 根据四个机械目标舵角的实际变化率计算本周期差速轮线速度前馈。 + /// + 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; + } + + /// + /// 将单个机械目标舵角变化率转换为带限幅的左右轮差速线速度前馈。 + /// + 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); + } + + /// + /// 清除差速转舵前馈历史和监控输出，避免恢复控制时使用过期目标角。 + /// + 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; + } + + /// + /// 判断单精度参数是否可安全参与底盘控制计算。 + /// + private static bool IsFinite(float value) + { + return !float.IsNaN(value) && + !float.IsInfinity(value); + } + // M层单车限速：按照加速度和减速度平滑更新实际下发速度上限。 private void UpdateSendSpeedLimit() { diff --git a/MedullaAdapter/WheelSpeedDiagnosticLogger.cs b/MedullaAdapter/WheelSpeedDiagnosticLogger.cs index b6b8218..d55975f 100644 --- a/MedullaAdapter/WheelSpeedDiagnosticLogger.cs +++ b/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), diff --git a/MedullaAdapter/build/Medulla/plugins/CommonUsage.dll b/MedullaAdapter/build/Medulla/plugins/CommonUsage.dll index 92a453a..128c1e7 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 6bb0299..059ddf8 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 a93d960..51a851a 100644 Binary files a/MedullaAdapter/build/Medulla/plugins/MedullaAdapter.pdb and b/MedullaAdapter/build/Medulla/plugins/MedullaAdapter.pdb differ diff --git a/MultiWheelC/Configuration/PilotConfig.ParkingControl.cs b/MultiWheelC/Configuration/PilotConfig.ParkingControl.cs index bfbaef3..1cd5fce 100644 --- a/MultiWheelC/Configuration/PilotConfig.ParkingControl.cs +++ b/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; diff --git a/MultiWheelC/Control/Abstractions/PathTrackingContext.cs b/MultiWheelC/Control/Abstractions/PathTrackingContext.cs index 8495b80..0f5ef4f 100644 --- a/MultiWheelC/Control/Abstractions/PathTrackingContext.cs +++ b/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; + /// + /// 获取沿轨迹点序适量预瞄后专供几何前馈使用的参考曲率，单位为1/m，左弯为正。 + /// + public double FeedforwardCurvaturePerMeter { get; } + /// /// 获取参考轨迹相对车辆的有符号横向误差，单位为m，轨迹在车辆左侧时为正。 /// diff --git a/MultiWheelC/Control/Execution/ParkingGeometricController.cs b/MultiWheelC/Control/Execution/ParkingGeometricController.cs index 015b7d1..da4ca60 100644 --- a/MultiWheelC/Control/Execution/ParkingGeometricController.cs +++ b/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; } /// @@ -218,6 +229,16 @@ namespace MultiWheelC.Control.Execution /// public double MaximumTerminalApproachSpeedMetersPerSecond { get; } + /// + /// 获取按照车辆纵向速度换算曲率前馈预瞄距离的预测时间，单位为s，0表示关闭。 + /// + public double CurvaturePreviewSeconds { get; } + + /// + /// 获取曲率前馈沿轨迹点序允许预瞄的最大距离，单位为m。 + /// + public double MaximumCurvaturePreviewMeters { get; } + /// /// 获取控制器当前是否持有并正在执行一条轨迹。 /// @@ -259,6 +280,16 @@ namespace MultiWheelC.Control.Execution /// public double? LastControlReferenceSpeedMetersPerSecond { get; private set; } + /// + /// 获取最近控制周期实际采用的曲率前馈预瞄距离，单位为m。 + /// + public double? LastCurvaturePreviewDistanceMeters { get; private set; } + + /// + /// 获取最近控制周期沿轨迹预瞄后交给横向控制器的曲率，单位为1/m。 + /// + public double? LastFeedforwardCurvaturePerMeter { get; private set; } + /// /// 获取最近一次控制周期的分阶段耗时诊断；尚未执行时为空。 /// @@ -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); } + /// + /// 根据有效实际纵向速度或控制参考速度计算带上限的曲率前馈预瞄距离。 + /// + 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); + } + + /// + /// 沿轨迹实际执行点序向前采样专供横向几何前馈使用的曲率。 + /// + private double ResolveFeedforwardCurvaturePerMeter( + TrajectoryProjection projection, + double previewDistanceMeters) + { + var previewArcLengthMeters = Math.Min( + _trajectory.TotalLengthMeters, + projection.ArcLengthMeters + + previewDistanceMeters); + + return _trajectory + .SampleAtArcLength(previewArcLengthMeters) + .CurvaturePerMeter; + } + /// /// 根据终点车头方向上的有符号剩余距离生成只保持原轨迹行驶方向的低速参考。 /// @@ -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; diff --git a/MultiWheelC/Control/Lateral/StanleyLateralController.cs b/MultiWheelC/Control/Lateral/StanleyLateralController.cs index da60f64..c45ba25 100644 --- a/MultiWheelC/Control/Lateral/StanleyLateralController.cs +++ b/MultiWheelC/Control/Lateral/StanleyLateralController.cs @@ -104,7 +104,7 @@ namespace MultiWheelC.Control.Lateral var feedforwardAngleRadians = travelDirection * Math.Atan( - context.ReferenceCurvaturePerMeter * + context.FeedforwardCurvaturePerMeter * ControlPointRadiusMeters); // 横向误差生成前后同向的共同转角，使四舵轮车辆平稳靠近轨迹。 diff --git a/MultiWheelC/Experiments/CompositeMotionPlanTests.cs b/MultiWheelC/Experiments/CompositeMotionPlanTests.cs index 2734fe5..c9f6914 100644 --- a/MultiWheelC/Experiments/CompositeMotionPlanTests.cs +++ b/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; diff --git a/MultiWheelC/Experiments/NewControllerTrackingTests.cs b/MultiWheelC/Experiments/NewControllerTrackingTests.cs index 62d320a..fcdb842 100644 --- a/MultiWheelC/Experiments/NewControllerTrackingTests.cs +++ b/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; diff --git a/MultiWheelC/Experiments/TrackingExperimentRecorder.cs b/MultiWheelC/Experiments/TrackingExperimentRecorder.cs index 55a3c5e..564e5c0 100644 --- a/MultiWheelC/Experiments/TrackingExperimentRecorder.cs +++ b/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", diff --git a/MultiWheelC/Movements/TrajectoryTrackingMovement.cs b/MultiWheelC/Movements/TrajectoryTrackingMovement.cs index d7fd0b0..a9e40fe 100644 --- a/MultiWheelC/Movements/TrajectoryTrackingMovement.cs +++ b/MultiWheelC/Movements/TrajectoryTrackingMovement.cs @@ -62,6 +62,16 @@ namespace MultiWheelC /// public bool? StanleyUsesActualSpeed; + /// + /// 获取或设置本次动作的Stanley曲率前馈预瞄时间覆盖值，单位为s，0为关闭；为空时读取车辆配置。 + /// + public double? StanleyCurvaturePreviewSeconds; + + /// + /// 获取或设置本次动作的Stanley曲率前馈最大预瞄距离覆盖值，单位为m；为空时读取车辆配置。 + /// + public double? StanleyMaximumCurvaturePreviewMeters; + /// /// 获取或设置本次动作的Stanley横向修正上限覆盖值，单位为rad；为空时读取车辆配置。 /// @@ -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 = diff --git a/MultiWheelC/build/Clumsy/CommonUsage.dll b/MultiWheelC/build/Clumsy/CommonUsage.dll index 92a453a..128c1e7 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 4dcd48e..11b41a8 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 71e5e30..8ab6e1c 100644 Binary files a/MultiWheelC/build/Clumsy/MultiWheelC.pdb and b/MultiWheelC/build/Clumsy/MultiWheelC.pdb differ diff --git a/output/C/CommonUsage.dll b/output/C/CommonUsage.dll index 92a453a..128c1e7 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 4dcd48e..11b41a8 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 71e5e30..8ab6e1c 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 92a453a..128c1e7 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 6bb0299..059ddf8 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 a93d960..51a851a 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 92a453a..128c1e7 100644 Binary files a/ref/CommonUsage.dll and b/ref/CommonUsage.dll differ