AGV運動學(xué)模型

1、第二章自動導(dǎo)引小車運動學(xué)模型2.1自動導(dǎo)引小車的結(jié)構(gòu)特點自動導(dǎo)引小車的研究涉及多種技術(shù)領(lǐng)域，是一個典型的機電一體化多技術(shù)多學(xué)科的集成系統(tǒng)，其機構(gòu)示意圖如圖 2-12-1自動導(dǎo)引小車的機械機構(gòu)部分主要包括如下幾個方面：(1)(1)車體車體由車架、相應(yīng)的機械電氣機構(gòu)、外觀件等部分組成，它是自動導(dǎo)引小車的基礎(chǔ)部分。車架的設(shè)計需要考慮剛性強度、整車的平穩(wěn)性等重要的機械性能，重心的位置是又一關(guān)鍵因素，重心越低越有利于抗傾翻。在滿足車載機械電氣機構(gòu)、外觀件以及其它裝置裝配空間，和整車剛性要求的條件下，盡量考慮整車的外形造型美觀和輕便小巧。(2)(2)驅(qū)動裝置驅(qū)動裝置是自動導(dǎo)引小車行走的執(zhí)行機構(gòu)，它主要由車

2、輪、減速器、制動器、電機及速度控制器等部分組成。通常情況下，驅(qū)動裝置和轉(zhuǎn)向裝置集成在一起使用。(3)(3)蓄電池和充電裝置蓄電池和充電裝置是自動導(dǎo)引小車的動力源。自動導(dǎo)引小車一般采用24V24V 或 48V48V 直流工業(yè)蓄電池電能為動力， 對于傳統(tǒng)的鉛酸蓄電池， 一般需保證 8 8 小時以上的安培小時值。隨著電池科技的發(fā)展，快速充電蓄電池問世，這類蓄電池的安培小時值根據(jù)實際生產(chǎn)需求而定，而且與之配套的采用一種先進智能快速充電技術(shù)，充五分鐘電可以使用一個多小時。從而提高自動導(dǎo)引小車的有效使用率。充電裝置有多種，目前最常用有地靴式和測掛式等。一般地，充電裝置需要安全保護。(4)(4)位姿傳感裝置

3、位姿傳感裝置主要是為了從自動導(dǎo)引小車的當(dāng)前環(huán)境中，獲得小車的位姿(位置與轉(zhuǎn)角)和其它相關(guān)的信息，如運行前方有無障礙等。位姿傳感裝置會因為采用的導(dǎo)引技術(shù)的不同而不同，如采用電磁感應(yīng)技術(shù)的位姿傳感裝置主要是安裝在小車上的一對探頭(即感應(yīng)線圈)和比較/放大電路等，而采用光學(xué)檢測技術(shù)的位姿傳感裝置則主要是光學(xué)檢測器和輔助裝置等。(5)(5)導(dǎo)向控制裝置導(dǎo)向控制裝置是整個導(dǎo)引小車運動控制的核心，主要色含有硬件部分和軟件部分。一般來說，盡管采用的導(dǎo)引技術(shù)千差萬別，但是，導(dǎo)向控制裝置的結(jié)構(gòu)大體相同。硬件部分主要是數(shù)字電路部分，主要是位置環(huán)、垢差控制器等，多采用單片機實現(xiàn)，從而可以通過程序方便的控制自動導(dǎo)引小

4、車跳加速、減速和勻速運動，需要的話也可以切換偏差控制器實現(xiàn)直道、彎道的多棒型控制同時由于自動導(dǎo)引小車行走過程中對實時性要求較高，對包括速度環(huán)、電流環(huán)及驅(qū)動部分的控制器及脈沖寬度調(diào)制(PulseWidthModulation)(PulseWidthModulation)信號發(fā)生器而言， 采用模擬電路控制具有實時快速、不占用 CPUCPU 寸間的優(yōu)點，特別適合電流，速度環(huán)快速 PIDPID 運算。(6)(6)專用功能執(zhí)行機構(gòu)自動導(dǎo)引小車的形式種類很多，每種實用型自動導(dǎo)引小車，它都不光是裝備了自動導(dǎo)引行走功能，還裝備了專用功能執(zhí)行機構(gòu)，以滿足自動導(dǎo)引小車的實際功能需求，如叉車式自動導(dǎo)引小車的自動叉體

5、機構(gòu)，轉(zhuǎn)道式自動導(dǎo)引小車的自動輾道機構(gòu)等。專用功能執(zhí)行機構(gòu)是根據(jù)各種自動導(dǎo)引小車需要完成的功能專門設(shè)計而成，具有特定專用性。(7)(7)安全保護裝置在實際生產(chǎn)中，人與機械處于同一環(huán)境，自動導(dǎo)引小車作為一種運動中的動作功能機械，具安全保障功能尤為重要。安全裝置的作用包括防止設(shè)備在運行中出錯，也預(yù)防運行出錯對人員及其運行環(huán)境設(shè)施產(chǎn)生的影響，直接地，安全裝置的功能就是保護自動導(dǎo)引小車自身，以及維護自動導(dǎo)引小車攻用的順利完成，同時在最大可能的范圍內(nèi)保護人員和運行環(huán)境設(shè)施的安全。自戴導(dǎo)引小車的安全裝置主要是障礙檢測傳感裝置以及避障控制裝置。本論文研究對象是車體采用三輪式結(jié)構(gòu)的自動導(dǎo)引小車，車體結(jié)構(gòu)示意圖

6、如圖 2-22-2 所示。兩個后輪作為驅(qū)動輪，利用兩輪的速度差實現(xiàn)轉(zhuǎn)向；前輪為隨動輪，僅起到支撐車體的作用,而沒有導(dǎo)向的作用。 采用這種結(jié)構(gòu)的自動導(dǎo)引小車只有直線運動和圓弧運動兩種運動形式， 較適合于固定路徑導(dǎo)引控制和運動路徑不是很復(fù)雜的導(dǎo)引控制中。此車體正向運行和反向運行的動力學(xué)模型是不同的， 可以只按車體正向運行情形求取其動力學(xué)模型。如果要求所求取的動力學(xué)模型對于車體正向運行與反向運行均適合，那么該車體結(jié)構(gòu)必須前后對稱，即前輪為操舵輪，中間兩輪是驅(qū)動輪，后輪也為操舵輪的變四輪結(jié)構(gòu)(如圖 1-21-2) )o o 實際運行中，如果需要反向運行，只需要將自動導(dǎo)引小車原地旋轉(zhuǎn) 180180 度以

7、后，就相當(dāng)于正向運行了。2.2.自動導(dǎo)引小車運動學(xué)模型的建立2.2.1自動導(dǎo)引小車運動學(xué)分析運動學(xué)模型主要根據(jù)運動學(xué)方面的理論直接求得伺服電機端電壓與車速的關(guān)系，在這種模型中，自動導(dǎo)引小車的速度只與電機端電壓及電機電樞回路的時間常數(shù)有關(guān)，而忽略了車體質(zhì)量、摩擦阻力對車速的影響。建立如圖 2-32-3 所示的坐標(biāo)系。圖中 XOXOS S世界坐標(biāo)系，v,v, ,v v,分別為左/右驅(qū)動輪輪心相對于地面的線速度(m/s),L(m/s),L 為兩驅(qū)動輪之間的距離，R R 為自動導(dǎo)引小車?yán)@瞬心口的轉(zhuǎn)彎半徑，必為自動導(dǎo)引圖圖2-2車體鰭構(gòu)示意圖車體鰭構(gòu)示意圖圖 2-3 總 V 運動學(xué)粳地小車?yán)@瞬心口轉(zhuǎn)動的

8、角速度(rad/s(rad/s) )自動導(dǎo)引小車的運動學(xué)模型是基于以下幾點假設(shè)：自動導(dǎo)引小車是剛性的；自動導(dǎo)引小車運動在水平面上；自動導(dǎo)引小車的左、右輪受 Y Y 力相等，且車輪與地面之間沒有相對滑動；自動導(dǎo)引小車的車體質(zhì)量、摩擦阻力、負(fù)載等的變化對車速的影響忽略不計。取為距離偏差(m(m),),為角度偏差(rad(rad),),在此設(shè)定角度以逆時針為正，順時針為負(fù)。根據(jù)自動導(dǎo)引小車的車體結(jié)構(gòu)和剛體平動原理可知，自動導(dǎo)引小車在任意時刻都是作繞車體瞬心 0 0 的轉(zhuǎn)動。由圖 2-32-3 可知，在 t t 時刻自動導(dǎo)引小車?yán)@瞬心口的轉(zhuǎn)彎半徑 R R 為：自動導(dǎo)引小車在 t t 時刻繞瞬心口轉(zhuǎn)動的

9、角速度必為由式（2-42-4）可以看出，只要正確標(biāo)定小車的初始位置，控制小車左右兩驅(qū)動輪的速度，可以使兩輪的中心跟蹤任意給定的運動軌跡，也就是說通過分別控制小車兩驅(qū)動輪的速度可以使小車實現(xiàn)路徑跟蹤控制。則自動導(dǎo)引小車運動方程為:角度偏差量為距離偏差量為較小，由式（2-52-5）可以得到角度偏差的變化率為:式中：一跟蹤路徑曲率變化對角度偏差變化率的影響，當(dāng)跟蹤路徑為直線時，其值為零。同樣的，由式（2-62-6）可以得到距離偏差血血_ _vf+K=dl2式中：化率的影響，當(dāng)跟蹤路徑為直線時，其值為零。-蹤路徑曲率變化對距離偏差變的變化率為:Jsin%+Ad令到狀態(tài)變量由公式(2-7(2-7) ),

10、(2-8)(2-8)即可以得的自動導(dǎo)引小車的狀態(tài)方程為(2-9)(2-9)描述自動導(dǎo)引小車左右輪速差的關(guān)系。它可以看成是以小車角度偏差為狀態(tài)變量，以車輛左右輪速差為輸入變量的狀態(tài)方程，即可以通過控制自動導(dǎo)引小車左右輪的速度差來調(diào)節(jié)小車的角度偏差和距離偏差但由于左右輪速差不是系統(tǒng)的直接輸入量，系統(tǒng)的控制輸入量為工業(yè)控制機送出的 D/AD/A 值，即電壓值，所以為了將控制電壓作為輸入加到狀態(tài)方程中，需要將小車的運動學(xué)方程與反映驅(qū)動系統(tǒng)動態(tài)特性的傳遞函數(shù)合并，得到描述整個被控過程動態(tài)特性的系統(tǒng)狀態(tài)空間數(shù)學(xué)模型。2.2.2 驅(qū)動系統(tǒng)數(shù)學(xué)模型的建立驅(qū)動系統(tǒng)的數(shù)學(xué)模型描述著計算機D/A輸出值（即電壓值）和

11、驅(qū)動輪轉(zhuǎn)速之間的關(guān)系，設(shè)計自動導(dǎo)引控制器及進行仿真和試驗研究都需要建立能夠正確地反映車輛驅(qū)動系統(tǒng)特性的數(shù)學(xué)模型。自動導(dǎo)引小車的驅(qū)動系統(tǒng)如圖 2-42-4 所示。由于 AGWAGW 區(qū)動系統(tǒng)環(huán)節(jié)較多，例如，電機調(diào)速機構(gòu)包括 D/AD/A 轉(zhuǎn)換、電機控制器、減速器、控制電機等；而且各部件的有關(guān)性能參數(shù)難以確定，如機械傳動機構(gòu)的剛度、阻尼、縱向運動慣量等機械性能參數(shù)都不便于測定，因此采用解析法建立車輛調(diào)速機構(gòu)數(shù)學(xué)模型的困難很大。由于系統(tǒng)的輸入輸出信號一般總是可以測量的，而系統(tǒng)的動態(tài)特性必然出現(xiàn)于這些輸入輸出數(shù)據(jù)中，故可以利用輸入輸出數(shù)據(jù)所提供的信息來建立系統(tǒng)的數(shù)學(xué)模型，這種建模方法就是系統(tǒng)辨識。隨著

12、系統(tǒng)辨識理論與方法的發(fā)展，應(yīng)用系統(tǒng)辨識的方法，通過實驗研究來確定系統(tǒng)的數(shù)學(xué)模型，是一種能滿足上述要求的行之有效的途徑。為了對驅(qū)動系統(tǒng)動態(tài)特性進行初步分析并便于以后的系統(tǒng)辨識，需要首先通過對其進行特定的輸入信號（如階躍輸入）下的動態(tài)響應(yīng)過程試驗，從而可以根據(jù)試驗結(jié)果判斷出驅(qū)動系統(tǒng)模型的階次。另外，由于輸入輸出信號均為離散數(shù)字信號，因此宜采用差分方程描述該系統(tǒng)。用差分方程定量描述一個動態(tài)系統(tǒng)時，必須確定出方程中的有關(guān)參數(shù)。所以，該種系統(tǒng)辨識的實質(zhì)是一個參數(shù)估計問題，可視為一種灰箱式部分辨識問題，同時，辨識過程中由于輸入輸出數(shù)據(jù)受到噪聲的影響，一般應(yīng)看成是隨機變量，因此也屬于統(tǒng)計學(xué)范疇。在參數(shù)估計時

13、，力求使某一個被適當(dāng)定義的誤差標(biāo)準(zhǔn)趨于最小，以便使尋求的數(shù)學(xué)模型與試驗數(shù)據(jù)有最佳擬合。在各種參數(shù)估計技術(shù)中，最小二乘法是從試驗數(shù)據(jù)進行參數(shù)估計的主要手段，其獲得的估計在一定條件下具有最佳的統(tǒng)計特性，因此該方法被廣泛應(yīng)用于系統(tǒng)辨識研究中。在系統(tǒng)辨識中，輸入信號的類別和形式影響著所采用的辨識方法和辨識精度。用于辨識輸入信號的最低要求是具有持續(xù)激勵特性，即在整個觀測周期上，過程的所有模態(tài)必須被輸入信號持續(xù)激勵。這意味著輸入信號不能隨意選擇，否則不但辨識精度不能保證，甚至可能造成不可辨識。目前常用的信號主要是隨機序列（如白噪聲）和偽隨機序列。理論分析表明，選用白噪聲作為辨識輸入信號可以保證獲得較好的辨

14、識效果，但是白噪聲在工程上不易實現(xiàn)，因此工程中一般選用最長線性移位寄存器序列（簡稱 M M 序列）作為辨識輸入信號。M M 序列是二進制偽隨機碼序列（PRBSPRBS）的一種形式，它的自相關(guān)函數(shù)接近脈沖函數(shù)，具有近似白噪聲的性質(zhì)，可保證有較好的辨識精度。對驅(qū)動系統(tǒng)的模型辨識而言， M M 序列的物理意義是驅(qū)動電機電源控制器輸入電壓信號，它在計算機內(nèi)由 D/AD/A 產(chǎn)生。M M 序列有 3 3 個參數(shù)，即一個電平的持續(xù)時選M M 序列就是確定這幾個參數(shù)。試驗開始時，應(yīng)先給系統(tǒng)以預(yù)激勵（或預(yù)擾動）。由于 M M 序列 u u（k k）實質(zhì)上是周期信號，具施加于系統(tǒng)后，輸出端響應(yīng) y y（k k）

15、最終也必定是周期波形。不過在 t=0t=0TsTs 的過渡過程初始階段內(nèi)，由于非零初始條件的作用，系統(tǒng)的輸出在一段時間內(nèi)是非平穩(wěn)的，為了保證辨識精度，要避開這段非平穩(wěn)過程。由于已選定,當(dāng) M M 序列的第一個周期己度過，y y（k k）dF+ +不不+ + +可可MO=4-/-/】t t+ +3與拉階差分方程與拉階差分方程口力下口力下(*+(*+獨獨) )+ +0#(+內(nèi)內(nèi)一一)+“一十一十0網(wǎng)網(wǎng),(,(4)=隊11(上+旌-1)之間的系之間的系數(shù)轉(zhuǎn)換數(shù)轉(zhuǎn)換關(guān)系為關(guān)系為+it產(chǎn)產(chǎn)(k),.2=2)4=%/,B=DB式中式中(2-13)JT，屬】(2-14)H=1b1.h，&df,B=：7+ +

16、烹烹p也也已成為周期期信號后，即可認(rèn)為 y(k)y(k)已具備平穩(wěn)性，此時進行。(k)(k)和 y(k)y(k)的相關(guān)運算刁有意義。因此在辨識試驗中將第一個周期內(nèi)的 M M 序列作為預(yù)激勵，而將第 1 1 個周期以后采集的試驗數(shù)據(jù)留作系統(tǒng)辨識之用，這樣做的目的是利用比較充足的數(shù)據(jù)，以提高辨識精度。系統(tǒng)辨識中，取電壓信號u(k)u(k)序列作為輸入值，小車速度信號y(k)y(k)序列作為輸出值進行辨識。通過研究電壓值與驅(qū)動輪轉(zhuǎn)速的相互關(guān)系建立描述驅(qū)動系統(tǒng)動態(tài)特性的差分方程。驅(qū)動系統(tǒng)的數(shù)學(xué)模型選用 ARXARX 真型，模型為：/(e)=(&)=1)*(*)+(*)其中：其中：用用)=)=1+口商口

17、商/+/+利用最小二乘方法進行系統(tǒng)辨識，可得描述驅(qū)動系統(tǒng)輸入輸出關(guān)系的差分模型。而描述線性連續(xù)系統(tǒng)的數(shù)學(xué)模型是微分方程和傳遞函數(shù)，必須把差分方程轉(zhuǎn)換成微分方程或者傳遞函數(shù)的形式。本文采用差分方程與微分方程系數(shù)的轉(zhuǎn)換法實現(xiàn)f7lanf7lan 階微分方程的周(2-10),分別是 m+1m+1 階和 m m 階方陣，即轉(zhuǎn)換矩陣。是一個下三角矩陣，其對角線商的元素之絕對值及最后一行的元素的絕對值都是 1;1;矩陣中每個元素決定；矩陣中絕對值不等于 1 1 的其它非零元素的絕對值由下式計算：利用 MatlabMatlab 編程實現(xiàn)最小二乘方法系統(tǒng)辨識和模型轉(zhuǎn)換算法，得到驅(qū)動系統(tǒng)頻域形式的傳遞函數(shù)為：為

18、了將驅(qū)動系統(tǒng)的動態(tài)特性方程加入到系統(tǒng)狀態(tài)方程中， 需對驅(qū)動系統(tǒng)進行降階， 即把三階傳遞函數(shù)降為一階形式，從而近似簡化成慣性環(huán)節(jié)和比例環(huán)節(jié)，具體計算采用 MatlaMatlab b工具軟件編程實現(xiàn)，可得驅(qū)動系統(tǒng)傳遞函數(shù)的一階形式為模型為：雖然對模型降階處理使模型與實際系統(tǒng)有差距，但是這樣可以把驅(qū)動系統(tǒng)的微分方程加入到狀態(tài)方程中，對建模是有利的。式(2-14)(2-14)中，可以理解成左右輪的速度差，可以理解成計算機給左右驅(qū)動電機電源控制器的輸入電壓之差。2.2.3 自動導(dǎo)引小車控制系統(tǒng)狀態(tài)空間方程由于自動導(dǎo)引小車采用控制兩驅(qū)動輪的轉(zhuǎn)速差實現(xiàn)路徑跟蹤，即當(dāng)自動導(dǎo)引小車在跟蹤路徑有偏差時，采用分別將

19、兩個驅(qū)動輪加、減一個糾偏的速度,而且已經(jīng)把驅(qū)動系統(tǒng)的動態(tài)特性簡化成一階系統(tǒng)，因此可以把工業(yè)控制機的 D/AD/A 輸出作為控制器的輸入,作為模型中的狀態(tài)變量分別為左、右電機的控制電壓（定義右輪的電壓為正），），則自動導(dǎo)引小車的控制結(jié)構(gòu)圖如圖 2-52-5。以自動導(dǎo)引小車的左右輪轉(zhuǎn)速差角度偏差和距離偏差作為系統(tǒng)狀態(tài)變量，工業(yè)控制機的控制信號作為系統(tǒng)輸入，可以得到被控系統(tǒng)狀態(tài)空間方程為：rfir/d1r-a00Sb至此，己建立了系統(tǒng)被控過程的狀態(tài)空間數(shù)學(xué)模型，也可記作(A,B,C,D)(A,B,C,D)因此自動導(dǎo)引小車的運行軌跡與距離偏差e e和角度偏差.,這兩個參數(shù)有關(guān)，它們之間存在著非常復(fù)雜的

20、非線性關(guān)系。此外，考慮到自動導(dǎo)引小車的車身中線的實際偏轉(zhuǎn)角度要比車輪的旋轉(zhuǎn)角度要小。因此，系統(tǒng)在角度偏差較小，且離控制線距離較近時，旋轉(zhuǎn)靈敏度要高：而在角度偏差較小，且離控制線較遠時，旋轉(zhuǎn)靈敏度要低。2.3自動導(dǎo)引小車導(dǎo)引方程2.3.1AGV 自動導(dǎo)引技術(shù)概述顧名思義， 自動導(dǎo)引小車包含一個自動導(dǎo)引系統(tǒng)， AGVSAGVS 是依靠它沿一定的路線自動行駛的。不同類型的 AGVSAGVS 統(tǒng)中采用的自動導(dǎo)引技術(shù)各不相同，而采用哪一種導(dǎo)引技術(shù)將直接影響自動導(dǎo)引小車系統(tǒng)各方面的性能。根據(jù) AGVAGV(統(tǒng)中 AGV!AGV!行路線的性質(zhì)，導(dǎo)引系統(tǒng)可分為固定路徑導(dǎo)引、自由路徑導(dǎo)引、組合路徑導(dǎo)引三種：固定

21、路徑導(dǎo)引是指 AGV!AGV!行路線是以某種具體的形式規(guī)定的，如圖 2-6(a)2-6(a)。具體的路線可以是電磁感應(yīng)導(dǎo)引中的導(dǎo)引電纜、磁導(dǎo)引中的磁條和光學(xué)導(dǎo)引中的反光帶等。自由路徑導(dǎo)引是指 AGVAGV 勺運行路線是無任何具體形式的運行軌道，AGV&AGV&虛擬的路線運行，如圖 2-6(b)2-6(b)。這種虛擬的路線由控制系統(tǒng)間接通過一些指示裝置來確定，計算機視覺等導(dǎo)引方式均屬此類。組合路徑導(dǎo)引是指 AGVftAGVft 多數(shù)工作區(qū)間內(nèi)沿某種具體形式的固定路線運行，而在某些區(qū)域可沿控制系統(tǒng)指定的虛擬路線運行，如圖 2-6(c)2-6(c), ,兩種路徑分別用實線和虛線來表示。一般綜合使用上

22、述兩類導(dǎo)引方式中的不同導(dǎo)引技術(shù)。一般而言，固定路徑導(dǎo)引實施較容易，技術(shù)較為成熟，但運行路線的更改相對較為困難。自由路徑導(dǎo)引的成本較高，同時在實際應(yīng)用中還有一些具體問題需要解決，但 AGV!AGV!行路線的更改容易，柔性較高。組合路徑導(dǎo)引可綜合上兩類導(dǎo)引系統(tǒng)的優(yōu)點。2.3.2 AGV自動導(dǎo)引方程自動導(dǎo)引小車的運動路徑一般都是直線與圓弧組成。下面就分別討論自動導(dǎo)引小車在直線段與圓弧段的導(dǎo)引方程。2.3.2.1 直線路徑導(dǎo)引方程AGVAGV 在直線路徑上導(dǎo)引示意圖如圖 2-7(a)2-7(a)，其中，Y Y 軸與路徑重合分別表示左右輪轉(zhuǎn)速和車體轉(zhuǎn)速,為車體轉(zhuǎn)角,驅(qū)動輪半徑，B B 為傳感器中心距兩驅(qū)動輪中心連線的距離。則車體轉(zhuǎn)動角速度為：令車體初始角度為考坐標(biāo)系 Y Y 軸的轉(zhuǎn)動角度為L L 為驅(qū)動輪中心距