6轴达妙机械臂

Pieper 准则指出：一个 6 轴串联机械臂，只要满足相邻三根相邻的旋转轴线交于一点（通常是最后三根），或者三根相邻的轴线相互平行，那么它就一定存在解析形式的逆运动学解。

解析解 (Analytical Method)：指的是你可以写出一套明确的数学公式（通过加减乘除、三角函数），把目标坐标代进去，瞬间算出 6 个角度。这种算法的时间复杂度是 $O(1)$ 的，运算极快，非常适合 STM32 等嵌入式芯片，能轻松跑到 500Hz 甚至 1000Hz 的控制频率。
数值解 (Numerical Method)：如果轴线不交于一点，上述简单的公式就不存在了。你必须使用雅可比伪逆、牛顿迭代法或非线性优化算法。这些算法通过不断猜测、试错、逼近来求解。运算量巨大，耗时长，而且在"奇异点"附近容易不收敛或产生突变，对底层芯片的算力和实时性要求极高。

6轴机械臂ik与fk代码详细解析

T1矩阵

cpp 复制代码

static Matrix4x4 get_urdf_T1(float t1) {
    Matrix4x4 T = {0}; float c = cosf(t1), s = sinf(t1);
    T.m[0][0] = c; T.m[0][1] = -s; T.m[0][2] = 0; T.m[0][3] = 0;
    T.m[1][0] = s; T.m[1][1] = c;  T.m[1][2] = 0; T.m[1][3] = 0;
    T.m[2][0] = 0; T.m[2][1] = 0;  T.m[2][2] = 1; T.m[2][3] = L1_Z_OFFSET;
    T.m[3][3] = 1;
    return T;
}

坐标系1相对坐标系0，沿着坐标系0的z轴向上偏移66.5mm

旋转坐标矩阵就是标准的绕z轴旋转

T2矩阵

cpp 复制代码

static Matrix4x4 get_urdf_T2(float t2) {
    Matrix4x4 T = {0}; float c = cosf(t2), s = sinf(t2);
    T.m[0][0] = c; T.m[0][1] = -s; T.m[0][2] = 0;  T.m[0][3] = 0;
    T.m[1][0] = 0; T.m[1][1] = 0;  T.m[1][2] = -1; T.m[1][3] = L2_Y_OFFSET;//-28.5
    T.m[2][0] = s; T.m[2][1] = c;  T.m[2][2] = 0;  T.m[2][3] = L2_Z_OFFSET;//43
    T.m[3][3] = 1;
    return T;
}

坐标系2相对坐标系1的关系：沿坐标系1的y轴-28.5mm=L2_Y_OFFSET，z轴+43mm=L2_Z_OFFSET。

坐标系2电机2：绕z轴旋转（全部电机都设置绕z轴旋转）只需要判断跟上一个坐标系的关系

他跟坐标系1的关系是：把坐标系1绕x轴旋转90度，得到坐标系2

T3矩阵

cpp 复制代码

static Matrix4x4 get_urdf_T3(float t3) {
    Matrix4x4 T = {0};
    // ⚠️ 注意：URDF 中 axis 为 "0 0 -1"，意味着该关节真实的物理旋转方向与右手法则相反
    float c = cosf(-t3), s = sinf(-t3);
    T.m[0][0] = -c; T.m[0][1] = s; T.m[0][2] = 0;  T.m[0][3] = L3_X_OFFSET;//14
    T.m[1][0] = s;  T.m[1][1] = c; T.m[1][2] = 0;  T.m[1][3] = 0;
    T.m[2][0] = 0;  T.m[2][1] = 0; T.m[2][2] = -1; T.m[2][3] = L3_Z_OFFSET;//-56.5
    T.m[3][3] = 1;
    return T;
}

1：为什么是 -t3？ ：按照右手法则，大拇指顺着 Z 轴，四指弯曲的方向就是数学正方向。但你的 URDF 里写着 <axis xyz="0 0 -1" />，这意味着电机实际的物理正转，在数学坐标系（上一级坐标系）看来是反着转的 。所以必须加个负号 -t3 来纠正它。

2：以坐标系 2 为标准，沿着坐标系 2 的 X 轴往前走 140mm。Y 轴为0。沿着坐标系 2 的 Z 轴向下走 56.5mm。

3：根据坐标系2的y不变，把坐标系2绕x轴旋转180，再绕z轴旋转180。

4：坐标系3是绕z轴旋转

T4矩阵

cpp 复制代码

static Matrix4x4 get_urdf_T4(float t4) {
    Matrix4x4 T = {0}; float c = cosf(t4), s = sinf(t4);
    T.m[0][0] = 0;  T.m[0][1] = 0; T.m[0][2] = 1; T.m[0][3] = L4_X_OFFSET; //22
    T.m[1][0] = s;  T.m[1][1] = c; T.m[1][2] = 0; T.m[1][3] = L4_Y_OFFSET;//83.5
    T.m[2][0] = -c; T.m[2][1] = s; T.m[2][2] = 0; T.m[2][3] = L4_Z_OFFSET;//-28.25
    T.m[3][3] = 1;
    return T;
}

顺着 X 轴方向偏了 22mm。顺着 Y 轴方向偏了 83.5mm。顺着 Z 轴方向下沉了 28.25mm。

把坐标系3绕y轴旋转90度

然后坐标系4是按z轴旋转

T5矩阵

cpp 复制代码

static Matrix4x4 get_urdf_T5(float t5) {
    Matrix4x4 T = {0}; float c = cosf(t5), s = sinf(t5);
    T.m[0][0] = 0; T.m[0][1] = 0;  T.m[0][2] = 1; T.m[0][3] = 0;
    T.m[1][0] = c; T.m[1][1] = -s; T.m[1][2] = 0; T.m[1][3] = 0;
    T.m[2][0] = s; T.m[2][1] = c;  T.m[2][2] = 0; T.m[2][3] = L5_Z_OFFSET;//157
    T.m[3][3] = 1;
    return T;
}