CN110632919A - 一种基于履带式救援机器人的自主定位导航方法 - Google Patents

Abstract

本发明公开了一种基于履带式救援机器人的自主定位导航方法，包括以下步骤：S1.以履带机器人作为移动机器人平台，进行必要零件的配置和传感器扩展；S2.设置TF变换，采集里程计，激光雷达数据；S3.采用Hector_Slam地图构建开源包建图及应用KLD采样，采用微粒滤波器定位；S4.利用路由器搭建的WIFI作为履带机器人与PC上位机之间的无线通讯，实现PC上位机对履带机器人传感器数据信息的提取以及观察者对履带机器人运行状态的实时监控；S5.打开自主定位与建图、导航的工作节点，对模拟环境中的任意自定义目标点指明，履带机器人自动根据算法规划的路径接收到控制指令，完成自主定位导航任务。

Description

一种基于履带式救援机器人的自主定位导航方法

技术领域

本发明涉及机器人的技术领域，尤其涉及到一种基于履带式救援机器人的自主定位导航方法。

背景技术

90年代来，移动机器人在控制技术、传感器与信息处理技术等方面迎来重大发展，开始了在未知环境探索、王业产品输送、灾难救援、军事侦察及家庭服务等领域的研究和应用，并展现出无法估量的应用前景。移动机器人在灾难救援领域的应用，即救援机器人，是一种为了在地震、火灾、核福射或人为恐怖活动等灾难环境中实施救援而设计研发的机器人。众所周知，灾难现场复杂程度高，危险性大，紧迫感强，为救援工作带来了极大的困难。救援机器人其体积小、灵活等诸多优点成为灾难辅助救援的有效工具。一方面救援机器人能辅助救援人员在灾后更高效地开展救援工作，在有限的时间内救助更多的受困者，另一方面能替代救援人员深入到危险的环境中，减少在救援过程中人员的二次伤。救援机器人的研究始于1995年日本神户－大阪地震及美国阿尔弗德联邦大楼***案发生之后。限于当时的移动机器人技术，救援机器人很难实现完全自主化，因此应用于救援的机器人多为遥控型的机器人。然而，实际救援环境复杂程度高，为了提髙救援水平，缩短搜救时间，要求机器人能自主或者半自主地完成作业任务，而不是完全依赖于远程手动操控。另一方面，人工智能、机器视觉和其他相关领域技术的研究和发展推动了机器人自主化或半自主化进程。具有感知能力、自主行为能力及智能决策和规划能力的自主式机器人成了救援机器人未来发展的趋势。

为了顺应人工智能时代崛起态势,适应救援机器人发展态势,本发明针对其中救援机器人体系中常见的履带机器人在室内救援环境当中进行自主定位导航，机器人硬件设计主要是双电机差分驱动的履带机器人配有STM32底层驱动模块、电源模块、WIFI网络发射模块、传感器模块等。软件设计主要是采用ROS机器人操作***,ROS提供了许多强大的工具程序和硬件驱动程序和许多非常实用的工具包程序,具有优秀的分布式处理框架和通信架构，良好的兼容性，集成度高。

发明内容

为了实现上述目的，本发明提供一种基于履带式救援机器人的自主定位导航方法。

具体的技术方案为：

包括以下步骤：

S1.以履带机器人作为移动机器人平台，进行必要零件的配置和传感器扩展；

S2.设置TF变换，采集里程计，激光雷达数据；

S3.采用常用得Hector_Slam地图构建开源包建图及应用KLD采样，采用微粒滤波器定位；

S4.利用路由器搭建的WIFI作为履带机器人与PC上位机之间的无线通讯，实现PC上位机对履带机器人传感器数据信息的提取以及观察者对履带机器人运行状态的实时监控；

S5.打开自主定位与建图、导航的工作节点，对模拟环境中的任意自定义目标点指明，履带机器人自动根据算法规划的路径接收到控制指令，完成自主定位导航任务。

进一步地，所述步骤S1的具体配置过程如下：

工控机装配无线网卡发送热点到PC上位机，PC上位机的工作是负责向机器人端发送指令；STM32机器人控制模块负责编码器里程计的数据采集，同时通过电机控制其上的运行；工控机进行传感器数据的采集，同时IMU的数据与激光数据进行融合，对基于ROS***的机器人的姿态进行调整，最后作一个算法程序的运行处理；PC上位机打开可视化工具可随时观察机器人移动进程。

进一步地，所述步骤S2的具体过程如下：

以TF树的形式发布各个相关参考系的坐标变换关系，坐标变化树即为***中不同坐标系之间的偏移；履带机器人平台拥有移动的底盘、位于底盘正上方并俯视车体中心的激光雷达；TF定义两个坐标系：一个坐标系原点位于履带机器人的底盘中心点为base_link，另一个位于激光雷达的中心为base_laser；把激光数据从从base_laser变换到base_link；

激光雷达要求能够通过ROS发布sensor_msgs/LaserScan格式的消息，即为二维雷达信息；履带机器人发布nav_msgs/Odometry格式的里程计信息，同时也要发布相应的TF变换odom-to-base_link；

里程计包含两方面信息，一方面是位姿，另一方面是速度；该履带机器人里程计信息获取方法采用IMU数据积分获取，IMU获取三轴线加速度和三轴角速度数据，通过数据解析节点封装成数据类型为sensor_msgs/Imu的消息，通过ROS提供的相关包imu_tools进行滤波，得到滤波后的sensor_msgs/Imu的消息；

通过电机的编码器获取左右轮的平均速度，而转向速度根据左右轮的在给定时间内的弧度差计算得到，位置的获取根据上面的前进速度推算出位置；姿态的获取根据转向速度推算出转角。

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

设置PC上位机操作者笔记本端和履带机器人本体工控机端之间的无线通信；同一路由下，分别配置好PC上位机、工控机于网段上的域名，然后给PC上位机端***变量设置工控机端的域名，最后操作者通过SSH连接工控机上的Ubuntu***上，打开ROS主节点，从而建立两台机的ROS通讯，完成履带机器人和PC上位机无线通信的配置任务；

配置完成后，操作者在PC上位机上订阅话题，相应地可以完成PC上位机与工控机的数据交互；操作者发送的位姿指令，工控端可通过无线网络接收传送的数据并在ROS***导航框架下进行位姿跟踪；

操作者在PC上位机端通过打开ROS的可视化工具Rviz上观察机器人的运动状态、运动方向、运动轨迹在一个用激光雷达数据信息的点构成的二维栅格地图展示出来，从而实现PC上位机对履带机器人传感器数据信息的提取以及观察者对机器人运行状态的实时监控。

进一步地，所述步骤S5包括：

履带机器人的路径规划算法采用A*算法，公式表示为f(n)＝g(n)+h(n),其中f(n)表示节点n从初始点到目标点的估价函数，g(n)表示在状态空间中从初始节点到n节点的实际代价，即起始节点到当前节点的实际代价，h(n)表示从n到目标节点最佳路径的估计代价，即当前节点到目标节点的估计代价；

g(n)是对g*(n)的一个估计，是当前的搜索图G中s到n的最优路径费用g(n)≥g*(n)；h(n)是对h*(n)的估计，是从n到目标节点的估计代价，称为启发函数；保证找到最短路径条件，关键在于估价函数h(n)的选取：

估价值h(n)<＝n到目标节点的距离实际值，这种情况下，搜索的点数多，搜索范围大，效率低，但能得到最优解；如果估价值>实际值,搜索的点数少，搜索范围小，效率高，但不能保证得到最优解；估价值与实际值越接近，估价函数取得就越好；

广度优先搜索：从起点开始，首先遍历起点周围邻近的点，然后再遍历已经遍历过的点邻近的点，逐步的向外扩散，直到找到终点；在执行算法的过程中，每个点需要记录达到该点的前一个点的位置；一旦到达终点，从终点开始，反过来顺着父节点的顺序找到起点，由此构成一条路径。

与现有技术相比，本方案原理及优点如下：

1.相对于现有的救援机器人，本方案采用STM32机器人控制模块负责运动控制算法；微型电脑即工控机，性能好价格便宜；激光雷达采用Hector_Slam地图构建开源包建图，实用性强，性价比高。

2.采用编码器、IMU、激光雷达、摄像头等多传感器的数据融合方法，使得对履带式救援机器人的控制及导航更加准确。

附图说明

为了更清楚地说明本发明实施例或现有技术中的技术方案，下面将对实施例或现有技术描述中所需要使用的服务作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他的附图。

图1为本发明一种基于履带式救援机器人的自主定位导航方法中采用到的机器人***框架图。

具体实施方式

下面结合具体实施例对本发明作进一步说明：

本实施例所述的一种基于履带式救援机器人的自主定位导航方法，包括以下步骤：

S1.以履带机器人作为移动机器人平台，进行必要零件的配置和传感器扩展；具体如图1所示：

工控机装配无线网卡发送热点到PC上位机，PC上位机的工作是负责向机器人端发送指令；STM32机器人控制模块负责编码器里程计的数据采集，同时通过电机控制其上的运行；工控机进行传感器数据的采集，同时IMU的数据与激光数据进行融合，对基于ROS***的机器人的姿态进行调整，最后作一个算法程序的运行处理；PC上位机打开可视化工具可随时观察机器人移动进程。

S2.设置TF变换，采集里程计，激光雷达数据：

以TF树的形式发布各个相关参考系的坐标变换关系，坐标变化树即为***中不同坐标系之间的偏移；履带机器人平台拥有移动的底盘、位于底盘正上方并俯视车体中心的激光雷达；TF定义两个坐标系：一个坐标系原点位于履带机器人的底盘中心点为base_link，另一个位于激光雷达的中心为base_laser；把激光数据从从base_laser变换到base_link；

激光雷达要求能够通过ROS发布sensor_msgs/LaserScan格式的消息，即为二维雷达信息；履带机器人发布nav_msgs/Odometry格式的里程计信息，同时也要发布相应的TF变换odom-to-base_link；

里程计包含两方面信息，一方面是位姿，另一方面是速度；该履带机器人里程计信息获取方法采用IMU数据积分获取，IMU获取三轴线加速度和三轴角速度数据，通过数据解析节点封装成数据类型为sensor_msgs/Imu的消息，通过ROS提供的相关包imu_tools进行滤波，得到滤波后的sensor_msgs/Imu的消息；

通过电机的编码器获取左右轮的平均速度，而转向速度根据左右轮的在给定时间内的弧度差计算得到，位置的获取根据上面的前进速度推算出位置；姿态的获取根据转向速度推算出转角。

S3.采用Hector_Slam地图构建开源包建图及应用KLD采样，采用微粒滤波器定位；

Hector SLAM是很多种Scan matching算法之中的其中一种。Hector SLAM由Kohlbrecher，von Stryk，Meyer和Klingauf在达姆施塔特工业大学提出(Kohlbrecher等，2013)。Hector SLAM算法利用现代二维LIDAR的高采样频率(如Hokuyo UTM-30LX的扫描频率为40hz，意味着每隔25ms进行一次激光数据的采样)，并以传感器的扫描速率提供2D姿态估计。虽然***不提供明确的回环检测，但对于许多真实世界场景来说它足够准确，基于扫描匹配的SLAM是基于最近邻扫描匹配来估计两次扫描间机器人的平移和旋转的算法。Hector SLAM最后生成的地图也是网格图跟GMapping一样。

1)利用已经获得的地图对激光束点阵进行优化,估计激光点在地图的表示,和占据网格的概率。

2)利用Gaussian-Newton方法解决scan matching问题，获得激光点集映射到已有地图的刚体变换(x,y,theta)。

3)为避免局部最小而非全局最优，使用多分辨率地图。

4)导航中的状态估计加入惯性测量***(IMU)，利用EKF滤波。

S4.利用路由器搭建的WIFI作为履带机器人与PC上位机之间的无线通讯，实现PC上位机对履带机器人传感器数据信息的提取以及观察者对履带机器人运行状态的实时监控；具体如下：

设置PC上位机操作者笔记本端和履带机器人本体工控机端之间的无线通信；同一路由下，分别配置好PC上位机、工控机于网段上的域名，然后给PC上位机端***变量设置工控机端的域名，最后操作者通过SSH连接工控机上的Ubuntu***上，打开ROS主节点，从而建立两台机的ROS通讯，完成履带机器人和PC上位机无线通信的配置任务；

配置完成后，操作者在PC上位机上订阅话题，相应地可以完成PC上位机与工控机的数据交互；操作者发送的位姿指令，工控端可通过无线网络接收传送的数据并在ROS***导航框架下进行位姿跟踪；

操作者在PC上位机端通过打开ROS的可视化工具Rviz上观察机器人的运动状态、运动方向、运动轨迹在一个用激光雷达数据信息的点构成的二维栅格地图展示出来，从而实现PC上位机对履带机器人传感器数据信息的提取以及观察者对机器人运行状态的实时监控。

S5.打开自主定位与建图、导航的工作节点，对模拟环境中的任意自定义目标点指明，履带机器人自动根据算法规划的路径接收到控制指令，完成自主定位导航任务；具体过程如下：

履带机器人的路径规划算法采用A*算法，公式表示为f(n)＝g(n)+h(n),其中f(n)表示节点n从初始点到目标点的估价函数，g(n)表示在状态空间中从初始节点到n节点的实际代价，即起始节点到当前节点的实际代价，h(n)表示从n到目标节点最佳路径的估计代价，即当前节点到目标节点的估计代价；

g(n)是对g*(n)的一个估计，是当前的搜索图G中s到n的最优路径费用g(n)≥g*(n)；h(n)是对h*(n)的估计，是从n到目标节点的估计代价，称为启发函数；保证找到最短路径条件，关键在于估价函数h(n)的选取：

估价值h(n)<＝n到目标节点的距离实际值，这种情况下，搜索的点数多，搜索范围大，效率低，但能得到最优解；如果估价值>实际值,搜索的点数少，搜索范围小，效率高，但不能保证得到最优解；估价值与实际值越接近，估价函数取得就越好；

广度优先搜索：从起点开始，首先遍历起点周围邻近的点，然后再遍历已经遍历过的点邻近的点，逐步的向外扩散，直到找到终点；在执行算法的过程中，每个点需要记录达到该点的前一个点的位置；一旦到达终点，从终点开始，反过来顺着父节点的顺序找到起点，由此构成一条路径。

最后履带机器人在操作者的遥控下完成定位与建图，工控端对操作者在上位PC机端指定的目标点等指令解析，发出对电机相应的控制指令，机器人根据路径规划器的全局规划和局部规划选择下，快速搜寻最短路径，并在Rviz上展现的地图上标明，同时配合摄像头观察机器人本体前方状况。

以上所述之实施例子只为本发明之较佳实施例，并非以此限制本发明的实施范围，故凡依本发明之形状、原理所作的变化，均应涵盖在本发明的保护范围内。

Claims (5)

1.一种基于履带式救援机器人的自主定位导航方法，其特征在于，包括以下步骤：

S1.以履带机器人作为移动机器人平台，进行必要零件的配置和传感器扩展；

S2.设置TF变换，采集里程计，激光雷达数据；

S3.采用Hector_Slam地图构建开源包建图及应用KLD采样，采用微粒滤波器定位；

S4.利用路由器搭建的WIFI作为履带机器人与PC上位机之间的无线通讯，实现PC上位机对履带机器人传感器数据信息的提取以及观察者对履带机器人运行状态的实时监控；

S5.打开自主定位与建图、导航的工作节点，对模拟环境中的任意自定义目标点指明，履带机器人自动根据算法规划的路径接收到控制指令，完成自主定位导航任务。

2.根据权利要求1所述的一种基于履带式救援机器人的自主定位导航方法，其特征在于，所述步骤S1的具体配置过程如下：

工控机装配无线网卡发送热点到PC上位机，PC上位机的工作是负责向机器人端发送指令；STM32机器人控制模块负责编码器里程计的数据采集，同时通过电机控制其上的运行；工控机进行传感器数据的采集，同时IMU的数据与激光数据进行融合，对基于ROS***的机器人的姿态进行调整，最后作一个算法程序的运行处理；PC上位机打开可视化工具可随时观察机器人移动进程。

3.根据权利要求1所述的一种基于履带式救援机器人的自主定位导航方法，其特征在于，所述步骤S2的具体过程如下：

以TF树的形式发布各个相关参考系的坐标变换关系，坐标变化树即为***中不同坐标系之间的偏移；履带机器人平台拥有移动的底盘、位于底盘正上方并俯视车体中心的激光雷达；TF定义两个坐标系：一个坐标系原点位于履带机器人的底盘中心点为base_link，另一个位于激光雷达的中心为base_laser；把激光数据从从base_laser变换到base_link；

激光雷达要求能够通过ROS发布sensor_msgs/LaserScan格式的消息，即为二维雷达信息；履带机器人发布nav_msgs/Odometry格式的里程计信息，同时也要发布相应的TF变换odom-to-base_link；

里程计包含两方面信息，一方面是位姿，另一方面是速度；该履带机器人里程计信息获取方法采用IMU数据积分获取，IMU获取三轴线加速度和三轴角速度数据，通过数据解析节点封装成数据类型为sensor_msgs/Imu的消息，通过ROS提供的相关包imu_tools进行滤波，得到滤波后的sensor_msgs/Imu的消息；

通过电机的编码器获取左右轮的平均速度，而转向速度根据左右轮的在给定时间内的弧度差计算得到，位置的获取根据上面的前进速度推算出位置；姿态的获取根据转向速度推算出转角。

4.根据权利要求1所述的一种基于履带式救援机器人的自主定位导航方法，其特征在于，所述步骤S4具体包括：

设置PC上位机操作者笔记本端和履带机器人本体工控机端之间的无线通信；同一路由下，分别配置好PC上位机、工控机于网段上的域名，然后给PC上位机端***变量设置工控机端的域名，最后操作者通过SSH连接工控机上的Ubuntu***上，打开ROS主节点，从而建立两台机的ROS通讯，完成履带机器人和PC上位机无线通信的配置任务；

配置完成后，操作者在PC上位机上订阅话题，相应地可以完成PC上位机与工控机的数据交互；操作者发送的位姿指令，工控端可通过无线网络接收传送的数据并在ROS***导航框架下进行位姿跟踪；

操作者在PC上位机端通过打开ROS的可视化工具Rviz上观察机器人的运动状态、运动方向、运动轨迹在一个用激光雷达数据信息的点构成的二维栅格地图展示出来，从而实现PC上位机对履带机器人传感器数据信息的提取以及观察者对机器人运行状态的实时监控。

5.根据权利要求1所述的一种基于履带式救援机器人的自主定位导航方法，其特征在于，所述步骤S5包括：

履带机器人的路径规划算法采用A*算法，公式表示为f(n)＝g(n)+h(n),其中f(n)表示节点n从初始点到目标点的估价函数，g(n)表示在状态空间中从初始节点到n节点的实际代价，即起始节点到当前节点的实际代价，h(n)表示从n到目标节点最佳路径的估计代价，即当前节点到目标节点的估计代价；

g(n)是对g*(n)的一个估计，是当前的搜索图G中s到n的最优路径费用g(n)≥g*(n)；h(n)是对h*(n)的估计，是从n到目标节点的估计代价，称为启发函数；保证找到最短路径条件，关键在于估价函数h(n)的选取：

估价值h(n)<＝n到目标节点的距离实际值，这种情况下，搜索的点数多，搜索范围大，效率低，但能得到最优解；如果估价值>实际值,搜索的点数少，搜索范围小，效率高，但不能保证得到最优解；估价值与实际值越接近，估价函数取得就越好；

广度优先搜索：从起点开始，首先遍历起点周围邻近的点，然后再遍历已经遍历过的点邻近的点，逐步的向外扩散，直到找到终点；在执行算法的过程中，每个点需要记录达到该点的前一个点的位置；一旦到达终点，从终点开始，反过来顺着父节点的顺序找到起点，由此构成一条路径。

Images