CN106444780A - 一种基于视觉定位算法的机器人的自主导航方法及系统 - Google Patents

Abstract

本发明提供了一种基于视觉定位算法的机器人的自主导航方法及系统，方法为：获得机器人所在区域的地图信息，初始位置信息及目标位置信息，在机器人行走过程中，获得机器人的累积位置信息，对机器人的累积位置信息进行修正，得到机器人的当前位置信息；获得机器人行走过程中周围的障碍信息，结合初始全部行走路径，进行路径规划处理，得到机器人的全局行走路径，获得机器人行走过程中周围的障碍信息，计算得到机器人行走的方向及速度，机器人进行行走，将当前位置信息与目标位置信息进行匹配，实现机器人的自主导航。本发明利用机器人周围的障碍信息，结合机器人自身位置信息，为机器人提供一条合理的行走路径，得到了精准的导航效果。

Description

一种基于视觉定位算法的机器人的自主导航方法及系统

技术领域

本发明涉及计算机视觉领域和自动化控制领域，尤其涉及一种基于视觉定位算法的机器人的自主导航方法及系统。

背景技术

自动化控制技术广泛用于工业、农业、军事、科学研究、交通运输、商业、医疗、服务和家庭等方面。采用自动化控制不仅可以把人从繁重的体力劳动、部分脑力劳动以及恶劣、危险的工作环境中解放出来，而且能扩展人的器官功能，极大地提高劳动生产率，增强人类认识世界和改造世界的能力。因此，自动化控制是工业、农业、国防和科学技术现代化的重要条件和显著标志。

导航是指运动体按照预先给定的任务命令，根据已知的地图信息做出全局路径规划，并在行进过程中，不断感知周围的局部环境信息，做出各种决策，随时调整自身的姿态与位置，引导自身安全行驶，直至目标位置。目前常见的导航方法有：惯性导航、电磁导航、卫星导航、激光导航、红外导航、无线电导航、视觉导航及各种方式的组合导航等。因此，自动化的导航的应用具有重大的意义。

传统视觉导航是采用CCD摄像头拍摄路面图像，运用机器视觉等相关技术识别路径，实现自动导航的一种导航方法。视觉导航通常采用被动工作方式，设备简单、成本低、经济性好、应用范围较广，在理论上具有最佳引导柔性，因此近年来发展非常迅速。对于在机器人的视觉导航中的应用，采用CCD摄像头的被动工作方式进行导航，主要是通过CCD摄像头拍摄的路面信息，指导机器人行走，但使用此种方式进行导航，不能对周围的复杂情况进行分析，为机器人提供精准的导航，比如机器人行进路径周围存在不同的障碍物，会阻碍机器人的正常行走，机器人不能自主避开障碍，而且受周围环境影响，机器人不能一条合理的最佳的行走路径，最终导致导航效果不好。

因此，现有技术中的技术缺陷是：现有技术中的机器人的视觉导航技术是基于CCD摄像头拍摄路面图像实现的，由于采用的是被动的工作方式，不能对机器人周围复杂情况进行考虑，且不能自主根据周围环境实时改变机器人的行走状态，导致导航效果不好。

发明内容

针对上述技术问题，本发明提供一种基于视觉定位算法的机器人的自主导航方法及系统，利用机器人周围的障碍信息，结合机器人自身位置信息，为机器人提供一条合理的行走路径，并实时对机器人的行走状态进行改变，得到了精准的导航效果。

为解决上述技术问题，本发明提供的技术方案是：

第一方面，本发明提供一种基于视觉定位算法的机器人的自主导航方法，包括：

步骤S1，获得机器人所在区域的地图信息、所述机器人的初始位置信息及目标位置信息；

步骤S2，根据所述地图信息，结合所述初始位置信息及目标位置信息，通过中央处理器进行规划处理，得到所述机器人的初始全局行走路径，所述初始全部行走路径作为所述机器人的行走路径；

步骤S3，在所述机器人行走过程中，通过多个传感器分别实时获得所述机器人行走过程中周围的环境信息，所述机器人的累积位置信息，所述周围的环境信息包括所述机器人周围的障碍信息和高度障碍物信息；

步骤S4，通过视觉定位算法对所述机器人的累积位置信息进行修正，得到所述机器人的当前位置信息；

步骤S5，根据所述当前位置信息，所述机器人周围的障碍信息和所述初始全部行走路径，通过所述中央处理器进行路径规划处理，得到所述机器人的全局行走路径，所述全局行走路径作为所述机器人的行走路径；

步骤S6，根据所述周围的环境信息，计算得到所述机器人行走的方向及速度，所述机器人按照所述方向及速度进行行走，将所述当前位置信息与所述目标位置信息进行匹配，实现所述机器人从初始位置到目标位置的自主导航。

本发明的基于视觉定位算法的机器人的自主导航方法，其技术方案为：先获得机器人所在区域的地图信息、所述机器人的初始位置信息及目标位置信息；然后根据所述地图信息，结合所述初始位置信息及目标位置信息，通过中央处理器进行规划处理，得到所述机器人的初始全局行走路径，所述初始全部行走路径作为所述机器人的行走路径；

接着在所述机器人行走过程中，通过多个传感器分别实时获得所述机器人行走过程中周围的环境信息，所述机器人的累积位置信息，所述周围的环境信息包括所述机器人周围的障碍信息和高度障碍物信息；然后，通过视觉定位算法对所述机器人的累积位置信息进行修正，得到所述机器人的当前位置信息；

接着，根据所述当前位置信息，所述机器人周围的障碍信息和所述初始全部行走路径，通过所述中央处理器进行路径规划处理，得到所述机器人的全局行走路径，所述全局行走路径作为所述机器人的行走路径；最后，根据所述周围的环境信息，计算得到所述机器人行走的方向及速度，所述机器人按照所述方向及速度进行行走，将所述当前位置信息与所述目标位置信息进行匹配，实现所述机器人从初始位置到目标位置的自主导航。

本发明的基于视觉定位算法的机器人的自主导航方法，首先结合机器人所在区域的地图信息，结合机器人的初始位置和目标位置，给机器人规划一条初始全局行走路径，然后在机器人行走过程中，通过机器人的传感器采集机器人周围的环境信息及其他自身状态信息(方向，行走速度等)，重新规划一条全局行走路径，可使机器人在行走过程中自主避免障碍物，并根据自身状态信息实时调整机器人的行走方向及速度，使机器人最终实现自主导航，到达目标位置。

本发明的基于视觉定位算法的机器人的自主导航方法，利用机器人周围的障碍信息，结合机器人自身位置信息，为机器人提供一条合理的行走路径，并实时对机器人的行走状态进行改变，得到了精准的导航效果。

进一步地，所述多个传感器包括深度摄像头、激光传感器和里程计；通过所述深度摄像头获得高度障碍物信息，通过所述激光传感器获得所述机器人周围的障碍物信息，通过所述里程计获得所述机器人的累积位置信息。

进一步地，所述步骤S2中，通过A*算法进行规划处理，得到所述机器人的初始全局行走路径，具体为：

对所述地图信息进行分析处理，将地图空间划分为多个障碍空间和多个可移动空间；

对所述地图空间进行膨胀处理，得到多个新的障碍空间和多个新的可移动空间；

根据多个所述新的障碍空间和多个所述新的可移动空间，形成新的地图空间，得到所述机器人的初始全局行走路径。

具体地，对所述地图空间进行膨胀处理，具体为：

获取每个所述障碍空间中的障碍点；

以每个所述障碍点为圆心，大于所述机器人高度的长度为半径的圆内作为新的障碍空间，得到多个新的障碍空间和多个新的可移动空间。

具体地，所述步骤S4，包括：

根据所述机器人的初始位置信息，在初始位置周围按照误差分布布设多个概率点，所述多个概率点作为视觉定位算法的初始状态空间；

在所述机器人行走过程中，所述初始状态空间中的多个点根据所述机器人的累积位置信息进行位置变换和姿态变换，得到变换后的多个点；

根据传感器模型，结合所述机器人周围的障碍物信息，对所述变换后的多个点的进行权重计算，得到所述变换后的多个点的权重；

对所述变换后的多个点的权重进行比较，通过重采样将权重大的点进行多次重采样，将权重小的点移除，得到的多个重采样的点形成状态空间；

根据所述高度障碍物信息，通过机器学习的方法对所述状态空间中的多个点进行分类，分类后的点形成所述状态空间中的类簇；

根据所述状态空间中的类簇，得到所述机器人的当前位置信息。

第二方面，本发明提供一种基于视觉定位算法的机器人的自主导航系统，包括：

初始化模块，用于获得机器人所在区域的地图信息、所述机器人的初始位置信息及目标位置信息；

初始全局路径模块，用于根据所述地图信息，结合所述初始位置信息及目标位置信息，通过中央处理器进行规划处理，得到所述机器人的初始全局行走路径，所述初始全部行走路径作为所述机器人的行走路径；

信息获取模块，用于在所述机器人行走过程中，通过多个传感器分别实时获得所述机器人行走过程中周围的环境信息，所述机器人的累积位置信息，所述周围的环境信息包括所述机器人周围的障碍信息和高度障碍物信息；

当前位置获取模块，用于通过视觉定位算法对所述机器人的累积位置信息进行修正，得到所述机器人的当前位置信息；

全局行走路径模块，用于根据所述当前位置信息，所述机器人周围的障碍信息和所述初始全部行走路径，通过所述中央处理器进行路径规划处理，得到所述机器人的全局行走路径，所述全局行走路径作为所述机器人的行走路径；

自主导航模块，用于根据所述周围的环境信息，计算得到所述机器人行走的方向及速度，所述机器人按照所述方向及速度进行行走，将所述当前位置信息与所述目标位置信息进行匹配，实现所述机器人从初始位置到目标位置的自主导航。

本发明的基于视觉定位算法的机器人的自主导航系统，其技术方案为：首先通过初始化模块，获得机器人所在区域的地图信息、所述机器人的初始位置信息及目标位置信息；

接着通过初始全局路径模块，根据所述地图信息，结合所述初始位置信息及目标位置信息，通过中央处理器进行规划处理，得到所述机器人的初始全局行走路径，所述初始全部行走路径作为所述机器人的行走路径；

然后通过信息获取模块，在所述机器人行走过程中，通过多个传感器分别实时获得所述机器人行走过程中周围的环境信息，所述机器人的累积位置信息，所述周围的环境信息包括所述机器人周围的障碍信息和高度障碍物信息；接着通过当前位置获取模块，通过视觉定位算法对所述机器人的累积位置信息进行修正，得到所述机器人的当前位置信息；

接着通过全局行走路径模块，根据所述当前位置信息，所述机器人周围的障碍信息和所述初始全部行走路径，通过所述中央处理器进行路径规划处理，得到所述机器人的全局行走路径，所述全局行走路径作为所述机器人的行走路径；

然后通过自主导航模块，根据所述周围的环境信息，计算得到所述机器人行走的方向及速度，所述机器人按照所述方向及速度进行行走，将所述当前位置信息与所述目标位置信息进行匹配，实现所述机器人从初始位置到目标位置的自主导航。

本发明的基于视觉定位算法的机器人的自主导航方法，首先结合机器人所在区域的地图信息，结合机器人的初始位置和目标位置，给机器人规划一条初始全局行走路径，然后在机器人行走过程中，通过机器人的传感器采集机器人周围的环境信息及其他自身状态信息(方向，行走速度等)，重新规划一条全局行走路径，可使机器人在行走过程中自主避免障碍物，并根据自身状态信息实时调整机器人的行走方向及速度，使机器人最终实现自主导航，到达目标位置。

本发明的基于视觉定位算法的机器人的自主导航系统，利用机器人周围的障碍信息，结合机器人自身位置信息，为机器人提供一条合理的行走路径，并实时对机器人的行走状态进行改变，得到了精准的导航效果。

进一步地，所述多个传感器包括深度摄像头、激光传感器和里程计；通过所述深度摄像头获得高度障碍物信息，通过所述激光传感器获得所述机器人周围的障碍物信息，通过所述里程计获得所述机器人的累积位置信息。

进一步地，所述初始全局路径模块具体用于通过A*算法进行规划处理，得到所述机器人的初始全局行走路径，具体为：

对所述地图信息进行分析处理，将地图空间划分为多个障碍空间和多个可移动空间；

对所述地图空间进行膨胀处理，得到多个新的障碍空间和多个新的可移动空间；

根据多个所述新的障碍空间和多个所述新的可移动空间，形成新的地图空间，得到所述机器人的初始全局行走路径。

进一步地，所述初始全局路径模块具体用于对所述地图空间进行膨胀处理，具体为：

获取每个所述障碍空间中的障碍点；

以每个所述障碍点为圆心，大于所述机器人高度的长度为半径的圆内作为新的障碍空间，得到多个新的障碍空间和多个新的可移动空间。

进一步地，所述当前位置获取模块具体用于：

根据所述机器人的初始位置信息，在初始位置周围按照误差分布布设多个概率点，所述多个概率点作为视觉定位算法的初始状态空间；

在所述机器人行走过程中，所述初始状态空间中的多个点根据所述机器人的累积位置信息进行位置变换和姿态变换，得到变换后的多个点；

根据传感器模型，结合所述机器人周围的障碍物信息，对所述变换后的多个点的进行权重计算，得到所述变换后的多个点的权重；

对所述变换后的多个点的权重进行比较，通过重采样将权重大的点进行多次重采样，将权重小的点移除，得到的多个重采样的点形成状态空间；

根据所述高度障碍物信息，通过机器学习的方法对所述状态空间中的多个点进行分类，分类后的点形成所述状态空间中的类簇；

根据所述状态空间中的类簇，得到所述机器人的当前位置信息。

附图说明

为了更清楚地说明本发明具体实施方式或现有技术中的技术方案，下面将对具体实施方式或现有技术描述中所需要使用的附图作简单地介绍。

图1示出了本发明第一实施例所提供的一种基于视觉定位算法的机器人的自主导航方法的流程图；

图2示出了本发明第二实施例所提供的一种基于视觉定位算法的机器人的自主导航方法的硬件系统示意图；

图3示出了本发明第二实施例所提供的一种基于视觉定位算法的机器人的自主导航方法的硬件系统流程图；

图4示出了本发明第三实施例所提供的一种基于视觉定位算法的机器人的自主导航系统的示意图。

具体实施方式

下面将结合附图对本发明技术方案的实施例进行详细的描述。以下实施例仅用于更加清楚地说明本发明的技术方案，因此只是作为示例，而不能以此来限制本发明的保护范围。

实施例一

图1示出了本发明第一实施例所提供的一种基于视觉定位算法的机器人的自主导航方法的流程图；如图1所示实施例一提供的一种基于视觉定位算法的机器人的自主导航方法，包括：

步骤S1，获得机器人所在区域的地图信息、机器人的初始位置信息及目标位置信息；

步骤S2，根据地图信息，结合初始位置信息及目标位置信息，通过中央处理器进行规划处理，得到机器人的初始全局行走路径，初始全部行走路径作为机器人的行走路径；

步骤S3，在机器人行走过程中，通过多个传感器分别实时获得机器人行走过程中周围的环境信息，机器人的累积位置信息，周围的环境信息包括机器人周围的障碍信息和高度障碍物信息；

步骤S4，通过视觉定位算法对机器人的累积位置信息进行修正，得到机器人的当前位置信息；

步骤S5，根据当前位置信息，机器人周围的障碍信息和初始全部行走路径，通过中央处理器进行路径规划处理，得到机器人的全局行走路径，全局行走路径作为机器人的行走路径；

步骤S6，根据周围的环境信息，计算得到机器人行走的方向及速度，机器人按照方向及速度进行行走，将当前位置信息与目标位置信息进行匹配，实现机器人从初始位置到目标位置的自主导航。

本发明的基于视觉定位算法的机器人的自主导航方法，其技术方案为：先

获得机器人所在区域的地图信息、机器人的初始位置信息及目标位置信息；然后根据地图信息，结合初始位置信息及目标位置信息，通过中央处理器进行规划处理，得到机器人的初始全局行走路径，初始全部行走路径作为机器人的行走路径；

接着在机器人行走过程中，通过多个传感器分别实时获得机器人行走过程中周围的环境信息，机器人的累积位置信息，周围的环境信息包括机器人周围的障碍信息和高度障碍物信息；然后，通过视觉定位算法对机器人的累积位置信息进行修正，得到机器人的当前位置信息；

接着，根据当前位置信息，机器人周围的障碍信息和初始全部行走路径，通过中央处理器进行路径规划处理，得到机器人的全局行走路径，全局行走路径作为机器人的行走路径；最后，根据周围的环境信息，计算得到机器人行走的方向及速度，机器人按照方向及速度进行行走，将当前位置信息与目标位置信息进行匹配，实现机器人从初始位置到目标位置的自主导航。

本发明的基于视觉定位算法的机器人的自主导航方法，首先结合机器人所在区域的地图信息，结合机器人的初始位置和目标位置，给机器人规划一条初始全局行走路径，然后在机器人行走过程中，通过机器人的传感器采集机器人周围的环境信息及其他自身状态信息(方向，行走速度等)，重新规划一条全局行走路径，通过中央处理芯片可使机器人在行走过程中自主避免障碍物，并根据自身状态信息实时调整机器人的行走方向及速度，使机器人最终实现自主导航，到达目标位置。

本发明的基于视觉定位算法的机器人的自主导航方法，利用机器人周围的障碍信息，结合机器人自身位置信息，为机器人提供一条合理的行走路径，并实时对机器人的行走状态进行改变，得到了精准的导航效果。

具体地，多个传感器包括深度摄像头、激光传感器和里程计；通过深度摄像头获得高度障碍物信息，通过激光传感器获得机器人周围的障碍物信息，通过里程计获得机器人的累积位置信息。

在系统初始化之后，用户指定机器人的目标位置，部署在机器人上的中央处理芯片规划出一条可行的路径，作为机器人的全局路径，中央处理芯片通过部署在机器人上的驱动芯片，对机器人进行自动操控。

在机器人行走的过程中，中央处理芯片通过部署在机器人上的传感器收集机器人周围的有效信息，传感器具体包括深度摄像头、激光传感器和里程计。其中具体作用为：通过激光传感器获得机器人周围的障碍物信息，通过深度摄像头获得高度障碍物信息，补充周围的障碍物信息，即深度摄像头和激光传感器提供机器人周围的障碍物信息，里程计提供机器人的累积位置信息。

具体地，传感器还包括磁强计、加速度计和陀螺仪，磁强计提供机器人的绝对方向，加速度计为提供机器人的瞬时加速度值，陀螺仪提供机器人的相对姿态信息。

具体地，步骤S2中，通过A*算法进行规划处理，得到机器人的初始全局行走路径，具体为：

对地图信息进行分析处理，将地图空间划分为多个障碍空间和多个可移动空间；

对地图空间进行膨胀处理，得到多个新的障碍空间和多个新的可移动空间；

根据多个新的障碍空间和多个新的可移动空间，形成新的地图空间，得到机器人的初始全局行走路径。

具体地，对地图空间进行膨胀处理，具体为：

获取每个障碍空间中的障碍点；

以每个障碍点为圆心，大于机器人高度的长度为半径的圆内作为新的障碍空间，得到多个新的障碍空间和多个新的可移动空间。

在机器人行走之前，首先根据给出的机器人所在区域的地图信息，通过A*算法设计一条从机器人初始位置到目标位置的初始全局行走路径。

具体的过程为：首先对地图信息进行分析，将整个地图划分为障碍空间和可移动空间，其次再对整个障碍空间进行膨胀处理，具体方法为以该障碍点为圆心，以略大于机器人的大小作为半径，将整个圆内区域作为新的障碍空间，遍历所有的旧障碍空间(障碍空间)，得到新的障碍空间即为最终的障碍空间。

优选地，本发明对A*算法的评估函数进行了修改，修改了其启发式代价算法，使用1.4倍的欧式距离作为其启发式代价计算方法，采用该计算方法能够使A*算法得出的路径更为平滑。在数据结构上，A*算法采用优先队列存储可移动空间中的点，提高了整个算法的计算速度。

本发明采用了A*算法作为全局规划算法，因为A*算法算法是一种静态路网中求解最短路最有效的直接搜索方法。通过上述改进的A*算法，导航系统得到一条可供机器人移动的全局路径。

具体地，步骤S4，包括：

根据机器人的初始位置信息，在初始位置周围按照误差分布布设多个概率点，多个概率点作为视觉定位算法的初始状态空间；

在机器人行走过程中，初始状态空间中的多个点根据机器人的累积位置信息进行位置变换和姿态变换，得到变换后的多个点；

根据传感器模型，结合机器人周围的障碍物信息，对变换后的多个点的进行权重计算，得到变换后的多个点的权重；

对变换后的多个点的权重进行比较，通过重采样将权重大的点进行多次重采样，将权重小的点移除，得到的多个重采样的点形成状态空间；

根据高度障碍物信息，通过机器学习的方法对状态空间中的多个点进行分类，分类后的点形成状态空间中的类簇；

根据状态空间中的类簇，得到机器人的当前位置信息。

在本发明中，视觉定位算法是集视觉算法、概率算法以及机器学习为一体的定位算法，首先根据机器人的初始位置信息，在初始状态周围按照误差分布撒下大量概率点，作为视觉定位算法的初始状态空间，在机器人行走的过程中，初始状态空间的点不断地根据里程信息作出位置变换以及姿态变幻，算法通过激光传感器收集的二维视觉信息，根据传感器模型对整个状态空间内的点的权重进行计算，通过重采样使权重大的点得到更多的后代，而权重小的点被移除，从而逐渐收敛成一个类簇。系统的状态则由所有点的加权平均得到。

再通过RGBD摄像头收集的三维视觉信息具像化出一个真实的三维环境，再通过机器学习的方法来更好的将状态空间的点聚集成类簇。

使用深度神经网络(DNN)对状态空间的点进行分类，步骤如下：首先收集数据集，具体在于对于不同的定位点，他在不同时间段上的状态空间的分布情况。然后将数据集分为两部分，一部分用于训练深度神经网络模型，另一部分用于测试深度神经网络模型。在训练深度神经网络模型的过程中，调节隐藏层的数量以及对应的参数，达到输出准确定位的目的。通过使用深度神经网路作为定位的辅助功能，最终当状态空间聚集为一个类簇时，那么系统即得到了一个精确地位置信息。

在整个自主导航过程中，系统会不断通过视觉匹配算法来修正里程计累积误差，实现高精度的定位需求。若在导航的途中，用户重新指定了新的目标位置，则机器人朝着新指定的目标位置自主导航。

实施例二

图2示出了本发明第二实施例所提供的一种基于视觉定位算法的机器人的自主导航方法的硬件系统平台示意图。如图2所示，整个系统主要分为三个部分，包括数据采集装置，数据分析平台和底层控制平台。数据采集装置是由各种传感器构成，具体包括深度摄像头、激光传感器、磁强计、加速度计、陀螺仪。各种传感器将其数据存储为数据采集装置中，供处理芯片使用。数据分析平台是一块中央处理芯片，处理芯片通过数据总线拿到数据采集装置中的传感器信息，再利用导航算法，计算出当前机器人的最佳行驶路线。底层控制平台主要包括底盘控制芯片、动力装置、测算元件、里程元件等设备，主要是接受中央处理芯片传输过来的运动信息，对可移动载体进行控制。

参见图3，图3示出了本发明第二实施例所提供的一种基于视觉定位算法的机器人的自主导航方法的硬件系统平台流程图。

具体实施步骤如下：

步骤1)：初始化系统，具体包括设置系统地图以及设置机器人初始位置，设置数据采集装置，包括深度摄像头和激光传感器、磁强计、加速度计、陀螺仪、里程计，收集机器人周围的环境信息，机器人的姿态信息，机器人的位置信息，并实时通过系统总线，实现中央处理芯片和底层驱动的信息交互。

步骤2)：当用户指定了机器人的目标位置后，中央处理芯片启动自主导航模块，首先根据当前机器人的初始位置以及目标位置，对机器人的行径路线做一个全局的规划，得到在系统默认地图中，机器人的最优行驶路线。系统采用了A*算法作为全局规划算法，因为A*算法算法是一种静态路网中求解最短路最有效的直接搜索方法。A*算法的公式为：

f(n)＝g(n)+h(n)

其中f(n)是从初始状态经由状态n到目标状态的代价估计，g(n)是在状态空间中从初始状态到状态n的实际代价，h(n)是从状态n到目标状态的最佳路径的估计代价，即启发式代价。本系统中选用1.4倍的欧式距离作为A*算法的启发式代价。

步骤3)：在机器人运动的过程中，不断的通过传感器采集周围环境信息，具体包括激光信息，RGBD摄像头信息，以及里程信息。激光信息是用来描述机器人周围的二维视觉信息，做一个全局的把控；RGBD摄像头信息是对机器人前方行驶区域做一个具象化的三维场景还原；里程信息是对机器人当前位置的一个粗略描述。

步骤4)：中央处理芯片在收集数据后，通过视觉定位算法，对里程计信息进行修正，存储该修正后的位置信息。

在本发明的方法中，视觉定位算法是集视觉算法、概率算法以及机器学习为一体的定位算法，首先根据机器人的初始位置信息，在初始状态周围按照误差分布撒下大量概率点，作为视觉定位算法的初始状态空间，在机器人行走的过程中，初始状态空间的点不断地根据里程信息作出位置变换以及姿态变换，然后通过激光传感器收集的二维视觉信息，即机器人周围的障碍物信息，根据Likelyhood field传感器模型对整个状态空间内的点的权重进行计算，通过重采样使权重大的点得到更多的后代，而权重小的点被移除，从而逐渐收敛成一个类簇。状态空间则由所有点的加权平均得到。

伪代码形式如下所示：

initial

x₁～p(x₀)

loop(k-1to k)

for i＝1:N

x_k ⁽ⁱ⁾～p(x_k|x_k-1 ⁽ⁱ⁾)

w_k ⁽ⁱ⁾＝p(y_k|x_k ⁽ⁱ⁾)

endfor

w_k＝w_k/sum(w_k)

x_k＝sum(w_kx_k)

endloop

再通过RGBD摄像头收集的三维视觉信息(高度障碍物信息)具像化出一个真实的三维环境，再通过机器学习的方法来更好的将状态空间的点聚集成类簇。

使用深度神经网络(DNN)对状态空间的点进行分类，步骤如下：首先收集数据集，具体在于对于不同的定位点，他在不同时间段上的状态空间的分布情况。然后将数据集分为两部分，一部分用于训练深度神经网络模型，另一部分用于测试深度神经网络模型。在训练深度神经网络模型的过程中，调节隐藏层的数量以及对应的参数，达到输出准确定位的目的。通过使用深度神经网路作为定位的辅助功能，最终当状态空间聚集为一个类簇时，那么系统即得到了一个精确地位置信息。

步骤5)：在计算出精确的定位信息后，实时避障算法通过修改后的定位信息，激光信息以及RGBD摄像头信息，计算出当前机器人的最佳行驶速度。并将该速度传给机器人的底层驱动芯片。

本发明中采用的实时避障算法是一种基于视觉的实时避障算法，同时利用了二维视觉信息以及三维视觉信息来更新障碍空间。该算法首先利用激光信息，获取了机器人的底层平面障碍信息，其次通过RGBD摄像头获取机器人正面的三维立体障碍信息(高度障碍物信息)。通过平面信息以及立体信息的融合能够准确的发现机器人的前方的障碍区，尽早的进行避障。再者，通过全局二维信息的不断刷新，可以有效的更新机器人周围的障碍信息，更新周围障碍的代价，计算出机器人的最佳行驶速度。

其中，计算机器人最佳行驶速度的方法如下：

evaluation(v,w)＝A1*heading(v,w)+A2*2dist(v,w)+A3*3dist(v,w)+A4*velocity(v,w)

其中A_i表示调节参数，用于调节各项距离的占比；heading表示朝向目标点的距离；2dist表示远离2D障碍物的距离；3dist表示原理3D障碍物的距离；velocity表示将机器人的速度最大化。

步骤6)：机器人的底层驱动芯片获取机器人最佳行驶速度，解算出此运动状态下，要达到该运动速度所需要的动力，并将其传入PID控制器，通过PID控制器的自反馈机制，从硬件的角度，精确估计出此状态下所需要的动力，并且传输给动力装置。动力装置传输给运动模块控制可移动载体的移动，实现机器人的自主导航。与此同时，动力装置也会将运动信息传输给测速模块以及里程模块，实现系统的自反馈控制和里程信息的测算。

步骤7)：中央处理芯片不断监测机器人当前是否到达目标位置，若未到达，则返回步骤3；若已到达，则判断用户是否需要继续导航，若需要继续导航则返回步骤2，若不需要继续导航，则表示导航结束。特别说明的是，在导航的过程中，用户可以在任意时刻更换导航的目标位置，则系统自动返回步骤2，执行新的导航流程。

本发明采用了RGBD摄像头以及激光传感器作为视觉信息获取元件，将周围的环境信息做一个融合，以便得到更为准确的导航信息，使机器人能够准确找到目标位置。再者，考虑到现实条件下环境的复杂，本发明添加了相应的避障措施，进一步保障了机器人导航过程中的安全性与稳定性。最后为了保证中央处理芯片给出速度信息后，机器人能迅速达到该速度要求，本发明采用了自动控制领域中的热门的控制技术——PID控制技术。PID控制技术是一种闭环自动化控制技术，能够进一步增加了控制速度的精确度以及敏感度。

实施例三

图4示出了本发明第二实施例所提供的一种基于视觉定位算法的机器人的自主导航系统的示意图。如图4所示实施例二提供的一种基于视觉定位算法的机器人的自主导航系统10，包括：

初始化模块101，用于获得机器人所在区域的地图信息、机器人的初始位置信息及目标位置信息；

初始全局路径模块102，用于根据地图信息，结合初始位置信息及目标位置信息，通过中央处理器进行规划处理，得到机器人的初始全局行走路径，初始全部行走路径作为机器人的行走路径；

信息获取模块103，用于在机器人行走过程中，通过多个传感器分别实时获得机器人行走过程中周围的环境信息，机器人的累积位置信息，周围的环境信息包括机器人周围的障碍信息和高度障碍物信息；

当前位置获取模块104，用于通过视觉定位算法对机器人的累积位置信息进行修正，得到机器人的当前位置信息；

全局行走路径模块105，用于根据当前位置信息，机器人周围的障碍信息和初始全部行走路径，通过中央处理器进行路径规划处理，得到机器人的全局行走路径，全局行走路径作为机器人的行走路径；

自主导航模块106，用于根据周围的环境信息，计算得到机器人行走的方向及速度，机器人按照方向及速度进行行走，将当前位置信息与目标位置信息进行匹配，实现机器人从初始位置到目标位置的自主导航。

本发明的基于视觉定位算法的机器人的自主导航系统10，其技术方案为：首先通过初始化模块101，获得机器人所在区域的地图信息、机器人的初始位置信息及目标位置信息；

接着通过初始全局路径模块102，根据地图信息，结合初始位置信息及目标位置信息，通过中央处理器进行规划处理，得到机器人的初始全局行走路径，初始全部行走路径作为机器人的行走路径；

然后通过信息获取模块103，在机器人行走过程中，通过多个传感器分别实时获得机器人行走过程中周围的环境信息，机器人的累积位置信息，周围的环境信息包括机器人周围的障碍信息和高度障碍物信息；接着通过当前位置获取模块104，通过视觉定位算法对机器人的累积位置信息进行修正，得到机器人的当前位置信息；

接着通过全局行走路径模块105，根据当前位置信息，机器人周围的障碍信息和初始全部行走路径，通过中央处理器进行路径规划处理，得到机器人的全局行走路径，全局行走路径作为机器人的行走路径；

然后通过自主导航模块106，根据周围的环境信息，计算得到机器人行走的方向及速度，机器人按照方向及速度进行行走，将当前位置信息与目标位置信息进行匹配，实现机器人从初始位置到目标位置的自主导航。

本发明的基于视觉定位算法的机器人的自主导航系统10，首先结合机器人所在区域的地图信息，结合机器人的初始位置和目标位置，给机器人规划一条初始全局行走路径，然后在机器人行走过程中，通过机器人的传感器采集机器人周围的环境信息及其他自身状态信息(方向，行走速度等)，重新规划一条全局行走路径，可使机器人在行走过程中自主避免障碍物，并根据自身状态信息实时调整机器人的行走方向及速度，使机器人最终实现自主导航，到达目标位置。

本发明的基于视觉定位算法的机器人的自主导航系统10，利用机器人周围的障碍信息，结合机器人自身位置信息，为机器人提供一条合理的行走路径，并实时对机器人的行走状态进行改变，得到了精准的导航效果。

具体地，多个传感器包括深度摄像头、激光传感器和里程计；通过深度摄像头获得高度障碍物信息，通过激光传感器获得机器人周围的障碍物信息，通过里程计获得机器人的累积位置信息。

在机器人行走的过程中，中央处理芯片通过部署在机器人上的传感器收集机器人周围的有效信息，传感器具体包括深度摄像头、激光传感器和里程计。其中具体作用为：通过激光传感器获得机器人周围的障碍物信息，通过深度摄像头获得高度障碍物信息，补充周围的障碍物信息，即深度摄像头和激光传感器提供机器人周围的障碍物信息，里程计提供机器人的累积位置信息。

具体地，传感器还包括磁强计、加速度计和陀螺仪，磁强计提供机器人的绝对方向，加速度计为提供机器人的瞬时加速度值，陀螺仪提供机器人的相对姿态信息。

具体地，初始全局路径模块102具体用于通过A*算法进行规划处理，得到机器人的初始全局行走路径，具体为：

对地图信息进行分析处理，将地图空间划分为多个障碍空间和多个可移动空间；

对地图空间进行膨胀处理，得到多个新的障碍空间和多个新的可移动空间；

根据多个新的障碍空间和多个新的可移动空间，形成新的地图空间，得到机器人的初始全局行走路径。

具体地，初始全局路径模块102具体用于对地图空间进行膨胀处理，具体为：

获取每个障碍空间中的障碍点；

以每个障碍点为圆心，大于机器人高度的长度为半径的圆内作为新的障碍空间，得到多个新的障碍空间和多个新的可移动空间。

在机器人行走之前，首先根据给出的机器人所在区域的地图信息，通过A*算法设计一条从机器人初始位置到目标位置的初始全局行走路径。

具体的过程为：首先对地图信息进行分析，将整个地图划分为障碍空间和可移动空间，其次再对整个障碍空间进行膨胀处理，具体方法为以该障碍点为圆心，以略大于机器人的大小作为半径，将整个圆内区域作为新的障碍空间，遍历所有的旧障碍空间(障碍空间)，得到新的障碍空间即为最终的障碍空间。

优选地，本发明对A*算法的评估函数进行了修改，修改了其启发式代价算法，使用1.4倍的欧式距离作为其启发式代价计算方法，采用该计算方法能够使A*算法得出的路径更为平滑。在数据结构上，A*算法采用优先队列存储可移动空间中的点，提高了整个算法的计算速度。

本发明采用了A*算法作为全局规划算法，因为A*算法算法是一种静态路网中求解最短路最有效的直接搜索方法。通过上述改进的A*算法，导航系统得到一条可供机器人移动的全局路径。

具体地，当前位置获取模块104具体用于：

根据机器人的初始位置信息，在初始位置周围按照误差分布布设多个概率点，多个概率点作为视觉定位算法的初始状态空间；

在机器人行走过程中，初始状态空间中的多个点根据机器人的累积位置信息进行位置变换和姿态变换，得到变换后的多个点；

根据传感器模型，结合机器人周围的障碍物信息，对变换后的多个点的进行权重计算，得到变换后的多个点的权重；

对变换后的多个点的权重进行比较，通过重采样将权重大的点进行多次重采样，将权重小的点移除，得到的多个重采样的点形成状态空间；

根据高度障碍物信息，通过机器学习的方法对状态空间中的多个点进行分类，分类后的点形成状态空间中的类簇；

根据状态空间中的类簇，得到机器人的当前位置信息。

在本发明中，视觉定位算法是集视觉算法、概率算法以及机器学习为一体的定位算法，首先根据机器人的初始位置信息，在初始状态周围按照误差分布撒下大量概率点，作为视觉定位算法的初始状态空间，在机器人行走的过程中，初始状态空间的点不断地根据里程信息作出位置变换以及姿态变幻，算法通过激光传感器收集的二维视觉信息，根据传感器模型对整个状态空间内的点的权重进行计算，通过重采样使权重大的点得到更多的后代，而权重小的点被移除，从而逐渐收敛成一个类簇。系统的状态则由所有点的加权平均得到。

再通过RGBD摄像头收集的三维视觉信息具像化出一个真实的三维环境，再通过机器学习的方法来更好的将状态空间的点聚集成类簇。

使用深度神经网络(DNN)对状态空间的点进行分类，步骤如下：首先收集数据集，具体在于对于不同的定位点，他在不同时间段上的状态空间的分布情况。然后将数据集分为两部分，一部分用于训练深度神经网络模型，另一部分用于测试深度神经网络模型。在训练深度神经网络模型的过程中，调节隐藏层的数量以及对应的参数，达到输出准确定位的目的。通过使用深度神经网路作为定位的辅助功能，最终当状态空间聚集为一个类簇时，那么系统即得到了一个精确地位置信息。

在整个自主导航过程中，系统会不断通过视觉匹配算法来修正里程计累积误差，实现高精度的定位需求。若在导航的途中，用户重新指定了新的目标位置，则机器人朝着新指定的目标位置自主导航。

本发明的优点在于：

一、有效的将计算机视觉领域和自动化控制领域结合起来，实现了在现实环境下机器人的自动导航技术；

二、系统在底层驱动芯片中添加了PID控制算法，提高了机器人速度控制的准确度，使可移动载体可以精确地执行导航目标；

三、系统考虑到现实环境的复杂性，加入了实时避障算法，提高了现实环境下导航技术的鲁棒性；

四、系统突破了传统计算机视觉领域单纯依靠CCD摄像头的瓶颈，加入了激光传感器以及RGBD传感器。通过多视觉传感器的数据融合，可以精确地获取周围的障碍信息。利用本发明涉及的相关技术，可以拓展现有计算机视觉技术的应用场景，同时也象征着自动化控制技术朝人工智能的方向进一步发展，这是在交叉领域的一个创新式地突破。

本发明的应用场景十分广泛，除适用于传统导航技术的主流应用场景，如超市导航机器人等服务型机器人，还适用于一些工业级的应用场景，如工厂叉车，仓储型机器人，无人驾驶汽车等。另外该方法及系统的使用方式简单，用户只需要输入机器人所在的全局地图和机器人的初始位置，之后机器人便可到达用户任意想去的位置，这就是自动化技术与计算机视觉技术结合下该技术的优势。

最后应说明的是：以上各实施例仅用以说明本发明的技术方案，而非对其限制；尽管参照前述各实施例对本发明进行了详细的说明，本领域的普通技术人员应当理解：其依然可以对前述各实施例所记载的技术方案进行修改，或者对其中部分或者全部技术特征进行等同替换；而这些修改或者替换，并不使相应技术方案的本质脱离本发明各实施例技术方案的范围，其均应涵盖在本发明的权利要求和说明书的范围当中。

Claims (10)

1.一种基于视觉定位算法的机器人的自主导航方法，其特征在于，包括：

步骤S1，获得机器人所在区域的地图信息、所述机器人的初始位置信息及目标位置信息；

步骤S2，根据所述地图信息，结合所述初始位置信息及目标位置信息，通过中央处理器进行规划处理，得到所述机器人的初始全局行走路径，所述初始全部行走路径作为所述机器人的行走路径；

步骤S3，在所述机器人行走过程中，通过多个传感器分别实时获得所述机器人行走过程中周围的环境信息，所述机器人的累积位置信息，所述周围的环境信息包括所述机器人周围的障碍信息和高度障碍物信息；

步骤S4，通过视觉定位算法对所述机器人的累积位置信息进行修正，得到所述机器人的当前位置信息；

步骤S5，根据所述当前位置信息，所述机器人周围的障碍信息和所述初始全部行走路径，通过所述中央处理器进行路径规划处理，得到所述机器人的全局行走路径，所述全局行走路径作为所述机器人的行走路径；

步骤S6，根据所述周围的环境信息，计算得到所述机器人行走的方向及速度，所述机器人按照所述方向及速度进行行走，将所述当前位置信息与所述目标位置信息进行匹配，实现所述机器人从初始位置到目标位置的自主导航。

2.根据权利要求1所述的基于视觉定位算法的机器人的自主导航方法，其特征在于，

所述多个传感器包括深度摄像头、激光传感器和里程计，通过所述深度摄像头获得高度障碍物信息，通过所述激光传感器获得所述机器人周围的障碍物信息，通过所述里程计获得所述机器人的累积位置信息。

3.根据权利要求1所述基于视觉定位算法的机器人的自主导航方法，其特征在于，

所述步骤S2中，通过A*算法进行规划处理，得到所述机器人的初始全局行走路径，具体为：

对所述地图信息进行分析处理，将地图空间划分为多个障碍空间和多个可移动空间；

对所述地图空间进行膨胀处理，得到多个新的障碍空间和多个新的可移动空间；

根据多个所述新的障碍空间和多个所述新的可移动空间，形成新的地图空间，得到所述机器人的初始全局行走路径。

4.根据权利要求3所述的基于视觉定位算法的机器人的自主导航方法，其特征在于，

对所述地图空间进行膨胀处理，具体为：

获取每个所述障碍空间中的障碍点；

以每个所述障碍点为圆心，大于所述机器人高度的长度为半径的圆内作为新的障碍空间，得到多个新的障碍空间和多个新的可移动空间。

5.根据权利要求1所述的基于视觉定位算法的机器人的自主导航方法，其特征在于，

所述步骤S4，包括：

根据所述机器人的初始位置信息，在初始位置周围按照误差分布布设多个概率点，所述多个概率点作为视觉定位算法的初始状态空间；

在所述机器人行走过程中，所述初始状态空间中的多个点根据所述机器人的累积位置信息进行位置变换和姿态变换，得到变换后的多个点；

根据传感器模型，结合所述机器人周围的障碍物信息，对所述变换后的多个点的进行权重计算，得到所述变换后的多个点的权重；

对所述变换后的多个点的权重进行比较，通过重采样将权重大的点进行多次重采样，将权重小的点移除，得到的多个重采样的点形成状态空间；

根据所述高度障碍物信息，通过机器学习的方法对所述状态空间中的多个点进行分类，分类后的点形成所述状态空间中的类簇；

根据所述状态空间中的类簇，得到所述机器人的当前位置信息。

6.一种基于视觉定位算法的机器人的自主导航系统，其特征在于，包括：

初始化模块，用于获得机器人所在区域的地图信息、所述机器人的初始位置信息及目标位置信息；

初始全局路径模块，用于根据所述地图信息，结合所述初始位置信息及目标位置信息，通过中央处理器进行规划处理，得到所述机器人的初始全局行走路径，所述初始全部行走路径作为所述机器人的行走路径；

信息获取模块，用于在所述机器人行走过程中，通过多个传感器分别实时获得所述机器人行走过程中周围的环境信息，所述机器人的累积位置信息，所述周围的环境信息包括所述机器人周围的障碍信息和高度障碍物信息；

当前位置获取模块，用于通过视觉定位算法对所述机器人的累积位置信息进行修正，得到所述机器人的当前位置信息；

全局行走路径模块，用于根据所述当前位置信息，所述机器人周围的障碍信息和所述初始全部行走路径，通过所述中央处理器进行路径规划处理，得到所述机器人的全局行走路径，所述全局行走路径作为所述机器人的行走路径；

自主导航模块，用于根据所述周围的环境信息，计算得到所述机器人行走的方向及速度，所述机器人按照所述方向及速度进行行走，将所述当前位置信息与所述目标位置信息进行匹配，实现所述机器人从初始位置到目标位置的自主导航。

7.根据权利要求6所述的基于视觉定位算法的机器人的自主导航系统，其特征在于，

所述多个传感器包括深度摄像头、激光传感器和里程计，通过所述深度摄像头获得高度障碍物信息，通过所述激光传感器获得所述机器人周围的障碍物信息，通过所述里程计获得所述机器人的累积位置信息。

8.根据权利要求6所述的基于视觉定位算法的机器人的自主导航系统，其特征在于，

所述初始全局路径模块具体用于通过A*算法进行规划处理，得到所述机器人的初始全局行走路径，具体为：

对所述地图信息进行分析处理，将地图空间划分为多个障碍空间和多个可移动空间；

对所述地图空间进行膨胀处理，得到多个新的障碍空间和多个新的可移动空间；

根据多个所述新的障碍空间和多个所述新的可移动空间，形成新的地图空间，得到所述机器人的初始全局行走路径。

9.根据权利要求8所述的基于视觉定位算法的机器人的自主导航系统，其特征在于，

所述初始全局路径模块具体用于对所述地图空间进行膨胀处理，具体为：

获取每个所述障碍空间中的障碍点；

以每个所述障碍点为圆心，大于所述机器人高度的长度为半径的圆内作为新的障碍空间，得到多个新的障碍空间和多个新的可移动空间。

10.根据权利要求6所述的基于视觉定位算法的机器人的自主导航系统，其特征在于，

所述当前位置获取模块具体用于：

根据所述机器人的初始位置信息，在初始位置周围按照误差分布布设多个概率点，所述多个概率点作为视觉定位算法的初始状态空间；

在所述机器人行走过程中，所述初始状态空间中的多个点根据所述机器人的累积位置信息进行位置变换和姿态变换，得到变换后的多个点；

根据传感器模型，结合所述机器人周围的障碍物信息，对所述变换后的多个点的进行权重计算，得到所述变换后的多个点的权重；

对所述变换后的多个点的权重进行比较，通过重采样将权重大的点进行多次重采样，将权重小的点移除，得到的多个重采样的点形成状态空间；

根据所述高度障碍物信息，通过机器学习的方法对所述状态空间中的多个点进行分类，分类后的点形成所述状态空间中的类簇；

根据所述状态空间中的类簇，得到所述机器人的当前位置信息。