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

　　上述功能中，不同于傳統(tǒng)數(shù)控系統(tǒng)之處在于系統(tǒng)中需要根據(jù)機械結(jié)構(gòu)實際的機構(gòu)模型嵌入特定的位姿正反解模型．因此，建立正確的正反解模型至關(guān)重要．

　　3.1 建立正反解幾何模型

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

圖3 機器人機構(gòu)簡圖

　　設(shè)定L2分支中繞X 軸旋轉(zhuǎn)虎克鉸的旋轉(zhuǎn)角度為α，繞Y軸旋轉(zhuǎn)虎克鉸的旋轉(zhuǎn)角度為β，3個分支的桿長分別為L1、L2、L3，串聯(lián)關(guān)節(jié)回轉(zhuǎn)轉(zhuǎn)動副和俯仰轉(zhuǎn)動副的轉(zhuǎn)角分別為γ和θ ，則由位姿坐標變換可得到：

(1)

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

(2)

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

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

　　在固定平臺OB1B3中，OB1B3 為已知三角形，則很容易得到其他兩桿長度為：

(4)

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

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

　　根據(jù)得到的α和β ，將已知的L2、γ 、θ 代入到位姿變化矩陣，即可將機器人的末端位姿矩陣求解出來，完