机械臂运动学（正运动学，逆运动学，求解方法）

问题

对于一个机械臂，考虑下面两个问题：

上面两个问题就是机械臂的运动学要研究的问题，问题一为正运动学研究的问题，问题二为逆运动学研究的问题。

正运动学

1. 作用

已知机械臂关节角度，求解机械臂末端在空间中位姿（关节空间到笛卡尔空间的映射）

2. 方程

3. 推导

1. 几何法:

通过分析机械臂的几何结构，利用几何关系和三角函数直接计算末端执行器的位置和姿态。

适用于结构对称或简化的机械臂。

2. Denavit-Hartenberg（DH）参数法

比较通用的方法，通过变换矩阵连乘（连续空间变换）得到末端与基座间的变换关系。

步骤：

1. 建立Denavit-Hartenberg（DH）参数：具体DH标定的参数描述与细节可以看我的另一篇描述DH标定的博文。

• 机械臂的每个关节可以定义了4个关键参数，用来描述相邻两个连杆的相对位置和方向；
• 通过这四个参数可以构建一个齐次变换矩阵Ai，用来描述相邻两个连杆在空间中的变换关系（包括位置和姿态）

2. 连续变换得到末端位姿：

通过将每个关节的齐次变换矩阵相乘，得到从基座到末端执行器的总变换矩阵 T6。

tips：为什么矩阵连乘可以得到末端相对于底座的位姿关系呢？因为齐次矩阵相当于一个空间变换，矩阵左乘相当于对右边的坐标系实施这个变换，因此从底座到末端的齐次变换矩阵连乘相当于把底座坐标系按照两两关节的位姿关系不断变换，最后得到的结果就是末端与基座的变换关系。

逆运动学

1. 作用

已知机械臂末端在空间中位姿，求机械臂关节角度（笛卡尔空间到关节空间的映射）

2. 方程

3. 求解

1. 概述

机械臂逆运动学求解方法主要有解析法，数值法，人工智能法。

解析法：适用于自由度较低、结构较简单的机械臂，可以通过数学推导得到关节变量的显式表达式，可以快速的得到精确的结果，但仅适用于特定的机械臂结构，难以推广到通用情况。。

数值法：适用于任意结构的机械臂，但是计算量大，可能收敛缓慢，存在收敛性问题，求得的解也是近似解。

人工智能法：需要训练模型，直接预测关节角度。具有计算速度快，适应性强的优点，但是需要大量训练数据，泛化能力取决于模型和数据质量。

由于运动学方程为复杂的非线性方程组（涉及三角函数），因此当机械臂较复杂时很难直接求得解析解，目前比较常用的解法为通过迭代的求得数值解，因此这里重点介绍数值法。

2. 解析法

定义:解析法通过代数和几何方法，直接推导出机械臂关节角度的闭式解(精确的解)，如下面的代数法和几何法就属于解析法。

代数法：

1. 原理

• 代数运算：代数法主要利用代数操作（如消元、因式分解、多项式求解等）来解决逆运动学方程组。
• 方程求解：将运动学方程转化为代数方程组，使用代数技巧求解关节变量。

2. 特点

• 符号计算：强调对方程的代数操作，可能涉及高次多项式的求解。
• 适用性：对于能够转化为代数形式且方程可解的机械臂结构有效。

几何法

1. 原理

• 几何关系分析：几何法利用机械臂的几何特性，通过分析关节和连杆之间的空间几何关系，直接求解关节变量。
• 图形直观性：借助几何图形和空间想象，使用三角学和几何定理。

2. 特点

• 直观性强：计算过程直观，易于理解和演示。
• 适用性有限：适用于几何关系明确、结构简单的机械臂。

3. 数值法

数值法通过迭代算法，逐步逼近逆运动学问题的解(本质是非线性优化问题求解)，求解非线性方程组的方法有很多，比如梯度下降，牛顿法，高斯牛顿法，LM法等。

但由于机械臂的运动学关系通常是高度非线性的，特别是对于具有多个自由度的机械臂，逆运动学问题可能存在多个解，甚至无解。

数值求解方法比较

对于机械臂逆解问题，目前LM法应用比较广泛，原因如下：

1. 相对于梯度下降法

• 收敛速度慢：梯度下降法的收敛速度为一阶，通常较慢，特别是在非凸或高度非线性的目标函数中。
• 对学习率（步长）的选择敏感：在机械臂逆运动学中，目标函数的曲率可能变化较大，固定的学习率可能无法适应，需要动态调整，这增加了实现的复杂性。
• 容易陷入局部最小值：机械臂逆运动学的目标函数可能存在多个局部最小值，梯度下降法可能会陷入局部极小点，无法找到全局最优解。
• 缺乏二阶信息：梯度下降法仅利用一阶导数信息，无法充分利用目标函数的曲率信息。在高度非线性的优化问题中，二阶信息（如海森矩阵）能够提供更准确的方向和步长，提高收敛速度。

2. 相对于牛顿法

• 需要计算海森矩阵：对于高维问题，计算和存储海森矩阵及其逆矩阵的开销过大。
• 对初始值敏感：牛顿法对初始猜测值要求较高，不适用于初始解未知或不精确的情况。

3. 相对于高斯-牛顿法

• 适用于弱非线性问题：高斯-牛顿法在残差函数接近线性时效果较好，但机械臂逆运动学的非线性程度通常较高。
• 收敛性可能较差：对于高度非线性的逆运动学问题，高斯-牛顿法可能会收敛缓慢或不收敛。

tips: 非线性优化问题求解可以看我的另一篇博客。

Levenberg-Marquardt （LM）方法

结合牛顿法和梯度下降法的混合优化算法，平衡了收敛速度和稳定性。

• 步骤：
  1. 初始化：选择初始关节角度猜测值 θ(0)\theta^{(0)}θ(0)。
  2. 计算梯度和海森矩阵：基于正运动学映射和误差。
  3. 更新关节角度：
  4. 迭代调整：根据收敛情况调整 λ。
  5. 收敛判断：当误差小于预设阈值时停止迭代。

4. 人工智能法

人工智能法利用机器学习和深度学习技术，通过大量数据训练模型，学习末端执行器位姿与关节角度之间的非线性映射关系，实现逆运动学求解。