Delta机器人的运动学分析

@(1@Personal)[DeltaRobot,BLOG]

暂时放出位置分析，稍后更新速度及加加速度分析。

约定

机构简述

The delta robot consists of two platforms: the upper one (1) with three motors (3) mounted on it, and smaller one (8) with an end effector (9). The platforms are connected through three arms with parallelograms, the parallelograms restrain the orientation of the lower platform to be parallel to the working surface (table, conveyor belt and so on). The motors (3) set the position of the arms (4) and, thereby, the XYZ-position of the end effector, while the fourth motor (11) is used for rotation of the end effector. You can find more detailed description of delta robot design in the corresponding Wikipedia article.

参数

名称	意义
bb	基座中心到电机的距离
aa	主动臂长度
ff	从动臂长度
pp	移动平台中心到链接点的距离
E1、2、3E_{1、2、3}	肘关节
{B}\{B\}	基座所在平面
B0B_0	移动平台中心位置
B1、2、3B_{1、2、3}	电机位置
{P}\{P\}	移动平台所在平面
P0P_0	移动平台中心位置
P1、2、3P_{1、2、3}	从动臂和移动平台连接点

机构坐标系

机构坐标系是一个笛卡尔坐标系统，原点与B0B_0重合，且B1B_1在y轴上，xy平面与{B}\{B\}重合。

轴坐标系

顺时针旋转为角度正方向。

前期分析

假设P0P_0坐标为(x,y,z)(x,y,z);三轴转角为(θ1,θ2,θ3)(\theta_1,\theta_2,\theta_3)。

约束环

为方便理解，Delta机器人的机械结构可以解耦成3个独立的运动学约束环。下图是以1轴为基准的约束环：

{B0P1}={B0B1}+{B1E1}+{E1P1}={B0P0}+{P0P1}

\begin{equation}\label{eq1} \{B_0P_1\}=\{B_0B_1\}+\{B_1E_1\}+\{E_1P_1\}=\{B_0P_0\}+\{P_0P_1\} \end{equation}
这里需要注意，由于Delta结构的特性， {B1E1}\{B_1E_1\}在yz平面上， {P0P1}\{P_0P_1\}平衡于y轴。

为求解方便，将等式(???)\eqref{eq1}变换为{E1P1}\{E_1P_1\}的表达式：

{E1P1}={B0P0}+{P0P1}−{B0B1}−{B1E1}

\begin{equation}\label{eq40} \{E_1P_1\}=\{B_0P_0\}+\{P_0P_1\}-\{B_0B_1\}-\{B_1E_1\} \end{equation}
其中：

{B0P0}=⎧⎩⎨⎪⎪xyz⎫⎭⎬⎪⎪

\begin{equation}\label{eq41}\{B_0P_0\}=\begin{Bmatrix}x\\y\\z\end{Bmatrix}\end{equation}

{P0P1}=⎧⎩⎨⎪⎪0−p0⎫⎭⎬⎪⎪

\begin{equation}\label{eq11}\{P_0P_1\}=\begin{Bmatrix}0\\-p\\0\end{Bmatrix}\end{equation}

{B0B1}=⎧⎩⎨⎪⎪0−b0⎫⎭⎬⎪⎪

\begin{equation}\label{eq5}\{B_0B_1\}=\begin{Bmatrix}0\\-b\\0\end{Bmatrix}\end{equation}

{B1E1}=⎧⎩⎨⎪⎪0−acosθ1−asinθ1⎫⎭⎬⎪⎪

\begin{equation}\label{eq10}\{B_1E_1\}=\begin{Bmatrix}0\\-a\cos\theta_1\\-a\sin\theta_1\end{Bmatrix}\end{equation}
将等式 (???)\eqref{eq41}、 (???)\eqref{eq11}、 (???)\eqref{eq5}和 (???)\eqref{eq10}代入 (???)\eqref{eq40}得：

{E1P1}=⎧⎩⎨⎪⎪xyz⎫⎭⎬⎪⎪+⎧⎩⎨⎪⎪0acosθ1+b−pasinθ1⎫⎭⎬⎪⎪

\begin{equation}\label{eq42}\{E_1P_1\}=\begin{Bmatrix}x\\y\\z\end{Bmatrix}+\begin{Bmatrix}0\\a\cos\theta_1+b-p\\a\sin\theta_1\end{Bmatrix}\end{equation}

三个约束环的关系

如图所示，三个约束环在空间坐标内绕Z轴旋转，相互形成120度的夹角。那么，以1轴为基准的各约束环坐标变换矩阵如下。

[Ri]=⎡⎣⎢cosαi−sinαi0sinαicosαi0001⎤⎦⎥i=1,2,3其中：⎧⎩⎨⎪⎪α1=0α2=120α3=240

\begin{equation}\label{eq7}[R_i]=\begin{bmatrix}\cos\alpha_i&\sin\alpha_i&0\\-\sin\alpha_i&\cos\alpha_i&0\\0&0&1\end{bmatrix}\qquad i=1,2,3\\其中：\left\{\begin{matrix}\alpha_1=0\\\alpha_2=120\\\alpha_3=240\end{matrix}\right.\end{equation}

推广

现在将等式(???)\eqref{eq42}推广为3个约束环通式：

{EiPi}=⎧⎩⎨⎪⎪xyz⎫⎭⎬⎪⎪+[Ri]⎧⎩⎨⎪⎪0acosθ1+b−pasinθ1⎫⎭⎬⎪⎪i=1,2,3

\begin{equation}\label{eq43}\{E_iP_i\}=\begin{Bmatrix}x\\y\\z\end{Bmatrix}+[R_i]\begin{Bmatrix}0\\a\cos\theta_1+b-p\\a\sin\theta_1\end{Bmatrix}\end{equation}\qquad i=1,2,3

展开

由于{EiPi}\{E_iP_i\}的模为ff，得等式：

∥∥{EiPi}∥∥2=f2i=1,2,3

\begin{equation}\label{eq13}\begin{Vmatrix}\{E_iP_i\}\end{Vmatrix}^2=f^2 \qquad i=1,2,3\end{equation}

将等式(???)\eqref{eq7}代入等式(???)\eqref{eq42}得到约束环方程组：

⎧⎩⎨⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪2a(y+A)cosθ1+2zasinθ1+x2+y2+z2+A2+a2+2yA−f2=0−a(3√(x+B)+y+C)cosθ2+2zasinθ2+x2+y2+z2+B2+C2+a2+2xB+2yC−f2=0a(3√(x−B)−y−C)cosθ3+2zasinθ3+x2+y2+z2+B2+C2+a2−2xB+2yC−f2=0其中：⎧⎩⎨⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪A=b−pB=3√2(p−b)C=12(p−b)

\begin{equation}\label{eq14}\left\{\begin{matrix}2a(y+A)\cos\theta_1+2za\sin\theta_1+x^2+y^2+z^2+A^2+a^2+2yA-f^2=0\\-a(\sqrt3(x+B)+y+C)\cos\theta_2+2za\sin\theta_2+x^2+y^2+z^2+B^2+C^2+a^2+2xB+2yC-f^2=0\\a(\sqrt3(x-B)-y-C)\cos\theta_3+2za\sin\theta_3+x^2+y^2+z^2+B^2+C^2+a^2-2xB+2yC-f^2=0\end{matrix}\right.\\其中：\left\{\begin{matrix}A=b-p\\B=\frac{\sqrt3}{2}(p-b)\\C=\frac{1}{2}(p-b)\end{matrix}\right.\end{equation}

反向位置运动学分析

为求解方便，将方程组(???)\eqref{eq14}转化成以下形式：

Gicosθi+Hisinθi+Ii=0i=1,2,3其中:⎧⎩⎨⎪⎪G1=2a(y+A)H1=2zaI1=x2+y2+z2+A2+a2+2yA−f2⎧⎩⎨⎪⎪G2=−a(3√(x+B)+y+C)H2=2zaI2=x2+y2+z2+B2+C2+a2+2(xB+yC)−f2⎧⎩⎨⎪⎪G3=a(3√(x−B)−y−C)H3=2zaI3=x2+y2+z2+B2+C2+a2+2(−xB+yC)−f2

\begin{equation}\label{eq15}G_i\cos\theta_i+H_i\sin\theta_i+I_i=0 \qquad i=1,2,3\\其中:\\\left\{\begin{matrix}G_1=2a(y+A)\\H_1=2za\\I_1=x^2+y^2+z^2+A^2+a^2+2yA-f^2\end{matrix}\right.\\\left\{\begin{matrix}G_2=-a(\sqrt3(x+B)+y+C)\\H_2=2za\\I_2=x^2+y^2+z^2+B^2+C^2+a^2+2(xB+yC)-f^2\end{matrix}\right.\\\left\{\begin{matrix}G_3=a(\sqrt3(x-B)-y-C)\\H_3=2za\\I_3=x^2+y^2+z^2+B^2+C^2+a^2+2(-xB+yC)-f^2\end{matrix}\right.\end{equation}

运用半角公式，定义ti=tanθi2i=1,2,3t_i=\tan\frac{\theta_i}{2}\quad i=1,2,3，并将其代入等式(???)\eqref{eq15}:

Gi(1−t2i)+Hi(2ti)+Ii(1+t2i)=0i=1,2,3

\begin{equation}\label{eq16}G_i(1-t_i^2)+H_i(2t_i)+I_i(1+t_i^2)=0 \qquad i=1,2,3\end{equation}

然后解得：

ti=−Hi±G2i+H2i−I2i−−−−−−−−−−−√Ii−Gi

\begin{equation}\label{eq17}t_i=\frac{-H_i\pm\sqrt{G_i^2+H_i^2-I_i^2}}{I_i-G_i}\end{equation}

最后解得：

θi=2tan−1(ti)

\begin{equation}\label{eq18}\theta_i=2\tan^{-1}(t_i)\end{equation}

这时候每个约束环可以解出两个角度，三个电机合起来有八组解。一般情况下我们只选择所有机器臂向外的那一组解。