【文章內(nèi)容簡介】 中采用的機(jī)械臂而言有五個(gè)其次變換矩陣，則末端連桿坐標(biāo)系相對(duì)于基坐標(biāo)系的齊次變換矩陣 (22) 式即為機(jī)械臂的運(yùn)動(dòng)方程，它反應(yīng)各關(guān)節(jié)變量與機(jī)械臂末端位姿之間的關(guān)系，上式左邊的五個(gè)矩陣含有五個(gè)關(guān)節(jié)變量。方程右邊為描述機(jī)械臂末端關(guān)節(jié)位置和姿態(tài)的齊次矩陣，由剛體姿態(tài)的描述可知，，，，分別為機(jī)械臂末關(guān)節(jié)坐標(biāo)系的三個(gè)坐標(biāo)軸與機(jī)械臂基坐標(biāo)系三個(gè)坐標(biāo)軸的方向余弦，，為機(jī)械臂末關(guān)節(jié)的坐標(biāo)原點(diǎn)在機(jī)械臂基坐標(biāo)系中的三維坐標(biāo)。 機(jī)械臂正運(yùn)動(dòng)學(xué)求解就是已知各連桿的關(guān)節(jié)變量求解末端連桿的位姿矩陣。即已知關(guān)節(jié)變量，求解上式機(jī)械臂運(yùn)動(dòng)學(xué)方程中等式右邊矩陣各元素的值[10]。 將上式中的機(jī)械臂五個(gè)關(guān)節(jié)的齊次變換矩陣帶入，即計(jì)算出中各元素值為： (23)其中：其中，。第二節(jié) 機(jī)械臂的逆運(yùn)動(dòng)學(xué)求解 機(jī)械臂的逆運(yùn)動(dòng)學(xué)解是對(duì)其運(yùn)動(dòng)學(xué)正解的反解，因而已知量和求解量相反，即已知機(jī)械臂末端的位置姿態(tài)對(duì)機(jī)械臂進(jìn)行驅(qū)動(dòng)，使各個(gè)關(guān)節(jié)從此刻的姿態(tài)運(yùn)動(dòng)到與末端位姿相對(duì)應(yīng)的位置，進(jìn)而得到關(guān)節(jié)變量[11]。 機(jī)械臂的運(yùn)動(dòng)學(xué)正、逆求解實(shí)質(zhì)是機(jī)械臂關(guān)節(jié)空間與工作空間之間的非線性映射關(guān)系，兩者可相互轉(zhuǎn)換。關(guān)系圖如下所示。圖1 關(guān)節(jié)空間與工作空間的關(guān)系 機(jī)械臂的逆運(yùn)動(dòng)學(xué)問題，指已知機(jī)械臂的末端位姿，即已知齊次變換矩陣，求解各轉(zhuǎn)動(dòng)關(guān)節(jié)的角度。 機(jī)械臂的逆運(yùn)行學(xué)問題，可以理解為通過運(yùn)動(dòng)學(xué)方程： (24)求解。 整理式，將含有的部分移到方程的左邊 (25) 將轉(zhuǎn)置，上式可以表達(dá)成為： (26) 假設(shè)上式的兩邊元素和式相等，得到： (27)可以得出的解。第三章 五軸機(jī)械臂軌跡規(guī)劃與仿真目前關(guān)于空間軌跡規(guī)劃的方法主要有三種，三次多項(xiàng)式插值，高階多項(xiàng)式插值以及樣條曲線等方法。主要討論軌跡在關(guān)節(jié)空間中的位移、速度與加速度等變量的關(guān)系。規(guī)劃實(shí)質(zhì)是根據(jù)需求，計(jì)算出預(yù)定的軌跡曲線，在軌跡規(guī)劃時(shí)可以再運(yùn)動(dòng)學(xué)與動(dòng)力學(xué)的基礎(chǔ)上進(jìn)行規(guī)劃，所以規(guī)劃是建立在運(yùn)動(dòng)學(xué)和動(dòng)力學(xué)基礎(chǔ)上的。圖2 機(jī)械臂的matlab生成第一節(jié) 軌跡規(guī)劃一般問題 軌跡規(guī)劃的一般方法是在機(jī)械臂末端的初始和目標(biāo)位置之間用多項(xiàng)式函數(shù)“內(nèi)插”來抵近給定的路徑，并沿著時(shí)間軸產(chǎn)生一系列的可供操作機(jī)使用的“控制設(shè)定點(diǎn)”[3]。其中關(guān)節(jié)坐標(biāo)和笛卡爾坐標(biāo)都可以對(duì)路徑端點(diǎn)進(jìn)行給出。一般是在笛卡爾坐標(biāo)中給出，由于在笛卡爾坐標(biāo)中機(jī)械臂末端形態(tài)更容易觀察。所以通常采用笛卡爾方法。在給定的兩端之間，常有多條可能路徑?？梢匝刂本€和光滑多項(xiàng)式曲線運(yùn)動(dòng)。本文將討論插值法，研究滿足路徑約束的簡單軌跡規(guī)劃[3]。第二節(jié) 關(guān)節(jié)空間的軌跡規(guī)劃 ，算法簡單、工具移動(dòng)效率高、關(guān)節(jié)空間與直角坐標(biāo)空間連續(xù)的對(duì)應(yīng)關(guān)系是不存在的，因此機(jī)構(gòu)的奇異性問題一般不會(huì)發(fā)生。對(duì)于無路徑的要求，應(yīng)盡量在關(guān)節(jié)空間進(jìn)行軌跡規(guī)劃。一、三次多項(xiàng)式插值法 三次多項(xiàng)式與其一階導(dǎo)數(shù)函數(shù)，總計(jì)有四個(gè)待定系數(shù)，對(duì)起始點(diǎn)和目標(biāo)點(diǎn)兩者的角度、角角速度同時(shí)給出約束條件，本文采用的是三次多項(xiàng)式插值法[5]?？梢詫?duì)通過空間的個(gè)點(diǎn)進(jìn)行分析并進(jìn)行軌跡規(guī)劃，讓速度和加速度在運(yùn)動(dòng)過程中保持軌跡平滑。本文算法可以實(shí)現(xiàn)對(duì)段中的每一段三次多項(xiàng)式系數(shù)求解，為了方便，對(duì)其進(jìn)行歸一化處理。 (1)時(shí)間標(biāo)準(zhǔn)化算法 根據(jù)三次多項(xiàng)式軌跡規(guī)劃流程，對(duì)每個(gè)關(guān)節(jié)進(jìn)行軌跡規(guī)劃時(shí)需要對(duì)段的軌跡進(jìn)行設(shè)計(jì)，為了能對(duì)個(gè)軌跡規(guī)劃方程進(jìn)行同樣處理，本文首先設(shè)計(jì)了時(shí)間標(biāo)準(zhǔn)化算法將時(shí)間進(jìn)行處理，經(jīng)過處理后的時(shí)間。 首先定義： ：標(biāo)準(zhǔn)化時(shí)間變量，； ：未標(biāo)準(zhǔn)化時(shí)間，單位為秒； ：第段軌跡規(guī)劃結(jié)束的未標(biāo)準(zhǔn)化時(shí)間，； 機(jī)械臂執(zhí)行第段軌跡所需要的實(shí)際時(shí)間：，其中。 時(shí)間歸一化后的三次多項(xiàng)式為： (2)機(jī)械臂軌跡規(guī)劃算法實(shí)現(xiàn)過程 ①已知初始位置為； ②給定初始速度為0； ③已知第一個(gè)中間點(diǎn)位置，它也是第一運(yùn)動(dòng)段三次多項(xiàng)式軌跡的終點(diǎn)； ④為了保證運(yùn)動(dòng)的連續(xù)性，需要設(shè)定所在點(diǎn)為三次多項(xiàng)式軌跡的起點(diǎn)，以確保運(yùn)動(dòng)的連續(xù)； ⑤為了保證處速度連續(xù)，三次多項(xiàng)式在處一階可導(dǎo)； ⑥為了保證處加速度連續(xù)，三次多項(xiàng)式在處二階可導(dǎo)； ⑦以此類推，每一個(gè)中間點(diǎn)的位置，都一定要在其原運(yùn)動(dòng)段軌跡的終點(diǎn)，并且也是它后運(yùn)動(dòng)段的起點(diǎn)。 ⑧的速度保持連續(xù)； ⑨的加速度保持連續(xù)； ⑩點(diǎn)位置。給定終點(diǎn)速度，設(shè)其為0。 (3)約束條件 第一個(gè)三次曲線為： 第二個(gè)三次曲線為： 第三個(gè)三次曲線為： ．．．．．． 第個(gè)三次曲線為： 在同一時(shí)間段內(nèi)，三次曲線每次的起始時(shí)刻，停止時(shí)刻，其中。 ①在標(biāo)準(zhǔn)化時(shí)間處，設(shè)定為第一條三次多項(xiàng)式運(yùn)動(dòng)段的起點(diǎn)，可以得出：； ②在標(biāo)準(zhǔn)化時(shí)間處，三次多項(xiàng)式運(yùn)動(dòng)段第一條的初始速度是已知變量，所以得出：； ③第一中間點(diǎn)位置與第一條三次多項(xiàng)式運(yùn)動(dòng)段在標(biāo)準(zhǔn)化時(shí)間時(shí)的終點(diǎn)相同，所以可以得出：； ④第一中間點(diǎn)位置與第一運(yùn)動(dòng)段在標(biāo)準(zhǔn)化時(shí)間時(shí)起點(diǎn)相同，所以得出：； ⑤三次多項(xiàng)式在處一階可導(dǎo)，因此可得出：； ⑥三次多項(xiàng)式在處二階可導(dǎo)，因此可得出：； ⑦第二個(gè)空間點(diǎn)的位置與第二運(yùn)動(dòng)段在標(biāo)準(zhǔn)化時(shí)間時(shí)的終點(diǎn)相同，所以有：； ⑧第二個(gè)中間點(diǎn)的位置應(yīng)與第三運(yùn)動(dòng)段在標(biāo)準(zhǔn)化時(shí)間時(shí)起點(diǎn)相同，所以有：； ⑨三次多項(xiàng)式在處一階可導(dǎo)，從而有：； ⑩三次多項(xiàng)式在處二階可導(dǎo)，從而有：； ．．．．．． ?第個(gè)中間點(diǎn)位置和第運(yùn)動(dòng)段在標(biāo)準(zhǔn)化時(shí)間時(shí)的終點(diǎn)相同，所以有：。 ?第個(gè)中間點(diǎn)位置應(yīng)與下一運(yùn)動(dòng)段在標(biāo)準(zhǔn)化時(shí)間時(shí)的起點(diǎn)位置相同，所以有； ?三次多項(xiàng)式在第個(gè)中間點(diǎn)處一階可導(dǎo)，從而： (31) ?三次多項(xiàng)式在第個(gè)中間點(diǎn)處二階可導(dǎo)，從而： (32) ?因此可以得出所有軌跡終點(diǎn)在標(biāo)準(zhǔn)化時(shí)間時(shí)的位置為： (33) ?因此可以得出所有軌跡終點(diǎn)在標(biāo)準(zhǔn)化時(shí)間時(shí)的速度為： (34) 以上公式改寫為矩陣為：。由該矩陣計(jì)算可以求出軌跡規(guī)劃的全部參數(shù)，(由五軸機(jī)械臂運(yùn)動(dòng)學(xué)逆解求出)于是求得段的運(yùn)動(dòng)方程，從而使五軸機(jī)械臂末端執(zhí)行器經(jīng)過所給定的位置坐標(biāo)。 通過以上分析可以確定機(jī)械臂在滿足速度要求的兩個(gè)位姿之間運(yùn)動(dòng)時(shí)各個(gè)關(guān)節(jié)軸的角度變化曲線。如下圖3所示是MATLAB仿真分析三次多項(xiàng)式插值：機(jī)械臂某關(guān)節(jié)角在4秒內(nèi)由初始點(diǎn)A經(jīng)過中間點(diǎn)B到達(dá)終點(diǎn)C的變化情況。三個(gè)位置點(diǎn)的速度和角速度如下所示： 圖中實(shí)線為角度變化曲線，虛線為角速度變化曲線。關(guān)節(jié)角度曲線平滑，而速度曲線在中間點(diǎn)B處出現(xiàn)突變。圖3 三次多項(xiàng)式插值法二、五次多項(xiàng)式插值 五次多項(xiàng)式共有六個(gè)待定系數(shù)，要想六個(gè)系數(shù)得到確定，至少需要六個(gè)條件。五次多項(xiàng)式可以看作是關(guān)節(jié)角度的時(shí)間函數(shù)，因此其一階可導(dǎo)和二階可導(dǎo)分別可以看作是關(guān)節(jié)角速度和關(guān)節(jié)角加速度的時(shí)間函數(shù)。五次多項(xiàng)式及一階、二階導(dǎo)數(shù)公式如下： (35) (36)