一种无人机位姿优化方法与流程

1.本发明涉及无人机技术领域，特别涉及一种无人机位姿优化方法。


背景技术：

2.传统的位姿估计方法分为两类：基于icp(icp,iterative closest point)方法的3d-3d、基于pnp方法的3d-2d优化方法。前者在三维空间通过最小化两个三维点集之间的距离函数求解变换矩阵，后者在二维图像平面通过最小化匹配点对之间的重投影误差求解变换矩阵。然而采用传统的位姿估计方法并不能无法很好的解决图像观测噪声以及帧间位姿估计误差导致初始运动及地图估计不确定等问题。
3.鉴于此，需要一种无人机位姿优化方法。


技术实现要素：

4.本发明实施例提供了一种无人机位姿优化方法，以至少解决相关技术中无人机位姿不准确的技术问题。
5.根据本发明实施例的一方面，提供了一种无人机位姿优化方法，包括：
6.获取无人机飞行的位姿视频；
7.对所述位姿视频的每一帧图片进行特征提取，并建立两帧之间的线特征关联关系；
8.根据所述线特征关联关系优化求解帧间的相对变换矩阵，完成位姿更新。
9.可选地，对所述位姿视频的每一帧图片进行特征提取，并建立两帧之间的线特征关联关系，包括：改进orb的特征提取与匹配方法建立帧间鲁棒的特征匹配关系。
10.可选地，所述改进orb的特征提取与匹配方法包括以下步骤：
11.采用自适应fast线特征检测方法检测图片的fast特征点；
12.计算fast特征点方向和特征描述子；
13.基于特征描述子进行暴力匹配；
14.采用基于运动平滑一致性约束和对极几何约束先后剔除暴力匹配得到的图像中的误匹配点，即得到两帧之间的线特征关联关系。
15.可选地，根据所述线特征关联关系优化求解帧间的相对变换矩阵，完成位姿更新，包括：
16.基于所述两帧之间的线特征关联关系，求解初始的变换矩阵；
17.构建混合重投影误差模型；
18.采用混合重投影误差模型同时最小化特征匹配点对的3d-2d和3d-3d重投影误差，求解最优位姿，完成位姿更新。
19.可选地，通过opencv库中的solvepnpransac函数求解初始的变换矩阵。
20.可选地，混合重投影误差模型t
cw
的表达式为：
21.t
cw
＝arg min{ω2e2+ω3e3}
22.上式中，ω2和ω3表示指示参数，对于有深度值的特征点，设为ω2＝0和ω3＝1，否则设为ω2＝1，ω3＝0；e2为n个3d-2d点对的重投影误差；e3为对于m个3d-3d点对的重投影误差。
23.可选地，在ba框架使用牛顿下降法对混合重投影误差模型求解最优位姿。
24.根据本发明实施例的另一方面，还提供了一种无人机位姿优化系统，包括：
25.数据获取模块，用于获取无人机飞行的位姿视频；
26.帧间特征关联模块；用于对所述位姿视频的每一帧图片进行特征提取，并建立两帧之间的线特征关联关系；
27.位姿求解与更新模块，用于根据所述线特征关联关系优化求解帧间的相对变换矩阵，完成位姿更新。
28.根据本发明实施例的另一方面，还提供了一种计算机可读存储介质，所述计算机可读存储介质包括存储的程序，其中，在所述程序运行时控制所述计算机可读存储介质所在设备执行上述任意一项所述的无人机位姿优化方法。
29.根据本发明实施例的另一方面，还提供了一种处理器，所述处理器用于运行程序，其中，所述程序运行时执行上述任意一项所述的无人机位姿优化方法。
30.与现有的技术相比，本发明具有如下有益效果：
31.本发明所提供的无人机位姿优化方法，通过获取无人机飞行的位姿视频；对所述位姿视频的每一帧图片进行特征提取，并建立两帧之间的线特征关联关系；根据所述线特征关联关系优化求解帧间的相对变换矩阵，完成位姿更新。利用所有测量信息优化无人机的运动姿态和地图特征位置，得到准确的位姿状态，从而解决现有无人机位姿不正确的问题，从而也解决了由于图像观测噪声以及帧间位姿估计误差导致初始运动及地图估计不确定等问题。
附图说明
32.为了更清楚地说明本发明的技术方案，下面将对实施例描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明的一个实施例，对于本领域普通技术人员来说，在不付出创造性劳动的前提下，还可以根据这些附图获得其他的附图。
33.图1是根据本发明实施例的一种无人机位姿优化方法的流程图；
34.图2是根据本发明实施例的改进orb的特征提取与匹配方法的流程图；
35.图3是根据本发明实施例的无人机位姿优化系统的示意图。
具体实施方式
36.需要说明的是，在不冲突的情况下，本技术中的实施例及实施例中的特征可以相互组合。下面将参考附图并结合实施例来详细说明本技术。
37.为了使本技术领域的人员更好地理解本技术方案，下面将结合本技术实施例中的附图，对本技术实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本技术一部分的实施例，而不是全部的实施例。基于本技术中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都应当属于本技术保护的范围。
38.需要说明的是，本技术的说明书和权利要求书及上述附图中的术语“第一”、“第二”等是用于区别类似的对象，而不必用于描述特定的顺序或先后次序。应该理解这样使用的数据在适当情况下可以互换，以便这里描述的本技术的实施例。此外，术语“包括”和“具有”以及他们的任何变形，意图在于覆盖不排他的包含，例如，包含了一系列步骤或单元的过程、方法、系统、产品或设备不必限于清楚地列出的那些步骤或单元，而是可包括没有清楚地列出的或对于这些过程、方法、产品或设备固有的其它步骤或单元。
39.实施例1
40.根据本发明实施例，提供了一种无人机位姿优化方法的实施例，需要说明的是，在附图的流程图示出的步骤可以在诸如一组计算机可执行指令的计算机系统中执行，并且，虽然在流程图中示出了逻辑顺序，但是在某些情况下，可以以不同于此处的顺序执行所示出或描述的步骤。
41.如图1是根据本发明实施例的一种无人机位姿优化方法的流程图，如图1所示，该方法包括如下步骤：
42.步骤s1、获取无人机飞行的位姿视频；
43.步骤s2、对所述位姿视频的每一帧图片进行特征提取，并建立两帧之间的线特征关联关系；
44.步骤s3、根据所述线特征关联关系优化求解帧间的相对变换矩阵，完成位姿更新。
45.作为一种可选的实施例，对所述位姿视频的每一帧图片进行特征提取，并建立两帧之间的线特征关联关系，包括：改进orb的特征提取与匹配方法建立帧间鲁棒的特征匹配关系。
46.具体的，如图2所示，步骤s2所述改进orb的特征提取与匹配方法包括以下步骤：
47.步骤s21、采用自适应fast线特征检测方法检测图片的fast特征点；
48.步骤s22、计算fast特征点方向和特征描述子(即rbrief描述子)；
49.步骤s23、基于特征描述子进行暴力匹配；
50.步骤s24、最后针对外点问题，采用基于运动平滑一致性约束和对极几何约束先后剔除暴力匹配得到的图像中的误匹配点，即得到两帧之间的线特征关联关系。
51.作为一种可选的实施例，步骤s3根据所述线特征关联关系优化求解帧间的相对变换矩阵，完成位姿更新，包括：
52.步骤s31、基于所述两帧之间的线特征关联关系，求解初始的变换矩阵；其中，采用opencv库中的solvepnpransac函数求解初始的变换矩阵。
53.步骤s32、构建混合重投影误差模型，混合重投影误差模型为基于3d-3d和3d-2d的混合重投影误差模型。
54.具体的，首先计算3d-2d重投影误差，设2d重投影点为ui，kc表示已知的相机内参矩阵，s表示已知的重投影尺度因子，对于n个3d-2d点对的重投影误差e2可表示为：
[0055][0056]
然后计算3d-3d重投影误差，对于m个3d-3d点对，重投影误差e3可表示为：
[0057][0058]
其中，pj为有当前帧上有深度值的特征点。
[0059]
最后，通过融合e2和e3，混合重投影误差模型t
cw
表示为：
[0060]
t
cw
＝arg min{ω2e2+ω3e3}
[0061]
上式中，ω2和ω3表示指示参数，对于有深度值的特征点，设为ω2＝0和ω3＝1，否则设为ω2＝1，ω3＝0；e2为n个3d-2d点对的重投影误差；e3为对于m个3d-3d点对的重投影误差。
[0062]
如图3所示，p1和p2表示空间路标点，设世界坐标系下的空间路标点为pi＝{p1,p1,...,pn}。通过重投影空间路标点可以得到图像平面上对应的像素点，由于误差的存在，检测点(线特征)和重投影点存在距离误差，优化该距离误差能获取更平滑的相机位姿。
[0063]
在线特征slam中，首先需要构造二进制描述子匹配器并在前一帧和当前帧的线特征中进行匹配，然后，根据线特征的描述子距离进行阈值判断，进一步根据前后帧的线端点的移动距离判断是否为优质匹配对，对于匹配成功的特征线对，取其线段中点，计算其位于像素平面的重投影误差，作为slam后端优化的视觉约束项；提取出的轮廓线有许多并不良好的轮廓线，比如椅子的影子提取出的轮廓，图像上的噪点等等，所以需要对提取出的轮廓线进行筛选。由于有小变化假设，相对的轮廓线一般相距较小。使用轮廓线的几何距离和hu矩实现粗匹配，满足以下两个条件的轮廓线认为粗匹配：求轮廓线的最小矩形边界框，边界框的中心在图像上接近，边界框的长和宽在数值上接近；轮廓的hu矩匹配度较高。为了便于误差优化，将轮廓匹配提取出特征点进行匹配。与直接提取特征点的方法不同，在两帧轮廓的匹配上，提取当前帧的轮廓线拐点，并对其做仿射变换，得到的就是参考帧中的对应点位置，而不取参考帧中对应像素点的实际位置，在已知仿射变换的情况下可以实现亚像素级匹配。其次，对基于增量平滑的slam后端优化算法进行改进，提高地图的构建精度及对机器人的定位精度；最后，将该算法应用于基于线特征的人机位姿的slam问题中。解决高维数据处理和无人机位姿处于非欧式空间产生的奇异值问题。
[0064]
步骤s33、采用混合重投影误差模型同时最小化特征匹配点对的3d-2d和3d-3d重投影误差，求解最优位姿，完成位姿更新。
[0065]
具体的，混合重投影误差模型实际上是一个凸优化问题，可以把它看成最小二乘问题，在ba框架使用牛顿下降法对混合重投影误差模型求解最优位姿。
[0066]
实施例2
[0067]
根据本发明实施例的另一方面，还提供了一种无人机位姿优化系统，无人机位姿优化系统应用上述的无人机位姿优化方法，该系统包括：
[0068]
数据获取模块，用于获取无人机飞行的位姿视频；
[0069]
帧间特征关联模块；用于对所述位姿视频的每一帧图片进行特征提取，并建立两帧之间的线特征关联关系；
[0070]
位姿求解与更新模块，用于根据所述线特征关联关系优化求解帧间的相对变换矩阵，完成位姿更新。
[0071]
本发明不局限于以上的具体实施方式，以上仅为本发明的较佳实施案例而已，并不用以限制本发明，凡在本发明的精神和原则之内所作的任何修改、等同替换和改进等，均
应包含在本发明的保护范围之内。
[0072]
实施例3
[0073]
根据本发明实施例的另一方面，还提供了一种计算机可读存储介质，该计算机可读存储介质包括存储的程序，其中，在程序运行时控制计算机可读存储介质所在设备执行上述中任意一项的无人机位姿优化方法。
[0074]
可选地，在本实施例中，上述计算机可读存储介质可以位于计算机网络中计算机终端群中的任意一个计算机终端中，或者位于移动终端群中的任意一个移动终端中，上述计算机可读存储介质包括存储的程序。
[0075]
可选地，在程序运行时控制计算机可读存储介质所在设备执行以下功能：获取无人机飞行的位姿视频；对所述位姿视频的每一帧图片进行特征提取，并建立两帧之间的线特征关联关系；根据所述线特征关联关系优化求解帧间的相对变换矩阵，完成位姿更新。
[0076]
实施例4
[0077]
根据本发明实施例的另一方面，还提供了一种处理器，该处理器用于运行程序，其中，程序运行时执行上述中任意一项的无人机位姿优化方法。
[0078]
本发明实施例提供了一种设备，该设备包括处理器、存储器及存储在存储器上并可在处理器上运行的程序，处理器执行程序时实现无人机位姿优化方法的步骤。
[0079]
上述本发明实施例序号仅仅为了描述，不代表实施例的优劣。
[0080]
在本发明的上述实施例中，对各个实施例的描述都各有侧重，某个实施例中没有详述的部分，可以参见其他实施例的相关描述。
[0081]
在本技术所提供的几个实施例中，应该理解到，所揭露的技术内容，可通过其它的方式实现。其中，以上所描述的系统实施例仅仅是示意性的，例如所述单元的划分，可以为一种逻辑功能划分，实际实现时可以有另外的划分方式，例如多个单元或组件可以结合或者可以集成到另一个系统，或一些特征可以忽略，或不执行。另一点，所显示或讨论的相互之间的耦合或直接耦合或通信连接可以是通过一些接口，单元或模块的间接耦合或通信连接可以是电性或其它的形式。
[0082]
所述作为分离部件说明的单元可以是或者也可以不是物理上分开的，作为单元显示的部件可以是或者也可以不是物理单元，即可以位于一个地方，或者也可以分布到多个单元上。可以根据实际的需要选择其中的部分或者全部单元来实现本实施例方案的目的。
[0083]
另外，在本发明各个实施例中的各功能单元可以集成在一个处理单元中，也可以是各个单元单独物理存在，也可以两个或两个以上单元集成在一个单元中。上述集成的单元既可以采用硬件的形式实现，也可以采用软件功能单元的形式实现。
[0084]
所述集成的单元如果以软件功能单元的形式实现并作为独立的产品销售或使用时，可以存储在一个计算机可读取存储介质中。基于这样的理解，本发明的技术方案本质上或者说对现有技术做出贡献的部分或者该技术方案的全部或部分可以以软件产品的形式体现出来，该计算机软件产品存储在一个存储介质中，包括若干指令用以使得一台计算机设备(可为个人计算机、服务器或者网络设备等)执行本发明各个实施例所述方法的全部或部分步骤。而前述的存储介质包括：u盘、只读存储器(rom,read-0nlymemory)、随机存取存储器(ram,randomaccessmemory)、移动硬盘、磁碟或者光盘等各种可以存储程序代码的介质。
[0085]
以上所述仅是本发明的优选实施方式，应当指出，对于本技术领域的普通技术人员来说，在不脱离本发明原理的前提下，还可以做出若干改进和润饰，这些改进和润饰也应视为本发明的保护范围。

技术特征：
1.一种无人机位姿优化方法，其特征在于，包括：获取无人机飞行的位姿视频；对所述位姿视频的每一帧图片进行特征提取，并建立两帧之间的线特征关联关系；根据所述线特征关联关系优化求解帧间的相对变换矩阵，完成位姿更新。2.根据权利要求1所述的无人机位姿优化方法，其特征在于，对所述位姿视频的每一帧图片进行特征提取，并建立两帧之间的线特征关联关系，包括：改进orb的特征提取与匹配方法建立帧间鲁棒的特征匹配关系。3.根据权利要求2所述的无人机位姿优化方法，其特征在于，所述改进orb的特征提取与匹配方法包括以下步骤：采用自适应fast线特征检测方法检测图片的fast特征点；计算fast特征点方向和特征描述子；基于特征描述子进行暴力匹配；采用基于运动平滑一致性约束和对极几何约束先后剔除暴力匹配得到的图像中的误匹配点，即得到两帧之间的线特征关联关系。4.根据权利要求1所述的无人机位姿优化方法，其特征在于，根据所述线特征关联关系优化求解帧间的相对变换矩阵，完成位姿更新，包括：基于所述两帧之间的线特征关联关系，求解初始的变换矩阵；构建混合重投影误差模型；采用混合重投影误差模型同时最小化特征匹配点对的3d-2d和3d-3d重投影误差，求解最优位姿，完成位姿更新。5.根据权利要求4所述的无人机位姿优化方法，其特征在于，通过opencv库中的solvepnpransac函数求解初始的变换矩阵。6.根据权利要求4所述的无人机位姿优化方法，其特征在于，混合重投影误差模型t
cw
的表达式为：t
cw
＝argmin{ω2e2+ω3e3}上式中，ω2和ω3表示指示参数，对于有深度值的特征点，设为ω2＝0和ω3＝1，否则设为ω2＝1，ω3＝0；e2为n个3d-2d点对的重投影误差；e3为对于m个3d-3d点对的重投影误差。7.根据权利要求1所述的无人机位姿优化方法，其特征在于，在ba框架使用牛顿下降法对混合重投影误差模型求解最优位姿。8.一种无人机位姿优化系统，其特征在于，包括：数据获取模块，用于获取无人机飞行的位姿视频；帧间特征关联模块；用于对所述位姿视频的每一帧图片进行特征提取，并建立两帧之间的线特征关联关系；位姿求解与更新模块，用于根据所述线特征关联关系优化求解帧间的相对变换矩阵，完成位姿更新。9.一种计算机可读存储介质，其特征在于，所述计算机可读存储介质包括存储的程序，其中，在所述程序运行时控制所述计算机可读存储介质所在设备执行权利要求1至7中任意一项所述的无人机位姿优化方法。10.一种处理器，其特征在于，所述处理器用于运行程序，其中，所述程序运行时执行权
利要求1至7中任意一项所述的无人机位姿优化方法。

技术总结
本发明公开了一种无人机位姿优化方法，通过获取无人机飞行的位姿视频；对位姿视频的每一帧图片进行特征提取，并建立两帧之间的线特征关联关系；根据线特征关联关系优化求解帧间的相对变换矩阵，完成位姿更新。利用所有测量信息优化无人机的运动姿态和地图特征位置，得到准确的位姿状态，从而解决现有无人机位姿不正确的问题，从而也解决了由于图像观测噪声以及帧间位姿估计误差导致初始运动及地图估计不确定等问题。不确定等问题。不确定等问题。


技术研发人员：祝文姬 欧发斌 卓浩泽 纪硕磊 王乐 张龙飞 刘旭 祁红涛 卢万里 凌颖 邹林
受保护的技术使用者：南方电网科学研究院有限责任公司
技术研发日：2023.03.07
技术公布日：2023/7/19