機(jī)械臂點(diǎn)到點(diǎn)運(yùn)動控制方法
【專利摘要】本發(fā)明屬于數(shù)控系統(tǒng)領(lǐng)域，公開了機(jī)械臂點(diǎn)到點(diǎn)運(yùn)動控制方法?，F(xiàn)有技術(shù)的機(jī)械臂的點(diǎn)到點(diǎn)運(yùn)動控制方法，采用較小的速度啟動、停止，來抑制振動，導(dǎo)致運(yùn)動效率低。本發(fā)明根據(jù)機(jī)械臂啟動、停止的精度要求，確定加加速度比、速度比，對加速階段的最大加加速度J1，減速階段的最大加加速度J2，加速階段的最大限制加速度A1，減速階段的最大限制加速度A2根據(jù)實(shí)際運(yùn)動需要進(jìn)行調(diào)整，能適應(yīng)諸多復(fù)雜的運(yùn)動，尤其是能夠滿足對末位置狀態(tài)有較高精度要求的情形，并且能保證運(yùn)動時(shí)間最優(yōu)。
【專利說明】
機(jī)械臂點(diǎn)到點(diǎn)運(yùn)動控制方法
技術(shù)領(lǐng)域
[0001] 本發(fā)明涉及數(shù)控系統(tǒng)領(lǐng)域，特別是機(jī)械臂點(diǎn)到點(diǎn)運(yùn)動控制方法。
【背景技術(shù)】
[0002] 隨著技術(shù)的飛速發(fā)展，開放式的數(shù)控系統(tǒng)研究已經(jīng)成為世界各國數(shù)控界研究的熱 點(diǎn)，而我國目前在這一領(lǐng)域的研究相對落后，尤其是在加減速控制技術(shù)的研究上，與國外還 有很大的差距。在機(jī)械運(yùn)動的控制上，運(yùn)動過程的平穩(wěn)、無沖擊、曲線光滑、均勻等特點(diǎn)對機(jī) 械運(yùn)動的質(zhì)量、精度都產(chǎn)生了極大的影響，受限于機(jī)械零件材質(zhì)、精度等因素，機(jī)械運(yùn)動往 往達(dá)不到所預(yù)期的效果，運(yùn)動過程可能存在較大振動，運(yùn)動末位置精度不良，運(yùn)動時(shí)間過長 效率低下等。采用對稱的點(diǎn)到點(diǎn)運(yùn)動控制方法，能較好的實(shí)現(xiàn)機(jī)械臂各軸的平滑啟停和速 度切換，可做到無加速的突變，能夠較好的減少機(jī)械臂的運(yùn)動沖擊和震蕩，從而有效的提高 工件的加工質(zhì)量，特別適用于高速高精加工。但是對稱的點(diǎn)到點(diǎn)運(yùn)動方法，但是如果加加速 度取得過大，在啟動和停止時(shí)運(yùn)動構(gòu)件會有明顯的振動，如果加加速度取得過小，就無法保 證加工的效率。對于機(jī)械臂的點(diǎn)到點(diǎn)運(yùn)動規(guī)劃，啟動階段可以不考慮振動問題，但是停止階 段必須考慮振動的影響，簡單的采用較小的速度策略，來抑制振動，必然會帶來加工效率低 的結(jié)果。

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

[0003] 為解決上述問題，本發(fā)明的目的在于提供一種在保證加工效率的情況，減少機(jī)械 振動，能夠適應(yīng)各種復(fù)雜運(yùn)動情形的機(jī)械臂點(diǎn)到點(diǎn)運(yùn)動控制方法。
[0004] 為實(shí)現(xiàn)上述目的，本發(fā)明的技術(shù)方案為：
[0005] 根據(jù)加加速度比以及機(jī)床硬件限制條件，進(jìn)行時(shí)間規(guī)劃;進(jìn)而根據(jù)各階段的時(shí)間 值求出的速度曲線以及起始點(diǎn)，進(jìn)行插補(bǔ)運(yùn)算計(jì)算出中間點(diǎn)的坐標(biāo)值，根據(jù)坐標(biāo)值變化向 相應(yīng)坐標(biāo)輸出脈沖信號，控制各執(zhí)行元件的進(jìn)給速度、進(jìn)給方向和進(jìn)給長度量等，進(jìn)而完成 工件的加工任務(wù)。
[0006] 對加速階段的最大加加速度J1，減速階段的最大加加速度J2,加速階段的最大限 制加速度A1，減速階段的最大限制加速度A2根據(jù)實(shí)際運(yùn)動需要進(jìn)行調(diào)整，能適應(yīng)復(fù)雜極端 的運(yùn)動，尤其是對末位置狀態(tài)有較高精度要求的運(yùn)動情況，在原有的S型軌跡規(guī)劃的基礎(chǔ)進(jìn) 行改進(jìn)，保留S型軌跡規(guī)劃的優(yōu)點(diǎn)。
[0007] 進(jìn)一步地，包括以下步驟：
[0008] 第一步，首先確定勻加加速度階段的時(shí)間t 根據(jù)末狀態(tài)位移約束和速度約 束條件以及加加速度比，計(jì)算出t jl，t j2;
[0009] 第二步，判斷規(guī)劃出的最大速度是否超過了所要求的最大限制速度值，若超過最 大限制速度值則令規(guī)劃出的最大速度等于最大限制速度值，根據(jù)速度公式重新計(jì)算tjl，并 根據(jù)tjl，tj2的關(guān)系式計(jì)算出tj2，
[0010] 判斷規(guī)劃出的最大加速度值是否滿足最大限制加速度，若超過所要求的最大限制 加速度值，則重新計(jì)算tjl，tj2,且tjl，tj2最終確定；
[0011] 第三步，確定勻加速階段的時(shí)間tal和勻減速階段的時(shí)間ta2,先考慮不存在勻速 階段的情況，根據(jù)末狀態(tài)位移約束和速度約束條件以及加速度比，計(jì)算出tal，ta2;
[0012] 第四步，計(jì)算規(guī)劃出的最大速度值是否超過最大限制速度值，若超出，則根據(jù)最大 限制速度值重新計(jì)算tal，并根據(jù)tal，ta2的關(guān)系式計(jì)算出ta2,此時(shí)，tal，ta2最終確定； [0013]第五步，根據(jù)已得到的幻1，幻2，丨 &1，丨&2來計(jì)算位移值，判斷是否滿足所要求的最 大位移值，若不滿足，則計(jì)算勻速階段的時(shí)間tv，此時(shí)所有時(shí)間量t j 1，t j2，tal，ta2，tv均已 確定。
[0014] 把整個(gè)運(yùn)動控制過程分為5步，使得復(fù)雜的軌跡規(guī)劃過程變得簡單有序，方便編程 實(shí)現(xiàn)。
[0015] 進(jìn)一步地，各時(shí)間節(jié)點(diǎn)的位移、速度、加速度、加加速度的表達(dá)公式如下：
[0016] T0 時(shí)刻
[0017] jerk〇 = 〇，Ao = 0，Vo = 0，So = 0
[0018] T1 時(shí)刻

[0053]其中：J1表示加速階段的最大加加速度(Jerk); J2表示減速階段的最大加加速度； A1表示加速階段的最大限制加速度;A2表示減速階段的最大限制加速度;V表示最大限制速 度;S表示運(yùn)動所要求的位移，其中，Rl = J2/Jl，R2=A2/Al;tjl表示加速時(shí)勻加加速度階段 的時(shí)間為;tj2表示減速時(shí)勻加加速度階段的時(shí)間；tal為勻加速階段時(shí)間；tj2表示勻減速 階段的時(shí)間；tv.為勻速階段的時(shí)間;A5表示減速階段中最大加加速度；；V3表示最大速度； V7表示末位置時(shí)速度，S7表示末位置時(shí)位移。
[0054 ]進(jìn)一步地，確定勾加加速度階段的時(shí)間t j 1、t j 2:
[0055] 位移約束:S7 = S
[0056] 速度約束:V7 = 0
[0057] 則，利用S7和V7的公式可以組成方程組(其中，tal、ta2、tv均為0):
[0058] ， : \S1 = J^j; +
[0059] 解得 t j 1、t j 2 的關(guān)系式為tji = ^/.Rj^j.2 _，令
[0060] 然后，
[0062]判斷最大速度V3與最大限制速度V的大小，若V3大于V，則令V3等于V，根據(jù)V3的表 達(dá)公式重新計(jì)算t jl:
[0064]分別判斷加速時(shí)最大加速度Ai與最大限制加速度A1的大小，和減速時(shí)最大加速度 A5與最大限制加速度A2的大小，若M大于A1，則，令心等于A1，根據(jù)公式重新求得tjl;若A5大 于A2，則令A(yù)5等于A2，根據(jù)公式重新求得t j2;
[0066] 至此，tjl、tj2已經(jīng)最終確定。
[0067] 進(jìn)一步地，確定勻加速階段的時(shí)間tal和勻減速階段的時(shí)間ta2:
[0068] 首先考慮最大速度未到達(dá)最大限制速度的情況
[0069] 位移約束:S7 = S，速度約束:V7 = 0 [0070]由位移約束可知tal和ta2的關(guān)系式：
[0072]帶入速度約束方程后可得一個(gè)關(guān)于tal的一元二次方程，其中tal取方程的非負(fù) 根，并根據(jù)tal解得ta2的值；
[0078]判斷最大速度V3與最大限制速度V的大小，若V3大于V，則令V3等于V，根據(jù)V3的表 達(dá)公式重新計(jì)算tal，ta2
[0080] 至此，tal，ta2已最終確定。
[0081] 與現(xiàn)有技術(shù)相比，本發(fā)明具有以下有益效果：
[0082] 本發(fā)明與現(xiàn)有技術(shù)相比有如下優(yōu)點(diǎn)和效果：
[0083] 現(xiàn)有的點(diǎn)到點(diǎn)3階對稱運(yùn)動軌跡規(guī)劃控制方法中，加速段和減速段曲線對稱，各運(yùn) 動參數(shù)也對稱，不能進(jìn)行調(diào)節(jié)，不能適應(yīng)復(fù)雜的、極端的運(yùn)動軌跡。本發(fā)明可對加速階段的 最大加加速度J1，減速階段的最大加加速度J2,加速階段的最大限制加速度A1，減速階段的 最大限制加速度A2根據(jù)實(shí)際運(yùn)動需要進(jìn)行調(diào)整，能適應(yīng)更多復(fù)雜極端的運(yùn)動，尤其是對末 位置狀態(tài)有較高精度要求的情況，并且能保證運(yùn)動時(shí)間最優(yōu)。同時(shí)，本發(fā)明兼容了 3階對稱 運(yùn)動控制方法的計(jì)算速度短，運(yùn)動平穩(wěn)、減少沖擊、震蕩等優(yōu)點(diǎn)，特別適合機(jī)械臂等高速高 精運(yùn)動領(lǐng)域，提高了運(yùn)動的精度和效率。
【附圖說明】
[0084] 圖1為本發(fā)明機(jī)械臂加速度變化圖；
[0085]圖2為本發(fā)明機(jī)械臂運(yùn)行狀態(tài)圖。
【具體實(shí)施方式】
[0086] 為了使本發(fā)明的目的、技術(shù)方案及優(yōu)點(diǎn)更加清楚明白，以下結(jié)合附圖及實(shí)施例，對 本發(fā)明進(jìn)行進(jìn)一步詳細(xì)說明。應(yīng)當(dāng)理解，此處所描述的具體實(shí)施例僅僅用以解釋本發(fā)明，并 不用于限定本發(fā)明。
[0087] 相反，本發(fā)明涵蓋任何由權(quán)利要求定義的在本發(fā)明的精髓和范圍上做的替代、修 改、等效方法以及方案。進(jìn)一步，為了使公眾對本發(fā)明有更好的了解，在下文對本發(fā)明的細(xì) 節(jié)描述中，詳盡描述了一些特定的細(xì)節(jié)部分。對本領(lǐng)域技術(shù)人員來說沒有這些細(xì)節(jié)部分的 描述也可以完全理解本發(fā)明。
[0088] 機(jī)械臂點(diǎn)到點(diǎn)運(yùn)動控制方法，始末狀態(tài)的速度，加速度，加加速度均為零，即始末 狀態(tài)均相對靜止，且位移能達(dá)到所要求的值并保證精度，保證運(yùn)動時(shí)間最短。其加速階段和 減速階段并不對稱，且可根據(jù)情況調(diào)整參數(shù)以達(dá)到實(shí)際運(yùn)動的目標(biāo)要求。具體可調(diào)參數(shù)包 括加速階段的最大加加速度，減速階段的最大加加速度，加速階段的最大限制加速度，減速 階段的最大限制加速度，最大限制速度。
[0089] 已知加工軌跡位移、機(jī)床硬件限制條件:最大限制速度、最大限制加速度、最大加 加速度，根據(jù)機(jī)床硬件限制，對時(shí)間進(jìn)行軌跡規(guī)劃，為滿足各種運(yùn)動情形，根據(jù)機(jī)械臂啟動、 停止的精度要求，確定加加速度比R1，加速度比R2，R1=J2/J1，，R2=A2/A1，
[0090] 其中：J1是加速段的最大加加速度，J2是減速段的最大加加速度，A1是加速段的最 大限制加速度，A2是減速段的最大限制加速度，同時(shí)兼顧運(yùn)動效率最優(yōu)。
[0091] 各時(shí)間節(jié)點(diǎn)的位移、速度、加速度、加加速度的表達(dá)公式如下：
[0092] T0 時(shí)刻
[0093] Jerko = 0 ,Ao = 0 ,Vo = 0 ,So = 0

[0129]其中：J1表示加速階段的最大加加速度(Jerk); J2表示減速階段的最大加加速度； A1表示加速階段的最大限制加速度;A2表示減速階段的最大限制加速度;V表示最大限制速 度;S表示運(yùn)動所要求的位移，其中，Rl = J2/Jl，R2=A2/Al;tjl表示加速時(shí)勻加加速度階段 的時(shí)間為;tj2表示減速時(shí)勻加加速度階段的時(shí)間；tal為勻加速階段時(shí)間；tj2表示勻減速 階段的時(shí)間；tv.為勻速階段的時(shí)間;A5表示減速階段中最大加加速度;V3表示最大速度;V7 表不末位置時(shí)速度;7表不末位置時(shí)位移。
[0130]本發(fā)明的工作步驟為：
[0131] 第一步，從機(jī)械臂控制系統(tǒng)中，得到各線段軌跡的起點(diǎn)和終點(diǎn)坐標(biāo)值等數(shù)據(jù)，進(jìn)而 計(jì)算出各線段軌跡的長度，根據(jù)控制系統(tǒng)以及工件的工藝要求分別確定加減速階段的加加 速度、最大限制加速度和最大限制速度。
[0132] 第二步，確定已知參數(shù):J1，表示加速階段的最大加加速度(Jerk); J2,表示減速階 段的最大加加速度;A1，表示加速階段的最大限制加速度;A2,表示減速階段的最大限制加 速度;V，表示最大限制速度;S，表示運(yùn)動所要求的位移。其中，Rl = J2/J1，R2=A2/A1.
[0133] 第三步，假設(shè)各運(yùn)動階段的時(shí)間量:設(shè)加速時(shí)勻加加速度階段的時(shí)間為t j 1;設(shè)減 速時(shí)勻加加速度階段的時(shí)間為t j 2 ;設(shè)勻加速階段是時(shí)間為t a 1;設(shè)勻減速階段的時(shí)間為 t j2;設(shè)勻速階段的時(shí)間為tv.
[0134] 第四步，求解各運(yùn)動階段時(shí)間量，具體如下：
[0135]按先后順序求解tjl，tj2，tal，ta2，tv
[0136] 求解tjl，tj2
[0137] 首先考慮僅有tj l，tj2的情況
[0138] 位移約束:S7 = S
[0139] 速度約束:V7 = 0
[0140] 則，利用S7和V7的公式可以組成方程組(其中，tal、ta2、tv均為0):
[0141] \ , ' ,
[0142] 解得tjl、tj2的關(guān)系式為tj:1 =：
[0143]然后，
[0145]判斷最大速度V3與最大限制速度V的大小，若V3大于V，則令V3等于V，根據(jù)V3的表 達(dá)公式重新計(jì)算tjl
[0147]分別判斷加速時(shí)最大加速度Ai與最大限制加速度A1的大小，和減速時(shí)最大加速度 A5與最大限制加速度A2的大小。若&大于A1，則，令&等于A1，根據(jù)公式重新求得tjl;若A5大 于A2，則令A(yù)5等于A2，根據(jù)公式重新求得t j2.
[0149] 至此，tjl、tj2已經(jīng)最終確定，可作為已知量在之后的計(jì)算中使用。
[0150] 求解 tal，ta2
[0151] 首先考慮最大速度未到達(dá)最大限制速度的情況
[0152] 位移約束:S7 = S，速度約束:V7 = 0
[0153] 由位移約束可知tal和ta2的關(guān)系式：
[0155]帶入速度約束方程后可得一個(gè)關(guān)于tal的一元二次方程，其中tal取方程的非負(fù) 根，并根據(jù)tal解得ta2的值。
[0161]判斷最大速度V3與最大限制速度V的大小，若V3大于V，則令V3等于V，根據(jù)V3的表 達(dá)公式重新計(jì)算tal，ta2
[0163] 至此，tal，ta2已最終確定。
[0164] 此時(shí)，tjl，tj2，tal，ta2已確定，代入S7的表達(dá)公式中，并令S7等于S，求出tv。
[0165] 至此第一段待加工軌跡的加減速變化時(shí)間都已經(jīng)求出，根據(jù)加減速變化時(shí)間，可 以得到此待加工軌跡的速度曲線。
[0166] 第五步:判斷是否還存在其他加工軌跡線段如果還有則重復(fù)第二、三、四步，如果 沒有其他線段則進(jìn)行第六步。
[0167] 第六步:根據(jù)求出的速度曲線以及起始點(diǎn)，進(jìn)行插補(bǔ)運(yùn)算計(jì)算出中間點(diǎn)的坐標(biāo)值， 根據(jù)坐標(biāo)值變化向相應(yīng)坐標(biāo)輸出脈沖信號，控制各執(zhí)行元件的進(jìn)給速度、進(jìn)給方向和進(jìn)給 長度量等，進(jìn)而完成工件的加工任務(wù)。
【主權(quán)項(xiàng)】
1. 機(jī)械臂點(diǎn)到點(diǎn)運(yùn)動控制方法，包括加工軌跡位移、機(jī)床硬件限制條件：最大限制速 度、最大限制加速度、最大加加速度，根據(jù)機(jī)床硬件限制，對軌跡進(jìn)行時(shí)間規(guī)劃，其特征在 于:為滿足各種運(yùn)動情形，根據(jù)機(jī)械臂啟動、停止的精度要求，確定加加速度比R1，加速度比 R2，R1=J2/J1，，R2=A2/A1， 其中：Jl是加速段的最大加加速度，J2是減速段的最大加加速度，Al是加速段的最大限 制加速度，A2是減速段的最大限制加速度，同時(shí)兼顧運(yùn)動效率最優(yōu)； 根據(jù)加加速度比以及機(jī)床硬件限制條件，進(jìn)行時(shí)間規(guī)劃;進(jìn)而根據(jù)各階段的時(shí)間值求 出的速度曲線以及起始點(diǎn)，進(jìn)行插補(bǔ)運(yùn)算計(jì)算出中間點(diǎn)的坐標(biāo)值，根據(jù)坐標(biāo)值變化向相應(yīng) 坐標(biāo)輸出脈沖信號，控制各執(zhí)行元件的進(jìn)給速度、進(jìn)給方向和進(jìn)給長度量等，進(jìn)而完成工件 的加工任務(wù)。2. 根據(jù)權(quán)利要求1所述的機(jī)械臂點(diǎn)到點(diǎn)運(yùn)動控制方法，其特征在于，包括以下步驟： 第一步，首先確定勻加加速度階段的時(shí)間tj I、tj2，根據(jù)末狀態(tài)位移約束和速度約束條 件以及加加速度比，計(jì)算出tjl，tj2; 第二步，判斷規(guī)劃出的最大速度是否超過了所要求的最大限制速度值，若超過最大限 制速度值則令規(guī)劃出的最大速度等于最大限制速度值，根據(jù)速度公式重新計(jì)算tjl，并根據(jù) tjl，tj2的關(guān)系式計(jì)算出tj2， 判斷規(guī)劃出的最大加速度值是否滿足最大限制加速度，若超過所要求的最大限制加速 度值，則重新計(jì)算tjl，tj2,且tjl，tj2最終確定； 第三步，確定勻加速階段的時(shí)間tal和勻減速階段的時(shí)間ta2,先考慮不存在勻速階段 的情況，根據(jù)末狀態(tài)位移約束和速度約束條件以及加速度比，計(jì)算出tal，ta2; 第四步，計(jì)算規(guī)劃出的最大速度值是否超過最大限制速度值，若超出，則根據(jù)最大限制 速度值重新計(jì)算tal，并根據(jù)tal，ta2的關(guān)系式計(jì)算出ta2,此時(shí)，tal，ta2最終確定； 第五步，根據(jù)已得到的幻1，幻2，丨&1，丨&2來計(jì)算位移值，判斷是否滿足所要求的最大位 移值，若不滿足，則計(jì)算勻速階段的時(shí)間tv，此時(shí)所有時(shí)間量t jI，t j2，tal，ta2，tv均已確 定。3. 根據(jù)權(quán)利要求1所述的機(jī)械臂點(diǎn)到點(diǎn)運(yùn)動控制方法，其特征在于，各時(shí)間節(jié)點(diǎn)的位 移、速度、加速度、加加速度的表達(dá)公式如下： TO時(shí)刻 Jerko = O, A〇 = 0,Vo = O,So = O Tl時(shí)刻 Jerki = Ji Ai = Jitji T2時(shí)刻Jerk2 = 0 A2 = Jltjl其中：Jl表示加速階段的最大加加速度(Jerk) ;J2表示減速階段的最大加加速度;Al表 示加速階段的最大限制加速度;A2表示減速階段的最大限制加速度;V表示最大限制速度;S 表示運(yùn)動所要求的位移，其中，Rl=J2/Jl，R2=A2/Al;tjl表示加速時(shí)勻加加速度階段的時(shí) 間為;tj2表示減速時(shí)勻加加速度階段的時(shí)間；tal為勻加速階段時(shí)間；tj2表示勻減速階段 的時(shí)間；tv.為勻速階段的時(shí)間;A5表示減速階段中最大加加速度；；V3表示最大速度;V7表 不末位置時(shí)速度，S7表不末位置時(shí)位移。4. 根據(jù)權(quán)利要求2所述的機(jī)械臂點(diǎn)到點(diǎn)運(yùn)動控制方法，其特征在于，確定勻加加速度階 段的時(shí)間tjl、tj2: 位移約束:S7 = S 速度約束:V7 = O 貝IJ，利用S7和V7的公式可以組成方程組(其中，tal、ta2、tv均為0):解得的關(guān)j 然后， 判斷最大速度V3與最大限制速度V的大小，若V3大于V，則令V3等于V，根據(jù)V3的表達(dá)公 式重新計(jì)算tjl:分別判斷加速時(shí)最大加速度A1與最大限制加速度Al的大小，和減速時(shí)最大加速度A5與 最大限制加速度A2的大小，若A1大于Al，則，令A(yù)1等于Al，根據(jù)公式重新求得tjl;若A5大于 A2，則令A(yù)5等于A2，根據(jù)公式重新求得t.i2; 至此，tjl、tj2已經(jīng)最終確定。5. 根據(jù)權(quán)利要求2所述的機(jī)械臂點(diǎn)到點(diǎn)運(yùn)動控制方法，其特征在于，確定勻加速階段的 時(shí)間tal和勻減速階段的時(shí)間ta2: 首先考慮最大速度未到達(dá)最大限制速度的情況 位移約束:S7 = S，速度約束:V7 = 0 由位務(wù)約東可知tal和ta2的關(guān)系式：至此，tal，ta2已最終確定
【文檔編號】G05B19/19GK105892402SQ201610444859
【公開日】2016年8月24日
【申請日】2016年6月17日
【發(fā)明人】楊亮亮, 沈波, 吳達(dá)偉, 胡建, 時(shí)軍, 歐陽博
【申請人】浙江理工大學(xué)