二连杆机械臂角度解算
二连杆机械臂角度解算 本文采用极坐标的平移变换公式或者余弦定理的方法在定坐标点的情况下去解算二连杆机械臂两个关节处应该旋转的角度。由于余弦定理更容易理解且极坐标的平移变换公式和余弦定理推导出来的角度解算结果公式一致,接下来会用余弦定理去推导最终公式。 我们要解决的问题是已知一个目标点坐标(x,y),已知两个连杆的长度OA,AB,我们的目标是求α和β这两个关节角。如下图所示: 根据同位角关系可知β=α-γ,所以我们只需要求出角α和角γ即可。角α可分为角1和角θ相加。 由于B点坐标已经给出,由 a r c t a n ( y x ) arctan(\frac{y}{x}) a r c t a n ( x y ) 可求出角θ,角1由余弦定理 c o s ∠ 1 = ( O A 2 + O B 2 − A B 2 2 × O A × O B ) cos∠1=(\frac{OA^2+OB^2-AB^2}{2×OA×OB}) c o s ∠ 1 = ( 2 × O A × O B O A 2 + O B 2 − A B 2 ) 可得 由于B点纵坐标y=OAsinα+ABsinγ。角α已知y已知,可求角γ。由于β=α-γ,角β可求。 下面给出具体C语言代码(代码中的角度全部都是弧度制): #define L1 105.0 f #define L2 145.0 f /