吳傳奇，甘 屹，齊從謙

（1.上海理工大學 機械工程學院，上海 200093；2.同濟大學 機械與能源工程學院，上海 201804）

0 引言

六自由度工業(yè)機器人適用面廣，主要運用于碼垛、焊接等工業(yè)領域。對其進行運動學建模與仿真，可以降低機器人生產(chǎn)成本與提高執(zhí)行效率[1]。現(xiàn)階段多數(shù)大型機器人廠商的仿真系統(tǒng)較好的實現(xiàn)了可視化與運動學仿真如ROBCAD[2]，但是封閉程度較高。另一部分則采用MATLAB[3]、ADAMS[4]以及二者結(jié)合等方式[5]，適合教學與研究，但其可移植程度較低。其他則采用高級語言如VRML、OpenGL[6]等，但其建模較為困難且3D可視化運動效果較差。

針對現(xiàn)階段工業(yè)機器人仿真系統(tǒng)封閉程度高且3D可視化運動效果較差的缺點，本文以六自由度工業(yè)機器人為研究對象，開發(fā)了一種運用Microsoft Visual C#2015的WPF設計的3D仿真及控制系統(tǒng)軟件。通過該軟件對機器人正、逆運動學算法及綜合優(yōu)選算法進行了現(xiàn)場驗證并同步了機器人數(shù)字模型3D實時仿真與機器人物理本體的實時同步。具有較好的系統(tǒng)可移植性與3D可視化運動效果。

1 運動學分析

1.1 連桿坐標系的建立

本文采用D-H法進行求解。在機器人關節(jié)1的軸線上建立基坐標系x0y0z0并依次建立機器人各個連桿的坐標系。x6y6z6為機器人末端坐標系。機器人各關節(jié)的D-H坐標系如圖1所示。

圖1 機器人D-H坐標系

1.2 正運動學求解

機器人DH參數(shù)如表1所示。

表1 機器人D-H參數(shù)表

機器人相鄰桿件間的坐標轉(zhuǎn)換矩陣如式(1)所示。

將各連桿間坐標轉(zhuǎn)換矩陣相乘得機器人末端坐標系到基坐標系的轉(zhuǎn)換矩陣，如式(2)所示。

(n,o,a)為姿態(tài)矩陣。實際應用中，為了與機器人的末端的位姿表示一致且提高運行速度，工業(yè)機器人大多采用一組廣義的坐標來表示位置和姿態(tài)。其中（px，py，pz）表示機器人的位置矩陣，歐拉角表示姿態(tài)。

1.3 逆運動學求解

在已知D-H參數(shù)和機器人末端位姿的基礎上，對式(2)分別相乘所對應的逆矩陣，從而求得各關節(jié)的轉(zhuǎn)角。例如對θ1的求解，式(2)兩端同時乘逆矩陣0T1-1后得：

左右兩端對應元素相等得：

解得θ1：

同理可得：

當s5≠0時：

由D-H參數(shù)知各關節(jié)運動范圍是有限制的，因此需對多組解進行篩選。對多組解的篩選通常有能量最小原則、行程最短原則[7]等。根據(jù)行程最短原則，其優(yōu)選函數(shù)如式(5)所示。

根據(jù)能量最小原則，其優(yōu)選函數(shù)如式(6)所示。

本文綜合考慮能量最小原則以及行程最短原則。得出綜合優(yōu)選函數(shù)如式(7)所示。

式中s為權重系數(shù)。

2 3D仿真及控制系統(tǒng)設計

運用Microsoft Visual C#2015的WPF設計了3D仿真及控制系統(tǒng)軟件，此系統(tǒng)基于WPF多媒體交互用戶圖形系統(tǒng)，采用其內(nèi)置3D渲染建模引擎構建矢量旋轉(zhuǎn)矩陣，并以C#編程語言設計運動控制邏輯。在系統(tǒng)示教模式下可實現(xiàn)機器人初始化、正運動學計算、工程的存取、時間及速度顯示；在系統(tǒng)滾輪模式下可實現(xiàn)逆運動學的計算；運行狀態(tài)的顯示、各關節(jié)運動。

機器人3D仿真及控制系統(tǒng)界面如圖2所示。

圖2 3D仿真及控制系統(tǒng)軟件界面

3 機器人實時控制實驗

在示教模式下，輸入各關節(jié)角度值（40，-135，30，0，-90，0），通過3D仿真及控制系統(tǒng)可計算出機器人末端位姿運動學正解并同時傳給機器人實物本體，在系統(tǒng)界面實時顯示數(shù)字3D位姿，正運動學求解位姿圖如圖3所示。

圖3 正運動學求解位姿圖

在滾輪模式下，3D仿真及控制系統(tǒng)實時采集到機器人實物本體的關節(jié)末端位姿（16.22，0.00，921.27，-173.32，-15.00，-6.68），通過3D仿真及控制系統(tǒng)可計算出機器人末端位姿運動學逆解并傳給機器人數(shù)字3D模型，在系統(tǒng)界面實時顯示數(shù)字3D位姿，逆運動學求解位姿圖如圖4所示。

圖4 逆運動學求解位姿圖

4 結(jié)束語

本文對六自由度工業(yè)機器人運動學進行了研究。采用D-H法建立了機器人連桿坐標系，由正運動學算法得出了以末端位置矢量和歐拉角表示的末端位姿；采用代數(shù)解析法對機器人逆運動學進行求解并用綜合優(yōu)選算法對多組解進行了最優(yōu)解的篩選。運用Microsoft Visual C#2015的WPF設計了3D仿真及控制系統(tǒng)軟件。通過生產(chǎn)現(xiàn)場驗證，表明本文所采用的正、逆運動學算法以及綜合優(yōu)選算法有效并且實現(xiàn)了3D仿真及控制系統(tǒng)中的機器人實時數(shù)字3D運動位姿和機器人本體實際末端位姿保持一致，具有優(yōu)良的機器人運動可視化效果和系統(tǒng)可移植性能。