PX4 结合 Mid360 与 Fast-LIO 实现无人机室内定点悬停
本文介绍基于 PX4 固件、Mid360 激光雷达及 Fast-LIO 算法，在无光流无 GPS 环境下实现无人机室内定位与定点悬停的配置流程。
硬件与环境配置
- 飞控：Pixhawk 6cmini (PX4 固件 1.15.4)
- 机载电脑：Jetson Orin Nano
- 激光雷达：大疆 Mid360
- 通信：通过 MAVROS 将雷达定位信息转换为 ENU 坐标系后发布给 PX4
- 远程控制：使用 Nomachine，需确保网络环境稳定以避免连接卡顿或掉线。部分情况下需等待特定状态指示后方可远程操控。
环境搭建与启动
1. 启动激光雷达驱动
roslaunch livox_ros_driver2 msg_MID360.launch
2. 启动 Fast-LIO 算法
roslaunch fast_lio mapping_mid360.launch
此时可获取当前无人机的里程计（odometry）信息。
3. 配置 PX4 串口权限
根据连线接口不同赋予串口权限，可使用以下命令一键赋予所有串口权限：
sudo chmod 777 /dev/tty*
4. 启动 MAVROS
roslaunch mavros px4.launch
坐标转换与节点开发
将雷达位置信息转换为 ENU 坐标系后发送给 PX4 无人机。需创建工作空间和相应节点，运行对应节点程序。
rosrun <package_name> <node_name>
通过 /mavros/vision_pose/pose 话题发布给 PX4，PX4 获取到位置后可在位置模式下解锁。若未解锁，请确认话题中是否有数据存在（使用 rostopic list 查看）。如仍无法解锁，可能缺少 mavros_extras 功能包，可尝试安装：
sudo apt-get install ros-melodic-mavros-extras
QGC 参数配置
在 QGC 中关闭罗盘信息以匹配 MAVROS 发布的坐标系。主要修改参数 EKF2_HGT_REF 为 Vision，即从上述节点程序中获取当前位置信息。
确定无人机机头所指方向为 X 轴正方向。即飞机朝机头方向运动时，发布的 /mavros/vision_pose/pose 中的 x 会增大；向上 z 数据会增大。若不一致（例如相反），起飞后可能出现漂移画圈现象，仅可实现定高效果，无法矫正回正确位置。
起飞控制节点代码
以下为 Python 编写的起飞节点示例，飞机将静止在 (0, 0, 0.5) 米处。
#! /usr/bin/env python3
import rospy
from geometry_msgs.msg import PoseStamped
mavros_msgs.msg State
mavros_msgs.srv CommandBool, CommandBoolRequest, SetMode, SetModeRequest
current_state = State()
():
current_state
current_state = msg
__name__ == :
rospy.init_node()
state_sub = rospy.Subscriber(, State, callback=state_cb)
local_pos_pub = rospy.Publisher(, PoseStamped, queue_size=)
rospy.wait_for_service()
arming_client = rospy.ServiceProxy(, CommandBool)
rospy.wait_for_service()
set_mode_client = rospy.ServiceProxy(, SetMode)
rate = rospy.Rate()
rospy.is_shutdown() current_state.connected:
rate.sleep()
pose = PoseStamped()
pose.pose.position.x =
pose.pose.position.y =
pose.pose.position.z =
i ():
rospy.is_shutdown():
local_pos_pub.publish(pose)
rate.sleep()
offb_set_mode = SetModeRequest()
offb_set_mode.custom_mode =
arm_cmd = CommandBoolRequest()
arm_cmd.value =
last_req = rospy.Time.now()
rospy.is_shutdown():
current_state.mode != (rospy.Time.now() - last_req) > rospy.Duration():
set_mode_client.call(offb_set_mode).mode_sent == :
rospy.loginfo()
last_req = rospy.Time.now()
:
current_state.armed (rospy.Time.now() - last_req) > rospy.Duration():
arming_client.call(arm_cmd).success == :
rospy.loginfo()
last_req = rospy.Time.now()
local_pos_pub.publish(pose)
rate.sleep()
