【VSLAM】以D435为例介绍如何将vins转换为mavros

阿栋阿栋

已于 2024-02-19 10:49:24 修改

阅读量1.3k

点赞数 28

文章标签：机器人计算机视觉

于 2023-12-26 15:25:29 首次发布

本文链接：https://blog.csdn.net/caiqidong321/article/details/135223136

版权

前言

无人机室内飞行中需要解决定位的问题，我现在使用vins-fusion算法的定位结果作为PX4的定位源，那么需要牵扯到如何把vins的定位结果转换到mavros坐标系下，关键是vins与mavros坐标系之间的转换。vins的定位结果包括位置(xyz)和姿态(q)，位置简单，换个轴变个正负号的问题，但是姿态就需要详细了解二者坐标系的关系了，上网查了几个教程感觉讲的有点复杂，因此本教程分享一种思路比较清晰的方法。

参考：

https://blog.csdn.net/qq_44998513/article/details/133877790#t4

一、VINS-Fusion坐标系

vins的定位结果包括位置（xyz）和姿态（q），位置很好理解就是在其世界坐标系（world）下的空间点位置，姿态用四元数q表示，需要明白姿态表示的是两个坐标系之间的旋转关系，也就是坐标系1如何旋转变成坐标系2，那么想对q进行变换首先得先明白姿态q对应的是哪两个坐标系之间的旋转，在vins中q对应的是世界坐标系（world）和机体坐标系（body）之间的旋转关系。

1.1 body坐标系

vins的body坐标系就是其使用的IMU传感器的坐标系，不同的IMU传感器的坐标系是不同的，例如D435i的IMU坐标系就和图像坐标系一样Z轴朝前，而PX4的IMU坐标系Z轴朝上。怎么看IMU传感器的坐标系，最简单的方法是直接打印IMU话题，如果旋转无人机观测数据即可。

1.2 world坐标系

vins算法中先选取初始滑窗中第一帧的 body 坐标系位置作为 world 坐标系的位置，然后将 world 坐标系与重力方向对齐得到最后的world 坐标系。算法看起来麻烦，但实操时，我们老办法打印vins的定位结果，上下左右移动无人机，确定 world 坐标系的朝向就行了。

1.3 补充

如果你的vins算法没有使用IMU，仅使用图像，那么姿态q就是world坐标系和camera坐标系之间的旋转关系了

二、Mavros坐标系

mavros需要的姿态q其实也是其世界坐标系和机体坐标系的旋转关系，为了和vins的这两个坐标系区分，我们换个名字：世界坐标系（ map ）和机体坐标系（ base_link）

2.1 base_link坐标系

base_link坐标系就是PX4的IMU坐标系，如果PX4安装位置是朝着机头的话，那应该是前左上的坐标系（即朝向机头的是X轴，朝天的是Z轴），实操时老办法，打印IMU话题观测一下即可。

2.2 map坐标系

map坐标系就是以无人机上电的地方为原点，坐标轴朝向也是前左上（即朝向机头的是X轴，朝天的是Z轴）

三、以D435i为例介绍如何转换

ok，回归正题，以我无人机的D435i为例介绍如何转换姿态q，先说明一下我无人机的情况：

vins使用D435i的图像和IMU，然后D435i是安装在无人机屁股的并且斜着安装，因此这四个坐标系比较混乱

我们先将确定这四个坐标系的朝向，然后画出来：（这四个坐标系有三个都很好确认，观测数据大法即可，唯独body坐标系有点难，因为我是斜着安装的，不知道其斜着的角度，仅仅观测IMU数据很难得到具体的角度，这里我用的方法是把无人机静止放置，运行vins，观测一下vins定位结果的 $q^w_b$ ，然后在网站中解算为欧拉角就知道此时body坐标系相当于world坐标系旋转了多少度了）

然后我们现在要发送给mavros的就是 $q^m_l$ ，而vins定位给我们的是 $q^w_b$ ，注意 $q^w_m$ 和 $q^b_l$ 都是固定的，因此这四个可形成闭环的四元数我们知道了三个，那么理论上就可以用这三个计算出剩下这一个了：

我们看看等式右侧的三个q：

$q^w_b$ 表示从world坐标系旋转到body坐标系，是vins输出的结果，该四元数会随着相机姿态的变换而变换；

$q^b_l$ 表示从body坐标系旋转到base_link坐标系，该四元数是固定不变的，具体来说是body坐标系先绕X轴旋转+122度，然后绕Z轴-90度（注意，我从坐标系1按欧拉角顺序旋转到坐标系2得到的是 $q^1_2$ ）；

$q^w_m$ 表示从world坐标系旋转到map坐标系，该四元数也是固定不变的，具体来说是world坐标系绕Z轴-90度；

至此，我们已经计算出了等式右侧的三个q，剩下的就是变为代码了，下面的是我的代码（当时时间紧张，代码写垃圾了点，不影响核心部分）：

#include <ros/ros.h>
#include <geometry_msgs/PoseStamped.h>

#include <Eigen/Eigen>
#include <nav_msgs/Odometry.h>


Eigen::Vector3d pos_drone_t265;
Eigen::Quaterniond q_t265;

Eigen::Vector3d pos_drone_slam;
Eigen::Vector3d v_drone_slam;
Eigen::Quaterniond q_slam;
// 设计该变量是为了只有接收到vins的数据才发转换后的数据，怕到时候忘记开vins了
bool send_b = false;


void slam_cb(const nav_msgs::Odometry::ConstPtr &msg)
{

    if(msg->header.frame_id == "world")
    {
        send_b = true;

        // 以下进行vins的结果转换
        pos_drone_slam = Eigen::Vector3d(-msg->pose.pose.position.y, msg->pose.pose.position.x, msg->pose.pose.position.z);
        Eigen::Quaterniond w_q_b = Eigen::Quaterniond(msg->pose.pose.orientation.w, msg->pose.pose.orientation.x, msg->pose.pose.orientation.y, msg->pose.pose.orientation.z);
        Eigen::AngleAxisd roll(M_PI*122/180,Eigen::Vector3d::UnitX());
        Eigen::AngleAxisd pitch(0,Eigen::Vector3d::UnitY());
        Eigen::AngleAxisd yaw(-90,Eigen::Vector3d::UnitZ());
        Eigen::Quaterniond b_q_l = roll * pitch * yaw;
        Eigen::AngleAxisd roll_1(0,Eigen::Vector3d::UnitX());
        Eigen::AngleAxisd pitch_1(0,Eigen::Vector3d::UnitY());
        Eigen::AngleAxisd yaw_1(-90,Eigen::Vector3d::UnitZ());
        Eigen::Quaterniond w_q_m = roll_1 * pitch_1 * yaw_1;
        q_slam = w_q_m.conjugate() * w_q_b * b_q_l;
    }
    else
    {
        //pub_message(message_pub, prometheus_msgs::Message::NORMAL, NODE_NAME, "wrong slam frame id.");
    }
}

int main(int argc, char **argv)
{
    ros::init(argc, argv, "vinsfusion_to_mavros");
    ros::NodeHandle nh;
    ros::Subscriber slam_sub = nh.subscribe<nav_msgs::Odometry>("/vins_estimator/imu_propagate", 200, slam_cb);

    ros::Publisher vision_pub = nh.advertise<geometry_msgs::PoseStamped>("/mavros/vision_pose/pose", 10);

    ros::Rate rate(25.0);
    ros::Time last_request = ros::Time::now();

    while(ros::ok()){
        geometry_msgs::PoseStamped vision;

        vision.pose.position.x = pos_drone_slam[0];
        vision.pose.position.y = pos_drone_slam[1];
        vision.pose.position.z = pos_drone_slam[2];

        vision.pose.orientation.x = q_slam.x();
        vision.pose.orientation.y = q_slam.y();
        vision.pose.orientation.z = q_slam.z();
        vision.pose.orientation.w = q_slam.w();
        vision.header.stamp = ros::Time::now();
        if(send_b){
            vision_pub.publish(vision);
            send_b = false;
            
        }
	    ros::spinOnce();
            
        
        rate.sleep();
    }

    return 0;
}