CN105910604A - 一种基于多传感器的自主避障导航系统 - Google Patents

Abstract

一种基于多传感器的自主避障导航系统，包括传感器处理层，局部算法处理层，错误探测层和中心处理层；传感器处理层包括若干个传感器处理节点，局部算法处理层包括若干个算法运算节点，错误探测层用于探测系统的硬件和软件错误，确定上述传感器处理节点和/或算法运算节点出现错误时，屏蔽出现错误的节点；中心处理层对局部算法处理层中若干个算法运算节点的结果进行全局优化。本发明采用了非集中式的融合设计，极大提高了系统的鲁棒性和容错性。

Description

一种基于多传感器的自主避障导航系统

技术领域

本发明属于机器人和无人飞机自主避障导航技术领域，特别涉及一种可根据外在环境的变化自动选择不同传感器组合的自主避障导航系统设计及其方法。

背景技术

随着科技的进步，机器人和无人飞机的应用领域越来越广泛。它们不仅可以提供各种服务(如家用服务型机器人，扫地机等)，还可以在复杂危险的环境下执行各种任务(如机器人进入日本福岛核电站事故现场探查内部的破坏情况，无人机航拍灾后现场等)。然而，相当多的应用依然是在人工远程遥控下完成的。要想机器人和无人机在复杂环境下完成复杂的任务，就必须要让它们具备在未知环境下的自主避障导航能力。自上世纪60年代，美国斯坦福大学就开始研究人工智能技术应用于机器人的自主推理、规划和控制，90年代以来，随着各类高精度传感器的产品化和信息处理技术的飞速发展，高适应性的移动机器人控制技术，真实环境下的自主导航等领域的研究进入到了新的阶段。进入本世纪，随着计算机技术，多传感器集成和控制技术的进一步成熟，机器人和无人飞机在真实环境里的自主避障导航开始成为可能。

自主避障导航系统可以分解为三个基本的子系统：绘图和定位子系统，避障子系统和路径规划子系统。绘图和定位模块是通过机器自身携带的各种传感器来感知周围的环境，快速绘制周围环境的地图，并计算出自身相对于环境地图的位置，速度和姿态等运动状态的最佳估计。避障子系统可以通过机器所携带的传感器来实时计算出自身到障碍物的距离，通过模糊逻辑或者人工智能的算法得到一个可靠的避障状态估计。路径规划模块有局部运算和全局运算两个功能，局部运算可以为机器如何绕开障碍物提供最佳的路径，全局运算则可以在周围环境已知的情况下计算出到达目的地的最佳路径。

机器人和无人机由于负载载荷，运动方式和使用环境不同，会采用不同的避障导航系统设计，主要是绘图和定位子系统的不同设计。比如，机器人上可以搭载较重的激光雷达，而无人飞机负载不够。又比如在室外环境下，可以采用全球卫星定位导航系统(GNSS)来获取绝对位置，而在室内环境是无法使用的。同样，在室内环境里，可以采用射频识别(RFID)技术来定位，而在室外环境里，这种方案是不现实的。

主流的避障导航系统会采用激光雷达，视觉传感器，惯性导航传感器，超声波传感器和RGB-D传感器等。但是，这些传感器都存在一定的局限性。

激光雷达可以快速获得周围环境的三维信息，广泛用于各种尺度的三维模型重建。近年来，激光雷达也被用于机器人避障定位导航。基于激光雷达的运动状态估计是通过连续帧的点云匹配，从而计算出相对运动变化。但是，由于每次点云匹配的时候总是会带有一些误差，经过一段时间之后，这种累计误差会导致定位精度严重降低。为了解决这种问题，往往需要采用闭环检测纠正技术(Loop Closure)。另外，激光雷达可以快速大范围扫描周围的环境，从而直接获得与前方障碍物的距离，避障算法简单有效。然而，激光雷达设备往往价格昂贵，体积大，重量沉，这大大限制了它在机器人和无人机避障定位导航上的应用。

视觉传感器由于造价便宜，体积小，重量轻，被广泛用于机器人和无人机的定位导航。视觉定位导航是通过对连续帧上的特征点跟踪匹配，从而计算出相机的相对运动。这类基于视觉的运动估计技术又称为视觉里程计技术，主要包括了单目视觉和双目视觉。单目视觉的问题在于无法无法提供绝对的尺度信息，仅能计算出相对的运动，无法获得实际的周围环境的深度信息。双目视觉中两个相机间的基线提供了绝对尺度信息，从而可以计算出真实的深度信息，但其运算往往复杂。视觉里程计技术的关键在于可以稳定可靠地跟踪特征点，这就要求环境的光线条件稳定和物体表面有丰富的纹理。这种要求大大限制了视觉里程计技术的应用场景。

惯性导航传感器的价格变化范围很大，在机器人和无人机上采用的往往是价格便宜，重量轻，体积小的MEMS惯性传感器。惯性传感器可以在短时间里提供可靠的加速度和角速度信息，从而计算出机器人和无人机的位置，速度和姿态。然而MEMS惯性传感器也存在较大的随机系统误差和时间漂移量，因此必须和其它传感器的测量值通过滤波技术来进行融合，以改正误差，提高运动状态的估计精度。

超声波传感器价格便宜，体积小，重量轻，广泛用于机器人和无人机避障。它是靠发射声波信号，利用物体界面上超声反射来检测障碍物的。超声波是具有一定方向性的波束，当超声波传感器和障碍物形成一定角度的时候，会发生镜面反射，反射而产生所谓的幻影，这种多路径效应会导致机器以为自身和障碍物还很远，从而发生误判。此外，声波在空气中传播衰减很快，有效探测距离一般只有5-10米。声波速度比光速慢许多，响应速度也较慢。

RGB-D传感器是一种新型视觉设备，可以同时获得周围环境的彩色图像和深度信息。它相对于激光雷达设备，价格便宜(大约是最便宜的激光雷达的1/20)，体积小，重量轻，这些优势推动其逐步取代激光雷达用于机器人和无人机的避障定位导航系统。近几年来，RGB-D传感器的室内移动机器人避障定位导航研究论文发表。不过这些研究，还基本沿用了激光雷达定位导航的思路，相当于用了一个便宜的激光雷达。虽然有些论文里谈到采用RGB相机辅助发现闭合路径，总体来说，当前的RGB-D定位导航算法并没有更深入地挖掘RGB传感器 和D传感器的融合。

目前基于不同类型的传感器，发展出了多种场景的绘图定位技术，并用于不同的场景中。其中，最常用的就是二维栅格地图。然而，二维栅格地图有其局限性，它只包含了场景内某高度上的横截面信息，信息缺失将影响到机器人避障功能，并限制了机器人的自动导航。此外，这种地图几乎无法应用在无人机上。

当前，自主避障导航系统及方法的设计往往都局限于应用的场合，每个不同的应用场合，根据传感器种类及搭配的不同，具体避障导航系统及方法也相应有区别，这样导致需要设计多种不同的避障导航系统及方法；同时，如果某个传感器故障，会导致避障导航系统及方法无法很好地适应，使得整个系统鲁棒性较差。目前还没有适应性好、容错性高的通用自主避障导航系统及方法。

发明内容

本发明针对当前自主避障导航系统设计的局限性，首次提出一种容错性高的自适应环境变化的自主导航避障系统设计及方法。该系统采用非集中融合设计，包括传感器数据处理层，局部算法处理层和主算法处理层。传感器数据处理层，按每个传感器的计算任务设计运算节点，所有的运算节点彼此独立，可并行运算。局部算法处理层，针对所有可能的传感器组合设计了不同的算法节点，各局部算法节点彼此独立，可并行处理。局部算法器的结果传递给主算法器，做全局优化。硬件上，本系统可以采用多个小型单机版电脑，比如树莓派零，来执行各节点的运算。本系统可以自动探测传感器的硬件错误和软件错误，从而选择最可靠的传感器组合和局部算法。这种设计具有高度可扩展性和容错性，便于根据需要添加和去除某些传感器，也能确保在某些传感器失灵的情况下，依然完成自主避障导航的任务。

针对以上自主避障导航系统设计的局限性，本发明提供了灵活的容错性高的系统设计。基于本发明，增加和去除传感器变得容易，即便在部分传感器失灵情况下，系统仍然可以完成任务，这大大拓展了机器人与无人机自主避障系统的应用领域。

为实现上述目的，本发明采用以下的技术方案：

一种基于多传感器的自主避障导航系统，包括

传感器处理层，局部算法处理层，错误探测层和中心处理层；

传感器处理层包括若干个传感器处理节点，每个传感器处理节点对多个传感器中的一个传感器的数据进行处理；每个传感器处理节点彼此独立，且分别处理不同的传感器数据；

局部算法处理层包括若干个算法运算节点，所述若干个算法运算节点分别基于两个或多个所述传感器处理节点中的输出数据进行运算；每个局部算法节点彼此独立，且分别基于不 同的输出数据进行运算；

错误探测层用于探测系统的硬件和软件错误，确定上述传感器处理节点和/或算法运算节点出现错误时，屏蔽出现错误的节点；

中心处理层对局部算法处理层中若干个算法运算节点的结果进行全局优化。

一种基于多传感器的自主避障导航系统，具有定位绘图子系统，所述定位绘图子系统包括：传感器处理层，局部算法处理层；

传感器处理层包括若干个定位传感器处理节点，每个定位传感器处理节点对多个定位传感器中的一个定位传感器的数据进行处理；每个定位传感器处理节点彼此独立，且分别处理不同的定位传感器数据；

局部算法处理层包括若干个算法运算节点，所述若干个算法运算节点分别基于两个或多个所述定位传感器处理节点中的输出数据进行运算；每个局部算法节点彼此独立，且分别基于不同的输出数据进行运算。

所述定位传感器包括图像传感器、深度传感器和惯导传感器。

所述定位传感器处理节点包括基于惯性传感器和图像传感器数据的扩展卡尔曼滤波器、基于惯性传感器和深度传感器数据的扩展卡尔曼滤波器，以及基于图像传感器和深度传感器数据的扩展卡尔曼滤波器。

所述定位绘图子系统还包括错误探测层和中心处理层；错误探测层用于探测定位绘图子系统的硬件和软件错误，确定上述定位传感器处理节点和/或算法运算节点出现错误时，屏蔽出现错误的节点；中心处理层，基于局部算法处理层中若干个算法运算节点的结果产生多个点云，并对多个点云进行融合，获得三维地图。

一种基于多传感器的自主避障导航系统，具有避障子系统，所述避障子系统包括：传感器处理层，局部算法处理层；

传感器处理层包括若干个障碍感知传感器处理节点，每个障碍感知传感器处理节点对多个障碍感知传感器中的一个障碍感知传感器的数据进行处理；每个障碍感知传感器处理节点彼此独立，且分别处理不同的障碍感知传感器数据；

局部算法处理层包括若干个算法运算节点，所述若干个算法运算节点分别基于两个或多个所述障碍感知传感器处理节点中的输出数据进行运算；每个局部算法节点彼此独立，且分别基于不同的输出数据进行运算。

所述障碍感知传感器包括若干个红外传感器，若干个超声波传感器和深度传感器。

所述障碍感知传感器处理节点包括基于惯性传感器和图像传感器数据的模糊逻辑控制器、基于惯性传感器和深度传感器数据的模糊逻辑控制器，以及基于图像传感器和深度传感 器数据的模糊逻辑控制器。

所述避障子系统还包括错误探测层和中心处理层；错误探测层用于探测避障子系统的硬件和软件错误，确定上述障碍感知传感器处理节点和/或算法运算节点出现错误时，屏蔽出现错误的节点；中心处理层，对局部算法处理层中若干个算法运算节点的结果进行优化处理。

一种基于多传感器的自主避障导航系统，包括上述定位绘图子系统和上述避障子系统。

一种无人机或机器人，包括上述基于多传感器的自主避障导航系统。

以上所述的自主避障导航系统设计的发明点在于：

(1)对定位绘图子系统和避障子系统采用了非集中式的融合设计，将传感器数据处理和局部算法运算分开，极大提高了系统的鲁棒性和容错性。

(2)在传感器数据处理层，各传感器数据处理节点可以并行运算。在局部算法处理层，各局部算法节点可以并行运算。

(3)在错误探测层，系统可以自动探测到硬件和算法的错误，并及时屏蔽不可靠的局部节点，保证主算法处理层能够正常运行。

本发明的有益效果为：采用了非集中式的融合设计，极大提高了系统的鲁棒性和容错性。该系统的设计灵活，添加或去掉某个传感器时，只需要添加或去除对应的传感器处理节点和算法节点即可。当传感器信号出现错误时，也能够自动方便地屏蔽掉该传感器节点，而不影响系统正常运行，从而可以自适应不同的环境(例如，室内室外白天黑夜的环境)，极大提高了机器人和无人机的应用领域。

附图说明

下面根据附图对本发明作进一步详细说明。

图1为本发明的非集中式融合系统结构框图

图2为本发明的定位绘图子系统的一种硬件俯视图

图3为本发明的一种可能的定位绘图子系统示意图

图4(a)为本发明定位绘图子系统所实现的MEMS+RGB的扩展卡尔曼滤波实现的流程图

图4(b)为本发明定位绘图子系统所实现的MEMS+D的扩展卡尔曼滤波实现的流程图

图4(c)为本发明的定位绘图子系统所实现的RGB+D的扩展卡尔曼滤波实现的流程

图5为本发明的避障子系统的一种硬件俯视图

图6为本发明的一种可能的避障子系统示意图

具体实施方式

一种自适应环境的自主避障导航系统的设计和实现方法，包括基于非集中式融合设计的定位绘图子系统，避障子系统和路径规划子系统。

定位绘图子系统用RGB‐D传感器和MEMS惯性导航传感器快速绘制三维地图并定位。该子系统可以探测MEMS传感器，RGB即彩色相机和D即深度相机的硬件故障和数据错误，依此选择合适的组合算法来实现定位绘图。

避障子系统用红外传感器，超声波传感器和D传感器来获取自身到障碍物的距离。该子系统可以探测到各传感器的硬件故障和数据错误，依此选择合适的策略来确定自身与障碍物的距离状态。

路径规划子系统实现了多个不同的算法，比如人工势场路径规划。该子系统会根据自身位置与所用地图的状态来确定所采用的算法。

定位绘图子系统的实现方案包括：

步骤一：传感器数据处理层的实现，以每个传感器的计算任务为基本数据处理单元，设计若干运算节点。在本实例中，采用了MEMS惯性导航传感器，RGB即彩色相机和D即深度传感器。

步骤二：局部滤波器处理层的设计，实现三个局部的扩展卡尔曼滤波器节点，即MEMS+RGB，MEMS+D和RGB+D三个局部滤波器。每个滤波器都独立并行计算，并得到运动模型的参数估计，即相对的位置移动和姿态变化。

步骤三：探测该子系统的硬件和软件错误。硬件错误一般在传感器数据读取的时候就会发现。软件错误是指数据处理节点和局部滤波器节点的错误。

步骤四：根据错误检测的结果，来选择合适的局部滤波器，把结果传递给主处理器做全局优化。这里会最终确定最佳的运动状态估计，即位置和姿态的估计。基于此，构建和更新三维地图。

避障子系统的实现方案包括：

步骤一：传感器数据处理层的实现，在本实例中，采用了红外传感器，超声波传感器和D传感器。

步骤二：本实例对每个传感器实现一个局部模糊控制器。对基于距离的隶属函数，设置输入的语言集合。对输出数据的隶属函数，设置输出的语言集合。根据模糊逻辑控制理论，基于输入和输出的语言集合设计模糊逻辑规则。将不同传感器的最佳探测距离范围，转化为一种可靠性(即概率)一并带入到输出数据。

步骤三：探测该子系统的硬件和软件错误。

步骤四：根据错误探测的结果，把有效的输出发送到主处理器，主处理根据输出到可靠性做加权平均获得最终的机器人和无人机的运动控制命令。

路径规划子系统的实现方案

对局部路径规划采用标准人工势能算法(已知算法)。它的基本思想是将机器在周围环境中的运动，设计成一种抽象的人造引力场中的运动。该经典算法的优势在于运算速度快，规划出来的路径平滑且安全。当全局的三维地图和目的地已知的情况下，直接采用基于最短路径的栅格法(已知算法)。它的基本思想是把已知三维地图抽象为一个二维的平面栅格，然后在该栅格上标记出目的地，再通过经典的最短路径算法来规划路径。

下面结合附图对本发明做进一步的详述。

本发明中的定位绘图子系统和避障子系统都采用了非集中式融合设计。图1为非集中式融合的设计图，整个系统分为四个层，分别是传感器处理层，局部算法处理层，错误探测层和中心处理层。传感器处理层内实现了若干个传感器处理节点，用于对传感器的数据进行处理，这些运算节点彼此独立，可并行处理。局部算法处理层内实现了若干个不同的算法，每个算法实现都是一个算法运算节点，这些局部算法节点彼此独立，可并行处理。错误探测层主要是自动探测系统的硬件和软件错误，一旦确定某个节点出现错误，可以及时屏蔽该节点而不至于影响整个系统的正常运行。中心处理层实现了一个中心算法，中心算法会对各局部子算法的结果做全局的优化。下面具体介绍如何实施定位绘图子系统和避障子系统。

定位绘图子系统具体实施方案

图2给出了一种可能的定位绘图子系统的硬件实现，包括了RGB传感器1，D深度传感器2和MEMS惯导传感器3。图3给出了一种可能的非集中式融合的定位绘图子系统设计方案。

首先是传感器的数据处理层的设计，以每个传感器的计算任务为基本数据处理单元，设计若干运算节点。MEMS运算节点任务是通过对加速度和角速度的积分获得运动的速度，位置和姿态。RGB运算节点任务是执行特征提取和跟踪，并计算出相对运动的位置变化和姿态变化。D运算节点的任务是对连续深度帧的点云数据做ICP运算，从而获得相对运动的位置变化和姿态变化。

然后，实现了3个局部的扩展卡尔曼滤波器(EKF)，分别是MEMS+RGB，MEMS+D和RGB+D。MEMS+RGB的扩展卡尔曼滤波实现的流程图如图4(a)所示，采用算法是单目相机和MEMS的扩展卡尔曼滤波。在RGB传感器连续帧上提取和跟踪特征点完成相对定向，即确定连续帧间的位置和姿态相对变化。基于MEMS传感器数据处理节点提供的位置和姿态信息可以有效缩小连续帧上特征点的搜索空间，从而提高运算的效率。滤波器输出的是对位置和姿态的最佳估计。MEMS+D的扩展卡尔曼滤波实现的流程图如图4(b)所示。首先，通过D传感器的连续帧产生连续点云，然后通过ICP算法(已知算法)对连续点云进行匹配，其中MEMS传感器数据处理节点所获得的位置和姿态信息可以帮助提供ICP算法的初始条件，从而提高ICP算法的成 功率。滤波器输出的是对位置和姿态的最佳估计。RGB+D的扩展卡尔曼滤波实现的流程如图4(c)所示。首先，RGB传感器和D传感器之间的几何关系必须通过严格的检较来确定，即通过双目相机检较的方式来确定RGB和D传感器之间的相对位移和姿态。通过这个关系，就确定了RGB像平面空间和D像平面空间的映射关系。基于深度图像的点云提取的特征(不局限于点特征)可以通过映射关系变换到RGB的像空间去，从而辅助RGB连续帧上对特征的提取和跟踪，即深度信息辅助的特征融合。基于此，可以计算出连续帧的位置和姿态的相对变化，即对位置和姿态的估计。这3个局部算法器的输出都是相对的位置变化和姿态变化。

接下来，探测错误层需要对传感器数据处理层和局部算法处理层探测错误。硬件错误往往可以直接通过数据包错误来发现。软件错误包括了原始数据处理节点中的错误和滤波器错误。RGB处理节点的错误可能是无法提取和跟踪特征点，比如在黑暗的环境里或者周围物体表面没有任何纹理。D处理节点的错误可能获得无效的深度信息，比如在户外强烈阳光直射的情况下。这一类的数据错误可以通过阀值检测来探测。

RGB数据错误可能的检测方法：

对每幅影像灰度值做直方图，对直方图进行分析，其理想分布应该接近于平均分布。如果其分布过于集中于某个狭窄区域，比如全黑或者全白，则认为数据错误。

RGB图像无法提取和跟踪足够的特征点，则认为数据错误。

基于RGB图像的相对定向结果的协方差矩阵分析，方差过大，则认为数据错误。

D传感器数据错误可能的检测方法：

对每帧深度图像的红外灰度值做直方图，与RGB直方图测错方法一样。

ICP算法无法收敛，则认为数据出错。

另外一类软件错误是滤波器错误，滤波器错误的探测方法是基于数理统计的方法。这里仅结合本实例，给出一种基于χ^2的检测方法，并解释。每个局部的扩展卡尔曼滤波实现是基于两个传感器的数据融合。结合本实例，观测方程的观测量就是由位置，速度和姿态组成的观测向量。一般的观测方程如下：

y_k＝H·x_k+v_k,v_k～N(0,R_k)

在滤波器运行正常的情况下，残差v应该是符合高斯正态分布，其数学期望为0，方差R，且协方差为0。

设计一个假设检验如下：

基于残差的方差D{v_k}计算统计量：

Ω：＝v_k^T D{v_k}^(‐1)v_k

假设检验：

H_0:E{v_k}＝0对H_a:E{v_k}≠0

在假设条件H_0下，统计量Ω应该符合χ^2－分布，据此，可判断滤波器是否运行正常。

Ω>χ_(n,α)^2滤波器错误

Ω<χ_(n,α)^2滤波器正常

最后，根据错误检测的结果，把有效的局部算法器的结果传递给主算法器做全局优化。在没有发生错误的情况下，三个局部算法器会把位置和姿态的最佳估计，传递给主算法器。主算法器会基于局部算法器的结果来产生3组不同的点云，并对它们再次通过ICP算法融合，从而获得最终的三维地图。当系统局部失灵的情况下，主算法器依然可以通过其它正常的局部算法器获得位置和状态的估计，并产生三维环境地图。另外，无论采用哪种组合，总会有误差存在，这种累积的误差会导致整个三维地图的精度随着时间而下降。这个问题可以通过闭环路径纠正来解决。在本实例中，主算法器会保留关键帧的数字影像和点云，通过比对当前帧的数据和关键帧数据，来判断是否发现闭合路径。一单发现闭合路径，则采用ELCH算法(已知算法)做全局优化，并更新地图。

避障子系统具体实施方案

图5给出了一种可能的避障子系统的硬件实现方案，包括了红外传感器阵列1，D深度传感器2，超声波传感器阵列3和MEMS惯导传感器4。红外传感器阵列包括了放置在机器人和无人机头部左侧，中间和右侧的三个红外传感器。超声波阵列包括了同样位置放置的三个超声波传感器。D传感器放置在中间的部位。这样放置的目的是使整个前部的探测扇面角度可以达到180度。

图6给出了一种可能的非集中式融合设计的避障子系统设计图。

首先，是传感器数据处理层的实现，3个红外和3个超声波传感器分别测量自身到障碍物左侧，中间和右侧的距离。D深度传感器可以给出整个前方覆盖范围的深度值，即到障碍物表面的距离(该实施例仅仅是为了说明的举例，本领域技术人员可以理解，上述传感器可以是其他类型传感器，传感器的数量也可以根据需要变化，并不限于此)。一共实现7个数据运算节点，它们可以并行运算，并提供出机器到障碍物的距离信息。

由于机器人和无人机的避障环境可以非常复杂，很难像定位绘图子系统那样基于运动模型建立精确的各种滤波器，采用模糊逻辑控制算法是更合适的选择。基于非集中式数据融合的设计思路，对每一种传感器会设计一个模糊逻辑控制器。按本实例，对红外传感器，超声波传感器和D传感器设计并实现3个局部的模糊逻辑控制器。模糊控制器的输入是距离信号，输出的是移动机器人或者无人机的动作，暂规定前进或旋转两种运动模式。根据一定的控制策略，建立输入输出的模糊语言集合和模糊控制规则。根据输入的距离数据建立隶属度函数， 其输入模糊语言集合为{近，中，远}。模糊控制器的输出为移动机器人或者无人机的前进或者旋转的运动命令，建立输出变量的隶属度函数，输出的语言集合为{右转，前进，左转}。红外传感器根据其CCD的分辨率存在一个最近探测距离，对中远距离的障碍物有比较高的测距精度。超声波传感器的探测距离在5‐10米，因此比较适合中等距离的障碍物探测。D传感器的有效范围在3米以内，适合近距离的障碍物探测。这些传感器的特性需要充分体现在模糊控制器的设计中。即在设定规则的时候，会根据每个传感器的最佳测距范围，加权输出，即对输出的控制参数附加一个取值范围在0到1间的可信度。

接下来，探测错误层探测硬件和软件错误。硬件错误可以通过数据包错误发现。软件错误主要就是由于各个传感器的错误数据所引起的模糊控制器错误。假设不存在软件错误的情况下，各模糊控制器所输出的运动参数应该是非常接近的，如果彼此间的输出出现较大区别的时候，以输出的可信度为标准决定模糊控制器的错误。

最后，将各局部模糊控制器的输出信息发送给主控制器，主控制会加权平均出合适的输出信息。同时会结合由定位绘图子系统所构建的三维地图来验证该输出运动命令是否可行。

在本实例中，路径规划子系统实现了人工势场方法的局部路径规划和基于三维地图的全局路径规划(已知算法)。

以上所描述的仅为本发明的一种实现实例，仅用于解释本发明，而非针对本发明的限制。本发明的保护范围并不局限于此。具体算法和所采用的传感器都可以改变和优化，但凡是基于本专利所提的系统设计和算法框架都因属于本专利的保护范畴。

Claims (10)

1.一种基于多传感器的自主避障导航系统，其特征在于：包括

传感器处理层，局部算法处理层，错误探测层和中心处理层；

传感器处理层包括若干个传感器处理节点，每个传感器处理节点对多个传感器中的一个传感器的数据进行处理；每个传感器处理节点彼此独立，且分别处理不同的传感器数据；

局部算法处理层包括若干个算法运算节点，所述若干个算法运算节点分别基于两个或多个所述传感器处理节点中的输出数据进行运算；每个局部算法节点彼此独立，且分别基于不同的输出数据进行运算；

错误探测层用于探测系统的硬件和软件错误，确定上述传感器处理节点和/或算法运算节点出现错误时，屏蔽出现错误的节点；

中心处理层对局部算法处理层中若干个算法运算节点的结果进行全局优化。

2.一种基于多传感器的自主避障导航系统，其特征在于：具有定位绘图子系统，所述定位绘图子系统包括：传感器处理层，局部算法处理层；

传感器处理层包括若干个定位传感器处理节点，每个定位传感器处理节点对多个定位传感器中的一个定位传感器的数据进行处理；每个定位传感器处理节点彼此独立，且分别处理不同的定位传感器数据；

局部算法处理层包括若干个算法运算节点，所述若干个算法运算节点分别基于两个或多个所述定位传感器处理节点中的输出数据进行运算；每个局部算法节点彼此独立，且分别基于不同的输出数据进行运算。

3.一种如权利要求2所述的基于多传感器的自主避障导航系统，其特征在于：所述定位传感器包括图像传感器、深度传感器和惯导传感器。

4.一种如权利要求2所述的基于多传感器的自主避障导航系统，其特征在于：所述定位传感器处理节点包括基于惯性传感器和图像传感器数据的扩展卡尔曼滤波器、基于惯性传感器和深度传感器数据的扩展卡尔曼滤波器，以及基于图像传感器和深度传感器数据的扩展卡尔曼滤波器。

5.一种如权利要求2-4中所述的基于多传感器的自主避障导航系统，其特征在于：所述定位绘图子系统还包括错误探测层和中心处理层；错误探测层用于探测定位绘图子系统的硬件和软件错误，确定上述定位传感器处理节点和/或算法运算节点出现错误时，屏蔽出现错误的节点；中心处理层，基于局部算法处理层中若干个算法运算节点的结果产生多个点云，并对多个点云进行融合，获得三维地图。

6.一种基于多传感器的自主避障导航系统，其特征在于：具有避障子系统，所述避障子系统包括：传感器处理层，局部算法处理层；

传感器处理层包括若干个障碍感知传感器处理节点，每个障碍感知传感器处理节点对多个障碍感知传感器中的一个障碍感知传感器的数据进行处理；每个障碍感知传感器处理节点彼此独立，且分别处理不同的障碍感知传感器数据；

局部算法处理层包括若干个算法运算节点，所述若干个算法运算节点分别基于两个或多个所述障碍感知传感器处理节点中的输出数据进行运算；每个局部算法节点彼此独立，且分别基于不同的输出数据进行运算。

7.一种如权利要求6所述的基于多传感器的自主避障导航系统，其特征在于：所述障碍感知传感器包括若干个红外传感器，若干个超声波传感器和深度传感器。

8.一种如权利要求6所述的基于多传感器的自主避障导航系统，其特征在于：所述障碍感知传感器处理节点包括基于惯性传感器和图像传感器数据的模糊逻辑控制器、基于惯性传感器和深度传感器数据的模糊逻辑控制器，以及基于图像传感器和深度传感器数据的模糊逻辑控制器。

9.一种如权利要求6-8中所述的基于多传感器的自主避障导航系统，其特征在于：所述避障子系统还包括错误探测层和中心处理层；错误探测层用于探测避障子系统的硬件和软件错误，确定上述障碍感知传感器处理节点和/或算法运算节点出现错误时，屏蔽出现错误的节点；中心处理层，对局部算法处理层中若干个算法运算节点的结果进行优化处理。

10.一种基于多传感器的自主避障导航系统，其特征在于：包括定位绘图子系统、避障子系统；所述定位绘图子系统与权利要求2-5中任意一项自主避障导航系统中的定位绘图子系统相同；所述避障子系统与权利要求6-9中任意一项自主避障导航系统中的避障子系统相同。