一、什么是ROS

简单来说在机器人领域中不同的机器人有不同的结构，总体虽然不一样，但是采用“模块化，分布式”的思想就可以将机器人分解成通用模块（雷达、双目、相机……）

所以可以理解为ROS就是机器人界的安卓；

ROS中的各种节点就相当于各种手机的app,任何可以用单片机控制的模块都能用ROS来实现；

二、linux系统的简单操作指令

1、启动终端：

简单的有两种方式，第一种：在左下角的显示应用程序里直接打开终端，第二种：使用快捷键ctrl+alt+t来调出来终端。显示出来像下边图片中的窗口就恭喜你会启动linux系统的终端了！！！

2、对文件进行操作：

查看当前文件夹的内容：在终端中输入ls命令就可以查看当前文件夹的内容了

生成一个文件夹：mkdir(make directory)输入这一命令就可以生成新的目录

进入目录：cd指令，输入cd+空格+目录名称；回到上一级目录的方法就是cd ..

有一个可以快速回到主文件夹中的方法就是cd ~（“~”这一符合在键盘的左上角，esc键下边）

tab键：可以自动补全，值得注意的是在内容唯一的情况下才能更好的其作用，否则需要多输入几个字母；

文版编辑工具：gedit指令可以创建一个文件，并弹出文件书写窗口，就可以写文件了；、

echo：echo 命令是 Linux 中最基本和常用的命令之一，用于在终端中显示文本或变量的值。

语法：echo [选项] [字符串]

• 在终端显示文本信息
• 输出变量的值
• 生成格式化的字符串
• 向文件追加内容

source指令:执行文件中的指令序列，一般用gedit编写一个sh文件，用source去加载运行；

中端程序启动脚本：.bashrc文件，打开终端会用到这个文件，在这个文件中添加一些指令可以在打开终端的时候运行这些指。

sudo指令：ubantu中的权限-一般用户只能操作自己主文件夹下的文件，而我们需要在其他文件下进行操作时需要加一些权限，就需要加一个sudo指令，意味以管理员权限执行本条命令。

更多的指令可以参考菜鸟教程的Linux命令大全。Linux 命令大全 | 菜鸟教程https://www.runoob.com/linux/linux-command-manual.html

三、ros安装

ros的安装可以参考鱼香ros的视频鱼香ros一键安装教程https://www.bilibili.com/video/BV1PT4y1o7JA/?spm_id_from=333.337.search-card.all.click安装好之后可以区检验一下：打开一个终端输入roscore,这个指令是ros的运行核心，打开就可以运行ros

显示这个您就是打开了；

之后我们再打开一个终端输入：rosrun rqt_robot_steering rqt_robot_steering

然后就会弹出来这么一个窗口，这是一个速度和方向的控制器：

然后再打开一个终端输入：rosrun turtlesim turtlesim_node然后我们就可以用这个速度的控制器来控制这个小乌龟！！！

至此恭喜你，你的ros就安装好了就可以进行下一步的学习了！！！

四、下载gazabo仿真，VScode等多个软件同样参考鱼香ros的一键安装即可

如果跟着阿杰学习ros的时候，配置VScode中ROS环境时请注意旧版的ROS已经被弃用了，您可以替换成Robot Developer Extensions for ROS 1效果是一样的。

在打开gazabo仿真系统后，第二次在终端中重新运行的时候会出现这种情况：

这是因为之前的仿真进程没有正常退出导致了端口被占用、资源冲突导致的，我们在终端输入下边的指令再打开就可以

 killall gzserver && killall gzclient

五、ROS基础单元

ros的基础单元是节点（node）节点的功能相对单一，实现很小的一个模块的功能，如雷达、相机、绘图等等，机器人之所以可以实现复杂的功能，就是一堆节点相互影响产生的效果；

简单的理解节点的容器就是包，我们在安装一个“软件”时，尽管只需要其中的一小部分功能，但是需要安装一整个安装包。所以ros的基础单元就是节点和包。

六、第一个节点（超声波节点）

创建一个ros节点的顺序是：

1、使用catkin_create_pkg创建一个软件包：catkin_create_pkg <包名>{依赖项}

2、在软件包的src文件夹下创建一个节点的cpp源码。

3、在节点的源码文件中include包含ROS的头文件<ros/ros.h>。

4、创建一个main函数，并在函数的开头执行ros::init()。（与ros相结合）

5、构建while循环，循环条件为ros::ok()。

6、在CMakeLists.txt中设置节点源码的编译规则

7、编译运行。ctrl+shift+B；

七、ROS的主要通讯方式：话题与消息

机器人的灵活行动需要不同的节点进行通讯，而不同的节点要进行通讯要借助一个群聊，通过群聊进行消息的发送，这个群聊就叫做话题，在话题中发消息的人叫做发布者（Publisher），接收消息的人叫做订阅者（Subscriber），在话题中消息要保持最新且持续就需要一直发送，就像我们平常聊天为防止刷屏就需要不断的发消息：

1. 话题Topic是节点间进行持续通讯的一种形式。
2. 话题通讯的两个节点通过话题的名称建立起话题通讯连接。
3. 话题中通讯数据叫做消息Message。
4. 消息Message通常会按照一定的频率持续不断的发送，以保证消息数据的实时性。
5. 消息的发送方叫做话题的发布者Publisher。
6. 消息的接收方叫做话题的订阅者Subscriber。

扩展：

1. 一个ROS节点网络中可以同时存在多个话题。
2. 一个话题可以有多个发布者，也可以有多个订阅者。
3. 一个节点可以对多个话题进行订阅，也可以发布多个话题。
4. 不同的传感器消息通常会拥有各自独立话题名称，每个话题只能有一个发布者。
5. 机器人速度指令话题通常有多个发布者，但是同一时间只能有一个发言人。

消息是携带信息的载体，所以消息有很多不同的类型，就像刚开始创建一个包的时候就添加几个依赖项，其中的std_msgs就是消息的依赖包。

八、用C++实现publisher

还是首先创建一个包，包内添加一个节点，依然以超声波探测仪为例：

将包的名称命名为ssr_pkg,在包内添加一个文件，文件名称是chao_node.cpp,很显然这是一个C++文件，在包内有必要的头文件，然后设置好之前所写的代码，下边在这个框架下进行添加：

我们要实现消息的发布，首先需要设置一个节点的控制者，叫做NodeHandle,然后设置节点中消息法发布者：Publisher，然后使用advertise函数实现消息的类型的设定与话题名称的设置，该函数的参数有两个，一个是话题的名称另一个是队列的大小，这个大小决定着发布者发布消息缓存的数量，例如：大小设置为10时，当消息发送了11条，那么第一条就会被丢弃；

#include<ros/ros.h>
#include<std_msgs/String.h>
int main(int argc, char *argv[])
{
    ros::init(argc,argv,"chao_node");
    ros::NodeHandle nh;
    ros::Publisher pub=nh.advertise<std_msgs::String>("chaoshengbotaice",10);

    while(ros::ok()){
        std::cout<<"消息已发送"<<std::endl;
        std_msgs::String msg;
        msg.data="前方还有5米";
        pub.publish(msg);

    }
    return 0;
}

消息的类型是字符串，使用date函数设置消息的内容，通过Publisher使用Publish函数将消息发送，注意：话题的名称必须是英文，要不然无法在人ros中接收到；

打开终端运行ros，在另一个中端中用ros跑这个节点的内容，rosrun 包名 节点名称，再打开一个终端，在终端里查看Publisher发送的消息，使用rostopic指令：

1. rostopic list：可以看到几个活跃着的话题名称，如果看到自己写的话题的名称就可以，说明以及运行起来了，如果没有就再重复上边的操作；
2. rostopic echo 话题名称：这个指令可以查看消息的内容，之后看到的是一堆十六进制的消息，然后我们再用echo -e指令去查看他们，就可以看到自己所设置的消息了；
3. rostopic hz 话题名称：这个可以查看Publisher发送消息的频率，每秒发送几条，如果直接查看之后会发现每秒消息发送的非常快，这是我们不需要的，我们可以在代码中手动的调整消息发送的频率，使用ros中所带的rate进行设置，然后再在while循环里用sleep函数进行约束；

   #include<ros/ros.h>
   #include<std_msgs/String.h>
   int main(int argc, char *argv[])
   {
       ros::init(argc,argv,"chao_node");
       ros::NodeHandle nh;
       ros::Publisher pub=nh.advertise<std_msgs::String>("chaoshengbotaice",10);
   
       ros::Rate loop_rate(10);
       while(ros::ok()){
           std::cout<<"消息已发送"<<std::endl;
           std_msgs::String msg;
           msg.data="前方还有5米";
           pub.publish(msg);
           loop_rate.sleep();
       }
       return 0;
   }
   

   然后编译，在终端里再次进行频率查看，就会发现频率固定到了设定的频率；

总结一下创建Publisher的过程：

1. 确定话题名称和消息类型。
2. 在代码文件中包含消息类型相对应的头文件。
3. 在main函数中通过NodeHandler大管家发布一个话题并得到消息发送对象。
4. 生成要发送的消息包并进行发送数据的赋值。
5. 调用消息发送对象的publish（）函数将消息包发送到话题里。

九、C++实现订阅者Subscriber

1. 确定话题名称和消息类型。
2. 在代码文件中include<ros.h>和消息类型对应的头文件。
3. 在main函数中通过NodeHandler大管家订阅一个话题并设置消息接受回调函数。
4. 定义一个回调函数，对接受到的消息包进行处理。
5. main函数中需要执行ros::spinOnce(),让回调函数能够响应接收到的消息包。

#include<ros/ros.h>
#include<std_msgs/String.h>

void chao_callback(std_msgs::String msg){
    ROS_INFO(msg.data.c_str());
}
void vision_callback(std_msgs::String msg){

    ROS_WARN(msg.data.c_str());
}
int main(int argc, char *argv[])
{
    setlocale(LC_ALL,"zh_CN.UTF-8");
    ros::init(argc,argv,"duoji_node");
    ros::NodeHandle nh;
    ros::Subscriber sub =nh.subscribe("chaoshengbotance",10,chao_callback);
    ros::Subscriber sub2=nh.subscribe("shijueganzhi",10,vision_callback);
    while (ros::ok())
    {
            ros::spinOnce();
    }


    return 0;
}

在运行节点的时候利用rqt_graph指令可以图形化显示当前系统活跃的节点以及节点间的话题通讯关系。

十、用launch启动多个节点

launch应用的是XML语法，用一个大的launch标签包含很多小的标签（节点）做到同时启动多个节点

<launch>
    <node pkg="ssr_pkg" type="chao_node" name="chao_node"/>
    <node pkg="ssr_pkg" type="vision_node" name="vision_node"/>
    <node pkg="atr_pkg" type="duoji_node" name="duoji_node" output="screen"/>
</launch>

再在终端里输入roslaunch 包名 launch文件名，就可以实现一个launch语句打开多个节点了；

在launch文件中，为节点添加output=“screen”属性，可以让节点信息输出在终端中。（ROS_WARN不受该属性控制）；

在launch文件中，为节点添加launch-prefix="gnome-terminal -e"属性，可以让节点单独运行在一个独立终端中。gnome-terminal是ubuntu系统自带的终端；

十一、消息包

ROS消息包包含了标准消息包和常规消息类型，标准消息类型中有基础类型，数组类型和结构体类型

而大部分的消息都用常规消息类型，因为里边的消息类型有更好的定义，比如之前接触过的几何消息包与传感器消息包，常用的消息类型由各种消息包所组成，这就根std_msgs不太一样了

• actionlib_msgs：定义 ROS 行为（action）通信的标准消息结构，是 actionlib 机制的基础。用于带反馈、可中断的长时间任务（比如机械臂运动、导航），规范了任务目标、执行结果、实时反馈的消息格式。
• diagnostic_msgs：系统诊断专用消息包，用于发布硬件、节点的运行状态、报错信息、自检数据，方便监控机器人系统状态、排查软硬件故障。
• geometry_msgs：ROS 最核心的几何基础消息包，定义了所有机器人常用的几何数据结构，比如点、位姿、速度、坐标变换、加速度等，机器人定位、底盘运动控制、坐标转换都依赖它。
• nav_msgs：机器人导航专用消息包，定义了导航相关的核心数据，比如里程计、栅格地图、规划路径、导航目标等，是 SLAM 建图、自主导航功能的核心消息载体。
• sensor_msgs：通用传感器消息包，定义了绝大多数常见传感器的标准数据格式，比如激光雷达、相机图像、IMU、点云、GPS、关节编码器等，统一传感器数据的发布格式，方便不同功能包兼容。
• shape_msgs：几何形状描述消息包，定义了立方体、球体、圆柱体、网格等基础形状的结构，用于碰撞检测、物体识别、机械臂抓取等场景的物体模型描述。
• stereo_msgs：双目相机专用消息包，专门定义双目立体视觉的相关数据，最常用的是视差图消息，用于双目测距、三维重建等双目视觉场景。
• trajectory_msgs：轨迹控制消息包，定义了机器人关节轨迹的标准格式，比如多关节的轨迹点序列，包含每个关节的位置、速度、加速度和时间戳，用于机械臂、移动机器人的平滑轨迹运动控制。
• visualization_msgs：可视化专用消息包，用于在 RViz 等可视化工具中绘制标记图形，比如箭头、文字、线条、障碍物标记、路径高亮等，方便调试时直观展示机器人的规划结果、环境感知信息。

比较重要的两个消息包：
geometry_msgs(几何消息包）
里边参数有Stamped的参数就是多了Header消息头的消息类型，就是有时间戳和坐标ID的参数；

sensor_msgs(传感器消息包）

自定义消息包

我们创建一个自定义的消息包，同样需要在终端中创建:
catkin_create_pkg是创建消息包指令，qq_msgs是消息包的名称，roscpp、rospy和std_msgs是平常我们创建一个节点包时经常会添加的依赖项，而后边的message_generation和message_runtime是消息包生成和运行是所需要的依赖项；

创建好包之后打开VScode，在这个消息包下新建一个msg文件夹，用来放我们自定义的消息，在msg中创建一个.msg文件就可以创建一个自定义的类型了（相当于给基础类型弄一个别名），在CMakeLists文件中增添编译规则，首先确保有一下依赖包：

find_package(catkin REQUIRED COMPONENTS
  message_generation
  message_runtime
  roscpp
  rospy
  std_msgs
)

有了之后找到
将这个取消注释，删去其中的Message1.msg和Message2.msg将其替换成你所编写的消息文件的名称（前边创建的.msg文件）
之后再找到：
将这个也取消注释，这个是这个消息包所依赖的其他消息包，注意我的自定义消息中只用到了std_msg里的基础消息类型，如果你的自定义消息包用到了其他消息包类型里的消息格式，也需要添加；
之后找到：
将第三行的这个代码取消注释，确保message_runtime在其中，这个是确保其他软件包可以使用这个自定义的消息包；至此在CMakeList中的修改就可以了；

打开下边的package.xml，找到其中的依赖项列表：
确保build_depend与exec_depend中都包含message_generation和message_runtime这两个依赖包
写完之后还需要手动进行编译，在终端中进入工作空间catkin_make编译就可以了!!!
如果不知道自己消息包是否设置好了，就在终端中使用 rosmsg show 消息包名/消息文件名 就可以显示自定义的消息包了；

使用自定义消息包

使用自定义消息包类型和用已有的消息包类型在cpp文件中的使用方法是一样的：
发布者：首先包含头文件，发布包含该消息类型的话题，创建一个消息包然后设置消息并发布就可以了；订阅者：同样包含头文件，回调函数接收到的消息要是新定义的消息的类型；

不同的是需要在CMakeList和package.xml文件中进行修改：
CMakelist文件中：找到
在其中包含自己写的消息包名称；
在末尾节点编译规则里添加一个add_dependencies(该节点名，消息包名_generate_messages_cpp):这是为节点添加一个依赖项；
在package.xml文件中的依赖项列表里：
将你创建的消息包加入到build_depend和exec_depend中去；这样就完成了自定义的消息类型的使用！！！

机器人运动控制（速度控制）

机器人的运动可以分为矢量运动和旋转运动，一般以机器人底座中心为坐标原点，以机器人前进的方向为X轴正方向建立右手坐标系，矢量速度都可以分解为朝向三轴的速度，速度单位为米每秒，旋转运动分为沿X，Y，Z轴进行旋转的运动，沿X轴为滚转运动（row），沿Y轴为俯仰运动（pitch),沿Z轴为旋转/偏航运动（yaw），任何的旋转运动都可以分解为这三个运动，旋转的单位统一为弧度每秒；

机器人运动控制需要速度信息那么它的消息包类型就得是速度类型：

消息包为geometry_msgs(几何类型消息包），里边包含Twist消息类型这个消息包包含两个消息类型一个是linear表示矢量运动的速度，一个是angular表示的是旋转运动的速度,linear和angular中都包含三个参数x，y，z都是64位浮点数，表示沿轴运动的速度（旋转速度）；

速度的话题名称："cmd/vel" 是command_velocity的缩写；

实现运动控制的思路：

1. 构建一个新的软件包，软件包名为"vel_pkg"；
2. 在软件包中创建一个节点，节点的名称叫做vel_node；
3. 在节点中，向ROS大管家NodeHandle申请发布话题/cmd_vel,并拿到发布对象vel_pub；
4. 构建相应消息包，承载发送的数据值；
5. 用vel_pub在while循环里发布消息包

#include<ros/ros.h>
#include<geometry_msgs/Twist.h>

int main(int argc, char *argv[])
{
    ros::init(argc,argv,"vel_node");
    ros::NodeHandle nh;
    ros::Publisher  vel_pub=nh.advertise<geometry_msgs::Twist>("/cmd_vel",10);
    geometry_msgs::Twist vel_msg;
    vel_msg.linear.x=0;
    vel_msg.linear.y=0;
    vel_msg.linear.z=0;
    vel_msg.angular.x=0;
    vel_msg.angular.y=0;
    vel_msg.angular.z=0.5;

    ros::Rate r(30);
    while (ros::ok())
    {
        vel_pub.publish(vel_msg);
        r.sleep();
    }
    

    return 0;
}

这样就创建好了速度控制的软件包消息发布,运用开源项目就可以实现速度的控制了！

激光雷达

激光雷达的种类繁多，但是其在ROS中数据的表现形式是单一的，激光雷达一般分为两部分一部分是固定底座，另一部分是旋转头部，旋转头部会射出一个机关由另一个接收激光的部分接收，而后计算出激光飞行的距离处以二就是测量的距离值，在ROS中激光雷达的表示是一个个点所组成的，下面我们看看在Rviz观测到的激光雷达数据：

打开中端输入roslaunch wpr_simulation wpb_simple.launch启动开源的项目
之后另起一个终端，在终端中输入rviz就启动了这个仿真环境；这里就有人问了之前用的gazebo是仿真环境这个rviz也是仿真环境，这两个有啥区别呢：
简单来书一个是输入一个是输出，gazebo相当于一个外界环境，一般真正作项目时这一部分会被真实环境所代替，但是rviz相当于机器人视角下的世界，你可以在这个环境中观察到机器人的一些感知，获得的一些数据等等，所以rviz的环境是非常重要的。

rviz的基本介绍：

左侧是项目列表，带三角形的就是我们打开的项目，点开三角形下边罗列的就是参数列表表；

中间这个栅格地图就是rviz的核心区域，用来显示机器人的视角世界；
下边是ROS的一些基本信息，比如运行时间等等；

下边我们来看rviz是怎么显示信息的，首先将Fixed Frame改为base_footprint，之后打开add里边显示的是rviz所能显示的数据类型的列表，找到机器人模型（RobotModel）图形中间就显示了上图中的机器人；再添加Laserscan将话题名称换为/scan，就可以发现机器人前方多了一些小点，点有点小，我们将Laserscan的size改为0.03就可以看到点列了

我们在gazebo里添加一些其他的障碍物也能够在rviz中进行显示

那我们每次打开rviz都需要配置以下显得非常麻烦所以我们只需要保存一下这个文件就可以了：
打开文件，点击save config as将文件保存为.rviz文件就可以了；
退出rviz，再次打开，在文件里点击open config，打开之前保存的文件就可以显示了；

激光雷达的消息包格式

我们会看到在传感器消息包，激光雷达消息类型中具有上图中的参数
我们来看一下雷达话题中消息包的具体数据来对比一下：

上边是时间戳和坐标系ID，显示信息采集完成的时间，angle_min是激光扫描的起始角度，对应angle_max就是激光扫描结束的角度，再看angle_increment和time_increment，这两个参数是相邻两次测距的时间差和角度差，决定了旋转一周会测定多少个测定点，range_min是雷达的最小测距量程，而对应的range_max则对应雷达的最大测距，超出最大和最小测距是无效的，ranges是一个浮点数数组用来存放在一圈测量中每一个测试点位到前方障碍物的距离；

ranges数组内有360个数据对应一圈中测量的360个点位距离，前边的测试开始角度是180度在机器人的正后方，那么第180个数据就是机器人正前方的障碍物距离了；对于基础的参数设定只需要知道前面这几个就可以了

机关雷达消息获取节点

我们来实现一下对激光雷达探测数据的获取和控制，激光类达一般会有一个对应的节点，这个节点一般有厂商来决定，这个节点会发布一个话题将消息发送，我们只需要订阅这个话题获取其中的信息就可以了，相同的我们创建一个包，创建一个订阅者去订阅话题，同时实现速度的控制；

#include<ros/ros.h>
#include<sensor_msgs/LaserScan.h>
#include<geometry_msgs/Twist.h>

ros::Publisher vel_pub;

int nCount=0;
void LidarCallback(const sensor_msgs::LaserScan msg){
    float fMidDist =msg.ranges[180];
    ROS_INFO("前方测距 ranges[180]= %f 米",fMidDist);

    if(nCount>0){ 
        nCount--;
        return ;
    }

    geometry_msgs::Twist vel_msg;
    if(fMidDist<1.5){
        vel_msg.angular.z=0.3;
        nCount=50;
    }else{
        vel_msg.linear.x=0.1;
    }
    vel_pub.publish(vel_msg);
}
int main(int argc, char *argv[])
{
    setlocale(LC_ALL,"");
    ros::init(argc,argv,"lidar_node");
    ros::NodeHandle nh;
    ros::Subscriber lidar_sub=nh.subscribe("/scan",10,&LidarCallback);
    vel_pub=nh.advertise<geometry_msgs::Twist>("/cmd_vel",10);
    ros::spin();
    return 0;
}

打开终端运行项目和这个节点就可以简单的实现机器人的避障了！！！

补充消息包类型：

一、消息头 std_msgs/Header header

所有带时间、坐标系信息的 ROS 标准消息，都会带这个消息头，是数据同步、坐标变换的核心。

字段	类型	核心作用	常规设置方法
seq	uint32	消息序列号，每发布 1 条消息自动 + 1，用来判断消息是否丢包、顺序是否错乱	无需手动设置，ROS 的发布器publisher会自动填充
stamp	time	这一帧激光数据采集完成的时间戳，是整个消息最关键的字段之一，用来做时间同步、TF 坐标变换，时间戳错误会直接导致定位漂移、导航报错	雷达驱动会自动用数据上传的时间填充；自己开发的话用ros::Time::now()填充，必须和雷达实际采集时间尽量一致
frame_id	string	激光数据所在的坐标系 ID，告诉 ROS 这组数据是在哪个坐标系下采集的，TF 会根据这个 ID 把激光数据转到机器人基坐标系、地图坐标系	必须和 URDF 模型里雷达的坐标系名称一致，通用默认值是laser_link/base_scan，不能为空、不能写错，否则 RViz 看不到激光、导航无法使用

---

二、激光扫描核心配置参数（均为float32单精度浮点数）

字段	单位	核心作用	常规设置方法
angle_min	弧度 rad	激光扫描的起始角度，对应雷达视野的最小角度（ROS 标准：机器人正前方为 0 度，逆时针为正，顺时针为负）	完全由雷达硬件参数决定，360° 激光雷达（如 RPLIDAR）填 -3.1415926（即 -π，对应 - 180°）；180° 雷达填 -1.5708（-90°），驱动会自动读取雷达参数填充
angle_max	弧度 rad	激光扫描的结束角度，对应雷达视野的最大角度	和angle_min对应，360° 雷达填 3.1415926（π，对应 180°）；180° 雷达填 1.5708（90°），必须满足angle_max > angle_min
angle_increment	弧度 / 每个采样点	相邻两个激光点的角度增量，也就是雷达的角分辨率，决定了一帧扫描的总点数	公式：angle_increment = (angle_max - angle_min) / (每帧总点数 - 1)，比如 360° 雷达一帧 360 个点，值约为 0.01745rad（1°/ 点），由雷达硬件决定，驱动自动填充，修改会导致点云错位、精度丢失
time_increment	秒 / 每个采样点	雷达采集相邻两个激光点的时间间隔，用于机器人运动时的点云运动补偿	公式：time_increment = 1 / (雷达帧率 × 每帧总点数)，比如 10Hz 帧率、360 点 / 帧的雷达，值约为 0.000278s，静止机器人影响不大，高速移动的机器人必须准确设置，否则会导致点云畸变、定位不准
scan_time	秒 / 每帧	雷达扫完一整帧数据的总时间，也就是一帧的扫描周期，和雷达转速直接相关	公式：scan_time = 1 / 雷达帧率，比如 10Hz 的雷达值为 0.1s，也等于time_increment × 每帧总点数，驱动自动填充，用于时间同步和运动补偿
range_min	米 m	雷达的最小测距量程，小于这个值的测距数据是无效、不可信的	参考雷达手册设置，比如 RPLIDAR A1 最小量程 0.15m，就填 0.15；小于这个值的点，导航 / SLAM 算法会自动忽略，避免把雷达自身结构、近距离干扰当成障碍物
range_max	米 m	雷达的最大测距量程，大于这个值的测距数据是无效的	参考雷达手册设置，比如 RPLIDAR A1 最大量程 12m，就填 12；不要盲目填过大的值，室内场景可适当缩小（如 8m），减少远处噪声的影响

---

三、激光核心数据数组

字段	类型	核心作用	常规设置方法
ranges	float32[] 浮点数数组，单位米	一帧扫描所有激光点的测距数据，数组长度 = 每帧采样点数，和angle_increment一一对应	数组第 i 个元素，对应角度为angle_min + i × angle_increment，值为该角度的障碍物距离；无效数据（小于range_min/ 大于range_max）必须填inf（无穷大）或NaN，不能填 0，否则会被算法当成 0 米处有障碍物
intensities	float32[] 浮点数数组	每个激光点的回波强度，和障碍物的材质、颜色、距离相关（白色反光强、黑色反光弱）	只有支持强度输出的雷达才会有有效数据，不支持的雷达数组为空 / 全 0；数组长度必须和ranges完全一致，常规导航、SLAM 用不到这个字段，仅特殊算法（反光板识别、材质识别）会用到

---

四、常见避坑说明 & 典型示例

1. 单位必看：所有角度单位都是弧度，不是角度！新手常犯的错误是直接填 - 180，必须用弧度值（比如deg2rad(-180)）。
2. 坐标系规则：ROS 标准是 x 轴向前（机器人正前方）、y 轴向左、z 轴向上，角度逆时针为正，0 度为正前方。
3. 典型配置（RPLIDAR A1 360° 激光雷达）

   plaintext

   angle_min: -3.1415926
   angle_max: 3.1415926
   angle_increment: 0.015708 (约0.9°/点)
   time_increment: 2.777e-4 (10Hz帧率)
   scan_time: 0.1
   range_min: 0.15
   range_max: 12.0

IMU（Inertial Measurement Unit)

IMU中文名称是惯性测量单元，IMU是安装在机器人内部的一个传感器模块，可以用来测量机器人的空间姿态，空间姿态是用来描述机器人矢量旋转的:

首先是一个文件头依旧记录了时间戳，坐标ID，第一个消息orientation是记录机器人空间姿态的，他的参数有四个，分别对应的四元数，解决了奇异性自锁的问题，第二个消息类型是角速度，三个参数分别对应绕X、Y、Z轴旋转的速度，第三个消息是记录机器人的矢量加速度的，同样包含了三个参数，对应沿X、Y、Z轴的加速度。

算上角速度和矢量加速度总共有六个数据，这就是一个6轴IMU，而有些IMU可以提供XYZ三个方向的磁强计输出，这就是九轴IMU，磁强计输出的数据有专门的消息包类型，并不在这个消息包内；

字段逐段深度解析

1. std_msgs/Header header

ROS 标准消息头，是整个消息的时间与空间锚点，直接决定数据的可用性。

子字段	深度含义与工程要求
uint32 seq	消息序列号，ROS1 中发布时自动递增，核心作用是丢包 / 乱序检测。IMU 通常是 100~1000Hz 高频输出，seq 跳变超过 1 即可判断丢包，滤波算法可据此做补偿；ROS2 已移除该字段，功能被 DDS 底层的 QoS 可靠性机制接管。
time stamp	时间戳（秒 + 纳秒），是整个消息最关键的字段之一。✅ 正确用法：必须填写IMU 硬件采集数据的真实时刻，最好通过 PPS 秒脉冲、硬件触发同步，保证多传感器（激光 / 相机 / IMU）的时间对齐误差在微秒级；❌ 错误用法：用 ROS 节点发布时刻ros::Time::now()填充，会引入系统调度延迟，直接导致多传感器融合失效、定位漂移。强制要求：时间戳必须单调递增，不能回退，否则滤波算法会直接崩溃。
string frame_id	数据对应的坐标系 ID，是 ROS TF 坐标变换体系的核心绑定点。通用命名为imu_link，必须和机器人 URDF 模型中 IMU 的坐标系名称完全一致，且在 TF 树中定义imu_link到机器人基坐标系base_link的静态变换。作用：告诉 ROS 系统，这个 IMU 数据是在哪个本体坐标系下测量的，让算法可以把 IMU 的本体运动转换到世界坐标系下计算。

---

2. 姿态相关字段

geometry_msgs/Quaternion orientation

• 物理意义：IMU 本体坐标系相对于 ENU 东北天世界坐标系的 3D 旋转，也就是 IMU 的绝对姿态，用四元数格式存储，避免欧拉角的万向锁问题。
• 核心细节：
  1. ROS 强制规定四元数顺序为 (x,y,z,w)，w是实部，x/y/z是虚部。这是高频踩坑点：Eigen 等数学库、很多 IMU 原厂 SDK 的默认顺序是(w,x,y,z)，赋值顺序错误会直接导致姿态完全异常。
  2. 数据来源：不是 IMU 原始输出，是内部 AHRS 系统通过加速度计 + 陀螺仪 + 磁力计融合解算的结果。6 轴 IMU（无磁力计）无法输出稳定的绝对偏航角（yaw），yaw 会随时间持续漂移。
  3. 强制约束：必须是单位四元数，满足 x²+y²+z²+w²=1，否则 ROS 算法会直接报错或计算错误。

float64[9] orientation_covariance

• 本质：3×3 的姿态协方差矩阵，按行优先（Row-major） 平铺为 9 元素数组，单位为rad²，描述绕 x (横滚 roll)、y (俯仰 pitch)、z (偏航 yaw) 三个轴的姿态测量不确定度。
• 行优先对应关系：

  plaintext

  数组索引：[0,1,2, 3,4,5, 6,7,8]
  对应矩阵：
  [cov_xx, cov_xy, cov_xz,
   cov_yx, cov_yy, cov_yz,
   cov_zx, cov_zy, cov_zz]
  

  其中对角元素[0][4][8]分别是 roll、pitch、yaw 三个轴的方差，非对角元素是轴间噪声的相关性。
• 工程核心规则：
  1. 99% 的场景下，非对角元素全部设为 0（对角矩阵），仅在完成高精度 IMU 标定、明确轴间噪声相关性后才填写非对角值。
  2. 赋值核心逻辑：数据越不可靠，协方差值越大，算法给的权重越低。
     • 9 轴 IMU（带磁力计）：roll/pitch 精度高，协方差设1e-6 rad²；yaw 精度稍低，设1e-4 rad²。
     • 6 轴 IMU（无磁力计）：yaw 不可观，将 yaw 的协方差设为1e6 rad²，告诉算法完全忽略该轴数据。
     • 无姿态输出：将数组第 0 位设为-1，所有标准算法会自动忽略整个姿态字段。
  3. 绝对禁止将数组全设为 0：协方差为 0 代表 “测量值绝对无误差”，会让算法完全信任该数据，一旦有噪声直接导致滤波发散。

---

3. 角速度相关字段

geometry_msgs/Vector3 angular_velocity

• 物理意义：陀螺仪的核心原始输出，IMU 绕本体 x/y/z 轴的转动角速度，单位rad/s，对应 roll/pitch/yaw 的转动速度。
• 核心细节：
  1. 这是 IMU 最核心、最可靠的原生数据，无额外解算，是所有姿态解算、里程计积分、运动控制的核心输入。
  2. 高频踩坑：很多 IMU 原厂 SDK 输出单位是°/s（度每秒），必须转换为rad/s（1°=π/180 rad），否则数据会偏差 57.3 倍，直接导致算法失效。
  3. 工程要求：高精度应用必须做 IMU 标定，补偿陀螺仪的零偏（静止时输出非 0 的固定偏移），否则积分出的角度会快速漂移。

float64[9] angular_velocity_covariance

• 本质：3×3 的角速度协方差矩阵，行优先平铺，单位(rad/s)²，描述陀螺仪测量的不确定度。
• 工程设置规则：
  1. 优先参考 IMU datasheet 中的角度随机游走（ARW） 参数计算，协方差 = (ARW)² / 采样时间间隔 dt。
  2. 通用默认值：消费级 IMU（MPU6050/ICM20602）设1e-4 ~ 1e-5 (rad/s)²，工业级 IMU 可设1e-6及以下。
  3. 无有效数据时，将数组第 0 位设为-1，标记无效。

---

4. 线加速度相关字段

geometry_msgs/Vector3 linear_acceleration

• 物理意义：加速度计的核心原始输出，IMU 在本体 x/y/z 轴感受到的线加速度，单位m/s²，默认包含重力加速度。
• 核心细节：
  1. 单位转换，很多 IMU 输出单位是g（重力加速度），必须转换为m/s²（1g=9.81m/s²）。
  2. 不要在驱动中提前去除重力。ROS 标准算法需要包含重力的原始加速度，来修正陀螺仪的漂移（重力是已知的固定参考，可解算 roll/pitch 角），重力补偿由上层滤波算法完成。
  3. 核心用途：一是静止时修正陀螺漂移，二是短时间内做里程计积分（加速度→速度→位移），弥补激光 / 相机的丢帧。

float64[9] linear_acceleration_covariance

• 本质：3×3 的线加速度协方差矩阵，行优先平铺，单位(m/s²)²，描述加速度计测量的不确定度。
• 工程设置规则：
  1. 优先参考 IMU datasheet 中的速度随机游走（VRW） 参数计算，协方差 = (VRW)² / 采样时间间隔 dt。
  2. 通用默认值：消费级 IMU 设1e-3 ~ 1e-4 (m/s²)²，工业级 IMU 可设1e-5及以下。
  3. 无有效数据时，将数组第 0 位设为-1，标记无效。

Imu消息包内的协方差矩阵

每一个消息后边跟了一个协方差矩阵，这个矩阵是记录变量的可信度和变量之间的相关性的，对角线上的数值是每个变量的方差，核心作用是告诉算法这个测量值的可信程度，这个值越大越不可信，非对角线上的元素是两个变量之间的协方差，告诉算法两个变量是否有关联，协方差为正则正相关，为负则负相关，为零则没有关系；
如果这个协方差矩阵对应的数值不存在那么这个矩阵的第一个数据会被设置为-1，如果要使用上边消息包中的数据时，我们可以先检验一下协方差矩阵第一个数据是否为-1，如果是的话相当于数据并不存在；
协方差在ROS中的作用一般为以下几点：

1.  给算法分配权重，决定「信传感器还是信预测模型」
2. 量化误差传播，实时告诉你「当前估计的可信度」
3. 多传感器融合，解决不同传感器的数据冲突
4. 标准化标记无效数据，兼容 ROS 全生态

IMU的话题与消息包类型

我们已经大致了解了IMU消息包内的数据了，下面就可以设置节点去获取IMU所给的数据了，这个话题总共有三个，第一个是"imu/data_raw"用到的消息包是sensor_msgs/Imu，这个是加速度计输出的矢量加速度和陀螺仪输出的旋转角速度，第二个是"imu/data"用到的消息包是sensor_msgs/Imu，这个话题在上一个的基础上融合了四元数姿态描述，第三个是"imu/mag"需要的消息包是sensor_msgs/MagneticField，这个话题里有磁强计输出的磁强数据；所以一般来说我们只需要第二个话题就可以了

C++实现IMU数据获取与定向巡航

#include<ros/ros.h>
#include<sensor_msgs/Imu.h>
#include<tf/tf.h>
#include<geometry_msgs/Twist.h>

ros::Publisher vel_pub;

void Imucallback(sensor_msgs::Imu msg){
    if(msg.orientation_covariance[0]<0){
        return ;
    }
        
    tf::Quaternion quaternion(
            msg.orientation.x,
            msg.orientation.y,
            msg.orientation.z,
            msg.orientation.w
        );

    double roll,pitch,yaw;
    tf::Matrix3x3(quaternion).getRPY(roll,pitch,yaw);
    roll=roll*180/M_PI;
    pitch=pitch*180/M_PI;
    yaw=yaw*180/M_PI;

    ROS_INFO("滚转= %f  俯仰= %f  朝向= %f",roll,pitch,yaw);

    double flag=90;
    double diff_yaw=flag-yaw;
    geometry_msgs::Twist vel_msg;
    vel_msg.angular.z=diff_yaw*0.01;
    vel_msg.linear.x=0.1;
    vel_pub.publish(vel_msg);
    

}

int main(int argc, char *argv[])
{   setlocale(LC_ALL,"");
    ros::init(argc,argv,"imu_node");
    ros::NodeHandle nh;
    ros::Subscriber imu_sub=nh.subscribe("/imu/data",10,Imucallback);
    vel_pub=nh.advertise<geometry_msgs::Twist>("/cmd_vel",10);
    ros::spin();
    return 0;
}

消息获取：创建一个imu的包后，创建一个节点，用这个节点去获取IMU传来的四元书，用tf工具将这个四元数转化为RPY角，最后输出打印；
定向巡航：设定一个目标角度，求出目标角度与现实角度间的差值，最后设定旋转速度，用一个发布者去发送；
最后别忘了在CMakelist里添加编译规则！！！

高频避坑指南

1. 单位错误：角速度用 °/s、加速度用 g、角度用欧拉角，是最常见的致命错误，直接导致算法完全失效。
2. 四元数顺序错误：混淆x,y,z,w与w,x,y,z的顺序，导致姿态完全异常。
3. 协方差乱设：全设为 0、全设为 1，或不可靠的轴设置过小的协方差，导致滤波发散、定位漂移。
4. 时间戳错误：用发布时间代替硬件采集时间，或时间戳不单调，导致多传感器时间对齐失败。
5. 坐标系错误：frame_id与 TF 树不匹配，或 IMU 坐标系不符合 REP 105 右手系规范（x 前、y 左、z 上），导致坐标转换错误。
6. 提前重力补偿：在驱动中去除重力，导致算法无法用重力参考修正姿态，解算结果完全错误。

栅格地图

栅格地图（占据栅格地图）是 ROS/ROS2 生态中移动机器人 2D 导航、SLAM、路径规划的标准环境表示格式，它将连续的二维物理环境离散化为固定分辨率的网格单元，每个单元用数值表示该位置被障碍物占据的概率，具备结构简单、计算高效、适配多传感器的核心优势，是 ROS 导航栈的核心基础。

一、核心标准消息类型

ROS 中栅格地图的标准消息类型为 nav_msgs/OccupancyGrid，该格式由 ROS REP 规范定义，ROS1 与 ROS2 通用（仅细节略有差异），完整消息结构如下：

# 二维栅格地图，每个单元代表障碍物占据概率
std_msgs/Header header              # 消息头
nav_msgs/MapMetaData info           # 地图元数据
int8[] data                         # 栅格核心数据

1. 消息头 std_msgs/Header

用于标记地图的时间与空间上下文，核心字段：

字段	说明	通用设定
stamp	时间戳	填写地图生成 / 发布的时间，用于节点间时间同步
frame_id	坐标系 ID	固定为map，符合 ROS REP 105 坐标系规范，代表全局固定的世界坐标系，是导航 TF 树的根节点之一
seq	消息序列号	ROS1 自动生成，ROS2 已移除该字段

2. 地图元数据 nav_msgs/MapMetaData

存储栅格地图的核心配置参数，决定了地图的物理尺寸、精度与坐标映射关系：

字段	类型	说明	通用设定
map_load_time	time	地图加载的时间戳	一般为地图发布时的时间
resolution	float32	地图分辨率，单位米 / 栅格 (m/cell)	室内常规场景默认 0.05m（5cm）；高精度场景用 0.025m；大场景室外用 0.1~0.2m，平衡精度与计算量
width	uint32	地图宽度，x 方向的栅格数量（列数）	由建图环境尺寸与分辨率共同决定
height	uint32	地图高度，y 方向的栅格数量（行数）	由建图环境尺寸与分辨率共同决定
origin	geometry_msgs/Pose	地图原点，即栅格矩阵中 **(0,0) 号栅格（左下角）** 对应的世界坐标系位姿	包含position(x,y,z，z 通常为 0，2D 地图) 与orientation(四元数，通常为单位四元数，无旋转)

3. 核心栅格数据 int8[] data

一维数组存储整个地图的栅格状态，数组长度为width * height，是栅格地图的核心：

1. 存储顺序：严格遵循行优先（row-major） 规则，从地图左下角 (0,0) 开始，先沿 x 轴从左到右填充一行，再向上递增 y 轴填充下一行。栅格 (x,y) 对应的数组索引公式：index = y * width + x
2. 数值语义（ROS 标准规范）：

   数值	语义	说明
   0	完全空闲（Free）	确定无障碍物，机器人可正常通行
   100	完全占据（Occupied）	确定为障碍物，机器人不可通行
   -1	未知区域（Unknown）	未被传感器探测到的区域，无环境信息
   1~99	占据概率	数值越高，该位置被障碍物占据的可能性越大，导航中通常设置阈值（如 > 65 视为障碍物）

二、栅格地图通用设定规范

1. 坐标转换规则（栅格索引 ↔ 世界坐标）

这是实际开发中最核心的映射关系，用于机器人定位、路径规划的坐标换算：

• 栅格索引 (x,y) → 世界坐标（map 坐标系下，栅格中心坐标）：

  world_x = origin.position.x + (x + 0.5) * resolution
  world_y = origin.position.y + (y + 0.5) * resolution
  

  其中0.5用于取栅格中心，避免边界计算误差。
• 世界坐标 → 栅格索引 (x,y)：

  x = floor((world_x - origin.position.x) / resolution)
  y = floor((world_y - origin.position.y) / resolution)
  

  其中floor为向下取整，确保索引落在正确的栅格内。

2. 坐标系规范

严格遵循 ROS REP 105 坐标系规范：

• 栅格地图的frame_id必须为map，属于全局固定坐标系，无累计漂移，用于全局定位与路径规划。
• 与odom（里程计坐标系）、base_link（机器人本体坐标系）构成完整的导航 TF 树，三者的坐标变换是机器人导航的基础。

3. 数值与格式规范

• 栅格数值严格限制在[-1, 100]范围内，超出范围的数值会导致 AMCL、move_base 等导航包解析异常。
• 数据类型必须为int8，禁止使用uint8，否则-1会被解析为 255，造成地图完全错误。
• 建图、定位、导航全流程的分辨率必须保持一致，否则会出现障碍物匹配错误、路径规划偏移等问题。

三、栅格地图标准存储格式

ROS 中静态栅格地图采用 yaml元数据文件 + pgm/png灰度图 的双文件格式存储，是 SLAM 建图后保存、map_server 加载发布的标准格式。

1. yaml 元数据文件

用于描述地图的核心配置，典型示例如下：

image: my_map.pgm          # 地图灰度图文件路径，支持相对/绝对路径
resolution: 0.05           # 地图分辨率，与消息中的resolution完全一致
origin: [-5.0, -5.0, 0.0]  # 地图左下角对应的世界坐标[x, y, yaw]，yaw为逆时针旋转角度，通常为0
occupied_thresh: 0.65       # 占据阈值，像素转换后的概率大于此值，视为完全占据（对应消息中的100）
free_thresh: 0.196          # 空闲阈值，像素转换后的概率小于此值，视为完全空闲（对应消息中的0）
negate: 0                    # 像素语义反转开关，0为默认（黑色=障碍物，白色=空闲），1为反转
mode: trinary                # 解析模式，默认trinary（三值：0、100、-1），可选scale/raw

2. pgm/png 灰度图文件

单通道灰度图，每个像素对应一个栅格，与 yaml 文件配合解析为栅格数据：

• 白色（255）：对应空闲区域，解析为消息中的 0
• 黑色（0）：对应障碍物区域，解析为消息中的 100
• 灰色（1~254）：对应未知区域，解析为消息中的 - 1

四、核心 ROS 包与常用话题

1. 核心功能包

包名	核心功能	常用命令
map_server	加载 yaml+pgm 地图文件，发布nav_msgs/OccupancyGrid消息，是地图发布的核心节点	ROS1：rosrun map_server map_server my_map.yaml ROS2：ros2 run nav2_map_server map_server --ros-args -p yaml_filename:=my_map.yaml
map_saver	订阅/map话题的栅格地图消息，保存为 yaml+pgm 文件，用于 SLAM 建图后保存地图	ROS1：rosrun map_server map_saver -f my_map ROS2：ros2 run nav2_map_server map_saver_cli -f my_map

配套相关包：

• SLAM 建图：gmapping、Cartographer、HectorSLAM 等，用于实时生成并发布栅格地图；
• 定位：AMCL，基于栅格地图实现机器人自适应蒙特卡洛定位；
• 导航：move_base（ROS1）、Nav2（ROS2），基于栅格地图实现全局与局部路径规划。

2. 标准话题

话题名	消息类型	作用
/map	nav_msgs/OccupancyGrid	栅格地图核心发布话题，map_server、SLAM 节点发布，导航、定位、RViz 可视化节点订阅
/map_metadata	nav_msgs/MapMetaData	单独发布地图元数据，用于轻量化获取地图配置
/map_updates	map_msgs/OccupancyGridUpdate	动态地图增量更新话题，用于动态障碍物的实时地图修改

五、常见误区与注意事项

1. 栅格原点与顺序错误：地图原点 (0,0) 是左下角，而非左上角，data 数组为行优先存储，新手常因顺序搞反导致地图翻转、坐标转换错误；
2. 坐标系配置错误：frame_id必须为map，若误设为odom或base_link，会导致 TF 树断裂，定位与导航完全失效；
3. 数据类型错误：必须使用int8类型存储 data 数组，使用无符号类型会导致 - 1 被解析为 255，造成地图完全异常；
4. 分辨率不一致：建图、定位、导航的分辨率必须统一，否则会出现障碍物匹配偏移、路径规划穿障等问题；
5. 未知区域处理：室内导航场景建议禁止在未知区域规划路径，避免机器人进入未探测的危险区域。

编写一个简单的地图：

1. 构建一个软件包map_pkg，在依赖项里添加上nav_msgs（地图需要的消息包就在这个包里）。
2. 在map_pkg里创建一个节点map_pub_node。
3. 在节点中让大管家发布话题/map，消息类型是nav_msgs::OccupancyGird。
4. 构建一个nav_msgs::OccupancyGird地图的消息包，并对其进行赋值（0带表空白，100代表完全占据，0～100表示占据的概率，-1代表未知）。
5. 将地图消息包发送到话题/map。

#include<ros/ros.h>
#include<nav_msgs/OccupancyGrid.h>
int main(int argc, char *argv[])
{
    ros::init(argc,argv,"map_pub_node");
    ros::NodeHandle nh;
    ros::Publisher map_pub=nh.advertise<nav_msgs::OccupancyGrid>("/map",10);
    ros::Rate r(1);
    while (ros::ok())
    {
        nav_msgs::OccupancyGrid msg;
        msg.header.frame_id="map";
        msg.header.stamp=ros::Time::now();
        msg.info.origin.position.x=0;
        msg.info.origin.position.y=0;
        msg.info.resolution=1.0;
        msg.info.width=4;
        msg.info.height=2;

        msg.data.resize(4*2);
        msg.data[0]=100;
        msg.data[1]=100;
        msg.data[2]=0;
        msg.data[3]=-1;

        map_pub.publish(msg);
        r.sleep();
    }
    
    return 0;
}

打开终端编译，用ros去跑新建的这个节点，跑完节点后打开rviz添加map项目，换上这个节点话题就可以看到自己编写的地图了：

黑色就代表障碍物，白色就是无障碍，灰色是不确定；

SLAM（Simultaneous Localization And Mapping）

基础的定位方式：走一段路程找几个标志物，将不同的两个地点中相同的标志物重合拼接就做到了将这一段路程的位置确定，走一次路确定好标志物之后就可以根据标志物进行导航；
建图：在之前的章节中介绍了激光雷达，激光雷达可以扫描障碍物，激光通过的地方设置为0，不能通过的地方设置为100，没有扫描到的地方就是-1，就建立了一个栅格地图。
那么SLAM最基础的定位和建图的方式就是这样的了。

一、3 个最核心的基础概念

1. 位姿：机器人的「位置 + 朝向」，也就是 “它在哪，面朝哪个方向”，是 SLAM 的核心输出之一。
2. 2D 激光雷达：SLAM 的核心传感器，会 360° 旋转扫描，测出周围障碍物离机器人的距离，输出一圈距离数据。
3. 栅格地图：SLAM 的最终输出，就是你之前了解的nav_msgs/OccupancyGrid格式。大白话就是把环境分成一个个小格子，每个格子标 3 种状态：
   • 白色：空地，机器人能走
   • 黑色：障碍物，不能走
   • 灰色：没扫到的未知区域

---

二、SLAM 的核心逻辑

1. 第一步：算自己走了多少机器人用雷达扫的前后两帧数据，配合轮子的里程计（轮子转了多少圈），算出自己从上个位置到现在，走了多远、转了多少度，得到一个粗略的位姿。
2. 第二步：修正误差刚才算的位姿会有误差（比如轮子打滑），SLAM 会把当前雷达扫到的障碍物，和之前画的局部地图做匹配，把位姿修正到最准确的位置，避免越走越偏。
3. 第三步：发现 “我来过这”当机器人走回之前去过的地方（比如绕了一圈回到起点），SLAM 能识别出 “这个地方我之前扫过”，这个功能叫回环检测，是解决长距离走飘的核心。
4. 第四步：更新并画地图用修正后的准确位姿，把当前雷达扫到的障碍物，更新到全局地图里，循环往复，最终画出一张完整的环境地图。

---

三、ROS 里 SLAM 必须懂的 2 个核心基础

这是初学者跑通 SLAM 的关键，90% 的报错都出在这里。

1. 必须正确的 TF 坐标树

TF 就是 ROS 里各个坐标系的位置关系，SLAM 要求必须有一条完整的、不中断的 TF 链，固定格式如下：

map → odom → base_link → laser_link

每个环节的含义和发布者，你必须记清楚：

• map：全局地图坐标系，固定不动，是整个地图的原点
• odom：里程计坐标系，由机器人的轮子里程计发布，输出odom → base_link的变换
• base_link：机器人本体的中心坐标系，是机器人的 “身体原点”
• laser_link：激光雷达的坐标系，和base_link的位置关系是固定的，提前写在机器人模型里发布
• 重点：map → odom这个变换，是由 SLAM 节点发布的，SLAM 启动前，这个环节是空的，TF 链是断的。

2. 核心话题（只记这 2 个输入、2 个输出）

ROS 里 SLAM 的所有数据都是通过话题传递的，初学者不用管其他杂七杂八的话题，只记这 4 个就够了：

类型	话题名	消息类型	作用
必须输入	/scan	sensor_msgs/LaserScan	激光雷达的扫描数据，SLAM 的核心输入，没有这个就跑不起来
必须输入	/tf	tf2_msgs/TFMessage	上面说的坐标变换链，必须提前有odom→base_link和base_link→laser_link
核心输出	/map	nav_msgs/OccupancyGrid	SLAM 实时画的栅格地图，可以在 RViz 里看到，建完可以保存
核心输出	/tf	tf2_msgs/TFMessage	补全map→odom的变换，让整个 TF 链完整

四、常见问题及注意事项

• TF 链断裂：启动 SLAM 前，必须先确保odom→base_link和base_link→laser_link的 TF 是正常的，用rosrun rqt_tf_tree rqt_tf_tree可以查看 TF 树，断了就肯定跑不起来。
• 激光雷达的 frame_id 不对：/scan话题的 frame_id 必须和 TF 里的laser_link完全一致，比如你雷达的 frame_id 是laser，那 TF 里也必须是laser，一个字母都不能差。
• 建图时跑太快：机器人走太快，雷达扫的数据会错位，地图会有重影、漂移，一定要慢走，尤其是转弯的时候。
• 雷达数据有无效值：雷达扫到的太近、太远的无效数据，会导致地图有很多噪点，建图前最好给雷达加个过滤，把无效值去掉。
• 不触发回环：建大场景时，一定要让机器人回到起点，不然误差会一直累计，地图会歪、会错位，回到起点触发回环，SLAM 会自动修正全局误差。

Hector_Mapping第一个SLAM建图

从前边所学的东西来看，SLAM建图需要一个激光雷达节点发送的激光雷达数据，SLAM节点只需要订阅这个节点并发布一个map节点启动rviz就实现了建图功能；
然而从零编写一套SLAM算法对于初学者来说是非常难的，但是我们可以复用别人已经写好的SLAM算法，只需要做一个调用就可以了；

Hector_Mapping订阅的话题有两个，第一个是scan话题，第二个是一个接收重新建图指令的话题（一般用不到）所以只需要订阅第一个就可以了；

发布的话题有1、地图数据（地图描述信息）2、map话题（栅格地图数据）3和4是原始和矫正后的机器人定位信息；

综上所述Hector_Mapping核心输入数据是激光雷达数据，核心输出数据是map栅格地图数据；

安装Hector_Mapping:
在终端里输入：sudo apt install ros-noetic-hector-mapping
打开开源项目roslaunch wpr_simulation wpb_stage_slam.launch
启动rviz：rosrun rviz rviz，在rviz里添加机器人模型，激光雷达，地图就可以显示当前的状态了

之后就是让机器人动起来，然后建图，使用速度控制工具：rosrun rqt_robot_steering rqt_robot_steering;
然后就完成了建图工作：

用launch启动Hector_Mapping

<launch>

    <include file="$(find wpr_simulation)/launch/wpb_stage_slam.launch"/>
    <node pkg="hector_mapping" type="hector_mapping" name="hector_mapping"/>
    <node pkg="rviz" type="rviz" name="rviz"/>
    <node pkg="rqt_robot_steering" type="rqt_robot_steering" name="rqt_robot_steering"/>

</launch>

新建一个launch文件夹，在文件夹中新建一个.launch文件，输入以上代码；
我们将生成的rviz保存为文件格式并写入launch中就变成了：

<launch>

    <include file="$(find wpr_simulation)/launch/wpb_stage_slam.launch"/>
    <node pkg="hector_mapping" type="hector_mapping" name="hector_mapping"/>
    <node pkg="rviz" type="rviz" name="rviz" args="-d $(find slam_pkg)/rviz/slam.rviz"/>
    <node pkg="rqt_robot_steering" type="rqt_robot_steering" name="rqt_robot_steering"/>

</launch>

在终端中编译打开就可以显示所有要打开的文件了！！！

hector_mapping参数的调整首先我们需要了解hector_mapping的参数：

下边展示参数是如何调整的：
我们在launch文件中进行调整，在hector_mapping节点中进行调整参数需要将node标签设置为一个盒子中间包含参数设定:

<launch>

    <include file="$(find wpr_simulation)/launch/wpb_stage_slam.launch"/>
    
    <node pkg="hector_mapping" type="hector_mapping" name="hector_mapping" output="screen">
    <param name="map_update_distance_thresh" value="0.1"/>
    <param name="map_update_angle_thresh" value="0.1"/>
    <param name="map_pub_period" value="0.1"/>
    </node>

    <node pkg="rviz" type="rviz" name="rviz" args="-d $(find slam_pkg)/rviz/slam.rviz"/>
    <node pkg="rqt_robot_steering" type="rqt_robot_steering" name="rqt_robot_steering"/>

</launch>

这三个参数分别是地图更新距离阈值，地图更新角度阈值和地图更新速度，我们都设定的0.1，最终结果是地图更新的频率变快了；

TF系统

tf 全称Transform Library，是 ROS 生态的核心基础设施，专门用来管理、维护、查询、计算机器人系统中所有坐标系之间的时空变换关系，解决了机器人开发中最基础的 “多坐标系数据统一” 问题。目前主流使用的是其升级版tf2，旧版 tf 已在 ROS Noetic 版本后被正式弃用，下文默认以 tf2 为核心讲解。

一、为什么需要 tf 系统？

机器人系统中存在大量独立的坐标系，例如：

• 全局参考系：map（地图坐标系）、odom（里程计坐标系）
• 机器人本体：base_link（机器人基坐标系）
• 传感器：laser_link（激光雷达）、camera_link（相机）、imu_link（IMU）
• 机械臂：各个关节的连杆坐标系

不同传感器的数据都在各自的坐标系下输出，要实现 SLAM、导航、运动规划等功能，必须把这些数据统一到同一个参考系下（比如把激光雷达扫到的障碍物，转换到全局地图坐标系中）。

如果没有 tf，开发者需要手动维护所有坐标系的变换关系、手动编写矩阵乘法实现坐标转换、处理传感器数据与位姿的时间同步，工作量极大且极易出错。tf 系统把这些重复的底层工作完全封装，让开发者只需要关注业务逻辑。

二、tf 系统的核心概念

1. 坐标系（Frame）

每个 Frame 是一个带原点和 XYZ 三轴的参考系，对应机器人的一个部件、一个传感器或一个全局参考。tf 系统中所有的坐标变换，都是围绕 Frame 展开的。

2. 变换（Transform）

描述两个 Frame 之间的相对位姿关系，包含两部分：

• 平移：两个坐标系原点之间的 XYZ 偏移量（单位：m）
• 旋转：两个坐标系之间的姿态差，通常用四元数存储（也可转换为欧拉角）

核心规则：tf 的变换是子坐标系→父坐标系的转换，即通过该变换，可以把子坐标系中的点 / 位姿，转换到父坐标系中。

3. tf 树（TF Tree）

tf 系统通过有向无环树的结构管理所有坐标系，这是 tf 最核心的设计规则：

• 每个子 Frame 有且仅有一个父 Frame，禁止一个 Frame 有多个父节点；
• 树中不允许出现闭环，否则会导致变换计算逻辑混乱；
• 任意两个在同一棵树中的 Frame，都可以通过树的级联关系，自动计算出它们之间的变换。

最经典的移动机器人 tf 树结构（和你之前问的 hector_mapping 完全对应）：

map（全局地图父节点）
└── odom（里程计父节点）
    └── base_link（机器人本体父节点）
        ├── laser_link（激光雷达）
        ├── imu_link（IMU）
        └── camera_link（相机）

三、tf 系统的核心功能

1. 带时间戳的时空变换管理tf 不会只存储当前的变换，而是默认缓存 10 秒内的所有历史变换数据。你可以查询 “3 秒前，base_link在map坐标系中的位姿”，完美解决传感器数据（带时间戳）与机器人位姿的时间同步问题，这是 SLAM、导航的核心基础。

2. 自动级联变换计算只要两个 Frame 在同一棵 tf 树中，无论中间隔了多少级，tf 都会自动完成变换矩阵的级联乘法，无需开发者手动计算。例如要把laser_link中的点转换到map中，tf 会自动计算laser_link→base_link→odom→map的完整变换。

3. 分布式的发布 - 监听架构tf 采用完全分布式的设计：

   • 广播者（Broadcaster）：不同节点可以独立发布自己负责的变换（比如里程计节点发布odom→base_link，SLAM 节点发布map→odom，机器人驱动发布传感器的静态变换），节点之间无需互相通信；
   • 监听者（Listener）：需要使用变换的节点，只需要创建一个 Listener，它会自动订阅所有 tf 话题，在本地维护完整的 tf 树缓存，随时供用户查询。

4. 丰富的调试与可视化工具tf 提供了一套开箱即用的工具，快速排查变换错误、tf 树断裂等问题，是机器人调试的必备工具。

四、tf2 与旧版 tf 的核心区别

特性	旧版 tf	新版 tf2
维护状态	已弃用，无更新	ROS 官方推荐，持续维护
性能	内存占用高，计算效率一般	重构了底层逻辑，性能大幅提升，内存占用更低
功能	仅支持基础变换	支持更多数据类型、更灵活的缓存配置、静态变换优化、多线程处理
接口	接口冗余，易用性一般	接口更简洁统一，ROS1 和 ROS2 中接口高度兼容

五、静态变换与动态变换

tf 系统把变换分为两类，对应不同的发布方式：

1. 静态变换

指两个坐标系之间的相对位姿固定不变，比如激光雷达和base_link之间的安装位置、相机和机器人本体的相对位姿。

• 发布方式：使用static_transform_publisher，只需要发布 1 次，tf 会永久缓存，无需高频发布；
• 命令行示例（发布base_link到laser_link的静态变换，激光装在机器人前方 0.2m，高度 0.1m，无旋转）：

rosrun tf2_ros static_transform_publisher 0.2 0 0.1 0 0 0 base_link laser_link

参数顺序：x y z yaw pitch roll 父Frame 子Frame

2. 动态变换

指两个坐标系之间的相对位姿会随时间变化，比如odom→base_link（机器人运动）、map→odom（SLAM 修正漂移）。

• 发布方式：通过代码创建TransformBroadcaster，以 10Hz 以上的频率持续发布最新的变换，保证位姿的实时性。

六、常见节点及其参数

一、核心发布类节点（机器人运行必用）

这类节点是构建 tf 树的基础，负责发布机器人系统的静态 / 动态坐标变换，是 SLAM、导航、运动控制的前提。

1. static_transform_publisher（tf2_ros 包）

作用

tf 系统最常用的节点，用于发布两个固定不变的坐标系之间的静态 tf 变换，比如激光雷达、相机、IMU 与机器人本体base_link的安装位姿。tf2 对静态变换做了专属优化：仅需发布 1 次，就会被 tf2 永久缓存，资源占用极低，远优于高频发布动态变换的方式。

核心参数与用法

支持两种姿态输入格式，可通过命令行或 launch 文件启动：

格式 1：欧拉角输入（最常用，Z-Y-X 顺规，单位 rad）

# 命令行格式
rosrun tf2_ros static_transform_publisher x y z yaw pitch roll 父坐标系 子坐标系

格式 2：四元数输入

rosrun tf2_ros static_transform_publisher x y z qx qy qz qw 父坐标系 子坐标系

参数说明

参数	含义	注意事项
x y z	子坐标系相对于父坐标系的平移量，单位 m	对应安装位置的前后、左右、上下偏移
yaw pitch roll	子坐标系相对于父坐标系的欧拉角，单位 rad	分别对应绕 Z、Y、X 轴的旋转，新手建议先转成弧度再输入
qx qy qz qw	子坐标系相对于父坐标系的四元数	注意四元数的顺序是x y z w，不要搞反
父坐标系 / 子坐标系	变换的父子节点	严格遵循「子→父」的变换规则，顺序错误会导致变换完全反向

示例

激光雷达安装在机器人前方 0.2m、高度 0.15m，无旋转，发布base_link→laser_link的静态变换：

rosrun tf2_ros static_transform_publisher 0.2 0 0.15 0 0 0 base_link laser_link

关键注意

旧版 tf 的static_transform_publisher默认以 100ms 周期高频发布，而 tf2 版本默认是静态 latched 发布（仅发 1 次），无需额外加周期参数，避免浪费资源。

2. robot_state_publisher

作用

机器人 tf 系统的核心节点，根据机器人的 URDF 模型描述，配合关节状态数据，自动发布机器人各个连杆（link）之间的 tf 变换，是带关节的机器人（机械臂、差速轮机器人、人形机器人）的必备节点。

• 对 URDF 中的fixed固定关节：默认以静态 tf 发布，节省资源；
• 对 URDF 中的revolute/prismatic活动关节：根据收到的关节角度，实时发布动态 tf。

核心参数

参数名	类型	默认值	含义与调参影响
robot_description	string	无（必选）	机器人 URDF 模型的 XML 字符串，一般通过rosparam从 URDF 文件加载
publish_frequency	double	50.0	tf 变换的发布频率，单位 Hz。导航 / 机械臂控制场景建议保持 50Hz 以上，保证 tf 实时性；低配设备可适当降低
use_tf_static	bool	true	是否将固定关节的变换以静态 tf 发布。开启后大幅降低资源占用，强烈建议保持默认 true
ignore_timestamp	bool	false	是否忽略关节状态的时间戳，强制用当前时间发布 tf。仅在仿真、调试时开启，真机运行保持 false

订阅话题

• joint_states（sensor_msgs/JointState）：关节角度 / 位置数据，一般来自硬件驱动、joint_state_publisher或仿真器。

典型 launch 启动示例

<launch>
  <!-- 加载URDF模型到参数服务器 -->
  <param name="robot_description" textfile="$(find my_robot)/urdf/robot.urdf" />
  <!-- 启动robot_state_publisher -->
  <node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" output="screen">
    <param name="publish_frequency" value="50.0" />
    <param name="use_tf_static" value="true" />
  </node>
</launch>

---

3. joint_state_publisher / joint_state_publisher_gui

作用

配合robot_state_publisher使用，用于发布joint_states话题，为 URDF 中的活动关节提供角度 / 位置数据。

• joint_state_publisher：后台节点，可设置关节初始值，适合仿真场景；
• joint_state_publisher_gui：带图形界面的版本，可通过滑块拖动调整关节角度，是调试 URDF 模型、验证 tf 树的神器。

核心参数

参数名	类型	默认值	含义
rate	double	10.0	joint_states话题的发布频率，单位 Hz
zeros	dict	无	各个关节的初始零位，格式为{"关节名": 初始值}
source_list	list	空	关节状态的源话题列表，用于合并多个节点发布的关节数据

注意

真机运行时，关节状态由硬件驱动直接发布，无需启动该节点；仅在 URDF 调试、仿真场景使用。

二、调试工具类节点（排查 tf 问题必备）

这类节点是 tf 系统的调试利器，用于快速定位 tf 树断裂、变换延迟、位姿错误、结构异常等常见问题。

1. tf2_monitor（tf2_ros 包）

作用

实时监控 tf 树中所有变换的发布状态，包括发布频率、平均延迟、最大延迟、发布节点信息，快速定位哪个变换发布异常、频率不足、延迟过高。

用法

# 1. 监控tf树中所有变换的状态
rosrun tf2_ros tf2_monitor

# 2. 仅监控指定两个坐标系之间的变换（比如map到base_link）
rosrun tf2_ros tf2_monitor map base_link

---

2. tf2_echo（tf2_ros 包）

作用

实时打印两个指定坐标系之间的变换关系（平移 + 旋转，同时输出欧拉角和四元数），并显示变换的时间戳、延迟、是否可用，快速验证变换是否正确、安装位姿是否符合预期。

用法

# 格式：rosrun tf2_ros tf2_echo 父坐标系 子坐标系
# 示例：查看laser_link相对于base_link的位姿
rosrun tf2_ros tf2_echo base_link laser_link

---

3. view_frames.py（tf2_tools 包）

作用

监听 5 秒内的所有 tf 变换，生成完整的 tf 树可视化 PDF 文件，包含所有坐标系的父子关系、发布频率、平均延迟、缓存状态，是排查 tf 树断裂、闭环、多父节点问题的首选工具。

用法

rosrun tf2_tools view_frames.py

执行后会在当前目录生成frames.pdf，打开即可看到完整的 tf 树结构，哪里出现断层、错误一眼可见。

---

4. roswtf tf 插件

作用

ROS 自带的系统诊断工具，内置 tf 扩展，可自动扫描 tf 树的常见错误：比如坐标系闭环、一个 frame 有多个父节点、未发布的变换、时间戳异常、tf 树断裂等，并自动给出报错原因和修复建议，是新手排查 tf 问题的神器。

用法

直接在终端运行：

roswtf

---

三、辅助功能类节点（特定场景使用）

1. buffer_server（tf2_ros 包）

作用

提供 tf 变换的 RPC 服务，允许其他节点通过 ROS 服务查询 tf 变换，而不需要自己维护 tf listener 和缓存，适合分布式多机通信、跨节点共享 tf 数据的场景。

用法

rosrun tf2_ros buffer_server

启动后会提供~tf2_buffer_server服务（tf2_msgs/LookupTransform类型），其他节点可通过服务请求查询变换。

2. tf2_web_republisher

作用

将 tf 变换转发到 web 端，用于 web 可视化场景（比如 roslibjs、3D 机器人可视化页面），适合需要在网页展示机器人位姿的场景。

核心参数

• rate：发布到 web 端的 tf 频率，默认 10Hz。

---

四、tf 系统通用核心参数与配置

1. 核心话题

tf2 将静态和动态变换分离到两个专属话题，是 tf 系统的全局通信基础：

话题名	类型	作用
/tf	tf2_msgs/TFMessage	动态变换话题，所有随时间变化的 tf（比如 odom→base_link）都发布到该话题
/tf_static	tf2_msgs/TFMessage	静态变换话题，latched 模式，仅发布 1 次，订阅者即可收到，用于固定不变的变换

2. tf 缓存配置

tf2 通过Buffer类缓存历史变换，核心配置参数：

• cache_time：缓存历史变换的时间长度，默认 10 秒。
  • 调大（比如 30 秒）：可查询更早的历史变换，适合处理延迟高的传感器（比如相机、点云），但会增加内存占用；
  • 调小（比如 1 秒）：内存占用降低，但容易出现「Lookup would require extrapolation into the past」的外推报错。

3. 全局关键参数

• use_sim_time：全局 bool 参数，默认 false。Gazebo 仿真场景必须设置为 true，否则 tf 的时间戳会和仿真时间不匹配，出现大量外推报错、变换查询失败。

七、新手常见坑与避坑指南

1. tf 树闭环 / 多父节点报错绝对禁止一个 Frame 设置多个父节点，也不允许出现闭环，否则 tf 树会直接断裂，所有变换查询都会失败。

2. 时间戳外推报错最常见的报错：Lookup would require extrapolation into the past/future

   • 原因：查询的时间戳对应的变换还没发布，或者已经超出了 tf 的缓存时间；
   • 解决方法：使用waitForTransform等待变换可用；用ros::Time(0)查询最新的变换；保证变换发布频率≥10Hz；按需增大 tf 的缓存时间。

3. 父子坐标系搞反变换的方向错误会导致坐标转换结果完全错误。记住核心规则：变换是子坐标系到父坐标系的转换，父节点是参考系，子节点是被转换的坐标系。

4. 坐标系名称不匹配比如 hector_mapping 中设置的base_frame是base_link，但实际 tf 中发布的是base_footprint，会直接导致 SLAM 找不到变换、定位漂移，所有节点的 Frame 名称必须和 tf 树中的完全一致。

5. 静态变换用动态方式发布固定不变的安装位姿，不要用高频动态广播发布，不仅浪费 CPU 资源，还可能出现延迟导致的变换查询失败，优先使用静态变换发布工具。

里程计及其在SLAM中的作用

一、里程计核心介绍

1. 基本定义与核心原理

狭义上，里程计特指轮式里程计，通过机器人轮子的编码器数据推算运动增量；广义上，里程计（Odometry）泛指所有通过传感器连续观测数据，以航位推算（Dead Reckoning） 为核心原理，估计载体（机器人、无人车等）相对位姿（位置 + 姿态）随时间变化的技术。

其核心逻辑是：从已知的初始位姿出发，通过相邻时刻传感器的增量数据，连续积分推算出载体当前相对于初始时刻的位姿，核心输出是帧间 / 相邻时刻的相对位姿变换。

2. 主流类型与技术特点

SLAM 领域主流的里程计方案及核心特性如下：

里程计类型	核心传感器	核心原理	核心优势	核心局限
轮式里程计	电机编码器	基于轮子运动学模型（差分、阿克曼、全向轮），通过编码器脉冲数计算轮速、转角，积分推算位姿	成本极低、频率高（百 Hz 级）、短时间内精度高、计算量极小	仅支持平面运动，受轮子打滑、地面不平、轮胎磨损影响大，累计误差随距离快速增长
视觉里程计 (VO)	单目 / 双目 / RGBD 相机	提取图像特征点（ORB/SIFT 等）或采用直接法（光流），匹配连续帧的观测，求解帧间位姿变换	场景适应性强，不受轮式打滑限制，可适配无人机、手持设备等非轮式载体，纹理丰富场景精度高	受光照、动态物体、纹理缺失场景影响大，单目存在尺度不确定性，计算量高于轮式里程计
激光里程计 (LO)	2D/3D 激光雷达	通过点云配准算法（ICP、NDT、线面特征匹配等），匹配连续帧点云，求解帧间位姿变换	抗光照变化能力极强，测距精度高，结构信息丰富，累计漂移远小于 VO，适配室内外复杂场景	重复结构（长廊 / 隧道）、空旷场景易退化，高动态场景匹配难度大，硬件成本与点云计算量较高
惯性里程计 (IO)	IMU（加速度计 + 陀螺仪）	对 IMU 的角速率、加速度数据积分，推算载体的角速度、速度与位姿	频率极高（千 Hz 级）、不受外界环境干扰，短时运动估计极稳定，可捕捉高速运动	纯 IMU 零偏与噪声会导致误差指数级发散，无法单独长时间工作，必须与其他传感器融合
融合里程计	多传感器组合（轮速 + IMU、视觉 + IMU（VIO）、激光 + IMU（LIO）等）	紧耦合 / 松耦合融合多源数据，互补传感器缺陷，联合优化求解位姿	鲁棒性与精度大幅提升，可适配极端退化场景，是当前 SLAM 的主流方案	系统复杂度高，标定与融合算法门槛高，对硬件同步性要求严格

二、里程计在 SLAM 中的核心作用

SLAM（同步定位与地图构建）的核心目标，是在未知环境中同时估计机器人自身的全局位姿，并构建环境的一致性地图。里程计是 SLAM 系统的前端核心模块，是整个系统稳定运行的基础，核心作用可分为以下 6 点：

1. 提供高频连续的位姿输出，保障 SLAM 系统的实时性

SLAM 的后端优化、回环检测均为低频模块（通常 1~10Hz），无法满足机器人运动控制、高频位姿反馈的需求；而里程计可输出几十到上千 Hz 的连续位姿，为机器人提供实时的运动状态反馈，保障运动控制的连续性与稳定性。

同时，里程计可为激光点云去畸变、图像帧间匹配提供高频运动插值，消除载体运动对传感器观测的影响，提升原始数据的质量。

2. 为帧间匹配提供可靠初始值，大幅提升配准成功率与效率

SLAM 中激光点云配准、图像位姿求解的核心算法（如 ICP、PNP），对初始位姿的精度要求极高：若初始位姿偏差过大，算法极易陷入局部最优，导致配准失败、位姿跳变甚至跟丢。

里程计输出的相邻帧相对位姿，可为帧间匹配提供精准的初始先验，大幅减少配准的迭代次数，同时避免算法陷入局部最优，是 SLAM 前端稳定跟踪的核心保障。

3. 提供位姿约束，缩小求解空间，抑制误差发散

纯全局位姿求解的自由度极高，极易出现位姿发散、地图漂移的问题。里程计为 SLAM 系统提供了相邻帧 / 关键帧之间的刚性位姿约束，在图优化 SLAM 框架中，这种约束对应着位姿图中相邻节点的边，与路标观测边、回环边共同构成优化目标。

它将无约束的全局位姿求解，转化为有连续先验的增量式优化问题，大幅缩小了位姿的求解空间，显著提升了后端优化的收敛速度与精度，从根源上抑制了位姿误差的快速发散。

4. 应对传感器退化场景，提升 SLAM 系统的鲁棒性

SLAM 的核心环境感知传感器（相机、激光雷达）极易出现观测退化：视觉会遭遇无纹理白墙、强光 / 弱光、运动模糊；激光会遭遇长廊、隧道、空旷无特征场景。此时纯视觉 / 激光 SLAM 会直接跟丢，系统完全失效。

而里程计（尤其是轮式、IMU 融合里程计）不受环境纹理、光照的影响，可在退化场景中持续提供可靠的位姿增量，维持系统的定位连续性，直到载体驶出退化场景，前端重新完成跟踪，是 SLAM 系统应对复杂环境的核心容错机制。

5. 解决尺度不确定性，完成坐标系对齐

单目视觉 SLAM 存在尺度不确定性，纯单目 VO 无法还原真实的物理尺度，只能得到尺度未知的位姿与地图。而轮式里程计、激光里程计、IMU 均带有真实的物理尺度信息，可通过融合为单目 SLAM 提供尺度约束，还原符合真实世界的尺度。

同时，IMU 里程计可输出重力向量，为 SLAM 系统提供世界坐标系的重力对齐，确定水平与垂直方向，构建符合物理规律的欧氏地图，适配机器人的导航与运动控制需求。

6. 优化关键帧管理，加速回环检测

里程计可实时输出载体的运动增量（平移量、旋转角度），为 SLAM 系统的关键帧筛选提供核心依据：仅当运动增量超过预设阈值时，才触发关键帧提取。这一机制可过滤掉冗余的、无场景变化的帧，在保证地图精度的前提下，大幅降低后端优化的计算量。

同时，里程计推算的当前位姿，可为回环检测提供搜索范围约束，无需全局遍历所有历史关键帧，仅需检索当前位姿附近的关键帧即可，显著提升回环检测的速度与准确率，减少误回环的概率。

Gmapping进行SLAM建图

一、核心逻辑与原理

Gmapping 的核心是将复杂的 SLAM 联合概率问题拆解为定位 + 建图两个独立过程：位姿轨迹地图观测控制位姿轨迹观测控制地图位姿轨迹观测通过粒子滤波解决机器人轨迹估计（定位），每个粒子独立维护一条位姿轨迹和一张栅格地图，在已知轨迹的前提下通过贝叶斯规则更新地图，大幅降低 SLAM 问题的计算复杂度。

核心算法流程

1. 粒子初始化生成 N 个粒子，每个粒子包含：机器人初始位姿(x,y,θ)、权重、独立的占用栅格地图，初始权重均匀分布，代表对机器人位姿的初始假设。

2. 运动模型位姿预测利用轮式里程计的控制量，通过差分运动模型更新每个粒子的位姿，预测新一时刻的位姿分布，完成粒子的先验更新。

3. 扫描匹配与权重更新（核心优化）通过扫描匹配（Scan Matching） 将当前激光扫描数据与每个粒子自身的地图做配准，修正里程计预测的位姿，得到更精准的局部位姿；再计算激光观测的似然概率，更新每个粒子的权重，权重越高代表该粒子的位姿和地图越可信。

4. 选择性重采样计算有效粒子数 Neff​=∑(wi​)21​，当重采样阈值（默认 0.5）时执行重采样：复制高权重粒子，淘汰低权重粒子，缓解粒子退化问题；同时避免每帧都重采样，保护粒子多样性，减少粒子耗散。

5. 栅格地图更新对每个粒子，根据修正后的位姿和激光观测数据，用贝叶斯规则更新其维护的栅格地图中每个单元格的占用概率（占用 / 空闲 / 未知三种状态）。

6. 地图与定位输出最终输出权重最高的最优粒子对应的全局栅格地图，同时发布map到odom的 TF 坐标变换，修正里程计的累计误差，完成定位与建图的闭环。

相对原始 FastSLAM 的两大核心优化

1. 提议分布优化：原始 FastSLAM 仅用里程计运动模型作为提议分布，误差大，需要成百上千个粒子；Gmapping 将激光扫描匹配结果融入提议分布，大幅缩小位姿的不确定性，通常仅需 30-80 个粒子即可稳定建图，计算量骤降。
2. 选择性重采样：仅当有效粒子数低于阈值时才执行重采样，避免频繁重采样导致的粒子多样性丢失，提升长时间建图的稳定性。

二、运行所需的核心数据与硬件

Gmapping 的运行强依赖里程计、激光雷达数据和TF 坐标变换，三者缺一不可。

1. 硬件基础

• 移动机器人底盘：差分驱动轮式机器人，搭载光电码盘，能输出精准的轮式里程计（线速度、角速度）。
• 2D 单线激光雷达：水平安装，扫描平面与地面平行，能输出标准的 LaserScan 数据（如 RPLIDAR、Hokuyo、SICK 等）。

2. 必须的输入数据（ROS 环境）

数据类型	话题 / 格式	核心要求
激光雷达数据	话题默认/scan，格式sensor_msgs/LaserScan	包含时间戳、frame_id、角度范围、角分辨率、测距数组；过滤雷达盲区、NaN/Inf 值；时间戳与里程计、TF 严格同步
里程计数据	话题/odom（格式nav_msgs/Odometry），核心通过/tf发布	输出机器人线速度、角速度，无明显打滑、累计误差过大问题；里程计噪声需与算法运动模型参数匹配

3. 必须的 TF 坐标变换

Gmapping 通过/tf和/tf_static获取坐标关系，以下两条 TF 链路必须完整，否则算法无法初始化：

1. 静态 TF：base_link（机器人底盘坐标系） → laser_link（激光雷达坐标系）描述激光雷达相对于底盘的安装偏移（xyz、rpy），固定不变，通常由 URDF 或static_transform_publisher发布。
2. 动态 TF：odom（里程计坐标系） → base_link（机器人底盘坐标系）由里程计节点实时发布，描述机器人相对于里程计原点的位姿变化，是算法运动模型的核心输入。

4. 算法输出

• /map：nav_msgs/OccupancyGrid格式，最终的 2D 占用栅格地图；
• /map_metadata：地图元数据，包含分辨率、尺寸、原点等信息；
• /tf：发布map→odom的动态 TF 变换，修正里程计累计误差，完成机器人在地图中的定位；
• /slam_gmapping/entropy：位姿估计的熵值，数值越大代表定位不确定性越高；
• 服务/dynamic_map：可主动调用获取当前完整地图。

三、核心可调参数详解

参数按优先级分为 5 大类，核心参数直接决定建图精度、稳定性和计算开销，以下为 ROS 环境下slam_gmapping节点的核心参数：

1. 激光雷达核心参数（最优先调整）

参数名	默认值	核心含义	调参建议
maxUrange	80.0	激光最大有效测距范围，超出该值的射线会被截断，参与地图边界计算	设为雷达额定最大测距的 80%，如 10 米雷达设为 8.0，过滤远距离噪点
maxRange	80.0	激光最大可接受距离，超出该值的射线直接丢弃，不参与计算	略大于maxUrange，等于雷达额定最大测距
lskip	0	激光射线跳数，每lskip+1条射线取 1 条参与计算	雷达线数过高时设为 1-2，降低计算量，不建议超过 4
throttle_scans	1	激光帧跳数，每throttle_scans帧取 1 帧处理	雷达帧率过高（20Hz+）时设为 2，降低 CPU 占用

2. 扫描匹配核心参数（决定位姿修正精度）

参数名	默认值	核心含义	调参建议
sigma	0.05	扫描匹配的测距噪声标准差（m）	室内小场景 0.05，大场景 / 噪点多设为 0.1-0.2
iterations	5	扫描匹配的最大迭代次数	室内场景 5-10，长走廊场景设为 15，提升匹配精度
lstep	0.05	扫描匹配的线性搜索步长（m）	0.05-0.1，步长越小匹配越精细，计算量越大
astep	0.05	扫描匹配的角度搜索步长（rad）	0.05-0.1，转弯多的场景调小，防旋转漂移
minimumScore	0.0	扫描匹配最小得分阈值，低于该值的匹配结果无效	室内场景设为 50-200，过滤无效匹配，避免地图漂移

3. 粒子滤波与重采样参数（决定算法稳定性）

参数名	默认值	核心含义	调参建议
particles	30	粒子数量，每个粒子维护一张独立地图	低配设备 30，大场景 / 里程计误差大设为 50-80，超过 100 收益极低，CPU 占用飙升
resampleThreshold	0.5	重采样触发阈值，阈值 时执行重采样	保持 0.5，粒子抖动时微调至 0.3-0.4，减少重采样频率

4. 地图更新与分辨率参数（决定地图精度）

参数名	默认值	核心含义	调参建议
linearUpdate	1.0	机器人直线移动该距离（m），触发一次地图更新	室内场景 0.1-0.3，越小地图细节越丰富
angularUpdate	0.5	机器人旋转该角度（rad），触发一次地图更新	室内场景 0.1-0.2，转弯多的场景调小
temporalUpdate	-1.0	强制更新的时间间隔（s），-1 为关闭	静止场景设为 1.0-3.0，避免机器人不动时地图停滞
map_update_interval	5.0	地图发布的最小时间间隔（s）	实时建图设为 1.0-2.0，平衡实时性和计算量
delta	0.05	地图栅格分辨率（m / 栅格）	高精度需求设为 0.025（2.5cm），大场景 / 低配设备保持 0.05
xmin/xmax/ymin/ymax	-100.0/100.0/-100.0/100.0	地图初始边界（m）	提前估算场地尺寸，留出 20% 余量，避免地图超限截断

5. 运动模型噪声参数（匹配里程计精度）

参数名	默认值	核心含义	调参建议
srr	0.01	平移分量带来的平移噪声（m/m）	里程计精度差 / 打滑严重时调大
srt	0.02	平移分量带来的旋转噪声（rad/m）	转弯漂移时调大
str	0.01	旋转分量带来的平移噪声（m/rad）	原地旋转漂移时调大
stt	0.02	旋转分量带来的旋转噪声（rad/rad）	原地旋转漂移时调大

6.接口

坐标系设置就是接口设置：
base_frame(默认base_link) 底盘坐标系名称
map_frame(默认map) 地图坐标系名称
odom_frame(默认odom_link) 里程计坐标名称

<launch>
  <node pkg="gmapping" type="slam_gmapping" name="slam_gmapping" output="screen">
    <!-- 坐标系配置 -->
    <param name="base_frame" value="base_link" />
    <param name="odom_frame" value="odom" />
    <param name="map_frame" value="map" />
    
    <!-- 激光雷达配置（10米雷达） -->
    <param name="maxUrange" value="8.0" />
    <param name="maxRange" value="10.0" />
    <param name="lskip" value="0" />
    
    <!-- 扫描匹配配置 -->
    <param name="sigma" value="0.05" />
    <param name="iterations" value="10" />
    <param name="minimumScore" value="100" />
    
    <!-- 粒子滤波配置 -->
    <param name="particles" value="50" />
    <param name="resampleThreshold" value="0.5" />
    
    <!-- 地图更新配置 -->
    <param name="linearUpdate" value="0.2" />
    <param name="angularUpdate" value="0.15" />
    <param name="temporalUpdate" value="1.0" />
    <param name="map_update_interval" value="1.0" />
    
    <!-- 地图分辨率与边界 -->
    <param name="delta" value="0.05" />
    <param name="xmin" value="-50.0" />
    <param name="xmax" value="50.0" />
    <param name="ymin" value="-50.0" />
    <param name="ymax" value="50.0" />
    
    <!-- 里程计噪声配置 -->
    <param name="srr" value="0.01" />
    <param name="srt" value="0.02" />
    <param name="str" value="0.01" />
    <param name="stt" value="0.02" />
    
    <!-- 激光话题重映射 -->
    <remap from="scan" to="/scan" />
  </node>
</launch>

打开开源项目：roslaunch wpr_simulation wpb_stage_robocup.launch

查看tf树：

之后我们就可以启动gmapping了：rosrun gmapping slam_gmapping;
启动rviz：rosrun rviz rviz,打开map，激光雷达和机器人模型：

打开键盘控制节点rosrun wpr_simulation keyboard_vel_ctrl,控制键盘让机器人运动完整个地图就实现了用gmapping创建一个地图：

用launch文件一键启动:

<launch>
    <include file="$(find wpr_simulation)/launch/wpb_stage_robocup.launch"/>

    <node pkg="gmapping" type="slam_gmapping" name="slam_gmapping"/>
    
    <node pkg="rviz" type="rviz" name="rviz" args="-d $(find slam_pkg)/rviz/gmapping.rviz"/>

    <node pkg="wpr_simulation" type="keyboard_vel_ctrl" name="keyboard_vel_ctrl"/>

</launch>

修改gmapping的参数：
一般来说初学者会修改的参数只有一下几种：

<launch>
    <include file="$(find wpr_simulation)/launch/wpb_stage_robocup.launch"/>

    <node pkg="gmapping" type="slam_gmapping" name="slam_gmapping">
        <param name="maxUrange" value="3.0"/>
        
        <param name="linearUpdate" value="0.1"/>

        <param name="map_update_interval" value="0.5"/>
    </node>
    
    <node pkg="rviz" type="rviz" name="rviz" args="-d $(find slam_pkg)/rviz/gmapping.rviz"/>

    <node pkg="wpr_simulation" type="keyboard_vel_ctrl" name="keyboard_vel_ctrl"/>

</launch>

SLAM建图的保存与调用

使用的是map_server包里的map_saver指令，将建好的地图保存下来，在终端中输入：
rosrun map_server map_saver [--ooc <threshold_occupied>] [--free <threshold_free>] [-f<mapname>] map:=路径地址

然后就得到了两个新的文件一个是.pgm文件，一个是.yaml文件，pgm文件存储的是建好的地图，我们来看看yaml文件：

首先记录了图像，就是那个pgm文件，resolution记录的是地图栅格的边长大小，地图的基础坐标在地图中心，向两边延伸到边界的长就是origin的前两个参数，最后一个参数是地图相对于地图坐标的偏转角度；

保存成功之后，要实现导航还需要对其进行加载（使用在map_server中的map_server节点进行加载）：rosrun map_server map_server mymap.yaml
打开rviz进行加载地图就可以了rosrun rviz rviz

至此就实现了建图保存以及调用！

Navigation导航栈

一、导航栈的整体架构与数据流

导航栈的核心是模块化协作，各模块通过 ROS 话题（Topic）和服务（Service）通信。以下是完整的数据流：

1. 传感器数据输入

   • 激光雷达（sensor_msgs/LaserScan）或深度相机（转换为激光数据）：用于实时检测障碍物。
   • 里程计（nav_msgs/Odometry）：记录机器人的位移和速度（如轮式编码器、IMU 融合）。
   • 地图（nav_msgs/OccupancyGrid）：静态地图，由 SLAM（如 GMapping、Cartographer）预先构建。

2. 数据预处理

   • 激光滤波：通过laser_filters包过滤噪声、限制检测范围。
   • 里程计校准：通过imu_filter_madgwick等融合 IMU 数据，提高里程计精度。

3. 核心模块处理

   • 定位：AMCL 利用激光数据和地图修正里程计误差，输出机器人位姿（geometry_msgs/PoseWithCovarianceStamped）。
   • 代价地图：结合静态地图、激光数据和机器人半径，生成 “可通行区域” 的代价地图。
   • 路径规划：全局规划器规划粗路径，局部规划器根据实时环境调整速度。
   • 运动控制：move_base协调各模块，输出速度指令（geometry_msgs/Twist）给机器人底盘。

二、核心简单解析

1. 定位模块：AMCL（自适应蒙特卡洛定位）

AMCL 是基于粒子滤波的概率定位算法，核心思想是用 “粒子群” 表示机器人的可能位姿，通过传感器数据不断修正粒子分布。

• 粒子滤波原理：

  1. 初始化：在地图中随机生成 N 个粒子（每个粒子代表一个可能的位姿：x, y, θ）。
  2. 预测：根据里程计数据，移动所有粒子（模拟机器人运动）。
  3. 更新：用激光数据计算每个粒子的 “权重”—— 粒子位姿对应的激光扫描与地图匹配度越高，权重越大。
  4. 重采样：淘汰低权重粒子，复制高权重粒子，使粒子群向真实位姿收敛。

• 关键参数（amcl.launch中配置）：

  • min_particles/max_particles：粒子数范围（通常 50-200，粒子越多定位越准但计算量越大）。
  • laser_model_type：激光模型（如likelihood_field，适合动态环境）。
  • update_min_d/update_min_a：仅当机器人移动超过距离 / 角度阈值时，才更新粒子（减少计算量）。

• 注意：AMCL 需要已知地图，且初始时需通过 RViz 的 “2D Pose Estimate” 给出大致位姿（粒子初始化）。

2. 地图与代价地图（Costmap）

代价地图是导航栈的 “环境感知核心”，用网格表示地图，每个网格有一个 “代价值”（0 = 可通行，255 = 障碍物，中间值 = 膨胀区域）。

• 代价地图的分层结构（costmap_common_params.yaml配置）：

  1. 静态层（Static Layer）：加载预先构建的 SLAM 地图，标记固定障碍物（如墙、家具）。
  2. 障碍物层（Obstacle Layer）：实时订阅激光数据，标记动态障碍物（如人、移动的物体）。
     • 关键参数：obstacle_range（障碍物检测范围，如 2.5m）、raytrace_range（激光穿透范围，如 3.0m，用于清除已消失的障碍物）。
  3. 膨胀层（Inflation Layer）：在障碍物周围膨胀出 “安全区域”，防止机器人碰撞。
     • 关键参数：inflation_radius（膨胀半径，需大于机器人半径，如 0.5m）、cost_scaling_factor（代价值衰减系数，值越大膨胀区域代价值越低）。

• 全局代价地图 vs 局部代价地图：

  • 全局代价地图：用于全局规划，范围大（如整个地图），更新频率低（如 1Hz）。
  • 局部代价地图：用于局部避障，范围小（如机器人周围 5m×5m），更新频率高（如 5Hz）。

3. 路径规划模块

导航栈包含全局规划器和局部规划器，两者协作实现 “全局最优 + 局部避障”。

（1）全局规划器（Global Planner）

在静态地图上规划从起点到目标点的全局路径（粗路径），常用算法：

• Dijkstra 算法：广度优先搜索，保证找到最短路径，但计算量较大（适合小地图）。

• A * 算法：启发式搜索，用 “曼哈顿距离” 或 “欧氏距离” 作为启发函数，比 Dijkstra 更快（最常用）。

• 配置：在move_base.launch中指定全局规划器，如：

  xml

  <param name="base_global_planner" value="global_planner/GlobalPlanner"/>
  

• 关键参数（global_planner_params.yaml）：

  • use_dijkstra：设为false则使用 A * 算法。
  • tolerance：目标点容差（如 0.2m，允许机器人到达目标点附近即可）。

（2）局部规划器（Local Planner）

根据全局路径和局部代价地图，实时计算机器人的速度指令（线速度 v、角速度 ω），实现避障和轨迹跟踪。常用两种：

① DWA（动态窗口法，Dynamic Window Approach）

• 核心思想：在机器人的 “速度空间”（v, ω）中采样多组速度，模拟每组速度下的轨迹，选择 “最优” 轨迹。

• 步骤：

  1. 速度采样：根据机器人硬件限制（如最大线速度 0.5m/s，最大角速度 1.0rad/s），生成速度窗口。
  2. 轨迹模拟：对每组（v, ω），模拟未来 1 秒内的轨迹。
  3. 轨迹评价：用评价函数打分，选择最高分的速度：
     • 评价函数 = 路径跟踪权重 ×(与全局路径的距离) + 速度权重 ×(线速度大小) + 障碍物权重 ×(与最近障碍物的距离)。

• 关键参数（dwa_local_planner_params.yaml）：

  • max_vel_x/min_vel_x：线速度范围（如 0.0~0.5m/s）。
  • max_vel_theta：最大角速度（如 1.0rad/s）。
  • path_distance_bias/goal_distance_bias/occdist_scale：评价函数的三个权重（需根据实际环境调试）。

② TEB（时间弹性带，Timed Elastic Band）

• 核心思想：将全局路径视为一条 “弹性带”，通过优化带的形状（位置、时间间隔），使轨迹满足 “动力学约束”（如速度、加速度限制）和 “避障约束”。
• 优势：比 DWA 更平滑，适合全向移动机器人，能处理复杂的动态环境。
• 配置：需安装teb_local_planner包，在move_base.launch中指定：

  xml

  <param name="base_local_planner" value="teb_local_planner/TebLocalPlannerROS"/>
  

4. 核心协调模块：move_base

move_base是导航栈的 “大脑”，负责：

• 接收目标点（通过 RViz 的 “2D Nav Goal” 或话题move_base_simple/goal）。
• 调用全局规划器生成全局路径。
• 调用局部规划器生成速度指令。
• 处理异常（如路径规划失败、机器人被困），通过 “恢复行为”（Recovery Behaviors）解决：
  • 旋转清除（rotate_recovery）：机器人原地旋转，清除局部代价地图的噪声。
  • 清除代价地图（clear_costmap_recovery）：重置全局 / 局部代价地图。

三、导航栈的配置文件结构

要让导航栈运行，需准备以下配置文件（以差速机器人为例）：

nav_config/
├── costmap_common_params.yaml    # 代价地图通用参数（分层、膨胀半径等）
├── global_costmap_params.yaml     # 全局代价地图参数（范围、分辨率等）
├── local_costmap_params.yaml      # 局部代价地图参数（范围、更新频率等）
├── dwa_local_planner_params.yaml  # DWA局部规划器参数
├── move_base.launch               # move_base节点启动文件（加载所有配置）
└── amcl.launch                    # AMCL定位节点启动文件

• 关键配置示例（costmap_common_params.yaml）：

  obstacle_range: 2.5          # 障碍物检测范围（m）
  raytrace_range: 3.0          # 激光穿透范围（m）
  inflation_radius: 0.5        # 膨胀半径（m，需大于机器人半径）
  robot_radius: 0.2            # 机器人半径（m，若用footprint则不用此参数）
  footprint: [[0.2, 0.2], [-0.2, 0.2], [-0.2, -0.2], [0.2, -0.2]]  # 机器人形状（四边形）
  

四、实际应用中的调试与常见问题

1. 调试工具：RViz

RViz 是导航调试的核心工具，需添加以下显示项：

• Map：显示静态地图。
• PoseArray：显示 AMCL 的粒子群（观察粒子是否收敛）。
• Path：显示全局路径（/move_base/NavfnROS/plan）和局部轨迹（/move_base/TebLocalPlannerROS/local_plan）。
• Costmap：显示全局 / 局部代价地图（通过 “Grid Cells” 或 “Costmap2D” 插件）。

2. 常见问题与解决方案

• 定位不准：

  • 检查激光雷达是否正常工作（rostopic echo /scan）。
  • 调整 AMCL 的粒子数（max_particles）和激光模型参数。
  • 确保里程计精度（可通过rosbag record记录数据，离线分析）。

• 路径规划失败：

  • 检查目标点是否在障碍物内（RViz 中查看代价地图）。
  • 调整全局规划器的容差（tolerance）。
  • 若局部规划失败，尝试增大 DWA 的path_distance_bias（加强路径跟踪）。

• 机器人碰撞障碍物：

  • 增大代价地图的inflation_radius。
  • 调整 DWA 的occdist_scale（增大障碍物权重）。

move_base节点

一、move_base 的核心定位与设计思想

move_base 的本质是一个基于 ActionLib 的状态机，它通过 “目标驱动” 的方式工作：

• 接收用户的导航目标（move_base_msgs/MoveBaseAction）；
• 协调全局规划器（找 “大方向”）和局部规划器（管 “脚下避障”）；
• 监控导航过程，处理失败场景（如路径被堵、规划失败），触发恢复行为；
• 最终输出速度指令（geometry_msgs/Twist）控制机器人底盘。

其设计核心是 **“解耦与插件化”**：全局规划器、局部规划器、恢复行为均通过 pluginlib 动态加载，用户可自定义替换，无需修改 move_base 源码。

二、move_base 的内部架构与模块组成

move_base 内部由以下关键模块构成，通过 ROS 话题 / 服务 / Action 与外部交互：

┌─────────────────────────────────────────────────────────────┐
│                         move_base                             │
├─────────────────────────────────────────────────────────────┤
│  ┌──────────────────┐    ┌──────────────────────────────┐  │
│  │ Action Server    │    │     状态机 (State Machine)    │  │
│  │ (接收/取消目标)  │───▶│  WAITING→PLANNING→CONTROLLING│  │
│  └──────────────────┘    │          →RECOVERING          │  │
│                           └──────────────────────────────┘  │
│  ┌──────────────────┐    ┌──────────────────────────────┐  │
│  │ 全局规划器接口   │───▶│  插件: GlobalPlanner/Navfn   │  │
│  │ (nav_core接口)   │    │  输出: 全局路径 (Path)        │  │
│  └──────────────────┘    └──────────────────────────────┘  │
│  ┌──────────────────┐    ┌──────────────────────────────┐  │
│  │ 局部规划器接口   │───▶│  插件: DWA/TEB/EBand         │  │
│  │ (nav_core接口)   │    │  输出: 速度指令 (Twist)       │  │
│  └──────────────────┘    └──────────────────────────────┘  │
│  ┌──────────────────┐    ┌──────────────────────────────┐  │
│  │ 代价地图管理器   │───▶│  global_costmap + local_costmap│  │
│  │ (Costmap2DROS)   │    │  输入: 激光/点云/里程计       │  │
│  └──────────────────┘    └──────────────────────────────┘  │
│  ┌──────────────────┐    ┌──────────────────────────────┐  │
│  │ 恢复行为管理器   │───▶│  插件: ClearCostmap/Rotate    │  │
│  │ (Recovery Logic) │    │  触发: 规划失败/振荡/超时      │  │
│  └──────────────────┘    └──────────────────────────────┘  │
└─────────────────────────────────────────────────────────────┘

1. Action 接口：目标的接收与反馈

move_base 对外提供 move_base_msgs/MoveBaseAction 接口，这是导航的 “入口”：

• 目标（Goal）：move_base_msgs/MoveBaseGoal，包含目标位姿（geometry_msgs/PoseStamped，需指定坐标系，通常为 map）。
• 反馈（Feedback）：move_base_msgs/MoveBaseFeedback，实时返回机器人当前位姿（base_position）。
• 结果（Result）：move_base_msgs/MoveBaseResult，导航结束时返回（成功 / 失败 / 被取消）。

用户可通过以下方式发送目标：

• RViz 中的 “2D Nav Goal” 按钮（本质是发布到 /move_base_simple/goal 话题，move_base 内部会将其转换为 Action 目标）；
• 代码中调用 actionlib::SimpleActionClient 发送 Action 目标；
• 直接发布 /move_base/goal 话题（不推荐，无法处理取消和反馈）。

2. 状态机：导航的核心逻辑

move_base 内部通过状态机管理导航流程，主要状态及转换如下：

状态	含义	触发转换的条件
WAITING	等待目标，空闲状态	收到新目标 → PLANNING；目标被取消 → WAITING
PLANNING	调用全局规划器生成全局路径	规划成功 → CONTROLLING；规划失败 → RECOVERING（或重试）
CONTROLLING	调用局部规划器，根据全局路径和局部代价地图输出速度指令	到达目标 → WAITING（成功）；局部规划失败 → RECOVERING；机器人振荡 → RECOVERING
RECOVERING	执行恢复行为（如清除代价地图、原地旋转），尝试解决问题	恢复成功 → PLANNING；恢复失败 → WAITING（目标失败）

3. 全局规划器：找 “全局最优路径”

move_base 通过 nav_core::BaseGlobalPlanner 接口加载全局规划器插件，负责在静态地图上规划从起点到目标点的 “粗路径”。

• 常用插件：
  • global_planner/GlobalPlanner：支持 A* 和 Dijkstra 算法（推荐，灵活度高）；
  • navfn/NavfnROS：基于 Dijkstra 算法的经典实现（ROS 早期默认）；
  • carrot_planner/CarrotPlanner：简单的 “跟随目标” 规划器（用于调试，不实用）。
• 输入：机器人当前位姿（来自定位，如 AMCL）、目标位姿、全局代价地图；
• 输出：全局路径（nav_msgs/Path，话题为 /move_base/NavfnROS/plan 或 /move_base/GlobalPlanner/plan）。

关键逻辑：

• 全局规划器会先检查目标点是否 “有效”：是否在地图内、是否在障碍物中（通过全局代价地图判断）；
• 若目标点在障碍物中，会尝试在目标点周围寻找 “最近的可通行点”（通过 make_plan_service 或规划器内部逻辑）；
• 全局路径的频率由 planner_frequency 参数控制（若设为 0，则仅在收到新目标或恢复后规划一次；若设为 1.0，则每秒重新规划一次）。

4. 局部规划器：管 “脚下避障与轨迹跟踪”

局部规划器通过 nav_core::BaseLocalPlanner 接口加载插件，负责将全局路径转换为实时速度指令，核心是 “轨迹跟踪 + 局部避障”。

• 常用插件：
  • dwa_local_planner/DWAPlannerROS：动态窗口法（差速机器人首选，计算快）；
  • teb_local_planner/TebLocalPlannerROS：时间弹性带（全向机器人 / 复杂环境首选，轨迹平滑）；
  • eband_local_planner/EBandPlannerROS：弹性带算法（TEB 的前身）。
• 输入：全局路径、局部代价地图、机器人里程计（nav_msgs/Odometry）；
• 输出：速度指令（geometry_msgs/Twist，话题为 /cmd_vel，直接控制底盘）。

关键逻辑：

• 局部规划器会先对全局路径进行 “裁剪”，只保留机器人前方的一段局部路径作为参考；
• 结合局部代价地图（实时检测动态障碍物），通过算法（如 DWA 的速度采样、TEB 的轨迹优化）生成最优速度；
• 控制频率由 controller_frequency 参数控制（通常为 10~20Hz，保证实时性）；
• 若局部规划器连续失败（如 controller_patience 时间内未找到有效速度），则触发恢复行为。

5. 代价地图管理器：环境感知的 “眼睛”

move_base 维护两个独立的代价地图（costmap_2d::Costmap2DROS 类），为规划器提供 “可通行区域” 的表示：

代价地图	用途	特点
global_costmap	为全局规划器提供静态环境信息	范围大（通常覆盖整个地图），更新频率低（如 1~2Hz），包含静态层 + 膨胀层
local_costmap	为局部规划器提供实时动态环境信息	范围小（如机器人周围 5m×5m），更新频率高（如 5~10Hz），包含障碍物层 + 膨胀层

代价地图的分层结构（通过 costmap_common_params.yaml 配置）：

1. 静态层（Static Layer）：加载 SLAM 构建的静态地图（.pgm+.yaml），标记固定障碍物；
2. 障碍物层（Obstacle Layer）：订阅激光雷达 / 深度相机数据（sensor_msgs/LaserScan 或 sensor_msgs/PointCloud2），实时标记动态障碍物；
3. 膨胀层（Inflation Layer）：在障碍物周围膨胀出 “安全区域”（inflation_radius），防止机器人碰撞；
4. 其他层（可选）：如 voxel_layer（3D 障碍物层）、range_sensor_layer（超声传感器层）。

6. 恢复行为管理器：处理异常的 “安全网”

当导航失败时（如全局规划找不到路径、局部规划连续失败、机器人振荡），move_base 会触发预设的恢复行为序列，尝试解决问题。

• 默认恢复行为顺序（可通过参数修改）：
  1. 清除代价地图（ClearCostmapRecovery）：重置 global_costmap 和 local_costmap，清除所有动态障碍物标记；
  2. 原地旋转（RotateRecovery）：机器人原地旋转 360°，通过激光雷达重新扫描环境，更新代价地图；
  3. 更激进的清除：若前两步失败，可配置 “清除更大范围的代价地图” 或 “多次旋转”。
• 自定义恢复行为：通过 recovery_behaviors 参数配置，例如添加 move_slow_and_clear/MoveSlowAndClear（缓慢前进并清除代价地图）。

关键参数：

• recovery_behavior_enabled：是否启用恢复行为（默认 true）；
• max_planning_retries：全局规划失败后的重试次数（默认 -1，无限重试）；
• controller_patience：局部规划失败后等待的时间（默认 15.0s，超过则触发恢复）。

三、move_base 的关键参数配置

move_base 的参数分为 “自身参数” 和 “插件参数”（全局规划器、局部规划器、代价地图的参数在各自配置文件中）。以下是 move_base 自身的核心参数（在 move_base.launch 或 move_base_params.yaml 中配置）：

# 1. 插件类型配置
base_global_planner: "global_planner/GlobalPlanner"  # 全局规划器插件
base_local_planner: "dwa_local_planner/DWAPlannerROS" # 局部规划器插件

# 2. 频率与超时配置
controller_frequency: 10.0      # 局部规划器控制频率（Hz，通常10~20）
planner_frequency: 0.0          # 全局规划器重新规划频率（0=仅规划一次）
planner_patience: 5.0           # 全局规划失败后等待时间（s）
controller_patience: 15.0       # 局部规划失败后等待时间（s）

# 3. 目标容差配置
xy_goal_tolerance: 0.10         # 目标位置容差（m，机器人到达此范围内算位置到达）
yaw_goal_tolerance: 0.10        # 目标角度容差（rad，约5.7度）

# 4. 振荡检测配置
oscillation_timeout: 10.0        # 振荡超时时间（s，机器人在小范围内移动超过此时间算振荡）
oscillation_distance: 0.5        # 振荡距离阈值（m，判断振荡的移动范围）

# 5. 恢复行为配置
recovery_behavior_enabled: true  # 启用恢复行为
max_planning_retries: -1         # 全局规划最大重试次数（-1=无限）
# 自定义恢复行为序列（示例）
recovery_behaviors:
  - name: "clear_costmap_recovery"
    type: "clear_costmap_recovery/ClearCostmapRecovery"
  - name: "rotate_recovery"
    type: "rotate_recovery/RotateRecovery"
  - name: "move_slow_and_clear"
    type: "move_slow_and_clear/MoveSlowAndClear"

四、move_base 的调试与常见问题分析

调试 move_base 的核心是 **“可视化 + 话题监控 + 参数调优”**，以下是关键方法：

1. 必备调试工具与话题

• RViz 可视化：添加以下显示项，直观观察导航状态：
  • Map：显示静态地图；
  • Path：显示全局路径（话题 /move_base/GlobalPlanner/plan）和局部轨迹（话题 /move_base/DWAPlannerROS/local_plan）；
  • PoseArray：显示 AMCL 的粒子群（话题 /particlecloud，观察定位是否收敛）；
  • Grid Cells：显示代价地图的障碍物（话题 /move_base/local_costmap/obstacles）和膨胀区域（话题 /move_base/local_costmap/inflated_obstacles）；
  • Interactive Markers：用于动态调整目标点（需安装 rviz_interactive_markers）。
• 话题监控：用 rostopic echo 或 rqt_topic 查看关键话题：
  • /move_base/feedback：查看机器人当前位姿和距离目标的距离；
  • /move_base/result：查看导航结束的原因（成功 / 失败 / 被取消）；
  • /cmd_vel：查看局部规划器输出的速度指令（若速度为 0，说明局部规划失败）；
  • /move_base/current_goal：查看当前目标点（若目标点频繁变化，说明全局规划器在重新规划）。
• Action 监控：用 rqt_action 查看 move_base 的 Action 状态（目标是否被接收、当前状态、反馈数据）。

2. 常见问题与解决方案

问题现象	可能原因	解决方法
move_base 一直处于 PLANNING 状态，无全局路径输出	目标点在障碍物内；全局代价地图未正确加载；全局规划器参数不合理	1. 在 RViz 中查看目标点是否在代价地图的障碍物区域；2. 检查 global_costmap_params.yaml 中的 static_map 参数是否为 true；3. 增大全局规划器的 tolerance 参数（如 global_planner 的 tolerance）。
机器人到达目标附近但不停下来	xy_goal_tolerance 或 yaw_goal_tolerance 设置太小；局部规划器的目标权重不够	1. 增大容差参数（如 xy_goal_tolerance 设为 0.2m）；2. 增大局部规划器的 goal_distance_bias（如 DWA 的 goal_distance_bias）。
机器人频繁触发恢复行为（原地旋转 / 清除代价地图）	局部代价地图更新不及时；膨胀半径太小；局部规划器的障碍物权重不够	1. 提高局部代价地图的更新频率（local_costmap_params.yaml 中的 update_frequency）；2. 增大 inflation_radius（如从 0.3m 改为 0.5m）；3. 增大局部规划器的 occdist_scale（如 DWA 的 occdist_scale）。
机器人 “振荡”（在小范围内来回移动）	局部规划器的路径跟踪权重和速度权重不平衡；oscillation_distance 设置太小	1. 调整 DWA 的 path_distance_bias 和 goal_distance_bias（增大前者，加强路径跟踪）；2. 增大 oscillation_distance（如从 0.5m 改为 1.0m）。
全局路径规划成功，但局部规划器输出速度为 0	局部代价地图中机器人周围全是障碍物；局部规划器的速度采样范围太小

简单的move_base应用：

我们依旧使用开源的项目来做，首先科隆环境：git clone https://github.com/6-robot/wpr_simulation.git,然后进入脚本文件夹cd wpr_simulation/scripts/，执行依赖型安装：./install_for_noetic.sh,然后回到工作空间catkin_make编译一下

还需要一个机器人的驱动源码包，下载工程文件：git clone https://github.com/6-robot/wpb_home.git,然后进入脚本文件夹cd wpb_home/wpb_home_bringup/scripts/,执行依赖型安装：./install_for_noetic.sh

创建一个包去放我们写的launch文件：

<launch>
    <node pkg="move_base" type="move_base" name="move_base">
        <rosparam file="$(find wpb_home_tutorials)/nav_lidar/costmap_common_params.yaml" command="load" ns="global_costmap" />
        <rosparam file="$(find wpb_home_tutorials)/nav_lidar/costmap_common_params.yaml" command="load" ns="local_costmap" />
        <rosparam file="$(find wpb_home_tutorials)/nav_lidar/global_costmap_params.yaml" command="load" />
        <rosparam file="$(find wpb_home_tutorials)/nav_lidar/local_costmap_params.yaml" command="load" />
        <param name="base_global_planner" value="global_planner/GlobalPlanner" /> 
        <param name="base_local_planner" value="wpbh_local_planner/WpbhLocalPlanner" />
    </node>

    <node pkg="map_server" type="map_server" name="map_server" args="$(find wpr_simulation)/maps/gmap.yaml"/>
    <node pkg="amcl" type="amcl" name="amcl"/>
</launch>

运行仿真环境：roslaunch wpr_simulation wpb_stage_robocup.launch
运行新写的这个launch文件：roslaunch nav_pkg nav.launch
运行起来rviz：rosrun rviz rviz添加地图、机器人模型、再提供2D Nav Goal就可以实现定位导航了：

全局规划器

其核心职责是：基于预先构建的全局静态栅格地图，在已知环境中计算一条从机器人当前位姿到目标点的、无碰撞、符合代价约束的全局参考路径，为后续局部路径规划器（如 DWA、TEB）提供宏观导航基准，仅处理静态障碍物，动态避障由局部规划器完成。

---

一、主流全局规划器详解

ROS1 生态中最常用的 4 类全局规划器，覆盖绝大多数导航场景，以下分别介绍其核心原理、适用场景、全参数说明。

1. navfn/NavfnROS（导航栈默认规划器）

核心原理

基于Dijkstra 单源最短路径算法，采用波前传播（Wavefront Propagation）机制：从目标点出发，向全地图可通行区域传播代价势场，最终从起点沿势场下降最快的方向回溯，得到全局代价最优的路径。

适用场景

中小型室内静态地图、全向移动机器人、对路径最优性要求高、无复杂运动学约束的场景。

全参数详解

参数分类	参数名	类型	默认值	核心含义与调优建议
基础通用参数	default_tolerance	double	0.0	目标点位置容忍度（米）。调优：室内导航建议0.2-0.5m，室外1.0-2.0m，小于该值即视为到达目标，避免机器人在目标点震荡。
	allow_unknown	bool	true	是否允许路径经过地图的未知区域（栅格值为 - 1）。调优：纯已知地图导航设为false，避免误入盲区；边建图边导航（SLAM + 导航）可设为true。
	visualize_potential	bool	false	是否发布代价势场点云用于 RViz 可视化。调优：调试时开启，可直观看到搜索范围与代价分布；正常运行关闭，节省算力。
特有规划参数	planner_window_x/planner_window_y	double	0.0	规划窗口大小（米）。调优：默认 0 为全地图规划；大地图场景设为10-20m，仅在机器人周边窗口内规划，大幅降低计算开销。
	use_final_approach_orientation	bool	false	是否强制机器人最终路径的末端朝向与目标点朝向一致。调优：需要精准对准的场景（如充电桩、装卸货）设为true。
代价阈值参数	lethal_cost	int	253	致命代价阈值，栅格代价≥该值视为完全不可通行。⚠️ 必须与全局代价地图的lethal_cost_threshold完全一致，否则会出现路径穿模或规划失败。
	cost_factor	double	3.0	地图代价权重，公式：规划代价 = cost_factor * 地图栅格代价 + neutral_cost。调优：值越大，路径越倾向于远离障碍物，越保守；值越小，路径越短，越贴近障碍物。
	neutral_cost	int	50	中性代价基准值，用于平衡自由空间的基础代价。调优：配合cost_factor调整，一般无需修改，仅在路径过于保守 / 激进时微调。

---

2. global_planner/GlobalPlanner（最常用增强版）

核心原理

navfn 的重构增强版，是目前 ROS1 导航的首选规划器。原生支持Dijkstra 与 A * 算法（参数一键切换），优化了搜索效率，支持二次 B 样条路径平滑，可灵活切换网格路径 / 连续路径，灵活性、计算速度远超 navfn。

• Dijkstra：保证全局最优解，搜索范围大，适合小地图；
• A*：引入欧氏距离启发函数，大幅缩小搜索范围，大地图下速度提升 3-10 倍，是绝大多数场景的首选。

适用场景

全尺寸地图（室内 / 室外）、对规划速度要求高、需要灵活调整路径特性、差分 / 全向机器人通用场景。

全参数详解

参数分类	参数名	类型	默认值	核心含义与调优建议
基础通用参数	default_tolerance	double	0.0	同 navfn，目标点容忍度，调优逻辑一致。
	allow_unknown	bool	true	同 navfn，是否允许路径经过未知区域。
	visualize_potential	bool	false	同 navfn，势场可视化开关。
算法核心参数	use_dijkstra	bool	true	算法切换开关。⚠️ 设为false启用 A * 算法，大地图场景必开，大幅提升规划速度。
	use_quadratic	bool	true	是否开启二次 B 样条平滑。调优：开启后路径更平滑，无锯齿；关闭后用线性插值，路径更锐利，适合简单网格场景。
	use_grid_path	bool	false	是否沿栅格中心线生成路径。调优：开启后生成多拐角的网格路径，计算快但平滑性差；关闭生成连续路径，是默认最优选择。
	old_navfn_behavior	bool	false	是否兼容旧版 navfn 的代价计算逻辑。调优：仅在从 navfn 迁移时需要兼容，否则保持false。
代价阈值参数	lethal_cost	int	253	同 navfn，必须与全局代价地图的致命代价阈值完全一致。
	cost_factor	double	3.0	同 navfn，地图代价权重，调整路径远离障碍物的程度。
	neutral_cost	int	50	同 navfn，自由空间基础代价基准。

---

3. carrot_planner/CarrotPlanner（轻量型目标适配规划器）

核心原理

极简轻量规划器，不做复杂的全局路径搜索，核心逻辑：

1. 检查目标点是否在可通行区域，若是，直接生成起点到目标点的直线路径；
2. 若目标点被障碍物占据（不可达），则沿起点 - 目标点的连线，向起点方向回溯，找到最近的可通行 “替代目标点”（胡萝卜点），生成到该点的直线路径。

适用场景

目标点频繁落在障碍物内的场景（如用户 RViz 手动点选目标）、开阔无复杂障碍的环境、简单定点导航场景，完美解决 “目标点不可达导致规划失败” 的问题。

全参数详解

参数名	类型	默认值	核心含义与调优建议
step_size	double	0.02	回溯搜索的步长（米）。调优：值越小，找到的替代目标点越精准，但计算开销略高；室内建议0.02-0.05m。
min_dist_from_robot	double	0.1	替代目标点与机器人的最小距离（米）。调优：避免替代点离机器人过近，导致规划无意义，一般设为机器人的外接半径。
inscribed_radius	double	0.0	机器人内切半径（米）。⚠️ 必须与全局代价地图的机器人内切半径一致，用于判断栅格是否可通行。
circumscribed_radius	double	0.0	机器人外接半径（米）。⚠️ 必须与全局代价地图的机器人外接半径一致。

---

4. sbpl_lattice_planner/SBPLLatticePlanner（非完整约束专用规划器）

核心原理

基于 SBPL（Search-Based Planning Library）搜索库，采用状态格（State Lattice） 搜索算法，原生支持机器人的非完整运动学约束（如差分驱动、阿克曼转向机器人，无法原地转向、横向移动），生成的路径天生符合机器人的运动能力，无需局部规划器做大量运动学修正。支持 ARA*、AD * 等启发式算法，兼顾最优性与计算速度。

适用场景

非完整约束的移动机器人（如无人车、差分 AGV）、室外长距离导航、对路径的运动学可行性要求极高的场景。

核心参数详解

参数名	类型	默认值	核心含义与调优建议
planner_type	string	"ARAPlanner"	规划算法类型。可选：ARAPlanner（随时最优 ARA*，平衡速度与最优性）、ADPlanner（动态 AD*，适合环境变化的场景）。
allocated_time	double	1.0	单次规划的最大时间上限（秒）。调优：大地图可设为2.0-3.0s，保证找到可行路径；实时性要求高的场景设为0.5s。
motion_primitive_file	string	""	运动基元文件的绝对路径。⚠️ 核心参数，必须与机器人的运动学模型完全匹配（差分 / 阿克曼），否则生成的路径不符合机器人运动能力。
cost_penalty	double	1.0	障碍物代价惩罚系数。调优：值越大，路径越远离障碍物，越保守。
lethal_obstacle_cost	int	253	致命障碍物代价阈值，必须与全局代价地图一致。
initial_epsilon	double	3.0	启发式搜索的初始膨胀系数。调优：值越大，规划速度越快，路径最优性越差；值越小，路径越优，速度越慢。

---

二、通用使用注意事项与避坑指南

1. 插件切换的正确操作

规划器是插件式设计，必须完成以下两步才能生效，否则会默认使用 navfn：

1. 安装对应规划器包（以 Noetic 为例）：

   # 基础导航包
   sudo apt install ros-noetic-navigation
   # 对应规划器包
   sudo apt install ros-noetic-global-planner ros-noetic-carrot-planner ros-noetic-sbpl-lattice-planner
   

2. 在move_base的 launch 文件中显式指定规划器：

   <node pkg="move_base" type="move_base" name="move_base" output="screen">
       <!-- 切换为GlobalPlanner，其他规划器替换对应值即可 -->
       <param name="base_global_planner" value="global_planner/GlobalPlanner" />
       <!-- 加载规划器参数文件 -->
       <rosparam file="$(find your_pkg)/config/global_planner_params.yaml" command="load" />
   </node>
   

2. 与全局代价地图的参数联动（最高优先级避坑）

全局规划器的所有代价阈值参数，必须与全局代价地图的参数完全一致，否则会出现致命问题：

• 必须一致的参数：lethal_cost/lethal_cost_threshold、inscribed_radius、circumscribed_radius、inflation_radius；
• 常见坑：代价地图的膨胀半径设的过大，导致可通行区域被完全遮挡，规划器无法找到路径；
• 联动调优：cost_factor与inflation_radius配合，膨胀半径决定障碍物的影响范围，cost_factor决定路径对障碍物的规避程度。

3. 规划频率与算力控制

• move_base的planner_frequency：全局规划器的运行频率，默认 0 为事件触发式规划（仅当目标点更新、地图更新、机器人偏离路径过远时重规划），是最优选择；
• 调优建议：不要设为高频（如 > 1Hz），否则大地图下会导致 CPU 占用过高，导航卡顿；最高建议不超过 1Hz，仅在环境频繁变化时微调。
• planner_patience：规划失败后的重试等待时间，默认 5.0s，不要设的过短，避免频繁触发规划失败。

4. 算法选择的核心原则

场景	首选规划器	核心配置
室内小地图、全向机器人、要最优路径	navfn/NavfnROS	默认配置，调整default_tolerance与allow_unknown
大地图、室内外通用、要速度与灵活性	global_planner/GlobalPlanner	关闭use_dijkstra启用 A*，开启use_quadratic平滑
目标点频繁不可达、开阔环境	carrot_planner/CarrotPlanner	配置好机器人内切 / 外接半径，调整step_size
无人车 / 差分 AGV、非完整约束	sbpl_lattice_planner/SBPLLatticePlanner	匹配对应运动基元文件，选择 ARA * 算法

5. 调试与故障排查核心技巧

1. 可视化调试：开启visualize_potential，在 RViz 中添加PointCloud2话题（如/move_base/GlobalPlanner/potential），直观看到规划器的搜索范围与代价分布；添加Path话题（/move_base/GlobalPlanner/plan）查看生成的全局路径。
2. 动态调参：使用rosrun rqt_reconfigure rqt_reconfigure，实时调整规划器的cost_factor、default_tolerance等参数，无需重启节点，大幅提升调参效率。
3. 常见故障排查：

   故障现象	核心排查方向
   提示 “无法找到路径”，规划失败	1. 目标点 / 机器人起点是否在障碍物内；2. allow_unknown设为 false 但路径需要经过未知区域；3. 代价地图膨胀半径过大，遮挡可通行区域；4. lethal_cost与代价地图不一致
   路径穿模，穿过障碍物	1. lethal_cost设的过高，把障碍物识别为可通行；2. 全局代价地图未正常更新；3. 机器人 TF 变换异常，定位飘移；4. 机器人 footprint 配置错误
   规划速度慢，CPU 占用高	1. 大地图下未启用 A * 算法；2. planner_frequency设的过高；3. 未限制规划窗口，全地图高频规划；4. 开启了不必要的可视化
   机器人到目标点附近震荡	1. default_tolerance设的过小；2. 目标点朝向约束过严；3. 全局路径与局部规划器跟踪参数不匹配

6. 其他关键注意事项

• 全局规划器仅处理静态地图中的静态障碍物，无法规避动态障碍物，动态避障完全由局部路径规划器负责；
• 必须保证map→odom→base_link的 TF 变换链路正常，否则规划的路径会出现严重偏移；
• 全局路径的平滑仅为预处理，最终的平滑跟踪、运动学约束适配，必须由局部规划器完成，不要指望全局规划器生成完美符合机器人运动能力的路径；
• 机器人的footprint（轮廓）必须在代价地图中正确配置，否则规划器会错误判断可通行区域，导致机器人卡住或路径不合理。

---

三、可直接复用的配置模板

GlobalPlanner 最优配置模板（global_planner_params.yaml）

# 算法核心配置
use_dijkstra: false          # 启用A*算法，大地图必开
use_quadratic: true          # 开启二次平滑，路径更顺滑
use_grid_path: false         # 生成连续路径，而非网格路径
old_navfn_behavior: false    # 不兼容旧版navfn

# 代价与阈值配置
cost_factor: 3.0             # 障碍物规避权重，值越大越保守
neutral_cost: 50             # 自由空间基础代价
lethal_cost: 253             # 致命代价，与全局代价地图一致
allow_unknown: false         # 不允许进入未知区域，纯已知地图导航用

# 基础配置
default_tolerance: 0.3       # 目标点容忍度0.3米，室内通用
visualize_potential: false   # 调试时改为true开启可视化

move_base launch 配套配置

<launch>
    <node pkg="move_base" type="move_base" respawn="false" name="move_base" output="screen">
        <!-- 全局规划器指定 -->
        <param name="base_global_planner" value="global_planner/GlobalPlanner" />
        <!-- 局部规划器（示例，可替换为TEB） -->
        <param name="base_local_planner" value="dwa_local_planner/DWAPlannerROS" />
        
        <!-- 加载参数文件 -->
        <rosparam file="$(find your_pkg)/config/global_planner_params.yaml" command="load" />
        <rosparam file="$(find your_pkg)/config/global_costmap_params.yaml" command="load" />
        <rosparam file="$(find your_pkg)/config/local_costmap_params.yaml" command="load" />
        <rosparam file="$(find your_pkg)/config/dwa_local_planner_params.yaml" command="load" />

        <!-- 规划频率与超时配置 -->
        <param name="planner_frequency" value="0.5" />
        <param name="planner_patience" value="5.0" />
        <param name="controller_frequency" value="10.0" />
        <param name="controller_patience" value="3.0" />
    </node>
</launch>

AMCL

一、AMCL 核心原理

1. 核心逻辑：自适应粒子滤波

AMCL 的本质是带 KLD 自适应采样的蒙特卡洛粒子滤波，核心思想是用一组 “粒子” 代表机器人位姿的概率假设，通过预测 - 更新 - 重采样的循环，不断收敛到机器人的真实位姿。

• 粒子：每个粒子对应一个「机器人可能的位姿 (x,y,θ)」，粒子的权重代表该位姿与真实环境的匹配度。
• 自适应：通过 KLD（Kullback-Leibler Divergence）采样，根据位姿的不确定性自动调整粒子数量：定位收敛时粒子数少（节省算力），定位不确定时粒子数多（提升鲁棒性），平衡精度与算力消耗。

2. 算法核心循环步骤

1. 初始化：根据初始位姿与不确定性，在地图中生成初始粒子集。
2. 运动模型预测：根据里程计的运动增量，给所有粒子叠加噪声，预测机器人运动后的新位姿。
3. 观测模型更新：将每个粒子的激光扫描数据与地图匹配，匹配度越高，粒子权重越大。
4. 重采样：淘汰权重低的粒子，复制权重高的粒子，保持粒子总数的动态平衡，避免粒子退化。
5. 位姿输出：所有粒子的加权平均，即为机器人的最优位姿估计，同时发布map→odom的 TF 变换。
6. 自适应调整：通过 KLD 采样，根据位姿分布的分散程度，动态调整下一轮的粒子数量。

3. 两个核心模型（参数设计的基础）

• 运动模型：描述里程计的误差特性，核心是量化里程计打滑、编码器误差带来的位姿不确定性，对应里程计噪声参数。
• 观测模型：描述激光雷达的匹配特性，核心是量化激光点与地图障碍物的匹配误差，对应激光匹配参数。

---

二、AMCL 全参数详解（含影响与通用设定）

AMCL 的参数可分为 6 大类，按调优优先级排序，每个参数均明确设定影响与通用推荐值，覆盖 99% 的室内外导航场景。

1. 基础 TF 与全局配置参数（优先级最高，错了直接无法运行）

这是 AMCL 运行的前提，必须与机器人 URDF、导航栈其他节点（move_base、costmap）的配置完全一致。

参数名	类型	默认值	设定影响	通用推荐值
global_frame_id	string	map	全局固定坐标系，AMCL 定位的参考系，必须与全局代价地图一致。	保持默认map，无特殊需求不修改
odom_frame_id	string	odom	里程计坐标系，必须与底盘发布的里程计帧一致。AMCL 的核心输出是map→odom的 TF 变换，用于修正里程计的累计误差。	保持默认odom，仅当里程计帧名修改时同步修改
base_frame_id	string	base_link	机器人基坐标系，必须与 URDF 中机器人的中心帧一致，代表机器人本体。	差分 / 全向机器人常用base_link或base_footprint，与 URDF 严格匹配
tf_broadcast	bool	true	是否发布map→odom的 TF 变换。	导航场景必须设为true，仅离线调试时可关闭
tf_publish_rate	double	10.0	TF 变换的发布频率（Hz）。	与导航控制器频率一致，通用 10-20Hz，高速机器人可设 20Hz
laser_topic	string	scan	激光雷达数据的话题名，必须与激光驱动发布的话题一致。	与激光驱动话题匹配，常用scan
odom_topic	string	odom	里程计数据的话题名，必须与底盘发布的话题一致。	与底盘里程计话题匹配，常用odom

2. 初始位姿与不确定性参数

控制机器人上电时的初始粒子分布，直接影响初始定位的收敛速度与成功率。

参数名	类型	默认值	设定影响	通用推荐值
initial_pose_x/y/a	double	0.0	机器人上电时的初始位姿（x 坐标、y 坐标、航向角 θ）。	机器人上电位置固定时，设为实际上电位姿；位置不固定时保持 0，启动后通过 RViz 的2D Pose Estimate手动给定
initial_cov_xx/yy	double	0.25	初始位姿 x/y 方向的协方差，代表位置的初始不确定性。→ 设得越大，初始粒子分布越分散，越容易覆盖真实位置，收敛所需粒子越多、时间越长；→ 设得越小，初始粒子越集中，收敛越快，但真实位置与初始位姿偏差大时，会直接收敛到错误位置。	上电位置固定：0.01-0.1；上电位置不固定 / 全局定位：1.0-4.0；大场景仓库：4.0-9.0
initial_cov_aa	double	0.0685	初始航向角的协方差，默认对应 ±15° 的不确定性。	上电航向固定：0.01；航向不固定：0.1-0.5（对应 ±30°~±60°）
save_pose_rate	double	0.5	自动保存机器人最后位姿的频率（Hz），保存到参数服务器，下次启动自动加载为初始位姿。	保持默认0.5，无需自动保存时设为0

3. 粒子滤波核心参数

控制粒子数量的上下限，直接平衡定位鲁棒性与 CPU 算力消耗。

参数名	类型	默认值	设定影响	通用推荐值
min_particles	int	100	自适应粒子数的下限，定位收敛时的最少粒子数。→ 设得越小，算力占用越低，但极端场景下容易因粒子不足丢失定位；→ 设得越大，最低算力占用越高，鲁棒性越强。	室内小场景 / 嵌入式设备：50-100；大场景 / 动态环境：100-200；算力极度紧张的树莓派：30-50
max_particles	int	5000	自适应粒子数的上限，定位不确定时的最多粒子数。→ 设得越大，初始定位、机器人打滑 / 被撞后的收敛能力越强，鲁棒性越好，但 CPU 占用会飙升；→ 设得越小，算力上限越低，极端场景容易定位丢失。	室内小场景：500-1000；大场景 / 仓库：2000-3000；算力充足的 X86 设备：5000；嵌入式设备：不超过2000

4. 运动模型参数（里程计噪声参数）

描述里程计的误差特性，核心是量化对里程计的信任度，是定位不飘、不丢失的关键。AMCL 支持 4 种运动模型，对应不同的机器人底盘类型。

4.1 运动模型类型选择

模型类型	适用场景	通用推荐
diff	差分驱动 / 履带式机器人（不能横向移动），默认模型	差分机器人优先用diff-corrected（修正版，精度更高）
diff-corrected	修正后的差分驱动模型，优化了里程计误差计算，精度更高	差分机器人通用首选
omni	全向轮 / 麦克纳姆轮机器人（可横向移动）	全向机器人优先用omni-corrected
omni-corrected	修正后的全向驱动模型，精度更高	全向机器人通用首选

4.2 里程计噪声核心参数（alpha1~alpha5）

所有 alpha 参数的核心逻辑：值越大，代表对里程计的信任度越低，给粒子叠加的噪声越大，抗打滑 / 抗误差能力越强，但定位精度会下降；值越小，对里程计信任度越高，定位精度越高，但里程计误差大时，粒子会快速偏离真实位置，导致定位丢失。

参数名	类型	默认值	误差含义	设定影响	通用推荐值
alpha1	double	0.2	旋转运动引起的旋转噪声（机器人转得越多，旋转误差越大）	调大：原地旋转时抗里程计误差能力强；调小：旋转时航向精度高	里程计精准：0.05-0.1；里程计一般 / 易打滑：0.1-0.2；原地旋转频繁：0.2-0.3
alpha2	double	0.2	平移运动引起的旋转噪声（机器人走得越远，航向跑偏误差越大）	调大：长距离行走时抗航向跑偏能力强；调小：长距离行走航向精度高	差分机器人：0.1-0.3；全向机器人：0.2-0.4；里程计精准：0.05-0.1
alpha3	double	0.2	平移运动引起的平移噪声（机器人走得越远，位置误差越大）	调大：平移时抗轮子打滑能力强；调小：平移时位置精度高	地面光滑 / 易打滑：0.2-0.4；地面摩擦力大 / 里程计准：0.05-0.1
alpha4	double	0.2	旋转运动引起的平移噪声（机器人转得越多，位置偏移误差越大）	调大：原地旋转后抗位置偏移能力强；调小：旋转后位置精度高	通用：0.1-0.3；原地旋转频繁：0.3-0.4
alpha5	double	0.2	全向机器人横向平移的噪声（仅 omni/omni-corrected 模型生效）	调大：横向移动时抗打滑能力强；调小：横向移动精度高	麦克纳姆轮机器人：0.2-0.4；全向轮机器人：0.1-0.2

5. 观测模型参数（激光匹配参数）

决定激光雷达与地图的匹配逻辑，是定位精度的核心，90% 的定位飘、定位不准问题都源于该组参数的错误配置。

5.1 观测模型类型选择

模型类型	适用场景	通用推荐
likelihood_field	似然场模型，默认值，计算速度快、抗动态障碍物能力强、精度高，适配室内结构化环境（墙、直角等特征）	99% 的场景通用首选，无需修改
beam	光束模型，基于射线追踪，计算慢、易受动态障碍物 / 镜面反射影响，仅适用于极度空旷无特征的环境	不推荐使用

5.2 似然场模型核心参数

参数名	类型	默认值	设定影响	通用推荐值
laser_min_range	double	-1.0	激光最小测距范围，小于该值的激光点会被忽略。-1.0表示使用激光雷达的原生最小量程。	设为激光雷达的实际最小测距，如 RPLIDAR A1 设为0.15，避免激光扫到机器人自身机身的点干扰匹配
laser_max_range	double	-1.0	激光最大测距范围，大于该值的激光点会被忽略。-1.0表示使用激光雷达的原生最大量程。	室内场景：10.0-15.0（太远的点噪声大，反而影响匹配）；室外大场景：20.0-30.0（匹配激光最大量程）
laser_max_beams	int	30	每次匹配使用的激光束数量，会对激光点均匀采样。→ 设得越大，匹配精度越高，计算量越大；→ 设得越小，计算速度越快，精度越低。	通用场景：30-60；嵌入式低算力设备：20-30；算力充足 / 高精度需求：60-100（超过 100 提升极小）
likelihood_field_sigma	double	0.2	【核心中的核心】似然场标准差，代表激光点与地图障碍物的匹配容错率，即对激光雷达的信任度。→ 设得越大，匹配容错率越高，抗激光噪声 / 地图误差能力越强，但定位精度下降，容易飘；→ 设得越小，只有激光与地图完全贴合的粒子才能获得高权重，定位精度极高，但抗干扰能力差，轻微噪声就会导致定位丢失。	激光精准 / 地图精度高：0.1-0.15；室内通用场景：0.2；低成本激光 / 地图有误差：0.25-0.3；⚠️ 绝对不要超过0.5，否则定位会完全失效
likelihood_field_max_dist	double	2.0	激光点与地图最近障碍物的最大匹配距离，超过该值的激光点视为无效，不参与匹配。	室内场景：2.0；大场景 / 长距离激光：3.0
max_obstacle_height	double	2.0	激光点的最大有效高度，超过该值的点会被忽略，避免天花板、吊灯等干扰。	设为「激光安装高度 + 0.5~1.0m」，如激光装在 0.3m 高，设为0.8-1.0，室内场景不要设太高
min_obstacle_height	double	0.0	激光点的最小有效高度，低于该值的点会被忽略，避免地面点干扰。	设为「激光安装高度 - 0.1m」，如激光装在 0.3m 高，设为0.2

6. 重采样、自适应与恢复机制参数

控制 AMCL 的更新频率、自适应逻辑与定位丢失后的恢复能力，决定了定位的鲁棒性与稳定性。

参数名	类型	默认值	设定影响	通用推荐值
update_min_d	double	0.2	机器人平移超过该距离（米），才会执行一次滤波更新（预测 + 匹配）。→ 设得越大，更新频率越低，CPU 占用越低，定位滞后越严重；→ 设得越小，更新越频繁，定位越实时，CPU 占用越高。	低速机器人（<0.5m/s）：0.1-0.2；高速机器人：0.05-0.1；嵌入式低算力设备：0.2-0.3
update_min_a	double	0.523	机器人旋转超过该角度（弧度，默认 30°），才会执行一次滤波更新。	通用场景：0.174-0.523（10°~30°）；精准转向需求：0.174（10°）
resample_interval	int	2	每多少次滤波更新，执行一次重采样。→ 设得越大，重采样频率越低，粒子多样性保持越好，不易退化，但收敛速度慢；→ 设得越小，重采样越频繁，收敛越快，但容易粒子退化（粒子全相同，出错后无法恢复）。	通用场景：2-3，⚠️ 不要设为 1，极易导致粒子退化
kld_err	double	0.01	KLD 自适应采样的最大允许位姿误差。→ 设得越大，粒子数越少，CPU 占用越低，位姿误差越大；→ 设得越小，粒子数越多，位姿越精准，CPU 占用越高。	通用场景：0.01；嵌入式低算力设备：0.05
kld_z	double	0.99	KLD 采样的置信度，代表位姿误差小于kld_err的概率。	通用场景保持0.99，算力极度紧张可降至0.95
recovery_alpha_slow	double	0.001	慢恢复系数，用于应对长期定位漂移的场景。当粒子平均权重持续偏低时，缓慢注入随机粒子，恢复定位。→ 设得越大，恢复越快，抗长期漂移能力越强，但容易误注入粒子导致定位飘；→ 设得越小，定位越稳定，但丢失后难以恢复。	稳定室内场景：0.001；动态环境多 / 易漂移：0.005-0.01
recovery_alpha_fast	double	0.1	快恢复系数，用于应对突发定位失效（机器人被撞、打滑、被抱起）。当粒子平均权重突然暴跌时，快速注入随机粒子。→ 设得越大，突发场景恢复能力越强，但容易受动态障碍物干扰误触发；→ 设得越小，定位越稳定，但突发失效后难以恢复。	稳定室内场景：0.1；动态环境多 / 人多遮挡：0.2-0.5

7. 调试与可视化参数

参数名	类型	默认值	设定影响	通用推荐值
publish_pose_with_covariance	bool	true	是否发布带协方差的位姿话题amcl_pose，用于调试、记录位姿。	保持true
publish_particle_cloud	bool	false	是否发布粒子云话题particle_cloud，用于 RViz 可视化调试。	调试时开启，正常运行关闭，节省带宽与 CPU

---

三、通用场景参数模板（可直接复制使用）

1. 室内通用差分机器人模板（办公室 / 厂房，最常用）

yaml

# 基础TF配置
global_frame_id: map
odom_frame_id: odom
base_frame_id: base_link
tf_broadcast: true
tf_publish_rate: 10.0

# 初始位姿配置
initial_pose_x: 0.0
initial_pose_y: 0.0
initial_pose_a: 0.0
initial_cov_xx: 0.25
initial_cov_yy: 0.25
initial_cov_aa: 0.0685
save_pose_rate: 0.5

# 粒子数配置
min_particles: 100
max_particles: 1000

# 运动模型（差分修正版）
odom_model_type: diff-corrected
alpha1: 0.1
alpha2: 0.15
alpha3: 0.1
alpha4: 0.1

# 观测模型（似然场）
laser_model_type: likelihood_field
laser_min_range: 0.15
laser_max_range: 12.0
laser_max_beams: 40
likelihood_field_sigma: 0.15
likelihood_field_max_dist: 2.0
max_obstacle_height: 1.0
min_obstacle_height: 0.1

# 更新与重采样
update_min_d: 0.15
update_min_a: 0.2618  # 15度
resample_interval: 2

# KLD自适应
kld_err: 0.01
kld_z: 0.99

# 恢复机制
recovery_alpha_slow: 0.001
recovery_alpha_fast: 0.1

# 调试配置
publish_pose_with_covariance: true
publish_particle_cloud: false

2. 大场景仓库 / 长距离导航模板

yaml

# 仅列出修改项，其余与通用模板一致
initial_cov_xx: 4.0
initial_cov_yy: 4.0
initial_cov_aa: 0.5

min_particles: 200
max_particles: 3000

# 运动模型（应对地面打滑）
alpha1: 0.2
alpha2: 0.25
alpha3: 0.2
alpha4: 0.2

# 观测模型（大场景适配）
laser_max_range: 20.0
laser_max_beams: 60
likelihood_field_sigma: 0.2
likelihood_field_max_dist: 3.0

# 更新频率
update_min_d: 0.1
update_min_a: 0.1745  # 10度

# 恢复机制
recovery_alpha_slow: 0.005
recovery_alpha_fast: 0.2

3. 嵌入式低算力设备（树莓派）模板

yaml

# 仅列出修改项，其余与通用模板一致
min_particles: 50
max_particles: 500

# 观测模型（降低计算量）
laser_max_beams: 20
likelihood_field_sigma: 0.2
laser_max_range: 10.0

# 更新频率（降低更新次数）
update_min_d: 0.25
update_min_a: 0.349  # 20度
resample_interval: 3

# KLD适配
kld_err: 0.05
kld_z: 0.95

# TF发布频率
tf_publish_rate: 5.0

代价地图

代价地图（Costmap）是 ROS 导航栈的环境感知核心，它将 SLAM 构建的静态地图、激光雷达 / 深度相机的实时感知数据，转换为带「通行风险代价值」的栅格地图，为全局路径规划器、局部路径规划器提供明确的通行规则：哪里绝对不能走、哪里要绕开、哪里可以安全通行，直接决定了导航的安全性、合理性与成功率。

---

一、代价地图核心基础

1. 核心定义与代价值标准

代价地图本质是一张与静态地图分辨率一致的 2D 栅格图，每个栅格被赋予0-255 的代价值，值越高代表通行风险越高，规划器会优先选择代价值更低的路径。ROS 官方定义的标准代价值如下：

代价值	官方定义	实际含义
0	FREE_SPACE	完全自由的可通行区域，无任何风险，规划器优先选择
1~252	膨胀过渡区	障碍物周边的风险过渡区域，离障碍物越近，值越高
253	INSCRIBED_INFLATED_OBSTACLE	内切膨胀障碍：机器人内切圆接触该栅格，必然发生碰撞
254	LETHAL_OBSTACLE	致命障碍：机器人中心落在该栅格，直接发生碰撞
255	NO_INFORMATION	未知区域：地图中无任何信息的栅格（如未探索区域）

2. 全局代价地图 vs 局部代价地图

ROS 导航栈默认分为全局和局部两套独立的代价地图，分别服务于全局规划器和局部规划器，核心区别如下：

对比维度	全局代价地图（Global Costmap）	局部代价地图（Local Costmap）
核心作用	为全局路径规划提供全地图的静态环境基准，计算宏观无碰撞路径	为局部路径规划提供机器人周边的实时动态环境，实现动态避障与轨迹跟踪
地图范围	覆盖整个 SLAM 构建的静态地图，固定不变	以机器人为中心的滚动窗口，范围小，随机器人移动实时更新
更新频率	低（1~5Hz），仅静态地图更新时重加载	高（5~20Hz），实时同步传感器的动态障碍物数据
核心图层	静态层 + 膨胀层为主，障碍物层可选	障碍物层 + 膨胀层为主，静态层可选
滚动窗口	通常关闭（rolling_window: false）	必须开启（rolling_window: true），否则无法跟随机器人
生命周期	导航启动时加载，全程不重置	随机器人移动实时滚动重置，始终保持机器人在窗口中心

3. 代价地图的分层架构

图层名称	核心作用	数据来源	必选 / 可选
静态层（Static Layer）	加载 SLAM 构建的静态栅格地图，提供全局静态障碍物信息	/map 话题（map_server 发布）	全局地图必选，局部地图可选
障碍物层（Obstacle Layer）	实时接收传感器数据，标记动态 / 静态障碍物，同时清除自由空间	激光雷达、深度相机、超声波等	局部地图必选，全局地图可选
膨胀层（Inflation Layer）	给所有障碍物叠加安全膨胀半径，生成风险过渡区，保证机器人与障碍物的安全距离	其他图层输出的障碍物栅格	全局 + 局部地图必选
体素层（Voxel Layer）	3D 版障碍物层，将 3D 点云数据转换为 2D 代价地图，可过滤地面 / 空中无效点	深度相机、3D 激光雷达	可选，3D 传感器场景替代障碍物层
其他自定义层	如范围限制层、禁行区层、社会规则层等	自定义规则 / 话题

---

二、代价地图通用核心参数（全局 / 局部共用）

以下参数是全局和局部代价地图的基础配置，直接决定代价地图的基本运行逻辑，需在通用配置文件（如common_costmap_params.yaml）中定义。

1. 坐标系与基础框架参数

参数名	类型	默认值	设定影响	通用推荐值
global_frame	string	/map	代价地图的全局固定参考坐标系，必须与 AMCL、全局规划器的坐标系完全一致。	保持默认/map，无特殊需求不修改
robot_base_frame	string	/base_link	机器人本体的基坐标系，必须与 URDF、AMCL 的基坐标系一致，代价地图以该坐标系为中心更新。	差分 / 全向机器人常用/base_link或/base_footprint，与 URDF 严格匹配
transform_tolerance	double	0.2	TF 变换的最大容忍延迟（秒），超过该时间未收到global_frame→robot_base_frame的 TF 变换，代价地图停止更新。	通用场景0.2~0.5；嵌入式低算力设备可设为1.0，避免因 TF 延迟导致地图失效

2. 地图尺寸与分辨率参数

参数名	类型	默认值	设定影响	通用推荐值
resolution	double	0.05	单个栅格对应的实际尺寸（米 / 格），必须与 SLAM 构建的静态地图分辨率完全一致，否则会出现地图缩放、匹配错误。	室内场景通用0.05（5cm）；室外大场景可设0.1（10cm），降低计算量
width/height	int	10	代价地图的宽度 / 高度（栅格数），实际物理尺寸 = 栅格数 × 分辨率。	全局地图：与静态地图尺寸完全一致；局部地图：室内场景60~100（对应 3~5 米窗口）；室外场景100~200（对应 5~10 米窗口）
origin_x/origin_y	double	0.0	代价地图左下角的全局坐标，决定地图在global_frame中的位置。	全局地图：与静态地图的原点完全一致；局部地图：滚动窗口开启后自动失效，无需设置

3. 更新与发布频率参数

参数名	类型	默认值	设定影响	通用推荐值
update_frequency	double	5.0	代价地图的更新频率（Hz），即每秒重新计算栅格代价值的次数。→ 设得越高，地图实时性越强，CPU 占用越高；→ 设得越低，CPU 占用越低，动态障碍物更新滞后，容易撞障。	全局地图：1.0~5.0（静态地图无需高频更新）；局部地图：5.0~10.0（室内低速机器人）；10.0~20.0（高速机器人 / 动态环境多）
publish_frequency	double	0.0	代价地图话题的发布频率（Hz），用于 RViz 可视化，不影响导航逻辑。→ 设为 0.0 则不发布，节省带宽与 CPU。	调试阶段：与update_frequency一致，方便观察地图变化；正常运行：1.0或0.0，无需高频发布

4. 机器人轮廓参数（核心中的核心）

二选一配置，必须与机器人实际尺寸完全匹配，否则会出现碰撞、无法通过窄路等问题。

参数名	类型	适用场景	设定影响	通用配置规则
robot_radius	double	圆形机器人	机器人的外接圆半径（米），决定机器人的最小通行宽度。→ 设得过大，窄路无法通过；→ 设得过小，容易发生碰撞。	设为机器人实际外接圆半径 + 0.02m 余量，如半径 0.2m 的圆形机器人，设为0.22
footprint	list	不规则形状机器人（长方形、差分 AGV 等）	机器人本体的轮廓坐标，以robot_base_frame为原点，按顺时针 / 逆时针顺序排列的二维坐标点列表，单位米。→ 轮廓越精准，膨胀范围越合理，越能充分利用狭窄空间。	长方形机器人示例：[[0.25, 0.2], [0.25, -0.2], [-0.25, -0.2], [-0.25, 0.2]]（长 0.5m，宽 0.4m），坐标需覆盖机器人所有边缘

5. 核心代价阈值与基础规则参数

参数名	类型	默认值	设定影响	通用推荐值
lethal_cost_threshold	int	254	致命代价阈值，静态地图中栅格值≥该值的区域，视为致命障碍（254）。⚠️ 必须与全局规划器的lethal_cost参数完全一致，否则会出现路径穿模、规划失败。	保持默认254，无特殊需求不修改
unknown_cost_value	int	255	未知区域的代价值，静态地图中栅格值为 - 1 的区域，赋值为该值。	保持默认255，无特殊需求不修改
track_unknown_space	bool	false	是否跟踪未知区域。→ 设为true：未知区域代价值为 255，与全局规划器的allow_unknown联动，决定是否允许进入未知区域；→ 设为false：未知区域视为自由空间（0），容易导致机器人误入盲区。	全局地图：纯已知地图导航设为false；SLAM + 导航同步探索设为true；局部地图：保持false，无需跟踪未知区域
footprint_padding	double	0.01	机器人轮廓的安全余量（米），给轮廓额外增加的膨胀量，避免轮廓误差导致碰撞。	通用场景0.01~0.02，无需过大

---

三、核心图层专属参数详解

图层参数需在plugins中先声明插件，再配置对应参数，必须先声明插件，否则图层不生效。

1. 插件声明规范（必须放在参数文件最前面）

yaml

# 全局代价地图插件声明示例
plugins:
  - {name: static_layer, type: "costmap_2d::StaticLayer"}
  - {name: obstacle_layer, type: "costmap_2d::ObstacleLayer"}
  - {name: inflation_layer, type: "costmap_2d::InflationLayer"}

# 局部代价地图插件声明示例
plugins:
  - {name: obstacle_layer, type: "costmap_2d::ObstacleLayer"}
  - {name: inflation_layer, type: "costmap_2d::InflationLayer"}

2. 静态层（Static Layer）参数

仅全局代价地图必选，负责加载静态地图，参数如下：

参数名	类型	默认值	设定影响	通用推荐值
map_topic	string	/map	静态地图的话题名，必须与 map_server 发布的话题一致。	保持默认/map
subscribe_to_updates	bool	false	是否订阅地图更新话题，用于地图动态更新的场景（如 SLAM 实时建图）。	纯静态地图导航保持false；SLAM + 导航同步建图设为true
track_unknown_space	bool	false	同通用参数，是否将静态地图的未知区域标记为 255。	与全局通用参数保持一致
use_maximum	bool	false	图层融合时，是否取该图层与其他图层的最大代价值。	保持false，默认叠加逻辑即可

3. 障碍物层（Obstacle Layer）参数

局部代价地图必选，负责实时处理传感器数据，标记 / 清除障碍物，是动态避障的核心。

3.1 图层全局参数

参数名	类型	默认值	设定影响	通用推荐值
enabled	bool	true	是否启用该图层。	保持true
footprint_clearing_enabled	bool	true	是否清除机器人轮廓范围内的障碍物，避免机器人自身被标记为障碍物。	保持true
max_obstacle_height	double	2.0	障碍物的最大有效高度（米），超过该高度的传感器数据会被过滤，避免天花板、吊灯等干扰。	设为「传感器安装高度 + 0.5~1.0m」，如激光装在 0.3m 高，设为0.8~1.0
min_obstacle_height	double	0.0	障碍物的最小有效高度（米），低于该高度的传感器数据会被过滤，避免地面点、地面杂物干扰。	设为「传感器安装高度 - 0.1m」，如激光装在 0.3m 高，设为0.2
obstacle_range	double	2.5	传感器标记障碍物的最大范围（米），超过该范围的障碍物不会被加入代价地图。→ 设得越大，能检测到更远的障碍物，但会引入更多噪声；→ 设得越小，地图越干净，但近距离障碍物才会被标记，避障不及时。	室内场景：3.0~5.0；室外场景：5.0~10.0；⚠️ 必须小于传感器的最大量程
raytrace_range	double	3.0	传感器射线清除自由空间的最大范围（米），传感器能 “看穿” 的最远距离，清除该范围内的无效障碍物。⚠️ 必须大于obstacle_range，否则障碍物无法被清除，会在地图中残留，导致机器人绕远路。	比obstacle_range大 1~2 米，如obstacle_range: 5.0，则设为6.0
combination_method	int	0	图层融合方式，0 = 覆盖，1 = 取最大值。	保持默认0即可

3.2 观测源参数（核心，支持多个传感器）

observation_sources参数定义所有用于检测障碍物的传感器，每个传感器需单独配置参数，示例如下：

yaml

# 障碍物层观测源配置
obstacle_layer:
  enabled: true
  max_obstacle_height: 1.0
  min_obstacle_height: 0.2
  obstacle_range: 5.0
  raytrace_range: 6.0
  # 声明观测源，多个传感器用空格分隔
  observation_sources: laser_scan
  # 每个观测源的单独配置
  laser_scan:
    topic: /scan                     # 传感器数据话题名
    sensor_frame: laser_link         # 传感器的坐标系
    data_type: LaserScan             # 数据类型，激光雷达为LaserScan，点云为PointCloud2
    marking: true                    # 是否用该传感器标记障碍物
    clearing: true                   # 是否用该传感器清除自由空间
    observation_persistence: 0.0     # 观测数据的保留时间（秒），0.0=实时更新，动态障碍物必须设为0
    expected_update_rate: 0.0        # 期望的更新频率（Hz），0.0=不检查，传感器稳定可设为10.0，超时报警
    min_obstacle_height: 0.2         # 可覆盖全局的最小/最大高度
    max_obstacle_height: 1.0

4. 膨胀层（Inflation Layer）参数

全局 + 局部地图必选，是导航安全的核心，负责给障碍物叠加安全膨胀区，参数如下：

参数名	类型	默认值	设定影响	通用推荐值
enabled	bool	true	是否启用该图层。	保持true
inflation_radius	double	0.55	【核心参数】障碍物的膨胀半径（米），即机器人中心到障碍物的最小安全距离。→ 必须≥机器人的外接圆半径，否则机器人会直接碰撞障碍物；→ 设得越大，机器人越保守，越远离障碍物，窄路越难通过；→ 设得越小，机器人越激进，越贴近障碍物，碰撞风险越高。	基础值：机器人外接半径 + 0.2~0.3m 安全余量；室内低速场景：0.5~0.8；室外高速场景：1.0~2.0；窄路通行场景：最小设为机器人外接半径 + 0.05m
cost_scaling_factor	double	10.0	【核心参数】膨胀区的代价衰减系数，决定离障碍物不同距离的代价值下降速度。核心公式：cost = exp(-cost_scaling_factor * (距离 - 内切半径)) * 252；→ 值越大，代价衰减越快，离障碍物近的区域代价值越低，机器人越敢靠近障碍物；→ 值越小，代价衰减越慢，离障碍物远的区域代价值越高，机器人越远离障碍物。	开阔保守场景：3.0~5.0；室内通用场景：8.0~10.0；窄路通行场景：10.0~20.0（让机器人能贴近障碍物通过窄路）
inflate_unknown	bool	false	是否给未知区域膨胀。	保持false，避免未知区域被过度膨胀，导致无法探索
inflate_around_footprint	bool	true	是否围绕机器人轮廓膨胀。	保持true，膨胀更精准

---

四、全局代价地图专属参数与通用配置

1. 全局代价地图特有参数

参数名	类型	默认值	设定影响	通用推荐值
rolling_window	bool	false	是否开启滚动窗口。→ 设为false：地图固定在global_frame，覆盖全静态地图，全局规划的基础；→ 设为true：地图随机器人滚动，仅能规划窗口内的路径，全局规划失效。	全局地图必须设为false，无特殊需求不修改
static_map	bool	true	是否加载静态地图，与静态层联动。	保持true，纯静态地图导航必须开启

2. 全局代价地图通用配置模板（global_costmap_params.yaml）

yaml

# 插件声明
plugins:
  - {name: static_layer, type: "costmap_2d::StaticLayer"}
  - {name: inflation_layer, type: "costmap_2d::InflationLayer"}

# 通用基础参数
global_frame: /map
robot_base_frame: /base_link
transform_tolerance: 0.5
resolution: 0.05
update_frequency: 2.0
publish_frequency: 1.0
static_map: true
rolling_window: false
track_unknown_space: false

# 机器人轮廓（二选一）
robot_radius: 0.22
# footprint: [[0.25, 0.2], [0.25, -0.2], [-0.25, -0.2], [-0.25, 0.2]]
footprint_padding: 0.01

# 代价阈值
lethal_cost_threshold: 254
unknown_cost_value: 255

# 静态层配置
static_layer:
  enabled: true
  map_topic: /map
  subscribe_to_updates: false

# 膨胀层配置
inflation_layer:
  enabled: true
  inflation_radius: 0.6
  cost_scaling_factor: 8.0

---

五、局部代价地图专属参数与通用配置

1. 局部代价地图特有参数

参数名	类型	默认值	设定影响	通用推荐值
rolling_window	bool	true	是否开启滚动窗口。→ 设为true：地图始终以机器人为中心，随机器人移动实时滚动，是局部动态避障的基础；→ 设为false：地图固定，机器人走出窗口后，局部规划完全失效。	局部地图必须设为true，无特殊需求不修改
static_map	bool	false	是否加载静态地图，局部地图无需加载全量静态地图，避免计算量过大。	保持false，如需静态障碍物信息，可在插件中添加静态层

2. 局部代价地图通用配置模板（local_costmap_params.yaml）

yaml

# 插件声明
plugins:
  - {name: obstacle_layer, type: "costmap_2d::ObstacleLayer"}
  - {name: inflation_layer, type: "costmap_2d::InflationLayer"}

# 通用基础参数
global_frame: /odom
robot_base_frame: /base_link
transform_tolerance: 0.2
resolution: 0.05
update_frequency: 10.0
publish_frequency: 2.0
static_map: false
rolling_window: true
track_unknown_space: false

# 滚动窗口尺寸（80栅格 × 0.05m = 4米窗口）
width: 80
height: 80
origin_x: -40
origin_y: -40

# 机器人轮廓（与全局地图完全一致）
robot_radius: 0.22
# footprint: [[0.25, 0.2], [0.25, -0.2], [-0.25, -0.2], [-0.25, 0.2]]
footprint_padding: 0.01

# 代价阈值
lethal_cost_threshold: 254
unknown_cost_value: 255

# 障碍物层配置
obstacle_layer:
  enabled: true
  max_obstacle_height: 1.0
  min_obstacle_height: 0.2
  obstacle_range: 5.0
  raytrace_range: 6.0
  footprint_clearing_enabled: true
  observation_sources: laser_scan
  laser_scan:
    topic: /scan
    sensor_frame: laser_link
    data_type: LaserScan
    marking: true
    clearing: true
    observation_persistence: 0.0
    expected_update_rate: 0.0

# 膨胀层配置（与全局地图一致，窄路场景可单独调整）
inflation_layer:
  enabled: true
  inflation_radius: 0.6
  cost_scaling_factor: 10.0

---

六、参数联动规则与调优核心逻辑

1. 必须严格联动的参数（不匹配直接导致导航失效）

参数组	联动要求
坐标系参数	全局代价地图的global_frame必须与 AMCL、全局规划器的global_frame_id完全一致；robot_base_frame必须与 AMCL、机器人 URDF 的基坐标系完全一致
分辨率参数	全局 + 局部代价地图的resolution必须与 SLAM 静态地图的分辨率完全一致
代价阈值参数	代价地图的lethal_cost_threshold必须与全局规划器的lethal_cost完全一致
机器人轮廓参数	全局 + 局部代价地图的robot_radius/footprint必须完全一致，且与 AMCL、全局规划器的内切 / 外接半径匹配
未知区域参数	全局代价地图的track_unknown_space必须与全局规划器的allow_unknown联动：track_unknown_space: true → allow_unknown可设为 true/false；track_unknown_space: false → allow_unknown必须设为 false

2. 调参优先级与核心原则

1. 先校准基础配置：先确认 TF 树正常、传感器话题正常、机器人轮廓准确、分辨率与静态地图一致，这是所有调参的前提。
2. 再调核心安全参数：优先调整inflation_radius，保证机器人不碰撞，再调整cost_scaling_factor平衡保守性与通行能力。
3. 然后调传感器参数：调整障碍物层的obstacle_range/raytrace_range，保证障碍物能正常标记和清除，无残留。
4. 最后调性能参数：根据 CPU 占用调整更新频率、地图窗口大小、传感器采样点数，平衡实时性与算力消耗。
5. 核心原则：全局地图重 “全局最优”，膨胀半径可稍大，保证路径无碰撞；局部地图重 “实时灵活”，膨胀半径可与全局一致，衰减系数可稍大，保证窄路通行能力。

恢复行为

恢复行为是 ROS 导航栈move_base节点的核心容错机制，当导航流程出现异常（规划失败、机器人卡住、路径跟踪失效等）时，通过预设的动作让机器人从异常状态中恢复，重新尝试导航任务，避免直接终止导航，大幅提升导航的鲁棒性与容错能力。

---

一、恢复行为核心基础

1. 核心作用与运行逻辑

move_base通过状态机管理导航全流程，正常状态为规划（PLANNING）→ 控制（CONTROLLING），当出现异常时，会进入恢复（RECOVERY）状态，按用户预设的恢复行为序列从轻到重依次执行，每执行完一个恢复动作，就会重新尝试规划与控制；若所有恢复行为执行完毕仍无法恢复正常，才会终止导航，返回ABORTED状态。

2. 触发恢复行为的核心场景

恢复行为的触发由move_base的全局参数控制，核心触发条件包括：

1. 全局规划器连续规划失败，超过planner_patience超时时间或max_planning_retries重试次数；
2. 局部控制器连续无法输出有效控制指令，超过controller_patience超时时间；
3. 机器人发生振荡 / 卡住：oscillation_timeout时间内，机器人位移小于oscillation_distance，视为原地卡住；
4. 机器人被障碍物完全包围，无可行路径，或跟踪路径严重偏离。

3. ROS 默认支持的恢复行为

ROS 导航栈原生提供 4 种常用恢复行为，覆盖绝大多数异常场景，按风险从低到高排序：

恢复行为类型	核心作用	风险等级	适用场景
clear_costmap_recovery/ClearCostmapRecovery	清除代价地图中的残留障碍物，重置地图状态	低	代价地图残留假障碍物、动态障碍物走后仍留痕，导致规划失败
rotate_recovery/RotateRecovery	机器人原地旋转 360°，重新扫描环境更新地图	中	机器人传感器有盲区、被障碍物包围，需要重新感知周围环境
back_up_recovery/BackUpRecovery	机器人直线后退一段距离，摆脱前方障碍物	中	机器人前方被障碍物挡住，后退后可重新规划路径
move_slow_and_clear/MoveSlowAndClear	机器人低速前进，同时清除路径上的代价地图	高	机器人被近距离假障碍物挡住，无法前进，需强行穿过安全区域

---

二、恢复行为全局控制参数

这组参数定义在move_base的主配置中，控制恢复行为的触发条件、执行序列、总开关，是恢复行为生效的前提，必须优先配置。

参数名	类型	默认值	核心影响	通用推荐值
recovery_behavior_enabled	bool	true	恢复行为的总开关。设为false时，任何导航异常都会直接终止，不执行任何恢复动作。	正常导航保持true；极狭窄空间、无恢复空间的场景可设为false
recovery_behaviors	list	见下文	【核心参数】定义恢复行为的执行序列，按列表顺序依次执行。若不手动配置，move_base会使用默认序列，忽略自定义的恢复行为参数。	按「从轻到重」原则配置，示例见下文
planner_patience	double	5.0	全局规划失败后，等待触发恢复的超时时间（秒）。超时后仍无法规划出有效路径，就进入恢复状态。	室内场景3.0~5.0；大场景 / 复杂环境5.0~10.0，避免频繁触发恢复
max_planning_retries	int	-1	全局规划失败的最大重试次数。-1表示无限重试，直到planner_patience超时；设为正整数时，失败对应次数后立刻触发恢复，无需等待超时。	对响应速度要求高的场景设为2~3；通用场景保持-1
controller_patience	double	15.0	局部控制器失效后，等待触发恢复的超时时间（秒）。超时后仍无法输出有效控制指令，进入恢复状态。	室内场景5.0~10.0；动态环境10.0~15.0
oscillation_timeout	double	0.0	振荡 / 卡住检测的超时时间（秒）。0.0表示关闭检测；设为非 0 值时，对应时间内机器人位移小于oscillation_distance，就视为卡住，触发恢复。	通用场景10.0~15.0；窄路场景20.0~30.0，避免误判
oscillation_distance	double	0.5	振荡检测的最小位移阈值（米）。超时时间内机器人移动距离小于该值，触发恢复。	小型机器人0.1~0.2；中型机器人0.2~0.5；大型 AGV0.5~1.0
clearing_rotation_allowed	bool	true	是否允许执行旋转类恢复行为。设为false时，禁用RotateRecovery。	不能原地旋转的机器人（如阿克曼无人车）、极狭窄空间设为false

关键注意：默认恢复序列

若不手动配置recovery_behaviors，move_base会使用默认的恢复序列，按以下顺序执行：

1. 执行ClearCostmapRecovery，仅清除局部代价地图；
2. 执行ClearCostmapRecovery，同时清除全局 + 局部代价地图；
3. 执行RotateRecovery，原地旋转 360°。

---

三、各恢复行为详细参数与设定

1. ClearCostmapRecovery（清除代价地图恢复，最常用）

核心原理

将代价地图中指定图层、指定范围的栅格重置为初始状态，清除传感器噪声、动态障碍物残留导致的假障碍物，解决「地图有脏东西导致规划不出路径」的问题，是风险最低、优先级最高的恢复行为。

全参数详解

参数名	类型	默认值	核心影响	通用推荐值
reset_distance	double	3.0	以机器人为中心，需要清除的圆形区域的半径（米）。→ 设得越大，清除范围越广，越能解决大范围残留问题，但会清除远处的有效动态障碍物，导致后续避障失效；→ 设得越小，仅清除机器人周边区域，对远处的残留无效。	第一次恢复（仅清局部）：3.0~5.0（等于局部地图半径）；第二次恢复（清全局）：5.0~10.0；室内场景不超过10.0，室外不超过20.0
layer_names	string[]	空	指定要清除的图层名称。不设置时，默认清除所有非静态层。	局部地图：["obstacle_layer"]；全局地图：["obstacle_layer", "voxel_layer"]；⚠️ 绝对不要加入static_layer，否则会清除静态地图的墙，导致路径穿模
reset_area	string	outside	清除区域的规则，可选inside/outside。→ inside：清除reset_distance以内的区域；→ outside：清除reset_distance以外的区域。	绝大多数场景用inside，机器人卡住通常是周边有假障碍物，仅需清除周围
costmap_to_clear	string	both	指定要清除的代价地图，可选local/global/both。	第一次恢复：local（仅清局部，风险最低）；第二次恢复：both（局部清除无效时，清全局 + 局部）

---

2. RotateRecovery（原地旋转恢复）

核心原理

控制机器人原地旋转 360°，旋转过程中通过激光雷达 / 深度相机重新扫描全向环境，更新代价地图，清除假障碍物、补全传感器盲区的障碍物信息，解决「机器人被包围、传感器有盲区，找不到可行出口」的问题。

全参数详解

参数名	类型	默认值	核心影响	通用推荐值
max_rotational_vel	double	0.4	旋转的最大角速度（rad/s）。→ 设得越大，旋转越快，恢复时间越短，但激光点云会出现畸变，扫描精度下降，差分机器人容易打滑；→ 设得越小，旋转越慢，扫描越精准，地图更新越可靠，但恢复时间长。	室内差分机器人0.3~0.5；全向机器人0.5~0.8；⚠️ 绝对不要超过机器人底盘的最大角速度，避免打滑导致里程计漂移
min_rotational_vel	double	0.3	旋转的最小角速度（rad/s），保证机器人旋转时不会因速度过低出现抖动。	必须小于max_rotational_vel，比最大值小0.1~0.2即可，如最大值 0.4，最小值设为 0.3
acceleration_lim	double	3.2	旋转的角加速度限制（rad/s²）。→ 设得越大，加减速越快，旋转越灵敏，但机器人会出现晃动，容易打滑；→ 设得越小，加减速越平缓，机器人越稳定。	室内场景1.0~2.0；大负载 AGV0.5~1.0，避免急转导致货物倾倒
sim_granularity	double	0.017	旋转过程中的碰撞检测步长（rad，默认对应 1°）。每旋转对应角度，就会检查一次是否碰撞。	保持默认0.017即可，无需修改；高精度场景可设为0.0087（0.5°）
frequency	double	20.0	旋转过程中控制指令的发布频率（Hz）。	必须与局部控制器的controller_frequency保持一致，通用10~20Hz

---

3. BackUpRecovery（后退恢复）

核心原理

控制机器人直线后退一段固定距离，摆脱前方障碍物的包围，后退到安全区域后重新规划路径，解决「机器人前方被挡住，无前进路径，但后方有空间」的场景，是窄路、走廊场景的常用恢复手段。

全参数详解

参数名	类型	默认值	核心影响	通用推荐值
backup_distance	double	-0.1	后退的距离（米），负数为后退，正数为前进。→ 绝对值越大，后退距离越长，越容易摆脱卡住状态，但后方有障碍物时碰撞风险越高；→ 绝对值越小，越安全，但可能无法摆脱卡住状态。	小型机器人-0.1~-0.3；中型机器人-0.3~-0.5；⚠️ 绝对不要超过-1.0，长距离后退无避障，风险极高
backup_vel	double	-0.05	后退的线速度（m/s），负数为后退，必须与backup_distance符号一致。	通用-0.05~-0.1，越慢越安全，绝对不要超过-0.2
controller_frequency	double	20.0	后退过程中控制指令的发布频率（Hz）。	与局部控制器频率保持一致，通用10~20Hz
min_radius	double	0.0	后退的转弯半径（米），0.0表示直线后退，非 0 值可实现弧线后退。	通用场景保持0.0，直线后退最安全，避免弧线后退撞到两侧障碍物

---

4. MoveSlowAndClear（低速前进清除恢复，风险最高）

核心原理

限制机器人的最大速度，让机器人低速向前移动一段距离，同时清除移动路径上的代价地图，强行穿过被假障碍物挡住的区域，解决「机器人门口 / 窄路前有假障碍物，不敢前进，实际可安全通行」的场景。⚠️ 风险提示：该恢复行为会清除路径上的代价地图，机器人在移动过程中无避障能力，仅适用于确认前方无真实障碍物的场景。

全参数详解

参数名	类型	默认值	核心影响	通用推荐值
max_translational_vel	double	0.1	移动的最大线速度（m/s），必须设置极低的值保证安全。	通用0.1~0.2，绝对不要超过0.3
lineal_vel	double	0.05	机器人实际移动的线速度（m/s），必须小于max_translational_vel。	通用0.05~0.1，越慢越安全
clearing_distance	double	0.5	要清除的机器人前方的距离（米），同时也是机器人的最大移动距离。	通用0.3~1.0，不要超过机器人的本体长度，避免进入未知风险区域
costmap_to_clear	string	local	要清除的代价地图，可选local/global/both。	必须保持local，绝对不要清除全局代价地图，避免丢失全局静态障碍物信息
frequency	double	10.0	移动过程中控制指令的发布频率（Hz）。	与局部控制器频率保持一致

---

四、可直接复用的完整配置示例

1. move_base 恢复行为主配置（launch 文件中）

<node pkg="move_base" type="move_base" name="move_base" output="screen">
    <!-- 全局控制参数 -->
    <param name="recovery_behavior_enabled" value="true" />
    <param name="planner_patience" value="5.0" />
    <param name="controller_patience" value="10.0" />
    <param name="max_planning_retries" value="3" />
    <param name="oscillation_timeout" value="10.0" />
    <param name="oscillation_distance" value="0.2" />
    <param name="clearing_rotation_allowed" value="true" />

    <!-- 【核心】恢复行为执行序列，从轻到重 -->
    <param name="recovery_behaviors" value="[
        {name: clear_local_costmap, type: clear_costmap_recovery/ClearCostmapRecovery},
        {name: clear_global_local_costmap, type: clear_costmap_recovery/ClearCostmapRecovery},
        {name: rotate_recovery, type: rotate_recovery/RotateRecovery},
        {name: back_up_recovery, type: back_up_recovery/BackUpRecovery}
    ]" />

    <!-- 加载各恢复行为的参数文件 -->
    <rosparam file="$(find your_pkg)/config/recovery_params.yaml" command="load" />
</node>

2. 恢复行为参数文件（recovery_params.yaml）

yaml

# 第一次恢复：仅清除局部代价地图
clear_local_costmap:
  reset_distance: 4.0
  layer_names: ["obstacle_layer"]
  reset_area: inside
  costmap_to_clear: local

# 第二次恢复：清除全局+局部代价地图
clear_global_local_costmap:
  reset_distance: 8.0
  layer_names: ["obstacle_layer"]
  reset_area: inside
  costmap_to_clear: both

# 第三次恢复：原地旋转360度
rotate_recovery:
  max_rotational_vel: 0.4
  min_rotational_vel: 0.3
  acceleration_lim: 1.5
  sim_granularity: 0.017
  frequency: 10.0

# 第四次恢复：后退0.3米
back_up_recovery:
  backup_distance: -0.3
  backup_vel: -0.08
  controller_frequency: 10.0
  min_radius: 0.0

ROS中的相机话题

一、相机话题核心分类（按功能）

ROS 中相机驱动（如usb_cam、realsense2_camera、uvc_camera）会发布标准化的话题，核心分为图像数据类、相机信息类、控制类三类，其中前两类是必选核心，控制类依相机型号可选。

话题类型	常见话题名	消息类型	核心作用	适用场景
原始图像话题	/camera/image_raw	sensor_msgs/Image	发布未压缩的原始图像数据（含像素值、分辨率、编码格式）	视觉算法（如 SLAM、目标检测）
压缩图像话题	/camera/image_compressed	sensor_msgs/CompressedImage	发布 JPEG/PNG 压缩后的图像，大幅降低带宽占用	远程传输、低算力设备
深度图像话题	/camera/depth/image_raw	sensor_msgs/Image	深度相机专属，发布深度值图像（单通道 16 位 / 32 位浮点型）	3D 感知、避障
深度点云话题	/camera/depth/points	sensor_msgs/PointCloud2	深度相机将深度图转换为 3D 点云发布，包含 x/y/z 坐标	三维重建、障碍物定位
相机信息话题	/camera/camera_info	sensor_msgs/CameraInfo	发布相机内参、畸变系数、分辨率等标定信息，用于图像校正	所有视觉应用（必配）
相机触发话题	/camera/trigger（自定义）	std_msgs/Empty	触发相机单次拍摄（部分工业相机支持）	按需采集图像
参数控制话题	/camera/set_camera_info	sensor_msgs/SetCameraInfo	设置相机内参（仅标定后使用）	相机标定后固化参数
曝光控制话题	/camera/set_exposure（自定义）	自定义消息（如std_msgs/Float32）	动态调整相机曝光值、增益等参数	自适应光照场景

注：话题命名可自定义（如/usb_cam/image_raw、/camera/color/image_raw），但核心格式遵循 ROS 标准，不同相机仅命名空间不同。

---

二、核心消息格式解析

1. 图像数据核心：sensor_msgs/Image

这是最基础的图像消息，核心字段决定图像的可解析性，新手需重点关注：

plaintext

# 基础头信息（含时间戳、坐标系）
Header header
# 图像分辨率
uint32 height
uint32 width
# 像素编码格式（关键！如mono8、bgr8、rgb8、16UC1（深度图））
string encoding
# 是否是大端字节序（几乎都为false）
bool is_bigendian
# 每行像素的字节数（可推导：width × 每个像素字节数）
uint32 step
# 图像原始数据（二进制数组，长度=height×step）
uint8[] data

常用编码格式（新手必记）：

• mono8：单通道 8 位灰度图（最常用，占内存最小）
• bgr8：3 通道 8 位 BGR 格式（OpenCV 默认格式）
• rgb8：3 通道 8 位 RGB 格式（部分相机 / 显示工具使用）
• 16UC1：16 位无符号整型（深度图，值为毫米级距离）
• 32FC1：32 位浮点型（高精度深度图）

2. 相机标定核心：sensor_msgs/CameraInfo

无相机信息的图像无法做校正和 3D 计算，该消息包含相机核心标定参数：

plaintext

Header header
# 图像分辨率（与Image消息一致）
uint32 height
uint32 width
# 相机内参矩阵（3×3，K[0]=fx, K[4]=fy, K[2]=cx, K[5]=cy）
float64[9] K
# 畸变系数（常用前4个：k1,k2,p1,p2）
float64[5] D
# 投影矩阵（用于图像→3D点转换）
float64[12] P
# 畸变模型（如plumb_bob针孔模型）
string distortion_model
...（其他辅助参数）

核心内参说明：

• fx/fy：x/y 轴焦距（像素单位）
• cx/cy：主点坐标（图像中心像素）
• 畸变系数D：校正鱼眼 / 径向畸变，无畸变则全为 0

---

三、实用操作与代码示例

1. 命令行快速查看相机话题（新手必备）

# 1. 查看当前所有相机相关话题
rostopic list | grep camera

# 2. 查看图像话题的消息格式
rostopic info /camera/image_raw
rostopic echo -n1 /camera/image_raw  # 查看单帧图像数据（仅看结构，不看二进制data）

# 3. 查看相机内参
rostopic echo -n1 /camera/camera_info

# 4. 可视化图像（需安装rqt）
rosrun rqt_image_view rqt_image_view  # 图形界面选择相机话题查看实时图像
# 或直接指定话题
rosrun image_view image_view image:=/camera/image_raw

2. C++ 核心依赖与编译配置

若用 C++ 开发，需在package.xml添加依赖：

<depend>roscpp</depend>
<depend>sensor_msgs</depend>
<depend>cv_bridge</depend>
<depend>image_transport</depend>
<depend>opencv2</depend>

在CMakeLists.txt添加：

find_package(catkin REQUIRED COMPONENTS
  roscpp
  sensor_msgs
  cv_bridge
  image_transport
  opencv2
)
include_directories(${catkin_INCLUDE_DIRS} ${OpenCV_INCLUDE_DIRS})

# 编译可执行文件
add_executable(camera_sub src/camera_sub.cpp)
target_link_libraries(camera_sub ${catkin_LIBRARIES} ${OpenCV_LIBRARIES})

---

四、关键避坑与实用要点

1. 高频踩坑点

• 图像编码不匹配：订阅时若指定bgr8但相机发布rgb8，图像会出现颜色错乱，需先通过rostopic echo确认编码格式。
• 相机信息与图像不同步：时间戳不一致会导致校正失败，需确保相机驱动同时发布image_raw和camera_info，且时间戳对齐。
• 深度图数值解析错误：16 位深度图（16UC1）需按像素值 × 单位（如 1mm）转换为实际距离，直接按 8 位灰度图解析会得到错误值。
• 话题带宽过高：原始图像（如 1080P@30fps）带宽约 100MB/s，低算力 / 远程场景务必使用image_compressed压缩话题。

2. 实用优化技巧

• 图像压缩：通过image_transport工具将原始图像转为压缩格式，无需修改驱动：

  rosrun image_transport republish raw compressed in:=/camera/image_raw out:=/camera/image_compressed
  

• 帧率限制：若相机帧率过高（如 60fps），可在订阅时设置队列大小为 1，并添加时间戳过滤，只处理最新帧。
• 坐标系配置：相机话题header.frame_id需设为camera_optical_frame（光学帧，x 朝右、y 朝下、z 朝前），与camera_link（机械帧）通过 TF 关联，否则视觉算法会出现坐标系错误。