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

目录

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

前言

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

环境配置

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

git clone https://github.com/hku-mars/FAST_LIO.gitcd FAST_LIOgit 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 #include #include