韓林峰 ,崔琦
(1.鄭州市第一中學(xué),河南鄭州,450006;2.洛陽(yáng)理工學(xué)院,河南洛陽(yáng),471023)
隨著機(jī)械、電子、控制、信息等技術(shù)的迅猛發(fā)展,智能機(jī)器人將在軍事、科研和工業(yè)領(lǐng)域中有著廣闊的應(yīng)用前景[1—4]。智能循跡小車(chē)是一種可自動(dòng)根據(jù)規(guī)劃路徑運(yùn)行的輪式機(jī)器人,也是一個(gè)集環(huán)境感知、分析判斷與決策、自動(dòng)行駛等功能于一體的綜合系統(tǒng)[5]。例如在國(guó)內(nèi)外的一些大型物流企業(yè),已經(jīng)開(kāi)始利用智能小車(chē)對(duì)貨物進(jìn)行掃碼識(shí)別,然后按照規(guī)劃路徑自動(dòng)運(yùn)送到相應(yīng)的出貨區(qū)域,實(shí)現(xiàn)了貨物的快速自動(dòng)分揀[6],節(jié)省了人工及勞動(dòng)強(qiáng)度,并提高了工作效率;在礦山行業(yè),也可以運(yùn)用智能小車(chē)進(jìn)行煤炭等貨物的運(yùn)輸。現(xiàn)在,許多學(xué)校和機(jī)構(gòu)通過(guò)舉辦循跡小車(chē)的設(shè)計(jì)制作活動(dòng)和學(xué)科競(jìng)賽,來(lái)培養(yǎng)學(xué)生的實(shí)踐、創(chuàng)新能力和團(tuán)隊(duì)協(xié)作精神等[7—13]。
循跡小車(chē)所采用的位置和速度控制模型及策略直接影響著小車(chē)循跡運(yùn)動(dòng)的精度、速度和流暢性。本文根據(jù)所設(shè)計(jì)循跡小車(chē)的工作原理進(jìn)行機(jī)械運(yùn)動(dòng)分析,建立位置+速度的串級(jí)PID控制模型,實(shí)現(xiàn)了循跡小車(chē)快速、精確的循跡控制。
設(shè)計(jì)的循跡小車(chē)的組成和工作原理圖如圖1所示。小車(chē)主要由機(jī)械機(jī)構(gòu)和控制系統(tǒng)組成。機(jī)械機(jī)構(gòu)主要包括車(chē)身、萬(wàn)向輪、左輪、右輪和減速器等組成??刂葡到y(tǒng)主要由單片機(jī)控制板、電機(jī)驅(qū)動(dòng)板、電池、紅外光電傳感器、測(cè)速器和控制軟件等組成。
萬(wàn)向輪安裝在車(chē)身前部,左輪和右輪安裝在車(chē)身后部,分別由左電機(jī)和右電機(jī)通過(guò)相應(yīng)的減速器減速后單獨(dú)驅(qū)動(dòng);單片機(jī)控制板采用模塊化設(shè)計(jì),CPU采用STC89C52芯片;左電機(jī)和右電機(jī)為直流電機(jī),由采用L293D芯片的電機(jī)驅(qū)動(dòng)板來(lái)驅(qū)動(dòng);電源模塊用2節(jié)3.7V鋰電池串聯(lián)給小車(chē)控制系統(tǒng)供電。對(duì)于每個(gè)直流電機(jī),單片機(jī)輸出一路PWM信號(hào)來(lái)控制電機(jī)的轉(zhuǎn)速,輸出一路TTL信號(hào)來(lái)控制電機(jī)的轉(zhuǎn)向;兩個(gè)槽型對(duì)射光電傳感器與安裝在左、右電機(jī)減速器上的測(cè)速碼盤(pán)組成了測(cè)速系統(tǒng),將左、右輪的轉(zhuǎn)速轉(zhuǎn)換為脈沖實(shí)時(shí)傳送到單片機(jī)控制系統(tǒng),可實(shí)現(xiàn)左、右輪轉(zhuǎn)速的閉環(huán)控制。5個(gè)紅外光電傳感器安裝在小車(chē)的前部,用來(lái)檢測(cè)黑色軌道與小車(chē)前進(jìn)方向之間的偏差。
圖1 循跡小車(chē)的組成和工作原理圖
當(dāng)小車(chē)正常前進(jìn)時(shí),紅外光電傳感器A檢測(cè)到黑色軌道;當(dāng)紅外光電傳感器B或C或D或E檢測(cè)到黑色軌道時(shí),說(shuō)明小車(chē)檢測(cè)到位置誤差,軌道出現(xiàn)了偏轉(zhuǎn);控制軟件根據(jù)5個(gè)紅外光電傳感器輸入狀態(tài)的變化來(lái)判斷位置誤差的大小和方向,并確定下一步調(diào)整時(shí)的轉(zhuǎn)向和轉(zhuǎn)角輸出,再計(jì)算出左、右電機(jī)的輸出轉(zhuǎn)速,然后再通過(guò)電機(jī)驅(qū)動(dòng)板、左電機(jī)、右電機(jī)、減速器來(lái)改變左輪、右輪的速度,從而改變小車(chē)的前進(jìn)方向,從而實(shí)現(xiàn)小車(chē)的自動(dòng)循跡運(yùn)動(dòng)。
根據(jù)小車(chē)的機(jī)械結(jié)構(gòu),將小車(chē)簡(jiǎn)化為圖2中粗黑線所示的模型。萬(wàn)向輪的中心點(diǎn)為G,左輪的中心點(diǎn)為H,右輪的中心點(diǎn)為J,H點(diǎn)和J點(diǎn)的中心點(diǎn)為I。設(shè)小車(chē)左輪的速度為v左,右輪的速度為v右,I點(diǎn)的速度為v中。由于小車(chē)前端為萬(wàn)向輪,所以小車(chē)的轉(zhuǎn)向完全由左輪和右輪的速度決定,小車(chē)轉(zhuǎn)向時(shí)的回轉(zhuǎn)中心點(diǎn)在直線HJ上。經(jīng)過(guò)Δt時(shí)間后,小車(chē)向右轉(zhuǎn)動(dòng)θ角,其運(yùn)動(dòng)軌跡如圖2所示,點(diǎn)G,H,I和J轉(zhuǎn)動(dòng)θ角后的位置分別為點(diǎn)G’,H’,I’和J’,O點(diǎn)為小車(chē)的回轉(zhuǎn)中心。若循跡小車(chē)是以萬(wàn)向輪的中點(diǎn)G沿軌道循跡運(yùn)動(dòng)為目標(biāo)的話,則小車(chē)轉(zhuǎn)彎時(shí)的回轉(zhuǎn)半徑為OG。
由公式
可得小車(chē)轉(zhuǎn)彎時(shí)各點(diǎn)的速度為:
式中:R左為OH的長(zhǎng)度,R右為OJ的長(zhǎng)度,R中為OI的長(zhǎng)度,RG為OG的長(zhǎng)度。
設(shè)HJ的長(zhǎng)度為k,則
所以:
將公式5帶入公式3可得:
由以上分析可以看出,當(dāng)k為常數(shù)時(shí),循跡小車(chē)的轉(zhuǎn)彎軌跡(轉(zhuǎn)動(dòng)方向和回轉(zhuǎn)中心點(diǎn)O的位置)是由左、右輪的速度v左和v右來(lái)確定的。
當(dāng)v左>v右時(shí),θ角為正值,小車(chē)右轉(zhuǎn);當(dāng)v左 當(dāng)O點(diǎn)與J點(diǎn)重合時(shí),v右=0,小車(chē)在繞J點(diǎn)轉(zhuǎn)彎。當(dāng)O點(diǎn)與I點(diǎn)重合時(shí),v左=—v右,v中=0,小車(chē)在繞I點(diǎn)轉(zhuǎn)彎;此時(shí),小車(chē)的轉(zhuǎn)彎半徑最小,即小車(chē)所能通過(guò)的理想最小轉(zhuǎn)彎半徑為h(h為IG的長(zhǎng)度)。所以,當(dāng)h越小時(shí),小車(chē)所能通過(guò)的最小轉(zhuǎn)彎半徑越小,其極限狀態(tài)是h=0,小車(chē)可繞I點(diǎn)原地打轉(zhuǎn)。 循跡小車(chē)沿軌跡前進(jìn)時(shí),事先是不知道軌跡的形狀和轉(zhuǎn)彎半徑的,無(wú)法提前做出規(guī)劃,只能不斷地根據(jù)紅外光電傳感器的檢測(cè)結(jié)果實(shí)時(shí)調(diào)整小車(chē)的轉(zhuǎn)向和轉(zhuǎn)角。所以,這是一種隨著軌跡變化而不斷調(diào)整小車(chē)輸出的隨動(dòng)控制系統(tǒng)。 圖3是一種循跡小車(chē)的簡(jiǎn)單控制模型??刂葡到y(tǒng)根據(jù)5個(gè)紅外光電傳感器的檢測(cè)結(jié)果,判斷小車(chē)下一步有以下幾種輸出:直行、小角度左轉(zhuǎn)、大角度左轉(zhuǎn)、小角度右轉(zhuǎn)、大角度右轉(zhuǎn)。然后分別針對(duì)這幾種情況,根據(jù)需要轉(zhuǎn)動(dòng)的方向和轉(zhuǎn)動(dòng)的角度進(jìn)行速度分配,確定左輪速度v左和右輪速度v右,然后以PWM方式控制左輪和右輪的速度,實(shí)現(xiàn)小車(chē)的轉(zhuǎn)向和直線運(yùn)動(dòng)。由于其控制簡(jiǎn)單,編程方便,比較適合一些對(duì)速度要求不高的場(chǎng)合[6—8]。圖 圖3 循跡小車(chē)的簡(jiǎn)單控制模型圖 采用簡(jiǎn)單控制模型后,循跡小車(chē)過(guò)彎道時(shí)的運(yùn)動(dòng)軌跡如4所示。 小車(chē)開(kāi)始時(shí)在直道上行駛,然后進(jìn)入彎道;在控制程序的第一個(gè)掃描周期后小車(chē)的萬(wàn)向輪到達(dá)G1點(diǎn),紅外光電傳感器C檢測(cè)到了黑線,小車(chē)進(jìn)行小角度右轉(zhuǎn)輸出,到第二個(gè)掃描周期時(shí),小車(chē)運(yùn)行到G2點(diǎn)。由圖2可知,小車(chē)在轉(zhuǎn)向時(shí)的運(yùn)動(dòng)軌跡是圓弧,所以G1和G2之間為圓弧。由于小車(chē)發(fā)生了轉(zhuǎn)向,所以在第二個(gè)掃描周期后,只有紅外光電傳感器A檢測(cè)到了黑線,小車(chē)進(jìn)行直行輸出,到第三個(gè)掃描周期時(shí),小車(chē)運(yùn)行到G3點(diǎn),G2和G3之間為直線;此時(shí)紅外光電傳感器C檢測(cè)到了黑線,小車(chē)再次進(jìn)行小角度右轉(zhuǎn)輸出。依次類推,小車(chē)在通過(guò)彎道時(shí)不斷地進(jìn)行轉(zhuǎn)彎和直行調(diào)整,其運(yùn)動(dòng)軌跡是一些圓弧和直線。由此看出,采用這種控制模型后,小車(chē)每個(gè)控制循環(huán)輸出的轉(zhuǎn)角是有級(jí)的且不連續(xù)的,循跡小車(chē)在轉(zhuǎn)彎時(shí)常出現(xiàn)擺動(dòng)等不連貫運(yùn)動(dòng),影響小車(chē)的運(yùn)動(dòng)速度。 圖4 循跡小車(chē)過(guò)彎道時(shí)的運(yùn)動(dòng)軌跡 圖5 位置+速度的串級(jí)PID控制模型 為了改善小車(chē)運(yùn)動(dòng)的流暢性,一些論文將小車(chē)前端的萬(wàn)向輪改為由舵機(jī)控制的導(dǎo)向輪,然后分別對(duì)舵機(jī)轉(zhuǎn)向的角度和小車(chē)的左、右輪的速度采用了PID閉環(huán)控制[14,15]。由于小車(chē)的轉(zhuǎn)向還與左、右輪的速度有關(guān),這種控制方式要求舵機(jī)的轉(zhuǎn)向角度與左、右輪的速度要實(shí)時(shí)匹配,否則就會(huì)出現(xiàn)輪子打滑現(xiàn)象,所以控制難度較大。 經(jīng)過(guò)以上分析可知,循跡小車(chē)是通過(guò)左、右輪的速度控制來(lái)實(shí)現(xiàn)小車(chē)沿黑色軌道循跡的位置控制,設(shè)計(jì)的位置+速度的串級(jí)PID控制模型如圖5所示。其內(nèi)部為左、右輪速度控制的兩個(gè)速度環(huán),外部為位置偏差控制的位置環(huán)。 小車(chē)前部的紅外光電傳感器檢測(cè)到小車(chē)與軌道的位置偏差可用檢測(cè)到黑色軌跡偏差的傳感器點(diǎn)和G點(diǎn)的直線與直線AG之間的夾角α表示,直線AG左邊的夾角為負(fù),右邊的為正。則5個(gè)紅外光電傳感器可檢測(cè)到的位置偏差分別為 0o,—45o,—90o,45o和 90o。在每次控制循環(huán)中,位置偏差的設(shè)定值0和實(shí)際值α輸入到轉(zhuǎn)角PID控制器,通過(guò)PID運(yùn)算產(chǎn)生小車(chē)的輸出轉(zhuǎn)角θ,θ角的正負(fù)決定了小車(chē)的轉(zhuǎn)向。由轉(zhuǎn)角θ再進(jìn)行速度分配,確定左輪速度v左和右輪速度v右。每個(gè)車(chē)輪速度的設(shè)定值與實(shí)際檢測(cè)值輸入到相應(yīng)的速度PID控制器,產(chǎn)生相應(yīng)車(chē)輪的速度輸出,再通過(guò)電機(jī)驅(qū)動(dòng)板以PWM方式控制電機(jī)的轉(zhuǎn)速;左輪和右輪的轉(zhuǎn)速差可實(shí)現(xiàn)小車(chē)的轉(zhuǎn)向和直線運(yùn)動(dòng),從而實(shí)現(xiàn)小車(chē)的循跡運(yùn)動(dòng)控制。 在本控制模型中,轉(zhuǎn)角和轉(zhuǎn)速都實(shí)現(xiàn)了PID控制,從而使轉(zhuǎn)角和轉(zhuǎn)速的輸出連續(xù)變化;同時(shí),采用兩個(gè)PID控制器串聯(lián)工作,即外環(huán)(位置環(huán))轉(zhuǎn)角PID控制器的輸出經(jīng)運(yùn)算后作為兩個(gè)內(nèi)環(huán)(速度環(huán))速度PID控制器的設(shè)定值,由內(nèi)環(huán)PID控制器的輸出去操縱小車(chē)的轉(zhuǎn)向和轉(zhuǎn)角,從而改善了控制質(zhì)量,對(duì)外環(huán)被控量具有更好的控制效果。 在公式5中,HJ的長(zhǎng)度k為常數(shù),每個(gè)控制循環(huán)的時(shí)間Δt也可確定,轉(zhuǎn)角PID控制器輸出了轉(zhuǎn)角θ,那么如何確定v左和v右這兩個(gè)變量呢? 為了盡快使小車(chē)轉(zhuǎn)向回到軌道上,可選用轉(zhuǎn)向最快策略。當(dāng)為θ正時(shí),小車(chē)需要右轉(zhuǎn),可令v右=0,則v左=θk/Δt;當(dāng)為θ負(fù)時(shí),小車(chē)需要左轉(zhuǎn),可令v左=0,則v右=—θk/Δt。這種方法雖然轉(zhuǎn)向較快,循跡效果很好,但是影響了小車(chē)轉(zhuǎn)彎的速度,勢(shì)必會(huì)影響循跡小車(chē)的比賽成績(jī)。 為了保證小車(chē)的速度,可選用速度最快策略。每個(gè)小車(chē)在直行時(shí)都有一個(gè)最大速度vm,那么在轉(zhuǎn)彎的時(shí)候可將外側(cè)輪子的速度選擇為最大速度vm。當(dāng)為θ正時(shí),小車(chē)需要右轉(zhuǎn),可令 v左=vm,則 v右=vm—θk/Δt;當(dāng)為 θ 負(fù)時(shí),小車(chē)需要左轉(zhuǎn),可令v右=vm,則v左=vm+θk/Δt。這種方法可以確保小車(chē)在轉(zhuǎn)彎時(shí)的速度較快。如果在調(diào)試時(shí)小車(chē)由于速度過(guò)快而滑出彎道,就要適當(dāng)減小外側(cè)輪子的最大速度。 將小車(chē)的硬件安裝好,然后按照串級(jí)PID控制模型編寫(xiě)小車(chē)的控制程序,然后根據(jù)小車(chē)在軌道上的循跡運(yùn)動(dòng)情況,不斷調(diào)整小車(chē)的最大速度vm和PID控制器的參數(shù)等,使小車(chē)能夠連續(xù)地、以較快的速度轉(zhuǎn)彎并跑完賽道。試驗(yàn)結(jié)果表明,采用串級(jí)PID控制模型的小車(chē)在遇到彎道能夠快速轉(zhuǎn)向,運(yùn)動(dòng)流暢連貫,比較準(zhǔn)確地完成了循跡運(yùn)動(dòng)。 進(jìn)一步改進(jìn)小車(chē)循跡性能的措施:若小車(chē)的前部安裝更多的紅外光電傳感器或者采用其它模擬量位置傳感器將使檢測(cè)到的軌道位置偏差更加精確,也會(huì)改善轉(zhuǎn)角PID控制器輸出的波動(dòng),使小車(chē)的轉(zhuǎn)向更加平穩(wěn)。若小車(chē)的單片機(jī)控制板采用性能更高的CPU(例如Arduino MEGA2560或32位ARM芯片),提高其運(yùn)算速度,就能減小中斷掃描周期Δt,即減小了PID控制器的掃描周期,提高了小車(chē)的響應(yīng)速度,使小車(chē)的循跡運(yùn)動(dòng)更加平穩(wěn)可靠。 本論文采用5個(gè)紅外光電傳感器檢測(cè)軌道的偏差,前部安裝萬(wàn)向輪,通過(guò)后部左輪和右輪的速度差來(lái)控制小車(chē)的轉(zhuǎn)向,實(shí)現(xiàn)小車(chē)的循跡運(yùn)動(dòng);分析了小車(chē)轉(zhuǎn)彎時(shí)的運(yùn)動(dòng)軌跡,討論了左輪和右輪速度與轉(zhuǎn)彎半徑等參數(shù)之間的關(guān)系;設(shè)計(jì)了位置+速度的串級(jí)PID控制模型,采用速度最快策略進(jìn)行速度分配,確保小車(chē)在轉(zhuǎn)向時(shí)快速流暢,使小車(chē)能夠快速、平穩(wěn)、準(zhǔn)確地沿賽道軌跡運(yùn)動(dòng)。該小車(chē)的設(shè)計(jì)方案和串級(jí)控制模型也可應(yīng)用于無(wú)人車(chē)間自動(dòng)搬運(yùn)的AGV小車(chē)、自主移動(dòng)機(jī)器人等服務(wù)機(jī)器人的循跡及定位控制。3 控制模型
3.1 簡(jiǎn)單控制模型
3.2 串級(jí)PID控制模型
3.3 速度分配策略
4 調(diào)試
5 結(jié)論