diff --git a/MedullaAdapter/build/Medulla/plugins/CommonUsage.dll b/MedullaAdapter/build/Medulla/plugins/CommonUsage.dll
index 93900a2..82a8d1d 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 1713d12..b225c77 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 3ff5098..f755db3 100644
Binary files a/MedullaAdapter/build/Medulla/plugins/MedullaAdapter.pdb and b/MedullaAdapter/build/Medulla/plugins/MedullaAdapter.pdb differ
diff --git a/MultiWheelC/Control/Execution/ParkingGeometricController.cs b/MultiWheelC/Control/Execution/ParkingGeometricController.cs
index ba123b3..b17c559 100644
--- a/MultiWheelC/Control/Execution/ParkingGeometricController.cs
+++ b/MultiWheelC/Control/Execution/ParkingGeometricController.cs
@@ -47,11 +47,11 @@ namespace MultiWheelC.Control.Execution
ILongitudinalController longitudinalController,
GcpCommandAllocator gcpAllocator,
GcpCommandExecutor commandExecutor,
- double finishDistanceMeters = 0.03,
+ double finishDistanceMeters = 0.04,
double finishSpeedMetersPerSecond = 0.02,
double finishHeadingToleranceRadians =
3.0 * Math.PI / 180.0,
- double maximumDistanceToTrajectoryMeters = 0.50)
+ double maximumDistanceToTrajectoryMeters = 0.30)
{
_stateProvider = stateProvider ??
throw new ArgumentNullException(
diff --git a/MultiWheelC/Control/Lateral/StanleyLateralController.cs b/MultiWheelC/Control/Lateral/StanleyLateralController.cs
index 26c64e1..9c281a7 100644
--- a/MultiWheelC/Control/Lateral/StanleyLateralController.cs
+++ b/MultiWheelC/Control/Lateral/StanleyLateralController.cs
@@ -73,7 +73,7 @@ namespace MultiWheelC.Control.Lateral
public double MinimumSpeedMetersPerSecond { get; }
///
- /// 获取是否优先使用Detour估算的实际纵向速度计算横向修正。
+ /// 获取是否优先使用当前状态源提供的实际纵向速度计算横向修正。
///
public bool UseActualSpeedForGain { get; }
diff --git a/MultiWheelC/Experiments/CompositeMotionPlanTests.cs b/MultiWheelC/Experiments/CompositeMotionPlanTests.cs
index d022c73..715b5e8 100644
--- a/MultiWheelC/Experiments/CompositeMotionPlanTests.cs
+++ b/MultiWheelC/Experiments/CompositeMotionPlanTests.cs
@@ -16,7 +16,7 @@ namespace MultiWheelC
///
/// 测试平滑左转轨迹、停车原地左转90°和再次直行的组合运动执行过程。
///
- [MovementTest(name = "新版控制器：曲线-停车自转-直线组合测试")]
+ [MovementTest(name = "新版控制器：直线-圆弧-折线组合测试")]
public sealed class CompositeStopTurnGoTest : MovementTest
{
private const float MillimetersPerMeter = 1000f;
@@ -52,6 +52,13 @@ namespace MultiWheelC
return;
}
+ if (!TrajectoryExperimentInput
+ .TryReadLateralOffsetMeters(
+ out var lateralOffsetMeters))
+ {
+ return;
+ }
+
if (!MovementTestPreparation.AreWheelsForward())
{
return;
@@ -65,19 +72,30 @@ namespace MultiWheelC
return;
}
- var stateProvider = new DetourVehicleStateProvider();
- if (!stateProvider.TryGetState(out var initialState))
+ var detourStateProvider =
+ new DetourVehicleStateProvider();
+ if (!detourStateProvider.TryGetState(
+ out var initialState))
{
Console.WriteLine(
"无法读取组合运动起点位姿：" +
- stateProvider.LastFailureReason);
+ detourStateProvider.LastFailureReason);
return;
}
+ var stateProvider =
+ new WheelFeedbackVehicleStateProvider(
+ detourStateProvider,
+ chassis);
+
+ var planStartPose =
+ TrajectoryExperimentInput.OffsetPoseLaterally(
+ initialState.PoseInWorld,
+ lateralOffsetMeters);
var firstTrajectory =
TestTrajectoryFactory
.CreateStraightSmoothLeftTurnStraight(
- initialState.PoseInWorld,
+ planStartPose,
StraightLengthMeters,
TurnRadiusMeters,
AngleMath.DegreesToRadians(
@@ -133,7 +151,9 @@ namespace MultiWheelC
_recorder = new TrackingExperimentRecorder(
controllerName: "NewStanleyPidComposite",
trajectoryName:
- "SmoothTurnStopRotateStraight",
+ TrajectoryExperimentInput.BuildTrajectoryName(
+ "SmoothTurnStopRotateStraight",
+ lateralOffsetMeters),
trialNumber: TrialNumber,
referenceStart: ToMillimeterVector(
firstTrajectory.StartPoint.PoseInWorld),
@@ -145,7 +165,8 @@ namespace MultiWheelC
referenceAccelerationMetersPerSecondSquared:
(float)AccelerationMetersPerSecondSquared,
referenceDecelerationMetersPerSecondSquared:
- (float)DecelerationMetersPerSecondSquared);
+ (float)DecelerationMetersPerSecondSquared,
+ diagnosticChassis: chassis);
_recorder.Start();
var controlPointRadiusMeters =
@@ -157,6 +178,7 @@ namespace MultiWheelC
StateProvider = stateProvider,
ConfigureTrackingMovement = tracking =>
{
+ tracking.StanleyUsesActualSpeed = true;
tracking.MaximumCommandSpeedMetersPerSecond =
StraightMaximumSpeedMetersPerSecond;
tracking
@@ -166,6 +188,7 @@ namespace MultiWheelC
SegmentStarted = (index, segment) =>
{
_recorder?.ClearControlReference();
+ _recorder?.ClearGcpCommand();
_recorder?.UpdateCommand(0f, 0f);
Console.WriteLine(
$"组合运动开始第{index + 1}段：" +
@@ -174,7 +197,8 @@ namespace MultiWheelC
TrackingCycleObserver = (index, controller) =>
RecordTrackingCycle(
controller,
- controlPointRadiusMeters),
+ controlPointRadiusMeters,
+ stateProvider),
RotationCommandObserver = (index, omega) =>
_recorder?.UpdateCommand(
0f,
@@ -276,7 +300,8 @@ namespace MultiWheelC
///
private void RecordTrackingCycle(
ParkingGeometricController controller,
- double controlPointRadiusMeters)
+ double controlPointRadiusMeters,
+ WheelFeedbackVehicleStateProvider stateProvider)
{
if (controller.LastVehicleState.HasValue)
{
@@ -284,6 +309,21 @@ namespace MultiWheelC
controller.LastVehicleState.Value);
}
+ if (stateProvider.TryGetLatestVelocityDiagnostics(
+ out var detourBodyVx,
+ out var detourVelocityValid,
+ out var rawWheelBodyVx,
+ out var filteredWheelBodyVx,
+ out var wheelVelocityValid))
+ {
+ _recorder?.UpdateVelocityDiagnostics(
+ detourBodyVx,
+ detourVelocityValid,
+ rawWheelBodyVx,
+ filteredWheelBodyVx,
+ wheelVelocityValid);
+ }
+
if (!controller.LastCommand.HasValue)
{
return;
@@ -305,6 +345,9 @@ namespace MultiWheelC
}
var command = controller.LastCommand.Value;
+ _recorder?.UpdateGcpCommand(
+ command.FrontAngleRadians,
+ command.RearAngleRadians);
var curvaturePerMeter = Math.Tan(
command.FrontAngleRadians) /
controlPointRadiusMeters;
diff --git a/MultiWheelC/Experiments/NewControllerTrackingTests.cs b/MultiWheelC/Experiments/NewControllerTrackingTests.cs
index d9269d3..ceab046 100644
--- a/MultiWheelC/Experiments/NewControllerTrackingTests.cs
+++ b/MultiWheelC/Experiments/NewControllerTrackingTests.cs
@@ -1,5 +1,6 @@
using System;
using System.Drawing;
+using System.Globalization;
using System.Numerics;
using System.Threading;
using ClumsyCore;
@@ -13,6 +14,82 @@ using MyParking.Shared;
namespace MultiWheelC
{
+ ///
+ /// 统一读取轨迹实验的有符号横向偏移，并将车体局部偏移转换到世界坐标系。
+ ///
+ internal static class TrajectoryExperimentInput
+ {
+ private const double MaximumOffsetCentimeters = 30.0;
+
+ ///
+ /// 从Clumsy输入框读取车体左正右负的横向偏移，单位转换为m。
+ ///
+ public static bool TryReadLateralOffsetMeters(
+ out double lateralOffsetMeters)
+ {
+ lateralOffsetMeters = 0.0;
+ var input = UI.GetInput(
+ "输入轨迹横向偏移（cm，左正右负，范围-30～30）：");
+ var parsed = double.TryParse(
+ input,
+ NumberStyles.Float,
+ CultureInfo.CurrentCulture,
+ out var offsetCentimeters) ||
+ double.TryParse(
+ input,
+ NumberStyles.Float,
+ CultureInfo.InvariantCulture,
+ out offsetCentimeters);
+
+ if (!parsed ||
+ double.IsNaN(offsetCentimeters) ||
+ double.IsInfinity(offsetCentimeters) ||
+ Math.Abs(offsetCentimeters) >
+ MaximumOffsetCentimeters)
+ {
+ Console.WriteLine(
+ "轨迹横向偏移必须是-30～30cm之间的有限数值，测试未启动。");
+ return false;
+ }
+
+ lateralOffsetMeters =
+ offsetCentimeters / 100.0;
+ return true;
+ }
+
+ ///
+ /// 沿初始车体左方向平移参考轨迹起点，同时保持世界坐标航向不变。
+ ///
+ public static Pose2D OffsetPoseLaterally(
+ Pose2D poseInWorld,
+ double lateralOffsetMeters)
+ {
+ var yawRadians = poseInWorld.YawRadians;
+ return new Pose2D(
+ poseInWorld.XMeters -
+ Math.Sin(yawRadians) *
+ lateralOffsetMeters,
+ poseInWorld.YMeters +
+ Math.Cos(yawRadians) *
+ lateralOffsetMeters,
+ yawRadians);
+ }
+
+ ///
+ /// 生成带毫米偏移标识的实验轨迹名称。
+ ///
+ public static string BuildTrajectoryName(
+ string baseName,
+ double lateralOffsetMeters)
+ {
+ return baseName +
+ "_Offset" +
+ (lateralOffsetMeters * 1000.0)
+ .ToString("+0;-0;0", CultureInfo.InvariantCulture) +
+ "mm";
+ }
+ }
+
///
/// 从当前Detour位姿开始执行新版控制器4m直线跟踪并保存实验数据。
///
@@ -27,7 +104,7 @@ namespace MultiWheelC
private DriveTask _task;
private TrackingExperimentRecorder _recorder;
- private DetourVehicleStateProvider _stateProvider;
+ private IVehicleStateProvider _stateProvider;
///
/// 获取或设置本次测试编号，用于区分重复实验CSV。
@@ -37,7 +114,7 @@ namespace MultiWheelC
///
/// 获取或设置4m直线的巡航参考速度，单位为m/s。
///
- public double CruiseSpeedMetersPerSecond = 0.30;
+ public double CruiseSpeedMetersPerSecond = 0.40;
///
/// 获取或设置参考速度加速度，单位为m/s²。
@@ -66,6 +143,13 @@ namespace MultiWheelC
return;
}
+ if (!TrajectoryExperimentInput
+ .TryReadLateralOffsetMeters(
+ out var lateralOffsetMeters))
+ {
+ return;
+ }
+
if (!MovementTestPreparation.AreWheelsForward())
{
return;
@@ -80,21 +164,30 @@ namespace MultiWheelC
return;
}
- _stateProvider =
+ var detourStateProvider =
new DetourVehicleStateProvider();
- if (!_stateProvider.TryGetState(
+ if (!detourStateProvider.TryGetState(
out var initialState))
{
Console.WriteLine(
"无法读取有效Detour起点位姿：" +
- _stateProvider.LastFailureReason);
+ detourStateProvider.LastFailureReason);
_stateProvider = null;
return;
}
+ _stateProvider =
+ new WheelFeedbackVehicleStateProvider(
+ detourStateProvider,
+ chassis);
+
+ var trajectoryStartPose =
+ TrajectoryExperimentInput.OffsetPoseLaterally(
+ initialState.PoseInWorld,
+ lateralOffsetMeters);
var trajectory =
TestTrajectoryFactory.CreateStraight4Meters(
- initialState.PoseInWorld,
+ trajectoryStartPose,
CruiseSpeedMetersPerSecond,
AccelerationMetersPerSecondSquared,
DecelerationMetersPerSecondSquared,
@@ -109,7 +202,10 @@ namespace MultiWheelC
var recorder =
new TrackingExperimentRecorder(
controllerName: "NewStanleyPid",
- trajectoryName: "ProfiledStraight4m",
+ trajectoryName:
+ TrajectoryExperimentInput.BuildTrajectoryName(
+ "ProfiledStraight4m",
+ lateralOffsetMeters),
trialNumber: TrialNumber,
referenceStart: referenceStart,
referenceEnd: referenceEnd,
@@ -119,7 +215,8 @@ namespace MultiWheelC
referenceAccelerationMetersPerSecondSquared:
(float)AccelerationMetersPerSecondSquared,
referenceDecelerationMetersPerSecondSquared:
- (float)DecelerationMetersPerSecondSquared);
+ (float)DecelerationMetersPerSecondSquared,
+ diagnosticChassis: chassis);
_recorder = recorder;
var controlPointRadiusMeters =
@@ -130,12 +227,15 @@ namespace MultiWheelC
{
Trajectory = trajectory,
StateProvider = _stateProvider,
+ StanleyUsesActualSpeed = true,
MaximumCommandSpeedMetersPerSecond = 0.50,
CycleObserver = controller =>
RecordControlCycle(
recorder,
controller,
- controlPointRadiusMeters)
+ controlPointRadiusMeters,
+ _stateProvider as
+ WheelFeedbackVehicleStateProvider)
};
recorder.Start();
@@ -239,7 +339,8 @@ namespace MultiWheelC
private static void RecordControlCycle(
TrackingExperimentRecorder recorder,
ParkingGeometricController controller,
- double controlPointRadiusMeters)
+ double controlPointRadiusMeters,
+ WheelFeedbackVehicleStateProvider stateProvider)
{
if (controller.LastVehicleState.HasValue)
{
@@ -247,6 +348,10 @@ namespace MultiWheelC
controller.LastVehicleState.Value);
}
+ UpdateVelocityDiagnostics(
+ recorder,
+ stateProvider);
+
if (!controller.LastCommand.HasValue)
{
return;
@@ -267,6 +372,9 @@ namespace MultiWheelC
}
var command = controller.LastCommand.Value;
+ recorder.UpdateGcpCommand(
+ command.FrontAngleRadians,
+ command.RearAngleRadians);
var curvaturePerMeter = Math.Tan(
command.FrontAngleRadians) /
controlPointRadiusMeters;
@@ -279,6 +387,32 @@ namespace MultiWheelC
(float)angularSpeedRadiansPerSecond);
}
+ ///
+ /// 将同一周期的Detour速度和轮速解算速度写入实验记录器。
+ ///
+ private static void UpdateVelocityDiagnostics(
+ TrackingExperimentRecorder recorder,
+ WheelFeedbackVehicleStateProvider stateProvider)
+ {
+ if (stateProvider == null ||
+ !stateProvider.TryGetLatestVelocityDiagnostics(
+ out var detourBodyVx,
+ out var detourVelocityValid,
+ out var rawWheelBodyVx,
+ out var filteredWheelBodyVx,
+ out var wheelVelocityValid))
+ {
+ return;
+ }
+
+ recorder.UpdateVelocityDiagnostics(
+ detourBodyVx,
+ detourVelocityValid,
+ rawWheelBodyVx,
+ filteredWheelBodyVx,
+ wheelVelocityValid);
+ }
+
///
/// 将Shared世界坐标系米制位姿转换为Clumsy绘图和旧记录器使用的毫米坐标。
///
@@ -293,6 +427,7 @@ namespace MultiWheelC
poseInWorld.YMeters *
MillimetersPerMeter));
}
+
}
///
@@ -310,7 +445,7 @@ namespace MultiWheelC
private DriveTask _task;
private TrackingExperimentRecorder _recorder;
- private DetourVehicleStateProvider _stateProvider;
+ private IVehicleStateProvider _stateProvider;
///
/// 获取或设置本次测试编号，用于区分重复实验CSV。
@@ -369,6 +504,13 @@ namespace MultiWheelC
return;
}
+ if (!TrajectoryExperimentInput
+ .TryReadLateralOffsetMeters(
+ out var lateralOffsetMeters))
+ {
+ return;
+ }
+
if (!MovementTestPreparation.AreWheelsForward())
{
return;
@@ -383,22 +525,31 @@ namespace MultiWheelC
return;
}
- _stateProvider =
+ var detourStateProvider =
new DetourVehicleStateProvider();
- if (!_stateProvider.TryGetState(
+ if (!detourStateProvider.TryGetState(
out var initialState))
{
Console.WriteLine(
"无法读取有效Detour起点位姿：" +
- _stateProvider.LastFailureReason);
+ detourStateProvider.LastFailureReason);
_stateProvider = null;
return;
}
+ _stateProvider =
+ new WheelFeedbackVehicleStateProvider(
+ detourStateProvider,
+ chassis);
+
+ var trajectoryStartPose =
+ TrajectoryExperimentInput.OffsetPoseLaterally(
+ initialState.PoseInWorld,
+ lateralOffsetMeters);
var trajectory =
TestTrajectoryFactory
.CreateStraightLeftSemicircleStraight(
- initialState.PoseInWorld,
+ trajectoryStartPose,
StraightLengthMeters,
TurnRadiusMeters,
CurvatureTransitionLengthMeters,
@@ -418,7 +569,9 @@ namespace MultiWheelC
new TrackingExperimentRecorder(
controllerName: "NewStanleyPid",
trajectoryName:
- "ProfiledStraightSmoothLeftTurnStraight",
+ TrajectoryExperimentInput.BuildTrajectoryName(
+ "ProfiledStraightSmoothLeftTurnStraight",
+ lateralOffsetMeters),
trialNumber: TrialNumber,
referenceStart: referenceStart,
referenceEnd: referenceEnd,
@@ -428,7 +581,8 @@ namespace MultiWheelC
referenceAccelerationMetersPerSecondSquared:
(float)AccelerationMetersPerSecondSquared,
referenceDecelerationMetersPerSecondSquared:
- (float)DecelerationMetersPerSecondSquared);
+ (float)DecelerationMetersPerSecondSquared,
+ diagnosticChassis: chassis);
_recorder = recorder;
var controlPointRadiusMeters =
@@ -439,13 +593,16 @@ namespace MultiWheelC
{
Trajectory = trajectory,
StateProvider = _stateProvider,
+ StanleyUsesActualSpeed = true,
MaximumCommandSpeedMetersPerSecond =
StraightMaximumSpeedMetersPerSecond,
CycleObserver = controller =>
RecordControlCycle(
recorder,
controller,
- controlPointRadiusMeters)
+ controlPointRadiusMeters,
+ _stateProvider as
+ WheelFeedbackVehicleStateProvider)
};
recorder.Start();
@@ -549,7 +706,8 @@ namespace MultiWheelC
private static void RecordControlCycle(
TrackingExperimentRecorder recorder,
ParkingGeometricController controller,
- double controlPointRadiusMeters)
+ double controlPointRadiusMeters,
+ WheelFeedbackVehicleStateProvider stateProvider)
{
if (controller.LastVehicleState.HasValue)
{
@@ -557,6 +715,10 @@ namespace MultiWheelC
controller.LastVehicleState.Value);
}
+ UpdateVelocityDiagnostics(
+ recorder,
+ stateProvider);
+
if (!controller.LastCommand.HasValue)
{
return;
@@ -577,6 +739,9 @@ namespace MultiWheelC
}
var command = controller.LastCommand.Value;
+ recorder.UpdateGcpCommand(
+ command.FrontAngleRadians,
+ command.RearAngleRadians);
var curvaturePerMeter = Math.Tan(
command.FrontAngleRadians) /
controlPointRadiusMeters;
@@ -589,6 +754,32 @@ namespace MultiWheelC
(float)angularSpeedRadiansPerSecond);
}
+ ///
+ /// 将同一周期的Detour速度和轮速解算速度写入实验记录器。
+ ///
+ private static void UpdateVelocityDiagnostics(
+ TrackingExperimentRecorder recorder,
+ WheelFeedbackVehicleStateProvider stateProvider)
+ {
+ if (stateProvider == null ||
+ !stateProvider.TryGetLatestVelocityDiagnostics(
+ out var detourBodyVx,
+ out var detourVelocityValid,
+ out var rawWheelBodyVx,
+ out var filteredWheelBodyVx,
+ out var wheelVelocityValid))
+ {
+ return;
+ }
+
+ recorder.UpdateVelocityDiagnostics(
+ detourBodyVx,
+ detourVelocityValid,
+ rawWheelBodyVx,
+ filteredWheelBodyVx,
+ wheelVelocityValid);
+ }
+
///
/// 将Shared世界坐标系米制位姿转换为Clumsy绘图和记录器使用的毫米坐标。
///
diff --git a/MultiWheelC/Experiments/TrackingExperimentRecorder.cs b/MultiWheelC/Experiments/TrackingExperimentRecorder.cs
index a09ef46..1bbcc21 100644
--- a/MultiWheelC/Experiments/TrackingExperimentRecorder.cs
+++ b/MultiWheelC/Experiments/TrackingExperimentRecorder.cs
@@ -7,6 +7,7 @@ using System.IO;
using System.Numerics;
using System.Text;
using System.Threading;
+using CommonUsage.Chassis;
using MyParking.Shared;
using MultiWheelC.StateEstimation;
@@ -47,6 +48,28 @@ namespace MultiWheelC
public double ControlHeadingErrorRadians;
public double ControlDistanceToTrajectoryMeters;
public double ControlRemainingDistanceMeters;
+
+ // 并列保存Detour速度与轮速解算速度，避免StateBodyVx的数据来源产生歧义。
+ public bool HasVelocityDiagnostics;
+ public double DetourEstimatedBodyVxMetersPerSecond;
+ public bool DetourVelocityEstimateValid;
+ public double WheelFeedbackRawBodyVxMetersPerSecond;
+ public double WheelFeedbackFilteredBodyVxMetersPerSecond;
+ public bool WheelFeedbackVelocityEstimateValid;
+
+ // 四舵轮机械角使用deg，前后虚拟GCP命令角使用rad。
+ public bool HasSteeringDiagnostics;
+ public double TargetSteerLeftFrontDegrees;
+ public double TargetSteerLeftRearDegrees;
+ public double TargetSteerRightFrontDegrees;
+ public double TargetSteerRightRearDegrees;
+ public double ActualSteerLeftFrontDegrees;
+ public double ActualSteerLeftRearDegrees;
+ public double ActualSteerRightFrontDegrees;
+ public double ActualSteerRightRearDegrees;
+ public bool HasGcpCommand;
+ public double CommandFrontGcpAngleRadians;
+ public double CommandRearGcpAngleRadians;
}
// C层实验工具：统一采集并保存轨迹跟踪实验数据。
@@ -63,6 +86,7 @@ namespace MultiWheelC
private readonly float _referenceAccelerationMetersPerSecondSquared;
private readonly float _referenceDecelerationMetersPerSecondSquared;
private readonly int _sampleIntervalMs;
+ private readonly MultiWheelChassis _diagnosticChassis;
private readonly List _samples =
new List();
@@ -97,6 +121,15 @@ namespace MultiWheelC
private double _controlHeadingErrorRadians;
private double _controlDistanceToTrajectoryMeters;
private double _controlRemainingDistanceMeters;
+ private bool _hasVelocityDiagnostics;
+ private double _detourEstimatedBodyVxMetersPerSecond;
+ private bool _detourVelocityEstimateValid;
+ private double _wheelFeedbackRawBodyVxMetersPerSecond;
+ private double _wheelFeedbackFilteredBodyVxMetersPerSecond;
+ private bool _wheelFeedbackVelocityEstimateValid;
+ private bool _hasGcpCommand;
+ private double _commandFrontGcpAngleRadians;
+ private double _commandRearGcpAngleRadians;
public TrackingExperimentRecorder(
string controllerName,
@@ -109,7 +142,8 @@ namespace MultiWheelC
int sampleIntervalMs = 50,
float referenceMotionFrameYawDegrees = 0f,
float referenceAccelerationMetersPerSecondSquared = 0f,
- float referenceDecelerationMetersPerSecondSquared = 0f)
+ float referenceDecelerationMetersPerSecondSquared = 0f,
+ MultiWheelChassis diagnosticChassis = null)
{
if (string.IsNullOrWhiteSpace(controllerName))
throw new ArgumentException(
@@ -140,6 +174,7 @@ namespace MultiWheelC
_referenceDecelerationMetersPerSecondSquared =
referenceDecelerationMetersPerSecondSquared;
_sampleIntervalMs = sampleIntervalMs;
+ _diagnosticChassis = diagnosticChassis;
}
// 保存成功后的CSV绝对路径；尚未保存时为空。
@@ -221,6 +256,60 @@ namespace MultiWheelC
}
}
+ ///
+ /// 保存同一控制周期的Detour纵向速度以及轮速解算的原始和滤波纵向速度。
+ ///
+ public void UpdateVelocityDiagnostics(
+ double detourEstimatedBodyVxMetersPerSecond,
+ bool detourVelocityEstimateValid,
+ double wheelFeedbackRawBodyVxMetersPerSecond,
+ double wheelFeedbackFilteredBodyVxMetersPerSecond,
+ bool wheelFeedbackVelocityEstimateValid)
+ {
+ lock (_stateSyncRoot)
+ {
+ _detourEstimatedBodyVxMetersPerSecond =
+ detourEstimatedBodyVxMetersPerSecond;
+ _detourVelocityEstimateValid =
+ detourVelocityEstimateValid;
+ _wheelFeedbackRawBodyVxMetersPerSecond =
+ wheelFeedbackRawBodyVxMetersPerSecond;
+ _wheelFeedbackFilteredBodyVxMetersPerSecond =
+ wheelFeedbackFilteredBodyVxMetersPerSecond;
+ _wheelFeedbackVelocityEstimateValid =
+ wheelFeedbackVelocityEstimateValid;
+ _hasVelocityDiagnostics = true;
+ }
+ }
+
+ ///
+ /// 保存经过GCP角速度限制后实际交给底盘的前后虚拟控制点转角。
+ ///
+ public void UpdateGcpCommand(
+ double frontGcpAngleRadians,
+ double rearGcpAngleRadians)
+ {
+ lock (_stateSyncRoot)
+ {
+ _commandFrontGcpAngleRadians =
+ frontGcpAngleRadians;
+ _commandRearGcpAngleRadians =
+ rearGcpAngleRadians;
+ _hasGcpCommand = true;
+ }
+ }
+
+ ///
+ /// 清除上一轨迹段的GCP命令，避免停车或原地自转阶段沿用旧角度。
+ ///
+ public void ClearGcpCommand()
+ {
+ lock (_stateSyncRoot)
+ {
+ _hasGcpCommand = false;
+ }
+ }
+
///
/// 保存新版控制器本周期实际使用的轨迹投影、误差和参考速度。
///
@@ -331,6 +420,15 @@ namespace MultiWheelC
double controlHeadingErrorRadians;
double controlDistanceToTrajectoryMeters;
double controlRemainingDistanceMeters;
+ bool hasVelocityDiagnostics;
+ double detourEstimatedBodyVxMetersPerSecond;
+ bool detourVelocityEstimateValid;
+ double wheelFeedbackRawBodyVxMetersPerSecond;
+ double wheelFeedbackFilteredBodyVxMetersPerSecond;
+ bool wheelFeedbackVelocityEstimateValid;
+ bool hasGcpCommand;
+ double commandFrontGcpAngleRadians;
+ double commandRearGcpAngleRadians;
lock (_commandSyncRoot)
{
@@ -382,6 +480,23 @@ namespace MultiWheelC
_controlDistanceToTrajectoryMeters;
controlRemainingDistanceMeters =
_controlRemainingDistanceMeters;
+ hasVelocityDiagnostics =
+ _hasVelocityDiagnostics;
+ detourEstimatedBodyVxMetersPerSecond =
+ _detourEstimatedBodyVxMetersPerSecond;
+ detourVelocityEstimateValid =
+ _detourVelocityEstimateValid;
+ wheelFeedbackRawBodyVxMetersPerSecond =
+ _wheelFeedbackRawBodyVxMetersPerSecond;
+ wheelFeedbackFilteredBodyVxMetersPerSecond =
+ _wheelFeedbackFilteredBodyVxMetersPerSecond;
+ wheelFeedbackVelocityEstimateValid =
+ _wheelFeedbackVelocityEstimateValid;
+ hasGcpCommand = _hasGcpCommand;
+ commandFrontGcpAngleRadians =
+ _commandFrontGcpAngleRadians;
+ commandRearGcpAngleRadians =
+ _commandRearGcpAngleRadians;
}
var sample = new TrackingSample
@@ -411,9 +526,28 @@ namespace MultiWheelC
ControlDistanceToTrajectoryMeters =
controlDistanceToTrajectoryMeters,
ControlRemainingDistanceMeters =
- controlRemainingDistanceMeters
+ controlRemainingDistanceMeters,
+ HasVelocityDiagnostics =
+ hasVelocityDiagnostics,
+ DetourEstimatedBodyVxMetersPerSecond =
+ detourEstimatedBodyVxMetersPerSecond,
+ DetourVelocityEstimateValid =
+ detourVelocityEstimateValid,
+ WheelFeedbackRawBodyVxMetersPerSecond =
+ wheelFeedbackRawBodyVxMetersPerSecond,
+ WheelFeedbackFilteredBodyVxMetersPerSecond =
+ wheelFeedbackFilteredBodyVxMetersPerSecond,
+ WheelFeedbackVelocityEstimateValid =
+ wheelFeedbackVelocityEstimateValid,
+ HasGcpCommand = hasGcpCommand,
+ CommandFrontGcpAngleRadians =
+ commandFrontGcpAngleRadians,
+ CommandRearGcpAngleRadians =
+ commandRearGcpAngleRadians
};
+ CaptureSteeringDiagnostics(sample);
+
if (processedState.HasValue)
{
var state = processedState.Value;
@@ -452,6 +586,90 @@ namespace MultiWheelC
}
}
+ ///
+ /// 按舵轮物理安装位置记录四轮目标角和实际反馈角。
+ ///
+ private void CaptureSteeringDiagnostics(
+ TrackingSample sample)
+ {
+ if (_diagnosticChassis == null)
+ {
+ return;
+ }
+
+#pragma warning disable CS0612, CS0618
+ var wheels = _diagnosticChassis.GetSteerWheels();
+#pragma warning restore CS0612, CS0618
+
+ var leftFront = FindWheel(
+ wheels,
+ requireFront: true,
+ requireLeft: true);
+ var leftRear = FindWheel(
+ wheels,
+ requireFront: false,
+ requireLeft: true);
+ var rightFront = FindWheel(
+ wheels,
+ requireFront: true,
+ requireLeft: false);
+ var rightRear = FindWheel(
+ wheels,
+ requireFront: false,
+ requireLeft: false);
+
+ if (leftFront == null ||
+ leftRear == null ||
+ rightFront == null ||
+ rightRear == null)
+ {
+ return;
+ }
+
+ sample.HasSteeringDiagnostics = true;
+ sample.TargetSteerLeftFrontDegrees =
+ leftFront.GetSendAngle();
+ sample.TargetSteerLeftRearDegrees =
+ leftRear.GetSendAngle();
+ sample.TargetSteerRightFrontDegrees =
+ rightFront.GetSendAngle();
+ sample.TargetSteerRightRearDegrees =
+ rightRear.GetSendAngle();
+ sample.ActualSteerLeftFrontDegrees =
+ leftFront.ReadAngle();
+ sample.ActualSteerLeftRearDegrees =
+ leftRear.ReadAngle();
+ sample.ActualSteerRightFrontDegrees =
+ rightFront.ReadAngle();
+ sample.ActualSteerRightRearDegrees =
+ rightRear.ReadAngle();
+ }
+
+ ///
+ /// 根据车体X向前、Y向左的物理坐标查找指定象限中的舵轮。
+ ///
+ private static SteerWheel FindWheel(
+ IReadOnlyList wheels,
+ bool requireFront,
+ bool requireLeft)
+ {
+ foreach (var wheel in wheels)
+ {
+ var isFront =
+ wheel.PhysicalPosition.X >= 0f;
+ var isLeft =
+ wheel.PhysicalPosition.Y >= 0f;
+
+ if (isFront == requireFront &&
+ isLeft == requireLeft)
+ {
+ return wheel;
+ }
+ }
+
+ return null;
+ }
+
// 将内存中的采样数据写入CSV。
private void SaveCsv()
{
@@ -523,7 +741,25 @@ namespace MultiWheelC
"ControlLateralErrorMeters," +
"ControlHeadingErrorRadians," +
"ControlDistanceToTrajectoryMeters," +
- "ControlRemainingDistanceMeters");
+ "ControlRemainingDistanceMeters," +
+ "HasVelocityDiagnostics," +
+ "DetourEstimatedBodyVxMetersPerSecond," +
+ "DetourVelocityEstimateValid," +
+ "WheelFeedbackRawBodyVxMetersPerSecond," +
+ "WheelFeedbackFilteredBodyVxMetersPerSecond," +
+ "WheelFeedbackVelocityEstimateValid," +
+ "HasSteeringDiagnostics," +
+ "TargetSteerLeftFrontDegrees," +
+ "TargetSteerLeftRearDegrees," +
+ "TargetSteerRightFrontDegrees," +
+ "TargetSteerRightRearDegrees," +
+ "ActualSteerLeftFrontDegrees," +
+ "ActualSteerLeftRearDegrees," +
+ "ActualSteerRightFrontDegrees," +
+ "ActualSteerRightRearDegrees," +
+ "HasGcpCommand," +
+ "CommandFrontGcpAngleRadians," +
+ "CommandRearGcpAngleRadians");
foreach (var sample in snapshot)
{
@@ -610,7 +846,65 @@ namespace MultiWheelC
sample.ControlDistanceToTrajectoryMeters),
FormatOptional(
sample.HasControlReference,
- sample.ControlRemainingDistanceMeters)));
+ sample.ControlRemainingDistanceMeters),
+ sample.HasVelocityDiagnostics
+ ? "1"
+ : "0",
+ FormatOptional(
+ sample.HasVelocityDiagnostics,
+ sample.DetourEstimatedBodyVxMetersPerSecond),
+ sample.HasVelocityDiagnostics
+ ? sample.DetourVelocityEstimateValid
+ ? "1"
+ : "0"
+ : string.Empty,
+ FormatOptional(
+ sample.HasVelocityDiagnostics,
+ sample.WheelFeedbackRawBodyVxMetersPerSecond),
+ FormatOptional(
+ sample.HasVelocityDiagnostics,
+ sample.WheelFeedbackFilteredBodyVxMetersPerSecond),
+ sample.HasVelocityDiagnostics
+ ? sample.WheelFeedbackVelocityEstimateValid
+ ? "1"
+ : "0"
+ : string.Empty,
+ sample.HasSteeringDiagnostics
+ ? "1"
+ : "0",
+ FormatOptional(
+ sample.HasSteeringDiagnostics,
+ sample.TargetSteerLeftFrontDegrees),
+ FormatOptional(
+ sample.HasSteeringDiagnostics,
+ sample.TargetSteerLeftRearDegrees),
+ FormatOptional(
+ sample.HasSteeringDiagnostics,
+ sample.TargetSteerRightFrontDegrees),
+ FormatOptional(
+ sample.HasSteeringDiagnostics,
+ sample.TargetSteerRightRearDegrees),
+ FormatOptional(
+ sample.HasSteeringDiagnostics,
+ sample.ActualSteerLeftFrontDegrees),
+ FormatOptional(
+ sample.HasSteeringDiagnostics,
+ sample.ActualSteerLeftRearDegrees),
+ FormatOptional(
+ sample.HasSteeringDiagnostics,
+ sample.ActualSteerRightFrontDegrees),
+ FormatOptional(
+ sample.HasSteeringDiagnostics,
+ sample.ActualSteerRightRearDegrees),
+ sample.HasGcpCommand
+ ? "1"
+ : "0",
+ FormatOptional(
+ sample.HasGcpCommand,
+ sample.CommandFrontGcpAngleRadians),
+ FormatOptional(
+ sample.HasGcpCommand,
+ sample.CommandRearGcpAngleRadians)));
}
}
}
diff --git a/MultiWheelC/Movements/TrajectoryTrackingMovement.cs b/MultiWheelC/Movements/TrajectoryTrackingMovement.cs
index 4ec7419..ad6f2cb 100644
--- a/MultiWheelC/Movements/TrajectoryTrackingMovement.cs
+++ b/MultiWheelC/Movements/TrajectoryTrackingMovement.cs
@@ -51,7 +51,7 @@ namespace MultiWheelC
public double StanleyMinimumSpeedMetersPerSecond = 0.15;
///
- /// 获取或设置Stanley是否优先使用Detour估算的实际速度。
+ /// 获取或设置Stanley是否优先使用当前状态源提供的实际纵向速度。
///
public bool StanleyUsesActualSpeed = true;
@@ -129,7 +129,7 @@ namespace MultiWheelC
///
/// 车辆允许偏离参考轨迹的最大欧氏距离，单位为m。
///
- public double MaximumDistanceToTrajectoryMeters = 0.50;
+ public double MaximumDistanceToTrajectoryMeters = 0.30;
///
/// 单次轨迹动作允许的最长执行时间，单位为s。
diff --git a/MultiWheelC/PilotConfig.cs b/MultiWheelC/PilotConfig.cs
index cff2ed1..f1ae1c2 100644
--- a/MultiWheelC/PilotConfig.cs
+++ b/MultiWheelC/PilotConfig.cs
@@ -38,7 +38,7 @@ public class PilotConfig : MultiWheelPilotConfig
#region 单车-临时
[FieldMember(desc = "原地旋转Kp")]
- public float InPlaceRotateKp = 1.1f;
+ public float InPlaceRotateKp = 1.05f;
[FieldMember(desc = "原地旋转Ki")]
public float InPlaceRotateKi = 0f;
diff --git a/MultiWheelC/build/Clumsy/CommonUsage.dll b/MultiWheelC/build/Clumsy/CommonUsage.dll
index 93900a2..82a8d1d 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 5584b5b..962de2a 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 d7fa53b..9369a82 100644
Binary files a/MultiWheelC/build/Clumsy/MultiWheelC.pdb and b/MultiWheelC/build/Clumsy/MultiWheelC.pdb differ
diff --git a/data_process/新版前后角解耦控制器测试处理/4m直线0.3/记录.txt b/data_process/新版前后角解耦控制器测试处理/4m直线0.3/记录.txt
deleted file mode 100644
index a94b318..0000000
--- a/data_process/新版前后角解耦控制器测试处理/4m直线0.3/记录.txt
+++ /dev/null
@@ -1,17 +0,0 @@
-第一次：
-ok
-
-第二次：
-: * (Exception):DriveTask failed, msg=车辆已在终点零速参考处停稳，但终点精度不满足要求：位置误差=0.031m，航向误差=0.03°。, stack:
- at ClumsyCore.DriveTask.Wait() in D:\MDCS\Source\Core\Clumsy\ClumsyCore\DriveTask.cs:line 205
- at MultiWheelC.NewControllerStraight4mTest.Test() in D:\Users\Desktop\入职培训\停车机器人\MyParking\MultiWheelC\Experiments\NewControllerTrackingTests.cs:line 146
- at ClumsyLite.ClumsyLiteUI.<>c__DisplayClass19_1.b__21() in D:\MDCS\Source\Core\Clumsy\ClumsyLite\ClumsyLiteUI.cs:line 592
-
- *p.InnerException * (InvalidOperationException):车辆已在终点零速参考处停稳，但终点精度不满足要求：位置误差=0.031m，航向误差=0.03°。, stack:
- at MultiWheelC.TrajectoryTrackingMovement.Get()+MoveNext() in D:\Users\Desktop\入职培训\停车机器人\MyParking\MultiWheelC\Movements\TrajectoryTrackingMovement.cs:line 257
- at ClumsyCore.DriveTask.<>c__DisplayClass9_0.<.ctor>b__2() in D:\MDCS\Source\Core\Clumsy\ClumsyCore\DriveTask.cs:line 112
-
-
-
-第三次：
-ok
\ No newline at end of file
diff --git a/data_process/新版控制器轨迹测试处理/plot_new_controller_experiment.py b/data_process/新版控制器轨迹测试处理/plot_new_controller_experiment.py
index 7f37601..c7a7fcb 100644
--- a/data_process/新版控制器轨迹测试处理/plot_new_controller_experiment.py
+++ b/data_process/新版控制器轨迹测试处理/plot_new_controller_experiment.py
@@ -1,4 +1,4 @@
-"""为新版4m直线控制器实验CSV生成轨迹、横向/航向误差和速度响应图。"""
+"""为新版控制器实验CSV生成包含轨迹、误差、速度和转角的六子图总图。"""
from __future__ import annotations
@@ -293,13 +293,87 @@ def load_experiment(csv_path: Path) -> dict[str, object]:
)
if np.any(has_control_reference):
reference_speed[~has_control_reference] = np.nan
- actual_speed = numeric_column(frame, "StateBodyVxMetersPerSecond")
+ state_body_vx = numeric_column(frame, "StateBodyVxMetersPerSecond")
velocity_valid = (
numeric_column(frame, "StateVelocityEstimateValid", 0.0) > 0.5
)
- actual_speed[~velocity_valid] = np.nan
+ state_body_vx[~velocity_valid] = np.nan
+ has_velocity_diagnostics = (
+ numeric_column(frame, "HasVelocityDiagnostics", 0.0) > 0.5
+ )
+ detour_speed = numeric_column(
+ frame,
+ "DetourEstimatedBodyVxMetersPerSecond",
+ )
+ detour_speed_valid = (
+ has_velocity_diagnostics
+ & (
+ numeric_column(
+ frame,
+ "DetourVelocityEstimateValid",
+ 0.0,
+ )
+ > 0.5
+ )
+ )
+ detour_speed[~detour_speed_valid] = np.nan
+ wheel_raw_speed = numeric_column(
+ frame,
+ "WheelFeedbackRawBodyVxMetersPerSecond",
+ )
+ wheel_filtered_speed = numeric_column(
+ frame,
+ "WheelFeedbackFilteredBodyVxMetersPerSecond",
+ )
+ wheel_speed_valid = (
+ has_velocity_diagnostics
+ & (
+ numeric_column(
+ frame,
+ "WheelFeedbackVelocityEstimateValid",
+ 0.0,
+ )
+ > 0.5
+ )
+ )
+ 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,
+ )
command_speed = numeric_column(frame, "CommandSpeed")
+ has_steering_diagnostics = (
+ numeric_column(frame, "HasSteeringDiagnostics", 0.0) > 0.5
+ )
+ steering_angles_degrees = {}
+ for wheel_name in (
+ "LeftFront",
+ "LeftRear",
+ "RightFront",
+ "RightRear",
+ ):
+ values = numeric_column(
+ frame,
+ f"ActualSteer{wheel_name}Degrees",
+ )
+ values[~has_steering_diagnostics] = np.nan
+ steering_angles_degrees[wheel_name] = values
+
+ has_gcp_command = (
+ numeric_column(frame, "HasGcpCommand", 0.0) > 0.5
+ )
+ front_gcp_degrees = np.rad2deg(
+ numeric_column(frame, "CommandFrontGcpAngleRadians")
+ )
+ rear_gcp_degrees = np.rad2deg(
+ numeric_column(frame, "CommandRearGcpAngleRadians")
+ )
+ front_gcp_degrees[~has_gcp_command] = np.nan
+ rear_gcp_degrees[~has_gcp_command] = np.nan
+
return {
"frame": frame,
"time": time_seconds,
@@ -316,7 +390,13 @@ def load_experiment(csv_path: Path) -> dict[str, object]:
"heading_error": heading_error,
"reference_speed": reference_speed,
"actual_speed": actual_speed,
+ "detour_speed": detour_speed,
+ "wheel_raw_speed": wheel_raw_speed,
+ "wheel_filtered_speed": wheel_filtered_speed,
"command_speed": command_speed,
+ "steering_angles_degrees": steering_angles_degrees,
+ "front_gcp_degrees": front_gcp_degrees,
+ "rear_gcp_degrees": rear_gcp_degrees,
"controller_name": first_text(
frame,
"ControllerName",
@@ -342,7 +422,7 @@ def save_figure(
show: bool,
) -> None:
"""保存并关闭一张实验图。"""
- fig.tight_layout()
+ fig.tight_layout(rect=(0.0, 0.0, 1.0, 0.97))
fig.savefig(destination, dpi=300, bbox_inches="tight")
if show:
plt.show()
@@ -354,14 +434,16 @@ def plot_experiment(
output_directory: Path,
show: bool,
) -> list[Path]:
- """为单份新版控制器CSV生成四类对比图。"""
+ """为单份新版控制器CSV生成一张包含六个子图的实验总图。"""
data = load_experiment(csv_path)
output_directory.mkdir(parents=True, exist_ok=True)
title = f"{data['controller_name']} - {data['trajectory_name']}"
- destinations: list[Path] = []
+ fig, axes = plt.subplots(3, 2, figsize=(18.0, 16.0))
+ fig.suptitle(title, fontsize=16)
+ # 1. 期望轨迹与实际轨迹。
+ axis = axes[0, 0]
valid_position = data["valid_position"]
- fig, axis = plt.subplots(figsize=(9.0, 6.5))
valid_reference_position = data["valid_reference_position"]
if np.count_nonzero(valid_reference_position) >= 2:
axis.plot(
@@ -390,47 +472,42 @@ def plot_experiment(
axis.set_aspect("equal", adjustable="box")
axis.set_xlabel("世界坐标X / m")
axis.set_ylabel("世界坐标Y / m")
- axis.set_title(f"期望轨迹与实际轨迹对比\n{title}")
+ axis.set_title("期望轨迹与实际轨迹对比")
axis.grid(True, alpha=0.3)
- axis.legend()
- destination = output_directory / f"{csv_path.stem}_trajectory.png"
- save_figure(fig, destination, show)
- destinations.append(destination)
+ axis.legend(fontsize=8)
+ # 2. 横向误差。
lateral_mm = data["lateral_error"] * 1000.0
lateral_rmse_mm = finite_rmse(lateral_mm)
- fig, axis = plt.subplots(figsize=(10.0, 5.5))
+ axis = axes[0, 1]
axis.plot(data["time"], lateral_mm, linewidth=1.5)
axis.axhline(0.0, color="black", linewidth=0.8)
axis.set_xlabel("时间 / s")
axis.set_ylabel("横向误差 / mm")
axis.set_title(
- f"横向误差（轨迹在车辆左侧为正）\n{title}，RMSE={lateral_rmse_mm:.2f}mm"
+ "横向误差（轨迹在车辆左侧为正）\n"
+ f"RMSE={lateral_rmse_mm:.2f}mm"
)
axis.grid(True, alpha=0.3)
- destination = output_directory / f"{csv_path.stem}_lateral_error.png"
- save_figure(fig, destination, show)
- destinations.append(destination)
+ # 3. 航向误差。
heading_degrees = np.rad2deg(data["heading_error"])
heading_rmse_degrees = finite_rmse(heading_degrees)
- fig, axis = plt.subplots(figsize=(10.0, 5.5))
+ axis = axes[1, 0]
axis.plot(data["time"], heading_degrees, linewidth=1.5)
axis.axhline(0.0, color="black", linewidth=0.8)
axis.set_xlabel("时间 / s")
axis.set_ylabel("航向角偏差 / °")
axis.set_title(
"航向角偏差：参考轨迹航向－实际车体航向（逆时针为正）\n"
- f"{title}，RMSE={heading_rmse_degrees:.3f}°"
+ f"RMSE={heading_rmse_degrees:.3f}°"
)
axis.grid(True, alpha=0.3)
- destination = output_directory / f"{csv_path.stem}_heading_error.png"
- save_figure(fig, destination, show)
- destinations.append(destination)
+ # 4. 参考、命令、Detour估计和轮速解算速度。
speed_error = data["actual_speed"] - data["reference_speed"]
speed_rmse = finite_rmse(speed_error)
- fig, axis = plt.subplots(figsize=(10.0, 5.8))
+ axis = axes[1, 1]
axis.plot(
data["time"],
data["reference_speed"],
@@ -444,29 +521,118 @@ def plot_experiment(
linewidth=1.3,
label="纵向控制器下发速度",
)
- axis.plot(
- data["time"],
- data["actual_speed"],
- linewidth=1.5,
- label="状态估计实际车体纵向速度",
- )
+ if np.any(np.isfinite(data["detour_speed"])):
+ axis.plot(
+ data["time"],
+ data["detour_speed"],
+ ":",
+ linewidth=1.2,
+ label="Detour估计Vx",
+ )
+ if np.any(np.isfinite(data["wheel_filtered_speed"])):
+ axis.plot(
+ data["time"],
+ data["wheel_filtered_speed"],
+ linewidth=1.5,
+ label="轮速解算滤波Vx（控制使用）",
+ )
+ else:
+ axis.plot(
+ data["time"],
+ data["actual_speed"],
+ linewidth=1.5,
+ label="控制器实际纵向速度",
+ )
axis.set_xlabel("时间 / s")
axis.set_ylabel("速度 / (m/s)")
- axis.set_title(f"参考速度与实际速度对比\n{title}，RMSE={speed_rmse:.4f}m/s")
+ axis.set_title(
+ "参考速度、控制命令与观测速度\n"
+ f"轮速Vx相对参考速度RMSE={speed_rmse:.4f}m/s"
+ )
axis.grid(True, alpha=0.3)
- axis.legend()
- destination = output_directory / f"{csv_path.stem}_speed_response.png"
+ axis.legend(fontsize=8)
+
+ # 5. 四个舵轮的实际机械转角。
+ axis = axes[2, 0]
+ wheel_labels = {
+ "LeftFront": "左前轮",
+ "LeftRear": "左后轮",
+ "RightFront": "右前轮",
+ "RightRear": "右后轮",
+ }
+ steering_data_available = False
+ for wheel_name, wheel_label in wheel_labels.items():
+ wheel_angles = data["steering_angles_degrees"][wheel_name]
+ if np.any(np.isfinite(wheel_angles)):
+ steering_data_available = True
+ axis.plot(
+ data["time"],
+ wheel_angles,
+ linewidth=1.2,
+ label=wheel_label,
+ )
+ if steering_data_available:
+ axis.axhline(0.0, color="black", linewidth=0.8)
+ axis.legend(fontsize=8, ncol=2)
+ else:
+ axis.text(
+ 0.5,
+ 0.5,
+ "CSV不含四舵轮转角诊断数据",
+ ha="center",
+ va="center",
+ transform=axis.transAxes,
+ )
+ axis.set_xlabel("时间 / s")
+ axis.set_ylabel("实际舵角 / °")
+ axis.set_title("四个舵轮实际反馈转角")
+ axis.grid(True, alpha=0.3)
+
+ # 6. 经过角速度限制后实际发送的前、后虚拟GCP转角。
+ axis = axes[2, 1]
+ gcp_data_available = (
+ np.any(np.isfinite(data["front_gcp_degrees"]))
+ or np.any(np.isfinite(data["rear_gcp_degrees"]))
+ )
+ if gcp_data_available:
+ axis.plot(
+ data["time"],
+ data["front_gcp_degrees"],
+ linewidth=1.4,
+ label="前GCP",
+ )
+ axis.plot(
+ data["time"],
+ data["rear_gcp_degrees"],
+ linewidth=1.4,
+ label="后GCP",
+ )
+ axis.axhline(0.0, color="black", linewidth=0.8)
+ axis.legend(fontsize=8)
+ else:
+ axis.text(
+ 0.5,
+ 0.5,
+ "CSV不含前后GCP转角数据",
+ ha="center",
+ va="center",
+ transform=axis.transAxes,
+ )
+ axis.set_xlabel("时间 / s")
+ axis.set_ylabel("GCP命令角 / °")
+ axis.set_title("前后虚拟GCP实际发送转角")
+ axis.grid(True, alpha=0.3)
+
+ destination = output_directory / f"{csv_path.stem}_summary_6plots.png"
save_figure(fig, destination, show)
- destinations.append(destination)
print(
f"{csv_path.name}: 横向RMSE={lateral_rmse_mm:.3f}mm, "
f"航向RMSE={heading_rmse_degrees:.4f}°, "
f"速度RMSE={speed_rmse:.5f}m/s"
)
- for destination in destinations:
- print(f"已生成：{destination}")
- return destinations
+ print(f"已生成六子图总图：{destination}")
+ return [destination]
def discover_csv_files(arguments: list[str]) -> list[Path]:
@@ -487,7 +653,7 @@ def discover_csv_files(arguments: list[str]) -> list[Path]:
def main() -> None:
"""解析命令行并批量处理新版控制器实验CSV。"""
parser = argparse.ArgumentParser(
- description="绘制新版控制器轨迹实验的四类对比图。"
+ description="绘制新版控制器轨迹实验的六子图总图。"
)
parser.add_argument("csv", nargs="*", help="需要处理的CSV文件路径。")
parser.add_argument(
diff --git a/output/C/CommonUsage.dll b/output/C/CommonUsage.dll
index 93900a2..82a8d1d 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 5584b5b..962de2a 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 d7fa53b..9369a82 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 93900a2..82a8d1d 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 1713d12..b225c77 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 3ff5098..f755db3 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 93900a2..82a8d1d 100644
Binary files a/ref/CommonUsage.dll and b/ref/CommonUsage.dll differ