CN105216905B - 即时定位与地图创建勘测搜救机器人 - Google Patents

Abstract

本发明公开了一种即时定位与地图创建勘测搜救机器人，应用于灾后隧道地图创建、地形勘探等领域，包括越障底盘，车身平台，三维扫描模块，主控单元，车身外壳和驱动模块组成。越障底盘采用六轮驱动的仿生学悬架结构，使车体具有对不同地形的被动自适应性。三维扫描模块包括有三维扫描机构，激光扫描雷达，红外温度传感器，激光惯性传感器和固定支架组成。该机器人可在行进过程中自主探测周围环境，并实时创建三维地形图，同时探测是否有可能生命体存在；能将实时三维地图、可能生命体搜救位置反馈给操作人员；帮助操作人员快速、准确地了解现场情况，以便尽快展开救援工作，同时减少勘测和搜救人员不必要的人员伤亡和劳动损耗。

Description

即时定位与地图创建勘测搜救机器人

技术领域

本发明涉及一种用于封闭空间中复杂环境地图创建、地形勘测、搜救等任务的机器人，尤其涉及一种地势崎岖条件下的狭小洞穴、灾区等未知区域的环境探测及可能生命体搜救的即时定位与地图创建勘测搜救机器人。

背景技术

近年来，地震、海啸、塌方、核电站泄漏等灾难相继发生。灾难事故发生后，现场建筑结构坍塌，导致其环境及地形结构空间狭小、复杂且不稳定，目前的救援方法是外部搜救，即利用一些生命体感知装置检测废墟下是否有待救人员，然而普通探测装置无法从外部直接了解内部情况，而对于结构坍塌的建筑物这种高危未知环境，在无法保证安全的情况下，救援人员是不能贸然进入的，致使搜救人员无法立刻进入灾区进行勘察和搜救，延误救援时机。而灾后“黄金72小时”内受灾人员的存活率随时间呈急速递减趋势：在第一天(即24小时内)，存活率为90％左右，第二天为50％～60％，第三天仅为20％～30％。因此，迫切需要一种可以代替搜救人员进入危险未知环境中进行地形勘测及生命搜救的机器人。救援人员可在废墟外部遥控机器人进入危险地带进行自主搜索，及时返回搜索区域三维地图；同时利用传感器感应可能的生命体，使救灾人员即时了解内部情况，确定有效方案，方便尽快展开搜救工作。该类机器人不仅可以大大缩短搜救时间，提高灾民生还希望，同时也减少了搜救人员的二次伤亡。

发明内容

本发明的目的是针对上述需求，提供一种即时定位与地图创建勘测搜救机器人，能够进入未知的核反应堆、洞穴、自然灾害后的封闭空间等不利于救灾人员和科研人员进入的环境，具有很强的地形适应性，具有即时定位与地图创建和进行生命体探测的功能，提高作业效率，减少了勘测和搜救人员劳动损耗和伤亡，可提升我国的勘测水平，为未来数字化勘测和救援提供技术手段。

本发明所提供的技术方案是提出一种即时定位与地图创建勘测搜救机器人，其包括：

其包括越障底盘，车身平台，三维扫描模块，主控单元，车身外壳和驱动模块；其中，越障底盘由主体框架和行走轮腿组成，使车体具有对不同地形的被动自适应性；三维扫描模块能够获得被勘测空间的三维地图信息以及可能生命体的数据。

所述越障底盘的左右两侧分别安装一个前轮退和二个后轮腿，各轮腿均采用直角梯形的四杆机构悬架，每个悬架在直角边和长边杆件间的铰链和短边与斜边杆件间的铰链处安装了一个空气减震器；每个悬架斜边杆件的末端都装有一个车轮；所述三维扫描模块包括有三维扫描机构、激光扫描雷达、红外温度传感器、激光惯性传感器和固定支架；三维扫描机构由二个完全一样且同步动作的无急回特性的铰链四杆机构组成，铰链四杆机构中的曲柄匀速转动时，二个同步动作的铰链四杆机构可实现其中的托架以同一个平均速度往复摆动；激光扫描雷达正下方对应扫描范围内，均匀扇形分布若干个红外温度传感器；所述主控单元为平板电脑，紧密镶嵌于车身外壳的顶部；所述越障底盘、三维扫描模块的三维扫描机构均采用高强度铝型材，车身平台采用不锈钢板，车身外壳采用ABS工程塑料。

此外，本发明还提出了应用上述即时定位与地图创建勘测搜救机器人进行地形勘测和可能生命体搜救的方法。

本发明的有益效果为：该即时定位与地图创建勘测搜救机器人进入不适宜工作人员进入的，危险未知且空间狭小的环境，该机器人可适应较崎岖的地形，且在行进过程中自动探测周围环境，并实时创建周边环境的三维地形图，同时探测该环境中是否有可能生命体存在；并能将实时三维地图、可能生命体搜救位置等重要信息反馈给操作人员；帮助操作人员快速、准确地了解位置环境及可能生命体的存在，以便搜救人员尽快展开救援工作，同时减少勘测和搜救人员不必要的人员伤亡和劳动损耗。

附图说明

下面结合附图及具体实施方式对本发明作进一步详细说明：

图1为本发明的即时定位与地图创建勘测搜救机器人的整体结构图；

图2为本发明的越障底盘结构图；

图3为本发明的三维扫描模块结构图；

图4为本发明的三维地图创建及可能生命体探测方法流程图。

附图标记：

1-越障底盘； 2-车身平台； 3-三维扫描模块； 4-激光惯性传感器；

5-主控单元； 6-车身外壳； 7-驱动模块； 8-车体惯性传感器；

9-主体框架； 10-前轮； 11-转向电机； 12-直流伺服电机；

13-悬架； 14-空气减震器； 15-后轮； 16-红外温度传感器；

17-激光扫描雷达； 18-连杆； 19-曲柄； 20-减速电机；

21-减速电机支架； 22-联轴器； 23-固定支架； 24-机架；

25-托架； 26-斜边杆件；

具体实施方式

本发明的即时定位与地图创建勘测搜救机器人，其较佳的具体实施方式如图1至图4所示：

如图1所示，本发明的即时定位与地图创建勘测搜救机器人包括越障底盘1，车身平台2，三维扫描模块3，主控单元5，车身外壳6和驱动模块7。如图1和图2所示，越障底盘1的主体框架9上搭载了主要机构和传感器，包括有车身平台2，驱动模块7，车体惯性传感器8和三维扫描模块3。车身平台2上安装有三维扫描模块3和车身外壳6，车身外壳6通过刚性支架支撑，以防止变形或破坏。即时定位与地图创建勘测搜救机器人的越障底盘1、三维扫描模块3的三维扫描机构均采用高强度铝型材，车身平台2采用不锈钢板，车身外壳6采用ABS工程塑料，在保证强度和高度的同时，大幅降低整体重量。

1、越障底盘1：

如图1和图2所示，本发明中，越障底盘1采用六轮驱动的仿生学悬架结构，由主体框架9和6个行走轮腿组成。主体框架9的左右两侧，分别安装了1个前轮腿和2个后轮腿，各轮腿均采用直角梯形的四杆机构悬架13，各杆件通过铰链连接，可发生相互转动；每个悬架13在直角边和长边杆件间的铰链和短边与斜边杆件26间的铰链处安装了一个空气减震器14，对悬架13的四杆机构起到约束作用，同时能够根据地形或者障碍物的不同调节空气减震器的伸缩量对车轮施加不同支撑力并使车轮产生移动，从而使车体具有对不同地形的被动自适应性；空气减震器14作为储能元件，当机器人在崎岖路面行进时，还可对车体起到缓冲吸振的作用；越障底盘1通过仿生学悬架机构对机器人运动姿态进行调整，实现越障的同时尽量减小机器人位姿发生较大变化。每个悬架13的斜边杆件26的末端都装有1个越野车轮，越野车轮为粗花深沟耐磨型实心橡胶车轮，不会因为碾压尖锐物体而破损，同时具有良好的爬坡和抓地能力；左右两侧的前轮10上分别安装有一个转向电机11和一个直流伺服电机12，其中转向电机11控制前轮10转动，实现机器人的转向功能，直流伺服电机12为前轮10提供行进动力；此外，左右两侧的后轮15上也安装了与前轮10一样的直流伺服电机12，使每个后轮都具有行进动力。所有的转向电机11和直流伺服电机12均通过电机支架安装于各轮腿之上。每个直流伺服电机12均配有编码里程计，用以测量每个车轮在一定时间周期内的行进距离，先利用式(1)分别计算出每排轮腿的平均行进距离，然后利用式(2)计算出机器人的行进距离。

其中，S_i为每排轮腿的平均行进距离，S_iL和S_iR分别为每排轮腿左侧车轮和右侧车轮的行进距离(i＝1，2，3，其中1为前排轮腿，2为中间排轮腿，3为后排轮腿)；S为机器人的行进距离。

2、三维扫描模块3：

三维扫描模块3能够三维扫描被勘测空间，并实时创建该环境的三维地形图，同时探测该环境中是否有可能生命体存在。如图3所示，三维扫描模块3包括有三维扫描机构，激光扫描雷达17，红外温度传感器16，激光惯性传感器4、无急回特性的铰链四杆机构和固定支架23组成，其中三维扫描机构由两个完全一样且同步动作的铰链四杆机构组成，包括有2个同轴曲柄19、2个连杆18、1个托架25和1个U型的机架24组成；一个减速电机20通过减速电机支架21固定在机架24上，并通过联轴器22与传动轴连接，带动2个曲柄19同步转动，传动轴与曲柄19固连，并通过滑动轴承安装于机架24之上；曲柄19与连杆18，连杆18与托架25，托架25与机架24之间的铰链均为滑动轴承；铰链四杆机构设计成无急回特性(即极位夹角为0°)的四杆机构，使其四个杆件的长度满足式(3)的要求：

其中，l₁，l₂，l₃，l₄分别为曲柄19、连杆18、托架25和机架24的长度。当减速电机20带动曲柄19沿某一方向匀速转动时，2个同步动作的铰链四杆机构可实现托架25以同一个平均速度在一定摆角范围内往复摆动；SICK LMS511Pro型激光扫描雷达17固定于托架25之上，激光扫描雷达17可实现与托架25平行的二维平面扫描并获得扫描区域内的物体的轮廓点数据，其扫描起始角度为10°，终止角度为170°，扫描角度范围为160°，扫描角度分辨率为0.5°，对应的数据更新频率为75Hz，最大探测距离为26m。激光扫描雷达17通过以太网与主控单元5进行数据通讯；激光扫描雷达17正下方对应160°扫描范围内，均匀扇形分布8个红外温度传感器16，用以非接触探测可能的生命体，各红外温度传感器16之间的夹角为20°，温度测量范围0℃～100℃，最大测量距离为2m；三维扫描机构的减速电机20带动曲柄19匀速转动，带动安装在托架25上的激光扫描雷达17和红外温度传感器16在一定摆角范围内垂直于车身平台2上下往复摆动(图1)，将二维扫描转化成三维扫描，用以勘测周围环境的三维空间信息；激光惯性传感器4紧密固定在激光扫描雷达17顶部，用以实时测量激光扫描雷达17在移动三维探测过程中的俯仰角P_L、滚动角R_L和航向角Y_L，与主控单元5通过RS485进行数据通讯，数据更新频率为100Hz。整个三维扫描机构通过螺栓紧密固定在固定支架23上，固定支架23紧密固定在车身平台2上。

3、驱动模块7：

如图1所示，本发明中，驱动模块7紧密固定在越障底盘1的主体框架9上，包括有电源和电机驱动卡。电源为36VDC大容量锂电池，并转换成24VDC，12VDC和5VDC电压为机器人的各电机和传感器供电，并为整个系统提供过流和限流保护；电机驱动卡根据主控单元5的控制指令，驱动2个转向电机11、6个直流伺服电机12和1个减速电机20动作。车载惯性传感器8同样紧密固定在越障底盘1的主体框架9上，其测量参数与激光惯性传感器4完全相同，用以检测机器人移动作业过程中，车身的俯仰角P_V、滚动角R_V和航向角Y_V。

4、主控单元5：

如图1所示，本发明中，主控单元5为工业级嵌入式平板电脑，紧密镶嵌于车身外壳6的顶部，方便操作人员观察和操作。主控单元5主要负责采集所有传感器数据，包括激光扫描雷达17，红外温度传感器16，激光惯性传感器4、车体惯性传感器8和每个直流伺服电机12的编码里程计，并根据传感器数据进行相应的坐标转化、地图创建、生命体识别和控制决策，向驱动模块7发送控制指令控制所有执行装置，包括转向电机11、直流伺服电机12和减速电机20。主控单元5还配有无线通讯模块，可将创建的三维空间地图和可能的生命体位置等信息，发送给外部操作人员。

5、应用即时定位与地图创建勘测搜救机器人进行地形勘测和可能生命体搜救的方法：

下面结合图4，对应用上述即时定位与地图创建勘测搜救机器人进行地形勘测和可能生命体搜救的方法做具体描述：

(1)传感器数据采集：即时定位与地图创建勘测搜救机器人在狭长空间中勘测或搜救时，在低速匀速行进的过程中，三维扫描机构的减速电机20带动激光扫描雷达17和红外温度传感器16上下往复摆动来探测周围环境。激光扫描雷达17数据采集，红外温度传感器16数据采集，激光惯性传感器4数据采集，车体惯性传感器8数据采集分别在4个线程中同时进行，其中主线程包括激光扫描雷达17数据采集、可能生命体匹配以及三维地图创建和可能生命体匹配。当激光扫描雷达17完成一次扫描后，得到激光扫描雷达17当前位姿下二维扫描平面内的点云数据，包括扫描角度和该扫描角度下的测量距离，连同红外温度传感器16获得的温度数据一并发送给主控单元5；主控单元5首先对点云数据进行距离滤波，去除不必要的噪声，将测量距离超过范围A的点云数据剔除；

(2)可能生命体数据匹配：激光扫描雷达17的二维扫描范围是160°，红外温度传感器16共有8个，每个红外温度传感器16所对应的激光扫描雷达17的扫描角度为20°；主控单元5筛选每个红外温度传感器16的温度数据，若数据在[35℃，39℃]范围内，认为是可能生命体；同时，在该红外温度传感器16所对应的激光扫描角度内，将测量距离不超过红外温度传感器16最大测量范围B的点云数据标记为可能生命体；

(3)激光大地坐标下三维地图创建：根据激光惯性传感器4获得的激光扫描雷达17当前时刻的俯仰角、滚动角和航向角，首先将步骤(1)和(2)得到的点云数据利用公式(4)转换到以激光扫描雷达17光源为原心的激光相对坐标系L’中：

其中，为第i个点云数据在激光相对坐标系L’中的坐标，θ_i为第i个点云数据的扫描角度，ρ_i为第i个点云数据的测量距离。然后再利用式(5)转换到激光大地坐标系中：

其中，为第i个点云数据在激光大地坐标系L中的坐标，P_L，R_L，Y_L分别为当前时刻激光扫描雷达17的俯仰角、滚动角和航向角。从而创建出激光大地坐标系下的三维地图；

(4)大地坐标系下三维地图叠加：根据车体惯性传感器8获得的即时定位与地图创建勘测搜救机器人当前时刻的俯仰角、滚动角和航向角，利用式(6)再将激光大地坐标系下的三维地图叠加到大地坐标系中：

其中，P_V，R_V，Y_V分别为车体惯性传感器8获得的即时定位与地图创建勘测搜救机器人当前时刻的俯仰角、滚动角和航向角，Δx_S，Δy_S，Δz_S分别为即时定位与地图创建勘测搜救机器人沿大地坐标系V的x，y，z三轴的平移量，可通过式(1)和(2)并根据P_V，R_V，Y_V计算得出。随着即时定位与地图创建勘测搜救机器人不断行进，主控单元5最终将各个时刻的激光大地坐标系三维地图都叠加到同一大地坐标系中，形成最终的三维空间地图(其中包含可能生命体的轮廓和所处位置信息)；

(5)导航路径规划及前轮转向控制：导航路径规划及前轮转向控制在另一单独线程中完成。六轮的越障底盘1的运动模型简化为一个基本的二轮运动模型，包括一个用于转向的前轮和一个被动的后轮，如图2所示，其中前轮的等效虚拟中心O_f位于越障底盘6两前轮10回转轴连线的中点处，后轮的等效虚拟中心O_r位于4个后轮15回转中心所构成的矩形的形心处。主控单元5根据创建的大地坐标系三维地图，实时判断狭长空间横截面形心连线的曲率变化趋势，并进行平滑滤波，避免曲率发出突然变化导致出现前轮转向电机11所无法实现的过大转角或过快转速，以平滑后的曲率在越障底盘1质心所在平面的投影为预期路线；然后根据越障底盘1的行进速度、轮胎直径、俯仰角P_V、滚动角R_V和航向角Y_V，采用模糊PID控制策略，遵循前后轮同一速度瞬心的原则，即转向过程中前轮和后轮的等效虚拟中心分别进行圆周运动，且拥有相同的圆心P，进行导航路径规划，得出前轮等效虚拟中心O_f需要的转角以及转动速度；最后根据两前轮10的轮距计算出各个前轮10的转角和转速，并由主控单元5向驱动模块7发出指令，控制两前轮10做出相应动作，实现自主导航；

(6)地图存储及无线传输：主控单元5将叠加形成的大地坐标系下的三维地图，即时定位与地图创建勘测搜救机器人所有传感器的数据，以及机器人自主导航的路线及其实际行走路线存贮到数据库中，方便操作人员进行数据管理和离线地图生成；同时通过无线传输将上述信息发送给封闭空间外的接收端(一般为便携式电脑)并实时显示，使外部操作人员清楚了解内部空间情况。

以上所述，仅为本发明较佳的具体实施方式，但本发明的保护范围并不局限于此，任何熟悉本技术领域的技术人员在本发明公开的技术范围内，可轻易想到的变化或者替换，都应涵盖在本发明的保护范围内。

Claims (4)

1.即时定位与地图创建勘测搜救机器人，其特征在于：其包括越障底盘，车身平台，三维扫描模块，主控单元，车身外壳和驱动模块；其中，越障底盘由主体框架和行走轮腿组成，使车体具有对不同地形的被动自适应性；三维扫描模块能够获得被勘测空间的三维地图信息以及可能生命体的数据，所述三维扫描模块包括有三维扫描机构、激光扫描雷达、红外温度传感器、激光惯性传感器和固定支架；三维扫描机构由二个完全一样且同步动作的无急回特性的铰链四杆机构组成，铰链四杆机构中的曲柄匀速转动时，二个同步动作的铰链四杆机构可实现其中的托架以同一个平均速度往复摆动，实现激光三维扫描；激光扫描雷达正下方对应扫描范围内，均匀扇形分布若干个红外温度传感器；越障底盘的左右两侧分别安装一个前轮腿和二个后轮腿，各轮腿均采用直角梯形的四杆机构悬架，每个悬架在直角边和长边杆件间的铰链处和短边与斜边杆件间的铰链处之间安装了一个空气减震器；每个悬架斜边杆件的末端都装有一个车轮。

2.根据权利要求1所述的机器人，所述主控单元为平板电脑，紧密镶嵌于车身外壳的顶部。

3.根据权利要求1所述的机器人，所述越障底盘、三维扫描模块的三维扫描机构均采用高强度铝型材，车身平台采用不锈钢板，车身外壳采用ABS工程塑料。

4.一种应用如权利要求1所述的即时定位与地图创建勘测搜救机器人进行地形勘测和可能生命体搜救的方法，其特征在于包括以下步骤：

(1)传感器数据采集：即时定位与地图创建勘测搜救机器人在狭长空间中勘测或搜救时，在低速匀速行进的过程中，三维扫描机构带动激光扫描雷达和红外温度传感器上下往复摆动来探测周围空间的数据；

(2)可能生命体数据匹配：主控单元筛选每个红外温度传感器的温度数据，判定是否为可能生命体；若是则将对应的激光点云数据标记为可能生命体；

(3)激光大地坐标系下三维地图创建：根据激光惯性传感器获得的激光扫描雷达当前时刻的俯仰角、滚动角和航向角，将步骤(1)和(2)得到的数据转换到以激光扫描雷达光源为原点的激光相对坐标系中，然后再转换到激光大地坐标系中，从而创建出激光大地坐标系下的三维地图；

(4)大地坐标系下三维地图叠加：根据车体惯性传感器获得的所述机器人当前时刻的俯仰角、滚动角和航向角，将激光大地坐标系下的三维地图叠加到大地坐标系中。

(5)导航路径规划及前轮转向控制：越障底盘的运动模型简化为一个二轮运动模型，主控单元根据创建的大地坐标系三维地图横截面形心连线的曲率在越障底盘质心所在平面的投影为预期路线，遵循前后轮同一速度瞬心的原则，进行导航路径规划，并由主控单元发出指令，控制两前轮做出相应动作，实现自主导航；

(6)地图存储及无线传输：主控单元将叠加形成的大地坐标系下的三维地图，即时定位与地图创建勘测搜救机器人所有传感器的数据，以及机器人自主导航的路线及其实际行走路线存贮到数据库中，方便操作人员进行数据管理和离线地图生成；同时通过无线传输将上述信息发送给封闭空间外的接收端并实时显示。