MAVROS坐标转换

飞控在OFFBOARD模式下通过MAVLINK的接口接收MAVROS上的期望,这些期望可以是期望位置、期望速度和期望姿态,而同时TX2也会从MAVROS上获取需要的飞机状态信息,一般包括飞机的控制模式、解锁状态、姿态、速度、位置信息等。
TX2获取的主要信息都来自MAVROS的/mavros/local_position/pose这个话题,但所有的位置和姿态信息都要根据坐标系来定义,本来以为它们都是使用的NED和Aircraft系,结果在使用它们运算的时候出现了很多错误,通过echo此topic的值,很容易就发现在位置上使用的是EDU坐标系,但是姿态由于是四元数的表示方法,很难明确使用的是哪两个坐标系之间的转换关系,因此,只有到MAVROS的源码中寻找了。
在plugins文件夹下找到local_position.cpp文件



advertise函数返回一个 Publisher对象。 通过调用对象的publish( )函数,我们可以在这个topic上发布 message



这就是MAVROS发布消息的过程。接下来我们看到的是位置信息是东北天(EDU)坐标系下的,而姿态信息也是EDU坐标系向Baselink坐标系的旋转关系,这些代码在imu.cpp文件中可以找到:

姿态信息来源代码:



ned_aircraft_orientation是来自飞控计算得到的NED-机体坐标系的四元数姿态坐标
enu_baselink_orientation是将坐标系经过两次转换变成了ENU-Baselink坐标系的四元数姿态坐标
机体坐标系是这样的(X在参考平面内沿机头方向向前,Y轴垂直机身参考平面向右,Z轴在参考平面内垂直XOY平面向下):


Baselink坐标系是这样的(X在参考平面内沿机头方向向前,Y轴垂直机身参考平面向左,Z轴在参考平面内垂直XOY平面向上):



而上述这些坐标转换都使用了Eigen库中的旋转公式来完成的:



上述的公式其实就是著名的罗德里格斯旋转公式,其源码是这样的,因为是绕轴逆时针旋转,所以其绕坐标轴旋转得到的结果和我们以前推导的旋转公式刚好是转置的关系:

罗德里格斯旋转公式是这样的:


©著作权归作者所有,转载或内容合作请联系作者
  • 序言:七十年代末,一起剥皮案震惊了整个滨河市,随后出现的几起案子,更是在滨河造成了极大的恐慌,老刑警刘岩,带你破解...
    沈念sama阅读 205,236评论 6 478
  • 序言:滨河连续发生了三起死亡事件,死亡现场离奇诡异,居然都是意外死亡,警方通过查阅死者的电脑和手机,发现死者居然都...
    沈念sama阅读 87,867评论 2 381
  • 文/潘晓璐 我一进店门,熙熙楼的掌柜王于贵愁眉苦脸地迎上来,“玉大人,你说我怎么就摊上这事。” “怎么了?”我有些...
    开封第一讲书人阅读 151,715评论 0 340
  • 文/不坏的土叔 我叫张陵,是天一观的道长。 经常有香客问我,道长,这世上最难降的妖魔是什么? 我笑而不...
    开封第一讲书人阅读 54,899评论 1 278
  • 正文 为了忘掉前任,我火速办了婚礼,结果婚礼上,老公的妹妹穿的比我还像新娘。我一直安慰自己,他们只是感情好,可当我...
    茶点故事阅读 63,895评论 5 368
  • 文/花漫 我一把揭开白布。 她就那样静静地躺着,像睡着了一般。 火红的嫁衣衬着肌肤如雪。 梳的纹丝不乱的头发上,一...
    开封第一讲书人阅读 48,733评论 1 283
  • 那天,我揣着相机与录音,去河边找鬼。 笑死,一个胖子当着我的面吹牛,可吹牛的内容都是我干的。 我是一名探鬼主播,决...
    沈念sama阅读 38,085评论 3 399
  • 文/苍兰香墨 我猛地睁开眼,长吁一口气:“原来是场噩梦啊……” “哼!你这毒妇竟也来了?” 一声冷哼从身侧响起,我...
    开封第一讲书人阅读 36,722评论 0 258
  • 序言:老挝万荣一对情侣失踪,失踪者是张志新(化名)和其女友刘颖,没想到半个月后,有当地人在树林里发现了一具尸体,经...
    沈念sama阅读 43,025评论 1 300
  • 正文 独居荒郊野岭守林人离奇死亡,尸身上长有42处带血的脓包…… 初始之章·张勋 以下内容为张勋视角 年9月15日...
    茶点故事阅读 35,696评论 2 323
  • 正文 我和宋清朗相恋三年,在试婚纱的时候发现自己被绿了。 大学时的朋友给我发了我未婚夫和他白月光在一起吃饭的照片。...
    茶点故事阅读 37,816评论 1 333
  • 序言:一个原本活蹦乱跳的男人离奇死亡,死状恐怖,灵堂内的尸体忽然破棺而出,到底是诈尸还是另有隐情,我是刑警宁泽,带...
    沈念sama阅读 33,447评论 4 322
  • 正文 年R本政府宣布,位于F岛的核电站,受9级特大地震影响,放射性物质发生泄漏。R本人自食恶果不足惜,却给世界环境...
    茶点故事阅读 39,057评论 3 307
  • 文/蒙蒙 一、第九天 我趴在偏房一处隐蔽的房顶上张望。 院中可真热闹,春花似锦、人声如沸。这庄子的主人今日做“春日...
    开封第一讲书人阅读 30,009评论 0 19
  • 文/苍兰香墨 我抬头看了看天上的太阳。三九已至,却和暖如春,着一层夹袄步出监牢的瞬间,已是汗流浃背。 一阵脚步声响...
    开封第一讲书人阅读 31,254评论 1 260
  • 我被黑心中介骗来泰国打工, 没想到刚下飞机就差点儿被人妖公主榨干…… 1. 我叫王不留,地道东北人。 一个月前我还...
    沈念sama阅读 45,204评论 2 352
  • 正文 我出身青楼,却偏偏与公主长得像,于是被迫代替她去往敌国和亲。 传闻我的和亲对象是个残疾皇子,可洞房花烛夜当晚...
    茶点故事阅读 42,561评论 2 343

推荐阅读更多精彩内容