Fast-LIO到MAVROS视觉定位转换（含自启动脚本）

解决无人机移动后飞控位置偏移问题：Fast-LIO到MAVROS视觉定位转换

问题描述

在使用无人机进行实际飞行时，发现一个问题：当将无人机移动到其他位置后再放回原处，飞控的local position ned会发生改变，导致无人机无法准确悬停在期望位置。经过分析，怀疑是因为没有将外部视觉定位数据正确发送给飞控。
本次使用的飞控版本是：PX4 1.16.0 stable

问题分析

代码结构检查

通过检查项目代码结构，发现：

1. ego planner不直接发布/mavros/vision_pose/pose话题
2. px4ctrl仅订阅通用odom话题，但不将其转发到飞控
3. Fast-LIO发布/Odometry话题，但未直接与 MAVROS 集成

关键代码分析

查看px4ctrl模块的代码：

• PX4CtrlFSM.cpp实现了状态机逻辑，包括起飞、降落、悬停等模式
• input.h定义了Odom_Data_t结构体，用于接收里程计数据
• px4ctrl_node.cpp订阅odom话题并绑定到Odom_Data_t::feed函数

发现px4ctrl模块虽然接收了里程计数据用于控制，但没有将这些数据发送回飞控进行位置修正。

解决方案

！！等待香橙派所有节点都启动完成后，再启动本次的里程计转发节点！！

创建一个转换节点，将 Fast-LIO 发布的/Odometry话题数据转换并发布到/mavros/vision_pose/pose话题，使飞控能够接收外部视觉定位数据。

QGC设置飞控vision的ekf融合：

设置数传2口：
按图中设置，否则不能识别vison_position_pose

然后设置波特率

实现步骤

1. 创建功能包结构

mkdir-p/home/rog/code/fast_drone_ws/src/fastlio_to_mavros/{launch,scripts}

2. 配置文件

package.xml：定义功能包依赖和元信息
CMakeLists.txt：配置编译规则和安装路径
fastlio_to_mavros.launch：提供可配置的启动参数

3. 核心实现：优化后的转换脚本

文件名：

fastlio_to_mavros.py

#!/usr/bin/env python3# -*- coding: utf-8 -*-importrospyfromnav_msgs.msgimportOdometryfromgeometry_msgs.msgimportPoseStampedimporttfimportnumpyasnpfromcollectionsimportdeque# 滑动窗口平均类，用于平滑 yaw 值classSlidingWindowAverage:def__init__(self,window_size):self.window_size=window_size self.data_queue=deque(maxlen=window_size)# 使用固定大小的队列defadd_data(self,new_data):# 如果队列非空且新数据与最新数据差异过大，重置队列ifself.data_queueandabs(new_data-self.data_queue[-1])>0.01:self.data_queue.clear()self.data_queue.append(new_data)returnself.get_avg()defget_size(self):returnlen(self.data_queue)defget_avg(self):ifself.data_queue:returnsum(self.data_queue)/len(self.data_queue)else:return0.0classFastLIOToMavros:def__init__(self):rospy.init_node('fastlio_to_mavros',anonymous=True)# 获取参数self.window_size=rospy.get_param('~window_size',8)publish_rate=rospy.get_param('~publish_rate',30.0)# 初始化位姿和四元数self.p_lidar_body=np.zeros(3)self.q_mav=[0,0,0,1]self.q_px4_odom=[0,0,0,1]# 创建滑动窗口平均器self.swa=SlidingWindowAverage(self.window_size)# 初始化标志self.init_flag=Falseself.init_q=tf.transformations.quaternion_from_euler(0,0,0)# 订阅和发布器self.vins_sub=rospy.Subscriber('~fastlio_odom',Odometry,self.fastlio_callback,queue_size=10)self.px4_odom_sub=rospy.Subscriber('~px4_odom',Odometry,self.px4_odom_callback,queue_size=10)self.vision_pub=rospy.Publisher('~vision_pose',PoseStamped,queue_size=10)# 定时器控制发布频率self.timer=rospy.Timer(rospy.Duration(1.0/publish_rate),self.publish_vision_pose)rospy.loginfo("FastLIO to MAVROS converter initialized")rospy.loginfo(f"Window size:{self.window_size}, Publish rate:{publish_rate}Hz")deffrom_quaternion_to_yaw(self,q):# 将四元数转换为 yaw 角euler=tf.transformations.euler_from_quaternion(q)returneuler[2]deffastlio_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]defpx4_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)# 初始化 yaw 角ifself.swa.get_size()==self.window_sizeandnotself.init_flag:init_yaw=self.swa.get_avg()self.init_q=tf.transformations.quaternion_from_euler(0,0,init_yaw)self.init_flag=Truerospy.loginfo(f"Initial yaw initialized:{init_yaw:.3f}rad")defpublish_vision_pose(self,event):ifnotself.init_flag:return# 旋转位姿以对齐初始 yaw 角rot_matrix=tf.transformations.quaternion_matrix(self.init_q)[:3,:3]p_enu=np.dot(rot_matrix,self.p_lidar_body)# 构建视觉位姿消息vision_msg=PoseStamped()vision_msg.header.stamp=rospy.Time.now()vision_msg.header.frame_id="map"# 根据实际情况设置vision_msg.pose.position.x=p_enu[0]vision_msg.pose.position.y=p_enu[1]vision_msg.pose.position.z=p_enu[2]vision_msg.pose.orientation.x=self.q_mav[0]vision_msg.pose.orientation.y=self.q_mav[1]vision_msg.pose.orientation.z=self.q_mav[2]vision_msg.pose.orientation.w=self.q_mav[3]# 发布消息self.vision_pub.publish(vision_msg)# 每100次发布打印一次信息（减少终端输出）ifrospy.get_time()%10<0.01:rospy.loginfo_throttle(1.0,f"Position (ENU): x={p_enu[0]:.3f}, y={p_enu[1]:.3f}, z={p_enu[2]:.3f}")defmain():try:converter=FastLIOToMavros()rospy.spin()exceptrospy.ROSInterruptException:rospy.loginfo("FastLIO to MAVROS converter shutting down")passif__name__=='__main__':main()

文件名：

fastlio_to_mavros.launch

<launch><nodepkg="fastlio_to_mavros"type="fastlio_to_mavros.py"name="fastlio_to_mavros"output="screen"><!-- Fast-LIO odometry topic --><remapfrom="~fastlio_odom"to="/Odometry"/><!-- PX4localposition odometry topic --><remapfrom="~px4_odom"to="/mavros/local_position/odom"/><!-- MAVROS vision pose topic --><remapfrom="~vision_pose"to="/mavros/vision_pose/pose"/><!-- 配置参数 --><paramname="window_size"type="int"value="8"/><paramname="publish_rate"type="double"value="30.0"/></node></launch>

关键技术实现

1. 滑动窗口平均

实现了SlidingWindowAverage类，用于平滑 yaw 角数据，提高定位稳定性：

classSlidingWindowAverage:def__init__(self,window_size):self.window_size=window_size self.data_queue=deque(maxlen=window_size)# 使用固定大小的队列defadd_data(self,new_data):# 如果队列非空且新数据与最新数据差异过大，重置队列ifself.data_queueandabs(new_data-self.data_queue[-1])>0.01:self.data_queue.clear()self.data_queue.append(new_data)returnself.get_avg()

2. 位姿转换与发布

将 Fast-LIO 提供的位姿数据转换为 MAVROS 所需的格式，并以稳定的频率发布：

defpublish_vision_pose(self,event):ifnotself.init_flag:return# 旋转位姿以对齐初始 yaw 角rot_matrix=tf.transformations.quaternion_matrix(self.init_q)[:3,:3]p_enu=np.dot(rot_matrix,self.p_lidar_body)# 构建视觉位姿消息vision_msg=PoseStamped()vision_msg.header.stamp=rospy.Time.now()vision_msg.header.frame_id="map"vision_msg.pose.position.x=p_enu[0]vision_msg.pose.position.y=p_enu[1]vision_msg.pose.position.z=p_enu[2]vision_msg.pose.orientation.x=self.q_mav[0]vision_msg.pose.orientation.y=self.q_mav[1]vision_msg.pose.orientation.z=self.q_mav[2]vision_msg.pose.orientation.w=self.q_mav[3]# 发布消息self.vision_pub.publish(vision_msg)

代码优化

针对原始代码，进行了以下优化：

1. 使用 rospy.Timer 控制发布频率：确保稳定的 30Hz 发布频率，避免依赖 while 循环的精度问题
2. 使用固定大小的 deque：简化滑动窗口实现，自动管理队列大小
3. 添加参数化配置：支持通过 launch 文件配置窗口大小和发布频率
4. 优化日志输出：使用 throttle 减少终端输出频率，提高性能
5. 改进初始化逻辑：更清晰的初始化状态管理，便于调试

编译与运行

编译功能包

cd/home/rog/code/fast_drone_ws catkin_make--pkgfastlio_to_mavros

运行功能包

roslaunch fastlio_to_mavros fastlio_to_mavros.launch

检查是否输出数据：

rostopicecho/mavros/vision_pose/pose

查看话题订阅关系：

rqt_graph

查看效果

打开QGC到MAVLink检测，查看VISION_POSITION_ESTIMATE视觉位置估计：

运行里程计转发节点，查看EKF状态：

rostopicecho/mavros/estimator_status

这份状态数据表明核心定位问题已经解决！所有自动驾驶必需的状态位均已变为True，飞控已经具备执行 AUTO_HOVER 等自主模式的条件。

状态位关键变化解读

下一步：测试 AUTO_HOVER 模式（验证修复效果）
现在可以放心测试自主悬停，步骤如下：

1. 重启所有节点（确保配置生效）

   pkill-9roslaunchbash你的启动脚本.sh# 包含mavros/Fast-LIO/里程计转发

2. 解锁无人机（通过RC遥控器）
3. 切换到 AUTO_HOVER 模式
   • 观察飞控日志，Failsafe activated提示应该彻底消失；
   • 无人机应能稳定悬停，无漂移。

优化参数，提升悬停稳定性
若悬停时有轻微漂移，可微调以下参数降低里程计噪声权重：

# 降低外部里程计的位置噪声（让PX4更信任里程计数据）rosrun mavros mavparamsetEKF2_POS_NOISE0.005rosrun mavros mavparamsetEKF2_VEL_NOISE0.005rosrun mavros mavparam save

注意事项

1. 确保话题正确：确保 Fast-LIO 正在发布/Odometry话题，MAVROS 已正确连接并发布/mavros/local_position/odom话题
2. 飞控参数设置：需要在 PX4 飞控中启用视觉定位输入（EKF2 参数设置）
3. 坐标系一致性：检查 Fast-LIO 和 PX4 使用的坐标系定义是否一致
4. 启动顺序：建议先启动 Fast-LIO 和 MAVROS，再启动转换节点

结论

通过创建fastlio_to_mavros转换节点，成功将 Fast-LIO 的视觉定位数据发送给飞控，解决了无人机移动后飞控位置偏移的问题。该方案具有以下优点：

• 模块化设计：作为独立功能包，易于集成和维护
• 可配置性：支持通过参数调整窗口大小和发布频率
• 稳定性：使用滑动窗口平均提高了数据稳定性
• 高效性：优化的代码结构和日志输出，减少了系统资源占用

此实现为无人机提供了更准确的视觉定位，确保无人机在移动后放回原处时能够准确悬停在期望位置。

一键启动脚本：

tibos@tibos:~/auto_run$ cat auto_run_odom_to_PX4.sh

#!/bin/bash# BEGIN EARLY_BOOT_VERIFY# Confirm this boot before startup delays to avoid accumulating warm-boot retries.echo"[boot] Verifying bootloader and rootfs boot status"if["${EUID}"-eq0];then/usr/sbin/nvbootctrl verifyelsesudo-n/usr/sbin/nvbootctrl verifyfiboot_verify_status=$?if["${boot_verify_status}"-ne0];thenecho"[boot] Boot verification failed (status=${boot_verify_status}); startup aborted">&2exit"${boot_verify_status}"fi# END EARLY_BOOT_VERIFYsleep10source/home/tibos/.bashrcsource/opt/ros/noetic/setup.bashecho"livox_ros_driver2"sleep10cd/home/tibos/code/livox_mid360s_wssourcedevel/setup.bashnohuproslaunch livox_ros_driver2 msg_MID360s.launch&echo"fast_lio"sleep10nohuproslaunch fast_lio mapping_mid360.launch&echo"mavros"sleep10cd/home/tibos/code/mavros_wssourcedevel/setup.bashnohuproslaunch mavros px4.launch&echo"fastlio_to_mavros"sleep10cd/home/tibos/code/livox_mid360s_wssourcedevel/setup.bash# cd /home/tibos/code/fast_drone_ws# source devel/setup.bashnohuproslaunch fastlio_to_mavros fastlio_to_mavros.launch&# echo "fastlio_to_mavros"sleep10cd/home/tibos/bag_save_auto1nohuprosbag record--split--duration=60/livox/imu /livox/lidar /mavros/vision_pose/pose /mavros/setpoint_raw/attitude /mavros/gps_input/gps_input /mavros/local_position/pose&sleep10# sleep 99999exit0

参考资料：

https://blog.csdn.net/qq_44998513/article/details/133144421
https://blog.csdn.net/zlm2004/article/details/148501757?ops_request_misc=elastic_search_misc&request_id=68cf906ad3b389dd1332eb1ff7323bbe&biz_id=0&utm_medium=distribute.pc_search_result.none-task-blog-2_allElasticSearch~search_v2-2-148501757-null-null.142^v102pc_search_result_base2&utm_term=PX4%E6%97%A0%E4%BA%BA%E6%9C%BA%20mid360%E4%BD%BF%E7%94%A8fastlio&spm=1018.2226.3001.4187
https://blog.csdn.net/u010196944/article/details/134466325
https://docs.px4.io/main/zh/ros/mavros_offboard_cpp