SLAM14讲——闲笔杂谈

、梦醒惊蛰 dream£

已于 2024-04-26 21:44:06 修改

阅读量1.9k

点赞数 37

文章标签：人工智能数码相机

于 2024-04-26 14:59:14 首次发布

本文链接：https://blog.csdn.net/weixin_45274074/article/details/137823176

版权

李群和李代数是用来解决旋转矩阵求导问题和无约束优化问题

SLAM的传感器

在SLAM14讲书中作者介绍了SLAM常用的传感器。我在前面说过，只要是完成同时建图和定位的任务，那么我们就把它叫做SLAM，我们可以想到能满足这个要求的传感器很多，比方说激光雷达，相机，轮式里程计惯性测量单元，我想这其中你最熟悉的传感器一定是：相机。别的传感器可能你听说过，可能没有听说过，你不必在他们上面多花时间去了解，因为《视觉SLAM十四讲》这本书只针对相机。

单目相机
　　作者在书中介绍了单目相机，由于现在是初识阶段，所以没有介绍单目相机的原理，作者不断地在强调一个问题，那就是单目相机的尺度问题。
　　我不知道你对单目相机的尺度是否理解，如果你不理解，下面请你跟我一起做一个实验，闭上你的一只眼睛，然后将你的双手掌心朝向你，两个手掌之间前后方向稍微有一些距离，这个时候你会发现，你根本不可能通过一只眼睛分清楚两个手掌谁在前谁在后。但是你可能会说，我明明知道他们的前后啊，对不起那并不是你的眼睛观察出来的，而是你通过近大远小的规律判断出来的，你知道两只手一样大，看着小的离我们就远，但是相机不知道啊！你能得到距离感那是因为你通过后天不断地学习获得的知识，并不是你的眼睛直接看出来的。
　　单目相机就相当于我们在用一只眼睛观察世界，尽管它看到了这一切，但是它并不知道它所看到的东西，具有多大的真实尺寸。由于还没有学到单目相机的模型，所以只能通过一个感性的方法，帮你理解一下单目的尺度问题。
　　由于没有尺度，也就是说没有真实的空间距离，那么我们后续产生的一切轨迹或者点，都没有真实单位信息，有的只是它们之间的相对关系，也许你还不能理解这句话，但是你可以记住它，留下一个疑问，日后你自会理解。

双目相机
双目相机，你就可以理解成像我们的眼睛一样，两个眼睛之间的距离，叫做基线。这个是双目相机出厂就知道的信息，然后我们想过两个双目之间三角关系和相似三角形关系，就可以知道照片上一个点，对应到空间上的真实位置。也许你觉得很神奇，或者匪夷所思，但是当你在后续文章中了解双目相机模型之后，你就会发现它的测距原理是如此的简单。

深度相机
深度相机由叫RGB-D相机，其中R、G、B表示的是图片的三个通道，D表示的是深度，说明RGB-D相机获得是一张，彩色的图片，并且每一个像素还额外获得了深度信息。作为我们对SLAM的应用，你对RGB-D相机的了解到此就足够了，没有必要再去深挖它的原理。

经典SLAM的视觉框架

作者介绍了目前视觉SLAM最为常见的流程，初看你一定对这每一部分都没有什么认识，也许根据“传感器数据”、“回环检测”、“建图”的字面意思，你能推断一些信息，但是这离完全理解框架的流程还很远。
　　
　　我觉得没有什么比举例子，然后带着你感受一遍这个流程来的直接。所以我下面采用一个例子，但是感受一下这个过程：

下面请你把自己想象成一个机器人，你的两个眼睛就是一个双目摄像头，假设你的眼睛不转，头也不转，眼睛的视角只能通过脚步移动去控制。
现在你在一个超市里面，你的双脚开始移动，你一边走一边通过眼睛观察你前方的场景，你每走一步，你就会更深入走进你眼前的场景，而你就能通过眼前场景的变化推断出你行进的轨迹，然后你就在这个超市里面不停地走动，那么你的轨迹越来越长，但是由于你的眼睛观察有误差，所以就会导致你大脑在形成轨迹时产生的误差越来越大。
走着走着，忽然，你发现自己好像来过这个地方，这时你马上发现了一个问题，我的脑海中推断的轨迹是我现在在水产区，实际上我现在在蔬果区，于是你可以断定自己前面的轨迹产生了多大的误差，于是你就想采用了一个不算太好，但是也只能这样的方法，你将你推断导致的误差，平均分给你前面走过每一步，这样的话虽然总体误差没有降低，但是你最后所在的位置会更接近你真实的位置，这样做会让我们看起来更加接近真实的运动轨迹。
最后，你走了很多的路，你也修正了自己在超市运动的轨迹，而超市的整个环境的样貌也就在你的脑海里，留下了印象。

其实上面的这一个过程就是一个经典的SLAM框架：

眼睛看到的东西，就对应于框架中的传感器数据；
我们通过前后两步看到的场景变换，推断出轨迹的过程，就是前端，也叫作视觉里程计；
忽然我们发现自己回到了走过的地方，就对应于回环检测；
上面例子中我们发现自己实际在蔬果区，但是根据累计的观察，我们觉得自己是在水产区，然后我们就优化整个误差，这个就对应于后端，常常采用的方法就是非线性优化；
最后我们记住了整个超市的环境样貌，这就是建图。

里程计

里程计是一种利用从移动传感器获得的数据来估计物体位置随时间的变化而改变的方法。该方法被用在许多机器人系统来估计机器人相对于初始位置移动的距离。

关于里程计，我要多说一句：在我们的印象中“XX计”，例如温度计，都是一种实际的物件，但是SLAM领域的里程计它并不是你可以摸得到的物件，它就是通过代码完成相邻时刻运动的计算（或者说一种计算方法），由于它的这个效果和我们实际的里程计一样，所以把它叫做：里程计。

常用的里程计定位方法有轮式里程计、视觉里程计以及视觉惯性里程计。

轮式里程计（Wheel Odometry）

我们用一个例子来解释轮式里程计的原理，假设你现在拥有一辆马车，你想知道你驾驶马车从A地到B地要有多远。现在你知道了马车轮子的周长，然后你在轮子上安装了一种驾驶时可以统计车轮所转圈数的装置，通过马车的周长、从A地到B地所用的时间和所得的车轮圈数，你就能计算得出两地之间的路程了。

轮式里程计是一种最简单，获取成本最低的方法。与其它定位方案一样，轮式里程计也需要传感器感知外部信息。

轮式里程计的航迹推算定位方法主要基于光电编码器在采样周期内脉冲的变化量计算出车轮相对于地面移动的距离和方向角的变化量，从而推算出移动机器人位姿的相对变化。
轨迹是由一个个位置组成的，这里的位置实际上是位姿，包括坐标值和角度，上期知识分享中有提到表示位置和位姿的方法有：欧拉角和四元数。

视觉里程计（Visual Odometry）

视觉里程计是通过移动机器人上搭载的单个或多个相机的连续拍摄图像作为输入，从而增量式地估算移动机器人的运动状态。

视觉里程计分为单目VO和双目VO。双目VO的优势在于，能够精确的估计运动轨迹，且具有确切的物理单位。在单目VO中，你仅仅只能知道移动机器人在x或y方向上移动了1个单位，而双目VO则可明确知道是移动了1cm。但是，对于距离很远的移动机器人，双目系统则会自动退化成为单目系统。

视觉惯性里程计(visual-inertial Odometry)

视觉惯性里程计，也叫视觉惯性系统（visual-inertial system），是将融合了相机和IMU数据实现SLAM的一种算法。根据融合框架的不同又分为松耦合和紧耦合。

松耦合中视觉运动估计和惯导运动估计系统是两个独立的模块，将每个模块的输出结果进行融合。

紧耦合则是使用两个传感器的原始数据共同估计一组变量，传感器噪声也是相互影响的。紧耦合算法比较复杂，但充分利用了传感器数据，可以实现更好的效果，是目前研究的重点。

在ROS当中，里程计是一种利用从移动传感器获得的数据来估计物体位置随时间的变化而改变的方法。

通俗来说：在ROS当中里程计信息可以分为两个部分：一个是位姿（位置和姿态），一个是速度（线速度和角速度）。

IMU

数据获取

IMU即为惯性测量单元，一般包含了三个单轴的加速度计、陀螺仪和磁力计，简单理解通过加速度二次积分就可以得到位移信息、通过角速度积分就可以得到三个角度，实时要比这个复杂许多。

误差补偿

Imu是一种高灵敏度的传感器，由于制造过程的物理因素，导致 IMU 惯性测量单元实际的坐标轴与理想的坐标轴之间会有一定的偏差，进而导致imu的噪声、尺度误差和轴偏差。出于这一点，需要对采集到的数据进行校准（内部标定），对这些误差进行补偿。

Imu校准后的九轴数据进行数据融合的过程中，由于加速度计和磁力计的高频噪声和陀螺仪的低频噪声，会导致解算姿态出现较大误差。对此，需要选择合适的九轴数据融合算法。常用的九轴数据融合算法包括：高低通互补滤波、扩展卡尔曼滤波 EKF、 Mahony 滤波。

特征提取算法

ORB、SURF 和 SIFT 都是计算机视觉领域常用的特征提取算法，它们在图像处理、目标识别、图像匹配等方面有着重要的应用。

简要地介绍一下它们：

ORB（Oriented FAST and Rotated BRIEF）

ORB是一种结合了FAST特征点检测器和BRIEF特征描述子的算法，同时具备了旋转不变性和尺度不变性。由于FAST检测速度快，而BRIEF描述子简单高效，因此ORB在实时应用中性能优越。它适用于快速特征提取和匹配的场景。

SURF（Speeded-Up Robust Features）

SURF是一种基于局部特征的检测和描述算法，具有速度快和鲁棒性好的特点。SURF使用Hessian矩阵来检测关键点，提取特征时采用了Haar小波特征描述子。SURF在图像匹配、物体识别等领域有着广泛的应用。

SIFT（Scale-Invariant Feature Transform）

SIFT是一种基于局部不变特征的描述方法，能够在不同尺度和旋转下提取出稳定的特征点。SIFT通过DoG（高斯差分）、关键点检测、特征向量描述等步骤实现特征提取。尽管SIFT计算复杂，在实际应用中仍然是一种性能优秀的特征提取算法。

机器人系统仿真

机器人坐标系

Riiz中坐标系（下图所示）

x:红色的轴

y:绿色的轴

z:蓝色的轴

特别注意：一切机器人建模均以 x 轴正方向为基准。想象你就是机器人本体，你的前方是 x 轴正方向，左方是 y 轴正方向，头顶上方就是 z 轴正方向。一切的一切都是以你的parent_link为坐标基准进行坐标运算求解的。这一点理解尤为重要。每一个link进行计算都要考虑自己的父link作为参考坐标系，有些书中又称为（链接原点）。切记不一定是物理中心！！！

案例分析

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

-->

代码实例：

<robot name="mycar">
    <!-- 底盘 -->
    <link name="base_link">
        <visual>
            <geometry>
                <box size="0.3 0.2 0.1" />
            </geometry>
            <origin xyz="0 0 0" rpy="0 0 0" />
            <material name="blue">
                <color rgba="0 0 1.0 0.5" />
            </material>
        </visual>
    </link>

    <!-- 摄像头 -->
    <link name="camera">
        <visual>
            <geometry>
                <box size="0.02 0.05 0.05" />
            </geometry>
            <origin xyz="0 0 0.025" rpy="0 0 0" />
            <material name="red">
                <color rgba="1 0 0 0.5" />
            </material>
        </visual>
    </link>

    <!-- 关节 -->
    <joint name="camera2baselink" type="continuous">
        <parent link="base_link"/>
        <child link="camera" />
        <!-- 需要计算两个 link 的物理中心之间的偏移量 -->
        <origin xyz="0.12 0 0.05" rpy="0 0 0" />
        <axis xyz="0 0 1" />
    </joint>

</robot>

效果展示：

在URDF（Unified Robot Description Format）中，每个<link>元素都定义了一个独立的坐标系，这个坐标系与该链接的几何形状相关联。对于您提供的URDF代码，base_link和camera分别代表底盘和摄像头的链接，并且它们各自拥有自己的坐标系。

底盘链接坐标系（base_link坐标系）：

这是底盘的参考坐标系。在您的代码中，<origin xyz="0 0 0" rpy="0 0 0" />在base_link的<visual>元素中定义了底盘的几何形状相对于该坐标系的原点位置和方向。由于xyz和rpy都是0，这意味着底盘的几何形状中心与底盘链接坐标系的原点重合，并且没有旋转。

摄像头链接坐标系（camera坐标系）：

这是摄像头的参考坐标系。在您的代码中，<origin xyz="0 0 0.05" rpy="0 0 0" />在camera的<visual>元素中定义了摄像头的几何形状相对于该坐标系的原点位置和方向。这里，xyz="0 0 0.05"表示摄像头的几何形状在摄像头链接坐标系的Z轴上偏移了0.05米，这意味着摄像头稍微“突出”在底盘的前面。同样，没有旋转（rpy="0 0 0"）。

相对坐标系

当您定义关节<joint>时，<origin xyz="0.12 0 0.05" rpy="0 0 0" />定义了摄像头链接坐标系（camera坐标系）相对于底盘链接坐标系（base_link坐标系）的位置和方向。这意味着，如果底盘链接坐标系是固定的，那么摄像头链接坐标系的原点会位于底盘链接坐标系X轴正方向0.12米、Z轴正方向0.05米的位置（Y轴无偏移）。

在机器人运动学和控制中，了解这些链接坐标系的位置和相对方向至关重要，因为它们决定了如何计算机器人各部分之间的相对位置和姿态，以及如何根据关节的运动来更新这些位置和姿态。在仿真或实际机器人控制中，这些坐标系用于转换传感器数据、执行运动规划以及控制机器人的动作。

坐标释义解读（帮助理解）

在机器人学和URDF（Unified Robot Description Format）中，链接坐标系的原点通常不是指物理中心，而是指一个用于描述该链接几何形状和位置的参考点。这个参考点可以是链接的几何中心，也可以是其他任何方便描述链接位置和姿态的点。

在URDF中定义链接时，<origin>标签用于指定链接坐标系相对于其父链接坐标系的位置和方向。这里的xyz值表示链接坐标系原点在父链接坐标系中的位置，而rpy值则表示链接坐标系相对于父链接坐标系的旋转角度。

因此，当您在URDF中看到链接坐标系的定义时，它通常是一个方便描述链接位置和姿态的参考点，而不是物理中心。物理中心可能是一个不同的点，具体取决于链接的几何形状和尺寸。但在URDF中，重要的是确保链接坐标系的定义能够准确地描述链接在机器人结构中的位置和姿态，以便进行运动学计算和控制。

在您提供的URDF代码中，base_link和camera的<origin>标签定义了它们各自坐标系的位置和方向。这些定义用于描述底盘和摄像头在机器人结构中的相对位置，而不是它们的物理中心。因此，当您提到“不是物理中心”时，您可能是在指出这些定义是基于链接的参考点，而不是基于物理上的中心点。

URDF与Rviz集成应用

需求描述:

在 Rviz 中显示一个盒状机器人

结果演示:

实现流程：

准备:新建功能包，导入依赖
核心:编写 urdf 文件
核心:在 launch 文件集成 URDF 与 Rviz
在 Rviz 中显示机器人模型

1.创建功能包，导入依赖

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

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

urdf: 存储 urdf 文件的目录

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

config: 配置文件

launch: 存储 launch 启动文件

2.编写 URDF 文件

新建一个子级文件夹:urdf(可选)，文件夹中添加一个.urdf文件,复制如下内容:

<robot name="mycar">
    <link name="base_link">
        <visual>
            <geometry>
                <box size="0.5 0.2 0.1" />
            </geometry>
        </visual>
    </link>
</robot>

3.在 launch 文件中集成 URDF 与 Rviz

在launch目录下，新建一个 launch 文件，该 launch 文件需要启动 Rviz，并导入 urdf 文件，Rviz 启动后可以自动载入解析urdf文件，并显示机器人模型，核心问题:如何导入 urdf 文件? 在 ROS 中，可以将 urdf 文件的路径设置到参数服务器，使用的参数名是:robot_description,示例代码如下:

<launch>

    <!-- 设置参数 -->
    <param name="robot_description" textfile="$(find 包名)/urdf/urdf/urdf01_HelloWorld.urdf" />

    <!-- 启动 rviz -->
    <node pkg="rviz" type="rviz" name="rviz" />

</launch>

4.在 Rviz 中显示机器人模型

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

设置完毕后，可以正常显示了

5.优化 rviz 启动

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

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

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

<launch>
    <param name="robot_description" textfile="$(find 包名)/urdf/urdf/urdf01_HelloWorld.urdf" />
    <node pkg="rviz" type="rviz" name="rviz" args="-d $(find 报名)/config/rviz/show_mycar.rviz" />
</launch>

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