上一篇文章《实现正向运动学》为正向运动学问题提供了解决方案。我们实现了一个名为ForwardKinematics的函数，该函数可以求取机械臂当前在空间中末端所在的坐标。
如果我们有一个想要到达的目标空间位置，可以利用ForwardKinematics函数根据当前关节的实际配置来估算接近的程度。到目标的距离可以通过如下方式进行计算：
public float DistanceFromTarget(Vector3 target, float [] angles)
{
Vector3 point = ForwardKinematics (angles);
return Vector3.Distance(point, target);
}
解决逆运动学问题意味着我们要最小化DistanceFromTarget函数所返回的值。在编程和数学领域，最小化是最常见的问题之一。我们将采用的方法基于一种名为"梯度下降"的技术。虽然它并非最高效的算法，但具有普适性优势——无需针对特定问题调整，且大多数程序员已掌握的基础知识即可求解。
梯度下降
理解梯度下降工作原理最简单的方式是想象一个起伏的地形。假设我们随机出现在某个位置，目标是找到该地形的最低点（即最小值）。在每一步，梯度下降会指引我们沿着降低海拔的方向移动。如果地形的几何结构相对简单，这种方法最终会收敛到山谷底部。
下图展示了梯度下降成功应用的典型场景。在这个简单示例中，函数接受单一参数（X轴），并返回误差值（Y轴）。从X轴上的随机初始点（蓝点和绿点）出发，梯度下降将引导我们沿着指向最小值的方向移动（蓝箭头和绿箭头所示）。
从函数的整体来看，我们需要移动的方向显然很明确。然而，梯度下降事先并不知道最小值的位置。算法能做出的最佳猜测是沿着斜率的方向移动，也就是函数的梯度方向。假设你站在山坡上，松开一个球，然后跟随它滚向山谷。下图展示了误差函数在两个不同点的梯度。
为了使梯度下降有效，我们试图求取最小化的函数应满足某些性质。如果函数的地形相对平滑，梯度下降更有可能成功。不连续的函数或存在大量峰谷的函数尤其具有挑战性，因为我们可能需要经过更长的路径才能到达山谷底部。
这是一段由两个关节（由θ0和θ1控制的机械臂的函数地形可能的形态：
梯度估算
若您具备微积分基础，可能知道函数的梯度与其导数密切相关。然而计算导数需要函数满足特定数学性质，而这些性质通常无法普遍适用于任意问题。此外，导数的解析求解要求误差函数必须能够显式表达——但现实情况中，您往往无法获得待最小化函数的解析表达式。
在上述所有场景下，获取真实导数值都是不可行的。此时我们需采用估算方法。下图展示了一维空间中的估算原理：通过采样邻近点，可以感知函数的局部梯度特性。若左侧误差函数值更小，则向左移动；同理，右侧更优则向右调整。
采样邻近点距离(sampling distance)功能将在后面实现。
梯度和导数的区别是什么？
梯度和导数的概念密切相关。
函数的梯度（或方向导数）是一个指向函数值增长最快方向的向量。对于一维函数（如图中所示的例子），梯度的方向由函数的增减决定：若函数在当前位置上升，则梯度方向为+1；若下降，则为-1。当函数涉及两个变量（例如具有两个关节的机械臂）时，梯度则是一个二维向量（通常为单位向量），指向该点的最大上升方向。
相比之下，导数是一个标量值，表示沿梯度方向移动时函数增长的速率。
需要注意的是，本教程并不直接计算函数的真实梯度。我们仅提供梯度的估计值。这一估计值理论上应指向函数值增长最快的方向，但实践中它可能并非严格的单位向量（即其长度可能不为1），如后续内容所示。
邻近点采样的重要性？
在梯度估计中，选择合适的扰动距离（即步长Δ）是关键，因为它直接影响梯度方向的准确性。看下图：
用于估计梯度的采样距离如果过大。梯度下降错误地认为右侧比左侧更高。结果导致算法朝着错误的方向推进。
减小采样距离可以缓解这一问题，但无法彻底解决。此外，更小的采样距离会导致收敛到解的速度变慢。
这一问题可以通过采用更先进的梯度下降变体来解决。
如果函数有多个局部极小值怎么办？
一般来说，这种贪心策略无法保证你实际上能到达山谷底部。如果有多个山谷，你可能会陷入其中一个，而无法到达实际目标。
根据到目前为止所描述的梯度下降简单方法，可以观察到图中函数会出现这种情况。该函数存在三个局部最小值，分别对应三个不同的波谷。如果从绿色区域的某个点初始化梯度下降，最终会到达绿色波谷的底部；同样地，从红色或蓝色区域初始化也会到达各自波谷的底部。再次说明，所有这些问题都可以通过采用更先进的梯度下降变体来解决。
数学原理
在对梯度下降的图形化原理有了初步理解后，现在我们将其转化为数学表达。第一步是计算误差函数 ( f ) 在特定点 ( p ) 处的梯度。我们需要找到函数增长的方向。函数的梯度与其导数密切相关。因此，我们的估算可以从导数的计算方式开始探究。
数学上，函数 ( f ) 的导数记为 ( f' )。在点 ( p ) 处的导数值 ( f'(p) ) 表示函数在此处的增长速率。其性质如下：
- 若 ( f'(p) > 0 )，则 ( f ) 在局部范围内上升；
- 若 ( f'(p) < 0 )，则 ( f ) 在局部范围内下降；
- 若 ( f'(p) = 0 )，则 ( f ) 在局部范围内平坦。
我们的目标是通过估算 ( f'(p) ) 来计算梯度 (▲f )。数学上，导数 ( f' ) 定义为：
$$
f'(p) = \lim_{\Delta x \rightarrow 0} \frac{f(p + \Delta x) - f(p)}{\Delta x}
$$
下图展示了这一公式的含义：
对我们所关心的问题而言，为了估算导数，我们需要在误差函数的两个不同点进行采样。这两个点之间的微小距离Δx 即为前文提到的采样距离。
简要回顾：函数的实际导数需要使用极限运算来计算。我们通过一个足够小的采样距离对导数进行估算：
导数
估计梯度
下一节我们将看到，对于多变量函数，这两个表达式有何差异。
一旦我们估算出导数，就需要沿着其反方向移动以“下降”函数。这意味着我们需要按如下方式更新参数p：
常数 𝐿 通常被称为学习率(Learning rate)，它决定了我们沿反梯度方向移动的速度。较大的𝐿值可能更快接近解，但也更可能因此“越过”目标点。
什么是极限运算？
如果你未学习过微积分，可能对“极限”（limit）这一概念感到陌生。极限是数学工具，允许我们触及无限或无限接近的情况。
以我们的示例为例：采样距离Δ𝑥越小，我们对函数实际梯度的估计就越准确。然而，我们无法将Δ𝑥设为 0，因为除以零是不允许的。极限帮助我们绕过这一问题。我们不能除以零，但通过极限，我们可以要求一个无限接近零的数，而它本身永远不会真正等于零。
多变量问题
我们目前找到的解决方案仅适用于单变量情形。这意味着我们仅定义了形如𝑓(𝑝)（其中𝑝 是单个数值）的函数的导数。在这种情况下，我们可以通过在两点𝑝和𝑝+Δ𝑥处采样函数𝑓，得到一个对导数的合理估计。结果是一个单一数值，指示函数是递增还是递减。我们将其作为梯度使用。
若函数包含多个参数（例如，对应具有多个关节的机械臂），则需扩展梯度的定义。例如，若机械臂有三个关节，函数形式会变为𝑓(𝛼0,𝛼1,𝛼2)。此时，梯度是一个由三个数值组成的向量，指示函数𝑓在三维参数空间中各方向的局部变化行为。
为此，我们引入偏导数（partial derivatives）的概念。偏导数本质上是“传统的导数”，但每次仅对单个变量求导：
它们（偏导数）代表三个不同的标量数值，每个数值指示函数在特定方向（或坐标轴）上的增长情况。为了计算总梯度，我们通过足够小的采样距离Δ𝑥、Δ𝑦和Δ𝑧，对每个偏导数进行近似估算：
在我们的梯度下降中，我们将使用整合了这三个结果并组成向量作为梯度：
记住，得出的结果不是单位向量。
函数的方向导数是一个单位向量，指示其增长最快的方向。方向必须是单位长度的向量。然而，我们计算出的梯度并不一定是一个单位向量。
虽然这听起来在数学上挺不严谨的（可能确实如此！），但这对我们的算法并没构成问题。我们真正需要的是一个指向最陡上升方向的向量。将偏导数的估计值作为该向量的元素，已经满足了我们的需求。若希望将其转化为单位向量，只需通过将其除以自身的模长来进行归一化即可。
使用单位向量的优势在于，我们能明确知道沿该方向移动的最大速度（即学习率𝐿）。而非归一化的梯度意味着，移动速度会因函数𝑓的倾斜程度而快慢不一。这既非优也非劣，只是解决这一问题的不同方法。
Unity实现
我们现在已经具备了用C#实现简单梯度下降所需的所有知识。让我们从一个函数开始，该函数用于预估第i个关节的偏导数∇𝑓𝛼𝑖。如前所述，我们需要做的，是通过误差函数𝑓（即上文中定义的 DistanceFromTarget函数）在两个不同的点进行采样：
public float PartialGradient (Vector3 target, float[] angles, int i)
{
// Saves the angle,
// it will be restored later
float angle = angles[i];
// Gradient : [F(x+SamplingDistance) - F(x)] / h
float f_x = DistanceFromTarget(target, angles);
angles[i] += SamplingDistance;
float f_x_plus_d = DistanceFromTarget(target, angles);
float gradient = (f_x_plus_d - f_x) / SamplingDistance;
// Restores
angles[i] = angle;
return gradient;
}
当调用该函数时，它会返回一个数字，这个数字表示目标距离如何随着关节旋转而变化。
我们需要做的就是遍历所有关节，计算每个关节对梯度的贡献。
public void InverseKinematics (Vector3 target, float [] angles)
{
for (int i = 0; i < Joints.Length; i ++)
{
// Gradient descent
// Update : Solution -= LearningRate * Gradient
float gradient = PartialGradient(target, angles, i);
angles[i] -= LearningRate * gradient;
}
}
反复调用InverseKinematics函数会使得机械臂逐渐接近目标点。
早期终止机制
使用这种简单的梯度下降实现逆向运动学的主要问题之一是它不太可能收敛。根据你为学习率（LearningRate）和采样距离（SamplingDistance）选择的值，机械臂很可能在实际解附近“抖动”。
这种情况的发生是因为我们对角度的更新幅度过大，导致机械臂越过实际的目标点。解决这一问题的一个合适方法是使用自适应学习率，它会根据我们与解的距离来调整。一个更简单的替代方案是，当误差小于某个阈值时，提前终止解算。
public void InverseKinematics (Vector3 target, float [] angles)
{
if (DistanceFromTarget(target, angles) < DistanceThreshold)
return;
for (int i = Joints.Length -1; i >= 0; i --)
{
// Gradient descent
// Update : Solution -= LearningRate * Gradient
float gradient = PartialGradient(target, angles, i);
angles[i] -= LearningRate * gradient;
// Early termination
if (DistanceFromTarget(target, angles) < DistanceThreshold)
return;
}
}
如果我们在每次关节旋转后重复此检查，就能确保我们执行了所需的最少移动。
为了进一步提高机械臂的性能，我们可以按逆序来应用梯度下降。从机械臂的末端开始，而不是从基座开始，可以让我们做出更小的调整。总体而言，这些小技巧有助于收敛到一个更自然的解。
约束条件
真实关节的一个特点是它们通常只能覆盖一定范围的角度，并非所有关节都可以绕其轴线完全旋转360度。目前，我们对算法没有施加任何限制。这意味着我们很可能会得到如下行为：
解决方案非常直接。我们可以在 RobotJoint 类中添加最小和最大角度：
using UnityEngine;
public class RobotJoint : MonoBehaviour
{
public Vector3 Axis;
public Vector3 StartOffset;
public float MinAngle;
public float MaxAngle;
void Awake ()
{
StartOffset = transform.localPosition;
}
}
然后，确保我们将角度限制在适当的范围内：
public void InverseKinematics (Vector3 target, float [] angles)
{
if (DistanceFromTarget(target, angles) < DistanceThreshold)
return;
for (int i = Joints.Length -1; i >= 0; i --)
{
// Gradient descent
// Update : Solution -= LearningRate * Gradient
float gradient = PartialGradient(target, angles, i);
angles[i] -= LearningRate * gradient;
// Clamp
angles[i] = Mathf.Clamp(angles[i], Joints[i].MinAngle, Joints[i].MaxAngle);
// Early termination
if (DistanceFromTarget(target, angles) < DistanceThreshold)
return;
}
}
问题
即使添加了角度约束和早期终止，我们所使用的算法仍然非常简单——过于简单。使用这种解决方案可能会遇到许多问题，其中大多数与梯度下降有关。正如《梯度下降简介》中所述，该算法可能会陷入局部最小值。这些局部最小值代表了次优解：即接近目标的方式可能是不自然或不理想的。
还有可能出现机械臂运动过度，现在当它返回到原始位置时，机械臂会扭曲。避免这种情况的一种更好的方法是添加一个舒适度函数。如果我们已经到达目标位置，我们应该尝试将机械臂重新调整到更舒适、更自然的姿态。需要注意的是，这不是一个完美的解决方案。重新调整机械臂的姿态可能会迫使算法增加与目标的距离，而这可能违背了原本的要求。
