One
0%

发表于 更新于 分类于
本文字数： 2.4k 阅读时长 ≈ 2 分钟

1. open_RMF 配置，操作

1.1. RMF安装

系统要求：ubuntu22.04 ### 1.1.1. 准备工作

1.1.2. 非ROS依赖安装

1
2
3
4
5
6
sudo apt update && sudo apt install \
  python3-pip \
  curl \
  python3-colcon-mixin \
  ros-dev-tools \
  -y

1
2
3
4
5
6
 sudo sh -c 'echo "deb http://packages.osrfoundation.org/gazebo/ubuntu-stable `lsb_release -cs` main" > /etc/apt/sources.list.d/gazebo-stable.list'  

wget https://packages.osrfoundation.org/gazebo.key -O - | sudo apt-key add -

# These pip packages are only used by rmf_demos which are not released as binaries
python3 -m pip install flask-socketio fastapi uvicorn

1.1.3. ROS依赖安装

1
2
sudo rosdep init # run if first time using rosdep.
rosdep update

刷新环境 

1
2
colcon mixin add default https://raw.githubusercontent.com/colcon/colcon-mixin-repository/master/index.yaml
colcon mixin update default

1、创建工作空间，拉取源码 

1
2
3
4
mkdir -p ~/rmf_ws/src
cd ~/rmf_ws
wget https://raw.githubusercontent.com/open-rmf/rmf/main/rmf.repos
vcs import src < rmf.repos

指令：wget https://raw.githubusercontent.com/open-rmf/rmf/main/rmf.repos
问题：Connecting to raw.githubusercontent.com (raw.githubusercontent.com)|::|:443… failed: Connection refused.

指令：vcs import src < rmf.repos
问题：Command ‘vcs’ not found

解决方法：sudo apt-get install python3-vcstool 问题：Failed to connect to github.com port 443 after 21047 ms 网络问题
解决方法: 

1
2
git config --global https.proxy  
git config --global --unset https.proxy

2、 安装依赖 

1
2
cd ~/rmf_ws
rosdep install --from-paths src --ignore-src --rosdistro humble -y

3、安装 clang 作为编译器和 lld 作为链接器

1
2
sudo apt update
sudo apt install clang clang-tools lldb lld libstdc++-12-dev

4、编译安装 

1
2
3
4
5
6
cd ~/rmf_ws
source /opt/ros/humble/setup.bash # replace humble with ROS 2 distro of choice.
 
export CXX=clang++
export CC=clang
colcon build --mixin release lld

1.2. RMF WEB 安装

1.2.1. 准备工作

安装nodejs 

1
2
3
4
sudo apt update && sudo apt install curl
curl -o- https://raw.githubusercontent.com/nvm-sh/nvm/v0.37.2/install.sh | bash
#重启终端后继续执行
nvm install 16

安装pnpm 

1
2
3
curl -fsSL https://get.pnpm.io/install.sh | bash -
#重启终端后继续执行
pnpm env use --global 16
安装pipenv 
1
2
sudo apt install python3-venv
pip3 install pipenv
问题：pipenv not found: https://blog.csdn.net/FloraCHY/article/details/119790395

1.2.2. 安装依赖

1
2
3
git clone https://github.com/open-rmf/rmf-web.git
cd  ~/rmf-web
pnpm install

查看pnpm源 pnpm config get registry
切换淘宝源 pnpm config set registry http://registry.npm.taobao.org
还原 pnpm config set registry https://registry.npmjs.org

1.3. 操作流程：

demo试运行：

1
2
cd ~/rmf-web/packages/dashboard
pnpm start

仿真

1
2
source ~/rmf_ws/install/setup.bash  
ros2 launch rmf_demos_gz office.launch.xml server_uri:="http://localhost:8000/_internal"

发表于 更新于 分类于
本文字数： 9.4k 阅读时长 ≈ 9 分钟

目录结构

自定义配置文件：src/cartographer_ros/cartographer_ros/configuration_files/config.lua
前端配置文件：src/cartographer/configuration_files/trajectory_builder_2d.lua
后端配置文件： src/cartographer/configuration_files/pose_graph.lua
# 参数解析 ## 基本参数 

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
map_frame = "map",    --生成的地图坐标系
tracking_frame = "base_link",    --跟踪的坐标系，可以是imu、小车、雷达
published_frame = "base_link",    --cartographer正在发布pose的坐标，一般就是小车
odom_frame = "odom",    --cartographer的里程计坐标系
provide_odom_frame = false,--    cartographer是否发布里程计坐标
publish_frame_projected_to_2d = true,    --是否无滚动、俯仰或z偏移
use_odometry = false,    --订阅里程计
use_nav_sat = false,    --订阅GPS
use_landmarks = false,    --订阅路标
num_laser_scans = 1,    --订阅雷达格式以及数量
num_multi_echo_laser_scans = 0,    --订阅雷达格式以及数量
num_subdivisions_per_laser_scan = 1,    --分割扫描点云
num_point_clouds = 0,    --订阅雷达格式以及数量
lookup_transform_timeout_sec = 0.2,   --tf2查找变换超时（s）
submap_publish_period_sec = 0.3,    --发布子图实时间间隔（s）
pose_publish_period_sec = 5e-3,    --发布pose时间间隔（s）
trajectory_publish_period_sec = 30e-3,   --发布轨迹标记间隔（s，这里为30ms）
rangefinder_sampling_ratio = 1.,    --以下5个参数为传感器（里程计、位姿、imu、反光板）采样比例
odometry_sampling_ratio = 1.,
fixed_frame_pose_sampling_ratio = 1.,
imu_sampling_ratio = 1.,
landmarks_sampling_ratio = 1.,

## 前端参数 
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114

TRAJECTORY_BUILDER_2D = {
  use_imu_data = true,            -- 是否使用imu数据
  min_range = 0.,                 -- 雷达数据的最远最近滤波, 保存中间值
  max_range = 30.,
  min_z = -0.8,                   -- 雷达数据的最高与最低的过滤, 保存中间值
  max_z = 2.,
  missing_data_ray_length = 5.,   -- 超过最大距离范围的数据点用这个距离代替
  num_accumulated_range_data = 1, -- 几帧有效的点云数据进行一次扫描匹配
  voxel_filter_size = 0.025,      -- 体素滤波的立方体的边长

  -- 使用固定的voxel滤波之后, 再使用自适应体素滤波器
  -- 体素滤波器 用于生成稀疏点云 以进行 扫描匹配
  adaptive_voxel_filter = {
    max_length = 0.5,             -- 尝试确定最佳的立方体边长, 边长最大为0.5
    min_num_points = 200,         -- 如果存在 较多点 并且大于'min_num_points', 则减小体素长度以尝试获得该最小点数
    max_range = 50.,              -- 距远离原点超过max_range 的点被移除
  },

  -- 闭环检测的自适应体素滤波器, 用于生成稀疏点云 以进行 闭环检测
  loop_closure_adaptive_voxel_filter = {
    max_length = 0.9,
    min_num_points = 100,
    max_range = 50.,
  },

  -- 是否使用 real_time_correlative_scan_matcher 为ceres提供先验信息
  -- 计算复杂度高 , 但是很鲁棒 , 在odom或者imu不准时依然能达到很好的效果
  use_online_correlative_scan_matching = false,
  real_time_correlative_scan_matcher = {
    linear_search_window = 0.1,             -- 线性搜索窗口的大小
    angular_search_window = math.rad(20.),  -- 角度搜索窗口的大小
    translation_delta_cost_weight = 1e-1,   -- 用于计算各部分score的权重
    rotation_delta_cost_weight = 1e-1,
  },

  -- ceres匹配的一些配置参数
  ceres_scan_matcher = {
    occupied_space_weight = 1.,
    translation_weight = 10.,
    rotation_weight = 40.,
    ceres_solver_options = {
      use_nonmonotonic_steps = false,
      max_num_iterations = 20,
      num_threads = 1,
    },
  },

  -- 为了防止子图里插入太多数据, 在插入子图之前之前对数据进行过滤
  motion_filter = {
    max_time_seconds = 5.,
    max_distance_meters = 0.2,
    max_angle_radians = math.rad(1.),
  },

  -- TODO(schwoere,wohe): Remove this constant. This is only kept for ROS.
  imu_gravity_time_constant = 10.,

  -- 位姿预测器
  pose_extrapolator = {
    use_imu_based = false,
    constant_velocity = {
      imu_gravity_time_constant = 10.,
      pose_queue_duration = 0.001,
    },
    imu_based = {
      pose_queue_duration = 5.,
      gravity_constant = 9.806,
      pose_translation_weight = 1.,
      pose_rotation_weight = 1.,
      imu_acceleration_weight = 1.,
      imu_rotation_weight = 1.,
      odometry_translation_weight = 1.,
      odometry_rotation_weight = 1.,
      solver_options = {
        use_nonmonotonic_steps = false;
        max_num_iterations = 10;
        num_threads = 1;
      },
    },
  },

  -- 子图相关的一些配置
  submaps = {
    num_range_data = 90,          -- 一个子图里插入雷达数据的个数的一半
    grid_options_2d = {
      grid_type = "PROBABILITY_GRID", -- 地图的种类, 还可以是tsdf格式
      resolution = 0.05,
    },
    range_data_inserter = {
      range_data_inserter_type = "PROBABILITY_GRID_INSERTER_2D",
      -- 概率占用栅格地图的一些配置
      probability_grid_range_data_inserter = {
        insert_free_space = true,
        hit_probability = 0.55,
        miss_probability = 0.49,
      },
      -- tsdf地图的一些配置
      tsdf_range_data_inserter = {
        truncation_distance = 0.3,
        maximum_weight = 10.,
        update_free_space = false,
        normal_estimation_options = {
          num_normal_samples = 4,
          sample_radius = 0.5,
        },
        project_sdf_distance_to_scan_normal = true,
        update_weight_range_exponent = 0,
        update_weight_angle_scan_normal_to_ray_kernel_bandwidth = 0.5,
        update_weight_distance_cell_to_hit_kernel_bandwidth = 0.5,
      },
    },
  },
}
## 后端参数 
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104

POSE_GRAPH = {
  -- 每隔多少个节点执行一次后端优化
  optimize_every_n_nodes = 90,

  -- 约束构建的相关参数
  constraint_builder = {
    sampling_ratio = 0.3,                 -- 对局部子图进行回环检测时的计算频率, 数值越大, 计算次数越多
    max_constraint_distance = 15.,        -- 对局部子图进行回环检测时能成为约束的最大距离
    min_score = 0.55,                     -- 对局部子图进行回环检测时的最低分数阈值
    global_localization_min_score = 0.6,  -- 对整体子图进行回环检测时的最低分数阈值
    loop_closure_translation_weight = 1.1e4,
    loop_closure_rotation_weight = 1e5,
    log_matches = true,                   -- 打印约束计算的log
    
    -- 基于分支定界算法的2d粗匹配器
    fast_correlative_scan_matcher = {
      linear_search_window = 7.,
      angular_search_window = math.rad(30.),
      branch_and_bound_depth = 7,
    },

    -- 基于ceres的2d精匹配器
    ceres_scan_matcher = {
      occupied_space_weight = 20.,
      translation_weight = 10.,
      rotation_weight = 1.,
      ceres_solver_options = {
        use_nonmonotonic_steps = true,
        max_num_iterations = 10,
        num_threads = 1,
      },
    },

    -- 基于分支定界算法的3d粗匹配器
    fast_correlative_scan_matcher_3d = {
      branch_and_bound_depth = 8,
      full_resolution_depth = 3,
      min_rotational_score = 0.77,
      min_low_resolution_score = 0.55,
      linear_xy_search_window = 5.,
      linear_z_search_window = 1.,
      angular_search_window = math.rad(15.),
    },

    -- 基于ceres的3d精匹配器
    ceres_scan_matcher_3d = {
      occupied_space_weight_0 = 5.,
      occupied_space_weight_1 = 30.,
      translation_weight = 10.,
      rotation_weight = 1.,
      only_optimize_yaw = false,
      ceres_solver_options = {
        use_nonmonotonic_steps = false,
        max_num_iterations = 10,
        num_threads = 1,
      },
    },
  },

  matcher_translation_weight = 5e2,
  matcher_rotation_weight = 1.6e3,

  -- 优化残差方程的相关参数
  optimization_problem = {
    huber_scale = 1e1,                -- 值越大,（潜在）异常值的影响就越大
    acceleration_weight = 1.1e2,      -- 3d里imu的线加速度的权重
    rotation_weight = 1.6e4,          -- 3d里imu的旋转的权重
    
    -- 前端结果残差的权重
    local_slam_pose_translation_weight = 1e5,
    local_slam_pose_rotation_weight = 1e5,
    -- 里程计残差的权重
    odometry_translation_weight = 1e5,
    odometry_rotation_weight = 1e5,
    -- gps残差的权重
    fixed_frame_pose_translation_weight = 1e1,
    fixed_frame_pose_rotation_weight = 1e2,
    fixed_frame_pose_use_tolerant_loss = false,
    fixed_frame_pose_tolerant_loss_param_a = 1,
    fixed_frame_pose_tolerant_loss_param_b = 1,

    log_solver_summary = false,
    use_online_imu_extrinsics_in_3d = true,
    fix_z_in_3d = false,
    ceres_solver_options = {
      use_nonmonotonic_steps = false,
      max_num_iterations = 50,
      num_threads = 7,
    },
  },

  max_num_final_iterations = 200,   -- 在建图结束之后执行一次全局优化, 不要求实时性, 迭代次数多
  global_sampling_ratio = 0.003,    -- 纯定位时候查找回环的频率
  log_residual_histograms = true,
  global_constraint_search_after_n_seconds = 10., -- 纯定位时多少秒执行一次全子图的约束计算

  --  overlapping_submaps_trimmer_2d = {
  --    fresh_submaps_count = 1,
  --    min_covered_area = 2,
  --    min_added_submaps_count = 5,
  --  },
}

# 参数调整经验 ## 准备工作 启动机器地盘，通过rosbag采集机器人里程计、激光雷达以及tf信息供后续调试使用。 1、检查rosbag数据，确认数据无误 
1
rosrun cartographer_ros cartographer_rosbag_validate -bag_filename test.bag
2、里程计状态信息确认，位置和角度准确度（本地SLAM（前端）和全局SLAM（后端）均需要根据里程计准确度进行调整） ## 前端参数 1、关闭全局SLAM，对本地SLAM（trajectory_builder_2d.lua）进行参数调整。 
1
optimize_every_n_nodes = 0
2、设置子图大小
子图必须足够小，以使其内部的漂移低于分辨率，以便它们在本地正确、另一方面，它们应该足够大以使环路闭合能够正常工作。

3、CeresScanMatcher调整
将CeresScanMatcher中的translation_weight和rotation_weight调到极大，可以看到优化后的轨迹和里程计完全相同，此时主要依靠里程计轨迹建图，CeresScanMatcher作用不大。由于里程计存在误差所以建图效果并不好，因此需要降低里程计在轨迹优化中的权重。里程计精度高则不需要。

后端参数

1、开启全局SLAM，对全局SLAM进行参数调整 

1
optimize_every_n_nodes = 70 #一般为子图大小两倍
2、前端与传感器优化权重配置 
1
2
3
4
POSE_GRAPH.optimization_problem.local_slam_pose_translation_weight 本地SLAM平移权重
POSE_GRAPH.optimization_problem.local_slam_pose_rotation_weight 本地SLAM旋转权重
POSE_GRAPH.optimization_problem.odometry_translation_weight 里程计平移权重
POSE_GRAPH.optimization_problem.odometry_rotation_weight 里程计旋转权重
以上权重参数越大其对全局SLAM的影响越大，里程计精度越高其权重也可适当调高，反之则降低其权重，使得全局优化更加依赖回环检测的结果。
3、适当增加min_score和huber_scale的大小，增加对机器人当前运动状态的置信度。
1
2
POSE_GRAPH.constraint_builder.min_score = 0.75
POSE_GRAPH.optimization_problem.huber_scale = 1e2
4、对于环境相似的场景，减小闭环检测窗口大小减小错误匹配发生的概率 
1
2
SPARSE_POSE_GRAPH.constraint_builder.fast_correlative_scan_matcher.linear_search_window = 6.
SPARSE_POSE_GRAPH.constraint_builder.fast_correlative_scan_matcher.angular_search_window = math.rad(20.)

系统实时性调整

1、降低前端延迟

  • 增加 voxel_filter_size（体素滤波的立方体的边长）
  • 增加 submaps.resolution（子图分辨率）
  • 对于自适应体素滤波, 降低 .min_num_points（如果存在 较多点 并且大于’min_num_points’, 则减小体素长度以尝试获得该最小点数）, .max_range（距远离原点超过max_range 的点被移除）, 增加 .max_length（尝试确定最佳的立方体边长）
  • 降低 max_range (especially if data is noisy)
  • 降低 submaps.num_range_data

2、降低后端延迟 - 降低 optimize_every_n_nodes - 增加 MAP_BUILDER.num_background_threads up to the number of cores - 降低 global_sampling_ratio（纯定位时候查找回环的频率） - 降低 constraint_builder.sampling_ratio（对局部子图进行回环检测时的计算频率） - 增加 constraint_builder.min_score（对局部子图进行回环检测时的最低分数阈值） - 对于自适应体素滤波, 降低 .min_num_points, .max_range, 增加 .max_length - 增加 voxel_filter_size, submaps.resolution, 降低 submaps.num_range_data - 降低搜索窗口大小, .linear_xy_search_window, .linear_z_search_window, .angular_search_window - 增加 global_constraint_search_after_n_seconds（纯定位时多少秒执行一次全子图的约束计算） - 降低 max_num_iterations（求解器迭代次数） # 地图保存脚本 

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
#!/bin/bash
# 刷新环境
source carto_ws/install_isolated/setup.sh 

# 地图保存路径
map_dir="${HOME}/map"      
# 地图名称：当前系统时间
map_name=$(date +%Y%m%d_%H-%M-%S)  

# 检测文件夹是否存在
if [ ! -d "$map_dir" ];then
  echo "文件夹不存在, 即将创建文件夹"
  mkdir -p $map_dir
fi

# 结束建图
rosservice call /finish_trajectory 0

# 生成 pbstream 文件
rosservice call /write_state "{filename: '$map_dir/$map_name.pbstream'}"

# pbstream 转换成 pgm 地图保存
rosrun cartographer_ros cartographer_pbstream_to_ros_map \
-pbstream_filename=$map_dir/$map_name.pbstream \
-map_filestem=$map_dir/$map_name

发表于 更新于 分类于
本文字数： 5.9k 阅读时长 ≈ 5 分钟 
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
  <!--参数配置看https://www.cnblogs.com/dyan1024/p/7825988.html-->
<launch>

  <node pkg="amcl" type="amcl" name="amcl" output="screen">
    <!-- Publish scans from best pose at a max of 10 Hz -->

    //全部滤波器参数
    <param name="min_particles" value="500"/>   //允许的粒子数量的最小值，默认100
    <param name="max_particles" value="5000"/> //允许的例子数量的最大值，默认5000
    <param name="kld_err" value="0.05"/>    //真实分布和估计分布之间的最大误差，默认0.01
    <param name="kld_z" value="0.99"/>   //上标准分位数（1-p），其中p是估计分布上误差小于kld_err的概率，默认0.99
    <param name="update_min_d" value="0.2"/>   //在执行滤波更新前平移运动的距离，默认0.2m(对于里程计模型有影响，模型中根据运动和地图求最终位姿的释然时丢弃了路径中的相关所有信息，已知的只有最终位姿，为了规避不合理的穿过障碍物后的非零似然，这个值建议不大于机器人半径。否则因更新频率的不同可能产生完全不同的结果)
    <param name="update_min_a" value="0.5"/>   //执行滤波更新前旋转的角度，默认pi/6 rad
    <param name="resample_interval" value="1"/>   //在重采样前需要的滤波更新的次数,默认2
    <param name="transform_tolerance" value="0.1"/>  //tf变换发布推迟的时间，为了说明tf变换在未来时间内是可用的
    <param name="recovery_alpha_slow" value="0.0"/> //慢速的平均权重滤波的指数衰减频率，用作决定什么时候通过增加随机位姿来recover，默认0（disable），可能0.001是一个不错的值
    <param name="recovery_alpha_fast" value="0.0"/>  //快速的平均权重滤波的指数衰减频率，用作决定什么时候通过增加随机位姿来recover，默认0（disable），可能0.1是个不错的值
    <param name="gui_publish_rate" value="10.0"/>  //扫描和路径发布到可视化软件的最大频率，设置参数为-1.0意为失能此功能，默认-1.0
    <param name="save_pose_rate" value="0.5"/>  //存储上一次估计的位姿和协方差到参数服务器的最大速率。被保存的位姿将会用在连续的运动上来初始化滤波器。-1.0失能。
    <param name="use_map_topic" value="false"/>  //当设置为true时，AMCL将会订阅map话题，而不是调用服务返回地图。也就是说，当设置为true时，有另外一个节点实时的发布map话题，也就是机器人在实时的进行地图构建，并供给amcl话题使用；当设置为false时，通过map server，也就是调用已经构建完成的地图。在navigation 1.4.2中新加入的参数。
    <param name="first_map_only" value="false"/>  //当设置为true时，AMCL将仅仅使用订阅的第一个地图，而不是每次接收到新的时更新为一个新的地图，在navigation 1.4.2中新加入的参数。

    //激光模型参数
    <param name="laser_min_range" value="-1.0"/>  //被考虑的最小扫描范围；参数设置为-1.0时，将会使用激光上报的最小扫描范围
    <param name="laser_max_range" value="-1.0"/>  //被考虑的最大扫描范围；参数设置为-1.0时，将会使用激光上报的最大扫描范围
    <param name="laser_max_beams" value="30"/>   //更新滤波器时，每次扫描中多少个等间距的光束被使用（减小计算量，测距扫描中相邻波束往往不是独立的可以减小噪声影响，太小也会造成信息量少定位不准）

    <param name="laser_z_hit" value="0.5"/> //模型的z_hit部分的混合权值，默认0.95(混合权重1.具有局部测量噪声的正确范围--以测量距离近似真实距离为均值，其后laser_sigma_hit为标准偏差的高斯分布的权重)
    <param name="laser_z_short" value="0.05"/> //模型的z_short部分的混合权值，默认0.1（混合权重2.意外对象权重（类似于一元指数关于y轴对称0～测量距离（非最大距离）的部分：--ηλe^(-λz)，其余部分为0，其中η为归一化参数，λ为laser_lambda_short,z为t时刻的一个独立测量值（一个测距值，测距传感器一次测量通常产生一系列的测量值）），动态的环境，如人或移动物体）
    <param name="laser_z_max" value="0.05"/> //模型的z_max部分的混合权值，默认0.05（混合权重3.测量失败权重（最大距离时为1，其余为0），如声呐镜面反射，激光黑色吸光对象或强光下的测量，最典型的是超出最大距离）
    <param name="laser_z_rand" value="0.5"/> //模型的z_rand部分的混合权值，默认0.05（混合权重4.随机测量权重--均匀分布（1平均分布到0～最大测量范围），完全无法解释的测量，如声呐的多次反射，传感器串扰）
    <param name="laser_sigma_hit" value="0.2"/> //被用在模型的z_hit部分的高斯模型的标准差，默认0.2m
    <param name="laser_lambda_short" value="0.1"/> //模型z_short部分的指数衰减参数，默认0.1（根据ηλe^(-λz)，λ越大随距离增大意外对象概率衰减越快）
    <param name="laser_likelihood_max_dist" value="2.0"/> //地图上做障碍物膨胀的最大距离，用作likelihood_field模型（likelihood_field_range_finder_model只描述了最近障碍物的距离，（目前理解应该是在这个距离内的障碍物膨胀处理,但是算法里又没有提到膨胀，不明确是什么意思）.这里算法用到上面的laser_sigma_hit。似然域计算测量概率的算法是将t时刻的各个测量（舍去达到最大测量范围的测量值）的概率相乘，单个测量概率：Zh * prob(dist,σ) +avg，Zh为laser_z_hit,avg为均匀分布概率，dist最近障碍物的距离，prob为0为中心标准方差为σ（laser_sigma_hit）的高斯分布的距离概率）
    <param name="laser_model_type" value="likelihood_field"/> //模型使用，可以是beam, likehood_field, likehood_field_prob（和likehood_field一样但是融合了beamskip特征--官网的注释），默认是“likehood_field” 

    //里程计模型参数
    <!--
    ×××里程计模型并没有涉及机器人漂移或打滑的情况，一旦出现这样的情况，后续定位基本废了，虽然Augmented_MCL有失效恢复，但是实际运行中耗时太长且结果不太理想（位置居然跳，这很不合理，可能参数配置不太好）
    -->
    <param name="odom_model_type" value="diff"/> //模型使用，可以是"diff", "omni", "diff-corrected", "omni-corrected",后面两  个是对老版本里程计模型的矫正，相应的里程计参数需要做一定的减小
    <param name="odom_alpha1" value="0.2"/> //指定由机器人运动部分的旋转分量估计的里程计旋转的期望噪声，默认0.2（旋转存在旋转噪声）
    <param name="odom_alpha2" value="0.2"/> //制定由机器人运动部分的平移分量估计的里程计旋转的期望噪声，默认0.2（旋转中可能出现平移噪声）
    <!-- translation std dev, m -->
    <param name="odom_alpha3" value="0.8"/> //指定由机器人运动部分的平移分量估计的里程计平移的期望噪声，默认0.2（类似上）
    <param name="odom_alpha4" value="0.2"/> //指定由机器人运动部分的旋转分量估计的里程计平移的期望噪声，默认0.2（类似上）
    <param name="odom_alpha5" value="0.1"/> //平移相关的噪声参数（仅用于模型是“omni”的情况--wiki官网的注释）
    <param name="odom_frame_id" value="odom"/>  //里程计默认使用的坐标系
    <param name="base_frame_id" value="base_link"/>  //用作机器人的基坐标系
    <param name="global_frame_id" value="map"/>  //由定位系统发布的坐标系名称
    <param name="tf_broadcast" value="true"/>  //设置为false阻止amcl发布全局坐标系和里程计坐标系之间的tf变换

    //机器人初始化数据设置
    <param name="initial_pose_x" value="0.0"/> //初始位姿均值（x），用于初始化高斯分布滤波器。（initial_pose_参数决定撒出去的初始位姿粒子集范围中心）
    <param name="initial_pose_y" value="0.0"/> //初始位姿均值（y），用于初始化高斯分布滤波器。（同上）
    <param name="initial_pose_a" value="0.0"/> //初始位姿均值（yaw），用于初始化高斯分布滤波器。（粒子朝向）
    <param name="initial_cov_xx" value="0.5*0.5"/> //初始位姿协方差（x*x），用于初始化高斯分布滤波器。（initial_cov_参数决定初始粒子集的范围）
    <param name="initial_cov_yy" value="0.5*0.5"/> //初始位姿协方差（y*y），用于初始化高斯分布滤波器。（同上）
    <param name="initial_cov_aa" value="(π/12)*(π/12)"/> //初始位姿协方差（yaw*yaw），用于初始化高斯分布滤波器。（粒子朝向的偏差）
  </node>
</launch>

发表于 更新于 分类于
本文字数： 5.3k 阅读时长 ≈ 5 分钟

teb参数含义 

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
 #*******************************************************************************
 # Trajectory
 #*******************************************************************************
 teb_autosize: True #优化期间允许改变轨迹的时域长度
 #TEB通过状态搜索树寻找最优路径，而dt_ref则是最优路径上的两个相邻姿态
 #（即位置、速度、航向信息，可通过TEB可视化在rivz中看到）的默认距离
 #此距离不固定，规划器自动根据速度大小调整这一距离，速度越大，相邻距离自然越大，
 #较小的值理论上可提供更高精度
 dt_ref: 0.3               #局部路径规划的解析度
 dt_hysteresis: 0.1        #允许改变的时域解析度的浮动范围， 一般为 dt_ref 的 10% 左右;
 max_samples: 200          #
 global_plan_overwrite_orientation: True      #覆盖全局路径中局部路径点的朝向
 allow_init_with_backwards_motion: False      #允许在开始时想后退来执行轨迹
 #考虑优化的全局计划子集的最大长度（累积欧几里得距离）
 #如果为0或负数：禁用；长度也受本地Costmap大小的限制
 max_global_plan_lookahead_dist: 2.5
 global_plan_viapoint_sep: -1  #从全局路径中选取的每两个连续通过点之间的最小间隔
 global_plan_prune_distance: 1  #该参数决定了从机器人当前位置的后面一定距离开始裁剪
 #如果为真，规划器在速度、加速度和转弯率计算中使用精确的弧长[增加的CPU时间]，否则使用欧几里德近似。
 exact_arc_length: False
 #在判断生成的轨迹是否冲突时使用，此时设置为3，即从轨迹起点开始逐个检查轨迹上的3个点，
 #若3个点均不发生碰撞，则认为本次轨迹有效。若小于0则检查所有路径点
 feasibility_check_no_poses: 3   #检测位姿可到达的时间间隔
 publish_feedback: False        #发布包含完整轨迹和活动障碍物列表的规划器反馈

#*******************************************************************************
  # Robot
#*******************************************************************************
 max_vel_x: 0.3             #最大x前向速度
 max_vel_x_backwards: 0.05  #最大x后退速度
 max_vel_y: 0.0             #最大y方向速度
 max_vel_theta: 0.25        #最大转向角速度
 acc_lim_x: 0.15            #最大x加速度
 acc_lim_theta: 0.15        #最大角速度
 min_turning_radius: 0.0    #车类机器人的最小转弯半径


 # types: "point", "circular", "two_circles", "line", "polygon"
 footprint_model: 
#type: "polygon"    #多边形
#vertices: [[-0.3, -0.2], [0.3, -0.2], [0.3, 0.2], [-0.3, 0.2]]
  type: "circular"   #圆
  radius: 0.25 # for type "circular"
 #line_start: [0.0, 0.0] # for type "line" #直线
 #line_end: [0.4, 0.0] # for type "line"
 #front_offset: 0.2 # for type "two_circles" 
 #front_radius: 0.2 # for type "two_circles"
 #rear_offset: 0.2 # for type "two_circles"
 #rear_radius: 0.2 # for type "two_circles"


 #*******************************************************************************
   # Obstacles
 #*******************************************************************************
 min_obstacle_dist: 0.22                     #和障碍物最小距离
 inflation_dist: 0.2                         #障碍物膨胀距离
 include_costmap_obstacles: True             #costmap 中的障碍物是否被直接考虑
 costmap_obstacles_behind_robot_dist: 0.5    #规划时考虑后面n米内的障碍物
 obstacle_poses_affected: 10                 #障碍物姿态受影响程度0-30

 dynamic_obstacle_inflation_dist: 0.4      #动态障碍物的膨胀范围
 include_dynamic_obstacles: False          #是否将动态障碍物预测为速度模型


#*******************************************************************************
   # Optimization
 #*******************************************************************************
 no_inner_iterations: 5         #被外循环调用后内循环执行优化次数
 no_outer_iterations: 4         #执行的外循环的优化次数执行的外循环的优化次数
 optimization_activate: True    #激活优化
 optimization_verbose: False    #打印优化过程详情
 penalty_epsilon: 0.1           #对于硬约束近似，在惩罚函数中添加安全范围
 obstacle_cost_exponent: 4
 weight_max_vel_x: 2            #最大x速度权重 0~2
 weight_max_vel_theta: 1        #最大角速度权重 0~1
 weight_acc_lim_x: 1            #最大x 加速度权重 0~1
 weight_acc_lim_theta: 1        #最大角速度权重 0~1
 weight_kinematics_nh: 1000     #非完整运动学的优化权重
 weight_kinematics_forward_drive: 5     #优化过程中，迫使机器人只选择前进方向，差速轮适用
 weight_kinematics_turning_radius: 1    #优化过程中，车型机器人的最小转弯半径的权重
 weight_optimaltime: 0.3 # must be > 0  #优化过程中，基于轨迹的时间上的权重
 weight_shortest_path: 0
 weight_obstacle: 100           #优化过程中，和障碍物最小距离的权重 0~50
 weight_inflation: 0.3          #优化过程中， 膨胀区的权重
 weight_dynamic_obstacle: 10    #优化过程中，和动态障碍物最小距离的权重
 weight_dynamic_obstacle_inflation: 0.2  #优化过程中，和动态障碍物膨胀区的权重 0~50
 weight_viapoint: 1             #优化过程中，和全局路径采样点距离的权重
 weight_adapt_factor: 2

 #*******************************************************************************
 # Homotopy Class Planner
 #*******************************************************************************
 enable_homotopy_class_planning: true   #激活并行规划
 enable_multithreading: True            #允许多线程并行处理
 max_number_classes: 3                  #考虑到的不同轨迹的最大数量
 selection_cost_hysteresis: 1.0
 selection_prefer_initial_plan: 0.9
 selection_obst_cost_scale: 100.0
 selection_alternative_time_cost: False

 roadmap_graph_no_samples: 15       #指定为创建路线图而生成的样本数
 roadmap_graph_area_width: 6        #指定该区域的宽度
 roadmap_graph_area_length_scale: 1.0
 h_signature_prescaler: 0.5         #（0.2 < value <= 1）缩放用于区分同伦类的内部参数
 h_signature_threshold: 0.1         #如果实部和复部的差都低于规定的阈值，则假定两个h签名相等。
 obstacle_heading_threshold: 0.45   #在障碍物航向和目标航向之间指定标量乘积的值，以便将障碍物考虑在内进行探索
 switching_blocking_period: 0.0     #指定允许切换到新的等效类之前需要终止的持续时间
 #为true，则将不同拓扑的所有轨迹附加到该组通孔点，否
 #则仅将与初始/全局计划共享相同拓扑的轨迹与它们连接 (对test_optim_node无效)
 viapoints_all_candidates: True 
 delete_detours_backwards: True 
 max_ratio_detours_duration_best_duration: 3.0
 visualize_hc_graph: False          #可视化创建的图形，用于探索不同的轨迹
 visualize_with_time_as_z_axis_scale: False  #在rviz里可看到优化使用的graph

 #*******************************************************************************
 # Recovery
 #*******************************************************************************
 #当规划器检测到系统异常，允许缩小时域规划范围,TEB将以更近的点作为规划目标，尝试重新规划出可行路径;
 shrink_horizon_backup: false         
 shrink_horizon_min_duration: 10  #如果检测到不可行的轨迹，激活缩小的水平线后备模式，本参数为其最短持续时间。
 oscillation_recovery: True       #尝试检测和解决振荡
oscillation_v_eps: 0.1          #(0,1)内的 normalized 线速度的平均值的阈值，判断机器人是否运动异常
oscillation_omega_eps: 0.1      #(0,1)内的 normalized 角速度的平均值，判断机器人是否运动异常
oscillation_recovery_min_duration: 10  #在这个时间内，是否再次发生FailureDetector检测的振荡
oscillation_filter_duration: 10  #failure_detector_中buffer容器的大小为oscillation_filter_duration * controller_frequency

发表于 更新于 分类于
本文字数： 3k 阅读时长 ≈ 3 分钟

costmap_common_params

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
max_obstacle_height: 2.0  #考虑最大障碍物高度范围

# Obstacle Cost Shaping (http://wiki.ros.org/costmap_2d/hydro/inflation)
footprint: [[0.35,-0.25],[0.35,0.25],[-0.35,0.25],[-0.35,-0.25]] #机器轮廓

static_layer:
  enabled:              true    #是否启用该层地图
  map_topic:            "/map"
  first_map_only:       false   #是否根据map_server提供的地图初始化
#障碍物层
obstacle_layer:
  enabled:              true
  max_obstacle_height:  2.0
  min_obstacle_height:  0.0
  combination_method:   1
  track_unknown_space:  true    #设置为true如果允许全局路径通过未知区域
  obstacle_range: 2.5     #机器人检测障碍物的最大范围  
  raytrace_range: 3       #在机器人移动过程中，实时清除代价地图上的障碍物的最大范围，更新可自由移动的空间数据。假如设置该值为3米，那么就意味着在3米内的障碍物，本来开始时是有的，但是本次检测却没有了，那么就需要在代价地图上来更新，将旧障碍物的空间标记为可以自由移动的空间。
  publish_voxel_map: false
  observation_sources:  scan
  scan:
    data_type: LaserScan
    topic: "/scan"
    marking: true
    clearing: true
    inf_is_valid: true
    expected_update_rate: 10 #20
#深度相机层
depth_layer:
  enabled:              true
  max_obstacle_height:  2.0
  min_obstacle_height:  0.0
  combination_method:   1
  track_unknown_space:  true    
  obstacle_range: 2.5 #2.0
  raytrace_range: 5 #5.0
  publish_voxel_map: false
  observation_sources:  scan 
  scan:
    data_type: LaserScan
    topic: "/wr_scan3"
    marking: true
    clearing: true
    inf_is_valid: true
    expected_update_rate: 5 #20

#超声波层
sonar_layer:
  enabled:            true
  clear_threshold:    0.8
  mark_threshold:     0.98
  topics: ["/ultrasonic_front_left", "/ultrasonic_front_right"]   
  clear_on_max_reading: true
#虚拟地图层(虚拟墙，禁止线，跌落风险区) 
virtual_layer:
  enabled:              true
  Forbidden_line:       [/virtual_costamp_layer/Forbidden_line]
  Drop_risk_areas:      [/virtual_costamp_layer/Drop_risk_areas]
  one_zone:             true


#膨胀层
inflation_layer:
  enabled:              true
  cost_scaling_factor:  10 #5 #10.0  # 代价变化率(default: 10)
  inflation_radius:     0.05 #0.50  # 膨胀半径

global_costmap_params

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
global_costmap:
   global_frame: map
   robot_base_frame: base_link
   update_frequency: 1 #更新频率
   publish_frequency: 1 #可视化发布频率
   static_map: true #是否为静态地图
   rolling_window: false
   inflation_radius: 0.8 #膨胀半径
   cost_scaling_factor: 2.0
   trace_unknow_space: true
   transform_tolerance: 1 #tf超时阈值
   plugins:
    - {name: static_layer,      type: "costmap_2d::StaticLayer"}
    - {name: virtual_layer,      type: "virtual_costmap_layer::VirtualLayer" }
    - {name: inflation_layer,     type: "costmap_2d::InflationLayer"}

local_costmap_params

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
local_costmap:
   global_frame: map #全局坐标系
   robot_base_frame: base_link #机器坐标系
   update_frequency:  10 #更新频率
   publish_frequency: 1 #可视化发布频率
   static_map: false
   rolling_window: true
   width:  5
   height: 5
   resolution: 0.05
   inflation_radius:     0.4
   transform_tolerance: 1 
   plugins:
    - {name: static_layer,      type: "costmap_2d::StaticLayer"}
    - {name: sonar_layer,         type: "range_sensor_layer::RangeSensorLayer"}
    - {name: obstacle_layer,      type: "costmap_2d::ObstacleLayer"}
    - {name: depth_layer,     type: "costmap_2d::ObstacleLayer"}
    - {name: virtual_layer,      type: "virtual_costmap_layer::VirtualLayer" }
    - {name: inflation_layer,     type: "costmap_2d::InflationLayer"}

发表于 更新于 分类于
本文字数： 5.5k 阅读时长 ≈ 5 分钟

卡尔曼滤波器（Kalman Filter）原理简述

卡尔曼滤波器是一种用于估计动态系统状态的递归算法，特别适用于噪声较大的测量系统。它广泛应用于导航、跟踪和控制系统中。其核心思想是通过预测和更新两个步骤，结合系统模型和测量数据，提供最优的状态估计。

基本原理

卡尔曼滤波器的基本原理包括状态预测和测量更新两个步骤。它通过不断修正预测值和测量值来实现对系统状态的最优估计。

1. 系统模型

卡尔曼滤波器假设系统可以用线性模型描述，包括状态方程和观测方程：

  • 状态方程：
    其中， 是在时间 的状态向量， 是状态转移矩阵， 是控制输入矩阵， 是控制输入， 是过程噪声，假设为均值为零、协方差为 的高斯白噪声。

  • 观测方程：
    其中， 是在时间 的观测向量， 是观测矩阵， 是观测噪声，假设为均值为零、协方差为 的高斯白噪声。

2. 预测步骤

在预测步骤中，根据上一时刻的状态估计和系统模型，对当前时刻的状态进行预测：

  • 状态预测：

  • 误差协方差预测：

3. 更新步骤

在更新步骤中，利用当前时刻的观测值对预测的状态进行修正：

  • 计算卡尔曼增益：

  • 状态更新：

  • 误差协方差更新：

结论

卡尔曼滤波器通过结合系统模型的预测和实际观测的更新，提供了一个在最小均方误差意义下最优的状态估计方法。其高效的递归计算方式，使得卡尔曼滤波器在实时应用中尤为适用。

扩展卡尔曼滤波器（EKF）原理简述

扩展卡尔曼滤波器（Extended Kalman Filter，EKF）是一种用于非线性系统状态估计的滤波算法，是卡尔曼滤波器（KF）的推广。EKF通过线性化非线性系统模型来实现对系统状态的估计。

基本原理

EKF的基本原理可以分为预测和更新两个步骤，与标准卡尔曼滤波器类似，但在处理非线性系统时有所不同。具体过程如下：

1. 系统模型

非线性系统可以表示为以下状态方程和观测方程：

  • 状态方程：

  • 观测方程：

其中， 是系统在时间 的状态， 是观测值， 和 分别是过程噪声和观测噪声，假设它们是均值为零的高斯白噪声。

2. 预测步骤

在预测步骤中，利用系统的状态方程对当前状态进行预测：

  • 状态预测：

  • 误差协方差预测：

    其中， 是对状态方程 的雅可比矩阵：

3. 更新步骤

在更新步骤中，利用观测方程对预测的状态进行修正：

  • 计算卡尔曼增益：

    其中， 是对观测方程 的雅可比矩阵：

  • 状态更新：

  • 误差协方差更新：

结论

EKF通过在每一步预测和更新中线性化非线性系统模型，成功地将卡尔曼滤波器扩展到非线性系统中。尽管线性化可能引入一定误差，但在许多应用中，EKF仍然是一种有效的状态估计方法。

无迹卡尔曼滤波器（UKF）原理简述

无迹卡尔曼滤波器（Unscented Kalman Filter，UKF）是一种用于非线性系统状态估计的滤波算法。UKF通过无迹变换（Unscented Transform，UT）来处理非线性系统中的状态预测和更新，避免了扩展卡尔曼滤波器（EKF）中的线性化近似误差。

基本原理

UKF的基本原理包括状态预测和测量更新两个步骤。与EKF不同的是，UKF使用一组精心选择的采样点（sigma点）来近似状态分布，从而更准确地捕捉非线性系统的特性。

1. 系统模型

UKF假设系统可以用以下非线性状态方程和观测方程描述：

  • 状态方程：

  • 观测方程：

其中， 是系统在时间 的状态， 是控制输入， 是观测值， 和 分别是过程噪声和观测噪声，假设它们是均值为零的高斯白噪声。

2. 无迹变换

无迹变换的核心思想是通过一组sigma点来近似状态分布。这些sigma点通过以下方式生成：

  • 计算状态均值和协方差的sigma点： 其中， 是第 个sigma点， 是状态维度， 是一个缩放参数， 表示协方差矩阵的第 列的平方根。

  • 通过非线性函数传播sigma点：

  • 计算预测状态均值和协方差：

    其中， 和 是sigma点的权重。

3. 预测步骤

  • 状态预测：

  • 误差协方差预测：

4. 更新步骤

  • 计算观测sigma点：

  • 计算预测观测均值和协方差：

  • 计算状态和观测的交叉协方差：

  • 计算卡尔曼增益：

  • 状态更新：

  • 误差协方差更新：

结论

UKF通过无迹变换和sigma点来处理非线性系统中的状态预测和更新，相比EKF具有更高的精度和鲁棒性。它在很多实际应用中，如导航和跟踪系统，提供了更加准确的状态估计。

以下是卡尔曼滤波器（KF）、扩展卡尔曼滤波器（EKF）和无迹卡尔曼滤波器（UKF）在IMU和里程计数据融合中的对比表：

特性 卡尔曼滤波器（KF） 扩展卡尔曼滤波器（EKF） 无迹卡尔曼滤波器（UKF）
处理系统类型 线性系统 非线性系统 非线性系统
线性化方法 不需要 泰勒展开（一次线性化） 无需线性化，使用无迹变换
计算复杂度 低 中等 高
预测精度 高（线性系统） 较高（轻度非线性系统） 很高（适用于高度非线性系统）
应用范围 线性系统 广泛用于工程实际 高精度需求的非线性系统
实时性 很好 较好 适中
误差特性 无误差（线性系统） 线性化引入误差 无迹变换更准确
计算资源需求 低 中等 高
适用性（IMU和里程计） 不适用 适用 最适用

无迹卡尔曼滤波器（UKF）： - 最适用于IMU和里程计数据融合。 - 能够处理复杂的非线性系统并提供更高的精度。 - 适合在高精度需求的情况下使用，但是计算复杂度较高，需要更多计算资源。

扩展卡尔曼滤波器（EKF）： - 适用于IMU和里程计数据融合。 - 能处理非线性系统，计算复杂度较低，适合实时性要求高、计算资源有限的情况。 - 可能会因线性化过程引入误差，适用于轻度非线性系统。

卡尔曼滤波器（KF）： - 不适用于IMU和里程计数据融合。 - 仅适用于线性系统，无法处理IMU和里程计融合中的非线性问题。

参考资料

发表于 更新于 分类于
本文字数： 3.6k 阅读时长 ≈ 3 分钟

TEB

TEB（Timed Elastic Band）局部路径规划算法是一种用于移动机器人路径规划的实时算法，专注于优化路径的时间和空间特性，以实现平滑且高效的运动。TEB算法通过将全局路径转化为一条时间弹性带，并对其进行优化，使机器人能够在动态环境中避障并到达目标位置。以下是TEB算法的基本原理：

TEB算法原理

1. 路径表示

TEB将路径表示为一系列带有时间约束的位姿（poses），即路径上的每个点都包含位置和时间信息。这样，可以对路径进行空间和时间上的优化。

2. 代价函数的构建

TEB算法通过一个代价函数来评估路径的质量。代价函数通常由以下几个分量构成： - 障碍物距离代价：衡量路径是否避开障碍物，越远离障碍物代价值越低。 - 速度代价：衡量机器人在路径上移动的速度，鼓励较高的速度以提高运动效率，但也需要平滑过渡。 - 加速度代价：衡量路径上速度的变化，鼓励平滑的速度变化，减少加速度和减速度带来的不平稳。 - 时间代价：衡量路径完成所需的时间，鼓励尽快到达目标。

3. 优化过程

TEB算法采用优化方法（如梯度下降、二次规划等）对路径进行迭代优化，以最小化代价函数。优化过程主要包括以下步骤： - 初始化路径：从全局路径规划算法（如A*或Dijkstra）获取初始路径，并对其进行离散化处理，得到初始的位姿序列。 - 时间参数优化：调整路径上每个位姿的时间分配，确保路径在时间上是可行的。 - 空间参数优化：调整路径上每个位姿的空间位置，确保路径在空间上是可行的，并避开障碍物。 - 联合优化：同时优化时间和空间参数，以获得全局最优解。

4. 实时更新

TEB算法在机器人运动过程中，实时更新路径以适应动态环境变化。每个时间步，机器人根据当前状态和环境信息，对路径进行重新优化，确保其能够安全避障并朝向目标前进。

步骤总结

  1. 路径初始化：从全局路径规划算法获取初始路径并离散化。
  2. 代价函数构建：定义障碍物距离代价、速度代价、加速度代价和时间代价等。
  3. 路径优化：采用优化方法对路径进行迭代优化，最小化代价函数。
  4. 实时更新：在机器人运动过程中，实时更新路径以适应动态环境变化。

TEB算法通过对路径的时间和空间参数进行联合优化，实现了机器人在动态环境中的实时路径规划和避障。其优点在于能够生成平滑、高效的路径，并适应动态环境变化，但优化过程可能较为复杂，需要较高的计算资源。 # DWA
DWA（Dynamic Window Approach）局部路径规划算法是一种用于移动机器人避障和路径规划的实时算法。其主要思想是通过在机器人当前速度空间中寻找最优速度，以确保机器人能够安全地避开障碍物并向目标前进。下面是DWA算法的基本原理：

DWA算法原理

1. 动态窗口的定义

动态窗口是一个以当前时间为基础的速度空间，包含了机器人在下一时间步内可以达到的速度。动态窗口受限于以下三个方面： - 运动学约束：机器人在当前时间步内的最大加速度和减速度限制了速度变化范围。 - 动态约束：机器人在当前时间步内能够安全停下所需的速度范围。 - 环境约束：考虑到环境中的障碍物，只有那些在下一时间步内不会导致碰撞的速度才是有效的。

2. 评价函数的构建

DWA算法通过一个评价函数来评估不同速度下的路径质量。评价函数通常由以下几个分量构成： - 目标函数：衡量速度是否朝向目标前进，通常是速度与目标方向的夹角或距离。 - 速度函数：衡量机器人速度的大小，鼓励较高的速度以提高运动效率。 - 障碍物距离函数：衡量路径是否避开障碍物，越远离障碍物得分越高。

3. 搜索最优速度

在动态窗口内，通过遍历所有可能的速度组合，计算每个组合的评价函数值，并选择评价函数值最高的速度作为机器人在下一时间步的速度。

4. 路径跟踪和更新

机器人按照选定的最优速度进行移动，并在每个时间步重复上述过程，实时更新动态窗口和评价函数，逐步朝向目标前进并避开障碍物。

步骤总结

  1. 生成动态窗口：根据当前速度和运动学约束生成可行的速度空间。
  2. 模拟轨迹：对动态窗口内的每个速度组合进行轨迹模拟。
  3. 评价轨迹：使用评价函数对模拟轨迹进行打分。
  4. 选择最优速度：选择评价得分最高的速度作为下一步的运动速度。
  5. 更新状态：机器人按照选定速度移动，更新当前位置和速度，并重复上述过程。

DWA算法通过在速度空间中寻找最优解，实现了机器人在动态环境中的实时路径规划和避障。其优点在于计算效率高，能够适应动态变化的环境，但也存在对复杂环境中的局部最优解问题敏感等缺点。 # MPC
MPC（Model Predictive Control，模型预测控制）是一种高级控制策略，广泛应用于机器人路径规划和运动控制中。MPC通过预测未来一段时间内系统的动态行为，优化控制输入以达到期望目标。以下是MPC局部路径规划算法的基本原理：

MPC算法原理

1. 预测模型

MPC基于系统的数学模型进行预测。对于机器人路径规划，通常使用运动学或动力学模型来描述机器人在未来一段时间内的行为。这些模型可以包括机器人的位置、速度、加速度等状态变量。

2. 预测时域

MPC在每个控制周期内，考虑一个固定的预测时域（prediction horizon），即未来若干时间步内的系统行为。在这个时域内，算法预测系统状态的变化，并计算相应的控制输入。

3. 代价函数的构建

MPC通过一个代价函数来评估不同控制输入的优劣。代价函数通常包含以下几个部分： - 轨迹跟踪误差：衡量机器人实际路径与目标路径之间的偏差，偏差越小代价值越低。 - 控制输入代价：衡量控制输入的大小和变化，鼓励较小的控制输入以实现平滑控制。 - 障碍物距离代价：衡量路径是否避开障碍物，越远离障碍物代价值越低。

4. 优化问题的求解

在每个控制周期内，MPC通过求解一个优化问题来确定最优的控制输入序列。优化问题通常是一个带约束的非线性规划问题，求解过程包括： - 初始化预测：根据当前系统状态，使用预测模型预测未来时域内的系统状态。 - 构建优化问题：根据预测状态和代价函数，构建优化问题。 - 求解优化问题：使用数值优化算法（如QP、SQP等）求解优化问题，获得最优的控制输入序列。

5. 应用控制输入和滚动优化

MPC只应用优化得到的第一个控制输入到系统，然后进入下一个控制周期。这样做的好处是能够实时应对环境变化和模型不确定性。具体步骤如下： 1. 应用控制输入：将优化得到的第一个控制输入应用到机器人系统。 2. 状态更新：根据新的系统状态和传感器信息，更新系统状态。 3. 滚动优化：进入下一个控制周期，重复上述过程。

步骤总结

  1. 预测模型：建立描述机器人动态行为的数学模型。
  2. 预测时域：确定一个固定的预测时域。
  3. 代价函数构建：定义轨迹跟踪误差、控制输入代价和障碍物距离代价。
  4. 优化问题求解：在每个控制周期内，求解优化问题以获得最优控制输入序列。
  5. 应用控制输入和滚动优化：应用第一个控制输入，更新系统状态，进入下一个控制周期。

MPC算法通过在未来时域内预测系统行为和优化控制输入，实现了机器人在动态环境中的实时路径规划和避障。其优点在于能够处理多种约束和目标，具有很高的灵活性和鲁棒性，但计算复杂度较高，需要较强的实时计算能力。

对比及结论

以下是对DWA、TEB和MPC三种局部路径规划算法的对比，从计算资源、适应性和复杂度等方面进行分析，以便说明在计算资源有限的室内场景中哪种算法更适合使用。

特性 DWA（Dynamic Window Approach） TEB（Timed Elastic Band） MPC（Model Predictive Control）
计算复杂度 较低 中等 较高
实时性 高 中等 较低
路径平滑性 一般 高 高
避障能力 较好 好 很好
适应动态环境 较好 很好 很好
实现难度 较低 中等 高
计算资源需求 低 中等 高
鲁棒性 较高 较高 很高

适用性分析

  • DWA：
    • 计算复杂度低，能够在较短时间内计算出合适的速度，适合实时应用。
    • 在避障和路径平滑性上表现一般，但足以应对大多数室内环境。
    • 计算资源需求较低，适合计算资源有限的场景。
  • TEB：
    • 通过优化时间和空间参数，可以生成平滑、高效的路径。
    • 计算复杂度和计算资源需求中等，在路径平滑性和避障能力上有良好表现。
    • 对于动态环境适应性好，但在资源有限的情况下，可能会出现性能瓶颈。
  • MPC：
    • 通过预测未来的系统行为进行优化，生成的路径在避障和平滑性上表现最好。
    • 计算复杂度高，优化问题求解需要较多的计算资源和时间。
    • 虽然具有很高的灵活性和鲁棒性，但在计算资源有限的环境中不太适用。

结论

在计算资源有限的室内场景中，DWA（Dynamic Window Approach） 更适合使用。这是因为DWA算法计算复杂度低，实时性高，对计算资源的需求较少，能够在有限的计算能力下实现较好的路径规划和避障效果。虽然在路径平滑性和优化效果上不如TEB和MPC，但其综合表现和资源适配性使其成为计算资源受限情况下的最佳选择。

参考博客：
路径规划算法对比
dwa原理解析
DWA、TEB、MPC算法优劣对比
DWA算法,TEB算法

发表于 更新于 分类于
本文字数： 649 阅读时长 ≈ 1 分钟

运动模型

图片

误差分析

运动模型的建立主要是为了：1、根据编码器数据求解里程计数据（正运动学）；2、导航建图运动控制（逆运动学）

影响里程计精度的主要因素（这里主要分析系统结构误差，编码器误差由选型决定）： - 速度：驱动轮半径 - 角速度：驱动轮半径和轮间距 - 位置：驱动轮半径和轮间距 - 角度：驱动轮半径和轮间距 

1
2
3
robotPos.X += (robotVel.X * cos(robotPos.Z) - robotVel.Y * sin(robotPos.Z)) * samplingTime; // 计算X方向的位移，单位：m
robotPos.Y += (robotVel.X * sin(robotPos.Z) + robotVel.Y * cos(robotPos.Z)) * samplingTime; // 计算Y方向的位移，单位：m
robotPos.Z += robotVel.Z * samplingTime;                                                    // 绕Z轴的角位移，单位：rad
TODO：编码器误差分析

里程计标定过程

手动校准

1、根据前期结构设计确认轮距和半径
2、半径校准：准备测试场地，让机器沿测量好的直线路线行进，通过里程计与实际距离值，确定最终轮胎半径；
3、轮距校准：机器沿特定闭合路线行驶，保证起始点位置姿态和结束位置完全相同，微调轮距到里程计轨迹完全闭合；

程序校准

直接线性法

https://github.com/KOTOKORURU/odometry_calibration

https://github.com/MegviiRobot/OdomLaserCalibraTool

发表于 更新于 分类于
本文字数： 263 阅读时长 ≈ 1 分钟

功能需求

移动机器人配备的单线激光雷达，只能探测到雷达所在平面的障碍物信息，为了减小视野盲区，需要融合深度相机信息进行避障，实现对不同高度范围障碍物进行感知，进一步结合障碍物信息进行避障；

设计思路

处理流程图

实现方式

基于rtab-map实现

体素滤波
点云融合
障碍物点云分割
深度点云转单线激光雷达数据
代价地图配置：添加深度相机障碍物层 深度相机层配置

基于PCL实现

性能调优

深度点云处理核心问题还是数据量，单个深度相机数据带宽可以达到20MB/S;
1、使用nodelet通过零复制指针传递数据，降低数据传输量；
2、通过体素滤波预处理降低后续处理数据量；