6R機器人工具端的運動學建模及仿真

廖偉東，李鍛能，王 強，廖 姣

（廣東工業大學 機電工程學院，廣東 廣州 510006）

1 引言

打磨、焊接機器人需要在末端裝夾磨具、焊槍等工具，而且工具工作點通常與機器人前一連桿坐標系存在沿X、Y、Z軸方向的偏置。機器人進行加工作業時，需要對工具的工作點進行路徑規劃。根據機器人不同的工作需要，獲取機器人運動軌跡有在線示教再現法和機器人離線編程兩種方法。

對于機器人運動學D-H法的建模，已有大量相關的研究，如文獻[1-2]，其研究的末端控制點設置在機器人手腕或末端法蘭盤中心，這種建立機器人運動學模型的方法，只能滿足機器人在線示教編程的需要。對機器人離線編程，首先在工件3D模型上提取加工路徑參數，生成機器人運動軌跡，然后需要控制機器人工具工作點的位姿。所以機器人控制點設置在手腕或末端法蘭盤中心的方法，無法滿足離線編程工作需求。研究的機器人控制點設置在工具工作點上，且相對于前一連桿存在沿X、Y、Z軸方向的偏置，在結構上不符合Pieper準則[3]，因此不能直接使用D-H法建立機器人工具端的運動學模型。

通過將工具坐標系從機器人連桿坐標系中獨立出來，先使用D-H法求得機器人手腕坐標系相對于機器人基坐標系的位姿坐標，再通過平移齊次變換求得工具坐標系的位姿坐標。最后利用Visual C++、OpenGL圖形庫搭建仿真平臺，對機器人運動學進行仿真驗證。

2 機器人正運動學分析

2.1 建立機器人運動學模型

以三菱RV-13FL-D機器人作為研究對象，根據其連桿尺寸參數，建立的機器人連桿坐標系，如圖1所示。再依據機器人連桿坐標系建立機器人D-H連桿參數表，如表1所示。

圖1 機器人連桿坐標系Fig.1 Connecting Rod Coordinate of Robot

表1 機器人D-H連桿參數Tab.1 D-H Link Parameters of Robot

其中，連桿長度ai-1為zi-1與zi軸的距離；連桿扭角?i-1為Zi-1與Zi軸的夾角；連桿轉角θi為Xi-1與Xi軸的夾角；連桿距離di為Xi-1與Xi軸的距離。

2.2 機器人腕部運動學正解

機器人的正運動學建模，即是已知機器人六個關節的轉角參數，求解機器人末端標志點相對于機器人基坐標系的位姿，根據表1的機器人D-H連桿參數，建立機器人相鄰連桿齊次變換矩陣i-1Ti，如式1所示：

式中：Cθi=cosθi；Sθi=sinθi，以下類似。

這里的機器人手腕中心P點，與連桿6坐標系原點重合。所以機器人手腕坐標系｛P｝相對于與固定在地面上的機器人基坐標系｛O｝的位姿坐標，等于連桿6與連桿0的變換矩陣，所以可以求得機器人手腕坐標系｛P｝的正解，即如式（2）所示：

2.3 機器人工具端運動學正解

由于工具末端坐標點G相對與機器人手腕坐標系｛P｝，存在沿X、Y、Z軸的平移偏置，姿態與第六連桿保持一致，如圖1所示。設點G相對與P點的相對坐標為（Δx，Δy，Δz）所以工具坐標系原點G相對機器人手腕中心點P的平移齊次變換矩陣PAG，如式（3）所示：

根據連桿坐標變換，求得工具坐標系｛G｝的正解：

2.4 實現RPY組合變換O

由于通過TP變換矩陣表達機器人正解的方式不夠直觀，所以通常利用歐拉變換或者RPY變換以表達機器人末端的運動姿態。采用的是RPY組合變換表示運動姿態，即是，先繞X軸旋轉ψ角，再繞基坐標系的Y軸旋轉θ角，最后繞基坐標系的Z軸旋轉φ角，得到：

因為 RPY（φ，θ，ψ）與OTP矩陣中的姿態參數相對應，所以可令兩式中對應的元素相等分別求得θ、ψ、φ，如下：

所以機器人工具坐標點G的正解OTG的RPY位姿表達式：

｛G｝=（Gx，Gy，Gz，ψ，θ，φ）

3 機器人運動學逆解

機器人運動學逆解的求解，即是根據已知的機器人工具坐標系｛G｝的位姿坐標，先求得機器人手腕坐標系｛P｝位姿坐標，然后反求機器人各關節角。由機器人連桿坐標系可以看出，其第4、5、6關節軸相交于一點，結構上滿足Pieper準則[3]，可以采取封閉解法對機器人逆運動學進行求解。

3.1 機器人手腕位姿坐標求解

已知工具坐標系位姿｛G｝即OTG，反求機器人手腕位姿坐標｛P｝即0T6，由2.3節可知：

3.2 機器人關節逆解

（1）求解 θ1、θ2、θ3

（0T1）-1·0T6=1T6，利用左右兩式（2，4）元素對應相等求得：θ1=atan2（Py，Px），θ′1=atan2（-Py，-Px）

因為2T5=（1T2）-1（0T1）-1（0T6）（5T6）-1，使得等式左右兩邊的（1，4）（2，4）元素分別對應相等，則可以求得：

將式（5）、式（6）分別乘以 w2與 w1，然后將兩式相減，求得：

θ3=atan（a3w2-d4w1，a3w1+d4w2）

其中，w1=c2m-s2n-a2；w2=-s2m-c2n

（2）求解 θ5、θ4、θ6

使得式中左、右兩側的（2，3）元素相等，即可以求得：

再利用式中左、右兩側（1，3）（3，3）元素分別對應相等求得：

最后，使式中左、右兩邊（2，1）（2，2）元素分別對應相等求得：

至此，已經求得了機器人全部六個關節轉角：θ1，θ2，θ3，θ4，θ5，θ6的解。由于6R機器人最多具有8組封閉解[4]，而實際控制中只需要選擇其中的一組解。在此先進行各組逆解關節范圍的校核，排除超出關節運動范圍的解，然后引入最佳柔順解準則[4]如式（8），得出Ex最小的一組解，從而選取最佳的一組逆解。

4 機器人工具端的軌跡運動仿真

利用Visual C++與OpenGL圖形庫搭建機器人仿真平臺。通過讀取機器人工具端經歷的空間任意不共線三點的坐標，作為機器人空間圓弧軌跡規劃示例。根據空間三點圓弧軌跡規劃運算求取圓弧上的各個插補點坐標。然后在仿真平臺上讀入逆解求得的機器人各關節轉角參數，進行機器人連桿坐標變換，最終實現機器人空間三點圓弧軌跡運動仿真。

4.1 機器人空間三點圓弧軌跡規劃

設在笛卡爾坐標空間中有不共線的三點P1（x1，y1，z1），P2（x2，y2，z2），P3（x3，y3，z3），則在空間中一定存在一段通過這三個點的圓弧，設該圓弧的圓心為P0點，如圖2所示。

圖2 圓弧軌跡規劃示意圖Fig.2 Diagram of Circular Trajectory Planning

首先求該圓弧的半徑、圓心角、圓心坐標：根據兩點間距離

則圓周角：

根據圓周角與圓心角的關系得：w=2β；同理可求得圓心角?，則該段圓弧對應的圓心角：θ=w+?

在P1P2P3平面中以圓心P0作為坐標原點，新建一個Po-UVW坐標系，如圖3所示。求出Po-UVW在基坐標系O-XYZ下的變換矩陣A，就可以把三維空間圓弧轉化為二維平面UPoV上的平面圓弧進行處理[5]。在UP0V平面上進行圓弧插補設插補周期為TS插補角速度為w，則可以求得插補次數：N=θ/（wTs）

則求得圓心角的時間函數：

θi＝w（iTs）；i=0，1，2，…，N

可求得UP0V平面上的圓弧插補點坐標為：

將UP0V平面上的插補點轉換為基坐標系下的齊次坐標：

4.2 搭建機器人運動仿真平臺

OpenGL（Open Graphic Library）是一個跨平臺、功能強大、調用方便的底層3D圖形庫，已成為三維圖形開發標準[6]。STL文件由多個帶矢量方向的三角面片的定義組成[7]。機器人仿真系統是通過在Visual C++平臺建立MFC單文檔應用程序，調用OpenGL圖形庫函數建立的。利用OpenGL開源庫，通過導入連桿STL文件的方式對機器人結構進行繪制。調用OpenGL庫函數glRotatef、glTranslated，并且依照機器人連桿參數在函數中讀入機器人各個關節角參數，進行機器人連桿坐標的變換，最終完成機器人仿真平臺的搭建。

4.3 機器人工具端的圓弧軌跡運動仿真

通過“機器人示教”對話框，如圖3所示。利用“關節模式”或“直交模式”點動控制機器人末端位姿，并在“直交坐標”、“關節坐標”對話框中，分別實時顯示當前末端工具位姿的坐標值。在“插補運動”對話框，如圖4所示。中分別讀取機器人末端工具點歷經空間不共線的三點P1、P2、P3的位置坐標，通過空間圓弧軌跡插補運算，可得到的插補運動軌跡，如圖5所示。

圖3 機器人示教對話框Fig.3 Dialog Box of Robot Teaching

圖4 插補運動對話框Fig.4 Dialog Box of Interpolation Motion

圖5 機器人空間圓弧軌跡運動仿真Fig.5 Interpolation Motion Trail Simulation of Robot

將圓弧軌跡規劃中得到的機器人工具末端插補點位姿坐標，代入機器人逆解運算求得各關節角的值。然后將其存儲在thetaRecord數組中。通過SetTimer函數設置一個時間間隔為25ms的定時器，在OnTimer定時器響應函數中調用OpenGL庫函數glRotatef，依序讀入thetaRecord數組中的關節角數值，進行機器人連桿變換。通過系統函數InvalidateRect實時刷新機器人工作窗口，即可實現機器人自動連續的軌跡運動仿真。

5 結束語

針對機器人工具坐標系相對于前一連桿坐標系存在沿X、Y、Z軸方向的偏置，無法直接使用D-H法建立機器人工具端運動學模型的問題，研究了將工具坐標系與機器人連桿坐標系分離，利用D-H法結合坐標平移齊次變換的方法，建立機器人運動學模型，并求得面向工具坐標系的機器人運動學正、逆解。運用RPY組合變換表達機器人工具坐標的運動位姿，并給出了各個參數的代數解。提出一種余弦定理結合圓心角定律的機器人空間三點圓弧軌跡規劃方法，避免了如文獻[5]中坐標變換矩陣的求逆運算，大大簡化了求解過程。通過VC++、OpenGL圖形庫，搭建機器人運動仿真系統，實現機器人空間圓弧軌跡運動的可視化仿真，使機器人運動學研究更直觀。驗證了機器人工具端的運動學模型和空間三點圓弧軌跡規劃算法是具有可行性以及準確性的，為后續的機器人離線編程、動力學分析提供了可靠的依據。