diff --git a/MedullaAdapter/build/Medulla/plugins/CommonUsage.dll b/MedullaAdapter/build/Medulla/plugins/CommonUsage.dll index cc330a2..48d9fcb 100644 Binary files a/MedullaAdapter/build/Medulla/plugins/CommonUsage.dll and b/MedullaAdapter/build/Medulla/plugins/CommonUsage.dll differ diff --git a/MedullaAdapter/build/Medulla/plugins/MedullaAdapter.dll b/MedullaAdapter/build/Medulla/plugins/MedullaAdapter.dll index 34dc787..c6e3d74 100644 Binary files a/MedullaAdapter/build/Medulla/plugins/MedullaAdapter.dll and b/MedullaAdapter/build/Medulla/plugins/MedullaAdapter.dll differ diff --git a/MedullaAdapter/build/Medulla/plugins/MedullaAdapter.pdb b/MedullaAdapter/build/Medulla/plugins/MedullaAdapter.pdb index 1e174f5..0409e29 100644 Binary files a/MedullaAdapter/build/Medulla/plugins/MedullaAdapter.pdb and b/MedullaAdapter/build/Medulla/plugins/MedullaAdapter.pdb differ diff --git a/MultiWheelC/StateEstimation/DetourVehicleStateProvider.cs b/MultiWheelC/StateEstimation/DetourVehicleStateProvider.cs index 4a07a9b..eecab94 100644 --- a/MultiWheelC/StateEstimation/DetourVehicleStateProvider.cs +++ b/MultiWheelC/StateEstimation/DetourVehicleStateProvider.cs @@ -80,25 +80,25 @@ namespace MultiWheelC.StateEstimation throw new ArgumentNullException( nameof(velocityEstimator)); - EnsureFinitePositive( + NumericGuard.EnsureFinitePositive( maximumLinearSpeedMetersPerSecond, nameof(maximumLinearSpeedMetersPerSecond)); - EnsureFinitePositive( + NumericGuard.EnsureFinitePositive( maximumAngularSpeedRadiansPerSecond, nameof(maximumAngularSpeedRadiansPerSecond)); - EnsureFiniteNonNegative( + NumericGuard.EnsureFiniteNonNegative( positionJumpMarginMeters, nameof(positionJumpMarginMeters)); - EnsureFiniteNonNegative( + NumericGuard.EnsureFiniteNonNegative( headingJumpMarginRadians, nameof(headingJumpMarginRadians)); - EnsureFinitePositive( + NumericGuard.EnsureFinitePositive( velocityPositionResidualMeters, nameof(velocityPositionResidualMeters)); - EnsureFinitePositive( + NumericGuard.EnsureFinitePositive( velocityHeadingResidualRadians, nameof(velocityHeadingResidualRadians)); - EnsureFinitePositive( + NumericGuard.EnsureFinitePositive( stationaryConfirmationSeconds, nameof(stationaryConfirmationSeconds)); @@ -238,9 +238,9 @@ namespace MultiWheelC.StateEstimation var location = DetourInterface.getCartLocation(); - EnsureFinite(location.x, "DetourX"); - EnsureFinite(location.y, "DetourY"); - EnsureFinite(location.th, "DetourTheta"); + NumericGuard.EnsureFinite(location.x, "DetourX"); + NumericGuard.EnsureFinite(location.y, "DetourY"); + NumericGuard.EnsureFinite(location.th, "DetourTheta"); return new Pose2D( location.x / MillimetersPerMeter, @@ -268,25 +268,6 @@ namespace MultiWheelC.StateEstimation return _latestState; } - /// - /// 接受跳变后的新位姿基准，但不让该位移进入速度差分和低通滤波器。 - /// - private VehicleState AcceptPoseAfterVelocityRebase( - Pose2D poseInWorld, - double timestampSeconds) - { - _latestState = - _velocityEstimator - .RebasePreservingVelocity( - poseInWorld, - timestampSeconds); - _acceptedPoseInWorld = poseInWorld; - _acceptedTimestampSeconds = - timestampSeconds; - _stationaryHoldActive = false; - return _latestState; - } - /// /// 接受首帧或静止后的首个新位姿并重新建立零速差分基准。 /// @@ -340,7 +321,7 @@ namespace MultiWheelC.StateEstimation Pose2D endPoseInWorld, double deltaTimeSeconds) { - if (!IsFinite(deltaTimeSeconds) || + if (!NumericGuard.IsFinite(deltaTimeSeconds) || deltaTimeSeconds <= 0.0) { return false; @@ -448,62 +429,5 @@ namespace MultiWheelC.StateEstimation 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/MultiWheelC/StateEstimation/FirstOrderLowPassFilter.cs b/MultiWheelC/StateEstimation/FirstOrderLowPassFilter.cs index 29464bc..26707f7 100644 --- a/MultiWheelC/StateEstimation/FirstOrderLowPassFilter.cs +++ b/MultiWheelC/StateEstimation/FirstOrderLowPassFilter.cs @@ -1,4 +1,4 @@ -using System; +using MyParking.Shared; namespace MultiWheelC.StateEstimation { @@ -17,7 +17,7 @@ namespace MultiWheelC.StateEstimation public FirstOrderLowPassFilter( double timeConstantSeconds) { - EnsureFinitePositive( + NumericGuard.EnsureFinitePositive( timeConstantSeconds, nameof(timeConstantSeconds)); @@ -25,35 +25,6 @@ namespace MultiWheelC.StateEstimation timeConstantSeconds; } - /// - /// 获取滤波时间常数，单位为s；数值越大，滤波越强但响应越慢。 - /// - public double TimeConstantSeconds => - _timeConstantSeconds; - - /// - /// 获取滤波器是否已经接收过有效初值。 - /// - public bool IsInitialized => - _isInitialized; - - /// - /// 获取当前滤波输出；尚未初始化时读取会抛出异常。 - /// - public double Value - { - get - { - if (!_isInitialized) - { - throw new InvalidOperationException( - "一阶低通滤波器尚未初始化。"); - } - - return _value; - } - } - /// /// 使用当前输入和真实采样间隔更新滤波结果。 /// @@ -61,10 +32,10 @@ namespace MultiWheelC.StateEstimation double input, double deltaTimeSeconds) { - EnsureFinite( + NumericGuard.EnsureFinite( input, nameof(input)); - EnsureFinitePositive( + NumericGuard.EnsureFinitePositive( deltaTimeSeconds, nameof(deltaTimeSeconds)); @@ -98,7 +69,7 @@ namespace MultiWheelC.StateEstimation /// public void Reset(double initialValue) { - EnsureFinite( + NumericGuard.EnsureFinite( initialValue, nameof(initialValue)); @@ -106,37 +77,5 @@ namespace MultiWheelC.StateEstimation _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/MultiWheelC/StateEstimation/VehicleState.cs b/MultiWheelC/StateEstimation/VehicleState.cs index af4814c..a93cc30 100644 --- a/MultiWheelC/StateEstimation/VehicleState.cs +++ b/MultiWheelC/StateEstimation/VehicleState.cs @@ -1,4 +1,3 @@ -using System; using MyParking.Shared; namespace MultiWheelC.StateEstimation @@ -17,13 +16,13 @@ namespace MultiWheelC.StateEstimation Twist2D twistInWorld, bool hasValidVelocityEstimate) { - EnsureFiniteNonNegative( + NumericGuard.EnsureFiniteNonNegative( sampleTimestampSeconds, nameof(sampleTimestampSeconds)); - EnsureFinitePose( + NumericGuard.EnsureFinite( poseInWorld, nameof(poseInWorld)); - EnsureFiniteTwist( + NumericGuard.EnsureFinite( twistInWorld, nameof(twistInWorld)); @@ -73,66 +72,9 @@ namespace MultiWheelC.StateEstimation 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/MultiWheelC/StateEstimation/VelocityEstimator2D.cs b/MultiWheelC/StateEstimation/VelocityEstimator2D.cs index 3d8a85d..dbfd79a 100644 --- a/MultiWheelC/StateEstimation/VelocityEstimator2D.cs +++ b/MultiWheelC/StateEstimation/VelocityEstimator2D.cs @@ -52,12 +52,6 @@ namespace MultiWheelC.StateEstimation angularFilterTimeConstantSeconds); } - /// - /// 获取是否已经保存了可用于下一次差分的位姿基准。 - /// - public bool HasPreviousSample => - _hasPreviousSample; - /// /// 使用一个新的有效定位样本更新并返回车辆状态。 /// @@ -65,10 +59,10 @@ namespace MultiWheelC.StateEstimation Pose2D poseInWorld, double sampleTimestampSeconds) { - EnsureFinitePose( + NumericGuard.EnsureFinite( poseInWorld, nameof(poseInWorld)); - EnsureFiniteNonNegative( + NumericGuard.EnsureFiniteNonNegative( sampleTimestampSeconds, nameof(sampleTimestampSeconds)); @@ -135,53 +129,6 @@ namespace MultiWheelC.StateEstimation 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); - } - /// /// 使用当前定位重新建立差分基准，并返回速度无效的零速状态。 /// @@ -189,10 +136,10 @@ namespace MultiWheelC.StateEstimation Pose2D poseInWorld, double sampleTimestampSeconds) { - EnsureFinitePose( + NumericGuard.EnsureFinite( poseInWorld, nameof(poseInWorld)); - EnsureFiniteNonNegative( + NumericGuard.EnsureFiniteNonNegative( sampleTimestampSeconds, nameof(sampleTimestampSeconds)); @@ -231,45 +178,5 @@ namespace MultiWheelC.StateEstimation _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/MultiWheelC/StateEstimation/WheelFeedbackVehicleStateProvider.cs b/MultiWheelC/StateEstimation/WheelFeedbackVehicleStateProvider.cs index 77537a5..21a0099 100644 --- a/MultiWheelC/StateEstimation/WheelFeedbackVehicleStateProvider.cs +++ b/MultiWheelC/StateEstimation/WheelFeedbackVehicleStateProvider.cs @@ -90,7 +90,7 @@ namespace MultiWheelC.StateEstimation var rawLongitudinalSpeedMetersPerSecond = (double)actualCarSpeed.Vx; - EnsureFinite( + NumericGuard.EnsureFinite( rawLongitudinalSpeedMetersPerSecond, "电机反馈车体纵向速度"); @@ -205,7 +205,7 @@ namespace MultiWheelC.StateEstimation double timestampSeconds, out bool hasValidWheelSpeedEstimate) { - EnsureFiniteNonNegative( + NumericGuard.EnsureFiniteNonNegative( timestampSeconds, nameof(timestampSeconds)); @@ -238,37 +238,5 @@ namespace MultiWheelC.StateEstimation deltaTimeSeconds); } - /// - /// 检查采样时刻是否为非负有限值。 - /// - 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 (double.IsNaN(value) || - double.IsInfinity(value)) - { - throw new ArgumentOutOfRangeException( - parameterName, - "车辆状态输入必须是有限值。"); - } - } } } diff --git a/MultiWheelC/build/Clumsy/CommonUsage.dll b/MultiWheelC/build/Clumsy/CommonUsage.dll index cc330a2..48d9fcb 100644 Binary files a/MultiWheelC/build/Clumsy/CommonUsage.dll and b/MultiWheelC/build/Clumsy/CommonUsage.dll differ diff --git a/MultiWheelC/build/Clumsy/MultiWheelC.dll b/MultiWheelC/build/Clumsy/MultiWheelC.dll index 98814d9..dc9516c 100644 Binary files a/MultiWheelC/build/Clumsy/MultiWheelC.dll and b/MultiWheelC/build/Clumsy/MultiWheelC.dll differ diff --git a/MultiWheelC/build/Clumsy/MultiWheelC.pdb b/MultiWheelC/build/Clumsy/MultiWheelC.pdb index 07890c2..f905320 100644 Binary files a/MultiWheelC/build/Clumsy/MultiWheelC.pdb and b/MultiWheelC/build/Clumsy/MultiWheelC.pdb differ diff --git a/Shared/Chassis/MultiWheelChassisAdapter.cs b/Shared/Chassis/MultiWheelChassisAdapter.cs index 5d2fbe2..b4f3ab9 100644 --- a/Shared/Chassis/MultiWheelChassisAdapter.cs +++ b/Shared/Chassis/MultiWheelChassisAdapter.cs @@ -11,7 +11,6 @@ namespace MyParking.Shared public sealed class MultiWheelChassisAdapter { #region 辅助内容 - private const double RadiansToDegrees = 180.0 / Math.PI; private const float BiasTolerance = 0.001f; private readonly MultiWheelChassis _chassis; /// @@ -44,15 +43,12 @@ namespace MyParking.Shared 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."); - } + NumericGuard.EnsureFinitePositive( + value, + nameof(value)); + EnsureRepresentableAsSingle( + value, + nameof(value)); _chassis.SteeringAlignmentSigmaDegrees = (float)value; @@ -74,18 +70,18 @@ namespace MyParking.Shared private void EnsureMotionFrameIsActive( double motionDirectionRadians) { - ValidateFinite( + NumericGuard.EnsureFinite( motionDirectionRadians, nameof(motionDirectionRadians)); var expectedBiasDegrees = - (float)( - -FrameTransform2D.NormalizeAngle( - motionDirectionRadians) * - RadiansToDegrees); + ConvertRadiansToSingleDegrees( + -AngleMath.NormalizeRadians( + motionDirectionRadians), + nameof(motionDirectionRadians)); var bias = _chassis.GetOriginBias(); var angleErrorDegrees = - NormalizeDegrees( + AngleMath.NormalizeDegrees( bias.Z - expectedBiasDegrees); if (Math.Abs(bias.X) <= BiasTolerance && @@ -102,46 +98,32 @@ namespace MyParking.Shared $"期望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( + EnsureRepresentableAsSingle( twist.VxMetersPerSecond, nameof(twist.VxMetersPerSecond)); - ValidateFinite( + EnsureRepresentableAsSingle( twist.VyMetersPerSecond, nameof(twist.VyMetersPerSecond)); - ValidateFinite( - twist.OmegaRadiansPerSecond, + EnsureRepresentableAsSingle( + AngleMath.RadiansToDegrees( + twist.OmegaRadiansPerSecond), nameof(twist.OmegaRadiansPerSecond)); } /// - /// 检查数值是否为有限值。 + /// 检查数值是否为有限值且可安全转换为float。 /// - private static void ValidateFinite( + private static void EnsureRepresentableAsSingle( double value, string parameterName) { - if (double.IsNaN(value) || - double.IsInfinity(value)) - { - throw new ArgumentOutOfRangeException( - parameterName, - "底盘速度命令不能是NaN或无穷大。"); - } + NumericGuard.EnsureFinite(value, parameterName); if (value > float.MaxValue || value < -float.MaxValue) @@ -152,6 +134,21 @@ namespace MyParking.Shared } } + /// + /// 将有限弧度值转换为float可表示的角度值。 + /// + private static float ConvertRadiansToSingleDegrees( + double angleRadians, + string parameterName) + { + var angleDegrees = + AngleMath.RadiansToDegrees(angleRadians); + EnsureRepresentableAsSingle( + angleDegrees, + parameterName); + return (float)angleDegrees; + } + /// /// 获取最近一次底盘运动分解失败原因。 /// @@ -175,15 +172,15 @@ namespace MyParking.Shared public void ActivateMotionFrame( double motionDirectionRadians) { - ValidateFinite( + NumericGuard.EnsureFinite( motionDirectionRadians, nameof(motionDirectionRadians)); var biasDegrees = - (float)( - -FrameTransform2D.NormalizeAngle( - motionDirectionRadians) * - RadiansToDegrees); + ConvertRadiansToSingleDegrees( + -AngleMath.NormalizeRadians( + motionDirectionRadians), + nameof(motionDirectionRadians)); var currentBias = _chassis.GetOriginBias(); @@ -192,7 +189,7 @@ namespace MyParking.Shared Math.Abs(currentBias.Y) <= BiasTolerance && Math.Abs( - NormalizeDegrees( + AngleMath.NormalizeDegrees( currentBias.Z - biasDegrees)) <= BiasTolerance) @@ -284,9 +281,9 @@ namespace MyParking.Shared var vyMetersPerSecond = (float)command.BodyTwist.VyMetersPerSecond; var omegaDegreesPerSecond = - (float)( - command.BodyTwist.OmegaRadiansPerSecond * - RadiansToDegrees); + ConvertRadiansToSingleDegrees( + command.BodyTwist.OmegaRadiansPerSecond, + nameof(command.BodyTwist.OmegaRadiansPerSecond)); var success = _chassis.SendXYThSpeed( vxMetersPerSecond, vyMetersPerSecond, @@ -312,13 +309,13 @@ namespace MyParking.Shared double steeringRadians, TimeSpan? interval = null) { - ValidateFinite( + NumericGuard.EnsureFinite( motionDirectionRadians, nameof(motionDirectionRadians)); - ValidateFinite( + EnsureRepresentableAsSingle( speedMetersPerSecond, nameof(speedMetersPerSecond)); - ValidateFinite( + NumericGuard.EnsureFinite( steeringRadians, nameof(steeringRadians)); EnsureMotionFrameIsActive( @@ -333,9 +330,9 @@ namespace MyParking.Shared } var steeringDegrees = - (float)( - steeringRadians * - RadiansToDegrees); + ConvertRadiansToSingleDegrees( + steeringRadians, + nameof(steeringRadians)); var success = _chassis.SendMotion( (float)speedMetersPerSecond, @@ -360,13 +357,13 @@ namespace MyParking.Shared double rearAngleRadians, TimeSpan? interval = null) { - ValidateFinite( + EnsureRepresentableAsSingle( speedMetersPerSecond, nameof(speedMetersPerSecond)); - ValidateFinite( + NumericGuard.EnsureFinite( frontAngleRadians, nameof(frontAngleRadians)); - ValidateFinite( + NumericGuard.EnsureFinite( rearAngleRadians, nameof(rearAngleRadians)); EnsureBodyFrameIsActive(); @@ -383,10 +380,12 @@ namespace MyParking.Shared var success = _chassis.SendMotion( (float)speedMetersPerSecond, - (float)(frontAngleRadians * - RadiansToDegrees), - (float)(rearAngleRadians * - RadiansToDegrees), + ConvertRadiansToSingleDegrees( + frontAngleRadians, + nameof(frontAngleRadians)), + ConvertRadiansToSingleDegrees( + rearAngleRadians, + nameof(rearAngleRadians)), interval); if (!success) @@ -422,8 +421,11 @@ namespace MyParking.Shared double directionRadians) { EnsureBodyFrameIsActive(); - var targetDegrees = (float)(FrameTransform2D.NormalizeAngle(directionRadians) * - RadiansToDegrees); + var targetDegrees = + ConvertRadiansToSingleDegrees( + AngleMath.NormalizeRadians( + directionRadians), + nameof(directionRadians)); #pragma warning disable CS0612, CS0618 var wheels = _chassis.GetSteerWheels(); @@ -463,23 +465,21 @@ namespace MyParking.Shared double directionRadians, double toleranceRadians) { - if (double.IsNaN(toleranceRadians) || - double.IsInfinity(toleranceRadians) || - toleranceRadians < 0.0) - { - throw new ArgumentOutOfRangeException( - nameof(toleranceRadians), - "舵轮到位容差必须是非负有限值。"); - } + NumericGuard.EnsureFiniteNonNegative( + toleranceRadians, + nameof(toleranceRadians)); EnsureBodyFrameIsActive(); - var targetDegrees = (float)( - FrameTransform2D.NormalizeAngle(directionRadians) * - 180.0 / Math.PI); + var targetDegrees = + ConvertRadiansToSingleDegrees( + AngleMath.NormalizeRadians( + directionRadians), + nameof(directionRadians)); - var toleranceDegrees = (float)( - Math.Abs(toleranceRadians) * - 180.0 / Math.PI); + var toleranceDegrees = + ConvertRadiansToSingleDegrees( + toleranceRadians, + nameof(toleranceRadians)); #pragma warning disable CS0612, CS0618 var wheels = _chassis.GetSteerWheels(); @@ -507,16 +507,12 @@ namespace MyParking.Shared TimeSpan? interval = null, double alignmentToleranceDegrees = 2.0) { - ValidateFinite( + NumericGuard.EnsureFiniteNonNegative( + alignmentToleranceDegrees, + nameof(alignmentToleranceDegrees)); + EnsureRepresentableAsSingle( alignmentToleranceDegrees, nameof(alignmentToleranceDegrees)); - - if (alignmentToleranceDegrees < 0.0) - { - throw new ArgumentOutOfRangeException( - nameof(alignmentToleranceDegrees), - "自转舵轮到位容差必须是非负有限值。"); - } EnsureBodyFrameIsActive(); @@ -540,23 +536,18 @@ namespace MyParking.Shared double toleranceRadians = 2.0 * Math.PI / 180.0) { - if (double.IsNaN(toleranceRadians) || - double.IsInfinity(toleranceRadians) || - toleranceRadians < 0.0) - { - throw new ArgumentOutOfRangeException( - nameof(toleranceRadians), - "自转状态交接容差必须是非负有限值。"); - } + NumericGuard.EnsureFiniteNonNegative( + toleranceRadians, + nameof(toleranceRadians)); EnsureBodyFrameIsActive(); var success = _chassis .AdoptPreparedRotateWheelsForXYTh( - (float)( - toleranceRadians * - RadiansToDegrees)); + ConvertRadiansToSingleDegrees( + toleranceRadians, + nameof(toleranceRadians))); if (!success) { diff --git a/Shared/Mathematics/FrameTransform2D.cs b/Shared/Mathematics/FrameTransform2D.cs index 58e33db..5ac93d5 100644 --- a/Shared/Mathematics/FrameTransform2D.cs +++ b/Shared/Mathematics/FrameTransform2D.cs @@ -9,27 +9,6 @@ namespace MyParking.Shared /// 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表示源坐标系在目标坐标系中的位姿。 @@ -106,7 +85,7 @@ namespace MyParking.Shared return new Pose2D( childPositionInParent.XMeters, childPositionInParent.YMeters, - NormalizeAngle( + AngleMath.NormalizeRadians( parentFromMiddle.YawRadians + middleFromChild.YawRadians)); } @@ -127,7 +106,7 @@ namespace MyParking.Shared sin * childPoseInParent.XMeters - cos * childPoseInParent.YMeters, - NormalizeAngle( + AngleMath.NormalizeRadians( -childPoseInParent.YawRadians)); } diff --git a/Shared/Models/ChassisCommand.cs b/Shared/Models/ChassisCommand.cs index 5f073fd..79671d5 100644 --- a/Shared/Models/ChassisCommand.cs +++ b/Shared/Models/ChassisCommand.cs @@ -81,36 +81,6 @@ namespace MyParking.Shared 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); - } - } /// /// 单辆车的车体坐标系在车队坐标系中的位姿。 diff --git a/Shared/Validation/NumericGuard.cs b/Shared/Validation/NumericGuard.cs index cf559bd..fcd4de3 100644 --- a/Shared/Validation/NumericGuard.cs +++ b/Shared/Validation/NumericGuard.cs @@ -7,6 +7,15 @@ namespace MyParking.Shared /// public static class NumericGuard { + /// + /// 判断指定浮点数是否既不是NaN也不是无穷大。 + /// + public static bool IsFinite(double value) + { + return !double.IsNaN(value) && + !double.IsInfinity(value); + } + /// /// 确保指定浮点数不是NaN或无穷大。 /// @@ -14,8 +23,7 @@ namespace MyParking.Shared double value, string parameterName) { - if (double.IsNaN(value) || - double.IsInfinity(value)) + if (!IsFinite(value)) { throw new ArgumentOutOfRangeException( parameterName, @@ -64,17 +72,31 @@ namespace MyParking.Shared 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)) + if (!IsFinite(pose.XMeters) || + !IsFinite(pose.YMeters) || + !IsFinite(pose.YawRadians)) { throw new ArgumentOutOfRangeException( parameterName, "二维位姿必须由有限值组成。"); } } + + /// + /// 确保二维刚体速度的两个线速度分量和角速度均为有限值。 + /// + public static void EnsureFinite( + Twist2D twist, + string parameterName) + { + if (!IsFinite(twist.VxMetersPerSecond) || + !IsFinite(twist.VyMetersPerSecond) || + !IsFinite(twist.OmegaRadiansPerSecond)) + { + throw new ArgumentOutOfRangeException( + parameterName, + "二维刚体速度必须由有限值组成。"); + } + } } } diff --git a/docs/数据融合.txt b/docs/数据融合.txt deleted file mode 100644 index b90a927..0000000 --- a/docs/数据融合.txt +++ /dev/null @@ -1,94 +0,0 @@ -是的，强烈建议做系统辨识，尤其是你这种要把电机反馈和 SLAM 融合的场景。 -为什么需要系统辨识？ -卡尔曼滤波（或 EKF）的效果很大程度上取决于过程模型有多准。模型不准的话，会出现： - -预测步持续往错误方向跑 -滤波器过度依赖测量（SLAM），或者反过来过度信任错误的模型 -速度估计系统性偏大/偏小 -原地自转时航向纠正效果变差 - -你现在已经知道电机反馈“偏大”，这本身就是典型的模型参数问题（可能是轮胎半径、减速比、编码器标定、打滑补偿等）。 -建议辨识的主要参数 -针对四轮差速车，优先考虑这些： - -运动学参数 -有效轮半径（或左右轮半径） -轮距（左右轮中心距离） -可能的轴距相关项（如果不是纯差速） - -速度相关 -电机速度到实际车速的缩放系数（你提到的“偏大”问题） -左右轮速度的不对称性（如果存在） -低速段的非线性（静摩擦、死区） - -可选但有用的 -速度偏置 / 零点漂移 -简单的打滑系数（如果经常在特定地面跑） - - -怎么做比较实际？ -不需要一开始就上很复杂的在线辨识，可以分两步： -离线标定（推荐先做） - -让车在平坦地面以不同速度直线行驶、原地旋转，记录： -四个电机反馈速度 -SLAM 给出的位置和航向变化 - -用最小二乘或简单优化，拟合出轮半径、轮距、速度缩放系数等 -原地自转特别适合标定轮距和左右轮差异 - -在线微调（可选） - -把关键参数（比如速度缩放系数、轮半径）也放进状态向量里一起估计（Augmented EKF） -或者用简单的自适应方法，定期用 SLAM 和电机速度的一致性去微调缩放因子 - -实际建议顺序 - -先建立基本运动学模型 + EKF 框架 -做一轮离线系统辨识，把明显偏差（尤其是速度偏大）修正掉 -再跑融合，看残差（innovation）是否变小、速度估计是否更合理 -如果还有残余误差，再考虑把个别参数做成在线估计 - -总结：建完模型后做系统辨识，是让卡尔曼滤波真正好用的关键一步。不做的话，即使融合了电机和 SLAM，效果也容易打折扣。需要的话，我可以给你一个针对差速车的简单离线标定思路（需要记录哪些数据、怎么拟合）。 - -有没有必要做卡尔曼/传感器融合？ -分两层看： - -如果只是为了速度闭环，且当前绿色线已经能让车跑得比较稳 -→ 性价比一般。 -你现在的处理已经解决了最严重的问题，继续花时间做完整的 EKF + 电机融合，短期内收益可能没有那么大。可以先把绿色线再优化一下（比如调整低通截止频率、加一点自适应滤波，或者用更温和的滑动平均），看能不能把残余抖动再压低一点。 -如果你对速度精度、平滑度要求比较高，或者后面还要做更精细的控制（轨迹跟踪、力控、高精度定位等） -→ 值得做，性价比会变高。 -原因是： -绿色线本质还是“从位置差分出来的速度”，天然会有延迟和残余噪声。 -电机反馈是更直接的速度源，频率高、相位更好。 -把电机速度和经过处理的 SLAM 差分速度做融合（哪怕是简单的互补滤波或一阶卡尔曼），通常能同时得到： -更低的噪声 -更小的滞后 -对电机“偏大”问题的一定校正能力 - - - -实际建议（按性价比排序） - -先低成本优化现有绿色线（最快） -检查低通滤波的截止频率是否合适（太低会滞后，太高会留噪声）。 -可以尝试一阶低通 + 中值滤波组合，或者简单的互补滤波（位置差分 + 电机速度）。 -看看闭环时车的实际表现，如果抖动已经可接受，就先用着。 - -中等成本：做简单融合（推荐优先考虑） -不用完整 EKF，先做一个互补滤波或一维卡尔曼只融速度： -高频信任电机反馈速度 -低频用处理后的 SLAM 差分速度去校正偏差 - -实现简单，效果通常比单纯低通好一截，滞后也更小。 - -完整 EKF + 系统辨识 -只有在你需要同时提升位置、航向、速度的整体一致性，或者后面要上更复杂的控制时，再上这个。 -工作量明显更大，但长期收益最高。 - - -总结 -从你这张图来看，目前绿色线已经能用，不是“必须立刻上卡尔曼”的紧急情况。 -但如果你觉得绿色线的残余抖动还是影响闭环效果，或者想要更干净、滞后更小的速度，那么把四个电机反馈融进去（哪怕先做简单融合）是性价比不错的下一步，比继续死磕低通滤波更有前途。 -你现在速度闭环时，车实际表现如何？是觉得绿色线的抖动已经导致控制不稳，还是只是看着不舒服？这能帮助判断要不要马上上融合。 \ No newline at end of file diff --git a/output/C/CommonUsage.dll b/output/C/CommonUsage.dll index cc330a2..48d9fcb 100644 Binary files a/output/C/CommonUsage.dll and b/output/C/CommonUsage.dll differ diff --git a/output/C/MultiWheelC.dll b/output/C/MultiWheelC.dll index 98814d9..dc9516c 100644 Binary files a/output/C/MultiWheelC.dll and b/output/C/MultiWheelC.dll differ diff --git a/output/C/MultiWheelC.pdb b/output/C/MultiWheelC.pdb index 07890c2..f905320 100644 Binary files a/output/C/MultiWheelC.pdb and b/output/C/MultiWheelC.pdb differ diff --git a/output/M/CommonUsage.dll b/output/M/CommonUsage.dll index cc330a2..48d9fcb 100644 Binary files a/output/M/CommonUsage.dll and b/output/M/CommonUsage.dll differ diff --git a/output/M/MedullaAdapter.dll b/output/M/MedullaAdapter.dll index 34dc787..c6e3d74 100644 Binary files a/output/M/MedullaAdapter.dll and b/output/M/MedullaAdapter.dll differ diff --git a/output/M/MedullaAdapter.pdb b/output/M/MedullaAdapter.pdb index 1e174f5..0409e29 100644 Binary files a/output/M/MedullaAdapter.pdb and b/output/M/MedullaAdapter.pdb differ diff --git a/ref/CommonUsage.dll b/ref/CommonUsage.dll index cc330a2..48d9fcb 100644 Binary files a/ref/CommonUsage.dll and b/ref/CommonUsage.dll differ