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

增加倒车测试

Browse Source
This commit is contained in:
yuxiang.shen
2026-08-13 13:49:39 +08:00
parent fd35047325
commit 7611deefa3
18 changed files with 257 additions and 19 deletions
Download Patch File Download Diff File Expand all files Collapse all files
MultiWheelC/Configuration/PilotConfig.ParkingControl.cs
+1 -1
View File
@@ -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;
MultiWheelC/Control/Execution/ParkingGeometricController.cs
+14 -1
View File
@@ -271,7 +271,12 @@ namespace MultiWheelC.Control.Execution
public TrajectoryProjection? LastProjection { get; private set; }
/// <summary>
/// 获取最近一次发送或准备发送的GCP运动命令。
/// 获取最近一次由控制器和分配器生成、尚未经过角速度限制的GCP运动命令。
/// </summary>
public GcpMotionCommand? LastRequestedCommand { get; private set; }
/// <summary>
/// 获取最近一次经过GCP角速度限制后实际发送给底盘的运动命令。
/// </summary>
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;
MultiWheelC/Experiments/CompositeMotionPlanTests.cs
+8 -1
View File
@@ -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(
MultiWheelC/Experiments/NewControllerTrackingTests.cs
+50 -4
View File
@@ -94,7 +94,7 @@ namespace MultiWheelC
/// 从当前Detour位姿开始执行新版控制器4m直线跟踪并保存实验数据。
/// </summary>
[MovementTest(name = "新版控制器：4m直线轨迹跟踪")]
public sealed class NewControllerStraight4mTest
public class NewControllerStraight4mTest
: MovementTest
{
private const float MillimetersPerMeter = 1000f;
@@ -131,6 +131,12 @@ namespace MultiWheelC
/// </summary>
public double PointSpacingMeters = 0.02;
/// <summary>
/// 获取实验记录使用的轨迹基础名称，供同一套直线测试流程区分前进和倒车。
/// </summary>
protected virtual string ExperimentTrajectoryBaseName =>
"ProfiledStraight4m";
/// <summary>
/// 读取当前位姿、绘制离散轨迹并启动新版轨迹跟踪动作。
/// </summary>
@@ -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
}
/// <summary>
/// 从当前Detour位姿开始，沿车体后方执行新版控制器4m直线倒车跟踪并保存实验数据。
/// </summary>
[MovementTest(name = "新版控制器：4m直线倒车轨迹跟踪")]
public sealed class NewControllerReverseStraight4mTest
: NewControllerStraight4mTest
{
/// <summary>
/// 使用负参考速度，使轨迹工厂沿车尾方向生成轨迹并触发倒车控制语义。
/// </summary>
public NewControllerReverseStraight4mTest()
{
CruiseSpeedMetersPerSecond = -0.40;
}
/// <summary>
/// 将倒车实验与前进直线实验的CSV名称明确区分。
/// </summary>
protected override string ExperimentTrajectoryBaseName =>
"ProfiledReverseStraight4m";
}
/// <summary>
/// 从当前Detour位姿开始执行“3m直线—左半圆—3m直线”新版控制器跟踪实验。
/// </summary>
@@ -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(
MultiWheelC/Experiments/TrackingExperimentRecorder.cs
+184 -12
View File
@@ -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
/// </summary>
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
}
/// <summary>
/// 保存经过GCP角速度限制后实际交给底盘的前后虚拟控制点转角。
/// 保存本周期经过GCP角速度限制前后的前后虚拟控制点转角。
/// </summary>
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;
}
/// <summary>
/// 在GCP命令有效时将布尔诊断输出为0或1，否则保持CSV空值。
/// </summary>
private static string FormatOptionalBoolean(
bool hasValue,
bool value)
{
return hasValue
? value
? "1"
: "0"
: string.Empty;
}
/// <summary>
/// 判断GCP角速度限制是否实质改变了控制器请求角度。
/// </summary>
private static bool IsGcpRateLimitActive(
double requestedAngleRadians,
double sentAngleRadians)
{
return Math.Abs(
requestedAngleRadians -
sentAngleRadians) >
GcpAngleComparisonToleranceRadians;
}
// 对CSV文本字段进行引号和逗号转义。
private static string EscapeCsv(string value)
{
MultiWheelC/build/Clumsy/CommonUsage.dll
BIN
View File
Binary file not shown.
MultiWheelC/build/Clumsy/MultiWheelC.dll
BIN
View File
Binary file not shown.
MultiWheelC/build/Clumsy/MultiWheelC.pdb
BIN
View File
Binary file not shown.