NED下姿态转换到ENU下

最新推荐文章于 2024-04-24 16:20:53 发布

大强强小强强

最新推荐文章于 2024-04-24 16:20:53 发布

阅读量1.6k

点赞数 1

分类专栏：数学基础文章标签： matlab 线性代数算法

本文链接：https://blog.csdn.net/weixin_41869763/article/details/126630987

版权

数学基础专栏收录该内容

5 篇文章 5 订阅

订阅专栏

问题

已知无人机在 NED 下的 ZYX 欧拉角为 $[\psi, \phi, \theta]$ ，分别表示偏航角、滚转角和俯仰角。请问该姿态在 ENU 下的表示 $[\psi', \phi', \theta']$ 是多少。

求解

直接使用欧拉角难以描述连续两次刚体旋转，此处使用旋转矩阵推导。

从Body系到NED的旋转矩阵为

$R_{b}^{n}=\left[\begin{array}{ccc} \cos \psi \cos \theta & \cos \psi \sin \theta \sin \phi-\sin \psi \cos \phi & \cos \psi \sin \theta \cos \phi+\sin \psi \sin \phi \\ \sin \psi \cos \theta & \sin \psi \sin \theta \sin \phi+\cos \psi \cos \phi & \sin \psi \sin \theta \cos \phi-\cos \psi \sin \phi \\ -\sin \theta & \cos \theta \sin \phi & \cos \theta \cos \phi \end{array}\right]$

从 NED 到 ENU 的旋转矩阵为

$R_{n}^{e}=\left[\begin{array}{ccc} 0 &1 & 0\\ 1 & 0 & 0\\ 0 & 0 & -1 \end{array}\right]$

因此，从 Body 到 ENU 的旋转矩阵为

$R_b^e = R_n^e R _b^n$

最后再根据旋转矩阵计算姿态角。

$\begin{aligned} \psi &= {\rm arctan2}\left({r_{21}},{r_{11}} \right)\\ \phi &= \arctan \left(\frac{r_{32}}{r_{33}} \right) \\ \theta &= \arcsin(-r_{31}) \end{aligned}$

注意， ${\rm arctan2}()$ 范围为 $[-\pi, \pi]$ ， ${\rm arcsin}()$ 范围为 $[-\pi/2, \pi/2]$ ， ${\rm arctan}()$ 范围为 $[-\pi/2, \pi/2]$ 。

根据以上原理，使用MATLAB符号表达式求解，结果如下

$\begin{aligned} \psi' &= {\rm arctan2}({1},{\tan\psi} ) \\ \phi' &= \phi \\ \theta' &= -\theta \end{aligned}$

代码

%% 计算
syms psi  phi theta  real

% Body --> NED
Rbn = [cos(psi)*cos(theta)   cos(psi)*sin(theta)*sin(phi)-sin(psi)*cos(phi)  cos(psi)*sin(theta)*cos(phi)+sin(psi)*sin(phi)
       sin(psi)*cos(theta)   sin(psi)*sin(theta)*sin(phi)+cos(psi)*cos(phi)  sin(psi)*sin(theta)*cos(phi)-cos(psi)*sin(phi)
       -sin(theta)           cos(theta)*sin(phi)                              cos(theta)*cos(phi)];
% NED --> ENU 
Rne = [0 1 0
       1 0 0
       0 0 -1];
   
% Body --> ENU   
Rbe = Rne * Rbn;

% Rotate Matrix --> Eular Angle
psi1 = simplify(atan2(Rbe(2,1), Rbe(1,1)))  % psi in [-180°,180°]
phi1 = simplify(atan(Rbe(3,2) / Rbe(3,3)))
theta1 = simplify(asin(-Rbe(3,1)))

% 结果
% psi1 = angle(cos(theta)*(sin(psi) + cos(psi)*1i)) = atan2(1,tan(psi))
% phi1 = atan(tan(phi)) = phi
% theta1 = -asin(sin(theta)) = -theta

%% 验证
psi = -170 * pi/180 ;
phi = 20 * pi / 180;
theta = 10 * pi / 180;

disp(['NED psi = ' num2str(psi*180/pi), ',  ENU psi = ' num2str(eval(psi1)*180/pi)])
disp(['NED phi = ' num2str(phi*180/pi), ',    ENU phi = ' num2str(eval(phi1)*180/pi)])
disp(['NED theta = ' num2str(theta*180/pi), ',  ENU theta = ' num2str(eval(theta1)*180/pi)])