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

解决无人机移动后飞控位置偏移问题Fast-LIO到MAVROS视觉定位转换问题描述在使用无人机进行实际飞行时发现一个问题当将无人机移动到其他位置后再放回原处飞控的local position ned会发生改变导致无人机无法准确悬停在期望位置。经过分析怀疑是因为没有将外部视觉定位数据正确发送给飞控。本次使用的飞控版本是PX4 1.16.0 stable问题分析代码结构检查通过检查项目代码结构发现ego planner不直接发布/mavros/vision_pose/pose话题px4ctrl仅订阅通用odom话题但不将其转发到飞控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_sizewindow_size self.data_queuedeque(maxlenwindow_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,anonymousTrue)# 获取参数self.window_sizerospy.get_param(~window_size,8)publish_raterospy.get_param(~publish_rate,30.0)# 初始化位姿和四元数self.p_lidar_bodynp.zeros(3)self.q_mav[0,0,0,1]self.q_px4_odom[0,0,0,1]# 创建滑动窗口平均器self.swaSlidingWindowAverage(self.window_size)# 初始化标志self.init_flagFalseself.init_qtf.transformations.quaternion_from_euler(0,0,0)# 订阅和发布器self.vins_subrospy.Subscriber(~fastlio_odom,Odometry,self.fastlio_callback,queue_size10)self.px4_odom_subrospy.Subscriber(~px4_odom,Odometry,self.px4_odom_callback,queue_size10)self.vision_pubrospy.Publisher(~vision_pose,PoseStamped,queue_size10)# 定时器控制发布频率self.timerrospy.Timer(rospy.Duration(1.0/publish_rate),self.publish_vision_pose)rospy.loginfo(FastLIO to MAVROS converter initialized)rospy.loginfo(fWindow size:{self.window_size}, Publish rate:{publish_rate}Hz)deffrom_quaternion_to_yaw(self,q):# 将四元数转换为 yaw 角eulertf.transformations.euler_from_quaternion(q)returneuler[2]deffastlio_callback(self,msg):# 获取 Fast-LIO 提供的位姿和四元数self.p_lidar_bodynp.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]# 更新滑动窗口平均yawself.from_quaternion_to_yaw(self.q_px4_odom)self.swa.add_data(yaw)# 初始化 yaw 角ifself.swa.get_size()self.window_sizeandnotself.init_flag:init_yawself.swa.get_avg()self.init_qtf.transformations.quaternion_from_euler(0,0,init_yaw)self.init_flagTruerospy.loginfo(fInitial yaw initialized:{init_yaw:.3f}rad)defpublish_vision_pose(self,event):ifnotself.init_flag:return# 旋转位姿以对齐初始 yaw 角rot_matrixtf.transformations.quaternion_matrix(self.init_q)[:3,:3]p_enunp.dot(rot_matrix,self.p_lidar_body)# 构建视觉位姿消息vision_msgPoseStamped()vision_msg.header.stamprospy.Time.now()vision_msg.header.frame_idmap# 根据实际情况设置vision_msg.pose.position.xp_enu[0]vision_msg.pose.position.yp_enu[1]vision_msg.pose.position.zp_enu[2]vision_msg.pose.orientation.xself.q_mav[0]vision_msg.pose.orientation.yself.q_mav[1]vision_msg.pose.orientation.zself.q_mav[2]vision_msg.pose.orientation.wself.q_mav[3]# 发布消息self.vision_pub.publish(vision_msg)# 每100次发布打印一次信息减少终端输出ifrospy.get_time()%100.01:rospy.loginfo_throttle(1.0,fPosition (ENU): x{p_enu[0]:.3f}, y{p_enu[1]:.3f}, z{p_enu[2]:.3f})defmain():try:converterFastLIOToMavros()rospy.spin()exceptrospy.ROSInterruptException:rospy.loginfo(FastLIO to MAVROS converter shutting down)passif__name____main__:main()文件名fastlio_to_mavros.launchlaunchnodepkgfastlio_to_mavrostypefastlio_to_mavros.pynamefastlio_to_mavrosoutputscreen!-- Fast-LIO odometry topic --remapfrom~fastlio_odomto/Odometry/!-- PX4localposition odometry topic --remapfrom~px4_odomto/mavros/local_position/odom/!-- MAVROS vision pose topic --remapfrom~vision_poseto/mavros/vision_pose/pose/!-- 配置参数 --paramnamewindow_sizetypeintvalue8/paramnamepublish_ratetypedoublevalue30.0//node/launch关键技术实现1. 滑动窗口平均实现了SlidingWindowAverage类用于平滑 yaw 角数据提高定位稳定性classSlidingWindowAverage:def__init__(self,window_size):self.window_sizewindow_size self.data_queuedeque(maxlenwindow_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_matrixtf.transformations.quaternion_matrix(self.init_q)[:3,:3]p_enunp.dot(rot_matrix,self.p_lidar_body)# 构建视觉位姿消息vision_msgPoseStamped()vision_msg.header.stamprospy.Time.now()vision_msg.header.frame_idmapvision_msg.pose.position.xp_enu[0]vision_msg.pose.position.yp_enu[1]vision_msg.pose.position.zp_enu[2]vision_msg.pose.orientation.xself.q_mav[0]vision_msg.pose.orientation.yself.q_mav[1]vision_msg.pose.orientation.zself.q_mav[2]vision_msg.pose.orientation.wself.q_mav[3]# 发布消息self.vision_pub.publish(vision_msg)代码优化针对原始代码进行了以下优化使用 rospy.Timer 控制发布频率确保稳定的 30Hz 发布频率避免依赖 while 循环的精度问题使用固定大小的 deque简化滑动窗口实现自动管理队列大小添加参数化配置支持通过 launch 文件配置窗口大小和发布频率优化日志输出使用 throttle 减少终端输出频率提高性能改进初始化逻辑更清晰的初始化状态管理便于调试编译与运行编译功能包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 等自主模式的条件。状态位关键变化解读标志位当前值含义状态判定attitude_status_flagTrue姿态滚转/俯仰/偏航有效✅ 基础条件满足velocity_horiz_status_flagTrue水平速度有效✅ 飞控能感知水平移动pos_horiz_rel_status_flagTrue相对水平位置有效✅核心外部里程计已被PX4识别并启用const_pos_mode_status_flagTrue位置控制模式已激活✅ 飞控已准备好执行悬停/定位指令pos_vert_abs_status_flagTrue垂直高度有效✅ 定高功能正常pos_horiz_abs_status_flagFalse绝对水平位置无效❌ 无GPS不影响我们用相对定位下一步测试 AUTO_HOVER 模式验证修复效果现在可以放心测试自主悬停步骤如下重启所有节点确保配置生效pkill-9roslaunchbash你的启动脚本.sh# 包含mavros/Fast-LIO/里程计转发解锁无人机通过RC遥控器切换到 AUTO_HOVER 模式观察飞控日志Failsafe activated提示应该彻底消失无人机应能稳定悬停无漂移。优化参数提升悬停稳定性若悬停时有轻微漂移可微调以下参数降低里程计噪声权重# 降低外部里程计的位置噪声让PX4更信任里程计数据rosrun mavros mavparamsetEKF2_POS_NOISE0.005rosrun mavros mavparamsetEKF2_VEL_NOISE0.005rosrun mavros mavparam save注意事项确保话题正确确保 Fast-LIO 正在发布/Odometry话题MAVROS 已正确连接并发布/mavros/local_position/odom话题飞控参数设置需要在 PX4 飞控中启用视觉定位输入EKF2 参数设置坐标系一致性检查 Fast-LIO 和 PX4 使用的坐标系定义是否一致启动顺序建议先启动 Fast-LIO 和 MAVROS再启动转换节点结论通过创建fastlio_to_mavros转换节点成功将 Fast-LIO 的视觉定位数据发送给飞控解决了无人机移动后飞控位置偏移的问题。该方案具有以下优点模块化设计作为独立功能包易于集成和维护可配置性支持通过参数调整窗口大小和发布频率稳定性使用滑动窗口平均提高了数据稳定性高效性优化的代码结构和日志输出减少了系统资源占用此实现为无人机提供了更准确的视觉定位确保无人机在移动后放回原处时能够准确悬停在期望位置。一键启动脚本tibostibos:~/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 statusif[${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 aborted2exit${boot_verify_status}fi# END EARLY_BOOT_VERIFYsleep10source/home/tibos/.bashrcsource/opt/ros/noetic/setup.bashecholivox_ros_driver2sleep10cd/home/tibos/code/livox_mid360s_wssourcedevel/setup.bashnohuproslaunch livox_ros_driver2 msg_MID360s.launchechofast_liosleep10nohuproslaunch fast_lio mapping_mid360.launchechomavrossleep10cd/home/tibos/code/mavros_wssourcedevel/setup.bashnohuproslaunch mavros px4.launchechofastlio_to_mavrossleep10cd/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_mavrossleep10cd/home/tibos/bag_save_auto1nohuprosbag record--split--duration60/livox/imu /livox/lidar /mavros/vision_pose/pose /mavros/setpoint_raw/attitude /mavros/gps_input/gps_input /mavros/local_position/posesleep10# sleep 99999exit0参考资料https://blog.csdn.net/qq_44998513/article/details/133144421https://blog.csdn.net/zlm2004/article/details/148501757?ops_request_miscelastic_search_miscrequest_id68cf906ad3b389dd1332eb1ff7323bbebiz_id0utm_mediumdistribute.pc_search_result.none-task-blog-2allElasticSearch~search_v2-2-148501757-null-null.142v102pc_search_result_base2utm_termPX4%E6%97%A0%E4%BA%BA%E6%9C%BA%20mid360%E4%BD%BF%E7%94%A8fastliospm1018.2226.3001.4187https://blog.csdn.net/u010196944/article/details/134466325https://docs.px4.io/main/zh/ros/mavros_offboard_cpp