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:




