搜救机器人手臂辅助同时定位及构建环境地图系统及方法与流程

本发明属于机器人领域，尤其涉及一种搜救机器人手臂辅助同时定位及构建环境地图系统及方法。

背景技术：

机器人(Robot)是自动执行工作的机器装置。它既可以接受人类指挥，又可以运行预先编排的程序，也可以根据以人工智能技术制定的原则纲领行动。它的任务是协助或取代人类工作的工作，例如生产业、建筑业，或是危险的工作。搜救机器人，为救援而采取先进科学技术研制的机器人，如地震救援机器人，它是一种专门用于大地震后在地下商场的废墟中寻找幸存者执行救援任务的机器人。

目前，在搜救机器人同时定位和构建环境地图(simultaneous localization and mapping,SLAM)时，有危险的目标如爆炸物在构建的环境地图中未能作标记，这样易伤害搜救中的人员；在搜救和构建地图区域探索被困者和牺牲者过程中，多会忽略一些遮盖物，易使搜救人员未注意到牺牲者；在灾难现场搜救中，同时定位和构建地图所形成的环境地图缺少感知性。上述均为现有技术的不足之处。

技术实现要素：

为了解决现有技术的不足，本发明提供了一种搜救机器人手臂辅助同时定位及构建环境地图系统，其具有同时定位和构建环境地图效果，解决了搜救机器人构建环境地图时的感知性问题。

本发明的一种搜救机器人手臂辅助同时定位及构建环境地图系统，包括：

机器人本体，所述机器人本体上安装有机器人控制器、定位传感器模块、图像采集模块和机器人手臂，所述机器人手臂与手臂驱动机构相连，所述手臂驱动机构与机器人控制器相连，所述定位传感器模块和图像采集模块分别与机器人控制器相连；

所述定位传感器模块用于实时检测指定目标的位置信息并传送至机器人控制器内，所述图像采集模块还用于实时采集机器人周围的图像信息并传送至机器人控制器内；所述机器人控制器用于根据接收到的图像信息以及指定目标的位置信息进行点云匹配来构建出目标环境地图；所述机器人控制器用于根据构建出的目标环境地图来实时向手臂驱动机构输出脉冲信号来控制机器人手臂实现搜救工作。

进一步的，所述定位传感器模块包括机器人前向定位声纳传感器、机器人后向定位声纳传感器、机器人激光测距仪和陀螺仪定位传感器。本发明通过机器人前向定位声纳传感器、机器人后向定位声纳传感器，其中，声纳传感器的原理为：声纳传感器发出一个声波信号，当遇到物体后会反射回来，依据反射时间及波型去计算它的距离及位置超声波传感器是利用超声波的特性研制而成的传感器。声纳传感器能够准确检测物体的大小。

进一步的，所述机器人手臂为多自由度手臂。多自由度手臂能够实现机器人手臂的多维度运动，使得机器人手臂更加灵活。

进一步的，所述机器人控制器还与机器人工控机相连，所述机器人工控机与显示装置相连。机器人工控机用于实时接收机器人控制器输出的目标环境地图，进而在显示装置上实时显示。

进一步的，所述机器人工控机还和云端服务器相连，所述云端服务器与远程监控终端相连。

进一步的，所述机器人手臂的末端还安装末端执行器，所述末端执行器还与夹持器相连，所述末端执行器与机器人控制器相连。本发明利用末端执行器来实时调整夹持器的工作状态，实现救援和搜救工作。

本发明还提供了一种搜救机器人手臂辅助同时定位及构建环境地图系统的工作方法。

本发明的搜救机器人手臂辅助同时定位及构建环境地图系统的工作方法，包括：

定位传感器模块和图像采集模块分别实时检测指定目标的位置信息和机器人周围的图像信息，并传送至机器人控制器内；

机器人控制器根据接收到的图像信息以及指定目标的位置信息进行点云匹配来构建出目标环境地图；

机器人控制器根据构建出的目标环境地图来实时向手臂驱动机构输出脉冲信号来控制机器人手臂实现搜救工作。

进一步的，在机器人控制器内采用分布粒子滤波算法来同时定位和构建目标环境地图。

进一步的，该方法还包括：机器人控制器将构建的目标环境地图输出至机器人工控机，在与机器人工控机相连的显示装置上实时显示。

进一步的，该方法还包括：远程监控终端还通过云端服务器实时调取机器人工控机接收到的目标环境地图并进行远程实时监控。

与现有技术相比，本发明的有益效果是：

本发明在搜救机器人构建环境地图过程中，将有危险的目标物标识于地图上，从而防止搜救人员受伤害；机器人手臂通过控制器作用将遮盖物翻动远离被困者和牺牲者的区域，防止搜救人员未发现搜救目标；搜救机器人在灾难现场探索时，同时定位和建图中所形成的地图更加精确和具备感知性，给搜救人员使用，防止恶劣灾难现场搜救中的人员伤害事故及被困者和牺牲者未被发现情况。

附图说明

构成本申请的一部分的说明书附图用来提供对本申请的进一步理解，本申请的示意性实施例及其说明用于解释本申请，并不构成对本申请的不当限定。

图1是本发明的一种搜救机器人手臂辅助同时定位及构建环境地图系统结构示意图；

图2是本发明的一种搜救机器人手臂辅助同时定位及构建环境地图流程图；

图3是本发明的一种搜救机器人手臂控制流程；

图4是本发明的一种搜救机器人手臂辅助同时定位及构建环境地图实验和仿真过程；

图5是本发明的一种搜救机器人构建的环境地图。

具体实施方式

应该指出，以下详细说明都是例示性的，旨在对本申请提供进一步的说明。除非另有指明，本文使用的所有技术和科学术语具有与本申请所属技术领域的普通技术人员通常理解的相同含义。

需要注意的是，这里所使用的术语仅是为了描述具体实施方式，而非意图限制根据本申请的示例性实施方式。如在这里所使用的，除非上下文另外明确指出，否则单数形式也意图包括复数形式，此外，还应当理解的是，当在本说明书中使用术语“包含”和/或“包括”时，其指明存在特征、步骤、操作、器件、组件和/或它们的组合。

图1是本发明的一种搜救机器人手臂辅助同时定位及构建环境地图系统结构示意图。

如图1所示，本发明的一种搜救机器人手臂辅助同时定位及构建环境地图系统，包括：

机器人本体，所述机器人本体上安装有机器人控制器、定位传感器模块、图像采集模块和机器人手臂，所述机器人手臂与手臂驱动机构相连，所述手臂驱动机构与机器人控制器相连，所述定位传感器模块和图像采集模块分别与机器人控制器相连；

所述定位传感器模块用于实时检测指定目标的位置信息并传送至机器人控制器内，所述图像采集模块还用于实时采集机器人周围的图像信息并传送至机器人控制器内；所述机器人控制器用于根据接收到的图像信息以及指定目标的位置信息进行点云匹配来构建出目标环境地图；所述机器人控制器用于根据构建出的目标环境地图来实时向手臂驱动机构输出脉冲信号来控制机器人手臂实现搜救工作。

其中，机器人本体可以为履带式机器人或轮式机器人。

定位传感器模块包括机器人前向定位声纳传感器、机器人后向定位声纳传感器、机器人激光测距仪和陀螺仪定位传感器。

本发明通过机器人前向定位声纳传感器、机器人后向定位声纳传感器，其中，声纳传感器的原理为：声纳传感器发出一个声波信号，当遇到物体后会反射回来，依据反射时间及波型去计算它的距离及位置超声波传感器是利用超声波的特性研制而成的传感器。声纳传感器能够准确检测物体的大小。

在本实施例中，机器人手臂为多自由度手臂。多自由度手臂能够实现机器人手臂的多维度运动，使得机器人手臂更加灵活。

机器人控制器还与机器人工控机相连，所述机器人工控机与显示装置相连。机器人工控机用于实时接收机器人控制器输出的目标环境地图，进而在显示装置上实时显示。

在另一实施例中，所述机器人工控机还云端服务器相连，所述云端服务器与远程监控终端相连。

在另一实施例中，所述机器人手臂的末端还安装末端执行器，所述末端执行器还与夹持器相连，所述末端执行器与机器人控制器相连。本发明利用末端执行器来实时调整夹持器的工作状态，实现救援和搜救工作。

图2是本发明的一种搜救机器人手臂辅助同时定位及构建环境地图流程图。

如图2所示，本发明的搜救机器人手臂辅助同时定位及构建环境地图系统的具体流程，包括：

定位传感器模块和图像采集模块分别实时检测指定目标的位置信息和机器人周围的图像信息，并传送至机器人控制器内；

机器人控制器根据接收到的图像信息以及指定目标的位置信息进行点云匹配来构建出目标环境地图；

机器人控制器根据构建出的目标环境地图来实时向手臂驱动机构输出脉冲信号来控制机器人手臂实现搜救工作。

其中，在机器人控制器内采用分布粒子滤波算法来同时定位和构建目标环境地图。

该方法还包括：机器人控制器将构建的目标环境地图输出至机器人工控机，在与机器人工控机相连的显示装置上实时显示。

该方法还包括：远程监控终端还通过云端服务器实时调取机器人工控机接收到的目标环境地图并进行远程实时监控。

图3是本发明的一种搜救机器人手臂控制流程。

如图3所示，搜救机器人手臂的速度可由公式(1)来表示：

W:q的向量函数；

α：标量常数，使得达到极小值时，V取得期望值；

V：长度为m维的末端执行器的状态向量；

q：长度为n维的关节位置向量，表示手臂关节到达的位置，其物理含义为关节到达的位置组成的一个向量；

q的时间微分，代表关节到达位置时的速度。

F:所有标准的加权和，是核心速度控制的标准向量，这个向量既可以是单个优化标准，也可以是多个优化标准的联合。

w_i:第i个标准的权重值；

F_i：第i个标准条件；

J:操作器m×n维的雅可比(Jacobian)矩阵，用以控制手臂运动；

N_J:n×(n-m)维包含零空间的雅可比矩阵的向量组集合，其秩为(n-m)，定义为

JN_J＝0 公式(3)

β(q,V):健壮广义因子，提供运动奇异点的鲁棒性。

本发明的控制器所用控制系统具备关节灵活度和能量消耗控制，运动算法保证在手臂运动的关节极限无控制操作的情况下，能够避障，而且使得运动能量消耗最小。控制系统用于约束机器人手臂按期望的方式运动，同时避免关节极限、奇异点，以及与环境中物体的碰撞。

本发明的手臂运动控制器设计具备通用性，控制系统被加载于手臂控制器。控制器的各种可能的控制算法可以实现不同控制标准的运动形式和各种结构目标物体的抓取，但是，手臂关节运动极限避免和与环境物体的避障是控制器的运动控制算法的最频繁使用的两个优化标准。在本发明的手臂控制器中，优化范围设定为这两个标准。本发明设计的手臂运动控制器只包含避免关节运动极限和与环境物体避障两个优化标准。通过对每个选择的优化标准设定权重因子来控制和优化手臂的运动。

在本发明中，机器人手臂辅助构建环境地图步骤：

步骤一：使用关节末端执行器集合制机器人手臂的一组关节角，关节末端执行器集合是公式(1)所述的手臂末端执行器到达的位置的向量V所需要的控制命令组成的集合；

步骤二：利用点末端执行器集合控制机器人手臂到达某一点，点末端执行器集合是公式(1)所述的手臂关节到达的位置向量b所需要控制命令组成的集合；

步骤三：利用框架末端执行器集合调整机器人手臂的抓取位姿，框架末端执行器集合是公式(1)所述的操纵机器人手臂整体协调运动的雅可比矩阵向量组N_J的控制命令集合；

步骤四：开合机器人手臂连接的夹持器；

步骤五：打开并运行之前创建的一系列操作的程序代码示例集，本发明支持离线建立机器人手臂运动命令集合，生成控制程序代码示例集，加载于手臂控制器的控制系统，提供机器人手臂抓取物体的控制参数；

步骤六：利用关节控制移动机器人手臂，并利用末端执行器控制移动夹持器；

步骤七：计算所需的距离和方位角，移动机器人手臂的关节状态，达到所期望的位姿；这个探测过程的轨迹可以是直线或曲线；

步骤八：以给定的速度移动机器人手臂的末端执行器；

步骤九：切换不同的末端执行器集合，控制机器人手臂运动，辅助机器人定位和构建地图过程。

本发明支持离线建立机器人手臂运动命令集合，生成控制程序代码示例集，存储于手臂控制器的控制系统，提供机器人手臂抓取物体的控制参数，同时可以实时地在控制器上编辑和设定机器人手臂的位姿控制命令语句。这些控制命令包括公式(1)所述的手臂末端执行器到达的位置的向量V所需要的控制命令集合、手臂关节到达的位置向量b所需要控制命令集合、操纵机器人手臂整体协调运动的雅可比矩阵向量组N_J的控制命令集合，这些控制命令集合由关节末端执行器集合、点末端执行器集合、框架末端执行器集合组成。

本发明在同时定位和构建地图过程采用分布粒子滤波(DP,Distributed Particle)算法。

本发明中所用搜救机器人还包含内置工控机(industrial PC,IPC)，安装机器人地图构建控制系统，且机器人内置的工控机能够接收远程PC计算机控制中心的控制。机器人手臂控制器的控制系统、分布粒子滤波器及其粒子滤波运动估计器都安装和运行于搜救机器人内置的工控机IPC上。手臂控制器的控制系统、分布粒子滤波器及其粒子滤波运动估计器同为搜救机器人构建地图控制器的组成部分。分布粒子滤波器算法持续估计搜救机器人的位置和姿态，以使机器人位姿配合手臂进行感知探测，当机器人手臂获取到搜救的兴趣目标地点或物体时，则附加的粒子被记录，作为地图构建信息，以确定机器人所处位置和方向，并在所构建的搜救区域的地图上标识出兴趣目标地点或物体。

全局地图分割为栅格单元，每个栅格单元独立完成定位和构建局部地图过程。每个栅格单元包含一定数量的随机粒子，根据每个粒子的权重因子，计算栅格地图的占有率，相应栅格单元通过分配空闲率被占有。

每一个栅格地图是一个二维栅格单元内的随机粒子，栅格单元的随机粒子的概率定义为：

P(G^k|x_k,m_k)＝P(G^k|p^k)P(p^k|x_k,m_k) 公式(4)

其中：

P(G^k|x_k,m_k):栅格地图单元随机粒子占有的概率；

m_k:表示第k个粒子所处的栅格单元；

x_k:表示第k个粒子当前所处的位置；

p^k:第k个粒子的栅格单元占有概率；

G^k:栅格单元的空闲率；

P(G^k|p^k):栅格单元空闲的条件概率；

P(p^k|x_k,m_k):栅格单元占有的概率，是确定性参数α，根据不同栅格单元在0～1之间取值，因此公式(4)表示为公式(5):

P(G^k|x_k,m_k)＝αP(G^k|p^k),α∈(0～1) 公式(5)

粒子滤波算法的栅格地图在空闲集群中以一种进化的方式编写程序代码。按照栅格地图的占有概率，采用相应的值进入空闲空间。当栅格地图进化并生成一个扩大的边界时，集群和空闲空间得到更新。在粒子滤波器中，从给定的标准地图配置生成地图，并产生多个不同粒子群的栅格地图。

粒子滤波运动估计器计算可用的栅格单元，当地图的栅格单元数目增加时，搜救移动机器人的定位信息得到更新。粗的栅格分辨率将会导致不准确的定位信息和地图构建，例如，较大的栅格单元尺寸将会涉及较少的位置信息。

相比于现有技术，分布粒子滤波器算法能够改善基于信息熵的探测灵敏度分析的闭环覆盖性能。同时，分布粒子滤波器算法的粒子渗透占有性和栅格地图构建的进化性，适合于搜救移动机器人在非结构的、灾后障碍物簇集场景的定位和环境地图构建。

栅格地图单元和机器人的位姿在粒子滤波器中保持一个联合分布，栅格地图是一个全局像栅格一样占有阵列，而且每一个栅格单元与机器人的位姿相关。分布粒子滤波器分开储存每一个粒子的栅格地图的副本，在地图构建过程中，关联机器人的位姿信息。

根据记录的粒子数量的增加，更新原有的标准地图和机器人的位姿。机器人在n时刻的位姿ρ_n表示为

ρ_n＝[x_n,y_n,θ_n]^T 公式(6)

其中，(x_n,y_n)是笛卡尔坐标位置，θ_n代表方位角。

在n时刻的栅格地图用一个二维矩阵表示，N_x是行数，N_y是列数，在m_n中的像素灰度值表明栅格单元的空闲、占有和未被探测三种状态。搜救机器人在n时刻的测量值是S_n，u_n∈R³和z_n∈R¹⁸¹.S_n定位信息包含ρ_nθ_n和m_n；u_n∈R³估计机器人位置的三个参数x_n,y_n,θ_n；z_n∈R¹⁸¹包含激光测距仪估计机器人前方超过180⁰的视角范围。

机器人的定位信息是一个概率密度函数，定义为：

bel(S_n)＝ρ(S_n|z₁,...,z_n,u₁,...,u_n) 公式(7)

在分布粒子滤波算法中，产生一系列粒子来估计概率密度函数bel(S_n)，每一个粒子含有一组机器人位姿信息和局部栅格地图。当机器人收集到一组新的测量数据，就分配给每个粒子一个影响因子。一个粒子对当前测量的支持越高，分配的影响因子就越高。因此，原先的栅格地图被更新，新的位姿通过重采样的粒子得到确认，而且，一个粒子的影响因子越高，越容易被选取。通过计算栅格地图占有率，重复这样一个重采样步骤，相同的粒子可以被多次选取。

本发明中所用机器人在给定的搜救区域周期性地进行全局覆盖式探索，这样每个特定粒子群与一个栅格地图相联系，形成各个栅格局部地图。每一个局部栅格地图有各自独立的定位和地图构建过程。然后根据各栅格地图的粒子的权重因子，即公式(5)所述确定性参数α，由公式(4)-(5),计算每个局部栅格地图的占有率。随着粒子数量的增加，由公式(6)-(7)更新机器人位姿信息、各栅格局部地图的定位信息，根据各局部栅格地图的占有概率，输出搜救区域的一个全局地图。

图4是本发明的一种搜救机器人手臂辅助同时定位及构建环境地图实验和仿真过程。

在进行仿真实验过程中，本发明的软件编译环境为Cmake、Visual studio 2013、Matlab；Matlab用来仿真MobileRobot移动机器人控制平台控制机器人探索和建图，Visual studio 2013用来控制程序生成，Cmake用来进行程序编译环境配置。

通过上述实验，得到的搜救机器人构建的环境地图，如图5所示。根据图5可知：本发明在搜救机器人构建环境地图过程中，将有危险的目标物标识于地图上，从而防止搜救人员受伤害；机器人手臂通过控制器作用将遮盖物翻动远离被困者和牺牲者的区域，防止搜救人员未发现搜救目标；搜救机器人在灾难现场探索时，同时定位和建图中所形成的地图更具备精确性，给搜救人员使用，防止恶劣灾难现场搜救中的人员伤害事故及被困者和牺牲者未被发现情况。机器人控制器用于搜救机器人手臂辅助构建环境地图，补偿了机器人自身定位和建图中的现有技术的感知性不足。

上述虽然结合附图对本发明的具体实施方式进行了描述，但并非对本发明保护范围的限制，所属领域技术人员应该明白，在本发明的技术方案的基础上，本领域技术人员不需要付出创造性劳动即可做出的各种修改或变形仍在本发明的保护范围以内。