一种未知环境中基于BundleAdjustment的双目相机目标定位方法

一种未知环境中基于bundle adjustment的双目相机目标定位方法
技术领域
1.本发明涉及目标定位技术领域，尤其是一种未知环境中基于bundle adjustment的双目相机目标定位方法。


背景技术：

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


技术实现要素：

4.本发明的技术解决问题是：克服现有技术的不足，提出一种未知环境中基于bundle adjustment的双目相机目标定位方法，该方法充分利用无人设备的优势，使得机器人可以在无gps的未知环境中快速执行搜索、救援等任务，无人机通过自身搭载的传感器进行位姿估计、感知周围环境。在定位目标时，不需要激光测距仪等其他额外设备。为了避免单帧估计可能产生误差较大情况，优化多帧目标定位估计结果。
5.为实现上述目的，本发明所提供的技术方案为：
6.一种未知环境中基于bundle adjustment的双目相机目标定位方法，包括以下步骤：
7.步骤一，由无人机获取机载传感器数据，包括双目图像信息和imu信息，然后根据获取的传感器数据构建视觉惯性里程计，最后根据构建的视觉惯性里程计获取无人机自身的位姿信息；
8.步骤二，对步骤一中传感器数据中的双目图像信息进行目标检测，得到检测框，以检测框的形心作为目标的位置，并对目标的位置进行立体测距；
9.步骤三，根据投影方程得到步骤二中确定的目标的位置在相机坐标系下的坐标，将步骤一获取的无人机自身的位姿信息与双目图像信息进行时间同步，得到包含目标图像帧的位姿信息，并通过得到的包含目标图像帧的位姿信息将目标在相机坐标系下的坐标转换为世界坐标系下的坐标，得到未知环境中双目相机的单帧估计的目标坐标值；
10.步骤四，根据bundle adjustment优化多帧估计结果，得到多帧优化的目标坐标值。
11.所述的步骤一中，无人机上搭载有双目相机和imu，通过搭载的双目相机获取双目图像信息，通过imu获取imu信息；
12.构建视觉惯性里程计的方法为：提取双目图像信息的harris角点，构建图像金字塔，利用光流跟踪构建起相邻图像帧之间的数据关联，利用ransac去除其中的误匹配，同时计算imu数据的预积分增量，双目图像信息和imu信息采用紧耦合的方式估计位姿，构建滑动窗口，采取非线性优化提高位姿估计的精度，在gps精度不高，或者无gps的未知环境中对无人机的自身姿态进行位姿估计，以获取双目相机及无人机自身的位姿信息，并保留飞行过程的轨迹信息，用于后续的位姿变换；
13.所述的步骤二中，无人机接收机载双目相机图像信息进行目标检测，具体方法为：以二维码作为需要识别的对象，检测步骤包括：对传感器采集的双目图像信息进行平滑滤波和二值化预处理后提取图像轮廓，选取有两个子轮廓特征的轮廓，从选取的轮廓中选择面积相近的三个来作为二维码的定位角点；根据二维码的定位角点的位置，对图像进行透视矫正，确定二维码四个顶点的坐标，并根据二维码四个顶点的坐标得到检测框，计算检测框的形心坐标作为目标的位置，对目标的位置进行立体测距获取目标的深度值s；
14.所述的步骤三中，根据投影方程确定目标的位置在相机坐标系下的坐标的方法为：当无人机检测到某时刻的两个相机图像都含有任务目标时，获取目标在像素坐标系下的坐标p＝(u，v，1)
t
，通过事先标定好的相机内参矩阵k，将像素坐标转换为归一化坐标系下的归一化坐标p1＝(x，y，1)
t
，再根据目标的深度值s得到目标位于相机坐标系下的坐标；
15.无人机自身的位姿信息与双目图像信息进行时间同步的方法为：将相机的位姿估计时间戳与含有任务目标的两个相机图像的时间戳进行时间同步，以获取在该时刻下相机的位姿信息，得到该时刻相机位姿与世界坐标系的关系，通过位姿变换得到目标在世界坐标系下的坐标，在搜索过程中所有的目标坐标都统一于世界坐标系下；
16.所述的步骤四中，考虑到单帧估计的不确定性，通过最小化重投影误差的方式，优化多帧估计结果，得到目标的最优估计。
17.有益效果
18.(1)相比于基于uwb定位技术需要极高的硬件成本和基于wi-fi的定位技术的鲁棒性差、定位精度低，本发明的方法充分发挥了无人设备的优势，利用无人机机载设备进行定位，不需要提前部署环境，可以应对突发状况。
19.(2)本发明的方法利用无人机的机动性和广阔的视野对目标进行检测并感知周围环境，在无gps的情况下可以估计自身位姿并定位目标，采用双目立体测距估计目标的深度值，不需要激光测距仪等额外设备。
20.(3)本发明的方法采用多帧定位的方式，避免了单帧估计出现的误差较大的情况，通过最小化重投影误差得到目标的最优估计。
21.(4)地面机器人在完成定位后，通过本发明的方法可以快速到达指定目标点执行救援任务。在多机器人执行多任务的情况下，减少了机器人探索环境所耗费的时间，使得机器人可以快速确定各自的目标位置。
22.(5)本发明的方法首先由无人机获取机载传感器数据，包括环境的双目图像信息和无人机的imu信息，构建视觉惯性里程计来获取无人机自身的位姿信息。对视觉图像进行目标检测，以检测框的形心作为位置点，对目标的双目图像进行立体测距。
23.(6)本发明的方法根据投影方程得到目标在相机坐标系下的坐标，同步无人机位姿和图像的时间信息，得到检测到目标的图像帧的位姿信息，通过位姿变换将目标在相机坐标系下的坐标转换为世界坐标系下的坐标。
24.(7)本发明的方法考虑到单帧估计的不确定性，根据bundle adjustment优化多帧估计结果，得到目标坐标的最优估计。解决了在无gps的未知环境下多机器人执行不同任务时重复探索造成的时间代价大和单帧估计误差较大等问题。
25.(8)本发明的方法充分发挥无人设备的优势，在减少额外负载的情况下实现对目标的精确定位，使得多个地面机器人可以统一坐标系，快速的到达各自目标点并执行救援任务。
附图说明
26.图1为双目相机参数模型示意图；
27.图2为相机成像模型示意图；
28.图3为目标在多帧中出现示意图；
29.图4为估计的定位结果与真实值的比较。
具体实施方式
30.结合附图及具体实例对本发明做详细说明，如下：
31.一种未知环境中基于bundle adjustment的双目相机目标定位方法，包括以下步骤：
32.步骤一、无人机获取传感器数据，构建视觉惯性里程计获取无人机的位姿信息；
33.无人机搭载相机、imu等传感器获取环境信息，提取图像的harris角点，构建图像金字塔，利用lk光流跟踪构建起相邻图像帧之间的数据关联，利用ransac去除其中的误匹配，由于双目相机可直接测得角点的深度值，因此可以直接求解pnp问题计算帧间位姿，同时计算imu数据的预积分增量，图像信息和imu信息采用紧耦合的方式估计帧间位姿，后端采用滑动窗口，构建视觉重投影误差，之前估计的累计误差和imu误差的最小二乘问题，采取非线性优化提高位姿估计的精度。在gps精度不高，或者无gps的未知环境中对无人机的自身姿态进行位姿估计，以获取相机及机体位姿信息，并保留飞行过程的轨迹信息，用于后
续的位姿矩阵变换。
34.步骤二、对图像进行目标检测，定位目标，通过双目立体测距获得目标的深度值；
35.接收相机图像信息进行目标检测，本发明中采用二维码作为需要识别的对象，二维码的识别可分为以下步骤：对传感器采集的图像进行平滑滤波，二值化等预处理，提取图像轮廓，选取有两个子轮廓的轮廓特征，从筛选的轮廓种选择面积相近的三个来作为二维码的定位角点；根据三个角点的位置，对图像进行透视矫正，确定二维码四个顶点的坐标，当在两个相机的当前帧都检测到所需的目标信息时，以检测框的形心坐标作为目标在像素坐标系下的坐标。然后通过双目立体测距获取目标的深度值s。双目相机的参数模型如图1，其中o1o2代表了两个相机的光心，两个光心之间的距离为焦距b，c1c2代表了两个相机的物理成像平面，光心到成像平面之间的距离为焦距f。对于标定好的双目相机，c1c2处于同一平面上，三维点p在两个相机坐标系下的坐标分别为(x1,y1,z),(x2,y2,z)，三维点p的投影点p`在两个相机成像平面上的成像点为p｀1(x1,y1),p｀2(x2,y2)，三维点p的水平视角和垂直视角分别为ω1,ω2,根据几何关系，z满足以下关系，其中tanω1＝-x1/f,tanω2＝-x2/f，深度值s＝z。
[0036][0037]
步骤三、同步图像和位姿时间戳，得到该时刻的无人机位姿信息，通过矩阵变换和投影方程得到目标在世界坐标系下的信息；
[0038]
当无人机检测到某时刻的两个相机图像都含有任务目标时，会开始计算目标点的坐标。环境中的三维点映射到图像坐标系中的二维点是通过投影方程实现的，如图2，假设三维点p在世界坐标系下的坐标为pw，图像上的投影点为p＝(x,y)
t
，经过步骤一计算得到的位姿信息r
cw
,t
cw
，可以将pw通过pc＝r
cw
pw t
cw
转换到相机坐标系下的坐标pc＝(x,y,z)
t
。pc通过光心o投影到物理成像平面上，满足z/f＝-x/x＝-y/y。考虑到针孔的图像是倒置的图像，而真实的相机图像会自动反转，可以将物理成像平面投影到光学中心的前部，去除公式中的负号，物理成像平面的原点与生成图像的像素平面的原点不一致，经过考虑缩放和平移，得到像素坐标系p
uv
，其中p
uv
＝1/z*k*pc，k代表了相机的内参矩阵，一般需要事先标定好。因此图像中的目标坐标p
uv
可以通过如下关系转换为世界坐标系下的坐标pw：
[0039][0040]
步骤四、考虑到单帧估计的不确定性，根据bundle adjustment优化多帧估计结果，得到目标坐标的最优估计。
[0041]
步骤三中的估计结果是根据单帧图像估计的，可能存在误差较大的情况，为了减少误差，使用最小化重投影误差的方式解决。无人机在感知环境的过程中，目标可以出现在多帧相机图像中，如图3所示。每一帧都满足世界坐标系下的三维点变换到像素坐标系下二维点的投影方程，得到一个关于三维点的方程组。
[0042]
k(r1pw t1)＝z1p1[0043]
k(r2pw t2)＝z2p2[0044][0045]
k(rkpw tk)＝zkpk[0046]
考虑到误差的存在，不可能严格满足所有投影方程，因此通过最小二乘法对所有投影方程进行优化，优化方程如下。
[0047][0048]ei
＝zip
i-k(ripw ti)
[0049]
由于残差函数的非线性，无法直接求解，对于不便于直接求解的最小二乘问题，可以采用迭代的方式，从初始值出发，逐步找到最优值使得损失函数最小。在这里采用lm算法求解。优化函数中是以最小化误差平方和来作为损失函数的，如果某个残差项给出的数据是偏差较大的，而算法并不能辨别这个数据，当调整这个数据的相关变量使得损失函数下降较快时，那么算法会专注于调整这个数据，出于这个原因，有了鲁棒核函数的存在，会将原误差函数的二范数度量替换成一个增长没有那么快的函数。我们采用huber核函数，最优优化方程如下：
[0050][0051][0052]
在仿真环境中测试了该算法，使用vin-fusion作为slam解决方案，为无人机提供姿态，二维码作为未知环境中检测的目标，二维码检测帧的中心作为测距点。在解决bundle adjustment问题时，我们使用了ceres-solver求解器。最终定位结果如图4所示。