180 lines
9.1 KiB
C#
180 lines
9.1 KiB
C#
using System;
|
|
using System.Collections.Generic;
|
|
using System.Globalization;
|
|
using MultiWheelC.TrajectoryPlanning.CoarsePath;
|
|
using MultiWheelC.TrajectoryPlanning.EMPlanner;
|
|
using TrajectoryPlanningVisualization;
|
|
|
|
namespace MultiWheelC.TrajectoryPlanning.TrajectoryObservation;
|
|
|
|
public sealed class TrajectoryObservationKinematicChartBuilder
|
|
{
|
|
public IReadOnlyList<VisualizationChart> Build(EmTrajectory trajectory, DirectionSegmentView segment,
|
|
VehicleParameters vehicle, EmPlannerConfiguration configuration)
|
|
{
|
|
if (trajectory == null) throw new ArgumentNullException(nameof(trajectory));
|
|
if (segment == null) throw new ArgumentNullException(nameof(segment));
|
|
if (vehicle == null) throw new ArgumentNullException(nameof(vehicle));
|
|
if (configuration == null) throw new ArgumentNullException(nameof(configuration));
|
|
|
|
LsBuildResult ls = BuildLs(trajectory, segment, configuration.Frenet.MaximumProjectionDistanceMeters);
|
|
double curvatureLimit = CurvatureLimit(vehicle);
|
|
double terminalTime = trajectory.Points.Count == 0 ? 0d : trajectory.Points[trajectory.Points.Count - 1].TimeFromStart;
|
|
return new[]
|
|
{
|
|
Chart("ls", "横向偏移", "ReferenceS (m)", "l (m)", ls.Points,
|
|
"完整活动方向段投影；投影失败=" + ls.ProjectionFailureCount.ToString(CultureInfo.InvariantCulture),
|
|
new[] { new VisualizationChartAnnotation("ls-s-end", "s-end", "s_end",
|
|
segment.SourceStartArcLength + segment.LengthMeters) }),
|
|
Chart("st", "时空轨迹", "t (s)", "PathS (m)", Points(trajectory, p => p.PathS), "",
|
|
new[] { new VisualizationChartAnnotation("st-s-end", "s-end", "s_end", terminalTime) }),
|
|
WithSymmetricLimit("curvature-s", "曲率-距离", "s (m)", "κ (m⁻¹)",
|
|
PointsBySegmentS(trajectory), curvatureLimit),
|
|
WithSymmetricLimit("curvature-t", "曲率-时间", "t (s)", "κ (m⁻¹)",
|
|
Points(trajectory, p => p.VehicleCurvature), curvatureLimit),
|
|
Velocity(trajectory, configuration),
|
|
Acceleration(trajectory, configuration),
|
|
Jerk(trajectory, configuration),
|
|
Chart("yaw-rate-t", "横摆角速度", "t (s)", "ω (rad/s)",
|
|
Points(trajectory, p => p.YawRate), "生效配置没有独立横摆角速度上限"),
|
|
};
|
|
}
|
|
|
|
public int CountProjectionFailures(EmTrajectory trajectory, DirectionSegmentView segment,
|
|
EmPlannerConfiguration configuration)
|
|
{
|
|
if (trajectory == null) throw new ArgumentNullException(nameof(trajectory));
|
|
if (segment == null) throw new ArgumentNullException(nameof(segment));
|
|
if (configuration == null) throw new ArgumentNullException(nameof(configuration));
|
|
return BuildLs(trajectory, segment, configuration.Frenet.MaximumProjectionDistanceMeters).ProjectionFailureCount;
|
|
}
|
|
|
|
private static VisualizationChart Velocity(EmTrajectory trajectory, EmPlannerConfiguration configuration)
|
|
{
|
|
IReadOnlyList<VisualizationPoint> points = Points(trajectory, p => p.SignedLongitudinalVelocity);
|
|
return new VisualizationChart("velocity-t", "速度", "t (s)", "v (m/s)", new[]
|
|
{
|
|
new VisualizationSeries("velocity-t-current", "当前轨迹", VisualizationLineStyle.Solid, points),
|
|
Limit("velocity-t-forward-limit", "前进生效限制", points, configuration.Longitudinal.MaximumForwardSpeedMetersPerSecond),
|
|
Limit("velocity-t-reverse-limit", "倒车生效限制", points, -configuration.Longitudinal.MaximumReverseSpeedMetersPerSecond),
|
|
});
|
|
}
|
|
|
|
private static VisualizationChart Acceleration(EmTrajectory trajectory, EmPlannerConfiguration configuration)
|
|
{
|
|
IReadOnlyList<VisualizationPoint> points = Points(trajectory, p => p.LongitudinalAcceleration);
|
|
return new VisualizationChart("acceleration-t", "加速度", "t (s)", "a (m/s²)", new[]
|
|
{
|
|
new VisualizationSeries("acceleration-t-current", "当前轨迹", VisualizationLineStyle.Solid, points),
|
|
Limit("acceleration-t-positive-limit", "加速生效限制", points, configuration.Longitudinal.MaximumAccelerationMetersPerSecondSquared),
|
|
Limit("acceleration-t-negative-limit", "减速生效限制", points, -configuration.Longitudinal.MaximumDecelerationMetersPerSecondSquared),
|
|
});
|
|
}
|
|
|
|
private static VisualizationChart Jerk(EmTrajectory trajectory, EmPlannerConfiguration configuration)
|
|
{
|
|
var intervals = new List<VisualizationPoint>();
|
|
for (int index = 0; index + 1 < trajectory.Points.Count; index++)
|
|
intervals.Add(new VisualizationPoint(trajectory.Points[index].TimeFromStart,
|
|
trajectory.Points[index].LongitudinalJerk));
|
|
return new VisualizationChart("jerk-t", "加加速度", "t (s)", "j (m/s³)", new[]
|
|
{
|
|
new VisualizationSeries("jerk-t-current", "当前轨迹", VisualizationLineStyle.Solid, intervals),
|
|
Limit("jerk-t-positive-limit", "正向生效限制", intervals, configuration.Longitudinal.MaximumJerkMetersPerSecondCubed),
|
|
Limit("jerk-t-negative-limit", "负向生效限制", intervals, -configuration.Longitudinal.MaximumJerkMetersPerSecondCubed),
|
|
}, "j[i] 表示 [tᵢ, tᵢ₊₁)；末点后无时间区间");
|
|
}
|
|
|
|
private static VisualizationChart WithSymmetricLimit(string id, string title, string xAxis, string yAxis,
|
|
IReadOnlyList<VisualizationPoint> points, double limit)
|
|
{
|
|
return new VisualizationChart(id, title, xAxis, yAxis, new[]
|
|
{
|
|
new VisualizationSeries(id + "-current", "当前轨迹", VisualizationLineStyle.Solid, points),
|
|
Limit(id + "-positive-limit", "生效限制", points, limit),
|
|
Limit(id + "-negative-limit", "生效限制", points, -limit),
|
|
});
|
|
}
|
|
|
|
private static VisualizationSeries Limit(string id, string legend, IReadOnlyList<VisualizationPoint> points, double y)
|
|
{
|
|
double end = points.Count == 0 ? 0d : points[points.Count - 1].X;
|
|
return new VisualizationSeries(id, legend, VisualizationLineStyle.Limit,
|
|
new[] { new VisualizationPoint(0d, y), new VisualizationPoint(end, y) });
|
|
}
|
|
|
|
private static VisualizationChart Chart(string id, string title, string xAxis, string yAxis,
|
|
IReadOnlyList<VisualizationPoint> points, string note,
|
|
IReadOnlyList<VisualizationChartAnnotation> annotations = null)
|
|
{
|
|
return new VisualizationChart(id, title, xAxis, yAxis,
|
|
new[] { new VisualizationSeries(id + "-current", "当前轨迹", VisualizationLineStyle.Solid, points) },
|
|
note, annotations ?? Array.Empty<VisualizationChartAnnotation>());
|
|
}
|
|
|
|
private static IReadOnlyList<VisualizationPoint> Points(EmTrajectory trajectory, Func<EmTrajectoryPoint, double> y)
|
|
{
|
|
var points = new List<VisualizationPoint>();
|
|
foreach (EmTrajectoryPoint point in trajectory.Points)
|
|
points.Add(new VisualizationPoint(point.TimeFromStart, y(point)));
|
|
return points;
|
|
}
|
|
|
|
private static IReadOnlyList<VisualizationPoint> PointsBySegmentS(EmTrajectory trajectory)
|
|
{
|
|
var points = new List<VisualizationPoint>();
|
|
foreach (EmTrajectoryPoint point in trajectory.Points)
|
|
points.Add(new VisualizationPoint(point.SegmentLocalS, point.VehicleCurvature));
|
|
return points;
|
|
}
|
|
|
|
private static double CurvatureLimit(VehicleParameters vehicle)
|
|
{
|
|
if (vehicle.MaximumCurvaturePerMeter.HasValue)
|
|
{
|
|
return Math.Abs(vehicle.MaximumCurvaturePerMeter.Value);
|
|
}
|
|
|
|
if (vehicle.MinimumTurningRadiusMeters.HasValue && vehicle.MinimumTurningRadiusMeters.Value > 0d)
|
|
{
|
|
return 1d / vehicle.MinimumTurningRadiusMeters.Value;
|
|
}
|
|
|
|
throw new ArgumentException("Vehicle curvature limit is required.", nameof(vehicle));
|
|
}
|
|
|
|
private static LsBuildResult BuildLs(EmTrajectory trajectory, DirectionSegmentView segment,
|
|
double maximumProjectionDistanceMeters)
|
|
{
|
|
var points = new List<VisualizationPoint>();
|
|
var projector = new FrenetProjector();
|
|
double seed = 0d;
|
|
int failures = 0;
|
|
foreach (EmTrajectoryPoint point in trajectory.Points)
|
|
{
|
|
if (!projector.TryProject(new Pose2D(point.X, point.Y, point.Yaw), segment, 0d, segment.LengthMeters,
|
|
maximumProjectionDistanceMeters, seed, out FrenetProjection projection))
|
|
{
|
|
failures++;
|
|
continue;
|
|
}
|
|
points.Add(new VisualizationPoint(segment.SourceStartArcLength + projection.ReferenceS,
|
|
projection.LateralOffset));
|
|
seed = projection.ReferenceS;
|
|
}
|
|
return new LsBuildResult(points, failures);
|
|
}
|
|
|
|
private sealed class LsBuildResult
|
|
{
|
|
public LsBuildResult(IReadOnlyList<VisualizationPoint> points, int projectionFailureCount)
|
|
{
|
|
Points = points;
|
|
ProjectionFailureCount = projectionFailureCount;
|
|
}
|
|
|
|
public IReadOnlyList<VisualizationPoint> Points { get; }
|
|
public int ProjectionFailureCount { get; }
|
|
}
|
|
}
|