PX4飞控定位源配置完全指南
·
——从视觉+GPS融合到MAVROS/Python实现
1. 概述
1.1 为什么需要配置定位源？
在无人机自主飞行中，精确定位是核心要素。不同的应用场景需要不同的定位方案：
-
室外空旷环境：GPS足够
-
室内/GPS拒止环境：需要视觉定位
-
复杂环境：需要多源融合
PX4通过EKF2_AID_MASK参数（通常参数ID为26）实现灵活的多源定位融合策略。
1.2 本文目标
本文将从零开始，详细介绍如何通过Python代码配置PX4飞控的定位源，实现GPS+视觉的融合定位，并提供完整的代码示例和故障排除方案。
2. 基础知识
2.1 PX4 EKF2架构
PX4使用扩展卡尔曼滤波器（EKF2）融合多种传感器数据：
2.2 定位源类型
| 定位源 | 优点 | 缺点 | 适用场景 |
|---|---|---|---|
| GPS | 全球覆盖，绝对定位 | 易受干扰，室内无效 | 室外开阔地 |
| 视觉定位 | 室内外通用，精度高 | 需要特征点，计算量大 | 室内、复杂环境 |
| 光流 | 成本低，简单 | 精度一般，易受光线影响 | 室内低空 |
| 激光雷达 | 精度极高 | 成本高，重量大 | 精密定位 |
3. 核心参数详解
3.1 EKF2_AID_MASK参数
这是配置定位源的核心参数，通过位掩码的方式启用不同的定位源。
位掩码定义
| 位 | 值 | 功能 | 说明 |
|---|---|---|---|
| 0 | 1 | GPS | 启用GPS定位 |
| 1 | 2 | 视觉位置（旧版） | 已废弃 |
| 2 | 4 | 视觉速度（旧版） | 已废弃 |
| 3 | 8 | 视觉位置 | 使用视觉提供位置信息 |
| 4 | 16 | 视觉偏航 | 使用视觉提供偏航角 |
| 5 | 32 | 外部偏航 | 使用外部源偏航 |
| 6 | 64 | 光流 | 启用光流定位 |
常用配置组合
# 纯GPS定位
GPS_ONLY = 1 # 二进制: 000001
# 纯视觉定位（位置+偏航）
VISION_ONLY = 24 # 二进制: 011000 (8+16)
# GPS + 视觉位置（无偏航）
GPS_VISION_POS = 9 # 二进制: 001001 (1+8)
# GPS + 视觉位置 + 视觉偏航（推荐）
GPS_VISION_FULL = 25 # 二进制: 011001 (1+8+16)
# GPS + 视觉位置 + 视觉偏航 + 光流
GPS_VISION_FLOW = 89 # 二进制: 1011001 (1+8+16+64)3.2 相关辅助参数
# 视觉数据延迟补偿（秒）
EKF2_EV_DELAY = 0.0
# 视觉传感器安装位置偏移（米）
EKF2_EV_POS_X = 0.0 # X方向偏移
EKF2_EV_POS_Y = 0.0 # Y方向偏移
EKF2_EV_POS_Z = 0.0 # Z方向偏移
# 视觉噪声模式
EKF2_EV_NOISE_MD = 0 # 0: 标准模式, 1: 低噪声模式4. 环境配置
4.1 系统要求
# Ubuntu 18.04/20.04/22.04
# ROS Melodic/Noetic
# Python 3.6+
4.2 安装依赖
# 安装ROS和MAVROS
sudo sh -c 'echo "deb http://packages.ros.org/ros/ubuntu $(lsb_release -sc) main" > /etc/apt/sources.list.d/ros-latest.list'
sudo apt-key adv --keyserver 'hkp://keyserver.ubuntu.com:80' --recv-key C1CF6E31E6BADE8868B172B4F42ED6FBAB17C654
sudo apt update
sudo apt install ros-noetic-desktop-full
sudo apt install ros-noetic-mavros ros-noetic-mavros-extras ros-noetic-mavros-msgs
# 安装Python依赖
pip3 install rospkg catkin_pkg pymavlink
# 下载MAVROS的Geoid模型
sudo /opt/ros/noetic/lib/mavros/install_geographiclib_datasets.sh4.3 创建ROS工作空间
mkdir -p ~/catkin_ws/src
cd ~/catkin_ws
catkin_make
source devel/setup.bash
echo "source ~/catkin_ws/devel/setup.bash" >> ~/.bashrc4.4 测试MAVROS连接
# 启动MAVROS（以USB连接为例）
roslaunch mavros px4.launch fcu_url:=/dev/ttyUSB0:921600
# 检查连接状态
rostopic echo /mavros/state
# 查看当前参数
rosparam list | grep mavros5. Python代码实现
5.1 基础版：单次设置脚本
#!/usr/bin/env python3
# -*- coding: utf-8 -*-
"""
文件名: set_vision_gps_simple.py
功能: 快速设置PX4定位源为GPS+视觉融合
"""
import rospy
import sys
from mavros_msgs.srv import ParamSet, ParamSetRequest
class SimpleParamSetter:
def __init__(self):
rospy.init_node('simple_param_setter', anonymous=True)
def set_ekf_mask(self, mask_value=25):
"""设置EKF2_AID_MASK参数"""
rospy.loginfo("等待服务可用...")
rospy.wait_for_service('/mavros/param/set')
try:
set_param = rospy.ServiceProxy('/mavros/param/set', ParamSet)
req = ParamSetRequest()
req.param_id = "EKF2_AID_MASK"
req.value.integer = mask_value
req.value.real = 0.0
response = set_param(req)
if response.success:
rospy.loginfo(f"✅ 成功设置 EKF2_AID_MASK = {mask_value}")
rospy.loginfo(f" 二进制: {bin(mask_value)}")
return True
else:
rospy.logwarn("❌ 参数设置失败")
return False
except rospy.ServiceException as e:
rospy.logerr(f"服务调用失败: {e}")
return False
if __name__ == "__main__":
# 使用命令行参数或默认值
mask = int(sys.argv[1]) if len(sys.argv) > 1 else 25
setter = SimpleParamSetter()
if setter.set_ekf_mask(mask):
rospy.loginfo("配置完成！建议重启飞控使配置生效")
else:
rospy.logerr("配置失败！")5.2 进阶版：完整功能类
#!/usr/bin/env python3
# -*- coding: utf-8 -*-
"""
文件名: px4_positioning_config.py
功能: PX4定位源完整配置工具
版本: 2.0
作者: UAV Development Team
"""
import rospy
import sys
import time
from enum import IntEnum
from mavros_msgs.srv import ParamSet, ParamGet, ParamSetRequest, ParamGetRequest
from mavros_msgs.msg import State
from geometry_msgs.msg import PoseStamped, TwistStamped
class PositioningMode(IntEnum):
"""定位模式枚举"""
GPS_ONLY = 1 # 纯GPS
VISION_ONLY = 24 # 纯视觉（位置+偏航）
GPS_VISION_POS = 9 # GPS + 视觉位置
GPS_VISION_FULL = 25 # GPS + 视觉位置 + 视觉偏航（推荐）
GPS_VISION_FLOW = 89 # GPS + 视觉 + 光流
class PX4PositioningConfig:
"""PX4定位源配置类"""
def __init__(self):
"""初始化配置器"""
rospy.init_node('px4_positioning_config', anonymous=True)
self.connection_status = False
self.current_mode = None
# 订阅飞控状态
self.state_sub = rospy.Subscriber('/mavros/state', State, self.state_callback)
rospy.loginfo("="*60)
rospy.loginfo("PX4定位源配置工具 v2.0")
rospy.loginfo("="*60)
def state_callback(self, msg):
"""飞控状态回调"""
self.connection_status = msg.connected
if msg.armed:
rospy.logwarn("⚠️ 飞控已解锁，修改参数可能导致失控！")
def wait_for_connection(self, timeout=10):
"""等待MAVROS连接"""
rospy.loginfo("等待MAVROS连接...")
start_time = time.time()
while not rospy.is_shutdown():
if self.connection_status:
rospy.loginfo("✅ MAVROS已连接")
return True
if time.time() - start_time > timeout:
rospy.logerr("❌ 连接超时")
return False
rospy.sleep(0.1)
def get_parameter(self, param_id):
"""获取PX4参数值"""
try:
rospy.wait_for_service('/mavros/param/get', timeout=3)
get_param = rospy.ServiceProxy('/mavros/param/get', ParamGet)
req = ParamGetRequest()
req.param_id = param_id
resp = get_param(req)
if resp.success:
# 判断参数类型
if resp.value.real != 0:
value = resp.value.real
else:
value = resp.value.integer
rospy.loginfo(f"📊 参数 {param_id} = {value}")
return value
else:
rospy.logwarn(f"⚠️ 无法获取参数 {param_id}")
return None
except rospy.ServiceException as e:
rospy.logerr(f"❌ 服务调用失败: {e}")
return None
def set_parameter(self, param_id, value, is_integer=True):
"""设置PX4参数"""
try:
rospy.wait_for_service('/mavros/param/set', timeout=3)
set_param = rospy.ServiceProxy('/mavros/param/set', ParamSet)
req = ParamSetRequest()
req.param_id = param_id
if is_integer:
req.value.integer = int(value)
req.value.real = 0.0
else:
req.value.real = float(value)
req.value.integer = 0
resp = set_param(req)
if resp.success:
rospy.loginfo(f"✅ 参数设置成功: {param_id} = {value}")
return True
else:
rospy.logwarn(f"⚠️ 参数设置失败: {param_id}")
return False
except rospy.ServiceException as e:
rospy.logerr(f"❌ 服务调用失败: {e}")
return False
def set_positioning_mode(self, mode: PositioningMode):
"""设置定位模式"""
rospy.loginfo(f"\n🔧 设置定位模式: {mode.name}")
# 获取当前模式
current = self.get_parameter("EKF2_AID_MASK")
if current is not None:
rospy.loginfo(f" 当前模式值: {current}")
# 设置新模式
success = self.set_parameter("EKF2_AID_MASK", mode.value, is_integer=True)
if success:
self.current_mode = mode
rospy.loginfo(f"✅ 已切换到 {mode.name} 模式")
self._show_mode_info(mode)
else:
rospy.logerr(f"❌ 切换模式失败")
return success
def _show_mode_info(self, mode: PositioningMode):
"""显示模式详细信息"""
info = {
PositioningMode.GPS_ONLY: {
"desc": "仅使用GPS定位",
"适用场景": "室外空旷环境",
"注意": "室内无法使用"
},
PositioningMode.VISION_ONLY: {
"desc": "仅使用视觉定位",
"适用场景": "室内/GPS拒止环境",
"注意": "需要视觉系统支持"
},
PositioningMode.GPS_VISION_POS: {
"desc": "GPS + 视觉位置融合",
"适用场景": "室内外过渡区域",
"注意": "偏航角仍来自GPS"
},
PositioningMode.GPS_VISION_FULL: {
"desc": "GPS + 视觉位置 + 视觉偏航",
"适用场景": "复杂环境，推荐配置",
"注意": "需要高质量视觉数据"
}
}
if mode in info:
rospy.loginfo("\n📋 模式详情:")
for key, value in info[mode].items():
rospy.loginfo(f" {key}: {value}")
def configure_vision_parameters(self):
"""配置视觉相关参数"""
rospy.loginfo("\n🎯 配置视觉参数...")
params = {
"EKF2_EV_DELAY": (0.0, False), # (值, 是否为整数)
"EKF2_EV_POS_X": (0.0, False),
"EKF2_EV_POS_Y": (0.0, False),
"EKF2_EV_POS_Z": (0.0, False),
"EKF2_EV_NOISE_MD": (0, True),
}
success_count = 0
for param_name, (value, is_int) in params.items():
if self.set_parameter(param_name, value, is_int):
success_count += 1
rospy.loginfo(f"视觉参数配置: {success_count}/{len(params)} 成功")
return success_count == len(params)
def verify_configuration(self):
"""验证配置是否生效"""
rospy.loginfo("\n🔍 验证配置...")
final_value = self.get_parameter("EKF2_AID_MASK")
if final_value == self.current_mode.value:
rospy.loginfo("✅ 配置验证通过")
return True
else:
rospy.logwarn(f"⚠️ 配置验证失败: 期望 {self.current_mode.value}, 实际 {final_value}")
return False
def restart_ekf(self):
"""重启EKF2（可选）"""
rospy.loginfo("\n🔄 发送EKF2重启命令...")
# 通过MAVROS服务重启
try:
from mavros_msgs.srv import CommandLong
rospy.wait_for_service('/mavros/cmd/command')
cmd = rospy.ServiceProxy('/mavros/cmd/command', CommandLong)
# MAV_CMD_PREFLIGHT_REBOOT_SHUTDOWN = 246
response = cmd(command=246, param1=1, param2=0, param3=0,
param4=0, param5=0, param6=0, param7=0)
if response.success:
rospy.loginfo("✅ 重启命令已发送")
return True
else:
rospy.logwarn("⚠️ 重启命令发送失败")
return False
except Exception as e:
rospy.logwarn(f"无法重启EKF: {e}")
rospy.loginfo("建议手动重启飞控")
return False
def main():
"""主函数"""
config = PX4PositioningConfig()
# 等待连接
if not config.wait_for_connection():
sys.exit(1)
# 显示菜单
print("\n" + "="*60)
print("请选择定位模式:")
print(" 1. 纯GPS定位")
print(" 2. 纯视觉定位")
print(" 3. GPS + 视觉位置")
print(" 4. GPS + 视觉位置 + 视觉偏航 (推荐)")
print(" 5. 自定义掩码值")
print(" q. 退出")
print("="*60)
choice = input("请输入选项 (1-5/q): ").strip()
mode_map = {
'1': PositioningMode.GPS_ONLY,
'2': PositioningMode.VISION_ONLY,
'3': PositioningMode.GPS_VISION_POS,
'4': PositioningMode.GPS_VISION_FULL,
}
if choice in mode_map:
# 设置模式
if config.set_positioning_mode(mode_map[choice]):
# 如果是视觉模式，配置视觉参数
if mode_map[choice] in [PositioningMode.VISION_ONLY,
PositioningMode.GPS_VISION_POS,
PositioningMode.GPS_VISION_FULL]:
config.configure_vision_parameters()
# 验证配置
rospy.sleep(1)
config.verify_configuration()
# 询问是否重启
restart = input("\n是否重启飞控使配置生效？(y/n): ").strip().lower()
if restart == 'y':
config.restart_ekf()
elif choice == '5':
# 自定义掩码
try:
custom_mask = int(input("请输入掩码值 (1-255): "))
config.set_parameter("EKF2_AID_MASK", custom_mask, True)
except ValueError:
rospy.logerr("无效的掩码值")
elif choice == 'q':
rospy.loginfo("退出程序")
sys.exit(0)
else:
rospy.logerr("无效选项")
rospy.loginfo("\n配置完成！")
if __name__ == "__main__":
try:
main()
except rospy.ROSInterruptException:
pass
except KeyboardInterrupt:
rospy.loginfo("\n用户中断")
sys.exit(0)5.3 高级版：带视觉数据接收
#!/usr/bin/env python3
# -*- coding: utf-8 -*-
"""
文件名: vision_fusion_system.py
功能: 完整的视觉融合系统，包括数据接收和配置
"""
import rospy
import numpy as np
from geometry_msgs.msg import PoseStamped, TwistStamped
from mavros_msgs.msg import GlobalPositionTarget, State
from mavros_msgs.srv import ParamSet, CommandBool, SetMode
import tf.transformations as tf
class VisionFusionSystem:
"""视觉融合系统完整实现"""
def __init__(self):
rospy.init_node('vision_fusion_system', anonymous=True)
# 系统状态
self.armed = False
self.offboard_mode = False
self.vision_available = False
# 配置参数
self.ekf_mask = 25 # GPS + 视觉位置 + 视觉偏航
# 订阅者
self.state_sub = rospy.Subscriber('/mavros/state', State, self.state_callback)
self.vision_pose_sub = rospy.Subscriber('/vision/pose', PoseStamped, self.vision_pose_callback)
# 发布者
self.vision_pose_pub = rospy.Publisher('/mavros/vision_pose/pose', PoseStamped, queue_size=10)
# 服务
self.arm_service = rospy.ServiceProxy('/mavros/cmd/arming', CommandBool)
self.set_mode_service = rospy.ServiceProxy('/mavros/set_mode', SetMode)
rospy.loginfo("视觉融合系统初始化完成")
def state_callback(self, msg):
"""飞控状态回调"""
self.armed = msg.armed
self.offboard_mode = msg.mode == "OFFBOARD"
def vision_pose_callback(self, msg):
"""视觉定位数据回调"""
self.vision_available = True
# 转发到MAVROS
self.vision_pose_pub.publish(msg)
def configure_ekf(self):
"""配置EKF2参数"""
rospy.loginfo("配置EKF2参数...")
# 设置定位源掩码
rospy.wait_for_service('/mavros/param/set')
set_param = rospy.ServiceProxy('/mavros/param/set', ParamSet)
params = {
"EKF2_AID_MASK": self.ekf_mask,
"EKF2_HGT_MODE": 2, # 使用视觉高度
"EKF2_EV_DELAY": 0.0,
}
for param_name, value in params.items():
req = ParamSet()
req.param_id = param_name
req.value.integer = value
try:
set_param(req)
rospy.loginfo(f"设置 {param_name} = {value}")
except:
rospy.logerr(f"设置 {param_name} 失败")
def start_vision_fusion(self):
"""启动视觉融合"""
rospy.loginfo("启动视觉融合系统...")
# 配置参数
self.configure_ekf()
# 等待视觉数据
rospy.loginfo("等待视觉数据...")
timeout = 30
start_time = rospy.Time.now().to_sec()
while not self.vision_available:
if rospy.Time.now().to_sec() - start_time > timeout:
rospy.logerr("视觉数据超时")
return False
rospy.sleep(0.1)
rospy.loginfo("视觉数据接收正常")
# 切换到OFFBOARD模式
rospy.loginfo("切换到OFFBOARD模式...")
if self.set_mode_service(custom_mode="OFFBOARD"):
rospy.loginfo("OFFBOARD模式已激活")
return True
else:
rospy.logerr("OFFBOARD模式切换失败")
return False
def publish_vision_pose(self, x, y, z, qx, qy, qz, qw):
"""发布视觉定位数据"""
pose_msg = PoseStamped()
pose_msg.header.stamp = rospy.Time.now()
pose_msg.header.frame_id = "vision"
pose_msg.pose.position.x = x
pose_msg.pose.position.y = y
pose_msg.pose.position.z = z
pose_msg.pose.orientation.x = qx
pose_msg.pose.orientation.y = qy
pose_msg.pose.orientation.z = qz
pose_msg.pose.orientation.w = qw
self.vision_pose_pub.publish(pose_msg)
def run(self):
"""主循环"""
rate = rospy.Rate(50) # 50Hz
while not rospy.is_shutdown():
if self.offboard_mode:
# 在这里可以添加自定义的控制逻辑
pass
rate.sleep()
def main():
system = VisionFusionSystem()
# 启动视觉融合
if system.start_vision_fusion():
rospy.loginfo("视觉融合系统运行中...")
system.run()
else:
rospy.logerr("启动失败")
if __name__ == "__main__":
try:
main()
except rospy.ROSInterruptException:
pass5.4 使用pymavlink的直接连接方式
#!/usr/bin/env python3
# -*- coding: utf-8 -*-
"""
文件名: mavlink_param_config.py
功能: 通过MAVLink直接连接配置参数
"""
import time
from pymavlink import mavutil
class MavlinkParamConfig:
"""MAVLink参数配置类"""
def __init__(self, connection_string='udp:127.0.0.1:14550'):
"""
初始化MAVLink连接
Args:
connection_string: 连接字符串
- 'udp:127.0.0.1:14550' (MAVROS默认)
- 'udpin:localhost:14551' (QGC默认)
- '/dev/ttyUSB0' (串口)
"""
self.connection_string = connection_string
self.master = None
def connect(self):
"""建立连接"""
print(f"连接到: {self.connection_string}")
self.master = mavutil.mavlink_connection(self.connection_string)
# 等待心跳包
self.master.wait_heartbeat()
print(f"已连接到系统 {self.master.target_system}, 组件 {self.master.target_component}")
return True
def get_all_parameters(self):
"""获取所有参数"""
print("\n获取所有参数...")
# 请求所有参数
self.master.mav.param_request_list_send(
self.master.target_system,
self.master.target_component
)
params = {}
while True:
msg = self.master.recv_match(type='PARAM_VALUE', blocking=True, timeout=3)
if not msg:
break
param_id = msg.param_id.decode('utf-8').strip('\x00')
params[param_id] = msg.param_value
# 检查是否接收完
if msg.param_index == msg.param_count - 1:
break
return params
def set_parameter(self, param_id, value):
"""设置参数"""
print(f"设置参数 {param_id} = {value}")
# 发送参数设置命令
self.master.mav.param_set_send(
self.master.target_system,
self.master.target_component,
param_id.encode('utf-8'),
float(value),
mavutil.mavlink.MAV_PARAM_TYPE_REAL32
)
# 等待确认
msg = self.master.recv_match(type='PARAM_VALUE', blocking=True, timeout=3)
if msg and msg.param_id.decode('utf-8').strip('\x00') == param_id:
print(f"确认: {param_id} = {msg.param_value}")
return True
else:
print(f"设置 {param_id} 失败")
return False
def configure_vision_gps(self):
"""配置视觉+GPS定位"""
print("\n" + "="*50)
print("配置视觉+GPS融合定位")
print("="*50)
# 设置EKF2_AID_MASK
if self.set_parameter("EKF2_AID_MASK", 25):
print("✅ GPS+视觉位置+视觉偏航已启用")
# 可选：配置其他参数
self.set_parameter("EKF2_HGT_MODE", 2) # 使用视觉高度
self.set_parameter("EKF2_EV_DELAY", 0.0) # 视觉延迟补偿
print("\n配置完成！")
# 显示关键参数
params = self.get_all_parameters()
for key in ['EKF2_AID_MASK', 'EKF2_HGT_MODE', 'EKF2_EV_DELAY']:
if key in params:
print(f"{key}: {params[key]}")
def monitor_ekf_status(self, duration=10):
"""监控EKF状态"""
print(f"\n监控EKF状态 {duration} 秒...")
start_time = time.time()
while time.time() - start_time < duration:
msg = self.master.recv_match(type=['EKF_STATUS_REPORT', 'ATTITUDE'],
blocking=True, timeout=0.1)
if msg:
msg_type = msg.get_type()
if msg_type == 'EKF_STATUS_REPORT':
print(f"EKF状态: flags={msg.flags}, position_horiz_accuracy={msg.position_horiz_accuracy}")
elif msg_type == 'ATTITUDE':
print(f"姿态: roll={msg.roll:.2f}, pitch={msg.pitch:.2f}, yaw={msg.yaw:.2f}")
def main():
# 创建配置器
config = MavlinkParamConfig('udp:127.0.0.1:14550')
# 连接
if not config.connect():
print("连接失败")
return
# 配置参数
config.configure_vision_gps()
# 监控状态
config.monitor_ekf_status(5)
if __name__ == "__main__":
main()6. 实际应用案例
6.1 案例1：室内无人机视觉定位
场景描述：在室内环境中，使用Intel RealSense T265进行视觉定位
#!/usr/bin/env python3
"""
室内视觉定位完整示例
硬件: Intel RealSense T265 + PX4飞控
"""
import rospy
import pyrealsense2 as rs
import numpy as np
from geometry_msgs.msg import PoseStamped, TwistStamped
class IndoorVisionSystem:
def __init__(self):
rospy.init_node('indoor_vision_system')
# 初始化RealSense
self.pipeline = rs.pipeline()
config = rs.config()
config.enable_stream(rs.stream.pose)
self.pipeline.start(config)
# MAVROS发布者
self.vision_pub = rospy.Publisher('/mavros/vision_pose/pose',
PoseStamped, queue_size=10)
# 配置参数服务
self.configure_ekf()
def configure_ekf(self):
"""配置EKF参数"""
rospy.wait_for_service('/mavros/param/set')
from mavros_msgs.srv import ParamSet
set_param = rospy.ServiceProxy('/mavros/param/set', ParamSet)
# 设置为纯视觉定位模式
req = ParamSet()
req.param_id = "EKF2_AID_MASK"
req.value.integer = 24 # 纯视觉模式
set_param(req)
rospy.loginfo("EKF2配置完成: 纯视觉定位模式")
def run(self):
"""主循环"""
rate = rospy.Rate(30) # 30Hz
while not rospy.is_shutdown():
# 获取T265数据
frames = self.pipeline.wait_for_frames()
pose_frame = frames.get_pose_frame()
if pose_frame:
# 转换数据
pose_msg = PoseStamped()
pose_msg.header.stamp = rospy.Time.now()
pose_msg.header.frame_id = "vision"
# 提取位置和姿态
data = pose_frame.get_pose_data()
pose_msg.pose.position.x = data.translation.x
pose_msg.pose.position.y = data.translation.y
pose_msg.pose.position.z = data.translation.z
pose_msg.pose.orientation.x = data.rotation.x
pose_msg.pose.orientation.y = data.rotation.y
pose_msg.pose.orientation.z = data.rotation.z
pose_msg.pose.orientation.w = data.rotation.w
# 发布到MAVROS
self.vision_pub.publish(pose_msg)
rospy.loginfo(f"位置: ({data.translation.x:.2f}, {data.translation.y:.2f}, {data.translation.z:.2f})")
rate.sleep()
if __name__ == "__main__":
system = IndoorVisionSystem()
system.run()6.2 案例2：室外GPS+视觉融合
#!/usr/bin/env python3
"""
室外GPS+视觉融合示例
场景: 桥梁检测，GPS信号可能受干扰
"""
import rospy
import message_filters
from sensor_msgs.msg import NavSatFix
from geometry_msgs.msg import PoseStamped
from mavros_msgs.msg import GlobalPositionTarget
class GPSVisionFusion:
def __init__(self):
rospy.init_node('gps_vision_fusion')
# 订阅GPS和视觉数据
self.gps_sub = message_filters.Subscriber('/mavros/global_position/global', NavSatFix)
self.vision_sub = message_filters.Subscriber('/vision/pose', PoseStamped)
# 时间同步
self.ts = message_filters.ApproximateTimeSynchronizer(
[self.gps_sub, self.vision_sub], 10, 0.1)
self.ts.registerCallback(self.fusion_callback)
# 配置参数
self.configure_fusion()
# 状态变量
self.gps_quality = 0
self.vision_quality = 0
def configure_fusion(self):
"""配置融合参数"""
rospy.wait_for_service('/mavros/param/set')
from mavros_msgs.srv import ParamSet
set_param = rospy.ServiceProxy('/mavros/param/set', ParamSet)
# 动态融合模式
req = ParamSet()
req.param_id = "EKF2_AID_MASK"
req.value.integer = 25 # GPS + 视觉完整融合
set_param(req)
rospy.loginfo("GPS+视觉融合模式已启用")
def fusion_callback(self, gps_msg, vision_msg):
"""融合回调"""
# 评估GPS质量
if gps_msg.status.status > 0:
self.gps_quality = gps_msg.status.status
else:
self.gps_quality = 0
# 评估视觉质量
# 可以根据协方差等信息评估
self.vision_quality = 1 if vision_msg.pose.position.z > 0 else 0
# 根据质量动态调整权重
if self.gps_quality < 2 and self.vision_quality > 0:
rospy.logwarn("GPS信号差，依赖视觉定位")
# 可以动态调整EKF参数
self.adjust_ekf_weights()
def adjust_ekf_weights(self):
"""动态调整EKF权重"""
# 实现动态调整逻辑
pass
if __name__ == "__main__":
fusion = GPSVisionFusion()
rospy.spin()7. 故障排除
7.1 常见问题及解决方案
问题1：参数设置失败
# 错误示例
rospy.wait_for_service('/mavros/param/set') # 可能超时
# 解决方案：增加超时时间和重试机制
def set_param_with_retry(param_id, value, retries=3):
for i in range(retries):
try:
rospy.wait_for_service('/mavros/param/set', timeout=5)
# 设置参数
return True
except rospy.ROSException:
rospy.logwarn(f"重试 {i+1}/{retries}")
rospy.sleep(1)
return False问题2：视觉数据未生效
检查清单：
# 1. 检查主题是否发布
rostopic echo /mavros/vision_pose/pose
# 2. 检查EKF参数
rosrun mavros mavparam get EKF2_AID_MASK
# 3. 检查EKF状态
rostopic echo /mavros/ekf/status
# 4. 检查时间戳同步
roswtf | grep -i time问题3：MAVROS连接失败
# 诊断脚本
import rospy
from mavros_msgs.msg import State
def check_mavros():
rospy.init_node('mavros_check')
try:
state = rospy.wait_for_message('/mavros/state', State, timeout=10)
if state.connected:
rospy.loginfo("✅ MAVROS连接正常")
rospy.loginfo(f" 模式: {state.mode}")
rospy.loginfo(f" 解锁: {state.armed}")
else:
rospy.logerr("❌ MAVROS未连接")
except:
rospy.logerr("❌ 无法接收MAVROS消息")
if __name__ == "__main__":
check_mavros()7.2 性能优化建议
-
降低视觉数据频率
# 视觉数据不需要太高频率，20-30Hz足够
rate = rospy.Rate(20)-
使用消息过滤器
# 避免过载
from message_filters import TimeSynchronizer, Subscriber-
监控延迟
def check_latency():
current_time = rospy.Time.now()
msg_time = pose_msg.header.stamp
latency = (current_time - msg_time).to_sec()
if latency > 0.1:
rospy.logwarn(f"高延迟: {latency:.3f}s")7.3 调试工具
# 1. 可视化参数
rqt_reconfigure
# 2. 绘制数据曲线
rqt_plot /mavros/vision_pose/pose/position/x
# 3. 查看参数变化
rosrun rqt_param rqt_param
# 4. 记录数据包
rosbag record -O vision_test /mavros/vision_pose/pose /mavros/global_position/global8. 总结与展望
8.1 关键要点回顾
-
核心参数：
EKF2_AID_MASK（参数ID 26）是配置定位源的关键 -
推荐配置：GPS+视觉位置+视觉偏航（掩码值25）
-
代码实现：提供了从基础到完整的三种实现方案
-
故障排除：系统性解决常见问题的方法
8.2 最佳实践
# 推荐的初始化流程
def recommended_setup():
"""
推荐的初始化步骤：
1. 等待MAVROS连接
2. 备份当前参数
3. 设置新参数
4. 验证配置
5. 监控状态
"""
pass8.3 进阶资源
-
PX4官方文档：https://docs.px4.io/
-
MAVROS Wiki：http://wiki.ros.org/mavros
-
EKF2详解：https://docs.px4.io/master/en/advanced_config/tuning_the_ecl_ekf.html
8.4 未来展望
-
自适应融合：根据环境自动切换定位源
-
多传感器融合：视觉+激光雷达+UWB
-
边缘计算：在机载计算机上实时优化
附录
A.常用命令速查表
| 命令 | 说明 |
|---|---|
rostopic list | grep mavros | 查看MAVROS主题 |
rosrun mavros mavparam get EKF2_AID_MASK | 获取参数值 |
rosservice call /mavros/set_message_interval | 设置消息频率 |
rostopic echo /mavros/state | 查看飞控状态 |
B. 参数参考表
| 参数名称 | 默认值 | 说明 |
|---|---|---|
| EKF2_AID_MASK | 1 | 定位源掩码 |
| EKF2_HGT_MODE | 1 | 高度模式（0:气压,1:GPS,2:视觉） |
| EKF2_EV_DELAY | 0.0 | 视觉数据延迟（秒） |
| EKF2_GPS_DELAY | 0.0 | GPS数据延迟（秒） |
结语
本文详细介绍了PX4飞控定位源的配置方法，特别是如何通过Python代码实现GPS+视觉的融合定位。通过提供的代码示例和故障排除指南，读者应该能够快速搭建自己的视觉定位系统。
记住，配置定位源只是第一步，真正的挑战在于如何根据具体应用场景优化参数。建议在实际飞行前进行充分的仿真和地面测试。
祝您飞行顺利！🚁
有“AI”的1024 = 2048，欢迎大家加入2048 AI社区
更多推荐
20
0- 0
已为社区贡献4条内容
所有评论(0)