PX4 无人机 MID360 结合 FAST_LIO 实现室内自主定位与定点

PX4 无人机 MID360 结合 FAST_LIO 实现室内自主定位与定点

环境准备

• PX4 固件版本：1.15.4
• QGroundControl (QGC) 地面站版本：4.4.5
• 飞控：微空科技 MicoAir743V2
• 机载电脑：12 代 i5, Ubuntu 20.04
• 安装位置：MID360 接口对应飞机后方

建议参考官方文档：Using Vision or Motion Capture Systems for Position Estimation | PX4 Guide

一、PX4 飞控设置

1. 将 PX4 定位数据源设置为 Vision

• 参数 EKF2_EV_CTRL：可保持默认或按需配置。
• 参数 EKF2_HGT_REF：设置为 Vision。

2. 关闭罗盘

注意： 完成上述飞控设置后，机头上电后会指向北。但在后续程序运行后，由于罗盘已关闭，上电后无人机机头可能指向东方。

此外，加一个激光测距融合好处多多。

二、程序部署

注意： 雷达频率可调至 30Hz，但实测定位精度在 30Hz 下仅为 10Hz 的 60% 不到，请慎重选择频率。

1. 代码实现

创建发布 /mavros/vision_pose/pose 话题的功能包。主要功能是将转化后的位姿信息以该话题发布。

#!/usr/bin/env python3
import rospy
from nav_msgs.msg import Odometry
from geometry_msgs.msg import PoseStamped
import tf
import numpy as np
from collections import deque
import math

class SlidingWindowAverage:
    def __init__(self, window_size):
        self.window_size = window_size
        self.data_queue = deque()
        self.window_sum = 0.0

    def add_data(self, new_data):
        if self.data_queue and abs(new_data - self.data_queue[-1]) > 0.01:
            self.data_queue.clear()
            self.window_sum = 0.0
        self.data_queue.append(new_data)
        self.window_sum += new_data
        if len(self.data_queue) > self.window_size:
            self.window_sum -= self.data_queue.popleft()
        return self.window_sum / len(self.data_queue)

class FastLIOToMavros:
    def __init__(self):
        rospy.init_node('fastlio_to_mavros', anonymous=True)
        self.p_lidar_body = np.zeros(3)
        self.q_mav = [0, 0, 0, 1]
        self.q_px4_odom = [0, 0, 0, 1]
        self.window_size = 8
        self.swa = SlidingWindowAverage(self.window_size)
        self.init_flag = False
        self.init_q = tf.transformations.quaternion_from_euler(0, 0, 0)
        
        rospy.Subscriber('/Odometry', Odometry, self.vins_callback)
        rospy.Subscriber('/mavros/local_position/odom', Odometry, self.px4_odom_callback)
        self.vision_pub = rospy.Publisher('/mavros/vision_pose/pose', PoseStamped, queue_size=10)
        self.rate = rospy.Rate(30.0)
        self.run()

    def from_quaternion_to_yaw(self, q):
        euler = tf.transformations.euler_from_quaternion(q)
        return euler[2]

    def vins_callback(self, msg):
        self.p_lidar_body = np.array([
            msg.pose.pose.position.x,
            msg.pose.pose.position.y,
            msg.pose.pose.position.z
        ])
        self.q_mav = [
            msg.pose.pose.orientation.x,
            msg.pose.pose.orientation.y,
            msg.pose.pose.orientation.z,
            msg.pose.pose.orientation.w
        ]

    def px4_odom_callback(self, msg):
        self.q_px4_odom = [
            msg.pose.pose.orientation.x,
            msg.pose.pose.orientation.y,
            msg.pose.pose.orientation.z,
            msg.pose.pose.orientation.w
        ]
        yaw = self.from_quaternion_to_yaw(self.q_px4_odom)
        self.swa.add_data(yaw)

    def run(self):
        while not rospy.is_shutdown():
            if self.swa.get_size() == self.window_size and not self.init_flag:
                init_yaw = self.swa.get_avg()
                self.init_q = tf.transformations.quaternion_from_euler(0, 0, init_yaw)
                self.init_flag = True
            
            if self.init_flag:
                rot_matrix = tf.transformations.quaternion_matrix(self.init_q)[:3, :3]
                p_enu = np.dot(rot_matrix, self.p_lidar_body)
                
                vision = PoseStamped()
                vision.header.stamp = rospy.Time.now()
                vision.header.frame_id = "map"
                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 = self.q_mav[0]
                vision.pose.orientation.y = self.q_mav[1]
                vision.pose.orientation.z = self.q_mav[2]
                vision.pose.orientation.w = self.q_mav[3]
                
                self.vision_pub.publish(vision)
                rospy.loginfo(
                    "\nPosition in ENU:\n x: {:.3f}\n y: {:.3f}\n z: {:.3f}\nOrientation of LiDAR:\n x: {:.3f}\n y: {:.3f}\n z: {:.3f}\n w: {:.3f}".format(
                        p_enu[0], p_enu[1], p_enu[2], self.q_mav[0], self.q_mav[1], self.q_mav[2], self.q_mav[3]
                    )
                )
            self.rate.sleep()

if __name__ == '__main__':
    try:
        FastLIOToMavros()
    except rospy.ROSInterruptException:
        pass

自行创建 fastlio_to_mavros 节点的 launch 文件。

2. 运行 MID360 和 FAST_LIO 程序

修改 livox_ros_driver2 中的 msg_MID360.launch，使其频率达到 30Hz（默认也能用）。

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

3. 运行 MAVROS

roslaunch mavros px4.launch

必须运行上述指令后马上运行下面的 fastlio_to_mavros。

4. 运行自定义 Launch 文件

运行自己创建的 fastlio_to_mavros 节点的 launch 文件。

三、验证

推荐用官方的仿真来理解坐标系。

你会发现一运行 fastlio_to_mavros 节点，无人机机头突然从指向北变成指向东方（确保罗盘已经被关了）。

在 Ubuntu 上位机看输入 rostopic echo /mavros/local_position/pose，机头所指的方向为正 X。X 在前，Y 朝向左，Z 朝向下。

打开 QGC，点左上角 Analyze Tools -> MAVlink 检测，出现了 LOCAL_POSITION_NED 数据。坐标系看法如下：NED 坐标系，X 为北，Y 为东，Z 为下。机头指向东，那么向东运动（机头方向），Y 会增大。向北运动（飞机左方向），X 会增大。那么向上运动，Z 会是负数，且越来越负。

实机向前方运动

会发现 QGC 的 LOCAL_POSITION_NED 的 Y 增大，/mavros/local_position/pose 的 X 增大。

实机向右方运动

会发现 QGC 的 LOCAL_POSITION_NED 的 X 变小为负数，/mavros/local_position/pose 的 Y 变小为负数。

当飞机向上运动

会发现 QGC 的 LOCAL_POSITION_NED 的 Z 变成负数并且不断减小。

四、总结

建议调试 PID 后再起飞，本文已验证定点飞行功能。