做了一個小激光雕刻機之後，研究了一下這款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滑塊與並聯臂相連點投影的坐標

最後，簡單提一下自動矯正的問題，選擇傳感器種類時最關鍵是傳感器的精度，當然微動開關並不是不行，但是你要知道它的行程。在自動矯正過程中，可以得到一系列數據，我們可以網格化探測，而後當打印時噴頭運行到某一個格子裏的時候，把四個角的值和當前值位置進行比較進行一些處理得到合理的矯正值就可以了。原理就是這樣，所以底床最好還是平一些，麻子太多再自動校準沒白費，除非每個點都探測一下，那可能要形成海量數據，就沒法愉快的玩耍了。

Kossel的一種滑塊位置計算方法