基于OpenGL的機(jī)械臂運(yùn)動(dòng)學(xué)仿真方案

朱森光

摘 要：使用OpenGL技術(shù)建立機(jī)械臂的三維模型，通過(guò)C++編程技術(shù)計(jì)算機(jī)械臂上的機(jī)械手末端作直線或曲線運(yùn)動(dòng)時(shí)對(duì)應(yīng)的機(jī)械臂各關(guān)節(jié)角度的變化數(shù)據(jù)，使用這些角度數(shù)據(jù)去動(dòng)態(tài)繪制機(jī)械臂三維模型實(shí)現(xiàn)機(jī)械臂運(yùn)動(dòng)的動(dòng)畫效果，機(jī)械臂運(yùn)動(dòng)時(shí)將軌跡數(shù)據(jù)和對(duì)應(yīng)的關(guān)節(jié)角度數(shù)據(jù)記錄下來(lái)供后續(xù)分析為機(jī)械臂設(shè)計(jì)和現(xiàn)場(chǎng)應(yīng)用中的軌跡規(guī)劃提供理論依據(jù)從而達(dá)到了運(yùn)動(dòng)學(xué)仿真的目的。

關(guān)鍵詞：機(jī)械臂;機(jī)械手;運(yùn)動(dòng)學(xué);工具中心點(diǎn);OpenGL;仿真

中圖分類號(hào)：TP242

文獻(xiàn)標(biāo)志碼：A

文章編號(hào)：1007-757X（2020）11-0118-05

Abstract：OpenGL technology is used to establish the 3D model of the robotic arm. The C++ programming technique is used to calculate the changing data of the joint angle of the arm when the manipulator end on the arm is used for linear or curved motion. These angle data are used to dynamically draw the 3D model of the robotic arm， and output animation effect of the robot arm movement. The trajectory data and the corresponding joint angle data are recorded. They can be used for the subsequent analysis and provide a theoretical basis for the trajectory planning in the robot arm design and field application. Thereby it achieves the purpose of kinematics simulation.

Key words：robotic arm;manipulator;kinematics;TCP;OpenGL;simulation

0?引言

隨著工業(yè)4.0時(shí)代的到來(lái)，越來(lái)越多的工廠使用機(jī)械臂代替人進(jìn)行枯燥和重復(fù)性的勞動(dòng)。機(jī)械臂實(shí)體結(jié)構(gòu)復(fù)雜、體積龐大且價(jià)格昂貴，由于機(jī)械臂是運(yùn)動(dòng)裝置使用不當(dāng)可能還會(huì)帶來(lái)一定的危險(xiǎn)。借助于計(jì)算機(jī)圖形仿真技術(shù)，通過(guò)電腦仿真研究機(jī)械臂成為一種既經(jīng)濟(jì)又安全的方法，本研究基于OpenGL編程技術(shù)[1-2]實(shí)現(xiàn)了一種機(jī)械臂運(yùn)動(dòng)學(xué)仿真的方案，可以讓用戶無(wú)需購(gòu)買昂貴的機(jī)械臂實(shí)體，僅憑一臺(tái)普通的電腦即可達(dá)到分析機(jī)械臂運(yùn)動(dòng)學(xué)特征和論證項(xiàng)目可行性的目的。

1?機(jī)械臂建模

建立機(jī)械臂模型是實(shí)現(xiàn)對(duì)機(jī)械臂運(yùn)動(dòng)學(xué)進(jìn)行仿真研究的重要前提，本研究將以典型的六軸聯(lián)動(dòng)機(jī)械臂為例進(jìn)行介紹，使用OpenGL技術(shù)建立的一個(gè)機(jī)械臂簡(jiǎn)單模型，如圖1所示。

在圖1中，在關(guān)節(jié)1處建立一個(gè)大地坐標(biāo)系XYZ，關(guān)節(jié)1可以繞Y軸旋轉(zhuǎn);在關(guān)節(jié)2處建立坐標(biāo)系X1Y1Z1，關(guān)節(jié)2可以繞Z1軸旋轉(zhuǎn);在關(guān)節(jié)3處建立坐標(biāo)系X2Y2Z2，關(guān)節(jié)3可以繞Z2軸旋轉(zhuǎn);在關(guān)節(jié)4處建立坐標(biāo)系X3Y3Z3，關(guān)節(jié)4可以繞X3軸旋轉(zhuǎn);在關(guān)節(jié)5處建立坐標(biāo)系X4Y4Z4，關(guān)節(jié)5可以繞Z4軸旋轉(zhuǎn);在關(guān)節(jié)6處建立坐標(biāo)系X5Y5Z5，關(guān)節(jié)6可以繞X5軸旋轉(zhuǎn);點(diǎn)O5是機(jī)械手在機(jī)械臂上的安裝位置，在機(jī)械手末端建立坐標(biāo)系X6Y6Z6。圖1中橢圓圈住的區(qū)域就是機(jī)械手的示意模型。

2?運(yùn)動(dòng)學(xué)分析

2.1?齊次變換矩陣

齊次變換矩陣是機(jī)器人技術(shù)里最重要的數(shù)學(xué)分析工具之一，關(guān)于齊次變換矩陣的原理在很多文獻(xiàn)[3-6]中已經(jīng)描述過(guò)在此不再詳述，這里僅描述使用齊次變換矩陣技術(shù)生成圖1中機(jī)械臂末端點(diǎn)O5位姿矩陣的過(guò)程。

首先定義一些變量符號(hào)，在圖1中，關(guān)節(jié)1繞Y軸旋轉(zhuǎn)的角度定義為θ0，當(dāng)θ0=0時(shí)，O1點(diǎn)在坐標(biāo)系XYZ內(nèi)的坐標(biāo)是（x0，y0，0）;關(guān)節(jié)2繞Z1軸旋轉(zhuǎn)的角度定義為θ1，圖1中的θ1當(dāng)前位置值為+90度;定義O1O2兩點(diǎn)距離為x1， 關(guān)節(jié)3繞Z2軸旋轉(zhuǎn)的角度定義為θ2，圖1中的θ2當(dāng)前位置值為-90度;定義O2O3兩點(diǎn)距離為x2，關(guān)節(jié)4繞X3軸旋轉(zhuǎn)的角度定義為θ3， 圖1中的θ3當(dāng)前位置值為0度;定義O3O4兩點(diǎn)距離為x3，關(guān)節(jié)5繞Z4軸旋轉(zhuǎn)的角度定義為θ4， 圖1中的θ4當(dāng)前位置值為-60度;定義O4O5兩點(diǎn)距離為x4，關(guān)節(jié)6繞X5軸旋轉(zhuǎn)的角度定義為θ5， 圖1中的θ5當(dāng)前位置值為0度。以上描述中的角度正負(fù)值定義符合右手法則，即當(dāng)右手除大拇指以外的其余四個(gè)手指握住旋轉(zhuǎn)軸并且四個(gè)手指指向旋轉(zhuǎn)方向時(shí)如果此時(shí)大拇指指向旋轉(zhuǎn)軸的正方向則旋轉(zhuǎn)角度就是正值，反之就是負(fù)值。

下面根據(jù)以上定義的變量符號(hào)描述齊次變換矩陣。

根據(jù)以上奇次變換矩陣可以寫出機(jī)械臂末端點(diǎn)O5的位姿矩陣C，如式（1）。

式中，Nx，Ox，Ax可以理解為末端點(diǎn)O5姿態(tài)的X軸投影向量;Ny，Oy，Ay可以理解為末端點(diǎn)O5姿態(tài)的Y軸投影向量;Nz，Oz，Az可以理解為末端點(diǎn)O5姿態(tài)的Z軸投影向量。

2.2?正運(yùn)動(dòng)學(xué)求解

正運(yùn)動(dòng)學(xué)求解就是求出式（1）中矩陣C的解析表達(dá)式，將式（1）展開后得到如下。

如果機(jī)械手末端點(diǎn)O6相對(duì)于坐標(biāo)系X5Y5Z5的相對(duì)坐標(biāo)為U（a，b，c），那么機(jī)械手末端點(diǎn)O6在大地坐標(biāo)系XYZ中的坐標(biāo)，如式（2）。

以上即為機(jī)器人正運(yùn)動(dòng)學(xué)的求解過(guò)程，Nx、Ny、Nz、Ox、Oy、Oz、Ax、Ay、Az、Px、Py、Pz表達(dá)式中的x0、y0、x1、x2、x3、x4為機(jī)械固有尺寸，c0、c1、c2、c3、c4、c5分別為cosθ0、cosθ1、cosθ2、cosθ3、cosθ4、cosθ5的簡(jiǎn)寫，s0、s1、s2、s3、s4、s5分別為sinθ0、sinθ1、sinθ2、sinθ3、sinθ4、sinθ5的簡(jiǎn)寫。實(shí)際上C的子矩陣M=NxOxAxNyOyAyNzOzAz就是坐標(biāo)系X5Y5Z5在大地坐標(biāo)系XYZ下的姿態(tài)矩陣，（Px，Py，Pz）就是點(diǎn)O5在大地坐標(biāo)系XYZ下的絕對(duì)坐標(biāo)。很多應(yīng)用還需要再定義一個(gè)機(jī)械手在坐標(biāo)系X5Y5Z5下的相對(duì)姿態(tài)矩陣N=Nx′Ox′Ax′Ny′Oy′Ay′Nz′Oz′Az′，姿態(tài)矩陣N就是圖1中坐標(biāo)系X6Y6Z6在坐標(biāo)系X5Y5Z5下的相對(duì)姿態(tài)，工具坐標(biāo)U（a，b，c）和姿態(tài)矩陣N一般通過(guò)現(xiàn)場(chǎng)標(biāo)定的方法[7-9]間接計(jì)算求得

機(jī)械手末端點(diǎn)O6的位姿矩陣D，如式（3）。

2.3?逆運(yùn)動(dòng)學(xué)求解

機(jī)器人逆運(yùn)動(dòng)學(xué)求解是根據(jù)位姿矩陣C反求六個(gè)關(guān)節(jié)的旋轉(zhuǎn)角θ0、θ1、θ2、θ3、θ4、θ5的問(wèn)題。為了便于求解，對(duì)式（1）進(jìn)行變換，令矩陣S0=R0*T0，然后將式（1）等號(hào)兩邊同時(shí)左乘矩陣S0的逆矩陣S0-1，如式（4）。

式（4）展開后等式左邊矩陣?yán)锏?2個(gè)元素跟等式右邊矩陣?yán)锏?2個(gè)元素互相相等于是就得到如下12個(gè)化簡(jiǎn)后的方程組成的方程組如下。

機(jī)器人逆運(yùn)動(dòng)求解就是根據(jù)以上12個(gè)方程求出θ0、θ1、θ2、θ3、θ4、θ5，由于三角函數(shù)存在非單調(diào)性和周期性的特點(diǎn)，因此會(huì)導(dǎo)致存在多組解，此時(shí)可以根據(jù)機(jī)械手的運(yùn)動(dòng)連續(xù)性條件濾除多余的解，求出θ0、θ1、θ2、θ3、θ4、θ5后就可以用這些角度去編程控制機(jī)械手三維模型作動(dòng)畫運(yùn)動(dòng)實(shí)現(xiàn)運(yùn)動(dòng)學(xué)仿真了。

2.4?姿態(tài)控制

2.4.1?歐拉角法

機(jī)械手末端的姿態(tài)可以用歐拉角來(lái)表示，歐拉角可以描述一個(gè)坐標(biāo)系相對(duì)于另一個(gè)坐標(biāo)系的姿態(tài)，機(jī)械手末端坐標(biāo)系X6Y6Z6相對(duì)于大地坐標(biāo)系XYZ的姿態(tài)就可以用歐拉角來(lái)表示。歐拉角有三個(gè)角分別是俯仰角α、偏航角β和橫滾角γ，在本研究建立的機(jī)械臂模型的坐標(biāo)系中，歐拉角與前文所述的矩陣M和矩陣N的關(guān)系，如式（5）。

由于矩陣M是矩陣C的子矩陣，所以改變歐拉角就能間接改變矩陣C，根據(jù)矩陣C進(jìn)行逆運(yùn)動(dòng)學(xué)求解就可以得出各關(guān)節(jié)的角度了，如圖2所示。

圖2示意了機(jī)械手的兩種姿態(tài)，兩種姿態(tài)之間漸變時(shí)保持機(jī)械手末端頂點(diǎn)的位置不變，僅僅改變機(jī)械手的俯仰角α，那么這種機(jī)械手本體繞末端頂點(diǎn)作俯仰動(dòng)作的運(yùn)動(dòng)就是一種姿態(tài)控制運(yùn)動(dòng)，如圖3所示。

圖3則示意了一種平移運(yùn)動(dòng)，此時(shí)姿態(tài)角α、β、γ均保持不變，僅改變Px的值。

2.4.2?四元數(shù)法

除了可以用歐拉角法來(lái)控制姿態(tài)外還可以用四元數(shù)法控制姿態(tài)，歐拉角法在兩個(gè)或兩個(gè)以上的歐拉角同時(shí)變化時(shí)不能保證姿態(tài)漸變的線性平滑度，而四元數(shù)法則沒有這個(gè)缺陷。

四元數(shù)表達(dá)式為Q=Qx*i+Qy*j+Qz*k+Qw，它有三個(gè)虛部和一個(gè)實(shí)部，假設(shè)A、B兩個(gè)坐標(biāo)系剛開始重合或平行，現(xiàn)在讓坐標(biāo)系B繞坐標(biāo)系A(chǔ)中的單位矢量軸u=ai+bj+ck旋轉(zhuǎn)一個(gè)角度θ，則坐標(biāo)系B在坐標(biāo)系A(chǔ)中的姿態(tài)可用四元數(shù)Q表示，如式（6）。

四元數(shù)Q與前文所述的矩陣M和矩陣N的關(guān)系，如式（7）。

3?運(yùn)動(dòng)仿真

運(yùn)動(dòng)仿真的對(duì)象是機(jī)械臂上安裝的機(jī)械手，不同的應(yīng)用場(chǎng)合需要安裝不同形狀和功能的機(jī)械手，很多資料上也將機(jī)械手稱之為工具[9-10]，所以才有前文中的工具坐標(biāo)U（a，b，c）這一描述。機(jī)械手的運(yùn)動(dòng)參考點(diǎn)也分好幾種，有以機(jī)械手的質(zhì)心為參考點(diǎn)的，有以機(jī)械手的形心為參考點(diǎn)的，也有以機(jī)械手上某一關(guān)鍵作用點(diǎn)為參考點(diǎn)的，運(yùn)動(dòng)參考點(diǎn)也稱之為工具中心點(diǎn)（Tool center point，TCP），在本研究建立的機(jī)械模型中將以機(jī)械手末端點(diǎn)O6為運(yùn)動(dòng)參考點(diǎn)來(lái)作運(yùn)動(dòng)仿真。

3.1?直線運(yùn)動(dòng)仿真

設(shè)機(jī)械手末端點(diǎn)O6作直線運(yùn)動(dòng)時(shí)起始點(diǎn)在XYZ坐標(biāo)系下的坐標(biāo)為A（srcX，srcY，srcZ），終止點(diǎn)在XYZ坐標(biāo)系下的坐標(biāo)為B（dstX，dstY，dstZ），則直線上任一點(diǎn)的坐標(biāo)可表示為P（（dstX-srcX）*k/N，（dstY-srcY）*k/N，（dstZ-srcZ）*k/N），其中N為總分割點(diǎn)數(shù)，k的范圍為0至N。對(duì)每個(gè)分割點(diǎn)進(jìn)行逆運(yùn)動(dòng)學(xué)求解得到N+1組關(guān)節(jié)角度θ0、θ1、θ2、θ3、θ4、θ5的值，按照時(shí)間序列依次調(diào)用OpenGL的API函數(shù)glRotated動(dòng)態(tài)繪制機(jī)械臂三維模型形成機(jī)械手末端頂點(diǎn)直線運(yùn)動(dòng)的仿真效果，如圖4所示。

3.2?圓弧運(yùn)動(dòng)仿真

假設(shè)已知圓弧上三點(diǎn)的坐標(biāo)，首先根據(jù)三點(diǎn)確定三點(diǎn)所在平面的方程，聯(lián)立三點(diǎn)到圓心的距離相等可以求得圓心坐標(biāo)A（Cx，Cy，Cz）和半徑r，同時(shí)也可以求得圓所在平面內(nèi)從圓心出發(fā)的一對(duì)正交單位向量U和V。設(shè)P（x，y，z）為圓上任意一點(diǎn)，圓的參數(shù)方程，如式（9）—式（11）。

對(duì)式（9）-式（11）中的θ進(jìn)行采樣產(chǎn)生圓弧上一組點(diǎn)的坐標(biāo)，對(duì)這些點(diǎn)的坐標(biāo)進(jìn)行逆運(yùn)動(dòng)學(xué)求解得到一組關(guān)節(jié)角度θ0、θ1、θ2、θ3、θ4、θ5的值，按照時(shí)間序列依次調(diào)用OpenGL的API函數(shù)glRotated動(dòng)態(tài)繪制機(jī)械臂三維模型形成機(jī)械手末端頂點(diǎn)圓弧運(yùn)動(dòng)的仿真效果，如圖5所示。

4?編程實(shí)現(xiàn)

通過(guò)前面的分析發(fā)現(xiàn)機(jī)械臂運(yùn)動(dòng)學(xué)方程是一組非線性方程組，由于各關(guān)節(jié)之間的正交性，所以存在關(guān)節(jié)角度θ0、θ1、θ2、θ3、θ4、θ5的解析表達(dá)式，可以用C++編程技術(shù)算出關(guān)節(jié)角度，使用關(guān)節(jié)角度編程控制機(jī)械臂三維模型動(dòng)態(tài)刷新視圖形成動(dòng)畫效果達(dá)到仿真的目的，如圖6所示。

圖6是使用C++開發(fā)工具開發(fā)的仿真軟件，實(shí)現(xiàn)了以下功能。

（1） 通過(guò)界面輸入六個(gè)關(guān)節(jié)角度進(jìn)行正運(yùn)動(dòng)學(xué)求解得到機(jī)械臂末端的位姿矩陣C。

（2） 通過(guò)界面輸入機(jī)械臂末端的位姿矩陣C進(jìn)行逆運(yùn)動(dòng)學(xué)求解得到六個(gè)關(guān)節(jié)角度。

（3） 通過(guò)界面控制末端機(jī)械手的俯仰動(dòng)作。

（4） 通過(guò)界面控制末端機(jī)械手做上下平移運(yùn)動(dòng)。

（5） 通過(guò)界面控制末端機(jī)械手做前后平移運(yùn)動(dòng)。

（6） 通過(guò)界面控制末端機(jī)械手做左右平移運(yùn)動(dòng)。

（7） 通過(guò)界面控制末端機(jī)械手的歐拉角。

（8） 固定姿態(tài)和漸變姿態(tài)作直線運(yùn)動(dòng)。

（9） 三點(diǎn)確定空間圓弧作圓弧運(yùn)動(dòng)。

5?總結(jié)

本研究通過(guò)建立合適的機(jī)械臂模型和相對(duì)坐標(biāo)系，借助于齊次變換矩陣、歐拉角和四元數(shù)技術(shù)演算得出了機(jī)械臂正逆運(yùn)動(dòng)學(xué)的一系列公式，以機(jī)械手末端作直線和圓弧運(yùn)動(dòng)為例計(jì)算軌跡數(shù)據(jù)和對(duì)應(yīng)的關(guān)節(jié)角度數(shù)據(jù)，使用這些數(shù)據(jù)去動(dòng)態(tài)刷新機(jī)械臂三維模型形成動(dòng)畫效果從而達(dá)到了機(jī)械臂運(yùn)動(dòng)學(xué)仿真的目的。本方案具備詳細(xì)的數(shù)據(jù)記錄功能、良好的交互性和可擴(kuò)展的界面框架，為后續(xù)更深層次的機(jī)械臂技術(shù)研究奠定了基礎(chǔ)。

參考文獻(xiàn)

[1]?喬林，費(fèi)廣正. OpenGL程序設(shè)計(jì)[M]. 北京：清華大學(xué)出版社，2000.

[2]?約翰·克賽尼希. OpenGL編程指南[M].北京：機(jī)械工業(yè)出版社，2017.

[3]?熊友倫，丁漢，劉恩滄. 機(jī)器人學(xué)[M]. 武漢：華中科技大學(xué)出版社，1993.

[4]?蔡自興. 機(jī)器人學(xué)[M]. 北京：清華大學(xué)出版社，2000.

[5]?John J Craig. 機(jī)器人導(dǎo)論[M]. 北京：機(jī)械工業(yè)出版社，2006.

[6]?楊靜宇，李德昌. 機(jī)器人學(xué)[M]. 北京：中國(guó)科學(xué)技術(shù)出版社，1989.

[7]?李德釗，鄧華. 基于改進(jìn)型RLM算法的六軸機(jī)械臂運(yùn)動(dòng)學(xué)標(biāo)定實(shí)驗(yàn)[J]. 測(cè)控技術(shù)，2019， 38（1）：42-48.

[8]?李祥云，王同合，范百興，向民志. 基于激光跟蹤儀的機(jī)器人工具坐標(biāo)系標(biāo)定[J]. 測(cè)繪科學(xué)與工程，2017（2）：19-22.

[9]?李宏勝，汪允鶴. 六關(guān)節(jié)工業(yè)機(jī)器人工具坐標(biāo)系的標(biāo)定[J]. 自動(dòng)化技術(shù)與應(yīng)用，2016， 35（8）：101-104.

[10]?陳欣欣. 工業(yè)機(jī)器人工具坐標(biāo)系建立的教學(xué)策略淺談[J]. 福建輕紡，2019（1）：47-50.

（收稿日期：2019.10.21）