基于雙DSPs架構(gòu)的移動(dòng)機(jī)器人運(yùn)動(dòng)控制系統(tǒng)的設(shè)計(jì)

李懷俊

(1.華南理工大學(xué) 機(jī)械與汽車工程學(xué)院，廣州 510640；2.廣東交通職業(yè)技術(shù)學(xué)院，廣州 510650)

0 引言

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

1 運(yùn)動(dòng)控制系統(tǒng)

1.1 機(jī)器人運(yùn)動(dòng)學(xué)模型

全向輪及其運(yùn)動(dòng)機(jī)構(gòu)如圖1所示。

圖1 全向輪及其運(yùn)動(dòng)機(jī)構(gòu)示意圖

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

基于全向輪的機(jī)器人運(yùn)動(dòng)學(xué)模型如圖2所示，VX為機(jī)器人體坐標(biāo)系橫坐標(biāo)方向速度，Vy為機(jī)器人體坐標(biāo)系縱坐標(biāo)方向速度， w為機(jī)器人角速度，角速度正方向?yàn)闄C(jī)器人體坐標(biāo)系逆時(shí)針方向。V1、V2、V3、V4分別為機(jī)器人1號(hào)、2號(hào)、3號(hào)、4號(hào)四個(gè)輪子的速度，正方向?yàn)闄C(jī)器人體坐標(biāo)系逆時(shí)針方向。 R為驅(qū)動(dòng)輪中心到車體中心的距離。

圖2 機(jī)器人運(yùn)動(dòng)學(xué)模型

則四個(gè)輪子的速度分解公式如下：

1.2 雙DSPs架構(gòu)

本設(shè)計(jì)選擇TI專門為工業(yè)控制和機(jī)器人控制設(shè)計(jì)的TMS320LF2407A[3,4]作為移動(dòng)機(jī)器人的控制核心芯片，因?yàn)樗哂匈Y源豐富，運(yùn)算速度快的優(yōu)點(diǎn)，主要表現(xiàn)在：(a)有兩個(gè)正交編碼脈沖（QEP）電路；(b) 有16個(gè)16位的脈寬調(diào)制通道；(c)有串行通信接口SCI模塊及串行外設(shè)接口SPI模塊；(d) 40MIPS的執(zhí)行速度為機(jī)器人的實(shí)時(shí)控制提供了保證。

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

1.3 系統(tǒng)硬件組成

圖3 運(yùn)動(dòng)控制系統(tǒng)整體結(jié)構(gòu)圖

運(yùn)動(dòng)控制系統(tǒng)整體結(jié)構(gòu)如圖3所示。

整個(gè)電路[5]分為主從DSPs單元、電機(jī)驅(qū)動(dòng)電路單元、電機(jī)碼盤接口電路單元、電平轉(zhuǎn)換電路單元、電池電壓檢測(cè)電路單元、其他輔助電路與功能擴(kuò)展接口電路單元等幾大塊。作為控制核心的DSPs的SCI模塊接收上位機(jī)發(fā)送過來的機(jī)器人速度控制命令，QEP模塊獲得電機(jī)的實(shí)際轉(zhuǎn)速，SPI模塊進(jìn)行主從DSPs間的通信。DSPs經(jīng)過控制算法，產(chǎn)生PWM，通過電機(jī)驅(qū)動(dòng)電路來控制電機(jī)。

2 系統(tǒng)軟件的設(shè)計(jì)

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

圖4 主DSPs程序流程圖

2.1 通訊接收子程序

TMS320LF2407A的串行通信接口SCI模塊支持CPU與其他使用標(biāo)準(zhǔn)格式的異步外設(shè)之間的數(shù)字通信。SCI接收器和發(fā)送器是雙緩沖的，每一個(gè)都有自己?jiǎn)为?dú)的使能和中斷標(biāo)志位。兩者都可以獨(dú)立工作，或者在全雙工的方式下同時(shí)工作。為了確保數(shù)據(jù)的完整性，SCI對(duì)接收到的數(shù)據(jù)進(jìn)行間斷檢測(cè)、奇偶性校驗(yàn)、超時(shí)和幀出錯(cuò)的檢驗(yàn)。本設(shè)計(jì)中上位機(jī)通過USB轉(zhuǎn)串口232的方式與主DSPs進(jìn)行通信。指令幀由5字節(jié)數(shù)據(jù)組成，數(shù)據(jù)幀第一字節(jié)為握手信號(hào),第二字節(jié)到第五字節(jié)分別為四個(gè)電機(jī)的給定速度值。當(dāng)DSPs接收一個(gè)完整的數(shù)據(jù)幀，通信接收程序?qū)⒃撝噶顜４嫫饋?，并將通信接收?biāo)志ReceValid置1。指令幀數(shù)據(jù)的格式如表1所示。此外，從DSPs也通過SCI模塊與數(shù)字指南針進(jìn)行通信。

表1 指令幀格式

2.2 正交編碼脈沖程序

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

2.3 模糊自適應(yīng)PID輪速控制

全向輪由四個(gè)電機(jī)驅(qū)動(dòng)的輪子構(gòu)成，任一時(shí)刻機(jī)器人的速度都由四個(gè)輪子的不同轉(zhuǎn)速所合成。為了使四個(gè)電機(jī)同時(shí)獲得較好的動(dòng)態(tài)和靜態(tài)特性，控制算法采用了模糊自適應(yīng)PID控制[7.8]。自適應(yīng)模糊PID控制以誤差 和誤差變化 作為輸入，利用模糊控制規(guī)則在線對(duì)PID參數(shù)進(jìn)行修改，以滿足不同時(shí)刻 和 對(duì)PID參數(shù)自整定的要求。離散PID算法如下所示：

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

PID參數(shù)的整定必須考慮不同時(shí)刻KP、Ki、Kd三個(gè)參數(shù)的作用及相互之間的關(guān)系。經(jīng)過反復(fù)實(shí)驗(yàn)，建立的模糊規(guī)則表如表2所示。

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

其模糊子集為：

則第K個(gè)采樣時(shí)間的整定為：

在線運(yùn)行時(shí)，控制系統(tǒng)通過對(duì)模糊邏輯規(guī)則的結(jié)果處理、查表和運(yùn)算，完成Kp、Ki、Kd參數(shù)的在線校正。

圖5 電機(jī)控制框圖

表2 KP、Ki 、Kd模糊規(guī)則表

2.4 PWM程序

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

2.5 主從DSPs間的通信

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

圖6 主從DSP間通信流程圖

3 系統(tǒng)異常預(yù)防與處理

系統(tǒng)異常主要有兩方面：電路系統(tǒng)異常和執(zhí)行機(jī)構(gòu)異常。為了機(jī)器人運(yùn)動(dòng)控制系統(tǒng)能夠穩(wěn)定可靠工作，必須要有異常預(yù)防與處理的機(jī)制。本設(shè)計(jì)中使用的異常預(yù)防與處理方法主要如下：

隔離：DSPs的輸入輸出端口經(jīng)過光耦隔離與電機(jī)驅(qū)動(dòng)電路連接；

電源處理：機(jī)器人采用一組24伏電池供電，每路的電機(jī)驅(qū)動(dòng)電路都有獨(dú)立的保護(hù)電路和濾波電路，將各個(gè)電路引起的影響減到最小，DSPs主控板經(jīng)過精密、可靠的濾波電路和穩(wěn)壓電路供電；

DSPs主控板有外圍硬件微處理器監(jiān)控電路，一旦出現(xiàn)異常，200毫秒內(nèi)使DSPs重新復(fù)位，保證控制系統(tǒng)在短時(shí)間內(nèi)恢復(fù)正常運(yùn)行；

電機(jī)長(zhǎng)時(shí)間堵轉(zhuǎn)會(huì)損壞電機(jī)。因此，通過底層控制程序?qū)Χ罗D(zhuǎn)電機(jī)進(jìn)行監(jiān)測(cè)，一旦發(fā)生電機(jī)堵轉(zhuǎn)，程序就進(jìn)入異常處理模式。電機(jī)堵轉(zhuǎn)檢測(cè)方法有兩種：1）電機(jī)輸出電壓高（高于18伏），但電機(jī)的轉(zhuǎn)速低并且持續(xù)一段時(shí)間（大于250毫秒），則該電機(jī)處于堵轉(zhuǎn)狀態(tài)；2）通過計(jì)算電機(jī)電流來檢測(cè)電機(jī)的工作狀態(tài)是否正常。本設(shè)計(jì)采用第一種方法：電機(jī)處于堵轉(zhuǎn)時(shí)，程序進(jìn)入異常處理模式，使電機(jī)工作在鎖住的狀態(tài)，整個(gè)機(jī)器人表現(xiàn)為原地不動(dòng)。程序進(jìn)入異常處理模式后開始計(jì)時(shí)，經(jīng)過2秒后底層控制程序切換到正常狀態(tài)，檢測(cè)電機(jī)工作是否正常，如此反復(fù)，直到電機(jī)停止堵轉(zhuǎn)。

4 實(shí)驗(yàn)結(jié)果及分析

機(jī)器人以0.85m/s的速度直線運(yùn)動(dòng)時(shí)，測(cè)得輪子的速度響應(yīng)曲線如圖7所示。

圖7 全向輪速度響應(yīng)曲線圖

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

5 結(jié)論

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

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

[2] 李磊,葉濤,等.移動(dòng)機(jī)器人技術(shù)研究現(xiàn)狀與未來[J].機(jī)器人,2002,24(5):475-480.

[3] 劉和平,嚴(yán)利平,張學(xué)鋒,桌清鋒.TMS320LF240x DSP結(jié)構(gòu)、原理及應(yīng)用[M],北京:北京航空航天大學(xué)出社,2002.

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

[5] 李哲英.實(shí)用電子電路設(shè)計(jì)[M],電子工業(yè)出版社,1992.

[6] 張培仁,張志堅(jiān),等.基于16/32位DSP機(jī)器人控制系統(tǒng)設(shè)計(jì)與實(shí)現(xiàn)[M].北京:清華大學(xué)出版社,2006.

[7] 舒志兵,等.交流伺服運(yùn)動(dòng)控制系統(tǒng)[M].北京:清華大學(xué)出版社,2006.

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

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