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

增加GCP运动学并完善车体平面速度反馈分析

Browse Source
This commit is contained in:
yuxiang.shen
2026-08-14 17:45:17 +08:00
parent 9fe8901c4c
commit d8de901a80
21 changed files with 445 additions and 38 deletions
Download Patch File Download Diff File Expand all files Collapse all files
MultiWheelC/Experiments/CompositeMotionPlanTests.cs
+4
View File
@@ -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);
}
MultiWheelC/Experiments/NewControllerTrackingTests.cs
+11
View File
@@ -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);
}
MultiWheelC/Experiments/TrackingExperimentRecorder.cs
+30 -2
View File
@@ -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
}
/// <summary>
/// 保存同一控制周期的Detour纵向速度以及轮速解算的原始和滤波纵向速度。
/// 保存同一控制周期的Detour纵向速度以及轮速解算的原始和滤波平面速度。
/// </summary>
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"