PX4 无人机 MID360 使用 FAST_LIO 实现自主飞行及定点配置

PX4 固件版本为 1.15.4

QGC 地面站版本为 4.4.5

飞控：微空科技 MicoAir743V2

机载电脑：12 代 i5, Ubuntu 20.04

安装位置：MID360 的接口对应飞机的后方

一、PX4 飞控设置

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

参数 EKF2_EV_CTRL：可以默认，或者看参数手册按需配置（该教程默认）。

参数 EKF2_HGT_REF：Vision

2. 关闭罗盘

需要注意的是，你现在走完上面飞控的设置，机头上电后会指向北。但在走完下面的程序后，你会发现上电后无人机机头指向东方（罗盘已经被关了）。

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

二、程序

注意，下面我把雷达等频率改到了 30HZ，但是在这频率下，定位精度实测只能有 10hz 的 60% 不到，慎重选择是否改频率！！！！！

1. 代码

接下来创建发布 /mavros/vision_pose/pose 话题的功能包过程了。这里只提供代码，具体创建工作空间和功能包的步骤建议查阅官方文档或相关教程。主要功能：将转化后的位姿信息以话题 /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

# 滑动窗口平均类，用于平滑 yaw 值
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 - .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: