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
1
2
3

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

cd src/livox_ros_driver2/
./build ROS1
1
2

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

运行fast-lio

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

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

执行后使用rostopic list查看话题列表，出现/Odometry话题即为成功运行

使用

rostopic echo /Odometry
1

可以查看当前的定位定姿信息。

修改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());
1
2
3

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

 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;
};
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40

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

p_enu = init_q*p_lidar_body;
1

将转换后的位置信息通过/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);
1
2
3
4
5
6
7
8
9
10
11
12

分别执行以下节点

在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;
}
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132