六自由度機械臂運動數學模型的建立

田小靜，李萬軍

（西安航空職業技術學院 自動化工程系，西安 710089）

0 引言

如何為機械臂輸入正確的指令序列以使其完成相應的任務，就要解決機械臂的運動的路徑規劃。六自由度機械臂位姿研究中，可簡化為7個連桿通過六個關節相連的機構，每個關節對應于一個自由度，通過旋轉六個關節可以使機器臂的指尖到達三維空間中一定范圍內的任意點。為了描述末端執行器在空間的位置和姿態，可以在每個關節上建立一個坐標系，利用坐標系之間的變換關系來描述末端執行器的位姿，其中轉動關節可以實現旋轉變換，連桿的長度可以實現平移變換。為研究方便，我們對六自由度機械臂做一下假設：

1）目標點在機械臂指尖所能到達的空間范圍內，即至少存在一種指令序列使指尖到達目標位置。

2）不考慮指尖的大小，將指尖抽象為空間中的一點。

3）不考慮關節所引起的長度變化，認為相鄰連桿之間無縫連接，連接處兩桿端點的距離為零。

4）機械臂各關節完全按照指令序列中的旋轉角度進行旋轉，不考慮轉動誤差的影響

1 數學模型的建立

1.1 建立連桿坐標系

常用的建立坐標系的方法有D-H法、五參數法及矩陣變換法等，D-H法是1955年由Denavit和Hartenberg提出的一種相對位姿的矩陣方法。本文基于D-H坐標變換的數學模型，首先按圖1所示為每個連桿建立一個坐標系，每個連桿由四個參數ai-1、αi-1、θi、di來描述，ai-1和αi-1描述連桿本身的特征；θi和di描述連桿i-1和連桿i之間的關系。對于移動關節i，di是關節變量；對于轉動關節i，θi是關節變量。

圖1 機械臂坐標系圖

根據問題給出的初始位置建立坐標系，可得機械臂的參數如表1所示。

表1 機械臂的連桿參數

1.2 建立機械臂方程

1）Rot(x,αi-1)：繞xi-1轉動αi-1角；

2）Trans(x,ai-1)：沿xi-1移動ai-1；

3）Rot(z,θi)：繞zi轉動θi角；

4）Trans(z,di)：沿zi移動di。

則有

根據連桿變換通式及表1所列出的連桿參數，得到相鄰連桿變換矩陣如下:

它表示末端連桿坐標系 相對于基坐標系{0}的位姿，上式定義為機器臂的運動方程。

1.3 機械臂運動方程逆解

上述文章中建立了D-H坐標系下的機械臂運動方程，通過運動學方程可以通過一組指令序列得到指尖的基準坐標位置，然而問題一中已經知指尖的位姿，需要求出相應的指令序列

為了計算簡單起見，我們做以下三點假設，在誤差分析中我們會對以下兩點假設進行進一步說明。

1）暫且不考慮自由度增量Δθi的離散性和其范圍，從全局范圍求解各個關節變量θi=(i=1...6)；

2）暫且將基坐標系原點與坐標系{1}的原點重合，將坐標系{0}原點與坐標系{0}原點重合，則連桿變換矩陣中的o1T與56T分別變為

3）令連桿坐標系{6}相對于基坐標系{0}的位姿為如下，即令坐標系 與坐標系 的相對位置為圖2所示。

圖2 末端坐標系位置

根據第2、3條假設，我們可以得到末端坐標系{6}原點位置(px,py,pz)與問題中所給的原基準坐標系中指尖坐標(Ox,Oy,Oz)之間的關系為：

我們將通過反變換法來求解各個關節變量θi=(i=1...6)的值。

（1）求解θ1

用逆變換o1T-1(θ1)左乘式(3)的兩端，其中c1=cos(θi)，s1=sin(θi)，下同。16T可以通過齊次變換矩陣的乘法求出，令矩陣方程(4)兩端的元素(2，4)對應相等得，

令矩陣方程式(5)兩端的元素(1，4)和(2，4)分別對應相等，得

令：pyc1-s1px=k1，將式(7)、式(8)平方后求和，得

（3）求θ3

令式(7)的左邊為k3，式(8)左邊為k4，即

兩等式兩邊分別相除得

令矩陣方程(2-9)兩邊的(1，3)、(2，3)、(3，3)元素分別對應相等，得：

上面三式左邊均為已知量，令三式左邊分別為k5、k6、k7，即

（5）求θ5

將式(10)和(12)兩邊分別平方然后求和，得

（6）求θ6

令矩陣方程式(9)的(2，1)、(2，2)元素分別對應相等，得

綜上所述：針對六自由度機械臂通過反變換法對機械臂的運動方程進行逆解，得到各個自由度從始點到目標點轉動的角度總量，總的角度轉量分為每步所要轉動的角度，即求出指令序列，實現機器人機械臂在空間內進行定位要求，誤差較很小。

[1] 林建亞,楊劍鳴.六自由度仿人手型機械手臂的位置逆算法[J].浙江大學學報,1988,22 (4):1012111.

[2] 蔡自興.機器人學[M].清華大學出版社,2000.

[3] 丁學恭.機器人控制研究[M].浙江大學出版社,2006.

[4] 周遠清,等.智能機器人系統[M].北京:清華大學出版社,1989.