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

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

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

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

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

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

筆者針對3D制圖軟件和MATLAB的不足，采用OpenGL 3D圖形接口建立服務機器人虛擬仿真模型，通過對仿真體系結構、系統的通信方法、3D模型的驅動、運動學算法等的設計，建立服務機器人虛擬仿真控制平臺。該平臺可以進行虛擬機器人的實時運動控制、軌跡規劃、狀態監控、虛擬漫游、虛擬示教等工作，并能夠結合智能規劃方法完成虛擬倒水等仿真作業。

1 服務機器人仿真系統的實現步驟

1.1 仿真模型的構建與存儲

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

為了提升仿真的真實性，構建了仿真環境模型，該模型中的室內房間、走廊、門等位置尺寸固定不變，室內家具、雜物可以通過3D模型數據庫導入，如圖2所示。仿真機器人在仿真環境中可以模擬現實機器人在現實環境中的各種任務，用戶可以通過操控仿真機器人在仿真環境中完成相應的任務。

▲圖1 虛擬雙臂服務機器人模型

▲圖2 仿真環境模型

1.2 仿真系統體系結構

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

▲圖3 仿真系統的體系結構

仿真系統的體系結構如圖3所示。仿真系統采用MVC（Model View Controller）結構設計，使控制器、視圖、模型相分離，形成一個開放式的閉環仿真系統。仿真系統控制器的輸入可以是鍵盤（鼠標）、屏幕點動、遙控操作控制器等；這些輸入產生的空間位移和方向轉換是一個空間矢量，這個空間矢量為機器人的目的矢量。通過對這個目的矢量的輸入進行位置計算、運動學計算、限制監測計算等來驅動相應的模型，最后通過業務模型的選擇來驅動相應的視圖展現給用戶。

1.3 仿真系統內部的數據通信

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

1.4 仿真系統的運動控制

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

表1 服務機器人手臂D-H參數

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

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

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

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

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

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

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

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

手臂的后3個關節為典型的球腕結構，對于給定的手臂姿態存在上仰腕和下垂腕兩種情況，前3個關節確定手腕的位置，后3個關節確定手腕的姿態。因此，最多存在8組逆運動學封閉解。

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

式中：λi為第 i關節的加權系數，i=0，1，…，6。

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

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

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

▲圖5 逆運動學求解流程

2 仿真實驗

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

▲圖6 運動學過程

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

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

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

▲圖7 機器人智能規劃推理及語義網絡

▲圖8 虛擬示教

▲圖9 仿真倒水作業過程

3 結論

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

（1）采用MVC的軟件架構，系統具有良好的開放性和可移植性，能夠不斷擴展仿真系統的應用領域，例如教學、科研。

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

［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］．機械設計與制造，2005（10）：63-64.

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

［4］ 樊雅萍,黃生學,溫佩芝,等.基于 OpenGL的機器人虛擬漫游系統開發 ［J］.系統仿真學報,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的三軸聯動平臺設計與運動學仿真 ［J］．組合機床與自動化技術，2012（9）：35-37.

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

［8］ 甘亞輝，戴先中．一種高效的開放式關節型機器人3D仿真環境構建方法［J］．機器人，2012，34（5）：628-633.

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

［10］馬壯,周建利,柳延領.基于 VB與 OpenGL的機器人三維運動仿真系統的設計［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.