CN111830977A - 一种移动机器人自主导航软件框架及导航方法 - Google Patents

Abstract

本发明涉及一种移动机器人自主导航软件框架及导航方法，软件框架分为用户接口层、自主导航层、系统监视层、设备层和底层应用层等5个层次。每个功能模块按独立线程运行，模块之间通过共享内存的方式进行数据交互。软件框架将感知、规划与执行模块分开，可以适应不同驱动形式的平台、不同的传感器配置和不同的任务。规范定义了导航系统中地图、路径、分类器、任务、路径规划器等常用的抽象接口，定义了常用坐标系以及传感器和功能模块涉及的数据结构。底层应用层实现了线程、数学运算、日志、硬件接口访问；设备层实现了功能模块的定义与基本操作，实现了常用设备的数据采集功能，具有较为完备的底层功能，可支持敏捷化开发。

Description

一种移动机器人自主导航软件框架及导航方法

技术领域

本发明涉及移动机器人控制技术领域，尤其涉及一种移动机器人自主导航软件框架及导航方法。

背景技术

为了提高软件开发的“起点”，加快开发速度，提高产品质量，基于框架开发已经成为一种普遍现象。一个项目采用一个或多个第三方框架是很常见的事情，例如Swing、.NET和MFC等都是流行的框架。

从概念角度讲，框架是一个可实例化的、部分完成的软件系统，它为系统定义了架构，并提高了构造系统的基本构造块，还为实现特定功能定义了可调整点。从实现角度讲，框架是一组抽象类，定义了它们各自的责任和相互之间的协作，形成某类软件的一个可复用设计。开发者通过继承框架类和组合其实例来定制生成特定的应用。

对于移动机器人自主导航而言，一般采用“感知-规划-执行”的慎思式控制架构，该控制架构具有通用的信息流程和运行逻辑，这就为开发通用化的软件框架提供了基础。另外，为了加快产品开发速度，并充分利用已有的研究成果，开发一种高度通用、易于扩展、底层功能完备的移动机器人软件框架非常有必要。

目前，在机器人领域有许多开源的软件框架。例如，由美国南加州大学开发的免费平台Player/Stage，包含网络服务部分Player和机器人平台仿真部分Stage，主要用于机器人仿真研究，但其扩展机制较为复杂。ROS是开源的机器人次级操作系统，它提供类似操作系统的功能，包括硬件抽象描述、底层驱动程序管理、共用功能的执行、程序间消息的传递、程序发行包管理等，ROS的首要设计目标是提高代码复用率。ROS采用分布式处理框架，并基于进程实现模块间P2P通信，但ROS本身不具有实时性。美国标准技术研究院(NIST)基于实时控制系统参考模型开发了实时控制系统(RCS)软件库，可用于实时智能系统的设计，该库基于进程运行，在统一节点模型下，实现了上下层节点间的通信，因此从实现的角度讲，它是一个通信中间件，缺乏必要的底层功能，不能称之为软件框架。

发明内容

为了克服已有框架的上述缺陷，本发明提供一种移动机器人自主导航软件框架及导航方法，各功能模块以独立线程运行，底层功能完备，具备通用化的自主导航运行流程，规范化的数据结构，并可部署在一台计算机上。

为达到上述目的，本发明提供了一种移动机器人自主导航软件框架，包括用户接口层、自主导航层、系统监视层、设备层和底层应用层；

所述用户接口层通过通信模块与用户交互；

所述自主导航层根据任务路径、通过成本地图和机器人当前的位置与姿态规划当前的目标路径，基于当前移动机器人的位置与姿态、速度和当前的目标路径生成控制移动机器人本体机动的运动控制指令；

所述系统监视层收集机器人状态、监视安全避碰距离以及通信链路连接状态，当判断移动机器人存在碰撞危险或通信链路处于断开状态时发出停止指令；

所述设备层实现对移动机器人感知和定位的传感器模块的访问，将所述运动控制指令输入给所述移动机器人本体的平台操控器模块执行；

所述底层应用层用于实现移动机器人的坐标系转换，记录日志，对关键事件和数据进行记录。

进一步地，所述自主导航层包括环境感知模块、局部路径规划模块和跟踪控制模块；环境感知模块获取多种传感器采集的数据建立环境地图，通过分类器对环境地图中的地形可通过性进行划分，信息融合后获得通过成本地图，通过成本包括通过时间或路径长度以及通过难度；局部路径规划模块以所述通过成本地图、任务路径以及移动机器人当前位置与姿态为输入，选择通过成本最低的路径作为当前的目标路径；跟踪控制模块基于机器人当前位置、姿态和当前的目标路径生成控制平台机动的运动控制指令。

进一步地，系统监视层包括状态收集模块、通信链路监视模块以及安全避碰模块；所述状态收集模块收集所述移动机器人位置、姿态、速度、局部路径规划模块的输出以及所述通信链路状态；所述通信链路监视模块监测通信链路的连接状态；所述安全避碰模块监视所述移动机器人距离障碍物的最小距离，在通信链路断开或最小距离小于安全距离阈值时，发出停止指令，控制所述移动机器人本体停止。

进一步地，所述设备层包括传感器模块、平台操控器模块、虚拟设备模块以及设备数据结构模块；所述传感器模块用于采集环境信息与定位信息，平台操控器模块接收所述运动控制指令并控制所述移动机器人本体移动；所述虚拟设备模块包括虚拟传感器和虚拟平台操控器，虚拟传感器用于对所述传感器模块用于所述传感器模块输出信号进行仿真，虚拟平台操控器用于对移动机器人采用的控制算法进行仿真；所述设备数据结构模块用于定义设备的输入输出数据格式及组成。

进一步地，所述底层应用层包括日志记录模块、矩阵计算模块、线程管理模块以及硬件设备接口模块；所述日志记录模块对运行过程中关键事件和数据进行记录；所述矩阵计算模块用于对所需的各类坐标系进行转换；所述线程管理模块，用于对自主导航软件框架中的所有线程进行管理；所述硬件设备接口模块驱动所述传感器模中各传感器的通信接口或控制接口。

进一步地，各个模块均按独立线程运行，模块之间通过共享内存的方式进行数据交互。

进一步地，所述自主导航层还包括导航器模块，将环境感知模块和局部路径规划模块关联实现导航；所述的环境感知模块包括单线激光雷达、多线激光雷达和相机，定期采集单线激光雷达、多线激光雷达和相机数据，分别通过独立线程建立环境地图；分别对每种环境地图进行分类，获得三种可通过性地图；通过对三种可通过性地图加权求和，融合获得最终的通过成本地图。

本发明另一方面提供一种利用所述的移动机器人自主导航软件框架进行导航的方法，包括：

所述的环境感知模块输出通过成本地图；

所述导航器模块运行，根据最新任务路径，根据通过成本地图输出当前的目标路径；

所述跟踪控制模块生成运动控制指令，并输入给所述平台操控器，平台控制器控制所述移动机器人本体移动；

所述导航器模块的运行流程如下：

(1)设置导航器模块的导航参数，包括地图大小、分辨率、最小安全距离、规划参数和速度剖面构建参数；其中，规划参数包括最大允许曲率、路径最大长度、备选路径数量、备选路径间隔距离；速度剖面构建参数包括加速度、减速度、紧急停车减速度和最大速度；

(2)环境感知模块更新环境地图，生成新的通过成本地图，获取移动机器人位置与姿态；

(3)计算当前至上次规划的时间间隔，当满足时间间隔阈值后进入步骤(4)；

(4)获取最新任务路径；以通过成本地图、移动机器人位置、姿态和任务路径为参数，调用局部路径规划器进行规划，获得可行的局部路径和速度剖面等规划结果；将规划结果推送到轨迹池中；所述轨迹池中的路径，作为当前的目标路径，轨迹池中的速度剖面作为当前的期望速度。

进一步地，所述跟踪控制模块运行流程如下：

(1)更新定位数据，获取所述移动机器人当前位置与姿态；

(2)从轨迹池中获取当前的目标路径和移动机器人的期望速度；

(3)计算出所述移动机器人到达目标路径所需的所述移动机器人本体运动控制指令；

(4)将所述运动控制指令输入给所述平台操控器，控制移动机器人本体机动。

本发明的上述技术方案具有如下有益的技术效果：

(1)通用化。软件框架基于移动机器人通用的慎思式自主导航架构，将感知、规划与执行模块分开，这种实现可以适应不同驱动形式的平台、不同的传感器配置和不同的任务。

(2)规范化。软件框架规范定义了移动机器人导航系统中地图、路径、分类器、任务、路径规划器等常用的抽象接口，定义了常用坐标系以及传感器和功能模块涉及的数据结构。

(3)完备化。底层应用层实现了线程、数学运算、日志、硬件接口访问等基本操作；设备层实现了功能模块的定义与基本操作，实现了常用设备的数据采集功能，具有较为完备的底层功能，可支持敏捷化开发。

(4)集中化。软件框架采用多线程运行机制，集中部署在一台高性能计算机上即可运行，对硬件计算资源要求相对较低。

(5)封装化。各层功能模块可封装成链接库，特别对于设备层，可将已开发的常用传感器和执行设备程序，封装成设备库。按库的形式进行使用和管理，有助于研究成果的积累、重用以及知识产权保护。

附图说明

图1是本发明软件框架层次图。

图2是本发明软件框架中自主导航层的类图。

图3是本发明软件框架中环境感知模块的基本流程图。

图4是本发明软件框架中设备类继承关系示例图。

具体实施方式

为使本发明的目的、技术方案和优点更加清楚明了，下面结合具体实施方式并参照附图，对本发明进一步详细说明。应该理解，这些描述只是示例性的，而并非要限制本发明的范围。此外，在以下说明中，省略了对公知结构和技术的描述，以避免不必要地混淆本发明的概念。

如图1所示，基于多线程的移动机器人软件框架划分为5个层次。从上至下分为用户接口层、自主导航层、系统监视层、设备层和底层应用层，5个层次。

1、用户接口层通过通信模块实现与用户的交互，包括从用户操控终端获取任务、操控指令和通信心跳信号，以及向用户操控终端反馈移动机器人状态信息。通信模块采用UDP协议，并以独立线程运行。运行过程中，根据数据帧头和校验结果判断接收到的消息类型，并执行相应操作；向操控端发送系统监控模块采集的状态信息。

2、自主导航层包括环境感知模块、局部路径规划模块和跟踪控制模块。在实现上，采用基于地图的导航器MapNavigator将环境感知模块Perception与局部路径规划模块PathPlanner关联起来，导航器MapNavigator基于环境感知模块输出的成本地图和移动机器人位置与姿态，调用局部路径规划模块规划出可行的局部路径和速度。导航器按独立线程运行，如图2所示。导航器MapNavigator线程函数基本流程如下：

(1)设置导航器参数MapNavigatorParameters，包括地图大小、分辨率、最小安全距离、规划器参数和速度剖面构建参数等。其中，规划器参数包括最大允许曲率、路径最大长度、备选路径数量、备选路径间隔距离；速度剖面构建参数包括加速度、减速度、紧急停车减速度和最大速度；

(2)环境感知模块更新输出，获取最新的通过成本地图和移动机器人位置与姿态；

(3)计算当前至上次规划的时间间隔；如果时间间隔大于设定的阈值，则进入步骤(4)，开始新的规划；

(4)规划流程包括：

(4.1)获取最新任务路径；

(4.2)以成本地图、移动机器人位置与姿态以及任务路径为参数，调用局部路径规划器进行规划，获得可行的局部路径和速度剖面等规划结果；

(4.3)将规划结果推送到轨迹池中，所述轨迹池中的路径，作为当前的目标路径，轨迹池中的速度剖面作为当前的期望速度。

所述的环境感知模块支持基于单线激光雷达、多线激光雷达和相机等多种传感器建图，如图3所示。每个传感器对应的建图模块都以独立线程运行，通过成本地图获取流程如下：

(1)定期采集传感器数据，包括单线雷达数据、多线雷达数据及相机数据，分别建立环境地图；

(2)利用各传感器特定的分类器对地形可通过性进行划分，获得对应的可通过性地图；

(3)通过加权求和，融合可通过性地图，获得最终的通过成本地图。

所述的局部路径规划模块以通过成本地图、任务路径以及机器人当前位置与姿态为输入，以基于备选路径优选的方法规划出可行的局部路径和速度剖面。为了适应不同的规划算法和路径模型，定义了规划器基类PathPlanner和路径基类Trajectroy，在具体实现时从这两个基类派生规划器和路径模型。

跟踪控制模块以独立线程运行，线程函数流程如下：

(1)更新定位数据，获取机器人最新位置与姿态；

(2)从轨迹池中获取当前的目标路径和期望速度；

(3)通过纯跟踪或模型预测控制算法计算出期望的运动控制指令，并输入给平台操控器；

(4)设备层的平台操控器利用期望的运动控制指令控制移动机器人本体机动。

为了适应不同的路径跟踪控制算法，定义了路径跟踪控制器基类FollowingController，在具体实现时从这个基类派生路径跟踪控制器。

3、系统监视层收集机器人状态和监视安全避碰距离，并据此决策是否发出停车指令，确保机器人运行安全。系统监视层包括状态收集模块、通信链路监视模块以及安全避碰模块；状态收集模块收集的状态包括机器人位置、姿态、速度、规划器输出；通信链路监视模块通过心跳信号监测通信链路通断；安全避碰模块通过避碰传感器，实时获取机器人本体与障碍的最小距离；在通信链路断开或障碍物距离小于阈值时向机器人操控器发出停车指令，确保移动机器人安全。监视层以独立的线程运行，线程函数基本流程如下：

(1)收集机器人状态和通信链路状态；

(2)获取障碍物最小距离；

(3)判断是否发出停车指令；如果通信链路断开或障碍物距离小于阈值，则发出停车指令；(3.2)否则，返回步骤(1)。

4、设备层实现对车载设备的访问，包括传感器模块、平台操控器模块、虚拟设备模块以及设备数据结构模块；所述传感器模块用于采集环境信息与定位信息，平台操控器模块接收所述运动控制指令并控制所述移动机器人本体机动；所述虚拟设备模块包括虚拟传感器和虚拟平台操控器，用于开展机器人控制算法仿真研究，其接口与物理设备一致；所述设备数据结构模块用于定义设备的输入输出数据格式及组成。传感器模块包括车载物理传感器、仿真用的虚拟传感器；平台操控器模块包括物理平台操控器和虚拟平台操控器。该层定义和实现了模块Module、车载模块MountedDevice等基类，这些类定义了模块和车载设备的初始化、关闭等基本操作，定义了常用设备的输入输出数据结构，具体的传感器类和平台操控器类派生于Module、MountedDevice等基类，如图4所示为单线激光雷达对应的类图示例，其中LMS151为一种具体的单线雷达类，它的父类为单线激光雷达类Laser2D，而Laser2D继承于MountedDevice类，MountedDevice类继承于Module类，Laser2dData为描述单线雷达输出数据的数据结构。

传感器模块用于采集环境信息与定位信息，包括单线激光雷达、多线激光雷达、相机等传感器，以及定位定向设备(包括北斗、GPS以及惯导等)

虚拟传感器为仿真环境中虚拟机器人上的传感器，包括单线激光雷达、多线激光雷达、相机等传感器，以及定位定向设备等等；这个框架支持真实物理环境下的机器人，也支持仿真环境下的虚拟机器人。

所述的传感器类，将采集的数据通过addValue()函数保存在环形缓存中。对于需要该传感器数据的模块，首先注册该传感器对象，获得访问权；然后，定义该传感器的读取器Reader对象，该对象调用getNext()函数从缓存中获得数据。

5、底层应用层实现了移动机器人涉及的常用坐标系定义与坐标变换计算、日志功能、线程管理操作以及硬件设备接口数据通信等。包括日志记录模块、矩阵计算模块、线程管理模块以及硬件设备接口模块。

所述的坐标系包括世界坐标系、局部坐标系、机器人坐标系、传感器坐标系以及地图坐标系。所有坐标系都遵守右手准则。矩阵计算模块通过定义相应的坐标和矩阵计算，支持坐标转换和矩阵间运算等基础功能。

所述日志记录模块对运行过程中关键事件和数据进行记录；所述的日志功能是在主控程序中定义一个全局日志对象，并设置记录日志的等级，实现对程序运行过程中关键事件和数据的记录，并保存在文件中，方便程序的调试和关键事件的查看。

所述的线程管理模块是通过定义各独立线程的基类而实现，具体定义了线程启动、暂停和结束等基本操作，特定功能的线程只需实现该类的虚函数run()即可。

所述的硬件设备接口模块利用数据通信功能实现了接口驱动程序，包括设备的串行接口，实现了这些接口的设置、连接、断开和接收与发送数据等操作。

本发明另一方面提供一种利用移动机器人自主导航软件框架进行导航的方法，流程包括：

(1)所述的环境感知模块输出通过成本地图；

(2)所述导航器模块运行，根据最新任务路径、通过成本地图输出当前的目标路径；具体包括：

2.1设置导航器模块的导航参数，包括地图大小、分辨率、最小安全距离、规划参数和速度剖面构建参数。其中，规划器参数包括最大允许曲率、路径最大长度、备选路径数量、备选路径间隔距离；速度剖面构建参数包括加速度、减速度、紧急停车减速度和最大速度；

2.2环境感知模块更新环境地图，生成新的通过成本地图，获取移动机器人位置与姿态；

2.3计算当前至上次规划的时间间隔，当满足时间间隔阈值后进入步骤2.4；

2.4获取最新任务路径；以通过成本地图、移动机器人位置与姿态以及任务路径为参数，调用局部路径规划器进行规划，获得可行的局部路径和速度剖面等规划结果；将规划结果推送到轨迹池中；所述轨迹池中的路径，作为当前的目标路径，轨迹池中的速度剖面作为当前的期望速度。

(3)所述跟踪控制模块生成运动控制指令，并输入给平台操控器，平台操控器控制所述移动机器人本体移动，具体包括：

3.1更新定位数据，获取所述移动机器人当前位置与姿态；

3.2从轨迹池中获取目标路径和期望速度；

3.3通过纯跟踪或模型预测控制算法计算出所述移动机器人本体的运动控制指令；

3.4将所述运动控制指令输入给移动机器人本体控制器，控制移动机器人本体机动。

综上所述，本发明涉及一种移动机器人自主导航软件框架及导航方法，软件框架分为用户接口层、自主导航层、系统监视层、设备层和底层应用层等5个层次。每个功能模块按独立线程运行，模块之间通过共享内存的方式进行数据交互。软件框架将感知、规划与执行模块分开，可以适应不同驱动形式的平台、不同的传感器配置和不同的任务。规范定义了导航系统中地图、路径、分类器、任务、路径规划器等常用的抽象接口，定义了常用坐标系以及传感器和功能模块涉及的数据结构。底层应用层实现了线程、数学运算、日志、硬件接口访问；设备层实现了功能模块的定义与基本操作，实现了常用设备的数据采集功能，具有较为完备的底层功能，可支持敏捷化开发。

应当理解的是，本发明的上述具体实施方式仅仅用于示例性说明或解释本发明的原理，而不构成对本发明的限制。因此，在不偏离本发明的精神和范围的情况下所做的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。此外，本发明所附权利要求旨在涵盖落入所附权利要求范围和边界、或者这种范围和边界的等同形式内的全部变化和修改例。

Claims (10)

1.一种移动机器人自主导航软件框架，其特征在于，包括用户接口层、自主导航层、系统监视层、设备层和底层应用层；

所述用户接口层通过通信模块与用户交互；

所述自主导航层根据任务路径、通过成本地图和机器人当前的位置与姿态规划当前的目标路径，基于当前移动机器人的位置与姿态、速度和当前的目标路径生成控制移动机器人本体机动的运动控制指令；

所述系统监视层收集机器人状态、监视安全避碰距离以及通信链路连接状态，当判断移动机器人存在碰撞危险或通信链路处于断开状态时发出停止指令；

所述设备层实现对移动机器人感知和定位的传感器模块的访问，将所述运动控制指令输入给所述移动机器人本体的平台操控器模块执行；

所述底层应用层用于实现移动机器人的坐标系转换，记录日志，对关键事件和数据进行记录。

2.根据权利要求1所述的移动机器人自主导航软件框架，其特征在于，所述自主导航层包括环境感知模块、局部路径规划模块和跟踪控制模块；环境感知模块获取多种传感器采集的数据建立环境地图，通过分类器对环境地图中的地形可通过性进行划分，信息融合后获得通过成本地图，通过成本包括通过时间或路径长度以及通过难度；局部路径规划模块以所述通过成本地图、任务路径以及移动机器人当前位置与姿态为输入，选择通过成本最低的路径作为当前的目标路径；跟踪控制模块基于机器人当前位置、姿态和当前的目标路径生成控制平台机动的运动控制指令。

3.根据权利要求2所述的移动机器人自主导航软件框架，其特征在于，系统监视层包括状态收集模块、通信链路监视模块以及安全避碰模块；所述状态收集模块收集所述移动机器人位置、姿态、速度、局部路径规划模块的输出以及所述通信链路状态；所述通信链路监视模块监测通信链路的连接状态；所述安全避碰模块监视所述移动机器人距离障碍物的最小距离，在通信链路断开或最小距离小于安全距离阈值时，发出停止指令，控制所述移动机器人本体停止。

4.根据权利要求3所述的移动机器人自主导航软件框架，其特征在于，所述设备层包括传感器模块、平台操控器模块、虚拟设备模块以及设备数据结构模块；所述传感器模块用于采集环境信息与定位信息，平台操控器模块接收所述运动控制指令并控制所述移动机器人本体移动；所述虚拟设备模块包括虚拟传感器和虚拟平台操控器，虚拟传感器用于对所述传感器模块用于所述传感器模块输出信号进行仿真，虚拟平台操控器用于对移动机器人采用的控制算法进行仿真；所述设备数据结构模块用于定义设备的输入输出数据格式及组成。

5.根据权利要求4所述的移动机器人自主导航软件框架，其特征在于，所述底层应用层包括日志记录模块、矩阵计算模块、线程管理模块以及硬件设备接口模块；所述日志记录模块对运行过程中关键事件和数据进行记录；所述矩阵计算模块用于对所需的各类坐标系进行转换；所述线程管理模块，用于对自主导航软件框架中的所有线程进行管理；所述硬件设备接口模块驱动所述传感器模中各传感器的通信接口或控制接口。

6.根据权利要求5所述的移动机器人自主导航软件框架，其特征在于，各个模块均按独立线程运行，模块之间通过共享内存的方式进行数据交互。

7.根据权利要求5所述的移动机器人自主导航软件框架，其特征在于，所述自主导航层还包括导航器模块，将环境感知模块和局部路径规划模块关联实现导航。

8.根据权利要求7所述的移动机器人自主导航软件框架，其特征在于，所述的环境感知模块包括单线激光雷达、多线激光雷达和相机，定期采集单线激光雷达、多线激光雷达和相机数据，分别通过独立线程建立环境地图；分别对每种环境地图进行分类，获得三种可通过性地图；通过对三种可通过性地图加权求和，融合获得最终的通过成本地图。

9.一种利用权利要求7或8所述的移动机器人自主导航软件框架进行导航的方法，其特征在于，包括：

所述的环境感知模块输出通过成本地图；

所述导航器模块运行，根据最新任务路径，根据通过成本地图输出当前的目标路径；

所述跟踪控制模块生成运动控制指令，并输入给所述平台操控器，平台控制器控制所述移动机器人本体移动；

所述导航器模块的运行流程如下：

(1)设置导航器模块的导航参数，包括地图大小、分辨率、最小安全距离、规划参数和速度剖面构建参数；其中，规划参数包括最大允许曲率、路径最大长度、备选路径数量、备选路径间隔距离；速度剖面构建参数包括加速度、减速度、紧急停车减速度和最大速度；

(2)环境感知模块更新环境地图，生成新的通过成本地图，获取移动机器人位置与姿态；

(3)计算当前至上次规划的时间间隔，当满足时间间隔阈值后进入步骤(4)；

(4)获取最新任务路径；以通过成本地图、移动机器人位置、姿态和任务路径为参数，调用局部路径规划器进行规划，获得可行的局部路径和速度剖面等规划结果；将规划结果推送到轨迹池中；所述轨迹池中的路径，作为当前的目标路径，轨迹池中的速度剖面作为当前的期望速度。

10.根据权利要求9所述的进行导航的方法，其特征在于，所述跟踪控制模块运行流程如下：

(1)更新定位数据，获取所述移动机器人当前位置与姿态；

(2)从轨迹池中获取当前的目标路径和移动机器人的期望速度；

(3)计算出所述移动机器人到达目标路径所需的所述移动机器人本体运动控制指令；

(4)将所述运动控制指令输入给所述平台操控器，控制移动机器人本体机动。

Images