Part 1把机械结构搭完、舵机装好之后你大概率会有一种“刚拿到驾照但不敢上路”的感觉——给舵机测试板随手发个角度四个关节确实会动但要让机械臂的末端准确停在你说的那个点它完全听不懂。我自己把第一台4-DOF机械臂拼完时就是对着一个只会乱摆姿势的“钢铁手指”发了半天呆。Part 2要做的就是把这台“会动的雕塑”变成一台“听得懂XYZ坐标”的执行机构。这篇文章会完整走一遍四自由度机械臂的控制链路运动学模型怎么建、逆运动学怎么解、舵机驱动怎么写、轨迹怎么平滑以及最后怎么调试标定让理论和现实对上。适合已经完成结构搭建、准备接电控和算法的朋友也适合想从舵机玩具进阶到正经机械臂控制的爱好者。1. 第二部分要解决的三个老大难从“会转”到“会干活”1.1 Part 1把机械臂装起来了然后呢Part 1结束后你手里应该有一台结构完整、四个舵机都装到位的机械臂样机。底座、上臂、前臂、腕部四个旋转关节活动范围正常单独给某个舵机发指令相应关节能转起来。但这时候的机械臂只能算“会动的雕塑”。因为机械臂的每个实际动作都是四个关节协同转动的结果单独转动任何一个关节都没有意义。你给舵机发一个120度的指令它确实转到120度但机械臂末端出现在哪里你不知道。机械臂自己也不知道。从“会转”到“会干活”中间隔着一条完整的数据链给定目标位置的XYZ坐标控制器先算出四个关节各自该转到多少度再转成舵机对应的PWM脉宽最后让四个舵机按合适的速度同步运动。这条链上任何一环对不上机械臂就会做出各种让你摸不着头脑的动作。1.2 三个关卡运动学、驱动、标定我把这个过程拆成三个老大难第一是几何模型。四个舵机的角度和末端位置之间是非线性映射你必须写出准确的数学关系也就是运动学模型。这个模型建不对后面一切免谈。第二是电控驱动。舵机表面看是“给个角度就动”实际上涉及PWM脉宽校准、供电稳定性、信号线接法。很多人第一步就栽在供电上四个舵机一起转电源电压瞬间掉下来机械臂一边抖动一边复位。第三是系统标定。理论计算时假设的连杆长度、关节零位角度和物理装配出来的一定有偏差。模型算得再漂亮不经过现场标定末端位置照样偏得离谱。这三关是循序渐进的。模型没建对后面全白搭模型对了但驱动不稳动作照样乱驱动稳了但没标定重复精度出不来。所以这篇文章就按这个顺序来。1.3 为什么4-DOF是入门进阶的最佳选择很多人问为什么不直接上6轴工业机械臂那种我的建议是先从4自由度玩透。3-DOF机械臂能控制末端在三维空间里到达某个位置但对“末端执行器本身的姿态”几乎没有控制能力。你可以让指尖到达某个点却不一定能让夹爪以你想要的角度去夹取。4-DOF多出来的腕部俯仰自由度正好解决了“末端姿态”这个问题已经能完成大量抓取、搬运类演示。6-DOF自由度多姿态空间大但逆运动学求解要面对多解、奇异点、非线性方程组入门门槛陡增。4-DOF刚好卡在中间覆盖了位置控制和末端姿态控制的核心概念逆解又能用平面几何直接手算。在这台机器上把“正逆运动学到底是怎么回事”彻底搞明白以后再上六轴思路是同一套只是数学复杂度变高而已。2. 运动学建模让机械臂“知道”自己的手在哪2.1 关节编号、连杆尺寸和坐标系约定先约定这台4-DOF机械臂的关节J1底座旋转关节绕竖直轴转动控制整条手臂水平旋转J2肩部俯仰关节J3肘部俯仰关节J4腕部俯仰关节然后是四个关键尺寸建议你用游标卡尺量别用钢卷尺估d1底座到肩关节中心线的垂直高度L1肩关节中心到肘关节中心的距离也就是上臂长度L2肘关节中心到腕关节中心的距离也就是前臂长度L3腕关节中心到末端夹爪参考点的距离这里特别提醒L3要格外注意。如果你以后换夹爪末端参考点是选在夹爪中心还是指尖L3都会变逆解结果完全不同。我调试时曾经因为L3差了3mm末端位置就偏了接近10mm。样机尺寸不同下面所有参数都要替换成你自己量出来的值。我样机的尺寸是d1 85mmL1 120mmL2 120mmL3 60mm。后面代码里都用这个值你跑的时候记得改。2.2 DH参数表机械臂的“身份档案”机器人学里描述连杆和关节的关系最标准的方法是DH参数法。四个参数a连杆长度指两个相邻关节轴之间的公垂线长度alpha连杆扭转角指两个相邻关节轴之间的空间夹角d关节偏移沿当前关节Z轴方向的偏移量theta关节角绕当前关节Z轴旋转的角度这个变量就是舵机驱动的角度这台机械臂的DH参数表长这样关节 ia (mm)alpha (deg)d (mm)theta1090d1θ12L100θ23L200θ34L300θ4这个表的核心信息是J1转动后坐标系扭转了90度把运动平面竖起来之后J2、J3、J4三个关节的旋转轴完全平行所以上臂和前臂始终在同一个竖直平面内运动。这正是4-DOF机械臂能直接用几何法解逆解的根本原因——底座旋转负责把整个平面转到目标方向平面内的三个关节负责控制距离和姿态。2.3 正运动学从关节角到末端坐标正运动学很简单已知四个关节角求末端位置和姿态。它是后面所有调试工作的“照妖镜”——逆解算出来的角度对不对用正解回算一遍坐标就知道了。用Python和NumPy实现代码量很小import numpy as np import math # 样机尺寸mm按你自己的机械臂实际测量值修改 D1 85.0 L1 120.0 L2 120.0 L3 60.0 def dh_mat(a, alpha, d, theta): ct math.cos(theta) st math.sin(theta) ca math.cos(alpha) sa math.sin(alpha) return np.array([ [ct, -st*ca, st*sa, a*ct], [st, ct*ca, -ct*sa, a*st], [0.0, sa, ca, d], [0.0, 0.0, 0.0, 1.0] ]) def forward(j1, j2, j3, j4): # 角度转弧度 t1 math.radians(j1) t2 math.radians(j2) t3 math.radians(j3) t4 math.radians(j4) # 按DH参数表连乘 T np.eye(4) T T dh_mat(0, math.pi/2, D1, t1) T T dh_mat(L1, 0, 0, t2) T T dh_mat(L2, 0, 0, t3) T T dh_mat(L3, 0, 0, t4) return T # 示例四个关节角都是0度时末端在哪 T forward(0, 0, 0, 0) print(T[:3, 3])矩阵连乘的物理含义是每乘一个矩阵就把坐标系沿着当前关节平移加旋转一次四次之后得到的变换矩阵前三列是末端姿态第四列前三行就是末端坐标。调正运动学的时候随便设几个角度在纸上用几何关系大致算一下末端大概在哪和代码输出对比。这一步能快速排除DH表写错、尺寸填错之类的基础问题。2.4 逆运动学从目标坐标反解关节角逆运动学才是真正干活的部分。给定目标坐标(x, y, z)和末端姿态角phi要反推出四个关节角。这台机械臂的逆解分四步走第一步基座角。从俯视图看J1要做的就是把整个机械臂平面转到目标点所在的方向直接用atan2θ1 atan2(y, x)第二步把问题压到平面里。目标点投影到水平面的距离是r sqrt(x² y²)在肩关节坐标系里目标点的坐标是(r, z - d1)。第三步考虑末端姿态。如果末端要按某个角度phi去夹取那么不能直接把目标点当成腕部中心因为末端还有L3这段长度在“带偏”。腕部中心的坐标要回推wx r - L3 * cos(phi) wz (z - d1) - L3 * sin(phi)第四步平面几何解算。肩关节、肘关节、腕部中心三点构成一个三角形两条边是L1和L2第三边是肩到腕的距离Lw sqrt(wx² wz²)用余弦定理求出肘部夹角θ3再通过角度组合求出肩部角θ2最后因为末端姿态约束有θ4 phi - θ2 - θ3完整代码def inverse(x, y, z, phi_deg): # 第一步基座旋转角 theta1 math.degrees(math.atan2(y, x)) # 目标在肩关节坐标系下的投影距离 r_xy math.sqrt(x*x y*y) z_shoulder z - D1 phi math.radians(phi_deg) # 第二步由末端姿态反推腕部中心 wx r_xy - L3 * math.cos(phi) wz z_shoulder - L3 * math.sin(phi) Lw math.sqrt(wx*wx wz*wz) # 检查可达性 if Lw L1 L2 or Lw abs(L1 - L2): raise ValueError(目标点不可达) # 第三步余弦定理求肘关节夹角 cos_t3 (L1*L1 L2*L2 - Lw*Lw) / (2 * L1 * L2) cos_t3 max(-1.0, min(1.0, cos_t3)) theta3 math.degrees(math.acos(cos_t3)) # 第四步肩关节角度 alpha math.atan2(wz, wx) beta math.acos((L1*L1 Lw*Lw - L2*L2) / (2 * L1 * Lw)) theta2 math.degrees(alpha - beta) # 第五步腕关节角度 theta4 phi_deg - theta2 - theta3 return [theta1, theta2, theta3, theta4] # 测试目标点(180, 0, 150)末端水平phi0 ang inverse(180, 0, 150, 0) print(逆解角度:, ang) # 用正运动学回算验证 T forward(*ang) print(正解坐标:, T[:3, 3], 期望坐标: [180, 0, 150])这里要解释清楚我对零位的约定θ2 0时上臂水平θ3 0时前臂与上臂在同一条直线上θ4 0时末端方向为水平。如果你的机械臂零位定义不同输出的角度要统一加一个偏移这就是第5章要说的零点标定。另外注意可达性问题。逆解必须在工作空间内才有解代码里判断了Lw是否超过L1L2或者小于两杆之差。很多初学者上来就卡在这里逆解报错然后怀疑公式错了实际上是目标点超出了机械臂能到达的范围。3. 电控与舵机驱动把角度命令变成真实的关节转动3.1 PWM脉宽到角度的映射关系运动学解算出来的是“目标角度”而舵机只认PWM脉宽。常规舵机的控制信号是50Hz的PWM波也就是周期20ms其中高电平脉宽在0.5ms到2.5ms之间对应0度到180度。换算关系是pulse_us 500 (angle / 180) * 2000比如90度对应的脉宽是500 (90/180)*2000 1500us。但这里有个坑这个线性关系是理论标称
4-DOF机械臂控制实战：运动学建模、逆解、舵机驱动与标定
Part 1把机械结构搭完、舵机装好之后你大概率会有一种“刚拿到驾照但不敢上路”的感觉——给舵机测试板随手发个角度四个关节确实会动但要让机械臂的末端准确停在你说的那个点它完全听不懂。我自己把第一台4-DOF机械臂拼完时就是对着一个只会乱摆姿势的“钢铁手指”发了半天呆。Part 2要做的就是把这台“会动的雕塑”变成一台“听得懂XYZ坐标”的执行机构。这篇文章会完整走一遍四自由度机械臂的控制链路运动学模型怎么建、逆运动学怎么解、舵机驱动怎么写、轨迹怎么平滑以及最后怎么调试标定让理论和现实对上。适合已经完成结构搭建、准备接电控和算法的朋友也适合想从舵机玩具进阶到正经机械臂控制的爱好者。1. 第二部分要解决的三个老大难从“会转”到“会干活”1.1 Part 1把机械臂装起来了然后呢Part 1结束后你手里应该有一台结构完整、四个舵机都装到位的机械臂样机。底座、上臂、前臂、腕部四个旋转关节活动范围正常单独给某个舵机发指令相应关节能转起来。但这时候的机械臂只能算“会动的雕塑”。因为机械臂的每个实际动作都是四个关节协同转动的结果单独转动任何一个关节都没有意义。你给舵机发一个120度的指令它确实转到120度但机械臂末端出现在哪里你不知道。机械臂自己也不知道。从“会转”到“会干活”中间隔着一条完整的数据链给定目标位置的XYZ坐标控制器先算出四个关节各自该转到多少度再转成舵机对应的PWM脉宽最后让四个舵机按合适的速度同步运动。这条链上任何一环对不上机械臂就会做出各种让你摸不着头脑的动作。1.2 三个关卡运动学、驱动、标定我把这个过程拆成三个老大难第一是几何模型。四个舵机的角度和末端位置之间是非线性映射你必须写出准确的数学关系也就是运动学模型。这个模型建不对后面一切免谈。第二是电控驱动。舵机表面看是“给个角度就动”实际上涉及PWM脉宽校准、供电稳定性、信号线接法。很多人第一步就栽在供电上四个舵机一起转电源电压瞬间掉下来机械臂一边抖动一边复位。第三是系统标定。理论计算时假设的连杆长度、关节零位角度和物理装配出来的一定有偏差。模型算得再漂亮不经过现场标定末端位置照样偏得离谱。这三关是循序渐进的。模型没建对后面全白搭模型对了但驱动不稳动作照样乱驱动稳了但没标定重复精度出不来。所以这篇文章就按这个顺序来。1.3 为什么4-DOF是入门进阶的最佳选择很多人问为什么不直接上6轴工业机械臂那种我的建议是先从4自由度玩透。3-DOF机械臂能控制末端在三维空间里到达某个位置但对“末端执行器本身的姿态”几乎没有控制能力。你可以让指尖到达某个点却不一定能让夹爪以你想要的角度去夹取。4-DOF多出来的腕部俯仰自由度正好解决了“末端姿态”这个问题已经能完成大量抓取、搬运类演示。6-DOF自由度多姿态空间大但逆运动学求解要面对多解、奇异点、非线性方程组入门门槛陡增。4-DOF刚好卡在中间覆盖了位置控制和末端姿态控制的核心概念逆解又能用平面几何直接手算。在这台机器上把“正逆运动学到底是怎么回事”彻底搞明白以后再上六轴思路是同一套只是数学复杂度变高而已。2. 运动学建模让机械臂“知道”自己的手在哪2.1 关节编号、连杆尺寸和坐标系约定先约定这台4-DOF机械臂的关节J1底座旋转关节绕竖直轴转动控制整条手臂水平旋转J2肩部俯仰关节J3肘部俯仰关节J4腕部俯仰关节然后是四个关键尺寸建议你用游标卡尺量别用钢卷尺估d1底座到肩关节中心线的垂直高度L1肩关节中心到肘关节中心的距离也就是上臂长度L2肘关节中心到腕关节中心的距离也就是前臂长度L3腕关节中心到末端夹爪参考点的距离这里特别提醒L3要格外注意。如果你以后换夹爪末端参考点是选在夹爪中心还是指尖L3都会变逆解结果完全不同。我调试时曾经因为L3差了3mm末端位置就偏了接近10mm。样机尺寸不同下面所有参数都要替换成你自己量出来的值。我样机的尺寸是d1 85mmL1 120mmL2 120mmL3 60mm。后面代码里都用这个值你跑的时候记得改。2.2 DH参数表机械臂的“身份档案”机器人学里描述连杆和关节的关系最标准的方法是DH参数法。四个参数a连杆长度指两个相邻关节轴之间的公垂线长度alpha连杆扭转角指两个相邻关节轴之间的空间夹角d关节偏移沿当前关节Z轴方向的偏移量theta关节角绕当前关节Z轴旋转的角度这个变量就是舵机驱动的角度这台机械臂的DH参数表长这样关节 ia (mm)alpha (deg)d (mm)theta1090d1θ12L100θ23L200θ34L300θ4这个表的核心信息是J1转动后坐标系扭转了90度把运动平面竖起来之后J2、J3、J4三个关节的旋转轴完全平行所以上臂和前臂始终在同一个竖直平面内运动。这正是4-DOF机械臂能直接用几何法解逆解的根本原因——底座旋转负责把整个平面转到目标方向平面内的三个关节负责控制距离和姿态。2.3 正运动学从关节角到末端坐标正运动学很简单已知四个关节角求末端位置和姿态。它是后面所有调试工作的“照妖镜”——逆解算出来的角度对不对用正解回算一遍坐标就知道了。用Python和NumPy实现代码量很小import numpy as np import math # 样机尺寸mm按你自己的机械臂实际测量值修改 D1 85.0 L1 120.0 L2 120.0 L3 60.0 def dh_mat(a, alpha, d, theta): ct math.cos(theta) st math.sin(theta) ca math.cos(alpha) sa math.sin(alpha) return np.array([ [ct, -st*ca, st*sa, a*ct], [st, ct*ca, -ct*sa, a*st], [0.0, sa, ca, d], [0.0, 0.0, 0.0, 1.0] ]) def forward(j1, j2, j3, j4): # 角度转弧度 t1 math.radians(j1) t2 math.radians(j2) t3 math.radians(j3) t4 math.radians(j4) # 按DH参数表连乘 T np.eye(4) T T dh_mat(0, math.pi/2, D1, t1) T T dh_mat(L1, 0, 0, t2) T T dh_mat(L2, 0, 0, t3) T T dh_mat(L3, 0, 0, t4) return T # 示例四个关节角都是0度时末端在哪 T forward(0, 0, 0, 0) print(T[:3, 3])矩阵连乘的物理含义是每乘一个矩阵就把坐标系沿着当前关节平移加旋转一次四次之后得到的变换矩阵前三列是末端姿态第四列前三行就是末端坐标。调正运动学的时候随便设几个角度在纸上用几何关系大致算一下末端大概在哪和代码输出对比。这一步能快速排除DH表写错、尺寸填错之类的基础问题。2.4 逆运动学从目标坐标反解关节角逆运动学才是真正干活的部分。给定目标坐标(x, y, z)和末端姿态角phi要反推出四个关节角。这台机械臂的逆解分四步走第一步基座角。从俯视图看J1要做的就是把整个机械臂平面转到目标点所在的方向直接用atan2θ1 atan2(y, x)第二步把问题压到平面里。目标点投影到水平面的距离是r sqrt(x² y²)在肩关节坐标系里目标点的坐标是(r, z - d1)。第三步考虑末端姿态。如果末端要按某个角度phi去夹取那么不能直接把目标点当成腕部中心因为末端还有L3这段长度在“带偏”。腕部中心的坐标要回推wx r - L3 * cos(phi) wz (z - d1) - L3 * sin(phi)第四步平面几何解算。肩关节、肘关节、腕部中心三点构成一个三角形两条边是L1和L2第三边是肩到腕的距离Lw sqrt(wx² wz²)用余弦定理求出肘部夹角θ3再通过角度组合求出肩部角θ2最后因为末端姿态约束有θ4 phi - θ2 - θ3完整代码def inverse(x, y, z, phi_deg): # 第一步基座旋转角 theta1 math.degrees(math.atan2(y, x)) # 目标在肩关节坐标系下的投影距离 r_xy math.sqrt(x*x y*y) z_shoulder z - D1 phi math.radians(phi_deg) # 第二步由末端姿态反推腕部中心 wx r_xy - L3 * math.cos(phi) wz z_shoulder - L3 * math.sin(phi) Lw math.sqrt(wx*wx wz*wz) # 检查可达性 if Lw L1 L2 or Lw abs(L1 - L2): raise ValueError(目标点不可达) # 第三步余弦定理求肘关节夹角 cos_t3 (L1*L1 L2*L2 - Lw*Lw) / (2 * L1 * L2) cos_t3 max(-1.0, min(1.0, cos_t3)) theta3 math.degrees(math.acos(cos_t3)) # 第四步肩关节角度 alpha math.atan2(wz, wx) beta math.acos((L1*L1 Lw*Lw - L2*L2) / (2 * L1 * Lw)) theta2 math.degrees(alpha - beta) # 第五步腕关节角度 theta4 phi_deg - theta2 - theta3 return [theta1, theta2, theta3, theta4] # 测试目标点(180, 0, 150)末端水平phi0 ang inverse(180, 0, 150, 0) print(逆解角度:, ang) # 用正运动学回算验证 T forward(*ang) print(正解坐标:, T[:3, 3], 期望坐标: [180, 0, 150])这里要解释清楚我对零位的约定θ2 0时上臂水平θ3 0时前臂与上臂在同一条直线上θ4 0时末端方向为水平。如果你的机械臂零位定义不同输出的角度要统一加一个偏移这就是第5章要说的零点标定。另外注意可达性问题。逆解必须在工作空间内才有解代码里判断了Lw是否超过L1L2或者小于两杆之差。很多初学者上来就卡在这里逆解报错然后怀疑公式错了实际上是目标点超出了机械臂能到达的范围。3. 电控与舵机驱动把角度命令变成真实的关节转动3.1 PWM脉宽到角度的映射关系运动学解算出来的是“目标角度”而舵机只认PWM脉宽。常规舵机的控制信号是50Hz的PWM波也就是周期20ms其中高电平脉宽在0.5ms到2.5ms之间对应0度到180度。换算关系是pulse_us 500 (angle / 180) * 2000比如90度对应的脉宽是500 (90/180)*2000 1500us。但这里有个坑这个线性关系是理论标称
