基于Jetson Orin Nano的激光雷达SLAM系统集成与实践

在嵌入式AI平台上实现激光SLAM与无人机控制是当前机器人研究的热点。本文介绍在Jetson Orin Nano平台上，结合Ubuntu 22.04、ROS2、宇树L2激光雷达和FAST-LIO2算法，实现PX4飞控定点模式的完整方案。

## 系统环境与基础配置

Jetson Orin Nano预装Ubuntu 22.04系统，首先需要配置基础开发环境。确保系统已更新并安装必要组件：

```bash

# 更新系统软件包

sudo apt update

sudo apt upgrade -y

# 安装基础开发工具

sudo apt install -y build-essential cmake git curl wget

sudo apt install -y libeigen3-dev libboost-all-dev

# 配置USB设备权限（激光雷达需要）

sudo usermod -a -G dialout $USER

sudo usermod -a -G tty $USER

```

由于Jetson平台采用ARM架构，部分库需要源码编译。安装PCL点云库：

```bash

# 安装PCL依赖

sudo apt install -y libflann-dev libvtk9-dev libqhull-dev

sudo apt install -y libproj-dev libpcap-dev

# 编译安装PCL

cd ~

git clone https://github.com/PointCloudLibrary/pcl.git

cd pcl && mkdir build && cd build

cmake -DCMAKE_BUILD_TYPE=Release ..

make -j6

sudo make install

```

## ROS2 Humble安装与配置

Jetson Orin Nano推荐使用ROS2 Humble版本，这是首 个支持ARM64的长期支持版本：

```bash

# 设置区域设置

sudo locale-gen en_US en_US.UTF-8

sudo update-locale LC_ALL=en_US.UTF-8.UTF-8

export.UTF-8

# 添加ROS2仓库

sudo apt install software-properties-common

sudo add-apt-repository universe

sudo apt update && sudo apt install curl -y

sudo curl -sSL https://raw.githubusercontent.com/ros/rosdistro/master/ros.key -o /usr/share/keyrings/ros-archive-keyring.gpg

# 安装ROS2基础包

sudo apt update

sudo apt install ros-humble-desktop

sudo apt install ros-dev-tools

# 配置环境变量

echo "source /opt/ros/humble/setup.bash" >> ~/.bashrc

source ~/.bashrc

```

## 宇树L2激光雷达驱动集成

宇树L2激光雷达需要通过ROS2驱动进行数据采集。创建工作空间并编译驱动：

```bash

# 创建ROS2工作空间

mkdir -p ~/ros2_ws/src

cd ~/ros2_ws/src

# 克隆激光雷达驱动

git clone https://github.com/YUSHU-LIDAR/YUSHU_L2_ROS2_Driver.git

# 安装依赖

sudo apt install -y ros-humble-tf2 ros-humble-tf2-ros

sudo apt install -y ros-humble-sensor-msgs ros-humble-pcl-ros

# 编译工作空间

cd ~/ros2_ws

colcon build --symlink-install --cmake-args -DCMAKE_BUILD_TYPE=Release

# 配置环境

echo "source ~/ros2_ws/install/setup.bash" >> ~/.bashrc

source ~/.bashrc

```

创建激光雷达启动配置文件`~/ros2_ws/src/YUSHU_L2_ROS2_Driver/config/lidar_params.yaml`：

```yaml

lidar:

  device_ip: "192.168.1.100"

  data_port: 2368

  firmware_port: 2369

  group_list: "0,1"

  pcap_path: ""

  pcap_speed: 1.0

publish:

  point_cloud_topic: "/points_raw"

  scan_topic: "/scan"

  frame_id: "laser_link"

  organized_cloud: false

sensor:

  min_range: 0.1

  max_range: 100.0

  horizontal_resolution: 0.4

  vertical_resolution: 0.33

```

启动激光雷达节点进行测试：

```bash

# 启动激光雷达驱动

ros2 launch yushu_l2_driver lidar.launch.py

# 新终端查看点云数据

ros2 topic echo /points_raw --field data --once | head -20

```

## FAST-LIO2算法部署

FAST-LIO2是高效的激光惯性里程计算法，适合嵌入式平台。编译安装过程需要特别注意Jetson平台的优化：

```bash

# 安装依赖

sudo apt install -y ros-humble-navigation2 ros-humble-nav2-bringup

sudo apt install -y libomp-dev

# 下载FAST-LIO2

cd ~/ros2_ws/src

git clone https://github.com/h  ku-mars/FAST_LIO.git

# 修改CMakeLists以适应Jetson平台

cd FAST_LIO

# 编辑CMakeLists.txt，添加ARM优化标志

# 在add_definitions中添加：-march=armv8-a+crypto -mtune=cortex-a78

```

创建FAST-LIO2配置文件`~/ros2_ws/src/FAST_LIO/config/YUSHU_L2.yaml`：

```yaml

common:

  lid_topic: "/points_raw"

  imu_topic: "/imu/data"

  time_sync_en: false

  extrinsic_T: [0.0, 0.0, 0.0]

  extrinsic_R: [1.0, 0.0, 0.0,

                0.0, 1.0, 0.0,

                0.0, 0.0, 1.0]

preprocess:

  lidar_type: 2              # 宇树L2对应类型

  blind: 0.01

  point_filter_num: 1

odometry:

  fov_degree: 180

  filter_size_surf: 0.5

  filter_size_map: 0.5

map:

  frame_id: "map"

  publish_odometry: true

  publish_path: true

```

编译并测试FAST-LIO2：

```bash

# 编译工作空间

cd ~/ros2_ws

colcon build --packages-select fast_lio --cmake-args -DCMAKE_BUILD_TYPE=Release

# 启动FAST-LIO2

ros2 launch fast_lio mapping.launch.py

# 查看生成的里程计数据

ros2 topic echo /Odometry

```

## PX4飞控通信配置

建立ROS2与PX4飞控的通信链路，使用MAVROS2桥接：

```bash

# 安装MAVROS2

sudo apt install -y ros-humble-mavros ros-humble-mavros-extras

# 安装地理库数据

sudo apt install -y geographiclib-tools

sudo geographiclib-get-geoids egm96-5

# 配置MAVROS连接

mkdir -p ~/ros2_ws/src/mavros_config

```

创建MAVROS启动文件`~/ros2_ws/src/mavros_config/launch/px4.launch.py`：

```python

from launch import LaunchDescription

from launch_ros.actions import Node

def generate_launch_description():

    return LaunchDescription([

        Node(

            package='mavros',

            executable='mavros_node',

            name='mavros',

            output='screen',

            parameters=[

                {'fcu_url': 'serial:///dev/ttyACM0:921600'},

                {'tgt_system': 1},

                {'tgt_component': 1},

                {'system_id': 1},

                {'component_id': 191},

                {'plugin_allowlist': [

                    'sys_status', 'command', 'position', 'setpoint',

                    'local_position', 'global_position'

                ]}

            ]

        )

    ])

```

创建定点控制节点`~/ros2_ws/src/px4_control/src/position_hold_node.cpp`：

```cpp

#include <rclcpp/rclcpp.hpp>

#include <geometry_msgs/msg/pose_stamped.hpp>

#include <mavros_msgs/msg/state.hpp>

#include <mavros_msgs/srv/command_bool.hpp>

#include <mavros_msgs/srv/set_mode.hpp>

class PositionHoldNode : public rclcpp::Node {

public:

    PositionHoldNode() : Node("position_hold_node") {

        // 订阅本地位置

        local_pose_sub_ = create_subscription<geometry_msgs::msg::PoseStamped>(

            "/mavros/local_position/pose", 10,

            std::bind(&PositionHoldNode::poseCallback, this, std::placeholders::_1));

        

        // 订阅飞控状态

        state_sub_ = create_subscription<mavros_msgs::msg::State>(

<"ted.yunruiwater.cn"><"hy.sxyicheng.cn"><"loj.jsnjz.cn"><"qwd.csxthr.com">

            "/mavros/state", 10,

            std::bind(&PositionHoldNode::stateCallback, this, std::placeholders::_1));

        

        // 服务客户端

        arm_client_ = create_client<mavros_msgs::srv::CommandBool>("/mavros/cmd/arming");

        mode_client_ = create_client<mavros_msgs::srv::SetMode>("/mavros/set_mode");

        

        timer_ = create_wall_timer(std::chrono::seconds(1),

            std::bind(&PositionHoldNode::controlLoop, this));

    }

    

private:

    void poseCallback(const geometry_msgs::msg::PoseStamped::SharedPtr msg) {

        current_pose_ = *msg;

    }

    

    void stateCallback(const mavros_msgs::msg::State::SharedPtr msg) {

        current_state_ = *msg;

    }

    

    void controlLoop() {

        if (!current_state_.connected) {

            RCLCPP_INFO(get_logger(), "等待飞控连接...");

            return;

        }

        

        // 切换到定点模式

        if (current_state_.mode != "OFFBOARD") {

            setMode("OFFBOARD");

        }

        

        // 解锁电机

        if (!current_state_.armed) {

            armDrone(true);

        }

        

        // 保持当前位置

        maintainPosition();

    }

    

    void setMode(const std::string& mode) {

        auto request = std::make_shared<mavros_msgs::srv::SetMode::Request>();

        request->custom_mode = mode;

        mode_client_->async_send_request(request);

    }

    

    void armDrone(bool arm) {

        auto request = std::make_shared<mavros_msgs::srv::CommandBool::Request>();

        request->value = arm;

        arm_client_->async_send_request(request);

    }

    

    void maintainPosition() {

        // 位置保持逻辑

        // 基于FAST-LIO2的定位数据调整

        RCLCPP_DEBUG(get_logger(), "维持位置: x=%.2f, y=%.2f, z=%.2f",

                     current_pose_.pose.position.x,

                     current_pose_.pose.position.y,

                     current_pose_.pose.position.z);

    }

    

    rclcpp::Subscription<geometry_msgs::msg::PoseStamped>::SharedPtr local_pose_sub_;

    rclcpp::Subscription<mavros_msgs::msg::State>::SharedPtr state_sub_;

    rclcpp::Client<mavros_msgs::srv::CommandBool>::SharedPtr arm_client_;

    rclcpp::Client<mavros_msgs::srv::SetMode>::SharedPtr mode_client_;

    rclcpp::TimerBase::SharedPtr timer_;

    

    geometry_msgs::msg::PoseStamped current_pose_;

    mavros_msgs::msg::State current_state_;

};

int main(int argc, char** argv) {

    rclcpp::init(argc, argv);

    rclcpp::spin(std::make_shared<PositionHoldNode>());

    rclcpp::shutdown();

    return 0;

}

```

## 系统集成与测试

创建完整的系统启动脚本`~/ros2_ws/launch/full_system.launch.py`：

```python

from launch import LaunchDescription

from launch.actions import IncludeLaunchDescription, ExecuteProcess

from launch.launch_description_sources import PythonLaunchDescriptionSource

from launch.substitutions import PathJoinSubstitution

from launch_ros.substitutions import FindPackageShare

<"kpo.zhaiLimao.com"><"wap.yunruiwater.cn"><"share.sxyicheng.cn">

def generate_launch_description():

    return LaunchDescription([

        # 启动激光雷达

        IncludeLaunchDescription(

            PythonLaunchDescriptionSource([

                PathJoinSubstitution([

                    FindPackageShare('yushu_l2_driver'),

                    'launch/lidar.launch.py'

                ])

            ])

        ),

        

        # 启动FAST-LIO2

        IncludeLaunchDescription(

            PythonLaunchDescriptionSource([

                PathJoinSubstitution([

                    FindPackageShare('fast_lio'),

                    'launch/mapping.launch.py'

                ])

            ]),

            launch_arguments={

                'config_path': PathJoinSubstitution([

                    FindPackageShare('fast_lio'),

                    'config/YUSHU_L2.yaml'

                ])

            }.items()

        ),

        

        # 启动MAVROS

        IncludeLaunchDescription(

            PythonLaunchDescriptionSource([

                PathJoinSubstitution([

                    FindPackageShare('mavros_config'),

                    'launch/px4.launch.py'

                ])

            ])

        ),

        

        # 启动定点控制节点

        ExecuteProcess(

            cmd=['ros2', 'run', 'px4_control', 'position_hold_node'],

            output='screen'

        )

    ])

```

测试整个系统：

```bash

# 启动完整系统

ros2 launch full_system.launch.py

# 查看所有活跃话题

ros2 topic list

# 查看FAST-LIO2定位精度

ros2 topic echo /Odometry --field pose.pose.position

```

## 性能优化注意事项

针对Jetson Orin Nano平台进行性能优化：

```bash

# 设置CPU性能模式

sudo nvpmodel -m 0  # 最大性能模式

sudo jetson_clocks  # 启用最大时钟频率

# 监控系统资源

sudo apt install -y htop

htop

# 优化ROS2通信

export ROS_DOMAIN_ID=10  # 减少网络冲突

export RCUTILS_CONSOLE_OUTPUT_FORMAT='[{severity} {time}] [{name}]: {message}'

```

通过上述配置，在Jetson Orin Nano平台上成功集成了激光雷达SLAM与无人机控制系统。该系统能够实时处理激光点云数据，通过FAST-LIO2算法提供精准定位，并控制PX4飞控实现稳定的定点悬停功能。这种方案展示了嵌入式AI平台在自主机器人系统中的实际应用潜力。