少有人走的路几何法逆解
从末端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
至此，逆解的角度值全部计算完毕，下一步需要求解每个关节运动过程中的转动速度
本文出自勇哥的网站《少有人走的路》wwww.skcircle.com，转载请注明出处！讨论可扫码加群：
