六自由度机械臂逆运动学怎么算？几何法不再靠猜

玩机械臂最上头也最容易劝退的一步，是让末端夹爪准准地伸到你想要的位置。一开始我只会写“把一号舵机转到90度”这种死代码，夹个固定差不多的物件也就罢了，一旦换个角度摆放就抓不到了。真正解决问题的是逆运动学，就是把目标位置反推成每根关节该转多少度，算一遍就不用再瞎调。

先说正运动学，它是已知每根关节的角度求末端在哪，顺着往前算，公式好推。逆运动学是反过来的难题：已知末端要到的(x,y,z)，求每根关节角度。难就难在解不唯一——手肘可以弓上来也可以沉下去，有好几种摆法都能让手到同一个点。

从平面桁架入手，先会2根杆

别一上来就啃六自由度的矩阵。最基础的情况是平面机械臂，两节连杆L1、L2，只在一个平面里转。给一个目标点(x,y)，先用距离公式算出从肩关节到目标的长度r，再用余弦定理就能解出手肘角，肩关节角再用反正切配合几何关系算出来。

这一步有个关键词叫“可达性检查”，如果r比L1+L2还大，说明目标太远根本够不着；如果r比两杆长度差还小，目标太近了卡在两杆之间，也要先判出来，否则算出反余弦会超范围。

真到六自由度，学界标准做法是DH参数法：把每根连杆用四个参数描述，一段段把坐标系变换串起来。这样系统有了一套统一的建模语言，不管底座、肩、肘还是球形腕，都能画进同一个变换矩阵里。

对着自己的机械臂量出每段连杆长度、关节偏置这些参数，填进DH表，再按变换公式连乘，就能得到末端位姿。这是后面所有逆运动学算法（解析法、雅可比数值法）的地基。

逆运动学的解法常见两种。解析法是针对你具体结构手推公式，快、准、适合实时控制，但每换一版机械臂结构就得重推一次。数值法是通用的，比如Levenberg-Marquardt这类迭代，或者CCD这种每次调一个关节逼近，写起来通用性好，但可能陷进局部最优。

对刚入门的人，我的建议是先做平面或简化模型的解析解，把2到3个关节算明白，跑通了再加自由度。像手头三连杆、五连杆的小实验臂，几何解析完全够用且能在Arduino这种小板子上跑得动。

就算解出来了也有选择题：同一目标有好几组关节角都行。要拟定约束，比如角度别超量程、选离当前姿态最近的一组，避免舵机大幅摆动还不自然。给每个关节设好活动范围，解出来的角度先夹限再下发。

实际下发给舵机时，很多DIY臂用两节解析加后面几节固定姿态的策略，减少解算量，看起来动作也更顺。轮子转起来前，先在电脑上或串口打印验证一下解的连续性，别让机器人突然抽风。

逆运动学是机械臂从“能动的玩具”走向“能干活工具”的关键一步，虽然一上来充满矩阵，但从一根平面臂算起，迈过第一个闭环你会觉得豁然开朗。