Kossel的一种滑块位置计算方法

        做了一个小激光雕刻机之后，研究了一下这款3D打印机的结构和工作原理，一下就对这个运动过程很感兴趣，这三个杆是怎么联动使得喷头保持在一个平面上运动呢？打算先做一个架构，然后把激光器放在上面不是可以方便雕刻不同厚度的东西了么，好吧，我承认也想做一下这个3D打印机。不过，入坑的时候，一点一点的下而已。

        写这个算法，需要首先简化一下模型，做一些转化和定义：

1、并联臂两根，简化成1根，假定为1根（对算法结果没影响，我猜两根就是为了更好的在重力不平衡的情况下保持平面的稳定性。有不对的地方还请大手子指教）。

2、假定的这根连杆，其与效应器的接点就在并联的两根鱼眼的中心连线的中点处，并且这个点称为效应器三角形的一个顶点。

3、同样的，假定的连杆另一端在滑块连接处两个鱼眼中心连线的中点处，设为A点。

4、从A点向下做垂直于效应器平面的直线，这条直线上的某一点就是我们要求的某一轴的目标位置。

观察一下下面的图（借用一下网上的图，绘图技术太烂，如果作者不允许请马上联系我更换）：

图中绿色三角就是效应器三角形，蓝色直线就是虚拟的平行于z轴的直线，红色直线就是虚拟的连杆。那么，现在我们进行分析，在当前位置下红蓝直线的交点怎么求：

        运动过程中，红色直线是定长的，其下端点可以根据挤出头求出——已知；其上端一定落在蓝色直线上——所求。现在，这个模型就很好转换了：已知红绿交点处坐标为一个半径为红色直线长度的球，求该球与蓝色直线的交点？这个交点可能没有——远离球，可能一个——相切，可能两个——穿过，可以猜测一下是一个一元二次方程。

        OK，现在我们就来解一下这个问题（使用向量好吧，先不要来XYZ）。设：三条虚拟直线垂直于底床平面，三个交点组成的等边三角形中心为世界坐标系原点，X=0,Y=0,Z=0：

1、直线方程：

P(t)=O+tD

若我们规范D（直线方向）向量，则t就是所求坐标。

2、球方程：

(P-C)(P-C)=R^2

R为假象连杆长度，可测，C为球心——上述已经可求，P为球上任意一点。

3、联立两个方程：

(D*D)t^2+2*D(O-c)t+(O-C)(O-C)-R^2=0 

解这个关于t的一元二次方程就可以得到两个解（除非你真的让某一个连杆水平起来了），我们取其中较高位置的一个（这个我没有去验证到底是近的一个还是高的一个，但是可以考虑一种情况：当效应器较低时，另一个交点低于效应器三角形的平面）。所以，我们的计算用到很多量，也体现出增加精度的方向：

1、仔细测量连杆两端鱼眼的中心的距离并且保证六根尽可能完全一样长。

2、安装挤出头时，让它的尖端的竖直投影与我们效应器的虚拟三角形（等边三角形）中心重合——效应器本身的对称程度、安装位置等因素。

3、工作台与效应器平面平行，至少在用微动开关或者压敏管矫正的时候要细心一些，安装光电计数器时也要仔细一点。我想我会自己写一个程序来校准它。

好了，啰嗦了不少，现在来写这个直线与球交点的解法：

    Function LineCrossSphere(Origin As Vector3D, Direction As Vector3D, Center As Vector3D, Radius As Double) As Vector3D
        Direction.Normalize()
        Dim VecCO = Origin - Center
        Dim a As Double = Vector3D.DotProduct(Direction, Direction)
        Dim b As Double = 2 * Vector3D.DotProduct(Direction, VecCO)
        Dim c As Double = Vector3D.DotProduct(VecCO, VecCO) - Radius ^ 2
        Dim delta As Double = b ^ 2 - 4 * a * c
        If delta >= 0 Then          '不相交
            Dim sqrtdelta As Double = Math.Sqrt(delta)
            Dim t1 As Double = (-b + sqrtdelta) / (2.0 * a)
            Dim t2 As Double = (-b - sqrtdelta) / (2.0 * a)
            Dim t As Double = Math.Max(t1, t2)
            Dim p = Origin + Vector3D.Multiply(t, Direction)    '取更高的一个解
            Return p
        Else
            Return Vector3DEmpty
        End If
    End Function

O是蓝色直线与三角形平面的交点，D是（0，0，1），C是绿色三角形和红线的交点，R是连杆长度。其实，这个函数还可以进行简化，但是我没有去做，它运行于上位机并且是预计算的，而且优化也没有太大提升。现在，我们只需要根据我们所需的挤出头位置A+挤出头到效应器平面的距离为绿色三角形中心，然后根据绿色三角形的大小计算出3个Origin参数。当然，这里有一个小技巧，我们设效应器三角形某一个顶点A在Y轴上则三角形中心到顶点的向量OA经过旋转正负120度就得到了另外两个顶点。

        函数整合和使用哪种3D组件进行计算就是你的问题了。我使用了System.Windows.Media.Media3D名空间中的函数进行计算，因为它的V3D中X,Y,Z都是双精度浮点并且前段时间稍微用过一下。当你取得下列数据时，就可以进行计算了：

    效应器平面到笔尖的竖直距离（激光头焦点、挤出头尖端）
    效应器XY平面上的Y点坐标（中心为笔尖到效应器平面的垂直投影）
    并联臂长度
    世界坐标系下XY平面上Y滑块与并联臂相连点投影的坐标

        最后，简单提一下自动矫正的问题，选择传感器种类时最关键是传感器的精度，当然微动开关并不是不行，但是你要知道它的行程。在自动矫正过程中，可以得到一系列数据，我们可以网格化探测，而后当打印时喷头运行到某一个格子里的时候，把四个角的值和当前值位置进行比较进行一些处理得到合理的矫正值就可以了。原理就是这样，所以底床最好还是平一些，麻子太多再自动校准没白费，除非每个点都探测一下，那可能要形成海量数据，就没法愉快的玩耍了。