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):
         .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)

 :
     ():
        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: