本發(fā)明涉及一種工業(yè)機(jī)器人應(yīng)用的過(guò)渡軌跡規(guī)劃方法。

背景技術(shù)：
工業(yè)機(jī)器人已經(jīng)被廣泛應(yīng)用于多個(gè)領(lǐng)域，在現(xiàn)代工業(yè)自動(dòng)化、智能化的發(fā)展中發(fā)揮著越來(lái)越突出的作用。運(yùn)動(dòng)軌跡規(guī)劃是機(jī)器人控制系統(tǒng)的一項(xiàng)基本任務(wù)，工業(yè)機(jī)器人的運(yùn)動(dòng)軌跡通常有笛卡爾空間的直線、圓弧，以及關(guān)節(jié)空間的點(diǎn)到點(diǎn)運(yùn)動(dòng)軌跡。實(shí)際應(yīng)用場(chǎng)合中，一般是由多條軌跡順序銜接完成一項(xiàng)加工任務(wù)，然而，相鄰兩條軌跡在銜接處存在拐角，而許多應(yīng)用要求軌跡平滑沒(méi)有拐角，即末端點(diǎn)從一條軌跡平滑過(guò)渡到下一條軌跡。兩條軌跡之間的切換軌跡稱(chēng)為過(guò)渡軌跡。目前，對(duì)過(guò)渡段的軌跡規(guī)劃有部分研究，常以圓弧、多項(xiàng)式曲線來(lái)構(gòu)造過(guò)渡軌跡。其中，圓弧過(guò)渡可以保證軌跡平滑、速度勻速，有較廣泛的應(yīng)用，如：中國(guó)發(fā)明專(zhuān)利“一種焊接機(jī)器人過(guò)渡軌跡規(guī)劃方法”(申請(qǐng)?zhí)枮?01110000264.3)，就是針對(duì)焊接機(jī)器人，過(guò)渡段焊縫采用圓弧來(lái)銜接直線段焊縫和圓弧段焊縫，但圓弧過(guò)渡在過(guò)渡段與軌跡段銜接處存在加速度的躍變，影響軌跡的平滑性，可能導(dǎo)致機(jī)械振動(dòng)，且圓弧過(guò)渡無(wú)法實(shí)現(xiàn)相鄰兩段僅有姿態(tài)變化的軌跡過(guò)渡，對(duì)其他有此需求的應(yīng)用場(chǎng)合就不適用了；多項(xiàng)式曲線過(guò)渡，包括樣條函數(shù)、Hermite函數(shù)、五次多項(xiàng)式等，借助高階連續(xù)性的特性可以保證加速度的平滑，減少振動(dòng)，有利于高速運(yùn)動(dòng)，其中五次多項(xiàng)式過(guò)渡算法的研究較多，如：文獻(xiàn)《機(jī)械手笛卡爾空間軌跡規(guī)劃研究[J].》(林仕高,劉曉麟,歐元賢《機(jī)械設(shè)計(jì)與制造》,2013(3):49-52)對(duì)位姿6個(gè)自由度分別用五次多項(xiàng)式構(gòu)造過(guò)渡曲線，不僅能保證軌跡、速度、加速度的平滑，且能完成僅有姿態(tài)變化的軌跡間的過(guò)渡，但五次曲線的幾何形狀難以控制，而且缺乏工程約束條件。以上研究是基于笛卡爾空間兩條軌跡之間的軌跡過(guò)渡，對(duì)關(guān)節(jié)空間軌跡到笛卡爾空間軌跡的過(guò)渡研究較少，而不同軌跡之間的不停頓運(yùn)動(dòng)對(duì)于提高工作效率是很有必要的。

技術(shù)實(shí)現(xiàn)要素：
本發(fā)明的目的在于，克服現(xiàn)有技術(shù)存在的缺陷，提出了一種工業(yè)機(jī)器人應(yīng)用的過(guò)渡軌跡規(guī)劃方法，構(gòu)造一條能保證軌跡、速度、加速度平滑的過(guò)渡曲線，實(shí)現(xiàn)關(guān)節(jié)空間軌跡與笛卡爾空間軌跡的過(guò)渡、笛卡爾空間兩條軌跡的過(guò)渡，對(duì)于僅姿態(tài)變化的軌跡也能實(shí)現(xiàn)過(guò)渡，過(guò)渡軌跡形狀能夠受控。同時(shí)從工程應(yīng)用角度，對(duì)過(guò)渡段的邊界路徑速度和姿態(tài)旋轉(zhuǎn)速度進(jìn)行約束。本發(fā)明的基本技術(shù)方案包括以下步驟：步驟1：將過(guò)渡軌跡規(guī)劃需要的運(yùn)動(dòng)參數(shù)導(dǎo)入機(jī)器人過(guò)渡軌跡規(guī)劃模塊機(jī)器人末端點(diǎn)位姿由位置矢量(x,y,z)和RPY姿態(tài)矢量(α,β,γ)共同描述，組合成一個(gè)6自由度的復(fù)合矢量(x,y,z,α,β,γ)，過(guò)渡軌跡規(guī)劃涉及的運(yùn)動(dòng)參數(shù)有：第一條軌跡起點(diǎn)位姿P0、終點(diǎn)位姿P1，第二條軌跡終點(diǎn)位姿P2，過(guò)渡參數(shù)百分比a，工程約束條件包括：系統(tǒng)最大速度Vmax、系統(tǒng)最大加速度Amax、系統(tǒng)允許的最大弓高誤差Emax。步驟2：確定軌跡P0P1與軌跡P1P2之間過(guò)渡軌跡的起點(diǎn)和終點(diǎn)的位姿當(dāng)軌跡P0P1是笛卡爾空間的直線軌跡時(shí)，過(guò)渡起點(diǎn)Ps到拐點(diǎn)P1的直線長(zhǎng)度是軌跡P0P1直線長(zhǎng)度的一半乘以過(guò)渡參數(shù)百分比a；當(dāng)軌跡P0P1是笛卡爾空間的圓弧軌跡時(shí)，過(guò)渡起點(diǎn)Ps到拐點(diǎn)P1的弧長(zhǎng)是軌跡P0P1弧長(zhǎng)的一半乘以過(guò)渡參數(shù)百分比a；設(shè)定過(guò)渡起點(diǎn)Ps到拐點(diǎn)P1的RPY姿態(tài)矢量變化對(duì)應(yīng)軌跡P0P1的RPY姿態(tài)矢量變化的一半乘以過(guò)渡參數(shù)百分比a。當(dāng)軌跡P0P1是關(guān)節(jié)空間軌跡時(shí)，設(shè)定過(guò)渡起點(diǎn)Ps到拐點(diǎn)P1的各關(guān)節(jié)位置變化對(duì)應(yīng)軌跡P0P1各關(guān)節(jié)位置變化的一半乘以過(guò)渡參數(shù)百分比a，進(jìn)一步根據(jù)正向運(yùn)動(dòng)學(xué)計(jì)算出過(guò)渡起點(diǎn)Ps的位姿。當(dāng)軌跡P1P2是笛卡爾空間的直線軌跡時(shí)，拐點(diǎn)P1到過(guò)渡終點(diǎn)Pe的直線長(zhǎng)度是軌跡P1P2直線長(zhǎng)度的一半乘以過(guò)渡參數(shù)百分比a；當(dāng)軌跡P1P2是笛卡爾空間的圓弧軌跡時(shí)，拐點(diǎn)P1到過(guò)渡終點(diǎn)Pe的弧長(zhǎng)是軌跡P1P2弧長(zhǎng)的一半乘以過(guò)渡參數(shù)百分比a；設(shè)定拐點(diǎn)P1到過(guò)渡終點(diǎn)Pe的RPY姿態(tài)矢量變化對(duì)應(yīng)軌跡P1P2的RPY姿態(tài)矢量變化的一半乘以過(guò)渡參數(shù)百分比a。當(dāng)軌跡P1P2是關(guān)節(jié)空間軌跡時(shí)，設(shè)定拐點(diǎn)P1到過(guò)渡終點(diǎn)Pe的各關(guān)節(jié)位置變化對(duì)應(yīng)軌跡P1P2各關(guān)節(jié)位置變化的一半乘以過(guò)渡參數(shù)百分比a，進(jìn)一步根據(jù)正向運(yùn)動(dòng)學(xué)計(jì)算出過(guò)渡終點(diǎn)Pe的位姿。步驟3：確定過(guò)渡軌跡的邊界速度由外部軟件模塊機(jī)器人的加減速軌跡規(guī)劃算法，輸入軌跡P0P1在過(guò)渡起點(diǎn)Ps處的速度，軌跡P1P2在過(guò)渡終點(diǎn)Pe處的速度，為簡(jiǎn)化計(jì)算，約定過(guò)渡軌跡起、終點(diǎn)的邊界速度相等。笛卡爾空間軌跡對(duì)應(yīng)末端點(diǎn)的路徑速度Vpath、末端點(diǎn)的姿態(tài)旋轉(zhuǎn)速度Vori，關(guān)節(jié)空間軌跡對(duì)應(yīng)各關(guān)節(jié)的速度，此情況下，借助雅可比矩陣，可由各關(guān)節(jié)的速度直接計(jì)算出機(jī)器人末端點(diǎn)位姿6個(gè)自由度的速度。步驟4：利用弓高誤差來(lái)約束過(guò)渡軌跡的邊界速度從工程應(yīng)用角度，當(dāng)相鄰軌跡切向速度矢量的夾角很大時(shí)，過(guò)大的軌跡銜接速度會(huì)對(duì)機(jī)械系統(tǒng)產(chǎn)生較大的沖擊，因此有必要對(duì)軌跡銜接速度進(jìn)行約束。本發(fā)明利用工程上的弓高誤差參數(shù)來(lái)約束軌跡銜接速度，保證過(guò)渡軌跡邊界速度Vpath和Vori不超過(guò)允許的軌跡銜接速度，具體方法如下：假定軌跡P0P1與軌跡P1P2之間有個(gè)假想的小圓弧，通過(guò)弓高誤差來(lái)調(diào)整曲率半徑r，然后利用r對(duì)軌跡銜接速度進(jìn)行約束，從而達(dá)到約束過(guò)渡軌跡邊界速度Vpath和Vori的目的。假想圓弧只是用來(lái)獲取速度的約束條件，并不影響實(shí)際軌跡。由于關(guān)節(jié)空間軌跡不直觀，用過(guò)渡點(diǎn)與拐點(diǎn)之間的直線連線替代原軌跡計(jì)算拐點(diǎn)處的夾角。軌跡P0P1與軌跡P1P2在拐點(diǎn)P1處的切向速度矢量的夾角為θ，對(duì)路徑速度Vpath的約束公式如下：其中，Vmax為系統(tǒng)最大速度，Amax為系統(tǒng)最大加速度，Emax系統(tǒng)允許的弓高誤差參數(shù)，定義一個(gè)角度到毫米的量綱轉(zhuǎn)換系數(shù)λ，將姿態(tài)旋轉(zhuǎn)速度等效為一個(gè)長(zhǎng)度上的速度矢量λVori，參照路徑速度Vpath的約束方式，對(duì)λVori進(jìn)行約束，即可達(dá)到對(duì)姿態(tài)旋轉(zhuǎn)速度Vori的約束目的。軌跡P0P1與軌跡P1P2在拐點(diǎn)P1處的切向姿態(tài)旋轉(zhuǎn)速度矢量的夾角為θori，對(duì)姿態(tài)旋轉(zhuǎn)速度Vori的約束公式如下：其中，步驟5：將笛卡爾空間軌跡在過(guò)渡軌跡邊界點(diǎn)的速度Vpath和Vori分解到位姿6個(gè)自由度上若軌跡P0P1是笛卡爾空間軌跡，將過(guò)渡起點(diǎn)Ps處的速度Vpath和Vori分解到位姿6個(gè)自由度上；若軌跡P1P2是笛卡爾空間軌跡，將過(guò)渡終點(diǎn)Pe處的速度Vpath和Vori分解到位姿6個(gè)自由度上。對(duì)于關(guān)節(jié)空間軌跡，已在步驟3中求出過(guò)渡起點(diǎn)Ps和過(guò)渡終點(diǎn)Pe處的位姿6個(gè)自由度上的速度。笛卡爾空間軌跡分直線和圓弧兩種軌跡類(lèi)型，以下分別闡述速度Vpath和Vori的分解方法：對(duì)于直線軌跡類(lèi)型，根據(jù)直線軌跡在笛卡爾坐標(biāo)系中的單位向量，可直接將Vpath分解到各軸上；對(duì)于圓弧軌跡類(lèi)型，利用文獻(xiàn)《工業(yè)機(jī)器人笛卡爾空間軌跡規(guī)劃[J].》(機(jī)械工程與自動(dòng)化,2014(5):141-143)基于局部坐標(biāo)系的空間圓弧插補(bǔ)方案中的計(jì)算步驟可得出圓弧軌跡圓心點(diǎn)Oarc的位置坐標(biāo)、及局部坐標(biāo)系的Z軸的單位方向向量Z′，由向量Z′與圓心點(diǎn)Oarc指向圓弧上某點(diǎn)的單位方向向量O′叉乘可計(jì)算出該點(diǎn)處的單位速度切向矢量，即可將Vpath分解到各軸上。然后以直線的位置處理方式來(lái)等效處理姿態(tài)軌跡，將姿態(tài)旋轉(zhuǎn)速度Vori分解到姿態(tài)軸3個(gè)自由度上。步驟6：計(jì)算過(guò)渡軌跡起點(diǎn)Ps和終點(diǎn)Pe的位姿6個(gè)自由度上的加速度加速度由速度差分計(jì)算得到，步驟5已將邊界速度Vpath和Vori分解到6個(gè)自由度上，假定過(guò)渡起點(diǎn)Ps處的瞬間是勻速，過(guò)渡終點(diǎn)Pe處的瞬間是勻速，可計(jì)算過(guò)渡軌跡起點(diǎn)Ps和終點(diǎn)Pe的位姿6個(gè)自由度上的加速度。步驟7：計(jì)算過(guò)渡軌跡運(yùn)行時(shí)間T假定過(guò)渡軌跡的執(zhí)行時(shí)間與從過(guò)渡起點(diǎn)Ps勻速運(yùn)動(dòng)到拐點(diǎn)P1再勻速運(yùn)動(dòng)到過(guò)渡終點(diǎn)Pe的直線段時(shí)間相等，依此設(shè)定，分別計(jì)算位置過(guò)渡的運(yùn)行時(shí)間T1和姿態(tài)過(guò)渡的運(yùn)行時(shí)間T2，則T選取較長(zhǎng)的時(shí)間：若其中存在位置或姿態(tài)不過(guò)渡，只需將對(duì)應(yīng)的運(yùn)行時(shí)間置為零即可。步驟8：對(duì)機(jī)器人過(guò)渡軌跡末端點(diǎn)位姿的6個(gè)自由度分別構(gòu)造過(guò)渡曲線方程P(σ)，采用兩條拋物線運(yùn)動(dòng)的疊加融合為過(guò)渡軌跡的運(yùn)動(dòng)，矩陣方程如下：P(σ)＝P1(σ)+η(σ)[P2(σ)-P1(σ)](4)其中，σ是過(guò)渡時(shí)間t的參數(shù)化變量；P1(σ)是與過(guò)渡起點(diǎn)Ps相連的拋物線，是關(guān)于變量σ的二次函數(shù)，與第一段軌跡相切于過(guò)渡起點(diǎn)Ps；P2(σ)是與過(guò)渡終點(diǎn)Pe相連的拋物線，是關(guān)于變量σ的二次函數(shù)，與第二段軌跡相切于過(guò)渡終點(diǎn)Pe；η(σ)是保證曲線P1(σ)過(guò)渡到曲線P2(σ)的過(guò)渡函數(shù)，可設(shè)計(jì)η(σ)＝6σ5-15σ4+10σ3，以保證P(σ)邊界在軌跡、速度、加速度上連續(xù)。將過(guò)渡軌跡起點(diǎn)Ps和終點(diǎn)Pe的邊界條件：位姿、速度、加速度，帶入公式(4)的矩陣方程，即可確定過(guò)渡軌跡的位姿6個(gè)自由度的曲線方程。設(shè)計(jì)的過(guò)渡軌跡保證了軌跡、速度、加速度的平滑性，對(duì)于僅姿態(tài)變化的軌跡也能實(shí)現(xiàn)過(guò)渡，過(guò)渡曲線由兩條拋物線融合而成，曲線形狀可控。根據(jù)以上的技術(shù)方案，可以實(shí)現(xiàn)以下的有益效果：1)可實(shí)現(xiàn)關(guān)節(jié)空間軌跡與笛卡爾空間軌跡之間的過(guò)渡，以及笛卡爾空間兩條軌跡之間的過(guò)渡，笛卡爾空間軌跡包括：直線、圓??；過(guò)渡軌跡統(tǒng)一在笛卡爾空間內(nèi)規(guī)劃，曲線直觀，邊界條件求取只涉及正向運(yùn)動(dòng)學(xué)，避免了逆向運(yùn)動(dòng)學(xué)的多解問(wèn)題。2)過(guò)渡曲線由兩條拋物線融合而成，保證了軌跡、速度、加速度的平滑性，曲線形狀可控；過(guò)渡軌跡由獨(dú)立的6條曲線構(gòu)成，對(duì)于無(wú)位置變化僅有姿態(tài)變化的軌跡也能實(shí)現(xiàn)過(guò)渡。3)從工程應(yīng)用角度對(duì)過(guò)渡軌跡的邊界路徑速度和姿態(tài)旋轉(zhuǎn)速度進(jìn)行約束，防止當(dāng)相鄰軌跡的夾角很大時(shí)，過(guò)大的銜接速度可能會(huì)對(duì)機(jī)械系統(tǒng)產(chǎn)生較大的沖擊。關(guān)鍵點(diǎn)：1)笛卡爾空間軌跡與關(guān)節(jié)空間軌跡的銜接統(tǒng)一處理成在笛卡爾空間下直觀地規(guī)劃過(guò)渡軌跡；2)采用兩條拋物線融合成過(guò)渡曲線的算法，以保證軌跡、速度、加速度的平滑，以及曲線形狀可控；3)從工程應(yīng)用角度利用軌跡間的夾角、弓高誤差對(duì)邊界路徑速度進(jìn)行約束，以及以類(lèi)似的方式對(duì)邊界姿態(tài)旋轉(zhuǎn)速度進(jìn)行約束。附圖說(shuō)明圖1是本發(fā)明中過(guò)渡軌跡規(guī)劃方法的流程圖。圖2是示教盒示教出的直線和圓弧軌跡示意圖。圖3是弓高誤差約束邊界速度方法中插入假想圓弧的示意圖。圖4是直線過(guò)渡到圓弧上的機(jī)器人末端點(diǎn)的路徑圖。圖5是過(guò)渡軌跡上的機(jī)器人末端點(diǎn)的位置曲線圖。具體實(shí)施方式下面結(jié)合SCARA機(jī)器人系統(tǒng)對(duì)本發(fā)明作進(jìn)一步說(shuō)明。該系統(tǒng)由一臺(tái)SCARA工業(yè)機(jī)器人、控制柜、示教盒組成，其中SCARA機(jī)器人的大臂長(zhǎng)300mm，小臂長(zhǎng)300mm，關(guān)節(jié)3上下行程是200mm。SCARA機(jī)器人僅有γ軸姿態(tài)，為統(tǒng)一描述，保留α軸姿態(tài)和β軸姿態(tài)，但使其數(shù)值為零，不影響計(jì)算過(guò)程。利用示教盒示教直線軌跡P0P1，再示教圓弧軌跡P1P2，PM是圓弧軌跡上一點(diǎn)，如圖2所示，示教出的P0位姿為(-200,-300,200,0,0,-45)，示教出的P1位姿為(0,-300,200,0,0,45)，示教出的P2位姿為(100,-400,200,0,0,45)，示教出的PM位姿為(50.2429,-386.7423,200,0,0,45)，其中，位置單位是毫米，姿態(tài)單位是度。系統(tǒng)參數(shù)設(shè)定如下：系統(tǒng)最大速度Vmax＝500mm/s、系統(tǒng)最大加速度Amax＝2000mm/s2、系統(tǒng)允許的最大弓高誤差Emax＝10mm，角度到毫米的量綱轉(zhuǎn)換系數(shù)λ＝2mm/°，過(guò)渡參數(shù)百分比a＝100％。確定過(guò)渡軌跡的過(guò)渡起點(diǎn)Ps的位姿和過(guò)渡終點(diǎn)Pe的位姿。軌跡P0P1是笛卡爾空間的直線軌跡，根據(jù)步驟2所述，過(guò)渡起點(diǎn)Ps到拐點(diǎn)P1的直線長(zhǎng)度是軌跡P0P1直線長(zhǎng)度的一半乘以過(guò)渡參數(shù)百分比a，過(guò)渡起點(diǎn)Ps到拐點(diǎn)P1的γ軸姿態(tài)變化對(duì)應(yīng)軌跡P0P1的γ軸姿態(tài)變化的一半乘以過(guò)渡參數(shù)百分比a，可計(jì)算出過(guò)渡起點(diǎn)Ps的位姿為(-100,-300,200,0,0,0)。軌跡P1P2是笛卡爾空間的圓弧軌跡，根據(jù)步驟2所述，拐點(diǎn)P1到過(guò)渡終點(diǎn)Pe的弧長(zhǎng)是軌跡P1P2弧長(zhǎng)的一半乘以過(guò)渡參數(shù)百分比a，拐點(diǎn)P1到過(guò)渡終點(diǎn)Pe的γ軸姿態(tài)變化對(duì)應(yīng)軌跡P1P2的γ軸姿態(tài)變化的一半乘以過(guò)渡參數(shù)百分比a，可計(jì)算出過(guò)渡終點(diǎn)Pe的位姿為(29.2893,-370.7107,200,0,0,45)。根據(jù)步驟3的約定，由外部軟件模塊機(jī)器人的加減速軌跡規(guī)劃算法，輸入過(guò)渡軌跡的邊界速度Vpath＝500mm/s和Vori＝150°/s。從工程應(yīng)用角度，當(dāng)相鄰軌跡切向速度矢量的夾角很大時(shí)，過(guò)大的軌跡銜接速度會(huì)對(duì)機(jī)械系統(tǒng)產(chǎn)生較大的沖擊，因此有必要對(duì)軌跡銜接速度進(jìn)行約束。本發(fā)明利用工程上的弓高誤差參數(shù)來(lái)約束軌跡銜接速度，保證過(guò)渡軌跡邊界速度Vpath和Vori不超過(guò)允許的軌跡銜接速度。根據(jù)步驟4所述，θ是直線P0P1在拐點(diǎn)P1處切向速度矢量與圓弧P1P2在拐點(diǎn)P1處切向速度矢量的夾角，如圖3所示，θ＝90°，帶入弓高誤差Emax，可得到r＝24.1421mm，進(jìn)一步由公式(1)計(jì)算出弓高誤差限制的最大速度為219.7368mm/s，Vpath超過(guò)了該限制，因此需要約束Vpath＝219.7368mm/s。根據(jù)步驟4所述，參考路徑速度Vpath的限制對(duì)姿態(tài)旋轉(zhuǎn)速度Vori進(jìn)行限制，軌跡P0P1的γ軸姿態(tài)有變化，軌跡P1P2的γ軸姿態(tài)無(wú)變化，即軌跡P0P1與軌跡P1P2在拐點(diǎn)P1處的切向姿態(tài)旋轉(zhuǎn)速度矢量的夾角為θori＝0°，理論上對(duì)Vori的限制速度是無(wú)窮大，因此Vori＝150°/s。將笛卡爾空間軌跡在過(guò)渡軌跡邊界點(diǎn)的速度Vpath和Vori分解到位姿6個(gè)自由度上。根據(jù)步驟5所述，軌跡P0P1是直線軌跡類(lèi)型，在笛卡爾坐標(biāo)系中的單位向量Vpath分解到x、y、z軸上的速度表示為單位是mm/s。軌跡P1P2是圓弧軌跡類(lèi)型，利用文獻(xiàn)《工業(yè)機(jī)器人笛卡爾空間軌跡規(guī)劃[J].》(機(jī)械工程與自動(dòng)化,2014(5):141-143)基于局部坐標(biāo)系的空間圓弧插補(bǔ)方案中的計(jì)算步驟可得出圓弧軌跡圓心點(diǎn)Oarc的位置坐標(biāo)為(100,-300,200)，及局部坐標(biāo)系的Z軸的單位方向向量Z′＝(0,0,1)，圓心點(diǎn)Oarc指向圓弧上Pe點(diǎn)的單位方向向量由向量Z′與向量O′叉乘可計(jì)算出Pe點(diǎn)處的單位速度切向矢量可將Vpath分解到x、y、z軸上的速度表示為單位是mm/s。SCARA機(jī)器人僅有γ軸姿態(tài)，因此Vori的速度即是γ軸的旋轉(zhuǎn)速度，過(guò)渡起點(diǎn)Ps處的姿態(tài)旋轉(zhuǎn)速度分解到α、β、γ軸上的旋轉(zhuǎn)速度表示為(0,0,150)，單位是°/s，軌跡P1P2的γ軸姿態(tài)無(wú)變化，過(guò)渡終點(diǎn)Pe處的姿態(tài)旋轉(zhuǎn)速度分解到α、β、γ軸上的旋轉(zhuǎn)速度表示為(0,0,0)，單位是°/s。計(jì)算過(guò)渡軌跡起點(diǎn)Ps和終點(diǎn)Pe的位姿6個(gè)自由度上的加速度。根據(jù)步驟6所述，假定過(guò)渡起點(diǎn)Ps處的瞬間是勻速，軌跡P0P1是直線軌跡，直線上任一點(diǎn)處單位速度切線矢量均是加速度由速度差分計(jì)算得到，可計(jì)算出過(guò)渡起點(diǎn)Ps處的x、y、z軸上的加速度為(0,0,0)，單位是mm/s2。軌跡P1P2是圓弧軌跡類(lèi)型，存在向心加速度，根據(jù)步驟6所述，假定過(guò)渡終點(diǎn)Pe處的瞬間是勻速，通過(guò)速度差分，計(jì)算出點(diǎn)Pe處的x、y、z軸上的加速度為(341.0459,341.7961,0)，單位是mm/s2。SCARA機(jī)器人僅有γ軸姿態(tài)，因此僅有γ軸的旋轉(zhuǎn)加速度，根據(jù)步驟6，假定過(guò)渡起點(diǎn)Ps處和過(guò)渡終點(diǎn)Pe處的瞬間是勻速，因此得出過(guò)渡起點(diǎn)Ps處和過(guò)渡終點(diǎn)Pe處的姿態(tài)旋轉(zhuǎn)速度分解到α、β、γ軸上的旋轉(zhuǎn)加速度表示為(0,0,0)，單位是°/s2。計(jì)算過(guò)渡軌跡運(yùn)行時(shí)間T。根據(jù)步驟7所述，假定過(guò)渡軌跡的執(zhí)行時(shí)間與從過(guò)渡起點(diǎn)Ps勻速運(yùn)動(dòng)到拐點(diǎn)P1再勻速運(yùn)動(dòng)到過(guò)渡終點(diǎn)Pe的直線段時(shí)間是一樣的，依此設(shè)定，分別計(jì)算位置過(guò)渡的運(yùn)行時(shí)間T1＝0.8034s和姿態(tài)過(guò)渡的運(yùn)行時(shí)間T2＝0.3s，T選取較長(zhǎng)的時(shí)間為0.8034s。利用矩陣方程(4)式，對(duì)機(jī)器人過(guò)渡軌跡末端點(diǎn)位姿的6個(gè)自由度分別構(gòu)造過(guò)渡曲線方程P(σ)，其中，σ是時(shí)間t的參數(shù)化變量，0≤t≤T，0≤σ≤1，P1(σ)是與過(guò)渡起點(diǎn)Ps相連的拋物線，是關(guān)于變量σ的二次函數(shù)，設(shè)P1(σ)＝A1σ2+B1σ+C1，P2(σ)是與過(guò)渡終點(diǎn)Pe相連的拋物線，是關(guān)于變量σ的二次函數(shù)，設(shè)P2(σ)＝A2σ2+B2σ+C2，根據(jù)步驟8，η(σ)＝6σ5-15σ4+10σ3，將過(guò)渡軌跡起點(diǎn)Ps、終點(diǎn)Pe的邊界條件：位姿、速度、加速度，帶入公式(4)的矩陣方程，計(jì)算出系數(shù)矩陣A1＝(0,0,0,0,0,0)、B1＝(132.4025,0,0,0,0,90.3826)、C1＝(-100,-300,200,0,0,0)、A2＝(11.0064,11.0307,0,0,0,0)、B2＝(77.113,-110.1687,0,0,0,0)、C2＝(-58.8302,-271.5726,200,0,0,45)，系數(shù)矩陣確定后，即確定了過(guò)渡軌跡的位姿6個(gè)自由度的曲線方程。圖4利用MATLAB軟件繪制了直線過(guò)渡到圓弧上的機(jī)器人末端點(diǎn)的路徑圖，其中點(diǎn)Ps到點(diǎn)Pe之間的實(shí)線就是過(guò)渡軌跡，圖5是過(guò)渡軌跡運(yùn)行期間位姿隨時(shí)間變化的曲線。