添加直线-半圆组合轨迹测试并调优Stanley参数与夹臂限位类型
Co-authored-by: Cursor <cursoragent@cursor.com>
This commit is contained in:
Cursor
@@ -47,7 +47,7 @@ namespace MultiWheelC
|
||||
/// <summary>
|
||||
/// 获取或设置参考速度减速度，单位为m/s²。
|
||||
/// </summary>
|
||||
public double DecelerationMetersPerSecondSquared = 0.20;
|
||||
public double DecelerationMetersPerSecondSquared = 0.12;
|
||||
|
||||
/// <summary>
|
||||
/// 获取或设置离散轨迹点间距，单位为m。
|
||||
@@ -294,4 +294,314 @@ namespace MultiWheelC
|
||||
MillimetersPerMeter));
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 从当前Detour位姿开始执行“3m直线—左半圆—3m直线”新版控制器跟踪实验。
|
||||
/// </summary>
|
||||
[MovementTest(name = "新版控制器：直线-左半圆-直线轨迹跟踪")]
|
||||
public sealed class NewControllerStraightSemicircleStraightTest
|
||||
: MovementTest
|
||||
{
|
||||
private const float MillimetersPerMeter = 1000f;
|
||||
|
||||
private readonly Painter _painter =
|
||||
UI.GetPainter(
|
||||
"NewControllerStraightSemicircleStraight");
|
||||
|
||||
private DriveTask _task;
|
||||
private TrackingExperimentRecorder _recorder;
|
||||
private DetourVehicleStateProvider _stateProvider;
|
||||
|
||||
/// <summary>
|
||||
/// 获取或设置本次测试编号，用于区分重复实验CSV。
|
||||
/// </summary>
|
||||
public int TrialNumber = 1;
|
||||
|
||||
/// <summary>
|
||||
/// 获取或设置半圆前后两段直线的长度，单位为m。
|
||||
/// </summary>
|
||||
public double StraightLengthMeters = 3.0;
|
||||
|
||||
/// <summary>
|
||||
/// 获取或设置左转半圆的转弯半径，单位为m。
|
||||
/// </summary>
|
||||
public double TurnRadiusMeters = 2.0;
|
||||
|
||||
/// <summary>
|
||||
/// 获取或设置直线与等曲率转弯之间的曲率过渡长度，单位为m。
|
||||
/// </summary>
|
||||
public double CurvatureTransitionLengthMeters = 0.60;
|
||||
|
||||
/// <summary>
|
||||
/// 获取或设置两段直线的最大参考速度，单位为m/s。
|
||||
/// </summary>
|
||||
public double StraightMaximumSpeedMetersPerSecond = 0.30;
|
||||
|
||||
/// <summary>
|
||||
/// 获取或设置半圆段的最大参考速度，单位为m/s。
|
||||
/// </summary>
|
||||
public double SemicircleMaximumSpeedMetersPerSecond = 0.25;
|
||||
|
||||
/// <summary>
|
||||
/// 获取或设置参考速度加速度，单位为m/s²。
|
||||
/// </summary>
|
||||
public double AccelerationMetersPerSecondSquared = 0.20;
|
||||
|
||||
/// <summary>
|
||||
/// 获取或设置参考速度减速度，单位为m/s²。
|
||||
/// </summary>
|
||||
public double DecelerationMetersPerSecondSquared = 0.12;
|
||||
|
||||
/// <summary>
|
||||
/// 获取或设置离散轨迹点间距，单位为m。
|
||||
/// </summary>
|
||||
public double PointSpacingMeters = 0.02;
|
||||
|
||||
/// <summary>
|
||||
/// 读取当前位姿、绘制组合轨迹并启动新版轨迹跟踪动作。
|
||||
/// </summary>
|
||||
public override void Test()
|
||||
{
|
||||
if (_task != null)
|
||||
{
|
||||
Console.WriteLine(
|
||||
"新版直线-左半圆-直线测试已经在运行，请先停止当前测试。");
|
||||
return;
|
||||
}
|
||||
|
||||
if (!MovementTestPreparation.AreWheelsForward())
|
||||
{
|
||||
return;
|
||||
}
|
||||
|
||||
var chassis =
|
||||
PilotDefinition.Chassis as MultiWheelChassis;
|
||||
if (chassis == null)
|
||||
{
|
||||
Console.WriteLine(
|
||||
"当前底盘不是MultiWheelChassis，无法执行新版轨迹测试。");
|
||||
return;
|
||||
}
|
||||
|
||||
_stateProvider =
|
||||
new DetourVehicleStateProvider();
|
||||
if (!_stateProvider.TryGetState(
|
||||
out var initialState))
|
||||
{
|
||||
Console.WriteLine(
|
||||
"无法读取有效Detour起点位姿：" +
|
||||
_stateProvider.LastFailureReason);
|
||||
_stateProvider = null;
|
||||
return;
|
||||
}
|
||||
|
||||
var trajectory =
|
||||
TestTrajectoryFactory
|
||||
.CreateStraightLeftSemicircleStraight(
|
||||
initialState.PoseInWorld,
|
||||
StraightLengthMeters,
|
||||
TurnRadiusMeters,
|
||||
CurvatureTransitionLengthMeters,
|
||||
StraightMaximumSpeedMetersPerSecond,
|
||||
SemicircleMaximumSpeedMetersPerSecond,
|
||||
AccelerationMetersPerSecondSquared,
|
||||
DecelerationMetersPerSecondSquared,
|
||||
PointSpacingMeters);
|
||||
|
||||
DrawTrajectory(trajectory);
|
||||
|
||||
var referenceStart = ToMillimeterVector(
|
||||
trajectory.StartPoint.PoseInWorld);
|
||||
var referenceEnd = ToMillimeterVector(
|
||||
trajectory.EndPoint.PoseInWorld);
|
||||
var recorder =
|
||||
new TrackingExperimentRecorder(
|
||||
controllerName: "NewStanleyPid",
|
||||
trajectoryName:
|
||||
"ProfiledStraightSmoothLeftTurnStraight",
|
||||
trialNumber: TrialNumber,
|
||||
referenceStart: referenceStart,
|
||||
referenceEnd: referenceEnd,
|
||||
referenceSpeed:
|
||||
(float)StraightMaximumSpeedMetersPerSecond,
|
||||
sampleIntervalMs: 50,
|
||||
referenceAccelerationMetersPerSecondSquared:
|
||||
(float)AccelerationMetersPerSecondSquared,
|
||||
referenceDecelerationMetersPerSecondSquared:
|
||||
(float)DecelerationMetersPerSecondSquared);
|
||||
_recorder = recorder;
|
||||
|
||||
var controlPointRadiusMeters =
|
||||
chassis.ControlPointRadius /
|
||||
MillimetersPerMeter;
|
||||
var movement =
|
||||
new TrajectoryTrackingMovement
|
||||
{
|
||||
Trajectory = trajectory,
|
||||
StateProvider = _stateProvider,
|
||||
MaximumCommandSpeedMetersPerSecond =
|
||||
StraightMaximumSpeedMetersPerSecond,
|
||||
CycleObserver = controller =>
|
||||
RecordControlCycle(
|
||||
recorder,
|
||||
controller,
|
||||
controlPointRadiusMeters)
|
||||
};
|
||||
|
||||
recorder.Start();
|
||||
|
||||
try
|
||||
{
|
||||
_task = new DriveTask(movement.Get());
|
||||
_task.Wait();
|
||||
|
||||
// 保留少量停车后原始Detour数据，并生成一帧处理后的静止状态。
|
||||
Thread.Sleep(400);
|
||||
recorder.UpdateCommand(0f, 0f);
|
||||
if (_stateProvider.TryGetState(
|
||||
out var stoppedState))
|
||||
{
|
||||
recorder.UpdateProcessedState(
|
||||
stoppedState);
|
||||
}
|
||||
}
|
||||
finally
|
||||
{
|
||||
_task?.Stop();
|
||||
recorder.UpdateCommand(0f, 0f);
|
||||
recorder.StopAndSave();
|
||||
_painter.Clear();
|
||||
_task = null;
|
||||
_recorder = null;
|
||||
_stateProvider = null;
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 停止组合轨迹测试、保存已有数据并清除Clumsy轨迹可视化。
|
||||
/// </summary>
|
||||
public override void TestStop()
|
||||
{
|
||||
_task?.Stop();
|
||||
_recorder?.UpdateCommand(0f, 0f);
|
||||
_recorder?.StopAndSave();
|
||||
_painter.Clear();
|
||||
_task = null;
|
||||
_recorder = null;
|
||||
_stateProvider = null;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 将组合轨迹的离散点和相邻线段转换为Clumsy毫米坐标后绘制。
|
||||
/// </summary>
|
||||
private void DrawTrajectory(
|
||||
Trajectory.Trajectory2D trajectory)
|
||||
{
|
||||
_painter.Clear();
|
||||
|
||||
for (var index = 0;
|
||||
index < trajectory.Count;
|
||||
index++)
|
||||
{
|
||||
var point = ToMillimeterVector(
|
||||
trajectory[index].PoseInWorld);
|
||||
_painter.DrawDot(
|
||||
Color.Cyan,
|
||||
point.X,
|
||||
point.Y,
|
||||
3f);
|
||||
|
||||
if (index == 0)
|
||||
{
|
||||
continue;
|
||||
}
|
||||
|
||||
var previousPoint = ToMillimeterVector(
|
||||
trajectory[index - 1].PoseInWorld);
|
||||
_painter.DrawLine(
|
||||
Color.DeepSkyBlue,
|
||||
previousPoint.X,
|
||||
previousPoint.Y,
|
||||
point.X,
|
||||
point.Y,
|
||||
width: 2);
|
||||
}
|
||||
|
||||
var start = ToMillimeterVector(
|
||||
trajectory.StartPoint.PoseInWorld);
|
||||
var end = ToMillimeterVector(
|
||||
trajectory.EndPoint.PoseInWorld);
|
||||
_painter.DrawDot(
|
||||
Color.LimeGreen,
|
||||
start.X,
|
||||
start.Y,
|
||||
8f);
|
||||
_painter.DrawDot(
|
||||
Color.OrangeRed,
|
||||
end.X,
|
||||
end.Y,
|
||||
8f);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 将组合轨迹控制周期的状态、参考量和最终GCP命令同步给实验记录器。
|
||||
/// </summary>
|
||||
private static void RecordControlCycle(
|
||||
TrackingExperimentRecorder recorder,
|
||||
ParkingGeometricController controller,
|
||||
double controlPointRadiusMeters)
|
||||
{
|
||||
if (controller.LastVehicleState.HasValue)
|
||||
{
|
||||
recorder.UpdateProcessedState(
|
||||
controller.LastVehicleState.Value);
|
||||
}
|
||||
|
||||
if (!controller.LastCommand.HasValue)
|
||||
{
|
||||
return;
|
||||
}
|
||||
|
||||
if (controller.LastProjection.HasValue &&
|
||||
controller.LastReferenceSpeedMetersPerSecond.HasValue)
|
||||
{
|
||||
var projection =
|
||||
controller.LastProjection.Value;
|
||||
recorder.UpdateControlReference(
|
||||
projection.ArcLengthMeters,
|
||||
controller.LastReferenceSpeedMetersPerSecond.Value,
|
||||
projection.LateralErrorMeters,
|
||||
projection.HeadingErrorRadians,
|
||||
projection.DistanceToTrajectoryMeters,
|
||||
projection.RemainingDistanceMeters);
|
||||
}
|
||||
|
||||
var command = controller.LastCommand.Value;
|
||||
var curvaturePerMeter = Math.Tan(
|
||||
command.FrontAngleRadians) /
|
||||
controlPointRadiusMeters;
|
||||
var angularSpeedRadiansPerSecond =
|
||||
command.SpeedMetersPerSecond *
|
||||
curvaturePerMeter;
|
||||
|
||||
recorder.UpdateCommand(
|
||||
(float)command.SpeedMetersPerSecond,
|
||||
(float)angularSpeedRadiansPerSecond);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 将Shared世界坐标系米制位姿转换为Clumsy绘图和记录器使用的毫米坐标。
|
||||
/// </summary>
|
||||
private static Vector2 ToMillimeterVector(
|
||||
Pose2D poseInWorld)
|
||||
{
|
||||
return new Vector2(
|
||||
(float)(
|
||||
poseInWorld.XMeters *
|
||||
MillimetersPerMeter),
|
||||
(float)(
|
||||
poseInWorld.YMeters *
|
||||
MillimetersPerMeter));
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
Reference in New Issue
Block a user