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

1、一、平面二連桿機(jī)器人手臂運(yùn)動(dòng)學(xué) 平面二連桿機(jī)械手臂如圖1所示，連桿1長(zhǎng)度，連桿2長(zhǎng)度。建立如圖1所示的坐標(biāo)系，其中，為基礎(chǔ)坐標(biāo)系，固定在基座上，、為連體坐標(biāo)系，分別固結(jié)在連桿1和連桿2上并隨它們一起運(yùn)動(dòng)。關(guān)節(jié)角順時(shí)針為負(fù)逆時(shí)針為正。 1 2 P ABCD 圖1平面雙連桿機(jī)器人示意圖1、用簡(jiǎn)單的平面幾何關(guān)系建立運(yùn)動(dòng)學(xué)方程連桿2末段與中線交點(diǎn)處一點(diǎn)P在基礎(chǔ)坐標(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在基礎(chǔ)坐標(biāo)系中的位置矢量為：

2、（5）即， （6）與用簡(jiǎn)單的平面幾何關(guān)系建立運(yùn)動(dòng)學(xué)方程（1）相同。建立以上運(yùn)動(dòng)學(xué)方程后，若已知個(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é)方程后，若已知個(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é)角的過(guò)程。推倒如下。（1）問(wèn)題已知末端位置坐標(biāo)，求關(guān)節(jié)角。（2）求由（6）式得到： （7）整理得到： （8）令 （9）由（8）式得到： （10）由此可解出。

3、 （11）（3）求由（6）式得到： （12）整理得到： （13）令 （14）由（14）式得到： （15）由此可解出。 （16）二、平面二連桿機(jī)器人手臂的速度雅可比矩陣速度雅可比矩陣的定義：從關(guān)節(jié)速度向末端操作速度的線性變換?，F(xiàn)已二連桿平面機(jī)器人為例推導(dǎo)速度雅可比矩陣。 上面的運(yùn)動(dòng)學(xué)方程兩邊對(duì)時(shí)間求導(dǎo)，得到下面的速度表達(dá)式： （17）把上式寫(xiě)成如下的矩陣形式： （18）令上式中的末端位置速度矢量，關(guān)節(jié)角速度矢量， 矩陣就是速度雅可比矩陣，實(shí)現(xiàn)從關(guān)節(jié)角速度向末端位置速度的轉(zhuǎn)變。（18）式可以寫(xiě)成：速度雅可比矩陣可以進(jìn)一步寫(xiě)成： （19）其中， （20）由此可知雅可比矩陣的定義： （21）三、平面二

4、連桿機(jī)器人手臂的動(dòng)力學(xué)方程推倒動(dòng)力學(xué)方程的方法很多，各有優(yōu)缺點(diǎn)。拉格朗日方法思路清晰、不考慮連桿之間的內(nèi)力，是推倒動(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)總勢(shì)能 系統(tǒng)總勢(shì)能為： （27）（3）求拉格朗日函數(shù) （28）（4）列寫(xiě)動(dòng)力學(xué)方程 按照拉格朗日方程，對(duì)應(yīng)關(guān)節(jié)1、2

5、的驅(qū)動(dòng)力矩分別為： （29） （30） 同理： （31）聯(lián)合（30）、（31）式，將動(dòng)力學(xué)方程寫(xiě)成如下矩陣形式： （32）四、平面二連桿機(jī)器人手臂的軌跡規(guī)劃軌跡規(guī)劃就是已知起點(diǎn)和終點(diǎn)的位置速度加速度等參數(shù)確定中間點(diǎn)的相應(yīng)參數(shù)的過(guò)程。軌跡規(guī)劃是機(jī)器人完成規(guī)定任務(wù)所必需的。它分為關(guān)節(jié)空間的軌跡規(guī)劃和直角坐標(biāo)空間的軌跡規(guī)劃、以及基于動(dòng)力學(xué)的軌跡規(guī)劃等幾種類(lèi)型。關(guān)節(jié)空間的軌跡規(guī)劃就是已知某連桿起點(diǎn)和終點(diǎn)的角位置角速度角加速度等參數(shù)確定中間點(diǎn)的相應(yīng)參數(shù)的過(guò)程。如圖所示，一兩自由度機(jī)械手，已知兩連桿起點(diǎn)和終點(diǎn)的關(guān)節(jié)角，確定中間位置的關(guān)節(jié)角。（1）非歸一化和歸一化問(wèn)題（2）末端位置的軌跡、關(guān)節(jié)空間軌跡規(guī)劃的缺點(diǎn)。 三次多項(xiàng)式軌跡規(guī)劃 舉例：要求一個(gè)軸機(jī)器人的第一關(guān)節(jié)在秒之內(nèi)從初始角度運(yùn)動(dòng)到終端角度，用三次多項(xiàng)式計(jì)算在第、秒時(shí)的關(guān)節(jié)角。五次多項(xiàng)式軌跡規(guī)劃拋物線過(guò)渡的線性運(yùn)動(dòng)軌跡規(guī)劃（略）具有中間點(diǎn)以及用拋物線過(guò)渡的線性運(yùn)動(dòng)軌跡規(guī)劃（略）高次多項(xiàng)式運(yùn)動(dòng)軌跡規(guī)劃（略）直角坐標(biāo)空間的軌跡規(guī)劃（1）所有用于關(guān)節(jié)空間的軌跡規(guī)劃方法都可以用于直角坐標(biāo)空間軌跡規(guī)劃；（2）直角坐標(biāo)軌跡規(guī)劃必須不斷進(jìn)行逆運(yùn)動(dòng)學(xué)運(yùn)算，以便及時(shí)得到關(guān)節(jié)角。這個(gè)過(guò)程可以歸納為以下計(jì)算循環(huán)：（a）將