PythonAI算法
PX4 无人机 MID360 结合 FAST_LIO 实现室内自主定位与定点
介绍基于 PX4 固件与 MID360 激光雷达，利用 FAST_LIO 算法实现无人机室内自主定位与定点悬停的配置流程。主要步骤包括：设置 PX4 飞控参数将定位源改为视觉模式并关闭磁力计；编写 Python 节点将 Fast-LIO 位姿发布至 MAVROS 话题；调整雷达扫描频率；运行 Mavros 及定位程序。最后通过 QGC 验证坐标系方向与运动数据一致性。
介绍基于 PX4 固件与 MID360 激光雷达，利用 FAST_LIO 算法实现无人机室内自主定位与定点悬停的配置流程。主要步骤包括：设置 PX4 飞控参数将定位源改为视觉模式并关闭磁力计；编写 Python 节点将 Fast-LIO 位姿发布至 MAVROS 话题；调整雷达扫描频率；运行 Mavros 及定位程序。最后通过 QGC 验证坐标系方向与运动数据一致性。
建议参考官方文档：Using Vision or Motion Capture Systems for Position Estimation | PX4 Guide
EKF2_EV_CTRL：可保持默认或按需配置。EKF2_HGT_REF：设置为 Vision。注意： 完成上述飞控设置后，机头上电后会指向北。但在后续程序运行后，由于罗盘已关闭，上电后无人机机头可能指向东方。
此外，加一个激光测距融合好处多多。
注意： 雷达频率可调至 30Hz，但实测定位精度在 30Hz 下仅为 10Hz 的 60% 不到，请慎重选择频率。
创建发布 /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
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)
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)
rospy.Subscriber('/Odometry', Odometry, self.vins_callback)
rospy.Subscriber('/mavros/local_position/odom', Odometry, self.px4_odom_callback)
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):
euler = tf.transformations.euler_from_quaternion(q)
return euler[2]
def vins_callback(self, msg):
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):
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():
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:
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 文件。
修改 livox_ros_driver2 中的 msg_MID360.launch，使其频率达到 30Hz（默认也能用）。
roslaunch livox_ros_driver2 msg_MID360.launch
# 在另一个终端中执行
roslaunch fast_lio mapping_mid360.launch
roslaunch mavros px4.launch
必须运行上述指令后马上运行下面的 fastlio_to_mavros。
运行自己创建的 fastlio_to_mavros 节点的 launch 文件。
推荐用官方的仿真来理解坐标系。
你会发现一运行 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 后再起飞，本文已验证定点飞行功能。
微信公众号「极客日志」，在微信中扫描左侧二维码关注。展示文案：极客日志 zeeklog
使用加密算法（如AES、TripleDES、Rabbit或RC4）加密和解密文本明文。 在线工具，加密/解密文本在线工具，online
生成新的随机RSA私钥和公钥pem证书。 在线工具，RSA密钥对生成器在线工具，online
基于 Mermaid.js 实时预览流程图、时序图等图表，支持源码编辑与即时渲染。 在线工具，Mermaid 预览与可视化编辑在线工具，online
解析常见 curl 参数并生成 fetch、axios、PHP curl 或 Python requests 示例代码。 在线工具，curl 转代码在线工具，online
将字符串编码和解码为其 Base64 格式表示形式即可。 在线工具，Base64 字符串编码/解码在线工具，online
将字符串、文件或图像转换为其 Base64 表示形式。 在线工具，Base64 文件转换器在线工具，online