具有慣性傳感裝置的四輪全向驅動小車設計

賴海清， 張 祺， 林凱偉， 林耀燦， 吳海生

（廣東工業大學 自動化學院，廣州510006）

近年來四輪全向驅動小車在機器人領域的應用很廣泛，其優點是可以向360°任一方向運動。但一般控制方式沒有考慮小車自身的慣性，且4個電動機的協調控制比較困難［1］，導致小車運動性能較差，特別是高速運動時軌跡跟蹤較為困難。因此，本文設計一種具有慣性傳感裝置的四輪全向驅動小車，以提高小車高速運動時軌跡跟蹤的準確性［2］。

小車主要由運動控制系統慣性傳感裝置組成。運動控制系統通過發出命令控制4個輪子的轉速，并同時采集來自慣性傳感裝置的數據進行反饋控制。

1 全向驅動小車機械結構

小車的總體結構從上往下分為2層：下層如圖1所示，為小車車架，上層為電路板。4組全向驅動輪各成90°分布在安裝在下層底板上，四輪驅動采用Faulbaher公司的空心杯減速伺服有刷直流電動機。該電動機自帶編碼器和減速器，輸出功率為70W。

圖1 全向驅動小車機械結構圖

2 系統硬件設計

整個系統硬件結構如圖2所示，包括小車驅動控制、系統電源、過流保護裝置和慣性傳感裝置。

2.1 驅動控制單元

圖2 雙DSP協調控制原理圖

小車驅動控制處理器選用TI公司的數字信號處理器（Digital Signal Processon，DSP）TMS320F2812［3］，它是基于TMS320C2XX內核的定點DSP。器件上集成了多種先進的外設，為電動機及其他控制領域應用的實現提供了良好的平臺，使控制系統的電路設計大為簡化［4］。同時采用雙DSP控制，一個DSP芯片為主處理器，另外一個DSP芯片為從處理器，主從DSP分別處理2路的電動機碼盤信號和輸出電動機驅動信號，克服了單DSP要處理4個電動機信號及其他外圍事件負擔重、實時性難以保證的缺點。

每片TMS320F2812有12路PWM輸出，同時對2個伺服電動機進行控制［5］。采用如圖3所示的H橋可逆PWM驅動系統，選用半橋驅動器IR2101（圖3中U1和U2）作為H橋上下臂驅動功率管。其中IR2101是專為驅動半橋逆變器中同橋臂的2個MOSFET或IGBT而設計的高壓、高速驅動器［6］。

圖3 電機驅動電路圖

2.2 系統電源

小車采用24V的鋰電池進行供電，由于電動機的驅動電路工作在開關狀態會產生高頻噪聲，為保證電路的正常工作，在電路中采用了電容及共軛線圈進行濾波對這些噪聲進行了處理。同時，在控制電路和驅動電路之間通過高速光耦進行隔離，從而減少了驅動電路對控制電路的干擾［7］。

2.3 過流保護裝置

在系統的保護方面，為防止電動機出現堵轉時產生的過電流損壞器件，加入了過流保護裝置。在電動機兩端串入采樣電阻（圖3中R137，R139），在采樣電阻兩端接入一個差分電路，當流過采樣電阻的電流超過預設值時，使電壓比較器輸出電流信號，使繼電器動作，切掉電源供應。

2.4 慣性傳感裝置

慣性傳感裝置由MENS加速度傳感器模塊、電子羅盤模塊、陀螺儀模塊和液晶顯示模塊組成［8］，如圖4所示。陀螺儀隨著積分誤差的累積會產生漂移，只能工作在相對較短的時間尺度內。加速度計雖然不會產生累積誤差［9］，在一定程度上能糾正陀螺儀的累積誤差，但當系統在三維的空間做變速運動時，因它不能區分重力加速度和外力加速度而會導致輸出不準確。本設計采用電子羅盤航向角清除陀螺儀航向角的累計誤差。電子羅盤的航向角為絕對值輸出，與陀螺儀融合后能得到更穩定的航向角。整合加速度傳感器、電子羅盤和陀螺儀，并均衡的利用三者各自的優點，實現一個所謂的慣性傳感單元［10］。

圖4 慣性傳感裝置結構框圖

加速度傳感器是采用ADI公司的三軸數字加速度傳感器ADXL345，其最大可感知16g的加速度，感應精度可達3.9mg／LSB，傾角測量典型誤差小于1°，可通過I2C數字接口訪問。因此，采用加速度模塊ADXL345便能測量到較精確的加速度值和傾角。電子羅盤則采用霍尼韋爾公司的HMC5883L模塊，該模塊自帶有自動消磁和自動校準庫功能，能很好地校正因外界磁場引起的偏差。此外，小車有時是在三維空間中運動，故選用三軸陀螺儀ITG－3200模塊來對X，Y，Z3軸進行測量。而所選傳感器內部都有溫度傳感器，故都不需要另外對溫度進行補償。由于這些傳感器都帶有I2C接口，故可以分別通過I2C總線與TMS320F28335的I2C接口進行通訊［11］。TMS320 F28335通過I2C總線驅動各個傳感器工作，并采集各個傳感器的數據，同時對采集回來的數據進行處理和發送。發送數據則是通過TMS320F28335上的CAN總線接口與小車主控制器TMS320F2812上的CAN總線接口通訊以及通過異步串口與PC機進行通信。

3 四輪全向驅動小車的軟件設計

圖5為具有慣性傳感裝置的四輪全向驅動小車的程序流程圖。

圖5 程序流程圖

四輪全向小車工作空間為1平面，建立固連在這個平面的絕對坐標系xw－yw，以及固連于車體并且原點與機器人中心重合的局部坐標系xm－ym。如圖1所示，其中θ為xm和xw的夾角，δ為輪子與ym的夾角［12］。

小車的運動方程為

式中，L為四輪全向小車中心到輪子中心的距離；vi為輪子i提供的驅動方向的速度。根據結構代入具體的數據，進一步可得全向小車的運動模型：

所以小車開始時，根據四個輪子的轉速運動模型（式（2））規劃路徑，并設定小車4個輪子的速度v1，v2，v3，v4進行行駛；同時啟用慣性傳感裝置對小車的加速度、位置和目標進行采集，根據慣性回饋計算出速度變化量 Δv1，Δv2，Δv3，Δv4，并對數據進行處理，把處理好的數據發送給小車主控制器，小車主控制器根據慣性參數改變4個輪子的轉速（設小車的4個輪子的電動機分別為1，2，3，4，要改變的轉速分別為v1＋Δv1，v2＋Δv2，v3＋v3，v4＋Δv4）來達到期望軌跡。而輪子的轉速是靠主控制器根據v1＋Δv1，v2＋Δv2，v3＋Δv3，v4＋Δv4來控制各電機的比例積分微分控制器 （Proportinal Band－Integral－Derivative Controller，PID），通過控制PID來改變脈寬調制器（Pulse－Width Modulation，PWM）輸出，從而改變4個輪子的轉速。慣性參數每更新一次都要重新對路徑進行規劃調整，并不斷地重復上述操作，從而達到小車在高速運動時軌跡跟蹤的準確性和快速性。

軟件設計主要包括：慣性傳感裝置里的數據采集程序、數據處理程序、數據通信程序和數據顯示程序以及主控制器里的電機協調控制程序［13］。電機協調控制算法采用的是增量式數字PID控制算法。

4 增量式數字PID控制算法

數字PID控制算法表達式為

式中k為采樣序號，k＝0，1，2，…；uk為第k次采樣時刻的PID控制器輸出值；ek為第k－1次采樣時刻輸入的偏差值；kp為比例系數；kD為微分系數；u0為進行PID控制器的初值。

增量式數字PID控制算法是控制器的輸出僅為控制量的增量Δvk。增量式數字PID控制算法可由式（3）推導出。由式（3）可得到PID控制器在第k－1個采樣時刻的輸出值為

將式（3）與式（4）相減并整理得到增量式數字PID控制算法公式為

5 結 語

本文研究了基于四輪全向驅動小車的雙微處理器控制系統，實現了以TMS320F2812為主處理器的雙DSP協調控制4個電機；同時加入了加速度計、電子羅盤、陀螺儀這些慣性傳感裝置來實現小車自身的定位和小車的運動狀態跟蹤，有效地提高了小車高速運動的性能和小車高速運動軌跡跟蹤的準確性。該設計功耗低、體積小、實時性好，在全自主機器人、工業生產及其他相關領域具有一定的參考價值。

［1］孫文煥，程善美，王曉翔，等.多電機協調控制的發展［J］電氣傳動，1999（6）：3－6.

［2］馮延蓬，仵 博，蔡學軍.一種基于多智能體系統的分布式規劃系統［J］.深圳職業技術學院學報，2009，8（5）：18－21.

［3］徐勇明，張培仁.基于F2812的足球機器人底層控制［J］.Automation and Instrumentation，2004（6）：10－14.

［4］李 磊，葉 濤，譚 民，等.移動機器人技術研究現狀與未來［J］.機器人，2002，24（5）：475－480.

［5］Lowery T F，Petro D W.Application consideration for PWM inerter－fed low－voltage induction motors［J］.Industry Applications，IEEE Trans，1994，30（2）：1532－1538.

［6］張奇志.IR2101帶有欠壓封鎖的半橋MOSFET驅動廣州：［J］.集成電路應用，2002（5）：64－65，58.

［7］付根平.全自主足球機器人運動控制系統的研制［D］.廣州：廣東工業大學，2009：64－65，58.

［8］秦 勇，臧希喆，王曉宇，等.基于MENS慣性傳感器的機器人姿態檢測系統的研究［J］.傳感技術學報2007，20（2）：298－301.

［9］劉晶紅，朱志強，沈宏海，等.加速度在控制系統擾動補償中的應用［J］.光學精密工程，2009，17（9）：2191－2197.

［10］Pasolini F.MEMS加速度計、陀螺儀和地磁感應計推動創新型消費電子應用發展［J］.中國集成電路（CIC），2010（139）：72－75.

［11］Texas Instrument Inc.TMS320F28335，TMS320F283 34，TMS320F28332，TMS320F28235，TMS32F28234，TMS28232Digital Signal Controller（DSCs）Data Manual［EB／OL］.（2009－04－）［2012－06］.http：／／focus.ti.com.cn／cn／docs／prod／folers／print／tms320f28335.html.

［12］Arai T，Pagello E，Parker L E.Advance in multirobot system［J］.IEEE Transaction on Robotics and Automation，2002，18（5）：655－661.

［13］劉陵順，高艷麗，張樹團，等.TMS320F28335DSP原理及開發編程［M］.北京：北京航空航天大學出版社，2011：324－349，208－418.