4自由度机械臂正运动学实现遇阻：旋转轴与计算问题求助

我正在做一个自制4自由度（4 DOF）机械臂的正运动学学习项目，目标是先掌握基础再推进逆运动学学习。但一直搞不懂旋转轴的概念，卡在这了。

我尝试用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) {
            // 舵机以90度为中位
            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++) {
                // 累计关节旋转状态
                cumulativeRotation += Quaternion.CreateFromAxisAngle(Joints[i].RotationAxis, degreeToRadian(jointAngles[i]));
                
                // 计算当前关节对末端位置的贡献
                endEffectorPosition += (float)Joints[i].Length * Vector3.Transform(Joints[i].RotationAxis, cumulativeRotation);
            }

            return endEffectorPosition;
        }
    }
}

测试时用舵机的1-180度数值：

Forward2 robotArm = new Forward2();

// 基座关节旋转整个手臂
robotArm.Joints.Add(new Joint(0, new Vector3(0, 1, 0)));

// 后续关节控制手臂伸展/抬起
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)));

// 获取各舵机位置
var jointAngles = new List<int> {
    GetServoPosition(0),
    GetServoPosition(1),
    GetServoPosition(2),
    GetServoPosition(3),
};

// 计算末端位置
Vector3 endEffectorPosition = robotArm.CalculateEndEffectorPosition(jointAngles);

log($"End Effector Position: X={endEffectorPosition.X}, Y={endEffectorPosition.Y}, Z={endEffectorPosition.Z}");

运行后结果异常：Y、Z轴位置会随着基座旋转值变化，而且Y、Z值和X值呈倍数关系。我调整过关节轴定义（基座是常见的垂直旋转关节，其余是上下转动的关节），也修改过计算语句：

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

但结果还是一样，Y、Z值严重依赖基座旋转，求解决方法。

问题分析与解决 #

1. 核心错误点 #

（1）四元数累计方式错误

四元数的组合不能用+运算符，必须用乘法（*），且顺序至关重要：新关节的旋转需要乘在累计旋转的左侧，因为每个关节的旋转是基于当前连杆的局部坐标系，而非全局坐标系。

（2）末端位置计算逻辑错误

你把连杆长度直接乘在旋转轴上，但旋转轴是关节的转动轴，不是连杆的延伸方向。正确逻辑是：连杆初始沿某个固定方向（比如全局Z轴），经过累计旋转变换后得到当前连杆的全局方向，再乘以长度累加到末端位置。

（3）旋转轴的理解偏差

旋转轴是关节局部坐标系下的转动参考轴，不是全局轴。比如大臂关节的旋转轴，在基座未旋转时是全局X轴，但基座旋转后，它的局部X轴会跟着基座转动，这部分四元数的累计会自动处理这个变换，不需要手动调整轴的方向。

2. 修正后的代码 #

修正CalculateEndEffectorPosition方法

public Vector3 CalculateEndEffectorPosition(List<int> jointAngles) {
    Vector3 endEffectorPosition = Vector3.Zero;
    Quaternion cumulativeRotation = Quaternion.Identity;

    for (int i = 0; i < Joints.Count; i++) {
        // 计算当前关节的旋转量（弧度）
        float rad = degreeToRadian(jointAngles[i]);
        // 创建当前关节的旋转四元数
        Quaternion jointRotation = Quaternion.CreateFromAxisAngle(Joints[i].RotationAxis, rad);
        
        // 累计旋转：新旋转乘在左侧，代表基于当前姿态的局部旋转
        cumulativeRotation = jointRotation * cumulativeRotation;

        // 连杆初始沿全局Z轴延伸，用累计旋转变换到全局坐标系下的方向
        // 若你的机械臂连杆初始是X轴，替换为Vector3.UnitX即可
        Vector3 linkDirection = Vector3.Transform(Vector3.UnitZ, cumulativeRotation);
        
        // 累加当前连杆对末端位置的贡献
        endEffectorPosition += linkDirection * (float)Joints[i].Length;
    }

    return endEffectorPosition;
}

修正关节轴定义（匹配常见4DOF机械臂结构）

Forward2 robotArm = new Forward2();

// 基座关节：绕全局Y轴旋转（左右转），连杆长度0
robotArm.Joints.Add(new Joint(0, new Vector3(0, 1, 0)));

// 大臂关节：绕局部X轴旋转（上下抬）
robotArm.Joints.Add(new Joint(128.0, new Vector3(1, 0, 0)));

// 小臂关节：绕局部X轴旋转（弯曲）
robotArm.Joints.Add(new Joint(148.0, new Vector3(1, 0, 0)));

// 腕部关节：绕局部X轴旋转（弯曲），若为拧转关节则用Vector3.UnitZ
robotArm.Joints.Add(new Joint(146.0, new Vector3(1, 0, 0)));

3. 突破旋转轴理解瓶颈的方法 #

• 实物模拟：用筷子当连杆、手指当关节，转动时观察关节绕哪个轴转动，这个轴就是该关节的局部旋转轴。
• 分步测试：先只加基座关节，验证旋转后末端位置变化是否符合预期；再逐个添加关节，逐步理解每个旋转的影响。
• 坐标系绘图：给每个关节画局部坐标系，初始与全局坐标系重合，每次旋转后更新局部坐标系的方向，直观感受旋转轴的作用。

内容的提问来源于stack exchange，提问作者Ponderer