用平面二連桿機(jī)器人為例貫穿運(yùn)動(dòng)學(xué)、雅可比、動(dòng)力學(xué)、軌跡規(guī)劃甚至控制及編程

.PAGE.一、平面二連桿機(jī)器人手臂運(yùn)動(dòng)學(xué)平面二連桿機(jī)械手臂如圖1所示，連桿1長度，連桿2長度。建立如圖1所示的坐標(biāo)系，其中，為根底坐標(biāo)系，固定在基座上，、為連體坐標(biāo)系，分別固結(jié)在連桿1和連桿2上并隨它們一起運(yùn)動(dòng)。關(guān)節(jié)角順時(shí)針為負(fù)逆時(shí)針為正。112PABCD圖1平面雙連桿機(jī)器人示意圖1、用簡單的平面幾何關(guān)系建立運(yùn)動(dòng)學(xué)方程連桿2末段與中線交點(diǎn)處一點(diǎn)P在根底坐標(biāo)系中的位置坐標(biāo)：〔1〕2、用D-H方法建立運(yùn)動(dòng)學(xué)方程假定、、垂直于紙面向里。從到的齊次旋轉(zhuǎn)變換矩陣為：〔2〕從到的齊次旋轉(zhuǎn)變換矩陣為：〔3〕從到的齊次旋轉(zhuǎn)變換矩陣為：〔4〕那么，連桿2末段與中線交點(diǎn)處一點(diǎn)P在根底坐標(biāo)系中的位置矢量為：〔5〕即，〔6〕與用簡單的平面幾何關(guān)系建立運(yùn)動(dòng)學(xué)方程〔1〕一樣。建立以上運(yùn)動(dòng)學(xué)方程后，假設(shè)個(gè)連桿的關(guān)節(jié)角，就可以用運(yùn)動(dòng)學(xué)方程求出機(jī)械手臂末端位置坐標(biāo)，這可以用于運(yùn)動(dòng)學(xué)仿真。3、平面二連桿機(jī)器人手臂逆運(yùn)動(dòng)學(xué)建立以上運(yùn)動(dòng)學(xué)方程后，假設(shè)個(gè)機(jī)械臂的末端位置，可以用運(yùn)動(dòng)學(xué)方程求出機(jī)械手臂二連桿的關(guān)節(jié)角，這叫機(jī)械臂的逆運(yùn)動(dòng)學(xué)。逆運(yùn)動(dòng)學(xué)可以用于對(duì)機(jī)械臂關(guān)節(jié)角和末端位置的控制。對(duì)于本例中平面二連桿機(jī)械臂，其逆運(yùn)動(dòng)學(xué)方程的建立就是末端位置求相應(yīng)關(guān)節(jié)角的過程。推倒如下?！?〕問題末端位置坐標(biāo)，求關(guān)節(jié)角。〔2〕求由〔6〕式得到：〔7〕整理得到：〔8〕令〔9〕由〔8〕式得到：〔10〕由此可解出。〔11〕〔3〕求由〔6〕式得到：〔12〕整理得到：〔13〕令〔14〕由〔14〕式得到：〔15〕由此可解出?！?6〕二、平面二連桿機(jī)器人手臂的速度雅可比矩陣速度雅可比矩陣的定義：從關(guān)節(jié)速度向末端操作速度的線性變換?，F(xiàn)已二連桿平面機(jī)器人為例推導(dǎo)速度雅可比矩陣。上面的運(yùn)動(dòng)學(xué)方程兩邊對(duì)時(shí)間求導(dǎo)，得到下面的速度表達(dá)式：〔17〕把上式寫成如下的矩陣形式：〔18〕令上式中的末端位置速度矢量，關(guān)節(jié)角速度矢量，矩陣就是速度雅可比矩陣，實(shí)現(xiàn)從關(guān)節(jié)角速度向末端位置速度的轉(zhuǎn)變?！?8〕式可以寫成：速度雅可比矩陣可以進(jìn)一步寫成：〔19〕其中，〔20〕由此可知雅可比矩陣的定義：〔21〕三、平面二連桿機(jī)器人手臂的動(dòng)力學(xué)方程推倒動(dòng)力學(xué)方程的方法很多，各有優(yōu)缺點(diǎn)。拉格朗日方法思路清晰、不考慮連桿之間的力，是推倒動(dòng)力學(xué)方程的常用方法。下面推導(dǎo)圖1所示的平面雙連桿機(jī)器人的動(dòng)力學(xué)方程。圖1中所示連桿均為均質(zhì)桿，其轉(zhuǎn)動(dòng)慣量分別是和。1、求兩連桿的拉格朗日函數(shù)〔1〕求系統(tǒng)總動(dòng)能連桿1的動(dòng)能為：〔21〕求連桿2質(zhì)心D處的線速度：對(duì)連桿2質(zhì)心位置求導(dǎo)得到其線速度。連桿2質(zhì)心位置為：〔22〕連桿2質(zhì)心速度為：〔23〕〔24〕連桿2的動(dòng)能：(25)系統(tǒng)總動(dòng)能：〔26〕〔2〕求系統(tǒng)總勢能系統(tǒng)總勢能為：〔27〕〔3〕求拉格朗日函數(shù)〔28〕〔4〕列寫動(dòng)力學(xué)方程按照拉格朗日方程，對(duì)應(yīng)關(guān)節(jié)1、2的驅(qū)動(dòng)力矩分別為：〔29〕〔30〕同理：〔31〕聯(lián)合〔30〕、〔31〕式，將動(dòng)力學(xué)方程寫成如下矩陣形式：〔32〕四、平面二連桿機(jī)器人手臂的軌跡規(guī)劃軌跡規(guī)劃就是起點(diǎn)和終點(diǎn)的位置速度加速度等參數(shù)確定中間點(diǎn)的相應(yīng)參數(shù)的過程。軌跡規(guī)劃是機(jī)器人完成規(guī)定任務(wù)所必需的。它分為關(guān)節(jié)空間的軌跡規(guī)劃和直角坐標(biāo)空間的軌跡規(guī)劃、以及基于動(dòng)力學(xué)的軌跡規(guī)劃等幾種類型。關(guān)節(jié)空間的軌跡規(guī)劃就是某連桿起點(diǎn)和終點(diǎn)的角位置角速度角加速度等參數(shù)確定中間點(diǎn)的相應(yīng)參數(shù)的過程。如下圖，一兩自由度機(jī)械手，兩連桿起點(diǎn)和終點(diǎn)的關(guān)節(jié)角，確定中間位置的關(guān)節(jié)角?！?〕非歸一化和歸一化問題〔2〕末端位置的軌跡、關(guān)節(jié)空間軌跡規(guī)劃的缺點(diǎn)。三次多項(xiàng)式軌跡規(guī)劃舉例：要求一個(gè)５軸機(jī)器人的第一關(guān)節(jié)在５秒之從初始角３０度運(yùn)動(dòng)到終端角７５度，用三次多項(xiàng)式計(jì)算在第１、２、３、４秒時(shí)的關(guān)節(jié)角。五次多項(xiàng)式軌跡規(guī)劃拋物線過渡的線性運(yùn)動(dòng)軌跡規(guī)劃〔略〕具有中間點(diǎn)以及用拋物線過渡的線性運(yùn)動(dòng)軌跡規(guī)劃〔略〕高次多項(xiàng)式運(yùn)動(dòng)軌跡規(guī)劃〔略〕直角坐標(biāo)空間的軌跡規(guī)劃〔1〕所有用于關(guān)節(jié)空間的軌跡規(guī)劃方法都可以用于直角坐標(biāo)空間軌跡規(guī)劃；