本文由 linuxROS 整理发布，首发于 linuxros.cn，转载请注明出处。

机器人数学库：Vec3/四元数/矩阵300行搞定

导读：做机器人仿真要不要引入 Eigen？引入后编译时间从秒级变分钟级，依赖管理复杂度翻倍。本文用 300 行头文件实现机器人学所需的全部数学基础——Vec3 向量、Quaternion 四元数、Mat4 齐次变换，没有外部依赖，编译即用。

一、为什么要自研数学库

方案	编译时间	依赖大小	学习成本
自研（本文）	秒级	0	理解原理
Eigen3	分钟级	\~1000个头文件	API记忆
GLM	中等	中等	API记忆

自研的理由很简单：只实现用得上的功能，最小集覆盖。机器人运动学只需要：

向量运算（点积、叉积、归一化）
四元数旋转（避免欧拉角万向锁）
4x4 齐次变换（平移×旋转）

这三个头文件加起来不到 300 行，覆盖 90% 的机器人数学需求。

二、Vec3 向量运算

2.1 数据结构

// Vec3.h · 三维向量
struct Vec3 {
    double x = 0.0;
    double y = 0.0;
    double z = 0.0;

    Vec3() = default;
    Vec3(double x_, double y_, double z_) : x(x_), y(y_), z(z_) {}
};

2.2 核心运算

// Vec3.h · 点积：投影运算，结果是标量
double dot(const Vec3& o) const {
    return x * o.x + y * o.y + z * o.z;
}

// Vec3.h · 叉积：垂直向量，结果垂直于两输入向量（右手定则）
Vec3 cross(const Vec3& o) const {
    return {y * o.z - z * o.y,
            z * o.x - x * o.z,
            x * o.y - y * o.x};
}

// Vec3.h · 归一化：除以长度得到单位向量
Vec3 normalized() const {
    double len = length();
    if (len < 1e-12) return {0, 0, 0};  // 防止除零
    return *this / len;
}

// Vec3.h · 向量长度
double length() const { return std::sqrt(x * x + y * y + z * z); }

2.3 几何意义速查

运算	几何意义
点积	两个向量夹角余弦 × 各自长度的乘积，直观理解是 a 在 b 方向上的投影长度
叉积	垂直于 a、b 所在平面的新向量，方向由右手定则决定
归一化	方向不变，长度压缩到1，保留"指向"丢掉"远近"

2.4 运算符重载

// Vec3.h · 运算符重载：让向量运算像数字一样写
Vec3 operator+(const Vec3& o) const { return {x + o.x, y + o.y, z + o.z}; }
Vec3 operator-(const Vec3& o) const { return {x - o.x, y - o.y, z - o.z}; }
Vec3 operator*(double s) const { return {x * s, y * s, z * s}; }  // 数乘
Vec3 operator-() const { return {-x, -y, -z}; }                   // 相反向量

// 使用示例
Vec3 a(1, 0, 0);
Vec3 b(0, 1, 0);
Vec3 c = a + b;              // (1, 1, 0)
double d = a.dot(b);         // 0（垂直）
Vec3 e = a.cross(b);         // (0, 0, 1)（Z轴方向）

三、Quaternion 四元数

3.1 为什么不用欧拉角

欧拉角的致命问题：万向锁（Gimbal Lock）。

当绕第二根轴旋转 90° 时，第一根轴和第三根轴的旋转方向会重合——原本三个独立的旋转变成只有两个有效。四元数用一个 4 个数的组合（w, x, y, z）表示三维旋转，永远不会丢失自由度。

// Quaternion.h · 四元数：q = w + xi + yj + zk
struct Quaternion {
    double w = 1.0;  // 实部
    double x = 0.0;  // 虚部i
    double y = 0.0;  // 虚部j
    double z = 0.0;  // 虚部k
};

四元数的物理意义：绕轴旋转 θ 角——实部 w 存的是旋转角度的一半的余弦，虚部 xyz 存的是旋转轴方向乘以角度一半的正弦。一句话理解：一个四元数 = 一次旋转。

// Quaternion.h · 从轴角创建四元数
static Quaternion fromAxisAngle(const Vec3& axis, double angleRad) {
    double half = angleRad * 0.5;
    double s = std::sin(half);
    Vec3 a = axis.normalized();  // 轴必须归一化
    return {std::cos(half), a.x * s, a.y * s, a.z * s};
}

// 从欧拉角ZYX创建四元数
static Quaternion fromEulerZYX(double roll, double pitch, double yaw) {
    double cr = std::cos(roll * 0.5), sr = std::sin(roll * 0.5);
    double cp = std::cos(pitch * 0.5), sp = std::sin(pitch * 0.5);
    double cy = std::cos(yaw * 0.5), sy = std::sin(yaw * 0.5);
    return {
        cr * cp * cy + sr * sp * sy,   // w
        sr * cp * cy - cr * sp * sy,   // x
        cr * sp * cy + sr * cp * sy,   // y
        cr * cp * sy - sr * sp * cy    // z
    };
}

3.2 四元数乘法 = 旋转合成

两次旋转等于一次四元数乘法。先旋转再旋转，效果等同于把两个四元数相乘——这就是旋转叠加的"数学快捷键"：

// Quaternion.h · 四元数乘法：q1 * q2 = 先应用q2，再应用q1
Quaternion operator*(const Quaternion& o) const {
    return {
        w * o.w - x * o.x - y * o.y - z * o.z,
        w * o.x + x * o.w + y * o.z - z * o.y,
        w * o.y - x * o.z + y * o.w + z * o.x,
        w * o.z + x * o.y - y * o.x + z * o.w
    };
}

// 示例：绕Z轴旋转90°
Quaternion q = Quaternion::fromAxisAngle({0, 0, 1}, M_PI / 2);

3.3 用四元数旋转向量

四元数旋转向量的方法是"三明治公式"：把向量夹在两个四元数中间——左边是旋转四元数本身，右边是它的镜像（共轭），乘出来的结果就是旋转后的向量。就像把向量包在面包里，夹心变成了旋转后的位置：

来自 linuxros.cn · linuxROS

// Quaternion.h · 用四元数旋转向量
Vec3 rotate(const Vec3& v) const {
    // 把向量嵌入四元数（w=0）
    Quaternion qv(0, v.x, v.y, v.z);
    // q * v * q⁻¹ = 旋转后的向量
    Quaternion result = (*this) * qv * conjugate();
    return {result.x, result.y, result.z};
}

// 示例
Quaternion q = Quaternion::fromAxisAngle({0, 0, 1}, M_PI / 2);  // 绕Z轴转90°
Vec3 v = q.rotate({1, 0, 0});  // (1,0,0)旋转后 → (0,1,0)

3.4 共轭与逆

// Quaternion.h · 共轭：虚部取反
Quaternion conjugate() const { return {w, -x, -y, -z}; }

// 归一化四元数的逆 = 共轭
Quaternion inverse() const { return conjugate(); }

// 归一化
Quaternion normalized() const {
    double n = norm();
    if (n < 1e-12) return identity();
    return {w / n, x / n, y / n, z / n};
}

3.5 四元数与欧拉角转换

// Quaternion.h · 四元数转欧拉角（ZYX顺序）
Vec3 toEulerZYX() const {
    double sinr_cosp = 2 * (w * x + y * z);
    double cosr_cosp = 1 - 2 * (x * x + y * y);
    double roll = std::atan2(sinr_cosp, cosr_cosp);

    double sinp = 2 * (w * y - z * x);
    double pitch;
    if (std::abs(sinp) >= 1)
        pitch = std::copysign(M_PI / 2, sinp);  // 万向锁保护
    else
        pitch = std::asin(sinp);

    double siny_cosp = 2 * (w * z + x * y);
    double cosy_cosp = 1 - 2 * (y * y + z * z);
    double yaw = std::atan2(siny_cosp, cosy_cosp);

    return {roll, pitch, yaw};
}

3.6 对比：四元数 vs 欧拉角 vs 旋转矩阵

维度	四元数	欧拉角	旋转矩阵
存储	4个数	3个数	9个数
万向锁	无	有	无
插值（Slerp）	方便	困难	不方便
复合旋转	乘法	加法	乘法
可读性	一般	好	差

四、Mat4 齐次变换矩阵

4.1 为什么用 4x4 而非 3x3

3x3 旋转矩阵只能表示旋转，4x4 齐次矩阵同时包含旋转+平移。多出来的第 4 行/列就是"作弊码"——把平移量藏在最后一列的顶上 3 个数里，第 4 行永远是 (0,0,0,1)，保证矩阵可逆。一次矩阵乘法就能同时完成旋转和平移：

// Mat4.h · 列主序4x4矩阵，std::array<double,16>存储
struct Mat4 {
    std::array<double, 16> data{};

    double& operator()(int row, int col) { return data[col * 4 + row]; }
    double  operator()(int row, int col) const { return data[col * 4 + row]; }
};

4.2 矩阵乘法（链式变换）

// Mat4.h · 矩阵乘法：列主序遍历
Mat4 operator*(const Mat4& o) const {
    Mat4 r;
    r.data.fill(0);
    for (int i = 0; i < 4; ++i) {        // 行
        for (int k = 0; k < 4; ++k) {    // 中间列
            double aik = (*this)(i, k);
            for (int j = 0; j < 4; ++j) { // 列
                r(i, j) += aik * o(k, j);
            }
        }
    }
    return r;
}

// 示例：先平移再旋转 ≠ 先旋转再平移
Mat4 T = Mat4::translate({1, 0, 0});     // 平移1米
Mat4 R = Mat4::rotate(axis, angle);       // 绕某轴旋转
Mat4 A = T * R;  // 旋转后平移（绕自身轴）
Mat4 B = R * T;  // 平移后旋转（绕世界轴）

4.3 从四元数创建旋转矩阵

// Mat4.h · 四元数 → 旋转矩阵
static Mat4 rotate(const Quaternion& q) {
    Mat4 m;
    double xx = q.x * q.x, yy = q.y * q.y, zz = q.z * q.z;
    double xy = q.x * q.y, xz = q.x * q.z, yz = q.y * q.z;
    double wx = q.w * q.x, wy = q.w * q.y, wz = q.w * q.z;

    m(0, 0) = 1 - 2 * (yy + zz); m(0, 1) = 2 * (xy - wz);     m(0, 2) = 2 * (xz + wy);
    m(1, 0) = 2 * (xy + wz);     m(1, 1) = 1 - 2 * (xx + zz); m(1, 2) = 2 * (yz - wx);
    m(2, 0) = 2 * (xz - wy);     m(2, 1) = 2 * (yz + wx);     m(2, 2) = 1 - 2 * (xx + yy);
    return m;
}

4.4 点 vs 向量的区别

机器人学中，点（位置）和向量（方向）有本质区别——前者是"我在哪"，后者是"往哪指"。变换一个点时需要加上平移量（第 4 列），变换一个方向时平移不应该影响方向：

// Mat4.h · 变换点：包含平移（w=1）
Vec3 transformPoint(const Vec3& p) const {
    return {
        (*this)(0, 0) * p.x + (*this)(0, 1) * p.y + (*this)(0, 2) * p.z + (*this)(0, 3),
        (*this)(1, 0) * p.x + (*this)(1, 1) * p.y + (*this)(1, 2) * p.z + (*this)(1, 3),
        (*this)(2, 0) * p.x + (*this)(2, 1) * p.y + (*this)(2, 2) * p.z + (*this)(2, 3)
    };
}

// 向量变换不含平移：直接用3x3旋转部分（w=0）
Vec3 transformVector(const Vec3& v) const {
    return {
        (*this)(0, 0) * v.x + (*this)(0, 1) * v.y + (*this)(0, 2) * v.z,
        (*this)(1, 0) * v.x + (*this)(1, 1) * v.y + (*this)(1, 2) * v.z,
        (*this)(2, 0) * v.x + (*this)(2, 1) * v.y + (*this)(2, 2) * v.z
    };
}

// 示例：关节末端位置是点（需要平移），关节轴方向是向量（不需要平移）

4.5 平移与旋转的工厂方法

// Mat4.h · 平移矩阵
static Mat4 translate(const Vec3& t) {
    Mat4 m;
    m(0, 3) = t.x;
    m(1, 3) = t.y;
    m(2, 3) = t.z;
    return m;
}

// 完整变换示例：机器人基座到末端
Mat4 baseToEnd = Mat4::translate({0, 0, 0.5})         // 基座上方0.5m
                * Mat4::rotate(Quaternion::fromAxisAngle({0,1,0}, 0.3))  // 绕Y轴转0.3rad
                * Mat4::translate({0.3, 0, 0});      // 再沿X推进0.3m

五、实战：机器人FK中的矩阵链

T1 文章中的 DH 变换链本质是矩阵连乘：

// DHChain.cpp · FK就是矩阵链乘
common::Mat4 DHChain::forwardKinematics(const std::vector<double>& jointAngles, int endLink) const {
    common::Mat4 result = common::Mat4::identity();  // 单位矩阵起步

    for (int i = 0; i < lastLink; ++i) {
        common::Mat4 linkTransform = dhTransform(links_[i], jointAngles[i]);
        result = result * linkTransform;  // 链式乘积
    }
    return result;
}

每个关节的变换矩阵乘在一起，得到末端在基座坐标系下的位姿。

六、测试验证

// test_types.cpp · 向量运算测试
TEST(vec3_basic) {
    Vec3 a(1, 0, 0);
    Vec3 b(0, 1, 0);
    ASSERT_NEAR(a.dot(b), 0.0, 1e-9);    // 垂直向量点积=0
    ASSERT_NEAR(a.cross(b).z, 1.0, 1e-9); // 右手定则
    return true;
}

// test_types.cpp · 四元数旋转测试
TEST(quaternion_rotate) {
    Quaternion q = Quaternion::fromAxisAngle({0, 0, 1}, M_PI / 2);  // 绕Z轴90°
    Vec3 v = q.rotate({1, 0, 0});
    ASSERT_NEAR(v.x, 0.0, 1e-6);  // (1,0,0)转90°后是(0,1,0)
    ASSERT_NEAR(v.y, 1.0, 1e-6);
    return true;
}

// test_types.cpp · 矩阵变换测试
TEST(mat4_transform) {
    Mat4 m = Mat4::translate({1, 2, 3});
    Vec3 p(0, 0, 0);
    Vec3 result = m.transformPoint(p);
    ASSERT_NEAR(result.x, 1.0, 1e-9);
    ASSERT_NEAR(result.y, 2.0, 1e-9);
    ASSERT_NEAR(result.z, 3.0, 1e-9);
    return true;
}

测试结果：15/15 通过

七、总结

自研数学库的核心理念是最小功能集：

Vec3  : 向量运算（点积/叉积/归一化）→ 约50行
Quaternion: 旋转表示（轴角/欧拉角/Slerp）→ 约90行
Mat4  : 齐次变换（平移×旋转/点变换）→ 约80行
总计  : 220行头文件，零依赖

相比 Eigen 的千头文件，这个数学库只解决机器人运动学中实际用到的功能。编译速度秒级，代码可读性强，出了问题也容易调试。

下期预告：机器人的动作怎么自然过渡？MotionGraph 运动图谱 + Slerp 姿态插值 + QP 二次规划关节平滑。

代码仓库：D:\code\test\motion_retargeting
源码路径：src/common/Vec3.h, src/common/Quaternion.h, src/common/Mat4.h