PX4 无人机 MID360 配合 FAST_LIO 实现自主飞行与定点配置

PX4 无人机 MID360 配合 FAST_LIO 实现自主飞行与定点配置

固件版本：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/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 (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: