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

news2025/1/22 19:39:17

目录

  • 前言
  • 环境配置
  • 运行fast-lio
  • 修改px4位置信息融合方式
  • 编写位置坐标转换及传输节点

前言

在配置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;
}

本文来自互联网用户投稿，该文观点仅代表作者本人，不代表本站立场。本站仅提供信息存储空间服务，不拥有所有权，不承担相关法律责任。如若转载，请注明出处：http://www.coloradmin.cn/o/1529265.html

如若内容造成侵权/违法违规/事实不符，请联系多彩编程网进行投诉反馈，一经查实，立即删除！

相关文章

springboot企业级抽奖项目-整体展示

springboot企业级抽奖项目-整体展示

项目地址 GitHub - kiorr/lottery: 企业红包雨项目 star截图q&#xff1a;3353441618可以领取资料 原型效果 前台 后台 业务分析 项目介绍 项目概述 京东的红包雨大家可能都参与过&#xff0c;在某段时间内随机发放不同的红包 本项目为一个通用的红包雨模式抽奖系统&…
阅读更多...
服务器端（Debian 12）配置jupyter与R 语言的融合

服务器端（Debian 12）配置jupyter与R 语言的融合

融合前&#xff1a; 服务器端Debian 12,域名&#xff1a;www.leyuxy.online 1.安装r-base #apt install r-base 2.进入R并安装IRkernel #R >install.packages(“IRkernel”) 3.通过jupyter notebook的Terminal执行&#xff1a; R >IRkernel::installspec() 报错 解决办…
阅读更多...
【Godot4.2】2D导航01 - AStar2D及其使用方法

【Godot4.2】2D导航01 - AStar2D及其使用方法

概述 对于2D平台跳跃或飞机大战&#xff0c;以及一些直接用键盘方向键操控玩家的游戏&#xff0c;是根本用不到寻路的&#xff0c;因为只需要检测碰撞就可以了。 但是对于像RTS或战棋这样需要操控玩家到地图指定位置的移动方式&#xff0c;就绝对绕不开寻路了。 导航、碰撞与…
阅读更多...
堆排序（数据结构）

堆排序（数据结构）

本期讲解堆排序的实现 —————————————————————— 1. 堆排序 堆排序即利用堆的思想来进行排序&#xff0c;总共分为两个步骤&#xff1a; 1. 建堆 • 升序&#xff1a;建大堆 • 降序&#xff1a;建小堆 2. 利用堆删除思想来进行排序. 建堆和堆删…
阅读更多...
QCustomPlot-绘制X轴为日期的折线图

QCustomPlot-绘制X轴为日期的折线图

主要代码如下&#xff1a; void Widget::InitQLineXDateAddData() {customPlot new QCustomPlot(this);// 创建日期时间类型的刻度生成器QSharedPointer<QCPAxisTickerDateTime> dateTimeTicker(new QCPAxisTickerDateTime);dateTimeTicker->setDateTimeFormat(&quo…
阅读更多...
基于python+vue智慧社区家政服务系统的设计与实现flask-django-nodejs

基于python+vue智慧社区家政服务系统的设计与实现flask-django-nodejs

论文主要是对智慧社区家政服务系统进行了介绍&#xff0c;包括研究的现状&#xff0c;还有涉及的开发背景&#xff0c;然后还对系统的设计目标进行了论述&#xff0c;还有系统的需求&#xff0c;以及整个的设计方案&#xff0c;对系统的设计以及实现&#xff0c;也都论述的比较…
阅读更多...
Kotlin进阶之协程从专家到出家

Kotlin进阶之协程从专家到出家

公众号「稀有猿诉」 原文链接 Kotlin进阶之协程从专家到出家 协程Coroutine是一种轻量级的实现异步的方式&#xff0c;是代码执行的控制框架&#xff0c;是最新式的并发编程范式。它在使用上大大简化了以往多线程并发带来的种种麻烦&#xff08;如状态同步和锁&#xff…
阅读更多...
1-postgresql数据库高可用脚本详解

1-postgresql数据库高可用脚本详解

问题&#xff1a; pgrep -f postgres > /dev/null && echo 0 || pkill keepalived 这是什么意思 建议换成 pgrep -f postmaster > /dev/null && echo 0 || pkill keepalived 回答 这条命令是一个复合命令&#xff0c;包含条件执行和重定向的元素。让我们…
阅读更多...
Docker部署dart-frog服务

Docker部署dart-frog服务

参考&#xff1a; dart-frog官网&#xff1a;https://dartfrog.vgv.dev/docs/overview 使用 Dart Frog 体验 Dart 服务端开发 - 简书 打包项目 按照demo新增项目后&#xff0c;执行&#xff1a; dart_frog build 等待build后生成 build 文件夹&#xff0c;这个文件夹就是需要…
阅读更多...
体验函数式组件简单实现Loading 加载（造轮子篇）

体验函数式组件简单实现Loading 加载（造轮子篇）

一、前言 最近想着优化一下网站&#xff0c;在文章列表页加一个Loading操作&#xff0c;于是就想到了函数式组件&#xff0c;于是本章就来和大家一起简单探讨下实现思路。 二、Loading设计 这里我想实现的效果是&#xff1a;当我们刷新页面的时候&#xff0c;前端请求接口&…
阅读更多...
Linux 建立链接（ln）

Linux 建立链接（ln）

目录 1、ln命令 创建软链接&#xff1a; 创建硬链接&#xff1a; 2、输出重定向&#xff08;>/>>&#xff09; 3、管道&#xff08;|&#xff09; 1、ln命令 &#xff08;英文全拼&#xff1a;link files&#xff09;为某一个文件在另外一个位置建立一个同步的…
阅读更多...
R语言：ggplot2做柱状图，随机生成颜色。

R语言：ggplot2做柱状图，随机生成颜色。

#加载包 > library(ggplot2) > library(tidyverse) > library(openxlsx) > library(reshape2) > library(RColorBrewer) > library(randomcoloR) > library(viridis) > set.seed(1233) #设立种子数。 > palette <- distinctColorPalette(30) …
阅读更多...
python爬取微博话题、关键词下方的所有帖子

python爬取微博话题、关键词下方的所有帖子

文章目录 github repository项目介绍输出安装必备库获取cookiegithub repository 网址：https://github.com/dataabc/weibo-search 在GitHub获取到的非常成熟的微博话题、关键词等微博帖子的获取方案，并且可以指定一个或多个关键词，指定获取微博类型，指定获取日期等等。 项…
阅读更多...
GIS学习

GIS学习

匹配查询，先连接两个表，然后在一个表里面查询 合并两个形状 比较好的colormap http://soliton.vm.bytemark.co.uk/pub/cpt-city/views/totp-cpt.html https://docs.gmt-china.org/latest/cpt/builtin-cpt/ 计算坡度时就要捕捉栅格 重分类时也要捕捉栅格 掩膜提取时也要捕…
阅读更多...
详细教---用Django封装写好的模型

详细教---用Django封装写好的模型

本次我们要用自己写好的热销词条爬虫代码来演示如何用Django把我们写好的模型封装。 第一步&#xff1a;代码准备 热搜词条搜集代码&#xff1a; import requests from lxml import etreeurl "https://tophub.today/n/KqndgxeLl9" headers{User-Agent: Mozilla/5.…
阅读更多...
Linux--如何在Linux上运行一个helloworld

Linux--如何在Linux上运行一个helloworld

一.安装vim和gcc sudo --是进入管理员模式 apt --是 Advanced Package Tool&#xff08;高级软件包工具&#xff09;的缩写&#xff0c;这是用于管理软件包的一种工具。 install --是安装的意思 后面跟软件的名称 完整的意思&#xff1a;在管理员的模式下安装 某个软件 …
阅读更多...
自动驾驶决策 - 规划 - 控制 （持续更新！！！）

自动驾驶决策 - 规划 - 控制 （持续更新！！！）

总目录 Frenet与Cartesian坐标系 Apollo基础 - Frenet坐标系 车辆模型 车辆运动学和动力学模型 控制算法 PID控制器轨迹跟随实现 Pure Pursuit控制器路径跟随 路径跟踪算法Stanley 实现 c 无人驾驶LQR控制算法 c 实现 MPC自动驾驶横向控制算法实现 c 双环PID控制详细讲解 …
阅读更多...
安防监控视频汇聚平台EasyCVR v3.5播放HTTP-FMP4出现卡顿跳帧是什么原因？

安防监控视频汇聚平台EasyCVR v3.5播放HTTP-FMP4出现卡顿跳帧是什么原因？

AI视频智能分析/视频监控管理平台EasyCVR能在复杂的网络环境中&#xff08;专网、内网、局域网、广域网、公网等&#xff09;&#xff0c;支持设备通过4G、5G、WIFI、有线等方式接入&#xff0c;并将设备进行统一集中接入与视频汇聚管理&#xff0c;经平台接入的视频流能实现多…
阅读更多...
推荐4个c++进度条开源库

推荐4个c++进度条开源库

在C中&#xff0c;有许多开源库可以帮助你创建进度条。以下是一些常用的C进度条库&#xff1a; 1. **indicators**: - GitHub链接: [https://github.com/p-ranav/indicators](https://github.com/p-ranav/indicators) - 特点: 轻量级&#xff0c;易于使用&#xff0c;支…
阅读更多...
【Super数据结构】线性表中的顺序表VS链表，谁才是最强赢家？

【Super数据结构】线性表中的顺序表VS链表，谁才是最强赢家？

&#x1f3e0;关于此专栏&#xff1a;Super数据结构专栏将使用C/C语言介绍顺序表、链表、栈、队列等数据结构&#xff0c;每篇博文会使用尽可能多的代码片段图片的方式。 &#x1f6aa;归属专栏&#xff1a;Super数据结构 &#x1f3af;每日努力一点点&#xff0c;技术累计看得…
阅读更多...
推荐文章
最新文章

Copyright @ 2022~2023