关于为什么经常出现一些初学者飞T265指点出现无人机起飞后偏航会转90度的情况

关于为什么经常出现一些初学者飞T265指点出现无人机起飞后偏航会转90度的情况

这个情况已经在很多群里面看见有人说过，笔者最开始飞T265定点时也遇见过。

正常的T265定点，初始时候，从飞控层面看，也即是在QGC地面站上看到的，机头初始方向是朝东的，并且会显示偏航是90度，如下图所示。此时板载层面看到的/mavros/local_position/pose的四元数是x:0 y:0 z:0 w:1，即板载层面对应的俯仰横滚偏航都是0度。

但是一些早期的T265定点功能包，比如vision_to_mavros，只是为了单纯实现T265定点，是使得T265定点的机头初始方向是朝北的，也好理解，机体是北东地坐标系，朝北时对机体而言偏航为0，这么飞T265定点没有问题，其实要飞成T265定点，初始机头方向朝向哪个方向都无所谓，就像GPS定点（偏航来自于磁力计），无人机初始朝哪个方向摆放，都是可以飞起来定得住的，位置环和角度环只需要维持xyz横滚俯仰偏航是某个定值，具体偏航是0度还是50度，不影响，相对变化是准确的就行，比如顺时针转了10度就是顺时针转了10度，都可以形成闭环，可以让飞机定住，但是飞T265指点，由于板载端发期望位置和偏航都是基于东北天坐标系发的，一般默认期望偏航就是0度了（比如发/mavros/setpoint_position/local话题），这个0度自然也是板载东北天坐标系下的偏航为0度，转到北东地坐标系下，就会变为期望偏航是90度了（具体可以看下面说明的东北天坐标系和北东地坐标系的关系），而他们原本用的T265定点机头初始方向是正北，现在给了个正东的期望偏航，自然就会转90度，换作GPS定点的无人机，这么发期望偏航，只要无人机机头不是朝向正北一样也会转动相应角度，道理是相同的。如果T265定点时使得初始偏航就是朝向正东的话，就不会有这个问题。 

可以用笔者的这个功能包实现T265定点指点，就不会出现上述问题，其实原理和代码都非常简单，代码不过几十行罢了。 

GitHub - maxibooksiyi/t265_to_mavros: 用于T265定点指点的功能包

关于板载东北天和机体北东地坐标系的说明，这里的东北天和北东地虽然不是现实的东和北了，但是它们之间相对关系是确定的，就是东北天和东和北东地的东都是一个方向，北也是如此。 只是东北天的东是x，北东地的东是y了，所以T265定点时，无人机往机头方向移动，板载层面会看到local_position的x正向增大，QGC里面看到的mavlink中的local_position的y正向增大。 东北天坐标系和北东地坐标系都是右手系，所以他们之间的关系是可以一个欧式变换矩阵写出的，也就是一个旋转矩阵+平移向量得到。

如果继续深究会发现一个问题，板载层面横滚是0度，对应转到东北天坐标系下，横滚应该是180度才对。 

地面站初始偏航显示90度，既可以从两个坐标系的变换出发，也可以从坐标系的偏航角度定义出发，都可以得到相同的结果。 

从坐标系变换角度严格分析一下

T265定点，板载初始

四元数

0 0 0 1

对应旋转矩阵为

1 0 0
0 1 0
0 0 1

 东北天到北东地的旋转矩阵为，这个可以手推算出

0 1 0
1 0 0
0 0 -1

所以东北天坐标系下此时机头方向对应的旋转矩阵为

0 1 0
1 0 0
0 0 -1

 此时对应的四元数为

[ 0.7071068, 0.7071068, 0, 0 ]

偏航角是90度时倒有可能产生奇异 

欧拉角奇异性产生的原因_姿态角的奇异问题-CSDN博客

但是注意，T265定点时，对于姿态角，飞控只取用了偏航，所以横滚俯仰还是来自于飞控的，所以飞控初始横滚角还是0.

ATTITUDE的横滚为什么是0呢，它这个坐标系可以叫东南地了。东北天转东南地，只需要绕x轴转180度。

无人机飞行肯定要维持横滚俯仰在0度呀，所以无人机里面初始横滚不可能是180度 肯定是0度，这样肯定会和东北天的初始横滚0度冲突，里面应该是有个转换的！
 

飞控为什么只取T265的偏航呢，因为横滚俯仰单靠IMU已经有很准确的测量了。 

这个里面也有详细的mavros源码层面的分析

https://mp.weixin.qq.com/s/17To6Y3eFNYUqm9xBQ5p8Q

先从ned到enu，再从enu到aircraft，从enu到aircraft就是横滚转了180度！！！！最后用的enu_baselink_orientation是经过两次转换后的

xyz应该没有做额外的180度旋转。

最后发布的/mavros/imu/data里的四元数就是这个enu_baselink_orientation，local_position里的四元数和这一致，板卡层面的四元数都是如此。 

这个aircraft坐标系好像就是ENU坐标系横滚转了180度，也就是绕x轴转了180度

飞行器姿态角_机体坐标系和地面坐标系-CSDN博客

 https://github.com/mavlink/mavros/pull/473

T265->飞控时，姿态上只取了偏航，所以横滚没关系不影响。

飞控->板卡时，XYZ做北东地到东北天的变化，姿态角在北东地到东北天变化的基础上，多个横滚转180度。 那这样把那块层面的位姿还是不是东北天呢，但和飞控表示的不是同一位姿，位置是一样，但是朝向不同，可以认为是两个向量，不是一个向量在不同坐标系下的表示了，是两个向量，当然这两个向量是有联系的，有确定的相对关系，确定了其中一个就可以确定另一个。 

从mavros代码可以确认，板载的xyz就是北东地的xyz直接转东北天得到的，速度也是，看来姿态是另外单独处理了。

姿态角似乎有专门的坐标系，mavlink里

订阅到/mavros/vision_pose/pose后，位置和姿态也是分开处理的，姿态也是分两步转换的！

/mavros/mavros_extras/src/plugins/vision_pose_estimate.cpp 

那现在飞控和板载的xyz及姿态之间的关系就非常清晰了。  这也是为什么把/mavros/local_position/pose里的四元数转为attitude mavlink发给地面站，单独东北天转北东地还不够，还需要再单独对横滚转180度的原因。