前瞻算法
前瞻算法
介绍
前瞻算法，负责确定每段轨迹的入口/出口速度，梯形加减速算法负责在每段内部生成速度曲线。无前瞻，每段都要启停；有前瞻，短段之间可以保持连续速度，只在必要拐弯或结束处减速。计算规划时间分配问题，v-t 图像。
代码实现
#include <stdio.h>
#include <math.h>
#define N_SEG 5
#define EPS 1e-6f
/* ================= 运动参数 ================= */
static const float VMAX = 300.0f; /* mm/s 段内最大速度 */
static const float ACC = 1000.0f; /* mm/s^2 加速度 */
static const float DEC = 1000.0f; /* mm/s^2 减速度 */
static const float V_START = 0.0f; /* 起点速度 */
static const float V_END = 0.0f; /* 终点速度 */
/* 各轴允许的瞬时速度突变，用于拐角速度限制 */
static const float JX = 20.0f;
static const float JY = 20.0f;
/* ================= 数据结构 ================= */
typedef struct {
float x0, y0;
float x1, y1;
float len; /* 段长 */
float ux, uy; /* 单位方向向量 */
float vmax; /* 段内最大速度 */
float v_in; /* 入口速度（前瞻结果） */
float v_out; /* 出口速度（前瞻结果） */
} Segment;
/* 梯形规划结果 */
typedef struct {
float v_in;
float v_out;
float v_peak; /* 实际峰值速度 */
float s_acc; /* 加速段距离 */
float s_const; /* 匀速段距离 */
float s_dec; /* 减速段距离 */
float t_acc; /* 加速段时间 */
float t_const; /* 匀速段时间 */
float t_dec; /* 减速段时间 */
float t_total; /* 总时间 */
int reach_vmax; /* 是否达到 vmax */
} TrapPlan;
/* ================= 工具函数 ================= */
static float clampf(float v, float lo, float hi)
{
if (v < lo) return lo;
if (v > hi) return hi;
return v;
}
static float dist2(float x0, float y0, float x1, float y1)
{
float dx = x1 - x0;
float dy = y1 - y0;
return sqrtf(dx*dx + dy*dy);
}
/* ================= 初始化线段数据 ================= */
void init_path(Segment *path)
{
/* 方向角 0,30,60,90,120 度，长度 100,80,60,90,70 */
const float angles_deg[N_SEG] = {0.0f, 30.0f, 60.0f, 90.0f, 120.0f};
const float lengths[N_SEG] = {100.0f, 80.0f, 60.0f, 90.0f, 70.0f};
float px = 0.0f, py = 0.0f;
for (int i = 0; i < N_SEG; i++) {
float th = angles_deg[i] * (float)M_PI / 180.0f;
float ux = cosf(th);
float uy = sinf(th);
float L = lengths[i];
path[i].x0 = px;
path[i].y0 = py;
path[i].x1 = px + L * ux;
path[i].y1 = py + L * uy;
path[i].len = L;
path[i].ux = ux;
path[i].uy = uy;
path[i].vmax = VMAX;
path[i].v_in = 0.0f;
path[i].v_out = 0.0f;
px = path[i].x1;
py = path[i].y1;
}
}
float junction_speed_limit(float ux_prev, float uy_prev,
float ux_next, float uy_next,
float vmax)
{
float dux = fabsf(ux_next - ux_prev);
float duy = fabsf(uy_next - uy_prev);
float v = vmax;
if (dux > EPS) {
float vx = JX / dux;
if (vx < v) v = vx;
}
if (duy > EPS) {
float vy = JY / duy;
if (vy < v) v = vy;
}
return v;
}
/* ================= 前瞻：反向 + 正向扫描 ================= */
/*
* v_b[i] 表示第 i 段入口速度，i = 0..N_SEG
* v_b[0] = 起点速度
* v_b[N_SEG] = 终点速度
* 第 i 段：v_in = v_b[i], v_out = v_b[i+1]
*/
void forward_lookahead(Segment *path, int n,
float v_start, float v_end,
float acc, float dec)
{
float v_b[N_SEG + 1];
/* ---------- 1. 初始化交界速度 ---------- */
v_b[0] = v_start;
v_b[n] = v_end;
for (int i = 0; i < n - 1; i++) {
float vj = junction_speed_limit(path[i].ux, path[i].uy,
path[i+1].ux, path[i+1].uy,
VMAX);
float v = vj;
if (path[i].vmax < v) v = path[i].vmax;
if (path[i+1].vmax < v) v = path[i+1].vmax;
v_b[i+1] = v;
}
/* ---------- 2. 反向扫描：保证能减速 ---------- */
for (int i = n - 1; i >= 1; i--) {
float v_limit = sqrtf(v_b[i+1]*v_b[i+1] + 2.0f * dec * path[i].len);
if (v_b[i] > v_limit)
v_b[i] = v_limit;
}
/* ---------- 3. 正向扫描：保证能加速 ---------- */
for (int i = 0; i < n - 1; i++) {
float v_limit = sqrtf(v_b[i]*v_b[i] + 2.0f * acc * path[i].len);
if (v_b[i+1] > v_limit)
v_b[i+1] = v_limit;
}
/* ---------- 4. 写回每段 ---------- */
for (int i = 0; i < n; i++) {
path[i].v_in = v_b[i];
path[i].v_out = v_b[i+1];
}
}
/* ================= 单段梯形加减速规划 ================= */
TrapPlan plan_trapezoid(float v_in, float v_out, float L,
float vmax, float acc, float dec)
{
TrapPlan p;
p.v_in = v_in;
p.v_out = v_out;
p.v_peak = v_in;
p.s_acc = p.s_const = p.s_dec = 0.0f;
p.t_acc = p.t_const = p.t_dec = p.t_total = 0.0f;
p.reach_vmax = 0;
/* 加速到 vmax 所需距离 */
float s_acc_max = (vmax*vmax - v_in*v_in) / (2.0f * acc);
if (s_acc_max < 0.0f) s_acc_max = 0.0f;
/* 从 vmax 减速到 v_out 所需距离 */
float s_dec_max = (vmax*vmax - v_out*v_out) / (2.0f * dec);
if (s_dec_max < 0.0f) s_dec_max = 0.0f;
float vp;
if (s_acc_max + s_dec_max <= L) {
/* 能达到 vmax，有匀速段 */
vp = vmax;
p.reach_vmax = 1;
p.s_acc = s_acc_max;
p.s_dec = s_dec_max;
p.s_const = L - p.s_acc - p.s_dec;
} else {
/* 达不到 vmax，求实际峰值 vp */
float num = 2.0f * acc * dec * L
+ dec * v_in * v_in
+ acc * v_out * v_out;
float den = acc + dec;
vp = sqrtf(num / den);
if (vp < v_in) vp = v_in;
if (vp < v_out) vp = v_out;
p.s_acc = (vp*vp - v_in*v_in) / (2.0f * acc);
p.s_dec = (vp*vp - v_out*v_out) / (2.0f * dec);
p.s_const = 0.0f;
if (p.s_acc < 0.0f) p.s_acc = 0.0f;
if (p.s_dec < 0.0f) p.s_dec = 0.0f;
/* 修正由于浮点误差导致的长度不匹配 */
float s_sum = p.s_acc + p.s_dec;
if (s_sum > L + EPS) {
float scale = L / s_sum;
p.s_acc *= scale;
p.s_dec *= scale;
}
}
p.v_peak = vp;
/* 计算时间 */
if (vp > v_in + EPS)
p.t_acc = (vp - v_in) / acc;
if (p.s_const > EPS)
p.t_const = p.s_const / vp;
if (vp > v_out + EPS)
p.t_dec = (vp - v_out) / dec;
p.t_total = p.t_acc + p.t_const + p.t_dec;
return p;
}
/* ================= 打印结果 ================= */
void print_path(const Segment *path, int n)
{
printf("==================== Path Data ====================\n");
for (int i = 0; i < n; i++) {
printf("L%d: (%.4f, %.4f) -> (%.4f, %.4f) len=%.4f u=(%.6f, %.6f)\n",
i+1,
path[i].x0, path[i].y0,
path[i].x1, path[i].y1,
path[i].len,
path[i].ux, path[i].uy);
}
printf("\n");
}
void print_lookahead(const Segment *path, int n)
{
printf("==================== Lookahead Velocity Result ====================\n");
printf("seg_n v_in(mm/s) v_out(mm/s)\n");
for (int i = 0; i < n; i++) {
printf("L%d %8.3f %8.3f\n",
i+1, path[i].v_in, path[i].v_out);
}
printf("\n");
}
void print_trap(const TrapPlan *p, int idx)
{
printf("---------------- L%d Trap Plan ----------------\n", idx+1);
printf(" v_in = %8.3f mm/s\n", p->v_in);
printf(" v_out = %8.3f mm/s\n", p->v_out);
printf(" v_peak = %8.3f mm/s (%s)\n",
p->v_peak, p->reach_vmax ? "达到 vmax" : "未达到 vmax");
printf(" s_acc = %8.3f mm, t_acc = %8.6f s\n", p->s_acc, p->t_acc);
printf(" s_const= %8.3f mm, t_const= %8.6f s\n", p->s_const,p->t_const);
printf(" s_dec = %8.3f mm, t_dec = %8.6f s\n", p->s_dec, p->t_dec);
printf(" t_total= %8.6f s\n", p->t_total);
printf("\n");
}
/* ================= 主函数 ================= */
int main(void)
{
Segment path[N_SEG];
/* 1. 初始化路径 */
init_path(path);
/* 2. 校验段长与坐标 */
for (int i = 0; i < N_SEG; i++) {
float real_len = dist2(path[i].x0, path[i].y0,
path[i].x1, path[i].y1);
if (fabsf(real_len - path[i].len) > 1e-3f) {
printf("Warning: L%d length not consist, real=%.6f stored=%.6f\n",
i+1, real_len, path[i].len);
}
}
print_path(path, N_SEG);
/* 3. 前瞻 */
forward_lookahead(path, N_SEG, V_START, V_END, ACC, DEC);
print_lookahead(path, N_SEG);
/* 4. 每段梯形规划 */
printf("==================== Per Trap Plan ====================\n\n");
float total_time = 0.0f;
for (int i = 0; i < N_SEG; i++) {
TrapPlan tp = plan_trapezoid(path[i].v_in,
path[i].v_out,
path[i].len,
path[i].vmax,
ACC, DEC);
print_trap(&tp, i);
total_time += tp.t_total;
}
printf("==================== Total ====================\n");
printf("total_time = %.6f s\n", total_time);
return 0;
}
结果验证
==================== Path Data ====================
L1: (0.0000, 0.0000) -> (100.0000, 0.0000) len=100.0000 u=(1.000000, 0.000000)
L2: (100.0000, 0.0000) -> (169.2820, 40.0000) len=80.0000 u=(0.866025, 0.500000)
L3: (169.2820, 40.0000) -> (199.2820, 91.9615) len=60.0000 u=(0.500000, 0.866025)
L4: (199.2820, 91.9615) -> (199.2820, 181.9615) len=90.0000 u=(-0.000000, 1.000000)
L5: (199.2820, 181.9615) -> (164.2820, 242.5833) len=70.0000 u=(-0.500000, 0.866025)
==================== Lookahead Velocity Result ====================
seg_n v_in(mm/s) v_out(mm/s)
L1 0.000 40.000
L2 40.000 54.641
L3 54.641 40.000
L4 40.000 40.000
L5 40.000 0.000
==================== Per Trap Plan ====================
---------------- L1 Trap Plan ----------------
v_in = 0.000 mm/s
v_out = 40.000 mm/s
v_peak = 300.000 mm/s (Reach vmax)
s_acc = 45.000 mm, t_acc = 0.300000 s
s_const= 10.800 mm, t_const= 0.036000 s
s_dec = 44.200 mm, t_dec = 0.260000 s
t_total= 0.596000 s
---------------- L2 Trap Plan ----------------
v_in = 40.000 mm/s
v_out = 54.641 mm/s
v_peak = 286.867 mm/s (Not reach vmax)
s_acc = 40.346 mm, t_acc = 0.246867 s
s_const= 0.000 mm, t_const= 0.000000 s
s_dec = 39.654 mm, t_dec = 0.232226 s
t_total= 0.479093 s
---------------- L3 Trap Plan ----------------
v_in = 54.641 mm/s
v_out = 40.000 mm/s
v_peak = 249.585 mm/s (Not reach vmax)
s_acc = 29.654 mm, t_acc = 0.194944 s
s_const= 0.000 mm, t_const= 0.000000 s
s_dec = 30.346 mm, t_dec = 0.209585 s
t_total= 0.404530 s
---------------- L4 Trap Plan ----------------
v_in = 40.000 mm/s
v_out = 40.000 mm/s
v_peak = 300.000 mm/s (Reach vmax)
s_acc = 44.200 mm, t_acc = 0.260000 s
s_const= 1.600 mm, t_const= 0.005333 s
s_dec = 44.200 mm, t_dec = 0.260000 s
t_total= 0.525333 s
---------------- L5 Trap Plan ----------------
v_in = 40.000 mm/s
v_out = 0.000 mm/s
v_peak = 266.083 mm/s (Not reach vmax)
s_acc = 34.600 mm, t_acc = 0.226083 s
s_const= 0.000 mm, t_const= 0.000000 s
s_dec = 35.400 mm, t_dec = 0.266083 s
t_total= 0.492165 s
==================== Total ====================
total_time = 2.497122 s
浙公网安备 33010602011771号