Jetson Orin NX 搭载 Fast-LIO2 的自主无人机系统部署实战

三、软件架构

3.1 系统三层架构

graph TB
subgraph Decision["🎯 Decision & Planning Layer"]
PathPlanning["📍 Path Planning<br/>A* / RRT / Dijkstra"]
LocalAvoid["🚧 Obstacle Avoidance<br/>Potential Field"]
end
subgraph Navigation["🧭 Navigation & Control Layer"]
SLAM["🗺️ SLAM Fast-LIO2<br/>Localization & Mapping<br/>100Hz"]
Controller["⚙️ PID Controller<br/>Trajectory Tracking"]
end
subgraph Hardware["🔧 Hardware Interface Layer"]
PX4Driver["🛩️ PX4 (MAVLink)<br/>Firmware Interface"]
LivoxDriver["📡 Livox Driver<br/>MID360 SDK"]
end
Decision --> Navigation
Navigation --> Hardware
SLAM -.->|Odometry| PathPlanning
SLAM -.->|Odometry| LocalAvoid
PathPlanning -.->|Global Path| Controller
LocalAvoid -.->|Avoid Velocity| Controller
Controller -.->|Control Command| PX4Driver
style Decision fill:#fff9c4
style Navigation fill:#c8e6c9
style Hardware fill:#bbdefb

3.2 数据流处理

Feedback 🔴 MID360 (100Hz) 📡 Livox Driver ☁️ Point Cloud (/livox/lidar) 🗺️ Fast-LIO2 (100Hz) 📍 Odometry (/fastlio2/odometry) 📍 Path Planning (1-5Hz) 🛣️ Global Path 🚧 Local Planner (20Hz) 👁️ Obstacle Detect ⬆️ Avoid Velocity 🎛️ Decision Fusion ➡️ Final Command 🎬 RViz Visualize

四、环境配置

4.1 Jetson Orin NX 初始化

Orin NX 内存有限，增加交换空间是编译大型项目的必要步骤。

# 1. 系统更新
sudo apt update && sudo apt upgrade -y

# 2. 安装基础开发工具
sudo apt install -y build-essential cmake git curl wget \
python3-dev python3-pip libssl-dev libopencv-dev

# 3. 增加交换空间（重要！）
sudo fallocate -l 12G /swapfile
sudo chmod 600 /swapfile
sudo mkswap /swapfile
sudo swapon /swapfile
echo '/swapfile none swap sw 0 0' | sudo tee -a /etc/fstab

# 4. 验证 CUDA
nvcc --version
nvidia-smi

4.2 ROS 2 Humble 安装

# 1. 添加 ROS2 源
sudo curl -sSL https://raw.githubusercontent.com/ros/rosdistro/master/ros.key | \
sudo apt-key add -
echo "deb [arch=$(dpkg --print-architecture)] http://packages.ros.org/ros2/ubuntu $(lsb_release -cs) main" | \
sudo tee /etc/apt/sources.list.d/ros2-latest.list > /dev/null

# 2. 安装 ROS 2 Humble
sudo apt update
sudo apt install -y ros-humble-desktop

# 3. 配置环境变量
echo "source /opt/ros/humble/setup.bash" >> ~/.bashrc
source ~/.bashrc

# 4. 验证安装
ros2 --version

4.3 核心依赖库安装

# PCL - 点云库
sudo apt install -y libpcl-dev pcl-tools ros-humble-pcl-ros

# Eigen - 线性代数
sudo apt install -y libeigen3-dev

# OpenCV - 计算机视觉
sudo apt install -y libopencv-dev opencv-data

# YAML - 配置文件处理
sudo apt install -y libyaml-cpp-dev

# Ceres Solver - 优化库（可选但推荐）
cd ~/workspace
git clone https://github.com/ceres-solver/ceres-solver.git
cd ceres-solver && mkdir build && cd build
cmake .. -DBUILD_TESTING=OFF -DBUILD_EXAMPLES=OFF
make -j$(nproc) && sudo make install

4.4 创建 ROS2 工作空间

# 创建工作空间
mkdir -p ~/uav_ws/src
cd ~/uav_ws

# 初始化工作空间
colcon build --symlink-install

# 配置环境
echo "source ~/uav_ws/install/setup.bash" >> ~/.bashrc
source ~/.bashrc

五、关键模块部署

5.1 Fast-LIO2 SLAM 部署

5.1.1 源码编译

cd ~/uav_ws/src
git clone https://github.com/hku-mars/Fast-LIO2.git

# 编译 Fast-LIO2
cd ~/uav_ws
colcon build --packages-select fast_lio2 \
--cmake-args -DCMAKE_BUILD_TYPE=Release

# 验证
ros2 pkg list | grep fast_lio2

5.1.2 MID360 配置文件

创建 ~/.ros/fast_lio2_mid360.yaml:

common:
  lidar_type: 1 # 1=Livox
  time_sync_en: false
  imu_en: true
  scan_pub_en: true
lidar:
  lidar_cov: 0.03 # 激光雷达噪声
  blind: 0.5 # 盲点距离 (m)
imu:
  gyr_cov: 0.1 # 陀螺仪噪声
  acc_cov: 0.1 # 加速度计噪声
  b_gyr_cov: 0.0001 # 偏差协方差
  b_acc_cov: 0.0001
mapping:
  res_mean_size: 0.05 # 体素大小
  cube_len: 200 # 地图范围
  max_cov: 100.0
  min_cov: 0.001

5.1.3 启动命令

# 启动 MID360 驱动
ros2 launch livox_ros2_driver livox_ros2_driver.launch.py

# 启动 Fast-LIO2（新终端）
ros2 run fast_lio2 fastlio2_node --ros-args \
-p config_file:=~/.ros/fast_lio2_mid360.yaml

# 验证输出（第三个终端）
ros2 topic hz /fastlio2/odometry
# 预期：100Hz

5.2 路径规划模块

5.2.1 A*算法实现核心

这里我们采用 C++ 实现核心逻辑，确保计算效率。

// src/path_planner.cpp
#include<rclcpp/rclcpp.hpp>
#include<nav_msgs/msg/path.hpp>
#include<sensor_msgs/msg/point_cloud2.hpp>
#include<queue>
#include<map>

class PathPlanner : public rclcpp::Node {
public:
  PathPlanner() : Node("path_planner") {
    // 初始化订阅和发布
    cloud_sub_ = this->create_subscription<sensor_msgs::msg::PointCloud2>(
      "/fastlio2/cloud_registered", 10, 
      std::bind(&PathPlanner::cloud_callback, this, std::placeholders::_1));
    path_pub_ = this->create_publisher<nav_msgs::msg::Path>("/planned_path", 10);
  }

  nav_msgs::msg::Path plan_path(const geometry_msgs::msg::Point& start,
                                const geometry_msgs::msg::Point& goal) {
    // A*算法实现
    std::priority_queue<Node> open_set;
    std::map<std::string, Node> closed_set;
    // ... 完整的 A*实现 ...
    return path;
  }

private:
  rclcpp::Subscription<sensor_msgs::msg::PointCloud2>::SharedPtr cloud_sub_;
  rclcpp::Publisher<nav_msgs::msg::Path>::SharedPtr path_pub_;

  void cloud_callback(const sensor_msgs::msg::PointCloud2::SharedPtr msg) {
    // 更新占用网格
  }
};

5.2.2 启动配置

# launch/path_planning.launch.py
import os
from launch import LaunchDescription
from launch_ros.actions import Node

def generate_launch_description():
  return LaunchDescription([
    Node(
      package='uav_autonomous_flight',
      executable='path_planner',
      name='path_planner',
      parameters=[{
        'planning_rate': 1.0,
        'goal_tolerance': 0.1,
        'grid_resolution': 0.2,
      }],
      output='screen'),
  ])

5.3 避障模块

5.3.1 势场法避障

// src/local_planner.cpp - 关键部分
void LocalPlanner::compute_avoidance_velocity() {
  // 1. 提取障碍物
  std::vector<geometry_msgs::msg::Point> obstacles = extract_obstacles(latest_cloud_);
  
  // 2. 计算排斥力
  double force_x = 0.0, force_y = 0.0;
  for(const auto& obs : obstacles) {
    double dist = std::hypot(obs.x, obs.y);
    if(dist < COLLISION_THRESHOLD) {
      double repulsion = (COLLISION_THRESHOLD - dist) / COLLISION_THRESHOLD;
      force_x -= repulsion * std::cos(std::atan2(obs.y, obs.x));
      force_y -= repulsion * std::sin(std::atan2(obs.y, obs.x));
    }
  }
  
  // 3. 输出避障速度
  geometry_msgs::msg::Twist cmd_vel;
  cmd_vel.linear.x = force_x;
  cmd_vel.linear.y = force_y;
  cmd_vel_pub_->publish(cmd_vel);
}

六、系统集成

6.1 完整启动脚本

为了方便管理，我们将所有模块封装在一个 Shell 脚本中自动拉起。

#!/bin/bash
# launch_autonomous_flight.sh
source ~/uav_ws/install/setup.bash
echo "Starting UAV Autonomous Flight System..."

# 终端 1: MID360 驱动
gnome-terminal -- bash -c "ros2 launch livox_ros2_driver livox_ros2_driver.launch.py; bash" &
sleep 2

# 终端 2: Fast-LIO2
gnome-terminal -- bash -c "ros2 launch fast_lio2 mid360_config.launch.py; bash" &
sleep 3

# 终端 3: MAVROS (PX4 通信)
gnome-terminal -- bash -c "ros2 launch mavros apm.launch fcu_url:=/dev/ttyTHS1:921600; bash" &
sleep 2

# 终端 4: 路径规划和避障
gnome-terminal -- bash -c "ros2 launch uav_autonomous_flight autonomous_flight.launch.py; bash" &

# 终端 5: RViz 可视化
gnome-terminal -- bash -c "rviz2 -d $(ros2 pkg prefix uav_autonomous_flight)/rviz/config.rviz; bash" &

echo "All modules started!"

6.2 系统健康检查

运行前建议先执行一次健康检查，确保各节点状态正常。

#!/bin/bash
# check_system.sh
echo "=== UAV System Health Check ==="

# 检查节点
echo "[1] Active Nodes:"
ros2 node list | tail -10

# 检查话题
echo "[2] Key Topics:"
echo " - SLAM: $(ros2 topic hz /fastlio2/odometry 2>/dev/null | head -1)"
echo " - LiDAR: $(ros2 topic hz /livox/lidar 2>/dev/null | head -1)"

# 检查系统资源
echo "[3] System Resources:"
free -h | grep Mem
nvidia-smi --query-gpu=memory.used,memory.total --format=csv,nounits

# 检查 PX4 状态
echo "[4] PX4 Status:"
ros2 topic echo /mavros/state -n 1 | grep mode

echo "=== Check Complete ==="

七、常见问题

问题 1: Fast-LIO2 运行缓慢（频率不稳定）

症状: ros2 topic hz /fastlio2/odometry 显示频率波动大

原因:

• Orin NX CPU 负载过高
• ROS 2 中间件配置不优化

解决方案:

# 1. 启用最高性能模式
sudo nvpmodel -m 0
sudo jetson_clocks

# 2. 使用高效的 DDS 实现
export RMW_IMPLEMENTATION=rmw_cyclonedds_cpp

# 3. 关闭不必要的发布
# 在 fast_lio2 配置文件中:
# feat_pub_en: false
# dense_publish_en: false

问题 2: MID360 无法连接

症状: lsusb 看不到 MID360 设备

解决方案:

# 1. 检查 USB 设备
lsusb | grep -i livox

# 2. 检查 USB 权限
sudo usermod -a -G dialout $USER
newgrp dialout

# 3. 重新插拔设备
# 检查 udev 规则
ls /etc/udev/rules.d/ | grep -i livox

# 4. 测试连接
ros2 launch livox_ros2_driver livox_ros2_driver.launch.py

问题 3: PX4 通信失败

症状: MAVROS 无法连接 PX4

原因: 波特率设置不匹配或权限问题

解决方案:

# 1. 验证波特率
stty -F /dev/ttyTHS1 921600

# 2. 检查 UART 权限
ls -l /dev/ttyTHS1

# 3. 在 QGroundControl 中验证 PX4 波特率设置
# 应设为 921600 bps

# 4. 测试连接
ros2 launch mavros apm.launch fcu_url:=/dev/ttyTHS1:921600

问题 4: 内存泄漏导致系统崩溃

症状: 运行时间越长，可用内存越少

解决方案:

# 1. 减少点云缓冲
# fast_lio2 配置中：cube_len: 100
# 从 200 改为 100

# 2. 优化 ROS 2 QoS
# 创建 qos_config.yaml 并在 launch 中引用

# 3. 定期监控
watch -n 1 'free -h'

八、参考资源

8.1 官方文档

8.2 推荐论文

1. Fast-LIO2: 'Fast Direct LIO SAM'
   • 关键贡献：紧耦合的 SLAM 系统，无需预扫描
2. 路径规划: 'Optimal RRT-based Motion Planning'
   • 关键贡献：高效的采样算法
3. 避障: 'Obstacle Avoidance using Artificial Potential Fields'
   • 关键贡献：实时避障方法