CN108489454A - 深度距离测量方法、装置、计算机可读存储介质及电子设备 - Google Patents

Abstract

本公开涉及一种深度距离测量方法、装置、计算机可读存储介质及电子设备，属于无人机领域，能够准确地实时测量无人机与周围环境之间的深度距离，有利于无人机的实时避障。该深度距离测量方法包括：接收无人机的姿态数据以及双目相机采集到的无人机周围环境的左目图像和右目图像；基于左、右目图像，获取无人机周围环境的视差值图像；基于视差值图像和双目相机的标定参数，将无人机周围环境在图像坐标系下的坐标转换成相机坐标系下的坐标；基于双目相机在所述无人机上的安装位置和姿态数据，将无人机周围环境在相机坐标系下的坐标转换成世界坐标系下的坐标；基于无人机周围环境在世界坐标系下的坐标，确定无人机周围环境与无人机之间的深度距离。

Description

深度距离测量方法、装置、计算机可读存储介质及电子设备

技术领域

本公开涉及无人机领域，具体地，涉及一种深度距离测量方法、装置、计算机可读存储介质及电子设备。

背景技术

通常，无人机上都会安装双目相机进行测距以实现无人机的避障目的。在测距时，会利用双目相机的两个摄像头采集无人机周围环境的信息，得到第一视频图像信息和第二视频图像信息，然后分别从第一视频图像信息中选取第一图像以及从第二视频图像信息中选取同一时刻的第二图像，并依据第一图像和第二图像得到无人机与周围环境之间的深度距离。然而，这样测得的深度距离并不准确。以安装在无人机机架前方的前向双目相机为例，若无人机按照一定的俯仰角向前飞行，则前向双目相机也有一定的俯仰角，因此，在有俯仰角的状态下，前向双目相机测到的无人机前方的深度距离是倾斜向前的，这样测得的深度距离会比实际的深度距离要大，不利于无人机实时避障。

发明内容

本公开的目的是提供一种深度距离测量方法、装置、计算机可读存储介质及电子设备，能够准确地实时测量无人机与周围环境之间的深度距离，有利于无人机的实时避障。

为了实现上述目的，本公开提供一种深度距离测量方法，该方法包括：

接收无人机的姿态数据以及双目相机采集到的无人机周围环境的左目图像和右目图像；

基于所述左目图像和所述右目图像，获取所述无人机周围环境的视差值图像；

基于所述视差值图像和所述双目相机的标定参数，将所述无人机周围环境在图像坐标系下的坐标转换成相机坐标系下的坐标；

基于所述双目相机在所述无人机上的安装位置和所述姿态数据，将所述无人机周围环境在所述相机坐标系下的坐标转换成世界坐标系下的坐标；

基于所述无人机周围环境在所述世界坐标系下的坐标，确定所述无人机周围环境与所述无人机之间的深度距离。

可选地，所述基于所述左目图像和所述右目图像，获取所述无人机周围环境的视差值图像，包括：

对所述左目图像和所述右目图像进行校正，得到图像像素行对准的左目校正图像和右目校正图像；

对所述左目校正图像和所述右目校正图像进行计算得到所述视差值图像。

可选地，所述基于所述视差值图像和所述双目相机的标定参数，将所述无人机周围环境在图像坐标系下的坐标转换成相机坐标系下的坐标，通过以下公式来实现：

其中，(X_c,Y_c,Z_c,W)为所述相机坐标系下的齐次坐标，W是所述齐次坐标的一个分量，x、y是所述图像坐标系下的像素坐标，d是与所述图像坐标系下的像素坐标相对应的视差值，c_x、c_y、f、T是所述双目相机的标定参数， c_x和c_y表示所述双目相机的主光轴与图像平面交点的像素坐标，f表示焦距， T表示所述双目相机的两个镜头的投影中心的连线的距离。

可选地，对于安装在所述无人机机架前方的前向双目相机而言，所述基于所述双目相机在所述无人机上的安装位置和所述姿态数据，将所述无人机周围环境在所述相机坐标系下的坐标转换成世界坐标系下的坐标，通过以下公式来实现：

P_w(X_w,Y_w,Z_w)＝R^-1*P_c(X_c,Y_c,Z_c)

R＝R_x*R_y*R_z

其中，P_w(X_w,Y_w,Z_w)是所述世界坐标系下的坐标，P_c(X_c,Y_c,Z_c)是所述相机坐标系下的坐标，R_x是绕所述世界坐标系的X_w轴的旋转矩阵，R_y是绕所述世界坐标系的Y_w轴的旋转矩阵，R_z是绕所述世界坐标系的Z_w轴的旋转矩阵，ψ是横滚角，θ是俯仰角，是航向角。

可选地，对于安装在所述无人机机架后方的后向双目相机而言，所述基于所述双目相机在所述无人机上的安装位置和所述姿态数据，将所述无人机周围环境在所述相机坐标系下的坐标转换成世界坐标系下的坐标，通过以下公式来实现：

P_w(X_w,Y_w,Z_w)＝R^-1*P_c(X_c,Y_c,Z_c)

R＝R_x*R_y*R_z

其中，P_w(X_w,Y_w,Z_w)是所述世界坐标系下的坐标，P_c(X_c,Y_c,Z_c)是所述相机坐标系下的坐标，R_x是绕所述世界坐标系的X_w轴的旋转矩阵，R_y是绕所述世界坐标系的Y_w轴的旋转矩阵，R_z是绕所述世界坐标系的Z_w轴的旋转矩阵，ψ是横滚角，θ是俯仰角，是航向角。

可选地，对于安装在所述无人机机架下方的下向双目相机而言，所述基于所述双目相机在所述无人机上的安装位置和所述姿态数据，将所述无人机周围环境在所述相机坐标系下的坐标转换成世界坐标系下的坐标，通过以下公式来实现：

P_w(X_w,Y_w,Z_w)＝R^-1*P_c(X_c,Y_c,Z_c)

R＝R_x*R_y*R_z

其中，P_w(X_w,Y_w,Z_w)是所述世界坐标系下的坐标，P_c(X_c,Y_c,Z_c)是所述相机坐标系下的坐标，R_x是绕所述世界坐标系的X_w轴的旋转矩阵，R_y是绕所述世界坐标系的Y_w轴的旋转矩阵，R_z是绕所述世界坐标系的Z_w轴的旋转矩阵，ψ是横滚角，θ是俯仰角，是航向角。

可选地，对于安装在所述无人机机架前方的前向双目相机或者安装在所述无人机机架后方的后向双目相机而言，所述基于所述无人机周围环境在所述世界坐标系下的坐标，确定所述无人机周围环境与所述无人机之间的深度距离，包括：

将所述航向角设置为零，则所述世界坐标系中的X_w坐标值被确定为所述无人机周围环境与所述无人机之间的深度距离；

过滤所述无人机周围环境与所述无人机之间的深度距离中位于地平线以下的深度距离，得到位于地平线以上的深度距离信息。

可选地，对于所述下向双目相机而言，所述基于所述无人机周围环境在所述世界坐标系下的坐标，确定所述无人机周围环境与所述无人机之间的深度距离，通过以下公式来实现：

其中，P_zw是所述无人机周围环境与所述无人机之间的下向深度距离，夹角α和β是所述无人机周围环境中的点P和光心连线与光轴的夹角，P_zc是所述无人机周围环境中的点P在下向相机坐标系的Z轴中的坐标。

根据本公开的又一实施例，提供一种深度距离测量装置，该装置包括：

接收模块，用于接收无人机的姿态数据以及双目相机采集到的无人机周围环境的左目图像和右目图像；

获取模块，用于基于所述左目图像和所述右目图像，获取所述无人机周围环境的视差值图像；

第一转换模块，用于基于所述视差值图像和所述双目相机的标定参数，将所述无人机周围环境在图像坐标系下的坐标转换成相机坐标系下的坐标；

第二转换模块，用于基于所述双目相机在所述无人机上的安装位置和所述姿态数据，将所述无人机周围环境在所述相机坐标系下的坐标转换成世界坐标系下的坐标；

确定模块，用于基于所述无人机周围环境在所述世界坐标系下的坐标，确定所述无人机周围环境与所述无人机之间的深度距离。

可选地，所述获取模块包括：校正子模块，用于对所述左目图像和所述右目图像进行校正，得到图像像素行对准的左目校正图像和右目校正图像；计算子模块，用于对所述左目校正图像和所述右目校正图像进行计算得到所述视差值图像。

可选地，所述第一转换模块通过以下公式来基于所述视差值图像和所述双目相机的标定参数，将所述无人机周围环境在图像坐标系下的坐标转换成相机坐标系下的坐标：

其中，(X_c,Y_c,Z_c,W)为所述相机坐标系下的齐次坐标，W是所述齐次坐标的一个分量，x、y是所述图像坐标系下的像素坐标，d是与所述图像坐标系下的像素坐标相对应的视差值，c_x、c_y、f、T是所述双目相机的标定参数， c_x和c_y表示所述双目相机的主光轴与图像平面交点的像素坐标，f表示焦距， T表示所述双目相机的两个镜头的投影中心的连线的距离。

可选地，对于安装在所述无人机机架前方的前向双目相机而言，所述第二转换模块通过以下公式来基于所述双目相机在所述无人机上的安装位置和所述姿态数据，将所述无人机周围环境在所述相机坐标系下的坐标转换成世界坐标系下的坐标：

P_w(X_w,Y_w,Z_w)＝R^-1*P_c(X_c,Y_c,Z_c)

R＝R_x*R_y*R_z

其中，P_w(X_w,Y_w,Z_w)是所述世界坐标系下的坐标，P_c(X_c,Y_c,Z_c)是所述相机坐标系下的坐标，R_x是绕所述世界坐标系的X_w轴的旋转矩阵，R_y是绕所述世界坐标系的Y_w轴的旋转矩阵，R_z是绕所述世界坐标系的Z_w轴的旋转矩阵，ψ是横滚角，θ是俯仰角，是航向角。

可选地，对于安装在所述无人机机架后方的后向双目相机而言，所述第二转换模块通过以下公式来基于所述双目相机在所述无人机上的安装位置和所述姿态数据，将所述无人机周围环境在所述相机坐标系下的坐标转换成世界坐标系下的坐标：

P_w(X_w,Y_w,Z_w)＝R^-1*P_c(X_c,Y_c,Z_c)

R＝R_x*R_y*R_z

其中，P_w(X_w,Y_w,Z_w)是所述世界坐标系下的坐标，P_c(X_c,Y_c,Z_c)是所述相机坐标系下的坐标，R_x是绕所述世界坐标系的X_w轴的旋转矩阵，R_y是绕所述世界坐标系的Y_w轴的旋转矩阵，R_z是绕所述世界坐标系的Z_w轴的旋转矩阵，ψ是横滚角，θ是俯仰角，是航向角。

可选地，对于安装在所述无人机机架下方的下向双目相机而言，所述第二转换模块通过以下公式来基于所述双目相机在所述无人机上的安装位置和所述姿态数据，将所述无人机周围环境在所述相机坐标系下的坐标转换成世界坐标系下的坐标：

P_w(X_w,Y_w,Z_w)＝R^-1*P_c(X_c,Y_c,Z_c)

R＝R_x*R_y*R_z

其中，P_w(X_w,Y_w,Z_w)是所述世界坐标系下的坐标，P_c(X_c,Y_c,Z_c)是所述相机坐标系下的坐标，R_x是绕所述世界坐标系的X_w轴的旋转矩阵，R_y是绕所述世界坐标系的Y_w轴的旋转矩阵，R_z是绕所述世界坐标系的Z_w轴的旋转矩阵，ψ是横滚角，θ是俯仰角，是航向角。

可选地，对于安装在所述无人机机架前方的前向双目相机或者安装在所述无人机机架后方的后向双目相机而言，所述确定模块包括：

确定子模块，用于将所述航向角设置为零，则所述世界坐标系中的X_w坐标值被确定为所述无人机周围环境与所述无人机之间的深度距离；

过滤子模块，用于过滤所述无人机周围环境与所述无人机之间的深度距离中位于地平线以下的深度距离，得到位于地平线以上的深度距离信息。

可选地，对于所述下向双目相机而言，所述确定模块通过以下公式来基于所述无人机周围环境在所述世界坐标系下的坐标，确定所述无人机周围环境与所述无人机之间的深度距离：

其中，P_zw是所述无人机周围环境与所述无人机之间的下向深度距离，夹角α和β是所述无人机周围环境中的点P和光心连线与光轴的夹角，P_zc是所述无人机周围环境中的点P在下向相机坐标系的Z轴中的坐标。

根据本公开的又一实施例，提供一种计算机可读存储介质，其上存储有计算机程序，该程序被处理器执行时实现上述方法的步骤。

根据本公开的又一实施例，提供一种电子设备，包括：

上述的计算机可读存储介质；以及

一个或者多个处理器，用于执行所述计算机可读存储介质中的程序。

通过采用上述技术方案，由于在无人机测距过程中考虑了无人机的姿态数据，所以能够实现无人机的准确和实时测距，并进而能够确保无人机的准确避障。以安装在无人机机架前方的前向双目相机为例，若无人机按照一定的俯仰角向前飞行，则前向双目相机也有一定的俯仰角，因此，在有俯仰角的状态下，根据本公开实施例的方法所测到的无人机前方的深度距离是水平向前的，这样测得的深度距离会非常接近实际的深度距离，进而有利于无人机实时避障。

本公开的其他特征和优点将在随后的具体实施方式部分予以详细说明。

附图说明

附图是用来提供对本公开的进一步理解，并且构成说明书的一部分，与下面的具体实施方式一起用于解释本公开，但并不构成对本公开的限制。在附图中：

图1是根据本公开一种实施例的深度距离测量方法的流程图。

图2是简单的双目立体成像原理图。

图3是从世界坐标系下的坐标向前向相机坐标系下的坐标的变换的示意图。

图4是在下向双目相机情况下无人机的姿态数据中的俯仰角θ和横滚角ψ对下向相机坐标系和世界坐标系的Z轴产生旋转的示意图。

图5是根据本公开一种实施例的深度距离测量装置的示意框图。

图6是根据本公开一种实施例的又一深度距离测量装置的示意框图。

具体实施方式

以下结合附图对本公开的具体实施方式进行详细说明。应当理解的是，此处所描述的具体实施方式仅用于说明和解释本公开，并不用于限制本公开。

根据本公开的一种实施例，提供一种深度距离测量方法，该方法可以应用于无人机中，能够在基于嵌入式实时双目视觉的无人机避障系统的三维环境信息重建算法中实现，以实现无人机的避障和测距等功能。如图1所示，该方法可以包括以下步骤：

在步骤S11中，接收无人机的姿态数据以及双目相机采集到的无人机周围环境的左目图像和右目图像。其中，无人机的姿态数据可以包括航向角、俯仰角、横滚角等。

在步骤S12中，基于所述左目图像和所述右目图像，获取所述无人机周围环境的视差值图像。

在步骤S13中，基于所述视差值图像和所述双目相机的标定参数，将所述无人机周围环境在图像坐标系下的坐标转换成相机坐标系下的坐标。

在步骤S14中，基于所述双目相机在所述无人机上的安装位置和所述姿态数据，将所述无人机周围环境在所述相机坐标系下的坐标转换成世界坐标系下的坐标。

在步骤S15中，基于所述无人机周围环境在所述世界坐标系下的坐标，确定所述无人机周围环境与所述无人机之间的深度距离。

通过采用上述技术方案，由于在无人机测距过程中考虑了无人机的姿态数据，所以能够实现无人机的准确和实时测距，并进而能够确保无人机的准确避障。以安装在无人机机架前方的前向双目相机为例，若无人机按照一定的俯仰角向前飞行，则前向双目相机也有一定的俯仰角，因此，在有俯仰角的状态下，根据本公开实施例的方法所测到的无人机前方的深度距离是水平向前的，这样测得的深度距离会非常接近实际的深度距离，进而有利于无人机实时避障。

通常，为了更好的控制无人机的飞行，会在无人机机架的前方、后方和下方三个方向上分别安装三个双目相机，以便无人机能够实时地获得前、后和下三个方向上的周围环境深度距离信息，进而实现无人机的避障等功能。而且，由于双目相机在无人机机架上的安装位置不同，则坐标系之间的转换关系也不同，深度距离的计算方法也随之不同，因此下面按照前向双目相机、后向双目相机、下向双目相机三种情况来分别描述如何获取无人机周围环境的深度距离。另外，在本申请中，均是以无人机的机头方向作为前向方向。

第一种情况是前向双目相机情况下如何获取无人机周围环境的深度距离。具体包括以下步骤。

第一步，接收无人机的姿态数据以及前向双目相机采集到的无人机前方环境的左目图像和右目图像。

第二步，对前向双目相机采集到的左目图像和右目图像进行校正，例如依据前向双目相机的旋转矩阵、平移向量以及畸变系数等参数来对左目图像和右目图像进行畸变校正，得到图像像素行对准的左目校正图像和右目校正图像。

第三步，对左目校正图像和右目校正图像进行计算得到视差值图像。其中，视差就是左目校正和右目校正后的行对准图像对应行匹配特征点的水平坐标的差值。

第四步，基于所得到的视差值图像和前向双目相机的标定参数，将无人机前方环境在前向图像坐标系下的坐标转换成前向相机坐标系下的坐标。该步骤的具体实现流程如下所述。

根据第三步中获得的视差值图像、以及前向双目相机的标定参数，依据双目立体视觉的三维测量原理，将前向图像坐标系下的像素坐标转换成前向相机坐标系下的坐标，公式如下：

其中，(X_c,Y_c,Z_c,W)为所述相机坐标系下的齐次坐标，W是所述齐次坐标的一个分量，x、y是前向图像坐标系下的像素坐标，d是与前向图像坐标系下的像素坐标相对应的视差值，c_x、c_y、f、T是前向双目相机的标定参数。

具体地，双目立体视觉三维测量是基于视差原理，图2所示为简单的双目立体成像原理图。事实上，前向双目相机的成像平面在镜头的光心后，图 2中将左、右成像平面绘制在镜头的光心前f(其中，f是前向双目相机的焦距)处，前向双目相机的两个镜头的投影中心的连线的距离是基线距T，c_x和c_y是前向双目相机的主光轴与图像平面交点的像素坐标。前向相机坐标系的原点在前向双目相机的镜头的光心处，X_c轴方向为平行前向双目相机水平向右，Y_c轴方向为平行前向双目相机垂直向下，Z_c轴方向为前向双目相机的光轴方向，单位都是毫米(mm)，符合右手定则。前向图像坐标系在成像平面上，设定成像平面为上述矫正后的左、右行对准图像，原点是图像的左上角点，u轴和v轴分别和前向相机坐标系的X_c轴和Y_c轴方向一致，单位是像素。如图2所示，无人机前向环境中的某点P在左目校正图和右目校正图中相应的坐标分别为P_l(u^l,v^l)和P_r(u^r,v^r)。由于左目、右目图像是行对准的校正图像，则点P在左目、右目图像上的像素_y坐标相同，即v^l＝v^r。由三角几何关系可以得到点P在前向相机坐标系下的坐标P_c(X_c,Y_c,Z_c)：

其中，d＝u^l-u^r是视差值。因此，前向图像坐标系下的像素坐标P(u,v)到前向相机坐标系下的坐标P_c(X_c,Y_c,Z_c)的变换关系为：

X_c＝X/W,Y_c＝Y/W,Z_c＝Z/W (6)

第五步，基于前向双目相机在无人机上的安装位置和姿态数据，将无人机前向环境在前向相机坐标系下的坐标转换成世界坐标系下的坐标。该步骤的具体实现方式如下所述。

根据前向双目相机在无人机机架上的安装位置以及实时接收到的姿态数据，计算前向相机坐标系到世界坐标系的旋转变换矩阵，因为接收到的无人机姿态数据是相对于世界坐标系之间的关系，由航向角俯仰角θ和横滚角ψ组成。在通过求得的旋转变换矩阵，将前方环境在前向相机坐标系下的坐标转换成世界坐标系下的坐标，从而计算前方环境到无人机之间的深度距离。

其中，前方世界坐标系的X_w轴方向是指向北的方向、Y_w轴方向是指向东的方向，Z_w轴方向是垂直地平面指向地的方向，符合右手定则。航向角为无人机绕Z_w轴旋转，机头方向与X_w轴间的夹角，以机头右偏航为正；俯仰角θ为无人机绕Y_w轴旋转、机头方向与水平面(平面X_wOY_w)间的夹角，以无人机抬头为正；横滚角ψ为无人机绕X_w轴旋转，无人机的左右方向与水平面的垂直平面(平面X_wOZ_w)的夹角，以无人机向右倾斜为正。

由于前向双目相机是镜头朝向无人机的前方、与机头平行安装在机架上，所以，无人机的姿态数据也就是前向双目相机的姿态数据。由于姿态数据是相对世界坐标系各个坐标轴的旋转角度，因此，前方环境信息点从世界坐标系下的坐标P_w(X_w,Y_w,Z_w)变换到前向相机坐标系下的坐标P_c(X_c,Y_c,Z_c)，是先绕世界坐标系的Z_w轴旋转变换，再绕世界坐标系的Y_w轴旋转变换，最后绕世界坐标系的X_w轴旋转变换，如图3所示，其变换公式如下：

组合到一起就是：

P_c＝R_x*R_y*R_z*P_w＝R*P_w (10)

其中，R_x是绕世界坐标系的X_w轴的旋转矩阵，R_y是绕世界坐标系的Y_w轴的旋转矩阵，R_z是绕世界坐标系的Z_w轴的旋转矩阵。由此可知，从世界坐标系到前向相机坐标系下的旋转矩阵R＝R_x*R_y*R_z。所以，从前向相机坐标系到世界坐标系的旋转变换矩阵就是R逆矩阵R^-1。即前向环境信息点从前向相机坐标系下的坐标P_c到世界坐标系下的坐标P_w的变换公式为：

P_w＝R^-1*P_c (11)

第六步，基于第五步中得到的无人机前方环境在世界坐标系下的坐标，确定无人机前方环境与无人机之间的深度距离。该步骤的具体实现方式如下所述。

为了计算无人机前向环境与无人机之间的深度距离，需要设置航向角目的是为了使得世界坐标系的X_w方向与无人机的前向方向重合，这样前向环境信息点P在前向图像坐标系中的坐标转换到的世界坐标系的X_w坐标值就是前向环境与无人机之间的深度距离。然后，过滤无人机前方环境与无人机之间的深度距离中位于地平线以下的深度距离，也就是过滤掉小于 0的深度距离，得到位于地平线以上的深度距离信息，进而可将所得到的深度距离应用在无人机避障系统中。

第二种情况是后向双目相机情况下如何获取无人机周围环境的深度距离。这种情况下的步骤流程与前向双目相机情况下的步骤流程类似，只是在第五步骤，也即将周围环境信息点在相机坐标下的坐标转换成世界坐标系下的坐标上有所不同。以下主要对这一区别步骤进行详细描述。

由于后向双目相机是镜头朝向无人机的后方、与机头平行安装在机架上，所以：(1)后向相机坐标系与前向相机坐标系不同，对于后向相机坐标系而言，其原点在后向双目相机的镜头的光心处，X_c轴方向为平行后向双目相机水平向左，Y_c轴方向为平行后向双目相机垂直向下，Z_c轴方向为后向双目相机的光轴方向，单位都是毫米(mm)，符合右手定则；(2)无人机的姿态数据与后向双目相机的姿态数据也不完全一致，其中，后向双目相机的俯仰角θ_back、横滚角ψ_back和航向角正好分别与无人机的俯仰角θ、横滚角ψ和航向角相反，即θ_back＝-θ，ψ_back＝-ψ，因此，在计算后向相机坐标系下的坐标向世界坐标系下的坐标的转换矩阵时需要用θ_back、ψ_back和替换公式(7)、(8)和(9)中的俯仰角θ、横滚角ψ和航向角即可。

另外，在第六步骤中，为了计算无人机后向环境的深度距离，与前向双目相机的情况一样，需要设置航向角目的是为了使得世界坐标系的X_w方向与无人机的后向方向重合，然后，后向环境中的信息点P在后向图像坐标系中的坐标转换到的世界坐标系的X_w坐标值就是后向环境的深度距离。最后，过滤无人机后方环境与无人机之间的深度距离中位于地平线以下的深度距离，也就是过滤掉小于0的深度距离，得到位于地平线以上的深度距离信息，进而可将得到的深度距离应用在无人机避障系统中。

第三种情况是下向双目相机情况下如何获取无人机周围环境的深度距离。这种情况下的步骤流程与前向双目相机情况下的步骤流程类似，只是在第五步骤，也即将周围环境信息点在相机坐标下的坐标转换成世界坐标系下的坐标上有所不同。以下主要对这一区别步骤进行详细描述。

由于下向双目相机是镜头朝向无人机的下方、与机头平行安装在机架上，所以，下向相机坐标系与前向相机坐标系是不同的，也即对于下向相机坐标系而言，其原点在下向双目相机的镜头的光心处，X_c轴方向为平行下向双目相机水平向右，Y_c轴方向为平行下向双目相机水平向后，Z_c轴方向为下向双目相机的光轴方向，单位都是毫米(mm)，符合右手定则。同时无人机的姿态数据与下向双目相机的姿态数据也不完全一致，其中，下向双目相机的俯仰角θ_down和无人机的俯仰角θ相同，下向双目相机的横滚角ψ_down和与无人机的航向角相反，下向双目相机的航向角与无人机的横滚角ψ相同，即θ_down＝θ，因此，在计算下向相机坐标系下的坐标向世界坐标系下的坐标的转换矩阵时需要用θ_down、ψ_down和替换公式(7)、(8) 和(9)中的俯仰角θ、横滚角ψ和航向角即可。最后得到在世界坐标系中的坐标Z_w就是准确的下向深度距离信息。

对于求解下向环境的深度距离来说，由于下向相机坐标系下的Z_c轴与深度距离信息相关，同时在世界坐标系下，也是Z_w轴与深度距离信息相关，所以为了计算下向环境的深度距离，只需计算由下向相机坐标系Z_c轴到世界坐标系Z_w轴的变换，然后求得在世界坐标系下的Z_w轴的坐标值，即是准确的下向深度距离信息。这样，还可以提高计算速度，更具有实时性。

由于无人机的姿态数据中的俯仰角θ和横滚角ψ对下向相机坐标系和世界坐标系的Z轴产生旋转，所以如图4所示，根据三角几何关系，可以得到：

对于横滚角ψ有：

对于俯仰角θ有：

其中，下向环境中的信息点P在下向相机坐标系下的Z轴的坐标为P_zc，在世界坐标系下的Z轴的坐标为P_zw，点P和光心连线与光轴的夹角α和β可根据相机成像原理求得：

所以，由上述可知，下向环境中的信息点P从下向相机坐标系下的坐标到世界坐标系下的坐标变换是先绕Y_w轴旋转、再绕X_w轴旋转，公式如下：

P_zw即是求得的下向环境的深度距离。

根据本公开的又一实施例，提供一种深度距离测量装置，该装置可以应用于无人机，能够在基于嵌入式实时双目视觉的无人机避障系统的三维环境信息重建算法中实现，以实现无人机的实时避障等功能。如图5所示，该装置可以包括：

接收模块51，用于接收无人机的姿态数据以及双目相机采集到的无人机周围环境的左目图像和右目图像；

获取模块52，用于基于所述左目图像和所述右目图像，获取所述无人机周围环境的视差值图像；

第一转换模块53，用于基于所述视差值图像和所述双目相机的标定参数，将所述无人机周围环境在图像坐标系下的坐标转换成相机坐标系下的坐标；

第二转换模块54，用于基于所述双目相机在所述无人机上的安装位置和所述姿态数据，将所述无人机周围环境在所述相机坐标系下的坐标转换成世界坐标系下的坐标；

确定模块55，用于基于所述无人机周围环境在所述世界坐标系下的坐标，确定所述无人机周围环境与所述无人机之间的深度距离。

通过采用上述技术方案，由于在无人机测距过程中考虑了无人机的姿态数据，所以能够实现无人机的准确和实时测距，并进而能够确保无人机的准确避障。以安装在无人机机架前方的前向双目相机为例，若无人机按照一定的俯仰角向前飞行，则前向双目相机也有一定的俯仰角，因此，在有俯仰角的状态下，根据本公开实施例的方法所测到的无人机前方的深度距离是水平向前的，这样测得的深度距离会非常接近实际的深度距离，进而有利于无人机实时避障。

可选地，所述获取模块52可以包括：校正子模块，用于对所述左目图像和所述右目图像进行校正，得到图像像素行对准的左目校正图像和右目校正图像；以及计算子模块，用于对所述左目校正图像和所述右目校正图像进行计算得到所述视差值图像。

可选地，所述第一转换模块53通过以下公式来基于所述视差值图像和所述双目相机的标定参数，将所述无人机周围环境在图像坐标系下的坐标转换成相机坐标系下的坐标：

其中，(X_c,Y_c,Z_c,W)为所述相机坐标系下的齐次坐标，W是所述齐次坐标的一个分量，x、y是所述图像坐标系下的像素坐标，d是与所述图像坐标系下的像素坐标相对应的视差值，c_x、c_y、f、T是所述双目相机的标定参数， c_x和c_y表示所述双目相机的主光轴与图像平面交点的像素坐标，f表示焦距， T表示所述双目相机的两个镜头的投影中心的连线的距离。

可选地，对于安装在所述无人机机架前方的前向双目相机而言，所述第二转换模块54通过以下公式来基于所述双目相机在所述无人机上的安装位置和所述姿态数据，将所述无人机周围环境在所述相机坐标系下的坐标转换成世界坐标系下的坐标：

P_w(X_w,Y_w,Z_w)＝R^-1*P_c(X_c,Y_c,Z_c) (17)

R＝R_x*R_y*R_z (18)

其中，P_w(X_w,Y_w,Z_w)是所述世界坐标系下的坐标，P_c(X_c,Y_c,Z_c)是所述相机坐标系下的坐标，R_x是绕所述世界坐标系的X_w轴的旋转矩阵，R_y是绕所述世界坐标系的Y_w轴的旋转矩阵，R_z是绕所述世界坐标系的Z_w轴的旋转矩阵，ψ是横滚角，θ是俯仰角，是航向角。

可选地，对于安装在所述无人机机架后方的后向双目相机而言，所述第二转换模块54通过以下公式来基于所述双目相机在所述无人机上的安装位置和所述姿态数据，将所述无人机周围环境在所述相机坐标系下的坐标转换成世界坐标系下的坐标：

P_w(X_w,Y_w,Z_w)＝R^-1*P_c(X_c,Y_c,Z_c) (22)

R＝R_x*R_y*R_z (23)

其中，P_w(X_w,Y_w,Z_w)是所述世界坐标系下的坐标，P_c(X_c,Y_c,Z_c)是所述相机坐标系下的坐标，R_x是绕所述世界坐标系的X_w轴的旋转矩阵，R_y是绕所述世界坐标系的Y_w轴的旋转矩阵，R_z是绕所述世界坐标系的Z_w轴的旋转矩阵，ψ是横滚角，θ是俯仰角，是航向角。

可选地，对于安装在所述无人机机架下方的下向双目相机而言，所述第二转换模块54通过以下公式来基于所述双目相机在所述无人机上的安装位置和所述姿态数据，将所述无人机周围环境在所述相机坐标系下的坐标转换成世界坐标系下的坐标：

P_w(X_w,Y_w,Z_w)＝R^-1*P_c(X_c,Y_c,Z_c) (27)

R＝R_x*R_y*R_z (28)

其中，P_w(X_w,Y_w,Z_w)是所述世界坐标系下的坐标，P_c(X_c,Y_c,Z_c)是所述相机坐标系下的坐标，R_x是绕所述世界坐标系的X_w轴的旋转矩阵，R_y是绕所述世界坐标系的Y_w轴的旋转矩阵，R_z是绕所述世界坐标系的Z_w轴的旋转矩阵，ψ是横滚角，θ是俯仰角，是航向角。

可选地，对于安装在所述无人机机架前方的前向双目相机或者安装在所述无人机机架后方的后向双目相机而言，所述确定模块55可以包括：确定子模块，用于将所述航向角设置为零，则所述世界坐标系中的X_w坐标值被确定为所述无人机周围环境与所述无人机之间的深度距离；过滤子模块，用于过滤所述无人机周围环境与所述无人机之间的深度距离中位于地平线以下的深度距离，得到位于地平线以上的深度距离信息。

可选地，对于所述下向双目相机而言，所述确定模块55可以通过以下公式来基于所述无人机周围环境在所述世界坐标系下的坐标，确定所述无人机周围环境与所述无人机之间的深度距离：

其中，P_zw是所述无人机周围环境与所述无人机之间的下向深度距离，夹角α和β是所述无人机周围环境中的点P和光心连线与光轴的夹角，P_zc是所述无人机周围环境中的点P在下向相机坐标系的Z轴中的坐标。

关于上述实施例中的装置，其中各个模块执行操作的具体方式已经在有关该方法的实施例中进行了详细描述，此处将不做详细阐述说明。

图6是根据一示例性实施例示出的一种电子设备700的框图。如图6所示，该电子设备700可以包括：处理器701，存储器702。该电子设备700 还可以包括多媒体组件703，输入/输出(I/O)接口704，以及通信组件705 中的一者或多者。

其中，处理器701用于控制该电子设备700的整体操作，以完成上述的深度距离测量方法中的全部或部分步骤。存储器702用于存储各种类型的数据以支持在该电子设备700的操作，这些数据例如可以包括用于在该电子设备700上操作的任何应用程序或方法的指令，以及应用程序相关的数据，例如联系人数据、收发的消息、图片、音频、视频等等。该存储器702可以由任何类型的易失性或非易失性存储设备或者它们的组合实现，例如静态随机存取存储器(Static Random Access Memory，简称SRAM)，电可擦除可编程只读存储器(Electrically Erasable Programmable Read-Only Memory，简称 EEPROM)，可擦除可编程只读存储器(Erasable Programmable Read-Only Memory，简称EPROM)，可编程只读存储器(Programmable Read-Only Memory，简称PROM)，只读存储器(Read-Only Memory，简称ROM)，磁存储器，快闪存储器，磁盘或光盘。多媒体组件703可以包括屏幕和音频组件。其中屏幕例如可以是触摸屏，音频组件用于输出和/或输入音频信号。例如，音频组件可以包括一个麦克风，麦克风用于接收外部音频信号。所接收的音频信号可以被进一步存储在存储器702或通过通信组件705发送。音频组件还包括至少一个扬声器，用于输出音频信号。I/O接口704为处理器701 和其他接口模块之间提供接口，上述其他接口模块可以是键盘，鼠标，按钮等。这些按钮可以是虚拟按钮或者实体按钮。通信组件705用于该电子设备 700与其他设备之间进行有线或无线通信。无线通信，例如Wi-Fi，蓝牙，近场通信(Near FieldCommunication，简称NFC)，2G、3G或4G，或它们中的一种或几种的组合，因此相应的该通信组件705可以包括：Wi-Fi模块，蓝牙模块，NFC模块。

在一示例性实施例中，电子设备700可以被一个或多个应用专用集成电路(Application Specific Integrated Circuit，简称ASIC)、数字信号处理器 (DigitalSignal Processor，简称DSP)、数字信号处理设备(Digital Signal Processing Device，简称DSPD)、可编程逻辑器件(Programmable Logic Device，简称PLD)、现场可编程门阵列(Field Programmable Gate Array，简称FPGA)、控制器、微控制器、微处理器或其他电子元件实现，用于执行上述的深度距离测量方法。

在另一示例性实施例中，还提供了一种包括程序指令的计算机可读存储介质，该程序指令被处理器执行时实现上述的深度距离测量方法的步骤。例如，该计算机可读存储介质可以为上述包括程序指令的存储器702，上述程序指令可由电子设备700的处理器701执行以完成上述的深度距离测量方法。

以上结合附图详细描述了本公开的优选实施方式，但是，本公开并不限于上述实施方式中的具体细节，在本公开的技术构思范围内，可以对本公开的技术方案进行多种简单变型，这些简单变型均属于本公开的保护范围。

另外需要说明的是，在上述具体实施方式中所描述的各个具体技术特征，在不矛盾的情况下，可以通过任何合适的方式进行组合。为了避免不必要的重复，本公开对各种可能的组合方式不再另行说明。

此外，本公开的各种不同的实施方式之间也可以进行任意组合，只要其不违背本公开的思想，其同样应当视为本公开所公开的内容。

Claims (18)

1.一种深度距离测量方法，其特征在于，该方法包括：

接收无人机的姿态数据以及双目相机采集到的无人机周围环境的左目图像和右目图像；

基于所述左目图像和所述右目图像，获取所述无人机周围环境的视差值图像；

基于所述视差值图像和所述双目相机的标定参数，将所述无人机周围环境在图像坐标系下的坐标转换成相机坐标系下的坐标；

基于所述双目相机在所述无人机上的安装位置和所述姿态数据，将所述无人机周围环境在所述相机坐标系下的坐标转换成世界坐标系下的坐标；

基于所述无人机周围环境在所述世界坐标系下的坐标，确定所述无人机周围环境与所述无人机之间的深度距离。

2.根据权利要求1所述的方法，其特征在于，所述基于所述左目图像和所述右目图像，获取所述无人机周围环境的视差值图像，包括：

对所述左目图像和所述右目图像进行校正，得到图像像素行对准的左目校正图像和右目校正图像；

对所述左目校正图像和所述右目校正图像进行计算得到所述视差值图像。

3.根据权利要求1所述的方法，其特征在于，所述基于所述视差值图像和所述双目相机的标定参数，将所述无人机周围环境在图像坐标系下的坐标转换成相机坐标系下的坐标，通过以下公式来实现：

其中，(X_c,Y_c,Z_c,W)为所述相机坐标系下的齐次坐标，W是所述齐次坐标的一个分量，x、y是所述图像坐标系下的像素坐标，d是与所述图像坐标系下的像素坐标相对应的视差值，c_x、c_y、f、T是所述双目相机的标定参数，c_x和c_y表示所述双目相机的主光轴与图像平面交点的像素坐标，f表示焦距，T表示所述双目相机的两个镜头的投影中心的连线的距离。

4.根据权利要求1所述的方法，其特征在于，对于安装在所述无人机机架前方的前向双目相机而言，所述基于所述双目相机在所述无人机上的安装位置和所述姿态数据，将所述无人机周围环境在所述相机坐标系下的坐标转换成世界坐标系下的坐标，通过以下公式来实现：

P_w(X_w,Y_w,Z_w)＝R^-1*P_c(X_c,Y_c,Z_c)

R＝R_x*R_y*R_z

其中，P_w(X_w,Y_w,Z_w)是所述世界坐标系下的坐标，P_c(X_c,Y_c,Z_c)是所述相机坐标系下的坐标，R_x是绕所述世界坐标系的X_w轴的旋转矩阵，R_y是绕所述世界坐标系的Y_w轴的旋转矩阵，R_z是绕所述世界坐标系的Z_w轴的旋转矩阵，ψ是横滚角，θ是俯仰角，是航向角。

5.根据权利要求1所述的方法，其特征在于，对于安装在所述无人机机架后方的后向双目相机而言，所述基于所述双目相机在所述无人机上的安装位置和所述姿态数据，将所述无人机周围环境在所述相机坐标系下的坐标转换成世界坐标系下的坐标，通过以下公式来实现：

P_w(X_w,Y_w,Z_w)＝R^-1*P_c(X_c,Y_c,Z_c)

R＝R_x*R_y*R_z

其中，P_w(X_w,Y_w,Z_w)是所述世界坐标系下的坐标，P_c(X_c,Y_c,Z_c)是所述相机坐标系下的坐标，R_x是绕所述世界坐标系的X_w轴的旋转矩阵，R_y是绕所述世界坐标系的Y_w轴的旋转矩阵，R_z是绕所述世界坐标系的Z_w轴的旋转矩阵，ψ是横滚角，θ是俯仰角，是航向角。

6.根据权利要求1所述的方法，其特征在于，对于安装在所述无人机机架下方的下向双目相机而言，所述基于所述双目相机在所述无人机上的安装位置和所述姿态数据，将所述无人机周围环境在所述相机坐标系下的坐标转换成世界坐标系下的坐标，通过以下公式来实现：

P_w(X_w,Y_w,Z_w)＝R^-1*P_c(X_c,Y_c,Z_c)

R＝R_x*R_y*R_z

其中，P_w(X_w,Y_w,Z_w)是所述世界坐标系下的坐标，P_c(X_c,Y_c,Z_c)是所述相机坐标系下的坐标，R_x是绕所述世界坐标系的X_w轴的旋转矩阵，R_y是绕所述世界坐标系的Y_w轴的旋转矩阵，R_z是绕所述世界坐标系的Z_w轴的旋转矩阵，ψ是横滚角，θ是俯仰角，是航向角。

7.根据权利要求4或5所述的方法，其特征在于，对于安装在所述无人机机架前方的前向双目相机或者安装在所述无人机机架后方的后向双目相机而言，所述基于所述无人机周围环境在所述世界坐标系下的坐标，确定所述无人机周围环境与所述无人机之间的深度距离，包括：

将所述航向角设置为零，则所述世界坐标系中的X_w坐标值被确定为所述无人机周围环境与所述无人机之间的深度距离；

过滤所述无人机周围环境与所述无人机之间的深度距离中位于地平线以下的深度距离，得到位于地平线以上的深度距离信息。

8.根据权利要求6所述的方法，其特征在于，对于所述下向双目相机而言，所述基于所述无人机周围环境在所述世界坐标系下的坐标，确定所述无人机周围环境与所述无人机之间的深度距离，通过以下公式来实现：

其中，P_zw是所述无人机周围环境与所述无人机之间的下向深度距离，夹角α和β是所述无人机周围环境中的点P和光心连线与光轴的夹角，P_zc是所述无人机周围环境中的点P在下向相机坐标系的Z轴中的坐标。

9.一种深度距离测量装置，其特征在于，该装置包括：

接收模块，用于接收无人机的姿态数据以及双目相机采集到的无人机周围环境的左目图像和右目图像；

获取模块，用于基于所述左目图像和所述右目图像，获取所述无人机周围环境的视差值图像；

第一转换模块，用于基于所述视差值图像和所述双目相机的标定参数，将所述无人机周围环境在图像坐标系下的坐标转换成相机坐标系下的坐标；

第二转换模块，用于基于所述双目相机在所述无人机上的安装位置和所述姿态数据，将所述无人机周围环境在所述相机坐标系下的坐标转换成世界坐标系下的坐标；

确定模块，用于基于所述无人机周围环境在所述世界坐标系下的坐标，确定所述无人机周围环境与所述无人机之间的深度距离。

10.根据权利要求9所述的装置，其特征在于，所述获取模块包括：

校正子模块，用于对所述左目图像和所述右目图像进行校正，得到图像像素行对准的左目校正图像和右目校正图像；

计算子模块，用于对所述左目校正图像和所述右目校正图像进行计算得到所述视差值图像。

11.根据权利要求9所述的装置，其特征在于，所述第一转换模块通过以下公式来基于所述视差值图像和所述双目相机的标定参数，将所述无人机周围环境在图像坐标系下的坐标转换成相机坐标系下的坐标：

其中，(X_c,Y_c,Z_c,W)为所述相机坐标系下的齐次坐标，W是所述齐次坐标的一个分量，x、y是所述图像坐标系下的像素坐标，d是与所述图像坐标系下的像素坐标相对应的视差值，c_x、c_y、f、T是所述双目相机的标定参数，c_x和c_y表示所述双目相机的主光轴与图像平面交点的像素坐标，f表示焦距，T表示所述双目相机的两个镜头的投影中心的连线的距离。

12.根据权利要求9所述的装置，其特征在于，对于安装在所述无人机机架前方的前向双目相机而言，所述第二转换模块通过以下公式来基于所述双目相机在所述无人机上的安装位置和所述姿态数据，将所述无人机周围环境在所述相机坐标系下的坐标转换成世界坐标系下的坐标：

P_w(X_w,Y_w,Z_w)＝R^-1*P_c(X_c,Y_c,Z_c)

R＝R_x*R_y*R_z

其中，P_w(X_w,Y_w,Z_w)是所述世界坐标系下的坐标，P_c(X_c,Y_c,Z_c)是所述相机坐标系下的坐标，R_x是绕所述世界坐标系的X_w轴的旋转矩阵，R_y是绕所述世界坐标系的Y_w轴的旋转矩阵，R_z是绕所述世界坐标系的Z_w轴的旋转矩阵，ψ是横滚角，θ是俯仰角，是航向角。

13.根据权利要求9所述的装置，其特征在于，对于安装在所述无人机机架后方的后向双目相机而言，所述第二转换模块通过以下公式来基于所述双目相机在所述无人机上的安装位置和所述姿态数据，将所述无人机周围环境在所述相机坐标系下的坐标转换成世界坐标系下的坐标：

P_w(X_w,Y_w,Z_w)＝R^-1*P_c(X_c,Y_c,Z_c)

R＝R_x*R_y*R_z

其中，P_w(X_w,Y_w,Z_w)是所述世界坐标系下的坐标，P_c(X_c,Y_c,Z_c)是所述相机坐标系下的坐标，R_x是绕所述世界坐标系的X_w轴的旋转矩阵，R_y是绕所述世界坐标系的Y_w轴的旋转矩阵，R_z是绕所述世界坐标系的Z_w轴的旋转矩阵，ψ是横滚角，θ是俯仰角，是航向角。

14.根据权利要求9所述的装置，其特征在于，对于安装在所述无人机机架下方的下向双目相机而言，所述第二转换模块通过以下公式来基于所述双目相机在所述无人机上的安装位置和所述姿态数据，将所述无人机周围环境在所述相机坐标系下的坐标转换成世界坐标系下的坐标：

P_w(X_w,Y_w,Z_w)＝R^-1*P_c(X_c,Y_c,Z_c)

R＝R_x*R_y*R_z

其中，P_w(X_w,Y_w,Z_w)是所述世界坐标系下的坐标，P_c(X_c,Y_c,Z_c)是所述相机坐标系下的坐标，R_x是绕所述世界坐标系的X_w轴的旋转矩阵，R_y是绕所述世界坐标系的Y_w轴的旋转矩阵，R_z是绕所述世界坐标系的Z_w轴的旋转矩阵，ψ是横滚角，θ是俯仰角，是航向角。

15.根据权利要求12或13所述的装置，其特征在于，对于安装在所述无人机机架前方的前向双目相机或者安装在所述无人机机架后方的后向双目相机而言，所述确定模块包括：

确定子模块，用于将所述航向角设置为零，则所述世界坐标系中的X_w坐标值被确定为所述无人机周围环境与所述无人机之间的深度距离；

过滤子模块，用于过滤所述无人机周围环境与所述无人机之间的深度距离中位于地平线以下的深度距离，得到位于地平线以上的深度距离信息。

16.根据权利要求14所述的装置，其特征在于，对于所述下向双目相机而言，所述确定模块通过以下公式来基于所述无人机周围环境在所述世界坐标系下的坐标，确定所述无人机周围环境与所述无人机之间的深度距离：

其中，P_zw是所述无人机周围环境与所述无人机之间的下向深度距离，夹角α和β是所述无人机周围环境中的点P和光心连线与光轴的夹角，P_zc是所述无人机周围环境中的点P在下向相机坐标系的Z轴中的坐标。

17.一种计算机可读存储介质，其上存储有计算机程序，其特征在于，该程序被处理器执行时实现权利要求1-8中任一项所述方法的步骤。

18.一种电子设备，其特征在于，包括：

权利要求17中所述的计算机可读存储介质；以及

一个或者多个处理器，用于执行所述计算机可读存储介质中的程序。