ABB_2600运动学

最新推荐文章于 2024-12-17 20:56:28 发布

狗哥的程序手册

最新推荐文章于 2024-12-17 20:56:28 发布

阅读量616

点赞数 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;