基于Mindstorms的智能機器人控制系統(tǒng)開發(fā)*

程 翀 郭曉彬 陳祥磊 李超群 熊 輝

(武漢理工大學(xué)現(xiàn)代汽車零部件技術(shù)湖北省重點實驗室1) 武漢 430070)(汽車零部件技術(shù)湖北省協(xié)同創(chuàng)新中心2) 武漢 430070)

0 引 言

智能機器人技術(shù)涵蓋了機械工程、信息技術(shù)、電子技術(shù)和數(shù)學(xué)物理等多學(xué)科知識，能夠完成重復(fù)乏味的工作，提高人們的工作效率和生活品質(zhì).具有代表性的樂高公司從1984年開始與美國麻省理工學(xué)院合作，研發(fā)出現(xiàn)代科技與傳統(tǒng)玩具相互結(jié)合的智能機器人.所推出的最新的Mindstorms EV3機器人因其智能性、易操作性、開放性及算法可移植性，在機器人算法開發(fā)中得到了廣泛的應(yīng)用.

本文以Mindstorms EV3機器人為研究平臺，利用超聲波傳感器、攝像頭對智能機器人控制系統(tǒng)進行開發(fā)，所開發(fā)的智能機器人包括基于機器視覺的車道保持、換道避障、緊急制動，以及人機共駕等功能.其中，車道保持功能利用手機攝像頭的機器視覺采集道路圖像，并通過基于Python的PC圖像處理程序提取車道線，將偏差等信號發(fā)送給機器人控制系統(tǒng)，從而保證機器人的車道保持功能.其次，機器人通過超聲波傳感器檢測前方障礙物信息，并在控制器的控制下完成換道避障及緊急制動的功能.現(xiàn)如今，由于自動駕駛模式出現(xiàn)故障時無法快速人為操控，無人駕駛車輛事故頻繁發(fā)生，因此，人為控制與自動控制模式之間控制權(quán)的快速轉(zhuǎn)換尤為重要.當(dāng)機器人自動控制模式出現(xiàn)故障時，能夠快速啟動人為控制模式，并借助人為操控轉(zhuǎn)向盤、油門踏板和制動踏板來完成機器人的正常行駛.

1 機器人結(jié)構(gòu)搭建

本研究的智能機器人為四輪移動機器人，在樂高EV3現(xiàn)有材料的種類與數(shù)量的基礎(chǔ)上，充分運用各種材料的功能，設(shè)計出擁有前驅(qū)前轉(zhuǎn)向和獨立懸架系統(tǒng)的機器人結(jié)構(gòu).其中，轉(zhuǎn)向系統(tǒng)由轉(zhuǎn)向電機通過齒輪齒條方式控制，并在后期的轉(zhuǎn)向測試中驗證了轉(zhuǎn)向系統(tǒng)的可行性、可靠性與快速響應(yīng)性.驅(qū)動系統(tǒng)由驅(qū)動電機通過齒輪、差速器及萬向軸傳遞給驅(qū)動輪，通過不斷調(diào)整傳動過程中的齒輪傳動比，將轉(zhuǎn)速與轉(zhuǎn)矩調(diào)整到最佳模式.為了保證機器人快速檢測到前方障礙物并作出反應(yīng)，將超聲波傳感器安裝于機器人最前方.為了充分檢測到機器人前方道路信息，將手機安裝于機器人前方較高處.最終，所搭建的智能機器人整體結(jié)構(gòu)見圖1.

圖1 智能機器人整體結(jié)構(gòu)圖

2 底層模塊開發(fā)

2.1 通信模塊

轉(zhuǎn)向盤、手機攝像頭和小車主機的通信采用UDP協(xié)議實現(xiàn)，見圖2，各硬件間的通信連接見圖3.

圖2 UDP原理圖

圖3 各硬件間的通信連接

2.2 底層控制模塊

轉(zhuǎn)向電機為一個中型電機，負(fù)責(zé)小車的橫向運動；驅(qū)制動電機為兩個大型電機，負(fù)責(zé)小車的縱向運動.底層控制模塊與應(yīng)用層的接口為小車前輪轉(zhuǎn)角和小車速度.其中小車前輪轉(zhuǎn)角由小車轉(zhuǎn)向傳動比和轉(zhuǎn)向電機輸出轉(zhuǎn)角推算得到；小車速度由驅(qū)制動電機輸出轉(zhuǎn)角、采樣時間,以及車輪半徑推算得到.

小車的前輪轉(zhuǎn)角控制和速度控制均采用PID控制器[1].小車前輪轉(zhuǎn)角控制器的輸入為轉(zhuǎn)向電機的期望轉(zhuǎn)角與實際轉(zhuǎn)角差值，輸出為轉(zhuǎn)向電機占空比；小車速度控制器的輸入為驅(qū)制動電機的期望轉(zhuǎn)角和實際轉(zhuǎn)角的差值、輸出分別為驅(qū)制動電機占空比.其中轉(zhuǎn)向電機和驅(qū)制動電機的期望轉(zhuǎn)角分別由期望前輪轉(zhuǎn)角和期望車速轉(zhuǎn)換得到.電機轉(zhuǎn)角控制框圖見圖4.上述控制器的控制策略見式(1)～(2).

圖4 電機轉(zhuǎn)角控制框圖

(1)

e(t)=δdes(t)-δact(t)

(2)

式中：UPWM為電機占空比；δdes為電機期望轉(zhuǎn)角；δact為電機實際轉(zhuǎn)角；KP，KI，KD分別為PID控制器的比例、積分、微分系數(shù).

3 控制策略開發(fā)

智能機器人的傳感器主要由手機攝像頭和超聲波傳感器組成，監(jiān)測車道線和實時獲取本機器人與前方障礙物之間的距離.整體控制系統(tǒng)見圖5.

圖5 機器人控制系統(tǒng)架構(gòu)圖

3.1 決策層控制策略

小車包含自主控制模式和人為控制模式.兩種工作模式可以通過轉(zhuǎn)向盤上的按鈕一鍵切換.當(dāng)小車進入自主控制模式時，默認(rèn)打開車道保持功能，此時小車將沿車道中心線以恒定速度行駛.若小車當(dāng)前車道前方出現(xiàn)低速障礙物且滿足自動換道功能的激活條件，則小車執(zhí)行換道操作.而當(dāng)小車前方突現(xiàn)障礙物或障礙物突然減速時，緊急制動功能被觸發(fā)，小車將緊急制動，以避免碰撞或減輕碰撞傷害.各個功能的觸發(fā)條件可見各功能的具體介紹.其中緊急避障擁有最高的優(yōu)先級.

上述各個功能的切換通過Simulink中的Stateflow模塊實現(xiàn).決策模塊的輸入接口包括由環(huán)境感知和數(shù)據(jù)融合模塊給出的信息，輸出接口為希望小車下一時刻執(zhí)行的前輪轉(zhuǎn)角和速度指令.

3.2 車道保持控制策略

車道保持功能主要基于機器視覺和預(yù)瞄控制實現(xiàn).

1) 圖像處理 將傳感器輸出的圖像由RGB轉(zhuǎn)為灰度圖，設(shè)定適當(dāng)?shù)拈撝担祷玫降幕叶葓D，最后通過邊緣檢測算法，得到車道線信息.

2) 預(yù)瞄距離模糊控制 對輸入的車輛速度和加速度進行模糊化，設(shè)定模糊規(guī)則，經(jīng)模糊推理接解模糊后，得到輸出量預(yù)瞄距離的精確值.

3) 方向盤角度模糊控制 對輸入的車輛偏離中心線距離及其變化率進行模糊化，設(shè)定模糊規(guī)則，經(jīng)模糊推理接解模糊后，得到輸出量預(yù)瞄距離的精確值.

3.3 換道避障控制策略

1) 自主換道決策機制 實際場景中根據(jù)換道動機和影響結(jié)果的不同，換道行為可分為強制性換道和選擇性換道兩類.強制性換道指的是車輛在指定區(qū)域內(nèi)必須進行換道操作，比如,交叉路口、匝道等.選擇性換道指的是駕駛員為追求更寬闊的行駛空間和更高的行車速度而進行的換道行為.根據(jù)相關(guān)文獻(xiàn)中數(shù)據(jù)，車輛換道的主要原因是當(dāng)前車道前方車輛車速過低[2].本課題也主要針對此種情況開展研究.換道意圖的產(chǎn)生主要依據(jù)期望間距和期望速度進行判斷.

換道意圖產(chǎn)生后還需要進行換道可行性分析，可行性分析往往需要綜合考慮本車與相鄰車道前后車的相對距離和相對速度兩方面因素，如為滿足本車的速度需求，相鄰車道的前車車速應(yīng)大于某一閾值；為保證安全，即當(dāng)相鄰車道前車出現(xiàn)特殊情況緊急剎車時，應(yīng)保證足夠的制動距離，因此,間距應(yīng)大于最小安全間距.同時相鄰車道后車也應(yīng)與本車保持安全距離，以便后車有足夠的距離緊急制動.不過由于本課題所用小車的車載傳感器所能夠獲取的周邊環(huán)境信息有限，本文假設(shè)換道意圖產(chǎn)生后，換道總是可行的.

另外，根據(jù)換道請求發(fā)起人的不同，換道行為又可分為駕駛員指令下?lián)Q道和自主換道.當(dāng)駕駛員發(fā)出換道請求時，系統(tǒng)需要確定換道請求的可行性，若可行，則進行換道操作；若不可行，則保持在當(dāng)前車道行駛.自主換道時，由系統(tǒng)自動實時判斷是否進行換道操作.本課題可實現(xiàn)駕駛員指令下?lián)Q道和自主換道，兩者是類似的，本文以自主換道為例介紹說明.

車輛對當(dāng)前車道的行車空間的期望一般是高于最小行車安全間距的，因此需要保證車輛安全行駛的最小車間距.本課題中期望間距Sdes為

Sdes=Sbrake+c

(3)

式中：最小車間距Sbrake與車輛制動性能有關(guān)；c為常數(shù).

因此，車輛換道意圖產(chǎn)生的一個條件為車輛間距S小于期望間距，且有減小趨勢.

在實際行駛過程中，車輛總希望以期望車速行駛.若前方車輛持續(xù)低速，車輛的期望車速得不到滿足，那么當(dāng)不滿持續(xù)到一定程度時換道意圖產(chǎn)生.本課題借助車速不滿積累度的概念衡量上述不滿程度[3].車速不滿積累度D(k)為

(4)

式中：udes為期望車速；upre為前車車速；T為采樣周期.

因此，車輛換道意圖產(chǎn)生的一個條件為車速不滿積累度達(dá)到某一閾值DThr.

綜上所述，車輛換道意圖產(chǎn)生條件為

SDThr

(5)

2) 自主換道控制指令 一般而言，換道的目標(biāo)車道確定后，將規(guī)劃出一條期望的換道路徑，并實時轉(zhuǎn)換為轉(zhuǎn)向盤轉(zhuǎn)角、車速等控制指令由底層模塊負(fù)責(zé)執(zhí)行.換道的路徑規(guī)劃方法包括了基于搜索的、基于幾何方法的、基于車輛模型的等.

考慮到本課題所用小車無法準(zhǔn)確獲知當(dāng)前位置信息，也即無法完成閉環(huán)控制，因此,本課題的小車前輪轉(zhuǎn)角指令采用開環(huán)控制.文獻(xiàn)中，正弦變化的方向盤轉(zhuǎn)角指令能夠滿足換道需求，且基本能夠保證換道結(jié)束后車輛航向角保持不變[4].換道過程中，理想的前輪轉(zhuǎn)角控制指令δ(t)變化見式(6).為方便底層模塊良好跟蹤轉(zhuǎn)角指令，本課題對理想前輪轉(zhuǎn)角控制指令進行平直化處理，處理后的前輪轉(zhuǎn)角控制指令見圖6.

δ(t)=Asin (ωt)

(6)

式中：δ為前輪轉(zhuǎn)角極值；t1，t2，t3為不同階段的轉(zhuǎn)角指令持續(xù)時間.圖6中各參數(shù)需要根據(jù)車速進行標(biāo)定，以保證小車換道后盡量保持在車道中心線附近.

圖6 處理后前輪轉(zhuǎn)角控制指令

考慮到小車車速較低，假定換道過程中車速恒定，車速指令設(shè)定為VALC.

3.4 緊急制動控制策略

1) 最小安全距離 在實際的行駛工況中，最小安全距離與前車的運動狀態(tài)強相關(guān)，因此在建立最小安全距離模型時，根據(jù)前車的運動狀態(tài)分為前車靜止(CCRs)、前車勻速(CCRm)及汽車減速(CCRb)[5-6].

圖7為車輛制動的四個階段，當(dāng)駕駛員發(fā)現(xiàn)前方危險時的反應(yīng)時間，在駕駛員反應(yīng)階段，因為車輛的制動協(xié)調(diào)時間(包括消除各鉸鏈和軸承間間隙的時間以及制動器摩擦片完全貼靠在制動盤上的時間t2和制動減速度從零增加到恒定值的時間t3)以上時間由車輛制動系統(tǒng)的性能決定，這一階段制動器起作用需耗費t2+t3的時間；在制動力達(dá)到最大值之后，車輛會保持tc時間的最大制動，并且制動減速度保持不變；在放松制動階段，即使駕駛員松開制動，由于液壓或者氣壓制動力的消除需要一段時間t4，車輛也會繼續(xù)受到制動力的減速.

圖7 車輛制動距離分析

分別求出各個期間的行駛距離，即L1，L2，L3，Lc，L4.如果不考慮其他影響因素，則

L1=Vstartt1

(7)

在制動傳遞延遲時間t2內(nèi)汽車以制動前的初速度勻速運動，因此所行駛的距離為

L2=Vstartt2

(8)

在t3時間內(nèi)車輛做變減速運動，從圖中可以得到每時刻所對應(yīng)的加速度為

(9)

速度為

(10)

經(jīng)積分變換，可求得該段時間內(nèi)所行駛的距離為

(11)

假設(shè)tc段的初始速度為Vc，則在恒減速階段的速度按為

(12)

又因為Vc也是t3階段的末尾時的速度，則有

(13)

在tc階段走過的路程

(14)

最終本車的制動行駛距離為

L=L1+L2+L3+Lc

(15)

將式

(16)

為了簡化起見，令t=t1+t2+t3，因為車輛的制動協(xié)調(diào)時間僅為零點幾秒，其1/2及二次方會更小，故等式中的后兩項可忽略不計.這里稱t0為制動操作反應(yīng)時間，它包括駕駛?cè)说闹苿臃磻?yīng)時間和制動協(xié)調(diào)時間.此時，本車的制動行駛距離公式簡化為

(17)

當(dāng)前車靜止時，此時前車的制動距離Lb=0，最危險的時刻發(fā)生在自車停止時與前車距離最近的地方.為保證自車的絕對安全，自車與前車之間存在一定的安全間距D0，則此時的最小安全間距為

(18)

當(dāng)前車勻速時，當(dāng)前車的速度高于自車的速度時，則兩車之間的距離會越來越大，不存在危險的工況出現(xiàn).但當(dāng)自車的速度高于前車的初速度時，則在行駛的過程中會出現(xiàn)安全臨界距離，此時就存在碰撞的危險.若駕駛員采取制動措施，直到與前車速度Vf相等，則此時自車行駛路過的距離為

(19)

自車從初速度減速到Vf，此過程經(jīng)歷的時間為

(20)

當(dāng)前車靜止時，即Vf=0，此時的最小安全距離即為Ls.

當(dāng)前車減速時，自車與前車的初速存在三種情形：Vstart>Vf,Vstart=Vf,Vstart

2) AEB介入策略 當(dāng)汽車在行駛過程中，AEB系統(tǒng)檢測到有碰撞危險，但危險的程度較低，系統(tǒng)采用燈光和報警提醒駕駛員危險的存在.若駕駛員沒有及時采取有效措施，隨著危險程度的加深，燈光由原來的橙色提醒變?yōu)楦鼮轱@眼的紅色提醒，與此同時提醒的報警聲更加急促.如果危險程度繼續(xù)上升，AEB系統(tǒng)在發(fā)出預(yù)警的同時并采取了部分制動，隨著危險程度的持續(xù)上升，碰撞即將發(fā)生或者無法避免時，AEB系統(tǒng)將采取完全制動的策略[8].

通過底層的傳感器，測量本車與前車的實時距離dis_front,然后將該值與選定的最小安全距離Ls進行比較，當(dāng)檢測的距離小于安全閾值時，此時激活A(yù)EB的執(zhí)行標(biāo)志位，然后將該標(biāo)志位，作為底層AEB激活的條件.

3.5 人機交互界面開發(fā)

基于python開發(fā)GUI界面，該界面中分為兩大顯示區(qū)域，其中參數(shù)端口區(qū)，主要是啟用/關(guān)閉IP Camera、其中Position error端口的作用是顯示車輛與當(dāng)前車道中心線的偏差值，同時也是用于車道保持最重要的參數(shù).

4 仿真結(jié)果分析

4.1 車輛底層控制模塊仿真分析

轉(zhuǎn)向電機和驅(qū)制動電機PID控制器的輸入為期望轉(zhuǎn)角和實際轉(zhuǎn)角的差值，輸出為電機的占空比.圖8為轉(zhuǎn)向電機(中型電機)和驅(qū)制動電機(大型電機)轉(zhuǎn)角的控制效果，圖中虛線為電機期望轉(zhuǎn)角、實線為電機實際轉(zhuǎn)角.由圖8可知，電機響應(yīng)的上升時間在0.15～0.2 s，且?guī)缀醪怀{(diào)，電機轉(zhuǎn)角的跟隨控制效果良好，可保證上層控制指令的執(zhí)行效果.

圖8 電機轉(zhuǎn)角

4.2 ALC功能仿真分析

小車自動換道過程中的前輪轉(zhuǎn)角采用開環(huán)控制，這里以小車向左換道為例說明.向左換道時，小車前輪轉(zhuǎn)角變化見圖9，其中虛線表示期望前輪轉(zhuǎn)角，實線表示實際前輪轉(zhuǎn)角(由轉(zhuǎn)向電機轉(zhuǎn)角轉(zhuǎn)換得到).由圖9可見，小車實際前輪轉(zhuǎn)角能夠較好地跟隨期望前輪轉(zhuǎn)角.圖10為Simulink中二自由度車輛模型向左換道的仿真結(jié)果.仿真開始0.4 s后換道功能觸發(fā)，換道過程中車輛始終以0.5 m/s的速度行駛.圖中黑色直線為車道線，車道寬度約為0.3 m，黑色曲線為車輛換道軌跡.由圖10可知，向左換道后車輛基本處于車道中心線上并且車輛航向角幾乎保持不變.因此，良好的前輪轉(zhuǎn)角跟隨性能和穩(wěn)定的車速保持性能能夠保證較好的小車自動換道效果.另外，實際場景中，即便換道后小車位置或航向角與目標(biāo)值存在偏差，也可由小車車道保持功能予以彌補修正.

圖9 小車期望與實際前輪轉(zhuǎn)角對比

圖10 小車自動向左換道軌跡

4.3 AEB功能仿真分析

小車以0.65 m/s的速度，勻速在直路上行駛，當(dāng)檢測到與前方障礙物的距離小于兩者之間的最小安全距離時，AEB系統(tǒng)被觸發(fā)，首先是預(yù)警系統(tǒng)作用，提醒司機危險的存在，若司機并未采取措施，隨著危險的進一步加重，制動系統(tǒng)開始介入工作，見圖11.在3.3 s時刻開始，采取完全制動的方式，小車的速度由原來的0.65 m/s迅速下降到0，從而避免了碰撞的發(fā)生.其中虛線代表的是期望車速，實線代表的是實際的車速，采用PID控制，從圖中可以看出，實際車速與期望車速之間的偏差較小，較好地滿足了設(shè)計的要求.小車超聲波傳感器從檢測到目標(biāo)到發(fā)現(xiàn)危險存在，到最后制動停車，與前方障礙物之間的距離關(guān)系見圖12.

圖11 小車緊急制動車速曲線

圖12 小車緊急制動與障礙車距離曲線

5 結(jié) 束 語

以Mindstorms為平臺搭建了智能四輪移動機器人，為使其結(jié)構(gòu)更接近真車，使用了前驅(qū)差速前轉(zhuǎn)向機構(gòu).通過自帶的超聲波傳感器和外加的手機攝像頭，在Simulink中開發(fā)出智能車輛輔助系統(tǒng)控制算法，可實現(xiàn)基本的車道保持、自主換道和AEB等.由于該智能機器人與實際車輛存在一定的差距，故在智能輔助系統(tǒng)控制算法移植性還存在一定的問題，接下來要重點對Simulink被控對象模型進行驗證.

[1] ZHAN J. An automotive longitudinal dynamic model building for adaptive cruise control[J]. Journal of Jilin University (Engineering Science Edition),2006,36(2):157-160.

[2] 朱愿.基于視覺和雷達(dá)的智能車輛自主換道決策機制與控制研究[D].北京：中國人民解放軍軍事醫(yī)學(xué)科學(xué)院,2006.

[3] 王政.智能車輛自主換道方法的研究[D].長春:吉林大學(xué),2016.

[4] ZHU X C, LIU Z C, LI L. Design and implementation of vehicle tracking controller for unmanned vehicles[J]. Journal of Xi’an University of Technology,2015(1):55-59.

[5] 李霖,朱西產(chǎn),董小飛,等.自主緊急制動系統(tǒng)避撞策略的研究[J].汽車工程,2015,37(2):168-174.

[6] 呂章潔.汽車AEB仿真控制算法優(yōu)化及驗證[D].重慶:重慶理工大學(xué),2016.

[7] 李文娜.汽車主動防撞預(yù)警系統(tǒng)的安全策略研究[D]. 長春：吉林大學(xué),2016.

[8] 柯振宇.基于毫米波雷達(dá)的車輛縱向碰撞預(yù)警系統(tǒng)設(shè)計[D].武漢:武漢理工大學(xué),2014.