发帖
雷达卡
本文介绍了一种适用于6自由度机械臂的完整逆运动学解析解法。当机械臂结构满足Pieper准则，即最后三个旋转轴交汇于同一点时，可采用封闭形式的解析方法求解逆运动学问题。
该实现基于标准Denavit-Hartenberg（DH）参数体系，并结合几何分析方法进行推导与计算。
import math
import numpy as np
from scipy.spatial.transform import Rotation as R类定义：CompleteInverseKinematics
该类封装了正/逆运动学的完整计算逻辑，初始化时设定机械臂的几何结构和运动限制。
class CompleteInverseKinematics:
"""
实现6自由度机械臂的逆运动学解析求解
采用标准DH参数建模与几何解析方法
"""初始化参数设置
在构造函数中配置机械臂的DH参数、各关节角度限制以及关键连杆尺寸。
def __init__(self):
# DH参数格式: [a, alpha, d, theta_offset]
# 分别表示：连杆长度、扭角、偏移量、初始角度补偿
self.dh_params = [
[0, 0, 89.2, 0], # 第1轴 - 基座回转
[0, -90, 0, -90], # 第2轴 - 肩部转动
[135, 0, 0, 0], # 第3轴 - 肘部摆动
[0, -90, 147, 0], # 第4轴 - 手腕俯仰
[0, 90, 0, 0], # 第5轴 - 手腕扭转
[0, 0, 65, 0] # 第6轴 - 末端执行器
]
# 各关节舵机允许的角度范围（单位：度）
self.servo_limits = [
(0, 180), # 关节1
(0, 180), # 关节2
(0, 180), # 关节3
(0, 180), # 关节4
(0, 180), # 关节5
(0, 180) # 关节6
]
# 提取常用几何参数便于后续计算
self.a1 = 0 # 第一连杆长度
self.a2 = 135 # 第二连杆长度
self.a3 = 0 # 第三连杆长度
self.d1 = 89.2 # 第一轴偏移高度
self.d4 = 147 # 第四轴到腕部中心的距离
self.d6 = 65 # 末端法兰到工具点距离
DH变换矩阵生成函数
根据给定的DH参数计算单个关节的齐次变换矩阵。
def dh_matrix(self, a, alpha, d, theta):
"""
构造Denavit-Hartenberg变换矩阵
参数说明：
a: 连杆长度
alpha: 扭转角（弧度制）
d: 沿z轴的偏移
theta: 关节角（弧度制）
返回值：
4×4的齐次变换矩阵
"""
return np.array([
[math.cos(theta), -math.sin(theta)*math.cos(alpha),
math.sin(theta)*math.sin(alpha), a*math.cos(theta)],
[math.sin(theta), math.cos(theta)*math.cos(alpha),
-math.cos(theta)*math.sin(alpha), a*math.sin(theta)],
[0, math.sin(alpha), math.cos(alpha), d],
[0, 0, 0, 1]
])
正运动学实现
通过输入六个关节角度，计算末端执行器的空间位置与姿态。
def forward_kinematics(self, joint_angles):
"""
正向运动学：由关节角度推导末端位姿
输入：
joint_angles: 包含6个角度值的列表（单位：度）
输出：
position: 三维坐标 [x, y, z]
orientation: 3×3旋转矩阵
"""
T = np.eye(4)
for i in range(6):
a, alpha, d, theta_offset = self.dh_params[i]
theta = math.radians(joint_angles[i] + theta_offset)
alpha_rad = math.radians(alpha)
T_i = self.dh_matrix(a, alpha_rad, d, theta)
T = T @ T_i
position = T[:3, 3]
orientation = T[:3, :3]
return position, orientation
逆运动学主函数
根据目标位置和姿态反解出所有可能的关节角度组合，支持选择“肘部向上”或“向下”的工作模式。
def inverse_kinematics(self, target_position, target_orientation=None, elbow_up=True):
"""
解析法求解完整的逆运动学问题
参数：
target_position: 目标空间坐标 [x, y, z]
target_orientation: 可选的目标方向（3×3矩阵 或 欧拉角[roll,pitch,yaw]）
elbow_up: 是否启用肘部抬升构型（True表示上抬）
返回：
list: 六个有效且在限位范围内的关节角度（单位：度）
"""
# 对输入的姿态信息进行统一处理
此方法能够高效地返回符合物理约束的多组解，并可根据实际应用场景筛选最优解。整个算法不依赖数值迭代，具备高实时性与稳定性，适合嵌入式控制系统部署。
# 如果未指定目标姿态，则使用单位旋转矩阵作为默认值
if target_orientation is None:
target_orientation = np.eye(3)
# 若输入为长度为3的列表或元组，视作欧拉角并转换为旋转矩阵
elif isinstance(target_orientation, (list, tuple)) and len(target_orientation) == 3:
roll, pitch, yaw = target_orientation
target_orientation = R.from_euler('xyz', [roll, pitch, yaw], degrees=True).as_matrix()
# 获取机械臂的DH参数（连杆长度与偏移）
a1, a2, a3 = self.a1, self.a2, self.a3
d1, d4, d6 = self.d1, self.d4, self.d6
# 第一步：计算腕部中心点位置
# 腕部中心 = 末端执行器位置 - d6沿末端Z轴方向的投影
wrist_center = target_position - d6 * target_orientation[:, 2]
x, y, z = wrist_center
# 第二步：求解关节角1（基座旋转）
# 利用x和y坐标确定水平面内的旋转角度
theta1 = math.atan2(y, x)
# 第三步：求解关节角2和关节角3
# 计算末端在XY平面上的投影距离原点的距离
r = math.sqrt(x**2 + y**2)
# 计算从肩部到腕部中心的空间距离
D = math.sqrt(r**2 + (z - d1)**2)
# 应用余弦定理求解关节3的角度
# 引入a2、a3及d4进行三角关系建模
cos_theta3 = (a2**2 + a3**2 + d4**2 - D**2) / (2 * a2 * math.sqrt(a3**2 + d4**2))
cos_theta3 = np.clip(cos_theta3, -1, 1) # 防止数值误差导致越界
# 根据用户选择决定肘部朝向（向上或向下），对应不同的解
if elbow_up:
theta3 = math.atan2(math.sqrt(1 - cos_theta3**2), cos_theta3)
else:
theta3 = math.atan2(-math.sqrt(1 - cos_theta3**2), cos_theta3)
# 求解关节2的角度
alpha = math.atan2(z - d1, r) # 垂直平面内的倾角
beta = math.atan2(d4, a3) # 由连杆偏移引入的补偿角
gamma = math.atan2(
math.sqrt(a3**2 + d4**2) * math.sin(theta3 + beta),
a2 + math.sqrt(a3**2 + d4**2) * math.cos(theta3 + beta)
)
theta2 = alpha - gamma
# 第四步：构建前三个关节的变换矩阵
T01 = self.dh_matrix(a1, 0, d1, theta1)
T12 = self.dh_matrix(0, -math.pi/2, 0, theta2)
T23 = self.dh_matrix(a2, 0, 0, theta3)
T03 = T01 @ T12 @ T23
# 第五步：求解后三个关节（关节4、5、6）
# 构造末端位姿的齐次变换矩阵
T06 = np.eye(4)
T06[:3, :3] = target_orientation
T06[:3, 3] = target_position
# 计算T36：即第三关节到末端的相对变换
T03_inv = np.linalg.inv(T03)
T36 = T03_inv @ T06
# 提取T36中的旋转部分用于求解剩余关节角
nx, ny, nz = T36[0, 0], T36[1, 0], T36[2, 0]
ox, oy, oz = T36[0, 1], T36[1, 1], T36[2, 1]
ax, ay, az = T36[0, 2], T36[1, 2], T36[2, 2]
# 求解关节5
theta5 = math.atan2(math.sqrt(ax**2 + ay**2), az)
# 处理关节5接近零时的奇异情况
if abs(theta5) < 1e-6:
# 此时关节4与关节6轴线重合，无法唯一确定各自角度，仅可确定其和
theta4 = 0
theta6 = math.atan2(ny, nx) - theta4
else:
# 正常情况下分别求解关节4和关节6
theta4 = math.atan2(ay, ax)
theta6 = math.atan2(oz, -nz)
def inverse_kinematics_with_multiple_solutions(self, target_position, target_orientation=None):
"""
计算逆运动学的多个可能解
参数:
target_position: 目标位置 [x, y, z]
target_orientation: 目标姿态矩阵 (3x3) 或欧拉角 [roll, pitch, yaw]
返回:
包含多个解的列表，每个解是6个关节角度的列表
"""
solutions = []
# 遍历不同的机械臂构型
for elbow_up in [True, False]:
# 尝试关节1的两种可能解（相差π）
for theta1_offset in [0, math.pi]:
try:
# 处理输入的姿态信息
if target_orientation is None:
target_orientation = np.eye(3)
elif isinstance(target_orientation, (list, tuple)) and len(target_orientation) == 3:
roll, pitch, yaw = target_orientation
target_orientation = R.from_euler('xyz', [roll, pitch, yaw], degrees=True).as_matrix()
# 获取DH参数中的几何常量
a1, a2, a3 = self.a1, self.a2, self.a3
d1, d4, d6 = self.d1, self.d4, self.d6
# 计算腕部中心点位置
wrist_center = target_position - d6 * target_orientation[:, 2]
x, y, z = wrist_center
# 求解第一个关节角度
theta1 = math.atan2(y, x) + theta1_offset
# 计算在水平面的投影距离和空间距离
r = math.sqrt(x**2 + y**2)
D = math.sqrt(r**2 + (z - d1)**2)
# 求解第三个关节角度
cos_theta3 = (a2**2 + a3**2 + d4**2 - D**2) / (2 * a2 * math.sqrt(a3**2 + d4**2))
cos_theta3 = np.clip(cos_theta3, -1, 1)
if elbow_up:
theta3 = math.atan2(math.sqrt(1 - cos_theta3**2), cos_theta3)
else:
theta3 = math.atan2(-math.sqrt(1 - cos_theta3**2), cos_theta3)
# 求解第二个关节角度
alpha = math.atan2(z - d1, r)
beta = math.atan2(d4, a3)
gamma = math.atan2(
math.sqrt(a3**2 + d4**2) * math.sin(theta3 + beta),
a2 + math.sqrt(a3**2 + d4**2) * math.cos(theta3 + beta)
)
theta2 = alpha - gamma
# 构造前三个关节的变换矩阵
T01 = self.dh_matrix(a1, 0, d1, theta1)
T12 = self.dh_matrix(0, -math.pi/2, 0, theta2)
T23 = self.dh_matrix(a2, 0, 0, theta3)
T03 = T01 @ T12 @ T23
# 构建末端执行器的目标齐次变换矩阵
T06 = np.eye(4)
T06[:3, :3] = target_orientation
T06[:3, 3] = target_position
# 计算第三到第六关节之间的相对变换
T03_inv = np.linalg.inv(T03)
T36 = T03_inv @ T06
# 提取第四个、第五个和第六个关节的角度
# （此处省略具体theta4, theta5, theta6的计算逻辑）
# 转换为角度并考虑DH参数中的偏移
joint_angles_rad = [theta1, theta2, theta3, theta4, theta5, theta6]
joint_angles_deg = [math.degrees(angle) for angle in joint_angles_rad]
# 应用DH参数中定义的角度偏移
for i in range(6):
joint_angles_deg[i] += self.dh_params[i][3]
# 确保各关节角度处于允许范围内
valid_angles = self.limit_joint_angles(joint_angles_deg)
solutions.append(valid_angles)
except Exception as e:
continue # 忽略无效解
return solutions
从变换矩阵 T36 中提取坐标系的法向、指向和接近向量分量：
nx, ny, nz = T36[0, 0], T36[1, 0], T36[2, 0]
ox, oy, oz = T36[0, 1], T36[1, 1], T36[2, 1]
ax, ay, az = T36[0, 2], T36[1, 2], T36[2, 2]
计算第五个关节的角度 theta5，使用 atan2 函数基于接近向量 ax、ay 和 az 的关系：
theta5 = math.atan2(math.sqrt(ax**2 + ay**2), az)
[此处为图片1]
针对第四个与第六个关节角度的求解：
当 theta5 接近零（奇异位形）时：
- 设定 theta4 为 0
- theta6 由 ny 与 nx 的比值决定，并减去 theta4
若非奇异情况，则分别通过以下方式计算：
- theta4 = math.atan2(ay, ax)
- theta6 = math.atan2(oz, -nz)
将所有计算出的弧度制关节角转换为角度制：
joint_angles_rad = [theta1, theta2, theta3, theta4, theta5, theta6]
joint_angles_deg = [math.degrees(angle) for angle in joint_angles_rad]
根据 DH 参数中的偏移项对每个角度进行补偿：
for i in range(6):
joint_angles_deg[i] += self.dh_params[i][3]
调用 limit_joint_angles 方法对角度进行范围限制处理：
limited_angles = self.limit_joint_angles(joint_angles_deg)
验证该组解是否满足目标位姿要求：
if self.is_valid_solution(limited_angles, target_position, target_orientation):
solutions.append(limited_angles)
except (ValueError, np.linalg.LinAlgError):
continue
返回所有符合条件的逆运动学解集：
return solutions
is_valid_solution 方法说明
用于判断一组关节角度是否构成有效的逆运动学解。
输入参数包括：当前关节角度、期望的目标位置及目标姿态。
首先检查各关节角是否处于预设的物理限位范围内：
for i, angle in enumerate(joint_angles):
min_angle, max_angle = self.servo_limits[i]
if not (min_angle <= angle <= max_angle):
return False
随后利用正运动学函数 forward_kinematics 计算该组角度下的实际末端位姿：
calculated_position, calculated_orientation = self.forward_kinematics(joint_angles)
比较实际位置与目标位置之间的欧氏距离误差：
position_error = np.linalg.norm(np.array(calculated_position) - np.array(target_position))
if position_error > 1.0: # 容许 1 毫米以内误差
return False
同时评估姿态差异：
orientation_error = np.linalg.norm(calculated_orientation - target_orientation)
if orientation_error > 0.1:
return False
若全部检验通过，则返回 True，表示此解有效。
limit_joint_angles 方法说明
该方法用于将输入的关节角度规范化并约束在允许区间内。
流程如下：
- 遍历每个关节角度
- 先将其模 360 映射至 [0, 360) 区间
- 若结果大于 180，则减去 360，使其落在 [-180, 180) 范围内
- 再根据伺服系统的上下限 min_angle 与 max_angle 进行裁剪
最终生成一组合规的角度值并返回：
return limited_angles
def select_best_solution(self, solutions, current_angles=None):
"""
从候选解集中挑选最优的逆运动学解
参数说明：
solutions: 包含多个可行解的列表
current_angles: 当前各关节的实际角度（用于选取最接近的解）
返回值：
最优解对应的关节角度数组
"""
if not solutions:
raise ValueError("未找到有效的逆运动学解")
# 若未提供当前角度，则默认返回第一个可行解
if current_angles is None:
return solutions[0]
best_solution = None
min_distance = float('inf')
# 遍历所有解，计算其与当前角度的欧氏距离
for solution in solutions:
distance = sum((s - c)**2 for s, c in zip(solution, current_angles))
if distance < min_distance:
min_distance = distance
best_solution = solution
return best_solution
以下为程序主运行部分的测试逻辑：
if __name__ == "__main__":
# 初始化完整逆运动学求解器
ik_solver = CompleteInverseKinematics()
# 正向运动学功能测试
test_angles = [90, 90, 90, 90, 90, 90] # 所有轴置于中间位置
position, orientation = ik_solver.forward_kinematics(test_angles)
print(f"正运动学测试:")
print(f" 输入角度: {test_angles}")
print(f" 计算位置: {position}")
print(f" 计算姿态: \n{orientation}")
# 开始逆运动学求解测试
print(f"\n逆运动学测试:")
solutions = ik_solver.inverse_kinematics_with_multiple_solutions(position, orientation)
print(f" 共找到 {len(solutions)} 组解:")
for i, sol in enumerate(solutions):
print(f" 解 {i+1}: {[round(angle, 2) for angle in sol]}")
# 在多个解中选择最优解
if solutions:
best_solution = ik_solver.select_best_solution(solutions, test_angles)
print(f"\n选定的最佳解: {[round(angle, 2) for angle in best_solution]}")
# 验证所选解的准确性
verified_position, verified_orientation = ik_solver.forward_kinematics(best_solution)
position_error = np.linalg.norm(np.array(verified_position) - np.array(position))
orientation_error = np.linalg.norm(verified_orientation - orientation)
print(f"验证结果:")
print(f" 位置误差: {position_error:.6f} mm")
print(f" 姿态误差: {orientation_error:.6f}")
if position_error < 1.0 and orientation_error < 0.1:
print(" ? 逆运动学解验证成功!")
else:
print(" ? 逆运动学解验证失败!")
[此处为图片1]
实际应用示例代码如下：
# 实例化完整的逆运动学处理器
ik_solver = CompleteInverseKinematics()
# 设定期望的末端执行器目标参数
target_position = [200, 100, 150] # 目标坐标 (单位：毫米)
target_orientation = [0, 0, 0] # 目标姿态角 (单位：度)
# 求解所有符合条件的关节配置组合
solutions = ik_solver.inverse_kinematics_with_multiple_solutions(
target_position,
target_orientation
)
该实现支持多解情况下的逆运动学处理，并通过最小化与当前状态的距离来优选最合适的关节角度组合。适用于需要平滑轨迹规划和避障切换的应用场景。
# 如果存在解，则选择最佳解（可传入当前角度以选取最接近的配置）
if solutions:
best_solution = ik_solver.select_best_solution(solutions)
print(f"最优关节角度: {best_solution}")
# 利用正运动学验证所选解的准确性
calculated_position, calculated_orientation = ik_solver.forward_kinematics(best_solution)
print(f"目标位置: {target_position}")
print(f"计算得出的位置: {calculated_position}")
# 计算实际误差
position_error = np.linalg.norm(np.array(calculated_position) - np.array(target_position))
print(f"位置误差: {position_error:.4f} mm")
else:
print("未找到有效的逆运动学解")
[此处为图片1]
核心功能说明
解析解完整性：
- 依据标准DH参数与几何原理构建
- 支持输出所有可行的关节角组合
- 能够处理机械臂处于奇异位形的情况
多解管理机制：
- 区分肘部朝上与朝下的不同构型
- 支持关节1的多种旋转方向判断
- 自动排除超出物理限制的无效解
结果验证流程：
- 通过正运动学反向验证逆解正确性
- 核查各关节是否在允许范围内
- 输出位置及姿态误差用于精度评估
最优解选取策略：
- 优先选择与当前状态最接近的解，确保动作平滑
- 避免出现大范围或突兀的关节变动
[此处为图片2]
使用注意事项
关于奇异位置：
- 当关节5接近0°时，系统进入奇异状态
- 此时关节4与关节6的旋转轴趋于重合，仅能确定二者角度之和
工作空间约束：
- 目标点必须位于机械臂可达范围内
- 若超出该范围，可能导致无解情况发生
参数校准建议：
- 根据实际设备尺寸精确设置DH参数
- 可结合实验数据进行标定，提升求解精度
京公网安备 11010802022788号
