精准姿态控制：旋转数学表示的转换技巧__来高科技（天津）有限公司

导言

在计算机图形学、机器人学以及航空航天等领域，准确描述和转换物体的姿态是关键的技术挑战之一。为了表达物体的旋转姿态，通常采用四种主要的数学表示方式：四元数、欧拉角、旋转矩阵和旋转向量。每种表示法都有其独特的优点和局限性，适用于不同的应用场景和技术需求。理解这些表示法之间的关系，并掌握它们之间进行转换的方法，对于开发人员来说至关重要。本文旨在提供一个清晰且专业的指南，帮助开发人员调用来高科技SDK智能接口在不同表示形式间实现无缝切换，从而提升系统兼容性和计算效率。

转换过程

创建绕z轴旋转90度的旋转矩阵。

旋转矩阵转换为欧拉角，其中欧拉角的顺序为绕x轴角，绕y轴角，绕z轴角。

欧拉角转换为旋转矩阵。

旋转矩阵转换为四元数。

四元数转换为旋转矩阵。

旋转矩阵转换为旋转向量。

旋转向量转换旋转矩阵。

结果

从上图程序运行结果可以看出各个旋转描述方式间转换的结果都正确无误。注意其中带e的负十几次方的数值可以看为0。

结语

通过对四元数、欧拉角、旋转矩阵和旋转向量之间转换的深入探讨，我们不仅掌握了它们之间的转换关系，还提高了在多种旋转描述方式间切换的能力。在实际应用中，选择匹配项目的旋转表示方式能够显著简化计算流程，提高数值稳定性并减少累积误差。因此，根据具体工程需求合理选择和使用旋转表示方式对项目成功具有决定性影响。希望本篇文章能为开发人员提供有价值的参考，助力于系统设计与实现过程中的姿态控制和分析任务。

源代码

#include "Eigen/Dense"

#define PI std::acos(-1)

void copy(const Eigen::Matrix3d& from, techlego::array_2d<double, 3, 3>& to)

{

for (int i = 0; i < 3; ++i)

{

for (int j = 0; j < 3; ++j)

{

to[i][j] = from(i, j);

}

Eigen::Matrix3d euler_angles_to_rotation_matrix(const Eigen::Vector3d& theta)

{

Eigen::Matrix3d R_x; // 计算旋转矩阵的X分量

R_x <<

1, 0, 0,

0, cos(theta[0]), -sin(theta[0]),

0, sin(theta[0]), cos(theta[0]);

Eigen::Matrix3d R_y; // 计算旋转矩阵的Y分量

R_y <<

cos(theta[1]), 0, sin(theta[1]),

0, 1, 0,

-sin(theta[1]), 0, cos(theta[1]);

Eigen::Matrix3d R_z; // 计算旋转矩阵的Z分量

R_z <<

cos(theta[2]), -sin(theta[2]), 0,

sin(theta[2]), cos(theta[2]), 0,

0, 0, 1;

Eigen::Matrix3d R = R_z * R_y * R_x;

return R;

}

std::string to_string(const techlego::array_2d<double, 3, 3>& v)

{

std::stringstream ss;

for (int i = 0; i < 3; ++i)

{

for (int j = 0; j < 3; ++j)

{

ss << v[i][j] << " ";

}

ss << "\n";

}

return ss.str();

}

template<typename T>

std::string to_string(const T& v)

{

std::stringstream ss;

for (int i = 0; i < v.size(); ++i)

{

ss << v[i] << " ";

}

return ss.str();

}

int main()

{

//使用绕z轴旋转90度的旋转矩阵作为例子

Eigen::AngleAxisd ax(PI / 2, Eigen::Vector3d::UnitZ());

Eigen::Matrix3d r = ax.toRotationMatrix();

techlego::rotate_3d r3d{};

//给rotate_3d赋值

copy(r, r3d.m_r);

std::cout << "origin rotation mat:\n" << to_string(r3d.m_r) << "\n";

//旋转矩阵转为欧拉角，顺序为绕x轴，绕y轴，绕z轴

std::array<double, 3> rpy{};

r3d.to_rpy(rpy);

std::cout << "rpy:\n" << to_string(rpy) << "\n";

//欧拉角转为旋转矩阵

techlego::rotate_3d e2r{};

e2r.from_rpy(rpy);

std::cout << "euler angles to rotation matrix res:\n" << to_string(e2r.m_r) << "\n";

//旋转矩阵转四元数

std::array<double, 4> quaternion{};

r3d.to_quaternion(quaternion);

std::cout << "quaternion:\n" << to_string(quaternion) << "\n";

//四元数转旋转矩阵

techlego::rotate_3d q2r{};

q2r.from_quaternion(quaternion);

std::cout << "quaternion to rotation matrix res:\n" << to_string(q2r.m_r) << "\n";

//旋转矩阵转旋转向量

std::array<double, 3> vec{};

r3d.to_vector(vec);

std::cout << "vector:\n" << to_string(vec) << "\n";

//旋转向量转旋转矩阵

techlego::rotate_3d v2r{};

v2r.from_vector(vec);

std::cout << "vector to rotation matrix res:\n" << to_string(v2r.m_r) << "\n";

return 0;

}

官网视频号·更多视频案例·关注我们

往期回顾

多机联动扫描系统助力嫦娥五号构建月表地形

喷漆自动化三维视觉全流程

混凝土立方试块尺寸三维检测全流程

↙点击“阅读原文”查看更多精彩内容