ROS坐标系常用坐标及其变换详细梳理

深入理解ROS中常用的几种坐标系及其变换，对理解ROS机器人自主导航的相关程序有较大的帮助，比如我在进行路径规划算法的研究中，编写程序逻辑时，时常需要涉及到坐标轴方向以及坐标间的变换。本文按照使用顺序对ROS中常用一些相关的坐标系进行梳理。

慕羽★

4167人浏览 · 2024-03-22 08:25:53

慕羽★ · 2024-03-22 08:25:53 发布

一、World与Map坐标系

假设现在我们要对如下所示的Gazebo中的仿真环境进行建图，图中标注的坐标系即为真实物理世界的坐标，也就是ROS中的world坐标系，常用单位为m，常用数据类型为float或double。

【注】：建图时机器人的位置是位于图中坐标系的原点处的，下图并非是在建图阶段获取的，而且在自主导航时截取的，程序设定的机器人初始位置在（-0.5，0，0）处，所以图中显示的机器人没在原点

–

我们使用SLAM算法建图后会生成一个pgm格式的图片文件，也就是导航中使用/map_server加载的全局静态地图，如下图所示，可以发现生成的地图方向并非与Gazebo地图中看到的正视图的方向相同，而是进行了旋转，使得X轴在水平方向上。SLAM算法建图生成的静态地图的左下角即为ROS中常用的map坐标系的原点，水平方向向右为X轴，垂直方向向上为Y轴

–

SLAM算法建图时，还会生成一个yaml格式的参数配置文件，如下所示

image: /home/gly/catkin_ws/src/ackerman_sim/bringup/map/complexmap.pgm
resolution: 0.050000
origin: [-18.600000, -17.000000, 0.000000]
negate: 0
occupied_thresh: 0.65
free_thresh: 0.196

上述各参数的含义如下：

1. image: 地图图像的路径。在这个例子中，图像文件位于/home/gly/catkin_ws/src/ackerman_sim/bringup/map/complexmap.pgm。

2. resolution: 地图的分辨率。这个参数表示地图中一个像素对应的实际长度（以米为单位）。在这个例子中，分辨率为0.05米/像素，意味着每个像素代表地图中0.05米的实际长度。

3. origin: 地图的原点坐标。这是一个包含三个值的数组，表示地图坐标系的原点在世界坐标系中的位置。在这个例子中，原点坐标为(-18.6, -17.0, 0.0)，表示地图坐标系的原点位于世界坐标系中的(-18.6, -17.0)位置，并且Z轴的高度为0.0。

4. negate: 用于反转地图像素值的参数。如果设置为1，则地图中的障碍物像素值将被反转（0变为255，255变为0），以便于某些算法处理。在这个例子中，设置为0，表示不反转地图像素值。

5. occupied_thresh: 表示一个像素被认为是占用的阈值。在地图构建过程中，如果一个像素的占用概率超过了这个阈值，那么它将被认为是占用的。在这个例子中，设置为0.65，意味着只有当一个像素的占用概率大于0.65时，才被认为是占用的。

6. free_thresh: 表示一个像素被认为是自由的（非占用）的阈值。在地图构建过程中，如果一个像素的占用概率低于这个阈值，那么它将被认为是自由的。在这个例子中，设置为0.196，意味着只有当一个像素的占用概率小于0.196时，才被认为是自由的。

↓↓↓↓↓↓↓→→【重点内容！！！】←←↓↓↓↓↓↓↓

☆☆☆ 接下来，对World坐标系与Map坐标系以及他们之间的关系进行总结☆☆☆

①常用单位和数据类型：

World坐标系表示真实的物理世界的坐标，常用单位为m，常用数据类型为float或double。Map坐标系表示按照设定的精度，也就是分辨率resolution，将真实物理世界建模为栅格地图的坐标，真实的物理世界的坐标，无单位，常用数据类型为unsigned int 或int

SLAM建图生成的yaml文件中的参数resolution表明了建图时的分辨率，如resolution: 0.05，即表示将真实世界中每0.05mx0.05m的一个小正方形建模为栅格地图中的一个小栅格，也是SLAM建图生成的pgm格式的图片文件中的一个像素点。

② 坐标系方向：

World坐标系与Map坐标系的方向是相同的，而且固定的，即这两个坐标系都是静态的，不会发生旋转和平移等改变。

③ 坐标系原点：

SLAM建图算法会将启动建图程序的那一刻机器人处于真实世界的位置设定为World坐标系的原点，将SLAM建图生成的pgm图片文件的左下角那个点作为Map坐标系的原点。

两者之前的坐标关系，可由建图生成的yaml文件中的参数origin确定，如 [-18.600000, -17.000000, 0.000000]，表示Map坐标系的原点在World坐标系的-18.600000m, -17.000000m处，这两个参数也确定了两个坐标系原点之间的相对位置关系

④ 坐标系转换：

ROS中常使用Costmap2D功能包中提供的mapToWorld和worldToMap两个函数进行World坐标系与Map坐标系的互相转换，这两个函数源码如下，有了上述对这两个坐标系以及参数resolution与origin，就很容易理解以下转换程序了

void Costmap2D::mapToWorld(unsigned int mx, unsigned int my, double& wx, double& wy) const
{
  wx = origin_x_ + (mx + 0.5) * resolution_;  //+0.5是为了取栅格中点坐标
  wy = origin_y_ + (my + 0.5) * resolution_;
}

bool Costmap2D::worldToMap(double wx, double wy, unsigned int& mx, unsigned int& my) const
{
  if (wx < origin_x_ || wy < origin_y_)
    return false;

  mx = (int)((wx - origin_x_) / resolution_);
  my = (int)((wy - origin_y_) / resolution_);

  if (mx < size_x_ && my < size_y_)
    return true;

  return false;
}

↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑

需要注意的是，我们在rviz中加载的地图方向与Gazebo中的仿真环境方向是一样的，如下面的左图所示，但是与地图坐标系map的方向是不同的，即如下所示的rviz中显示的界面中，Map坐标系的原点位于右下角，垂直向上为X轴，水平向左为Y轴。Map坐标系与World坐标系并没有发生变化，只是在Gazebo与rviz中显示的时候，与SLAM建图生成的pgm文件中的显示方向不同而已，只是视觉效果。

二、World与base_link坐标系

base_link坐标系，即机器人坐标系，与Map和World坐标系不同，base_link坐标系是动态坐标系，是跟随机器人运行，原点一般设定在机器人的中心，X轴方向取为机器人向前运动的方向，Y轴按右手定则取为朝小车左侧的方向。

base_link坐标系的初始位置和方向，一般在启动加载机器人模型的launch文件中设定，如下图初始位置设定为 -0.5，0，0，初始姿态设定为跟世界坐标系相同，后续通过定位算法来确定机器人的位置和姿态（注意：下图中标注的map并非表示上文中介绍的map坐标系，而是World坐标系！！！这里很容易混淆）。在使用上常通过调用getRobotPose函数来获取机器人在世界坐标系World下的位置，若想了解该部分内容，可点击以下链接，跳转查看博客《深入分析常规ROS机器人如何使用Navigation导航包实现实时定位》同样，该博客中描述的map坐标系并非本文第一部分提到的map坐标系，该博客描述的map坐标系，就是下图标注map的坐标系，表示的其实是本文上面提到的World坐标系的含义。即RVIZ界面中显示的map系，并非我们说的栅格地图所使用的map系，实际上表示真实的物理世界坐标系，在程序里面对应World坐标系。

https://blog.csdn.net/qq_44339029/article/details/134241454

ROS常通过调用getRobotPose函数来获取当前机器人在世界坐标系World下的位置，然后通过worldToMap函数将其转换到栅格地图所使用的map坐标系下

三、Map与OpenCV坐标系

前文提到SLAM算法建好的地图文件，以左下角为坐标系原点，水平向右为X轴方向，垂直向上为Y轴方向，而在进行图像处理时，比如常用的OpenCV、MATLAB里面图像处理相关的库函数所使用的坐标系原点一般取为图像文件的左上角，水平向右为X轴方向，垂直向下为Y轴方向，如下图所示。

所以当ROS中用到OpenCV中的库函数对地图进行处理时，常常需要将所使用的栅格地图坐标系，也就是map坐标系，转换为OpenCV坐标系，恰好图像的一个像素点对应一个栅格，对于X轴而言方向相同可以不用转换，Y轴的方向相反，需要用Y轴的像素个数减去当前像素进行转换，如下式所示，其中mx和my是栅格地图坐标，rows是Y轴像素个数或者说栅格个数

row = rows - my - 1; 
col = mx

以下内容对全覆盖路径规划开源项目Clean-robot-turtlebot3不感兴趣的小伙伴，可以跳过

附： Clean-robot-turtlebot3项目相关示例

特别的，还存在一些特殊的转换，比如在全覆盖路径规划开源项目Clean-robot-turtlebot3中，在进行全覆盖时，认为机器人每走过原来栅格地图中的一个栅格，就可以完成对其周围SIZE_OF_CELL 个栅格的覆盖。这样在转换时就不再遵守原来1:1的转换比例，而是栅格地图坐标系map中每SIZE_OF_CELL 个栅格转换为OpenCV坐标系中一个像素点，将原来比较密集的栅格地图，转换为比较稀疏的新地图，表达式如下

initPoint.row = cellMat_.rows - my / SIZE_OF_CELL - 1; 
initPoint.col = mx / SIZE_OF_CELL;

Clean-robot-turtlebot3项目的关键函数mainPlanningLoop()中的以下源码使用了上述转换后得到的OpenCV坐标系来判断机器人沿某个方向运动是否会超出地图边界的问题


        vector<float> thetaVec = {0, 45, 90, 135, 180, 225, 270, 315};
        
        。
        。
        。

        for (int id = 0; id < 8; id++)
        {
            deltaTheta = max(thetaVec[id], lasttheta) - min(thetaVec[id], lasttheta);
            if (deltaTheta > 180)
                deltaTheta = 360 - deltaTheta;
            e = 1 - abs(deltaTheta) / 180; //角度参数？
            switch (id)
            {
            case 0:
                if (currentPoint.col == neuralizedMat_.cols - 1)
                {
                    v = -100000;
                    break;
                } //处于边界？
                v = neuralizedMat_.at<float>(currentPoint.row, currentPoint.col + 1) + c_0 * e;

                break;
            case 1:
                if (currentPoint.col == neuralizedMat_.cols - 1 || currentPoint.row == 0)
                {
                    v = -100000;
                    break;
                }
                v = neuralizedMat_.at<float>(currentPoint.row - 1, currentPoint.col + 1) + c_0 * e - 200;

                break;
            case 2:
                if (currentPoint.row == 0)
                {
                    v = -100000;
                    break;
                }
                v = neuralizedMat_.at<float>(currentPoint.row - 1, currentPoint.col) + c_0 * e;

                break;
            case 3:
                if (currentPoint.col == 0 || currentPoint.row == 0)
                {
                    v = -100000;
                    break;
                }
                v = neuralizedMat_.at<float>(currentPoint.row - 1, currentPoint.col - 1) + c_0 * e - 200;

                break;
            case 4:
                if (currentPoint.col == 0)
                {
                    v = -100000;
                    break;
                }
                v = neuralizedMat_.at<float>(currentPoint.row, currentPoint.col - 1) + c_0 * e;

                break;
            case 5:
                if (currentPoint.col == 0 || currentPoint.row == neuralizedMat_.rows - 1)
                {
                    v = -100000;
                    break;
                }
                v = neuralizedMat_.at<float>(currentPoint.row + 1, currentPoint.col - 1) + c_0 * e - 200;

                break;
            case 6:
                if (currentPoint.row == neuralizedMat_.rows - 1)
                {
                    v = -100000;
                    break;
                }
                v = neuralizedMat_.at<float>(currentPoint.row + 1, currentPoint.col) + c_0 * e;

                break;
            case 7:
                if (currentPoint.col == neuralizedMat_.cols - 1 || currentPoint.row == neuralizedMat_.rows - 1)
                {
                    v = -100000;
                    break;
                }
                v = neuralizedMat_.at<float>(currentPoint.row + 1, currentPoint.col + 1) + c_0 * e - 200;

                break;
            default:
                break;
            }
            if (v > max_v)
            {
                max_v = v;
                maxIndex = id;
            }

            if (v == max_v && id > maxIndex)
            {
                max_v = v;
                maxIndex = id;
            }
        }

–

根据程序推断，上述0, 45, 90, 135, 180, 225, 270, 315这八个角度对应的八个方向的0度是水平向右的，即OpenCV坐标系的X轴方向，且按照逆时针方向角度增加的原则，如下图所示

–

现在再来看上面程序中活力值v的计算方法，就容易的多了，如case 0 时，沿角度为0的移动方向时，如果当前机器人位于地图的右边界时，即currentPoint .col == neuralizedMat_ .cols - 1，继续沿着图中角度为0的方向移动，必然会超出地图边界，从而此时当前方向需要给一个很小的活力值，如程序中的v = -100000，来使机器人不要向该方向移动。

再比如case 1 时，沿角度为45的移动方向时，如果当前机器人位于地图的上边界或者右边界时，继续沿着图中角度为45的方向移动，必然会超出地图边界，从而此时当前方向需要给一个很小的活力值，如程序中的v = -100000，来使机器人不要向该方向移动。

以此类推，我们同样理解其他6个方向基于极小的活力值的程序，接下来我们来看不在边界的情况，例如case 0时，如下所示，其中(currentPoint.row, currentPoint.col + 1)即为机器人从当前位置沿着case0 对应的角度为0方向走一步后OpenCV坐标系坐标，c0为系数，程序设定值为50，e是当前方向与上一次移动方向的相关系数，取值范围为0~1，两个端点，为0则当前移动方向与上一次移动方向相反，为1当前移动方向与上一次移动方向相同，所以附加项 c_0 * e的作用是更倾向于选取与上一次移动方向相同或者相近的方向

v = neuralizedMat_.at<float>(currentPoint.row, currentPoint.col + 1) + c_0 * e;

此外，所有对角方向，即45, 135, 225, 315这四个方向计算活力值时，后面还有一个附加项-200，如case1对应的45度方向的表达式如下，这个-200的附加项的作用可能是尽量选取上下左右这四个方向，所以对对角的四个方向施加了额外的代价

 v = neuralizedMat_.at<float>(currentPoint.row - 1, currentPoint.col + 1) + c_0 * e - 200;

上述neuralizedMat_是活力值地图，是使用稀疏后的新地图cellMat_来初始化的，初始化规则如下：如果新地图中某个栅格处有障碍物，则其对应的活力值地图中存储一个较大的负数-100000，如果没有障碍物，则活力值地图中对应的位置存储的活力值为50/j，其中j是其所在的列数，这种活力值设置模式使得同一列无障碍物点的初始活力值相同，而随着所在列编号的增加，活力值依次降低，在后续的步骤中，机器人会倾向于向活力值高的区域移动，这样初始化活力值，将引导机器人按列形成从弓字型的覆盖路径，且避开障碍物。