二连杆机械臂角度解算

二连杆机械臂角度解算

本文采用极坐标的平移变换公式或者余弦定理的方法在定坐标点的情况下去解算二连杆机械臂两个关节处应该旋转的角度。由于余弦定理更容易理解且极坐标的平移变换公式和余弦定理推导出来的角度解算结果公式一致，接下来会用余弦定理去推导最终公式。
我们要解决的问题是已知一个目标点坐标(x，y)，已知两个连杆的长度OA，AB，我们的目标是求α和β这两个关节角。如下图所示：

根据同位角关系可知β=α-γ，所以我们只需要求出角α和角γ即可。角α可分为角1和角θ相加。

由于B点坐标已经给出，由$arctan(\frac{y}{x})$可求出角θ，角1由余弦定理 $cos\angle 1=(\frac{O{A}^2+O{B}^2-A{B}^2}{2\times OA\times OB})$可得

由于B点纵坐标y=OAsin⁡α+ABsin⁡γ。角α已知y已知，可求角γ。由于β=α-γ，角β可求。
下面给出具体C语言代码（代码中的角度全部都是弧度制）：

#define L1 105.0f
#define L2 145.0f    //此处L1，L2为上文的OA和AB
#define LIMIT(x,min,max) (x)=(((x)<=(min))?(min):(((x)>=(max))?(max):(x)))
//坐标系
typedef struct
{
    
    
	int16_t x;
	int16_t y;
	int32_t sqr_rho;
	float rho;
	float theta;
}Coordinate;

typedef struct
{
    
    
	float alpha;
	float beta;
}MchArmAngle;


//直角坐标转为极坐标
void CordTF(Coordinate* cord , int16_t x , int16_t y)
{
    
    
	cord->x = x;
	cord->y = y;
	cord->sqr_rho = x*x + y*y;
	cord->rho = sqrt((float)cord->sqr_rho);
	cord->theta = (float)atan2((double)y,(double)x);
}

//机械臂角度解算
void AngleCalc(MchArmAngle* angle,Coordinate* cord)
{
    
    
	if((cord->sqr_rho<(L1+L2)*(L1+L2))&&(cord->sqr_rho>(L1-L2)*(L1-L2)))
	{
    
    
		float temp1,temp2;
		temp1 = (cord->sqr_rho + L1*L1 - L2*L2)/(2*L1*cord->rho);
		angle->alpha = cord->theta + acos(temp1); //此处的加号是下文提到的加号
		temp2 = ((float)cord->y - L1*sin(angle->alpha))/L2;
		angle->beta = asin(temp2) - angle->alpha + PI/2.0f;
		LIMIT(angle->alpha,0,PI);
		LIMIT(angle->beta,0,PI);
	}
	return;
}

CordTF这个函数意义主要是计算出OB和角θ。由于OB和角θ相当于B点的极坐标，所以函数叫坐标变换。
AngleCalc函数if是因为假如机械臂两个关节是可以360度旋转的，那么B点可以到的范围就是一个圆环，加入这个if就是防止人为输入一个机械臂不可能达到的点，导致角度解算出一个NAN值。
细心的同学可能发现了在计算角α的时候有一个用到了一个acos，我们都知道cos是一个偶函数，所以其实在解算过程中到达指定坐标的角度其实是两组值，如图所示：

将代码中注释提到的加号改成减号就可以让到达B点的方式由上面的情况变成下面的情况。同时在解算角γ用到的是sin函数而不是cos函数也是这个原因。
重点： 此函数只适用于1，4象限角度的解算，对于2，3象限角γ的解算会有问题。不过由于我们的机械臂在底端加了yaw轴，将1,4象限旋转180度就变成了2,3象限。所以可以忽略此问题。（2,3象限我也做过推导，出现问题的原因是因为arcsin的值域问题。感兴趣的同学可以自行推导尝试。）
最后解释一下两个LIMIT的意思，由于这个二连杆机械臂是由舵机驱动的，但舵机能够旋转的范围只有0-180度，超过这个范围舵机就会松掉，为了防止给出的坐标解算出的角度超过这个范围，导致舵机松掉才加上这个宏，来限制解算出的角度范围。