diff --git a/MedullaAdapter/build/Medulla/plugins/CommonUsage.dll b/MedullaAdapter/build/Medulla/plugins/CommonUsage.dll
index 71ef99f..9ef0dbd 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 0602883..54db927 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 517fda7..f777dfe 100644
Binary files a/MedullaAdapter/build/Medulla/plugins/MedullaAdapter.pdb and b/MedullaAdapter/build/Medulla/plugins/MedullaAdapter.pdb differ
diff --git a/MultiWheelC/Control/Allocation/GcpKinematics.cs b/MultiWheelC/Control/Allocation/GcpKinematics.cs
new file mode 100644
index 0000000..952c2c6
--- /dev/null
+++ b/MultiWheelC/Control/Allocation/GcpKinematics.cs
@@ -0,0 +1,125 @@
+using System;
+using MyParking.Shared;
+
+namespace MultiWheelC.Control.Allocation
+{
+ ///
+ /// 在对称前后GCP方向命令与车体中心刚体速度之间执行纯几何转换。
+ ///
+ public static class GcpKinematics
+ {
+ private const double ParallelDirectionTolerance = 1e-9;
+ private const double StopSpeedDeadbandMetersPerSecond = 1e-6;
+
+ ///
+ /// 将有符号中心速度和前后GCP方向转换为真实车体坐标系中的Twist2D。
+ ///
+ public static Twist2D ToBodyTwist(
+ GcpMotionCommand command,
+ double controlPointRadiusMeters)
+ {
+ NumericGuard.EnsureFinitePositive(
+ controlPointRadiusMeters,
+ nameof(controlPointRadiusMeters));
+
+ if (Math.Abs(command.SpeedMetersPerSecond) <=
+ StopSpeedDeadbandMetersPerSecond)
+ {
+ return Twist2D.Zero;
+ }
+
+ var frontAngleRadians =
+ command.FrontAngleRadians;
+ var rearAngleRadians =
+ command.RearAngleRadians;
+ var directionDeterminant =
+ Math.Sin(
+ rearAngleRadians -
+ frontAngleRadians);
+
+ if (Math.Abs(directionDeterminant) <=
+ ParallelDirectionTolerance)
+ {
+ var averageDirectionRadians =
+ Math.Atan2(
+ Math.Sin(frontAngleRadians) +
+ Math.Sin(rearAngleRadians),
+ Math.Cos(frontAngleRadians) +
+ Math.Cos(rearAngleRadians));
+
+ return new Twist2D(
+ command.SpeedMetersPerSecond *
+ Math.Cos(averageDirectionRadians),
+ command.SpeedMetersPerSecond *
+ Math.Sin(averageDirectionRadians),
+ 0.0);
+ }
+
+ var frontCosine =
+ Math.Cos(frontAngleRadians);
+ var frontSine =
+ Math.Sin(frontAngleRadians);
+ var rearCosine =
+ Math.Cos(rearAngleRadians);
+ var rearSine =
+ Math.Sin(rearAngleRadians);
+
+ // 两个GCP速度方向的法线交点就是瞬时旋转中心，坐标位于真实车体系。
+ var rotationCenterXMeters =
+ controlPointRadiusMeters *
+ (frontCosine * rearSine +
+ frontSine * rearCosine) /
+ directionDeterminant;
+ var rotationCenterYMeters =
+ -2.0 *
+ controlPointRadiusMeters *
+ frontCosine *
+ rearCosine /
+ directionDeterminant;
+ var centerRadiusMeters =
+ Math.Sqrt(
+ rotationCenterXMeters *
+ rotationCenterXMeters +
+ rotationCenterYMeters *
+ rotationCenterYMeters);
+
+ if (!NumericGuard.IsFinite(centerRadiusMeters) ||
+ centerRadiusMeters <= 0.0)
+ {
+ throw new InvalidOperationException(
+ "前后GCP方向不能生成有效的车体中心旋转半径。");
+ }
+
+ // 用有符号速度决定绕ICR的实际转向，避免倒车时把同一组GCP轴向解释成反向运动。
+ var requestedDirectionSign =
+ Math.Sign(command.SpeedMetersPerSecond);
+ var positiveAngularFrontVelocityXMetersPerSecond =
+ rotationCenterYMeters;
+ var positiveAngularFrontVelocityYMetersPerSecond =
+ controlPointRadiusMeters -
+ rotationCenterXMeters;
+ var frontDirectionAlignment =
+ positiveAngularFrontVelocityXMetersPerSecond *
+ requestedDirectionSign *
+ frontCosine +
+ positiveAngularFrontVelocityYMetersPerSecond *
+ requestedDirectionSign *
+ frontSine;
+ var omegaSign =
+ frontDirectionAlignment >= 0.0
+ ? 1.0
+ : -1.0;
+ var omegaRadiansPerSecond =
+ omegaSign *
+ Math.Abs(command.SpeedMetersPerSecond) /
+ centerRadiusMeters;
+
+ return new Twist2D(
+ omegaRadiansPerSecond *
+ rotationCenterYMeters,
+ -omegaRadiansPerSecond *
+ rotationCenterXMeters,
+ omegaRadiansPerSecond);
+ }
+ }
+}
diff --git a/MultiWheelC/Experiments/CompositeMotionPlanTests.cs b/MultiWheelC/Experiments/CompositeMotionPlanTests.cs
index f7a9a1d..047557b 100644
--- a/MultiWheelC/Experiments/CompositeMotionPlanTests.cs
+++ b/MultiWheelC/Experiments/CompositeMotionPlanTests.cs
@@ -307,6 +307,8 @@ namespace MultiWheelC
out var detourVelocityValid,
out var rawWheelBodyVx,
out var filteredWheelBodyVx,
+ out var rawWheelBodyVy,
+ out var filteredWheelBodyVy,
out var wheelVelocityValid))
{
_recorder?.UpdateVelocityDiagnostics(
@@ -314,6 +316,8 @@ namespace MultiWheelC
detourVelocityValid,
rawWheelBodyVx,
filteredWheelBodyVx,
+ rawWheelBodyVy,
+ filteredWheelBodyVy,
wheelVelocityValid);
}
diff --git a/MultiWheelC/Experiments/NewControllerTrackingTests.cs b/MultiWheelC/Experiments/NewControllerTrackingTests.cs
index a5fa694..2ac7e9f 100644
--- a/MultiWheelC/Experiments/NewControllerTrackingTests.cs
+++ b/MultiWheelC/Experiments/NewControllerTrackingTests.cs
@@ -224,6 +224,9 @@ namespace MultiWheelC
referenceSpeed:
(float)CruiseSpeedMetersPerSecond,
sampleIntervalMs: 50,
+ referenceMotionFrameYawDegrees:
+ (float)AngleMath.RadiansToDegrees(
+ MotionDirectionInBodyRadians),
referenceAccelerationMetersPerSecondSquared:
(float)AccelerationMetersPerSecondSquared,
referenceDecelerationMetersPerSecondSquared:
@@ -432,6 +435,8 @@ namespace MultiWheelC
out var detourVelocityValid,
out var rawWheelBodyVx,
out var filteredWheelBodyVx,
+ out var rawWheelBodyVy,
+ out var filteredWheelBodyVy,
out var wheelVelocityValid))
{
return;
@@ -442,6 +447,8 @@ namespace MultiWheelC
detourVelocityValid,
rawWheelBodyVx,
filteredWheelBodyVx,
+ rawWheelBodyVy,
+ filteredWheelBodyVy,
wheelVelocityValid);
}
@@ -855,6 +862,8 @@ namespace MultiWheelC
out var detourVelocityValid,
out var rawWheelBodyVx,
out var filteredWheelBodyVx,
+ out var rawWheelBodyVy,
+ out var filteredWheelBodyVy,
out var wheelVelocityValid))
{
return;
@@ -865,6 +874,8 @@ namespace MultiWheelC
detourVelocityValid,
rawWheelBodyVx,
filteredWheelBodyVx,
+ rawWheelBodyVy,
+ filteredWheelBodyVy,
wheelVelocityValid);
}
diff --git a/MultiWheelC/Experiments/TrackingExperimentRecorder.cs b/MultiWheelC/Experiments/TrackingExperimentRecorder.cs
index 95fc6fd..b04bac1 100644
--- a/MultiWheelC/Experiments/TrackingExperimentRecorder.cs
+++ b/MultiWheelC/Experiments/TrackingExperimentRecorder.cs
@@ -53,12 +53,14 @@ namespace MultiWheelC
public double CurvaturePreviewDistanceMeters;
public double FeedforwardCurvaturePerMeter;
- // 并列保存Detour速度与轮速解算速度，避免StateBodyVx的数据来源产生歧义。
+ // 并列保存Detour速度与轮速解算速度，避免StateBodyVx/Vy的数据来源产生歧义。
public bool HasVelocityDiagnostics;
public double DetourEstimatedBodyVxMetersPerSecond;
public bool DetourVelocityEstimateValid;
public double WheelFeedbackRawBodyVxMetersPerSecond;
public double WheelFeedbackFilteredBodyVxMetersPerSecond;
+ public double WheelFeedbackRawBodyVyMetersPerSecond;
+ public double WheelFeedbackFilteredBodyVyMetersPerSecond;
public bool WheelFeedbackVelocityEstimateValid;
// 四舵轮机械角使用deg，前后虚拟GCP命令角使用rad。
@@ -176,6 +178,8 @@ namespace MultiWheelC
private bool _detourVelocityEstimateValid;
private double _wheelFeedbackRawBodyVxMetersPerSecond;
private double _wheelFeedbackFilteredBodyVxMetersPerSecond;
+ private double _wheelFeedbackRawBodyVyMetersPerSecond;
+ private double _wheelFeedbackFilteredBodyVyMetersPerSecond;
private bool _wheelFeedbackVelocityEstimateValid;
private bool _hasGcpCommand;
private double _requestedFrontGcpAngleRadians;
@@ -340,13 +344,15 @@ namespace MultiWheelC
}
///
- /// 保存同一控制周期的Detour纵向速度以及轮速解算的原始和滤波纵向速度。
+ /// 保存同一控制周期的Detour纵向速度以及轮速解算的原始和滤波平面速度。
///
public void UpdateVelocityDiagnostics(
double detourEstimatedBodyVxMetersPerSecond,
bool detourVelocityEstimateValid,
double wheelFeedbackRawBodyVxMetersPerSecond,
double wheelFeedbackFilteredBodyVxMetersPerSecond,
+ double wheelFeedbackRawBodyVyMetersPerSecond,
+ double wheelFeedbackFilteredBodyVyMetersPerSecond,
bool wheelFeedbackVelocityEstimateValid)
{
lock (_stateSyncRoot)
@@ -359,6 +365,10 @@ namespace MultiWheelC
wheelFeedbackRawBodyVxMetersPerSecond;
_wheelFeedbackFilteredBodyVxMetersPerSecond =
wheelFeedbackFilteredBodyVxMetersPerSecond;
+ _wheelFeedbackRawBodyVyMetersPerSecond =
+ wheelFeedbackRawBodyVyMetersPerSecond;
+ _wheelFeedbackFilteredBodyVyMetersPerSecond =
+ wheelFeedbackFilteredBodyVyMetersPerSecond;
_wheelFeedbackVelocityEstimateValid =
wheelFeedbackVelocityEstimateValid;
_hasVelocityDiagnostics = true;
@@ -543,6 +553,8 @@ namespace MultiWheelC
bool detourVelocityEstimateValid;
double wheelFeedbackRawBodyVxMetersPerSecond;
double wheelFeedbackFilteredBodyVxMetersPerSecond;
+ double wheelFeedbackRawBodyVyMetersPerSecond;
+ double wheelFeedbackFilteredBodyVyMetersPerSecond;
bool wheelFeedbackVelocityEstimateValid;
bool hasGcpCommand;
double requestedFrontGcpAngleRadians;
@@ -618,6 +630,10 @@ namespace MultiWheelC
_wheelFeedbackRawBodyVxMetersPerSecond;
wheelFeedbackFilteredBodyVxMetersPerSecond =
_wheelFeedbackFilteredBodyVxMetersPerSecond;
+ wheelFeedbackRawBodyVyMetersPerSecond =
+ _wheelFeedbackRawBodyVyMetersPerSecond;
+ wheelFeedbackFilteredBodyVyMetersPerSecond =
+ _wheelFeedbackFilteredBodyVyMetersPerSecond;
wheelFeedbackVelocityEstimateValid =
_wheelFeedbackVelocityEstimateValid;
hasGcpCommand = _hasGcpCommand;
@@ -681,6 +697,10 @@ namespace MultiWheelC
wheelFeedbackRawBodyVxMetersPerSecond,
WheelFeedbackFilteredBodyVxMetersPerSecond =
wheelFeedbackFilteredBodyVxMetersPerSecond,
+ WheelFeedbackRawBodyVyMetersPerSecond =
+ wheelFeedbackRawBodyVyMetersPerSecond,
+ WheelFeedbackFilteredBodyVyMetersPerSecond =
+ wheelFeedbackFilteredBodyVyMetersPerSecond,
WheelFeedbackVelocityEstimateValid =
wheelFeedbackVelocityEstimateValid,
HasGcpCommand = hasGcpCommand,
@@ -905,6 +925,8 @@ namespace MultiWheelC
"DetourVelocityEstimateValid," +
"WheelFeedbackRawBodyVxMetersPerSecond," +
"WheelFeedbackFilteredBodyVxMetersPerSecond," +
+ "WheelFeedbackRawBodyVyMetersPerSecond," +
+ "WheelFeedbackFilteredBodyVyMetersPerSecond," +
"WheelFeedbackVelocityEstimateValid," +
"HasSteeringDiagnostics," +
"TargetSteerLeftFrontDegrees," +
@@ -1034,6 +1056,12 @@ namespace MultiWheelC
FormatOptional(
sample.HasVelocityDiagnostics,
sample.WheelFeedbackFilteredBodyVxMetersPerSecond),
+ FormatOptional(
+ sample.HasVelocityDiagnostics,
+ sample.WheelFeedbackRawBodyVyMetersPerSecond),
+ FormatOptional(
+ sample.HasVelocityDiagnostics,
+ sample.WheelFeedbackFilteredBodyVyMetersPerSecond),
sample.HasVelocityDiagnostics
? sample.WheelFeedbackVelocityEstimateValid
? "1"
diff --git a/MultiWheelC/StateEstimation/ParkingVehicleStateProviderFactory.cs b/MultiWheelC/StateEstimation/ParkingVehicleStateProviderFactory.cs
index 19474e4..4d1a4e0 100644
--- a/MultiWheelC/StateEstimation/ParkingVehicleStateProviderFactory.cs
+++ b/MultiWheelC/StateEstimation/ParkingVehicleStateProviderFactory.cs
@@ -5,7 +5,7 @@ using MyParking.Shared;
namespace MultiWheelC.StateEstimation
{
///
- /// 根据车载配置创建Detour位姿过滤与电机反馈纵向速度组合的停车状态源。
+ /// 根据车载配置创建Detour位姿过滤与电机反馈平面速度组合的停车状态源。
///
public static class ParkingVehicleStateProviderFactory
{
diff --git a/MultiWheelC/StateEstimation/WheelFeedbackVehicleStateProvider.cs b/MultiWheelC/StateEstimation/WheelFeedbackVehicleStateProvider.cs
index 21a0099..efdb882 100644
--- a/MultiWheelC/StateEstimation/WheelFeedbackVehicleStateProvider.cs
+++ b/MultiWheelC/StateEstimation/WheelFeedbackVehicleStateProvider.cs
@@ -6,7 +6,7 @@ using System.Diagnostics;
namespace MultiWheelC.StateEstimation
{
///
- /// 保留外部状态源的Detour位姿，并以舵轮电机反馈解算的车体纵向速度替换Detour差分纵向速度。
+ /// 保留外部状态源的Detour位姿，并以舵轮电机反馈解算的车体平面速度替换Detour差分线速度。
///
public sealed class WheelFeedbackVehicleStateProvider
: IVehicleStateProvider
@@ -19,6 +19,7 @@ namespace MultiWheelC.StateEstimation
private readonly IVehicleStateProvider _poseProvider;
private readonly MultiWheelChassis _chassis;
private readonly FirstOrderLowPassFilter _longitudinalSpeedFilter;
+ private readonly FirstOrderLowPassFilter _lateralSpeedFilter;
private bool _hasPreviousTimestamp;
private double _previousTimestampSeconds;
@@ -27,10 +28,12 @@ namespace MultiWheelC.StateEstimation
private bool _latestDetourVelocityValid;
private double _latestRawWheelBodyVxMetersPerSecond;
private double _latestFilteredWheelBodyVxMetersPerSecond;
+ private double _latestRawWheelBodyVyMetersPerSecond;
+ private double _latestFilteredWheelBodyVyMetersPerSecond;
private bool _latestWheelVelocityValid;
///
- /// 创建使用默认0.10s低通时间常数的电机反馈纵向速度状态源。
+ /// 创建使用默认0.10s低通时间常数的电机反馈平面速度状态源。
///
public WheelFeedbackVehicleStateProvider(
IVehicleStateProvider poseProvider,
@@ -43,7 +46,7 @@ namespace MultiWheelC.StateEstimation
}
///
- /// 创建使用指定低通时间常数的电机反馈纵向速度状态源。
+ /// 创建使用指定低通时间常数的电机反馈平面速度状态源。
///
public WheelFeedbackVehicleStateProvider(
IVehicleStateProvider poseProvider,
@@ -59,6 +62,9 @@ namespace MultiWheelC.StateEstimation
_longitudinalSpeedFilter =
new FirstOrderLowPassFilter(
velocityFilterTimeConstantSeconds);
+ _lateralSpeedFilter =
+ new FirstOrderLowPassFilter(
+ velocityFilterTimeConstantSeconds);
}
///
@@ -87,40 +93,50 @@ namespace MultiWheelC.StateEstimation
{
var actualCarSpeed =
_chassis.GetCarSpeed(true);
- var rawLongitudinalSpeedMetersPerSecond =
+ var rawBodyVxMetersPerSecond =
(double)actualCarSpeed.Vx;
+ var rawBodyVyMetersPerSecond =
+ (double)actualCarSpeed.Vy;
NumericGuard.EnsureFinite(
- rawLongitudinalSpeedMetersPerSecond,
+ rawBodyVxMetersPerSecond,
"电机反馈车体纵向速度");
+ NumericGuard.EnsureFinite(
+ rawBodyVyMetersPerSecond,
+ "电机反馈车体横向速度");
var wheelSpeedTimestampSeconds =
_wheelSpeedClock.Elapsed.TotalSeconds;
- var filteredLongitudinalSpeedMetersPerSecond =
- UpdateLongitudinalSpeedFilter(
- rawLongitudinalSpeedMetersPerSecond,
- wheelSpeedTimestampSeconds,
- out var hasValidWheelSpeedEstimate);
+ UpdateBodyVelocityFilters(
+ rawBodyVxMetersPerSecond,
+ rawBodyVyMetersPerSecond,
+ wheelSpeedTimestampSeconds,
+ out var filteredBodyVxMetersPerSecond,
+ out var filteredBodyVyMetersPerSecond,
+ out var hasValidWheelSpeedEstimate);
_latestDetourBodyVxMetersPerSecond =
poseState.TwistInBody.VxMetersPerSecond;
_latestDetourVelocityValid =
poseState.HasValidVelocityEstimate;
_latestRawWheelBodyVxMetersPerSecond =
- rawLongitudinalSpeedMetersPerSecond;
+ rawBodyVxMetersPerSecond;
_latestFilteredWheelBodyVxMetersPerSecond =
- filteredLongitudinalSpeedMetersPerSecond;
+ filteredBodyVxMetersPerSecond;
+ _latestRawWheelBodyVyMetersPerSecond =
+ rawBodyVyMetersPerSecond;
+ _latestFilteredWheelBodyVyMetersPerSecond =
+ filteredBodyVyMetersPerSecond;
_latestWheelVelocityValid =
hasValidWheelSpeedEstimate;
_hasVelocityDiagnostics = true;
- // 第一阶段只替换控制器使用的车体纵向速度；横向速度和角速度
- // 继续使用Detour估计，避免轮速差和舵角误差放大Vy与Omega噪声。
+ // 车体平面线速度来自四轮电机和舵角反馈；角速度继续使用Detour，
+ // 避免轮速差和舵角误差放大Omega噪声。
var twistInBody = new Twist2D(
- filteredLongitudinalSpeedMetersPerSecond,
- poseState.TwistInBody
- .VyMetersPerSecond,
+ filteredBodyVxMetersPerSecond,
+ filteredBodyVyMetersPerSecond,
poseState.TwistInBody
.OmegaRadiansPerSecond);
@@ -151,13 +167,15 @@ namespace MultiWheelC.StateEstimation
}
///
- /// 读取最近一帧Detour纵向速度和轮速解算纵向速度，供实验记录使用。
+ /// 读取最近一帧Detour纵向速度和轮速解算平面速度，供实验记录使用。
///
public bool TryGetLatestVelocityDiagnostics(
out double detourBodyVxMetersPerSecond,
out bool detourVelocityValid,
out double rawWheelBodyVxMetersPerSecond,
out double filteredWheelBodyVxMetersPerSecond,
+ out double rawWheelBodyVyMetersPerSecond,
+ out double filteredWheelBodyVyMetersPerSecond,
out bool wheelVelocityValid)
{
lock (_syncRoot)
@@ -170,6 +188,10 @@ namespace MultiWheelC.StateEstimation
_latestRawWheelBodyVxMetersPerSecond;
filteredWheelBodyVxMetersPerSecond =
_latestFilteredWheelBodyVxMetersPerSecond;
+ rawWheelBodyVyMetersPerSecond =
+ _latestRawWheelBodyVyMetersPerSecond;
+ filteredWheelBodyVyMetersPerSecond =
+ _latestFilteredWheelBodyVyMetersPerSecond;
wheelVelocityValid =
_latestWheelVelocityValid;
return _hasVelocityDiagnostics;
@@ -184,6 +206,7 @@ namespace MultiWheelC.StateEstimation
lock (_syncRoot)
{
_longitudinalSpeedFilter.Reset();
+ _lateralSpeedFilter.Reset();
_wheelSpeedClock.Restart();
_hasPreviousTimestamp = false;
_previousTimestampSeconds = 0.0;
@@ -192,17 +215,22 @@ namespace MultiWheelC.StateEstimation
_latestDetourVelocityValid = false;
_latestRawWheelBodyVxMetersPerSecond = 0.0;
_latestFilteredWheelBodyVxMetersPerSecond = 0.0;
+ _latestRawWheelBodyVyMetersPerSecond = 0.0;
+ _latestFilteredWheelBodyVyMetersPerSecond = 0.0;
_latestWheelVelocityValid = false;
LastFailureReason = string.Empty;
}
}
///
- /// 使用真实状态时间间隔更新纵向速度低通滤波，并在首帧建立基准。
+ /// 使用同一个真实采样间隔更新车体Vx和Vy低通滤波，并在首帧建立共同时间基准。
///
- private double UpdateLongitudinalSpeedFilter(
- double rawLongitudinalSpeedMetersPerSecond,
+ private void UpdateBodyVelocityFilters(
+ double rawBodyVxMetersPerSecond,
+ double rawBodyVyMetersPerSecond,
double timestampSeconds,
+ out double filteredBodyVxMetersPerSecond,
+ out double filteredBodyVyMetersPerSecond,
out bool hasValidWheelSpeedEstimate)
{
NumericGuard.EnsureFiniteNonNegative(
@@ -212,11 +240,17 @@ namespace MultiWheelC.StateEstimation
if (!_hasPreviousTimestamp)
{
_longitudinalSpeedFilter.Reset(
- rawLongitudinalSpeedMetersPerSecond);
+ rawBodyVxMetersPerSecond);
+ _lateralSpeedFilter.Reset(
+ rawBodyVyMetersPerSecond);
_previousTimestampSeconds = timestampSeconds;
_hasPreviousTimestamp = true;
hasValidWheelSpeedEstimate = false;
- return rawLongitudinalSpeedMetersPerSecond;
+ filteredBodyVxMetersPerSecond =
+ rawBodyVxMetersPerSecond;
+ filteredBodyVyMetersPerSecond =
+ rawBodyVyMetersPerSecond;
+ return;
}
var deltaTimeSeconds =
@@ -227,15 +261,26 @@ namespace MultiWheelC.StateEstimation
if (deltaTimeSeconds <= 0.0)
{
_longitudinalSpeedFilter.Reset(
- rawLongitudinalSpeedMetersPerSecond);
+ rawBodyVxMetersPerSecond);
+ _lateralSpeedFilter.Reset(
+ rawBodyVyMetersPerSecond);
hasValidWheelSpeedEstimate = false;
- return rawLongitudinalSpeedMetersPerSecond;
+ filteredBodyVxMetersPerSecond =
+ rawBodyVxMetersPerSecond;
+ filteredBodyVyMetersPerSecond =
+ rawBodyVyMetersPerSecond;
+ return;
}
hasValidWheelSpeedEstimate = true;
- return _longitudinalSpeedFilter.Update(
- rawLongitudinalSpeedMetersPerSecond,
- deltaTimeSeconds);
+ filteredBodyVxMetersPerSecond =
+ _longitudinalSpeedFilter.Update(
+ rawBodyVxMetersPerSecond,
+ deltaTimeSeconds);
+ filteredBodyVyMetersPerSecond =
+ _lateralSpeedFilter.Update(
+ rawBodyVyMetersPerSecond,
+ deltaTimeSeconds);
}
}
diff --git a/MultiWheelC/build/Clumsy/CommonUsage.dll b/MultiWheelC/build/Clumsy/CommonUsage.dll
index 71ef99f..9ef0dbd 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 ed0cd34..ac634e6 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 8412bc4..0569912 100644
Binary files a/MultiWheelC/build/Clumsy/MultiWheelC.pdb and b/MultiWheelC/build/Clumsy/MultiWheelC.pdb differ
diff --git a/Shared/Models/MotionModels.cs b/Shared/Models/MotionModels.cs
new file mode 100644
index 0000000..f36c14d
--- /dev/null
+++ b/Shared/Models/MotionModels.cs
@@ -0,0 +1,154 @@
+// 纯数据层：只描述坐标、速度和命令
+// 定义二维坐标、位姿、速度、车队布局和单车底盘命令。
+// 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 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/data_process/plot_new_controller_experiment.py b/data_process/plot_new_controller_experiment.py
index 8af14eb..4211d71 100644
--- a/data_process/plot_new_controller_experiment.py
+++ b/data_process/plot_new_controller_experiment.py
@@ -293,11 +293,22 @@ def load_experiment(csv_path: Path) -> dict[str, object]:
)
if np.any(has_control_reference):
reference_speed[~has_control_reference] = np.nan
+ motion_frame_yaw_radians = np.deg2rad(
+ numeric_column(frame, "ReferenceMotionFrameYawDegrees", 0.0)
+ )
+ motion_direction_cosine = np.cos(motion_frame_yaw_radians)
+ motion_direction_sine = np.sin(motion_frame_yaw_radians)
state_body_vx = numeric_column(frame, "StateBodyVxMetersPerSecond")
+ state_body_vy = numeric_column(frame, "StateBodyVyMetersPerSecond")
velocity_valid = (
numeric_column(frame, "StateVelocityEstimateValid", 0.0) > 0.5
)
state_body_vx[~velocity_valid] = np.nan
+ state_body_vy[~velocity_valid] = np.nan
+ state_motion_speed = (
+ state_body_vx * motion_direction_cosine
+ + state_body_vy * motion_direction_sine
+ )
has_velocity_diagnostics = (
numeric_column(frame, "HasVelocityDiagnostics", 0.0) > 0.5
)
@@ -317,14 +328,22 @@ def load_experiment(csv_path: Path) -> dict[str, object]:
)
)
detour_speed[~detour_speed_valid] = np.nan
- wheel_raw_speed = numeric_column(
+ wheel_raw_body_vx = numeric_column(
frame,
"WheelFeedbackRawBodyVxMetersPerSecond",
)
- wheel_filtered_speed = numeric_column(
+ wheel_filtered_body_vx = numeric_column(
frame,
"WheelFeedbackFilteredBodyVxMetersPerSecond",
)
+ wheel_raw_body_vy = numeric_column(
+ frame,
+ "WheelFeedbackRawBodyVyMetersPerSecond",
+ )
+ wheel_filtered_body_vy = numeric_column(
+ frame,
+ "WheelFeedbackFilteredBodyVyMetersPerSecond",
+ )
wheel_speed_valid = (
has_velocity_diagnostics
& (
@@ -336,12 +355,33 @@ def load_experiment(csv_path: Path) -> dict[str, object]:
> 0.5
)
)
+ wheel_raw_speed = (
+ wheel_raw_body_vx * motion_direction_cosine
+ + wheel_raw_body_vy * motion_direction_sine
+ )
+ wheel_filtered_speed = (
+ wheel_filtered_body_vx * motion_direction_cosine
+ + wheel_filtered_body_vy * motion_direction_sine
+ )
+
+ # 兼容尚未记录轮速Vy的旧版β=0实验；非零β缺少Vy时不能伪造投影速度。
+ body_x_motion = np.abs(motion_direction_sine) <= 1e-12
+ missing_raw_projection = ~np.isfinite(wheel_raw_speed)
+ missing_filtered_projection = ~np.isfinite(wheel_filtered_speed)
+ wheel_raw_speed[body_x_motion & missing_raw_projection] = (
+ wheel_raw_body_vx[body_x_motion & missing_raw_projection]
+ )
+ wheel_filtered_speed[
+ body_x_motion & missing_filtered_projection
+ ] = wheel_filtered_body_vx[
+ body_x_motion & missing_filtered_projection
+ ]
wheel_raw_speed[~wheel_speed_valid] = np.nan
wheel_filtered_speed[~wheel_speed_valid] = np.nan
actual_speed = np.where(
np.isfinite(wheel_filtered_speed),
wheel_filtered_speed,
- state_body_vx,
+ state_motion_speed,
)
command_speed = numeric_column(frame, "CommandSpeed")
@@ -504,7 +544,7 @@ def plot_experiment(
)
axis.grid(True, alpha=0.3)
- # 4. 参考、命令、Detour估计和轮速解算速度。
+ # 4. 参考、命令、Detour车头分量和沿β投影的轮速解算速度。
speed_error = data["actual_speed"] - data["reference_speed"]
speed_rmse = finite_rmse(speed_error)
axis = axes[1, 1]
@@ -527,7 +567,7 @@ def plot_experiment(
data["detour_speed"],
":",
linewidth=1.2,
- label="Detour估计Vx",
+ label="Detour估计Vx（车头分量）",
)
if np.any(np.isfinite(data["wheel_filtered_speed"])):
wheel_filtered_valid = np.isfinite(
@@ -540,7 +580,7 @@ def plot_experiment(
s=14,
marker="o",
zorder=5,
- label="轮速解算滤波Vx（控制使用）",
+ label="轮速解算β方向速度（控制使用）",
)
else:
axis.plot(
@@ -553,7 +593,7 @@ def plot_experiment(
axis.set_ylabel("速度 / (m/s)")
axis.set_title(
"参考速度、控制命令与观测速度\n"
- f"轮速Vx相对参考速度RMSE={speed_rmse:.4f}m/s"
+ f"轮速β方向速度相对参考速度RMSE={speed_rmse:.4f}m/s"
)
axis.grid(True, alpha=0.3)
axis.legend(fontsize=8)
diff --git a/output/C/CommonUsage.dll b/output/C/CommonUsage.dll
index 71ef99f..9ef0dbd 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 ed0cd34..ac634e6 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 8412bc4..0569912 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 71ef99f..9ef0dbd 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 0602883..54db927 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 517fda7..f777dfe 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 71ef99f..9ef0dbd 100644
Binary files a/ref/CommonUsage.dll and b/ref/CommonUsage.dll differ