添加直线-半圆组合轨迹测试并调优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));
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
@@ -92,6 +92,399 @@ namespace MultiWheelC
|
||||
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>
|
||||
|
||||
@@ -38,7 +38,7 @@ namespace MultiWheelC
|
||||
/// <summary>
|
||||
/// Stanley横向误差增益，单位为1/s。
|
||||
/// </summary>
|
||||
public double StanleyCrossTrackGainPerSecond = 1.0;
|
||||
public double StanleyCrossTrackGainPerSecond = 0.4;
|
||||
|
||||
/// <summary>
|
||||
/// Stanley航向误差增益。
|
||||
@@ -48,7 +48,7 @@ namespace MultiWheelC
|
||||
/// <summary>
|
||||
/// Stanley低速分母保护速度，单位为m/s。
|
||||
/// </summary>
|
||||
public double StanleyMinimumSpeedMetersPerSecond = 0.05;
|
||||
public double StanleyMinimumSpeedMetersPerSecond = 0.15;
|
||||
|
||||
/// <summary>
|
||||
/// 获取或设置Stanley是否优先使用Detour估算的实际速度。
|
||||
|
||||
@@ -33,10 +33,10 @@ public class PilotDefinition : MultiWheelPilotDefinition<PilotConfig, PilotDefin
|
||||
[AsLowerIO(desc = "右后左轮实际位置")] public float RRLActualPos;
|
||||
[AsLowerIO(desc = "右后右轮实际位置")] public float RRRActualPos;
|
||||
|
||||
[AsLowerIO(desc = "左夹臂低限位")] public float LeftArmLowerPos;
|
||||
[AsLowerIO(desc = "左夹臂高限位")] public float LeftArmUpperPos;
|
||||
[AsLowerIO(desc = "右夹臂低限位")] public float RightArmLowerPos;
|
||||
[AsLowerIO(desc = "右夹臂高限位")] public float RightArmUpperPos;
|
||||
[AsLowerIO(desc = "左夹臂低限位")] public int LeftArmLowerPos;
|
||||
[AsLowerIO(desc = "左夹臂高限位")] public int LeftArmUpperPos;
|
||||
[AsLowerIO(desc = "右夹臂低限位")] public int RightArmLowerPos;
|
||||
[AsLowerIO(desc = "右夹臂高限位")] public int RightArmUpperPos;
|
||||
|
||||
[AsUpperIO(desc = "从C往驱动器下使能")] public bool DisableFromC = false;
|
||||
[AsUpperIO(desc = "从C上复位")] public bool ResetFromC = false;
|
||||
|
||||
Reference in New Issue
Block a user