diff --git a/ClumsyPilot/ClumsyPilot.csproj b/ClumsyPilot/ClumsyPilot.csproj index cbff9fa..21e8698 100644 --- a/ClumsyPilot/ClumsyPilot.csproj +++ b/ClumsyPilot/ClumsyPilot.csproj @@ -23,6 +23,9 @@ + + + diff --git a/ClumsyPilot/Control/Abstractions/ILateralController.cs b/ClumsyPilot/Control/Abstractions/ILateralController.cs new file mode 100644 index 0000000..25fbcaf --- /dev/null +++ b/ClumsyPilot/Control/Abstractions/ILateralController.cs @@ -0,0 +1,19 @@ +namespace MultiWheelC.Control.Abstractions +{ + /// + /// 定义Stanley、LQR和MPC等车体中心横向控制器的统一接口。 + /// + public interface ILateralController + { + /// + /// 根据本周期车辆状态和轨迹误差计算车体中心目标曲率。 + /// + LateralControlCommand Compute( + PathTrackingContext context); + + /// + /// 清除控制器跨周期状态，以便开始新轨迹或异常恢复后重新运行。 + /// + void Reset(); + } +} diff --git a/ClumsyPilot/Control/Abstractions/ILongitudinalController.cs b/ClumsyPilot/Control/Abstractions/ILongitudinalController.cs new file mode 100644 index 0000000..810eda3 --- /dev/null +++ b/ClumsyPilot/Control/Abstractions/ILongitudinalController.cs @@ -0,0 +1,19 @@ +namespace MultiWheelC.Control.Abstractions +{ + /// + /// 定义根据参考速度和实际纵向速度生成底盘命令速度的统一接口。 + /// + public interface ILongitudinalController + { + /// + /// 根据本周期速度目标、速度反馈和时间间隔计算有符号底盘命令速度。 + /// + double ComputeSpeedMetersPerSecond( + PathTrackingContext context); + + /// + /// 清除积分、历史误差和其他跨周期状态，以便安全开始新的控制过程。 + /// + void Reset(); + } +} diff --git a/ClumsyPilot/Control/Abstractions/LateralControlCommand.cs b/ClumsyPilot/Control/Abstractions/LateralControlCommand.cs new file mode 100644 index 0000000..cb170c9 --- /dev/null +++ b/ClumsyPilot/Control/Abstractions/LateralControlCommand.cs @@ -0,0 +1,76 @@ +using System; + +namespace MultiWheelC.Control.Abstractions +{ + /// + /// 表示横向控制器生成的前、后GCP目标转角，单位为rad，逆时针为正。 + /// + public readonly struct LateralControlCommand + { + /// + /// 创建前、后GCP目标转角命令。 + /// + public LateralControlCommand( + double frontGcpAngleRadians, + double rearGcpAngleRadians) + { + EnsureFinite( + frontGcpAngleRadians, + nameof(frontGcpAngleRadians)); + EnsureFinite( + rearGcpAngleRadians, + nameof(rearGcpAngleRadians)); + + FrontGcpAngleRadians = + frontGcpAngleRadians; + RearGcpAngleRadians = + rearGcpAngleRadians; + } + + /// + /// 获取前GCP目标转角，单位为rad，逆时针为正。 + /// + public double FrontGcpAngleRadians { get; } + + /// + /// 获取后GCP目标转角，单位为rad，逆时针为正。 + /// + public double RearGcpAngleRadians { get; } + + /// + /// 获取前后GCP的共同转角分量，主要用于横向平移修正。 + /// + public double CommonAngleRadians => + (FrontGcpAngleRadians + + RearGcpAngleRadians) / 2.0; + + /// + /// 获取前后GCP的差动转角分量，主要用于曲率前馈和航向修正。 + /// + public double DifferentialAngleRadians => + (FrontGcpAngleRadians - + RearGcpAngleRadians) / 2.0; + + /// + /// 创建前后GCP均保持车头方向的直线命令。 + /// + public static LateralControlCommand Straight => + new LateralControlCommand(0.0, 0.0); + + /// + /// 检查GCP目标转角是否为有限值。 + /// + private static void EnsureFinite( + double value, + string parameterName) + { + if (double.IsNaN(value) || + double.IsInfinity(value)) + { + throw new ArgumentOutOfRangeException( + parameterName, + "GCP目标转角必须是有限值。"); + } + } + } +} diff --git a/ClumsyPilot/Control/Abstractions/PathTrackingContext.cs b/ClumsyPilot/Control/Abstractions/PathTrackingContext.cs new file mode 100644 index 0000000..578468a --- /dev/null +++ b/ClumsyPilot/Control/Abstractions/PathTrackingContext.cs @@ -0,0 +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/ClumsyPilot/Control/Allocation/GcpCommandAllocator.cs b/ClumsyPilot/Control/Allocation/GcpCommandAllocator.cs new file mode 100644 index 0000000..77ccdca --- /dev/null +++ b/ClumsyPilot/Control/Allocation/GcpCommandAllocator.cs @@ -0,0 +1,104 @@ +using System; +using MultiWheelC.Control.Abstractions; + +namespace MultiWheelC.Control.Allocation +{ + /// + /// 独立限制前后GCP目标转角并与纵向速度组合成底盘运动命令。 + /// + public sealed class GcpCommandAllocator + { + /// + /// 创建使用指定前后GCP最大转角的命令分配器。 + /// + public GcpCommandAllocator(double maximumGcpAngleRadians) + { + EnsureFinitePositive( + maximumGcpAngleRadians, + nameof(maximumGcpAngleRadians)); + + if (maximumGcpAngleRadians >= Math.PI / 2.0) + { + throw new ArgumentOutOfRangeException( + nameof(maximumGcpAngleRadians), + "最大GCP转角必须小于π/2。"); + } + + MaximumGcpAngleRadians = maximumGcpAngleRadians; + } + + /// + /// 获取前后GCP允许的最大转角绝对值，单位为rad。 + /// + public double MaximumGcpAngleRadians { get; } + + /// + /// 将纵向速度和前后GCP转角组合为底盘运动命令。 + /// + public GcpMotionCommand Allocate( + double speedMetersPerSecond, + LateralControlCommand lateralCommand) + { + EnsureFinite( + speedMetersPerSecond, + nameof(speedMetersPerSecond)); + + var frontAngleRadians = ClampSymmetric( + lateralCommand.FrontGcpAngleRadians, + MaximumGcpAngleRadians); + var rearAngleRadians = ClampSymmetric( + lateralCommand.RearGcpAngleRadians, + MaximumGcpAngleRadians); + + return new GcpMotionCommand( + speedMetersPerSecond, + frontAngleRadians, + rearAngleRadians); + } + + /// + /// 将数值按正负对称方式限制在指定绝对值内。 + /// + private static double ClampSymmetric( + double value, + double maximumAbsoluteValue) + { + return Math.Max( + -maximumAbsoluteValue, + Math.Min(maximumAbsoluteValue, value)); + } + + /// + /// 检查参数是否为正有限值。 + /// + private static void EnsureFinitePositive( + double value, + string parameterName) + { + EnsureFinite(value, parameterName); + + if (value <= 0.0) + { + throw new ArgumentOutOfRangeException( + parameterName, + "GCP分配参数必须是正有限值。"); + } + } + + /// + /// 检查参数或命令是否为有限值。 + /// + private static void EnsureFinite( + double value, + string parameterName) + { + if (double.IsNaN(value) || + double.IsInfinity(value)) + { + throw new ArgumentOutOfRangeException( + parameterName, + "GCP分配参数和命令必须是有限值。"); + } + } + } +} diff --git a/ClumsyPilot/Control/Allocation/GcpMotionCommand.cs b/ClumsyPilot/Control/Allocation/GcpMotionCommand.cs new file mode 100644 index 0000000..f36ce0e --- /dev/null +++ b/ClumsyPilot/Control/Allocation/GcpMotionCommand.cs @@ -0,0 +1,67 @@ +using System; + +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)); + + 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/ClumsyPilot/Control/Common/PidController.cs b/ClumsyPilot/Control/Common/PidController.cs new file mode 100644 index 0000000..9d81320 --- /dev/null +++ b/ClumsyPilot/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/ClumsyPilot/Control/Execution/GcpCommandExecutor.cs b/ClumsyPilot/Control/Execution/GcpCommandExecutor.cs new file mode 100644 index 0000000..ee21a84 --- /dev/null +++ b/ClumsyPilot/Control/Execution/GcpCommandExecutor.cs @@ -0,0 +1,177 @@ +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/ClumsyPilot/Control/Execution/ParkingGeometricController.cs b/ClumsyPilot/Control/Execution/ParkingGeometricController.cs new file mode 100644 index 0000000..b17c559 --- /dev/null +++ b/ClumsyPilot/Control/Execution/ParkingGeometricController.cs @@ -0,0 +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.08; + + private readonly IVehicleStateProvider _stateProvider; + private readonly ILateralController _lateralController; + private readonly ILongitudinalController _longitudinalController; + private readonly GcpCommandAllocator _gcpAllocator; + private readonly GcpCommandExecutor _commandExecutor; + + private Trajectory2D _trajectory; + + /// + /// 创建具有终点判定和轨迹偏离保护的单车轨迹控制器。 + /// + public ParkingGeometricController( + IVehicleStateProvider stateProvider, + ILateralController lateralController, + ILongitudinalController longitudinalController, + GcpCommandAllocator gcpAllocator, + GcpCommandExecutor commandExecutor, + double finishDistanceMeters = 0.04, + double finishSpeedMetersPerSecond = 0.02, + double finishHeadingToleranceRadians = + 3.0 * Math.PI / 180.0, + double maximumDistanceToTrajectoryMeters = 0.30) + { + _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/ClumsyPilot/Control/Lateral/StanleyLateralController.cs b/ClumsyPilot/Control/Lateral/StanleyLateralController.cs new file mode 100644 index 0000000..9c281a7 --- /dev/null +++ b/ClumsyPilot/Control/Lateral/StanleyLateralController.cs @@ -0,0 +1,250 @@ +using System; +using MultiWheelC.Control.Abstractions; + +namespace MultiWheelC.Control.Lateral +{ + /// + /// 将参考曲率、横向误差和航向误差分别转换为前、后GCP目标转角。 + /// + public sealed class StanleyLateralController : ILateralController + { + /// + /// 创建使用指定GCP几何、Stanley增益和转角保护参数的横向控制器。 + /// + public StanleyLateralController( + double controlPointRadiusMeters, + double crossTrackGainPerSecond, + double headingErrorGain, + double minimumSpeedMetersPerSecond, + bool useActualSpeedForGain = true, + double maximumCrossTrackCorrectionRadians = + 10.0 * Math.PI / 180.0, + double maximumHeadingCorrectionRadians = + 10.0 * Math.PI / 180.0) + { + EnsureFinitePositive( + controlPointRadiusMeters, + nameof(controlPointRadiusMeters)); + EnsureFiniteNonNegative( + crossTrackGainPerSecond, + nameof(crossTrackGainPerSecond)); + EnsureFiniteNonNegative( + headingErrorGain, + nameof(headingErrorGain)); + EnsureFinitePositive( + minimumSpeedMetersPerSecond, + nameof(minimumSpeedMetersPerSecond)); + EnsureFinitePositive( + maximumCrossTrackCorrectionRadians, + nameof(maximumCrossTrackCorrectionRadians)); + EnsureFinitePositive( + maximumHeadingCorrectionRadians, + nameof(maximumHeadingCorrectionRadians)); + + ControlPointRadiusMeters = controlPointRadiusMeters; + CrossTrackGainPerSecond = crossTrackGainPerSecond; + HeadingErrorGain = headingErrorGain; + MinimumSpeedMetersPerSecond = minimumSpeedMetersPerSecond; + UseActualSpeedForGain = useActualSpeedForGain; + MaximumCrossTrackCorrectionRadians = + maximumCrossTrackCorrectionRadians; + MaximumHeadingCorrectionRadians = + maximumHeadingCorrectionRadians; + } + + /// + /// 获取车体中心到前、后GCP的距离，单位为m。 + /// + public double ControlPointRadiusMeters { get; } + + /// + /// 获取横向误差增益，单位为1/s。 + /// + public double CrossTrackGainPerSecond { get; } + + /// + /// 获取航向误差的无量纲增益。 + /// + public double HeadingErrorGain { get; } + + /// + /// 获取Stanley分母使用的最小速度绝对值，单位为m/s。 + /// + public double MinimumSpeedMetersPerSecond { get; } + + /// + /// 获取是否优先使用当前状态源提供的实际纵向速度计算横向修正。 + /// + public bool UseActualSpeedForGain { get; } + + /// + /// 获取横向误差共同转角分量的最大绝对值，单位为rad。 + /// + public double MaximumCrossTrackCorrectionRadians { get; } + + /// + /// 获取航向误差差动转角分量的最大绝对值，单位为rad。 + /// + public double MaximumHeadingCorrectionRadians { get; } + + /// + /// 分别计算横向共同转角以及曲率和航向差动转角，并生成前后GCP命令。 + /// + 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 = + ClampSymmetric( + Math.Atan( + CrossTrackGainPerSecond * + context.LateralErrorMeters / + speedMagnitude), + MaximumCrossTrackCorrectionRadians); + + // 航向误差生成前后反向的差动转角，只负责调整车身朝向。 + var headingCorrectionRadians = + ClampSymmetric( + HeadingErrorGain * + context.HeadingErrorRadians, + MaximumHeadingCorrectionRadians); + + var commonAngleRadians = + travelDirection * + crossTrackCorrectionRadians; + var differentialAngleRadians = + feedforwardAngleRadians + + travelDirection * + headingCorrectionRadians; + + return new LateralControlCommand( + commonAngleRadians + + differentialAngleRadians, + commonAngleRadians - + differentialAngleRadians); + } + + /// + /// 清除横向控制器状态；当前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 ClampSymmetric( + double value, + double maximumAbsoluteValue) + { + return Math.Max( + -maximumAbsoluteValue, + Math.Min(maximumAbsoluteValue, 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/ClumsyPilot/Control/Longitudinal/PidLongitudinalController.cs b/ClumsyPilot/Control/Longitudinal/PidLongitudinalController.cs new file mode 100644 index 0000000..ecf61f0 --- /dev/null +++ b/ClumsyPilot/Control/Longitudinal/PidLongitudinalController.cs @@ -0,0 +1,224 @@ +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, + double speedErrorDeadbandMetersPerSecond = 0.025) + { + EnsureFinitePositive( + maximumCommandSpeedMetersPerSecond, + nameof(maximumCommandSpeedMetersPerSecond)); + EnsureFiniteNonNegative( + speedErrorDeadbandMetersPerSecond, + nameof(speedErrorDeadbandMetersPerSecond)); + + _feedbackPid = new PidController( + proportionalGain, + integralGainPerSecond, + derivativeGainSeconds, + maximumIntegralCorrectionMetersPerSecond, + derivativeOnMeasurement: true); + MaximumCommandSpeedMetersPerSecond = + maximumCommandSpeedMetersPerSecond; + SpeedErrorDeadbandMetersPerSecond = + speedErrorDeadbandMetersPerSecond; + } + + /// + /// 获取负责计算速度误差修正量的通用PID控制器。 + /// + public PidController FeedbackPid => _feedbackPid; + + /// + /// 获取底盘命令速度的最大绝对值，单位为m/s。 + /// + public double MaximumCommandSpeedMetersPerSecond { get; } + + /// + /// 获取不触发纵向PID修正的速度误差死区，单位为m/s。 + /// + public double SpeedErrorDeadbandMetersPerSecond { 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); + } + + var speedErrorMetersPerSecond = + referenceSpeedMetersPerSecond - + context.ActualLongitudinalSpeedMetersPerSecond; + + // Detour差分速度在参考速度附近会有小幅波动；死区内只使用速度前馈， + // 同时清除PID历史，避免噪声持续积累后产生突发修正。 + if (Math.Abs(speedErrorMetersPerSecond) <= + SpeedErrorDeadbandMetersPerSecond) + { + 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, + "纵向控制器最大命令速度必须是正有限值。"); + } + } + + /// + /// 检查速度误差死区是否为非负有限值。 + /// + 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/ClumsyPilot/Movements/TrajectoryTrackingMovement.cs b/ClumsyPilot/Movements/TrajectoryTrackingMovement.cs new file mode 100644 index 0000000..ad6f2cb --- /dev/null +++ b/ClumsyPilot/Movements/TrajectoryTrackingMovement.cs @@ -0,0 +1,306 @@ +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 = 0.4; + + /// + /// Stanley航向误差增益。 + /// + public double StanleyHeadingErrorGain = 1.0; + + /// + /// Stanley低速分母保护速度，单位为m/s。 + /// + public double StanleyMinimumSpeedMetersPerSecond = 0.15; + + /// + /// 获取或设置Stanley是否优先使用当前状态源提供的实际纵向速度。 + /// + public bool StanleyUsesActualSpeed = true; + + /// + /// Stanley横向误差共同转角分量的最大绝对值，单位为rad。 + /// + public double MaximumCrossTrackCorrectionRadians = + AngleMath.DegreesToRadians(10.0); + + /// + /// Stanley航向误差差动转角分量的最大绝对值，单位为rad。 + /// + public double MaximumHeadingCorrectionRadians = + AngleMath.DegreesToRadians(10.0); + + /// + /// 纵向速度外环比例增益。 + /// + public double LongitudinalKp = 0.5; + + /// + /// 纵向速度外环积分增益，单位为1/s。 + /// + public double LongitudinalKiPerSecond; + + /// + /// 纵向速度外环微分增益，单位为s。 + /// + public double LongitudinalKdSeconds; + + /// + /// 纵向积分项允许产生的最大速度修正绝对值，单位为m/s。 + /// + public double MaximumIntegralCorrectionMetersPerSecond = 0.05; + + /// + /// 纵向PID不进行反馈修正的速度误差死区，单位为m/s。 + /// + public double LongitudinalSpeedErrorDeadbandMetersPerSecond = + 0.025; + + /// + /// 底盘纵向命令速度绝对值上限，单位为m/s。 + /// + public double MaximumCommandSpeedMetersPerSecond = 0.50; + + /// + /// 前后GCP允许的最大转角绝对值，单位为rad。 + /// + public double MaximumGcpAngleRadians = + AngleMath.DegreesToRadians(45.0); + + /// + /// 前后GCP目标转角最大变化率，单位为rad/s。 + /// + public double MaximumGcpAngleRateRadiansPerSecond = + AngleMath.DegreesToRadians(15.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.30; + + /// + /// 单次轨迹动作允许的最长执行时间，单位为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, + MaximumCrossTrackCorrectionRadians, + MaximumHeadingCorrectionRadians); + var longitudinalController = + new PidLongitudinalController( + LongitudinalKp, + LongitudinalKiPerSecond, + LongitudinalKdSeconds, + MaximumIntegralCorrectionMetersPerSecond, + MaximumCommandSpeedMetersPerSecond, + LongitudinalSpeedErrorDeadbandMetersPerSecond); + var gcpAllocator = + new GcpCommandAllocator( + 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/ClumsyPilot/Shared/Chassis/MultiWheelChassisAdapter.cs b/ClumsyPilot/Shared/Chassis/MultiWheelChassisAdapter.cs new file mode 100644 index 0000000..5d2fbe2 --- /dev/null +++ b/ClumsyPilot/Shared/Chassis/MultiWheelChassisAdapter.cs @@ -0,0 +1,576 @@ +// 将统一命令转换为原 Chassis API 调用 +using System; +using CommonUsage.Chassis; + +namespace MyParking.Shared +{ + /// + /// 将统一的单车车体速度命令转换为旧版MultiWheelChassis调用。 + /// 车体坐标系固定为X向前、Y向左、逆时针为正。 + /// + public sealed class MultiWheelChassisAdapter + { + #region 辅助内容 + private const double RadiansToDegrees = 180.0 / Math.PI; + private const float BiasTolerance = 0.001f; + private readonly MultiWheelChassis _chassis; + /// + /// 当前适配器对应的车辆编号。 + /// + public int VehicleId { get; } + + /// + /// Maximum distance from the body origin to a wheel center, in metres. + /// + public double MaximumWheelRadiusMeters { get; } + + /// + /// Maximum longitudinal wheel offset from the body origin, in metres. + /// For a symmetric four-wheel-steering chassis this is half the wheelbase. + /// + public double HalfWheelBaseMeters { get; } + + /// + /// 车体原点到最外侧舵轮中心的最大横向距离，单位为米。 + /// 对称四舵轮底盘中，它也是蟹行虚拟阿克曼模型的半轴距。 + /// + public double HalfTrackWidthMeters { get; } + + /// + /// Width of the steering-alignment speed gate, in degrees. + /// + public double SteeringAlignmentSigmaDegrees + { + get => _chassis.SteeringAlignmentSigmaDegrees; + set + { + if (double.IsNaN(value) || + double.IsInfinity(value) || + value <= 0.0 || + value > float.MaxValue) + { + throw new ArgumentOutOfRangeException( + nameof(value), + "Steering alignment sigma must be a positive finite value."); + } + + _chassis.SteeringAlignmentSigmaDegrees = + (float)value; + } + } + + /// + /// 检查旧底盘是否仍处于无偏置的真实车体坐标系。 + /// + private void EnsureBodyFrameIsActive() + { + EnsureMotionFrameIsActive(0.0); + } + + /// + /// 检查旧底盘当前是否处于指定的运动坐标系。 + /// motionDirectionRadians表示该运动系X轴在真实车体坐标系中的方向。 + /// + private void EnsureMotionFrameIsActive( + double motionDirectionRadians) + { + ValidateFinite( + motionDirectionRadians, + nameof(motionDirectionRadians)); + + var expectedBiasDegrees = + (float)( + -FrameTransform2D.NormalizeAngle( + motionDirectionRadians) * + RadiansToDegrees); + var bias = _chassis.GetOriginBias(); + var angleErrorDegrees = + NormalizeDegrees( + bias.Z - expectedBiasDegrees); + + if (Math.Abs(bias.X) <= BiasTolerance && + Math.Abs(bias.Y) <= BiasTolerance && + Math.Abs(angleErrorDegrees) <= + BiasTolerance) + { + return; + } + + throw new InvalidOperationException( + "MultiWheelChassis当前运动坐标系与命令不一致。" + + $"当前偏置为X={bias.X}, Y={bias.Y}, Th={bias.Z}°，" + + $"期望Th={expectedBiasDegrees}°。"); + } + + /// + /// 将角度归一化到[-180°,180°]附近。 + /// + private static float NormalizeDegrees(float degrees) + { + return (float)( + degrees - + Math.Round(degrees / 360.0) * 360.0); + } + /// + /// 检查底盘命令是否包含无效数值。 + /// + private static void ValidateTwist(Twist2D twist) + { + ValidateFinite( + twist.VxMetersPerSecond, + nameof(twist.VxMetersPerSecond)); + + ValidateFinite( + twist.VyMetersPerSecond, + nameof(twist.VyMetersPerSecond)); + + ValidateFinite( + twist.OmegaRadiansPerSecond, + nameof(twist.OmegaRadiansPerSecond)); + } + /// + /// 检查数值是否为有限值。 + /// + private static void ValidateFinite( + double value, + string parameterName) + { + if (double.IsNaN(value) || + double.IsInfinity(value)) + { + throw new ArgumentOutOfRangeException( + parameterName, + "底盘速度命令不能是NaN或无穷大。"); + } + + if (value > float.MaxValue || + value < -float.MaxValue) + { + throw new ArgumentOutOfRangeException( + parameterName, + "底盘速度命令超过float可表示范围。"); + } + } + + /// + /// 获取最近一次底盘运动分解失败原因。 + /// + public string LastFailureReason => + _chassis.LastMotionDecomposeFailureReason; + + #endregion + + /// + /// 将旧底盘的原点偏置恢复为真实单车车体坐标系。 + /// + public void ResetToBodyFrame() + { + ActivateMotionFrame(0.0); + } + + /// + /// 激活指定运动方向对应的SendMotion坐标系。 + /// 0表示真实车头，正90度表示将车体左侧作为虚拟车头。 + /// + public void ActivateMotionFrame( + double motionDirectionRadians) + { + ValidateFinite( + motionDirectionRadians, + nameof(motionDirectionRadians)); + + var biasDegrees = + (float)( + -FrameTransform2D.NormalizeAngle( + motionDirectionRadians) * + RadiansToDegrees); + var currentBias = + _chassis.GetOriginBias(); + + if (Math.Abs(currentBias.X) <= + BiasTolerance && + Math.Abs(currentBias.Y) <= + BiasTolerance && + Math.Abs( + NormalizeDegrees( + currentBias.Z - + biasDegrees)) <= + BiasTolerance) + { + return; + } + + _chassis.SetOriginBias( + x: 0.0f, + y: 0.0f, + th: biasDegrees); + } + public MultiWheelChassisAdapter(MultiWheelChassis chassis, int vehicleId) + { + _chassis = chassis ?? throw new ArgumentNullException(nameof(chassis)); + if (vehicleId <= 0) + { + throw new ArgumentOutOfRangeException( + nameof(vehicleId), + "车辆编号必须大于零。"); + } + VehicleId = vehicleId; +#pragma warning disable CS0612, CS0618 + var wheels = _chassis.GetSteerWheels(); +#pragma warning restore CS0612, CS0618 + + if (wheels.Count == 0) + { + throw new InvalidOperationException( + "MultiWheelChassis尚未完成舵轮初始化，" + + "不能创建底盘适配器。"); + } + // 禁用旧版DirectionAngle/ZeroDirection坐标偏置， + // 保证SendXYThSpeed直接使用真实车体坐标系。 + var maximumWheelRadiusMillimeters = 0.0; + var maximumLongitudinalOffsetMillimeters = 0.0; + var maximumLateralOffsetMillimeters = 0.0; + foreach (var wheel in wheels) + { + maximumWheelRadiusMillimeters = Math.Max( + maximumWheelRadiusMillimeters, + wheel.PhysicalPosition.Length()); + + maximumLongitudinalOffsetMillimeters = Math.Max( + maximumLongitudinalOffsetMillimeters, + Math.Abs(wheel.PhysicalPosition.X)); + + maximumLateralOffsetMillimeters = Math.Max( + maximumLateralOffsetMillimeters, + Math.Abs(wheel.PhysicalPosition.Y)); + } + + MaximumWheelRadiusMeters = + maximumWheelRadiusMillimeters / 1000.0; + HalfWheelBaseMeters = + maximumLongitudinalOffsetMillimeters / 1000.0; + HalfTrackWidthMeters = + maximumLateralOffsetMillimeters / 1000.0; + + if (MaximumWheelRadiusMeters <= 0.0 || + HalfWheelBaseMeters <= 0.0 || + HalfTrackWidthMeters <= 0.0) + { + throw new InvalidOperationException( + "Wheel positions cannot produce valid chassis dimensions."); + } + + // 通过反转轮速表达反向运动，避免蟹行正反切换时舵轮无意义地旋转180°。 + _chassis.PreferMinimumSteeringTravel = true; + } + + /// + /// 将车体坐标系速度命令发送给多舵轮底盘。 + /// + public bool Send(ChassisCommand command, TimeSpan? interval = null) + { + if (command.VehicleId != VehicleId) + { + throw new InvalidOperationException( + $"命令车辆编号{command.VehicleId}与适配器车辆编号" + + $"{VehicleId}不一致。"); + } + ValidateTwist(command.BodyTwist); + // 防止其他旧逻辑再次调用DirectionAngle或 + // SetOriginBias改变底盘坐标语义。 + EnsureBodyFrameIsActive(); + var vxMetersPerSecond = + (float)command.BodyTwist.VxMetersPerSecond; + var vyMetersPerSecond = + (float)command.BodyTwist.VyMetersPerSecond; + var omegaDegreesPerSecond = + (float)( + command.BodyTwist.OmegaRadiansPerSecond * + RadiansToDegrees); + var success = _chassis.SendXYThSpeed( + vxMetersPerSecond, + vyMetersPerSecond, + omegaDegreesPerSecond, + interval, + enableDifferentialSteerFeedforward: true); + if (!success) + { + // 防止分解失败后继续执行上一条运动命令。 + _chassis.PredefinedDriveStop(); + } + return success; + } + + + /// + /// 在已经激活的运动坐标系中使用SendMotion执行虚拟阿克曼运动。 + /// 转向角均相对该运动坐标系表达；正90度运动系对应车体左侧蟹行。 + /// + public bool SendVirtualAckermannMotion( + double motionDirectionRadians, + double speedMetersPerSecond, + double steeringRadians, + TimeSpan? interval = null) + { + ValidateFinite( + motionDirectionRadians, + nameof(motionDirectionRadians)); + ValidateFinite( + speedMetersPerSecond, + nameof(speedMetersPerSecond)); + ValidateFinite( + steeringRadians, + nameof(steeringRadians)); + EnsureMotionFrameIsActive( + motionDirectionRadians); + + if (Math.Abs(steeringRadians) >= + Math.PI / 2.0) + { + throw new ArgumentOutOfRangeException( + nameof(steeringRadians), + "虚拟阿克曼转向角必须位于正负90度以内。"); + } + + var steeringDegrees = + (float)( + steeringRadians * + RadiansToDegrees); + var success = + _chassis.SendMotion( + (float)speedMetersPerSecond, + steeringDegrees, + -steeringDegrees, + interval); + + if (!success) + { + _chassis.PredefinedDriveStop(); + } + + 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; + } + + /// + /// 立即将所有驱动轮速度下发为零。 + /// + public void StopImmediately() + { + _chassis.PredefinedDriveStop(); + } + + /// + /// 清零XYTh驱动速度，但保留已经准备好的自转舵角和轮速方向。 + /// + public void StopXYThDrivePreserveSteeringState() + { + _chassis.StopXYThDrivePreserveSteeringState(); + } + + /// + /// 停车并将所有舵轮转到指定的车体角度。 + /// 只调整舵轮角度，不产生车辆线速度。 + /// + public bool PrepareParallelDirection( + double directionRadians) + { + EnsureBodyFrameIsActive(); + var targetDegrees = (float)(FrameTransform2D.NormalizeAngle(directionRadians) * + RadiansToDegrees); + +#pragma warning disable CS0612, CS0618 + var wheels = _chassis.GetSteerWheels(); +#pragma warning restore CS0612, CS0618 + + // 没有舵轮时不能认为预对齐成功。 + if (wheels.Count == 0) + { + return false; + } + + // 先检查所有舵轮能否到达目标机械角度。 + foreach (var wheel in wheels) + { + if (targetDegrees < wheel.AngleLowerLimit || + targetDegrees > wheel.AngleUpperLimit) + { + return false; + } + } + + // 模式切换前立即停止驱动轮。 + _chassis.PredefinedDriveStop(); + // 检查完成后再统一下发，避免只转动一部分舵轮。 + foreach (var wheel in wheels) + { + wheel.WriteAngle(targetDegrees); + } + + return true; + } + + /// + /// 检查所有舵轮是否已经对准给定方向。 + /// + public bool AreParallelWheelsAligned( + double directionRadians, + double toleranceRadians) + { + if (double.IsNaN(toleranceRadians) || + double.IsInfinity(toleranceRadians) || + toleranceRadians < 0.0) + { + throw new ArgumentOutOfRangeException( + nameof(toleranceRadians), + "舵轮到位容差必须是非负有限值。"); + } + + EnsureBodyFrameIsActive(); + var targetDegrees = (float)( + FrameTransform2D.NormalizeAngle(directionRadians) * + 180.0 / Math.PI); + + var toleranceDegrees = (float)( + Math.Abs(toleranceRadians) * + 180.0 / Math.PI); + +#pragma warning disable CS0612, CS0618 + var wheels = _chassis.GetSteerWheels(); +#pragma warning restore CS0612, CS0618 + + foreach (var wheel in wheels) + { + var angleErrorDegrees = targetDegrees - wheel.ReadAngle(); + + if (Math.Abs(angleErrorDegrees) > + toleranceDegrees) + { + return false; + } + } + + return true; + } + + /// + /// 停车并将舵轮预对齐到原地自转方向。 + /// 返回是否成功生成舵轮目标。 + /// + public bool PrepareSpin( + TimeSpan? interval = null, + double alignmentToleranceDegrees = 2.0) + { + ValidateFinite( + alignmentToleranceDegrees, + nameof(alignmentToleranceDegrees)); + + if (alignmentToleranceDegrees < 0.0) + { + throw new ArgumentOutOfRangeException( + nameof(alignmentToleranceDegrees), + "自转舵轮到位容差必须是非负有限值。"); + } + + EnsureBodyFrameIsActive(); + + var success = + _chassis.PrepareRotateWheels( + alignmentToleranceDegrees: + (float)alignmentToleranceDegrees); + + if (!success) + { + _chassis.PredefinedDriveStop(); + } + return success; + } + + /// + /// 将已到位的自转舵角和轮速方向一次性交接给XYTh， + /// 防止普通SendXYThSpeed正式运动首帧重新初始化运动状态。 + /// + public bool AdoptPreparedSpinForXYTh( + double toleranceRadians = + 2.0 * Math.PI / 180.0) + { + if (double.IsNaN(toleranceRadians) || + double.IsInfinity(toleranceRadians) || + toleranceRadians < 0.0) + { + throw new ArgumentOutOfRangeException( + nameof(toleranceRadians), + "自转状态交接容差必须是非负有限值。"); + } + + EnsureBodyFrameIsActive(); + + var success = + _chassis + .AdoptPreparedRotateWheelsForXYTh( + (float)( + toleranceRadians * + RadiansToDegrees)); + + if (!success) + { + _chassis.PredefinedDriveStop(); + } + + return success; + } + /// + /// 所有舵轮是否已对齐到原地自转方向。 + /// + public bool AreSpinWheelsAligned => _chassis.LastRotateAligned; + + + + } +} diff --git a/ClumsyPilot/Shared/Mathematics/AngleMath.cs b/ClumsyPilot/Shared/Mathematics/AngleMath.cs new file mode 100644 index 0000000..ca3803f --- /dev/null +++ b/ClumsyPilot/Shared/Mathematics/AngleMath.cs @@ -0,0 +1,138 @@ +using System; + +namespace MyParking.Shared +{ + /// + /// 提供与坐标系无关的角度归一化、角度差和单位转换功能。 + /// + public static class AngleMath + { + public const double TwoPi = 2.0 * Math.PI; + + /// + /// 将弧度归一化到[-π, π)区间。 + /// -π包含在结果中，+π不包含在结果中，因此+π会返回-π。 + /// + public static double NormalizeRadians(double angleRadians) + { + EnsureFinite(angleRadians, nameof(angleRadians)); + + var normalized = angleRadians % TwoPi; + + if (normalized >= Math.PI) + { + normalized -= TwoPi; + } + else if (normalized < -Math.PI) + { + normalized += TwoPi; + } + + return normalized == 0.0 ? 0.0 : normalized; + } + + /// + /// 将角度归一化到[-180°, 180°)区间。 + /// -180°包含在结果中，+180°不包含在结果中，因此+180°会返回-180°。 + /// + public static double NormalizeDegrees(double angleDegrees) + { + EnsureFinite(angleDegrees, nameof(angleDegrees)); + + var normalized = angleDegrees % 360.0; + + if (normalized >= 180.0) + { + normalized -= 360.0; + } + else if (normalized < -180.0) + { + normalized += 360.0; + } + + return normalized == 0.0 ? 0.0 : normalized; + } + + /// + /// 计算从当前方向旋转到目标方向的最短有符号角度差，单位为弧度。 + /// 返回值位于[-π, π)；正值表示逆时针，负值表示顺时针。 + /// + public static double ShortestDifferenceRadians( + double targetRadians, + double currentRadians) + { + EnsureFinite(targetRadians, nameof(targetRadians)); + EnsureFinite(currentRadians, nameof(currentRadians)); + + return NormalizeRadians(targetRadians - currentRadians); + } + + /// + /// 计算从当前方向旋转到目标方向的最短有符号角度差，单位为度。 + /// 返回值位于[-180°, 180°)；正值表示逆时针，负值表示顺时针。 + /// + public static double ShortestDifferenceDegrees( + double targetDegrees, + double currentDegrees) + { + EnsureFinite(targetDegrees, nameof(targetDegrees)); + EnsureFinite(currentDegrees, nameof(currentDegrees)); + + return NormalizeDegrees(targetDegrees - currentDegrees); + } + + /// + /// 沿圆周最短方向在两个航向角之间插值，输入和结果单位均为弧度。 + /// ratio为0时返回起始角，ratio为1时返回终止角；本方法不限制ratio， + /// 轨迹线段内插值时应先使用InterpolationMath.Clamp01进行限制。 + /// 结果归一化到[-π, π)区间；角度差恰好为π时按负方向插值。 + /// + public static double LerpRadians( + double startRadians, + double endRadians, + double ratio) + { + EnsureFinite(startRadians, nameof(startRadians)); + EnsureFinite(endRadians, nameof(endRadians)); + EnsureFinite(ratio, nameof(ratio)); + + var shortestDifference = ShortestDifferenceRadians( + endRadians, + startRadians); + + return NormalizeRadians( + startRadians + ratio * shortestDifference); + } + + /// + /// 将角度从度转换为弧度，不进行归一化。 + /// + public static double DegreesToRadians(double angleDegrees) + { + EnsureFinite(angleDegrees, nameof(angleDegrees)); + return angleDegrees * Math.PI / 180.0; + } + + /// + /// 将角度从弧度转换为度，不进行归一化。 + /// + public static double RadiansToDegrees(double angleRadians) + { + EnsureFinite(angleRadians, nameof(angleRadians)); + return angleRadians * 180.0 / Math.PI; + } + + /// + /// 验证角度是可用于计算的有限数值。 + /// + private static void EnsureFinite(double angle, string parameterName) + { + if (double.IsNaN(angle) || double.IsInfinity(angle)) + { + throw new ArgumentOutOfRangeException( + parameterName, + "角度必须是有限数值。"); + } + } + } +} diff --git a/ClumsyPilot/Shared/Mathematics/FrameTransform2D.cs b/ClumsyPilot/Shared/Mathematics/FrameTransform2D.cs new file mode 100644 index 0000000..58e33db --- /dev/null +++ b/ClumsyPilot/Shared/Mathematics/FrameTransform2D.cs @@ -0,0 +1,165 @@ +// 车体、运动、车队坐标系之间的转换 +using System; + +namespace MyParking.Shared +{ + /// + /// 提供二维刚体坐标系之间的点、向量、位姿和速度变换。 + /// 坐标系采用X向前、Y向左、逆时针为正的右手系。 + /// + public static class FrameTransform2D + { + /// + /// 将角度归一化到[-π, π)范围。 + /// + public static double NormalizeAngle(double angleRadians) + { + return AngleMath.NormalizeRadians(angleRadians); + } + + /// + /// 计算从current到target的最短角度差。 + /// 返回正值表示逆时针旋转。 + /// + public static double ShortestAngleDifference( + double targetRadians, + double currentRadians) + { + return AngleMath.ShortestDifferenceRadians( + targetRadians, + currentRadians); + } + + /// + /// 将源坐标系中的点变换到目标坐标系。 + /// sourcePoseInTarget表示源坐标系在目标坐标系中的位姿。 + /// + public static Point2D TransformPoint( + Pose2D sourcePoseInTarget, + Point2D pointInSource) + { + var cos = Math.Cos(sourcePoseInTarget.YawRadians); + var sin = Math.Sin(sourcePoseInTarget.YawRadians); + + return new Point2D( + sourcePoseInTarget.XMeters + + cos * pointInSource.XMeters - + sin * pointInSource.YMeters, + + sourcePoseInTarget.YMeters + + sin * pointInSource.XMeters + + cos * pointInSource.YMeters); + } + + /// + /// 将目标坐标系中的点反向变换到源坐标系。 + /// + public static Point2D InverseTransformPoint( + Pose2D sourcePoseInTarget, + Point2D pointInTarget) + { + var dx = pointInTarget.XMeters - sourcePoseInTarget.XMeters; + + var dy = pointInTarget.YMeters - sourcePoseInTarget.YMeters; + + var cos = Math.Cos(sourcePoseInTarget.YawRadians); + var sin = Math.Sin(sourcePoseInTarget.YawRadians); + + return new Point2D( + cos * dx + sin * dy, + -sin * dx + cos * dy); + } + + /// + /// 将源坐标系中的向量旋转到目标坐标系。 + /// 向量没有位置，因此不叠加平移量。 + /// + public static Point2D TransformVector( + Pose2D sourcePoseInTarget, + Point2D vectorInSource) + { + var cos = Math.Cos(sourcePoseInTarget.YawRadians); + var sin = Math.Sin(sourcePoseInTarget.YawRadians); + + return new Point2D( + cos * vectorInSource.XMeters - + sin * vectorInSource.YMeters, + + sin * vectorInSource.XMeters + + cos * vectorInSource.YMeters); + } + + /// + /// 组合两级坐标变换。 + /// parentFromMiddle表示middle在parent中的位姿； + /// middleFromChild表示child在middle中的位姿； + /// 返回child在parent中的位姿。 + /// + public static Pose2D Compose( + Pose2D parentFromMiddle, + Pose2D middleFromChild) + { + var childPositionInParent = TransformPoint( + parentFromMiddle, + middleFromChild.Position); + + return new Pose2D( + childPositionInParent.XMeters, + childPositionInParent.YMeters, + NormalizeAngle( + parentFromMiddle.YawRadians + + middleFromChild.YawRadians)); + } + + /// + /// 对坐标变换求逆。 + /// 输入child在parent中的位姿，返回parent在child中的位姿。 + /// + public static Pose2D Inverse(Pose2D childPoseInParent) + { + var cos = Math.Cos(childPoseInParent.YawRadians); + var sin = Math.Sin(childPoseInParent.YawRadians); + + return new Pose2D( + -cos * childPoseInParent.XMeters - + sin * childPoseInParent.YMeters, + + sin * childPoseInParent.XMeters - + cos * childPoseInParent.YMeters, + + NormalizeAngle( + -childPoseInParent.YawRadians)); + } + + + /// + /// 将源坐标系中的位姿变换到目标坐标系。 + /// + public static Pose2D TransformPose( + Pose2D sourcePoseInTarget, + Pose2D poseInSource) + { + return Compose(sourcePoseInTarget, poseInSource); + } + + /// + /// 转换同一物理参考点处的速度表达坐标系。 + /// 只旋转线速度，角速度保持不变。 + /// + public static Twist2D TransformTwistAtSamePoint( + Pose2D sourcePoseInTarget, + Twist2D twistInSource) + { + var linearVelocityInTarget = TransformVector( + sourcePoseInTarget, + new Point2D( + twistInSource.VxMetersPerSecond, + twistInSource.VyMetersPerSecond)); + + return new Twist2D( + linearVelocityInTarget.XMeters, + linearVelocityInTarget.YMeters, + twistInSource.OmegaRadiansPerSecond); + } + } +} diff --git a/ClumsyPilot/Shared/Mathematics/InterpolationMath.cs b/ClumsyPilot/Shared/Mathematics/InterpolationMath.cs new file mode 100644 index 0000000..61db6e3 --- /dev/null +++ b/ClumsyPilot/Shared/Mathematics/InterpolationMath.cs @@ -0,0 +1,61 @@ + +using System; + +namespace MyParking.Shared +{ + /// + /// 提供与具体业务和坐标系无关的基础插值功能。 + /// + public static class InterpolationMath + { + /// + /// 将插值比例限制到[0, 1]闭区间。 + /// + public static double Clamp01(double value) + { + EnsureFinite(value, nameof(value)); + + if (value <= 0.0) + { + return 0.0; + } + + if (value >= 1.0) + { + return 1.0; + } + + return value; + } + + /// + /// 对两个标量执行线性插值。 + /// ratio为0时返回start，ratio为1时返回end；本方法不限制ratio， + /// 因此也支持区间外的线性外插。 + /// + public static double Lerp( + double start, + double end, + double ratio) + { + EnsureFinite(start, nameof(start)); + EnsureFinite(end, nameof(end)); + EnsureFinite(ratio, nameof(ratio)); + + return start + ratio * (end - start); + } + + /// + /// 验证输入是可用于插值计算的有限数值。 + /// + private static void EnsureFinite(double value, string parameterName) + { + if (double.IsNaN(value) || double.IsInfinity(value)) + { + throw new ArgumentOutOfRangeException( + parameterName, + "插值参数必须是有限数值。"); + } + } + } +} diff --git a/ClumsyPilot/Shared/Models/ChassisCommand.cs b/ClumsyPilot/Shared/Models/ChassisCommand.cs new file mode 100644 index 0000000..73545dd --- /dev/null +++ b/ClumsyPilot/Shared/Models/ChassisCommand.cs @@ -0,0 +1,183 @@ +// 纯数据层：只描述坐标、速度和命令 +// 定义二维坐标、位姿、速度、车队布局和单车底盘命令。 +// Shared层统一使用SI单位：位置m、线速度m/s、角度rad、角速度rad/s。 +// 车体坐标系采用右手系：X向前、Y向左、逆时针角度和角速度为正。 +// 命名约定：XxxInYyy表示Xxx在Yyy坐标系中的表达。 + +namespace MyParking.Shared +{ + /// + /// 二维坐标点，X、Y单位均为米。 + /// + public readonly struct Point2D + { + public Point2D(double xMeters, double yMeters) + { + XMeters = xMeters; + YMeters = yMeters; + } + + public double XMeters { get; } + + public double YMeters { get; } + + public static Point2D Zero => new Point2D(0.0, 0.0); + } + + /// + /// 二维局部坐标系在父坐标系中的位姿。 + /// 位置单位为米，朝向单位为弧度，逆时针为正。 + /// 具体父子关系由变量名称说明，例如RadarPoseInBody。 + /// + public readonly struct Pose2D + { + public Pose2D( + double xMeters, + double yMeters, + double yawRadians) + { + XMeters = xMeters; + YMeters = yMeters; + YawRadians = yawRadians; + } + + public double XMeters { get; } + + public double YMeters { get; } + + public double YawRadians { get; } + + public Point2D Position => + new Point2D(XMeters, YMeters); + + public static Pose2D Identity => + new Pose2D(0.0, 0.0, 0.0); + } + + /// + /// 二维刚体速度。 + /// 线速度单位为m/s，角速度单位为rad/s。 + /// 速度所属坐标系由持有该Twist2D的外层类型或变量名称确定。 + /// + public readonly struct Twist2D + { + public Twist2D( + double vxMetersPerSecond, + double vyMetersPerSecond, + double omegaRadiansPerSecond) + { + VxMetersPerSecond = vxMetersPerSecond; + VyMetersPerSecond = vyMetersPerSecond; + OmegaRadiansPerSecond = omegaRadiansPerSecond; + } + + public double VxMetersPerSecond { get; } + + public double VyMetersPerSecond { get; } + + public double OmegaRadiansPerSecond { get; } + + public static Twist2D Zero => + new Twist2D(0.0, 0.0, 0.0); + } + + /// + /// 发送给单辆车的车体坐标系速度命令。 + /// + public readonly struct ChassisCommand + { + public ChassisCommand( + int vehicleId, + Twist2D bodyTwist) + { + VehicleId = vehicleId; + BodyTwist = bodyTwist; + } + + public int VehicleId { get; } + + /// + /// 单车车体坐标系速度：X向前、Y向左、逆时针旋转为正。 + /// + public Twist2D BodyTwist { get; } + + /// + /// 创建指定车辆的停止命令。 + /// + public static ChassisCommand Stop(int vehicleId) + { + return new ChassisCommand( + vehicleId, + Twist2D.Zero); + } + } + + /// + /// 单辆车的车体坐标系在车队坐标系中的位姿。 + /// + public readonly struct VehicleLayout + { + public VehicleLayout( + int vehicleId, + Pose2D poseInFleet) + { + VehicleId = vehicleId; + PoseInFleet = poseInFleet; + } + + public int VehicleId { get; } + + public Pose2D PoseInFleet { get; } + } + + /// + /// 车队整体运动命令，速度分量均在车队坐标系中表达。 + /// + public readonly struct FleetMotionCommand + { + public FleetMotionCommand( + Point2D referencePointInFleet, + Twist2D twistAtReferencePoint) + { + ReferencePointInFleet = referencePointInFleet; + TwistAtReferencePoint = twistAtReferencePoint; + } + + /// + /// 速度命令对应的参考点，也可作为自定义旋转中心。 + /// + public Point2D ReferencePointInFleet { get; } + + /// + /// 参考点处的车队速度。 + /// + public Twist2D TwistAtReferencePoint { get; } + + /// + /// 创建绕指定中心原地旋转的车队命令。 + /// + public static FleetMotionCommand RotateAround( + Point2D rotationCenterInFleet, + double omegaRadiansPerSecond) + { + return new FleetMotionCommand( + rotationCenterInFleet, + new Twist2D( + 0.0, + 0.0, + omegaRadiansPerSecond)); + } + + /// + /// 创建车队停止命令。 + /// + public static FleetMotionCommand Stop() + { + return new FleetMotionCommand( + Point2D.Zero, + Twist2D.Zero); + } + } + + +} diff --git a/ClumsyPilot/StateEstimation/DetourVehicleStateProvider.cs b/ClumsyPilot/StateEstimation/DetourVehicleStateProvider.cs new file mode 100644 index 0000000..4a07a9b --- /dev/null +++ b/ClumsyPilot/StateEstimation/DetourVehicleStateProvider.cs @@ -0,0 +1,509 @@ +using System; +using System.Diagnostics; +using ClumsyCore.Interfaces; +using MyParking.Shared; + +namespace MultiWheelC.StateEstimation +{ + /// + /// 读取Detour位姿，忽略重复或明显异常的观测，并估算车辆二维速度。 + /// + public sealed class DetourVehicleStateProvider + : IVehicleStateProvider + { + public const double DefaultMaximumLinearSpeedMetersPerSecond = + 1.20; + public const double DefaultMaximumAngularSpeedRadiansPerSecond = + Math.PI / 4.0; + public const double DefaultPositionJumpMarginMeters = + 0.03; + public const double DefaultHeadingJumpMarginRadians = + 5.0 * Math.PI / 180.0; + public const double DefaultVelocityPositionResidualMeters = + 0.04; + public const double DefaultVelocityHeadingResidualRadians = + 5.0 * Math.PI / 180.0; + public const double DefaultStationaryConfirmationSeconds = + 0.35; + + private const double MillimetersPerMeter = 1000.0; + private const double PositionEqualityToleranceMeters = 1e-9; + private const double HeadingEqualityToleranceRadians = 1e-8; + + private readonly object _syncRoot = new object(); + private readonly Stopwatch _clock = Stopwatch.StartNew(); + private readonly VelocityEstimator2D _velocityEstimator; + private readonly double _maximumLinearSpeedMetersPerSecond; + private readonly double _maximumAngularSpeedRadiansPerSecond; + private readonly double _positionJumpMarginMeters; + private readonly double _headingJumpMarginRadians; + private readonly double _velocityPositionResidualMeters; + private readonly double _velocityHeadingResidualRadians; + private readonly double _stationaryConfirmationSeconds; + + private bool _hasAcceptedPose; + private Pose2D _acceptedPoseInWorld; + private double _acceptedTimestampSeconds; + private VehicleState _latestState; + private bool _stationaryHoldActive; + + /// + /// 创建使用停车机器人默认物理边界和速度滤波参数的Detour状态源。 + /// + public DetourVehicleStateProvider() + : this( + new VelocityEstimator2D(), + DefaultMaximumLinearSpeedMetersPerSecond, + DefaultMaximumAngularSpeedRadiansPerSecond, + DefaultPositionJumpMarginMeters, + DefaultHeadingJumpMarginRadians, + DefaultVelocityPositionResidualMeters, + DefaultVelocityHeadingResidualRadians, + DefaultStationaryConfirmationSeconds) + { + } + + /// + /// 创建使用指定物理边界、静止确认时间和速度估计器的Detour状态源。 + /// + public DetourVehicleStateProvider( + VelocityEstimator2D velocityEstimator, + double maximumLinearSpeedMetersPerSecond, + double maximumAngularSpeedRadiansPerSecond, + double positionJumpMarginMeters, + double headingJumpMarginRadians, + double velocityPositionResidualMeters, + double velocityHeadingResidualRadians, + double stationaryConfirmationSeconds) + { + _velocityEstimator = velocityEstimator ?? + throw new ArgumentNullException( + nameof(velocityEstimator)); + + EnsureFinitePositive( + maximumLinearSpeedMetersPerSecond, + nameof(maximumLinearSpeedMetersPerSecond)); + EnsureFinitePositive( + maximumAngularSpeedRadiansPerSecond, + nameof(maximumAngularSpeedRadiansPerSecond)); + EnsureFiniteNonNegative( + positionJumpMarginMeters, + nameof(positionJumpMarginMeters)); + EnsureFiniteNonNegative( + headingJumpMarginRadians, + nameof(headingJumpMarginRadians)); + EnsureFinitePositive( + velocityPositionResidualMeters, + nameof(velocityPositionResidualMeters)); + EnsureFinitePositive( + velocityHeadingResidualRadians, + nameof(velocityHeadingResidualRadians)); + EnsureFinitePositive( + stationaryConfirmationSeconds, + nameof(stationaryConfirmationSeconds)); + + _maximumLinearSpeedMetersPerSecond = + maximumLinearSpeedMetersPerSecond; + _maximumAngularSpeedRadiansPerSecond = + maximumAngularSpeedRadiansPerSecond; + _positionJumpMarginMeters = + positionJumpMarginMeters; + _headingJumpMarginRadians = + headingJumpMarginRadians; + _velocityPositionResidualMeters = + velocityPositionResidualMeters; + _velocityHeadingResidualRadians = + velocityHeadingResidualRadians; + _stationaryConfirmationSeconds = + stationaryConfirmationSeconds; + } + + /// + /// 获取最近一次读取失败或异常观测被忽略的原因，正常时为空字符串。 + /// + public string LastFailureReason { get; private set; } = ""; + + /// + /// 尝试读取Detour；重复帧保留最近状态，明显异常帧只忽略本次观测。 + /// + public bool TryGetState(out VehicleState state) + { + lock (_syncRoot) + { + try + { + var poseInWorld = + ReadDetourPoseInWorld(); + var timestampSeconds = + _clock.Elapsed.TotalSeconds; + + if (!_hasAcceptedPose) + { + state = AcceptPoseAfterReset( + poseInWorld, + timestampSeconds); + LastFailureReason = ""; + return true; + } + + if (ArePosesEquivalent( + poseInWorld, + _acceptedPoseInWorld)) + { + state = HandleRepeatedPose( + timestampSeconds); + LastFailureReason = ""; + return true; + } + + // 静止保持后出现新定位时重新建立差分基准， + // 避免用很长的静止时间稀释第一次运动速度。 + if (_stationaryHoldActive) + { + state = AcceptPoseAfterReset( + poseInWorld, + timestampSeconds); + LastFailureReason = ""; + return true; + } + + var elapsedSeconds = + timestampSeconds - + _acceptedTimestampSeconds; + + if (!IsMotionPlausible( + _acceptedPoseInWorld, + poseInWorld, + elapsedSeconds)) + { + // 单帧异常不进入差分器，也不中断调用方；下一次 + // 正常观测仍相对最近有效位姿和真实时间差计算。 + state = _latestState; + LastFailureReason = + "Detour位姿变化超过车辆绝对运动边界，本次观测已忽略。"; + return true; + } + + if (IsVelocityInnovationAbnormal( + poseInWorld, + elapsedSeconds)) + { + state = AcceptPoseAfterReset( + poseInWorld, + timestampSeconds); + LastFailureReason = + "Detour位姿偏离速度预测，已重新建立速度估计基准。"; + return true; + } + + state = AcceptContinuousPose( + poseInWorld, + timestampSeconds); + LastFailureReason = ""; + return true; + } + catch (Exception exception) + { + state = default; + LastFailureReason = + "Detour车辆状态读取失败：" + + exception.Message; + return false; + } + } + } + + /// + /// 清除Detour位姿历史和速度估计状态。 + /// + public void Reset() + { + lock (_syncRoot) + { + _velocityEstimator.Reset(); + _hasAcceptedPose = false; + _acceptedPoseInWorld = Pose2D.Identity; + _acceptedTimestampSeconds = 0.0; + _latestState = default; + _stationaryHoldActive = false; + LastFailureReason = ""; + } + } + + /// + /// 读取Detour毫米和角度数据并转换为世界坐标SI位姿。 + /// + private static Pose2D ReadDetourPoseInWorld() + { + var location = + DetourInterface.getCartLocation(); + + EnsureFinite(location.x, "DetourX"); + EnsureFinite(location.y, "DetourY"); + EnsureFinite(location.th, "DetourTheta"); + + return new Pose2D( + location.x / MillimetersPerMeter, + location.y / MillimetersPerMeter, + AngleMath.NormalizeRadians( + AngleMath.DegreesToRadians( + location.th))); + } + + /// + /// 接受连续有效定位并更新速度估计和差分基准。 + /// + private VehicleState AcceptContinuousPose( + Pose2D poseInWorld, + double timestampSeconds) + { + _latestState = + _velocityEstimator.Update( + poseInWorld, + timestampSeconds); + _acceptedPoseInWorld = poseInWorld; + _acceptedTimestampSeconds = + timestampSeconds; + _stationaryHoldActive = false; + return _latestState; + } + + /// + /// 接受跳变后的新位姿基准，但不让该位移进入速度差分和低通滤波器。 + /// + private VehicleState AcceptPoseAfterVelocityRebase( + Pose2D poseInWorld, + double timestampSeconds) + { + _latestState = + _velocityEstimator + .RebasePreservingVelocity( + poseInWorld, + timestampSeconds); + _acceptedPoseInWorld = poseInWorld; + _acceptedTimestampSeconds = + timestampSeconds; + _stationaryHoldActive = false; + return _latestState; + } + + /// + /// 接受首帧或静止后的首个新位姿并重新建立零速差分基准。 + /// + private VehicleState AcceptPoseAfterReset( + Pose2D poseInWorld, + double timestampSeconds) + { + _latestState = + _velocityEstimator.Reset( + poseInWorld, + timestampSeconds); + _acceptedPoseInWorld = poseInWorld; + _acceptedTimestampSeconds = + timestampSeconds; + _hasAcceptedPose = true; + _stationaryHoldActive = false; + return _latestState; + } + + /// + /// 对重复Detour观测保留最近状态，并在持续不变后将速度归零。 + /// + private VehicleState HandleRepeatedPose( + double timestampSeconds) + { + var unchangedSeconds = + timestampSeconds - + _acceptedTimestampSeconds; + + if (!_stationaryHoldActive && + unchangedSeconds >= + _stationaryConfirmationSeconds) + { + _latestState = + new VehicleState( + timestampSeconds, + _acceptedPoseInWorld, + Twist2D.Zero, + true); + _stationaryHoldActive = true; + } + + return _latestState; + } + + /// + /// 判断两次有效Detour观测之间的变化是否超过车辆绝对运动能力。 + /// + private bool IsMotionPlausible( + Pose2D startPoseInWorld, + Pose2D endPoseInWorld, + double deltaTimeSeconds) + { + if (!IsFinite(deltaTimeSeconds) || + deltaTimeSeconds <= 0.0) + { + return false; + } + + var deltaX = + endPoseInWorld.XMeters - + startPoseInWorld.XMeters; + var deltaY = + endPoseInWorld.YMeters - + startPoseInWorld.YMeters; + var displacementMeters = + Math.Sqrt( + deltaX * deltaX + + deltaY * deltaY); + var headingChangeRadians = + Math.Abs( + AngleMath.ShortestDifferenceRadians( + endPoseInWorld.YawRadians, + startPoseInWorld.YawRadians)); + + var maximumDisplacementMeters = + _maximumLinearSpeedMetersPerSecond * + deltaTimeSeconds + + _positionJumpMarginMeters; + var maximumHeadingChangeRadians = + _maximumAngularSpeedRadiansPerSecond * + deltaTimeSeconds + + _headingJumpMarginRadians; + + return displacementMeters <= + maximumDisplacementMeters && + headingChangeRadians <= + maximumHeadingChangeRadians; + } + + /// + /// 判断新位姿是否明显偏离上一滤波速度给出的恒速预测。 + /// + private bool IsVelocityInnovationAbnormal( + Pose2D poseInWorld, + double deltaTimeSeconds) + { + if (!_latestState.HasValidVelocityEstimate) + { + return false; + } + + var predictedX = + _acceptedPoseInWorld.XMeters + + _latestState.TwistInWorld + .VxMetersPerSecond * + deltaTimeSeconds; + var predictedY = + _acceptedPoseInWorld.YMeters + + _latestState.TwistInWorld + .VyMetersPerSecond * + deltaTimeSeconds; + var predictedYaw = + AngleMath.NormalizeRadians( + _acceptedPoseInWorld.YawRadians + + _latestState.TwistInWorld + .OmegaRadiansPerSecond * + deltaTimeSeconds); + + var positionResidualX = + poseInWorld.XMeters - predictedX; + var positionResidualY = + poseInWorld.YMeters - predictedY; + var positionResidualMeters = + Math.Sqrt( + positionResidualX * positionResidualX + + positionResidualY * positionResidualY); + var headingResidualRadians = + Math.Abs( + AngleMath.ShortestDifferenceRadians( + poseInWorld.YawRadians, + predictedYaw)); + + return positionResidualMeters > + _velocityPositionResidualMeters || + headingResidualRadians > + _velocityHeadingResidualRadians; + } + + /// + /// 判断两次读取是否为Detour保持输出的同一数值帧。 + /// + private static bool ArePosesEquivalent( + Pose2D firstPose, + Pose2D secondPose) + { + return Math.Abs( + firstPose.XMeters - + secondPose.XMeters) <= + PositionEqualityToleranceMeters && + Math.Abs( + firstPose.YMeters - + secondPose.YMeters) <= + PositionEqualityToleranceMeters && + Math.Abs( + AngleMath.ShortestDifferenceRadians( + firstPose.YawRadians, + secondPose.YawRadians)) <= + HeadingEqualityToleranceRadians; + } + + /// + /// 检查数值是否为正有限值。 + /// + private static void EnsureFinitePositive( + double value, + string parameterName) + { + EnsureFinite(value, parameterName); + + if (value <= 0.0) + { + throw new ArgumentOutOfRangeException( + parameterName, + "状态源参数必须是正有限值。"); + } + } + + /// + /// 检查数值是否为非负有限值。 + /// + private static void EnsureFiniteNonNegative( + double value, + string parameterName) + { + EnsureFinite(value, parameterName); + + if (value < 0.0) + { + throw new ArgumentOutOfRangeException( + parameterName, + "状态源参数必须是非负有限值。"); + } + } + + /// + /// 检查数值是否为有限值。 + /// + private static void EnsureFinite( + double value, + string parameterName) + { + if (!IsFinite(value)) + { + throw new ArgumentOutOfRangeException( + parameterName, + "状态源参数和Detour位姿必须是有限值。"); + } + } + + /// + /// 判断数值是否可用于状态估计。 + /// + private static bool IsFinite(double value) + { + return !double.IsNaN(value) && + !double.IsInfinity(value); + } + } +} diff --git a/ClumsyPilot/StateEstimation/FirstOrderLowPassFilter.cs b/ClumsyPilot/StateEstimation/FirstOrderLowPassFilter.cs new file mode 100644 index 0000000..29464bc --- /dev/null +++ b/ClumsyPilot/StateEstimation/FirstOrderLowPassFilter.cs @@ -0,0 +1,142 @@ +using System; + +namespace MultiWheelC.StateEstimation +{ + /// + /// 使用真实采样时间间隔对单个连续量执行在线一阶低通滤波。 + /// + public sealed class FirstOrderLowPassFilter + { + private readonly double _timeConstantSeconds; + private bool _isInitialized; + private double _value; + + /// + /// 创建使用指定时间常数的一阶低通滤波器。 + /// + public FirstOrderLowPassFilter( + double timeConstantSeconds) + { + EnsureFinitePositive( + timeConstantSeconds, + nameof(timeConstantSeconds)); + + _timeConstantSeconds = + timeConstantSeconds; + } + + /// + /// 获取滤波时间常数，单位为s；数值越大，滤波越强但响应越慢。 + /// + public double TimeConstantSeconds => + _timeConstantSeconds; + + /// + /// 获取滤波器是否已经接收过有效初值。 + /// + public bool IsInitialized => + _isInitialized; + + /// + /// 获取当前滤波输出；尚未初始化时读取会抛出异常。 + /// + public double Value + { + get + { + if (!_isInitialized) + { + throw new InvalidOperationException( + "一阶低通滤波器尚未初始化。"); + } + + return _value; + } + } + + /// + /// 使用当前输入和真实采样间隔更新滤波结果。 + /// + public double Update( + double input, + double deltaTimeSeconds) + { + EnsureFinite( + input, + nameof(input)); + EnsureFinitePositive( + deltaTimeSeconds, + nameof(deltaTimeSeconds)); + + if (!_isInitialized) + { + _value = input; + _isInitialized = true; + return _value; + } + + var alpha = + deltaTimeSeconds / + (_timeConstantSeconds + + deltaTimeSeconds); + + _value += alpha * (input - _value); + return _value; + } + + /// + /// 清除历史输出，使下一次有效输入直接成为新的初值。 + /// + public void Reset() + { + _value = 0.0; + _isInitialized = false; + } + + /// + /// 将滤波器立即重置到指定的有限初值。 + /// + public void Reset(double initialValue) + { + EnsureFinite( + initialValue, + nameof(initialValue)); + + _value = initialValue; + _isInitialized = true; + } + + /// + /// 检查数值是否为正有限值。 + /// + private static void EnsureFinitePositive( + double value, + string parameterName) + { + EnsureFinite(value, parameterName); + + if (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/ClumsyPilot/StateEstimation/IVehicleStateProvider.cs b/ClumsyPilot/StateEstimation/IVehicleStateProvider.cs new file mode 100644 index 0000000..fcb25d8 --- /dev/null +++ b/ClumsyPilot/StateEstimation/IVehicleStateProvider.cs @@ -0,0 +1,14 @@ +namespace MultiWheelC.StateEstimation +{ + /// + /// 为轨迹控制器提供与具体定位来源无关的统一车辆状态读取接口。 + /// + public interface IVehicleStateProvider + { + /// + /// 尝试读取当前有效车辆状态；定位不可用或过期时返回false。 + /// + bool TryGetState(out VehicleState state); + } +} + diff --git a/ClumsyPilot/StateEstimation/VehicleState.cs b/ClumsyPilot/StateEstimation/VehicleState.cs new file mode 100644 index 0000000..af4814c --- /dev/null +++ b/ClumsyPilot/StateEstimation/VehicleState.cs @@ -0,0 +1,138 @@ +using System; +using MyParking.Shared; + +namespace MultiWheelC.StateEstimation +{ + /// + /// 保存一次经过校验的车辆位姿和速度估计快照，统一使用SI单位。 + /// + public readonly struct VehicleState + { + /// + /// 创建车辆状态，并将世界坐标速度同步转换到车体坐标系。 + /// + public VehicleState( + double sampleTimestampSeconds, + Pose2D poseInWorld, + Twist2D twistInWorld, + bool hasValidVelocityEstimate) + { + EnsureFiniteNonNegative( + sampleTimestampSeconds, + nameof(sampleTimestampSeconds)); + EnsureFinitePose( + poseInWorld, + nameof(poseInWorld)); + EnsureFiniteTwist( + twistInWorld, + nameof(twistInWorld)); + + SampleTimestampSeconds = + sampleTimestampSeconds; + PoseInWorld = new Pose2D( + poseInWorld.XMeters, + poseInWorld.YMeters, + AngleMath.NormalizeRadians( + poseInWorld.YawRadians)); + HasValidVelocityEstimate = + hasValidVelocityEstimate; + + // 第一帧或定位重置后的速度不可用于闭环控制， + // 此时显式置零，避免调用方误用残留速度。 + TwistInWorld = hasValidVelocityEstimate + ? twistInWorld + : Twist2D.Zero; + + var worldPoseInBody = + FrameTransform2D.Inverse( + PoseInWorld); + TwistInBody = + FrameTransform2D.TransformTwistAtSamePoint( + worldPoseInBody, + TwistInWorld); + } + + /// + /// 获取状态源单调时钟中的采样时刻，单位为s。 + /// + public double SampleTimestampSeconds { get; } + + /// + /// 获取车体中心在Detour世界坐标系中的位姿，单位为m和rad。 + /// + public Pose2D PoseInWorld { get; } + + /// + /// 获取在世界坐标系中表达的车辆速度，单位为m/s和rad/s。 + /// + public Twist2D TwistInWorld { get; } + + /// + /// 获取在车体坐标系中表达的车辆速度，X向前、Y向左、逆时针为正。 + /// + public Twist2D TwistInBody { get; } + + /// + /// 获取当前速度是否已由至少两个连续有效定位样本估算得到。 + /// + public bool HasValidVelocityEstimate { get; } + + /// + /// 检查位姿是否由有限数值组成。 + /// + 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 EnsureFiniteTwist( + Twist2D twist, + string parameterName) + { + if (!IsFinite(twist.VxMetersPerSecond) || + !IsFinite(twist.VyMetersPerSecond) || + !IsFinite(twist.OmegaRadiansPerSecond)) + { + throw new ArgumentOutOfRangeException( + parameterName, + "车辆速度必须由有限数值组成。"); + } + } + + /// + /// 检查数值是否为非负有限值。 + /// + private static void EnsureFiniteNonNegative( + 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/ClumsyPilot/StateEstimation/VelocityEstimator2D.cs b/ClumsyPilot/StateEstimation/VelocityEstimator2D.cs new file mode 100644 index 0000000..3d8a85d --- /dev/null +++ b/ClumsyPilot/StateEstimation/VelocityEstimator2D.cs @@ -0,0 +1,275 @@ +using System; +using MyParking.Shared; + +namespace MultiWheelC.StateEstimation +{ + /// + /// 根据连续有效的Detour世界位姿和真实时间差估算车辆二维速度。 + /// + public sealed class VelocityEstimator2D + { + public const double DefaultLinearFilterTimeConstantSeconds = + 0.15; + public const double DefaultAngularFilterTimeConstantSeconds = + 0.20; + + private readonly FirstOrderLowPassFilter + _worldVelocityXFilter; + private readonly FirstOrderLowPassFilter + _worldVelocityYFilter; + private readonly FirstOrderLowPassFilter + _angularVelocityFilter; + + private bool _hasPreviousSample; + private Pose2D _previousPoseInWorld; + private double _previousTimestampSeconds; + + /// + /// 创建使用默认0.15s线速度和0.20s角速度时间常数的估计器。 + /// + public VelocityEstimator2D() + : this( + DefaultLinearFilterTimeConstantSeconds, + DefaultAngularFilterTimeConstantSeconds) + { + } + + /// + /// 创建使用指定线速度和角速度滤波时间常数的估计器。 + /// + public VelocityEstimator2D( + double linearFilterTimeConstantSeconds, + double angularFilterTimeConstantSeconds) + { + _worldVelocityXFilter = + new FirstOrderLowPassFilter( + linearFilterTimeConstantSeconds); + _worldVelocityYFilter = + new FirstOrderLowPassFilter( + linearFilterTimeConstantSeconds); + _angularVelocityFilter = + new FirstOrderLowPassFilter( + angularFilterTimeConstantSeconds); + } + + /// + /// 获取是否已经保存了可用于下一次差分的位姿基准。 + /// + public bool HasPreviousSample => + _hasPreviousSample; + + /// + /// 使用一个新的有效定位样本更新并返回车辆状态。 + /// + public VehicleState Update( + Pose2D poseInWorld, + double sampleTimestampSeconds) + { + EnsureFinitePose( + poseInWorld, + nameof(poseInWorld)); + EnsureFiniteNonNegative( + sampleTimestampSeconds, + nameof(sampleTimestampSeconds)); + + var normalizedPoseInWorld = + new Pose2D( + poseInWorld.XMeters, + poseInWorld.YMeters, + AngleMath.NormalizeRadians( + poseInWorld.YawRadians)); + + if (!_hasPreviousSample) + { + return Reset( + normalizedPoseInWorld, + sampleTimestampSeconds); + } + + var deltaTimeSeconds = + sampleTimestampSeconds - + _previousTimestampSeconds; + + if (deltaTimeSeconds <= 0.0) + { + throw new ArgumentOutOfRangeException( + nameof(sampleTimestampSeconds), + "新定位样本的单调时间戳必须严格大于上一帧。"); + } + + var rawVelocityXInWorld = + (normalizedPoseInWorld.XMeters - + _previousPoseInWorld.XMeters) / + deltaTimeSeconds; + var rawVelocityYInWorld = + (normalizedPoseInWorld.YMeters - + _previousPoseInWorld.YMeters) / + deltaTimeSeconds; + var rawAngularVelocity = + AngleMath.ShortestDifferenceRadians( + normalizedPoseInWorld.YawRadians, + _previousPoseInWorld.YawRadians) / + deltaTimeSeconds; + + var filteredTwistInWorld = + new Twist2D( + _worldVelocityXFilter.Update( + rawVelocityXInWorld, + deltaTimeSeconds), + _worldVelocityYFilter.Update( + rawVelocityYInWorld, + deltaTimeSeconds), + _angularVelocityFilter.Update( + rawAngularVelocity, + deltaTimeSeconds)); + + _previousPoseInWorld = + normalizedPoseInWorld; + _previousTimestampSeconds = + sampleTimestampSeconds; + + return new VehicleState( + sampleTimestampSeconds, + normalizedPoseInWorld, + filteredTwistInWorld, + true); + } + + /// + /// 更新位姿差分基准但保留当前滤波速度，避免定位跳变形成虚假速度尖峰。 + /// + public VehicleState RebasePreservingVelocity( + Pose2D poseInWorld, + double sampleTimestampSeconds) + { + EnsureFinitePose( + poseInWorld, + nameof(poseInWorld)); + EnsureFiniteNonNegative( + sampleTimestampSeconds, + nameof(sampleTimestampSeconds)); + + var normalizedPoseInWorld = + new Pose2D( + poseInWorld.XMeters, + poseInWorld.YMeters, + AngleMath.NormalizeRadians( + poseInWorld.YawRadians)); + + _previousPoseInWorld = + normalizedPoseInWorld; + _previousTimestampSeconds = + sampleTimestampSeconds; + _hasPreviousSample = true; + + var hasValidVelocityEstimate = + _worldVelocityXFilter.IsInitialized && + _worldVelocityYFilter.IsInitialized && + _angularVelocityFilter.IsInitialized; + + var retainedTwistInWorld = + hasValidVelocityEstimate + ? new Twist2D( + _worldVelocityXFilter.Value, + _worldVelocityYFilter.Value, + _angularVelocityFilter.Value) + : Twist2D.Zero; + + return new VehicleState( + sampleTimestampSeconds, + normalizedPoseInWorld, + retainedTwistInWorld, + hasValidVelocityEstimate); + } + + /// + /// 使用当前定位重新建立差分基准，并返回速度无效的零速状态。 + /// + public VehicleState Reset( + Pose2D poseInWorld, + double sampleTimestampSeconds) + { + EnsureFinitePose( + poseInWorld, + nameof(poseInWorld)); + EnsureFiniteNonNegative( + sampleTimestampSeconds, + nameof(sampleTimestampSeconds)); + + _previousPoseInWorld = + new Pose2D( + poseInWorld.XMeters, + poseInWorld.YMeters, + AngleMath.NormalizeRadians( + poseInWorld.YawRadians)); + _previousTimestampSeconds = + sampleTimestampSeconds; + _hasPreviousSample = true; + + _worldVelocityXFilter.Reset(); + _worldVelocityYFilter.Reset(); + _angularVelocityFilter.Reset(); + + return new VehicleState( + sampleTimestampSeconds, + _previousPoseInWorld, + Twist2D.Zero, + false); + } + + /// + /// 清除差分基准和全部滤波历史，使下一帧重新初始化估计器。 + /// + public void Reset() + { + _hasPreviousSample = false; + _previousPoseInWorld = Pose2D.Identity; + _previousTimestampSeconds = 0.0; + + _worldVelocityXFilter.Reset(); + _worldVelocityYFilter.Reset(); + _angularVelocityFilter.Reset(); + } + + /// + /// 检查位姿是否由有限数值组成。 + /// + 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 EnsureFiniteNonNegative( + 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/ClumsyPilot/Trajectory/LegacyTrackAdapter.cs b/ClumsyPilot/Trajectory/LegacyTrackAdapter.cs new file mode 100644 index 0000000..d8a2ebb --- /dev/null +++ b/ClumsyPilot/Trajectory/LegacyTrackAdapter.cs @@ -0,0 +1 @@ +// 兼容现有 MDCS 的 AbstractTrack \ No newline at end of file diff --git a/ClumsyPilot/Trajectory/Trajectory2D.cs b/ClumsyPilot/Trajectory/Trajectory2D.cs new file mode 100644 index 0000000..a991bde --- /dev/null +++ b/ClumsyPilot/Trajectory/Trajectory2D.cs @@ -0,0 +1,260 @@ +using System; +using System.Collections.Generic; +using System.Collections.ObjectModel; +using System.Linq; +using MyParking.Shared; + +namespace MultiWheelC.Trajectory +{ + /// + /// 保存一条经过基本合法性检查的只读二维参考轨迹。 + /// + public sealed class Trajectory2D + { + private const double StartArcLengthToleranceMeters = 1e-9; + private const double MinimumSegmentLengthMeters = 1e-6; + + private readonly TrajectoryPoint[] _points; + private readonly ReadOnlyCollection _readOnlyPoints; + + /// + /// 复制并验证按累计弧长升序排列的参考轨迹点。 + /// + public Trajectory2D( + IEnumerable points) + { + if (points == null) + { + throw new ArgumentNullException( + nameof(points)); + } + + _points = points.ToArray(); + + if (_points.Length < 2) + { + throw new ArgumentException( + "二维轨迹至少需要两个轨迹点。", + nameof(points)); + } + + if (Math.Abs(_points[0].ArcLengthMeters) > + StartArcLengthToleranceMeters) + { + throw new ArgumentException( + "二维轨迹起点的累计弧长必须为0m。", + nameof(points)); + } + + for (var index = 1; + index < _points.Length; + index++) + { + ValidateSegment( + _points[index - 1], + _points[index], + index, + nameof(points)); + } + + _readOnlyPoints = + Array.AsReadOnly(_points); + } + + /// + /// 获取轨迹点数量。 + /// + public int Count => _points.Length; + + /// + /// 获取指定索引处的轨迹点。 + /// + public TrajectoryPoint this[int index] => + _points[index]; + + /// + /// 获取不可修改的有序轨迹点集合。 + /// + public IReadOnlyList Points => + _readOnlyPoints; + + /// + /// 获取轨迹起点。 + /// + public TrajectoryPoint StartPoint => + _points[0]; + + /// + /// 获取轨迹终点。 + /// + public TrajectoryPoint EndPoint => + _points[_points.Length - 1]; + + /// + /// 获取轨迹总弧长，单位为m。 + /// + public double TotalLengthMeters => + EndPoint.ArcLengthMeters; + + /// + /// 根据当前累计弧长计算到轨迹终点的剩余距离。 + /// + public double GetRemainingDistanceMeters( + double arcLengthMeters) + { + EnsureFinite( + arcLengthMeters, + nameof(arcLengthMeters)); + + if (arcLengthMeters <= 0.0) + return TotalLengthMeters; + + if (arcLengthMeters >= TotalLengthMeters) + return 0.0; + + 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; + } + + /// + /// 检查相邻轨迹点是否构成有效的非零长度有序线段。 + /// + private static void ValidateSegment( + TrajectoryPoint previous, + TrajectoryPoint current, + int currentIndex, + string parameterName) + { + if (current.ArcLengthMeters <= + previous.ArcLengthMeters) + { + throw new ArgumentException( + $"轨迹点{currentIndex}的累计弧长必须严格大于前一个点。", + parameterName); + } + + var deltaX = + current.PoseInWorld.XMeters - + previous.PoseInWorld.XMeters; + var deltaY = + current.PoseInWorld.YMeters - + previous.PoseInWorld.YMeters; + var segmentLengthSquared = + deltaX * deltaX + + deltaY * deltaY; + var minimumLengthSquared = + MinimumSegmentLengthMeters * + MinimumSegmentLengthMeters; + + if (segmentLengthSquared < + minimumLengthSquared) + { + throw new ArgumentException( + $"轨迹点{currentIndex}与前一个点的位置过近，无法构成有效投影线段。", + parameterName); + } + } + + /// + /// 检查数值是否为有限值。 + /// + private static void EnsureFinite( + double value, + string parameterName) + { + if (double.IsNaN(value) || + double.IsInfinity(value)) + { + throw new ArgumentOutOfRangeException( + parameterName, + "轨迹弧长必须是有限值。"); + } + } + } +} diff --git a/ClumsyPilot/Trajectory/TrajectoryPoint.cs b/ClumsyPilot/Trajectory/TrajectoryPoint.cs new file mode 100644 index 0000000..83dd56c --- /dev/null +++ b/ClumsyPilot/Trajectory/TrajectoryPoint.cs @@ -0,0 +1,102 @@ +using System; +using MyParking.Shared; + +namespace MultiWheelC.Trajectory +{ + /// + /// 描述按弧长参数化的车体中心参考轨迹点，统一使用SI单位。 + /// + public readonly struct TrajectoryPoint + { + /// + /// 创建包含中心位姿、曲率和速度信息的参考轨迹点。 + /// + public TrajectoryPoint( + double arcLengthMeters, + Pose2D poseInWorld, + double curvaturePerMeter, + double referenceSpeedMetersPerSecond) + { + EnsureFiniteNonNegative( + arcLengthMeters, + nameof(arcLengthMeters)); + EnsureFinite( + poseInWorld.XMeters, + nameof(poseInWorld)); + EnsureFinite( + poseInWorld.YMeters, + nameof(poseInWorld)); + EnsureFinite( + poseInWorld.YawRadians, + nameof(poseInWorld)); + EnsureFinite( + curvaturePerMeter, + nameof(curvaturePerMeter)); + EnsureFinite( + referenceSpeedMetersPerSecond, + nameof(referenceSpeedMetersPerSecond)); + ArcLengthMeters = arcLengthMeters; + PoseInWorld = new Pose2D( + poseInWorld.XMeters, + poseInWorld.YMeters, + AngleMath.NormalizeRadians( + poseInWorld.YawRadians)); + CurvaturePerMeter = curvaturePerMeter; + ReferenceSpeedMetersPerSecond = + referenceSpeedMetersPerSecond; + } + + /// + /// 获取从轨迹起点累计到当前点的弧长，单位为m。 + /// + public double ArcLengthMeters { get; } + + /// + /// 获取车体中心参考坐标系在世界坐标系中的位姿。 + /// + public Pose2D PoseInWorld { get; } + + /// + /// 获取车体中心参考轨迹曲率，单位为1/m，左转为正。 + /// + public double CurvaturePerMeter { get; } + + /// + /// 获取沿轨迹切线方向的有符号参考速度，单位为m/s。 + /// + public double ReferenceSpeedMetersPerSecond { get; } + + /// + /// 检查数值是否为有限值。 + /// + private static void EnsureFinite( + double value, + string parameterName) + { + if (double.IsNaN(value) || + double.IsInfinity(value)) + { + throw new ArgumentOutOfRangeException( + parameterName, + "轨迹点参数必须是有限值。"); + } + } + + /// + /// 检查数值是否为非负有限值。 + /// + private static void EnsureFiniteNonNegative( + double value, + string parameterName) + { + EnsureFinite(value, parameterName); + + if (value < 0.0) + { + throw new ArgumentOutOfRangeException( + parameterName, + "轨迹累计弧长不能为负数。"); + } + } + } +} diff --git a/ClumsyPilot/Trajectory/TrajectoryProjection.cs b/ClumsyPilot/Trajectory/TrajectoryProjection.cs new file mode 100644 index 0000000..798ae17 --- /dev/null +++ b/ClumsyPilot/Trajectory/TrajectoryProjection.cs @@ -0,0 +1,123 @@ +using System; +using MyParking.Shared; + +namespace MultiWheelC.Trajectory +{ + /// + /// 保存车体中心投影到二维参考轨迹后得到的只读结果。 + /// + public readonly struct TrajectoryProjection + { + /// + /// 创建包含轨迹进度、参考状态和跟踪误差的投影结果。 + /// + public TrajectoryProjection( + int segmentStartIndex, + TrajectoryPoint referencePoint, + double lateralErrorMeters, + double headingErrorRadians, + double distanceToTrajectoryMeters, + double remainingDistanceMeters) + { + if (segmentStartIndex < 0) + { + throw new ArgumentOutOfRangeException( + nameof(segmentStartIndex), + "投影线段起点索引不能为负数。"); + } + + EnsureFinite( + lateralErrorMeters, + nameof(lateralErrorMeters)); + EnsureFinite( + headingErrorRadians, + nameof(headingErrorRadians)); + EnsureFiniteNonNegative( + distanceToTrajectoryMeters, + nameof(distanceToTrajectoryMeters)); + EnsureFiniteNonNegative( + remainingDistanceMeters, + nameof(remainingDistanceMeters)); + + SegmentStartIndex = segmentStartIndex; + ReferencePoint = referencePoint; + LateralErrorMeters = lateralErrorMeters; + HeadingErrorRadians = + AngleMath.NormalizeRadians( + headingErrorRadians); + DistanceToTrajectoryMeters = + distanceToTrajectoryMeters; + RemainingDistanceMeters = + remainingDistanceMeters; + } + + /// + /// 获取投影所在轨迹线段的起点索引，线段终点索引为该值加1。 + /// + public int SegmentStartIndex { get; } + + /// + /// 获取投影位置插值得到的车体中心参考轨迹点。 + /// + public TrajectoryPoint ReferencePoint { get; } + + /// + /// 获取有符号横向误差，单位为m，参考轨迹位于车辆左侧时为正。 + /// + public double LateralErrorMeters { get; } + + /// + /// 获取参考航向减实际车体航向的最短角差，单位为rad，逆时针为正。 + /// + public double HeadingErrorRadians { get; } + + /// + /// 获取车体中心到投影点的欧氏距离，单位为m。 + /// + public double DistanceToTrajectoryMeters { get; } + + /// + /// 获取投影位置沿轨迹到终点的剩余弧长，单位为m。 + /// + public double RemainingDistanceMeters { get; } + + /// + /// 获取投影位置从轨迹起点累计的弧长，单位为m。 + /// + public double ArcLengthMeters => + ReferencePoint.ArcLengthMeters; + + /// + /// 检查数值是否为有限值。 + /// + private static void EnsureFinite( + double value, + string parameterName) + { + if (double.IsNaN(value) || + double.IsInfinity(value)) + { + throw new ArgumentOutOfRangeException( + parameterName, + "轨迹投影参数必须是有限值。"); + } + } + + /// + /// 检查数值是否为非负有限值。 + /// + private static void EnsureFiniteNonNegative( + double value, + string parameterName) + { + EnsureFinite(value, parameterName); + + if (value < 0.0) + { + throw new ArgumentOutOfRangeException( + parameterName, + "轨迹投影距离不能为负数。"); + } + } + } +} diff --git a/ClumsyPilot/Trajectory/TrajectoryProjector.cs b/ClumsyPilot/Trajectory/TrajectoryProjector.cs new file mode 100644 index 0000000..43cc3c5 --- /dev/null +++ b/ClumsyPilot/Trajectory/TrajectoryProjector.cs @@ -0,0 +1,219 @@ +using System; +using MyParking.Shared; + +namespace MultiWheelC.Trajectory +{ + /// + /// 将Detour给出的实际车体中心位姿投影到二维离散参考轨迹。 + /// + public static class TrajectoryProjector + { + /// + /// 在整条轨迹上查找距离实际车体中心最近的线段投影结果。 + /// + public static TrajectoryProjection Project( + Trajectory2D trajectory, + Pose2D vehiclePoseInWorld) + { + if (trajectory == null) + { + throw new ArgumentNullException( + nameof(trajectory)); + } + + EnsureFinitePose( + vehiclePoseInWorld, + nameof(vehiclePoseInWorld)); + + var bestSegmentStartIndex = 0; + var bestInterpolationRatio = 0.0; + var bestProjectedX = 0.0; + var bestProjectedY = 0.0; + var bestDistanceSquared = + double.PositiveInfinity; + + for (var segmentStartIndex = 0; + segmentStartIndex < trajectory.Count - 1; + segmentStartIndex++) + { + var segmentStart = + trajectory[segmentStartIndex]; + var segmentEnd = + trajectory[segmentStartIndex + 1]; + + var segmentX = + segmentEnd.PoseInWorld.XMeters - + segmentStart.PoseInWorld.XMeters; + var segmentY = + segmentEnd.PoseInWorld.YMeters - + segmentStart.PoseInWorld.YMeters; + var segmentLengthSquared = + segmentX * segmentX + + segmentY * segmentY; + + var vehicleFromSegmentStartX = + vehiclePoseInWorld.XMeters - + segmentStart.PoseInWorld.XMeters; + var vehicleFromSegmentStartY = + vehiclePoseInWorld.YMeters - + segmentStart.PoseInWorld.YMeters; + + var interpolationRatio = + InterpolationMath.Clamp01( + (vehicleFromSegmentStartX * segmentX + + vehicleFromSegmentStartY * segmentY) / + segmentLengthSquared); + + var projectedX = + InterpolationMath.Lerp( + segmentStart.PoseInWorld.XMeters, + segmentEnd.PoseInWorld.XMeters, + interpolationRatio); + var projectedY = + InterpolationMath.Lerp( + segmentStart.PoseInWorld.YMeters, + segmentEnd.PoseInWorld.YMeters, + interpolationRatio); + + var projectionErrorX = + projectedX - + vehiclePoseInWorld.XMeters; + var projectionErrorY = + projectedY - + vehiclePoseInWorld.YMeters; + var distanceSquared = + projectionErrorX * projectionErrorX + + projectionErrorY * projectionErrorY; + + if (distanceSquared >= bestDistanceSquared) + { + continue; + } + + bestSegmentStartIndex = + segmentStartIndex; + bestInterpolationRatio = + interpolationRatio; + bestProjectedX = projectedX; + bestProjectedY = projectedY; + bestDistanceSquared = distanceSquared; + } + + return BuildProjection( + trajectory, + vehiclePoseInWorld, + bestSegmentStartIndex, + bestInterpolationRatio, + bestProjectedX, + bestProjectedY, + bestDistanceSquared); + } + + /// + /// 根据最近线段和插值比例生成控制器使用的完整投影结果。 + /// + private static TrajectoryProjection BuildProjection( + Trajectory2D trajectory, + Pose2D vehiclePoseInWorld, + int segmentStartIndex, + double interpolationRatio, + double projectedX, + double projectedY, + double distanceSquared) + { + var segmentStart = + trajectory[segmentStartIndex]; + var segmentEnd = + trajectory[segmentStartIndex + 1]; + + var referenceYawRadians = + AngleMath.LerpRadians( + segmentStart.PoseInWorld.YawRadians, + segmentEnd.PoseInWorld.YawRadians, + interpolationRatio); + var referenceArcLengthMeters = + InterpolationMath.Lerp( + segmentStart.ArcLengthMeters, + segmentEnd.ArcLengthMeters, + interpolationRatio); + var referenceCurvaturePerMeter = + InterpolationMath.Lerp( + segmentStart.CurvaturePerMeter, + segmentEnd.CurvaturePerMeter, + interpolationRatio); + var referenceSpeedMetersPerSecond = + InterpolationMath.Lerp( + segmentStart.ReferenceSpeedMetersPerSecond, + segmentEnd.ReferenceSpeedMetersPerSecond, + interpolationRatio); + + var referencePoint = + new TrajectoryPoint( + referenceArcLengthMeters, + new Pose2D( + projectedX, + projectedY, + referenceYawRadians), + referenceCurvaturePerMeter, + referenceSpeedMetersPerSecond); + + var segmentX = + segmentEnd.PoseInWorld.XMeters - + segmentStart.PoseInWorld.XMeters; + var segmentY = + segmentEnd.PoseInWorld.YMeters - + segmentStart.PoseInWorld.YMeters; + var segmentLength = + Math.Sqrt( + segmentX * segmentX + + segmentY * segmentY); + + // 以轨迹线段的前进方向判断左右： + // 从车辆指向参考轨迹的向量位于轨迹左侧时为正。 + var vehicleToProjectionX = + projectedX - + vehiclePoseInWorld.XMeters; + var vehicleToProjectionY = + projectedY - + vehiclePoseInWorld.YMeters; + var lateralErrorMeters = + (segmentX * vehicleToProjectionY - + segmentY * vehicleToProjectionX) / + segmentLength; + + var headingErrorRadians = + AngleMath.ShortestDifferenceRadians( + referenceYawRadians, + vehiclePoseInWorld.YawRadians); + + return new TrajectoryProjection( + segmentStartIndex, + referencePoint, + lateralErrorMeters, + headingErrorRadians, + Math.Sqrt(distanceSquared), + trajectory.GetRemainingDistanceMeters( + referenceArcLengthMeters)); + } + + /// + /// 检查用于投影的实际车体中心位姿是否包含有限数值。 + /// + private static void EnsureFinitePose( + Pose2D pose, + string parameterName) + { + if (double.IsNaN(pose.XMeters) || + double.IsInfinity(pose.XMeters) || + double.IsNaN(pose.YMeters) || + double.IsInfinity(pose.YMeters) || + double.IsNaN(pose.YawRadians) || + double.IsInfinity(pose.YawRadians)) + { + throw new ArgumentOutOfRangeException( + parameterName, + "用于轨迹投影的车体位姿必须是有限值。"); + } + } + } +} diff --git a/ClumsyPilot/ref/CommonUsage.dll b/ClumsyPilot/ref/CommonUsage.dll index effb81e..82a8d1d 100644 Binary files a/ClumsyPilot/ref/CommonUsage.dll and b/ClumsyPilot/ref/CommonUsage.dll differ