基于DSP和模糊控制的巡線機(jī)器人控制系統(tǒng)設(shè)計(jì)與實(shí)現(xiàn)

趙錦芝，楊 威

ZHAO Jin-zhi,YANG Wei

（中原工學(xué)院，鄭州 450007）

0 引言

鑒于AGV小車在現(xiàn)代物流與柔性制造系統(tǒng)中的重要應(yīng)用，設(shè)計(jì)一款巡線機(jī)器人控制系統(tǒng)將非常必要。因AGV小車是集各種機(jī)械結(jié)構(gòu)、傳感器和計(jì)算機(jī)等一體的非線性復(fù)雜系統(tǒng)，對(duì)其建立數(shù)學(xué)模型很困難，傳統(tǒng)的基于模型的控制方法很難適用該模型，而且也很難滿足AGV小車對(duì)控制器的實(shí)時(shí)性、魯棒性和精確性的要求[1]。所以本文綜合選用模糊邏輯控制與PID控制對(duì)巡線機(jī)器人進(jìn)行有效控制，模糊控制主要完成機(jī)器人對(duì)導(dǎo)引線的實(shí)時(shí)跟蹤，PID控制實(shí)現(xiàn)機(jī)器人速度的穩(wěn)定與快速調(diào)整。

1 巡線機(jī)器人結(jié)構(gòu)設(shè)計(jì)

本巡線機(jī)器人為3輪式移動(dòng)機(jī)器人，該機(jī)器人采用雙輪差動(dòng)加萬向輪控制，前面2個(gè)是驅(qū)動(dòng)輪，后輪為萬向輪。在機(jī)器人底部安裝兩排自制的間距可微調(diào)的紅外傳感器檢測(cè)板，用來檢測(cè)導(dǎo)引線。

2 巡線機(jī)器人控制系統(tǒng)設(shè)計(jì)

巡線機(jī)器人控制系統(tǒng)的微處理器選用TI公司C2000系列的TMS320LF2407A數(shù)字處理芯片，為了充分利用DSP芯片上的各功能端口，增加整個(gè)控制系統(tǒng)的功能多樣化，并沒有將24路巡線傳感器的采集直接接在DSP的I/O口上，而是使用4片74HC245芯片外擴(kuò)了32路巡線傳感器的采集端口，如圖1所示為系統(tǒng)硬件結(jié)構(gòu)框圖，該系統(tǒng)硬件上可以實(shí)現(xiàn)10路AD信號(hào)的采集，兩路繼電器控制（可控制制動(dòng)器完成驅(qū)動(dòng)輪的剎車），3路限位開關(guān)或接近開關(guān)的采集，32路巡線傳感器的采集，完成兩路步進(jìn)電機(jī)的開環(huán)控制或兩路伺服電機(jī)的閉環(huán)控制，并且對(duì)電機(jī)碼盤采集的差分信號(hào)進(jìn)行了濾波處理，增加了系統(tǒng)的抗干擾能力。在通訊方面，實(shí)現(xiàn)了高速的CAN總線通訊，與串口通訊兩種通訊方式。

圖1 系統(tǒng)硬件結(jié)構(gòu)框圖

使用自制的驅(qū)動(dòng)器完成對(duì)伺服電機(jī)的驅(qū)動(dòng)，該驅(qū)動(dòng)器選用具有溫度報(bào)警，過熱與短路保護(hù)功能的LMD18200芯片，該芯片的邏輯真值表如表1所示。對(duì)進(jìn)入驅(qū)動(dòng)器的控制信號(hào)（PWM，方向，剎車）進(jìn)行了光電隔離，確保該驅(qū)動(dòng)器能夠可靠的驅(qū)動(dòng)伺服電機(jī)。

圖2 控制系統(tǒng)原理圖

表1 邏輯真值表[2]

整個(gè)控制系統(tǒng)的工作原理如圖2所示。模糊控制器根據(jù)巡線傳感器判斷機(jī)器人的位姿，給出左右輪的速度偏差Vei，經(jīng)速度規(guī)劃后在保證給定機(jī)器人質(zhì)心速度不變的情況下，得到機(jī)器人左右輪的理想速度，再經(jīng)PID控制器調(diào)節(jié)實(shí)現(xiàn)機(jī)器人的速度閉環(huán)控制。

轉(zhuǎn)彎時(shí)巡線機(jī)器人從一個(gè)導(dǎo)引線過渡到另一個(gè)導(dǎo)引線，轉(zhuǎn)彎的好壞是考核巡線機(jī)器人的重要指標(biāo)。本控制系統(tǒng)采用原地打轉(zhuǎn)的轉(zhuǎn)彎策略，轉(zhuǎn)彎半徑為零，轉(zhuǎn)彎快速且易于實(shí)現(xiàn)。

2.1 模糊控制策略

為了實(shí)現(xiàn)機(jī)器人巡線移動(dòng)功能，根據(jù)采集巡線傳感器的狀態(tài)，來感知機(jī)器人偏離導(dǎo)引線的大小，進(jìn)而不斷地調(diào)整兩驅(qū)動(dòng)輪的速度，從而調(diào)整機(jī)器人的運(yùn)動(dòng)軌跡，使其沿導(dǎo)引線運(yùn)動(dòng)。根據(jù)以上原理，控制器可以選用模糊控制器[3,4]。

巡線機(jī)器人系統(tǒng)中，前后兩排傳感器的布局如圖3 所示，其中前排安裝兩個(gè)傳感器檢測(cè)板，后排安裝一個(gè)傳感器檢測(cè)板，且每個(gè)檢測(cè)板包含8路紅外傳感器探頭。每個(gè)探頭之間的間距D為10mm，機(jī)器人前后兩排檢測(cè)板之間的距離L為470mm。導(dǎo)引線的寬度至少要略大于每個(gè)探頭之間的間距D，保證任意時(shí)刻有1到2個(gè)傳感器在導(dǎo)引線上。為了增加機(jī)器人對(duì)導(dǎo)引線在中間位置的分辨率，前后排中間位置的兩個(gè)傳感器的距離為5mm。然后分別對(duì)前后兩排紅外傳感器的進(jìn)行編碼，具體編碼如表2所示。可以得到模糊控制器的傳感器檢測(cè)點(diǎn)的兩個(gè)輸入iF和iB編碼，一個(gè)輸出，機(jī)器人左右輪速度差Vei。

圖3 傳感器（檢測(cè)點(diǎn)）布局分布圖

模糊控制器輸入變量iF，iB的語(yǔ)言值選取為：{LB，LM，LS，Z0，RS，RM，RB}，其中，LB=左大、LM=左中、LS=左小、Z0=中心、RS=右小、RM=右中、RB=右大。輸出量Vei的選取為：{NVL，NL，NM，NS，Z0，PS，PM，PL，PVL}，其中，NVL= 負(fù)很大，NL= 負(fù)大，NM= 負(fù)中，NS= 負(fù)小，Z0= 零，PS= 正小，PM= 正中，PL= 正大，PVL= 正很大。

表2 傳感器（檢測(cè)點(diǎn)）編碼表

根據(jù)模糊隸屬函數(shù)的定義與規(guī)則，各語(yǔ)言變量在其論域上的隸屬函數(shù)如圖4所示，模糊控制規(guī)則的建立是基于專家知識(shí)或熟練操作人員的經(jīng)驗(yàn)，它是根據(jù)人的認(rèn)識(shí)能力推理出的一種語(yǔ)言表達(dá)形式，根據(jù)生活中汽車駕駛和智能車的調(diào)車經(jīng)驗(yàn)，建立如表3所示的控制規(guī)則。

圖4 輸入輸出語(yǔ)言變量的隸屬函數(shù)

表3 模糊控制規(guī)則表

當(dāng)機(jī)器人偏離導(dǎo)引線不大時(shí)，為了防止控制過度，出現(xiàn)機(jī)器人不斷來回調(diào)整的現(xiàn)象，控制規(guī)則中的調(diào)整幅度較小，如iF為Z0，iB為L(zhǎng)M輸出Vei為NS。當(dāng)機(jī)器人偏離導(dǎo)引線較大時(shí)，為了加快其調(diào)整速度，控制規(guī)則中的調(diào)整幅度較大，如iF為L(zhǎng)B，iB為Z0輸出Vei為PVL。模糊推理采用Mamdani法，采用重心法進(jìn)行反模糊，雖然計(jì)算量比較大，但比較準(zhǔn)確。如圖5所示為反解模糊后得到的輸入量iF，iB與輸出量的Vei對(duì)應(yīng)的關(guān)系曲線圖。

圖5 輸入iF，iB與輸出Vei對(duì)應(yīng)的關(guān)系曲線圖

模糊控制器得到的左右輪速度偏差Vei的區(qū)域?yàn)椋?10～10），需要尺度變換為實(shí)際的速度偏差值才可以使用，該變換可以是線性的也可以是非線性的。經(jīng)試驗(yàn)測(cè)試當(dāng)前排最左邊和最右邊4個(gè)傳感器檢測(cè)到導(dǎo)引線時(shí)所乘的比例系數(shù)Ku為40，其余情況比例系數(shù)Ku為35，此時(shí)，巡線機(jī)器人有比較好的控制狀態(tài)。為了保證機(jī)器人移動(dòng)的平穩(wěn)性與調(diào)整的快速性，且各種情況下保證機(jī)器人按照給定速度運(yùn)行，打破傳統(tǒng)的一個(gè)驅(qū)動(dòng)輪速度固定，令一個(gè)驅(qū)動(dòng)輪單獨(dú)調(diào)整的控制方法，選擇兩個(gè)驅(qū)動(dòng)輪同時(shí)調(diào)整的方法，其左右輪的理想轉(zhuǎn)速如公式（1）所示：

其中：robot_v為機(jī)器人質(zhì)心速度；

VL為機(jī)器人左輪速度；

VR為機(jī)器人右輪速度；

2.2 轉(zhuǎn)彎的實(shí)現(xiàn)

巡線機(jī)器人在遍歷整個(gè)倉(cāng)庫(kù)時(shí)，不僅需要準(zhǔn)確平穩(wěn)地巡直線運(yùn)動(dòng)，而且轉(zhuǎn)彎運(yùn)動(dòng)同樣重要。考慮到倉(cāng)庫(kù)中路徑結(jié)構(gòu)復(fù)雜，空間狹小，所以本系統(tǒng)采用原地打轉(zhuǎn)的轉(zhuǎn)彎策論，該策略具有轉(zhuǎn)彎半徑為零且易于實(shí)現(xiàn)的優(yōu)點(diǎn)。有機(jī)器人的轉(zhuǎn)彎半徑公式（2）可知，當(dāng)機(jī)器人左輪速度與右輪速度大小相等方向相反時(shí)，可實(shí)現(xiàn)機(jī)器人原地打轉(zhuǎn)。具體需要左轉(zhuǎn)彎還是右轉(zhuǎn)彎依靠前排邊緣的兩個(gè)紅外傳感器檢測(cè)轉(zhuǎn)彎標(biāo)記來判斷。即使機(jī)器人由于速度過快、導(dǎo)引線間斷或其他原因沖出導(dǎo)引線，機(jī)器人也會(huì)依靠記錄的上一次的狀態(tài)進(jìn)行原地打轉(zhuǎn)尋找導(dǎo)引線，一定程度上增加了系統(tǒng)的魯棒性。

其中：R為移動(dòng)機(jī)器人的轉(zhuǎn)彎半徑；

b為兩驅(qū)動(dòng)輪輪間距的一半；

VL為機(jī)器人左輪速度；

VR為機(jī)器人右輪速度。

3 實(shí)驗(yàn)與結(jié)果分析

為了使系統(tǒng)兼有穩(wěn)定啟動(dòng)、快速調(diào)節(jié)、較小的超調(diào)量等特性，經(jīng)試驗(yàn)不斷調(diào)整后的PID控制的參數(shù)為。DSP對(duì)巡線傳感器的采集由中心向兩邊同步進(jìn)行，實(shí)驗(yàn)表明，機(jī)器人在導(dǎo)引線線寬為12-55mm之間都能很好地完成巡線任務(wù)，增加了巡線機(jī)器人的魯棒性,但最佳工作線寬為15mm。為了保證整個(gè)控制系統(tǒng)的實(shí)時(shí)性，主程序完成一次巡線傳感器的采集之后，模糊控制器根據(jù)傳感器采集的機(jī)器人位姿得出理想的左右輪速度，緊接著PID控制器對(duì)速度進(jìn)行調(diào)節(jié)，進(jìn)而機(jī)器人又進(jìn)入下一次巡線傳感器的采集。經(jīng)過DSP仿真可知，一個(gè)循環(huán)周期僅需要3ms左右，由此可見本文設(shè)計(jì)的巡線系統(tǒng)具有很高的實(shí)時(shí)性。

通過串口每隔50ms將下位機(jī)（DSP）采集到的紅外巡線傳感器的狀態(tài)和速度返回到上位機(jī)，如圖6所示為VC上位機(jī)人機(jī)界面，通過該界面可以實(shí)時(shí)清楚地看到機(jī)器人偏離導(dǎo)引線的程度與模糊控制器輸出的左右輪的速度。實(shí)驗(yàn)表明，機(jī)器人可以順利通過圖7包括的路徑類型，且直線行走，巡線機(jī)器人的速度在500mm/s時(shí)，偏差在10mm以內(nèi)，速度在250mm/s時(shí)，偏差在5mm以內(nèi)，彎道行駛速度在200-500mm/s時(shí)，隨著速度的增加偏差不斷增大，但均在15mm以內(nèi)且運(yùn)動(dòng)比較平穩(wěn)。

圖6 VC上位機(jī)人機(jī)界面

圖7 路徑類型

4 結(jié)束語(yǔ)

本文開發(fā)了一種綜合模糊控制和PID控制的巡線機(jī)器人控制系統(tǒng)，實(shí)驗(yàn)證明，基本滿足了AGV小車對(duì)控制系統(tǒng)實(shí)時(shí)性、魯棒性、準(zhǔn)確性的要求，可以平穩(wěn)快速地通過直線通道，直角通道圓弧通道，該巡線機(jī)器人在常規(guī)的生產(chǎn)中能滿足使用要求。

[1] 廖華麗,周祥,董豐,等.基于模糊控制的AGV尋跡算法[J].哈爾濱工業(yè)大學(xué)學(xué)報(bào),2005,37(07):896-898.

[2] LMD18200 3A,55VH-Bridge Datasheet.National Semiconductor Corporation.1999.

[3] 韓寧寧,梁清,馬海濤,等.一種巡線機(jī)器人控制系統(tǒng)的設(shè)計(jì)與實(shí)現(xiàn)[J].自動(dòng)化與儀表,2008,09:39-43.

[4] 李國(guó)勇.智能控制及MATLAB實(shí)現(xiàn)[M].北京:電子工業(yè)出版社,2005.