曾令棟，裴 東，2，王全州，高文輝

(1.西北師范大學(xué) 物理與電子工程學(xué)院，甘肅 蘭州 730070； 2.甘肅省智能信息技術(shù)與應(yīng)用工程研究中心,甘肅 蘭州 730070)

0 引言

全方位移動(dòng)平臺(tái)由于其出色的靈活性，在服務(wù)機(jī)器人的設(shè)計(jì)中成為最佳的選擇。而全方位移動(dòng)平臺(tái)運(yùn)動(dòng)控制的性能又是機(jī)器人穩(wěn)定性的主要影響因素，因此機(jī)器人底層運(yùn)動(dòng)控制也成為機(jī)器人研究中的熱點(diǎn)之一[1-10]。本文研究了一種基于FPGA的全方位移動(dòng)平臺(tái)運(yùn)動(dòng)控制的方法。根據(jù)實(shí)際機(jī)器人底層調(diào)試的需求增加了無(wú)線控制、速度顯示等模塊，并對(duì)移動(dòng)平臺(tái)進(jìn)行避障的開(kāi)發(fā)，減輕上位機(jī)的運(yùn)算負(fù)載。為了方便后續(xù)工作，該移動(dòng)平臺(tái)建立了獨(dú)立運(yùn)行于個(gè)人PC的上位機(jī)，從而形成完整的運(yùn)動(dòng)控制系統(tǒng)。系統(tǒng)控制的優(yōu)先級(jí)自下而上逐層遞減，保證了系統(tǒng)能夠穩(wěn)定運(yùn)行。

1 系統(tǒng)總體設(shè)計(jì)

全方位運(yùn)動(dòng)平臺(tái)的總體結(jié)構(gòu)如圖1所示。首先是上位機(jī)通過(guò)無(wú)線藍(lán)牙模塊發(fā)送對(duì)核心控制模塊的控制指令，再由匹配的另一個(gè)藍(lán)牙模塊進(jìn)行接受，傳送至核心控制模塊處理，在核心控制模塊將指令進(jìn)行解析分配，然后以PWM形式送至驅(qū)動(dòng)模塊，驅(qū)動(dòng)電機(jī)運(yùn)轉(zhuǎn)。在控制平臺(tái)進(jìn)行移動(dòng)操作時(shí)，超聲波測(cè)距模塊將實(shí)時(shí)測(cè)距數(shù)據(jù)傳送至核心控制模塊，在核心控制模塊對(duì)超聲波測(cè)距信息進(jìn)行分析，達(dá)到避障的目的。液晶顯示模塊進(jìn)行間斷刷新，用以顯示測(cè)速模塊實(shí)時(shí)采集的輪子速度和核心控制模塊下發(fā)的理論速度。

圖1 系統(tǒng)總體硬件模塊

FPGA作為整個(gè)系統(tǒng)的核心控制模塊，主要實(shí)現(xiàn)對(duì)上位機(jī)信號(hào)接收、超聲波測(cè)距信號(hào)處理、測(cè)速信號(hào)的采集以及對(duì)驅(qū)動(dòng)模塊下發(fā)驅(qū)動(dòng)指令、LCD液晶顯示模塊間斷刷新指令。FPGA從上位機(jī)接受到指令后經(jīng)過(guò)指令解析，將指令進(jìn)行分配，即分析指令內(nèi)容再由FPGA內(nèi)部分配至各硬件電路[11]。而后將數(shù)據(jù)傳輸至各個(gè)模塊。

在FPGA分配解析得到機(jī)器人坐標(biāo)系下的速度后，將機(jī)器人坐標(biāo)系下的速度值轉(zhuǎn)化成機(jī)器人的3個(gè)全向輪的角速度，將得到的角速度值計(jì)算出相應(yīng)的占空比，生成相應(yīng)占空比的PWM波形，輸出信號(hào)接到直流伺服電機(jī)驅(qū)動(dòng)器[2]，然后通過(guò)測(cè)速模塊，計(jì)算出輪子實(shí)際的速度值，在LCD液晶顯示模塊實(shí)時(shí)顯示速度。

2 運(yùn)動(dòng)控制算法及實(shí)現(xiàn)

2.1 全方位移動(dòng)平臺(tái)的運(yùn)動(dòng)模型

運(yùn)動(dòng)平臺(tái)在平面上的位置如圖2所示，世界坐標(biāo)系為xoy，平臺(tái)坐標(biāo)系為XOY，點(diǎn)O為在運(yùn)動(dòng)平臺(tái)上的參考點(diǎn)，平臺(tái)的位置和姿態(tài)用ξ=[x,y,θ]T表示，其中(x,y)為點(diǎn)O在平面直角坐標(biāo)系中的位置，θ為世界坐標(biāo)系下x軸到機(jī)器人坐標(biāo)系X軸的角度，順時(shí)針為反方向[3]。

圖2 平臺(tái)坐標(biāo)系

假設(shè)在平臺(tái)坐標(biāo)中平臺(tái)的移動(dòng)速度為[VxVyφ]。其中Vx、Vy分別為平臺(tái)移動(dòng)速度V在平臺(tái)坐標(biāo)系下的X軸與Y軸的分量，φ為運(yùn)動(dòng)平臺(tái)自轉(zhuǎn)角速度。則存在：

(1)

平臺(tái)移動(dòng)可分解為軸向可控的主動(dòng)輪移動(dòng)和徑向不可控的從動(dòng)輪移動(dòng)，運(yùn)動(dòng)平臺(tái)通過(guò)調(diào)節(jié)主動(dòng)輪速度來(lái)調(diào)節(jié)車(chē)體的速度。如圖3所示，以XOY為平臺(tái)坐標(biāo)系，建立在車(chē)體上布置三個(gè)全向輪時(shí)主動(dòng)輪和從動(dòng)滾子的運(yùn)動(dòng)學(xué)模型[2]。

圖3 運(yùn)動(dòng)學(xué)模型圖

(2)

根據(jù)三輪速度之間的關(guān)系可得全方位移動(dòng)平臺(tái)的主動(dòng)輪在平面世界坐標(biāo)系中的速度要求：

(3)

根據(jù)這一速度要求進(jìn)而可以對(duì)電機(jī)發(fā)出相應(yīng)的控制信號(hào)。

在實(shí)際情況中，全方位移動(dòng)平臺(tái)有直線運(yùn)動(dòng)、原地自轉(zhuǎn)、圓弧運(yùn)動(dòng)這幾種情況，其中圓弧運(yùn)動(dòng)又包含平動(dòng)圓弧和轉(zhuǎn)動(dòng)圓弧。對(duì)于不同的情況進(jìn)行以下分析。

(1)全向方位移動(dòng)平臺(tái)沿任意方作直線移動(dòng)時(shí)，φ=0。

(4)

當(dāng)Vx=0、Vy≠0時(shí)，平臺(tái)作前后直線移動(dòng)；

當(dāng)Vx≠0、Vy=0時(shí)，平臺(tái)作左右直線移動(dòng)；

當(dāng)Vx≠0、Vy≠0時(shí)，平臺(tái)作斜向直線移動(dòng)。

(2)移動(dòng)平臺(tái)繞自身中心自轉(zhuǎn)時(shí)，Vx=Vy=0，φ≠0，這時(shí)：

V1l=V2l=V3l=Lφ

(5)

(3)圓弧運(yùn)動(dòng)：

①平動(dòng)圓弧：φ=0，運(yùn)動(dòng)平臺(tái)憑借Vx、Vy改變其運(yùn)動(dòng)方向，從而實(shí)現(xiàn)圓弧運(yùn)動(dòng)。

(6)

圖4 運(yùn)動(dòng)平臺(tái)的圓弧運(yùn)動(dòng)軌跡

(7)

2.2 全方位移動(dòng)平臺(tái)運(yùn)動(dòng)算法實(shí)現(xiàn)

在得到全方位移動(dòng)平臺(tái)的算法后，系統(tǒng)要進(jìn)行運(yùn)動(dòng)控制的矢量分解，其作用是將平臺(tái)坐標(biāo)系下的速度轉(zhuǎn)化為三個(gè)全向輪的速度。根據(jù)系統(tǒng)平臺(tái)實(shí)際的機(jī)械結(jié)構(gòu)，運(yùn)動(dòng)平臺(tái)矢量分解采用下式實(shí)現(xiàn)：

(8)

由于單個(gè)驅(qū)動(dòng)器的最大速度歸一化擴(kuò)大百倍后為100整，所以平臺(tái)坐標(biāo)系中Vx和Vy的速度范圍通過(guò)MATLAB分析得到如圖5所示的平臺(tái)坐標(biāo)系速度范圍圖。

圖5 平臺(tái)坐標(biāo)系Vx和Vy的速度范圍圖

從圖5中可以看出X和Y的方向會(huì)限制在第四幅圖中的不規(guī)則八邊形內(nèi)。八邊形的邊為X和Y綜合速度極限。如果在限制范圍外，運(yùn)動(dòng)平臺(tái)會(huì)出現(xiàn)速度飽和的情況。

根據(jù)運(yùn)動(dòng)算法和MATLAB仿真分析的結(jié)果，在FPGA中產(chǎn)生相應(yīng)的PWM波進(jìn)行全方位移動(dòng)平臺(tái)的運(yùn)動(dòng)控制。其仿真結(jié)果如圖6所示。

圖6 速度控制仿真圖

3 各硬件模塊的實(shí)現(xiàn)

3.1 藍(lán)牙通信模塊

為了方便對(duì)全方位移動(dòng)平臺(tái)在行進(jìn)中發(fā)送控制指令，平臺(tái)采用藍(lán)牙通信。由于對(duì)傳輸速率的要求較低，試驗(yàn)階段傳輸距離在10 m以內(nèi)即可，平臺(tái)涉及的藍(lán)牙模塊型號(hào)為HC05。該模塊有命令響應(yīng)、工作連接兩種工作模式。在對(duì)藍(lán)牙模塊設(shè)置時(shí)，將藍(lán)牙模塊置為命令響應(yīng)模式，將兩個(gè)藍(lán)牙模塊的串口波特率設(shè)置為9 600 b/s，然后將工作模式設(shè)置一個(gè)為主一個(gè)為從[5]。藍(lán)牙開(kāi)啟后會(huì)自動(dòng)配對(duì)，配對(duì)成功之后便可以進(jìn)行數(shù)據(jù)的接收和發(fā)送。

3.2 超聲波避障模塊

超聲波模塊的原理是通過(guò)發(fā)送多組等時(shí)長(zhǎng)的超聲波，然后檢測(cè)聲波回響的時(shí)間長(zhǎng)短，根據(jù)聲波回響時(shí)間的長(zhǎng)短測(cè)定聲波源和障礙物之間的距離。超聲波模塊有兩個(gè)端口，分別是觸發(fā)端和回響端。FPGA通過(guò)向觸發(fā)端發(fā)送一個(gè)時(shí)長(zhǎng)為10 ms的高電平開(kāi)啟超聲波模塊的發(fā)送器，然后接收器會(huì)在回響端產(chǎn)生一個(gè)根據(jù)實(shí)際距離變化的高電平。FPGA通過(guò)測(cè)量回響端接收的高電平時(shí)長(zhǎng)來(lái)計(jì)算超聲波模塊與被測(cè)物之間的距離[6-7]。

超聲波模塊在FPGA中控制的RTL級(jí)電路如圖7所示。U1產(chǎn)生10 ms高電平的觸發(fā)信號(hào)Trig_Sig，U2將接收到的回響信號(hào)Echo_Sig經(jīng)過(guò)計(jì)時(shí)得出實(shí)際距離。通過(guò)并行線Dis_Num傳送至其他模塊。根據(jù)實(shí)際需求，只需測(cè)量距離在1 m以內(nèi)，所以Dis_Num的位寬為7。

圖7 超聲波測(cè)距模塊控制電路(RTL級(jí))

3.3 LCD顯示模塊

液晶顯示在全方位移動(dòng)運(yùn)動(dòng)平臺(tái)中的功能是為了實(shí)時(shí)顯示超聲波測(cè)距數(shù)值以及三個(gè)輪子的指定轉(zhuǎn)速。該顯示模塊采用無(wú)字庫(kù)的驅(qū)動(dòng)芯片，所以作為顯示只需要其繪圖功能即可。

液晶屏的輸入模式采用串行傳輸。通過(guò)SPI總線協(xié)議將數(shù)據(jù)發(fā)送至液晶屏顯示。SPI是一種高速全雙工同步的通信總線，共4線，分別是CS，A0，SCL和SDI。由于SPI總線有主機(jī)和從機(jī)之分，因此在平臺(tái)中，液晶顯示為從機(jī)而FPGA為主機(jī)。FPGA將需要顯示的數(shù)據(jù)通過(guò)SPI總線傳送至液晶顯示，SI、SCL、CS都是主機(jī)輸出、從機(jī)輸入。關(guān)于SPI總線的液晶顯示時(shí)序圖如圖8所示。

圖8 SPI總線液晶時(shí)序圖

3.4 驅(qū)動(dòng)模塊與測(cè)速模塊

平臺(tái)采用的驅(qū)動(dòng)器是型號(hào)為MLDS3610的直流伺服驅(qū)動(dòng)器。以PWM信號(hào)速度控制的方式進(jìn)行驅(qū)動(dòng)。在FPGA中設(shè)計(jì)PWM波形輸出模塊，模塊采用500 Hz的頻率。PWM的占空比范圍為1%～99%。而在此范圍內(nèi)，線性度能夠保證在0.1%以內(nèi)。在驅(qū)動(dòng)器中設(shè)定占空比=50%時(shí)，電機(jī)速度V=0；占空比<50%時(shí)，電機(jī)反轉(zhuǎn)；占空比>50%時(shí)，電機(jī)正轉(zhuǎn)。從而達(dá)到調(diào)節(jié)速度和方向的目的。

FPGA除了要對(duì)驅(qū)動(dòng)器指定需要的速度外，還需要一個(gè)實(shí)時(shí)的速度反饋來(lái)達(dá)到閉環(huán)控制的效果。常見(jiàn)的測(cè)速算法有四種：M法測(cè)速，T法測(cè)速，M/T法測(cè)速以及變M/T法測(cè)速。通過(guò)對(duì)運(yùn)動(dòng)平臺(tái)電機(jī)的實(shí)際測(cè)試，其速度變化為1～9 600 r/m；速度為1時(shí)實(shí)測(cè)編碼器輸出方波頻率為8.5 Hz左右。速度為9 600 r/m時(shí)實(shí)測(cè)編碼器輸出測(cè)方波頻率為82 000 Hz左右。本文中將速度歸一化處理后擴(kuò)大一百倍，速度就為0～100 r/m之間，而此時(shí)速度為1 r/m時(shí)，編碼器輸出大約816 Hz左右。所以對(duì)編碼器測(cè)速范圍應(yīng)該在800～82 500 Hz之間。測(cè)速結(jié)果將結(jié)果統(tǒng)一在0～100 r/m之間，結(jié)果每增加1 r/m實(shí)際速度增加96 r/m。所以M法測(cè)速能夠滿足平臺(tái)的測(cè)速需求[8]。

4 系統(tǒng)評(píng)述

與兩輪差速驅(qū)動(dòng)的底盤(pán)結(jié)構(gòu)相比，全向輪結(jié)構(gòu)的移動(dòng)底盤(pán)和兩輪差速驅(qū)動(dòng)的底盤(pán)有很大的不同。全向輪結(jié)構(gòu)具有很大的靈活性，在狹小的空間里能夠?qū)崿F(xiàn)零轉(zhuǎn)彎半徑的操作，防止后期的上位單目攝像頭結(jié)構(gòu)在底盤(pán)轉(zhuǎn)彎時(shí)丟失跟蹤的目標(biāo)。

與文獻(xiàn)[2]的系統(tǒng)相比，文獻(xiàn)[2]中FPGA核心板僅進(jìn)行了運(yùn)動(dòng)控制的處理，板子資源稍有浪費(fèi)。本文中的FPGA不僅實(shí)現(xiàn)了運(yùn)動(dòng)控制算法，還增加了速度的實(shí)時(shí)顯示、超聲波避障、藍(lán)牙通信等功能。在下位機(jī)部分就能夠?qū)崿F(xiàn)避障的功能，有利于降低上位機(jī)計(jì)算量的開(kāi)銷。同時(shí)，下位機(jī)使用藍(lán)牙通信使得下位機(jī)自成一系統(tǒng)，即使在行進(jìn)中也可以對(duì)底盤(pán)的一些運(yùn)動(dòng)參數(shù)進(jìn)行修改。

5 結(jié)論

針對(duì)服務(wù)機(jī)器人底層運(yùn)動(dòng)控制系統(tǒng)，設(shè)計(jì)了一種基于FPGA的全方位移動(dòng)平臺(tái)。在實(shí)際測(cè)試中發(fā)現(xiàn)這種采用FPGA的方案具有很好的實(shí)時(shí)性，速度控制精確度高，而且由于FPGA優(yōu)越的處理能力和多引腳的特性，平臺(tái)的可擴(kuò)展性較強(qiáng)，如后期配置Kinect傳感器進(jìn)行圖像處理，則可實(shí)現(xiàn)機(jī)器人跟隨的目的。同時(shí)該平臺(tái)為多電機(jī)的機(jī)器人運(yùn)動(dòng)控制系統(tǒng)提供了具體的實(shí)現(xiàn)方案，具有一定的參考價(jià)值。

[1] 趙偉雄. 全方位移動(dòng)機(jī)器人控制系統(tǒng)研究與實(shí)現(xiàn)[D].天津：河北工程大學(xué),2016.

[2] 王全州,裴東,陶中幸,等. 中型組機(jī)器人運(yùn)動(dòng)控制系統(tǒng)的FPGA設(shè)計(jì)[J]. 現(xiàn)代電子技術(shù),2010, 33(14):127-130.

[3] 陳汝佳. 輪式人形家庭服務(wù)機(jī)器人設(shè)計(jì)與實(shí)現(xiàn)[D].成都：電子科技大學(xué),2014.

[4] 梁延德,于華玉,何福本. 全方位小型足球機(jī)器人運(yùn)動(dòng)特性分析[J]. 組合機(jī)床與自動(dòng)化加工技術(shù),2008(4):5-8.

[5] 郭棟.藍(lán)牙HCI-UART主控制接口的FPGA設(shè)計(jì)與實(shí)現(xiàn)[J]. 微計(jì)算機(jī)信息,2010, 26(11):140-141，144.

[6] 吳超. 基于FPGA的超聲波測(cè)距系統(tǒng)[D].武漢：武漢理工大學(xué),2009.

[7] 劉明,肖文健,齊曉慧.超聲避障技術(shù)在輪式機(jī)器人導(dǎo)航中的應(yīng)用研究[J].科技信息,2012(22):129-130.

[8] CHARITKHUAN C, BHURIPANYO J, CHOOMUANG R. FPGA implementation of closed-loop control system for small-sized RoboCup[C]//2006 IEEE Conference on Robotics, Automation and Mechatronics, IEEE, 2006:1-6.

[9] 胡振旺,陳益民,李林. 基于STM32的家庭服務(wù)機(jī)器人系統(tǒng)設(shè)計(jì)[J]. 電子產(chǎn)品世界,2015, 22(9):55-57.

[10] 王少君,劉永強(qiáng),楊紹普,等. 基于光電編碼器的測(cè)速方法研究及實(shí)驗(yàn)驗(yàn)證[J]. 自動(dòng)化與儀表,2015, 30(6):68-72.

[11] 王先勝.基于FPGA的UART電路的設(shè)計(jì)與應(yīng)用[D].西安:西安電子科技大學(xué),2014.