DEMO
Created 2021.06.03 by Cong Yu; Last modified: 2021.06.03-v1.0.2
Contact: windmillyucong@163.com
Copyleft! 2022 Cong Yu. Some rights reserved.
Rotate Matrix
约定
- robot坐标系 front x, left y, up z
- 欧拉角顺序 zyx ypr yaw pitch roll
- $\Psi$ 方向或偏航 (heading or yaw)
- $\theta$ 升降或俯仰 (elevation or pitch)
- $\phi$ 倾斜或横滚 (bank or roll)
- n系 导航坐标系
- b系 机体坐标系
1. 基本概念
1.1. 表达姿态
2d姿态
\[R\{\theta\} = \begin{bmatrix} \cos\theta & -\sin\theta \\ \sin\theta & \cos\theta \end{bmatrix}\]- 2d旋转矩阵有4个数字,其实只有一个自由度
3d姿态
3D旋转矩阵可以通过三个基本旋转矩阵的组合来表示,分别是绕X轴、Y轴和Z轴的旋转:
绕X轴旋转(Roll): \(R_x(\phi) = \begin{bmatrix} 1 & 0 & 0 \\ 0 & \cos\phi & -\sin\phi \\ 0 & \sin\phi & \cos\phi \end{bmatrix}\)
绕Y轴旋转(Pitch): \(R_y(\theta) = \begin{bmatrix} \cos\theta & 0 & \sin\theta \\ 0 & 1 & 0 \\ -\sin\theta & 0 & \cos\theta \end{bmatrix}\)
绕Z轴旋转(Yaw): \(R_z(\psi) = \begin{bmatrix} \cos\psi & -\sin\psi & 0 \\ \sin\psi & \cos\psi & 0 \\ 0 & 0 & 1 \end{bmatrix}\)
完整的3D旋转矩阵可以通过这三个基本旋转矩阵的组合得到,例如按照ZYX顺序(先绕Z轴,再绕Y轴,最后绕X轴): \(R = R_x(\phi)R_y(\theta)R_z(\psi)\)
- 3D旋转矩阵有9个数字,但只有3个自由度
- 旋转矩阵是正交矩阵,满足 $R^T R = I$ 且 $\det(R) = 1$
- 旋转矩阵有 $R^{-1} = R^T$
1.2 位姿
\[T = \begin{bmatrix} R & t \\ 0^T & 1 \end{bmatrix}\]2. 表达旋转
姿态间的旋转
姿态R1到姿态R2的旋转动作有
\[\begin{gather} \\R_2 = R_1^2R_1 \\R_1^2 = R_2R_1^{-1}= R_2R_1^T \end{gather}\]点的旋转
\[p_2 = R_1^2 p_1\]逆变换
\[R^{-1} = R^T\] \[T^{-1} = \begin{bmatrix} R^T & -R^Tt \\ 0^T & 1 \end{bmatrix}\]逆变换推导
\[T = \begin{bmatrix} R & t \\ 0^T & 1\end{bmatrix}\]其中R为旋转矩阵,有特性 $R^{-1} = R^T$
线性代数公式: \(M = \begin{bmatrix} A & B \\ 0 & D \end{bmatrix}\) 求逆 \(M^{-1} = \begin{bmatrix} A^{-1} & -A^{-1}BD^{-1} \\ 0 & D^{-1} \end{bmatrix}\)
所以 \(T^{-1} = \begin{bmatrix} R^{-1} & -R^{-1}t \\ 0^T & 1 \end{bmatrix} = \begin{bmatrix} R^{T} & -R^{T}t \\ 0^T & 1 \end{bmatrix}\)
\[\begin{gather} p_g = T_b^g p_b = Rp_b + t \\ p_b = R^{-1}(p_g - t) = R^T p_g - R^T t \end{gather}\]多次旋转
旋转叠加:左乘 \(R_0^2 = R_1^2 R_0^1\)
点的旋转叠加:左乘给一个点p表达坐标系变化之后该点的坐标 \(\begin{align} p_1 &= R_0^1 p_0 \\ p_2 &= R_1^2 p_1 \\ p_2 &= R_0^2 p_0 \\ \end{align}\)
3. 旋转矩阵微分推导
对于旋转矩阵 $R(t)$,其导数满足:
\[\dot{R}(t) = R(t)[\omega(t)]_\times\]其中 $\omega$ 是角速度向量,
其中 $[\omega(t)]_\times$ 是角速度向量 $\omega(t)$ 的反对称矩阵:
\[[\omega]_\times = \begin{bmatrix} 0 & -\omega_z & \omega_y \\ \omega_z & 0 & -\omega_x \\ -\omega_y & \omega_x & 0 \end{bmatrix}\]这个方程的推导过程如下:
1) 考虑一个固定向量 $p$ 在旋转坐标系中的表示:
- 在全局坐标系中:$p_g = R(t)p_b$
- 其中 $p_b$ 是向量在旋转坐标系中的表示
2) 对时间求导: \(\dot{p}_g = \dot{R}(t)p_b\)
3) 另一方面,根据角速度的定义,固定向量在旋转坐标系中的变化率满足: \(\dot{p}_g = \omega(t) \times p_g = [\omega(t)]_\times p_g\)
4) 将 $p_g = R(t)p_b$ 代入上式: \(\dot{p}_g = [\omega(t)]_\times R(t)p_b\)
5) 联立两个表达式: \(\dot{R}(t)p_b = [\omega(t)]_\times R(t)p_b\)
6) 由于这个关系对任意 $p_b$ 都成立,因此: \(\dot{R}(t) = [\omega(t)]_\times R(t)\)
7) 最终得到旋转矩阵的微分方程: \(\dot{R}(t) = R(t)[\omega(t)]_\times\)
这个微分方程描述了旋转矩阵如何随时间演化,其中:
- $\omega(t)$ 是角速度向量
- $[\omega(t)]_\times$ 是角速度的反对称矩阵
- $R(t)$ 是旋转矩阵
这个方程是旋转矩阵积分的基础,它表明旋转矩阵的变化率与当前旋转矩阵和角速度有关。
反对称矩阵性质
反对称矩阵具有以下性质:
-
对于任意向量 $a, b \in \mathbb{R}^3$: \([a]_\times b = a \times b\)
-
反对称矩阵的幂: \([\omega]_\times^2 = \omega\omega^T - \|\omega\|^2I\)
-
反对称矩阵的指数映射: \(\exp([\omega]_\times) = I + \frac{\sin\|\omega\|}{\|\omega\|}[\omega]_\times + \frac{1-\cos\|\omega\|}{\|\omega\|^2}[\omega]_\times^2\)
旋转矩阵的指数映射
旋转矩阵可以通过指数映射从李代数映射到李群:
\[R = \exp([\omega]_\times)\]其中 $\omega$ 是旋转向量,其方向表示旋转轴,大小表示旋转角度。
旋转矩阵的扰动
对于旋转矩阵 $R$,其扰动可以表示为:
\[R' = R\exp([\delta\omega]_\times) \approx R(I + [\delta\omega]_\times)\]其中 $\delta\omega$ 是一个小扰动。
旋转矩阵的复合导数
对于复合旋转 $R = R_1R_2$,其导数为:
\[\dot{R} = \dot{R}_1R_2 + R_1\dot{R}_2\]4. 旋转矩阵表达下的陀螺仪积分
对于陀螺仪测量得到的角速度 $\omega(t)$,我们可以通过以下方式对旋转矩阵进行积分:
4.1. 连续时间下的积分:
对于旋转矩阵 $R(t)$,其导数满足: \(\dot{R}(t) = R(t)[\omega(t)]_\times\)
对于微分方程 $\dot{R}(t) = R(t)[\omega(t)]_\times$,有积分:
解析解(当角速度为常数时):
- 如果角速度 $\omega$ 在时间间隔 $[t_0, t]$ 内为常数
- 则解为:$R(t) = R(t_0)\exp([\omega(t-t_0)]_\times)$
- 其中 $\exp([\omega(t-t_0)]_\times)$ 可以通过 Rodrigues 公式计算
4.2. 离散时间下的积分:
\(R_{k+1} = R_k \exp([\omega_k \Delta t]_\times)\)
其中:
- $R_k$ 是当前时刻的旋转矩阵
- $\omega_k$ 是当前时刻测量的角速度
- $\Delta t$ 是采样时间间隔
- $\exp([\omega_k \Delta t]_\times)$ 可以通过 Rodrigues 公式计算: \(\exp([\omega \Delta t]_\times) = I + \frac{\sin\|\omega \Delta t\|}{\|\omega \Delta t\|}[\omega]_\times + \frac{1-\cos\|\omega \Delta t\|}{\|\omega \Delta t\|^2}[\omega]_\times^2\)
实际应用中,当角速度较小时,可以使用一阶近似: \(R_{k+1} \approx R_k(I + [\omega_k \Delta t]_\times)\)
数值积分方法:
a) 欧拉法(一阶方法): \(R_{k+1} = R_k(I + [\omega_k \Delta t]_\times)\)
- 优点:计算简单
- 缺点:精度较低,误差随 $\Delta t$ 线性增长
b) 中点法(二阶方法): \(\begin{align} \omega_{mid} &= \frac{\omega_k + \omega_{k+1}}{2} \\ R_{k+1} &= R_k\exp([\omega_{mid} \Delta t]_\times) \end{align}\)
- 优点:精度更高,误差随 $\Delta t^2$ 增长
- 缺点:需要两次角速度测量
c) 四阶龙格库塔法(RK4):
基础理论详见2022-05-05-Runge-Kutta
\(\begin{align} k_1 &= [\omega_k]_\times R_k \\ k_2 &= [\omega_{k+1/2}]_\times(R_k + \frac{\Delta t}{2}k_1) \\ k_3 &= [\omega_{k+1/2}]_\times(R_k + \frac{\Delta t}{2}k_2) \\ k_4 &= [\omega_{k+1}]_\times(R_k + \Delta t k_3) \\ R_{k+1} &= R_k + \frac{\Delta t}{6}(k_1 + 2k_2 + 2k_3 + k_4) \end{align}\)
- 优点:精度最高,误差随 $\Delta t^4$ 增长
- 缺点:计算量最大,需要多次角速度测量
实际应用建议:
- 对于高精度要求:使用 RK4 方法
- 对于实时性要求:使用欧拉法或中点法
- 采样率越高,积分精度越好
- 注意保持旋转矩阵的正交性,可以通过 Gram-Schmidt 正交化进行修正
5. 常用代码块
使用
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
// bot pose in global frame
Eigen::Matrix4d bot_T_global_;
// 初始化 与 set
const Eigen::Matrix3d &camera_matrix;
const Eigen::Vector3d &trans;
bot_T_global_ = Eigen::Matrix4d::Identity();
bot_T_global_.block<3, 3>(0, 0) = rotation_matrix;
bot_T_global_.topRightCorner(3, 1) = trans;
// get
Eigen::Matrix3d GetPoseRotate() const {
return Eigen::Matrix3d(bot_T_global_.block<3, 3>(0, 0));
}
Eigen::Vector3d GetPoseTrans() const {
return bot_T_global_.topRightCorner(3, 1);
}
// trans a point from bot frame to global frame
// p_g = T * p_b
Eigen::Vector3d p_global =
(bot_T_global * Eigen::Vector4d(p_bot.x(), p_bot.y(),
p_bot.z(), 1.0))
.head(3);
// trans a point from global frame to bot frame
// p_b = T_inv * p_g
// T_inv = [ R^T -R^T * t
// 0^T 1 ]
Eigen::Vector3d p_bot =
(bot_T_global.inverse() * Eigen::Vector4d(p_global.x(),
p_global.y(),
p_global.z(), 1.0))
.head(3);
互相转换
RotateMatrix 转 AngleAxisd
1
2
3
4
5
const Eigen::Matrix3d rotation_matrix;
Eigen::AngleAxisd rotate_angle_axis(rotation_matrix);
LOG(ERROR) << "pose:\n"
<< "rvec:axis:" << rotate_angle_axis.axis().transpose()
<< "angle:" << rotate_angle_axis.angle() << "\n"
AngleAxisd 转 RotateMatrix
1
2
3
4
5
6
7
8
Eigen::Vector3d rotate_vector(rvec[0], rvec[1], rvec[2]);
Eigen::Vector3d rotate_axis = rotate_vector.normalized();
double rotate_angle = rotate_vector.norm();
auto rotate_angle_axis = Eigen::AngleAxisd(rotate_angle,
rotate_axis);
Eigen::Matrix4d bot_T_global = Eigen::Matrix4d::Identity();
bot_T_global.block<3, 3>(0, 0) = rotate_angle_axis.toRotationMatrix();
RotateMatrix 转 EulerAngles
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
/**
* @brief Euler2RotationMatrix 欧拉角转旋转矩阵
* @param Eigen::Vector3d eular_angle zyx ypr yaw,pitch,roll
* @retval Eigen::Matrix3d RotationMatrix
*/
Eigen::Matrix3d Euler2RotationMatrix(const double yaw, const double pitch,
const double roll) {
Eigen::AngleAxisd yawAngle(yaw, Eigen::Vector3d::UnitZ());
Eigen::AngleAxisd pitchAngle(pitch, Eigen::Vector3d::UnitY());
Eigen::AngleAxisd rollAngle(roll, Eigen::Vector3d::UnitX());
Eigen::Quaterniond q = yawAngle * pitchAngle * rollAngle;
Eigen::Matrix3d R = q.matrix();
std::cout << "Euler2RotationMatrix result is:" << std::endl;
std::cout << "R = " << std::endl << R << std::endl << std::endl;
return R;
}
/**
* @brief RotationMatrix2Euler 旋转矩阵转欧拉角
* @param Eigen::Matrix3d
* @retval Eigen::Vector3d eular_angle zyx ypr yaw,pitch,roll
*/
Eigen::Vector3d RotationMatrix2Euler(Eigen::Matrix3d R) {
Eigen::Matrix3d m;
m = R;
Eigen::Vector3d euler = m.eulerAngles(2, 1, 0); // zyx ypr yaw,pitch,roll
std::cout << "RotationMatrix2Euler result is:" << std::endl;
std::cout << "z,yaw = " << euler[0] << std::endl; // z,yaw
std::cout << "y,pitch = " << euler[1] << std::endl; // y,pitch
std::cout << "x,roll = " << euler[2] << std::endl; // x,roll
std::cout << std::endl;
// radian to angle
std::cout << "z,yaw = " << euler[0] * (180.0 / M_PI) << "deg" << std::endl;
std::cout << "y,pitch = " << euler[1] * (180.0 / M_PI) << "deg" << std::endl;
std::cout << "x,roll = " << euler[2] * (180.0 / M_PI) << "deg" << std::endl;
std::cout << std::endl;
return euler;
}