PX4 无人机结合 MID360 与 FAST-LIO 实现室内自主定位与定点
硬件与环境配置
本次实验基于以下软硬件环境：
- 飞控固件：PX4 1.15.4
- 地面站：QGC 4.4.5
- 飞控型号：微空科技 MicoAir743V2
- 机载电脑：Intel 12 代 i5，Ubuntu 20.04
- 雷达安装：MID360 接口对应飞机后方
注意：确保机载电脑与飞控连接稳定，建议参考官方文档关于外部位置估计的配置。
一、PX4 飞控参数设置
1. 设置定位数据源
将 PX4 的定位数据源切换为视觉/外部系统。主要涉及以下参数：
EKF2_EV_CTRL：保持默认或根据手册按需配置（本教程采用默认）。EKF2_HGT_REF：设置为 Vision。
2. 关闭磁力计（罗盘）
由于使用激光雷达进行定位，需关闭罗盘以避免干扰。
重要提示：完成上述飞控设置后，上电时无人机会指向北方。但在后续程序运行后，你会发现机头指向东方（因为罗盘已关闭），这是正常现象。如下图所示（实际以 QGC 显示为准）。
此外，融合激光测距数据能显著提升定位稳定性。
二、ROS 节点开发
我们需要创建一个 ROS 节点，负责将 Fast-LIO 输出的位姿信息转换为 MAVROS 可识别的 /mavros/vision_pose/pose 话题。
1. 核心代码逻辑
该脚本主要功能包括订阅 Fast-LIO 的里程计数据和 PX4 的本地位置数据，通过滑动窗口平滑 yaw 角，并将转换后的位姿发布给 MAVROS。
#!/usr/bin/env python3
# -*- coding: utf-8 -*-
import rospy
from nav_msgs.msg import Odometry
from geometry_msgs.msg import PoseStamped
import tf.transformations as tf_trans
import numpy as np
from collections import deque
class SlidingWindowAverage:
"""用于平滑 yaw 值的滑动窗口平均类"""
def __init__(self, window_size):
self.window_size = window_size
.data_queue = deque()
.window_sum =
():
.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_trans.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_trans.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_trans.quaternion_from_euler(, , init_yaw)
.init_flag =
.init_flag:
rot_matrix = tf_trans.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:
