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;
}
相关推荐
Zwawa20 小时前
陀螺仪数据为什么看起来总在动:零偏、噪声和零漂
c语言·单片机·嵌入式
soso19681 天前
低功耗设计与BMS开发指南
c语言·开发语言·汇编
老师我太想进步了20261 天前
【数据结构-8】栈的实现
c语言·数据结构·算法
LCG元1 天前
串口乱码,先算波特率误差再动代码
c语言·嵌入式
another heaven2 天前
【C/C++ 第一次跨越(上):用 C++11/14 停止制造新的旧代码】
c语言·c++·制造
another heaven3 天前
【C/C++ Ranges 是零开销抽象吗?拆开管道、惰性求值和编译期膨胀】
c语言·开发语言·c++
Code_Solitude3 天前
C语言动态内存 + 柔性数组 + 数组名退化
c语言·算法·柔性数组
Zwawa3 天前
让屏幕动起来:OLED动画效果,丝滑光标、帧动画和一只贪吃蛇
c语言·单片机·嵌入式
꧁细听勿语情꧂3 天前
直接选择排序,堆排序,冒泡排序,递归快速排序、hoare版本、挖坑法、lomuto前后指针,非递归快速排序
c语言·开发语言·数据结构·算法
§ꦿCFོ༉4 天前
嵌入式八股文—C语言入门学习(四)
c语言·开发语言·学习