NED下姿态转换到ENU下

问题

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

求解

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

从Body系到NED的旋转矩阵为

Rbn=[cos⁡ψcos⁡θcos⁡ψsin⁡θsin⁡ϕ−sin⁡ψcos⁡ϕcos⁡ψsin⁡θcos⁡ϕ+sin⁡ψsin⁡ϕsin⁡ψcos⁡θsin⁡ψsin⁡θsin⁡ϕ+cos⁡ψcos⁡ϕsin⁡ψsin⁡θcos⁡ϕ−cos⁡ψsin⁡ϕ−sin⁡θcos⁡θsin⁡ϕcos⁡θcos⁡ϕ]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] Rbn=⎣⎡cosψcosθsinψcosθ−sinθcosψsinθsinϕ−sinψcosϕsinψsinθsinϕ+cosψcosϕcosθsinϕcosψsinθcosϕ+sinψsinϕsinψsinθcosϕ−cosψsinϕcosθcosϕ⎦⎤

从 NED 到 ENU 的旋转矩阵为

Rne=[01010000−1]R_{n}^{e}=\left[\begin{array}{ccc} 0 &1 & 0\\ 1 & 0 & 0\\ 0 & 0 & -1 \end{array}\right] Rne=⎣⎡01010000−1⎦⎤

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

Rbe=RneRbnR_b^e = R_n^e R _b^n Rbe=RneRbn

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

ψ=arctan2(r21,r11)ϕ=arctan⁡(r32r33)θ=arcsin⁡(−r31)\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} ψϕθ=arctan2(r21,r11)=arctan(r33r32)=arcsin(−r31)

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

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

ψ′=arctan2(1,tan⁡ψ)ϕ′=ϕθ′=−θ\begin{aligned} \psi' &= {\rm arctan2}({1},{\tan\psi} ) \\ \phi' &= \phi \\ \theta' &= -\theta \end{aligned} ψ′ϕ′θ′=arctan2(1,tanψ)=ϕ=−θ

代码

%% 计算
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 01 0 00 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)])

NED下姿态转换到ENU下相关推荐

ROS中ENU坐标系与无人机中NED坐标系的转换关系理解
ROS中ENU坐标系与无人机中NED坐标系的转换关系理解项目地址无人机中NED坐标理解 ENU与NED转换无人机中NED坐标理解机体坐标系:机体坐标系固连飞机,其原点取在多旋翼的重心位置上. ...
linux windows 字符编码转换,Linux操作系统下汉字编码的转换
因为项目的需要linux下将GBK编码转换为utf8编码,google一下,网上的相关资源比较少,下面的操作经过本人的反复试验.本例子同样适用于其他的编码转换. 有gbk到utf8的转换过程,需要经过 ...
将windows下编辑好的文件(GBK)转换成Linux下的格式(UTF8)
背景:一般我们在windows上编辑好的文件默认编码是GBK,而且换行符是^M,为了将这些文件用在linux上,我们一般会将它们转换成Linux下的文件格式,即去掉^M,且将文件格式转换成UTF8. ...
php文件夹转换网页,PHP转换文件夹下所有文件的编码适合发布网站的其他编码版本...
PHP转换文件夹下所有文件的编码适合发布网站的其他编码版本比如你有一个GBK版本你想有一个UTF8版本或者你只有GBK的源码你想二次开发但是你不想改变IDE的编码方式你可以用这个程序将其 ...
linux中iconv函数,Linux下编码转换（iconv函数族）
转自:http://www.linuxdiyf.com/viewarticle.php?id=45164 在Linux上进行编码转换时,既可以利用iconv函数族编程实现,也可以利用iconv命令来实 ...
c语言linux下可用函数,[原创]linux下编码转换问题,C语言实现，使用iconv函数族68...
[原创]linux下编码转换问题,C语言实现,使用iconv函数族68 (2012-07-30 02:49:00) 标签: linux 68 c语言杂谈 #include #include #inc ...
Windows下批量转换Silk v3音频文件为MP3格式
转自:https://kn007.net/topics/batch-convert-silk-v3-audio-files-to-mp3-in-windows/ 前有<解码转换QQ微信的SILK ...
直角坐标积分化为极坐标积分_将直角坐标系下的二次积分转换成极坐标系下的二次积分...
龙源期刊网 http://www.qikan.com.cn 将直角坐标系下的二次积分转换成极坐标系下的二次积分作者:朱清芳来源:<科技信息 · 上旬刊> 2017 年第 06 期摘 ...
python 图像格式转换文件夹下 jpg 转 bmp | 目录遍历
python 图像格式转换文件夹下 jpg 转 bmp import os from PIL import Imagedef jpgToBmp(imgFile):dst_dir = "/ho ...

NED下姿态转换到ENU下

问题

求解

代码

NED下姿态转换到ENU下相关推荐

最新文章

热门文章