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

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

　　3.1 建立正反解幾何模型

　　機(jī)器人機(jī)構(gòu)簡(jiǎn)圖如圖3所示，在本文所述的機(jī)構(gòu)中，將光軸位姿參數(shù)（虛軸坐標(biāo)）換算到驅(qū)動(dòng)坐標(biāo)位置（實(shí)軸坐標(biāo)）稱(chēng)為反解運(yùn)算．

圖3 機(jī)器人機(jī)構(gòu)簡(jiǎn)圖

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

(1)

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

(2)

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

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

　　在固定平臺(tái)OB1B3中，OB1B3 為已知三角形，則很容易得到其他兩桿長(zhǎng)度為：

(4)

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

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

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