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
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>
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>
这里我们定义了两个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
- 建立URDF文件
建立URDF文件夹,创建urdf文件
cd fishbot_description && mkdir urdf
touch fishbot_base.urdfCopy to clipboardErrorCopied
完成后src下的目录结构:
├── fishbot_description
│ ├── __init__.py
├── package.xml
├── setup.cfg
├── setup.py
└── urdf
└── fishbot_base.urdfCopy to clipboardErrorCopied
编辑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
- 建立launch文件
在目录src/fishbot_description
下创建launch
文件夹并在其下新建display_rviz2.launch.py
文件。
mkdir launch
touch display_rviz2.launch.pyCopy to clipboardErrorCopied
完成后的目录结构:
├── 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
想要可视化模型需要三个节点参与
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
加入目录安装
('share/ament_index/resource_index/packages',
['resource/' + package_name]),
('share/' + package_name, ['package.xml']),Copy to clipboardErrorCopied
完整
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
- 编译测试
编译
colcon buildCopy to clipboardErrorCopied
运行测试
source install/setup.bash
ros2 launch fishbot_description display_rviz2.launch.pyCopy to clipboardErrorCopied
添加robotmodel模块,分别选择link名称,即可看到机器人的模型显示
二、SLAM建图
通过SLAM就是解决地图和定位问题
机器人通过自身传感器数据处理进行位置估计,同时通过不断移动完成对整个未知环境的地图构建。这就是SLAM解决的问题。
那又是如何解决的呢?SLAM实现的方案很多,但是几个比较关键的技术如下:
- 传感器感知 通过各类传感器实现对环境的感知,比如通过激光雷达获取环境的深度信息。同时可以通过传感器融合来提高位置估计的精度,比如融合轮式里程计、IMU、雷达、深度相机数据等。
- 视觉/激光里程计 基本原理是通过前后数据之间对比得出机器人位置的变化。
- 回环检测 判断机器人是否到达之前到过的位置,可以解决位置估计误差问题,建图时可以纠正地图误差。
经典视觉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
需要注意我们不是直接使用cartographer
,而是通过cartographer-ros
功能包进行相关操作,所以我们还需要安装下cartographer-ros
sudo apt install ros-humble-cartographer-rosCopy to clipboardErrorCopied
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
安装依赖
这里我们使用rosdepc进行依赖的安装,rosdepc指令找不到可以先运行下面的一键安装命令,选择一键配置rosdep即可。
wget http://fishros.com/install -O fishros && . fishrosCopy to clipboardErrorCopied
接着在fishbot_ws下运行下面这个命令进行依赖的安装。
rosdepc 是小鱼制作的国内版rosdep,是一个用于安装依赖的工具。该工具的安装可以采用一键安装进行,选项编号为3。安装完成后运行一次rodepc update即可使用。
rosdepc install -r --from-paths src --ignore-src --rosdistro $ROS_DISTRO -yCopy to clipboardErrorCopied
编译
这里有一个新的命令–packages-up-to,意思是其所有依赖后再编译该包
colcon build --packages-up-to cartographer_rosCopy to clipboardErrorCopied
2.3 测试是否安装成功
如果是源码编译请先source下工作空间后再使用下面指令查看是否安装成功;
ros2 pkg list | grep cartographerCopy to clipboardErrorCopied
能看到下面的结果即可
cartographer_ros
cartographer_ros_msgsCopy to clipboardErrorCopied
可能你会好奇为什么没有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
请你打开这个文件自行浏览,小鱼对其中我们可能会在初次建图配置的参数进行介绍。
-- 是否使用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
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
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.,
}