达妙机械臂

can通信发送邮箱两路can通信接入4个4310与3个4340p电机。每路can的发送端有3个发送邮箱底层是由4个寄存器构成一个邮箱。分别是:TIR,TDTR,TDLR,TDHR。普通的寄存器是死的但是邮箱非常灵活。比如填装阶段CPU 干活CPU 像填表一样把 ID 写入 TIR把长度写入 TDTR把数据写入 TDLR 和 TDHR。最后CPU 在 TIR 寄存器的最低位写一个1这个位叫 TXRQTransmit 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算。总时间是13020130280us。单路can4个电机通信时间280*41120us。代码中设定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.5mmz轴43mm。坐标系2电机2绕z轴旋转全部电机都设置绕z轴旋转只需要判断跟上一个坐标系的关系他跟坐标系1的关系是把坐标系1绕x轴旋转90度得到坐标系2新y是旧z新z是旧的-yT3矩阵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 xyz0 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是旧-z4坐标系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是旧xT6矩阵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方向91mmFK正运动学已知每个关节旋转角度求出机械臂末端在空间中的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矩阵进行讲解假设现在有一个玩具飞机棍子 1X 轴插在飞机的机顶直直指向上方。棍子 2Y 轴插在飞机的左机翼直直指向左边。棍子 3Z 轴插在飞机的机头直直指向前方。假设你现在只看机头第三列的棍子。电脑是一个瞎子它看不见你的飞机你必须用 3 个数字来回答电脑的 3 个问题第一个数字 (G)你的机头有没有顺着房间原点的正前方 (X)指指了多少第二个数字 (H)你的机头有没有顺着房间的正左方 (Y)指指了多少第三个数字 (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为0sqrtf求平方根目标点距离底座正中心的直线水平距离。2:求解底座旋转角j1float alpha1 atan2f(P_wc_y, P_wc_x); float beta1 asinf(-Wy / R0);P_wc_x和P_wc_y是腕部中心在底座坐标系下的 X、Y 坐标atan2f可以非常智能算出角alpha1并知道在第几象限。beta1机械臂横向偏执的补偿角此机械臂为03寻找最优解做准备float best_joints[6] {0}; float min_diff FLT_MAX; int found_valid_solution 0;best_joints[6]底座正转/反转、手肘朝上/朝下、手腕翻转/不翻转8种不同的姿态把动作幅度最小最安全的角度存入该数组中min_diff先设置成最大值3.4*10的38次方接下来计算每组姿态计算它跟当前姿态的差值只要比这个值小就成为冠军把插值刷新给min_difffound_valid_solution虽然数学上能算出 8 组解但这 8 组解在现实中可能会撞到桌子或者超出你设定的限位边界。每算出一组解系统都会先把它送进arm_check_joint_limits函数进行检查。只有既满足数学公式又没有越过物理限位的解才会把这个标志位置为14底座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。我们用它减去垫高的高度就算出了目标点相对于 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的距离 x179 y:83.5kj2到腕部中心的直线距离平方