2. 二轴直线插补
任务目标
设计一个二轴联动直线插补器，输入起点、终点与插补步长，输出各轴步进脉冲序列，使合成运动轨迹逼近理想斜线，并验证插补精度（轮廓误差）。
实现方案
采用逐点比较法实现二轴直线插补。核心思想：每步根据当前偏差符号决定进给方向，使实际轨迹始终贴近理想直线。偏差递推公式仅含加法与比较，适合FPGA硬件实现。
核心代码（Verilog）
module line_interpolator #(
parameter W = 16 // 坐标位宽
)(
input wire clk,
input wire rst_n,
input wire start, // 启动插补
input wire [W-1:0] x0, y0, // 起点
input wire [W-1:0] x1, y1, // 终点
output reg x_pulse, // X轴步进脉冲
output reg y_pulse, // Y轴步进脉冲
output reg done // 插补完成
);
// 计算增量（取值）
wire [W-1:0] dx = (x1 >= x0) ? (x1 - x0) : (x0 - x1);
wire [W-1:0] dy = (y1 >= y0) ? (y1 - y0) : (y0 - y1);
wire x_dir = (x1 >= x0); // 1:正向
wire y_dir = (y1 >= y0);
// 插补状态
reg [W-1:0] cnt; // 总步数计数
reg [W:0] f; // 偏差（带符号，W+1位）
reg [W-1:0] total_steps; // 总步数 = dx + dy
reg busy;
// 计算总步数
wire [W:0] sum_steps = dx + dy;
always @(posedge clk or negedge rst_n) begin
if (!rst_n) begin
x_pulse <= 0;
y_pulse <= 0;
done <= 0;
busy <= 0;
cnt <= 0;
f <= 0;
end
else if (start && !busy) begin
busy <= 1;
done <= 0;
cnt <= 0;
f <= 0;
total_steps <= sum_steps[W-1:0];
x_pulse <= 0;
y_pulse <= 0;
end
else if (busy) begin
// 默认无脉冲
x_pulse <= 0;
y_pulse <= 0;
if (cnt < total_steps) begin
// 逐点比较法核心：偏差判别与进给
if (f >= 0) begin
// 向X方向进给
x_pulse <= 1;
// 新偏差：f = f - dy
f <= f - {1'b0, dy};
end
else begin
// 向Y方向进给
y_pulse <= 1;
// 新偏差：f = f + dx
f <= f + {1'b0, dx};
end
cnt <= cnt + 1;
end
else begin
busy <= 0;
done <= 1;
end
end
end
endmodule偏差递推原理
设理想直线从原点指向(dx, dy)，当前动点偏差定义为：
F = x_e·dy − y_e·dx（其中x_e、y_e为动点相对起点的坐标）
每步规则：
- 若
F ≥ 0，向X方向进给一步，新偏差F' = F − dy - 若
F < 0，向Y方向进给一步，新偏差F' = F + dx
总步数为 dx + dy，走完即到达终点。方向由x_dir/y_dir决定（本代码中脉冲方向由外部电机驱动器根据方向信号处理，此处仅输出脉冲）。
仿真验证
使用以下testbench验证插补轨迹与精度：
module tb_line_interpolator;
reg clk = 0;
reg rst_n = 0;
reg start = 0;
reg [15:0] x0 = 0, y0 = 0;
reg [15:0] x1 = 8, y1 = 5; // 终点(8,5)
wire x_pulse, y_pulse, done;
line_interpolator #(.W(16)) dut (
.clk(clk), .rst_n(rst_n), .start(start),
.x0(x0), .y0(y0), .x1(x1), .y1(y1),
.x_pulse(x_pulse), .y_pulse(y_pulse), .done(done)
);
always #5 clk = ~clk;
// 记录轨迹
integer x_pos = 0, y_pos = 0;
integer step_cnt = 0;
real max_err = 0;
always @(posedge clk) begin
if (x_pulse) x_pos <= x_pos + 1;
if (y_pulse) y_pos <= y_pos + 1;
if (x_pulse || y_pulse) begin
step_cnt <= step_cnt + 1;
// 计算轮廓误差：点到直线距离
// 直线方程: 5x - 8y = 0
real dist;
dist = $abs(5*x_pos - 8*y_pos) / $sqrt(5.0*5.0 + 8.0*8.0);
if (dist > max_err) max_err = dist;
end
end
initial begin
#20 rst_n = 1;
#10 start = 1;
#10 start = 0;
wait (done == 1);
#50;
$display("总步数: %0d", step_cnt);
$display("终点坐标: (%0d, %0d)", x_pos, y_pos);
$display("最大轮廓误差: %0.3f", max_err);
// 判定：终点正确且最大误差小于0.5
if (x_pos == 8 && y_pos == 5 && max_err < 0.5)
$display("TEST PASSED");
else
$display("TEST FAILED");
$finish;
end
endmodule运行方式与通过条件
- 在Vivado/ModelSim中编译两个文件，运行仿真。
- 通过条件：
- 仿真输出
TEST PASSED； - 终点坐标精确到达(8,5)，总步数为13；
- 最大轮廓误差小于0.5（即不超过一个脉冲当量）。
精度分析
逐点比较法的最大轮廓误差不超过一个脉冲当量（步长）。对于(8,5)直线，理论最大偏差出现在轨迹中点附近，仿真中max_err应约为0.29，满足精度要求。实际FPGA实现时，偏差寄存器f需比坐标位宽多1位以容纳符号。
上图对比了理想直线与逐点比较法产生的实际阶梯轨迹，可见实际轨迹始终在理想直线附近摆动，最大偏差不超过一个步长。