feat(*): 添加触发样本偏移与实发轨迹分析导出

* 为 RobotConfig 增加 trigger_sample_index_offset_cycles 配置
  * 让 DO 事件携带示教点关节角并按最接近 sample 绑定触发
  * 调整运行时 IO 地址位掩码映射并补充 ShotEvents 导出
  * 新增 2026042802-1 抓包分析脚本、数据产物与结论文档
  * 补齐配置兼容、规划绑定和运行时触发相关测试
This commit is contained in:
2026-05-09 11:12:31 +08:00
parent 1779067b5c
commit f7e2bb0e7b
35 changed files with 5772 additions and 55 deletions

View File

@@ -22,6 +22,12 @@ public sealed class ControllerClientTrajectoryOrchestrator
/// </summary>
private const double DenseLimitStretchFactor = 1.01;
/// <summary>
/// 平滑起停重定时混合系数0 表示不平滑1 表示完全使用平滑时间律。
/// 当前取值用于“弱平滑”,仅轻微拉伸首尾时间段,避免起停过慢。
/// </summary>
private const double SmoothStartStopBlend = 0.60;
private readonly ICspPlanner _icspPlanner;
private readonly SelfAdaptIcspPlanner _selfAdaptIcspPlanner;
private readonly ShotTimelineBuilder _shotTimelineBuilder = new(new WaypointTimestampResolver());
@@ -81,7 +87,12 @@ public sealed class ControllerClientTrajectoryOrchestrator
var executionTrajectory = plannedTrajectory;
var denseJointTrajectory = CreateLimitCompliantDenseTrajectory(ref executionTrajectory, shapeTrajectoryEdges: false);
var shotTimeline = new ShotTimeline(Array.Empty<ShotEvent>(), Array.Empty<TrajectoryDoEvent>());
var result = CreateResult(executionTrajectory, shotTimeline, denseJointTrajectory, usedCache: false);
var result = CreateResult(
executionTrajectory,
shotTimeline,
denseJointTrajectory,
usedCache: false,
triggerSampleIndexOffsetCycles: 0);
_logger?.LogInformation(
"PlanOrdinaryTrajectory 完成: 时长={Duration}s, 采样点数={SampleCount}",
@@ -156,7 +167,12 @@ public sealed class ControllerClientTrajectoryOrchestrator
holdCycles: settings.IoKeepCycles,
samplePeriod: planningRobot.ServoPeriod,
useDo: settings.UseDo);
var result = CreateResult(smoothedExecutionTrajectory, shotTimeline, denseJointTrajectory, usedCache: false);
var result = CreateResult(
smoothedExecutionTrajectory,
shotTimeline,
denseJointTrajectory,
usedCache: false,
triggerSampleIndexOffsetCycles: settings.TriggerSampleIndexOffsetCycles);
var bundle = new PlannedExecutionBundle(plannedTrajectory, shotTimeline, result);
_logger?.LogInformation(
@@ -259,6 +275,7 @@ public sealed class ControllerClientTrajectoryOrchestrator
hash.Add(options.SaveTrajectory);
hash.Add(settings.UseDo);
hash.Add(settings.IoKeepCycles);
hash.Add(settings.TriggerSampleIndexOffsetCycles);
hash.Add(settings.AdaptIcspTryNum);
hash.Add(settings.SmoothStartStopTiming);
@@ -309,6 +326,7 @@ public sealed class ControllerClientTrajectoryOrchestrator
useDo: true,
ioAddresses: Array.Empty<int>(),
ioKeepCycles: 0,
triggerSampleIndexOffsetCycles: 7,
accLimitScale: 1.0,
jerkLimitScale: 1.0,
adaptIcspTryNum: 5);
@@ -399,7 +417,8 @@ public sealed class ControllerClientTrajectoryOrchestrator
PlannedTrajectory plannedTrajectory,
ShotTimeline shotTimeline,
IReadOnlyList<IReadOnlyList<double>> denseJointTrajectory,
bool usedCache)
bool usedCache,
int triggerSampleIndexOffsetCycles)
{
return new TrajectoryResult(
programName: plannedTrajectory.OriginalProgram.Name,
@@ -413,6 +432,7 @@ public sealed class ControllerClientTrajectoryOrchestrator
usedCache: usedCache,
originalWaypointCount: plannedTrajectory.OriginalWaypointCount,
plannedWaypointCount: plannedTrajectory.PlannedWaypointCount,
triggerSampleIndexOffsetCycles: triggerSampleIndexOffsetCycles,
denseJointTrajectory: denseJointTrajectory);
}
@@ -476,36 +496,51 @@ public sealed class ControllerClientTrajectoryOrchestrator
}
/// <summary>
/// 为飞拍执行生成一条平滑起停时间轴。
/// 保持路点位置不变,只重映射路点时刻,让起点和终点附近的速度自然收敛。
/// 为飞拍执行生成平滑起停时间轴(仅重定时,不改几何路点)
/// 该方法保持 <see cref="PlannedTrajectory.PlannedWaypoints"/> 不变,只重映射 <see cref="PlannedTrajectory.WaypointTimes"/>
/// 让轨迹在起点和终点附近具有更柔和的速度变化,从而降低“突然起步/突然收尾”带来的离散差分尖峰。
/// </summary>
/// <param name="plannedTrajectory">规划器输出的原始轨迹(通常是线性时间轴)。</param>
/// <returns>
/// 若满足平滑条件,则返回新的重定时轨迹;若路点数量不足或总时长无效,则直接返回输入轨迹。
/// </returns>
private static PlannedTrajectory ApplySmoothStartStopTiming(PlannedTrajectory plannedTrajectory)
{
var originalTimes = plannedTrajectory.WaypointTimes;
// 至少需要“起点-中间点-终点”三类点,才有可平滑的中间区间。
if (originalTimes.Count < 3)
{
return plannedTrajectory;
}
var totalDuration = originalTimes[^1];
// 轨迹总时长必须为正,避免后续归一化进度出现除零或无意义映射。
if (totalDuration <= 0.0)
{
return plannedTrajectory;
}
var smoothedTimes = new double[originalTimes.Count];
// 强制固定边界:起点仍在 0终点仍在总时长保证任务总耗时不变。
smoothedTimes[0] = 0.0;
smoothedTimes[^1] = totalDuration;
for (var index = 1; index < originalTimes.Count - 1; index++)
{
// 把原始时刻归一化到 [0, 1],用于统一时间律变换。
var normalizedProgress = originalTimes[index] / totalDuration;
smoothedTimes[index] = totalDuration * InvertSmoothStartStopProgress(normalizedProgress);
// 先求完整平滑时间律对应的时刻,再与原线性时刻按比例混合。
// 这里采用弱平滑blend=0.35):保留大部分原节奏,仅轻微降低首尾突变。
var smoothedProgress = InvertSmoothStartStopProgress(normalizedProgress);
var blendedProgress = ((1.0 - SmoothStartStopBlend) * normalizedProgress)
+ (SmoothStartStopBlend * smoothedProgress);
smoothedTimes[index] = totalDuration * blendedProgress;
}
var segmentDurations = new double[smoothedTimes.Length - 1];
for (var index = 0; index < segmentDurations.Length; index++)
{
// 重建每段持续时间,供后续稠密采样和限幅检查使用。
segmentDurations[index] = smoothedTimes[index + 1] - smoothedTimes[index];
}
@@ -522,17 +557,24 @@ public sealed class ControllerClientTrajectoryOrchestrator
}
/// <summary>
/// 反解 7 次 smootherstep 的时间进度,用二分法把原始线性进度映射成平滑时间轴
/// 反解平滑时间进度:给定目标进度 p求满足 f(t)=p 的 t
/// 其中 f(t) 由 <see cref="EvaluateSmoothStartStopProgress"/> 给出,是单调递增的 7 次 smootherstep 曲线,
/// 通过二分法可稳定得到对应的归一化时间,避免显式求高次方程根带来的复杂性和数值不稳定。
/// </summary>
/// <param name="normalizedProgress">目标归一化进度,理论范围 [0, 1]。</param>
/// <returns>与目标进度对应的归一化时间,范围 [0, 1]。</returns>
private static double InvertSmoothStartStopProgress(double normalizedProgress)
{
// 防御式裁剪,避免上游浮点误差将进度推到区间外。
var target = Math.Clamp(normalizedProgress, 0.0, 1.0);
var low = 0.0;
var high = 1.0;
// 固定迭代次数确保耗时可预测40 次足以把区间收敛到工程可用精度。
for (var iteration = 0; iteration < 40; iteration++)
{
var middle = (low + high) / 2.0;
var progress = EvaluateSmoothStartStopProgress(middle);
// f(middle) 小于目标,说明根在右半区间;否则在左半区间。
if (progress < target)
{
low = middle;
@@ -547,10 +589,15 @@ public sealed class ControllerClientTrajectoryOrchestrator
}
/// <summary>
/// 计算 7 次 smootherstep 进度值,用于整段平滑起停时间律
/// 计算 7 次 smootherstep 进度函数值 f(u)
/// 该函数在 u=0 和 u=1 处具有更高阶导数连续性,可让起停段速度变化更平滑,
/// 适合用于飞拍轨迹的时间重参数化,减少离散导数在边界处的突变。
/// </summary>
/// <param name="normalizedTime">归一化时间 u理论范围 [0, 1]。</param>
/// <returns>归一化进度 f(u),范围 [0, 1]。</returns>
private static double EvaluateSmoothStartStopProgress(double normalizedTime)
{
// 先裁剪再计算多项式,确保数值稳定且不受外部越界输入影响。
var u = Math.Clamp(normalizedTime, 0.0, 1.0);
var u2 = u * u;
var u3 = u2 * u;