機(jī)電控制原理及應(yīng)用

1、 機(jī)器運(yùn)動(dòng)控制的指令生成與實(shí)現(xiàn)葛為民天津理工學(xué)院機(jī)械工程學(xué)院機(jī)械電子工程2022/9/191Tianjin University of Technology 本章重點(diǎn)討論機(jī)器運(yùn)動(dòng)控制原理與分類、指令生成方法以及軌跡控制的實(shí)現(xiàn)問題。所介紹的三種指令生成技術(shù)可用于開環(huán)系統(tǒng)、閉環(huán)系統(tǒng)以及多軸控制系統(tǒng)。其中，數(shù)字微分分析器用于增量控制；直線插補(bǔ)器可在設(shè)定點(diǎn)之間產(chǎn)生常速度變化指令；圓弧插補(bǔ)器可在圓周上生成一系列設(shè)定點(diǎn)；三次多項(xiàng)式樣條函數(shù)軌跡規(guī)劃器可以對(duì)加速度和速度受控的多軸機(jī)器方便地生成復(fù)雜的運(yùn)動(dòng)軌跡。最后，給出了典型系統(tǒng)實(shí)例。 2022/9/192Tianjin University of Techn

2、ology61 運(yùn)動(dòng)控制原理與分類 凡是能進(jìn)行自動(dòng)化作業(yè)(包括加工、檢測(cè)、裝配及運(yùn)輸)的機(jī)器，都是多軸的機(jī)電一體化設(shè)備。最常見的有數(shù)控(NC)機(jī)床、工業(yè)機(jī)器人、自動(dòng)測(cè)量機(jī)以及自主式自動(dòng)導(dǎo)引車等。 隨著計(jì)算機(jī)數(shù)控(CNC)系統(tǒng)的應(yīng)用，這些自動(dòng)化機(jī)器的運(yùn)動(dòng)路徑或軌跡都是可編程的，有關(guān)的運(yùn)動(dòng)指令都是由計(jì)算機(jī)生成的。只有當(dāng)機(jī)器的各個(gè)軸沿著預(yù)定路徑協(xié)調(diào)運(yùn)動(dòng)時(shí)，由計(jì)算機(jī)程序編制的作業(yè)才能順利完成。因此，機(jī)器運(yùn)動(dòng)軌跡規(guī)劃及控制是機(jī)電一體化設(shè)備的一個(gè)重要組成部分。它的優(yōu)劣反映了機(jī)器的智能化程度。2022/9/193Tianjin University of Technology 數(shù)控機(jī)床的運(yùn)動(dòng)控制原理 圖6-

3、l為一種數(shù)控立銑床的信息流圖。NC零件程序由穿孔紙帶或磁帶，或者人工操作鍵盤輸入，經(jīng)過計(jì)算機(jī)處理后，區(qū)分為工藝數(shù)據(jù)和幾何數(shù)據(jù)。工藝數(shù)據(jù)由主軸速度、工具選擇以及冷卻液等開關(guān)功能組成。幾何數(shù)據(jù)經(jīng)過粗、精兩次插補(bǔ)計(jì)算，生成運(yùn)動(dòng)指令信號(hào)，輸入驅(qū)動(dòng)工作臺(tái)移動(dòng)的伺服系統(tǒng)。只要伺服系統(tǒng)工作正常，數(shù)控機(jī)床就能加工出所要求的零件形狀。2022/9/194Tianjin University of Technology數(shù)控機(jī)床的信息流圖 2022/9/195Tianjin University of Technology 插補(bǔ):就是產(chǎn)生一系列固定長(zhǎng)度的步距，近似不能直接到達(dá)的幾何圖形，使得工具路徑的最大偏差保持在

4、一個(gè)步長(zhǎng)以內(nèi)。數(shù)控機(jī)床采用了插補(bǔ)器生成運(yùn)動(dòng)控制指令。 粗插補(bǔ):采用離線近似算法，預(yù)先在整個(gè)高階曲線上選擇間隔的點(diǎn)列，將曲線分割為區(qū)段。 精插補(bǔ):在粗插補(bǔ)的基礎(chǔ)上,對(duì)每一區(qū)段采用簡(jiǎn)單的直線或圓弧，或者三次多項(xiàng)式樣條函數(shù)作近似計(jì)算，并把它們輸入數(shù)控機(jī)床運(yùn)動(dòng)控制器實(shí)現(xiàn)。2022/9/196Tianjin University of Technology 點(diǎn)位控制:將工具或零件(如機(jī)床主軸或工作臺(tái))由源點(diǎn)運(yùn)動(dòng)到規(guī)定的目標(biāo)點(diǎn)，以便在該點(diǎn)進(jìn)行加工作業(yè)。 數(shù)控機(jī)床根據(jù)所要完成的加工作業(yè)任務(wù)的不同，其運(yùn)動(dòng)控制分為兩種類型：點(diǎn)位控制和連續(xù)路徑控制，如表6-1所列。表中列出了運(yùn)動(dòng)控制類型、對(duì)應(yīng)的路徑以及應(yīng)用場(chǎng)合。

5、 連續(xù)路徑控制:同時(shí)控制每一個(gè)軸的位置和速度，使得它們同步協(xié)調(diào)到達(dá)目標(biāo)點(diǎn)。 2022/9/197Tianjin University of Technology表6-1 運(yùn)動(dòng)控制類型 2022/9/198Tianjin University of Technology工業(yè)機(jī)器人運(yùn)動(dòng)控制方案 點(diǎn)位控制機(jī)器人: 通常用于物料搬運(yùn)作業(yè)，即在一個(gè)地點(diǎn)抓取物體，輸送到另一個(gè)地點(diǎn)放下。它們可以在一些不同的編程位置停留，但是，無法確定編程點(diǎn)之間的具體路徑或軌跡。工業(yè)機(jī)器人按照控制水平可以分為三種不同的類型： 連續(xù)路徑控制機(jī)器人:主要用于噴漆和弧焊，其特征是“示教”。它預(yù)先由人工將機(jī)械手沿著期望軌跡移動(dòng)并記錄

6、整個(gè)運(yùn)動(dòng)，所獲得的完整軌跡按照時(shí)間順序點(diǎn)列儲(chǔ)存在計(jì)算機(jī)中，然后再回放出來控制機(jī)器人運(yùn)動(dòng)。 控制路徑機(jī)器人：通用工業(yè)機(jī)器人都屬于這一類。軌跡是根據(jù)預(yù)先定義并存入存儲(chǔ)器中的某些中間點(diǎn)由控制計(jì)算機(jī)生成的。 2022/9/199Tianjin University of Technology 圖6-2 圓柱型工業(yè)機(jī)器人運(yùn)動(dòng)控制 2022/9/1910Tianjin University of Technology 參看圖6-2，假設(shè)圓柱型工業(yè)機(jī)器人在水平面內(nèi)工作，只考慮它的第二和第三連桿的運(yùn)動(dòng)。操作工具安裝在手爪的中心，并與xy平面垂直。沿著z軸方向進(jìn)給工具到目標(biāo)位置。然后，沿著給定工作路徑完成技術(shù)操

7、作；或者，在路徑的給定目標(biāo)點(diǎn)上，完成材料處理或裝配作業(yè)等。 由于該機(jī)械手具有旋轉(zhuǎn)和位移兩個(gè)連桿。令q1為旋轉(zhuǎn)連桿坐標(biāo)，q3為位移運(yùn)動(dòng)坐標(biāo)。注意，q2是z軸坐標(biāo)，在這里為常量，圖中沒有表示。 機(jī)械手的正向運(yùn)動(dòng)學(xué)方程：2022/9/1911Tianjin University of Technology將q1和q3的任意兩個(gè)數(shù)值轉(zhuǎn)換為xy平面中的直角坐標(biāo)，可以表示為 如果q1和q3是變化量，并且它們的速度值為 和 ，那么，沿著x， y軸的瞬時(shí)速度值為此正向運(yùn)動(dòng)學(xué)方程定義了由機(jī)械手連桿坐標(biāo)計(jì)算空間直角坐標(biāo)系中的運(yùn)動(dòng)軌跡。2022/9/1912Tianjin University of Technol

8、ogy 由方程(6-1)和方程(6-2)，可求得由空間直角坐標(biāo)計(jì)算機(jī)械手各個(gè)連桿坐標(biāo)的運(yùn)動(dòng)方程，即機(jī)械手的逆向運(yùn)動(dòng)學(xué)方程 ：2022/9/1913Tianjin University of Technology根據(jù)程序編譯或機(jī)器人示教，可以獲得運(yùn)動(dòng)路徑各個(gè)區(qū)段的源點(diǎn)和目標(biāo)點(diǎn)的坐標(biāo)。例如，對(duì)于圖6-2中的第一段直線，兩端點(diǎn)的坐標(biāo)分別為(xA，yA)和(xB，yB)； 通過軌跡規(guī)劃器專用計(jì)算機(jī)軟件，給出希望運(yùn)動(dòng)路徑或軌跡的中間點(diǎn)坐標(biāo)(x，y)和每一點(diǎn)的速度值 ；對(duì)于路徑中每一點(diǎn)(x，y)，應(yīng)用逆向運(yùn)動(dòng)方程決定相應(yīng)的ql，q3， ， 值?？偨Y(jié)以上結(jié)果，機(jī)器人運(yùn)動(dòng)控制方案可以采用方塊圖表示： 根據(jù)以上

9、分析，機(jī)器人運(yùn)動(dòng)控制的一般方案如下： 2022/9/1914Tianjin University of Technology圖6-3 運(yùn)動(dòng)控制的基本原理方案 2022/9/1915Tianjin University of Technology 數(shù)字微分分析(DDA)法是由計(jì)算機(jī)軟件生成參考脈沖序列的一種方法。它以數(shù)字量(整數(shù))表達(dá)加速度、速度及位置坐標(biāo)，采用求和公式進(jìn)行加速度到速度和速度到位置的近似數(shù)值積分，從而產(chǎn)生增量式運(yùn)動(dòng)控制指令。DDA法在簡(jiǎn)易數(shù)控系統(tǒng)中不僅能應(yīng)用于點(diǎn)位控制，也可完成雙軸或多軸系統(tǒng)的連續(xù)路徑控制。 下面，先討論兩種速度分布情況的DDA原理，然后把它們推廣應(yīng)用到直線和圓弧

10、運(yùn)動(dòng)控制指令的生成。62 數(shù)字微分分析器2022/9/1916Tianjin University of Technology621 常速度分布情況 假定速度v為常量，那么，位置函數(shù)p(t)可以表示為 令N是n的四舍五入取整，而n為 于是，可寫出等價(jià)的差分方程：P和V分別是p和v的整數(shù)。 進(jìn)一步，如果將速度V的量綱轉(zhuǎn)換為單位采樣周期的位置增量，那么，上式中的T恒等于1。因而，式(6-12)可以改寫為2022/9/1917Tianjin University of Technology 這樣，只要采用一個(gè)時(shí)鐘和兩個(gè)寄存器，就可以生成常速度分布運(yùn)動(dòng)控制指令。其中，位置寄存器P存放P(tk)數(shù)據(jù)，速

11、度寄存器V存放V數(shù)據(jù)。每當(dāng)時(shí)鐘信號(hào)電平由0變到1，V中的內(nèi)容便累加到P中一次。P寄存器的溢出脈沖可作為控制步進(jìn)電動(dòng)機(jī)的參考脈沖。每一個(gè)參考脈沖代表一個(gè)步進(jìn)角或基本長(zhǎng)度單位(BLU)。當(dāng)時(shí)鐘頻率固定時(shí)，輸出參考脈沖速率由V的數(shù)值決定。圖6-5表示了由DDA法生成常速度分布運(yùn)動(dòng)控制指令的原理圖。2022/9/1918Tianjin University of Technology 圖6-5 常速度分布運(yùn)動(dòng)控制指令生成原理圖(a)常速度分布圖；(b)運(yùn)動(dòng)控制指令生成原理 DDA法在計(jì)算機(jī)中實(shí)現(xiàn)時(shí)，必須采用整型變量算法，因?yàn)檎麛?shù)運(yùn)算速度快，可產(chǎn)生高速率參考脈沖。這樣，P(tk)，V以及N都必須用二進(jìn)制

12、整數(shù)表示。2022/9/1919Tianjin University of Technology 例6-1 根據(jù)下列已知信息，設(shè)計(jì)常速度分布指令信號(hào)的生成方案： 總的希望轉(zhuǎn)角1000脈沖 希望速度51脈沖s 時(shí)鐘頻率10 kHz P和V寄存器字長(zhǎng)16位帶符號(hào)小數(shù) 解 ：V寄存器中的希望值為式中，已將“Hz”轉(zhuǎn)換為“加s”。單位“脈沖加”表示每一時(shí)鐘周期的脈沖數(shù)，即V寄存器的內(nèi)容向P寄存器每做一次加法所增加的脈沖數(shù)。2022/9/1920Tianjin University of Technology 將這個(gè)數(shù)轉(zhuǎn)換為帶符號(hào)16位二進(jìn)制小數(shù)： v0.005l167.1168215 脈沖加。經(jīng)取整

13、V167215 0000000101001110.0050964356脈沖加 應(yīng)注意，Vv，其誤差是由于在寄存器中必須用固定位數(shù)表示v所造成的。根據(jù)這個(gè)V值，希望的時(shí)鐘周期數(shù)(或輸出1000個(gè)脈沖所要求的加法次數(shù))為 經(jīng)過四舍五入取整后，實(shí)際使用的時(shí)鐘信號(hào)由“0”到“1”的轉(zhuǎn)換次數(shù) N196216加2022/9/1921Tianjin University of Technology 這樣，可保證實(shí)際得到的脈沖數(shù)VN1000，滿足題意要求。實(shí)現(xiàn)時(shí)，P寄存器的初始內(nèi)容為0，V寄存器中存入000000010100111。時(shí)鐘信號(hào)由“0”到“1”每轉(zhuǎn)換一次，V寄存器中的數(shù)加到P寄存器中一次。加到第1

14、97次時(shí)，P寄存器將產(chǎn)生第一個(gè)溢出脈沖輸出，直到加了l96216次后，便完成1000個(gè)溢出脈沖輸出。 2022/9/1922Tianjin University of Technology622 常加速度分布情況 考慮到電動(dòng)機(jī)加速度的限制，經(jīng)常需要采用常加速度分布運(yùn)動(dòng)控制指令，如圖6-6所示。圖6-6(a)表示等加速度分布圖，先由靜止?fàn)顟B(tài)加速到步進(jìn)電動(dòng)機(jī)額定速度，維持等速運(yùn)行，然后又等減速到停止位置。在等加速度分布情況下，數(shù)字微分分析器的計(jì)算公式如下： 式中，P，V，A分別為位置、速度及加速度。速度V的量綱為單位采樣間隔(即采樣周期)的位置增量。加速度A的量綱是單位采樣間隔的V的增量。2022

15、/9/1923Tianjin University of Technology 圖6-6 常加速度分布運(yùn)動(dòng)控制指令生成原理圖 (a)等加速度分布圖；(b)等加速度分布運(yùn)動(dòng)指令的生成原理 2022/9/1924Tianjin University of Technology 圖6-6(b)是生成等加速度分布運(yùn)動(dòng)指令的原理，由一個(gè)時(shí)鐘和三個(gè)寄存器組成。每當(dāng)時(shí)鐘信號(hào)由“0”到“1”的轉(zhuǎn)換瞬間，寄存器V中的內(nèi)容加到寄存器P，同時(shí)，寄存器A中的內(nèi)容加到寄存器V。P和V的初始值都為零，A在t0時(shí)刻的初始值置正的最大值。在t1時(shí)刻，V達(dá)到它的極限值。這里，t1與寄存器A的內(nèi)容有關(guān)。在等加速段，寄存器P的溢出

16、速率是勻速增長(zhǎng)的。在加速段終點(diǎn)，寄存器A置0。在常速度段終點(diǎn)t2時(shí)刻，A置負(fù)的最大值，并且繼續(xù)進(jìn)行加法，直至t3時(shí)刻速度為零。加速度寄存器A、速度寄存器V以及總的步進(jìn)脈沖數(shù)可由速度分布圖決定。給定源點(diǎn)和目標(biāo)點(diǎn)位置坐標(biāo)、容許速度及容許加速度，三段時(shí)間關(guān)系式如下 2022/9/1925Tianjin University of Technology式中：ax容許加速度；vx容許速度； x0，xf源點(diǎn)和目標(biāo)點(diǎn)位置坐標(biāo)。 如果已知時(shí)鐘頻率為f(Hz)，則每一段的加法次數(shù) 為 令Ni是ni取整，那么，總的運(yùn)動(dòng)時(shí)間區(qū)間的加法次數(shù)為 2022/9/1926Tianjin University of Tech

17、nology 例6-2 生成下列等加速度分布指令信號(hào)：由零初始速度加速0.1s達(dá)到20000脈沖min，然后恒速度20000脈沖/min維持2s。最后，減速0.1s到停止位置。減速度與初始加速度相等。假設(shè)時(shí)鐘信號(hào)頻率為l000Hz，寄存器字長(zhǎng)為16位帶符號(hào)小數(shù)。希望在產(chǎn)生該速度分布時(shí)，只改變A寄存器的內(nèi)容。解：求解的問題是決定A寄存器中設(shè)置的希望值，以及每一段由設(shè)置信號(hào)引起的加法次數(shù)。 第一段 0t0.1s 即al3.333310-3109.2266667215脈沖加2截?cái)嗳≌?，Al1092150.00000000110110212脈沖加2 2022/9/1927Tianjin Univer

18、sity of Technology0.1s時(shí)間間隔內(nèi)，加法次數(shù)為 N10.1l000100加l00次加法后，實(shí)際達(dá)到的速度為 V1N1Al10900215脈沖/加 0.326416脈沖/加19958.496脈沖/min 第二段 0.1t2.1s A20，N22l0002000加 第三段 2.1t2.2s A2Al1092-15脈沖/加2，N3N1100加 這樣，所有三段的A值及加法運(yùn)算次數(shù)N分別為 Al109215， A20， A3-109215 N1l00， N22000， N3100 2022/9/1928Tianjin University of Technology623直線運(yùn)動(dòng) 圖

19、6-7表示了常速度直線運(yùn)動(dòng)控制情況。常速度向量V的運(yùn)動(dòng)方程可表示為 2022/9/1929Tianjin University of Technology 圖6-7 常速度直線運(yùn)動(dòng)(a)直線運(yùn)動(dòng)分布；(b)直線運(yùn)動(dòng)控制指令生成原理 2022/9/1930Tianjin University of Technology 例6-4 x和y步進(jìn)電動(dòng)機(jī)驅(qū)動(dòng)xy定位工作臺(tái)，二者的位置分辨率為10脈沖mm。希望以1000mmmin的速度產(chǎn)生與x軸成30角的直線運(yùn)動(dòng)，距離為500mm。采用16位帶符號(hào)寄存器，時(shí)鐘頻率為1000Hz。解 要求時(shí)鐘周期數(shù)(即加法次數(shù)，簡(jiǎn)記為“加”)為 因此，x和y的速度分別為

20、2022/9/1931Tianjin University of Technology 采用常速度分布實(shí)現(xiàn)直線運(yùn)動(dòng)控制指令時(shí)，只要1個(gè)時(shí)鐘和4個(gè)寄存器(x軸和y軸各兩個(gè))，其中，兩個(gè)作為速度寄存器，另外兩個(gè)作為位置寄存器。速度寄存器的內(nèi)容設(shè)為Vx和Vy。位置寄存器的初值設(shè)為0。時(shí)鐘信號(hào)每由0到1轉(zhuǎn)換一次，速度寄存器的內(nèi)容向位置寄存器累加一次。位置寄存器的溢出脈沖就是步進(jìn)電動(dòng)機(jī)的參考脈沖指令。 2022/9/1932Tianjin University of Technology624 圓周運(yùn)動(dòng) 在xy平面中生成圓運(yùn)動(dòng)是一種比較復(fù)雜的情況，其中一根軸的運(yùn)動(dòng)速度正比于另一根軸的位移。圖6-8表示了

21、在xy平面上生成圓周運(yùn)動(dòng)的有關(guān)變量的定義。 圖6-8 圓周運(yùn)動(dòng)的生成2022/9/1933Tianjin University of Technology根據(jù)圖6-9所示的圓周運(yùn)動(dòng)，假設(shè)角速度 常數(shù)，那么，可得下列方程：圓弧線速度xy直角坐標(biāo)位置x，y軸上的速度 根據(jù)x，y軸的速度表達(dá)式(6-29)和(6-30)、可寫出等價(jià)的差分表達(dá)式: 2022/9/1934Tianjin University of Technology 在實(shí)現(xiàn)時(shí)，x每前進(jìn)一步，必須將這一步與 的乘積加到y(tǒng)速度寄存器；y每前進(jìn)一步，則將其與 的乘積從x速度寄存器中減去。由于每一步的x和y的值為1，所以乘運(yùn)算是沒有必要的,只

22、需要向相應(yīng)的速度寄存器直接加/減 。注意， 的量綱為單位采樣間隔的弧度增量角。 同時(shí)，位置寄存器每一步都按下列關(guān)系式改變其內(nèi)容： 2022/9/1935Tianjin University of Technology式中，速度vx，vy的量綱為單位采樣間隔的位置增量。在運(yùn)算方程(6-33)和(6-34)時(shí)，每一步，只需要將速度寄存器的內(nèi) 容向位置寄存器相加一次即可。 根據(jù)以上原理，生成圓周運(yùn)動(dòng)控制指令的方法如圖6-9所示。x軸和y軸都由三個(gè)寄存器組成：加速度寄存器分別存放- 和+ ，速度寄存器分別存放Vx和Vy，位置寄存器分別存放x和y。2022/9/1936Tianjin Universit

23、y of Technology 每當(dāng)時(shí)鐘信號(hào)由0到1轉(zhuǎn)換一次時(shí)，速度寄存器的內(nèi)容就向位置寄存器累加一次，加速度寄存器的內(nèi)容就向速度寄存器累加一次。若位置寄存器有溢出脈沖時(shí)，則溢出脈沖作為步進(jìn)電動(dòng)機(jī)的控制指令脈沖。 圖6-9 圓周運(yùn)動(dòng)控制指令生成方法 2022/9/1937Tianjin University of Technology例6-4 假設(shè)欲產(chǎn)生半徑為50mm的45圓弧，沿圓弧的步進(jìn)速率為1000mmmin位置分辨率為10脈沖mm，寄存器為16位帶符號(hào)小數(shù)，時(shí)鐘頻率為1000Hz，初始角i為0。設(shè)計(jì)產(chǎn)生回運(yùn)動(dòng)指令所需的所有寄存器的初值和加法次數(shù)。 解 (1)圓周運(yùn)動(dòng)角速度 (2)初始位

24、置坐標(biāo) 2022/9/1938Tianjin University of Technology(3)各個(gè)寄存器的初始值 注意，這里pxi0脈沖是由于寄存器只保留一步的余數(shù)(即一個(gè)脈沖的小數(shù)部分)。 最后，為了生成45圓弧，還必須計(jì)算時(shí)鐘由“0”到“1”的轉(zhuǎn)換次數(shù)或加法次數(shù)。 2022/9/1939Tianjin University of Technology63 直線與圓弧插補(bǔ)器 對(duì)于一個(gè)復(fù)雜的輪廓圖形往往先通過直接函數(shù)計(jì)算法(粗插補(bǔ)器)，由位置函數(shù)方程產(chǎn)生一系列參考點(diǎn)，然后，在相鄰兩參考點(diǎn)之間利用直線或圓弧進(jìn)行精插補(bǔ)。例如，圖6-10表示了采用端面銑刀直接銑出大型齒輪的例子。根據(jù)輸入?yún)?shù)P

25、s，PE，R及，通過函數(shù)計(jì)算在漸開線上產(chǎn)生參考點(diǎn)Pl，P2，PE。然后通過精插補(bǔ)產(chǎn)生位置坐標(biāo)xi，yi，i1，2，N。 2022/9/1940Tianjin University of Technology 圖6-10 生成漸開線的粗精插補(bǔ)方法 2022/9/1941Tianjin University of Technology631 直線插補(bǔ)器單軸情況 通常，被插補(bǔ)的軌跡函數(shù)由粗插補(bǔ)器生成的一組參考點(diǎn)存放在存儲(chǔ)器中，直線插補(bǔ)器作為精插補(bǔ)產(chǎn)生中間點(diǎn)。其中，2022/9/1942Tianjin University of Technology 兩軸情況 假設(shè)在xy平面上有一段直線，其起始點(diǎn)坐標(biāo)

26、為(x0，y0)，終點(diǎn)坐標(biāo)為(xf，yf)，切割的線速度為v，采樣周期為T，則完成該段直線插補(bǔ)所需的采樣周期數(shù)N為其中，n1，2，N；初始條件為(x0，y0)；直線插補(bǔ)的遞推公式可以表示為2022/9/1943Tianjin University of Technology632 圓弧插補(bǔ)器 在圓弧插補(bǔ)器中，圓以參量方程表示為 圖6-10 圓弧插補(bǔ)器原理 2022/9/1944Tianjin University of Technology 一次遞推算法 參考圖6-10，Pn+l點(diǎn)的坐標(biāo)可用Pn的坐標(biāo)(即xnRcos，ynRsin)表示如下： 在給定的條件下，圓弧插補(bǔ)的參考點(diǎn)可用遞推公式求解。

27、 2022/9/1945Tianjin University of Technology二次遞推算法 1、在方程(6-51)的第二等式右邊加減2、在方程（6-52）的等式右邊加/減 得： 2022/9/1946Tianjin University of Technology 方程組（6-53）和（6-54），以及上面方程組是兩組二次遞推圓插補(bǔ)器。兩組公式具有同等結(jié)果。只要前兩點(diǎn)（x0，y0）和（x1，y1）在圓上，不管cos或者sin是否存在計(jì)算誤差，（xn，yn），n=0，1，2，N，都在同一個(gè)圓上。實(shí)現(xiàn)時(shí)，先由初始點(diǎn)（x0，y0）一次遞推計(jì)算出第一點(diǎn)（x1，y1）；然后，采用二次遞推公式計(jì)

28、算出其余的參考點(diǎn)。同理還可以得到另外一組公式：2022/9/1947Tianjin University of Technology64 樣條函數(shù)軌跡規(guī)劃器 樣條函數(shù)通常是指多項(xiàng)式函數(shù)?！皹訔l”這一名詞來源于造船工業(yè)的仿樣。三次多項(xiàng)式樣條函數(shù)可以用來在計(jì)算機(jī)內(nèi)存中細(xì)密地儲(chǔ)存指令輸入，以及實(shí)時(shí)產(chǎn)生復(fù)雜的機(jī)器運(yùn)動(dòng)控制指令序列。它可以用于點(diǎn)位運(yùn)動(dòng)控制，也可以用于連續(xù)路徑控制，以及多軸系統(tǒng)的協(xié)調(diào)運(yùn)動(dòng)控制。2022/9/1948Tianjin University of Technology641 點(diǎn)位運(yùn)動(dòng)控制 一切機(jī)器的傳動(dòng)系統(tǒng)都具有有限的速度和加速度。點(diǎn)位運(yùn)動(dòng)控制是在容許加速度條件下，盡可能以最大速

29、度由源坐標(biāo)位置運(yùn)動(dòng)到目的坐標(biāo)位置，對(duì)于兩點(diǎn)之間的軌跡是沒有精度要求的。在許多定位系統(tǒng)中，為了精密控制和可靠工作，控制加速度是關(guān)鍵。此外，一般的機(jī)器都是多軸的，所有的運(yùn)動(dòng)軸都必須同時(shí)啟動(dòng)同時(shí)停止，即完成協(xié)調(diào)運(yùn)動(dòng)。在直角坐標(biāo)系中的協(xié)調(diào)運(yùn)動(dòng)將產(chǎn)生直線運(yùn)動(dòng)軌跡。 2022/9/1949Tianjin University of Technology 下面，集中討論利用三次多項(xiàng)式樣條函數(shù)，生成點(diǎn)位控制指令的設(shè)計(jì)方法。 （1）單軸運(yùn)動(dòng)的情況 假設(shè)機(jī)器的x軸由t0時(shí)刻的x0運(yùn)動(dòng)到tf時(shí)刻的xf，軸的速度在運(yùn)動(dòng)前后都為零。機(jī)器運(yùn)動(dòng)時(shí)，由起始點(diǎn)x0加速，在運(yùn)動(dòng)到中點(diǎn)時(shí)達(dá)到某最大速度，然后立即開始減速，抵達(dá)xf時(shí)

30、停止。 采用三次多項(xiàng)式樣條函數(shù)，路徑表示為 對(duì)t微分一次，得速度表達(dá)式為2022/9/1950Tianjin University of Technology位置和速度必須滿足下列邊界條件，即這樣就可以求得： 三次多項(xiàng)式樣條函數(shù)點(diǎn)位控制路徑指令為 拋物線速度分布為 2022/9/1951Tianjin University of Technology 線性加速度為 路徑、速度以及加速度時(shí)間曲線如圖6-17所示。由圖可以看出，采用三次多項(xiàng)式樣條函數(shù)的點(diǎn)位控制，其最大速度和最大加速度分別為2022/9/1952Tianjin University of Technology圖6-11 路徑、速度以

31、及加速度時(shí)間曲線 2022/9/1953Tianjin University of Technology 假設(shè)vmax和amax分別為控制系統(tǒng)的最大容許跟蹤速度和最大容許跟蹤加速度，那么，給定起始點(diǎn)坐標(biāo)z0和終點(diǎn)坐標(biāo)zf，所對(duì)應(yīng)的最小運(yùn)動(dòng)時(shí)間區(qū)間分別為 在實(shí)際運(yùn)動(dòng)時(shí)，為了保證速度和加速度都不超過容許值，最小運(yùn)動(dòng)時(shí)間應(yīng)選用二者中較長(zhǎng)的一個(gè)，即 2022/9/1954Tianjin University of Technology（2）多軸系統(tǒng)的運(yùn)動(dòng) 假設(shè)有n個(gè)自由度，分別有n個(gè)起始坐標(biāo)0i，n個(gè)終點(diǎn)坐標(biāo)fi，n個(gè)容許最大速度vimax以及n個(gè)容許最大加速度aimax。這里，i1，2，n。那么，對(duì)

32、于每個(gè)軸，均可計(jì)算出 2022/9/1955Tianjin University of Technology 為了保證所有各軸的速度和加速度都不超過容許值，容許協(xié)調(diào)運(yùn)動(dòng)的最短時(shí)間區(qū)間應(yīng)為 進(jìn)一步，引入歸一化時(shí)間=（tt0）/(tft0)，01。代入方程(6-30)，可得 2022/9/1956Tianjin University of Technology例6-5 考慮xy平面上的直線運(yùn)動(dòng)路徑，源點(diǎn)坐標(biāo)為(6，-3)，終點(diǎn)坐標(biāo)為(2，-1)假設(shè)兩軸只有加速度限制：ax=ay=2。試設(shè)計(jì)三次多項(xiàng)式樣條函數(shù)點(diǎn)位控制指令。 解 由于只有相同的加速度限制，兩軸運(yùn)動(dòng)的距離分別為x|62|4，y|3一(一

33、1)|2，且xy，所以協(xié)調(diào)運(yùn)動(dòng)是先設(shè)計(jì)x軸，然后利用下列直線方程變換出y軸的控制指令，即 2022/9/1957Tianjin University of Technology642連續(xù)路徑控制 令符號(hào) ，i=0，1，N，是空間軌跡上給定參考點(diǎn)的位置和速度向量。對(duì)于第i段ti1tti，引入相對(duì)時(shí)間變量，則并且，將通過i一1和i點(diǎn)的三次多項(xiàng)式樣條函數(shù)表示為2022/9/1958Tianjin University of Technology則邊界條件為2022/9/1959Tianjin University of Technology例6-6 假設(shè)給定xy平面上的軌跡如圖6-12所示。該軌跡共

34、分為4段，每段的邊界條件列于表6-2中。試采用三次多項(xiàng)式樣條函數(shù)生成控制指令。 圖6-12 xy平面上的軌跡 2022/9/1960Tianjin University of Technology表6-2 例6-6邊界條件 解 第一段 0t5s2022/9/1961Tianjin University of Technology第二段 5st15s 第三段 15st30s 第四段 30st40s 2022/9/1962Tianjin University of Technology643 軌跡控制的實(shí)現(xiàn) 根據(jù)三次多項(xiàng)式樣條函數(shù)運(yùn)動(dòng)指令，實(shí)現(xiàn)軌跡控制的系統(tǒng)方案如圖6-l3所示。 圖6-13 實(shí)現(xiàn)

35、軌跡控制的系統(tǒng)方案 2022/9/1963Tianjin University of Technology為了控制沿軌跡的運(yùn)動(dòng)速度，可以通過改變點(diǎn)的間隔(tk+1tk)來實(shí)現(xiàn) 。為了進(jìn)一步縮短插補(bǔ)運(yùn)算時(shí)間，可以將三次多項(xiàng)式函數(shù)擬合改為簡(jiǎn)單的直線插補(bǔ)法。 為了縮小擬合誤差，直線插補(bǔ)的間隔應(yīng)該選得更小。 2022/9/1964Tianjin University of Technology65 典型系統(tǒng)舉例 作為軌跡規(guī)劃和控制的實(shí)例，在這一節(jié)中，集中討論一種試驗(yàn)用的自主式自動(dòng)導(dǎo)引車。自主式自動(dòng)導(dǎo)引車實(shí)質(zhì)上是一種輪式移動(dòng)機(jī)器人，通過編程可以任意改變運(yùn)動(dòng)軌跡。 自主導(dǎo)引方式是一種不依賴外界環(huán)境的、最具

36、有通用性和環(huán)境適應(yīng)性的一種導(dǎo)航方式。其中，輪距法是一種非常簡(jiǎn)單而價(jià)廉的方法。它利用小車左右側(cè)安裝的位置檢測(cè)裝置，如光電編碼器，檢測(cè)左右輪轉(zhuǎn)過的角度，確定小車的位置和方向。 2022/9/1965Tianjin University of Technology651 總體結(jié)構(gòu)方案 試驗(yàn)小車分機(jī)械本體和電控系統(tǒng)兩部分。機(jī)械本體結(jié)構(gòu)如圖6-14所示。車身尺寸660mm(長(zhǎng))540mm(寬)430mm(高)。驅(qū)動(dòng)輪(前輪)和小腳輪(后輪)皆為硬橡膠制品。驅(qū)動(dòng)輪半徑r638mm，齒輪減速比為3.6。驅(qū)動(dòng)輪采用閉環(huán)直流伺服系統(tǒng)控制。反饋元件光電編碼器又是輪距法中的輪距測(cè)量裝置。左右輪伺服系統(tǒng)的原理結(jié)構(gòu)如圖

37、6-15所示。2022/9/1966Tianjin University of Technology 圖6-14 自主式自動(dòng)導(dǎo)引車原理結(jié)構(gòu) 2022/9/1967Tianjin University of Technology圖6-15 左右輪伺服系統(tǒng)的原理結(jié)構(gòu) 伺服系統(tǒng)的控制器采用8098單片機(jī)。上位機(jī)采用8088CPU進(jìn)行軌跡規(guī)劃和運(yùn)動(dòng)學(xué)逆運(yùn)算。上下位機(jī)之間通信由雙口RAM完成。車載控制器的計(jì)算機(jī)結(jié)構(gòu)如圖6-16所示。車載控制器的主要功能是：根據(jù)行車命令或環(huán)境模型，規(guī)劃其自身的運(yùn)動(dòng)路徑，控制兩個(gè)獨(dú)立驅(qū)動(dòng)輪的轉(zhuǎn)角和轉(zhuǎn)速，使小車跟蹤規(guī)劃路徑運(yùn)動(dòng)。 2022/9/1968Tianjin Univ

38、ersity of Technology圖6-16 車載控制器的計(jì)算機(jī)結(jié)構(gòu) 2022/9/1969Tianjin University of Technology652 運(yùn)動(dòng)學(xué)方程 小車的運(yùn)動(dòng)學(xué)正問題 這就是小車的定位問題，即由左右輪運(yùn)動(dòng)速度vL(t)和vR(t)確定小車的空間位置坐標(biāo)(x(t)，y(t)和方向(t)。 參考圖6-17所示小車結(jié)構(gòu)參數(shù)和幾何位置，oxy表示地面固定坐標(biāo)系，CR是兩個(gè)驅(qū)動(dòng)輪的中點(diǎn)。GP是小車中心線上的一點(diǎn)，定義為小車的參考點(diǎn)或?qū)бc(diǎn)。該點(diǎn)的運(yùn)動(dòng)軌跡就是小車的運(yùn)動(dòng)軌跡。是描述車體方位的參數(shù)。一般選擇GP與CR重合，即D0。GP與CR重合可使運(yùn)動(dòng)學(xué)方程簡(jiǎn)化。 2022

39、/9/1970Tianjin University of Technology圖6-17 小車結(jié)構(gòu)參數(shù)和幾何位置 小車在oxy直角坐標(biāo)系中的位置和方向可以表示如下： 2022/9/1971Tianjin University of Technology 小車的運(yùn)動(dòng)學(xué)逆問題 這就是小車的軌跡跟蹤問題，即給定小車的運(yùn)動(dòng)軌跡，求解左右輪伺服系統(tǒng)的控制指令。 當(dāng)小車在地面上作無滑動(dòng)運(yùn)動(dòng)時(shí)，小車左右輪的中心點(diǎn)CR既是小車的軌跡曲率中心又是小車的速度瞬心。 2022/9/1972Tianjin University of Technology653 軌跡規(guī)劃與控制指令的生成 根據(jù)上述理論，為了使小車平穩(wěn)移

40、動(dòng)和軌跡規(guī)劃方便，采用小車沿直線過渡曲線另一直線移動(dòng)的軌跡，并保證整個(gè)軌跡曲率半徑連續(xù)。采用三次曲線作為過渡曲線，既可以實(shí)現(xiàn)曲率半徑連續(xù)的軌跡規(guī)劃，又可以由軌跡輪廓和最小曲率半徑唯一確定。 設(shè)小車先由直線1前進(jìn)，然后經(jīng)過過渡曲線b1b2，再沿直線2移動(dòng)，如圖6-18所示。 2022/9/1973Tianjin University of Technology圖6-18 導(dǎo)引車運(yùn)動(dòng)軌跡圖 每一段弧長(zhǎng)的曲率半徑Ri和對(duì)應(yīng)的圓心角i： 2022/9/1974Tianjin University of Technology 在每一小段弧上，導(dǎo)引車左右驅(qū)動(dòng)輪的速度分別為: 在每一個(gè)t內(nèi)，左右驅(qū)動(dòng)輪伺服系統(tǒng)的控制指令應(yīng)為: 式中，符號(hào)Kb為光電編碼器的脈沖當(dāng)量，mm脈沖。 這樣，(SLi，SRi)，i0，1，2，就形成了導(dǎo)引車控制指令序列。2022/9/1975Tianjin University of Technology654上下位機(jī)軟件編程 上位機(jī)8088編程 上位機(jī)擴(kuò)大了8路優(yōu)先中斷板，可將外部的8路中斷進(jìn)行優(yōu)先權(quán)排隊(duì)后，從STDBUS上向8088申請(qǐng)中斷。當(dāng)采用矢量中斷方式時(shí)，8路優(yōu)先中斷板還向8088提供中斷類型號(hào)。 上位機(jī)采用Turbo C20編程，在STDDOS操作系統(tǒng)中運(yùn)行。上位機(jī)軟件共計(jì)由initiali