欧拉角微分方程的推导

前言

最近学习了深蓝学院的《视觉slam进阶:从零开始手写VIO》，第二章讲IMU传感器，提到了旋转积分的欧拉角形式，正好最近也用到这个公式，所以把该公式的详细推导过程认真推导了一遍。

一些说明

按照习惯，推导过程中使用的导航坐标系为东北天坐标系（$ENU$），IMU的载体坐标系为载体坐标系为：右、前，上（$X$, $Y$, $Z$）,欧拉角的旋转顺序为：$Z$, $X$, $Y$ (偏航、俯仰，滚转)。待会从下面的推导过程中我们可以看到，基于欧拉角法的推导公式和载体坐标系的定义以及旋转顺序都是有关的，所以在这里我们做一些说明。另外可以用matlab辅助公式推导，很方便，不易出错。

推导过程

$$R_n^b=R_1^b(y)\cdot R_2^1(p)\cdot R_n^2(r)$$
其中：
$$R_1^b(y)=\left[\begin{array}{ccc}cy& -sy& 0\\ sy& cy& 0\\ 0& 0& 1\end{array}\right]$$
$$R_2^1(p)=\left[\begin{array}{ccc}1& 0& 0\\ 0& cp& -sp\\ 0& sp& cp\end{array}\right]$$
$$R_n^2(r)=\left[\begin{array}{ccc}cr& 0& sr\\ 0& 1& 0\\ -sr& 0& cr\end{array}\right]$$
$R_1^b(y)$表示绕 $Z$轴的旋转矩阵，$R_2^1(p)$表示绕 $X$轴的旋转矩阵， $R_n^2(r)$表示绕 $Y$轴的旋转矩阵。 $R_n^b$为导航系到载体坐标系的转换矩阵, $X^b$表示载体坐标系下的坐标， $X^n$表示导航坐标系下的坐标，则有以下公式：
$$X^n=R_n^b\cdot X^b$$
$X^b$的求解如下：
$$X^b=R_2^n(r)\cdot R_1^2(p)\cdot R_b^1(y)\cdot X^n$$
第一步：

假设已经完成绕三个轴的旋转，则最后绕 $Y$轴的角速度在载体坐标系和导航坐标系都是一样的，所以有如下公式：
$${\omega }_b(r)=\left[\begin{array}{c}0\\ \frac{dr}{dt}\\ 0\end{array}\right]$$
第二步：

假设已经完成了前两组旋转，最后一组旋转没有完成，如果接着完成最后一步旋转，则和第一步一样，绕 $X$轴的角速度在两个坐标系下是一样的，则有如下公式：
$${\omega }_b(p)=R_2^n(r)\cdot \left[\begin{array}{c}\frac{dp}{dt}\\ 0\\ 0\end{array}\right]$$
第三步：
分析和第二步类似。假设已经完成了第一组旋转，最后两组旋转没有完成，如果接着完成最后两组旋转，则绕$Z$轴的角速度在两个坐标系下是一样的，则有如下公式：
$${\omega }_b(y)=R_2^n(r)\cdot R_1^2(p)\cdot \left[\begin{array}{c}0\\ 0\\ \frac{dy}{dt}\end{array}\right]$$
最后有：
$${\omega }_b={\omega }_b(y)+{\omega }_b(p)+{\omega }_b(r)$$
$${\omega }_b=R_2^n(r)\cdot R_1^2(p)\cdot \left[\begin{array}{c}0\\ 0\\ \frac{dy}{dt}\end{array}\right]+R_2^n(r)\cdot \left[\begin{array}{c}\frac{dp}{dt}\\ 0\\ 0\end{array}\right]+\left[\begin{array}{c}0\\ \frac{dr}{dt}\\ 0\end{array}\right]=\left[\begin{array}{ccc}cr& 0& -sr\cdot cp\\ 0& 1& sp\\ sr& 0& cr\cdot cp\end{array}\right]\cdot \left[\begin{array}{c}\frac{dp}{dt}\\ \frac{dr}{dt}\\ \frac{dy}{dt}\end{array}\right]$$
上式中${\omega }_b$是角速度在载体坐标系下的投影，即陀螺仪数据，$\frac{dp}{dt}$，$\frac{dr}{dt}$，$\frac{dy}{dt}$是角速度在导航坐标系下的投影。
同样可以得到：
$$\left[\begin{array}{l}\frac{dp}{dt}\\ \frac{dr}{dt}\\ \frac{dy}{dt}\end{array}\right]=\left[\begin{array}{ccc}cr& 0& sr\\ sp\cdot \mathrm{sr}/cp& 1& -sp\cdot cr/cp\\ -sr/cp& 0& cr/cp\end{array}\right]\cdot {\omega }_b$$
从上式可以看出，当俯仰角接近$90^{\circ }$时，是无解的，所以欧拉角法不能用在全姿态飞行器中，等于与平台惯导的“环架自锁”，这一点在使用该公式的时候要注意。

Matlab辅助推导的代码如下：

% 姿态矩阵更新的欧拉角形式公式推导
% 旋转顺序：Z,X,Y
% 参考： KINEMATICS OF MOVING FRAMES MIT KINEMATICS OF MOVING FRAMES MIT
syms r p y
syms dpdt drdt  dydt

x_p = [1,      0,       0;
       0, cos(p), -sin(p); 
       0, sin(p), cos(p)];
  
y_r = [cos(r),  0,  sin(r);
            0,  1,       0;
      -sin(r),  0,  cos(r)]; 

z_y = [cos(y),  -sin(y),  0;
       sin(y),  cos(y),   0;
            0,       0,   1]; 
        
x_p_trans = x_p';
y_r_trans = y_r';
z_y_trans = z_y';
dp_t_vector=[dpdt;0;0]; %X轴
dr_t_vector=[0;drdt;0]; %Y轴
dy_t_vector=[0;0;dydt]; %Z轴

% y_r_trans x_p_trans z_y_trans
% 陀螺仪角速度
ang_vel=y_r_trans*x_p_trans*dy_t_vector+y_r_trans*dp_t_vector+dr_t_vector

% 结果如下：
% ang_vel=
% dpdt*cos(r) - dydt*cos(p)*sin(r)
% drdt + dydt*sin(p)
% dpdt*sin(r) + dydt*cos(p)*cos(r)

% 整理可得如下结果： % [dpdt;drdt;dydt]
% 转换矩阵如下：
% [cos(r),   0, -sin(r)*cos(p);
%       0,   1,        sin(p);
%  sin(r),   0, cos(r)*cos(p)]

% 求矩阵的转置
ang_vel_matrix= [cos(r),   0, -sin(r)*cos(p);
                      0,   1,         sin(p);
                 sin(r),   0, cos(r)*cos(p)];
            
ang_vel_inv_matrix = inv(ang_vel_matrix)

% 结果如下：
% ang_vel_inv_matrix =
%  
% [                        cos(r)/(cos(r)^2 + sin(r)^2), 0,                         sin(r)/(cos(r)^2 + sin(r)^2)]
% [ (sin(p)*sin(r))/(cos(p)*cos(r)^2 + cos(p)*sin(r)^2), 1, -(cos(r)*sin(p))/(cos(p)*cos(r)^2 + cos(p)*sin(r)^2)]
% [         -sin(r)/(cos(p)*cos(r)^2 + cos(p)*sin(r)^2), 0,           cos(r)/(cos(p)*cos(r)^2 + cos(p)*sin(r)^2)]

% 整理可得：
% [                  cos(r),  0,                sin(r)]
% [  (sin(p)*sin(r))/cos(p),  1, -cos(r)*sin(p)/cos(p)]
% [          -sin(r)/cos(p),  0,         cos(r)/cos(p)]

同时我也顺带验证了参考2中的推导公式，参考2中的旋转顺序为$Z$,$Y$,$X$,推导结果和2中一致。代码如下：

% 姿态矩阵更新的欧拉角形式公式推导
% 旋转顺序：Z,Y,X
% 参考： KINEMATICS OF MOVING FRAMES MIT KINEMATICS OF MOVING FRAMES MIT
syms r p y
syms dpdt drdt  dydt

x_r = [1,      0,       0;
       0, cos(r), -sin(r); 
       0, sin(r), cos(r)];
  
y_p = [cos(p),  0,  sin(p);
            0,  1,       0;
      -sin(p),  0,  cos(p)]; 

z_y = [cos(y),  -sin(y),  0;
       sin(y),  cos(y),   0;
            0,       0,   1]; 
        
x_r_trans = x_r';
y_p_trans = y_p';
z_y_trans = z_y';
dr_t_vector = [drdt;0;0]; %x轴 
dp_t_vector = [0;dpdt;0]; %y轴
dy_t_vector = [0;0;dydt]; %z轴

% 旋转顺序：x_r_trans y_p_trans z_y_trans
% 陀螺仪角速度
ang_vel = x_r_trans*y_p_trans*dy_t_vector+x_r_trans*dp_t_vector+dr_t_vector

% 结果如下
% ang_vel =
% drdt - dydt*sin(p)
% dpdt*cos(r) + dydt*cos(p)*sin(r)
% dydt*cos(p)*cos(r) - dpdt*sin(r)
        
% drdt; dpdt; dydt
% 整理得：
% 1   ,       0   ,        -sin(p);
% 0   ,   cos(r)  ,  sin(r)*cos(p);
% 0   ,  -sin(r)  ,  cos(p)*cos(r);

% 求矩阵的转置
 ang_vel_matrix=[1  ,      0   ,        -sin(p);
                 0  ,   cos(r) ,  sin(r)*cos(p);
                 0  ,  -sin(r) ,  cos(r)*cos(p)];

 ang_vel_inv_matrix = inv(ang_vel_matrix)

 % 结果如下：
%  [ 1, (sin(p)*sin(r))/(cos(p)*cos(r)^2 + cos(p)*sin(r)^2), (cos(r)*sin(p))/(cos(p)*cos(r)^2 + cos(p)*sin(r)^2)]
% [ 0,                        cos(r)/(cos(r)^2 + sin(r)^2),                       -sin(r)/(cos(r)^2 + sin(r)^2)]
% [ 0,          sin(r)/(cos(p)*cos(r)^2 + cos(p)*sin(r)^2),          cos(r)/(cos(p)*cos(r)^2 + cos(p)*sin(r)^2)]

% 化简得：
%  [ 1, sin(r)*tan(p), cos(r)*tan(p)]
%  [ 0,        cos(r),       -sin(r)]
%  [ 0, sin(r)/cos(p),  cos(r)/cos(p)]
        

参考

1.《惯性导航基本原理》 刘保中
2. KINEMATICS OF MOVING FRAMES MIT
3.深蓝学院《视觉slam进阶:从零开始手写VIO》课程