机器人学基础第三章逆向运动学

首次发布: 2025-04-16

... 次访问

机械臂的逆向运动学

3.1 运动学逆解的目标#

运动学逆解的目标是给定一个可能的末端位姿 $\ ^{S}_{E}T$ ，求出对应的各个关节变量 $q$ .

T^{-1}: SE(3) \rightarrow \mathbb{Q}

求解逆过程相比于求解正过程一般都是更加困难的一个过程（例如积分相较于微分困难），求运动学逆解也不例外。不似正向运动学，给定了关节变量的值，就一定可以确定末端位姿。求解运动学逆解，需要考虑几个问题

解是否存在
解是否唯一
如何求解

将方程 $\ ^{S}_{E}T(q) = \begin{bmatrix} r_{11} & r_{12} & r_{13} &p_x \\ r_{21} & r_{22} & r_{23} &p_y \\ r_{31} & r_{32} & r_{33} &p_z \\ 0 &0 &0 &1\end{bmatrix}$ 拆开，可以得到 6 个非线性超越方程（末端位姿是 $6$ 自由度的），而需要求解 $N$ 个关节变量（ $N$ 通常取 $6, 4, 5$ 或 $7$ 等）

3.2 解的存在性#

为了解决解是否存在的问题，定义工作空间为机械臂的末端执行器能够到达的空间范围，即

\mathbb{W} = \{^{S}_{E}T(q) \in SE(3)| \forall q = \begin{bmatrix} q_1 \\ q_2 \\\vdots \\ q_n\end{bmatrix}\in \mathbb{Q}\}

十分自然地，可以得出解的存在性结论

解的存在性: 只有当给定的机械臂末端位姿 $^{S}_{E}T$ 位于工作空间中时运动学逆解才存在

工作空间

有时我们关注的更多的是给定末端坐标然后求解对应的关节变量的情形，此时也可以定义工作空间，不过这个工作空间就变成了末端坐标的集合了。

3.3 多解性或解唯一性#

具有冗余自由度的机械臂 ( $N > 6$ ) 的机械臂有无穷解
通常对于一个给定位姿，逆解不止一个
一般当末端位姿位于工作空间的边界上时，逆解是唯一的（例如机械臂完全伸展开）

multisolutions

3.4 求解运动学逆解#

求解运动学逆解的方法主要分为解析法和数值法。需要指出的是数值法才是主流的求解逆解的方法，因为大部分机械臂很难有闭式解。不过闭式解的结果肯定是比数值法计算要快且准确的。

3.4.1 数值解法#

设末端位姿 $T= \begin{bmatrix} r_{11} & r_{12} & r_{13} &p_x \\ r_{21} & r_{22} & r_{23} &p_y \\ r_{31} & r_{32} & r_{33} &p_z \\ 0 &0 &0 &1\end{bmatrix}$ 已知，方程为

T = \ ^{S}_{E}T(q)

可以使用 Newton 方法等数值优化方法进行方程求解（需要给定初始猜测、关节变量等的取值范围约束条件、收敛依据等）

\min_{q\in\mathbb{Q}} \Vert^{S}_{E}T(q) - T\Vert^2

这里不做赘述

3.4.2 解析解法#

解析解法一般是通过建立与原始方程等价的代数或几何方程进行求解（一般是从中间关节处的运动学信息出发建立新的方程）

下面将给出一些经典的机械臂的例子

平面 3R 机械臂#

这是一个自由度为 $3$ (两个位置坐标，一个角度朝向) 的机构，其工作空间是 $SE(3)$ 的一个 $3$ 维子空间

根据前向运动学，可以得到其末端位姿关于三个旋转关节的旋转角度的方程为

^{S}_{E}T(\theta_1, \theta_2, \theta_3) = \begin{bmatrix} \cos(\theta_1 + \theta_2 + \theta_3) & -\sin(\theta_1 + \theta_2 + \theta_3) & 0 & L_1 \cos\theta_1 + L_2 \cos(\theta_1 + \theta_2)+L_3 \cos(\theta_1 + \theta_2 + \theta_3) \\ \sin(\theta_1 + \theta_2 + \theta_3) & \cos(\theta_1 + \theta_2 + \theta_3) & 0 & L_1 \sin\theta_1 + L_2 \sin(\theta_1 + \theta_2)+L_3 \sin(\theta_1 + \theta_2 + \theta_3) \\ 0 &0 &1 &0\\ 0 &0 &0 &1 \end{bmatrix}

假设

^{S}_{E}T(\theta_1, \theta_2, \theta_3) = \begin{bmatrix} \cos\phi &-\sin\phi & 0 & x\\ \sin\phi &\cos\phi &0 &y\\ 0 &0 &1 &0\\ 0 &0 &0 &1 \end{bmatrix}

其中 $L_1, L_2, L_3$ 均已只，则有

\begin{cases} &\phi = \theta_1 + \theta_2 + \theta_3\\ &x = L_1 \cos\theta_1 + L_2 \cos(\theta_1 + \theta_2)+L_3 \cos(\theta_1 + \theta_2 + \theta_3)\\ &y = L_1 \sin\theta_1 + L_2 \sin(\theta_1 + \theta_2)+L_3 \sin(\theta_1 + \theta_2 + \theta_3) \end{cases}

代数求解#

构造

\begin{cases} &x' = x - L_3\cos(\theta_1 + \theta_2 + \theta_3)\\ &y' = y - L_3\sin(\theta_1 + \theta_2 + \theta_3) \end{cases}

则有

\begin{cases} &\phi = \theta_1 + \theta_2 + \theta_3\\ &x' = L_1 \cos\theta_1 + L_2 \cos(\theta_1 + \theta_2)\\ &y' = L_1 \sin\theta_1 + L_2 \sin(\theta_1 + \theta_2) \end{cases}

于是，有

x'^2 + y'^2 = L_1^2 + L_2^2 + 2L_1 L_2 \cos\theta_2

可以求得

\cos\theta_2 = \frac{x'^2 + y'^2 - L_1^2 - L_2^2}{2L_1 L_2} \in [-1, 1]

这里需要检测 $\cos\theta_2$ 范围是否在 $[-1, 1]$ 内，如果不在，说明该逆解不存在。

接着可以由此求出 $\theta_2$

\begin{aligned} \sin\theta_2 &= \pm\sqrt{1 - \cos\theta_2^2}\quad\text{(多解性)}\\ \theta_2 &= \text{atan2}(\sin\theta_2, \cos\theta_2) \end{aligned}

接下来，展开 $x', y'$ ，有

\begin{cases} &x' = (L_1 + L_2\cos\theta_2)\cos\theta_1 - L_2\sin\theta_2\sin\theta_1\\ &y' = (L_1 + L_2\cos\theta_2)\sin\theta_1 + L_2\sin\theta_2\cos\theta_1 \end{cases}

取 $r = \sqrt{(L_1 + L_2\cos\theta_2)^2 + (L_2\sin\theta_2)^2}$ ，并令

\begin{aligned} \cos\gamma &= \frac{L_1 + L_2 \cos\theta_2}{r}\\ \sin\gamma &= \frac{L_2 \sin\theta_2}{r} \end{aligned}

则

\begin{cases} &x' = r(\cos\gamma\cos\theta_1 - \sin\gamma\sin\theta_1) = \cos(\theta_1 + \gamma)\\ &y' = r(\cos\gamma\sin\theta_1 + \sin\gamma\cos\theta_1) = \sin(\theta_1 + \gamma) \end{cases}

由此可以求出

\theta_1 = \text{atan2}(x', y') - \text{atan2}(L_2\sin\theta_2, L_1 + L_2\cos\theta_2)

其中 $\gamma = \text{atan2}(L_2\sin\theta_2, L_1 + L_2\cos\theta_2)$

最后可以求得 $\theta_3$

\theta_3 = \phi - \theta_1 - \theta_2

需要指出的是这里求解 $\theta_2$ 的代数方法的斧凿痕迹很明显，如果换用几何方法（余弦定理与角度加减）是可以很清楚的看出构造过程的。