采用修正DH参数推导三菱6轴机械手逆运动学求解及c#程序

我现在正在SolidWorks中开发工业机械手动作及CT模拟的功能插件。目前实现在SolidWorks中获取机械手坐标信息，然后与三菱、爱普生品牌机械手仿真软件通信，仿真软件后台根据坐标点数据运算并反馈结果到SolidWorks中。功能基本实现，但是现在在SolidWorks中的坐标目前都是以关节坐标表示的。在SolidWorks中调试不太方便，所以研究了一下采用世界坐标参数的方法，需要用到机械手逆解方法，特记录一下逆解计算过程。

本逆解以三菱RV-13FR机械手为例。

修正DH参数如下。具体DH参数这里就不做说明了。

由于采用修正DH参数法，所以连杆变换矩阵如下。

带入DH参数可以得到：

由于： (1)

其中为已知

由等式（1）可以求得。

通过，计算。

由,可求得。

具体计算过程由于比较复杂，在此不能很好地表达。如有需要完整的计算过程可私信。

此计算有4个理论解。

可用C#代码如下，C#中矩阵运算需用到免费开源的MathNet.Numerics类.

public double[] IK_MI_MDH(double[] ptdata, double[] lastPt)
        {
            double[,] thetas = new double[8, 6];//4组解，每组解六个角
            double[] bestRst = new double[6];
            double x = ptdata[0] * 0.001;
            double y = ptdata[1] * 0.001;
            double z = ptdata[2] * 0.001;
            double nx = ptdata[3] / radToAgl;
            double ny = ptdata[4] / radToAgl;
            double nz = ptdata[5] / radToAgl;

            //1.计算末端旋转矩阵
            //Matrix<double> MR = ROT_z(nz) * ROT_y(ny) * ROT_x(nx);
            Matrix<double> MR = ROT_xyz(nx, ny, nz);
            Matrix<double> M6 = new DenseMatrix(4);
            M6.SetSubMatrix(0, 0, MR);
            M6.SetColumn(3, new double[] { x, y, z, 1 });

            //2.求解
            double wx, wy, wz;
            wx = x - MR[0, 2] * d[5];
            wy = y - MR[1, 2] * d[5];
            wz = z - MR[2, 2] * d[5];
            // theta1
            thetas[0, 0] = Math.Atan2(wy, wx);
            thetas[1, 0] = thetas[0, 0];
            thetas[2, 0] = thetas[0, 0];
            thetas[3, 0] = thetas[0, 0];
            //if (wy>=0)
            //{
            //    thetas[0, 0] = thetas[0, 0] - Math.PI;
            //}
            //else
            //{
            //    thetas[0, 0] = thetas[0, 0] + Math.PI;
            //}
            ////thetas[4, 0] = Math.Atan2(-wy , -wx );
            //thetas[1, 0] = thetas[4, 0];
            //thetas[2, 0] = thetas[4, 0];
            //thetas[3, 0] = thetas[4, 0];

            //theta3
            double k1 = d[0] - wz;
            double squ1 = a[3] * a[3] + d[3] * d[3];

            //thetas2
            for (int i = 0; i < 4; i += 2)
            {
                double k2 = wx * Math.Cos(thetas[i, 0]) + wy * Math.Sin(thetas[i, 0]) - a[1];
                double squ2 = k1 * k1 + k2 * k2;
                double k3 = (squ2 - a[2] * a[2] - squ1) / a[2] / 2;
                if (squ1 - k3 * k3 < 0)
                {
                    //无解
                    continue;
                }
                if (i % 4 == 0)
                {
                    thetas[i, 2] = Math.Atan2(a[3], d[3]) - Math.Atan2(k3, Math.Sqrt(squ1 - k3 * k3));
                    thetas[i + 1, 2] = thetas[i, 2];
                    thetas[i + 2, 2] = Math.Atan2(a[3], d[3]) - Math.Atan2(k3, -Math.Sqrt(squ1 - k3 * k3));
                    thetas[i + 3, 2] = thetas[i + 2, 2];
                }

                double k4 = Math.Cos(thetas[i, 2]) * d[3] + Math.Sin(thetas[i, 2]) * a[3];
                double k5 = Math.Cos(thetas[i, 2]) * a[3] - Math.Sin(thetas[i, 2]) * d[3] + a[2];
                thetas[i, 1] = Math.Atan2((k1 * k5 - k2 * k4) / squ2, (k1 * k4 + k2 * k5) / squ2);
                thetas[i + 1, 1] = thetas[i, 1];
            }

            //theta5、theta4、theta6
            for (int i = 0; i < 4; i++)
            {
                //求T1.T2.T3的逆矩阵
                Matrix<double> M1 = MDH_T(alpha[0] / radToAgl, a[0], d[0], thetas[i, 0]);
                Matrix<double> M2 = MDH_T(alpha[1] / radToAgl, a[1], d[1], thetas[i, 1]);
                Matrix<double> M3 = MDH_T(alpha[2] / radToAgl, a[2], d[2], thetas[i, 2]);
                Matrix<double> M321 = (M1 * M2 * M3).Inverse();
                //求M456
                Matrix<double> M456 = M321 * M6;
                //theta5
                if (i % 2 == 0)
                {
                    thetas[i, 4] = Math.Acos(M456[1, 2]);
                    thetas[i + 1, 4] = -Math.Acos(M456[1, 2]);
                }
                //theta5不能为0
                if (Math.Sin(thetas[i, 4]) != 0)
                {
                    //theta4
                    thetas[i, 3] = Math.Atan2(M456[2, 2] / Math.Sin(thetas[i, 4]), -M456[0, 2] / Math.Sin(thetas[i, 4]));
                    //theta6
                    thetas[i, 5] = Math.Atan2(-M456[1, 1] / Math.Sin(thetas[i, 4]), M456[1, 0] / Math.Sin(thetas[i, 4]));
                }
                else
                {
                    thetas[i, 3] = lastPt[3] / radToAgl;
                    thetas[i, 5] = Math.Atan2(-M456[0, 1] * Math.Cos(thetas[i, 3]) - M456[0, 0] * Math.Sin(thetas[i, 3]), M456[0, 0] * Math.Cos(thetas[i, 3]) - M456[0, 1] * Math.Sin(thetas[i, 3]));
                }
                double[] theta = new double[6];
                string outstr = "";
                for (int j = 0; j < 6; j++)
                {
                    thetas[i, j] = thetas[i, j] * radToAgl - th[j];
                    thetas[i, j] = angleModify(thetas[i, j]);
                    ////输出并检验
                    theta[j] = thetas[i, j];
                    outstr = outstr + "      θ" + (j + 1).ToString() + " = " + theta[j].ToString();
                }
                Debug.WriteLine("第" + (i + 1).ToString() + "组解：" + outstr);
                FK(theta);
            }
            bestRst=IKRusltSelect_MI_MDH(thetas, lastPt);
            return bestRst;
        }

第一次在网上发文章。。。