王 輝, 廖峰林,2

(1.成都大學(xué) 機(jī)械工程學(xué)院，四川 成都 610106; 2.四川新綠色藥業(yè)科技發(fā)展有限公司，四川 成都 610036)

0 引 言

智能小車作為無人駕駛汽車的“縮小版”，綜合了車輛工程、測控工程與人工智能等多個(gè)學(xué)科領(lǐng)域的相關(guān)技術(shù)，其環(huán)境適應(yīng)性強(qiáng)，易于開發(fā)，具有廣闊的應(yīng)用前景[1-2].目前在實(shí)際應(yīng)用中，智能小車需要解決的關(guān)鍵技術(shù)問題是循跡和避障的實(shí)現(xiàn)[3].對此，本研究采用純跟蹤算法對智能小車進(jìn)行運(yùn)動(dòng)控制，選擇適用于坐標(biāo)軸或黑白地圖搜尋點(diǎn)的A*算法對路徑進(jìn)行構(gòu)建，通過對該算法進(jìn)行優(yōu)化，使得兩點(diǎn)一線之間的連線由循跡跟蹤點(diǎn)轉(zhuǎn)變?yōu)檠E跟蹤線.同時(shí)，將控制算法應(yīng)用到實(shí)物智能小車中，通過超聲波傳感器測距與灰度傳感器循跡來進(jìn)行實(shí)際測試，驗(yàn)證了本仿真構(gòu)建的智能小車運(yùn)動(dòng)模型、PID的參數(shù)調(diào)節(jié)、控制算法及規(guī)劃出的路徑的有效性.在具體測試中，實(shí)物智能小車實(shí)現(xiàn)了按照設(shè)定要求行走的目的.

1 智能小車的模型構(gòu)建

1.1 智能小車運(yùn)動(dòng)模型

實(shí)際運(yùn)動(dòng)中的智能小車可以視為一個(gè)剛體，用其質(zhì)心的空間坐標(biāo)與空間姿態(tài)的6個(gè)坐標(biāo)可近似描述其運(yùn)動(dòng)過程[4].基于此，本研究的智能小車循跡與避障主要以仿真模擬為基礎(chǔ)，選取前輪轉(zhuǎn)向后輪驅(qū)動(dòng)模式，同時(shí)僅考慮智能小車的運(yùn)動(dòng)特性，而忽略諸如車身滑動(dòng)、輪胎摩擦等動(dòng)態(tài)因素.據(jù)此建立智能小車的理想運(yùn)動(dòng)模型，即把智能小車的運(yùn)動(dòng)簡化為平面運(yùn)動(dòng)，具體構(gòu)建的智能小車運(yùn)動(dòng)模型如圖1所示，

圖1 智能小車運(yùn)動(dòng)模型示意圖

圖1中，設(shè)定智能小車的質(zhì)心坐標(biāo)為(x,y)，后軸中心坐標(biāo)為(x1,y1)，前軸中心坐標(biāo)為(x2,y2)，角度θ為智能小車的航向角，為車身與X軸正方向的夾角，角度φ為智能小車前輪轉(zhuǎn)向角，軸距為L，前軸中心到質(zhì)心長度為a，后軸中心到質(zhì)心長度為b，據(jù)此，可以得到智能小車的平面運(yùn)動(dòng)模型方程式為，

(1)

式中，V是智能小車的速度.

1.2 基于MATLAB/Simulink仿真工具的智能小車仿真模塊構(gòu)建

在MATLAB/Simulink仿真工具中構(gòu)建的智能小車的整體仿真模塊主要由智能小車的運(yùn)動(dòng)仿真模塊、速度仿真模塊和轉(zhuǎn)向角仿真模塊所組成.

1.2.1 運(yùn)動(dòng)仿真模塊

智能小車的運(yùn)動(dòng)模型經(jīng)轉(zhuǎn)換后變?yōu)檫\(yùn)動(dòng)仿真模塊，具體如圖2所示.

圖2 智能小車的運(yùn)動(dòng)仿真模塊

1.2.2 速度仿真模塊

在建立智能小車速度仿真模擬中，由于智能小車的輸入環(huán)節(jié)存在速度的變化，仿真中可能會(huì)出現(xiàn)振蕩，因此需要添加PID控制器對其進(jìn)行控制.同時(shí)，在設(shè)定車速時(shí)，需要將帶動(dòng)車輪旋轉(zhuǎn)的電機(jī)驅(qū)動(dòng)轉(zhuǎn)換為速度的轉(zhuǎn)換函數(shù)，從而得到智能小車的速度仿真模塊，具體如圖3所示，圖3中，PID(s)為MATLAB/Simulink仿真工具中自帶的控制模塊，可以進(jìn)行相關(guān)參數(shù)的調(diào)整.

圖3 智能小車的速度仿真模塊

1.2.3 轉(zhuǎn)向角仿真模塊

智能小車的轉(zhuǎn)向角仿真模塊同樣也需要利用PID控制.由于智能小車的轉(zhuǎn)向角采用平滑的曲線達(dá)到預(yù)設(shè)角度，為了使智能小車在開始有位移時(shí)能夠快速動(dòng)起來，其速度需要先超過預(yù)設(shè)數(shù)值再退回至預(yù)設(shè)數(shù)值，因此其需要通過PD控制來實(shí)現(xiàn)，具體如圖4所示，

圖4 智能小車轉(zhuǎn)向角第一模塊

此外，智能小車在轉(zhuǎn)向時(shí)通常會(huì)引起較大的慣性，對此需要添加一階慣性環(huán)節(jié)作為轉(zhuǎn)向角的第二模塊，具體如圖5所示.

圖5 智能小車轉(zhuǎn)向角第二模塊

1.2.4 智能小車整體仿真模塊

把智能小車的運(yùn)動(dòng)仿真模塊、速度仿真模塊、轉(zhuǎn)向角仿真模塊與輸入輸出模塊連接起來，最終得到智能小車的整體仿真模塊，具體如圖6所示.

圖6 智能小車仿真模塊的整體構(gòu)架示意圖

圖6中，輸入為智能小車的目標(biāo)速度V和目標(biāo)轉(zhuǎn)向角φ，輸出為智能小車當(dāng)前所處的位置(x,y)、航向角θ、軸向速度vμ和法向速度vω以及當(dāng)前智能小車的轉(zhuǎn)向角φ，實(shí)現(xiàn)實(shí)時(shí)顯示智能小車的當(dāng)前位置與姿態(tài).

2 控制算法

本研究通過選用純跟蹤控制算法對智能小車的輸入?yún)?shù)進(jìn)行控制，可以使智能小車按照設(shè)定路徑運(yùn)行且輸入平滑、振蕩減小.

2.1 純跟蹤控制算法

智能小車在行進(jìn)過程中通常都是直接跟蹤由一系列路徑點(diǎn)所構(gòu)成的規(guī)劃路徑來自動(dòng)識別路線.本研究以智能小車的輸入?yún)?shù)轉(zhuǎn)向角φ為目標(biāo)，選擇純跟蹤控制算法，利用路徑信息對智能小車的轉(zhuǎn)向進(jìn)行控制[5]，其基本思路是，在每個(gè)控制周期，通過前方目標(biāo)軌跡上的一個(gè)點(diǎn)，控制智能小車當(dāng)前方向盤的動(dòng)作，使其產(chǎn)生向目標(biāo)點(diǎn)的運(yùn)動(dòng)，同時(shí)反映智能小車的位置、速度、加速度與時(shí)間變化的關(guān)系.其具體原理如圖7所示，

圖7 純跟蹤控制算法示意圖

圖7中，R是智能小車的轉(zhuǎn)彎半徑；L是智能小車的軸距；δ是智能小車的前輪轉(zhuǎn)角；α是智能小車車身與預(yù)瞄點(diǎn)的夾角；G(gx,gy)是智能小車在軌跡上的預(yù)瞄點(diǎn)；ld是智能小車的預(yù)瞄距離，用于在前方循跡的引導(dǎo)線上選擇某個(gè)期望點(diǎn)[6]；圓弧表示智能小車的目標(biāo)軌跡.

將純跟蹤控制算法化簡后得到其計(jì)算式為，

δ(t)=arctan(2L*sin(a(t))/ld)

(2)

2.2 尋找預(yù)瞄點(diǎn)

在本研究中，純跟蹤算法中的預(yù)瞄點(diǎn)是智能小車下一個(gè)需要到達(dá)的地方.而在MATLAB仿真軟件中，預(yù)瞄點(diǎn)的實(shí)現(xiàn)可通過修改代碼為尋找最近點(diǎn)來實(shí)現(xiàn).預(yù)瞄點(diǎn)的計(jì)算方法為：設(shè)定當(dāng)前智能小車的位置坐標(biāo)為(x1,y1)，其他目標(biāo)的坐標(biāo)為(x2,y2)，d為其他目標(biāo)點(diǎn)到當(dāng)前坐標(biāo)點(diǎn)的距離，其計(jì)算式為，

(3)

將計(jì)算得到的距離d進(jìn)行對比分析，并選擇最小的距離d，然后將對應(yīng)的坐標(biāo)點(diǎn)輸入至預(yù)瞄點(diǎn)，即獲得下一個(gè)需要到達(dá)的坐標(biāo)點(diǎn).

2.3 PID控制

智能小車避障最關(guān)鍵的技術(shù)是控制好智能小車行駛的方向和速度.PID控制具有魯棒性強(qiáng)的特點(diǎn)，因而在工業(yè)工程過程控制中被廣泛地應(yīng)用.PID控制的計(jì)算式為，

(4)

1)本研究智能小車的輸入速度在PID控制的前后對比如圖8所示，

圖8 智能小車速度PID控制前后對比

圖8中，虛線為PID控制調(diào)節(jié)前的速度輸入，實(shí)線為PID控制調(diào)節(jié)后的速度輸入.在1 s時(shí)輸入速度設(shè)定為1 m/s.由圖8可以看出，在PID控制調(diào)節(jié)前速度輸入偏大，在接近3 s時(shí)穩(wěn)定，而PID控制調(diào)節(jié)后輸入速度為設(shè)定的1 m/s速度，在接近2 s時(shí)輸入穩(wěn)定.

2)智能小車的輸入轉(zhuǎn)向角在PID控制前后的對比如圖9所示，

圖9 轉(zhuǎn)向角的PID控制前后對比

圖9中，圖9(a)為沒有PID控制調(diào)節(jié)的轉(zhuǎn)向角輸入，圖9(b)為有PID控制調(diào)節(jié)的轉(zhuǎn)向角輸入.通過對比可以看出，PID控制調(diào)節(jié)后的轉(zhuǎn)向角輸入平滑穩(wěn)定，且振蕩較小.

3 地圖與路徑構(gòu)建及優(yōu)化

3.1 地圖構(gòu)建

利用MATLAB仿真工具的矩陣數(shù)據(jù)可構(gòu)建costmap地圖.即將坐標(biāo)地圖顯示成柵格化地圖[7].在本研究中，地圖坐標(biāo)中含有障礙物、可行區(qū)域、起始點(diǎn)及目標(biāo)終點(diǎn)，具體如圖10和圖11所示，

圖10 矩陣數(shù)

圖11 cotsmap地圖

圖10中，有數(shù)據(jù)的地方為可行區(qū)域，Inf表示障礙物，0表示起始點(diǎn)及終點(diǎn).

圖11為轉(zhuǎn)換后的cotsmap地圖，其上障礙物的位置與圖10矩陣中障礙物的坐標(biāo)一一對應(yīng).由于圖中的障礙物數(shù)據(jù)是變化的，所以每次生成的地圖也是變化的，而障礙物的比例是設(shè)定的，故障礙物的方格在每次的數(shù)量是一致的.

3.2 路徑構(gòu)建及優(yōu)化

路徑構(gòu)建即是避開障礙物尋找一條或者多條可行性的道路，本研究的路徑構(gòu)建采用A*算法，其基本思路是：首先，設(shè)定2個(gè)空矩陣：一個(gè)命名“道路”，一個(gè)命名“距離”；然后，對所生成的地圖矩陣進(jìn)行搜尋，將搜尋過程中遇到的可行性路徑坐標(biāo)放入“道路”矩陣之中，并將該點(diǎn)和目標(biāo)點(diǎn)的距離放入“距離”矩陣之中，在起始點(diǎn)到終點(diǎn)之間一直循環(huán)該步驟，直到尋找到目標(biāo)點(diǎn)位置后跳出循環(huán)；第三，跳出循環(huán)后，對“距離”矩陣進(jìn)行各點(diǎn)比較，直到距離為“0”，即尋到了目標(biāo)點(diǎn)為止；最后，得到這一系列點(diǎn)之后，再從目標(biāo)點(diǎn)出發(fā)，尋回至初始點(diǎn)，得出路徑中每一個(gè)點(diǎn)是如何由上一個(gè)點(diǎn)尋找出來的，再將各點(diǎn)連成線，即可得到該地圖中搜尋出來的一條由起始點(diǎn)到終點(diǎn)的路徑.本研究依據(jù)此思路搜尋得出的路徑如圖12所示.

圖12 地圖路徑構(gòu)建

本研究利用A*算法搜尋出的地圖路徑各點(diǎn)坐標(biāo)及對該路徑優(yōu)化后的部分坐標(biāo)對比結(jié)果如圖13所示，

圖13 路徑構(gòu)建及優(yōu)化

圖13中，左邊為A*算法搜尋出來的路徑坐標(biāo)、右邊為優(yōu)化后的部分路徑坐標(biāo)，橫向1、2表示(x,y)坐標(biāo)，縱向1～21表示從起始點(diǎn)到目標(biāo)終點(diǎn)搜尋出來并按順序得到的各點(diǎn)坐標(biāo).

4 智能小車仿真運(yùn)行

智能小車與路徑的匹配度，不僅可以檢驗(yàn)智能小車的控制算法是否有效與運(yùn)動(dòng)模型建立是否匹配，還可以判斷所獲得的路徑是否適用.由于避障路徑的分度值為m，本研究設(shè)定的智能小車的車身長度僅為0.2m，故控制算法向前搜尋的范圍僅有0.25 m.這就意味著，智能小車不能夠完全看清范圍點(diǎn)內(nèi)的位置而導(dǎo)致搜尋點(diǎn)失敗.對此，本研究的解決方法是將得出的路徑分度值變小，并將優(yōu)化后的路徑坐標(biāo)重新部署到智能小車的跟蹤地圖之中，可以發(fā)現(xiàn)路徑中的坐標(biāo)點(diǎn)之間的距離變小了，而路徑的長度沒有變化.將搭建好的智能小車模塊在有規(guī)劃出路徑的地圖中進(jìn)行循跡與避障仿真運(yùn)行，其仿真運(yùn)行軌跡如圖14所示，

圖14 智能小車仿真運(yùn)行軌跡

圖14中，紅線為智能小車運(yùn)行軌跡，藍(lán)線為規(guī)劃出來的路徑，可以看出智能小車在循跡避障過程中能夠按照設(shè)定的路徑運(yùn)行，只有在直角拐彎時(shí)，會(huì)出現(xiàn)不重合情況.這是因?yàn)橹悄苄≤囋O(shè)定轉(zhuǎn)向是由前輪轉(zhuǎn)向角來控制，因本研究的智能小車前輪最多可以轉(zhuǎn)向50°，導(dǎo)致其在直角轉(zhuǎn)彎時(shí)不能按照直角路線行走，而是按照圓弧路線行走.

智能小車仿真運(yùn)行的實(shí)時(shí)數(shù)據(jù)如圖15所示，

圖15 智能小車仿真實(shí)時(shí)數(shù)據(jù)

圖15中，圖15(a)顯示智能小車的實(shí)時(shí)速度,其中，黑色的線為智能小車的最大速度，設(shè)定值為0.5 m/s，藍(lán)色的線為智能小車的X軸向速度，紅色的線為智能小車的Y軸向速度.當(dāng)智能小車以45 °方向行走時(shí)，速度為恒定值.由紅色的線與黑色的線交點(diǎn)可以得知智能小車此刻的車身朝向，X軸向速度在向負(fù)值變化，Y軸向速度在向正值變化，說明智能小車在向車頭的左側(cè)轉(zhuǎn)彎行駛.圖15(b)顯示智能小車實(shí)時(shí)轉(zhuǎn)向角，正值為向左轉(zhuǎn)彎，負(fù)值為向右轉(zhuǎn)彎，而最大轉(zhuǎn)角則和之前設(shè)定的一樣為50 °.由圖15的智能小車仿真實(shí)時(shí)數(shù)據(jù)與圖14的智能小車行進(jìn)軌跡對比可知，本研究的智能小車構(gòu)建的運(yùn)動(dòng)模型、控制算法、設(shè)定軌跡與實(shí)際運(yùn)行狀態(tài)一一對應(yīng)，即創(chuàng)建的各個(gè)模塊合理、有效.

5 智能小車實(shí)車測試

本研究的智能小車實(shí)車采用差速輪運(yùn)行及轉(zhuǎn)向，構(gòu)建的控制算法模型轉(zhuǎn)換為C語言[8]嵌入其中，并采用灰度傳感器及超聲波傳感器進(jìn)行循跡與避障.灰度傳感器的作用是使智能小車循著黑線運(yùn)行，超聲波傳感器則感應(yīng)前方是否有障礙物及判定障礙物的距離.在實(shí)際測中，將靜態(tài)地圖的路徑探索轉(zhuǎn)換為在地面鋪設(shè)的軌跡.鋪設(shè)的軌跡可以作為分度值趨近于0的坐標(biāo)點(diǎn)，使得智能小車能夠有路徑可以跟蹤.在智能小車的車頭布置了4個(gè)灰度傳感器，將仿真中的純跟蹤算法以傳感器的感應(yīng)來表示.

本研究智能小車實(shí)車循跡的思路為：設(shè)定智能小車的4個(gè)灰度傳感器的順序以沿著智能小車車頭方向從左后、左前、右前、右后的順序依次為1、2、3、4.當(dāng)傳感器2和3感應(yīng)為黑且傳感器1和4感應(yīng)為白時(shí)，智能小車向前行；當(dāng)傳感器2感應(yīng)為黑且傳感器1、3和4感應(yīng)為白時(shí)，智能小車輕微左轉(zhuǎn)，此時(shí)，左輪電機(jī)轉(zhuǎn)速降低，右輪電機(jī)轉(zhuǎn)速不變，直到傳感器2和3感應(yīng)為黑，1和4感應(yīng)為白時(shí)左右輪電機(jī)轉(zhuǎn)速恢復(fù)相等，繼續(xù)前行；輕微右轉(zhuǎn)同理；當(dāng)傳感器1和2感應(yīng)為黑時(shí)且傳感器3和4感應(yīng)為白時(shí)，智能小車向左大轉(zhuǎn)彎，左輪電機(jī)減速，右輪電機(jī)加速，直到傳感器2和3感應(yīng)為黑且傳感器1和4感應(yīng)為白時(shí)繼續(xù)前行；當(dāng)4個(gè)傳感器均感應(yīng)為黑時(shí)，智能小車停止運(yùn)行.智能小車實(shí)車測試結(jié)果如圖16所示.

圖16中，圖16(a)是智能小車正常循跡的過程，圖16(b)是智能小車遇到障礙物躲避的過程，圖16(c)是智能小車躲避障礙物后重新進(jìn)行循跡，回到正常路線上來的過程.

圖16 智能小車循跡與避障實(shí)物測試示意圖

6 結(jié) 論

本研究通過建立智能小車的運(yùn)動(dòng)模型，并基于MATLAB/Simulink分析工具進(jìn)行了仿真模擬，完成了對仿真中智能小車控制算法的選型及優(yōu)化，使智能小車在仿真中能按照設(shè)定路徑運(yùn)行，確定了仿真中智能小車的避障算法，完成了仿真所需要的對智能小車速度及轉(zhuǎn)向角的PID控制參數(shù)調(diào)整及確定，使仿真中的智能小車軌跡能夠與設(shè)定軌跡相吻合.

同時(shí)，本研究對構(gòu)建的智能小車實(shí)車進(jìn)行了相關(guān)測試，結(jié)果驗(yàn)證了本智能小車控制算法的有效性.