Delta機器人：運動學正反解分析

Delta機器人：運動學正反解分析

一、Delta機構簡介

Delta機構是並聯機構中的一種典型機構，起原始結構如圖1所示。Delta機構由R.Clavel 博士在 1985年發明，是現在並聯機器人中使用最多的一種，具備了並聯機構所特有的優點，負載能力強、效率高、末端執行器精度高、運動慣性小，可以高速穩定運動等。因此在機器人領域獲得了越來越廣泛的應用。以實現高速、精準、高效的運動。

$\ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ 圖1\ R.Clavel 博士發明的Delta機構$

二、數學模型建立

建立Delta機構簡化數學模型如圖2所示，其中圓$Ο$所在平面爲定平臺，圓$p$所在平面爲動平臺，$∆C_1C_2C_3$和$∆A_1A_2A_3$爲等邊三角形，點$C_1、C_2、C_3、A_1、A_2、A_3$分別爲三個主動臂和三個從動臂與上下兩個平臺的連接點。

$\ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ 圖2\ Delta機構簡化數學模型$
如圖2所示，以定平臺中心$Ο$建立座標系$Ο-XYZ$，以動平臺中心$p$建立座標系$p-xyz$。由Delta機構的設計原理可知，三條支鏈完全對稱，因此不妨設第$i（i=1,2,3）$條支鏈的主動臂$\left|B_iC_i\right|$長度爲$L$，從動臂$\left|A_iB_i\right|$長度爲$l$，主動臂與定平臺夾角爲$\theta_i$，三條支鏈與X軸的夾角爲$\varphi_i=\left(2\left(i-1\right)\pi/3\right)，i=1,2,3，$定平臺半徑爲R，動平臺半徑爲$r$。

三、運動學正解

Delta機構的正解，是在已知三個主動臂轉角的情況下求出動平臺中心點$p$在定平臺所在座標系中的座標。Delta機構運動學正解的求法有很多種，此處採取幾何解法，將$A_iB_i$分別沿$A_ip$平移使其交於點$p$得到$D_ip$，連接$D_1D_2$、$D_2D_3$、$D_3D_1$得到四棱錐$p{-D}_1D_2D_3$,如圖3所示。

$\ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ 圖3\ Delta機構幾何法正解簡化模型$
根據上圖不難得到，定平臺三個鉸接點$C_1、C_2、C_3$的座標爲
$\ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \left[\begin{matrix}x_i\\y_i\\z_i\\\end{matrix}\right]=\left[\begin{matrix}R\cos{\varphi_i}\\R\sin{\varphi_i}\\0\\\end{matrix}\right]，i=1,2,3\ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ (1)$
向量$\vec{OB_i}$可表示爲
$\ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \vec{\ OB_i}=\vec{OC_i}+\vec{C_iB_i}，i=1,2,3\ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ (2)$
其中$\vec{C_iB_i}$又可表示爲
$\ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \left[\begin{matrix}x_i\\y_i\\z_i\\\end{matrix}\right]=\left[\begin{matrix}-L\sin{\theta_i}\cos{\varphi_i}-L\sin{\theta_i}\sin{\varphi_i}\\-L\cos{\theta_i}\\\end{matrix}\right]，i=1,2,3\ \ \ \ \ \ (3)$
又$\vec{A_ip}$可表示爲
$\ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \left[\begin{matrix}x_i\\y_i\\z_i\\\end{matrix}\right]=\left[\begin{matrix}-r\cos{\varphi_i}\\-r\sin{\varphi_i}\\0\\\end{matrix}\right]，i=1,2,3\ \ \ \ \ \ \ \ \ \ \ (4)$
則$\vec{OD_i}$可以表示爲
$\ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \vec{OD_i}=\vec{\ OB_i}+\vec{\ B_iD_i}\ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ (5)$
其中由Delta機構的幾何性質可知
$\ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \vec{\ B_iD_i}=\vec{A_ip}\ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ (6)$
所以
$\ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \vec{OD_i}=\vec{OC_i}+\vec{C_iB_i}+\vec{A_ip}\ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ (7)$
綜合式（1）—（7）可得，在座標系Ο-XYZ中D_i的座標爲
$\ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \left[\begin{matrix}x_i\\y_i\\z_i\\\end{matrix}\right]=\left[\begin{matrix}\left(R-r-L\sin{\theta_i}\right)\cos{\varphi_i}\\\left(R-r-L\sin{\theta_i}\right)\sin{\varphi_i}\\-L\cos{\theta_i}\\\end{matrix}\right]，i=1,2,3\ \ \ \ \ \ \ \ \ (8)$
此時不難發現，Delta機構的正運動學解算已經轉化爲已知三個頂點座標和各棱的長度求解另外一個頂點座標的問題。將圖2.4中的四棱錐$p{-D}_1D_2D_3$取出單獨分析，作垂線$pE$垂直於平面$D_1D_2D_3$於點$E$，取$D_2D_3$中點$F$，連接$EF$、$ED_2$，如圖3所示。

$\ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ 圖4\ 等效四棱錐$
不難證明，點E爲三角形D_1D_2D_3的外接圓圓心。
則向量$\vec{Op}$可表示爲
$\ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \vec{Op}=\vec{OE}+\vec{Ep}\ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ (9)$
$\vec{OE}$可以表示爲
$\ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \vec{OE}=\vec{OF}+\vec{FE}\ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ (10)$

其中，$\vec{OF}=\left(\vec{OD_2}+\vec{OD_3}\right)/2$，$\vec{FE}=\left|\vec{FE}\right|\vec{n_{FE}}$。

對於向量$\vec{FE}$其長度爲
$\ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \left|\vec{FE}\right|=\sqrt{\left|D_2E\right|^2-\left|D_2F\right|^2}\ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ (11)$
其中，$\left|D_2E\right|$爲三角形$D_1D_2D_3$的外接圓半徑，可用公式(12)計算

$\ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \left|D_2E\right|=\frac{abc}{4S}\ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \left(12\right)$

其中

$S=\sqrt{p\left(p-a\right)\left(p-b\right)\left(p-c\right)}\ \ \ \ \ \ \ \ \left(13\right)\ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ p=\frac{\left(a+b+c\right)}{2}\ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ (14)$

$a、b、c$是三角形$D_1D_2D_3$的邊長。

聯立（11）—（14）可得

$\ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \left|\vec{FE}\right|=\frac{\left(a^2+b^2-c^2\right)c}{8S}\ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ (15)$
向量$\vec{FE}$的單位方向向量爲
$\ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \vec{n_{FE}}=\frac{\vec{D_2D_1}\times\vec{D_2D_3}\times\vec{D_3D_2}}{\left|\vec{D_2D_1}\times\vec{D_2D_3}\times\vec{D_3D_2}\right|}\ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ (16)$
又向量$\vec{Ep}$的方向向量
$\ \ \ \ \ \\ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \vec{n_{Ep}}=\frac{\vec{D_2D_1}\times\vec{D_2D_3}}{\left|\vec{D_2D_1}\times\vec{D_2D_3}\right|}\ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ (17)$
長度爲
$\ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \vec{Ep}=\sqrt{\left|\vec{D_1p}\right|^2-\left|\vec{D_1E}\right|^2}\ \ \ \ \ \ \ \ \ \ \ \ \ \ (18)$
其中$\left|\vec{D_1p}\right|=\left|\vec{B_1A_1}\right|=l，\vec{D_1E}$爲外接圓半徑。
將（10）—（18）式聯立求解帶入（9）式即可求得Delta機構正解。

四、運動學反解

運動學反解是在已知機器人$p$點的位置$(x,y,z)$的情況下求解三個主動臂需要轉過的角度$\theta_1$、$\theta_2$、$\theta_3$，與串聯機器人不同，並聯機器人的反解較易求得，此處只需要根據杆長進行約束即可很容易解出，求解過程在文獻[3]中有詳細的說明，此處不再推導，僅根據圖5所示的單支鏈求解示意圖給出最終的求解結果。

$\ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ 圖5\ 單支鏈求解示意圖$

$\ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \theta_i=2arctan\left(\frac{-B_i-\sqrt{B_i^2-4A_iC_i}}{2A_i}\right)，i=1,2,3\ \ \ \ \ \ \ \ \ \ \ \ \ \ \ (19)$
其中
$A1=x2+y2+z2+R-r2+L2-l2-2x(R-r)2L-(R-r-x)$
$B1=2z$
$C1=x2+y2+z2+R-r2+L2-l2-2x(R-r)2L+(R-r-x)$
$A2=x2+y2+z2+R-r2+L2-l2+(x-3y)(R-r)L-2R-r-(x-3y)$
$B2=4z$
$C2=x2+y2+z2+R-r2+L2-l2+(x-3y)(R-r)L+2R-r+(x-3y)$
$A3=x2+y2+z2+R-r2+L2-l2+(x+3y)(R-r)L-2R-r-(x+3y)$
$B3=4z$
$C3=x2+y2+z2+R-r2+L2-l2+(x+3y)(R-r)L+2R-r+(x+3y)$
至此，Delta機構的運動學正反解均以求解完畢。

五、參考文獻

[1] Clavel R. Dispositif pour le deplacement et le positionnement d’un element dans l’espace[P].Switzerland: CH1985005348856,1985.
[2] 趙傑,朱延河,蔡鶴皋.Delta型並聯機器人運動學正解幾何解法[J].哈爾濱工業大學學報,2003(01):25-27.
[3] 伍經紋,徐世許,王鵬,宋婷婷.基於Adams的三自由度Delta機械手的運動學仿真分析[J].軟件,2017,38(06):108-112.