ROS机器人小车建模仿真与SLAM_小车 slam仿真

ROS机器人小车建模仿真与SLAM

一、机器小车建模

1、URDF

• URDF（Unified Robot Description Format）是一种用于描述机器人模型的XML文件格式。在URDF中，Joint（关节）和Link（连杆）是两个重要的概念。

XML是 被设计用来传输和存储数据的可扩展标记语言，注意语言本身是没有含义的，只是规定了其数据格式

比如说下面这段信息：

<robot name="阿童木">
<link name="大手臂">具体的描述</link>
<joint name="胳膊肘">具体的描述</joint>
<link name="小手臂">具体的描述</link>
</robot>Copy to clipboardErrorCopied
1
2
3
4
5

XML格式注释： 标签: robot link

robot标签的属性name: <robot name="fishbot"></robot>

robot标签的子标签link: <robot name="fishbot"><link name="base_link"></link>link></robot>

使用XML定义的一个最简单的URDF模型可以像下面这样

<?xml version="1.0"?>
<robot name="fishbot">
  <link name="base_link">
    <visual>
      <geometry>
        <cylinder length="0.18" radius="0.06"/>
      </geometry>
    </visual>
  </link>
</robot>
1
2
3
4
5
6
7
8
9
10

2、Joint

Joint（关节）：关节是机器人模型中连接两个连杆的部分，它定义了两个连杆之间的相对运动。关节可以描述两个连杆之间的旋转运动（例如旋转关节）或平移运动（例如滑块关节）。关节还可以有不同的类型，如旋转关节、固定关节、滑块关节等。在URDF中，关节通过其类型、连接的两个连杆、相对运动的轴线等参数来定义。

3、Link

Link（连杆）：连杆是机器人模型中的刚性部件，它们通过关节连接在一起形成机器人的结构。每个连杆都有一个名称和一个几何形状（通常是包围盒、球体、柱体等）。连杆之间的关系通过关节来描述，从而构成了机器人的整体结构。

4、实例

下面是一个简单的URDF示例，展示了如何使用Joint和Link来描述一个简单的二连杆机器人模型：

<?xml version="1.0"?>
<robot name="simple_robot">
  
  <link name="link1">
    <visual>
      <geometry>
        <box size="0.1 1 0.1"/>
      </geometry>
    </visual>
  </link>
  
  <link name="link2">
    <visual>
      <geometry>
        <box size="0.1 1 0.1"/>
      </geometry>
    </visual>
  </link>
  
  <joint name="joint1" type="revolute">
    <parent link="link1"/>
    <child link="link2"/>
    <origin xyz="0 0 0.5" rpy="0 0 0"/>
    <axis xyz="0 1 0"/>
    <limit effort="10" lower="-3.1416" upper="3.1416" velocity="5"/>
  </joint>
  
</robot>
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

这里我们定义了两个Link：link1和link2。它们都有一个名称和一个几何形状，和相同的box形状。

然后，我们定义了一个Joint：joint1。这是一个旋转关节（revolute），连接了link1和link2。Joint有一个名称、类型和连接的两个Link。我们还定义了关节的初始姿态（origin）、旋转轴（axis）和运动限制（limit）。

• 机器人建模与仿真：URDF常用于描述各种类型的机器人，包括工业机器人、移动机器人、人形机器人等。这些机器人模型可以用于机器人仿真软件（如ROS中的Gazebo）中进行虚拟环境中的运动学、动力学分析和控制算法的验证。
• 路径规划与运动控制：在机器人路径规划和运动控制中，URDF模型可以提供机器人的几何信息、关节限制等重要参数，帮助规划算法生成机器人的运动轨迹，并执行相应的控制策略。
• 机器人视觉与感知：URDF模型也可以用于机器人的视觉感知任务，如目标检测、目标跟踪等。通过将机器人的URDF模型与摄像头数据进行配准，可以实现更准确的目标定位和识别。

5、RVIZ2可视化工具的使用

• 建立工具包

先创建一个chapt8_ws工作空间，然后建立功能包，包的类型选ament_python

ros2 pkg create fishbot_description --build-type ament_pythonCopy to clipboardErrorCopied
1

• 建立URDF文件

建立URDF文件夹，创建urdf文件

cd fishbot_description && mkdir urdf 
touch fishbot_base.urdfCopy to clipboardErrorCopied
1
2

完成后src下的目录结构：

├── fishbot_description
│   ├── __init__.py
├── package.xml
├── setup.cfg
├── setup.py
└── urdf
    └── fishbot_base.urdfCopy to clipboardErrorCopied
1
2
3
4
5
6
7

编辑fishbot_base.urdf

<?xml version="1.0"?>
<robot name="fishbot">
    
  <!-- base link -->
  <link name="base_link">
      <visual>
      <origin xyz="0 0 0.0" rpy="0 0 0"/>
      <geometry>
        <cylinder length="0.12" radius="0.10"/>
      </geometry>
    </visual>
  </link>
    
  <!-- laser link -->
  <link name="laser_link">
      <visual>
      <origin xyz="0 0 0" rpy="0 0 0"/>
      <geometry>
        <cylinder length="0.02" radius="0.02"/>
      </geometry>
      <material name="black">
          <color rgba="0.0 0.0 0.0 0.8" /> 
      </material>
    </visual>
  </link>
    
  <!-- laser joint -->
    <joint name="laser_joint" type="fixed">
        <parent link="base_link" />
        <child link="laser_link" />
        <origin xyz="0 0 0.075" />
    </joint>

</robot>Copy to clipboardErrorCopied
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

• 建立launch文件

在目录src/fishbot_description下创建launch文件夹并在其下新建display_rviz2.launch.py文件。

mkdir launch
touch display_rviz2.launch.pyCopy to clipboardErrorCopied
1
2

完成后的目录结构：

├── fishbot_description
│   ├── __init__.py
├── launch
│   └── display_rviz2.launch.py
├── package.xml
├── setup.cfg
├── setup.py
└── urdf
    └── fishbot_base.urdfCopy to clipboardErrorCopied
import os
from launch import LaunchDescription
from launch.substitutions import LaunchConfiguration
from launch_ros.actions import Node
from launch_ros.substitutions import FindPackageShare


def generate_launch_description():
    package_name = 'fishbot_description'
    urdf_name = "fishbot_base.urdf"

    ld = LaunchDescription()
    pkg_share = FindPackageShare(package=package_name).find(package_name) 
    urdf_model_path = os.path.join(pkg_share, f'urdf/{urdf_name}')

    robot_state_publisher_node = Node(
        package='robot_state_publisher',
        executable='robot_state_publisher',
        arguments=[urdf_model_path]
        )

    joint_state_publisher_node = Node(
        package='joint_state_publisher_gui',
        executable='joint_state_publisher_gui',
        name='joint_state_publisher_gui',
        arguments=[urdf_model_path]
        )

    rviz2_node = Node(
        package='rviz2',
        executable='rviz2',
        name='rviz2',
        output='screen',
        )

    ld.add_action(robot_state_publisher_node)
    ld.add_action(joint_state_publisher_node)
    ld.add_action(rviz2_node)

    return ldCopy to clipboardErrorCopied
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

想要可视化模型需要三个节点参与

• joint_state_publisher_gui 负责发布机器人关节数据信息，通过joint_states话题发布
• robot_state_publisher_node负责发布机器人模型信息robot_description，并将joint_states数据转换tf信息发布
• rviz2_node负责显示机器人的信息

joint_state_publisher_gui，还有一个兄弟叫做joint_state_publisher两者区别在于joint_state_publisher_gui运行起来会跳出一个界面，通过界面可以操作URDF中能动的关节

• 修改setup.py

导入头文件

from glob import glob
import osCopy to clipboardErrorCopied
1
2

加入目录安装

('share/ament_index/resource_index/packages',
['resource/' + package_name]),
('share/' + package_name, ['package.xml']),Copy to clipboardErrorCopied
1
2
3

完整

from setuptools import setup
from glob import glob
import os

package_name = 'fishbot_description'

setup(
    name=package_name,
    version='0.0.0',
    packages=[package_name],
    data_files=[
        ('share/ament_index/resource_index/packages',
            ['resource/' + package_name]),
        ('share/' + package_name, ['package.xml']),
        (os.path.join('share', package_name, 'launch'), glob('launch/*.launch.py')),
        (os.path.join('share', package_name, 'urdf'), glob('urdf/**')),
    ],
    install_requires=['setuptools'],
    zip_safe=True,
    maintainer='root',
    maintainer_email='root@todo.todo',
    description='TODO: Package description',
    license='TODO: License declaration',
    tests_require=['pytest'],
    entry_points={
        'console_scripts': [
        ],
    },
)Copy to clipboardErrorCopied
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

• 编译测试

编译

colcon buildCopy to clipboardErrorCopied
1

运行测试

source install/setup.bash
ros2 launch fishbot_description display_rviz2.launch.pyCopy to clipboardErrorCopied
1
2

添加robotmodel模块，分别选择link名称，即可看到机器人的模型显示

二、SLAM建图

通过SLAM就是解决地图和定位问题

机器人通过自身传感器数据处理进行位置估计，同时通过不断移动完成对整个未知环境的地图构建。这就是SLAM解决的问题。

那又是如何解决的呢？SLAM实现的方案很多，但是几个比较关键的技术如下：

1. 传感器感知 通过各类传感器实现对环境的感知，比如通过激光雷达获取环境的深度信息。同时可以通过传感器融合来提高位置估计的精度，比如融合轮式里程计、IMU、雷达、深度相机数据等。
2. 视觉/激光里程计 基本原理是通过前后数据之间对比得出机器人位置的变化。
3. 回环检测 判断机器人是否到达之前到过的位置，可以解决位置估计误差问题，建图时可以纠正地图误差。

经典视觉SLAM结构

• SLAM算法分类

从算法的对数据的处理方式上看，目前常用的SLAM开源算法可以分为两类

1.基于滤波，比如扩展卡尔曼滤波（EKF: Extended Kalman Filter）、粒子滤波(PF: Particle Filter)等。

ROS中的gmapping、hector_slam算法都是基于滤波实现的。

2.基于图优化，先通过传感器进行构图，然后对图进行优化。

目前比较主流的是图优化的方法，Cartographer就是基于图优化实现的。图优化相对于滤波，不用实时的进行计算，效率更高，消耗的资源更少，所以在实际场景中使用的更多。

1.Cartographer介绍

Cartographer是Google开源的一个可跨多个平台和传感器配置以2D和3D形式提供实时同时定位和建图（SLAM）的系统。

github地址：https://github.com/cartographer-project/cartographer 文档地址：https://google-cartographer.readthedocs.io/en/latest

Cartographer系统架构概述（简单看看即可，如果大家后面确定研究方向是SLAM可以深入学习）：

简单的可以看到左边的可选的输入有深度信息、里程计信息、IMU数据、固定Frame姿态。

2.Carttographer安装

2.1 apt安装

安装carotgrapher

sudo apt install ros-humble-cartographerCopy to clipboardErrorCopied
1

需要注意我们不是直接使用cartographer，而是通过cartographer-ros功能包进行相关操作，所以我们还需要安装下cartographer-ros

sudo apt install ros-humble-cartographer-rosCopy to clipboardErrorCopied
1

2.2 源码安装

将下面的源码克隆到fishbot_ws的src目录下：

git clone https://ghproxy.com/https://github.com/ros2/cartographer.git -b ros2
git clone https://ghproxy.com/https://github.com/ros2/cartographer_ros.git -b ros2Copy to clipboardErrorCopied
1
2

安装依赖

这里我们使用rosdepc进行依赖的安装，rosdepc指令找不到可以先运行下面的一键安装命令，选择一键配置rosdep即可。

wget http://fishros.com/install -O fishros && . fishrosCopy to clipboardErrorCopied
1

接着在fishbot_ws下运行下面这个命令进行依赖的安装。

rosdepc 是小鱼制作的国内版rosdep，是一个用于安装依赖的工具。该工具的安装可以采用一键安装进行，选项编号为3。安装完成后运行一次rodepc update即可使用。

rosdepc install -r --from-paths src --ignore-src --rosdistro $ROS_DISTRO -yCopy to clipboardErrorCopied
1

编译

这里有一个新的命令–packages-up-to，意思是其所有依赖后再编译该包

colcon build --packages-up-to cartographer_rosCopy to clipboardErrorCopied
1

2.3 测试是否安装成功

如果是源码编译请先source下工作空间后再使用下面指令查看是否安装成功；

ros2 pkg list | grep cartographerCopy to clipboardErrorCopied
1

能看到下面的结果即可

cartographer_ros
cartographer_ros_msgsCopy to clipboardErrorCopied
1
2

可能你会好奇为什么没有cartographer，因为cartographer包的编译类型原因造成的，不过没关系，cartographer_ros依赖于cartographer，所以有cartographer_ros一定有cartographer。

3.Cartographer参数配置

作为一个优秀的开源库，Cartographer提供了很多可以配置的参数，虽然灵活性提高了，但同时也提高了使用难度（需要对参数进行调节配置），所以有必要在正式使用前对参数进行基本的介绍。

因为我们主要使用其进行2D的建图定位，所以我们只需要关注2D相关的参数。

Cartographer参数是使用lua文件来描述的，不会lua也没关系，我们只是改改参数而已。

3.1 前端参数

文件：trajectory_builder_2d

src/cartographer/configuration_files/trajectory_builder_2d.lua
1

请你打开这个文件自行浏览，小鱼对其中我们可能会在初次建图配置的参数进行介绍。

  -- 是否使用IMU数据
  use_imu_data = true, 
  -- 深度数据最小范围
  min_range = 0.,
  -- 深度数据最大范围
  max_range = 30.,
  -- 传感器数据超出有效范围最大值时，按此值来处理
  missing_data_ray_length = 5.,
  -- 是否使用实时回环检测来进行前端的扫描匹配
  use_online_correlative_scan_matching = true
  -- 运动过滤，检测运动变化，避免机器人静止时插入数据
  motion_filter.max_angle_radiansCopy to clipboardErrorCopied
1
2
3
4
5
6
7
8
9
10
11
12

3.2 后端参数

文件：pose_graph.lua-后端参数配置项

路径src/cartographer/configuration_files/pose_graph.lua

该文件主要和地图构建

--Fast csm的最低分数，高于此分数才进行优化。
constraint_builder.min_score = 0.65
--全局定位最小分数，低于此分数则认为目前全局定位不准确
constraint_builder.global_localization_min_score = 0.7Copy to clipboardErrorCopied
1
2
3
4

3.3 Carotgrapher_ROS参数配置

该部分参数主要是用于和ROS2进行通信和数据收发的配置，比如配置从哪个话题读取里程记数据，从哪个话题来获取深度信息（雷达）。

文件：backpack_2d.lua

路径：src/cartographer_ros/cartographer_ros/configuration_files/backpack_2d.lua

include "map_builder.lua"
include "trajectory_builder.lua"

options = {
  map_builder = MAP_BUILDER,
  trajectory_builder = TRAJECTORY_BUILDER,
  -- 用来发布子地图的ROS坐标系ID，位姿的父坐标系，通常是map。
  map_frame = "map",
  -- SLAM算法跟随的坐标系ID
  tracking_frame = "base_link",
  -- 将发布map到published_frame之间的tf
  published_frame = "base_link",
  -- 位于“published_frame ”和“map_frame”之间，用来发布本地SLAM结果（非闭环），通常是“odom”
  odom_frame = "odom",
  -- 是否提供里程计
  provide_odom_frame = true,
  -- 只发布二维位姿态（不包含俯仰角）
  publish_frame_projected_to_2d = false,
  -- 是否使用里程计数据
  use_odometry = false,
  -- 是否使用GPS定位
  use_nav_sat = false,
  -- 是否使用路标
  use_landmarks = false,
  -- 订阅的laser scan topics的个数
  num_laser_scans = 0,
  -- 订阅多回波技术laser scan topics的个数
  num_multi_echo_laser_scans = 1,
  -- 分割雷达数据的个数
  num_subdivisions_per_laser_scan = 10,
  -- 订阅的点云topics的个数
  num_point_clouds = 0,
  -- 使用tf2查找变换的超时秒数
  lookup_transform_timeout_sec = 0.2,
  -- 发布submap的周期间隔
  submap_publish_period_sec = 0.3,
  -- 发布姿态的周期间隔
  pose_publish_period_sec = 5e-3,
  -- 轨迹发布周期间隔
  trajectory_publish_period_sec = 30e-3,
  -- 测距仪的采样率
  rangefinder_sampling_ratio = 1.,
  --里程记数据采样率
  odometry_sampling_ratio = 1.,
  -- 固定的frame位姿采样率
  fixed_frame_pose_sampling_ratio = 1.,
  -- IMU数据采样率
  imu_sampling_ratio = 1.,
  -- 路标采样率
  landmarks_sampling_ratio = 1.,
}
laser_scan = 10,
  -- 订阅的点云topics的个数
  num_point_clouds = 0,
  -- 使用tf2查找变换的超时秒数
  lookup_transform_timeout_sec = 0.2,
  -- 发布submap的周期间隔
  submap_publish_period_sec = 0.3,
  -- 发布姿态的周期间隔
  pose_publish_period_sec = 5e-3,
  -- 轨迹发布周期间隔
  trajectory_publish_period_sec = 30e-3,
  -- 测距仪的采样率
  rangefinder_sampling_ratio = 1.,
  --里程记数据采样率
  odometry_sampling_ratio = 1.,
  -- 固定的frame位姿采样率
  fixed_frame_pose_sampling_ratio = 1.,
  -- IMU数据采样率
  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