齐次变换矩阵、欧拉角

齐次变换矩阵

因为老是忘记齐次变换矩阵的含义以及方向,每次推导公式都很费劲,写下这篇文章用于快速回顾齐次变换矩阵。

表示的是:坐标系A到坐标系B的齐次变换矩阵,也是坐标系B在坐标系A下的位姿。

对于这个矩阵,有三个物理意义:

复制代码
(1)坐标变换:通过变换矩阵可以获得{B}坐标系下的向量(坐标)在{A}坐标系下的描述(坐标)。

(2)坐标系的描述:描述了坐标系{B}在{A}下的位姿(位置和姿态)

(3)可以作为算子,将{B}中的矢量或物体变换到{A}中,描述了某个刚体的运动情况。

将B坐标系下的点或者向量左乘这个齐次变换矩阵,能够得到坐标系A下的坐标描述

后续的讲解均以这个齐次变换矩阵进行描述。

旋转矩阵如何计算

常用右手系。首先要确定旋转的正反方向,用右手的大拇指指向旋转轴的正方向,弯曲手指手指。手指方向即是轴的正旋转方向。

那么绕各个坐标轴的旋转θ角可以用如下的旋转矩阵R(注意是左乘)表示:

平移矩阵如何计算

坐标系A是固定坐标系,坐标系B进行移动。

其实就是坐标系原点的偏移,平移部分其实就是坐标系B的原点在坐标系A下的坐标

实例

一开始坐标系B和坐标系A重合,首先**B相对于A的Z轴逆时针(视线沿着z轴正方向)旋转90°(此时旋转方向为负,应该取-90°),然后沿着A的x轴移动-3.75个单位,沿着A的y轴移动26.25个方向,**那么最终的齐次变换矩阵为

欧拉角和齐次变换矩阵互相转换

欧拉角以ZYX顺序,输入为弧度

cpp 复制代码
Eigen::Matrix3d rpy2R(const Eigen::Vector3d& rpy) // 单位为弧度
{
	double roll = rpy[0]; // A - Roll (绕X轴旋转)
	double pitch = rpy[1];// B - Pitch (绕Y轴旋转)
	double yaw = rpy[2];// C - Yaw (绕Z轴旋转)

	Eigen::Matrix3d rotation_matrix; // 按照ZYX 即yaw pitch roll顺序计算得到
	rotation_matrix = Eigen::AngleAxisd(yaw, Eigen::Vector3d::UnitZ())
		* Eigen::AngleAxisd(pitch, Eigen::Vector3d::UnitY())
		* Eigen::AngleAxisd(roll, Eigen::Vector3d::UnitX());

	return rotation_matrix; // 输出单位为mm

}
cpp 复制代码
Eigen::Vector3d R2ypr(const Eigen::Matrix3d& rotation_matrix) // 旋转矩阵必须严格按照ZYX顺序得到的,且单位为mm
{
	double roll, pitch, yaw;

	if (rotation_matrix(2, 0) < 1) {
		if (rotation_matrix(2, 0) > -1) {
			pitch = asin(-rotation_matrix(2, 0));
			yaw = atan2(rotation_matrix(1, 0), rotation_matrix(0, 0));
			roll = atan2(rotation_matrix(2, 1), rotation_matrix(2, 2));
		}
		else { // 仰角为 -90 度
			pitch = -M_PI / 2.0;
			yaw = -atan2(-rotation_matrix(1, 2), rotation_matrix(1, 1));
			roll = 0;
		}
	}
	else { // 俯角为 90 度
		pitch = M_PI / 2.0;
		yaw = atan2(-rotation_matrix(1, 2), rotation_matrix(1, 1));
		roll = 0;
	}

	// 将弧度转换为度数
	roll = roll * 180.0 / M_PI;
	pitch = pitch * 180.0 / M_PI;
	yaw = yaw * 180.0 / M_PI;

	Eigen::Vector3d rpy;
	rpy[0] = roll, rpy[1] = pitch, rpy[2] = yaw;
	return rpy; // 输出单位为度数
}
相关推荐
爱学习的小鱼gogo13 小时前
python 单词搜索(回溯-矩阵-字符串-中等)含源码(二十)
开发语言·数据结构·python·矩阵·字符串·回溯·递归栈
熬了夜的程序员16 小时前
【LeetCode】82. 删除排序链表中的重复元素 II
数据结构·算法·leetcode·链表·职场和发展·矩阵·深度优先
前端小L17 小时前
动态规划的“降维”艺术:二维矩阵中的建筑奇迹——最大矩形
线性代数·矩阵
小龙19 小时前
【理论知识】Q/K/V权重矩阵学习笔记
矩阵·大模型·transformer·多头注意力机制·理论基础
无限进步_19 小时前
【C语言】在矩阵中高效查找数字的算法解析
c语言·开发语言·数据结构·c++·其他·算法·矩阵
张晓~183399481212 天前
碰一碰发抖音源码技术搭建部署方案
线性代数·算法·microsoft·矩阵·html5
woshihonghonga2 天前
PyTorch矩阵乘法函数区别解析与矩阵高级索引说明——《动手学深度学习》3.6.3、3.6.4和3.6.5 (P79)
人工智能·pytorch·python·深度学习·jupyter·矩阵
郝学胜-神的一滴2 天前
矩阵的奇异值分解(SVD)及其在计算机图形学中的应用
程序人生·线性代数·算法·矩阵·图形渲染
豆沙沙包?3 天前
2025年--Lc201- 378. 有序矩阵中第 K 小的元素(排序)--Java版
java·线性代数·矩阵
_码力全开_4 天前
P1005 [NOIP 2007 提高组] 矩阵取数游戏
java·c语言·c++·python·算法·矩阵·go