MAVROS PX4外部坐标系输入与内部坐标系对齐

小小洋洋

已于 2024-10-14 20:31:36 修改

阅读量459

点赞数 1

分类专栏：无人机ROS 文章标签： cocos2d 游戏引擎

于 2024-10-14 20:27:17 首次发布

本文链接：https://blog.csdn.net/hot_ant/article/details/142927752

版权

无人机ROS 专栏收录该内容

9 篇文章

订阅专栏

MAVROS PX4外部坐标系输入与内部坐标系对齐

有一个激光雷达，安装在无人机顶部，其坐标轴朝向相对于无人机为右-前-上

一、QGC调整内部参数

调整EKF和HGT为外部视觉输入；
调整延时EKF2_EV_DELAY为一个合适的值，例如20ms。

二、写转换脚本

转换脚本部分代码：

vehicle_type = sys.argv[1]
vehicle_id = sys.argv[2]
local_pose = PoseStamped()
local_pose.header.frame_id = 'world'
quaternion = tf.transformations.quaternion_from_euler(0, -math.pi/2, math.pi/2)
q = Quaternion([quaternion[3],quaternion[0],quaternion[1],quaternion[2]])

def vins_callback(data):    
    local_pose.pose.position.x = data.pose.pose.position.x
    local_pose.pose.position.y = data.pose.pose.position.y
    local_pose.pose.position.z = data.pose.pose.position.z
    q_= Quaternion([data.pose.pose.orientation.w,data.pose.pose.orientation.x,data.pose.pose.orientation.y,data.pose.pose.orientation.z])
    q_ = q_*q
    local_pose.pose.orientation.w = q_[0]
    local_pose.pose.orientation.x = q_[1]
    local_pose.pose.orientation.y = q_[2]
    local_pose.pose.orientation.z = q_[3]

坐标系xyz转换：由于激光雷达是RFU，需要转换成FLU，因此需要y幅值给x，-x幅值给y，z不改变。
旋转角转换：旋转角的坐标系似乎比较奇怪，是RFU的，正好与激光雷达一致，因此角度转换不需要，最终修改的部分代码如下：


quaternion = tf.transformations.quaternion_from_euler(0, 0, 0)

def vins_callback(data):    
    local_pose.pose.position.x = data.pose.pose.position.y
    local_pose.pose.position.y = -data.pose.pose.position.x
    local_pose.pose.position.z = data.pose.pose.position.z