基于VC平臺的服務(wù)機器人運動控制與仿真方法*

□ 黃萬永 □ 屠大維,2 □ 江濟良

1.上海大學 機電工程與自動化學院 上海 200072

2.上海市智能制造及機器人重點實驗室 上海 200072

機器人仿真技術(shù)為機器人的設(shè)計和研究提供了一個高效、便利的手段，是機器人學研究中的一項重要內(nèi)容。迄今為止，國內(nèi)外進行了大量的機器人仿真方法和仿真系統(tǒng)的研究，主要研究工作都集中在3D制圖軟件仿真模塊的應(yīng)用、MATLAB仿真以及OpenGL仿真［1-5］等方面。

3D制圖軟件包括UG、Pro/E等主要應(yīng)用于產(chǎn)品的設(shè)計和分析，仿真只在虛擬制造和虛擬加工領(lǐng)域應(yīng)用較廣［6］，其目的主要在于制作動畫、展示加工過程；可擴展性低，不具備開放性。MATLAB在機器人領(lǐng)域應(yīng)用比較廣泛，能夠借助MATLAB的強大算法進行仿真實驗。但MATLAB具有兩大先天的缺陷，一是它在3D圖形顯示方面有著明顯的不足，無法直觀地表達出機器人的末端位姿和工具手姿態(tài)，很難判斷出機器人與外部環(huán)境之間的干涉或碰撞［7］；另一個是它作為一個獨立的數(shù)學計算平臺，缺少與外部控制系統(tǒng)的接口，一般只應(yīng)用于算法的驗證［8］。OpenGL是計算機圖形技術(shù)領(lǐng)域的集大成者，它獨立于硬件與窗口系統(tǒng)，具有內(nèi)核小、運行快、可移植等特點，并能夠與VC平臺“無縫”鏈接，因此被廣泛應(yīng)用于虛擬仿真系統(tǒng)的開發(fā)［9-11］。

筆者針對3D制圖軟件和MATLAB的不足，采用OpenGL 3D圖形接口建立服務(wù)機器人虛擬仿真模型，通過對仿真體系結(jié)構(gòu)、系統(tǒng)的通信方法、3D模型的驅(qū)動、運動學算法等的設(shè)計，建立服務(wù)機器人虛擬仿真控制平臺。該平臺可以進行虛擬機器人的實時運動控制、軌跡規(guī)劃、狀態(tài)監(jiān)控、虛擬漫游、虛擬示教等工作，并能夠結(jié)合智能規(guī)劃方法完成虛擬倒水等仿真作業(yè)。

1 服務(wù)機器人仿真系統(tǒng)的實現(xiàn)步驟

1.1 仿真模型的構(gòu)建與存儲

虛擬機器人是以現(xiàn)實的服務(wù)機器人為藍本，借助OpenGL 3D強大的圖形接口，完成機器人基本模型的繪制。如圖1所示，機器人的幾何模型由若干個部件模型拼裝而成，部件間的相互位置關(guān)系由裝配信息（關(guān)節(jié)的結(jié)構(gòu)參數(shù)和運動參數(shù)）唯一確定，裝配信息包括關(guān)節(jié)類型、關(guān)節(jié)之間相對坐標關(guān)系、關(guān)節(jié)取向等。虛擬服務(wù)機器人模型包括底座、中間軀干、模塊化雙臂、頭部，為了控制服務(wù)機器人的組合運動和整體運動，服務(wù)機器人采用樹狀連接，每個節(jié)點相當于一個模塊，各個模快是分級的，上一節(jié)點可以帶動下一節(jié)點的運動，這樣就可以通過編程來控制各個模塊的獨自運動和整體運動來驅(qū)動仿真機器人運動。

為了提升仿真的真實性，構(gòu)建了仿真環(huán)境模型，該模型中的室內(nèi)房間、走廊、門等位置尺寸固定不變，室內(nèi)家具、雜物可以通過3D模型數(shù)據(jù)庫導(dǎo)入，如圖2所示。仿真機器人在仿真環(huán)境中可以模擬現(xiàn)實機器人在現(xiàn)實環(huán)境中的各種任務(wù)，用戶可以通過操控仿真機器人在仿真環(huán)境中完成相應(yīng)的任務(wù)。

▲圖1 虛擬雙臂服務(wù)機器人模型

▲圖2 仿真環(huán)境模型

1.2 仿真系統(tǒng)體系結(jié)構(gòu)

為了使仿真系統(tǒng)能夠真正模擬真實機器人的控制和操作過程，并能夠與虛擬機器人相連完成模擬真實機器人的控制過程，因此必須采用合理的仿真體系結(jié)構(gòu)，保證仿真系統(tǒng)的可靠性和對機器人仿真的真實性。

▲圖3 仿真系統(tǒng)的體系結(jié)構(gòu)

仿真系統(tǒng)的體系結(jié)構(gòu)如圖3所示。仿真系統(tǒng)采用MVC（Model View Controller）結(jié)構(gòu)設(shè)計，使控制器、視圖、模型相分離，形成一個開放式的閉環(huán)仿真系統(tǒng)。仿真系統(tǒng)控制器的輸入可以是鍵盤（鼠標）、屏幕點動、遙控操作控制器等；這些輸入產(chǎn)生的空間位移和方向轉(zhuǎn)換是一個空間矢量，這個空間矢量為機器人的目的矢量。通過對這個目的矢量的輸入進行位置計算、運動學計算、限制監(jiān)測計算等來驅(qū)動相應(yīng)的模型，最后通過業(yè)務(wù)模型的選擇來驅(qū)動相應(yīng)的視圖展現(xiàn)給用戶。

1.3 仿真系統(tǒng)內(nèi)部的數(shù)據(jù)通信

在VC平臺下編寫仿真系統(tǒng)，需要封裝很多不同功能的類，并且從屬于不同的線程，這些線程和類之間在不斷地進行大量的數(shù)據(jù)交換。例如：機器人運動學模塊計算出當前各個模塊的位姿，此時位姿顯示對話框類、3D視圖變換類、機器人關(guān)節(jié)限位檢測類、機器人正反解計算的運動學類等位姿都需進行更新，這種典型的廣播式通信可以采用設(shè)計模式中的觀察者模式來解決。定義共享數(shù)據(jù)源為觀察者模式中的“主題類”，需要數(shù)據(jù)源數(shù)據(jù)的其它類為“觀察者類”，當觀察者需要接收主題類的信息時，可以通過主題類進行登記，這樣主題類就可以對登記的觀察者進行消息廣播，更新觀察者類的自身數(shù)據(jù)。這種方式避免了對C++數(shù)據(jù)封裝的破壞，同時也消除了內(nèi)存在不同線程和類之間快速切換中導(dǎo)致亂序的風險。

1.4 仿真系統(tǒng)的運動控制

根據(jù)手臂硬件連接結(jié)構(gòu)和D-H （Denavit-Hartenberg）參數(shù)描述方法，建立雙臂服務(wù)機器人六自由度手臂的連桿坐標系模型，如圖4所示，并得到其D-H參數(shù)。D-H參數(shù)和各關(guān)節(jié)范圍列于表1中。

表1 服務(wù)機器人手臂D-H參數(shù)

其中，α表示相鄰Z軸之間的轉(zhuǎn)角，a表示相鄰Z軸之間公垂線段長度，d表示相鄰X軸之間的距離，θ表示繞Z軸的旋轉(zhuǎn)角。

手臂各相鄰桿件之間的D-H變換矩陣：

i-1Ti表示連桿坐標系｛i｝相對于連桿坐標系｛i-1｝的齊次變換矩陣，Rot（X，αi-1）表示相對于 X 軸旋轉(zhuǎn) αi-1角度，Trans（X，ai-1）表示相對于 X 軸平移 ai-1，同理可知Rot （Z，θ）、Trans （Z，di）。 根據(jù)表 1 中 D-H 參數(shù)和式（1）， 可得各連桿之間奇次變換矩陣0T1、1T2、2T3、3T4、4T5、5T6。

機器人手臂采用圖4所示與豎直方向成10°的安裝。將左右手臂的坐標統(tǒng)一到同一個基坐標系 ｛Ob-XbYbZb｝下，其中Ob為服務(wù)機器人兩主動輪中心連線的中點，根據(jù)機器人本身的結(jié)構(gòu)尺寸和坐標旋轉(zhuǎn)角度可得左右手臂的奇次變換矩陣bT0。因此，服務(wù)機器人手臂末端坐標系｛O6｝相對于服務(wù)機器人基座坐標系｛Ob｝的奇次變換矩陣為：

式中：［n s a］表示手臂末端的姿態(tài)，[p]表示手臂末端坐標系原點位于基坐標系中的位置。

根據(jù)歐拉公式可得手臂末端位姿［x，y，z，O，A，T］，O、A、T 為歐拉角。

手臂逆運動學求解流程如圖5所示，采用幾何法求解出手腕坐標（pAx，pAy，pAz），進而通過代數(shù)法求解出對應(yīng)的關(guān)節(jié)角。主要求解公式：

式中：si、ci表示 sinθi、cosθi。

手臂的后3個關(guān)節(jié)為典型的球腕結(jié)構(gòu)，對于給定的手臂姿態(tài)存在上仰腕和下垂腕兩種情況，前3個關(guān)節(jié)確定手腕的位置，后3個關(guān)節(jié)確定手腕的姿態(tài)。因此，最多存在8組逆運動學封閉解。

根據(jù)經(jīng)驗，遵循運動的關(guān)節(jié)角加權(quán)和最小原則并考慮基座約束等因素，選擇一組實際可行的最優(yōu)解。假設(shè)服務(wù)機器人手臂當前關(guān)節(jié)角為 θi（i=0，1，…6），通過逆運動學求解總共得到n組解（1≤n≤8，n∈Z），其中第 j組封閉解為 θij（i=0，1，…，6，1≤j≤n），定義第 j組封閉解運動的關(guān)節(jié)角加權(quán)和為，則有：

式中：λi為第 i關(guān)節(jié)的加權(quán)系數(shù)，i=0，1，…，6。

考慮各個關(guān)節(jié)所承受的力矩、載荷和沖擊等因素，確定出加權(quán)系數(shù)集｛λi｝=｛0.29，0.24，0.19，0.14，0.09，0.05｝，通過比較選擇出最小的關(guān)節(jié)角加權(quán)和，即有：

至此即選擇出第k組封閉解為實際可行的最優(yōu)解。在實際控制中，除了要考慮基座約束的影響，還要考慮雙臂協(xié)作防碰、空間可達、安全高效等要求，選擇出一組空間可達、安全、運動量小的最優(yōu)解。

采用以上的方法建立的仿真系統(tǒng)運動過程如圖 6 所示，圖 6（a）為運動學求解視圖，圖6（b）為機器人根據(jù)求解值的運動視圖。機器人根據(jù)指定的目標點走出一條平穩(wěn)連續(xù)光滑的軌跡路線圖。

▲圖5 逆運動學求解流程

2 仿真實驗

通過以上方法，建立起模塊化的與真實機器人控制器相同的實時仿真系統(tǒng)，在3D仿真功能的基礎(chǔ)上可以很方便地實現(xiàn)其它功能擴展，如虛擬示教、虛擬漫游、軌跡規(guī)劃等，完成機器人虛擬任務(wù)作業(yè)。

▲圖6 運動學過程

虛擬機器人倒水任務(wù)作業(yè)是一個復(fù)雜的過程，本文提出了一種機器人智能規(guī)劃方法來完成虛擬機器人的任務(wù)作業(yè)。機器人智能規(guī)劃方法采用語義網(wǎng)絡(luò)推理的方式，如圖7所示，圍繞著取水這個語義，據(jù)此推理出取一個水杯、找到茶壺、把水倒入杯中這一系列子任務(wù)語義網(wǎng)絡(luò)。語義網(wǎng)絡(luò)可以不斷分層，根據(jù)子任務(wù)語義把水倒入杯中可以繼續(xù)推理出把茶壺放到水杯上方、把茶壺傾斜、把茶壺豎直等動作規(guī)劃語義，最終再根據(jù)動作規(guī)劃語義，推理出軌跡規(guī)劃語義，完成仿真任務(wù)。

根據(jù)機器人的智能規(guī)劃方法，明確機器人的任務(wù)規(guī)劃過程及子任務(wù)的順序。例如機器人對茶壺的抓取是采用示教再現(xiàn)的方式，由仿真系統(tǒng)中提供的室內(nèi)物品空間位置數(shù)據(jù)庫（桌子、椅子、杯子、茶壺等），通過虛擬示教，記錄手動控制虛擬機器人在仿真系統(tǒng)中的一系列空間離散位置，示教整個過程。圖8為示教過程的主要視圖，位置顯示視圖實時顯示空間物品相對于機器人和世界坐標的位置信息，示教視圖可以完成虛擬機器人運動的示教再現(xiàn)控制。當物品到達手臂可達空間區(qū)域內(nèi)，通過手臂軌跡規(guī)劃完成目標抓取。

圖9所示是完成仿真系統(tǒng)的機器人倒水任務(wù)作業(yè)過程中的幾幅畫面，其中圖9（a）～（d）為智能規(guī)劃抓取茶壺、抓取杯子、倒水作業(yè)、放回茶壺和水杯，整個過程通過智能規(guī)劃、虛擬示教、虛擬漫游、軌跡規(guī)劃器完成。

▲圖7 機器人智能規(guī)劃推理及語義網(wǎng)絡(luò)

▲圖8 虛擬示教

▲圖9 仿真倒水作業(yè)過程

3 結(jié)論

本文提出并實現(xiàn)了服務(wù)機器人虛擬運動控制與仿真系統(tǒng)的構(gòu)建方法，建立完整的虛擬仿真控制平臺。應(yīng)用MVC軟件架構(gòu)，形成了一個開放式仿真體系；通過“觀察者”設(shè)計模式，實現(xiàn)了系統(tǒng)中各個模塊的實時通信；分析了機器人的運動學模型，完成了機器人的虛擬示教、軌跡規(guī)劃；通過引入語義智能規(guī)劃，完成機器人的虛擬倒水作業(yè)等。該平臺下的虛擬仿真控制系統(tǒng)具有的意義如下。

（1）采用MVC的軟件架構(gòu)，系統(tǒng)具有良好的開放性和可移植性，能夠不斷擴展仿真系統(tǒng)的應(yīng)用領(lǐng)域，例如教學、科研。

（2）該系統(tǒng)具有獨立的仿真功能，為機器人智能規(guī)劃方法和離線編程系統(tǒng)的設(shè)計提供了驗證平臺。

［1］ Wang Y S,Gai Y X,Wu F Y.A Robot Kinematics Simulation System Based on OpenGL ［C］.Robotics,Automation and Mechatronics （RAM）,Qingdao,China:IEEE,2011.

［2］ 楊文通，張第，何煜琛，等．基于Pro/E的仿真分析平臺的建立［J］．機械設(shè)計與制造，2005（10）：63-64.

［3］ 龔建球，劉守斌．基于ADAMS和MATLAB的自平衡機器人仿真［J］．機電工程，2008，25（2）：8-10,73.

［4］ 樊雅萍,黃生學,溫佩芝,等.基于 OpenGL的機器人虛擬漫游系統(tǒng)開發(fā) ［J］.系統(tǒng)仿真學報,2005，17 （10）：2426-2429.

［5］ Zhang Z,Ma S,Li B,et al.Development of an OpenGL Based Multi-robotSimulating Platform ［C］.Control,Automation,Robotics and Vision Conference,Kunming，China：IEEE,2004.

［6］ 張飛，芮延年，喬冬冬，等．基于Pro/E和ADAMS的三軸聯(lián)動平臺設(shè)計與運動學仿真 ［J］．組合機床與自動化技術(shù)，2012（9）：35-37.

［7］ 祁若龍,周維佳,劉金國,等.VC平臺下機器人虛擬運動控制及 3D運動仿真的有效實現(xiàn)方法 ［J］.機器人,2013,35（5）:594-599.

［8］ 甘亞輝，戴先中．一種高效的開放式關(guān)節(jié)型機器人3D仿真環(huán)境構(gòu)建方法［J］．機器人，2012，34（5）：628-633.

［9］ 李清玲,李爽.基于 OpenGL的三維人體運動仿真 ［J］.計算機仿真,2011,28（4）:270-273.

［10］馬壯,周建利,柳延領(lǐng).基于 VB與 OpenGL的機器人三維運動仿真系統(tǒng)的設(shè)計［J］.煤礦機械,2008,29（11）:14-16.

［11］ Ma Y,Dong T,Lan X,et al.Industrial Robot Simulation Based on OpenGL ［C］.Proceedings of the International Conference on Information Engineering and Applications（IEA）,Springer London,2013.