一種機器人倒立擺實驗系統(tǒng)的研究

李璐君，韓 冰，張華良，劉廣輝，李慶鑫

1(中國科學院 網(wǎng)絡化控制系統(tǒng)重點實驗室，中國科學院 沈陽自動化研究所機器人學國家重點實驗室， 沈陽 110016)

2(中國科學院 機器人與智能制造創(chuàng)新研究院，沈陽 110169)

3(中國科學院大學， 北京 100049)

4(東北大學 機器人科學與工程學院， 沈陽 110819)

E-mail：hanbing@sia.cn

1 引 言

機器人教學系統(tǒng)是工程實驗教學的平臺，人們應以教育理念為指導，以培養(yǎng)學生分析能力、創(chuàng)造能力和實踐能力為目的，對機器人進行設計和開發(fā)，使其具有教學適用性、開放性、可擴展性和友好的人機交互等特性[1].目前，我國高校機器人專業(yè)教育尚處于初步發(fā)展階段.機器人教學系統(tǒng)的研究對促進機器人教育的發(fā)展具有重要意義.

當前，機器人教學課程所采用的大多數(shù)工業(yè)機器人的控制系統(tǒng)是封閉的，無法寫入底層控制算法.這就限制了算法在機器人實際應用上的實驗與驗證.而只在計算機中對控制算法進行仿真并不能有力地證明機器人控制算法的實際性能以及發(fā)現(xiàn)算法在實際應用中的問題.因此，建立起一個開放式機器人學算法驗證的教學系統(tǒng)迫在眉睫.

本文開發(fā)出了一種采用工業(yè)機器人硬件本體，并基于Matlab/Simulink軟件建模和自動代碼生成，進行開放式機器人學算法驗證的教學系統(tǒng)，最后以機器人倒立擺的實現(xiàn)為例，介紹了教學系統(tǒng)的具體應用.

2 實驗平臺總體組成

實驗平臺的總體組成包括機器人倒立擺、機器人控制器、PC機，如圖1(a)所示.實驗系統(tǒng)可分成兩個控制部分，即倒立擺控制部分和機器人控制部分，兩部分共同組成一個閉環(huán)控制系統(tǒng).PC機主要用于實驗算法開發(fā)，并通過以太網(wǎng)將算法遠程部署到機器人控制器對機器人進行實時控制，并在運行期間承擔非實時數(shù)據(jù)通信.倒立擺的機械結(jié)構(gòu)如圖1(b)所示.包括編碼器、擺桿、連接件、旋轉(zhuǎn)軸等.擺桿繞著旋轉(zhuǎn)軸轉(zhuǎn)動，編碼器檢測倒立擺擺桿繞旋轉(zhuǎn)軸偏移的角度，機器人法蘭帶動連接件進行水平移動，實現(xiàn)一階倒立擺的控制.

圖1 實驗平臺構(gòu)造圖

3 教學系統(tǒng)軟件架構(gòu)

本系統(tǒng)是基于windows系統(tǒng)下Matlab2016a/Simulink軟件建模和自動代碼生成所建立的機器人系統(tǒng)快速開發(fā)平臺，允許科研人員和學生直接在MATLAB2016a及以上版本的Simulink 仿真環(huán)境下進行算法開發(fā)與驗證工作.本系統(tǒng)不要求學生對整個機器人控制系統(tǒng)有深入的認識，讓學生將精力集中于算法開發(fā)上，如倒立擺控制算法、機器人正逆解算法、插補算法等.

3.1 實驗系統(tǒng)開發(fā)接口

本實驗系統(tǒng)將機器人控制系統(tǒng)分為四層，最上層為應用層，第二層為運動規(guī)劃層、第三層為控制層、第四層為驅(qū)動層.運動規(guī)劃層與控制層提供了大量的二次開發(fā)接口，如EtherCAT通信接口、機器人模型接口、插補規(guī)劃接口等.并支持學生對開發(fā)接口進行重寫.

EtherCAT總線接口約有100多個，支持多條總線同時使用，每條總線又支持多種設備，如：數(shù)字io,模擬io,附加軸等.可利用一個總線管理帶有數(shù)字io,模擬io,多組附加軸的雙臂機器人甚至多機器人系統(tǒng).本系統(tǒng)實現(xiàn)的EtherCAT接口將被管理的EtherCAT設備分為四個層次，并通過字符名稱對不同層級的設備進行管理.我們利用相應接口并指定設備名稱來創(chuàng)建一個EtherCAT設備，系統(tǒng)會自動對io、機器人等子設備進行分類并命名，方便底層對EtherCAT從站設備進行數(shù)據(jù)操作.

機器人模型接口包括管理接口、運動學模型接口及動力學模型接口.管理接口主要完成配置文件數(shù)據(jù)的解析和模型創(chuàng)建工作，即利用配置文件中配置的模型數(shù)據(jù)創(chuàng)建模型，可用于運動學與動力學模型求解.管理接口可管理多個模型數(shù)據(jù)，通過不同的名稱即可實現(xiàn)對不同模型的操作，創(chuàng)建的模型數(shù)據(jù)全局有效.運動學模型接口包括正運動學接口和逆運動學接口，分別用于機器人正運動學和逆運動學的求解.模型可以用通用的數(shù)據(jù)解法或解析解法，完成關(guān)節(jié)角度與末端位置和姿態(tài)的相互轉(zhuǎn)換.動力學模型接口包括正動力學接口和逆動力學接口.分別用于機器人正動力學和逆動力學的求解.正動力學模型可用作被控對象的數(shù)字模型，可以用于算法的仿真驗證.而逆運動學模型一般可用于控制算法中力矩的前饋.在基于模型的控制算法中一般需要獲取機器人的慣性矩陣、奧利科利矩陣，重力矩、摩擦力矩、外部力矩.所以動力學接口中還包括這部分數(shù)據(jù)的獲取.

由于機器人的驅(qū)動裝置提供的功率僅能保證關(guān)節(jié)的速度、加/減速度在一定的范圍.因此，機器人某個關(guān)節(jié)從一個位置到另一個位置并不是簡單的給定目標為位置就可以的，而是需要在當前位置與目標位置之間分割若干小段，以保證機器人關(guān)節(jié)的運動速度、加/減速度不超過最大限制，也就是對機器人的運動進行插補和規(guī)劃.機器人基礎(chǔ)規(guī)劃插補接口能夠獲取機器人關(guān)節(jié)在每個規(guī)劃時刻的規(guī)劃位置、速度、加速度、加加速度，對機器人的運動進行基本的規(guī)劃與插補.

3.2 實驗系統(tǒng)接口模塊

為了學生能夠在MATLAB/Simulink下進行算法開發(fā)，如圖2(a)所示，本系統(tǒng)提供了相對應的接口模塊，并以機器人倒立擺控制實驗為例，進行控制算法的開發(fā)與驗證.

圖2 接口模塊與實驗搭建圖

利用上述提供的開發(fā)接口，我們搭建了如圖2(b)所示MATLAB/Simulink實驗工程.其中包括倒立擺算法模塊、機器人控制模塊、3維模型顯示模塊、安全模塊等.四個開關(guān)分別控制仿真切換、倒立擺初始化、倒立擺控制的啟停、移動控制.實驗學生可根據(jù)需要對倒立擺算法模塊和機器人控制模塊進行重寫，從而驗證自己的控制算法.

4 機器人倒立擺控制實驗

單純的倒立擺控制算法這些年已經(jīng)發(fā)展得十分成熟.但是，用六自由度機器人實現(xiàn)穩(wěn)定控制倒立擺的研究還很少有人涉及.機器人倒立擺控制結(jié)合了動力學、機器人學、數(shù)據(jù)插補、控制工程等學科，在機器人教學方面有著重要意義.同時，該實驗也對機器人系統(tǒng)的穩(wěn)定性和平衡性研究有積極作用.

本文以機器人倒立擺的控制實驗為例，介紹了開放式機器人教學系統(tǒng)的應用.該實驗的目的，是通過實現(xiàn)最優(yōu)控制算法在機器人倒立擺實驗中的實際應用，培養(yǎng)學生設計、開發(fā)和驗證機器人控制算法的能力，證明本文所提出的開放式機器人教學系統(tǒng)具有教學適用性、開放性、可擴展性和友好的人機交互等特性.

圖3 機器人倒立擺控制系統(tǒng)結(jié)構(gòu)框圖

需要指出的是，機器人控制一般需要較高的控制頻率，而倒立擺的控制頻率可以低一些.鑒于倒立擺編碼器的分辨率較低，倒立擺也沒有必要采用更高的控制頻率.因此，倒立擺與機器人可以采用不同的控制頻率來保證較好的控制效果.由于機器人采用了比倒立擺更高的控制頻率，所以需要對倒立擺的輸出控制量進行插補再驅(qū)動機器人.

4.1 倒立擺控制算法實現(xiàn)

4.1.1 單級倒立擺的數(shù)學模型

倒立擺系統(tǒng)是一個典型的不穩(wěn)定、多變量和非線性的被控對象,其控制算法可應用于軍工、航天、行走機器人等領(lǐng)域[2].因此，對倒立擺系統(tǒng)的算法研究具有深遠意義.PID控制簡單、穩(wěn)定性好，但對于倒立擺這樣非線性、多變量、強耦合及參數(shù)不確定的控制系統(tǒng)，控制效果不是很理想[3-6].

線性二次型調(diào)節(jié)器(Linear Quadratic Regulator,LQR)是基于狀態(tài)空間方法設計的動態(tài)控制器，其性能指標易于分析、處理和計算，便于工程實現(xiàn).被廣泛應用于機器人控制[7]、導彈控制[8]等工程領(lǐng)域.

單級倒立擺系統(tǒng)可抽象為小車與擺桿構(gòu)成的系統(tǒng)，設小車質(zhì)量為M，擺桿質(zhì)量為m，擺桿質(zhì)心位置為l，小車移動位移為x，擺桿的偏角為θ，小車上的外力為F.

根據(jù)牛頓力學原理，可得運動方程為：

(1)

(2)

式(2)中，y為輸出矩陣；μ為控制輸入矩陣.

4.1.2 單級倒立擺的數(shù)學模型

對于倒立擺系統(tǒng)的狀態(tài)方程，可以通過最優(yōu)控制率得出

μ=-Kz

(3)

(4)

式(4)中，Q,R為加權(quán)矩陣，是用來平衡狀態(tài)變量和輸入向量的權(quán)重，它們決定了系統(tǒng)誤差與控制能量消耗的相對重要性.

由最優(yōu)控制理論可知，使線性二次型最優(yōu)控制指標J取得最小值的線性最優(yōu)反饋增益矩陣為：

K=R-1BTP

(5)

則

μ(t)=-R-1BTP(t)x(t)

(6)

其中，P為黎卡提方程的解.P可由黎卡提方程P(t)A+ATP(t)-P(t)BR-1BTP(t)+Q=0解得[10].

我們使用MATLAB中的求解語句lqr()來求取最優(yōu)反饋增益矩陣：

[K,P]=lqr(A,B,Q,R)

在MATLAB中求解lqr(A,B,Q,R)需要選擇參數(shù)Q，R，其中Q1,1代表小車位置的權(quán)重，Q3,3代表擺桿角度的權(quán)重.

我們通過反饋的控制效果的好壞不斷改變參數(shù)Q,R的取值，最終確定了Q,R的取值，并通過MATLAB求得K值.

4.2 機器人目標位置解算

機器人倒立擺控制系統(tǒng)中機器人主要負責帶動倒立擺水平移動，準確執(zhí)行倒立擺系統(tǒng)的期望輸入，相當于一個線性伺服裝置.由于機器人具有更高的采樣頻率，需要對倒立擺的期望輸入進行插補.同時因為機器人采用位置控制，只能獲得機器人末端法蘭盤移動速度，需要得到倒立擺期望的控制輸入和機器人末端移動速度之間的換算關(guān)系并對末端速度進行積分以獲得位置偏移量.該位置偏移量為機器人末端位移，為了驅(qū)動機器人運動還需要利用機器人逆運動學方程求得關(guān)節(jié)上的位置偏移量.機器人控制流程圖如圖4所示.

圖4 機器人控制流程圖

為了保證機器人運動的平滑化，需要對倒立擺的期望輸入進行插補，并依次執(zhí)行插補點.由于倒立擺的期望輸入為施加在倒立擺上的控制力，保證了該控制力的連續(xù)即可保證機器人末端運動軌跡的加速度連續(xù).倒立擺控制周期與機器人控制周期的關(guān)系示意圖，如圖5所示.

圖5 倒立擺控制周期與機器人控制周期關(guān)系

考慮機器人運動的平滑性要求，本系統(tǒng)采用了五次多項式對倒立擺的期望輸入μ進行插補.機器人的控制量是機器人末端法蘭盤的移動速度.經(jīng)過多次試驗與驗證，我們發(fā)現(xiàn)機器人法蘭盤的移動速度v和倒立擺的輸入力矩μ之間成正比關(guān)系，其比例系數(shù)為1.對倒立擺的期望輸入進行插補即可轉(zhuǎn)換成對機器人末端法蘭盤的速度進行插補.五次多項式表達式為：

v(t)=a0+a1t+a2t2+a3t3+a4t4+a5t5

(7)

(8)

對插補的控制量進行積分，獲取機器人期望的末端位移，從而獲得期望的末端位置.再利用機器人的逆運動學方程便可獲取機器人目標關(guān)節(jié)位置.進而實現(xiàn)機器人以倒立擺期望的控制力驅(qū)動倒立擺.

5 實驗測試

本實現(xiàn)系統(tǒng)的實驗過程如下：

1)建立MATLAB/Simulink 實驗工程，并拖拽相關(guān)模塊搭建完整的機器人倒立擺閉環(huán)控制系統(tǒng).

2)通過相關(guān)接口分別設置倒立擺和機器人的控制周期.

3)在倒立擺控制模塊中實現(xiàn)倒立擺控制算法，或直接調(diào)用系統(tǒng)提供的底層API接口.如LQR、PID等.

4)在機器人模塊中實現(xiàn)目標位置的差補與目標位姿的逆解，或直接調(diào)用系統(tǒng)提供的底層API接口.

5)數(shù)據(jù)采集與分析，確定倒立擺或機器人的控制算法性能.

圖6 倒立擺運行數(shù)據(jù)

如圖6所示，在1s時加入外部擾動，機器人以倒立擺期望的控制力矩驅(qū)動倒立擺，在5s左右使倒立擺恢復平衡，回到初始位置.

機器人倒立擺控制效果圖如圖7所示.圖7(a)是在穩(wěn)定狀態(tài)下的機器人倒立擺系統(tǒng).圖7(b)時對倒立擺施加一個推力進行干擾.倒立擺受到干擾后偏離平衡位置，如圖7(c)所示.隨后，機器人通過控制法蘭盤水平移動將倒立擺重新移動到平衡點處，如圖7(d)所示.到圖7(e)時，機器人倒立擺系統(tǒng)已經(jīng)重新恢復到穩(wěn)定狀態(tài).

圖7 倒立擺控制效果圖

實驗證明，機器人倒立擺系統(tǒng)具有一定的抗干擾性.受到擾動偏離平衡點時能夠穩(wěn)定的收斂回平衡點.該系統(tǒng)驗證了最優(yōu)控制算法用于機器人倒立擺控制系統(tǒng)的可行性，，從而證明，本文所提出的開放式機器人教學系統(tǒng)能夠幫助學生在實際應用層面進行算法驗證與改進，提高學生的開發(fā)能力與動手能力.

6 總 結(jié)

本文介紹了一種可用于機器人算法驗證的開放式教學系統(tǒng).并以機器人倒立擺實驗為例，對該教學系統(tǒng)進行了具體應用.相比于傳統(tǒng)的封閉式工業(yè)機器人控制系統(tǒng)，開放式的機器人控制系統(tǒng)能夠幫助學生將所學到的專業(yè)知識應用到具體的工程項目中，具有更廣泛的應用空間.在該系統(tǒng)的幫助下，學生可以通過理論與實踐結(jié)合的實驗課程提高自己的專業(yè)素質(zhì)與實際能力.