当前位置：首页 > news >正文

PX4无人机|MID360使用FAST_LIO，实现自主定位及定点——PX4无人机配置流程(五)

news 来源：原创 2025/6/15 9:45:23

PX4固件版本为1.15.4

qgc地面站版本为4.4.5

飞控，使用微空科技MicoAir743V2

机载电脑：12代i5,ubuntu20.04

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

推荐阅读px4+vio实现无人机室内定位_px4+室内视觉定位-CSDN博客

和飞控连接机载电脑相关，有用

代码参考：

PX4|基于FAST-LIO mid360的无人机室内自主定位及定点悬停_fastlio mid360-CSDN博客

使用视觉或动作捕捉系统进行位置估计 | PX4 指南（主） --- Using Vision or Motion Capture Systems for Position Estimation | PX4 Guide (main)

一.px4飞控设置

建议看官方文档：Using Vision or Motion Capture Systems for Position Estimation | PX4 Guide (main)

1.将px4定位数据源设置为vinsion

参数EKF2_EV_CTRL：可以默认，或者看参数手册按需配置（该教程默认）Parameter Reference | PX4 Guide (main)

EKF2_HGT_REF参数：Vision

2.关闭罗盘：

教程：
PX4 | 无人机关闭磁力计罗盘飞行（yaw estimate error报错解决方法）-CSDN博客

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

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

二、程序

1.代码

接下来创建发布 /mavros/vision_pose/pose 话题的功能包过程了

这里只提供代码，具体创建工作空间和功能包的步骤建议问AI

主要功能：将转化后的位姿信息以话题 /mavros/vision_pose/pose 发布。

#!/usr/bin/python3
#上面的python3不一定是这样写，建议找ai优化一下代码，和前面的推荐文章里的CPP代码其实一样的，该代码发布频率不太对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_sizeself.data_queue = deque()self.window_sum = 0.0def add_data(self, new_data):# 如果新数据与上一个数据差异过大，重置队列if self.data_queue and abs(new_data - self.data_queue[-1]) > 0.01:self.data_queue.clear()self.window_sum = 0.0self.data_queue.append(new_data)self.window_sum += new_data# 如果队列大小超过窗口大小，移除最早的数据if len(self.data_queue) > self.window_size:self.window_sum -= self.data_queue.popleft()return self.window_sum / len(self.data_queue)def get_size(self):return len(self.data_queue)def get_avg(self):if self.data_queue:return self.window_sum / len(self.data_queue)else:return 0.0class FastLIOToMavros:def __init__(self):rospy.init_node('fastlio_to_mavros', anonymous=True)# 初始化位姿和四元数self.p_lidar_body = np.zeros(3)self.q_mav = [0, 0, 0, 1]self.q_px4_odom = [0, 0, 0, 1]self.window_size = 8self.swa = SlidingWindowAverage(self.window_size)self.init_flag = Falseself.init_q = tf.transformations.quaternion_from_euler(0, 0, 0)# 订阅 Fast-LIO 的 Odometry 数据rospy.Subscriber('/Odometry', Odometry, self.vins_callback)# 订阅 PX4 的本地位置 Odometry 数据rospy.Subscriber('/mavros/local_position/odom', Odometry, self.px4_odom_callback)# 发布视觉位姿数据到 PX4self.vision_pub = rospy.Publisher('/mavros/vision_pose/pose', PoseStamped, queue_size=10)self.rate = rospy.Rate(30.0)self.run()def from_quaternion_to_yaw(self, q):# 将四元数转换为 yaw 角euler = tf.transformations.euler_from_quaternion(q)return euler[2]def vins_callback(self, msg):# 获取 Fast-LIO 提供的位姿和四元数self.p_lidar_body = np.array([msg.pose.pose.position.x,msg.pose.pose.position.y,msg.pose.pose.position.z])self.q_mav = [msg.pose.pose.orientation.x,msg.pose.pose.orientation.y,msg.pose.pose.orientation.z,msg.pose.pose.orientation.w]def px4_odom_callback(self, msg):# 获取 PX4 的本地位置四元数，并计算 yaw 角self.q_px4_odom = [msg.pose.pose.orientation.x,msg.pose.pose.orientation.y,msg.pose.pose.orientation.z,msg.pose.pose.orientation.w]yaw = self.from_quaternion_to_yaw(self.q_px4_odom)self.swa.add_data(yaw)def run(self):while not rospy.is_shutdown():# 初始化 yaw 角if self.swa.get_size() == self.window_size and not self.init_flag:init_yaw = self.swa.get_avg()self.init_q = tf.transformations.quaternion_from_euler(0, 0, init_yaw)self.init_flag = Trueif self.init_flag:# 旋转位姿以对齐初始 yaw 角rot_matrix = tf.transformations.quaternion_matrix(self.init_q)[:3, :3]p_enu = np.dot(rot_matrix, self.p_lidar_body)# 构建并发布视觉位姿消息vision = PoseStamped()vision.header.stamp = rospy.Time.now()vision.header.frame_id = "map"  # 根据实际情况设置vision.pose.position.x = p_enu[0]vision.pose.position.y = p_enu[1]vision.pose.position.z = p_enu[2]vision.pose.orientation.x = self.q_mav[0]vision.pose.orientation.y = self.q_mav[1]vision.pose.orientation.z = self.q_mav[2]vision.pose.orientation.w = self.q_mav[3]self.vision_pub.publish(vision)rospy.loginfo("\nPosition in ENU:\n   x: {:.3f}\n   y: {:.3f}\n   z: {:.3f}\nOrientation of LiDAR:\n   x: {:.3f}\n   y: {:.3f}\n   z: {:.3f}\n   w: {:.3f}".format(p_enu[0], p_enu[1], p_enu[2],self.q_mav[0], self.q_mav[1], self.q_mav[2], self.q_mav[3]))self.rate.sleep()if __name__ == '__main__':try:FastLIOToMavros()except rospy.ROSInterruptException:pass

自行创建fastlio_to_mavros节点的launch文件

2.运行mid360和fastlio的程序

建议参考MID360+fastlio功能笔记-CSDN博客

需要强调的是，安装官方说法修改livox_ros_driver2 msg_MID360.launch ，使其频率达到30HZ（其实默认也能用）方法在livox_ros_driver2的github底下写了。

输入下图指令查看是否修改了

roslaunch livox_ros_driver2 msg_MID360.launch 
在另一个终端中执行
roslaunch fast_lio mapping_mid360.launch

3.运行mavros

安装步骤参考：mavros安装——解决疑难杂症- PX4无人机配置流程(三)-CSDN博客


roslaunch mavros px4.launch

必须运行上面指令后马上运行下面的 fastlio_to_mavros

4.运行自己创建的fastlio_to_mavros节点的launch文件

运行结果，和qgc里的画面（飞控通过数传和Windows电脑QGC连接，故而频率很低）

三、验证

推荐用官方的仿真来理解坐标系

你会发现一运行fastlio_to_mavros节点，无人机机头突然从指向北变成指向东方（确保罗盘已经被关了）

在Ubuntu上位机看输入rostopic echo /mavros/local_position/pose机头所指的方向为正X。X在前，Y朝向左，Z朝向右

打开qgc、点左上角Analyze Tools——>MAVlink检测，出现了LOCAL_POSITION_NED数据，坐标系看法如下：NED坐标系，X为北，Y为东，Z为下，机头指向东，那么向东运动（机头方向），Y会增大。向北运动（飞机左方向），X会增大。那么向上运动，Z会是负数，且越来越负。