3 位置正反解模塊的實現（Realization of the forward and inverse displacement modules）

　　上述功能中，不同於(yu) 傳(chuan) 統數控係統之處在於(yu) 係統中需要根據機械結構實際的機構模型嵌入特定的位姿正反解模型．因此，建立正確的正反解模型至關(guan) 重要．

　　3.1 建立正反解幾何模型

　　機器人機構簡圖如圖3所示，在本文所述的機構中，將光軸位姿參數（虛軸坐標）換算到驅動坐標位置（實軸坐標）稱為(wei) 反解運算．

圖3 機器人機構簡圖

　　設定L2分支中繞X 軸旋轉虎克鉸的旋轉角度為(wei) α，繞Y軸旋轉虎克鉸的旋轉角度為(wei) β，3個(ge) 分支的杆長分別為(wei) L1、L2、L3，串聯關(guan) 節回轉轉動副和俯仰轉動副的轉角分別為(wei) γ和θ ，則由位姿坐標變換可得到：

(1)

　　已知機器人末端執行器相對於(yu) 固定參考坐標係O-XYZ 的位姿矩陣：

(2)

　　求解此矩陣方程可得α、β 、γ 、θ 、．

　　根據求解得到的α、β 和L2，將T_s2^O 和T_O1^O求解出來．得到S2和O1在固定參考坐標係O-XYZ 中的坐標，則得到下式：

　　在固定平台OB1B3中，OB1B3 為(wei) 已知三角形，則很容易得到其他兩(liang) 杆長度為(wei) ：

(4)

　　正解過程與(yu) 反解過程正好相反：已知機器人的關(guan) 節變量L1、L2、L3和γ 、θ ，求解機器人末端位姿矩陣．

　　根據關(guan) 節變量L1、L2和L3 ，以及B1、O、B3 點坐標，列距離方程可以求解到L2分支繞X 軸和繞Y軸的旋轉角度α和β ．其求解方程如下：

　　根據得到的α和β ，將已知的L2、γ 、θ 代入到位姿變化矩陣，即可將機器人的末端位姿矩陣求解出來，完成運動學正解．