TRIO-Basic 从入门到精通（二十）：Trio通过空间圆弧三点计算圆弧中心和半径

一、教程背景

在空间运动控制中，经常需要根据圆弧上的三个点（起点、中点、终点）计算圆弧的圆心和半径，例如圆弧插补、空间轨迹规划、示教编程等场景。TRIO 控制器虽然内置了 SPHERE_CENTRE 计算函数，但理解其背后的几何原理、并能在需要时自行实现算法，对工程师深入掌握空间圆弧控制非常有帮助。本文通过 TRIO-Basic 手写实现"空间三点求圆弧圆心与半径"的算法，并与 Trio 原装算法进行对比验证。

二、技术要点

1. Trio 内置指令：SPHERE_CENTRE 为 Trio 提供的圆弧计算指令，从其指令帮助可知，该指令的起点必须是 0,0,0，这意味着计算必须采用增量计算方式。
2. 几何原理：空间三点 P、Q、R 确定唯一圆弧。算法思路为：
   • 由向量 PQ、PR 叉乘得到圆弧所在平面的法向量（pi0, pj0, pk0），若法向量为零（三点共线）则无法构成圆弧，程序停止；
   • 分别过 PQ 中点、PR 中点作平面内与 PQ、PR 垂直的中垂线；
   • 两条中垂线的交点即为圆弧圆心，圆心到任意一点的距离即为半径。
3. 参数约定：输入参数通过 TABLE 表传递——Table0-2 为 P 点坐标（起点），Table3-5 为 Q 点坐标（中点），Table6-8 为 R 点坐标（终点）；输出 Table10-13 为圆弧圆心 XYZ 与半径 R。
4. 精度处理：计算结果通过 INT(值*100000+0.5)/100000 四舍五入保留 5 位小数，避免浮点误差累积。

三、实施步骤

1. 导入计算数据：使用 TABLE 指令写入三个空间点的坐标（示例数据为 P(0,0,0)、Q(5,5,5)、R(5,0,0)）：

'导入计算数据
TABLE(100,0,0,0,5,5,5,5,0,0)

2. 调用自写算法：调用 calcriclecen(100, FALSE)，以表 100 为起始地址，第二参数为 FALSE 表示不打印计算过程数据：

'自己写算法实现
calcriclecen(100,FALSE)
PRINT "XYZ 1=",TABLE(100+10),TABLE(100+11),TABLE(100+12)

3. 调用 Trio 原装算法对比：使用 SPHERE_CENTRE 指令计算同一组数据，输出结果对比：

'Trio原装算法实现
SPHERE_CENTRE(100+3, 100+6, 100+10)
PRINT "XYZ 2=",TABLE(100+10),TABLE(100+11),TABLE(100+12)

4. 验证结果：运行后对比两组输出的圆心坐标与半径，两个方法运行结果一致，说明自写算法正确。

四、代码示例

自写空间三点圆弧计算函数：

'=================================================================
'=================================================================
'给定空间三点计算圆弧中心和半径
'partable给定输入参数
'Table0-2  给定P点坐标（起点）
'Table3-5  给定Q点坐标（中点）
'Table6-8  给定Q点坐标（终点)

'Table10-13  输出圆弧中心XYZR

'needprint 确认是否需要打印计算过程数据 false不打印，true打印
FUNCTION calcriclecen(partable AS INTEGER,needprint AS BOOLEAN) AS BOOLEAN

    '输入参数检查
    IF partable<0 OR partable>511980 THEN
        PRINT "Parameter out of range,Par=",partable
        RETURN FALSE
    ENDIF

    '给定空间三点计算圆弧中心和半径
    DIM dpx,dpy,dpz AS FLOAT
    dpx=TABLE(partable+0)
    dpy=TABLE(partable+1)
    dpz=TABLE(partable+2)
    DIM dqx,dqy,dqz AS FLOAT
    dqx=TABLE(partable+3)
    dqy=TABLE(partable+4)
    dqz=TABLE(partable+5)
    DIM drx,dry,drz AS FLOAT
    drx=TABLE(partable+6)
    dry=TABLE(partable+7)
    drz=TABLE(partable+8)

    '打印过程数据
    IF needprint THEN
        PRINT "========================="
        PRINT "Input Parmater:"
        PRINT "P [XYZ]=",dpx,dpy,dpz
        PRINT "Q [XYZ]=",dqx,dqy,dqz
        PRINT "R [XYZ]=",drx,dry,drz
    ENDIF

'输出空间原点和半径
    DIM dx0,dy0,dz0,dr AS FLOAT
    DIM dx1,dy1,dz1 AS FLOAT
    dx0=0
    dy0=0
    dz0=0
    dr=0

'-------------------------------------------
    DIM pi0,pj0,pk0 AS FLOAT
    DIM x1,x2,y1,y2,z1,z2 AS FLOAT
    x1=dqx-dpx
    x2=drx-dpx
    y1=dqy-dpy
    y2=dry-dpy
    z1=dqz-dpz
    z2=drz-dpz

    pi0=y1*z2-z1*y2
    pj0=z1*x2-x1*z2
    pk0=x1*y2-y1*x2

    '打印过程数据
    IF needprint THEN
        PRINT "Proc Parmater:"
        PRINT "IJK =",pi0,pj0,pk0
    ENDIF
    IF (pi0=0) AND (pj0=0) AND (pk0=0) THEN
        STOP
    ENDIF

'-------------------------------------------
'求PQ和PR的中垂线
'---------------------
'1，过PQ的中点(Mx,My,Mz)
    DIM dmx,dmy,dmz AS FLOAT
    dmx=(dpx+dqx)/2
    dmy=(dpy+dqy)/2
    dmz=(dpz+dqz)/2

'与（Mi,Mj,Mk）＝（pi，pj，pk）×（x1,y1,z1）垂直
    DIM dmi,dmj,dmk AS FLOAT
    dmi=pj0*z1-pk0*y1
    dmj=pk0*x1-pi0*z1
    dmk=pi0*y1-pj0*x1

    IF needprint THEN
        PRINT "PQ M=",dmi,dmj,dmk
    ENDIF
'---------------------
'2，过PR的中点(Nx,Ny,Nz)
    DIM dnx,dny,dnz AS FLOAT
    dnx=(dpx+drx)/2
    dny=(dpy+dry)/2
    dnz=(dpz+drz)/2

'与（Ni,Nj,Nk）＝（pi，pj，pk）×（x2,y2,z2）垂直
    DIM dni,dnj,dnk AS FLOAT
    dni=pj0*z2-pk0*y2
    dnj=pk0*x2-pi0*z2
    dnk=pi0*y2-pj0*x2

    IF needprint THEN
        PRINT "PR M=",dni,dnj,dnk
    ENDIF

'合并计算
    DIM ds AS FLOAT
'ds=dNi*dNi+dNj*dNj+dNk*dNk
    ds=SQR(pi0*pi0+pj0*pj0+pk0*pk0)

    IF ds=0 THEN
        dx1=0
        dy1=0
        dz1=0
    ELSE
        dx1=pi0/ds
        dy1=pj0/ds
        dz1=pk0/ds
    ENDIF

    IF needprint THEN
        PRINT "DX1 =",dx1,dy1,dz1,ds
    ENDIF

'-------------------------------------------
'得到的两条中垂线为{X=dMx+dMi*tm;Y=dMy+dMj*tm;Z=dMz+dMk*tm;}
'或(x-dmx)/dmi=(y-dmy)/dmj=(z-dmz)/dmk
' {X=dNx+dNi*tn;Y=dNy+dNj*tn;Z=dNz+dNk*tn;}
'或(x-dnx)/dni=(y-dny)/dnj=(z-dnz)/dnk;

'解两直线交点
    DIM tm,tn AS FLOAT
    tn=((dmy-dny)*dmi+dmj*(dnx-dmx))/(dnj*dmi-dmj*dni)
    tm=(dnx+dni*tn-dmx)/dmi

'dx0,dy0,dz0,dr
    dx0=INT((dmx+dmi*tm)*100000+0.5)/100000
    dy0=INT((dmy+dmj*tm)*100000+0.5)/100000
    dz0=INT((dmz+dmk*tm)*100000+0.5)/100000

    dr=INT((SQR((dx0-dpx)*(dx0-dpx)+(dy0-dpy)*(dy0-dpy)+(dz0-dpz)*(dz0-dpz)))*100000+0.5)/100000

    TABLE(partable+10,dx0,dy0,dz0,dr)
    IF needprint THEN
        PRINT "Out Parmater:"
        PRINT "OUT X Y Z R =",dx0,dy0,dz0,dr
        PRINT "========================="
    ENDIF
    RETURN TRUE
ENDFUNC
'=================================================================

调用示例与结果对比：

'导入计算数据
TABLE(100,0,0,0,5,5,5,5,0,0)

'自己写算法实现
calcriclecen(100,FALSE)
PRINT "XYZ 1=",TABLE(100+10),TABLE(100+11),TABLE(100+12)

'Trio原装算法实现
SPHERE_CENTRE(100+3, 100+6, 100+10)
PRINT "XYZ 2=",TABLE(100+10),TABLE(100+11),TABLE(100+12)

五、应用价值

1. 掌握空间三点求圆弧圆心与半径的完整几何推导与代码实现，深入理解运动控制底层原理。
2. 自写算法与 Trio 原装 SPHERE_CENTRE 指令结果一致，可互为校验，提升现场调试信心。
3. 算法基于 TABLE 表传参，输入输出接口清晰，便于集成到圆弧插补、轨迹规划等应用中。
4. 支持增量式数据处理，配合 Trio 指令的增量计算要求，适配多种应用场景。
5. 四舍五入保留 5 位小数，兼顾计算精度与数据可读性，可直接用于工艺参数下发。

六、SEO关键词

TRIO运动控制器, TRIO-Basic, 空间圆弧, 三点定圆, 圆心计算, 半径计算, SPHERE_CENTRE, 圆弧插补, 运动控制算法

本文内容整理自 guimotion 的《TRIO-Basic从入门到精通教程》系列（CSDN，CC BY-SA 4.0），由 ZeroOne AI 重新组织编写。