三轴机械臂逆解（几何法）

几何法逆解

几何推导

从末端A点开始逆推，

第一步：求B点坐标

已知A点坐标X,Y和俯仰角γ，可以通过一下公式求出B点坐标

Bx = val_x - a3*cos(pitch);

By = val_y - a3*sin(pitch);

第二步：求出理想的θ1

通过余弦定理公式计算出β

cosbeta = (Bx^2 + By^2 + a1^2 - a2^2)/(2*a1*sqrt(Bx^2+By^2));

beta = acos(cosbeta)*180/pi;

alpha = atan2(By,Bx)*180/pi;

所以得出

theta1 = alpha + beta

第三步：求出理想的θ2

通过余弦定理公式计算出（180 - θ2）

costheta2 = -(Bx^2 + By^2 - a1^2 - a2^2)/(2*a1*a2);

theta2 = -(180 - acos(costheta2)*180/pi)

第四步：求出θ3

通过

γ = θ1 + θ2 + θ3

求出

θ3 = γ- θ1- θ2

至此，逆解的角度值全部计算完毕，下一步需要求解每个关节运动过程中的转动速度