六轴机械臂角度的计算

gaohaikuo1

机械臂的示意图如下，左图为前视图，右图为顶视图

根据上图可以写出根据末端坐标推出各偏转角的程序，需要注意的是，计算完成后需要根据实际舵机偏转加入角度偏置

1. getThetaFromXYZ(dobule x_D, double y_D, double z_D)
   
2. {
   
3.         //输入量为D的坐标x_D,y_D,z_D
   
4. 
   
5.         //定义变量
   
6.         double d_D,d_C,z_D,z_C;
   
7.         double angle_BAC,angle_BCA,angle_CAM;
   
8.         double alpha1,alpha2,alpha3;//机械臂和水平线夹角
   
9.         double theta0,theta1,theta2,theta3;//机械臂相对于上一机械臂的偏角
   
10.         double edge_AC,edge_AC2;
    
11.        
    
12.         //输入臂长
    
13.         double l1 = 50;
    
14.         double l2 = 50;
    
15.         double l3 = 70;       
    
16.        
    
17.         //规定范围,这几个初始值根据情况调整
    
18.         double d_D_max = 100;//最远距离
    
19.         double d_D_min = 10;//最近距离
    
20.         double alpha3_max= -30;//最远距离对应的末臂角度（相对于水平线）
    
21.         double alpha3_min = -90;//最近距离对应的末臂角度，-90就是垂直向下
    
22.        
    
23.         //alpha3由以下差值获得
    
24.         d_D=sqrt(x_D^2+y_D^2);
    
25.         alpha3 = alpha3_min + (d_D-d_D_min)/(d_D_max-d_D_min)*(alpha3_max-alpha3_min);
    
26.        
    
27.         //进一步推得C 坐标
    
28.         z_C = z_D – l3*sin(alpha3);
    
29.         d_C = d_D – l3*cos(alpha3);
    
30. 
    
31.         //进一步获得角度CAM的大小和边AC的长度
    
32.         angle_CAM = atan(z_C/d_C);
    
33.         edge_AC2 = z_C^2 + d_C^2; //记录中间量，减小计算量
    
34.         edge_AC = sqrt(edge_AC2);
    
35. 
    
36.         //在三角形ABC中运用余弦定理，可求得其三个内角
    
37.         angle_BAC = acos((l1^2+ edge_AC2 – l2^2)/(2*l1*edge_AC));
    
38.         angle_BCA = acos((l2^2+ edge_AC2 – l1^2)/(2*l2*edge_AC));
    
39. 
    
40.         //进而可推出AB和BC段机械臂和水平的夹角
    
41.         alpha1 = angle_CAM + angle_BAC;
    
42.         alpha2 = angle_CAM – angle_BCA;
    
43. 
    
44.         //因此我们可以得出机械臂的偏转角度（假设垂直向上为初始状态, 水平转舵机正前方为初始状态）
    
45.         theta1 = 90 - alpha1；
    
46.         theta2 = alpha1 - alpha2;
    
47.         theta3 = alpha2 - alpha3;
    
48.         theta0 = atan(x_D/y_D);
    
49. }
    

复制代码

gaohaikuo1

ppt也附上