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

□ 黃萬永 □ 屠大維,2 □ 江濟(jì)良

1.上海大學(xué) 機(jī)電工程與自動化學(xué)院 上海 200072

2.上海市智能制造及機(jī)器人重點(diǎn)實(shí)驗(yàn)室 上海 200072

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

i-1Ti表示連桿坐標(biāo)系｛i｝相對于連桿坐標(biāo)系｛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。

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

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

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

手臂逆運(yùn)動學(xué)求解流程如圖5所示，采用幾何法求解出手腕坐標(biāo)（pAx，pAy，pAz），進(jìn)而通過代數(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組逆運(yùn)動學(xué)封閉解。

根據(jù)經(jīng)驗(yàn)，遵循運(yùn)動的關(guān)節(jié)角加權(quán)和最小原則并考慮基座約束等因素，選擇一組實(shí)際可行的最優(yōu)解。假設(shè)服務(wù)機(jī)器人手臂當(dāng)前關(guān)節(jié)角為 θi（i=0，1，…6），通過逆運(yùn)動學(xué)求解總共得到n組解（1≤n≤8，n∈Z），其中第 j組封閉解為 θij（i=0，1，…，6，1≤j≤n），定義第 j組封閉解運(yùn)動的關(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組封閉解為實(shí)際可行的最優(yōu)解。在實(shí)際控制中，除了要考慮基座約束的影響，還要考慮雙臂協(xié)作防碰、空間可達(dá)、安全高效等要求，選擇出一組空間可達(dá)、安全、運(yùn)動量小的最優(yōu)解。

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

▲圖5 逆運(yùn)動學(xué)求解流程

2 仿真實(shí)驗(yàn)

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

▲圖6 運(yùn)動學(xué)過程

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

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

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

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

▲圖8 虛擬示教

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

3 結(jié)論

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

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

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

［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］．機(jī)械設(shè)計(jì)與制造，2005（10）：63-64.

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

［4］ 樊雅萍,黃生學(xué),溫佩芝,等.基于 OpenGL的機(jī)器人虛擬漫游系統(tǒng)開發(fā) ［J］.系統(tǒng)仿真學(xué)報,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ì)與運(yùn)動學(xué)仿真 ［J］．組合機(jī)床與自動化技術(shù)，2012（9）：35-37.

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

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

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

［10］馬壯,周建利,柳延領(lǐng).基于 VB與 OpenGL的機(jī)器人三維運(yùn)動仿真系統(tǒng)的設(shè)計(jì)［J］.煤礦機(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.