中国网站建设市场规模,石家庄装修公司排名前十强,磁力搜索引擎不死鸟,wordpress 占内存PX4固件版本为1.15.4
qgc地面站版本为4.4.5
飞控#xff0c;使用微空科技MicoAir743V2
机载电脑#xff1a;12代i5,ubuntu20.04
安装位置#xff1a;mid360的接口对应飞机的后方 推荐阅读px4vio实现无人机室内定位_px4室内视觉定位-CSDN博客 和飞控连接机载电脑相关使用微空科技MicoAir743V2
机载电脑12代i5,ubuntu20.04
安装位置mid360的接口对应飞机的后方 推荐阅读px4vio实现无人机室内定位_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.关闭罗盘 教程 PX4 | 无人机关闭磁力计罗盘飞行yaw estimate error报错解决方法-CSDN博客 需要注意的是你现在走完上面飞控的设置机头上电后会指向北。但在走完下面的程序后你会发现上电后无人机机头指向东方罗盘已经被关了如下图所示 此外加一个激光测距融合好处多多 二、程序 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_sizeself.data_queue deque()self.window_sum 0.0def 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.0self.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.0class FastLIOToMavros:def __init__(self):rospy.init_node(fastlio_to_mavros, anonymousTrue)# 初始化位姿和四元数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 8self.swa SlidingWindowAverage(self.window_size)self.init_flag Falseself.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)# 发布视觉位姿数据到 PX4self.vision_pub rospy.Publisher(/mavros/vision_pose/pose, PoseStamped, queue_size10)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 Trueif 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的程序 建议参考MID360fastlio功能笔记-CSDN博客 需要强调的是安装官方说法 修改livox_ros_driver2 msg_MID360.launch 使其频率达到30HZ其实默认也能用方法在livox_ros_driver2的github底下写了。 输入下图指令查看是否修改了 roslaunch livox_ros_driver2 msg_MID360.launch
在另一个终端中执行
roslaunch fast_lio mapping_mid360.launch
3.运行mavros 安装步骤参考mavros安装——解决疑难杂症- PX4无人机配置流程(三)-CSDN博客 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变成负数并且不断减小 实机建议调好pid再起飞本人只试过定点飞行没有任何问题
