機(jī)器人控制系統(tǒng)軌跡加減速插補(bǔ)算法
【技術(shù)領(lǐng)域】
[0001] 本發(fā)明設(shè)及機(jī)器人控制領(lǐng)域，具體設(shè)及一種機(jī)器人控制系統(tǒng)軌跡加減速插補(bǔ)算 法。
【背景技術(shù)】
[0002] 運(yùn)動(dòng)控制加減速方式有： 陽(yáng)00引1、直線加減速。缺點(diǎn)：加速度有突變，速度曲線不平滑。
[0004] 2、Ξ角函數(shù)加減速。缺點(diǎn)：計(jì)算復(fù)雜。 陽(yáng)0化]3、指數(shù)型加減速。缺點(diǎn)：在啟動(dòng)和停止2點(diǎn)對(duì)機(jī)床有沖擊。
[0006] 4、s形加減速。在任何一點(diǎn)的加速度都是連續(xù)變化的，從而避免速度的柔性沖擊， 速度的平滑性好，運(yùn)動(dòng)精度高。缺點(diǎn)：當(dāng)加速度設(shè)定跟直線加速度設(shè)定相同時(shí)，加速時(shí)間是 直線加速時(shí)間的2倍。
[0007] 傳統(tǒng)插補(bǔ)算法：
[0008] 1、直線插補(bǔ)：根據(jù)2點(diǎn)求出直線長(zhǎng)度以由要求的運(yùn)動(dòng)速度V及采樣周期T，得步 長(zhǎng)ΔL和總步數(shù)S:ΔL=VT，S=INT(L/AL)+l，于是可得插補(bǔ)點(diǎn)坐標(biāo)（S+1個(gè)點(diǎn)）。
[0009] 2、圓弧插補(bǔ)：根據(jù)Ξ點(diǎn)求出圓屯、坐標(biāo)、半徑及圓屯、角Φ。根據(jù)速度求步長(zhǎng)/^以再 求總步數(shù)S，S=INT(Φ/ΔL)+l，最后據(jù)ΔΦ=Φ/s及圓弧曲線求插補(bǔ)點(diǎn)坐標(biāo)。
[0010] W上兩種方法都會(huì)存在當(dāng)插補(bǔ)周期一定，速度越快，AL越長(zhǎng)，插補(bǔ)精度越差。

【發(fā)明內(nèi)容】

[0011] 有鑒于此，本發(fā)明的目的在于提供一種機(jī)器人控制系統(tǒng)軌跡加減速插補(bǔ)算法。
[0012] 為達(dá)到上述目的，本發(fā)明提供如下技術(shù)方案：一種機(jī)器人控制系統(tǒng)軌跡加減速插 補(bǔ)算法，
[0013] S1.光順曲線：插補(bǔ)軌跡先用Ξ次樣條spline插值使曲線光滑，計(jì)算出各個(gè)節(jié)點(diǎn) (節(jié)點(diǎn)是指什么意思）處的函數(shù)值和兩個(gè)邊界處的導(dǎo)數(shù)信息；
[0014] S2.在S1已經(jīng)計(jì)算好的S次樣條曲線上插入等距點(diǎn)，通過(guò)D-H建模后的運(yùn)動(dòng)逆運(yùn) 算計(jì)算各個(gè)轉(zhuǎn)動(dòng)軸的運(yùn)動(dòng)距離。
[0015] 進(jìn)一步，所述曲線包括：加加速段、勻加速段、減加速段、勻速段、加減速段、勻減速 段、減減速段，對(duì)每個(gè)加速度積分得到各段對(duì)應(yīng)速度；
[0016]
陽(yáng)017] A為最大加速度，J為加加速度，V。為起始速度，Vm。、為勻加速階段速度。
[0018] 本發(fā)明的有益效果在于：
[0019] 本發(fā)明的機(jī)器人運(yùn)動(dòng)控制方法加減速采用S形+直線加減速，加速度連續(xù)變化，加 速時(shí)間快，運(yùn)動(dòng)精度高，速度平滑性好。插補(bǔ)軌跡采用多次樣條插補(bǔ)作為粗插補(bǔ)，再采用數(shù) 字積分法W及等時(shí)間插補(bǔ)對(duì)軌跡做精插補(bǔ)，V速度高，不會(huì)存在插補(bǔ)精度變差。
【附圖說(shuō)明】
[0020] 為了使本發(fā)明的目的、技術(shù)方案和優(yōu)點(diǎn)更加清楚，下面將結(jié)合附圖對(duì)本發(fā)明作進(jìn) 一步的詳細(xì)描述，其中：
[0021] 圖1為本發(fā)明的方法流程圖；
[0022] 圖2為Ξ次樣條插值曲線圖；
[0023] 圖3為分段線性插值曲線圖；
[0024] 圖4為距離與時(shí)間圖，速度與時(shí)間圖，加速度與時(shí)間圖，加加速與時(shí)間圖。
【具體實(shí)施方式】
[0025] 下面將結(jié)合附圖，對(duì)本發(fā)明的優(yōu)選實(shí)施例進(jìn)行詳細(xì)的描述。
[00%] -種機(jī)器人控制系統(tǒng)軌跡加減速插補(bǔ)算法，包括W下步驟：
[0027] 1、光順曲線。插補(bǔ)軌跡先用Ξ次樣條spline插值使曲線光滑，給出各個(gè)節(jié)點(diǎn) (spline插值點(diǎn)）處的函數(shù)值和兩個(gè)邊界處的導(dǎo)數(shù)信息，便可得到不同光滑性的曲線，曲線 越光滑，速度越快。運(yùn)里采用的Ξ次樣條是標(biāo)準(zhǔn)的插值方式，不論在區(qū)間內(nèi)或其邊界上，其 一階導(dǎo)數(shù)dy/dx平滑，二階導(dǎo)數(shù)d2y/dx連續(xù)。我們運(yùn)里不采用分段線性插值的原因是其各 個(gè)節(jié)點(diǎn)用折線連接，拐點(diǎn)的折線夾角越小，運(yùn)行速度就越低，降低了機(jī)器的整體運(yùn)行速度。 Ξ次樣條插值和分段線性插值的區(qū)別如圖1所示：
[0028] 2、機(jī)器人坐標(biāo)逆運(yùn)算升降頻。在第一步已經(jīng)計(jì)算好的Ξ次樣條曲線上插入等距 點(diǎn)，通過(guò)D-H建模（4X4的奇次變換矩陣描述相鄰關(guān)節(jié)空間坐標(biāo)系的關(guān)系）后的運(yùn)動(dòng)逆運(yùn) 算計(jì)算各個(gè)轉(zhuǎn)動(dòng)軸的運(yùn)動(dòng)距離。加速過(guò)程中的加加速和減加速中間插入一段等加速度Ai， 在減速過(guò)程中的加加速和減加速過(guò)程中也插入一段勻減加速度A2,使速度快速升到最高 速，勻速后從最高速快速降到最低速，且曲線平滑。
[0029] 所述曲線包括：加加速段I、勻加速段II、減加速段III、勻速段IV、加減速段V、勻 減速段VI、減減速段VII，對(duì)每個(gè)加速度積分得到各段對(duì)應(yīng)速度；
[0030]
陽(yáng)03UA為最大加速度，J為加加速度，V。為起始速度，Vm。為勻加速階段速度。
[0032] 在時(shí)間0~ti段，加速度按線性方式增長(zhǎng)，加速度曲線的I，速度按拋物線方式增 長(zhǎng)從V。到Vi。J設(shè)定越大，加速度升到Ai越快。 陽(yáng)03引時(shí)間ti~12段，加速度不變，加速度曲線的II，速度按直線方式增長(zhǎng)從V劇V2。
[0034] 在時(shí)間t2~13段，加速度按線性方式減小到0,加速度曲線的III，速度按反拋物 線方式增長(zhǎng)從V2到V3。
[0035] 在時(shí)間t3~14段，加速度為0,加速度曲線的IV，速度勻速V3到V4。
[0036] 在時(shí)間t4~t嚴(yán)，減加速度按線性方式增加，加速度曲線V，速度按反拋物線方式 減小從V4到V5。
[0037] 在時(shí)間ts~te段，加速度不變，加速度曲線的VI，速度按線性方式減小從Vs到Ve。
[0038]在時(shí)間t4~tg段，減加速度按線性方式減小，加速度曲線V，速度按反拋物線方式 減小從Ve到V7。
[0039] 最后說(shuō)明的是，W上優(yōu)選實(shí)施例僅用W說(shuō)明本發(fā)明的技術(shù)方案而非限制，盡管通 過(guò)上述優(yōu)選實(shí)施例已經(jīng)對(duì)本發(fā)明進(jìn)行了詳細(xì)的描述，但本領(lǐng)域技術(shù)人員應(yīng)當(dāng)理解，可W在 形式上和細(xì)節(jié)上對(duì)其作出各種各樣的改變，而不偏離本發(fā)明權(quán)利要求書所限定的范圍。
【主權(quán)項(xiàng)】
1. 一種機(jī)器人控制系統(tǒng)軌跡加減速插補(bǔ)算法，其特征在于：51. 光順曲線：插補(bǔ)軌跡先用三次樣條spline插值使曲線光滑，計(jì)算出各個(gè)節(jié)點(diǎn)處的 函數(shù)值和兩個(gè)邊界處的導(dǎo)數(shù)信息；52. 在S1已經(jīng)計(jì)算好的三次樣條曲線上插入等距點(diǎn)，通過(guò)D-Η建模后的運(yùn)動(dòng)逆運(yùn)算計(jì) 算各個(gè)轉(zhuǎn)動(dòng)軸的運(yùn)動(dòng)距離。2. 根據(jù)權(quán)利要求1所述的機(jī)器人控制系統(tǒng)軌跡加減速插補(bǔ)算法，其特征在于：所述曲 線包括：加加速段、勻加速段、減加速段、勻速段、加減速段、勻減速段、減減速段，對(duì)每個(gè)加 速度積分得到各段對(duì)應(yīng)速度；A1為最大加速度，A2是最大減速度，J為加加速度，V。為起始速度，v_為勻加速階段 速度。
【專利摘要】本發(fā)明涉及一種機(jī)器人控制系統(tǒng)軌跡加減速插補(bǔ)算法，S1.光順曲線：插補(bǔ)軌跡先用三次樣條spline插值使曲線光滑，計(jì)算出各個(gè)節(jié)點(diǎn)處的函數(shù)值和兩個(gè)邊界處的導(dǎo)數(shù)信息；S2.在S1已經(jīng)計(jì)算好的三次樣條曲線上插入等距點(diǎn)，通過(guò)D-H建模后的運(yùn)動(dòng)逆運(yùn)算計(jì)算各個(gè)轉(zhuǎn)動(dòng)軸的運(yùn)動(dòng)距離。本發(fā)明的機(jī)器人運(yùn)動(dòng)控制方法加減速采用S形+直線加減速，加速度連續(xù)變化，加速時(shí)間快，運(yùn)動(dòng)精度高，速度平滑性好。插補(bǔ)軌跡采用多次樣條插補(bǔ)作為粗插補(bǔ)，再采用數(shù)字積分法以及等時(shí)間插補(bǔ)對(duì)軌跡做精插補(bǔ)，V速度高，不會(huì)存在插補(bǔ)精度變差。
【IPC分類】G05B19/41
【公開號(hào)】CN105278462
【申請(qǐng)?zhí)枴緾N201510826995
【發(fā)明人】孫碎可, 冉娟蓮
【申請(qǐng)人】重慶市靈龍自動(dòng)化設(shè)備有限公司
【公開日】2016年1月27日
【申請(qǐng)日】2015年11月24日