基于 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 state_cb(msg):
    global current_state
    current_state = msg

if __name__ == "__main__":
    rospy.init_node("offb_node_py")
    state_sub = rospy.Subscriber("mavros/state", State, callback=state_cb)
    local_pos_pub = rospy.Publisher("mavros/setpoint_position/local", PoseStamped, queue_size=10)
    
    rospy.wait_for_service("/mavros/cmd/arming")
    arming_client = rospy.ServiceProxy("/mavros/cmd/arming", CommandBool)
    rospy.wait_for_service("/mavros/set_mode")
    set_mode_client = rospy.ServiceProxy("/mavros/set_mode", SetMode)
    
    rate = rospy.Rate(20)
    while not rospy.is_shutdown() and not current_state.connected:
        rate.sleep()
    
    pose = PoseStamped()
    pose.pose.position.x = 0
    pose.pose.position.y = 0
    pose.pose.position.z = 0.5
    
    for i in range(100):
        if rospy.is_shutdown():
            break
        local_pos_pub.publish(pose)
        rate.sleep()
    
    offb_set_mode = SetMode()
    offb_set_mode.custom_mode = 'OFFBOARD'
    arm_cmd = CommandBool()
    arm_cmd.value = True
    last_req = rospy.Time.now()
    
    while not rospy.is_shutdown():
        if current_state.mode != "OFFBOARD" and (rospy.Time.now() - last_req) > rospy.Duration(5.0):
            if set_mode_client.call(offb_set_mode).mode_sent:
                rospy.loginfo("OFFBOARD enabled")
                last_req = rospy.Time.now()
        else:
            if not current_state.armed and (rospy.Time.now() - last_req) > rospy.Duration(5.0):
                if arming_client.call(arm_cmd).success:
                    rospy.loginfo("Vehicle armed")
                    last_req = rospy.Time.now()
        local_pos_pub.publish(pose)
        rate.sleep()

问题排查与注意事项

1. 桨叶干扰: 实物飞行中可能遇到雷达定位漂移问题。大桨叶旋转或小桨叶上安装可能影响雷达定位（气流或检测距离因素）。建议更换小桨叶并下置安装以解决干扰。
2. 日志分析: 飞行中出现异常可通过 QGC 下载日志进行数据分析，参考官方日志解析平台。
3. 网络延迟: 远程控制建议使用千兆路由器，较差的网络环境可能导致连接卡顿或掉线。