□郭小寶 □趙 振 □陳落根杭州娃哈哈集團(tuán)機(jī)電研究院智能裝備研究所 杭州 310020

碼垛機(jī)器人運(yùn)動(dòng)學(xué)幾何解法的研究*

□郭小寶 □趙 振 □陳落根
杭州娃哈哈集團(tuán)機(jī)電研究院智能裝備研究所 杭州 310020

機(jī)器人的正向、逆向運(yùn)動(dòng)學(xué)是機(jī)器人運(yùn)動(dòng)控制系統(tǒng)與機(jī)械系統(tǒng)連接的橋梁，其封閉解問題至今還沒有通用的解法，比較通用的是代數(shù)方程組的數(shù)值迭代算法，通常難以保證實(shí)時(shí)性和精度要求。基于幾何方法通過分析碼垛機(jī)器人的機(jī)構(gòu)特點(diǎn)簡化運(yùn)動(dòng)學(xué)模型，詳細(xì)推導(dǎo)正向運(yùn)動(dòng)學(xué)和逆向運(yùn)動(dòng)學(xué)解法，推導(dǎo)過程簡單、直觀。最后通過MATLAB編程和SolidWorks三維實(shí)體模型運(yùn)動(dòng)仿真，驗(yàn)證幾何解法的正確性。

多關(guān)節(jié)重載碼垛機(jī)器人在工業(yè)生產(chǎn)搬運(yùn)領(lǐng)域占有重要市場，隨著中國制造自動(dòng)化進(jìn)程的加快，得到越來越多的應(yīng)用［1-2］。

機(jī)器人的正向、逆向運(yùn)動(dòng)學(xué)是機(jī)器人運(yùn)動(dòng)控制系統(tǒng)與機(jī)械系統(tǒng)連接的橋梁，是指導(dǎo)機(jī)器人動(dòng)作指令的核心部分，簡單且能夠快速執(zhí)行的運(yùn)動(dòng)學(xué)算法是機(jī)器人實(shí)現(xiàn)高速、高精度運(yùn)行的重要手段［3-4］。

機(jī)器正向運(yùn)動(dòng)學(xué)是已知關(guān)節(jié)變量，求解末端執(zhí)行器的空間位置和姿態(tài)；逆向運(yùn)動(dòng)學(xué)是已知末端執(zhí)行器的位置和姿態(tài)來求解關(guān)節(jié)變量。機(jī)器人運(yùn)動(dòng)學(xué)算法主要有解析法和數(shù)值法，解析法根據(jù)機(jī)構(gòu)的結(jié)構(gòu)組成特征建立約束方程組，采用多種方法從約束方程組中消去中間參數(shù)，獲得單參數(shù)多項(xiàng)式后再求解，方法包括矢量代數(shù)法、幾何法、矩陣法和四元數(shù)法等，優(yōu)點(diǎn)是可以得到全部解，缺點(diǎn)在于難度較大，方法通用性不好。數(shù)值法是直接求解約束方程組，可以通過迭代運(yùn)算求得任何機(jī)構(gòu)的實(shí)數(shù)解，但一般不能得到全部解，一般而言，初值選取及搜索算法對(duì)收斂性和精度影響較大［5-7］。

筆者通過分析碼垛機(jī)器人的機(jī)構(gòu)特點(diǎn)，簡化運(yùn)動(dòng)學(xué)模型，基于傳統(tǒng)幾何方法，詳細(xì)推導(dǎo)正向運(yùn)動(dòng)學(xué)和逆向運(yùn)動(dòng)學(xué)解法，推導(dǎo)過程力求簡單、直觀。最后通過實(shí)際工程實(shí)例，采用MATLAB編程和基于SolidWorks三維實(shí)體模型運(yùn)動(dòng)仿真，驗(yàn)證幾何解法的準(zhǔn)確性。

1 碼垛機(jī)器人運(yùn)動(dòng)學(xué)模型

圖1所示的是雙平行四邊形碼垛機(jī)器人，其基礎(chǔ)部件包括底座、腰部、大臂、小臂、手腕、末端法蘭，實(shí)現(xiàn)空間范圍內(nèi)的三平動(dòng)和繞腰部軸線轉(zhuǎn)動(dòng)的4個(gè)自由度。由三角架及其連桿從動(dòng)部件與大臂和小臂構(gòu)成三組平行四邊形，實(shí)現(xiàn)在搬運(yùn)過程中末端手腕的水平。

圖1 關(guān)節(jié)型碼垛機(jī)器人三維模型

機(jī)器人的機(jī)構(gòu)簡圖如圖2所示，Oi-xiyizi（i=0～5）分別為底座、腰部、大臂、小臂、手腕、末端法蘭的轉(zhuǎn)動(dòng)中心和連體坐標(biāo)系，θi（i=1～5）分別為其轉(zhuǎn)動(dòng)關(guān)節(jié)的關(guān)節(jié)變量，di、ai為機(jī)器人的桿長參數(shù)。

考慮機(jī)構(gòu)運(yùn)動(dòng)特點(diǎn)，可以將圖2簡化為圖3，機(jī)器人末端法蘭的連體坐標(biāo)系O5-x5y5z5在底座的連體坐標(biāo)系O0-x0y0z0下的位置坐標(biāo)為x、y、z和歐拉角姿態(tài)坐標(biāo)（Z-Y-Z順規(guī)）為φ、ψ、θ，考慮到機(jī)構(gòu)為四自由度機(jī)器人，具備三移動(dòng)和繞Z軸轉(zhuǎn)動(dòng)的4個(gè)自由度，從而φ=0、ψ=0。

機(jī)器人的桿長參數(shù)和關(guān)節(jié)變量見表1。

基于平行四邊形特點(diǎn)，驅(qū)動(dòng)關(guān)節(jié)變量滿足：

θ3=-θ2-θ4（1）

圖2 機(jī)器人機(jī)構(gòu)簡圖

圖3 機(jī)器人簡化機(jī)構(gòu)簡圖

表1 機(jī)器人桿長參數(shù)和關(guān)節(jié)變量

從而機(jī)器人的4個(gè)獨(dú)立驅(qū)動(dòng)關(guān)節(jié)變量為腰部關(guān)節(jié)θ1、大臂關(guān)節(jié)θ2、腕部關(guān)節(jié)θ4和末端法蘭θ5。

機(jī)器人回零狀態(tài)（大臂豎直，小臂水平）下：θ1=0°，θ2=-90°，θ3=90°，θ4=0°，θ5=0°。

2 運(yùn)動(dòng)學(xué)幾何解法分析

機(jī)器人運(yùn)動(dòng)學(xué)指機(jī)器人驅(qū)動(dòng)關(guān)節(jié)變量與末端剛體獨(dú)立位姿坐標(biāo)之間的映射關(guān)系。

2.1 正向運(yùn)動(dòng)學(xué)

求解操作空間變量：u=（xy zθ）T

由圖3容易求得末端法蘭相對(duì)于底座連體坐標(biāo)系的轉(zhuǎn)角θ：

θ=θ1+θ5（2）

θ3根據(jù)式（1）求得。

在x'0軸和z0軸構(gòu)成的平面內(nèi)，O0O5沿x'0軸的分量可表示為：

l1=a1+a2sinθ2+a3sin（θ2+θ3）+a4（3）

O0O5沿z0軸的分量即z為：

z=d1+d2+a2cosθ2+a3cos（θ2+θ3）+d5（4）

末端法蘭的連體坐標(biāo)系O5-x5y5z5在底座的連體坐標(biāo)系O0-x0y0z0下的位置坐標(biāo)x、y可通過l1求出：

x=l1cosθ1

y=l1sinθ1（5）

從而完成正向運(yùn)動(dòng)學(xué)求解，表示為：

x=（a1+a2sinθ2+a3sin（θ2+θ3）+a4）cosθ1

y=（a1+a2sinθ2+a3sin（θ2+θ3）+a4）sinθ1

z=d1+d2+a2cosθ2+a3cos（θ2+θ3）+d

θ=θ1+θ5

2.2 逆向運(yùn)動(dòng)學(xué)

根據(jù)式（5），關(guān)節(jié)1轉(zhuǎn)角θ1容易由x、y求出，考慮到反正切函數(shù)不能確定θ1所在象限，從而通過四象限反正切函數(shù)求得：

θ1=arctan［2（y，x）］ （7）

由式（2）可求出θ5：

θ5=θ-θ1（8）

在三角形△O2O3O4中，邊O2O4可用已知末端法蘭的坐標(biāo)值求得，記為m：

O2O4相對(duì)于x'0軸的夾角為α，可用已知末端法蘭的坐標(biāo)值求得：

三角形△O2O3O4中∠O3O2O4設(shè)為β，根據(jù)余弦定理可得：同理，求∠O2O3O4，記為γ：

由于機(jī)構(gòu)特點(diǎn)，θ2和θ3會(huì)有兩組解：｛θ2=-α-β

θ3=180°-γ或（13）

θ3根據(jù)式（1）求得。從而完成逆向運(yùn)動(dòng)學(xué)求解。

3 工程實(shí)例驗(yàn)證

3.1 MATLAB驗(yàn)證

在MATLAB開發(fā)環(huán)境下，根據(jù)前文的幾何算法編寫如圖4所示的正向、逆向運(yùn)動(dòng)學(xué)算法腳本，腳本不足30行代碼，完全順序結(jié)構(gòu)，沒有任何數(shù)值迭代，具有很高的執(zhí)行速度。

圖4 運(yùn)動(dòng)學(xué)算法的MATLAB腳本截圖

運(yùn)動(dòng)學(xué)測試點(diǎn)見表2，通過MATLAB校驗(yàn)，測試點(diǎn)滿足正向運(yùn)動(dòng)學(xué)和逆向運(yùn)動(dòng)學(xué)算法，從而驗(yàn)證了幾何解法的有效性和正確性。

表2 運(yùn)動(dòng)學(xué)算法驗(yàn)證點(diǎn)對(duì)應(yīng)關(guān)系

3.2 運(yùn)動(dòng)仿真驗(yàn)證

為了反映運(yùn)動(dòng)學(xué)幾何算法的真實(shí)性，在Solid-Works三維建模軟件下，設(shè)計(jì)真實(shí)的碼垛機(jī)器人模型，定義好配合和約束條件，為運(yùn)動(dòng)學(xué)仿真準(zhǔn)備。

首先，規(guī)劃機(jī)器人工作空間末端法蘭的運(yùn)動(dòng)軌跡，即逆向運(yùn)動(dòng)學(xué)的輸入u=（x y z θ）T，其中x=1 000 mm，θ=0°，在y-z平面的軌跡如圖5所示。

通過逆向運(yùn)動(dòng)學(xué)和軌跡規(guī)劃，計(jì)算出各驅(qū)動(dòng)軸的關(guān)節(jié)角位移，如圖6所示。

將各關(guān)節(jié)的角位移導(dǎo)入基于SolidWorks設(shè)計(jì)的碼垛機(jī)器人模型作為各關(guān)節(jié)驅(qū)動(dòng)馬達(dá)的輸入量，經(jīng)過實(shí)時(shí)運(yùn)動(dòng)仿真，獲得如圖7所示的真實(shí)模擬軌跡，該仿真的軌跡與預(yù)先規(guī)劃的軌跡完全一致，從而驗(yàn)證了幾何運(yùn)動(dòng)學(xué)解法的正確性。

4 結(jié)論

基于幾何學(xué)方法研究了雙平行四邊形碼垛機(jī)器人運(yùn)動(dòng)學(xué)的解析算法，得出了以下結(jié)論。

圖5 機(jī)器人末端控制點(diǎn)在y-z平面內(nèi)軌跡

（1）分析了雙平行四邊形碼垛機(jī)器人，它是三平動(dòng)一轉(zhuǎn)動(dòng)的機(jī)構(gòu)，獨(dú)立驅(qū)動(dòng)的關(guān)節(jié)分別為腰部、大臂、手腕和末端法蘭。

（2）建立了機(jī)器人的機(jī)構(gòu)簡化模型，為幾何法求解運(yùn)動(dòng)學(xué)奠定了基礎(chǔ)。

（3）基于立體幾何和矢量的方法，詳細(xì)推導(dǎo)了正向運(yùn)動(dòng)學(xué)和逆向運(yùn)動(dòng)學(xué)算法，獲得了簡單直觀的運(yùn)動(dòng)學(xué)計(jì)算公式。

圖6 驅(qū)動(dòng)馬達(dá)的角位移曲線

（4）以MATLAB和SolidWorks軟件為工具，通過三維運(yùn)動(dòng)仿真，驗(yàn)證了機(jī)器人幾何解法的正確性和有效性。

圖7 碼垛機(jī)器人運(yùn)動(dòng)仿真末端真實(shí)軌跡

［1］游瑋.雙平行四邊形高速重載搬運(yùn)機(jī)器人動(dòng)態(tài)設(shè)計(jì)與控制研究［D］.哈爾濱：哈爾濱工業(yè)大學(xué)，2011.

［2］王昊杰，薛強(qiáng).一種三坐標(biāo)并聯(lián)動(dòng)力頭——Sprint Z3的運(yùn)動(dòng)學(xué)逆解及尺度綜合 ［J］.機(jī)床與液壓，2011（17）：53-56.

［3］黃真.高等空間機(jī)構(gòu)學(xué)［M］.北京：高等教育出版社，2004.

［4］陸晶，高國琴，朱彩紅，等.新型三平移并聯(lián)機(jī)構(gòu)的運(yùn)動(dòng)分析和工作空間分析 ［J］.機(jī)械設(shè)計(jì)與制造，2007（11）：163-165.

［5］ Li Yangmin，Xu Qingsong.Kinematic Analysis ofa 3-PRS Parallel Manipulator［J］.Robotics and Computer Integrated Manufacturing，2007，23（4）：395-408.

［6］劉辛軍，汪勁松，李劍鋒，等.一種新型空間3自由度并聯(lián)機(jī)構(gòu)的正反解及工作空間分析 ［J］.機(jī)械工程學(xué)報(bào)，2001（10）：36-39.

［7］趙杰，朱延河，蔡鶴皋.Delta型并聯(lián)機(jī)器人運(yùn)動(dòng)學(xué)正解幾何解法［J］.哈爾濱工業(yè)大學(xué)學(xué)報(bào)，2003（1）：25-27.

Forward，inverse kinematics of the robot is a bridge to connect control system and mechanical system for robot motion and currently there is no universal solved process for the closed form solution.Although the numerical iterative algorithm of algebraic equations is in common use，however it is difficult to guarantee timeliness and accuracy requirements.By analyzing the mechanical feature of palletizing robot to simplify kinematics model based on the method of geometry，it is possible toderive detailed solving process for the forward and the inverse kinematics.The derivation process is simple and intuitive.Finally the correctness of the Geometric solution is verified by MATLABprogramming and motion simulation of SolidWorks 3-D solid model.

碼垛機(jī)器人；幾何解法；正向運(yùn)動(dòng)學(xué)；逆向運(yùn)動(dòng)學(xué)

Palletizing Robot；Geometric Solution；Forward Kinematics；Inverse Kinematics

TP242.2

A

1672-0555（2015）04-044-04

*國家科技重大專項(xiàng)（編號(hào)：2011ZX04013-011）

2015年8月

郭小寶（1988-），男，碩士，助理工程師，主要從事機(jī)器人機(jī)構(gòu)學(xué)及其運(yùn)動(dòng)控制的研究