1.一种基于四旋翼无人机的单目相机尺度估计方法,其特征在于:所述方法包括以下步骤:
1)获取图像及IMU参数信息;
2)提取二维图像特征点并且进行特征匹配;
3)求解相机外参矩阵;
4)融合IMU信息,估计单目相机的尺度,过程如下:首先根据采集到的IMU参数信息和通过视觉计算得到的无人机的运动姿态,即位置P和旋转矩阵R,根据两者参数估计步骤4)中单目相机的尺度因子,包括以下步骤:
4.1)视觉和IMU参数时域上的同步比较陀螺仪的参时间戳和由视觉获取的角速度时间戳,选取最小平方差的偏移值为最优的时间偏移量to,将较小的时间戳加上获取的最优的时间偏移量,实现视觉和IMU在时间上的同步;
其中,由视觉获取的角速度是根据相机的外参矩阵中的旋转矩阵 变换而来,角速度由ωx、ωy和ωz组成,其变换关系为:其中, 表示世界坐标系到相机坐标系的旋转矩阵; 表示在相机标系下的视觉角速度;
4.2)视觉和IMU参数频域上的同步由4.1)获取的基于视觉的角速度与从陀螺仪读出的角速度分别在两个坐标系下,即相机坐标系和IMU坐标系,因此,需要求解相机坐标系与IMU坐标系的旋转矩阵 用于视觉与IMU参数在频域上的同步, 的计算公式如下:其中, 为在相机坐标系下的陀螺仪零偏;to为最优的时间戳偏移量;
4.3)相机坐标系下重力加速度的估计采用比较视觉和IMU获取的加速度值进行尺度的估计,由于地球的重力加速度gw不能直接通过相机观测到,所以不能直接比较视觉和IMU的加速度值,进而需要估计相机坐标系下的gw:
其中, 表示相机坐标系下的加速度,可由视觉获取的位置坐标二次积分获取; 表示IMU坐标系下加速度计的读数; 表示经过旋转变换后的在相机坐标系下的加速度;s表示单目相机的初始尺度因子; 表示加速度计的零偏,s和 的计算公式如下:
4.4)单目相机尺度因子的估计在频域中估计单目的尺度,通过傅里叶变换将时域中的相机坐标系下的视觉和IMU的V I
加速度变换到频域中A(f),A(f):其中,F{.}表示傅立叶变化,通过最小化视觉和IMU加速度的幅值估计在频域中的单目相机尺度:
2
||gw||=9.8 (10)其中,fmax设置为默认值1.2,至此,即可估计出单目相机的尺度;
所述步骤4)融合视觉和IMU的参数信息,进而估计单目相机尺度的过程为:通过对相机拍摄图片进行处理,可获取运动相机的姿态:旋转矩阵R和平移向量t,将旋转矩阵变化得到基于视觉的角速度;对平移向量进行二次积分得到基于视觉的加速度,融合IMU参数提供的加速度计和陀螺仪读数,估计出单目相机的尺度信息。
2.如权利要求1所述的一种基于四旋翼无人机的单目相机尺度估计方法,其特征在于:所述步骤1)中,获取图像及IMU参数信息的过程包括以下步骤:
1.1)采集图像
基于四旋翼飞行器平台的Linux开发环境,使用机器人操作系统ROS订阅图像主题的方式获取图像的,相机驱动由ROS和openCV实现;包括:时间戳和图片;
1.2)获取IMU参数
基于四旋翼飞行器平台的Linux开发环境,使用机器人操作系统ROS订阅图像主题的方式获取IMU参数,包括:时间戳、加速度计读数、陀螺仪读数。
3.如权利要求1或2所述的一种基于四旋翼无人机的单目相机尺度估计方法,其特征在于:所述步骤2)中,提取二维图像特征点并且进行特征匹配的方法,步骤如下:
2.1)SIFT提取特征点
采用实现尺度不变的高斯变换核,高斯核定义为:其中σ表示正态分布方差,从而尺度空间表示为图像与高斯核的卷积结果如式为:其值越小代表图像经过的平滑处理越小,也就是相应的尺度越小,其中大尺度代表图像的轮廓,而小尺度代表图像的细节部分;描述特征算子中加上一个方向向量作为算子置信度的评价标准,SIFT算子采用一个向量算子作为描述;
2.2)SURF特征描述符建立在过滤特征点时,如式(13),先采用Hessian矩阵获取图像的局部特征值,利用图像灰度性质的算法积分图像这种方法,这大大加快了程序的运行速度,降低了运行时间;
首先计算图像的Hessian矩阵,其中Hessian是SURF算子的核心,在计算图像Hessian矩阵前需先对其高斯滤波,使其具有尺度无关性,引入图像每个像素计算出其Hessian矩阵行列式作为评价值来判断,通过这种方法,为了旋转一个值来判别特征点,图像尺度变化与加权值有对应关系,即真实值与近似值之间的差异关系,则Hessian矩阵判别如式(14):2
det(Happrox)=DxxDyy‑(0.9Dxy) (14)随后,把高斯函数作为处理因子对图像函数进行多次的卷积并且获得二次抽样结果,最终生成图像金字塔,匹配度根据特征点及特征值的相似度来计算;
2.3)特征描述符匹配
当得到两幅图像的SURF描述符后,对两幅图像的描述符进行匹配,采用了K最临近法来对两个描述符进行匹配;KNN的思想是假设每一个类包含多个样本数据,而且每个数据都有一个唯一的类标记表示这些样本是属于哪个分类,计算每个样本数据到待分类数据的距离,取和待分类数据最近的K个样本数据,这K个样本数据中哪个类别的样本数据占多数,则待分类数据就属于该类别;对于目标模板图像中的每个特征点,在输入图像中查找该点的最近邻的两个匹配,比较这两个匹配之间的距离,如果模板图像中一点的匹配距离小于0.8倍输入图像的匹配距离,认为模板中的点和输入图像对应的点为有效匹配,记录下相应的坐标值,当两幅图像间的匹配点多于4个,本文认为在输入图像中找到了目标物体,对应的坐标信息即为二维特征信息。
4.如权利要求1所述的一种基于四旋翼无人机的单目相机尺度估计方法,其特征在于:所述步骤3)中,求解相机外参矩阵的步骤如下:
3.1)三个坐标系的变换,过程如下:
3.1.1)图像坐标系与像平面坐标系的转换T
图像坐标系是以左上角为原点的图像像素坐标系[u,v] ,该坐标系没有物理单位,因此T
引入原点OI在光轴上的像平面坐标系I=[xI,yI] ,像平面是相机根据小孔成像模型构建出来的具有物理意义的平面,假设每一个像素在u轴和v轴方向上的物理尺寸为dx和dy,其含义是感光芯片上像素的实际大小,是连接图像坐标系和真实尺寸坐标系的桥梁,dx和dy与摄像机焦距f有关,则像平面坐标系上的点(x1,y1)与像素坐标系中点(u1,v1)对应关系如下:
其中,(u0,v0)为图像坐标系中的中心点,即像平面坐标系的原点所对应的像素点,令包含四个与相机内部结构有关的参数,称为相机的内参矩阵;
3.1.2)像平面坐标系与相机坐标系的转换假设相机坐标系中一点PC1=(xC,yC,zC),连接光心在图像坐标系中的投影点为PI1=(xI,yI),则这两点之间的坐标转换关系如下:转换成矩阵形式如下:
其中,f为相机焦距;
3.1.3)相机坐标系与世界坐标系的转换T
首先,由于飞行器与相机存在安装误差,这里用[α,β,γ]表示安装固定的三维误差角,T
用[xe,ye,ze]表示摄像机到机身坐标原点的空间距离,则相机坐标系和机体坐标系的关系可以用 来表示,即
C=TB (18)其中C表示相机坐标系,B表示机体坐标系;
3.2)对极约束、基本矩阵、本质矩阵在立体视觉中两个相机拍摄所成的图片平面之间存在约束关系,成为对极约束,要想获取两张图片I1和I2之间的运动,假设第一帧到第二帧的运动为R,t,两个相机中心分别为O1和O2,在图I1中的一个p1,它在I2中对应着特征点p2,其中,p1和p2是在步骤2.3)中获取的匹配点集,二者是同一个空间点在两个两个成像平面上的投影,根据3.1)坐标系的变化,假设空间一点P=[X Y Z],可知两个像素点p1和p2的像素位置为:s1p1=KP,s2p2=K(RP+t) (19)其中K为相机的内参矩阵,R,t为两个相平面的相机运动,根据对极约束得:‑1
这两个式子都称为对极约束,x1和x2是两个像素点的归一化平面上的坐标:x1=K p1,x2‑1
=K p2,基础举证F和本质矩阵E,简化对极约束:
3.3)相机外参数的求解
在求解出相机的本质矩阵和已知相机内参数,通过奇异值分解SVD分解方法对本质矩阵进行分解求出旋转矩阵R和平移向量t,过程为:T
3.3.1)通过SVD分解方法对本质矩阵进行分解为E=UDV,这里先假设奇异值矩阵为:
3.3.2)由E性质,修改上式D的值,得到E为:
3.3.3)得到R,t
T T T T
R=UGV或R=UGV t^=VZV (24)其中,