```
import numpy as np
from math import cos, sin, atan2, sqrt, pi
import math
# 定义D-H参数示例(需根据实际参数修改)
dh_params = [
{'a': 0, 'd': 0.1, 'alpha': pi / 2}, # 关节1
{'a': 0.5, 'd': 0, 'alpha': 0}, # 关节2
{'a': 0.5, 'd': 0, 'alpha': 0}, # 关节3
{'a': 0.3, 'd': 0, 'alpha': pi / 2}, # 关节4
{'a': 0, 'd': 0, 'alpha': 0} # 关节5
]
cam_offset = np.array([0, -0.05, 0]) # 摄像头偏移量
R_cam_in_j5 = np.eye(3) # 假设摄像头与J5坐标系对齐
def camera_to_j5(target_cam):
"""将摄像头坐标系下的目标转换到关节5坐标系"""
target_j5 = R_cam_in_j5.T @ (target_cam - cam_offset)
return target_j5
def inverse_kinematics(target_pos, target_orient):
# 坐标转换
target_j5 = camera_to_j5(target_pos)
# 前三个关节求解(位置解)
x, y, z = target_j5
# 关节1角度计算
theta1 = atan2(y, x)
# 关节2-3几何解
L = sqrt(x ** 2 + y ** 2) - dh_params[0]['d']
D = (L ** 2 + z ** 2 - dh_params[1]['a'] ** 2 - dh_params[2]['a'] ** 2) / (
2 * dh_params[1]['a'] * dh_params[2]['a'])
theta3 = atan2(-sqrt(1 - D ** 2), D)
# 关节2角度计算
theta2 = atan2(z, L) - atan2(dh_params[2]['a'] * sin(theta3), dh_params[1]['a'] + dh_params[2]['a'] * cos(theta3))
# 后两个关节求解(姿态解)
# 计算工具坐标系到基坐标系的齐次变换矩阵
T = forward_kinematics([theta1, theta2, theta3, 0, 0]) # 需要正运动学实现
R_base_tool = T[:3, :3]
# 计算目标姿态与当前姿态的旋转差
R_error = target_orient @ R_base_tool.T
theta4, theta5, _ = extract_euler_angles(R_error) # 需要欧拉角提取实现
return [theta1, theta2, theta3, theta4, theta5]
def forward_kinematics(theta):
"""正运动学计算(需完整实现)"""
T = np.eye(4)
for i in range(5):
a = dh_params[i]['a']
d = dh_params[i]['d']
alpha = dh_params[i]['alpha']
T_i = np.array([
[cos(theta[i]), -sin(theta[i]) * cos(alpha), sin(theta[i]) * sin(alpha), a * cos(theta[i])],
[sin(theta[i]), cos(theta[i]) * cos(alpha), -cos(theta[i]) * sin(alpha), a * sin(theta[i])],
[0, sin(alpha), cos(alpha), d],
[0, 0, 0, 1]
])
T = T @ T_i
return T
def extract_euler_angles(rotation_matrix):
"""
从3x3旋转矩阵提取ZYX顺序的欧拉角(单位:度)
参数格式:列主序矩阵(例如OpenGL)
"""
m = rotation_matrix
theta_y = math.asin(-m[2][0]) # 计算绕Y轴旋转角度
# 避免万向锁
if abs(math.cos(theta_y)) > 1e-6:
theta_z = math.atan2(m[1][0], m[0][0]) # 绕Z轴角度
theta_x = math.atan2(m[2][1], m[2][2]) # 绕X轴角度
else:
theta_z = 0.0
theta_x = math.atan2(-m[0][1], m[1][1])
# 转换为角度并返回
return (
math.degrees(theta_x),
math.degrees(theta_y),
math.degrees(theta_z)
)
# 示例用法
matrix = [
[0.0, -1.0, 0.0],
[1.0, 0.0, 0.0],
[0.0, 0.0, 1.0]
]
print(extract_euler_angles(matrix)) # 输出应接近 (0.0, 0.0, 90.0)
# 示例使用
target_cam = np.array([0.7, 0.2, 0.4]) # 摄像头坐标系下的目标位置
target_orient = np.eye(3) # 目标姿态(需根据实际情况设置)
solutions = inverse_kinematics(target_cam, target_orient)
print("关节角度解:", solutions)```解释代码,给出数学原理和相关尺寸单位