郝建豹 洪志杰
　
1 引言
　　
欠驅(qū)動(dòng)機(jī)器人是一種部分關(guān)節(jié)為被動(dòng)關(guān)節(jié)，能用較少的驅(qū)動(dòng)裝置完成復(fù)雜任務(wù)的機(jī)械系統(tǒng)。欠驅(qū)動(dòng)機(jī)器人是近年來(lái)開(kāi)始出現(xiàn)的一個(gè)較新的研究領(lǐng)域，針對(duì)欠驅(qū)動(dòng)機(jī)器人系統(tǒng)的研究目前得到不少學(xué)者的關(guān)注。欠驅(qū)動(dòng)機(jī)器人系統(tǒng)在很多的機(jī)器人系統(tǒng)例如多指機(jī)器人手、輪式移動(dòng)機(jī)器人、太空機(jī)器人等非完整約束系統(tǒng)中都存在。因此近幾年來(lái)，欠驅(qū)動(dòng)機(jī)器人的運(yùn)動(dòng)控制問(wèn)題引起國(guó)內(nèi)外廣泛的研究興趣。同時(shí)，隨著集成芯片技術(shù)的飛速發(fā)展，人們對(duì)欠驅(qū)動(dòng)機(jī)器人控制的實(shí)時(shí)性和精度提出了越來(lái)越高的要求。
　　
本文設(shè)計(jì)了一種基于DSP的機(jī)器人控制系統(tǒng)。該控制系統(tǒng)采用兩級(jí)控制結(jié)構(gòu)。以通用PC作為上位機(jī)，完成目標(biāo)設(shè)定、軌跡生成、系統(tǒng)管理和人機(jī)接口等功能；以美國(guó)Analog Device公司的定點(diǎn)數(shù)字信號(hào)處理芯片ADSP2181為核心，作為機(jī)器人控制的下位機(jī)。該控制器充分應(yīng)用了DSP運(yùn)算的高速性，提高了系統(tǒng)的實(shí)時(shí)和穩(wěn)定性。
　　
2 體操機(jī)器人硬件系統(tǒng)設(shè)計(jì)
　　
2．1 控制系統(tǒng)硬件結(jié)構(gòu)
　　
控制對(duì)象為三關(guān)節(jié)欠驅(qū)動(dòng)體操機(jī)器人，其中手臂關(guān)節(jié)為被動(dòng)關(guān)節(jié)。控制轉(zhuǎn)矩來(lái)自?xún)膳_(tái)直流伺服電機(jī)，配5l：l減速器伺服電機(jī)，經(jīng)軸線(xiàn)相交的圓錐齒輪傳輸至驅(qū)動(dòng)關(guān)節(jié)，驅(qū)動(dòng)關(guān)節(jié)通過(guò)動(dòng)力耦合作用使被動(dòng)關(guān)節(jié)產(chǎn)生運(yùn)動(dòng)。欠驅(qū)動(dòng)機(jī)器人控制系統(tǒng)的任務(wù)就是對(duì)驅(qū)動(dòng)關(guān)節(jié)電機(jī)進(jìn)行控制，電機(jī)運(yùn)行之前從控制界面輸入控制目標(biāo)參數(shù)，上位PC機(jī)負(fù)責(zé)發(fā)送控制命令與數(shù)據(jù)采集。DSP處于整個(gè)控制系統(tǒng)的底層，主要用來(lái)接收來(lái)自PC機(jī)的控制命令，對(duì)驅(qū)動(dòng)關(guān)節(jié)執(zhí)行電機(jī)控制，同時(shí)把底層信息反饋給上位PC機(jī)，以實(shí)現(xiàn)機(jī)器人運(yùn)動(dòng)信息的數(shù)據(jù)分析等功能，用一塊DSP運(yùn)動(dòng)控制卡對(duì)機(jī)器人的肩關(guān)節(jié)或髖關(guān)節(jié)進(jìn)行插補(bǔ)計(jì)算和伺服控制，采用PCI標(biāo)準(zhǔn)總線(xiàn)進(jìn)行上、下位機(jī)的通訊，實(shí)現(xiàn)雙速率運(yùn)行?？刂葡涿姘逄峁┝烁麝P(guān)節(jié)工作狀態(tài)顯示及伺服報(bào)警提示，還可以對(duì)關(guān)節(jié)進(jìn)行手動(dòng)控制。三關(guān)節(jié)欠驅(qū)動(dòng)體操機(jī)器人控制系統(tǒng)結(jié)構(gòu)如圖l所示，體操機(jī)器人本體如圖2所示。





2．2 運(yùn)動(dòng)控制器結(jié)構(gòu)

系統(tǒng)采用ADSP218l數(shù)字信號(hào)處理器為核心，實(shí)現(xiàn)高性能的控制運(yùn)算的伺服運(yùn)動(dòng)控制器。如圖3所示，運(yùn)動(dòng)控制器的控制過(guò)程為增量編碼器的A、B相信號(hào)作為位置反饋輸入信號(hào)，運(yùn)動(dòng)控制器通過(guò)四倍頻、加減計(jì)數(shù)器得到實(shí)際位置。實(shí)際位置的信息保存在位置寄存器中，PC機(jī)可通過(guò)控制寄存器讀取。運(yùn)動(dòng)控制器的目標(biāo)位置由PC機(jī)設(shè)定，通過(guò)內(nèi)部計(jì)算得到位置誤差，經(jīng)過(guò)數(shù)字伺服濾波器后，送到數(shù)模轉(zhuǎn)換(DAC)或脈寬調(diào)制器(PWM)硬件處理電路，經(jīng)過(guò)轉(zhuǎn)換最后輸出伺服電機(jī)的控制信號(hào)：+／-lOV模擬信號(hào)或PWM信號(hào)。



2．3 系統(tǒng)的通訊

系統(tǒng)采用PCI總線(xiàn)進(jìn)行通訊。PCI總線(xiàn)的主要優(yōu)點(diǎn)是性能高(數(shù)據(jù)傳輸率可達(dá)到132 264Mb／s)，總線(xiàn)通用性強(qiáng)，成本低，使用方便靈活。系統(tǒng)通訊采用PLX9054接口芯片，結(jié)合雙口RAM，實(shí)現(xiàn)了DSP和PCI總線(xiàn)間的雙向高速實(shí)時(shí)數(shù)據(jù)交換。PCI總線(xiàn)與雙口RAM的數(shù)據(jù)交換，采用了定時(shí)傳送加握手信號(hào)的方式進(jìn)行。具體實(shí)現(xiàn)如下：上位機(jī)每隔一個(gè)固定的時(shí)間T下傳一組數(shù)據(jù)，數(shù)據(jù)傳送完成后，發(fā)出一個(gè)發(fā)送完信號(hào)，下位機(jī)接受到這個(gè)數(shù)據(jù)后，立即從雙口RAM中讀取數(shù)據(jù)。下位機(jī)上傳數(shù)據(jù)也采用同樣的處理方式。這種方式特別適合于機(jī)器人控制系統(tǒng)的通訊。
　　
2．4 驅(qū)動(dòng)元件的選擇
　　
驅(qū)動(dòng)元件選擇了直流伺服電機(jī)，其參數(shù)為200W／7220mm／107mN•m Maxon。其驅(qū)動(dòng)器可實(shí)現(xiàn)位置、速度和轉(zhuǎn)矩三種不同的控制方式：具有共振抑制和控制功能，可彌補(bǔ)機(jī)械的剛性不足，從而實(shí)現(xiàn)高速定位。同時(shí)，還采用PID濾波器，外加速度和加速度前饋，即PID+Kvff+Kaff濾波器。通過(guò)調(diào)節(jié)各參數(shù)，濾波器能對(duì)大多數(shù)系統(tǒng)實(shí)現(xiàn)精確而穩(wěn)定的控制。因此非常適合應(yīng)用于機(jī)器人控制系統(tǒng)。

3 搖起控制策略
　　
由于體操機(jī)器人大范圍的運(yùn)動(dòng)，搖起問(wèn)題是高度非線(xiàn)性又極具挑戰(zhàn)性的問(wèn)題。搖起過(guò)程如同人在單杠一樣，先使體操機(jī)器人來(lái)回?fù)u動(dòng)幾次，體操機(jī)器人始終在下，一旦有足夠的能量施加到系統(tǒng)上，機(jī)器人即進(jìn)入倒立狀態(tài)，如圖4所示。



Spong建議一種非線(xiàn)性反饋方法，這種方法利用部分線(xiàn)性反饋定義PD控制器。從直觀(guān)上講，當(dāng)體操機(jī)器人從懸掛穩(wěn)定平衡狀態(tài)轉(zhuǎn)移到倒立不穩(wěn)定平衡狀態(tài)的過(guò)程中，其勢(shì)能是不斷增加的，因此需要向系統(tǒng)輸入足夠的能量。本研究從能量增加的角度出發(fā)，采用帶有振幅和頻率的正弦方式和斜坡函數(shù)輸入，這樣可同時(shí)增加擺動(dòng)。
　　
根據(jù)拉格朗日運(yùn)動(dòng)方程式可推導(dǎo)出三關(guān)節(jié)機(jī)器人的動(dòng)力學(xué)模型：



τ為系統(tǒng)的廣義外力，體操機(jī)器人在運(yùn)動(dòng)過(guò)程中的總能量為：



Ki，Pi分別為第f個(gè)關(guān)節(jié)(手臂、軀干、腿)的動(dòng)能和勢(shì)能。
　　
在整個(gè)搖起過(guò)程，為保證能量不斷增加須滿(mǎn)足，能量的導(dǎo)數(shù)必須滿(mǎn)足如下條件：



根據(jù)文獻(xiàn)的研究結(jié)果：利用M(q)一C(q，q)為反對(duì)稱(chēng)矩陣，有：



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



式中sgn()為取符號(hào)函數(shù)，Ni為附加力。

實(shí)驗(yàn)證明采用正弦和斜坡函數(shù)，此算法位于基于能量搖起模塊void swing_up()中，這些數(shù)據(jù)分別從運(yùn)．動(dòng)控制器的2、1通道輸入，需說(shuō)明的是，在對(duì)體操機(jī)器人系統(tǒng)進(jìn)行數(shù)學(xué)建模時(shí)已明確，體操機(jī)器人系統(tǒng)是一個(gè)力控設(shè)備，根據(jù)牛頓第二定律，力與加速度成正比，因此給系統(tǒng)施加的控制量就是加速度，而速度則是一個(gè)事先設(shè)定的較大數(shù)值，在控制過(guò)程中，一般不會(huì)達(dá)到這個(gè)速度值。這樣保證產(chǎn)生的附加力隨能量的增加而減小，可搖起體操機(jī)器人到倒立平衡位置。
　　
4 控制系統(tǒng)軟件設(shè)計(jì)
　　
實(shí)施控制時(shí)，主要是控制軟件的編寫(xiě)與控制其參數(shù)的調(diào)節(jié)。由于體操機(jī)器人控制系統(tǒng)的實(shí)時(shí)性要求較高，控制軟件必須滿(mǎn)足實(shí)時(shí)性的要求，本系統(tǒng)控制周期為6ms?？刂栖浖仨氃跍?zhǔn)確的控制周期內(nèi)完成數(shù)據(jù)采樣，處理并且發(fā)出控制信號(hào)給運(yùn)動(dòng)控制器。
　　
首先由上位PC機(jī)設(shè)置好控制參數(shù)，系統(tǒng)開(kāi)始運(yùn)行并完成初始化工作。底層控制器對(duì)各轉(zhuǎn)動(dòng)關(guān)節(jié)進(jìn)行位置采樣，同時(shí)接收來(lái)自上位PC機(jī)的控制指令，并把兩者結(jié)合在一起進(jìn)行分析，通過(guò)編寫(xiě)進(jìn)去的控制算法生成相應(yīng)的轉(zhuǎn)矩控制信號(hào)，經(jīng)功率放大后送給執(zhí)行電機(jī)，同時(shí)把各關(guān)節(jié)的運(yùn)動(dòng)信息上傳給PC機(jī)，如此反復(fù)，完成整個(gè)閉環(huán)運(yùn)動(dòng)控制。
　　
下位機(jī)控制器上電后主程序進(jìn)行控制器初始化操作、禁止看門(mén)狗、設(shè)置關(guān)鍵寄存器、設(shè)置中斷向量和中斷寄存器、初始化事件管理器、基于能量搖起、進(jìn)行實(shí)時(shí)控制模塊等。其中實(shí)時(shí)控制模塊中用到ADSP2181可編程定時(shí)器，它能夠產(chǎn)生周期性的定時(shí)中斷，定時(shí)間隔是處理器時(shí)鐘周期的整數(shù)倍。當(dāng)定時(shí)器被使能后，一個(gè)16位的計(jì)數(shù)寄存器TCOUNT每隔m個(gè)周期就會(huì)減1，其初始化程序如下：



上述中斷初始化代碼創(chuàng)建一個(gè)6毫秒的定時(shí)器中斷，即當(dāng)打開(kāi)中斷后，將每隔6毫秒對(duì)體操機(jī)器人系統(tǒng)進(jìn)行一次實(shí)時(shí)控制。6毫秒的定時(shí)時(shí)間是通過(guò)向數(shù)據(jù)寄存器寫(xiě)入0x1bf6=7158獲得的。
　　
5 結(jié)語(yǔ)
　　
基于高性能ADSP2181的控制器設(shè)計(jì)能夠滿(mǎn)足機(jī)器人的搖起控制要求，且由于采用PC+控制器分級(jí)控制和模塊化設(shè)計(jì)思想，將有利于軟、硬件升級(jí)，及大大縮短開(kāi)發(fā)周期。系統(tǒng)在實(shí)驗(yàn)室進(jìn)行機(jī)器人搖起實(shí)驗(yàn)時(shí)，各電機(jī)軸運(yùn)轉(zhuǎn)平穩(wěn)，動(dòng)作協(xié)調(diào)軌跡跟蹤實(shí)時(shí)性好，機(jī)器人沒(méi)有出現(xiàn)抖動(dòng)、喘振等現(xiàn)象。
　　
今后還需要探討更多的搖起與平衡控制策略，完善DSP的控制程序。

http://article.ednchina.com/CPUDSP/200904100828501.htm