PX4固件版本为1.15.4
qgc地面站版本为4.4.5
飞控，使用微空科技MicoAir743V2
机载电脑：12代i5,ubuntu20.04
安装位置：mid360的接口对应飞机的后方
推荐阅读px4+vio实现无人机室内定位_px4+室内视觉定位-CSDN博客
和飞控连接机载电脑相关，有用
代码参考：
PX4|基于FAST-LIO mid360的无人机室内自主定位及定点悬停_fastlio mid360-CSDN博客
使用视觉或动作捕捉系统进行位置估计 | 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.关闭罗盘：
需要注意的是，你现在走完上面飞控的设置，机头上电后会指向北。但在走完下面的程序后，你会发现上电后无人机机头指向东方（罗盘已经被关了）如下图所示
此外，加一个激光测距融合好处多多
二、程序
注意，下面我把雷达等频率改到了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的程序
需要强调的是，安装官方说法 修改livox_ros_driver2 msg_MID360.launch ，使其频率达到30HZ（其实默认也能用）方法在livox_ros_driver2的github底下写了。
输入下图指令查看是否修改了
roslaunch livox_ros_driver2 msg_MID360.launch
在另一个终端中执行
roslaunch fast_lio mapping_mid360.launch3.运行mavros
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变成负数并且不断减小
