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

前言

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