基于 PX4 与 Mid360 激光雷达的无人机 FAST-LIO 定点悬停

硬件与软件环境

• 无人机固件: PX4 1.15.4 (Pixhawk 6C Mini)
• 机载电脑: Jetson Orin Nano
• 激光雷达: DJI Mid360
• 算法: FAST-LIO
• 通信: MAVROS
• 远程连接: 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*

启动 MAVROS 节点：

roslaunch mavros px4.launch

坐标转换与发布

将雷达位置信息转换为 ENU 坐标系后发送给 PX4。通过 /mavros/vision_pose/pose 话题发布给 PX4，PX4 获取到位置后可在位置模式下解锁。若未解锁，请确认话题中是否有数据存在（使用 rostopic list 查看），并检查是否缺失 mavros_extras 功能包。

安装 mavros_extras 功能包：

sudo apt-get install ros-melodic-mavros-extras

QGC 参数配置

在 QGC 中关闭罗盘信息以对应 MAVROS 发布的坐标系。主要修改参数 EKF2_HGT_REF 为 Vision，即从上述节点程序中获取当前的高度信息。

确定无人机机头所指的方向为 X 轴正方向。飞机朝机头方向运动时，发布的 /mavros/vision_pose/pose 中的 x 应增大；向上 z 数据应增大。如果不一致，起飞后可能出现漂移或画圈现象，仅可实现定高效果。

OFFBOARD 模式控制代码

创建工作空间及相应节点，运行以下 Python 脚本实现起飞与悬停：

#!/usr/bin/env python3
import rospy
from geometry_msgs.msg import PoseStamped
from mavros_msgs.msg import State
from mavros_msgs.srv import CommandBool, SetMode

current_state = State()

def ():
     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 = SetMode()
    offb_set_mode.custom_mode = 
    arm_cmd = CommandBool()
    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()