PX4 固件版本为 1.15.4
QGC 地面站版本为 4.4.5
飞控：微空科技 MicoAir743V2
机载电脑：12 代 i5, Ubuntu 20.04
安装位置：MID360 的接口对应飞机的后方
一、PX4 飞控设置
1. 将 PX4 定位数据源设置为 Vision
参数 EKF2_EV_CTRL：可以默认，或者看参数手册按需配置（该教程默认）。
参数 EKF2_HGT_REF：Vision
2. 关闭罗盘
需要注意的是，你现在走完上面飞控的设置，机头上电后会指向北。但在走完下面的程序后，你会发现上电后无人机机头指向东方（罗盘已经被关了）。
此外，加一个激光测距融合好处多多。
二、程序
注意，下面我把雷达等频率改到了 30HZ，但是在这频率下，定位精度实测只能有 10hz 的 60% 不到，慎重选择是否改频率！！！！！
1. 代码
接下来创建发布 /mavros/vision_pose/pose 话题的功能包过程了。这里只提供代码，具体创建工作空间和功能包的步骤建议查阅官方文档或相关教程。主要功能：将转化后的位姿信息以话题 /mavros/vision_pose/pose 发布。
#!/usr/bin/env python3
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 - .data_queue[-]) > :
.data_queue.clear()
.window_sum =
.data_queue.append(new_data)
.window_sum += new_data
(.data_queue) > .window_size:
.window_sum -= .data_queue.popleft()
.window_sum / (.data_queue)
():
(.data_queue)
():
.data_queue:
.window_sum / (.data_queue)
:
:
():
rospy.init_node(, anonymous=)
.p_lidar_body = np.zeros()
.q_mav = [, , , ]
.q_px4_odom = [, , , ]
.window_size =
.swa = SlidingWindowAverage(.window_size)
.init_flag =
.init_q = tf.transformations.quaternion_from_euler(, , )
rospy.Subscriber(, Odometry, .vins_callback)
rospy.Subscriber(, Odometry, .px4_odom_callback)
.vision_pub = rospy.Publisher(, PoseStamped, queue_size=)
.rate = rospy.Rate()
.run()
():
euler = tf.transformations.euler_from_quaternion(q)
euler[]
():
.p_lidar_body = np.array([
msg.pose.pose.position.x,
msg.pose.pose.position.y,
msg.pose.pose.position.z
])
.q_mav = [
msg.pose.pose.orientation.x,
msg.pose.pose.orientation.y,
msg.pose.pose.orientation.z,
msg.pose.pose.orientation.w
]
():
.q_px4_odom = [
msg.pose.pose.orientation.x,
msg.pose.pose.orientation.y,
msg.pose.pose.orientation.z,
msg.pose.pose.orientation.w
]
yaw = .from_quaternion_to_yaw(.q_px4_odom)
.swa.add_data(yaw)
():
rospy.is_shutdown():
.swa.get_size() == .window_size .init_flag:
init_yaw = .swa.get_avg()
.init_q = tf.transformations.quaternion_from_euler(, , init_yaw)
.init_flag =
.init_flag:
rot_matrix = tf.transformations.quaternion_matrix(.init_q)[:, :]
p_enu = np.dot(rot_matrix, .p_lidar_body)
vision = PoseStamped()
vision.header.stamp = rospy.Time.now()
vision.header.frame_id =
vision.pose.position.x = p_enu[]
vision.pose.position.y = p_enu[]
vision.pose.position.z = p_enu[]
vision.pose.orientation.x = .q_mav[]
vision.pose.orientation.y = .q_mav[]
vision.pose.orientation.z = .q_mav[]
vision.pose.orientation.w = .q_mav[]
.vision_pub.publish(vision)
rospy.loginfo(.(
p_enu[], p_enu[], p_enu[], .q_mav[], .q_mav[], .q_mav[], .q_mav[]))
.rate.sleep()
__name__ == :
:
FastLIOToMavros()
rospy.ROSInterruptException:
