达妙机械臂

can通信

发送邮箱

两路can通信：接入4个4310与3个4340p电机。

每路can的发送端：有3个发送邮箱，底层是由4个寄存器构成一个邮箱。分别是:TIR,TDTR,TDLR,TDHR。

普通的寄存器是死的，但是邮箱非常灵活。比如：

• 填装阶段（CPU 干活） ：CPU 像填表一样，把 ID 写入 TIR，把长度写入 TDTR，把数据写入 TDLR 和 TDHR。最后，CPU 在 TIR 寄存器的最低位写一个 1（这个位叫 TXRQ：Transmit Request 发送请求）。

• 投递阶段（硬件干活） ：CPU 写完这个 1 之后，就可以立刻走人。此时，"邮箱"背后的硬件逻辑电路被激活，它开始自主执行一系列极其复杂的操作。（在这操作过程中cpu的时间非常短，时间花销主要是在can线路上占用）

can通信时间

一个标准 CAN 帧的结构如下：

• 帧头结构：帧起始(1) + ID(11) + 控制位(7) = 19 bits

• 数据段：8 字节 = 64 bits

• 帧尾结构：CRC(15) + 各种界定符和结束符(13) = 28 bits

• 基础长度 ：19 + 64 + 28 = 111 bits

为了保持时钟同步，CAN 协议规定只要有连续 5 个相同的电平，就必须强行塞入一个反向的"填充位"。对于 8 字节的数据帧，通常会触发 10~15 个填充位。 所以，一帧数据的实际总长度大约在 120 ~ 125 bits 左右。我们假设在130us。

stm32与电机通信是一发一答的机制，电机内部处理大概按20us算。总时间是130+20+130=280us。

单路can，4个电机通信时间280*4=1120us。代码中设定500hz的频率，也就是2ms，现在占用不到60%。

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

前置知识：静态矩阵

T1矩阵

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矩阵

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，z轴+43mm。

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

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

新y是旧z，新z是旧的-y

T3矩阵

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。

新x是旧-x，新z是旧-z

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

T4矩阵

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度。新x是旧-z，新y是旧y，新z是旧x

坐标系4是按z轴旋转

T5矩阵

static Matrix4x4 get_urdf_T5(float t5) {
    Matrix4x4 T = {0}; 
    float c = cosf(t5), s = sinf(t5);
    
    // 严格基于 SolidWorks 图纸物理对齐推导：
    // 新 X(5) 指向 老 Z(4)
    // 新 Y(5) 指向 老 X(4)
    // 新 Z(5) 指向 老 Y(4)
    T.m[0][0] = s;  T.m[0][1] = c;  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] = 0;
    T.m[2][0] = c;  T.m[2][1] = -s; T.m[2][2] = 0; T.m[2][3] = L5_Z_OFFSET; // 157
    T.m[3][3] = 1;
    
    return T;
}

从坐标系4z轴正方向走157mm达到坐标系5

新x是旧z，新z是旧y，新y是旧x

T6矩阵

static Matrix4x4 get_urdf_T6(float t6) {
    Matrix4x4 T = {0}; float c = cosf(t6), s = sinf(t6);
    T.m[0][0] = 0;  T.m[0][1] = 0;  T.m[0][2] = -1; T.m[0][3] = L6_X_OFFSET;//91
    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] = 0;
    T.m[3][3] = 1;
    return T;
}

新x指向老z，新y指向老-y，新z指向老x

沿着坐标系5的x方向+91mm

FK正运动学

已知每个关节旋转角度，求出机械臂末端在空间中的xyz坐标

齐次变换矩阵：包含旋转与平移。

T1描述底座到关节1的位置关系，T2描述关节2到关节1的位置关系，以此类推，把矩阵相乘，得到末端点到原点的位置关系。

float t1 = r1 + J1_HW_OFFSET + g_arm_cali.j1_bias;
// ... (t2 到 t6 同理)

r1是电机实际旋转角度

J_HW_OFFSET：电机0位置，与urdf模型的0位置，有偏差，用这个变量进行补充

g_arm_cali.j_bias：机械臂安装时会产生误差，用该变量进行补充

Matrix4x4 T1 = get_urdf_T1(t1);
// ...
Matrix4x4 T5 = get_urdf_T5(t5);
Matrix4x4 T6 = get_urdf_T6(t6);

把完整的角度传入矩阵中

Matrix4x4 T12 = matrix_multiply(T1, T2);
Matrix4x4 T123 = matrix_multiply(T12, T3);
Matrix4x4 T1234 = matrix_multiply(T123, T4);
Matrix4x4 T12345 = matrix_multiply(T1234, T5);
Matrix4x4 T_end = matrix_multiply(T12345, T6);

进行矩阵连乘。用stm32f4的fpu运算浮点单元，这段代码大概只需要几微妙完成。

IK逆运动学

已知目标点与手腕姿态，求出机械臂每个电机应该旋转到多少度。

对于空间中的同一个目标点，6 轴机械臂通常有 8 组不同的解（比如：左手/右手构型、手肘向上/向下、手腕翻转/不翻转）。

**无解点：**比如手臂全长1m，需要伸到2m的地方，工作空间是无解的

**奇异点：**当机械臂的几个轴在空间中连成一条直线（共线）时，数学矩阵的行列式会变成 0，导致方程除以 0 而崩溃。

工业对ik的解算：数值解法，解析法

**数值法：**原理靠猜，先随便给个角度，算出现状和目标的误差，然后用微积分（雅可比矩阵、梯度下降法、牛顿-拉夫逊迭代法）一步一步逼近目标。

**解析法：**利用精妙的几何关系和三角函数，机械臂需要满足：Pieper 准则

本方法采用解析法进行解算。

Pieper 准则

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

现在代码按两个步骤完成，前三轴只管位置，负责把手腕中心点送到指定坐标

后三轴：只管姿态，手腕中心到了之后，由于这三个轴交于一点，它们怎么转都不会改变手腕中心的位置了。它们只负责在原地"扭手腕"，把最终的姿态调整到你的期望值。

TCP矩阵含义

在机器人学中，我们通常把机械臂最末端的执行器（法兰盘或夹爪）称为 TCP (Tool Center Point) 。 R_tcp 这个矩阵，本质上就是把TCP的 X、Y、Z 三根坐标轴，在底座（世界）坐标系下的方向，生硬地拼在了一起。

对tcp矩阵进行讲解：

假设现在有一个玩具飞机：

• 棍子 1（X 轴）： 插在飞机的机顶，直直指向上方。

• 棍子 2（Y 轴）： 插在飞机的左机翼，直直指向左边。

• 棍子 3（Z 轴）： 插在飞机的机头，直直指向前方。

假设你现在只看机头（第三列）的棍子。电脑是一个瞎子，它看不见你的飞机，你必须用 3 个数字来回答电脑的 3 个问题：

1. 第一个数字 (G)： 你的机头，有没有顺着房间（原点）的正前方 (X) 指？指了多少？

2. 第二个数字 (H)： 你的机头，有没有顺着房间的正左方 (Y) 指？指了多少？

3. 第三个数字 (I)： 你的机头，有没有顺着房间的正上方 (Z) 指？指了多少？

如果这根棍子完全指着某个方向，数值就是 1；如果完全背对着指，就是 -1；如果跟这个方向垂直（一点都没蹭上），就是 0。

场景 A：飞机平稳停在地上，机头正对前方

• 看机头 (第三列)： 机头笔直朝前 (房间 X)。所以第三列是 [1, 0, 0]。

• 看左翼 (第二列)： 左机翼笔直朝左 (房间 Y)。所以第二列是 [0, 1, 0]。

• 看机顶 (第一列)： 机顶笔直朝上 (房间 Z)。所以第一列是 [0, 0, 1]。 把这三列拼起来，就是初始的单位矩阵！

1. 第一列：坐标系 6 的 X 轴

• 物理意义 ：代表夹爪的法向

• 相当于我们第一课"玩具飞机"里插在机顶的那根棍子。夹爪的正上方正指着房间的哪个方向。

2. 第二列：坐标系 6 的 Y 轴

• 物理意义 ：代表夹爪的滑动/指向方向。

• 相当于玩具飞机的左机翼。夹爪的侧面正指着房间的哪个方向。

3. 第三列：坐标系 6 的 Z 轴

• 物理意义 ：这是整个矩阵里最最重要的一列！它代表夹爪的接近方向 (Approach)。

• 相当于玩具飞机的机头。在实际抓取时，这根轴就是工具的延长线（比如焊枪喷火的方向、螺丝刀插进去的方向）。我们在前面解算手腕姿态时，拼命盯着矩阵第三列找角度，就是为了先把"机头"对准！

TCP矩阵由来

用户输入ROLL Pitch Yaw三个角度,转换成tcp矩阵

    if (argc < 8) {
        rt_kprintf("Usage: arm_move6 <x> <y> <z> <roll> <pitch> <yaw> <time>\n");
        return;
    }

    float tx = atof(argv[1]); float ty = atof(argv[2]); float tz = atof(argv[3]);
    float r  = atof(argv[4]); float p  = atof(argv[5]); float y  = atof(argv[6]);
    float move_time = atof(argv[7]);

    float joints[6] = {0};

    Matrix3x3 R_tcp;
    float cr = cosf(r), sr = sinf(r);
    float cp = cosf(p), sp = sinf(p);
    float cy = cosf(y), sy = sinf(y);

    R_tcp.m[0][0] = cy*cp; R_tcp.m[0][1] = cy*sp*sr - sy*cr; R_tcp.m[0][2] = cy*sp*cr + sy*sr;
    R_tcp.m[1][0] = sy*cp; R_tcp.m[1][1] = sy*sp*sr + cy*cr; R_tcp.m[1][2] = sy*sp*cr - cy*sr;
    R_tcp.m[2][0] = -sp;   R_tcp.m[2][1] = cp*sr;            R_tcp.m[2][2] = cp*cr;

1：倒推腕部中心点

已知机头尖端要去的目标点，已知飞机当前的姿态，求腕部（驾驶舱）应该停在哪里？

    float z_vec_x = R_tcp.m[0][2], z_vec_y = R_tcp.m[1][2], z_vec_z = R_tcp.m[2][2];

    // 1. 求腕部中心 (Wrist Center)
    float P_wc_x = target_x - L6_X_OFFSET * z_vec_x;
    float P_wc_y = target_y - L6_X_OFFSET * z_vec_y;
    float P_wc_z = target_z - L6_X_OFFSET * z_vec_z;

该矩阵的第三列：可以得到机头基于原点朝向什么地方

d6就是关节5到关节6的距离：91mm(按实际值)

你先走到房间里的目标点 target（机头尖端所在位置）。 然后，顺着机头指着的方向，往反方向（往后退）走 91mm (d6)。 你退到的位置，必然就是腕部中心 (P_wc) 的绝对坐标！

    float Wy = -(L3_Z_OFFSET - L4_Z_OFFSET) + L2_Y_OFFSET;//为0
    float R0 = sqrtf(P_wc_x * P_wc_x + P_wc_y * P_wc_y);//算出目标点距离坐标系0的直线距离

    if (R0 < fabsf(Wy)) return -1; // 物理上够不到

大部分工业机械臂，腕部中心会跟底座原点产生一个横向水平偏置，本机械臂没有，wy为0

sqrtf：求平方根，目标点距离底座正中心的直线水平距离。

2:求解底座旋转角j1

float alpha1 = atan2f(P_wc_y, P_wc_x);
float beta1 = asinf(-Wy / R0);

P_wc_x 和 P_wc_y 是腕部中心在底座坐标系下的 X、Y 坐标，atan2f可以非常智能算出角alpha1，并知道在第几象限。

beta1：机械臂横向偏执的补偿角，此机械臂为0

3：寻找最优解做准备

    float best_joints[6] = {0};
    float min_diff = FLT_MAX;
    int found_valid_solution = 0;

best_joints6：底座正转/反转、手肘朝上/朝下、手腕翻转/不翻转，8种不同的姿态，把动作幅度最小，最安全的角度，存入该数组中

min_diff：先设置成最大值3.4*10的38次方，接下来计算每组姿态，计算它跟当前姿态的差值，只要比这个值小，就成为冠军，把插值刷新给min_diff

found_valid_solution：虽然数学上能算出 8 组解，但这 8 组解在现实中可能会撞到桌子，或者超出你设定的限位边界。每算出一组解，系统都会先把它送进 arm_check_joint_limits 函数进行检查。只有既满足数学公式，又没有越过物理限位 的解，才会把这个标志位置为 1

4：底座2种姿态

    float theta1_sol[2] = { alpha1 + beta1, alpha1 + PI - beta1 };

达到目标点，底座j1有两种选择，正向与背向

alpha1 + beta1：机械臂"正脸"朝着目标，加上偏置补偿角（beta1），直接伸长手臂去抓取。

alpha1 + PI - beta1：机械臂转过身去（加上 PI，也就是转了 180 度），用"后脑勺"对准目标，然后大臂小臂向后仰。

    for (int sol_base = 0; sol_base < 2; sol_base++) {
        float theta1 = theta1_sol[sol_base];
        while(theta1 > PI) theta1 -= 2*PI; //角度归一化在-180到180之间
        while(theta1 < -PI) theta1 += 2*PI;

机械臂正脸朝向目标，开始第一种解，进行角度归一化。

        float x1 = P_wc_x * cosf(theta1) + P_wc_y * sinf(theta1);
        float z1_prime = P_wc_z - L1_Z_OFFSET - L2_Z_OFFSET;

想象你站在一个可以旋转的底盘上（底座 J1）。目标点在全局坐标里是 (P_wc_x, P_wc_y)。现在你已经转了 theta1 角度，正面对着目标了。你只关心"目标离我正前方有多远"。

相当于：坐标系转换，底座j1已经发生旋转。该机械臂的R0与此值距离是一样的。但是x1可以反应出是否有负值。

z1_prime：目标点的绝对高度是 P_wc_z。我们用它减去垫高的高度，就算出了j5相对于 J2 关节中心的纯垂直高度差。此时j2作为原点。

        float dx = L5_Z_OFFSET + L4_X_OFFSET;
        float dy = L4_Y_OFFSET;
        float K = x1 * x1 + z1_prime * z1_prime;

j3到j5的距离 x：179 y:83.5

k：j2到腕部中心的直线距离平方

        float A = 2.0f * L3_X_OFFSET * dx;
        float B = 2.0f * L3_X_OFFSET * dy;

以j2作为坐标原点，推导j5的坐标

大臂长度为：L3x，小臂水平偏置dx，小臂上下偏置dy。

j3发生旋转后：