3 位置正反解模塊的實現(xiàn)(Realization of the forward and inverse displacement modules)
上述功能中,不同于傳統(tǒng)數(shù)控系統(tǒng)之處在于系統(tǒng)中需要根據(jù)機械結構實際的機構模型嵌入特定的位姿正反解模型.因此,建立正確的正反解模型至關重要.
3.1 建立正反解幾何模型
機器人機構簡圖如圖3所示,在本文所述的機構中,將光軸位姿參數(shù)(虛軸坐標)換算到驅動坐標位置(實軸坐標)稱為反解運算.
圖3 機器人機構簡圖
設定L2分支中繞X 軸旋轉虎克鉸的旋轉角度為α,繞Y軸旋轉虎克鉸的旋轉角度為β,3個分支的桿長分別為L1、L2、L3,串聯(lián)關節(jié)回轉轉動副和俯仰轉動副的轉角分別為γ和θ ,則由位姿坐標變換可得到:
(1)
已知機器人末端執(zhí)行器相對于固定參考坐標系O-XYZ 的位姿矩陣:
(2)
求解此矩陣方程可得α、β 、γ 、θ 、.
根據(jù)求解得到的α、β 和L2,將Ts2O 和TO1O求解出來.得到S2和O1在固定參考坐標系O-XYZ 中的坐標,則得到下式:
在固定平臺OB1B3中,OB1B3 為已知三角形,則很容易得到其他兩桿長度為:
(4)
正解過程與反解過程正好相反:已知機器人的關節(jié)變量L1、L2、L3和γ 、θ ,求解機器人末端位姿矩陣.
根據(jù)關節(jié)變量L1、L2和L3 ,以及B1、O、B3 點坐標,列距離方程可以求解到L2分支繞X 軸和繞Y軸的旋轉角度α和β .其求解方程如下:
根據(jù)得到的α和β ,將已知的L2、γ 、θ 代入到位姿變化矩陣,即可將機器人的末端位姿矩陣求解出來,完
轉載請注明出處。