基于QNX-Simulink的機(jī)器人創(chuàng)新實(shí)驗(yàn)平臺(tái)研究

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

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

基于QNX-Simulink的機(jī)器人創(chuàng)新實(shí)驗(yàn)平臺(tái)研究

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

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

設(shè)計(jì)了一種基于QNX-Simulink的機(jī)器人實(shí)時(shí)控制平臺(tái)。該平臺(tái)采用基于模型的設(shè)計(jì)方法,通過(guò)Simulink進(jìn)行機(jī)器人系統(tǒng)的可視化建模及控制算法編程；經(jīng)過(guò)二次開(kāi)發(fā)Matlab/RTW工具箱自動(dòng)生成QNX控制代碼；以TCP/IP方式進(jìn)行數(shù)據(jù)通信,實(shí)現(xiàn)對(duì)機(jī)器人的實(shí)時(shí)控制。以靈巧手控制平臺(tái)設(shè)計(jì)及其位置控制為例,該平臺(tái)編程簡(jiǎn)單、調(diào)試方便、可移植性較好,仿真結(jié)果與實(shí)驗(yàn)結(jié)果誤差趨勢(shì)一致,證明了該平臺(tái)的可行性。

計(jì)算機(jī)仿真； 機(jī)器人靈巧手； 實(shí)驗(yàn)平臺(tái)； 實(shí)時(shí)控制

1 機(jī)器人控制平臺(tái)設(shè)計(jì)方法簡(jiǎn)析

機(jī)器人學(xué)是一門交叉學(xué)科,圍繞機(jī)器人本體結(jié)構(gòu)、控制平臺(tái)及控制算法的研究都有了巨大的進(jìn)步。其中控制平臺(tái)作為算法實(shí)現(xiàn)的載體,需要滿足數(shù)據(jù)傳輸實(shí)時(shí)可靠、控制系統(tǒng)設(shè)計(jì)方便、用戶界面友好、便于擴(kuò)展與維護(hù)的要求。

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

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

2 基于QNX的實(shí)時(shí)控制平臺(tái)

2.1 平臺(tái)的組成及其功能

實(shí)時(shí)控制平臺(tái)由2臺(tái)PC機(jī)及靈巧手平臺(tái)組成(見(jiàn)圖1)。

(1) PC機(jī)A。它是Simulink的子平臺(tái),硬件包括1個(gè)網(wǎng)卡,軟件包括基于Windows系統(tǒng)的Matlab/Simulink及QNX編譯器,具有可視化的編程及數(shù)據(jù)處理/顯示功能。用戶首先利用Simulink內(nèi)建的SimMechanic模塊建立機(jī)器人的運(yùn)動(dòng)及驅(qū)動(dòng)等模型,繼而利用Simulink的可視化圖形編程方法編寫控制算法并進(jìn)行仿真。如果仿真結(jié)果可以滿足控制要求,則調(diào)用Real-Time Workshop(RTW)及QNX編譯器生成QNX可執(zhí)行文件。該文件包含的控制命令通過(guò)TCP/IP協(xié)議傳輸?shù)絈NX子平臺(tái),同時(shí)接收來(lái)自QNX的反饋信息。

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

(3) 靈巧手平臺(tái)。它是配備有網(wǎng)絡(luò)接口模塊的靈巧手子平臺(tái),其主控芯片為FPGA。PC機(jī)B生成的控制指令通過(guò)正常連接的網(wǎng)線傳輸?shù)届`巧手,靈巧手執(zhí)行控制指令并實(shí)時(shí)回傳各傳感器的數(shù)據(jù)。該數(shù)據(jù)既通過(guò)UDP通信協(xié)議傳到PC機(jī)B,又傳到靈巧手的控制器以形成控制閉環(huán)。傳感器數(shù)據(jù)最終回傳到Simulink平臺(tái),便于使用者處理數(shù)據(jù)、調(diào)試控制算法。

圖1 QNX-Simulink控制系統(tǒng)原理

2.2 Simulink子平臺(tái)的設(shè)計(jì)

Matlab/Simulink的SimMechanics模塊提供了大量對(duì)應(yīng)于實(shí)際元件的單元體,能夠直觀地建立多種剛體動(dòng)力學(xué)模型。以靈巧手建模為例,在其物理模型的基礎(chǔ)上,通過(guò)編制子程序的方式將靈巧手各模型進(jìn)行封裝,建立了控制模型庫(kù)(見(jiàn)圖2),主要模型庫(kù)介紹如下。

圖2 靈巧手Simulink模型

(1) 無(wú)刷直流電機(jī)驅(qū)動(dòng)模型(BLDCM)。靈巧手各關(guān)節(jié)均采用直流無(wú)刷電機(jī)驅(qū)動(dòng),電機(jī)轉(zhuǎn)子的轉(zhuǎn)動(dòng)慣量很小,在實(shí)際的建模中予以忽略。采用二階系統(tǒng)描述電機(jī)的轉(zhuǎn)矩-電流關(guān)系[7]。

(2) 單手指的正/逆運(yùn)動(dòng)學(xué)模型(LFingers)。靈巧手手指的實(shí)物及簡(jiǎn)化模型見(jiàn)圖3,手指具有3個(gè)回轉(zhuǎn)關(guān)節(jié),簡(jiǎn)化為三連桿串聯(lián)機(jī)器人,根據(jù)D-H方法建立如圖3(b)所示的坐標(biāo)系[8],通過(guò)坐標(biāo)變換可以求得手指的正向運(yùn)動(dòng)學(xué)模型。

(3) 整手的動(dòng)力學(xué)模型(Hand Mechanics)。動(dòng)力學(xué)模型采用SimMechanics建立。首先確定各指節(jié)長(zhǎng)度尺寸,然后根據(jù)靈巧手的PRO/E模型測(cè)得各指

圖3 靈巧手手指實(shí)物及簡(jiǎn)化模型

節(jié)的質(zhì)心位置、繞質(zhì)心的轉(zhuǎn)動(dòng)慣量等參數(shù),輸入到Simulink中的“Rigid body”模塊實(shí)現(xiàn)各桿件的建模,通過(guò)“Revolute”模塊定義各關(guān)節(jié)之間的轉(zhuǎn)動(dòng)關(guān)系,通過(guò)“Joint Actuator”模塊定義關(guān)節(jié)驅(qū)動(dòng)力矩。根據(jù)靈巧手的結(jié)構(gòu)在相應(yīng)的關(guān)節(jié)模型處添加位置/力矩傳感器(見(jiàn)圖4)。為了進(jìn)一步實(shí)現(xiàn)QNX系統(tǒng)中模型的運(yùn)動(dòng)可視化,本文將靈巧手在PRO/E中的三維實(shí)體模型進(jìn)行簡(jiǎn)化,抽取其“殼”特征,轉(zhuǎn)換為“*.iv”文件,以節(jié)點(diǎn)方式導(dǎo)入Open Inventor中,按照各零件裝配“父子”關(guān)系依次裝配。當(dāng)靈巧手在Simulink平臺(tái)中進(jìn)行仿真時(shí),各關(guān)節(jié)的角度參數(shù)將傳輸?shù)絈NX系統(tǒng)中驅(qū)動(dòng)仿真模型運(yùn)動(dòng),從而實(shí)現(xiàn)仿真的可視化。

圖4 手指的動(dòng)力學(xué)模型

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

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

2.3 QNX子平臺(tái)的設(shè)計(jì)

QNX子平臺(tái)基于QNX Neutrino RTOS,RTOS通過(guò)標(biāo)準(zhǔn)API的方式將系統(tǒng)的I/O、定時(shí)器等各種資源封裝起來(lái)。該平臺(tái)通過(guò)分布式通信以支持不同的系統(tǒng)間通信。其中該平臺(tái)采用CPU空閑執(zhí)行的方式與Simulink子平臺(tái)進(jìn)行通信,主要是由于Simulink與QNX之間傳輸?shù)挠脩糁噶罴翱刂茀?shù)傳輸量很小,不需要很強(qiáng)的實(shí)時(shí)性；該平臺(tái)與靈巧手平臺(tái)之間的通信采用定時(shí)器中斷的方式,以保證發(fā)送控制命令與接收靈巧手傳感器數(shù)據(jù)之間的實(shí)時(shí)性。

2.4 靈巧手子平臺(tái)的設(shè)計(jì)

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

該UDP接口主要實(shí)現(xiàn)兩方面的功能:(1) 接收QNX平臺(tái)發(fā)送的控制參數(shù),通過(guò)FPGA內(nèi)置的RAM傳輸?shù)綄?shí)時(shí)控制NIOS核,如圖5所示；(2) 讀取靈巧手位置/力矩傳感器信號(hào),經(jīng)過(guò)預(yù)處理后通過(guò)TCP/IP通信回傳到QNX子平臺(tái),如圖1所示。

圖5 通信系統(tǒng)硬件組成

3 QNX模型生成

原則上,Simulink的RTW工具箱具備自動(dòng)生成C代碼的功能,QNX系統(tǒng)也支持ANSI C代碼,兩者具備統(tǒng)一的接口文件。但是RTW生成的C代碼無(wú)法直接應(yīng)用于其他編譯系統(tǒng),因此本文對(duì)RTW及QNX編譯器部分進(jìn)行了二次開(kāi)發(fā),實(shí)現(xiàn)控制算法經(jīng)由RTW直接生成QNX可執(zhí)行文件的功能。代碼生成流程如圖6所示,其中有2個(gè)文件需要編寫:

(1) Simulink的目標(biāo)語(yǔ)言模板文件(*.tmf)；

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

通過(guò)上述二次開(kāi)發(fā),將Simulink與QNX整合為一體,實(shí)現(xiàn)了仿真與控制的無(wú)縫連接。

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

4 位置控制實(shí)驗(yàn)及其結(jié)果

采用機(jī)器人技術(shù)與系統(tǒng)國(guó)家重點(diǎn)實(shí)驗(yàn)室研制的三關(guān)節(jié)串聯(lián)機(jī)器手指進(jìn)行了笛卡爾空間的位置控制實(shí)驗(yàn),系統(tǒng)Simulink仿真控制模型如圖7所示。其中Traj_Cartersian為笛卡爾空間的軌跡規(guī)劃模塊,x、y、z為期望的笛卡爾位置,JointPos1為期望的關(guān)節(jié)角度,BLDC為無(wú)刷直流電機(jī)驅(qū)動(dòng)模型,HIT/DLR HAND為靈巧手的動(dòng)力學(xué)模型。

指尖笛卡爾軌跡為一個(gè)圓,根據(jù)微分逆運(yùn)動(dòng)學(xué)方法[10]可以求得任意指尖位置對(duì)應(yīng)的一組關(guān)節(jié)角度θ1、θ2、θ3。進(jìn)一步采用Paul多項(xiàng)式軌跡規(guī)劃方法得到各關(guān)節(jié)的期望軌跡[11],其中插值時(shí)間為1 s,加速度時(shí)間為0.2 s。

控制算法采用基于PI的關(guān)節(jié)間交叉耦合控制方法[12],手指基關(guān)節(jié)、中關(guān)節(jié)以及末關(guān)節(jié)比例系數(shù)Kp分別為318.25、505.4、519.65,積分系數(shù)Ki分別為16.75、26.6、27.35?？紤]到代碼的自動(dòng)生成,在算法仿真過(guò)程中需要將求解器步長(zhǎng)設(shè)定為固定,因?yàn)镼NX編譯器不支持可變步長(zhǎng)。

仿真及實(shí)驗(yàn)結(jié)果對(duì)比如圖8所示。通過(guò)對(duì)比可以看出,Matlab仿真與實(shí)驗(yàn)在各方向上的誤差變化趨勢(shì)具有較高的一致性,說(shuō)明機(jī)械手各模型的建立以及PI控制參數(shù)的調(diào)整基本正確。理論上如果指尖的笛卡爾位置是可檢測(cè)的,則可以通過(guò)調(diào)整參數(shù)使指尖的跟蹤精度保證在任意精度范圍內(nèi)。但實(shí)際上,目前的機(jī)器人系統(tǒng)還很難做到這一點(diǎn)。

圖7 靈巧手位置控制模型

圖8 手指位置控制結(jié)果

5 結(jié)束語(yǔ)

基于QNX-Simulink的機(jī)器人實(shí)時(shí)控制平臺(tái)與其他基于文本編程的實(shí)驗(yàn)平臺(tái)相比具有以下優(yōu)勢(shì):充分利用了Simulink強(qiáng)大的圖形化建模功能,在進(jìn)行實(shí)驗(yàn)之前可以對(duì)算法進(jìn)行仿真,在通過(guò)仿真驗(yàn)證后整個(gè)仿真模型可以直接移植到實(shí)時(shí)控制平臺(tái)中進(jìn)行實(shí)驗(yàn),避免了對(duì)算法進(jìn)行繁瑣的重復(fù)編程；采用QNX微內(nèi)核實(shí)時(shí)操作系統(tǒng)具有硬實(shí)時(shí)性、高穩(wěn)定性和高可靠性,可以有效保證系統(tǒng)的實(shí)時(shí)性；具有良好的可讀性,方便算法的調(diào)式及移植。

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的飛行控制系統(tǒng)實(shí)時(shí)仿真[J].系統(tǒng)仿真學(xué)報(bào),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] 陳兆芃.具有網(wǎng)絡(luò)功能的靈巧手嵌入式控制系統(tǒng)設(shè)計(jì)[D].哈爾濱:哈爾濱工業(yè)大學(xué),2007.

[7] 何平.基于物體阻抗的機(jī)器人靈巧手控制[D].哈爾濱:哈爾濱工業(yè)大學(xué),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].哈爾濱工業(yè)大學(xué)學(xué)報(bào),2005,37(11)：1555-1559.

[11] Paul R P.機(jī)器人操作手:數(shù)字、編程與控制[M].鄭時(shí)雄,謝存禧,譯.北京:機(jī)械工業(yè)出版社,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

國(guó)家自然科學(xué)青年基金項(xiàng)目(51305460)；中央高?；究蒲袠I(yè)務(wù)費(fèi)專項(xiàng)資金資助項(xiàng)目(14CX02068A)；中國(guó)石油大學(xué)教學(xué)改革重點(diǎn)項(xiàng)目(JY-A201408)

王新慶(1982—),男,山東萊蕪,博士,講師,主要研究方向?yàn)闄C(jī)器人技術(shù).

E-mail：thinkpalm@126.com

G42

A

1002-4956(2015)7- 0139- 05