路徑軌跡規(guī)劃

1、路徑軌跡規(guī)劃(1)加減速控制簡述加減速控制算法的目標是建立加減速過程中速度相對于時間的函數(shù)關系式 f=V(t)。按照加減速控制算法與插補算法的先后位置關系，加減速控制方式可分為前 加減速控制和后加減速控制。前加減速控制即插補計算前進行加減速運算，其優(yōu) 點在于對合成速度進行控制，不影響位置精度，但是需要預測減速點；后加減速 控制即插補計算后進行加減速運算，它是對各插補軸分別進行加減速控制，由于 各軸沒有協(xié)調(diào)關系，因此合成位置可能不準確。后加減速控制只適用線性插補， 在應用上有很大的局限性。(2)幾種速度控制模型直線加減速速度控制模型直線加減速是當機床啟動、停止或者運動速速改變時，速度將按照一定斜

2、率 的直線上升或下降。數(shù)學表達式為：v(t)=v + at直線加減速控制算法的主要優(yōu)點是算法簡單，機器人響應快，效率高，適合 進行實時運算，但是機器人運動存在柔性沖擊，速度的過渡不夠平滑。指數(shù)加減速速度控制模型指數(shù)加減速是啟動或停止時的速度發(fā)生突變，并且速度變化隨時間按指數(shù)規(guī) 律上升或下降。速度數(shù)學表達式為：加速時：v(t) = v (1 - e)減速時：v(t) = v e-%加速度數(shù)學表達式為：加速時：a(t) = (vc，)e-t t減速時：a(t ) = -()e -/t指數(shù)型加減速曲線的優(yōu)點是數(shù)學表達式相對簡單，可以實時計算，加減速結 束時加速度變小沖擊變??；缺點是啟動過程仍存在較大

3、沖擊。2)S曲線加減速速度控制模型通過對啟動階段即高速階段的加速度衰減，來保證電機性能的充分發(fā)揮和減 小啟動沖擊。正常情況下S曲線加減速的運行過程分為7段：加加速段、勻加速段、減加 速段、勻速段、加減速段、勻減速段、減減速段，如下圖所示：t圖中符號分別表示: :起始速度匕：終點速度tk: (k = 0L.7)各個階段的過渡點時刻T k: (k = 2.7)局部時間坐標，表示以各個階段的起始點作為零點的時間表示,Tk:(k = 12刀各個階段的持續(xù)運行時間L :整個運行長度加加速度J、加速度。、速度、位移，等計算公式的通用形式如下所示:J0 t t10t t t12-Jt t t230t t t

4、34-Jt t t450t t t56Jt t t67Jt0 t ti1JTt t ti12JTJtt t t13230t t t34Jtt t t5；45一JTt t t5:56-JT + Jtt t ta(t)= J(t) = *5767v + 0.5Jt 20 t tt = t時v = v + 0.5JT 2s11101s1v+ JT tt t tt = t時v = v+ JTT011 212202011 2v+ JT t 0.5Jt 2t t tt = t時v = v+ 0.5JT 2021 3323303021vt t tt = t時v = v033440403v0.5Jt 2t t

5、 tt = t時v = v0.5JT 204545505045vJT tt t tt = t時v = vJTT055 656606055 6vJT t + 0.5Jt 2t t tt =t時v = v0.5JT 2V 065 7767707065v(t)=rv t+1 Jt 30 t tt=t時s 二=vT +1 JT 3s1 611101s1 6 11s+v t+- JTt 2t t tt=t時s =s+v T +JTT 20101 221 21-122020101 221 2s+v t+1 JTt 2-Jt 3t t tt=t時s 二=s+v T +1JT 30202 321 363233

6、030202 131s+v tt t tt=t時s =s+v T0303 41 -344040303 41s+v t一一Jt 3t t tt=t時s =s+v T 1JT30404 56 5455050404 565s+v t1 JT t 2t t tt=t時s =s+v T 1JTT 20505 625 61566060505 625 6s+v t1 JT t 2 -Jt 3t t tt =t時s =s+v T -1JT 30606 725 767677070606 535(3 )插補模式軌跡規(guī)劃是指在笛卡兒坐標系中的規(guī)劃。由于關節(jié)空間的軌跡規(guī)劃是對單個 軸的規(guī)劃，由于機器人機構的特殊性，關

7、節(jié)空間規(guī)劃不能保證特定的軌跡，如果 對于那些路徑、姿態(tài)有嚴格要求的作業(yè)，例如弧焊作業(yè)，就必須在笛卡兒坐標系內(nèi)進行規(guī)劃。由于末端執(zhí)行器的位姿都是時間的函數(shù)，所以對軌跡的空間形狀有一定的設 計要求，這需要相應的機器人軌跡插補算法和逆運動學計算來確定一個機器人的 各關節(jié)角，以實現(xiàn)要求的空間軌跡。直線插補和圓弧插補是機器人軌跡規(guī)劃系統(tǒng)中不可缺少的基本插補算法，也 是機器人軌跡規(guī)劃中最常用的規(guī)劃方法，本機器人系統(tǒng)實現(xiàn)了直線插補和圓弧插 補。1)直線插補O0 (a , P , Y ) O0 (a , P , Y ) P ( x , j , z )11111111XY。(a , P , Y )2222P 3

8、 , j , z )2222直線插補及梯形速度控制,直線插補梯形加減速控制參數(shù)求解直線插補流程圖直線段運動的速度為V。,直線插補梯形加減速控制參數(shù)求解直線插補流程圖直線段運動的速度為V。2)圓弧插補如圖，圓弧三點坐標如圖所示始點坐標和姿態(tài)為P (X , j ,z )、0 (a ,p ,Y )，終點坐標和姿態(tài)為 TOC o 1-5 h z 11111111P (X , j ,z )、0 (a ,P ,Y )，開始時的加速段或結束時的減速段(加速段與減 22222222速段具有對稱性)的加速度為a,j,z )、p G , j,z)、p G, j ,z)，姿態(tài)為111222233330 (a , P

9、 ,Y )、0 (a , p ,y )、0 (a , p ,y ),始末加速段加速度為a ,中間段速度111122223333圓弧插補圓弧插補流程圓弧插補圓弧插補流程判斷三點共線利用向量PP和向量PP叉乘來判斷。1 22 3三平面法求圓心和半徑P、P和P點確定的平面M123X 一 XX 一 X3X 一 XX - Xy y3 yi - y3 y 2 - y3z - z3Z - z3 = 0 z 2 - z 3X - (x + X )(X - X )+y -1 (y + y )(y - y )+L-吳+z)(2122121221212過4P2中點且與之垂直的平面T2 -z= 0X -1 (X +

10、X )(X - X )+y -1 (y + y )(y-y )+z -1 (z + z )(2233222332223過P2P3中點且與之垂直的平面S3 - z2 )=聯(lián)立三個平面方程，用消去法可求得圓心，在求解過程中要討論六種情況(消 去過程中分母不能為零的六種情況)。求半徑：r = j(X - x * +(y - y * +(z - z *101010求變換矩陣以圓心P為原點O建立坐標系，以PP方向為U軸，其單位方向矢量為: 0R0 1PPuPPu = 01PP0W軸為與向量PP和PP相垂直的方向，單位方向矢量為:2PP X PP1 2-3PP X PP1 22 3v軸按右手法則來定，v軸

11、按右手法則來定，V = w X u因此，變換矩陣如下:uVwPxxxoxuVwpyyyoyuVwPzzzoz0001TR逆矩陣如下:T-1 =RuVT-1 =RuVwpxxxoxuVw,P =pyyyoyuVwp其中，R =ozzzzRt0-RtP01將各點轉換為新坐標-u -x -u -x -u -x 一112233V=T -1yV=T -1yV=T -1y11922933wRzwRzwRz112233111111=w = 0，半徑 r = uu = V = w=w = w平面圓弧插補在平面Or-UV平面內(nèi)進行圓弧插補，9 0為圓弧的弧度:A tan 2(v , u )兀2兀+ A tan 2(v , u )p點為圓弧上任一點，弧度為0,則有:則插補點坐標為:u = r cos（0） v = r sin G）w = 0插補點原坐標系坐標p點在原坐標系中的坐標G,y,z）為:x yv=TzRw11姿態(tài)的求解姿態(tài)各軸的增量：以=以一以3。=。：；丫 =丫 -yI 31可得插補點姿態(tài)如下：a = a + XAa P -p1 +XAP1y =y +人AyI 1（4）連續(xù)直線路徑、軌跡在直線插補規(guī)劃中，啟動加速停止減速，若連續(xù)直線運動，則再啟動運動到 下一點，這樣使電機不停地啟動和停止，引起較大的振動和磨損。為避免此問題，可用圓弧過渡的方法將相鄰直線連接，完成平滑勻速過渡。3,七,可用圓