操作臂逆运动学

| |

操作臂的工作空间:操作臂末端执行器所能达到的范围。
灵巧工作空间:机器人末端执行器能够从各个方向上达到的空间区域。
可达工作空间:机器人至少从一个方向上可以达到的工作空间。
灵巧工作空间是可达工作空间的子集。
要使机器人逆运动学方程有解,则被指定的目标点位姿必须在工作空间内。

下面以两连杆机构为例,阐述逆运动学问题。

两连杆长度分别为$l_1$,$l_2$,若$l_1=l_2$,则可达工作空间是半径为$2l_1$的圆,灵巧工作空间仅为原点一个点。如果$l_1 \not = l_2$则可达工作空间为外径 $l_1+l_2$ ,内径$|l_1 - l_2|$ 的圆环,在可达工作空间内部,末端执行器有两种可能的方向,在工作空间的边界上只有一种可能的方向(假设操作臂可以360度旋转)。

多解问题
系统最终只能选择一个解,比较合理的方法是选取最近解,即使得每一个运动关节的运动量最小,计算最近解时需要加权,使移动对象尽可能是小连杆而不是大连杆,在存在障碍的情况下,应该选取较远的解。

求解方法
可以分为解析解和数值解两种。解析解又可分为几何法和代数法。

最新的研究结果是:包含转动关节和移动关节的串联型6自由度机构均是可解的。但这种解一般是数值解,偶尔有解析解。设计者通常使机械臂足够简单,以获取解析解。

代数法求解平面三连杆机械臂

第一部分:正向运动学

现有平面三连杆机械臂,三个关节均为转动关节(因此也称RRR机构或3R机构)。

20231221193925.jpg20231221193925.jpg

建立坐标系如图,其中所有的 $z$ 轴均与关节轴的轴线重合,由于机械臂位于同一个平面上,因此所有的 $\hat Z$ 轴相互平行,连杆偏距 $d_i$ 均为0。得到连杆参数如下表:

$i$ $\alpha_{i-1}$ $a_{i-1}$ $d_i$ $\theta_i$
$1$ $0$ $0$ $0$ $\theta_1$
$2$ $0$ $L_1$ $0$ $\theta_2$
$3$ $0$ $L_2$ $0$ $\theta_3$

20231221194637.jpg20231221194637.jpg

第二部分:代数法求解逆向运动学

由前面的操作臂运动学,易得机械臂的运动学方程为:

$$ \begin{align} {}^B_W T = {}^0_3 T = \begin{pmatrix} c_{123} & -s_{123} & 0 & l_1 c_1 +l_2 c_{12} \\ s_{123} & c_{123} & 0 & l_1 s_1 +l_2 s_{12} \\ 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 1 \end{pmatrix} \end{align} $$

机械臂末端位姿用三个参数来描述:

$$ \begin{align} \begin{pmatrix} x \\ y \\ \phi \end{pmatrix} \end{align} $$

则有变换矩阵的形式为:

$$ \begin{align} {}^B_W T = \begin{pmatrix} c_{ \phi } & -s_{\phi } & 0 & x \\ s_{\phi } & c_{\phi } & 0 & y \\ 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 1 \end{pmatrix} \end{align} $$

显然所有可达目标点均需要位于上式(3)所描述的子空间内。
令(3)式与(2)式相等,可以得到非线性方程组如下:

$$ \begin{align} c_{\phi} &= c_{123} \\ s_{\phi} &= s_{123} \\ x &= l_1 c_1 +l_2 c_{12} \\ y &= l_1 s_1 +l_2 s_{12} \end{align} $$

下面要做的就是求解3个关节角$\theta_1 \theta_2 \theta_3$的值

$$ \begin{align} (6)^2 + (7)^2 = x^2 + y^2 &= l_1 ^2 + l_2^2(c_{12}^2 + s_{12}^2) + 2 l_1 l_2 c_1 c_{12} + 2 l_1 l_2 s_1 s_{12} \notag \\ &= l_1 ^2 + l_2^2 + 2 l_1 l_2 (c_1(c_1 c_2 - s_1 s_2) + s_1 (c_1 s_2 + s_1 c_2)) \notag \\ & = l_1 ^2 + l_2^2 + 2 l_1 l_2 (c_1^2 c_2 + s_1^2 c_2 ) \notag \\ & = l_1 ^2 + l_2^2 + 2 l_1 l_2 c_2 \\ \Darr \notag \\ c_2 \equiv \cos \theta_2 &= \frac{x^2 + y^2 - l_1 ^2 - l_2^2}{2 l_1 l_2} \\ \Darr \notag \\ -1 \leq \cos \theta_2 &= \frac{x^2 + y^2 - l_1 ^2 - l_2^2}{2 l_1 l_2} \leq 1 \end{align} $$

上式(10)可以用来判断解的存在性。

$$ \begin{align} s_2 = \pm \sqrt{1-c_2^2} \\ \theta_2 = Atan2(s_2 ,c_2) \end{align} $$

上式(11)有多个解,需要根据实际情况选择适当的解。
下面根据式(6)(7)求解 $\theta_1$ ,有:

$$ \begin{align} x &= (l_1 + l_2 c_2 ) c_1 - l_2 s_2 s_1 = k_1 c_1 - k_2 s_1 \\ y &=(l_1 + l_2 c_2 ) s_1 + l_2 s_2 c_1 = k_1 s_1 + k_2 c_1 \\ 令 r &= \pm \sqrt{k_1^2 + k_2^2} , \gamma = A tan2(k_2,k_1) \\ 有 k_1 &= r \cos \gamma , k_2 = r \sin \gamma \text{代入前式:}\\ \frac{x}{r} &= \cos \gamma \cos \theta_1 - \sin \gamma \sin \theta_1 = \cos (\gamma + \theta_1)\\ \frac{y}{r} &= \cos \gamma \sin \theta_1 + \sin \gamma \cos \theta_1 = \sin (\gamma + \theta_1) \\ & \Darr \notag \\ \gamma + \theta_1 &= Atan2(\frac{y}{r},\frac{x}{r}) = Atan2(y,x) \\ & \Darr \notag \\ \theta_1 &= Atan2(y,x) - Atan2(k_2,k_1) \end{align} $$

最后再由(4)、(5)两式求得三个关节角的和:

$$ \begin{align} \theta_1 + \theta_2 + \theta_3 = Atan2(s_{\phi},c_{\phi}) = \phi \end{align} $$

至此,可以求出三个关节角了。
需要注意可能存在多解。

第三部分:几何法求解逆向运动学

几何解法利用余弦定理,只需具备基本的高中数学知识即可求解,过程如下图所示,内容引自《机器人学导论》

20231221233751.jpg20231221233751.jpg

代数变换的技巧

$$ \begin{align} u &=\tan \frac{\theta}{2} \notag \\ \cos \theta &=\frac{1-u^2}{1+u^2} \\ \sin \theta &= \frac{2u}{1+u^2} \notag \end{align} $$

可以将超越方程转换为关于u的多项式方程,简化计算。