From 19b1e49189ac3511772cc6c6c30359b9fc9c2ba1 Mon Sep 17 00:00:00 2001 From: shenyuxiang Date: Thu, 6 Aug 2026 15:14:12 +0800 Subject: [PATCH] =?UTF-8?q?=E6=89=93=E9=80=9A=E6=96=B0=E7=89=88Stanley?= =?UTF-8?q?=E8=BD=A8=E8=BF=B9=E8=B7=9F=E8=B8=AA=E9=97=AD=E7=8E=AF=E5=B9=B6?= =?UTF-8?q?=E8=A1=A5=E5=85=85=E5=AE=9E=E9=AA=8C=E6=B5=8B=E8=AF=95=E4=B8=8E?= =?UTF-8?q?=E6=95=B0=E6=8D=AE=E5=88=86=E6=9E=90=E8=84=9A=E6=9C=AC?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Co-authored-by: Cursor --- .../Abstractions/ILateralController.cs | 20 +- .../Abstractions/ILongitudinalController.cs | 20 +- .../Abstractions/LateralControlCommand.cs | 52 +- .../Abstractions/PathTrackingContext.cs | 127 +++- .../Allocation/AckermannGcpAllocator.cs | 134 ++++- .../Control/Allocation/GcpMotionCommand.cs | 72 ++- MultiWheelC/Control/Common/PidController.cs | 307 ++++++++++ .../Control/Execution/GcpCommandExecutor.cs | 178 +++++- .../Execution/ParkingGeometricController.cs | 552 +++++++++++++++++- .../Lateral/StanleyLateralController.cs | 223 +++++++ .../Longitudinal/PidLongitudinalController.cs | 182 ++++++ .../Experiments/NewControllerTrackingTests.cs | 297 ++++++++++ .../Experiments/TestTrajectoryFactory.cs | 163 ++++++ .../Experiments/TrackingExperimentRecorder.cs | 249 +++++++- .../Movements/TrajectoryTrackingMovement.cs | 286 +++++++++ MultiWheelC/Trajectory/Trajectory2D.cs | 88 +++ Shared/Chassis/MultiWheelChassisAdapter.cs | 47 ++ .../plot_new_controller_experiment.py | 487 +++++++++++++++ .../新版控制器轨迹测试处理/requirements.txt | 3 + .../compare_state_estimation.py | 0 .../plot_angular_command.py | 0 .../plot_speed_response.py | 0 .../plot_tracking_errors.py | 0 .../plot_trajectory_comparison.py | 0 .../requirements.txt | 0 .../run_all_plots.py | 0 docs/记录.txt | 17 +- 27 files changed, 3482 insertions(+), 22 deletions(-) create mode 100644 MultiWheelC/Control/Common/PidController.cs create mode 100644 MultiWheelC/Experiments/NewControllerTrackingTests.cs create mode 100644 MultiWheelC/Experiments/TestTrajectoryFactory.cs create mode 100644 MultiWheelC/Movements/TrajectoryTrackingMovement.cs create mode 100644 data_process/新版控制器轨迹测试处理/plot_new_controller_experiment.py create mode 100644 data_process/新版控制器轨迹测试处理/requirements.txt rename data_process/{轨迹测试处理 => 旧版控制器轨迹测试处理}/compare_state_estimation.py (100%) rename data_process/{轨迹测试处理 => 旧版控制器轨迹测试处理}/plot_angular_command.py (100%) rename data_process/{轨迹测试处理 => 旧版控制器轨迹测试处理}/plot_speed_response.py (100%) rename data_process/{轨迹测试处理 => 旧版控制器轨迹测试处理}/plot_tracking_errors.py (100%) rename data_process/{轨迹测试处理 => 旧版控制器轨迹测试处理}/plot_trajectory_comparison.py (100%) rename data_process/{轨迹测试处理 => 旧版控制器轨迹测试处理}/requirements.txt (100%) rename data_process/{轨迹测试处理 => 旧版控制器轨迹测试处理}/run_all_plots.py (100%) diff --git a/MultiWheelC/Control/Abstractions/ILateralController.cs b/MultiWheelC/Control/Abstractions/ILateralController.cs index 18d7fbe..25fbcaf 100644 --- a/MultiWheelC/Control/Abstractions/ILateralController.cs +++ b/MultiWheelC/Control/Abstractions/ILateralController.cs @@ -1 +1,19 @@ -// 所有横向控制器的统一接口 +namespace MultiWheelC.Control.Abstractions +{ + /// + /// 定义Stanley、LQR和MPC等车体中心横向控制器的统一接口。 + /// + public interface ILateralController + { + /// + /// 根据本周期车辆状态和轨迹误差计算车体中心目标曲率。 + /// + LateralControlCommand Compute( + PathTrackingContext context); + + /// + /// 清除控制器跨周期状态，以便开始新轨迹或异常恢复后重新运行。 + /// + void Reset(); + } +} diff --git a/MultiWheelC/Control/Abstractions/ILongitudinalController.cs b/MultiWheelC/Control/Abstractions/ILongitudinalController.cs index 20d7e3e..810eda3 100644 --- a/MultiWheelC/Control/Abstractions/ILongitudinalController.cs +++ b/MultiWheelC/Control/Abstractions/ILongitudinalController.cs @@ -1 +1,19 @@ -// 统一纵向控制接口 +namespace MultiWheelC.Control.Abstractions +{ + /// + /// 定义根据参考速度和实际纵向速度生成底盘命令速度的统一接口。 + /// + public interface ILongitudinalController + { + /// + /// 根据本周期速度目标、速度反馈和时间间隔计算有符号底盘命令速度。 + /// + double ComputeSpeedMetersPerSecond( + PathTrackingContext context); + + /// + /// 清除积分、历史误差和其他跨周期状态，以便安全开始新的控制过程。 + /// + void Reset(); + } +} diff --git a/MultiWheelC/Control/Abstractions/LateralControlCommand.cs b/MultiWheelC/Control/Abstractions/LateralControlCommand.cs index e123283..2198a8d 100644 --- a/MultiWheelC/Control/Abstractions/LateralControlCommand.cs +++ b/MultiWheelC/Control/Abstractions/LateralControlCommand.cs @@ -1 +1,51 @@ -// 横向控制器的输出 +using System; + +namespace MultiWheelC.Control.Abstractions +{ + /// + /// 表示横向控制器生成的车体中心目标曲率命令。 + /// + public readonly struct LateralControlCommand + { + /// + /// 创建统一使用SI单位和左转为正约定的横向控制命令。 + /// + public LateralControlCommand( + double targetCurvaturePerMeter) + { + EnsureFinite( + targetCurvaturePerMeter, + nameof(targetCurvaturePerMeter)); + + TargetCurvaturePerMeter = + targetCurvaturePerMeter; + } + + /// + /// 获取车体中心目标轨迹曲率，单位为1/m，左转为正、右转为负。 + /// + public double TargetCurvaturePerMeter { get; } + + /// + /// 创建保持直线行驶的零曲率命令。 + /// + public static LateralControlCommand Straight => + new LateralControlCommand(0.0); + + /// + /// 检查横向曲率命令是否为有限值。 + /// + private static void EnsureFinite( + double value, + string parameterName) + { + if (double.IsNaN(value) || + double.IsInfinity(value)) + { + throw new ArgumentOutOfRangeException( + parameterName, + "横向控制目标曲率必须是有限值。"); + } + } + } +} diff --git a/MultiWheelC/Control/Abstractions/PathTrackingContext.cs b/MultiWheelC/Control/Abstractions/PathTrackingContext.cs index 7ffeb45..578468a 100644 --- a/MultiWheelC/Control/Abstractions/PathTrackingContext.cs +++ b/MultiWheelC/Control/Abstractions/PathTrackingContext.cs @@ -1 +1,126 @@ -// 保存一次控制周期需要的完整输入 +using System; +using MultiWheelC.StateEstimation; +using MultiWheelC.Trajectory; + +namespace MultiWheelC.Control.Abstractions +{ + /// + /// 保存一次轨迹跟踪控制周期使用的车辆状态、轨迹投影和真实时间间隔。 + /// + public readonly struct PathTrackingContext + { + /// + /// 创建横向和纵向控制器共享的只读控制输入快照。 + /// + public PathTrackingContext( + VehicleState vehicleState, + TrajectoryProjection projection, + double referenceSpeedMetersPerSecond, + double deltaTimeSeconds) + { + EnsureFinite( + referenceSpeedMetersPerSecond, + nameof(referenceSpeedMetersPerSecond)); + EnsureFinitePositive( + deltaTimeSeconds, + nameof(deltaTimeSeconds)); + + VehicleState = vehicleState; + Projection = projection; + ReferenceSpeedMetersPerSecond = + referenceSpeedMetersPerSecond; + DeltaTimeSeconds = deltaTimeSeconds; + } + + /// + /// 获取本周期经过校验的实际车辆位姿和速度状态。 + /// + public VehicleState VehicleState { get; } + + /// + /// 获取实际车体中心投影到参考轨迹后得到的参考状态和跟踪误差。 + /// + public TrajectoryProjection Projection { get; } + + /// + /// 获取本次控制计算距离上次计算的真实时间间隔，单位为s。 + /// + public double DeltaTimeSeconds { get; } + + /// + /// 获取轨迹投影点要求的有符号参考速度，单位为m/s。 + /// + public double ReferenceSpeedMetersPerSecond { get; } + + /// + /// 获取车辆在车体X轴方向上的实际纵向速度，单位为m/s。 + /// + public double ActualLongitudinalSpeedMetersPerSecond => + VehicleState.TwistInBody + .VxMetersPerSecond; + + /// + /// 获取轨迹投影点的参考曲率，单位为1/m，左转为正。 + /// + public double ReferenceCurvaturePerMeter => + Projection.ReferencePoint + .CurvaturePerMeter; + + /// + /// 获取参考轨迹相对车辆的有符号横向误差，单位为m，轨迹在车辆左侧时为正。 + /// + public double LateralErrorMeters => + Projection.LateralErrorMeters; + + /// + /// 获取参考航向减实际车体航向的最短角差，单位为rad，逆时针为正。 + /// + public double HeadingErrorRadians => + Projection.HeadingErrorRadians; + + /// + /// 获取当前投影位置沿参考轨迹到终点的剩余距离，单位为m。 + /// + public double RemainingDistanceMeters => + Projection.RemainingDistanceMeters; + + /// + /// 获取实际速度是否已经由至少两个连续有效定位样本估算得到。 + /// + public bool HasValidVelocityEstimate => + VehicleState.HasValidVelocityEstimate; + + /// + /// 检查控制周期是否为正有限值。 + /// + 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 EnsureFinite( + double value, + string parameterName) + { + if (double.IsNaN(value) || + double.IsInfinity(value)) + { + throw new ArgumentOutOfRangeException( + parameterName, + "轨迹跟踪参考速度必须是有限值。"); + } + } + } +} diff --git a/MultiWheelC/Control/Allocation/AckermannGcpAllocator.cs b/MultiWheelC/Control/Allocation/AckermannGcpAllocator.cs index c5f2291..bf79863 100644 --- a/MultiWheelC/Control/Allocation/AckermannGcpAllocator.cs +++ b/MultiWheelC/Control/Allocation/AckermannGcpAllocator.cs @@ -1 +1,133 @@ -// 车体中心命令曲率转换成前后GCP方向 +using System; +using MultiWheelC.Control.Abstractions; + +namespace MultiWheelC.Control.Allocation +{ + /// + /// 将车体中心目标曲率按对称前后转向策略转换为旧版底盘的前后GCP方向。 + /// + public sealed class AckermannGcpAllocator + { + /// + /// 创建使用指定GCP半间距和最大GCP转角的对称转向分配器。 + /// + public AckermannGcpAllocator( + double controlPointRadiusMeters, + double maximumGcpAngleRadians) + { + EnsureFinitePositive( + controlPointRadiusMeters, + nameof(controlPointRadiusMeters)); + EnsureFinitePositive( + maximumGcpAngleRadians, + nameof(maximumGcpAngleRadians)); + + if (maximumGcpAngleRadians >= + Math.PI / 2.0) + { + throw new ArgumentOutOfRangeException( + nameof(maximumGcpAngleRadians), + "最大GCP转角必须小于π/2，避免曲率换算出现奇异值。"); + } + + ControlPointRadiusMeters = + controlPointRadiusMeters; + MaximumGcpAngleRadians = + maximumGcpAngleRadians; + } + + /// + /// 获取车体中心到前、后GCP的距离，单位为m。 + /// + public double ControlPointRadiusMeters { get; } + + /// + /// 获取前后GCP允许的最大转角绝对值，单位为rad。 + /// + public double MaximumGcpAngleRadians { get; } + + /// + /// 获取当前GCP几何和转角限制允许的最大车体中心曲率，单位为1/m。 + /// + public double MaximumCurvaturePerMeter => + Math.Tan(MaximumGcpAngleRadians) / + ControlPointRadiusMeters; + + /// + /// 将纵向命令速度和车体中心目标曲率分配为前后GCP运动命令。 + /// + public GcpMotionCommand Allocate( + double speedMetersPerSecond, + LateralControlCommand lateralCommand) + { + EnsureFinite( + speedMetersPerSecond, + nameof(speedMetersPerSecond)); + + var limitedCurvaturePerMeter = + Clamp( + lateralCommand + .TargetCurvaturePerMeter, + -MaximumCurvaturePerMeter, + MaximumCurvaturePerMeter); + + var frontAngleRadians = + Math.Atan( + limitedCurvaturePerMeter * + ControlPointRadiusMeters); + var rearAngleRadians = + -frontAngleRadians; + + return new GcpMotionCommand( + speedMetersPerSecond, + frontAngleRadians, + rearAngleRadians); + } + + /// + /// 将数值限制在指定闭区间内。 + /// + private static double Clamp( + double value, + double minimum, + double maximum) + { + return Math.Max( + minimum, + Math.Min(maximum, 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 8a5c726..f36ce0e 100644 --- a/MultiWheelC/Control/Allocation/GcpMotionCommand.cs +++ b/MultiWheelC/Control/Allocation/GcpMotionCommand.cs @@ -1,9 +1,67 @@ -// 表示发送给底盘前的中间命令 -// public readonly struct GcpMotionCommand -// { -// public double SpeedMetersPerSecond { get; } +using System; -// public double FrontAngleRadians { get; } +namespace MultiWheelC.Control.Allocation +{ + /// + /// 表示发送给旧版多舵轮四轮解算前的有符号速度和前后GCP角度命令。 + /// + public readonly struct GcpMotionCommand + { + /// + /// 创建统一使用m/s和rad的前后几何控制点运动命令。 + /// + public GcpMotionCommand( + double speedMetersPerSecond, + double frontAngleRadians, + double rearAngleRadians) + { + EnsureFinite( + speedMetersPerSecond, + nameof(speedMetersPerSecond)); + EnsureFinite( + frontAngleRadians, + nameof(frontAngleRadians)); + EnsureFinite( + rearAngleRadians, + nameof(rearAngleRadians)); -// public double RearAngleRadians { get; } -// } \ No newline at end of file + SpeedMetersPerSecond = + speedMetersPerSecond; + FrontAngleRadians = + frontAngleRadians; + RearAngleRadians = + rearAngleRadians; + } + + /// + /// 获取准备交给底盘的有符号纵向速度，单位为m/s，正值表示前进。 + /// + public double SpeedMetersPerSecond { get; } + + /// + /// 获取前几何控制点相对车体X轴的目标方向，单位为rad，逆时针为正。 + /// + public double FrontAngleRadians { get; } + + /// + /// 获取后几何控制点相对车体X轴的目标方向，单位为rad，逆时针为正。 + /// + 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/Common/PidController.cs b/MultiWheelC/Control/Common/PidController.cs new file mode 100644 index 0000000..9d81320 --- /dev/null +++ b/MultiWheelC/Control/Common/PidController.cs @@ -0,0 +1,307 @@ +using System; + +namespace MultiWheelC.Control.Common +{ + /// + /// 使用真实控制周期计算带积分限幅、输出限幅和抗饱和的通用有状态PID输出。 + /// + public sealed class PidController + { + private double _integralState; + private double _previousError; + private double _previousMeasurement; + private bool _hasPreviousSample; + + /// + /// 创建具有指定增益、积分输出限制和微分形式的PID控制器。 + /// + public PidController( + double proportionalGain, + double integralGainPerSecond, + double derivativeGainSeconds, + double maximumIntegralOutput, + bool derivativeOnMeasurement = true) + { + EnsureFiniteNonNegative( + proportionalGain, + nameof(proportionalGain)); + EnsureFiniteNonNegative( + integralGainPerSecond, + nameof(integralGainPerSecond)); + EnsureFiniteNonNegative( + derivativeGainSeconds, + nameof(derivativeGainSeconds)); + EnsureFiniteNonNegative( + maximumIntegralOutput, + nameof(maximumIntegralOutput)); + + ProportionalGain = proportionalGain; + IntegralGainPerSecond = integralGainPerSecond; + DerivativeGainSeconds = derivativeGainSeconds; + MaximumIntegralOutput = maximumIntegralOutput; + DerivativeOnMeasurement = derivativeOnMeasurement; + } + + /// + /// 获取比例增益。 + /// + public double ProportionalGain { get; } + + /// + /// 获取积分增益，单位为1/s。 + /// + public double IntegralGainPerSecond { get; } + + /// + /// 获取微分增益，单位为s。 + /// + public double DerivativeGainSeconds { get; } + + /// + /// 获取积分项允许产生的最大输出绝对值。 + /// + public double MaximumIntegralOutput { get; } + + /// + /// 获取微分项是否作用于测量值，以避免设定值变化产生微分冲击。 + /// + public bool DerivativeOnMeasurement { get; } + + /// + /// 获取最近一次设定值减测量值的误差。 + /// + public double LastError { get; private set; } + + /// + /// 获取最近一次比例项输出。 + /// + public double LastProportionalOutput { get; private set; } + + /// + /// 获取最近一次积分项输出。 + /// + public double LastIntegralOutput { get; private set; } + + /// + /// 获取最近一次微分项输出。 + /// + public double LastDerivativeOutput { get; private set; } + + /// + /// 获取最近一次经过输出范围限制后的PID输出。 + /// + public double LastOutput { get; private set; } + + /// + /// 根据设定值、测量值、真实时间间隔和本周期输出范围更新PID。 + /// + public double Update( + double setPoint, + double measurement, + double deltaTimeSeconds, + double minimumOutput, + double maximumOutput) + { + EnsureFinite(setPoint, nameof(setPoint)); + EnsureFinite(measurement, nameof(measurement)); + EnsureFinitePositive( + deltaTimeSeconds, + nameof(deltaTimeSeconds)); + EnsureFinite(minimumOutput, nameof(minimumOutput)); + EnsureFinite(maximumOutput, nameof(maximumOutput)); + + if (minimumOutput > maximumOutput) + { + throw new ArgumentOutOfRangeException( + nameof(minimumOutput), + "PID最小输出不能大于最大输出。"); + } + + var error = setPoint - measurement; + var proportionalOutput = + ProportionalGain * error; + var derivativeOutput = CalculateDerivativeOutput( + error, + measurement, + deltaTimeSeconds); + + var candidateIntegralState = + _integralState + + error * deltaTimeSeconds; + var integralOutput = CalculateIntegralOutput( + candidateIntegralState); + + // 同步截断积分状态本身，避免积分输出虽已限幅、内部状态仍继续增长。 + candidateIntegralState = + IntegralGainPerSecond > 0.0 && + MaximumIntegralOutput > 0.0 + ? integralOutput / + IntegralGainPerSecond + : 0.0; + + var unlimitedOutput = + proportionalOutput + + integralOutput + + derivativeOutput; + var output = Clamp( + unlimitedOutput, + minimumOutput, + maximumOutput); + + // 根据实际允许输出反算积分项，避免执行器饱和期间继续积累误差。 + if (IntegralGainPerSecond > 0.0 && + output != unlimitedOutput) + { + integralOutput = Clamp( + output - + proportionalOutput - + derivativeOutput, + -MaximumIntegralOutput, + MaximumIntegralOutput); + candidateIntegralState = + integralOutput / + IntegralGainPerSecond; + } + + _integralState = + IntegralGainPerSecond > 0.0 && + MaximumIntegralOutput > 0.0 + ? candidateIntegralState + : 0.0; + _previousError = error; + _previousMeasurement = measurement; + _hasPreviousSample = true; + + LastError = error; + LastProportionalOutput = proportionalOutput; + LastIntegralOutput = integralOutput; + LastDerivativeOutput = derivativeOutput; + LastOutput = output; + + return output; + } + + /// + /// 清除积分、历史采样和最近一次PID诊断输出。 + /// + public void Reset() + { + _integralState = 0.0; + _previousError = 0.0; + _previousMeasurement = 0.0; + _hasPreviousSample = false; + LastError = 0.0; + LastProportionalOutput = 0.0; + LastIntegralOutput = 0.0; + LastDerivativeOutput = 0.0; + LastOutput = 0.0; + } + + /// + /// 使用测量值微分或误差微分计算本周期微分项输出。 + /// + private double CalculateDerivativeOutput( + double error, + double measurement, + double deltaTimeSeconds) + { + if (!_hasPreviousSample || + DerivativeGainSeconds <= 0.0) + { + return 0.0; + } + + if (DerivativeOnMeasurement) + { + return -DerivativeGainSeconds * + (measurement - _previousMeasurement) / + deltaTimeSeconds; + } + + return DerivativeGainSeconds * + (error - _previousError) / + deltaTimeSeconds; + } + + /// + /// 根据积分状态计算经过绝对值限制的积分项输出。 + /// + private double CalculateIntegralOutput( + double integralState) + { + if (IntegralGainPerSecond <= 0.0 || + MaximumIntegralOutput <= 0.0) + { + return 0.0; + } + + return Clamp( + IntegralGainPerSecond * integralState, + -MaximumIntegralOutput, + MaximumIntegralOutput); + } + + /// + /// 将数值限制在指定闭区间内。 + /// + private static double Clamp( + double value, + double minimum, + double maximum) + { + return Math.Max( + minimum, + Math.Min(maximum, value)); + } + + /// + /// 检查参数是否为正有限值。 + /// + private static void EnsureFinitePositive( + double value, + string parameterName) + { + EnsureFinite(value, parameterName); + + if (value <= 0.0) + { + throw new ArgumentOutOfRangeException( + parameterName, + "PID时间间隔必须是正有限值。"); + } + } + + /// + /// 检查参数是否为非负有限值。 + /// + private static void EnsureFiniteNonNegative( + double value, + string parameterName) + { + EnsureFinite(value, parameterName); + + if (value < 0.0) + { + throw new ArgumentOutOfRangeException( + parameterName, + "PID增益和积分输出限幅必须是非负有限值。"); + } + } + + /// + /// 检查参数是否为有限值。 + /// + private static void EnsureFinite( + double value, + string parameterName) + { + if (double.IsNaN(value) || + double.IsInfinity(value)) + { + throw new ArgumentOutOfRangeException( + parameterName, + "PID参数和输入必须是有限值。"); + } + } + } +} diff --git a/MultiWheelC/Control/Execution/GcpCommandExecutor.cs b/MultiWheelC/Control/Execution/GcpCommandExecutor.cs index 92633be..ee21a84 100644 --- a/MultiWheelC/Control/Execution/GcpCommandExecutor.cs +++ b/MultiWheelC/Control/Execution/GcpCommandExecutor.cs @@ -1 +1,177 @@ -// 负责把纯数学命令转换成现有底盘调用 \ No newline at end of file +using System; +using MultiWheelC.Control.Allocation; +using MyParking.Shared; + +namespace MultiWheelC.Control.Execution +{ + /// + /// 将SI单位的GCP运动命令安全转换为现有多舵轮底盘调用。 + /// + public sealed class GcpCommandExecutor + { + private const double StopSpeedDeadbandMetersPerSecond = + 1e-6; + + private readonly MultiWheelChassisAdapter _chassisAdapter; + private double _lastFrontAngleRadians; + private double _lastRearAngleRadians; + + /// + /// 创建绑定指定单车底盘适配器的GCP命令执行器。 + /// + public GcpCommandExecutor( + MultiWheelChassisAdapter chassisAdapter, + double maximumGcpAngleRateRadiansPerSecond = + 10.0 * Math.PI / 180.0) + { + _chassisAdapter = chassisAdapter ?? + throw new ArgumentNullException( + nameof(chassisAdapter)); + EnsureFinitePositive( + maximumGcpAngleRateRadiansPerSecond, + nameof(maximumGcpAngleRateRadiansPerSecond)); + + MaximumGcpAngleRateRadiansPerSecond = + maximumGcpAngleRateRadiansPerSecond; + } + + /// + /// 获取执行器绑定的车辆编号。 + /// + public int VehicleId => + _chassisAdapter.VehicleId; + + /// + /// 获取前后GCP目标角度允许的最大变化率，单位为rad/s。 + /// + public double MaximumGcpAngleRateRadiansPerSecond { get; } + + /// + /// 获取最近一次控制器请求的未限速GCP命令。 + /// + public GcpMotionCommand? LastRequestedCommand { get; private set; } + + /// + /// 获取最近一次经过GCP角速度限制后实际发送给底盘的命令。 + /// + public GcpMotionCommand? LastSentCommand { get; private set; } + + /// + /// 获取最近一次旧版底盘运动分解失败原因。 + /// + public string LastFailureReason { get; private set; } = + string.Empty; + + /// + /// 使用真实控制周期执行一条GCP命令，并在分解失败时保持停车。 + /// + public bool Execute( + GcpMotionCommand command, + double deltaTimeSeconds) + { + EnsureFinitePositive( + deltaTimeSeconds, + nameof(deltaTimeSeconds)); + LastRequestedCommand = command; + + if (Math.Abs(command.SpeedMetersPerSecond) <= + StopSpeedDeadbandMetersPerSecond) + { + Stop(); + LastSentCommand = new GcpMotionCommand( + 0.0, + _lastFrontAngleRadians, + _lastRearAngleRadians); + return true; + } + + var maximumAngleChangeRadians = + MaximumGcpAngleRateRadiansPerSecond * + deltaTimeSeconds; + _lastFrontAngleRadians = MoveTowards( + _lastFrontAngleRadians, + command.FrontAngleRadians, + maximumAngleChangeRadians); + _lastRearAngleRadians = MoveTowards( + _lastRearAngleRadians, + command.RearAngleRadians, + maximumAngleChangeRadians); + + var limitedCommand = new GcpMotionCommand( + command.SpeedMetersPerSecond, + _lastFrontAngleRadians, + _lastRearAngleRadians); + LastSentCommand = limitedCommand; + + var success = _chassisAdapter.SendGcpMotion( + limitedCommand.SpeedMetersPerSecond, + limitedCommand.FrontAngleRadians, + limitedCommand.RearAngleRadians, + TimeSpan.FromSeconds(deltaTimeSeconds)); + + LastFailureReason = success + ? string.Empty + : BuildFailureReason(); + + return success; + } + + /// + /// 立即清零底盘驱动速度并清除执行器失败状态。 + /// + public void Stop() + { + _chassisAdapter.StopImmediately(); + LastFailureReason = string.Empty; + } + + /// + /// 以不超过指定单周期变化量的速度使当前值接近目标值。 + /// + private static double MoveTowards( + double current, + double target, + double maximumChange) + { + var difference = target - current; + + if (Math.Abs(difference) <= maximumChange) + { + return target; + } + + return current + + Math.Sign(difference) * + maximumChange; + } + + /// + /// 将底盘返回的空失败原因替换为可诊断的默认说明。 + /// + private string BuildFailureReason() + { + return string.IsNullOrWhiteSpace( + _chassisAdapter.LastFailureReason) + ? "旧版SendMotion未能完成GCP运动分解。" + : _chassisAdapter.LastFailureReason; + } + + /// + /// 检查控制周期是否为正有限值且能够转换为TimeSpan。 + /// + private static void EnsureFinitePositive( + double value, + string parameterName) + { + if (double.IsNaN(value) || + double.IsInfinity(value) || + value <= 0.0 || + value > TimeSpan.MaxValue.TotalSeconds) + { + throw new ArgumentOutOfRangeException( + parameterName, + "GCP命令控制周期必须是TimeSpan可表示的正有限秒数。"); + } + } + } +} diff --git a/MultiWheelC/Control/Execution/ParkingGeometricController.cs b/MultiWheelC/Control/Execution/ParkingGeometricController.cs index eef8148..7c4f944 100644 --- a/MultiWheelC/Control/Execution/ParkingGeometricController.cs +++ b/MultiWheelC/Control/Execution/ParkingGeometricController.cs @@ -1 +1,551 @@ -// 负责组织一个控制周期 +using System; +using MultiWheelC.Control.Abstractions; +using MultiWheelC.Control.Allocation; +using MultiWheelC.StateEstimation; +using MultiWheelC.Trajectory; +using MyParking.Shared; + +namespace MultiWheelC.Control.Execution +{ + /// + /// 表示新版停车机器人单周期轨迹控制的执行结果。 + /// + public enum ParkingControlCycleResult + { + Inactive = 0, + CommandSent = 1, + Completed = 2, + StateUnavailable = 3, + Faulted = 4 + } + + /// + /// 组织状态读取、轨迹投影、横纵向控制、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.05; + + private readonly IVehicleStateProvider _stateProvider; + private readonly ILateralController _lateralController; + private readonly ILongitudinalController _longitudinalController; + private readonly AckermannGcpAllocator _gcpAllocator; + private readonly GcpCommandExecutor _commandExecutor; + + private Trajectory2D _trajectory; + + /// + /// 创建具有终点判定和轨迹偏离保护的单车轨迹控制器。 + /// + public ParkingGeometricController( + IVehicleStateProvider stateProvider, + ILateralController lateralController, + ILongitudinalController longitudinalController, + AckermannGcpAllocator gcpAllocator, + GcpCommandExecutor commandExecutor, + double finishDistanceMeters = 0.03, + double finishSpeedMetersPerSecond = 0.02, + double finishHeadingToleranceRadians = + 3.0 * Math.PI / 180.0, + double maximumDistanceToTrajectoryMeters = 0.50) + { + _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( + finishDistanceMeters, + nameof(finishDistanceMeters)); + EnsureFiniteNonNegative( + finishSpeedMetersPerSecond, + nameof(finishSpeedMetersPerSecond)); + EnsureFinitePositive( + finishHeadingToleranceRadians, + nameof(finishHeadingToleranceRadians)); + EnsureFinitePositive( + maximumDistanceToTrajectoryMeters, + nameof(maximumDistanceToTrajectoryMeters)); + + FinishDistanceMeters = finishDistanceMeters; + FinishSpeedMetersPerSecond = + finishSpeedMetersPerSecond; + FinishHeadingToleranceRadians = + finishHeadingToleranceRadians; + MaximumDistanceToTrajectoryMeters = + maximumDistanceToTrajectoryMeters; + } + + /// + /// 获取终点位置和剩余弧长允许的误差，单位为m。 + /// + public double FinishDistanceMeters { get; } + + /// + /// 获取判定轨迹执行完成时允许的最大实际线速度，单位为m/s。 + /// + public double FinishSpeedMetersPerSecond { get; } + + /// + /// 获取判定轨迹完成时允许的最大终点航向误差，单位为rad。 + /// + public double FinishHeadingToleranceRadians { get; } + + /// + /// 获取允许车辆偏离参考轨迹的最大距离，单位为m。 + /// + public double MaximumDistanceToTrajectoryMeters { 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 VehicleState? LastVehicleState { get; private set; } + + /// + /// 获取最近一次车体中心到参考轨迹的投影结果。 + /// + public TrajectoryProjection? LastProjection { get; private set; } + + /// + /// 获取最近一次发送或准备发送的GCP运动命令。 + /// + public GcpMotionCommand? LastCommand { get; private set; } + + /// + /// 获取最近控制周期实际交给纵向控制器的参考速度，单位为m/s。 + /// + public double? LastReferenceSpeedMetersPerSecond { get; private set; } + + /// + /// 停止当前底盘并从起点开始执行指定二维轨迹。 + /// + public void Start(Trajectory2D trajectory) + { + if (trajectory == null) + { + throw new ArgumentNullException( + nameof(trajectory)); + } + + StopAndResetControllers(); + _trajectory = trajectory; + IsActive = true; + IsCompleted = false; + ClearDiagnostics(); + } + + /// + /// 读取本周期车辆状态并执行一次完整的轨迹跟踪控制计算。 + /// + public ParkingControlCycleResult ExecuteCycle( + double deltaTimeSeconds) + { + EnsureFinitePositive( + deltaTimeSeconds, + nameof(deltaTimeSeconds)); + + if (!IsActive || _trajectory == null) + { + return ParkingControlCycleResult.Inactive; + } + + try + { + if (!_stateProvider.TryGetState( + out var vehicleState)) + { + StopForUnavailableState(); + return ParkingControlCycleResult + .StateUnavailable; + } + + LastVehicleState = vehicleState; + + var projection = TrajectoryProjector.Project( + _trajectory, + vehicleState.PoseInWorld); + LastProjection = projection; + + if (projection.DistanceToTrajectoryMeters > + MaximumDistanceToTrajectoryMeters) + { + return EnterFault( + "车辆距离参考轨迹" + + $"{projection.DistanceToTrajectoryMeters:F3}m，" + + "超过允许值" + + $"{MaximumDistanceToTrajectoryMeters:F3}m。"); + } + + if (HasReachedEnd( + vehicleState, + projection)) + { + CompleteTrajectory(); + return ParkingControlCycleResult.Completed; + } + + if (HasStoppedAtUnsatisfiedTerminal( + vehicleState, + projection, + out var terminalFailureReason)) + { + return EnterFault( + terminalFailureReason); + } + + var referenceSpeedMetersPerSecond = + ResolveReferenceSpeedForControl( + projection); + LastReferenceSpeedMetersPerSecond = + referenceSpeedMetersPerSecond; + var context = new PathTrackingContext( + vehicleState, + projection, + referenceSpeedMetersPerSecond, + deltaTimeSeconds); + var lateralCommand = + _lateralController.Compute(context); + var commandSpeedMetersPerSecond = + _longitudinalController + .ComputeSpeedMetersPerSecond(context); + var gcpCommand = _gcpAllocator.Allocate( + commandSpeedMetersPerSecond, + lateralCommand); + + if (!_commandExecutor.Execute( + gcpCommand, + deltaTimeSeconds)) + { + return EnterFault( + string.IsNullOrWhiteSpace( + _commandExecutor.LastFailureReason) + ? "GCP底盘命令执行失败。" + : _commandExecutor.LastFailureReason); + } + + LastCommand = + _commandExecutor.LastSentCommand; + + LastFailureReason = string.Empty; + LastException = null; + return ParkingControlCycleResult.CommandSent; + } + catch (Exception exception) + { + return EnterFault( + "停车机器人轨迹控制周期异常：" + + exception.Message, + exception); + } + } + + /// + /// 主动取消当前轨迹、立即停车并清除全部控制器状态。 + /// + public void Cancel() + { + StopAndResetControllers(); + _trajectory = null; + IsActive = false; + IsCompleted = false; + ClearDiagnostics(); + } + + /// + /// 在轨迹起点零速固定点处读取前方速度，并限制为低速起步命令。 + /// + private double ResolveReferenceSpeedForControl( + TrajectoryProjection projection) + { + var currentReferenceSpeed = + projection.ReferencePoint + .ReferenceSpeedMetersPerSecond; + + var requiresStartupRelease = + projection.ArcLengthMeters <= + StartupRegionMeters && + projection.RemainingDistanceMeters > + FinishDistanceMeters && + Math.Abs(currentReferenceSpeed) <= + ZeroReferenceSpeedToleranceMetersPerSecond; + + if (!requiresStartupRelease) + { + return currentReferenceSpeed; + } + + var previewArcLengthMeters = Math.Min( + _trajectory.TotalLengthMeters, + projection.ArcLengthMeters + + StartupPreviewDistanceMeters); + var previewReferenceSpeed = + _trajectory + .SampleAtArcLength( + previewArcLengthMeters) + .ReferenceSpeedMetersPerSecond; + + if (Math.Abs(previewReferenceSpeed) <= + ZeroReferenceSpeedToleranceMetersPerSecond) + { + return 0.0; + } + + return Math.Sign(previewReferenceSpeed) * + Math.Min( + Math.Abs(previewReferenceSpeed), + MaximumStartupSpeedMetersPerSecond); + } + + /// + /// 根据终点距离、剩余弧长和实际线速度判断轨迹是否完成。 + /// + private bool HasReachedEnd( + VehicleState vehicleState, + TrajectoryProjection projection) + { + if (!vehicleState.HasValidVelocityEstimate) + { + return false; + } + + return projection.RemainingDistanceMeters <= + FinishDistanceMeters && + CalculateDistanceToEndMeters( + vehicleState) <= + FinishDistanceMeters && + CalculateHeadingErrorToEndRadians( + vehicleState) <= + FinishHeadingToleranceRadians && + CalculateActualLinearSpeedMetersPerSecond( + 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 || + CalculateActualLinearSpeedMetersPerSecond( + 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)); + } + + /// + /// 计算车体坐标系实际线速度的合速度绝对值，单位为m/s。 + /// + private static double CalculateActualLinearSpeedMetersPerSecond( + VehicleState vehicleState) + { + return Math.Sqrt( + vehicleState.TwistInBody.VxMetersPerSecond * + vehicleState.TwistInBody.VxMetersPerSecond + + vehicleState.TwistInBody.VyMetersPerSecond * + vehicleState.TwistInBody.VyMetersPerSecond); + } + + /// + /// 在状态暂不可用时停车并重置反馈控制器，同时保留轨迹等待下一周期恢复。 + /// + private void StopForUnavailableState() + { + _commandExecutor.Stop(); + _lateralController.Reset(); + _longitudinalController.Reset(); + LastCommand = null; + LastFailureReason = + "当前无法获得有效车辆状态，底盘已停车并等待定位恢复。"; + LastException = null; + } + + /// + /// 完成当前轨迹并停车，但保留最后状态和投影供实验记录读取。 + /// + private void CompleteTrajectory() + { + StopAndResetControllers(); + IsActive = false; + IsCompleted = true; + LastCommand = new GcpMotionCommand( + 0.0, + 0.0, + 0.0); + LastFailureReason = string.Empty; + LastException = null; + } + + /// + /// 发生不可继续的控制故障时停车、退出活动状态并保存诊断信息。 + /// + private ParkingControlCycleResult EnterFault( + string reason, + Exception exception = null) + { + StopAndResetControllers(); + IsActive = false; + IsCompleted = false; + LastCommand = null; + LastFailureReason = reason; + LastException = exception; + return ParkingControlCycleResult.Faulted; + } + + /// + /// 立即停止底盘并清除横向和纵向控制器的跨周期状态。 + /// + private void StopAndResetControllers() + { + _commandExecutor.Stop(); + _lateralController.Reset(); + _longitudinalController.Reset(); + } + + /// + /// 清除上一条轨迹留下的状态、命令和故障诊断信息。 + /// + private void ClearDiagnostics() + { + LastVehicleState = null; + LastProjection = null; + LastCommand = null; + LastReferenceSpeedMetersPerSecond = null; + LastFailureReason = string.Empty; + LastException = null; + } + + /// + /// 检查控制参数是否为正有限值。 + /// + 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/Lateral/StanleyLateralController.cs b/MultiWheelC/Control/Lateral/StanleyLateralController.cs index e69de29..939b897 100644 --- a/MultiWheelC/Control/Lateral/StanleyLateralController.cs +++ b/MultiWheelC/Control/Lateral/StanleyLateralController.cs @@ -0,0 +1,223 @@ +using System; +using MultiWheelC.Control.Abstractions; + +namespace MultiWheelC.Control.Lateral +{ + /// + /// 使用参考曲率前馈、航向误差和横向误差计算车体中心目标曲率。 + /// + public sealed class StanleyLateralController : ILateralController + { + private const double MaximumMathematicalAngleRadians = + Math.PI / 2.0 - 1e-3; + + /// + /// 创建使用指定GCP几何、Stanley增益和低速保护参数的横向控制器。 + /// + public StanleyLateralController( + double controlPointRadiusMeters, + double crossTrackGainPerSecond, + double headingErrorGain, + double minimumSpeedMetersPerSecond, + bool useActualSpeedForGain = true) + { + EnsureFinitePositive( + controlPointRadiusMeters, + nameof(controlPointRadiusMeters)); + EnsureFiniteNonNegative( + crossTrackGainPerSecond, + nameof(crossTrackGainPerSecond)); + EnsureFiniteNonNegative( + headingErrorGain, + nameof(headingErrorGain)); + EnsureFinitePositive( + minimumSpeedMetersPerSecond, + nameof(minimumSpeedMetersPerSecond)); + + ControlPointRadiusMeters = controlPointRadiusMeters; + CrossTrackGainPerSecond = crossTrackGainPerSecond; + HeadingErrorGain = headingErrorGain; + MinimumSpeedMetersPerSecond = minimumSpeedMetersPerSecond; + UseActualSpeedForGain = useActualSpeedForGain; + } + + /// + /// 获取车体中心到前、后GCP的距离，单位为m。 + /// + public double ControlPointRadiusMeters { get; } + + /// + /// 获取横向误差增益，单位为1/s。 + /// + public double CrossTrackGainPerSecond { get; } + + /// + /// 获取航向误差的无量纲增益。 + /// + public double HeadingErrorGain { get; } + + /// + /// 获取Stanley分母使用的最小速度绝对值，单位为m/s。 + /// + public double MinimumSpeedMetersPerSecond { get; } + + /// + /// 获取是否优先使用Detour估算的实际纵向速度计算横向误差项。 + /// + public bool UseActualSpeedForGain { get; } + + /// + /// 根据参考曲率、航向误差和横向误差计算车体中心目标曲率。 + /// + public LateralControlCommand Compute( + PathTrackingContext context) + { + var speedForGain = SelectSpeedForGain(context); + var speedMagnitude = Math.Max( + Math.Abs(speedForGain), + MinimumSpeedMetersPerSecond); + var travelDirection = SelectTravelDirection(context); + + // 参考曲率提供前馈；没有跟踪误差时也能沿曲线行驶。 + var feedforwardAngleRadians = Math.Atan( + context.ReferenceCurvaturePerMeter * + ControlPointRadiusMeters); + + // 轨迹位于车辆左侧时横向误差为正，对应正的左转修正。 + var crossTrackCorrectionRadians = Math.Atan( + CrossTrackGainPerSecond * + context.LateralErrorMeters / + speedMagnitude); + + // 倒车时需要反转反馈修正方向；参考曲率前馈仍由轨迹本身决定。 + var feedbackAngleRadians = travelDirection * + (HeadingErrorGain * context.HeadingErrorRadians + + crossTrackCorrectionRadians); + + // 这里只避开tan奇点，实际GCP机械限制由AckermannGcpAllocator处理。 + var targetEquivalentAngleRadians = Clamp( + feedforwardAngleRadians + feedbackAngleRadians, + -MaximumMathematicalAngleRadians, + MaximumMathematicalAngleRadians); + var targetCurvaturePerMeter = Math.Tan( + targetEquivalentAngleRadians) / + ControlPointRadiusMeters; + + return new LateralControlCommand( + targetCurvaturePerMeter); + } + + /// + /// 清除横向控制器状态；当前Stanley实现没有跨周期状态。 + /// + public void Reset() + { + } + + /// + /// 选择Stanley横向误差项使用的实际速度或旧版参考速度。 + /// + private double SelectSpeedForGain( + PathTrackingContext context) + { + if (UseActualSpeedForGain && + context.HasValidVelocityEstimate) + { + return context + .ActualLongitudinalSpeedMetersPerSecond; + } + + return context.ReferenceSpeedMetersPerSecond; + } + + /// + /// 根据有符号参考速度确定前进或倒车的反馈修正方向。 + /// + private static double SelectTravelDirection( + PathTrackingContext context) + { + const double directionDeadbandMetersPerSecond = 1e-6; + + if (Math.Abs(context.ReferenceSpeedMetersPerSecond) > + directionDeadbandMetersPerSecond) + { + return Math.Sign( + context.ReferenceSpeedMetersPerSecond); + } + + if (context.HasValidVelocityEstimate && + Math.Abs( + context.ActualLongitudinalSpeedMetersPerSecond) > + directionDeadbandMetersPerSecond) + { + return Math.Sign( + context.ActualLongitudinalSpeedMetersPerSecond); + } + + return 1.0; + } + + /// + /// 将数值限制在指定闭区间内。 + /// + private static double Clamp( + double value, + double minimum, + double maximum) + { + return Math.Max( + minimum, + Math.Min(maximum, value)); + } + + /// + /// 检查控制参数是否为正有限值。 + /// + private static void EnsureFinitePositive( + double value, + string parameterName) + { + EnsureFinite(value, parameterName); + + if (value <= 0.0) + { + throw new ArgumentOutOfRangeException( + parameterName, + "Stanley控制器的几何尺寸和最小速度必须是正有限值。"); + } + } + + /// + /// 检查控制增益是否为非负有限值。 + /// + private static void EnsureFiniteNonNegative( + double value, + string parameterName) + { + EnsureFinite(value, parameterName); + + if (value < 0.0) + { + throw new ArgumentOutOfRangeException( + parameterName, + "Stanley控制增益必须是非负有限值。"); + } + } + + /// + /// 检查控制参数是否为有限值。 + /// + private static void EnsureFinite( + double value, + string parameterName) + { + if (double.IsNaN(value) || + double.IsInfinity(value)) + { + throw new ArgumentOutOfRangeException( + parameterName, + "Stanley控制参数必须是有限值。"); + } + } + } +} diff --git a/MultiWheelC/Control/Longitudinal/PidLongitudinalController.cs b/MultiWheelC/Control/Longitudinal/PidLongitudinalController.cs index e69de29..ab5f957 100644 --- a/MultiWheelC/Control/Longitudinal/PidLongitudinalController.cs +++ b/MultiWheelC/Control/Longitudinal/PidLongitudinalController.cs @@ -0,0 +1,182 @@ +using System; +using MultiWheelC.Control.Abstractions; +using MultiWheelC.Control.Common; + +namespace MultiWheelC.Control.Longitudinal +{ + /// + /// 将轨迹参考速度前馈与通用PID速度反馈组合为有符号底盘命令速度。 + /// + public sealed class PidLongitudinalController + : ILongitudinalController + { + private const double ReferenceStopDeadbandMetersPerSecond = + 1e-6; + + private readonly PidController _feedbackPid; + + /// + /// 创建具有积分抗饱和和命令速度限幅的纵向速度外环。 + /// + public PidLongitudinalController( + double proportionalGain, + double integralGainPerSecond, + double derivativeGainSeconds, + double maximumIntegralCorrectionMetersPerSecond, + double maximumCommandSpeedMetersPerSecond) + { + EnsureFinitePositive( + maximumCommandSpeedMetersPerSecond, + nameof(maximumCommandSpeedMetersPerSecond)); + + _feedbackPid = new PidController( + proportionalGain, + integralGainPerSecond, + derivativeGainSeconds, + maximumIntegralCorrectionMetersPerSecond, + derivativeOnMeasurement: true); + MaximumCommandSpeedMetersPerSecond = + maximumCommandSpeedMetersPerSecond; + } + + /// + /// 获取负责计算速度误差修正量的通用PID控制器。 + /// + public PidController FeedbackPid => _feedbackPid; + + /// + /// 获取底盘命令速度的最大绝对值，单位为m/s。 + /// + public double MaximumCommandSpeedMetersPerSecond { get; } + + /// + /// 获取最近一次有效控制周期的参考速度减实际速度，单位为m/s。 + /// + public double LastSpeedErrorMetersPerSecond => + _feedbackPid.LastError; + + /// + /// 获取最近一次比例项产生的速度修正，单位为m/s。 + /// + public double LastProportionalCorrectionMetersPerSecond => + _feedbackPid.LastProportionalOutput; + + /// + /// 获取最近一次积分项产生的速度修正，单位为m/s。 + /// + public double LastIntegralCorrectionMetersPerSecond => + _feedbackPid.LastIntegralOutput; + + /// + /// 获取最近一次微分项产生的速度修正，单位为m/s。 + /// + public double LastDerivativeCorrectionMetersPerSecond => + _feedbackPid.LastDerivativeOutput; + + /// + /// 根据轨迹参考速度和Detour实际纵向速度计算底盘命令速度。 + /// + public double ComputeSpeedMetersPerSecond( + PathTrackingContext context) + { + var referenceSpeedMetersPerSecond = + context.ReferenceSpeedMetersPerSecond; + + // 轨迹明确要求停车时直接输出零，防止速度反馈使车辆在终点反向纠偏。 + if (Math.Abs(referenceSpeedMetersPerSecond) <= + ReferenceStopDeadbandMetersPerSecond) + { + Reset(); + return 0.0; + } + + // 定位速度尚不可用时只透传参考速度，不使用无效反馈更新PID状态。 + if (!context.HasValidVelocityEstimate) + { + Reset(); + return LimitReferenceSpeed( + referenceSpeedMetersPerSecond); + } + + GetCorrectionOutputRange( + referenceSpeedMetersPerSecond, + out var minimumCorrectionMetersPerSecond, + out var maximumCorrectionMetersPerSecond); + + var correctionMetersPerSecond = + _feedbackPid.Update( + referenceSpeedMetersPerSecond, + context + .ActualLongitudinalSpeedMetersPerSecond, + context.DeltaTimeSeconds, + minimumCorrectionMetersPerSecond, + maximumCorrectionMetersPerSecond); + + return referenceSpeedMetersPerSecond + + correctionMetersPerSecond; + } + + /// + /// 清除纵向速度外环的积分、历史测量值和诊断输出。 + /// + public void Reset() + { + _feedbackPid.Reset(); + } + + /// + /// 根据参考行驶方向计算PID修正量允许使用的动态输出范围。 + /// + private void GetCorrectionOutputRange( + double referenceSpeedMetersPerSecond, + out double minimumCorrectionMetersPerSecond, + out double maximumCorrectionMetersPerSecond) + { + if (referenceSpeedMetersPerSecond > 0.0) + { + minimumCorrectionMetersPerSecond = + -referenceSpeedMetersPerSecond; + maximumCorrectionMetersPerSecond = + MaximumCommandSpeedMetersPerSecond - + referenceSpeedMetersPerSecond; + return; + } + + minimumCorrectionMetersPerSecond = + -MaximumCommandSpeedMetersPerSecond - + referenceSpeedMetersPerSecond; + maximumCorrectionMetersPerSecond = + -referenceSpeedMetersPerSecond; + } + + /// + /// 在没有有效速度反馈时限制参考速度的绝对值。 + /// + private double LimitReferenceSpeed( + double referenceSpeedMetersPerSecond) + { + return Math.Max( + -MaximumCommandSpeedMetersPerSecond, + Math.Min( + MaximumCommandSpeedMetersPerSecond, + referenceSpeedMetersPerSecond)); + } + + /// + /// 检查最大命令速度是否为正有限值。 + /// + private static void EnsureFinitePositive( + double value, + string parameterName) + { + if (double.IsNaN(value) || + double.IsInfinity(value) || + value <= 0.0) + { + throw new ArgumentOutOfRangeException( + parameterName, + "纵向控制器最大命令速度必须是正有限值。"); + } + } + } +} diff --git a/MultiWheelC/Experiments/NewControllerTrackingTests.cs b/MultiWheelC/Experiments/NewControllerTrackingTests.cs new file mode 100644 index 0000000..ff48c25 --- /dev/null +++ b/MultiWheelC/Experiments/NewControllerTrackingTests.cs @@ -0,0 +1,297 @@ +using System; +using System.Drawing; +using System.Numerics; +using System.Threading; +using ClumsyCore; +using ClumsyCore.DTools; +using ClumsyCore.Interfaces; +using ClumsyCore.Pilot; +using CommonUsage.Chassis; +using MultiWheelC.Control.Execution; +using MultiWheelC.StateEstimation; +using MyParking.Shared; + +namespace MultiWheelC +{ + /// + /// 从当前Detour位姿开始执行新版控制器4m直线跟踪并保存实验数据。 + /// + [MovementTest(name = "新版控制器：4m直线轨迹跟踪")] + public sealed class NewControllerStraight4mTest + : MovementTest + { + private const float MillimetersPerMeter = 1000f; + + private readonly Painter _painter = + UI.GetPainter("NewControllerStraight4m"); + + private DriveTask _task; + private TrackingExperimentRecorder _recorder; + private DetourVehicleStateProvider _stateProvider; + + /// + /// 获取或设置本次测试编号，用于区分重复实验CSV。 + /// + public int TrialNumber = 1; + + /// + /// 获取或设置4m直线的巡航参考速度，单位为m/s。 + /// + public double CruiseSpeedMetersPerSecond = 0.30; + + /// + /// 获取或设置参考速度加速度，单位为m/s²。 + /// + public double AccelerationMetersPerSecondSquared = 0.20; + + /// + /// 获取或设置参考速度减速度，单位为m/s²。 + /// + public double DecelerationMetersPerSecondSquared = 0.20; + + /// + /// 获取或设置离散轨迹点间距，单位为m。 + /// + public double PointSpacingMeters = 0.02; + + /// + /// 读取当前位姿、绘制离散轨迹并启动新版轨迹跟踪动作。 + /// + public override void Test() + { + if (_task != null) + { + Console.WriteLine( + "新版4m直线轨迹测试已经在运行，请先停止当前测试。"); + return; + } + + if (!MovementTestPreparation.AreWheelsForward()) + { + return; + } + + var chassis = + PilotDefinition.Chassis as MultiWheelChassis; + if (chassis == null) + { + Console.WriteLine( + "当前底盘不是MultiWheelChassis，无法执行新版轨迹测试。"); + return; + } + + _stateProvider = + new DetourVehicleStateProvider(); + if (!_stateProvider.TryGetState( + out var initialState)) + { + Console.WriteLine( + "无法读取有效Detour起点位姿：" + + _stateProvider.LastFailureReason); + _stateProvider = null; + return; + } + + var trajectory = + TestTrajectoryFactory.CreateStraight4Meters( + initialState.PoseInWorld, + CruiseSpeedMetersPerSecond, + AccelerationMetersPerSecondSquared, + DecelerationMetersPerSecondSquared, + PointSpacingMeters); + + DrawTrajectory(trajectory); + + var referenceStart = ToMillimeterVector( + trajectory.StartPoint.PoseInWorld); + var referenceEnd = ToMillimeterVector( + trajectory.EndPoint.PoseInWorld); + var recorder = + new TrackingExperimentRecorder( + controllerName: "NewStanleyPid", + trajectoryName: "ProfiledStraight4m", + trialNumber: TrialNumber, + referenceStart: referenceStart, + referenceEnd: referenceEnd, + referenceSpeed: + (float)CruiseSpeedMetersPerSecond, + sampleIntervalMs: 50, + referenceAccelerationMetersPerSecondSquared: + (float)AccelerationMetersPerSecondSquared, + referenceDecelerationMetersPerSecondSquared: + (float)DecelerationMetersPerSecondSquared); + _recorder = recorder; + + var controlPointRadiusMeters = + chassis.ControlPointRadius / + MillimetersPerMeter; + var movement = + new TrajectoryTrackingMovement + { + Trajectory = trajectory, + StateProvider = _stateProvider, + MaximumCommandSpeedMetersPerSecond = 0.50, + CycleObserver = controller => + RecordControlCycle( + recorder, + controller, + controlPointRadiusMeters) + }; + + recorder.Start(); + + try + { + _task = new DriveTask(movement.Get()); + _task.Wait(); + + // 保留少量停车后原始Detour数据，并生成一帧处理后的静止状态。 + Thread.Sleep(400); + recorder.UpdateCommand(0f, 0f); + if (_stateProvider.TryGetState( + out var stoppedState)) + { + recorder.UpdateProcessedState( + stoppedState); + } + } + finally + { + _task?.Stop(); + recorder.UpdateCommand(0f, 0f); + recorder.StopAndSave(); + _painter.Clear(); + _task = null; + _recorder = null; + _stateProvider = null; + } + } + + /// + /// 停止正在运行的测试、保存已有数据并清除Clumsy轨迹可视化。 + /// + public override void TestStop() + { + _task?.Stop(); + _recorder?.UpdateCommand(0f, 0f); + _recorder?.StopAndSave(); + _painter.Clear(); + _task = null; + _recorder = null; + _stateProvider = null; + } + + /// + /// 将离散轨迹点和相邻线段从SI单位转换为Clumsy毫米坐标后绘制。 + /// + private void DrawTrajectory( + Trajectory.Trajectory2D trajectory) + { + _painter.Clear(); + + for (var index = 0; + index < trajectory.Count; + index++) + { + var point = ToMillimeterVector( + trajectory[index].PoseInWorld); + _painter.DrawDot( + Color.Cyan, + point.X, + point.Y, + 3f); + + if (index == 0) + { + continue; + } + + var previousPoint = ToMillimeterVector( + trajectory[index - 1].PoseInWorld); + _painter.DrawLine( + Color.DeepSkyBlue, + previousPoint.X, + previousPoint.Y, + point.X, + point.Y, + width: 2); + } + + var start = ToMillimeterVector( + trajectory.StartPoint.PoseInWorld); + var end = ToMillimeterVector( + trajectory.EndPoint.PoseInWorld); + _painter.DrawDot( + Color.LimeGreen, + start.X, + start.Y, + 8f); + _painter.DrawDot( + Color.OrangeRed, + end.X, + end.Y, + 8f); + } + + /// + /// 将控制器本周期使用的状态和最终GCP命令同步给实验记录器。 + /// + private static void RecordControlCycle( + TrackingExperimentRecorder recorder, + ParkingGeometricController controller, + double controlPointRadiusMeters) + { + if (controller.LastVehicleState.HasValue) + { + recorder.UpdateProcessedState( + controller.LastVehicleState.Value); + } + + if (!controller.LastCommand.HasValue) + { + return; + } + + if (controller.LastProjection.HasValue && + controller.LastReferenceSpeedMetersPerSecond.HasValue) + { + var projection = + controller.LastProjection.Value; + recorder.UpdateControlReference( + projection.ArcLengthMeters, + controller.LastReferenceSpeedMetersPerSecond.Value, + projection.LateralErrorMeters, + projection.HeadingErrorRadians, + projection.DistanceToTrajectoryMeters, + projection.RemainingDistanceMeters); + } + + var command = controller.LastCommand.Value; + var curvaturePerMeter = Math.Tan( + command.FrontAngleRadians) / + controlPointRadiusMeters; + var angularSpeedRadiansPerSecond = + command.SpeedMetersPerSecond * + curvaturePerMeter; + + recorder.UpdateCommand( + (float)command.SpeedMetersPerSecond, + (float)angularSpeedRadiansPerSecond); + } + + /// + /// 将Shared世界坐标系米制位姿转换为Clumsy绘图和旧记录器使用的毫米坐标。 + /// + private static Vector2 ToMillimeterVector( + Pose2D poseInWorld) + { + return new Vector2( + (float)( + poseInWorld.XMeters * + MillimetersPerMeter), + (float)( + poseInWorld.YMeters * + MillimetersPerMeter)); + } + } +} diff --git a/MultiWheelC/Experiments/TestTrajectoryFactory.cs b/MultiWheelC/Experiments/TestTrajectoryFactory.cs new file mode 100644 index 0000000..4e7ea70 --- /dev/null +++ b/MultiWheelC/Experiments/TestTrajectoryFactory.cs @@ -0,0 +1,163 @@ +using System; +using System.Collections.Generic; +using MultiWheelC.Trajectory; +using MyParking.Shared; + +namespace MultiWheelC +{ + /// + /// 为新版控制器实验生成不依赖正式规划层的简单世界坐标系参考轨迹。 + /// + public static class TestTrajectoryFactory + { + private const double StraightLengthMeters = 4.0; + + /// + /// 从给定车体中心位姿沿当前航向生成带梯形速度规划的4m直线轨迹。 + /// + public static Trajectory2D CreateStraight4Meters( + Pose2D startPoseInWorld, + double cruiseSpeedMetersPerSecond = 0.30, + double accelerationMetersPerSecondSquared = 0.20, + double decelerationMetersPerSecondSquared = 0.20, + double pointSpacingMeters = 0.02) + { + EnsureFinitePose( + startPoseInWorld, + nameof(startPoseInWorld)); + EnsureFinitePositive( + cruiseSpeedMetersPerSecond, + nameof(cruiseSpeedMetersPerSecond)); + EnsureFinitePositive( + accelerationMetersPerSecondSquared, + nameof(accelerationMetersPerSecondSquared)); + EnsureFinitePositive( + decelerationMetersPerSecondSquared, + nameof(decelerationMetersPerSecondSquared)); + EnsureFinitePositive( + pointSpacingMeters, + nameof(pointSpacingMeters)); + + if (pointSpacingMeters > StraightLengthMeters) + { + throw new ArgumentOutOfRangeException( + nameof(pointSpacingMeters), + "直线轨迹点间距不能大于轨迹总长度。"); + } + + var segmentCount = (int)Math.Ceiling( + StraightLengthMeters / + pointSpacingMeters); + var points = new List( + segmentCount + 1); + var directionX = Math.Cos( + startPoseInWorld.YawRadians); + var directionY = Math.Sin( + startPoseInWorld.YawRadians); + + for (var index = 0; + index <= segmentCount; + index++) + { + // 均分后最后一个点严格落在4m终点，避免浮点累加越界。 + var arcLengthMeters = + StraightLengthMeters * + index / + segmentCount; + var remainingDistanceMeters = + StraightLengthMeters - + arcLengthMeters; + var referenceSpeedMetersPerSecond = + CalculateReferenceSpeed( + arcLengthMeters, + remainingDistanceMeters, + cruiseSpeedMetersPerSecond, + accelerationMetersPerSecondSquared, + decelerationMetersPerSecondSquared); + + points.Add( + new TrajectoryPoint( + arcLengthMeters, + new Pose2D( + startPoseInWorld.XMeters + + directionX * arcLengthMeters, + startPoseInWorld.YMeters + + directionY * arcLengthMeters, + startPoseInWorld.YawRadians), + curvaturePerMeter: 0.0, + referenceSpeedMetersPerSecond: + referenceSpeedMetersPerSecond)); + } + + return new Trajectory2D(points); + } + + /// + /// 根据起步、巡航和制动能力计算指定弧长位置允许的参考速度。 + /// + private static double CalculateReferenceSpeed( + double arcLengthMeters, + double remainingDistanceMeters, + double cruiseSpeedMetersPerSecond, + double accelerationMetersPerSecondSquared, + double decelerationMetersPerSecondSquared) + { + // 由v²=2as分别得到从静止起步和到终点静止允许的速度上限。 + var accelerationLimitedSpeed = Math.Sqrt( + 2.0 * + accelerationMetersPerSecondSquared * + Math.Max(0.0, arcLengthMeters)); + var brakingLimitedSpeed = Math.Sqrt( + 2.0 * + decelerationMetersPerSecondSquared * + Math.Max(0.0, remainingDistanceMeters)); + + return Math.Min( + cruiseSpeedMetersPerSecond, + Math.Min( + accelerationLimitedSpeed, + brakingLimitedSpeed)); + } + + /// + /// 检查世界坐标系起点位姿是否全部为有限值。 + /// + private static void EnsureFinitePose( + Pose2D pose, + string parameterName) + { + if (!IsFinite(pose.XMeters) || + !IsFinite(pose.YMeters) || + !IsFinite(pose.YawRadians)) + { + throw new ArgumentOutOfRangeException( + parameterName, + "直线测试轨迹的起点位姿必须由有限值组成。"); + } + } + + /// + /// 检查测试轨迹参数是否为正有限值。 + /// + private static void EnsureFinitePositive( + double value, + string parameterName) + { + if (!IsFinite(value) || value <= 0.0) + { + throw new ArgumentOutOfRangeException( + parameterName, + "直线测试轨迹的速度、加速度和点间距必须是正有限值。"); + } + } + + /// + /// 判断数值是否可用于轨迹计算。 + /// + private static bool IsFinite(double value) + { + return !double.IsNaN(value) && + !double.IsInfinity(value); + } + } +} diff --git a/MultiWheelC/Experiments/TrackingExperimentRecorder.cs b/MultiWheelC/Experiments/TrackingExperimentRecorder.cs index 7d15f46..1fd9ee3 100644 --- a/MultiWheelC/Experiments/TrackingExperimentRecorder.cs +++ b/MultiWheelC/Experiments/TrackingExperimentRecorder.cs @@ -8,6 +8,7 @@ using System.Numerics; using System.Text; using System.Threading; using MyParking.Shared; +using MultiWheelC.StateEstimation; namespace MultiWheelC { @@ -26,6 +27,26 @@ namespace MultiWheelC public float CommandVx; public float CommandVy; public float CommandAngularSpeed; + + // 新版状态估计统一使用SI单位；无有效控制器状态时HasProcessedState为false。 + public bool HasProcessedState; + public double StateTimestampSeconds; + public double StateXmeters; + public double StateYMeters; + public double StateYawRadians; + public double StateWorldVxMetersPerSecond; + public double StateWorldVyMetersPerSecond; + public double StateBodyVxMetersPerSecond; + public double StateBodyVyMetersPerSecond; + public double StateAngularSpeedRadiansPerSecond; + public bool StateVelocityEstimateValid; + public bool HasControlReference; + public double ControlReferenceArcLengthMeters; + public double ControlReferenceSpeedMetersPerSecond; + public double ControlLateralErrorMeters; + public double ControlHeadingErrorRadians; + public double ControlDistanceToTrajectoryMeters; + public double ControlRemainingDistanceMeters; } // C层实验工具：统一采集并保存轨迹跟踪实验数据。 @@ -39,6 +60,8 @@ namespace MultiWheelC private readonly float _referenceSpeed; private readonly float _referenceAngularSpeed; private readonly float _referenceMotionFrameYawDegrees; + private readonly float _referenceAccelerationMetersPerSecondSquared; + private readonly float _referenceDecelerationMetersPerSecondSquared; private readonly int _sampleIntervalMs; private readonly List _samples = @@ -50,6 +73,9 @@ namespace MultiWheelC private readonly object _commandSyncRoot = new object(); + private readonly object _stateSyncRoot = + new object(); + private readonly Stopwatch _stopwatch = new Stopwatch(); @@ -63,6 +89,14 @@ namespace MultiWheelC private float _externalCommandVx; private float _externalCommandVy; private float _externalCommandAngularSpeed; + private VehicleState? _latestProcessedState; + private bool _hasControlReference; + private double _controlReferenceArcLengthMeters; + private double _controlReferenceSpeedMetersPerSecond; + private double _controlLateralErrorMeters; + private double _controlHeadingErrorRadians; + private double _controlDistanceToTrajectoryMeters; + private double _controlRemainingDistanceMeters; public TrackingExperimentRecorder( string controllerName, @@ -73,7 +107,9 @@ namespace MultiWheelC float referenceSpeed, float referenceAngularSpeed = 0f, int sampleIntervalMs = 50, - float referenceMotionFrameYawDegrees = 0f) + float referenceMotionFrameYawDegrees = 0f, + float referenceAccelerationMetersPerSecondSquared = 0f, + float referenceDecelerationMetersPerSecondSquared = 0f) { if (string.IsNullOrWhiteSpace(controllerName)) throw new ArgumentException( @@ -99,6 +135,10 @@ namespace MultiWheelC _referenceAngularSpeed = referenceAngularSpeed; _referenceMotionFrameYawDegrees = referenceMotionFrameYawDegrees; + _referenceAccelerationMetersPerSecondSquared = + referenceAccelerationMetersPerSecondSquared; + _referenceDecelerationMetersPerSecondSquared = + referenceDecelerationMetersPerSecondSquared; _sampleIntervalMs = sampleIntervalMs; } @@ -162,6 +202,46 @@ namespace MultiWheelC } } + /// + /// 保存新版控制器本周期实际使用的校验后车辆状态，供后台采样线程写入CSV。 + /// + public void UpdateProcessedState(VehicleState state) + { + lock (_stateSyncRoot) + { + _latestProcessedState = state; + } + } + + /// + /// 保存新版控制器本周期实际使用的轨迹投影、误差和参考速度。 + /// + public void UpdateControlReference( + double arcLengthMeters, + double referenceSpeedMetersPerSecond, + double lateralErrorMeters, + double headingErrorRadians, + double distanceToTrajectoryMeters, + double remainingDistanceMeters) + { + lock (_stateSyncRoot) + { + _controlReferenceArcLengthMeters = + arcLengthMeters; + _controlReferenceSpeedMetersPerSecond = + referenceSpeedMetersPerSecond; + _controlLateralErrorMeters = + lateralErrorMeters; + _controlHeadingErrorRadians = + headingErrorRadians; + _controlDistanceToTrajectoryMeters = + distanceToTrajectoryMeters; + _controlRemainingDistanceMeters = + remainingDistanceMeters; + _hasControlReference = true; + } + } + // 停止采样并将本次实验保存为CSV；重复调用只保存一次。 public void StopAndSave() { @@ -224,6 +304,14 @@ namespace MultiWheelC float commandVx; float commandVy; float commandAngularSpeed; + VehicleState? processedState; + bool hasControlReference; + double controlReferenceArcLengthMeters; + double controlReferenceSpeedMetersPerSecond; + double controlLateralErrorMeters; + double controlHeadingErrorRadians; + double controlDistanceToTrajectoryMeters; + double controlRemainingDistanceMeters; lock (_commandSyncRoot) { @@ -257,6 +345,26 @@ namespace MultiWheelC } } + lock (_stateSyncRoot) + { + processedState = + _latestProcessedState; + hasControlReference = + _hasControlReference; + controlReferenceArcLengthMeters = + _controlReferenceArcLengthMeters; + controlReferenceSpeedMetersPerSecond = + _controlReferenceSpeedMetersPerSecond; + controlLateralErrorMeters = + _controlLateralErrorMeters; + controlHeadingErrorRadians = + _controlHeadingErrorRadians; + controlDistanceToTrajectoryMeters = + _controlDistanceToTrajectoryMeters; + controlRemainingDistanceMeters = + _controlRemainingDistanceMeters; + } + var sample = new TrackingSample { ElapsedSeconds = @@ -268,9 +376,50 @@ namespace MultiWheelC CommandVx = commandVx, CommandVy = commandVy, CommandAngularSpeed = - commandAngularSpeed + commandAngularSpeed, + HasProcessedState = + processedState.HasValue, + HasControlReference = + hasControlReference, + ControlReferenceArcLengthMeters = + controlReferenceArcLengthMeters, + ControlReferenceSpeedMetersPerSecond = + controlReferenceSpeedMetersPerSecond, + ControlLateralErrorMeters = + controlLateralErrorMeters, + ControlHeadingErrorRadians = + controlHeadingErrorRadians, + ControlDistanceToTrajectoryMeters = + controlDistanceToTrajectoryMeters, + ControlRemainingDistanceMeters = + controlRemainingDistanceMeters }; + if (processedState.HasValue) + { + var state = processedState.Value; + sample.StateTimestampSeconds = + state.SampleTimestampSeconds; + sample.StateXmeters = + state.PoseInWorld.XMeters; + sample.StateYMeters = + state.PoseInWorld.YMeters; + sample.StateYawRadians = + state.PoseInWorld.YawRadians; + sample.StateWorldVxMetersPerSecond = + state.TwistInWorld.VxMetersPerSecond; + sample.StateWorldVyMetersPerSecond = + state.TwistInWorld.VyMetersPerSecond; + sample.StateBodyVxMetersPerSecond = + state.TwistInBody.VxMetersPerSecond; + sample.StateBodyVyMetersPerSecond = + state.TwistInBody.VyMetersPerSecond; + sample.StateAngularSpeedRadiansPerSecond = + state.TwistInBody.OmegaRadiansPerSecond; + sample.StateVelocityEstimateValid = + state.HasValidVelocityEstimate; + } + lock (_sampleSyncRoot) { _samples.Add(sample); @@ -336,7 +485,27 @@ namespace MultiWheelC "ReferenceEndY," + "ReferenceSpeed," + "ReferenceAngularSpeedRadPerSecond," + - "ReferenceMotionFrameYawDegrees"); + "ReferenceMotionFrameYawDegrees," + + "ReferenceAccelerationMetersPerSecondSquared," + + "ReferenceDecelerationMetersPerSecondSquared," + + "HasProcessedState," + + "StateTimestampSeconds," + + "StateXMeters," + + "StateYMeters," + + "StateYawRadians," + + "StateWorldVxMetersPerSecond," + + "StateWorldVyMetersPerSecond," + + "StateBodyVxMetersPerSecond," + + "StateBodyVyMetersPerSecond," + + "StateAngularSpeedRadiansPerSecond," + + "StateVelocityEstimateValid," + + "HasControlReference," + + "ControlReferenceArcLengthMeters," + + "ControlReferenceSpeedMetersPerSecond," + + "ControlLateralErrorMeters," + + "ControlHeadingErrorRadians," + + "ControlDistanceToTrajectoryMeters," + + "ControlRemainingDistanceMeters"); foreach (var sample in snapshot) { @@ -363,7 +532,67 @@ namespace MultiWheelC Format(_referenceEnd.Y), Format(_referenceSpeed), Format(_referenceAngularSpeed), - Format(_referenceMotionFrameYawDegrees))); + Format(_referenceMotionFrameYawDegrees), + Format( + _referenceAccelerationMetersPerSecondSquared), + Format( + _referenceDecelerationMetersPerSecondSquared), + sample.HasProcessedState + ? "1" + : "0", + FormatOptional( + sample.HasProcessedState, + sample.StateTimestampSeconds), + FormatOptional( + sample.HasProcessedState, + sample.StateXmeters), + FormatOptional( + sample.HasProcessedState, + sample.StateYMeters), + FormatOptional( + sample.HasProcessedState, + sample.StateYawRadians), + FormatOptional( + sample.HasProcessedState, + sample.StateWorldVxMetersPerSecond), + FormatOptional( + sample.HasProcessedState, + sample.StateWorldVyMetersPerSecond), + FormatOptional( + sample.HasProcessedState, + sample.StateBodyVxMetersPerSecond), + FormatOptional( + sample.HasProcessedState, + sample.StateBodyVyMetersPerSecond), + FormatOptional( + sample.HasProcessedState, + sample.StateAngularSpeedRadiansPerSecond), + sample.HasProcessedState + ? sample.StateVelocityEstimateValid + ? "1" + : "0" + : string.Empty, + sample.HasControlReference + ? "1" + : "0", + FormatOptional( + sample.HasControlReference, + sample.ControlReferenceArcLengthMeters), + FormatOptional( + sample.HasControlReference, + sample.ControlReferenceSpeedMetersPerSecond), + FormatOptional( + sample.HasControlReference, + sample.ControlLateralErrorMeters), + FormatOptional( + sample.HasControlReference, + sample.ControlHeadingErrorRadians), + FormatOptional( + sample.HasControlReference, + sample.ControlDistanceToTrajectoryMeters), + FormatOptional( + sample.HasControlReference, + sample.ControlRemainingDistanceMeters))); } } } @@ -392,6 +621,18 @@ namespace MultiWheelC CultureInfo.InvariantCulture); } + /// + /// 在新版状态尚未产生时为空，否则按统一小数格式输出状态数值。 + /// + private static string FormatOptional( + bool hasValue, + double value) + { + return hasValue + ? Format(value) + : string.Empty; + } + // 对CSV文本字段进行引号和逗号转义。 private static string EscapeCsv(string value) { diff --git a/MultiWheelC/Movements/TrajectoryTrackingMovement.cs b/MultiWheelC/Movements/TrajectoryTrackingMovement.cs new file mode 100644 index 0000000..5c7a140 --- /dev/null +++ b/MultiWheelC/Movements/TrajectoryTrackingMovement.cs @@ -0,0 +1,286 @@ +using System; +using System.Collections.Generic; +using System.Diagnostics; +using ClumsyCore.Interfaces; +using ClumsyCore.Pilot; +using CommonUsage.Chassis; +using MultiWheelC.Control.Allocation; +using MultiWheelC.Control.Execution; +using MultiWheelC.Control.Lateral; +using MultiWheelC.Control.Longitudinal; +using MultiWheelC.StateEstimation; +using MultiWheelC.Trajectory; +using MyParking.Shared; + +namespace MultiWheelC +{ + /// + /// 使用新版横纵向控制器持续跟踪一条世界坐标系二维轨迹。 + /// + public sealed class TrajectoryTrackingMovement + : MovementDefinition + { + /// + /// 获取或设置本次动作需要跟踪的世界坐标系轨迹。 + /// + public Trajectory2D Trajectory; + + /// + /// 获取或设置本次动作使用的车辆状态源；为空时自动创建Detour状态源。 + /// + public IVehicleStateProvider StateProvider; + + /// + /// 获取或设置每个有效控制周期结束后的诊断数据观察回调。 + /// + public Action CycleObserver; + + /// + /// Stanley横向误差增益，单位为1/s。 + /// + public double StanleyCrossTrackGainPerSecond = 1.0; + + /// + /// Stanley航向误差增益。 + /// + public double StanleyHeadingErrorGain = 1.0; + + /// + /// Stanley低速分母保护速度，单位为m/s。 + /// + public double StanleyMinimumSpeedMetersPerSecond = 0.05; + + /// + /// 获取或设置Stanley是否优先使用Detour估算的实际速度。 + /// + public bool StanleyUsesActualSpeed = true; + + /// + /// 纵向速度外环比例增益。 + /// + public double LongitudinalKp = 0.5; + + /// + /// 纵向速度外环积分增益，单位为1/s。 + /// + public double LongitudinalKiPerSecond; + + /// + /// 纵向速度外环微分增益，单位为s。 + /// + public double LongitudinalKdSeconds; + + /// + /// 纵向积分项允许产生的最大速度修正绝对值，单位为m/s。 + /// + public double MaximumIntegralCorrectionMetersPerSecond = 0.05; + + /// + /// 底盘纵向命令速度绝对值上限，单位为m/s。 + /// + public double MaximumCommandSpeedMetersPerSecond = 0.50; + + /// + /// 前后GCP允许的最大转角绝对值，单位为rad。 + /// + public double MaximumGcpAngleRadians = + AngleMath.DegreesToRadians(45.0); + + /// + /// 前后GCP目标转角最大变化率，单位为rad/s。 + /// + public double MaximumGcpAngleRateRadiansPerSecond = + AngleMath.DegreesToRadians(10.0); + + /// + /// 终点位置和剩余弧长的完成容差，单位为m。 + /// + public double FinishDistanceMeters = 0.03; + + /// + /// 终点停稳判定允许的实际线速度，单位为m/s。 + /// + public double FinishSpeedMetersPerSecond = 0.02; + + /// + /// 终点航向完成容差，单位为rad。 + /// + public double FinishHeadingToleranceRadians = + AngleMath.DegreesToRadians(3.0); + + /// + /// 车辆允许偏离参考轨迹的最大欧氏距离，单位为m。 + /// + public double MaximumDistanceToTrajectoryMeters = 0.50; + + /// + /// 单次轨迹动作允许的最长执行时间，单位为s。 + /// + public double ExecutionTimeoutSeconds = 120.0; + + /// + /// 获取本次动作创建的控制器，尚未开始时为空。 + /// + public ParkingGeometricController Controller { get; private set; } + + /// + /// 创建控制器并持续执行控制周期，直到轨迹完成、失败或动作被取消。 + /// + public override IEnumerable Get() + { + ValidateParameters(); + + var chassis = + PilotDefinition.Chassis as MultiWheelChassis; + if (chassis == null) + { + throw new InvalidOperationException( + "当前底盘不是MultiWheelChassis，无法执行新版轨迹跟踪动作。"); + } + + var adapter = new MultiWheelChassisAdapter( + chassis, + PilotDefinition.Self.CarNum); + + // 新版GCP控制统一以真实车头为车体X正方向，避免继承上一次蟹行偏置。 + adapter.ResetToBodyFrame(); + + var stateProvider = + StateProvider ?? + new DetourVehicleStateProvider(); + var controlPointRadiusMeters = + chassis.ControlPointRadius / 1000.0; + + var lateralController = + new StanleyLateralController( + controlPointRadiusMeters, + StanleyCrossTrackGainPerSecond, + StanleyHeadingErrorGain, + StanleyMinimumSpeedMetersPerSecond, + StanleyUsesActualSpeed); + var longitudinalController = + new PidLongitudinalController( + LongitudinalKp, + LongitudinalKiPerSecond, + LongitudinalKdSeconds, + MaximumIntegralCorrectionMetersPerSecond, + MaximumCommandSpeedMetersPerSecond); + var gcpAllocator = + new AckermannGcpAllocator( + controlPointRadiusMeters, + MaximumGcpAngleRadians); + var commandExecutor = + new GcpCommandExecutor( + adapter, + MaximumGcpAngleRateRadiansPerSecond); + + Controller = new ParkingGeometricController( + stateProvider, + lateralController, + longitudinalController, + gcpAllocator, + commandExecutor, + FinishDistanceMeters, + FinishSpeedMetersPerSecond, + FinishHeadingToleranceRadians, + MaximumDistanceToTrajectoryMeters); + + var clock = Stopwatch.StartNew(); + var previousCycleSeconds = + clock.Elapsed.TotalSeconds; + Controller.Start(Trajectory); + + try + { + while (true) + { + if (clock.Elapsed.TotalSeconds > + ExecutionTimeoutSeconds) + { + throw new TimeoutException( + $"新版轨迹跟踪超过{ExecutionTimeoutSeconds:F1}s仍未完成。"); + } + + var currentCycleSeconds = + clock.Elapsed.TotalSeconds; + var deltaTimeSeconds = + currentCycleSeconds - + previousCycleSeconds; + previousCycleSeconds = + currentCycleSeconds; + + // 极短首周期不参与PID和GCP角速度限制，等待调度器进入下一周期。 + if (deltaTimeSeconds <= 1e-6) + { + yield return true; + continue; + } + + var result = + Controller.ExecuteCycle( + deltaTimeSeconds); + + if (Controller.LastVehicleState.HasValue) + { + CycleObserver?.Invoke(Controller); + } + + if (result == + ParkingControlCycleResult.Completed) + { + break; + } + + if (result == + ParkingControlCycleResult.Faulted) + { + throw new InvalidOperationException( + string.IsNullOrWhiteSpace( + Controller.LastFailureReason) + ? "新版轨迹跟踪控制器发生未知故障。" + : Controller.LastFailureReason, + Controller.LastException); + } + + if (result == + ParkingControlCycleResult.Inactive) + { + throw new InvalidOperationException( + "新版轨迹跟踪控制器在轨迹完成前意外停止活动。"); + } + + // CommandSent和短暂StateUnavailable均继续下一控制周期； + // 后者已经由控制器主动停车，等待Detour恢复。 + yield return true; + } + } + finally + { + Controller.Cancel(); + } + + yield return false; + } + + /// + /// 在接管实际底盘前检查动作自身无法由子控制器检查的参数。 + /// + private void ValidateParameters() + { + if (Trajectory == null) + { + throw new InvalidOperationException( + "新版轨迹跟踪动作没有设置Trajectory。"); + } + + if (double.IsNaN(ExecutionTimeoutSeconds) || + double.IsInfinity(ExecutionTimeoutSeconds) || + ExecutionTimeoutSeconds <= 0.0) + { + throw new ArgumentOutOfRangeException( + nameof(ExecutionTimeoutSeconds), + "轨迹跟踪超时时间必须是正有限值。"); + } + } + } +} diff --git a/MultiWheelC/Trajectory/Trajectory2D.cs b/MultiWheelC/Trajectory/Trajectory2D.cs index c4d6fe0..a991bde 100644 --- a/MultiWheelC/Trajectory/Trajectory2D.cs +++ b/MultiWheelC/Trajectory/Trajectory2D.cs @@ -2,6 +2,7 @@ using System; using System.Collections.Generic; using System.Collections.ObjectModel; using System.Linq; +using MyParking.Shared; namespace MultiWheelC.Trajectory { @@ -114,6 +115,93 @@ namespace MultiWheelC.Trajectory return TotalLengthMeters - arcLengthMeters; } + /// + /// 按累计弧长在线性位置、航向、曲率和参考速度之间插值得到轨迹点。 + /// + public TrajectoryPoint SampleAtArcLength( + double arcLengthMeters) + { + EnsureFinite( + arcLengthMeters, + nameof(arcLengthMeters)); + + if (arcLengthMeters <= 0.0) + { + return StartPoint; + } + + if (arcLengthMeters >= TotalLengthMeters) + { + return EndPoint; + } + + var segmentStartIndex = + FindSegmentStartIndex( + arcLengthMeters); + var segmentStart = + _points[segmentStartIndex]; + var segmentEnd = + _points[segmentStartIndex + 1]; + var interpolationRatio = + (arcLengthMeters - + segmentStart.ArcLengthMeters) / + (segmentEnd.ArcLengthMeters - + segmentStart.ArcLengthMeters); + + return new TrajectoryPoint( + arcLengthMeters, + new Pose2D( + InterpolationMath.Lerp( + segmentStart.PoseInWorld.XMeters, + segmentEnd.PoseInWorld.XMeters, + interpolationRatio), + InterpolationMath.Lerp( + segmentStart.PoseInWorld.YMeters, + segmentEnd.PoseInWorld.YMeters, + interpolationRatio), + AngleMath.LerpRadians( + segmentStart.PoseInWorld.YawRadians, + segmentEnd.PoseInWorld.YawRadians, + interpolationRatio)), + InterpolationMath.Lerp( + segmentStart.CurvaturePerMeter, + segmentEnd.CurvaturePerMeter, + interpolationRatio), + InterpolationMath.Lerp( + segmentStart.ReferenceSpeedMetersPerSecond, + segmentEnd.ReferenceSpeedMetersPerSecond, + interpolationRatio)); + } + + /// + /// 使用二分查找获取包含指定累计弧长的线段起点索引。 + /// + private int FindSegmentStartIndex( + double arcLengthMeters) + { + var lowerIndex = 0; + var upperIndex = _points.Length - 1; + + while (upperIndex - lowerIndex > 1) + { + var middleIndex = + lowerIndex + + (upperIndex - lowerIndex) / 2; + + if (_points[middleIndex].ArcLengthMeters <= + arcLengthMeters) + { + lowerIndex = middleIndex; + } + else + { + upperIndex = middleIndex; + } + } + + return lowerIndex; + } + /// /// 检查相邻轨迹点是否构成有效的非零长度有序线段。 /// diff --git a/Shared/Chassis/MultiWheelChassisAdapter.cs b/Shared/Chassis/MultiWheelChassisAdapter.cs index 8a73a54..a23224f 100644 --- a/Shared/Chassis/MultiWheelChassisAdapter.cs +++ b/Shared/Chassis/MultiWheelChassisAdapter.cs @@ -351,6 +351,53 @@ namespace MyParking.Shared return success; } + /// + /// 在真实车体坐标系中将有符号速度和独立前后GCP角度发送给旧版SendMotion。 + /// + public bool SendGcpMotion( + double speedMetersPerSecond, + double frontAngleRadians, + double rearAngleRadians, + TimeSpan? interval = null) + { + ValidateFinite( + speedMetersPerSecond, + nameof(speedMetersPerSecond)); + ValidateFinite( + frontAngleRadians, + nameof(frontAngleRadians)); + ValidateFinite( + rearAngleRadians, + nameof(rearAngleRadians)); + EnsureBodyFrameIsActive(); + + if (Math.Abs(frontAngleRadians) >= + Math.PI / 2.0 || + Math.Abs(rearAngleRadians) >= + Math.PI / 2.0) + { + throw new ArgumentOutOfRangeException( + nameof(frontAngleRadians), + "前后GCP角度必须位于正负90度以内，避免四轮几何解算出现奇异值。"); + } + + var success = _chassis.SendMotion( + (float)speedMetersPerSecond, + (float)(frontAngleRadians * + RadiansToDegrees), + (float)(rearAngleRadians * + RadiansToDegrees), + interval); + + if (!success) + { + // 分解失败后立即清除上一条驱动速度，避免车辆继续执行陈旧命令。 + _chassis.PredefinedDriveStop(); + } + + return success; + } + /// /// 立即将所有驱动轮速度下发为零。 /// diff --git a/data_process/新版控制器轨迹测试处理/plot_new_controller_experiment.py b/data_process/新版控制器轨迹测试处理/plot_new_controller_experiment.py new file mode 100644 index 0000000..a483532 --- /dev/null +++ b/data_process/新版控制器轨迹测试处理/plot_new_controller_experiment.py @@ -0,0 +1,487 @@ +"""为新版4m直线控制器实验CSV生成轨迹、横向/航向误差和速度响应图。""" + +from __future__ import annotations + +import argparse +from pathlib import Path + +import matplotlib + +matplotlib.use("Agg") +import matplotlib.pyplot as plt +import numpy as np +import pandas as pd + + +SCRIPT_DIR = Path(__file__).resolve().parent + + +def configure_matplotlib() -> None: + """配置可显示中文和负号的Matplotlib字体。""" + plt.rcParams["font.sans-serif"] = [ + "Microsoft YaHei", + "SimHei", + "Noto Sans CJK SC", + "Arial Unicode MS", + "DejaVu Sans", + ] + plt.rcParams["axes.unicode_minus"] = False + + +def numeric_column( + frame: pd.DataFrame, + name: str, + default: float = np.nan, +) -> np.ndarray: + """将CSV列安全转换为浮点数组，缺失列使用指定默认值。""" + if name not in frame.columns: + return np.full(len(frame), default, dtype=float) + return pd.to_numeric(frame[name], errors="coerce").to_numpy( + dtype=float, + copy=True, + ) + + +def first_finite(values: np.ndarray, default: float) -> float: + """读取数组中的第一个有限值。""" + finite = values[np.isfinite(values)] + return float(finite[0]) if finite.size else default + + +def first_text(frame: pd.DataFrame, name: str, default: str) -> str: + """读取文本元数据列中的第一个非空值。""" + if name not in frame.columns: + return default + values = frame[name].dropna().astype(str) + values = values[values.str.strip() != ""] + return values.iloc[0] if not values.empty else default + + +def fill_reference_series( + values: np.ndarray, + fallback: np.ndarray, +) -> np.ndarray: + """前后填充后台采样得到的控制参考值，缺失时使用解析速度曲线。""" + series = pd.Series(values, dtype=float) + filled = series.ffill().bfill().to_numpy( + dtype=float, + copy=True, + ) + missing = ~np.isfinite(filled) + filled[missing] = fallback[missing] + return filled + + +def planned_motion( + time_seconds: np.ndarray, + length_meters: float, + cruise_speed_mps: float, + acceleration_mps2: float, + deceleration_mps2: float, +) -> tuple[np.ndarray, np.ndarray]: + """计算从静止出发并在终点静止的梯形或三角形理想时间速度轨迹。""" + acceleration_distance = ( + cruise_speed_mps**2 / (2.0 * acceleration_mps2) + ) + deceleration_distance = ( + cruise_speed_mps**2 / (2.0 * deceleration_mps2) + ) + + if acceleration_distance + deceleration_distance <= length_meters: + peak_speed = cruise_speed_mps + else: + peak_speed = np.sqrt( + 2.0 + * length_meters + / (1.0 / acceleration_mps2 + 1.0 / deceleration_mps2) + ) + acceleration_distance = ( + peak_speed**2 / (2.0 * acceleration_mps2) + ) + deceleration_distance = ( + peak_speed**2 / (2.0 * deceleration_mps2) + ) + + acceleration_time = peak_speed / acceleration_mps2 + deceleration_time = peak_speed / deceleration_mps2 + cruise_distance = max( + 0.0, + length_meters - acceleration_distance - deceleration_distance, + ) + cruise_time = cruise_distance / peak_speed + deceleration_start_time = acceleration_time + cruise_time + finish_time = deceleration_start_time + deceleration_time + + progress = np.zeros_like(time_seconds, dtype=float) + speed = np.zeros_like(time_seconds, dtype=float) + + accelerating = time_seconds <= acceleration_time + progress[accelerating] = ( + 0.5 * acceleration_mps2 * time_seconds[accelerating] ** 2 + ) + speed[accelerating] = acceleration_mps2 * time_seconds[accelerating] + + cruising = ( + (time_seconds > acceleration_time) + & (time_seconds <= deceleration_start_time) + ) + progress[cruising] = ( + acceleration_distance + + peak_speed * (time_seconds[cruising] - acceleration_time) + ) + speed[cruising] = peak_speed + + decelerating = ( + (time_seconds > deceleration_start_time) + & (time_seconds <= finish_time) + ) + remaining_time = finish_time - time_seconds[decelerating] + progress[decelerating] = ( + length_meters + - 0.5 * deceleration_mps2 * remaining_time**2 + ) + speed[decelerating] = deceleration_mps2 * remaining_time + + finished = time_seconds > finish_time + progress[finished] = length_meters + speed[finished] = 0.0 + return progress, speed + + +def load_experiment(csv_path: Path) -> dict[str, object]: + """读取新版CSV并构造绘图所需的统一SI单位数据。""" + frame = pd.read_csv(csv_path, encoding="utf-8-sig") + if frame.empty: + raise ValueError("CSV没有任何采样行。") + + time_seconds = numeric_column(frame, "ElapsedSeconds") + valid_time = np.isfinite(time_seconds) + frame = frame.loc[valid_time].reset_index(drop=True) + time_seconds = time_seconds[valid_time] + if time_seconds.size < 2: + raise ValueError("CSV中的有效时间采样不足2帧。") + time_seconds = time_seconds - time_seconds[0] + + state_x = numeric_column(frame, "StateXMeters") + state_y = numeric_column(frame, "StateYMeters") + has_processed = numeric_column(frame, "HasProcessedState", 0.0) > 0.5 + processed_valid = has_processed & np.isfinite(state_x) & np.isfinite(state_y) + + raw_x_meters = numeric_column(frame, "DetourX") / 1000.0 + raw_y_meters = numeric_column(frame, "DetourY") / 1000.0 + actual_x = np.where(processed_valid, state_x, raw_x_meters) + actual_y = np.where(processed_valid, state_y, raw_y_meters) + valid_position = np.isfinite(actual_x) & np.isfinite(actual_y) + if np.count_nonzero(valid_position) < 2: + raise ValueError("CSV中没有足够的有效车辆位置。") + + start = np.array( + [ + first_finite(numeric_column(frame, "ReferenceStartX"), np.nan), + first_finite(numeric_column(frame, "ReferenceStartY"), np.nan), + ], + dtype=float, + ) / 1000.0 + end = np.array( + [ + first_finite(numeric_column(frame, "ReferenceEndX"), np.nan), + first_finite(numeric_column(frame, "ReferenceEndY"), np.nan), + ], + dtype=float, + ) / 1000.0 + if not np.all(np.isfinite(start)) or not np.all(np.isfinite(end)): + raise ValueError("CSV缺少有效的参考起点或终点。") + + line = end - start + length_meters = float(np.linalg.norm(line)) + if length_meters <= 1e-6: + raise ValueError("参考直线长度必须大于0。") + tangent = line / length_meters + left_normal = np.array([-tangent[1], tangent[0]]) + displacement = np.column_stack([actual_x, actual_y]) - start + # 与C# TrajectoryProjector保持一致：轨迹位于车辆左侧时为正。 + derived_lateral_error = -(displacement @ left_normal) + recorded_lateral_error = numeric_column( + frame, + "ControlLateralErrorMeters", + ) + has_control_reference = ( + numeric_column(frame, "HasControlReference", 0.0) > 0.5 + ) + lateral_error = np.where( + has_control_reference & np.isfinite(recorded_lateral_error), + recorded_lateral_error, + derived_lateral_error, + ) + + state_yaw = numeric_column(frame, "StateYawRadians") + raw_yaw = np.deg2rad(numeric_column(frame, "DetourTheta")) + actual_yaw = np.where( + has_processed & np.isfinite(state_yaw), + state_yaw, + raw_yaw, + ) + reference_yaw = np.arctan2(tangent[1], tangent[0]) + derived_heading_error = np.arctan2( + np.sin(reference_yaw - actual_yaw), + np.cos(reference_yaw - actual_yaw), + ) + recorded_heading_error = numeric_column( + frame, + "ControlHeadingErrorRadians", + ) + heading_error = np.where( + has_control_reference & np.isfinite(recorded_heading_error), + recorded_heading_error, + derived_heading_error, + ) + + cruise_speed = first_finite( + numeric_column(frame, "ReferenceSpeed"), + 0.30, + ) + acceleration = first_finite( + numeric_column( + frame, + "ReferenceAccelerationMetersPerSecondSquared", + ), + 0.20, + ) + deceleration = first_finite( + numeric_column( + frame, + "ReferenceDecelerationMetersPerSecondSquared", + ), + 0.20, + ) + if acceleration <= 0.0: + acceleration = 0.20 + if deceleration <= 0.0: + deceleration = 0.20 + + _, ideal_speed = planned_motion( + time_seconds, + length_meters, + cruise_speed, + acceleration, + deceleration, + ) + reference_speed = fill_reference_series( + numeric_column(frame, "ControlReferenceSpeedMetersPerSecond"), + ideal_speed, + ) + actual_speed = numeric_column(frame, "StateBodyVxMetersPerSecond") + velocity_valid = ( + numeric_column(frame, "StateVelocityEstimateValid", 0.0) > 0.5 + ) + actual_speed[~velocity_valid] = np.nan + command_speed = numeric_column(frame, "CommandSpeed") + + return { + "frame": frame, + "time": time_seconds, + "actual_x": actual_x, + "actual_y": actual_y, + "valid_position": valid_position, + "start": start, + "end": end, + "length": length_meters, + "lateral_error": lateral_error, + "heading_error": heading_error, + "reference_speed": reference_speed, + "actual_speed": actual_speed, + "command_speed": command_speed, + "controller_name": first_text( + frame, + "ControllerName", + "NewController", + ), + "trajectory_name": first_text( + frame, + "TrajectoryName", + "Trajectory", + ), + } + + +def finite_rmse(values: np.ndarray) -> float: + """计算忽略无效样本后的均方根值。""" + finite = values[np.isfinite(values)] + return float(np.sqrt(np.mean(finite**2))) if finite.size else np.nan + + +def save_figure( + fig: plt.Figure, + destination: Path, + show: bool, +) -> None: + """保存并关闭一张实验图。""" + fig.tight_layout() + fig.savefig(destination, dpi=300, bbox_inches="tight") + if show: + plt.show() + plt.close(fig) + + +def plot_experiment( + csv_path: Path, + output_directory: Path, + show: bool, +) -> list[Path]: + """为单份新版控制器CSV生成四类对比图。""" + data = load_experiment(csv_path) + output_directory.mkdir(parents=True, exist_ok=True) + title = f"{data['controller_name']} - {data['trajectory_name']}" + destinations: list[Path] = [] + + valid_position = data["valid_position"] + fig, axis = plt.subplots(figsize=(9.0, 6.5)) + axis.plot( + [data["start"][0], data["end"][0]], + [data["start"][1], data["end"][1]], + "--", + linewidth=2.0, + label="期望4m直线轨迹", + ) + axis.plot( + data["actual_x"][valid_position], + data["actual_y"][valid_position], + linewidth=1.5, + label="状态估计后的实际轨迹", + ) + axis.scatter(*data["start"], color="green", s=45, label="起点") + axis.scatter(*data["end"], color="red", s=45, label="终点") + axis.set_aspect("equal", adjustable="box") + axis.set_xlabel("世界坐标X / m") + axis.set_ylabel("世界坐标Y / m") + axis.set_title(f"期望轨迹与实际轨迹对比\n{title}") + axis.grid(True, alpha=0.3) + axis.legend() + destination = output_directory / f"{csv_path.stem}_trajectory.png" + save_figure(fig, destination, show) + destinations.append(destination) + + lateral_mm = data["lateral_error"] * 1000.0 + lateral_rmse_mm = finite_rmse(lateral_mm) + fig, axis = plt.subplots(figsize=(10.0, 5.5)) + axis.plot(data["time"], lateral_mm, linewidth=1.5) + axis.axhline(0.0, color="black", linewidth=0.8) + axis.set_xlabel("时间 / s") + axis.set_ylabel("横向误差 / mm") + axis.set_title( + f"横向误差（轨迹在车辆左侧为正）\n{title}，RMSE={lateral_rmse_mm:.2f}mm" + ) + axis.grid(True, alpha=0.3) + destination = output_directory / f"{csv_path.stem}_lateral_error.png" + save_figure(fig, destination, show) + destinations.append(destination) + + heading_degrees = np.rad2deg(data["heading_error"]) + heading_rmse_degrees = finite_rmse(heading_degrees) + fig, axis = plt.subplots(figsize=(10.0, 5.5)) + axis.plot(data["time"], heading_degrees, linewidth=1.5) + axis.axhline(0.0, color="black", linewidth=0.8) + axis.set_xlabel("时间 / s") + axis.set_ylabel("航向角偏差 / °") + axis.set_title( + "航向角偏差：参考轨迹航向－实际车体航向（逆时针为正）\n" + f"{title}，RMSE={heading_rmse_degrees:.3f}°" + ) + axis.grid(True, alpha=0.3) + destination = output_directory / f"{csv_path.stem}_heading_error.png" + save_figure(fig, destination, show) + destinations.append(destination) + + speed_error = data["actual_speed"] - data["reference_speed"] + speed_rmse = finite_rmse(speed_error) + fig, axis = plt.subplots(figsize=(10.0, 5.8)) + axis.plot( + data["time"], + data["reference_speed"], + linewidth=1.8, + label="控制器实际参考速度", + ) + axis.plot( + data["time"], + data["command_speed"], + "--", + linewidth=1.3, + label="纵向控制器下发速度", + ) + axis.plot( + data["time"], + data["actual_speed"], + linewidth=1.5, + label="状态估计实际车体纵向速度", + ) + axis.set_xlabel("时间 / s") + axis.set_ylabel("速度 / (m/s)") + axis.set_title(f"参考速度与实际速度对比\n{title}，RMSE={speed_rmse:.4f}m/s") + axis.grid(True, alpha=0.3) + axis.legend() + destination = output_directory / f"{csv_path.stem}_speed_response.png" + save_figure(fig, destination, show) + destinations.append(destination) + + print( + f"{csv_path.name}: 横向RMSE={lateral_rmse_mm:.3f}mm, " + f"航向RMSE={heading_rmse_degrees:.4f}°, " + f"速度RMSE={speed_rmse:.5f}m/s" + ) + for destination in destinations: + print(f"已生成：{destination}") + return destinations + + +def discover_csv_files(arguments: list[str]) -> list[Path]: + """读取命令行文件；未指定时扫描脚本目录及data子目录中的CSV。""" + if arguments: + files = [Path(item).expanduser().resolve() for item in arguments] + else: + files = sorted(SCRIPT_DIR.glob("*.csv")) + files.extend(sorted((SCRIPT_DIR / "data").glob("*.csv"))) + files = [path for path in files if path.is_file()] + if not files: + raise FileNotFoundError( + "没有找到CSV；请传入文件路径，或将文件放到脚本目录/data中。" + ) + return files + + +def main() -> None: + """解析命令行并批量处理新版控制器实验CSV。""" + parser = argparse.ArgumentParser( + description="绘制新版控制器4m直线实验的四类对比图。" + ) + parser.add_argument("csv", nargs="*", help="需要处理的CSV文件路径。") + parser.add_argument( + "--output-dir", + help="图片输出目录；默认使用脚本目录/plots。", + ) + parser.add_argument( + "--show", + action="store_true", + help="保存图片后同时显示窗口。", + ) + arguments = parser.parse_args() + configure_matplotlib() + + output_directory = ( + Path(arguments.output_dir).expanduser().resolve() + if arguments.output_dir + else SCRIPT_DIR / "plots" + ) + failed = 0 + for csv_path in discover_csv_files(arguments.csv): + try: + plot_experiment(csv_path, output_directory, arguments.show) + except Exception as exception: + failed += 1 + print(f"处理失败：{csv_path}：{exception}") + + if failed: + raise SystemExit(f"共有{failed}个CSV处理失败。") + + +if __name__ == "__main__": + main() diff --git a/data_process/新版控制器轨迹测试处理/requirements.txt b/data_process/新版控制器轨迹测试处理/requirements.txt new file mode 100644 index 0000000..60f0133 --- /dev/null +++ b/data_process/新版控制器轨迹测试处理/requirements.txt @@ -0,0 +1,3 @@ +numpy>=1.26 +pandas>=2.2 +matplotlib>=3.8 diff --git a/data_process/轨迹测试处理/compare_state_estimation.py b/data_process/旧版控制器轨迹测试处理/compare_state_estimation.py similarity index 100% rename from data_process/轨迹测试处理/compare_state_estimation.py rename to data_process/旧版控制器轨迹测试处理/compare_state_estimation.py diff --git a/data_process/轨迹测试处理/plot_angular_command.py b/data_process/旧版控制器轨迹测试处理/plot_angular_command.py similarity index 100% rename from data_process/轨迹测试处理/plot_angular_command.py rename to data_process/旧版控制器轨迹测试处理/plot_angular_command.py diff --git a/data_process/轨迹测试处理/plot_speed_response.py b/data_process/旧版控制器轨迹测试处理/plot_speed_response.py similarity index 100% rename from data_process/轨迹测试处理/plot_speed_response.py rename to data_process/旧版控制器轨迹测试处理/plot_speed_response.py diff --git a/data_process/轨迹测试处理/plot_tracking_errors.py b/data_process/旧版控制器轨迹测试处理/plot_tracking_errors.py similarity index 100% rename from data_process/轨迹测试处理/plot_tracking_errors.py rename to data_process/旧版控制器轨迹测试处理/plot_tracking_errors.py diff --git a/data_process/轨迹测试处理/plot_trajectory_comparison.py b/data_process/旧版控制器轨迹测试处理/plot_trajectory_comparison.py similarity index 100% rename from data_process/轨迹测试处理/plot_trajectory_comparison.py rename to data_process/旧版控制器轨迹测试处理/plot_trajectory_comparison.py diff --git a/data_process/轨迹测试处理/requirements.txt b/data_process/旧版控制器轨迹测试处理/requirements.txt similarity index 100% rename from data_process/轨迹测试处理/requirements.txt rename to data_process/旧版控制器轨迹测试处理/requirements.txt diff --git a/data_process/轨迹测试处理/run_all_plots.py b/data_process/旧版控制器轨迹测试处理/run_all_plots.py similarity index 100% rename from data_process/轨迹测试处理/run_all_plots.py rename to data_process/旧版控制器轨迹测试处理/run_all_plots.py diff --git a/docs/记录.txt b/docs/记录.txt index 7d90dc9..a842a20 100644 --- a/docs/记录.txt +++ b/docs/记录.txt @@ -2,7 +2,6 @@ powershell -NoProfile -ExecutionPolicy Bypass -File .\build-and-package.ps1 - 1. TrajectoryPoint.cs 已完成 2. Trajectory2D.cs 下一步 3. TrajectoryProjection.cs 定义一次投影结果 @@ -20,9 +19,19 @@ powershell -NoProfile -ExecutionPolicy Bypass -File .\build-and-package.ps1 5. DetourVehicleStateProvider.cs - private const double LinearVelocityFilterTimeConstantSeconds = - 0.10; + 0.15; private const double AngularVelocityFilterTimeConstantSeconds = - 0.10; \ No newline at end of file + 0.20; + +PathTrackingContext.cs +LateralControlCommand.cs +ILateralController.cs +ILongitudinalController.cs +GcpMotionCommand.cs +AckermannGcpAllocator.cs +StanleyLateralController.cs +PidLongitudinalController.cs +GcpCommandExecutor.cs +ParkingGeometricController.cs