4、物品抓取（6自由度机械臂逆运动学）

目录

1.坐标系建立

2.运用解析法计算各个舵机旋转角度

​3.举例 

---

1.坐标系建立

        采用笛卡尔坐标系图1，即由三个互相垂直的坐标轴所组成的坐标系，以机械臂正向为X轴方向，横向为Y轴方向，纵向为Z轴方向。

图1 笛卡尔坐标系

2.运用解析法计算各个舵机旋转角度

3.举例 

         一定要看懂2、再来看例子，如果实在看不懂2、，可以打开两个网页，2、和3、对比这来看，更容易理解。

//函数中：
//x,y,z分别为所要抓取物品的三维坐标，
//Alpha为L3与水平方向的夹角 
//kinematics_t为定义的结构体类型，用来存储机械臂的四节臂的长度，*kinematics为定义的结构体指针
//为什么6自由度机械臂只有四节臂的长度，看标题2、的开头部分

uint8_t kinematics_analysis(float x, float y, float z, float Alpha, kinematics_t *kinematics) {
	float theta0, theta1, theta2, theta3;	//存储D0-D3四个舵机的旋转角度
	float l0, l1, l2, l3;					//设置变量存储四个关节长度
	float aaa, bbb, ccc, zf_flag;			//中间变量，往下看
	
	//将得到的坐标放大10倍
	x = x*10;
	y = y*10;
	z = z*10;
	
	//将四个关节长度分别赋值，方便使用
	l0 = kinematics->L0;
	l1 = kinematics->L1;
	l2 = kinematics->L2;
	l3 = kinematics->L3;

/*****对应2、的第二步*************************************************************/
	//如果平面坐标系x的坐标为0，则视觉机械臂中的theta0角置0
	//theta0角为平面坐标系x与y的夹角
	//否则则得到xy之间的角度（弧度制）
	if(x == 0)
	{
		theta0 = 0.0;
	} 
	else
	{
		theta0 = atan(x/y)*180.0/pi;
	}

/*****对应2、的第三步*************************************************************/
	//算平方根
	y = sqrt(x*x + y*y);				//公式（4-3）    sqrt函数是算平方根    
    y = y-l3 * cos(Alpha*pi/180.0);  	//公式（4-5）
    z = z-l0-l3*sin(Alpha*pi/180.0); 	//公式（4-6）
	
	//
    if(z < -l0) {
        return 1;
	}
	//
    if(sqrt(y*y + z*z) > (l1+l2)) {
        return 2;
	}
	//
	ccc = acos(y / sqrt(y * y + z * z));    //ccc为β1的角度，公式（4-8），图4.6
    bbb = (y*y+z*z+l1*l1-l2*l2)/(2*l1*sqrt(y*y+z*z));    //公式（4-9）bbb是cosβ2
	//由于cos只能在-1和1之间，所以有以下判断
    if(bbb > 1 || bbb < -1) {
        return 5;
	}
	//
    if (z < 0) {
        zf_flag = -1;
	} else {
        zf_flag = 1;
	}
	//
    theta1 = ccc * zf_flag + acos(bbb);    //公式（4-10）
    theta1 = theta1 * 180.0 / pi;
	//
    if(theta1 > 180.0 || theta1 < 0.0) {
        return 6;
	}
	//

/******对应2、的第四步************************************************************/
    aaa = -(y*y+z*z-l1*l1-l2*l2)/(2*l1*l2);    //公式（4-11），aaa为cosβ3
    if (aaa > 1 || aaa < -1) {
        return 3;
	}
    theta2 = acos(aaa); 
    theta2 = 180.0 - theta2 * 180.0 / pi ;  
	//
    if (theta2 > 135.0 || theta2 < -135.0) {
        return 4;
	}
	//

/******对应2、的第五步************************************************************/
    theta3 = Alpha - theta1 + theta2;        //公式（4-13）
    if(theta3 > 90.0 || theta3 < -90.0) {
        return 7;
	}
	
//到此，D0-D3四个舵机的角度已经解出，分别给相应控制舵机的定时器通道赋值即可

	kinematics->servo_angle[0] = theta0;
	kinematics->servo_angle[1] = theta1-90;
	kinematics->servo_angle[2] = theta2;
	kinematics->servo_angle[3] = theta3;    
	
	kinematics->servo_pwm[0] = (int)(1500+2000.0 * kinematics->servo_angle[0] / 270.0);
	kinematics->servo_pwm[1] = (int)(1500+2000.0 * kinematics->servo_angle[1] / 270.0);
	kinematics->servo_pwm[2] = (int)(1500+2000.0 * kinematics->servo_angle[2] / 270.0);
	kinematics->servo_pwm[3] = (int)(1500+2000.0 * kinematics->servo_angle[3] / 270.0);

	return 0;	//返回0之后舵机才会行动
}

        将1-4这四节整理，再编写总逻辑，即可完成物流抓取搬运机器人设计。

物流抓取机器人整体设计方案-CSDN博客 

1、巡线功能实现（7路数字循迹）-CSDN博客 

2、目标识别（颜色识别）-CSDN博客 

3、目标定位（视觉测距）-CSDN博客