基于QNX-Simulink的機器人創新實驗平臺研究

王新慶, 陳兆芃, 王冬劍, 潘 軍, 姜 力

(1. 中國石油大學(華東) 機電工程學院, 山東 青島 266580；2. 哈爾濱工業大學 機電工程學院, 黑龍江 哈爾濱 150001)

基于QNX-Simulink的機器人創新實驗平臺研究

王新慶1, 陳兆芃2, 王冬劍2, 潘 軍2, 姜 力2

(1. 中國石油大學(華東) 機電工程學院, 山東 青島 266580；2. 哈爾濱工業大學 機電工程學院, 黑龍江 哈爾濱 150001)

設計了一種基于QNX-Simulink的機器人實時控制平臺。該平臺采用基于模型的設計方法,通過Simulink進行機器人系統的可視化建模及控制算法編程；經過二次開發Matlab/RTW工具箱自動生成QNX控制代碼；以TCP/IP方式進行數據通信,實現對機器人的實時控制。以靈巧手控制平臺設計及其位置控制為例,該平臺編程簡單、調試方便、可移植性較好,仿真結果與實驗結果誤差趨勢一致,證明了該平臺的可行性。

計算機仿真； 機器人靈巧手； 實驗平臺； 實時控制

1 機器人控制平臺設計方法簡析

機器人學是一門交叉學科,圍繞機器人本體結構、控制平臺及控制算法的研究都有了巨大的進步。其中控制平臺作為算法實現的載體,需要滿足數據傳輸實時可靠、控制系統設計方便、用戶界面友好、便于擴展與維護的要求。

傳統的控制平臺主要采用自上而下分層控制的設計方法。例如哈工大/德宇航聯合開發的HIT/DLR Platform平臺分為3層,底層為基于FPGA的控制執行層,中間層為基于DSP的控制計算層,頂層為基于PC機的人機交互層[1]；NASA靈巧手采用了上層Sparc 10工作站,底層FPGA+DSP的控制方式[2]。上述研究中各平臺運行不同的軟件系統,缺乏統一的接口,程序的可移植性較差,且主要依賴于文本編程,對復雜算法無能為力且調試不便。

與分層設計方法不同,基于模型的設計方法解決了各軟件系統的接口不統一的問題,且具有代碼自動生成功能,能夠顯著降低編程的難度、提高編程效率,在科研及工業中得到了廣泛應用。例如文獻[3]在飛行控制仿真系統中采用了基于RTW和VxWorks的設計方法,文獻[4]在飛機制造中采用了基于RTW的設計方法。這些方法的核心在于微內核實時操作系統的應用,例如VxWorks以及加拿大QSSL公司的QNX[5]。其中QNX系統具有強大的多機通信及資源共享功能,其微內核具有硬實時性,能夠滿足實時控制的要求。因此,本文結合Simulink強大的可視化算法編程及代碼生成功能,搭建一套模塊化的高效實時控制平臺,既可應用于教學,又能用作實踐創新。

2 基于QNX的實時控制平臺

2.1 平臺的組成及其功能

實時控制平臺由2臺PC機及靈巧手平臺組成(見圖1)。

(1) PC機A。它是Simulink的子平臺,硬件包括1個網卡,軟件包括基于Windows系統的Matlab/Simulink及QNX編譯器,具有可視化的編程及數據處理/顯示功能。用戶首先利用Simulink內建的SimMechanic模塊建立機器人的運動及驅動等模型,繼而利用Simulink的可視化圖形編程方法編寫控制算法并進行仿真。如果仿真結果可以滿足控制要求,則調用Real-Time Workshop(RTW)及QNX編譯器生成QNX可執行文件。該文件包含的控制命令通過TCP/IP協議傳輸到QNX子平臺,同時接收來自QNX的反饋信息。

(2) PC機B。它是QNX的子平臺,安裝有2個網卡。由于靈巧手內部集成了多達20個位置/力矩傳感器,數據傳輸量很大,因而基于控制的實時性要求,2個網卡的傳輸速率均要求100 Mbit/s以上；在軟件上安裝有QNX Neutrino RTOS系統[6]。PC機A生成的QNX可執行文件通過一根交叉接法的網線傳輸到PC機B的其中一個網卡,其通信協議為TCP/IP,控制指令經過運算后通過UDP協議發送到靈巧手,并接收來自靈巧手的傳感器信息。

(3) 靈巧手平臺。它是配備有網絡接口模塊的靈巧手子平臺,其主控芯片為FPGA。PC機B生成的控制指令通過正常連接的網線傳輸到靈巧手,靈巧手執行控制指令并實時回傳各傳感器的數據。該數據既通過UDP通信協議傳到PC機B,又傳到靈巧手的控制器以形成控制閉環。傳感器數據最終回傳到Simulink平臺,便于使用者處理數據、調試控制算法。

圖1 QNX-Simulink控制系統原理

2.2 Simulink子平臺的設計

Matlab/Simulink的SimMechanics模塊提供了大量對應于實際元件的單元體,能夠直觀地建立多種剛體動力學模型。以靈巧手建模為例,在其物理模型的基礎上,通過編制子程序的方式將靈巧手各模型進行封裝,建立了控制模型庫(見圖2),主要模型庫介紹如下。

圖2 靈巧手Simulink模型

(1) 無刷直流電機驅動模型(BLDCM)。靈巧手各關節均采用直流無刷電機驅動,電機轉子的轉動慣量很小,在實際的建模中予以忽略。采用二階系統描述電機的轉矩-電流關系[7]。

(2) 單手指的正/逆運動學模型(LFingers)。靈巧手手指的實物及簡化模型見圖3,手指具有3個回轉關節,簡化為三連桿串聯機器人,根據D-H方法建立如圖3(b)所示的坐標系[8],通過坐標變換可以求得手指的正向運動學模型。

(3) 整手的動力學模型(Hand Mechanics)。動力學模型采用SimMechanics建立。首先確定各指節長度尺寸,然后根據靈巧手的PRO/E模型測得各指

圖3 靈巧手手指實物及簡化模型

節的質心位置、繞質心的轉動慣量等參數,輸入到Simulink中的“Rigid body”模塊實現各桿件的建模,通過“Revolute”模塊定義各關節之間的轉動關系,通過“Joint Actuator”模塊定義關節驅動力矩。根據靈巧手的結構在相應的關節模型處添加位置/力矩傳感器(見圖4)。為了進一步實現QNX系統中模型的運動可視化,本文將靈巧手在PRO/E中的三維實體模型進行簡化,抽取其“殼”特征,轉換為“*.iv”文件,以節點方式導入Open Inventor中,按照各零件裝配“父子”關系依次裝配。當靈巧手在Simulink平臺中進行仿真時,各關節的角度參數將傳輸到QNX系統中驅動仿真模型運動,從而實現仿真的可視化。

圖4 手指的動力學模型

(4) PWM控制仿真模型(PWM control simmodel)。由于實際的靈巧手采用PWM控制,因此本文以PWM信號、關節力矩、期望角度為輸入變量,以實際關節角度、角速度為輸出,建立了各手指的PWM仿真模型,仿真周期根據靈巧手的實際控制周期確定。

(5) API模型(Operate API)。API模型主要用于建立Simulink與靈巧手之間的信息通道,包括系統上電自檢、通信狀態的監測、位置霍爾信號/力矩傳感器信號的采集等,從而能夠完成對系統硬件資源的訪問。

2.3 QNX子平臺的設計

QNX子平臺基于QNX Neutrino RTOS,RTOS通過標準API的方式將系統的I/O、定時器等各種資源封裝起來。該平臺通過分布式通信以支持不同的系統間通信。其中該平臺采用CPU空閑執行的方式與Simulink子平臺進行通信,主要是由于Simulink與QNX之間傳輸的用戶指令及控制參數傳輸量很小,不需要很強的實時性；該平臺與靈巧手平臺之間的通信采用定時器中斷的方式,以保證發送控制命令與接收靈巧手傳感器數據之間的實時性。

2.4 靈巧手子平臺的設計

靈巧手子平臺為基于FPGA的嵌入式操作系統,完全集成于靈巧手的機械本體內。主控芯片采用Altera公司的Cyclone II,通過2個NIOS軟核處理器分別實現Ethernet通信與實時控制。其中Ethernet通信部分不需要硬實時通信,采用了VHDL語言及IP核的方式實現,即控制層MAC由IP核實現,通過MII接口輸入到以太網控制芯片DP83848中,再通過網絡變壓器J1102轉換后輸出(見圖5)。由于存儲空間的限制,完整的TCP/IP協議很難集成到嵌入式系統中,本文采用了精簡版的UDP協議來完成QNX平臺與靈巧手平臺的通信[9]。

該UDP接口主要實現兩方面的功能:(1) 接收QNX平臺發送的控制參數,通過FPGA內置的RAM傳輸到實時控制NIOS核,如圖5所示；(2) 讀取靈巧手位置/力矩傳感器信號,經過預處理后通過TCP/IP通信回傳到QNX子平臺,如圖1所示。

圖5 通信系統硬件組成

3 QNX模型生成

原則上,Simulink的RTW工具箱具備自動生成C代碼的功能,QNX系統也支持ANSI C代碼,兩者具備統一的接口文件。但是RTW生成的C代碼無法直接應用于其他編譯系統,因此本文對RTW及QNX編譯器部分進行了二次開發,實現控制算法經由RTW直接生成QNX可執行文件的功能。代碼生成流程如圖6所示,其中有2個文件需要編寫:

(1) Simulink的目標語言模板文件(*.tmf)；

(2) makefile(*.mk)文件,在該文件中需要指定編譯器,對程序編譯進行指導以及生成的模型源代碼進行鏈接。

通過上述二次開發,將Simulink與QNX整合為一體,實現了仿真與控制的無縫連接。

圖6 基于模型的代碼生成流程圖

4 位置控制實驗及其結果

采用機器人技術與系統國家重點實驗室研制的三關節串聯機器手指進行了笛卡爾空間的位置控制實驗,系統Simulink仿真控制模型如圖7所示。其中Traj_Cartersian為笛卡爾空間的軌跡規劃模塊,x、y、z為期望的笛卡爾位置,JointPos1為期望的關節角度,BLDC為無刷直流電機驅動模型,HIT/DLR HAND為靈巧手的動力學模型。

指尖笛卡爾軌跡為一個圓,根據微分逆運動學方法[10]可以求得任意指尖位置對應的一組關節角度θ1、θ2、θ3。進一步采用Paul多項式軌跡規劃方法得到各關節的期望軌跡[11],其中插值時間為1 s,加速度時間為0.2 s。

控制算法采用基于PI的關節間交叉耦合控制方法[12],手指基關節、中關節以及末關節比例系數Kp分別為318.25、505.4、519.65,積分系數Ki分別為16.75、26.6、27.35。考慮到代碼的自動生成,在算法仿真過程中需要將求解器步長設定為固定,因為QNX編譯器不支持可變步長。

仿真及實驗結果對比如圖8所示。通過對比可以看出,Matlab仿真與實驗在各方向上的誤差變化趨勢具有較高的一致性,說明機械手各模型的建立以及PI控制參數的調整基本正確。理論上如果指尖的笛卡爾位置是可檢測的,則可以通過調整參數使指尖的跟蹤精度保證在任意精度范圍內。但實際上,目前的機器人系統還很難做到這一點。

圖7 靈巧手位置控制模型

圖8 手指位置控制結果

5 結束語

基于QNX-Simulink的機器人實時控制平臺與其他基于文本編程的實驗平臺相比具有以下優勢:充分利用了Simulink強大的圖形化建模功能,在進行實驗之前可以對算法進行仿真,在通過仿真驗證后整個仿真模型可以直接移植到實時控制平臺中進行實驗,避免了對算法進行繁瑣的重復編程；采用QNX微內核實時操作系統具有硬實時性、高穩定性和高可靠性,可以有效保證系統的實時性；具有良好的可讀性,方便算法的調式及移植。

References)

[1] Haidacher S,Butterfass J,Fischer M,et al.DLR Hand II:Hard- and Software Architecture for Information Processing[C]// Proceedings of the 2003 International Conference on Robotics and Automation. 2003:684-689.

[2] Lovchik C S,Difler M A.The Robonaut Hand:A Dextrous Robotic Hand for Space[C]//Proceedings of the 1999 IEEE International Conference on Robotics and Automation.Detroit,1999:907-912.

[3] 孔繁峨,陳宗基.基于RTW和VxWorks的飛行控制系統實時仿真[J].系統仿真學報,2007,19(11):2455-2457.

[4] The MathWorks Inc.MATLAB and Simulink Help Land Unpiloted Boeing Spacecraft [EB/OL].[2014-11-06].http://cn.mathworks.com/company/user_stories/matlab-and-simulink-help-land-unpiloted-boeing-spacecraft.html.

[5] Jensen E D.Hard and Soft Real-Time[EB/OL].(2010-02-18)[2014-12-26].http://www.real-time.org/hard and soft real time.htm.

[6] 陳兆芃.具有網絡功能的靈巧手嵌入式控制系統設計[D].哈爾濱:哈爾濱工業大學,2007.

[7] 何平.基于物體阻抗的機器人靈巧手控制[D].哈爾濱:哈爾濱工業大學,2004.

[8] Montana D J.The Kinematics of Multifingered Manipulation [J].IEEE Transactions on Robotics and Automation,1999,11(4):491-503.

[9] Chen Z P,Lii N Y,Wu K,et al.Flexible FPGA-based Controller Architecture for Five-Fingered Dexterous Robot Hand with Effective Impedance Control[C]//IEEE International Conference on Robotics and Biomimetics. 2009(9):1063 -1068.

[10] 何平,高曉輝,楊磊,等.HIT/DLR多指手抓取操作研究[J].哈爾濱工業大學學報,2005,37(11)：1555-1559.

[11] Paul R P.機器人操作手:數字、編程與控制[M].鄭時雄,謝存禧,譯.北京:機械工業出版社,1991:130-134.

[12] Sun D, Shao X Y, Feng G A. Model-free Cross-coupled Control for Position Synchronization of Multi-axis Motions:Theory and Experiments[J].IEEE Transactions on Control Systems Technology,2007,15(2):306-313.

Research on innovative experiment platform of robot based on QNX-Simulink

Wang Xinqing1, Chen Zhaopeng2, Wang Dongjian2, Pan Jun2, Jiang Li2

(1. College of Mechanical and Electronic Engineering ,China University of Petroleum (East), Qingdao 266580,China; 2. College of Mechanical and Electronic Engineering,Harbin Institute of Technology, Harbin 150001, China)

Due to the problems of difficult coding,inconvenient debugging and poor portability of currently existing robot experiment platform,a real-time control platform is designed using QNX-Simulink.The model-based design method is utilized,the robot model and control algorithm are visually designed by Simulink; the Matlab/RTW toolbox is partially modified in order to generate the control code automatically; and the TCP/IP protocol is adapted to realize data communication.Taking the platform design and control of a dexterous robot hand as an example, this paper compares the simulation result with the experimentation result,the trend of the error has consistence,which has proved the feasibility of the platform.

computer simulation; robot hand; experiment platform; real-time control

2014- 11- 11

國家自然科學青年基金項目(51305460)；中央高校基本科研業務費專項資金資助項目(14CX02068A)；中國石油大學教學改革重點項目(JY-A201408)

王新慶(1982—),男,山東萊蕪,博士,講師,主要研究方向為機器人技術.

E-mail：thinkpalm@126.com

G42

A

1002-4956(2015)7- 0139- 05