diff --git a/MedullaAdapter/build/Medulla/plugins/CommonUsage.dll b/MedullaAdapter/build/Medulla/plugins/CommonUsage.dll
index 128c1e7..cc330a2 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 059ddf8..34dc787 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 51a851a..1e174f5 100644
Binary files a/MedullaAdapter/build/Medulla/plugins/MedullaAdapter.pdb and b/MedullaAdapter/build/Medulla/plugins/MedullaAdapter.pdb differ
diff --git a/MultiWheelC/Configuration/PilotConfig.ParkingControl.cs b/MultiWheelC/Configuration/PilotConfig.ParkingControl.cs
index 1cd5fce..a041b4d 100644
--- a/MultiWheelC/Configuration/PilotConfig.ParkingControl.cs
+++ b/MultiWheelC/Configuration/PilotConfig.ParkingControl.cs
@@ -94,7 +94,7 @@ public partial class PilotConfig
#region 停车控制-原地自转
[FieldMember(desc = "停车控制：原地自转Kp")]
- public float InPlaceRotateKp = 1.05f;
+ public float InPlaceRotateKp = 1.03f;
[FieldMember(desc = "停车控制：原地自转Ki")]
public float InPlaceRotateKi = 0f;
diff --git a/MultiWheelC/Control/Execution/ParkingGeometricController.cs b/MultiWheelC/Control/Execution/ParkingGeometricController.cs
index da4ca60..0e3573a 100644
--- a/MultiWheelC/Control/Execution/ParkingGeometricController.cs
+++ b/MultiWheelC/Control/Execution/ParkingGeometricController.cs
@@ -271,7 +271,12 @@ namespace MultiWheelC.Control.Execution
public TrajectoryProjection? LastProjection { get; private set; }
///
- /// 获取最近一次发送或准备发送的GCP运动命令。
+ /// 获取最近一次由控制器和分配器生成、尚未经过角速度限制的GCP运动命令。
+ ///
+ public GcpMotionCommand? LastRequestedCommand { get; private set; }
+
+ ///
+ /// 获取最近一次经过GCP角速度限制后实际发送给底盘的运动命令。
///
public GcpMotionCommand? LastCommand { get; private set; }
@@ -488,6 +493,7 @@ namespace MultiWheelC.Control.Execution
var gcpCommand = _gcpAllocator.Allocate(
commandSpeedMetersPerSecond,
lateralCommand);
+ LastRequestedCommand = gcpCommand;
controllerComputeMilliseconds =
GetElapsedMilliseconds(
@@ -942,6 +948,7 @@ namespace MultiWheelC.Control.Execution
_commandExecutor.Stop();
_lateralController.Reset();
_longitudinalController.Reset();
+ LastRequestedCommand = null;
LastCommand = null;
LastFailureReason =
"当前无法获得有效车辆状态，底盘已停车并等待定位恢复。";
@@ -956,6 +963,10 @@ namespace MultiWheelC.Control.Execution
StopAndResetControllers();
IsActive = false;
IsCompleted = true;
+ LastRequestedCommand = new GcpMotionCommand(
+ 0.0,
+ 0.0,
+ 0.0);
LastCommand = new GcpMotionCommand(
0.0,
0.0,
@@ -974,6 +985,7 @@ namespace MultiWheelC.Control.Execution
StopAndResetControllers();
IsActive = false;
IsCompleted = false;
+ LastRequestedCommand = null;
LastCommand = null;
LastFailureReason = reason;
LastException = exception;
@@ -1000,6 +1012,7 @@ namespace MultiWheelC.Control.Execution
_previousStateTimestampSeconds = 0.0;
LastVehicleState = null;
LastProjection = null;
+ LastRequestedCommand = null;
LastCommand = null;
LastControlReferenceSpeedMetersPerSecond = null;
LastCurvaturePreviewDistanceMeters = null;
diff --git a/MultiWheelC/Experiments/CompositeMotionPlanTests.cs b/MultiWheelC/Experiments/CompositeMotionPlanTests.cs
index c9f6914..f7a9a1d 100644
--- a/MultiWheelC/Experiments/CompositeMotionPlanTests.cs
+++ b/MultiWheelC/Experiments/CompositeMotionPlanTests.cs
@@ -291,7 +291,9 @@ namespace MultiWheelC
{
_recorder?.RecordControlCycleTiming(
controller.LastCycleTiming.Value,
- motionSegmentIndex);
+ motionSegmentIndex,
+ controller.LastRequestedCommand,
+ controller.LastCommand);
}
if (controller.LastVehicleState.HasValue)
@@ -338,8 +340,13 @@ namespace MultiWheelC
projection.ReferencePoint.CurvaturePerMeter);
}
+ var requestedCommand =
+ controller.LastRequestedCommand ??
+ controller.LastCommand.Value;
var command = controller.LastCommand.Value;
_recorder?.UpdateGcpCommand(
+ requestedCommand.FrontAngleRadians,
+ requestedCommand.RearAngleRadians,
command.FrontAngleRadians,
command.RearAngleRadians);
var curvaturePerMeter = Math.Tan(
diff --git a/MultiWheelC/Experiments/NewControllerTrackingTests.cs b/MultiWheelC/Experiments/NewControllerTrackingTests.cs
index fcdb842..ad06f9a 100644
--- a/MultiWheelC/Experiments/NewControllerTrackingTests.cs
+++ b/MultiWheelC/Experiments/NewControllerTrackingTests.cs
@@ -94,7 +94,7 @@ namespace MultiWheelC
/// 从当前Detour位姿开始执行新版控制器4m直线跟踪并保存实验数据。
///
[MovementTest(name = "新版控制器：4m直线轨迹跟踪")]
- public sealed class NewControllerStraight4mTest
+ public class NewControllerStraight4mTest
: MovementTest
{
private const float MillimetersPerMeter = 1000f;
@@ -131,6 +131,12 @@ namespace MultiWheelC
///
public double PointSpacingMeters = 0.02;
+ ///
+ /// 获取实验记录使用的轨迹基础名称，供同一套直线测试流程区分前进和倒车。
+ ///
+ protected virtual string ExperimentTrajectoryBaseName =>
+ "ProfiledStraight4m";
+
///
/// 读取当前位姿、绘制离散轨迹并启动新版轨迹跟踪动作。
///
@@ -197,7 +203,7 @@ namespace MultiWheelC
controllerName: "NewStanleyPid",
trajectoryName:
TrajectoryExperimentInput.BuildTrajectoryName(
- "ProfiledStraight4m",
+ ExperimentTrajectoryBaseName,
lateralOffsetMeters),
trialNumber: TrialNumber,
referenceStart: referenceStart,
@@ -336,7 +342,11 @@ namespace MultiWheelC
if (controller.LastCycleTiming.HasValue)
{
recorder.RecordControlCycleTiming(
- controller.LastCycleTiming.Value);
+ controller.LastCycleTiming.Value,
+ requestedCommand:
+ controller.LastRequestedCommand,
+ sentCommand:
+ controller.LastCommand);
}
if (controller.LastVehicleState.HasValue)
@@ -371,8 +381,13 @@ namespace MultiWheelC
projection.ReferencePoint.CurvaturePerMeter);
}
+ var requestedCommand =
+ controller.LastRequestedCommand ??
+ controller.LastCommand.Value;
var command = controller.LastCommand.Value;
recorder.UpdateGcpCommand(
+ requestedCommand.FrontAngleRadians,
+ requestedCommand.RearAngleRadians,
command.FrontAngleRadians,
command.RearAngleRadians);
var curvaturePerMeter = Math.Tan(
@@ -430,6 +445,28 @@ namespace MultiWheelC
}
+ ///
+ /// 从当前Detour位姿开始，沿车体后方执行新版控制器4m直线倒车跟踪并保存实验数据。
+ ///
+ [MovementTest(name = "新版控制器：4m直线倒车轨迹跟踪")]
+ public sealed class NewControllerReverseStraight4mTest
+ : NewControllerStraight4mTest
+ {
+ ///
+ /// 使用负参考速度，使轨迹工厂沿车尾方向生成轨迹并触发倒车控制语义。
+ ///
+ public NewControllerReverseStraight4mTest()
+ {
+ CruiseSpeedMetersPerSecond = -0.40;
+ }
+
+ ///
+ /// 将倒车实验与前进直线实验的CSV名称明确区分。
+ ///
+ protected override string ExperimentTrajectoryBaseName =>
+ "ProfiledReverseStraight4m";
+ }
+
///
/// 从当前Detour位姿开始执行“3m直线—左半圆—3m直线”新版控制器跟踪实验。
///
@@ -702,7 +739,11 @@ namespace MultiWheelC
if (controller.LastCycleTiming.HasValue)
{
recorder.RecordControlCycleTiming(
- controller.LastCycleTiming.Value);
+ controller.LastCycleTiming.Value,
+ requestedCommand:
+ controller.LastRequestedCommand,
+ sentCommand:
+ controller.LastCommand);
}
if (controller.LastVehicleState.HasValue)
@@ -737,8 +778,13 @@ namespace MultiWheelC
projection.ReferencePoint.CurvaturePerMeter);
}
+ var requestedCommand =
+ controller.LastRequestedCommand ??
+ controller.LastCommand.Value;
var command = controller.LastCommand.Value;
recorder.UpdateGcpCommand(
+ requestedCommand.FrontAngleRadians,
+ requestedCommand.RearAngleRadians,
command.FrontAngleRadians,
command.RearAngleRadians);
var curvaturePerMeter = Math.Tan(
diff --git a/MultiWheelC/Experiments/TrackingExperimentRecorder.cs b/MultiWheelC/Experiments/TrackingExperimentRecorder.cs
index 564e5c0..95fc6fd 100644
--- a/MultiWheelC/Experiments/TrackingExperimentRecorder.cs
+++ b/MultiWheelC/Experiments/TrackingExperimentRecorder.cs
@@ -9,6 +9,7 @@ using System.Text;
using System.Threading;
using CommonUsage.Chassis;
using MyParking.Shared;
+using MultiWheelC.Control.Allocation;
using MultiWheelC.Control.Execution;
using MultiWheelC.StateEstimation;
@@ -71,6 +72,13 @@ namespace MultiWheelC
public double ActualSteerRightFrontDegrees;
public double ActualSteerRightRearDegrees;
public bool HasGcpCommand;
+ public double RequestedFrontGcpAngleRadians;
+ public double RequestedRearGcpAngleRadians;
+ public double SentFrontGcpAngleRadians;
+ public double SentRearGcpAngleRadians;
+ public bool FrontGcpRateLimitActive;
+ public bool RearGcpRateLimitActive;
+ // 兼容既有分析脚本：Command角仍表示限速后实际发送角。
public double CommandFrontGcpAngleRadians;
public double CommandRearGcpAngleRadians;
}
@@ -86,19 +94,28 @@ namespace MultiWheelC
public ControlCycleTimingRecord(
double recorderElapsedSeconds,
int motionSegmentIndex,
- ParkingControlCycleTiming timing)
+ ParkingControlCycleTiming timing,
+ GcpMotionCommand? requestedCommand,
+ GcpMotionCommand? sentCommand)
{
RecorderElapsedSeconds =
recorderElapsedSeconds;
MotionSegmentIndex = motionSegmentIndex;
Timing = timing;
+ RequestedCommand = requestedCommand;
+ SentCommand = sentCommand;
}
public double RecorderElapsedSeconds { get; }
public int MotionSegmentIndex { get; }
public ParkingControlCycleTiming Timing { get; }
+ public GcpMotionCommand? RequestedCommand { get; }
+ public GcpMotionCommand? SentCommand { get; }
}
+ private const double GcpAngleComparisonToleranceRadians =
+ 1e-9;
+
private readonly string _controllerName;
private readonly string _trajectoryName;
private readonly int _trialNumber;
@@ -161,6 +178,12 @@ namespace MultiWheelC
private double _wheelFeedbackFilteredBodyVxMetersPerSecond;
private bool _wheelFeedbackVelocityEstimateValid;
private bool _hasGcpCommand;
+ private double _requestedFrontGcpAngleRadians;
+ private double _requestedRearGcpAngleRadians;
+ private double _sentFrontGcpAngleRadians;
+ private double _sentRearGcpAngleRadians;
+ private bool _frontGcpRateLimitActive;
+ private bool _rearGcpRateLimitActive;
private double _commandFrontGcpAngleRadians;
private double _commandRearGcpAngleRadians;
@@ -288,12 +311,16 @@ namespace MultiWheelC
///
public void RecordControlCycleTiming(
ParkingControlCycleTiming timing,
- int motionSegmentIndex = -1)
+ int motionSegmentIndex = -1,
+ GcpMotionCommand? requestedCommand = null,
+ GcpMotionCommand? sentCommand = null)
{
var record = new ControlCycleTimingRecord(
_stopwatch.Elapsed.TotalSeconds,
motionSegmentIndex,
- timing);
+ timing,
+ requestedCommand,
+ sentCommand);
lock (_controlCycleTimingSyncRoot)
{
@@ -339,18 +366,37 @@ namespace MultiWheelC
}
///
- /// 保存经过GCP角速度限制后实际交给底盘的前后虚拟控制点转角。
+ /// 保存本周期经过GCP角速度限制前后的前后虚拟控制点转角。
///
public void UpdateGcpCommand(
- double frontGcpAngleRadians,
- double rearGcpAngleRadians)
+ double requestedFrontGcpAngleRadians,
+ double requestedRearGcpAngleRadians,
+ double sentFrontGcpAngleRadians,
+ double sentRearGcpAngleRadians)
{
lock (_stateSyncRoot)
{
+ _requestedFrontGcpAngleRadians =
+ requestedFrontGcpAngleRadians;
+ _requestedRearGcpAngleRadians =
+ requestedRearGcpAngleRadians;
+ _sentFrontGcpAngleRadians =
+ sentFrontGcpAngleRadians;
+ _sentRearGcpAngleRadians =
+ sentRearGcpAngleRadians;
+ _frontGcpRateLimitActive =
+ IsGcpRateLimitActive(
+ requestedFrontGcpAngleRadians,
+ sentFrontGcpAngleRadians);
+ _rearGcpRateLimitActive =
+ IsGcpRateLimitActive(
+ requestedRearGcpAngleRadians,
+ sentRearGcpAngleRadians);
+ // 保留原字段语义，避免既有绘图脚本失效。
_commandFrontGcpAngleRadians =
- frontGcpAngleRadians;
+ sentFrontGcpAngleRadians;
_commandRearGcpAngleRadians =
- rearGcpAngleRadians;
+ sentRearGcpAngleRadians;
_hasGcpCommand = true;
}
}
@@ -499,6 +545,12 @@ namespace MultiWheelC
double wheelFeedbackFilteredBodyVxMetersPerSecond;
bool wheelFeedbackVelocityEstimateValid;
bool hasGcpCommand;
+ double requestedFrontGcpAngleRadians;
+ double requestedRearGcpAngleRadians;
+ double sentFrontGcpAngleRadians;
+ double sentRearGcpAngleRadians;
+ bool frontGcpRateLimitActive;
+ bool rearGcpRateLimitActive;
double commandFrontGcpAngleRadians;
double commandRearGcpAngleRadians;
@@ -569,6 +621,18 @@ namespace MultiWheelC
wheelFeedbackVelocityEstimateValid =
_wheelFeedbackVelocityEstimateValid;
hasGcpCommand = _hasGcpCommand;
+ requestedFrontGcpAngleRadians =
+ _requestedFrontGcpAngleRadians;
+ requestedRearGcpAngleRadians =
+ _requestedRearGcpAngleRadians;
+ sentFrontGcpAngleRadians =
+ _sentFrontGcpAngleRadians;
+ sentRearGcpAngleRadians =
+ _sentRearGcpAngleRadians;
+ frontGcpRateLimitActive =
+ _frontGcpRateLimitActive;
+ rearGcpRateLimitActive =
+ _rearGcpRateLimitActive;
commandFrontGcpAngleRadians =
_commandFrontGcpAngleRadians;
commandRearGcpAngleRadians =
@@ -620,6 +684,18 @@ namespace MultiWheelC
WheelFeedbackVelocityEstimateValid =
wheelFeedbackVelocityEstimateValid,
HasGcpCommand = hasGcpCommand,
+ RequestedFrontGcpAngleRadians =
+ requestedFrontGcpAngleRadians,
+ RequestedRearGcpAngleRadians =
+ requestedRearGcpAngleRadians,
+ SentFrontGcpAngleRadians =
+ sentFrontGcpAngleRadians,
+ SentRearGcpAngleRadians =
+ sentRearGcpAngleRadians,
+ FrontGcpRateLimitActive =
+ frontGcpRateLimitActive,
+ RearGcpRateLimitActive =
+ rearGcpRateLimitActive,
CommandFrontGcpAngleRadians =
commandFrontGcpAngleRadians,
CommandRearGcpAngleRadians =
@@ -841,7 +917,13 @@ namespace MultiWheelC
"ActualSteerRightRearDegrees," +
"HasGcpCommand," +
"CommandFrontGcpAngleRadians," +
- "CommandRearGcpAngleRadians");
+ "CommandRearGcpAngleRadians," +
+ "RequestedFrontGcpAngleRadians," +
+ "RequestedRearGcpAngleRadians," +
+ "SentFrontGcpAngleRadians," +
+ "SentRearGcpAngleRadians," +
+ "FrontGcpRateLimitActive," +
+ "RearGcpRateLimitActive");
foreach (var sample in snapshot)
{
@@ -992,7 +1074,25 @@ namespace MultiWheelC
sample.CommandFrontGcpAngleRadians),
FormatOptional(
sample.HasGcpCommand,
- sample.CommandRearGcpAngleRadians)));
+ sample.CommandRearGcpAngleRadians),
+ FormatOptional(
+ sample.HasGcpCommand,
+ sample.RequestedFrontGcpAngleRadians),
+ FormatOptional(
+ sample.HasGcpCommand,
+ sample.RequestedRearGcpAngleRadians),
+ FormatOptional(
+ sample.HasGcpCommand,
+ sample.SentFrontGcpAngleRadians),
+ FormatOptional(
+ sample.HasGcpCommand,
+ sample.SentRearGcpAngleRadians),
+ FormatOptionalBoolean(
+ sample.HasGcpCommand,
+ sample.FrontGcpRateLimitActive),
+ FormatOptionalBoolean(
+ sample.HasGcpCommand,
+ sample.RearGcpRateLimitActive)));
}
}
}
@@ -1046,7 +1146,14 @@ namespace MultiWheelC
"HasStateTimestamp," +
"StateTimestampSeconds," +
"StateTimestampChanged," +
- "CycleResult");
+ "CycleResult," +
+ "HasGcpCommand," +
+ "RequestedFrontGcpAngleRadians," +
+ "RequestedRearGcpAngleRadians," +
+ "SentFrontGcpAngleRadians," +
+ "SentRearGcpAngleRadians," +
+ "FrontGcpRateLimitActive," +
+ "RearGcpRateLimitActive");
foreach (var record in snapshot)
{
@@ -1060,6 +1167,23 @@ namespace MultiWheelC
0.0,
timing.TotalCycleMilliseconds -
measuredStageMilliseconds);
+ var hasGcpCommand =
+ record.RequestedCommand.HasValue &&
+ record.SentCommand.HasValue;
+ var requestedCommand =
+ record.RequestedCommand.GetValueOrDefault();
+ var sentCommand =
+ record.SentCommand.GetValueOrDefault();
+ var frontRateLimitActive =
+ hasGcpCommand &&
+ IsGcpRateLimitActive(
+ requestedCommand.FrontAngleRadians,
+ sentCommand.FrontAngleRadians);
+ var rearRateLimitActive =
+ hasGcpCommand &&
+ IsGcpRateLimitActive(
+ requestedCommand.RearAngleRadians,
+ sentCommand.RearAngleRadians);
writer.WriteLine(string.Join(
",",
@@ -1092,7 +1216,28 @@ namespace MultiWheelC
? "1"
: "0"
: string.Empty,
- EscapeCsv(timing.Result.ToString())));
+ EscapeCsv(timing.Result.ToString()),
+ hasGcpCommand
+ ? "1"
+ : "0",
+ FormatOptional(
+ hasGcpCommand,
+ requestedCommand.FrontAngleRadians),
+ FormatOptional(
+ hasGcpCommand,
+ requestedCommand.RearAngleRadians),
+ FormatOptional(
+ hasGcpCommand,
+ sentCommand.FrontAngleRadians),
+ FormatOptional(
+ hasGcpCommand,
+ sentCommand.RearAngleRadians),
+ FormatOptionalBoolean(
+ hasGcpCommand,
+ frontRateLimitActive),
+ FormatOptionalBoolean(
+ hasGcpCommand,
+ rearRateLimitActive)));
}
}
}
@@ -1133,6 +1278,33 @@ namespace MultiWheelC
: string.Empty;
}
+ ///
+ /// 在GCP命令有效时将布尔诊断输出为0或1，否则保持CSV空值。
+ ///
+ private static string FormatOptionalBoolean(
+ bool hasValue,
+ bool value)
+ {
+ return hasValue
+ ? value
+ ? "1"
+ : "0"
+ : string.Empty;
+ }
+
+ ///
+ /// 判断GCP角速度限制是否实质改变了控制器请求角度。
+ ///
+ private static bool IsGcpRateLimitActive(
+ double requestedAngleRadians,
+ double sentAngleRadians)
+ {
+ return Math.Abs(
+ requestedAngleRadians -
+ sentAngleRadians) >
+ GcpAngleComparisonToleranceRadians;
+ }
+
// 对CSV文本字段进行引号和逗号转义。
private static string EscapeCsv(string value)
{
diff --git a/MultiWheelC/build/Clumsy/CommonUsage.dll b/MultiWheelC/build/Clumsy/CommonUsage.dll
index 128c1e7..cc330a2 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 11b41a8..98814d9 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 8ab6e1c..07890c2 100644
Binary files a/MultiWheelC/build/Clumsy/MultiWheelC.pdb and b/MultiWheelC/build/Clumsy/MultiWheelC.pdb differ
diff --git a/output/C/CommonUsage.dll b/output/C/CommonUsage.dll
index 128c1e7..cc330a2 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 11b41a8..98814d9 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 8ab6e1c..07890c2 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 128c1e7..cc330a2 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 059ddf8..34dc787 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 51a851a..1e174f5 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 128c1e7..cc330a2 100644
Binary files a/ref/CommonUsage.dll and b/ref/CommonUsage.dll differ