2026-10-17 15:38:00 +00:00
|
|
|
|
/* 局部可观测时间律（2026-10-17，方案 §4.5）
|
|
|
|
|
|
* 折线按"巡航时间" τ 参数化：每段 τ = max_j |Δq_j| / VMAX_j（各轴同步、主导轴满速）。
|
|
|
|
|
|
* 归一化速度 v∈[0,1]（1 = 巡航）：v ← min(1, v + A·dt, sqrt(2·A·剩余 τ))，只依赖当前速度与剩余路程。
|
2026-10-19 10:43:05 +00:00
|
|
|
|
* 输出 50 Hz（dt = 20 ms）状态序列，末点精确落在终点。
|
|
|
|
|
|
* 2026-10-19（审查 review_closedloop F7 / review_data_actions C10）：opt.v0 = 起步归一化速度（默认 0 = 静止起步）。
|
|
|
|
|
|
* DAgger 重新标注的起点来自策略闭环、本来就在运动，原来一律从静止加速，标签与策略真正到达的 (q, dq) 状态不符。
|
|
|
|
|
|
* initSpeed(P, qPrev) 把当前速度投影到首段方向上得到 v0（gen_dataset.cjs 用）。 */
|
2026-10-17 15:38:00 +00:00
|
|
|
|
'use strict';
|
|
|
|
|
|
const VMAX = [60, 60, 90, 90, 120, 180]; // °/s，与 tests/route_bench.cjs 同（轴速比 0.6/0.6/0.9/0.9/1.2/1.8）
|
|
|
|
|
|
const A = 5.0; // 1/s²：0→巡航约 0.2 s（a_max 待真机参数）
|
|
|
|
|
|
const DT = 0.02;
|
|
|
|
|
|
function timeParam(P, opt) {
|
|
|
|
|
|
opt = opt || {};
|
|
|
|
|
|
const vmax = opt.vmax || VMAX, a = opt.acc || A, dt = opt.dt || DT;
|
|
|
|
|
|
const segT = [];
|
|
|
|
|
|
for (let i = 1; i < P.length; i++) { let m = 0; for (let j = 0; j < 6; j++) m = Math.max(m, Math.abs(P[i][j] - P[i - 1][j]) / vmax[j]); segT.push(m); }
|
|
|
|
|
|
const cum = [0]; for (const s of segT) cum.push(cum[cum.length - 1] + s);
|
|
|
|
|
|
const total = cum[cum.length - 1];
|
|
|
|
|
|
const at = tau => {
|
|
|
|
|
|
if (tau >= total) return P[P.length - 1].slice();
|
|
|
|
|
|
let i = 0; while (i < segT.length - 1 && cum[i + 1] < tau) i++;
|
|
|
|
|
|
const u = segT[i] > 0 ? (tau - cum[i]) / segT[i] : 1;
|
|
|
|
|
|
return P[i].map((v, j) => v + (P[i + 1][j] - v) * u);
|
|
|
|
|
|
};
|
2026-10-19 10:43:05 +00:00
|
|
|
|
const out = [P[0].slice()], taus = [0]; let tau = 0, v = Math.max(0, Math.min(1, opt.v0 || 0)), guard = 0;
|
2026-10-17 15:38:00 +00:00
|
|
|
|
while (tau < total - 1e-9 && guard++ < 100000) {
|
|
|
|
|
|
const rem = total - tau;
|
|
|
|
|
|
const v1 = Math.min(1, v + a * dt, Math.sqrt(2 * a * Math.max(0, rem - 0.5 * v * dt)) + 1e-9);
|
|
|
|
|
|
let step = 0.5 * (v + v1) * dt;
|
|
|
|
|
|
if (step <= 1e-9) step = Math.min(rem, a * dt * dt); // 防止末端速度掉到 0 卡住
|
|
|
|
|
|
tau = Math.min(total, tau + step); v = v1;
|
|
|
|
|
|
out.push(at(tau)); taus.push(tau);
|
|
|
|
|
|
}
|
|
|
|
|
|
return { traj: out, taus, cum, total, steps: out.length - 1 };
|
|
|
|
|
|
}
|
2026-10-19 10:43:05 +00:00
|
|
|
|
/* 起步速度：w_j = 当前每步增量 / STEP_j（qPrev → P[0]），u_j = 巡航时首段每步增量 / STEP_j（max|u_j| = 1），
|
|
|
|
|
|
v0 = clamp(⟨w,u⟩/⟨u,u⟩, 0, 1)——当前速度在首段方向上的分量；反向或首段为零时 0。wrap：连续轴差值的最短弧函数（可缺省）。 */
|
|
|
|
|
|
function initSpeed(P, qPrev, wrap, cont) {
|
|
|
|
|
|
if (!qPrev || !P || P.length < 2) return 0;
|
|
|
|
|
|
const dd = (a, b, j) => (wrap && cont && cont[j]) ? wrap(a - b) : a - b;
|
|
|
|
|
|
let seg = 0; for (let j = 0; j < 6; j++) seg = Math.max(seg, Math.abs(P[1][j] - P[0][j]) / VMAX[j]);
|
|
|
|
|
|
if (seg < 1e-9) return 0;
|
|
|
|
|
|
let wu = 0, uu = 0;
|
|
|
|
|
|
for (let j = 0; j < 6; j++) {
|
|
|
|
|
|
const u = (P[1][j] - P[0][j]) / VMAX[j] / seg, w = dd(P[0][j], qPrev[j], j) / (VMAX[j] * DT);
|
|
|
|
|
|
wu += w * u; uu += u * u;
|
|
|
|
|
|
}
|
|
|
|
|
|
return Math.max(0, Math.min(1, wu / uu));
|
|
|
|
|
|
}
|
|
|
|
|
|
module.exports = { timeParam, initSpeed, VMAX, A, DT };
|
