自動循跡小車的位置和速度控制

韓林峰 ，崔琦

（1.鄭州市第一中學，河南鄭州，450006；2.洛陽理工學院，河南洛陽，471023）

0 引言

隨著機械、電子、控制、信息等技術的迅猛發展，智能機器人將在軍事、科研和工業領域中有著廣闊的應用前景[1—4]。智能循跡小車是一種可自動根據規劃路徑運行的輪式機器人，也是一個集環境感知、分析判斷與決策、自動行駛等功能于一體的綜合系統[5]。例如在國內外的一些大型物流企業，已經開始利用智能小車對貨物進行掃碼識別，然后按照規劃路徑自動運送到相應的出貨區域，實現了貨物的快速自動分揀[6]，節省了人工及勞動強度，并提高了工作效率；在礦山行業，也可以運用智能小車進行煤炭等貨物的運輸。現在，許多學校和機構通過舉辦循跡小車的設計制作活動和學科競賽，來培養學生的實踐、創新能力和團隊協作精神等[7—13]。

循跡小車所采用的位置和速度控制模型及策略直接影響著小車循跡運動的精度、速度和流暢性。本文根據所設計循跡小車的工作原理進行機械運動分析，建立位置+速度的串級PID控制模型，實現了循跡小車快速、精確的循跡控制。

1 工作原理

1.1 組成

設計的循跡小車的組成和工作原理圖如圖1所示。小車主要由機械機構和控制系統組成。機械機構主要包括車身、萬向輪、左輪、右輪和減速器等組成。控制系統主要由單片機控制板、電機驅動板、電池、紅外光電傳感器、測速器和控制軟件等組成。

1.2 工作原理

萬向輪安裝在車身前部，左輪和右輪安裝在車身后部，分別由左電機和右電機通過相應的減速器減速后單獨驅動；單片機控制板采用模塊化設計，CPU采用STC89C52芯片；左電機和右電機為直流電機，由采用L293D芯片的電機驅動板來驅動；電源模塊用2節3.7V鋰電池串聯給小車控制系統供電。對于每個直流電機，單片機輸出一路PWM信號來控制電機的轉速，輸出一路TTL信號來控制電機的轉向；兩個槽型對射光電傳感器與安裝在左、右電機減速器上的測速碼盤組成了測速系統，將左、右輪的轉速轉換為脈沖實時傳送到單片機控制系統，可實現左、右輪轉速的閉環控制。5個紅外光電傳感器安裝在小車的前部，用來檢測黑色軌道與小車前進方向之間的偏差。

圖1 循跡小車的組成和工作原理圖

當小車正常前進時，紅外光電傳感器A檢測到黑色軌道；當紅外光電傳感器B或C或D或E檢測到黑色軌道時，說明小車檢測到位置誤差，軌道出現了偏轉；控制軟件根據5個紅外光電傳感器輸入狀態的變化來判斷位置誤差的大小和方向，并確定下一步調整時的轉向和轉角輸出，再計算出左、右電機的輸出轉速，然后再通過電機驅動板、左電機、右電機、減速器來改變左輪、右輪的速度，從而改變小車的前進方向，從而實現小車的自動循跡運動。

2 機械運動分析

根據小車的機械結構，將小車簡化為圖2中粗黑線所示的模型。萬向輪的中心點為G，左輪的中心點為H，右輪的中心點為J，H點和J點的中心點為I。設小車左輪的速度為v左，右輪的速度為v右，I點的速度為v中。由于小車前端為萬向輪，所以小車的轉向完全由左輪和右輪的速度決定，小車轉向時的回轉中心點在直線HJ上。經過Δt時間后，小車向右轉動θ角，其運動軌跡如圖2所示，點G，H，I和J轉動θ角后的位置分別為點G’，H’，I’和J’，O點為小車的回轉中心。若循跡小車是以萬向輪的中點G沿軌道循跡運動為目標的話，則小車轉彎時的回轉半徑為OG。

由公式

可得小車轉彎時各點的速度為：

式中：R左為OH的長度，R右為OJ的長度，R中為OI的長度，RG為OG的長度。

設HJ的長度為k，則

所以：

將公式5帶入公式3可得：

由以上分析可以看出，當k為常數時，循跡小車的轉彎軌跡（轉動方向和回轉中心點O的位置）是由左、右輪的速度v左和v右來確定的。

當v左>v右時，θ角為正值，小車右轉；當v左

當O點與J點重合時，v右=0，小車在繞J點轉彎。當O點與I點重合時，v左=—v右，v中=0，小車在繞I點轉彎；此時，小車的轉彎半徑最小，即小車所能通過的理想最小轉彎半徑為h（h為IG的長度）。所以，當h越小時，小車所能通過的最小轉彎半徑越小，其極限狀態是h=0，小車可繞I點原地打轉。

3 控制模型

3.1 簡單控制模型

循跡小車沿軌跡前進時，事先是不知道軌跡的形狀和轉彎半徑的，無法提前做出規劃，只能不斷地根據紅外光電傳感器的檢測結果實時調整小車的轉向和轉角。所以，這是一種隨著軌跡變化而不斷調整小車輸出的隨動控制系統。

圖3是一種循跡小車的簡單控制模型。控制系統根據5個紅外光電傳感器的檢測結果，判斷小車下一步有以下幾種輸出：直行、小角度左轉、大角度左轉、小角度右轉、大角度右轉。然后分別針對這幾種情況，根據需要轉動的方向和轉動的角度進行速度分配，確定左輪速度v左和右輪速度v右，然后以PWM方式控制左輪和右輪的速度，實現小車的轉向和直線運動。由于其控制簡單，編程方便，比較適合一些對速度要求不高的場合[6—8]。圖

圖3 循跡小車的簡單控制模型圖

采用簡單控制模型后，循跡小車過彎道時的運動軌跡如4所示。

小車開始時在直道上行駛，然后進入彎道；在控制程序的第一個掃描周期后小車的萬向輪到達G1點，紅外光電傳感器C檢測到了黑線，小車進行小角度右轉輸出，到第二個掃描周期時，小車運行到G2點。由圖2可知，小車在轉向時的運動軌跡是圓弧，所以G1和G2之間為圓弧。由于小車發生了轉向，所以在第二個掃描周期后，只有紅外光電傳感器A檢測到了黑線，小車進行直行輸出，到第三個掃描周期時，小車運行到G3點，G2和G3之間為直線；此時紅外光電傳感器C檢測到了黑線，小車再次進行小角度右轉輸出。依次類推，小車在通過彎道時不斷地進行轉彎和直行調整，其運動軌跡是一些圓弧和直線。由此看出，采用這種控制模型后，小車每個控制循環輸出的轉角是有級的且不連續的，循跡小車在轉彎時常出現擺動等不連貫運動，影響小車的運動速度。

圖4 循跡小車過彎道時的運動軌跡

圖5 位置+速度的串級PID控制模型

為了改善小車運動的流暢性，一些論文將小車前端的萬向輪改為由舵機控制的導向輪，然后分別對舵機轉向的角度和小車的左、右輪的速度采用了PID閉環控制[14,15]。由于小車的轉向還與左、右輪的速度有關，這種控制方式要求舵機的轉向角度與左、右輪的速度要實時匹配，否則就會出現輪子打滑現象，所以控制難度較大。

3.2 串級PID控制模型

經過以上分析可知，循跡小車是通過左、右輪的速度控制來實現小車沿黑色軌道循跡的位置控制，設計的位置+速度的串級PID控制模型如圖5所示。其內部為左、右輪速度控制的兩個速度環，外部為位置偏差控制的位置環。

小車前部的紅外光電傳感器檢測到小車與軌道的位置偏差可用檢測到黑色軌跡偏差的傳感器點和G點的直線與直線AG之間的夾角α表示，直線AG左邊的夾角為負，右邊的為正。則5個紅外光電傳感器可檢測到的位置偏差分別為 0o，—45o，—90o，45o和 90o。在每次控制循環中，位置偏差的設定值0和實際值α輸入到轉角PID控制器，通過PID運算產生小車的輸出轉角θ，θ角的正負決定了小車的轉向。由轉角θ再進行速度分配，確定左輪速度v左和右輪速度v右。每個車輪速度的設定值與實際檢測值輸入到相應的速度PID控制器，產生相應車輪的速度輸出，再通過電機驅動板以PWM方式控制電機的轉速；左輪和右輪的轉速差可實現小車的轉向和直線運動，從而實現小車的循跡運動控制。

在本控制模型中，轉角和轉速都實現了PID控制，從而使轉角和轉速的輸出連續變化；同時，采用兩個PID控制器串聯工作，即外環（位置環）轉角PID控制器的輸出經運算后作為兩個內環（速度環）速度PID控制器的設定值，由內環PID控制器的輸出去操縱小車的轉向和轉角，從而改善了控制質量，對外環被控量具有更好的控制效果。

3.3 速度分配策略

在公式5中，HJ的長度k為常數，每個控制循環的時間Δt也可確定，轉角PID控制器輸出了轉角θ，那么如何確定v左和v右這兩個變量呢？

為了盡快使小車轉向回到軌道上，可選用轉向最快策略。當為θ正時，小車需要右轉，可令v右=0，則v左=θk/Δt；當為θ負時，小車需要左轉，可令v左=0，則v右=—θk/Δt。這種方法雖然轉向較快，循跡效果很好，但是影響了小車轉彎的速度，勢必會影響循跡小車的比賽成績。

為了保證小車的速度，可選用速度最快策略。每個小車在直行時都有一個最大速度vm，那么在轉彎的時候可將外側輪子的速度選擇為最大速度vm。當為θ正時，小車需要右轉，可令 v左=vm，則 v右=vm—θk/Δt；當為 θ 負時，小車需要左轉，可令v右=vm，則v左=vm+θk/Δt。這種方法可以確保小車在轉彎時的速度較快。如果在調試時小車由于速度過快而滑出彎道，就要適當減小外側輪子的最大速度。

4 調試

將小車的硬件安裝好，然后按照串級PID控制模型編寫小車的控制程序，然后根據小車在軌道上的循跡運動情況，不斷調整小車的最大速度vm和PID控制器的參數等，使小車能夠連續地、以較快的速度轉彎并跑完賽道。試驗結果表明，采用串級PID控制模型的小車在遇到彎道能夠快速轉向，運動流暢連貫，比較準確地完成了循跡運動。

進一步改進小車循跡性能的措施：若小車的前部安裝更多的紅外光電傳感器或者采用其它模擬量位置傳感器將使檢測到的軌道位置偏差更加精確，也會改善轉角PID控制器輸出的波動，使小車的轉向更加平穩。若小車的單片機控制板采用性能更高的CPU（例如Arduino MEGA2560或32位ARM芯片），提高其運算速度，就能減小中斷掃描周期Δt，即減小了PID控制器的掃描周期，提高了小車的響應速度，使小車的循跡運動更加平穩可靠。

5 結論

本論文采用5個紅外光電傳感器檢測軌道的偏差，前部安裝萬向輪，通過后部左輪和右輪的速度差來控制小車的轉向，實現小車的循跡運動；分析了小車轉彎時的運動軌跡，討論了左輪和右輪速度與轉彎半徑等參數之間的關系；設計了位置+速度的串級PID控制模型，采用速度最快策略進行速度分配，確保小車在轉向時快速流暢，使小車能夠快速、平穩、準確地沿賽道軌跡運動。該小車的設計方案和串級控制模型也可應用于無人車間自動搬運的AGV小車、自主移動機器人等服務機器人的循跡及定位控制。