一、三维空间的位姿描述和齐次变换

首先规定一个坐标系，相对于该坐标系，点的位置可以用3维列向量表示;刚体的方位可用3×3的旋转矩阵来表示。而4×4的齐次变换矩阵则可将刚体位置和姿态（位姿）的描述统一起来，它具有以下优点：

(1) 它可描述刚体的位姿，描述坐标系的相对位姿（描述）。

(2) 它可表示点从一个坐标系的描述转换到另一坐标系的描述（映射）。

(3)它可表示刚体运动前、后位姿描述的变换（算子）。

1、位置描述——位置矢量

$_{}^{A}\textrm{P}=\begin{bmatrix} P_{x}\\ P_{y} \\ P_{z} \end{bmatrix}$

2、方位的描述——旋转矩阵

以后经常用到的旋转变换矩阵是绕X轴、绕Y轴或绕Z轴转一角度 $\theta$ 。它们是

$R(X,\theta )=\begin{bmatrix} 1 & 0&0 \\ 0 & cos\theta& -sin\theta \\ 0 & sin\theta & cos\theta \end{bmatrix}$

$R(Y,\theta )=\begin{bmatrix} cos\theta & 0&sin\theta \\ 0 & 1& 0\\ -sin\theta & 0 & cos\theta \end{bmatrix}$

$R(Z,\theta )=\begin{bmatrix} cos\theta & -sin\theta&0 \\ sin\theta & cos\theta& 0\\ 0 & 0 & 1 \end{bmatrix}$

向量a经过一次旋转R和一次平移t后，得到a':

非齐次表述

$a'=R*a+t$

齐次表述

$\begin{bmatrix} a'\\ 1 \end{bmatrix} =\begin{bmatrix} R&t \\ 0^{T}& 1 \end{bmatrix}\begin{bmatrix} a\\ 1 \end{bmatrix}=T\begin{bmatrix} a\\ 1 \end{bmatrix}$

通用旋转矩阵R为：
$R=R_{z}(\beta )R_{y}(\alpha )R_{x}(\theta )$

这里的旋转矩阵旋转过程，先绕X轴旋转θ角，然后绕Y轴旋转α角，最后绕Z轴旋转β角。

绕定轴分解示意图：

将Odo坐标系先绕X轴（即Xo轴）转顺时针转90°（顺时针为负，顺时针还是逆时针需要把轴朝向自己后再判断），然后绕原Y轴 （即Yo轴）转0°，最后绕原Z轴（即Zo轴）顺时针转90°，可以转到Cam坐标系下。

所以旋转矩阵按照定轴旋转方式分解为欧拉角可以表示为：

用四元数表示为：

 Eigen::Vector3d EulerAngles_OC(-1.57, 0, -1.57);Eigen::Vector3d t_OC(0.15, 0, 1);Eigen::Matrix3d R_OC;R_OC = Eigen::AngleAxisd(EulerAngles_OC[2], Eigen::Vector3d::UnitZ()) *Eigen::AngleAxisd(EulerAngles_OC[1], Eigen::Vector3d::UnitY()) *Eigen::AngleAxisd(EulerAngles_OC[0], Eigen::Vector3d::UnitX());Eigen::Vector3d EulerAngles_OC1(0.72, 0, 0);Eigen::Matrix3d R_OC1;R_OC1 = Eigen::AngleAxisd(EulerAngles_OC1[2], Eigen::Vector3d::UnitZ()) *Eigen::AngleAxisd(EulerAngles_OC1[1], Eigen::Vector3d::UnitY()) *Eigen::AngleAxisd(EulerAngles_OC1[0], Eigen::Vector3d::UnitX());Eigen::Matrix4d T_OC = Eigen::Matrix4d::Identity();T_OC.block<3, 3>(0, 0) = R_OC * R_OC1;T_OC.block<3, 1>(0, 3) = t_OC;Eigen::Vector3d t = T_OC.block<3, 1>(0, 3);Eigen::Vector3d r = T_OC.block<3, 3>(0, 0).eulerAngles(2,1,0);Eigen::Quaterniond quaternion(T_OC.block<3, 3>(0, 0));static tf::TransformBroadcaster br;tf::Transform transform;transform.setOrigin(tf::Vector3(t[0], t[1], t[2]));tf::Quaternion q(quaternion.x(),quaternion.y(),quaternion.z(),quaternion.w());
//    q.setRPY(r[0], r[1], r[2]);transform.setRotation(q);br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "map", "camera23"));

二、旋转角与四元数之间的转换

四元数（Quaternions）是由爱尔兰数学家哈密顿（William Rowan Hamilton）在1843年提出。

三维空间的任意旋转，都可以用绕三维空间的某个轴旋转过某个角度来表示，即所谓的Axis-Angle表示方法。这种表示方法里，Axis可用一个三维向量(x,y,z)来表示，θ可以用一个角度值来表示，直观来讲，一个四维向量(θ,x,y,z)(θ,x,y,z)就可以表示出三维空间任意的旋转。注意，这里的三维向量(x,y,z)只是用来表示axis的方向朝向，因此更紧凑的表示方式是用一个单位向量来表示方向axis，而用该三维向量的长度来表示角度值θ。这样以来，可以用一个三维向量(θ∗x,θ∗y,θ∗z)就可以表示出三维空间任意的旋转，前提是其中(x,y,z)是单位向量。这就是旋转向量(Rotation Vector)的表示方式，OpenCV里大量使用的就是这种表示方法来表示旋转(见OpenCV相机标定部分的rvec)。

单位向量(x,y,z)旋转θ角度后的四元数：(cosθ/2, x*sinθ/2, y*sinθ/2, z*sinθ/2)
对于三维坐标的旋转，可以通过四元数乘法直接操作，与旋转矩阵操作可以等价，但是表示方式更加紧凑，计算量也可以小一些。

首先是轴角到四元数：

给定一个单位长度的旋转轴(x, y, z)和一个角度θ。对应的四元数为：q=((x,y,z)sinθ/2, cosθ/2)

欧拉角到四元数：

给定一个欧拉旋转(X, Y, Z)（即分别绕x轴、y轴和z轴旋转X、Y、Z度），则对应的四元数为：

x = sin(Y/2)sin(Z/2)cos(X/2)+cos(Y/2)cos(Z/2)sin(X/2)
y = sin(Y/2)cos(Z/2)cos(X/2)+cos(Y/2)sin(Z/2)sin(X/2)
z = cos(Y/2)sin(Z/2)cos(X/2)-sin(Y/2)cos(Z/2)sin(X/2)
w = cos(Y/2)cos(Z/2)cos(X/2)-sin(Y/2)sin(Z/2)sin(X/2)

q = ((x, y, z), w)

注：q和-q表示同一个旋转

欧拉角奇异性：https://blog.csdn.net/lipi37/article/details/83871031

万向节死锁：https://blog.csdn.net/AndrewFan/article/details/60981437#

三、eigen 代码

1、表示：旋转矩阵直接使用 Matrix3d 或 Matrix3f：Eigen::Matrix3d rotationMatrix=Eigen::Matrix3d::Identity();//初始化为一个单位阵。旋转向量使用 AngleAxis：Eigen::AngleAxisd rotationVector(M_PI/4,Eigen::Vector3d(0,0,1)); //角+轴：沿 Z 轴旋转 45 度欧拉角：Eigen::Vector3d ea0(yaw,pitching,droll);2、转化：旋转向量->旋转矩阵：rotationMatrix=rotation_vector.toRotationMatrix();旋转向量->四元数：Eigen::Quaterniond q = Eigen::Quaterniond ( rotation_vector );旋转矩阵->四元数：Eigen::Quaterniond q = Eigen::Quaterniond ( rotation_matrix );四元素->旋转矩阵：Eigen::Matrix3d Rx = q.toRotationMatrix();旋转向量->欧拉角：Eigen::Vector3d eulerAngle=rotationVector.matrix().eulerAngles(0,1,2);旋转矩阵->欧拉角：Eigen::Vector3d euler_angles = rotation_matrix.eulerAngles ( 2,1,0 ); // ZYX顺序，即roll pitch yaw顺序

#include <iostream>
#include <cmath>
using namespace std;#include <Eigen/Core>
// Eigen 几何模块
#include <Eigen/Geometry>/****************************
* 本程序演示了 Eigen 几何模块的使用方法
****************************/int main ( int argc, char** argv )
{// Eigen/Geometry 模块提供了各种旋转和平移的表示// 3D 旋转矩阵直接使用 Matrix3d 或 Matrix3fEigen::Matrix3d rotation_matrix = Eigen::Matrix3d::Identity();// 旋转向量使用 AngleAxis, 它底层不直接是Matrix，但运算可以当作矩阵（因为重载了运算符）Eigen::AngleAxisd rotation_vector ( M_PI/4, Eigen::Vector3d ( 0,0,1 ) );     //沿 Z 轴旋转 45 度cout .precision(3);cout<<"rotation matrix =\n"<<rotation_vector.matrix() <<endl;                //用matrix()转换成矩阵// 也可以直接赋值rotation_matrix = rotation_vector.toRotationMatrix();// 用 AngleAxis 可以进行坐标变换Eigen::Vector3d v ( 1,0,0 );Eigen::Vector3d v_rotated = rotation_vector * v;cout<<"(1,0,0) after rotation = "<<v_rotated.transpose()<<endl;// 或者用旋转矩阵v_rotated = rotation_matrix * v;cout<<"(1,0,0) after rotation = "<<v_rotated.transpose()<<endl;// 欧拉角: 可以将旋转矩阵直接转换成欧拉角Eigen::Vector3d euler_angles = rotation_matrix.eulerAngles ( 2,1,0 ); // ZYX顺序，即roll pitch yaw顺序cout<<"yaw pitch roll = "<<euler_angles.transpose()<<endl;// 欧氏变换矩阵使用 Eigen::IsometryEigen::Isometry3d T=Eigen::Isometry3d::Identity();                // 虽然称为3d，实质上是4＊4的矩阵T.rotate ( rotation_vector );                                     // 按照rotation_vector进行旋转T.pretranslate ( Eigen::Vector3d ( 1,3,4 ) );                     // 把平移向量设成(1,3,4)cout << "Transform matrix = \n" << T.matrix() <<endl;// 用变换矩阵进行坐标变换Eigen::Vector3d v_transformed = T*v;                              // 相当于R*v+tcout<<"v tranformed = "<<v_transformed.transpose()<<endl;// 对于仿射和射影变换，使用 Eigen::Affine3d 和 Eigen::Projective3d 即可，略// 四元数// 可以直接把AngleAxis赋值给四元数，反之亦然Eigen::Quaterniond q = Eigen::Quaterniond ( rotation_vector );cout<<"quaternion = \n"<<q.coeffs() <<endl;   // 请注意coeffs的顺序是(x,y,z,w),w为实部，前三者为虚部// 也可以把旋转矩阵赋给它q = Eigen::Quaterniond ( rotation_matrix );cout<<"quaternion = \n"<<q.coeffs() <<endl;// 使用四元数旋转一个向量，使用重载的乘法即可v_rotated = q*v; // 注意数学上是qvq^{-1}cout<<"(1,0,0) after rotation = "<<v_rotated.transpose()<<endl;return 0;
}

三维空间的位姿描述和齐次变换相关推荐

如何使用matlab进行二维和三维空间的位姿描述及坐标系的变换
本篇文章主要与大家分享一下如何使用matlab进行二维和三维空间的位姿描述及坐标系的变换,文章内容处于更新和补充中,(我同时安装了机器人工具箱9.10版本和10.4版本,因此文章也介绍了两种版本工 ...
【机器人学与计算机视觉基础】（一）位置与姿态描述 3 三维空间位姿描述
文章目录位置与姿态描述 3 三维空间位姿描述 3.1 三维空间姿态描述 3.1.1 正交旋转矩阵 3.1.2 三角度表示法 3.1.3 双向量表示法 3.1.4 绕任意向量旋转 3.1.5 单位四元 ...
机器人与matlab—Robotics Toolbox—01空间位姿描述
机器人学与matlab-Robotics Toolbox-01空间位姿描述 1.二维空间位姿描述 2.三维空间位姿描述 2.1 正交旋转矩阵 2.2 三角度表示法(欧拉角.RPY角) 2.3 等效轴角 ...
《动手学机器人学》第四节(上):位姿描述
本系列教程作者:小鱼公众号:鱼香ROS QQ交流群:139707339 教学视频地址:小鱼的B站完整文档地址:鱼香ROS官网版权声明:如非允许禁止转载与商业用途. 目录 <动手学机器人学& ...
机器人学——1.1-二维空间位姿描述
二维空间位姿描述二维世界或平面,是我们在高中学习欧几里得几何时就熟悉的.笛卡儿坐标系,或以 x x x 轴和 y y y 轴为正交轴的坐标系,通常绘制成 x x x 轴水平. y y y 轴竖直,两 ...
[VP] 投影空间坐标和线的齐次变换
投影空间坐标和线的齐次变换一.基本原理公式二.代码实现 1.证明共线点 x1,x2,x3x_{1},x_{2}, x_{3}x1,x2,x3 在线 lll 上 2.证明xnewx_{new} ...
机器人学——1.2-三维空间位姿描述
三维情况实际上是之前讨论的二维情况的延伸.我们在二维坐标系上增加一个额外的坐标轴,通常用 zzz 表示,它同时与 xxx 轴和 yyy 轴正交.zzz 轴的方向服从右手规则,并构成右手坐标系.与各坐标 ...
六自由度机器人逆向运动学_【课程笔记】Notes for Robotics/机器人学 (Part1)
之前打ICRA2018的Tidy Up My Room challenge的时候就已经接触了一些机器人学的内容,不过以偏应用的ROS,轨迹规划,控制居多,对于机械臂和力学确实了解的不多,这学期选了ME ...
机器人学回炉重造（1-2）：各种典型机械臂的正运动学建模（标准D-H法）
文章目录写在前面三连杆平面机械臂平行四边形操作臂闭链结构例:平行四边形操作臂球形臂拟人臂球腕斯坦福机械臂带球形手腕的拟人化机械臂 DLR机械臂参考文献写在前面本文所有机械臂均 ...
MATLAB Robotics Toolbox(Release 10)模块库--总结（一）
在MATLAB命令行窗口输入 >>roblocks 出现模块库,如下图: 目录 (一)Toolbox function: 1.1 SE3 operations三维空间中的操作 1.2 SE ...

三维空间的位姿描述和齐次变换