8.6 机械臂的运动
(Robot Arm Motion)

问题定义

运动规划领域中一个具有重要实际意义的分支是固定基座机械臂(robot arm)的运动规划。本节将探讨一个特别简单的实例，即平面多连杆机械臂。这是由一系列固定长度的线段组成的链。那些线段被称为连杆(link)，记为$L_i, i = 1, \dots, n$。它们通过关节(joint)连接，记为 $J_i, i = 0, \dots, n$。关节 $J_0$ 固定在原点，有时称之为机械臂的肩部(shoulder)。对于 $0 < i < n$，$J_i$ 是 $L_i$ 和 $L_{i+1}$ 之间的关节。$J_n$ 是 $L_n$ 的末端，有时称为手(hand)。参见图 8.17。

我们需要符号来表示与给定机械臂相关的各种量。令 $\ell_i$ 为连杆 $L_i$ 的长度，$j_i$ 为关节 $J_i$ 处的角度，它表示 $L_i$ 到 $L_{i+1}$ 的逆时针角度，这两根连杆分别视为从 $J_{i-1}$ 到 $J_i$ 以及从 $J_i$ 到 $J_{i+1}$ 的向量。角度 $j_0$ 是从正 $x$ 轴开始测量的，$j_n$ 未定义。机械臂 $A$ 由其连杆长度列表 $(\ell_1, \dots, \ell_n)$ 指定。

我们将探讨这一通用问题的一个特别简单的版本，其简单之处体现在两个方面:

对关节角度不作任何限制。特别是，允许机械臂自相交。
假设平面为空，即不存在任何障碍物。

在这些条件下，我们研究可达性问题。给定定义机械臂 $A$ 的 $n$ 个连杆长度 $\ell_i$，以及平面中的一点 $p$，判定 $A$ 是否能到达 $p$。如果能，找出一组关节角度使得 $J_n = p$。你会发现，判定 $p$ 是否可达是容易的，但计算出能实现该解的关节角度会有一些挑战性。

历史

机械臂运动的算法问题最早由 Hopcroft, Joseph & Whitesides(1985) 在一篇论文中进行了探讨。他们证明了，无障碍物环境下是容易的。存在任意障碍物的问题是困难的，专业术语称为 NP-hard。机械臂被限制在圆内运动的问题是可解的，即具有多项式时间复杂度。此后，许多研究人员对他们提出的圆内限制算法进行了改进，或者针对不同的障碍物环境开发出了类似的算法。关于该课题的综述，可参阅 Whitesides(1991)。

8.6.1. 可达性:判定 (Reachability: Decision)

多连杆机械臂可达的点集是什么? 答案出奇地简单: 它总是一个以原点为中心的圆环，即位于两个同心圆之间的闭点集。我们在下文的引理 8.6.1 中说明这一点，然后在定理 8.6.3 中根据连杆长度确定圆环的内外半径 $r_i, r_o$。

可达区域 (Reachability Region)

单连杆机械臂可达的区域是一个以原点为中心的圆，这是一个内外半径相等的圆环。

设 $A = (\ell_1, \ell_2)$ 为一个双连杆机械臂。如果 $\ell_1 \ge \ell_2$，那么显然可达区域是一个圆环，其外半径 $r_o = \ell_1 + \ell_2$$，内半径 $r_i = \ell_1 - \ell_2$。如图 8.18(a) 所示。如果 $\ell_1 = \ell_2$，则 $r_i = 0$，该圆环退化为半径为 $r_o$ 的圆盘。

当 $\ell_1 < \ell_2$ 时，情况或许不那么直观。但正如图 8.18(b) 所示，结果仍然是一个圆环，其 $r_o = \ell_1 + \ell_2$，$r_i = \ell_2 - \ell_1$。为了书写方便，也可以记作 $r_i = |\ell_1 - \ell_2|$。

将双连杆可达区域视为两个圆的闵可夫斯基和(Minkowski sum)(见8.3.1 节)是很有启发意义的。在半径为 $\ell_1$ 的圆 $C_1$ 上的每一点，以此点为中心作一个半径为 $\ell_2$ 的圆。由此可知，两个以原点为中心的圆之和，便是一个以原点为中心的圆环。

此外，显然，两个以原点中心的图形 —— 一个圆环和一个圆的和，仍然是一个以原点为中心的圆环。我们有:

引理 8.6.1. $n$ 连杆机械臂的可达区域是一个以原点，即肩部，为中心的圆环。

圆环半径 (Annulus Radii)

尽管引理 8.6.1 中圆环的外半径显而易见，$r_o = \sum_{i=1}^n \ell_i$，即所有连杆完全伸直时的长度。但内半径却不那么直观。下面我们来计算内半径 $r_i$。

$r_i > 0$ 与否，取决于最长连杆的长度与其他连杆长度之间的关系。具体而言，当且仅当最长连杆的长度大于所有其他连杆长度之和时，有$r_i > 0$。如果最长连杆是机械臂的第一节，这一点最容易理解。接下来，我们将展示如何在不失一般性地情况下，该命题也成立。

引理 8.6.2. 机械臂的可达区域与连杆的排列顺序无关。

证明: 这源于向量加法的交换律。例如，考虑如图 8.19(a) 所示的特定双连杆机械臂构型。沿着平行四边形的另外两条边移动，显然也能到达同一个终点。对于如图 (b) 所示的三连杆机械臂，这一结论同样成立，实际上对于 $n$ 连杆机械臂也是如此。□

因此，不失一般性，我们重点关注首个连杆 $L_1$ 最长的机械臂上。对于此类机械臂，由图 8.20 可知，只要和 $\ell_1 - \sum_{i=2}^n \ell_i$ 为正，就有 $r_i = \ell_1 - \sum_{i=2}^n \ell_i$。否则 $r_i = 0$。现在我们可以总结这些结论，该结论最早由 Hopcroft et al.(1985) 提出。

定理 8.6.3. $n$ 连杆机械臂的可达区域是一个以原点为中心的圆环。其外半径为 $r_o = \sum_{i=1}^n \ell_i$。如果最长连杆长度 $\ell_M$ 小于或等于连杆总长度的一半，则内半径 $r_i = 0$。否则，内半径 $r_i = \ell_M - \sum_{i \neq M} \ell_i$。

该定理的一个直接推论是，我们可以在 $O(n)$ 时间内判定可达性。首先找到 $\ell_M$ 并计算 $r_o$ 和 $r_i$。那么点 $p$ 可达当且仅当 $r_i \le |p| \le r_o$。然而，该定理并未提示如何找到能够到达给定点的构型的方法。接下来我们讨论这个问题。

8.6.2. 可达性:构造 (Reachability: Construction)

乍看之下，要找到一种能让 $n$ 连杆机械臂触及可达区域内某一点的构型的方法，并不是显而易见的。从某种意义上说，解空间太大了，如果试图系统地探索所有潜在解，往往会陷入指数级增长的可能性之中。若试图划定每个关节处可能包含解的角度范围，很快就会分裂成指数级数量的区间。

幸运的是，我们只需要找到其中一个解就行了。利用这点，可以设计出效率高很多的算法。在探讨 $n$ 连杆情况之前，我们先研究 2 连杆和 3 连杆的问题。

2-连杆可达性 (2-Link Reachability)

对于单连杆机械臂，确定使其末段触及圆周上某一点的肩部角度 $j_0$ 是轻而易举的。解决 2-连杆问题也不复杂。设 $p$ 为目标点。只需将以原点为圆心、半径为 $\ell_1$ 的圆 $C_1$ 与以 $p$ 为圆心、半径为 $\ell_2$ 的圆 $C_2$ 求交。通常会有两个解，但也可能有零个、一个或无穷多个解的情况，这取决于圆的相交方式，如图 8.21 所示。我们将在 8.6.3 节讨论如何计算这一交点。

3-连杆可达性 (3-Link Reachability)

我们的总体思想是将多连杆问题简化为2-连杆问题。设 $A_3 = (\ell_1, \ell_2, \ell_3)$。根据引理 8.6.1，可知 $A_2 = (\ell_1, \ell_2)$ 的可达区域是一个圆环，记该区域为 $R$。注意，圆环 $R$ 的边界 $\partial R$ 上的所有点都对应 $A_2$ 的极值构型，即要么 $j_1 = 0$ 两臂共线(aligned)，要么 $j_1 = \pi$ 反向共线(antialigned)。在这些位置上，$A_2$ 就像一根长度分别为 $\ell_1 + \ell_2$ 或 $|\ell_1 - \ell_2|$ 的单连杆。

现在考察以 $p = J_3$ 为圆心，半径为 $\ell_3$ 的圆 $C$ 是如何与 $R$ 相交的。我们的目标是将3-连杆问题归结为2-连杆的共线的情形，以便将它们视为2-连杆问题的解来处理。根据 $\partial R \cap C$ 是否为空集，我们分两种情况来讨论。

情况 1: $\partial R \cap C \neq \emptyset$，如图 8.22(a,b) 所示

在这种情况下，通过使 $L_1$ 和 $L_2$ 共线(图a) 或反向共线(图b)，问题可以简化为2-连杆问题。当然，通常存在无限多其他解，但我们只限于寻找其中一个。为了避免连杆反向共线带来的不便，我们对图 8.22(b) 的情形进行更深入的分析。

设 $\partial R = I \cup O$，其中 $I$ 是内环，$O$ 是外环。若如图 8.22(b) 那样，满足 $O \cap C = \emptyset$ 且 $I \cap C \neq \emptyset$，我们可以选择一个半径为 $\ell_2$ 且与 $C$ 相切的圆 $C_2$，这就可以通过使 $L_2$ 和 $L_3$ 的共线来到达 $p$。注意不是 $L_1$ 和 $L_2$ 的反向共线，见图 8.23。
情况 2: $\partial R \cap C = \emptyset$

这里可以进一步根据圆 $C$ 是否包围原点 $J_0$ 区分出两种情况:
1. $C$ 不包围 $J_0$，如图 8.22(c) 所示。
  
  这种情况同样可以找到一个使两根连杆共线的解。设 $C_2$ 为圆环 $R$ 内，半径为 $\ell_2$ 且与 $C$ 相切的圆。那么 $L_2$ 和 $L_3$ 可以共线，类似于图 8.23 的方式，这再次将问题简化为2-连杆问题。
2. $C$ 包围 $J_0$, 如图 8.22(d) 所示。
  
  此情形下，不存在两连杆共线或反向共线的解，这打破了“所有3-连杆问题都可以通过此类共线方式求解”的设想。尽管如此，这种情况还有另一个特征使其易于解决: 对于任意 $j_0$ 值都存在一个解!
  
  要理解这一点，不妨任意选择一个 $j_0$，并以 $J_1$ 为圆心画一个圆 $C_2$。因为 $C$ 在圆环 $R$ 内且包围原点，它必然包围 $R$ 的内边界 $I$。由于 $C_2$ 连接 $R$ 的内边界到外边界，它必然在某处与 $C$ 相交。该交点便构成了任意 $j_0$ 的一个解。
  
  因此，我们终究可以将这种情况简化为2-连杆问题，任意选择 $j_0$，比如 $j_0 = 0$，然后求解由此产生的2-连杆问题即可。

我们将结论总结为一个引理:

引理 8.6.4。 每个3连杆问题都可以通过以下2连杆问题之一来解决:

$(\ell_1 + \ell_2, \ell_3)$
$(\ell_1, \ell_2 + \ell_3)$
$j_0 = 0 \text{and} (\ell_2, \ell_3)$

证明: 图 8.22(a) 对应(1)，图 8.22(b)及图 8.23和图 8.22(c) 对应(2)，图 8.22(d) 对应 (3)。□

n-连杆可达性 (n-Link Reachability)

n-连杆可达性的线性算法。重新审视图 8.22，但此时将圆环 $R$ 想象成 $n$-连杆机械臂 $A$ 中前 $n-1$ 个连杆所构成的区域，并将 $C$ 视为圆心在 $p$ 的半径为 $\ell_n$ 的圆。由于我们假设 $A$ 能够到达目标点，可知 $R \cap C$ 非空。实际上，可能得相交情况也仅限于图 8.22 中所示的那几种。这启发我们采用以下递归过程来确定 $n$-连杆到达点 $p$ 的构型:

$\partial R \cap C \neq \emptyset$，如图 8.22(a,b) 所示，在(通常为两个的)交点中任选一点 $t$。
$R \supseteq C$，如图 8.22(c,d)，选择 $C$ 上的任意点 $t$, 比如说距离 $J_0$ 最远的那个点。

在这两种情况下，都要递归求解 $A_{n-1} = (\ell_1, \dots, \ell_{n-1})$ 找到一个到达 $t$ 的构型。然后根据 $C$ 是以 $p$ 为中心的圆，将最后一个连杆 $L_n$ 附加到该解中。递归的终止条件可以采用之前概述的 3-连杆问题的解。

因为图 8.22 涵盖了所有可能得情况，所以如果存在的解，那么该过程定能找到一个解。它只需要 $O(n)$ 的时间，这是因为将 $n$ 减少 1 是在常数时间内完成的。具体做法是计算 $C$ 与 $O, I$ 的交集，其中 $\partial R = I \cup O$。

至此，我们的目标达成了。给定目标点 $p$，以及机械臂的连杆长度列表，首先利用定理 8.6.3 确定 $p$ 是否可达，如果可达，则通过此递归过程找到一个构型。

两个折点 (Two Kinks)。虽然无法优化 $O(n)$ 这一渐近时间复杂度，因为仅计算各连杆长度之和就需要这么长时间，但实际上在概念上是可以进行显著简化。上述算法在情形 1 中得到的解法简单明了，这给我们一个启发: 如果 $p \in O$，则前 $n-1$ 个连杆均被拉直。如果 $p \in I$，则仅在最长连杆两端的关节处出现"折点(kinked)"。后者可由 $r_i$ 的公式推出: 所有连杆均需"对抗"最长连杆 $L_M$，才能触及内环上的某点。因此，在情形 1 中，机械臂不一定有许多折点。而在情形 2 中，$p$ 可以位于 $C$ 上的任何位置，这表明可以利用这种自由度来避免折点。

事实上，有一个非凡的定理指出: 若一个 $n$-连杆机械臂能够触及某点，那么它仅需要两个折点关节即可实现这一目标！此外，这两个关节也很容易确定。这意味着任何 $n$ 连杆问题都可以直接简化为一个 3 连杆问题！我们现在着手证明这一点。

定理 8.6.5. (两个折点 Two Kinks) 如果一个 $n$ 连杆机械臂 $A$ 能够到达某一点，那么它最多只需两个关节处于"折叠"状态就可以到达该点，即，在关节 $J_1, \ldots, J_{n-1}$ 中只有两个关节具有非零角度。这两个关节可以选择为位于"中位连杆(median link)"两端的关节。该中位连杆 $L_m$ 满足 $\sum_{i=1}^{m-1} \ell_i$ 小于或等于连杆总长度的一半，但 $\sum_{i=1}^m \ell_i$ 大于总长度一半。

证明: 证明的策略是，通过冻结除指定的两个关节之外的所有关节，对机械臂 $A$ 进行修改，并证明由此产生的新机械臂 $A'$ 具有相同的可达区域。所谓的冻结是指将关节的角度固定为 0。因为 $r_o$ 仅取决于连杆长度之和(定理 8.6.3)，这种冻结操作不会改变 $r_o$ 的值。所以我们只需证明 $r_i$ 也保持不变。

设 $\ell$ 为连杆的总长度。我们根据 $r_i$ 是否为 0，分两种情况来证明:

情况 $r_i > 0$，如图 8.24(a) 所示。

根据定理 8.6.3，$r_i$ 仅在最长连杆 $L_M$ 超过其余连杆长度之和时非零。此时必然有 $\ell_M > \ell/2$。因此，无论 $L_M$ 在连杆序列中的位置如何，都有 $L_M = L_m$。因为 $L_M$ 太长了，它一定会覆盖机械臂的中点。

既然 $L_m = L_M$ 且 $\ell_M > \sum_{i \neq M} \ell_i$，如果我们冻结除 $L_M$ 端点处的关节以外的所有关节，形成一个新的机械臂 $A'$，并没有改变 $L_M$ 是最长连杆这一事实。在图 8.24(a) 中，$A$ 和 $A'$ 的最长连杆长度均为 6。根据定理 8.6.3，$r_i$ 仅取决于 $\ell$ 和 $\ell_M$，因此 $A'$ 具有与 $A$ 相同的可达区域。

情况 $r_i = 0$，如图 8.24(b) 所示。

根据定理 8.6.3 有，最长连杆 $L_M$ 的长度 $\le \ell/2$，因为 $\ell_M \le \sum_{i \neq M} \ell_i$。设 $L_m$ 为中位连杆，锁定 $L_m$ 之前和之后的所有关节，形成机械臂 $A'$。这可能会改变最长连杆的选择。在图 8.24(b) 中，$A$ 中最长连杆长度为 6，而在 $A'$ 中为 8。但请注意，新的最长连杆 $L'_M$ 的长度不可能超过 $\ell/2$。因为 $L_m$ 跨越了机械臂的中点，其前面和后面的部分长度必然都 $\le \ell/2$。由于 $r_i$ 仅在最长连杆超过 $\ell/2$ 时才非零，我们可以确定 $r_i$ 仍然为零。因此，$A'$ 的可达区域与 $A$ 相同。

综上得证。□

定理 8.6.5 为我们提供了一种替代的 $O(n)$ 算法，其中唯一依赖于 $n$ 的部分是计算 $n$ 个连杆的长度之和。在此之后，该算法只需常数时间。因此，如果我们统计圆交点测试次数，递归算法需要 $O(n)$ 次测试，而"两个折点算法"仅需 $O(1)$ 次。因为在识别出中位连杆 $L_m$ 后，问题被简化为一个 3-连杆问题。根据引理 8.6.4，该问题进一步简化为三个 2-连杆问题，而每个 2-连杆问题只需执行一次圆相交测试。虽然如此，但是对于实际的机械臂而言，这样的解好像没啥用呀。

8.6.3. 连杆配置的实现(Implementation of Link Configuration)

要实现上述算法还是比较简单的，只要细致处理两个圆的交点就行。在深入探讨圆相交的细节之前，我们首先介绍顶层的处理过程。

连杆长度存储在一个整数数组中。在代码的大部分逻辑中，我们坚持使用整型数据，迫不得已时也会使用双精度浮点数(doubles)。当然，在进行圆求交运算时就会被迫使用 double 类型。这样做有助于隔离浮点运算引发的问题。main 函数和数据结构如 Code8.6 所示。在使用 ReadLinks 读取连杆长度后，main 函数进入一个循环，读取目标位置，并通过调用 SolveN 来解决针对该目标的可达性问题。该函数调用了一连串的函数，每一层都将问题简化为一个更简单的问题: $Solven \rightarrow Solve3 \rightarrow Solve2 \rightarrow TwoCircles \rightarrow TwoCircles0a \rightarrow TwoCircles0b \rightarrow TwoCircles00$。其中的三个 $Solvex$ 函数都是布尔函数，仅当目标可达时返回 TRUE。另有四个 $TwoCircles$ 函数用于计算圆的交点数量以及其中一个交点 $p$。该点作为参数 $J$ 向上传递给 $Solve3$ 并打印出来。现在我们逐一描述这些主要函数。

展开 Code 8.6. main.>>>

        /* Global variables. */
        int     linklen[NLINKS];    /* link lengths */
        int     nlinks;             /* number of links */
        tPointi target;             /* target point */
        
        main()
        {
            tPointi origin = {0,0};
        
            nlinks = ReadLinks();
            while (TRUE) {  /* loop broken by EOF in ReadTarget */
                ReadTarget( target );
                MoveTo_i( origin );
                if ( !Solven( nlinks ) )
                    printf("Solven: no solutions!\n");
                LineTo_i( target );
            }
        }

如 Code 8.7 所示，函数 $Solven$ 用于识别中位连杆，并在其前后关节冻结的状态下调用 $Solve3$。整个代码中，使用 $L1$、$L2$ 等来表示长度 $\ell_1, \ell_2, \dots$，以避免"l1"这种容易混淆的排版形式。如 Code8.8 所示，$Solve3$ 根据引理 8.6.4，最多调用三次 $Solve2$。只有最后一次调用会产生两个折点关节。 $Solve2$，如 Code8.9 所示，仅仅是为 $TwoCircles$ 函数整理参数。函数 $TwoCircles$ 具体负责计算两个圆的交点。

展开 Code 8.7. Solven.>>>

        bool Solven( int nlinks )
        {
            int i;
            int m;                  /* index of median link */
            int L1, L2, L3;         /* length of links between kinks */
            int totlength;          /* total length of all links */
            int halflength;         /* floor of half of total */
        
            /* Compute total and half length. */
            totlength = 0;
            for ( i = 0; i < nlinks; i++ )
                totlength += linklen[i];
            halflength = totlength / 2;
        
            /* Find median link. */
            L1 = 0;
            for ( m = 0; m < nlinks; m++ ) {
                if ( (L1 + linklen[m]) > halflength)
                    break;
                L1 += linklen[m];
            }
        
            L2 = linklen[m];
            L3 = totlength - L1 - L2;
            if ( Solve3( L1, L2, L3, target ) )
                return TRUE;
            else return FALSE;
        }

展开 Code 8.8. Solve3.>>>

        bool Solve3( int L1, int L2, int L3, tPointi target )
        {
            tPointd Jk;             /* coords of kinked joint returned by Solve2 */
            tPointi J1;             /* Joint1 on x axis */
            tPointi Ttarget;        /* translated target */
        
            if ( Solve2( L1 + L2, L3, target, Jk ) ) {
                LineTo_d( Jk );
                return TRUE;
            }
            else if ( Solve2( L1, L2 + L3, target, Jk ) ) {
                LineTo_d( Jk );
                return TRUE;
            }
            else {      /* pin J0 to 0. */
                /* Shift so J1 is origin. */
                J1[X] = L1; J1[Y] = 0;
                SubVec( target, J1, Ttarget );
                if ( Solve2( L2, L3, Ttarget, Jk ) ) {
                    /* Shift solution back to origin. */
                    Jk[X] += L1;
                    LineTo_i( J1 );
                    LineTo_d( Jk );
                    return TRUE;
                }
                else
                    return FALSE;
            }
        }

展开 Code 8.9. Solve2.>>>

        bool Solve2( int L1, int L2, tPointi target, tPointd J )
        {
            tPointi c1 = {0,0};       /* center of circle 1 */
            int nsoln;                /* # of solns: 0,1,2,3(infinite) */
        
            nsoln = TwoCircles( c1, L1, target, L2, J );
            return nsoln != 0;
        }

两圆相交 (Intersection of Two Circles)

显然，两个圆的相交可以在常数时间内完成，因此唯一的问题在于实际应用层面。我们将开发足够通用的代码，以便在其他应用中使用。

设两圆 $C_1, C_2$ 的圆心分别为 $c_i = (a_i, b_i)$，半径为 $r_i$，其中 $i = 1, 2$。因为圆的方程是二次方程，根据一般的代数原理，我们可以预期交点不超过四个。但实际上，由于方程具有特殊形式，交点最多只有两个。当然，如图 8.21 所示，也可能存在零个、一个或无穷多个交点。我们的首要任务是区分这些情况，其次是求解存在两个交点的一般情况。

若能根据坐标系合理安排两圆的位置，可以极大简化问题。不失一般性的，假设 $c_1 = (0, 0), c_2 = (a_2, 0)$。 Code8.10 中的函数 $TwoCircles$ 的唯一功能是通过平移使得 $c_1 = (0, 0)$，然后调用 $TwoCircles0a$。

展开 Code 8.10. TwoCircles. SubVec is in Code7.6.>>>

        /* TwoCircles finds an intersection point between two circles.
           General routine: no assumptions. Returns # of intersections; point in p. */
        int TwoCircles( tPointi c1, int r1, tPointi c2, int r2, tPointd p )
        {
            tPointi c;
            tPointd q;
            int nsoln = -1;
        
            /* Translate so that c1 = {0,0}. */
            SubVec( c2, c1, c );
            nsoln = TwoCircles0a( r1, c, r2, q );
        
            /* Translate back. */
            p[X] = q[X] + c1[X];
            p[Y] = q[Y] + c1[Y];
        
            return nsoln;
        }

Code8.11 中函数 $TwoCircles0a$ 负责处理所有特殊情况。为了坚持使用整形数据，我们在进行浮点除法之前先检测所有的特殊情况。我们假设目标点具有整数坐标。通过计算 $(r_1 + r_2)^2$ 和 $(r_1 - r_2)^2$ 并与到 $c_2$ 的距离平方进行比较，可以检测出零个、一个或无限个交点的情况。为了提高精度并防止平方运算时发生正数溢出，我们会采用 7.2 节的方案，转换成 double 类型数据，但比较操作仍然在整数之间进行。若无这种保护，半径为 $10^5$ 时就会溢出。在只有一个交点的情况下，我们知道交点位于距离原点距离为 $r_1$ 处，且该点位于原点到 $c_2$ 的连线上。例如，如果 $r_1 = 10, r_2 = 15, c_2 = (-3, -4)$，那么 $(r_1 - r_2)^2 = 25 = |c_2|^2$，并且使用比例 $f = \frac{10}{-5} = -2$ 来计算交点 $p = f \cdot c_2 = (6, 8)$。

展开 Code 8.11. TwoCircles0a; Length2 not shown.>>>

        /* TwoCircles0a assumes that the first circle is centered on the origin.
           Returns # of intersections: 0, 1, 2, 3 (inf); point in p. */
        int TwoCircles0a( int r1, tPointi c2, int r2, tPointd p )
        {
            double dc2;             /* dist to center 2 squared */
            double rplus2, rminus2; /* (r1 +/- r2)^2 */
            double f;               /* fraction along c2 for nsoln = 1 */
        
            /* Handle special cases. */
            dc2 = Length2( c2 );
            rplus2 = (r1 + r2) * (r1 + r2);
            rminus2 = (r1 - r2) * (r1 - r2);
        
            /* No solution if c2 out of reach + or -. */
            if ( ( dc2 > rplus2 ) || ( dc2 < rminus2 ) )
                return 0;
        
            /* One solution if c2 just reached. */
            /* Then solution is r1-of-the-way (f) to c2. */
            if ( dc2 == rplus2 ) {
                f = r1 / (double)(r1 + r2);
                p[X] = f * c2[X]; p[Y] = f * c2[Y];
                return 1;
            }
        
            if ( dc2 == rminus2 ) {
                if ( rminus2 == 0 ) { /* Circles coincide. */
                    p[X] = r1; p[Y] = 0;
                    return 3;
                }
                f = r1 / (double)(r1 - r2);
                p[X] = f * c2[X]; p[Y] = f * c2[Y];
                return 1;
            }
        
            /* Two intersections. */
            return TwoCircles0b( r1, c2, r2, p );
        }

如果没有特殊情况，$TwoCircles0a$ 就会调用 $TwoCircles0b$(Code8.12)。该函数通过将 $c_2$ 放置在 $x$ 轴上，来实现坐标系的一种便捷布局。它先旋转 $c_2$ 使其位于 $x$ 轴上，调用 $TwoCircles00$ 在此旋转后的坐标系中求解，最后再将结果旋转回原坐标系。旋转操作采用图形学中常用的标准方法，将点 $q$ 乘以旋转矩阵 $R$:

$$ \begin{equation}\label{f8.1} \begin{bmatrix} p_0 \\ p_1 \end{bmatrix} = \begin{bmatrix} \cos \theta & -\sin \theta \\ \sin \theta & \cos \theta \end{bmatrix} \cdot \begin{bmatrix} q_0 \\ q_1 \end{bmatrix} \end{equation} \tag{8.1} $$

注意，$\sin \theta$ 和 $\cos \theta$ 可以通过简单的比率计算得出，不需要调用三角函数库。

展开 Code 8.12. TwoCircles0b>>>

        /* TwoCircles0b also assumes that the 1st circle is origin-centered. */
        int TwoCircles0b( int r1, tPointi c2, int r2, tPointd p )
        {
            double a2;              /* center of 2nd circle when rotated to x axis */
            tPointd q;              /* one solution when c2 on x axis */
            double cost, sint;      /* sine and cosine of angle of c2 */
        
            /* Rotate c2 to a2 on x axis. */
            a2 = sqrt( Length2( c2 ) );
            cost = c2[X] / a2;
            sint = c2[Y] / a2;
        
            TwoCircles00( r1, a2, r2, q );
        
            /* Rotate back */
            p[X] = cost * q[X] + -sint * q[Y];
            p[Y] = sint * q[X] +  cost * q[Y];
        
            return 2;
        }

最后，$TwoCircles00$(Code8.13) 负责计算通用的两个圆交点。其任务是联立求解以下两个方程:

$$ \begin{aligned} x^2 + y^2 &= r_1^2, \\ (x - a_2)^2 + y^2 &= r_2^2. \end{aligned} $$

由第一个方程解出 $y^2$ 并代入第二个方程，得到 $(x - a_2)^2 + r_1^2 - x^2 = r_2^2$，由此可解出 $x$:

$$ x = \frac{1}{2} \left( a_2 + \frac{r_1^2 - r_2^2}{a_2} \right) $$ 展开 Code 8.13. TwoCircles00>>>

        /* TwoCircles00 assumes circle centers are (0,0) and (a2,0). */
        void TwoCircles00( int r1, double a2, int r2, tPointd p )
        {
            double r1sq, r2sq;
            r1sq = r1 * r1;
            r2sq = r2 * r2;
        
            /* Return only positive-y soln in p. */
            p[X] = ( a2 + ( r1sq - r2sq ) / a2 ) / 2;
            p[Y] = sqrt( r1sq - p[X] * p[X] );
        }

注意 $a_2 \neq 0$，因为我们已经排除了无解和无穷多解的情况。得到 $x$ 后，我们将其代入其中一个圆方程求解 $y$。这两个解具有相同的 $x$ 坐标，且其中一个解的 $y$ 坐标是另一个的相反数。这正是在合适的坐标系下进行计算的优势。构建完整可运行代码所需的其余辅助函数都很简单，在此不再赘述。

遗憾的是，计算几何代码往往存在一个典型的特征，大部分精力都花在了处理各种特殊情况上。在本例中，圆的实际求交运算仅需两行代码，但在这两行代码之前，还需要编写大量代码确保它们能正确运行。

示例

考虑一个 4 连杆机械臂，各连杆长度分别为 $\{100,10,40,90\}$。我们首先详细分析一个特定的目标，然后查看针对一系列目标的输出结果。首先设定目标为回到肩部位置，即 $(0, 0)$。$SolveN$ 计算出总长度为 240，并识别出第三根连杆为中位连杆。接着它调用 $Solve3(110, 40, 90)$ 进而调用 $Solve2(150, 90)$ 和 $Solve2(110, 130)$，但两个都失败了。前者是因为机械手无法回伸到 $(0, 0)$，后者是因为机械手必然会越过 $(0, 0)$。随后我们进入引理 8.6.4的第三种情况，第一个关节固定在 $j_0 = 0$，并调用 $Solve2(40, 90)$，此时试图到达 $(-110, 0)$。因为前两根连杆被冻结在 $0^\circ$，长度 $\ell_1 = 100 + 10$$。这次成功了，在 $TwoCircles00$ 中找到了交点 $p = (25.45, 30.86)$，经过逆变换后，该点作为 $p = (-25.45, -30.86)$ 从 $Solve2$ 返回，最终作为 $p = (84.55, -30.86)$ 从 $Solve3$ 返回。程序输出的 $J_3$ 坐标正是该点。相应的机械臂构型如图 8.25 所示。

现在我们考察该代码在相同的四根连杆上，针对一系列目标点 $(5k, 5k), k = 0, 1, \dots, 30$ 的运动表现。代码输出结果如图 8.26 所示。当 $k=0$ 时，目标为 $(0, 0)$，我们将获得图 8.25 中显示的解。在 $k=3$ 时，目标点 $(15, 15)$ 对应引理 8.6.4 的第二种情况，首次通过 $Solve2(110, 130)$ 返回。这导致 $J_2$ 跳跃到 $(-100.67, -44.33)$。$k=9$ 时，目标点 $(45, 45)$ 首次通过 $Solve2(150, 90)$ 可达，导致构型再次出现不连续。对于更大的 $k$ 值，目标点可通过引理 8.6.4 的首选项到达。从这个例子可以看出，寻找一系列平滑变化的构型以跟踪移动目标将是一个有趣的问题(练习 8.6.4[6])。

8.6.4. 练习(Exercises)

多边形内外翻转 Turning a polygon inside out。设想一个多边形，其边是刚性连杆，顶点为关节。将多边形内外翻转是指，通过平面内的连续运动将其转换为，原多边形关于平面内任意一条直线的镜像 (Lenhart & Whitesides 1991)。此处允许中间形态为自相交多边形。对于任意多边形，这是否可行? 若行证明之。若不行，找出让它行的条件。
[Programming] 除以零 Division by zero。确定在何种情况下 $TwoCircles0a$, $TwoCircles0b$ 或 $TwoCircles00$中会发生除以零的问题。是否存在某些输入能导致代码触发这些条件? 请在代码上验证你的结论。
含障碍物的可达区域 Reachability region with pole(s)。如果平面内存在不可穿透的障碍物，比如一些柱体，确定 2-连杆机械臂的可达区域是什么。特别地，考虑以下障碍物：
1. 单个点。
2. 两个点。
3. 一个圆盘。
直线跟踪 Line tracking。如果机械臂末端沿直线移动，则称该机械臂的连续运动为直线跟踪(Whitesides 1991)。
1. 2-连杆机械臂能进行跟踪直线吗? 它能跟踪任意直线吗?
2. 3-连杆机械臂能进行跟踪直线吗? 它能跟踪任意直线吗?
关节约束 Joint constraints。假设每个关节仅能在特定角度范围内自由移动，即 $j_i$ 的运动范围为 $\pm\theta_i$。
1. 具有关节约束的 2-连杆机械臂的可达区域是什么？
2. 具有关节约束的 3-连杆机械臂的可达区域是什么？
平滑跟踪 Smooth tracking。设目标点 $p(t)$ 随时间 $t$ 平滑移动。
1. 定义机械臂构型平滑跟踪 $p(t)$ 的含义。
2. 你能否找到一个例子，其中机械臂在所有时刻 $t$ 都能到达 $p(t)$，但不存在满足你关于平滑跟踪定义的构型序列?

8.6 机械臂的运动(Robot Arm Motion)