ABB_2600运动学

狗哥的程序手册

于 2024-06-27 08:59:09 发布

阅读量418

点赞数 4

文章标签：算法 c++ c# 机器人

本文链接：https://blog.csdn.net/jjjjjjjbbee/article/details/140003553

版权

DH参数建模

图ABB_2600_1.65_20G DH参数

说明：ABB采用MDH参数进行建模，注意MDH(改进DH参数)和DH(标准DH参数的区别)

ABB_2600的正运动学分析

2600采用改进DH参数进行建模，其所对应的正运动学方程为：

其中：

通过机器人正运动学方程我们可以求解出机器人末端相对于基座标系的旋转矩阵，通过公式可以进一步转化为位置+姿态的表达形式

机器人正运动学代码：


/*--------------------------ABB_2600DH参数表---------------------------*/
        double d0 = 445;double d1 = 0;double d2 = 0;double d3 = 795;double d4 = 0;double d5 = 85;
        double a0 = 0;double a1 = 150;double a2 = -700;double a3 = -115;double a4 = 0;double a5 = 0;
        double alpha0 = 0;double alpha1 = -Math.PI / 2; double alpha2 = 0;
        double alpha3 = Math.PI / 2; double alpha4 = -Math.PI / 2; double alpha5 = Math.PI / 2;

        /*---------------------------------------------------------------------*/

        /*---------------------------ABB_2600连杆矩阵--------------------------*/
        double[,] T01 = new double[4, 4];
        double[,] T12 = new double[4, 4];
        double[,] T23 = new double[4, 4];
        double[,] T34 = new double[4, 4];
        double[,] T45 = new double[4, 4];
        double[,] T56 = new double[4, 4];//末端变换矩阵
        /*---------------------------------------------------------------------*/

        MatrixMath Kine_MatrixMath = new MatrixMath();
        

        /// <summary>
        /// 机器人正运动学
        /// </summary>
        /// <param name="theta"></param>
        /// <returns></returns>
        public double[,] Rob_Fkine()
        {                     
            double[,] T02 = Kine_MatrixMath.MatrixMult(T01, T12);
            double[,] T03 = Kine_MatrixMath.MatrixMult(T02, T23);
            double[,] T04 = Kine_MatrixMath.MatrixMult(T03, T34);
            double[,] T05 = Kine_MatrixMath.MatrixMult(T04, T45);
            double[,] T06 = Kine_MatrixMath.MatrixMult(T05, T56);           
            return T06;
        }

        /// <summary>
        /// 机器人连杆参数矩阵相乘
        /// </summary>
        public void M_Trans_Mult(double[] theta)
        {
            //T01连杆参数矩阵
            T01[0, 0] =Math.Round(Math.Cos(theta[0]),4);
            T01[0, 1] =Math.Round(-Math.Sin(theta[0]),4);
            T01[0, 2] = 0;
            T01[0, 3] = a0;
            T01[1, 0] = Math.Round((Math.Sin(theta[0]) * Math.Cos(alpha0)),4);
            T01[1, 1] = Math.Round((Math.Cos(theta[0]) * Math.Cos(alpha0)), 4);
            T01[1, 2] = Math.Round(-Math.Sin(alpha0));
            T01[1, 3] = Math.Round((-Math.Sin(alpha0) * d0), 4);
            T01[2, 0] = Math.Round((Math.Sin(theta[0]) * Math.Sin(alpha0)), 4);
            T01[2, 1] = Math.Round((Math.Cos(theta[0]) * Math.Sin(alpha0)), 4);
            T01[2, 2] = Math.Round(Math.Cos(alpha0), 4);
            T01[2, 3] = Math.Round((Math.Cos(alpha0) * d0), 4);
            T01[3, 0] = 0;
            T01[3, 1] = 0;
            T01[3, 2] = 0;
            T01[3, 3] = 1;
            //T12连杆参数矩阵
            T12[0, 0] = Math.Round(Math.Cos(theta[1]), 4);
            T12[0, 1] = Math.Round(-Math.Sin(theta[1]), 4);
            T12[0, 2] = 0;
            T12[0, 3] = a1;
            T12[1, 0] = Math.Round((Math.Sin(theta[1]) * Math.Cos(alpha1)), 4);
            T12[1, 1] = Math.Round((Math.Cos(theta[1]) * Math.Cos(alpha1)), 4);
            T12[1, 2] = Math.Round(-Math.Sin(alpha1));
            T12[1, 3] = Math.Round((-Math.Sin(alpha1) * d1), 4);
            T12[2, 0] = Math.Round((Math.Sin(theta[1]) * Math.Sin(alpha1)), 4);
            T12[2, 1] = Math.Round((Math.Cos(theta[1]) * Math.Sin(alpha1)), 4);
            T12[2, 2] = Math.Round(Math.Cos(alpha1), 4);
            T12[2, 3] = Math.Round((Math.Cos(alpha1) * d1), 4);
            T12[3, 0] = 0;
            T12[3, 1] = 0;
            T12[3, 2] = 0;
            T12[3, 3] = 1;
            //T23连杆参数矩阵
            T23[0, 0] = Math.Round(Math.Cos(theta[2]), 4);
            T23[0, 1] = Math.Round(-Math.Sin(theta[2]), 4);
            T23[0, 2] = 0;
            T23[0, 3] = a2;
            T23[1, 0] = Math.Round((Math.Sin(theta[2]) * Math.Cos(alpha2)), 4);
            T23[1, 1] = Math.Round((Math.Cos(theta[2]) * Math.Cos(alpha2)), 4);
            T23[1, 2] = Math.Round(-Math.Sin(alpha2));
            T23[1, 3] = Math.Round((-Math.Sin(alpha2) * d2), 4);
            T23[2, 0] = Math.Round((Math.Sin(theta[2]) * Math.Sin(alpha2)), 4);
            T23[2, 1] = Math.Round((Math.Cos(theta[2]) * Math.Sin(alpha2)), 4);
            T23[2, 2] = Math.Round(Math.Cos(alpha2), 4);
            T23[2, 3] = Math.Round((Math.Cos(alpha2) * d2), 4);
            T23[3, 0] = 0;
            T23[3, 1] = 0;
            T23[3, 2] = 0;
            T23[3, 3] = 1;
            //T34连杆参数矩阵
            T34[0, 0] = Math.Round(Math.Cos(theta[3]), 4);
            T34[0, 1] = Math.Round(-Math.Sin(theta[3]), 4);
            T34[0, 2] = 0;
            T34[0, 3] = a3;
            T34[1, 0] = Math.Round((Math.Sin(theta[3]) * Math.Cos(alpha3)), 4);
            T34[1, 1] = Math.Round((Math.Cos(theta[3]) * Math.Cos(alpha3)), 4);
            T34[1, 2] = Math.Round(-Math.Sin(alpha3));
            T34[1, 3] = Math.Round((-Math.Sin(alpha3) * d3), 4);
            T34[2, 0] = Math.Round((Math.Sin(theta[3]) * Math.Sin(alpha3)), 4);
            T34[2, 1] = Math.Round((Math.Cos(theta[3]) * Math.Sin(alpha3)), 4);
            T34[2, 2] = Math.Round(Math.Cos(alpha3), 4);
            T34[2, 3] = Math.Round((Math.Cos(alpha3) * d3), 4);
            T34[3, 0] = 0;
            T34[3, 1] = 0;
            T34[3, 2] = 0;
            T34[3, 3] = 1;
            //T45连杆参数矩阵
            T45[0, 0] = Math.Round(Math.Cos(theta[4]), 4);
            T45[0, 1] = Math.Round(-Math.Sin(theta[4]), 4);
            T45[0, 2] = 0;
            T45[0, 3] = a4;
            T45[1, 0] = Math.Round((Math.Sin(theta[4]) * Math.Cos(alpha4)), 4);
            T45[1, 1] = Math.Round((Math.Cos(theta[4]) * Math.Cos(alpha4)), 4);
            T45[1, 2] = Math.Round(-Math.Sin(alpha4));
            T45[1, 3] = Math.Round((-Math.Sin(alpha4) * d4), 4);
            T45[2, 0] = Math.Round((Math.Sin(theta[4]) * Math.Sin(alpha4)), 4);
            T45[2, 1] = Math.Round((Math.Cos(theta[4]) * Math.Sin(alpha4)), 4);
            T45[2, 2] = Math.Round(Math.Cos(alpha4), 4);
            T45[2, 3] = Math.Round((Math.Cos(alpha4) * d4), 4);
            T45[3, 0] = 0;
            T45[3, 1] = 0;
            T45[3, 2] = 0;
            T45[3, 3] = 1;
            //T56连杆参数矩阵
            T56[0, 0] = Math.Round(Math.Cos(theta[5]), 4);
            T56[0, 1] = Math.Round(-Math.Sin(theta[5]), 4);
            T56[0, 2] = 0;
            T56[0, 3] = a5;
            T56[1, 0] = Math.Round((Math.Sin(theta[5]) * Math.Cos(alpha5)), 4);
            T56[1, 1] = Math.Round((Math.Cos(theta[5]) * Math.Cos(alpha5)), 4);
            T56[1, 2] = Math.Round(-Math.Sin(alpha5));
            T56[1, 3] = Math.Round((-Math.Sin(alpha5) * d5), 4);
            T56[2, 0] = Math.Round((Math.Sin(theta[5]) * Math.Sin(alpha5)), 4);
            T56[2, 1] = Math.Round((Math.Cos(theta[5]) * Math.Sin(alpha5)), 4);
            T56[2, 2] = Math.Round(Math.Cos(alpha5), 4);
            T56[2, 3] = Math.Round((Math.Cos(alpha5) * d5), 4);
            T56[3, 0] = 0;
            T56[3, 1] = 0;
            T56[3, 2] = 0;
            T56[3, 3] = 1;
        }

ABB_2600逆运动分析

逆运动学采用解析解的方式求解机器人各轴角度，主要利用到的求解关系有：

求解

记最终位姿矩阵为，则有关系：

通过该关系式则可以求得的值

求解和

可利用关系式：

通过该关系式则可以求得和

求解、和

可利用关系式：

通过该关系式则可以求得、和

机器人逆运动学代码：


1.public double[] Rob_Ikine(double[,] T)
2.        {
3.            /*--------------------------------求解theta1-----------------------------------*/
4.            double[] theta = new double[6];
5.            //arctan值域为(-pi/2,pi/2);
6.            //atctan2值域为(-pi,pi);
7.            theta[0] = Math.Atan(((T[1, 2] * d5) - T[1, 3])/((T[0, 2] * d5) - T[0, 3]));
8.            /*-----------------------------------------------------------------------------*/
9.
10.            /*--------------------------------求解theta2-----------------------------------*/
11.            double k1 = a1 - ((T[1, 3] - (d5 * T[1, 2])) / Math.Sin(theta[0]));
12.            double k2 = d0 - (T[2, 3] - (d5 * T[2, 2]));
13.            double y = (k1 * k1) + (k2 * k2) + (a2 * a2) - (a3 * a3) - (d3 * d3);
14.            double x = 2 * a2 * Math.Sqrt((k1 * k1) + (k2 * k2));
15.            double xx = 2 * a2 * Math.Sqrt(2);
16.            double fi_sin= Math.Asin(k2 / (Math.Sqrt((k1 * k1) + (k2 * k2))));
17.            double fi_cos = Math.Cos(k1 / (Math.Sqrt((k1 * k1) + (k2 * k2))));
18.            double fi = Math.Atan2(fi_sin, fi_cos);
19.            theta[1] = Math.Asin(y/x) - fi_sin;
20.            theta[1] = -theta[1];
21.            /*-----------------------------------------------------------------------------*/
22.
23.            /*--------------------------------求解theta3-----------------------------------*/
24.           
25.            /*-----------------------------------------------------------------------------*/
26.
27.            /*--------------------------------求解theta5-----------------------------------*/
28.           
29.            /*-----------------------------------------------------------------------------*/
30.
31.            /*--------------------------------求解theta4-----------------------------------*/
32.            
33.            /*-----------------------------------------------------------------------------*/
34.
35.            /*--------------------------------求解theta6-----------------------------------*/
36.           
37.            /*-----------------------------------------------------------------------------*/
38.
39.            for(int i = 0; i < 6; i++)
40.            {
41.                theta[i] = theta[i] * (180 / Math.PI);
42.            }
43.            return theta;
44.        }

说明：逆解在运动学中较为重要，希望可以通过自己手推的方式推出各关节角度，所以再此代码只给出两个角度的示例，后续的通过自己推导公式并求出和写出相应的代码

姿态表达方式转化

ABB_2600采用四元数对姿态进行表达，我们上述求解的正运动学方程最终结果是以齐次变换矩阵作为输出，所以我们需要将旋转矩阵转化为四元数

该内容原理再此不多做阐述，附上原理链接：

https://www.cnblogs.com/flyinggod/p/8144100.html

旋转矩阵转四元数代码：


1.public double Max(double x,double y,double z)
2.        {
3.            double t = x;
4.            if (y > t)
5.            {
6.                t = y;
7.            }
8.            if (z > t)
9.            {
10.                t = z;
11.            }
12.            return t;               
13.        }
14.
15.        /// <summary>
16.        /// 旋转矩阵转四元数
17.        /// </summary>
18.        /// <param name="T"></param>
19.        /// <returns></returns>
20.        public double[] T2Q(double[,] T)
21.        {
22.            double[] Quate = new double[4];
23.            double tr = T[0, 0] + T[1, 1] + T[2, 2];
24.            double max = Max(T[0, 0], T[1, 1], T[2, 2]);
25.            //if max(r11,r22,r33) = r11
26.            double t1 = Math.Sqrt(1 + T[0, 0] - T[1, 1] - T[2, 2]);
27.            //if max(r11,r22,r33) = r22
28.            double t2 = Math.Sqrt(1 - T[0, 0] + T[1, 1] - T[2, 2]);
29.            //if max(r11,r22,r33) = r33
30.            double t3 = Math.Sqrt(1 - T[0, 0] - T[1, 1] + T[2, 2]);
31.            if (tr != 0 && tr + 1 > 0)
32.            {
33.                Quate[0] = Math.Sqrt(tr + 1);
34.                Quate[1] = (T[2, 1] - T[1, 2]) / (4 * Quate[0]);
35.                Quate[2] = (T[0, 2] - T[2, 0]) / (4 * Quate[0]);
36.                Quate[3] = (T[1, 0] - T[0, 1]) / (4 * Quate[0]);
37.            }
38.            else if (max == T[0,0])
39.            {
40.                Quate[0] = (T[2, 1] - T[1, 2]) / t1;
41.                Quate[1] = t1 / 4;
42.                Quate[2] = (T[0, 2] + T[2, 0]) / t1;
43.                Quate[3] = (T[0, 1] + T[1, 0]) / t1;
44.            }
45.            else if (max == T[1,1])
46.            {
47.                Quate[0] = (T[0, 2] - T[2, 0]) / t2;
48.                Quate[1] = (T[0, 1] + T[1, 0]) / t2;
49.                Quate[2] = t2 / 4;
50.                Quate[3] = (T[2, 1] + T[1, 2]) / t2;
51.            }
52.            else if(max == T[2,2])
53.            {
54.                Quate[0] = (T[1, 0] - T[0, 1]) / t3;
55.                Quate[1] = (T[0, 2] + T[2, 0]) / t3;
56.                Quate[2] = (T[1, 2] - T[2, 1]) / t3;
57.                Quate[3] = t3 / 4;
58.            }
59.            return Quate;
60.        }

实验结果

1.正解验证：

左边为机器人实际位姿，右边为运动学求解出位姿

2.逆解验证

上面text文本框为验证解，下面文本框为算法输出解

MtaLab代码

代码段1：

1.L(1)= Link ([0 445 0 0 0],'modified');
2.L(2)= Link ([0 0 150 -pi/2 pi/2],'modified');
3.L(2).offset = pi/2;
4.L(3)= Link ([0 0 -700 0 0],'modified');
5.L(4)= Link ([0 795 -115 pi/2 0],'modified');
6.L(5)= Link ([0 0 0 -pi/2],'modified');
7.L(6)= Link ([0 85 0 pi/2 0],'modified');
8.
9.ABB_2600 = SerialLink(L);
10.% ABB_2600.teach();
11.ABB_2600.display();
12.theta = [20 60 50 10 30 40];
13.theta1 = theta*(pi/180);
14.T1 = ABB_2600.fkine(theta1);
15.display(T1);
16.
17.T01 = [1 0 0 0;0 1 0 0;0 0 1 445;0 0 0 1];
18.T12 = [0 -1 0 150;0 0 1 0;-1 0 0 0;0 0 0 1];
19.T23 = [1 0 0 -700;0 1 0 0;0 0 1 0;0 0 0 1];
20.T34 = [1 0 0 -115;0 0 -1 -795;0 1 0 0;0 0 0 1];
21.T45 = [1 0 0 0;0 0 1 0;0 -1 0 0;0 0 0 1];
22.T56 = [1 0 0 0;0 0 -1 -85;0 1 0 0;0 0 0 1];
23.T06 = T01*T12*T23*T34*T45*T56;
24.display(T06)
25.
26.theta1 = ABB_2600.ikunc(T1);
27.display(theta1*(180/pi));

代码段2：

1.clear all
2.clc
3.syms t1 t2 t3 t4 t5 t6
4.syms a0 a1 a2 a3 a4 a5
5.syms d1 d2 d3 d4 d5 d6
6.syms ap0 ap1 ap3 ap4 ap5 ap6
7.syms nx ny nz ox oy oz ax ay az px py pz;
8.Tend = [nx ox ax px;ny oy ay py;nz oz az pz; 0 0 0 1];
9.T1 = [cos(t1) -sin(t1) 0 0;sin(t1) cos(t1) 0 0;0 0 1 d1;0 0 0 1];
10.Tinv1 = [cos(t1) sin(t1) 0 0;-sin(t1) cos(t1) 0 0;0 0 1 -d1;0 0 0 1];
11.T2 = [-sin(t2) -cos(t2) 0 a1;0 0 1 0;-cos(t2) sin(t2) 1 0;0 0 0 1];
12.Tinv2 = [-sin(t2) cos(t2) -cos(t2) a1*sin(t2);-cos(t2) -sin(t2) sin(t2) a1*cos(t2);0 1 0 0;0 0 0 1];
13.T3 = [cos(t3) -sin(t3) 0 a2;sin(t3) cos(t3) 0 0;0 0 1 0;0 0 0 1];
14.Tinv3 = [cos(t3) sin(t3) 0 -a2*cos(t3);-sin(t3) cos(t3) 0 a2*sin(t3);0 0 1 0;0 0 0 1];
15.T4 = [cos(t4) -sin(t4) 0 a3;0 0 -1 -d4;sin(t4) cos(t4) 0 0;0 0 0 1];
16.Tinv4 = [cos(t4) 0 sin(t4) -a3*cos(t4);-sin(t4) 0 cos(t4) a3*sin(t4);0 -1 0 -d4;0 0 0 1];
17.T5 = [cos(t5) -sin(t5) 0 0;0 0 1 0;-sin(t5) -cos(t5) 0 0;0 0 0 1];
18.Tinv5 = [cos(t5) 0 -sin(t5) 0;-sin(t5) 0 -cos(t5) 0;0 1 0 0;0 0 0 1];
19.T6 = [cos(t6) -sin(t6) 0 0;0 0 -1 -d6;sin(t6) cos(t6) 0 0;0 0 0 1];
20.Tinv6 = [cos(t6) 0 sin(t6) 0;-sin(t6) 0 cos(t6) 0;0 -1 0 -d6;0 0 0 1];
21.
22.T03 = [-cos(t1)*sin(t2+t3) -cos(t1)*cos(t2+t3) -sin(t1) a1*cos(t1)-a2*cos(t1)*sin(t2);
23.       -sin(t1)*sin(t2+t3) -sin(t1)*cos(t2+t3)  cos(t1) a1*sin(t1)-a2*sin(t1)*sin(t2);
24.       -cos(t2+t3) sin(t2+t3) 1 d1-a2*cos(t2);
25.       0 0 0 1]
26.  %求解模块
27.  T_left_1 = Tinv1*Tend;
28.  T16 = T2*T3*T4*T5*T6;
29.  T_left_2 = Tend*Tinv6*Tinv5;
30.  display(T_left_2)