机器人系统仿真

在 ROS 中，仿真实现主要涉及的内容：

  • 对机器人建模(URDF)
  • 创建仿真环境(Gazebo)
  • 感知环境(Rviz)

如果安装的是完整版ros，Gazebo和Rviz默认安装

URDF：Unified Robot Description Format

可以以一种 XML 的方式描述机器人的部分结构，比如底盘、摄像头、激光雷达、机械臂以及不同关节的自由度…..，该文件可以被 C++ 内置的解释器转换成可视化的机器人模型，是 ROS 中实现机器人仿真的重要组件

rviz：ROS Visualization Tool

以三维方式显示ROS消息，可以将数据进行可视化表达

Gazebo：

Gazebo是一款3D动态模拟器，用于显示机器人模型并创建仿真环境，能够在复杂的室内和室外环境中准确有效地模拟机器人

三者应用中，只是创建 URDF 意义不大，一般需要结合 Gazebo 或 Rviz 使用，在 Gazebo 或 Rviz 中可以将 URDF 文件解析为图形化的机器人模型，一般的使用组合为:

  • 如果非仿真环境，那么使用 URDF 结合 Rviz 直接显示感知的真实环境信息
  • 如果是仿真环境，那么需要使用 URDF 结合 Gazebo 搭建仿真环境，并结合 Rviz 显示感知的虚拟环境信息

仿真缺陷：

机器人在仿真环境与实际环境下的表现差异较大，仿真并不能完全做到模拟真实的物理世界，存在一些”失真”的情况

  1. 仿真器所使用的物理引擎目前还不能够完全精确模拟真实世界的物理情况

  2. 仿真器构建的是关节驱动器（电机&齿轮箱）、传感器与信号通信的绝对理想情况，目前不支持模拟实际硬件缺陷或者一些临界状态等情形

素材链接:

URDF集成Rviz

URDF与Rviz的集成较之于URDF与Gazebo的集成更为简单

需求描述：在 Rviz 中显示一个盒状机器人

实现流程：

  1. 新建功能包，导入依赖
  2. 核心：编写 urdf 文件
  3. 核心：在 launch 文件集成 URDF 与 Rviz
  4. 在 Rviz 中显示机器人模型

创建功能包：

创建一个新的功能包，名称自定义，导入依赖包：urdf与xacro

这个时候目录下是没有src文件的，因为不需要编码

xacro(XML Macros)是 ROS中用于简化机器人模型描述的核心工具包，专门用于处理 URDF文件

在当前功能包下，再新建几个目录：

urdf: 存储 urdf 文件的目录

meshes:机器人模型渲染文件(暂不使用)

config: 配置文件

launch: 存储 launch 启动文件

编写 URDF 文件：

在urdf文件夹下新建子级文件夹urdf和xacro

urdf文件夹中添加一个.urdf文件，复制如下内容：

1
2
3
4
5
6
7
8
9
<robot name="mycar">
    <link name="base_link">
        <visual>
            <geometry>
                <box size="0.5 0.2 0.1" />
            </geometry>
        </visual>
    </link>
</robot>

在 launch 文件中集成 URDF 与 Rviz：

1
2
3
4
5
6
7
<launch>
    <!-- 1. 在参数服务器载入urdf文件 -->
    <!-- 参数名name固定 robot_description -->
    <param name = "robot_description" textfile= "$(find urdf01_rviz)/urdf/urdf/demo01.urdf"  />
    <!-- 2. 启动 rviz -->
    <node  pkg = "rviz" type = "rviz" name = "rviz" />
</launch>

在 Rviz 中显示机器人模型：

rviz 启动后，会发现并没有盒装的机器人模型，这是因为默认情况下没有添加机器人显示组件，需要手动添加，添加方式如下

01_URDF文件执行rviz配置01 02_URDF文件执行rviz配置02

优化 rviz 启动

重复启动launch文件时，Rviz 之前的组件配置信息不会自动保存，需要重复执行显示操作，为了方便使用，可以使用如下方式优化：

首先，将当前配置保存进config目录

10_rviz配置保存

launch文件中 Rviz 的启动配置添加参数：args，值设置为 -d 配置文件路径

1
<node  pkg = "rviz" type = "rviz" name = "rviz" args = "-d $(find urdf01_rviz)/config/show_car.rviz" />

再启动时，就可以包含之前的组件配置了，使用更方便快捷

URDF语法

URDF 文件是一个标准的 XML 文件，在 ROS 中预定义了一系列的标签用于描述机器人模型

机器人模型可能较为复杂，但是 ROS 的 URDF 中机器人的组成却是较为简单，

URDF主要有四类标签

  • robot 根标签，类似于 launch文件中的launch标签
  • link 连杆标签
  • joint 关节标签
  • gazebo

joint标签对应的数据在模型中是不可见的

gazebo标签不是机器人模型必须的，只有在仿真时才需设置，用于配置仿真环境所需参数

属性跟在标签的<>内

robot

urdf 中为了保证 xml 语法的完整性，使用了robot标签作为根标签

所有的 link 和 joint 以及其他标签都必须包含在 robot 标签内

在该标签内可以通过 name 属性设置机器人模型的名称

urdf 中的 link 标签用于描述机器人某个部件(也即刚体部分)的外观和物理属性

比如: 机器人底座、轮子、激光雷达、摄像头…

每一个部件都对应一个 link，在 link 标签内，可以设计该部件的形状、尺寸、颜色、惯性(inertial)矩阵、碰撞(collision)参数等一系列属性

20250610link

属性：name —> 为连杆命名

子标签：

visual —> 描述外观(对应的数据是可视的)

  • geometry 设置连杆的形状

    • 标签1: box(盒状)

      • 属性：size = “[x] [y] [z]” 单位为m

    • 标签2: cylinder(圆柱)

      • 属性：radius=“ ” length=“ ”

    • 标签3: sphere(球体)

      • 属性：radius=“ ”

    • 标签4: mesh(为连杆添加皮肤)

      • 属性：filename=资源路径

        资源路径格式如下：

        1
        package://<packagename>/<path>/文件

  • origin 设置偏移量与倾斜弧度

    • 属性1：xyz=”[x] [y] [z]”

    • 属性2：rpy=”[翻滚] [俯仰] [偏航]”

      逆着轴方向看逆时针旋转为正

      偏移多在joint中实现

  • metrial 设置材料属性(颜色)

    • 属性: name
    • 标签: color
      • 属性: rgba=”[红] [绿] [蓝] [透明度]” 取值均[0,1]

  • collision —> 连杆的碰撞属性

  • Inertial —> 连杆的惯性矩阵

urdf文件：

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
<robot name = "car">
    <link name ="base_link">
        <!-- 可视化标签 -->
        <visual>
            <!-- 1.形状 -->
            <geometry>
                <!-- 立方体 -->
                <!-- <box size = "0.2 0.2 0.2" /> -->
                <!-- 圆柱 -->
                <!-- <cylinder radius = "0.2" length = "2" /> -->
                <!-- 球体 -->
                <!-- <sphere radius = "0.2" /> -->
                <!-- 皮肤 -->
                <mesh filename = "package://urdf01_rviz/meshes/autolabor_mini.stl" />
            </geometry>
            <!-- 2. 偏移量和倾斜弧度 -->
            <origin xyz="0 0 0" rpy="1.57 0 1.57" />
            <!-- 3. 颜色 -->
            <material name = "car_color">
                <color rgba = "0.3 0.4 0.2 1" />
            </material>
        </visual>
    </link>
</robot>

joint

urdf 中的 joint 标签用于描述机器人关节的运动学和动力学属性，还可以指定关节运动的安全极限

机器人的两个部件(分别称之为 parent link 与 child link)以”关节”的形式相连接

不同的关节有不同的运动形式：旋转、滑动、固定、旋转速度、旋转角度限制….

官方02_link

属性：

  • name —> 为关节命名
  • type —> 关节运动形式
    • continuous：旋转关节，可以绕单轴无限旋转
    • revolute：旋转关节，类似于 continues，但是有旋转角度限制
    • prismatic：滑动关节，沿某一轴线移动的关节，有位置极限
    • planer：平面关节，允许在平面正交方向上平移或旋转
    • floating：浮动关节，允许进行平移、旋转运动
    • fixed：固定关节，不允许运动的特殊关节

子标签

  • parent(必需的)

    parent link的名字是一个强制的属性：

    • link：父级连杆的名字，是这个link在机器人结构树中的名字

  • child(必需的)

    child link的名字是一个强制的属性：

    • link：子级连杆的名字，是这个link在机器人结构树中的名字

  • origin

    • 属性：xyz=各轴线上的偏移量 rpy=各轴线上的偏移弧度

  • axis

    • 属性：xyz用于设置围绕哪个关节轴运动

需求：创建机器人模型，底盘为长方体，在长方体的前面添加一摄像头，摄像头可以沿着 Z 轴 360 度旋转

urdf文件：

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
<robot name = "car">
    <!-- 1. 底盘link -->
    <link name = "base_link">
        <visual>
            <geometry>
                <box size = "0.3 0.2 0.1" />
            </geometry>
            <origin xyz="0 0 0" rpy = "0 0 0" />
            <material name = "car_color">
                <color rgba = "0.3 0.4 0.2 0.5" />
            </material>
        </visual>
    </link>
    <!-- 2. 摄像头link -->
    <link name = "camera">
        <visual>
            <geometry>
                <box size="0.02 0.05 0.05"/>
            </geometry>
            <origin xyz="0 0 0.027" rpy = "0 0 0" />
            <material name = "camere_color">
                <color rgba = "0 0 1 0.5" />
            </material>
        </visual>
    </link>
    <!-- 3. 关节 -->
    <joint  name = "camerabase" type = "continuous">
        <!-- 父级link -->
        <parent link = "base_link" />
        <!-- 子级link -->
        <child link = "camera"/>
        <!-- 设置偏移量 -->
        <!-- 注意这里的偏移是两个坐标系之间的偏移, 要考虑不嵌入需要另外增加偏移量 -->
        <origin xyz = "-0.12 0 0.05" rpy ="0 0 0" />
        <!-- 设置关节旋转参考的坐标轴 -->
        <axis xyz="0 0 1" />
    </joint>
</robot>

launch文件：

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
<launch>
    <!-- 1. 在参数服务器载入urdf文件 -->
    <!-- 参数名name固定 robot_description -->
    <param name = "robot_description" textfile= "$(find urdf01_rviz)/urdf/urdf/demo03.urdf"  />
    <!-- 2. 启动 rviz -->
    <node  pkg = "rviz" type = "rviz" name = "rviz" args = "-d $(find urdf01_rviz)/config/show_car.rviz" />
    <!--
        只有这两句:
            表现:设置显示的位置与颜色异常
            提示:No transform from [camera] to [base_link]   
            原因:rviz中显示URDF时，必须发布不同部件之间的坐标系关系
            解决:ROS中提供了关于机器人模型显示的坐标发布相关节点（两个）
    -->
    <!--  关节状态发布节点 -->
    <node pkg="joint_state_publisher" type="joint_state_publisher" name="joint_state_publisher"/>
    <!--  机器人状态发布节点 -->
    <node pkg="robot_state_publisher" type="robot_state_publisher" name="robot_state_publisher"/>
</launch>

状态发布节点在此是必须的

关节运动控制节点(可选)，会生成关节控制的UI，用于测试关节运动是否正常

1
2
<!-- 可选:用于控制关节运动的节点 -->
<node pkg="joint_state_publisher_gui" type="joint_state_publisher_gui" name="joint_state_publisher_gui" />

注意，摄像头的便宜设置为0.027是为了避免旋转控制时0.025和底盘贴合可能出现旋转抖动现象

base_footprint优化urdf

前面实现的机器人模型是半沉到地下的，因为默认情况下底盘的中心点位于地图原点上

可以将初始 link 设置为一个尺寸极小的 link(比如半径为 0.001m 的球体，或边长为 0.001m 的立方体)，然后再在初始 link 上添加底盘等刚体，这样虽然仍然存在初始link半沉的现象，但是基本可以忽略了

这个初始 link 一般称之为 base_footprint，在rviz中将坐标系换为base_footprint发现机器人已经回到地面，这种方法避免了偏移导致的混乱

修改后的urdf文件

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
<robot name = "car">
    <!-- 添加一个尺寸极小的link,再去关联base_link,
    关节高度和base_link下沉一致(半个底盘高度)-->
    <link name = "base_footprint">
        <visual>
            <geometry>
                <box size = "0.001 0.001 0.001" />
            </geometry>
        </visual>
    </link>
    <!-- 1. 底盘link -->
    <link name = "base_link">
        <visual>
            <geometry>
                <box size = "0.3 0.2 0.1" />
            </geometry>
            <origin xyz="0 0 0" rpy = "0 0 0" />
            <material name = "car_color">
                <color rgba = "0.3 0.4 0.2 0.5" />
            </material>
        </visual>
    </link>
    <!-- 2. 摄像头link -->
    <link name = "camera">
        <visual>
            <geometry>
                <box size="0.02 0.05 0.05"/>
            </geometry>
            <origin xyz="0 0 0.027" rpy = "0 0 0" />
            <material name = "camere_color">
                <color rgba = "0 0 1 0.5" />
            </material>
        </visual>
    </link>
    <!-- 关联base_footprint 与base_link-->
    <joint  name = "link2footfrint" type = "fixed">
        <!-- 父级link -->
        <parent link = "base_footprint" />
        <!-- 子级link -->
        <child link = "base_link"/>
        <!-- 设置偏移量 -->
        <origin xyz = "0 0 0.05" rpy ="0 0 0" />
    </joint>   
    <!-- 3. 关节 -->
    <joint  name = "camera2base" type = "continuous">
        <!-- 父级link -->
        <parent link = "base_link" />
        <!-- 子级link -->
        <child link = "camera"/>
        <!-- 设置偏移量 -->
        <!-- 注意这里的偏移是两个坐标系之间的偏移, 要考虑不嵌入需要另外增加偏移量 -->
        <origin xyz = "-0.12 0 0.05" rpy ="0 0 0" />
        <!-- 设置关节旋转参考的坐标轴 -->
        <axis xyz="0 0 1" />
    </joint>
</robot>

urdf实操

需求描述：创建一个四轮圆柱状机器人模型，底盘为圆柱状，半径 10cm，高 8cm，四轮由两个驱动轮和两个万向支撑轮组成，两个驱动轮半径为 3.25cm，轮胎宽度1.5cm，两个万向轮为球状，半径 0.75cm，底盘离地间距为 1.5cm(与万向轮直径一致)

偏移多使用joint来实现，link内origin一般都设为0，这样能保证坐标原点在物体中心

可以这么理解，joint改变了两个坐标系原点的位置，link不改变原点位置，只是改变了偏移量

实现流程：

创建机器人模型可以分步骤实现

  1. 新建 urdf 文件，并与 launch 文件集成
  2. 搭建底盘
  3. 在底盘上添加两个驱动轮
  4. 在底盘上添加两个万向轮

launch文件：

1
2
3
4
5
6
7
8
9
10
<launch>
    <param name="robot_description" textfile="$(find urdf01_rviz)/urdf/urdf/test.urdf" />
    <node pkg = "rviz" type = "rviz" name = "rviz" args = "-d $(find urdf01_rviz)/config/show_car.rviz" />
    <!--  关节状态发布节点 -->
    <node pkg="joint_state_publisher" type="joint_state_publisher" name="joint_state_publisher"/>
    <!--  机器人状态发布节点 -->
    <node pkg="robot_state_publisher" type="robot_state_publisher" name="robot_state_publisher"/>
    <!-- 添加控制关节运动的节点 -->
    <node pkg="joint_state_publisher_gui" type="joint_state_publisher_gui" name="joint_state_publisher_gui" />
</launch>

urdf文件：

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
127
128
129
130
131
132
133
134
135
136
137
138
<robot name="mycar">
    <!-- 设置 base_footprint  -->
   <link name="base_footprint">
        <visual>
            <geometry>
                <sphere radius="0.001"/>
            </geometry>
        </visual>
   </link>
    <!-- 添加底盘 -->
    <!-- 
        参数
            形状:圆柱 
            半径:10cm -> 0.1m
            高度:8cm  -> 0.08m
            离地:1.5cm-> 0.015m
    -->
    <link name="base_link">
        <visual>
            <geometry>
                <cylinder radius="0.1" length="0.08"/>
            </geometry>
            <!-- 偏移尽量去joint设置 -->
            <origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0"/>
            <material name="base_link_color">
                <color rgba="0.8 0.3 0.1 0.5"/>
            </material>
        </visual>       
    </link>
    <!-- 添加关节 -->
    <joint name="base_link2base_footprint" type="fixed">
        <parent link="base_footprint"/>
        <child link="base_link"/>
        <!-- 关节z设置: 车身高度/2 + 离地距离 0.08/2+0.015-->
        <origin xyz="0.0 0.0 0.055" rpy="0.0 0.0 0.0"/>
    </joint>
    <!-- 添加驱动轮 -->
    <!--
        驱动轮是侧翻的圆柱
        参数
            半径: 3.25 cm -> 0.0325m
            宽度: 1.5  cm -> 0.015m
            颜色: 黑色 -> (0,0,0)
    -->
    <link name="left_wheel">
        <visual>
            <geometry>
                <cylinder radius="0.0325" length="0.015"/>
            </geometry>
            <!--  -->
            <origin xyz="0.0 0.0 0.0" rpy="1.5707 0.0 0.0"/>
            <material name="wheel_color">
                <color rgba="0.0 0.0 0.0 1"/>
            </material>
        </visual>
    </link>

    <link name="right_wheel">
        <visual>
            <geometry>
                <cylinder radius="0.0325" length="0.015"/>
            </geometry>
            <!--  -->
            <origin xyz="0.0 0.0 0.0" rpy="-1.5707 0.0 0.0"/>
            <material name="wheel_color">
                <color rgba="0.0 0.0 0.0 1"/>
            </material>
        </visual>
    </link>
    <!-- 添加关节 -->
    <!-- 关节设置:
            x = 0
            y = 底盘的半径 + 轮胎宽度 / 2 = 10 + 1.5/2 = 10.75cm = 0.1075m
            z = 离地间距 + 底盘长度 / 2 - 轮胎半径 = 1.5 + 4 - 3.25 = 2.25(cm)
            axis = 0 1 0
    -->
    <joint name="left_wheel2base_link" type="continuous">
        <parent link="base_link"/>
        <child link="left_wheel"/>
        <origin xyz="0.0 0.1075 -0.0225" rpy="0.0 0.0 0.0"/>
        <axis xyz="0 1 0"/>
    </joint>

    <joint name="right_wheel2base_link" type="continuous">
        <parent link="base_link"/>
        <child link="right_wheel"/>
        <origin xyz="0.0 -0.1075 -0.0225" rpy="0.0 0.0 0.0"/>
        <axis xyz="0 1 0"/>
    </joint>
    <!-- 添加万向轮(支撑轮) -->  
    <!--
        参数
            形状: 球体
            半径: 0.75 cm
            颜色: 黑色
    -->  
    <link name="front_wheel">
        <visual>
            <geometry>
                <sphere radius="0.0075"/>
            </geometry>
            <origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0"/>
            <material name="wheel_color">
                <color rgba="0.0 0.0 0.0 1.0"/>
            </material>
        </visual>
    </link>
    <link name="back_wheel">
        <visual>
            <geometry>
                <sphere radius="0.0075"/>
            </geometry>
            <origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0"/>
            <material name="wheel_color">
                <color rgba="0.0 0.0 0.0 1.0"/>
            </material>
        </visual>
    </link>
    <!--
        关节设置:
        x = 自定义
        y = 0
        z = 底盘长度 / 2 + 离地间距 - 轮胎半径 = 0.08 / 2 + 0.015 / 2 = 0.0475 
        axis= 1 1 1
    -->
    <joint name="front_wheel2base_link" type="continuous">
        <parent link="base_link"/>
        <child link="front_wheel"/>
        <origin xyz="0.08 0.0 -0.0475" rpy="0.0 0.0 0.0"/>
        <axis xyz="1 1 1"/>
    </joint>
    <joint name="back_wheel2base_link" type="continuous">
        <parent link="base_link"/>
        <child link="back_wheel"/>
        <origin xyz="-0.08 0.0 -0.0475" rpy="0.0 0.0 0.0"/>
        <axis xyz="1 1 1"/>
    </joint>
</robot>

代码似乎有些冗长，复用性差

urdf工具

在 ROS 中，提供了一些工具来方便 URDF 文件的编写

  • check_urdf命令可以检查复杂的 urdf 文件是否存在语法问题
  • urdf_to_graphiz命令可以查看 urdf 模型结构，显示不同 link 的层级关系

首先需要安装，安装命令：sudo apt install liburdfdom-tools

在urdf所在文件夹打开终端，调用check_urdf [urdf文件]，如果不抛出异常，说明文件合法，否则非法

进入urdf文件所属目录，调用:urdf_to_graphiz [urdf文件]，当前目录下会生成 pdf 文件，使用evince命令可以查看pdf

URDF优化

前面 URDF 文件构建机器人模型的过程中，存在若干问题

  1. 在设计关节的位置时，需要按照一定的公式计算，公式是固定的，但是在 URDF 中依赖于人工计算，存在不便，容易计算失误，且当某些参数发生改变时，还需要重新计算
  2. URDF 中的部分内容是高度重复的，驱动轮与支撑轮的设计实现，不同轮子只是部分参数不同，形状、颜色、翻转量都是一致的，在实际应用中，构建复杂的机器人模型时，更是易于出现高度重复的设计，按照一般的编程涉及到重复代码应该考虑封装

在 ROS 中，已经给出了类似编程的优化方案，称之为Xacro

Xacro 是 XML Macros 的缩写，Xacro 是一种 XML 宏语言，是可编程的 XML

Xacro 可以声明变量，可以通过数学运算求解，使用流程控制控制执行顺序，还可以通过类似函数的实现，封装固定的逻辑，将逻辑中需要的可变的数据以参数的方式暴露出去，从而提高代码复用率以及程序的安全性。

Xacro体验

注意：该案例编写生成的是非法的 URDF 文件，目的在于演示 Xacro 的极简使用以及优点

编写 Xacro 文件，以变量的方式封装属性(常量半径、高度、车轮半径…)，以函数的方式封装重复实现(车轮的添加)

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
<robot name="mycar" xmlns:xacro="http://wiki.ros.org/xacro">
    <!-- 属性封装 -->
    <xacro:property name="wheel_radius" value="0.0325" />
    <xacro:property name="wheel_length" value="0.0015" />
    <xacro:property name="PI" value="3.1415927" />
    <xacro:property name="base_link_length" value="0.08" />
    <xacro:property name="lidi_space" value="0.015" />

    <!-- 宏 -->
    <xacro:macro name="wheel_func" params="wheel_name flag" >
        <link name="${wheel_name}_wheel">
            <visual>
                <geometry>
                    <cylinder radius="${wheel_radius}" length="${wheel_length}" />
                </geometry>

                <origin xyz="0 0 0" rpy="${PI / 2} 0 0" />

                <material name="wheel_color">
                    <color rgba="0 0 0 0.3" />
                </material>
            </visual>
        </link>

        <!-- 3-2.joint -->
        <joint name="${wheel_name}2link" type="continuous">
            <parent link="base_link"  />
            <child link="${wheel_name}_wheel" />
            <!-- 
                x 无偏移
                y 车体半径
                z z= 车体高度 / 2 + 离地间距 - 车轮半径

            -->
            <origin xyz="0 ${0.1 * flag} ${(base_link_length / 2 + lidi_space - wheel_radius) * -1}" rpy="0 0 0" />
            <axis xyz="0 1 0" />
        </joint>

    </xacro:macro>
    <xacro:wheel_func wheel_name="left" flag="1" />
    <xacro:wheel_func wheel_name="right" flag="-1" />
</robot>

命令行进入 xacro文件所属目录，执行:rosrun xacro xacro xxx.xacro > xxx.urdf, 会将 xacro 文件解析为 urdf 文件(同一目录下)

Xacro语法详解

xacro 提供了可编程接口，类似于计算机语言，包括变量声明调用、函数声明与调用等语法实现

在使用 xacro 生成 urdf 时，根标签robot中必须包含命名空间声明

1
xmlns:xacro="http://wiki.ros.org/xacro"

属性与算数运算

用于封装 URDF 中的一些字段，比如：PAI 值，小车的尺寸，轮子半径 ….

属性定义：

1
<xacro:property name="PI" value="3.1415927" />

属性调用：

1
2
<!-- ${属性名称} -->
<UseProperty name = "${PI}"/>

算术运算：

1
2
<!--${数学表达式}-->
<UseProperty1 result = "${PI/2}" />

宏

类似于函数实现，提高代码复用率，优化代码结构，提高安全性

宏定义：

1
2
3
4
5
6
7
<xacro:macro name="宏名称" params="参数列表(多参数之间使用空格分隔)">

    .....

    参数调用格式: ${参数名}

</xacro:macro>

宏调用：

1
<xacro:宏名称 参数1="" 参数2="" />

文件包含

机器人由多部件组成，不同部件可能封装为单独的 xacro 文件，最后再将不同的文件集成，组合为完整机器人，可以使用文件包含实现

1
2
3
4
5
6
<robot name="xxx" xmlns:xacro="http://wiki.ros.org/xacro">
      <xacro:include filename="my_base.xacro" />
      <xacro:include filename="my_camera.xacro" />
      <xacro:include filename="my_laser.xacro" />
      ....
</robot>

Xacro完整使用

需求描述：使用 Xacro 优化 URDF 版的小车底盘模型实现

编写 Xacro 文件：

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
<robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="mycar">
    <!-- 封装常量 -->
    <xacro:property name="PI" value="3.1415927"/>
    <xacro:property name="footprint_radius" value="0.001"/>
    <!-- 宏:黑色设置 -->
    <material name="black">
        <color rgba="0.0 0.0 0.0 1.0" />
    </material>
    <!-- 底盘属性 -->
    <xacro:property name="base_radius" value="0.1"/>
    <xacro:property name="base_length" value="0.08"/>
    <xacro:property name="ground_distance" value="0.015"/>

    <!-- 设置 base_footprint  -->
    <link name="base_footprint">
        <visual>
            <geometry>
                <sphere radius="${footprint_radius}"/>
            </geometry>
        </visual>
    </link>    
    <!-- 添加底盘 -->
    <link name="base_link">
        <visual>
            <geometry>
                <cylinder radius="${base_radius}" length="${base_length}"/>
            </geometry>
            <origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0"/>
            <material name="base_link_color">
                <color rgba="0.8 0.3 0.1 0.5"/>
            </material>
        </visual>       
    </link>
    <!-- 添加关节 -->
    <joint name="base_link2base_footprint" type="fixed">
        <parent link="base_footprint"/>
        <child link="base_link"/>
        <!-- 关节z设置: 车身高度/2 + 离地距离-->
        <origin xyz="0.0 0.0 ${base_length/2 + ground_distance}"/>
    </joint>
    <!-- 驱动轮属性 -->
    <xacro:property name="wheel_radius" value="0.0325" /><!-- 半径 -->
    <xacro:property name="wheel_length" value="0.015" /><!-- 宽度 -->
    <xacro:property name="wheel_joint_z" value="${-(base_length/2 + ground_distance - wheel_radius)}" />
    <!-- 驱动轮宏 -->
    <!-- 
        direction: left or right
        flag: 1 or -1
    -->
    <xacro:macro name = "wheel_func" params="direction flag">
        <link name="${direction}_wheel">
            <visual>
                <geometry>
                    <cylinder radius="${wheel_radius}" length="${wheel_length}"/>
                </geometry>
                <origin xyz="0.0 0.0 0.0" rpy="${flag*PI/2} 0.0 0.0"/>
                <material name="black"/>
            </visual>
        </link>   

        <joint name="${direction}_wheel2base_link" type="continuous">
            <parent link="base_link"/>
            <child link="${direction}_wheel"/>
            <origin xyz="0.0 ${flag*(base_radius)} ${wheel_joint_z}" rpy="0.0 0.0 0.0"/>
            <axis xyz="0 1 0"/>
        </joint>
    </xacro:macro>

    <xacro:wheel_func direction="left" flag="1" />
    <xacro:wheel_func direction="right" flag="-1" />

    <!-- 万向轮属性 -->
    <xacro:property name = "support_wheel_radius" value ="0.0075" />
    <xacro:property name="support_wheel_joint_z" value="${-(base_length/2 + ground_distance - support_wheel_radius)}" />

    <!-- 万向轮(支撑轮)宏 -->  
    <xacro:macro name = "add_support_wheel" params="direction flag">
        <link name="${direction}_wheel">
            <visual>
                <geometry>
                    <sphere radius="${support_wheel_radius}"/>
                </geometry>
                <origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0"/>
                <material name="black"/>
            </visual>
        </link>
        <joint name="${direction}_wheel2base_link" type="continuous">
            <parent link="base_link"/>
            <child link="${direction}_wheel"/>
            <origin xyz="${flag*(base_radius - support_wheel_radius)} 0.0 ${support_wheel_joint_z}" rpy="0.0 0.0 0.0"/>
            <axis xyz="1 1 1"/>
        </joint>
    </xacro:macro>

    <xacro:add_support_wheel direction="front" flag="1" />
    <xacro:add_support_wheel direction="back" flag="-1" />
</robot>

对比之前的版本实现了代码复用

集成launch文件：在 launch 文件中直接加载 xacro

1
2
3
4
5
6
7
8
9
10
11
<launch>
    <!-- <param name="robot_description" textfile="$(find urdf01_rviz)/urdf/urdf/test.urdf" /> -->
    <param name="robot_description" command = "$(find xacro)/xacro $(find urdf01_rviz)/urdf/xacro/test.urdf.xacro" />
    <node pkg = "rviz" type = "rviz" name = "rviz" args = "-d $(find urdf01_rviz)/config/show_car.rviz" />
    <!--  关节状态发布节点 -->
    <node pkg="joint_state_publisher" type="joint_state_publisher" name="joint_state_publisher"/>
    <!--  机器人状态发布节点 -->
    <node pkg="robot_state_publisher" type="robot_state_publisher" name="robot_state_publisher"/>
    <!-- 添加控制关节运动的节点 -->
    <node pkg="joint_state_publisher_gui" type="joint_state_publisher_gui" name="joint_state_publisher_gui" />
</launch>

核心代码：

1
2
<param name="robot_description" command = "$(find xacro)/xacro $(find urdf01_rviz)/urdf/xacro/test.urdf.xacro" />

加载robot_description时使用command属性，属性值就是调用 xacro 功能包的 xacro 程序直接解析 xacro 文件

Rviz控制运动

Arbotix 是一款控制电机、舵机的控制板，并提供相应的 ros 功能包

这个功能包不仅可以驱动真实的 Arbotix 控制板，它还提供一个差速控制器，通过接受速度控制指令更新机器人的 joint 状态，从而帮助我们实现机器人在 rviz 中的运动

Arbotix使用流程：

  1. 安装 Arbotix
  2. 创建新功能包，准备机器人 urdf、xacro 文件
  3. 添加 Arbotix 配置文件
  4. 编写 launch 文件配置 Arbotix
  5. 启动 launch 文件并控制机器人模型运动

安装 Arbotix

方式1：命令行调用

1
2
sudo apt-get install ros-<<VersionName()>>-arbotix
sudo apt-get install ros-noetic-arbotix

将 <<VsersionName()>> 替换成当前 ROS 版本名称，如果提示功能包无法定位，请采用方式2

方式2：源码安装

先从 github 下载源码，然后调用 catkin_make 编译

1
git clone https://github.com/vanadiumlabs/arbotix_ros.git

创建新功能包，准备机器人 urdf、xacro

urdf 和 xacro 调用上一讲实现即可

添加 arbotix 所需的配置文件

在config文件夹下增加文件control.yaml

范本：

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
# 该文件是控制器配置,一个机器人模型可能有多个控制器，比如: 底盘、机械臂、夹持器(机械手)....
# 因此，根 name 是 controller
controllers: {
   # 单控制器设置
   base_controller: {
          #类型: 差速控制器
       type: diff_controller,
       #参考坐标(一般是根坐标系)
       base_frame_id: base_footprint, 
       #两个轮子之间的间距
       base_width: 0.2,
       #控制频率
       ticks_meter: 2000, 
       #PID控制参数，使机器人车轮快速达到预期速度
       Kp: 12, 
       Kd: 12, 
       Ki: 0, 
       Ko: 50, 
       #加速限制
       accel_limit: 1.0 
    }
}

launch 文件中配置 arbotix 节点

增添部分

1
2
3
4
<node name="arbotix" pkg="arbotix_python" type="arbotix_driver" output="screen">
     <rosparam file="$(find my_urdf05_rviz)/config/hello.yaml" command="load" />
     <param name="sim" value="true" />
</node>

  • node 调用了 arbotix_python 功能包下的 arbotix_driver 节点

  • rosparam：arbotix 驱动机器人运行时，需要获取机器人信息，可以通过 file 加载配置文件

  • param：在仿真环境下，需要配置 sim 为 true

启动 launch 文件并控制机器人模型运动

配置 rviz：

Snipaste_2025-06-15_19-48-15

控制小车运动：

发布 cmd_vel 话题消息控制小车运动

1
rostopic pub -r 10 /cmd_vel [TAB] [TAB]

URDF集成Gazebo

基本集成流程

创建功能包

导入依赖包: urdf、xacro、gazebo_ros、gazebo_ros_control、gazebo_plugins

编写URDF文件

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
<robot name="mycar">
    <link name="base_link">
        <visual>
            <geometry>
                <box size="0.5 0.2 0.1"/>
            </geometry>
            <origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0"/>
            <material name="yellow">
                <color rgba="1 1 0.0 0.5"/>
            </material>
        </visual>
        <!-- 设置碰撞参数 -->
        <!-- 如果是标准几何物体,直接复制 visual 的 geometry 和 origin 即可-->
        <collision>
            <geometry>
                <box size="0.5 0.2 0.1"/>
            </geometry>
            <origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0"/>
        </collision>
        <!-- 设置惯性矩阵 -->
        <inertial>
            <!-- 相当于设置重心偏移,如果质量均匀基本就和关节一样 -->
            <origin xyz="0.0 0.0 0.0"/>
            <mass value="2"/>
            <!-- 不同维度上的惯性值 -->
            <inertia ixx="1" ixy="0.0" ixz="0.0" iyy="1" iyz="0.0" izz="1"/>
        </inertial>
        <!-- gzebo有自己的颜色设置标签 -->
    </link>
    <gazebo reference ="base_link">
        <!-- 注意这里Gazobo和颜色首字母必须大写 -->
        <material>Gazebo/Red</material> 
    </gazebo>
</robot>

当 URDF 需要与 Gazebo 集成时，和 Rviz 有明显区别：

1.必须使用 collision 标签，因为既然是仿真环境，那么必然涉及到碰撞检测，collision 提供碰撞检测的依据。

2.必须使用 inertial 标签，此标签标注了当前机器人某个刚体部分的惯性矩阵，用于一些力学相关的仿真计算。

3.颜色设置，也需要重新使用 gazebo 标签标注，因为之前的颜色设置为了方便调试包含透明度，仿真环境下没有此选项

启动Gazebo并显示模型

launch 文件实现

1
2
3
4
5
6
7
8
<launch>
    <!-- 将 Urdf 文件的内容加载到参数服务器 -->
    <param name="robot_description" textfile = "$(find urdf02_gazebo)/urdf/demo01_helloworld.urdf"/>
    <!-- 启动 gazebo -->
    <include file="$(find gazebo_ros)/launch/empty_world.launch" />
    <!-- 在 gazebo 中显示机器人模型 -->
    <node pkg="gazebo_ros" type="spawn_model" name="model" args="-urdf -model mycar -param robot_description"  />
</launch>

代码解释：

1
2
<include file="$(find gazebo_ros)/launch/empty_world.launch" />
<!-- 启动 Gazebo 的仿真环境，当前环境为空环境 -->
1
2
3
4
5
6
7
8
9
10
11
<node pkg="gazebo_ros" type="spawn_model" name="model" args="-urdf -model mycar -param robot_description"  />

<!-- 
    在 Gazebo 中加载一个机器人模型，该功能由 gazebo_ros 下的 spawn_model 提供:
    -urdf 加载的是 urdf 文件
    -model mycar 模型名称是 mycar
    -param robot_description 从参数 robot_description 中载入模型
    -x 模型载入的 x 坐标
    -y 模型载入的 y 坐标
    -z 模型载入的 z 坐标
-->

相关设置

较之于 rviz，gazebo在集成 URDF 时，需要做些许修改

比如：必须添加 collision 碰撞属性相关参数、必须添加 inertial 惯性矩阵相关参数

另外，如果直接移植 Rviz 中机器人的颜色设置是没有显示的，颜色设置也必须做相应的变更

collision

如果机器人link是标准的几何体形状，和link的visual属性设置一致即可

inertial

惯性矩阵的设置需要结合link的质量与外形参数动态生成，标准的球体、圆柱与立方体的惯性矩阵公式如下(封装为 xacro 实现)：

球体惯性矩阵：

1
2
3
4
5
6
7
8
9
<!-- Macro for inertia matrix -->
    <xacro:macro name="sphere_inertial_matrix" params="m r">
        <inertial>
            <mass value="${m}" />
            <inertia ixx="${2*m*r*r/5}" ixy="0" ixz="0"
                iyy="${2*m*r*r/5}" iyz="0" 
                izz="${2*m*r*r/5}" />
        </inertial>
    </xacro:macro>

圆柱惯性矩阵：

1
2
3
4
5
6
7
8
<xacro:macro name="cylinder_inertial_matrix" params="m r h">
        <inertial>
            <mass value="${m}" />
            <inertia ixx="${m*(3*r*r+h*h)/12}" ixy = "0" ixz = "0"
                iyy="${m*(3*r*r+h*h)/12}" iyz = "0"
                izz="${m*r*r/2}" /> 
        </inertial>
    </xacro:macro>

立方体惯性矩阵：

1
2
3
4
5
6
7
8
<xacro:macro name="Box_inertial_matrix" params="m l w h">
      <inertial>
              <mass value="${m}" />
              <inertia ixx="${m*(w*w + h*h)/12}" ixy = "0" ixz = "0"
                  iyy="${m*(h*h + l*l)/12}" iyz= "0"
                  izz="${m*(l*l + w*w)/12}" />
      </inertial>
  </xacro:macro>

原则上，除了 base_footprint 外，机器人的每个刚体部分都需要设置惯性矩阵，且惯性矩阵必须经计算得出，如果随意定义刚体部分的惯性矩阵，那么可能会导致机器人在 Gazebo 中出现抖动，移动等现象

在 gazebo 中显示 link 的颜色，必须要使用指定的标签

1
2
3
<gazebo reference="link节点名称">
     <material>Gazebo/Blue</material>
</gazebo>

material 标签中，设置的值区分大小写，颜色可以设置为 Red Blue Green Black

实操

需求描述：将之前的机器人模型(xacro版)显示在 gazebo 中

实现流程：

  1. 需要编写封装惯性矩阵算法的 xacro 文件
  2. 为机器人模型中的每一个 link 添加 collision 和 inertial 标签，并且重置颜色属性
  3. 在 launch 文件中启动 gazebo 并添加机器人模型

编写封装惯性矩阵算法的 xacro 文件

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
<robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="base">
    <!-- Macro for inertia matrix -->
    <xacro:macro name="sphere_inertial_matrix" params="m r">
        <inertial>
            <mass value="${m}" />
            <inertia ixx="${2*m*r*r/5}" ixy="0" ixz="0"
                iyy="${2*m*r*r/5}" iyz="0" 
                izz="${2*m*r*r/5}" />
        </inertial>
    </xacro:macro>

    <xacro:macro name="cylinder_inertial_matrix" params="m r h">
        <inertial>
            <mass value="${m}" />
            <inertia ixx="${m*(3*r*r+h*h)/12}" ixy = "0" ixz = "0"
                iyy="${m*(3*r*r+h*h)/12}" iyz = "0"
                izz="${m*r*r/2}" /> 
        </inertial>
    </xacro:macro>

     <xacro:macro name="Box_inertial_matrix" params="m l w h">
       <inertial>
               <mass value="${m}" />
               <inertia ixx="${m*(w*w + h*h)/12}" ixy = "0" ixz = "0"
                   iyy="${m*(h*h + l*l)/12}" iyz= "0"
                   izz="${m*(l*l + w*w)/12}" />
       </inertial>
   </xacro:macro>
</robot>

复制相关 xacro 文件，并设置 collision inertial 以及 color 等参数

将上一节的xacro文件复制一份进行修改

以底盘为例：

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
...  
	<xacro:property name="base_mass" value="2"/>
    <!-- 添加底盘 -->
    <link name="base_link">
        <visual>
            <geometry>
                <cylinder radius="${base_radius}" length="${base_length}"/>
            </geometry>
            <origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0"/>
        </visual>
        <collision>
            <geometry>
                <cylinder radius="${base_radius}" length="${base_length}"/>
            </geometry>
        </collision>
        <!-- 调用惯性矩阵函数 -->
        <xacro:cylinder_inertial_matrix m="${base_mass}" r="${base_radius}" h="${base_length}" />
    </link>
    <gazebo reference="base_link">
        <material>Gazebo/Yellow</material>
    </gazebo>
...
    <!-- 驱动轮属性 -->
    <xacro:property name="wheel_mass" value="0.05" /><!-- 质量 -->
    <!-- 驱动轮宏 -->
    <!-- 
        direction: left or right
        flag: 1 or -1
    -->
    <xacro:macro name = "wheel_func" params="direction flag">
        <link name="${direction}_wheel">
            <visual>
                <geometry>
                    <cylinder radius="${wheel_radius}" length="${wheel_length}"/>
                </geometry>
                <origin xyz="0.0 0.0 0.0" rpy="${flag*PI/2} 0.0 0.0"/>
            </visual>
            <collision>
                <geometry>
                    <cylinder radius="${wheel_radius}" length="${wheel_length}"/>
                </geometry>
                <origin xyz="0.0 0.0 0.0" rpy="${flag*PI/2} 0.0 0.0"/>
            </collision>
            <xacro:cylinder_inertial_matrix m="${wheel_mass}" r="${wheel_radius}" h="${wheel_length}" />
        </link> 
        <gazebo reference="${direction}_wheel">
            <material>Gazebo/Red</material>
        </gazebo>
    ...
    </xacro:macro>

    <xacro:wheel_func direction="left" flag="1" />
    <xacro:wheel_func direction="right" flag="-1" />
...
    <!-- 万向轮属性 -->
    <xacro:property name = "support_wheel_mass" value ="0.01" />

    <!-- 万向轮(支撑轮)宏 -->  
    <xacro:macro name = "add_support_wheel" params="direction flag">
        <link name="${direction}_wheel">
            <visual>
                <geometry>
                    <sphere radius="${support_wheel_radius}"/>
                </geometry>
                <origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0"/>
            </visual>
            <collision>
                <geometry>
                    <sphere radius="${support_wheel_radius}"/>
                </geometry>
                <origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0"/>
            </collision>
            <xacro:sphere_inertial_matrix m="${support_wheel_mass}" r="${support_wheel_radius}" />
        </link>
        <gazebo reference="${direction}_wheel">
            <material>Gazebo/Red</material>
        </gazebo>
    ...
    </xacro:macro>

    <xacro:add_support_wheel direction="front" flag="1" />
    <xacro:add_support_wheel direction="back" flag="-1" />
</robot>

主要删除原有visual下的material标签，然后在link标签内增加collision标签以及调用惯性矩阵函数，在link标签外使用gazebo下的material附加颜色

如果机器人模型在 Gazebo 中产生了抖动，滑动，缓慢位移诸如此类情况，需要考虑：

  1. 惯性矩阵是否设置了，且设置是否正确合理
  2. 车轮翻转需要依赖于 PI 值，如果 PI 值精度偏低，也可能导致上述情况产生

同理修改camera和laser，具体代码不附

仿真环境搭建

到目前为止，我们已经可以将机器人模型显示在 Gazebo 之中了，但是当前默认情况下，在 Gazebo 中机器人模型是在 empty world 中，并没有类似于房间、家具、道路、树木… 之类的仿真物

Gazebo 中创建仿真实现方式有两种：

  • 方式1: 直接添加内置组件创建仿真环境
  • 方式2: 手动绘制仿真环境(更为灵活)

也还可以直接下载使用官方或第三方提高的仿真环境插件

将之前下载的资源中box_house.world移动到当前功能包目录下的worlds文件夹中

改写launch文件

1
2
3
4
5
6
7
8
9
<launch>
    <param name="robot_description" command="$(find xacro)/xacro $(find urdf02_gazebo)/urdf/xacro/combination.urdf.xacro"/>
    <!-- 启动 gazebo -->
    <include file="$(find gazebo_ros)/launch/empty_world.launch" >
        <arg name="world_name" value="$(find urdf02_gazebo)/worlds/box_house.world" />
    </include>
    <!-- 在 gazebo 中显示机器人模型 -->
    <node pkg="gazebo_ros" type="spawn_model" name="model" args="-urdf -model mycar -param robot_description"  />
</launch>

核心代码：启动 empty_world 后，再根据arg加载自定义的仿真环境

注意，arg的name固定，只能是world_name

当前 Gazebo 提供的仿真道具有限，还可以下载官方支持，可以提供更为丰富的仿真实现

1
git clone https://github.com/osrf/gazebo_models

将得到的gazebo_models文件夹内容复制到 /usr/share/gazebo-*/models

重启 Gazebo，选择左侧菜单栏的 insert 可以选择并插入相关道具了

1
rosrun gazebo_ros gazebo

URDF、Gazebo与Rviz综合应用

本节中大量配置文件来源于官方网站，但是网站已经更新到ROS2了，可能有的直接复制教程里面的了

机器人运动控制

gazebo 中已经可以正常显示机器人模型了，那么如何像在 rviz 中一样控制机器人运动呢？

需要涉及到ros中的组件：ros_control

ros_control 是一组软件包，它包含了控制器接口，控制器管理器，传输和硬件接口

ros_control 是一套规范，不同的机器人平台只要按照这套规范实现，那么就可以保证与ROS 程序兼容，通过这套规范，实现了一种可插拔的架构设计，大大提高了程序设计的效率与灵活性

运动控制实现流程Gazebo：

  1. 编写一个单独的 xacro 文件，为机器人模型添加传动装置以及控制器
  2. 将此文件集成进xacro文件
  3. 启动 Gazebo 并发布 /cmd_vel 消息控制机器人运动

为 joint 添加传动装置以及控制器：

新建 Xacro 文件，配置两轮差速：(直接复制)

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
<robot name="my_car_move" xmlns:xacro="http://wiki.ros.org/xacro">

    <!-- 传动实现:用于连接控制器与关节 -->
    <xacro:macro name="joint_trans" params="joint_name">
        <!-- Transmission is important to link the joints and the controller -->
        <transmission name="${joint_name}_trans">
            <type>transmission_interface/SimpleTransmission</type>
            <joint name="${joint_name}">
                <hardwareInterface>hardware_interface/VelocityJointInterface</hardwareInterface>
            </joint>
            <actuator name="${joint_name}_motor">
                <hardwareInterface>hardware_interface/VelocityJointInterface</hardwareInterface>
                <mechanicalReduction>1</mechanicalReduction>
            </actuator>
        </transmission>
    </xacro:macro>

    <!-- 每一个驱动轮都需要配置传动装置 -->
    <!-- 需要修改的位置 -->
    <xacro:joint_trans joint_name="left_wheel2base_link" />
    <xacro:joint_trans joint_name="right_wheel2base_link" />

    <!-- 控制器 -->
    <gazebo>
        <plugin name="differential_drive_controller" filename="libgazebo_ros_diff_drive.so">
            <rosDebugLevel>Debug</rosDebugLevel>
            <publishWheelTF>true</publishWheelTF>
            <robotNamespace>/</robotNamespace>
            <publishTf>1</publishTf>
            <publishWheelJointState>true</publishWheelJointState>
            <alwaysOn>true</alwaysOn>
            <updateRate>100.0</updateRate>
            <legacyMode>true</legacyMode>
            <leftJoint>left_wheel2base_link</leftJoint> <!-- 左轮 -->
            <rightJoint>right_wheel2base_link</rightJoint> <!-- 右轮 -->
            <wheelSeparation>${base_link_radius * 2}</wheelSeparation> <!-- 车轮间距 -->
            <wheelDiameter>${wheel_radius * 2}</wheelDiameter> <!-- 车轮直径 -->
            <broadcastTF>1</broadcastTF>
            <wheelTorque>30</wheelTorque>
            <wheelAcceleration>1.8</wheelAcceleration>
            <commandTopic>cmd_vel</commandTopic> <!-- 运动控制话题 -->
            <odometryFrame>odom</odometryFrame> 
            <odometryTopic>odom</odometryTopic> <!-- 里程计话题 -->
            <robotBaseFrame>base_footprint</robotBaseFrame> <!-- 根坐标系 -->
        </plugin>
    </gazebo>

</robot>

将上述 xacro 文件集成进总的机器人模型xacro文件

1
2
3
4
5
6
7
8
9
10
<robot name="my_car_camera" xmlns:xacro="http://wiki.ros.org/xacro">
    <!-- 包含惯性矩阵 -->
    <xacro:include filename="head.xacro"/>
    <!-- 组合小车底盘与摄像头与雷达 -->
    <xacro:include filename="base.urdf.xacro"/>
    <xacro:include filename="camera.urdf.xacro"/>
    <xacro:include filename="laser.urdf.xacro"/>
    <!-- 运动控制 -->
    <xacro:include filename="$(find urdf02_gazebo)/urdf/gazebo/move.xacro"/>
</robot>

launch文件不变（先启动gazebo虚拟环境，再启动rviz）

使用命令控制小车运动

1
rosrun teleop_twist_keyboard  teleop_twist_keyboard.py

Rviz查看里程计信息

里程计：利用从移动传感器获得的数据来估计物体位置随时间的变化而改变的方法

启动 Rviz

1
2
3
4
5
6
7
<launch>
    <node pkg = "rviz" type = "rviz" name = "rviz" args = "-d $(find urdf01_rviz)/config/show_car.rviz" />
    <!--  关节状态发布节点 -->
    <node pkg="joint_state_publisher" type="joint_state_publisher" name="joint_state_publisher"/>
    <!--  机器人状态发布节点 -->
    <node pkg="robot_state_publisher" type="robot_state_publisher" name="robot_state_publisher"/>
</launch>

执行 launch 文件后，在 Rviz 中添加图示组件：

21_Rviz显示里程计数据

雷达信息仿真

雷达仿真基本流程:

  1. 已经创建完毕的机器人模型，编写一个单独的 xacro 文件，为机器人模型添加雷达配置；
  2. 将此文件集成进xacro文件；
  3. 启动 Gazebo，使用 Rviz 显示雷达信息。

新建 Xacro 文件，配置雷达传感器信息：(直接复制)

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
<robot name="my_sensors" xmlns:xacro="http://wiki.ros.org/xacro">

  <!-- 雷达 -->
  <gazebo reference="laser">
    <sensor type="ray" name="rplidar">
      <pose>0 0 0 0 0 0</pose>
      <visualize>true</visualize>
      <update_rate>5.5</update_rate>
      <ray>
        <scan>
          <!-- 雷达参数 -->
          <horizontal>
            <!-- 采样个数,与仿真压力挂钩 -->
            <samples>360</samples>  
            <resolution>1</resolution>
            <!-- 采样弧度范围 -->
            <min_angle>-3</min_angle>
            <max_angle>3</max_angle>
          </horizontal>
        </scan>
        <!-- 采样距离范围 -->
        <range>
          <min>0.10</min>
          <max>30.0</max>
          <!-- 距离精度 -->
          <resolution>0.01</resolution>
        </range>
        <!-- 仿真噪声 -->
        <noise>
          <type>gaussian</type>
          <mean>0.0</mean>
          <stddev>0.01</stddev>
        </noise>
      </ray>
      <plugin name="gazebo_rplidar" filename="libgazebo_ros_laser.so">
        <topicName>/scan</topicName>
        <frameName>laser</frameName>
      </plugin>
    </sensor>
  </gazebo>

</robot>

gazebo reference / frameName 这两个位置的名称和定义雷达的link名称需要相同

集成到总的xacro 文件中：

1
2
3
4
5
<robot name="my_car_camera" xmlns:xacro="http://wiki.ros.org/xacro">
    ...
    <!-- 雷达 -->
    <xacro:include filename="$(find urdf02_gazebo)/urdf/gazebo/laser.xacro"/>
</robot>

launch文件不变，先启动gazebo再启动rviz

添加雷达信息显示插件

雷达仿真2

摄像头信息仿真

步骤类似雷达

新建 Xacro 文件，配置摄像头传感器信息：(直接复制)

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
<robot name="my_sensors" xmlns:xacro="http://wiki.ros.org/xacro">
  <!-- 被引用的link -->
  <gazebo reference="camera">
    <!-- 类型设置为 camara -->
    <sensor type="camera" name="camera_node">
      <update_rate>30.0</update_rate> <!-- 更新频率 -->
      <!-- 摄像头基本信息设置 -->
      <camera name="head">
        <horizontal_fov>1.3962634</horizontal_fov>
        <image>
          <width>1280</width>
          <height>720</height>
          <format>R8G8B8</format>
        </image>
        <clip>
          <near>0.02</near>
          <far>300</far>
        </clip>
        <noise>
          <type>gaussian</type>
          <mean>0.0</mean>
          <stddev>0.007</stddev>
        </noise>
      </camera>
      <!-- 核心插件 -->
      <plugin name="gazebo_camera" filename="libgazebo_ros_camera.so">
        <alwaysOn>true</alwaysOn>
        <updateRate>0.0</updateRate>
        <!-- 为话题设置一个命名空间 -->
        <cameraName>/camera</cameraName>
        <imageTopicName>image_raw</imageTopicName>
        <cameraInfoTopicName>camera_info</cameraInfoTopicName>
        <!-- link名称 -->
        <frameName>camera</frameName>
        <hackBaseline>0.07</hackBaseline>
        <distortionK1>0.0</distortionK1>
        <distortionK2>0.0</distortionK2>
        <distortionK3>0.0</distortionK3>
        <distortionT1>0.0</distortionT1>
        <distortionT2>0.0</distortionT2>
      </plugin>
    </sensor>
  </gazebo>
</robot>

集成到总的xacro文件

1
2
3
4
5
6
<robot name="my_car_camera" xmlns:xacro="http://wiki.ros.org/xacro">
    ...
    <!-- 摄像头 -->
    <xacro:include filename="$(find urdf02_gazebo)/urdf/gazebo/camera.xacro"/>

</robot>

launch文件不变，先启动gazebo再启动rviz

在 Rviz 中添加摄像头组件

摄像头仿真3

kinect信息仿真

kinect是深度信息摄像头

步骤类似上述

新建 Xacro 文件，配置 kinetic传感器信息(直接复制)：

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
<robot name="my_sensors" xmlns:xacro="http://wiki.ros.org/xacro">
    <gazebo reference="kinect link名称">  
      <sensor type="depth" name="camera">
        <always_on>true</always_on>
        <update_rate>20.0</update_rate>
        <camera>
          <horizontal_fov>${60.0*PI/180.0}</horizontal_fov>
          <image>
            <format>R8G8B8</format>
            <width>640</width>
            <height>480</height>
          </image>
          <clip>
            <near>0.05</near>
            <far>8.0</far>
          </clip>
        </camera>
        <plugin name="kinect_camera_controller" filename="libgazebo_ros_openni_kinect.so">
          <cameraName>camera</cameraName>
          <alwaysOn>true</alwaysOn>
          <updateRate>10</updateRate>
          <imageTopicName>rgb/image_raw</imageTopicName>
          <depthImageTopicName>depth/image_raw</depthImageTopicName>
          <pointCloudTopicName>depth/points</pointCloudTopicName>
          <cameraInfoTopicName>rgb/camera_info</cameraInfoTopicName>
          <depthImageCameraInfoTopicName>depth/camera_info</depthImageCameraInfoTopicName>
          <frameName>kinect link名称</frameName>
          <baseline>0.1</baseline>
          <distortion_k1>0.0</distortion_k1>
          <distortion_k2>0.0</distortion_k2>
          <distortion_k3>0.0</distortion_k3>
          <distortion_t1>0.0</distortion_t1>
          <distortion_t2>0.0</distortion_t2>
          <pointCloudCutoff>0.4</pointCloudCutoff>
        </plugin>
      </sensor>
    </gazebo>
</robot>

可以把上一步camera的xacro注释掉，把camera换成kinect

集成到总的xacro 文件中：

1
2
3
4
5
6
7
<robot name="my_car_camera" xmlns:xacro="http://wiki.ros.org/xacro">
    ...
    <!-- 摄像头 -->
    <!-- <xacro:include filename="$(find urdf02_gazebo)/urdf/gazebo/camera.xacro"/> -->
    <!-- kinect -->
    <xacro:include filename="$(find urdf02_gazebo)/urdf/gazebo/kinect.xacro"/>
</robot>

launch文件不变，先启动gazebo再启动rviz

kinect 点云数据显示：

点云数据_默认

但在rviz中显示时错位

原因：在kinect中图像数据与点云数据使用了两套坐标系统，且两套坐标系统位姿并不一致

在插件中为kinect设置坐标系，修改配置文件的<frameName>标签内容

1
<frameName>camera_depth</frameName>

发布新设置的坐标系到kinect连杆的坐标变换关系，在启动rviz的launch中，添加

1
2
<!-- 添加点云坐标系到kinect坐标系的变化=换  -->
    <node pkg="tf2_ros" type="static_transform_publisher" name="static_transform_publisher" args =" 0 0 0 -1.57 0 -1.57 /camera /camera_depth"/>

为什么是-1.57：逆着轴方向看逆时针旋转为正

启动rviz，重新显示

总结

本章主要介绍了ROS中仿真实现涉及的三大知识点：

  • URDF(Xacro)
  • Rviz
  • Gazebo

URDF 是用于描述机器人模型的 xml 文件，可以使用不同的标签具代表不同含义，URDF 编写机器人模型代码冗余，xacro 可以优化 URDF 实现，代码实现更为精简、高效、易读

rviz需要已有数据，强调把已有的数据可视化显示

rviz提供了很多插件，这些插件可以显示图像、模型、路径等信息，但是前提都是这些数据已经以话题、参数的形式发布，rviz做的事情就是订阅这些数据，并完成可视化的渲染

gazebo是三维物理仿真平台，创建虚拟的仿真环境，**不需要数据，而是创造数据，**不仅可以仿真机器人的运动功能，还可以仿真机器人的传感器数据

机器人导航(仿真)

导航是机器人系统中最重要的模块之一

概述

在ROS中机器人导航(Navigation)由多个功能包组合实现，ROS 中又称之为导航功能包集

关于导航模块，官方介绍：一个二维导航堆栈，它接收来自里程计、传感器流和目标姿态的信息，并输出发送到移动底盘的安全速度命令

ROS 中导航相关的功能包集为机器人导航提供了一套通用的实现，开发者不再需要关注于导航算法、硬件交互等偏复杂、偏底层的实现，这些实现都由更专业的研发人员管理、迭代和维护，开发者可以更专注于上层功能，而对于导航功能的调用，只需要根据自身机器人相关参数合理设置各模块的配置文件即可

简而言之调参

导航模块简介

ROS 官方为了提供了一张导航功能包集的图示，该图中囊括了 ROS 导航的一些关键技术：

02_导航官方架构

这个图的解读：

先看图例：

  • provided node：必须节点
  • optional provided node：可选择节点
  • platform specific node：平台相关节点

外部数据输入：

  • map_server：读取或发布静态环境地图（occupancy grid），供全局规划器和代价地图使用
  • sensor sources：各类传感器数据（激光雷达、深度相机等），用于构建局部环境的代价地图
  • odometry source：里程计数据（车轮编码器、惯导等），提供机器人相对运动信息
  • amcl：自适应 Monte‑Carlo 本地化（Adaptive Monte Carlo Localization），结合传感器与地图给出机器人在地图中的全局位姿
  • sensor transforms：TF 数 据，负责把各个传感器坐标系和机器人基座（base_link）坐标系对齐

move_base 核心：

  • global_costmap：用静态地图（map_server）以及部分传感器数据（静态障碍）构建的全局代价栅格（occupancy grid），主要用于规划从当前位置到目标位置的全局路径
  • global_planner：基于 global_costmap 和 amcl 提供的位姿，运行 A*、Dijkstra 或 Navfn 等算法，生成一条从起点到目标点的全局路径
  • local_costmap：动态地整合传感器实时数据，构建机器人周围的局部代价栅格，用于避障
  • local_planner：接收全局路径（waypoints）和 local_costmap，生成实际的速度命令（线速度、角速度）给底层运动控制器（base controller），在避障的同时跟踪全局路径
  • recovery_behaviors：当局部规划失败（如被障碍物完全包围、无法前进）时，触发恢复行为：比如向后退、旋转扫描、清空局部地图等，尝试脱困

底层执行：

  • base controller：接收 local_planner 输出的速度指令，驱动机器人底盘（电机、差分驱动或者四轮全向等）去移动
Snipaste_2025-06-21_16-22-29

总结下来，涉及的关键技术有如下五点:

  1. 全局地图
  2. 自身定位
  3. 路径规划
  4. 运动控制
  5. 环境感知

机器人导航实现与无人驾驶类似，关键技术也是由上述五点组成，只是无人驾驶是基于室外的，而当前介绍的机器人导航更多是基于室内的

全局地图：

  1. SLAM(simultaneous localization and mapping)，也称为CML (Concurrent Mapping and Localization)，即时定位与地图构建
  2. SLAM问题：机器人在未知环境中从一个未知位置开始移动,在移动过程中根据位置估计和地图进行自身定位，同时在自身定位的基础上建造增量式地图，以绘制出外部环境的完全地图
  3. 如果要完成 SLAM ，机器人必须要具备感知外界环境的能力，尤其是要具备获取周围环境深度信息的能力。感知的实现需要依赖于传感器，比如：激光雷达、摄像头、RGB-D摄像头…
  4. SLAM 可以用于地图生成，而生成的地图还需要被保存以待后续使用，在 ROS 中保存地图的功能包是 map_server

SLAM 虽然是机器人导航的重要技术之一，但是二者并不等价，确切的讲，SLAM 只是实现地图构建和即时定位

自身定位：

导航开始和过程中，机器人都需要确定当前自身的位置

如果在室外，那么 GPS 是一个不错的选择，而如果室内、隧道、地下或一些特殊的屏蔽 GPS 信号的区域，由于 GPS 信号弱化甚至完全不可用，那么就必须另辟蹊径了

前面提到的 SLAM 就可以实现自身定位，除此之外，ROS 中还提供了一个用于定位的功能包：amcl (ROS1/Ros2 经典实现已经老化与不够鲁棒)

路径规划：

在 ROS 中提供了 move_base 包来实现路径规则，该功能包主要由两大规划器组成：

  1. 全局路径规划(gloable_planner)

    根据给定的目标点和全局地图实现总体的路径规划，使用 Dijkstra 或 A* 算法进行全局路径规划，计算最优路线，作为全局路线

  2. 本地时时规划(local_planner)

    在实际导航过程中，机器人可能无法按照给定的全局最优路线运行，本地规划的作用就是使用一定算法(Dynamic Window Approaches) 来实现障碍物的规避，并选取当前最优路径以尽量符合全局最优路径

运动控制：

导航功能包集假定通过话题”cmd_vel”发布geometry_msgs/Twist类型的消息，这个消息基于机器人的基坐标系，传递的是运动命令。这意味着必须有一个节点订阅”cmd_vel”话题，将该话题上的速度命令转换为电机命令并发送

导航之坐标系

定位实现需要依赖于机器人自身，机器人需要逆向推导参考系原点并计算坐标系相对关系，该过程实现常用方式有两种：

  • 通过里程计定位：时时收集机器人的速度信息计算并发布机器人坐标系与父级参考系的相对关系
  • 通过传感器定位：通过传感器收集外界环境信息通过匹配计算并发布机器人坐标系与父级参考系的相对关系

两种定位方式都有各自的优缺点

里程计定位:

  • 优点：里程计定位信息是连续的，没有离散的跳跃
  • 缺点：里程计存在累计误差，不利于长距离或长期定位

传感器定位:

  • 优点：比里程计定位更精准；
  • 缺点：传感器定位会出现跳变的情况，且传感器定位在标志物较少的环境下，其定位精度会大打折扣

坐标系变换：

上述两种定位实现中，机器人坐标系一般使用机器人模型中的根坐标系(base_link 或 base_footprint)

  • 里程计定位时，父级坐标系一般称之为 odom
  • 传感器定位时，父级参考系一般称之为 map

当二者结合使用时，map 和 odom 都是机器人模型根坐标系的父级，这是不符合坐标变换中”单继承”的原则的

一般会将转换关系设置为：map -> odom -> base_link 或 base_footprint

因为map一般精度更高

导航条件说明

硬件

虽然导航功能包集被设计成尽可能的通用，在使用时仍然有三个主要的硬件限制：

  1. 它是为差速驱动的轮式机器人设计的。它假设底盘受到理想的运动命令的控制并可实现预期的结果，命令的格式为：x速度分量，y速度分量，$\theta$分量
  2. 它需要在底盘上安装一个单线激光雷达，这个激光雷达用于构建地图和定位
  3. 导航功能包集是为正方形的机器人开发的，所以方形或圆形的机器人将是性能最好的。 它也可以工作在任意形状和大小的机器人上，但是较大的机器人将很难通过狭窄的空间

软件

在仿真环境下，机器人可以正常接收 /cmd_vel 消息，并发布里程计消息，传感器消息发布也正常，也即导航模块中的运动控制和环境感知实现完毕

主要关注于: 使用 SLAM 绘制地图、地图服务、自身定位与路径规划

导航实现

准备工作

先安装相关的ROS功能包：

  • 安装 gmapping 包(用于构建地图)

    1
    sudo apt install ros-noetic-gmapping

  • 安装地图服务包(用于保存与读取地图)

    1
    sudo apt install ros-noetic-map-server

  • 安装 navigation 包(用于定位以及路径规划)

    1
    sudo apt install ros-noetic-navigation

安装map-server时会出现错误，提示：

1
2
3
4
5
6
7
8
9
有一些软件包无法被安装。如果您用的是 unstable 发行版，这也许是
因为系统无法达到您要求的状态造成的。该版本中可能会有一些您需要的软件
包尚未被创建或是它们已被从新到(Incoming)目录移出。
下列信息可能会对解决问题有所帮助：

下列软件包有未满足的依赖关系：
 ros-noetic-map-server : 依赖: libsdl-image1.2-dev 但是它将不会被安装
                         依赖: libsdl1.2-dev 但是它将不会被安装
E: 无法修正错误，因为您要求某些软件包保持现状，就是它们破坏了软件包间的依赖关系

需要通过降级兼容，安装 aptitude 工具

1
2
sudo apt install aptitude
sudo aptitude install libsdl-image1.2-dev libsdl1.2-dev

选择 n y y

参考文章：ROS安装软件包有未满足的依赖关系应如何解决

新建功能包，并导入依赖: gmapping map_server amcl move_base

因为不需要编码，roscpp rospy可以不导入

SLAM建图

SLAM算法有多种，当前选用gmapping

gmapping可以根据移动机器人里程计数据和激光雷达数据来绘制二维的栅格地图

gmapping 功能包中的核心节点是 slam_gmapping

节点内容

订阅的Topic：

  • tf (tf/tfMessage) ：用于雷达、底盘与里程计之间的坐标变换消息
  • scan(sensor_msgs/LaserScan)：SLAM所需的雷达信息

发布的Topic：

  • map_metadata(nav_msgs/MapMetaData)：地图元数据，包括地图的宽度、高度、分辨率等，该消息会固定更新
  • map(nav_msgs/OccupancyGrid)：地图栅格数据，一般会在rviz中以图形化的方式显示
  • ~entropy(std_msgs/Float64)：机器人姿态分布熵估计(值越大，不确定性越大)

注意，~代表私有，具体见基础知识部分的名称重名

服务：

  • dynamic_map(nav_msgs/GetMap)：用于获取地图数据

参数（参数较多，下面是几个较为常用的参数）：

  • ~base_frame(string, default:”base_link”)：机器人基坐标系
  • ~map_frame(string, default:”map”)：地图坐标系
  • ~odom_frame(string, default:”odom”)：里程计坐标系
  • ~map_update_interval(float, default: 5.0)：地图更新频率，根据指定的值设计更新间隔
  • ~maxUrange(float, default: 80.0)：激光探测的最大可用范围(超出此阈值，被截断)
  • ~maxRange(float)：激光探测的最大范围

所需的坐标变换：

  • 雷达坐标系→基坐标系：一般由 robot_state_publisher 或 static_transform_publisher 发布
  • 基坐标系→里程计坐标系：一般由里程计节点发布

发布的坐标变换：

  • 地图坐标系→里程计坐标系

gmapping使用

launch文件：(直接复制)

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
<launch>
<!-- 仿真环境下value设为true -->
<param name="use_sim_time" value="true"/>
    <!-- gmapping节点 -->
    <node pkg="gmapping" type="slam_gmapping" name="slam_gmapping" output="screen">
      <!-- 设置雷达话题 -->
      <remap from="scan" to="scan"/>
      <!-- 设置关键参数:坐标系 -->
      <param name="base_frame" value="base_footprint"/><!--底盘坐标系-->
      <param name="map_frame" value="map"/> <!--地图坐标系-->
      <param name="odom_frame" value="odom"/> <!--里程计坐标系-->
      <param name="map_update_interval" value="5.0"/>
      <param name="maxUrange" value="16.0"/>
      <param name="sigma" value="0.05"/>
      <param name="kernelSize" value="1"/>
      <param name="lstep" value="0.05"/>
      <param name="astep" value="0.05"/>
      <param name="iterations" value="5"/>
      <param name="lsigma" value="0.075"/>
      <param name="ogain" value="3.0"/>
      <param name="lskip" value="0"/>
      <param name="srr" value="0.1"/>
      <param name="srt" value="0.2"/>
      <param name="str" value="0.1"/>
      <param name="stt" value="0.2"/>
      <param name="linearUpdate" value="1.0"/>
      <param name="angularUpdate" value="0.5"/>
      <param name="temporalUpdate" value="3.0"/>
      <param name="resampleThreshold" value="0.5"/>
      <param name="particles" value="30"/>
      <param name="xmin" value="-50.0"/>
      <param name="ymin" value="-50.0"/>
      <param name="xmax" value="50.0"/>
      <param name="ymax" value="50.0"/>
      <param name="delta" value="0.05"/>
      <param name="llsamplerange" value="0.01"/>
      <param name="llsamplestep" value="0.01"/>
      <param name="lasamplerange" value="0.005"/>
      <param name="lasamplestep" value="0.005"/>
    </node>

    <node pkg="joint_state_publisher" name="joint_state_publisher" type="joint_state_publisher" />
    <node pkg="robot_state_publisher" name="robot_state_publisher" type="robot_state_publisher" />

    <node pkg="rviz" type="rviz" name="rviz" />
    <!-- 可以保存 rviz 配置并后期直接使用-->
    <!--
    <node pkg="rviz" type="rviz" name="rviz" args="-d $(find nav_demo)/config/nav.rviz"/>
    -->
</launch>

启动过程：

  1. 先启动 Gazebo 仿真环境
  2. 然后再启动地图绘制的 launch 文件
  3. 启动键盘键盘控制节点，用于控制机器人运动建图
  4. 在 rviz 中添加组件，显示栅格地图
slam演示

可以通过键盘控制gazebo中的机器人运动

机器人跑一圈会获得完整地图数据

接下来要保存地图，编写一个launch文件，内容如下：

1
2
3
4
<launch>
    <arg name="filename" value="$(find nav_demo)/map/nav" />
    <node name="map_save" pkg="map_server" type="map_saver" args="-f $(arg filename)" />
</launch>

SLAM建图完毕后，执行该launch文件即可

在指定路径下会生成两个文件，xxx.pgm 与 xxx.yaml

xxx.pgm 本质是一张图片，直接使用图片查看程序即可打开

xxx.yaml 保存的是地图的元数据信息，用于描述图片

1
2
3
4
5
6
7
8
9
10
11
12
13
14
# 声明图片路径,图片也可以根据需求编辑
image: /home/ros/demo05_ws/src/nav_demo/map/nav.pgm
# 地图刻度尺 米/每像素
resolution: 0.050000
# 地图位姿信息(相对于rviz原点,x,y,偏航角)
origin: [-50.000000, -50.000000, 0.000000]
# 白色是可通行区域，黑色是障碍物，蓝灰是未知区域
# 占用阈值
occupied_thresh: 0.65
# 空闲阈值
free_thresh: 0.196
# 二者结合用于判断是不是障碍物
# 取反
negate: 0

map_server 中障碍物计算规则：

  1. 地图中的每一个像素取值在 [0,255] 之间，白色为 255，黑色为 0，该值设为 x；

  2. map_server 会将像素值作为判断是否是障碍物的依据，首先计算比例：p = (255 - x) / 255.0，白色为0，黑色为1

    negate为true，则p = x / 255.0

  3. 根据步骤2计算的比例判断是否是障碍物，如果 p > occupied_thresh 那么视为障碍物，如果 p < free_thresh 那么视为无物

地图服务

map_server节点

发布话题：

  • map_metadata（nav_msgs / MapMetaData）：发布地图元数据
  • map（nav_msgs / OccupancyGrid）：地图数据

服务：

  • static_map（nav_msgs / GetMap）：通过此服务获取地图

参数：

  • 〜frame_id（字符串，默认值：“map”）：地图坐标系

通过 map_server 的 map_server 节点可以读取栅格地图数据，编写 launch 文件：

1
2
3
4
5
6
<launch>
    <!-- 设置地图的配置文件 -->
    <arg name="map" default="nav.yaml" />
    <!-- 运行地图服务器，并且加载设置的地图-->
    <node name="map_server" pkg="map_server" type="map_server" args="$(find nav_demo)/map/$(arg map)"/>
</launch>

执行该launch文件，该节点会发布话题：map(nav_msgs/OccupancyGrid)

在 rviz 中使用 map 组件可以显示栅格地图

定位

SLAM中也包含定位算法实现，不过SLAM的定位是用于构建全局地图的，是属于导航开始之前的阶段

当前定位是用于导航中，机器人需要按照设定的路线运动，通过定位可以判断机器人的实际轨迹是否符合预期

AMCL(adaptive Monte Carlo Localization) 是用于2D移动机器人的概率定位系统，它实现了自适应（或KLD采样）蒙特卡洛定位方法，可以根据已有地图使用粒子滤波器推算机器人位置

amcl已经被集成到了navigation包

订阅的Topic：

  • scan(sensor_msgs/LaserScan)：激光雷达数据

  • tf(tf/tfMessage)：坐标变换消息

  • initialpose(geometry_msgs/PoseWithCovarianceStamped)：用来初始化粒子滤波器的均值和协方差

  • map(nav_msgs/OccupancyGrid)：获取地图数据

发布的Topic：

  • amcl_pose(geometry_msgs/PoseWithCovarianceStamped)：机器人在地图中的位姿估计

  • particlecloud(geometry_msgs/PoseArray)：位姿估计集合，rviz中可以被 PoseArray 订阅然后图形化显示机器人的位姿估计集合。

  • tf(tf/tfMessage)：发布从 odom 到 map 的转换

服务：

  • global_localization(std_srvs/Empty)：初始化全局定位的服务

  • request_nomotion_update(std_srvs/Empty)：手动执行更新和发布更新的粒子的服务

  • set_map(nav_msgs/SetMap)：手动设置新地图和姿态的服务

调用的服务：

  • static_map(nav_msgs/GetMap)：调用此服务获取地图数据

参数：

  • ~odom_model_type(string, default:”diff”)：里程计模型选择”diff”,”omni”,”diff-corrected”,”omni-corrected” (diff 差速、omni 全向轮)

  • ~odom_frame_id(string, default:”odom”)：里程计坐标系

  • ~base_frame_id(string, default:”base_link”)：机器人坐标系

  • ~global_frame_id(string, default:”map”)：地图坐标系

里程计本身也是可以协助机器人定位的，不过里程计存在累计误差且一些特殊情况时(车轮打滑)会出现定位错误的情况

amcl 则可以通过估算机器人在地图坐标系下的姿态，再结合里程计提高定位准确度

编写amcl节点相关的launch文件

找到amcl功能包的example

1
2
3
roscd amcl
ls examples/
gedit examples/amcl_diff.launch

修改案例

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
<launch>
    <node pkg="amcl" type="amcl" name="amcl" output="screen">
        <!-- Publish scans from best pose at a max of 10 Hz -->
        <param name="odom_model_type" value="diff"/>
        <param name="odom_alpha5" value="0.1"/>
        <param name="gui_publish_rate" value="10.0"/>
        <param name="laser_max_beams" value="30"/>
        <param name="min_particles" value="500"/>
        <param name="max_particles" value="5000"/>
        <param name="kld_err" value="0.05"/>
        <param name="kld_z" value="0.99"/>
        <param name="odom_alpha1" value="0.2"/>
        <param name="odom_alpha2" value="0.2"/>
        <!-- translation std dev, m -->
        <param name="odom_alpha3" value="0.8"/>
        <param name="odom_alpha4" value="0.2"/>
        <param name="laser_z_hit" value="0.5"/>
        <param name="laser_z_short" value="0.05"/>
        <param name="laser_z_max" value="0.05"/>
        <param name="laser_z_rand" value="0.5"/>
        <param name="laser_sigma_hit" value="0.2"/>
        <param name="laser_lambda_short" value="0.1"/>
        <param name="laser_model_type" value="likelihood_field"/>
        <!-- <param name="laser_model_type" value="beam"/> -->
        <param name="laser_likelihood_max_dist" value="2.0"/>
        <param name="update_min_d" value="0.2"/>
        <param name="update_min_a" value="0.5"/>
        <!-- 里程计坐标系 -->
        <param name="odom_frame_id" value="odom"/>
        <!-- 设置机器人坐标系 -->
        <param name="base_frame_id" value="base_footprint"/>
        <!-- 地图坐标系默认,不写 -->
        <param name="resample_interval" value="1"/>
        <param name="transform_tolerance" value="0.1"/>
        <param name="recovery_alpha_slow" value="0.0"/>
        <param name="recovery_alpha_fast" value="0.0"/>
    </node>
</launch>

编写测试launch文件

amcl节点是不可以单独运行的，运行 amcl 节点之前，需要先加载全局地图，然后启动 rviz 显示定位结果，上述节点可以集成进launch文件

1
2
3
4
5
6
7
8
9
10
<launch>
    <!-- 启动rviz -->
    <node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher" output="screen"/>
    <node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" output="screen"/>
    <node name="rviz" pkg="rviz" type="rviz" />
    <!-- 启动地图服务 -->
    <include file="$(find nav_demo)/launch/nav03_map_server.launch"/>
    <!-- amcl -->
    <include file="$(find nav_demo)/launch/nav04_amcl.launch"/> 
</launch>

执行：

  1. 先启动 Gazebo 仿真环境
  2. 启动键盘控制节点
  3. 启动集成地图服务、amcl 与 rviz 的 launch 文件
  4. 在启动的 rviz 中，添加RobotModel、Map组件，分别显示机器人模型与地图，添加 posearray 插件，设置topic为particlecloud来显示 amcl 预估的当前机器人的位姿，箭头越是密集，说明当前机器人处于此位置的概率越高
  5. 通过键盘控制机器人运动，会发现 posearray 也随之而改变

路径规划

在ROS的导航功能包集navigation中提供了 move_base 功能包，用于实现此功能，包在之前已经安装

move_base 功能包提供了基于动作(action)的路径规划实现，move_base 可以根据给定的目标点，控制机器人底盘运动至目标位置，并且在运动过程中会连续反馈机器人自身的姿态与目标点的状态信息

Actions 是一种客户端/服务器 (Client/Server) 模式的通信方式，专为长时间运行、可抢占的任务而设计，并提供进度反馈和结果

Actions 实际上是建立在 Topics 和 Services (服务) 之上的更高级别的抽象

move_base主要由全局路径规划与本地路径规划组成

move_base功能包中的核心节点是：move_base

动作订阅：

  • move_base/goal(move_base_msgs/MoveBaseActionGoal)：move_base 的运动规划目标

  • move_base/cancel(actionlib_msgs/GoalID)：取消目标

动作发布：

  • move_base/feedback(move_base_msgs/MoveBaseActionFeedback)：连续反馈的信息，包含机器人底盘坐标

  • move_base/status(actionlib_msgs/GoalStatusArray)：发送到move_base的目标状态信息

  • move_base/result(move_base_msgs/MoveBaseActionResult)：操作结果(此处为空)

订阅的Topic：

  • move_base_simple/goal(geometry_msgs/PoseStamped)：运动规划目标(与action相比，没有连续反馈，无法追踪机器人执行状态)

发布的Topic：

  • cmd_vel(geometry_msgs/Twist)：输出到机器人底盘的运动控制消息

服务：

  • ~make_plan(nav_msgs/GetPlan)：请求该服务，可以获取给定目标的规划路径，但是并不执行该路径规划

  • ~clear_unknown_space(std_srvs/Empty)：允许用户直接清除机器人周围的未知空间

  • ~clear_costmaps(std_srvs/Empty)：允许清除代价地图中的障碍物，可能会导致机器人与障碍物碰撞，请慎用

代价地图

ROS中的地图其实就是一张图片，这张图片有宽度、高度、分辨率等元数据，在图片中使用灰度值来表示障碍物存在的概率

不过SLAM构建的地图在导航中是不可以直接使用的，因为：

  1. SLAM构建的地图是静态地图，而导航过程中，障碍物信息是可变的，可能障碍物被移走了，也可能添加了新的障碍物，导航中需要时时的获取障碍物信息；
  2. 在靠近障碍物边缘时，虽然此处是空闲区域，但是机器人在进入该区域后可能由于其他一些因素，比如：惯性、或者不规则形体的机器人转弯时可能会与障碍物产生碰撞，安全起见，最好在地图的障碍物边缘设置警戒区，尽量禁止机器人进入

所以，静态地图无法直接应用于导航，其基础之上需要添加一些辅助信息的地图，比如时时获取的障碍物数据，基于静态地图添加的膨胀区等数据