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融合：

实现步骤

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. 核心实现：优化后的转换脚本

#!/usr/bin/env python3
# -*- coding: utf-8 -*-

import rospy
from nav_msgs.msg import Odometry
from geometry_msgs.msg import PoseStamped
import tf
import numpy as np
from collections import deque

# 滑动窗口平均类，用于平滑 yaw 值
class SlidingWindowAverage:
    def __init__(self, window_size):
        self.window_size = window_size
        self.data_queue = deque(maxlen=window_size)  # 使用固定大小的队列
        
    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.data_queue.append(new_data)
        return self.get_avg()
    
    def get_size(self):
        return len(self.data_queue)
    
    def get_avg(self):
        if self.data_queue:
            return sum(self.data_queue) / len(self.data_queue)
        else:
            return 0.0

class FastLIOToMavros:
    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 = False
        self.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")
        
    def from_quaternion_to_yaw(self, q):
        # 将四元数转换为 yaw 角
        euler = tf.transformations.euler_from_quaternion(q)
        return euler[2]
    
    def fastlio_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)
        
        # 初始化 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
            rospy.loginfo(f"Initial yaw initialized: {init_yaw:.3f} rad")
    
    def publish_vision_pose(self, event):
        if not self.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次发布打印一次信息（减少终端输出）
        if rospy.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}"
            )

def main():
    try:
        converter = FastLIOToMavros()
        rospy.spin()
    except rospy.ROSInterruptException:
        rospy.loginfo("FastLIO to MAVROS converter shutting down")
        pass

if __name__ == '__main__':
    main()

关键技术实现

1. 滑动窗口平均

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

class SlidingWindowAverage:
    def __init__(self, window_size):
        self.window_size = window_size
        self.data_queue = deque(maxlen=window_size)  # 使用固定大小的队列
        
    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.data_queue.append(new_data)
        return self.get_avg()

2. 位姿转换与发布

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

def publish_vision_pose(self, event):
    if not self.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 --pkg fastlio_to_mavros

运行功能包

roslaunch fastlio_to_mavros fastlio_to_mavros.launch

检查是否输出数据：

rostopic echo /mavros/vision_pose/pose

查看话题订阅关系：

rqt_graph

查看效果

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

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

rostopic echo /mavros/estimator_status

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

状态位关键变化解读

标志位	当前值	含义	状态判定
attitude_status_flag	True	姿态（滚转/俯仰/偏航）有效	✅ 基础条件满足
velocity_horiz_status_flag	True	水平速度有效	✅ 飞控能感知水平移动
pos_horiz_rel_status_flag	True	相对水平位置有效	✅ 核心！外部里程计已被PX4识别并启用
const_pos_mode_status_flag	True	位置控制模式已激活	✅ 飞控已准备好执行悬停/定位指令
pos_vert_abs_status_flag	True	垂直高度有效	✅ 定高功能正常
pos_horiz_abs_status_flag	False	绝对水平位置无效	❌ 无GPS，不影响（我们用相对定位）

下一步：测试 AUTO_HOVER 模式（验证修复效果）

现在可以放心测试自主悬停，步骤如下：

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

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

2. 解锁无人机（通过RC遥控器）

3. 切换到 AUTO_HOVER 模式

   • 观察飞控日志，Failsafe activated 提示应该彻底消失；
   • 无人机应能稳定悬停，无漂移。

优化参数，提升悬停稳定性

若悬停时有轻微漂移，可微调以下参数降低里程计噪声权重：

# 降低外部里程计的位置噪声（让PX4更信任里程计数据）
rosrun mavros mavparam set EKF2_POS_NOISE 0.005
rosrun mavros mavparam set EKF2_VEL_NOISE 0.005
rosrun 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 的视觉定位数据发送给飞控，解决了无人机移动后飞控位置偏移的问题。该方案具有以下优点：

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

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

参考资料：

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~all~ElasticSearch\~search_v2-2-148501757-null-null.142^v102^pc_search_result_base2\&utm_term=PX4无人机 mid360使用fastlio\&spm=1018.2226.3001.4187

https://blog.csdn.net/u010196944/article/details/134466325

https://docs.px4.io/main/zh/ros/mavros_offboard_cpp