全方位移動平臺運動控制系統的實現

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

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

0 引言

全方位移動平臺由于其出色的靈活性，在服務機器人的設計中成為最佳的選擇。而全方位移動平臺運動控制的性能又是機器人穩定性的主要影響因素，因此機器人底層運動控制也成為機器人研究中的熱點之一[1-10]。本文研究了一種基于FPGA的全方位移動平臺運動控制的方法。根據實際機器人底層調試的需求增加了無線控制、速度顯示等模塊，并對移動平臺進行避障的開發，減輕上位機的運算負載。為了方便后續工作，該移動平臺建立了獨立運行于個人PC的上位機，從而形成完整的運動控制系統。系統控制的優先級自下而上逐層遞減，保證了系統能夠穩定運行。

1 系統總體設計

全方位運動平臺的總體結構如圖1所示。首先是上位機通過無線藍牙模塊發送對核心控制模塊的控制指令，再由匹配的另一個藍牙模塊進行接受，傳送至核心控制模塊處理，在核心控制模塊將指令進行解析分配，然后以PWM形式送至驅動模塊，驅動電機運轉。在控制平臺進行移動操作時，超聲波測距模塊將實時測距數據傳送至核心控制模塊，在核心控制模塊對超聲波測距信息進行分析，達到避障的目的。液晶顯示模塊進行間斷刷新，用以顯示測速模塊實時采集的輪子速度和核心控制模塊下發的理論速度。

圖1 系統總體硬件模塊

FPGA作為整個系統的核心控制模塊，主要實現對上位機信號接收、超聲波測距信號處理、測速信號的采集以及對驅動模塊下發驅動指令、LCD液晶顯示模塊間斷刷新指令。FPGA從上位機接受到指令后經過指令解析，將指令進行分配，即分析指令內容再由FPGA內部分配至各硬件電路[11]。而后將數據傳輸至各個模塊。

在FPGA分配解析得到機器人坐標系下的速度后，將機器人坐標系下的速度值轉化成機器人的3個全向輪的角速度，將得到的角速度值計算出相應的占空比，生成相應占空比的PWM波形，輸出信號接到直流伺服電機驅動器[2]，然后通過測速模塊，計算出輪子實際的速度值，在LCD液晶顯示模塊實時顯示速度。

2 運動控制算法及實現

2.1 全方位移動平臺的運動模型

運動平臺在平面上的位置如圖2所示，世界坐標系為xoy，平臺坐標系為XOY，點O為在運動平臺上的參考點，平臺的位置和姿態用ξ=[x,y,θ]T表示，其中(x,y)為點O在平面直角坐標系中的位置，θ為世界坐標系下x軸到機器人坐標系X軸的角度，順時針為反方向[3]。

圖2 平臺坐標系

假設在平臺坐標中平臺的移動速度為[VxVyφ]。其中Vx、Vy分別為平臺移動速度V在平臺坐標系下的X軸與Y軸的分量，φ為運動平臺自轉角速度。則存在：

(1)

平臺移動可分解為軸向可控的主動輪移動和徑向不可控的從動輪移動，運動平臺通過調節主動輪速度來調節車體的速度。如圖3所示，以XOY為平臺坐標系，建立在車體上布置三個全向輪時主動輪和從動滾子的運動學模型[2]。

圖3 運動學模型圖

(2)

根據三輪速度之間的關系可得全方位移動平臺的主動輪在平面世界坐標系中的速度要求：

(3)

根據這一速度要求進而可以對電機發出相應的控制信號。

在實際情況中，全方位移動平臺有直線運動、原地自轉、圓弧運動這幾種情況，其中圓弧運動又包含平動圓弧和轉動圓弧。對于不同的情況進行以下分析。

(1)全向方位移動平臺沿任意方作直線移動時，φ=0。

(4)

當Vx=0、Vy≠0時，平臺作前后直線移動；

當Vx≠0、Vy=0時，平臺作左右直線移動；

當Vx≠0、Vy≠0時，平臺作斜向直線移動。

(2)移動平臺繞自身中心自轉時，Vx=Vy=0，φ≠0，這時：

V1l=V2l=V3l=Lφ

(5)

(3)圓弧運動：

①平動圓弧：φ=0，運動平臺憑借Vx、Vy改變其運動方向，從而實現圓弧運動。

(6)

圖4 運動平臺的圓弧運動軌跡

(7)

2.2 全方位移動平臺運動算法實現

在得到全方位移動平臺的算法后，系統要進行運動控制的矢量分解，其作用是將平臺坐標系下的速度轉化為三個全向輪的速度。根據系統平臺實際的機械結構，運動平臺矢量分解采用下式實現：

(8)

由于單個驅動器的最大速度歸一化擴大百倍后為100整，所以平臺坐標系中Vx和Vy的速度范圍通過MATLAB分析得到如圖5所示的平臺坐標系速度范圍圖。

圖5 平臺坐標系Vx和Vy的速度范圍圖

從圖5中可以看出X和Y的方向會限制在第四幅圖中的不規則八邊形內。八邊形的邊為X和Y綜合速度極限。如果在限制范圍外，運動平臺會出現速度飽和的情況。

根據運動算法和MATLAB仿真分析的結果，在FPGA中產生相應的PWM波進行全方位移動平臺的運動控制。其仿真結果如圖6所示。

圖6 速度控制仿真圖

3 各硬件模塊的實現

3.1 藍牙通信模塊

為了方便對全方位移動平臺在行進中發送控制指令，平臺采用藍牙通信。由于對傳輸速率的要求較低，試驗階段傳輸距離在10 m以內即可，平臺涉及的藍牙模塊型號為HC05。該模塊有命令響應、工作連接兩種工作模式。在對藍牙模塊設置時，將藍牙模塊置為命令響應模式，將兩個藍牙模塊的串口波特率設置為9 600 b/s，然后將工作模式設置一個為主一個為從[5]。藍牙開啟后會自動配對，配對成功之后便可以進行數據的接收和發送。

3.2 超聲波避障模塊

超聲波模塊的原理是通過發送多組等時長的超聲波，然后檢測聲波回響的時間長短，根據聲波回響時間的長短測定聲波源和障礙物之間的距離。超聲波模塊有兩個端口，分別是觸發端和回響端。FPGA通過向觸發端發送一個時長為10 ms的高電平開啟超聲波模塊的發送器，然后接收器會在回響端產生一個根據實際距離變化的高電平。FPGA通過測量回響端接收的高電平時長來計算超聲波模塊與被測物之間的距離[6-7]。

超聲波模塊在FPGA中控制的RTL級電路如圖7所示。U1產生10 ms高電平的觸發信號Trig_Sig，U2將接收到的回響信號Echo_Sig經過計時得出實際距離。通過并行線Dis_Num傳送至其他模塊。根據實際需求，只需測量距離在1 m以內，所以Dis_Num的位寬為7。

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

3.3 LCD顯示模塊

液晶顯示在全方位移動運動平臺中的功能是為了實時顯示超聲波測距數值以及三個輪子的指定轉速。該顯示模塊采用無字庫的驅動芯片，所以作為顯示只需要其繪圖功能即可。

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

圖8 SPI總線液晶時序圖

3.4 驅動模塊與測速模塊

平臺采用的驅動器是型號為MLDS3610的直流伺服驅動器。以PWM信號速度控制的方式進行驅動。在FPGA中設計PWM波形輸出模塊，模塊采用500 Hz的頻率。PWM的占空比范圍為1%～99%。而在此范圍內，線性度能夠保證在0.1%以內。在驅動器中設定占空比=50%時，電機速度V=0；占空比<50%時，電機反轉；占空比>50%時，電機正轉。從而達到調節速度和方向的目的。

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

4 系統評述

與兩輪差速驅動的底盤結構相比，全向輪結構的移動底盤和兩輪差速驅動的底盤有很大的不同。全向輪結構具有很大的靈活性，在狹小的空間里能夠實現零轉彎半徑的操作，防止后期的上位單目攝像頭結構在底盤轉彎時丟失跟蹤的目標。

與文獻[2]的系統相比，文獻[2]中FPGA核心板僅進行了運動控制的處理，板子資源稍有浪費。本文中的FPGA不僅實現了運動控制算法，還增加了速度的實時顯示、超聲波避障、藍牙通信等功能。在下位機部分就能夠實現避障的功能，有利于降低上位機計算量的開銷。同時，下位機使用藍牙通信使得下位機自成一系統，即使在行進中也可以對底盤的一些運動參數進行修改。

5 結論

針對服務機器人底層運動控制系統，設計了一種基于FPGA的全方位移動平臺。在實際測試中發現這種采用FPGA的方案具有很好的實時性，速度控制精確度高，而且由于FPGA優越的處理能力和多引腳的特性，平臺的可擴展性較強，如后期配置Kinect傳感器進行圖像處理，則可實現機器人跟隨的目的。同時該平臺為多電機的機器人運動控制系統提供了具體的實現方案，具有一定的參考價值。

[1] 趙偉雄. 全方位移動機器人控制系統研究與實現[D].天津：河北工程大學,2016.

[2] 王全州,裴東,陶中幸,等. 中型組機器人運動控制系統的FPGA設計[J]. 現代電子技術,2010, 33(14):127-130.

[3] 陳汝佳. 輪式人形家庭服務機器人設計與實現[D].成都：電子科技大學,2014.

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

[5] 郭棟.藍牙HCI-UART主控制接口的FPGA設計與實現[J]. 微計算機信息,2010, 26(11):140-141，144.

[6] 吳超. 基于FPGA的超聲波測距系統[D].武漢：武漢理工大學,2009.

[7] 劉明,肖文健,齊曉慧.超聲避障技術在輪式機器人導航中的應用研究[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的家庭服務機器人系統設計[J]. 電子產品世界,2015, 22(9):55-57.

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

[11] 王先勝.基于FPGA的UART電路的設計與應用[D].西安:西安電子科技大學,2014.