PX4 无人机 MID360 激光雷达 FAST-LIO 室内定位与定点悬停配置

PX4 无人机 MID360 激光雷达 FAST-LIO 室内定位与定点悬停配置

硬件与环境准备

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

建议查阅官方文档了解外部定位系统配置细节。

一、PX4 飞控设置

1. 将定位数据源设置为视觉

修改以下参数，确保飞控使用视觉/激光里程计作为主要定位源：

• EKF2_EV_CTRL：保持默认或按需配置（本教程采用默认）
• EKF2_HGT_REF：设置为 Vision

2. 关闭罗盘

由于依赖视觉/激光定位，需关闭磁力计以避免干扰。注意，完成此设置后上电，机头可能不再指向正北。

注意：在后续程序运行前，飞控上电默认指向北方；但加载程序后，若罗盘已关闭，机头可能指向东方（具体取决于坐标系定义）。

此外，融合激光测距数据能显著提升定位稳定性。

二、程序开发

我们需要创建一个 ROS 节点，将 Fast-LIO 输出的位姿信息转换为 MAVROS 可识别的 /mavros/vision_pose/pose 话题。

1. 核心代码

以下是 Python 脚本示例，实现了滑动窗口平滑处理及四元数转换逻辑：

#!/usr/bin/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

# 滑动窗口平均类，用于平滑 yaw 值
class SlidingWindowAverage:
    def __init__(self, window_size):
        self.window_size = window_size
        self.data_queue = deque()
        self.window_sum = 0.0

    def ():
        
         .data_queue  (new_data - .data_queue[-]) > :
            .data_queue.clear()
            .window_sum = 
        .data_queue.append(new_data)
        .window_sum += new_data
        
         (.data_queue) > .window_size:
            .window_sum -= .data_queue.popleft()
         .window_sum / (.data_queue)

     ():
         (.data_queue)

     ():
         .data_queue:
             .window_sum / (.data_queue)
        :
             

 :
     ():
        rospy.init_node(, anonymous=)
        
        .p_lidar_body = np.zeros()
        .q_mav = [, , , ]
        .q_px4_odom = [, , , ]
        .window_size = 
        .swa = SlidingWindowAverage(.window_size)
        .init_flag = 
        .init_q = tf.transformations.quaternion_from_euler(, , )

        
        rospy.Subscriber(, Odometry, .vins_callback)
        
        rospy.Subscriber(, Odometry, .px4_odom_callback)
        
        .vision_pub = rospy.Publisher(, PoseStamped, queue_size=)
        .rate = rospy.Rate()
        .run()

     ():
        euler = tf.transformations.euler_from_quaternion(q)
         euler[]

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

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

     ():
          rospy.is_shutdown():
            
             .swa.get_size() == .window_size   .init_flag:
                init_yaw = .swa.get_avg()
                .init_q = tf.transformations.quaternion_from_euler(, , init_yaw)
                .init_flag = 

             .init_flag:
                
                rot_matrix = tf.transformations.quaternion_matrix(.init_q)[:, :]
                p_enu = np.dot(rot_matrix, .p_lidar_body)

                
                vision = PoseStamped()
                vision.header.stamp = rospy.Time.now()
                vision.header.frame_id = 
                vision.pose.position.x = p_enu[]
                vision.pose.position.y = p_enu[]
                vision.pose.position.z = p_enu[]
                vision.pose.orientation.x = .q_mav[]
                vision.pose.orientation.y = .q_mav[]
                vision.pose.orientation.z = .q_mav[]
                vision.pose.orientation.w = .q_mav[]

                .vision_pub.publish(vision)
                rospy.loginfo(
                    .(
                        p_enu[], p_enu[], p_enu[], .q_mav[], .q_mav[], .q_mav[], .q_mav[]
                    )
                )
            .rate.sleep()

 __name__ == :
    :
        FastLIOToMavros()
     rospy.ROSInterruptException: