无人机以mid360+光流实现无GPS定位

最新推荐文章于 2025-03-28 14:37:59 发布

逃跑的木可

最新推荐文章于 2025-03-28 14:37:59 发布

阅读量2k

点赞数 33

文章标签： 1024程序员节无人机

本文链接：https://blog.csdn.net/weixin_67505885/article/details/143215008

版权

参考链接：PX4|基于FAST-LIO mid360的无人机室内自主定位及定点悬停

使用硬件：

pixhawk6c飞控，livox-mid360雷达，MTF-01_Micolink光流测距一体传感器，香橙派5B开发板。

连线：

开发板-飞控：usb-ttl，接飞控tele2

光流-飞控：tele3

原理：将来自mid360雷达的位姿信息（odometry）和来自 PX4 飞控系统的位姿信息进行融合和转换。然后将的位置信息在初始偏航角（光流获取，也可以通过其他方式）确定后转换到 ENU（East-North-Up）坐标系下，并发布转换后的位姿消息。

详细过程

基于mid360复现fast-lio

光流使用：

MTF-01光流测距一体传感器-用户手册

微空助手下载和使用说明

当在QGC地面站-analyze tools-MAVLink检测中看见DISTANCE_SENSOR和LOCAL_POSITION_NED就代表配置成功了。

直接上源码：

#include <ros/ros.h>
#include <geometry_msgs/PoseStamped.h>
#include <nav_msgs/Odometry.h>
#include <Eigen/Eigen>
#include <cmath>
#include <queue>

// 三维向量，用于存储激光雷达在机体坐标系下的位置
Eigen::Vector3d p_lidar_body, p_enu;
// 四元数，用于存储从 VINS 得到的姿态信息
Eigen::Quaterniond q_mav;
// 四元数，用于存储从 PX4 里程计得到的姿态信息
Eigen::Quaterniond q_px4_odom;

class SlidingWindowAverage {
public:
    // 构造函数，接收窗口大小作为参数
    SlidingWindowAverage(int windowSize) : windowSize(windowSize), windowSum(0.0) {}

    // 添加数据的方法
    double addData(double newData) {
        bool needReset = false;
        // 如果队列不为空且新数据与队尾数据差值大于 0.01，则需要重置队列
        if (!dataQueue.empty() && std::fabs(newData - dataQueue.back()) > 0.01) {
            needReset = true;
        }

        dataQueue.push(newData);
        windowSum += newData;

        // 如果队列大小超过窗口大小，弹出队首元素并更新总和
        if (dataQueue.size() > windowSize) {
            windowSum -= dataQueue.front();
            dataQueue.pop();
        }

        // 如果需要重置或者队列大小小于窗口大小，直接将平均值设为新数据
        if (needReset || dataQueue.size() < windowSize) {
            windowAvg = newData;
        } else {
            // 否则计算并更新平均值
            windowAvg = windowSum / dataQueue.size();
        }

        return windowAvg;
    }

    // 获取队列大小的方法
    int get_size() {
        return dataQueue.size();
    }

    // 获取平均值的方法
    double get_avg() {
        return windowAvg;
    }

private:
    int windowSize;
    double windowSum;
    double windowAvg;
    std::queue<double> dataQueue;
};

// 从四元数计算偏航角的函数
double fromQuaternion2yaw(const Eigen::Quaterniond& q) {
    double yaw = std::atan2(2 * (q.x()*q.y() + q.w()*q.z()), q.w()*q.w() + q.x()*q.x() - q.y()*q.y() - q.z()*q.z());
    return yaw;
}

int main(int argc, char **argv) {
    ros::init(argc, argv, "vins_to_mavros");
    ros::NodeHandle nh("~");

    // 订阅 VINS 的里程计消息
    ros::Subscriber slam_sub = nh.subscribe<nav_msgs::Odometry>("/Odometry", 100, [&](const nav_msgs::Odometry::ConstPtr& msg) {
        // 将消息中的位置信息存储到向量中
        p_lidar_body = Eigen::Vector3d(msg->pose.pose.position.x, msg->pose.pose.position.y, msg->pose.pose.position.z);
        // 将消息中的姿态信息存储到四元数中
        q_mav = Eigen::Quaterniond(msg->pose.pose.orientation.w, msg->pose.pose.orientation.x, msg->pose.pose.orientation.y, msg->pose.pose.orientation.z);
    });
    // 订阅 PX4 的里程计消息
    ros::Subscriber px4_odom_sub = nh.subscribe<nav_msgs::Odometry>("/mavros/local_position/odom", 5, [&](const nav_msgs::Odometry::ConstPtr& msg) {
        // 将消息中的姿态信息存储到四元数中
        q_px4_odom = Eigen::Quaterniond(msg->pose.pose.orientation.w, msg->pose.pose.orientation.x, msg->pose.pose.orientation.y, msg->pose.pose.orientation.z);
        // 将 PX4 里程计的偏航角添加到滑动平均类中
        swa.addData(fromQuaternion2yaw(q_px4_odom));
    });

    // 发布转换后的位姿消息
    ros::Publisher vision_pub = nh.advertise<geometry_msgs::PoseStamped>("/mavros/vision_pose/pose", 10);

    // 设置发布频率
    ros::Rate rate(20.0);

    int windowSize = 8;
    SlidingWindowAverage swa(windowSize);
    float init_yaw = 0.0;
    bool init_flag = false;
    Eigen::Quaterniond init_q;
    while (ros::ok()) {
        // 如果滑动平均队列大小达到窗口大小且未初始化，则进行初始化
        if (swa.get_size() == windowSize &&!init_flag) {
            init_yaw = swa.get_avg();
            init_flag = true;
            // 根据初始偏航角创建初始四元数
            init_q = Eigen::AngleAxisd(init_yaw, Eigen::Vector3d::UnitZ())
                      * Eigen::AngleAxisd(0.0, Eigen::Vector3d::UnitY())
                      * Eigen::AngleAxisd(0.0, Eigen::Vector3d::UnitX());
        }

        if (init_flag) {
            geometry_msgs::PoseStamped vision;
            // 根据初始四元数将激光雷达在机体坐标系下的位置转换到 ENU 坐标系下
            p_enu = init_q * p_lidar_body;

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

            vision.pose.orientation.x = q_mav.x();
            vision.pose.orientation.y = q_mav.y();
            vision.pose.orientation.z = q_mav.z();
            vision.pose.orientation.w = q_mav.w();

            vision.header.stamp = ros::Time::now();
            vision_pub.publish(vision);

            ROS_INFO("\nposition in enu:\n   x: %.18f\n   y: %.18f\n   z: %.18f\norientation of lidar:\n   x: %.18f\n   y: %.18f\n   z: %.18f\n   w: %.18f",
                     p_enu[0], p_enu[1], p_enu[2], q_mav.x(), q_mav.y(), q_mav.z(), q_mav.w());
        }

        ros::spinOnce();
        rate.sleep();
    }

    return 0;
}

依次运行：

启动雷达

roslaunch livox_ros_driver2 msg_MID360.launch

启动fast-lio

roslaunch fast_lio mapping_mid360.launch

启动mavros

roslaunch mavros px4.launch

启动上述雷达定位节点

rosrun mid2px4_pkg mid2px4_node

启动offboard

rosrun offboard_run offboard_run_node

就可以实现真机飞行了