一、旋转矩阵变换 

坐标变换

二、算法代码（MATLAB） 

D-H参数
                            单位:mm
    关节转角    关节距离    连杆长度        转角
    Theta(n)     d(n)       a(n-1)     Alpha(n-1)    
    theta1        0           0             0
    theta2        0           54           pi/2
    theta3        0           61            0
      0           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);
}