精品1_亚洲第一综合_午夜精品久久久久久毛片_精品国产一区二区三区成人影院_中文字幕免费播放_亚洲精品一区二区三区在线看

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

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

　　3.1 建立正反解幾何模型

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

圖3 機器人機構簡圖

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

(1)

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

(2)

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

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

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

(4)

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

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

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