6 AMR 开发指南

自主移动机器人（Autonomous Mobile Robot，AMR）是一类能够在环境中自主导航和执行任务的机器人。AMR 不同于自动导航车辆 (AGV)，AGV 依赖于轨道或预定义路线，并且通常需要操作员监督。AMR 通过多传感器融合、人工智能、机器学习等多种技术来理解环境并在其中导航，不受有线电源的限制。因其高度灵活的移动能力和智能导航系统，AMR 在工业自动化、物流、医疗等领域得到了广泛应用。

1. 样例介绍

1.1 外观效果

1.2 功能框架

2. 样例组装

2.1 组件清单

名称	数量	备注
RDK X5	1	地瓜
AMR 底盘（舵轮）+上装+螺丝零件	1	煜禾森
单线激光雷达	1	氪见
tof 摄像头	1	光鉴
双目摄像头（230ai）	1	地瓜配件
IMU（BMI088）	1	地瓜配件
usb 转网口转换器	1
0.3m 网线	1
12v 转 5V4A(MAX 5A)	1
tof 相机支架（3D 打印）	1
双目相机支架（3D 打印）	1
盖板（3D 打印）	1
m2*16 螺丝+m2 螺母	1

2.2 组装步骤讲解

2.2.1 底盘上装安装

原生底盘无法安装摄像头以及存放各种线缆，所以需要另外的上装接入底盘，只需将上装底部的两个孔位对齐底盘安装滑轨上的零件，锁紧螺丝即可

2.2.2 RDK X5 接线

所有传感器以及底盘都直接接入 RDK X5，需要注意双目摄像头使用的是两根同面的 22pin 排线，请按照下图所示方向安装

2.2.3 盖板安装

盖板为 3D 打印件（图纸见文末附件），主要用于安装 RDK X5 并且收纳其余线缆。黄框位置四个安装孔用于安装 RDK X5，安装位置在上盖板的背面。红框位置的安装孔对齐底盘安装滑轨上的零件，锁紧螺丝即可。

2.2.4 激光雷达模块接入

先将激光雷达安装至载板上，螺丝从载板背面安装，然后将接线穿过载板上的孔洞（右图中黄框所示）连接电源管理器。电源管理器安装在载板背面，螺丝从载板正面安装（如左图红框所示），安装完成后接入网线以及 12V 电源，网线与电源线穿过上装的孔洞（右图中红框所示）。

2.2.5 摄像头安装

1. tof 摄像头安装

首先将 tof 摄像头、3D 打印件、钣金支架连接在一起，注意 3D 打印件凹槽朝下。连接完成后将整体塞入上装上方凹槽，电源线与信号线从上装内部孔洞穿过（图 3 黄框）。钣金件的螺丝孔位（图 1 红框）与上装内部螺丝孔位（图 3 红框）对齐，锁紧螺丝即可。

2. 双目摄像头安装

先将双目摄像头与 3D 打印件连接，然后接入底盘尾部的钣金支架即可

2.2.6 电源说明

底盘上提供 12V 的电源。12V 提供给雷达，tof 摄像头以及变压器，变压器输出 5V 提供给 RDK X5

2.2.7 其他部件安装

3. 运行环境准备

注意

tof 相机、激光雷达、底盘、imu 请根据实际设备型号获取代码或者功能包，此处仅提供地瓜配件 imu 的源代码。

3.1 传感器检查

3.1.1 双目摄像头

#检查i2c设备
root@ubuntu:~# i2cdetect -y -r 4
1  2  3  4  5  6  7  8  9  a  b  c  d  e  f
                        -- -- -- -- -- -- -- --
-- -- -- -- -- -- -- -- -- -- -- -- -- -- -- --
-- -- -- -- -- -- -- -- -- -- -- -- -- -- -- --
-- -- 32 -- -- -- -- -- -- -- -- -- -- -- -- --
-- -- -- -- -- -- -- -- -- -- -- -- -- -- -- --
50 -- -- -- -- -- -- -- 58 -- -- -- -- -- -- --
-- -- -- -- -- -- -- -- -- -- -- -- -- -- -- --
-- -- -- -- -- -- -- --

root@ubuntu:~# i2cdetect -y -r 6
1  2  3  4  5  6  7  8  9  a  b  c  d  e  f
                        -- -- -- -- -- -- -- --
-- -- -- -- -- -- -- -- -- -- -- -- -- -- -- --
-- -- -- -- -- -- -- -- -- -- -- -- -- -- -- --
30 -- -- -- -- -- -- -- -- -- -- -- -- -- -- --
-- -- -- -- -- -- -- -- -- -- -- -- -- -- -- --
50 -- -- -- -- -- -- -- 58 -- -- -- -- -- -- --
-- -- -- -- -- -- -- -- -- -- -- -- -- -- -- --
-- -- -- -- -- -- -- --

3.1.2 雷达

#确保雷达与板子的网关与掩码一致，确认是否能ping通
ping 雷达ip

3.1.3 底盘

#启动can设备，请根据具体can设备号以及波特率设置
ip link set can1 up type can bitrate 500000

#检查设备，会有如下输出：can1: <NOARP,UP,LOWER_UP> mtu 16 qdisc mq state UP mode DEFAULT group default qlen 10link/can 
ip link show can1

#检查能否获取can数据，请根据具体can设备号设置
candump can1

3.1.4 IMU

#检查i2c设备 
root@ubuntu:~# i2cdetect -y -r 5
     0  1  2  3  4  5  6  7  8  9  a  b  c  d  e  f
00:                         -- -- -- -- -- -- -- --
10: -- -- -- -- -- -- -- -- -- 19 -- -- -- -- -- --
20: -- -- -- -- -- -- -- -- -- -- -- -- -- -- -- --
30: -- -- -- -- -- -- -- -- -- -- -- -- -- -- -- --
40: -- -- -- -- -- -- -- -- -- -- -- -- -- -- -- --
50: -- -- -- -- -- -- -- -- -- -- -- -- -- -- -- --
60: -- -- -- -- -- -- -- -- -- 69 -- -- -- -- -- --
70: -- -- -- -- -- -- -- --

#获取imu数据(会打印三个数，且随着imu的位姿变化)
cat /sys/devices/virtual/input/input2/acc_bal
cat /sys/devices/virtual/input/input2/gry_bal

3.2 功能安装

sudo apt -y tros-hobot-nav2 ros-foxy-navigation2 ros-foxy-nav-msgs

3.3 源码获取（无安装包的功能，需编译源码）

mkdir -p ~/amr_ws/src && cd src
#tofSLAM 
git clone https://github.com/wunuo1/Tofslam_ros2.git

#点云过滤
git clone https://github.com/wunuo1/voxel_filter.git

#指定位置导航
git clone https://github.com/wunuo1/pose_setter.git

#双目深度
git clone https://github.com/D-Robotics/hobot_stereonet.git

3.4 代码编译

cd ~/amr_ws
source /opt/tros/humble/setup.bash
colcon build

3.5 传感器标定

3.5.1 标定前准备

1. 标定环境准备（标定工具为 kalibr，此处提供包含 kalibr 以及其他标定脚本的 dokcer）

i. 下载 docker 文件

链接: https://pan.baidu.com/s/1oXegKORgWi8Kzf4kdQXL2g?pwd=ur2z 提取码: ur2z

ii. 创建环境

#加载docker
docker load -i ubuntu18.04_calibration.tar

#检查docker是否加载成功（出现TAG为calibration的镜像）
docker images

#从image生成containe
docker run -it --privileged -v /home/nuo.wu/share_dir:/share_dir -e DISPLAY=$DISPLAY -e GDK_SCALE -e GDK_DPI_SCALE --net=host ubuntu18.04:calibration /bin/bash

iii.docker 支持界面显示

# 在host端运行
xhost +local:docker
# 在host端运行以下指令
echo $DISPLAY
# 在docker运行以下指令
echo $DISPLAY
# 查看两者输出是否相同
# 若两者输出不同,若host端显示的结果是:0, 则在docker里运行以下指令
export DISPLAY=:0

2. 准备标定板并配置参数

棋盘格或 aprilgrid 均可（aprilgrid 文件见附件）。棋盘格尺寸方格边长>=10cm 为宜，且为避免标定时棋盘格角点提取/连线出错问题，棋盘格行列应不同。aprilgrid 标定板数据采集时比较方便，操作简单，但对图像质量的要求较高，则目前阶段还是建议使用棋盘格。标定板外尺寸> 1m。

#设置aprilgrid.yaml文件
target_type: 'aprilgrid' #gridtype
tagCols: 6               #number of apriltags
tagRows: 6               #number of apriltags
tagSize: 0.024           #size of apriltag, edge to edge [m]
tagSpacing: 0.3083          #ratio of space between tags to tagSize
codeOffset: 0            #code offset for the first tag in the aprilboard

#设置棋盘格标定板参数，其中targetCols，targetRows分别为每列、每行的内点数，如格子数为9列x8列，则内角点数为8列x7列。
target_type: 'checkboard' 
targetCols: 8 
targetRows: 7  
rawSpacingMeters: 0.1        
colSpacingMeters: 0.1    

3.5.2 双目摄像头内参标定（双目深度算法使用）

1. 标定数据采集

不同的距离移动相机或标定板，确保标定板尽可能覆盖图像的 FOV 范围，移动过程中，棋盘格标定板不要移出画面。稳慢速移动相机或标定板，避免图像因运动导致的模糊。

i. 启动双目摄像头

source /opt/tros/humble/setup.bash
ros2 run mipi_cam mipi_cam --ros-args -p device_mode:="dual" -p out_format:="nv12" -p dual_combine:=2 -p framerate:=10.0 --log-level warn

ii. ros1 与 ros2 消息转换并保存图像数据集（ubuntu20.04 系统，已安装 ros1 与 ros2）

#安装ros1_bridge
sudo apt update
sudo apt install ros-foxy-ros1-bridge

#启动终端
source /opt/ros/noetic/setup.bash
roscore

#启动新终端
source /opt/ros/noetic/setup.bash
source /opt/ros/foxy/setup.bash
ros2 run ros1_bridge dynamic_bridge --bridge-all-topics=false --bridge-topic sensor_msgs/msg/Image /image_combine_raw

#启动新终端 记录图像数据（移动摄像头至不同位置）
source /opt/ros/noetic/setup.bash
rosbag record image_combine_raw

iii. 调用标定程序进行标定（需指定记录的 bag 包路径以及标定配置文件的路径）

$1=path_to_bag
$2=path_to_calib_yaml(checkbord.yaml or aprilgrid.yaml)
rosrun kalibr kalibr_calibrate_cameras --bag $1 --topics /camera/left/image_raw /camera/right/image_raw --models pinhole-radtan pinhole-radtan --target $2 --show-extraction

iv. 标定完成后，标定文件如下

cam0:
  cam_overlaps: [1]
  camera_model: pinhole
  distortion_coeffs: [-0.30512368344314034, 0.06869074995752597, -0.0007768971335288749, -0.0018077365618742228]
  distortion_model: radtan
  intrinsics: [874.7756769752957, 876.9016366753913, 988.9147013767086, 574.9147170323885]
  resolution: [1920, 1080]
  rostopic: /camera/left/image_raw
cam1:
  T_cn_cnm1:
  - [0.9998783828690954, -0.0007967376839525958, 0.015575130180209843, -0.06826569753821157]
  - [0.0008025928358794327, 0.9999996095896897, -0.0003696825565397285, 8.695548926243514e-05]
  - [-0.01557482955949466, 0.0003821380847083855, 0.9998786319622305, 0.0011080079883866683]
  - [0.0, 0.0, 0.0, 1.0]
  cam_overlaps: [0]
  camera_model: pinhole
  distortion_coeffs: [-0.30647450034075296, 0.06904546877578269, -0.0015178028297652521, -0.00016199661118514476]
  distortion_model: radtan
  intrinsics: [875.6405166670517, 877.1839943792446, 963.0392879775055, 519.3086364230766]
  resolution: [1920, 1080]
  rostopic: /camera/right/image_raw

3.5.3 单目摄像头内参标定（用于后续 imu 与 rgb_cam 的外参标定，若使用模组已提供内参，则忽略此步骤）

1. 运行摄像头（请根据实际使用的模组运行指令执行）

2. 记录为 ros1 的 bag 包（ros1 与 ros2 的转换可参考双目内参标定）

3. 运行标定指令（docker 镜像中）

#cam.bag记录的bag文件  /image相机话题  pinhole-radtan相机模型  checkborad.yaml标定板描述文件
rosrun kalibr kalibr_calibrate_cameras --bag cam.bag --topic /image --model pinhole-radtan --target checkboard.yaml

4. 标定完成后内容如下（distortion_coeffs：畸变系数 intrinsics：内参）

3.5.4 IMU 参数标定（用于 imu 与 rgb_cam 的外参标定）

1. 数据采集

i. 运行 imu

source ~/amr_ws/install/setup.bash 
ros2 launch imu_sensor imu_sensor.launch.py

ii. ros1 与 ros2 消息转换并保存图像数据集（ubuntu20.04 系统，已安装 ros1 与 ros2）

#启动终端
source /opt/ros/noetic/setup.bash
roscore

#启动新终端
source /opt/ros/noetic/setup.bash
source /opt/ros/foxy/setup.bash
ros2 run ros1_bridge dynamic_bridge --bridge-all-topics=false --bridge-topic sensor_msgs/msg/Imu /imu_data

#启动新终端 记录imu数据（imu需静置30min以上，存储数据）
source /opt/ros/noetic/setup.bash
rosbag record imu_data

iii. 修改脚本文件参数（docker 环境中）

vim /root/catkin_ws/src/imu_utils/launch/oal.launch

#imu_topic         imu的topic
#data_save_path    标定文件的保存路径
#max_time_min      采集imu数据的时间长短

iv. 运行标定脚本（待 imu bag 数据播放完毕会打印标定结果）

roslaunch imu_utils oal.lauch

v.播放 imu 的 bag 数据

rosbag play imu.bag

vi. 修改 IMU 的参数配置文件

# 将上文生成的 BMI088_imu_param.yaml文件重新复制一份，作为备份，后续需要使用，执行如下命令：
cp -r BMI088_imu_param.yaml imu.yaml

vim imu.yaml

配置文件打开后，如下图所示：

将原始的 BMI088_imu_param.yaml 文件内容重写，修改为如下格式：

#Accelerometers
accelerometer_noise_density: 3.0219972277096316e-02   #Noise density (continuous-time)对应上述截图中的 Acc->avg-axis->acc_n
accelerometer_random_walk:   1.9955401630133244e-04   #Bias random walk 对应上述截图中的 Acc->avg-axis->acc_w
 
#Gyroscopes
gyroscope_noise_density:     2.8131368978892658e-03   #Noise density (continuous-time)对应上述截图中的 Gyr->avg-axis->gyr_n
gyroscope_random_walk:       1.9705283783356781e-05   #Bias random walk 对应上述截图中的 Gyr->avg-axis->gyr_w
 
rostopic:                    /imu_data     #the IMU ROS topic 对应上文生成rosbag格式数据章节数据bag中topic里面的/imu_data
update_rate:                 400.0      #Hz (for discretization of the values above)对应真实使用的IMU频率

3.5.5 rgb_cam-imu 外参标定（用于获取 tof_cam-imu 的变换）

1. 启动双目摄像头

source /opt/tros/humble/setup.bash
ros2 run mipi_cam mipi_cam --ros-args -p device_mode:="dual" -p out_format:="nv12" -p dual_combine:=2 -p framerate:=10.0 --log-level warn

2. 启动 imu

source ~/amr_ws/install/setup.bash 
ros2 launch imu_sensor imu_sensor.launch.py

3. ros1 与 ros2 消息转换并保存图像数据集（ubuntu20.04 系统，已安装 ros1 与 ros2）

#安装ros1_bridge
sudo apt update
sudo apt install ros-foxy-ros1-bridge

#启动终端
source /opt/ros/noetic/setup.bash
roscore

#启动新终端
source /opt/ros/noetic/setup.bash
source /opt/ros/foxy/setup.bash
ros2 run ros1_bridge dynamic_bridge --bridge-all-topics

#启动新终端 记录图像数据（移动摄像头至不同位置）
source /opt/ros/noetic/setup.bash
rosbag record /image_combine_raw /imu_data

4. 运行标定程序

rosrun kalibr kalibr_calibrate_imu_camera --bag cam_imu.bag --target april.yaml --cam cam.yaml --imu imu.yaml

标定结束后，查看标定结果(当 dJ 收敛,降低到 0.1 以下时,表示标定准确)

# storage-results-imucam.txt 
vim storage-results-imucam.txt

外参结果如下：

3.5.6 其他

1. rgb_cam-tof_cam 变换由模组给出，tof_cam-bask_link 变换由设备装配图纸给出。

2. tof_cam-imu 的变换（用于 tofSLAM），可通过 rgb_cam-imu 与 rgb_cam-tof_cam 变化得到（变换矩阵相乘）

3. bask_link-imu 的变换（用于 tofSLAM），可通过 tof_cam-imu 与 tof_cam-bask_link 变化得到（变换矩阵相乘）

3.6 更改配置文件（用于 tofSLAM）

修改 config 文件夹下 mapping.yaml 文件，关注带有说明的参数：

YAML: 1.0

preprocess:
  point_filter_num: 3  #点云下采样
  lidar_type: 7  # 1-AVIA 2-velodyne 3-ouster  4-robosense 5-pandar 6-dtof 7-mtof
  blind: 0.01


common:
  imu_topic: /imu #imu消息名称
  lid_topic: /nebula/mtof_points2 #点云或雷达消息名称
  odom_topic: /odom  #里程计消息
  img_topic: /camera/color/image_raw/compressed #rgb图像消息，可使用压缩消息也可用原图
  Localization_mode: False #是否进行定位，否则为建图
  output_path: /home/slam_ws_2/output #存放建图产物的路径
  model_path: /home/slam_ws_2/output/hfnet.onnx #指定hfnet模型路径
  imuhz: 400 #imu的频率
 
mapping:
  extrinsic_est_en: true
  #tof到imu的变化，获取方式请查看标定说明
  tof2imu_extrinsic_T: [ -0.0039799203111821468, -0.027189542185995823831, -0.14001955020902342461]
  tof2imu_extrinsic_R: [-0.993063627638373553564 ,0.052518963674774226572 ,-0.01498737732389914978, 
                        -0.033814436045461613632 ,-0.016796566829113524293 ,-0.99928717438014507495, 
                        -0.00801808196977665167 ,-0.99847864521832871386  ,0.03465425204866725379]
  
  #bask_link到imu的变化，获取方式请查看标定说明
  robot2imu_extrinsic_T: [0.12588064308116462841, 0.008162559660163853708, 0.20705414746130172184]
  robot2imu_extrinsic_R: [0.052268869631951098023, -0.93852017660344945467, 0.033846560965168889575, 
                          0.93808656435923075909, -0.051737213472478319612, 0.015014368436287678477, 
                          0.033019704102458273174        , 0.01675474093936925155, 0.99931446977489903968]


delay_time: 0.3

odometry:
  wheelmode: 1 #0-relativepose 1-velocity
  max_trans_diff: 0.1
  max_drift_num: 15
  surf_res: 0.1 #0.4
  log_print: true
  max_num_iteration: 5
  # ct_icp
  icpmodel: CT_POINT_TO_PLANE  # CT_POINT_TO_PLANE  #CT_POINT_TO_PLANE                    # Options: [CT_POINT_TO_PLANE, POINT_TO_PLANE]
  size_voxel_map: 0.1 #0.4                         # The voxel size of in the voxel map
  min_distance_points: 0.05
  max_num_points_in_voxel: 20                 # The maximum number of points per voxel of the map
  max_distance: 50.0                        # The threshold of the distance to suppress voxels from the map
  weight_alpha: 0.9
  weight_neighborhood: 0.1
  max_dist_to_plane_icp: 0.1 #0.3
  init_num_frames: 20
  voxel_neighborhood: 1
  max_number_neighbors: 20
  threshold_voxel_occupancy: 1
  estimate_normal_from_neighborhood: true
  min_number_neighbors: 20                    # The minimum number of neighbor points to define a valid neighborhood
  power_planarity: 2.0
  num_closest_neighbors: 1

  sampling_rate: 1.0
  ratio_of_nonground: 2
  # max_num_residuals: 1000
  max_num_residuals: 2000
  min_num_residuals: 100
  motion_compensation: CONSTANT_VELOCITY #NONE #CONSTANT_VELOCITY  #CONTINUOUS #NONE, CONSTANT_VELOCITY, ITERATIVE, CONTINUOUS
  beta_location_consistency: 1.0
  beta_orientation_consistency: 1.0
  beta_constant_velocity: 1.0
  beta_small_velocity: 0.0

  thres_translation_norm: 0.03
  thres_orientation_norm: 0.05

  use_ground_constraint: 0

#以下为定位功能参数
reloc:
  mapfile: /home/slam_ws_2/output/final-voxel.pcd #pcd点云地图路径，建图完成的产物
  reloc_mode: 1  #0-scancontext 1-hfnet  2-no   #重定位检测方式，0为点云检测，1为特征点检测
  scancontext_dbfile: /home/slam_ws_2/output/ScanContext.bin #点云检测模型路径
  hfnet_dbfile: /home/slam_ws_2/output/HFNet.bin #特征点检测模型路径
  icp_threshold: 0.03

4. 功能体验

4.1 tofSLAM 建立三维地图

#确保完成运行环境准备中提及的各项内容
#确保建图开始时请确保摄像头可以看到完整的apriltag，且各类话题参数匹配，运行后会在output文件夹下生成map坐标系到apriltag的变换
source ~/amr_ws/install/setup.bash
ros2 run demo demo --ros-args -p build_map:=true

#config配置文件中Localization_mode参数设置为False，且各类话题参数匹配
source ~/amr_ws/install/setup.bash
ros2 run ct_lio ct_lio_eskf

启动之后需静置 3~4 秒，让 imu 初始化成功，随后可移动机器人进行建图，建立完成后关闭程序。运行完成后如下图所示：在 mapping.yaml 文件中指定的路径会生成以下产物

使用 pcl_viewer 工具查看点云地图：

建图效果视频： Video: https://www.bilibili.com/video/BV1T4tKeoEsH

4.2 tofSLAM 定位

#确保完成运行环境准备中提及的各项内容，config配置文件中Localization_mode参数设置为True，且各类话题参数匹配
source ~/amr_ws/install/setup.bash
ros2 run ct_lio ct_lio_eskf

启动后，当显示帧序号一直为 1，说明还未重定位成功当帧序号大于 1，说明重定位成功，可开启 rviz2 查看路径

4.3 三维点云地图转二维栅格地图

该功能包为开源功能包，为 ros1 的功能包，请在已安装 ros1 的环境下使用，使用方法如下：

1. 源码下载与编译

mkdir -p ~/pcd3d-2d/src && cd ~/pcd3d-2d/src
git clone https://github.com/Hinson-A/pcd2pgm_package
cd ..
source /opt/ros/noetic/setup.bash
catkin_make

2. 运行功能

source /opt/ros/noetic/setup.bash
source ~/pcd3d-2d/devel/setup.bash

#将launch文件中的pcd文件路径更换为实际路径
roslaunch pcd2pgm run.launch

3. 保存地图

#开启新的终端（需安装ros-noetic-map-server）
source /opt/ros/noetic/setup.bash
rosrun map_server map_saver

执行后会在当前路径下生成 map.pgm 和 map.yaml 文件，这两个文件可用于 nav2，使用时需更换 hobot_nav2 的 maps 文件夹中的文件，一般路径为/opt/tros/humble/share/hobot_nav2/maps，点云地图以及栅格地图效果如下：

4.4 定点位置导航

1. 启动 tof 摄像头，激光雷达，底盘，imu

#各传感器启动请根据实际的设备运行相应指令，此处仅给出imu启动指令
source /opt/tros/humble/setup.bash
ros2 launch imu_sensor imu_sensor.launch.py

#打开新终端，启动点云过滤
ros2 run point_cloud_processing point_cloud_sparsification

2. 启动 nav2 导航功能

source /opt/tros/humble/setup.bash
ros2 launch hobot_nav2 hobot_nav2_bringup.launch.py

3. 启动定点位置导航功能（确保启动后摄像头图像中能看到完整的 AprilTag）

source ~/amr_ws/install/setup.bash
ros2 run demo demo

4. PC 端打开 rviz 查看导航效果

ros2 launch nav2_bringup rviz_launch.py

Video: https://www.bilibili.com/video/BV1t4tKeoEgh

4.5 目标检测与分割

1. 复制该 launch 脚本到板端，更改对应参数：

import os
from launch import LaunchDescription
from launch_ros.actions import Node
from launch.actions import IncludeLaunchDescription
from launch.launch_description_sources import PythonLaunchDescriptionSource, AnyLaunchDescriptionSource
from ament_index_python import get_package_share_directory

def generate_launch_description():
    #将tof摄像头发布的bgr8图像编码为jpeg，用于web端展示
    bgr2jpeg =  Node(
        package='hobot_codec',
        executable='hobot_codec_republish',
        output='screen',
        parameters=[{
            'in_mode': 'ros',
            'in_format': 'bgr8',
            'out_mode': 'ros',
            'out_format': 'jpeg',
            'sub_topic': '/nebula200/stof_rgb/image_raw',
            'dump_output': False
        }],
        arguments=['--ros-args', '--log-level', 'warn']
    )
    #将jpeg图像解码为nv12，用于yolov8推理
    jpeg2nv12 =  Node(
        package='hobot_codec',
        executable='hobot_codec_republish',
        output='screen',
        parameters=[{
            'in_mode': 'ros',
            'in_format': 'jpeg',
            'out_mode': 'ros',
            'out_format': 'nv12',
            'channel': 1,
            'sub_topic': '/image_raw/compressed',
            'dump_output': False,
            'pub_topic': '/image'
        }],
        arguments=['--ros-args', '--log-level', 'warn']
    )
    #运行yolov8检测分割
    yolov8 =  Node(
        package='dnn_node_example',
        executable='example',
        output='screen',
        parameters=[{
            'feed_type': 1,
            'is_shared_mem_sub': 0,
            'config_file': 'config/yolov8segworkconfig.json'
        }],
        arguments=['--ros-args', '--log-level', 'warn']
    )
    #启动web端展示功能
    web_node = IncludeLaunchDescription(
        PythonLaunchDescriptionSource(
            os.path.join(
                get_package_share_directory('websocket'),
                'launch/websocket.launch.py')),
        launch_arguments={
            'websocket_image_topic': '/image_raw/compressed',
            'websocket_image_type': 'mjpeg',
            'websocket_smart_topic': '/hobot_dnn_detection'
        }.items()
    )

    return LaunchDescription([
        bgr2jpeg,
        jpeg2nv12,
        yolov8,
        web_node
    ])

2. 将命令中 path_of_launch_file 更换为自己的 launch 文件路径，运行命令

source /opt/tros/humble/setup.bash
ros2 launch <path_of_launch_file>

3. 同一局域网下的 PC 打开浏览器，在网址处输入：板子 ip:8000，即可查看识别效果

5. 代码介绍

5.1 tofSLAM

5.1.1 代码仓库：

https://github.com/wunuo1/TofSLAM_ros2

5.1.2 工程框架逻辑：

5.1.3 详细说明：

1. ESKF（Error State Kalman Filter）

误差状态卡尔曼滤波器，是一种传感器融合的算法。它基于经典的卡尔曼滤波器（Kalman Filter, KF）原理，并对其进行改进，以更好地处理非线性系统中的误差传播

状态量：p、R、v、bg、ba、g

预测模型：IMU

观测（更新）模型 1：底盘轮速&底盘 imu 通过 EKF 计算的位姿——松耦合（R、p 更新）

观测（更新）模型 2：点云 ICP 计算得到的位姿——松耦合（R、p 更新）

2. IMU 静态初始化

imu 初始化保持静态，根据加速度计估计出重力方向、陀螺仪零偏、加速度计零偏，设置初始位姿为原点，初始速度为 0

3. 地图格式：哈系体素地图

用一个哈系列表来保存体素地图，每个体素（0.1×0.1×0.1）里面保存若干个点，点与点之间保持一定距离（0.05）。

4. 点云 ICP

1. 关键点提取：对每一帧的点云采用 gridsample 的方法进行降采样，提取关键点。

对于每个关键点：

寻找最近邻：通过 voxelmap，在相邻的 voxel 中寻找距离最近的 N 个点 qi 作为邻居点。
计算邻居点的法向 n、协方差矩阵和平滑度权重 a
计算关键点到邻居点所在平面的距离，满足距离小于阈值的点对，加入 icp 优化。

5. CT_ICP 算法（CT-ICP: Real-time Elastic LiDAR Odometry with Loop Closure）

1. 前端位姿估计

CTICP 算法前端部分整体框架对于每一帧使用了两个 pose 来描述：不需要再提前对每一帧点云单独去运动畸变了，而是直接在优化过程中去畸变，该算法对于高速运动更鲁棒。

优化方程：

2. 后端回环优化

每次取 Nmap 帧 scan 拼成点云，把每个点插入到一个 2D 高程网格中。再由 2D 高程网格生成 2D 高程图，每个像素对应 Z 轴最大的点。对 2D 高程图进行特征提取，提取的特征与 2D 高程网格一起随着关键帧保存在内存中。当每次提取完高程图特征后都与内存中的特征进行匹配；对匹配成功的高程图对先用 Ransac 估计一次 2D 变换矩阵，根据内点个数来验证匹配可信度；再用 2D 变换矩阵作为初值，对 2D 高程网格中的点云进行 ICP，最终为成功匹配的关键帧添加一条边，使用 g2o 进行姿态图优化。（该方法只适用于平面运动）

5.2 双目深度

5.2.1 代码仓库:

https://github.com/D-Robotics/hobot_stereonet.git

5.2.2 工程逻辑框架:

5.2.3 详细说明：

1. 双目深度感知原理：

2. 双目立体匹配算法步骤:

3. 模型介绍

Backbone：MixVarGENet+UNet。针对 X5 优化的高效 backbone+UNet 恢复细节；
Cost Volume：Groupwise Correlation Cost Volume。计算左右图特征相关性构建 cost volume；
Cost Aggregation：UNet。通过 UNet 对 cost volume 进行 aggregation，更精细化的整合 cost volume；
Refinement：GRU。采用 GRU 模块带来边缘细节的性能提升。
Spatial Upsampling：使用 Conv 代替 Unfold 操作进行张量的切分，通过加权组合来生成完整分辨率的视差图

5.3 目标检测与分割

5.3.1 代码仓库:

https://github.com/D-Robotics/hobot_dnn/tree/

5.3.2 工程逻辑框架:

5.3.3 详细说明：

使用模型为官版 yolov8-seg，源码仓库链接https://github.com/ultralytics/ultralytics
编解码功能使用 RDK X5 上的硬件单元加速，大幅降低 CPU 占用的同时提升格式转换效率
工程使用代码为 tros 的 hobot_dnn_example，除 yolov8-seg 外，同时支持多种模型的推理使用

5.4 导航以及任务调度

5.4.1 代码仓库:

https://github.com/wunuo1/pose_setter.git

5.4.2 工程逻辑框架:

5.4.3 详细说明：

程序在启动进行 AprilTag 检测，检测成功后，会缓存十帧的变换并取平均值。以此得到 rgb 到 AprilTag 的变换，读取建图时记录的 map 到 AprilTag 的变换，计算出 rgb 到 map 的变换。而前期的标定过程中，已经可以计算出 rgb 到 robot 的变换，所以可以得到 robot 到 map 的变换，以此发布机器人在 map 坐标系下的初始位置。
初始位置发布完成之后，激活线程开始请求导航到目标位置，若请求被拒绝，则说明姿态初始化失败，重新发布初始位置。
请求导航到多个目标位置，只有前一个目标位置导航成功之后，才会激活线程进行下一个请求
当初始位置之后，会进行 AprilTag 的检测，并重复循环进行之前的步骤
nav2 导航插件中，在障碍物层增加了点云信息做，用于避开高度低于激光雷达的障碍物，但因原生点云数据会有噪声，并且点云数据量过多，所以增加点云筛选节点对点云进行稀疏化以及过滤。

6. 附件

链接: https://pan.baidu.com/s/1uHSifO6fOIqhKUgP3yYUzw?pwd=ttfn 提取码: ttfn

1. 样例介绍​

1.1 外观效果​

1.2 功能框架​

2. 样例组装​

2.1 组件清单​

2.2 组装步骤讲解​

2.2.1 底盘上装安装​

2.2.2 RDK X5 接线​

2.2.3 盖板安装​

2.2.4 激光雷达模块接入​

2.2.5 摄像头安装​

1. tof 摄像头安装​

2. 双目摄像头安装​

2.2.6 电源说明​

2.2.7 其他部件安装​

3. 运行环境准备​

3.1 传感器检查​

3.1.1 双目摄像头​

3.1.2 雷达​

3.1.3 底盘​

3.1.4 IMU​

3.2 功能安装​

3.3 源码获取（无安装包的功能，需编译源码）​

3.4 代码编译​

3.5 传感器标定​

3.5.1 标定前准备​

1. 标定环境准备（标定工具为 kalibr，此处提供包含 kalibr 以及其他标定脚本的 dokcer）​

i. 下载 docker 文件​

ii. 创建环境​

iii.docker 支持界面显示​

2. 准备标定板并配置参数​

3.5.2 双目摄像头内参标定（双目深度算法使用）​

1. 标定数据采集​

i. 启动双目摄像头​

ii. ros1 与 ros2 消息转换并保存图像数据集（ubuntu20.04 系统，已安装 ros1 与 ros2）​

iii. 调用标定程序进行标定（需指定记录的 bag 包路径以及标定配置文件的路径）​

iv. 标定完成后，标定文件如下​

3.5.3 单目摄像头内参标定（用于后续 imu 与 rgb_cam 的外参标定，若使用模组已提供内参，则忽略此步骤）​

1. 运行摄像头（请根据实际使用的模组运行指令执行）​

2. 记录为 ros1 的 bag 包（ros1 与 ros2 的转换可参考双目内参标定）​

3. 运行标定指令（docker 镜像中）​

4. 标定完成后内容如下（distortion_coeffs：畸变系数 intrinsics：内参）​

3.5.4 IMU 参数标定（用于 imu 与 rgb_cam 的外参标定）​

1. 数据采集​

i. 运行 imu​

ii. ros1 与 ros2 消息转换并保存图像数据集（ubuntu20.04 系统，已安装 ros1 与 ros2）​

iii. 修改脚本文件参数（docker 环境中）​

iv. 运行标定脚本（待 imu bag 数据播放完毕会打印标定结果）​

v.播放 imu 的 bag 数据​

vi. 修改 IMU 的参数配置文件​

3.5.5 rgb_cam-imu 外参标定（用于获取 tof_cam-imu 的变换）​

1. 启动双目摄像头​

2. 启动 imu​

3. ros1 与 ros2 消息转换并保存图像数据集（ubuntu20.04 系统，已安装 ros1 与 ros2）​

4. 运行标定程序​

3.5.6 其他​

1. rgb_cam-tof_cam 变换由模组给出，tof_cam-bask_link 变换由设备装配图纸给出。​

2. tof_cam-imu 的变换（用于 tofSLAM），可通过 rgb_cam-imu 与 rgb_cam-tof_cam 变化得到（变换矩阵相乘）​

3. bask_link-imu 的变换（用于 tofSLAM），可通过 tof_cam-imu 与 tof_cam-bask_link 变化得到（变换矩阵相乘）​

3.6 更改配置文件（用于 tofSLAM）​

4. 功能体验​

4.1 tofSLAM 建立三维地图​

4.2 tofSLAM 定位​

4.3 三维点云地图转二维栅格地图​

1. 源码下载与编译​

2. 运行功能​

3. 保存地图​

4.4 定点位置导航​

1. 启动 tof 摄像头，激光雷达，底盘，imu​

2. 启动 nav2 导航功能​

3. 启动定点位置导航功能（确保启动后摄像头图像中能看到完整的 AprilTag）​

4. PC 端打开 rviz 查看导航效果​

4.5 目标检测与分割​

1. 复制该 launch 脚本到板端，更改对应参数：​

2. 将命令中 path_of_launch_file 更换为自己的 launch 文件路径，运行命令​

3. 同一局域网下的 PC 打开浏览器，在网址处输入：板子 ip:8000，即可查看识别效果​

5. 代码介绍​

5.1 tofSLAM​

5.1.1 代码仓库：​

5.1.2 工程框架逻辑：​

1. 样例介绍

1.1 外观效果

1.2 功能框架

2. 样例组装

2.1 组件清单

2.2 组装步骤讲解

2.2.1 底盘上装安装

2.2.2 RDK X5 接线

2.2.3 盖板安装

2.2.4 激光雷达模块接入

2.2.5 摄像头安装

1. tof 摄像头安装

2. 双目摄像头安装

2.2.6 电源说明

2.2.7 其他部件安装

3. 运行环境准备

3.1 传感器检查

3.1.1 双目摄像头

3.1.2 雷达

3.1.3 底盘

3.1.4 IMU

3.2 功能安装

3.3 源码获取（无安装包的功能，需编译源码）

3.4 代码编译

3.5 传感器标定

3.5.1 标定前准备

1. 标定环境准备（标定工具为 kalibr，此处提供包含 kalibr 以及其他标定脚本的 dokcer）

i. 下载 docker 文件

ii. 创建环境

iii.docker 支持界面显示

2. 准备标定板并配置参数

3.5.2 双目摄像头内参标定（双目深度算法使用）

1. 标定数据采集

i. 启动双目摄像头

ii. ros1 与 ros2 消息转换并保存图像数据集（ubuntu20.04 系统，已安装 ros1 与 ros2）

iii. 调用标定程序进行标定（需指定记录的 bag 包路径以及标定配置文件的路径）

iv. 标定完成后，标定文件如下

3.5.3 单目摄像头内参标定（用于后续 imu 与 rgb_cam 的外参标定，若使用模组已提供内参，则忽略此步骤）

1. 运行摄像头（请根据实际使用的模组运行指令执行）

2. 记录为 ros1 的 bag 包（ros1 与 ros2 的转换可参考双目内参标定）

3. 运行标定指令（docker 镜像中）

4. 标定完成后内容如下（distortion_coeffs：畸变系数 intrinsics：内参）

3.5.4 IMU 参数标定（用于 imu 与 rgb_cam 的外参标定）

1. 数据采集

i. 运行 imu

ii. ros1 与 ros2 消息转换并保存图像数据集（ubuntu20.04 系统，已安装 ros1 与 ros2）

iii. 修改脚本文件参数（docker 环境中）

iv. 运行标定脚本（待 imu bag 数据播放完毕会打印标定结果）

v.播放 imu 的 bag 数据

vi. 修改 IMU 的参数配置文件

3.5.5 rgb_cam-imu 外参标定（用于获取 tof_cam-imu 的变换）

1. 启动双目摄像头

2. 启动 imu

3. ros1 与 ros2 消息转换并保存图像数据集（ubuntu20.04 系统，已安装 ros1 与 ros2）

4. 运行标定程序

3.5.6 其他

1. rgb_cam-tof_cam 变换由模组给出，tof_cam-bask_link 变换由设备装配图纸给出。

2. tof_cam-imu 的变换（用于 tofSLAM），可通过 rgb_cam-imu 与 rgb_cam-tof_cam 变化得到（变换矩阵相乘）

3. bask_link-imu 的变换（用于 tofSLAM），可通过 tof_cam-imu 与 tof_cam-bask_link 变化得到（变换矩阵相乘）

3.6 更改配置文件（用于 tofSLAM）

4. 功能体验

4.1 tofSLAM 建立三维地图

4.2 tofSLAM 定位

4.3 三维点云地图转二维栅格地图

1. 源码下载与编译

2. 运行功能

3. 保存地图

4.4 定点位置导航

1. 启动 tof 摄像头，激光雷达，底盘，imu

2. 启动 nav2 导航功能

3. 启动定点位置导航功能（确保启动后摄像头图像中能看到完整的 AprilTag）

4. PC 端打开 rviz 查看导航效果

4.5 目标检测与分割

1. 复制该 launch 脚本到板端，更改对应参数：

2. 将命令中 path_of_launch_file 更换为自己的 launch 文件路径，运行命令

3. 同一局域网下的 PC 打开浏览器，在网址处输入：板子 ip:8000，即可查看识别效果

5. 代码介绍

5.1 tofSLAM

5.1.1 代码仓库：

5.1.2 工程框架逻辑：