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

2024-03-20 02:36

本文主要是介绍PX4|基于FAST-LIO mid360的无人机室内自主定位及定点悬停,希望对大家解决编程问题提供一定的参考价值,需要的开发者们随着小编来一起学习吧!

目录

  • 前言
  • 环境配置
  • 运行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_driverlivox_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 2Hzros::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;
}

这篇关于PX4|基于FAST-LIO mid360的无人机室内自主定位及定点悬停的文章就介绍到这儿,希望我们推荐的文章对编程师们有所帮助!



http://www.chinasem.cn/article/828022

相关文章

无人叉车3d激光slam多房间建图定位异常处理方案-墙体画线地图切分方案

墙体画线地图切分方案 针对问题:墙体两侧特征混淆误匹配,导致建图和定位偏差,表现为过门跳变、外月台走歪等 ·解决思路:预期的根治方案IGICP需要较长时间完成上线,先使用切分地图的工程化方案,即墙体两侧切分为不同地图,在某一侧只使用该侧地图进行定位 方案思路 切分原理:切分地图基于关键帧位置,而非点云。 理论基础:光照是直线的,一帧点云必定只能照射到墙的一侧,无法同时照到两侧实践考虑:关

内卷时代无人机培训机构如何做大做强

在当今社会,随着科技的飞速发展,“内卷”一词频繁被提及,反映了各行业竞争日益激烈的现象。对于无人机培训行业而言,如何在这样的时代背景下脱颖而出,实现做大做强的目标,成为每个培训机构必须深思的问题。以下是从八个关键方面提出的策略,旨在帮助无人机培训机构在内卷时代中稳步前行。 1. 精准定位市场需求 深入研究市场:通过市场调研,了解无人机行业的最新趋势、政策导向及未来发展方向。 明确目标

js定位navigator.geolocation

一、简介   html5为window.navigator提供了geolocation属性,用于获取基于浏览器的当前用户地理位置。   window.navigator.geolocation提供了3个方法分别是: void getCurrentPosition(onSuccess,onError,options);//获取用户当前位置int watchCurrentPosition(

flume系列之:记录一次flume agent进程被异常oom kill -9的原因定位

flume系列之:记录一次flume agent进程被异常oom kill -9的原因定位 一、背景二、定位问题三、解决方法 一、背景 flume系列之:定位flume没有关闭某个时间点生成的tmp文件的原因,并制定解决方案在博主上面这篇文章的基础上,在机器内存、cpu资源、flume agent资源都足够的情况下,flume agent又出现了tmp文件无法关闭的情况 二、

【LVI-SAM】激光雷达点云处理特征提取LIO-SAM 之FeatureExtraction实现细节

激光雷达点云处理特征提取LIO-SAM 之FeatureExtraction实现细节 1. 特征提取实现过程总结1.0 特征提取过程小结1.1 类 `FeatureExtraction` 的整体结构与作用1.2 详细特征提取的过程1. 平滑度计算(`calculateSmoothness()`)2. 标记遮挡点(`markOccludedPoints()`)3. 特征提取(`extractF

一次生产环境大量CLOSE_WAIT导致服务无法访问的定位过程

1.症状 生产环境的一个服务突然无法访问,服务的交互过程如下所示: 所有的请求都是通过网关进入,之后分发到后端服务。 现在的情况是用户服务无法访问商旅服务,网关有大量java.net.SocketTimeoutException: Read timed out报错日志,商旅服务也不断有日志打印,大多是回调和定时任务日志,所以故障点在网关和商旅服务,大概率是商旅服务无法访问导致网关超时。 后

定位cpu占用过高的线程和对应的方法

如何定位cpu占用过高的线程和对应的方法? 主要是通过线程id找到对应的方法。 1 查询某个用户cpu占用最高的进程号 top -u 用户名 2 查询这个进程中占用cpu最高的线程号 top –p 进程号-H    3 查询到进程id后把进程相关的代码打印到jstack文件 jstack -l pid > jstack.txt 4 在jstack文件中通过16进制的线程id搜索到

Autoware 定位之ndt定位(八)

0. 简介 这一讲按照《Autoware 技术代码解读(三)》梳理的顺序,我们来说一说Autoware中的ndt定位,这个软件包有两个主要功能:1. 通过扫描匹配进行位置估计,2. 通过ROS服务使用蒙特卡洛方法估计初始位置 1. 代码阅读 1.1 debug.cpp 这段代码是一个用于生成用于调试的可视化标记数组的函数。它接受时间戳、坐标系、缩放比例、粒子信息和索引作为输入,并返回一个

每天一道面试题(2):fail-safe 机制与 fail-fast 机制分别有什么作用?

当谈论Java集合的 fail-fast 和 fail-safe 机制时,涉及的是在集合被并发修改时的行为和处理方式。这些机制对保证程序的正确性和稳定性非常重要,尤其是在多线程环境中。 1. Fail-Fast 机制 定义: Fail-fast 机制的核心是在检测到集合在遍历过程中被修改时,立即抛出 ConcurrentModificationException 异常,从而中断迭代操作。这种

无人机执照拿到后怎么就业?方向有哪些?就业率如何?

无人机执照拿到后,就业方向广泛且多样,就业率也呈现出逐年上升的趋势。这主要得益于无人机技术的广泛应用和无人机市场的不断扩大。以下是对无人机执照持有者就业情况的详细分析: 就业方向 1. 无人机飞行操作: 无人机飞手可以从事无人机的起飞、飞行和降落等具体操作,满足不同行业对无人机飞行的需求。 应用领域包括但不限于农业植保、电力巡线、石油管道巡线、航拍、国土资源勘查、应急救援、交通监控