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

已于 2024-03-19 22:13:50 修改
阅读量2.2k 收藏 41
点赞数 29
分类专栏： 无人机 px4 智驾 文章标签： 无人机
于 2024-03-19 22:10:03 首次发布
版权声明：本文为博主原创文章，遵循 CC 4.0 BY-SA 版权协议，转载请附上原文出处链接和本声明。
本文链接：https://blog.csdn.net/m0_46182398/article/details/136855714
版权
px4 同时被 3 个专栏收录
14 篇文章 4 订阅
订阅专栏
智驾
4 篇文章 0 订阅
订阅专栏
无人机
2 篇文章 1 订阅
订阅专栏

目录

前言

在配置mid360运行环境后，可使用mid360进行室内的精准定位。

环境配置

在livox_ros_driver2的上级目录src下保存fast-lio的工程

git clone https://github.com/hku-mars/FAST_LIO.git
cd FAST_LIO
git submodule update --init

为使用mid360作为硬件输入修改源代码中的所有livox_ros_driver为livox_ros_driver2（包括.cpp .h 以及 package.xml）
在livox_ros_driver2的pkg中编译

cd src/livox_ros_driver2/
./build ROS1

编译过程大概需要3g的内存，若机载板物理内存不足，需要增大swap大小增加交换空间，可参考增加swap解决。

运行fast-lio

执行下述指令时请确保mid360运行环境中的rviz可以成功显示环境点云信息。
执行以下指令

roslaunch livox_ros_driver2 msg_MID360.launch 
在另一个终端中执行
roslaunch fast_lio mapping_mid360.launch

执行后使用rostopic list查看话题列表，出现/Odometry话题即为成功运行
在这里插入图片描述
使用

rostopic echo /Odometry

可以查看当前的定位定姿信息。
在这里插入图片描述

修改px4位置信息融合方式

这里使用光流以及激光定位信息。
修改EKF2_AID_MASK为10
在这里插入图片描述

编写位置坐标转换及传输节点

使用/mavros/vision_pose/pose话题将激光得到的定位信息传递至px4进行融合，需注意该话题的位置信息应建立在ENU坐标系下（MAVROS使用该坐标系作为惯性系），传递至px4接收时会自动转化为NED坐标系供EKF2进行融合。
因此需首先计算出初始化时fast-lio所产生的坐标系与ENU坐标系的旋转关系（主要为偏航角），并将该转换关系定为初始值

 init_q = Eigen::AngleAxisd(init_yaw,Eigen::Vector3d::UnitZ())//des.yaw
    * Eigen::AngleAxisd(0.0,Eigen::Vector3d::UnitY())
    * Eigen::AngleAxisd(0.0,Eigen::Vector3d::UnitX());

为减小初始偏航角误差，使用滑动窗口求平均值。

 class SlidingWindowAverage {
public:
    SlidingWindowAverage(int windowSize) : windowSize(windowSize), windowSum(0.0) {}

    double addData(double newData) {
        if(!dataQueue.empty()&&fabs(newData-dataQueue.back())>0.01){
            dataQueue = std::queue<double>();
            windowSum = 0.0;
            dataQueue.push(newData);
            windowSum += newData;
        }
        else{            
            dataQueue.push(newData);
            windowSum += newData;
        }

        // 如果队列大小超过窗口大小，弹出队列头部元素并更新窗口和队列和
        if (dataQueue.size() > windowSize) {
            windowSum -= dataQueue.front();
            dataQueue.pop();
        }
        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;
};

求解得到较为准确的初始偏航角后，该偏航角可视为fast-lio位置信息所在坐标系与惯性系的旋转关系。
在不考虑机体中心与激光雷达中心位置平动的情况下，可以将位置信息直接进行坐标转换。

p_enu = init_q*p_lidar_body;

将转换后的位置信息通过/mavros/vision_pose/pose传递

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.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);

分别执行以下节点
在这里插入图片描述
在QGC中可以查看LOCAL_POSITION_NED观察定位结果，静止时定位信息在3厘米以内漂移。
在这里插入图片描述
在调整好飞行时位置控制内外环的情况下，可以遥控起飞后切换至position模式，可以实现定点悬停。

位置转换的源码如下

#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;
Eigen::Quaterniond q_mav;
Eigen::Quaterniond q_px4_odom;

 class SlidingWindowAverage {
public:
    SlidingWindowAverage(int windowSize) : windowSize(windowSize), windowSum(0.0) {}

    double addData(double newData) {
        if(!dataQueue.empty()&&fabs(newData-dataQueue.back())>0.01){
            dataQueue = std::queue<double>();
            windowSum = 0.0;
            dataQueue.push(newData);
            windowSum += newData;
        }
        else{            
            dataQueue.push(newData);
            windowSum += newData;
        }

        // 如果队列大小超过窗口大小，弹出队列头部元素并更新窗口和队列和
        if (dataQueue.size() > windowSize) {
            windowSum -= dataQueue.front();
            dataQueue.pop();
        }
        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;
};

int windowSize = 8;
SlidingWindowAverage swa=SlidingWindowAverage(windowSize);

double fromQuaternion2yaw(Eigen::Quaterniond q)
{
  double yaw = 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;
}

void vins_callback(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);
}
 
void px4_odom_callback(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);
    swa.addData(fromQuaternion2yaw(q_px4_odom));
} 

int main(int argc, char **argv)
{
    ros::init(argc, argv, "vins_to_mavros");
    ros::NodeHandle nh("~");
 
    ros::Subscriber slam_sub = nh.subscribe<nav_msgs::Odometry>("/Odometry", 100, vins_callback);
    ros::Subscriber px4_odom_sub = nh.subscribe<nav_msgs::Odometry>("/mavros/local_position/odom", 5, px4_odom_callback);
 
    ros::Publisher vision_pub = nh.advertise<geometry_msgs::PoseStamped>("/mavros/vision_pose/pose", 10);
 
 
    // the setpoint publishing rate MUST be faster than 2Hz
    ros::Rate rate(20.0);
 
    ros::Time last_request = ros::Time::now();
    float init_yaw = 0.0;
    bool init_flag = 0;
    Eigen::Quaterniond init_q;
    while(ros::ok()){
        if(swa.get_size()==windowSize&&!init_flag){
            init_yaw = swa.get_avg();
            init_flag = 1;
            init_q = Eigen::AngleAxisd(init_yaw,Eigen::Vector3d::UnitZ())//des.yaw
    * Eigen::AngleAxisd(0.0,Eigen::Vector3d::UnitY())
    * Eigen::AngleAxisd(0.0,Eigen::Vector3d::UnitX());
        // delete swa;
        }

        if(init_flag){
            geometry_msgs::PoseStamped vision;
            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.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;
}
yangjh542426
关注
  • 29
    点赞
  • 踩
  • 41
    收藏
    觉得还不错? 一键收藏
  • 25
    评论
专栏目录
fast-lio三维定位数据集
12-30
这里的资源是fast-lio三维slam需要的数据集，需要先安装好fast-lio环境才能使用，具体可以参考我发布的博客 https://blog.csdn.net/qq_35598561/article/details/135181110
fast-lio.zip
01-13
解压缩后放在src文件夹下
livox mid-360 使用记录
Strinkin_的博客
03-31 328
## 此处参考参考链接：https://blog.csdn.net/Hahalim/article/details/129414327。
FAST-LIO,FAST-LIO2与FASTER-LIO
lovely_yoshino的博客
07-18 9660
0. 简介 继LLOAM后，三维SLAM迎来了蓬勃的发展，最近一只FAST-SLAM为代表的3D-SLAM迎来了蓬勃的发展，FASTER-LIO也可以看到国内知名SLAMer高博的影子，这里我们将来看一下
基于mid-360的FAST_LIO2,Point-LIO,faster-lio,LIO-SAM部署(持续更新)
bingshuiyuyi的博客
04-08 762
基于mid-360的FAST_LIO2,Point-LIO,faster-lio,LIO-SAM部署(持续更新)
在gazebo里搭建一个livox mid360 + 惯导仿真平台测试 FAST-LIO2
月照银海似蛟龙的博客
11-30 3262
livox mid360 在官网一直没有货，在gazebo里可以仿真该雷达形式的点云。但是其只发布雷达的数据，没有imu数据，实际的雷达是可以发布既有雷达也有imu的数据的运行 FAST-LIO2 也需要雷达和惯导的数据本篇博客在gazebo中搭建了一个有livox mid360 和惯导的平台，并成功运行了FAST-LIO2。
Livox激光MID-360使用与fast-lio2激光SLAM建图
热门推荐
weixin_42608723的博客
03-13 1万+
mid-360激光雷达使用前首先需要安装和。。安装 SDK2比较简单，按照README可以顺利完成。ROS2驱动部分：1、：新建mid360驱动工程，livox_mid360_driver_ws，修改livox_ros_driver2/config/MID360_config.json文件内的参数中ip部分，cmd_data_ip改为192.168.1.50，下面的ip改为雷达的ip，192.168.1.1** 最后两位为雷达S/N码最后两位数字，否则只有livox/imu的数据发布。
fast-lio的bag包
04-21
这是一个用于定位于构图算法Fast_lio的数据包
对于fast-lio论文和代码的一些个人理解
07-07
对于fast-lio论文和代码的一些个人理解
fast-lio2代码
05-26
fast-lio2代码，做了调整，调用库，生成头文件。
[保姆式教程]使用目标检测模型YOLO V5 OBB进行旋转目标的检测：训练自己的数据集（基于卫星和无人机的农业大棚数据集）
qq_23865133的博客
05-17 700
最近需要做基于卫星和无人机的农业大棚的旋转目标检测，基于YOLO V5 OBB的原因是因为尝试的第一个模型就是YOLO V5，后面会基于其他YOLO系列模型做农业大棚的旋转目标检测，尤其是YOLO V9，YOLO V9目前还不能进行旋转目标的检测，需要修改代码。
低空经济+无人机：如何评估低空经济与无人机产业的投资机会？
05-20 287
本文将从市场规模与增长、技术创新进展、产业链发展情况、政策环境与标准、竞争格局与参与者、资金投入与退出、风险评估与管理以及长期发展趋势等方面，为您解析如何评估低空经济与无人机产业的投资机会。同时，要关注市场细分领域的表现，如无人机在农业、物流、航拍等领域的应用情况，以发掘潜在的增长点。只有充分了解市场规模与增长、技术创新进展、产业链发展情况、政策环境与标准、竞争格局与参与者、资金投入与退出、风险评估与管理以及长期发展趋势等因素，才能做出明智的投资决策。最后，要关注低空经济与无人机产业的长期发展趋势。
蓝牙模块在无人机 ID识别、标准制定发挥的作用及其应用优势和面临的挑战
MinewSemi2022的博客
05-17 376
随着无人机使用的日益频繁，国际社会对无人机的监管和规范也提出了更高的要求。在这一背景下，蓝牙模块以其独特的优势，通过提供稳定可靠的通信连接、满足国际标准要求以及保障数据传输的安全性，蓝牙模块为无人机的合规飞行和安全应用提供了有力保障，在无人机Remote ID识别和标准制定执行中发挥着不可或缺的作用。蓝牙模块为无人机提供了与遥控器或智能手机之间稳定的通信连接，在保持高效通信的同时，降低了能耗，有助于延长无人机的飞行时间，相对于其他无线通信技术，蓝牙模块的成本相对较低，这有助于降低无人机的整体制造成本。
[保姆式教程]使用目标检测模型YOLO V8 OBB进行旋转目标的检测：训练自己的数据集（基于卫星和无人机的农业大棚数据集）
qq_23865133的博客
05-20 635
最近需要做基于卫星和无人机的农业大棚的旋转目标检测，基于YOLO V8 OBB的原因是因为尝试的第二个模型就是YOLO V8，后面会基于YOLO V9模型做农业大棚的旋转目标检测。YOLO V9目前还不能进行旋转目标的检测，需要修改代码PS:欢迎大家分享农业大棚数据集，数据制作太花时间了......下面是我制作的农业大棚图像。
植保无人机技术详解
05-18 847
植保无人机技术的应用为农业生产和生态保护带来了革命性的变革。2. 油动植保无人机：以燃油为动力源，具有飞行时间长、载重能力强的特点，适用于大型植保作业。1. 电动植保无人机：以电池为动力源，具有低噪音、零排放、维护成本低等优点，适用于小型和中型植保作业。1. 航电系统：包括飞控、IMU（惯性测量单元）、传感器等，用于实现无人机的自主飞行和导航。植保无人机通常搭载多种传感器，如高度传感器、陀螺仪、GPS等，以实现飞行控制和导航。4. 喷洒系统：负责农药的装载、稀释和喷洒，需具备精准、均匀和稳定的特点。
Qt5 互动地图，实现无人机地面站效果
ever__ever的博客
05-11 1041
文主要通过Qt5+opmapcontrol实现一个简单的无人机地面站效果。 功能： （1） 支持缓存地图 （2）支持选择各厂商地图，以及切换街道地图 （3）支持地图互动：拖动、放大缩小 （4）支持添加航点，以及航点的编辑、删除、保存、加载、航点信息显示 （5）支持设置home，以及安全区域 （6）支持显示运动轨迹
HP-Socket编译-Linux
05-20
HP-Socket编译-Linux
JavaScript_生活在Discord上的开源社区列表.zip
最新发布
05-20
JavaScript
MID360 FAST-LIO2
01-30
MID360 FAST-LIO2是一种基于Livox激光MID-360雷达和FAST-LIO2激光SLAM算法的建图系统。下面是使用MID360 FAST-LIO2的步骤： 1. 安装Livox激光MID-360 SDK2和ROS2驱动。按照README文件中的说明进行安装。 2. 配置ROS2驱动。livox_mid360_driver_ws工程中，修改livox_ros_driver2/config/MID360_config.json文件中的参数。将cmd_data_ip改为192.168.1.50，将下面的ip改为雷达的IP地址，最后两位为雷达S/N码的最后两位数字。这样配置后，才能发布雷达和IMU的数据。 3. 编译并运行ROS2驱动。在livox_mid360_driver_ws目录下执行以下命令： ```shell catkin_make source ./devel/setup.bash roslaunch livox_ros_driver2 msg_MID360.launch ``` 4. 下载并安装Livox Viewer2。可以从Livox官网的下载页面（https://www.livoxtech.com/downloads）下载Livox Viewer2。 5. 下载并安装LIO-Livox和Livox-Mapping。可以从Livox-SDK的GitHub页面（https://github.com/Livox-SDK/LIO-Livox和https://github.com/Livox-SDK/livox_mapping）下载相应的代码。 6. 配置LIO-Livox。根据LIO-Livox的README文件中的说明进行配置。 7. 启动Livox Viewer2，并连接MID-360雷达。 8. 启动LIO-Livox。在LIO-Livox的工程目录下执行以下命令： ```shell roslaunch lio_livox lio_livox.launch ``` 9. 启动Livox-Mapping。在Livox-Mapping的工程目录下执行以下命令： ```shell roslaunch livox_mapping livox_mapping.launch ``` 通过以上步骤，你就可以使用MID360 FAST-LIO2进行建图了。

“相关推荐”对你有帮助么？

  • 非常没帮助
  • 没帮助
  • 一般
  • 有帮助
  • 非常有帮助
提交
评论 25
添加红包

请填写红包祝福语或标题

个

红包个数最小为10个

元

红包金额最低5元

当前余额3.43元 前往充值 >
需支付：10.00元
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付元
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明：

1.余额是钱包充值的虚拟货币，按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载，可以购买VIP、付费专栏及课程。

余额充值