我快到了吗？ 4 DOF 机器人手臂正向运动学

Question

我开始了一个项目来学习我制作的定制 4 自由度机器人手臂的正向运动学。本质上，我想学习基础知识，将它们应用于学习逆运动学手指交叉。我似乎无法掌握旋转轴。一旦我克服了阻碍我的任何心理障碍，它就会在我的大脑中点击。

我开始创建一个 Joint 类来识别每个关节和旋转轴。再说一次，为什么我的大脑不能让我的头围绕旋转轴旋转？您有什么建议来克服这种心理障碍吗？

  public class Joint {

    public double Length { get; }
    public Vector3 RotationAxis { get; }

    public Joint(double length, Vector3 rotationAxis) {

      Length = length;
      RotationAxis = Vector3.Normalize(rotationAxis);
    }
  }

该类的核心是循环遍历每个关节并保持旋转状态，同时计算末端执行器的 Vector3 偏移量。但我觉得我滥用了四元数。或者是变换轴不正确。最有可能的是，轴不正确，因为我无法将头绕到它周围。

using System;
using System.Collections.Generic;
using System.Numerics;

namespace InverseKinematicsMover {

  public class Joint {

    public double Length { get; }
    public Vector3 RotationAxis { get; }

    public Joint(double length, Vector3 rotationAxis) {

      Length = length;
      RotationAxis = Vector3.Normalize(rotationAxis);
    }
  }

  public class Forward2 {

    public List<Joint> Joints = new List<Joint>();

    public Forward2() {

      Joints = new List<Joint>();
    }

    float degreeToRadian(int angle) {

      // because 90 is the center for servos
      int angleDegrees = angle - 90;

      return (float)Math.PI * angleDegrees / 180.0f;
    }

    public Vector3 CalculateEndEffectorPosition(List<int> jointAngles) {

      Vector3 endEffectorPosition = Vector3.Zero;
      Quaternion cumulativeRotation = Quaternion.Identity;

      for (int i = 0; i < Joints.Count; i++) {

        // continue to keep track of the rotation as we progress through joints
        // ie 45 degrees of the current joint is relative to 90 degrees of previous joint
        cumulativeRotation += Quaternion.CreateFromAxisAngle(Joints[i].RotationAxis, degreeToRadian(jointAngles[i]));
        
        // add to the end effector with the quaternion transformation of this joint's rotation axis
        endEffectorPosition += (float)Joints[i].Length * Vector3.Transform(Joints[i].RotationAxis, cumulativeRotation);
      }

      return endEffectorPosition;
    }
  }
}

最后，我的测试程序使用每个舵机的值（1-180）。

      Forward2 robotArm = new Forward2();

      // Base joint rotates the arm
      robotArm.Joints.Add(new Joint(0, new Vector3(0, 1, 0)));

      // Joints extend the robot arm
      robotArm.Joints.Add(new Joint(128.0, new Vector3(1, 0, 0)));
      robotArm.Joints.Add(new Joint(148.0, new Vector3(1, 0, 0)));
      robotArm.Joints.Add(new Joint(146.0, new Vector3(1, 0, 0)));

      // Specify joint angles (in degrees) for each joint
      var  jointAngles = new List<int> {
        GetServoPosition(0),
        GetServoPosition(1),
        GetServoPosition(2),
        GetServoPosition(3),
      };

      // Calculate the end effector position
      Vector3 endEffectorPosition = robotArm.CalculateEndEffectorPosition(jointAngles);

      // Display the end effector position
      log($"End Effector Position: X={endEffectorPosition.X}, Y={endEffectorPosition.Y}, Z={endEffectorPosition.Z}");

我的记录输出出现了奇怪的行为。看来 Y 轴和 Z 轴取决于 X（基本旋转）不为零。由于某种原因，Y 和 Z 轴值严重依赖于 X 值作为乘数。

我尝试为不同的轴配置关节定义。第一个关节是基础旋转，如常见的机器人手臂上所示。其余关节均垂直（上下）旋转手臂。

我也尝试过认为末端执行器累积值可能需要在变换过程中乘以旋转轴的长度。

  endEffectorPosition += Vector3.Transform(Joints[i].RotationAxis * (float)Joints[i].Length, cumulativeRotation);

这提供了类似的结果，其中 Y 和 Z 值严重依赖于 X 轴，并且似乎也是 X 的乘数。

Answer 1

啊，经过进一步的研究，我找到了轴的问题。您会看到，当所有关节值都为 0 时，手臂从底座向上延伸。这意味着所有关节都沿着相对于彼此的 y 轴。

此外，第一个关节配置为 z 轴旋转，这是不正确的。它应该是 Y 轴，因为它使手臂围绕它旋转。

主要问题是在矢量变换中使用旋转轴。因为手臂从 y 轴延伸到底座之外，所以只需要将此代码更改为...

‘ endEffectorPosition += (float)Joints[i].Length * Vector3.Transform(Vector3.UnitY,cumulativeRotation); } '