机械臂的舞蹈从数学开始——xArm6运动学拆解日记

xarm6 机械臂正逆运动学分析 使用改进的 DH 坐标系进行建模，进行正逆运动学分析，逆运动学利用解析解求出全部关节逆运动学分析

搞机械臂就像玩拼装模型，只不过这里的零件是数学公式。今天咱们拿xArm6开刀，试试用改进DH参数法拆解它的运动学。改进DH坐标系这玩意儿，说白了就是把传统DH参数里坐标系的位置挪了挪，关节坐标系原点直接放在关节末端，这样建模时更符合直觉。

先画个骨架。xArm6的改进DH参数表长这样（单位mm和rad）：

正运动学就是搭积木，每个关节的变换矩阵像乐高块一样叠起来。来看Python实现：

import numpy as np from math import cos, sin def dh_transform(theta, d, a, alpha): return np.array([ [cos(theta), -sin(theta)*cos(alpha), sin(theta)*sin(alpha), a*cos(theta)], [sin(theta), cos(theta)*cos(alpha), -cos(theta)*sin(alpha), a*sin(theta)], [0, sin(alpha), cos(alpha), d ], [0, 0, 0, 1 ] ]) # 正运动学计算 def forward_kinematics(q): T = np.eye(4) params = [ (q[0], 267, 0, np.pi/2), (q[1], 0, 289.5, 0), (q[2], 0, 77.5, np.pi/2), (q[3], 286, 0, -np.pi/2), (q[4], 0, 0, np.pi/2), (q[5], 72, 0, 0) ] for param in params: T = T @ dh_transform(*param) return T

这段代码的精髓在dh_transform函数。注意α旋转轴变成绕x轴旋转，这和传统DH参数不同。矩阵连乘时用@运算符，比np.dot更直观。实际使用时，输入六个关节角度，就能得到末端位姿矩阵。

逆运动学才是真刺激。六轴机械臂的解析解需要找几何特征，咱们分步击破：

1. 腕部中心定位：先确定第5关节的位置，这里用几何法找前三关节角度
2. 腕部旋转解耦：后三关节处理姿态，用欧拉角分解

代码太长咱们拆关键部分看。比如求θ₁时，根据末端位置反推：

# 计算θ1的两个可能解 theta1_1 = np.arctan2(py, px) theta1_2 = np.arctan2(-py, -px)

这里px,py是末端坐标，利用反正切直接定位基座旋转角度。两个解对应机械臂左右两种构型。

求θ3时用余弦定理玩三角游戏：

# 根据平面几何求θ3 c3 = (x**2 + y**2 - L1**2 - L2**2) / (2*L1*L2) theta3_1 = np.arctan2(np.sqrt(1 - c3**2), c3) theta3_2 = np.arctan2(-np.sqrt(1 - c3**2), c3)

L1、L2是机械臂大臂和小臂长度。这个式子其实在解平面二连杆的逆运动学，得到两个可能的弯折方向。

解后三轴时，需要构造旋转矩阵：

R = T[0:3, 0:3] # 提取姿态矩阵 # ZYZ欧拉角分解 theta4 = np.arctan2(R[1,2], R[0,2]) theta5 = np.arctan2(np.sqrt(R[0,2]**2 + R[1,2]**2), R[2,2]) theta6 = np.arctan2(R[2,1], -R[2,0])

这里有个坑：当θ5接近0时会出万向节锁，需要特殊处理。实际代码中得加个条件判断。

验证逆解时别忘物理限制。比如这段过滤非法解的代码：

valid_solutions = [] for sol in all_solutions: if all(-np.pi <= angle <= np.pi for angle in sol[:3]) and \ -135*np.pi/180 <= sol[3] <= 135*np.pi/180 and \ -np.pi/2 <= sol[4] <= np.pi/2: valid_solutions.append(sol)

角度限制根据xArm6的实际情况设定，这里假设关节4的活动范围是±135度，关节5是±90度。

最后来个灵魂拷问：8组逆解中选哪个？这时候需要最优解算法，比如关节空间最短路径法。不过那是另一个故事了。运动学就像俄罗斯套娃，解开一层还有一层，但正是这种层层剥茧的过程，让冰冷的钢铁跳出了优美的轨迹。