CN115560760A

CN115560760A - 一种面向无人机的视觉/激光测距高空导航方法

Info

Publication number: CN115560760A
Application number: CN202211113811.3A
Authority: CN
Inventors: 尚克军; 李至; 扈光锋; 庄广琛; 刘崇亮; 王大元; 李茜茜; 王海军; 裴新凯; 焦浩; 明丽; 段昊雨
Original assignee: Beijing Automation Control Equipment Institute BACEI
Current assignee: Beijing Automation Control Equipment Institute BACEI
Priority date: 2022-09-14
Filing date: 2022-09-14
Publication date: 2023-01-03

Abstract

本发明提供一种面向无人机的视觉/激光测距高空导航方法，包括：视觉三角化求解三维地图点深度，利用时间戳同步的激光测距数据求解尺度初始值；构建视觉重投影误差和激光测距误差联合优化函数；建立新的图优化模型，定义节点、边和误差雅可比矩阵，通过迭代求解获取优化参数；最后根据优化参数更新整个视觉跟踪线程，完成尺度更新和位姿更新，获取无人机的精准定位。本发明方案解决了解决高空无人机场景，单目相机存在尺度偏差，难以实现高精度的导航定位和深度估计的技术问题。

Description

一种面向无人机的视觉/激光测距高空导航方法

技术领域

本发明属于计算机视觉技术领域，具体涉及一种面向无人机的视觉/激光测距高空导航方法。

背景技术

惯性现代军事化作战逐渐向智能化作战方向转变，无人机作为一种智能化作战载体在战场发挥着愈发重要的作用。无人机导航方法是引导飞行器沿着一定的速度、方向完成规定飞行过程的技术和方法，其中最关键的技术之一就是对无人机进行实时导航定位。具体表现为：在无人机的飞行过程中，实时获取较高精准度的六自由度位置和姿态信息，转化为无人机的空间经纬度坐标反馈到飞机控制系统，从而帮助无人机实现自主导航和着陆功能。

传统的无人机导航方法多依赖于卫星信息进行导航定位，基于差分GPS的方法通常可以获取较精准的空间位置。但战场形势变幻莫测，卫星拒止情况屡屡出现，不依赖卫星的自主导航方案需求迫切。基于惯性传感器(IMU)的方法受噪声影响较大，累积误差大，容易发生导航偏移。视觉传感器具备隐蔽性、轻便、低功耗、价格便宜、定位精度高的优势，是近些年无人机自主导航领域的研究热点。

高空应用场景给视觉传感器带来了一定的挑战，其关键难点在于高空场景深度估计上。对于双目相机，高空场景下，基于左右相机视差确定深度的方法失效；对于深度相机，其深度获取范围大多为几米或者十几米；对于单目相机，视觉可以计算出场景深度，但是直接在高空飞行过程中进行视觉初始化，单目相机无法建立评测标准，存在尺度偏差。

发明内容

本发明旨在至少解决现有技术或相关技术中存在的技术问题之一。

为此，本发明提供了一种面向无人机的视觉/激光测距高空导航方法。

本发明的技术解决方案如下：提供一种面向无人机的视觉/激光测距高空导航方法，该方法包括：

利用激光测距和视觉三角化获取初始尺度估计值；

基于视觉特征匹配结果构造视觉重投影误差方程，根据激光测距机与视觉观测量的关联构造激光测距误差方程，并联合两个方程以获取视觉重投影误差和激光测距误差联合优化函数，所述联合优化函数包括待优化参数：尺度因子、位姿转换矩阵和世界坐标系下的三维地图点；

基于所述初始尺度估计值，采用图优化方式对所述联合优化函数进行求解以获取优化参数；

根据优化参数更新整个视觉跟踪线程，完成尺度更新和位姿更新，获取无人机的精准定位。

进一步地，所述利用激光测距和视觉三角化获取初始尺度估计值，包括：

利用视觉传感器在高空平飞阶段进行初始跟踪以创建关键帧；

利用三角化技术获取第i个关键帧第j′个特征点对应的三维地图点的深度值d_j′，0≤j′≤n；

根据所述三维地图点的深度值获取第i个关键帧对应的三维地图点的平均深度depth_i：

接受激光测距的输入值，将其时间戳与关键帧图像时间戳对齐，经过时间戳筛选获取第i个关键帧对应的激光测距高度height_i；

根据所述激光测距高度height_i和平均深度depth_i获取第i个关键帧对应的初始尺度估计值s_begin：

进一步地，通过下述方式基于视觉特征匹配结果构造视觉重投影误差方程：

对于前n个关键帧，重投影误差

表示为下式：

其中，j表示地图点ID，

表示世界坐标系下第j个地图点的三维坐标，p_ij表示第i个关键图像帧上

对应的像素观测点，K表示相机内参矩阵，

为位姿矩阵，表示从第i个关键帧对应的世界坐标系向相机坐标系的转换，π为深度。

进一步地，通过下述方式根据激光测距机与视觉观测量的关联构造激光测距误差方程：

其中，

代表激光测距误差，s代指尺度因子，

表示激光方向区域的世界坐标系下第j个地图点的三维坐标，

代表第i个关键帧对应的相机坐标系到世界坐标系下的转换，

简化表示为

其中旋转矩阵为R_wc，平移向量为t_wc，

代表相机坐标系下激光测距方向的向量，L_i表示激光输出长度。

进一步地，通过下式获取视觉重投影误差和激光测距误差联合优化函数：

其中，

为视觉重投影误差，

为激光测距误差，m代指视觉重投影中用到的地图点数目最大值，m'代指视觉重投影中用到的地图点数目最大值，由于激光具有方向性，因此m'≤m，y_k为待优化参数，包括尺度因子s，位姿转换矩阵T_cw，可以表示为T_cw＝[R_cw|t_cw]，和世界坐标系下的三维地图点

进一步地，基于所述初始尺度估计值，采用图优化方式对所述联合优化函数进行求解以获取优化参数，包括：

1)根据所述联合优化函数建立新的图优化模型，定义模型节点、边和节点更新方式，并计算优化参数的雅可比矩阵；

2)基于所述新的图优化模型和优化参数的雅可比矩阵，采用迭代求解的方式获取优化后的尺度因子、三维地图点坐标和视觉关键帧位姿量，其中，设置待优化参数尺度的初始赋值为所述初始尺度估计值。

进一步地，通过下述方式定义模型节点和边：

图优化模型中节点定义为待优化参数，包括：尺度因子、位姿转换矩阵和三维地图点；

图优化模型中边的类型定义为超边，其连接三个节点。

进一步地，在定义节点更新方式中，设置尺度因子更新方式如下式所示：

s′＝sexp(δs)。

进一步地，通过下述方式计算优化参数的雅可比矩阵：

按照所述联合优化函数定义模型误差函数；

计算模型误差函数对于三个节点的雅可比矩阵，包括：

分别获取视觉重投影误差部分对应的三个节点的雅可比矩阵和激光测距误差部分对应的三个节点的雅可比矩阵；

将视觉重投影误差部分对应的三个节点的雅可比矩阵和激光测距误差部分对应的三个节点的雅可比矩阵对应相加即得各个节点的雅可比矩阵。

进一步地，通过下述方式获取激光误差部分对应的三个节点的雅可比矩阵：

对于节点三维地图点，在激光测距误差方程基础上对其求偏导，得到该节点的雅可比矩阵

如下式所示：

对于节点尺度因子，在激光测距误差方程基础上对其求偏导，得到该节点的雅可比矩阵r_d(s)如下式所示：

对于节点位姿转换矩阵，在激光测距误差方程基础上，分别获取在微小扰动下的位姿转换矩阵中的旋转矩阵R_cw和平移量t_cw对应雅可比矩阵。

进一步地，通过下式获取在微小扰动下的平移量t_cw对应雅可比矩阵r_d(t_cw)：

进一步地，通过下式获取在微小扰动下的平移量R_cw对应雅可比矩阵r_d(R_cw)：

进一步地，所述根据优化参数更新整个视觉跟踪线程，完成尺度更新和位姿更新，包括：

根据得到的优化参数尺度因子更新当前三维地图点的深度，得到更新后的三维地图点深度，其中，所述三维地图点为得到的优化参数三维地图点；

根据得到的优化参数位姿转换矩阵更新每一帧的旋转矩阵和平移向量。

上述技术方案一方面针对无人机实际高空初始化场景中尺度难以估计的问题，利用视觉传感器和激光测距机两种传感器，结合真实观测的激光测距高度和视觉求解的估计三维地图点深度获取尺度初值；建立误差方程有效求解了场景的尺度因子，实现了场景的尺度更新，整体提升了高空场景下跟踪定位中深度估计的精准性。另一方面针对实际高空应用场景中出现的视觉位姿求解精度低的问题，增加新的约束条件。利用视觉测量获取三维地图点坐标，利用激光测距观测值增加误差约束，通过增加有效的约束建立联合误差优化方程，获取了更精准的相机位姿转换矩阵，整体提升了高空场景下跟踪定位中六自由度位姿估计的精准性和鲁棒性。综上，本发明解决了高空初始化场景，单目相机存在尺度偏差，难以实现高精度的导航定位和深度估计的技术问题，对于高空无人机的位姿估计和深度估计具备较高的精准性、实时性、鲁棒性和实用性。

附图说明

所包括的附图用来提供对本发明实施例的进一步的理解，其构成了说明书的一部分，用于例示本发明的实施例，并与文字描述一起来阐释本发明的原理。显而易见地，下面描述中的附图仅仅是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他的附图。

图1示出了本发明实施例方法的流程示意图。

具体实施方式

需要说明的是，在不冲突的情况下，本申请中的实施例及实施例中的特征可以相互组合。下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。以下对至少一个示例性实施例的描述实际上仅仅是说明性的，决不作为对本发明及其应用或使用的任何限制。基于本发明中的实施例，本领域普通技术人员在没有作出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。

需要注意的是，这里所使用的术语仅是为了描述具体实施方式，而非意图限制根据本申请的示例性实施方式。如在这里所使用的，除非上下文另外明确指出，否则单数形式也意图包括复数形式，此外，还应当理解的是，当在本说明书中使用术语“包含”和/或“包括”时，其指明存在特征、步骤、操作、器件、组件和/或它们的组合。

除非另外具体说明，否则在这些实施例中阐述的部件和步骤的相对布置、数字表达式和数值不限制本发明的范围。同时，应当明白，为了便于描述，附图中所示出的各个部分的尺寸并不是按照实际的比例关系绘制的。对于相关领域普通技术人员已知的技术、方法和设备可能不作详细讨论，但在适当情况下，所述技术、方法和设备应当被视为授权说明书的一部分。在这里示出和讨论的所有示例中，任何具体值应被解释为仅仅是示例性的，而不是作为限制。因此，示例性实施例的其它示例可以具有不同的值。应注意到：相似的标号和字母在下面的附图中表示类似项，因此，一旦某一项在一个附图中被定义，则在随后的附图中不需要对其进行进一步讨论。

如图1所示，在本发明的一个实施例中，提供一种面向无人机的视觉/激光测距高空导航方法，该方法包括：

步骤一、利用激光测距和视觉三角化获取初始尺度估计值；

步骤二、基于视觉特征匹配结果构造视觉重投影误差方程，根据激光测距机与视觉观测量的关联构造激光测距误差方程，并联合两个方程以获取视觉重投影误差和激光测距误差联合优化函数，所述联合优化函数包括待优化参数：尺度因子、位姿转换矩阵和世界坐标系下的三维地图点；

步骤三、基于所述初始尺度估计值，采用图优化方式对所述联合优化函数进行求解以获取优化参数；

步骤四、根据优化参数更新整个视觉跟踪线程，完成尺度更新和位姿更新，获取无人机的精准定位。

也即，面对高空无人机应用场景单目相机难以实现高精度的导航定位和深度估计的问题，本发明实施例引入激光测距仪建立深度评测标准，提出一种面向无人机的视觉激光测距高空初始化方法，建立传感器深度耦合模型，求解单目视觉的尺度信息，实现高精度的空中自主导航定位。

本发明实施例一方面针对无人机实际高空初始化场景中尺度难以估计的问题，利用视觉传感器和激光测距机两种传感器，结合真实观测的激光测距高度和视觉求解的估计三维地图点深度获取尺度初值；建立误差方程有效求解了场景的尺度因子，实现了场景的尺度更新，整体提升了高空场景下跟踪定位中深度估计的精准性。另一方面针对实际高空应用场景中出现的视觉位姿求解精度低的问题，增加新的约束条件。利用视觉测量获取三维地图点坐标，利用激光测距观测值增加误差约束，通过增加有效的约束建立联合误差优化方程，获取了更精准的相机位姿转换矩阵，整体提升了高空场景下跟踪定位中六自由度位姿估计的精准性和鲁棒性。综上，本发明解决了高空初始化场景，单目相机存在尺度偏差，难以实现高精度的导航定位和深度估计的技术问题，对于高空无人机的位姿估计和深度估计具备较高的精准性、实时性、鲁棒性和实用性。

在上述实施例中，为了准确获取初始尺度估计值，所述利用激光测距和视觉三角化获取初始尺度估计值，包括：

本发明实施中的三角化技术为本领域公知技术，在此不再详细赘述。

具体来说，在高空平飞阶段，视觉传感器进行初始跟踪，创建关键帧；提取图像中的ORB特征，并根据相邻两帧之间的特征匹配进行初始位姿计算；利用三角化技术对两帧图像匹配进行求解，获取第i个关键帧第j′个特征点对应的三维地图点的深度值d_j′，例如前10个关键帧对应的三维地图点深度，并根据该深度值求解第i个关键帧对应的三维地图点的平均深度depth_i。同时采用时间戳对齐的方式筛选出第i个关键帧对应的激光测距高度height_i，进而由平均深度和激光测距高度获取初始尺度估计值。

在上述实施例中，为了准确建立视觉重投影误差方程和激光测距误差方程，通过下述方式基于视觉特征匹配结果构造视觉重投影误差方程：

对于前n个关键帧，重投影误差

表示为下式：

其中，j表示地图点ID，

对应的像素观测点，K表示相机内参矩阵，

以及通过下述方式根据激光测距机与视觉观测量的关联构造激光测距误差方程：

其中，

代表激光测距误差，s代指尺度因子，

表示激光方向区域的世界坐标系下第j个地图点的三维坐标，

代表第i个关键帧对应的相机坐标系到世界坐标系下的转换。

简化表示为

其中旋转矩阵为R_wc，平移向量为t_wc，

具体来说：针对于视觉导航，主要涉及到三种坐标系：图像坐标系、相机坐标系和世界坐标系，本文定义相机坐标系表示为c系，世界坐标系表示w系。通过世界坐标系上三维地图点投影到图像坐标系，可以建立观测的像素点和估计的投影像素点之间的误差模型。

通过相机内参，可以将两个像素点从图像坐标系转换到相机坐标系。其中，图像坐标系上像素点定义为p，可以表示为p＝(u,v)；c坐标系上特征点定义为P，可以表示为P＝(x,y,z)，坐标系转换公式如下：

其中，K代表相机内参矩阵，其中f是相机焦距，包括f_x和f_y，c表示相机平面中心点偏移，包括c_x和c_y。

相机坐标系和世界坐标系的转换，涉及到相机的导航位姿矩阵T，包括旋转矩阵R和平移t两部分。

T＝[R|t]

世界坐标系下坐标表示为X，那么坐标系下转换可以表示为：

P＝TX

视觉传感器通常使用BA(Bundle Adjustment)优化，其本质是最小化重投影误差，视觉重投影误差是求解在图像坐标系下观测值与估计值的差。对于前i个关键帧而言，最小化重投影误差

表示为如上所示。

此外，激光测距误差方程的构造原理是激光测距方向上深度观测值应该与相机在该方向上求解的深度值相差一个尺度因子。特别注意的是，由于激光测距是具备指向方向的，因此三维地图点需要选择指向方向区域的地图点。本发明实施例划定相机中心区域15*15像素点矩阵区域，仅仅选择这些像素点对应的三维地图点参与运算。

具体激光测距误差方程表示如下式所示：

其中

代表激光测距误差，s代指尺度因子，

表示激光方向区域的世界坐标系下第j个地图点的三维坐标，P_o ^w表示世界坐标系下相机中心点坐标，

为世界坐标系下激光测距方向的单位向量。

假设第i个关键帧对应的相机坐标系到世界坐标系下的转换为

简化表示为

其中旋转矩阵为R_wc，平移向量为t_wc。

因为相机坐标系的原点即为相机中心点，那么P_o ^w可以转换表示为t_wc，因此式子可以转换为：

可以进一步分解，将其转换到相机坐标系下，如下式所示：

其中

代表相机坐标系下激光测距方向的向量。由于相机与激光测距安装在一个平面，因此该向量方向为(0，0，1)。

因此，式子可以进一步转换为：

也即，联合上述两个误差方程，通过下式获取视觉重投影误差和激光测距误差联合优化函数：

其中，

为视觉重投影误差，

在上述实施例中，为了准确解算联合优化函数中的优化参数，基于所述初始尺度估计值，采用图优化方式对所述联合优化函数进行求解以获取优化参数，包括：

也即，本发明实施例采用图优化方式对误差模型进行求解，图优化方式需要建立新的图优化模型，确认模型的边、节点和节点更新方式，通过计算三个优化参数的雅可比矩阵，采用迭代优化的方式，不断对雅可比矩阵进行迭代优化，直至其值缩小至最小，这时即可对应求出对应的优化参数。

较佳地，本发明实施例中，通过下述方式定义模型节点和边：

图优化模型中边的类型定义为超边，其连接三个节点。

本发明实施例中，通过下述方式计算优化参数的雅可比矩阵：

按照所述联合优化函数定义模型误差函数；

计算模型误差函数对于三个节点的雅可比矩阵，包括：

也即，对于误差函数需要求解该函数对于三个节点的雅可比矩阵，误差函数以加和形式表达，因此视觉重定义误差部分和激光误差部分可以分开求解，最后同样以相加的模式再次组合起来。

其中，视觉重投影误差部分对应的雅可比可以参照算法ORB-SLAM3中的推导进行构造，本发明在此不进行详述。本发明实施例的关键之处在于获取激光误差部分的雅可比矩阵。

本发明实施例中，通过下述方式获取激光误差部分对应的三个节点的雅可比矩阵：

如下式所示：

本发明实施例中，通过下式获取在微小扰动下的平移量t_cw对应雅可比矩阵r_d(t_cw)：

以及通过下式获取在微小扰动下的平移量R_cw对应雅可比矩阵r_d(R_cw)：

也即，对于位姿节点，其求解较为复杂，将旋转矩阵R_cw和平移量t_cw分开进行求解。通过加入微小扰动，可以计算其雅可比。

对于平移量t_cw，其微小扰动可以表示成δt_cw，可以建立包含扰动的更新变量方式为：

t_wc'＝t_wc+R_wcδt_cw

那么在发生微小扰动后，激光误差方程应变化为：

根据上式可以获取平移量t_cw的雅可比为：

对于旋转矩阵R_cw，其微小扰动为旋转小角度，表示为δφ，建立包含扰动的更新变量方式为：

R_cw'＝R_cwExp(δφ)

那么在发生微小扰动后，激光误差方程应变化为：

根据小角度近似原理，可以得到公式：

Exp(δφ)＝I+δφ^

其中δφ^代表δφ的反对称矩阵。那么上式可以进一步表示为：

由反对称矩阵的性质可以获得：

根据上式可以获取，旋转矩阵R_cw的雅可比为：

此外，在迭代计算时，可采用非线性优化框架g2o，构造线性求解器，设置最大迭代次数为30次，设置尺度的初始赋值为步骤一求解的尺度初始值，通过Levenberg-Marquardt方法迭代收敛至最小值雅可比。可以获取优化后的相机位姿、尺度因子和场景三维地图点。

较佳地，在定义节点更新方式中，为了保证尺度因子更新的非负性，设置尺度因子更新方式如下式所示：

s′＝sexp(δs)。

其中，δs表示增加量，s′,s分别表示更新后和更新前的尺度因子。

根据本发明一种实施例，所述根据优化参数更新整个视觉跟踪线程，完成尺度更新和位姿更新，包括：

具体来说：

在视觉/激光测距联合误差优化之后，获取到精准的尺度因s，按照尺度更新当前的三维地图点的深度，具体包括：

调用当前局部地图集，获取所有地图点的编号；对于每个地图点ID，获取对应的三维地图点坐标信息(步骤三优化后的三维地图点坐标信息)；然后用新的尺度因子与原始深度信息相乘，获取新的地图点深度；最后用新的地图点深度求解该局部地图集的法向量和三维地图点的中值深度。

位姿更新主要更新每一帧的旋转矩阵和平移向量，具体包括：

在获取新的位姿矩阵之后，首先设置优化标志为True，跟踪线程标志为TrackOK；然后保存每一个关键帧的位姿转换矩阵，并调用相邻两个关键帧；最后利用新的位姿矩阵重新进行视觉重投影，获取非关键帧之间的优化位姿矩阵。

综上，本发明实施例通过视觉三角化求解三维地图点深度，利用时间戳同步的激光测距数据求解尺度初始值；构建视觉重投影误差和激光测距误差联合优化函数；建立新的图优化模型，定义节点、边和误差雅可比矩阵，通过迭代求解获取优化参数；最后根据优化参数更新整个视觉跟踪线程，完成尺度更新和位姿更新，获取无人机的精准定位。本发明实施例面向无人机，在高空飞行初始阶段能精准恢复视觉难以获取的尺度信息，并且通过多源约束优化相机的六自由度位姿。本发明实施例方法对于高空无人机的位姿估计和深度估计具备较高的精准性、实时性、鲁棒性和实用性。本发明实施例方案解决了高空无人机场景，单目相机存在尺度偏差，难以实现高精度的导航定位和深度估计的技术问题。

如上针对一种实施例描述和/或示出的特征可以以相同或类似的方式在一个或更多个其它实施例中使用，和/或与其它实施例中的特征相结合或替代其它实施例中的特征使用。

应该强调，术语“包括/包含”在本文使用时指特征、整件、步骤或组件的存在，但并不排除一个或更多个其它特征、整件、步骤、组件或其组合的存在或附加。

本发明以上的方法可以由硬件实现，也可以由硬件结合软件实现。本发明涉及这样的计算机可读程序，当该程序被逻辑部件所执行时，能够使该逻辑部件实现上文所述的装置或构成部件，或使该逻辑部件实现上文所述的各种方法或步骤。本发明还涉及用于存储以上程序的存储介质，如硬盘、磁盘、光盘、DVD、flash存储器等。

这些实施例的许多特征和优点根据该详细描述是清楚的，因此所附权利要求旨在覆盖这些实施例的落入其真实精神和范围内的所有这些特征和优点。此外，由于本领域的技术人员容易想到很多修改和改变，因此不是要将本发明的实施例限于所例示和描述的精确结构和操作，而是可以涵盖落入其范围内的所有合适修改和等同物。

本发明未详细说明部分为本领域技术人员公知技术。