機電控制原理及應用

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

34、分為4段，每段的邊界條件列于表6-2中。試采用三次多項式樣條函數(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 軌跡控制的實現(xiàn) 根據(jù)三次多項式樣條函數(shù)運動指令，實現(xiàn)軌跡控制的系統(tǒng)方案如圖6-l3所示。 圖6-13 實現(xiàn)

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

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

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

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

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

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