一、旋转矩阵变换
坐标变换
二、算法代码（MATLAB）
D-H参数单位:mm关节转角 关节距离 连杆长度 转角Theta(n) d(n) a(n-1) Alpha(n-1) theta1 0 0 0theta2 0 54 pi/2theta3 0 61 00 0 155 0// 正运动解算
x = cos(theta1) * (L1 + L3 * cos(theta2 + theta3) + L2 * cos(theta2));
y = sin(theta1) * (L1 + L3 * cos(theta2 + theta3) + L2 * cos(theta2));
z = L3 * sin(theta2 + theta3) + L2 * sin(theta2);// 逆运动解算
L1 = 0.054; %单位m
L2 = 0.061;
L3 = 0.155;
R = sqrt(x * x + y * y);
aerfaR = atan2(-z, R - L1); %使用atan2以获得正确的象限
Lr = sqrt(z * z + (R - L1) * (R - L1));
aerfa1 = acos((L2 * L2 + Lr * Lr - L3 * L3) / (2 * Lr * L2));
theta1_new = atan2(y, x); %atan2自动处理y=0的情况
theta2_new = aerfa1 - aerfaR;
aerfa2 = acos((Lr * Lr + L3 * L3 - L2 * L2) / (2 * Lr * L3));
theta3_new = -(aerfa1 + aerfa2);三、正逆运动学（C语言）
// 正运动学解算(输入关节角度计算足端坐标)
void Forward_kinematics(double theta1, double theta2, double theta3, uint8_t leg)
{ Myaxis_Init(&Pi3_axis[leg]);Pi3_axis[leg].x = cos(theta1) * (L1 + L3 * cos(theta2 + theta3) + L2 * cos(theta2));Pi3_axis[leg].y = sin(theta1) * (L1 + L3 * cos(theta2 + theta3) + L2 * cos(theta2));Pi3_axis[leg].x = L3 * sin(theta2 + theta3) + L2 * sin(theta2);
}
// 逆运动学解算(根据足端坐标计算出三个角度rad)
void Inverse_Kinematics(double x, double y, double z, uint8_t leg)
{Hexapod_thetas_Init(&Hexapod_leg[leg]);double R = sqrt(x * x + y * y);double aerfaR = atan2(-z, R - L1); // 使用atan2以获得正确的象限double Lr = sqrt(z * z + (R - L1) * (R - L1));double aerfa1 = acos((L2 * L2 + Lr * Lr - L3 * L3) / (2 * Lr * L2));Hexapod_leg[leg].Theta[0] = atan2(y, x); // atan2自动处理y=0的情况Hexapod_leg[leg].Theta[1] = aerfa1 - aerfaR;double aerfa2 = acos((Lr * Lr + L3 * L3 - L2 * L2) / (2 * Lr * L3));Hexapod_leg[leg].Theta[2] = -(aerfa1 + aerfa2);
}