CN110456785A - 一种基于履带机器人的室内自主探索方法 - Google Patents

Abstract

本发明公开了一种基于履带机器人的室内自主探索方法，包括下述步骤：步骤一，以履带机器人作为移动机器人平台，进行拓展；步骤二，用已经获得的地图对激光束点阵进行优化，估计激光点在地图的表示，和占据网格的概率；用Gaussian‑Newton方法解决scan matching问题，获得激光点集映射到已有地图的刚体变换(x,y,theta)；步骤三，采用hector_navigation导航包和hector_slam建图包相结合，以实现室内自主探索的具体框架；导航中的状态估计加入惯性测量系统IMU，并利用EKF滤波；本发明能够更容易、更方便去控制机器人进行室内的自主探索，得到室内环境的栅格地图，从而更有效地帮助救援人员搜救伤员。

Description

一种基于履带机器人的室内自主探索方法

技术领域

本发明涉及机器人控制技术领域，具体涉及一种基于履带机器人的室内自主探索方法。

背景技术

为完成灾害救援任务而设计的自主移动机器人通常称为救援机器人(RescueRobot)，其典型形象通常和PackBot相仿：履带式结构、具备一付或多付副履带以及一个用于搜救的机械臂。然而，在通过多次救援行动后人们发现，PackBot已不能很好的完成现代救援任务，其结构在许多环境下已显得不够适用，因此针对救援机器人的研究成果也不断涌现。在城市搜救环境中，自主移动机器人往往需要在能够通过复杂地形环境的同时完成多项救援任务，这就给救援机器人的机械结构设计和软件算法设计带来了极大的挑战。

自主移动机器人一般需要回答如下三个问题：一是在哪里(Where am I？)，即自身的定位问题，机器人要能够知道在给定地图坐标系下自己的坐标或给定的二维、三维地图中自己所处的位置；二是去哪里(Where am I going？)，即目标识别的问题，机器人要能够自己确定目标、发现目标；三是怎么去(How should I go there？)，即导航的问题，包括可通行区域识别、路径搜索、路径规划等多个方面。能否回答这三个问题，决定了移动机器人能不能在缺乏人为监控的复杂环境下自主的完成任务。

现有技术中，救援机器人进入灾后环境，依然需要操作人员进行远程遥控，难以在脱离远程控制下在救援环境中移动。而在灾后环境中，意外的事情是难以预知的，同时操作人员通过摄像头对灾后现场的辨识能力并不强，所以急需一种可以对救援环境进行自主探索的方法。

发明内容

本发明的目的在于克服现有技术的缺点与不足，提供一种基于履带机器人的室内自主探索方法，该方法利用ROS的代码重用性，可以更容易、更方便去控制机器人进行室内的自主探索，得到室内环境的栅格地图，从而更有效地帮助救援人员搜救伤员。

本发明的目的通过下述技术方案实现：

一种基于履带机器人的室内自主探索方法，包括下述步骤：

步骤一，以履带机器人作为移动机器人平台，进行拓展；

步骤二，用已经获得的地图对激光束点阵进行优化，估计激光点在地图的表示，和占据网格的概率；用Gaussian-Newton方法解决scan matching问题，获得激光点集映射到已有地图的刚体变换(x,y,theta)；

步骤三，采用hector_navigation导航包和hector_slam建图包相结合，以实现室内自主探索的具体框架；导航中的状态估计加入惯性测量系统IMU，并利用EKF滤波；

步骤四，利用路由器搭建的wifi作为履带机器人与PC机之间的无线通讯，实现PC机对机器人信息的提取，以及对机器人的运行状态进行实时监控；

步骤五，设置机器人的路径规划算法；然后再分别打开建图和导航的节点，让机器人在救援环境下自主行驶，同时探索未知环境。

优选地，所述步骤一种的移动机器人平台由由四个部分组成，包括距离信息采集模块、数据处理模块、运动控制模块和远程控制模块。

优选地，所述距离信息采集模块主要为激光雷达，微型计算机通过usb接口接收激光雷达所传回的信息；所述运动控制模块中的惯性测量单元(IMU)用于测量机器人位姿，提供了更加精确的机器人位置信息，为机器人导航提供更加精确的位姿信息。

优选地，所述步骤二中在获取地图阶段，首先预设置一个栅格的尺寸，该栅格的大小将会确定离散化的程度；而离散化的基本思想为：从机器人当前所在坐标开始，以一个栅格的大小去扩散四周，使得平面内铺满整个虚拟栅格，然后将激光雷达的初始数据落到相应的栅格内，由栅格的占有概率值来代表所有落到该栅格内的测距前端数据；该过程通过双线性滤波算法实现；在离散化了激光雷达返回的数据之后，局部占有栅格地图就会生成。

优选地，所述步骤二中的Gaussian-Newton方法具体为：

牛顿法也是一种经典的最优化算法，它的搜索方向是根据目标函数的负梯度和二阶导数矩阵构造的，称为牛顿方向；

将函数f(X)在点X^k出泰勒二次近似式：

令函数f(X)的梯度等于零，并设X^k+1是函数：

由此解得：

令：

则有：

X^k+1＝X^k+S^k (5)

利用高斯-牛顿方法进行扫描匹配，并对激光雷达数据进行处理，即通过局部地图对机器人的位姿进行估计，将局部环境地图融合为全局环境地图。

优选地，所述步骤三中的工作过程具体为：

导航滤波以常频率100hz运行，只要扫描匹配点和其它传感器到达保持同步更新；

①同步与伪数据(Navigation Filter)；

测量更新无额外反馈使得系统不稳定的对策：阻止状态更新的无限增长(累积偏差)，设定一个变量阈值，当未达到时认为无测量到达，伪造(pseudo)一个零速进行更新；

②二维SLAM与三维EKF估计；

三维的EKF点估计过程投影到二维平面，将其作为扫描匹配过程的起始点，在扫描时表示具有协方差P的卡尔曼估计，然后给出协方差融合结果：

使用观察者矩阵C将完整的状态空间投影到SLAM系统的三维子空间中；选择参数来调整SLAM update的效果；类似于卡尔曼滤波器和信息滤波器之间的二元性，协方差交集也可以用其协方差形式写成：

优选地，所述步骤五中的路径规划算法为Dijkstra算法，Dijkstra算法为改进的广度优先搜索算法，具体为：

①Dijkstra算法按路径长度递增次序产生最短路径；先把V分成两组：

S：已求出最短路径的顶点的集合；

V-S＝T：尚未确定最短路径的顶点集合；

将T中顶点按最短路径递增的次序加入到S中，依据：可以证明V0到T中顶点Vk的最短路径，或是从V0到Vk的直接路径的权值或是从V0经S中顶点到Vk的路径权值之和；

②求最短路径步骤：

初使时令S＝{V0}，T＝{其余顶点}，T中顶点对应的距离值，若存在<V0,Vi>，为<V0,Vi>弧上的权值(和SPFA初始化方式不同)，若不存在<V0,Vi>，为Inf(无限)；

从T中选取一个其距离值为最小的顶点W，加入S，对T中顶点的距离值进行修改：若加进W作中间顶点，从V0到Vi的距离值比不加W的路径要短，则修改此距离值；

重复上述步骤，直到S中包含所有顶点，即S＝V为止(说明最外层是除起点外的遍历)。

本发明与现有技术相比具有以下的有益效果：

本发明能够更容易、更方便地控制机器人进行室内的自主探索，得到室内环境的栅格地图，从而更有效、更安全地帮助救援人员搜救伤员；将激光、惯性测量单元信息进行融合，并结合Dijkstra算法与深度优先算法在路径规划上的应用，有利于进一步提高履带机器人的自主导航能力。

附图说明

图1为本发明整体流程示意图；

图2为本发明救援机器人系统组成框图；

图3为本发明hector_navigation导航包与hector_slam建图包结合框图；

图4为本发明Dijkstra算法流程图；

图5为本发明深度优先算法示意图。

具体实施方式

下面结合实施例及附图对本发明作进一步详细的描述，但本发明的实施方式不限于此。

如图1～5所示，一种基于履带机器人的室内自主探索方法，包括下述步骤：

步骤一，以履带机器人作为移动机器人平台，进行拓展；

如图2所示，所述移动机器人平台由由四个部分组成，包括距离信息采集模块、数据处理模块、运动控制模块和远程控制模块。硬件则使用差分驱动的履带机器人以及STM32底层驱动模块、电源模块、激光雷达、无线路由器等模块。其中控制核心为基于Intel英特尔I5中央处理器的微型计算机。救援机器人是基于两轮差速驱动运动模型的履带车，虽然履带式结构在速度这一环节上比不过轮式结构，但是履带车可以原地转动，在不平的路面上性能较好，稳定性较强，该机器人底盘具有运行稳定、续航时间长等特点；距离信息采集模块主要为激光雷达，微型计算机通过usb接口接收激光雷达所传回的信息，通常，激光SLAM系统通过对不同时刻两片点云的匹配与比对，利用不同算法计算激光雷达相对运动的距离和姿态的改变，也就完成了对机器人自身的定位；运动控制模块中，惯性测量单元(IMU)用于测量机器人位姿，提供了更加精确的机器人位置信息，为机器人导航提供更加精确的位姿信息。

步骤二，用已经获得的地图对激光束点阵进行优化，估计激光点在地图的表示，和占据网格的概率；用Gaussian-Newton方法解决scanmatching问题，获得激光点集映射到已有地图的刚体变换(x,y,theta)；

所述Gaussian-Newton方法具体为：

牛顿法也是一种经典的最优化算法，它的搜索方向是根据目标函数的负梯度和二阶导数矩阵构造的，称为牛顿方向；

将函数f(X)在点X^k出泰勒二次近似式：

令函数f(X)的梯度等于零，并设X^k+1是函数：

由此解得：

令：

则有：

X^k+1＝X^k+S^k (5)

利用高斯-牛顿方法进行扫描匹配，并对激光雷达数据进行处理，即通过局部地图对机器人的位姿进行估计，将局部环境地图融合为全局环境地图。

在获取地图阶段，首先预设置一个栅格的尺寸，该栅格的大小将会确定离散化的程度；而离散化的基本思想为：从机器人当前所在坐标开始，以一个栅格的大小去扩散四周，使得平面内铺满整个虚拟栅格，然后将激光雷达的初始数据落到相应的栅格内，由栅格的占有概率值来代表所有落到该栅格内的测距前端数据；该过程通过双线性滤波算法实现；在离散化了激光雷达返回的数据之后，局部占有栅格地图就会生成。

步骤三，采用hector_navigation导航包和hector_slam建图包相结合，以实现室内自主探索的具体框架；导航中的状态估计加入惯性测量系统IMU，并利用EKF滤波；

如图3所示，所述hector_navigation导航包和hector_slam建图包相结合，就是相当于边建图边进行自主探索。

导航滤波以常频率100hz运行，只要扫描匹配点和其它传感器到达保持同步更新；

①同步与伪数据(Navigation Filter)；

测量更新无额外反馈使得系统不稳定的对策：阻止状态更新的无限增长(累积偏差)，设定一个变量阈值，当未达到时认为无测量到达，伪造(pseudo)一个零速进行更新；

②二维SLAM与三维EKF估计；

三维的EKF点估计过程投影到二维平面，将其作为扫描匹配过程的起始点，在扫描时表示具有协方差P的卡尔曼估计，然后给出协方差融合结果：

使用观察者矩阵C将完整的状态空间投影到SLAM系统的三维子空间中；选择参数来调整SLAM update的效果；类似于卡尔曼滤波器和信息滤波器之间的二元性，协方差交集也可以用其协方差形式写成：

步骤四，利用路由器搭建的wifi作为履带机器人与PC机之间的无线通讯，实现PC机对机器人信息的提取，以及对机器人的运行状态进行实时监控；

所述步骤四为上位机和机器人之间的通信。在同一路由器下，分别配置PC机以及微型计算机在网段上的域名，然后操作者通过SSH连接微型计算机的Ubuntu上，最后在PC机上的系统变量上设置微型计算机的域名，从而完成机器人和上位机通信的配置；此时，操作者可以在PC机上面订阅一个话题，就可以完成PC机跟微型计算机进行数据交换，因此一些算法上的运算可以在微型计算机上进行，也可以在PC机上进行，例如导航算法的运算。操作者可以发送位姿指令，移动机器人接收到通过wifi传来的数据，然后在ROS的导航框架下进行位姿跟踪，移动机器人会移动到想要机器人达到的位置。操作者可以在ROS的图形化工具Rviz上监控到机器人的运动方向、运行轨迹以及运行状态等，也包括获取的激光雷达数据信息形成的环境二维地图；也可以在Ubuntu的终端上观察到机器人的运行速度、机器人的位姿信息等。

步骤五，设置机器人的路径规划算法；然后再分别打开建图和导航的节点，让机器人在救援环境下自主行驶，同时探索未知环境。

如图4所示，所述路径规划算法为Dijkstra算法，Dijkstra算法为改进的广度优先搜索算法，具体为：

①Dijkstra算法按路径长度递增次序产生最短路径；先把V分成两组：

S：已求出最短路径的顶点的集合；

V-S＝T：尚未确定最短路径的顶点集合；

将T中顶点按最短路径递增的次序加入到S中，依据：可以证明V0到T中顶点Vk的最短路径，或是从V0到Vk的直接路径的权值或是从V0经S中顶点到Vk的路径权值之和；

②求最短路径步骤：

初使时令S＝{V0}，T＝{其余顶点}，T中顶点对应的距离值，若存在<V0,Vi>，为<V0,Vi>弧上的权值(和SPFA初始化方式不同)，若不存在<V0,Vi>，为Inf(无限)；

从T中选取一个其距离值为最小的顶点W，加入S，对T中顶点的距离值进行修改：若加进W作中间顶点，从V0到Vi的距离值比不加W的路径要短，则修改此距离值；

重复上述步骤，直到S中包含所有顶点，即S＝V为止(说明最外层是除起点外的遍历)。

如图5所示，探索未知环境所使用的探索算法为边界探索(FrontierExploration)算法(深度优先算法)，机器人一开始只探索了部分地图的时候，机器人选择广度比较大的地方去搜索，如图5(a)中，机器人后方有两个目标点(0和1)，机器人前方只要一个目标点(2)，所以机器人就会选择后方去探索，如图5(f)所示，当后方探索完之后，机器人会搜索最后的目标点(2)。当所有边界都搜索完(边界封闭)之后，机器人就会进行内部探索，但是当内部的地图也已经探索完了以后，机器人就会停下来。

针对恶劣的救援环境，灾难发生后的一段时间内仍然存在很多不可预知的危险，因此利用高自主能力的救援机器人能够大大提高救援人员的搜救效率，也是一个操作人员操作多台救援机器人的前提条件；本发明利用履带机器人进行救援环境下的自主探索，构建灾后环境的二维栅格地图，为救援人员进行搜救提供了方向。

本发明能够更容易、更方便地控制机器人进行室内的自主探索，得到室内环境的栅格地图，从而更有效、更安全地帮助救援人员搜救伤员；将激光、惯性测量单元信息进行融合，并结合Dijkstra算法与深度优先算法在路径规划上的应用，有利于进一步提高履带机器人的自主导航能力。

上述为本发明较佳的实施方式，但本发明的实施方式并不受上述内容的限制，其他的任何未背离本发明的精神实质与原理下所作的改变、修饰、替代、组合、简化，均应为等效的置换方式，都包含在本发明的保护范围之内。

Claims (7)

1.一种基于履带机器人的室内自主探索方法，其特征在于，包括下述步骤：

步骤一，以履带机器人作为移动机器人平台，进行拓展；

步骤二，用已经获得的地图对激光束点阵进行优化，估计激光点在地图的表示，和占据网格的概率；用Gaussian-Newton方法解决scan matching问题，获得激光点集映射到已有地图的刚体变换(x,y,theta)；

步骤三，采用hector_navigation导航包和hector_slam建图包相结合，以实现室内自主探索的具体框架；导航中的状态估计加入惯性测量系统IMU，并利用EKF滤波；

步骤四，利用路由器搭建的wifi作为履带机器人与PC机之间的无线通讯，实现PC机对机器人信息的提取，以及对机器人的运行状态进行实时监控；

步骤五，设置机器人的路径规划算法；然后再分别打开建图和导航的节点，让机器人在救援环境下自主行驶，同时探索未知环境。

2.根据权利要求1所述的基于履带机器人的室内自主探索方法，其特征在于，所述步骤一种的移动机器人平台由由四个部分组成，包括距离信息采集模块、数据处理模块、运动控制模块和远程控制模块。

3.根据权利要求2所述的基于履带机器人的室内自主探索方法，其特征在于，所述距离信息采集模块主要为激光雷达，微型计算机通过usb接口接收激光雷达所传回的信息；所述运动控制模块中的惯性测量单元(IMU)用于测量机器人位姿，提供了更加精确的机器人位置信息，为机器人导航提供更加精确的位姿信息。

4.根据权利要求1所述的基于履带机器人的室内自主探索方法，其特征在于，所述步骤二中在获取地图阶段，首先预设置一个栅格的尺寸，该栅格的大小将会确定离散化的程度；而离散化的基本思想为：从机器人当前所在坐标开始，以一个栅格的大小去扩散四周，使得平面内铺满整个虚拟栅格，然后将激光雷达的初始数据落到相应的栅格内，由栅格的占有概率值来代表所有落到该栅格内的测距前端数据；该过程通过双线性滤波算法实现；在离散化了激光雷达返回的数据之后，局部占有栅格地图就会生成。

5.根据权利要求1所述的基于履带机器人的室内自主探索方法，其特征在于，所述步骤二中的Gaussian-Newton方法具体为：

牛顿法也是一种经典的最优化算法，它的搜索方向是根据目标函数的负梯度和二阶导数矩阵构造的，称为牛顿方向；

将函数f(X)在点X^k出泰勒二次近似式：

令函数f(X)的梯度等于零，并设X^k+1是函数：

由此解得：

令：

则有：

X^k+1＝X^k+S^k (5)

利用高斯-牛顿方法进行扫描匹配，并对激光雷达数据进行处理，即通过局部地图对机器人的位姿进行估计，将局部环境地图融合为全局环境地图。

6.根据权利要求1所述的基于履带机器人的室内自主探索方法，其特征在于，所述步骤三中的工作过程具体为：

导航滤波以常频率100hz运行，只要扫描匹配点和其它传感器到达保持同步更新；

①同步与伪数据(Navigation Filter)；

测量更新无额外反馈使得系统不稳定的对策：阻止状态更新的无限增长(累积偏差)，设定一个变量阈值，当未达到时认为无测量到达，伪造(pseudo)一个零速进行更新；

②二维SLAM与三维EKF估计；

三维的EKF点估计过程投影到二维平面，将其作为扫描匹配过程的起始点，在扫描时表示具有协方差P的卡尔曼估计，然后给出协方差融合结果：

使用观察者矩阵C将完整的状态空间投影到SLAM系统的三维子空间中；选择参数来调整SLAM update的效果；类似于卡尔曼滤波器和信息滤波器之间的二元性，协方差交集也可以用其协方差形式写成：

7.根据权利要求1所述的基于履带机器人的室内自主探索方法，其特征在于，所述步骤五中的路径规划算法为Dijkstra算法，Dijkstra算法为改进的广度优先搜索算法，具体为：

①Dijkstra算法按路径长度递增次序产生最短路径；先把V分成两组：

S：已求出最短路径的顶点的集合；

V-S＝T：尚未确定最短路径的顶点集合；

将T中顶点按最短路径递增的次序加入到S中，依据：可以证明V0到T中顶点Vk的最短路径，或是从V0到Vk的直接路径的权值或是从V0经S中顶点到Vk的路径权值之和；

②求最短路径步骤：

初使时令S＝{V0}，T＝{其余顶点}，T中顶点对应的距离值，若存在<V0,Vi>，为<V0,Vi>弧上的权值(和SPFA初始化方式不同)，若不存在<V0,Vi>，为Inf(无限)；

从T中选取一个其距离值为最小的顶点W，加入S，对T中顶点的距离值进行修改：若加进W作中间顶点，从V0到Vi的距离值比不加W的路径要短，则修改此距离值；

重复上述步骤，直到S中包含所有顶点，即S＝V为止(说明最外层是除起点外的遍历)。