二連桿機械臂角度解算

本文采用極坐標的平移變換公式或者余弦定理的方法在定坐標點的情況下去解算二連桿機械臂兩個關節處應該旋轉的角度，由于余弦定理更容易理解且極坐標的平移變換公式和余弦定理推匯出來的角度解算結果公式一致，接下來會用余弦定理去推導最終公式，
我們要解決的問題是已知一個目標點坐標(x，y)，已知兩個連桿的長度OA，AB，我們的目標是求α和β這兩個關節角，如下圖所示：

根據同位角關系可知β=α-γ，所以我們只需要求出角α和角γ即可，角α可分為角1和角θ相加，

由于B點坐標已經給出，由 a r c t a n ? ( y x ) arctan?(\frac{y}{x}) arctan?(xy?)可求出角θ，角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}) cos∠1=(2×OA×OBOA2+OB2?AB2?)可得

由于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度，超過這個范圍舵機就會松掉，為了防止給出的坐標解算出的角度超過這個范圍，導致舵機松掉才加上這個宏，來限制解算出的角度范圍，