网站建设项目文档,常州发布信息的有什么网站,物流网站建设推广,asp网站建设专家序号内容1【数理知识】自由度 degree of freedom 及自由度的计算方法2【数理知识】刚体 rigid body 及刚体的运动3【数理知识】刚体基本运动#xff0c;平动#xff0c;转动4【数理知识】向量数乘#xff0c;内积#xff0c;外积#xff0c;matlab代码实现5【数理知识】最…序号内容1【数理知识】自由度 degree of freedom 及自由度的计算方法2【数理知识】刚体 rigid body 及刚体的运动3【数理知识】刚体基本运动平动转动4【数理知识】向量数乘内积外积matlab代码实现5【数理知识】最小二乘法从线性回归出发数值举例并用最小二乘法求解回归模型6【数理知识】最小二乘法一般线性情况矩阵化表示过程最佳参数的求解公式过程7【数理知识】协方差随机变量的的协方差随机变量分别是单个数字和向量时的协方差8【数理知识】奇异值分解从数据的线性变换角度来理解9【数理知识】旋转矩阵的推导过程基于向量的旋转来实现同时解决欧式变换的非线性局限10【数理知识】三维空间旋转矩阵的欧拉角表示法四元数表示法两者之间的转换Matlab 代码实现11【数理知识】已知 N3 个点在前后时刻的坐标求刚体平移矩阵旋转矩阵且这 N3 点间距离始终不变代表一个刚体 文章目录 1. 欧拉角Euler Angles表示法2. 四元数Quaternion表示法3. 四元数转欧拉角4. 欧拉角转四元数Ref 之前我们已经讨论过旋转矩阵。需要再次强调的是旋转的顺序很重要并且会影响最终的结果。先旋转 X X X 轴再旋转 Y Y Y 轴最后旋转 Z Z Z 轴得到的结果与先旋转 Z Z Z 轴再旋转 Y Y Y 轴最后旋转 X X X 轴得到的结果是不同的。这种顺序的差异导致了不同的方向和空间方向的变化。这也是为什么在实际应用中我们需要明确指定旋转顺序以确保我们得到正确和一致的结果。
这次基于三维空间讨论下旋转矩阵的两种表示方法分别是欧拉角表示法四元数表示法以及二者之间的转换关系如何。
在三维空间中旋转矩阵 R R R 的维度为 3 × 3 3 \times 3 3×3其是一个正交矩阵行列式为 1 1 1。
1. 欧拉角Euler Angles表示法
欧拉角通常由三个角度组成
滚转角roll常用符号为 ϕ \phi ϕ俯仰角pitch常用符号为 θ \theta θ偏航角yaw常用符号为 ψ \psi ψ
这三个角度分别描述了绕 X , Y , Z X, Y, Z X,Y,Z 轴旋转的角度。
绕 X X X 轴旋转 ϕ \phi ϕ 角度的旋转矩阵为 R x ( ϕ ) [ 1 0 0 0 cos ( ϕ ) − sin ( ϕ ) 0 sin ( ϕ ) cos ( ϕ ) ] R_x(\phi) \left[\begin{matrix} 1 0 0 \\ 0 \cos(\phi) -\sin(\phi) \\ 0 \sin(\phi) \cos(\phi) \\ \end{matrix}\right] Rx(ϕ) 1000cos(ϕ)sin(ϕ)0−sin(ϕ)cos(ϕ)
绕 Y Y Y 轴旋转 θ \theta θ 角度的旋转矩阵为 R y ( θ ) [ cos ( θ ) 0 sin ( θ ) 0 1 0 − sin ( θ ) 0 cos ( θ ) ] R_y(\theta) \left[\begin{matrix} \cos(\theta) 0 \sin(\theta) \\ 0 1 0 \\ -\sin(\theta) 0 \cos(\theta) \\ \end{matrix}\right] Ry(θ) cos(θ)0−sin(θ)010sin(θ)0cos(θ)
绕 Z Z Z 轴旋转 ψ \psi ψ 角度的旋转矩阵为 R z ( ψ ) [ cos ( ψ ) − sin ( ψ ) 0 sin ( ψ ) cos ( ψ ) 0 0 0 1 ] R_z(\psi) \left[\begin{matrix} \cos(\psi) -\sin(\psi) 0 \\ \sin(\psi) \cos(\psi) 0 \\ 0 0 1 \\ \end{matrix}\right] Rz(ψ) cos(ψ)sin(ψ)0−sin(ψ)cos(ψ)0001
例如对于一个分别依次绕固定轴 X Y Z XYZ XYZ 的欧拉角表示其旋转矩阵为 R R z ( ϕ ) R y ( θ ) R x ( ψ ) [ cos ( θ ) cos ( ψ ) sin ( ϕ ) sin ( θ ) cos ( ψ ) − cos ( ϕ ) sin ( ψ ) cos ( ϕ ) sin ( θ ) cos ( ψ ) sin ( ϕ ) sin ( ψ ) cos ( θ ) sin ( ψ ) sin ( ϕ ) sin ( θ ) sin ( ψ ) cos ( ϕ ) cos ( ψ ) cos ( ϕ ) sin ( θ ) sin ( ψ ) − sin ( ϕ ) cos ( ψ ) − sin ( θ ) sin ( ϕ ) cos ( θ ) cos ( ϕ ) cos ( θ ) ] \begin{aligned} R R_z(\phi) R_y(\theta) R_x(\psi) \\ \left[\begin{matrix} \cos(\theta)\cos(\psi) \sin(\phi)\sin(\theta)\cos(\psi) - \cos(\phi)\sin(\psi) \cos(\phi)\sin(\theta)\cos(\psi) \sin(\phi)\sin(\psi) \\ \cos(\theta)\sin(\psi) \sin(\phi)\sin(\theta)\sin(\psi) \cos(\phi)\cos(\psi) \cos(\phi)\sin(\theta)\sin(\psi) - \sin(\phi)\cos(\psi) \\ -\sin(\theta) \sin(\phi)\cos(\theta) \cos(\phi)\cos(\theta) \\ \end{matrix}\right] \end{aligned} RRz(ϕ)Ry(θ)Rx(ψ) cos(θ)cos(ψ)cos(θ)sin(ψ)−sin(θ)sin(ϕ)sin(θ)cos(ψ)−cos(ϕ)sin(ψ)sin(ϕ)sin(θ)sin(ψ)cos(ϕ)cos(ψ)sin(ϕ)cos(θ)cos(ϕ)sin(θ)cos(ψ)sin(ϕ)sin(ψ)cos(ϕ)sin(θ)sin(ψ)−sin(ϕ)cos(ψ)cos(ϕ)cos(θ)
这个矩阵代表了首先绕 X X X 轴旋转 ϕ \phi ϕ 角然后绕 Y Y Y 轴旋转 θ \theta θ 角再然后绕 Z Z Z 轴旋转 ψ \psi ψ 角的总的旋转效果。
更多关于欧拉角的推导和细节可参考文章第3章-数理知识基础 - 坐标转换和【数理知识】旋转矩阵的推导过程基于向量的旋转来实现同时解决欧式变换的非线性局限。
% 给定欧拉角 phi theta psi
phi deg2rad(10); % 示例10度记得转换为弧度
theta deg2rad(22); % 示例22度
psi deg2rad(35); % 示例35度R_x [ 1 0 00 cos(phi) -sin(phi)0 sin(phi) cos(phi)];R_y [ cos(theta) 0 sin(theta)0 1 0-sin(theta) 0 cos(theta)];R_z [ cos(psi) -sin(psi) 0sin(psi) cos(psi) 00 0 1];R R_z * R_y * R_x;R [cos(theta)*cos(psi) sin(phi)*sin(theta)*cos(psi)-cos(phi)*sin(psi) cos(phi)*sin(theta)*cos(psi)sin(phi)*sin(psi)cos(theta)*sin(psi) sin(phi)*sin(theta)*sin(psi)cos(phi)*cos(psi) cos(phi)*sin(theta)*sin(psi)-sin(phi)*cos(psi)-sin(theta) sin(phi)*cos(theta) cos(phi)*cos(theta)];R 0.7595 -0.5116 0.40180.5318 0.8440 0.0694-0.3746 0.1610 0.9131% 给定点坐标
point_1 [ 102235];
point_2 R * point_1;figure()
scatter3(point_1(1), point_1(2), point_1(3), 150, r); hold on;
scatter3(point_2(1), point_2(2), point_2(3), 150, b);2. 四元数Quaternion表示法
四元数是由 1 1 1 个实数加上 3 3 3 个复数组合而成通常可以表示为 q q w q x i q y j q z k q q_w q_x \text{i} q_y \text{j} q_z \text{k} qqwqxiqyjqzk
其中 q w , q x , q y , q z q_w, q_x, q_y, q_z qw,qx,qy,qz 都是实数 i, j, k \text{i, j, k} i, j, k 是四元数的基元满足如下所示的乘法关系 i 2 j 2 k 2 ijk − 1 \text{i}^2 \text{j}^2 \text{k}^2 \text{i}\text{j}\text{k} -1 i2j2k2ijk−1 i 0 j 0 k 0 1 \text{i}^0 \text{j}^0 \text{k}^0 1 i0j0k01
四元数还可看作由一个标量和一个向量组成其中 q w q_w qw 是四元数的标量部分 q x , q y , q z q_x, q_y, q_z qx,qy,qz 构成四元数的向量部分。 假设有两个四元数分别为 q 1 ( q w 1 , [ q x 1 , q y 1 , q z 1 ] ) q_1 (q_{w1}, [q_{x1}, q_{y1}, q_{z1}]) q1(qw1,[qx1,qy1,qz1]) q 2 ( q w 2 , [ q x 2 , q y 2 , q z 2 ] ) q_2 (q_{w2}, [q_{x2}, q_{y2}, q_{z2}]) q2(qw2,[qx2,qy2,qz2])同时令 v 1 [ q x 1 , q y 1 , q z 1 ] v_1 [q_{x1}, q_{y1}, q_{z1}] v1[qx1,qy1,qz1] v 2 [ q x 2 , q y 2 , q z 2 ] v_2 [q_{x2}, q_{y2}, q_{z2}] v2[qx2,qy2,qz2]则有如下运算法则
四元数的和 q 1 q 2 ( q w 1 q w 2 , ( v 1 v 2 ) ) q_1 q_2 (q_{w1}q_{w2}, (v_1 v_2)) q1q2(qw1qw2,(v1v2))四元数乘法 q 1 q 2 q w 1 q w 2 − v 1 ⋅ v 2 q w 1 v 2 q w 2 v 1 v 1 × v 2 ( q w 1 q w 2 − v 1 ⋅ v 2 , ( q w 1 v 2 q w 2 v 1 v 1 × v 2 ) ) q_1 q_2 q_{w1} q_{w2} - v_1 \cdot v_2 q_{w1} v_2 q_{w2} v_1 v_1 \times v_2 (q_{w1} q_{w2} - v_1 \cdot v_2, (q_{w1} v_2 q_{w2} v_1 v_1 \times v_2)) q1q2qw1qw2−v1⋅v2qw1v2qw2v1v1×v2(qw1qw2−v1⋅v2,(qw1v2qw2v1v1×v2))四元数的模 ∥ q 1 ∥ q w 1 2 q x 1 2 q y 1 2 q z 1 2 \|q_1\| \sqrt{q_{w1}^2 q_{x1}^2 q_{y1}^2 q_{z1}^2} ∥q1∥qw12qx12qy12qz12 单位四元数 ∥ q 1 ∥ 1 \|q_1\| 1 ∥q1∥1四元数的共轭 q 1 ∗ ( q w 1 , − v 1 ) q_1^* (q_{w1}, -v_1) q1∗(qw1,−v1)四元数的逆 q 1 − 1 q 1 ∗ ∥ q 1 ∥ q_1^{-1} \frac{q_1^*}{\|q_1\|} q1−1∥q1∥q1∗ 四元数是一个扩展的复数系统常用于表示三维空间中的旋转。
一个单位四元数长度为 1 1 1可以表示 3D 空间中的旋转。将一个点旋转到另一个位置可以通过四元数乘法来完成。
绕 X X X 轴旋转 ϕ \phi ϕ 角度的四元数为 q ϕ ( cos ( ϕ 2 ) , sin ( ϕ 2 ) , 0 , 0 ) q_\phi (\cos(\frac{\phi}{2}), \sin(\frac{\phi}{2}), 0, 0) qϕ(cos(2ϕ),sin(2ϕ),0,0)
绕 Y Y Y 轴旋转 θ \theta θ 角度的四元数为 q θ ( cos ( θ 2 ) , 0 , sin ( θ 2 ) , 0 , 0 ) q_\theta (\cos(\frac{\theta}{2}), 0, \sin(\frac{\theta}{2}), 0, 0) qθ(cos(2θ),0,sin(2θ),0,0)
绕 Z Z Z 轴旋转 ψ \psi ψ 角度的四元数为 q ψ ( cos ( ψ 2 ) , 0 , 0 , sin ( ψ 2 ) ) q_\psi (\cos(\frac{\psi}{2}), 0, 0, \sin(\frac{\psi}{2})) qψ(cos(2ψ),0,0,sin(2ψ))
总旋转的四元数是这三个四元数的乘积。四元数乘法不是通常的标量乘法它有特定的乘法规则。
给定一个四元数 q q q模长为 1有关系 q w 2 q x 2 q y 2 q z 2 1 \sqrt{q_w^2q_x^2q_y^2q_z^2}1 qw2qx2qy2qz2 1假设采用的旋转顺序为 X Y Z XYZ XYZ其对应的旋转矩阵 R R R 可以表示为 R [ 1 − 2 ( q y 2 q z 2 ) 2 ( q x q y − q w q z ) 2 ( q x q z q w q y ) 2 ( q x q y q w q z ) 1 − 2 ( q x 2 q z 2 ) 2 ( q y q z − q w q x ) 2 ( q x q z − q w q y ) 2 ( q y q z q w q x ) 1 − 2 ( q x 2 q y 2 ) ] \begin{aligned} R \left[\begin{matrix} 1 - 2(q_y^2 q_z^2) 2(q_x q_y - q_w q_z) 2(q_x q_z q_w q_y) \\ 2(q_x q_y q_w q_z) 1 - 2(q_x^2 q_z^2) 2(q_y q_z - q_w q_x) \\ 2(q_x q_z - q_w q_y) 2(q_y q_z q_w q_x) 1 - 2(q_x^2 q_y^2) \\ \end{matrix}\right] \end{aligned} R 1−2(qy2qz2)2(qxqyqwqz)2(qxqz−qwqy)2(qxqy−qwqz)1−2(qx2qz2)2(qyqzqwqx)2(qxqzqwqy)2(qyqz−qwqx)1−2(qx2qy2)
单位四元数在描述 3D 旋转时有一些优势其不受欧拉角中的 “万向锁” 问题的影响。
% 给定四元数
quaternion [0.9376 0.0244 0.2070 0.2782]; q_w quaternion(1);
q_x quaternion(2);
q_y quaternion(3);
q_z quaternion(4);% 计算旋转矩阵 R
R(1,1) 1 - 2*(q_y^2 q_z^2);
R(1,2) 2*(q_x*q_y - q_w*q_z);
R(1,3) 2*(q_x*q_z q_w*q_y);
R(2,1) 2*(q_x*q_y q_w*q_z);
R(2,2) 1 - 2*(q_x^2 q_z^2);
R(2,3) 2*(q_y*q_z - q_w*q_x);
R(3,1) 2*(q_x*q_z - q_w*q_y);
R(3,2) 2*(q_y*q_z q_w*q_x);
R(3,3) 1 - 2*(q_x^2 q_y^2);R 0.7595 -0.5116 0.40170.5318 0.8440 0.0694-0.3746 0.1609 0.91313. 四元数转欧拉角
从四元数到欧拉角的转换并不是唯一的因为对于某些旋转存在多种欧拉角表示。但是对于大多数实际应用可以从一个特定的四元数计算一个特定的欧拉角集。
给定四元数 q ( q w , q x , q y , q z ) q (q_w, q_x, q_y, q_z) q(qw,qx,qy,qz)若想将它转换为 X Y Z XYZ XYZ 顺序的欧拉角 ( ϕ , θ , ψ ) (\phi, \theta, \psi) (ϕ,θ,ψ)。以下是从四元数到欧拉角的转换方法 ϕ atan2 ( 2 ( q w q x q y q z ) , 1 − 2 ( q x 2 q y 2 ) ) θ arcsin ( 2 ( q w q y − q x q z ) ) ψ atan2 ( 2 ( q w q z q x q y ) , 1 − 2 ( q y 2 q z 2 ) ) \begin{aligned} \phi \text{atan2} (2(q_w q_x q_y q_z), 1-2(q_x^2 q_y^2)) \\ \theta \text{} \arcsin (2(q_w q_y - q_x q_z)) \\ \psi \text{atan2} (2(q_w q_z q_x q_y), 1-2(q_y^2 q_z^2)) \end{aligned} ϕθψatan2(2(qwqxqyqz),1−2(qx2qy2))arcsin(2(qwqy−qxqz))atan2(2(qwqzqxqy),1−2(qy2qz2))
其中 atan2 ( ) \text{atan2}() atan2() 不是 arctan ( ) \arctan() arctan()。
举例说明因为若使用 arctan ( y / x ) \arctan(y/x) arctan(y/x)其返回值在 [ − π / 2 , π / 2 ] [-\pi/2, \pi/2] [−π/2,π/2] 之间因为它不能区分 x x x 的正负。而 atan2 ( y , x ) \text{atan2}(y, x) atan2(y,x)其返回值在 [ − π , π ] [-\pi, \pi] [−π,π] 之间可以区分 x x x 的正负因此更为实用尤其是在计算欧拉角时。更重要的是 atan2 ( y , x ) \text{atan2}(y, x) atan2(y,x) 能够处理 x 0 x0 x0 的情况这在计算角度或欧拉角时非常有用。
注意由于使用 arcsin ( ) \arcsin() arcsin()当 θ \theta θ 接近 ± 90 ° \pm 90\degree ±90° 时可能会出现数值不稳定。这是因为在这些极端情况下航向和滚动变得不可区分这就是所谓的万向锁问题。
% 给定四元数
quaternion [0.9376 0.0244 0.2070 0.2782]; q_w quaternion(1);
q_x quaternion(2);
q_y quaternion(3);
q_z quaternion(4);% 转换四元数到欧拉角
phi atan2(2*(q_w*q_x q_y*q_z), 1 - 2*(q_x^2 q_y^2));
theta asin(2*(q_w*q_y - q_z*q_x));
psi atan2(2*(q_w*q_z q_x*q_y), 1 - 2*(q_y^2 q_z^2));% 如果需要角度形式而不是弧度可以转换为度
phi_deg rad2deg(phi);
theta_deg rad2deg(theta);
psi_deg rad2deg(psi);phi_deg 9.9953theta_deg 21.9990psi_deg 34.99834. 欧拉角转四元数
给定三个欧拉角 ϕ , θ , ψ \phi, \theta, \psi ϕ,θ,ψ相应的四元数为 q w cos ( ϕ 2 ) cos ( θ 2 ) cos ( ψ 2 ) sin ( ϕ 2 ) sin ( θ 2 ) sin ( ψ 2 ) q x sin ( ϕ 2 ) cos ( θ 2 ) cos ( ψ 2 ) − cos ( ϕ 2 ) sin ( θ 2 ) sin ( ψ 2 ) q y cos ( ϕ 2 ) sin ( θ 2 ) cos ( ψ 2 ) sin ( ϕ 2 ) cos ( θ 2 ) sin ( ψ 2 ) q z cos ( ϕ 2 ) cos ( θ 2 ) sin ( ψ 2 ) − sin ( ϕ 2 ) sin ( θ 2 ) cos ( ψ 2 ) \begin{aligned} q_w \cos(\frac{\phi}{2}) \cos(\frac{\theta}{2}) \cos(\frac{\psi}{2}) \sin(\frac{\phi}{2}) \sin(\frac{\theta}{2}) \sin(\frac{\psi}{2}) \\ q_x \sin(\frac{\phi}{2}) \cos(\frac{\theta}{2}) \cos(\frac{\psi}{2}) - \cos(\frac{\phi}{2}) \sin(\frac{\theta}{2}) \sin(\frac{\psi}{2}) \\ q_y \cos(\frac{\phi}{2}) \sin(\frac{\theta}{2}) \cos(\frac{\psi}{2}) \sin(\frac{\phi}{2}) \cos(\frac{\theta}{2}) \sin(\frac{\psi}{2}) \\ q_z \cos(\frac{\phi}{2}) \cos(\frac{\theta}{2}) \sin(\frac{\psi}{2}) - \sin(\frac{\phi}{2}) \sin(\frac{\theta}{2}) \cos(\frac{\psi}{2}) \end{aligned} qwqxqyqzcos(2ϕ)cos(2θ)cos(2ψ)sin(2ϕ)sin(2θ)sin(2ψ)sin(2ϕ)cos(2θ)cos(2ψ)−cos(2ϕ)sin(2θ)sin(2ψ)cos(2ϕ)sin(2θ)cos(2ψ)sin(2ϕ)cos(2θ)sin(2ψ)cos(2ϕ)cos(2θ)sin(2ψ)−sin(2ϕ)sin(2θ)cos(2ψ)
得到的四元数是 ( q w , q x , q y , q z ) (q_w, q_x, q_y, q_z) (qw,qx,qy,qz)。
phi deg2rad(10); % 示例10度记得转换为弧度
theta deg2rad(22); % 示例22度
psi deg2rad(35); % 示例35度% 计算四元数
q_w cos(phi/2) * cos(theta/2) * cos(psi/2) sin(phi/2) * sin(theta/2) * sin(psi/2);
q_x sin(phi/2) * cos(theta/2) * cos(psi/2) - cos(phi/2) * sin(theta/2) * sin(psi/2);
q_y cos(phi/2) * sin(theta/2) * cos(psi/2) sin(phi/2) * cos(theta/2) * sin(psi/2);
q_z cos(phi/2) * cos(theta/2) * sin(psi/2) - sin(phi/2) * sin(theta/2) * cos(psi/2);quaternion [q_w, q_x, q_y, q_z];quaternion 0.9376 0.0244 0.2070 0.2782Ref
旋转矩阵 - Wikipedia干货整理欧拉角、旋转矩阵、四元数合辑
