六自由度机械臂拖动示教：基于STM32F103的轨迹记录与回放系统

把一个六自由度机械臂从散件组装成能自动跑动作第一段是结构第二段才是灵魂。很多人在“六自由度机械臂示教操作、轨迹记录与复现”这类标题面前会觉得高深其实拆开看它要完成的事并不复杂用手把机械臂拖到一个姿势单片机记住这一路上的每个关节角度点一下按钮机械臂再按同样的顺序把这些角度演一遍。真正上手做一次问题才会暴露出来有的机械臂根本拖不动有的角度信号跳来跳去有的记录时还能看回放时却歪得离谱。这些坑和芯片算力关系不大。用 stm32f103 做六轴示教芯片能力完全够用。这个阶段的难点不在这里。先说清楚我的判断这个项目本质不是“做一个能动的机械臂”也不是“训练一个会学习的机器人”而是搭建一套“带时间标签的六维关节角度记录与回放系统”。你记录的不是动作画面而是一组随时刻变化的关节角度序列。把这件事理解透了后面调参、排错、扩展功能才不会走弯路。1. 六轴示教先搞清楚你究竟在采集什么数据示教操作听起来很“工业”实际拆开就是三件事获取关节角度、保存关节角度、输出关节角度。一个六自由度机械臂一次采样要得到六个角度值。拖动的过程可以看成一条连续的曲线单片机要做的是在这条曲线上密集打点每个点都记录“这一刻是第几毫秒六个关节分别转到多少度”。回放则是反向过程按相同的时间顺序把这一串点重新喂给六个关节的执行器。1.1 示教内容不是视频而是“时间 六个角度”的数据流如果只把示教理解成“拍一段视频然后播放”很容易误以为需要摄像头、图像识别、人工智能这些高级部件。真正低成本 DIY 机械臂的示教多数是直接记录关节角度。我们需要的数据结构核心是一个轨迹帧。每一帧大概包含时间戳这段轨迹从开始到现在的相对时间六个关节角度值分别对应底座、肩、肘、前臂、腕等运动关节。拖动时程序以固定间隔读取六个信号源记录结束后这段轨迹本质上就是一组数组或链表。回放时按照时间戳逆推每个时刻的期望角度再把角度发送给电机执行器。这里最容易被忽略的是“固定间隔”四个字。很多人用主循环直接采样但主循环里如果同时处理按键、串口打印、电机刷新循环时间就会忽快忽慢。表面上看每个角度都记录到了回放却会发现动作时快时慢。所以不要急着写业务逻辑先把时基固定住。1.2 为什么关节空间直接记录比逆运动学更适合这个阶段网上搜索六轴机械臂很容易被 dh 参数、轨迹规划、逆运动学 ros 开发这些词带走。它们不是没有用而是在这个“拖动示教 轨迹复现”项目里优先级可以往后放。原因是拖动手臂时你要记录的是物理关节角度而不是笛卡尔空间坐标。整个系统并不需要“根据末端位置反算六个关节角”这个过程。你从传感器读到的角度已经是机械臂实际所在的角度记住它就好。逆运动学、DH 参数真正派得上用场是在你把动作放到 gazebo、mujoco 这类仿真环境里验证时。如果你希望在电脑里先看一遍动作是否符合预期再决定要不要让实体机械臂执行那才需要建立运动学模型。第二个阶段可以先不做这个做出来以后再补也来得及。这个区分很重要它决定了你第一版固件的复杂度可以压得很低也更容易跑通。先把真实硬件上的“采集、存储、回放”做好再去碰仿真和算法才不容易被一堆抽象概念卡住。2. 组装前先确认三件事能不能拖动、能不能读角、供电稳不稳六自由度机械臂的机械结构装完不等于可以开始写示教代码。在实际动手写程序之前要先确认三个物理层面的问题。这三个问题如果没确认好程序写得再漂亮也白搭。2.1 电机扭矩和减速比决定“手感”不是所有电机都能手动示教常见的桌面级机械臂关节处用的是舵机或者直流减速电机。舵机内部有减速齿轮减速比越大输出扭矩越大但反向驱动也越困难。所谓反向驱动就是你不给电机通电、或者让电机处于松力状态时用手能不能轻松转动输出轴。示教操作要求在断电或释放力矩的状态下人手能把关节拖到预期位置同时角度传感器还能跟着转动并输出正确数值。如果使用的是普通大扭矩舵机锁住时手基本掰不动强行掰容易扫齿对示教来说体验很差。如果使用的是总线舵机或者带力矩输出控制的电机可以通过指令让电机处于无力矩模式这时候关节可以自由活动拖动示教就顺理成章。这里没必要一开始就算复杂的“机械臂电机扭矩计算”公式。真正影响体验的是两点减速比和输出扭矩是否适合这个尺寸的机械臂是否支持释放力矩或反向驱动。如果是套件材料包里已经配好的舵机先别拆只做一件事把舵机输出轴单独接上关节不给控制信号手动转动关节感受一下阻力大小。如果阻力大到不能平滑转动就需要考虑换装支持力矩释放的舵机或者在结构上加入离合器。2.2 角度采样前先把传感器的尺子标定好要记录角度硬件上必须有角度传感器。常见低成本方案有几种舵机内部电位器很多模拟舵机内部本身就有位置反馈电位器但未必引出信号所以不一定能直接读关节处单独安装电位器结构简单但安装位置要求与关节同轴否则机械臂动作时传感器读数会和真实关节角不一致磁编码器精度更高但成本也高且需要贴磁铁安装要求更高带角度反馈的总线舵机例如串行总线舵机可以直接通过串口读取当前角度使用体验好很多。选完传感器后不要急于做高精度标定。先做一个最简单的两点标定让关节转到机械限位的一端记录传感器读数再让关节转到另一端记录读数。为了保证标定可靠先用一个已知角度比如用直角尺或量角器确认限位处真的是 0 度和某个预设角度而不是凭感觉。注意很多 DIY 机械臂的角度偏差不是算法导致的而是电位器安装时没有和关节轴对齐。传感器主轴每偏一度最终显示的关节角度就会带一个固定误差而且误差随转动方向非线性变化。2.3 供电、控制线和电流上限是故障率最高的地方六自由度机械臂工作起来六个电机同时动作瞬间电流会比单个电机大不少。很多人用一块开发板直接给六个舵机供电结果就是动作时电压跌落、单片机复位、角度乱跳。至少要有两路电源思路单片机逻辑供电一路电机驱动供电一路。两路电源的地线要共地但功率电流不要走单片机板载稳压器。舵机或电机驱动部分的电源线尽量短线径要能承受峰值电流。还有一个很常见但容易忽略的问题stm32f103 的 GPIO 多为 3.3V 逻辑如果控制板上的舵机控制板是 5V 逻辑需要先确认电平兼容不要长期把 5V 信号直接灌进芯片引脚。很多套件在正常使用中没问题但在插拔线、上电瞬间却容易烧 IO往往就是电平转换和电源时序没处理好。3. 轨迹记录的最小实现把一次拖动变成可回放的数据物理框架和传感器确认之后程序上的最小实现其实很精简。你可以先不接任何按键只用串口触发也可以先做一个按钮。先跑通记录和回放的基本链路情绪价值会立刻上来然后再去完善功能。3.1 用结构体保存轨迹帧F103 的 RAM 不大如果只是保存几条几十秒的轨迹完全可以放在内存数组里。假设 10ms 采样一帧一秒钟 100 帧20 秒轨迹约 2000 帧。每帧保存时间戳、6 个角度值即使每个角度用 float 或 int16_t占用空间也不算夸张。示例结构体可以这样设计typedef struct { uint32_t timestamp; // 相对起始时间的毫秒数 int16_t joint[6]; // 6个关节角度单位为0.1度适合int16存储 } track_frame_t; #define MAX_FRAME_COUNT 3000 track_frame_t track[MAX_FRAME_COUNT]; uint16_t frame_count 0;角度用 int16_t 且按 0.1 度为单位是很多低成本方案里的常见做法。它比 float 更省 RAM也方便观察原始值。如果传感器输出的是模拟电压对应的 ADC 值可以先把 ADC 值换算成角度再在这个结构体里保存不要让后续逻辑依赖 ADC 量程。3.2 实时记录时的主循环采样、时间、保存边界最简单的记录流程是一个状态机常见状态包括IDLE等待开始记录RECORD循环读取角度写入轨迹数组STOP停止记录等待回放PLAY按时间回放轨迹。状态切换可以用按键也可以用串口命令。记录时最关键的是保证采样周期尽量稳定。建议使用定时器中断或定时器标志位每到一个固定时间片才触发一次采样而不是在主循环里用 delay 空等。在 stm32f103 上不需要上 FreeRTOS 也能完成这个任务。程序本质上是“一个时间片状态机 一个存储队列”。引入操作系统反而会让初版调试更难。示例逻辑大致是uint8_t sample_flag 0; // 假设定时器每10ms产生一次中断 void TIM_IRQHandler(void) { if (TIM_GetITStatus(TIMx, TIM_IT_Update) ! RESET) { TIM_ClearITPendingBit(TIMx, TIM_IT_Update); sample_flag 1; } } while (1) { if (state STATE_RECORD sample_flag) { sample_flag 0; if (frame_count MAX_FRAME_COUNT) { track[frame_count].timestamp elapsed_ms; // 累计毫秒 read_joint_angles(track[frame_count].joint[0]); frame_count; elapsed_ms SAMPLING_INTERVAL_MS; } else { state STATE_FULL; } } }这只是示意不是完整工程。要注意的地方是不要让角度读取函数内部有长时间阻塞比如等待串口数据、延时等待传感器稳定等。一旦阻塞采样间隔就会抖动。3.3 存储和输出先写串口调试再放到内部 Flash/SD第一版不一定需要掉电保存。先把轨迹通过串口发送到电脑格式简单一些例如第一列为毫秒时间戳后面六列为角度值用逗号或制表符分隔。这样不但能看到轨迹数据也能用绘图工具画出六个关节的角度曲线快速判断有没有明显跳变。如果要让机械臂断电后还能记住动作可以将轨迹写入单片机内部的 Flash 模拟 EEPROM 区域或者外接 SPI Flash、SD 卡。注意 F103 的 Flash 有擦写寿命不能每次执行都整片擦写。常见做法是把轨迹区规划成固定扇区以扇区为单位写入或者只存少量常用轨迹在内部大量数据放外部存储。这一步有两个必要性串口导出数据可以用于排查传感器异常固化存储才能让“示教一次、以后重复使用”成立。如果每回一次电都需要重新拖一遍那这套系统还算不上完整工具。4. 回放不是简单地把角度逐个写出去记录完成后回放才是真正暴露问题的环节。很多人发现明明记录时动作流畅回放时关节一顿一顿甚至最后位姿和记录时对不上。原因通常不是芯片速度不够而是回放过程少做了几个关键处理。4.1 为什么不插值会抖动如果记录间隔是 10ms直接每 10ms 把上一帧角度赋给舵机其实已经接近可行。但真正回放时会遇到一个问题解析到的时间戳不是严格等间隔或者舵机执行角度变化需要时间直接跳变会让关节承受很大的角速度突变听起来就是咔哒咔哒的顿挫。尤其在记录时如果中间有停顿、犹豫或者采样被其他任务打断相邻几帧的角度差会忽大忽小。解决方法是回放时做插值处理。对于这种教育级机械臂线性插值通常已经够用。也就是在两个已知轨迹帧之间按当前回放时间算出比例再逐个关节做角度插值float ratio (current_time - frame_a.timestamp) / (float)(frame_b.timestamp - frame_a.timestamp); for (int i 0; i 6; i) { target_angle[i] frame_a.joint[i] ratio * (frame_b.joint[i] - frame_a.joint[i]); }线性插值适合起点终点确定、速度要求不高的场景。如果动作比较复杂可以换成梯形速度规划让关节在启动和停止时更平缓。但不要一上来就上高阶曲线先用线性插值跑通再判断是否有必要平滑。4.2 从记录到回放起始判断、时间基准和增量匹配回放时另一个常见问题是“从错误的起点开始”。如果机械臂当前姿态和记录轨迹的第一帧不一致程序直接跳到第一帧机械臂会突然猛抽一下甚至撞到限位。规范的流程是开始回放前先让机械臂回到记录动作的起始姿态判断六个关节是否都接近记录首帧的角度满足一定误差范围后再开始回放回放开始后按照相对时间戳逐渐推进。回到起始姿态时也不建议六个关节同时高速转过去。应该先计算每个关节需要转多少度设定一个统一或按关节限制的速度依次或同步移动到起始点。如果轨迹数据里的时间戳是“从 0 开始的相对时间”回放时的时钟也要从 0 开始计时不要直接用当前系统时间。用定时器计数让回放速度可调。常见的“速度倍率”本质只是把相对时间乘一个系数例如 1.0 倍速用原始时间戳0.5 倍速用原始时间戳乘 2表示更慢。4.3 复现异常时的排查顺序回放如果出现问题不要先怀疑代码逻辑更不要立刻去改运动学算法。按下面顺序排查通常能快速定位先看原始轨迹数据把记录时的串口输出保存下来画出角度曲线。如果曲线本身有跳变问题在传感器或采样再看回放目标值在程序里把回放时每个时刻计算出的目标角度通过串口输出确认插值结果是否正确再查实际执行观察舵机或电机的实际角度是否到达目标角度。如果响应慢增大每一步的时间间隔或降低速度倍率最后查时间抖动如果记录时用了阻塞调用回放时可能出现时间错位导致动作越来越快或越来越慢检查 PWM 配置如果控制的是 PWM 舵机确认识别周期和占空比范围有没有覆盖完整角度范围。有些配置中用定时器输出 PWM 时占空比到不了 100%舵机末端会差几度回放结果自然不完全一致。注意不要只用“看起来差不多”来判断回放是否正确。给每个关节装不装编码器是一回事调试时至少要用串口打印某个关节的目标角度和实际反馈角度看两者误差趋势而不是只看动作是否丝滑。5. 拖动示教方式不止一种低成本方案先跑通再升级做这个项目时还会遇到一个更实际的问题机械臂到底应该怎么“拖”不同舵机方案会带来完全不同的操作体验。这直接影响你要不要换硬件。5.1 普通舵机、总线舵机、带电流反馈的执行器差在哪里普通 PWM 舵机比如常见的 SG90、MG996R特点是便宜、简单但通常没有角速度控制和力矩释放功能。要让这种舵机实现拖动示教常见套路是“断电后拖动舵机内部电位器仍随输出轴转动再重新上电读取角度”。听起来可行实际结构上往往做不到舵机在不通电时输出轴可以被手转动但内部的电位器是否随之转动需要看减速齿轮和电位器之间的连接方式很多舵机断电后转动不顺滑强行扭还有扫齿风险。总线舵机在这类项目里越来越受欢迎。它通过串口协议控制角度和读取当前角度有些还支持扭矩开关。打开扭矩释放模式后输出轴可以反向转动同时还能通过读取位置反馈得到角度。这让示教实现简单很多你只要周期性地向舵机发送读取角度命令并记录下来即可。缺点是成本高而且串行总线在六个舵机同时回传时通信周期要做合理规划不能让一帧数据等待太久。再往上走就是带电流检测、力矩控制的执行器例如一些开源机械臂项目里使用的直流无刷电机加编码器方案。它们可以做恒力拖动示教力反馈更细腻但难度也上了一个量级。对于刚接触这个方向的人来说不建议一开始就上这种方案。先把总线舵机或电位器方案跑通已经能覆盖大部分教学和毕设需求。5.2 如何把电位器或磁编码器正确“绑”在关节上如果你选择在关节处外接电位器结构安装是成败关键。先把一个基本原则记下来电位器的转轴必须和关节旋转轴尽量同轴转动范围不能超过电位器的电气行程。在三个地方最容易出问题电位器没有固定在机械结构上随着关节转动一起旋转线被拧断电位器转了超过 300 度超过碳膜行程输出不再单调角度方向接反导致关节往正方向转读数反而减小。推荐的做法是先在关节上临时安装电位器手动从限位的一端转到另一端用串口打印 ADC 数值确认数值是单调递增还是递减。确认之后再决定代码里要不要做反向变换。零点标定也需要重视。组装完成后机械臂的零点未必和电位器机械零位一致。不要试图通过硬装去凑零点应该在程序里保存一个“零点偏移值”。每次上电时先让机械臂回到一个已知的机械基准姿态比如都转到限位区然后读取各关节角度记为偏移后续采集时减去这个偏移即可。5.3 记录时要保护机械臂不要硬碰结构极限拖动示教时人很容易只关注“我想让末端走到哪里”而忽略六个关节是否快到了机械限位。尤其是六轴机械臂中间关节反转末端姿态可能越走越拧。如果示教过程中真撞到机械限位轻则舵机堵转发热重则齿轮扫齿或结构断裂。代码里至少要做三件事每个关节设置软限位角度目标超出允许范围时禁止执行记录时如果连续多帧角度值不变而电机仍在输出大电流可以认为关节被堵住自动停止记录回放前检查整段轨迹是否都在安全角度范围内不要等跑到一半才去判断。提醒很多材料包采用 3D 打印结构件机械强度有限。示教时不要对末端用力过猛扭矩冲击很容易让打印件在螺丝孔附近开裂。宁可多花点时间慢慢拖也不要试图模拟高动态动作。6. 从“能复现一次”到“可教学、可调试、可迭代”的小机器当你能拖一段动作、让机械臂自己复现出来这个项目已经完成了一半。继续往下走不是急着加更多花哨功能而是把这段经验沉淀成可维护的系统。6.1 这个方案适合谁不适合谁先划清边界。这个方案适合的人刚开始接触单片机控制和机械臂、想亲手搭一套完整系统的学生或爱好者要做课程设计、毕业设计需要一套可视化演示效果的场景想理解“采样、存储、回放”这一基本数据流的人。不适合的场景工业级的高精度装配任务它的重复精度通常达不到要求需要根据实时视觉或传感器反馈调整轨迹的场景对动作精度有严格指标的批量生产环节。不要期待一套 F103 拖动示教系统能直接变成工业机械臂。工业示教器的价值在于精确标定、安全逻辑、误差补偿和长期稳定性这些内容在这个项目里不会完整出现也不需要在这个阶段出现。6.2 下一步工程化建议动作命名、离线验证、参数化如果你想让这套系统更接近“可长期使用”的工具下面几个方向值得逐步补上给动作编号和命名。不要只保存一条轨迹把轨迹存成多个文件或扇区通过按键或串口选择一个动作执行。增加串口指令协议。例如REC开始记录、STOP停止、PLAY 1 0.8表示播放 1 号动作速度倍率 0.8。在电脑端把轨迹导出成 CSV然后绘制曲线。如果发现某个关节角度变化太陡就能提前发现不用反复在实体机械臂上试错。如果打算做运动学分析这时候再补 DH 参数。从结构装配图或 3D 模型里测量各个关节距离建立坐标系。有了 DH 参数才谈得上把轨迹放到 gazebo、mujoco 这类仿真环境里跑。掉电保存。把这套记录逻辑从内存数组改成内部 Flash 或外置存储动作才能真正沉淀下来。真正常用的进阶配置不是一次全上而是一个一个加。每加一项都要确认它不会破坏时序。比如串口传输大量数据如果采用轮询方式会阻塞主循环导致采样间隔不稳定那就改 DMA 或用缓存区拼接数据让主循环不被长时间卡住。6.3 最后建议先把动作精度表达清楚再谈提高精度很多初学者在机械臂示教项目里最容易犯的错是看到回放没有完全贴合原来的动作就急着调算法或换更高精度的舵机。但更合理的第一步是把“误差是多少”量化出来。让机械臂回到某个起点点击开始记录匀速走一段路径再手动回到起点。对比记录的首帧和结束帧通常能看出零点偏差和回程误差。再记录一条来回重复的摆臂轨迹让机械臂连续回放观察每条轨迹的电机关节角差多少。只有当你能用数字说出“这次回放和上次回放角度差了多少”后续的 PID 参数整定、舵机性能升级、加编码器闭环才有明确方向。如果一开始就追求“看起来一模一样”你很难知道到底是控制器问题、电机问题还是机械间隙问题。示教操作和轨迹复现真正吸引人的地方不在于让机械臂学会了一个动作而在于它把一次含糊的手部动作翻译成了一组精确、可保存、可修改的数据。当你意识到这一点stm32f103 便不再只是驱动舵机的开发板而是一台小小的“动作编译器”。先跑通最小闭环再慢慢把动作变成可命名的指令、可导出的数据、可复现的轨迹这条路会比盯着算法术语走要踏实得多。