添加直线-半圆组合轨迹测试并调优Stanley参数与夹臂限位类型
Co-authored-by: Cursor <cursoragent@cursor.com>
This commit is contained in:
Cursor
@@ -47,7 +47,7 @@ namespace MultiWheelC
|
|||||||
/// <summary>
|
/// <summary>
|
||||||
/// 获取或设置参考速度减速度，单位为m/s²。
|
/// 获取或设置参考速度减速度，单位为m/s²。
|
||||||
/// </summary>
|
/// </summary>
|
||||||
public double DecelerationMetersPerSecondSquared = 0.20;
|
public double DecelerationMetersPerSecondSquared = 0.12;
|
||||||
|
|
||||||
/// <summary>
|
/// <summary>
|
||||||
/// 获取或设置离散轨迹点间距，单位为m。
|
/// 获取或设置离散轨迹点间距，单位为m。
|
||||||
@@ -294,4 +294,314 @@ namespace MultiWheelC
|
|||||||
MillimetersPerMeter));
|
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));
|
||||||
|
}
|
||||||
|
}
|
||||||
}
|
}
|
||||||
|
|||||||
@@ -92,6 +92,399 @@ namespace MultiWheelC
|
|||||||
return new Trajectory2D(points);
|
return new Trajectory2D(points);
|
||||||
}
|
}
|
||||||
|
|
||||||
|
/// <summary>
|
||||||
|
/// 从当前位姿生成“3m直线、平滑进入半径2m左转、平滑退出、3m直线”的180°转弯轨迹。
|
||||||
|
/// </summary>
|
||||||
|
public static Trajectory2D CreateStraightLeftSemicircleStraight(
|
||||||
|
Pose2D startPoseInWorld,
|
||||||
|
double straightLengthMeters = 3.0,
|
||||||
|
double turnRadiusMeters = 2.0,
|
||||||
|
double curvatureTransitionLengthMeters = 0.60,
|
||||||
|
double straightMaximumSpeedMetersPerSecond = 0.30,
|
||||||
|
double semicircleMaximumSpeedMetersPerSecond = 0.25,
|
||||||
|
double accelerationMetersPerSecondSquared = 0.20,
|
||||||
|
double decelerationMetersPerSecondSquared = 0.12,
|
||||||
|
double pointSpacingMeters = 0.02)
|
||||||
|
{
|
||||||
|
EnsureFinitePose(
|
||||||
|
startPoseInWorld,
|
||||||
|
nameof(startPoseInWorld));
|
||||||
|
EnsureFinitePositive(
|
||||||
|
straightLengthMeters,
|
||||||
|
nameof(straightLengthMeters));
|
||||||
|
EnsureFinitePositive(
|
||||||
|
turnRadiusMeters,
|
||||||
|
nameof(turnRadiusMeters));
|
||||||
|
EnsureFinitePositive(
|
||||||
|
curvatureTransitionLengthMeters,
|
||||||
|
nameof(curvatureTransitionLengthMeters));
|
||||||
|
EnsureFinitePositive(
|
||||||
|
straightMaximumSpeedMetersPerSecond,
|
||||||
|
nameof(straightMaximumSpeedMetersPerSecond));
|
||||||
|
EnsureFinitePositive(
|
||||||
|
semicircleMaximumSpeedMetersPerSecond,
|
||||||
|
nameof(semicircleMaximumSpeedMetersPerSecond));
|
||||||
|
EnsureFinitePositive(
|
||||||
|
accelerationMetersPerSecondSquared,
|
||||||
|
nameof(accelerationMetersPerSecondSquared));
|
||||||
|
EnsureFinitePositive(
|
||||||
|
decelerationMetersPerSecondSquared,
|
||||||
|
nameof(decelerationMetersPerSecondSquared));
|
||||||
|
EnsureFinitePositive(
|
||||||
|
pointSpacingMeters,
|
||||||
|
nameof(pointSpacingMeters));
|
||||||
|
|
||||||
|
var originalSemicircleLengthMeters =
|
||||||
|
Math.PI * turnRadiusMeters;
|
||||||
|
var constantCurvatureLengthMeters =
|
||||||
|
originalSemicircleLengthMeters -
|
||||||
|
curvatureTransitionLengthMeters;
|
||||||
|
|
||||||
|
if (constantCurvatureLengthMeters <= 0.0)
|
||||||
|
{
|
||||||
|
throw new ArgumentOutOfRangeException(
|
||||||
|
nameof(curvatureTransitionLengthMeters),
|
||||||
|
"曲率过渡段长度必须小于半径对应的原始半圆弧长。");
|
||||||
|
}
|
||||||
|
|
||||||
|
// 两段平滑过渡的平均曲率均为最大曲率的一半；
|
||||||
|
// 将等曲率段缩短一个过渡长度后，总曲率积分仍严格等于π。
|
||||||
|
var turnLengthMeters =
|
||||||
|
2.0 * curvatureTransitionLengthMeters +
|
||||||
|
constantCurvatureLengthMeters;
|
||||||
|
var turnStartArcLengthMeters =
|
||||||
|
straightLengthMeters;
|
||||||
|
var turnEndArcLengthMeters =
|
||||||
|
straightLengthMeters +
|
||||||
|
turnLengthMeters;
|
||||||
|
var maximumCurvaturePerMeter =
|
||||||
|
1.0 / turnRadiusMeters;
|
||||||
|
|
||||||
|
var sampleArcLengths =
|
||||||
|
BuildCompositeArcLengthSamples(
|
||||||
|
straightLengthMeters,
|
||||||
|
curvatureTransitionLengthMeters,
|
||||||
|
constantCurvatureLengthMeters,
|
||||||
|
pointSpacingMeters);
|
||||||
|
var speedLimits = new double[
|
||||||
|
sampleArcLengths.Count];
|
||||||
|
var referenceSpeeds = new double[
|
||||||
|
sampleArcLengths.Count];
|
||||||
|
|
||||||
|
for (var index = 0;
|
||||||
|
index < sampleArcLengths.Count;
|
||||||
|
index++)
|
||||||
|
{
|
||||||
|
var arcLengthMeters =
|
||||||
|
sampleArcLengths[index];
|
||||||
|
|
||||||
|
// 整个转弯及两侧曲率过渡段采用转弯限速。
|
||||||
|
speedLimits[index] =
|
||||||
|
arcLengthMeters >=
|
||||||
|
turnStartArcLengthMeters &&
|
||||||
|
arcLengthMeters <=
|
||||||
|
turnEndArcLengthMeters
|
||||||
|
? semicircleMaximumSpeedMetersPerSecond
|
||||||
|
: straightMaximumSpeedMetersPerSecond;
|
||||||
|
}
|
||||||
|
|
||||||
|
ApplyAccelerationAndBrakingLimits(
|
||||||
|
sampleArcLengths,
|
||||||
|
speedLimits,
|
||||||
|
referenceSpeeds,
|
||||||
|
accelerationMetersPerSecondSquared,
|
||||||
|
decelerationMetersPerSecondSquared);
|
||||||
|
|
||||||
|
var points = new List<TrajectoryPoint>(
|
||||||
|
sampleArcLengths.Count);
|
||||||
|
var startCos = Math.Cos(
|
||||||
|
startPoseInWorld.YawRadians);
|
||||||
|
var startSin = Math.Sin(
|
||||||
|
startPoseInWorld.YawRadians);
|
||||||
|
var localX = 0.0;
|
||||||
|
var localY = 0.0;
|
||||||
|
var localYawRadians = 0.0;
|
||||||
|
var previousArcLengthMeters = 0.0;
|
||||||
|
|
||||||
|
for (var index = 0;
|
||||||
|
index < sampleArcLengths.Count;
|
||||||
|
index++)
|
||||||
|
{
|
||||||
|
var arcLengthMeters =
|
||||||
|
sampleArcLengths[index];
|
||||||
|
if (index > 0)
|
||||||
|
{
|
||||||
|
var segmentLengthMeters =
|
||||||
|
arcLengthMeters -
|
||||||
|
previousArcLengthMeters;
|
||||||
|
var segmentMiddleArcLengthMeters =
|
||||||
|
(arcLengthMeters +
|
||||||
|
previousArcLengthMeters) /
|
||||||
|
2.0;
|
||||||
|
var segmentCurvaturePerMeter =
|
||||||
|
CalculateSmoothTurnCurvature(
|
||||||
|
segmentMiddleArcLengthMeters -
|
||||||
|
turnStartArcLengthMeters,
|
||||||
|
curvatureTransitionLengthMeters,
|
||||||
|
constantCurvatureLengthMeters,
|
||||||
|
maximumCurvaturePerMeter);
|
||||||
|
var segmentYawChangeRadians =
|
||||||
|
segmentCurvaturePerMeter *
|
||||||
|
segmentLengthMeters;
|
||||||
|
|
||||||
|
if (Math.Abs(segmentCurvaturePerMeter) <=
|
||||||
|
1e-12)
|
||||||
|
{
|
||||||
|
localX +=
|
||||||
|
Math.Cos(localYawRadians) *
|
||||||
|
segmentLengthMeters;
|
||||||
|
localY +=
|
||||||
|
Math.Sin(localYawRadians) *
|
||||||
|
segmentLengthMeters;
|
||||||
|
}
|
||||||
|
else
|
||||||
|
{
|
||||||
|
var nextYawRadians =
|
||||||
|
localYawRadians +
|
||||||
|
segmentYawChangeRadians;
|
||||||
|
localX +=
|
||||||
|
(Math.Sin(nextYawRadians) -
|
||||||
|
Math.Sin(localYawRadians)) /
|
||||||
|
segmentCurvaturePerMeter;
|
||||||
|
localY +=
|
||||||
|
(Math.Cos(localYawRadians) -
|
||||||
|
Math.Cos(nextYawRadians)) /
|
||||||
|
segmentCurvaturePerMeter;
|
||||||
|
}
|
||||||
|
|
||||||
|
localYawRadians +=
|
||||||
|
segmentYawChangeRadians;
|
||||||
|
}
|
||||||
|
|
||||||
|
var curvaturePerMeter =
|
||||||
|
CalculateSmoothTurnCurvature(
|
||||||
|
arcLengthMeters -
|
||||||
|
turnStartArcLengthMeters,
|
||||||
|
curvatureTransitionLengthMeters,
|
||||||
|
constantCurvatureLengthMeters,
|
||||||
|
maximumCurvaturePerMeter);
|
||||||
|
|
||||||
|
var worldX =
|
||||||
|
startPoseInWorld.XMeters +
|
||||||
|
startCos * localX -
|
||||||
|
startSin * localY;
|
||||||
|
var worldY =
|
||||||
|
startPoseInWorld.YMeters +
|
||||||
|
startSin * localX +
|
||||||
|
startCos * localY;
|
||||||
|
var worldYawRadians =
|
||||||
|
AngleMath.NormalizeRadians(
|
||||||
|
startPoseInWorld.YawRadians +
|
||||||
|
localYawRadians);
|
||||||
|
|
||||||
|
points.Add(
|
||||||
|
new TrajectoryPoint(
|
||||||
|
arcLengthMeters,
|
||||||
|
new Pose2D(
|
||||||
|
worldX,
|
||||||
|
worldY,
|
||||||
|
worldYawRadians),
|
||||||
|
curvaturePerMeter,
|
||||||
|
referenceSpeeds[index]));
|
||||||
|
|
||||||
|
previousArcLengthMeters =
|
||||||
|
arcLengthMeters;
|
||||||
|
}
|
||||||
|
|
||||||
|
return new Trajectory2D(points);
|
||||||
|
}
|
||||||
|
|
||||||
|
/// <summary>
|
||||||
|
/// 分别采样直线、入弯过渡、等曲率段和出弯过渡，保证所有边界均为精确轨迹点。
|
||||||
|
/// </summary>
|
||||||
|
private static List<double> BuildCompositeArcLengthSamples(
|
||||||
|
double straightLengthMeters,
|
||||||
|
double curvatureTransitionLengthMeters,
|
||||||
|
double constantCurvatureLengthMeters,
|
||||||
|
double pointSpacingMeters)
|
||||||
|
{
|
||||||
|
var samples = new List<double> { 0.0 };
|
||||||
|
var accumulatedArcLengthMeters = 0.0;
|
||||||
|
|
||||||
|
AppendSectionArcLengthSamples(
|
||||||
|
samples,
|
||||||
|
ref accumulatedArcLengthMeters,
|
||||||
|
straightLengthMeters,
|
||||||
|
pointSpacingMeters);
|
||||||
|
AppendSectionArcLengthSamples(
|
||||||
|
samples,
|
||||||
|
ref accumulatedArcLengthMeters,
|
||||||
|
curvatureTransitionLengthMeters,
|
||||||
|
pointSpacingMeters);
|
||||||
|
AppendSectionArcLengthSamples(
|
||||||
|
samples,
|
||||||
|
ref accumulatedArcLengthMeters,
|
||||||
|
constantCurvatureLengthMeters,
|
||||||
|
pointSpacingMeters);
|
||||||
|
AppendSectionArcLengthSamples(
|
||||||
|
samples,
|
||||||
|
ref accumulatedArcLengthMeters,
|
||||||
|
curvatureTransitionLengthMeters,
|
||||||
|
pointSpacingMeters);
|
||||||
|
AppendSectionArcLengthSamples(
|
||||||
|
samples,
|
||||||
|
ref accumulatedArcLengthMeters,
|
||||||
|
straightLengthMeters,
|
||||||
|
pointSpacingMeters);
|
||||||
|
|
||||||
|
return samples;
|
||||||
|
}
|
||||||
|
|
||||||
|
/// <summary>
|
||||||
|
/// 计算180°左转中连续变化的参考曲率，过渡段两端的曲率变化率均为零。
|
||||||
|
/// </summary>
|
||||||
|
private static double CalculateSmoothTurnCurvature(
|
||||||
|
double distanceInTurnMeters,
|
||||||
|
double transitionLengthMeters,
|
||||||
|
double constantCurvatureLengthMeters,
|
||||||
|
double maximumCurvaturePerMeter)
|
||||||
|
{
|
||||||
|
var totalTurnLengthMeters =
|
||||||
|
2.0 * transitionLengthMeters +
|
||||||
|
constantCurvatureLengthMeters;
|
||||||
|
|
||||||
|
if (distanceInTurnMeters <= 0.0 ||
|
||||||
|
distanceInTurnMeters >= totalTurnLengthMeters)
|
||||||
|
{
|
||||||
|
return 0.0;
|
||||||
|
}
|
||||||
|
|
||||||
|
if (distanceInTurnMeters < transitionLengthMeters)
|
||||||
|
{
|
||||||
|
return maximumCurvaturePerMeter *
|
||||||
|
SmoothStep01(
|
||||||
|
distanceInTurnMeters /
|
||||||
|
transitionLengthMeters);
|
||||||
|
}
|
||||||
|
|
||||||
|
var exitTransitionStartMeters =
|
||||||
|
transitionLengthMeters +
|
||||||
|
constantCurvatureLengthMeters;
|
||||||
|
if (distanceInTurnMeters <=
|
||||||
|
exitTransitionStartMeters)
|
||||||
|
{
|
||||||
|
return maximumCurvaturePerMeter;
|
||||||
|
}
|
||||||
|
|
||||||
|
var exitRatio =
|
||||||
|
(distanceInTurnMeters -
|
||||||
|
exitTransitionStartMeters) /
|
||||||
|
transitionLengthMeters;
|
||||||
|
return maximumCurvaturePerMeter *
|
||||||
|
(1.0 - SmoothStep01(exitRatio));
|
||||||
|
}
|
||||||
|
|
||||||
|
/// <summary>
|
||||||
|
/// 将零到一的比例转换为两端一阶导数均为零的三次平滑比例。
|
||||||
|
/// </summary>
|
||||||
|
private static double SmoothStep01(double ratio)
|
||||||
|
{
|
||||||
|
var limitedRatio = Math.Max(
|
||||||
|
0.0,
|
||||||
|
Math.Min(1.0, ratio));
|
||||||
|
return limitedRatio *
|
||||||
|
limitedRatio *
|
||||||
|
(3.0 - 2.0 * limitedRatio);
|
||||||
|
}
|
||||||
|
|
||||||
|
/// <summary>
|
||||||
|
/// 将一段指定长度的轨迹追加为均匀弧长采样，并使最后一个点严格落在该段终点。
|
||||||
|
/// </summary>
|
||||||
|
private static void AppendSectionArcLengthSamples(
|
||||||
|
ICollection<double> samples,
|
||||||
|
ref double accumulatedArcLengthMeters,
|
||||||
|
double sectionLengthMeters,
|
||||||
|
double pointSpacingMeters)
|
||||||
|
{
|
||||||
|
var sectionStartArcLengthMeters =
|
||||||
|
accumulatedArcLengthMeters;
|
||||||
|
var segmentCount = (int)Math.Ceiling(
|
||||||
|
sectionLengthMeters /
|
||||||
|
pointSpacingMeters);
|
||||||
|
|
||||||
|
for (var index = 1;
|
||||||
|
index <= segmentCount;
|
||||||
|
index++)
|
||||||
|
{
|
||||||
|
samples.Add(
|
||||||
|
sectionStartArcLengthMeters +
|
||||||
|
sectionLengthMeters *
|
||||||
|
index /
|
||||||
|
segmentCount);
|
||||||
|
}
|
||||||
|
|
||||||
|
accumulatedArcLengthMeters =
|
||||||
|
sectionStartArcLengthMeters +
|
||||||
|
sectionLengthMeters;
|
||||||
|
}
|
||||||
|
|
||||||
|
/// <summary>
|
||||||
|
/// 对逐点速度上限执行前向加速约束和反向制动约束，生成连续可执行的空间速度曲线。
|
||||||
|
/// </summary>
|
||||||
|
private static void ApplyAccelerationAndBrakingLimits(
|
||||||
|
IReadOnlyList<double> arcLengthsMeters,
|
||||||
|
IReadOnlyList<double> speedLimitsMetersPerSecond,
|
||||||
|
double[] referenceSpeedsMetersPerSecond,
|
||||||
|
double accelerationMetersPerSecondSquared,
|
||||||
|
double decelerationMetersPerSecondSquared)
|
||||||
|
{
|
||||||
|
referenceSpeedsMetersPerSecond[0] = 0.0;
|
||||||
|
|
||||||
|
for (var index = 1;
|
||||||
|
index < arcLengthsMeters.Count;
|
||||||
|
index++)
|
||||||
|
{
|
||||||
|
var segmentLengthMeters =
|
||||||
|
arcLengthsMeters[index] -
|
||||||
|
arcLengthsMeters[index - 1];
|
||||||
|
var accelerationLimitedSpeed = Math.Sqrt(
|
||||||
|
referenceSpeedsMetersPerSecond[index - 1] *
|
||||||
|
referenceSpeedsMetersPerSecond[index - 1] +
|
||||||
|
2.0 *
|
||||||
|
accelerationMetersPerSecondSquared *
|
||||||
|
segmentLengthMeters);
|
||||||
|
|
||||||
|
referenceSpeedsMetersPerSecond[index] =
|
||||||
|
Math.Min(
|
||||||
|
speedLimitsMetersPerSecond[index],
|
||||||
|
accelerationLimitedSpeed);
|
||||||
|
}
|
||||||
|
|
||||||
|
var finalIndex =
|
||||||
|
referenceSpeedsMetersPerSecond.Length - 1;
|
||||||
|
referenceSpeedsMetersPerSecond[finalIndex] = 0.0;
|
||||||
|
|
||||||
|
for (var index = finalIndex - 1;
|
||||||
|
index >= 0;
|
||||||
|
index--)
|
||||||
|
{
|
||||||
|
var segmentLengthMeters =
|
||||||
|
arcLengthsMeters[index + 1] -
|
||||||
|
arcLengthsMeters[index];
|
||||||
|
var brakingLimitedSpeed = Math.Sqrt(
|
||||||
|
referenceSpeedsMetersPerSecond[index + 1] *
|
||||||
|
referenceSpeedsMetersPerSecond[index + 1] +
|
||||||
|
2.0 *
|
||||||
|
decelerationMetersPerSecondSquared *
|
||||||
|
segmentLengthMeters);
|
||||||
|
|
||||||
|
referenceSpeedsMetersPerSecond[index] =
|
||||||
|
Math.Min(
|
||||||
|
referenceSpeedsMetersPerSecond[index],
|
||||||
|
brakingLimitedSpeed);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
/// <summary>
|
/// <summary>
|
||||||
/// 根据起步、巡航和制动能力计算指定弧长位置允许的参考速度。
|
/// 根据起步、巡航和制动能力计算指定弧长位置允许的参考速度。
|
||||||
/// </summary>
|
/// </summary>
|
||||||
|
|||||||
@@ -38,7 +38,7 @@ namespace MultiWheelC
|
|||||||
/// <summary>
|
/// <summary>
|
||||||
/// Stanley横向误差增益，单位为1/s。
|
/// Stanley横向误差增益，单位为1/s。
|
||||||
/// </summary>
|
/// </summary>
|
||||||
public double StanleyCrossTrackGainPerSecond = 1.0;
|
public double StanleyCrossTrackGainPerSecond = 0.4;
|
||||||
|
|
||||||
/// <summary>
|
/// <summary>
|
||||||
/// Stanley航向误差增益。
|
/// Stanley航向误差增益。
|
||||||
@@ -48,7 +48,7 @@ namespace MultiWheelC
|
|||||||
/// <summary>
|
/// <summary>
|
||||||
/// Stanley低速分母保护速度，单位为m/s。
|
/// Stanley低速分母保护速度，单位为m/s。
|
||||||
/// </summary>
|
/// </summary>
|
||||||
public double StanleyMinimumSpeedMetersPerSecond = 0.05;
|
public double StanleyMinimumSpeedMetersPerSecond = 0.15;
|
||||||
|
|
||||||
/// <summary>
|
/// <summary>
|
||||||
/// 获取或设置Stanley是否优先使用Detour估算的实际速度。
|
/// 获取或设置Stanley是否优先使用Detour估算的实际速度。
|
||||||
|
|||||||
@@ -33,10 +33,10 @@ public class PilotDefinition : MultiWheelPilotDefinition<PilotConfig, PilotDefin
|
|||||||
[AsLowerIO(desc = "右后左轮实际位置")] public float RRLActualPos;
|
[AsLowerIO(desc = "右后左轮实际位置")] public float RRLActualPos;
|
||||||
[AsLowerIO(desc = "右后右轮实际位置")] public float RRRActualPos;
|
[AsLowerIO(desc = "右后右轮实际位置")] public float RRRActualPos;
|
||||||
|
|
||||||
[AsLowerIO(desc = "左夹臂低限位")] public float LeftArmLowerPos;
|
[AsLowerIO(desc = "左夹臂低限位")] public int LeftArmLowerPos;
|
||||||
[AsLowerIO(desc = "左夹臂高限位")] public float LeftArmUpperPos;
|
[AsLowerIO(desc = "左夹臂高限位")] public int LeftArmUpperPos;
|
||||||
[AsLowerIO(desc = "右夹臂低限位")] public float RightArmLowerPos;
|
[AsLowerIO(desc = "右夹臂低限位")] public int RightArmLowerPos;
|
||||||
[AsLowerIO(desc = "右夹臂高限位")] public float RightArmUpperPos;
|
[AsLowerIO(desc = "右夹臂高限位")] public int RightArmUpperPos;
|
||||||
|
|
||||||
[AsUpperIO(desc = "从C往驱动器下使能")] public bool DisableFromC = false;
|
[AsUpperIO(desc = "从C往驱动器下使能")] public bool DisableFromC = false;
|
||||||
[AsUpperIO(desc = "从C上复位")] public bool ResetFromC = false;
|
[AsUpperIO(desc = "从C上复位")] public bool ResetFromC = false;
|
||||||
|
|||||||
+20
@@ -35,3 +35,23 @@ StanleyLateralController.cs
|
|||||||
PidLongitudinalController.cs
|
PidLongitudinalController.cs
|
||||||
GcpCommandExecutor.cs
|
GcpCommandExecutor.cs
|
||||||
ParkingGeometricController.cs
|
ParkingGeometricController.cs
|
||||||
|
|
||||||
|
|
||||||
|
轨迹投影没有进度连续性
|
||||||
|
[TrajectoryProjector.cs (line 33)](/D:/Users/Desktop/入职培训/停车机器人/MyParking/MultiWheelC/Trajectory/TrajectoryProjector.cs:33) 每个控制周期都会遍历整条轨迹并选择全局最近线段。
|
||||||
|
当前4 m直线、圆弧、普通 S 曲线一般没有问题；但以后遇到自交、回环、相邻平行路径或 Detour 跳变时，投影可能突然跳到另一段轨迹。
|
||||||
|
建议在开始复杂曲线测试前增加：
|
||||||
|
上一次投影线段索引
|
||||||
|
有限前向搜索窗口
|
||||||
|
少量允许回退范围
|
||||||
|
现阶段测试直线不需要马上改。
|
||||||
|
|
||||||
|
通用轨迹动作没有起点航向保护
|
||||||
|
当前直线测试以实时 Detour 位姿作为起点，因此起点位置和航向天然匹配，没有问题。
|
||||||
|
但 [TrajectoryTrackingMovement.cs (line 119)](/D:/Users/Desktop/入职培训/停车机器人/MyParking/MultiWheelC/Movements/TrajectoryTrackingMovement.cs:119) 本身允许传入任意世界坐标轨迹。如果将来传入的轨迹起点航向和车辆实际航向差别很大，车辆会直接边走边纠正。
|
||||||
|
后续至少应选择一种：
|
||||||
|
规划层保证起点位姿和实际车辆一致；
|
||||||
|
控制器检查初始航向误差，超限则拒绝启动；
|
||||||
|
增加起步航向对齐状态。
|
||||||
|
第一阶段建议采用第二种，简单、安全。
|
||||||
|
|
||||||
|
|||||||
Reference in New Issue
Block a user