基于OpenGL的機械臂運動學仿真方案

朱森光

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

關鍵詞：機械臂;機械手;運動學;工具中心點;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?引言

隨著工業4.0時代的到來，越來越多的工廠使用機械臂代替人進行枯燥和重復性的勞動。機械臂實體結構復雜、體積龐大且價格昂貴，由于機械臂是運動裝置使用不當可能還會帶來一定的危險。借助于計算機圖形仿真技術，通過電腦仿真研究機械臂成為一種既經濟又安全的方法，本研究基于OpenGL編程技術[1-2]實現了一種機械臂運動學仿真的方案，可以讓用戶無需購買昂貴的機械臂實體，僅憑一臺普通的電腦即可達到分析機械臂運動學特征和論證項目可行性的目的。

1?機械臂建模

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

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

2?運動學分析

2.1?齊次變換矩陣

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

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

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

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

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

2.2?正運動學求解

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

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

以上即為機器人正運動學的求解過程，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下的姿態矩陣，（Px，Py，Pz）就是點O5在大地坐標系XYZ下的絕對坐標。很多應用還需要再定義一個機械手在坐標系X5Y5Z5下的相對姿態矩陣N=Nx′Ox′Ax′Ny′Oy′Ay′Nz′Oz′Az′，姿態矩陣N就是圖1中坐標系X6Y6Z6在坐標系X5Y5Z5下的相對姿態，工具坐標U（a，b，c）和姿態矩陣N一般通過現場標定的方法[7-9]間接計算求得

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

2.3?逆運動學求解

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

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

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

2.4?姿態控制

2.4.1?歐拉角法

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

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

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

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

2.4.2?四元數法

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

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

四元數Q與前文所述的矩陣M和矩陣N的關系，如式（7）。

3?運動仿真

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

3.1?直線運動仿真

設機械手末端點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為總分割點數，k的范圍為0至N。對每個分割點進行逆運動學求解得到N+1組關節角度θ0、θ1、θ2、θ3、θ4、θ5的值，按照時間序列依次調用OpenGL的API函數glRotated動態繪制機械臂三維模型形成機械手末端頂點直線運動的仿真效果，如圖4所示。

3.2?圓弧運動仿真

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

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

4?編程實現

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

圖6是使用C++開發工具開發的仿真軟件，實現了以下功能。

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

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

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

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

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

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

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

（8） 固定姿態和漸變姿態作直線運動。

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

5?總結

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

參考文獻

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

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

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

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

[5]?John J Craig. 機器人導論[M]. 北京：機械工業出版社，2006.

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

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

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

[9]?李宏勝，汪允鶴. 六關節工業機器人工具坐標系的標定[J]. 自動化技術與應用，2016， 35（8）：101-104.

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

（收稿日期：2019.10.21）