PX4无人机|MID360使用FAST_LIO，实现自主飞行及定点——PX4无人机配置流程(六)
PX4固件版本为1.15.4
qgc地面站版本为4.4.5
飞控，使用微空科技MicoAir743V2
机载电脑：12代i5,ubuntu20.04
安装位置：mid360的接口对应飞机的后方
推荐阅读px4+vio实现无人机室内定位_px4+室内视觉定位-ZEEKLOG博客
和飞控连接机载电脑相关，有用
代码参考：
PX4|基于FAST-LIO mid360的无人机室内自主定位及定点悬停_fastlio mid360-ZEEKLOG博客
使用视觉或动作捕捉系统进行位置估计 | 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报错解决方法）-ZEEKLOG博客
需要注意的是，你现在走完上面飞控的设置，机头上电后会指向北。但在走完下面的程序后，你会发现上电后无人机机头指向东方（罗盘已经被关了）如下图所示
此外，加一个激光测距融合好处多多
二、程序
注意，下面我把雷达等频率改到了30HZ，但是在这频率下，定位精度实测只能有10hz的60%不到，慎重选择是否改频率！！！！！
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_size self.data_queue = deque() self.window_sum = 0.0 def 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.0 self.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.0 class 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 = 8 self.swa = SlidingWindowAverage(self.window_size) self.init_flag = False self.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) # 发布视觉位姿数据到 PX4 self.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 = True if 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功能笔记-ZEEKLOG博客
按照官方说法 修改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无人机配置流程(三)-ZEEKLOG博客
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会是负数，且越来越负。
实机向前方运动
会发现 QGC的LOCAL_POSITION_NED的Y增大
/mavros/local_position/pose的X增大
实机向右方运动
会发现 QGC的LOCAL_POSITION_NED的X变小为负数
/mavros/local_position/pose的Y变小为负数
当飞机向上运动
会发现 QGC的LOCAL_POSITION_NED的Z变成负数并且不断减小
