首页 / 博客 / 机械臂逆运动学求解器：CCD与Jacobian对比实战
ESC
输入关键词搜索文章标题和内容

机械臂逆运动学求解器：CCD与Jacobian对比实战

2026-05-28 | 机器人 | 6
机器人学 逆运动学 数值方法

本文由 linuxROS 整理发布，首发于 linuxros.cn，转载请注明出处。

机械臂逆运动学求解器：CCD与Jacobian对比实战

导读：给定机械臂末端要到达的三维坐标，如何反算每个关节该转多少度？本文从零实现两种数值IK算法——CCD（循环坐标下降）利用几何投影快速迭代，Jacobian（阻尼最小二乘）用梯度下降稳定收敛。零外部依赖，300行C++代码覆盖原理推导与工程实现。

一、逆运动学问题建模

逆运动学（IK）的核心问题是：已知末端要到达的目标位置，反算每个关节该转多少度。这就像你知道手要碰到桌子上的杯子，反推肩膀和肘部该怎么弯——给定结果，求过程。

数值IK的本质是迭代优化——从初始猜测出发，逐步调整关节角直到末端到达目标位置。每次迭代就像闭着眼睛用手去摸索目标：偏差太大就往回缩一点，方向不对就转一点，反复几次总能碰到。不同于解析IK（如6轴机械臂的三角函数闭式解），数值IK适用于任意结构的机器人。

flowchart TB subgraph Input["📥 输入"] A["目标位置 x, y, z"] B["初始关节角 θ₀"] end subgraph Loop["🔄 迭代循环"] C["FK计算末端位置"] C --> D{"末端误差 < ε?"} D -->|"否"| E["计算关节更新量"] E --> F["更新关节角"] F --> C D -->|"是"| G["返回关节角"] end subgraph Output["📤 输出"] H(["最优关节角 θ*"]) end A --> E B --> C G --> H style Input fill:#E3F2FD,stroke:#1976D2 style Loop fill:#FFF8E1,stroke:#F57C00 style Output fill:#E8F5E9,stroke:#388E3C

1.1 正运动学（FK）基础

FK是IK的反向：已知关节角，求末端位置。本项目用标准DH参数建模：

DH参数 含义
a 连杆长度
alpha 连杆扭角
theta 关节转角
d 连杆偏移

单关节的齐次变换矩阵：

// DHChain.cpp · 两连杆FK
common::Mat4 DHChain::dhTransform(const DHParameter& dh, double theta) const {
    double ct = std::cos(theta);
    double st = std::sin(theta);
    double ca = std::cos(dh.alpha);
    double sa = std::sin(dh.alpha);

    common::Mat4 m;
    m(0, 0) = ct;         m(0, 1) = -st * ca;  m(0, 2) = st * sa;   m(0, 3) = dh.a * ct;
    m(1, 0) = st;         m(1, 1) = ct * ca;   m(1, 2) = -ct * sa;  m(1, 3) = dh.a * st;
    m(2, 0) = 0;          m(2, 1) = sa;        m(2, 2) = ca;        m(2, 3) = dh.d;
    m(3, 0) = 0;          m(3, 1) = 0;         m(3, 2) = 0;         m(3, 3) = 1;
    return m;
}

串联n个关节的FK就是矩阵连乘：

common::Mat4 DHChain::forwardKinematics(const std::vector<double>& jointAngles, int endLink) const {
    common::Mat4 result = common::Mat4::identity();
    int lastLink = (endLink < 0) ? static_cast<int>(links_.size()) : endLink;

    for (int i = 0; i < lastLink; ++i) {
        common::Mat4 linkTransform = dhTransform(links_[i], jointAngles[i]);
        result = result * linkTransform;  // 链式乘积
    }
    return result;
}

1.2 DH参数建模实例：两连杆机械臂

最简单的2D两连杆模型：

   θ2
    ↖
   ┌─── joint2
   │
 θ1│
  ↙
joint1 (原点)

// 创建两连杆DH链
DHChain chain;
chain.addLink({1.0, 0.0, 0.0, 0.0}, {-1.5, 1.5});  // link1: a=1.0, θ1可变
chain.addLink({1.0, 0.0, 0.0, 0.0}, {-1.5, 1.5});  // link2: a=1.0, θ2可变

std::vector<double> angles = {PI/2, PI/2};
Mat4 result = chain.forwardKinematics(angles);
Vec3 pos = result.translation();  // 得到末端位置

二、CCD循环坐标下降

2.1 几何原理

CCD的核心思想是从末端向根部依次调整每个关节，让末端逐步逼近目标：

flowchart TB S(["初始关节角 θ"]) --> A{"iter < maxIter<br/>且 误差 > ε?"} A -->|"否"| H(["返回最优 θ"]) A -->|"是|继续迭代"| B["遍历关节 i = n-1 → 0"] B -->|"i ≥ 0"| C["计算当前末端位置 FK"] C --> D["计算末端指向和目标指向"] D --> E{"已对齐?"} E -->|"是|跳过"| F{"i--"} E -->|"否|求夹角"| G["θ[i] += angle"] G --> F F -->|"是|i--"| B F -->|"否|i=-1"| A style A fill:#FFF8E1,stroke:#F57C00 style B fill:#E3F2FD,stroke:#1976D2 style H fill:#E8F5E9,stroke:#388E3C

关键：如何计算关节转角？

投影法——把末端和目标都投影到当前关节的旋转平面，计算夹角：

// IKSolver.cpp · CCD核心计算
// 计算当前关节位置
common::Mat4 baseTransform = chain.forwardKinematics(angles, i);
common::Vec3 jointPos = baseTransform.translation();

// 计算末端和目标相对关节的向量
common::Vec3 toEnd = (endPos - jointPos).normalized();    // 指向当前末端
common::Vec3 toTarget = (targetPosition - jointPos).normalized(); // 指向目标

// 两向量夹角的余弦值
double cosAngle = toEnd.dot(toTarget);

// 防止acos越界
if (cosAngle > 0.9999) continue;
if (cosAngle < -0.9999) cosAngle = -0.9999;

// arccos得到旋转角度
double angle = std::acos(cosAngle);

// DH链中关节绕自身Z轴，无需额外方向判断
angles[i] += angle;  // 全步长

2.2 完整CCD实现

// IKSolver.cpp · CCD循环坐标下降求解器
std::vector<double> IKSolver::solveCCD(
    const DHChain& chain,
    const common::Vec3& targetPosition,
    const std::vector<double>& initialGuess,
    int endLink) const
{
    size_t n = chain.dof();
    int lastLink = (endLink < 0) ? static_cast<int>(n) : endLink;
    if (lastLink <= 0) return initialGuess;

    std::vector<double> angles = initialGuess;
    if (angles.size() < n) angles.resize(n, 0.0);

    for (int iter = 0; iter < maxIterations_; ++iter) {
        for (int i = lastLink - 1; i >= 0; --i) {
            // 1. 计算当前末端位置
            common::Mat4 baseTransform = chain.forwardKinematics(angles, i);
            common::Mat4 fullTransform = chain.forwardKinematics(angles, lastLink);

            common::Vec3 endPos = fullTransform.translation();
            common::Vec3 jointPos = baseTransform.translation();

            // 2. 投影到关节旋转平面
            common::Vec3 toEnd = (endPos - jointPos).normalized();
            common::Vec3 toTarget = (targetPosition - jointPos).normalized();

            double cosAngle = toEnd.dot(toTarget);
            if (cosAngle > 0.9999) continue;
            if (cosAngle < -0.9999) cosAngle = -0.9999;

            // 3. 计算旋转角并应用
            double angle = std::acos(cosAngle);
            angles[i] += angle;

            // 4. 关节限位钳制
            angles[i] = std::max(chain.jointMin(i), std::min(chain.jointMax(i), angles[i]));

            // 5. 检查收敛
            fullTransform = chain.forwardKinematics(angles, lastLink);
            double error = (fullTransform.translation() - targetPosition).length();
            if (error < tolerance_) {
                return angles;
            }
        }
    }
    return angles;
}

2.3 CCD特性分析

特性 说明
收敛速度 快，通常5-10轮迭代收敛
收敛质量 可能陷入局部最优，非全局最优
关节限位 自然处理（钳制后仍继续调整其他关节）
奇异位姿 关节接近180°时，旋转轴退化

三、Jacobian阻尼最小二乘

3.1 数学原理

Jacobian IK的核心思想是**"关节怎么转，末端就怎么动"**——把关节到末端的关系写成一个表（雅可比矩阵），表里的每一列告诉你"单独转这个关节，末端会往哪个方向移动"。有了这张表，就可以反查：要末端往目标方向移动，每个关节该转多少。

来自 linuxros.cn · linuxROS

但这张表在机器人完全伸直（奇异位姿）时会失效，就像门轴和门把手在一条线上时你推不动门。加入阻尼项（给表加一个很小的缓冲），可以有效抑制这种数值爆炸。

工程上用梯度下降+线搜索实现——梯度指出"最陡的下坡方向"，线搜索决定"这一步迈多大"：
工程实现中用梯度下降+线搜索代替矩阵求逆：

// 梯度 = J^T · Δp
for (size_t i = 0; i < n; ++i) {
    gradient[i] = jacobianCols[i].dot(posError);
}

// 步长 = 1 / (max(dot(J,J)) + λ²)
double alpha = 1.0 / (jtjMax + lambda2 + 1e-12);

3.2 差分法计算雅可比

不需要手写雅可比解析式，用数值差分自动计算：

// IKSolver.cpp · 差分法计算雅可比列
for (size_t i = 0; i < n; ++i) {
    std::vector<double> anglesPerturbed = angles;
    anglesPerturbed[i] += epsilon;  // 对第i个关节加扰动
    common::Mat4 perturbedPose = chain.forwardKinematics(anglesPerturbed, lastLink);

    // 位置变化量 / 扰动量 = 雅可比第i列
    jacobianCols[i] = (perturbedPose.translation() - currentPose.translation()) / epsilon;
}

3.3 带线搜索的梯度下降

直接用梯度下降可能过冲导致误差增大，用线搜索找最优步长：

// IKSolver.cpp · 线搜索优化步长
double alpha = 1.0 / (jtjMax + lambda2 + 1e-12);  // 初始步长
double prevError = errorLen;
std::vector<double> bestAngles = angles;
double bestError = errorLen;

// 最多尝试8次倍增/减半
for (int ls = 0; ls < 8; ++ls) {
    std::vector<double> trialAngles = angles;
    for (size_t i = 0; i < n; ++i) {
        trialAngles[i] += alpha * gradient[i];
        trialAngles[i] = std::max(chain.jointMin(i), std::min(chain.jointMax(i), trialAngles[i]));
    }
    common::Mat4 trialPose = chain.forwardKinematics(trialAngles, lastLink);
    double trialError = (trialPose.translation() - targetPose.translation()).length();

    if (trialError < bestError) {
        bestAngles = trialAngles;
        bestError = trialError;
        alpha *= 2.0;  // 上次有效，倍增加速
    } else {
        alpha *= 0.5;  // 过冲了，减半
    }
}
angles = bestAngles;  // 用误差最小的步长

3.4 完整Jacobian实现

// IKSolver.cpp · Jacobian阻尼最小二乘求解器
std::vector<double> IKSolver::solveJacobian(
    const DHChain& chain,
    const common::Mat4& targetPose,
    const std::vector<double>& initialGuess,
    int endLink) const
{
    size_t n = chain.dof();
    int lastLink = (endLink < 0) ? static_cast<int>(n) : endLink;
    std::vector<double> angles = initialGuess;
    if (angles.size() < n) angles.resize(n, 0.0);

    double epsilon = 1e-6;
    double lambda2 = damping_ * damping_;

    for (int iter = 0; iter < maxIterations_; ++iter) {
        common::Mat4 currentPose = chain.forwardKinematics(angles, lastLink);

        // 1. 计算位置误差
        common::Vec3 posError = targetPose.translation() - currentPose.translation();
        double errorLen = posError.length();
        if (errorLen < tolerance_) {
            return angles;  // 收敛
        }

        // 2. 数值法计算雅可比矩阵
        std::vector<common::Vec3> jacobianCols(n);
        for (size_t i = 0; i < n; ++i) {
            std::vector<double> anglesPerturbed = angles;
            anglesPerturbed[i] += epsilon;
            common::Mat4 perturbedPose = chain.forwardKinematics(anglesPerturbed, lastLink);
            jacobianCols[i] = (perturbedPose.translation() - currentPose.translation()) / epsilon;
        }

        // 3. 计算梯度向量
        std::vector<double> gradient(n, 0.0);
        double jtjMax = 0.0;
        for (size_t i = 0; i < n; ++i) {
            gradient[i] = jacobianCols[i].dot(posError);
            double jtj = jacobianCols[i].dot(jacobianCols[i]);
            if (jtj > jtjMax) jtjMax = jtj;
        }

        // 4. 线搜索梯度下降
        double alpha = 1.0 / (jtjMax + lambda2 + 1e-12);
        std::vector<double> bestAngles = angles;
        double bestError = errorLen;

        for (int ls = 0; ls < 8; ++ls) {
            std::vector<double> trialAngles = angles;
            for (size_t i = 0; i < n; ++i) {
                trialAngles[i] += alpha * gradient[i];
                trialAngles[i] = std::max(chain.jointMin(i), std::min(chain.jointMax(i), trialAngles[i]));
            }
            common::Mat4 trialPose = chain.forwardKinematics(trialAngles, lastLink);
            double trialError = (trialPose.translation() - targetPose.translation()).length();

            if (trialError < bestError) {
                bestAngles = trialAngles;
                bestError = trialError;
                alpha *= 2.0;
            } else {
                alpha *= 0.5;
            }
        }
        angles = bestAngles;
    }
    return angles;
}

3.5 Jacobian特性分析

特性 说明
收敛速度 慢，通常20-100轮迭代
收敛质量 稳定逼近局部最优
计算量 O(n²×iter)，需计算n列雅可比
奇异鲁棒 阻尼项λ²抑制奇异震荡
关节限位 需要钳制（影响梯度方向）

四、算法对比与选型

flowchart TB A["输入目标位置"] --> B{"有解析解?"} B -->|"是|6轴等标准结构"| C["解析IK"] B -->|"否|通用需求"| D{"实时性要求?"} D -->|"毫秒级"| E["CCD"] D -->|"一般"| F["Jacobian DLS"] style C fill:#E8F5E9,stroke:#388E3C style E fill:#FFF8E1,stroke:#F57C00 style F fill:#F3E5F5,stroke:#7B1FA2
维度 CCD Jacobian DLS 解析IK
收敛速度 快（5-10轮） 慢（20-100轮） 微秒级
收敛质量 局部最优 稳定逼近 全局最优（若有解）
计算量 O(n×iter) O(n²×iter) O(1)
通用性 任意结构 任意结构 特定结构
奇异处理 一般 阻尼项保护 需特殊处理
关节限位 自然处理 需钳制 需后处理

工程建议：

  • 实时控制（毫秒级延迟要求）：选CCD
  • 离线规划（稳定性要求高）：选Jacobian DLS
  • 已知结构的6轴机械臂：选解析IK + 数值法兜底

五、统一调用接口

两种算法封装为统一接口，便于切换和对比：

// IKSolver.h · 统一solve接口
std::vector<double> solve(
    common::IKSolverType type,  // CCD 或 JACOBIAN
    const DHChain& chain,
    const common::Mat4& targetPose,
    const std::vector<double>& initialGuess,
    int endLink = -1) const;

std::vector<double> solveCCD(...) const;
std::vector<double> solveJacobian(...) const;

// 使用示例
IKSolver solver;
solver.setMaxIterations(100);
solver.setTolerance(1e-6);
solver.setDamping(0.01);  // Jacobian专用

// 选CCD或Jacobian
std::vector<double> result = solver.solve(
    common::IKSolverType::CCD,
    chain, targetPose, initialGuess);

六、测试验证

// test_kinematics.cpp · CCD收敛性测试
TEST(ik_ccd_simple_reach) {
    DHChain chain;
    chain.addLink({1.0, 0.0, 0.0, 0.0}, {-1.5, 1.5});
    chain.addLink({1.0, 0.0, 0.0, 0.0}, {-1.5, 1.5});

    std::vector<double> initial = {0.0, 0.0};
    Vec3 target(1.5, 0.0, 0.0);  // 目标在伸展方向

    IKSolver solver;
    solver.setMaxIterations(100);
    solver.setTolerance(1e-4);

    std::vector<double> result = solver.solveCCD(chain, target, initial);
    Mat4 fk = chain.forwardKinematics(result);
    Vec3 pos = fk.translation();

    ASSERT_NEAR(pos.x, 1.5, 1e-3);
    ASSERT_NEAR(pos.y, 0.0, 1e-3);
    return true;
}

// test_kinematics.cpp · Jacobian收敛性测试
TEST(ik_jacobian_simple_reach) {
    DHChain chain;
    chain.addLink({1.0, 0.0, 0.0, 0.0}, {-1.5, 1.5});
    chain.addLink({1.0, 0.0, 0.0, 0.0}, {-1.5, 1.5});

    std::vector<double> initial = {0.0, 0.0};
    Mat4 target;
    target(0,3) = 1.5; target(1,3) = 0.0; target(2,3) = 0.0;

    IKSolver solver;
    solver.setMaxIterations(100);
    solver.setDamping(0.01);

    std::vector<double> result = solver.solveJacobian(chain, target, initial);
    Mat4 fk = chain.forwardKinematics(result);
    double error = (fk.translation() - target.translation()).length();

    ASSERT_TRUE(error < 0.1);
    return true;
}

测试结果：

========================================
[Test Summary]
Total: 34 | Passed: 34 | Failed: 0
========================================

七、总结

数值IK的核心是迭代优化——CCD用几何投影快速调整关节，Jacobian用梯度下降稳定收敛。两种方法各有优劣：

  • CCD：速度快，适合实时控制，但可能陷入局部最优
  • Jacobian DLS：收敛稳定，带线搜索和阻尼保护，适合离线规划

实际工程中，通常组合使用：先用CCD快速逼近，再用Jacobian精细调整，或在解析IK失败时做兜底。

下期预告：机器人动作的数学基础——零依赖C++自研Vec3/Quaternion/Mat4数学库，聊聊为什么不用Eigen。

代码仓库：D:\code\test\motion_retargeting
源码路径：src/kinematics/IKSolver.cpp, src/kinematics/DHChain.cpp

版权声明

作者linuxROS
原文https://www.linuxros.cn/blog/T1_数值IK求解
协议本作品采用 CC BY-NC-SA 4.0 许可协议：署名-非商业性使用-相同方式共享
关注欢迎关注微信公众号 linuxROS，获取更多机器人 / 嵌入式 / Linux 干货
相关文章
加载中...
返回首页