机器人避障系统基础

一、避障传感器种类

用来采集运动时环境中障碍物信息的环境信息采集系统是自动避障系统的重要组成部分。全面、精确信息的获取，能够增强智能小车的自动避障性能，同时必须考虑信息处理的及时性和难易程度。视觉传感器、激光雷达测距传感器、红外测距传感器和超声波测距传感器等是主要的几种应用于智能小车自动避障的探测器。

1.超声波传感器

超声波测距传感器的工作机理是发射一定频率的信号，通过检测被物体反射、散射回来的信号，判断前方是否有物体3.超声波拥有比较好的传播方向性，能够沿直线传播，且穿透力比较强，能够得到相对集中的超声波能量。超声波频率的增加，导致其绕过障碍物能力不断减弱以及反射能力逐渐增强38.超声波测距传感器具有测距迅速、处理信息简单、控制实时、计算方便、价格低廉等特点，被广泛应用于智能小车测距、测速、定位和环境建模中。

1.1超声波测距原理

相位探测法、渡越时间探测法和声波幅值探测法是超声波的三种主要的测距方法

相位探测法，通过计算返回波与发射波间隔的相位来判断距离；声波幅值探测法，通过计算二者间相差的幅度，判断距离；渡越时间探测法，通过计算两者间的时间延误，判断距离。相位探测法测量精确度比较高，但是探测范围小。声波幅值探测法的探测精度受反射波的影响比较大。论文采用超声波测距最常用的方法一渡越时间探测法。下面主要介绍超声测距的渡越时间探测法原理。

渡越时间探测法，指的是超声波发生器往某个方向发射超声波，计时开始于发射的时间点，此后超声波沿直线传播，当超声波撞击到物体时就被反射回来，当超声波接收器接收到返回来的回波时计时停止42.超声波测距传感器与物体之间的距离d可以由公式（2.1）得出，其中℃为空气中超声波沿直线传播的速度，t为计时器所记录的时间。

[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-x7UCxyd7-1655011794615)(https://raw.githubusercontent.com/xkyvvv/blogpic3/main/img/202206112043599.png)]

在实际环境中温度和湿度会对超声波的传播速度c造成影响，但是由于其变化很小，并不影响智能小车自动避障对测距的要求，所以设计中c取常值，即c=340m/s。图2.2为超声波完成测距过程的原理图

[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-c0BXft6d-1655011794617)(https://raw.githubusercontent.com/xkyvvv/blogpic3/main/img/202206112044302.png)]

1.2超声波传感器测距的优缺点

利用超声波测距传感器测距具有如下优点：

（1）环境适应能力强。与视觉传感器相比较，超声波传感器能够在亮度低、有灰尘，煤烟等不理想的环境下，保持良好的测距性能，确保及时无误地采集到距离信息，然后将数据发送到信息处理设备。

（2）对色彩、光照度不敏感。超声波测距传感器可识别玻璃，抛光体等能透过光线和表面粗糙的物体。

（3）与视觉传感器相比超声波传感器可以得到距离信息。视觉传感器只能检测到物体的存在，而不能检测到障碍物和智能小车之间的距离，超声波传感器在检测到有物体的同时也可以得到与物体的距离信息。

（4）探测速度快。虽然在空气中光速比声速快，但视觉传感器较超声波传感器而言并没有速度上的优势。由于视觉传感器采集到的图片信息还需要经过图像识别过程，这个过程需要比较长的时间，制约了智能小车采集信息的速度。

（5）和视觉传感器、激光测距仪相比，超声波传感器还具有构造简易、占空间小、价格便宜，数据处理简单可靠，不容易损坏等优点。

所以超声波测距传感器，易于小型化和集成化，这更适合智能小车微型化的要求。

当然，利用超声波测距传感器测距也不能避免以下缺点：

（1）抗干扰性差：超声波能量较强，因此多个超声波传感器一起使用会相互干扰。

即使只使用一个超声波传感器，仍存在受声波的反复反射而造成干扰误差。

（2）由于超声波的波束角比较大，对物体位置的检测不够准确。

（3）单个超声波传感器的稳定性不理想，近距离测距有盲区45。

（4）超声波测距只能检测障碍物的距离数据，无法得到障碍物的尺寸和形状等信息。

综上所述，超声波测距传感器广泛应用在智能小车自动避障、汽车倒车雷达测距，物位（液位）探测，超声波开关和安防监控等相关领域4.但是超声波测距传感器不适用于需要提供尺寸和形状的应用环境，多个超声波传感器不能同时同方向近距离使用。

2.红外测距传感器

红外测距传感器是利用红外线反射特性制成的测距传感器。红外线属于光波的一种，也有其特殊性，具有光波反射传输的物理性质。红外测距通过信号发射源发射信息并反馈回来得到距离信息。由于红外测距具有测量精确度高、速度快等优势，所以红外测距技术被广泛应用于智能小车自动避障、地形测量、坦克飞机等对打击物的测量、通信卫星测量等领域。

2.1红外测距传感器的原理

红外测距是利用三角测量原理实现测距的。图2.3为红外测距的原理图，首先红外发射器发射红外光束，发射的红外光速是成一定角度的，当红外光束碰到物体时，会被物体反射回来，CCD检测器能够检测到返回的光束，而获得偏移值，利用三角定理，可得到如公式（2.2）的几何关系式。

[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-f7r3Zpep-1655011794617)(https://raw.githubusercontent.com/xkyvvv/blogpic3/main/img/202206112049049.png)]

其中，D为红外传感器到日标物体的距离，f为滤镜的焦距，L为偏移值，X为中心距，a表示发射角，c表示红外线在空气中的传播速度（c=3*108m/s），红外测距的原理如图2.3所示，

[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-ae8kLNBD-1655011794618)(https://raw.githubusercontent.com/xkyvvv/blogpic3/main/img/202206112049313.png)]

由图23可知，当目标物体距离红外传感器D足够小时，偏移值L会非常的大，甚至大于CCD检测器的检测最大值。同样当D值很大时，L则很小。也就是说是否能够获得偏移值L的关键在于滤镜的焦距。

2.2红外测距传感器测距的优缺点

利用红外测距传感器测量距离具有的优点如下：

（1）抗干扰能力强。同时同方向使用多个红外传感器时不会相互干扰。

（2）测量精度高。红外测距传感器与超声波测距传感器相比，具有测量精度的优势。

（3）测距速度快。由于光速比声音在空气中传播的速度快，所以利用红外光线的反射原理制成的红外测距传感器比利用声波反射原理制成的超声波测距传感器测距，反应时间短。

（4）适用于长距离测距。由于红外线在空气中传播具有不扩散的性质，并且它在穿透物体时对折射率的影响不大，所以红外测距经常被用在距离较大的测量任务中。

（5）具有更强的发射和接收能力。因为自然界中只要温度高于绝对零度的物体都会反射红外线，所以利用红外反射原理制成的测距传感器发射和接收能力优越。

红外测距传感器测距的主要缺点是强光直射的环境会对测量精度造成影响。

3.激光雷达测距传感器

市面上有多种类型的激光传感器，从测距原理上大致可以分为四类：脉冲法，相位法，干涉法和三角法。其中前两种方法都是根据激光的飞行时间来换算为距离。干涉法是通过的相干光源产生的干涉现象中的干涉条纹数量来计算距离。三角法则是通过三角关系来计算距离。三角测距雷达的结构如下图2-7所示：

[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-9AB1jg63-1655011794618)(https://raw.githubusercontent.com/xkyvvv/blogpic3/main/img/202206112100595.png)]

式中的f和s己知，则可以通过像素点的位置来计算出该点的距离值q。

[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-GiZY58XR-1655011794618)(https://raw.githubusercontent.com/xkyvvv/blogpic3/main/img/202206112100122.png)]

上式对x求导，可以得到：

从上式可以得到，三角测距原理的激光雷达的测量噪声会随着测量距离的增加而增加，噪声水平与距离平方成正比。三角测距雷达的原理简单，成本低廉，但是测量精度会随着测量距离而降低，因此，三角测距原理的激光雷达适用于中短距离测量。市面上的低成本商用激光雷达多采用此方案。将三角测距的模块在一个平面中进行回转，就可以生成周围场景的平面扫描图。激光雷达实物图如图2-8所示。

[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-HUHZt51B-1655011794619)(https://raw.githubusercontent.com/xkyvvv/blogpic3/main/img/202206112102687.png)]

图2-8（a）基于三角测距原理的激光雷达测距范围多在20米以内，而（b）中的雷达测距范围可达100米甚至更远。前者的数据采样频率大约为4000Hz，后者的采样频率可达300000Hz。当然前者的售价也为大大低于后者。

4.相机

在使用相机之前，先要对相机进行标定。其目的是建立起世界坐标中，相机坐标系和图像坐标系这三者之间的转换关系。首先介绍相机坐标系与图像坐标系之间的关系。

像素坐标系是一个二维坐标系，其一般定义是以图像的左上角为原点O，水平向右为u轴正方向，垂直向下为y轴正方向。

图像坐标系是一个二维坐标系，其一般定义是以光轴和像平面的交点为原点O1，水平向右为x轴正方向，垂直向下为y轴正方向。单位为物理单位，比如：米。

相机坐标系是一个三维坐标系（Xc,Yc,Zc），其一般定义是以相机的光心为原点Oc, Zc轴和相机的光轴重合，Xc轴和Y。轴分别平行于图像坐标系的和v轴。

相机坐标系和像素坐标系如下图2-4（a）所示，像素坐标和图像坐标系的关系如下图2-4（b）所示：

[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-ZRw3iA0S-1655011794619)(https://raw.githubusercontent.com/xkyvvv/blogpic3/main/img/202206112107503.png)]

上图2-4中，（o,vo）为相机坐标系的Z轴和图像平面的交点，称为主点，以像素为单位。

在已知像素的物理尺寸后，可以建立起图像坐标系中的点和像素坐标系中的点的对应关系。图像坐标系下的点（x,y）变换到像素坐标系下（u,y）点的公式如下式（2-5）、（2-6）

所示。

[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-c4QlxyTd-1655011794620)(https://raw.githubusercontent.com/xkyvvv/blogpic3/main/img/202206112108150.png)]

上式（2-5）中中，dx表示x轴方向上像素的物理尺寸，dy表示y轴方向上像素的物理尺寸。uo,Vo为主点的像素坐标将上式写作齐次形式可以得式C（2-7）

[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-4CK8Q1Fv-1655011794620)(https://raw.githubusercontent.com/xkyvvv/blogpic3/main/img/202206112108154.png)]

相机坐标系下的点坐标和像素坐标系下的点的坐标变化关系称为相机内部参数。这个参数需要通过标定得到。考虑小孔成像模型，忽略畸变，通过相似三角形并进行一定的简化之后可以得到这样的齐次变换式，如式（2-8）所示，变换的示意图如图2-5所示。

上式中，f为镜头的焦距，（Xc,Yc,Zc）为相机坐标系下的一个三维点。

世界坐标系用于表示空间点的物理位置。可以通过一个旋转平移矩阵来表示世界坐标系和相机坐标系之间的位姿关系。这个旋转平移矩阵也称作相机外部参数。这个变换如图2-6所示。

[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-Iusu6UvK-1655011794621)(https://raw.githubusercontent.com/xkyvvv/blogpic3/main/img/202206112110346.png)]

在得到旋转和平移矩阵之后即可将世界坐标系下的点坐标转换到像素坐标系中。转换式如式（2-9）所示。

[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-oiXDvHbA-1655011794621)(https://raw.githubusercontent.com/xkyvvv/blogpic3/main/img/202206112111018.png)]

上式（2-9）中，K为相机内参矩阵，P为包含了内部参数和外部参数的投影矩阵。R为33的三维旋转矩阵，T为31的平移矩阵。

5.传感器的选择方案

为了给智能小车环境信息采集系统选择具体的测距传感器，对测距传感器提出下列要求：

（1）满足功能：即能够测距，且输入、输出接口符合Arduino要求。

（2）满足性能：即满足环境信息采集系统的性能指标要求。

（3）经济性：在功能和性能都满足的前提下，测距传感器的价格越低越好。

下面列举的是测距传感器的主要性能指标

（1）测量精度：由于布置在车头位置的测距传感器，容易受到智能小车运动的影响，所以要求精度为1.由于车体侧面发生碰撞的可能性比较小，所以布置在车体两侧的测距传感器精度要求为10mm即可。

（2）测量范围：两种传感器的最小测量值要求在0.5倍的安全距离（安全距离见3.2节）以内，即为0~150mm。

（3）工作温度，耐腐蚀性以及光亮度：传感器须满足智能小车工作环境的中温度，腐蚀度以及光亮度的要求。其中温度范围要求为0~50°，耐腐蚀性要求为适用于中性环境即可，亮度要求为适应环境光，智能小车运动中没有强光直射车头。

二、姿态传感器种类

飞行器若想平稳飞行，必须依靠姿态检测模块对其进行实时的姿态检测，将检测到的数据实时反馈给主控芯片，由主控芯片对其进行分析并实时调整飞行器各个旋翼的转速以使飞行器按正确的姿态平稳飞行。

常见的姿态检测器件有加速度计、陀螺仪、磁力计、超声波传感器、气压传感器以及GPS等；在当前的姿态检测研究当中，囊括了以上传感器的所有数据较是较为复杂的融合表示方法，这种方法可以获得飞行器较准确的飞行姿态；可是针对飞行器飞行系统的应用，这种复杂的基于多传感器数据融合算法会受到嵌入式平台运算能力的限制，所以我们需要选择一种轻量而且廉价的惯性测量单元（IMUU8））用来输出飞行器的姿态信息；本设计中采用只使用角速度和加速度两种传感器来做为姿态测量器件，其相关的知识和原理详见如下介绍。

1.加速度计

加速度计是一种用来测量加速力的传感器，加速力是在物体在加速过程中作用在物体上的力，例如地球引力，也就是重力；我们可以通过测量由重力引起的加速度，从而计算出设备相对于水平面的倾斜角度。

在三维立体空间中，利用重力和传感器的关系，可以分析得到如图3.5所示的三个姿态角p（pitch）、p（roll）、日（yaw）：X轴相对于地面的角度是p、是Y轴相对于地面的角度是φ、Z轴相对于重力的夹角是0。

[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-2OCZhJlV-1655011794621)(https://raw.githubusercontent.com/xkyvvv/blogpic3/main/img/202206112125168.png)]

因此可以推导出：

[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-dLeTLb6B-1655011794621)(https://raw.githubusercontent.com/xkyvvv/blogpic3/main/img/202206112125749.png)]

其中：

ax是重力在X轴方向产生的加速度

ay是重力在Y轴方向产生的加速度

az是重力在Z轴方向产生的加速度

2.陀螺仪

陀螺仪（角速度的传感器）是一种以角动量的守恒为工作原理的传感器，它是一个用于维持方向以及测量角度的设备，通常的陀螺仪分为Gimbal陀螺仪及MEMS陀螺仪两种类型。

[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-2yO7hOVM-1655011794622)(https://raw.githubusercontent.com/xkyvvv/blogpic3/main/img/202206112126499.png)]

（1）Gimbal2型陀螺仪

如图3.4（A）所示的是传统的Gimba陀螺仪，其中在外面的三个环称作平衡环，而中间的是一个转子，它是在不停旋转的所以由于惯性的作用不会受到外界的影响，然而外面的三个环却会因为设备姿态的改变而跟着改变，这样就可以以此来检测所测物体的当前状态，陀螺仪里面的X、Y、Z三个轴就是外面的这三个环所在的轴；但是Gimbal陀螺仪有一个万向节锁24，即指当三个万向节中的两个轴发生重合的时候，会失去一个自由度的情况。为了消除这一个缺陷，一般的应用中就需要使用更多更昂贵的机械部件来进行协助补偿。

（b）MEMS陀螺仪

MEMS陀螺仪相比于体积较大的Gimbal陀螺仪有着更大的优势；如图3.4-B中的旋转盘，在直线0A上的两个点B和C，当磁盘顺时针旋转时，B位置的点会移动到C的位置，虽然这两个点的角度率是相同的，但是在C点的速度是比B点快，这说明存在一个作用力引起了这个速度的改变，而这个力被称为科里奥利力；MEMS陀螺仪的工作原理就是通过利用科里奥利力来对物体的角速度进行测量的。

（3）传感器选择

目前MEMS陀螺仪与加速度传感器由于其体积和价格上的优势，被广泛应用于电子设备中进行姿态检测，故本设计采用MEMS陀螺仪与加速度传感器，在选择传感器类型时需要综合考虑到器件的量程、灵敏度及供电电压，表3.1和表3.2列出了目前市场上主流的3轴MMS陀螺仪和加速度传感器。

[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-d8Pjt8B1-1655011794622)(https://raw.githubusercontent.com/xkyvvv/blogpic3/main/img/202206112129481.png)]

通过上表参数，考虑到重量等各方面因素，本设计选取了加速度和角速度一体的MUP-6050芯片作为飞行器的姿态控制传感器。

---

本文采用的PU6500集成了三轴MEMS数字陀螺仪、数字加速度计，可以测量多旋翼无人飞行器运动状态下的三轴角速率®，0,0：、三轴加速度Q,a,a。陀螺仪测量的角速度数据动态响应速度快，长期使用时，存在较大的积分误差，因此，陀螺仪无法单独使用。加速度计是一种长期稳定的惯性器件。

解算姿态时，通过融合珈速度计与陀螺仪的数据，能够得到稳定的姿态角信息；计算速度和位置信息时，通过加速度计积分获得相应的速度与位置。

3.磁力计

三轴磁力计通过磁敏器件，获取地磁场在机体坐标系下的三维磁场矢量投影。通过磁力计对航向角解算时，无需进行积分运算，因此它是一种长期稳定的器件。

4.气压计

所采用的M$5611气压高度计可以获得高度信息。气压计通过测量大气压的值能够计算当前海拔高度。其中，气压与高度的关系如下所示：

式中，R代表大气温度下限值、g代表重力加速度，P，为当前高度测得的大气静压，T6P,h，代表当前大气压的大气温度下限值、大气压力和重力势高度，B为温度垂直变化率。通过上述公式，能够计算得到当前高度。

5.GPS

GPS能够测量三轴速度、经度、维度以及海拔高度。在开阔的户外环境中，GPS能够达到2.5米的位置定位精度（采用差分GPS甚至可达到厘米级的定位精度），速度测量精度能够达到约0.1m/s。然而，GPS的海拔高度以及竖直方向速度测量精度较低，且附近有遮挡物时，由于信号折射等原因，GPS的位置定位精度会大幅下降，但速度测量精度影响较小。

综上所述，陀螺仪测量得到的三轴角速度数据通过积分能够得到短时精度较高的姿态角。然而，随着时间的推移，由于陀螺仪传感器存在零偏、随机游走以及温漂等误差存在，导致了其长期姿态角解算精度较低。因此，本文将加速度计、磁力计传感器测量得到的数据与陀螺仪测量得到的数据进行融合，进行姿态解算，从而得到长期稳定的姿态角信息。另外，GP$虽然能够测量速度、位置信息，但其测量精度与工作环境相关，存在延时大，输出频率低，信号不稳定的问题。此外，气压计虽然能够检测海拔高度，但由于大气易受干扰，短时间内无法反映真实的海拔高度。综上所述，四旋翼飞行器所配置的每一个传感器都存在一定的局限性，单一的传感器测量数据都无法满足无人机的导航精度要求，因此，需要进行优势互补，以得到长期解算稳定，短期解算精度高的目的。

三、传感器数据融合

多传感器信息融合（Multi-sensor Information Fusion,MSIF），就是利用计算机技术将来自多传感器或多源的信息和数据，在一定的准则下加以自动分析和综合，以完成所需要的决策和估计而进行的信息处理过程。

1.多传感器融合几个概念

硬件同步、硬同步：使用同一种硬件同时发布触发采集命令，实现各传感器采集、测量的时间同步。做到同一时刻采集相同的信息。

软件同步：时间同步、空间同步。

时间同步、时间戳同步、软同步：通过统一的主机给各个传感器提供基准时间，各传感器根据已经校准后的各自时间为各自独立采集的数据加上时间戳信息，可以做到所有传感器时间戳同步，但由于各个传感器各自采集周期相互独立，无法保证同一时刻采集相同的信息。

空间同步： 将不同传感器坐标系的测量值转换到同一个坐标系中，其中激光传感器在高速移动的情况下需要考虑当前速度下的帧内位移校准。

[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-bbSrqDKo-1655011794623)(https://raw.githubusercontent.com/xkyvvv/blogpic3/main/img/202206112305811.png)]

2.基本原理

多传感器融合基本原理就像人脑综合处理信息的过程一样，将各种传感器进行多层次、多空间的信息互补和优化组合处理，最终产生对观测环境的一致性解释。在这个过程中要充分利用多源数据进行合理支配与使用，而信息融合的最终目标则是基于各传感器获得的分离观测信息，通过对信息多级别、多方面组合导出更多有用信息。这不仅是利用了多个传感器相互协同操作的优势，而且也综合处理了其它信息源的数据来提高整个传感器系统的智能化。

具体来讲，多传感器数据融合原理如下：

（1）多个不同类型传感器（有源或无源）收集观测目标的数据；

（2）对传感器的输出数据（离散或连续的时间函数数据、输出矢量、成像数据或一个直接的属性说明）进行特征提取的变换，提取代表观测数据的特征矢量Yi；

（3）对特征矢量Yi进行模式识别处理（如聚类算法、自适应神经网络或其他能将特征矢量Yi变换成目标属性判决的统计模式识别法等），完成各传感器关于目标的说明；

（4）将各传感器关于目标的说明数据按同一目标进行分组，即关联；

（5）利用融合算法将目标的各传感器数据进行合成，得到该目标的一致性解释与描述。

3.多传感器的前融合与后融合技术

3.1后融合算法：

每个传感器各自独立处理生成的目标数据。
每个传感器都有自己独立的感知，比如激光雷达有激光雷达的感知，摄像头有摄像头的感知，毫米波雷达也会做出自己的感知。
当所有传感器完成目标数据生成后，再由主处理器进行数据融合。

[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-Mtkwubmt-1655011794623)(https://raw.githubusercontent.com/xkyvvv/blogpic3/main/img/202206121323298.png)]

3.2前融合算法：

只有一个感知的算法。对融合后的多维综合数据进行感知。
在原始层把数据都融合在一起，融合好的数据就好比是一个Super传感器，而且这个传感器不仅有能力可以看到红外线，还有能力可以看到摄像头或者RGB，也有能力看到LiDAR的三维信息，就好比是一双超级眼睛。在这双超级眼睛上面，开发自己的感知算法，最后会输出一个结果层的物体。

[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-eMXO5e4W-1655011794623)(https://raw.githubusercontent.com/xkyvvv/blogpic3/main/img/202206112305779.png)]

4.融合算法

对于多传感器系统而言，信息具有多样性和复杂性，因此对信息融合算法的基本要求是具有鲁棒性和并行处理能力。其他要求还有算法的运算速度和精度；与前续预处理系统和后续信息识别系统的接口性能；与不同技术和方法的协调能力；对信息样本的要求等。一般情况下，基于非线性的数学方法，如果具有容错性、自适应性、联想记忆和并行处理能力，则都可以用来作为融合方法。

多传感器数据融合的常用方法基本上可分为两大类：随机类和人工智能类。

4.1随机类

（1）加权平均法

信号级融合方法最简单直观的方法是加权平均法，将一组传感器提供的冗余信息进行加权平均，结果作为融合值。该方法是一种直接对数据源进行操作的方法。

（2）卡尔曼滤波法

主要用于融合低层次实时动态多传感器冗余数据。该方法用测量模型的统计特性递推，决定统计意义下的最优融合和数据估计。如果系统具有线性动力学模型，且系统与传感器的误差符合高斯白噪声模型，则卡尔曼滤波将为融合数据提供唯一统计意义下的最优估计。

卡尔曼滤波的递推特性使系统处理无需大量的数据存储和计算。但是采用单一的卡尔曼滤波器对多传感器组合系统进行数据统计时，存在很多严重问题，例如：① 在组合信息大量冗余情况下，计算量将以滤波器维数的三次方剧增，实时性难以满足。② 传感器子系统的增加使故障概率增加，在某一系统出现故障而没有来得及被检测出时，故障会污染整个系统，使可靠性降低。

（3）多贝叶斯估计法

将每一个传感器作为一个贝叶斯估计，把各单独物体的关联概率分布合成一个联合的后验概率分布函数，通过使联合分布函数的似然函数为最小，提供多传感器信息的最终融合值，融合信息与环境的一个先验模型以提供整个环境的一个特征描述。

（4）D-S证据推理法

该方法是贝叶斯推理的扩充，包含3个基本要点：基本概率赋值函数、信任函数和似然函数。

D-S方法的推理结构是自上而下的，分为三级：第一级为目标合成，其作用是把来自独立传感器的观测结果合成为一个总的输出结果（ID）；第二级为推断，其作用是获得传感器的观测结果并进行推断，将传感器观测结果扩展成目标报告。这种推理的基础是：一定的传感器报告以某种可信度在逻辑上会产生可信的某些目标报告；第三级为更新，各传感器一般都存在随机误差，因此在时间上充分独立地来自同一传感器的一组连续报告比任何单一报告更加可靠。所以在推理和多传感器合成之前，要先组合（更新）传感器的观测数据。

（5）产生式规则

采用符号表示目标特征和相应传感器信息之间的联系，与每一个规则相联系的置信因子表示它的不确定性程度。当在同一个逻辑推理过程中，2个或多个规则形成一个联合规则时，可以产生融合。应用产生式规则进行融合的主要问题是每个规则置信因子的定义与系统中其他规则的置信因子相关，如果系统中引入新的传感器，需要加入相应的附加规则。

4.2.AI类

（1）模糊逻辑推理

模糊逻辑是多值逻辑，通过指定一个0到1之间的实数表示真实度（相当于隐含算子的前提），允许将多个传感器信息融合过程中的不确定性直接表示在推理过程中。如果采用某种系统化的方法对融合过程中的不确定性进行推理建模，则可以产生一致性模糊推理。

与概率统计方法相比，逻辑推理存在许多优点，它在一定程度上克服了概率论所面临的问题，对信息的表示和处理更加接近人类的思维方式，一般比较适合于在高层次上的应用（如决策）。但是逻辑推理本身还不够成熟和系统化。此外由于逻辑推理对信息的描述存在很多的主观因素，所以信息的表示和处理缺乏客观性。

模糊集合理论对于数据融合的实际价值在于它外延到模糊逻辑，模糊逻辑是一种多值逻辑，隶属度可视为一个数据真值的不精确表示。在MSF过程中，存在的不确定性可以直接用模糊逻辑表示，然后使用多值逻辑推理，根据模糊集合理论的各种演算对各种命题进行合并，进而实现数据融合。

（2）人工神经网络法

神经网络具有很强的容错性以及自学习、自组织及自适应能力，能够模拟复杂的非线性映射。神经网络的这些特性和强大的非线性处理能力，恰好满足多传感器数据融合技术处理的要求。在多传感器系统中，各信息源所提供的环境信息都具有一定程度的不确定性，对这些不确定信息的融合过程实际上是一个不确定性推理过程。神经网络根据当前系统所接受的样本相似性确定分类标准，这种确定方法主要表现在网络的权值分布上，同时可以采用学习算法来获取知识，得到不确定性推理机制。利用神经网络的信号处理能力和自动推理功能，即实现了多传感器数据融合。

再推荐几篇比较好的文章：

传感器融合是什么？及其类型和应用

多传感器融合（算法）综述

---

四、避障常用算法

首先我们要区分避障算法和路径规划算法。在讲避障算法之前，我们假定机器人已经有了一个导航规划算法对自己的运动进行规划，并按照规划的路径行走。避障算法的任务就是在机器人执行正常行走任务的时候，由于传感器的输入感知到了障碍物的存在，实时地更新目标轨迹，绕过障碍物。

1.Bug算法

Bug算法应该是最简单的一种避障算法了，它的基本思想是在发现障碍后，围着检测到的障碍物轮廓行走，从而绕开它。Bug算法目前有很多变种， 比如Bug1算法，机器人首先完全地围绕物体，然后从距目标最短距离的点离开。Bug1算法的效率很低，但可以保证机器人达到目标。

[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-fTskXRXh-1655011794624)(https://raw.githubusercontent.com/xkyvvv/blogpic3/main/img/202206112201110.png)]

Bug1算法示例

改进后的Bug2算法中，机器人开始时会跟踪物体的轮廓，但不会完全围绕物体一圈，当机器人可以直接移动至目标时，就可以直接从障碍分离，这样可以达到比较短的机器人行走总路径。

[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-8h6wxkWt-1655011794624)(https://raw.githubusercontent.com/xkyvvv/blogpic3/main/img/202206112201218.png)]

Bug2算法示例

除此之外，Bug算法还有很多其他的变种， 比如正切Bug算法等等。在许多简单的场景中，Bug算法是实现起来比较容易和方便的，但是它们并没有考虑到机器人的动力学等限制，因此在更复杂的实际环境中就不是那么可靠好用了。

2.势场法（PFM）

人工势场避障控制法，是一种比较简单又新颖的做法，是另一种仿生学，仿照物理学中电势和电场力的概念，建立机器人工作空间中的虚拟势场，按照虚拟势场力方向，实现局部路径规划。通过构造目标位姿引力场和障碍物周围斥力场共同作用的人工势场，来搜索势函数的下降方向，然后寻找无碰撞路径。听起来很玄乎，但是早已经有应用产品了，Khatib曾应用于移动机器人的导航上。但是并没有得到大规模应用。因为即使对于简单环境很有效，但是都是在静态的研究中得出的，而没有考虑障碍物的速度和加速度的影响，所以在动态避障控制中，人工势场法避障控制不是很理想。因为在复杂的多障碍环境中，不合理的势场数学方程容易产生局部极值点，导致机器人未到达目标就停止运动，或者产生振荡、摆动等现象。另外，传统的人工势场法着眼于得到一条能够避障的可行路径，还没有研究出什么最优路径。

实际上，势场法不仅仅可以用来避障，还可以用来进行路径的规划。势场法把机器人处理在势场下的 一个点，随着势场而移动，目标表现为低谷值，即对机器人的吸引力，而障碍物扮演的势场中的一个高峰，即斥力，所有这些力迭加于机器人身上，平滑地引导机器人走向目标，同时避免碰撞已知的障碍物。当机器人移动过程中检测新的障碍物，则需要更新势场并重新规划。

上面这个图是势场比较典型的示例图，最上的图a左上角是出发点，右下角是目标点，中间三个方块是障碍物。中间的图b就是等势位图，图中的每条连续的线就代表了一个等势位的一条线，然后虚线表示的在整个势场里面所规划出来的一条路径，我们的机器人是沿着势场所指向的那个方向一直行走，可以看见它会绕过这个比较高的障碍物。最下面的图，即我们整个目标的吸引力还有我们所有障碍物产生的斥力最终形成的一个势场效果图，可以看到机器人从左上角的出发点出发，一路沿着势场下降的方向达到最终的目标点，而每个障碍物势场表现出在很高的平台，所以，它规划出来的路径是不会从这个障碍物上面走的。

一种扩展的方法在基本的势场上附加了?另外两个势场：转运势场和任务势场。它们额外考虑了由于机器人本身运动方向、运动速度等状态和障碍物之间的相互影响。

转动势场"：考虑了障碍与机器人的相对方位，当机器人朝着障碍物行走时，增加斥力， 而当平行于物体行走时，因为很明显并不会撞到障碍物，则减小斥力。任务势场：则排除了那些根据当前机器人速度不会对近期势能造成影响的障碍，因此允许规划出 一条更为平滑的轨迹。

另外还有谐波势场法等其他改进方法。势场法在理论上有诸多局限性， 比如局部最小点问题，或者震荡性的问题，但实际应用过程中效果还是不错的，实现起来也比较容易。

3.向量场直方图（VFH）

它执行过程中针对移动机器人当前周边环境创建了一个基于极坐标表示的局部地图，这个局部使用栅格图的表示方法，会被最近的一些传感器数据所更新。VFH算法产生的极坐标直方图如图所示：

[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-zgwXCsve-1655011794625)(https://raw.githubusercontent.com/xkyvvv/blogpic3/main/img/202206112204261.png)]

图中x轴是以机器人为中心感知到的障碍物的角度，y轴表示在该方向存在障碍物的概率大小p。实际应用的过程中会根据这个直方图首先辨识出允许机器人通过的足够大的所有空隙，然后对所有这些空隙计算其代价函数，最终选择具有最低代价函数的通路通过。

代价函数受三个因素影响： 目标方向、机器人当前方向、之前选择的方向，最终生成的代价是这三个因素的加权值，通过调节不同的权重可以调整机器人的选择偏好。VFH算法也有其他的扩展和改进，比如在VFH+算法中，就考虑了机器人运动学的限制。由于实际底层运动结构的不同，机器的实际运动能力是受限的，比如汽车结构，就不能随心所欲地原地转向等。VFH+算法会考虑障碍物对机器人实际运动能力下轨迹的阻挡效应，屏蔽掉那些虽然没有被障碍物占据但由于其阻挡实际无法达到的运动轨迹。我们的E巡机器人采用的是两轮差动驱动的运动形式，运动非常灵活，实际应用较少受到这些因素的影响。

具体可以看 一下这个图示：

[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-1pouRYYc-1655011794625)(https://raw.githubusercontent.com/xkyvvv/blogpic3/main/img/202206112204219.png)]

类似这样传统的避障方法还有很多，除此之外，还有许多其他的智能避障技术，比如神经网络、模糊逻辑等。

神经网络方法对机器人从初始位置到目标位置的整个行走路径进行训练建模，应用的时候，神经网络的输 入为之前机器人的位姿和速度以及传感器的输 入，输出期望的下一目标或运动方向。

模糊逻辑方法核心是模糊控制器，需要将专家的知识或操作人员的经验写成多条模糊逻辑语句，以此控制机器人的避障过程。 比如这样的模糊逻辑：第一条，若右前方较远处检测到障碍物，则稍向左转；第 二条，若右前方较近处检测到障碍物，则减速并向左转更多角度；等等。

避障过程中存在哪些问题？

4.模糊逻辑控制

不必创建可分析的环境模型，目前模糊逻辑方法在解决机器人避开障碍物问题上己经有了大量的研究工作。另一个独特优点也让用专家知识调整规则成为可能，因为规则库的每条规则具有明确的物理意义。在模糊逻辑控制避障法中，模糊控制规则是模糊控制的核心。当前研究工作的新趋势之一是它的渐增本质，特别是在模糊控制规则的自动生成方面，即连同自动模糊数据获取，给予算法在线模糊规则学习能力，数据获取和规则生成均自动执行。

[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-TcOBxZF0-1655011794625)(https://raw.githubusercontent.com/xkyvvv/blogpic3/main/img/202206112205264.png)]

5.人工神经网络避障控制法

人工神经网络是由许多单元（又称神经元），按照一定的拓扑结构相互连接而成的一种具有并行计算能力的网络系统，它具有较强的非线性拟合能力和多输入多输出同时处理的能力。用在机器人上，就是通过模拟人脑神经网络处理信息的方式，从另一个研究角度来获取具有人脑那样的信息处理能力。对于智能机器人来说，采用人工神经网络进行信息融合有一个最大优势，即可大规模地并行处理和分布式信息存储，具有良好的自适应、自组织性，以及很强的学习功能、联想功能和容错功能，接近人脑的信息处理模式。

6. 栅格法避障控制法

于用启发式算法在单元中搜索安全路径。赋予每个栅格一个通行因子后，路径规划问题就变成在栅格网上寻求两个栅格节点间的最优路径问题。搜索过程多采用四叉树或八叉树表示工作空间。栅格法以基本元素为最小栅格粒度，将地图进行栅格划分，比如基本元素位于自由区取值为0，处在障碍物区或包含障碍物区为1，直到每个区域中所包含的基本单元全为0或全为1，这样在计算机中就较容易建立一幅可用于路径规划的地图。栅格粒度越小，障碍物的表示会越精确，也就更好避障。但往往会占用大量的储存空间，而且算法也将按指数增加。

五、路径规划算法

在规划模块中首先明确经常容易混淆的几个概念：

运动规划Motion Planning

路径规划Path Planning

轨迹规划Trajectory Planning

1.路径规划+轨迹规划=运动规划

2.路径规划：已知几何约束的情况下（障碍物、地图信息），求解一条路径，也即是找到一条无碰撞的路径。

**3.轨迹规划：已知运动学约束（时间约束、速度约束等）**情况下求解轨迹。

当然也有的路径规划算法在搜索路径过程中也将动力学约束（Hybrid A*）考虑进去，本系列的所涉及的规划算法主要是路径规划，也会涉及一部分轨迹规划。

1. 作用

**路径规划（path planning）**目的是为了寻找成本最小的路径, 需要给出的条件包括: 地图, 起始地点, 目标地点, 成本函数.

2. 组成

路径规划组成

路径规划主要由两部分组成，前端是路径搜索，后端是**轨迹规划，**最后生成一条机器人可执行的路径。

**路径搜索——**在地图中，搜索出一条避开障碍物的轨迹；

**轨迹规划（优化）——**对搜索到的轨迹进行优化，从而符合机器人的运动学和动力学约束。

规划示意图

如上图所示，a路径为前端路径搜索得出的，为一条无碰撞的路径，b为a路径经过轨迹规划过的路径，该路径满足运动学和动力学约束，为一条机器人可执行的路径。

3. 分类

路径规划（搜索）的算法主要可分成这三类，后续的章节也是按照以下的顺序进行介绍的。

3.1 基于搜索的算法

DIjkstra、A*、JPS

3.2 基于采样的算法

PRM、RRT、RRT*

3.3 满足动力学约束的搜索算法

Hybrid A*

更多的内容可看下面的介绍：

焉知科技：路径规划五种算法简述及对比

路径规划算法总结

---

六、SLAM

SLAM是 Simultaneous Localization And Mapping的 英文首字母组合，一般翻译为：同时定位与建图、同时定位与地图构建。

「同时定位与地图构建」这几个词，乍一听起来非常拗口，为了不在一开始就吓跑读者，我们先不对其进行专业的解释，用一个日常生活中形象的例子来进行说明。

1.初步认识SLAM

我们知道现在有不少家用的扫地机器人，可以代替人对室内进行自动清扫。早期的扫地机器人并不智能，它只是具有简单的避障功能，在室内随机游走，遇到障碍物就转弯，这样会导致有很多地方会漏掉，扫地效率非常低。

而现在随着SLAM技术在扫地机器人中的应用，现在的扫地机器人已经变的非常智能，可以通过自身的传感器对室内进行扫描建图，根据当前的定位进行Z字形规划清扫，还能实现自动回充、断点续扫等高级功能。

根据前面介绍，我们总结一下，要想真正实现智能的清扫，扫地机器人至少需要知道以下几件事情：

1、我在哪里？也就是扫地机器人在工作过程中要知道自己在房间的具体位置。对应的术语叫：定位（Localization)。

2、我周围的环境是什么样子？也就是扫地机器人需要知道整个房间的地面结构信息。对应的术语叫：建图(Mapping)。

3、我怎样到达指定地点（充电器）？当扫地机器人电量不足时，如何以最短的路径到达充电器所在位置进行自动充电。对应的术语叫：路径规划（Route Planning）。

有了以上的几个能力，扫地机器人就变的非常智能了，不再像无头苍蝇一样在室内乱跑，而是可以从任意位置出发，按照建立好的地图进行规划清扫。当然房间里的物品摆放也会发生变化，所以扫地机器人每次清扫也会根据当前的定位及扫描情况，在建好的地图基础上对进行更新。随着使用时间的增长，扫地机器人建立的地图会越来越准确，规划的清扫路线越来越高效，变的越来越智能。

看明白了上面的例子，我们给出SLAM的定义。SLAM是指当某种移动设备（如机器人、无人机、手机等）从一个未知环境里的未知地点出发，在运动过程中通过传感器（如激光雷达、摄像头等）观测定位自身位置、姿态、运动轨迹，再根据自身位置进行增量式的地图构建，从而达到同时定位和地图构建的目的。定位和建图是两个相辅相成的过程，地图可以提供更好的定位，而定位也可以进一步扩建地图。需要说明的是，上述扫地机器人例子中，定位和建图是SLAM的基本要求，而路径规划是在此基础上的高级功能，不属于SLAM的讨论范畴。

2.SLAM的应用

SLAM所使用的传感器主要分为激光雷达和视觉两大类。在SLAM研究史上，早期SLAM研究几乎全使用激光雷达作为传感器，其优点是精度高，解决方案相对成熟。但是缺点也非常明显，比如价格贵、体积大，信息少不够直观等。

视觉SLAM就是用摄像头作为主传感器，用拍摄的视频流作为输入来实现同时定位与建图。视觉SLAM广泛应用于AR、自动驾驶、智能机器人、无人机等前沿领域。我们知道SLAM的两大核心：定位和建图。下面分别进行说明。

1、定位相关应用

SLAM在自动驾驶中最主要的功能就是更加精确的确定汽车自身的位置（当然建图也很重要）。说到这里，很多人会有疑问：现在手机上的百度/高德/腾讯等地图类App就可以定位，为什么还需要SLAM来定位呢？

原因是这样的：目前地图类App在室外定位、导航方面确实做的很不错，而且衍生出很多基于地理位置的游戏、社交、生活类应用。不过现在的定位系统仍然存在以下两个主要问题。

1、地图类App背后使用的是GPS技术，但民用GPS最多也就达到几米的定位精度，驾车的读者应该深有体会：这些地图类App导航时并不知道当前车辆在哪个车道上。

2、GPS只能在室外使用。而在建筑物内、洞穴、海底等很多地方，GPS会失效。而往往这些地方对于定位的需求很强烈，要想解决GPS失效区域的定位，目前最有效的就是SLAM技术。

如下图所示是著名开源算法ORB-SLAM2使用架设在汽车上的摄像头进行定位的结果。从图中不难看出，算法非常清晰的展现了汽车当前所在的车道。

同样的，对于室内使用的自主移动机器人来说，定位也完全依赖SLAM技术。如下图是单目SLAM的过程。此外像京东，阿里这种大型的电商已经在自己的仓库配备了仓储机器人，也称AGV（Automated Guided Vehicle），可以根据建立的地图确定自身的位置，然后根据任务需求进行路径规划来搬运货物。

2、建图相关应用

比如使用手机上的单目摄像头可以对物体进行扫描，生成对应的三维模型。这称为小尺度下的三维建模。当然，也可以对较大尺度的场景进行三维重建，比如下图下就是Google project tango利用鱼眼摄像头对室外的大场景进行的三维重建。

[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-yDWZU1fA-1655011794628)(https://raw.githubusercontent.com/xkyvvv/blogpic3/main/img/202206112338080.jpeg)]

最近几年，随着消费级RGB-D相机的普及，室内场景的三维重建也是一个非常热门的方向。利用重建的结果可以实现很多 功能，比如室内移动机器人可以根据重建结果判断障碍物距离、识别理解环境，进行导航；比如可以根据重建结果在家里玩增强现实游戏；还可以将二维图片和重建结果进行融合实现三维漫游等。

3.讨论

SFM（structure from motion）是一种和SLAM非常类似的算法，关于SFM和SLAM的异同，下面说法错误的是？

A. SFM和SLAM基本讨论的是同一问题，不过SFM最早来自计算机视觉领域，而SLAM则是来自于机器人领域。SFM的structure对应SLAM的mapping，SFM的camera pose对应SLAM的location。

B. SFM一般是离线处理，而SLAM更强调实时性，一般在线处理。

C. SFM处理的图片一般没有要求，可以是不同时间不同相机拍摄的同一个场景，而SLAM一般要求是同一相机拍摄的序列图像或连续视频。

D. 目前SFM主要是要完成3D reconstuction，而SLAM主要是要完成localization。

E. SFM通常使用摄像机作为传感器，而SLAM在传感器选择方面，除了摄像机，通常还包括惯导、激光雷达等传感器。

F. 如果想要快速对自由女神像进行3D重建，在没有特殊硬件的情况下，最好的方式是选择SLAM。

---