專利名稱：定位機(jī)器人運(yùn)動(dòng)軌跡的方法和裝置的制作方法
技術(shù)領(lǐng)域：
本發(fā)明涉及半導(dǎo)體自動(dòng)化加工領(lǐng)域，尤指一種工業(yè)機(jī)器人加減速運(yùn)動(dòng)控制中的定位機(jī)器人運(yùn)動(dòng)軌跡的方法和裝置。
背景技術(shù)：
在機(jī)器人控制領(lǐng)域內(nèi)，如何能讓電機(jī)按預(yù)期的軌跡平穩(wěn)安全的運(yùn)行是決定機(jī)器人性能的關(guān)鍵問題；控制器對(duì)驅(qū)動(dòng)器脈沖發(fā)送的時(shí)間間隔控制是保證電機(jī)平穩(wěn)運(yùn)行的先決條件；插補(bǔ)計(jì)算是實(shí)現(xiàn)機(jī)器人高速、高精度軌跡控制的關(guān)鍵技術(shù)。為使機(jī)器人能夠按照預(yù)期的軌跡運(yùn)行，其運(yùn)動(dòng)精度和定位精度能夠達(dá)到預(yù)定的要求，避免由于控制系統(tǒng)、驅(qū)動(dòng)系統(tǒng)以及機(jī)器人本體的電氣和機(jī)械慣性，導(dǎo)致電機(jī)的速度突變，產(chǎn)生沖擊、震蕩、超調(diào)或失步等動(dòng)態(tài)誤差，造成系統(tǒng)精度降低等問題的出現(xiàn)。國內(nèi)各大機(jī)器人生產(chǎn)企業(yè)，通常采取在機(jī)器人內(nèi)部安裝美國DALTA TAU Data System公司的PMAC(Programable Multi Axix Controller)控制器提供的PVT模式以產(chǎn)生光滑而精確的軌跡,大大提高了企業(yè)的生產(chǎn)成本。

發(fā)明內(nèi)容
本發(fā)明針對(duì)以上問題提出了一種能夠應(yīng)用于工業(yè)機(jī)器人，實(shí)現(xiàn)高速、高精度定位的定位機(jī)器人運(yùn)動(dòng)軌跡的方法和裝置，能夠盡量避免由于控制系統(tǒng)、驅(qū)動(dòng)系統(tǒng)以及機(jī)器人本體的電氣和機(jī)械慣性，使它的速度突變，產(chǎn)生沖擊、震蕩、超調(diào)或失步等動(dòng)態(tài)誤差，造成系統(tǒng)精度降低的情況發(fā)生。本發(fā)明的技術(shù)方案是提供一種定位機(jī)器人運(yùn)動(dòng)軌跡的方法，包括以下步驟:一脈沖控制器接收一機(jī)器人控制器每周期的指令脈沖數(shù)，并對(duì)接收的位置點(diǎn)進(jìn)行S形曲線加減速作為粗插補(bǔ)運(yùn)算，所述機(jī)器人控制器用以控制一機(jī)器人本體；在S形曲線基礎(chǔ)上劃分為η段，對(duì)第η段進(jìn)行PVT精插補(bǔ)運(yùn)算，通過第η段內(nèi)相鄰兩點(diǎn)間的斜率值可知機(jī)器人到達(dá)每一位置點(diǎn)的速度，并在S形曲線上確定所屬的速度段；調(diào)用所述脈沖控制器的DSP數(shù)字信號(hào)處理器中的脈沖輸出函數(shù)進(jìn)行脈沖發(fā)送，并傳給一驅(qū)動(dòng)器控制一電機(jī)轉(zhuǎn)動(dòng)，所述電機(jī)控制所述機(jī)器人本體的運(yùn)動(dòng)。進(jìn)一步地，所述對(duì)第η段進(jìn)行PVT精插補(bǔ)運(yùn)算包括:S形曲線劃分為η段基礎(chǔ)上對(duì)其中任意一段進(jìn)行i等分，其中每一等分點(diǎn)的位置值為Ki ;如果為變速運(yùn)動(dòng)，即電機(jī)運(yùn)動(dòng)在加速段、減速段和勻速段中至少2個(gè)速度段，則Ki等于第η段起始位置值；如果為非變速運(yùn)動(dòng)，即電機(jī)運(yùn)動(dòng)在加速段或減速段或勻速段，則位置值&進(jìn)行累加；判斷位置值Ki是否小于控制周期的位置值，若位置值Ki小于控制周期的位置值，則計(jì)算第i段的p、v、t值，將計(jì)算得到的PVT值保存到自定的數(shù)組中，并進(jìn)行i++ ;若位置值Ki大于控制周期的位置值，則程序結(jié)束。進(jìn)一步地，所述脈沖控制器還包括一 FPGA信號(hào)處理器、一 CAN接口電路、一模擬量輸出電路、一數(shù)字量輸出電路以及一編碼器接口電路，所述DSP數(shù)字信號(hào)處理器通過所述CAN接口電路接收來自所述機(jī)器人控制器的指令，所述DSP數(shù)字信號(hào)處理器將指令發(fā)給所述FPGA信號(hào)處理器，經(jīng)所述FPGA信號(hào)處理器內(nèi)部操作將信號(hào)經(jīng)所述模擬量輸出電路、所述數(shù)字量輸出電路、所述編碼器接口電路與所述驅(qū)動(dòng)器進(jìn)行直接的通訊。進(jìn)一步地，所述機(jī)器人本體具有若干關(guān)節(jié)，每個(gè)關(guān)節(jié)圍繞固定的坐標(biāo)軸旋轉(zhuǎn)，通過關(guān)節(jié)值顯示當(dāng)前位置給所述脈沖控制器。進(jìn)一步地，S形曲線加減速控制方程 v(t) = Vs+(ve-vs) exp (kt/ x)/[exp(kt/τ ) +exp (kt0/ τ )]，vs為開始時(shí)的速度，Ve為結(jié)束時(shí)的速度，Vc為穩(wěn)態(tài)速度，k為系數(shù)，τ為過渡過程時(shí)間常數(shù)，加速段vs = O, ve = V。，則v(t) =v。exp (kt/ τ )/[exp (kt/τ ) +exp (kt0/ τ)],當(dāng) t<< τ 且 k>>l 時(shí)，v(t) “Ο,當(dāng) t>> τ 且 k>>l 時(shí)，V (t) ^ vc ;勻速段 Vs = ve = vc,貝丨J V (t) = vc ;減速段 Vs = vc, Ve = O,貝丨J V (t) = exp (kt0/τ ) vc/ (ekt/T+exp (kt。/ τ))，當(dāng) t<< τ 且 k>>l 時(shí)，當(dāng) t>> τ 且 k>>I 時(shí)，V(t) ^ O0進(jìn)一步地，PVT空間圓弧精插補(bǔ)運(yùn)算公式
權(quán)利要求
1.一種定位機(jī)器人運(yùn)動(dòng)軌跡的方法，其特征在于，包括以下步驟: 一脈沖控制器接收一機(jī)器人控制器每周期的指令脈沖數(shù)，并對(duì)接收的位置點(diǎn)進(jìn)行S形曲線加減速作為粗插補(bǔ)運(yùn)算，所述機(jī)器人控制器用以控制一機(jī)器人本體； 在S形曲線基礎(chǔ)上劃分為η段，對(duì)第η段進(jìn)行PVT精插補(bǔ)運(yùn)算，通過第η段內(nèi)相鄰兩點(diǎn)間的斜率值可知機(jī)器人到達(dá)每一位置點(diǎn)的速度，并在S形曲線上確定所屬的速度段； 調(diào)用所述脈沖控制器的DSP數(shù)字信號(hào)處理器中的脈沖輸出函數(shù)進(jìn)行脈沖發(fā)送，并傳給一驅(qū)動(dòng)器控制一電機(jī)轉(zhuǎn)動(dòng)，所述電機(jī)控制所述機(jī)器人本體的運(yùn)動(dòng)。
2.根據(jù)權(quán)利要求1所述的定位機(jī)器人運(yùn)動(dòng)軌跡的方法，其特征在于，所述對(duì)第η段進(jìn)行PVT精插補(bǔ)運(yùn)算包括: S形曲線劃分為η段基礎(chǔ)上對(duì)其中任意一段進(jìn)行i等分，其中每一等分點(diǎn)的位置值為Ki ;如果為變速運(yùn)動(dòng)，即電機(jī)運(yùn)動(dòng)在加速段、減速段和勻速段中的至少2個(gè)速度段，則Ki等于第η段起始位置值；如果為非變速運(yùn)動(dòng)，即電機(jī)運(yùn)動(dòng)在加速段或減速段或勻速段，則位置值Ki進(jìn)行累加； 判斷位置值Ki是否小于控制周期的位置值，若位置值Ki小于控制周期的位置值，則計(jì)算第i段的P、V、t值，將計(jì)算得到的PVT值保存到自定的數(shù)組中，并進(jìn)行i++ ;若位置值Ki大于控制周期的位置值，則程序結(jié)束。
3.根據(jù)權(quán)利要求1所述的定位機(jī)器人運(yùn)動(dòng)軌跡的方法，其特征在于，所述脈沖控制器還包括一 FPGA信號(hào)處理器、一 CAN接口電路、一模擬量輸出電路、一數(shù)字量輸出電路以及一編碼器接口電路，所述DSP數(shù)字信號(hào)處理器通過所述CAN接口電路接收來自所述機(jī)器人控制器的指令，所述DSP數(shù)字信號(hào)處理器將指令發(fā)給所述FPGA信號(hào)處理器，經(jīng)所述FPGA信號(hào)處理器內(nèi)部操作將信號(hào)經(jīng)所述模擬量輸出電路、所述數(shù)字量輸出電路、所述編碼器接口電路與所述驅(qū)動(dòng)器進(jìn)行直接的通訊。
4.根據(jù)權(quán)利要求1所述的定位機(jī)器人運(yùn)動(dòng)軌跡的方法，其特征在于，所述機(jī)器人本體具有若干關(guān)節(jié)，每個(gè)關(guān)節(jié)圍繞固定的坐標(biāo)軸旋轉(zhuǎn)，通過關(guān)節(jié)值顯示當(dāng)前位置給所述脈沖控制器。
5.根據(jù)權(quán)利要求1所述的定位機(jī)器人運(yùn)動(dòng)軌跡的方法，其特征在于，S形曲線加減速控制方程 V (t) = Vs+ (Ve-Vs) exp (kt/ τ )/[exp (kt/ τ ) +exp (kt0/ τ)]，vs 為開始時(shí)的速度,Vei為結(jié)束時(shí)的速度，Vc為穩(wěn)態(tài)速度，k為系數(shù)，τ為過渡過程時(shí)間常數(shù),加速段Vs = O, Ve
6.根據(jù)權(quán)利要求1所述的定位機(jī)器人運(yùn)動(dòng)軌跡的方法，其特征在于，PVT空間圓弧精插補(bǔ)運(yùn)算公式
7.—種定位機(jī)器人運(yùn)動(dòng)軌跡的裝置，其特征在于:所述裝置包括一機(jī)器人控制器、一脈沖控制器、一驅(qū)動(dòng)器、一電機(jī)以及一機(jī)器人本體，所述機(jī)器人控制器向所述脈沖控制器發(fā)送指令，所述脈沖控制器接受指令帶動(dòng)所述驅(qū)動(dòng)器控制所述電機(jī)旋轉(zhuǎn)，所述脈沖控制器接收所述機(jī)器人控制器每周期的指令脈沖數(shù)，并對(duì)接收的位置點(diǎn)進(jìn)行S形曲線加減速作為粗插補(bǔ)運(yùn)算，在S形曲線基礎(chǔ)上劃分為η段，對(duì)第η段進(jìn)行PVT精插補(bǔ)運(yùn)算，通過第η段內(nèi)相鄰兩點(diǎn)間的斜率值可知機(jī)器人到達(dá)每一位置點(diǎn)的速度，并在S形曲線上確定所屬的速度段調(diào)用所述脈沖控制器的DSP數(shù)字信號(hào)處理器中的脈沖輸出函數(shù)進(jìn)行脈沖發(fā)送，并傳給所述驅(qū)動(dòng)器控制所述電機(jī)轉(zhuǎn)動(dòng)，所述電機(jī)控制所述機(jī)器人本體的運(yùn)動(dòng)。
8.根據(jù)權(quán)利要求7所述的定位機(jī)器人運(yùn)動(dòng)軌跡的裝置，其特征在于，所述對(duì)第η段進(jìn)行PVT精插補(bǔ)運(yùn)算包括:S形曲線劃分為η段基礎(chǔ)上對(duì)其中任意一段進(jìn)行i等分，其中每一等分點(diǎn)的位置值為Ki ;如果為變速運(yùn)動(dòng)，即電機(jī)運(yùn)動(dòng)在加速段、減速段和勻速段中至少2個(gè)速度段，則Ki等于第η段起 始位置值；如果為非變速運(yùn)動(dòng)，即電機(jī)運(yùn)動(dòng)在加速段或減速段或勻速段，則位置值Ki進(jìn)行累加；判斷位置值Ki是否小于控制周期的位置值，若變量Ki小于控制周期的位置值，則計(jì)算第i段的P、V、t值，將計(jì)算得到的PVT值保存到自定的數(shù)組中，并進(jìn)行i++ ;若位置值Ki大于控制周期的位置值，則程序結(jié)束，調(diào)用所述脈沖控制器的DSP數(shù)字信號(hào)處理器中的脈沖輸出函數(shù)進(jìn)行脈沖發(fā)送。
9.根據(jù)權(quán)利要求7所述的定位機(jī)器人運(yùn)動(dòng)軌跡的裝置，其特征在于，所述脈沖控制器還包括一 FPGA信號(hào)處理器、一 CAN接口電路、一模擬量輸出電路、一數(shù)字量輸出電路以及一編碼器接口電路，所述DSP數(shù)字信號(hào)處理器通過所述CAN接口電路接收來自所述機(jī)器人控制器的指令，所述DSP數(shù)字信號(hào)處理器將指令發(fā)給所述FPGA信號(hào)處理器，經(jīng)所述FPGA信號(hào)處理器內(nèi)部操作將信號(hào)經(jīng)所述模擬量輸出電路、所述數(shù)字量輸出電路、所述編碼器接口電路與所述驅(qū)動(dòng)器進(jìn)行直接的通訊。
10.根據(jù)權(quán)利要求7所述的定位機(jī)器人運(yùn)動(dòng)軌跡的裝置，其特征在于，所述脈沖控制器對(duì)接收的位置點(diǎn)進(jìn)行S形曲線加減速作為粗插補(bǔ)運(yùn)算，S形曲線加減速控制方程V(t)=vs+(Ve-Vs) exp (kt/ τ )/[exp (kt/ τ ) +exp (kt0/ τ )], VsS 開始時(shí)的速度，Ve 為結(jié)束時(shí)的速度，V。為穩(wěn)態(tài)速度，k為系數(shù)，τ為過渡過程時(shí)間常數(shù)，加速段Vs = 0，Ve = V。，則V (t) =Vcexp (kt/ τ ) / [exp (kt/ τ ) +exp (kt。/ ],當(dāng) t<< τ 且 k>>l 時(shí)，v(t) ()，當(dāng) t>>τ 且 k >> I 時(shí)，v(t) ^ vc ;勻速段 vs = ve = vc,則 v(t) = vc ;減速段 vs = vc, ve = 0,則 V (t) = exp (kt。/ τ ) vc/ (ekt/ τ +exp (kt。/ τ))，當(dāng) t<< τ 且 k>>l 時(shí)，v(t) vc,當(dāng)t >> τ 且 k>>l 時(shí)，v(t) O。
11.根據(jù)權(quán)利要求7所述的定位機(jī)器人運(yùn)動(dòng)軌跡的裝置，其特征在于，所述脈沖控制器對(duì)每段進(jìn)行PVT空間圓弧精插補(bǔ)運(yùn)算，PVT空間圓弧精插補(bǔ)運(yùn)算公式
全文摘要
一種定位機(jī)器人運(yùn)動(dòng)軌跡的方法及裝置，包括機(jī)器人控制器、脈沖控制器、驅(qū)動(dòng)器、電機(jī)以及機(jī)器人本體，機(jī)器人控制器向脈沖控制器發(fā)送指令，脈沖控制器接受指令帶動(dòng)驅(qū)動(dòng)器控制電機(jī)旋轉(zhuǎn)，脈沖控制器接收機(jī)器人控制器每周期的指令脈沖數(shù)，并對(duì)接收的位置點(diǎn)進(jìn)行S形曲線加減速作為粗插補(bǔ)運(yùn)算，取機(jī)器人控制器每周期連續(xù)指令位置值，通過相鄰兩點(diǎn)間的斜率值得機(jī)器人本體到達(dá)每一位置點(diǎn)的速度，并在S曲線上確定所屬的速度段，在兩個(gè)位置點(diǎn)之間劃分為變量i段，每段進(jìn)行PVT空間圓弧精插補(bǔ)運(yùn)算，調(diào)用脈沖控制器的DSP數(shù)字信號(hào)處理器中的脈沖輸出函數(shù)進(jìn)行脈沖發(fā)送。本發(fā)明機(jī)器人運(yùn)行更加平穩(wěn)，提高了系統(tǒng)的定位精度和速度響應(yīng)性，節(jié)約了企業(yè)的生產(chǎn)成本。
文檔編號(hào)G05D1/02GK103197673SQ20121000214
公開日2013年7月10日 申請(qǐng)日期2012年1月5日 優(yōu)先權(quán)日2012年1月5日
發(fā)明者徐方, 曲道奎, 鄒風(fēng)山, 賈凱, 李崇, 劉曉帆, 宋吉來, 鄭春暉 申請(qǐng)人:沈陽新松機(jī)器人自動(dòng)化股份有限公司