当前位置：首页 > news >正文

GRBL运动控制算法（二）圆弧插补

news 来源：原创 2025/7/1 3:09:16

前言

GRBL 是一款高性能、开源的嵌入式 CNC（计算机数控）控制器固件，专为 Arduino 平台优化，广泛应用于雕刻机、激光切割机、3D 打印机及其他精密运动控制场景。自 2009 年发布以来，GRBL 凭借其高效的运动规划算法、稳定的实时控制性能和紧凑的代码结构，成为创客、工程师和制造商的首选解决方案。

本文深入剖析 GRBL1.1 的圆弧插补实现原理，从参数解析、误差控制逐层拆解，帮助读者理解其底层逻辑与设计思想，为开发者优化运动算法或用户调参提供理论参考。

一.GRBL圆弧插补流程

1 .函数定义部分

#ifdef USE_LINE_NUMBERS
  void mc_arc(float *position, float *target, float *offset, float radius, float feed_rate, 
    uint8_t invert_feed_rate, uint8_t axis_0, uint8_t axis_1, uint8_t axis_linear, uint8_t is_clockwise_arc, int32_t line_number)
#else
  void mc_arc(float *position, float *target, float *offset, float radius, float feed_rate,
    uint8_t invert_feed_rate, uint8_t axis_0, uint8_t axis_1, uint8_t axis_linear, uint8_t is_clockwise_arc)
#endif

这是一个条件编译的函数定义，根据是否定义了USE_LINE_NUMBERS宏决定是否包含line_number参数

参数说明：

position: 当前位置坐标数组

target: 目标位置坐标数组

**offset: **从起点到圆心的偏移量

radius: 圆弧半径

**feed_rate: **进给速率

invert_feed_rate: 是否反转进给速率标志

**axis_0: **第一个圆弧平面轴(X/Y/Z中的某一个)

axis_1: 第二个圆弧平面轴

**axis_linear: **线性轴(垂直于圆弧平面的轴)

**is_clockwise_arc: **是否为顺时针圆弧标志

**line_number: **(可选)行号，用于调试或G代码解析

2 .流程图

以下是圆弧插补流程图

二.GRBL圆弧插补算法

1 .圆心坐标计算

根据 gc_execute_line()函数，对G02，G03代码进行解析，通过向量几何和勾股定理，将G代码中的两点+半径参数转换为圆心坐标。

uint8_t gc_execute_line(char *line) 
{
...
    // 如果当前单位为英寸模式，将半径值转换为毫米
    if (gc_block.modal.units == UNITS_MODE_INCHES) { gc_block.values.r *= MM_PER_INCH; }

    // 计算中间变量 h_x2_div_d = 4*r² - x² - y²
    float h_x2_div_d = 4.0 * gc_block.values.r*gc_block.values.r - x*x - y*y;
    
    // 如果计算结果为负，说明圆弧半径错误（无法构成有效圆弧）
    if (h_x2_div_d < 0) { FAIL(STATUS_GCODE_ARC_RADIUS_ERROR); } // [圆弧半径错误]
    
    // 完成 h_x2_div_d 的计算：-sqrt(h_x2_div_d)/hypot(x,y) 
    // 等价于 -(h * 2 / d)
    h_x2_div_d = -sqrt(h_x2_div_d)/hypot_f(x,y); 
    
    // 如果是逆时针圆弧（CCW），反转 h_x2_div_d 的符号（参见下方示意图）
    if (gc_block.modal.motion == MOTION_MODE_CCW_ARC) { h_x2_div_d = -h_x2_div_d; }  
    
    // 处理负半径情况
    if (gc_block.values.r < 0) { 
        h_x2_div_d = -h_x2_div_d; 
        gc_block.values.r = -gc_block.values.r; // 将半径取正值
    }        
    
    // 通过计算得出圆弧的实际中心坐标（完成操作）
    gc_block.values.ijk[axis_0] = 0.5*(x-(y*h_x2_div_d));  // 计算x轴的圆心坐标
    gc_block.values.ijk[axis_1] = 0.5*(y+(x*h_x2_div_d)); // 计算y轴的圆心坐标
}

圆心坐标求解图

由距离公式：

$\begin{align*} d = \sqrt{x^2 + y^2} \end{align*}$

由勾股定理：

$\begin{align*} h &= \sqrt{r^2 - \left(\frac{d}{2}\right)^2} = \frac{\sqrt{4r^2 - x^2 - y^2}}{2} \end{align*}$

由△ADO 和△TBC 相似：

$\begin{align*} |AD| &= \frac{y}{d} \cdot h \\[10pt] |DO| &= \frac{x}{d} \cdot h \end{align*}$

P(0,0)为当前位置，T(x,y)为目标位置，圆心坐标（向量推导）：

A 的坐标 $(\frac{x}{2} ,\frac{y}{2})$

$\begin{align*} i &= \frac{x}{2}-|AD|= \frac{x}{2} - \frac{y}{d} \cdot h \\[10pt] % X轴圆心坐标（i = x/2 - (y/d)*h） j &=\frac{y}{2}+|DO|= \frac{y}{2} + \frac{x}{d} \cdot h % Y轴圆心坐标（j = y/2 + (x/d)*h） \end{align*}$

半径有效性检查：

计算 $4r^2 - x^2 - y^2$ ，若为负说明半径太小（无法通过两点）

触发错误条件： $\frac{d}{2}$ （即半径小于两点距离的一半）

2.半径向量计算

调用mc_arc()函数进行半径向量计算

float center_axis0 = position[axis_0] + offset[axis_0];
float center_axis1 = position[axis_1] + offset[axis_1];
float r_axis0 = -offset[axis_0];  // 圆心到起点的向量
float r_axis1 = -offset[axis_1];
float rt_axis0 = target[axis_0] - center_axis0;  // 圆心到终点的向量
float rt_axis1 = target[axis_1] - center_axis1;

半径坐标求解图

圆心坐标计算：

$\begin{align*} x_c &= x_p + i \\ y_c &= y_p + j \\ \end{align*}$

半径向量计算：

$\begin{align*} \vec{r} &= (-i, -j) \\ \vec{r}_t &= (x_t - x_c, y_t - y_c) \end{align*}$

3.角度行程计算

求解angular_travel的大小，即圆心指向起点向量与圆心指向终点向量间的夹角

float angular_travel = atan2(r_axis0*rt_axis1 - r_axis1*rt_axis0, 
                            r_axis0*rt_axis0 + r_axis1*rt_axis1);
if (is_clockwise_arc) { 
    if (angular_travel >= -EPSILON) angular_travel -= 2*M_PI;  // 顺时针补全负角度
} else {
    if (angular_travel <= EPSILON) angular_travel += 2*M_PI;   // 逆时针补全正角度
}

角度行程求解图

反正切函数的加减法公式：

$\begin{align*} \arctan A + \arctan B &= \arctan\left(\frac{A+B}{1-AB}\right) \\ \arctan A - \arctan B &= \arctan\left(\frac{A-B}{1+AB}\right) \end{align*}$

给定两点：

$A(x_0,y_0), \quad B(x_1,y_1)$

它们的极角分别为：

$\angle A = \arctan\left(\frac{y_0}{x_0}\right), \quad \angle B = \arctan\left(\frac{y_1}{x_1}\right)$

根据反正切减法公式，两角之差为：

$\begin{align*} \angle B - \angle A &= \arctan\left(\frac{y_1}{x_1}\right) - \arctan\left(\frac{y_0}{x_0}\right) \\ &= \arctan\left(\frac{\frac{y_1}{x_1} - \frac{y_0}{x_0}}{1 + \frac{y_1}{x_1} \cdot \frac{y_0}{x_0}}\right) \quad \text{（应用反正切减法公式）} \\ &= \arctan\left(\frac{x_0 y_1 - x_1 y_0}{x_0 x_1 + y_0 y_1}\right) \quad \text{（分子分母同乘 $x_0 x_1$）} \end{align*}$

4.分段数计算（几何逼近）

将圆弧插补成足够多的小线段，arc_tolerance为圆弧上任意两点连接而成的小线段到圆弧的最大距离arc_tolerance越小，则圆弧微分成的小线段越多，插补精度也越高。

uint16_t segments = floor(fabs(0.5 * angular_travel * radius) / 
                    sqrt(settings.arc_tolerance * (2 * radius - settings.arc_tolerance));

数学推导图

弦高误差 at（即 arc_tolerance）与半弦长 k 的关系：

$\begin{align*} k &= \sqrt{r^2 - (r - at)^2 }= \sqrt{at(2r - at)} \\ \end{align*}$

总弧长 arc 和分段数 s 的关系：

$\begin{align*} arc&=r\cdot |\theta| \\ s&=\frac{arc}{2k} \\ \end{align*}$

代入后得到

$\begin{align*} s &= \frac{a r c}{2 k} = \frac{0.5 |\theta| \cdot r}{\sqrt{at(2r - at)}} \\ \end{align*}$

5.小角度近似优化（泰勒展开）

三角函数的泰勒展开

float theta_per_segment = angular_travel/segments;//每个线段对应的圆心角
// axis_linear，除了圆弧平面之外的第三个轴，即与圆弧平面垂直的轴
float linear_per_segment = (target[axis_linear] - position[axis_linear])/segments;
float cos_T = 2.0 - theta_per_segment * theta_per_segment;  // 2 - θ²
// θ*(1 - θ²/6) (sin的三阶泰勒近似)
float sin_T = theta_per_segment * 0.16666667 * (cos_T + 4.0); 
cos_T *= 0.5;  // → 1 - θ²/2 (cos的二阶泰勒近似)

数学推导图

每个线段对应的圆心角:

$\begin{align*} \theta_p &= \frac{\theta_n}{s} \end{align*}$

泰勒展开式：

$\begin{align*} sin x &= x - \frac{x^3}{3!} + o(x^3) \\ cos x &= 1 - \frac{x^2}{2!} + o(x^2) \end{align*}$

cosx 公式取前两项,sinx 公式取前三项得：

$\begin{align*} \cos\theta_p &\approx 1 - \frac{\theta_p^2}{2} \\ \sin\theta_p &\approx \theta_p - \frac{\theta_p^3}{6} \approx \frac{1}{6}\cdot\theta_p \cdot(2 - \theta_p^2 +4) \end{align*}$

6.位置计算

通过混合近似与精确计算来优化圆弧插补的精度和效率：快速近似法（旋转矩阵增量更新，40μs/次）用于多数计算，而高精度修正（直接三角函数计算，375μs/次）每隔N_ARC_CORRECTION次执行一次，既避免累积误差，又维持整体速度；对于微小圆弧（分段数少），仅用近似计算也能保证误差可控，实现速度与精度的最佳平衡。

float sin_Ti;       // 临时存储当前角度的正弦值
float cos_Ti;       // 临时存储当前角度的余弦值
float r_axisi;      // 旋转后的临时半径向量（用于中间计算）
uint16_t i;         // 循环计数器
uint8_t count = 0;  // 圆弧修正计数器（控制修正频率）

for (i = 1; i < segments; i++) { // 遍历每个线段（共 segments-1 次）
    
    // 如果未达到修正间隔（N_ARC_CORRECTION），使用增量旋转矩阵近似计算
    if (count < N_ARC_CORRECTION) {
        // 应用旋转矩阵更新半径向量（约 40 微秒/次）
        r_axisi = r_axis0 * sin_T + r_axis1 * cos_T;  // 计算旋转后的新半径分量
        r_axis0 = r_axis0 * cos_T - r_axis1 * sin_T;  // 更新x轴半径分量
        r_axis1 = r_axisi;                            // 更新y轴半径分量
        count++;                                      // 递增修正计数器
    } 
    // 达到修正间隔时，进行精确的圆弧半径修正（约 375 微秒/次）
    else {
        // 通过初始半径向量（=-offset）和变换矩阵计算精确位置
        cos_Ti = cos(i * theta_per_segment);          // 计算当前角度的余弦
        sin_Ti = sin(i * theta_per_segment);          // 计算当前角度的正弦
        r_axis0 = -offset[axis_0] * cos_Ti + offset[axis_1] * sin_Ti; //精确计算x轴半径分量
        r_axis1 = -offset[axis_0] * sin_Ti - offset[axis_1] * cos_Ti; //精确计算y轴半径分量
        count = 0;                                    // 重置修正计数器
    }

    // 更新圆弧目标位置坐标
    position[axis_0] = center_axis0 + r_axis0;       // x轴坐标 = 圆心坐标 + x半径分量
    position[axis_1] = center_axis1 + r_axis1;       // y轴坐标 = 圆心坐标 + y半径分量
    position[axis_linear] += linear_per_segment;     // 线性轴（如Z轴）按分段增量移动
}

快速近似法：

通过叠加半径分量得到当前点坐标:

$\begin{align*} P_0 &= \begin{cases} r \cos \varphi = \mathrm{r\_axis}_0 \\ r \sin \varphi = \mathrm{r\_axis}_1 \end{cases} \\ \end{align*}$

$\begin{align*} P_1 &= \begin{cases} r \cos (\varphi + \theta_p) = r\cos\varphi\cos\theta_p - r\sin\varphi\sin\theta_p = \mathrm{r\_axis}_0 \cdot \cos \theta_p - \mathrm{r\_axis}_1 \cdot \sin \theta_p \\ r \sin (\varphi + \theta_p) = r\sin\varphi\cos\theta_p + r\cos\varphi\sin\theta_p = \mathrm{r\_axis}_1 \cdot \cos \theta_p + \mathrm{r\_axis}_0 \cdot \sin \theta_p \end{cases} \end{align*}$

$\begin{align*} P_2 &= \begin{cases} r \cos (\varphi_1 + \theta_p) = r\cos\varphi_1\cos\theta_p - r\sin\varphi_1\sin\theta_p = \mathrm{r\_axis}_0 \cdot \cos \theta_p - \mathrm{r\_axis}_1 \cdot \sin \theta_p \\ r \sin (\varphi_1 + \theta_p) = r\sin\varphi_1\cos\theta_p + r\cos\varphi_1\sin\theta_p = \mathrm{r\_axis}_1 \cdot \cos \theta_p + \mathrm{r\_axis}_0 \cdot \sin \theta_p \end{cases} \end{align*}$

高精度修正：

每隔N_ARC_CORRECTION次执行一次，既避免累积误差，又维持整体速度

$\begin{align*} P_0 &= \begin{cases} r \cos \varphi = r\_{\text{axis}0} = -\text{offset}[\text{axis}_0] \\ r \sin \varphi = r\_{\text{axis}1} = -\text{offset}[\text{axis}_1] \end{cases} \end{align*}$

$\begin{align*} P_i &= \begin{cases} \begin{aligned} r \cos (\varphi + i\theta) &= r \cos \varphi \cos i\theta - r \sin \varphi \sin i\theta \\ &= -\mathrm{offset}[\mathrm{axis}_0] \cdot \cos i\theta + \mathrm{offset}[\mathrm{axis}_1] \cdot \sin i\theta \\[5pt] r \sin (\varphi + i\theta) &= r \sin \varphi \cos i\theta + r \cos \varphi \sin i\theta \\ &= -\mathrm{offset}[\mathrm{axis}_1] \cdot \cos i\theta - \mathrm{offset}[\mathrm{axis}_0] \cdot \sin i\theta \end{aligned} \end{cases} \end{align*}$