最新域名网站,中国建设银行甘肃省分行官网站,衡粘水佩网站建设,河南建设政务网参考引用 张虎#xff0c;机器人SLAM导航核心技术与实战[M]. 机械工业出版社#xff0c;2022.本博客未详尽之处可自行查阅上述书籍 一、编程基础篇
1. ROS 入门必备知识
ROS学习笔记#xff08;文章链接汇总#xff09;
2. C 编程范式
《21天学通C》读书笔记#xff0… 参考引用 张虎机器人SLAM导航核心技术与实战[M]. 机械工业出版社2022.本博客未详尽之处可自行查阅上述书籍 一、编程基础篇
1. ROS 入门必备知识
ROS学习笔记文章链接汇总
2. C 编程范式
《21天学通C》读书笔记文章链接汇总
3. OpenCV 图像处理
《OpenCV 计算机视觉编程攻略》学习笔记持续更新中
二、硬件基础篇
1. 机器人传感器
1.1 IMU
1.1.1 定义 惯性测量单元 (Inertial Measurement UnitIMU) 是用来测量惯性物理量的设备 比如测量加速度的加速度计、测量角速度的陀螺仪等由于 IMU 具有非常高的测量频率而相机、雷达、轮式里程计等测量频率较低因此往往将 IMU 与相机或雷达或轮式里程计做融合会得到更好的测量效果 航空航天、无人机、自动驾驶汽车、机器人、智能穿戴等领域广泛使用 IMU 进行运动测量和状态估计一般从 IMU 设备采集到的原始数据都存在较大的误差所以需要对测量到的原始数据进行标定和滤波
1.1.2 常见术语解释
IMU 惯性测量单元是测量物体三轴角速度和加速度的设备。一个IMU内可能会装有三轴陀螺仪和三轴加速度计来测量物体在三维空间中的角速度和加速度。严格来说 IMU 只为用户提供三轴角速度以及三轴加速度数据VRU 垂直参考单元是在 IMU 的基础上以重力向量作为参考用卡尔曼或者互补滤波等算法为用户提供有重力向量参考的俯仰角、横滚角以及无参考标准的航向角。通常所说的 6 轴姿态模块就属于这类系统MARG 是指能测量磁力、角速度、重力这三个物理量的传感器。可以说IMU 是 6 轴传感器而 MARG 是 9 轴传感器MARG 比 IMU 多了一个 3 轴的磁力计一般组合导航不用磁力计干扰问题不好解决AHRS 是指航姿参考系统即以地球重力场和磁场为参考计算导航物体的空间姿态即 “航姿”。AHRS 比 MARG 多了一个航姿计算的过程通常所说的 9 轴姿态传感器就属于这类系统GNSS/INS 组合导航系统是将两种或以上的导航设备组合在一起使用的导航系统利用全球卫星导航系统 (GNSS它是 GPS北斗GLONASS、GALILEO 等系统的统称) 与惯性导航 (Inertial Navigation System) 各自的优势进行算法融合能够提供更精确、全面的导航信息比如航姿信息位置信息、速度信息等
1.2 激光雷达LiDAR
1.2.1 定义
雷达是指利用探测介质探测物体距离的设备比如无线电测距雷达、激光测距雷达、超声波测距雷达等 由于激光具有很好的抗干扰性和直线传播特性因此激光测距具有很高的精度基于激光测距原理的激光雷达测距精度往往可以达到厘米级或毫米级激光雷达测距方式主要是三角测距和 TOF (Time of Fly飞行时间) 测距两种在软件层面上通常是在上位机电脑上运行 ROS 驱动程序获取雷达扫描数据对扫描数据进行滤波等必要的数据处理最后扫描数据就交给上层的建图、避障、导航等算法进行运算激光雷达数据与轮式里程计、IMU 和相机等进行多传感器融合往往效果更好
1.2.2 激光雷达的两种测距原理
三角测距 随着物体距离不断变远反射激光在图像传感器上的位置变化会越来越小也就是越来越难以分辨。这正是三角测距的大缺点物体距离越远测距误差越大 TOF 测距 由于光速传播太快要获取精确的传播时间很难。所以这种激光雷达自然成本也会高很多但是其测距精度很高 上面介绍测距原理时只是单个激光器也就是说只能扫描单个点这个装置也叫单线激光测距模组。激光雷达中将单线激光测距模组安装到旋转机构中就能对环境进行多点扫描 激光雷达中的旋转机构需要考虑的关键问题是通信与供电问题这也直接决定了激光雷达的性能和寿命。因为激光测距模组处于旋转状态所以模组不能直接用导线与底座连接行业内一般用滑环或光磁耦合实现连接 1、滑环是机械式的触点旋转过程中触点始终保持接触实现电路的连接导通缺点是使用久了会磨损机械触点也会导致旋转发出很多噪音2、光磁耦合不需要接触式的连接模组与底座通过电磁感应进行电能输送另外模组和底座之间有一一对应的发光二极管和光接收二极管用于光通信成本更高
1.2.3 常见激光雷达类型
单线激光雷达 单线激光模组和旋转机构构成了单线激光雷达单线激光雷达扫描点通常处在同一平面上的 360 度范围内因此也叫 2D 激光雷达 多线激光雷达 单线激光雷达只能扫描同一平面上的障碍信息也就是环境的某一个横截面的轮廓这样扫描的数据信息很有限。在垂直方向同时发射多束激光再结合旋转机构就能扫描多个横截面的轮廓这就是多线激光雷达也叫 3D 激光雷达 固态激光雷达 固态激光雷达的扫描不需要机械旋转部件而是用微机电系统、光学相控阵、脉冲成像等技术替代。固态激光雷达的优点是结构简单、体积小、扫描精度高、扫描速度快等缺点是扫描角度有限、核心部件加工难度大、生产昂贵等
1.2.4 激光雷达性能参数
激光线数 是指测距模组发射激光束的个数一般有单线、16 线、32线、64 线、128 线激光线数越多扫描到的信息越多相应的数据处理所消耗的计算资源也将更大 测距频率 也叫测距模组的采样率是指每秒模组能完成的测距操作的次数。采样率越高对雷达硬件的性能要求也越高 扫描频率 是指带动测距模组旋转的电机每秒钟转过的圈数扫描频率越高获取一圈扫描点数据帧的时间越短对障碍物的探测和避让实时性更好 测距量程 是对障碍物能有效探测的范围只有落在量程范围内的障碍物才能被探测到。小于量程下限值的范围就是盲区大于量程上限值的范围就是超量程区域。通常机器人会在必要部位安装其他避障传感器来弥补激光雷达的盲区问题 扫描角度 也可以说是雷达的探测视野通常都是 360° 全方位的探测视野。不过有一些激光雷达由于构造的原因有一些角度范围被外壳等结构遮挡所以探测视野会是 270° 或 180°比如当激光雷达被安装在机器人的正前方时由于雷达后面的视野完全被遮挡了所以有效的数据只有前面视野部分将雷达的扫描角度设置成 180° 就很有必要这样既能去除被遮挡区域的无效数据又能因数据量的减少而加快算法运算效率 距离分辨率 测距精度测距精度越高越有利于机器人的导航避障。基于三角测距原理的雷达测距精度在厘米级精度随测量距离增大而增大这也是三角测距的典型缺点。基于 TOF 测距原理的雷达测距精度可达毫米级当然价格也更高 角度分辨率 两个相邻扫描点之间的夹角由于雷达是通过旋转进行扫描的随着距离增加点云会越来越稀疏。如果角度分辨率比较低在扫描远距离物体时只能得到非常稀疏的几个点云这样的点云基本上没有什么用角度分辨率 扫描频率 × 360° / 测距频率 使用寿命 采用机械滑环连接的激光雷达连续工作寿命只有几个月而采用光磁耦合连接的激光雷达连续工作寿命可长达几年时间。滑环结构的雷达寿命短价格也便宜光磁耦合的雷达寿命长价格也高一些
1.2.5 激光雷达数据处理
上位机 ROS 驱动程序
将装配到机器人上的激光雷达用串口或网口与上位机进行连接上位机一般会运行 Ubuntu 和 ROS 系统ROS 解析雷达传回的数据并发布到 /scan 话题中这样其他 ROS 程序就可通过订阅该话题来获取雷达数据
扫描点处理
通过驱动程序获取到激光雷达的扫描数据后一般要对数据进行一些必要的预处理 单线激光雷达在 ROS 中发布的数据格式通常是 LaserScan 类型 其实就是用极坐标的方式表示扫描点的坐标值。单线激光雷达的数据处理相对容易比如过滤掉特定范围内的无效点、扫描角度范围截取、插值等如功能包 laser_filters 多线激光雷达在 ROS 中发布的数据格式通常是 PointCloud2 类型 PCL (Point Cloud Library点云库) 是处理点云数据非常有名的工具。PCL 中包含点云滤波、特征点提取、点云配准、点云分割、点云曲面重建等操作
扫描点时间同步
由于激光雷达安装在机器人上当机器人移动时激光雷达自身的旋转扫描就会受到影响 当机器人静止时激光雷达旋转一圈扫描到的点序列都是以机器人当前静止位置作为参考的激光雷达的测距误差仅来自测距方法本身当机器人移动时因为激光雷达并不知道自身处于移动状态所以激光雷达旋转一圈扫描到的点序列依然是以该帧时间戳时刻的机器人位置为参考此时激光雷达的测距误差除来自测距方法本身外还来自机器人运动产生的畸变 例如当机器人以雷达相同大小的角速度反方向旋转时雷达一帧扫描点 { P 0 , P 1 , P 2 , ⋯ } \{P_0,P_1,P_2,\cdots\} {P0,P1,P2,⋯} 其实是对 P 点的重复多次测量但是雷达并不知道自身的运动依旧把各个扫描点当成机器人在起始时间戳位置静止状态参考下的测量也就是雷达畸变数据点 { P 0 ′ , P 1 ′ , P 2 ′ , ⋯ } \{P_0^{\prime},P_1^{\prime},P_2^{\prime},\cdots\} {P0′,P1′,P2′,⋯}显然畸变数据远远偏离了实际情况如下左图所示再如当机器人匀速前进时雷达一帧扫描点 { P 0 , P 1 , P 2 , ⋯ } \{P_0,P_1,P_2,\cdots\} {P0,P1,P2,⋯} 其实是机器人在不同位置上的测量但是雷达并不知道自身的运动依旧把各个扫描点当成机器人在起始时间戳位置静止状态参考下的测量也就是雷达畸变数据点 { P 0 ′ , P 1 ′ , P 2 ′ ⋯ } \{P_0^{\prime},P_1^{\prime},P_2^{\prime}\cdots\} {P0′,P1′,P2′⋯}显然畸变数据远远偏离了实际情况如下右图 校正方法 纯估计法 不借助额外的传感器只利用激光雷达的数据对机器人的运动进行估计并利用估计的运动信息对雷达扫描数据进行补偿这就是纯估计法已知机器人中两帧雷达扫描数据 X i − 1 X^{i-1} Xi−1 和 X i X^{i} Xi求机器人的位置转移关系 T也就是激光里程计的问题最常用的求解方法是 ICP(Iterative Closest Point最近邻点迭代) 算法用位置转移关系 T 近似表示其当前的运动 V然后利用这个运动信息 V 对当前的雷达数据 X i X^{i} Xi 做补偿 里程计辅助法 采用外部的 IMU 或者轮式里程计来提供机器人的运动信息就可直接对雷达运动畸变做校正这就是里程计辅助法。IMU 虽然可以提供极高频率的姿态更新但是存在很高的累积误差选用轮式里程计会更稳定也存在累计误差但相对更小下图所示轮式里程计采集到的机器人位姿序列点为 { p 1 , p 2 , p 3 , ⋯ } \{p_1,p_2,p_3,\cdots\} {p1,p2,p3,⋯}激光雷达一顿扫描数据点为 { x 1 , x 2 , x 3 , ⋯ } \{x_1,x_2,x_3,\cdots\} {x1,x2,x3,⋯}由于里程计序列和激光雷达序列之间点的个数不匹配和时间上不对齐为了让里程计与激光雷达点之间能一一对应起来因此对 { p 1 , p 2 , p 3 , ⋯ } \{p_1,p_2,p_3,\cdots\} {p1,p2,p3,⋯} 做二次曲线插值得到 { p 1 ′ , p 2 ′ , p 3 ′ , ⋯ } \{p_1^{\prime},p_2^{\prime},p_3^{\prime},\cdots\} {p1′,p2′,p3′,⋯}接下来就可以利用对齐后的里程计位姿 p k ′ p_{k}^{\prime} pk′ 对激光雷达点 x k x_{k} xk 逐一进行校正。最后将校正后的雷达点 x k ′ x_k^{\prime} xk′ 转换回极坐标的形式 ( ρ , θ ) (\rho,\theta) (ρ,θ)重新发布出去就可以了 可以先利用 IMU 或轮式里程计进行初步校正再利用 ICP 迭代修正 1.3 相机
1.3.1 单目相机 特点 单目相机其实就是通常说的摄像头由镜头和图像传感器(CMOS 或 CCD)构成简单点说摄像头的原理就是小孔成像成像信息由图像传感器转换成数字图像输出利用对应的摄像头驱动程序就可以读取并显示该数字图像受摄像头制造偏差等因素的影响原始的数字图像存在畸变问题需要对摄像头进行标定最后图像数据可以被压缩成不同的形式便于传输和访问单目相机无法测量物体的深度信息 上位机 ROS 驱动程序 按照视频传输协议的不同相机设备的接口主要分为 DVP、LVDS、MIPI、USB 等接口类型将 USB 相机直接用 USB 线连接到上位机上位机运行 Ubuntu 和 ROS 系统在上位机上运行对应 ROS 驱动程序解析相机传回来的数据并将结果发布到 ROS 话题 cam_name/image_raw 之中这样上位机上的其他 ROS 程序就可以通过订阅该话题来获取图像数据
1.3.2 双目相机 单目相机无法测量物体点的深度信息用两个单目相机组成的双目相机就可测量深度信息也把双目相机叫深度相机 上位机 ROS 驱动程序 双目相机的 ROS 驱动图像发布主要有两种方式 一种是双目相机直接被上位机系统识别成两个独立的单目相机设备按单目相机的方法分别读取左、右相机的图像并分别发布到对应的话题另一种是双目相机上传的左图、右图拼接在起形成一张大图上位机可以直接发布这张拼接的大图也可以将大图拆分后分别发布
1.3.3 RGB-D 相机 双目相机虽然能测量深度信息但是需要事先找到同一物体点在左右相机中成像点对也就是要先匹配。匹配过程很容易受到光照强度等环境因素干扰在没有特征的环境中匹配会失效深度信息将无法测量 RGB-D 相机是主动测量深度的传感器受环境的干扰会小一些。RGB-D 相机一般有 3 个镜头中间的镜头是普通的摄像头采集彩色图像另外两个镜头分别用来发射红外光和接收红外光 有时候机器人上只安装了 RGB-D 相机而没有激光雷达。如果要运行激光 SLAM可以将 RGB-D 相机的深度图转换成激光雷达扫描图就可以运行激光 SLAM 了。转换用的功能包是 depthimage_to_laserscan 转换原理按某个高度平面截取深度图截取出来的轮廓作为激光雷达扫描点发布 由于 RGB-D 相机的视野角度小就激光 SLAM 建图效果看没有 360 度扫描的激光雷达建的地图好。所以通常不用 RGB-D 相机建图而只做避障用
1.4 带编码器的减速电机
1.4.1 电机和编码器
按照电机的电源类型分为直流电机、交流电机按照电机的换相方式分为有刷电机、无刷电机按照电机转子的构造可以分为内转子电机、外转子电机电机在实际的使用中往往配合减速箱一起使用来提高输出力矩和载重性能。减速箱有普通的减速箱和行星减速箱。行星减速箱的优势是在同等减速比条件下减速箱体积可以更小一般一些对体积敏感的场景用得多编码器是对电机转速测量反馈的重要部件有霍尔编码器、光电编码器、碳刷编码器通常采用正交编码形式进行信号输出也就是 AB 两相信号正交输出有些还等会有 Z 相信号对转过一圈进行脉冲输出
1.4.2 电机控制主板
上、下位机主板方案对比 像树莓派、Jetson-tx2 等上位机主板一般都会搭载 Linux 等操作系统虽然操作系统中运行的程序看上去是实时的可是硬件 I/O 这类操作要求的是绝对的实时性并且不能出现任何差错尤其是电机这种持续运行的硬件带操作系统的上位机主板就不能达到这些要求而 STM32、Arduino 这一类的下位机单片机原本就是为硬件控制驱动等应用而生的运行的是裸机程序由硬件电路直接保证绝对实时性并且用下位机单片机来控制电机可以实现功能解耦合更利于整个上层大系统的维护 PID 控制 如果给定一个指定的 PWM 值来控制电机以速度 V 转动实际情况中器件偏差噪声干扰、负载变化等因素电机并不会保持运行速度 V。为了达到控制电机稳定转动的目的就需要引入反馈控制机制电机如果转慢了就加大控制量如果转快了就减小控制量。而 PID (Proportional Integral Derivative比例积分微分) 是首选算法电机速度控制属于速度环控制以 PWM 信号控制输人以编码器进行速度反馈选用增量型 PID 会更合适。等实际的电机控制系统软硬件都搭建完毕后还需要对 PID 的 3 个参数进行整定以使控制达到更佳的效果 通信协议 在单片机上实现好电机的 PID 控制后还需要将速度控制封装成接口通过特定的协议让单片机和上位机通信 单片机反馈编码器值给上位机上位机发送速度控制值和一些配置参数给单片机 这里上位机与单片机采用串口通信 基于串口通信的通信协议可以采用 ROS 社区提供的现成的 rosserial 库中封装的协议也可以自定义一套符合自己项目需要的协议 1.4.3 轮式里程计
一旦单片机按照通信协议封装好后对于上位机来说下位机的整个系统其实就是一个 API 接口只需要按照协议调用就行了。有了这个 API 接口就可以开发上位机的 ROS 驱动程序如下图所示在驱动程序运行中一方面按照通信协议与下位机交换数据另一方面向 ROS 中发布里程计话题 /odom 和订阅速度控制话题 /cmd_vel。通信协议数据的收发、解析相对容易但根据下位机传回来的编码值求解里程计是核心与难点利用轮式编码器反馈经过航迹推演算法得到底盘当前时刻位姿整个过程就是机器人技术中常说的轮式里程计 2. 机器人主机
2.1 X86 与 ARM 主机对比
X86 和 ARM 是指计算机处理器的两种架构 个人台式电脑和笔记本电脑基本都是 X86 架构而像手机、智能终端、物联网设备等一般都是 ARM 架构 X86 是通用型架构基本上对大多数厂商的硬件、驱动程序、系统软件都有很好的支持兼容性比较好ARM 是精简型架构因为具有精简指令的灵活性和低功耗等特点广泛应用在嵌人式领域但硬件厂商、驱动程序、系统软件这些就没有像 X86 那样好的支持性了很多都需要自己裁剪、移植来适应具体的主板型号这里讨论的是机器人主机台式电脑和笔记本电脑不在讨论范围内 如果说要在机器人上选择 X86 主机推荐使用 Intel-NUC它的体积比较小性能较好缺点是功耗高价格也远高于同性能的 ARM 主机ARM 主机非常适合在机器人上使用推荐三款 ARM 主机即树莓派 3B、RK3399、Jetson-tx2 如果是像扫地机器人那样只需要运行激光 SLAM就可以选择树莓派 3B 作为机器人的主机如果要运行视觉 SLAM就需要选中高端的 RK3399 作为机器人的主机如果除了 SLAM 外机器人还需要运行神经网络等深度学习算法来实现人脸识别、语言交互、语义地图等就需要选择高端的 Jetson-tx2 作为机器人的主机
2.2 分布式架构主机
2.2.1 ROS 网络通信
ROS 网络通信是一种分布式的计算机通信方式可以为运行在不同主机中的 ROS 节点间通信提供接口 机器人和工作台上的 ROS 节点是运行在 Ubuntu 系统上的一般基于 roscpp 和 rospy 库来编写手机上的 ROS 节点是运行在 Android 系统上的一般基于 rosjava 库来编写不管这些 ROS 节点是运行在什么设备中只要将各个设备连接到同一台路由器组成一个局域网并在每台设备上都配置好 ROS 网络通信的环境变量ROS 网络通信就设置完成 ROS 网络通信是中心式结构也就是参与 ROS 网络通信的所有主机必须指定一台主机作为 master (主节点) 负责整个 ROS 网络通信的管理工作而参与 ROS 网络通信的所有主机都要向外声明自己的 host 身份 也就是每台主机中都要设置 master 和 host 两个环境变量master 和 host 的取值均为局域网内主机的真实 IP 地址一般以机器人作为 master 即所有主机中的 ROS_MASTER_URI 环境变量取值都要设置成机器人的 IP 地址值而参与 ROS 网络通信的所有主机各自都是一台 host也就是每台主机中的 ROS_HOSTNAME 环境变量取值都设置成该主机自己的 IP 地址值 2.2.2 机器人程序远程开发
通过 ssh 工具从工作台远程登录到机器人主机然后就可远程编辑和启动/停止机器人上的程序远程登录命令如下$ ssh 主机用户名主机ip远程登录到机器人后在登录终端只能用 vim 编辑一些简单的程序代码。如果要修改的代码涉及的文件比较复杂可以将机器人文件远程挂载到工作台然后在工作台直接打开编辑远程登录到机器人后该终端下执行的所有命令其实最终都是在机器人本地运行只不过通过 ssh 进行命令的传递。在登录后的终端下运行 rviz 这类 GUI 程序而报错因为 ssh 只能传递文本信息到工作台端无法传递 GUI 信息
3. 机器人底盘
3.1 两轮差速模型 两轮差速模型可以说是最简单的底盘模型在底盘的左右两边平行安装两个由电机驱动的动力轮考虑到至少需要 3 点才能稳定支撑底盘上还需要安装用于支撑的万向轮 3.1.1 前向运动学
底盘中各个动力轮的速度可以由编码器值计算得到那么通过各个轮子的速度求底盘整体的运动速度即前向运动学。已知左动力轮的线速度为 V L V_L VL右动力轮的线速度为 V R V_R VR动力轮的线速度可以用编码器计数值计算如下式所示 V M P ⋅ N ⋅ 2 π ⋅ R ⋅ 1 Δ t V\frac{M}{P}\cdot N\cdot2\pi\cdot R\cdot\frac{1}{\Delta t} VPM⋅N⋅2π⋅R⋅Δt1 其中 M 为采样周期内编码器的计数值P 为编码器的线数即编码盘旋转一圈触发的总脉冲数N 为电机减速比为小于 1 的分数R 为轮子的半径 Δ t \Delta t Δt 为编码器采样周期 在很短的时间 t 内底盘可以看成绕 O 点做圆周运动两个圆周的半径分别为 ρ \rho ρ 和 ρ d \rhod ρd其中 d 为两轮轴距两个轮子走过的路程分别为 S L S_L SL 和 S R S_R SR转过的角度为 φ \varphi φ底盘速度瞬心走过的路程为 s S L S_L SL 和 S R S_R SR 满足 s R φ ⋅ ( ρ d ) V R ⋅ t s L φ ⋅ ρ V L ⋅ t \begin{aligned}s_\mathrm{R}\varphi\cdot(\rhod)V_\mathrm{R}\cdot t\\s_\mathrm{L}\varphi\cdot\rhoV_\mathrm{L}\cdot t\end{aligned} sRsLφ⋅(ρd)VR⋅tφ⋅ρVL⋅t φ s R − s L d V R − V L d ⋅ t s φ ⋅ ( ρ d 2 ) φ ⋅ ρ φ ⋅ d 2 s L s R − s L 2 s R s L 2 V R V L 2 ⋅ t \begin{aligned} \varphi\frac{s_{\mathrm{R}}-s_{\mathrm{L}}}{d}\frac{V_{\mathrm{R}}-V_{\mathrm{L}}}{d}\cdot t \\ s\varphi\cdot\left(\begin{array}{c}\rho\frac{d}{2}\\\end{array}\right)\varphi\cdot\rho\varphi\cdot\frac{d}{2}s_{L}\frac{s_{R}-s_{L}}{2}\frac{s_{R}s_{L}}{2}\frac{V_{R}V_{L}}{2}\cdot t \end{aligned} φdsR−sLdVR−VL⋅tsφ⋅(ρ2d)φ⋅ρφ⋅2dsL2sR−sL2sRsL2VRVL⋅t
由于运动时间 t 很小利用极限的思想 φ \varphi φ 趋近于 0s 也趋近于 0那么 s 可以等效为底盘速度瞬心轨迹圆弧切线方向的一小段位移有了这个近似假设条件用 s 和 φ \varphi φ 直接除以 t 就可以求出底盘的整体速度 v x v_{x} vx 与 ω z \omega_{z} ωz [ v x ω z ] [ 1 2 1 2 − 1 d 1 d ] [ V L V R ] \begin{bmatrix}v_x\\\omega_z\end{bmatrix}\begin{bmatrix}\dfrac{1}{2}\dfrac{1}{2}\\-\dfrac{1}{d}\dfrac{1}{d}\end{bmatrix}\begin{bmatrix}V_\mathrm{L}\\V_\mathrm{R}\end{bmatrix} [vxωz] 21−d121d1 [VLVR] 3.1.2 逆向运动学 [ V L V R ] [ 1 − d 2 1 d 2 ] [ v x ω z ] \left[\begin{matrix}V_{\mathrm{L}}\\V_{\mathrm{R}}\end{matrix}\right]\left[\begin{matrix}1-\dfrac{d}{2}\\1\dfrac{d}{2}\end{matrix}\right]\left[\begin{matrix}v_{x}\\\omega_{z}\end{matrix}\right] [VLVR] 11−2d2d [vxωz]
3.1.3 轮式里程计 底盘在运动过程中轮式编码器可以反馈每个轮子的转速经过前向运动学公式解算就可以得到底盘的整体速度 v x v_x vx 和 ω z \omega_z ωz。基于 v x v_x vx 和 ω z \omega_z ωz就可以由前一时刻底盘的位姿 P k − 1 \boldsymbol{P}_{k-1} Pk−1 推算出当前时刻底盘的位姿 P k \boldsymbol{P}_{k} Pk这个推算过程也叫 “航迹推演算法”由于底盘不是严格的质点所以底盘的位姿代表的是底盘速度瞬心在世界坐标系的位姿 利用轮式编码器反馈经过航迹推演算法得到底盘当前时刻位姿整个过程就是机器人技术中常说的轮式里程计 下面具体分析一下轮式里程计的解算过程如下图所示 机器人学中通常采用右手坐标系底盘的正前方为 x 轴正方向、底盘的正左方为 y 轴正方向、底盘的正上方为 z 轴正方向、航向角 θ \theta θ 以 x 轴为 0 度角并沿逆时针方向增大 一般在底盘上电时刻在底盘的位姿处建立里程计坐标系 (odom)随时间推移底盘的实时位姿 P 0 , P 1 , P 2 , P 3 , ⋯ , P n P_0, P_1, P_2, P_3,\cdots, P_n P0,P1,P2,P3,⋯,Pn 连接起来就形成了底盘的航行轨迹。同时底盘自身的坐标系 (robot) 建立在其速度瞬心处 建立起 robot 坐标系中的速度量 v x v_x vx 和 ω z \omega_z ωz 与 odom 坐标系中的底盘位姿 P k \boldsymbol{P}_{k} Pk 之间的关系就能解算出轮式里程计。在二维平面中移动的底盘可以用 P [ X , Y , θ ] T \boldsymbol{P}[X,Y,\theta]^\mathrm{T} P[X,Y,θ]T 表示底盘的坐标和航向即底盘的位姿那么位姿 P \boldsymbol{P} P 满足下式所示微分方程 P ˙ [ X ˙ Y ˙ θ ˙ ] [ cos θ − s i n θ 0 sin θ cos θ 0 0 0 1 ] [ v x v y ω z ] \dot{\boldsymbol{P}}\left[\begin{array}{c}\dot{X}\\\dot{Y}\\\dot{\theta}\end{array}\right]\left[\begin{array}{ccc}\cos\theta-sin\theta0\\\sin\theta\cos\theta0\\001\end{array}\right]\left[\begin{array}{c}v_x\\v_y\\\omega_z\end{array}\right] P˙ X˙Y˙θ˙ cosθsinθ0−sinθcosθ0001 vxvyωz 其中两轮差分底盘不发生侧向滑动故 v y 0 v_y0 vy0 将 P ˙ \dot{\boldsymbol{P}} P˙ 在时间 t 上进行积分就可以求出底盘实时的位姿也就是里程计信息。实际情况下需要在离散时间域上进行计算由于底盘相邻两位姿相隔时间 Δ t \Delta{t} Δt 很小积分运算可以用下式的累加运算来替代 P k P k − 1 P ˙ k − 1 ⋅ Δ t \boldsymbol{P}_k\boldsymbol{P}_{k-1}\dot{\boldsymbol{P}}_{k-1}\cdot\Delta t PkPk−1P˙k−1⋅Δt 由于里程计提供底盘在 odom 坐标系中的位姿而 odom 坐标系与底盘上电启动时所处的地方有关也就是说里程计中的底盘位姿是一个相对值。另外里程计随时间的推移存在较大的累积误差所以一般都是取不同时刻之间的里程计差值并应用于其他算法。由于轮式里程计只能提供底盘的局部位姿并且存在较大累积误差因此需要结合地图及 SLAM 重定位技术来获取全局位姿
3.2 四轮差速模型
四轮差速模型与两轮差速模型非常相似主要靠左右两边轮子的转速差实现转弯同一边的两个轮子的转速是完全一样的即前轮和后轮的速度是同步的实现前、后轮速度同步的方式主要有 3 种 履带连接方式直接将前轮和后轮用履带进行连接在机械上实现前后轮速度同步 履带底盘因接触地面面积的增大而具有很好的越野性能但也存在较大弊端因此在对轮式里程计精度要求较高的应用场景很少选用履带底盘 链条连接方式用链条将前、后轮的输出轴连接在一起在机械上实现前后轮速度同步电控方式在玩具和简易底盘等要求低成本的场景下为了避免设计复杂的机械结构常常使用 4 个独立的电机控制 4 个轮子并通过控制主板实现前轮和后轮的速度同步 后两种方式是各个轮子单点接触地面所以轮式里程计精度比履带底盘中的要高 在有些对底盘控制和里程计精度要求不是特别高的场景也常常将这种四轮底盘简单等效为两轮差速模型来处理。等效的条件是轮子不发生侧向滑动当底盘的长宽比足够大时也就是前轮、后轮距离很近左轮、右轮距离很远底盘自转时才会足够顺畅保证轮子不发生侧向滑动。但是实际四轮差速模型应用中不可能把底盘的长宽比设计得过大所以轮子转弯会存在侧向滑动。为了得到更高精度的里程计就需要在四轮差速模型的侧向滑动情况下对其前向运动学、逆向运动学和轮式里程计进行讨论
3.2.1 前向运动学
由于前、后轮的速度是同步的可以以底盘几何中心 COG 沿 y 轴方向上的点 ICR 作为整个底盘进行圆周运动时的圆心ICR 与 COG 的距离大小与圆周运动角速度大小有关4 个轮子到 ICR 的距离为 d i d_i di轮子的实际速度 v i v_i vi 是侧向滑动速度 v i y v_{iy} viy 和预设目标速度 v i x v_{ix} vix 的合成速度这里的 i 1,2,3,4在侧向滑动的四轮底盘中底盘的速度瞬心在其质心 COM 处而 COM 与 COG 往往是不重合的。可以用 COM 位置处的线速度 v c v_c vc 和角速度 ω c \omega_c ωc 表示整个底盘的运动速度其中 COM 到 ICR 的距离为 d c d_c dc。 v c v_c vc 垂直于 ICR-COM 线段 v c v_c vc 不仅有预设目标速度 v c x v_{cx} vcx 分量还有侧向滑动速度 v c y v_{cy} vcy 分量底盘中左轮、右轮轴距为c点 COM 与底盘后端及前端的距离分别为 a 和 b 四轮差速底盘的前向运动学关系推理略 [ v c x ω c ] [ 1 2 1 2 − 1 c 1 c ] [ V L V R ] \begin{bmatrix}v_\mathrm{cx}\\\omega_\mathrm{c}\end{bmatrix}\begin{bmatrix}\dfrac{1}{2}\dfrac{1}{2}\\-\dfrac{1}{c}\dfrac{1}{c}\end{bmatrix}\begin{bmatrix}V_\mathrm{L}\\V_\mathrm{R}\end{bmatrix} [vcxωc] 21−c121c1 [VLVR]
3.2.2 逆向运动学 [ V L V R ] [ 1 − c 2 1 c 2 ] [ v c x ω e ] \begin{bmatrix}V_\mathrm{L}\\V_\mathrm{R}\end{bmatrix}\begin{bmatrix}1-\dfrac{c}{2}\\1\dfrac{c}{2}\end{bmatrix}\begin{bmatrix}v_\mathrm{cx}\\\omega_\mathrm{e}\end{bmatrix} [VLVR] 11−2c2c [vcxωe]
3.2.3 轮式里程计
基本同两轮模型此处不赘述
3.3 阿克曼模型
阿克曼底盘通过前轮的机械转向让底盘中的 4 个轮子在基本不发生侧向滑动的情况下能顺畅地转弯但缺点是阿克曼底盘不能原地旋转也就是最小转弯半径不为 0在直线行驶时4 个车轮的轴线互相平行。在进行转弯时需要让 4 个车轮只沿车轮切线方向前进而不发生侧移以避免侧移给车轮带来的巨大磨损。即 4 个轮子转弯弧线必须处于共同的圆心 ICR 之上此时前轮内侧转向角 δ i n \delta_{\mathrm{in}} δin 会大于外侧转向角 δ o u t \delta_{\mathrm{out}} δout。在阿克曼转向几何中利用梯形四连杆的相等曲柄控制前轮的转向以实现 δ i n \delta_{\mathrm{in}} δin 和 δ o u t \delta_{\mathrm{out}} δout 需要的约束关系避免车轮发生侧移让 4 个车轮能顺畅地转弯 3.3.1 前向运动学
在前轮转向角 δ i n \delta_{\mathrm{in}} δin 和 δ o u t \delta_{\mathrm{out}} δout 不变的瞬时状态下4 个轮子绕同一个点 ICR 做圆周运动。左后轮、右后轮的线速度为 V L V_L VL和 V R V_R VR后轮中轴位置 O b a c k O_{back} Oback 为底盘的速度瞬心 O b a c k O_{back} Oback 位置处的线速度 v b a c k v_{back} vback 和角速度 ω b a c k \omega_{back} ωback 为底盘的整体运动速度 [ v b a c k ω b a c k ] [ 1 2 1 2 − 1 d 1 d ] [ V L V R ] \left[\begin{matrix}v_\mathrm{back}\\\omega_\mathrm{back}\end{matrix}\right]\left[\begin{matrix}\frac{1}{2}\frac{1}{2}\\-\frac{1}{d}\frac{1}{d}\end{matrix}\right]\left[\begin{matrix}V_\mathrm{L}\\V_\mathrm{R}\end{matrix}\right] [vbackωback][21−d121d1][VLVR] 3.3.2 逆向运动学 [ V L V R ] [ 1 − d 2 1 d 2 ] [ v b a c k ω b a c k ] \begin{bmatrix}V_{\mathrm{L}}\\V_{\mathrm{R}}\end{bmatrix}\begin{bmatrix}1-\dfrac{d}{2}\\1\dfrac{d}{2}\end{bmatrix}\begin{bmatrix}v_{\mathrm{back}}\\\omega_{\mathrm{back}}\end{bmatrix} [VLVR] 11−2d2d [vbackωback] 在阿克曼底盘中除了需要求左、右后轮的速度外还需要求出前轮的转向角。前轮的转向角可以用两个前轮中轴位置的平均转向角 δ \delta δ 表示 tan δ l R \text{tan}\delta\frac lR tanδRl 其中 R v b a c k ω b a c k R\frac{v_{\mathrm{back}}}{\omega_{\mathrm{back}}} Rωbackvback 从而求出 δ arctan ( l ⋅ ω b a c k v b a c k ) \delta\arctan\left(\frac{l\cdot\omega_{\mathrm{back}}}{v_{\mathrm{back}}}\right) δarctan(vbackl⋅ωback) 求出的转向角 δ \delta δ 不是最终控制量。还要将转向角 δ \delta δ 映射成舵机的输出舵量 Angle才能最终实现底盘转向。而转向角 δ \delta δ 与舵量 Angle 之间的映射关系与舵机及转向悬臂具体安装位置有关系通常都是通过实验标定法获取到转向角 δ \delta δ 与舵量 Angle 之间的映射表 δ \delta δ-Angle
3.3.3 轮式里程计
基本同两轮模型此处不赘述
3.4 全向模型
速度向量约束 底盘的速度受到约束的原因是普通轮子不能侧向移动只有切向移动一个自由度。如果把轮子换成包含切向和侧向两个自由度的轮子则底盘速度不受约束也就成了全向底盘。在业界把这种具有两个自由度的轮子称为麦克纳姆轮
3.4.1 前向运动学
全向底盘可以用底盘中心处的线速度 v ⃗ \vec{v} v 和角速度 ω ⃗ \vec{\omega} ω 描述底盘的整体运动其中线速度 v ⃗ \vec{v} v 由分量 v x ⃗ \vec{v_x} vx 和 v y ⃗ \vec{v_y} vy 合成 v ⃗ \vec{v} v 可以是任何方向即取值不受约束底盘中 4 个轮子的速度分别为 v 1 ⃗ \vec{v_1} v1 、 v 2 ⃗ \vec{v_2} v2 、 v 3 ⃗ \vec{v_3} v3 和 v 4 ⃗ \vec{v_4} v4 。底盘前轮、后轮轴距为 2a左轮、右轮轴距为 2b 前向运动学公式 [ v x v y ω ] 1 4 [ 1 1 1 1 − 1 1 1 − 1 − 1 a b 1 a b − 1 a b 1 a b ] [ V 1 V 2 V 3 V 4 ] \begin{bmatrix}v_x\\v_y\\\omega\end{bmatrix}\dfrac14\begin{bmatrix}1111\\-111-1\\-\dfrac1{ab}\dfrac1{ab}-\dfrac1{ab}\dfrac1{ab}\end{bmatrix}\begin{bmatrix}V_1\\V_2\\V_3\\V_4\end{bmatrix} vxvyω 41 1−1−ab111ab111−ab11−1ab1 V1V2V3V4
3.4.2 逆向运动学 [ V 1 V 2 V 3 V 4 ] [ 1 − 1 − ( a b ) 1 1 ( a b ) 1 1 − ( a b ) 1 − 1 ( a b ) ] [ v x v y ω ] \begin{bmatrix}V_1\\V_2\\V_3\\V_4\end{bmatrix}\begin{bmatrix}1-1-(ab)\\11(ab)\\11-(ab)\\1-1(ab)\end{bmatrix}\begin{bmatrix}v_x\\v_y\\\omega\end{bmatrix} V1V2V3V4 1111−111−1−(ab)(ab)−(ab)(ab) vxvyω
3.4.3 轮式里程计
基本同两轮模型此处不赘述
三、SLAM 篇
1. SLAM 发展简史
最初机器人定位问题和机器人建图问题是被看成两个独立的问题来研究 定位问题的关键是必须事先给定环境地图 机器人定位问题是在已知全局地图的条件下通过机器人传感器测量环境利用测量信息与地图之间存在的关系求解机器人在地图中的位姿比如分拣仓库中地面粘贴的二维码路标就是人为提供给机器人的环境地图路标信息机器人只需要识别二维码并进行简单推算就能求解出当前所处的位姿 建图问题的关键是必须事先给定机器人观测时刻的全局位姿 机器人建图问题是在已知机器人全局位姿的条件下通过机器人传感器测量环境利用测量地图路标时的机器人位姿和测量距离及方位信息求解出观测到的地图路标点坐标值比如装载了 GPS 定位的测绘飞机飞机由 GPS 提供全局定位信息测量设备基于 GPS 定位信息完成对地形的测绘。很显然这种建立在环境先验基础之上的定位和建图具有很大的局限性 将机器人放置到未知环境前面这种上帝视角般的先验信息将不复存在机器人将陷入一种进退两难的局面即所谓的 “先有鸡还是先有蛋” 的问题。如果没有全局地图信息机器人位姿将无法求解没有机器人位姿地图又将如何求解呢? CMU-自主探索导航系统TARE FAR Planner学习-All in one 1986年Smith 和 Cheeseman 将机器人定位问题和机器人建图问题放在基于概率论理论框架之下进行统一研究其中有两个开创性的点 第一是采用了基于概率论理论框架对机器人的不确定性进行讨论第二是将定位和建图中的机器人位姿量与地图路标点作为统一的估计量进行整体状态估计这算得上是 SLAM 问题研究的起源 2006年Durrant-Whyte 和 Bailey 在其研究课题中首次使用 SLAM 这一名词进行表述给 SLAM 问题制定了详细的概率理论分析框架并对 SLAM 问题中的计算效率、数据关联、收敛性、一致性等进行了讨论可以认为这是 SLAM 问题真正进入系统性研究的元年 古典 SLAM 时期 将机器人定位和建图问题转换成状态估计问题在概率框架之中展开研究利用扩展卡尔曼滤波(EKF)、粒子滤波(PF)等滤波方法来求解 现代 SLAM 时期 在贝叶斯网络中采用滤波法求解 SLAM 的方法需要实时获取每一时刻的信息并把信息分解到贝叶斯网络的概率分布中去可以看出滤波方法是一种在线 SLAM 系统计算代价非常大鉴于滤波方法计算代价昂贵这一前提机器人只能采用基于激光等观测数据量不大的测距仪并且只能构建小规模的地图这是古典 SLAM 鲜明的特征为了进行大规模建图在因子图中采用优化方法求解 SLAM 的方法被提出优化方法的思路与滤波方法恰恰相反它只是简单地累积获取到的信息然后利用之前所有时刻累积到的全局性信息离线计算机器人的轨迹和路标点即优化方法是一种完全 SLAM 系统
2. 数据关联、收敛和一致性
2.1 数据关联
SLAM 建图是一个增量的过程随着机器人不断对环境的探测环境路标信息需要不断被加入已构建出来的地图中。所谓数据关联就是将传感器观测数据与已构建出来的地图进行匹配判断传感器观测数据中哪些路标特征是新的、哪些是旧的并将新路标特征数据有效地融入地图中
2.2 收敛
收敛性用于衡量 SLAM 系统在理论上的可行性。由于机器人观测模型和运动模型都具有不确定性所以将 SLAM 放在概率框架下讨论并利用状态估计去求解即估计量都是带有不确定性的估计值。由路标特征组成的地图是实际环境特征的估计估计出来的路标特征与实际环境特征存在偏差的原因是观测存在不充分性。换句话说只要给机器人足够的机会对环境进行彻底的观测所得到地图的不确定性将最终消除即构建出来的地图路标特征收敛于实际环境特征
2.3 一致性
不仅要关注估计问题的收敛性还需要关注收敛的一致性。收敛一致性讨论的是估计量收敛于实际数值的问题下图展示了收敛一致性问题 其中 θ \theta θ 是待估计参数真值 θ ^ \hat{\theta} θ^ 是估计量 Z { z 1 , z 2 , ⋯ , z k } Z\{z_1,z_2,\cdots,z_k\} Z{z1,z2,⋯,zk} 是当前能获取到的 k 个观测值第一种收敛并不能保证估计量的取值一定与真值一致而第二种收敛能保证估计量的取值一定与真值一致。考虑到机器人中参数随时间变化的动态性比如机器人的位姿会经常移动那么估计不仅要具有良好的一致性还要能在少量观测值条件下快速收敛 3. 激光 SLAM 系统
3.1 TF 系统简介
45.初识ROS的TF系统
3.2 里程计简介
46.什么是里程计
3.3 Gmapping 算法
用 RBPF 粒子滤波器来求解 SLAM 问题也有人基于 RBPF 来研究构建栅格地图grid map的 SLAM 算法它就是 ROS 中开源功能包 Gmapping 算法。它是一种基于粒子滤波的算法但对 RBPF 的建议分布proposal distribution和重采样进行了改进
3.3.1 Gmapping 代码框架
Gmapping 算法用了 2 个 ROS 功能包来组织代码分别为 slam_gmapping 功能包和 openslam_gmapping 功能包 其中 slam_gmapping 功能包用于实现算法的 ROS 相关接口其实 slam_gmapping 本身没有实质性的内容是一个元功能包具体实现被放在其所包含的 gmapping 功能包中单线激光雷达数据通过 /scan 话题输入 gmapping 功能包里程计数据通过 /tf 关系输入 gmapping 功能包gmapping 功能包通过调用 openslam_gmapping 功能包中的建图算法将构建好的地图发布到 /map 等话题 3.3.2 Gmapping 程序调用流程
程序 main() 函数很简单就是创建了一个 SlamGMapping 类的对象 gn。然后SlamGMapping 类的构造函数会自动调用 init() 函数执行初始化包括创建 GridSlamProcessor 类的对象 gsp_ 和设置 Gmapping 算法参数 SlamGMapping 类在 gmapping 功能包中实现GridSlamProcessor 类在 openslam_gmapping 功能包中实现而 GridSlamProcessor 类以成员变量的形式被 SlamGMapping 类调用接着调用 SlamGMapping 类的 startLiveSlam() 函数就可以进行在线 SLAM 建图了。startLiveSlam 函数首先对建图过程所需要的 ROS 订阅和发布话题进行了创建然后开启双线程进行工作 其中 laserCallback 线程在激光雷达数据的驱动下对雷达数据进行处理并更新地图其中调用到的 GridSlamProcessor 类的 processScan 函数就是改进的 RBPF 算法具体实现而 publishLoop 线程负责维护 map-odom 之间的 tf 关系 3.3.3 安装与运行
安装 Gmapping 依赖库# 安装 openslam-gmapping 和 slam-gmapping 功能包及其依赖
$ sudo apt install ros-melodic-openslam-gmapping ros-melodic-gmapping
# 卸载 openslam-gmapping 和 slam-gmapping 功能包但保留其依赖
$ sudo apt remove ros-melodic-openslam-gmapping ros-melodic-gmapping源码下载并编译安装# 创建工作空间
$ mkdir -p gmapping_ws/src
$ cd gmapping_ws/src# 源码下载
$ git clone https://github.com/ros-perception/slam_gmapping.git
$ cd slam_gmapping
$ git clone https://github.com/ros-perception/openslam_gmapping.git
$ cd ~/gmapping_ws/# 编译安装
$ catkin_make -DCATKIN_WHITELIST_PACKAGESopenslam_gmapping
$ catkin_make -DCATKIN_WHITELIST_PACKAGESgmapping运行示例程序$ roslaunch gmapping slam_gmapping_pr2.launch# bag 下载地址 http://download.ros.org/data/amcl/?CD;OA
$ rosbag play basic_localization_stage_indexed.bag# 在 rviz 中订阅地图话题 /map 即可看到地图
$ rviz3.3.4 注意事项
订阅的话题 tf(tf/tfMessage)坐标系转换关系 雷达坐标系laser→ 底盘坐标系base_link 雷达坐标系名称要和 /scan 话题中雷达数据包的 header 里的 frame_id 一致 $ rostopic echo /scan --noarr底盘坐标系base_link→ 里程计坐标系odom scan(sensor_msgs/LaserScan) 激光雷达数据话题 发布的话题 map_metadata(nav_msgs/MapMetaData) 地图加载时间、地图分辨率、地图的宽和高、地图原点坐标 map(nav_msgs/OccupancyGrid) 栅格地图数据 ~entropy(std_msgs/Float64) 机器人定位的置信度值越大则机器人定位错误的可能性越大 提供的 TF 变换关系map → odom轮式里程计累积误差的动态修正量 激光雷达 SLAM 输出的是 map → base_footprint 的 TFbase_link 是与机器人中心重合而 base_footprint 是 base_link 在地面的投影里程计输出的是 odom → base_footprint 的 TFGmapping 算法核心思想先使用里程计推算机器人的位移然后通过激光雷达点云配准算法用于修正里程计的误差即 map → odom
3.3.5 离线运行
# 录制数据集
$ rosbag record -O test.bag /scan /tf /tf_static# 启动建图记得打开 use_sim_time 参数
$ roslaunch gmapping slam_gmapping.launch# 播放数据集
$ rosbag play test.bag3.4 Cartographer 算法
Gmapping 代码实现相对简洁但基于滤波方法的 SLAM 系统无法构建大规模的地图而基于优化的方法可以基于优化的方法实现的激光 SLAM 算法也有很多比如 Hector、Karto、Cartographer 等而 Cartographer 是其中获好评最多的算法其开发团队来自谷歌公司代码的工程稳定性较高是少有的兼具建图和重定位功能的算法基于优化方法的 SLAM 系统通常采用前端局部建图、闭环检测和后端全局优化这种经典框架 3.4.1 Cartographer 代码框架
Cartographer 算法主要由 3 部分组成 1. cartographer_ros 功能包
cartographer_ros 功能包用于实现算法的 ROS 相关接口Cartographer 算法是一个支持多激光雷达、IMU、轮式里程计、GPS、环境已知信标的传感器融合建图算法 激光雷达数据可以通过多种接口输入算法用户可根据自己激光雷达的数据类型选择合适的话题 (/scan、/echoes 或/points2)进行输入由于 Cartographer 算法支持 2D 和 3D 建图所以支持单线激光雷达和多线激光雷达IMU 数据通过话题 /imu 输入算法轮式里程计数据通过话题 /odom 输入算法GPS 数据通过话题 /fix 输人算法环境已知信标数据通过话题/landmarks 输人算法Cartographer 支持多种模式建图既可以只采用激光雷达数据建图也可以采用激光雷达数据 IMU、激光雷达 轮式里程计、激光雷达 IMU 轮式里程计等模式建图并且还可以用 GPS 和环境已知信标辅助建图过程。Cartographer 的工作模式和各种参数采用 *.lua 配置文件进行配置Cartographer 建图结果通过 2 个话题输出其中话题 /scan_matched_points2 输出 scan-to-submap 匹配结果话题 /submap_list 输出整个 Cartographer 最终地图结果 cartographer_ros 程序调用流程 主要涉及 Node 和 MapBuilder 这两个类 其中 Node 类在 cartographer_ros 功能包中实现Node 类通过类成员调用的方式将从 ROS 接口中获取的传感器数据传入 MapBuilder 类MapBuilder 类在 cartographer 核心库中实现MapBuilder 类主要包含传感器数据融合、局部建图和全局建图这 3 个部分 2. cartographer 核心库 cartographer 核心库实现建图算法的具体过程也就是局部建图、闭环检测和全局建图的实现下图为谷歌官方给出的流程框图。激光雷达数据、轮式里程计数据、IMU 数据和外部位姿辅助数据为整个算法的输入其中激光雷达为必需数据而轮式里程计、IMU 和外部位姿辅助即 GPS 和已知信标为可选数据 传感器数据输入 激光雷达数据用于前端局部建图 先经过体素滤波(voxel filter)体素滤波其实就是对点云降采样一般是将点云划分到不同体素栅格内再用体素栅格内所有点的重心表示此体素栅格经过体素滤波后的激光雷达数据有 2 个流向一个是直接传给 Submaps 用于子图构建另一个是经自适应体素滤波后用于扫描匹配 轮式里程计、IMU 等数据用于后端全局优化 与扫描匹配得到的观测位姿进行多传感器融合经融合后的位姿作为更高精度的初始位姿输入给扫描匹配这样能大大提高扫描匹配的效率和精度当前位姿位置 position姿态 orientation可以由前一时刻位姿推算而来观测位姿IMU 可用时更信任 IMU 提供的 orientationodom 可用时更信任 odom 提供的 position否则只能假设匀速模型即上一个时刻的线速度和角速度在当前时刻依然不变用线速度和角速度乘以时间间隔就能求出 position 与 orientation 前端局部建图 首先利用雷达数据和给定初始位姿进行扫描匹配得出观测位姿然后进行运动滤波。运动滤波的作用是避免重复插入相同雷达帧数据当位姿变化不明显时新雷达帧数据将不会被插入子图当构建完 1 个子图 submap(1) 后就接着构建另一个子图 submap(2)这样不断构建局部建图过程会生成多个子图 submaps {submap(m)} 后端全局建图 由闭环检测驱动闭环检测是在程序后台持续运行的传感器每输入一帧雷达数据都要对其进行闭环检测。当闭环检测中匹配得分超过设定阈值就判定闭环此时将闭环约束加入整个建图约束中并对全局位姿约束进行一次全局优化全局优化会对路径上的所有位姿及对应子图进行修正 3. ceres-solver 非线性优化库
Cartographer 采用优化库 Ceres-Solver 来求解优化问题主要包括局部建图中扫描匹配涉及的局部优化问题和全局建图中涉及的全局优化问题
3.4.2 安装与运行
手把手教你编译 cartographer手把手教你运行 cartographer
3.5 LOAM 算法
3.5.1 LOAM 算法框架 LOAM 的核心思想将 SLAM 问题拆分成独立的定位和建图分别来处理 特征提取模块scanRegistration 从雷达点云中提取特征点对当前帧点云中的每个点计算平滑度将平滑度小于某阙值(min)的点判断为 corner 特征点而平滑度大于某值(max)的点判断为 surface 特征点 定位模块laserOdometry 利用 scan-to-scan 方法对相邻两雷达点云中的特征点进行匹配这种帧间特征匹配能得到较低精度的里程计用该里程计来校正雷达特征点云的运动畸变 建图模块laserMapping 利用 scan-to-map 方法进行高精度定位该方法以前面低精度的里程计作为位姿初始值将校正后的雷达特征点云与地图进行匹配这种扫描帧到地图的匹配能得到较高精度的里程计1Hz 里程计基于该高精度的里程计所提供的位姿将校正后的雷达特征点云加入已有地图 里程计融合transformIntegration 将低精度里程计和高精度里程计融合融合通过以 1Hz 的高精度里程计为基准利用 10Hz 的低精度里程计对其进行插值从而输出更新速度和精度都较高的里程计 3.5.2 LOAM 源码解读
A-LOAM_Notes
3.5.3 LOAM 安装运行
依赖项安装 Ceres Solver 安装 $ sudo apt install cmake
$ sudo apt install libgoogle-glog-dev libgflags-dev
$ sudo apt install libatlas-base-dev
$ sudo apt-get install libeigen3-dev
$ sudo apt-get install libsuitesparse-dev$ git clone https://github.com/ceres-solver/ceres-solver.git
$ cd ceres-solver
$ mkdir build
$ cd build
$ cmake ..
$ make
$ make test
$ sudo make installPCL 安装 PCL学习一点云与PCL基础 源码安装$ mkdir -p a_loam_ws/src
$ cd ~/a_loam_ws/src
$ git clone https://github.com/HKUST-Aerial-Robotics/A-LOAM.git
$ cd ../
$ catkin_makeNSH indoor outdoor.bag 数据集运行$ cd ~/a_loam_ws
$ source devel/setup.bash
$ roslaunch aloam_velodyne aloam_velodyne_VLP_16.launch $ cd bag
$ rosbag play nsh_indoor_outdoor.bag
# nsh_indoor_outdoor.bag 下载地址
# 链接: https://pan.baidu.com/s/1zpS7rfLzVmyErVDlLdNP4g?pwdtvfb 提取码: tvfb4. 视觉 SLAM 系统待更新
4.1 ORB-SLAM2
5小时让你假装大概看懂ORB-SLAM2源码
4.2 VINS
四、自主导航篇
1. 理论概述
1.1 自主导航
自主导航问题的本质从地点 A 自主移动到地点 B机器人会有三个问题 “我在哪”、“我将到何处去” 和 “我该如何去”自主导航系统分类 层级式体系结构则能够对高层级任务逐层进行条理清晰的逻辑推理响应式体系结构能在低层级逻辑上对任务作出迅速响应(比如当障碍物突然出现时系统可以立马触发避障任务的响应而不需经过其他任务处理结果的层层触发) 1.2 环境感知
环境感知就是机器人利用传感器获取自身及环境状态信息的过程。自主导航机器人的环境感知主要包括实时定位、环境建模建图、语义理解等
1.2.1 实时定位
定位其实就是在回答机器人提出 “我在哪” 问题更确切点说应该是实时定位因为机器人不仅要知道自身的起始位姿还要知道导航过程中的实时位姿 实时定位可以分为被动定位和主动定位两种 被动定位依赖外部人工信标如 GPS、UWB、RFID、二维码等主动定位不依赖外部人工信标以 SLAM 为代表
主动定位
所谓主动定位就是机器人依靠自身传感器对未知环境进行感知并获取定位信息SLAM 导航方案由建图mapping、定位localization和路径规划path planning3 大基本问题组成这 3 大问题互相重叠和嵌套又组成新的问题也就是 SLAM 问题、导航问题、探索问题等 大多数 SLAM 算法支持两种工作模式SLAM 建图模式和 SLAM 重定位模式 商用场合通常采用 SLAM 重定位模式进行定位即先手动遥控机器人进行 SLAM 环境扫描并将构建好的地图保存下来然后载人事先构建好的离线地图并启动 SLAM 重定位模式获取机器人的实时位姿比如Gmapping 将构建出的地图保存为 *pgm 和 *yaml 文件然后利用 map_server 功能包载人 *pgm 和 *yaml 文件并发布到 ROS 话题最后利用 SLAM 重定位模式这里通常为 AMCL 算法及当前传感器信息与地图信息的匹配程度来估计位姿 但在外星球表面、荒野、岩洞等不便于人工手动遥控建图的情况下机器人需要自发地进行探索、建图和定位 CMU-自主探索导航系统TARE FAR Planner学习-All in one
1.2.2 环境建模建图
环境建模其实就是对环境状态进行描述也就是构建环境地图 地图可用于定位也可用于避障因此定位用到的地图与避障用到的地图并不一定相同环境地图有多种比如特征地图、点云地图、几何地图、栅格地图、拓扑地图等。视觉 SLAM 通常以构建特征地图为主激光 SLAM 则以构建栅格地图二维或点云地图三维为主由于导航过程中需要避开障碍物所以特征地图或点云地图必须转换成栅格地图后才能导航
二维栅格地图
机器人通常采用二维占据栅格地图其是对划分出来的每个栅格用一个占据概率值进行量化概率为 1 的栅格被标记为占据状态概率为 0 的栅格被标记为非占据状态概率在 0 到 1 之间的栅格被标记为未知状态。机器人在导航过程中要避开占据状态的栅格在非占据状态的栅格中通行通过传感器来探明未知状态的栅格的状态
三维栅格地图
将三维空间用立体栅格进行离散划分就得到了三维栅格地图。三维占据栅格地图是对划分出来的每个立体栅格用一个占据概率值进行量化相比于二维栅格三维栅格的数量更大。为了提高三维栅格地图数据处理效率通常采用八叉树Octree对三维栅格数据进行编码存储这样就得到了八叉树地图OctoMap
1.2.3 语义理解
对环境状态的理解是多维度的 对于定位问题来说环境状态被机器人理解为特征点或点云对于导航避障问题来说环境状态被机器人理解为二维或三维占据栅格 站在更高层次去理解机器人会得到环境状态数据之间的各种复杂关系即语义理解 无人驾驶汽车要学会车道识别、路障识别、交通信号灯识别、移动物体识别、地面分割等室内机器人要学会电梯识别、门窗识别、玻璃墙识别、镂空物体识别、斜坡识别等
1.3 路径规划
路径规划回答机器人提出 “我该如何去” 问题。无论是在已知地图上导航还是在未知环境中一边探索地图一边导航路径规划其实就是在地图上寻找一条从起点到目标点可行的通路机器人自主导航通常是在给定的栅格地图上进行路径规划。如下图通过对整个栅格地图遍历找到一条从 A 到 B 的路径比如路径 L1 或 L2。在实际机器人导航中不是找到一条可通行路径就行还要考虑路径的各项性能如长度、平滑性、碰撞风险、各种附加约束等 1.3.1 常见的路径规划算法
基于图结构的路径搜索 Dijkstra 算法 假如 P(A, F) 是 A 到 F 的最短路径D 是该最短路径上的某个点那么 P(A, D) 必定也是 A 到 D 的最短路径 P ( A , F ) ⏟ arg min P ( A , D ) ⏟ arg min P ( D , F ) \underbrace{P(A,F)}_{\arg\min}\underbrace{P(A,D)}_{\arg\min}P(D,F) argmin P(A,F)argmin P(A,D)P(D,F) Dijkstra 算法是按照广度优先先在当前搜索节点上横向生长出子节点然后从所有叶子节点中选择最优节点继续同样的生长方式进行搜索就是先遍历父节点周围的子节点然后选择一个符合条件的子节点继续 栅格地图上表示的 Dijkstra 算法搜索过程 Dijkstra 算法包含两个嵌套的循环时间复杂度为 O ( n 2 ) O(n^2) O(n2) 当大规模地图中包含的搜索节点数目 n 急剧增加时Dijkstra 算法的实时性将大幅下降Dikstra 算法的优点是其搜索策略是完备的也就是说如果最短路径存在的话只要花足够多的时间就一定能搜索到该路径 由上图搜索过程来看从扩展出的叶子节点(浅灰色节点)中挑选出下一个搜索节点时仅考虑了源节点到这些叶子节点的信息 假设以这些叶子节点的代价函数 f(x) 为挑选依据源节点到其他任意节点的实际代价用 g(x) 表示Dijkstra 算法是以下式 ① 为依据挑选搜索方向 A* 算法 A* 算法在代价函数 f(x) 中加估计代价(启发函数)来改进 Djkstra 算法的实时性如上式 ② 所示。启发函数描述了途经节点到目标节点的估计代价。引入启发函数可以有效降低搜索节点的范围从而提高整个搜索效率估计代价 h(x) 越小搜索节点的范围会越大。当估计代价 h(x) 为 0 时将得不到任何启发信息此时 A* 算法就退化成了 Dijkstra 算法。估计代价 h(x) 不能大于节点到目标节点的真实代价 h ( x ) ⩽ cos t ∗ ( x , t a r g e t ) h(x)\leqslant\cos t^*(x,\mathrm{target}) h(x)⩽cost∗(x,target)h(x) 的常见形式有曼哈顿距离、对角线距离、欧氏距离等 当栅格地图中只允许向 4 个方向上、下、左、右移动时选用曼哈顿距离当栅格地图中允许向 8 个方向上、下、左、右、左上、左下、右上、右下移动时选用对角线距离当栅格地图中允许向任意方向移动时选用欧氏距离 基于采样的路径搜索 以 PRM、RRT 等为代表的基于采样的路径搜索算法是将机器人所处的连续空间用随机采样离散化然后在离散采样点上进行路径搜索 PRM 算法 第 1 步随机采样。即在给定的地图上抛撒一定数量的随机点利用这些随机点对地图中的连续空间进行离散化 第 2 步移除无效采样点也就是将落在障碍物上的采样点删除 第 3 步连接也就是按照最近邻规则将采样点与周围相邻点进行连接 第 4 步移除无效连接也就是将横穿障碍物的连接删除这样就构建出了所谓的 PRM 路线图 第 5 步添加导航任务的源节点和目标节点也就是将源节点和目标节点与 PRM 路线图相连 第 6 步搜索路径在构建出来的 PRM 路线图上利用 A* 算法搜索源节点到目标节点之间的路径 PRM 在构建出路线图后仍然采用 A* 搜索路径PRM 在路径规划问题上是不完备的当采样点较少时可能规划不出路径。由于采样点不能覆盖所有情况所以 PRM 规划出的路径也不是最优的 RRT 算法 虽然 PRM 利用采样方式对连续空间进行离散化可以大大缩小搜索范围但最后路径依然是在图结构上进行搜索 RRT 算法通过采样方式构建树结构并且一边构建树结构一边进行路径搜索如下图所示RRT 工作流程为以源节点位置为树根利用随机采样再生长出树枝在各个树枝上继续利用随机采样再生长出树枝最终总会有树枝抵达目标节点。那么从源节点所在的树根位置沿着树枝生长方向有且仅有一条路径抵达目标节点这条路径就是 RRT 规划出来的路径 1.4 运动控制
对于自主导航来说目标点为外部给定的一个已知量起始点由 SLAM 定位模块提供。寻路和控制策略则比较复杂包括全局路径规划、局部路径规划、轨迹规划、轨迹跟踪等过程 SLAM 主要提供全局地图和定位信息而 SLAM 主要有两种工作模式 第一种模式SLAM 先运行建图模式构建好环境地图后将地图保存接着载入已保存的全局地图并启动 SLAM 重定位模式提供定位信息第二种模式SLAM 直接运行在线建图模式建图过程中直接提供地图和定位信息 全局路径规划以起始点和目标点为输入利用全局地图描述的障碍物信息规划出一条从起始点到目标点的全局路径 全局地图一般为离散、静态形式因此规划出来的全局路径也是离散、静态形式因为全局路径由一个个离散路径点连接而成并且只考虑了静态障碍物信息所以全局路径无法直接用于导航控制只能作为导航的宏观参考 局部路径规划相当于全局路径规划的细化过程以机器人能感知的局部边界上的全局路径点为局部目标点以机器人能感知到的局部动态障碍物信息为基础规划出一条从机器人当前位置点到局部目标点的局部路径 局部路径通常为连续、动态形式局部路径并不与全局路径重合而是尽量跟随着全局路径因为局部路径常常在突然出现的动态障碍影响下发生较大变化且机器人实际控制误差使得真实行走路径偏离局部路径局部路径也只能作为导航的宏观参考 轨迹规划相当于局部路径规划的细化过程局部路径只考虑了几何约束而在局部路径上添加运动学约束和动力学约束后就生成了机器人实际能执行的轨迹这就是所谓的轨迹规划 理想情况下可以直接取轨迹规划中各个轨迹点的速度信息作为动作量输入执行器电机不过电机控制误差、路面起伏、轮胎打滑等导致这种开环控制策略很难奏效 轨迹跟踪相当于轨迹规划的细化过程机器人按照轨迹规划出来的参考轨迹开始运动运动一段时间后发现偏离到参考轨迹左边机器人调整运动方向以便逼近参考轨迹。但由于惯性其在逼近参考轨迹之后立马又偏离到参考轨迹右边 机器人会根据真实轨迹与参考轨迹的偏差不断调整自身运动轨迹跟踪其实就是基于误差反馈的闭环控制真实轨迹始终跟随着参考轨迹左右摆动并不断逼近 1、全局路径规划和局部路径规划统称为路径规划 2、局部路径规划和轨迹规划统称为运动规划 3、轨迹规划和轨迹跟踪统称为运动控制轨迹规划为轨迹跟踪器提供参考轨迹而轨迹跟踪器生成动作量实现执行器的最终操控 1.4.1 基于 PID 的运动控制 机器人在世界坐标系的状态量为 q [ x , y , θ ] q[x,y,\theta] q[x,y,θ]其中 x 和 y 为机器人的位置坐标 θ \theta θ 为机器人的航向角 通常选择参考轨迹中离机器人最近的点 q r e f [ x r e f , y r e f , θ r e f ] q_{ref} [x_{ref},y_{ref},\theta_{ref}] qref[xref,yref,θref] 作为追踪目标以 q q q 和 q r e f q_{ref} qref 与之间的误差为反馈进行运动控制。最简单的方式就是用距离误差调节两轮差速机器人的线速度以航向角误差调节角速度 这里采用离散位置型 PID 控制算法离散其实是指 PID 算法中的积分运算与微分运算分别用累加运算与差分运算替代位置型是指 PID 算法的计算结果直接为控制量。利用两轮差速机器人运动学模型将 PID 算法得到的线速度量和角速度量转换成每个电机的速度控制量来实现最终控制 D e D_e De 表示机器人相对于追踪目标的位置误差 k p 、 k i 、 k d k_p、k_i、k_d kp、ki、kd 分别表示 PID 算法中的比例、积分和微分系数 v r e f v_{ref} vref 为线速度参考量用于修正。计算结果为机器人的线速度量 v v v θ e \theta_e θe 表示机器人相对于追踪目标的航向角误差 k p ′ 、 k i ′ 、 k d ′ k_p^{\prime}\text{、}k_i^{\prime}\text{、}k_d^{\prime} kp′、ki′、kd′ 分别表示 PID 算法中的比例、积分和微分系数 ω r e f \omega_{ref} ωref 为线速度参考量用于修正。计算结果为机器人的角速度量 ω \omega ω v ( t ) k p ⋅ D e ( t ) k i ⋅ ∑ t D e ( t ) k d ⋅ ( D e ( t ) − D e ( t − 1 ) ) v r e f v(t)k_{p}\cdot D_{e}(t)k_{i}\cdot\sum_{t}D_{e}(t)k_{d}\cdot(D_{e}(t)-D_{e}(t-1))v_{\mathrm{ref}} v(t)kp⋅De(t)ki⋅t∑De(t)kd⋅(De(t)−De(t−1))vref 其中 D e ( t ) ( x − x r e f ) 2 ( y − y r e f ) 2 D_e(t)\sqrt{(x-x_{\mathrm{ref}})^2(y-y_{\mathrm{ref}})^2} De(t)(x−xref)2(y−yref)2 ω ( t ) k p ′ ⋅ θ e ( t ) k i ′ ⋅ ∑ t θ e ( t ) k d ′ ⋅ ( θ e ( t ) − θ e ( t − 1 ) ) ω r e f \omega(t)k_{p}^{\prime}\cdot\theta_{e}(t)k_{i}^{\prime}\cdot\sum_{t}\theta_{e}(t)k_{d}^{\prime}\cdot(\theta_{e}(t)-\theta_{e}(t-1))\omega_{\mathrm{ref}} ω(t)kp′⋅θe(t)ki′⋅t∑θe(t)kd′⋅(θe(t)−θe(t−1))ωref 其中 θ e ( t ) θ − θ r e f \theta_e(t)\theta-\theta_\mathrm{ref} θe(t)θ−θref
1.4.2 基于 MPC 的运动控制
PID 算法属于典型的无模型控制系统其缺点是控制量对被控对象的控制永远是滞后的 对于低速运动的室内移动机器人这种控制滞后的影响并不明显而对于高速运动的室外无人驾驶汽车这种控制滞后的影响就很明显 解决方法是在计算控制量时将下一个时刻的预判状态也考虑进来也就是有模型控制系统。MPCModel Predictive Control模型预测控制算法属于典型的有模型控制系统该算法包括构建损失函数和优化求解两个步骤 J L o s s { f ( q ( k − 1 ) , u ( k ) ) , q r e f ( k ) } J\mathrm{Loss}\left\{f(\boldsymbol{q}(k-1),\boldsymbol{u}(k)),\boldsymbol{q}_{\mathrm{ref}}(k)\right\} JLoss{f(q(k−1),u(k)),qref(k)} u ( k ) arg min u ( k ) J \boldsymbol{u}(k)\arg\min_{\boldsymbol{u}(k)}J u(k)argu(k)minJ
所谓模型预测就是控制量与状态量之间的映射关系 f ( q ( k − 1 ) , u ( k ) f(\boldsymbol{q}(k-1),\boldsymbol{u}(k) f(q(k−1),u(k)。由于控制方式不同用于预测的模型并不唯一。比如在由电机控制的机器人中一般通过加在电机上的电压或者 PWM 波来控制电机转速而各电机转速到机器人实际线速度和角速度的映射借助运动学模型完成最后由里程计模型预测出机器人的状态转移量 Δ q \Delta \boldsymbol{q} Δq
2. 典型自主导航系统
2.1 ros-navigation ROS学习7ROS机器人导航仿真 2.1.1 原理分析 AMCL AMCLAdaptiveMonte Carlo Localization自适应蒙特卡洛定位方案包含两种代码实现即用于二维地图定位的代码实现 amcl 和用于三维地图定位的代码实现 amcl3d其中 ros-navigation 默认集成了二维地图定位的 amcl 代码包AMCL 是 MCLMonte Carlo Localization蒙特卡洛定位的改进版本而 MCL 属于粒子滤波的范畴因此 AMCL 也属于粒子滤波的范畴 Costmap 为了解决各种复杂障碍物的度量问题ros-navigation 采用 Costmap 代价地图对障碍物进行统一度量。Costmap 采用多个独立的栅格化图层来维护障碍物信息每个图层可以独立维护某个来源的障碍物信息这些图层可以根据不同需求进行叠加形成特定的障碍描述层 ros-navigation 系统框架 ros-navigaion 是一个功能包集里面包含了大量的 ROS 功能包以及各种算法的具体实现节点。这些节点可以分为 3 类 必要节点move_base通过插件机制组织代码使得 move_base 中的 global_planner、local_planner、global_costmap、local_costmap、recovery_behaviors 等算法能被轻易替换和改进可选节点amcl定位 和 map_server加载静态地图机器人平台相关节点sensor transforms障碍信息反馈、odometry source运动反馈、sensor sources 和 base controller执行器 2.1.2 源码解读待更新
2.1.3 安装与运行
ROS学习7ROS机器人导航仿真
2.1.4 路径规划改进
ros-navigation 中集成了一些不同的路径规划插件供用户在不同场景下使用其中可选的 全局路径规划插件包括 navfn、global_planner 和 carrot_planner局部路径规划插件包括 base_local_planner 和 dwa_local_planner不过这些插件还远不能满足研究和工程应用时的多样化需求 此时用户可以选择其他第三方路径规划插件加载到 move_base 中来使用或者根据 nav_core 的接口规范自己开发所需的路径规划插件 SBPL_Lattice_Planner 和 srl_global_planner 为比较典型的第三方全局路径规划插件teb_local_planner 为比较典型的第三方局部路径规划插件
2.1.5 环境探索
自主导航和 SLAM 都是在受限条件下进行环境交互 自主导航的受限条件是环境地图必须已知SLAM 的受限条件是构建未知环境地图的过程需要人为操控如果将自主导航与 SLAM 结合起来就可以让机器人与环境的交互过程真正自主化即所谓的环境探索。在机器人的环境探索过程中SLAM 为自主导航提供实时更新的地图自主导航则为 SLAM 提供操控这样就能让机器人在完全未知的环境中自主构建地图并利用该地图自主导航 环境探索涉及 3 个基本问题建图、定位和路径规划 从某种意义上说自主导航问题是环境探索问题的一种特殊情况也就是将环境探索问题、定位和路径规划中的建图部分替换成已知地图另外自主导航的导航目标点是从外部获取的某个确定值而环境探索的导航目标点是探索算法根据当前状态临时生成的 环境探索与自主导航非常相似在 ros-navigation 导航系统上稍做修改就能实现环境探索 将 map_server 部分更改成直接从外部获取实时地图数据将 amcl 部分更改成由 SLAM 提供全局定位将 ros-navigation 中从外部接收导航目标点的部分更改成从探索程序中自动生成导航目标点 1、frontier_exploration 探索算法 frontier_exploration 的探索策略其实就是不断将机器人导航到地图边缘区域这样地图边缘区域的未知环境就可以通过观测变为已知区域将机器人放人未知环境后机器人通过传感器能建立一个小范围的初始地图利用计算机视觉方法可以从这个初始地图中提取未闭合的边界区域并挑选其中一个边界区域作为导航目标点在机器人导航到该边界区域的过程中地图会不断扩大被探明的区域可能会出现新的边界区域机器人按照深度优先搜索的策略不断对边界区域进行探索直到所有边界区域都被探明为止安装后就可以与 ros-navigation 导航系统和 SLAM 系统比如Gmapping、Cartographer 等结合使用了 2、rrt_exploration 探索算法 rrt_exploration 集成了 frontier_exploration 中基于计算机视觉的边界区域探索策略还集成了适用于多机器人探索的 RRT 探索策略rrt_exploration 包含5个节点 全局边界检测器Global Frontier Detector局部边界检测器Local Frontier Detector过滤器Filter机器人任务整合Allocator传统边界检测器Opencv-based Frontier Detector
2.2 riskrrt 导航系统
riskrrt 的系统框架与 ros-navigation 基本一样riskrrt 的特别之处体现在对动态障碍物的处理更加严格在走廊、机场、路口等人流量大的场景中对机器人与动态障碍物发生碰撞的风险把控更加严格主要通过基于风险的 RRT 全局路径规划实现代码框架
2.3 autoware 导航系统
ros-navigation 和 riskrrt 导航系统主要用于机器人低速导航并且大多基于 2D 地图autoware 导航系统主要用于无人驾驶汽车的高速导航并且基于 3D 地图虽然 autoware 导航系统主要应用于无人驾驶汽车领域但整个框架依然是基于 ROS 开发的。autoware 框架同样是由定位、感知识别、路径规划等主要模块组成的
