本文研究了以TMS320LF2407 DSP為核心的移動機器人運動控制器的設計方案，介紹了主要的系統組成。本系統采用模塊化設計，硬件電路簡潔，控制可靠。通過步進電機的細分控制，改善了低速性能，以較低的成本滿足了移動機器人運動控制的性能要求。
關鍵詞：移動機器人；TMS320LF2407；運動控制；模塊設計
引言
運動控制器是移動機器人的執行機構，對系統平穩運行起著重要作用。在機器人運動控制器中，處理器件接受高層控制級的指令，計算和輸出多路控制信號，協調各驅動輪，並對系統狀態進行監控。因此要求處理器要有高速的運算能力和高度的可靠性。DSP芯片集實時處理和控制器外設於一身，精度高、體積小、運算速度快，同時具有多級流水線操作，容易實現復雜算法，為運動控制器的開發提供了理想的解決方案。
本文提出的移動機器人運動控制器的實現方案，以TI公司的定點DSP芯片TMS320LF2407為核心。利用步進電機作為驅動機構，DSP從上位機接收指令，控制步進電機實現要求的動作。系統以較低的成本滿足了控制要求。
運動控制器的設計
機器人移動平台是一個六輪小車，兩側的前輪和中輪采用搖臂式結構，前輪與中輪間距小於車體寬度，易於轉向。機器人主控計算機是一台PC機，作用融合各種信息，發出控制指令，進行遠程監控；下位機是一台工控機，固定在移動車體上，負責接收主控計算機指令並分配給各子系統，同時把各種檢測信息反饋到PC機，兩者通過無線網橋進行通信。運動控制器接收下位機傳送的指令，計算各個電機的控制量、輸出控制脈沖、檢測各反饋信號並發送至下位機。六個車輪均為驅動輪，控制時可簡化為二輪情形。運動控制器以LF2407為核心，主要模塊包括：控制脈沖輸出、檢測處理、通信接口、復位電路、I/O擴展和必要的外圍電路，基本結構如圖1所示。
控制脈沖輸出
本系統步進電機為兩相混合式，步進角為1.8涮濁魑猄H-2H057M。驅動器輸入信號有步進脈沖信號CP、方向電平信號DIR、以及脫機信號FREE。OPTO是三路信號的公共陽極。驅動器撥位開關的前3位用於設定細分數。
LF2407利用事件管理器(EVA、EVB)中的定時器中斷形成步進脈沖。EVA通過定時器1，EVB通過定時器3分別輸出兩路步進脈沖信號CP和方向信號DIR到各電機驅動器。控制時，LF2407根據上位機指令計算各驅動電機轉速和方向，選擇回路內部分頻值，確定對應定時器的周期寄存器初值，然後計數器循環計數。當計數器的值與周期寄存器匹配時，即發生定時中斷，向對應I/O口輸出控制信號。如：車輪直徑為30cm，減速箱減速比為1:15，如設定電機驅動器細分數為5，即步進角為0.36绻笮〕抵畢咴碩俣任?5cm/s，則每秒鐘各電機所需的步進脈沖數為15×(0.25/(0.3×p)×360/0.36≈3978。LF2407內部時鐘為24MHz，如選擇時鐘分頻值為64，則周期寄存器的值為24M/(64×3978)≈94，計數器每次計數到94時，即發生中斷，輸出一次步進脈沖和方向標志。
為防止給定速度突變使步進電機失步，LF2407在接受到速度指令後首先進行積分平滑，平滑後的結果作為控制要求轉換成步進電機的脈沖數值，平滑公式為：
(1)
其中，t為控制的時間周期，與系統積電時間常數有關，需通過實驗確定合適的數值。如果平滑後的速度與現有速度相差較大，則由系統取兩者平均值作為過渡速度，運行若干周期後再轉換到實際給定速度。
此時應注意：(1)設定脈沖頻率時要保證CP信號的脈沖寬度不能小於5ms，且為低電平有效；(2)電機換向時，方向信號DIR要在前一個方向的最後一個CP脈沖結束且下一個方向的第一個CP脈沖前發出；(3)OPTO端須外接+5V的VCC，保證給驅動器內部光耦提供8~15mA的驅動電流，如果VCC不是+5V則須加限流電阻。
檢測處理模塊
檢測模塊監控系統當前狀態，由光電編碼器、ADC和其他檢測電路組成，把驅動電機的轉速、轉向和電流及蓄電池電壓反饋至LF2407。蓄電池電壓通過ADC轉換成數字信號，當電池電壓低於警戒值時，向上位機發出電池能量不足的警告信息。電機電流利用分流電阻檢測，經A/D轉換後輸入到LF2407中。分流電阻的阻值應能在功放電路達到最大電流時激活過流保護功能。此模塊中，ADC的參考電壓由外部電壓源提供，其模擬供電電壓(3.3V)VCCA和模擬地VSSA需與數字供電電壓和數字地分開，以提高精度。
本系統在兩個後輪安裝了光電編碼器用於檢測電機轉速和方向。檢測信息反饋到DSP的正交編碼脈沖電路(QEP)，通過把檢測到的脈沖信息與給定的脈沖數進行比較，分析電機是否出現堵轉、失步等故障。當LF2407的QEP被使能(禁止捕獲功能)，定時器2對引腳CAP1/QEP1和CAP2/QEP2、定時器4對引腳CAP4/QEP3和CAP5/QEP4上的脈沖數目和頻率分別計數，可得到兩個電機的角位移和轉速，方向檢測邏輯通過檢測哪個序列領先可確定電機轉向。此時正交編碼器脈沖作為時鐘源，定時器設置為外部時鐘、定向增/減計數方式。
通信模塊
通信模塊涉及到DSP和下位機(工控機)之間的雙向通信。主要任務是接收控制指令，向工控機傳送運動控制器的當前狀態和裡程計信息。本模塊由串行通信接口(SCI)負責通信轉換，利用中斷進行控制，其接收器和發送器可以單獨或同時工作，並具有獨立的中斷請求信號和中斷矢量。工控機和LF2407之間采用符合RS-232標准的驅動芯片MAX232進行串行通信。由於LF2407采用+3.3V供電，故在MAX232與LF2407之間加入TI公司提供的典型電平匹配電路，整個接口電路簡單、可靠，其電路圖見圖2所示。
LF2407要實現雙向通信，必須在初始化時正確設置一系列通信接口控制寄存器，包括操作模式、協議、波特率、字符長度、奇/偶校驗位、停止位的位數、中斷優先級和使能控制位。通信時，上位機給出控制要求(平移速度和角速度)和數據校驗和，等待反饋信號。LF2407以中斷方式接收數據、計算校驗和，並與接收到的校驗和比較。若一致，則回發“0DDH”，表示接收成功；否則，回發“0BBH”，表示數據錯誤，要求重發。同時，LF2407以中斷方式向上位機發送計算結果、電機轉速和裡程計信息。
復位電路
要使控制系統上電後正常工作，必須對DSP寄存器和I/O端口進行復位。LF2407的復位引腳RS為低電平時DSP終止執行，並使PC=0；當RS為高電平時，從程序0位置開始執行。當看門狗定時器溢出時，在該引腳產生一個系統復位脈沖。圖3為復位電路。剛上電時，LF2407處於復位狀態，復位時間由R和C確定。為使芯片初始化正確，低電平應保持至少3個時鐘周期，同時需滿足DSP晶體振蕩器的穩定時間。
圖3中A點電壓 ，其中t為時間常數，且t=RC。假設低電平與高電平分界點為V0=1.5V，則當V=V0時所經過的時間t0為：
(2)
選擇R＝200KW，C＝4.7mF，VCC=5.0V，由上式得t0=334ms，采用16.0000MHz晶振可滿足系統復位要求。
航跡推算
本系統結合光纖陀螺儀和傾角傳感器的角度變換進行航跡推算，上位機建立三維環境坐標系和車體坐標系，包括機器人的位置(x,y,z)和姿態(a,b)，根據LF2407提供的裡程計信息進行推算。
在控制中，每個固定時間段(50ms)內，LF2407計算左右前輪角位移平均值(芼)作為車體位移，即：
(3)
車體平面移動時，裡程計信息的計算公式為：
(4)
其中，芼為該時段車體的位移；芶、芺分別為左右前輪在一個檢測周期內轉過的弧度；r為車輪半徑；q為光纖陀螺提供的航向角度；f為傾角傳感器提供的俯仰角度。
程序設計
本系統控制程序主要有初始化子程序、通信中斷子程序、主程序以及脈沖輸出中斷子程序。主程序調用系統初始化子程序，進行通信接口、I/O、QEP、定時器等部分及必要中間寄存器的初始化，設置中斷標志，讀取定時器4的計數值。當發生中斷時，調用相應中斷程序，並修改或重置標志位。
通信中斷子程序包括接收中斷和發送中斷，完成數據的接收和發送。由SCI的中斷向量寄存器PIVR的值判斷是接收中斷還是發送中斷，並調用相應服務子程序。接收數據時，要進行奇偶校驗，保證數據准確並回發信號，同時對給定值進行積分平滑，然後分別確定三路輸出電路的分頻值和轉向標志，計算各定時器周期寄存器的值，結果放入中間寄存器，並把計數器清零。發送中斷時，系統讀取蓄電池電壓、電機電流、轉速和方向等寄存器的值，同時相應寄存器清零，並計算裡程計信息和後輪轉速和方向，將各結果發送到上位機。
當某定時器中計數器與周期寄存器匹配時，即發生輸出中斷，相應標志位置位。脈沖輸出中斷子程序根據標志位讀取相應中間寄存器的脈沖值和方向標志，並輸出到I/O口，隨即計數器清零，重新開始計數，同時標志位清零。為增強程序容錯性能，系統使能看門狗定時器。另外，為防止干擾影響控制性能，需增加返回中斷服務子程序，此程序只有一條“return”指令，當發生干擾中斷時，系統直接返回主程序。
結語
本文簡要介紹了移動機器人運動控制器主要部分的實現方案，針對移動機器人的特點和應用背景，以TMS320LF2407作為核心，對六輪搖臂式移動機器人進行運動控制。經過初步實驗，機器人運行平穩，能夠以較低的成本滿足移動機器人運動控制要求。■
參考文獻
1 王文學,孫萍,徐心和. 足球機器人系統結構與關鍵技術研究[J].控制與決策. 2001,16(2)
2 TMS320F/C24x DSP Controllers Reference Guide [M]. Texas Instruments. 1999.
3 TMS320LF2407 TMS320LF2406 TMS320LF2402 DSP Controllers [M], Revised. Texas Instruments. 2001
4 劉和平 等. TMS320LF24x DSP結構、原理及應用[M]. 北京：北京航空航天大學出版社，2003
