一種機器人倒立擺實驗系統的研究

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

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

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

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

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

E-mail：hanbing@sia.cn

1 引 言

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

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

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

2 實驗平臺總體組成

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

圖1 實驗平臺構造圖

3 教學系統軟件架構

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

3.1 實驗系統開發接口

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

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

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

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

3.2 實驗系統接口模塊

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

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

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

4 機器人倒立擺控制實驗

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

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

圖3 機器人倒立擺控制系統結構框圖

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

4.1 倒立擺控制算法實現

4.1.1 單級倒立擺的數學模型

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

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

單級倒立擺系統可抽象為小車與擺桿構成的系統，設小車質量為M，擺桿質量為m，擺桿質心位置為l，小車移動位移為x，擺桿的偏角為θ，小車上的外力為F.

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

(1)

(2)

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

4.1.2 單級倒立擺的數學模型

對于倒立擺系統的狀態方程，可以通過最優控制率得出

μ=-Kz

(3)

(4)

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

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

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()來求取最優反饋增益矩陣：

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

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

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

4.2 機器人目標位置解算

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

圖4 機器人控制流程圖

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

圖5 倒立擺控制周期與機器人控制周期關系

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

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

(7)

(8)

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

5 實驗測試

本實現系統的實驗過程如下：

1)建立MATLAB/Simulink 實驗工程，并拖拽相關模塊搭建完整的機器人倒立擺閉環控制系統.

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

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

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

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

圖6 倒立擺運行數據

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

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

圖7 倒立擺控制效果圖

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

6 總 結

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