工業(yè)機(jī)器人空間圓弧算法的實(shí)現(xiàn)

南京埃斯頓自動(dòng)化股份有限公司 江蘇 南京 211106

0 引言

機(jī)器人的軌跡規(guī)劃,在機(jī)器人控制中具有重要的作用,直接影響著控制的準(zhǔn)確性和快速性。而插補(bǔ)算法是整個(gè)機(jī)器人軌跡規(guī)劃控制過(guò)程的精華所在,占據(jù)著舉足輕重的地位。通過(guò)示教機(jī)器人運(yùn)動(dòng)路徑上的某些關(guān)鍵點(diǎn),然后根據(jù)軌跡特征算出這些示教點(diǎn)之間必須到達(dá)的中間位置點(diǎn),通過(guò)插補(bǔ)進(jìn)行控制,從而實(shí)現(xiàn)高效率高精度的運(yùn)動(dòng)控制。

當(dāng)曲線軌跡為圓弧時(shí),除了示教圓弧起點(diǎn)和終點(diǎn)外,至少還應(yīng)知道圓心或圓弧上一個(gè)輔助點(diǎn)。顯然,示教圓心是很困難的,因而工業(yè)機(jī)器人終端執(zhí)行器的軌跡圓弧通常由示教的圓弧起點(diǎn)、輔助點(diǎn)及圓弧終點(diǎn)決定,而這三點(diǎn)所決定的平面通常不一定平行于某一坐標(biāo)平面,因而需要研究空間任意三點(diǎn)圓弧的插補(bǔ)算法[1]～[4]。

目前普遍采用基于坐標(biāo)轉(zhuǎn)換的空間圓弧插補(bǔ)方法,即通過(guò)坐標(biāo)轉(zhuǎn)換將空間圓弧轉(zhuǎn)化為平面圓弧[5],利用平面圓弧插補(bǔ)算法進(jìn)行計(jì)算,之后再通過(guò)坐標(biāo)轉(zhuǎn)換把插補(bǔ)點(diǎn)從平面結(jié)果轉(zhuǎn)換為空間結(jié)果。此方法需要進(jìn)行多次坐標(biāo)變換,計(jì)算過(guò)程復(fù)雜,計(jì)算工作量大?，F(xiàn)有空間圓弧插補(bǔ)方法未提及過(guò)輔助點(diǎn)姿態(tài);隨著工業(yè)發(fā)展的進(jìn)步,控制指令的簡(jiǎn)便,某些特殊應(yīng)用場(chǎng)合如焊接、涂膠等需要經(jīng)過(guò)輔助點(diǎn)的姿態(tài)。

針對(duì)以上問(wèn)題,提出了一種用于工業(yè)機(jī)器人的過(guò)輔助點(diǎn)姿態(tài)空間圓弧插補(bǔ)方法。該插補(bǔ)方法示教任意不共線的空間三點(diǎn),不需要坐標(biāo)轉(zhuǎn)換將空間點(diǎn)轉(zhuǎn)換成平面點(diǎn)進(jìn)行計(jì)算,而是直接進(jìn)行空間離散點(diǎn)的計(jì)算,并且對(duì)姿態(tài)進(jìn)行規(guī)劃使其過(guò)輔助點(diǎn)姿態(tài),能根據(jù)給定的圓弧起點(diǎn)、輔助點(diǎn)及終點(diǎn)完成任意空間圓弧插補(bǔ)。

圖1 空間圓弧插補(bǔ)算法的流程圖

2 空間圓弧插補(bǔ)算法

本算法的基本技術(shù)方案為:機(jī)器人控制器通過(guò)通信端口確定了示教器示教的空間不共線三點(diǎn);根據(jù)空間幾何關(guān)系直接進(jìn)行空間離散點(diǎn)的計(jì)算,求出空間圓弧的圓心、半徑、法向量、圓心角和弧長(zhǎng);對(duì)姿態(tài)進(jìn)行規(guī)劃使運(yùn)動(dòng)軌跡經(jīng)過(guò)輔助點(diǎn)姿態(tài)且軌跡光滑;速度軌跡規(guī)劃模塊計(jì)算出每個(gè)插補(bǔ)周期的插補(bǔ)位移;利用實(shí)時(shí)插補(bǔ)算法計(jì)算出每個(gè)插補(bǔ)周期的插補(bǔ)點(diǎn)位姿;將最終滿足示教要求的位姿通過(guò)通信端口提供給機(jī)器人運(yùn)動(dòng)機(jī)構(gòu)并執(zhí)行。

2.1 空間圓弧的圓心、半徑、法向量、圓心角和弧長(zhǎng)的求解 機(jī)器人軌跡上各點(diǎn)位姿由位置矢量(x,y,z)和RPY姿態(tài)矢量(α,β,γ)共同描述,組合成一個(gè)6自由度的復(fù)合矢量(x,y,z,α,β,γ),即(x,y,z,a,b,c)。通過(guò)外部示教器示教確定工業(yè)機(jī)器人的要求位姿,機(jī)器人控制器通過(guò)通信端口獲取示教器提供的示教點(diǎn)信息;確定工業(yè)機(jī)器人軌跡的空間圓弧起點(diǎn)、輔助點(diǎn)及終點(diǎn)空間坐標(biāo)及位姿,分別為:P1(x1,y1,z1,a1,b1,c1)、P2(x2,y2,z2,a2,b2,c2)、P3(x3,y3,z3,a3,b3,c3);

空間圓弧的圓心、半徑、法向量、圓心角和弧長(zhǎng)的求解的步驟具體如下:

第1步,在空間坐標(biāo)系中,空間圓弧起點(diǎn)、輔助點(diǎn)及終點(diǎn)為不共線的空間三點(diǎn),確定了空間圓弧所在的平面(簡(jiǎn)稱圓弧平面),根據(jù)圓心到三點(diǎn)的距離都為半徑的條件,聯(lián)立方程,計(jì)算可求出圓弧的圓心P0(x0,y0,z0),進(jìn)而求得由三點(diǎn)確定的圓弧的半徑R;

第2步,根據(jù)外積公式,可求得垂直于空間圓弧平面的單位法向量n?(nx,ny,nz);

第3步,根據(jù)余弦定理和三個(gè)角之間的關(guān)系,求得空間圓弧的起點(diǎn)到輔助點(diǎn)、輔助點(diǎn)到終點(diǎn)、起點(diǎn)到終點(diǎn)的圓心角分別為:θ1,θ2,θ3(θ3=θ1+θ2)(如圖2);

第4步,根據(jù)圓弧弧長(zhǎng)公式,求得弧長(zhǎng)L=Rθ3;進(jìn)而求得空間圓弧插補(bǔ)的總位移S;

圖2

2.2 姿態(tài)規(guī)劃模塊規(guī)劃出經(jīng)過(guò)空間圓弧輔助點(diǎn)的姿態(tài) 機(jī)器人各軸姿態(tài)隨圓心角變化的變化率公式為:

分別對(duì)w(θ)積分即是機(jī)器人各軸姿態(tài)值,故可根據(jù)極值法設(shè)計(jì)使得各軸姿態(tài)變化率連續(xù),并求得相應(yīng)的系數(shù)值k1,m1,k2,m2;

2.3 軌跡速度規(guī)劃模塊,計(jì)算出每個(gè)插補(bǔ)周期的插補(bǔ)位移 軌跡速度規(guī)劃模塊,可以規(guī)劃基于梯形曲線加減速控制或基于S形曲線加減速控制或其它曲線控制;根據(jù)不同的曲線控制方式,進(jìn)行速度規(guī)劃處理,計(jì)算圓弧段所需要的插補(bǔ)總時(shí)間和插補(bǔ)信息,最后計(jì)算出每個(gè)插補(bǔ)周期的插補(bǔ)位移;

2.4 計(jì)算得到插補(bǔ)點(diǎn)的空間位姿 根據(jù)空間圓弧平面的法向量和插補(bǔ)位移,計(jì)算機(jī)器人空間圓弧插補(bǔ)點(diǎn)位姿Pi(xi,yi,zi,ai,bi,ci)步驟如下:

第1步、根據(jù)圓弧弧長(zhǎng)公式,每個(gè)插補(bǔ)周期的插補(bǔ)位移除以半徑得到空間圓弧每個(gè)插補(bǔ)周期的插補(bǔ)角度θ;

第2步、利用以下公式可求得插補(bǔ)點(diǎn)的空間坐標(biāo)位置

第3步、利用以下公式可求得插補(bǔ)點(diǎn)的姿態(tài)

最后,機(jī)器人控制器的中央處理器將最終得到的位姿通過(guò)通信端口提供給機(jī)器人運(yùn)動(dòng)機(jī)構(gòu)進(jìn)行執(zhí)行。本文實(shí)現(xiàn)的方法可以實(shí)現(xiàn)工業(yè)機(jī)器人的高精高效的運(yùn)動(dòng)控制,經(jīng)過(guò)輔助點(diǎn)姿態(tài),并且運(yùn)動(dòng)平滑。

3 結(jié)束語(yǔ)

本文實(shí)現(xiàn)的方法根據(jù)空間任意三點(diǎn)進(jìn)行圓弧插補(bǔ),避免了示教圓心的困難和確定圓弧方向的問(wèn)題。不需要進(jìn)行坐標(biāo)轉(zhuǎn)換計(jì)算平面圓弧插補(bǔ),而是直接計(jì)算空間角度和空間離散點(diǎn)得到實(shí)際空間圓弧插補(bǔ)點(diǎn)坐標(biāo),方法過(guò)程簡(jiǎn)潔方便、算法易于實(shí)現(xiàn)、計(jì)算效率高、插補(bǔ)精度高。過(guò)姿態(tài)規(guī)劃模塊,使得實(shí)際空間圓弧插補(bǔ)點(diǎn)經(jīng)過(guò)輔助點(diǎn)姿態(tài),滿足某些特殊應(yīng)用場(chǎng)合。實(shí)現(xiàn)姿態(tài)變化率與插補(bǔ)角度的連續(xù)關(guān)系,使得各軸姿態(tài)變化連續(xù),機(jī)器人運(yùn)動(dòng)平滑。