一种未知救援环境中基于VI-SLAM的目标定位方法

一种未知救援环境中基于vi-slam的目标定位方法
技术领域
1.本发明属于目标定位的技术领域，具体涉及一种未知救援环境中基于vi-slam(visual-inertial simultaneous localization and mapping)的目标定位方法。


背景技术：

2.随着无人设备的发展和相关技术的智能化，无人机和各类其他机器人被广泛应用于灾难场景下的搜索、救援等活动。无人机具有机动性强、视野开阔等特点，地面机器人具有载重量大、稳定等特点，两者之间的协同配合能充分发挥各自的优势，使得在灾难场景下可以快速实施搜索、救援等任务，同时降低了工作人员在危险区域执行任务的安全风险。然而在室内、野外等灾难环境中，gps及其他有源定位技术精度较差甚至失效，导致无人机与地面机器人无法获取自身位姿信息，执行任务受阻。
3.同步定位与建图(slam)技术使得机器人在未知环境中的定位成为可能。机器人利用搭载的imu、雷达、相机等传感器，记录自身的位姿，并对周围环境进行感知建图。根据传感器的类型不同，slam可以分为雷达slam和视觉slam，由于雷达设备价格昂贵，成本较小的相机往往作为机器人进行位姿估计的载体。而纯视觉slam在快速运动等场景下容易出现跟踪丢失，价格低廉的imu可以弥补相机在快速运动场景下跟踪丢失的情况。视觉与imu相互融合的slam精度较高，可以达到自身定位的需求。但在定位目标测量深度时，深度相机探测距离较短，而激光测距仪价格昂贵、且会额外无人机的负载。同时地面机器人在未知环境中执行搜索任务时，往往耗时较长，尤其是多机器人执行多任务的情况，对环境中的同一位置可能会执行多次搜索，造成资源上的浪费，而多机器人协同建图的效果又难以达到导航的需求。如果地面机器人在面对不属于自己执行的任务时，由于机器人各自地图坐标系不统一，就无法将任务位置发送给执行该任务的机器人，那么在最差的情况下，每个机器人在找到自己的目标时都要遍历一次任务区域，使得执行任务的时间代价增加。
4.因此，如何充分发挥无人设备的优势，在减少额外负载的情况下实现对目标的精确定位，使得多个地面机器人可以统一坐标系，快速的到达各自目标点并执行救援任务成为在该场景下亟待解决的问题。


技术实现要素：

5.有鉴于此，本发明提供了一种未知救援环境中基于vi-slam的目标定位方法，能够充分利用无人设备的优势，使得机器人可以在无gps的未知环境中快速执行搜索、救援等任务。
6.实现本发明的技术方案如下：
7.一种未知救援环境中基于vi-slam的目标定位方法，包括以下步骤：
8.步骤一：获取无人机的传感器数据，包括双目图像信息、深度图像信息和imu信息，利用双目图像信息和imu信息构建视觉惯性里程计获取无人机的位姿信息；
9.步骤二：在得到无人机的位姿信息之后，对双目图像进行目标检测，定位目标，通
过双目立体测距及投影方程得到目标在相机坐标系下的坐标；
10.步骤三：同步图像和位姿的时间戳，得到当前时刻的无人机位姿信息，将当前时刻的无人机位姿信息和相机坐标系下的坐标进行矩阵变换得到目标在世界坐标系下的位置信息；
11.步骤四：通过深度图像信息和无人机的位姿信息构建八叉树地图，将八叉树地图和目标在世界坐标系下的位置信息传递给地面机器人，用作自主导航。
12.进一步地，在步骤一中，无人机搭载包括相机、imu的传感器获取环境信息，提取图像的harris角点，构建图像金字塔，利用光流跟踪构建起相邻图像帧之间的数据关联，利用ransac去除其中的误匹配，同时计算imu数据的预积分增量，图像信息和imu信息采用紧耦合的方式估计位姿，构建滑动窗口，采取非线性优化提高位姿估计的精度；在gps精度不高，或者无gps的未知环境中对无人机的自身姿态进行位姿估计。
13.进一步地，在步骤二中，无人机利用机载相机所采集的双目图像信息进行目标检测，采用qr code(二维码)作为需要识别的对象，qr code的识别包括以下步骤：对传感器采集的图像进行包括平滑滤波、二值化的预处理，提取图像轮廓，选取有两个子轮廓的轮廓特征，从筛选的轮廓中选择面积相近的三个来作为qr code的定位角点；根据三个角点的位置，对图像进行透视矫正，确定qr code四个顶点的坐标，并计算中心坐标作为目标位置。
14.进一步地，定位目标之后，获取目标的像素坐标p＝(u,v,1)
t
，通过事先标定好的相机内参矩阵k，将p转换为归一化坐标系下的归一化坐标p1＝(x,y,1)
t
，其中p1＝k-1
p；通过双目立体测距获取目标的深度值s，得到目标位于相机坐标系下的坐标p＝(x,y,z)
t
，其中p＝sp1。
15.进一步地，在步骤三中，当无人机检测到某时刻的两个相机图像都含有任务目标时，同步相机的位姿估计时间戳与该两个相机图像的时间戳，以获取在该时刻下相机的位姿信息，得到该时刻相机位姿与世界坐标系的关系，即该时刻相机相较于世界坐标系下的旋转矩阵r和平移向量t；通过pw＝r-1
(p-t)；得到目标在世界坐标系下的坐标pw，在搜索过程中所有的目标坐标都统一于世界坐标系下。
16.进一步地，在步骤四中，通过无人机的机载传感器所获取的深度图像和步骤一所计算得到无人机的位姿信息来构建环境八叉树地图；首先将深度信息转换为点云信息，然后根据里程计内的位姿关系进行点云拼接，使用octomap将点云地图转换为八叉树地图，由于八叉树地图可以进行避障检测，因此可以用作自主导航，根据步骤三所定位的目标坐标与所建地图属于同一坐标系，多个地面机器人可以共享地图，在实现基于地图的定位之后，快速到达指定目标点执行救援任务，减少搜索时间。
17.有益效果：
18.相比于基于uwb定位技术需要极高的硬件成本和基于wi-fi的定位技术的鲁棒性差、定位精度低，本发明可以充分发挥无人设备的优势，利用无人机机载设备进行定位，不需要提前部署环境，可以应对突发状况。同时利用无人机的机动性和广阔的视野对目标进行检测并感知周围环境，在无gps的情况下可以估计自身位姿并定位目标，采用双目立体测距估计目标的深度值，不需要激光测距仪等额外设备。同时建立环境的描述地图，目标的坐标是统一于所建环境地图的坐标系，将目标位置和环境地图发送给地面机器人，多个地面机器人可以共享地图，在实现基于地图的定位之后，快速到达指定目标点执行救援任务。在
多机器人执行多任务的情况下，减少了机器人探索环境所耗费的时间，使得机器人可以快速确定各自的目标位置，地图的存在使得机器人可以快速进行避障检测和路径规划，确定自己的移动轨迹，执行救援任务。
附图说明
19.图1为本发明未知环境中空地协同流程图；
20.图2为本发明实施例仿真环境俯视图；
21.图3为本发明实施例仿真环境主视图；
22.图4为本发明目标的深度值s计算示意图；
23.图5为本发明实施例估计的定位结果与真实值xy方向上的比较；
24.图6为本发明实施例估计的定位结果与真实值z方向上的比较；
25.图7为本发明实施例构建的八叉树地图。
具体实施方式
26.下面结合附图并举实施例，对本发明进行详细描述。
27.本发明提出了一种未知救援环境中基于vi-slam的目标定位方法，系统的整体框架如图1所示。
28.步骤一、无人机获取传感器数据，构建视觉惯性里程计获取无人机的位姿信息。
29.无人机搭载相机、imu等传感器获取环境信息，提取图像的harris角点，构建图像金字塔，利用lk光流跟踪构建起相邻图像帧之间的数据关联，利用ransac去除其中的误匹配，由于双目相机可直接测得角点的深度值，因此可以直接求解pnp问题计算帧间位姿，同时计算imu数据的预积分增量，图像信息和imu信息采用紧耦合的方式估计帧间位姿，后端采用滑动窗口，构建视觉重投影误差，之前估计的累计误差和imu误差的最小二乘问题，采取非线性优化提高位姿估计的精度。在gps精度不高，或者无gps的未知环境中对无人机的自身姿态进行位姿估计，以获取相机及机体位姿信息，并保留飞行过程的轨迹信息，用于后续的矩阵变换。
30.步骤二、对图像进行目标检测，定位目标，通过双目立体测距及投影方程得到目标在相机坐标系下的坐标。
31.接收相机图像信息进行目标检测，本发明中采用qr code作为需要识别的对象，在仿真环境中放置了3个qr code作为需要检测的目标，如图2、图3。qr code的识别可分为以下步骤：对传感器采集的图像进行平滑滤波，二值化等预处理，提取图像轮廓，选取有两个子轮廓的轮廓特征，从筛选的轮廓中选择面积相近的三个来作为qr code的定位角点；根据三个角点的位置，对图像进行透视矫正，确定qr code四个顶点的坐标，当在两个相机的当前帧都检测到所需的目标信息时，计算中心坐标作为目标位置。定位目标之后获取目标的像素坐标p＝(u,v,1)
t
，通过事先标定好的相机内参矩阵k，将p转换为归一化坐标系下的归一化坐标p1＝(x,y,1)
t
,其中p1＝k-1
p。接下来通过双目立体测距获取目标的深度值s。s的计算由下面的公式得到。
32.如图4所示，对于标定好的双目相机，左右光心o1,o2与成像平面相互平行，相机基线为b，焦距为f,q1,q2是q点在两个成像平面上的投影。根据相似三角形的性质，有
化简得求解s之后，得到目标位于相机坐标系下的坐标p＝(x,y,z)
t
，其中p＝sp1。
33.步骤三、同步图像和位姿时间戳，得到该时刻的无人机位姿信息，通过矩阵变换得到目标在世界坐标系下的位置信息。
34.当无人机检测到某时刻的两个相机图像都含有任务目标时，同步相机的位姿估计时间戳与该图像的时间戳，以获取在该时刻下相机的位姿信息，得到该时刻相机位姿与世界坐标系的关系，及该时刻相机相较于世界坐标系下的旋转矩阵r和平移向量t。通过pw＝r-1
(p-t)。得到目标在世界坐标系下的坐标pw。将估计得到的世界坐标pw与仿真环境中的真值比较如图5、图6。
35.步骤四、通过深度图像和里程计信息(无人机的位姿信息)构建八叉树地图，将八叉树地图和目标在世界坐标系下的位置信息传递给地面机器人。
36.通过传感器的深度图像和计算得到里程计信息构建八叉树地图。首先将深度信息转换为点云信息，然后根据里程计内的位姿关系进行点云拼接，使用octomap将点云地图转换为八叉树地图，如图7。八叉树是一种树状数据结构，每个节点表示一个正方体的体积元素，每个体素由0-1之间的浮点数表示节点被占据的概率。由于八叉树地图可以进行避障检测，因此可以用作地面机器人的自主导航。步骤三定位的目标坐标与所建地图属于同一坐标系，多个地面机器人可以共享地图，在实现基于地图的定位之后，快速到达指定目标点执行救援任务，减少搜索时间。
37.综上所述，以上仅为本发明的较佳实施例而已，并非用于限定本发明的保护范围。凡在本发明的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。