基于雙DSPs架構的移動機器人運動控制系統的設計

李懷俊

(1.華南理工大學 機械與汽車工程學院，廣州 510640；2.廣東交通職業技術學院，廣州 510650)

0 引言

近年來，隨著技術的發展，移動機器人廣泛地應用在工業現場、醫院和家庭等環境。為了適應不同的環境，機器人的機動性變得越來越重要，而機器人的機動性取決于移動平臺的設計。因各種不同環境的需求與限制，目前已研制出足型機器人、輪型移動平臺和爬蟲型移動平臺等移動機構，其中輪型平臺的移動效率最好[1,2]。設計運動快速靈活的移動機構必須考慮兩個重要因素：運動機構的性能和運動機構的可控性。運動機構性能決定了機器人運動性能的潛能，運動機構的可控性決定了運動機構性能能否被有效地利用起來。由于采用全向輪運動機構的機器人向任意方向做直線運動的同時可做自身旋轉運動調整機器人的姿態，這樣，就使得移動機器人有運動快速靈活、易于控制等優點。因此，全向輪已逐漸取代傳統的差動輪系，成為移動機器人設計發展的趨勢。

1 運動控制系統

1.1 機器人運動學模型

全向輪及其運動機構如圖1所示。

圖1 全向輪及其運動機構示意圖

全向輪運動機構包括全向輪、電機、驅動軸系等幾部分。全向輪由全方向性的輪子組成，此種輪子在周圍上裝有與輪軸垂直的小輪，因此可自由地沿著兩種方向移動。當驅動電機提供動力時，全向輪沿著軸向轉動，此時全向輪上的小輪則無作用。反之，當全向輪往輪軸方向移動時，驅動電機則不輸出扭矩，此時全向輪上的小輪變為輔助輪，減少移動時的摩擦力，幫助機器人移動。全向輪的優點如下：(a) 輪軸不需移動，即可改變機器人方向；(b) 原地旋轉不需要回轉半徑；(c) 可同時且獨立地控制機器人的移動與轉動。

基于全向輪的機器人運動學模型如圖2所示，VX為機器人體坐標系橫坐標方向速度，Vy為機器人體坐標系縱坐標方向速度， w為機器人角速度，角速度正方向為機器人體坐標系逆時針方向。V1、V2、V3、V4分別為機器人1號、2號、3號、4號四個輪子的速度，正方向為機器人體坐標系逆時針方向。 R為驅動輪中心到車體中心的距離。

圖2 機器人運動學模型

則四個輪子的速度分解公式如下：

1.2 雙DSPs架構

本設計選擇TI專門為工業控制和機器人控制設計的TMS320LF2407A[3,4]作為移動機器人的控制核心芯片，因為它具有資源豐富，運算速度快的優點，主要表現在：(a)有兩個正交編碼脈沖（QEP）電路；(b) 有16個16位的脈寬調制通道；(c)有串行通信接口SCI模塊及串行外設接口SPI模塊；(d) 40MIPS的執行速度為機器人的實時控制提供了保證。

但全向輪有四個電機，一個TMS320LF2407A提供兩路QEP用來檢測電機轉速，如何處理另外兩個電機的碼盤信號？目前的方案有：DSPs＋CPLD、DSPs+集成芯片，但這將導致電路復雜、電路的可靠性降低，而且單個DSPs要處理四個電機的信號和其他外圍事件，使得單個DSPs負擔很重，實時性很難保證。因此，本設計采用雙DSPs架構，一個DSPs為主處理器，另一個DSPs為從處理器。主處理器負責與上位機通信，接收上位機發出的四個電機速度值和執行一些控制命令，并且返回底層的信息（如電機碼盤信號、指南針、電池電壓等）給上位機，實時監控機器人底層硬件狀態。從DSPs通過SPI通信協議與主DSPs進行通信，主DSPs將從上位機接收的3、4號電機的速度值發送給從DSPs，從DSPs將3、4號電機的碼盤信號、電池電壓和指南針等信息傳送給主DSPs。由于控制系統采用雙DSPs架構，提高了電路的可靠性和穩定性，同時雙DSPs協同處理信息，處理速度快，實時性強。

1.3 系統硬件組成

圖3 運動控制系統整體結構圖

運動控制系統整體結構如圖3所示。

整個電路[5]分為主從DSPs單元、電機驅動電路單元、電機碼盤接口電路單元、電平轉換電路單元、電池電壓檢測電路單元、其他輔助電路與功能擴展接口電路單元等幾大塊。作為控制核心的DSPs的SCI模塊接收上位機發送過來的機器人速度控制命令，QEP模塊獲得電機的實際轉速，SPI模塊進行主從DSPs間的通信。DSPs經過控制算法，產生PWM，通過電機驅動電路來控制電機。

2 系統軟件的設計

系統軟件的設計[6]主要包括主DSPs、從DSPs、主從DSPs間的通信等部分，其中主DSPs程序流程如圖4所示。從DSPs沒有主DSPs的通信接收子程序，但多了與電子指南針的通信部分，整體來說從DSPs的程序流程與主DSPs基本相同。

圖4 主DSPs程序流程圖

2.1 通訊接收子程序

TMS320LF2407A的串行通信接口SCI模塊支持CPU與其他使用標準格式的異步外設之間的數字通信。SCI接收器和發送器是雙緩沖的，每一個都有自己單獨的使能和中斷標志位。兩者都可以獨立工作，或者在全雙工的方式下同時工作。為了確保數據的完整性，SCI對接收到的數據進行間斷檢測、奇偶性校驗、超時和幀出錯的檢驗。本設計中上位機通過USB轉串口232的方式與主DSPs進行通信。指令幀由5字節數據組成，數據幀第一字節為握手信號,第二字節到第五字節分別為四個電機的給定速度值。當DSPs接收一個完整的數據幀，通信接收程序將該指令幀保存起來，并將通信接收標志ReceValid置1。指令幀數據的格式如表1所示。此外，從DSPs也通過SCI模塊與數字指南針進行通信。

表1 指令幀格式

2.2 正交編碼脈沖程序

TMS320LF2407A帶有兩路正交編碼脈沖（QEP）。該模塊被使能后，可以在編碼和計數引腳上輸入正交編碼脈沖。正交編碼脈沖是兩個頻率變化且正交（即相位相差90度）的脈沖，當它由電機軸上的光電編碼器產生時，電機的旋轉方向可通過檢測兩個脈沖序列中的哪一列先到達來確定，轉速可由脈沖數來確定。由于每個TMS320LF2407A帶有兩路QEP，所以可以很方便地測出四個輪子的實際轉速而無需外加測試電路。

2.3 模糊自適應PID輪速控制

全向輪由四個電機驅動的輪子構成，任一時刻機器人的速度都由四個輪子的不同轉速所合成。為了使四個電機同時獲得較好的動態和靜態特性，控制算法采用了模糊自適應PID控制[7.8]。自適應模糊PID控制以誤差 和誤差變化 作為輸入，利用模糊控制規則在線對PID參數進行修改，以滿足不同時刻 和 對PID參數自整定的要求。離散PID算法如下所示：

式中T為采樣周期，K為采用序號。e(K)為第K個采樣周期的偏差值，Kp為比例系數，Ki為積分作用系數，Kd為微分作用系數。電機控制框圖如圖5所示：

PID參數的整定必須考慮不同時刻KP、Ki、Kd三個參數的作用及相互之間的關系。經過反復實驗，建立的模糊規則表如表2所示。

將系統誤差e和誤差變化ec定義為模糊集上的論域，如果e和ec超出了論域，則以論域中的最大或最小值代替，即：

其模糊子集為：

則第K個采樣時間的整定為：

在線運行時，控制系統通過對模糊邏輯規則的結果處理、查表和運算，完成Kp、Ki、Kd參數的在線校正。

圖5 電機控制框圖

表2 KP、Ki 、Kd模糊規則表

2.4 PWM程序

PWM信號是一串寬度變化的脈沖序列。這些脈沖平均分布在一段定長的周期中，從而每個周期中有一個脈沖。該定長周期為PWM（載波）周期，其倒數為PWM（載波）頻率。PWM脈沖的寬度由另一個具有所需值的調制信號決定。PWM頻率過小，高低電平轉換就過慢，不利于電動機的控制。PWM頻率過大，計算的精度不是很好。這里設定其頻率為200KHZ。要產生一個PWM信號，需要有一個合適的定時器來重復產生一個與PWM周期相同的計數周期和一個用來保持調制值的比較寄存器。比較寄存器的值不斷與定時器的值相比較，當兩個值匹配時，在相應的輸出引腳就會產生一個電平轉換(從低到高或從高到低)。當兩個值之間的第二個匹配產生或一個定時器周期結束時，相應的輸出引腳會產生又一轉換 (從高到低或從低到高)。在每個定時器周期中，這個過程都會出現，但每次比較寄存器中的調制值可以是不同的。這樣在相應的輸出就會產生一串占空比不同的脈沖信號。

2.5 主從DSPs間的通信

主從DSPs間通過串行外設接口(SPI)模塊進行數據的交互。SPI是一個高速、同步串行I/O口，它允許長度可編程的串行位流（1～16位）以可編程的位傳輸速度移入或移出。通常SPI用于DSPs和外部外設以及其它處理器之間的通信。主DSP與從DSP通信流程圖如圖6所示。

圖6 主從DSP間通信流程圖

3 系統異常預防與處理

系統異常主要有兩方面：電路系統異常和執行機構異常。為了機器人運動控制系統能夠穩定可靠工作，必須要有異常預防與處理的機制。本設計中使用的異常預防與處理方法主要如下：

隔離：DSPs的輸入輸出端口經過光耦隔離與電機驅動電路連接；

電源處理：機器人采用一組24伏電池供電，每路的電機驅動電路都有獨立的保護電路和濾波電路，將各個電路引起的影響減到最小，DSPs主控板經過精密、可靠的濾波電路和穩壓電路供電；

DSPs主控板有外圍硬件微處理器監控電路，一旦出現異常，200毫秒內使DSPs重新復位，保證控制系統在短時間內恢復正常運行；

電機長時間堵轉會損壞電機。因此，通過底層控制程序對堵轉電機進行監測，一旦發生電機堵轉，程序就進入異常處理模式。電機堵轉檢測方法有兩種：1）電機輸出電壓高（高于18伏），但電機的轉速低并且持續一段時間（大于250毫秒），則該電機處于堵轉狀態；2）通過計算電機電流來檢測電機的工作狀態是否正常。本設計采用第一種方法：電機處于堵轉時，程序進入異常處理模式，使電機工作在鎖住的狀態，整個機器人表現為原地不動。程序進入異常處理模式后開始計時，經過2秒后底層控制程序切換到正常狀態，檢測電機工作是否正常，如此反復，直到電機停止堵轉。

4 實驗結果及分析

機器人以0.85m/s的速度直線運動時，測得輪子的速度響應曲線如圖7所示。

圖7 全向輪速度響應曲線圖

圖中，曲線1為常規PID控制策略下的速度響應曲線，KP、Ki、Kd及采樣周期T分別為1、0.04、10、0.1s；曲線2、3為采用模糊PID控制策略下的速度響應曲線，PID參數均與曲線1相同，采樣周期T分別為0.1s和0.05s。由曲線3可知，采用模糊PID算法且采樣周期較小時，輪子的響應時間小于300ms、超調量σ=1.5%,響應時間短，移動機器人運動快速靈活。

5 結論

作為一種新型的運動機構，全向輪可以使移動機器人更加靈活。全向輪通過給定不同的速度，使得機器人能夠自由、自主地360度全方位運動，使得機器人涉及的領域更加廣泛。為了充分發揮全向輪結構的優勢，必須選用轉速快、輸出扭矩大的電機組合，因此電機驅動電路的負擔較重，同時主控板的硬件資源和軟件處理能力也必須足以滿足移動機器人實時控制的需要。因此，本文設計的運動控制系統采用了雙DSPs架構，克服了DSPs+CPLD、DSPs+集成芯片造成的電路復雜，可靠性低、實時性不強等缺點。該系統具有電路簡潔、集成度高、可靠性好等優點，實驗證明移動機器人運動快速靈活、具有很好的可控性和穩定性。

[1] 謝云,楊宜民.全自主機器人足球系統的研究綜述[J].機器人,2004,26(5):474-480.

[2] 李磊,葉濤,等.移動機器人技術研究現狀與未來[J].機器人,2002,24(5):475-480.

[3] 劉和平,嚴利平,張學鋒,桌清鋒.TMS320LF240x DSP結構、原理及應用[M],北京:北京航空航天大學出社,2002.

[4] 章云,謝莉萍,熊紅艷.DSP控制器及其應用[M].北京:機械工業出版社,2001.

[5] 李哲英.實用電子電路設計[M],電子工業出版社,1992.

[6] 張培仁,張志堅,等.基于16/32位DSP機器人控制系統設計與實現[M].北京:清華大學出版社,2006.

[7] 舒志兵,等.交流伺服運動控制系統[M].北京:清華大學出版社,2006.

[8] 陶永華.新型PID控制及其應用[M].北京:機械工業出版社, 2002.

[9] 劉金琨.智能控制[M].北京:電子工業出版社,2007.