使用6轴机器人，在给定末端执行器位置和方向范围的情况下，如何找到最佳关节值

10

给定六轴铰接式机械手将工具固定在其末端执行器上，如果我具有所需的工具位置和工具方向，则逆运动学方程式中将有1个正确的解决方案，以使机器人到达该位置。
（或更确切地说，最多16种不同的解决方案，具体取决于关节的范围）

但是，如果机器人拿着类似笔的东西，并且我希望机器人用该笔在目标上标记一个特定点，那么我不在乎笔的方向，只要它垂直于标记的表面即可。

因此，逆运动学方程将具有无限多个解。

如何在这些解决方案中选择最接近当前配置的联合配置：需要最少移动量的联合配置？
（或根据其他类似准则最佳的关节配置，例如所有关节角度均离其最大和最小值最远？）

— 雨果·朗
source

9

首先，我们需要定义最优。由于您没有说出什么是最佳选择，因此大多数人会选择二次表达式。例如，假设您当前的关节角度由矢量。我们可以考虑最小化所需的运动-有错误，可以定义一个成本函数对一些矩阵。我们通常使用对角矩阵，但是任何正定矩阵都可以。 $\vec{\alpha}$ $\vec{x} = \vec{\alpha} - \vec{\alpha}_{start}$ $J=\vec{x}^TQ\vec{x}$ $Q$

在具有两个关节角度的简化示例中，如果关节的电机便宜（也许更靠近末端执行器），我们的成本函数可能为 $a$

，即关节运动成本是关节两倍。 $J=\left[\begin{matrix}x_a\\x_b\end{matrix} \right]\left[\begin{matrix}1&0\\0&2\\\end{matrix} \right]\left[\begin{matrix}x_a&x_b\end{matrix} \right]$ $b$ $a$

现在，运动方程是一个矩阵公式，用Denavit-Hartenberg表示是：

，其中右手侧表示的位置和取向（当前被设置为零旋转），给定关节角度。 $\prod{T_i}=\left[\begin{matrix}1&0&0&x\\0&1&0&y\\0&0&1&z\\0&0&0&1\end{matrix} \right]$ $(x,y,z)$

由于我们不在乎方向，仅在乎位置，因此我们可以截断最后一个变换矩阵的前3列和第一个变换矩阵的最后一行。我们可以等效地将该公式表示为：

$\left[\begin{matrix}1&0&0&0\\0&1&0&0\\0&0&1&0\end{matrix}\right]\prod{T_i}\left[\begin{matrix}0\\0\\0\\1\end{matrix}\right]=\left[\begin{matrix}x\\y\\z\end{matrix} \right]$

乘以左侧，我们得到三个方程。如果参数是线性的，则很容易求解。如果所有执行器都是线性执行器，就是这种情况。在这种情况下，问题实际上是二次程序。我们可以重新排列左侧以获得等式：

对于某些矩阵，。 $K\vec{x}=\left[\begin{matrix}x\\y\\z\end{matrix}\right]$ $K$

二次程序是一个问题，可以用以下形式表示：

最小化 $J=\frac{1}{2}\vec{x}^TQ\vec{x}+\vec{c}^T\vec{x}$

除， $A\vec{x}\leq\vec{b}$ $E\vec{x}=\vec{d}$

为了解决这个问题，您可以使用多种算法，例如，内部点，活动集等。只要找到一个合适的库，它就会为您解决。

非线性方程组更难求解。这称为非线性编程，但是如果您有旋转关节，它就是您所拥有的。

本质上，您具有非线性函数，而不是矩阵方程。

$f(x)$ $\vec{h}(x) = 0$ $\vec{g}(x) \leq 0$

用于解决此问题的算法甚至更加复杂，但包括内部点，顺序二次规划（SQP），活动集，信任区反射算法。显然，关于这些算法如何工作的解释非常冗长，我将其排除在此答案范围之外。可以说，仅用于二次编程的算法的内容量本身就是一个完整的过程。

您只需要找到一个库来解决问题，编写一个有效的实现将花费很长时间，并且有效的实现一次可以处理100个（或更多）变量。例如，如果您使用MATLAB，则可以从“优化工具箱”中找到有关如何使用函数fmincon的文档。

要在线解决该问题，您可能需要C ++或其他本机实现，例如NLopt。请注意，这可能不是微控制器可以快速解决的问题，并且许多库可能还具有其他依赖关系，这些依赖关系在微控制器上不易使用（因为它们旨在用于计算机）。

如果您不担心效率，只是想自己编写一些代码，那么假设有一个可以调用来解决运动学反问题的函数，您可以简单地执行梯度下降方法。例如，任意选择一个随机的起始方向，解决反问题，然后检查成本函数。然后，您可以使用扰动分析来检查如何改变方向。例如，如果您检查当前方向周围的相似方向（即立方网格中的8个点），则可以得到成本函数在每个方向上如何变化的二阶近似值。

使用二阶近似（因为它是多元的-定向为3维，所以称为Hessian矩阵），您可以找到成本函数的梯度的零交叉（即，预测的局部最小值）。

使用新的预测方位，只需再次将其放置在逆求解器中，然后重复进行直到精度足够。

请注意，这可能不会那么有效，因为逆运动学问题本身必须迭代解决（因此，您反复使用本身需要一段时间才能解决的函数）。而且，所涉及的代码可能少于成熟的优化算法，但它仍然是相当可观的，并且对时间的投入并不小。

使用这两种方法（作为非线性程序正式求解或使用函数迭代求解反问题），如果存在多个局部极小值，则求解可能不是最佳的。在这种情况下，您可以尝试使用各种方法来找到全局最小值。即使使用非线性编程求解器，也应使用初始值（例如，关节角度）为它播种。您可以使用各种方式生成的种子重复运行任一方法：

随机重启（随机生成）
基于网格

或其他自定义方法。

但是请注意，如果有很多最小值，则没有很好的方法来保证找到全局最小值。您只能提高机会。

— 罗纳尔奇恩
source

2

由于问题是关于工业机器人的，因此我们可能没有机器人动力学模型，因此我假设我们正在寻找的解决方案仅能优化运动学标准。

机器人的逆运动学具有封闭形式的解决方案，但不幸的是，末端执行器具有额外的旋转自由度，这意味着机器人具有7个自由度。但是，由于这只是一个自由度，因此它并没有像人们想象的那么重要。

$0.05$ $1$ $360$ $1^\circ$ $18$

如果大多数时候笔只移动一点（例如，画一条线时），另一个加快搜索速度的技巧是使用数字IK，例如伪逆方法：

$q_1$ $J$ $\Delta x$ $\Delta x = J \, \Delta q$ $\Delta q$ $q_2 = q_1 + \Delta q$ $\Delta q$ $\|\Delta q\|$

$q_2$

— 安托纳科斯
source

1

$r_z$

J^{- 1} \dot{X} = \dot{Θ} = [\begin{matrix} \vec{j_{1}} & \vec{j_{2}} & \vec{j_{3}} & \vec{j_{4}} & \vec{j_{5}} & \vec{j_{6}} \end{matrix}] [\begin{matrix} \dot{x} \\ \dot{y} \\ \dot{z} \\ \dot{r_{x}} \\ \dot{r_{y}} \\ \dot{r_{z}} \end{matrix}] = [\begin{matrix} \dot{θ_{1}} \\ \dot{θ_{2}} \\ \dot{θ_{3}} \\ \dot{θ_{4}} \\ \dot{θ_{5}} \\ \dot{θ_{6}} \end{matrix}]

$\mathbf{J}^{-1}\dot{\mathbf{X}}= \dot{\mathbf{\Theta}}= \begin{bmatrix} \vec{j_{1}} & \vec{j_{2}} & \vec{j_{3}} & \vec{j_{4}} & \vec{j_{5}} & \vec{j_{6}} \end{bmatrix} \begin{bmatrix} \dot{x}\\ \dot{y}\\ \dot{z}\\ \dot{r_x}\\ \dot{r_y}\\ \dot{r_z} \end{bmatrix} = \begin{bmatrix} \dot{\theta_1}\\ \dot{\theta_2}\\ \dot{\theta_3}\\ \dot{\theta_4}\\ \dot{\theta_5}\\ \dot{\theta_6}\\ \end{bmatrix}$

\vec{j_{i}}

$\vec{j_{i}}$

i^{t h}

$i^{th}$

J^{- 1}

$\mathbf{J}^{-1}$

\dot{Θ}

$\dot{\mathbf{\Theta}}$

\dot{r_{z}}

$\dot{r_z}$

\dot{Θ} = {\dot{Θ}}_{\dot{x} \dots \dot{r_{y}}} + {\dot{Θ}}_{\dot{r_{z}}} {\dot{Θ}}_{\dot{r_{z}}} = \vec{j_{6}} \dot{r_{z}}

$\dot{\mathbf{\Theta}}= \dot{\mathbf{\Theta}}_{\dot{x}\dots \dot{r_y}} + \dot{\mathbf{\Theta}}_{\dot{r_z}} \\ \dot{\mathbf{\Theta}}_{\dot{r_z}}= \vec{j_{6}} \dot{r_z}$

({\dot{Θ}}_{\dot{x} \dots \dot{r_{y}}} + {\dot{Θ}}_{\dot{r_{z}}})^{T} D ({\dot{Θ}}_{\dot{x} \dots \dot{r_{y}}} + {\dot{Θ}}_{\dot{r_{z}}})

$(\dot{\mathbf{\Theta}}_{\dot{x}\dots \dot{r_y}} + \dot{\mathbf{\Theta}}_{\dot{r_z}})^T D(\dot{\mathbf{\Theta}}_{\dot{x}\dots \dot{r_y}} + \dot{\mathbf{\Theta}}_{\dot{r_z}})$

D

$D$

A = {\dot{Θ}}_{\dot{x} \dots \dot{r_{y}}}

$A=\dot{\mathbf{\Theta}}_{\dot{x}\dots \dot{r_y}}$

B = {\dot{Θ}}_{{\dot{r_{z}}}^{*}}

$B=\dot{\mathbf{\Theta}}_{\dot{r_z}^*}$

{一个}^{Ť} d 一个 + 2 乙^{Ť} d 一个 + 乙^{Ť} d 乙 要么 {一个}^{Ť} d 一个 + 2 \dot{{[R}_{ž}} {\vec{Ĵ_{6}}}^{Ť} d 一个 + {\dot{{[R}_{ž}}}^{2} {\vec{Ĵ_{6}}}^{Ť} d \vec{Ĵ_{6}}

$A^TDA+2B^TDA+B^TDB\\ \text{or}\\ A^TDA+2\dot{r_z}\vec{j_{6}}^TDA+\dot{r_z}^2\vec{j_{6}}^TD\vec{j_{6}}$

$\dot{r_z}$ $\dot{r_z}$ $0$

2 {\vec{Ĵ_{6}}}^{Ť} d 一个 + 2 \dot{{[R}_{ž}} {\vec{Ĵ_{6}}}^{Ť} d \vec{Ĵ_{6}} = 0 \dot{{[R}_{ž}} = \frac{- {\vec{Ĵ_{6}}}^{Ť} d 一个}{{\vec{Ĵ_{6}}}^{Ť} d \vec{Ĵ_{6}}}

$2\vec{j_{6}}^TDA+2\dot{r_z}\vec{j_{6}}^TD\vec{j_{6}}=0\\ \dot{r_z} = \frac{-\vec{j_6}^TDA}{\vec{j_6}^TD\vec{j_6}}$

— Joshkarges
source