3、位置分析：闭环矢量法、正向运动学求解、逆向运动学求解

好，咱们进入并联机构最核心的环节——位置分析。

说白了，位置分析就是搞清楚：你给电机一个角度，动平台到底跑到哪儿去了？ 或者反过来，你想让动平台到某个位置，电机该转多少度？

这两个问题，分别对应着正向运动学和逆向运动学。我个人习惯把逆向运动学叫做“送分题”，正向运动学叫做“烧脑题”。为什么？咱们往下看。

3.1 闭环矢量法：建立几何关系的核心工具

并联机构的结构，说白了就是一堆闭环。你想想看，从基座到动平台，至少能画出两条以上的路径。这些路径围起来，就是一个闭环。

处理闭环，最直观的方法就是闭环矢量法。

具体怎么做？我习惯这么干：

1. 选原点：一般选在基座中心或者某个铰链点。
2. 画矢量：从原点出发，沿着连杆画到下一个铰链点，再画到动平台，最后回到原点。
3. 列方程：所有矢量的和等于零。这就是闭环方程。

举个例子，对于一个6自由度Stewart平台，我会把每个支链都看作一个独立的闭环。每个闭环包含：基座铰链矢量、下连杆矢量、上连杆矢量、动平台铰链矢量。加起来等于零。

嗯，这里要注意：矢量方程是二维还是三维的？ 这取决于你的机构。平面机构用二维矢量，空间机构用三维矢量。别搞混了，否则后面算出来的结果全是错的。

3.2 逆向运动学求解：简单但容易踩坑

逆向运动学，就是已知动平台的位姿（位置+姿态），求各个驱动关节的转角或位移。

为什么说它简单？因为你可以直接利用闭环矢量方程，把未知的驱动变量解出来。通常是一个显式表达式，一步到位。

我给大家总结一下标准步骤：

1. 给定动平台位姿：比如位置坐标 (x, y, z) 和姿态角 (α, β, γ)。
2. 计算动平台铰链点坐标：利用姿态矩阵，把动平台上的铰链点从局部坐标系转换到全局坐标系。
3. 计算支链矢量：用动平台铰链点坐标减去基座铰链点坐标，得到每个支链的矢量。
4. 求解驱动变量：根据支链矢量，反算出驱动关节的长度或角度。

下面是一个典型的逆向运动学代码片段（以平面3-RRR机构为例）：

// 已知：动平台中心位置 Px, Py，姿态角 theta
// 求：三个驱动关节的转角 q1, q2, q3

// 1. 计算动平台铰链点坐标（相对于基座坐标系）
for (int i = 0; i < 3; i++) {
    Ai_x = Px + r_platform * cos(theta + beta_i);
    Ai_y = Py + r_platform * sin(theta + beta_i);
}

// 2. 计算支链矢量
for (int i = 0; i < 3; i++) {
    Li_x = Ai_x - Bi_x;  // Bi_x 是基座铰链点坐标
    Li_y = Ai_y - Bi_y;
}

// 3. 求解驱动转角（利用几何关系）
for (int i = 0; i < 3; i++) {
    // 这里用余弦定理求解
    double L = sqrt(Li_x*Li_x + Li_y*Li_y);
    double cos_q = (L*L + l1*l1 - l2*l2) / (2*L*l1);
    q[i] = atan2(Li_y, Li_x) - acos(cos_q);
}

3.3 正向运动学求解：真正的硬骨头

正向运动学，就是已知驱动关节的转角或位移，求动平台的位姿。

为什么难？因为并联机构的闭环约束是非线性的。你没法像串联机构那样，直接乘几个变换矩阵就得到结果。说白了，你需要解一组非线性方程组。

常用的方法有几种：

• 数值法（牛顿-拉夫森法）：最通用，但需要初值。我项目中90%的情况都用它。
• 解析法：针对特定机构，可以推导出封闭解。但推导过程极其繁琐，我一般只在写论文时才用。
• 半解析法：比如利用消元法，把方程组降维。适合中等复杂度的机构。

我个人最推荐数值法，因为它通用性强，而且实现起来不复杂。下面是我常用的牛顿-拉夫森法步骤：

1. 定义误差函数：给定一组驱动变量，用逆向运动学算出一个位姿，然后计算这个位姿下的驱动变量与给定值的差。这个差就是误差。
2. 计算雅可比矩阵：误差函数对位姿变量的偏导数矩阵。
3. 迭代更新：用雅可比矩阵的逆，乘以误差向量，得到位姿的修正量。重复直到误差足够小。

下面是一个牛顿-拉夫森法的代码框架：

// 正向运动学求解（牛顿-拉夫森法）
// 输入：驱动变量 q[3]
// 输出：动平台位姿 Px, Py, theta

// 1. 设置初值（比如用上一时刻的位姿）
Px = prev_Px; Py = prev_Py; theta = prev_theta;

// 2. 迭代
for (int iter = 0; iter < max_iter; iter++) {
    // 计算当前位姿下的驱动变量（调用逆向运动学）
    double q_calc[3];
    inverse_kinematics(Px, Py, theta, q_calc);
    
    // 计算误差
    double error[3];
    for (int i = 0; i < 3; i++) {
        error[i] = q_calc[i] - q[i];
    }
    
    // 如果误差足够小，退出
    if (norm(error) < tolerance) break;
    
    // 计算雅可比矩阵（数值差分或解析）
    double J[3][3];
    compute_jacobian(Px, Py, theta, J);
    
    // 求解线性方程组 J * delta = -error
    double delta[3];
    solve_linear_system(J, delta, error);
    
    // 更新位姿
    Px += delta[0];
    Py += delta[1];
    theta += delta[2];
}

3.4 正向与逆向的对比：什么时候用哪个？

在实际工程中，这两个求解器的使用场景完全不同：

特性	逆向运动学	正向运动学
计算速度	快（显式表达式）	慢（需要迭代）
精度	高（解析解）	依赖初值和迭代次数
通用性	机构专用	通用（数值法）
主要用途	轨迹规划、实时控制	仿真、标定、奇异分析

你想想看，在实时控制中，你肯定希望用逆向运动学，因为它快。但在仿真中，你需要知道驱动角度变化时动平台的实际运动轨迹，这时候就必须用正向运动学了。

3.5 本章知识体系图

下面我用一张SVG图，把本章的核心逻辑串起来。你可以把它当作一个思维导图来看：

这张图把本章的内容浓缩成了三个层次：最上层是工具（闭环矢量法），中间是两大求解方向，最下层是各自的特点。你把这个逻辑理清了，位置分析这块就算拿下了。

---