基于量子框架的六自由度機器人控制系統(tǒng)研究

董伯麟，鞠 毅

(合肥工業(yè)大學(xué)機械與汽車工程學(xué)院，合肥 230009)

0 引言

機器人技術(shù)是建立在機械、微電子、控制、人工智能技術(shù)等基礎(chǔ)上的一門高新技術(shù)。經(jīng)過60多年的發(fā)展，如今機器人已經(jīng)成為工廠自動化 (FA)、柔性制造系統(tǒng)(FSM)中必不可少的單元［1-2］。

機器人系統(tǒng)和數(shù)控系統(tǒng)一樣，是一個行為復(fù)雜的系統(tǒng)，當(dāng)向原有系統(tǒng)中添加新的功能需求、更新或替換已存在的子系統(tǒng)時，其控制邏輯和行為狀態(tài)將必然發(fā)生很大改變［3］。目前機器人系統(tǒng)主要針對機器人硬件結(jié)構(gòu)方面的重構(gòu)進行了研究，通過將可重構(gòu)模塊化的概念引入到機器人系統(tǒng)設(shè)計中，增加其可復(fù)用性［4］。在軟件上也主要以標(biāo)準(zhǔn)總線為基礎(chǔ)，通過總線的易擴展性實現(xiàn)系統(tǒng)的行為重構(gòu)和功能擴展，但其開放性還很低，還是一個封閉式結(jié)構(gòu)［5］。

本文提出一種基于量子框架的六自由機器人控制系統(tǒng)的設(shè)計，在硬件方面將嵌入式平臺跟機器人控制系統(tǒng)的設(shè)計相結(jié)合，大大降低了系統(tǒng)硬件成本并增加了系統(tǒng)的靈活性。同時在機器人控制系統(tǒng)軟件設(shè)計中引入量子框架的編程思想。量子框架結(jié)合面向活動對象的概念，針對操作系統(tǒng)實時性的需求，以事件驅(qū)動和活動對象為基礎(chǔ)，實現(xiàn)了更高層次上的封裝，有效地解決了應(yīng)用軟件與操作系統(tǒng)的耦合問題。在該系統(tǒng)下，模塊之間耦合性低、可重構(gòu)程度高，系統(tǒng)的可靠性更強。

1 基于量子框架的軟件架構(gòu)

量子框架(QF)是一個可移植和可重構(gòu)的基礎(chǔ)軟件框架特別適合嵌入式實時系統(tǒng)領(lǐng)域。在軟件設(shè)計過程中，量子框架可看作系統(tǒng)輕量級軟件總線，在這個軟件總線上連接多個的活動對象和硬件中斷服務(wù)程序ISR，量子框架負(fù)責(zé)事件的存貯、分發(fā)，也就是事件驅(qū)動，起到信息交換中心的作用。在控制系統(tǒng)設(shè)計過程中，可將傳統(tǒng)軟件的各個模塊劃分為不同的活動對象，活動對象是一個并發(fā)執(zhí)行、被賦予了輕量級控制各自線程的事件驅(qū)動對象。活動對象之間不直接共享資源，它們之間惟一的通信是通過量子框架這個“應(yīng)用骨架”訂閱和發(fā)布不同的事件實例進行交換。QF的結(jié)構(gòu)對于任何基于活動對象的框架來說都是典型的。QF的通用結(jié)構(gòu)具有以下特點①Q(mào)F是采取分層設(shè)計，在實時系統(tǒng)多線程的基礎(chǔ)上，給活動對象提供事件隊列和內(nèi)存池的服務(wù);②QF是一套相互協(xié)作的類，能給活動對象提供基類，從而派生具體的活動對象。并為每個活動對象分配執(zhí)行線程和事件隊列;③基于QF的應(yīng)用程序通過從活動對象和事件派生子類來擴展框架。應(yīng)用程序通過量子框架應(yīng)用程序接口來使用的通信和定時服務(wù)，不需要直接訪問底層實時系統(tǒng)API［7］。其基礎(chǔ)結(jié)構(gòu)如圖1所示。

圖1 量子框架基礎(chǔ)結(jié)構(gòu)

2 活動對象的構(gòu)建

在構(gòu)架一個基于QF的控制系統(tǒng)應(yīng)用最重要的步驟是構(gòu)建具體的活動對象。和其他的基于活動對象的框架一樣，QF也提供了一個基類(QActive)來規(guī)劃具體的活動對象。它包含了3個基本要素:①是一個有限層次狀態(tài)機;②有一個事件隊列;③有一個執(zhí)行線程，如圖2所示。

圖2 活動對象類圖

2.1 有限層次狀態(tài)機

每個具體的活動對象都是一個有限狀態(tài)機，它間接繼承于QHsm基類，使得行為繼承模式能夠全部并迅速的用于構(gòu)造活動對象［8］。用戶在規(guī)劃活動對象時，就不需要考慮活動對象的一些附加元素例如事件隊列和執(zhí)行線程等，只需要將重點放在比如內(nèi)部狀態(tài)機狀態(tài)的轉(zhuǎn)換和事件的細(xì)化方面來。QF最大的好處之一就是對快速構(gòu)造可執(zhí)行模型(活動對象)的支持?？蚣苡兄趯?gòu)成應(yīng)用的活動對象快速地放置在一起，然后逐步地細(xì)化其內(nèi)部細(xì)節(jié)，以保持應(yīng)用的執(zhí)行的穩(wěn)定性。

狀態(tài)機目前最流行的技術(shù)是嵌套的switch語句，使用標(biāo)量狀態(tài)變量作為switch第一級中的鑒別器，并在第二級中使用事件信號。用戶只需根據(jù)活動對象內(nèi)部狀態(tài)的轉(zhuǎn)換，規(guī)劃狀態(tài)轉(zhuǎn)換路徑和狀態(tài)處理函數(shù)，完成活動對象的構(gòu)建。圖3為IO狀態(tài)機的具體實現(xiàn)的部分代碼，其代碼如下:

圖3 IO狀態(tài)機的具體實現(xiàn)的部分代碼

2.2 執(zhí)行線程的實現(xiàn)

執(zhí)行線程具有平臺依賴性，在不同的操作系統(tǒng)中，有不同的執(zhí)行方式和代碼。SDOF機器人軟件程序通過VS2005軟件進行編寫，最后燒入到WinCE 6.0平臺中去，所以下圖4列出了在Win32環(huán)境中執(zhí)行線程的代碼，該代碼提供了重新將活動的對象的優(yōu)先級映射到下層操作系統(tǒng)所支持的線程優(yōu)先級的一個實例。

圖4 活動對象執(zhí)行線程代碼

機器人控制系統(tǒng)中主要任務(wù)有實時監(jiān)控、位置控制、IO信號處理、譯碼、插補、界面顯示等任務(wù)。實時監(jiān)控是緊迫性任務(wù)，當(dāng)機器人運行出現(xiàn)異常和故障時，應(yīng)該立即啟動報警，并采取自動保護措施，因此，伺服監(jiān)控任務(wù)具有最高優(yōu)先級。位置控制是關(guān)鍵性任務(wù)，位置控制的精度直接影響機器人的工作精度，運動控制的性能好壞影響機器人動力學(xué)特性和壽命，它的優(yōu)先級僅次于伺服監(jiān)控任務(wù)。I/O信號處理主要任務(wù)是面板和開關(guān)量信號的傳遞和處理，也要求一定的實時性，所以它的優(yōu)先級次于位置控制。譯碼任務(wù)主要是處理代碼數(shù)據(jù)文件，不是關(guān)鍵性緊迫任務(wù)，只有在譯碼方式執(zhí)行時才要求具有實時性［9］。顯示任務(wù)實現(xiàn)控制系統(tǒng)和用戶交互，非關(guān)鍵非緊迫任務(wù)，優(yōu)先級可以安排較低。根據(jù)上述分析，數(shù)控系統(tǒng)中優(yōu)先級按照高到低安排如下:伺服監(jiān)控任務(wù)，位置控制任務(wù)，IO信號處理，插補任務(wù)，譯碼任務(wù)，顯示模塊，各個活動對象的優(yōu)先級的可以根據(jù)實際情況可調(diào)。

2.3 事件隊列

事件隊列是活動的對象的基本成分，因為它保證了每個活動對象使用RTC形式處理每個事件。事件隊列可看作是一個提供外部接口的外殼。它將事件注入活動對象中，并同時保護在RTC處理過程中保護內(nèi)部狀態(tài)，使得相應(yīng)的活動對象看起來易于接受事件，并提供了保護內(nèi)部狀態(tài)機的緩沖空間，防止事件爆發(fā)產(chǎn)生帶來的破壞。在各自的線程中執(zhí)行的活動對象能自由地彼此搶占，因而達到更好的響應(yīng)特性和對CPU的高效率利用。

2.4 出版-訂閱模型

QF是一個事件驅(qū)動框架，應(yīng)用程序可以創(chuàng)建事件實例，放到事件池中，并分發(fā)事件到相應(yīng)注冊了這個事件的活動對象(AO)中。應(yīng)用程序可以注冊特定的事件，當(dāng)有事件發(fā)生時，QF就會分發(fā)事件到AO中。應(yīng)用程序是由多個AO組成，每個AO可以認(rèn)為是一個任務(wù)。把硬件中斷程序也當(dāng)作AO看待，只是優(yōu)先級比較高［9］。

活動對象之間通過異步事件交換進行通信。QF使用了出版-訂閱交互模型來進行活動對象之間事件和信息的交互與傳遞?？梢园裃F理解為軟件總線。在這個軟件總線上連接著很多的活動對象和中斷服務(wù)程序ISR，QF負(fù)責(zé)事件的存貯、分發(fā)，也就是事件驅(qū)動。圖5顯示了QF中出版-訂閱模型的運行機制，其中事件的分發(fā)有兩種分發(fā)方式。①直接分發(fā)事件:一個AO直接分發(fā)事件到另一個AO，就是直接投遞事件。這種方式分發(fā)事件的AO要知道目標(biāo)AO。使用void QActive_postFIFO(QActive*me，QEvent const*e)函數(shù)分發(fā)事件。②P/S分發(fā)事件:利用量子框架中的AO訂閱事件表，以P/S(Publish-Subscribe出版-訂閱)方式分發(fā)事件，也就是P/S投遞事件。這種方式分發(fā)事件活動對象之間不需要知道目標(biāo)對象，QF通過查找活動對象訂閱事件表就知道把事件分發(fā)到什么地方，使得活動對象之間具有松耦合性，從而易于系統(tǒng)模塊之間的可重組。

圖5 出版-訂閱模型運行機制

2.5 量子框架的運行

圖6展示了在Win32下QF應(yīng)用中的main()結(jié)構(gòu)。在典型地初始化框架和啟動各個活動對象之后，main()進入一個for永久循環(huán)，周期性地調(diào)用QF::tick()始終節(jié)拍，提供任務(wù)級的事件管理機制。

圖6 QF應(yīng)用下main結(jié)構(gòu)

3 SDOF機器人控制系統(tǒng)的具體實現(xiàn)

3.1 SDOF機器人系統(tǒng)的硬件結(jié)構(gòu)

SDOF-ROBOT是以WindowsCE6.0為軟件平臺的嵌入式數(shù)控系統(tǒng)，機器人控制系統(tǒng)采用ARM+運動控制卡結(jié)構(gòu)，組成該數(shù)控系統(tǒng)的硬件結(jié)構(gòu)如圖7所示。系統(tǒng)采用主從式多CPU結(jié)構(gòu)［10］，ARM工業(yè)主板具有主CPU，通過PC/104總線分別與六軸運動控制卡及PC/104數(shù)據(jù)采集卡相連。上位機采用Windows CE嵌入式系統(tǒng)進行系統(tǒng)管理等弱實時任務(wù)的調(diào)度，下位機采用運動控制卡調(diào)度以運動控制為主的強實時控制任務(wù)。

圖7 SDOF-ROBOT硬件結(jié)構(gòu)

3.2 SDOF機器人系統(tǒng)的軟件實現(xiàn)

SDOF-ROBOT系統(tǒng)包括IO活動對象、Decode活動對象、InterP活動對象、HMI活動對象等?；顒訉ο蟮木唧w實現(xiàn)最重要的是狀態(tài)圖，通過在量子框架特有的建模工具(QM)中規(guī)劃每個活動對象的簡要狀態(tài)圖，其具體實現(xiàn)如下:

IO活動對象:主要檢測I/O信號，管理示教盒的按鍵輸入響應(yīng)。該活動對象的狀態(tài)機分為空閑和工作兩個子狀態(tài)，其簡化狀態(tài)圖如圖8所示。首先狀態(tài)機進入空閑狀態(tài)，在空閑狀態(tài)下，I0活動對象不斷檢查是否有鍵按下或者信號的輸入。當(dāng)有信號輸入時，狀態(tài)機從空閑狀態(tài)轉(zhuǎn)換到工作狀態(tài)，在工作狀態(tài)中會創(chuàng)建一個該按鍵或信號的事件，并將該事件出版到QF中，由QF將該事件分發(fā)給其他活動對象。

圖8 IO活動對象狀態(tài)圖

Decode活動對象:主要對程序進行編譯和預(yù)處理，并將編譯后的程序以事件實例的方式出版給插補活動對象。Decode活動對象接受從量子框架傳來的事件實例(包含有加工代碼信息)，直接進入譯碼子狀態(tài)，它分段完成譯碼工作，并將數(shù)據(jù)存入譯碼緩沖區(qū);接著進入速度處理狀態(tài)，從譯碼緩沖區(qū)取出數(shù)據(jù)，進行速度預(yù)處理計算，形成按指令類型及長度分成的多個指令段，并以事件實例的形式出版給量子框架，由框架把該事件實例傳送給插補活動對象，進行插補計算。在分發(fā)指令狀態(tài)機中，需訂閱運動控制卡指令處理完成事件、運動控制卡指令等待事件，其簡化狀態(tài)圖如圖9所示。

圖9 Decode活動對象狀態(tài)圖

插補活動對象:管理運動控制卡插補工作，其簡化狀態(tài)圖如圖10所示。在插補空閑狀態(tài)，狀態(tài)機接收到INTERP_EVT事件后進行插補工作狀態(tài)，系統(tǒng)根據(jù)直線、圓弧插補算法以及預(yù)處理信息，計算出每個插補周期Ts內(nèi)向各軸電機應(yīng)該發(fā)送的脈沖數(shù)，完成插補工作后向位置控制活動對象發(fā)送事件，控制伺服電機運行。

圖10 插補活動對象狀態(tài)圖

HMI活動對象:主要負(fù)責(zé)界面顯示，參數(shù)設(shè)置，程序編輯等，并對整個機器人系統(tǒng)的運行進行監(jiān)控。在機器人系統(tǒng)開始運行后，會進入Idle偽狀態(tài)，系統(tǒng)完成一些初始化操作如調(diào)用Gui顯示函數(shù)等。根據(jù)特定的按鍵事件，進入不同的工作狀態(tài)。例如在自動運行狀態(tài)，會首先進入程序代碼輸入子狀態(tài)，并在終端顯示自動運行界面，等待加工代碼輸入。狀態(tài)圖如圖11所示。

圖11 HMI活動對象狀態(tài)圖

六自由度機器人活動對象的規(guī)劃與數(shù)控機床具有相似之處，但機器人跟數(shù)控機床不同在于機器人具有示教-再現(xiàn)的功能，相對于數(shù)控機床更為復(fù)雜化，根據(jù)機器人運行過程的特性和狀態(tài)，可以將機器人控制系統(tǒng)的再現(xiàn)-示教功能劃分在手動模式中，當(dāng)機器人進入手動模式后，按下示教按鈕，可手動進行示教，方便操作。

3.3 SODF機器人軟硬件平臺

根據(jù)在QM建模工具所規(guī)劃的機器人狀態(tài)圖，開發(fā)機器人控制系統(tǒng)軟件。SODF機器人為工業(yè)機器人，其用途主要是用于車床自動上下工件，從而節(jié)省勞動力并加快工作效率。圖12～圖14為機器人的軟硬件平臺實物圖，正在測試運行。

圖12 機器人控制器和示教盒

圖13 機器人自動回原點界面圖

圖14 機器人自動運行界面圖

4 結(jié)束語

本文通過將面向活動對象的設(shè)計方法應(yīng)用到機器人系統(tǒng)的開發(fā)中，構(gòu)建一種可重用的基于量子框架的控制系統(tǒng)軟件框架，測試表明:

①運行過程中各個活動對象之間通過量子框架進行事件和信息的交換，系統(tǒng)中的各模塊之間是松耦合的關(guān)系。在需要增加新的模塊時，只需考慮修改原有的活是否滿足新功能，如果滿足不了，增加新的活動對象，修改狀態(tài)圖和事件處理方式即可，無需對整個系統(tǒng)的重新編寫，實現(xiàn)真正意義上的可重構(gòu)。

②QF特有的QM建模工具使得規(guī)劃活動對象狀態(tài)圖時更加便捷和清晰。同時量子框架提供完整的框架基礎(chǔ)代碼，具有自動代碼生成綜合功能，避免程序設(shè)計過程中大量文檔的編寫和重復(fù)，并提供了控制系統(tǒng)的可移植性。

③量子框架使用RTC(Run To Complete)形式處理事件，排除了內(nèi)部的并發(fā)問題，大大降低了系統(tǒng)運行過程中出現(xiàn)競爭、死鎖和不確定等問題的概率，提高了系統(tǒng)的可靠性和安全性。

［1］畢勝.國內(nèi)外工業(yè)機器人的發(fā)展現(xiàn)狀［J］.機械工程師，2008(7):5－8.

［2］胡貴錢.我國機器人技術(shù)獲得突飛猛進發(fā)展［J］.世界科學(xué)技術(shù)，1998(2):22－24.

［3］CHITOS ，WASHBURNJ ，WANGS ，et al.Specifying reconfigurable control flow for open architecture controller［J］.Proceedings 1998 Japan-USA Symposium on Flexible Automation ，1998 ，7(2):659 －666.

［4］劉思寧，陳勇，張文俊.模塊機器人及計算機輔助設(shè)計［J］.機器人，1999，1(6):16 －21.

［5］王太勇，李波，萬淑敏，等.基于現(xiàn)場總線的可重構(gòu)數(shù)控系統(tǒng)的研究［J］.計算機集成制造系統(tǒng)，2006，10(12):1662－1667.

［6］時良仁.基于DSP的機械手開放式運動控制器設(shè)計［J］.齊齊哈爾大學(xué)學(xué)報(自然科學(xué)版)，2011，27(3):14－16.

［7］Miro Samek.嵌入式系統(tǒng)的微模塊化程序設(shè)計—實用狀態(tài)圖C/C++實現(xiàn)［M］.北京:北京航空航天大學(xué)出版社，2004.

［8］李霞，王永章，梁宏斌，等.有限狀態(tài)機在開放式數(shù)控系統(tǒng)中的應(yīng)用［J］.計算機集成制造系統(tǒng) －CIMS，2005，11(3):428－ 432.

［9］楊凡.嵌入式數(shù)控軟件平臺關(guān)鍵技術(shù)研究［D］.武漢:華中科技大學(xué)，2007.

［10］朱華炳，羅祖平，董伯麟，等.面向活動對象的可重構(gòu)數(shù)控系統(tǒng)軟件設(shè)計［J］.中國機械工程，2012，24(6):781 －786.

［11］Liandong Pan，Xinhan Huang.Implementation of a PC-based Robot Controller with Open Architecture.［C］Proceedings of the 2004 IEEE International Conference on Robotics and Biomimetics，Shenyang，China，August 2004，22－26.

［12］Douglass BP.State machines and statecharts.Proceedings of embedded systems conference［C］.San Francisco，2001.

［13］李洪斌，張承瑞.基于量子框架的開放式汽車電控系統(tǒng)體系結(jié)構(gòu)［J］.吉林大學(xué)學(xué)報(工學(xué)版)，2006，36(2):168－171.