基于DSP的欠驅動體操機器人的搖起控制設計

τ為系統的廣義外力，體操機器人在運動過程中的總能量為：

Ki，Pi分別為第f個關節(手臂、軀干、腿)的動能和勢能。
在整個搖起過程，為保證能量不斷增加須滿足，能量的導數必須滿足如下條件：

根據文獻的研究結果：利用M(q)一C(q，q)為反對稱矩陣，有：

顯然，為滿足能量不斷增加的不等式條件，搖起控制轉矩可選擇為：

式中sgn()為取符號函數，Ni為附加力。
實驗證明采用正弦和斜坡函數，此算法位于基于能量搖起模塊void swing_up()中，這些數據分別從運．動控制器的2、1通道輸入，需說明的是，在對體操機器人系統進行數學建模時已明確，體操機器人系統是一個力控設備，根據牛頓第二定律，力與加速度成正比，因此給系統施加的控制量就是加速度，而速度則是一個事先設定的較大數值，在控制過程中，一般不會達到這個速度值。這樣保證產生的附加力隨能量的增加而減小，可搖起體操機器人到倒立平衡位置。

4 控制系統軟件設計
實施控制時，主要是控制軟件的編寫與控制其參數的調節。由于體操機器人控制系統的實時性要求較高，控制軟件必須滿足實時性的要求，本系統控制周期為6ms。控制軟件必須在準確的控制周期內完成數據采樣，處理并且發出控制信號給運動控制器。
首先由上位PC機設置好控制參數，系統開始運行并完成初始化工作。底層控制器對各轉動關節進行位置采樣，同時接收來自上位PC機的控制指令，并把兩者結合在一起進行分析，通過編寫進去的控制算法生成相應的轉矩控制信號，經功率放大后送給執行電機，同時把各關節的運動信息上傳給PC機，如此反復，完成整個閉環運動控制。
下位機控制器上電后主程序進行控制器初始化操作、禁止看門狗、設置關鍵寄存器、設置中斷向量和中斷寄存器、初始化事件管理器、基于能量搖起、進行實時控制模塊等。其中實時控制模塊中用到ADSP2181可編程定時器，它能夠產生周期性的定時中斷，定時間隔是處理器時鐘周期的整數倍。當定時器被使能后，一個16位的計數寄存器TCOUNT每隔m個周期就會減1，其初始化程序如下：
void init_interrupt()
{disable0；∥關閉中斷(在初始化中斷過程中嚴禁產生中斷)
outportb(0x43,0x36)；／／寫中斷控制字
outportb(0x40,0xf6)；∥寫數據寄存器：0x40為數據寄存器內存映射地址
outportb(0x40,0xlb)；∥數據為16位時鐘細分比率，先寫入低字節0xf6，后寫入高字節0xlb
oldhandler=getvect(INTR)；∥保存原有的中斷服務程序句柄
setvect(INTR，handler)；／／設置新的中斷服務程序句柄
enable()；∥打開中斷}
上述中斷初始化代碼創建一個6毫秒的定時器中斷，即當打開中斷后，將每隔6毫秒對體操機器人系統進行一次實時控制。6毫秒的定時時間是通過向數據寄存器寫入0x1bf6=7158獲得的。

5 結語
基于高性能ADSP2181的控制器設計能夠滿足機器人的搖起控制要求，且由于采用PC+控制器分級控制和模塊化設計思想，將有利于軟、硬件升級，及大大縮短開發周期。系統在實驗室進行機器人搖起實驗時，各電機軸運轉平穩，動作協調軌跡跟蹤實時性好，機器人沒有出現抖動、喘振等現象。
今后還需要探討更多的搖起與平衡控制策略，完善DSP的控制程序。