First_Dev_Dept/ParkingRobot
10
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

备份

Browse Source
This commit is contained in:
yuxiang.shen
2026-08-14 13:55:19 +08:00
parent 7611deefa3
commit a13e345f83
23 changed files with 144 additions and 596 deletions
Download Patch File Download Diff File Expand all files Collapse all files
MedullaAdapter/build/Medulla/plugins/CommonUsage.dll
BIN
View File
Binary file not shown.
MedullaAdapter/build/Medulla/plugins/MedullaAdapter.dll
BIN
View File
Binary file not shown.
MedullaAdapter/build/Medulla/plugins/MedullaAdapter.pdb
BIN
View File
Binary file not shown.
MultiWheelC/StateEstimation/DetourVehicleStateProvider.cs
+11 -87
View File
@@ -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;
}
/// <summary>
/// 接受跳变后的新位姿基准，但不让该位移进入速度差分和低通滤波器。
/// </summary>
private VehicleState AcceptPoseAfterVelocityRebase(
Pose2D poseInWorld,
double timestampSeconds)
{
_latestState =
_velocityEstimator
.RebasePreservingVelocity(
poseInWorld,
timestampSeconds);
_acceptedPoseInWorld = poseInWorld;
_acceptedTimestampSeconds =
timestampSeconds;
_stationaryHoldActive = false;
return _latestState;
}
/// <summary>
/// 接受首帧或静止后的首个新位姿并重新建立零速差分基准。
/// </summary>
@@ -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;
}
/// <summary>
/// 检查数值是否为正有限值。
/// </summary>
private static void EnsureFinitePositive(
double value,
string parameterName)
{
EnsureFinite(value, parameterName);
if (value <= 0.0)
{
throw new ArgumentOutOfRangeException(
parameterName,
"状态源参数必须是正有限值。");
}
}
/// <summary>
/// 检查数值是否为非负有限值。
/// </summary>
private static void EnsureFiniteNonNegative(
double value,
string parameterName)
{
EnsureFinite(value, parameterName);
if (value < 0.0)
{
throw new ArgumentOutOfRangeException(
parameterName,
"状态源参数必须是非负有限值。");
}
}
/// <summary>
/// 检查数值是否为有限值。
/// </summary>
private static void EnsureFinite(
double value,
string parameterName)
{
if (!IsFinite(value))
{
throw new ArgumentOutOfRangeException(
parameterName,
"状态源参数和Detour位姿必须是有限值。");
}
}
/// <summary>
/// 判断数值是否可用于状态估计。
/// </summary>
private static bool IsFinite(double value)
{
return !double.IsNaN(value) &&
!double.IsInfinity(value);
}
}
}
MultiWheelC/StateEstimation/FirstOrderLowPassFilter.cs
+5 -66
View File
@@ -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;
}
/// <summary>
/// 获取滤波时间常数，单位为s；数值越大，滤波越强但响应越慢。
/// </summary>
public double TimeConstantSeconds =>
_timeConstantSeconds;
/// <summary>
/// 获取滤波器是否已经接收过有效初值。
/// </summary>
public bool IsInitialized =>
_isInitialized;
/// <summary>
/// 获取当前滤波输出；尚未初始化时读取会抛出异常。
/// </summary>
public double Value
{
get
{
if (!_isInitialized)
{
throw new InvalidOperationException(
"一阶低通滤波器尚未初始化。");
}
return _value;
}
}
/// <summary>
/// 使用当前输入和真实采样间隔更新滤波结果。
/// </summary>
@@ -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
/// </summary>
public void Reset(double initialValue)
{
EnsureFinite(
NumericGuard.EnsureFinite(
initialValue,
nameof(initialValue));
@@ -106,37 +77,5 @@ namespace MultiWheelC.StateEstimation
_isInitialized = true;
}
/// <summary>
/// 检查数值是否为正有限值。
/// </summary>
private static void EnsureFinitePositive(
double value,
string parameterName)
{
EnsureFinite(value, parameterName);
if (value <= 0.0)
{
throw new ArgumentOutOfRangeException(
parameterName,
"滤波时间常数和采样间隔必须是正有限值。");
}
}
/// <summary>
/// 检查数值是否为有限值。
/// </summary>
private static void EnsureFinite(
double value,
string parameterName)
{
if (double.IsNaN(value) ||
double.IsInfinity(value))
{
throw new ArgumentOutOfRangeException(
parameterName,
"滤波输入必须是有限值。");
}
}
}
}
MultiWheelC/StateEstimation/VehicleState.cs
+4 -62
View File
@@ -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; }
/// <summary>
/// 获取当前速度是否已由至少两个连续有效定位样本估算得到。
/// 获取当前速度估计是否已经初始化并可用于闭环控制。
/// </summary>
public bool HasValidVelocityEstimate { get; }
/// <summary>
/// 检查位姿是否由有限数值组成。
/// </summary>
private static void EnsureFinitePose(
Pose2D pose,
string parameterName)
{
if (!IsFinite(pose.XMeters) ||
!IsFinite(pose.YMeters) ||
!IsFinite(pose.YawRadians))
{
throw new ArgumentOutOfRangeException(
parameterName,
"车辆位姿必须由有限数值组成。");
}
}
/// <summary>
/// 检查速度是否由有限数值组成。
/// </summary>
private static void EnsureFiniteTwist(
Twist2D twist,
string parameterName)
{
if (!IsFinite(twist.VxMetersPerSecond) ||
!IsFinite(twist.VyMetersPerSecond) ||
!IsFinite(twist.OmegaRadiansPerSecond))
{
throw new ArgumentOutOfRangeException(
parameterName,
"车辆速度必须由有限数值组成。");
}
}
/// <summary>
/// 检查数值是否为非负有限值。
/// </summary>
private static void EnsureFiniteNonNegative(
double value,
string parameterName)
{
if (!IsFinite(value) || value < 0.0)
{
throw new ArgumentOutOfRangeException(
parameterName,
"采样时刻必须是非负有限值。");
}
}
/// <summary>
/// 判断数值是否可用于车辆状态计算。
/// </summary>
private static bool IsFinite(double value)
{
return !double.IsNaN(value) &&
!double.IsInfinity(value);
}
}
}
MultiWheelC/StateEstimation/VelocityEstimator2D.cs
+4 -97
View File
@@ -52,12 +52,6 @@ namespace MultiWheelC.StateEstimation
angularFilterTimeConstantSeconds);
}
/// <summary>
/// 获取是否已经保存了可用于下一次差分的位姿基准。
/// </summary>
public bool HasPreviousSample =>
_hasPreviousSample;
/// <summary>
/// 使用一个新的有效定位样本更新并返回车辆状态。
/// </summary>
@@ -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);
}
/// <summary>
/// 更新位姿差分基准但保留当前滤波速度，避免定位跳变形成虚假速度尖峰。
/// </summary>
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);
}
/// <summary>
/// 使用当前定位重新建立差分基准，并返回速度无效的零速状态。
/// </summary>
@@ -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();
}
/// <summary>
/// 检查位姿是否由有限数值组成。
/// </summary>
private static void EnsureFinitePose(
Pose2D pose,
string parameterName)
{
if (!IsFinite(pose.XMeters) ||
!IsFinite(pose.YMeters) ||
!IsFinite(pose.YawRadians))
{
throw new ArgumentOutOfRangeException(
parameterName,
"速度估计使用的车辆位姿必须由有限数值组成。");
}
}
/// <summary>
/// 检查数值是否为非负有限值。
/// </summary>
private static void EnsureFiniteNonNegative(
double value,
string parameterName)
{
if (!IsFinite(value) || value < 0.0)
{
throw new ArgumentOutOfRangeException(
parameterName,
"速度估计使用的采样时刻必须是非负有限值。");
}
}
/// <summary>
/// 判断数值是否可用于速度估计。
/// </summary>
private static bool IsFinite(double value)
{
return !double.IsNaN(value) &&
!double.IsInfinity(value);
}
}
}
MultiWheelC/StateEstimation/WheelFeedbackVehicleStateProvider.cs
+2 -34
View File
@@ -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);
}
/// <summary>
/// 检查采样时刻是否为非负有限值。
/// </summary>
private static void EnsureFiniteNonNegative(
double value,
string parameterName)
{
EnsureFinite(value, parameterName);
if (value < 0.0)
{
throw new ArgumentOutOfRangeException(
parameterName,
"采样时刻必须是非负有限值。");
}
}
/// <summary>
/// 检查状态输入是否为有限值。
/// </summary>
private static void EnsureFinite(
double value,
string parameterName)
{
if (double.IsNaN(value) ||
double.IsInfinity(value))
{
throw new ArgumentOutOfRangeException(
parameterName,
"车辆状态输入必须是有限值。");
}
}
}
}
MultiWheelC/build/Clumsy/CommonUsage.dll
BIN
View File
Binary file not shown.
MultiWheelC/build/Clumsy/MultiWheelC.dll
BIN
View File
Binary file not shown.
MultiWheelC/build/Clumsy/MultiWheelC.pdb
BIN
View File
Binary file not shown.
Shared/Chassis/MultiWheelChassisAdapter.cs
+86 -95
View File
@@ -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;
/// <summary>
@@ -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}°。");
}
/// <summary>
/// 将角度归一化到[-180°,180°]附近。
/// </summary>
private static float NormalizeDegrees(float degrees)
{
return (float)(
degrees -
Math.Round(degrees / 360.0) * 360.0);
}
/// <summary>
/// 检查底盘命令是否包含无效数值。
/// </summary>
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));
}
/// <summary>
/// 检查数值是否为有限值。
/// 检查数值是否为有限值且可安全转换为float。
/// </summary>
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
}
}
/// <summary>
/// 将有限弧度值转换为float可表示的角度值。
/// </summary>
private static float ConvertRadiansToSingleDegrees(
double angleRadians,
string parameterName)
{
var angleDegrees =
AngleMath.RadiansToDegrees(angleRadians);
EnsureRepresentableAsSingle(
angleDegrees,
parameterName);
return (float)angleDegrees;
}
/// <summary>
/// 获取最近一次底盘运动分解失败原因。
/// </summary>
@@ -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)
{
Shared/Mathematics/FrameTransform2D.cs
+2 -23
View File
@@ -9,27 +9,6 @@ namespace MyParking.Shared
/// </summary>
public static class FrameTransform2D
{
/// <summary>
/// 将角度归一化到[-π, π)范围。
/// </summary>
public static double NormalizeAngle(double angleRadians)
{
return AngleMath.NormalizeRadians(angleRadians);
}
/// <summary>
/// 计算从current到target的最短角度差。
/// 返回正值表示逆时针旋转。
/// </summary>
public static double ShortestAngleDifference(
double targetRadians,
double currentRadians)
{
return AngleMath.ShortestDifferenceRadians(
targetRadians,
currentRadians);
}
/// <summary>
/// 将源坐标系中的点变换到目标坐标系。
/// 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));
}
Shared/Models/ChassisCommand.cs
-30
View File
@@ -81,36 +81,6 @@ namespace MyParking.Shared
new Twist2D(0.0, 0.0, 0.0);
}
/// <summary>
/// 发送给单辆车的车体坐标系速度命令。
/// </summary>
public readonly struct ChassisCommand
{
public ChassisCommand(
int vehicleId,
Twist2D bodyTwist)
{
VehicleId = vehicleId;
BodyTwist = bodyTwist;
}
public int VehicleId { get; }
/// <summary>
/// 单车车体坐标系速度：X向前、Y向左、逆时针旋转为正。
/// </summary>
public Twist2D BodyTwist { get; }
/// <summary>
/// 创建指定车辆的停止命令。
/// </summary>
public static ChassisCommand Stop(int vehicleId)
{
return new ChassisCommand(
vehicleId,
Twist2D.Zero);
}
}
/// <summary>
/// 单辆车的车体坐标系在车队坐标系中的位姿。
Shared/Validation/NumericGuard.cs
+30 -8
View File
@@ -7,6 +7,15 @@ namespace MyParking.Shared
/// </summary>
public static class NumericGuard
{
/// <summary>
/// 判断指定浮点数是否既不是NaN也不是无穷大。
/// </summary>
public static bool IsFinite(double value)
{
return !double.IsNaN(value) &&
!double.IsInfinity(value);
}
/// <summary>
/// 确保指定浮点数不是NaN或无穷大。
/// </summary>
@@ -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,
"二维位姿必须由有限值组成。");
}
}
/// <summary>
/// 确保二维刚体速度的两个线速度分量和角速度均为有限值。
/// </summary>
public static void EnsureFinite(
Twist2D twist,
string parameterName)
{
if (!IsFinite(twist.VxMetersPerSecond) ||
!IsFinite(twist.VyMetersPerSecond) ||
!IsFinite(twist.OmegaRadiansPerSecond))
{
throw new ArgumentOutOfRangeException(
parameterName,
"二维刚体速度必须由有限值组成。");
}
}
}
}
docs/数据融合.txt
-94
View File
@@ -1,94 +0,0 @@
是的，强烈建议做系统辨识，尤其是你这种要把电机反馈和 SLAM 融合的场景。
为什么需要系统辨识？
卡尔曼滤波（或 EKF）的效果很大程度上取决于过程模型有多准。模型不准的话，会出现：
预测步持续往错误方向跑
滤波器过度依赖测量（SLAM），或者反过来过度信任错误的模型
速度估计系统性偏大/偏小
原地自转时航向纠正效果变差
你现在已经知道电机反馈“偏大”，这本身就是典型的模型参数问题（可能是轮胎半径、减速比、编码器标定、打滑补偿等）。
建议辨识的主要参数
针对四轮差速车，优先考虑这些：
运动学参数
有效轮半径（或左右轮半径）
轮距（左右轮中心距离）
可能的轴距相关项（如果不是纯差速）
速度相关
电机速度到实际车速的缩放系数（你提到的“偏大”问题）
左右轮速度的不对称性（如果存在）
低速段的非线性（静摩擦、死区）
可选但有用的
速度偏置 / 零点漂移
简单的打滑系数（如果经常在特定地面跑）
怎么做比较实际？
不需要一开始就上很复杂的在线辨识，可以分两步：
离线标定（推荐先做）
让车在平坦地面以不同速度直线行驶、原地旋转，记录：
四个电机反馈速度
SLAM 给出的位置和航向变化
用最小二乘或简单优化，拟合出轮半径、轮距、速度缩放系数等
原地自转特别适合标定轮距和左右轮差异
在线微调（可选）
把关键参数（比如速度缩放系数、轮半径）也放进状态向量里一起估计（Augmented EKF）
或者用简单的自适应方法，定期用 SLAM 和电机速度的一致性去微调缩放因子
实际建议顺序
先建立基本运动学模型 + EKF 框架
做一轮离线系统辨识，把明显偏差（尤其是速度偏大）修正掉
再跑融合，看残差（innovation）是否变小、速度估计是否更合理
如果还有残余误差，再考虑把个别参数做成在线估计
总结：建完模型后做系统辨识，是让卡尔曼滤波真正好用的关键一步。不做的话，即使融合了电机和 SLAM，效果也容易打折扣。需要的话，我可以给你一个针对差速车的简单离线标定思路（需要记录哪些数据、怎么拟合）。
有没有必要做卡尔曼/传感器融合？
分两层看：
如果只是为了速度闭环，且当前绿色线已经能让车跑得比较稳
→ 性价比一般。
你现在的处理已经解决了最严重的问题，继续花时间做完整的 EKF + 电机融合，短期内收益可能没有那么大。可以先把绿色线再优化一下（比如调整低通截止频率、加一点自适应滤波，或者用更温和的滑动平均），看能不能把残余抖动再压低一点。
如果你对速度精度、平滑度要求比较高，或者后面还要做更精细的控制（轨迹跟踪、力控、高精度定位等）
→ 值得做，性价比会变高。
原因是：
绿色线本质还是“从位置差分出来的速度”，天然会有延迟和残余噪声。
电机反馈是更直接的速度源，频率高、相位更好。
把电机速度和经过处理的 SLAM 差分速度做融合（哪怕是简单的互补滤波或一阶卡尔曼），通常能同时得到：
更低的噪声
更小的滞后
对电机“偏大”问题的一定校正能力
实际建议（按性价比排序）
先低成本优化现有绿色线（最快）
检查低通滤波的截止频率是否合适（太低会滞后，太高会留噪声）。
可以尝试一阶低通 + 中值滤波组合，或者简单的互补滤波（位置差分 + 电机速度）。
看看闭环时车的实际表现，如果抖动已经可接受，就先用着。
中等成本：做简单融合（推荐优先考虑）
不用完整 EKF，先做一个互补滤波或一维卡尔曼只融速度：
高频信任电机反馈速度
低频用处理后的 SLAM 差分速度去校正偏差
实现简单，效果通常比单纯低通好一截，滞后也更小。
完整 EKF + 系统辨识
只有在你需要同时提升位置、航向、速度的整体一致性，或者后面要上更复杂的控制时，再上这个。
工作量明显更大，但长期收益最高。
总结
从你这张图来看，目前绿色线已经能用，不是“必须立刻上卡尔曼”的紧急情况。
但如果你觉得绿色线的残余抖动还是影响闭环效果，或者想要更干净、滞后更小的速度，那么把四个电机反馈融进去（哪怕先做简单融合）是性价比不错的下一步，比继续死磕低通滤波更有前途。
你现在速度闭环时，车实际表现如何？是觉得绿色线的抖动已经导致控制不稳，还是只是看着不舒服？这能帮助判断要不要马上上融合。
output/C/CommonUsage.dll
BIN
View File
Binary file not shown.
output/C/MultiWheelC.dll
BIN
View File
Binary file not shown.
output/C/MultiWheelC.pdb
BIN
View File
Binary file not shown.
output/M/CommonUsage.dll
BIN
View File
Binary file not shown.
output/M/MedullaAdapter.dll
BIN
View File
Binary file not shown.
output/M/MedullaAdapter.pdb
BIN
View File
Binary file not shown.
ref/CommonUsage.dll
BIN
View File
Binary file not shown.