朱森光

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

關(guān)鍵詞：機械臂;機械手;運動學(xué);工具中心點;OpenGL;仿真

中圖分類號：TP242

文獻標志碼：A

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

1?機械臂建模

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

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

2?運動學(xué)分析

2.1?齊次變換矩陣

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

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

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

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

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

2.2?正運動學(xué)求解

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

如果機械手末端點O6相對于坐標系X5Y5Z5的相對坐標為U（a，b，c），那么機械手末端點O6在大地坐標系XYZ中的坐標，如式（2）。

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

機械手末端點O6的位姿矩陣D，如式（3）。

2.3?逆運動學(xué)求解

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

式（4）展開后等式左邊矩陣里的12個元素跟等式右邊矩陣里的12個元素互相相等于是就得到如下12個化簡后的方程組成的方程組如下。

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

2.4?姿態(tài)控制

2.4.1?歐拉角法

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

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

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

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

2.4.2?四元數(shù)法

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

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

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

3?運動仿真

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

3.1?直線運動仿真

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

3.2?圓弧運動仿真

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

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

4?編程實現(xiàn)

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

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

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

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

（3） 通過界面控制末端機械手的俯仰動作。

（4） 通過界面控制末端機械手做上下平移運動。

（5） 通過界面控制末端機械手做前后平移運動。

（6） 通過界面控制末端機械手做左右平移運動。

（7） 通過界面控制末端機械手的歐拉角。

（8） 固定姿態(tài)和漸變姿態(tài)作直線運動。

（9） 三點確定空間圓弧作圓弧運動。

5?總結(jié)

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

參考文獻

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

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

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

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

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

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

[7]?李德釗，鄧華. 基于改進型RLM算法的六軸機械臂運動學(xué)標定實驗[J]. 測控技術(shù)，2019， 38（1）：42-48.

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

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

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

（收稿日期：2019.10.21）