yunxiao.zhu
d120ef2a39
* 将 speedRatio 前移到规划/准备阶段，运行时只消费已生成的 8ms 队列 * 区分旧格式规划导出与 ActualSend 实发诊断工件 * 补充普通轨迹、MoveJoint、飞拍队列和严格限幅回归测试
353 lines
16 KiB
C#
353 lines
16 KiB
C#
using Flyshot.Core.Planning;
|
||
|
||
namespace Flyshot.Core.Planning.Sampling;
|
||
|
||
/// <summary>
|
||
/// 负责把规划层关节轨迹重采样为 J519 物理发送周期上的角度制目标。
|
||
/// <para>
|
||
/// 算法约定：
|
||
/// 输入的稠密关节轨迹行格式固定为 [time, j1..jN]，time 为规划轨迹时间，关节单位为弧度；
|
||
/// 输出的 J519 采样点按物理伺服周期排列，关节单位转换为角度制，供 UDP 60015 实时下发和离线 ActualSend 文件共用。
|
||
/// </para>
|
||
/// <para>
|
||
/// 采样点数先按轨迹时间步长 trajectoryStep = servoPeriod * speedRatio 计算：
|
||
/// sampleCount = ceil(max(0, duration / trajectoryStep - 1e-9)) + 1。
|
||
/// 末尾额外保留一个终点钳制周期，确保轨迹时长不是周期整数倍时仍会输出最终点。
|
||
/// </para>
|
||
/// <para>
|
||
/// 飞拍最终发送队列优先从 <see cref="PlannedTrajectory"/> 重建连续三次样条并直接取点；
|
||
/// 旧稠密点入口仅保留给运行时 fallback 和兼容导出。
|
||
/// 第 k 个采样点的物理发送时间为 sendTime = k * servoPeriod；
|
||
/// speedRatio 不改变物理发送周期，只用于把发送时间映射回规划轨迹时间：
|
||
/// trajectoryTime = min(sendTime * speedRatio, duration)。
|
||
/// 之后在连续样条或旧稠密轨迹上求取关节，并把每个关节从 rad 转为 deg。
|
||
/// </para>
|
||
/// <para>
|
||
/// 诊断行也在这里统一生成：Timing 行格式为 sample_index + send_time + trajectory_time + speed_ratio；
|
||
/// Jerk 行使用相邻发送点上的角度制关节目标做后向差分，依次近似速度、加速度和跃度，格式为
|
||
/// start_time + end_time + dt + max_abs_jerk + jerk[j1..jN]。
|
||
/// </para>
|
||
/// </summary>
|
||
public static class J519SendTrajectorySampler
|
||
{
|
||
/// <summary>
|
||
/// 根据连续规划样条和 J519 伺服周期生成完整实发采样序列。
|
||
/// </summary>
|
||
/// <param name="trajectory">规划阶段产出的连续轨迹信息。</param>
|
||
/// <param name="servoPeriodSeconds">J519 物理发送周期，单位为秒。</param>
|
||
/// <param name="speedRatio">速度倍率；只缩放轨迹采样时间，不改变物理发送周期。</param>
|
||
/// <returns>按 J519 发送周期排列的角度制采样序列。</returns>
|
||
public static IReadOnlyList<J519SendSample> SamplePlannedTrajectory(
|
||
PlannedTrajectory trajectory,
|
||
double servoPeriodSeconds,
|
||
double speedRatio)
|
||
{
|
||
ArgumentNullException.ThrowIfNull(trajectory);
|
||
ValidatePlannedTrajectoryInputs(trajectory, servoPeriodSeconds, speedRatio);
|
||
|
||
var durationSeconds = trajectory.WaypointTimes[^1];
|
||
var trajectoryStepSeconds = servoPeriodSeconds * speedRatio;
|
||
var sampleCount = CalculateSampleCount(durationSeconds, trajectoryStepSeconds);
|
||
var spline = RebuildSpline(trajectory);
|
||
var samples = new List<J519SendSample>((int)Math.Min(sampleCount, int.MaxValue));
|
||
|
||
for (long sampleIndex = 0; sampleIndex < sampleCount; sampleIndex++)
|
||
{
|
||
// 直接在连续样条上按 trajectoryTime 取点，避免先离散再线性回采样引入局部跃度尖峰。
|
||
var sendTime = sampleIndex * servoPeriodSeconds;
|
||
var trajectoryTime = Math.Min(sendTime * speedRatio, durationSeconds);
|
||
var joints = spline.Evaluate(trajectoryTime).Select(RadiansToDegrees).ToArray();
|
||
samples.Add(new J519SendSample(sampleIndex, sendTime, trajectoryTime, speedRatio, joints));
|
||
}
|
||
|
||
return samples;
|
||
}
|
||
|
||
/// <summary>
|
||
/// 根据 J519 伺服周期和 speed_ratio 生成完整实发采样序列。
|
||
/// </summary>
|
||
/// <param name="denseJointTrajectory">规划层稠密关节轨迹，每行格式为 [time, j1..jN]，关节单位为弧度。</param>
|
||
/// <param name="durationSeconds">规划轨迹总时长，单位为秒。</param>
|
||
/// <param name="servoPeriodSeconds">J519 物理发送周期，单位为秒。</param>
|
||
/// <param name="speedRatio">速度倍率；只缩放轨迹采样时间，不改变物理发送周期。</param>
|
||
/// <returns>按 J519 发送周期排列的角度制采样序列。</returns>
|
||
public static IReadOnlyList<J519SendSample> SampleDenseJointTrajectory(
|
||
IReadOnlyList<IReadOnlyList<double>> denseJointTrajectory,
|
||
double durationSeconds,
|
||
double servoPeriodSeconds,
|
||
double speedRatio)
|
||
{
|
||
ArgumentNullException.ThrowIfNull(denseJointTrajectory);
|
||
ValidateInputs(denseJointTrajectory, durationSeconds, servoPeriodSeconds, speedRatio);
|
||
|
||
var trajectoryStepSeconds = servoPeriodSeconds * speedRatio;
|
||
var sampleCount = CalculateSampleCount(durationSeconds, trajectoryStepSeconds);
|
||
var samples = new List<J519SendSample>((int)Math.Min(sampleCount, int.MaxValue));
|
||
var segmentIndex = 0;
|
||
|
||
for (long sampleIndex = 0; sampleIndex < sampleCount; sampleIndex++)
|
||
{
|
||
// J519 物理周期固定，speed_ratio 只用于把发送时间映射回原始轨迹时间。
|
||
var sendTime = sampleIndex * servoPeriodSeconds;
|
||
var trajectoryTime = Math.Min(sendTime * speedRatio, durationSeconds);
|
||
var joints = SampleDenseJointTrajectoryDegrees(denseJointTrajectory, trajectoryTime, ref segmentIndex);
|
||
samples.Add(new J519SendSample(sampleIndex, sendTime, trajectoryTime, speedRatio, joints));
|
||
}
|
||
|
||
return samples;
|
||
}
|
||
|
||
/// <summary>
|
||
/// 按原始轨迹时长和 speed_ratio 后的轨迹时间步长计算 J519 实发采样数。
|
||
/// </summary>
|
||
/// <param name="durationSeconds">规划轨迹总时长，单位为秒。</param>
|
||
/// <param name="trajectoryStepSeconds">每个物理发送周期对应的轨迹时间步长，单位为秒。</param>
|
||
/// <returns>包含终点钳制周期的采样点数量。</returns>
|
||
public static long CalculateSampleCount(double durationSeconds, double trajectoryStepSeconds)
|
||
{
|
||
if (durationSeconds < 0.0)
|
||
{
|
||
throw new ArgumentOutOfRangeException(nameof(durationSeconds), "轨迹时长不能为负数。");
|
||
}
|
||
|
||
if (trajectoryStepSeconds <= 0.0 || double.IsNaN(trajectoryStepSeconds) || double.IsInfinity(trajectoryStepSeconds))
|
||
{
|
||
throw new ArgumentOutOfRangeException(nameof(trajectoryStepSeconds), "轨迹采样步长必须是有限正数。");
|
||
}
|
||
|
||
// 非周期整数倍时多保留一个终点钳制周期，和真实 J519 下发序列保持一致。
|
||
return (long)Math.Ceiling(Math.Max(0.0, (durationSeconds / trajectoryStepSeconds) - 1e-9)) + 1;
|
||
}
|
||
|
||
/// <summary>
|
||
/// 构造实发时间映射文本行，格式为 sample_index + send_time + trajectory_time + final_speed_ratio。
|
||
/// </summary>
|
||
/// <param name="sample">待写出的 J519 实发采样点。</param>
|
||
/// <returns>与 ActualSendTiming.txt 兼容的数值行。</returns>
|
||
public static IReadOnlyList<double> BuildTimingRow(J519SendSample sample)
|
||
{
|
||
ArgumentNullException.ThrowIfNull(sample);
|
||
return
|
||
[
|
||
sample.SampleIndex,
|
||
Math.Round(sample.SendTime, 6),
|
||
Math.Round(sample.TrajectoryTime, 6),
|
||
Math.Round(sample.SpeedRatio, 6)
|
||
];
|
||
}
|
||
|
||
/// <summary>
|
||
/// 构造带倍率诊断信息的实发时间映射文本行。
|
||
/// </summary>
|
||
/// <param name="sample">待写出的 J519 实发采样点。</param>
|
||
/// <param name="requestedSpeedRatio">用户请求的执行倍率。</param>
|
||
/// <param name="stretchIterationCount">历史诊断字段；当前执行侧不自动改写 speed_ratio，因此正常为 0。</param>
|
||
/// <returns>前 4 列兼容 ActualSendTiming.txt，后续列记录请求倍率和倍率改写次数。</returns>
|
||
public static IReadOnlyList<double> BuildTimingRow(
|
||
J519SendSample sample,
|
||
double requestedSpeedRatio,
|
||
int stretchIterationCount)
|
||
{
|
||
if (requestedSpeedRatio <= 0.0 || double.IsNaN(requestedSpeedRatio) || double.IsInfinity(requestedSpeedRatio))
|
||
{
|
||
throw new ArgumentOutOfRangeException(nameof(requestedSpeedRatio), "请求 speed_ratio 必须是有限正数。");
|
||
}
|
||
|
||
if (stretchIterationCount < 0)
|
||
{
|
||
throw new ArgumentOutOfRangeException(nameof(stretchIterationCount), "倍率改写次数不能为负数。");
|
||
}
|
||
|
||
var legacyRow = BuildTimingRow(sample);
|
||
return
|
||
[
|
||
legacyRow[0],
|
||
legacyRow[1],
|
||
legacyRow[2],
|
||
legacyRow[3],
|
||
Math.Round(requestedSpeedRatio, 6),
|
||
stretchIterationCount
|
||
];
|
||
}
|
||
|
||
/// <summary>
|
||
/// 构造相邻发送点之间的角度制跃度统计行。
|
||
/// </summary>
|
||
/// <param name="previousTime">上一帧发送时间，单位为秒。</param>
|
||
/// <param name="currentTime">当前帧发送时间，单位为秒。</param>
|
||
/// <param name="previousJoints">上一帧角度制关节目标。</param>
|
||
/// <param name="currentJoints">当前帧角度制关节目标。</param>
|
||
/// <param name="previousVelocity">上一帧关节速度，调用后更新为当前帧速度。</param>
|
||
/// <param name="previousAcceleration">上一帧关节加速度，调用后更新为当前帧加速度。</param>
|
||
/// <returns>与 ActualSendJerkStats.txt 兼容的数值行。</returns>
|
||
public static IReadOnlyList<double> BuildJerkRow(
|
||
double previousTime,
|
||
double currentTime,
|
||
IReadOnlyList<double> previousJoints,
|
||
IReadOnlyList<double> currentJoints,
|
||
ref double[]? previousVelocity,
|
||
ref double[]? previousAcceleration)
|
||
{
|
||
ArgumentNullException.ThrowIfNull(previousJoints);
|
||
ArgumentNullException.ThrowIfNull(currentJoints);
|
||
|
||
var dt = currentTime - previousTime;
|
||
if (dt <= 0.0)
|
||
{
|
||
dt = 1e-9;
|
||
}
|
||
|
||
var jointCount = currentJoints.Count;
|
||
var currentVelocity = new double[jointCount];
|
||
var currentAcceleration = new double[jointCount];
|
||
var currentJerk = new double[jointCount];
|
||
var maxAbsJerk = 0.0;
|
||
|
||
for (var index = 0; index < jointCount; index++)
|
||
{
|
||
currentVelocity[index] = (currentJoints[index] - previousJoints[index]) / dt;
|
||
if (previousVelocity is not null)
|
||
{
|
||
currentAcceleration[index] = (currentVelocity[index] - previousVelocity[index]) / dt;
|
||
}
|
||
|
||
if (previousAcceleration is not null)
|
||
{
|
||
currentJerk[index] = (currentAcceleration[index] - previousAcceleration[index]) / dt;
|
||
maxAbsJerk = Math.Max(maxAbsJerk, Math.Abs(currentJerk[index]));
|
||
}
|
||
}
|
||
|
||
previousVelocity = currentVelocity;
|
||
previousAcceleration = currentAcceleration;
|
||
|
||
var row = new double[jointCount + 4];
|
||
row[0] = Math.Round(previousTime, 6);
|
||
row[1] = Math.Round(currentTime, 6);
|
||
row[2] = Math.Round(dt, 6);
|
||
row[3] = Math.Round(maxAbsJerk, 6);
|
||
for (var index = 0; index < jointCount; index++)
|
||
{
|
||
row[index + 4] = Math.Round(currentJerk[index], 6);
|
||
}
|
||
|
||
return row;
|
||
}
|
||
|
||
/// <summary>
|
||
/// 在稠密关节轨迹上按时间线性插值，并转换成 J519 下发使用的角度制目标。
|
||
/// </summary>
|
||
private static double[] SampleDenseJointTrajectoryDegrees(
|
||
IReadOnlyList<IReadOnlyList<double>> denseJointTrajectory,
|
||
double trajectoryTime,
|
||
ref int segmentIndex)
|
||
{
|
||
if (denseJointTrajectory.Count == 1 || trajectoryTime <= denseJointTrajectory[0][0])
|
||
{
|
||
return denseJointTrajectory[0].Skip(1).Select(RadiansToDegrees).ToArray();
|
||
}
|
||
|
||
var lastIndex = denseJointTrajectory.Count - 1;
|
||
if (trajectoryTime >= denseJointTrajectory[lastIndex][0])
|
||
{
|
||
return denseJointTrajectory[lastIndex].Skip(1).Select(RadiansToDegrees).ToArray();
|
||
}
|
||
|
||
while (segmentIndex < lastIndex - 1 && denseJointTrajectory[segmentIndex + 1][0] < trajectoryTime)
|
||
{
|
||
segmentIndex++;
|
||
}
|
||
|
||
var start = denseJointTrajectory[segmentIndex];
|
||
var end = denseJointTrajectory[segmentIndex + 1];
|
||
var startTime = start[0];
|
||
var endTime = end[0];
|
||
var segmentDuration = endTime - startTime;
|
||
var alpha = segmentDuration <= 0.0 ? 0.0 : (trajectoryTime - startTime) / segmentDuration;
|
||
var joints = new double[start.Count - 1];
|
||
for (var index = 0; index < joints.Length; index++)
|
||
{
|
||
joints[index] = RadiansToDegrees(start[index + 1] + ((end[index + 1] - start[index + 1]) * alpha));
|
||
}
|
||
|
||
return joints;
|
||
}
|
||
|
||
/// <summary>
|
||
/// 校验 J519 实发采样的基础输入，避免错误时间轴进入运行时链路。
|
||
/// </summary>
|
||
private static void ValidateInputs(
|
||
IReadOnlyList<IReadOnlyList<double>> denseJointTrajectory,
|
||
double durationSeconds,
|
||
double servoPeriodSeconds,
|
||
double speedRatio)
|
||
{
|
||
if (denseJointTrajectory.Count == 0)
|
||
{
|
||
throw new InvalidOperationException("稠密关节轨迹为空。");
|
||
}
|
||
|
||
if (durationSeconds < 0.0)
|
||
{
|
||
throw new ArgumentOutOfRangeException(nameof(durationSeconds), "轨迹时长不能为负数。");
|
||
}
|
||
|
||
if (servoPeriodSeconds <= 0.0 || double.IsNaN(servoPeriodSeconds) || double.IsInfinity(servoPeriodSeconds))
|
||
{
|
||
throw new ArgumentOutOfRangeException(nameof(servoPeriodSeconds), "J519 伺服周期必须是有限正数。");
|
||
}
|
||
|
||
if (speedRatio <= 0.0 || double.IsNaN(speedRatio) || double.IsInfinity(speedRatio))
|
||
{
|
||
throw new ArgumentOutOfRangeException(nameof(speedRatio), "speed_ratio 必须是有限正数。");
|
||
}
|
||
}
|
||
|
||
/// <summary>
|
||
/// 校验连续规划轨迹采样入口的基础输入，避免异常时间轴进入最终发送队列。
|
||
/// </summary>
|
||
private static void ValidatePlannedTrajectoryInputs(
|
||
PlannedTrajectory trajectory,
|
||
double servoPeriodSeconds,
|
||
double speedRatio)
|
||
{
|
||
if (trajectory.WaypointTimes.Count < 2)
|
||
{
|
||
throw new InvalidOperationException("规划轨迹至少需要两个路点才能重建连续样条。");
|
||
}
|
||
|
||
if (trajectory.WaypointTimes[^1] < 0.0)
|
||
{
|
||
throw new ArgumentOutOfRangeException(nameof(trajectory), "规划轨迹总时长不能为负数。");
|
||
}
|
||
|
||
if (servoPeriodSeconds <= 0.0 || double.IsNaN(servoPeriodSeconds) || double.IsInfinity(servoPeriodSeconds))
|
||
{
|
||
throw new ArgumentOutOfRangeException(nameof(servoPeriodSeconds), "J519 伺服周期必须是有限正数。");
|
||
}
|
||
|
||
if (speedRatio <= 0.0 || double.IsNaN(speedRatio) || double.IsInfinity(speedRatio))
|
||
{
|
||
throw new ArgumentOutOfRangeException(nameof(speedRatio), "speed_ratio 必须是有限正数。");
|
||
}
|
||
}
|
||
|
||
/// <summary>
|
||
/// 从规划轨迹重建连续三次样条，供飞拍最终发送队列直接采样。
|
||
/// </summary>
|
||
private static CubicSplineInterpolator RebuildSpline(PlannedTrajectory trajectory)
|
||
{
|
||
var times = trajectory.WaypointTimes.ToArray();
|
||
var values = trajectory.PlannedWaypoints.Select(static waypoint => waypoint.Positions.ToArray()).ToArray();
|
||
return new CubicSplineInterpolator(times, values);
|
||
}
|
||
|
||
/// <summary>
|
||
/// 角度单位转换：rad -> deg。
|
||
/// </summary>
|
||
private static double RadiansToDegrees(double radians)
|
||
{
|
||
return radians * 180.0 / Math.PI;
|
||
}
|
||
}
|