当前位置： 首页 > news >正文

C# 六自由度机械臂正反解计算

news 2025/11/1 9:27:03

目录

一、数学建模

1、连杆定义

2、建立连杆坐标系的步骤和举例

（1）步骤说明

（2）举例说明

3、PUMA560串联机械臂建模

（1）各关节和坐标系建立

（2）正解计算各关节矩阵如下

（3）逆解计算如下

二、C#程序实现

1、软件界面

2、关键功能代码实现

（1）UI中绘制机械臂

（2）正解计算

① MDH参数

②正解计算

（3）逆解计算

① MDH参数

②逆解计算

（4）UI中应用

三、工程下载链接

一、数学建模

（1）Matlab中PUMA560六自由度机械臂正反解，博客连接：https://blog.csdn.net/panjinliang066333/article/details/121681602?spm=1011.2415.3001.5331

（2）机械臂动画示意图，见博客：https://blog.csdn.net/panjinliang066333/article/details/141264547?spm=1011.2415.3001.5331

（3）PUMA560六自由度机械臂，关节示意图说明

模型示意图

实物示意图

1、连杆定义

备注：文档介绍说明，引用教材《机器人学》-蔡自兴

2、建立连杆坐标系的步骤和举例

（1）步骤说明

（2）举例说明

3、PUMA560串联机械臂建模

（1）各关节和坐标系建立

（2）正解计算各关节矩阵如下

则末关节矩阵：

（3）逆解计算如下

多个解情况

二、C#程序实现

1、软件界面

主界面-初始

主界面-运行

参数设置

2、关键功能代码实现

（1）UI中绘制机械臂

机械臂如上面主界面图片所示，使用SharpGL库文件在Winform中，绘制具有工业风格的六轴机械臂

代码

public void DrawRobot1(ref OpenGL gl, float[] angle, float[] yLength, bool isPuma560_Six){// 开启深度测试和光照gl.Enable(OpenGL.GL_DEPTH_TEST);gl.Enable(OpenGL.GL_LIGHTING);gl.Enable(OpenGL.GL_LIGHT0);gl.Enable(OpenGL.GL_LIGHT1);gl.ShadeModel(OpenGL.GL_SMOOTH);// 设置更强的光源float[] light0Pos = { 20.0f, 30.0f, 40.0f, 1.0f };float[] light0Ambient = { 0.4f, 0.4f, 0.4f, 1.0f };float[] light0Diffuse = { 1.0f, 1.0f, 1.0f, 1.0f };float[] light0Specular = { 1.0f, 1.0f, 1.0f, 1.0f };float[] light1Pos = { -20.0f, 20.0f, -30.0f, 1.0f };float[] light1Ambient = { 0.2f, 0.2f, 0.2f, 1.0f };float[] light1Diffuse = { 0.6f, 0.6f, 0.6f, 1.0f };gl.Light(OpenGL.GL_LIGHT0, OpenGL.GL_POSITION, light0Pos);gl.Light(OpenGL.GL_LIGHT0, OpenGL.GL_AMBIENT, light0Ambient);gl.Light(OpenGL.GL_LIGHT0, OpenGL.GL_DIFFUSE, light0Diffuse);gl.Light(OpenGL.GL_LIGHT0, OpenGL.GL_SPECULAR, light0Specular);gl.Light(OpenGL.GL_LIGHT1, OpenGL.GL_POSITION, light1Pos);gl.Light(OpenGL.GL_LIGHT1, OpenGL.GL_AMBIENT, light1Ambient);gl.Light(OpenGL.GL_LIGHT1, OpenGL.GL_DIFFUSE, light1Diffuse);// 设置材质属性gl.Material(OpenGL.GL_FRONT, OpenGL.GL_AMBIENT, matAmbient);gl.Material(OpenGL.GL_FRONT, OpenGL.GL_DIFFUSE, matDiffuse);gl.Material(OpenGL.GL_FRONT, OpenGL.GL_SPECULAR, matSpecular);gl.Material(OpenGL.GL_FRONT, OpenGL.GL_SHININESS, matShininess);// 绘制机器人底座DrawBase(ref gl, yLength[0]);// 在基座左下角绘制固定的世界坐标系DrawFixedWorldCoordinateSystem(ref gl, yLength[0]);// 绘制底座与第一关节的连接器gl.PushMatrix();gl.Translate(0.0f, -yLength[0] / 2, 0.0f);DrawBaseToJoint1Connector(ref gl);gl.PopMatrix();// 第一关节 - 绕Y轴旋转gl.PushMatrix();{gl.Translate(0.0f, -yLength[0] / 2, 0.0f);gl.Rotate(angle[0], 0.0f, 1.0f, 0.0f);DrawJoint1(ref gl, yLength[0]);}gl.PopMatrix();// 第二关节 - 绕X轴旋转gl.PushMatrix();{gl.Translate(0.0f, -yLength[0] / 2, 0.0f);gl.Rotate(angle[0], 0.0f, 1.0f, 0.0f);gl.Translate(2.5f, yLength[0] / 2, 0.0f);gl.Rotate(angle[1], 1.0f, 0.0f, 0.0f);// 注意：关节2现在从关节1的圆柱末端开始DrawJoint2(ref gl, yLength[1]);}gl.PopMatrix();// 第三关节 - 绕X轴旋转gl.PushMatrix();{gl.Translate(0.0f, -yLength[0] / 2, 0.0f);gl.Rotate(angle[0], 0.0f, 1.0f, 0.0f);gl.Translate(2.5f, yLength[0] / 2, 0.0f);gl.Rotate(angle[1], 1.0f, 0.0f, 0.0f);gl.Translate(0.0f, yLength[1], 0.0f);gl.Rotate(angle[2], 1.0f, 0.0f, 0.0f);DrawJoint3(ref gl, yLength[2]);}gl.PopMatrix();// 第四关节 - 绕Y轴旋转gl.PushMatrix();{gl.Translate(0.0f, -yLength[0] / 2, 0.0f);gl.Rotate(angle[0], 0.0f, 1.0f, 0.0f);gl.Translate(2.5f, yLength[0] / 2, 0.0f);gl.Rotate(angle[1], 1.0f, 0.0f, 0.0f);gl.Translate(0.0f, yLength[1], 0.0f);gl.Rotate(angle[2], 1.0f, 0.0f, 0.0f);gl.Translate(0.0f, yLength[2], 0.0f);gl.Rotate(angle[3], 0.0f, 1.0f, 0.0f);DrawJoint4(ref gl, yLength[3]);}gl.PopMatrix();if (isPuma560_Six){// 第五关节 - 绕X轴旋转gl.PushMatrix();{gl.Translate(0.0f, -yLength[0] / 2, 0.0f);gl.Rotate(angle[0], 0.0f, 1.0f, 0.0f);gl.Translate(2.5f, yLength[0] / 2, 0.0f);gl.Rotate(angle[1], 1.0f, 0.0f, 0.0f);gl.Translate(0.0f, yLength[1], 0.0f);gl.Rotate(angle[2], 1.0f, 0.0f, 0.0f);gl.Translate(0.0f, yLength[2], 0.0f);gl.Rotate(angle[3], 0.0f, 1.0f, 0.0f);gl.Translate(0.0f, yLength[3], 0.0f);gl.Rotate(angle[4], 1.0f, 0.0f, 0.0f);// 注意：关节5现在从关节4的球体顶部开始DrawJoint5(ref gl, yLength[4]);}gl.PopMatrix();// 第六关节 - 绕Y轴旋转 + 末端坐标系gl.PushMatrix();{gl.Translate(0.0f, -yLength[0] / 2, 0.0f);gl.Rotate(angle[0], 0.0f, 1.0f, 0.0f);gl.Translate(2.5f, yLength[0] / 2, 0.0f);gl.Rotate(angle[1], 1.0f, 0.0f, 0.0f);gl.Translate(0.0f, yLength[1], 0.0f);gl.Rotate(angle[2], 1.0f, 0.0f, 0.0f);gl.Translate(0.0f, yLength[2], 0.0f);gl.Rotate(angle[3], 0.0f, 1.0f, 0.0f);gl.Translate(0.0f, yLength[3], 0.0f);gl.Rotate(angle[4], 1.0f, 0.0f, 0.0f);gl.Translate(0.0f, yLength[4], 0.0f);gl.Rotate(angle[5], 0.0f, 1.0f, 0.0f);DrawJoint6(ref gl, yLength[5]);}gl.PopMatrix();}// 绘制世界坐标系DrawWorldCoordinateSystem(ref gl);gl.Disable(OpenGL.GL_LIGHTING);}

（2）正解计算

① MDH参数
/// <summary>/// 构造函数，内部默认MDH参数/// </summary>public Puma560FK(){_mdhParams = new double[,]{//[  d,          a,              alpha]{  0,          0,              0               },{  0,          0.180,          -Math.PI/2      },{  0,          0.600,          0               },{  0.630,      0.130,          -Math.PI/2      },{  0,          0,              Math.PI/2       },{  0,          0,              -Math.PI/2      }};}/// <summary>/// 构造函数，外部输入MDH参数/// </summary>/// <param name="mdhParameters"></param>public Puma560FK(double[,] mdhParameters){if (mdhParameters == null)throw new ArgumentNullException("mdhParameters");if (mdhParameters.GetLength(0) != 6 || mdhParameters.GetLength(1) != 3)throw new ArgumentException("MDH参数必须是6x4矩阵");_mdhParams = (double[,])mdhParameters.Clone();}
②正解计算
        /// <summary>/// 计算机器人末端位姿（齐次变换矩阵）/// </summary>/// <param name="theta">6个关节角度数组(弧度)</param>/// <returns>4x4齐次变换矩阵</returns>public double[,] ForwardKinematics(double[] theta){if (theta == null || theta.Length != 6)throw new ArgumentException("需要6个关节角度参数");// 计算各关节变换矩阵double[,] T01 = CalculateTransform(theta[0] * Math.PI / 180.0f, _mdhParams[0, 0], _mdhParams[0, 1], _mdhParams[0, 2]);double[,] T12 = CalculateTransform(theta[1] * Math.PI / 180.0f, _mdhParams[1, 0], _mdhParams[1, 1], _mdhParams[1, 2]);double[,] T23 = CalculateTransform(theta[2] * Math.PI / 180.0f, _mdhParams[2, 0], _mdhParams[2, 1], _mdhParams[2, 2]);double[,] T34 = CalculateTransform(theta[3] * Math.PI / 180.0f, _mdhParams[3, 0], _mdhParams[3, 1], _mdhParams[3, 2]);double[,] T45 = CalculateTransform(theta[4] * Math.PI / 180.0f, _mdhParams[4, 0], _mdhParams[4, 1], _mdhParams[4, 2]);double[,] T56 = CalculateTransform(theta[5] * Math.PI / 180.0f, _mdhParams[5, 0], _mdhParams[5, 1], _mdhParams[5, 2]);// 级联变换矩阵double[,] T02 = MatrixMultiply(T01, T12);double[,] T03 = MatrixMultiply(T02, T23);double[,] T04 = MatrixMultiply(T03, T34);double[,] T05 = MatrixMultiply(T04, T45);double[,] T06 = MatrixMultiply(T05, T56);return T06;}

（3）逆解计算

① MDH参数
        MDH = new double[,]{//[  d,          a-1,            alpha-1         ]{  0,          0,              0               },{  0,          0.180,          -Math.PI/2      },{  0,          0.600,          0               },{  0.630,      0.130,          -Math.PI/2      },{  0,          0,              Math.PI/2       },{  0,          0,              -Math.PI/2      }};}/// <summary>/// 构造函数，外部输入MDH参数/// </summary>/// <param name="mdhParameters"></param>public Puma560IK(double[,] mdhParameters){if (mdhParameters == null)throw new ArgumentNullException("mdhParameters");if (mdhParameters.GetLength(0) != 6 || mdhParameters.GetLength(1) != 3)throw new ArgumentException("MDH参数必须是6x3矩阵");MDH = (double[,])mdhParameters.Clone();}
②逆解计算
        public double[,] ComputeIK(double[,] Tbe){// Extract position and orientation components from transformation matrixdouble nx = Tbe[0, 0], ny = Tbe[1, 0], nz = Tbe[2, 0];double ox = Tbe[0, 1], oy = Tbe[1, 1], oz = Tbe[2, 1];double ax = Tbe[0, 2], ay = Tbe[1, 2], az = Tbe[2, 2];double px = Tbe[0, 3], py = Tbe[1, 3], pz = Tbe[2, 3];//// Extract MDH parameters from the 6x3 matrix//double d4 = MDH[3, 0];//double a1 = MDH[1, 1];//double a2 = MDH[2, 1];//double a3 = MDH[3, 1];//double d2 = 0, d3 = 0; // These are zero in the MDH table//// Extract alpha angles from MDH//double f1 = MDH[1, 2];  // alpha1//double f3 = MDH[3, 2];  // alpha3//double f4 = MDH[4, 2];  // alpha4//double f5 = MDH[5, 2];  // alpha5double d1 = MDH[0, 0];double d2 = MDH[1, 0];double d3 = MDH[2, 0];double d4 = MDH[3, 0];double d5 = MDH[4, 0];double d6 = MDH[5, 0];double a1 = MDH[1, 1];double a2 = MDH[2, 1];double a3 = MDH[3, 1];double a4 = MDH[4, 1];double a5 = MDH[5, 1];double f1 = MDH[1, 2];  // alpha1double f2 = MDH[2, 2];  // alpha2double f3 = MDH[3, 2];  // alpha3double f4 = MDH[4, 2];  // alpha4double f5 = MDH[5, 2];  // alpha5// Calculate theta1 solutionsdouble t11 = -Math.Atan2(-py, px) + Math.Atan2((d2 - d3) / Math.Sin(f1),Math.Sqrt(Math.Pow(px * Math.Sin(f1), 2) + Math.Pow(py * Math.Sin(f1), 2) - Math.Pow(d2 - d3, 2)));double t12 = -Math.Atan2(-py, px) + Math.Atan2((d2 - d3) / Math.Sin(f1),-Math.Sqrt(Math.Pow(px * Math.Sin(f1), 2) + Math.Pow(py * Math.Sin(f1), 2) - Math.Pow(d2 - d3, 2)));// Calculate intermediate values for theta3double m3_1 = pz * Math.Sin(f1);double n3_1 = a1 - px * Math.Cos(t11) - py * Math.Sin(t11);double m3_2 = pz * Math.Sin(f1);double n3_2 = a1 - px * Math.Cos(t12) - py * Math.Sin(t12);// Calculate theta3 solutionsdouble t31 = -Math.Atan2(a2 * a3 / Math.Sin(f3), a2 * d4) +Math.Atan2((Math.Pow(m3_1, 2) + Math.Pow(n3_1, 2) - Math.Pow(a2, 2) - Math.Pow(a3, 2) - Math.Pow(d4, 2)) / Math.Sin(f3),Math.Sqrt(Math.Pow(2 * a2 * d4 * Math.Sin(f3), 2) + Math.Pow(2 * a2 * a3, 2) -Math.Pow(Math.Pow(m3_1, 2) + Math.Pow(n3_1, 2) - Math.Pow(a2, 2) - Math.Pow(a3, 2) - Math.Pow(d4, 2), 2)));double t32 = -Math.Atan2(a2 * a3 / Math.Sin(f3), a2 * d4) +Math.Atan2((Math.Pow(m3_1, 2) + Math.Pow(n3_1, 2) - Math.Pow(a2, 2) - Math.Pow(a3, 2) - Math.Pow(d4, 2)) / Math.Sin(f3),-Math.Sqrt(Math.Pow(2 * a2 * d4 * Math.Sin(f3), 2) + Math.Pow(2 * a2 * a3, 2) -Math.Pow(Math.Pow(m3_1, 2) + Math.Pow(n3_1, 2) - Math.Pow(a2, 2) - Math.Pow(a3, 2) - Math.Pow(d4, 2), 2)));double t33 = -Math.Atan2(a2 * a3 / Math.Sin(f3), a2 * d4) +Math.Atan2((Math.Pow(m3_2, 2) + Math.Pow(n3_2, 2) - Math.Pow(a2, 2) - Math.Pow(a3, 2) - Math.Pow(d4, 2)) / Math.Sin(f3),Math.Sqrt(Math.Pow(2 * a2 * d4 * Math.Sin(f3), 2) + Math.Pow(2 * a2 * a3, 2) -Math.Pow(Math.Pow(m3_2, 2) + Math.Pow(n3_2, 2) - Math.Pow(a2, 2) - Math.Pow(a3, 2) - Math.Pow(d4, 2), 2)));double t34 = -Math.Atan2(a2 * a3 / Math.Sin(f3), a2 * d4) +Math.Atan2((Math.Pow(m3_2, 2) + Math.Pow(n3_2, 2) - Math.Pow(a2, 2) - Math.Pow(a3, 2) - Math.Pow(d4, 2)) / Math.Sin(f3),-Math.Sqrt(Math.Pow(2 * a2 * d4 * Math.Sin(f3), 2) + Math.Pow(2 * a2 * a3, 2) -Math.Pow(Math.Pow(m3_2, 2) + Math.Pow(n3_2, 2) - Math.Pow(a2, 2) - Math.Pow(a3, 2) - Math.Pow(d4, 2), 2)));// Calculate intermediate values for theta2double m2_1 = a2 + a3 * Math.Cos(t31) + d4 * Math.Sin(f3) * Math.Sin(t31);double n2_1 = a3 * Math.Sin(t31) - d4 * Math.Sin(f3) * Math.Cos(t31);double m2_2 = a2 + a3 * Math.Cos(t32) + d4 * Math.Sin(f3) * Math.Sin(t32);double n2_2 = a3 * Math.Sin(t32) - d4 * Math.Sin(f3) * Math.Cos(t32);double m2_3 = a2 + a3 * Math.Cos(t33) + d4 * Math.Sin(f3) * Math.Sin(t33);double n2_3 = a3 * Math.Sin(t33) - d4 * Math.Sin(f3) * Math.Cos(t33);double m2_4 = a2 + a3 * Math.Cos(t34) + d4 * Math.Sin(f3) * Math.Sin(t34);double n2_4 = a3 * Math.Sin(t34) - d4 * Math.Sin(f3) * Math.Cos(t34);// Calculate theta2 solutionsdouble t21 = Math.Atan2(m3_1 * m2_1 + n2_1 * n3_1, m3_1 * n2_1 - m2_1 * n3_1);double t22 = Math.Atan2(m3_1 * m2_2 + n2_2 * n3_1, m3_1 * n2_2 - m2_2 * n3_1);double t23 = Math.Atan2(m3_2 * m2_3 + n2_3 * n3_2, m3_2 * n2_3 - m2_3 * n3_2);double t24 = Math.Atan2(m3_2 * m2_4 + n2_4 * n3_2, m3_2 * n2_4 - m2_4 * n3_2);// Calculate intermediate values for theta5double m5_1 = -Math.Sin(f5) * (ax * Math.Cos(t11) * Math.Cos(t21) + ay * Math.Sin(t11) * Math.Cos(t21) + az * Math.Sin(f1) * Math.Sin(t21));double n5_1 = Math.Sin(f5) * (ax * Math.Cos(t11) * Math.Sin(t21) + ay * Math.Sin(t11) * Math.Sin(t21) - az * Math.Sin(f1) * Math.Cos(t21));double m5_2 = -Math.Sin(f5) * (ax * Math.Cos(t11) * Math.Cos(t22) + ay * Math.Sin(t11) * Math.Cos(t22) + az * Math.Sin(f1) * Math.Sin(t22));double n5_2 = Math.Sin(f5) * (ax * Math.Cos(t11) * Math.Sin(t22) + ay * Math.Sin(t11) * Math.Sin(t22) - az * Math.Sin(f1) * Math.Cos(t22));double m5_3 = -Math.Sin(f5) * (ax * Math.Cos(t12) * Math.Cos(t23) + ay * Math.Sin(t12) * Math.Cos(t23) + az * Math.Sin(f1) * Math.Sin(t23));double n5_3 = Math.Sin(f5) * (ax * Math.Cos(t12) * Math.Sin(t23) + ay * Math.Sin(t12) * Math.Sin(t23) - az * Math.Sin(f1) * Math.Cos(t23));double m5_4 = -Math.Sin(f5) * (ax * Math.Cos(t12) * Math.Cos(t24) + ay * Math.Sin(t12) * Math.Cos(t24) + az * Math.Sin(f1) * Math.Sin(t24));double n5_4 = Math.Sin(f5) * (ax * Math.Cos(t12) * Math.Sin(t24) + ay * Math.Sin(t12) * Math.Sin(t24) - az * Math.Sin(f1) * Math.Cos(t24));// Calculate theta5 solutionsdouble t51 = Math.Atan2(Math.Sqrt(Math.Pow(ay * Math.Cos(t11) - ax * Math.Sin(t11), 2) +Math.Pow(m5_1 * Math.Cos(t31) + n5_1 * Math.Sin(t31), 2)),(m5_1 * Math.Sin(t31) - n5_1 * Math.Cos(t31)) / (Math.Sin(f3) * Math.Sin(f4)));double t52 = Math.Atan2(-Math.Sqrt(Math.Pow(ay * Math.Cos(t11) - ax * Math.Sin(t11), 2) +Math.Pow(m5_1 * Math.Cos(t31) + n5_1 * Math.Sin(t31), 2)),(m5_1 * Math.Sin(t31) - n5_1 * Math.Cos(t31)) / (Math.Sin(f3) * Math.Sin(f4)));double t53 = Math.Atan2(Math.Sqrt(Math.Pow(ay * Math.Cos(t11) - ax * Math.Sin(t11), 2) +Math.Pow(m5_2 * Math.Cos(t32) + n5_2 * Math.Sin(t32), 2)),(m5_2 * Math.Sin(t32) - n5_2 * Math.Cos(t32)) / (Math.Sin(f3) * Math.Sin(f4)));double t54 = Math.Atan2(-Math.Sqrt(Math.Pow(ay * Math.Cos(t11) - ax * Math.Sin(t11), 2) +Math.Pow(m5_2 * Math.Cos(t32) + n5_2 * Math.Sin(t32), 2)),(m5_2 * Math.Sin(t32) - n5_2 * Math.Cos(t32)) / (Math.Sin(f3) * Math.Sin(f4)));double t55 = Math.Atan2(Math.Sqrt(Math.Pow(ay * Math.Cos(t12) - ax * Math.Sin(t12), 2) +Math.Pow(m5_3 * Math.Cos(t33) + n5_3 * Math.Sin(t33), 2)),(m5_3 * Math.Sin(t33) - n5_3 * Math.Cos(t33)) / (Math.Sin(f3) * Math.Sin(f4)));double t56 = Math.Atan2(-Math.Sqrt(Math.Pow(ay * Math.Cos(t12) - ax * Math.Sin(t12), 2) +Math.Pow(m5_3 * Math.Cos(t33) + n5_3 * Math.Sin(t33), 2)),(m5_3 * Math.Sin(t33) - n5_3 * Math.Cos(t33)) / (Math.Sin(f3) * Math.Sin(f4)));double t57 = Math.Atan2(Math.Sqrt(Math.Pow(ay * Math.Cos(t12) - ax * Math.Sin(t12), 2) +Math.Pow(m5_4 * Math.Cos(t34) + n5_4 * Math.Sin(t34), 2)),(m5_4 * Math.Sin(t34) - n5_4 * Math.Cos(t34)) / (Math.Sin(f3) * Math.Sin(f4)));double t58 = Math.Atan2(-Math.Sqrt(Math.Pow(ay * Math.Cos(t12) - ax * Math.Sin(t12), 2) +Math.Pow(m5_4 * Math.Cos(t34) + n5_4 * Math.Sin(t34), 2)),(m5_4 * Math.Sin(t34) - n5_4 * Math.Cos(t34)) / (Math.Sin(f3) * Math.Sin(f4)));// Calculate theta4 solutionsdouble t41 = (Math.Sin(t51) == 0) ? 0 : Math.Atan2(((ay * Math.Cos(t11) - ax * Math.Sin(t11)) * Math.Sin(f1) * Math.Sin(f5)) / (-Math.Sin(t51) * Math.Sin(f3)),(-m5_1 * Math.Cos(t31) - n5_1 * Math.Sin(t31)) / Math.Sin(t51));double t42 = (Math.Sin(t52) == 0) ? 0 : Math.Atan2(((ay * Math.Cos(t11) - ax * Math.Sin(t11)) * Math.Sin(f1) * Math.Sin(f5)) / (-Math.Sin(t52) * Math.Sin(f3)),(-m5_1 * Math.Cos(t31) - n5_1 * Math.Sin(t31)) / Math.Sin(t52));double t43 = (Math.Sin(t53) == 0) ? 0 : Math.Atan2(((ay * Math.Cos(t11) - ax * Math.Sin(t11)) * Math.Sin(f1) * Math.Sin(f5)) / (-Math.Sin(t53) * Math.Sin(f3)),(-m5_2 * Math.Cos(t32) - n5_2 * Math.Sin(t32)) / Math.Sin(t53));double t44 = (Math.Sin(t54) == 0) ? 0 : Math.Atan2(((ay * Math.Cos(t11) - ax * Math.Sin(t11)) * Math.Sin(f1) * Math.Sin(f5)) / (-Math.Sin(t54) * Math.Sin(f3)),(-m5_2 * Math.Cos(t32) - n5_2 * Math.Sin(t32)) / Math.Sin(t54));double t45 = (Math.Sin(t55) == 0) ? 0 : Math.Atan2(((ay * Math.Cos(t12) - ax * Math.Sin(t12)) * Math.Sin(f1) * Math.Sin(f5)) / (-Math.Sin(t55) * Math.Sin(f3)),(-m5_3 * Math.Cos(t33) - n5_3 * Math.Sin(t33)) / Math.Sin(t55));double t46 = (Math.Sin(t56) == 0) ? 0 : Math.Atan2(((ay * Math.Cos(t12) - ax * Math.Sin(t12)) * Math.Sin(f1) * Math.Sin(f5)) / (-Math.Sin(t56) * Math.Sin(f3)),(-m5_3 * Math.Cos(t33) - n5_3 * Math.Sin(t33)) / Math.Sin(t56));double t47 = (Math.Sin(t57) == 0) ? 0 : Math.Atan2(((ay * Math.Cos(t12) - ax * Math.Sin(t12)) * Math.Sin(f1) * Math.Sin(f5)) / (-Math.Sin(t57) * Math.Sin(f3)),(-m5_4 * Math.Cos(t34) - n5_4 * Math.Sin(t34)) / Math.Sin(t57));double t48 = (Math.Sin(t58) == 0) ? 0 : Math.Atan2(((ay * Math.Cos(t12) - ax * Math.Sin(t12)) * Math.Sin(f1) * Math.Sin(f5)) / (-Math.Sin(t58) * Math.Sin(f3)),(-m5_4 * Math.Cos(t34) - n5_4 * Math.Sin(t34)) / Math.Sin(t58));// Calculate theta6 solutionsdouble e1 = nx * Math.Sin(t11) - ny * Math.Cos(t11);double f1_val = ox * Math.Sin(t11) - oy * Math.Cos(t11);double t61 = Math.Atan2(Math.Cos(t41) * e1 - Math.Cos(t51) * Math.Sin(t41) * f1_val,Math.Cos(t41) * f1_val + Math.Cos(t51) * Math.Sin(t41) * e1);double t62 = Math.Atan2(Math.Cos(t42) * e1 - Math.Cos(t52) * Math.Sin(t42) * f1_val,Math.Cos(t42) * f1_val + Math.Cos(t52) * Math.Sin(t42) * e1);double t63 = Math.Atan2(Math.Cos(t43) * e1 - Math.Cos(t53) * Math.Sin(t43) * f1_val,Math.Cos(t43) * f1_val + Math.Cos(t53) * Math.Sin(t43) * e1);double t64 = Math.Atan2(Math.Cos(t44) * e1 - Math.Cos(t54) * Math.Sin(t44) * f1_val,Math.Cos(t44) * f1_val + Math.Cos(t54) * Math.Sin(t44) * e1);double e2 = nx * Math.Sin(t12) - ny * Math.Cos(t12);double f2_val = ox * Math.Sin(t12) - oy * Math.Cos(t12);double t65 = Math.Atan2(Math.Cos(t45) * e2 - Math.Cos(t55) * Math.Sin(t45) * f2_val,Math.Cos(t45) * f2_val + Math.Cos(t55) * Math.Sin(t45) * e2);double t66 = Math.Atan2(Math.Cos(t46) * e2 - Math.Cos(t56) * Math.Sin(t46) * f2_val,Math.Cos(t46) * f2_val + Math.Cos(t56) * Math.Sin(t46) * e2);double t67 = Math.Atan2(Math.Cos(t47) * e2 - Math.Cos(t57) * Math.Sin(t47) * f2_val,Math.Cos(t47) * f2_val + Math.Cos(t57) * Math.Sin(t47) * e2);double t68 = Math.Atan2(Math.Cos(t48) * e2 - Math.Cos(t58) * Math.Sin(t48) * f2_val,Math.Cos(t48) * f2_val + Math.Cos(t58) * Math.Sin(t48) * e2);// Combine all solutions into a 8x6 matrixdouble[,] ikine_t = new double[8, 6]{{t11, t21, t31, t41, t51, t61},{t11, t21, t31, t42, t52, t62},{t11, t22, t32, t43, t53, t63},{t11, t22, t32, t44, t54, t64},{t12, t23, t33, t45, t55, t65},{t12, t23, t33, t46, t56, t66},{t12, t24, t34, t47, t57, t67},{t12, t24, t34, t48, t58, t68}};return ikine_t;}

（4）UI中应用

正解

/// <summary>/// 正解计算/// </summary>/// <param name="jointAngles"></param>void RunDemoFK(double[] jointAngles){//for (int i = 0; i < jointAngles.Length;i++ )//{//    jointAngles[i] = jointAngles[i] * Math.PI / 180.0f;//}bDataModel = false;try{var fk = new Puma560FK(MDH);double[] jointAnglesTemp = new double[6];for (int i = 0; i < jointAnglesTemp.Length; i++){jointAnglesTemp[i] = jointAngles[i] ;}// 计算正运动学double[,] T06 = fk.ForwardKinematics(jointAnglesTemp);// 打印变换矩阵for (int i = 0; i < 4; i++){for (int j = 0; j < 4; j++){//Console.Write(string.Format("{0,12:F6}", T06[i, j]));}}txtMatrixFK_1_1.Text = T06[0, 0].ToString("f2");txtMatrixFK_1_2.Text = T06[0, 1].ToString("f2");txtMatrixFK_1_3.Text = T06[0, 2].ToString("f2");txtMatrixFK_1_4.Text = T06[0, 3].ToString("f2");txtMatrixFK_2_1.Text = T06[1, 0].ToString("f2");txtMatrixFK_2_2.Text = T06[1, 1].ToString("f2");txtMatrixFK_2_3.Text = T06[1, 2].ToString("f2");txtMatrixFK_2_4.Text = T06[1, 3].ToString("f2");txtMatrixFK_3_1.Text = T06[2, 0].ToString("f2");txtMatrixFK_3_2.Text = T06[2, 1].ToString("f2");txtMatrixFK_3_3.Text = T06[2, 2].ToString("f2");txtMatrixFK_3_4.Text = T06[2, 3].ToString("f2");txtMatrixFK_4_1.Text = T06[3, 0].ToString("f2");txtMatrixFK_4_2.Text = T06[3, 1].ToString("f2");txtMatrixFK_4_3.Text = T06[3, 2].ToString("f2");txtMatrixFK_4_4.Text = T06[3, 3].ToString("f2");// 打印末关节位置double[] position = fk.GetPosition(T06);txtPostionAbsX_FK.Text = position[0].ToString("f4");txtPostionAbsY_FK.Text = position[1].ToString("f4");txtPostionAbsZ_FK.Text = position[2].ToString("f4");// 打印末关节欧拉角double[] euler = fk.GetEulerAngles(T06);//转换为角度double[] eulerDeg = new double[3];for (int i = 0; i < 3; i++){eulerDeg[i] = euler[i] * 180 / Math.PI;                   }txtPostionAbsA_FK.Text = eulerDeg[0].ToString("f4");     //RolltxtPostionAbsB_FK.Text = eulerDeg[1].ToString("f4");     //PitchtxtPostionAbsC_FK.Text = eulerDeg[2].ToString("f4");     //Yaw}catch (Exception ex){MessageBox.Show("Error: " + ex.Message);}txtJoinActualTheta1.Text = jointAngles[0].ToString("f4");txtJoinActualTheta2.Text = jointAngles[1].ToString("f4");txtJoinActualTheta3.Text = jointAngles[2].ToString("f4");txtJoinActualTheta4.Text = jointAngles[3].ToString("f4");txtJoinActualTheta5.Text = jointAngles[4].ToString("f4");txtJoinActualTheta6.Text = jointAngles[5].ToString("f4");//逆解txtPostionAbsX_IK.Text = txtPostionAbsX_FK.Text;txtPostionAbsY_IK.Text = txtPostionAbsY_FK.Text;txtPostionAbsZ_IK.Text = txtPostionAbsZ_FK.Text;txtPostionAbsA_IK.Text = txtPostionAbsA_FK.Text;txtPostionAbsB_IK.Text = txtPostionAbsB_FK.Text;txtPostionAbsC_IK.Text = txtPostionAbsC_FK.Text;x = Convert.ToDouble(txtPostionAbsX_IK.Text);y = Convert.ToDouble(txtPostionAbsY_IK.Text);z = Convert.ToDouble(txtPostionAbsZ_IK.Text);A = Convert.ToDouble(txtPostionAbsA_IK.Text);B = Convert.ToDouble(txtPostionAbsB_IK.Text);C = Convert.ToDouble(txtPostionAbsC_IK.Text);RunDemoIK();}

逆解

void RunDemoIK(){bDataModel = true;txtPostionAbsX_IK.Text = x.ToString("f4");txtPostionAbsY_IK.Text = y.ToString("f4");txtPostionAbsZ_IK.Text = z.ToString("f4");txtPostionAbsA_IK.Text = A.ToString("f4");txtPostionAbsB_IK.Text = B.ToString("f4");txtPostionAbsC_IK.Text = C.ToString("f4");            try{Puma560IK solver = new Puma560IK(MDH);// 测试用例1：标准位置double[,] testPose1 = Puma560IK.CreateTransformMatrix(x, y, z, A, B, C);Double[,] Solves_IK = solver.ComputeIK(testPose1);          //角度值//转换为角度值for (int i = 0; i < Solves_IK.GetLength(0); i++){for (int j = 0; j < Solves_IK.GetLength(1); j++){Solves_IK[i, j] = Solves_IK[i, j] * 180 / Math.PI;    if(double.IsNaN(Solves_IK[i, j])){throw new Exception("当前逆解计算不存在解！！！");}}}//第一组解txtSolveIK_1_1.Text = Solves_IK[0, 0].ToString("f2");txtSolveIK_1_2.Text = Solves_IK[0, 1].ToString("f2");txtSolveIK_1_3.Text = Solves_IK[0, 2].ToString("f2");txtSolveIK_1_4.Text = Solves_IK[0, 3].ToString("f2");txtSolveIK_1_5.Text = Solves_IK[0, 4].ToString("f2");txtSolveIK_1_6.Text = Solves_IK[0, 5].ToString("f2");//第二组解txtSolveIK_2_1.Text = Solves_IK[1, 0].ToString("f2");txtSolveIK_2_2.Text = Solves_IK[1, 1].ToString("f2");txtSolveIK_2_3.Text = Solves_IK[1, 2].ToString("f2");txtSolveIK_2_4.Text = Solves_IK[1, 3].ToString("f2");txtSolveIK_2_5.Text = Solves_IK[1, 4].ToString("f2");txtSolveIK_2_6.Text = Solves_IK[1, 5].ToString("f2");//第三组解txtSolveIK_3_1.Text = Solves_IK[2, 0].ToString("f2");txtSolveIK_3_2.Text = Solves_IK[2, 1].ToString("f2");txtSolveIK_3_3.Text = Solves_IK[2, 2].ToString("f2");txtSolveIK_3_4.Text = Solves_IK[2, 3].ToString("f2");txtSolveIK_3_5.Text = Solves_IK[2, 4].ToString("f2");txtSolveIK_3_6.Text = Solves_IK[2, 5].ToString("f2");//第四组解txtSolveIK_4_1.Text = Solves_IK[3, 0].ToString("f2");txtSolveIK_4_2.Text = Solves_IK[3, 1].ToString("f2");txtSolveIK_4_3.Text = Solves_IK[3, 2].ToString("f2");txtSolveIK_4_4.Text = Solves_IK[3, 3].ToString("f2");txtSolveIK_4_5.Text = Solves_IK[3, 4].ToString("f2");txtSolveIK_4_6.Text = Solves_IK[3, 5].ToString("f2");//第五组解txtSolveIK_5_1.Text = Solves_IK[4, 0].ToString("f2");txtSolveIK_5_2.Text = Solves_IK[4, 1].ToString("f2");txtSolveIK_5_3.Text = Solves_IK[4, 2].ToString("f2");txtSolveIK_5_4.Text = Solves_IK[4, 3].ToString("f2");txtSolveIK_5_5.Text = Solves_IK[4, 4].ToString("f2");txtSolveIK_5_6.Text = Solves_IK[4, 5].ToString("f2");//第六组解txtSolveIK_6_1.Text = Solves_IK[5, 0].ToString("f2");txtSolveIK_6_2.Text = Solves_IK[5, 1].ToString("f2");txtSolveIK_6_3.Text = Solves_IK[5, 2].ToString("f2");txtSolveIK_6_4.Text = Solves_IK[5, 3].ToString("f2");txtSolveIK_6_5.Text = Solves_IK[5, 4].ToString("f2");txtSolveIK_6_6.Text = Solves_IK[5, 5].ToString("f2");//第七组解txtSolveIK_7_1.Text = Solves_IK[6, 0].ToString("f2");txtSolveIK_7_2.Text = Solves_IK[6, 1].ToString("f2");txtSolveIK_7_3.Text = Solves_IK[6, 2].ToString("f2");txtSolveIK_7_4.Text = Solves_IK[6, 3].ToString("f2");txtSolveIK_7_5.Text = Solves_IK[6, 4].ToString("f2");txtSolveIK_7_6.Text = Solves_IK[6, 5].ToString("f2");//第八组解txtSolveIK_8_1.Text = Solves_IK[7, 0].ToString("f2");txtSolveIK_8_2.Text = Solves_IK[7, 1].ToString("f2");txtSolveIK_8_3.Text = Solves_IK[7, 2].ToString("f2");txtSolveIK_8_4.Text = Solves_IK[7, 3].ToString("f2");txtSolveIK_8_5.Text = Solves_IK[7, 4].ToString("f2");txtSolveIK_8_6.Text = Solves_IK[7, 5].ToString("f2");}catch (Exception ex){MessageBox.Show("Error: " + ex.Message);}}

三、工程下载链接

https://download.csdn.net/download/panjinliang066333/92233890

http://www.dtcms.com/a/553368.html

相关文章：

  • 【开题答辩全过程】以 基于Java的旅游网站的设计与开发为例，包含答辩的问题和答案
  • 【深入学习Vue丨第一篇】Props 完全指南
  • U-net 系列算法总结
  • 什么网站可以做模型挣钱网站建设公司有多少家
  • 网站建设的杂志建筑专业网站建设
  • vue3+ts面试题（一）JSX，SFC
  • 网站开发 数字证书网站制作设计方案
  • PCB设计----阻抗不连续的解决方法
  • 网站制作北京网站建设公司哪家好安平县哪家做网站
  • 14. setState是异步更新
  • 22. React中CSS使用方案
  • 深度对比 ArrayList 与 LinkedList：从底层数据结构到增删查改的性能差异实测
  • 信任的重构：S11e Protocol 如何以算法取代中介
  • Python 第二十五节 常用练习问题（三）
  • Spring AI Alibaba 【五】
  • 什么网站是用html做的2023年时政热点事件
  • 泉州企业免费建站制作网站首页
  • 2025年10月31日全球AI前沿动态
  • Linux线程同步（四）
  • 全新升级！山海鲸4.6.3版本正式亮相
  • 图像压缩-将8bit数据压缩为2bit
  • 海宁网站怎么做seo企业网站能起到什么作用
  • 手机在网上怎么创建自己的网站网站开发人员职位
  • LineSlam线特征投影融合（Fuse） 中pML-＞GetLineNormalVector()的理解代码理解
  • 【图像处理基石】多波段图像融合算法入门：从概念到实践
  • Docker核心文件：DockerCompose文件
  • 企业网站 自助建站网站做竞价经常会被攻击吗
  • Android ble理解
  • 深入理解 Spring Boot Web 开发中的静态资源处理机制
  • 网站建设费记什么科目产品ui设计是什么